一種雙機(jī)械臂的避障運(yùn)動(dòng)軌跡規(guī)劃方法

付 青，王 直

（江蘇科技大學(xué) 電子與信息學(xué)院，江蘇 鎮(zhèn)江 212003）

一種雙機(jī)械臂的避障運(yùn)動(dòng)軌跡規(guī)劃方法

付 青，王 直

（江蘇科技大學(xué) 電子與信息學(xué)院，江蘇 鎮(zhèn)江 212003）

本文討論一種應(yīng)用于血凝檢測(cè)系統(tǒng)的兩自由度的雙機(jī)械臂的運(yùn)動(dòng)軌跡規(guī)劃的算法優(yōu)化問題。機(jī)械臂是該系統(tǒng)的重要組成部分，其工作狀態(tài)直接關(guān)系到整個(gè)系統(tǒng)運(yùn)行的穩(wěn)定和可靠。文中采用DH矩陣對(duì)雙機(jī)械臂的位置和姿態(tài)進(jìn)行數(shù)學(xué)建模，進(jìn)而通過基于笛卡爾空間的圓弧插補(bǔ)法進(jìn)行路徑規(guī)劃，得到更加平滑機(jī)械臂運(yùn)動(dòng)軌跡。最后，使用優(yōu)化的人工勢(shì)場(chǎng)法，使雙機(jī)械臂在無碰撞的情況下順利達(dá)到目標(biāo)點(diǎn)。MATLAB仿真實(shí)驗(yàn)證明改進(jìn)的算法能夠較好的彌補(bǔ)傳統(tǒng)人工勢(shì)場(chǎng)法的缺陷，提升機(jī)械臂運(yùn)動(dòng)軌跡規(guī)劃效果。

D-H矩陣；笛卡爾空間；圓弧插補(bǔ)法；人工勢(shì)場(chǎng)法

機(jī)械臂的運(yùn)動(dòng)軌跡是指通過給定的路徑的起點(diǎn)與終點(diǎn)[1]，以及機(jī)構(gòu)本身或者機(jī)構(gòu)運(yùn)行所存在的約束條件，求出每個(gè)關(guān)節(jié)的位移S，速度V，加速a的完整過程。

文中采用笛卡爾空間的圓弧插值法使得機(jī)械臂運(yùn)動(dòng)平滑。笛卡爾空間的規(guī)劃與機(jī)械臂目標(biāo)點(diǎn)的坐標(biāo)位姿函數(shù)相關(guān)，必須通過機(jī)械臂的逆向運(yùn)動(dòng)學(xué)求解，如此可獲取各個(gè)關(guān)節(jié)變量的函數(shù)。笛卡爾空間的軌跡規(guī)劃方法中的圓弧插值法適用于所研究主從機(jī)械臂運(yùn)動(dòng)狀態(tài)，主從機(jī)械臂最基本的要求是在起點(diǎn)與目標(biāo)點(diǎn)的速度為零，兩機(jī)械臂運(yùn)行平滑，時(shí)間和速度保持最優(yōu)的情況下，主從機(jī)械臂同步運(yùn)行且不發(fā)生碰撞的現(xiàn)象[2]。

1 雙機(jī)械臂的位姿描述

討論雙機(jī)械臂的運(yùn)動(dòng)狀態(tài)與操作，應(yīng)先建立對(duì)應(yīng)的數(shù)學(xué)模型，其應(yīng)包括空間中某個(gè)點(diǎn)的位置和姿態(tài)等等。需要多個(gè)坐標(biāo)系，以便于建立機(jī)械臂連桿之間的聯(lián)系。首先確定機(jī)械臂連桿與末端執(zhí)行器位姿，其次，將兩機(jī)械臂的基座固定，將其設(shè)為基座標(biāo)，根據(jù)機(jī)械臂的連桿和關(guān)節(jié)兩者的聯(lián)系，提出許多適用的運(yùn)動(dòng)學(xué)建模方法[3-4]。文中采用D-H矩陣參數(shù)法建模。

1.1 運(yùn)動(dòng)模型簡(jiǎn)化

