冗余機(jī)器人的笛卡兒空間螺旋線性插值軌跡規(guī)劃

摘要：通過分析對(duì)偶四元數(shù)與剛體運(yùn)動(dòng)、螺旋運(yùn)動(dòng)之間的關(guān)系并結(jié)合指數(shù)積公式，得到基于對(duì)偶四元數(shù)的末端位姿變換算子;根據(jù)牛頓-拉夫森法，將對(duì)偶四元數(shù)用于求解冗余機(jī)器人的逆運(yùn)動(dòng)學(xué)數(shù)值解;根據(jù)對(duì)偶四元數(shù)的線性插值特點(diǎn)提出一種基于螺旋線性插值的軌跡規(guī)劃方法。兩種冗余機(jī)器人驗(yàn)證了提出的軌跡規(guī)劃方法能實(shí)現(xiàn)笛卡兒空間平滑運(yùn)動(dòng)和速度可控，且在求解運(yùn)動(dòng)學(xué)數(shù)值解時(shí)，使用對(duì)偶四元數(shù)比齊次矩陣具有更好的穩(wěn)定性和更高的解算速度。

關(guān)鍵詞：對(duì)偶四元數(shù)；軌跡規(guī)劃；逆運(yùn)動(dòng)學(xué)；指數(shù)積；冗余機(jī)器人

中圖分類號(hào)：TP242

DOI：10.3969/j.issn.1004-132X.2025.01.011

開放科學(xué)（資源服務(wù)）標(biāo)識(shí)碼（OSID）：

Cartesian Space Screw Linear Interpolation Trajectory Planning for

Redundant Robots

WANG Zhiqiang1"HAN Jianhai1，2，3*"LI Xiangpan1，2"GUO Bingjing1，2"DU Ganqin4

1.School of Mechatronics Engineering，Henan University of Science and Technology，Luoyang，

Henan，471003

2.Henan Provincial Key Laboratory of Robotics and Intelligent Systems，Luoyang，Henan，471003

3.Collaborative Innovation Center of Machinery Equipment Advanced Manufacturing of Henan

Province，Luoyang，Henan，471000

4.The First Affiliated Hospital of Henan University of Science and Technology，Luoyang，

Henan，471003

Abstract： By analyzing the relationship between dual quaternions and rigid body motion， and the relationship between dual quaternions and screw motion， a transformation operator of the end-effector pose was obtained based on dual quaternions with the combination of PoE formula. Based on the Newton-Raphson method， the dual quaternions were applied to solve the numerical solution of the inverse kinematics of redundant robots. Based on the linear interpolation characteristics of dual quaternions， a trajectory planning method was proposed based on screw linear interpolation. Taking two types of redundant robots as examples， the proposed trajectory planning method that achieved smooth motion and controllable velocity in Cartesian space was verified. Dual quaternions have better stability and higher solving speed than homogeneous matrices in solving kinematics numerical solutions.

Key words： dual quaternion; trajectory planning; inverse kinematics; product of exponentials（PoE）; redundant robot

0"引言

對(duì)偶四元數(shù)可被視為四元數(shù)的擴(kuò)展，能同時(shí)表達(dá)平移和旋轉(zhuǎn)，與齊次矩陣相比，它使用的參數(shù)更少，形式更簡潔［1-4］。平移運(yùn)動(dòng)和旋轉(zhuǎn)運(yùn)動(dòng)是研究剛體運(yùn)動(dòng)的基礎(chǔ)。計(jì)算機(jī)圖形學(xué)蒙皮算法中，用對(duì)偶四元數(shù)表示骨骼的剛體運(yùn)動(dòng)具有內(nèi)存占用少、計(jì)算速度高的特點(diǎn)［5-6］。用對(duì)偶四元數(shù)表示航天器位姿能精確、穩(wěn)定地實(shí)現(xiàn)運(yùn)動(dòng)控制［7-8］。機(jī)器人運(yùn)動(dòng)學(xué)中，位姿的表達(dá)與剛體運(yùn)動(dòng)有關(guān)，而對(duì)偶四元數(shù)和齊次矩陣表示剛體運(yùn)動(dòng)是等效的，但對(duì)偶四元數(shù)計(jì)算位姿變換的效率更高［9］。

旋量理論能以簡潔的形式分析機(jī)器人的結(jié)構(gòu)，在機(jī)器人運(yùn)動(dòng)學(xué)和動(dòng)力學(xué)的研究中得到深入而廣泛的應(yīng)用［10-13］，可用李代數(shù)表示剛體的螺旋運(yùn)動(dòng)，并通過指數(shù)映射將李代數(shù)se（3）轉(zhuǎn)換為特殊歐氏群SE（3）。以李代數(shù)的指數(shù)建立的運(yùn)動(dòng)學(xué)建模方法即指數(shù)積公式（product of exponentials，PoE）簡化了建模過程，并將對(duì)偶四元數(shù)和李代數(shù)引入機(jī)器人運(yùn)動(dòng)學(xué)領(lǐng)域。對(duì)偶四元數(shù)與旋量有緊密聯(lián)系，三維空間的軸線和速度旋量可用純對(duì)偶四元數(shù)唯一表示［14-15］。純對(duì)偶四元數(shù)的指數(shù)對(duì)應(yīng)李代數(shù)的指數(shù)運(yùn)算，單位對(duì)偶四元數(shù)的對(duì)數(shù)對(duì)應(yīng)特殊歐氏群的對(duì)數(shù)運(yùn)算，因而指數(shù)積公式建立的運(yùn)動(dòng)學(xué)模型可用于對(duì)偶四元數(shù)的計(jì)算。

