多剛體動(dòng)力學(xué)仿真的李群變分積分算法*

黃子恒 陳菊? 張志娟 田強(qiáng)

（1.北京理工大學(xué) 宇航學(xué)院力學(xué)系，北京 100081）（2.北京空間飛行器總體設(shè)計(jì)部，北京 100094）

引言

常用多剛體系統(tǒng)動(dòng)力學(xué)建模方法有自然坐標(biāo)方法[1]、歐拉角方法[2]、歐拉四元數(shù)方法[3]、李群李代數(shù)方法[4]等.自然坐標(biāo)方法通過若干剛體上的基點(diǎn)以及若干內(nèi)嵌在剛體上的單位向量表示剛體位姿，采用該方法可以得到含常數(shù)質(zhì)量矩陣的多剛體系統(tǒng)動(dòng)力學(xué)方程.其余常規(guī)動(dòng)力學(xué)建模方法則通過剛體質(zhì)心坐標(biāo)與剛體的姿態(tài)來確定剛體的位姿，以姿態(tài)的不同表示方法進(jìn)行區(qū)別.常用的姿態(tài)表示方法有歐拉角[2]、歐拉四元數(shù)[3]、李群SO（3）矩陣[4]等.歐拉角方法是常用描述剛體姿態(tài)的方法，計(jì)算簡便.描述剛體在三維空間中的運(yùn)動(dòng)姿態(tài)可采用2類12種歐拉角系統(tǒng)，但無論采用哪種歐拉角系統(tǒng)，都不可避免會(huì)含有奇異點(diǎn)[5].歐拉四元數(shù)方法用4個(gè)有約束關(guān)系的參數(shù)描述剛體姿態(tài)，避免了奇異問題[5].然而，歐拉四元數(shù)方法的表述并不直觀，內(nèi)蘊(yùn)約束方程會(huì)影響動(dòng)力學(xué)方程的求解效率，其雙值性甚至?xí)?dǎo)致剛體系統(tǒng)控制中的退繞現(xiàn)象[6].李群方法則采取指數(shù)映射進(jìn)行迭代，使得剛體姿態(tài)旋轉(zhuǎn)矩陣始終保持為正交矩陣.李群SO（3）對(duì)應(yīng)的李代數(shù)空間so（3）同構(gòu)于三維歐氏空間R3，因此李代數(shù)空間的元素由3個(gè)獨(dú)立的坐標(biāo)構(gòu)成.在每一步迭代的過程中李群方法無需考慮多個(gè)參數(shù)之間的約束關(guān)系，有助于提高計(jì)算效率.

剛體的動(dòng)力學(xué)應(yīng)具備守恒性質(zhì)，如保守系統(tǒng)的能量守恒，動(dòng)量守恒等.為不與物理規(guī)律相悖，守恒律已經(jīng)成為檢驗(yàn)動(dòng)力學(xué)建模與數(shù)值算法的重要標(biāo)準(zhǔn).雖然用李群方法建模的性質(zhì)優(yōu)越，但由于李群自身的非線性性質(zhì)，使得歐氏空間上的常規(guī)數(shù)值算法在求解多剛體動(dòng)力學(xué)方程時(shí)失效，如常規(guī)的Runge-Kutta方法不僅無法保證首次積分守恒，還會(huì)產(chǎn)生較大的能量耗散與結(jié)構(gòu)誤差[7].目前主要用如下兩種方法改進(jìn)已有李群算法的不足：

方法一、采用新數(shù)值離散格式離散系統(tǒng)連續(xù)動(dòng)力學(xué)方程.該方法通過引入李括號(hào)項(xiàng)可使系統(tǒng)的位形空間始終在真實(shí)的李群空間中迭代.例如，Munthe-Kaas基于經(jīng)典Runge-Kutta算法，通過引入校正函數(shù)[7，8]提出了一種 Runge-Kutta-Munthe-Kaas（RKMK）算法.該算法能保證迭代過程在系統(tǒng)正確的微分流形上進(jìn)行.Wieloch與Arnold[9]將BDF方法與李群方法相結(jié)合提出了BLieDF方法，在盡可能不丟失精度的同時(shí)使用更少的李括號(hào)項(xiàng)，提高了計(jì)算效率 .Brüls[3，10]基于兩種描述剛體位形的李群（SE（3）與?3× SO（3）），結(jié)合廣義α方法提出了Lie-廣義α方法.Lie-廣義α方法已被成功應(yīng)用于多種多體動(dòng)力學(xué)建模與計(jì)算，例如：空間曲柄滑塊[11]，柔性四桿機(jī)構(gòu)[12]以及空間捕獲飛網(wǎng)[13]等 .李亞男等[14]使用Lie-廣義α方法求解指標(biāo)為1的微分代數(shù)方程，使仿真過程中能夠同時(shí)保持位移約束、速度約束與加速度約束，提高了計(jì)算精度.

