葉上高，劉電霆

(桂林理工大學(xué)機(jī)械與控制工程學(xué)院，廣西桂林541004)

機(jī)器人運(yùn)動(dòng)學(xué)逆解，即根據(jù)工具坐標(biāo)系相對(duì)于基坐標(biāo)系的目標(biāo)位姿，求解機(jī)器人各關(guān)節(jié)角。逆運(yùn)動(dòng)學(xué)在機(jī)器人學(xué)中占有非常重要的地位，是機(jī)器人軌跡規(guī)劃和運(yùn)動(dòng)控制的基礎(chǔ)，直接影響著控制的快速性與準(zhǔn)確性［1］。一般機(jī)器人運(yùn)動(dòng)學(xué)逆解算法可分為以下幾種［2］:解析法(又稱反變換法)、幾何法和數(shù)值解法。由PAUL 等提出的反變換法求解過程直觀，因而被廣泛采納，但其求解過程中需多次進(jìn)行齊次變換矩陣的逆運(yùn)算和4 ×4 維矩陣的乘積運(yùn)算，導(dǎo)致求解過程復(fù)雜耗時(shí)［3］。

大多數(shù)工業(yè)機(jī)器人的幾何結(jié)構(gòu)都滿足Pieper 準(zhǔn)則［4］，即3 個(gè)相鄰關(guān)節(jié)軸交于一點(diǎn)或相互平行，其運(yùn)動(dòng)學(xué)逆解可以得到數(shù)量一定的若干組封閉解。對(duì)于最后3 個(gè)關(guān)節(jié)為旋轉(zhuǎn)關(guān)節(jié)而且軸線相交于一點(diǎn)的六自由度機(jī)器人，其前3 個(gè)關(guān)節(jié)決定末端執(zhí)行器的位置，后3 個(gè)關(guān)節(jié)決定末端執(zhí)行器的姿態(tài)［5］。故文中以其中的典型代表——ABB 公司的IRB2600 機(jī)器人為研究對(duì)象，提出了一種有別于傳統(tǒng)方法的位姿分離逆解算法，根據(jù)末端執(zhí)行器的位置矢量計(jì)算前3 個(gè)關(guān)節(jié)變量，利用末端執(zhí)行器的旋轉(zhuǎn)矩陣計(jì)算后3 個(gè)關(guān)節(jié)角。此算法逆解過程完全避免了矩陣求逆和4 ×4 維矩陣相乘的運(yùn)算，過程直觀，因此，比一般的解析算法更加簡單高效，便于實(shí)時(shí)控制。此外，文中對(duì)逆解所涉及的奇異和多解問題的處理也進(jìn)行了詳細(xì)分析。最后，用ADAMS 對(duì)該逆解算法的正確性進(jìn)行了仿真驗(yàn)證。

1 運(yùn)動(dòng)學(xué)正解

IRB2600 為6R 機(jī)構(gòu)，采用D-H 方法［6］，建立連桿坐標(biāo)系如圖1所示。

圖1 IRB2600 機(jī)器人的連桿坐標(biāo)系

IRB2600 機(jī)器人的D-H 參數(shù)如表1所示。

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

由此求得各連桿變換矩陣后連乘得機(jī)器人腕點(diǎn)的齊次變換矩陣

2 運(yùn)動(dòng)學(xué)逆解算法

2.1 前3 個(gè)關(guān)節(jié)角

求θ1。因a1+a3c23－d4s23+a2c2正負(fù)未定，故由式(2)、(3)求得θ1有兩個(gè)可能解

θ1A=arctan2(wy，wx)，θ1B=arctan2(－wy，－wx)

求θ2。若c1≠0，由式(2)、(4)整理得

將式(6)和(7)等號(hào)兩邊平方后取和，整理得

若c1=0，為避免分母為0，則改用式(3)、(4)用同樣的方法求解。

求θ3。若c1≠0，因θ1和θ2已求得，則由式(2)、(4)整理所得的式(6)、(7)兩式等號(hào)左邊已知，即式(6)和(7)構(gòu)成二元一次方程組，可解得未知數(shù)為

其中:l1=k1－a2c2，l3=k3+a2s2

故θ3=Atan2(s23，c23)－θ2。若c1=0，為避免分母為0，則改用式(3)、(4)用同樣方法求解。

2.2 后3 個(gè)關(guān)節(jié)角

因前3 個(gè)角和式(5)已求得，故上式等號(hào)左邊已知。

求θ5。由c5=r'23得

求θ4。由－c4s5=r'13及s4s5=r'33，得

求θ6。由c6s5= r'21及－ s5s6= r'22，用與θ4相同的方法求取。

3 奇異點(diǎn)

若式(1)中wx和wy都為0，則θ1的變化不影響腕點(diǎn)的位置，故為奇異點(diǎn)。若式(8)算出的θ2的兩個(gè)可能解相等，則連桿坐標(biāo)系原點(diǎn)連線O2O3與O3O4共線，故為奇異點(diǎn)。若式(9)解得θ5為零，則關(guān)節(jié)4 和6 的轉(zhuǎn)軸共線，故為奇異點(diǎn)［7］。

