基于單位球面軌跡的四元數(shù)插值算法評價(jià)方法

錢超群，歐 屹，馮虎田，黃俊朋

(南京理工大學(xué)機(jī)械工程學(xué)院，江蘇 南京 210094)

剛體的運(yùn)動軌跡規(guī)劃無論是在計(jì)算機(jī)圖形學(xué)還是在機(jī)器人學(xué)中都是一個重要的問題[1-2]。其中剛體的朝向問題，也即剛體的姿態(tài)規(guī)劃問題是核心問題，其平滑度直接影響動畫的連貫性與運(yùn)動效果的逼真性[3]，以及機(jī)械加工中產(chǎn)品的加工質(zhì)量[4]等。在歷經(jīng)了旋轉(zhuǎn)矩陣法、歐拉角法、軸角法[5-6]后，單位四元數(shù)法憑借其高效簡潔[7-8]、可以避免萬向節(jié)鎖死[9]等優(yōu)點(diǎn)，已逐漸成為計(jì)算機(jī)動畫和機(jī)器人軌跡規(guī)劃領(lǐng)域的首選[10-12]。眾多學(xué)者對四元數(shù)插值算法進(jìn)行了研究，但對插值算法的評價(jià)方法還沒有統(tǒng)一標(biāo)準(zhǔn)，都是基于其研究特點(diǎn)進(jìn)行描述。例如在計(jì)算機(jī)圖形學(xué)中，給出的是3D關(guān)鍵幀插值動畫實(shí)例[13-16]，即將插值結(jié)果圖形平鋪進(jìn)行對比。而在機(jī)器人學(xué)中，常給出其固定在末端執(zhí)行器端點(diǎn)處坐標(biāo)系的旋轉(zhuǎn)插值變換圖[2,17-19]。這樣既不直觀，也不能反映機(jī)器人關(guān)節(jié)空間的軌跡變化。而機(jī)器人的笛卡爾空間軌跡都是由關(guān)節(jié)空間各關(guān)節(jié)角的變化驅(qū)動的。為了既能直觀感受姿態(tài)軌跡變化情況，又能得到相應(yīng)關(guān)節(jié)角的運(yùn)動曲線，本文從單位四元數(shù)與3D旋轉(zhuǎn)的關(guān)系出發(fā)，構(gòu)建了基于單位球面軌跡的四元數(shù)插值算法評價(jià)方法。

1 四元數(shù)及其與3D旋轉(zhuǎn)的關(guān)系

四元數(shù)由英國數(shù)學(xué)家William Rowan Hamilton在1843年發(fā)明，其可以看成是復(fù)數(shù)向四元空間的推廣。與復(fù)數(shù)的區(qū)別是復(fù)數(shù)只有一個虛部，而四元數(shù)有3個虛部。任意一個四元數(shù)q(q∈H)都可以寫成以下的形式：

q=a+bi+cj+dk

(1)

式中：a,b,c,d為實(shí)數(shù)，即a,b,c,d∈R；i,j,k為虛數(shù)單位，即i2=j2=k2=ijk=jki=-1。

此外常用一個三維的向量來表示四元數(shù)的虛部，將其表示為標(biāo)量和向量的有序?qū)π问剑?/p>

q=[s,v]

(2)

其中s=a,v=[bcd]T。當(dāng)s=a=0時(shí)，q被稱為純四元數(shù)。

1.1 四元數(shù)的基本性質(zhì)

因?yàn)樗脑獢?shù)是復(fù)數(shù)的推廣，所以許多性質(zhì)與復(fù)數(shù)類似。其模長公式為：

(3)

其加減法也是對應(yīng)分量相加減。設(shè)q1=[s1,v1],q2=[s2,v2]，則有

q1±q2=[s1±s2,v1±v2]

(4)

其與標(biāo)量的乘法是遵守交換律的。但是兩個四元數(shù)之間的乘法不遵守交換律，即q1q2≠q2q1。其乘法規(guī)則如下：

q1q2=[s1s2-v1·v2,s1v2+s2v1+v1×v2]

(5)

則四元數(shù)的逆的計(jì)算公式為：

(6)

1.2 四元數(shù)與3D旋轉(zhuǎn)

圖1 旋轉(zhuǎn)示意圖 圖2 分解示意圖

由式(2)可將矢量u、v、v′及其分量轉(zhuǎn)化為純四元數(shù)：

(7)

(8)

(9)

令q=cosθ+usinθ，則可將其轉(zhuǎn)化為四元數(shù)形式：

