馬艷艷，曾臺英，江海林

基于MATLAB的機械臂運動學(xué)分析和軌跡規(guī)劃

馬艷艷，曾臺英，江海林

（上海理工大學(xué)，上海 200093）

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

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

在包裝行業(yè)中，包裝機械占據(jù)著十分重要的位置。在某種程度上，包裝機械水平直接決定了整個包裝行業(yè)的發(fā)展水平[1-2]。傳統(tǒng)的手工包裝作業(yè)由于勞動強度大、生產(chǎn)效率低、質(zhì)量不穩(wěn)定等缺點已經(jīng)無法滿足人們?nèi)找嬖鲩L的生產(chǎn)生活的需要，因此，機械臂以其智能、高效、安全等優(yōu)點在包裝作業(yè)中得到了越來越廣泛的應(yīng)用。由于抓取搬運等工序?qū)Πb流水線的高效工作非常重要，因此對機械臂的要求也越來越高[3-5]。

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

1 包裝機械臂運動學(xué)分析

1.1 機械臂D–H模型

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

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

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

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

將表1中各關(guān)節(jié)參數(shù)代入式（1）可得各個關(guān)節(jié)的變換矩陣，然后將這些變換矩陣相乘，則可得基坐標(biāo)系和末端執(zhí)行器間的變換矩陣為：

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

1.2 MATLAB仿真模型及正逆運動學(xué)分析

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

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

圖2 機械臂仿真模型

對模型進行逆運動學(xué)分析，用逆運動學(xué)求解指令ikine來求解各個關(guān)節(jié)的角度，起始位置的關(guān)節(jié)角度a=robot.ikine（a），終止位置的關(guān)節(jié)角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]。結(jié)果表明關(guān)節(jié)角a與起始點位置相一致，關(guān)節(jié)角b與起始點位置不一致，再對關(guān)節(jié)角b進行正運動學(xué)分析可得位姿矩陣c，且c=b。結(jié)果表明，對逆運動學(xué)計算求出的關(guān)節(jié)角再進行正運動學(xué)求解得出的位姿矩陣c與正運動學(xué)求解得出的位姿矩陣b一致，從而驗證了機械臂的參數(shù)設(shè)定和仿真模型是正確的。

2 機械臂軌跡規(guī)劃

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

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

2.1 多項式插值法

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

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

若初始關(guān)節(jié)和終止關(guān)節(jié)的角度、角速度和角加速度的6個約束條件為：

對式（5）求一階導(dǎo)數(shù)可得角速度函數(shù)為：

對式（5）求二階導(dǎo)數(shù)可得角加速度函數(shù)為：

聯(lián)立式（6）—（8）的方程組求解可得五次多項式系數(shù)為：

2.2 軌跡規(guī)劃仿真結(jié)果與分析

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

圖3 末端執(zhí)行器位移圖

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

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

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

圖4 各關(guān)節(jié)位置曲線

圖5 各關(guān)節(jié)角速度曲線

圖6 各關(guān)節(jié)角加速度曲線

3 機械臂軌跡運動試驗

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

表2 抓取點位置坐標(biāo)

Tab.2 Position coordinate of grabbing point

表3 關(guān)節(jié)路徑點

Tab.3 Joint path point

圖7 末端執(zhí)行器運動軌跡曲線

4 結(jié)語

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

[1] 溫景陽, 杜宇凡, 秦濤, 等. 基于Dobot機械臂的包裝自動線運動規(guī)劃與仿真[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] 李憲華, 范凱杰, 疏楊, 等. 六自由度模塊化機械臂的軌跡規(guī)劃與仿真[J]. 制造技術(shù)與機床, 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并聯(lián)機械手軌跡規(guī)劃方法[J]. 機械工程學(xué)報, 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] 鄭濤, 劉滿祿. 一種機械臂分段插值軌跡規(guī)劃方法[J]. 機械設(shè)計與制造, 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法的包裝分揀機械手運動學(xué)分析和軌跡規(guī)劃[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] 孫紹杰, 齊曉慧, 蘇立軍, 等. 基于多項式插值的五自由度機械臂軌跡規(guī)劃方法研究[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] 韓建海. 工業(yè)機器人[M]. 4版. 武漢: 華中科技大學(xué)出版社, 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] 李琳琳, 寇子明, 吳娟. 六自由度機械臂的設(shè)計與軌跡規(guī)劃研究[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] 王文壇. 機械臂軌跡規(guī)劃及動力學(xué)研究[D]. 濟南: 山東大學(xué), 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—），女，博士，副教授，主要研究方向為機械設(shè)計與機器人技術(shù)。

責(zé)任編輯：曾鈺嬋