基于同倫延拓的冗余機械臂逆運動學優化算法研究

張國慶，李宗道，吳劍雄，顧浩宇，李清都

基于同倫延拓的冗余機械臂逆運動學優化算法研究

張國慶，李宗道，吳劍雄，顧浩宇，李清都*

（上海理工大學，上海 200093）

針對偏置冗余機械臂的逆運動學，采用傳統數值法存在依賴初始值、奇異位姿收斂性差等問題，提出一種改進數值法。首先將非線性方程組轉化為同倫方程組，引入同倫延拓算法能夠有效避免依賴初始值的問題，同時能夠獲取逆運動學解空間。然后考慮奇異位姿，將同倫方程組轉化為最小二乘問題，采用Levenberg Marquardt算法對同倫方程組進行路徑追蹤，以獲取逆運動學解空間。最后將關節極限避免問題映射為解空間優化問題，引入二進制改進粒子群優化算法，獲得最優逆運動學解。實驗結果表明，相較于傳統數值法，文中所提數值法針對逆運動學求解具有更高的收斂率、更快的收斂速度，同時二進制改進粒子群算法能夠有效避免關節極限問題。采用文中所提數值法求解逆運動學的精度較高，能夠滿足實時性要求，對于機械臂用于包裝作業具有一定的理論意義和工程應用價值。

冗余機械臂；逆運動學；Levenberg Marquardt；同倫延拓；二進制改進粒子群算法

機器人技術廣泛應用于包裝行業，針對包裝的不同類型（如產品裝箱、分揀等），需要機械臂高速、高精度地操作，以提高其生產效率。冗余機械臂關節空間的維數大于任務空間的維數，導致七自由度機械臂逆運動學欠約束[1]，增大了逆運動的復雜性。

許多學者已經提出了多種方法來解決冗余自由度機械臂的逆運動學問題，其中包括基于位置的解析法[2]、基于智能優化算法求解和基于雅可比的數值解。在2007年，Shimizu等[3]引入“臂角”的概念，并作為參考平面來描述肘部的自運動特性，進而推導出期望肘部位置時所有關節的表達式?；谖恢玫慕馕龇椒▽C械臂的幾何構型提出了較高的要求，因此不適用于帶偏置結構的機械臂。

近年來，智能算法通過將機器人運動學方程轉化為最優控制問題進行求解。這類算法主要包含粒子群優化算法（Partical Swarm Optimization, PSO)[4]、遺傳算法（Genetic Algorithm, GA）[5]、神經網絡算法（Neural Network Algorithm, NNA）[6]和模擬退火算法（Simulated Annealing Algorithm, SAA）[7]。雖然優化算法能夠有效解決奇異位姿逆運動學問題，但復雜的計算需要昂貴的時間成本，這不利于實時控制。

目前，牛頓拉夫森法（Newton Raphon, NR）[8]是最常見的數值求解方法，但是NR在以下2種情況下會收斂失敗：奇異位姿、初始點距離目標點過遠。房立金等[9]采用線性插值方法，在每個插值點進行逆運動學求解，逐步逼近目標點，但是未從根本上解決問題。高斯牛頓法（Gauss Newton Raphon, GNR）[10]通過最小二乘問題最小化，獲取了誤差最小的逆運動學解。阻尼牛頓法（Damping Newton Raphon, DNR）[11]通過在每次迭代過程中引入一個阻尼因子，能夠改善DNR中存在的發散和局部最小值問題，但卻降低了收斂速度。Levenberg Marquardt算法[12]結合了高斯牛頓法和阻尼牛頓法的思想，提高了算法的穩定性。Ananthanarayanan等[13]應用Levenberg Marquardt算法找到逆運動學方程組的根，具有較好的穩定性。上述數值法都不能有效避免對初始值的過度依賴性，其收斂速度較慢，易出現不收斂問題，從而影響算法的魯棒性。

文中針對傳統數值法對初始值敏感及冗余機械臂逆運動學解空間等問題，將奇異性考慮在內，提出一種改進的數值法，將Levenberg Marquardt算法與同倫延拓算法融合，以獲得逆運動學解空間，并且針對關節極限規避問題進行優化處理。最后，對文中所提逆運動學算法進行仿真驗證，進一步探究機械臂在包裝自動化中的應用。

1 冗余機械臂運動鏈分析

采用帶偏置七自由度機械臂，相應的MDH結構和MDH參數如圖1、表1所示。

圖1 七自由度機械臂MDH結構

表1 七自由度機械臂MDH參數

Tab.1 MDH parameters of the 7-DOF manipulator

根據MDH參數表，計算得到機械臂相鄰關節之間的齊次變換矩陣，見式（1）。

