基于MATLAB的機械臂運動學分析和軌跡規劃

馬艷艷，曾臺英，江海林

基于MATLAB的機械臂運動學分析和軌跡規劃

馬艷艷，曾臺英，江海林

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

研究包裝機械臂運動學和軌跡規劃，以實現機器臂的平穩運行和精確控制。以六自由度包裝機械臂模型為研究對象，采用改進的D–H參數法建立坐標系，推導運動學方程，運用MATLAB搭建機械臂模型并進行正逆運動學仿真，以驗證模型的正確性。其次，采用五次多項式插值算法對機械臂關節空間進行軌跡規劃。最后，針對包裝作業中待包裝產品的定點抓取搬運任務進行軌跡規劃和仿真。搭建的機械臂模型正確，五次多項式插值法軌跡優化效果好，可保證各關節角度、速度和加速度曲線光滑連續；計算出抓取搬運任務中各關節的關節量，并得到了末端執行器運動軌跡曲線。機械臂能以預期的運動精度完成抓取搬運的動作，為進一步探究機械臂的運動控制和實際應用奠定了基礎。

六自由度機械臂；包裝機械；軌跡規劃；多項式插值法

在包裝行業中，包裝機械占據著十分重要的位置。在某種程度上，包裝機械水平直接決定了整個包裝行業的發展水平[1-2]。傳統的手工包裝作業由于勞動強度大、生產效率低、質量不穩定等缺點已經無法滿足人們日益增長的生產生活的需要，因此，機械臂以其智能、高效、安全等優點在包裝作業中得到了越來越廣泛的應用。由于抓取搬運等工序對包裝流水線的高效工作非常重要，因此對機械臂的要求也越來越高[3-5]。

為了提高機械臂的運動精度和穩定性，對其進行運動學分析和軌跡規劃是機器人技術的核心內容之一，國內外很多學者在這一領域進行了研究。付香雪等[6]基于標準D–H法對包裝分揀機械手進行了運動學分析和軌跡規劃，結果表明建立的運動學方程正確可行，仿真所獲得的數據可以應用到實際機械臂的運動控制中，實現準確、可靠的運行。孫紹杰等[7]基于多項式插值對機械臂軌跡規劃方法進行了研究，結果表明插值算法實現了機械臂的軌跡規劃要求。Parikh等[8]以五自由度機器人為例研究了利用六次多項式計算多自由度機器人的光滑軌跡方法，可減少對末端執行器的沖擊，保護關節免受機械振動。Guo等[9]使用直接方法來獲得最佳軌跡規劃和最大動態載荷，提出了一種五自由度混合動力機械臂的容量算法。文中以六自由度機械臂為研究對象，利用改進的D–H參數法建立機械臂的運動模型，用MATLAB搭建機械臂仿真模型并進行正逆運動學分析，最后用多項式插值算法進行關節空間軌跡規劃，并對實際工程應用進行仿真驗證，以期為進一步探究機械臂運動的精確控制和實際在包裝自動化中的應用奠定基礎。

1 包裝機械臂運動學分析

1.1 機械臂D–H模型

文中以某六自由度包裝機械臂為研究對象，該機械臂是串聯開鏈式機械臂，見圖1。坐標系{0}是基坐標系，其中坐標系{0}和坐標系{1}重合，坐標系{3}和坐標系{4}重合，軸2、軸5和軸6相互平行，軸1和軸3相互平行。已知兩軸朝向，可由右手法則確定第三軸[10]。

考慮到改進的D–H方法有著便于建模和計算的優點，文中在描述相鄰連桿和相鄰關節的相對位置關系時采用了改進的D–H方法建立坐標系。與原D–H方法相比，改進D–H方法坐標系前置，即改進的D–H方法是描述連桿?1至連桿的相對位置關系[10]。按照改進D?H方法，確定六自由度機械臂的改進D?H參數，見表1。

表1 六自由度機械臂改進D–H參數

