實現(xiàn)運動控制系統(tǒng)s曲線加減速的控制電路的制作方法
【專利摘要】一種實現(xiàn)運動控制系統(tǒng)S曲線加減速的控制電路，CPU總線接口電路把加加速度、平均速度、起始速度、終止速度與指令位移參數(shù)寫入指令參數(shù)寄存器中的相應(yīng)寄存器，電路響應(yīng)外部請求，送出本電路的工作狀態(tài)；初始化電路讀取指令參數(shù)寄存器中的指令參數(shù)，計算速度變化總量并合理規(guī)劃各段速度變化量；加加速度積分電路和加速度積分電路執(zhí)行積分運算，并寫入執(zhí)行寄存器中的相應(yīng)寄存器；驅(qū)動脈沖脈間計算電路讀取速度寄存器，計算驅(qū)動脈沖的脈間計數(shù)值；脈沖發(fā)生電路利用脈間計數(shù)值對基準(zhǔn)時鐘計數(shù)，實現(xiàn)所需要的驅(qū)動脈沖；位移控制電路對輸出的驅(qū)動脈沖計數(shù)，計算輸出位移并寫入位移寄存器；狀態(tài)轉(zhuǎn)換與控制電路響應(yīng)基準(zhǔn)時鐘，控制本S曲線加減速控制電路有序協(xié)調(diào)動作。
【專利說明】實現(xiàn)運動控制系統(tǒng)S曲線加減速的控制電路
【技術(shù)領(lǐng)域】
[0001]本技術(shù)方案屬于運動控制領(lǐng)域，具體是一種實現(xiàn)運動控制系統(tǒng)S曲線加減速的控制電路。
【背景技術(shù)】
[0002]加減速控制是機器人、高性能數(shù)控系統(tǒng)等的關(guān)鍵技術(shù)。相較于其它方法，S曲線加減速控制具有加速度連續(xù)，速度曲線光滑、均勻，運動平穩(wěn)、無沖擊等優(yōu)勢，能夠獲得較理想的運動控制效果。結(jié)合高性能數(shù)控、機器人等的實際需求，先后出現(xiàn)了 S曲線加減速的嵌套式前瞻快速算法、實時軟件等成果，但其速度規(guī)劃模型、求解算法及實時性等是該領(lǐng)域研究亟待解決的問題。
[0003]相對而言，現(xiàn)有的S曲線加減速控制方法多采用高性能計算機，通過軟件的方式實現(xiàn)。加減速控制過程中，計算機根據(jù)指令參數(shù)，通過復(fù)雜、繁瑣的不等式求解運算，確定加減速邊界條件與約束條件；而后，根據(jù)得到的約束條件，進行速度規(guī)劃，求解高次方程或不等式，確定各運動段的各時間點、運動速度、加速度等參數(shù)。運算量大，對系統(tǒng)實時性，尤其是嵌入式處理器為主的運動控制系統(tǒng)，存在較大影響。

【發(fā)明內(nèi)容】

