六軸機器臂搬運過程軌跡規劃與仿真研究

羅恩澤

六軸機器臂搬運過程軌跡規劃與仿真研究

羅恩澤

（西南交通大學 機械工程學院，四川 成都 610031）

針對機械臂搬運過程中的模擬仿真需求，提出基于搬運機械臂姿態的解析方法和末端定位方案，并基于逆運動學分析，給出一套軌跡生成方案。首先運用D-H法分析機械臂結構參數確定軸間坐標關系，建立運動學模型，通過解析法確定末端位置和機械臂位姿的正、逆運動學關系，并求出末端可行域以及多解的確定方案。基于逆運動學分析，運用五次多項式插值來避免沖擊。根據機械臂末端起止點生成平滑的末端運動軌跡以及位姿關于時間的參數方程。提出搬運時間最短原則的軌跡規劃方案。在MATLAB仿真下，結果表明該方案設計的軌跡連續平滑，關節運動滿足速度與加速度約束，并無跳變和沖擊，說明了方案的合理性。該方案為搬運機械手運動規劃方案提供借鑒。

串聯機器人；運動學逆解；MATLAB仿真；五次樣條插值

機械手是一種自動化操作系統，用于模擬人類的手和手臂來攜帶、抓取和繪制物體，可以代替人實現機械繁復的艱苦工作，并且可以工作在較為惡劣的環境下，保證人身安全。工業機器臂[1]在諸多領域正在逐步代替人類的工作。機械臂在實際運用過程中可以采用示跡法提供搬用路線。為了在仿真中規劃路線以減少試錯成本，需要提前設計和規劃搬用路線。

目前針對具體搬運問題還沒有固定方法。一個搬運軌跡方案設計需要兩步：①建立關節運動量與位姿關系的運動學模型。②根據需求規劃搬運路線，基于逆運動學分析求解位姿。逆運動學求解方法不唯一。目前有PIEPER[2]、幾何法[3]等方法。其中Paul等[4]提出的反變換法對后來的研究起到指導性作用。實際機械臂除了考慮末端軌跡的連續性外，還需要考慮速度和加速度的連續性，防止沖擊對機械臂的損害、增大疲勞。軌跡規劃需要解決：①機械臂初始軌跡的插值擬合。②最優控制，即在一定約束和性能[5-6]要求下，獲得最優軌跡。一般為了提高產量，要求時間最短，其次是能量沖擊最小等。目前主要有改進三次B樣條[7]、五次多項式擬合[8]法以及NURBS曲線[9]。優化方法則有灰狼算法[10]、改進鯨魚優化算法[11]、改進螢火蟲算法[12]，以及為解決最大速度問題提出的差分法[13]。

本文對六軸機械臂用D-H（Denavit- Hartenberg）法[14-20]建立模型。對于已知起始點和終止點，在保持搬運對象不晃動、末端不旋轉的情況下，解析地求解關節運動量。結合該方案，進一步根據運動初位置和末位置，采用五次樣條插值，在速度和加速度約束下生成軌跡，求解搬運任務中的機械臂位姿變化。最后運用MATLAB直觀準確地反映運動狀態與軌跡。該方案不僅滿足了搬運的軌跡平滑性要求，運動平穩，關節運動沒有沖擊，同時考慮了速度約束和加速度約束，有一定的借鑒意義。

1 機械臂結構與建模

1.1 機器人運動學正解

基于D-H法，針對六自由度工業機器人模型，在各個關節處建立坐標系，如圖1所示。工業機器人的D-H參數如表1所示。

表1 六自由度工業機器人D-H參數表

圖1 六自由度工業機器人連桿坐標系（單位：mm）

D-H法中每次從坐標系{}到{－1}由兩次平移和兩次旋轉得到，其矩陣連乘后得到：

考慮到平移矩陣為初等矩陣，而初等矩陣具有乘法交換律，故式（1）可改寫為：

由矩陣連乘可以計算得：

在定義連桿坐標系和相對的連桿參數后，可以建立機器人的運動學方程。根據連桿參數得到連桿的變換矩陣，再將連桿變換矩陣相乘就可計算出：

將各個連桿矩陣連乘可得：

根據圖1可得：

1.2 機器人運動學逆解

