Lie-group integration method for constrained multibody systems in state space

Abstract

Coordinate-free Lie-group integration method of arbitrary (and possibly higher) order of accuracy for constrained multibody systems (MBS) is proposed in the paper. Mathematical model of MBS dynamics is shaped as a DAE system of equations of index 1, whereas dynamics is evolving on the system state space modeled as a Lie-group. Since the formulated integration algorithm operates directly on the system manifold via MBS elements’ angular velocities and rotational matrices, no local rotational coordinates are necessary, and kinematical differential equations (that are prone to singularities in the case of three-parameter-based local description of the rotational kinematics) are completely avoided. Basis of the integration procedure is the Munthe–Kaas algorithm for ODE integration on Lie-groups, which is reformulated and expanded to be applicable for the integration of constrained MBS in the DAE-index-1 form. In order to eliminate numerical constraint violation for generalized positions and velocities during the integration procedure, a constraint stabilization projection method based on constrained least-square minimization algorithm is introduced. Two numerical examples, heavy top dynamics and satellite with mounted 5-DOF manipulator, are presented. The proposed Lie-group DAE-index-1 integration scheme is easy-to-use for an MBS with kinematical constraints of general type, and it is especially suitable for dynamics of mechanical systems with large 3D rotations where standard (vector space) formulations might be inefficient due to kinematical singularities (three-parameter-based rotational coordinates) or additional kinematical constraints (redundant quaternion formulations).

Appendices

Appendix A: Differential of the exponential map

Starting from the rotational motion of one body, we introduce the differential of the exponential mapping $\operatorname{dexpm}: \mathit{so}(3) \times\mathit{so}(3) \to\mathit{so}(3)$ via ‘left trivialized’ tangent of the matrix exponential map ‘expm’ in a way that the following expression is valid:

$$ \frac{\mathrm{d}}{\mathrm{d}t}\operatorname{expm} \bigl(\bar{w}(t)\bigr) = \operatorname{expm}\bigl(\bar{w}(t)\bigr) \mathrm{dexpm}_{ - \bar{w}(t)} \bigl(\dot{\bar{w}}(t)\bigr), $$

(A.1)

where the function $\mathrm{dexpm}_{ - \bar{w}}$ is defined as

$$\begin{aligned} \mathrm{dexpm}_{ - \bar{w}}(w) &= w - \frac{1}{2!} [ \bar{w},w ] + \frac{1}{3!} \bigl[ \bar{w}, [ \bar{w},w ] \bigr] + \frac{1}{4!} \bigl[ \bar{w}, \bigl[ \bar{w}, [ \bar{w},w ] \bigr] \bigr] +\cdots \\ &= \sum_{j = 0}^{\infty} \frac{1}{(j + 1)!} \bigl( - \mathrm{ad}_{\bar{w}}^{j}(w)\bigr), \end{aligned}$$

(A.2)

and the adjoint operator $\mathrm{ad}_{\bar{w}}$ is given as the Lie bracket

$$ \mathrm{ad}_{\bar{w}}(w) = \bar{w}w - w\bar{w} = [ \bar{w},w ], \quad \mbox{for all}\ w(t), \bar{w}(t) \in\mathit{so}(3). $$

(A.3)

Furthermore, the inverse function $\mathrm{dexpm}_{ - \bar{w}}^{ - 1}$ is defined by

$$ \mathrm{dexpm}_{ - \bar{w}}^{ - 1}(w) = w + \frac{1}{2} [ \bar{w},w ] + \frac{1}{12} \bigl[ \bar{w}, [ \bar{w},w ] \bigr] +\cdots = \sum_{j = 0}^{\infty} \frac{B_{j}}{j!} \bigl( - \mathrm{ad}_{\bar{w}}^{j}(w)\bigr), $$

(A.4)

where B _j are Bernoulli numbers [39]. Before we proceed, please note that Eqs. (A.2) and (A.4) are derived under the assumption of the ‘left trivialization’ expression in (A.1). This is in accordance with our formulation of the Lie-algebra , where the left-invariant vector field $\tilde{\boldsymbol{\omega}}_{i} \in\mathit{so}(3)$ is used in its definition. However, in the literature, the differential of the exponential mapping is usually defined by using the ‘right trivialized’ formulation in the form

$$ \frac{\mathrm{d}}{\mathrm{d}t}\operatorname{expm} \bigl(\bar{w}(t)\bigr) = \mathrm{dexpm}_{\bar{w}(t)} \bigl(\dot{\bar{w}}(t)\bigr)\operatorname{expm} \bigl(\bar{w}(t)\bigr). $$

(A.5)

This would lead to expressions of $\mathrm{dexpm}_{\bar {w}}$ and $\mathrm{dexpm}_{\bar{w}}^{ - 1}$ with Lie brackets that would appear with different signs in (A.2) and (A.4). However, please note that if the right trivialization had been used, the final expressions for $\mathrm{dexpm}_{\bar{w}}$ and $\mathrm{dexpm}_{\bar{w}}^{ - 1}$ would have differed from (A.2) and (A.4) only in the sign of the second term $\pm \frac{1}{2} [ \bar{w},w ]$.

After deriving the differential of the exponential map for the rotational motion of the rigid body, we need to include its translational part as well. Accordingly, the differential of the exponential mapping for the unconstrained body i motion $\boldsymbol{\mathcal {R}}^{3} \times\mathit {SO}(3)$ is given by

