王秋玥，方明，張海濤

（長春理工大學(xué) 計(jì)算機(jī)科學(xué)技術(shù)學(xué)院，長春 130022）

機(jī)器人手臂軌跡規(guī)劃是當(dāng)今服務(wù)控制領(lǐng)域研究的熱點(diǎn)。針對(duì)機(jī)器人軌跡規(guī)劃方法可以在笛卡爾空間和在關(guān)節(jié)空間進(jìn)行，在笛卡爾空間使用逆運(yùn)動(dòng)學(xué)求解直接控制NAO機(jī)器人的末端效應(yīng)器［1］。此空間的軌跡規(guī)劃比較直觀，但是由于冗余運(yùn)動(dòng)，規(guī)劃過程中容易使機(jī)器人產(chǎn)出奇異位形；關(guān)節(jié)空間直接控制各個(gè)關(guān)節(jié)的角度，能保證運(yùn)動(dòng)路徑的唯一性，但計(jì)算量很大，導(dǎo)致控制時(shí)間延長，并且末端效應(yīng)器的運(yùn)動(dòng)軌跡不直觀［2-4］。

本文提出用笛卡兒空間和關(guān)節(jié)空間相結(jié)合控制方法進(jìn)行軌跡規(guī)劃。在抓取目標(biāo)大小已知條件下，利用關(guān)節(jié)空間控制方法預(yù)先設(shè)定各個(gè)關(guān)節(jié)的抓取角度，保證手臂抓取的最佳姿態(tài)，再用笛卡兒空間控制方法進(jìn)行軌跡插值從而抓取目標(biāo)。

1 混合空間描述

1.1 笛卡兒空間位置控制

在笛卡爾坐標(biāo)空間中進(jìn)行軌跡規(guī)劃是指將手部的姿態(tài)表示成時(shí)間的函數(shù)，相應(yīng)的關(guān)節(jié)位移、速度和加速度由手部的信息導(dǎo)出［5］。笛卡兒空間對(duì)NAO機(jī)器人進(jìn)行控制使用NAOqi中API函數(shù)（如position-Interpolation（）是專門用來在笛卡爾空間中使用逆運(yùn)動(dòng)學(xué)求解直接控制NAO機(jī)器人的效應(yīng)器。）效應(yīng)器在NAO機(jī)器人中提前預(yù)定義三維點(diǎn)產(chǎn)生在生成鏈的尾部，如圖1所示，根據(jù)已知坐標(biāo)點(diǎn)可以在不同的坐標(biāo)系（NAO，WORLD，TORSO）下建立不同點(diǎn)的坐標(biāo)，本文在NAO坐標(biāo)系下建立。NAO坐標(biāo)系原點(diǎn)：NAO機(jī)器人的兩腳垂直平分線上，X軸正方向始終在NAO的前方，Y軸正方向在NAO的左方，Z軸正方向在NAO的上方。

圖1 NAO的各個(gè)效應(yīng)器的位置和原點(diǎn)坐標(biāo)

為保證運(yùn)動(dòng)軌跡的確定性，需要計(jì)算出機(jī)器人末端效應(yīng)器經(jīng)過路徑上各個(gè)點(diǎn)時(shí)相對(duì)于機(jī)器人基本位姿，這個(gè)過程就是笛卡爾空間中路徑規(guī)劃過程［5］。

1.2 關(guān)節(jié)空間位置控制

在關(guān)節(jié)空間中對(duì)機(jī)器人進(jìn)行控制，利用視覺測量得到目標(biāo)位姿求解關(guān)節(jié)角，利用開始時(shí)刻和運(yùn)動(dòng)停止時(shí)刻的角度作為邊界條件，然后使用常見的差值函數(shù)對(duì)關(guān)鍵點(diǎn)進(jìn)行函數(shù)擬合［5］。關(guān)節(jié)控制可以是一個(gè)關(guān)節(jié)、關(guān)節(jié)鏈、一組關(guān)節(jié)。如關(guān)節(jié)控制NAO機(jī)器人手臂各個(gè)關(guān)節(jié)名稱和各個(gè)關(guān)節(jié)的坐標(biāo)軸。NAO機(jī)器人手臂具有5個(gè)自由度，其結(jié)構(gòu)如圖2所示。肩膀處有俯仰和橫滾兩個(gè)自由度，肘部具有偏轉(zhuǎn)和橫滾兩個(gè)自由度，腕部具有偏轉(zhuǎn)一個(gè)自由度。除此之外，手爪具有張合一個(gè)自由度。

