普亞松，張文斌，藺小軍，郭德偉，閔 潔

(1.紅河學(xué)院 工學(xué)院,云南 蒙自 661199； 2.西北工業(yè)大學(xué) 現(xiàn)代設(shè)計(jì)與集成制造技術(shù)實(shí)驗(yàn)室,陜西 西安 710072)

0 引 言

機(jī)械臂的姿態(tài)規(guī)劃直接影響到機(jī)械臂的工作性能[1]。描述機(jī)械臂末端姿態(tài)的方法主要有歐拉角、旋轉(zhuǎn)矩陣與四元數(shù)法。采用歐拉角時(shí)，當(dāng)?shù)?個(gè)歐拉角接近90°時(shí)，會(huì)出現(xiàn)奇異現(xiàn)象[2]，旋轉(zhuǎn)角會(huì)發(fā)生突變，稱為“萬向節(jié)鎖死”現(xiàn)象。旋轉(zhuǎn)矩陣需要用9個(gè)元素描述姿態(tài)，運(yùn)算效率低且不便于插值。四元數(shù)不存在“萬向節(jié)鎖死”的現(xiàn)象，運(yùn)算效率高，且便于插值，因此四元數(shù)被廣泛應(yīng)用于描述旋轉(zhuǎn)姿態(tài)。

機(jī)械臂要完成某項(xiàng)工作任務(wù)，有時(shí)需要經(jīng)歷某幾個(gè)關(guān)鍵的有限姿態(tài)Q1,Q2,…,Qn，也即機(jī)械臂的末端執(zhí)行器在某個(gè)時(shí)刻需要呈現(xiàn)規(guī)定的工作角度α1,α2,…,αn。除了這些規(guī)定的關(guān)鍵姿態(tài)之外，并沒有規(guī)定機(jī)械臂的工作姿態(tài)應(yīng)該呈現(xiàn)什么角度，因此需要在各關(guān)鍵姿態(tài)之間進(jìn)行合理的插值，確定中間姿態(tài)，即機(jī)械臂姿態(tài)規(guī)劃。因其特有的優(yōu)勢，四元數(shù)常被用于機(jī)械臂的姿態(tài)規(guī)劃[3-5]。

判斷四元數(shù)姿態(tài)插值是否平滑的方法有求導(dǎo)法及圖形法。四元數(shù)的求導(dǎo)比一般函數(shù)的難度更大，且有些四元數(shù)插值算法本身就比較復(fù)雜，給求導(dǎo)帶來較大的困難[6]。相對而言，圖形法更加簡單、直觀、方便。然而四元數(shù)是四維向量，不能直接在三維空間中直觀地表達(dá)姿態(tài)的插值曲線。為了便于觀察插值曲線是否平滑，需要研究將四元數(shù)插值點(diǎn)映射到三維空間。

Hopf映射是一個(gè)由四維空間的超球面S3到三維空間的球面S2的連續(xù)滿射，它由Heniz Hopf發(fā)現(xiàn)。Hopf映射及其拓展理論在工程領(lǐng)域有廣泛的應(yīng)用，例如應(yīng)用于多參數(shù)非線性控制系統(tǒng)[7]、碰撞振動(dòng)系統(tǒng)分析[8]等。降維映射是Hopf映射的特征之一，應(yīng)用Hopf映射可以把四維向量降維映射到三維空間，可運(yùn)用于四元數(shù)插值，并繪制插值曲線。

筆者提出一種運(yùn)用四元數(shù)插值曲線判斷插值平滑性的方法，并運(yùn)用相關(guān)算例對該方法的可行性進(jìn)行驗(yàn)證。

1 機(jī)器臂姿態(tài)四元數(shù)路徑規(guī)劃

1.1 四元數(shù)

四元數(shù)Q的定義如下：

Q=w+xi+yj+zk

(1)

式中：w—四元數(shù)的實(shí)部，為任意實(shí)數(shù)；x,y,z—四元數(shù)的虛部，均為任意實(shí)數(shù)；i,j,k—復(fù)數(shù)單位。

當(dāng)四元數(shù)的實(shí)部為0時(shí)，稱為純四元數(shù)。四元數(shù)也可寫為向量的形式：

Q=(w,x,y,z)=(w,u)

(2)

式中：u—虛部向量，u=(x,y,z)。

共軛四元數(shù)Q*的定義為：

Q*=w-xi-yj-zk

(3)

一對共軛四元數(shù)，實(shí)部相同，虛部互為相反數(shù)。

