羅恩澤

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

羅恩澤

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

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

串聯(lián)機器人；運動學(xué)逆解；MATLAB仿真；五次樣條插值

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

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

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

1 機械臂結(jié)構(gòu)與建模

1.1 機器人運動學(xué)正解

基于D-H法，針對六自由度工業(yè)機器人模型，在各個關(guān)節(jié)處建立坐標(biāo)系，如圖1所示。工業(yè)機器人的D-H參數(shù)如表1所示。

表1 六自由度工業(yè)機器人D-H參數(shù)表

圖1 六自由度工業(yè)機器人連桿坐標(biāo)系（單位：mm）

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

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

由矩陣連乘可以計算得：

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

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

根據(jù)圖1可得：

1.2 機器人運動學(xué)逆解

為了求得仿真軌跡，需要根據(jù)末端坐標(biāo)確定機械臂位姿。本文采用解析法求解六個關(guān)節(jié)參數(shù)。

則：

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

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

式中：為機械臂夾角，即關(guān)節(jié)2、3、4之間夾角，取正值。

另一方面，根據(jù)式（18）可求得：

由克萊姆法則解得：

因而式（22）唯一確定：

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

式中：為虛數(shù)單位。

約束下，考慮：

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

式（27）最右邊根據(jù)式（25）求出。

至此求出了所有關(guān)節(jié)運動量與末端坐標(biāo)的關(guān)系。

1.3 末端可行域的求解

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

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

表2 各關(guān)節(jié)運動范圍

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

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

2 軌跡生成

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

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

末端位移的參數(shù)方程為：

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

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

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

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

初末時刻邊界條件為：

式中：(k)為函數(shù)的階導(dǎo)數(shù)。

由此，僅有運動時間不確定，而這也正是優(yōu)化的目標(biāo)。根據(jù)約束：

注意到：

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

綜上：

3 實驗驗證

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

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

表3 各關(guān)節(jié)速度和加速度約束

圖3 末端位移曲線圖

4 結(jié)論

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

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

圖4 關(guān)節(jié)1、4運動量圖

[1]侯俊逵，熊瑞平，彭博，等. 一種氣動助力機械臂設(shè)計[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]焦建民，左洪福. 多約束條件下機械臂關(guān)節(jié)軌跡模糊控制算法[J]. 南京航空航天大學(xué)學(xué)報，2006，38（5）：623-627.

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

[7]劉寶，狄鑫，韓麗華. 改進(jìn)三次樣條插值在機械臂軌跡規(guī)劃中的應(yīng)用[J]. 機械科學(xué)與技術(shù)，2021，40（8）：1158-1163.

[8]田錫天，楊海成，F(xiàn)ichtner. 五次插值樣條在NC過程鏈中的應(yīng)用研究[J]. 機械科學(xué)與技術(shù)，2002（S1）：75-77.

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

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

[11]趙晶，祝錫晶，孟小玲，等. 改進(jìn)鯨魚優(yōu)化算法在機械臂時間最優(yōu)軌跡規(guī)劃的應(yīng)用[J/OL]. 機械科學(xué)與技術(shù)，2021-10-21.

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

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

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

[15]黃曉辰，張明路，張小俊，等. 機器人坐標(biāo)系建立的改進(jìn)DH方法[J]. 農(nóng)業(yè)機械學(xué)報，2014，45（10）：313-318，325.

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

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

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

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

[20]畢諸明，蔡鶴皋. 六自由度操作手的逆運動學(xué)問題[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。