多刚体动力学仿真的李群变分积分算法

黄子恒; 陈菊; 张志娟; 田强; Huang Ziheng; Chen Ju; Zhang Zhijuan; Tian Qiang

网刊加载中。。。

使用Chrome浏览器效果最佳，继续浏览，你可能不会看到最佳的展示效果，

确定继续浏览么?

复制成功，请在其他浏览器进行阅读

多刚体动力学仿真的李群变分积分算法

[] 黄子恒 ¹
[] 陈菊 ¹
[] 张志娟 ²
[] 田强 ¹

1. 北京理工大学宇航学院力学系，北京 100081； 2. 北京空间飞行器总体设计部，北京 100094

中图分类号： O313.7； O302

发布日期：2022-08-23

DOI：10.6052/1672-6553-2021-021

摘要

刚体的构形可用其质心位置和姿态矩阵描述.刚体的位置可以在欧几里得空间中表示，但是其姿态矩阵是在李群上演化的.由于李群独特的非线性性质，基于欧氏空间的多体动力学建模与数值算法难以完全真实地描述系统的动力学特性，特别是长时间历程的动力学特性.本文基于几何力学理论，首先根据离散Hamilton变分原理与离散Legendre变换，建立了多刚体系统的Hamilton体系李群变分积分公式.其次，给出李群变分积分公式的两种离散格式：一般离散格式和RATTLie离散格式.最后，采用这两种不同离散格式构建的算法计算了重力作用下空间刚体双摆的动力学问题，对比研究了算法在保持系统群结构、系统能量等方面的性质.计算结果表明，RATTLie离散格式较一般格式精度更高，且能更好地保持系统群结构与能量.

关键词

离散变分; 李群; 变分积分算法; 多刚体动力学; 系统能量

* 国家自然科学基金资助项目（11832005）

引言

常用多刚体系统动力学建模方法有自然坐标方法^［

1］、欧拉角方法^{［参考文献 2
查找原文}2］、欧拉四元数方法^{［参考文献 3
查找原文}3］、李群李代数方法^{［参考文献 4
查找原文}4］等.自然坐标方法通过若干刚体上的基点以及若干内嵌在刚体上的单位向量表示刚体位姿，采用该方法可以得到含常数质量矩阵的多刚体系统动力学方程.其余常规动力学建模方法则通过刚体质心坐标与刚体的姿态来确定刚体的位姿，以姿态的不同表示方法进行区别.常用的姿态表示方法有欧拉角^{［参考文献 2
查找原文}2］、欧拉四元数^{［参考文献 3
查找原文}3］、李群SO（3）矩阵^{［参考文献 4
查找原文}4］等.欧拉角方法是常用描述刚体姿态的方法，计算简便.描述刚体在三维空间中的运动姿态可采用2类12种欧拉角系统，但无论采用哪种欧拉角系统，都不可避免会含有奇异点^{［参考文献 5
查找原文}5］.欧拉四元数方法用4个有约束关系的参数描述刚体姿态，避免了奇异问题^{［参考文献 5
查找原文}5］.然而，欧拉四元数方法的表述并不直观，内蕴约束方程会影响动力学方程的求解效率，其双值性甚至会导致刚体系统控制中的退绕现象^{［参考文献 6
查找原文}6］.李群方法则采取指数映射进行迭代，使得刚体姿态旋转矩阵始终保持为正交矩阵.李群SO（3）对应的李代数空间so（3）同构于三维欧氏空间R³，因此李代数空间的元素由3个独立的坐标构成.在每一步迭代的过程中李群方法无需考虑多个参数之间的约束关系，有助于提高计算效率.

刚体的动力学应具备守恒性质，如保守系统的能量守恒，动量守恒等.为不与物理规律相悖，守恒律已经成为检验动力学建模与数值算法的重要标准.虽然用李群方法建模的性质优越，但由于李群自身的非线性性质，使得欧氏空间上的常规数值算法在求解多刚体动力学方程时失效，如常规的Runge-Kutta方法不仅无法保证首次积分守恒，还会产生较大的能量耗散与结构误差^［

7］.目前主要用如下两种方法改进已有李群算法的不足：

方法一、采用新数值离散格式离散系统连续动力学方程.该方法通过引入李括号项可使系统的位形空间始终在真实的李群空间中迭代.例如，Munthe-Kaas基于经典Runge-Kutta算法，通过引入校正函数^［

7， 8］提出了一种Runge-Kutta-Munthe-Kaas（RKMK）算法.该算法能保证迭代过程在系统正确的微分流形上进行.Wieloch与Arnold^{［参考文献 9
查找原文}9］将BDF方法与李群方法相结合提出了BLieDF方法，在尽可能不丢失精度的同时使用更少的李括号项，提高了计算效率.Brüls^{［参考文献 3
查找原文}3， 10］基于两种描述刚体位形的李群（SE（3）与ℝ³× SO（3）），结合广义α方法提出了Lie-广义α方法.Lie-广义α方法已被成功应用于多种多体动力学建模与计算，例如：空间曲柄滑块^{［参考文献 11
查找原文}11］，柔性四杆机构^{［参考文献 12
查找原文}12］以及空间捕获飞网^{［参考文献 13
查找原文}13］等.李亚男等^{［参考文献 14
查找原文}14］使用Lie-广义α方法求解指标为1的微分代数方程，使仿真过程中能够同时保持位移约束、速度约束与加速度约束，提高了计算精度.

方法二、李群变分积分算法^［

