基于運(yùn)動(dòng)微分方程與加速度的剛體運(yùn)動(dòng)姿態(tài)算法

張 楠，王 智，王德倫

（大連理工大學(xué)機(jī)械工程學(xué)院，遼寧 大連116024）

1 引言

在精密裝備的運(yùn)動(dòng)精度測(cè)量過程中，被測(cè)對(duì)象的運(yùn)動(dòng)通常被視為剛體的一般空間運(yùn)動(dòng)，并被分解為隨運(yùn)動(dòng)參考點(diǎn)的三向平移與繞參考點(diǎn)的三向轉(zhuǎn)動(dòng)。其中三向平移參數(shù)的描述及測(cè)量相對(duì)簡(jiǎn)單，如點(diǎn)的位移、速度、加速度等，而三向轉(zhuǎn)動(dòng)（即姿態(tài)）的描述及測(cè)量則較為困難。

現(xiàn)常用的剛體運(yùn)動(dòng)測(cè)量方法有以計(jì)算機(jī)視覺為核心的光電視覺運(yùn)動(dòng)測(cè)量［1］、基于激光跟蹤儀的六自由度運(yùn)動(dòng)測(cè)量［2，3］等，前者采用高分辨率的光電成像系統(tǒng)，需結(jié)合其他輔助光電信息，且測(cè)量精度往往達(dá)不到理想的要求，后者測(cè)量準(zhǔn)確，但安裝耗時(shí)且只能實(shí)現(xiàn)準(zhǔn)靜態(tài)測(cè)量。還有以加速度計(jì)和陀螺儀組成慣性測(cè)量單元進(jìn)行傾角計(jì)算［4］的方法，但積分運(yùn)算往往導(dǎo)致測(cè)量誤差隨時(shí)間累積，測(cè)量結(jié)果發(fā)散。

由于點(diǎn)的運(yùn)動(dòng)測(cè)量相對(duì)簡(jiǎn)單，在很多場(chǎng)合，經(jīng)常通過測(cè)量運(yùn)動(dòng)構(gòu)件上點(diǎn)的運(yùn)動(dòng)參數(shù)間接得到運(yùn)動(dòng)構(gòu)件的姿態(tài)參數(shù)，如機(jī)器人末端姿態(tài)的跟蹤測(cè)量［5］、機(jī)床的多線法測(cè)量［6，7］等；這些測(cè)量均為準(zhǔn)靜態(tài)測(cè)量，難以反映構(gòu)件的運(yùn)動(dòng)性質(zhì)。加速度傳感器具有體積小，安裝方便且能實(shí)現(xiàn)動(dòng)態(tài)測(cè)量的優(yōu)點(diǎn)，可應(yīng)用于姿態(tài)參數(shù)的動(dòng)態(tài)測(cè)量中，其前提是建立加速度與剛體的姿態(tài)參數(shù)、角速度等之間的關(guān)系。

這里首先通過相伴運(yùn)動(dòng)與瞬軸性質(zhì)找到了坐標(biāo)變換矩陣與角速度矩陣的關(guān)系，從而得到了固定坐標(biāo)系中任意時(shí)刻剛體瞬時(shí)運(yùn)動(dòng)角速度與繞各軸轉(zhuǎn)動(dòng)的角度之間的關(guān)系，然后，建立了剛體上不共線三點(diǎn)的線加速度與剛體任意時(shí)刻空間運(yùn)動(dòng)三個(gè)姿態(tài)角之間關(guān)系的運(yùn)動(dòng)學(xué)非線性微分方程組，最后，采用空間RCCC機(jī)構(gòu)驗(yàn)證模型正解與反解的正確性，為進(jìn)一步實(shí)現(xiàn)姿態(tài)參數(shù)的動(dòng)態(tài)加速度測(cè)量奠定理論基礎(chǔ)。

2 姿態(tài)求解運(yùn)動(dòng)學(xué)模型

