高 涵

（天津大學(xué)仁愛學(xué)院，天津 301636）

0 引言

目前，衛(wèi)星裝配主要采用專用的工裝用于定位、支撐各個(gè)部件，然后通過人工輔助進(jìn)行對接裝配。然而，人工輔助工裝存在生產(chǎn)效率低、關(guān)鍵對接部位可視性不強(qiáng)等問題，亟需一批具有快速、高精度、高可靠性的自動(dòng)化裝備來滿足發(fā)展的需求。六自由度機(jī)械臂作為自動(dòng)化裝備的典型代表，由于具有可靠性高、靈活性強(qiáng)、冗余性好等特點(diǎn)，在焊接、搬運(yùn)、裝配及水下等領(lǐng)域得到了廣泛的應(yīng)用[1-5]。

六自由度機(jī)械臂運(yùn)動(dòng)學(xué)分析是末端位姿精確控制的基礎(chǔ)。與正運(yùn)動(dòng)學(xué)問題相比，逆運(yùn)動(dòng)學(xué)求解較為復(fù)雜，其復(fù)雜程度與機(jī)械臂構(gòu)型有關(guān)，且對于不同的機(jī)械臂構(gòu)型沒有通用的解法。六自由度機(jī)械臂逆運(yùn)動(dòng)學(xué)求解方法主要包括解析法、迭代法及幾何法，國內(nèi)外研究學(xué)者對此展開了研究。姜宏超等[6]采用解析法求解出六自由度模塊化機(jī)械臂全部逆解，雖然該方法可以求解出全部解，但求解較為復(fù)雜，計(jì)算量較大；Perez等[7]采用代數(shù)法求解七自由度機(jī)械臂逆運(yùn)動(dòng)學(xué)，但該方法不能保證得到有效的封閉解；李憲華等[8]根據(jù)六自由度機(jī)械臂特殊結(jié)構(gòu)，采用幾何方法和解析法結(jié)合得到機(jī)械臂逆運(yùn)動(dòng)學(xué)完整解析解，該方法主要依賴于機(jī)械臂的特殊構(gòu)型，通過幾何關(guān)系進(jìn)行求解，但需要與解析法結(jié)合使用，完成逆運(yùn)動(dòng)學(xué)求解工作；Nearchou[9]基于遺傳算法對逆運(yùn)動(dòng)學(xué)問題進(jìn)行了求解，但該算法的局限性在于逆運(yùn)動(dòng)學(xué)求解結(jié)果的收斂性依賴于初始點(diǎn)的選取。

針對以上問題，本文以課題組研制的六自由度機(jī)械臂為研究對象，提出一種基于牛頓迭代的逆運(yùn)動(dòng)學(xué)求解方法，并通過MATLAB GUI構(gòu)建仿真平臺(tái)，驗(yàn)證正運(yùn)動(dòng)學(xué)模型及逆運(yùn)動(dòng)學(xué)求解的可行性和準(zhǔn)確性。

1 面向衛(wèi)星裝配的六自由度機(jī)械臂正運(yùn)動(dòng)學(xué)分析

1.1 機(jī)械臂結(jié)構(gòu)建模

圖1 六自由度機(jī)械臂三維及結(jié)構(gòu)圖

本文以課題組研制的六自由度機(jī)械臂作為研究對象，其三維模型及結(jié)構(gòu)如圖1所示。該機(jī)械臂由基座、肩部、肘部、手腕1、手腕2及手腕3組成，其中關(guān)節(jié)1～關(guān)節(jié)3決定了機(jī)械臂的末端位置；關(guān)節(jié)4～關(guān)節(jié)6確定了機(jī)械臂的末端姿態(tài)。為了描述機(jī)械臂關(guān)節(jié)變量與末端位姿之間的函數(shù)關(guān)系，采用Denavit-Hartenberg（D-H）建模方法構(gòu)建機(jī)械臂連桿坐標(biāo)系，如圖2所示；根據(jù)圖2中的結(jié)構(gòu)建立D-H模型，如表1所示。