逆運(yùn)動(dòng)學(xué)是將笛卡兒爾空間運(yùn)動(dòng)軌跡映射到機(jī)器人關(guān)節(jié)空間。冗余機(jī)器人的自由度多于所在約束空間的自由度，其笛卡兒爾空間到關(guān)節(jié)空間的映射有無窮多個(gè)結(jié)果。使用數(shù)值法求解冗余機(jī)器人逆運(yùn)動(dòng)學(xué)問題是一種常用方法，而對(duì)偶四元數(shù)的應(yīng)用有助于提高解算速度。數(shù)值法由牛頓-拉夫森法擴(kuò)展而來，利用末端位姿的一階偏導(dǎo)數(shù)——雅可比矩陣，根據(jù)梯度下降逐漸減小位姿誤差，通用于各種機(jī)器人模型［16-22］。使用齊次矩陣計(jì)算末端誤差時(shí)，需要將旋轉(zhuǎn)和平移分離，并分別使用矩陣乘法和向量減法計(jì)算末端位姿誤差。使用對(duì)偶四元數(shù)計(jì)算末端誤差只需進(jìn)行對(duì)偶四元數(shù)乘法運(yùn)算。與齊次矩陣對(duì)數(shù)運(yùn)算相比，對(duì)偶四元數(shù)對(duì)數(shù)運(yùn)算具有數(shù)據(jù)量少、計(jì)算效率高的特點(diǎn)［23］。

軌跡規(guī)劃是機(jī)器人運(yùn)動(dòng)學(xué)的重要組成。機(jī)器人關(guān)節(jié)的運(yùn)動(dòng)受加速度和速度的約束，而軌跡規(guī)劃通常還需考慮機(jī)器人運(yùn)動(dòng)的平滑性問題。多項(xiàng)式規(guī)劃、梯形速度規(guī)劃、S形速度規(guī)劃這些常用的規(guī)劃方法都能使機(jī)器人平滑運(yùn)動(dòng)。笛卡兒爾空間中，上述規(guī)劃方法需要根據(jù)6個(gè)自由度的差值進(jìn)行逐一規(guī)劃［24-26］。螺旋線性插值（screw linear interpolation，ScLERP）是一種對(duì)偶四元數(shù)插值方法［5，27-29］，起始和終止位姿已知時(shí)，插值軌跡呈現(xiàn)出繞某一空間軸線螺旋運(yùn)動(dòng)的特點(diǎn)。插值軌跡是一段螺旋線且插值變量是單變量。當(dāng)改變插值步長并保持插值點(diǎn)時(shí)間間隔相等時(shí)，機(jī)器人末端運(yùn)動(dòng)速度發(fā)生改變。

為實(shí)現(xiàn)冗余機(jī)器人笛卡兒爾空間軌跡的平滑運(yùn)動(dòng)和速度可控，本文使用對(duì)偶四元數(shù)和旋量理論的一些方法，在冗余機(jī)器人運(yùn)動(dòng)學(xué)建模、逆運(yùn)動(dòng)學(xué)求解過程中替代齊次矩陣；基于螺旋線性插值方法的特性，提出了使用五次多項(xiàng)式改變插值步長的方法，使規(guī)劃的笛卡兒爾空間軌跡平滑，機(jī)器人運(yùn)動(dòng)無沖擊和抖動(dòng)，還可根據(jù)笛卡兒爾空間的速度約束調(diào)節(jié)插值參數(shù)。

1"剛體運(yùn)動(dòng)、螺旋運(yùn)動(dòng)與對(duì)偶四元數(shù)

1.1"剛體運(yùn)動(dòng)和螺旋運(yùn)動(dòng)的單位對(duì)偶四元數(shù)表示

剛體運(yùn)動(dòng)是平移和旋轉(zhuǎn)的運(yùn)動(dòng)組合。如圖1所示，參考坐標(biāo)系下，剛體平移向量為t，旋轉(zhuǎn)軸通過參考坐標(biāo)系原點(diǎn)，用單位向量l表示，旋轉(zhuǎn)角度為θ。對(duì)偶四元數(shù)表示的剛體運(yùn)動(dòng)變換算子為

q^=cosθ2+sinθ2l+ε（12tcosθ2－12t·lsinθ2+12t×lsinθ2）（1）

式中：ε為對(duì)偶算子，ε×1=ε，ε×ε=0。

螺旋運(yùn)動(dòng)和剛體運(yùn)動(dòng)等效，根據(jù)Challes定理，螺旋運(yùn)動(dòng)是剛體繞空間中某一軸線旋轉(zhuǎn)和沿該軸線平移的復(fù)合運(yùn)動(dòng)。如圖2所示，假設(shè)剛體繞軸線的旋轉(zhuǎn)角度為θ，沿該軸線的平移距離為d，其中，旋轉(zhuǎn)軸用普呂克坐標(biāo)（l;m）表示，向量m為軸線的矢矩。用對(duì)偶四元數(shù)表示的螺旋運(yùn)動(dòng)算子為

q^=cosθ^2+（l+εm）sinθ^2=（cosθ2+lsinθ2）+ε（msinθ2－d2sinθ2+ld2cosθ2）（2）

θ^=θ+εd

螺旋運(yùn)動(dòng)中的旋轉(zhuǎn)角度θ=0時(shí)，q^=1+dεl/2。螺旋運(yùn)動(dòng)中的平移距離d=0時(shí)，q^=cos（θ/2）+lsin（θ/2）+εmsin（θ/2），如果螺旋運(yùn)動(dòng)的軸線穿過剛體坐標(biāo)系原點(diǎn)，則旋轉(zhuǎn)軸的矢矩為零，q^=cos（θ/2）+lsin（θ/2）+ε0。

螺旋運(yùn)動(dòng)的變換算子與對(duì)偶四元數(shù)的指數(shù)運(yùn)算相等，即

q^=exp（θ^ξ^/2）（3）

ξ^=0+l+ε（0+m）

