基于Matlab的六自由度工業(yè)機(jī)器人運(yùn)動(dòng)學(xué)逆解分析及仿真

丁志鵬，江 明，游 瑋，梁兆東，馮海生，葛景國(guó)

（1.安徽工程大學(xué)電氣工程學(xué)院，安徽 蕪湖241000；2.埃夫特智能裝備股份有限公司，安徽 蕪湖241000）

引言

近年來，工業(yè)機(jī)器人發(fā)展迅猛，各行各業(yè)中對(duì)工業(yè)機(jī)器人的需求不僅從數(shù)量上大幅度提高，對(duì)其性能要求也越來越高，必須不斷提高對(duì)機(jī)器人的控制精度，才能滿足企業(yè)對(duì)于制造能力和效率的要求，所以有必要對(duì)工業(yè)機(jī)器人的運(yùn)動(dòng)學(xué)進(jìn)行研究。機(jī)械臂作為強(qiáng)耦合性、高復(fù)雜性的機(jī)械系統(tǒng)，只有準(zhǔn)確分析其末端執(zhí)行器在空間中的位置和姿態(tài)，才能讓其完成預(yù)想的軌跡運(yùn)動(dòng)［1］。

機(jī)器人運(yùn)動(dòng)過程中，受到生產(chǎn)現(xiàn)場(chǎng)實(shí)際條件約束，如操作空間中存在奇異點(diǎn)，影響其實(shí)際運(yùn)動(dòng)。傳統(tǒng)解析法求解逆運(yùn)動(dòng)學(xué)方法的計(jì)算量大，易受非線性影響，存在無解或者少解的情況，因此擬采用幾何法來求解逆運(yùn)動(dòng)方程，提高求逆解運(yùn)算速度，避免奇異點(diǎn)對(duì)逆運(yùn)動(dòng)方程的解造成不良影響。

目前，國(guó)內(nèi)許多學(xué)者已在尋求更好的運(yùn)動(dòng)學(xué)求逆解方法上做了不少研究。四川大學(xué)韓興國(guó)［2］提出一種基于動(dòng)態(tài)模糊神經(jīng)網(wǎng)絡(luò)進(jìn)行求逆解的方法，建立了運(yùn)動(dòng)學(xué)逆解預(yù)測(cè)模型；上海交通大學(xué)朱曉龍［3］根據(jù)其提出的末端姿態(tài)描述，得到一種對(duì)五自由度機(jī)械臂運(yùn)動(dòng)方程求逆的幾何方法。

Matlab環(huán)境下的機(jī)器人仿真功能十分強(qiáng)大，本文基于此建立仿真模型，模擬機(jī)器人在操作空間中的軌跡運(yùn)動(dòng)，能為研究機(jī)械臂運(yùn)動(dòng)過程中的平穩(wěn)性提供有力依據(jù)。

1 機(jī)器人結(jié)構(gòu)及坐標(biāo)系的建立

ER7B-C10機(jī)器人是一款埃夫特公司生產(chǎn)的可用于裝配和物料搬運(yùn)等的多功能六自由度工業(yè)機(jī)器人，其本體結(jié)構(gòu)如圖1所示。進(jìn)行機(jī)器人正運(yùn)動(dòng)學(xué)分析之前，首先分析其機(jī)械臂結(jié)構(gòu)，這對(duì)于關(guān)節(jié)鏈中如何在每一個(gè)連桿上建立合適的坐標(biāo)系十分重要。D-H方法由Denavit和Hartenberg于1955年提出，其參數(shù)使用連桿參數(shù)來描述機(jī)構(gòu)運(yùn)動(dòng)關(guān)系［4］。按照D-H規(guī)則來定義連桿坐標(biāo)系，對(duì)求解機(jī)器人運(yùn)動(dòng)學(xué)能帶來較大的便利。D-H方法具體建立過程為：

圖1 ER7B-C10機(jī)器人本體結(jié)構(gòu)

步驟1：從低序到高序來尋找定義關(guān)節(jié)軸，將軸的方向設(shè)為z0，…，zn－1。