圖2 LArm各個(gè)關(guān)節(jié)名稱和坐標(biāo)軸

1.3 手臂軌跡規(guī)劃

手臂運(yùn)動(dòng)的軌跡是指機(jī)器人在運(yùn)動(dòng)過程中的位移、速度、加速度，而軌跡規(guī)劃是根據(jù)任務(wù)的要求，計(jì)算出預(yù)期的運(yùn)動(dòng)軌跡［4］。軌跡規(guī)劃是在關(guān)節(jié)或笛卡爾空間中，對(duì)已知關(guān)鍵點(diǎn)的邊界條件進(jìn)行插值，求取相應(yīng)符合任務(wù)需求的運(yùn)動(dòng)方程。

2 多空間混合約束手臂規(guī)劃算法

NAO機(jī)器人的軌跡規(guī)劃可以在笛卡爾空間，使機(jī)器人在位姿空間簡化了一個(gè)坐標(biāo)點(diǎn)即機(jī)器人末端效應(yīng)器的坐標(biāo)點(diǎn)，但會(huì)產(chǎn)生歧義位形。也可以在關(guān)節(jié)空間進(jìn)行軌跡規(guī)劃，關(guān)節(jié)空間中利用逆運(yùn)動(dòng)學(xué)求取各個(gè)關(guān)節(jié)的角度，但是需要大量復(fù)雜的矩陣計(jì)算。

本文提出一種使用關(guān)節(jié)空間控制和笛卡兒空間控制相結(jié)合的算法，進(jìn)行軌跡規(guī)劃，從而使NAO機(jī)器人完成抓取任務(wù)。NAO機(jī)器人在手臂抓取的范圍之內(nèi)，初始為站立狀態(tài)的手臂軌跡規(guī)劃如圖3所示。

圖3 NAO坐標(biāo)系下多空間混合約束算法描述

通過NAOqi API自帶的getPosition（）函數(shù)可以確定LArm的末端效應(yīng)器在NAO坐標(biāo)系下的坐標(biāo)，即確定點(diǎn)PS。

由于抓取目標(biāo)大小已知，可以在關(guān)節(jié)空間中預(yù)先設(shè)定抓取目標(biāo)姿態(tài)，即設(shè)定LArm手臂的各個(gè)角度。NAO機(jī)器人的手臂只有5個(gè)DOF，其中WristYaw對(duì)軌跡規(guī)劃的影響很小可以忽略，因此可以使用4個(gè)DOF，定義一個(gè)確定手部位置和姿態(tài)［2］，即設(shè)定NAO機(jī)器人LArm中ShoulderPitch、ShoulderRoll、ElbowYaw、ElbowRoll各個(gè)關(guān)節(jié)的角度，通過使用NAO機(jī)器人LArm的D-H參數(shù)如表1所示，其中UALen（UpperArmLength）的值如圖4所示。相應(yīng)的變換矩陣如公式（1）所示，表示任意兩個(gè)參考系可以用一定的齊次變換矩陣來描述，其中包括兩個(gè)旋轉(zhuǎn)矩陣、兩個(gè)平移矩陣。根據(jù)D-H坐標(biāo)系變換方法，將表1中參數(shù)分別代入公式（1）中，可以求得總的變換矩陣，這個(gè)過程叫做正運(yùn)動(dòng)學(xué)分析。如公式（2）所示，通過依次右乘四個(gè)矩陣就可以得到總的變換矩陣。

