劉 佳，楊 凱，楊 蓓，楊偉超

（石家莊學(xué)院機(jī)電學(xué)院，河北石家莊 050035）

約束多體系統(tǒng)的動(dòng)力學(xué)模型常被處理為若干微分方程與約束方程組成的方程組進(jìn)行求解處理。理論上講，如果存在解析解，此解在滿(mǎn)足加速度層面約束方程的同時(shí)，也應(yīng)該滿(mǎn)足速度層面約束方程和位置層面約束方程。

然而，在數(shù)值求解時(shí)，由于數(shù)值近似和截?cái)嗾`差等的影響，數(shù)值解將僅僅能滿(mǎn)足加速度層面約束方程，而嚴(yán)重偏離位置約束方程和速度約束方程，出現(xiàn)約束違約現(xiàn)象。

對(duì)于移動(dòng)機(jī)器人來(lái)講，由于其存在的非完整約束，使得建立其解析形式的動(dòng)力學(xué)方程具有一定的困難。而基于分析力學(xué)界經(jīng)典的Udwadia?Kalaba方程建立的解析形式的動(dòng)力學(xué)方程依然存在約束違約［1］。

其原因除數(shù)值近似和截?cái)嗾`差等因素外，將移動(dòng)機(jī)器人本身固有的非完整約束方程融入到質(zhì)量矩陣而導(dǎo)致的矩陣奇異也應(yīng)該是不可忽略的因素。

當(dāng)前，處理約束違約的方法主要有兩種：（1）約束違約消除方法［2］；（2）約束違約穩(wěn)定性方法［3］。

約束違約消除方法是將滿(mǎn)足約束方程的解視為約束流形上的映射，通過(guò)將由動(dòng)力學(xué)方程積分得到的廣義坐標(biāo)和廣義速度投影到約束流形上，使廣義坐標(biāo)和廣義速度滿(mǎn)足約束方程從而實(shí)現(xiàn)違約校正。其中代表性的應(yīng)用是：文獻(xiàn)［4?5］分別采用幾何投影法對(duì)空間曲柄滑塊機(jī)構(gòu)和牛頭刨床機(jī)構(gòu)做了分析研究，并取得滿(mǎn)意效果。文獻(xiàn)［6］基于改進(jìn)Udwadia?Kalaba方程建立了雙臂協(xié)調(diào)機(jī)械手的動(dòng)力學(xué)方程，約束違約得到一定程度的抑制。但由于僅考慮了忽視速度層面約束方程對(duì)約束違約的影響，因此結(jié)果仍有待改善。

約束違約穩(wěn)定性方法通過(guò)引入包含約束方程的修正項(xiàng)以達(dá)到減小甚至消除約束漂移的目的。這其中最著名的是Baumgarte穩(wěn)定方法。然而，Baumgarte穩(wěn)定性方法需要合理選擇修正項(xiàng)中涉及到的參數(shù)，這是一件比較困難的事情［7］。尤其當(dāng)系統(tǒng)具有多個(gè)約束方程時(shí)，穩(wěn)定參數(shù)的選擇就成為一個(gè)單調(diào)、繁瑣的試錯(cuò)過(guò)程。

基于此，這里利用Baumgarte穩(wěn)定方法，基于求解常微分方程常用的四階Runge?Kutta法探討穩(wěn)定參數(shù)的選擇問(wèn)題，并將其應(yīng)用到基于Udwadia?Kalaba方程建立的移動(dòng)機(jī)器人解析動(dòng)力學(xué)方程上以驗(yàn)證其有效性。

2 Baumgarte穩(wěn)定方法

系統(tǒng)的約束方程可表示為以下形式：

式中：q—系統(tǒng)的廣義變量；t—時(shí)間變量；

p—系統(tǒng)約束方程的個(gè)數(shù)。

進(jìn)一步可表示為：

根據(jù)Baumgarte穩(wěn)定方法，將加速度層面約束方程改寫(xiě)為：

基于Udwadia?Kalaba 方程的［8?10］機(jī)器人解析動(dòng)力學(xué)方程可表示為：

式中：Q—不考慮約束時(shí)系統(tǒng)所受的約束力（矩）；

Qc—考慮約束時(shí)機(jī)器人所需的約束力（矩）。

基于Baumgarte 穩(wěn)定方法修正的Udwadia—Kalaba 方程［11］，可得：

3 Baumgarte參數(shù)的估計(jì)

針對(duì)一階方程的初值問(wèn)題［12］：

所謂數(shù)值解法，就是尋求解y(x)在一系列離散節(jié)點(diǎn)x1

求解初值問(wèn)題式（7）的顯示單步法：

這里ci，λi，μij均為常數(shù)，式（8）稱(chēng)為r級(jí)顯式Runge?Kutta法。當(dāng)r= 4時(shí)，即為經(jīng)典四階Runge?Kutta法：

將式（3）改寫(xiě)為：

采用經(jīng)典的四階Runge?Kutta法求解式（10），由式（9）得：

其中：L1=hBKn，

將L1，L2，L3，L4代入式（11）得：

對(duì)式（12）進(jìn)行Z變換得：

系統(tǒng)的特征方程為：

根據(jù)控制理論的相關(guān)知識(shí)可知，當(dāng) |Z|≤1時(shí)和Φ將會(huì)收斂到0。當(dāng) |Z|≤1時(shí)的選取點(diǎn)，如圖1所示。根據(jù)圖1，并結(jié)合式（15），一旦指定步長(zhǎng)h，則Baumgarte參數(shù)即可選定。

圖1 的穩(wěn)定區(qū)域Fig.1 Stability Region in the Plane

4 仿真實(shí)例

