The constraint-stabilized implicit methods on Lie group for differential-algebraic equations of multibody system dynamics

Abstract

The implicit methods on Lie group are developed for multibody system dynamics. To avoid the violation of the displacement, velocity and acceleration constraints of the nonlinear differential-algebraic equations, the constraint-stabilized equations on Lie group are formulated. The implicit method for the Euler–Lagrange equations and the symplectic Euler method for the constrained Hamilton equations are presented. These methods can keep the stability of the displacement, velocity and acceleration constraints at the same time during the longer simulation. A three-dimensional single pendulum and a three-dimensional double pendulum are used to verify the accuracy of the constraint-stabilized Euler methods on Lie group and the orthogonality preservation of the rotation matrix.

Introduction

Differential-algebraic equations (DAEs), which have strong nonlinearity that need special numerical integrations,^1–3 are the common form in Lagrangian and Hamiltonian dynamics of multibody system. The traditional methods for DAEs are developed by ordinary differential equations (ODEs).^4,5 Researches focus on the stability of the constraints which have influence on the stability of the numerical methods of DAEs.

The structure preserving integration methods^6,7 developed in recent years improve the numerical stability through keeping as many invariants of continuous systems, such as symplectic algorithms, ⁸ energy methods,^9–11 energy preserving variational integrations based on the discrete variational principles of mechanics,^12–14 and Lie group methods. ¹⁵ Among them, Lie group methods keep the special orthogonal group characteristics of body motion, while other approaches focus on the parametric expression of the body motion, ¹⁶ such as the classical Euler angles and Bryant angles. The corresponding algorithm cannot keep the inherent characteristics of Lie groups.

The singularity caused by the parametric expression can be effectively avoided by using the special orthogonal groups^17–20 directly. Because of the non-vector characteristics in group space, the traditional numerical integrations cannot be used directly. The algorithm designed by Lie algebra vector is proposed earlier in Simo and Vu-Quoc ²¹ and Lewis and Simo ²² to maintain the orthogonality of the attitude matrix with the aim of numerical stability. The basic researches are the CG (Crouch–Grossman) method ²³ and MK (Munthe-Kaas) method ²⁴ for the matrix differential equations. The CG method is based on the canonical coordinates of the second kind (CCSK) which is discrete Lie algebra vector and is applied in the generalized-α method for index 3 DAEs.^25–27 The MK method is based on the canonical coordinates of the first kind (CCFK) which is discrete pseudo-rotation angle increment and is applied in the Runge–Kutta method for index 1 DAEs.^28,29 The basic approaches in solving the matrix differential equations are similar to the developed methods of multibody dynamics in Park and Chung ³⁰ and Ding et al. ³¹ The aforementioned methods do not involve the constraint stabilization.

The stability of the constraints means that the constraints are always satisfied during the simulation of multibody systems. Furthermore, the velocity constraints and the acceleration constraints are also satisfied. Numerical methods for DAEs with indices 1, 2, and 3 can only keep one of the stabilities of the displacement, velocity, and acceleration constraints. One of the widely used method is Baumgarte’s constraint stabilization method,^32,33 which introduces the displacement and velocity constraints into the acceleration constraint equations by using the feedback control theory to get stabilized dynamic equations through constraint modification. This method only improves the constraint violation partly. Gear et al. ³⁴ presented an approach of reducing the index 3 DAEs to index 2 to keep the constraint stabilization by adding variables. This method essentially used the projection method, which is another efficient method to avoid the constraints violation. ³⁵ In Gear’s method, the acceleration constraint violation still exists. To make the displacement, velocity, and acceleration constraints satisfied exactly at the same time, the new constraint-stabilized method is presented in this article.

For the new constraint-stabilized equations on Lie group of Lagrangian and Hamilton dynamics, the implicit Euler method, which is a special case of the implicit multi-step methods and implicit Runge–Kutta methods, is used to solve the Euler–Lagrange equations. The symplectic Euler method is used to solve the constrained Hamilton equations. The Euler methods on Lie group are significant for relevant higher order algorithm design and large-scale system simulations. For the discrete mapping between Lie group and Lie algebra, the CCSK coordinates are used for implicit first-order CG Euler (CGE) methods of DAEs.

This article is organized as follows. In section “Constraint-stabilized motion equations on Lie group,” the expressions on Lie group for DAEs of Lagrangian and Hamiltonian dynamics are described, and the constraint-stabilized equations are formulated for index 2 and index 1 cases. In section “Implicit Euler methods of Lagrangian and Hamiltonian DAEs,” the implicit Euler methods on Lie group for DAEs are presented. Section “Numerical examples” gives some numerical examples to test and compare the algorithms in section “Implicit Euler methods of Lagrangian and Hamiltonian DAEs.” Conclusions are given finally.

Constraint-stabilized motion equationson Lie group