表1 NAO機(jī)器人左胳膊的D-H參數(shù)表

圖4 NAO機(jī)器人左胳膊相關(guān)鏈接的信息

其中，dn表示連桿的偏移量，an表示兩桿的長度，αn表示連桿旋轉(zhuǎn)角度，θn表示旋轉(zhuǎn)關(guān)節(jié)的角度。

其中（px，py，pz）物體在基坐標(biāo)系下的三維空間的坐標(biāo)，（αx，αy，αz）方向?yàn)槟┒诵?yīng)器進(jìn)入剛體的方向?yàn)閦向矢量又稱接近矢量，（ox，oy，oz）指尖指向另一個(gè)指尖的方向?yàn)閥方向矢量，稱為方向矢量。（nx，ny，nz）方向?yàn)?x向矢量，稱為法向矢量n，其中（nx，ny，nz）=（ox，oy，oz）*（αx，αy，αz），滿足右手準(zhǔn)則。

當(dāng)機(jī)器人設(shè)定姿態(tài)后，其手部的姿態(tài)和位置也就確定，則可以在笛卡爾空間中進(jìn)行軌跡插值［7］。

在笛卡爾空間中對(duì)PP和PG之間軌跡插值進(jìn)行描述。由于NAO機(jī)器人手部坐標(biāo)描述為6D（x,y,z，wx，wy，wz），其中手部姿態(tài)（wx，wy，wz）確定，因此只需考慮位置矢量（x,y,z）。既要保證NAO機(jī)器人抓取目標(biāo)的最佳姿態(tài)，又要將手臂末端效應(yīng)器移動(dòng)到指定目標(biāo)，很可能有一定的誤差。即當(dāng)機(jī)器人在空間中的姿態(tài)一定之后，機(jī)器人將不能再沿直線運(yùn)動(dòng)［8，9］。但是NAO機(jī)器人在設(shè)定姿態(tài)后，可以在一定范圍內(nèi)沿直線運(yùn)動(dòng)。給定抓取的目標(biāo)具有一定的高度，Z軸的誤差可以忽略，因此只考慮X，Y軸誤差在1cm附近且NAO機(jī)器人手臂可抓取的范圍內(nèi)。將設(shè)定抓取位姿設(shè)置為坐標(biāo)原點(diǎn)（0，0）。如圖5所示，固定Y軸，得到X軸正方向移動(dòng)最大的距離是6.5cm，此時(shí)X軸誤差1.0cm，Y軸的誤差0.4cm可忽略。X軸的負(fù)方向最大移動(dòng)的距離是-5.5cm，此時(shí)X軸誤差-0.001cm可忽略，Y軸誤差-1.0cm。如圖6所示，固定X軸，得到Y(jié)軸正方向移動(dòng)最大的距離13.5cm此時(shí)Y軸誤差1.15cm，X軸的誤差0.6cm可忽略。Y軸負(fù)方向最大移動(dòng)的距離是-0.13cm此時(shí)Y軸誤差1.0cm，X軸的誤差0.45cm可忽略。

圖5 設(shè)定姿態(tài)后X軸移動(dòng)的誤差范圍

圖6 設(shè)定姿態(tài)后Y軸移動(dòng)的誤差范圍

故X軸，Y軸在設(shè)定姿態(tài)后抓取的范圍：

手臂軌跡規(guī)劃的偽代碼如下所示：

（1）預(yù)先設(shè)定抓取目標(biāo)的姿態(tài)；

（2）在關(guān)節(jié)空間中NAO坐標(biāo)系下利用正向運(yùn)動(dòng)學(xué)獲取手臂末端效應(yīng)器移動(dòng)點(diǎn)的坐標(biāo)（x1，y1，z1）；

（3）設(shè)在NAO坐標(biāo)系下目標(biāo)點(diǎn)的坐標(biāo)（xG，yG，zG），計(jì)算目標(biāo)點(diǎn)與設(shè)定姿態(tài)點(diǎn)的相對(duì)坐標(biāo)（xG-x1，yG-y1，zG-z1）；

