四元數(shù)在工業(yè)機(jī)器人編程操作中的應(yīng)用

程福

（北京城市學(xué)院信息學(xué)部，北京 101300）

1 引言

相對(duì)于旋轉(zhuǎn)矩陣、歐拉角、旋轉(zhuǎn)向量等機(jī)器人姿態(tài)描述方法，四元數(shù)因其具有能夠?qū)崿F(xiàn)平滑插值、避免萬(wàn)向節(jié)死鎖等優(yōu)點(diǎn)，被廣泛應(yīng)用于機(jī)器人末端操作器（工具）的姿態(tài)規(guī)劃中[1-2]。目前廣泛應(yīng)用的基于四元數(shù)的姿態(tài)插補(bǔ)算法主要有球面線性插值 (Spherical Linear Interpolation，SLERP) 和球面立體插值 (Spherical and Quadrangle，SQUAD)[3]。SLERP 算法計(jì)算簡(jiǎn)單，可以高效地實(shí)現(xiàn)兩姿態(tài)間的平滑插值。

由于應(yīng)用四元數(shù)能夠確保工具在三維空間中姿態(tài)變換的平滑性，其在工業(yè)機(jī)器人研發(fā)中得到了深入的研究和廣泛的應(yīng)用，但介紹四元數(shù)應(yīng)用于機(jī)器人編程操作的公開(kāi)文獻(xiàn)并不多見(jiàn)[4-5]。在機(jī)器人的編程操作時(shí)，可以應(yīng)用四元數(shù)實(shí)現(xiàn)工具角位移的精確度和重復(fù)精度的準(zhǔn)確檢測(cè)，也可以進(jìn)行直線運(yùn)動(dòng)軌跡的姿態(tài)規(guī)劃。

2 四元數(shù)用作姿態(tài)描述和旋轉(zhuǎn)變換

2.1 四元數(shù)與歐拉旋轉(zhuǎn)定理

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

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

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

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

工具坐標(biāo)系實(shí)現(xiàn)姿態(tài)變換的旋轉(zhuǎn)軸如圖1 所示，用向量及繞其旋轉(zhuǎn)角度描述坐標(biāo)系在三維空間的姿態(tài)如圖2 所示。如果在關(guān)節(jié)空間圍繞工具中心點(diǎn)（Tool Center Point，TCP）改變工具坐標(biāo)系的姿態(tài)，即使繞世界坐標(biāo)系的X，Y，Z軸進(jìn)行多次旋轉(zhuǎn)，仍然等效于工具坐標(biāo)系繞經(jīng)過(guò)工具中心點(diǎn)TCP 的一個(gè)向量u旋轉(zhuǎn)角度θ實(shí)現(xiàn)姿態(tài)變換。如果工具中心點(diǎn)TCP 的位置不變，編程人員使用直線移動(dòng)指令改變工具的姿態(tài)，大多數(shù)機(jī)器人的控制器會(huì)采用繞經(jīng)過(guò)工具中心點(diǎn)TCP 的一個(gè)向量u旋轉(zhuǎn)角度θ實(shí)現(xiàn)姿態(tài)變換，所應(yīng)用的是基于四元數(shù)的SLERP 軌跡插值法[3]。

圖1 工具坐標(biāo)系實(shí)現(xiàn)姿態(tài)變換的旋轉(zhuǎn)軸

圖2 用向量及繞其旋轉(zhuǎn)角度描述坐標(biāo)系在三維空間的姿態(tài)

2.2 應(yīng)用四元數(shù)描述三維空間的姿態(tài)

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

使向量u分別代表世界坐標(biāo)系的z、y、x軸，則四元數(shù)描述的坐標(biāo)系繞向量u旋轉(zhuǎn)角度θ變換而來(lái)的姿態(tài)分別如圖3a、3b 和3c 所示。

圖3 四元數(shù)描述的坐標(biāo)系繞世界坐標(biāo)系z(mì)、y、x 軸旋轉(zhuǎn)一定角度的姿態(tài)

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

2.3 單位四元數(shù)用作旋轉(zhuǎn)變換

