機器人航天員冗余機械臂自運動優(yōu)化

劉世平，赫向陽，，韓亮亮，李世其，付 艷

（1.華中科技大學(xué)機械科學(xué)與工程學(xué)院，武漢430074；2.上海宇航系統(tǒng)工程研究所，上海201108）

·工程技術(shù)·

機器人航天員冗余機械臂自運動優(yōu)化

劉世平1，赫向陽1，2，韓亮亮2，李世其1，付 艷1

（1.華中科技大學(xué)機械科學(xué)與工程學(xué)院，武漢430074；2.上海宇航系統(tǒng)工程研究所，上海201108）

為增強機器人航天員的工作能力，機器人航天員的手臂采用七自由度仿人機械臂。根據(jù)S?R?S構(gòu)型七自由度冗余手臂的結(jié)構(gòu)特性，采用臂型角參數(shù)化的方法求解運動學(xué)。該方法將冗余機械臂的自運動用臂型角來描述，通過對臂型角的二次優(yōu)化來限制機械臂的自運動。為了使仿人臂處于一種自然的構(gòu)型和姿態(tài)，定義了“偏離中心度”的指標(biāo)來表示關(guān)節(jié)偏離中心位置的程度，通過這個評價指標(biāo)來描述仿人臂的最優(yōu)位形，從而建立自運動優(yōu)化函數(shù)。采用具有線性減小慣性權(quán)重的粒子群優(yōu)化算法來尋找自運動優(yōu)化函數(shù)的最優(yōu)解。最后在機器人航天員平臺進行驗證，可以快速有效地完成冗余手臂自運動優(yōu)化。

冗余機械臂；運動學(xué)；自運動優(yōu)化；粒子群算法

1 引言

隨著機器人技術(shù)的發(fā)展，機器人在空間的應(yīng)用彰顯了巨大的潛力［1］，尤其是具有高靈活性和運動協(xié)調(diào)性的類人機器人，可以輔助人類宇航員完成空間站內(nèi)外的設(shè)備維護和科學(xué)實驗，降低出艙風(fēng)險。針對機器人航天員在空間站和星體探索中的工作任務(wù)特點，本文設(shè)計了雙臂冗余手臂機器人。冗余機械臂由于機械臂的自由度數(shù)目大于末端位姿參數(shù)數(shù)目，關(guān)節(jié)空間存在著無數(shù)組解與目標(biāo)末端位姿對應(yīng)。采用冗余結(jié)構(gòu)的機械臂相比于常見的六自由度機械臂，有著許多不可替代的優(yōu)點，在增強機械臂在執(zhí)行任務(wù)的可靠性、實用性［2］的同時，還可以實現(xiàn)避障［3］、容錯［4］和關(guān)節(jié)力矩優(yōu)化［5］等功能。NASA和通用汽車公司聯(lián)合研發(fā)的新一代靈巧機器人Robonaut 2［6］以及德國航空航天中心研制的仿人雙臂機器人 Rollin’Justin［7］都是采用了七自由度冗余機械臂。

冗余機械臂的自運動是指在末端位姿確定的情況下，機械臂關(guān)節(jié)空間存在無數(shù)組連續(xù)的解與目標(biāo)末端位姿相對應(yīng)，這些關(guān)節(jié)空間解的集合形成冗余機械臂的自運動流形［8］。冗余機械臂相對于非冗余機械臂的優(yōu)點，都是通過自運動表現(xiàn)出來的，所以自運動的優(yōu)化有很重要的意義。

目前冗余機械臂的自運動優(yōu)化問題，通常通過微分運動學(xué)法或者數(shù)值方法來求解。張雨濃等［9］通過微分運動學(xué)法，考慮關(guān)節(jié)位置極限和速度極限，對PA10機械臂進行了自運動規(guī)劃。任子武等［10］通過智能迭代算法，構(gòu)建冗余關(guān)節(jié)角的最優(yōu)組合來求解機械臂的逆運動學(xué)。這些方法有一定的局限性，基于微分運動學(xué)的規(guī)劃并不能求得逆運動學(xué)的全部可行解，智能迭代算法，需要大量的運算去逼近目標(biāo)值，在保證計算精度的同時難以獲得求解的快速性。