選擇D-H矩陣來設(shè)置雙機(jī)械臂的坐標(biāo)系，Z軸的方向與連桿方向一致，X軸垂直兩連桿的公垂線，Y軸的方向由右手法則確定。

圖1 雙機(jī)械臂機(jī)構(gòu)及連桿圖

圖2 雙臂軌跡示意圖

為了便于區(qū)分與分析，將機(jī)械臂分為主機(jī)械臂和從機(jī)械臂，因兩機(jī)械臂對(duì)稱，只需要計(jì)算出其中一個(gè)機(jī)械臂的位姿，另一機(jī)械臂就可相應(yīng)得出。從上圖中可以列出相應(yīng)的表格。

1.2 機(jī)械臂的正運(yùn)動(dòng)學(xué)分析與逆運(yùn)動(dòng)學(xué)分

由圖1和表格1，主機(jī)械臂的各連桿的位姿矩陣可表示為：

表1 機(jī)構(gòu)關(guān)節(jié)角

在機(jī)械臂的運(yùn)行設(shè)計(jì)中，已知機(jī)械臂的初始位置，為了使得機(jī)械臂達(dá)到期望位姿，那么需要知道達(dá)到指定位姿各個(gè)關(guān)節(jié)需要運(yùn)動(dòng)的值，根據(jù)下列逆運(yùn)動(dòng)學(xué)方程[5-7]，計(jì)算關(guān)節(jié)角數(shù)值。

2 雙機(jī)械臂的協(xié)調(diào)工作

機(jī)械臂的軌跡規(guī)劃指機(jī)械臂的運(yùn)行過程中的路徑規(guī)劃，也就是在機(jī)械臂從起始點(diǎn)運(yùn)動(dòng)到目標(biāo)點(diǎn)，機(jī)械臂運(yùn)行過程中受到某些限制（位移，速度，加速度），簡(jiǎn)而言之，雙機(jī)械臂任務(wù)就是，兩個(gè)機(jī)械臂在共同運(yùn)行的過程中達(dá)到目標(biāo)點(diǎn)完成某項(xiàng)任務(wù)，關(guān)節(jié)空間法與笛卡爾空間為軌跡規(guī)劃的兩種方法，關(guān)節(jié)空間法計(jì)算簡(jiǎn)潔明了，僅僅能顯示機(jī)械臂的開端和結(jié)尾，不能直觀描述機(jī)械臂到達(dá)終端過程中運(yùn)動(dòng)軌跡。而笛卡爾坐標(biāo)系中的路徑規(guī)劃，就是在笛卡爾坐標(biāo)系中找出一條與已知路徑相似運(yùn)動(dòng)軌跡。

2.1 笛卡爾空間的路徑點(diǎn)規(guī)劃

雙機(jī)械臂共同工作的過程中，機(jī)械臂的工作任務(wù)是用機(jī)械臂的末端執(zhí)行器的笛卡爾空間坐標(biāo)節(jié)點(diǎn)定義，節(jié)點(diǎn)為終端執(zhí)行器位姿的齊次變換矩陣。用笛卡爾空間[8]的方法規(guī)劃兩機(jī)械臂運(yùn)動(dòng)，在笛卡爾坐標(biāo)系中運(yùn)算進(jìn)行大部分的計(jì)算及優(yōu)化。

笛卡爾空間的軌跡規(guī)劃中，為使機(jī)械臂按照理想方向運(yùn)行，每個(gè)路經(jīng)點(diǎn)的位姿必須可知。這里就需要算出機(jī)械臂在路徑各點(diǎn)的變換矩陣。笛卡爾空間的軌跡規(guī)劃以其擁有直觀明確的運(yùn)行軌跡的優(yōu)勢(shì)而得到廣泛應(yīng)用，笛卡爾空間是軌跡規(guī)劃的重要的方法，它的求法如下：

1）通過機(jī)械臂的各個(gè)關(guān)節(jié)角度和機(jī)械臂正向運(yùn)動(dòng)學(xué)計(jì)算公式求得[8-9]相應(yīng)的位姿，在上文中已經(jīng)解決。

2）對(duì)機(jī)械臂的運(yùn)動(dòng)軌跡進(jìn)行笛卡爾空間插補(bǔ)：直線插補(bǔ)法與圓弧插補(bǔ)法等等，形成笛卡爾空間路徑點(diǎn)。