步驟2：選擇坐標(biāo)系0，將原點(diǎn)定于軸z0上，并選擇x0和y0構(gòu)成右手系。

步驟3：將原點(diǎn)Oi定位于軸zi、軸zi－1和zi公垂線的交點(diǎn)。對(duì)于轉(zhuǎn)動(dòng)關(guān)節(jié)，如果軸zi和zi－1平行，則建立原點(diǎn)使得偏置為0；對(duì)于移動(dòng)關(guān)節(jié)，則將原點(diǎn)建立在關(guān)節(jié)延伸范圍的某一個(gè)參考點(diǎn)上。

步驟4：將xi軸建立在軸zi－1和zi公垂線上，其方向由關(guān)節(jié)i指向關(guān)節(jié)i＋1。

步驟5：根據(jù)右手定則，由xi軸和zi軸的方向確定yi的方向。

步驟6：坐標(biāo)系n的建立由關(guān)節(jié)n的類型決定，對(duì)于轉(zhuǎn)動(dòng)關(guān)節(jié)，則按照zn－1來設(shè)置zn；對(duì)于移動(dòng)關(guān)節(jié)，則任意選擇zn的方向。按照上述方法建立坐標(biāo)系如圖2所示。

圖2 關(guān)節(jié)坐標(biāo)系位置示意圖

臂結(jié)構(gòu)運(yùn)動(dòng)學(xué)分析中，D-H法巧妙地運(yùn)用4個(gè)基本參數(shù)αi、ai、di、θi來描述機(jī)械臂中的連桿，其中αi和ai描述了關(guān)節(jié)i和關(guān)節(jié)i－1之間的關(guān)于z軸的結(jié)構(gòu)關(guān)系，而di和θi則描述了兩個(gè)關(guān)節(jié)之間關(guān)于x軸的位置關(guān)系。圖中Li表示機(jī)器人各關(guān)節(jié)間的連桿偏移量參數(shù)，ER7BC10機(jī)器人的D-H參數(shù)見表1。

表1 機(jī)器人D-H參數(shù)

2 運(yùn)動(dòng)學(xué)方程的建立

2．1 正運(yùn)動(dòng)學(xué)方程

建立正運(yùn)動(dòng)學(xué)的目的是通過各個(gè)關(guān)節(jié)變量的值計(jì)算出機(jī)械臂末端執(zhí)行器在笛卡爾空間的位置和姿態(tài)，描述末端執(zhí)行器與基坐標(biāo)之間關(guān)系的齊次坐標(biāo)變換矩陣可由D-H法得到［5］。根據(jù)D-H法定義的規(guī)則，坐標(biāo)系i－1與坐標(biāo)系i之間的關(guān)系可描述為坐標(biāo)系i沿軸zi－1平移di，并繞軸zi－1旋轉(zhuǎn)θi，之后再沿軸xi平移ai，旋轉(zhuǎn)αi，得到如下齊次變換矩陣：

將表1中的參數(shù)αi、ai、di、θi帶入齊次變換矩陣（1），可得到機(jī)械臂中相鄰兩個(gè)關(guān)節(jié)坐標(biāo)系之間的變換關(guān)系：

末端執(zhí)行器與基坐標(biāo)之間的齊次變換矩陣T06是由機(jī)械臂中從低序到高序的所有相鄰坐標(biāo)系之間的變換關(guān)系A(chǔ)ii－1依次相乘而得：

T06即為末端執(zhí)行器的位置和姿態(tài)在基坐標(biāo)系下的表示，該機(jī)器人正運(yùn)動(dòng)學(xué)模型可以被劃分為兩個(gè)部分：（a）機(jī)器人手臂部分T03（qA）和機(jī)器人手腕（工具）部分

其中：

若機(jī)器人末端執(zhí)行器實(shí)際位置和姿態(tài)與式（4）中所計(jì)算出的結(jié)果有偏差，那么說明機(jī)器人運(yùn)動(dòng)過程并未絕對(duì)按照軌跡到達(dá)目的點(diǎn)，這可能是受到誤差影響，因此需要對(duì)機(jī)器人在投入使用前進(jìn)行標(biāo)定并運(yùn)用修正誤差模型來校正其在操作空間的位姿［7］。

