平面冗余自由度機器人自尋優運動優化方法

霍希建，劉伊威，姜力，劉宏

(哈爾濱工業大學機器人技術與系統國家重點實驗室，黑龍江哈爾濱150080)

冗余自由度機器人具有自運動特性(或冗余性)。該特性可實現機器人在完成給定任務的前提下，進行二次目標的優化，以改善機械臂的綜合性能。冗余自由機器人運動學研究主要包括逆運動學求解及運動優化。逆運動學求解方法相對較為成熟，根據解的形式可以分為:幾何解析法［1］、數值解析法［2］、數值迭代法［3］、智能算法［4］等。目前，冗余自由度機器人運動學的研究重點主要是運動優化，即在滿足工作要求的前提下，實現機器人避障、避奇異構型、遠離關節限位等單目標或多目標的優化，最終提高機器人整體的任務操作能力?，F有的機器人運動優化方法可分為4類:1)根據優化任務設置機器人的指標評價函數，然后在自運動流形范圍內進行全局搜索，最終獲得運動學最優解［5］。該類方法可以得到全局最優解，但計算復雜性與優化效果之間為對立關系，不適于實時運動控制;2)采用人工勢場思想，將機器人優化目標轉化為虛擬力作用于機器人上，通過虛擬力作用確定運動學最優解［6-8］。該類方法通常用于機器人避障;3)速度級的零空間優化方法。該方法將運動優化目標轉化到雅克比零空間，在速度水平進行的優化［9-12］。該類方法計算過程中存在累計誤差，難以保證運動學逆解的全局最優性;4)采用神經網絡等智能算法建立機器人運動學的優化模型，從而完成機器人的運動控制［5，13］。目前，該類方法計算精度較低，且沒有統一的建模理論指導。

平面四自由度機器人是冗余自由度機器人的一個重要的基礎模型。平面四自由度機器人運動學研究對三維空間機器人發展具有重要的理論指導意義。由于機器人在關節工作范圍的中間位置會比在極限位置有更好的工作效果，因此，本文采用自尋優方法對機器人進行優化，使運動過程中各關節盡量遠離關節限位。

1 正運動學分析

機器人正運動學是關節空間Cn到操作空間Wm的非線性映射［14］。它是根據機器人各關節位置q∈Cn求解末端位姿T∈Wm。正運動學的映射關系可以表示為

式中:n為機器人自由度數目，m為操作空間的任務維數。

若n＞m，則機器人具有冗余性，映射f為多對一映射。對于操作空間的同一位姿點(不考慮邊界點)，關節空間有無數組關節構型與之對應，所有這些關節構型的集合稱為機器人的自運動流形。

設u∈Wm是操作空間的任意一點，則u對應的自運動流形可以表示為構型空間C和映射集F構成的集合，即

由式(1)可知，冗余自由度機器人的冗余特性存在于自運動流形內，即自運動流形內的關節運動不影響機器人的末端輸出狀態。自運動流形的具體形式與操作空間的位姿點有關。

本文研究對象為4R型串聯機器人，各關節的旋轉軸線相互平行，且各連桿處于同一平面內，機器人DH坐標系如圖1所示。其中，L1=1 m，L2=1 m，L3=1 m，L4=0.2 m 。

圖1 4R機器人的DH坐標系Fig.1 DH coordinates of the 4R robot

設P、φ分別為機器人末端在基坐標系O0xyz下的位置與姿態，q為關節位置，則機器人的正運動學方程表示為

式中:O為3×1零矩陣，R∈SO(3)為旋轉矩陣，i－1Ti為相鄰坐標系之間的齊次變換矩陣，

因此，已知各關節位置，根據方程(2)可以唯一地確定機器人的末端位姿。

2 逆運動學分析

機器人逆運動學是操作空間到關節空間的映射，根據機器人末端位姿T求解各關節位置q。逆運動學映射關系表示為

2.1 逆運動學求解