2.2 血凝儀的軌跡要求

笛卡爾空間軌跡規(guī)劃方法有：直線插補(bǔ)法和圓弧插補(bǔ)法，直線插補(bǔ)法即已知點(diǎn)與點(diǎn)之間運(yùn)行軌跡為直線，圓弧插補(bǔ)法的運(yùn)行軌跡為圓弧。從圖2可以看出雙機(jī)械臂的路線為：

1）血凝儀兩機(jī)械臂的任務(wù)是提供四種試劑和N種血液樣本進(jìn)行混合。

2）主臂的任務(wù)是先從（A→O）吸取試劑，吸取試劑后，主臂在上升的過程中同時(shí)進(jìn)行旋轉(zhuǎn)（O→E），其運(yùn)行放如上圖。

3）主臂完成第一種試劑的進(jìn)樣任務(wù)之后，主臂按照反方向運(yùn)行，進(jìn)行進(jìn)樣針清洗，在按照第二個(gè)步驟的順序，進(jìn)行第二種試劑的取樣。

4）從臂的運(yùn)行過程與主臂的運(yùn)行過程相似，可根據(jù)主臂的方向得出。

2.3 圓弧插補(bǔ)法

圓弧插補(bǔ)法一般為兩種，在平面范圍內(nèi)的平面圓弧插補(bǔ)，另一種為三維空間范圍內(nèi)的空間圓弧插補(bǔ)，文中所用的為三維空間的圓弧插補(bǔ)。平面圓弧[10]指代圓弧平面與基座標(biāo)系的三大平面之一重疊，空間圓弧為三維空間任一平面內(nèi)的圓弧。

空間圓弧插補(bǔ)法可分為三步執(zhí)行：

1）將三維問題轉(zhuǎn)換成兩維，求出圓弧所在平面

2）利用二維平面插補(bǔ)計(jì)算插補(bǔ)點(diǎn)對(duì)應(yīng)坐標(biāo)（Xi+1，Yi+1）。

3）把該點(diǎn)的坐標(biāo)值轉(zhuǎn)變?yōu)榛A(chǔ)坐標(biāo)系下的值，

通過非一條直線的三點(diǎn)P1，P2，P3可確定一個(gè)圓及三點(diǎn)間的圓弧，它的圓心OR半徑R，基礎(chǔ)坐標(biāo)平面和圓弧所在平面的交線分別為AB、BC、CA。

建立圓弧平面插補(bǔ)坐標(biāo)系，使ORXRYRZR與圓心O重疊，令ORXRYRZR平面為圓弧所在平面，且保持ZR為外法線方向，因此，三維問題就轉(zhuǎn)換成平面問題[11-12]，用上述所講的平面圓弧插補(bǔ)的方法即可。在MATLAB中，編程實(shí)現(xiàn)機(jī)械臂笛卡爾圓弧插補(bǔ)軌跡規(guī)劃為，并實(shí)現(xiàn)動(dòng)畫仿真，結(jié)果如圖3、4所示。

圖3 使用笛卡爾軌跡規(guī)劃生成軌跡曲線圖

圖4 軌跡規(guī)劃直角坐標(biāo)系下的曲線圖

3 雙機(jī)械臂的協(xié)調(diào)工作

在兩機(jī)械臂進(jìn)行作業(yè)的路徑中，主機(jī)械臂的運(yùn)動(dòng)路徑可通過笛卡爾空間圓弧插補(bǔ)法得出，那么在兩機(jī)械臂同時(shí)運(yùn)行期間，相當(dāng)于實(shí)驗(yàn)的過程中主機(jī)械臂的位置狀態(tài)已經(jīng)人為確定，因此，從臂的運(yùn)行狀態(tài)是在主機(jī)械臂已知的情況下運(yùn)行的，主機(jī)械臂相當(dāng)于從機(jī)械臂的障礙物，這個(gè)障礙物的雖然是可移動(dòng)的，但是其運(yùn)動(dòng)路徑是已知的，因此可以使用人工勢(shì)場(chǎng)法規(guī)劃兩機(jī)械臂的路徑，人工勢(shì)場(chǎng)的簡(jiǎn)潔性和高效性試用于機(jī)械臂路徑規(guī)劃領(lǐng)域。