本文建立了基于臂形角的七自由度運動學(xué)求解方法，并通過臂形角來描述冗余機械臂的自運動，采用最優(yōu)位形來優(yōu)化機械臂的自運動，實現(xiàn)冗余機械臂運動過程中機械臂自運動的優(yōu)化，保證了計算過程的精確度和快速性。

2 臂形角參數(shù)化運動學(xué)

2.1冗余機械臂正運動學(xué)

仿人機械臂采用串聯(lián)球形關(guān)節(jié)?旋轉(zhuǎn)關(guān)節(jié)?球形關(guān)節(jié)的形式（簡稱S?R?S），前三個關(guān)節(jié)互相垂直相交于一點，形成球鉸對應(yīng)人體的肩關(guān)節(jié)，同理后三個關(guān)節(jié)垂直相交于一點，形成腕關(guān)節(jié)，中間的旋轉(zhuǎn)關(guān)節(jié)稱之為肘關(guān)節(jié)。這種構(gòu)型能夠最大程度的模擬人體的手臂，根據(jù)這種構(gòu)型設(shè)計了仿人機械臂，如圖1所示。為了增加機械臂的運動范圍，肘關(guān)節(jié)設(shè)計有一定的位置偏置。

為了求解機械臂的運動學(xué)，建立機械臂的DH坐標(biāo)系，其中坐標(biāo)系｛0｝為基坐標(biāo)系，與連桿i固接的坐標(biāo)系記為坐標(biāo)系｛i｝，如圖2所示。同時建立DH參數(shù)表，其中，ai為連桿長度，αi為連桿扭角，di為相鄰連桿距離，θi為兩連桿的夾角，如表1所示。

表1 七自由度機械臂DH參數(shù)Table 1 DH parameters of the 7?DOF manipulator

根據(jù)D?H參數(shù)表，可以得到相鄰坐標(biāo)系的齊次變換矩陣如式（1）所示：

將相鄰坐標(biāo)系的齊次變換矩陣依次連乘，可以得到機械臂末端坐標(biāo)系相對于基座的變換矩陣0T7，即機械臂運動學(xué)正解的通式：

2.2冗余機械臂逆運動學(xué)

冗余機械臂在工作過程中的一種姿態(tài)可以用圖3表示，其中基座、肩關(guān)節(jié)、肘關(guān)節(jié)、腕關(guān)節(jié)和末端分別記為B、S、E、W和T。從當(dāng)前的姿態(tài)可以發(fā)現(xiàn)，肩關(guān)節(jié)、肘關(guān)節(jié)和腕關(guān)節(jié)可以構(gòu)成一個平面，定義這個平面為臂平面。在已知末端位置0T7時，S點、W點以及向量SW的值是唯一的。冗余機械臂的自運動表現(xiàn)為：肩關(guān)節(jié)、肘關(guān)節(jié)和腕關(guān)節(jié)構(gòu)成的平面繞SW旋轉(zhuǎn)運動。本文定義關(guān)節(jié)1軸線的方向BS與肩關(guān)節(jié)和腕關(guān)節(jié)方向SW形成的平面為參考平面。臂平面與參考平面生成的夾角，定義為臂型角，記作ψ。這樣，冗余機械臂的自運動可以通過臂形角完整地描述。

已知機械臂末端位姿0P7和0R7，向量SW可以表示為：

機械臂自運動不會影響肘關(guān)節(jié)大小，所以θ4與臂型角ψ無關(guān)：

在基坐標(biāo)系中，臂平面相對于參考平面的旋轉(zhuǎn)矩陣為：

式中：I3為3×3的單位陣，SW?為向量SW的叉乘因子。

當(dāng)三號坐標(biāo)系在參考平面內(nèi)時，三號坐標(biāo)系與基座坐標(biāo)系之間的關(guān)系用來表示，0x3，0y3和0z3表示三號坐標(biāo)系的方向向量。

由于肘關(guān)節(jié)偏置的存在，0y3相對于基坐標(biāo)系的角度γ由兩部分組成：轉(zhuǎn)動形成的角度α和偏置產(chǎn)生的角度 β，存在公式（6）～（8）所示的關(guān)系：

0y3相對于基坐標(biāo)系的旋轉(zhuǎn)矩陣為：

