冗余機(jī)械臂運(yùn)動(dòng)學(xué)建模與軌跡規(guī)劃分析

季曉明

（江蘇安全技術(shù)職業(yè)學(xué)院智能制造與應(yīng)急裝備學(xué)院，江蘇 徐州，221011）

引言

與六軸機(jī)械臂相比，冗余機(jī)械臂具有多余的操作自由度，可以用來實(shí)現(xiàn)機(jī)械臂本體的避障、避關(guān)節(jié)限位、避奇異、優(yōu)化關(guān)節(jié)力矩等任務(wù)，是當(dāng)前工業(yè)中最常見的一種智能化裝備。[1]冗余機(jī)械臂運(yùn)動(dòng)規(guī)劃問題一直是學(xué)術(shù)界的研究熱點(diǎn)，它需要通過優(yōu)化算法或策略來選擇合適的機(jī)械臂關(guān)節(jié)運(yùn)動(dòng)軌跡，并綜合考慮性能指標(biāo)、工作空間和避免碰撞等條件，從而最大程度地發(fā)揮機(jī)械臂的靈活性和效率。然而，冗余機(jī)械臂的運(yùn)動(dòng)學(xué)模型較為復(fù)雜，尤其是較難獲得逆向運(yùn)動(dòng)學(xué)的解析解法。因此，如何提高冗余機(jī)械臂的運(yùn)動(dòng)學(xué)建模效率和完成其軌跡規(guī)劃是本文研究的重點(diǎn)。

通常，求解冗余機(jī)械臂逆向運(yùn)動(dòng)學(xué)最多的方法是數(shù)值解法。目前，很多學(xué)者傾向于利用人工智能優(yōu)化算法來解決冗余機(jī)械臂逆向運(yùn)動(dòng)學(xué)求解問題，像遺傳算法[2]、粒子群算法[3]、螢火蟲算法[4]等。然而，這些求解算法具有很強(qiáng)的非線性，實(shí)時(shí)性較差，不利于機(jī)械臂的實(shí)際控制。文獻(xiàn)[5]只考慮機(jī)械臂的主任務(wù)和部分約束任務(wù)，提出了一種變權(quán)重逆向運(yùn)動(dòng)學(xué)求解算法，獲得較高的計(jì)算效率。文獻(xiàn)[6]采用一種零空間飽和算法來解決冗余機(jī)械臂逆向運(yùn)動(dòng)學(xué)求解問題。文獻(xiàn)[7]利用最小二乘法處理機(jī)械臂關(guān)節(jié)約束，從而提高了逆向運(yùn)動(dòng)學(xué)求解效率。文獻(xiàn)[8]在此基礎(chǔ)上提出了加權(quán)最小二乘法，得到虛擬關(guān)節(jié)方法，進(jìn)一步提高了逆向運(yùn)動(dòng)學(xué)的求解精度。

上述方法為本文的研究提供了很好的借鑒思路。本文擬采用改進(jìn)的DH 參數(shù)法建立冗余機(jī)械臂正向運(yùn)動(dòng)學(xué)模型，首先利用加權(quán)最小二乘法獲得其逆向運(yùn)動(dòng)學(xué)模型，接著利用五次多項(xiàng)式對(duì)其進(jìn)行軌跡規(guī)劃，最后通過仿真算例對(duì)本文所提方法的有效性進(jìn)行驗(yàn)證。

一、運(yùn)動(dòng)學(xué)建模

本文以類人七自由度冗余機(jī)械臂為例，采用改進(jìn)的DH參數(shù)[9]來描述其連桿坐標(biāo)系，如圖1所示。從圖1中可以獲得該機(jī)械臂連桿參數(shù)，即連桿長度ai-1、連桿轉(zhuǎn)角αi-1、連桿偏距di和關(guān)節(jié)角θi。

表1 七自由度冗余機(jī)械臂的DH 參數(shù)

圖1 七自由度冗余機(jī)械臂

接著，采用運(yùn)動(dòng)學(xué)遞推方式可以獲得7關(guān)節(jié)坐標(biāo)系O7X7Y7Z7在基坐標(biāo)系O0X0Y0Z0的位姿描述，即該機(jī)械臂的正向運(yùn)動(dòng)學(xué)方程為：

一般情況下，冗余機(jī)械臂從關(guān)節(jié)空間到末端執(zhí)行器操作空間的映射關(guān)系可描述為：

式中，θ?∈R7為機(jī)械臂關(guān)節(jié)角速度矩陣，J ∈R6×7為雅可比矩陣，x?∈R6為末端執(zhí)行器的廣義速度矩陣。

