Singularity-free time integration of rotational quaternions using non-redundant ordinary differential equations

Abstract

A novel ODE time stepping scheme for solving rotational kinematics in terms of unit quaternions is presented in the paper. This scheme inherently respects the unit-length condition without including it explicitly as a constraint equation, as it is common practice. In the standard algorithms, the unit-length condition is included as an additional equation leading to kinematical equations in the form of a system of differential-algebraic equations (DAEs). On the contrary, the proposed method is based on numerical integration of the kinematic relations in terms of the instantaneous rotation vector that form a system of ordinary differential equations (ODEs) on the Lie algebra $\mathit{so}(3)$ of the rotation group $\mathit{SO}(3)$. This rotation vector defines an incremental rotation (and thus the associated incremental unit quaternion), and the rotation update is determined by the exponential mapping on the quaternion group. Since the kinematic ODE on $\mathit{so}(3)$ can be solved by using any standard (possibly higher-order) ODE integration scheme, the proposed method yields a non-redundant integration algorithm for the rotational kinematics in terms of unit quaternions, avoiding integration of DAE equations. Besides being ‘more elegant’—in the opinion of the authors—this integration procedure also exhibits numerical advantages in terms of better accuracy when longer integration steps are applied during simulation. As presented in the paper, the numerical integration of three non-linear ODEs in terms of the rotation vector as canonical coordinates achieves a higher accuracy compared to integrating the four (linear in ODE part) standard-quaternion DAE system. In summary, this paper solves the long-standing problem of the necessity of imposing the unit-length constraint equation during integration of quaternions, i.e. the need to deal with DAE’s in the context of such kinematical model, which has been a major drawback of using quaternions, and a numerical scheme is presented that also allows for longer integration steps during kinematic reconstruction of large three-dimensional rotations.

Appendices

Appendix A: Differential of the exponential map and its inverse

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

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

(A.1)

where the function $\operatorname {dexp}_{ - \bar{w}}$ is defined as