方法二、李群變分積分算法[15].該算法采用離散Hamilton變分原理建立系統(tǒng)的動(dòng)力學(xué)方程，獲得系統(tǒng)的非線性方程組并求解，具有理想的保辛、保動(dòng)量、保能量等性質(zhì).已有研究已構(gòu)造了多種Hamilton變分過程中Lagrange量的多種離散格式，提出了相應(yīng)的李代數(shù)離散格式下的李群變分積分算法.李群變分積分算法充分結(jié)合李群李代數(shù)方法與變分積分算法的優(yōu)勢，相較常規(guī)變分積分算法不僅能夠直接減少參數(shù)之間的約束方程，顯著提高計(jì)算效率，還能保持系統(tǒng)幾何結(jié)構(gòu)，提高計(jì)算精度.本文將基于差分方法推導(dǎo)的李群變分積分公式稱為一般格式李群變分積分公式.一般格式李群變分積分算法已經(jīng)被應(yīng)用于空間雙連桿機(jī)械臂[16]、無人潛艇[17]、無人機(jī)編隊(duì)[18]、軌道控制[19，20]等動(dòng)力學(xué)建模與控制研究中，在解決這些實(shí)際問題時(shí)表現(xiàn)出了保持系統(tǒng)能量與結(jié)構(gòu)的特性.

進(jìn)一步，Hante 和 Arnold[21]通過對(duì)數(shù)映射改進(jìn)了以上一般格式算法中的李代數(shù)元素離散方式得到RATTLie變分積分算法.該算法在Cosserat柔性梁動(dòng)力學(xué)分析中表現(xiàn)出較好的能量守恒特性.史東華和Zenkov[22]通過直接對(duì)李代數(shù)元素進(jìn)行變分的方式提出了Hamel場變分積分算法.該方法已被應(yīng)用于抓取機(jī)械臂[23]、柔性梁[24]等多體動(dòng)力學(xué)與控制研究，展現(xiàn)出了較好的數(shù)值特性.Hall和Leok[25]提出了李群譜變分積分算法，重力作用下的單擺動(dòng)力學(xué)算例研究表明：采用大積分步長，該方法同樣能長時(shí)間保持系統(tǒng)能量與數(shù)值穩(wěn)定.隨著多自由度復(fù)雜約束動(dòng)力學(xué)問題的出現(xiàn)，白龍等[26]通過Kelly變換與Newton迭代克服了李群變分積分算法的隱式求解問題，提高了計(jì)算效率.Lee等[27]提出了多體系統(tǒng)的線性變分積分器，通過離散遞歸牛頓歐拉算法（Discrete recursive Newton-Euler algorithm）求解殘差向量，以及鉸接體慣性算法（Articulated body inertia algorithm）計(jì)算更新迭代值，提高了計(jì)算效率.

本文基于李群李代數(shù)的離散Hamilton方程，建立了Hamilton體系下多剛體系統(tǒng)動(dòng)力學(xué)兩類李群變分積分算法.分別采用三種算法（一般格式的李群變分積分算法、RATTLie變分積分算法與Lie-廣義α算法）計(jì)算了重力作用下空間剛體雙擺的動(dòng)力學(xué)問題，對(duì)比研究了各算法的能量誤差、約束違約等特性.研究表明一般格式的李群變分積分算法與RATTLie變分積分算法在長時(shí)間保持系統(tǒng)結(jié)構(gòu)、能量等方面存在顯著優(yōu)勢，具有潛在工程應(yīng)用前景，如：航天器軌道動(dòng)力學(xué)問題、大型柔性空間結(jié)構(gòu)在軌服務(wù)操作動(dòng)力學(xué)問題等.

