一種機(jī)器人點(diǎn)動(dòng)操作的速度控制方法
【技術(shù)領(lǐng)域】
[0001] 本發(fā)明涉及機(jī)器人示教模式下點(diǎn)動(dòng)操作的一種速度控制方法，屬于工業(yè)機(jī)器人技 術(shù)。
【背景技術(shù)】
[0002] 手動(dòng)示教模式下點(diǎn)動(dòng)（JOG)操作是機(jī)器人示教人員在各類關(guān)節(jié)坐標(biāo)系空間或笛 卡爾坐標(biāo)系、工具坐標(biāo)系和用戶坐標(biāo)系等空間下驅(qū)動(dòng)機(jī)器人運(yùn)動(dòng)到達(dá)示教點(diǎn)的操作。點(diǎn)動(dòng) 運(yùn)行時(shí)，控制器需要配合示教過(guò)程的按鍵狀態(tài)，實(shí)時(shí)和同步地進(jìn)行插補(bǔ)計(jì)算和執(zhí)行。傳統(tǒng)控 制器采用一種直觀的解決方案：控制器在每個(gè)實(shí)時(shí)控制周期計(jì)算下一個(gè)插補(bǔ)點(diǎn)位置，并在 同一周期內(nèi)完成執(zhí)行任務(wù)。然而，這種方案存在以下缺點(diǎn)：①點(diǎn)動(dòng)速度并不平滑，會(huì)產(chǎn)生機(jī) 械沖擊；②開始運(yùn)動(dòng)加速度一般很大，無(wú)法實(shí)現(xiàn)微小位移點(diǎn)動(dòng)；③只考慮到單個(gè)周期內(nèi)的 運(yùn)動(dòng)范圍，若當(dāng)前運(yùn)動(dòng)速度很大時(shí)，可能因無(wú)法及時(shí)停止而超出運(yùn)動(dòng)限位。

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