[0004]結(jié)合數(shù)控系統(tǒng)、機器人等領(lǐng)域的實際應(yīng)用，本發(fā)明研究分析利用S曲線加減速過程中速度、加速度、加加速度曲線的幾何特征及其相互之間的數(shù)學(xué)關(guān)系，通過對其過程及運算的合理規(guī)劃，適當(dāng)簡化，借助分布式存儲與計算等手段，避免現(xiàn)有S曲線加減速控制的復(fù)雜、繁瑣運算，探索實時性高、便于實現(xiàn)的S曲線加減速控制方法與電路。
[0005]本發(fā)明結(jié)合高性能數(shù)控、機器人等領(lǐng)域的實際控制需求，本技術(shù)方案提出了 S曲線加減速的速度規(guī)劃模型及硬件實現(xiàn)方法，以及相關(guān)高性能實時控制電路及邏輯控制芯片，以簡化相關(guān)運動控制系統(tǒng)的硬、軟件，提高運動控制系統(tǒng)實時性、集成度與可靠性，提供聞性能的運動控制解決方案。
[0006]本技術(shù)方案的原理說明如下:
[0007]OS曲線加減速過程的運動分析:
[0008]運動控制過程中，平穩(wěn)、光滑、均勻的S型速度曲線要求運動段的速度-時間變化曲線處處連續(xù)、可導(dǎo)；同時要求運動段的加加速度、加速度與速度變化曲線相互之間滿足嚴(yán)格的積分關(guān)系:
[0009]1.1)加加速曲線
[0010]S曲線加減速的加加速度曲線為依賴運動時間的分段曲線(加減速過程的加加速度曲線如圖1所示)，假定加加速度大小為JM，假定當(dāng)前時間t、加加速段終點時間h、勻加速段終點時間、減加速段終點時間t2、勻速段終點時間t3、加減速段終點時間t4、勻減速段終點時間t5與減減速段終點時間t6，根據(jù)加加速度曲線，得到加加速度J的計算公式(I):[0011 ]
【權(quán)利要求】
1.一種實現(xiàn)運動控制系統(tǒng)S曲線加減速的控制電路，其特征是在使用時候，本S曲線加減速控制電路接收來自外部的運動控制指令，根據(jù)指令參數(shù)計算在S曲線加減速運動段的各運動時刻的加速度、速度以及速度控制量；而后，根據(jù)速度控制量，動態(tài)調(diào)節(jié)脈間與脈沖頻率，產(chǎn)生符合控制指令的伺服電機驅(qū)動脈沖； S曲線加減速控制電路包括:CPU總線接口電路、指令參數(shù)寄存器、初始化電路、加加速度積分電路、加速度積分電路、執(zhí)行寄存器、狀態(tài)轉(zhuǎn)換與控制電路、驅(qū)動脈沖發(fā)生電路、位移控制電路和驅(qū)動脈沖脈間計算電路； CPU總線接口電路包括并行總線接口電路與SPI總線接口電路； 所述指令參數(shù)寄存器包括加加速度寄存器、平均速度寄存器、起始速度寄存器、終止速度寄存器與指令位移寄存器； 所述執(zhí)行寄存器包括位移寄存器、速度寄存器、加速度寄存器和運動狀態(tài)寄存器； 各部分功能及作用如下: CPU總線接口電路把加加速度、平均速度、起始速度、終止速度與指令位移參數(shù)寫入指令參數(shù)寄存器中的相應(yīng)寄存器，本S曲線加減速控制電路響應(yīng)外部請求，送出本S曲線加減速控制電路的工作狀態(tài)； 初始化電路讀取指令參數(shù)寄存器中的指令參數(shù)，計算速度變化總量并合理規(guī)劃各段速度變化量； 加加速度積分電路和加速度積分電路執(zhí)行積分運算，分別求取即時加速度和速度，并與入執(zhí)行寄存器中的相應(yīng)寄存器；· 驅(qū)動脈沖脈間計算電路讀取速度寄存器，根據(jù)即時速度計算驅(qū)動脈沖的脈間計數(shù)值；脈沖發(fā)生電路利用脈間計數(shù)值對基準(zhǔn)時鐘計數(shù)，實現(xiàn)所需要的驅(qū)動脈沖； 位移控制電路對輸出的驅(qū)動脈沖計數(shù)，計算輸出位移并寫入位移寄存器； 狀態(tài)轉(zhuǎn)換與控制電路響應(yīng)基準(zhǔn)時鐘，根據(jù)運行狀態(tài)、即時速度、加速度修改本S曲線加減速控制電路運行狀態(tài)，控制本S曲線加減速控制電路有序協(xié)調(diào)動作。
2.根據(jù)權(quán)利要求2所述的實現(xiàn)運動控制系統(tǒng)S曲線加減速的控制電路，其特征是本控制電路電路的設(shè)計中的參數(shù)要求如下: (I)速度控制: 設(shè)脈寬計數(shù)值Cntci，脈間計數(shù)值cnt，基準(zhǔn)時鐘周期Tcx，則通用的驅(qū)動脈沖頻率f的算式為公式(11):1f - 1η ηI% ? Jr I
(cnt + cnt0)TCK 步當(dāng)量S取I μ m，基準(zhǔn)時鐘周期50ns即20MHz，驅(qū)動脈沖脈寬取定值I μ s，則脈寬計數(shù)值CntciS 20，則對于(I)速度控制: 由此得到速度V (單位mm/s)與計數(shù)值cnt的關(guān)系式(12): v—0—s— 1^ir3 — 2_o3麵 F /</ ——rt —XlImi
(cnt+ CntnWck (c?/ + 20)50 X IO^9 cnt + 20
則速度控制量cnt的算式為公式(13):
3.根據(jù)權(quán)利要求2所述的實現(xiàn)運動控制系統(tǒng)S曲線加減速的控制電路，其特征是本S曲線加減速控制電路運行狀態(tài)定義與狀態(tài)轉(zhuǎn)換條件如下: 本加減速控制電路的設(shè)置10種運行狀態(tài)，見表1: 表1加減速控制電路的運行狀態(tài)
4.根據(jù)權(quán)利要求2所述的實現(xiàn)運動控制系統(tǒng)S曲線加減速的控制電路，其特征是還包括段間轉(zhuǎn)換寄存器，段間轉(zhuǎn)換寄存器包括勻速段起始位移寄存器、勻加速段終止速度寄存器和勻加速段起始速度寄存器； 本加減速控制電路工作過程如下: 上電以后，本S曲線加減速控制電路電路進入空閑狀態(tài)； 寫信號有效，電路轉(zhuǎn)數(shù)據(jù)傳送狀態(tài)，總線接口響應(yīng)寫信號WR，依次寫入加加速度、平均速度、指令位移、起始速度和終止速度； 加減速使能信號有效，運動初始化狀態(tài)被激活，電路讀取指令參數(shù)寄存器，根據(jù)寫入的參數(shù)判別加減速模式，初算各段速度變化量與勻加速段的起始速度Vf、終止速度Vs ； 空閑狀態(tài)下，加減速使能信號也會激活運動初始化狀態(tài)，此時電路直接讀取指令參數(shù)寄存器進行初始化； 運動初始化完成后，本S曲線加減速控制電路開始S曲線加減速控制過程，加加速度與加速度積分電路?ο μ S積分一次，求取即時加速度、速度并計算速度控制量cnt，由脈沖發(fā)生電路產(chǎn)生相應(yīng)驅(qū)動脈沖，實現(xiàn)相應(yīng)的運動段； 初始化結(jié)束，若指令為均速運動或加速運動(m=0，l)，本S曲線加減速控制電路轉(zhuǎn)入加加速狀態(tài)；若指令為減速運動模式(m=2)，本S曲線加減速控制電路轉(zhuǎn)加減速態(tài)； 加加速狀態(tài)，若即時加速度a > aMax，本S曲線加減速控制電路將即時速度V作為新的勻加速段起始速度Vf存入段間轉(zhuǎn)換寄存器，同時重算并存儲勻加速段終止速度Vs，而后轉(zhuǎn)入勻加速狀態(tài)；若a < aMax且V > Vf，電路直接進入勻加速狀態(tài)；勻加速狀態(tài)，即時速度V^ Vs,電路進入減加速狀態(tài)； 而后的減加速狀態(tài)，當(dāng)加速度減至O值，本S曲線加減速控制電路存儲即時位移s作為勻速段起始位移Lf，進入勻速運動狀態(tài)； 勻速狀態(tài)下，若為均速運動(m=0)且位移s ^ L-Lf,電路轉(zhuǎn)加減速態(tài)；若為加速運動(m=l)且即時位移s 3L，本S曲線加減速控制電路轉(zhuǎn)空閑態(tài)，完成加減速控制； 加減速狀態(tài)，電路等到即時速度V降至Vs后進入勻減速狀態(tài)；然而，如果加減速狀態(tài)下，出現(xiàn)a ( -aMax的情況，電路將即時速度V作為\，同時重算速度Vf并存儲，直接轉(zhuǎn)入勻減速狀態(tài)；· 勻減速狀態(tài)，即時速度V ( Vf,電路進入減減速狀態(tài)； 最后，減減速狀態(tài)下，即時位移s > L，本S曲線加減速控制電路轉(zhuǎn)空閑態(tài)，完成加減速控制； 轉(zhuǎn)換條件⑩..@為均速運動(m=0)模式下的異常處理，在加加速、勻加速、減加速段的任何時刻，若當(dāng)前位移達(dá)到指令位移的50%，本S曲線加減速控制電路直接越過正常處理進入加減速段； 轉(zhuǎn)換條件# 為減速運動(m=2)模式下的異常處理，在運動的任意時刻，若當(dāng)前位移達(dá)到指令位移，本S曲線加減速控制電路直接進入空閑狀態(tài)，完成加減速控制； 轉(zhuǎn)換條件fW?-杏為加速運動(m=l)模式下的異常處理，在運動的任意時刻，若當(dāng)前位移達(dá)到指令位移，本S曲線加減速控制電路直接進入空閑狀態(tài)，完成加減速控制。
【文檔編號】G05D13/62GK103713660SQ201310684405
【公開日】2014年4月9日 申請日期:2013年12月12日 優(yōu)先權(quán)日:2013年12月12日
【發(fā)明者】葛紅宇, 李宏勝, 張建華, 王建紅, 樊紅梅, 邵祥兵 申請人:南京工程學(xué)院