普亞松 張文斌 郭德偉 閔 潔

（1.紅河學(xué)院 工學(xué)院 云南省高校高原機(jī)械性能分析與優(yōu)化重點(diǎn)實(shí)驗(yàn)室；2.西北工業(yè)大學(xué) 現(xiàn)代設(shè)計(jì)與集成制造技術(shù)教育部重點(diǎn)實(shí)驗(yàn)室）

姿態(tài)運(yùn)動軌跡規(guī)劃是工業(yè)機(jī)器人一項(xiàng)重要內(nèi)容。本文提出用單位球面上的三維矢量進(jìn)行姿態(tài)四元數(shù)插值，描述了該方法的推導(dǎo)與使用步驟，結(jié)合單位球面上的三維矢量，對四元數(shù)插值姿態(tài)的合成進(jìn)行說明。算例驗(yàn)證了該插補(bǔ)方法合理可行。

引言：工業(yè)機(jī)器人末端執(zhí)行器在工作空間的運(yùn)動軌跡規(guī)劃是一項(xiàng)重要的內(nèi)容，合理的運(yùn)動軌跡對工業(yè)機(jī)器人的工作質(zhì)量、效率和使用壽命有著極大的影響。工業(yè)機(jī)器人末端執(zhí)行器的運(yùn)動軌跡規(guī)劃分為位移規(guī)劃與姿態(tài)規(guī)劃，姿態(tài)規(guī)劃比位移規(guī)劃更為復(fù)雜，本文僅對姿態(tài)插值規(guī)劃進(jìn)行研究。

剛體運(yùn)動姿態(tài)的表示方法主要有旋轉(zhuǎn)矩陣、歐拉角和四元數(shù)。四元數(shù)描述姿態(tài)主要有如下優(yōu)點(diǎn)：1）避免萬向節(jié)鎖死現(xiàn)象；2）運(yùn)算效率更高；3）便于提供平滑插值。所以四元數(shù)大量運(yùn)用以各種旋轉(zhuǎn)的場合，目前已成為主流趨向。

姿態(tài)插補(bǔ)的一般方法是球面線性插補(bǔ)(Slerp)，該方法推理與運(yùn)用都比較簡單，但不直觀形象，四維向量夾角的物理意義不明顯。本文采用球面矢量進(jìn)行四元數(shù)姿態(tài)插補(bǔ)，用三維矢量直觀表達(dá)四元數(shù)，有助于理解四元數(shù)姿態(tài)合成的過程，并能夠?qū)崿F(xiàn)姿態(tài)平滑插值。

1.四元數(shù)與轉(zhuǎn)動

四元數(shù)一般表示方法為：

w稱為四元數(shù)的實(shí)部，稱為四元數(shù)的虛部或向量部。實(shí)部為零，即w=0時，四元數(shù)稱為純屬四元數(shù)。

任意兩個四元數(shù)：

單位四元數(shù)可以用轉(zhuǎn)角與轉(zhuǎn)軸表示：

其中θ為轉(zhuǎn)角，為轉(zhuǎn)軸矢量，公式里帶入的角度值為轉(zhuǎn)角的一半。如圖1a所示，剛體上一點(diǎn)，繞轉(zhuǎn)軸 ，轉(zhuǎn)動θ角，轉(zhuǎn)動之后新的位置點(diǎn)為，該轉(zhuǎn)動過程用數(shù)學(xué)方程描述為：，其中S1,S2分別為組成的純四元數(shù)，四元數(shù)物理意義明顯，一個四元數(shù)對應(yīng)一個轉(zhuǎn)動。

圖1

如果被旋轉(zhuǎn)的矢量與旋轉(zhuǎn)軸正交，如圖1b所示，則轉(zhuǎn)動過程用數(shù)學(xué)方程描述為：，此時的四元數(shù)格式變?yōu)椋骸Ｕ粫鴮懜袷娇梢韵鄳?yīng)地轉(zhuǎn)換為非正交的格式。

2.矢量法四元數(shù)姿態(tài)插值的原理與步驟

2.1 姿態(tài)與四元數(shù)

剛體轉(zhuǎn)動之后，不僅是剛體上質(zhì)點(diǎn)的位置坐標(biāo)發(fā)生了變化，而且剛體的方位，也即姿態(tài)，也會發(fā)生相應(yīng)的變化。一個四元數(shù)對應(yīng)一個轉(zhuǎn)軸與轉(zhuǎn)角，也就意味著發(fā)生了一個轉(zhuǎn)動，對應(yīng)著一個姿態(tài)。剛體連續(xù)轉(zhuǎn)動，每一個轉(zhuǎn)動對應(yīng)的四元數(shù)分別為，最終合成的姿態(tài)qe，。剛體無論發(fā)生多少個轉(zhuǎn)動，可視為最后合成一個轉(zhuǎn)動，可由一個轉(zhuǎn)軸與轉(zhuǎn)角一次性轉(zhuǎn)動到位。

