Hopf映射在機械臂四元數姿態規劃中的應用研究*

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

(1.紅河學院 工學院,云南 蒙自 661199； 2.西北工業大學 現代設計與集成制造技術實驗室,陜西 西安 710072)

0 引 言

機械臂的姿態規劃直接影響到機械臂的工作性能[1]。描述機械臂末端姿態的方法主要有歐拉角、旋轉矩陣與四元數法。采用歐拉角時，當第2個歐拉角接近90°時，會出現奇異現象[2]，旋轉角會發生突變，稱為“萬向節鎖死”現象。旋轉矩陣需要用9個元素描述姿態，運算效率低且不便于插值。四元數不存在“萬向節鎖死”的現象，運算效率高，且便于插值，因此四元數被廣泛應用于描述旋轉姿態。

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

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

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

筆者提出一種運用四元數插值曲線判斷插值平滑性的方法，并運用相關算例對該方法的可行性進行驗證。

1 機器臂姿態四元數路徑規劃

1.1 四元數

四元數Q的定義如下：

Q=w+xi+yj+zk

(1)

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

當四元數的實部為0時，稱為純四元數。四元數也可寫為向量的形式：

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

(2)

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

共軛四元數Q*的定義為：

Q*=w-xi-yj-zk

(3)

一對共軛四元數，實部相同，虛部互為相反數。

四元數的模‖Q‖定義為：

(4)

當四元數的模等于1時，也即‖Q‖=1，稱為單位四元數。一般情況下，用于表達物體旋轉姿態的四元數都是單位四元數。

四元數的逆Q-1定義為：

Q-1=Q*/Q2

(5)

因為單位四元數的模等于1，所以單位四元數的逆等于其共軛，即Q-1=Q*。

四元數還可以寫成旋轉角與旋轉軸的關系：

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

(6)

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

四元數描述旋轉的物理意義：剛體內某一質點位置為v0，剛體經過姿態旋轉Q之后，該質點的在原坐標系中的位置發生變化，即：

v1=Qv0Q-1

(7)

式中：v0—質點轉動前的位置，屬于純四元數；v1—質點旋轉后的位置，屬于純四元數；Q—轉動四元數；Q-1—轉動四元數的逆。

1.2 四元數插值法

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

球面線性插值法主要適合于兩個姿態間的四元數插值，表達式為[9]：

(8)

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

球面立體插值法是球面線性插值法的多次運用，適合于多個姿態間的四元數插值，表達式為[10]：

(9)

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

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

在求第一個輔助四元數S0時，需要使用到Q-1。但因為不存在Q-1，所以規定第一個輔助四元數等于第一個關鍵姿態，即S0=Q0。同理，規定最后一個輔助四元數等于最后一個關鍵姿態，即Sn-1=Qn-1。

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

繪制四元數插值曲線核心的問題即怎樣把四維向量轉化為三維向量，在三維空間中表達出來。

2 Hopf映射在機械臂姿態四元數規劃中的應用

2.1 Hopf映射與四元數插值曲線

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

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

(10)

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

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

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

2.2 基于構造曲線的四元數插值曲線

有關學者提出一種剛體姿態變換的四元數轉換為單位球面三維矢量的插值曲線，即四元數姿態變換曲線，即[12-13]：

(11)

三維空間構造的函數對應的矢量都認作實部為0的純四元數。

四元數構造曲線如圖1所示。

圖1 四元數插值構造曲線

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

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

(12)

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

(13)

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

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

實際上，該方法更適合于兩姿態之間四元數插值曲線的繪制。可見，筆者提出的Hopf映射繪制四元數插值曲線的方法更簡潔實用。

3 仿真實驗及結果分析

為了驗證Hopf映射在機械臂姿態四元數規劃中的可行性，筆者通過算例來進行分析說明。

算例：機械臂要完成某個焊接任務，要經歷3個關鍵姿態，初始姿態為繞固定坐標系X軸轉45°，第2個姿態為繞固定坐標系Z軸轉60°，最后姿態為固定坐標系下繞旋轉軸n轉15°，轉軸n的矢量為(1,2,3)，對這3個關鍵姿態進行四元數姿態平滑插值。

(1)采用球面線性插值法(SLERP)進行機械臂姿態四元數插值：

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

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

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

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

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

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

(2)采用球面立體插值法進行機械臂姿態四元數插值：

第一步：3個關鍵點的姿態轉化為四元數，結果如前所述；

第二步：采用SQUAD進行四元數插值時，需要求出關鍵姿態的輔助四元數。根據式(9)，求得輔助四元數如下：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)；

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

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

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

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

(3)結果分析：

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

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

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

下面使用相應的插值角速度曲線圖來驗證上述結論的正確性。姿態規劃插值角速度曲線如圖3所示。

圖3 姿態規劃插值角速度曲線

4 結束語

對比常用機械臂姿態四元數路徑規劃方法的基礎上，筆者提出了Hopf映射繪制四元數插值曲線的方法，并通過算例對Hopf映射在機械臂姿態四元數規劃中的可行性與正確性進行了驗證，得到結果如下：

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

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