根據(jù)公式（10）、（11）可得：

則三號坐標(biāo)系相對于基坐標(biāo)系的姿態(tài)可以表示為：

根據(jù)正運動學(xué)方程（1）可得0R3：

式中：si＝ sinθi，ci＝ cosθi。

方程（13）（14）對應(yīng)項相等，即可求出關(guān)節(jié)角θ1、θ2和 θ3。

同理，坐標(biāo)系4相對于坐標(biāo)系7可以表示為：

方程（15）（16）對應(yīng)項相等，即可求出關(guān)節(jié)角θ5、θ6和 θ7。

至此，可以得到機械臂臂型角參數(shù)化的運動學(xué)逆解。根據(jù)已知的機械臂末端位置和臂型角ψ的值，按照本文提出的運動學(xué)求解方法，可以準(zhǔn)確地得到關(guān)節(jié)位置的8組解，通過合理的二次規(guī)劃可以實現(xiàn)冗余機械臂自運動控制。

3 機械臂自運動優(yōu)化

經(jīng)過臂形角參數(shù)化的機械臂，其自運動可以通過臂形角來描述。對于給定的機械臂末端位置，只要確定機械臂的自運動就可以確定機械臂各關(guān)節(jié)的角度值。為了使運動學(xué)求解的結(jié)果滿足人形機械臂處于自然位姿的要求，同時也為后續(xù)機械臂的軌跡規(guī)劃和避障提供足夠的運動空間，機械臂的最優(yōu)位形定義為最大程度地靠近關(guān)節(jié)的中心位置，為關(guān)節(jié)運動提供最大的活動裕量。

粒子群算法（Particle Swarm Optimization，PSO）是一種智能優(yōu)化算法［11］。PSO類似于遺傳算法，通過迭代的方式進行優(yōu)化尋值。粒子初始化為一群隨機粒子，在每一次迭代中通過跟蹤粒子本身歷史的最優(yōu)位置和群體的最優(yōu)位置，在一定隨機擾動的情況下決定下一步的搜索方向。在迭代過程中，粒子的位置目標(biāo)函數(shù)都被評估。

受關(guān)節(jié)限位的約束，根據(jù)實際機械臂的構(gòu)型，各關(guān)節(jié)可到達(dá)的關(guān)節(jié)運動范圍如表2所示。

表2 機械臂關(guān)節(jié)運動范圍Table 2 Range of the joint angles

機械臂自運動的優(yōu)化，轉(zhuǎn)化為對“偏離中心度”指標(biāo)μ的優(yōu)化。根據(jù)關(guān)節(jié)“偏離中心度”指標(biāo)最小化要求，確定機械臂的最優(yōu)位形，定義PSO適應(yīng)度函數(shù)如式（18）：

式中，μ（i）max表示七個關(guān)節(jié)中關(guān)節(jié)“偏離中心度”指標(biāo)μ最大的值。適應(yīng)度函數(shù)的值越小，表示七個關(guān)節(jié)中最大的“偏離中心度”指標(biāo)越小，獲得的機械臂自運動優(yōu)化越好。適應(yīng)度函數(shù)的自變量ψ就是所求的最優(yōu)臂型角。

為了提高算法性能，采用引入慣性權(quán)重w的PSO。慣性權(quán)重w決定了粒子先前速度對當(dāng)前速度的影響程度，慣性權(quán)重w大，全局探索能力強，慣性權(quán)重w小，局部搜索能力強。Y.Shi和R. Eberhart［13］提出了線性減小慣性權(quán)重w的自適應(yīng)策略，慣性權(quán)重w表示為式（19）：

式中：wmax為最大權(quán)重系數(shù)，wmin為最小權(quán)重系數(shù)，tmax為最大進化次數(shù)。

這樣，速度更新方程表示為式（20）：

式中：c1和c2表示粒子自我總結(jié)和學(xué)習(xí)的能力，通常范圍在［1，2］之間的尋優(yōu)效果最好；rand（）是［0，1］之間的隨機數(shù)；粒子的移動速度被限制在［－Vmax，Vmax］之間；pij是粒子歷史最優(yōu)位置，pgj是群體全局最優(yōu)位置。粒子群算法的流程如圖4所示。

4 運動仿真與試驗驗證