姿態(tài)求解運(yùn)動(dòng)學(xué)模型旨在研究空間運(yùn)動(dòng)剛體上點(diǎn)的位移、速度、加速度與剛體空間姿態(tài)角之間的關(guān)系。如圖1所示，為剛體空間運(yùn)動(dòng)的一般表示，為了描述剛體在空間中的位姿，在固定機(jī)架上建立固定坐標(biāo)系G{Of；if，jf，kf}，在剛體上建立運(yùn)動(dòng)坐標(biāo)系B{Om；im，jm，km}。運(yùn)動(dòng)坐標(biāo)系原點(diǎn)Om在固定坐標(biāo)系中的位置用矢量ROm來(lái)描述，剛體上任意一點(diǎn)Pi在剛體上的位置用矢量RPmi描述，該點(diǎn)在固定坐標(biāo)系的軌跡為

圖1 剛體空間運(yùn)動(dòng)一般表示與z-x-z歐拉角Fig.1 General Expression of Motion of Rigid Body and Euler Angle

式中：[Rmf]-運(yùn)動(dòng)坐標(biāo)系相對(duì)于固定坐標(biāo)系的旋轉(zhuǎn)變換矩陣，其有多種數(shù)學(xué)表達(dá)，如RPY角，歐拉角，二面角和四元數(shù)等，其中僅歐拉角就有12種不同的組合方式，上式可以采用任意旋轉(zhuǎn)變換矩陣。對(duì)（1）式求導(dǎo)，可得該點(diǎn)在固定坐標(biāo)系的速度。

式中：導(dǎo)數(shù)均為對(duì)時(shí)間的導(dǎo)數(shù)。由于點(diǎn)是剛體上的定點(diǎn)，其在剛體坐標(biāo)系中的位置不隨時(shí)間變化，R′Pmi=0，所以，

剛體在空間中的運(yùn)動(dòng)可以視為隨運(yùn)動(dòng)坐標(biāo)系原點(diǎn)Om的牽連運(yùn)動(dòng)和相對(duì)于原點(diǎn)Om的相對(duì)運(yùn)動(dòng)。

剛體有限次數(shù)的旋轉(zhuǎn)可以等效成繞一個(gè)特定軸進(jìn)行特定的旋轉(zhuǎn)，這個(gè)特定軸為剛體的瞬時(shí)螺旋軸（瞬軸）［8，9］，而任意時(shí)刻剛體運(yùn)動(dòng)的角速度矢量與瞬軸之間的關(guān)系為：角速度矢量的大小為剛體繞瞬軸轉(zhuǎn)動(dòng)的速度，角速度矢量的單位方向?yàn)樗草S的方向，因此剛體上任意一點(diǎn)的速度還可以寫成：

式中：GωB-運(yùn)動(dòng)剛體在固定坐標(biāo)系G中的角速度矢量。將角速度矢量寫成矩陣形式，有：

所以，角速度矩陣與坐標(biāo)變換矩陣之間的關(guān)系為

以z-x-z歐拉角φ,θ,ψ為例，其坐標(biāo)變換矩陣為：

式中：s-sin；c-cos。

求得角速度矩陣為：

其中，

對(duì)角速度矢量GωB求導(dǎo)可得角加速度矢量GεB：

角速度矢量GωB和角加速度矢量GεB已知時(shí)，剛體上任意一點(diǎn)P在固定坐標(biāo)系中的加速度為：

再將剛體上與P1不共線的另外兩點(diǎn)在剛體坐標(biāo)系的坐標(biāo)值P2(h1,h2,h3)，P3(q1,q2,q3)帶入（13）式，整理得到

上述三個(gè)式子不相互獨(dú)立，還可以做進(jìn)一步化簡(jiǎn)。分別用（17）式和（18）式減（16）式，并帶入角速度表達(dá)式（9）與角加速度表達(dá)式（12），

由此可知，已知?jiǎng)傮w的空間姿態(tài)變化時(shí)，剛體上任意一點(diǎn)的加速度可以根據(jù)公式（13）求出，反之，當(dāng)剛體上任意三點(diǎn)的線加速度值已知后，剛體的空間位置姿態(tài)角可由非線性微分方程組（19）反解得到。

式中：x-三點(diǎn)空間相對(duì)位置關(guān)系。

3 空間RCCC機(jī)構(gòu)算例