15］.该算法采用离散Hamilton变分原理建立系统的动力学方程，获得系统的非线性方程组并求解，具有理想的保辛、保动量、保能量等性质.已有研究已构造了多种Hamilton变分过程中Lagrange量的多种离散格式，提出了相应的李代数离散格式下的李群变分积分算法.李群变分积分算法充分结合李群李代数方法与变分积分算法的优势，相较常规变分积分算法不仅能够直接减少参数之间的约束方程，显著提高计算效率，还能保持系统几何结构，提高计算精度.本文将基于差分方法推导的李群变分积分公式称为一般格式李群变分积分公式.一般格式李群变分积分算法已经被应用于空间双连杆机械臂^{［参考文献 16
查找原文}16］、无人潜艇^{［参考文献 17
查找原文}17］、无人机编队^{［参考文献 18
查找原文}18］、轨道控制^{［参考文献 19
查找原文}19， 20］等动力学建模与控制研究中，在解决这些实际问题时表现出了保持系统能量与结构的特性.

进一步，Hante 和Arnold^［

21］通过对数映射改进了以上一般格式算法中的李代数元素离散方式得到RATTLie变分积分算法.该算法在Cosserat柔性梁动力学分析中表现出较好的能量守恒特性.史东华和Zenkov^{［参考文献 22
查找原文}22］通过直接对李代数元素进行变分的方式提出了Hamel场变分积分算法.该方法已被应用于抓取机械臂^{［参考文献 23
查找原文}23］、柔性梁^{［参考文献 24
查找原文}24］等多体动力学与控制研究，展现出了较好的数值特性.Hall和Leok^{［参考文献 25
查找原文}25］提出了李群谱变分积分算法，重力作用下的单摆动力学算例研究表明：采用大积分步长，该方法同样能长时间保持系统能量与数值稳定.随着多自由度复杂约束动力学问题的出现，白龙等^{［参考文献 26
查找原文}26］通过Kelly变换与Newton迭代克服了李群变分积分算法的隐式求解问题，提高了计算效率.Lee等^{［参考文献 27
查找原文}27］提出了多体系统的线性变分积分器，通过离散递归牛顿欧拉算法（Discrete recursive Newton-Euler algorithm）求解残差向量，以及铰接体惯性算法（Articulated body inertia algorithm）计算更新迭代值，提高了计算效率.

本文基于李群李代数的离散Hamilton方程，建立了Hamilton体系下多刚体系统动力学两类李群变分积分算法.分别采用三种算法（一般格式的李群变分积分算法、RATTLie变分积分算法与Lie-广义α算法）计算了重力作用下空间刚体双摆的动力学问题，对比研究了各算法的能量误差、约束违约等特性.研究表明一般格式的李群变分积分算法与RATTLie变分积分算法在长时间保持系统结构、能量等方面存在显著优势，具有潜在工程应用前景，如：航天器轨道动力学问题、大型柔性空间结构在轨服务操作动力学问题等.

1 离散Hamilton体系下的李群变分积分算法

李群变分积分公式是通过离散的Hamilton变分原理得到的动力学方程组，而非对连续的动力学方程组直接进行离散.本文用G表示李群，用 $g$ 表示李代数，用 $g^{*}$ 表示李代数的对偶空间.

首先，用离散作用积分和 $s_{d} = \sum_{n = 0}^{N - 1} L_{d} (g_{n}, f_{n})$ 来近似作用积分 $s = \int_{t_{0}}^{t_{f}} L (g, \dot{g}) d t$ ，其中，N表示初始时刻t₀到最终时刻t_f的时间离散区间总数， $L_{d}$ $(g_{n}, f_{n})$ 表示 $[t_{n}, t_{n + 1})$ 区间的离散拉格朗日量.将离散拉格朗日量 $L_{d}$ $(g_{n}, f_{n})$ 采用Störmer-Verlet格式^［

15］表达为：

\begin{array}{l} L_{d} (g_{n}, f_{n}) = T (f_{n}) - \frac{1}{2} h U (g_{n}) - \frac{1}{2} h U (g_{n + 1}) \\ \approx \int_{t_{n}}^{t_{n + 1}} L (g, \dot{g}) d t \end{array}

（1）

其中 $g_{n}, g_{n + 1} \in G$ 分别表示刚体 $t_{n}$ 和 $t_{n + 1}$ 时刻的位形， $f_{n}$ 表示从 $t_{n}$ 到 $t_{n + 1}$ 时刻李群元素的变化量，满足数值迭代格式 $g_{n + 1} = g_{n} f_{n}$ ， $T (f_{n})$ 为 $[t_{n}, t_{n + 1})$ 区间内系统的动能， $U (g_{n})$ 为 $t_{n}$ 时刻系统势能， $U (g_{n + 1})$ 为 $t_{n + 1}$ 时刻系统的势能， $h = t_{n + 1} - t_{n}$ 为离散时间步长.

根据离散Hamilton变分原理可得到离散的Euler-Lagrange方程，再通过离散的Legendre变换得到Hamilton体系下的李群变分积分公式^［

28］

\begin{array}{l} μ_{n} = - T_{e}^{*} L_{g_{n}} \cdot D_{g_{n}} L_{d} (g_{n}, f_{n}) + A d_{f_{n}^{- 1}}^{*} (T_{e}^{*} L_{f_{n}} \cdot D_{f_{n}} L_{d} (g_{n}, f_{n})) \\ μ_{n + 1} = T_{e}^{*} L_{f_{n}} \cdot D_{f_{n}} L_{d} (g_{n}, f_{n}) \end{array}

（2）