根據4R機器人的構型特點可知，機器人的末端位置與姿態是解耦的，即前3個關節確定末端位置，第4個關節調整末端姿態。定義機器人的期望末端位置與姿態分別為P和R，平面內第4關節的軸線位置為w點，則w點的位置Pw可表示為

由式(3)可得

由于Pwz為常數，式(3)與式(4)構成欠定方程組，難以直接求解前3個關節位置。本文將七自由度機械臂的“臂角”概念引入到平面冗余機器人逆運動學中［7］。定義“臂角”ψ為繞z0方向y0軸至y3軸的旋轉角度，ψ∈(－π，π]，如圖2所示。

圖2 臂角定義Fig.2 Definition of the arm angle

根據圖1、圖2可得關系式:

由式(4)、(5)確定前2個關節的位置:

式中:

由式(5)、(6)和(7)得

由于R=0R3·3R4，從而確定關節4的位置:

至此，已知4R機器人的期望末端位姿及臂角，即可根據逆運動學封閉解公式，確定對應的各關節位置。

2.2 運動學逆解討論

設各關節運動范圍q∈ (－π， π ]，給定操作空間的一個期望位姿，若q2≠nπ (n=0，1)，則對于任意一個可行臂角，可求得2組可行解。進而得到以下結論:

設C+，C-分別為q2≥0、q2≤0時對應的機器人自運動流形，則:

1)若C+∩C-≠?，則可行解中q2=0或q2=π成立，自運動流形C可表示為:C=C+∪C-。

2)若C+∩C-=? ，則≠nπ (n=0，1)，自運動流形C可表示為:C=C+⊕C-。

3)C+、C-的存在性與操作空間內的期望位姿及關節限位有關，由于受關節實際運動范圍的影響，C+、C-往往由若干個獨立的子流形構成，具體可以表示為:(m、n為整數)。

4)C+、C-內的任意元素均不會在2個流形之間相互變換。

3 基于自尋優方法的運動優化

由式(3)可知，給定機器人的一個末端位姿，則Pw不變。4R機器人的自運動表現為前3個臂桿之間的協調運動，其自運動形式與四連桿運動相似。本文將第四臂桿與虛擬基座相連，將機器人等效為四連桿機構。機器人的自運動使得等效四連桿機構具有無數組構型，從而導致機器人構型不唯一，如圖3所示。由于4R機器人與其等效四連桿機構具有等價的運動關系，根據機構學分析可知四連桿機構具有一個自由度，進而也證明了4R機器人自運動的存在性。

假設在四連桿機構的其中一個關節上施加驅動力，則機構在可行范圍內會產生連續運動，即四連桿機構有無數組構型;若在4個關節分別施加一個驅動力，則在可行運動范圍內存在一類平衡點，使四連桿機構處于靜力學平衡狀態，從而使四連桿機構具有固定的構型。基于上述現象，本文提出一種自尋優方法，將機器人運動優化指標轉化為虛擬轉矩作用于各個關節上，等效四連桿機構會在虛擬力驅動下向著平衡狀態運動，最終達到平衡點，從而實現機器人的運動優化，如圖4所示。

圖3 自由構型的等效四連桿機構Fig.3 The four-bar linkage with uncertain configurations

圖4 確定構型的等效四連桿機構Fig.4 The four-bar linkage with a fixed configuration

因此，本文將通過對等效四連桿機構的研究實現4R冗余自由度機器人的運動優化，使運動過程中各關節盡量遠離關節限位。

3.1 虛擬轉矩函數

由于機器人運動優化過程是在虛擬轉矩驅動下完成的，虛擬轉矩函數的性能直接影響到最終的優化效果。虛擬轉矩函數將關于機器人關節位置的優化指標轉化為虛擬轉矩作用于四連桿機構，轉矩函數可以為線性函數、非線性函數或分段函數。設a為關節活動范圍的中間位置，本文在高斯函數的基礎上建立了轉矩函數H(q):