剛體姿態(tài)插值時，已知剛體初始姿態(tài)，可理解為經(jīng)歷一個轉(zhuǎn)動，對應(yīng)四元數(shù)為qs。還知道結(jié)束姿態(tài)，也理解為經(jīng)歷一個轉(zhuǎn)動，對應(yīng)的四元數(shù)為qe。初始姿態(tài)再經(jīng)歷一個過渡轉(zhuǎn)動（對應(yīng)四元數(shù)為qm）之后變?yōu)榻Y(jié)束姿態(tài)，也即，初始姿態(tài)的轉(zhuǎn)動再疊加一個過度轉(zhuǎn)動可合成為一個結(jié)束姿態(tài)的轉(zhuǎn)動，。均勻分配中間過渡轉(zhuǎn)動，能得到一系列的末尾插值姿態(tài)qei，。

2.2 單位球面矢量表達(dá)四元數(shù)

剛體姿態(tài)插值時，知道初始姿態(tài)與結(jié)束姿態(tài)，對應(yīng)四元數(shù)分別為qs,qe，可求初始姿態(tài)與結(jié)束姿態(tài)之間過渡轉(zhuǎn)動的四元數(shù)qm，，可得到過渡轉(zhuǎn)動qm的轉(zhuǎn)角θ和轉(zhuǎn)軸。

綜上所述，初始姿態(tài)與結(jié)束姿態(tài)之間過渡轉(zhuǎn)動的四元數(shù)qm，可由單位球面上垂直于過渡轉(zhuǎn)動轉(zhuǎn)軸的兩個旋轉(zhuǎn)向量表示。如圖2所示，兩個旋轉(zhuǎn)向量垂直于轉(zhuǎn)軸，夾角為轉(zhuǎn)角θ，一個向量旋轉(zhuǎn)到另一個向量時，在單位球面上形成一段圓弧，這兩個向量及圓弧就表示過渡轉(zhuǎn)動的四元數(shù)qm。均勻分配圓弧上的點(diǎn)，則可以得到均勻分配得到一系列過渡轉(zhuǎn)動四元素qmi，與初始姿態(tài)疊加之后，得到一系列均勻的插值姿態(tài)qei。這樣，四元數(shù)在超復(fù)數(shù)空間S3轉(zhuǎn)化為歐氏空間單位球面上的圓弧及其向量，直觀形象，有肋于理解姿態(tài)插值以及合成過程。

圖2

2.3 單位球面上的空間圓弧參數(shù)方程

再結(jié)合本研究的需要，單位球面上的空間圓弧可表示為如下方程。

2.4 矢量法四元數(shù)姿態(tài)插值的步驟

1）由給定的初始姿態(tài)與結(jié)束姿態(tài)得到過渡轉(zhuǎn)動的轉(zhuǎn)軸矢量與轉(zhuǎn)角，相關(guān)方程為。

2）由空間圓弧參數(shù)方程，在旋轉(zhuǎn)圓弧上取一系列均勻的點(diǎn)，相關(guān)方程上述。

3.算例驗(yàn)證

算例：已知工業(yè)機(jī)器人未端執(zhí)行器繞定軸X轉(zhuǎn)30度為初始姿態(tài)，繞定軸Y轉(zhuǎn)45度為結(jié)束姿態(tài)，均勻插值中間姿態(tài)。

根據(jù)上述方法與步驟，配合Matlab軟件進(jìn)行相關(guān)的編程計(jì)算，可求得相關(guān)插值姿態(tài)。

解算過程如下：

2）得到空間圓弧方程：

3）t在區(qū)間內(nèi)按間隔為0.01均勻取100個值，得到空間圓弧上100個點(diǎn)的向量。

4）每個點(diǎn)的向量寫成純四元數(shù)，并與起始點(diǎn)進(jìn)行上述相關(guān)運(yùn)算，最后得到均勻插補(bǔ)姿態(tài)。

圖3為本算例對應(yīng)的插補(bǔ)姿態(tài)圖，從圖中可以看出，姿態(tài)轉(zhuǎn)角平滑，變化均勻，無沖擊，有利于機(jī)械臂使用壽命。圖4為姿態(tài)轉(zhuǎn)角對應(yīng)到繞X/Y/X軸轉(zhuǎn)動的歐拉角，從圖中可以看出繞X/Y/Z軸的轉(zhuǎn)角是線性變化的，各軸轉(zhuǎn)速基本保持定值，勻速平穩(wěn)轉(zhuǎn)動，說明姿態(tài)插值合理可行。

圖3

圖4

4.結(jié)束語

用單位球面上的圓弧及向量表達(dá)四元數(shù)，均勻分配圓弧上的點(diǎn)，通過圓弧上各點(diǎn)向量進(jìn)行姿態(tài)四元數(shù)插值，得到的插值姿態(tài)平穩(wěn)無沖擊，結(jié)果合理可行。該方法比較直接地展現(xiàn)了四元數(shù)插值姿態(tài)的合成過程，有助于進(jìn)一步進(jìn)行機(jī)械手姿態(tài)的相關(guān)研究。