對(duì)偶四元數(shù)存在對(duì)數(shù)運(yùn)算：

θ^2ξ^=ln q^（4）

螺旋運(yùn)動(dòng)與剛體運(yùn)動(dòng)等效，兩者表示的變換算子也相等。由式（1）、式（2）可知，2個(gè)對(duì)偶四元數(shù)的主部和對(duì)偶部都相等。對(duì)偶四元數(shù)主部都是單位四元數(shù)，因此式（1）、式（2）表示的旋轉(zhuǎn)軸方向和旋轉(zhuǎn)角度相同。式（1）、式（2）的對(duì)偶部相等，都是一個(gè)常規(guī)四元數(shù)，從而得到

d=t·l

m=（t×l+（l×t）×lcot（θ/2））/2（5）

表示齊次變換的對(duì)偶四元數(shù)q^直接計(jì)算得到的平移運(yùn)動(dòng)計(jì)算公式為

t^=q^q-"^*=1+ε（dl+msin θ+（1－cos θ）l×m）

平移向量為

t=dl+msin θ+（1－cos θ）l×m（6）

其中，q-"^*為單位對(duì)偶四元數(shù)的組合共軛，同時(shí)具有四元數(shù)共軛和對(duì)偶數(shù)共軛的性質(zhì)。

1.2"旋量理論與對(duì)偶四元數(shù)

旋量理論中，李代數(shù)se（3）的伴隨變換表示將李代數(shù)從一個(gè)坐標(biāo)系變換到另一個(gè)坐標(biāo)系：

Adgξ=R0t∧RRwv（7）

ξ=［wT"vT］T∈R6

式中：Adg為伴隨變換算子，下標(biāo)g∈SE（3）是齊次矩陣，表示兩個(gè)坐標(biāo)系之間的變換關(guān)系；R為旋轉(zhuǎn)矩陣；t∧為平移向量t的反對(duì)稱矩陣；ξ為六維向量，表示李代數(shù)se（3）；w、v都是三維向量，分別表示角速度和線速度。

用一個(gè)單位對(duì)偶四元數(shù)替代齊次矩陣，則對(duì)偶四元數(shù)形式的伴隨變換為

Adq^ξ^=q^ξ^q^*（8）

式中：q^*為q^的共軛。

使用單位對(duì)偶四元數(shù)表示的坐標(biāo)變換存在逆變換，即滿足q^q^－1=q^q^*=1［30］，對(duì)q^q^－1=q^q^*=1進(jìn)行微分可得q^·q^*+q^q^·*=0。根據(jù)對(duì)偶四元數(shù)乘法和共軛性質(zhì)，可得

q^·q^*=－（q^·q^*）*（9）

由式（9）可知，q^·q^*是一個(gè)純對(duì)偶四元數(shù)。令δ^=q^·q^*，則q^·=δ^q^。對(duì)q^·=δ^q^積分可得q^=exp（δ^）q^0，令q^0=1，得到q^=exp（δ^）。由旋量理論可知，如果用單位對(duì)偶四元數(shù)表示李群，那么李代數(shù)可用純對(duì)偶四元數(shù)表示，且李代數(shù)映射到李群同樣也是指數(shù)運(yùn)算。

2"機(jī)器人運(yùn)動(dòng)學(xué)與軌跡規(guī)劃

2.1"指數(shù)積（PoE）公式的對(duì)偶四元數(shù)形式表示

指數(shù)積公式只需要建立參考坐標(biāo)系和末端坐標(biāo)系，簡化了建模過程，使運(yùn)動(dòng)變換更加直觀。指數(shù)積公式利用指數(shù)運(yùn)算將李代數(shù)映射為李群，最終的末端位姿仍使用齊次矩陣表示。

螺旋運(yùn)動(dòng)可用對(duì)偶四元數(shù)表示，通過指數(shù)積公式建立的運(yùn)動(dòng)學(xué)模型也可由對(duì)偶四元數(shù)表示。在參考坐標(biāo)系下，使用普呂克坐標(biāo)表示機(jī)器人運(yùn)動(dòng)關(guān)節(jié)的靜態(tài)位置。如圖3所示，對(duì)于旋轉(zhuǎn)關(guān)節(jié)，通過關(guān)節(jié)軸線的單位方向向量l和軸線上任意一點(diǎn)r來描述該旋轉(zhuǎn)軸線，得到由向量l和矢矩m=r×l組成的普呂克坐標(biāo)（l;m）。對(duì)于移動(dòng)關(guān)節(jié)，移動(dòng)方向使用單位向量l描述，得到普呂克坐標(biāo)（0;l）。旋轉(zhuǎn)和平移的運(yùn)動(dòng)量均使用θ表示，旋轉(zhuǎn)關(guān)節(jié)中θ為轉(zhuǎn)角，平移關(guān)節(jié)中θ為距離。

通過對(duì)運(yùn)動(dòng)關(guān)節(jié)的建模，并結(jié)合螺旋運(yùn)動(dòng)的對(duì)偶四元數(shù)表示方法，可得運(yùn)動(dòng)變換后的連桿與其初始位姿之間的轉(zhuǎn)換關(guān)系。將普呂克坐標(biāo)用純對(duì)偶四元數(shù)ξ^表示，得到一個(gè)用純對(duì)偶四元數(shù)表示的李代數(shù)θξ^/2。使用對(duì)偶四元數(shù)指數(shù)運(yùn)算得到描述位姿變換的單位對(duì)偶四元數(shù)算子q^=exp（θξ^/2）。

對(duì)于串聯(lián)機(jī)器人，單個(gè)關(guān)節(jié)建模完成后，機(jī)器人的運(yùn)動(dòng)變換可看作是由末端關(guān)節(jié)到基座連接處關(guān)節(jié)依次的位姿變換，則串聯(lián)機(jī)器人末端位姿與初始位姿之間的變換關(guān)系為

