移动机器人动力学方程的约束违约稳定方法

刘 佳，杨 凯，杨 蓓，杨伟超

（石家庄学院机电学院，河北石家庄 050035）

约束多体系统的动力学模型常被处理为若干微分方程与约束方程组成的方程组进行求解处理。理论上讲，如果存在解析解，此解在满足加速度层面约束方程的同时，也应该满足速度层面约束方程和位置层面约束方程。

然而，在数值求解时，由于数值近似和截断误差等的影响，数值解将仅仅能满足加速度层面约束方程，而严重偏离位置约束方程和速度约束方程，出现约束违约现象。

对于移动机器人来讲，由于其存在的非完整约束，使得建立其解析形式的动力学方程具有一定的困难。而基于分析力学界经典的Udwadia−Kalaba方程建立的解析形式的动力学方程依然存在约束违约［1］。

其原因除数值近似和截断误差等因素外，将移动机器人本身固有的非完整约束方程融入到质量矩阵而导致的矩阵奇异也应该是不可忽略的因素。

当前，处理约束违约的方法主要有两种：（1）约束违约消除方法［2］；（2）约束违约稳定性方法［3］。

约束违约消除方法是将满足约束方程的解视为约束流形上的映射，通过将由动力学方程积分得到的广义坐标和广义速度投影到约束流形上，使广义坐标和广义速度满足约束方程从而实现违约校正。其中代表性的应用是：文献［4−5］分别采用几何投影法对空间曲柄滑块机构和牛头刨床机构做了分析研究，并取得满意效果。文献［6］基于改进Udwadia−Kalaba方程建立了双臂协调机械手的动力学方程，约束违约得到一定程度的抑制。但由于仅考虑了忽视速度层面约束方程对约束违约的影响，因此结果仍有待改善。

约束违约稳定性方法通过引入包含约束方程的修正项以达到减小甚至消除约束漂移的目的。这其中最著名的是Baumgarte稳定方法。然而，Baumgarte稳定性方法需要合理选择修正项中涉及到的参数，这是一件比较困难的事情［7］。尤其当系统具有多个约束方程时，稳定参数的选择就成为一个单调、繁琐的试错过程。

基于此，这里利用Baumgarte稳定方法，基于求解常微分方程常用的四阶Runge−Kutta法探讨稳定参数的选择问题，并将其应用到基于Udwadia−Kalaba方程建立的移动机器人解析动力学方程上以验证其有效性。

2 Baumgarte稳定方法

系统的约束方程可表示为以下形式：

式中：q—系统的广义变量；t—时间变量；

p—系统约束方程的个数。

进一步可表示为：

根据Baumgarte稳定方法，将加速度层面约束方程改写为：

基于Udwadia−Kalaba 方程的［8−10］机器人解析动力学方程可表示为：

式中：Q—不考虑约束时系统所受的约束力（矩）；

Qc—考虑约束时机器人所需的约束力（矩）。

基于Baumgarte 稳定方法修正的Udwadia—Kalaba 方程［11］，可得：

3 Baumgarte参数的估计

针对一阶方程的初值问题［12］：

所谓数值解法，就是寻求解y(x)在一系列离散节点x1

求解初值问题式（7）的显示单步法：

这里ci，λi，μij均为常数，式（8）称为r级显式Runge−Kutta法。当r= 4时，即为经典四阶Runge−Kutta法：

将式（3）改写为：

采用经典的四阶Runge−Kutta法求解式（10），由式（9）得：

其中：L1=hBKn，

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

对式（12）进行Z变换得：

系统的特征方程为：

根据控制理论的相关知识可知，当 |Z|≤1时和Φ将会收敛到0。当 |Z|≤1时的选取点，如图1所示。根据图1，并结合式（15），一旦指定步长h，则Baumgarte参数即可选定。

图1 的稳定区域Fig.1 Stability Region in the Plane

4 仿真实例

移动机器人运动模型，如图2 所示。Oxyz为大地坐标系。xpPyp为移动机器人平台上固定点P的局部坐标系，P点坐标为(x，y)，xp与x之间的夹角为θ。P点与移动机器人质心C点之间的距离为a= 0.145m。两侧驱动轮相距2d= 0.8m，驱动轮半径为r= 0.08m。移动机器人质量为m1= 6.32kg，转动惯量Iz=0.063kg.m2。每个驱动轮质量为m2= 0.31kg，转动惯量为Il=Iz=0.001kg.m2。移动机器人广义坐标为θ=[x，y，θ，θl，θr]，其中θl，θr分别为左右驱动轮的转角。

图2 移动机器人运动模型Fig.2 The Motion Diagram of Mobile Robot

移动机器人运动轨迹为：

根据文献［13］的建模方法得移动机器人未受约束时的动力学方程为：

将移动机器人的运行轨迹视为外部约束并形，如式（2）的形式：

假设初始条件为：x( 0 )= 0m，y( 0 )=−4m，θ( 0 )=θl( 0 )=θr( 0 )= 0rad，̇( 0 )= 3m/s，ẏ( 0 )= 0m/ṡ( 0 )= 1.33rad/s，̇( 0 )=( 0 )= 0rad/s。由式（6）可得基于Baumgarte 稳定性方法修正的动力学方程：

为验证所提方法的有效性，根据图1选择的三组数据，如表1所示。

表1 不同稳定参数表Tab.1 The Table of Different Stability Parameters

仿真结果，如图3~图7所示。图3和图5分别表示移动机器人在x方向和y方向的位移示意图。x未修正和y未修正分别表示没有采用Baumgarte参数时的位移。x修正和y修正分别表示采用稳定区域Baumgarte参数时的位移。x非稳定和y非稳定分别表示采用非稳定区域Baumgarte参数时的位移。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参数时的位移存在明显的约束违约，如图5所示。

移动机器人在x方向和y方向的位移误差示意图，如图4、图6所示。其中，error1=x−3 sint表示x方向的位移误差；error2=y+ 4 cost表示y方向的位移误差。下标文字含义与上述相同。图4、图6均表明没有采用Baumgarte参数时的位移误差随时间逐渐增大，采用非稳定区域Baumgarte参数时的位移误差则得到了较好的控制，而采用稳定区域Baumgarte参数时的位移误差则得到了很好的改善，x方向误差数量级为10−4，y方向误差数量级为10−3。移动机器人运动轨迹示意图，如图7所示。

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

图7 运动轨迹对比示意图Fig.7 The Comparison Diagram of Trajectory

5 结论

这里针对移动机器人动力学方程存在的约束违约问题，基于常微分方程常见的四阶Runge−Kutta 法，探讨了Baumgarte 稳定性方法的参数选择问题，主要结论有：

（1）基于四阶Runge−Kutta 法获得了Baumgarte 稳定参数的存在区域，为快速选择合适的Baumgarte稳定参数提供了一种方法。这种方法也可用于其他求解动力学方程的方法。

（2）当选择非稳定区域时的Baumgarte参数时，移动机器人的仿真结果表明违约现象也得到了较好的改善，尽管达不到选择稳定区域时的Baumgarte 参数的数量级，但也从另一个角度说明Baumgarte违约稳定方法的有效性。在此基础上获得稳定参数的选择范围对创建精确的动力学模型具有重要意义。