為了驗證上述冗余機械臂運動學(xué)的求解和自運動優(yōu)化效果，以仿人七自由度機械臂為平臺，通過數(shù)值計算和實物來驗證算法的有效性。

4.1參數(shù)化運動學(xué)求解

在冗余機械臂的工作空間內(nèi)，選擇任意的一個末端位置，位置0P7＝［－0.6，0.3，0.8］，0R7＝［0，－90，0］（采用ZXZ旋轉(zhuǎn)歐拉角）。在不考慮關(guān)節(jié)限位的情況下，選擇一個任意位置的臂型角ψ＝－160°，對機械臂的運動學(xué)求解。逆運動學(xué)解可以獲得8組解，如表3所示。根據(jù)運動學(xué)方程（2）對8組關(guān)節(jié)角求正運動學(xué)解，然后進行歐拉角的轉(zhuǎn)換得到實際末端位姿，與期望位姿進行比較，誤差在10－14m左右，由此可以驗證參數(shù)化運動學(xué)算法的精確性。對于固定臂型角時逆運動學(xué)8組解的選取，采用偏離中心度最小的方法。對8組解分別求偏離中心度值，最優(yōu)解是第三組解。

表3 機械臂逆運動學(xué)解Table 3 Results of inverse kinematics

4.2自運動優(yōu)化驗證

在參數(shù)化運動學(xué)求解驗證過程中，臂型角ψ通過人為來設(shè)定。在考慮機械臂自運動優(yōu)化的條件下，利用文中提出的PSO優(yōu)化算法，對機械臂的自運動進行優(yōu)化。求解過程中，粒子群算法的參數(shù)設(shè)置如下：搜尋空間臂形角ψ∈［－π，π］，種群規(guī)模N＝50，迭代次數(shù)為50，c1＝2，c2＝2，粒子速度更新式中的慣性權(quán)系數(shù)wmax＝1，wmin＝0．8，速度限定vmax＝1，vmin＝－1。

通過計算，可以得到臂型角ψ的最優(yōu)值，ψopt＝175．97°，適應(yīng)度值最小為0.305，計算時間為8 ms。繪制適應(yīng)度值與進化代數(shù)曲線，如圖5所示。通過曲線可以看到進化20代以后，適應(yīng)度已經(jīng)非常接近最優(yōu)值。

此時，最優(yōu)自運動條件下的運動學(xué)解為當(dāng)前臂型角的第 3組解：q ＝［－81.069 －3.172－58.72 －83.73－91.21－49.69－96.06］。在MATLAB機械建模與仿真環(huán)境（Sim Mechanics）中進行仿真，分別輸入優(yōu)化前和優(yōu)化后關(guān)節(jié)位置，得到機械臂實際的關(guān)節(jié)位置如圖6所示。

與不考慮自運動優(yōu)化，ψ＝－160°的第3組解比較，得到兩種情況下各關(guān)節(jié)“偏離中心度”μ的值，優(yōu)化前 μinit＝［0.740 0.111 0.289 0.014 0.415 0.140 0.334］，優(yōu)化后μopt＝［0.681 0.2880.106 0.014 0.272 0.305 0.301］，如圖7所示。

通過比較可以發(fā)現(xiàn)，關(guān)節(jié)4優(yōu)化后與優(yōu)化前效果相同，這是由于基于臂形角的自運動優(yōu)化對肘關(guān)節(jié)沒有作用。優(yōu)化后比優(yōu)化前的關(guān)節(jié)“偏離中心度”值更小，即關(guān)節(jié)的活動裕量更大，機械臂的位形更符合人體關(guān)節(jié)構(gòu)型，自運動優(yōu)化達(dá)到了期望的效果。

4.3軌跡跟蹤優(yōu)化驗證

在驗證固定點位置的自運動優(yōu)化后，為保證算法的適用性，進行軌跡跟蹤優(yōu)化驗證。只考慮關(guān)節(jié)末端的位置變化，不考慮末端姿態(tài)變化，定義機械臂期望軌跡為：

設(shè)置最小步長Δt＝0.1 s，利用提出的自運動優(yōu)化算法進行軌跡跟蹤。在仿真模型里，可以得到機械臂在笛卡爾空間的軌跡跟蹤結(jié)果，如圖8所示。通過笛卡爾空間的軌跡可以看出，跟蹤軌跡的點基本都落在期望軌跡上，表明自運動優(yōu)化方法在軌跡跟蹤中普遍適用。

