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

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

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