基于Euler-poincare方程高效率的机械多体系统动力学建模

李炳志  王爽

摘  要：伪速度的引入导致了拉氏方程的广义化，使得通过欧拉-庞卡莱方程建立多体系统的动力学模型，实现了动力学的全局描述，并保持了潜在理论结构和拉氏方程的优点。并且其惯量矩阵相对简单，对于完整或非完整约束系统都适用。空间算子代数中的移位算子结合了关联矩阵、通路矩阵和低序体阵列的特点，而且形式更简单，更容易在计算机上实现，最重要的是它包含了运动学和动力学递推关系。将其引入动能函数的计算，提高了效率，避免了重复计算、交叉运算。最后给出了算例及仿真。

关键词：伪速度；欧拉-庞卡莱方程；移位算子；高效率；动力学模型

中图分类号：TP241  文献标识码：A  文章编号：2096-4706（2023）24-0147-06

High Efficient Dynamics Modeling of Mechanical Multibody System Based On

Euler-Poincare Equation

HAO Yongjiang1， LI Bingzhi2， WANG Shuang3

（1.Kunshan Key Industry Development and Promotion Center， Suzhou  215300， China; 2.China National Heavy Duty Truck Group Co.， Ltd.， Ji'nan  250000， China; 3.Nanjing University of Information Science & Technology， Nanjing  210044， China）

Abstract： The introduction of quasi-velocities leads to a generalization of Lagrange's equation， and the dynamics model of multi-body system established by Euler-Poincare equation achieves global description of the dynamics， and keeps the advantages of underlying theoretical structure and the Lagrange's equation. Its inertia matrix is relatively simple， which can be application to holonomic or non-holonomic constrained systems. The shift operator of spatial operator algebra combines the features of correlation matrix， access matrix and lower body arrays， and its simple structure is easy to be achieved on computer. More importantly， it contains recurrence relation of kinematics and dynamics. It is introduced into the calculation of the kinetic energy function， which improves the efficiency， avoids double counting and cross operation. Finally， the calculation examples and the simulation are given.

Keywords： quasi-velocities; Euler-Poincare equation; shift operator; high efficiency; dynamics model

0  引  言

過去十多年来，不得不面对一系列控制设计问题—多体复杂机电系统，其中非线性起着重要的作用，幸运的是非线性控制系统分析的几何理论在20世纪八九十年代取得了实质性的进展，为解决许多需要，提供了关键性的概念化的工具，但是诚于许多控制工程师所承认的，在工程实践上，快速建模，计算和实现是主要关心的事情，这一问题又提出了如何快速地对复杂多体系统进行数学建模，因此建立和处理复杂的模型对于设备和控制器都是首先需要的。

然而，面对中-高等多体链式系统，建立准确的数学模型也相当复杂和困难。自然，利用计算机进行自动化推导的兴趣一直在增加，大多数实际工程中主要集中于链式或树式结构，这是一类适用于重要机器人和车辆的系统。但是，许多系统并非树形系统，它们会带有闭环以及其形式的代数和（或）微分约束，需要加到打开的树形系统上，典型的例子是机器人手爪的抓取，或者车辆带有滚轮，这种系统附加的复杂化增大了计算机组装方程的规模。

Poincare方程[1]也叫作伪坐标下的拉格朗日方程或者欧拉—庞卡来方程[2]，庞卡来方程包含了潜在的链式结构特点和模块化的优点，但它们常常保持自然本质属性，链式方式建立动力学方程相比拉氏方程更简单，此外它们的组装更容易，适于自动使他们成为“工业强度”系统的实际选择[3-6]。

然而，符号计算，因为它对于非线性，参数依赖的系统是基本的，而它对于控制工程师是相当新的工具，符号计算不取代数值计算，它而是补充和加强数值计算，认识符号和数值计算的区别以及如何更好地将它们集成起来是一项有意义的挑战，采用符号计算于以下几个目的：基本的数学计算（实现坐标变换、计算李括号）；建立显示的数学模型；简化模型（通过Taylor线性化，对称化简化）；产生数字仿真指令；完成非线性控制的构造（计算逆系统或完成反馈线性化）；实现控制器的数值指令生成。

