求解得到對加速-勻速過渡段的第i個(gè)時(shí)間片的插補(bǔ)量Θ i (t)進(jìn)行插值的函數(shù) 為：
[0032] Part6 :勻速段時(shí)間片的插補(bǔ)序列的計(jì)算和執(zhí)行
[0033] 由于勻速段的時(shí)間片的Θ i。和&可以看作是確定量，因此同樣可以考慮使用一次 線性多項(xiàng)式對勻速段內(nèi)的第i個(gè)時(shí)間片的插補(bǔ)量Θ Jt)進(jìn)行插值，BP :
[0034] Θ j (t) = ω ?0+ω nt
[0035] 其中U1。和ω η為多項(xiàng)式系數(shù)，其約束條件為：
[0037] 求解得到對勻速段內(nèi)的第i個(gè)時(shí)間片的插補(bǔ)量Θ Jt)進(jìn)行插值的函數(shù)為：
[0038] 4(/) = 4+4#
[0039] Part7 :停止段的插補(bǔ)序列的計(jì)算和執(zhí)行
[0040] 若點(diǎn)動操作結(jié)束，則注銷計(jì)算進(jìn)程；若開啟了執(zhí)行線程，等待停止段序列執(zhí)行完畢 后，注銷執(zhí)行線程，等待下一次點(diǎn)動請求。
[0041] 一旦發(fā)現(xiàn)點(diǎn)動操作結(jié)束、超出機(jī)器人可達(dá)范圍或發(fā)生碰撞，則直接跳轉(zhuǎn)到停止段； 在停止段，點(diǎn)動操作以運(yùn)行時(shí)間最短而且平滑的方式將速度降為0,在停止段不再使用時(shí)間 片進(jìn)行分割，而使用運(yùn)行允許的最大減速度為正值）作為減速度峰值進(jìn) 行規(guī)劃。
[0042] 由于停止段的Θ。。、'、& (=〇)、I （=Q)和成,可以看做 是確定量，但是運(yùn)行時(shí)間Λ t是一個(gè)未知變量，同樣可以考慮使用四階多項(xiàng)式對停止段的 插補(bǔ)量Q〇(t)進(jìn)行插值，BP :
[0043] Θ 〇 (t) = ω 00+ ω 01t+ ω 02t2+ ω 03t3+ ω 04t4
[0044] 其中ω。。、ω^、（〇。2、（〇。3和ω。4為多項(xiàng)式系數(shù)，其約束條件為：
[0049] 有益效果：本發(fā)明提供的機(jī)器人點(diǎn)動操作的速度控制方法，相對于現(xiàn)有技術(shù)，具有 如下優(yōu)勢：1、改善點(diǎn)動操作的運(yùn)動性能。相比于傳統(tǒng)控制器的點(diǎn)動解決方案，本發(fā)明點(diǎn)動的 規(guī)劃結(jié)果加速度曲線連續(xù)，速度平滑，能夠有效降低點(diǎn)動過程中的機(jī)械沖擊與磨損，延長設(shè) 備壽命；點(diǎn)動初始階段加速度較小并逐漸加大，能夠?qū)崿F(xiàn)微小位移點(diǎn)動，更便于用戶對機(jī)器 人進(jìn)行示教操作；以時(shí)間片為單位，規(guī)劃過程充分考慮點(diǎn)動能否在可達(dá)范圍內(nèi)停止，在算法 層面避免了機(jī)器人高速點(diǎn)動容易越出運(yùn)動軟限位的問題。2、具有通用性。本發(fā)明并沒有涉 及到機(jī)器人的具體類型，對機(jī)器人示教的點(diǎn)動操作實(shí)現(xiàn)方法具有通用性。
【附圖說明】
[0050] 圖1為點(diǎn)動軌跡規(guī)劃的狀態(tài)跳轉(zhuǎn)示意圖。程序最初處于等待點(diǎn)動請求狀態(tài)，當(dāng)檢 測到按鍵信號，跳轉(zhuǎn)到周期變加速段狀態(tài)；若按鍵時(shí)間較長，將通過加速-勻速過渡段進(jìn)入 勻速段狀態(tài)。過程中，一旦檢測到按鍵松開或有不可達(dá)插補(bǔ)點(diǎn)，將進(jìn)入停止段狀態(tài)。圖中： ①檢測到點(diǎn)動請求信號；②下一時(shí)間片加速度遞增（但不得超過最大加速度），規(guī)劃所得的 終止速度仍小于給定勻速段速度；③下一時(shí)間片加速度若遞增（但不得超過最大加速度）， 規(guī)劃所得的終止速度將超過給定勻速段速度；④未收到點(diǎn)動結(jié)束請求，且下一時(shí)間片運(yùn)動 可達(dá)；⑤收到點(diǎn)動結(jié)束請求，或下一時(shí)間片有不可達(dá)點(diǎn)。
[0051] 圖2為本發(fā)明的四階段插補(bǔ)增量及其變化速度、變化加速度和變化加加速度的規(guī) 劃示意圖。
[0052] 圖3為本發(fā)明的用戶主動停止情形下，計(jì)算線程與執(zhí)行線程的同步舉例。
[0053] 圖4為本發(fā)明的有不可達(dá)點(diǎn)情形下，計(jì)算線程與執(zhí)行線程的同步舉例。
[0054] 圖5為本發(fā)明點(diǎn)動過程中各個(gè)時(shí)間片插補(bǔ)序列的計(jì)算與執(zhí)行情況。圖中：①初始 化：接收到點(diǎn)動請求，開始計(jì)算第一個(gè)時(shí)間片的兩段插補(bǔ)點(diǎn)序列；若計(jì)算成功，開始執(zhí)行序 列。②時(shí)間片間過渡："繼續(xù)運(yùn)行標(biāo)志位"有效，即檢測到下一時(shí)間片的兩段插補(bǔ)點(diǎn)序列計(jì) 算完成，說明這些路徑點(diǎn)均可達(dá)；于是丟棄上一時(shí)間片計(jì)算得到的停止段序列，并執(zhí)行新的 加速（勻速）段。③停止信號："繼續(xù)運(yùn)動標(biāo)志位"被清零。④停止："繼續(xù)運(yùn)動標(biāo)志位"失 效，不再計(jì)算新的兩段序列，下階段執(zhí)行停止段序列。
[0055] 圖6為本發(fā)明兩種不同的點(diǎn)動停止（用戶主動停止/有不可達(dá)點(diǎn)）條件下插補(bǔ)序 列的計(jì)算與執(zhí)行結(jié)果。
[0056] 圖7以垂直六軸關(guān)節(jié)型工業(yè)機(jī)器人為例，展示關(guān)節(jié)坐標(biāo)系空間下單軸點(diǎn)動操作的 示意圖。
[0057] 圖8為笛卡爾坐標(biāo)系空間下，機(jī)器人工具末端點(diǎn)TCP沿著笛卡爾坐標(biāo)系的X、Y或 Z軸移動，或繞著笛卡爾坐標(biāo)系的X、Y或Z軸轉(zhuǎn)動，進(jìn)行點(diǎn)動操作的示意圖。
[0058] 圖9為工具坐標(biāo)系空間下，機(jī)器人工具末端點(diǎn)TCP沿著工具坐標(biāo)系的X、Y或Z軸 移動，或繞著工具坐標(biāo)系的X、Y或Z軸轉(zhuǎn)動，進(jìn)行點(diǎn)動操作的示意圖。
【具體實(shí)施方式】
[0059] 下面結(jié)合附圖對本發(fā)明作更進(jìn)一步的說明。
[0060] 本發(fā)明實(shí)施的關(guān)鍵在于控制程序?qū)C(jī)器人點(diǎn)動插補(bǔ)序列的計(jì)算線程與執(zhí)行線程 的同步管理。當(dāng)控制器接收到點(diǎn)動請求信號時(shí)，程序相繼開啟插補(bǔ)序列的計(jì)算線程與執(zhí)行 行程；循環(huán)計(jì)算與執(zhí)行若干個(gè)加速&勻速段時(shí)間片的插補(bǔ)序列后，當(dāng)檢測到用戶主動停止 點(diǎn)動（松開按鍵）或機(jī)器人運(yùn)動有不可達(dá)點(diǎn)（超出機(jī)器人可達(dá)范圍或?qū)l(fā)生碰撞），執(zhí)行停 止段序列，依次注銷計(jì)算進(jìn)程與執(zhí)行線程，等待下一次點(diǎn)動請求。時(shí)間片的長度對程序而言 是固定的，需要程序開發(fā)人員根據(jù)控制器實(shí)際情況決定，舉例：某控制器實(shí)時(shí)周期性控制時(shí) 鐘為8毫秒，可考慮以20個(gè)周期為一個(gè)時(shí)間片，即每個(gè)時(shí)間片T 2。為160毫秒。
[0061] 圖3和圖4分別是用戶主動停止情形下、有不可達(dá)點(diǎn)情形下，采用本發(fā)明技術(shù)方案 的實(shí)現(xiàn)步驟，計(jì)算行程與執(zhí)行線程同步的典型舉例。在運(yùn)行時(shí)間片內(nèi)，當(dāng)執(zhí)行線程接收到按 鍵松開消息或計(jì)算失敗消息，會將"繼續(xù)運(yùn)動標(biāo)志位"清零，下個(gè)時(shí)間片決策為執(zhí)行停止段 插補(bǔ)序列。下一時(shí)間片到底是執(zhí)行新的加速（勻速）段序列，還是執(zhí)行當(dāng)前時(shí)間片緊接的 停止段序列，是控制程序在每個(gè)時(shí)間片插補(bǔ)序列執(zhí)行結(jié)束時(shí)通過讀取"繼續(xù)運(yùn)動標(biāo)志位"的 狀態(tài)決定的。點(diǎn)動過程中各個(gè)時(shí)間片插補(bǔ)序列的計(jì)算與執(zhí)行結(jié)果如圖5和圖6所示。
[0062] 下面以關(guān)節(jié)坐標(biāo)系空間下的單軸點(diǎn)動（圖7)、笛卡爾坐標(biāo)系空間下的移動和轉(zhuǎn)動 點(diǎn)動（圖8)、以及工具坐標(biāo)系空間下的移動和轉(zhuǎn)動點(diǎn)動（圖9)來進(jìn)一步介紹本發(fā)明的實(shí)現(xiàn) 步驟。
[0063] 使用N表示每個(gè)時(shí)間片包含的周期數(shù)，舉例：N = 20 ;使用m表示停止段占用的周 期數(shù)
Λ t為停止段的運(yùn)行時(shí)間；T為控制周期時(shí)間，舉例：T = 8ms)。
[0064] (1)關(guān)節(jié)坐標(biāo)系空間下的單軸點(diǎn)動（圖7)
[0065] Stepl :記錄點(diǎn)動軸的初始位置或關(guān)節(jié)角度P0;
[0066] Step2 :計(jì)算插補(bǔ)序列
[0067] 對周期變加速段內(nèi)的第i個(gè)時(shí)間片的插補(bǔ)量Θ i⑴進(jìn)行插值的函數(shù)為
對加速-勻速過渡段的第i個(gè)時(shí)間片的插補(bǔ)量S1U)進(jìn)行 插值的函數(shù)為
.對勻速段內(nèi)的第i個(gè)時(shí)間片的插補(bǔ)量 S1⑴進(jìn)行插值的函數(shù)為W= %焉，對停止段的插補(bǔ)量Θ⑴進(jìn)行插值的函數(shù)為
[0069] 若在第i個(gè)時(shí)間片兩端插補(bǔ)序列存在不可達(dá)點(diǎn)（超出點(diǎn)動軸限位或?qū)?dǎo)致機(jī)器人 碰撞），或檢測到此時(shí)按