q^=q^1q^2…q^n（10）

串聯(lián)機(jī)器人末端坐標(biāo)系的初始位姿與參考坐標(biāo)系存在單位對(duì)偶四元數(shù)q^0表示的固定變換關(guān)系，則機(jī)器人末端相對(duì)參考坐標(biāo)系的變換關(guān)系為

q^=q^1q^2…q^nq^0（11）

2.2"機(jī)器人運(yùn)動(dòng)學(xué)的雅可比矩陣

由指數(shù)積公式可得串聯(lián)機(jī)器人末端位姿q^end=q^1q^2…q^nq^0，將其微分得到

q^·end=（θ·1ξ^1q^1q^2…q^nq^0+θ·2q^1ξ^2q^2…q^nq^0+…+

θ·nq^1q^2…ξ^nq^nq^0）/2（12）

ξ^i=li+εmi

式中：θ·i為關(guān)節(jié)i的角速度。

對(duì)機(jī)器人末端位姿求逆得到

q^－1end=q^*0q^*n…q^*2q^*1（13）

對(duì)偶四元數(shù)表示的機(jī)器人末端的空間速度為

δ^end=2q^·endq^－1end=

θ·1ξ^1+q^1θ·2ξ^2q^*1+…+

q^1q^2…q^n－1θ·nξ^nq^*n－1…q^*2q^*1（14）

令

θ·=［θ·1θ·2…θ·n］T

J^=［J^1J^2…J^n］

J^是元素為純對(duì)偶四元數(shù)的行向量，可映射為幾何雅可比矩陣，從而得到

δ^end=J^θ·

J^i=（Adq^1q^2…q^i－1）ξ^i=q^1q^2…q^i－1ξ^iq^*i－1…q^*2q^*1（15）

由于無法計(jì)算純對(duì)偶四元數(shù)向量J^的逆，為表示笛卡兒爾空間速度與關(guān)節(jié)空間速度的映射關(guān)系，將J^中的所有元素變換成六維列向量。最終得到與文獻(xiàn)［31］結(jié)果一致的空間雅可比矩陣：

J=［J1J2…Jn］（16）

Ji=（AdK）ξi""K=exp（∑i－1j=1θjξ∧j）

2.3"逆運(yùn)動(dòng)學(xué)的求解

逆運(yùn)動(dòng)學(xué)的數(shù)值解由迭代計(jì)算獲得。機(jī)器人零位末端位姿的對(duì)偶四元數(shù)表示為q^0，目標(biāo)末端位姿的對(duì)偶四元數(shù)表示為q^d，則用對(duì)偶四元數(shù)表示的位姿誤差為

e^=q^dq^－10=q^dq^*0（17）

e^為初始誤差，使用對(duì)偶四元數(shù)對(duì)數(shù)運(yùn)算得到e^對(duì)應(yīng)的速度旋量即末端位姿的梯度。

雅可比矩陣存在不滿秩的情況，因此矩陣的逆不一定存在。為避免矩陣奇異無法得到速度旋量，使用阻尼最小二乘法求解逆雅可比矩陣。將雅可比矩陣進(jìn)行奇異值分解：

J=UΣVT=u1u2uzTσ10…00σ2…000…σzv1v2vz（18）

雅可比矩陣J的行數(shù)m大于列數(shù)n時(shí)，z=n；反之，z=m。求解逆矩陣可得

J-1=∑zi=1σ2iσ2i+k2viuTi（19）

式中：k為阻尼因子。

雅可比矩陣可將關(guān)節(jié)速度映射為末端速度，位姿誤差代表的末端速度可通過逆雅可比矩陣映射為關(guān)節(jié)速度。因此更新關(guān)節(jié)位置，得到新的末端位姿對(duì)偶四元數(shù)，直至與期望位姿相等，具體步驟如下：

1）計(jì)算末端位姿誤差e^i=q^dq^*i，根據(jù)對(duì)偶四元數(shù)對(duì)數(shù)運(yùn)算，獲得笛卡兒爾空間位姿梯度λ^i=ln e^i。

2）由于λ^i是純對(duì)偶四元數(shù)，故將其轉(zhuǎn)換為六維向量λ。使用阻尼最小二乘法計(jì)算空間雅可比矩陣的逆J-1（θi），將笛卡兒爾空間位姿梯度映射到關(guān)節(jié)空間，得到關(guān)節(jié)角度梯度Δθ=J-1（θi）λ。

3）‖λ^i‖若小于誤差η，則結(jié)束迭代。否則，更新關(guān)節(jié)角度θi+1=θi+Δθ和末端姿態(tài)q^i+1=q^（θi+1），重新執(zhí)行步驟1）。

2.4"軌跡規(guī)劃

笛卡兒爾空間軌跡規(guī)劃一般需要在6個(gè)自由度上獨(dú)立規(guī)劃［26-27］。盡管單獨(dú)規(guī)劃能保證機(jī)器人平滑運(yùn)動(dòng)，但不容易實(shí)現(xiàn)空間速度控制。螺旋線性插值具有螺旋運(yùn)動(dòng)特征，插值變量只有一個(gè)，改變?cè)摬逯底兞烤蜁?huì)改變空間速度，從而實(shí)現(xiàn)對(duì)空間速度的控制。

螺旋線性插值由四元數(shù)的球面線性插值推廣而來。四元數(shù)可表示旋轉(zhuǎn)運(yùn)動(dòng)，球面線性插值保持旋轉(zhuǎn)軸不變，沿著旋轉(zhuǎn)軌跡進(jìn)行插值，得到的圓弧軌跡是球面上兩點(diǎn)之間的最短軌跡。對(duì)偶四元數(shù)可表示螺旋運(yùn)動(dòng)，螺旋線性插值的插值軌跡是螺旋運(yùn)動(dòng)結(jié)果。螺旋線雖然超出球面范圍，但仍是螺旋運(yùn)動(dòng)中的最短軌跡。