同時，可以得到軌跡跟蹤的誤差，如圖9所示。得到的誤差精度在10－15m范圍內(nèi)，表明算法具有很高的精確性。

在仿真周期內(nèi)對最優(yōu)臂型角和粒子群算法的適應(yīng)度值進行記錄，得到的結(jié)果如圖10所示。最優(yōu)臂型角和PSO適應(yīng)度連續(xù)變化，沒有出現(xiàn)突變。PSO適應(yīng)度值最大為0.52，表明距離關(guān)節(jié)限位最近的距離還有很大的運動裕量，滿足最優(yōu)位形的要求。

在仿真周期內(nèi)對各關(guān)節(jié)角度變化進行記錄，得到的結(jié)果如圖11所示。曲線表明，在一個周期內(nèi)關(guān)節(jié)角度連續(xù)變化，沒有突變。每條曲線左右兩端點角度值相等，表明整個過程連續(xù)并且可以循環(huán)。

在機器人航天員平臺上進行驗證，采用與仿真模型相同的運動軌跡，運動過程中機械臂響應(yīng)快速穩(wěn)定，運動過程如圖12所示。圖（a）、（b）、（c）、（d）分別表示在時間2／5π、4／5π、6／5π和8／5π時的位姿。

5 結(jié)論

本文根據(jù)S?R?S形式的冗余機械臂結(jié)構(gòu)特性，采用臂型角的方法對機械臂進行關(guān)節(jié)的參數(shù)化，并通過臂型角對機械臂的自運動進行優(yōu)化。這種參數(shù)化的封閉解能夠獲取所有的可行解，同時保證了計算精度。采用具有線形減小慣性權(quán)重的PSO算法，避免了陷入局部最優(yōu)解，同時提高了搜尋速度。通過在機器人航天員平臺的驗證，表明提出的自運動優(yōu)化算法具有保持機械臂最優(yōu)位形的作用。

這種方法對機械臂的構(gòu)型有特殊要求，但這種構(gòu)型普遍應(yīng)用在仿人機械臂中，具有廣泛的適用性?；谧顑?yōu)位形的自運動優(yōu)化可以使機械臂快速、平穩(wěn)地到達(dá)目標(biāo)位姿，如果要實現(xiàn)避障等功能，可以通過在最優(yōu)位形的基礎(chǔ)上對機械臂進行二次規(guī)劃來實現(xiàn)。

（References）

［1］ Diftler M A，Culbert C J，Ambrose R O，et al.Evolution of the NASA／DARPA robonaut control system［C］／／Robotics and Automation，2003.Proceedings.ICRA＇03.IEEE Inter?national Conference on.IEEE，2003，2：2543?2548.

［2］ Zanganeh K E，Angeles J.Mobility and position analyses of a novel redundant parallel manipulator［C］／／Robotics and Au?tomation，1994.Proceedings.，1994 IEEE International Con?ference on.IEEE，1994：3049?3054.

［3］ Lee K K，Komoguchi Y，Buss M.Multiple obstacles avoid?ance for kinematically redundant manipulators using Jacobian transpose method［C］ ／／SICE，2007 Annual Conference. IEEE，2007：1070?1076.

［4］ Assal S F M，Watanabe K，Izumi K.Neural network?based kine?matic inversion of industrial redundant robots using cooperative fuzzy hint for the joint limits avoidance［J］.Mechatronics，IEEE／ASME Transactions on，2006，11（5）：593?603.

［5］ Zhang Y，Ge S S，Lee T H.A unified quadratic?program?ming?based dynamical system approach to joint torque optimi?zation of physically constrained redundant manipulators［J］. Systems，Man，and Cybernetics，Part B：Cybernetics，IEEE Transactions on，2004，34（5）：2126?2132.

［6］ Diftler M A，Mehling J S，Abdallah M E，et al.Robonaut 2?the first humanoid robot in space［C］／／Robotics and Automa?tion（ICRA），2011 IEEE International Conference on. IEEE，2011：2178?2183.

