[發明專利]實現運動控制系統S曲線加減速的控制電路

有效

• 申請號: 201310684405.7
• 申請日: 2013-12-12
• 公開（公告）號: CN103713660A
• 公開（公告）日: 2014-04-09
• 發明（設計）人: 葛紅宇;李宏勝;張建華;王建紅;樊紅梅;邵祥兵
• 申請（專利權）人: 南京工程學院
• 主分類號: G05D13/62
• 分類號: G05D13/62
• 代理公司: 江蘇圣典律師事務所 32237
• 代理人: 朱慶華

1.一種實現運動控制系統S曲線加減速的控制電路，其特征是在使用時候，本S曲線加減速控制電路接收來自外部的運動控制指令，根據指令參數計算在S曲線加減速運動段的各運動時刻的加速度、速度以及速度控制量；而后，根據速度控制量，動態調節脈間與脈沖頻率，產生符合控制指令的伺服電機驅動脈沖；

S曲線加減速控制電路包括：CPU總線接口電路、指令參數寄存器、初始化電路、加加速度積分電路、加速度積分電路、執行寄存器、狀態轉換與控制電路、驅動脈沖發生電路、位移控制電路和驅動脈沖脈間計算電路；

CPU總線接口電路包括并行總線接口電路與SPI總線接口電路；

所述指令參數寄存器包括加加速度寄存器、平均速度寄存器、起始速度寄存器、終止速度寄存器與指令位移寄存器；

所述執行寄存器包括位移寄存器、速度寄存器、加速度寄存器和運動狀態寄存器；

各部分功能及作用如下：

CPU總線接口電路把加加速度、平均速度、起始速度、終止速度與指令位移參數寫入指令參數寄存器中的相應寄存器，本S曲線加減速控制電路響應外部請求，送出本S曲線加減速控制電路的工作狀態；

初始化電路讀取指令參數寄存器中的指令參數，計算速度變化總量并合理規劃各段速度變化量；

加加速度積分電路和加速度積分電路執行積分運算，分別求取即時加速度和速度，并寫入執行寄存器中的相應寄存器；

驅動脈沖脈間計算電路讀取速度寄存器，根據即時速度計算驅動脈沖的脈間計數值；脈沖發生電路利用脈間計數值對基準時鐘計數，實現所需要的驅動脈沖；

位移控制電路對輸出的驅動脈沖計數，計算輸出位移并寫入位移寄存器；

狀態轉換與控制電路響應基準時鐘，根據運行狀態、即時速度、加速度修改本S曲線加減速控制電路運行狀態，控制本S曲線加減速控制電路有序協調動作。

2.根據權利要求2所述的實現運動控制系統S曲線加減速的控制電路，其特征是本控制電路電路的設計中的參數要求如下：

（1）速度控制：

設脈寬計數值cnt₀，脈間計數值cnt，基準時鐘周期T_CK，則通用的驅動脈沖頻率f的算式為公式（11）：

f=1(cnt+cnt0)Tck---(11)]]>

步當量δ取1μm，基準時鐘周期50ns即20MHz，驅動脈沖脈寬取定值1μs，則脈寬計數值cnt₀為20，則對于（1）速度控制：

由此得到速度v（單位mm/s）與計數值cnt的關系式（12）：

v=fδ=δ(cnt+cnt0)Tck=1×10-3(cnt+20)50×10-9=20×103cnt+20---(12)]]>

則速度控制量cnt的算式為公式（13）：

cnt=20×103v-20---(13)]]>

（2）運動參數的確定：

運動參數采用遞推公式，根據前一時刻的運動狀態，獲得當前時刻的運動控制參數值：

設初始速度為V₀，當前采樣時刻k，對應的加加速度為J_k，加速度為a_k，運動速度為V_k，則基于前一時刻的各遞推公式分別為公式（8）、（9）與（10），此處k取0、1、2……：

Jk=JM,0≤t≤t0,t5<t≤t6,0≤k≤nt0,<nt5k≤nt60,t0<t≤t1,t2<t≤t3,t4<t≤t5,nt0<k≤nt1,nt2<k≤nt3,nt4<k≤nt5-JMt1<t≤t2,t3<t≤t4,nt1<k≤nt2,nt3<k≤nt4---(8)]]>

a0=0ak=ak-1+JkΔt---(9)]]>

V0=0Vk=Vk-1+akΔt---(10)]]>

其中，加加速度大小為J_M，t為當前時間，t₀為加加速段終點時間，t₁為勻加速段終點時間，t₂為減加速段終點時間，t₃為勻速段終點時間，t₄為加減速段終點時間、t₅為勻減速段終點時間，t₆為減減速段終點時間；設定采樣時間為Δt，同時t、t₀、t₁、t₂、t₃、t₄、t₅與t₆分別對應于采樣時刻n、n_t0、n_t1、n_t2、n_t3、n_t4、n_t5與n_t6；

然后，假定對式（8）～（10）進行修訂，式（14）為得到的新通用加速度遞推算式：

ak=a0+sigmaJkΔtsigmaJk=sigmaJk-1+Jka0=0---(14)]]>

其中，sigmaJ_kΔt為0-k時刻加速度變化量，即加加速度的一次積分；

設置采樣時間Δt不變，為10μs，得到適用于S曲線加減速控制電路的專用加速度遞推算式（15）；式中加速度與加加速度單位分別為mm/s²與mm/s³；

ak=a0+sigmaJk/105sigmaJk=sigmaJk-1a0=0+Jk---(15)]]>

設公式（16）為修訂后的通用速度遞推算式：

Vk=V0+(ka0+sigmaAkΔt)ΔtsigmaAk=sigmaAk-1+sigmaJka0=0,V0=0---(16)]]>

其中，(ka₀+sigmaA_kΔt)Δt為0-k時刻速度變化總量，即加速度的一次積分。

同樣取采樣時間Δt為10μs，得到適用于S曲線加減速控制電路的專用速度遞推算式（17）；其中加速度與速度單位分別為mm/s²與mm/s；

Vk=V0+sigmaAk/1010sigmaAk=sigmaAk-1+sigmaJkV0=0---(17)]]>

式中10¹⁰的除運算需要較多的硬件資源，因此計算時把速度分為整數與余數部分，sigmaA_k用作余數寄存器；每次運動采樣執行一次積分運算后，若sigmaA_k≥10¹⁰，即時速度V的整數部分執行加1運算，而后將sigmaA_k-10¹⁰得到的新余數存入sigmaA_k，供下次速度計算使用；反之，sigmaA_k不變。