q=cosθ+usinθ=[cosθ,0]+[0,usinθ]=[cosθ,usinθ]

(10)

qq=q2=[cos(2θ),usin(2θ)]

(11)

所以左乘旋轉(zhuǎn)算子q的物理意義就是使得矢量繞旋轉(zhuǎn)軸u旋轉(zhuǎn)角θ，qn表示繞u旋轉(zhuǎn)角度nθ。至此，可以將旋轉(zhuǎn)用四元數(shù)表示為：

(12)

(13)

由式(13)可知，任意一個矢量v繞單位旋轉(zhuǎn)軸u旋轉(zhuǎn)θ角之后的v′可以通過如下的方式獲得。

(14)

2 四元數(shù)的常用插值算法

假設(shè)有兩個旋轉(zhuǎn)變換q0=[cosθ0,sinθ0u0]和q1=[cosθ1,sinθ1u1]，四元數(shù)插值的目的就是找出一些中間變換qt，其中t為比例系數(shù)，t∈[0,1]，使得初始變換q0能夠平滑地過渡到最終變換q1。由于插值的對象是兩個變換，無法具象化，因此不妨假設(shè)3D空間中有一個任意向量v，q0,q1分別將其變換為v0,v1，而qt的作用就是將v0變換到中間位置vt，如圖3所示。

圖3 插值示意圖

從圖中可以看到，這個旋轉(zhuǎn)的變化量依然是一個旋轉(zhuǎn)。這個旋轉(zhuǎn)對應(yīng)一個固定的旋轉(zhuǎn)軸ut，旋轉(zhuǎn)角為θt。不管是哪種插值算法，中間向量vt都可以寫成v0,v1的線性組合，即

vt=αv0+βv1

(15)

式中：α,β為系數(shù)，其值由不同插值算法得到的q1所決定。

此外，-q表示的旋轉(zhuǎn)實(shí)際上與q是同一個旋轉(zhuǎn)，只是作為四維向量，兩者之間相差了π。它們對向量的最終變換效果是一樣的，但是插值時(shí)的路徑不同。如圖4所示，q1與q0之間的夾角為鈍角，其所插值的路徑明顯要比-q1與q0之間的路徑長。

圖4 雙倍覆蓋示意圖

因此本文所有插值算法在對兩個單位四元數(shù)q0,q1插值之前，都先檢測它們之間的夾角是否為鈍角。若q0·q1<0，則將q1改為-q1，然后再進(jìn)行插值，保證插值的路徑最短。

在進(jìn)行實(shí)驗(yàn)的過程中，選取在本院治療糖尿病的患者50例作為本次的實(shí)驗(yàn)對象，在進(jìn)行實(shí)驗(yàn)的過程中，將其分為兩組，各25例，對照組接受社區(qū)糖尿病自我管理模式健康教育；觀察組不接受糖尿病自我管理模式健康教育。

2.1 正規(guī)化線性插值(Nlerp)

(16)

圖5 Nlerp插值示意圖

2.2 球面線性插值(Slerp)

與Nlerp不同，Slerp是對旋轉(zhuǎn)角度θt進(jìn)行線性插值。也就是讓被旋轉(zhuǎn)向量v在球面上的一個弧上旋轉(zhuǎn)，所以被稱為球面線性插值，其示意圖如圖6所示。

圖6 Slerp插值示意圖

依據(jù)圖6進(jìn)行建模，即可將式(15)轉(zhuǎn)換為四元數(shù)的Slerp公式：

(17)

其中q0,q1之間的夾角θ=arccos(q0·q1)。

該算法能保證每兩個四元數(shù)之間插值之后的角速度相同，但當(dāng)需要對多個四元數(shù)進(jìn)行插值時(shí)，角速度會在切換點(diǎn)處出現(xiàn)斷點(diǎn)，即切換點(diǎn)處不可導(dǎo)，插值得到的曲線只是C0連續(xù)。

2.3 球面四邊形插值(Squad)

Squad不再是著眼于兩個四元數(shù)之間的插值，而是考慮全局插值效果，再在兩兩四元數(shù)之間進(jìn)行插值，使得插值曲線為C1連續(xù)的一種全局插值方法。該插值方法是Ken Shoemake提出的基于三次Bézier曲線的插值算法的一種近似優(yōu)化算法，其中兩兩四元數(shù)之間的插值算法還是Slerp。