移動(dòng)機(jī)器人運(yùn)動(dòng)模型，如圖2 所示。Oxyz為大地坐標(biāo)系。xpPyp為移動(dòng)機(jī)器人平臺(tái)上固定點(diǎn)P的局部坐標(biāo)系，P點(diǎn)坐標(biāo)為(x，y)，xp與x之間的夾角為θ。P點(diǎn)與移動(dòng)機(jī)器人質(zhì)心C點(diǎn)之間的距離為a= 0.145m。兩側(cè)驅(qū)動(dòng)輪相距2d= 0.8m，驅(qū)動(dòng)輪半徑為r= 0.08m。移動(dòng)機(jī)器人質(zhì)量為m1= 6.32kg，轉(zhuǎn)動(dòng)慣量Iz=0.063kg.m2。每個(gè)驅(qū)動(dòng)輪質(zhì)量為m2= 0.31kg，轉(zhuǎn)動(dòng)慣量為Il=Iz=0.001kg.m2。移動(dòng)機(jī)器人廣義坐標(biāo)為θ=[x，y，θ，θl，θr]，其中θl，θr分別為左右驅(qū)動(dòng)輪的轉(zhuǎn)角。

圖2 移動(dòng)機(jī)器人運(yùn)動(dòng)模型Fig.2 The Motion Diagram of Mobile Robot

移動(dòng)機(jī)器人運(yùn)動(dòng)軌跡為：

根據(jù)文獻(xiàn)［13］的建模方法得移動(dòng)機(jī)器人未受約束時(shí)的動(dòng)力學(xué)方程為：

將移動(dòng)機(jī)器人的運(yùn)行軌跡視為外部約束并形，如式（2）的形式：

假設(shè)初始條件為：x( 0 )= 0m，y( 0 )=?4m，θ( 0 )=θl( 0 )=θr( 0 )= 0rad，?( 0 )= 3m/s，y?( 0 )= 0m/s?( 0 )= 1.33rad/s，?( 0 )=( 0 )= 0rad/s。由式（6）可得基于Baumgarte 穩(wěn)定性方法修正的動(dòng)力學(xué)方程：

為驗(yàn)證所提方法的有效性，根據(jù)圖1選擇的三組數(shù)據(jù)，如表1所示。

表1 不同穩(wěn)定參數(shù)表Tab.1 The Table of Different Stability Parameters

仿真結(jié)果，如圖3~圖7所示。圖3和圖5分別表示移動(dòng)機(jī)器人在x方向和y方向的位移示意圖。x未修正和y未修正分別表示沒(méi)有采用Baumgarte參數(shù)時(shí)的位移。x修正和y修正分別表示采用穩(wěn)定區(qū)域Baumgarte參數(shù)時(shí)的位移。x非穩(wěn)定和y非穩(wěn)定分別表示采用非穩(wěn)定區(qū)域Baumgarte參數(shù)時(shí)的位移。x理論和y理論分別表示理論位移曲線。三種情況下x方向位移偏差不是很大，如圖3所示。

圖3 x方向位移示意圖Fig.3 The Displacement Diagram of x Direction

圖4 x方向位移誤差示意圖Fig.4 The Displacement Error Diagram of x Direction

圖5 y方向的位移示意圖Fig.5 The Displacement Diagram of y Direction

而沒(méi)有采用Baumgarte參數(shù)時(shí)的位移存在明顯的約束違約，如圖5所示。

移動(dòng)機(jī)器人在x方向和y方向的位移誤差示意圖，如圖4、圖6所示。其中，error1=x?3 sint表示x方向的位移誤差；error2=y+ 4 cost表示y方向的位移誤差。下標(biāo)文字含義與上述相同。圖4、圖6均表明沒(méi)有采用Baumgarte參數(shù)時(shí)的位移誤差隨時(shí)間逐漸增大，采用非穩(wěn)定區(qū)域Baumgarte參數(shù)時(shí)的位移誤差則得到了較好的控制，而采用穩(wěn)定區(qū)域Baumgarte參數(shù)時(shí)的位移誤差則得到了很好的改善，x方向誤差數(shù)量級(jí)為10?4，y方向誤差數(shù)量級(jí)為10?3。移動(dòng)機(jī)器人運(yùn)動(dòng)軌跡示意圖，如圖7所示。

圖6 y方向位移誤差示意圖Fig.6 The Displacement Error Diagram of y Direction

圖7 運(yùn)動(dòng)軌跡對(duì)比示意圖Fig.7 The Comparison Diagram of Trajectory

5 結(jié)論

這里針對(duì)移動(dòng)機(jī)器人動(dòng)力學(xué)方程存在的約束違約問(wèn)題，基于常微分方程常見(jiàn)的四階Runge?Kutta 法，探討了Baumgarte 穩(wěn)定性方法的參數(shù)選擇問(wèn)題，主要結(jié)論有：

（1）基于四階Runge?Kutta 法獲得了Baumgarte 穩(wěn)定參數(shù)的存在區(qū)域，為快速選擇合適的Baumgarte穩(wěn)定參數(shù)提供了一種方法。這種方法也可用于其他求解動(dòng)力學(xué)方程的方法。

（2）當(dāng)選擇非穩(wěn)定區(qū)域時(shí)的Baumgarte參數(shù)時(shí)，移動(dòng)機(jī)器人的仿真結(jié)果表明違約現(xiàn)象也得到了較好的改善，盡管達(dá)不到選擇穩(wěn)定區(qū)域時(shí)的Baumgarte 參數(shù)的數(shù)量級(jí)，但也從另一個(gè)角度說(shuō)明Baumgarte違約穩(wěn)定方法的有效性。在此基礎(chǔ)上獲得穩(wěn)定參數(shù)的選擇范圍對(duì)創(chuàng)建精確的動(dòng)力學(xué)模型具有重要意義。