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

(1.沈陽理工大學(xué)機(jī)械工程學(xué)院，沈陽 110159；2.中國科學(xué)院沈陽自動化研究所機(jī)器人學(xué)國家重點(diǎn)實(shí)驗(yàn)室，沈陽 110016)

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

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

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

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

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

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

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

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

1 修整機(jī)械臂運(yùn)動學(xué)建模

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

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

修整機(jī)械臂本體和關(guān)節(jié)連桿坐標(biāo)系定義如圖1 所示，圖中定義了各關(guān)節(jié)連桿坐標(biāo)系的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 機(jī)械臂D-H 參數(shù)

圖1 機(jī)械臂坐標(biāo)系及D-H 參數(shù)

1.2 正運(yùn)動學(xué)分析

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

其中:

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

1.3 逆運(yùn)動學(xué)分析

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

其中:

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

1.4 機(jī)械臂運(yùn)動學(xué)仿真

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

圖2 正運(yùn)動學(xué)驗(yàn)證

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

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

圖3 逆運(yùn)動學(xué)驗(yàn)證

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

2 跳臺修整曲線擬合

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

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

圖4 滑雪跳臺修整標(biāo)準(zhǔn)模型

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

圖5 貝塞爾曲線確定控制點(diǎn)

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

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

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

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

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

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

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

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

圖6 滑雪跳臺參考曲線

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

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

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

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

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

圖8 丈量法示意圖

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

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

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

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

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

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

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

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

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

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

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

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

4 實(shí)驗(yàn)與分析

4.1 實(shí)驗(yàn)系統(tǒng)與流程

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

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

實(shí)驗(yàn)過程如下:

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

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

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

4.2 實(shí)驗(yàn)結(jié)果與分析

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

圖13 機(jī)械臂實(shí)際修整雪面

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

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

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

表3 量測法誤差(°)

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

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

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

5 結(jié)論

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

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

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

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

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