$$ \operatorname{dexp}_{ - \bar{z}_{i}}(z_{i}) = \operatorname{dexp}_{( - \bar {\mathbf{v}}_{i}, - \bar{w}_{i})} (\mathbf{v}_{i},w_{i}) = \bigl(\mathbf{v}_{i}, \mathrm{dexpm}_{ - \bar {w}_{i}}(w_{i})\bigr), $$

(A.6)

(where $\mathrm{dexpm}_{ - \bar{w}_{i}}$ function is introduced as above), and its inverse is readily given as

$$ \operatorname{dexp}_{ - \bar{z}_{i}}^{ - 1}(z_{i}) = \operatorname{dexp}_{_{( - \bar{\mathbf{v}}_{i}, - \bar{w}_{i})}}^{ - 1}(\mathbf{v}_{i},w_{i}) = \bigl(\mathbf{v}_{i}, \mathrm{dexpm}_{ - \bar{w}_{i}}^{ - 1}(w_{i})\bigr). $$

(A.7)

With the above results, we can define the differential of the exponential mapping for the whole unconstrained MBS system as the function given by

$$ \operatorname{dexp}_{ - \bar{z}}(z) = \bigl(\operatorname{dexp}_{ - \bar{z}_{1}}(z_{1}),\ldots, \operatorname{dexp}_{ - \bar{z}_{k}}(z_{k}),\dot{\mathbf{v}}_{1},\dot {w}_{1},\ldots,\dot{\mathbf{v}}_{k},\dot{w}_{k}\bigr), $$

(A.8)

and its inverse can be written as

$$ \operatorname{dexp}_{ - \bar{z}}^{ - 1}(z) = \bigl(\operatorname{dexp}_{ - \bar{z}_{1}}^{ - 1}(z_{1}),\ldots,\operatorname{dexp}_{ - \bar {z}_{k}}^{ - 1}(z_{k}),\dot{\mathbf{v}}_{1},\dot{w}_{1},\ldots,\dot{\mathbf {v}}_{k},\dot{w}_{k}\bigr). $$

(A.9)

Appendix B: Derivation of algebraic equations imposed by the fixed-point constraint

In order to model fixed-point constraint, three algebraic constraint equations at the generalized displacement level can be written as

$$ \boldsymbol{\Phi}^{\mathrm{FP}}=\mathbf{r}_1^{\mathrm {FP}}-\mathbf{r}_1-\mathbf{R}_i\mathrm{r}_{b_1}^{\mathrm{FP}}=0, $$

(B.1)

where $\mathbf{r}_{b_{1}}^{\mathrm{FP}}$ represents the location of the fixed-point constraint in the body 1 local coordinate system that is fixed to the body (see Fig. 26), r ₁ is the position of the mass centre of body 1 in the global coordinate system, and $\mathbf{r}_{1}^{\mathrm{FP}}$ is a constant vector that determines, in the global coordinate system, the position of the body point that is fixed in space.

Fig. 26

Fixed-point constraint position vectors

After differentiating with respect to time, the velocity level constraint is obtained in the form

$$ \dot{\boldsymbol{\Phi}}^{\mathrm{FP}}=-\mathbf{v}_1-\mathbf {R}_1\tilde{\boldsymbol{\omega}}_1 \mathbf{r}_{b_1}^{\mathrm{FP}}=\mathbf{0}, $$

(B.2)

whereas additional differentiation yields an acceleration constraint in the form

$$ \ddot{\boldsymbol{\Phi}}^{\mathrm{FP}}=-\dot{\mathbf {v}}_1-\mathbf{R}_1 \tilde{\mathbf{r}}_{b_1}^{\mathrm{FP}} \dot{\boldsymbol{\omega}}_1-\mathbf{R}_1\tilde{\boldsymbol{\omega }}_1\tilde{\boldsymbol{\omega}}_1 \mathbf{r}_{b_1}^{\mathrm{FP}} =\mathbf{0}. $$

(B.3)

To obtain the acceleration constraint in the form that can be included in the system dynamics governing equations (13) the acceleration constraint (B.3) can be written as

$$ \mathbf{C}_1^{\mathrm{FP}(3\times6)}\dot{\mathbf{v}}_1^{(6\times1)}= \boldsymbol{\xi}_1^{\mathrm{FP}(3\times1)}, $$

(B.4)

where $\dot{\mathbf{v}}_{1}^{(6 \times1)} = [\dot{\mathbf{v}}_{1}^{T} \ \dot{\boldsymbol{\omega}}_{1}^{T} ]^{T}$ is the generalized acceleration vector of body 1, and the fixed-point constraint matrix $\mathbf{C}_{1}^{\mathrm{FP}(3\times6)}$ and the right-hand-side acceleration term $\boldsymbol{\xi}_{1}^{\mathrm{FP}(3\times1)}$ are given as

$$ \mathbf{C}_1^{\mathrm{FP}(3\times6)}=\bigl[ -\mathbf{I}_3\quad\mathbf{R}_1\tilde{\mathbf{r}}_{b_1}^{\mathrm{FP}} \bigr], $$

(B.5)

and

$$ \boldsymbol{\xi}_1^{\mathrm{FP}(3\times1)}=\mathbf{R}_1\tilde{\boldsymbol {\omega}}_1 \tilde{\boldsymbol{\omega}}_1 \mathbf{r}_{b_1}^{\mathrm{FP}}. $$

(B.6)