滑雪跳臺修整機械臂軌跡控制方法

劉 拓，李艷杰，卜春光，眭 晉，范曉亮

(1.沈陽理工大學機械工程學院，沈陽 110159；2.中國科學院沈陽自動化研究所機器人學國家重點實驗室，沈陽 110016)

跳臺作為空中技巧項目訓練、比賽場地的核心設(shè)施，對保障運動員訓練具有重要作用。 國內(nèi)外針對跳臺修整主要依賴人工。 由于跳臺空間跨度大、跳臺曲面曲率非線性，人工方法存在以下問題:天氣寒冷、修整人員體力消耗大，工作舒適性差；跳臺為非線性曲面，僅規(guī)定了必要的點位和角度，即關(guān)鍵點表單，沒有標準修整曲線，“尺量鏟修”的人工修整方法難以保證修整精度，對修整人員的技術(shù)要求極高；人工測量難度大，教練員難以定量評估跳臺動態(tài)變化對運動員訓練的影響。

采用機械臂對滑雪跳臺的修整需要解決的問題有:一是要建立一條滑雪跳臺修整規(guī)定的跳臺標準曲線；二是針對標準曲線進行軌跡規(guī)劃，以方便機械臂的修整工作。

針對第一個問題，國內(nèi)外一般采用樣條曲線擬合的解決方案，其中最常用的有三次樣條曲線、B 樣條曲線和NURBS 曲線等[1]。 Yeh 等[2]采用了最小二乘法將離散的數(shù)據(jù)點擬合成連續(xù)的NURBS 曲線，再樣條插補。 Wang 等[3]設(shè)計了NURBS 插補器，可以先將多個關(guān)鍵點連成短線段，然后擬合成NURBS 曲線進行插補。 針對NURBS 無法經(jīng)過部分關(guān)鍵點的問題，袁旭華等[4]通過在B 樣條曲線原有控制點上添加新控制點的方式對傳統(tǒng)B 樣條曲線進行改進，以保證其能通過全部控制點。 董甲甲等[5]通過在B樣條曲線上增加型值點的方式，在不損害曲線原有的連續(xù)性、凸包性等優(yōu)點的情況下，提高了曲線的擬合精度。 張躍明等[6]通過對曲率進行約束，實現(xiàn)了多段B 樣條曲線的二階連續(xù)。 孔慶博等[7]通過在五次多項式插值的不平滑處引入B 樣條插值，使關(guān)節(jié)運動更加穩(wěn)定，有效減少了沖擊和震動。

針對自由曲線的軌跡規(guī)劃問題，王旭浩等[8]在相鄰軌跡段間采用圓弧模型對銜接拐角處平滑過渡，在給定軌跡銜接點坐標和過渡圓弧半徑等參數(shù)的情況下，規(guī)劃出銜接圓弧處的最優(yōu)速度。倪曉偉等[9]通過使用三次B 樣條曲線對多示教點進行擬合，并用復化辛普森積分求取B 樣條曲線的曲線長度和姿態(tài)轉(zhuǎn)角作為參數(shù)進行軌跡規(guī)劃。 Li 等[10]則是將三次B 樣條曲線應(yīng)用到機械臂抓取和放置運動中，通過限制初始速度、終止速度和加速度實現(xiàn)關(guān)節(jié)空間下B 樣條曲線的軌跡規(guī)劃。 陸原超等[11]則構(gòu)造了關(guān)節(jié)空間下的軌跡規(guī)劃函數(shù)，但該方法只適用于關(guān)節(jié)空間，無法應(yīng)用于笛卡爾空間。

基于上述文獻，本文將多路徑點擬合和自由曲線軌跡規(guī)劃相結(jié)合，提出一種通過多路徑點的平滑擬合曲線的梯形波軌跡規(guī)劃方法，并將該方法運用到滑雪跳臺修整，研究內(nèi)容如下:

1)針對修整作業(yè)用機械臂進行運動學建模，通過Matlab 對機械臂運動學模型進行仿真驗證；

2)針對滑雪跳臺模型基于多關(guān)鍵點表示的特點，采用曲線擬合方法確定跳臺修整曲線；

3)針對滑雪跳臺修整曲線為多段相連非線性曲線，長度計算困難的問題，提出丈量法結(jié)合梯形波的修整軌跡規(guī)劃方法。

1 修整機械臂運動學建模

建模包括正向運動學模型和逆向運動學模型。 本文研究的修整機械臂為六自由度工業(yè)機械臂，正運動學建模通常采用Denavit-Hartenberg(D-H)參數(shù)法[12-13]，其逆運動學模型為復雜非線性方程，采用解析方法建立逆向運動學模型。

1.1 D-H 參數(shù)確定

