冗余機械臂運動學建模與軌跡規劃分析

季曉明

（江蘇安全技術職業學院智能制造與應急裝備學院，江蘇 徐州，221011）

引言

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

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

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

一、運動學建模

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

表1 七自由度冗余機械臂的DH 參數

圖1 七自由度冗余機械臂

接著，采用運動學遞推方式可以獲得7關節坐標系O7X7Y7Z7在基坐標系O0X0Y0Z0的位姿描述，即該機械臂的正向運動學方程為：

一般情況下，冗余機械臂從關節空間到末端執行器操作空間的映射關系可描述為：

式中，θ?∈R7為機械臂關節角速度矩陣，J ∈R6×7為雅可比矩陣，x?∈R6為末端執行器的廣義速度矩陣。

如果已知末端執行器的速度軌跡為x?，則關節角速度θ?有無窮多組解，那么采用最小范數解的形式來描述θ?，即

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

式中，I7-J?J為雅可比矩陣的零空間矩陣，(I7-J?J)z可實現冗余機械臂的自運動而不影響主運動。z∈R7為任意n維梯度向量，即性能指標函數。對于z的描述有多種形式[10]，例如可構建一種避關節極限性能指標梯度投影法來實現冗余機械臂避關節位置極限或避障，即：

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

此外，利用加權最小二乘法同樣能實現冗余機械臂避關節位置極限或避障。對雅可比矩陣做如下變換：

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

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

當Jw處于滿秩狀態時，基于加權最小范數的上式逆向運動學的解可表示為：

很顯然，當J接近奇異點或wi過大時，Jw將變得病態。為解決這個問題，在上式中加入一個阻尼因子λ，即：

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

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

根據上述分析，基于梯度投影法的冗余機械臂逆向運動學的解（7）可調整為：

采用加權最小二乘法可將冗余機械臂逆向運動學的解（11）可調整為：

二、軌跡規劃算法

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

對于冗余機械臂，若已知運動軌跡的起始點與終止點，以及運動過程中各關節處的位置、速度和加速度，便可確定一個五階多項式插值函數：

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

求解上式，便可獲得五階多項式插值函數的解為：

三、仿真算例

本節將通過一個運動學仿真算例來驗證所提運動學建模方法和軌跡規劃方法的有效性。仿真環境為Inter Core i7-4720 HQ，Matlab 2023a。

為了更好地獲得冗余機械臂的運動軌跡，先要計算出其工作空間。本文引入蒙特卡羅法[13]隨機獲得機械臂的可達工作區域，其計算公式為：

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

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

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

通過式(15)和積分運算，可以得到冗余機械臂初始和終止的關節角（如下所示）。逆向運動學求解時間為0.26 s，計算效率較高。

進而，通過式(16)～(18)，可以計算出冗余機械臂末端執行器從初始狀態運動到終止狀態各關節角的變化，如圖3～圖5 所示。整個仿真時間持續7 s。

圖3 關節角變化曲線

圖4 關節角速度變化曲線

圖5 關節角加速度變化曲線

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

圖6 關節角加速度變化曲線

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

圖7 末端執行器位置變化曲線

圖8 末端執行器姿態變化曲線

四、小結

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

未來我們將進一步研究冗余機械臂運動學解析解法，并研究其動力學建模和運動控制。