如果Rq是3×3 旋轉(zhuǎn)矩陣，其對(duì)應(yīng)的姿態(tài)用四元數(shù)q來(lái)描述，Rp為另一個(gè)四元數(shù)p對(duì)應(yīng)的3×3 旋轉(zhuǎn)矩陣，那么旋轉(zhuǎn)矩陣RqRp與四元數(shù)qp描述的姿態(tài)相同。由于兩個(gè)3×3 旋轉(zhuǎn)矩陣需要經(jīng)過(guò)27 次乘法和10 次加/減法運(yùn)算，而兩個(gè)四元數(shù)相乘只需要經(jīng)過(guò)16 次乘法和12 次加/減法運(yùn)算，所以應(yīng)用四元數(shù)描述姿態(tài)的優(yōu)點(diǎn)是計(jì)算效率較高。

一個(gè)3×3 旋轉(zhuǎn)矩陣的逆矩陣等于其轉(zhuǎn)置矩陣，即Rq-1=RqT。四元數(shù)的求逆運(yùn)算描述的姿態(tài)變換更為直觀，它是由同一個(gè)坐標(biāo)系繞向量u以-θ角度旋轉(zhuǎn)而 來(lái)，即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}。這說(shuō)明，四元數(shù)與其求逆后的四元數(shù)相乘得到的四元數(shù)描述的姿態(tài)與3×3 單位矩陣姿態(tài)相同。

如圖4 所示，如果純四元數(shù)v={0，vx，vy，vz}描述一個(gè)坐標(biāo)系的姿態(tài)，單位四元數(shù)q作為坐標(biāo)系姿態(tài)變換的算子，那么，四元數(shù)qvq-1描述的姿態(tài)與四元數(shù)v描述的姿態(tài)相同。

圖4 四元數(shù)qvq-1 描述的姿態(tài)v1 與純四元數(shù)v 描述的姿態(tài)v0 相同

應(yīng)用四元數(shù)，可以實(shí)現(xiàn)與應(yīng)用歐拉角法相同的工具坐標(biāo)系的姿態(tài)變換；首先把應(yīng)用歐拉角法實(shí)現(xiàn)姿態(tài)變換的三個(gè)分量角度分別轉(zhuǎn)化為四元數(shù)，然后將這三個(gè)四元數(shù)相乘，依據(jù)四元數(shù)乘法的幾何意義則得到表示旋轉(zhuǎn)的四元數(shù)。

設(shè)歐拉角的三個(gè)旋轉(zhuǎn)角度分量分別為，繞z軸旋轉(zhuǎn)α角，繞y軸旋轉(zhuǎn)β角，繞x軸旋轉(zhuǎn)γ角。這三個(gè)旋轉(zhuǎn)角度分量對(duì)應(yīng)的四元數(shù)為qz，qy，qx，則Q表示最終得到的旋轉(zhuǎn)四元數(shù)：

2.4 單位四元數(shù)與姿態(tài)變換矩陣的轉(zhuǎn)換

對(duì)于單位四元數(shù)，其對(duì)應(yīng)的姿態(tài)變換矩陣為。

3 應(yīng)用四元數(shù)檢測(cè)工具的角位移

等量的歐拉角變化并不一定對(duì)應(yīng)等量的姿態(tài)旋轉(zhuǎn)，對(duì)應(yīng)同一個(gè)姿態(tài)，歐拉角的角度變化量可以有多組，而且姿態(tài)輕微的改變，歐拉角變換很大。所以，應(yīng)用歐拉角很難準(zhǔn)確評(píng)價(jià)實(shí)現(xiàn)姿態(tài)變換的角位移的精確度和重復(fù)精度；應(yīng)用單位四元數(shù)能夠很容易根據(jù)實(shí)現(xiàn)初始姿態(tài)變換到結(jié)束姿態(tài)的角位移，準(zhǔn)確檢測(cè)出角位移的精確度和重復(fù)精度。

如果q是描述工具坐標(biāo)系在姿態(tài)1 時(shí)的四元數(shù)，p是描述工具坐標(biāo)系在姿態(tài)2 時(shí)的四元數(shù)，則工具在姿態(tài)2相對(duì)于姿態(tài)1 時(shí)的姿態(tài)用四元數(shù)描述為

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

圖5 四元數(shù)q 和p 描述的姿態(tài)在世界坐標(biāo)系中的旋轉(zhuǎn)變換

4 應(yīng)用單位四元數(shù)實(shí)現(xiàn)機(jī)器人工具直線運(yùn)動(dòng)軌跡的姿態(tài)規(guī)劃