中 $μ_{n}, μ_{n + 1} \in g^{*}$ 表示离散动量， $T_{e}^{*} L_{g_{n}}$ ， $T_{e}^{*} L_{f_{n}}$ 分别表示拉回映射， $A d_{f_{n}^{- 1}}^{*}$ 表示余伴随映射，详细符号含义读者可参考文献［

28］.

2 多刚体系统动力学方程的两种离散格式

2.1　一般离散格式

如图1所示，任一空间刚体运动的位形可用特殊Euclid群 $S E (3) = R^{3} ⋉ S O (3)$ 来表示

S E (3) : \{H = [\begin{matrix} R & x \\ 0 & 1 \end{matrix}] \in ℝ^{4 \times 4}| R \in S O (3), x \in ℝ^{3}\}

（3）

其中，R表示刚体的随体坐标系相对于惯性坐标系的旋转矩阵，x表示刚体质心在惯性坐标系下的位置矢量，SO（3）表示特殊正交群^［

18］.因此，M个多刚体组成的系统位形^{［参考文献 28
查找原文}28］可表示为：

\underset{M}{\underset{︸}{S E (3) \times \cdot \cdot \cdot \times S E (3)}}

.通过对系统刚体编号可以得到：

g_{i, n} = [\begin{matrix} R_{i, n} & x_{i, n} \\ 0 & 1 \end{matrix}], f_{i, n} = [\begin{matrix} F_{i, n} & Y_{i, n} \\ 0 & 1 \end{matrix}] \in S E (3)

（4）

式中，下标i表示刚体编号， $i = 1,2, \cdot \cdot \cdot M$ ，下标n表示 $t_{n}$ 时刻， $n = 0,1, \cdot \cdot \cdot N$ .如图1所示， $g_{i, n}$ 表示刚体i在 $t_{n}$ 时刻位形的李群元素，其中 $R_{i, n}$ 表示 $t_{n}$ 时刻刚体i的随体坐标系O_i-X_iY_iZ_i相对于惯性坐标系O-XYZ的旋转矩阵， $x_{i, n}$ 表示 $t_{n}$ 时刻刚体i的质心在惯性坐标系下的位置矢量. $f_{i, n}$ 表示从 $t_{n}$ 到 $t_{n + 1}$ 时刻表示刚体i位形的李群元素变化量，其中， $F_{i, n}$ 用来描述从 $t_{n}$ 到 $t_{n + 1}$ 时刻刚体i的旋转矩阵变化量， $Y_{i, n}$ 用来描述从 $t_{n}$ 到 $t_{n + 1}$ 时刻刚体i在随体坐标系O_i-X_iY_iZ_i下的质心位置矢量变化量^［

28］.将系统所有刚体的位形用对角分块矩阵表示为

g_{n} = d i a g (g_{1, n}, \cdot \cdot \cdot, g_{M, n}), f_{n} = d i a g (f_{1, n}, \cdot \cdot \cdot, f_{M, n})

（5）

将代入迭代公式 $g_{n + 1} = g_{n} f_{n}$ ，根据可得系统的刚体位形迭代公式为

R_{i, n + 1} = R_{i, n} F_{i, n}, x_{i, n + 1} = x_{i, n} + R_{i, n} Y_{i, n}

（6）

接下来计算多刚体系统的动能项 $T (f_{n})$ .系统的动能等于转动动能与平动动能之和，根据文献［

17］，采用中心差分格式可得到近似的

t_{n + 1 / 2}

时刻刚体i的角速度^{［参考文献 28
查找原文}28］

{\tilde{Ω}}_{i, n + 1 / 2} \approx \frac{1}{h} R_{i, n}^{T} (R_{i, n + 1} - R_{i, n}) = \frac{1}{h} (F_{i, n} - I_{3 \times 3})

（7）

其中 ${\tilde{Ω}}_{i, n + 1 / 2}$ 为李代数空间so（3）元素，该李代数空间与欧氏空间 $ℝ^{3}$ 同构，其同构关系由帽子映射 $\tilde{•} : ℝ^{3} \to$ so（3）， $a \in ℝ^{3} \mapsto \tilde{a} \in$ so（3），给出^［

17］.

图1 多刚体系统构形示意图

Fig.1 Schematic view of a multi-rigid body system configuration

因此，根据与可得到系统在 $[t_{n}, t_{n + 1})$ 区间的总动能为：

T (f_{n}) = \sum_{i = 1}^{M} \frac{1}{h} t r [(I_{3 \times 3} - F_{i, n}) J_{i, d}] + \frac{m_{i}}{2 h} Y_{i, n}^{T} Y_{i, n}

，

（8）

其中 $m_{i}$ 表示刚体i的质量， $J_{i, d}$ 表示刚体i的非标准惯性张量矩阵，其与常用的惯性张量矩阵J关系^［

18］为：

J_{d} = \frac{1}{2} t r (J) I_{3 \times 3} - J

，“tr”表示矩阵的迹.

其次，对于完整约束力学系统，可将约束项与拉格朗日乘子乘积直接作为势能项处理，此时势能项由重力势能与约束项势能组成.设多刚体系统的约束方程共有m个，则约束方程与对应的拉格朗日乘子可分别表达为：

\begin{array}{l} Φ_{n} = {[\begin{matrix} Φ_{1, n}^{T} & \cdot \cdot \cdot & Φ_{m, n}^{T} \end{matrix}]}^{T}, \\ λ_{n} = {[\begin{matrix} λ_{1, n}^{T} & \cdot \cdot \cdot & λ_{m, n}^{T} \end{matrix}]}^{T} \end{array}

（9）

中 $Φ_{j, n}$ 与 $Φ_{j, n + 1}$ 分别表示 $t_{n}, t_{n + 1}$ 时刻编号j的约束方程， $λ_{j, n}$ 与 $λ_{j, n + 1}$ 分别表示 $t_{n}, t_{n + 1}$ 时刻编号j的拉格朗日乘子.