For a typical multibody system with $n$ rigid bodies and $l$ constraints, the absolute position of mass center ${\mathit{x}}_i\in R^3$ and orientation ${\mathit{R}}_i\in R^{3\times 3}$ of the $i\mathrm{th}$ body form the generalized coordinates ${\mathit{q}}_i=(\begin{array}{cc}{\mathit{x}}_i\hfill & {\mathit{R}}_i\hfill \end{array})(i=1,\dots ,n)$ . Because each body-fixed frame are parallel to the three principle inertia axes, the corresponding velocity is described as ${\mathit{V}}_i=(\begin{array}{cc}{\mathit{v}}_i\hfill & {\Omega }_i\hfill \end{array})(i=1,\dots ,n)$ , where ${\mathit{v}}_i\in R^3$ is the absolute translation velocity, ${\mathbf{\Omega }}_i\in R^3$ is the absolute angular velocity expressed in body-fixed frame which satisfies the relationship ${\stackrel{·}{\mathit{R}}}_i={\mathit{R}}_i{\tilde{\mathit{\Omega }}}_i$ , ${\mathit{R}}_i$ forms the special orthogonal group SO(3), and ${\tilde{\mathit{\Omega }}}_i$ which is the skew-symmetry matrix of ${\mathbf{\Omega }}_i$ forms the Lie algebra so(3).

The motion equations of multibody systems in Lagrangian dynamics are written as