為了求得仿真軌跡，需要根據末端坐標確定機械臂位姿。本文采用解析法求解六個關節參數。

則：

這個過程是可逆的，因而也是等價的。進行代換如下：

將式（16）、式（17）代入式（11），得：

式中：為機械臂夾角，即關節2、3、4之間夾角，取正值。

另一方面，根據式（18）可求得：

由克萊姆法則解得：

因而式（22）唯一確定：

由式（18）前兩行結合求得唯一解：

式中：為虛數單位。

約束下，考慮：

由于變換矩陣滿足交換律，變換矩陣的連乘積可以分離成旋轉矩陣與位移矩陣的乘積：

式（27）最右邊根據式（25）求出。

至此求出了所有關節運動量與末端坐標的關系。

1.3 末端可行域的求解

為了解決軌跡生成問題，首先要說明機械臂的可行域，即末端可以到達的區域。取遍6個角度參數取值范圍，根據運動學正解，得到末端可以到達位置圖像。

各角度取值范圍如表2所示。

表2 各關節運動范圍

在坐標系原點1、2、3所在平面截取截面，用MATLAB繪制可行域，如圖4所示。

圖2 末端可行域（水平面投影）

2 軌跡生成

解決了末端坐標與位姿關系，可以進行軌跡生成以及關節運動量的生成。為了確保機械臂在搬運過程中的平穩，啟動和停止以及運動時，關節處無沖擊，采用五次多項式插值來生成軌跡。

運用上文解析法分別求運動逆解，得到邊界條件為：

末端位移的參數方程為：

由于搬運過程中，要求機械臂開始運動和終止運動平穩，初始和終止時刻的邊界狀態都要使速度和加速度為0，得到初時刻和末時刻邊界條件為：

式中：為速度，m/s；為加速度，m/s2。

可以驗證使邊界處機械臂開始運動和終止運動時運動平穩的充分條件為：

考慮到機械臂關節運行不能有過高的速度和加速度，因而添加約束條件：

初末時刻邊界條件為：

式中：(k)為函數的階導數。

由此，僅有運動時間不確定，而這也正是優化的目標。根據約束：

注意到：

將式（40）代入式（39），得：

綜上：

3 實驗驗證

已知機械臂基座位于(0, 0, 0)，需要從機床P1(728, 20, 938)，搬運到貨車P2(387, -459, 1152)。由于機械零件特性和驅動約束，機械臂各關節速度和加速度約束如表3所示。

基于邊界條件，運用五次樣條插值和式（38），得到關節運動量隨時間的變化函數。末端搬運軌跡如圖3所示，部分關節運動量如圖4所示。

表3 各關節速度和加速度約束

圖3 末端位移曲線圖

4 結論

本文針對六軸機械臂提出一種搬運過程的軌跡規劃方案。

首先根據運動機理建立運動學仿真模型、末端坐標與位姿的正運動學關系，然后基于正運動學方程解析地得到逆運動學求解方法，并指出解的關系和定解條件，使逆解唯一。分析搬運要求，得到邊界條件，采用五次多項式插值的方法，依據時間最短原則確定參數，最終確定軌跡方程。最后用MATLAB仿真，根據邊界條件及加速度和速度約束，確定關節運動函數，運用正運動學關系得到末端運動軌跡。得到的軌跡曲線平滑，各關節運動變化無跳變和沖擊，可以完成搬運要求。

圖4 關節1、4運動量圖

[1]侯俊逵，熊瑞平，彭博，等. 一種氣動助力機械臂設計[J]. 機械，2021，48（2）：42-48.

[2]LEE C S G，ZIEGLEＲ M． A geometric approach in solving the inverse kinematics of PUMA robots[J]． IEEE Trans，Aerospace and Electronic Systems，1984，20（6）：695-706.

[3]PIEPEＲ D. ＲOTH B. The kinematics of manipulators under computer control[C]. Proc. of the ZND International Congress on Theory of Machines and Mechanisms，1969：159-169.

[4]PAUL R P，SHIMANO B E，MAYEＲ G． Kinematic control equations for simple manipulators[J]. IEEE Trans SMC，1981，11（6）：449-455.

[5]焦建民，左洪福. 多約束條件下機械臂關節軌跡模糊控制算法[J]. 南京航空航天大學學報，2006，38（5）：623-627.