综上所述，本文介绍树结构的运动学和欧拉—庞卡莱符号建模以及模块化的建模并以一个实例为对象和仿真。

1  伪速度

机械多体系统常假设为链式树结构，即使事实上带有闭环，也可以方便的建造相应的潜在树模型（打开闭环），再加上附加的约束条件。所考虑的系统是刚体组成的，通过关节将刚体联系起来，多体系统是由刚体通过关节相连而组成。每个关节有一组速度变量及其形位（configuration）参数，这些参数的个数等于关节的自由度数，相应于关节参数组成的广义坐标定义为q。

考虑完整约束的系统，其可能形位对应于光滑流形M，（维数为m）称之为形位流形。M上的局部坐标定义了系统的形位，称之为广义坐标[7]。任何在时间间隔（t1，t2）内系统的运动，跟踪了M上的一个轨迹，该轨迹通过局部q（t）：[t1，t2]→M，任何q ∈ M上的一点有广义速度（dq/dt）属于M的切空间，在q这点，用TqM来描述。动力系统的状态空间是2m维流形，，称之为余切从。

在q ∈ M形位处系统的虚位移是一个无限小位移δq，它使系统转移到一个许可形位q′ ∈ M，q′ = q + δq，很显然δq是虚位移的条件为：当且仅当它是无限小的，而且δq ∈ TqM，若系统在形位q处有一个广义力Q的作用，则在虚位移δq作用下的虚功为δw = QTδq。

设M是m维形位流形，对于拉氏系统，又设V1，…，Vm构成系统的m个线性独立的矢量场，作用在M上，于是每个换拉子或李括号可表达为：

的确，这些系数容易在局部坐标系中计算，定义：

设q（t）：[t1，t2]→M是一个光滑轨迹，则q（t）定义了在点q（t） ∈ M处轨迹的切向量，所以我们总是能够将q（t）表达为切向量vi的线性组合（i=1，2，…，m）

在局部坐标中，m维流形上的矢量场可以想象为m维列向量，余矢量场为m维列向量，用这种办法来计算，其中：

变量p称为伪速度。由于这些量是“速度”，我们或许试图将它们与一组坐标π联系起来，使  成立，但这并不能总是得到保证，考察（4），有：

δπ = U（q）δp

但是实际上，右手边（每个δπi）不总是可微的（应为积分）。

2  树形拓扑结构描述

一个多体系统可以视为潜在的树结构，附加的代数或微分约束方程。描述MBS树和数据结构，则展示如何计算树中任意位置在参考系中的形位坐标和速度，一个树定义为一组链，每个链始于根刚体。带有闭环的系统可由树系统加上约束实现，一个树有n的刚体和n个关节组成，每个系统包括基架参考坐标系，它由体“O”来定义，否则体和铰可以任意编号。

独立数据体的结构描述如下，关节和体由他们在数据列表中的位置隐含地编号，每个物体包含一个唯一的“向内的”接点（in board node），对应于（外边的）关节，经过此关节，该物体与树的内部分支联系起来，或联到根上去（体O），每个物体也可能包含“向外的”接点（outboard nodes），这些向外的接点区分了体的地址（locations），这些地址可能与关节地址（关节的向内边）有关，或与传感器地址有关，或与外部作用点有关，或任意其他作用点有关。既然一个关节把树连接到根（根的接点视为体O的外向接点），则必然有n-1个外向接点，存在于n个物体上，对应于余下n-1的个关节，这些就是n个向外的接点。向外关节接点必然依1到n的次序进行编号，且对应于相应的关节序号，特定与关节有关的序号不是基本的，但是通常根接点的序号被赋予序号1，而余下的外向接点可以任意编号，向内接点不必分配序号[8，9]。

一个树由一组链组成，如一个树：0，1，2，4；0，1，2，3，5；0，1，2，3，6。所有的链皆从0号刚体开始，因此不必将它列入。不過体序列本身不足以定义一个树，例如体5和6皆连到了体3上，但是她们通过不同的铰连上去。此中信息由定义每个链为一组有序列表来提供；每组包含一个体和它的向内铰接点，即{向内铰好，体号}。例如下述三链：{{1，1}，{2，2}，{5，4}}；{{1，1}，{2，2}，{3，3}，{4，5}}；{{1，1}，{2，2}，{3，3}，{6，6}}；链1包含了体1，2，4；体1通过铰1连到参考系，体2通过铰2连到体1，体4通过铰5连到体4，数据表明体5在铰4处连到了体3（铰4在第二条链上），体6通过铰6（在第三条链上）连到体3上。

