四元數在工業機器人編程操作中的應用

程福

（北京城市學院信息學部，北京 101300）

1 引言

相對于旋轉矩陣、歐拉角、旋轉向量等機器人姿態描述方法，四元數因其具有能夠實現平滑插值、避免萬向節死鎖等優點，被廣泛應用于機器人末端操作器（工具）的姿態規劃中[1-2]。目前廣泛應用的基于四元數的姿態插補算法主要有球面線性插值 (Spherical Linear Interpolation，SLERP) 和球面立體插值 (Spherical and Quadrangle，SQUAD)[3]。SLERP 算法計算簡單，可以高效地實現兩姿態間的平滑插值。

由于應用四元數能夠確保工具在三維空間中姿態變換的平滑性，其在工業機器人研發中得到了深入的研究和廣泛的應用，但介紹四元數應用于機器人編程操作的公開文獻并不多見[4-5]。在機器人的編程操作時，可以應用四元數實現工具角位移的精確度和重復精度的準確檢測，也可以進行直線運動軌跡的姿態規劃。

2 四元數用作姿態描述和旋轉變換

2.1 四元數與歐拉旋轉定理

四元數是對復數的擴展。四元數實質上是實數、復數及三維空間向量的擴充，包含一個實部和三個虛部。一個復數表達式為a+bi。其中，a和b都是實數，i是單位虛數，i2=-1。一個四元數表達式：q=q1+q2i+q3j+q4k，其中i2=j2=k2=-1，ij=k=-ji，jk=i=-kj，ki=j=-ki，{q1，q2，q3，q4}都是實數，表示四元數的四個組成元素。

四元數的加法運算：如果p={p1，p2，p3，p4} 表示另一個四元數，則兩個四元數加法運算：q+p=p+q=(q1+p1)+(q2+p2)i+(q3+p3)j+(q4+p4)k；四元數的乘法運算：qp=(q1p1–q2p2–q3p3–q4p4)+(q1p2+q2p1+q3p4–q4p3)i+(q1p3+q3p1–q2p4+q4p2)j+(q1p4+q4p1+q2p3–q3p2)k。

四元數的加法滿足結合律和交換律。但是，四元數的乘法和矩陣的乘法相類似，只滿足結合律，不滿足交換律，即qp≠pq。

歐拉旋轉定理指出，對于空間中任意姿態的坐標系，總可以在空間里找到某個軸，使其中一個坐標系繞軸旋轉一個角度就能與另一個坐標系姿態重合。所以，坐標系在三維空間中的姿態可以用一個代表旋轉軸的向量u和繞向量u旋轉的角度θ來描述，向量u的方向遵循右手螺旋法則。如果用u與θ描述一個坐標系的姿態，那么-u與-θ，以及-u與 360°-θ分別描述的坐標系的姿態與該坐標系的姿態相同。

工具坐標系實現姿態變換的旋轉軸如圖1 所示，用向量及繞其旋轉角度描述坐標系在三維空間的姿態如圖2 所示。如果在關節空間圍繞工具中心點（Tool Center Point，TCP）改變工具坐標系的姿態，即使繞世界坐標系的X，Y，Z軸進行多次旋轉，仍然等效于工具坐標系繞經過工具中心點TCP 的一個向量u旋轉角度θ實現姿態變換。如果工具中心點TCP 的位置不變，編程人員使用直線移動指令改變工具的姿態，大多數機器人的控制器會采用繞經過工具中心點TCP 的一個向量u旋轉角度θ實現姿態變換，所應用的是基于四元數的SLERP 軌跡插值法[3]。

圖1 工具坐標系實現姿態變換的旋轉軸

圖2 用向量及繞其旋轉角度描述坐標系在三維空間的姿態

2.2 應用四元數描述三維空間的姿態

對于單位四元數q，|q|=q12+q22+q32+q42=1，q可以描述為q={cos(θ/2)，uxsin(θ/2)，uysin(θ/2)，uzsin(θ/2)}，其中，ux，uy，uz是向量u三個互相垂直的主軸的分量，θ是繞向量u旋轉的角度。q所描述的姿態是由于世界坐標系姿態相同的四元數描述的坐標系繞以向量u為軸旋轉角度θ變換而來。所以，僅用u和θ就可以描述坐標系在三維空間的姿態。

使向量u分別代表世界坐標系的z、y、x軸，則四元數描述的坐標系繞向量u旋轉角度θ變換而來的姿態分別如圖3a、3b 和3c 所示。

圖3 四元數描述的坐標系繞世界坐標系z、y、x 軸旋轉一定角度的姿態

由于同一個四元數描述的姿態與用繞向量-u旋轉360°-θ角度的姿態相同，即q={cos(180°-θ/2)，-uxsin(180 °-θ/2)，-uysin(180 °-θ/2)，-uzsin(180 °-θ/2)}={-cos(θ/2)，-uxsin(θ/2)，-uysin(θ/2)，-uzsin(θ/2))=-q。所以，一個四元數與其四個組成元素取負值后的四元數，它們描述的是同一個姿態。

2.3 單位四元數用作旋轉變換

如果Rq是3×3 旋轉矩陣，其對應的姿態用四元數q來描述，Rp為另一個四元數p對應的3×3 旋轉矩陣，那么旋轉矩陣RqRp與四元數qp描述的姿態相同。由于兩個3×3 旋轉矩陣需要經過27 次乘法和10 次加/減法運算，而兩個四元數相乘只需要經過16 次乘法和12 次加/減法運算，所以應用四元數描述姿態的優點是計算效率較高。