當各個關節角度已知時，得出各相鄰關節之間的齊次變換矩陣，將7個齊次變換矩陣相乘，得到機械臂末端位姿相對于基座的齊次變換矩陣，見式（2）。

2 冗余機械臂逆運動學求解

同倫延拓算法（Homotopy Continuation Method，HCM）與傳統的數值迭代法相比，具有不依賴初始值、求解效率高、只需改變輔助同倫函數就能無發散地求出方程所有解等優點[14]，因此將Levenberg Marquardt（LM）算法與HCM相結合，以提高逆運動學數值法的穩定性和收斂速度。

2.1 牛頓同倫延拓算法

七自由度機械臂的逆運動學數學模型是一個混合三角函數多項式系統的高度非線性方程組，在滿足末端執行器位姿的情況下，為機械臂找到一組可行的關節角度，具體方程組見式（3）。

常用的牛頓法求解該方程組的迭代見式（4）。

Wu[17]詳細介紹了輔助同倫函數的選擇依據，將同倫方程定義為式（6）。

2.2 Levenberg-Marquardt同倫延拓算法

Shafiee-Ashtiani使用四階同倫延拓算法，通過改變輔助同倫函數解決并聯機械臂正運動學多解問題，并成功獲取了全部16組正向運動學解。將奇異位姿考慮在內，文中采用LM算法對同倫方程進行校正，目的是避免傳統數值法對初始值依賴的問題，且能夠獲取逆運動學解空間。

當機械臂處于奇異位姿時，LM算法通過添加阻尼項，能夠有效計算出逆運動學解，并且具有更快的收斂性和魯棒性。將式（3）表示為最小二乘最小化問題，殘差的計算見式（9）。

將式（10）帶入式（9），并添加阻尼項，可得式（11）。

(T+λI)δ=T(12)

3 關節極限規避

LMHCM獲取的逆運動學解空間為七維離散空間，為了在解空間中找到一組滿足極限要求的最優解，將關節極限避免問題轉化為最優問題。二進制粒子群優化算法（Binary Particle Swarm Optimization, BPSO）是求解二進制（離散）優化問題常用的群體智能算法之一，具有參數少、結構簡單、執行速度快等特點[19]。Liegeois[20]利用機械臂的關節范圍中心來優化關節運動，則任意關節的勢函數見式（17）。

4 實驗仿真驗證

為了驗證文中所提逆運動學算法的有效性，以冗余機械臂（圖1）為例進行仿真驗證，其連桿參數和關節運動范圍如表1所示。實驗基于AMD R7-5800H 3.2 GHz CPU的Windows系統下的Matlab 2021a進行。

4.1 逆運動學求解驗證

采用Matlab軟件中的機器人工具箱（Robotic System Toolbox）進行仿真驗證，機械臂目標位姿如圖2所示。

圖2 機械臂目標位姿

4.2 關節極限避免驗證

為了驗證所提關節極限避免算法的有效性，進行如下仿真。在仿真開始前，設置粒子群的數量為7，最大學習因子均為1.5，最小學習因子均為0.5，最大慣性權重為0.9，最小慣性權重為0.4，粒子的最大速度為5，最小速度為?5，且將迭代步驟的數量設置為50。研究的機械臂末端位姿見式（22），初始姿態為機械臂起始關節角度。首先，通過逆運動學求得該目標位姿對應的100組關節角度，然后根據機械臂的關節極限（表1）及式（18）建立目標函數。通過式（20）～（21）不斷更新粒子的搜索方向及位置。

圖4反映了SABPSO算法在搜索過程中目標函數的變化情況。當函數取值最小時，即為遠離關節極限的最優關節角度。由圖4可知，將文中所提的逆運動學求解方法LMHCM與SABPSO算法相結合，可以實現關節極限規避。

4.3 實驗對比驗證

從表3可以看出，傳統NR、LM算法求解逆運動學的收斂能力分別僅有37%、56%，平均運行時間需要0.126 6、0.139 5 s。雖然NRHCM算法具有較高的收斂率，但仍無法處理奇異位姿下的逆運動學，且收斂速度相較于NR、LM算法更慢，不適于實時控制。文中提出的算法LMHCM的收斂能力達到了97%，運行時間僅需0.04 s左右。可以得出結論，文中所提算法LMHCM相較于傳統的數值法具有更高的收斂率和收斂速度。

表2 逆運動學解

Tab.2 Inverse kinematics solution

圖3 位姿誤差曲線

圖4 適應度函數變化曲線

表3 4種數值法實驗效果對比

Tab.3 Comparison of experimental results of four numerical methods