如果已知末端執(zhí)行器的速度軌跡為x?，則關(guān)節(jié)角速度θ?有無窮多組解，那么采用最小范數(shù)解的形式來描述θ?，即

其中，J?= JT(JJT)-1為雅可比矩陣的偽逆。由于存在冗余自由度，所以可以在完成末端執(zhí)行器軌跡主運(yùn)動(dòng)的基礎(chǔ)上參照某些性能指標(biāo)實(shí)現(xiàn)機(jī)械臂的自運(yùn)動(dòng)，例如關(guān)節(jié)位置極限、避奇異、避關(guān)節(jié)速度極限等。很明顯，式（4）沒有充分挖掘機(jī)械臂冗余自由度的操作性能，即存在非零齊次解。因此，在式（4）的右邊加上這個(gè)非零齊次解，可得：

式中，I7-J?J為雅可比矩陣的零空間矩陣，(I7-J?J)z可實(shí)現(xiàn)冗余機(jī)械臂的自運(yùn)動(dòng)而不影響主運(yùn)動(dòng)。z∈R7為任意n維梯度向量，即性能指標(biāo)函數(shù)。對(duì)于z的描述有多種形式[10]，例如可構(gòu)建一種避關(guān)節(jié)極限性能指標(biāo)梯度投影法來實(shí)現(xiàn)冗余機(jī)械臂避關(guān)節(jié)位置極限或避障，即：

式中，k是權(quán)重因子，?H(θ)是H(θ)的梯度向量。

此外，利用加權(quán)最小二乘法同樣能實(shí)現(xiàn)冗余機(jī)械臂避關(guān)節(jié)位置極限或避障。對(duì)雅可比矩陣做如下變換：

式中，Jw與θ?w分別為加權(quán)雅可比矩陣與加權(quán)角速度。W∈Rn×n為正定對(duì)角加權(quán)矩陣，W中的第i個(gè)對(duì)角元素wi可被定義為：

整理式（3）（8）與（9），可得：

當(dāng)Jw處于滿秩狀態(tài)時(shí)，基于加權(quán)最小范數(shù)的上式逆向運(yùn)動(dòng)學(xué)的解可表示為：

很顯然，當(dāng)J接近奇異點(diǎn)或wi過大時(shí)，Jw將變得病態(tài)。為解決這個(gè)問題，在上式中加入一個(gè)阻尼因子λ，即：

式中，I∈Rm為單位矩陣。值得注意的是，較小的λ會(huì)導(dǎo)致算法在奇異點(diǎn)附近的魯棒性變低，較大的λ會(huì)導(dǎo)致跟蹤誤差變大，故應(yīng)根據(jù)雅可比矩陣的最小奇異值σmin來選擇λ的值：

式中，ε為奇異值閾值，λmax為最大阻尼因子。

根據(jù)上述分析，基于梯度投影法的冗余機(jī)械臂逆向運(yùn)動(dòng)學(xué)的解（7）可調(diào)整為：

采用加權(quán)最小二乘法可將冗余機(jī)械臂逆向運(yùn)動(dòng)學(xué)的解（11）可調(diào)整為：

二、軌跡規(guī)劃算法

對(duì)于冗余機(jī)械臂的軌跡規(guī)劃，一般包括關(guān)節(jié)空間軌跡規(guī)劃和笛卡兒軌跡規(guī)劃兩種。[11-12]由于笛卡兒軌跡規(guī)劃要實(shí)時(shí)計(jì)算各路徑點(diǎn)的逆向運(yùn)動(dòng)學(xué)，計(jì)算效率低，所以本文選擇關(guān)節(jié)空間軌跡規(guī)劃。如果已知冗余機(jī)械臂的初始狀態(tài)x0和終止?fàn)顟B(tài)xf，先通過差分法獲得初始廣義速度x?0和終止廣義速度x?f，再由式(15)計(jì)算出對(duì)應(yīng)的關(guān)節(jié)角速度，最終通過積分方式獲得初始和終止的關(guān)節(jié)角。

對(duì)于冗余機(jī)械臂，若已知運(yùn)動(dòng)軌跡的起始點(diǎn)與終止點(diǎn)，以及運(yùn)動(dòng)過程中各關(guān)節(jié)處的位置、速度和加速度，便可確定一個(gè)五階多項(xiàng)式插值函數(shù)：

上式含有6 個(gè)系數(shù)a0、a1、a2、a3、a4與a5，則必須滿足6個(gè)限制條件，即：

求解上式，便可獲得五階多項(xiàng)式插值函數(shù)的解為：