［7］ Albu?Sch?ffer A，Eiberger O，F(xiàn)uchs M，et al.Anthropomor?phic soft robotics?from torque control to variable intrinsic com?pliance［M］.Robotics research.Springer Berlin Heidelberg，2011：185?207.

［8］ 趙建文，杜志江，孫立寧.7自由度冗余手臂的自運動流形［J］.機械工程學(xué)報，2007，40（9）：132?137. Zhao Jianwen，Du Zhijiang，Sun Lining.Self?motion manifolds of a 7?DOF redundant robot arm［J］.Chinese Journal of Mechan?ical Engineering，2007，40（9）：132?137.（in Chinese）

［9］ 張雨濃，譚寧，譚志國，等.基于二次型優(yōu)化的不定目標(biāo)自運動規(guī)劃方案及PA10機械臂仿真驗證［J］.暨南大學(xué)學(xué)報：自然科學(xué)與醫(yī)學(xué)版，2010，31（3）：229?234. Zhang Yunong，Tan Ning，Tan Zhiguo，et al.Quadratic?opti?mization based self?motion planning with no target?configura?tion assigned and verified via PA10 simulations［J］.Journal of Jinan University（Natural Science），2010，31（3）：229?234.（in Chinese）

［10］ 任子武，王振華，孫立寧.全局和聲搜索方法及其在仿人靈巧臂逆運動學(xué)求解中應(yīng)用［J］.控制理論與應(yīng)用，2012，29（7）：867?876. Ren Zhiwu，Wang zhenhua，Sun Lining.A global harmony search algorithm and its application to inverse kinematics problem for humanoid arm［J］.Control Theory＆ Applica?tions，2012，29（7）：867?876.（in Chinese）

［11］ Eberhart R C，Kennedy J.A new optimizer using particle swarm theory［C］／／Proceedings of the sixth international symposium on micro machine and human science.1995，1：39?43.

［12］ 陳鵬，劉璐，余飛，等.一種仿人機械臂的運動學(xué)逆解的幾何求解方法［J］.機器人，2012，34（2）：211?216. Chen Peng，Liu Lu，Yu Fei，et al.A geometrical method for inverse kinematics of a kind of humanoid manipulator［J］. ROBOT，2012，34（2）：211?216.（in Chinese）

［13］ Shi Y，Eberhart R.A modified particle swarm optimizer［C］／／Evolutionary Computation Proceedings，1998.IEEE World Congress on Computational Intelligence.，The 1998 IEEE International Conference on.IEEE，1998：69?73.

（責(zé)任編輯：康金蘭）

Self?motion Optimization for Redundant Manipulator of Robonaut

LIU Shiping1，HE Xiangyang1，2，HAN Liangliang2，LI Shiqi1，F(xiàn)U Yan1

（1.School of Mechanical Science and Engineering，Huazhong University of Science and Technology，Wuhan 430074，China；2.Aerospace System Engineering Shanghai，Shanghai 201108，China）

The robonaut arm adopts the 7?DOF humanoid manipulator to expand its working capabil?ity.According to the structural characteristics of the 7?DOF S?R?S humanoid manipulator，the ma?nipulator kinematics can be solved with the arm angle parameter method.This method describes the self?motion of the redundant manipulator with the arm angle，thus the self?motion can be controlled through the quadratic optimization of the arm angle.In order to make the humanoid manipulator in a natural configuration，the indicator named＂Off?center Level＂was defined to indicate the degree of deviation from the center of the joint position and the optimal configuration could be described through this indicator，thus a self?motion optimization function was established.The particle swarm optimization algorithm with linear inertia was used to find the optimal solution of the self?motion opti?mization function.The method was tested on the robonaut platform and it could complete the self?motion optimization of redundant manipulator quickly and effectively.

redundant manipulator；kinematics；self?motion optimization；particle swarm optimiza?tion algorithm

TP242.6

：A

：1674?5825（2017）02?0143?07

2016?08?25；

2017?02?24

載人航天預(yù)先研究項目（030401）；上海航天科技創(chuàng)新基金資助項目（SAST201409）

劉世平，男，副教授，研究方向為智能機械設(shè)計與仿真、虛擬樣機技術(shù)。E?mail：liushiping＠m(xù)ail.hust.edu.cn