螺旋線性插值公式為

S（γ;q^A，q^B）=q^A（q^*Aq^B）γ（20）

式中：γ為插值步長， γ∈［0，1］；q^A、q^B分別為機(jī)器人末端的起始位姿和終止位姿。

螺旋線性插值軌跡由起始位姿和終止位姿決定，機(jī)器人沿插值軌跡的運(yùn)動(dòng)速度受插值步長影響。為使機(jī)器人能平滑運(yùn)動(dòng)，將插值步長設(shè)計(jì)為一個(gè)關(guān)于時(shí)間t的高階函數(shù)或分段函數(shù)。本文選擇使用五次多項(xiàng)式

γ=a0+a1t+a2t2+a3t3+a4t4+a5t5（21）

進(jìn)行軌跡規(guī)劃。根據(jù)軌跡規(guī)劃的笛卡兒爾空間速度和加速度約束條件確定多項(xiàng)式中的參數(shù)：

γ（0）=0γ·（0）=0γ¨（0）=0a0=0a1=0a2=0（22）

γ（T）=1γ·（T）=0γ¨（T）=0a3=10/T3a4=－15/T4a5=6/T5（23）

式中：T為機(jī)器人運(yùn)動(dòng)總時(shí)長。

3"實(shí)例分析

本文選擇平面3R機(jī)器人和七自由度Sawyer機(jī)器人進(jìn)行軌跡規(guī)劃分析。平面3R機(jī)器人屬于平面冗余機(jī)器人。螺旋線性插值可保證運(yùn)動(dòng)速度穩(wěn)定可控，圓弧軌跡能增加關(guān)節(jié)活動(dòng)量。Sawyer機(jī)器人是一種空間冗余機(jī)器人，其逆運(yùn)動(dòng)學(xué)解算有一定困難。將對(duì)偶四元數(shù)用于Sawyer機(jī)器人的軌跡規(guī)劃和逆運(yùn)動(dòng)學(xué)求解，計(jì)算效率更高，也能充分發(fā)揮機(jī)器人的靈活性。

如圖4所示，使用指數(shù)積公式對(duì)上述2個(gè)機(jī)器人進(jìn)行運(yùn)動(dòng)學(xué)建模。通過建立參考坐標(biāo)系{O}、末端坐標(biāo)系{Oend}、關(guān)節(jié)軸線得到兩種機(jī)器人關(guān)節(jié)軸線的普呂克坐標(biāo)參數(shù)，如表1、表2所示。

由平面3R機(jī)器人的普呂克坐標(biāo)可得連桿及末端相對(duì)零位的坐標(biāo)變換算子：

q^1=cosθ12+zsinθ12+ε0

q^2=cosθ22+zsinθ22+ε（b1xsinθ22）

q^3=cosθ32+zsinθ32+ε0

q^0=1+ε（－b32y）（24）

連桿的實(shí)際長度b1、b2、b3分別為0.36 m、0.36 m、0.23 m。

Sawyer機(jī)器人關(guān)節(jié)的幾何尺寸根據(jù)MATLAB軟件中Sawyer機(jī)器人的URDF文件得到。連桿1～7相對(duì)零位時(shí)的坐標(biāo)變換算子為

q^1=cosθ12+zsinθ12+ε0

q^2=cosθ22+ysinθ22+ε（m2sinθ22）q^3=cosθ32+xsinθ32+ε（m3sinθ32）q^4=cosθ42+ysinθ42+ε（m4sinθ42）q^5=cosθ52+xsinθ52+ε（m5sinθ52）q^6=cosθ62+ysinθ62+ε（m6sinθ62）q^7=cosθ72+xsinθ72+ε（m7sinθ72）（25）

末端相對(duì)零位時(shí)的坐標(biāo)變換算子為

q^0=0.5+0.5i+0.5j+0.5k+

ε（－0.353+0.234i－0.1545j+0.273k）（26）

3.1"逆運(yùn)動(dòng)學(xué)分析

在相同的計(jì)算平臺(tái)下，使用MATLAB將平面3R機(jī)器人和Sawyer機(jī)器人的正運(yùn)動(dòng)學(xué)、雅可比矩陣等編寫為獨(dú)立的函數(shù)文件，按照2.3節(jié)的數(shù)值解求解步驟，隨機(jī)選取500個(gè)不同的末端位姿進(jìn)行實(shí)例分析。與使用齊次矩陣求解逆運(yùn)動(dòng)數(shù)值解相比，對(duì)偶四元數(shù)法在解算速度和穩(wěn)定性上都更優(yōu)。

圖5、圖6所示分別為平面3R機(jī)器人逆運(yùn)動(dòng)學(xué)數(shù)值解的耗用時(shí)間和迭代次數(shù)。對(duì)偶四元數(shù)法迭代次數(shù)不超過設(shè)定閾值1000的有492例，而齊次矩陣法有465例。去除迭代次數(shù)超過閾值的算例后，對(duì)偶四元數(shù)法平均解算時(shí)間是0.587 ms，而齊次矩陣法平均解算時(shí)間是0.976 ms。

Sawyer機(jī)器人實(shí)例中，對(duì)偶四元數(shù)法迭代次數(shù)不超過設(shè)定閾值的有500例，而齊次矩陣法有496例，如圖7所示。去除迭代次數(shù)超過閾值的解算例，對(duì)偶四元數(shù)法平均解算時(shí)間是1.961 ms，齊次矩陣法平均解算時(shí)間是8.295 ms，如圖8所示。