四元數(shù)的?！琎‖定義為：

(4)

當(dāng)四元數(shù)的模等于1時(shí)，也即‖Q‖=1，稱為單位四元數(shù)。一般情況下，用于表達(dá)物體旋轉(zhuǎn)姿態(tài)的四元數(shù)都是單位四元數(shù)。

四元數(shù)的逆Q-1定義為：

Q-1=Q*/Q2

(5)

因?yàn)閱挝凰脑獢?shù)的模等于1，所以單位四元數(shù)的逆等于其共軛，即Q-1=Q*。

四元數(shù)還可以寫成旋轉(zhuǎn)角與旋轉(zhuǎn)軸的關(guān)系：

Q=[cos(φ/2),sin(φ/2)*(a,b,c)]

(6)

式中：φ—旋轉(zhuǎn)角；(a,b,c)—轉(zhuǎn)軸矢量，轉(zhuǎn)軸矢量需要轉(zhuǎn)換為單位向量。

四元數(shù)描述旋轉(zhuǎn)的物理意義：剛體內(nèi)某一質(zhì)點(diǎn)位置為v0，剛體經(jīng)過姿態(tài)旋轉(zhuǎn)Q之后，該質(zhì)點(diǎn)的在原坐標(biāo)系中的位置發(fā)生變化，即：

v1=Qv0Q-1

(7)

式中：v0—質(zhì)點(diǎn)轉(zhuǎn)動(dòng)前的位置，屬于純四元數(shù)；v1—質(zhì)點(diǎn)旋轉(zhuǎn)后的位置，屬于純四元數(shù)；Q—轉(zhuǎn)動(dòng)四元數(shù)；Q-1—轉(zhuǎn)動(dòng)四元數(shù)的逆。

1.2 四元數(shù)插值法

機(jī)器臂姿態(tài)四元數(shù)路徑規(guī)劃常用的方法有：球面線性插值法(SLERP)和球面立體插值法(SQUAD)。

球面線性插值法主要適合于兩個(gè)姿態(tài)間的四元數(shù)插值，表達(dá)式為[9]：

(8)

式中：Q1—第一個(gè)關(guān)鍵姿態(tài)；Q2—第二個(gè)關(guān)鍵姿態(tài)；θ—向量Q1與Q2的夾角，cosθ=Q1·Q2，其中Q1·Q2是兩個(gè)向量的點(diǎn)乘；t—插值變量，在[0,1]范圍內(nèi)取值，可以得到一系列的插值四元數(shù)。

球面立體插值法是球面線性插值法的多次運(yùn)用，適合于多個(gè)姿態(tài)間的四元數(shù)插值，表達(dá)式為[10]：

(9)

式中：Qi—某個(gè)插值姿態(tài)；Qi+1—下一個(gè)插值姿態(tài)，Q0,Q1,…,Qn-1是若干個(gè)關(guān)鍵姿態(tài)；Si—某個(gè)輔助四元數(shù)；Si+1—下一個(gè)輔助四元數(shù)，S0,S1,…,Sn-1是若干個(gè)輔助四元數(shù)；h—插值變量，在[0,1]范圍內(nèi)取值，可以得到一系列的插值四元數(shù)。

要進(jìn)行插值運(yùn)算，需先求出輔助四元數(shù)S0,S1,…,Sn-1，其求取輔助四元數(shù)方法如式(9)的第2個(gè)式子。

在求第一個(gè)輔助四元數(shù)S0時(shí)，需要使用到Q-1。但因?yàn)椴淮嬖赒-1，所以規(guī)定第一個(gè)輔助四元數(shù)等于第一個(gè)關(guān)鍵姿態(tài)，即S0=Q0。同理，規(guī)定最后一個(gè)輔助四元數(shù)等于最后一個(gè)關(guān)鍵姿態(tài)，即Sn-1=Qn-1。

判斷四元數(shù)插值是否平滑，需要對插值函數(shù)進(jìn)行求導(dǎo)。但是可以看出，在式(9)對變量h求導(dǎo)時(shí)，不易求出其導(dǎo)數(shù)，可以采用圖形法繪制插值曲線，觀察插值曲線是否平滑。

繪制四元數(shù)插值曲線核心的問題即怎樣把四維向量轉(zhuǎn)化為三維向量，在三維空間中表達(dá)出來。

2 Hopf映射在機(jī)械臂姿態(tài)四元數(shù)規(guī)劃中的應(yīng)用

2.1 Hopf映射與四元數(shù)插值曲線