為了更好的區分關節活動范圍內不同位置的重要性，本文要求轉矩函數越接近關節極限位置，虛擬轉矩變化越快，即函數H(q)在 [a，qmax]區間應為下凸函數，在 [qmin，a]區間應為上凸函數。其中，qmax、qmin為關節活動范圍的上、下限，Rq為關節活動范圍的大小。則參數σ需滿足條件:

3.2 自尋優方法

在四連桿模型的每個關節上施加一個虛擬轉矩，若系統保持靜力學平衡，則模型內任意一點都處于平衡狀態。本文以第3臂桿上的點w為對象進行受力分析。

點w受到各關節轉矩和虛擬基座力fbase的綜合作用。設關節i產生的虛擬轉矩ni在點w處的等效力為Fw，i，則

若點w處于靜力平衡狀態，則

式(10)中的力平衡關系是通過虛擬關節轉矩與虛擬基座力的相互作用實現的，本文在此不作討論。力矩平衡關系則是各關節的虛擬轉矩作用相互平衡的結果。

由于每個關節的重要程度存在差異，因此，為了獲得更合理的優化效果，引入關節的加權系數ki，從而機器人的平衡條件滿足:

由于自運動流形的子流形C+、C-內的元素不會在2個流形之間相互變換，則C+、C-內均至少存在一點使得四連桿機構處于靜力平衡狀態。因此機器人的最終優化構型與初始狀態有關。至此，4R機器人的運動優化過程轉化為自運動流形內四連桿機構趨向于平衡狀態的運動過程。

3.3 等效轉矩計算

為了確定機器人的受力狀態，需要根據關節i的驅動轉矩計算出點w處的等效轉矩nw，i。計算等效轉矩的方法很多，可以通過四連桿機構的靜力分析，根據桿件的平衡關系來計算點w處的等效轉矩［15］。本文則根據虛功原理進行等效轉矩的計算。

設n=[n1n2n3]為施加在前3個關節的虛擬轉矩，F=[fxfy0 0 0nz]T為前3個關節在點w處施加的等效作用力，點w處的虛位移為 δx=[δxδy0 0 0δφ ]T，δq為前3個關節的虛擬轉角，根據虛功原理得

由于 δx=J·δq，最終得

式中:(JT)+為雅克比矩陣轉置的M-P逆矩陣。

第四關節的虛擬轉矩在點w處的等效作用力為

根據式(11)和(12)，點w處的合力可以表示為

若M≠0，則第3臂桿在合力M驅動下繞第四關節轉動，臂角ψ發生變化，機器人各關節都隨之轉動，使機構沿著M減小的方向運動，這一運動過程表現為機器人自運動流形內的運動。

令Δψ=λM，λ為步長。臂角的計算表示為

根據式(13)得到變化后的臂角，進而確定下一時刻的機械臂構型。計算過程中應合理設置步長參數，λ太大或太小，都會影響優化速度。

當機構靜力平衡后，機器人優化過程結束。因此，四連桿機構在虛擬轉矩驅動下自運動至平衡狀態的過程，即為機器人二次目標的優化過程。基于自尋優方法的運動優化流程如圖5所示。

圖5 基于自尋優方法的優化流程Fig.5 Workflows of optimization using self-optimizing method

與經典的速度級優化方法相比，自尋優方法具有較高的計算精度及更好的優化效果，可以實現單點位置優化及純位置水平的軌跡優化控制。此外，與位置級的全局搜索方法相比，自尋優方法具有較高的計算效率。因此，本文方法在計算精度、優化效果及計算效率的綜合性能方面具有一定的優勢。

4 仿真驗證

4.1 機器人運動學逆解驗證

已知機器人末端的期望位置P、姿態φ及臂角ψ，設期望的位置參數如下:

根據逆運動學封閉解式(6)～(9)，得到2組逆運動學解，分別為