以图1所示的树形多体系统为例，对多体系统的各种拓扑结构描述方法进行比较。首先，利用罗伯森和维腾堡首先提出利用图论的方法，建立其关联矩阵S和通路矩阵T分别为：

根据文献[10]方法，建立图1系统的低序体阵列见表1。

根据空间算子代数标号规则[11]，将图1系统进行重行标号（如图2所示），对于分枝的编号L1，…，Ll各分枝中体的编号遵照规则标号与关联矩阵方法恰好相反，即内侧分枝号及体编号大于外侧分枝及体编号。建立其移位算子为：

其中，，。

考察由k+1各刚体组成的单链，通过关节连接起来，如图3和4所示，体由0到k的序号编号，0表示参考刚体，同时表示任意常规的惯性参考系，第k号关节将k-1刚体在点Ok-1与k号刚体在点Ok点联系起来。

图3  由k+1个刚体组成序号从0到k的k关节，序号从1到k的单链MBS

图4  在任意k号连杆上，向内向外关节铰接点定义为Ok和Ck，体固联坐标系以Ok为其原点

设F k表示固定在体上的参考系，以Ok为其原点，rCOk表示从Ok到Ck的矢量，r k表示Ok到Ok+1的矢量，皆定义在F k中，我们将采用坐标的特别说明，在其中表示矢量，或者在切空间中描述，以上标加以“k”标识，而与坐标无关就不加任何上标。有时需要应用一个固定在体k的坐标系，系由F k平移，但是以Pk点而不是Ok点为其原点，此时我们采用Fpkk的记号，设rpok定义了从O到P的矢量，它也是F k中的矢量，则从F k到FPKk的平移变换产生形位矩阵：

第k个中关节有nk，1≤nk≤6个自由度，它们通过nk个坐标q（k）以及相应的nk个伪速度β（k）以及一个形位矩阵Xk[q（k）]，希望计算一个参考系的欧几里得形位矩阵，该系固定在最后一个刚体上，以末端接点为其原点，例如：图4的系统可以有5个接点，我们可以得到相对于定向空间坐标系的形位：

关节1的作用→移动到C1→…→关节k的作用→移动到Pk：

式（8）可被修正，用来计算，链或树中不同体坐标系下任意两个接点的形位，为了使之对树适用这个计算需要一个简单程序来找出联系两个接点的链。

3  效率动力学建模

一般情况下，多体系统可视为潜在的树结构，其上可附加作用有代数的和（或）微分的约束。树形系统都是由一组链组成，本部分通过链式系统来说明通过庞卡莱方程建立高效率动力学方程的过程，可进一步扩展到树形系统。建立拉格朗日方程及庞卡来方程的关键是建立动能函数，首先根据空间算子代数理论[12]，以n个连杆的串联机械臂（如图5所示）为例，得到空间速度的递推关系如下。

图5中k+1个物体组成的链，Rordriguez定义了C点的空间速度，在以C为其原点的坐标系下，Vc = [ω，vc]，vc表示C点的速度，ω表示连杆的角速度，设O为同一物体上的另一点，rCO表示C对于O在体坐标系中的位置，则C点的空间速度对于O点的速度存在关系：

其中：。

其转置：

关节k的关节映射矩阵H（k） ∈ R6×nk，故：

于是，对（10）到（12）的重复导致了如下速度递推关系，将其在坐标说明（coordinate specific notation）中表示：

式中，上标i表示第i坐标系，假定H（k），β（k）定义在F k坐标系中，而V（k-1）在F k-1坐标系中计算，于是容易在k坐标系中计算出V（k）：

若V 0（0）给定，则（14）使我们能够作递推计算，对于k = 1…k，递推计算出F k原点的速度，以及其角速度，它们二者在F k坐标系中的表示，随后我们设V 0（0） = 0。有点滥用说明，容易定义：

则式（15）改写为式（16）：

