基于多空間混合約束的NAO機器人抓取軌跡規劃

王秋玥，方明，張海濤

（長春理工大學 計算機科學技術學院，長春 130022）

機器人手臂軌跡規劃是當今服務控制領域研究的熱點。針對機器人軌跡規劃方法可以在笛卡爾空間和在關節空間進行，在笛卡爾空間使用逆運動學求解直接控制NAO機器人的末端效應器［1］。此空間的軌跡規劃比較直觀，但是由于冗余運動，規劃過程中容易使機器人產出奇異位形；關節空間直接控制各個關節的角度，能保證運動路徑的唯一性，但計算量很大，導致控制時間延長，并且末端效應器的運動軌跡不直觀［2-4］。

本文提出用笛卡兒空間和關節空間相結合控制方法進行軌跡規劃。在抓取目標大小已知條件下，利用關節空間控制方法預先設定各個關節的抓取角度，保證手臂抓取的最佳姿態，再用笛卡兒空間控制方法進行軌跡插值從而抓取目標。

1 混合空間描述

1.1 笛卡兒空間位置控制

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

圖1 NAO的各個效應器的位置和原點坐標

為保證運動軌跡的確定性，需要計算出機器人末端效應器經過路徑上各個點時相對于機器人基本位姿，這個過程就是笛卡爾空間中路徑規劃過程［5］。

1.2 關節空間位置控制

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

圖2 LArm各個關節名稱和坐標軸

1.3 手臂軌跡規劃

手臂運動的軌跡是指機器人在運動過程中的位移、速度、加速度，而軌跡規劃是根據任務的要求，計算出預期的運動軌跡［4］。軌跡規劃是在關節或笛卡爾空間中，對已知關鍵點的邊界條件進行插值，求取相應符合任務需求的運動方程。

2 多空間混合約束手臂規劃算法

NAO機器人的軌跡規劃可以在笛卡爾空間，使機器人在位姿空間簡化了一個坐標點即機器人末端效應器的坐標點，但會產生歧義位形。也可以在關節空間進行軌跡規劃，關節空間中利用逆運動學求取各個關節的角度，但是需要大量復雜的矩陣計算。

本文提出一種使用關節空間控制和笛卡兒空間控制相結合的算法，進行軌跡規劃，從而使NAO機器人完成抓取任務。NAO機器人在手臂抓取的范圍之內，初始為站立狀態的手臂軌跡規劃如圖3所示。

圖3 NAO坐標系下多空間混合約束算法描述

通過NAOqi API自帶的getPosition（）函數可以確定LArm的末端效應器在NAO坐標系下的坐標，即確定點PS。

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

表1 NAO機器人左胳膊的D-H參數表

圖4 NAO機器人左胳膊相關鏈接的信息

其中，dn表示連桿的偏移量，an表示兩桿的長度，αn表示連桿旋轉角度，θn表示旋轉關節的角度。

其中（px，py，pz）物體在基坐標系下的三維空間的坐標，（αx，αy，αz）方向為末端效應器進入剛體的方向為z向矢量又稱接近矢量，（ox，oy，oz）指尖指向另一個指尖的方向為y方向矢量，稱為方向矢量。（nx，ny，nz）方向為 x向矢量，稱為法向矢量n，其中（nx，ny，nz）=（ox，oy，oz）*（αx，αy，αz），滿足右手準則。

當機器人設定姿態后，其手部的姿態和位置也就確定，則可以在笛卡爾空間中進行軌跡插值［7］。

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

圖5 設定姿態后X軸移動的誤差范圍

圖6 設定姿態后Y軸移動的誤差范圍

故X軸，Y軸在設定姿態后抓取的范圍：

手臂軌跡規劃的偽代碼如下所示：

（1）預先設定抓取目標的姿態；

（2）在關節空間中NAO坐標系下利用正向運動學獲取手臂末端效應器移動點的坐標（x1，y1，z1）；

（3）設在NAO坐標系下目標點的坐標（xG，yG，zG），計算目標點與設定姿態點的相對坐標（xG-x1，yG-y1，zG-z1）；

（4）在笛卡爾空間中進行NAO坐標系下操作；

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

（6）逼近目標進行抓取。

3 實驗

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

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

表2 多空間混合約束手臂軌跡規劃實驗

4 結論

本文提出對確定目標的前提下進行抓取的方法，既消除在笛卡兒空間中NAO機器人手臂出現冗余現象，又減少了在關節空間中計算各個關節角度的困難。實驗表明，該方法在一定范圍內進行軌跡規劃目標抓取的方法是準確和可行的，計算量小而且容易實現。

［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，Frese 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］任敬鐵，孫漢旭.一種新穎的笛卡爾空間軌跡規劃方法［J］.機器人學報，2002，24（3）：217-221.

［4］孫秀萍.機器人手臂規劃的研究［J］.內蒙古師范大學學報：自然科學漢文版，2009，38（3）：174-177.

［5］程小寧.基于視覺反饋的機械手臂運動軌跡優化控制［D］.杭州：浙江大學，2007.

［6］楊賀然.基于機器視覺的移動目標機器人抓取研究［D］.北京：北京化工大學，2012.

［7］李光日，黃強，徐乾，等.小型防人機器人步態穩定控制方法研究［J］.長春理工大學學報：自然科學版，2014，37（3）：60-65.

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

［9］熊家新，劉麗，徐鶴，等.全向輪式移動機器人的定位導航算法［J］.長春理工大學學報：自然科學版，2014，37（4）：83-88.