通过推导，可进一步得到 $t_{n}$ 与 $t_{n + 1}$ 时刻受约束多刚体系统的势能为：

\begin{matrix} \begin{array}{l} U_{n}^{C} = U^{C} (g_{n}) = U (g_{n}) + \sum_{j = 1}^{m} λ_{j, n}^{T} Φ_{j, n} \\ = \sum_{i = 1}^{M} (- m_{i} g e_{3}^{T} x_{i, n}) + \sum_{j = 1}^{m} λ_{j, n}^{T} Φ_{j, n} \end{array} \\ \begin{array}{l} U_{n + 1}^{C} = U^{C} (g_{n} f_{n}) = U (g_{n} f_{n}) + \sum_{j = 1}^{m} λ_{j, n + 1}^{T} Φ_{j, n + 1} \\ = \sum_{i = 1}^{M} (- m_{i} g e_{3}^{T} x_{i, n + 1}) + \sum_{j = 1}^{m} λ_{j, n + 1}^{T} Φ_{j, n + 1} \end{array} \end{matrix}

（10）

其中 $e_{3}$ 为重力方向的单位矢量.上标“C”表示受约束的多刚体系统.

最后，根据离散Hamilton变分原理与离散Legendre变换，得到多刚体系统一般格式李群变分积分公式^［

28］

Π_{i, n} = \frac{1}{h} α (f_{i, n}) J_{i} f_{i, n} + \frac{1}{h} β (f_{i, n}) {\tilde{f}}_{i, n} J_{i} f_{i, n} - \frac{1}{2} h M_{i, n}^{C}

，

（11）

Π_{i, n + 1} = F_{i, n}^{T} Π_{i, n} + \frac{1}{2} h F_{i, n}^{T} M_{i, n}^{C} + \frac{1}{2} h M_{i, n + 1}^{C}

，

（12）

x_{i, n + 1} = x_{i, n} + \frac{h}{m_{i}} γ_{i, n} - \frac{h^{2}}{2 m_{i}} \frac{\partial U_{n}^{C}}{\partial x_{i, n}}

，

（13）

γ_{i, n + 1} = γ_{i, n} - \frac{1}{2} h \frac{\partial U_{n}^{C}}{\partial x_{i, n}} - \frac{1}{2} h \frac{\partial U_{n + 1}^{C}}{\partial x_{i, n + 1}}

，

（14）

与中 $Π_{i, n}$ $, Π_{i, n + 1}$ 分别为 $t_{n}$ 和 $t_{n + 1}$ 时刻刚体i的体角动量.与中，平动部分用惯性坐标系表示， $γ_{i, n}, γ_{i, n + 1}$ 为 $t_{n}$ 和 $t_{n + 1}$ 时刻刚体i在惯性坐标系中的线动量^［

28］.与中的

{\tilde{M}}_{i, n}^{C} = {(\frac{\partial U_{n}^{C}}{\partial R_{i, n}})}^{T} R_{i, n} - R_{i, n}^{T} \frac{\partial U_{n}^{C}}{\partial R_{i, n}}

.根据指数映射公式^{［参考文献 17
查找原文}17］

F_{i, n} = e x p ({\tilde{f}}_{i, n}) = I_{3 \times 3} + α (f_{i, n}) {\tilde{f}}_{i, n} + β (f_{i, n}) {\tilde{f}}_{i, n} {\tilde{f}}_{i, n}

，可推导得到中

α (f_{i, n}) = \frac{s i n (‖f_{i, n}‖)}{‖f_{i, n}‖}

，

β (f_{i, n}) = \frac{1 - c o s (‖f_{i, n}‖)}{{‖f_{i, n}‖}^{2}}

2.2　RATTLie离散格式

RATTLie变分积分算法与上述一般离散格式算法的区别，在于对李代数元素的离散方式.表示一般离散格式的角速度 $Ω_{i, n + 1 / 2}$ ，而RATTLie离散格式的角速度 $Ω_{i, n + 1 / 2}$ 与惯性坐标系下质心的线速度 ${\dot{x}}_{i, n + 1 / 2}$ 分别为^［

21］

\begin{array}{l} {\tilde{Ω}}_{i, n + 1 / 2} = \frac{1}{h} l o g_{S O (3)} (R_{i, n}^{T} R_{i, n + 1}), {\dot{x}}_{i, n + 1 / 2} \\ = \frac{1}{h} (x_{i, n + 1} - x_{i, n}) \end{array}

，

（15）

其中 $l o g_{S O (3)} : G \to g, q \mapsto l o g (q) = e x p^{- 1} (q)$ 称为对数映射^［

21］，是指数映射的逆映射.对于积分

[t_{n}, t_{n + 1})

区间，RATTLie变分积分算法与一般离散格式动能项表达也不同，为

T (R_{i, n}, x_{i, n}, R_{i, n + 1}, x_{i, n + 1}) = \sum_{i = 1}^{N} \frac{1}{h} t r [l o g_{S O (3)} (R_{i, n}^{T} R_{i, n + 1}) J_{i, d} l o g_{S O (3)} (R_{i, n + 1}^{T} R_{i, n})] + \frac{m_{i}}{2 h} {(x_{i, n + 1} - x_{i, n})}^{T} (x_{i, n + 1} - x_{i, n})

（16）

根据离散Hamilton变分原理，得到RATTLie离散格式下的多刚体系统离散Euler-Lagrange方程组^［

21］