圖2 六自由度機(jī)械臂的連桿坐標(biāo)系

表1 六自由度機(jī)械臂連桿參數(shù)表

1.2 正運(yùn)動(dòng)學(xué)分析

相鄰兩連桿坐標(biāo)間的齊次變換矩陣，可由公式表示為：

根據(jù)表1和式（1）即可求得機(jī)械臂末端相對于基坐標(biāo)系的變換矩陣：

2 面向衛(wèi)星裝配的六自由度機(jī)械臂逆運(yùn)動(dòng)學(xué)分析

由于解析法求解機(jī)械臂逆運(yùn)動(dòng)學(xué)計(jì)算復(fù)雜，故本文采用數(shù)值迭代法對六自由度機(jī)械臂逆運(yùn)動(dòng)學(xué)問題進(jìn)行求解。設(shè)六自由度機(jī)械臂末端坐標(biāo)系相對于基坐標(biāo)系每一次迭代后得到的齊次變換矩陣為：

將式（2）和式（3）兩個(gè)齊次變換矩陣對應(yīng)元素相減組成非線性方程組，由公式表示為：

將式（4）齊次變換矩陣中的12個(gè)元素分別對關(guān)節(jié)角度θi(i=1,2,3,…,6)求偏導(dǎo)，得到此方程組的雅可比矩陣為：

由于關(guān)節(jié)空間速度與操作空間速度存在如下關(guān)系：

式中：q˙為關(guān)節(jié)空間的廣義速度；x˙為機(jī)械臂末端執(zhí)行器在操作空間的廣義速度；J為六自由度機(jī)械臂雅可比矩陣。

當(dāng)時(shí)間間隔足夠短時(shí)，式（6）可改寫為：

根據(jù)式（5）～（7）即可對六自由度機(jī)械臂逆運(yùn)動(dòng)學(xué)進(jìn)行求解，由于J(θt)為12×6階矩陣，且該矩陣為非方陣矩陣，只能對其求偽逆矩陣，每一次迭代求得的關(guān)節(jié)角度可由下式表示：

式中：θt、θt+1分別為第t次和第t+1次迭代求得的關(guān)節(jié)角度值。

基于數(shù)值迭代法求解六自由度機(jī)械臂逆運(yùn)動(dòng)學(xué)流程如圖3所示。首先根據(jù)初始關(guān)節(jié)角度值求解雅可比矩陣，得到下一次迭代的關(guān)節(jié)角度值；然后計(jì)算‖‖F(xiàn)(θ)2并判別‖‖F(xiàn)(θ)2是否不大于0.000 1，若滿足條件，則輸出期望位姿對應(yīng)的關(guān)節(jié)角度值，否則繼續(xù)迭代，直至滿足條件為止。

圖3 數(shù)值迭代法逆運(yùn)動(dòng)學(xué)求解流程圖

3 六自由度機(jī)械臂運(yùn)動(dòng)學(xué)仿真驗(yàn)證

3.1 正運(yùn)動(dòng)學(xué)仿真驗(yàn)證

在Solidworks軟件中構(gòu)建基坐標(biāo)系，并按照該坐標(biāo)系繪制六自由度機(jī)械臂三維模型，如圖4所示。此時(shí)機(jī)械臂各關(guān)節(jié)角度分別為0°、90°、180°、180°、180°、0°，選中末端零件的圓心，得到當(dāng)前末端相對于基坐標(biāo)系的坐標(biāo)為(-775,23,806)mm，采用MATLAB編寫六自由度機(jī)械臂正運(yùn)動(dòng)學(xué)程序，并將初始關(guān)節(jié)角度代入其中，得到末端相對于基坐標(biāo)系的變換矩陣為：。結(jié)果表明，求得的末端位置與繪圖軟件中得到的位置坐標(biāo)一致，故構(gòu)建的正運(yùn)動(dòng)學(xué)模型正確。