1 離散Hamilton體系下的李群變分積分算法

李群變分積分公式是通過離散的Hamilton變分原理得到的動(dòng)力學(xué)方程組，而非對(duì)連續(xù)的動(dòng)力學(xué)方程組直接進(jìn)行離散.本文用G表示李群，用g表示李代數(shù)，用g*表示李代數(shù)的對(duì)偶空間.

2 多剛體系統(tǒng)動(dòng)力學(xué)方程的兩種離散格式

2.1 一般離散格式

圖1 多剛體系統(tǒng)構(gòu)形示意圖Fig.1 Schematic view of a multi-rigid body system configuration

2.2 RATTLie離散格式

3 數(shù)值計(jì)算與分析

考察如圖2所示的重力作用下的空間剛體雙擺動(dòng)力學(xué)特性.兩根擺均為圓柱剛桿，桿1（OA）與桿2（AB）的質(zhì)心為O1，O2，兩根相同桿的基本參數(shù)為：密度ρ=7850kg?m-3，底部半徑為r=0.05m，母線長為l=1m.桿1與桿2質(zhì)量為m1=m2=61.6538kg，桿1與桿2的慣性張量矩陣為初始時(shí)刻t0=0時(shí)空間雙擺模型狀態(tài)如圖2虛線所示，桿1一端球鉸約束在固定點(diǎn)O，桿1與桿2由A處球鉸連接.初始時(shí)兩擺呈垂直關(guān)系，桿1的位形由位姿矩陣R1，0=I3×3與質(zhì)心坐標(biāo)x1，0=[0 0.5 0]T確定，桿2的位形由位姿矩陣與質(zhì)心坐標(biāo)x2，0=[-0.5 1 0]T確定 .初始桿 1 與桿 2的體角速度Ω1，0=Ω2，0=[0 0 0]T，初始桿1與桿2的質(zhì)心平動(dòng)速度1，0=2，0=[0 0 0]T.

圖2 重力作用下的空間雙擺模型示意圖Fig.2 Schematic view of a spatial double pendulum under the gravity action

桿1的局部坐標(biāo)系O1-X1Y1Z1與桿2的局部坐標(biāo)系O2-X2Y2Z2如圖2所示，兩桿運(yùn)動(dòng)過程中只考慮重力影響，設(shè)經(jīng)過時(shí)間tn后桿1可到達(dá)OA′處，桿2可到達(dá)A′B′處.

雙擺系統(tǒng)有兩處約束，O處球鉸為約束1，A處球鉸為約束2，綜合寫為

式（24）中的Xij為約束j處的球鉸到擺i的質(zhì)心的位置向量，本算例中X11=[0 -0.5 0]T，X12=[0 0.5 0]T，X21=[0 -0.5 0]T.表1給出了雙擺對(duì)應(yīng)的約束Jacobi矩陣表達(dá).

表1 空間雙擺的約束Jacobi矩陣Table 1 Jacobi matrix of the double pendulum’s constraints

采用Lie-廣義α方法計(jì)算時(shí)，算法譜半徑選取為0.9.另外兩類Hamilton體系變分積分算法則直接求解非線性方程組.Lie-廣義α方法，一般格式的李群變分積分算法與RATTLie變分積分算法均使用10-3步長進(jìn)行計(jì)算.采用商業(yè)軟件Recurdyn分別使用10-4、10-5兩種步長進(jìn)行計(jì)算.用“Lie-alpha”表示Lie-廣義α方法，“DH”表示一般格式的李群變分積分算法，“RL”表示RATTLie變分積分算法.仿真總時(shí)間設(shè)為50s，使用Matlab進(jìn)行編程，在一臺(tái)具有Intel Core i7-7700 3.6GHz處理器及16GB RAM的PC機(jī)上運(yùn)行.