\{\begin{array}{l} \begin{array}{l} T^{- T} (- h Ω_{i, n + 1 / 2}) J_{i} Ω_{i, n + 1 / 2} - T^{- T} (h Ω_{i, n - 1 / 2}) \\ J_{i} Ω_{i, n - 1 / 2} - h {\tilde{M}}_{i, n}^{} + h \sum_{j = 1}^{m} D_{R_{i, n}} Φ_{j, n}^{T} λ_{j, n}^{-} = 0 \end{array} \\ \begin{array}{l} m_{i} {\dot{x}}_{i, n + 1 / 2} - m_{i} {\dot{x}}_{i, n - 1 / 2} + h \frac{\partial U_{n}}{\partial x_{i, n}} + \\ h \sum_{j = 1}^{m} D_{x_{i, n}} Φ_{j, n}^{T} λ_{j, n}^{-} = 0 \end{array} \\ Φ_{n + 1} (R_{i, n + 1}, x_{i, n + 1}) = 0 \end{array}

，

（17）

其中 $U_{n}$ 表示不考虑约束的 $t_{n}$ 时刻的系统势能，T为切映射矩阵， $M_{i, n}^{}$ 为编号为i刚体的广义力矩.根据文献［

21］，计算切映射矩阵T，

M_{i, n}^{}

表达式分别为

T^{- T} (a) = I_{3 \times 3} - \frac{1}{2} \tilde{a} + \frac{1 - γ (a)}{{‖a‖}^{2}} \tilde{a} \tilde{a}, a \in ℝ^{3}

，

（18）

{\tilde{M}}_{i, n} = {(\frac{\partial U_{n}^{}}{\partial R_{i, n}})}^{T} R_{i, n} - R_{i, n}^{T} \frac{\partial U_{n}^{}}{\partial R_{i, n}}

（19）

其中 $γ (a) = \frac{‖a‖}{2} c o t \frac{‖a‖}{2}$ .令 $a = - h Ω_{i, n + 1 / 2}$ ， $h Ω_{i, n - 1 / 2}$ 并代入，可求出 $T^{- T} (- h Ω_{i, n + 1 / 2})$ 与 $T^{- T} (h Ω_{i, n - 1 / 2})$ 的表达式.

中， $D_{R_{i, n}} Φ_{j, n}^{}$ ， $D_{x_{i, n}} Φ_{j, n}^{}$ 分别表示 $t_{n}$ 时刻约束j关于刚体i的转动项，平动项的Jacobi矩阵，其中符号 $D_{R_{i, n}} Φ_{j, n}^{}$ 表示约束 $Φ_{j, n}^{}$ 关于 $R_{i, n}$ 的求导运算， $D_{x_{i, n}} Φ_{j, n}^{}$ 表示约束 $Φ_{j, n}^{}$ 关于 $x_{i, n}$ 的求导运算，详细运算定义读者可参考文献［

28］.令

{\tilde{η}}_{i, n} = R_{i, n}^{T} δ R_{i, n}

，

{\tilde{η}}_{i, n} \in

so（3）为描述转动项变分的元素，约束关于转动项与平动项的Jacobi矩阵

D_{R_{i, n}} Φ_{j, n}

与

D_{x_{i, n}} Φ_{j, n}

的关系为

\sum_{i = 1}^{M} (D_{R_{i, n}} Φ_{j, n} η_{i, n} + D_{x_{i, n}} Φ_{j, n} δ x_{i, n}) = δ Φ_{j, n}

（20）

值得注意的是，与平动部分 $δ x_{i, n} \in ℝ^{3}$ 不同，转动部分 $R_{i, n} \in S O (3)$ 的变分需在李代数空间so（3）刻画^［

28］.中

D_{R_{i, n}} Φ_{j, n}

，

D_{x_{i, n}} Φ_{j, n}

分别表示对约束项

Φ_{j, n}^{}

变分后所得到的对应转动部分

η_{i, n}

与平动部分

δ x_{i, n}

的系数矩阵.RATTLie变分积分算法的李群元素迭代格式为

\begin{array}{l} R_{i, n + 1} = R_{i, n} e x p (h {\tilde{Ω}}_{_{i, n + 1 / 2}}), \\ x_{i, n + 1} = x_{i, n} + h x_{i, n + 1 / 2} \end{array}

（21）

为了使系统约束的速度违约更小，引入新Lagrange乘子 $λ_{j, n + 1}^{-}$ 与 $λ_{j, n + 1}^{+}$ 以更好地满足速度约束，注意 $λ_{j, n + 1}^{+} \neq λ_{j, n + 1}^{-}$ .于是对进行离散的Legendre变换得到RATTLie离散格式下的李群变分积分公式.根据文献［

21］，RATTLie方法将一步

[t_{n}, t_{n + 1}]

积分区间细分为两个积分子步

[t_{n}, t_{n + 1 / 2}]

与

[t_{n + 1 / 2}, t_{n + 1}]

，在区间

[t_{n}, t_{n + 1 / 2}]

的迭代公式为

\{\begin{array}{l} \begin{array}{l} J_{i} Ω_{i, n} = T^{- T} (- h Ω_{i, n + 1 / 2}) J_{i} Ω_{i, n + 1 / 2} - \frac{1}{2} h {\tilde{M}}_{i, n}^{} \\ + \frac{1}{2} h \sum_{j = 1}^{m} D_{R_{i, n}} Φ_{j, n}^{T} λ_{j, n}^{-} \end{array} \\ \begin{array}{l} m_{i} {\dot{x}}_{i, n} = m_{i} {\dot{x}}_{i, n + 1 / 2} + \frac{1}{2} h \frac{\partial U_{n}}{\partial x_{i, n}} \\ + \frac{1}{2} h \sum_{j = 1}^{m} D_{x_{i, n}} Φ_{j, n}^{T} λ_{j, n}^{-} \end{array} \\ Φ_{n + 1} (R_{i, n + 1}, x_{i, n + 1}) = 0 \end{array}

，

（22）

在 $[t_{n + 1 / 2}, t_{n + 1}]$ 区间的迭代公式为

\{\begin{array}{l} \begin{array}{l} J_{i} Ω_{i, n + 1} = T^{- T} (h Ω_{i, n + 1 / 2}) J_{i} Ω_{i, n + 1 / 2} + \frac{1}{2} h {\tilde{M}}_{i, n + 1}^{} \\ - \frac{1}{2} h \sum_{j = 1}^{m} D_{R_{i, n + 1}} Φ_{j, n + 1}^{T} λ_{j, n + 1}^{+} \end{array} \\ \begin{array}{l} m_{i} {\dot{x}}_{i, n + 1} = m_{i} {\dot{x}}_{i, n + 1 / 2} - \frac{1}{2} h \frac{\partial U_{n + 1}}{\partial x_{i, n + 1}} - \\ \frac{1}{2} h \sum_{j = 1}^{m} D_{x_{i, n + 1}} Φ_{j, n + 1}^{T} λ_{j, n + 1}^{+} \end{array} \\ {\dot{Φ}}_{n + 1} (Ω_{i, n + 1}, {\dot{x}}_{i, n + 1}) = 0 \end{array}

（23）

因此，RATTLie方法在一步时间积分区间 $[t_{n}, t_{n + 1}]$ 的迭代过程为：首先根据 $t_{n}$ 时刻的 $Ω_{i, n}$ ， ${\dot{x}}_{i, n}$ 求解中的 $Ω_{i, n + 1 / 2}$ ， ${\dot{x}}_{i, n + 1 / 2}$ 与 $λ_{j, n}^{-}$ ；然后根据计算得到 $R_{i, n + 1}$ ， $x_{i, n + 1}$ ；最后求解中 $t_{n + 1}$ 时刻的 $Ω_{i, n + 1}$ ， ${\dot{x}}_{i, n + 1}$ 与 $λ_{j, n + 1}^{+}$ .

3 数值计算与分析

考察如图2所示的重力作用下的空间刚体双摆动力学特性.两根摆均为圆柱刚杆，杆1（OA）与杆2（AB）的质心为O₁，O₂，两根相同杆的基本参数为：密度 $ρ = 7850 k g \cdot m^{- 3}$ ，底部半径为 $r = 0.05 m$ ，母线长为 $l = 1 m$ .杆1与杆2质量为 $m_{1} = m_{2} = 61.6538 k g$ ，杆1与杆2的惯性张量矩阵为 $J_{1} = J_{2} = [\begin{matrix} 5.1763 & 0 & 0 \\ 0 & 0.0771 & 0 \\ 0 & 0 & 5.1763 \end{matrix}] k g \cdot m^{2}$ .初始时刻 $t_{0} = 0$ 时空间双摆模型状态如图2虚线所示，杆1一端球铰约束在固定点O，杆1与杆2由A处球铰连接.初始时两摆呈垂直关系，杆1的位形由位姿矩阵 $R_{1,0} = I_{3 \times 3}$ 与质心坐标 $x_{1,0} = {[\begin{matrix} 0 & 0.5 & 0 \end{matrix}]}^{T}$ 确定，杆2的位形由位姿矩阵 $R_{2,0} = [\begin{matrix} 0 & - 1 & 0 \\ 1 & 0 & 0 \\ 0 & 0 & 1 \end{matrix}]$ 与质心坐标 $x_{2,0} = {[\begin{matrix} - 0.5 & 1 & 0 \end{matrix}]}^{T}$ 确定.初始杆1与杆2的体角速度 $Ω_{1,0} = Ω_{2,0} = {[\begin{matrix} 0 & 0 & 0 \end{matrix}]}^{T}$ ，初始杆1与杆2的质心平动速度 ${\dot{x}}_{1,0} = {\dot{x}}_{2,0} = {[\begin{matrix} 0 & 0 & 0 \end{matrix}]}^{T}$ .

图2 重力作用下的空间双摆模型示意图

Fig.2 Schematic view of a spatial double pendulum under the gravity action

杆1的局部坐标系O₁-X₁Y₁Z₁与杆2的局部坐标系O₂-X₂Y₂Z₂如图2所示，两杆运动过程中只考虑重力影响，设经过时间 $t_{n}$ 后杆1可到达 $O A^{'}$ 处，杆2可到达 $A^{'} B^{'}$ 处.

双摆系统有两处约束，O处球铰为约束1，A处球铰为约束2，综合写为

\begin{array}{l} Φ = {[\begin{matrix} Φ_{1}^{T} & Φ_{2}^{T} \end{matrix}]}^{T} \\ \begin{array}{l} Φ_{1} = R_{1, n + 1} X_{11} + x_{1, n + 1}, Φ_{2} = R_{1, n + 1} X_{12} + \\ x_{1, n + 1} - R_{2, n + 1} X_{21} - x_{2, n + 1} \end{array} \end{array}

（24）

中的 $X_{i j}$ 为约束j处的球铰到摆i的质心的位置向量，本算例中 $X_{11} = {[\begin{matrix} 0 & - 0.5 & 0 \end{matrix}]}^{T}$ ， $X_{12} = {[\begin{matrix} 0 & 0.5 & 0 \end{matrix}]}^{T}$ ， $X_{21} = {[\begin{matrix} 0 & - 0.5 & 0 \end{matrix}]}^{T}$ .表1给出了双摆对应的约束Jacobi矩阵表达.

采用Lie-广义α方法计算时，算法谱半径选取为0.9.另外两类Hamilton体系变分积分算法则直接求解非线性方程组.Lie-广义α方法，一般格式的李群变分积分算法与RATTLie变分积分算法均使用10^-3步长进行计算.采用商业软件Recurdyn分别使用10^-4、10^-5两种步长进行计算.用“Lie-alpha”表示Lie-广义α方法，“DH”表示一般格式的李群变分积分算法，“RL”表示RATTLie变分积分算法.仿真总时间设为50s，使用Matlab进行编程，在一台具有Intel Core i7-7700 3.6GHz处理器及16GB RAM的PC机上运行.

图3为以上所有方法摆2的质心O₂点位移矢量的X轴方向分量结果，图4为以上所有方法杆2的角速度矢量绕O₂X₂轴方向的分量结果.根据图3与图4可知：当步长为10^-4时，商业软件Recurdyn在4s以后计算结果逐渐发散；当步长为10^-5s 时，Lie-广义α方法与李群变分积分算法（DH， RL）的所得结果与商业软件Recurdyn计算结果几乎重合，说明了本文建立的建模与计算方法的正确性.

图3 杆2质心O₂点位移矢量的X轴方向分量

Fig.3 X-component of the second pendulum’s mass center O₂

图4 杆2角速度矢量绕O₂X₂轴方向分量

Fig.4 Component of the second pendulum’s angular velocity about axis-O₂X₂

图5为DH方法、RL方法与Lie-广义α方法的能量波动情况对比图，其中图5（b）为图3（a）在7.5~10s的放大图.由图5可知：1、Lie-广义α方法的能量波动远远大于李群变分积分算法.2、当步长选取为10^-3 时，DH方法已经远远优于步长为10^-4的商业软件方法，可知DH方法在计算过程中耗散极低基本保持稳定.3、RL方法在能量保持方面优于DH方法，可知数值离散的高精度格式可以提高计算精度.

图5 空间双摆的能量变化曲线对比图：（a） 0~50s （b） 7.5~10s

Fig.5 Comparison of the spatial double pendulum’s energy variations ：（a） 0~50s （b） 7.5~10s

图6为Lie-广义α方法，DH方法，RL方法的SO（3）正交性误差曲线对比图.SO（3）群元素R需要满足正交性条件，而 $‖I_{3 \times 3} - R R^{T}‖$ 表示SO（3）群元素R的正交性误差，可以反映算法迭代过程中的群结构保持情况.从图6中明显可以看出，三种方法的正交性误差保持量级均在10^-14，算法使得系统的李群结构保持很好.

图6 空间双摆SO（3）正交性误差曲线对比图

Fig. 6 Comparison of the spatial double pendulum’s SO（3） orthogonality error curves

图7给出了DH方法与离散Euler-Lagrange方程建模方法（DL方法）、Lie-广义α方法的A处球铰的速度约束误差对比曲线.从图7可以看出，由于Lie-广义α方法与离散Euler-Lagrange方程组^［

14］并未对速度进行违约校正，速度误差的量级已经达到10^-4.DH方法引入速度约束方程后速度误差量级为1e^-16，显著改善了约束违约情况.引入速度约束方程后，基于Hamilton体系的RATTLie算法对系统约束保持情况也将显著改善.

图7 空间双摆速度约束违约曲线对比图

Fig.7 Comparison of the spatial double pendulum’s velocity constraint violation curves

4 结论

基于离散变分原理建立了多刚体动力学模型的一般格式李群变分积分算法和RATTLie变分积分算法.通过算例对比分析发现：一般格式的李群变分积分算法和RATTLie变分积分算法具有保能量、保结构的性质.在积分步长选取较大时，该方法远远优于步长较小的商业软件的计算结果；RATTLie方法的系统能量保持特性优于一般格式算法；采用Hamilton体系的李群变分积分算法相比离散Lagrange体系的算法能量波动范围更小，约束违约更小.后续可进一步研究这类算法的并行计算问题以及基于这类算法的多柔体动力学与控制问题，特别是在轨大型柔性空间结构的组装过程动力学与控制问题.

参考文献

García de Jalón J. Twenty-five years of natural coordinates. Multibody System Dynamics， 2007， 18（1）：15~33

Shah S V， Saha S K， Dutt J K. Denavit-Hartenberg parameterization of Euler angles. Journal of Computational & Nonlinear Dynamics， 2012， 7（2）：021006

Sherif K， Nachbagauer K， Steiner W， et al. A modified HHT method for the numerical simulation of rigid body rotations with Euler parameters. Multibody System Dynamics， 2019， 46， 181~202

Arnold M， Brüls O， Cardona A. Error analysis of generalized-α Lie group time integration methods for constrained mechanical systems. Numerische Mathematik， 2015， 129（1）： 149~179

徐小明，钟万勰. 四元数与欧拉角刚体动力学数值积分算法及其比较. 计算机辅助工程， 2014， 23（1）： 59~63

Xu Xiaoming， Zhong Wanxie. Numerical integration algorithms and comparison for rigid dynamics in terms of quaternion and Euler angle. Computer Aided Engineering， 2014， 23（1）： 59~63（in Chinese）

Bhat S P， Bernstein D S. A topological obstruction to continuous global stabilization of rotational motion and the unwinding phenomenon. Systems and Control Letters， 2000， 39（1）：63~70

Munthe-Kaas H. Runge-Kutta methods on Lie groups. Bit Numerical Mathematics， 1998， 38（1）： 92~111

Munthe-Kaas H， Stern A， Verdier O. Invariant connections， Lie algebra actions and foundations of numerical integration on manifolds. Society for Industrial and Applied Mathematics， 2020， 4（1）：49~68

Wieloch V， Arnold M. BDF integrators for constrained mechanical systems on Lie groups. Journal of Computational and Applied Mathematics， 2019： 112517

Sonneville V. A geometric local frame approach for flexible multibody systems. ［Ph. D Thesis］. Belgium： Université de Liège， 2015

Sonneville V， Brüls O. A formulation on the Special Euclidean Group for dynamic analysis of multibody systems. Journal of Computational and Applied Mathematics， 2014

Brüls O， Cardona A， Arnold M. Lie group generalized-α time integration of constrained flexible multibody systems. Mechanism and Machine Theory， 2012， 48（1）：121~137

刘铖，胡海岩.基于李群局部标架的多柔体系统动力学建模与计算.力学学报，2021，53（1）：213~233

Liu C， Hu H Y. Dynamic modeling and computation for flexible multibody systems based on the local frame of Lie group. Chinese Journal of Theoretical and Applied Mechanics， 2021，53（1）：213~233（in Chinese）

李亚男，李博文，丁洁玉，等.多体系统动力学Lie群微分-代数方程约束稳定方法. 动力学与控制学报， 2018，16（2）：97~101

Li Y N， Li B W， Ding J Y， et al. Constraints stabilization method for DAEs on Lie group of multibody system dynamic. Journal of Dynamics and Control. 2018， 16（2）：97~101（in Chinese）

Marsden J E， West M. Discrete mechanics and variational integrators. Acta Numerica， 2001，10（1）：357-514

李亚男.基于李群的多体系统动力学仿真［硕士学位论文］.山东：青岛大学， 2019

Dynamic simulation of multibody system based on Lie group ［Master Thesis］. Shandong： Qingdao University， 2019 （in Chinese）

黄彬. 变分积分子在非线性系统中的一些应用［硕士学位论文］. 北京：北京理工大学， 2011

Huang B. Some applications of variational integrator to the nonlinear systems. ［Master Thesis］. Beijing： Beijing Institute of Technology， 2011 （in Chinese）

杨盛庆. 基于几何力学与最优控制的无人机编队方法研究［博士学位论文］. 北京：北京理工大学， 2014

Yang S Q. Geometric mechanics and optimal control of UAvs Formation ［Ph. D Thesis］. Beijing： Beijing Institute of Technology， 2014 （in Chinese）

Lee T， Leok M， McClamroch N H. Lie group variational integrators for full body problem. Celestial Mechanics and Dynamical Astronomy， 2007， 196： 2907-2924

Lee T， Leok M， McClamroch N H. Lie group variational integrators for full body problem in orbital mechanics. Celestial Mechanics and Dynamical Astronomy， 2007，98（2）：121~144

Hante S， Arnold M. RATTLie： A variational Lie group integration scheme for constrained mechanical systems. Journal of Computational and Applied Mathematics， 2019，387：112492

Shi D H， Zenkov D V， Bloch A M. Hamel's formalism for classical field theories. Journal of Nonlinear Science， 2020， 30（1）：1307~1353

安志朋. Hamel场变分积分子及其应用［博士学位论文］. 北京：北京理工大学， 2020

An Zhipeng. The field-theoretic Hamel’s variational integrator and its applications. ［Ph. D Thesis］. Beijing： Beijing Institute of Technology，2020 （in Chinese）

王亮. 几何精确梁的Hamel场变分积分子. ［硕士论文］. 北京：北京理工大学， 2016

Wang L. Hamel’s field variational integrator of geometrically exact beam ［Master Thesis］. Beijing： Beijing Institute of Technology， 2016 （in Chinese）

Hall J， Leok M. Lie group spectral variational integrators. Foundations of Computational Mathematics， 2017， 17（1）： 199-257

白龙，董志峰，戈新生. 基于Lie群的刚体动力学建模及数值计算方法研究. 应用数学和力学， 2015， 36（8）：833-843

Bai L， Dong Z F， Ge X S. Lie group and Lie algebra modeling for numerical calculation of rigid body dynamics. Applied Mathematics and Mechanics， 2015， 36（8）：833~843

Lee J， Liu C K， Park F C， et al. A linear-time variational integrator for multibody systems. In： Goldberg K， Abbeel P， Bekris K eds. Algorithmic Foundations of Robotics XII， Proceedings of the Twelfth Workshop on the Algorithmic Foundations of Robotics， WAFR2016， San Francisco， 2016-12-18-20（Springer， 2020，352~367）

Lee T. Computational geometric mechanics and control of rigid bodies. ［Ph.D Thesis］. Michigan： University of Michigan， 2008

多刚体动力学仿真的李群变分积分算法

摘要

关键词

引言

1 离散Hamilton体系下的李群变分积分算法

2 多刚体系统动力学方程的两种离散格式

2.1　一般离散格式

2.2　RATTLie离散格式

3 数值计算与分析

4 结论

参考文献

微信公众号二维码

手机版网站二维码

多刚体动力学仿真的李群变分积分算法

摘要

关键词

引 言

1 离散Hamilton体系下的李群变分积分算法

2 多刚体系统动力学方程的两种离散格式

2.1 一般离散格式

2.2 RATTLie离散格式

3 数值计算与分析

4 结 论

参考文献

微信公众号二维码

手机版网站二维码

引言

2.1　一般离散格式

2.2　RATTLie离散格式

4 结论