2．2 逆運(yùn)動(dòng)學(xué)方程

與正運(yùn)動(dòng)學(xué)相反，逆運(yùn)動(dòng)學(xué)主要解決的是由機(jī)器人末端執(zhí)行器在操作空間中的位置和姿態(tài)來逆向推導(dǎo)出機(jī)械臂上各個(gè)關(guān)節(jié)變量的值［8］。逆運(yùn)功學(xué)問題較正運(yùn)動(dòng)學(xué)問題更為復(fù)雜，其原因主要有：（1）計(jì)算過程中較多出現(xiàn)非線性方程；（2）計(jì)算結(jié)果存在多種情況，多解或無窮多解；（3）機(jī)器人操作空間存在奇異點(diǎn)導(dǎo)致方程無解或漏解［9］。所以計(jì)算逆運(yùn)動(dòng)學(xué)需要選擇正確合適的方法，才能簡(jiǎn)化其復(fù)雜的計(jì)算過程。

總的來說，解決機(jī)器人逆運(yùn)動(dòng)學(xué)問題的方法主要有兩大類［10］：封閉解和數(shù)值解。前者可分為解析法和幾何法進(jìn)行求解，目前引用較為廣泛的方法為解析法，但是解析法的缺點(diǎn)是解析中代數(shù)較多、求逆復(fù)雜、計(jì)算量較大，因此本文采用幾何法對(duì)該逆運(yùn)動(dòng)方程進(jìn)行求解，計(jì)算過程中所涉及參數(shù)均在圖3中標(biāo)注。

圖3 機(jī)器人坐標(biāo)系逆解幾何圖

先對(duì)機(jī)器人手臂部分關(guān)節(jié)變量θ1、θ2、θ3進(jìn)行求解，再對(duì)機(jī)器人手腕部分變量θ4、θ5、θ6進(jìn)行求解，這既可以逐步減少變量、減少計(jì)算，又充分利用了前3個(gè)關(guān)節(jié)與位置相關(guān)而后3個(gè)關(guān)節(jié)與姿態(tài)有關(guān)的特性，減少了變量之間相互的影響［11］。cosθ1、cosθ2、cosθ3的值可在關(guān)節(jié)5于笛卡爾坐標(biāo)系中的位置關(guān)系中求得。

則關(guān)節(jié)變量θ1有：

根據(jù)圖中幾何關(guān)系，通過相似定理和余弦定理很容易得到：

則關(guān)節(jié)變量θ3：

同理可以計(jì)算出：

則關(guān)節(jié)變量θ2：

求出前3個(gè)關(guān)節(jié)變量后，將θ1、θ2、θ3作為已知量帶入到T03中，由T06右乘T03的逆矩陣后得到球形手腕齊次矩陣T36

T36中各元素為計(jì)算后常數(shù)值，僅與3個(gè)關(guān)節(jié)變量θ4、θ5、θ6有關(guān)，求法如下：

根據(jù)式（4）中關(guān)于手腕部分正運(yùn)動(dòng)方程的表達(dá)式可以求得T36內(nèi)包含θ4、θ5、θ6的關(guān)系式，帶入A34、A45、A56中進(jìn)行推導(dǎo)可求得：

3 基于Matlab的運(yùn)動(dòng)學(xué)仿真

3．1 機(jī)械臂模型的建立

Matlab是Mathworks公司開發(fā)的功能強(qiáng)大的工程計(jì)算應(yīng)用軟件，配備了強(qiáng)大的矩陣計(jì)算能力，其軟件中的Robotics Toolbox工具箱不僅可以實(shí)現(xiàn)對(duì)機(jī)器人運(yùn)動(dòng)學(xué)的可視化仿真，還可以對(duì)手工計(jì)算正逆解結(jié)果進(jìn)行驗(yàn)證［12］。

通過調(diào)用建立連桿函數(shù)指令Link將表1中D-H參數(shù)輸入到仿真機(jī)械臂模型中，再通過SerialLink函數(shù)建立模型，如圖4所示，模型與機(jī)器人實(shí)物大小比例為1∶100。ER7B-C10機(jī)器人連桿的實(shí)際參數(shù)見表2。