圖4 六自由度機(jī)械臂三維模型

圖5 逆運(yùn)動(dòng)學(xué)求解關(guān)節(jié)角度

3.2 逆運(yùn)動(dòng)學(xué)仿真驗(yàn)證

為驗(yàn)證數(shù)值迭代法求解機(jī)械臂逆運(yùn)動(dòng)學(xué)的有效性和可行性，采用MATLAB軟件編寫機(jī)械臂逆運(yùn)動(dòng)學(xué)程序，仿真中使機(jī)械臂末端沿直線運(yùn)動(dòng)。機(jī)械臂從（-245.821 9,210.948 5,866.207 9）mm運(yùn)動(dòng)到（30.6,430.3,1 000.9）mm，總運(yùn)動(dòng)時(shí)間為5 s，時(shí)間間隔為0.1 s，機(jī)械臂逆運(yùn)動(dòng)學(xué)求得的關(guān)節(jié)角度、對應(yīng)的末端位置及直線運(yùn)動(dòng)曲線如圖5～7所示。從圖中可以看出，數(shù)值迭代法求得的關(guān)節(jié)角度連續(xù)，且不存在突變，仿真求得的關(guān)節(jié)角度對應(yīng)的末端執(zhí)行器位置與期望運(yùn)動(dòng)的位置一致，故基于數(shù)值迭代法求解六自由度機(jī)械臂逆運(yùn)動(dòng)學(xué)的方法是可行有效的，且機(jī)械臂末端能準(zhǔn)確地跟蹤期望軌跡運(yùn)動(dòng)。

圖6 逆解對應(yīng)的機(jī)械臂末端位置

圖7 六自由度機(jī)械臂直線運(yùn)動(dòng)曲線

3.3 仿真界面

為便于觀測求解結(jié)果情況，采用MATLAB GUI搭建了仿真平臺(tái)，如圖8所示，該仿真平臺(tái)可實(shí)現(xiàn)機(jī)械臂的正逆運(yùn)動(dòng)學(xué)求解，同時(shí)也可對直線和圓弧運(yùn)動(dòng)軌跡進(jìn)行規(guī)劃。通過觸發(fā)課題研制的六自由度機(jī)械臂D-H參數(shù)、正逆運(yùn)動(dòng)學(xué)求解、逆運(yùn)動(dòng)學(xué)求解、直線運(yùn)動(dòng)及圓弧運(yùn)動(dòng)按鈕，可將機(jī)械臂D-H參數(shù)表、正逆運(yùn)動(dòng)學(xué)結(jié)果、機(jī)械臂運(yùn)動(dòng)曲線及關(guān)節(jié)角度曲線顯示于仿真界面中。

4 結(jié)束語

圖8 六自由度機(jī)械臂仿真平臺(tái)

以課題組研制的六自由度機(jī)械臂作為研究對象，針對解析法求解逆運(yùn)動(dòng)學(xué)計(jì)算量大、求解復(fù)雜的問題，提出了一種基于數(shù)值迭代的方法對六自由度機(jī)械臂逆運(yùn)動(dòng)學(xué)進(jìn)行求解，通過每次迭代，使迭代矩陣逐漸逼近于期望位姿矩陣，并基于MATLAB GUI建立了仿真平臺(tái)，仿真結(jié)果驗(yàn)證了本文所述方法的可行性和準(zhǔn)確性。該方法也適合于其他構(gòu)型的六自由度機(jī)械臂逆運(yùn)動(dòng)學(xué)求解，后續(xù)課題組將在此基礎(chǔ)上對衛(wèi)星裝配機(jī)器人軌跡規(guī)劃問題展開更為深入地研究，以實(shí)現(xiàn)衛(wèi)星裝配過程中的高效作業(yè)規(guī)劃。