[6]楊松山，徐增丙，王志剛. 冗余串聯機械臂軌跡規劃與控制[J]. 機械制造，2021，59（4）：24-2842.

[7]劉寶，狄鑫，韓麗華. 改進三次樣條插值在機械臂軌跡規劃中的應用[J]. 機械科學與技術，2021，40（8）：1158-1163.

[8]田錫天，楊海成，Fichtner. 五次插值樣條在NC過程鏈中的應用研究[J]. 機械科學與技術，2002（S1）：75-77.

[9]王昕，王均偉，饒志，等. 基于NURBS曲線軌跡規劃與速度規劃的研究[J]. 系統仿真學報，2008，20（15）：3973-3980.

[10]范子彥，李立君，李宇航，等. 基于改進灰狼算法的油茶果采摘機械臂軌跡規劃[J]. 機械設計與研究，2022，38（1）：195-201.

[11]趙晶，祝錫晶，孟小玲，等. 改進鯨魚優化算法在機械臂時間最優軌跡規劃的應用[J/OL]. 機械科學與技術，2021-10-21.

[12]郭鑫鑫，薄瑞峰，賈竣臣，等. 基于改進螢火蟲算法的機械臂時間最優軌跡規劃[J]. 機械設計與研究，2021，37（3）：55-59.

[13]王學琨，李剛，周東凱，等. 基于DE的時間最優6-DOF機械臂軌跡規劃算法[J]. 計算機仿真，2015，32（8）：332-337.

[14]陽方平，李洪誼，王越超，等. 一種求解冗余機械臂逆運動學的優化方法[J]. 機器人，2012，34（1）：17-21，31.

[15]黃曉辰，張明路，張小俊，等. 機器人坐標系建立的改進DH方法[J]. 農業機械學報，2014，45（10）：313-318，325.

[16]馬叢俊，趙濤，向國菲，等. 基于逆運動學的柔性機械臂末端定位控制[J]. 機械工程學報，2021，57（13）：163-171.

[17]馬國慶，劉麗，于正林，等. 基于MATLAB的HP20D機器人運動學分析與仿真[J]. 制造業自動化，2019，41（8）：104-107.

[18]張新敏，朱學軍，趙晨晨，等. 基于MATLAB的HP20機器人運動學分析與仿真[J]. 制造業自動化，2014，36（13）：12-15.

[19]張普行，嚴軍輝，賈秋玲. 六自由度機械手的運動學分析[J]. 制造業自動化，2011，33（20）：68-71.

[20]畢諸明，蔡鶴皋. 六自由度操作手的逆運動學問題[J]. 機器人，1994，16（2）：92-97.

Trajectory Planning and Simulation of Six-Axis Manipulator Handling Process

LUO Enze

( School of Mechanical Engineering, Southwest Jiaotong University, Chengdu 610031, China )

Aiming at the simulation requirements of six-axis manipulator in the process of handling, the analytical method and the end positioning scheme based on the attitude of manipulator are proposed, and a set of trajectory generation scheme is proposed based on the inverse kinematics analysis. Firstly, the D-H method is used to analyze the structural parameters of the manipulator to determine the coordinate relationship between the axes, and the kinematics model is established. The forward and inverse kinematics relationship of the end position and the manipulator pose was determined by the analytical method, and the feasible domain of the end and the determination scheme of multiple solutions are obtained. On the basis of the inverse kinematics analysis, the quintic polynomial interpolation is used to avoid impact. According to the starting and ending points of the end of the manipulator, the smooth end trajectory and the time parameter equation of the pose are generated. A path planning scheme based on the principle of the shortest handling time is proposed. The result of the MATLAB simulation shows that the trajectory of the design scheme is continuously smooth and the joint motion meets the constraints of velocity and acceleration without jump and impact, which shows the rationality of the scheme. The scheme provides reference for the motion planning scheme of manipulators.

tandem robot；inverse kinematics；MATLAB simulation；quintic spline interpolation

TP242

A

10.3969/j.issn.1006-0316.2023.01.012

1006-0316 (2023) 01-0074-07

2022-04-18

羅恩澤（2000－），男，江蘇南京人，主要研究方向為機械制造及其自動化，E-mail：1607944002@qq.com。