鍵已松開，直接跳轉(zhuǎn)至Step4,進(jìn)入停止段，點(diǎn)動(dòng)軸不動(dòng)作；否則，控 制程序?qū)㈤_啟插補(bǔ)序列的執(zhí)行程序，并繼續(xù)進(jìn)行插補(bǔ)序列的計(jì)算。
[0070] 對(duì)加速&勻速段增量函數(shù)取N個(gè)插補(bǔ)時(shí)刻t = 0, T，2T，…（N-I) T，得到加速&勻 速段的N個(gè)插補(bǔ)增量Λ k(k = 1，2,…，N);記錄最后插補(bǔ)點(diǎn)的增量大小Λ N和增量速度。 對(duì)停止段增量函數(shù)取m+1個(gè)插補(bǔ)點(diǎn)時(shí)刻t = 0, Τ, 2Τ,…（m-l)T, Λ t，得到停止段的m+1個(gè) 插補(bǔ)增量八，& = N+l，N+2,…，N+m，Ν+m+l)，第一個(gè)時(shí)間片的起始增量和起始增量速度均為 0,對(duì)于周期變加速段：時(shí)間片的中點(diǎn)增量加速度不能超過運(yùn)動(dòng)允許的最大加速度a_; 第一個(gè)時(shí)間片的中點(diǎn)增量加速度可以取一個(gè)較小值，舉例：〇. 〇3Xa_;相鄰兩個(gè)時(shí)間片加 速度遞增量可以取一個(gè)合適值，舉例：〇. 〇5Xa_。
[0071] 對(duì)于關(guān)節(jié)坐標(biāo)系空間下的單軸點(diǎn)動(dòng)，周期變加速段時(shí)間片的中點(diǎn)增量加速度尾不 能超過點(diǎn)動(dòng)軸運(yùn)動(dòng)允許的最大角加速度或線加速度，加速-勻速過渡段時(shí)間片的終止增量 速度卷對(duì)應(yīng)為指令給定的點(diǎn)動(dòng)軸期望運(yùn)動(dòng)速度，停止段的中點(diǎn)增量減速度| A對(duì)應(yīng)點(diǎn)動(dòng)軸運(yùn) 動(dòng)允許的最大角減速度或線減速度。由Pc+ Λ 14可以計(jì)算得到第一個(gè)時(shí)間片的N個(gè)加速& 勻速段插補(bǔ)序列點(diǎn)的軸關(guān)節(jié)給定值，以及與該時(shí)間片緊接的m+1個(gè)停止段插補(bǔ)序列點(diǎn)的軸 關(guān)節(jié)給定值。
[0072] St印3 :執(zhí)行已經(jīng)計(jì)算好的時(shí)間片插補(bǔ)序列，同步返回St印2計(jì)算下一個(gè)時(shí)間片插 補(bǔ)序列。
[0073] 執(zhí)行線程讀取并執(zhí)行已經(jīng)計(jì)算好的當(dāng)前時(shí)間片加速（勻速）段序列；與此同時(shí)，計(jì) 算線程開始計(jì)算下一時(shí)間片的兩段插補(bǔ)序列，記錄兩段中的加速（勻速）段最后插補(bǔ)點(diǎn)的 增量大小和增量速度，并更新點(diǎn)動(dòng)過程的運(yùn)動(dòng)階段狀態(tài)。
[0074] 更新點(diǎn)動(dòng)過程的運(yùn)動(dòng)階段狀態(tài)需依據(jù)圖1的狀態(tài)流程圖導(dǎo)向，插補(bǔ)序列計(jì)算過程 中，下一時(shí)間片的起始增量大小和角速度/線速度分別為當(dāng)前時(shí)間片加速（勻速）段最后 插補(bǔ)點(diǎn)的增量大小和角速度/線速度。
[0075] Step4 :點(diǎn)動(dòng)結(jié)束
[0076] 注銷計(jì)算進(jìn)程；若開啟了執(zhí)行線程，等待停止段序列執(zhí)行完畢后，注銷執(zhí)行線程。 等待下一次點(diǎn)動(dòng)請(qǐng)求。
[0077] (2)笛卡爾坐標(biāo)系空間下的移動(dòng)和轉(zhuǎn)動(dòng)點(diǎn)動(dòng)（圖8)
[0078] 笛卡爾坐標(biāo)系空間下的點(diǎn)動(dòng)以笛卡爾坐標(biāo)系為參考坐標(biāo)系，機(jī)器人工具末端點(diǎn) TCP可以是沿坐標(biāo)軸X/Y/Z進(jìn)行+X/-X/+Y/-Y/+Z/-Z方向的移動(dòng)點(diǎn)動(dòng)，也可以是繞著坐標(biāo)軸 X/Y/Z進(jìn)行+RX/-RX/+Ry/-Ry/+Rz/-Rz方向的轉(zhuǎn)動(dòng)點(diǎn)動(dòng)。具體步驟如下：
[0079] Step 1 :記錄機(jī)器人TCP的初始位姿
[0080] 在接收到用戶點(diǎn)動(dòng)請(qǐng)求后，首先記錄各關(guān)節(jié)位置，通過求解機(jī)器人的運(yùn)動(dòng)學(xué)正解， 得到機(jī)器人當(dāng)前工具末端點(diǎn)TCP的4X4變換矩陣為
，R。為表示姿態(tài)的 3X3坐標(biāo)旋轉(zhuǎn)矩陣，p。為表示位置的3X 1位置偏移向量。
[0081] St印2 :計(jì)算第一個(gè)時(shí)間片的插補(bǔ)序列
[0082] 首先需要根據(jù)是移動(dòng)點(diǎn)動(dòng)還是轉(zhuǎn)動(dòng)點(diǎn)動(dòng)，明確點(diǎn)動(dòng)的規(guī)劃對(duì)象。
[0083] 若是移動(dòng)點(diǎn)動(dòng)，規(guī)劃對(duì)象為TCP在坐標(biāo)軸X/Y/Z方向上的位置。以沿著X軸正方 向移動(dòng)為例，若沿+X方向上的位置增量為A jU = 1，2, "·，Ν)，則移動(dòng)維度上產(chǎn)生的路徑位 置偏移增量為
；轉(zhuǎn)動(dòng)維度上沒有變化，對(duì)應(yīng)的旋轉(zhuǎn)矩陣增量為單位矩陣R,= I3x3。因此，增量變換矩陣為
。1運(yùn)動(dòng)的參考坐標(biāo)系為笛卡爾坐標(biāo)系，插補(bǔ)點(diǎn) 的對(duì)應(yīng)變換矩陣為原變換矩陣Τ。左乘增量變換矩陣Λ T j，即：
[0085] 若是轉(zhuǎn)動(dòng)點(diǎn)動(dòng)，規(guī)劃對(duì)象為TCP繞著坐標(biāo)軸X/Y/Z的正或反方向旋轉(zhuǎn)的角度大小。 以繞著X軸正方向轉(zhuǎn)動(dòng)為例，若繞+X方向的旋轉(zhuǎn)角度增量為l〇 V2，···,均，則轉(zhuǎn)動(dòng)維度 上產(chǎn)生的旋轉(zhuǎn)矩陣增量Δ/), =/^#(A,);移動(dòng)維度上沒有變化，對(duì)應(yīng)的位置偏移增量為零 向量：。.因此，增量變換矩陣為
。運(yùn)動(dòng)的參考坐標(biāo)系為笛卡爾 坐標(biāo)系，對(duì)應(yīng)插補(bǔ)點(diǎn)的對(duì)應(yīng)變換矩陣為原變換矩陣Τ。左乘增量變換矩陣Δ?，即：
[0087] 明確規(guī)劃對(duì)象后，本步驟的具體內(nèi)容可以描述為：開啟計(jì)算線程，計(jì)算第一個(gè)時(shí)間 片的兩段插補(bǔ)序列，并記錄兩段序列中加速&勻速段（即周期變加速段）的最后插補(bǔ)點(diǎn)的 增量大小以及增量速度。若兩段插補(bǔ)序列中存在不可達(dá)點(diǎn)（超出機(jī)器人可達(dá)范圍或?qū)l(fā)生 碰撞），或檢測(cè)到此時(shí)按鍵已松開，跳轉(zhuǎn)到Step 5,機(jī)器人不動(dòng)作。否則控制程序?qū)㈤_啟插 補(bǔ)序列的執(zhí)行線程，進(jìn)入Step 3。
[0088] 兩段插補(bǔ)序列插補(bǔ)增量的計(jì)算方法與第一種情況計(jì)算兩段插補(bǔ)序列插補(bǔ)增量的 方法是相同的。利用移動(dòng)點(diǎn)動(dòng)計(jì)算式或轉(zhuǎn)動(dòng)點(diǎn)動(dòng)計(jì)算式可以計(jì)算各個(gè)插補(bǔ)點(diǎn)的變換矩陣， 通過求解機(jī)器人的運(yùn)動(dòng)學(xué)逆解，可以得到機(jī)器人的各軸關(guān)節(jié)給定值，作為某個(gè)插補(bǔ)點(diǎn)的一 組插補(bǔ)序列；因而，可以計(jì)算得到第一個(gè)時(shí)間片的N組加速（勻速）段插補(bǔ)序列點(diǎn)的軸關(guān)節(jié) 給定值，以及與該時(shí)間片緊接的m+1組停止段插補(bǔ)序列點(diǎn)的軸關(guān)節(jié)給定值。
[0089] Step 3 :執(zhí)行已經(jīng)計(jì)算好的時(shí)間片插補(bǔ)序列，同步計(jì)算下一個(gè)時(shí)間片插補(bǔ)序列。
[0090] 執(zhí)行線程讀取并執(zhí)行已經(jīng)計(jì)算好的當(dāng)前時(shí)間片加速&勻速段序列；與此同時(shí)，計(jì) 算線程開始計(jì)算下一時(shí)間片的兩段插補(bǔ)序列，記錄兩段中的加速&勻速段最后插補(bǔ)點(diǎn)的增 量大小和增量速度，并更新點(diǎn)動(dòng)過程的運(yùn)動(dòng)階段狀態(tài)。
[0091] 更新點(diǎn)動(dòng)過程的運(yùn)動(dòng)階段狀態(tài)需依據(jù)圖1的狀態(tài)流程圖導(dǎo)向，插補(bǔ)序列計(jì)算過程 中，下一時(shí)間片的增量大小以及增量速度分別為當(dāng)前時(shí)間片加速&勻速段最后插補(bǔ)點(diǎn)的增 量大小以及增量速度。
[0092] St印4 :重復(fù)St印3,循環(huán)執(zhí)行與計(jì)算時(shí)間片插補(bǔ)序列，直到滿足停止條件。
[0093] 下一個(gè)時(shí)間片兩段插補(bǔ)序列計(jì)算完畢后，若各組序列點(diǎn)下機(jī)器人均處于可達(dá)范圍 內(nèi)，計(jì)算線程等待執(zhí)行線程將當(dāng)前時(shí)間片的加速&勻速段序列插補(bǔ)點(diǎn)執(zhí)行完畢；當(dāng)執(zhí)行完 畢后，若按鍵仍處于按下狀態(tài)，返回并重復(fù)Step 3進(jìn)行后續(xù)時(shí)間片插補(bǔ)序列的同步計(jì)算與 執(zhí)行。若下一個(gè)時(shí)間片的兩段插補(bǔ)序列存在不可達(dá)點(diǎn)，或執(zhí)行完畢后檢測(cè)到按鍵松開，注銷 計(jì)算線程，下一時(shí)間片開始執(zhí)行當(dāng)前時(shí)間片緊接的停止段序列。
[0094] Step 5 :點(diǎn)動(dòng)結(jié)束。
[0095] 注銷計(jì)算進(jìn)程；若開啟了執(zhí)行線程，等待停止段序列執(zhí)行完畢后，注銷執(zhí)行線程。 等待下一次點(diǎn)動(dòng)請(qǐng)求。
[0096] (3)工具坐標(biāo)系空間下的移動(dòng)和轉(zhuǎn)動(dòng)點(diǎn)動(dòng)（圖9)
[0097] 工具系空間下的點(diǎn)動(dòng)以工具坐標(biāo)系為參考坐標(biāo)系，機(jī)器人工具末端點(diǎn)TCP可以是 沿工具坐標(biāo)系坐標(biāo)軸X/Y/Z進(jìn)行+X/-X/+Y/-Y/+Z/-Z方向的移動(dòng)點(diǎn)動(dòng)，也可以是繞著工具 坐標(biāo)軸X/Y/Z進(jìn)行+Rx/-RX/+Ry/-Ry/+RZ/-Rz方向的轉(zhuǎn)動(dòng)點(diǎn)動(dòng)。
[0098] 具體步驟與第（2)種情況笛卡爾坐標(biāo)系空間下移動(dòng)和轉(zhuǎn)動(dòng)點(diǎn)動(dòng)的實(shí)現(xiàn)步驟Step