存在任意的三維向量(X,Y,Z)，向量各元素均為實(shí)數(shù)，且滿足X2+Y2+Z2=1，這樣的三維向量組成的向量空間定義為S2域。S2域是三維向量空間的特殊子集，是半徑等于1的三維單位球表面上的點(diǎn)的集合。

Hopf映射是S3域到S2域的映射，表達(dá)式為[11]：

(10)

式中：(q0,q1,q2,q3)—S3域的元素；(X,Y,Z)—S2域的元素。

從式(10)可以看出，通過Hopf映射，四維單位球面上的四維向量(q0,q1,q2,q3)經(jīng)過降維，可以映射成為三維單位球面上的三維向量(X,Y,Z)。

用于描述轉(zhuǎn)動(dòng)的四元數(shù)通常情況下都是單位四元數(shù)，單位四元數(shù)的模為1，即單位四元數(shù)向量各元素的平方和等于1，滿足屬于S3域的條件，因此單位四元數(shù)是四維單位球面上的點(diǎn)，即Q∈S3。所以通過Hopf映射可以把單位四元數(shù)插值點(diǎn)映射到三維球面上，并繪制三維曲線，從而判斷四元數(shù)插值曲線是否平滑。

2.2 基于構(gòu)造曲線的四元數(shù)插值曲線

有關(guān)學(xué)者提出一種剛體姿態(tài)變換的四元數(shù)轉(zhuǎn)換為單位球面三維矢量的插值曲線，即四元數(shù)姿態(tài)變換曲線，即[12-13]：

(11)

三維空間構(gòu)造的函數(shù)對應(yīng)的矢量都認(rèn)作實(shí)部為0的純四元數(shù)。

四元數(shù)構(gòu)造曲線如圖1所示。

圖1 四元數(shù)插值構(gòu)造曲線

對單位四元數(shù)姿態(tài)曲線的構(gòu)造等價(jià)為對歐氏空間中，單位球面三維矢量在單位球面上滑動(dòng)所構(gòu)成的曲線函數(shù)為s(t)。

可以用插值四元數(shù)反求三維空間的構(gòu)造曲線s(t)，即：

(12)

將式(12)進(jìn)一步展開，則三維空間的構(gòu)造曲線s(t)更加具體的表達(dá)式如下：

(13)

式中：(a0,a1,a2,a3)—初始姿態(tài)四元數(shù)，也即P0；(b0,b1,b2,b3)—姿態(tài)插值四元數(shù)，也即P(t)；(0,c1,c2,c3)—三維構(gòu)造曲線的初始點(diǎn)矢量，也即s0；(W,X,Y,Z)—構(gòu)造曲線s(t)的位置向量。

從式(13)中可以看出：相對于Hopf映射方法，該方法更復(fù)雜、運(yùn)算效率更低；且要使構(gòu)造曲線s(t)成為三維矢量，式中的W必須為0，那么構(gòu)造曲線的初始點(diǎn)矢量s0不能隨意取值，必須與姿態(tài)變換量Q(t)的轉(zhuǎn)軸矢量nt正交，即初始值s0的取值要限制在過球心的nt的垂平面與單位球面相交的截圓H上，增加額外的計(jì)算量。

實(shí)際上，該方法更適合于兩姿態(tài)之間四元數(shù)插值曲線的繪制?？梢?，筆者提出的Hopf映射繪制四元數(shù)插值曲線的方法更簡潔實(shí)用。

3 仿真實(shí)驗(yàn)及結(jié)果分析

為了驗(yàn)證Hopf映射在機(jī)械臂姿態(tài)四元數(shù)規(guī)劃中的可行性，筆者通過算例來進(jìn)行分析說明。

算例：機(jī)械臂要完成某個(gè)焊接任務(wù)，要經(jīng)歷3個(gè)關(guān)鍵姿態(tài)，初始姿態(tài)為繞固定坐標(biāo)系X軸轉(zhuǎn)45°，第2個(gè)姿態(tài)為繞固定坐標(biāo)系Z軸轉(zhuǎn)60°，最后姿態(tài)為固定坐標(biāo)系下繞旋轉(zhuǎn)軸n轉(zhuǎn)15°，轉(zhuǎn)軸n的矢量為(1,2,3)，對這3個(gè)關(guān)鍵姿態(tài)進(jìn)行四元數(shù)姿態(tài)平滑插值。

(1)采用球面線性插值法(SLERP)進(jìn)行機(jī)械臂姿態(tài)四元數(shù)插值：