（4）在笛卡爾空間中進(jìn)行NAO坐標(biāo)系下操作；

（5）分別在X，Y，Z軸上每隔2cm進(jìn)行插值；

（6）逼近目標(biāo)進(jìn)行抓取。

3 實(shí)驗(yàn)

實(shí)驗(yàn)場景：NAO機(jī)器人抓取目標(biāo)為紅綠色柱。通過視覺測量得到在NAO坐標(biāo)系下紅綠色柱的坐標(biāo)，確定紅綠色柱坐標(biāo)在手臂抓取范圍內(nèi)。NAO預(yù)先設(shè)定的姿態(tài)如圖7（a）所示，通過NAOqi中API函數(shù)getPosition（）可知LArm的末端效應(yīng)器的位置矢量坐標(biāo)為（12.4379cm，13.0437cm，32.5971cm）。設(shè)定姿態(tài)后通過在笛卡爾坐標(biāo)系下進(jìn)行軌跡規(guī)劃。NAO手臂移動(dòng)的軌跡如圖7（b）、（c）所示，最終抓取目標(biāo)如圖7（d）所示，實(shí)驗(yàn)結(jié)果如表2所示。

圖7 基于混合空間NAO機(jī)器人手臂抓取過程

表2 多空間混合約束手臂軌跡規(guī)劃實(shí)驗(yàn)

4 結(jié)論

本文提出對(duì)確定目標(biāo)的前提下進(jìn)行抓取的方法，既消除在笛卡兒空間中NAO機(jī)器人手臂出現(xiàn)冗余現(xiàn)象，又減少了在關(guān)節(jié)空間中計(jì)算各個(gè)關(guān)節(jié)角度的困難。實(shí)驗(yàn)表明，該方法在一定范圍內(nèi)進(jìn)行軌跡規(guī)劃目標(biāo)抓取的方法是準(zhǔn)確和可行的，計(jì)算量小而且容易實(shí)現(xiàn)。

［1］Louloudi A，Mosallam A，Marturi N，et al.Integration ofthe humanoid robotNao inside asmart home：A case study［C］.Proceedings of the Swedish AI Society Workshop（SAIS）.Link?ping Electronic Conference Proceedings.2010，48：35-44.

［2］Muller J，F(xiàn)rese U，Rofer T.Grab a mug-object detection and grasp motion planning with the Nao robot［C］.Humanoid Robots（Humanoids），2012 12th IEEE-RAS InternationalConference on.IEEE，2012：349-356.

［3］任敬鐵，孫漢旭.一種新穎的笛卡爾空間軌跡規(guī)劃方法［J］.機(jī)器人學(xué)報(bào)，2002，24（3）：217-221.

［4］孫秀萍.機(jī)器人手臂規(guī)劃的研究［J］.內(nèi)蒙古師范大學(xué)學(xué)報(bào)：自然科學(xué)漢文版，2009，38（3）：174-177.

［5］程小寧.基于視覺反饋的機(jī)械手臂運(yùn)動(dòng)軌跡優(yōu)化控制［D］.杭州：浙江大學(xué)，2007.

［6］楊賀然.基于機(jī)器視覺的移動(dòng)目標(biāo)機(jī)器人抓取研究［D］.北京：北京化工大學(xué)，2012.

［7］李光日，黃強(qiáng)，徐乾，等.小型防人機(jī)器人步態(tài)穩(wěn)定控制方法研究［J］.長春理工大學(xué)學(xué)報(bào)：自然科學(xué)版，2014，37（3）：60-65.

［8］于清波.笛卡爾空間-五自由度機(jī)器人手臂自由度分析［J］.煤礦機(jī)械，2009，30（9）：113-114.

［9］熊家新，劉麗，徐鶴，等.全向輪式移動(dòng)機(jī)器人的定位導(dǎo)航算法［J］.長春理工大學(xué)學(xué)報(bào)：自然科學(xué)版，2014，37（4）：83-88.