通過正運動學方程計算得出:q'、q″所對應的末端位姿與期望位姿之間的誤差均小于10－20，從而證明了逆運動學算法的正確性。

4.2 機器人自運動分析

設4R機器人期望位置P和姿態φ與上節一致，每個關節的運動范圍為，加權系數ki=1 ，i=1，2，3，4 。轉矩函數參數 σ =2π。

在關節活動范圍受限的情況下，4R機器人期望末端位姿對應的全部可行解及每組解對應的合力M如圖6(a)所示。圖6(b)表示了可行解區域與q1、q2的關系。可以發現，C+由、兩部分構成，C-由、、3部分構成。C+∩C-≠?，可行解中q2=0成立，這一點也可從圖7得證。

圖6 可行解區域Fig.6 The feasible solutions

圖7表示了機器人所有可行解與合力M變化關系?？尚薪鈪^域由5部分構成:、、、和。由于和相連，可行解區域可以分為4個獨立的區域A、B、C、D。機器人在每個區域內的運動是連續的，且都存在一個平衡狀態，即 SA、SB、SC、SD。此外，在奇異構型附近區域，合力M變化較大，機器人狀態不穩定，如圖中S區域所示。

圖8表示了q2與臂角的關系，通過對比A與C、B與D，可以發現:對應同一臂角，至多存在兩組可行解，進而也證明了C+和C-的存在性。

圖7 可行解與合力M的變化關系Fig.7 Relationship between feasible solutions and M

圖8 q2與ψ的相對關系Fig.8 Relationship between q2and ψ

4.3 期望位姿的逆運動學優化

設機器人期望位置P、姿態φ及轉矩函數與上節一致，ki=1，初始臂角

根據現有機器人運動優化的研究成果，能量最小法往往作為一個典型的優化指標。本文定義機器人運動過程中的能量函數E:

圖9表示了優化過程中合力M及能量E的變化。可以看出，兩者之間的變化趨勢是一致的，都是基于物理意義建立的評價指標。在接近平衡狀態區域時，收斂速度減慢，為了提高運動優化效率，本文定義M≤0.001即視為達到平衡狀態。

圖9 合力M與能量E的變化Fig.9 Relationship between M and E

經過29步計算，得到優化的運動學逆解:q=[－0.865 5 0.988 1 1.185 9－0.261 4 ]rad

圖10表示了優化過程中運動學逆解的變化過程，與優化之前相比，優化后的關節位置更好的避開了關節限位。

圖10 優化過程中q的變化Fig.10 Changes of q in optimizations

4.4 軌跡跟蹤優化驗證

已知末端姿態為φ，機器人末端軌跡函數為

軌跡跟蹤過程中逆運動學最優解如圖11所示，可以看出，優化后的關節位置距關節極限位置均有一定裕度。圖12表示了優化過程中機器人很好的達到了平衡狀態。圖13、14分別表示了軌跡跟蹤過程中機器人末端的位置與姿態誤差。仿真結果表明，基于自尋優方法的運動優化具有較高的跟蹤精度。

圖11 最優解Fig.11 The optimized solutions

圖12 優化過程中合力M變化Fig.12 Changes of M in optimizations

圖13 軌跡跟蹤的位置誤差Fig.13 Position error during trajectory tracking

圖14 軌跡跟蹤的姿態誤差Fig.14 Orientation error during trajectory tracking

5 結束語

本文對平面四自由度機器人的自運動流形進行了分析，并提出一種自尋優算法，可以快速、有效的實現機器人的運動優化。自尋優方法適用于任意構型的冗余自由度機器人運動優化問題，尤其在單冗余自由度機器人運動優化問題中更能體現其優勢。在后續的工作中有以下2個問題有待繼續研究:

1)自尋優方法中初始參數的選擇原則及各參數對優化結果的影響規律，為改善運動優化效果提供理論依據;

2)基于自尋優方法的冗余自由度機器人多目標運動優化問題，以保證機器人的整體操作性能。