與三次Bézier曲線插值算法采用嵌套三層一次插值的方法不同，Squad使用的是一層二次插值嵌套了一層一次插值的方式。仍然以被旋轉(zhuǎn)向量v的插值為例，如圖7所示，考慮全局插值效果，在v0和其之前的一個矢量v-1之間采用Slerp算法可得到w0。同理，在v1和其之后的一個矢量v2之間插值得到w1。這兩次插值所用的四元數(shù)分別稱為s0,s1。然后分別在v0,v1之間和w0,w1之間采用Slerp插值得到v01和w02。最后以2t(1-t)為參數(shù)對v01和w01進(jìn)行二次插值，獲得最終的插值向量vt。

圖7 Squad插值示意圖

由上面的分析可得Squad的插值公式為：

Squad(qi,si,si+1,qi+1,t)=Slerp(Slerp(qi,qi+1,t),Slerp(si,si+1,t),2t(1-t))

(18)

si的求解方針是讓插值曲線在切換點(diǎn)處可導(dǎo)，即qi-1qi插值時(shí)在t=1處的導(dǎo)數(shù)與qiqi+1插值時(shí)在t=0處的導(dǎo)數(shù)相同。則由此可推導(dǎo)得：

(19)

3 評價(jià)方法及實(shí)驗(yàn)

由前述可知：任何一個單位四元數(shù)都是表示一種姿態(tài)，如果通過式(14)將其作用于任何一個向量v，也就是讓v做了一個相應(yīng)的旋轉(zhuǎn)?；诖耍环寥∫粋€單位向量v，例如v=[0 1 0]T，將插值過后的四元數(shù)序列分別利用式(14)對其進(jìn)行旋轉(zhuǎn)，則v的端點(diǎn)的運(yùn)動軌跡一直在一個單位球面上。如圖8所示，v1v2分別為v在q1,q2作用下旋轉(zhuǎn)所得的向量。

圖8 評價(jià)方法示意圖

ΔPanθi=atan[2(yi+1,xi+1)]-atan[2(yi,xi)]

(20)

(21)

其中：

任取6個關(guān)鍵姿態(tài)，將其列于表1。其中，每一行表示繞u旋轉(zhuǎn)θ角。先將各旋轉(zhuǎn)軸u單位化，再將每一行利用式(14)轉(zhuǎn)化成四元數(shù)，然后分別利用第3節(jié)所述算法進(jìn)行插值，再將插值結(jié)果利用本文所述方法進(jìn)行表示，如圖9～圖11所示。

表1 關(guān)鍵姿態(tài)表

圖9 Nlerp插值算法結(jié)果

圖10 Slerp插值算法結(jié)果

圖11 Squad插值算法結(jié)果

從圖9～圖11的圖(a)可以直觀地看到插值結(jié)果，且與實(shí)際效果一致，Squad的插值結(jié)果優(yōu)于其他兩種算法。這是因?yàn)橄啾扔谄渌麅煞N算法只關(guān)注兩點(diǎn)之間的插值結(jié)果，Squad是考慮全局軌跡順滑的插值方法。圖11的結(jié)果還證實(shí)了Squad是 連續(xù)的。

4 結(jié)束語

隨著機(jī)器人行業(yè)與計(jì)算機(jī)動畫行業(yè)的快速發(fā)展，人們對姿態(tài)順滑過渡的要求越來越高，對四元數(shù)的插值算法提出了許多新的要求。然而，對其的評價(jià)方法多種多樣，且常常忽略其對應(yīng)的關(guān)節(jié)空間的情況。本文基于四元數(shù)與3D旋轉(zhuǎn)的關(guān)系，引入單位向量v=[0 1 0]T，將插值得到的四元數(shù)序列分別作用于v，得到v的端點(diǎn)的軌跡圖。同時(shí)構(gòu)造實(shí)現(xiàn)該位姿變化的關(guān)節(jié)空間Pan和Tilt角，以其增量變化作為另一個評價(jià)指標(biāo)。相比于傳統(tǒng)的評價(jià)方法，該方法的優(yōu)勢為：1)將四元數(shù)的旋轉(zhuǎn)效果直接用單位球面上的軌跡進(jìn)行描述，非常直觀；2)描述了關(guān)節(jié)空間變化情況，能夠評價(jià)插值算法求得的結(jié)果與關(guān)節(jié)空間的契合度，利于后面的關(guān)節(jié)空間的軌跡規(guī)劃與控制。由此可知，該評價(jià)方法特別適合用于評價(jià)機(jī)器人行業(yè)中姿態(tài)四元數(shù)插值算法的優(yōu)劣。