基于模糊控制的多体系统动力学约束违约稳定方法

Research on Constraint Violation Stabilization Method of Multibody System Dynamics Based on Fuzzy Control

摘要: 提出了一种基于模糊逻辑控制的多体动力学约束违约稳定方法。该方法以位置约束方程及其导数的误差作为模糊输入，通过模糊推理机制计算反馈输出，精准控制多体系统的约束违约。与传统方法不同，该方法不受积分算法选择、时间步长设置等因素的限制，在长时间实时仿真场景中展现出良好的适应性与稳定性。通过平面三摆和曲柄滑块两个典型仿真算例，有效验证了该方法在约束违约控制中的可靠性与有效性，为多体系统动力学仿真提供了新的技术路径。

Abstract: A constraint violation stabilization method of multibody system dynamics is presented using fuzzy logic control. The method uses the errors of position constraint equations and their derivatives as fuzzy inputs, and calculates feedback outputs through a fuzzy inference mechanism to accurately control constraint violations in multibody systems. Unlike traditional methods, this approach is not limited by factors, such as the choice of integration algorithm or the setting of time steps, demonstrating good adaptability and stability in long-term real-time simulation scenarios. The reliability and effectiveness of the proposed method in constraint violation control are validated through two typical simulation cases: a planar triple pendulum and a crank-slider mechanism, providing a new technical pathway for multibody system dynamics simulation.

1. 引言

约束多体系统的数学模型通常可描述为指标3的微分–代数方程组(DAEs)。为简化求解过程，常将其转换为指标1的欧拉–拉格朗日方程。然而，这种求解策略会引发一个关键问题——约束违约现象。具体而言，当约束条件被转换至加速度层面时，位置和速度层面的约束状态由受约束变为自由状态，导致位置和速度层面的约束条件被违背[1]。为了避免这种违约现象，目前主要的处理方法有坐标分离法[2] [3]、直接校正法[4] [5]和约束稳定法[6]。

坐标分离法通过将广义坐标划分为独立坐标和非独立坐标，对独立坐标进行积分运算后，再通过求解约束方程获得非独立坐标。该方法能够在指定精度水平下严格满足所有约束条件，并实现良好的误差控制。然而，该方法的核心难点在于独立坐标的选取。合理选取一组独立坐标是保证计算效率的关键，但若在计算过程中需要变更独立坐标，可能导致数值计算效率显著下降，这是该方法的显著局限性[7]。

直接校正法在求解微分–代数方程组的数值解时，可直接消除约束方程的违约情况。每次完成积分操作后，会将数值解投影至约束流形上。此方法能够有效解决位置和速度层面的约束违约问题，并且可以在不改变动力学运动方程的前提下，不受所采用积分算法的影响。不过，位置约束方程和速度约束方程的校正一般借助迭代数值算法来实现。在这个校正过程中，所需的迭代次数存在差异，难以在迭代次数内同时确保约束违约消除的一致性。鉴于此，该方法不太适用于中长时间的仿真工作[1]。

在约束稳定法中，Baumgarte稳定方法是克服约束违约最简单高效的技术之一。本质上，Baumgarte稳定方法是一种PD反馈控制，通过在二阶导数的约束方程中添加约束方程的位置项和速度项来减小违约。但是在引入位置项和速度项后，数值算法的稳定性受到反馈参数的极大影响，选择合适的反馈参数至关重要。在实际应用中，反馈参数的选择一般依据经验方法，其取值范围通常处于5~50 [8]。该参数的确定会受到积分器类型、仿真时间步长以及动力学模型等多种因素的综合影响。这种多因素影响的情况使得反馈参数的选择存在一定的模糊性，而这种模糊性可能会导致仿真过程无法顺利完成[9]。

本文受Baumgarte稳定方法的启发，为解决其反馈参数选择困难的问题，利用模糊控制方法构造反馈项来减小约束违约，并通过算例仿真结果来说明所提出方法的有效性。

2. 一般约束多体系统描述

多体系统的广义坐标可以通过矢量$q={\left[q_1{q}_2{q}_3\cdots q_n\right]}^{\text{T}}$ 表示，n表示广义坐标的数量。多体系统的约束方程可以用广义坐标$q$ 和时间$t$ 表示为：

$\Phi =\left(q_1,q_2,q_3\text{,}\cdots \text{,}{q}_n,t\right)=\Phi \left(q,t\right)=0$ (1)