Tab.1 Modified D-H parameter for 6-DOF manipulator

將表1中各關節參數代入式（1）可得各個關節的變換矩陣，然后將這些變換矩陣相乘，則可得基坐標系和末端執行器間的變換矩陣為：

由式（2）可得坐標系{6}相對于坐標系{0}的變換矩陣，通過計算可以獲得機械臂末端執行器的位姿。

1.2 MATLAB仿真模型及正逆運動學分析

根據改進的D–H參數，用MATLAB Robotics Toolbox工具箱搭建包裝機械臂仿真模型。機械臂初始狀態時，各關節參數為0，機械臂末端空間位置為（0, ?105, 80），建立的機械臂仿真模型見圖2。

對模型進行正運動學分析，起始點位置為[0 0 0 0 0 0]，終止點位置為[3×π/4 π/8 0 π/5 π/6 0]。用正運動學求解指令fkine解得初始點位姿矩陣a見式（3），終止點位姿矩陣b見式（4）。

圖2 機械臂仿真模型

對模型進行逆運動學分析，用逆運動學求解指令ikine來求解各個關節的角度，起始位置的關節角度a=robot.ikine（a），終止位置的關節角b=robot.ikine（b），將其輸入到Matlab命令窗口解得：a=[0 0 0 0 0 0]，b=[2.356 2 0.392 7 0.314 2 0.314 2 ?2.879 8 ?2.879 8]。結果表明關節角a與起始點位置相一致，關節角b與起始點位置不一致，再對關節角b進行正運動學分析可得位姿矩陣c，且c=b。結果表明，對逆運動學計算求出的關節角再進行正運動學求解得出的位姿矩陣c與正運動學求解得出的位姿矩陣b一致，從而驗證了機械臂的參數設定和仿真模型是正確的。

2 機械臂軌跡規劃

機械臂的軌跡規劃目的是在完成作業任務的過程中，保證各個角關節變量的位置、速度及加速度函數曲線是連續的，且在起點和終點位置的速度和加速度均為零。通過軌跡規劃可實現機械臂的平穩運動，減少振動和沖擊，保證其運動軌跡精度的同時還可以避免額外的能量消耗，減少機械部件磨損，提高運動穩定性和準確度，從而提高作業的效率和任務完成的效果。

機械臂軌跡規劃可分為笛卡爾空間軌跡規劃和關節空間軌跡規劃，由于笛卡爾空間軌跡規劃要進行逆運動學求解得出各關節的關節角，求解不便，而關節空間的軌跡規劃通過控制各個關節角的規劃軌跡，可實現機械臂的實時控制[11-12]。根據包裝作業的要求，機器人的軌跡規劃為點到點運動，因此，可采用關節空間軌跡規劃，通過控制各個關節角來實現機械臂的實時控制。

2.1 多項式插值法

關節空間軌跡規劃是用逆運動學方法將笛卡爾空間中的路徑點轉換為其對應的關節角，然后用插值計算法計算機械臂的各個關節插值點，使其各個關節同步并且順滑地到達目標點?，F在進行軌跡規劃時大多采用多項式插值計算方法中的三次多項式和五次多項式插值函數。由于低于三次函數的插值方法的階次低，求導得到的加速度是常數，故無法保證加速度軌跡的連續性，而高次多項式插值函數的階次過高會使插值區間兩端出現大幅度的縱向波動[13]。利用三次多項式插值法得到的機械臂運動的關節角度、角速度是連續的，其缺點是沒有對關節角加速度進行約束，因此其加速度有突變，且加速度的變化率很大，這會導致機械臂運動不穩定，機械系統會產生一定的沖擊[14-15]。五次多項式插值法不僅對路徑起點和終點的角度和角速度進行約束，還對角加速度有約束，故可保證關節角度、角速度和角加速度的光滑連續。綜上，五次多項式插值法為關節空間軌跡規劃的理想算法[16]。