$$\{\begin{array}{c}\stackrel{·}{\mathit{q}}=\left(\begin{array}{cc}\mathit{v}\hfill & \mathit{R}\tilde{\mathit{\Omega }}\hfill \end{array}\right)\hfill \\ \multicolumn{1}{c}{\mathit{M}\stackrel{·}{\mathit{V}}+{\mathit{B}}^{\mathrm{T}}(\mathit{q})\lambda =\mathit{Q}(\mathit{q},\mathit{V})}\\ \multicolumn{1}{c}{\mathbf{\Phi }(\mathit{q})=0}\end{array}$$

(1)

where $\mathit{V}=[\begin{array}{cc}{\mathit{v}}^{\mathrm{T}}\hfill & {\mathbf{\Omega }}^{\mathrm{T}}\hfill \end{array}{]}^{\mathrm{T}}$ is the velocity vector; $\mathit{M}=\mathrm{diag}(\mathit{m}\mathit{J})$ is the inertia matrix of the system, $\mathit{m}=\mathrm{diag}({\mathit{m}}_1{\mathit{m}}_2\dots {\mathit{m}}_n)$ and $\mathit{J}=\mathrm{diag}({\mathit{J}}_1{\mathit{J}}_2\dots {\mathit{J}}_n)$ ; ${\mathit{m}}_i$ and ${\mathit{J}}_i(i=1,\dots ,n)$ are the translation and rotation inertia matrices, respectively, of the body $i$ ; $\mathit{Q}(\mathit{q},\mathit{V})={\left[\begin{array}{cccc}{\mathit{Q}}_1^{\mathrm{T}}(\mathit{q},\mathit{V})\hfill & {\mathit{Q}}_2^{\mathrm{T}}(\mathit{q},\mathit{V})\hfill & \dots \hfill & {\mathit{Q}}_{\mathit{n}}^{\mathrm{T}}(\mathit{q},\mathit{V})\hfill \end{array}\right]}^{\mathrm{T}}$ is the generalized force vector; ${\mathit{Q}}_i(\mathit{q},\mathit{V})=\left[\begin{array}{c}0\hfill \\ -{\tilde{\mathit{\Omega }}}_i{\mathit{J}}_i{\mathit{\Omega }}_i\hfill \end{array}\right]+{\mathit{Q}}_{ie},(i=1,\dots ,n)$ , $Q_{\mathrm{ie}}$ is the generalized external force vector; $\mathbf{\Phi }(\mathit{q})={\left[\begin{array}{cccc}{\mathbf{\Phi }}_1(\mathit{q})\hfill & {\mathbf{\Phi }}_2(\mathit{q})\hfill & \dots \hfill & {\mathbf{\Phi }}_n(\mathit{q})\hfill \end{array}\right]}^{\mathrm{T}}=0$ is the scleronomic constraint equations; $\lambda$ is the related Lagrangian multiplier vector; and $\mathit{B}(\mathit{q})$ is its Jacobian which is expressed as

$$\stackrel{·}{\mathit{\Phi }}(\mathit{q})={\mathbf{\Phi }}_{\mathit{x}}\stackrel{·}{\mathit{x}}+〈\begin{array}{cc}{\mathbf{\Phi }}_{\mathit{R}}\hfill & \stackrel{·}{\mathit{R}}\hfill \end{array}〉\equiv \mathit{B}(\mathit{q})\mathit{V}=0$$

(2)

$$\stackrel{··}{\mathit{\Phi }}(\mathit{q})\equiv \mathit{B}(\mathit{q})\stackrel{·}{\mathit{V}}+\stackrel{·}{\mathit{B}}(\mathit{q})\mathit{V}=0$$

(3)

According to the classic index theory, equation (1) is DAEs of index 3, which is named DAE3L_L in this article. The first L means Lagrangian dynamics, and the second L means Lie group. Equation (1) considers only the displacement constraints. Therefore, it will cause the velocity and acceleration constraint violation. Similarly, the following index 2 which is named DAE2L_L here considers only the velocity constraints, and it will cause the displacement and acceleration constraint violation

$$\{\begin{array}{c}\stackrel{·}{\mathit{q}}=\left(\begin{array}{cc}\mathit{v}\hfill & \mathit{R}\tilde{\mathit{\Omega }}\hfill \end{array}\right)\hfill \\ \mathit{M}\stackrel{·}{\mathit{V}}+{\mathit{B}}^{\mathrm{T}}(\mathit{q})\lambda =\mathit{Q}(\mathit{q},\mathit{V})\hfill \\ \mathit{B}(\mathit{q})\mathit{V}=0\hfill \end{array}$$

(4)

In Gear et al., ³⁴ the stabilized index 2 is derived by adding variables $\mathit{v}\prime ,\tilde{\mathit{\Omega }}\prime$ and a new Lagrangian multiplier $\mathbf{\mu }$ . It is named DAE2L_LS

$$\{\begin{array}{c}\stackrel{·}{\mathit{q}}=\left(\begin{array}{cc}\mathit{v}\prime \hfill & \mathit{R}\tilde{\mathit{\Omega }}\prime \hfill \end{array}\right)\hfill \\ \left[\begin{array}{c}\mathit{v}\prime \hfill \\ \multicolumn{1}{c}{\mathbf{\Omega }\prime }\end{array}\right]=\mathit{V}-{\mathit{B}}^{\mathrm{T}}(\mathit{q})\mathbf{\mu }\hfill \\ \mathit{M}\stackrel{·}{\mathit{V}}+{\mathit{B}}^{\mathrm{T}}(\mathit{q})\lambda =\mathit{Q}(\mathit{q},\mathit{V})\hfill \\ \mathbf{\Phi }(\mathit{q})=0\hfill \\ \mathit{B}(\mathit{q})\mathit{V}=0\hfill \end{array}$$

(5)

Equation (5) considers only the displacement and velocity constraints, which means only the displacement and velocity constraints are satisfied, while the acceleration constraints violation still exist. In order to satisfy the constraints at the displacement, velocity, and acceleration level, the following stabilized index 1 is formulated by combining the displacement constraints in equation (1), the velocity constraints in equation (2), and the acceleration constraints in equation (3). It is named DAE1L_LS

$$\{\begin{array}{c}\stackrel{·}{\mathit{q}}=\left(\begin{array}{cc}\mathit{v}\prime \hfill & \mathit{R}\tilde{\mathit{\Omega }}\prime \hfill \end{array}\right)\hfill \\ \multicolumn{1}{c}{\mathit{V}\prime =\mathit{V}-{\mathit{B}}^{\mathrm{T}}(\mathit{q})\mathbf{\mu }}\\ \multicolumn{1}{c}{\stackrel{·}{\mathit{V}}\prime =\stackrel{·}{\mathit{V}}-{\mathit{B}}^{\mathrm{T}}(\mathit{q})\nu }\\ \multicolumn{1}{c}{\mathit{M}\stackrel{·}{\mathit{V}}+{\mathit{B}}^{\mathrm{T}}(\mathit{q})\lambda =\mathit{Q}(\mathit{q},\mathit{V})}\\ \multicolumn{1}{c}{\mathbf{\Phi }(\mathit{q})=0}\\ \multicolumn{1}{c}{\mathit{B}(\mathit{q})\mathit{V}=0}\\ \multicolumn{1}{c}{\mathit{B}(\mathit{q})\stackrel{·}{\mathit{V}}+\stackrel{·}{\mathit{B}}(\mathit{q})\mathit{V}=0}\end{array}$$

(6)

where $\mathit{V}\prime ={\left[\begin{array}{cc}{\mathit{v}\prime }^{\mathrm{T}}\hfill & {\mathbf{\Omega }\prime }^{\mathrm{T}}\hfill \end{array}\right]}^{\mathrm{T}}\mathrm{and}\stackrel{·}{\mathit{V}}\prime ={\left[\begin{array}{cc}{\stackrel{·}{\mathit{v}\prime }}^{\mathrm{T}}\hfill & {\stackrel{·}{\mathit{\Omega }}\prime }^{\mathrm{T}}\hfill \end{array}\right]}^{\mathrm{T}}$ .

Introducing the canonical coordinates, the constraint-stabilized motion equations of multibody systems on Lie group in Hamilton dynamics are written as

$$\{\begin{array}{c}\stackrel{·}{\mathit{q}}=\left(\begin{array}{cc}{\mathit{m}}^{-1}{\mathit{p}\prime }_{\mathit{x}}\hfill & \mathit{R}{\left({\mathit{J}}^{-1}{\mathit{p}\prime }_{\mathit{R}}\right)}^{~}\hfill \end{array}\right)\hfill \\ \mathit{p}\prime =\mathit{p}-{\mathit{B}}^{\mathrm{T}}(\mathit{q})\mathbf{\mu }\hfill \\ \stackrel{·}{\mathit{p}}\prime =\stackrel{·}{\mathit{p}}-{\mathit{B}}^{\mathrm{T}}(\mathit{q})\nu \hfill \\ \stackrel{·}{\mathit{p}}+{\mathit{B}}^{\mathrm{T}}(\mathit{q})\lambda =\mathit{Q}(\mathit{q},\mathit{p})\hfill \\ \mathbf{\Phi }(\mathit{q})=0\hfill \\ \mathit{B}(\mathit{q}){\mathit{M}}^{-1}\mathit{p}=0\hfill \\ \mathit{B}(\mathit{q}){\mathit{M}}^{-1}\stackrel{·}{\mathit{p}}+\stackrel{·}{\mathit{B}}(\mathit{q}){\mathit{M}}^{-1}\mathit{p}=0\hfill \end{array}$$

(7)

where the generalized momenta $\mathit{p}=[\begin{array}{cc}{\mathit{p}}_{\mathit{x}}^{\mathrm{T}}\hfill & {\mathit{p}}_{\mathit{R}}^{\mathrm{T}}\hfill \end{array}{]}^{\mathrm{T}}$ , ${\mathit{p}}_{\mathit{x}}=\mathit{mv}$ , and ${\mathit{p}}_{\mathit{R}}=\mathit{J}\mathbf{\Omega }$ .

Equation (7) is called DAE1H_LS, in which H means Hamilton dynamics, L means Lie group, and S means stabilization. The DAE3H_L, DAE2H_L, and DAE2H_LS are named similarly as DAE3L_L, DAE2L_L, and DAE2L_LS.

Implicit Euler methods of Lagrangian and Hamiltonian DAEs

The implicit Euler formulations of the generalized coordinate ${\mathit{q}}_i$ and the corresponding velocity ${\mathit{V}}_i(i=1,\dots ,n)$ , are

$${\stackrel{·}{\mathit{q}}}^{i+1}=\frac{{\mathit{q}}^{i+1}-{\mathit{q}}^i}{h}$$

(8)

$${\stackrel{·}{\mathit{V}}}^{i+1}=\frac{{\mathit{V}}^{i+1}-{\mathit{V}}^i}{h}$$

(9)

For the DAEs on Lie group, $\mathit{R}$ belongs to SO(3), and $\stackrel{·}{\mathit{R}}$ cannot be discretized as equation (8). According to the CG method, the first-order discretized version of ${\mathit{R}}_{\mathit{i}}={\mathit{R}}_{\mathit{i}}{\tilde{\mathit{\Omega }}}_{\mathit{i}}$ is

$${\mathit{R}}^{i+1}={\mathit{R}}^i\exp \left(h{\tilde{\mathit{\Omega }}}^{i+1}\right)$$

(10)

Therefore, the first-order discretization of the generalized coordinate is

$${\mathit{q}}^{i+1}=\left(\begin{array}{cc}{\mathit{x}}^i+h{\mathit{v}}^{i+1}\hfill & {\mathit{R}}^i\exp \left(h{\tilde{\mathit{\Omega }}}^{i+1}\right)\hfill \end{array}\right)$$

(11)

Substituting equations (11) and (9) into equation (1) at $t^{i+1}$ , the implicit Euler method of DAEs (1) with index 3 on Lie group in Lagrangian dynamics is

$$\{\begin{array}{c}\mathit{M}{\stackrel{·}{\mathit{V}}}^{i+1}+{\mathit{B}}^{\mathrm{T}}({\mathit{q}}^{i+1}){\lambda }^{i+1}=\mathit{Q}({\mathit{q}}^{i+1},{\mathit{V}}^{i+1})\hfill \\ \multicolumn{1}{c}{\mathbf{\Phi }({\mathit{q}}^{i+1})=0}\end{array}$$

(12)

which is named DAE3L_LE, and E means the implicit Euler method.

Equation (12) is solved by the following Newton–Raphson iterative scheme

$$\begin{array}{c}\multicolumn{1}{c}{\left[\begin{array}{c}{\mathit{V}}^{i+1,k+1}\hfill \\ \multicolumn{1}{c}{{\lambda }^{i+1,k+1}}\end{array}\right]=\left[\begin{array}{c}{\mathit{V}}^{i+1,k}\hfill \\ \multicolumn{1}{c}{{\lambda }^{i+1,k}}\end{array}\right]-\nabla {\mathit{F}}^{-1}({\mathit{V}}^{i+1,k},{\lambda }^{i+1,k})}\\ \mathit{F}({\mathit{V}}^{i+1,k},{\lambda }^{i+1,k})\hfill \end{array}$$

(13)

where

$$\{\begin{array}{c}{\mathit{F}}_1({\mathit{V}}^{i+1},{\lambda }^{i+1})=\mathit{M}({\mathit{V}}^{i+1}-{\mathit{V}}^i)+h{\mathit{B}}^{\mathrm{T}}\left({\mathit{q}}^{i+1}\right){\lambda }^{i+1}-h\mathit{Q}({\mathit{q}}^{i+1},{\mathit{V}}^{i+1})\hfill \\ {\mathit{F}}_2\left({\mathit{V}}^{i+1}\right)=\mathbf{\Phi }\left({\mathit{q}}^{i+1}\right)\hfill \end{array}$$

(14)

and

$$\begin{array}{c}\nabla \mathit{F}({\mathit{V}}^{i+1},{\lambda }^{i+1})=\hfill \\ \left[\begin{array}{cc}{\nabla }_{{\mathit{V}}^{i+1}}{\mathit{F}}_1({\mathit{V}}^{i+1},{\lambda }^{i+1})\hfill & h{\mathit{B}}^{\mathrm{T}}\left({\mathit{q}}^{i+1}\right)\hfill \\ \multicolumn{1}{c}{h\mathit{B}\left({\mathit{q}}^{i+1}\right)}& 0\hfill \end{array}\right]\hfill \end{array}$$

(15)

DAE2L_LE formulations are obtained from DAE2L_L by using the implicit Euler formulations (11) and (9). Similarly, the DAE2L_LSE, which is the implicit Euler method of DAEs (5) with index 2 on Lie group, is

$$\{\begin{array}{c}{\mathit{R}}^{i+1}={\mathit{R}}^i\exp \left({\tilde{\mathit{\Theta }}}^{i+1}\right)\hfill \\ \left[\begin{array}{c}{\mathit{x}}^{i+1}-{\mathit{x}}^i\hfill \\ \multicolumn{1}{c}{{\mathbf{\Theta }}^{i+1}}\end{array}\right]=h{\mathit{V}}^{i+1}-{\mathit{B}}^{\mathrm{T}}({\mathit{x}}^{i+1},{\mathit{R}}^{i+1}){\mathbf{\mu }}^{i+1}\hfill \\ \mathit{M}({\mathit{V}}^{i+1}-{\mathit{V}}^i)+h{\mathit{B}}^{\mathrm{T}}({\mathit{q}}^{i+1}){\lambda }^{i+1}=h\mathit{Q}({\mathit{q}}^{i+1},{\mathit{V}}^{i+1})\hfill \\ \mathbf{\Phi }\left({\mathit{q}}^{i+1}\right)=0\hfill \\ \mathit{B}\left({\mathit{q}}^{i+1}\right){\mathit{V}}^{i+1}=0\hfill \end{array}$$

(16)

The DAE1L_LSE, which is the implicit Euler method of DAEs (6) with index 1 on Lie group, is

$$\{\begin{array}{c}{\mathit{R}}^{i+1}={\mathit{R}}^i\exp \left({\tilde{\mathit{\Theta }}}^{i+1}\right)\hfill \\ \begin{array}{c}\left[\begin{array}{c}{\mathit{x}}^{i+1}-{\mathit{x}}^i\hfill \\ \multicolumn{1}{c}{{\mathbf{\Theta }}^{i+1}}\end{array}\right]=h{\mathit{V}}^{i+1}-{\mathit{B}}^{\mathrm{T}}({\mathit{x}}^{i+1},{\mathit{R}}^{i+1}){\mathbf{\mu }}^{i+1}\hfill \\ \left[\begin{array}{c}{\mathit{v}}^{i+1}-{\mathit{v}}^i\hfill \\ \multicolumn{1}{c}{{\mathbf{\Omega }}^{i+1}-{\mathbf{\Omega }}^i}\end{array}\right]=h{\stackrel{·}{\mathit{V}}}^{i+1}-{\mathit{B}}^{\mathrm{T}}({\mathit{x}}^{i+1},{\mathit{R}}^{i+1}){\mathbf{\nu }}^{i+1}\hfill \end{array}\hfill \\ \mathit{M}({\mathit{V}}^{i+1}-{\mathit{V}}^i)+h{\mathit{B}}^{\mathrm{T}}({\mathit{q}}^{i+1}){\lambda }^{i+1}=h\mathit{Q}({\mathit{q}}^{i+1},{\mathit{V}}^{i+1})\hfill \\ \mathbf{\Phi }({\mathit{q}}^{i+1})=0\hfill \\ \begin{array}{c}\mathit{B}({\mathit{q}}^{i+1}){\mathit{V}}^{i+1}=0\hfill \\ \multicolumn{1}{c}{\mathit{B}({\mathit{q}}^{i+1})({\mathit{V}}^{i+1}-{\mathit{V}}^i)+h\stackrel{·}{\mathit{B}}\left({\mathit{q}}^{i+1}\right){\mathit{V}}^{i+1}=0}\end{array}\hfill \end{array}$$

(17)

In Hamiltonian dynamics, the DAE1H_LSE, which is the symplectic Euler method of the constrained Hamilton equations on Lie group, is

$$\{\begin{array}{c}{\mathit{R}}^{i+1}={\mathit{R}}^i\exp \left({\left({\mathit{J}}^{-1}{\mathit{p}\prime }_{\mathit{R}}\right)}^{~}\right)\hfill \\ \multicolumn{1}{c}{\left[\begin{array}{c}\mathit{m}({\mathit{x}}^{i+1}-{\mathit{x}}^i)\hfill \\ \multicolumn{1}{c}{{{\mathit{p}\prime }_{\mathit{R}}}^{i+1}}\end{array}\right]=h{\mathit{p}}^{i+1}-{\mathit{B}}^{\mathrm{T}}\left({\mathit{q}}^{i+1}\right){\mathbf{\mu }}^{i+1}}\\ \multicolumn{1}{c}{\left[\begin{array}{c}{{\mathit{p}\prime }_{\mathit{x}}}^{i+1}-{{\mathit{p}\prime }_{\mathit{x}}}^i\hfill \\ \multicolumn{1}{c}{{{\mathit{p}\prime }_{\mathit{R}}}^{i+1}-{{\mathit{p}\prime }_{\mathit{R}}}^i}\end{array}\right]=h{\stackrel{·}{\mathit{p}}}^{i+1}-{\mathit{B}}^{\mathrm{T}}({\mathit{x}}^{i+1},{\mathit{R}}^{i+1}){\mathbf{\nu }}^{i+1}}\\ \multicolumn{1}{c}{{\mathit{p}}^{i+1}-{\mathit{p}}^i+h{\mathit{B}}^{\mathrm{T}}\left({\mathit{q}}^{i+1}\right){\lambda }^{i+1}=h\mathit{Q}({\mathit{q}}^{i+1},{\mathit{p}}^{i+1})}\\ \multicolumn{1}{c}{({\mathit{q}}^{i+1})=0}\\ \multicolumn{1}{c}{\mathit{B}({\mathit{q}}^{i+1}){\mathit{M}}^{-1}{\mathit{p}}^{i+1}=0}\\ \multicolumn{1}{c}{\mathit{B}({\mathit{q}}^{i+1}){\mathit{M}}^{-1}({\mathit{p}}^{i+1}-{\mathit{p}}^i)+h\stackrel{·}{\mathit{B}}\left({\mathit{q}}^{i+1}\right){\mathit{M}}^{-1}{\mathit{p}}^{i+1}=0}\end{array}$$

(18)

The DAE3H_LE, DAE2H_LE, and DAE2H_LSE are similar to the DAE3L_LE, DAE2L_LE, and DAE2L_LSE.

Numerical examples

Figure 1 is a three-dimensional (3D) single pendulum, in which the generalized coordinate and the corresponding velocity are $\mathit{q}=(\begin{array}{cc}\mathit{x}\hfill & \mathit{R}\hfill \end{array})$ and $\mathit{V}=[\begin{array}{cc}{\mathit{v}}^{\mathrm{T}}\hfill & {\mathbf{\Omega }}^{\mathrm{T}}\hfill \end{array}{]}^{\mathrm{T}}$ , and the inertia matrix of the system is $\mathit{M}=\mathrm{diag}(\begin{array}{cc}\mathit{m}\hfill & \mathit{J}\hfill \end{array})$ , where $\mathit{m}=\mathrm{diag}(\overline{m},\overline{m},\overline{m})$ , $\mathit{J}=\mathrm{diag}(J_1,J_2,J_3)$ , $l=1\mathrm{m},\overline{m}=5\mathrm{kg}$ , $r=0.25\mathrm{m},\mathrm{and}h=0.25\mathrm{m}$ ; the generalized force vector is $\mathit{Q}(\mathit{q},\mathit{V})=\left[\begin{array}{c}-\mathit{mg}\hfill \\ \multicolumn{1}{c}{-\tilde{\mathit{\Omega }}\mathit{J}\mathbf{\Omega }}\end{array}\right]$ , $\mathit{g}={\left[\begin{array}{ccc}0\hfill & 0\hfill & 9.81\hfill \end{array}\right]}^{\mathrm{T}}$ ; the constraint equations are $\mathbf{\Phi }(\mathit{q})=\mathit{x}+\mathit{RX}=0$ , where $\mathit{X}={\left[\begin{array}{ccc}0\hfill & -l\hfill & 0\hfill \end{array}\right]}^{\mathrm{T}}$ , and the initial values are $\mathit{R}(0)=\mathit{E}$ and $\mathbf{\Omega }(0)={\left[\begin{array}{ccc}0\hfill & 150\hfill & -4.6154\hfill \end{array}\right]}^{\mathrm{T}}$ .

Figure 1.

A 3D single pendulum.

Figures 2–4 show the results obtained by using the DAE3L_LE method with time step h = 0.00001 and simulation time t = 10 s. Figure 2 indicates the space trajectory of the end of the 3D single pendulum, the time history of the absolute position of mass center, and the time history of the Lagrange multiplier. Figure 3 demonstrates the kinetic, potential, and total energy of the system. Figure 4 indicates the displacement, velocity, and acceleration constraints. These results illustrate that the simulation trajectory has higher accuracy; the total energy and the displacement constraints are kept well in the smaller time step case for longer time simulation. Due to analysis of the DAE3L_LE method, the violation appears in the velocity and acceleration constraints.

Figure 2.

The trajectory of the end of the 3D single pendulum by DAE3L_LE.

Figure 3.

The energies of the single pendulum by DAE3L_LE.

Figure 4.

The constraints of the single pendulum by DAE3L_LE.

Figures 5–7 show the displacement, velocity, and acceleration constraints of the 3D single pendulum obtained by different Lie group Euler methods DAE2L_LE, DAE2L_LSE, and DAE1L_LSE. It is illustrated that the stabilized index 2 method DAE2L_LSE can keep the stability of displacement and velocity constraints, and the stabilized index 1 method DAE1L_LSE can keep the stability of displacement, velocity, and acceleration constraints at the same time, which achieve the real constraint stabilization.

Figure 5.

The constraints of the single pendulum by DAE2L_LE.

Figure 6.

The constraints of the single pendulum by DAE2L_LSE.

Figure 7.

The constraints of the single pendulum by DAE1L_LSE.

Figures 8–10 indicate the results of the rotation matrix R . Figure 9 shows the time history of the nine elements of the rotation matrix R during the simulation of the 3D single pendulum by the DAE1_IELS. Figure 10 illustrates the orthogonality error of the rotation matrix R , which is defined as $e\mathit{R}={\mathit{R}}^{\mathrm{T}}\mathit{R}-\mathit{I}$ , where $\mathit{I}$ is the identity matrix. Figure 11 demonstrates that the orthogonality of the rotation matrix is kept well during longer time simulation.

Figure 8.

The rotation matrix of the single pendulum by DAE1_IELS (t = 1 s).

Figure 9.

The orthogonality error of the rotation matrix of the single pendulum by DAE1_IELS (t = 1 s).

Figure 10.

The orthogonality error of the rotation matrix of the single pendulum by DAE1_IELS (t = 10 s).

Figure 11.

A 3D double pendulum.

Table 1 shows the errors of the total energy and constraints of the 3D single pendulum obtained by different Lie group Euler methods DAE3L_LE, DAE2L_LE, DAE2L_LSE, DAE1L_LSE, DAE3H_LE, DAE2H_LE, DAE2H_LSE, and DAE1H_LSE. ε(H) means the absolute error of the total energy H; δ(H) means the relative error of the total energy H; and ε(Φ), $\epsilon (\stackrel{·}{\mathit{\Phi }})$ , and $\epsilon (\stackrel{··}{\mathit{\Phi }})$ mean the absolute error of the displacement constraints Φ, the velocity constraints $\stackrel{·}{\mathit{\Phi }}$ , and the acceleration constraints $\stackrel{··}{\mathit{\Phi }}$ . The results indicate that the maximum errors of constraints obtained by the constraint-stabilized method DAE1L_LSE and DAE1H_LSE are all kept in a very small range. The relative errors of the total energy H are smaller than other methods. This ensures the accuracy of the simulation results.

Table 1.

The errors of the total energy and constraints of the single pendulum by different Lie group Euler methods (t = 10 s).

	Max|ε(H)|	Max|δ(H)|	Max|ε(Φ)|	$\mathrm{Max}|\epsilon (\stackrel{·}{\mathit{\Phi }})|$	$\mathrm{Max}|\epsilon (\stackrel{··}{\mathit{\Phi }})|$
DAE3L-LE	26.4216	0.0049	2.2204 × 10−16	0.0047	346.1551
DAE2L-LE	11.1966	0.0021	0.0171	2.6645 × 10−15	0.6665
DAE2L-LSE	9.9687	0.0018	3.3307 × 10−15	2.6645 × 10−15	0.6661
DAE1L-LSE	9.9687	0.0018	3.3307 × 10−16	3.5527 × 10−15	6.8212 × 10−13
DAE3H-LE	26.4216	0.0049	2.2204 × 10−16	0.0047	346.1551
DAE2H-LE	11.1966	0.0021	0.0171	3.5527 × 10−15	0.6665
DAE2H-LSE	11.1957	0.0021	1.0034 × 10−13	2.9310 × 10−14	0.6658
DAE1H-LSE	11.1957	0.0021	3.3307 × 10−16	3.5527 × 10−15	9.1397 × 10−11

Figure 11 illustrates a 3D double pendulum, in which the inertia matrix of the system is ${\mathit{M}}_i=\mathrm{diag}(\begin{array}{cc}{\mathit{m}}_i\hfill & {\mathit{J}}_i\hfill \end{array})$ , where ${\mathit{m}}_i=\mathrm{diag}({\overline{m}}_i,{\overline{m}}_i,{\overline{m}}_i)$ , ${\mathit{J}}_i=\mathrm{diag}(J_{i1},J_{i2},J_{i3})$ , $i=1,2$ , $l_1=l_2=1\mathrm{m}$ , and ${\overline{m}}_1={\overline{m}}_2=5\mathrm{kg}$ , and the generalized force vector is ${\mathit{Q}}_i(\mathit{q},\mathit{V})=\left[\begin{array}{c}-{\mathit{m}}_i\mathit{g}\hfill \\ \multicolumn{1}{c}{-{\tilde{\mathit{\Omega }}}_i{\mathit{J}}_i{\mathbf{\Omega }}_i}\end{array}\right]$ , $i=1,2$ , $\mathit{g}={\left[\begin{array}{ccc}0\hfill & 0\hfill & 9.81\hfill \end{array}\right]}^{\mathrm{T}}$ ; the constraint equations are

$$\begin{array}{c}{\mathbf{\Phi }}_1(\mathit{q})={\mathit{x}}_1+{\mathit{R}}_1{\mathit{X}}_{10},{\mathit{X}}_{10}={\left(\begin{array}{ccc}0\hfill & 0\hfill & -l_1/l_{1}22\hfill \end{array}\right)}^{\mathrm{T}}\hfill \\ {\mathbf{\Phi }}_2(\mathit{q})={\mathit{x}}_1+{\mathit{R}}_1{\mathit{X}}_{11}-{\mathit{x}}_2-{\mathit{R}}_2{\mathit{X}}_{21},\hfill \\ {\mathit{X}}_{11}={\left(\begin{array}{ccc}0\hfill & 0\hfill & l_1/l_{1}22\hfill \end{array}\right)}^{\mathrm{T}},{\mathit{X}}_{21}={\left(\begin{array}{ccc}0\hfill & 0\hfill & -l_2/l_{2}22\hfill \end{array}\right)}^{\mathrm{T}}\hfill \end{array}$$

The initial values are

$$\begin{array}{c}{\mathit{R}}_1(0)=\mathit{E},{\mathit{R}}_2(0)=\mathit{E},{\mathbf{\Omega }}_1(0)={\left[\begin{array}{ccc}10\pi \hfill & 0\hfill & 0\hfill \end{array}\right]}^T,\hfill \\ {\mathbf{\Omega }}_2(0)={\left[\begin{array}{ccc}10\pi \hfill & 10\pi \hfill & 20\pi \hfill \end{array}\right]}^T\hfill \end{array}$$

Figures 12–14 show the results obtained by using the DAE1L_LES method with time step h = 0.00001 and simulation time t = 1 s. Figure 12 demonstrates the space trajectory of the two mass centers of the 3D double pendulum, the time history of the absolute position of the two mass centers, and the time history of the Lagrange multipliers. Figure 13 shows the kinetic, potential, and total energy of the system. Figure 14 indicates the displacement, velocity, and acceleration constraints. It is shown that the simulation trajectory has higher accuracy, and the total energy and the displacement, velocity, and acceleration constraints are all kept well in the smaller time step case for longer time simulation.

Figure 12.

The trajectory of the two mass centers of the 3D double pendulum by DAE1_IELS.

Figure 13.

The energies of the double pendulum by DAE1_IELS.

Figure 14.

The constraints of the double pendulum by DAE1_IELS.

Figures 15 and 16 show the time history of the nine elements and the orthogonality errors of the rotation matrix R during the simulation of the 3D double pendulum by the DAE1_IELS. For clarity, the simulation time displays to 0.2 s. It is indicated that the orthogonality of the rotation matrix is kept well during simulation.

Figure 15.

The rotation matrix of the double pendulum by DAE1_IELS.

Figure 16.

The orthogonality error of the rotation matrix of the double pendulum by DAE1_IELS.

Table 2 illustrates the errors of the total energy and constraints of the 3D double pendulum obtained by different Lie group Euler methods. It is shown that the maximum errors of constraints by the constraint-stabilized method DAE1L_LSE and DAE1H_LSE are all kept in a very small range. The relative errors of the total energy H are smaller than other methods. This ensures the accuracy of the simulation results.

Table 2.

The errors of the total energy and constraints of the double pendulum by different Lie group Euler methods (t = 1 s).

	Max|ε(H)|	Max|δ(H)|	Max|ε(Φ)|	Max|ε(Φ)|	Max|ε(Φ)|
DAE3-IEL	179.2942	0.0237	3.3307 × 10−16	0.0093	740.1607
DAE2-IEL	96.0559	0.0127	0.0047	1.9540 × 10−14	0.5730
DAE2-IELS	96.0631	0.0127	3.7192 × 10−15	1.2079 × 10−13	0.5732
DAE1-IELS	96.0631	0.0127	3.3307 × 10−16	2.1316 × 10−14	2.7285 × 10−12
DAE3-SEL	179.2942	0.0237	3.3307 × 10−16	0.0093	740.1607
DAE2-SEL	96.0559	0.0127	0.0047	2.1316 × 10−14	0.5730
DAE2-SELS	96.2233	0.0127	1.7319 × 10−14	4.6896 × 10−13	0.5849
DAE1-SELS	96.2233	0.0127	3.3307 × 10−16	2.4869 × 10−14	4.7748 × 10−12

Conclusion

Constraint-stabilized equations on Lie group of Lagrangian and Hamiltonian dynamics are formulated in this article. The implicit Euler method DAE1L_LSE is presented to solve the Euler–Lagrange equations. The symplectic Euler method DAE1H_LSE is presented to solve the constrained Hamiltonian equations. The methods presented here can keep the stability of the displacement, velocity, and acceleration constraints at the same time during the longer time simulation. Numerical examples verified the accuracy of the constraint-stabilized methods and the orthogonality preservation of the rotation matrix. Based on these methods, higher order algorithms can be easily designed to improve the accuracy and efficiency, so that they can be used in the large-scale system simulations.