对约束方程进行一阶求导和二阶求导，能够相应地得出速度约束方程和加速度约束方程：

$\dot{\Phi }\left(q,\dot{q},t\right)={\Phi }_q\left(q,t\right)\dot{q}+{\Phi }_t\left(q,t\right)=0$ (2)

$\ddot{\Phi }\left(q,\dot{q},\ddot{q},t\right)={\Phi }_q\left(q,t\right)\ddot{q}-\eta \left(q,\dot{q},t\right)=0$ (3)

其中，${\Phi }_t=\partial \Phi /\partial t$ ，${\Phi }_q\left(q,t\right)$ 为约束雅克比矩阵，$\eta =-{\left({\Phi }_q\dot{q}\right)}_q\dot{q}-2{\Phi }_{qt}\dot{q}-{\Phi }_{tt}$ 称为加速度右项。

多体系统中基于拉格朗日乘子法构建的指标3的微分–代数可表示为：

$\{\begin{array}{c}M\left(q,t\right)\ddot{q}+{\Phi }_q^{\text{T}}\left(q,t\right)\lambda -Q\left(q,\dot{q},t\right)=0\\ \Phi \left(q,t\right)=0\end{array}$ (4)

表示成指标1的微分–代数方程为：

$\left[\begin{array}{cc}M& {\Phi }_q^{\text{T}}\\ {\Phi }_q^{}& 0\end{array}\right]\left[\begin{array}{c}\ddot{q}\\ \lambda \end{array}\right]=\left[\begin{array}{c}Q\\ \eta \end{array}\right]$ (5)

在方程(5)中未引入与运动学约束相关的位置方程和速度方程。因此，在中等时长或长时间的仿真模拟中，受积分过程数值误差、初始条件不精确等因素影响，约束方程可能会被违背。

3. Baumgarte约束稳定法

方程(5)具有不稳定性，原因在于积分过程中数值误差引发的微小扰动无法得到纠正。因此，必须消除或至少控制位置方程和速度方程中的误差。Baumgarte提出在方程中引入包含位置和速度约束方程的反馈项，当发生约束违反时，该反馈项会对系统响应进行控制。经修正后，方程(3)变为：

$\ddot{\Phi }+2\alpha \dot{\Phi }+{\beta }^2\Phi =0$ (6)

其中，𝛼和β是正常数，分别对速度约束方程和位置约束方程的违反程度进行加权，起到控制误差的作用。该方法实质上是将约束加速度方程从原有的开环系统转变为带有位置和速度约束反馈的闭环系统，形成一个PD型控制，如图1所示。因此，约束多体系统的运动方程可以表示为：

$\left[\begin{array}{cc}M& {\Phi }_q^{\text{T}}\\ {\Phi }_q^{}& 0\end{array}\right]\left[\begin{array}{c}\ddot{q}\\ \lambda \end{array}\right]=\left[\begin{array}{c}Q\\ \eta -2\alpha \dot{\Phi }-{\beta }^2\Phi \end{array}\right]$ (7)

Figure 1. Baumgarte constraint violation stabilization method

图1. Baumgarte约束稳定控制法

在Baumgarte约束稳定方法中，反馈参数𝛼和β通常通过经验方法来选择。大量数值实验表明，这两个参数的取值在5~50区间。当α等于β时，系统可实现临界阻尼，展现出较高的数值稳定性。然而，实践表明，基于传统经验取值法，仅有75%的数值分析能够取得成功[10]。事实上，Baumgarte约束反馈参数的合理选择是一个高度依赖多因素的复杂过程。文献[11] [12]指出，参数取值与多体动力学模型的复杂度、积分器类型以及积分时间步长密切相关。当时间步长过小时，若稳定参数取值过大，动力学方程将显著失真，进而导致系统不稳定[13]。此外，不同的数值积分算法及时间步长设置，均会显著影响反馈参数的可行取值范围[14]。这种多因素耦合的特性，使得Baumgarte方法在确定最优反馈参数时存在一定模糊性，成为制约其应用的关键因素。

4. 基于模糊控制的约束违约稳定方法

模糊控制作为一种智能控制策略，以可观测变量为输入，通过预先构建的规则集生成补偿输出。在多体系统的约束稳定中，输入变量是约束函数$\Phi$ 及其时间导数$\dot{\Phi }$ 。该控制方法主要涵盖模糊化、模糊推理以及去模糊化这三个阶段[15]：

1. 模糊化阶段：将实际输入值转换为“大”“小”“快”“慢”等模糊语言变量，并映射至对应的模糊集合。这些集合由隶属度函数(MF)曲线描述，常见类型包括三角形、钟形、梯形、高斯型和Sigmoid型等。通过隶属度函数，实现输入数据从精确值到模糊概念的转换。

2. 模糊推理阶段：依据已构建的规则库，结合模糊化后的输入信息计算控制输出。目前，Mamdani和T-S是应用最为广泛的两种模糊推理方法。其中，T-S模型的输出为输入变量的线性或常数函数，可直接输出精确值，相较于Mamdani模型输出模糊集合的特性，显著减少了计算量[16]，基于此优势，本文选用T-S模型开展模糊推理运算。

3. 去模糊化阶段：运用加权平均法或加权求和法，对各条规则的输出结果进行整合，最终生成清晰明确的控制输出。

本文提出的基于模糊控制的约束违约稳定方法流程如图2所示，该方法通过系统性的模糊控制框架，实现对多体系统约束状态的动态调节与稳定维持。

Figure 2. Fuzzy control constraint violation stabilization method

图2. 模糊控制约束稳定图

4.1. 模糊化

本文中采用高斯型隶属度函数，将输入约束函数$\Phi$ 及其时间导数$\dot{\Phi }$ 在物理论域上划分为负大(NB)、负小(NS)、零(ZE)、正小(PS)和正大(PB)五个模糊语言变量，并映射至离散论域N = {−2, −1, 0, 1, 2}。基于以上定义，构建的模糊化隶属度函数如图3所示。

Figure 3. Gaussian membership function for input variables

图3. 输入变量的高斯隶属度函数

4.2. 模糊推理及去模糊化

在模糊推理过程中，需建立输入和输出变量间的模糊规则体系。模糊规则采用“if…then…”条件语句描述，每条规则对应一个子系统，整个模糊系统则是各子系统的线性组合。本文构建的模糊控制系统为双输入单输出结构，采用T-S模糊系统模型。该模型无需通过二维函数积分计算质心，而是以单个尖峰作为输出隶属度函数。对于零阶T-S模型，当输入1为x，输入2为y时，输出z = c (c为常数)。在此框架下，系统最终输出由所有规则输出的加权平均值确定，具体计算方式如下：

$U={\displaystyle \sum _{i=1}^N{w}^i{z}^i}/{\displaystyle \sum _{i=1}^N{w}^i}$ (8)

对于双输入单输出模型，可得出输出的规则表，具体见表1。

Table 1. The output fuzzy rule table

表1. 输出的模糊规则表

$C/\dot{C}$	负大	负小	零	正小	正大
负大	−2	−2	−2	−1	0
负小	−2	−2	−1	0	1
零	−2	−1	0	1	2
正小	−1	0	1	2	2
正大	0	1	2	2	2

根据输出规则库表，得到推理输出表面如图4所示。

Figure 4. Output control surface

图4. 输出控制面

4.3. 模糊控制约束违约控制

在基于模糊控制的约束违约控制过程中，约束加速度方程转化为：

$\ddot{\Phi }+k_u{U}^h=0$ (9)

其中$k_u$ 是模糊控制输出系数，$U^h=U^h\left(\dot{\Phi },\Phi ,t\right)$ 是模糊控制输出。微分方程可以表示为：

$\ddot{\Phi }+k_u{U}^h={\Phi }_q\ddot{q}-\eta +k_u{U}^h=0$ (10)

所以，方程(10)的右边可以表示为：

${\Phi }_q\ddot{q}=\eta -k_u{U}^h$ (11)

4.4. 模糊控制违约稳定策略

在运用模糊控制策略进行约束违约控制求解的过程中，主要涉及两种情形：

情形一：当初始速度为零，系统起始状态在位姿和速度层面不存在违约现象，这导致输入变量的物理论域难以设定。由于初始计算阶段不存在违约情况，因此可直接以输入变量$\Phi$ 及$\dot{\Phi }$ 作为输入来求解$U^h$ ，

即$U^h=FLC\left(\left[\Phi ,\dot{\Phi }\right]\right)$ ，$k_u$ 的数值可以选取100。

情形二：当初始速度不为零，系统在起始数值计算时，约束矩阵导数层面存在违约。此时，可将第一个仿真步长中位置和速度的违约值作为输入变量的物理论域，并采用违约值的$\infty$ 范数作为违约上限。

则模糊控制中的输入变成$n\cdot k_p\cdot \Phi$ 及$n\cdot k_d\cdot \dot{\Phi }$ ，其中$k_p=1/{\Vert \Phi \Vert }_{\infty }^h$ ，$k_d=1/{\Vert \dot{\Phi }\Vert }_{\infty }^h$ (n是离散论域上限)，输出$k_u{U}^h=k_u\cdot \text{FLC}\left(\begin{array}{cc}n\cdot k_p\cdot \Phi ,& n\cdot k_d\cdot \dot{\Phi }\end{array}\right)$，$k_u$ 可以选为$k_p\cdot k_f$ ，其中$k_f=\Vert {\dot{q}}_{initial}\Vert$ 。

5. 仿真算例

针对3.4节模糊控制违约稳定的两种求解情形，分别用平面三摆模型和曲柄滑块模型进行仿真说明。

算例1：平面三摆模型仿真

以平面三摆为例(见图5)，该系统由三个刚体通过三个旋转铰依次连接而成。各摆杆几何形状与惯性参数均相同，具体为：杆长l = 2 m，质量m = 1 kg，绕质心的转动惯量J = 0.001 kg∙m²。基于图中所示坐

标系，建立各摆杆的质量矩阵$M_i=diag\left(\left[m_i{m}_i{J}_i\right]\right)$ 。系统受到重力载荷$Q_i={\left[0m_{i}g0\right]}^{\text{T}}$ 作用，重力加速

度g = −9.81 m/s²。其约束矩阵可以表示为：

$\Phi =\left[\begin{array}{c}{R}_1+A_1{\overline{u}}_o\\ R_1+A_1{\overline{u}}_{1P_1}-\left(R_2+A_2{\overline{u}}_{2P_1}\right)\\ R_2+A_2{\overline{u}}_{2P_2}-\left(R_3+A_3{\overline{u}}_{3P_2}\right)\end{array}\right]$ ，其中$A_i=\left[\begin{array}{cc}\cos {\theta }_i& -\sin {\theta }_i\\ \sin {\theta }_i& \cos {\theta }_i\end{array}\right]$ ，$R_i={\left[\begin{array}{cc}{x}_i& y_i\end{array}\right]}^{\text{T}}$ ，${\overline{u}}_o={\left[\begin{array}{cc}-l/2& 0\end{array}\right]}^{\text{T}}$，${\overline{u}}_{1P_1}={\left[\begin{array}{cc}l/2& 0\end{array}\right]}^{\text{T}}$，${\overline{u}}_{2P_2}={\left[\begin{array}{cc}l/2& 0\end{array}\right]}^{\text{T}}$，${\overline{u}}_{3P_2}={\left[\begin{array}{cc}-l/2& 0\end{array}\right]}^{\text{T}}$。

仿真时，设定系统初始位置$q={\left[l/2003l/2005l/200\right]}^{\text{T}}$ ，初始速度$\dot{q}=zeros\left(9,1\right)$ 。数值计算采用Matlab的ode45积分算法，设置仿真时间步长为h = 0.01 s，其余参数保持默认配置。由于模糊控制违约稳定策略中初始速度均设为零，故用$100\cdot U^h=100\cdot FLC\left(\left[\Phi ,\dot{\Phi }\right]\right)$ 作为反馈变量，以实现控制目标。

Figure 5. Plane three pendulum model

图5. 平面三摆模型

(a) (b)

Figure 6. $\Phi$ and $\dot{\Phi }$ violation value of plane pendulum

图6. 三摆模型$\Phi$ 和$\dot{\Phi }$ 违约值

平面三摆模型对初始条件具有高度敏感性，当系统初始位移较大时，三摆的重复振荡将演变为混沌运动。随着仿真时间的延长，系统违约现象逐渐加剧，不同条件下三摆的运动轨迹差异显著。图6(a)和图6(b)展示了约束方程及其速度的违约值变化趋势：在未采用违约稳定控制的情况下，$\Phi$ 和$\dot{\Phi }$ 的违约值持续增大；相比之下，应用模糊控制违约稳定策略后，$\Phi$ 和$\dot{\Phi }$ 违约值始终被限制在一个极小的量级。从数值量级来看，无违约稳定的违约值处于10⁻⁵量级，尽管该数值看似微小，但如图7(a)和图7(b)所示，在仿真运行后期，未控制状态下三摆后两杆的弧度值与采用模糊控制策略的结果呈现显著差异，这表明即使是微小的违约也会导致仿真结果出现偏差，验证了模糊控制策略在维持系统稳定性方面的有效性。

(a) (b)

Figure 7. The radian values of pendulum 2 and pendulum 3

图7. 摆2和摆3的弧度值

算例2：曲柄滑块模型仿真

以图8所示的曲柄滑块模型为例开展分析。该平面机构的参数详见表2，所有关节均假设为理想状

态，曲柄的初始角速度设定为逆时针方向10 rpm。系统的广义坐标可表示为$q={\left[{\theta }_1{x}_2{\text{ y}}_2{\theta }_2{x}_3\right]}^{\text{T}}$ ，广义质量矩阵$M=diag\left(\left[J_1{m}_2{m}_2{J}_2{m}_3\right]\right)$ ，同时系统受重力载荷作用$Q={\left[0\text{ 0 }{m}_{2}g\text{ 0 }0\right]}^{\text{T}}$ ，重力加速度取g = −9.81 m/s²。其约束矩阵具体如下：

$\Phi =\left[\begin{array}{c}{L}_1\cos {\theta }_1+L_2\cos {\theta }_2-x_2\\ L_{1}sin{\theta }_1+L_{2}sin{\theta }_2-y_2\\ x_2+L_2\cos {\theta }_2-x_3\\ y_2+L_{2}sin{\theta }_2\end{array}\right]=0$

仿真过程中，系统初始位置设为$q={\left[0L_1+L_2/200L_1+L_2\right]}^{\text{T}}$ ，初始速度设定为$\dot{q}={\left[\begin{array}{cc}\pi /3& \begin{array}{ccc}0& 0& \begin{array}{cc}0& 0\end{array}\end{array}\end{array}\right]}^{\text{T}}$。计算求解采用Matlab的ode45算法，配置仿真时间步长h = 0.01 s，其余参数保持默认设置。在模糊控制违约稳定策略的实施中，输出变量$k_u{U}^h=k_u\cdot \text{FLC}\left(n\cdot k_p\cdot \Phi ,n\cdot k_d\cdot \dot{\Phi }\right)$，$k_u$ 选为$k_p\cdot k_f$ ，其中$k_f=\Vert {\dot{q}}_{initial}\Vert$ 。

Table 2. Mechanical characteristics of plane crank slider

表2. 平面曲柄滑块的机构特性

体	长度(m)	质量(kg)	转动惯量(kg∙m^2)
曲柄1	0.2	20	45
连杆2	0.35	3.5	3.5
滑块3	-	2.5	0.02

Figure 8. Planar crank slider model

图8. 平面曲柄滑块模型

若不采用违约控制策略，仿真计算将因约束矛盾无法进行。图9(a)和图9(b)分别展示了采用Baumgarte约束稳定和模糊控制约束稳定法后，$\Phi$ 和$\dot{\Phi }$ 的违约值变化情况。其中，Baumgarte约束稳定法的反馈参数$\alpha =\beta =20$ 。

(a) (b)

Figure 9. $\Phi$ and $\dot{\Phi }$ violation value of crank slider model

图9. 曲柄滑块模型$\Phi$ 和$\dot{\Phi }$ 违约值

从图9中可清晰观察到，在采用情形二的模糊控制设置时，能够显著实现约束违约控制目标。该控制策略在设定过程中不受仿真时间步长、积分算法及动力学模型的限制，相较于Baumgarte方法，可有效规避反馈参数选择的复杂性难题，展现出更强的鲁棒性与普适性。

6. 结论

针对多体系统Baumgarte约束违约稳定方法反馈参数难以选取的问题，本文提出了一种基于模糊控制方法用于多体系统中的约束违约稳定。该策略依据动力学系统初始速度的状态(零或非零)，构建了两种差异化的输出变量计算机制，并通过平面三摆与曲柄滑块两个典型仿真案例，验证了方法的有效性与可靠性。两种计算机制可根据实际工况灵活切换，以适应不同系统需求。此外，该方法突破了时间步长、积分算法及动力学模型的限制，在违约稳定控制方面表现优异，尤其适用于长时间实时仿真场景，展现出良好的工程应用潜力。