考虑第k个刚体连杆，设Icm（k）表示其关于质心在坐标系F k下的惯量张量，m（k）表示质量，a（k）表示从质心到任意点O的位置矢量关于质心的空间惯量，MCM，对于O，MO为：

式中：IO是对于O的惯性张量。

空间速度和空间惯性矩阵，由此对于整个链而言，就容易方便地构造出来，定义空间速度和关节偽速度：

V = [V T（1），…，V T（n）]，β = [β T（1），…，β T（n）]（18）

因此得到：

其中：

由连杆1到n组成链的动能函数为：

其中，，。

为：

通过上述定义和构造得出伪速度形式的动能函数：

因此，得到简化形式的庞卡方程为：

其中：，，。

u（q）表示势能函数，Qp表示伪速度坐标系下的广义力，与拉氏方程相比庞卡莱方程有重要的优点，一个明显又实际的是惯量矩阵相对简单，并且拉氏方程本质上构造了一个局部表示，无论对于何种局部坐标，而庞卡莱方程可以导致一个动力学的全局描述。

4  算例：数值计算

各连杆坐标系如图6所示，系统的Denavit-Hartenberg参数列表如表2所示。

假设各杆件质量分别m1 = 2.1 kg，m2 = 2.5 kg，m3 = 3.0 kg，L1 = 2.1 m，L2 = 2.5 m，L3 = 3.0 m，假设没有外力矩情况下，仿真得到各关节角度及伪速度变化曲线如图7。

5  结  论

伪速度的引入导致了拉氏方程的广义化，使得通过欧拉-庞卡莱方程建立多体系统的动力学模型，实现了动力学的全局描述。并且其惯量矩阵相对简单，对于完整或非完整约束系统都适用。将空间算子代数中的移位算子引入动能函数的计算，提高了效率，同时，避免了重复计算、交叉运算。最后给出了算例及仿真结果。

参考文献：

[1] EMELYANOV S V，KOROVIN S K，LEVANTOVSKY L V. A Drift Algorithm in Control of Uncertain Process [J].Problems of Control and Information Theory，1986，15（6）：425-438.

[2] YONG K D，KWATNY H G. Variable Structure Servomehanism and its Applicationto Overspeed Protection Control [J].Automatica，1982，18（4）：385-400.

[3] MARINO R. High Gain Feedback NonLinear Control System [J].International Joural of Control，1985，42（6）：1369-1385.

[4] KWATNY H G. Variable Structure of AC Drives [M].Variable Structure Control for Robotics Aerospace Applicatons，Amsterdam：Elsevier，1993.

[5] YOUNG K K，KOKOTOVIC P，UTKIN V. A singular perturbation analysis of high-gain feedback systems [J].IEEE Transactions on Automatic Control，1977，22（6）：931-938.

[6] SLOTIONE J J，SASTRY S S. Tracking Control of Nonlinear Systems Using Sliding Surfaces，With Application to Robot Manipulators [J].Internatonal Journal of Control，1983.38（2）：465-492.

[7] JIAN A，RODRIGUEZ G. Computational robot dynamics using spatial operators [C]//IEEE International Conference on Robotics and Automation.San Francisco，IEEE，2000：843-849.

[8] FEATHERSTONE R. Efficient Factorization of the Joint-Space Inertia Matrix for Branched Kinematic Trees [J].The International Journal of Robotics Research，2005，24（6）：487-500.

[9] 刘延柱，洪嘉振，扬海兴.多刚体动力学 [M].北京：高等教育出版社，1989.

[10] 马霞，张晋珠，李灿，等.应用R软件的ARIMA模型教学案例设计 [J].福建电脑，2022，38（11）：113-117.

[11] RODRIGUEZ G，JIAN A，KREUTZ-DELGADO K. Spatial Operator Algebra for multibody system dynamics [J].Journal of the Astronautical Sciences，1992，40：27-50.

[12] FEATHERSTONE R. Robot Dynamics Algorithms [M].New York：Springer，1987.

作者簡介：郝永江（1978—），男，汉族，山东牟平人，中级工程师，博士研究生，研究方向：机械控制；李炳志（1980—），男，汉族，山东德州人，工程师，硕士研究生，研究方向：汽车电子；王爽（1999—），男，汉族，江苏宿迁人，硕士研究生在读，研究方向：视觉SLAM。