對(duì)于特定的空間機(jī)構(gòu)來(lái)說(shuō)，連桿上任意一點(diǎn)的軌跡、速度、加速度，連桿的空間姿態(tài)角以及瞬軸的單位方向矢量都是已知的。因此，以RCCC機(jī)構(gòu)為例，正解時(shí)，將RCCC機(jī)構(gòu)連桿的空間姿態(tài)角帶入第2節(jié)模型角速度公式（9）求出的瞬軸單位矢量，與機(jī)構(gòu)運(yùn)動(dòng)微分幾何學(xué)方法求出的瞬軸單位矢量進(jìn)行對(duì)比，驗(yàn)證正解的正確性；反解時(shí)，將RCCC機(jī)構(gòu)連桿上任意三點(diǎn)在固定坐標(biāo)系的加速度值帶入微分方程組（19）求出的角度，與RCCC機(jī)構(gòu)空間姿態(tài)角進(jìn)行對(duì)比，驗(yàn)證反解的正確性，以達(dá)到對(duì)運(yùn)動(dòng)學(xué)模型驗(yàn)證的目的。

3.1 RCCC機(jī)構(gòu)基本運(yùn)動(dòng)參數(shù)求解

如圖2所示為空間RCCC四桿機(jī)構(gòu)，連架桿3相對(duì)于機(jī)架的轉(zhuǎn)角為θ0，連架桿1相對(duì)于機(jī)架的轉(zhuǎn)角為θ1，連桿2相對(duì)于連架桿1的轉(zhuǎn)角為θ2，連架桿3相對(duì)于連桿的轉(zhuǎn)角為θ3，連桿2相對(duì)連架桿1在z1軸上的位移為h1。取RCCC機(jī)構(gòu)尺寸參數(shù)為α01=30°，h0=0。θ0與θ1的關(guān)系為：

其中，

李云[33]重點(diǎn)闡述碑學(xué)與帖學(xué)書風(fēng)的風(fēng)格差異，從其概念及特點(diǎn)進(jìn)行分析，認(rèn)為古人對(duì)“南帖”、“北碑”、“帖派”、“碑派”的劃分及其藝術(shù)特色的評(píng)價(jià)，盡管有時(shí)并不十分科學(xué)，但從其總體風(fēng)貌上來(lái)把握，還是很有道理的。相對(duì)來(lái)說(shuō)，南派重優(yōu)美，北派尚壯美，重帖者，偏好陰柔之美；重碑者，側(cè)于陽(yáng)剛之態(tài)。“帖”重“書卷氣”，“碑”重“金石氣”。書法中的“書卷氣”是一種性靈、氣質(zhì)、情趣的流露。“金石氣”相對(duì)于書卷氣來(lái)說(shuō)，所倡導(dǎo)的是蒼茫、渾厚、樸拙的審美范疇。

θ2與θ1的關(guān)系為：

其中，

θ3與θ1的關(guān)系為：

其中，

連桿2相對(duì)于連架桿1在z1軸上的位移h1與轉(zhuǎn)角θ1的關(guān)系：

連桿2運(yùn)動(dòng)坐標(biāo)系{RC;x2,y2,z2}中坐標(biāo)為(xPm,yPm,zPm)的點(diǎn)P，在機(jī)架坐標(biāo)系{RA;x0,y0,z0}中生成連桿曲線ΓP:RP=(xPf,yPf,zPf)T，坐標(biāo)(xPf,yPf,zPf)和(xPm,yPm,zPm)之間的轉(zhuǎn)化關(guān)系由下式確定：

其中，

任取RCCC機(jī)構(gòu)連桿2上運(yùn)動(dòng)坐標(biāo)系中坐標(biāo)為（0，0，0），（2，2，2），（1，4，3）的三點(diǎn)，其在固定坐標(biāo)系內(nèi)2個(gè)周期的位移，速度，加速度如圖3所示。

圖3 三點(diǎn)的三向運(yùn)動(dòng)參數(shù)Fig.3 Three Directions Motion Parameters of Three Points

由于RCCC機(jī)構(gòu)所建固定坐標(biāo)系與第2節(jié)模型的固定坐標(biāo)系方向不一致，因此用下式將（28）中的坐標(biāo)變換矩陣轉(zhuǎn)化成（7）形式，得到對(duì)應(yīng)的角度φ,θ,ψ。

則三個(gè)角度分別是：

如圖4所示，2個(gè)周期內(nèi)的三個(gè)角度值及其導(dǎo)數(shù)。