修整機械臂本體和關(guān)節(jié)連桿坐標系定義如圖1 所示，圖中定義了各關(guān)節(jié)連桿坐標系的x軸方向和z軸方向，而y軸方向則通過x軸和z軸的方向共同確定。 根據(jù)D-H 建模方法，確定的D-H 參數(shù)見表1，表中i(i＝1，2，3，4，5，6)為關(guān)節(jié)連桿編號，θi為繞zi軸、從xi-1軸到xi軸所需的角度，di為沿zi軸、從xi-1軸到xi軸所需的距離，ai-1為沿xi-1軸、從zi-1軸到zi軸所需的距離，αi-1為繞xi-1軸、從zi-1軸到zi軸所需的角度。

表1 機械臂D-H 參數(shù)

圖1 機械臂坐標系及D-H 參數(shù)

1.2 正運動學分析

依據(jù)表1，可得相鄰關(guān)節(jié)間的坐標變換矩陣[14]如式(1)所示。

其中:

以上表達式即構(gòu)成了正運動學方程，表示各關(guān)節(jié)角度到機械臂末端執(zhí)行點位姿的變換關(guān)系。

1.3 逆運動學分析

本文采用解析法進行修整機械臂逆向運動學推導。 對式(2)兩邊進行逆轉(zhuǎn)換運算，篩選矩陣兩端合適的對應(yīng)元素進行求解，求出各關(guān)節(jié)角的解。 基于上述思路解得各關(guān)節(jié)角度的表達式如下。

其中:

以上為機械臂逆運動學的解析解，共存在八組，需要確定最優(yōu)解，本文使用構(gòu)型法確定最優(yōu)解。 根據(jù)機械臂六個關(guān)節(jié)的關(guān)節(jié)范圍，從第一關(guān)節(jié)開始到機械臂末端進行逐一限定，通過保持構(gòu)型不變條件，篩選得到最優(yōu)解，如仍然存在多解，則取上一點角度相近的一組解為最優(yōu)解。

1.4 機械臂運動學仿真

本文使用Matlab 仿真軟件對機械臂的運動學進行驗證，分別用Matlab 機器人工具箱和1.2中推導出的機械臂正運動學公式編寫腳本，設(shè)置一組關(guān)節(jié)角度值θ＝[0°， -90°，60°，60°， -60°，-60°]，其結(jié)果對比如圖2 所示。

圖2 正運動學驗證

圖2中modt06 為機器人工具箱所得正運動學矩陣，modmyt06 為利用機械臂正運動學公式得到的結(jié)果，從圖2 中可以看出兩者保持一致，證明了1.2 中推導的正運動學公式的有效性。

將圖2 中的正運動學矩陣代入機械臂逆運動學方程中，可得出針對該姿態(tài)矩陣的解析解，其結(jié)果如圖3 所示。

圖3 逆運動學驗證

圖3中ikine_t 為逆運動學給出的之前由正運動學得出的位姿矩陣的八組解，而ik06 為從該組解中選出的最優(yōu)解，其在八組解中的位置圖中加粗表示，其結(jié)果和正運動學驗證中預先設(shè)置的角度θ相同，由此驗證了該逆運動學算法的正確性。

2 跳臺修整曲線擬合

圖4所示為體育部門提供的滑雪跳臺修整的標準模型，表2 為滑雪跳臺的關(guān)鍵點表單。 為適應(yīng)機器人修整軌跡控制需要，本文需要利用關(guān)鍵點擬合確定跳臺修整曲線，考慮跳臺為柱體形狀，截面曲線形狀相同，利用修整曲線沿著跳臺寬度方向移動，可得到覆蓋跳臺的全部修整曲線。

表2 關(guān)鍵點表單

圖4 滑雪跳臺修整標準模型

為確保擬合后的跳臺曲線能經(jīng)過關(guān)鍵點，本文在每兩個相鄰的關(guān)鍵點之間建立一條三階貝塞爾曲線，即三個關(guān)鍵點由兩條貝塞爾曲線連接，其中間的關(guān)鍵點為兩條曲線交點。 一條三階貝塞爾曲線由首尾點和兩個控制點組成，相鄰關(guān)鍵點為其首尾點，以兩條貝塞爾曲線在交點處一階導數(shù)相等為限制條件，確定其與交點相鄰的控制點，從而確定各條三階貝塞爾曲線。 確定貝塞爾曲線控制點的過程如圖5 所示。

圖5 貝塞爾曲線確定控制點