圖4 ER7B-C10機(jī)器人仿真模型

表2 ER7B-C10連桿參數(shù)（單位：mm）

3．2 正逆運(yùn)動(dòng)學(xué)仿真驗(yàn)證

調(diào)用Robotics Toolbox中的fkine函數(shù)，可以對(duì)上述建立的正運(yùn)動(dòng)學(xué)模型進(jìn)行正確性驗(yàn)證［13］。將操作空間中末端點(diǎn)位姿所對(duì)應(yīng)的關(guān)節(jié)變量值targ_ang帶入模型中，若所得結(jié)果與fkine函數(shù)所得解相一致，則說明正運(yùn)動(dòng)學(xué)模型建立正確，重復(fù)多次驗(yàn)證可保證結(jié)果準(zhǔn)確可靠。

調(diào)用Robotics Toolbox工具箱中ikine逆解函數(shù)指令來驗(yàn)證運(yùn)動(dòng)學(xué)逆解的方法存在一定的局限性，這是由于ikine函數(shù)運(yùn)用的是迭代法來求解逆運(yùn)動(dòng)學(xué)，迭代的收斂性將可能引起無解或者漏解的情況［14］，不能完成上述運(yùn)動(dòng)學(xué)逆解驗(yàn)證的任務(wù)。因此，擬采用代入驗(yàn)證的方式來驗(yàn)證幾何法計(jì)算運(yùn)動(dòng)學(xué)逆解是否正確，設(shè)計(jì)驗(yàn)算流程如圖5所示。將預(yù)設(shè)位姿的值代入運(yùn)動(dòng)學(xué)逆解計(jì)算，得到的關(guān)節(jié)變量代回上述驗(yàn)算正確的正運(yùn)動(dòng)學(xué)模型中，只要得到的正運(yùn)動(dòng)學(xué)模型的解與預(yù)設(shè)位置的誤差在合理的范圍內(nèi)，則說明上述幾何法計(jì)算運(yùn)動(dòng)學(xué)逆解公式的推導(dǎo)正確。

圖5 驗(yàn)證逆解流程設(shè)計(jì)

對(duì)運(yùn)動(dòng)學(xué)逆解需要充分考慮其存在性，即所取預(yù)設(shè)位姿的值必須在機(jī)械臂可達(dá)工作空間內(nèi)才能夠?qū)崿F(xiàn)對(duì)運(yùn)動(dòng)學(xué)逆解的驗(yàn)證。4組在可達(dá)工作空間內(nèi)的位置坐標(biāo)見表3。

表3 預(yù)設(shè)位置坐標(biāo)

將預(yù)設(shè)位置坐標(biāo)代入上述幾何法求解運(yùn)動(dòng)學(xué)逆解的公式中，所求得的各關(guān)節(jié)變量見表4。

表4 預(yù)設(shè)位置運(yùn)動(dòng)學(xué)逆解

在Matlab中使用驗(yàn)證正確的運(yùn)動(dòng)學(xué)正解模型來計(jì)算，由逆解計(jì)算出來的各個(gè)關(guān)節(jié)的關(guān)節(jié)變量帶入正解模型，得到4組關(guān)節(jié)變量對(duì)應(yīng)的位姿矩陣。

由位姿矩陣分析可得，所得位姿的第4列元素與給定位置Px、Py、Pz相比較，結(jié)果見表5。

從表5可知，預(yù)設(shè)位置與計(jì)算位置存在一定誤差，引起誤差的可能原因?yàn)椋河?jì)算過程中保留小數(shù)點(diǎn)位數(shù)，導(dǎo)致數(shù)值近似處理，最終計(jì)算結(jié)果存在偏差，見表6。

表5 預(yù)設(shè)位置與計(jì)算位置比較結(jié)果

表6 坐標(biāo)位置絕對(duì)誤差