[0003] 發(fā)明目的：為了克服現(xiàn)有技術(shù)中存在的不足，本發(fā)明針對(duì)伺服電機(jī)處于位置控制 模式的情況，提供一種機(jī)器人手動(dòng)示教模式下點(diǎn)動(dòng)操作的速度控制方法，為實(shí)現(xiàn)機(jī)器人關(guān) 節(jié)坐標(biāo)系空間下的單軸平滑點(diǎn)動(dòng)操作提供一種軌跡規(guī)劃步驟和程序?qū)崿F(xiàn)方法，同時(shí)為實(shí)現(xiàn) 笛卡爾坐標(biāo)系、工具坐標(biāo)系和用戶坐標(biāo)系等空間下的移動(dòng)點(diǎn)動(dòng)和轉(zhuǎn)動(dòng)點(diǎn)動(dòng)操作提供一種軌 跡規(guī)劃步驟和程序?qū)崿F(xiàn)方法。本發(fā)明方法能使點(diǎn)動(dòng)過(guò)程速度平滑，能有效降低對(duì)機(jī)械結(jié)構(gòu) 的沖擊、磨損以及減少運(yùn)動(dòng)能耗，實(shí)現(xiàn)微小距離點(diǎn)動(dòng)，并解決高速點(diǎn)動(dòng)無(wú)法及時(shí)平滑停止的 問(wèn)題。
[0004] 技術(shù)方案：為實(shí)現(xiàn)上述目的，本發(fā)明采用的技術(shù)方案為：
[0005] -種機(jī)器人點(diǎn)動(dòng)操作的速度控制方法，將點(diǎn)動(dòng)操作的速度控制過(guò)程劃分為加速& 勻速段和停止段，其中加速&勻速段包括周期變加速段、加速-勻速過(guò)渡段和勻速段三個(gè)子 階段，加速-勻速過(guò)渡段和勻速段為兩個(gè)可或缺的子階段，每個(gè)子階段以固定長(zhǎng)度的時(shí)間 片為單位進(jìn)行軌跡插補(bǔ)規(guī)劃，且各個(gè)時(shí)間片內(nèi)插補(bǔ)增量變化的加速度連續(xù)，且每個(gè)時(shí)間片 的起始增量加速度和終止增量加速度均為零；每個(gè)時(shí)間片由連續(xù)的若干個(gè)控制周期組成， 每個(gè)時(shí)間片完成后，根據(jù)判斷條件確定下一時(shí)刻采用加速&勻速段插補(bǔ)序列還是停止段插 補(bǔ)序列，其中判斷條件包括：①當(dāng)前控制過(guò)程處于哪個(gè)子階段，②點(diǎn)動(dòng)操作是否結(jié)束，③機(jī) 器人運(yùn)動(dòng)是否在可達(dá)范圍內(nèi)，④機(jī)器人是否產(chǎn)生碰撞。
[0006] 周期變加速段、加速-勻速過(guò)渡段和勻速段的狀態(tài)跳轉(zhuǎn)關(guān)系如圖1所示：當(dāng)點(diǎn)動(dòng)操 作時(shí)間較長(zhǎng)時(shí)，點(diǎn)動(dòng)操作包含上述三個(gè)子階段和停止段；當(dāng)點(diǎn)動(dòng)操作時(shí)間較短時(shí)，加速-勻 速過(guò)渡段和勻速段可能或缺。各階段插補(bǔ)增量的動(dòng)態(tài)指標(biāo)如圖2所示，點(diǎn)動(dòng)的插補(bǔ)序列點(diǎn) 由初始位置域插補(bǔ)增量相加確定。該方法的具體包括如下部分：
[0007] Partl :記錄初始位置或位姿信息
[0008] 控制器在接收到點(diǎn)動(dòng)請(qǐng)求命令后，記錄初始位置或位姿信息。若是在關(guān)節(jié)空間下 執(zhí)行單軸點(diǎn)動(dòng)操作，點(diǎn)動(dòng)的初始位置為點(diǎn)動(dòng)軸的初始角度；若是在笛卡爾坐標(biāo)系、工具坐標(biāo) 系和用戶坐標(biāo)系等空間下執(zhí)行單軸點(diǎn)動(dòng)操作，點(diǎn)動(dòng)的初始位姿為機(jī)器人工具末端點(diǎn)（Tool Center Point，TCP)在參考坐標(biāo)系下位姿變換矩陣。
[0009] Part 2 :參數(shù)的定義
[0010] 以i = 1，2,…，X表示加速&勻速段的所有時(shí)間片，時(shí)間片的時(shí)間長(zhǎng)度為T2(]^i 個(gè)時(shí)間片的起始增量為θ1(]，對(duì)應(yīng)的起始增量速度和加速度分別為&和Θ先；第i個(gè)時(shí)間 片的中點(diǎn)增量為Θ lh，對(duì)應(yīng)的中點(diǎn)增量速度和加速度分別為6^和式,，，&同時(shí)為第i個(gè)時(shí)間 片的加速度峰值；第i個(gè)時(shí)間片的終止增量為Θ lf，對(duì)應(yīng)的終止增量速度和加速度分別為& 和|;勻速段的給定速度為么；；插補(bǔ)時(shí)間為
，初始時(shí) i = l；
[0011] 停止段的起始增量為Θ。。，對(duì)應(yīng)的起始增量速度和加速度分別為為^和爲(wèi)^;停止段 的終止增量為θ M，對(duì)應(yīng)的終止增量速度和加速度分別為么，和成^
[0012] Part3 :插補(bǔ)規(guī)則設(shè)計(jì)
[0013] 在執(zhí)行第i個(gè)時(shí)間片的插補(bǔ)序列時(shí)，同時(shí)計(jì)算第（i+1)個(gè)時(shí)間片的插補(bǔ)序列，目 的是為了能夠在第i個(gè)時(shí)間片插補(bǔ)完成時(shí)能立即執(zhí)行第（i+1)個(gè)時(shí)間片的插補(bǔ)序列，保證 運(yùn)動(dòng)的連貫性；所述第（i+1)個(gè)時(shí)間片的插補(bǔ)序列分為兩類：①加速&勻速段的插補(bǔ)序列 Normal和停止段的插補(bǔ)序列Ending ;
[0014] 所述插補(bǔ)序列Normal分為三種情況：①若第（i+1)個(gè)時(shí)間片的加速度峰值 沒(méi)有達(dá)到最大加速度&_，第（i+1)個(gè)時(shí)間片的速度也沒(méi)有達(dá)到速度峰值珠，則第（i+1)個(gè) 時(shí)間片為周期變加速段，采用周期變加速段插補(bǔ)方法計(jì)算第（i+1)個(gè)時(shí)間片的插補(bǔ)序列； ②若第（i+1)個(gè)時(shí)間片的加速度峰值達(dá)到最大加速度 &_，但是第（i+1)個(gè)時(shí)間片的 速度沒(méi)有達(dá)到速度峰值堯，則第（i+1)個(gè)時(shí)間片為加速-勻速過(guò)渡段，采用加速-勻速過(guò)渡 段插補(bǔ)方法計(jì)算第（i+1)個(gè)時(shí)間片的插補(bǔ)序列；③若第（i+1)個(gè)時(shí)間片的速度達(dá)到速度峰 值么，則第（i+1)個(gè)時(shí)間片為勻速段，采用勻速段插補(bǔ)方法計(jì)算第（i+1)個(gè)時(shí)間片的插補(bǔ)序 列；
[0015] 由于不知道點(diǎn)動(dòng)操作何時(shí)停止，因此在執(zhí)行每一個(gè)時(shí)間片的插補(bǔ)序列的同時(shí)都需 要計(jì)算插補(bǔ)序列Ending，插補(bǔ)序列Ending的起點(diǎn)為第i個(gè)插補(bǔ)序列Normal的延續(xù)。
[0016] Part 4:周期變加速段的時(shí)間片的插補(bǔ)序列的計(jì)算和執(zhí)行
[0017] 控制程序開啟插補(bǔ)序列的計(jì)算過(guò)程，首先計(jì)算周期變加速段的第i個(gè)時(shí)間片的插 補(bǔ)序列，周期變加速段的插補(bǔ)增量曲線函數(shù)確定方法如下：
[0018] 周期變加速段可以包括若干個(gè)時(shí)間片；在周期變加速段內(nèi)，相鄰兩個(gè)時(shí)間片的加 速度峰值依次遞增固定值Λ a，即△? =先+4)/; -%，:要求第（i+1)個(gè)時(shí)間片規(guī)劃后所得的加速 度峰值不超過(guò)最大加速度amax。
[0019] 由于周期變加速段的每個(gè)時(shí)間片的θι(]、4、4 (=〇)、4和4 (=如可以看作 是確定量，T2。為固定值，因此可以考慮使用四階多項(xiàng)式對(duì)周期變加速段內(nèi)的第i個(gè)時(shí)間片 的插補(bǔ)量G1 (t)進(jìn)行插值，SP :
[0020] Θ j (t) = ω i0+ ω nt+ ω i2t2+ ω i3t3+ ω i4t4
[0021] 其中ωι()、c〇u、ωι2、ωι3和ω i4為多項(xiàng)式系數(shù)，其約束條件為：
[0023] 求解得到對(duì)周期變加速段內(nèi)的第i個(gè)時(shí)間片的插補(bǔ)量Θ Jt)進(jìn)行插值的函數(shù)為：
[0025] Part5 :加速-勻速過(guò)渡段時(shí)間片的插補(bǔ)序列的計(jì)算和執(zhí)行
[0026] 由于加速-勻速過(guò)渡段的時(shí)間片的
以看作是確定量，因此同樣可以考慮使用四階多項(xiàng)式對(duì)加速-勻速過(guò)渡段的第i個(gè)時(shí)間片 的插補(bǔ)量G1 (t)進(jìn)行插值，SP :
[0027] Θ j (t) = ω i0+ ω nt+ ω i2t2+ ω i3t3+ ω i4t4
[0028] 其中ωι()、c〇u、ωι2、ωι3和ω i4為多項(xiàng)式系數(shù)，其約束條件為：
[0030]