3.2"運(yùn)動(dòng)軌跡規(guī)劃

3.2.1"平面3R機(jī)器人軌跡規(guī)劃

平面3R機(jī)器人（康復(fù)機(jī)器人）第三連桿的延長方向與腳踝到腳底的方向相同。設(shè)定平面3R機(jī)器人末端的起始位姿為（0.82 m，-0.14 m；100°），終止位姿為（0.56 m，0.56 m；150°），規(guī)劃總時(shí)長為2 s，100°和150°是第三連桿的末端位姿與零位狀態(tài)的夾角（按照右手定則確定）。根據(jù)2.4節(jié)所述內(nèi)容對(duì)機(jī)器人笛卡兒爾空間軌跡進(jìn)行規(guī)劃，并使用逆運(yùn)動(dòng)學(xué)求解關(guān)節(jié)空間軌跡，軌跡規(guī)劃的仿真結(jié)果如圖9所示。

關(guān)節(jié)1～3的運(yùn)動(dòng)軌跡如圖10所示，其中，第一關(guān)節(jié)的仿真軌跡與實(shí)際軌跡有較大誤差，最大絕對(duì)誤差為4.747°；第二關(guān)節(jié)、第三關(guān)節(jié)的仿真軌跡與實(shí)際軌跡的最大絕對(duì)誤差分別為0.415°和0.221°。第一關(guān)節(jié)運(yùn)動(dòng)范圍比其他兩個(gè)關(guān)節(jié)大，運(yùn)動(dòng)速度也較快，因此第一關(guān)節(jié)運(yùn)動(dòng)軌跡的誤差比其他兩個(gè)關(guān)節(jié)大。實(shí)際運(yùn)動(dòng)軌跡與仿真軌跡不重合的主要原因是機(jī)器人伺服控制不理想。笛卡兒爾空間的實(shí)際軌跡與仿真軌跡不重合，如圖11所示，相同時(shí)刻的最大誤差為69 mm。盡管出現(xiàn)較大的軌跡誤差，但笛卡兒爾空間的實(shí)際軌跡仍具有螺旋線性插值的特點(diǎn)，且軌跡起始和終止處的實(shí)際軌跡與仿真軌跡是重合的。

平面3R機(jī)器人末端軌跡的笛卡兒爾空間速度如圖12所示。實(shí)際速度、仿真速度的X軸分量最大絕對(duì)值分別為0.3333 m/s和0.3403 m/s；實(shí)際速度、仿真速度的Y軸分量最大絕對(duì)值分別為0.6303 m/s和0.6841 m/s；笛卡兒爾空間速度的實(shí)際最大值、仿真最大值分別為0.6833 m/s和0.7227 m/s。機(jī)器人控制不理想，實(shí)際速度與仿真速度的X軸、Y軸分量最大絕對(duì)誤差分別為0.0961 m/s和0.1343 m/s，空間速度的最大絕對(duì)誤差為0.1601 m/s。機(jī)器人實(shí)際運(yùn)動(dòng)速度不能達(dá)到仿真最大值的主要原因是機(jī)器人只使用PID控制，機(jī)器人關(guān)節(jié)實(shí)際軌跡跟蹤沒有超調(diào)，后續(xù)可通過增加速度前饋來改善控制效果。圖12中，實(shí)際速度曲線滯后于仿真曲線，但兩條曲線的平滑特征相同，這表明機(jī)器人運(yùn)動(dòng)過程平滑、無沖擊，具有五次多項(xiàng)式插值的特點(diǎn)。

3.2.2"Sawyer機(jī)器人軌跡規(guī)劃

Sawyer機(jī)器人具有冗余的自由度，可以更加靈活進(jìn)行軌跡規(guī)劃。機(jī)器人關(guān)節(jié)1～7初始角度依次為-30°、-60°、5°、0°、0°、0°、0°，終止時(shí)關(guān)節(jié)角度依次為30°、0°、60°、0°、-30°、0°、0°，共用時(shí)2 s。軌跡規(guī)劃仿真結(jié)果如圖13所示，笛卡兒爾空間軌跡具有螺旋線特征。

Sawyer機(jī)器人關(guān)節(jié)1～7的運(yùn)行軌跡仿真結(jié)果如圖14所示，各個(gè)關(guān)節(jié)運(yùn)動(dòng)軌跡光滑，沒有出現(xiàn)突變。如圖15所示，笛卡兒爾空間的速度曲線也是光滑的。由仿真結(jié)果可知，螺旋線性插值能實(shí)現(xiàn)五次多項(xiàng)式軌跡規(guī)劃，且更容易通過調(diào)節(jié)插值步長來改變笛卡兒爾空間速度。

冗余機(jī)器人在笛卡兒爾空間軌跡規(guī)劃時(shí)常使用數(shù)值解進(jìn)行逆運(yùn)動(dòng)學(xué)求解。為提高求解精度，保證關(guān)節(jié)軌跡不出現(xiàn)突變，數(shù)值解求解需將上一時(shí)間步的逆解作為迭代的初始結(jié)果。與齊次矩陣法相比，對(duì)偶四元數(shù)法能減少關(guān)節(jié)軌跡突變的產(chǎn)生。

4"結(jié)論