從表6可知，4組坐標(biāo)絕對(duì)誤差不大且在允許的范圍內(nèi)，即可認(rèn)為：上述通過解析幾何法求解ER7B-C10機(jī)器人運(yùn)動(dòng)學(xué)逆運(yùn)動(dòng)方程可行。

3．3 機(jī)器人動(dòng)態(tài)仿真

軌跡規(guī)劃的目的是生成運(yùn)動(dòng)控制系統(tǒng)的參考輸入，以確保機(jī)器人完成軌跡的規(guī)劃。規(guī)劃是一組由期望軌跡的內(nèi)插函數(shù)所得到的時(shí)間序列值構(gòu)成的［15］。調(diào)用Robotics Toolbox工具箱中jtraj函數(shù)指令，對(duì)機(jī)器人關(guān)節(jié)空間進(jìn)行從初始點(diǎn)到目的點(diǎn)過程中的五次插值多項(xiàng)式進(jìn)行機(jī)器人運(yùn)動(dòng)軌跡規(guī)劃，主要研究了機(jī)械臂末端位姿與六個(gè)關(guān)節(jié)變量之間的關(guān)系［16］。選取機(jī)械臂末端初始位置A的各關(guān)節(jié)變量init_ang＝［0 0 0 0 0 0］，目標(biāo)點(diǎn)B的各關(guān)節(jié)變量tag_ang＝［－1.46088－0.27699 0.455 0.31 0 1.4451］，其中目標(biāo)點(diǎn)B為機(jī)械臂在操作空間中的非奇異點(diǎn)，且機(jī)械臂運(yùn)動(dòng)到目標(biāo)點(diǎn)B的過程中各關(guān)節(jié)變化較為直觀，便于觀測(cè)其運(yùn)動(dòng)的可行性。機(jī)械臂仿真模型3維動(dòng)態(tài)仿真和末端運(yùn)動(dòng)軌跡如圖6所示。

圖6 模型軌跡仿真結(jié)果

機(jī)械臂6個(gè)旋轉(zhuǎn)關(guān)節(jié)的位移變化曲線、速度變化曲線、加速度變化曲線如圖7所示。從圖7可知，機(jī)械臂末端從初始點(diǎn)A到目標(biāo)點(diǎn)B的運(yùn)動(dòng)過程中運(yùn)行軌跡連續(xù)且平穩(wěn)，6個(gè)關(guān)節(jié)的位移變化曲線平滑且持續(xù)穩(wěn)定，說明機(jī)械臂實(shí)現(xiàn)了由關(guān)節(jié)空間關(guān)節(jié)量變化到末端執(zhí)行器在笛卡爾空間進(jìn)行起始點(diǎn)到目標(biāo)點(diǎn)之間運(yùn)動(dòng)的轉(zhuǎn)化；速度、加速度的初值和末端值均為零，則說明機(jī)器人在兩個(gè)點(diǎn)之間可以平滑地起停，進(jìn)一步證明了ER7B-C10機(jī)器人在工作空間內(nèi)運(yùn)動(dòng)的可行性。

圖7 關(guān)節(jié)位移、速度、加速度曲線

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

針對(duì)ER7B-C10機(jī)器人進(jìn)行運(yùn)動(dòng)學(xué)分析，運(yùn)用D-H法合理建立坐標(biāo)系，推導(dǎo)出運(yùn)動(dòng)學(xué)正解，并針對(duì)解析法的計(jì)算量大、受奇異點(diǎn)影響大的缺點(diǎn)，選用另一種幾何方法求解逆運(yùn)動(dòng)方程，利用Matlab進(jìn)行機(jī)器人建模，設(shè)計(jì)運(yùn)動(dòng)學(xué)逆解驗(yàn)證方法。驗(yàn)證結(jié)果表明幾何方法求逆可行，并在仿真中輸出運(yùn)動(dòng)過程中各關(guān)節(jié)運(yùn)動(dòng)的位移、速度、加速度曲線。仿真結(jié)果表明了ER7B-C10機(jī)器人運(yùn)動(dòng)的可行性。本文研究為后續(xù)分析機(jī)器人動(dòng)力學(xué)、軌跡規(guī)劃提供了運(yùn)動(dòng)學(xué)參考。