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

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

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

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

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

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

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

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

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

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

2 Baumgarte穩(wěn)定方法

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

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

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

進一步可表示為：

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

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

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

Qc—考慮約束時機器人所需的約束力（矩）。

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

3 Baumgarte參數(shù)的估計

針對一階方程的初值問題［12］：

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

求解初值問題式（7）的顯示單步法：

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

將式（3）改寫為：

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

其中：L1=hBKn，

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

對式（12）進行Z變換得：

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

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

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

4 仿真實例

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

圖2 移動機器人運動模型Fig.2 The Motion Diagram of Mobile Robot

移動機器人運動軌跡為：

根據(jù)文獻［13］的建模方法得移動機器人未受約束時的動力學(xué)方程為：

將移動機器人的運行軌跡視為外部約束并形，如式（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)定性方法修正的動力學(xué)方程：

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

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

仿真結(jié)果，如圖3~圖7所示。圖3和圖5分別表示移動機器人在x方向和y方向的位移示意圖。x未修正和y未修正分別表示沒有采用Baumgarte參數(shù)時的位移。x修正和y修正分別表示采用穩(wěn)定區(qū)域Baumgarte參數(shù)時的位移。x非穩(wěn)定和y非穩(wěn)定分別表示采用非穩(wěn)定區(qū)域Baumgarte參數(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

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

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

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

圖7 運動軌跡對比示意圖Fig.7 The Comparison Diagram of Trajectory

5 結(jié)論

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

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

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