已知在初始和終止時刻的關節位置、速度和加速度，則可采用五次多項式保證關節角度、角速度以及角加速度的光滑連續，即[10]：

若初始關節和終止關節的角度、角速度和角加速度的6個約束條件為：

對式（5）求一階導數可得角速度函數為：

對式（5）求二階導數可得角加速度函數為：

聯立式（6）—（8）的方程組求解可得五次多項式系數為：

2.2 軌跡規劃仿真結果與分析

上述機械臂從初始點（0 ?105 80）到終止點（64.7 67.5 87.4）末端執行器的位移見圖3。在0～2 s時，關節1角度從0°到135°，關節2角度從0°到22.5°，關節3和關節6角度不變，關節4角度從0°到36°，關節5角度從0°到30°。

圖3 末端執行器位移圖

設初始時間=0 s和終止時間f=2 s，對各關節的關節角、關節角速度以及關節角加速度進行仿真分析，得到了機械臂的各關節位置、速度和加速度變化的曲線圖，結果見圖4—6。

從圖4—6中可以看出，各關節的啟停角速度、角加速度均為零，機械臂各關節位置、速度和加速度曲線均光滑連續，得到的各關節最大角度、角速度、角加速度差別不大。關節3和關節6不轉動，關節1、2、4和5在運動過程中角速度先從零逐漸增大到極值，然后又逐漸減小到零，而關節角加速度有2個方向相反的極值點，運動過程中加速度先從零增大到極大值后減小，又反方向增大到極值后再減小到零，說明機械臂的運動是一個先加速后減速的過程。符合機械臂實際運動過程中加速度先增大后減小的實際情況。因此，仿真分析結果與實際使用情況相符合。

從機械臂各關節位置、角速度和角加速度的仿真結果表明，五次多項式插值法軌跡規劃使得各個關節角的角度、角速度以及角加速度呈光滑連續的曲線，運動時期無突變現象，機械臂從初始點運動到終止點的過程中，各關節運動平穩，無沖擊和振動，各關節性能良好。綜上，可以證明前面所建立的模型正確，且五次多項式插值法優化效果好，能夠較好地解決關節空間軌跡規劃的問題，同時也驗證了多項式插值法的有效性，確保了機械臂較高的工作效率和平穩性。

圖4 各關節位置曲線

圖5 各關節角速度曲線

圖6 各關節角加速度曲線

3 機械臂軌跡運動試驗

在實際的工程應用中，包裝作業中常使用機械臂運送流水線傳送帶上的產品，即將傳送帶上的產品搬運到目的點處，這種情況下機械臂運行軌跡的起始點和終止點均是固定的，可采用PTP點位作業。設計一個模擬生產線上的定點抓取搬運任務，給定4個關鍵路徑點，機械臂用來搬運固定位置的工件，送至固定位置的存放點?，F以機械臂的基坐標系為參考坐標系，設待包裝產品起始點位置為1，將產品搬運到目標點4的位置，給定2個中間路徑點2、3，機械臂抓取點目標和其位置坐標見表2。根據MATLAB進行仿真計算各關節的關節量，由逆運動學求解可得各路徑點關節空間的變量值，見表3。機械臂執行器末端位置的運行軌跡見圖7。仿真結果證明了軌跡規劃的正確性，表明機械臂能以預期的運動精度完成待包裝產品的抓取搬運工作。

表2 抓取點位置坐標

Tab.2 Position coordinate of grabbing point

表3 關節路徑點

Tab.3 Joint path point

圖7 末端執行器運動軌跡曲線

4 結語