圖6 奇異位姿同倫路徑追蹤對比

如圖5所示，在非奇異位姿下，文中提出的算法LMHCM具有更好的收斂性和平滑性。如圖6所示，在奇異位姿下，采用NRHCM逆運動學求解會導致發散，原因是不能計算得到雅克比的逆，而LMHCM能夠保持良好的收斂效果。

4.4 機械臂搬運應用案例

在實際工程應用中，常使用機械臂搬運產品線上的產品。文中提出的逆運動學算法可以更精確地計算出機械臂抓取產品各關節的角度，進而運動到產品位置進行搬運。文中設計了一個模擬產品線上機械臂抓起搬運的任務，機械臂需要將點的產品經過點運送到點，最終回到起始位置。假設產品的位置、中間路徑點及終止點的位姿信息已知，機械臂末端執行器抓取點的位姿如表4所示，位置坐標為[,,]，姿態坐標采用歐拉角描述為[,,]。根據文中所提LMHCM逆運動學算法計算上述各位姿的關節角度，結果見表5。通過逆運動學計算得到3個路徑點的關節角度后，在關節空間采用三次樣條曲線進行軌跡規劃，末端執行器運行軌跡如圖7所示。經過仿真驗證，機械臂能夠以預定的運動精度完成待包裝產品的抓取搬運工作。

表4 抓取點位姿坐標

Tab.4 Pose coordinate of grabbing point

表5 抓取點關節角度

Tab.5 Joints angles of grabbing point

圖7 末端執行器搬運軌跡

5 結語

為了獲得具有自運動特性和關節極限規避的帶偏置七自由度機械臂運動學逆解，提出了一種改進數值求解方法。將LM算法與HCM相結合，不僅有效避免了數值法對初始值敏感的問題，提高了數值法的魯棒性和收斂速度，而且利用同倫延拓算法能夠獲取逆運動學解空間。同時，將避免關節極限映射為解空間中關節角度的最優選擇問題，建立適應度函數為優化目標，利用SABPSO算法搜索出一組在關節極限內的最優關節角度。仿真驗證了所提出的逆運動學求解空間方法及關節極限避免算法的正確性和可行性，對于機械臂后續用于包裝作業具有一定的理論意義和工程應用價值。

[1] SHI X H, GUO Y, CHEN X C, et al. Kinematics and Singularity Analysis of a 7-DOF Redundant Manipulator[J]. Sensors, 2021, 21(21): 7257.

[2] ZHANG Q. Robotics Mechanisms, Kinematics, Dynamics and Motion Planning[J]. 2019, 15(3): 125-141.

[3] SHIMIZU M, KAKUYA H, YOON W K, et al. Analytical Inverse Kinematics for 7 DOF Redundant Manipulators with Joint Limits[J]. Journal of the Robotics Society of Japan, 2007, 25(4): 606-617.

[4] 朱經緯, 方虎生, 邵發明, 等. 自適應粒子群算法求冗余機械臂逆運動學解[J]. 計算機工程與應用, 2019, 55(14): 215-220.

ZHU Jing-wei, FANG Hu-sheng, SHAO Fa-ming, et al. Self-Adaptive Particle Swarm Optimization Algorithm for Solving Inverse Kinematics Problem of Redundant Manipulator[J]. Computer Engineering and Applications, 2019, 55(14): 215-220.

[5] 張立博, 李宇鵬, 朱德明, 等. 基于遺傳算法的護理機器人逆運動學求解方法[J]. 北京航空航天大學學報, 2022, 48(10): 1925-1932.

ZHANG Li-bo, LI Yu-peng, ZHU De-ming, et al. Inverse Kinematic Solution of Nursing Robot Based on Genetic Algorithm[J]. Journal of Beijing University of Aeronautics and Astronautics, 2022, 48(10): 1925-1932.

[6] GAO R H. Inverse Kinematics Solution of Robotics Based on Neural Network Algorithms[J]. Journal of Ambient Intelligence and Humanized Computing, 2020, 11(12): 6199-6209.

[7] K?KER R. A Neuro-Simulated Annealing Approach to the Inverse Kinematics Solution of Redundant Robotic Manipulators[J]. Engineering with Computers, 2013, 29(4): 507-515.

[8] LEE S, LEE J, BANG J, et al. 7 DOF Manipulator Construction and Inverse Kinematics Calculation and Analysis Using Newton-Raphson Method[C]// 2021 18th International Conference on Ubiquitous Robots (UR). IEEE, 2021: 235-238.

