基于運動微分方程與加速度的剛體運動姿態算法

張 楠，王 智，王德倫

（大連理工大學機械工程學院，遼寧 大連116024）

1 引言

在精密裝備的運動精度測量過程中，被測對象的運動通常被視為剛體的一般空間運動，并被分解為隨運動參考點的三向平移與繞參考點的三向轉動。其中三向平移參數的描述及測量相對簡單，如點的位移、速度、加速度等，而三向轉動（即姿態）的描述及測量則較為困難。

現常用的剛體運動測量方法有以計算機視覺為核心的光電視覺運動測量［1］、基于激光跟蹤儀的六自由度運動測量［2，3］等，前者采用高分辨率的光電成像系統，需結合其他輔助光電信息，且測量精度往往達不到理想的要求，后者測量準確，但安裝耗時且只能實現準靜態測量。還有以加速度計和陀螺儀組成慣性測量單元進行傾角計算［4］的方法，但積分運算往往導致測量誤差隨時間累積，測量結果發散。

由于點的運動測量相對簡單，在很多場合，經常通過測量運動構件上點的運動參數間接得到運動構件的姿態參數，如機器人末端姿態的跟蹤測量［5］、機床的多線法測量［6，7］等；這些測量均為準靜態測量，難以反映構件的運動性質。加速度傳感器具有體積小，安裝方便且能實現動態測量的優點，可應用于姿態參數的動態測量中，其前提是建立加速度與剛體的姿態參數、角速度等之間的關系。

這里首先通過相伴運動與瞬軸性質找到了坐標變換矩陣與角速度矩陣的關系，從而得到了固定坐標系中任意時刻剛體瞬時運動角速度與繞各軸轉動的角度之間的關系，然后，建立了剛體上不共線三點的線加速度與剛體任意時刻空間運動三個姿態角之間關系的運動學非線性微分方程組，最后，采用空間RCCC機構驗證模型正解與反解的正確性，為進一步實現姿態參數的動態加速度測量奠定理論基礎。

2 姿態求解運動學模型

姿態求解運動學模型旨在研究空間運動剛體上點的位移、速度、加速度與剛體空間姿態角之間的關系。如圖1所示，為剛體空間運動的一般表示，為了描述剛體在空間中的位姿，在固定機架上建立固定坐標系G{Of；if，jf，kf}，在剛體上建立運動坐標系B{Om；im，jm，km}。運動坐標系原點Om在固定坐標系中的位置用矢量ROm來描述，剛體上任意一點Pi在剛體上的位置用矢量RPmi描述，該點在固定坐標系的軌跡為

圖1 剛體空間運動一般表示與z-x-z歐拉角Fig.1 General Expression of Motion of Rigid Body and Euler Angle

式中：[Rmf]-運動坐標系相對于固定坐標系的旋轉變換矩陣，其有多種數學表達，如RPY角，歐拉角，二面角和四元數等，其中僅歐拉角就有12種不同的組合方式，上式可以采用任意旋轉變換矩陣。對（1）式求導，可得該點在固定坐標系的速度。

式中：導數均為對時間的導數。由于點是剛體上的定點，其在剛體坐標系中的位置不隨時間變化，R′Pmi=0，所以，

剛體在空間中的運動可以視為隨運動坐標系原點Om的牽連運動和相對于原點Om的相對運動。

剛體有限次數的旋轉可以等效成繞一個特定軸進行特定的旋轉，這個特定軸為剛體的瞬時螺旋軸（瞬軸）［8，9］，而任意時刻剛體運動的角速度矢量與瞬軸之間的關系為：角速度矢量的大小為剛體繞瞬軸轉動的速度，角速度矢量的單位方向為瞬軸的方向，因此剛體上任意一點的速度還可以寫成：

式中：GωB-運動剛體在固定坐標系G中的角速度矢量。將角速度矢量寫成矩陣形式，有：

所以，角速度矩陣與坐標變換矩陣之間的關系為

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

式中：s-sin；c-cos。

求得角速度矩陣為：

其中，

對角速度矢量GωB求導可得角加速度矢量GεB：

角速度矢量GωB和角加速度矢量GεB已知時，剛體上任意一點P在固定坐標系中的加速度為：

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

上述三個式子不相互獨立，還可以做進一步化簡。分別用（17）式和（18）式減（16）式，并帶入角速度表達式（9）與角加速度表達式（12），

由此可知，已知剛體的空間姿態變化時，剛體上任意一點的加速度可以根據公式（13）求出，反之，當剛體上任意三點的線加速度值已知后，剛體的空間位置姿態角可由非線性微分方程組（19）反解得到。

式中：x-三點空間相對位置關系。

3 空間RCCC機構算例