一個3×3 旋轉矩陣的逆矩陣等于其轉置矩陣，即Rq-1=RqT。四元數的求逆運算描述的姿態變換更為直觀，它是由同一個坐標系繞向量u以-θ角度旋轉而 來，即q-1={cos(-θ/2)，uxsin(-θ/2)，uysin(-θ/2)，uzsin(-θ/2)}={cos(θ/2)，-uxsin(θ/2)，-uysin(θ/2)，-uzsin(θ/2)}。因此，可以得到q-1q=qq-1={1，0，0，0}。這說明，四元數與其求逆后的四元數相乘得到的四元數描述的姿態與3×3 單位矩陣姿態相同。

如圖4 所示，如果純四元數v={0，vx，vy，vz}描述一個坐標系的姿態，單位四元數q作為坐標系姿態變換的算子，那么，四元數qvq-1描述的姿態與四元數v描述的姿態相同。

圖4 四元數qvq-1 描述的姿態v1 與純四元數v 描述的姿態v0 相同

應用四元數，可以實現與應用歐拉角法相同的工具坐標系的姿態變換；首先把應用歐拉角法實現姿態變換的三個分量角度分別轉化為四元數，然后將這三個四元數相乘，依據四元數乘法的幾何意義則得到表示旋轉的四元數。

設歐拉角的三個旋轉角度分量分別為，繞z軸旋轉α角，繞y軸旋轉β角，繞x軸旋轉γ角。這三個旋轉角度分量對應的四元數為qz，qy，qx，則Q表示最終得到的旋轉四元數：

2.4 單位四元數與姿態變換矩陣的轉換

對于單位四元數，其對應的姿態變換矩陣為。

3 應用四元數檢測工具的角位移

等量的歐拉角變化并不一定對應等量的姿態旋轉，對應同一個姿態，歐拉角的角度變化量可以有多組，而且姿態輕微的改變，歐拉角變換很大。所以，應用歐拉角很難準確評價實現姿態變換的角位移的精確度和重復精度；應用單位四元數能夠很容易根據實現初始姿態變換到結束姿態的角位移，準確檢測出角位移的精確度和重復精度。

如果q是描述工具坐標系在姿態1 時的四元數，p是描述工具坐標系在姿態2 時的四元數，則工具在姿態2相對于姿態1 時的姿態用四元數描述為

式中，wx，wy，wz是單位向量w的分量。公式（1）表明，工具從姿態q變換到姿態p，需要繞向量w旋轉角度φ，如圖5 所示。為了實現以最小角度的旋轉，限定旋轉角度范圍為0<φ≤180，因此，如果d的第一個單元為負數，則計算角度φ時需要將d=—d，即cos(φ/2)，以確保總是取d的第一個單元數的正值來計算角度φ。由公式cos(φ/2)=可以求出旋轉角度φ。

圖5 四元數q 和p 描述的姿態在世界坐標系中的旋轉變換

4 應用單位四元數實現機器人工具直線運動軌跡的姿態規劃

盡管以逆時針和順時針兩個方向可以實現等效的姿態變換，但以小于180°的角度實現姿態變換是最快捷的角度變化路徑，也是驅動轉矩最小的角度變化路徑。

為了實現四元數q描述的姿態以等角速度平滑變換到四元數p描述的姿態，需要應用公式（5）描述的球面線性插值SLERP 法得到相等的角度變化量的序列值。

式中，t∈[0,1],t=0 對應四元數q描述的姿態，t=1 對應四元數p描述的姿態。

根據前述分析，d=q-1p={cos(φ/2)，wxsin(φ/2)，wysin(-φ/2)，wzsin(-φ/2)}，球面線性插值法的公式可以視為實現四元數q描述的姿態以等角速度平滑變換到四元數p 描述的姿態，需要以角度φ旋轉次。所以，dt={cos(tφ/2)，wzsin(tφ/2)，wysin(-tφ/2)，wzsin(-tφ/2)}。

如果令t=0，1/n，2/n，…，1，那么，球面線性插值法實質上是通過每次以相等的旋轉角度φ/n，實現旋轉角度為φ的姿態變換。應該注意，要確保φ小于180°。式（6）與式（5）等效，但更便于計算。式（5）中的每一個t值，對應一個四元數。

如果機器人的工具中心點TCP 位置不變，只是姿態發生變化，可以應用球面線性插值法以等角速度實現初始姿態平滑變換到結束姿態，如圖6 所示。

圖6 機器人的工具中心點位置不變時姿態變換過程

根據程序6 的計算出來的序列姿態，應用工業機器人編程軟件RoboDK 編程控制ABB 機器人實現工具的姿態變換過程如圖7 所示[6]。

圖7 機器人在工具中心點位置不變時姿態變換過程

如果機器人的工具中心點TCP 沿直線運動過程中不僅姿態發生變化，位置也發生變化。編程人員為了控制工具沿直線運動過程中的姿態和位置變換，可以應用球面線性插值（SLERP）法以等角速度實現工具從初始姿態平滑變換到結束姿態，應用式（7）描述的線性插值（LERP）法實現工具從初始位置經過若干序列點沿直線運動到結束位置，如圖8 所示。

圖8 應用SLERP 和LERP 在工具沿直線運動位姿平滑變換

根據程序7 的計算出來的序列位姿，應用工業機器人編程軟件RoboDK 編程控制ABB 機器人實現沿直線運動時工具的姿態和位置變換過程如圖9 所示。

圖9 機器人在工具沿直線運動位姿平滑變換

5 結束語

盡管絕大多數工業機器人軟件采用歐拉角法描述機器人工具的姿態，但在機器人編程操作中應用歐拉角法能夠準確地檢測工具角位移的精確度和重復精度。編程操作人員應用四元數法不僅能夠準確地檢測工具角位移的精確度和重復精度，也可以根據操作任務要求（例如，要求以驅動轉矩最小的角度變化路徑進行姿態變換）重新進行機械臂工具的姿態規劃。