[9] 房立金, 高瑞. 一般6R機器人逆運動學算法的改進[J].機械科學與技術, 2018, 37(9): 1325-1330. FANG Li-jin, GAO Rui. Improving Inverse Kinematics Algorithm for General 6-DOF Robots[J]. Mechanical Science and Technology for Aerospace Engineering, 2018, 37(9): 1325-1330.

[10] XIE Y E, WU X D, SHI Z, et al. The Path Planning of Space Manipulator Based on Gauss–Newton Iteration Method[J]. Advances in Mechanical Engineering, 2017, 9(8): 168781401771847.

[11] LIAO L C, CHEN W B, YAN D, et al. Solution of Inverse Kinematic Problem for Serial Robot Based on Screw Theory[C]// SPIE Smart Structures + Nondestructive Evaluation Proc SPIE 11376, Active and Passive Smart Structures and Integrated Systems XIV, 2020: 492-496.

[12] WU L, YANG X D, MIAO D J, et al. Inverse Kinematics of a Class of 7R 6-DOF Robots with Non-Spherical Wrist[C]// 2013 IEEE International Conference on Mechatronics and Automation, 2013: 69-74.

[13] ANANTHANARAYANAN H, ORDó?EZ R. Real- Time Inverse Kinematics of (2+1) DOF Hyper-Redundant Manipulator Arm via a Combined Numerical and Analytical Approach[J]. Mechanism and Machine Theory, 2015, 91: 209-226.

[14] SHAFIEE-ASHTIANI M, YOUSEFI-KOMA A, IRAVANIMANESH S, et al. Kinematic Analysis of a 3-UPU Parallel Robot Using the Ostrowski-Homotopy Continuation[C]// 2016 24th Iranian Conference on Electrical Engineering (ICEE), 2016: 1306-1311.

[15] LAFMEJANI A S, KALHOR A, MASOULEH M T. A New Development of Homotopy Continuation Method, Applied in Solving Nonlinear Kinematic System of Equations of Parallel Mechanisms[C]// 2015 3rd RSI International Conference on Robotics and Mechatronics (ICROM), 2015: 737-742.

[16] WU T M. Solving the Nonlinear Equations by the Newton-Homotopy Continuation Method with Adjustable Auxiliary Homotopy Function[J]. Applied Mathematics and Computation, 2006, 173(1): 383-388.

[17] WU T M. A Study of Convergence on the Newton-Homotopy Continuation Method[J]. Applied Mathematics and Computation, 2005, 168(2): 1169-1174.

[18] LOVE I B. Levenberg-Marquardt Algorithm in Robotic Controls[D]. Washington: University of Washington, 2020: 10-42.

[19] BEHESHTI Z. A Time-Varying Mirrored S-Shaped Transfer Function for Binary Particle Swarm Optimization[J]. Information Sciences: an International Journal, 2020, 512(1): 1503-1542.

[20] LIEGEOIS A. Automatic Supervisory Control of the Configuration and Behavior of Multibody Mechanisms[J]. IEEE Transactions on Systems, Man, and Cybernetics, 1977, 7(12): 868-871.

Inverse Kinematics Optimization Algorithm of Redundant Manipulator Based on Homotopy Continuation

ZHANG Guoqing,LI Zongdao,WU Jianxiong,GU Haoyu,LI Qingdu*

(University of Shanghai for Science and Technology, Shanghai 200093, China)

The work aims to propose an improved numerical method to solve problems of dependence on the initial value and poor convergence of singular poses in the traditional numerical method in terms of complex inverse kinematics of the offset redundant manipulator.First, the nonlinear equations were transformed into homotopy equations. The introduction of homotopy continuation algorithm could effectively avoid the initial value dependence problem, and at the same time obtain the inverse kinematics solution space. Then taking the singular pose into account, the homotopy equations were transformed into a least squares problem, and the Levenberg Marquardt algorithm was used to track the path of the homotopy equations to obtain the inverse kinematics solution space. Finally, the joint limit avoidance problem was mapped to a solution space optimization problem, and the binary improved particle swarm optimization algorithm was introduced to obtain the optimal inverse kinematics solution. The experimental results showed that the numerical method proposed in this paper had a higher convergence rate and faster convergence speed for inverse kinematics than the traditional numerical method, and the binary improved particle swarm optimization algorithm could effectively avoid the joint limit problem. This numerical method has high accuracy in solving inverse kinematics and can meet real-time requirements, and has certain theoretical significance and engineering application value for the subsequent use of the manipulator in packaging operations.

redundant manipulator; inverse kinematics; Levenberg Marquardt; homotopy continuation; binary improved particle swarm algorithm

TP241；TB486

A

1001-3563(2024)07-0197-08

10.19554/j.cnki.1001-3563.2024.07.025

2023-07-10

國家自然科學基金（92048205）

通信作者