對于特定的空間機構來說，連桿上任意一點的軌跡、速度、加速度，連桿的空間姿態角以及瞬軸的單位方向矢量都是已知的。因此，以RCCC機構為例，正解時，將RCCC機構連桿的空間姿態角帶入第2節模型角速度公式（9）求出的瞬軸單位矢量，與機構運動微分幾何學方法求出的瞬軸單位矢量進行對比，驗證正解的正確性；反解時，將RCCC機構連桿上任意三點在固定坐標系的加速度值帶入微分方程組（19）求出的角度，與RCCC機構空間姿態角進行對比，驗證反解的正確性，以達到對運動學模型驗證的目的。

3.1 RCCC機構基本運動參數求解

如圖2所示為空間RCCC四桿機構，連架桿3相對于機架的轉角為θ0，連架桿1相對于機架的轉角為θ1，連桿2相對于連架桿1的轉角為θ2，連架桿3相對于連桿的轉角為θ3，連桿2相對連架桿1在z1軸上的位移為h1。取RCCC機構尺寸參數為α01=30°，h0=0。θ0與θ1的關系為：

其中，

李云[33]重點闡述碑學與帖學書風的風格差異，從其概念及特點進行分析，認為古人對“南帖”、“北碑”、“帖派”、“碑派”的劃分及其藝術特色的評價，盡管有時并不十分科學，但從其總體風貌上來把握，還是很有道理的。相對來說，南派重優美，北派尚壯美，重帖者，偏好陰柔之美；重碑者，側于陽剛之態。“帖”重“書卷氣”，“碑”重“金石氣”。書法中的“書卷氣”是一種性靈、氣質、情趣的流露?！敖鹗瘹狻毕鄬τ跁須鈦碚f，所倡導的是蒼茫、渾厚、樸拙的審美范疇。

θ2與θ1的關系為：

其中，

θ3與θ1的關系為：

其中，

連桿2相對于連架桿1在z1軸上的位移h1與轉角θ1的關系：

連桿2運動坐標系{RC;x2,y2,z2}中坐標為(xPm,yPm,zPm)的點P，在機架坐標系{RA;x0,y0,z0}中生成連桿曲線ΓP:RP=(xPf,yPf,zPf)T，坐標(xPf,yPf,zPf)和(xPm,yPm,zPm)之間的轉化關系由下式確定：

其中，

任取RCCC機構連桿2上運動坐標系中坐標為（0，0，0），（2，2，2），（1，4，3）的三點，其在固定坐標系內2個周期的位移，速度，加速度如圖3所示。

圖3 三點的三向運動參數Fig.3 Three Directions Motion Parameters of Three Points

由于RCCC機構所建固定坐標系與第2節模型的固定坐標系方向不一致，因此用下式將（28）中的坐標變換矩陣轉化成（7）形式，得到對應的角度φ,θ,ψ。

則三個角度分別是：

如圖4所示，2個周期內的三個角度值及其導數。

圖4 φ,θ,ψ角及其導數Fig.4 Angleφ,θ,ψand Derivative

3.2 運動學模型正解驗證

根據機構運動微分幾何學理論［10］，RCCC機構連桿2空間運動的瞬軸方向矢量參數為：

其中，

圖5 RCCC機構約束曲面Fig.5 Constraint Surface of RCCC Linkage

為使RCCC機構的固定坐標系與第2節中剛體運動模型固定坐標系保持一致，需對上式求出的瞬軸乘坐標變換矩陣：

得到的瞬軸單位方向矢量在固定坐標系中的分量如下，圖中給出2周期內的圖像。

將圖4的角度帶入第2節角速度表達式（9），可得到第2節所建立運動模型求出的角速度，其各方向分量如下圖，對角速度進行單位化處理，即得到了瞬軸的單位方向，如圖7所示。

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

對比圖7與圖6可以發現，第2節所建立的運動模型求解得到的瞬軸計算值與通過微分運動幾何學方法求解得到的瞬軸理論值完全相同，即驗證了運動模型正解問題的正確性。

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

3.3 運動學模型反解驗證

反之，已知RCCC機構連桿2運動坐標系中任意三點坐標以及三點在固定坐標系的加速度值，也可求得三個角度值及其一階導數和二階導數。將圖3中（0，0，0），（2，2，2），（1，4，3）三點的坐標與加速度值帶入微分方程組（19），求解結果如下：

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

求解結果顯示：反解出的RCCC機構角度值與理論結果完全一致，即驗證了模型在反解機構問題時的可行性。當剛體上三點的位置及其在固定坐標系的三向加速度值已知時，可以通過反解微分方程組求得空間運動剛體的姿態角。

4 結論

（1）剛體空間運動的角速度等于其坐標變換矩陣的導數乘坐標變換矩陣的逆矩陣，從而可以得到角速度與剛體繞各軸轉動的角度之間的關系。

（2）空間運動剛體上不共線三點的加速度與剛體的姿態之間滿足非線性微分方程組的關系，當已知剛體上三點的三向線加速度時，剛體空間姿態角可以通過反解方程組求得。

（3）空間RCCC機構驗證結果表明該運動學模型可以應用于機構的求解中，若將其應用到誤差測量或參數標定等工程實際領域還需進一步研究剛體與真實機構之間的關系。