本文通過分析剛體運(yùn)動(dòng)和螺旋運(yùn)動(dòng)使用對(duì)偶四元數(shù)表示的特點(diǎn)，結(jié)合旋量理論，得到矩陣指數(shù)和對(duì)偶四元數(shù)指數(shù)運(yùn)算的相關(guān)關(guān)系。通過指數(shù)積公式，建立了機(jī)器人運(yùn)動(dòng)學(xué)的對(duì)偶四元數(shù)表示方法。根據(jù)牛頓-拉夫森法、求逆矩陣的阻尼最小二乘法，將對(duì)偶四元數(shù)用于逆運(yùn)動(dòng)學(xué)數(shù)值法求解。根據(jù)螺旋線性插值方法的特點(diǎn)，通過使用五次多項(xiàng)式改變插值步長，提出基于螺旋線性插值的一種笛卡兒爾空間軌跡規(guī)劃方法。平面3R機(jī)器人的實(shí)際運(yùn)行和七自由度Sawyer機(jī)器人的仿真證明，使用對(duì)偶四元數(shù)的逆運(yùn)動(dòng)學(xué)數(shù)值解法比齊次矩陣法具有更好的穩(wěn)定性、更高的解算速度；提出的笛卡兒爾空間軌跡規(guī)劃方法在2臺(tái)機(jī)器人上都能實(shí)現(xiàn)平滑運(yùn)動(dòng)，且笛卡兒爾空間速度具有五次多項(xiàng)式軌跡規(guī)劃的特點(diǎn)。

參考文獻(xiàn)：

［1］"ZGR E， MEZOUAR Y. Kinematic Modeling and Control of a Robot Arm Using Unit Dual Quaternions［J］. Robotics and Autonomous Systems， 2016， 77：66-73.

［2］"ABAUNZA H， CHANDRA R， ZGR E， et al. Kinematic Screws and Dual Quaternion Based Motion Controllers［J］. Control Engineering Practice， 2022， 128：105325.

［3］"葛為民， 宇旭東， 王肖鋒， 等. 基于對(duì)偶四元數(shù)的機(jī)械臂運(yùn)動(dòng)學(xué)建模及分析［J］. 機(jī)械傳動(dòng)， 2018， 42（7）：112-117.

GE Weimin， YU Xudong， WANG Xiaofeng， et al. Kinematics Modeling and Analysis of Manipulator Based on Dual Quaternion［J］. Journal of Mechanical Transmission， 2018， 42（7）：112-117.

［4］"陳良港， 張方， 張建光， 等. 基于對(duì)偶四元數(shù)移動(dòng)機(jī)械臂運(yùn)動(dòng)學(xué)建模與控制［J］. 科技創(chuàng)新與應(yīng)用， 2022， 12（5）：1-5.

CHEN Lianggang， ZHANG Fang， ZHANG Jianguang， et al. Kinematics Modeling and Control of Mobile Manipulator Based on Dual Quaternion［J］. Technology Innovation and Application， 2022， 12（5）：1-5.

［5］"KAVAN L， COLLINS S，RA J， et al. Geometric Skinning with Approximate Dual Quaternion Blending［J］. ACM Transactions on Graphics， 2008， 27（4）：105.

［6］"LE B H， HODGINS J K. Real-time Skeletal Skinning with Optimized Centers of Rotation［J］. ACM Transactions on Graphics， 2016， 35（4）：1-10.

［7］"錢萍， 王惠南. 基于對(duì)偶四元數(shù)的航天器交會(huì)對(duì)接位姿雙目視覺測(cè)量算法［J］. 宇航學(xué)報(bào)， 2013， 34（1）：32-38.

QIAN Ping， WANG Huinan. A Binocular Vision Algorithm Based on Dual Quaternion for Relative Position and Attitude Determination of RVD Spacecrafts［J］. Journal of Astronautics， 2013， 34（1）：32-38.

［8］"翟洪民. 基于對(duì)偶四元數(shù)的航天器近距離接近位姿同步規(guī)劃與控制［D］. 哈爾濱：哈爾濱工業(yè)大學(xué)， 2021.

ZHAI Hongmin. Synchronous Planning and Control of Spacecraft’s Close Approach Position and Attitude Based on Dual Quaternion［D］.Harbin：Harbin Institute of Technology， 2021.

［9］"WANG Xiangke， ZHU Huayong. On the Comparisons of Unit Dual Quaternion and Homogeneous Transformation Matrix［J］. Advances in Applied Clifford Algebras， 2014， 24（1）：213-229.

［10］"戴建生. 機(jī)構(gòu)學(xué)與旋量理論的歷史淵源以及有限位移旋量的發(fā)展［J］. 機(jī)械工程學(xué)報(bào)， 2015， 51（13）：13-26.

DAI Jiansheng. Historical Relation between Mechanisms and Screw Theory and the Development of Finite Displacement Screws［J］. Journal of Mechanical Engineering， 2015， 51（13）：13-26.

［11］"黃曉華， 王興成. 機(jī)器人動(dòng)力學(xué)的李群表示及其應(yīng)用［J］. 中國機(jī)械工程， 2007， 18（2）：201-205.

HUANG Xiaohua， WANG Xingcheng. Lie Group Representation of Robot Dynamics and Its Applications［J］. China Mechanical Engineering， 2007， 18（2）：201-205.

［12］"裴九芳， 許德章， 王海. 基于旋量理論的三指機(jī)器人靈巧手逆運(yùn)動(dòng)學(xué)分析［J］. 中國機(jī)械工程， 2017， 28（24）：2975-2980.

PEI Jiufang， XU Dezhang， WANG Hai. Inverse Kinematics Analyses of 3-finger Robot Dexterous Hand Based on Screw Theory［J］. China Mechanical Engineering， 2017， 28（24）：2975-2980.

［13］"邵兵， 吳洪濤， 程世利， 等. 基于李群李代數(shù)的主被動(dòng)關(guān)節(jié)機(jī)器人動(dòng)力學(xué)及控制［J］. 中國機(jī)械工程， 2010， 21（3）：253-257.

SHAO Bing， WU Hongtao， CHENG Shili， et al. Dynamics and Control of Robot with Active and Passive Joints Using Lie Groups and Lie Algebras［J］. China Mechanical Engineering， 2010， 21（3）：253-257.