$$\begin{aligned} \operatorname {dexp}_{ - \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( - \operatorname {ad}_{\bar{w}}^{j}(w)\bigr), \end{aligned}$$

(A.2)

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

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

(A.3)

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

$$ \operatorname {dexp}_{ - \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( - \operatorname {ad}_{\bar{w}}^{j}(w)\bigr), $$

(A.4)

where $B_{j}$ are Bernoulli numbers [39]; see also [40]. In above expressions, please note that Eqs. (A.2) and (A.4) are derived under assumption of the ‘left trivialization’ expression in (A.1). This is in accordance with our formulation where the left invariant vector field $\tilde{\boldsymbol{\omega}}_{i} \in \mathit{so}(3)$ is used in the presented study. 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}\exp \bigl(\bar{w}(t)\bigr) = \operatorname {dexp}_{\bar{w}(t)}\bigl(\dot{ \bar{w}}(t)\bigr)\exp \bigl(\bar{w}(t)\bigr). $$

(A.5)

This would lead to expressions of $\operatorname {dexp}_{\bar{w}}$ and $\operatorname {dexp}_{\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 $\operatorname {dexp}_{\bar{w}}$ and $\operatorname {dexp}_{\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 ]$.

Appendix B: Newly proposed ODE-based quaternion integration algorithm that inherently respects the unit-length condition without including it explicitly as a constraint equation

By assuming the fourth-order required integration accuracy, the proposed algorithm can be written in the form

$$\begin{gathered} \boldsymbol{\omega}_{0}, \mathrm{q}_{0}, \Delta t, \\ K_{1} = \Delta t \cdot \operatorname {dexp}(\boldsymbol{\omega}_{0}, \boldsymbol{0}), \\ k_{1} = \Delta t \cdot f(\boldsymbol{\omega}_{0}, \mathrm{q}_{0}), \\ K_{2} = \Delta t \cdot \operatorname {dexp}\biggl( \boldsymbol{\omega}_{0} + \frac{1}{2}k_{1}, \frac{1}{2}K_{1} \biggr), \\ k_{2} = \Delta t \cdot f \biggl( \boldsymbol{\omega}_{0} + \frac{1}{2}k_{1}, \mathrm{q}_{0} \circ \exp_{\boldsymbol{\mathcal{S}}^{3}} \biggl( \frac{1}{2}K_{1} \biggr) \biggr), \\ \vdots \\ K_{4} = \Delta t \cdot \operatorname {dexp}(\boldsymbol{\omega}_{0} + k_{3}, K_{3}), \\ k_{4} = \Delta t \cdot f\bigl(\boldsymbol{\omega}_{0} + k_{3}, \mathrm{q}_{0} \circ \exp_{\boldsymbol{\mathcal{S}}^{3}}(K_{3}) \bigr), \\ \mathrm{q}_{\mathrm{out}} = \mathrm{q}_{0} \circ \exp_{\boldsymbol{\mathcal{S}}^{3}} \biggl( \frac{1}{6}(K_{1} + 2K_{2} + 2K_{3} + K_{4}) \biggr), \\ \boldsymbol{\omega}_{\mathrm{out}} = \boldsymbol{\omega}_{0} + \frac{1}{6}(k_{1} + 2k_{2} + 2k_{3} + k_{4}), \end{gathered}$$

(B.1)

where $f(\boldsymbol{\omega},\mathrm{q})$ represents determination of the derivative of the angular velocity $\boldsymbol{\omega}$ (by using Euler’s rotational equation, for example) and $\operatorname {dexp}$ is given by (5) and (7). Lower-order integration algorithms can be written accordingly.

Appendix C

3.1 C.1 Formulation I

The first formulation of rigid body rotational dynamics that includes standard quaternion integration algorithm is described in [6, Chap. 11.2]. Here, the rotational dynamics equations of motion and unit-length quaternion constraint equation $\mathrm{q}_{i}^{T}\ddot{\mathrm{q}} + \dot{\mathrm{q}}_{i}^{T}\dot{\mathrm{q}}_{i} = 0$ at the acceleration level are assembled in the matrix form, which yields

$$ \left[ \textstyle\begin{array}{c@{\quad}c} \mathbf{J}^{*} & \mathrm{q}\\ \mathrm{q}^{T} & 0 \end{array}\displaystyle \right] \left[ \textstyle\begin{array}{c} \ddot{\mathrm{q}}\\ \boldsymbol{\sigma} \end{array}\displaystyle \right] + \left[ \textstyle\begin{array}{c} 2\mathbf{H}\dot{\mathrm{q}} \\ \dot{\mathrm{q}}^{T}\dot{\mathrm{q}} \end{array}\displaystyle \right] = \left[ \textstyle\begin{array}{c} \mathbf{n}^{*} \\ 0 \end{array}\displaystyle \right], $$

(C.1)

where $\mathbf{n}^{*} = 2\mathbf{L}^{T}\mathbf{n}$ is the sum of the moments acting on the body expressed in terms of four components. Here

$$ \mathbf{J}^{*} = 4\mathbf{L}^{T}\mathbf{J}\mathbf{L}, $$

(C.2)

is the inertia tensor expressed in terms of four components and

$$ \mathbf{L} = \left[ \textstyle\begin{array}{c@{\quad}c@{\quad}c@{\quad}c} - q_{1} & q_{0} & q_{3} & - q_{2}\\ - q_{2} & - q_{3} & q_{0} & q_{1}\\ - q_{3} & q_{2} & - q_{1} & q_{0} \end{array}\displaystyle \right] $$

(C.3)

is the Euler parameters transformation matrix, while $\mathbf{H}$ is given as

$$ \mathbf{H} = 4\dot{\mathbf{L}}^{T}\mathbf{J}\mathbf{L}. $$

(C.4)

Using $\mathbf{L}$, it can be shown that

$$ \dot{\mathrm{q}} = \frac{1}{2}\mathbf{L}^{T}\boldsymbol{\omega}, $$

(C.5)

which represents the identity between the time derivative of the Euler parameters and the angular velocity vector. This means that the first formulation leads to the following integration algorithm:

$$\begin{gathered} \mathrm{q}_{0}, \dot{\mathrm{q}}_{0}, \Delta t, \\ K_{1} = \Delta t \cdot \dot{\mathrm{q}}_{0}, \\ k_{1} = \Delta t \cdot f(\mathrm{q}_{0},\dot{ \mathrm{q}}_{0}), \\ K_{2} = \Delta t \cdot \biggl( \dot{\mathrm{q}}_{0} + \frac{1}{2}k_{1} \biggr), \\ k_{2} = \Delta t \cdot f\biggl(\mathrm{q}_{0} + \frac{1}{2}K_{1}, \dot{\mathrm{q}}_{0} + \frac{1}{2}k_{1}\biggr), \\ \vdots \\ K_{4} = \Delta t \cdot ( \dot{\mathrm{q}}_{0} + k_{3} ), \\ k_{4} = \Delta t \cdot f(\mathrm{q}_{0} + K_{3}, \dot{\mathrm{q}}_{0} + k_{3}), \\ \mathrm{q}_{\mathrm{out}} = \mathrm{q}_{0} + \frac{1}{6}(K_{1} + 2K_{2} + 2K_{3} + K_{4}), \\ \dot{\mathrm{q}}_{\mathrm{out}} = \dot{\mathrm{q}}_{0} + \frac{1}{6}(k_{1} + 2k_{2} + 2k_{3} + k_{4}), \end{gathered}$$

(C.6)

where $f(\mathrm{q},\dot{\mathrm{q}})$ represents determination of the second derivative of the quaternion $\mathrm{q}$ (by using Eq. (C.1)). Here it should be emphasized that, after each integration step (C.6), the numerically calculated quaternions must be stabilized by using additional explicit constraint equation:

$$ \mathrm{q} = \frac{1}{\sqrt{1 + \delta}} \mathrm{q}_{\mathrm{out}}, $$

(C.7)

where $\delta = \mathrm{q}_{\mathrm{out}}^{T}\mathrm{q}_{\mathrm{out}} - 1$ is the quaternion unit-length constraint violation and $\mathrm{q}_{\mathrm{out}}$ is numerical value obtained after integration step (C.6), and before applying the unit-length correction equation (C.7) that provides the final body attitude for a step. Furthermore, besides the stabilization procedure at the position level, the first formulation also requires stabilization at the velocity level. This algorithm resembles stabilization procedure at the position level, being based on the correction formula for the velocities:

$$ \dot{\mathrm{q}} = \dot{\mathrm{q}}_{\mathrm{out}} - \sigma \mathrm{q}, $$

(C.8)

where $\sigma = \dot{\mathrm{q}}_{\mathrm{out}}^{T}\mathrm{q}$ is the quaternion constraint violation at the velocity level.

3.2 C.2 Formulation II

Here, see [6, Chap. 11.2], the rotational equations of motion are formulated via body angular velocity $\boldsymbol{\omega}$ in the form of Euler equations:

$$ \mathbf{J}\dot{\boldsymbol{\omega}} + \tilde{\boldsymbol{\omega}} \mathbf{J}\boldsymbol{\omega} = \mathbf{n}, $$

(C.9)

which leads to the following rotational dynamics integration algorithm in terms of quaternions (description of all variables is the same as above)

$$\begin{gathered} \boldsymbol{\omega}_{0}, \mathrm{q}_{0}, \Delta t, \\ K_{1} = \Delta t \cdot \frac{1}{2} \cdot \mathbf{L}^{T}( \mathrm{q}_{0}) \cdot \boldsymbol{\omega}_{0}, \\ k_{1} = \Delta t \cdot f(\boldsymbol{\omega}_{0}, \mathrm{q}_{0}), \\ K_{2} = \Delta t \cdot \frac{1}{2} \cdot \mathbf{L}^{T} \biggl( \mathrm{q}_{0} + \frac{1}{2}K_{1} \biggr) \cdot \biggl( \boldsymbol{\omega}_{0} + \frac{1}{2}k_{1} \biggr), \\ k_{2} = \Delta t \cdot f\biggl(\boldsymbol{\omega}_{0} + \frac{1}{2}k_{1}, \mathrm{q}_{0} + \frac{1}{2}K_{1} \biggr), \\ \vdots \end{gathered}$$

(C.10)

$$\begin{gathered} K_{4} = \Delta t \cdot \frac{1}{2} \cdot \mathbf{L}^{T}( \mathrm{q}_{0} + K_{3}) \cdot (\boldsymbol{ \omega}_{0} + k_{3}), \\ k_{4} = \Delta t \cdot f(\boldsymbol{\omega}_{0} + k_{3}, \mathrm{q}_{0} + K_{3}), \\ \mathrm{q}_{\mathrm{out}} = \mathrm{q}_{0} + \frac{1}{6}(K_{1} + 2K_{2} + 2K_{3} + K_{4}), \\ \boldsymbol{\omega}_{\mathrm{out}} = \boldsymbol{\omega}_{0} + \frac{1}{6}(k_{1} + 2k_{2} + 2k_{3} + k_{4}). \end{gathered}$$

(C.11)

As was the case within the framework of Formulation I, after the integration step (C.10) the integration result $\mathrm{q}_{\mathrm{out}}$ must be re-normalized by using (C.7).