［1］SHIMIZU M，KAKUYA H，YOON W K，et al.Analytical inverse kinematic computation for 7-DOF redundant manipulators with joint limits and its application to redundancy resolution［J］.IEEE Transactions on Robotics，2008，24(5):1131-1142.

［2］DUBEY R V，EULER J A，BABCOCK S M.An efficient gradient projection optimization scheme for a seven-degreeof-freedom redundant robot with spherical wrist［C］//IEEE International Conference on Robotics and Automation.Philadelphia，USA，1988:28-36.

［3］GOLDENBERG A，BENHABIB B，FENTON R.A complete generalized solution to the inverse kinematics of robots［J］.IEEE Journal of Robotics and Automation，1985，1(1):14-20.

［4］MAO Z，HSIA T C.Obstacle avoidance inverse kinematics solution of redundant robots by neural networks［J］.Robotica，1997，15(1):3-10.

［5］YU C，JIN M，LIU H.An analytical solution for inverse kinematic of 7-DOF redundant manipulators with offset-wrist［C］//International Conference on Mechatronics and Automation.Chengdu，China，2012:92-97.

［6］杜廣龍，張平.基于人工勢場的機器人遙操作安全預警域動態生成方法［J］.機器人，2012，34(1):44-49.DU Guanglong，ZHANG Ping.A method for generating dynamic security warning region in robotic teleoperation based on artificial potential field［J］.Robot，2012，34(1):44-49.

［7］董春，楊耕，徐文立.七自由度冗余仿人臂的障礙實時回避［J］.清華大學學報:自然科學版，2004，44(10):1341-1344.DONG Chun，YANG Geng，XU Wenli.Real-time collision avoidance for 7-DOF redundant anthropomorphic arm［J］.Journal of Tsinghua University:Science and Technology，2004，44(10):1341-1344.

［8］KHATIB O.Real-time obstacle avoidance for manipulators and mobile robots［J］.International Journal of Robotics Research，1986，5(1):90-98.

［9］SICILIANO B.Kinematic control of redundant robot manipulators:a tutorial［J］.Journal of Intelligent and Robotic Systems，1990，3(3):201-212.

［10］陽方平，李洪誼，王越超，等.一種求解冗余機械臂逆運動學的優化方法［J］.機器人，2012，34(1):17-21.YANG Fangping，LI Hongyi，WANG Yuechao，et al.An optimization method for solving the inverse kinematics of redundant manipulator［J］.Robot，2012，34(1):17-21.

［11］孫立寧，劉宇.基于擴展雅可比矩陣的冗余度機器人多性能準則優化研究［J］.機器人，2003，25(7):589-592.SUN Lining，LIU Yu.Research on multiple performance criteria optimization of a redundant robot based on extended Jacobian matrix［J］.Robot，2003，25(7):589-592.

［12］DAS H，SLOTINE J E，SHERIDAN T B.Inverse kinematic algorithms for redundant systems［C］//IEEE International Conference on Robotics and Automation.Philadelphia，USA，1988:43-48.

［13］XIA Y，WANG J.A dual neural network for kinematic control of redundant robot manipulators［J］.IEEE Transactions on Systems，Man，and Cybernetics-Part B:Cybernetics，2001，31(1):147-154.

［14］葛新鋒，趙東標.七自由度自動鋪絲機器人參數化的自運動流形［J］.機械工程學報，2012，48(13):27-31.GE Xinfeng，ZHAO Dongbiao.Parameterized self-motion manifold of 7-DOF automatic fiber placement Robotic manipulator［J］.Journal of Mechanical Engineering，2012，48(13):27-31.

［15］萬克剛.平面四連桿機構的靜力分析［J］.南昌大學學報:工科版，1986，8(1):58-62.WAN Kegang.The static analysis of four-bar-mechanical in a plane［J］.Journal of Jiangxi Polytechnic University:Engineering and Technology，1986，8(1):58-62.