4 逆解中多解問題的處理步驟

逆解可能存在多解，而機(jī)器人的控制需要唯一的確切解。若不考慮避障要求，可按如下步驟獲取唯一最優(yōu)解。首先，若關(guān)節(jié)角的求解表達(dá)式的值域不能完全包含該關(guān)節(jié)運(yùn)動(dòng)范圍，則在必要情況下，需將求得的關(guān)節(jié)角可能解加減360°，補(bǔ)出關(guān)節(jié)運(yùn)動(dòng)范圍內(nèi)的其他可能解［8］;其次，剔除關(guān)節(jié)運(yùn)動(dòng)范圍以外的可能解;最后，由運(yùn)動(dòng)連續(xù)性［9］，選取與該關(guān)節(jié)角上一時(shí)刻的值距離最近的可能解為唯一最優(yōu)解。各關(guān)節(jié)角的求解順序?yàn)?先確定θ1的唯一最優(yōu)解，再利用θ1的最優(yōu)解按前述逆解方法和多解處理步驟求取θ2的最優(yōu)解。其余關(guān)節(jié)角的求取順序依此類推。最后所得的這組唯一的最優(yōu)關(guān)節(jié)角即可作為實(shí)現(xiàn)機(jī)器人目標(biāo)位姿的控制依據(jù)。

表2 IRB2600 機(jī)器人的關(guān)節(jié)活動(dòng)范圍

5 算法仿真驗(yàn)證

在ADAMS 中建立IRB2600 機(jī)器人模型進(jìn)行逆解算法的仿真驗(yàn)證。保持不變，使TCP 在t∈［0，40］沿螺旋線x =944 +400cost，y=－400sint，z=1 354－20t 運(yùn)動(dòng)，在t∈(40，50］從該螺旋線終點(diǎn)沿直線回到螺旋線起點(diǎn)。t =0 時(shí)刻關(guān)節(jié)角初始值為θ1=0，θ2=－0.988 6 rad，θ3=－1.178 rad，θ4=0，θ5=1.381 1 rad，θ6=0 rad。ADAMS 仿真過程中關(guān)節(jié)角的測(cè)量結(jié)果為圖2 中的實(shí)線，用文中方法逆解所得關(guān)節(jié)角變化曲線在圖2 中用叉號(hào)表示。由圖2 可見逆解結(jié)果與仿真結(jié)果完全重合，證明該算法正確有效。仿真過程中t =40 s 時(shí)刻的截圖見圖3。

圖2 逆解結(jié)果與仿真結(jié)果對(duì)照?qǐng)D

圖3 仿真過程中t=40 s 時(shí)刻的截圖

6 結(jié)論

仿真證明文中提出的逆解算法正確有效。逆解過程比一般的解析算法更加簡單高效，便于實(shí)時(shí)控制，對(duì)其他機(jī)器人的逆解也具有一定參考意義。

【1】劉華山，朱世強(qiáng)，吳劍波，等.基于奇異攝動(dòng)理論的輸入有界機(jī)器人軌跡跟蹤控制［J］.控制理論與應(yīng)用，2009，26(12):1371－1377.

【2】王其軍，杜建軍.MOTOMAN 機(jī)器人逆運(yùn)動(dòng)學(xué)新分析［J］.哈爾濱工業(yè)大學(xué)學(xué)報(bào)，2010，42(3):451－454.

【3】PAUL R P.Robot Manipulators:Mathematics，Programming，andControl［M］.Cambridge:MIT Press，1981.

【4】PAUL R P，SHINANO B E.Kinematical Control Equations for SimpleManipulators［J］.IEEE Trans SMC，1981，11(6):449－455.

【5】GOLDENBERG A A，BENHABIB B，F(xiàn)ENTON R G.A Complete Generalized Solution to the Inverse Kinematics of Robots［J］.IEEE Journal of Robotics and Automation，1985，1(1):14－20.

【6】JORGE Angeles.Fundamentals of Robotic Mechanical Systems［M］.New York:Spriger-Verlag，2003.

【7】楊武，蔣梁中.排爆機(jī)器人機(jī)械手運(yùn)動(dòng)規(guī)劃［J］.機(jī)械設(shè)計(jì)與制造，2008(5):184－186.

【8】劉華山，朱世強(qiáng)，吳劍波，等.基于向量內(nèi)積的機(jī)器人實(shí)時(shí)逆解算法［J］.農(nóng)業(yè)機(jī)械學(xué)報(bào)，2009，40(6):212－216，207.

【9】胡杰，張鐵.SCARA 機(jī)器人控制系統(tǒng)的設(shè)計(jì)與研究［J］.機(jī)械設(shè)計(jì)與制造，2012(5):162－163.