第一步：把3個(gè)關(guān)鍵姿態(tài)轉(zhuǎn)化為四元數(shù)，其中第3個(gè)關(guān)鍵姿態(tài)轉(zhuǎn)化為四元數(shù)之前，需要把轉(zhuǎn)軸矢量轉(zhuǎn)化為單位矢量。根據(jù)式(6)，這3個(gè)關(guān)鍵姿態(tài)的四元數(shù)分別為：Q1=(0.924,0.383,0,0)，Q2=(0.866,0,0,0.5)，Q3=(0.991,0.035,0.07,0.105)；

第二步：根據(jù)式(8)，采用球面線性插值法在Q1與Q2、Q2與Q3姿態(tài)之間進(jìn)行四元數(shù)插值；

第三步：根據(jù)式(10)，采用Hopf映射，把四元數(shù)插值點(diǎn)轉(zhuǎn)化為三維球面上的三維向量；

第四步：在三維球面上繪制四元數(shù)插值曲線，根據(jù)插值曲線的平滑性判斷四元數(shù)插值是否平滑。

SLERP四元數(shù)插值及Hopf映射如表1所示。

表1 SLERP四元數(shù)插值及Hopf映射(部分)

(2)采用球面立體插值法進(jìn)行機(jī)械臂姿態(tài)四元數(shù)插值：

第一步：3個(gè)關(guān)鍵點(diǎn)的姿態(tài)轉(zhuǎn)化為四元數(shù)，結(jié)果如前所述；

第二步：采用SQUAD進(jìn)行四元數(shù)插值時(shí)，需要求出關(guān)鍵姿態(tài)的輔助四元數(shù)。根據(jù)式(9)，求得輔助四元數(shù)如下：S1=(0.924,0.383,0,0)，S2=(0.724,-0.108,-0.008,0.681)，S3=(0.991,0.035,0.07,0.105)；

第三步：再根據(jù)式(9)，采用球面立體插值法在Q1與Q2、Q2與Q3姿態(tài)之間進(jìn)行四元數(shù)插值；

第四步：根據(jù)公式(10)，采用Hopf映射，把四元數(shù)插值點(diǎn)轉(zhuǎn)化為三維球面上的三維向量；

第五步：在三維球面上繪制四元數(shù)插值曲線，根據(jù)插值曲線的平滑性判斷四元數(shù)插值是否平滑。

SQUAD四元數(shù)插值及Hopf映射如表2所示。

(3)結(jié)果分析：

本算例采用SLERP法和SQUAD法得到的四元數(shù)插值點(diǎn)，經(jīng)過Hopf映射可以轉(zhuǎn)化成各自的三維矢量，因此可以在三維空間繪制SLERP法和SQUAD法的四元數(shù)插值曲線，如圖2所示。

表2 SQUAD四元數(shù)插值及Hopf映射(部分)

從圖2可以看出：(1)SLERP方法得到的兩段插值曲線，各段曲線都是平滑的曲線，但在兩段插值曲線的連接點(diǎn)K處有明顯尖銳點(diǎn)的出現(xiàn)，說明整個(gè)插值曲線是不平滑的，因此SLERP不適合用于兩個(gè)以上的姿態(tài)插值；(2)SQUAD方法得到的兩段插值曲線，每段曲線都是平滑曲線，并且兩段插值曲線的連接點(diǎn)K處是平滑過渡的。因此，相對于SLERP法，SQUAD法更適合用于兩個(gè)以上的多姿態(tài)四元數(shù)插值。

下面使用相應(yīng)的插值角速度曲線圖來驗(yàn)證上述結(jié)論的正確性。姿態(tài)規(guī)劃插值角速度曲線如圖3所示。

圖3 姿態(tài)規(guī)劃插值角速度曲線

4 結(jié)束語

對比常用機(jī)械臂姿態(tài)四元數(shù)路徑規(guī)劃方法的基礎(chǔ)上，筆者提出了Hopf映射繪制四元數(shù)插值曲線的方法，并通過算例對Hopf映射在機(jī)械臂姿態(tài)四元數(shù)規(guī)劃中的可行性與正確性進(jìn)行了驗(yàn)證，得到結(jié)果如下：

(1)Hopf映射繪制四元數(shù)插值曲線的方法避免了復(fù)雜四元數(shù)插值函數(shù)的求導(dǎo)過程，能夠判斷四元數(shù)插值是否平滑，操作簡便、實(shí)用；

(2)SQUAD插值方法比SLERP更適合于兩個(gè)以上的多姿態(tài)插值。