以六自由度機械臂為研究對象，首先采用改進的D–H參數法建立了坐標系，推導運動學方程，運用MATLAB搭建機械臂仿真模型并進行正逆運動學分析，驗證了模型的正確性。其次，采用五次多項式插值算法對機械臂關節空間進行了軌跡規劃，給定機器人初始位姿和終止位姿，對末端執行器的運動軌跡進行分析，得到了各關節的運動曲線、角速度曲線和角加速度曲線，并對仿真結果進行了分析。結果表明，五次多項式插值法優化效果好，各個關節角的角度、角速度以及角加速度呈光滑連續的曲線，運動過程中加速度先增大后減小，運動時期無突變現象，能夠很好地解決關節空間軌跡規劃的問題，同時也驗證了五次多項式插值算法的有效性，確保了機械臂較高的工作效率和運動平穩性。最后針對包裝作業中的待包裝產品的定點抓取搬運任務，根據MATLAB進行仿真計算各關節的關節量，并得到了末端執行器運動軌跡曲線。仿真結果表明，機械臂能夠以預期的精度完成抓取搬運作業，達到預期運動精度和平穩性的要求。機器臂軌跡規劃的順利完成，可保證機械臂在作業環境中運動平穩和安全可靠，為機器臂的平穩、實時和精確控制奠定了理論基礎，對機械臂后續用于包裝作業具有一定的理論意義和工程應用價值。

[1] 溫景陽, 杜宇凡, 秦濤, 等. 基于Dobot機械臂的包裝自動線運動規劃與仿真[J]. 包裝工程, 2021, 42(21): 231-238.

WEN Jing-yang, DU Yu-fan, QIN Tao, et al. Motion Planning and Simulation of Automatic Packaging Line Based on Dobot Manipulator[J]. Packaging Engineering, 2021, 42(21): 231-238.

[2] 曹華, 李偉. 基于遺傳算法的包裝機械臂位置精度控制方法[J]. 包裝工程, 2021, 42(9): 249-253.

CAO Hua, LI Wei. Position Accuracy Control Method of Packaging Manipulator Based on Genetic Algorithm[J]. Packaging Engineering, 2021, 42(9): 249-253.

[3] 李憲華, 范凱杰, 疏楊, 等. 六自由度模塊化機械臂的軌跡規劃與仿真[J]. 制造技術與機床, 2018(9): 73-77.

LI Xian-hua, FAN Kai-jie, SHU Yang, et al. Trajectory Planning and Simulation of Six Degrees of Freedom Modular Manipulator[J]. Manufacturing Technology & Machine Tool, 2018(9): 73-77.

[4] 梅江平, 臧家煒, 喬正宇, 等. 三自由度Delta并聯機械手軌跡規劃方法[J]. 機械工程學報, 2016, 52(19): 9-17.

MEI Jiang-ping, ZANG Jia-wei, QIAO Zheng-yu, et al. Trajectory Planning of 3-DOF Delta Parallel Manipulator[J]. Journal of Mechanical Engineering, 2016, 52(19): 9-17.

[5] 鄭濤, 劉滿祿. 一種機械臂分段插值軌跡規劃方法[J]. 機械設計與制造, 2020(3): 261-264.

ZHENG Tao, LIU Man-lu. A Method of Segmental Interpolation Trajectory Planning for Robotic Arm[J]. Machinery Design & Manufacture, 2020(3): 261-264.

[6] 付香雪, 韓順杰, 張冬冬, 等. 基于D–H法的包裝分揀機械手運動學分析和軌跡規劃[J]. 包裝與食品機械, 2018, 36(5): 47-50.

FU Xiang-xue, HAN Shun-jie, ZHANG Dong-dong, et al. Kinematics Analysis and Trajectory Planning of Package Sorting Manipulator Based on D-H Method[J]. Packaging and Food Machinery, 2018, 36(5): 47-50.

[7] 孫紹杰, 齊曉慧, 蘇立軍, 等. 基于多項式插值的五自由度機械臂軌跡規劃方法研究[J]. 機電一體化, 2011, 17(11): 33-37.

Sun Shao-jie, Qi Xiao-hui, Su Li-jun, et al. Trajectory Planning of 5 - DOF Manipulator Based on Plynomial Method[J]. Mechatronics, 2011, 17(11): 33-37.