［14］"VILHENA ADORNO B. Robot Kinematic Modeling and Control Based on Dual Quaternion Algebra — Part I：Fundamentals［Z/OL］. 2017［2022-11-06］. https：∥hal.archives-ouvertes.fr/hal-01478225.

［15］"ADORNO B V，MARQUES MARINHO M. DQ Robotics：a Library for Robot Modeling and Control［J］. IEEE Robotics amp; Automation Magazine， 2021， 28（3）：102-116.

［16］"LECHUGA-GUTIERREZ L， MACIAS-GARCIA E， MARTNEZ-TERN G， et al. IterativeInverse Kinematics for Robot Manipulators Using Quaternion Algebra and Conformal Geometric Algebra［J］. Meccanica， 2022， 57（6）：1413-1428.

［17］"AHMED A， YUMeng， CHEN Feifei. Inverse Kinematic Solution of 6-DOF Robot-arm Based on Dual Quaternions and Axis Invariant Methods［J］. Arabian Journal for Science and Engineering， 2022， 47（12）：15915-15930.

［18］"QIAO Shuguang， LIAO Qizheng， WEI Shimin， et al. Inverse Kinematic Analysis of the General 6R Serial Manipulators Based on Double Quaternions［J］. Mechanism and Machine Theory， 2010， 45（2）：193-199.

［19］"ZHAO Zhiyong， WANG Tao， WANG Dongqing. Inverse Kinematic Analysis of the General 6R Serial Manipulators Based on Unit Dual Quaternion and Dixon Resultant［C］∥2017 Chinese Automation Congress （CAC）. IEEE， 2017：2646-2650.

［20］"WANG Hengsheng， ZHAN Deyou， HUANG Pinglun， et al. InverseKinematics of a Heavy Duty Manipulator with 6-DOF Based on Dual Quaternion［J］. Journal of Central South University， 2015， 22（7）：2568-2577.

［21］"KENWRIGHT B. Inverse Kinematics with Dual-quaternions， Exponential-maps， and Joint Limits［J］. arXiv， 2022：2211.01466.

［22］"CHEN Qingcheng， ZHU Shiqiang， ZHANG Xuequn. Improved Inverse Kinematics Algorithm Using Screw Theory for a Six-DOF Robot Manipulator［J］. International Journal of Advanced Robotic Systems， 2015， 12（10）：140.

［23］"DANTAM N T.Robust and Efficient Forward， Differential， and Inverse Kinematics Using Dual Quaternions［J］. The International Journal of Robotics Research， 2021， 40（10/11）：1087-1105.

［24］"任敬軼， 孫漢旭. 一種新穎的笛卡兒爾空間軌跡規(guī)劃方法［J］. 機(jī)器人， 2002， 24（3）：217-221.

REN Jingyi， SUN Hanxu. A Novel Method of Trajectory Planning in Cartesian Space［J］. Robot， 2002， 24（3）：217-221.

［25］"金榮玉， 耿云海. 空間機(jī)器人動(dòng)力學(xué)奇異回避的笛卡爾軌跡規(guī)劃［J］. 宇航學(xué)報(bào)， 2020， 41（8）：989-999.

JIN Rongyu， GENG Yunhai. Cartesian Trajectory Planning of Free-floating Space Robot with Dynamic Singularities Avoidance［J］. Journal of Astronautics， 2020， 41（8）：989-999.

［26］"李振娜， 王濤， 王斌銳， 等. 基于帶約束S型速度曲線的機(jī)械手笛卡兒爾空間軌跡規(guī)劃［J］. 智能系統(tǒng)學(xué)報(bào)， 2019， 14（4）：655-661.

LI Zhenna， WANG Tao， WANG Binrui， et al. Trajectory Planning for Manipulator in Cartesian Space Based on Constrained S-curve Velocity［J］. CAAI Transactions on Intelligent Systems， 2019， 14（4）：655-661.

［27］"SARKER A， SINHA A， CHAKRABORTY N. On Screw Linear Interpolation for Point-to-point Path Planning［C］∥2020 IEEE/RSJ International Conference on Intelligent Robots and Systems （IROS）. Las Vegas， 2020：9480-9487.

［28］"KAVAN L， COLLINS S， RA J， et al. Skinning with Dual Quaternions［C］∥Proceedings of the 2007 Symposium on Interactive 3D graphics and games. New York， 2007：39-46.［LinkOut］

［29］"KENWRIGHT B. Dual-Quaternion Interpolation［J］. arXiv， 2023：2303.13395.

［30］"SOL J， DERAY J， ATCHUTHAN D. A micro Lie theory for state estimation in robotics［J］. arXiv， 2018：1812.01537.

［31］"LYNCH K M. Modern Robotics：Mechanics， Planning， and Control［M］. New York， NY：Cambridge University Press.

（編輯"張"洋）

作者簡介：

王志強(qiáng)，男，1992年生，博士研究生。研究方向康復(fù)機(jī)器人技術(shù)。E-mail：wzq1992@stu.haust.edu.cn。

韓建海*（通信作者），男，1961年生，教授、博士研究生導(dǎo)師。研究方向?yàn)闄C(jī)器人技術(shù)。E-mail：jianhaihan@haust.edu.cn。

本文引用格式：

王志強(qiáng)，韓建海，李向攀，等.冗余機(jī)器人的笛卡兒爾空間螺旋線性插值軌跡規(guī)劃［J］. 中國機(jī)械工程，2025，36（1）：104-112.

WANG Zhiqiang， HAN Jianhai， LI Xiangpan， et al. Cartesian Space Screw Linear Interpolation Trajectory Planning for Redundant Robots［J］. China Mechanical Engineering， 2025， 36（1）：104-112.

收稿日期：2023-11-23

基金項(xiàng)目：河南省科技攻關(guān)項(xiàng)目（212102310890，212102310249）