三、仿真算例

本節(jié)將通過一個(gè)運(yùn)動(dòng)學(xué)仿真算例來驗(yàn)證所提運(yùn)動(dòng)學(xué)建模方法和軌跡規(guī)劃方法的有效性。仿真環(huán)境為Inter Core i7-4720 HQ，Matlab 2023a。

為了更好地獲得冗余機(jī)械臂的運(yùn)動(dòng)軌跡，先要計(jì)算出其工作空間。本文引入蒙特卡羅法[13]隨機(jī)獲得機(jī)械臂的可達(dá)工作區(qū)域，其計(jì)算公式為：

式中，randn(·)為具有正態(tài)分布特性的隨機(jī)函數(shù)。將獲得的隨機(jī)關(guān)節(jié)角序列代入式(1)中，就可以獲得冗余機(jī)械臂大致的工作空間，如圖2所示。

圖2 七自由度冗余機(jī)械臂的工作空間

上圖所示的工作空間中用Matlab 自帶的標(biāo)記工具選定冗余機(jī)械臂末端執(zhí)行器的初始廣義位置x0和終止廣義位置xf（如下所示）。同時(shí)，初始和終止的廣義速度均為0。

通過式(15)和積分運(yùn)算，可以得到冗余機(jī)械臂初始和終止的關(guān)節(jié)角（如下所示）。逆向運(yùn)動(dòng)學(xué)求解時(shí)間為0.26 s，計(jì)算效率較高。

進(jìn)而，通過式(16)～(18)，可以計(jì)算出冗余機(jī)械臂末端執(zhí)行器從初始狀態(tài)運(yùn)動(dòng)到終止?fàn)顟B(tài)各關(guān)節(jié)角的變化，如圖3～圖5 所示。整個(gè)仿真時(shí)間持續(xù)7 s。

圖3 關(guān)節(jié)角變化曲線

圖4 關(guān)節(jié)角速度變化曲線

圖5 關(guān)節(jié)角加速度變化曲線

為了進(jìn)一步描述冗余機(jī)械臂在笛卡兒空間中的運(yùn)動(dòng)過程，本文借助Robotics Toolbox 建立了該機(jī)械臂的可視化模型，并給出了末端執(zhí)行器的三維運(yùn)動(dòng)軌跡，如圖6 所示。從圖中可以看出末端執(zhí)行器的變化趨勢，三維運(yùn)動(dòng)軌跡較為光滑平坦，有利于優(yōu)化關(guān)節(jié)控制力矩。

圖6 關(guān)節(jié)角加速度變化曲線

最后，根據(jù)式(2)提取冗余機(jī)械臂末端執(zhí)行器的位置變化序列和姿態(tài)變換序列，如圖7和圖8 所示。這里需要說明的是，本文將方向余弦表征的姿態(tài)矩陣轉(zhuǎn)換成了歐拉角的形式。圖中結(jié)果也證明了所提出的運(yùn)動(dòng)學(xué)方程的有效性，可以有效計(jì)算出冗余機(jī)械臂的運(yùn)動(dòng)狀態(tài)。

圖7 末端執(zhí)行器位置變化曲線

圖8 末端執(zhí)行器姿態(tài)變化曲線

四、小結(jié)

本文提出了結(jié)合改進(jìn)DH 參數(shù)法和加權(quán)最小二乘法的綜合方法，來解決七自由度冗余機(jī)械臂的運(yùn)動(dòng)學(xué)建模問題，并采用五次多項(xiàng)式插值函數(shù)來計(jì)算其關(guān)節(jié)空間內(nèi)的關(guān)節(jié)運(yùn)動(dòng)軌跡。仿真結(jié)果顯示，改進(jìn)DH 參數(shù)法可以很好地建立冗余機(jī)械臂正向運(yùn)動(dòng)學(xué)模型，并可以結(jié)合蒙特卡羅法得到機(jī)械臂的工作空間；加權(quán)最小二乘法可以快速計(jì)算出冗余機(jī)械臂的逆向運(yùn)動(dòng)學(xué)解，效率較高；五次多項(xiàng)式可以幫助機(jī)械臂快速規(guī)劃出運(yùn)動(dòng)所需的關(guān)節(jié)運(yùn)動(dòng)軌跡，且關(guān)節(jié)運(yùn)動(dòng)較為平坦。

未來我們將進(jìn)一步研究冗余機(jī)械臂運(yùn)動(dòng)學(xué)解析解法，并研究其動(dòng)力學(xué)建模和運(yùn)動(dòng)控制。