圖4 φ,θ,ψ角及其導(dǎo)數(shù)Fig.4 Angleφ,θ,ψand Derivative

3.2 運(yùn)動(dòng)學(xué)模型正解驗(yàn)證

根據(jù)機(jī)構(gòu)運(yùn)動(dòng)微分幾何學(xué)理論［10］，RCCC機(jī)構(gòu)連桿2空間運(yùn)動(dòng)的瞬軸方向矢量參數(shù)為：

其中，

圖5 RCCC機(jī)構(gòu)約束曲面Fig.5 Constraint Surface of RCCC Linkage

為使RCCC機(jī)構(gòu)的固定坐標(biāo)系與第2節(jié)中剛體運(yùn)動(dòng)模型固定坐標(biāo)系保持一致，需對(duì)上式求出的瞬軸乘坐標(biāo)變換矩陣：

得到的瞬軸單位方向矢量在固定坐標(biāo)系中的分量如下，圖中給出2周期內(nèi)的圖像。

將圖4的角度帶入第2節(jié)角速度表達(dá)式（9），可得到第2節(jié)所建立運(yùn)動(dòng)模型求出的角速度，其各方向分量如下圖，對(duì)角速度進(jìn)行單位化處理，即得到了瞬軸的單位方向，如圖7所示。

圖7 角速度矢量與瞬軸單位方向矢量計(jì)算值Fig.7 Calculated Value of Angular Velocity and ISA

對(duì)比圖7與圖6可以發(fā)現(xiàn)，第2節(jié)所建立的運(yùn)動(dòng)模型求解得到的瞬軸計(jì)算值與通過微分運(yùn)動(dòng)幾何學(xué)方法求解得到的瞬軸理論值完全相同，即驗(yàn)證了運(yùn)動(dòng)模型正解問題的正確性。

圖6 瞬軸單位方向矢量理論值Fig.6 Theoretical Value of ISA

3.3 運(yùn)動(dòng)學(xué)模型反解驗(yàn)證

反之，已知RCCC機(jī)構(gòu)連桿2運(yùn)動(dòng)坐標(biāo)系中任意三點(diǎn)坐標(biāo)以及三點(diǎn)在固定坐標(biāo)系的加速度值，也可求得三個(gè)角度值及其一階導(dǎo)數(shù)和二階導(dǎo)數(shù)。將圖3中（0，0，0），（2，2，2），（1，4，3）三點(diǎn)的坐標(biāo)與加速度值帶入微分方程組（19），求解結(jié)果如下：

圖8 反解微分方程組得到的角度參數(shù)計(jì)算值Fig.8 Angle Parameters Calculated by Differential Equations

求解結(jié)果顯示：反解出的RCCC機(jī)構(gòu)角度值與理論結(jié)果完全一致，即驗(yàn)證了模型在反解機(jī)構(gòu)問題時(shí)的可行性。當(dāng)剛體上三點(diǎn)的位置及其在固定坐標(biāo)系的三向加速度值已知時(shí)，可以通過反解微分方程組求得空間運(yùn)動(dòng)剛體的姿態(tài)角。

4 結(jié)論

（1）剛體空間運(yùn)動(dòng)的角速度等于其坐標(biāo)變換矩陣的導(dǎo)數(shù)乘坐標(biāo)變換矩陣的逆矩陣，從而可以得到角速度與剛體繞各軸轉(zhuǎn)動(dòng)的角度之間的關(guān)系。

（2）空間運(yùn)動(dòng)剛體上不共線三點(diǎn)的加速度與剛體的姿態(tài)之間滿足非線性微分方程組的關(guān)系，當(dāng)已知?jiǎng)傮w上三點(diǎn)的三向線加速度時(shí)，剛體空間姿態(tài)角可以通過反解方程組求得。

（3）空間RCCC機(jī)構(gòu)驗(yàn)證結(jié)果表明該運(yùn)動(dòng)學(xué)模型可以應(yīng)用于機(jī)構(gòu)的求解中，若將其應(yīng)用到誤差測(cè)量或參數(shù)標(biāo)定等工程實(shí)際領(lǐng)域還需進(jìn)一步研究剛體與真實(shí)機(jī)構(gòu)之間的關(guān)系。