盡管以逆時(shí)針和順時(shí)針兩個(gè)方向可以實(shí)現(xiàn)等效的姿態(tài)變換，但以小于180°的角度實(shí)現(xiàn)姿態(tài)變換是最快捷的角度變化路徑，也是驅(qū)動(dòng)轉(zhuǎn)矩最小的角度變化路徑。

為了實(shí)現(xiàn)四元數(shù)q描述的姿態(tài)以等角速度平滑變換到四元數(shù)p描述的姿態(tài)，需要應(yīng)用公式（5）描述的球面線性插值SLERP 法得到相等的角度變化量的序列值。

式中，t∈[0,1],t=0 對(duì)應(yīng)四元數(shù)q描述的姿態(tài)，t=1 對(duì)應(yīng)四元數(shù)p描述的姿態(tài)。

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

如果令t=0，1/n，2/n，…，1，那么，球面線性插值法實(shí)質(zhì)上是通過(guò)每次以相等的旋轉(zhuǎn)角度φ/n，實(shí)現(xiàn)旋轉(zhuǎn)角度為φ的姿態(tài)變換。應(yīng)該注意，要確保φ小于180°。式（6）與式（5）等效，但更便于計(jì)算。式（5）中的每一個(gè)t值，對(duì)應(yīng)一個(gè)四元數(shù)。

如果機(jī)器人的工具中心點(diǎn)TCP 位置不變，只是姿態(tài)發(fā)生變化，可以應(yīng)用球面線性插值法以等角速度實(shí)現(xiàn)初始姿態(tài)平滑變換到結(jié)束姿態(tài)，如圖6 所示。

圖6 機(jī)器人的工具中心點(diǎn)位置不變時(shí)姿態(tài)變換過(guò)程

根據(jù)程序6 的計(jì)算出來(lái)的序列姿態(tài)，應(yīng)用工業(yè)機(jī)器人編程軟件RoboDK 編程控制ABB 機(jī)器人實(shí)現(xiàn)工具的姿態(tài)變換過(guò)程如圖7 所示[6]。

圖7 機(jī)器人在工具中心點(diǎn)位置不變時(shí)姿態(tài)變換過(guò)程

如果機(jī)器人的工具中心點(diǎn)TCP 沿直線運(yùn)動(dòng)過(guò)程中不僅姿態(tài)發(fā)生變化，位置也發(fā)生變化。編程人員為了控制工具沿直線運(yùn)動(dòng)過(guò)程中的姿態(tài)和位置變換，可以應(yīng)用球面線性插值（SLERP）法以等角速度實(shí)現(xiàn)工具從初始姿態(tài)平滑變換到結(jié)束姿態(tài)，應(yīng)用式（7）描述的線性插值（LERP）法實(shí)現(xiàn)工具從初始位置經(jīng)過(guò)若干序列點(diǎn)沿直線運(yùn)動(dòng)到結(jié)束位置，如圖8 所示。

圖8 應(yīng)用SLERP 和LERP 在工具沿直線運(yùn)動(dòng)位姿平滑變換

根據(jù)程序7 的計(jì)算出來(lái)的序列位姿，應(yīng)用工業(yè)機(jī)器人編程軟件RoboDK 編程控制ABB 機(jī)器人實(shí)現(xiàn)沿直線運(yùn)動(dòng)時(shí)工具的姿態(tài)和位置變換過(guò)程如圖9 所示。

圖9 機(jī)器人在工具沿直線運(yùn)動(dòng)位姿平滑變換

5 結(jié)束語(yǔ)

盡管絕大多數(shù)工業(yè)機(jī)器人軟件采用歐拉角法描述機(jī)器人工具的姿態(tài)，但在機(jī)器人編程操作中應(yīng)用歐拉角法能夠準(zhǔn)確地檢測(cè)工具角位移的精確度和重復(fù)精度。編程操作人員應(yīng)用四元數(shù)法不僅能夠準(zhǔn)確地檢測(cè)工具角位移的精確度和重復(fù)精度，也可以根據(jù)操作任務(wù)要求（例如，要求以驅(qū)動(dòng)轉(zhuǎn)矩最小的角度變化路徑進(jìn)行姿態(tài)變換）重新進(jìn)行機(jī)械臂工具的姿態(tài)規(guī)劃。