圖3為以上所有方法擺2的質(zhì)心O2點(diǎn)位移矢量的X軸方向分量結(jié)果，圖4為以上所有方法桿2的角速度矢量繞O2X2軸方向的分量結(jié)果.根據(jù)圖3與圖4可知：當(dāng)步長為10-4時(shí)，商業(yè)軟件Recurdyn在4s以后計(jì)算結(jié)果逐漸發(fā)散；當(dāng)步長為10-5s時(shí)，Lie-廣義α方法與李群變分積分算法（DH，RL）的所得結(jié)果與商業(yè)軟件Recurdyn計(jì)算結(jié)果幾乎重合，說明了本文建立的建模與計(jì)算方法的正確性.

圖3 桿2質(zhì)心O2點(diǎn)位移矢量的X軸方向分量Fig.3 X-component of the second pendulum’s mass center O2

圖4 桿2角速度矢量繞O2X2軸方向分量Fig.4 Component of the second pendulum’s angular velocity about axis-O2X2

圖5為DH方法、RL方法與Lie-廣義α方法的能量波動(dòng)情況對(duì)比圖，其中圖5（b）為圖3（a）在7.5～10s的放大圖.由圖5可知：1、Lie-廣義α方法的能量波動(dòng)遠(yuǎn)遠(yuǎn)大于李群變分積分算法.2、當(dāng)步長選取為10-3時(shí)，DH方法已經(jīng)遠(yuǎn)遠(yuǎn)優(yōu)于步長為10-4的商業(yè)軟件方法，可知DH方法在計(jì)算過程中耗散極低基本保持穩(wěn)定.3、RL方法在能量保持方面優(yōu)于DH方法，可知數(shù)值離散的高精度格式可以提高計(jì)算精度.

圖5 空間雙擺的能量變化曲線對(duì)比圖：（a）0～50s（b）7.5～10sFig.5 Comparison of the spatial double pendulum’s energy variations ：（a）0～50s（b）7.5～10s

圖6為Lie-廣義α方法，DH方法，RL方法的SO（3）正交性誤差曲線對(duì)比圖.SO（3）群元素R需要滿足正交性條件，而‖I3×3-RRT‖表示SO（3）群元素R的正交性誤差，可以反映算法迭代過程中的群結(jié)構(gòu)保持情況.從圖6中明顯可以看出，三種方法的正交性誤差保持量級(jí)均在10-14，算法使得系統(tǒng)的李群結(jié)構(gòu)保持很好.

圖6 空間雙擺SO（3）正交性誤差曲線對(duì)比圖Fig.6 Comparison of the spatial double pendulum’s SO（3）orthogonality error curves

圖7給出了DH方法與離散Euler-Lagrange方程建模方法（DL方法）、Lie-廣義α方法的A處球鉸的速度約束誤差對(duì)比曲線.從圖7可以看出，由于Lie-廣義α方法與離散Euler-Lagrange方程組[14]并未對(duì)速度進(jìn)行違約校正，速度誤差的量級(jí)已經(jīng)達(dá)到10-4.DH方法引入速度約束方程后速度誤差量級(jí)為1e-16，顯著改善了約束違約情況.引入速度約束方程后，基于Hamilton體系的RATTLie算法對(duì)系統(tǒng)約束保持情況也將顯著改善.

圖7 空間雙擺速度約束違約曲線對(duì)比圖Fig.7 Comparison of the spatial double pendulum’s velocity constraint violation curves

4 結(jié)論

基于離散變分原理建立了多剛體動(dòng)力學(xué)模型的一般格式李群變分積分算法和RATTLie變分積分算法.通過算例對(duì)比分析發(fā)現(xiàn)：一般格式的李群變分積分算法和RATTLie變分積分算法具有保能量、保結(jié)構(gòu)的性質(zhì).在積分步長選取較大時(shí)，該方法遠(yuǎn)遠(yuǎn)優(yōu)于步長較小的商業(yè)軟件的計(jì)算結(jié)果；RATTLie方法的系統(tǒng)能量保持特性優(yōu)于一般格式算法；采用Hamilton體系的李群變分積分算法相比離散Lagrange體系的算法能量波動(dòng)范圍更小，約束違約更小.后續(xù)可進(jìn)一步研究這類算法的并行計(jì)算問題以及基于這類算法的多柔體動(dòng)力學(xué)與控制問題，特別是在軌大型柔性空間結(jié)構(gòu)的組裝過程動(dòng)力學(xué)與控制問題.