[8] PARIKH P A, TRIVEDI R R, JOSHI K D. Trajectory Planning of a 5 DOF Feeding Serial Manipulator Using 6th Order Polynomial Method[C]// Journal of Physics: Conference Series, Bristol, 2021.

[9] GUO Wan-jin, LI Rui-feng, CAO Chu-qing, et al. A New Methodology for Solving Trajectory Planning and Dynamic Load-Carrying Capacity of a Robot Manipulator[J].Mathematical Problems in Engineering, 2016, 1302537: 1-28.

[10] 韓建海. 工業機器人[M]. 4版. 武漢: 華中科技大學出版社, 2019: 1-5.

HAN Jian-hai. Industrial Robot[M]. 4th ed. Wuhan: Huazhong University of Science and Technology Press, 2019: 1-5.

[11] BOURBONNAIS F, BIGRAS P, BONEV I A. Minimum-Time Trajectory Planning and Control of a Pick- And-Place Five-Bar Parallel Robot[J]. IEEE/ASME Trans Mech, 2015, 20: 740-749.

[12] ZHANG Xiao-qing, MING Zheng-feng. Trajectory Planning and Optimization for a Par4 Parallel Robot Based on Energy Consumption[J]. Applied Sciences, 2019, 9: 1-19.

[13] GIL A, REINOSO O, MARIN J M, et al. Development and Deployment of a New Robotics Toolbox for Education[J]. Computer Applications in Engineering Education, 2015, 23(3): 443-454.

[14] LI Xian-hua, LYU Lei, SHENG Rui, et al. An Effective Construction Method of Modular Manipulator 3D Virtual Simulation Platform [J]. 3D Research, 2018, 9(24): 1-12.

[15] 李琳琳, 寇子明, 吳娟. 六自由度機械臂的設計與軌跡規劃研究[J]. 煤礦機械, 2019, 40(8): 43-46.

LI Lin-lin, KOU Zi-ming, WU Juan. Research on Design and Trajectory Planning of Six Degrees of Freedom Manipulator[J]. Coal Mine Machinery, 2019, 40(8): 43-46.

[16] 王文壇. 機械臂軌跡規劃及動力學研究[D]. 濟南: 山東大學, 2017: 31-34.

WANG Wen-tan. Research on Trajectory Planning and Dynamics of Manipulator[D]. Jinan: Shandong University, 2017: 31-34.

Kinematics Analysis and Trajectory Planning of Manipulator Based on MATLAB

MA Yan-yan, ZENG Tai-ying, JIANG Hai-lin

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

The work aims to study the kinematics and trajectory planning of packaging manipulator to achieve smooth operation and accurate control. With 6-DOF packaging manipulator model as research object, the coordinate system was established by the modified D-H parameter method, and the kinematics equation was deduced. Based on MATLAB, the manipulator model was built and direct and inverse kinematics simulation was carried out to verify the correctness of the model. Secondly, a quintic polynomial interpolation algorithm was used to plan the trajectory of joint space. In the end, the trajectory planning and simulation was carried out for the fixed-point grabbing and carrying tasks of the products to be packed in the packaging operation. The model of the manipulator was correct, and the trajectory optimization effect of the quintic polynomial interpolation method was good, which could ensure smooth and continuous angle, velocity and acceleration curves of each joint. The joint parameters in grabbing tasks were calculated and the trajectory curve of the end-effector was obtained. The manipulator can complete grabbing and carrying tasks with expected precision, which lays a foundation for further research on the motion control and practical application of the manipulator.

6-DOF manipulator; packaging machinery; trajectory planning; polynomial interpolation method

TB486；TP242.2

A

1001-3563(2023)03-0187-07

10.19554/j.cnki.1001-3563.2023.03.023

2022?03?13

馬艷艷（1977—），女，博士，副教授，主要研究方向為機械設計與機器人技術。

責任編輯：曾鈺嬋