3.1 人工勢(shì)場(chǎng)法的優(yōu)化

在研究機(jī)械臂在人工勢(shì)場(chǎng)中運(yùn)行的過程中，如果當(dāng)機(jī)械臂逼近終點(diǎn)的同時(shí)也接近障礙物，而斥力比引力大的時(shí)候，那么機(jī)械臂就很難運(yùn)行到目標(biāo)位置[15-16]，因此為了機(jī)械臂能夠到達(dá)指定位置，必須重定義斥力函數(shù)，使其無論在何種狀況下都能達(dá)到終點(diǎn)。

3.2 新的斥力函數(shù)

因?yàn)闄C(jī)械臂不能達(dá)到指定位置的情況，假設(shè)當(dāng)機(jī)械臂向終點(diǎn)靠近時(shí)，斥力大小趨近于0，這樣可以解決達(dá)不到終點(diǎn)的問題。因此我們將機(jī)械臂與終點(diǎn)的相對(duì)位移也考慮在內(nèi)，新斥力場(chǎng)公式為

上式中的（X-Xgoal）為機(jī)械臂與目標(biāo)點(diǎn)之間的距離，q 代指物體當(dāng)前的位置，qg指終端位置，ρ（q）=||qqc||，qc指到障礙物最近的位姿點(diǎn)。Δρ（q）指 qc→q 的單位向量。機(jī)械臂運(yùn)動(dòng)方向受引力和斥力的合力Fsum控制，其合力使得機(jī)械臂逼近障礙物，其中，可以將斥力可看成是兩個(gè)分力Ferp1，F(xiàn)erp2的合力：

用傳統(tǒng)人工勢(shì)場(chǎng)法與優(yōu)化的人工勢(shì)場(chǎng)法公式，分別對(duì)機(jī)械臂的運(yùn)動(dòng)軌跡的進(jìn)行仿真如圖6、7所示。

圖5 機(jī)械臂在優(yōu)化人工勢(shì)場(chǎng)中的合力分析

圖6 傳統(tǒng)人工勢(shì)場(chǎng)下機(jī)械臂的軌跡圖

圖7 新人工勢(shì)場(chǎng)下機(jī)械臂的軌跡圖

4 結(jié)束語(yǔ)

雙機(jī)械臂的避障路徑規(guī)劃，其中兩機(jī)械臂都為兩自由度，具有平移和旋轉(zhuǎn)的兩種運(yùn)動(dòng)方式，兩機(jī)械臂在運(yùn)動(dòng)過程中，部分路徑可能發(fā)生碰撞，因此需要對(duì)路徑進(jìn)行詳細(xì)的規(guī)劃，防止發(fā)生碰撞。

1）首先應(yīng)用D-H矩陣建立雙機(jī)械臂的位姿的運(yùn)動(dòng)模型，應(yīng)用笛卡爾空間的圓弧插補(bǔ)法，使得機(jī)械臂在笛卡爾空間中的運(yùn)動(dòng)軌跡平滑。

2）通過對(duì)主機(jī)械臂的路徑規(guī)劃，可以預(yù)知主機(jī)械臂的路徑，將其作為從機(jī)械臂運(yùn)動(dòng)的已知障礙。

3）人工勢(shì)場(chǎng)法是避障路徑規(guī)劃的方法之一，但在有些情況中存在目標(biāo)達(dá)不到的情況，這里對(duì)傳統(tǒng)的人工勢(shì)場(chǎng)法進(jìn)行深入分析，優(yōu)化，使其滿足雙機(jī)械臂避障的路徑規(guī)劃。

[1]閆華文.一種醫(yī)用光電檢測(cè)前置放大電路設(shè)計(jì)[J].電子產(chǎn)品世界，2014（2）:34-36.

[2]姜銘，易紅.李鷺楊.RGRR-I構(gòu)造混聯(lián)6R機(jī)器人[J].2010，46（2）:1-36.

[3]李憲，郭永存，張軍華.模塊化六自由度機(jī)械臂逆運(yùn)動(dòng)學(xué)解算與驗(yàn)證[J].計(jì)算機(jī)仿真，2012，29（3）:239-243.