假設(shè)P0和Pn為圖4 中跳臺的關(guān)鍵點9 和跳臺終點，Pi-2、Pi-1、Pi、Pi＋1、Pi＋2均為跳臺控制點，也為兩條三階貝塞爾曲線的交點。 以Pi為例，若達到其兩端的兩條三階貝塞爾曲線在該點處平滑，要求兩曲線在該點的切線斜率相同，則過Pi點作曲線的切線，將位于Pi左側(cè)的貝塞爾曲線控制點Bi-1和位于Pi右側(cè)的貝塞爾曲線控制點Ai均取在過Pi點所作的切線上。 連接Pi-1和Pi＋1作直線，使過Pi點的切線的斜率與直線Pi-1Pi＋1的斜率相等，此時控制點Ai、Bi-1的坐標可以表示為

式中:(xi-1，yi-1)、(xi，yi)、(xi＋1，yi＋1)分別為Pi-1、Pi和Pi＋1坐標；k1、k2為任意給定的正數(shù)。

同理可以得到Pi-1兩側(cè)控制點Ai-1、Bi-2。 通過Pi、Pi-1、Ai-1和Bi-1可確定Pi和Pi-1兩點之間的三階貝塞爾曲線。

通過上述方法，可以獲得包括跳臺終點在內(nèi)的10 個關(guān)鍵點形成的9 條曲線中的7 條。 但也存在特殊情況，在求取P0右側(cè)控制點A0時，如使用公式(30)，則需要P0左側(cè)存在關(guān)鍵點P-1，但P-1點不存在，因此需要用P0的值作為P-1的值，可得P0右側(cè)控制點A0的坐標為

式中:(x0，y0)、(x1，y1)分別為P0、P1的坐標；k0為任意給定的正數(shù)。

同理，在求取Pn的左側(cè)控制點Bn-1時，如使用公式(31)，則需要關(guān)鍵點Pn的右側(cè)存在關(guān)鍵點Pn＋1，該點同樣不存在。 因此需要用Pn的值作為Pn＋1的值，可得Pn左側(cè)控制點Bn-1的坐標為

式中:(xn，yn)、(xn-1，yn-1)分別為Pn、Pn-1的坐標；kn為任意給定的正數(shù)。

通過上述方法得到滑雪跳臺的參考曲線如圖6 所示。

圖6 滑雪跳臺參考曲線

由圖6 可見，整個曲線由9 段不同曲線組成，其整體首尾相連，其中實心點為關(guān)鍵點。 根據(jù)該曲線得出的跳臺三維模型如圖7 所示。

3 跳臺修整軌跡規(guī)劃算法

機器人修整軌跡規(guī)劃是在保證運動速度平滑的前提條件下，確定修整機械臂在每個運動控制周期的位姿，以實現(xiàn)跳臺修整的精確性、修整運動的穩(wěn)定性和機械臂的安全性。

因為整體曲線是由多段三階貝塞爾曲線擬合而成，所以需要進行整體曲線的速度規(guī)劃，不能分段規(guī)劃，否則在兩段曲線的連接點處會出現(xiàn)速度不一致的問題，為此本文提出采用丈量法進行修整軌跡規(guī)劃。

丈量法是利用基于梯形波速度規(guī)劃確定的軌跡點間隔為半徑，通過丈量圓與修整曲線的交點確定機械臂修整運動軌跡的方法。 丈量法實現(xiàn)簡單，不僅滿足了速度平滑要求，同時也避免了非線性貝塞爾曲線求解曲線長度的難題。 基于丈量法的修整軌跡規(guī)劃算法示意圖如圖8 所示。

圖8 丈量法示意圖

在運用丈量法過程中，首先需要確定丈量圓的原點和半徑。 設(shè)跳臺修整曲線上某點Pk(xk，yk)為丈量圓原點，為確定丈量圓半徑，假設(shè)速度為v，給定加速度為a，給定時間間隔為te，最大速度為vmax。 基于梯形波規(guī)劃的機械臂修整軌跡方程為

式(37)中基于梯形波速度規(guī)劃確定的軌跡點間隔長度s即為丈量圓的半徑。

在得到原點和丈量圓的半徑后，以Pk(xk，yk)為圓心，以s為半徑畫圓，該丈量圓與貝塞爾曲線f(x(t)，y(t))相交，可得以當前貝塞爾曲線參數(shù)t為未知數(shù)的一元二次方程式如下。

采用牛頓迭代法[15]對式(38)進行求解得出未知數(shù)t的當前值，將其代入當前貝塞爾曲線參數(shù)方程即可得出該圓弧與當前貝塞爾曲線f(x(t)，y(t))的交點，記為Pk＋1(xk＋1，yk＋1)，即符合梯形加減速軌跡規(guī)劃的軌跡點。

以點Pk＋1(xk＋1，yk＋1)為新的原點，以新的軌跡間隔長度為半徑，可得新的交點，重復這一過程得出整條曲線的速度軌跡，且速度變化符合梯形加減速變化。