[4]劉極峰.丁繼斌.機(jī)器人技術(shù)基礎(chǔ)[M].2版.北京：高等教育出版社，2012.

[5]黃祥.基于最小區(qū)域發(fā)的圓柱度幾何誤差評(píng)定[J].組合機(jī)床與自動(dòng)化加工技，2013（6）:27-29.

[6]田桂，謝建，陳永寧.大型導(dǎo)彈起豎過程最優(yōu)時(shí)間軌跡規(guī)劃研究[J].計(jì)算機(jī)仿真，2012，29（1）:41-44.

[7]王會(huì)方，朱世強(qiáng)，吳文祥.基于INSGA-算法的機(jī)械手多目標(biāo)軌跡規(guī)劃[J].浙江大學(xué)學(xué)報(bào)（工學(xué)版），2012，46（4）:622-628.

[8]李小霞，汪木蘭，劉坤.基于B樣條的機(jī)械手關(guān)節(jié)空間平滑軌跡規(guī)劃[J].組合加床與自動(dòng)化加工技術(shù)，2013（12）:18-20.

[9]王東霞，溫秀蘭，汪鳳林.基于微分進(jìn)化算法的平面誤差評(píng)定[J].計(jì)算機(jī)真，2012，29（1）:41-44.

[10]Zhang Xiaochao，Jiang Xiangqian，Scott P J.A reliable method of minimum zone evaluation of cylindricity and conicity from coordinate measurement data[J].Precision Engineering，2011，35（3）:484-489.

[11]Wisnu Aribowo，Kazuhihko Terashima.Cubic Spine Trajectory Planning and Vibration Suppression of Semiconductor Wafer Transfer Robot Arm [J].Automation Technology，2014，8（2）:265-274.

[12]王立剛，建天成.基于光電二極管檢測(cè)電路的噪聲分析與電路設(shè)計(jì)[J].大慶石油學(xué)院學(xué)報(bào)，2009，33（2）：88-92.

[13]付文羽.硅光電二極管的線性度及信噪比分析[J].半導(dǎo)體光電，2003，24（4）：267-269.

[14]陳強(qiáng)，黃聲享.小波去噪效果評(píng)價(jià)的另一指標(biāo)[J].測(cè)繪信息與工程，2008，33（5）:13.

[15]MATLAB技術(shù)聯(lián)盟，孔玲軍.MATLAB小波分析超級(jí)學(xué)習(xí)手冊(cè)[M].北京：人民郵電出版社，2014:297-308.

An obstacle avoidance trajectory planning for double manipulators

FU Qing，WANG Zhi
（School of Electrical and Information，Jiangsu University of Science and Technology，Zhenjiang 212003，China）

This article discusses an improved algorithm of trajectory planning for double 2-Dof-Freedom manipulators which apply to the cruor measurement system.With people pay more attention to the cardiovascular disease and healthy，the cruor measurement system becomes one of the hot spots in the research of automatic medical detection.As well know，manipulator is a vital component of the automatic system.The working state of the manipulator is related to the stability and reliability of the system directly.This paper provides a mathematical model for position and orientation of the double manipulators system by using D-H Transforming Matrix，and a path planning base on the method of Circular Interpolation in Cartesian Space，which makes the trajectory of the Manipulator more smooth.The traditional algorithm of Artificial Potential Field has a defect that can't reach the target point near the obstacle.In order to solve this problem，a new repulsive function was created which taking the relative distance between the double manipulators and the target into consideration to improve the traditional Artificial Potential Field，and making the manipulators reach the target points without collision.Through simulation of MATLAB and experiment，it was proved that the improved algorithm can make up for the deficiency of traditional Artificial Potential Field，and can improve the efficiency of trajectory planning and movement of the double 2-Dof-Freedom manipulators.

D-H transforming matrix； cartesianspace； method of circular interpolation； artificial potential field

TN02

A

1674－6236（2017）16-0068-05

2016-06-22稿件編號(hào)：201606162

付 青（1990—），女，江蘇連云港人，碩士研究生。研究方向：工業(yè)控制。