因為跳臺修整曲線由多段三階貝塞爾曲線頭尾相連，所以當丈量法進行到兩段三階貝塞爾曲線的交點處時，因半徑長度大于圓心到該段曲線末點的距離，故無法產(chǎn)生有效交點，此時應(yīng)以該點為圓心，以當前路程為半徑畫圓，求與下一段三階貝塞爾曲線的交點，以解決由多段貝塞爾曲線連接構(gòu)成的修整曲線的軌跡規(guī)劃問題。

針對上述算法，利用Matlab 軟件畫出滑雪跳臺軌跡規(guī)劃曲線，如圖9 所示。

圖9 滑雪跳臺軌跡規(guī)劃曲線

由圖9 可知，通過丈量法進行軌跡規(guī)劃的曲線與圖6 所示滑雪跳臺參考曲線保持一致，其整體軌跡誤差值保持在0.02 mm 以內(nèi)。 其速度和加速度曲線分別如圖10 和圖11 所示。

圖10 軌跡規(guī)劃速度曲線

圖11 軌跡規(guī)劃加速度曲線

從圖10 和圖11 可以看出，軌跡規(guī)劃后整體速度符合梯形加減速的要求。

4 實驗與分析

4.1 實驗系統(tǒng)與流程

在仿真驗證的基礎(chǔ)上，課題組在沈陽市白清寨空中技巧訓練基地通過現(xiàn)場應(yīng)用試驗，對自由式滑雪跳臺修整機械臂軌跡控制算法進行測試。測試結(jié)果通過量測和目視兩種方法確定。

如圖12 所示，試驗用滑雪跳臺為自由式滑雪跳臺的單周臺，該滑雪跳臺的關(guān)鍵點表單如表2所示。 本文實驗系統(tǒng)由移動平臺、機械臂和修整工具、視覺定位系統(tǒng)、地面指揮系統(tǒng)構(gòu)成。

實驗過程如下:

1)控制機器人進入試驗場地、接近單周跳臺，使跳臺處于修整機械臂工作空間內(nèi)；

2)開啟視覺定位系統(tǒng)測量跳臺位姿，將位姿數(shù)據(jù)發(fā)送給機械臂；

3)控制機械臂以直線運動方式移動到目標修整點；4)控制機械臂開始進行滑雪跳臺修整作業(yè)；5)作業(yè)結(jié)束后，機械臂自動回到工作姿態(tài)。

4.2 實驗結(jié)果與分析

圖13所示為一條軌跡修整完成后的跳臺，其中數(shù)字1 為機械臂修整跳臺曲面，數(shù)字2 為人工修整跳臺曲面。

圖13 機械臂實際修整雪面

1)量測法。 采用人工修整測量，用高精度水平儀針對滑雪跳臺機器人修整的曲面進行測量。高精度數(shù)顯水平儀如圖14 所示，水平尺的長度為60 cm。

圖14 高精度數(shù)顯水平儀

對應(yīng)圖4 中各關(guān)鍵點測量得到的相對大地水平面的角度值見表3。

表3 量測法誤差(°)

人工修整標準要求關(guān)鍵點角度誤差小于正負0.3°。 由表2 可見，全部關(guān)鍵點的角度值誤差均小于人工修整誤差的要求。 量測法顯示，機器人修整誤差滿足修整精度要求。

2)目視法。 圖13 為機械臂修整與人工修整的效果對比，從圖中可以明顯看出，人工修整的跳臺表面相對粗糙，整體可見起伏，而機械臂修整的跳臺表面平滑、整體無可見起伏，目視結(jié)果顯示，機器人修整效果優(yōu)于人工。

綜合量測、目視兩種方法的評估，表明機械臂跳臺修整效果優(yōu)于人工方法，本文自由式滑雪跳臺修整機械臂軌跡控制方法有效。

5 結(jié)論

針對自由式滑雪跳臺修整自動化需要，研究了機械臂軌跡控制算法。

1)采用三階貝塞爾曲線擬合方法，將由關(guān)鍵點表達的標準跳臺模型轉(zhuǎn)換為由曲線表示的跳臺模型。

2)針對由多段貝塞爾曲線構(gòu)成的修整曲線，研究了丈量法軌跡規(guī)劃算法，降低了規(guī)劃難度。

3)采用D-H 建模方法，建立了修整機械臂的正逆運動學模型，打通了機械臂軌跡規(guī)劃和控制的通道。

4)借助視覺定位技術(shù)，進行了跳臺修整控制算法的現(xiàn)場測試并對比人工修整效果，驗證了機械臂修整軌跡控制研究的有效性。