Kinematics

Kinematics#

The eqm module in c4dynamics provides a collection of functions for integrating the equations of motion of data points and rigid bodies in the three-dimensional space.

Background Material#

Introduction#

Motion models for points (particles) and rigid bodies in space and time are based on mathematical equations.

Three degrees of freedom models employ translational equations of motion. Six degrees of freedom models incorporate both translational and rotational equations of motion.

The inputs to the equations of motion are the forces and moments acting on the body; yielding body accelerations as outputs [MI].

Nomenclature and Convention#

Typically, the forces and moments on a body are resolved into components in the body coordinate system. Fig-1 shows the components of force, moment velocity, and angular rate of a body resolved in the body coordinate system. The six projections of the linear and angular velocity vectors on the moving body frame axes are the six degrees of freedom. The nomenclature and conventions for positive directions are as shown in Fig-1 and in the following Table:

../_images/rigidbody.svg

Fig-1: Forces, velocities, moments, and angular rates in body reference frame#

Axis

Force along axis

Moment about axis

Linear velocity

Angular displacement

Angular velocity

Moment of Inertia

\(x_b\)

\({F_x}_b\)

\(L\)

\(u\)

\(\varphi\)

\(p\)

\(I_{xx}\)

\(y_b\)

\({F_y}_b\)

\(M\)

\(v\)

\(\theta\)

\(q\)

\(I_{yy}\)

\(z_b\)

\({F_z}_b\)

\(N\)

\(w\)

\(\psi\)

\(r\)

\(I_{zz}\)

The position of the mass center of the body is given by its Cartesian coordinates expressed in an inertial frame of reference, such as the fixed-earth frame \((x, y, z)\).

The body’s angular orientation is defined by three rotations \((\psi, \theta, \varphi)\) relative to the inertial frame of reference. These are called Euler rotations, and the order of the successive rotations is important. Starting with the body coordinate frame aligned with the earth coordinate frame, the adopted order here is 3-2-1, i.e.:

  1. Rotate the body frame about the \(z_b\) axis through the heading angle \(\psi\),

  2. Rotate about the \(y_b\) axis through the pitch angle \(\theta\), and

  3. Rotate about the \(x_b\) axis through the roll angle \(\varphi\)

The total inertial velocity \(V\) has components \(u, v\), and \(w\) on the body frame axes, and \((v_x, v_y, v_z)\) on the earth-frame axes.

Rotation Matrix Convention#

The relation between the body frame and the reference frame is described by a Direction Cosine Matrix (DCM). c4dynamics names a DCM by two letters, read from right to left: \([BR]\) is the rotation matrix from the reference frame \(R\) to the body frame \(B\), i.e. \(v_B = [BR] \cdot v_R\), and \([RB] = [BR]^T\) is the opposite direction. For the 3-2-1 Euler rotations defined above:

\[[BR] = R_x(\varphi) \cdot R_y(\theta) \cdot R_z(\psi)\]

For the reference frames, the rotation matrix notation, the elementary rotations, the conversions between Euler angles, rotation matrices, and quaternions, and examples of rotating vectors between frames, see the Rigid Body Transformations concept page.

In the equations of motion, these matrices are used to move forces, moments, and velocities between the frames. For example, the translational equations below take the forces in the reference frame. A force that is given in the body frame, \(F_B\) (e.g. the thrust of a motor), is first rotated to the reference frame:

\[F_R = [RB] \cdot F_B\]

Similarly, the inertial velocity is resolved in the body frame by:

\[[u, v, w]^T = [BR] \cdot [v_x, v_y, v_z]^T\]

Both rigid body state objects, rigidbody and quatbody, provide their current rotation matrices as the properties BR and RB.

Newton’s Second Law#

Newton’s second law of motion establishes the foundational equation governing the relationship among force, mass, and acceleration.

in the context of Newton’s second law, the force \((F)\) acting on an object is the derivative of its momentum \((m \cdot v)\) with respect to time \((t)\):

\[F = {d(m \cdot v) \over dt}\]

Where:

  • \(F\) is the total force acting on the object

  • \(m\) is the mass of the object

  • \(v\) is the velocity

  • \(t\) is time

This equation yields the final form of the equations of linear motion. In the final form, acceleration is represented by the rate of change of the velocity:

\[F = m \cdot \dot{v}\]

Where:

  • \(F\) is the total force acting on the object

  • \(m\) is the mass of the object

  • \(\dot{v}\) is the acceleration of the object

A direct extension of Newton’s second law to rotational motion reveals that the moment of force (torque) on a body about a given axis equals the time rate of change of the angular momentum of the paricle about that axis.

\[M = {dh \over dt}\]

Where:

  • \(M\) is the total moment (torque) acting on the object

  • \(h\) is the angular momentum vector of the object

Hence, the final form of the equations of angular motion is given by:

\[M = [I] \cdot \dot{\omega}\]

Where:

  • \(M\) is the total moment (torque) acting on the object

  • \([I]\) is the inertia matrix of the body relative to the axis of rotation

  • \(\dot{\omega}\) is the absolute angular acceleration vector of the body

Translational Equations of Motion#

The basis of the translational equation of motion was introduced above. The usual procedure used to solve this equation is to sum the external forces \(F\) acting on the body, express them in an inertial frame, and substitute \(F\) into the equation. Once the acceleration, namely the forces divided by the mass, is expressed in inertial coordinates, it is integrated twice to yield the translational displacement.

\[ \begin{align}\begin{aligned}dx = v_x\\dy = v_y\\dz = v_z\\dv_x = {F[0] \over m}\\dv_y = {F[1] \over m}\\dv_z = {F[2] \over m}\end{aligned}\end{align} \]

Where:

  • \(dx, dy, dz\) are the changes in position in the \(x, y, z\) inertial directions, respectively

  • \(dv_x, dv_y, dv_z\) are the changes in velocity in the \(x, y, z\) inertial directions, respectively

  • \(v_x, v_y, v_z\) are the velocities in the \(x, y, z\) inertial directions, respectively

  • \(f[0], f[1], f[2]\) are the input force components in the \(x, y, z\) inertial directions, respectively

  • \(m\) is the mass of the body.

These equations describe the dynamics of a datapoint in three-dimensional space (3DOF). Which is the rate of change of position \((x, y, z)\) with respect to time equals to the velocity, and the rate of change of velocity \((v_x, v_y, v_z)\) with respect to time equals to the force divided by the mass \((m)\).

Rotational Equations of Motion#

As mentioned earlier, the rotational analog of Newton’s law describes the relationship between torque, moment of inertia, and angular acceleration. We also saw that a double integration on the translational acceleration produces the change of the body in position.

However, the angular accelerations are typically expressed with respect to a body frame and must be adjusted in order to produce the attitude of the body. For that purpose we introduced the euler angles (see Nomenclature and Conventions) which describe the body attitude with respect to an inertial frame of reference.

The orientation of the body reference frame is specified by the three Euler angles, \(\psi, \theta, \varphi\).

As a rigid body changes its orientation in space, the Euler angles change. The rates of change of the Euler angles are related to the angular rates \((p, q, r)\) of the body frame.

The rate of change of the Euler angles together with the rotational analog of Newton’s law provide the set of differential equations that describe the equations governing the motion of a rigid body:

\[ \begin{align}\begin{aligned}d\varphi = p + (q \cdot sin(\varphi) + r \cdot cos(\varphi)) \cdot tan(\theta)\\d\theta = q \cdot cos(\varphi) - r \cdot sin(\varphi)\\d\psi = {q \cdot sin(\varphi) + r \cdot cos(\phi) \over cos(\theta)}\\dp = {M[0] - q \cdot r \cdot (I_{zz} - I_{yy}) \over I_{xx}}\\dq = {M[1] - p \cdot r \cdot (I_{xx} - I_{zz}) \over I_{yy}}\\dr = {M[2] - p \cdot q \cdot (I_{yy} - I_{xx}) \over I_{zz}}\end{aligned}\end{align} \]

Where:

  • \(d\varphi, d\theta, d\psi\) are the changes in Euler roll, Euler pitch, and Euler yaw angles, respectively

  • \(dp, dq, dr\) are the changes in body roll rate, pitch rate, and yaw rate, respectively

  • \(\varphi, \theta, \psi\) are the Euler roll, Euler pitch, and uler yaw angles, respectively

  • \(p, q, r\) are the body roll rate, pitch rate, and yaw rate, respcetively

  • \(M[0], M[1], M[2]\) are the input moment of force components about the \(x, y, z\) in the body direction, respectively

  • \(I_{xx}, I_{yy}, I_{zz}\) are the moments of inertia about the \(x, y,\) and \(z\) in body direction, respectively

These equations describe the angular dynamics of a rigid body. Together with the equations that describe the translational motion of the body they form the six-dimensional motion in space (6DOF).

Quaternion Kinematics#

The Euler angles singularity

The Euler angles rates above contain the terms \(tan(\theta)\) and \(1 / cos(\theta)\). As the pitch angle approaches \(\theta = \pm 90°\), these terms grow without bound and the equations become singular. At this attitude the roll axis and the yaw axis are aligned, \(\varphi\) and \(\psi\) describe the same rotation, and they are not uniquely defined. This phenomenon is known as gimbal lock.

Any set of three angles has such a singular attitude. For vehicles that never approach it, such as a transport aircraft in level flight, the Euler angles are a natural and intuitive choice. Agile vehicles, however, such as drones performing flips, aerobatic aircraft, missiles in a vertical launch, or tumbling spacecraft, may reach any attitude. For those, the attitude is better represented by a quaternion [DI].

The attitude quaternion

According to Euler’s rotation theorem, any attitude can be reached from the reference frame by a single rotation through an angle \(\alpha\) about a unit axis \(n = [n_x, n_y, n_z]\). The attitude quaternion encodes this rotation by four parameters:

\[q = [q_w, q_x, q_y, q_z] = [cos{\alpha \over 2}, \; n_x \cdot sin{\alpha \over 2}, \; n_y \cdot sin{\alpha \over 2}, \; n_z \cdot sin{\alpha \over 2}]\]

Where:

  • \(q_w\) is the scalar part of the quaternion

  • \(q_x, q_y, q_z\) are the vector part of the quaternion

c4dynamics uses the scalar-first (Hamilton) convention.

Four parameters describe three degrees of freedom, therefore the quaternion is subject to one constraint. An attitude quaternion is a unit quaternion:

\[q_w^2 + q_x^2 + q_y^2 + q_z^2 = 1\]

Note that \(q\) and \(-q\) represent the same attitude (a rotation of \(\alpha\) about \(n\) equals a rotation of \(\alpha - 360°\) about the same axis).

Relation to the Euler angles

The quaternion of the attitude that is defined by the 3-2-1 Euler angles \((\psi, \theta, \varphi)\) is given by:

\[ \begin{align}\begin{aligned}q_w = c{\varphi \over 2} c{\theta \over 2} c{\psi \over 2} + s{\varphi \over 2} s{\theta \over 2} s{\psi \over 2}\\q_x = s{\varphi \over 2} c{\theta \over 2} c{\psi \over 2} - c{\varphi \over 2} s{\theta \over 2} s{\psi \over 2}\\q_y = c{\varphi \over 2} s{\theta \over 2} c{\psi \over 2} + s{\varphi \over 2} c{\theta \over 2} s{\psi \over 2}\\q_z = c{\varphi \over 2} c{\theta \over 2} s{\psi \over 2} - s{\varphi \over 2} s{\theta \over 2} c{\psi \over 2}\end{aligned}\end{align} \]

where \(c \equiv cos\) and \(s \equiv sin\).

The inverse relation, from a quaternion to the Euler angles, is given by:

\[ \begin{align}\begin{aligned}\varphi = atan2(2(q_w q_x + q_y q_z), \; 1 - 2(q_x^2 + q_y^2))\\\theta = asin(2(q_w q_y - q_x q_z))\\\psi = atan2(2(q_w q_z + q_x q_y), \; 1 - 2(q_y^2 + q_z^2))\end{aligned}\end{align} \]

The extracted Euler angles are, of course, still singular at \(\theta = \pm 90°\). The quaternion itself is not. It is therefore common to propagate the quaternion and to compute the Euler angles only for presentation.

The rotation matrix from the reference frame to the body frame, \([BR]\) (see Rigid Body Transformations), is given in terms of the quaternion by the following matrix, which is identical to the matrix of the 3-2-1 Euler angles of the same attitude:

\[\begin{split}[BR] = \begin{bmatrix} 1 - 2(q_y^2 + q_z^2) & 2(q_x q_y + q_w q_z) & 2(q_x q_z - q_w q_y) \\ 2(q_x q_y - q_w q_z) & 1 - 2(q_x^2 + q_z^2) & 2(q_y q_z + q_w q_x) \\ 2(q_x q_z + q_w q_y) & 2(q_y q_z - q_w q_x) & 1 - 2(q_x^2 + q_y^2) \end{bmatrix}\end{split}\]

The quaternion rates

The rate of change of the attitude quaternion is related to the angular rates \((p, q, r)\) of the body frame by the quaternion product (\(\otimes\)) of the quaternion and the angular velocity vector:

\[\dot{q} = {1 \over 2} \cdot q \otimes [0, p, q, r]\]

Or equivalently, in a matrix form:

\[\begin{split}\begin{bmatrix} \dot{q}_w \\ \dot{q}_x \\ \dot{q}_y \\ \dot{q}_z \end{bmatrix} = {1 \over 2} \begin{bmatrix} 0 & -p & -q & -r \\ p & 0 & r & -q \\ q & -r & 0 & p \\ r & q & -p & 0 \end{bmatrix} \begin{bmatrix} q_w \\ q_x \\ q_y \\ q_z \end{bmatrix}\end{split}\]

Compared to the Euler angles rates, the quaternion rates:

  • Contain no trigonometric functions and no division, and are therefore free of singularities at any attitude.

  • Are linear in the quaternion components, which makes them cheap to evaluate and well behaved in numerical integration.

Numerical integration, however, does not preserve the unit norm exactly. Small errors accumulate, and the quaternion drifts from a unit quaternion. A standard remedy is to normalize the quaternion after each integration step:

\[q \leftarrow {q \over \|q\|}\]

The quaternion equations of motion

Replacing the Euler angles rates by the quaternion rates yields the quaternion form of the rotational equations of motion. The angular rates equations remain unchanged:

\[ \begin{align}\begin{aligned}dq_w = -{1 \over 2} (q_x \cdot p + q_y \cdot q + q_z \cdot r)\\dq_x = {1 \over 2} (q_w \cdot p + q_y \cdot r - q_z \cdot q)\\dq_y = {1 \over 2} (q_w \cdot q - q_x \cdot r + q_z \cdot p)\\dq_z = {1 \over 2} (q_w \cdot r + q_x \cdot q - q_y \cdot p)\\dp = {M[0] - q \cdot r \cdot (I_{zz} - I_{yy}) \over I_{xx}}\\dq = {M[1] - p \cdot r \cdot (I_{xx} - I_{zz}) \over I_{yy}}\\dr = {M[2] - p \cdot q \cdot (I_{yy} - I_{xx}) \over I_{zz}}\end{aligned}\end{align} \]

Where:

  • \(dq_w, dq_x, dq_y, dq_z\) are the changes in the attitude quaternion components

  • \(q_w, q_x, q_y, q_z\) are the attitude quaternion components

  • the rest of the variables are as defined for the Euler angles equations above.

Note the two different meanings of the symbol \(q\): the quaternion \(q = [q_w, q_x, q_y, q_z]\), and the pitch rate \(q\), the second component of the angular rates \((p, q, r)\).

Together with the translational equations, these equations form a singularity-free 6DOF model with 13 state variables:

\[X = [x, y, z, v_x, v_y, v_z, q_w, q_x, q_y, q_z, p, q, r]^T\]

In c4dynamics, this model is implemented by the quatbody state object, whose equations of motion are given by eqm6q and integrated by int6q. The Euler angles model is implemented by the rigidbody state object with eqm6 and int6. Away from the singularity, both models produce the same motion.

Quadcopter Equations of Motion#

The 6DOF equations above take the forces and the moments as inputs. For a quadcopter, these come from four rotors, aerodynamic drag, and gravity. quadforces computes them, and quadeqm passes them through the rigid body equations: the Euler angles model of eqm6 for a 12-state vector, or the quaternion model of eqm6q for a 13-state vector.

Frames

The model supports the two common pairs of an inertial frame and a body frame. Both use the 3-2-1 rotation of the convention above, so the body-from-inertial matrix is \([BI]\) with no extra rotation:

frame

Inertial frame

Body frame

\(s\)

'NED'

\(x\) north, \(y\) east, \(z\) down

\(x\) forward, \(y\) right, \(z\) down (FRD)

\(-1\)

'ENU'

\(x\) east, \(y\) north, \(z\) up

\(x\) forward, \(y\) left, \(z\) up (FLU)

\(+1\)

The sign \(s\) is the direction of up along the body \(z\) axis. It appears wherever the model depends on the frame: the rotors thrust along \(s \cdot z_b\), and gravity acts along \(-s \cdot z\).

Rotors

Rotor \(i\) at the body position \((x_i, y_i)\) spins at \(\Omega_i\) in the direction \(d_i\) (\(+1\) counterclockwise seen from above, \(-1\) clockwise). It produces a thrust and a reaction torque:

\[F_i = k_T \cdot \Omega_i^2, \qquad Q_i = k_Q \cdot \Omega_i^2\]

The default rotor layout is an \(X\) configuration with the moment arm \(l\), in the motor order front-right, rear-left, front-left, rear-right, and \(d = [1, 1, -1, -1]\). Any other layout is given by explicit rotor positions and directions.

Forces

The body force is the total thrust along the body up axis and a linear drag that opposes the body velocity relative to the air:

\[\begin{split}F_b = \begin{bmatrix} -A_x \cdot u \\ -A_y \cdot v \\ s \cdot \sum F_i - A_z \cdot w \end{bmatrix}, \qquad \begin{bmatrix} u \\ v \\ w \end{bmatrix} = [BI] \cdot (V - V_{wind})\end{split}\]

The force in the inertial frame, which enters the translational equations of motion, adds gravity:

\[\begin{split}F = [BI]^T \cdot F_b + \begin{bmatrix} 0 \\ 0 \\ -s \cdot m \cdot g \end{bmatrix}\end{split}\]

\(F_b / m\) is the specific force, i.e. what an accelerometer at the center of mass measures.

Moments

The thrust of each rotor acts at its position and produces roll and pitch moments. The reaction torques produce the yaw moment: a counterclockwise rotor turns the body clockwise seen from above. A rotational drag opposes the angular rates, and the angular momentum of the rotors, \(h = s \cdot I_R \cdot \sum d_i \cdot \Omega_i\) along the body \(z\) axis, adds a gyroscopic coupling \(-\omega \times h\):

\[ \begin{align}\begin{aligned}M[0] = s \cdot \sum y_i \cdot F_i - A_r \cdot p - q \cdot h\\M[1] = -s \cdot \sum x_i \cdot F_i - A_r \cdot q + p \cdot h\\M[2] = -s \cdot \sum d_i \cdot Q_i - A_r \cdot r\end{aligned}\end{align} \]

Where:

  • \(k_T, k_Q\) are the rotor thrust and torque coefficients

  • \(A_x, A_y, A_z\) are the linear drag coefficients along the body axes

  • \(A_r\) is the rotational drag coefficient

  • \(I_R\) is the rotor moment of inertia

  • \(V, V_{wind}\) are the inertial velocity of the body and the wind velocity

  • \(m, g\) are the mass and the gravity acceleration

  • the rest of the variables are as defined above.

\(F\) and \(M\) are the inputs of the translational and the rotational equations of motion of the previous sections.

Integration

quadeqm has the signature of scipy.integrate.solve_ivp. The vehicle parameters are attributes of the state object, and the rotor speeds are held constant over a step:

from scipy.integrate import solve_ivp
from c4dynamics.models.quad import default_quad_config

quad = c4d.rigidbody()
for k, v in default_quad_config().items():
    setattr(quad, k, v)

sol = solve_ivp(c4d.eqm.quadeqm, [t, t + dt], quad.X, args = (quad, rotor_speeds, 'ENU'))
quad.X = sol.y[:, -1]

A quatbody instead of a rigidbody selects the quaternion model. The Cascade-PID and the EKF quadcopter examples fly this model.

The rotmat Module#

The rotmat module implements the rotation operations used on this page: the elementary rotations, the 3-2-1 DCM, and the conversions between Euler angles, rotation matrices, and quaternions. rigidbody computes \([BR]\) by dcm321, and quatbody by quat2dcm.

For the functions and examples, see the Rigid Body Transformations concept page.

References#

[MI]

17 July 1995, “Missile Flight Simulation, Part One, Surface-to-Air Missiles”, Ch 4 In: Military Handbook. 1995, MIL-HDBK-1211(MI)

[DI]

Diebel, J., 2006, “Representing Attitude: Euler Angles, Unit Quaternions, and Rotation Vectors”, Stanford University.

Examples#

For examples, see the various functions.

See Also#

eqm3

Translational motion derivatives of a data point in three-dimensional space

eqm6

Translational and rotational motion derivatives of a rigid body in three-dimensional space

int3

A step integration of the equations of translational motion

int6

A step integration of the equations of translational and rotational motion

eqm6q

Translational and rotational motion derivatives of a rigid body with a quaternion attitude

int6q

A step integration of the equations of translational and rotational motion with a quaternion attitude

quadeqm, quadforces

Quadcopter equations of motion (rotor forces and moments through eqm6 / eqm6q), shaped for scipy.integrate.solve_ivp

quatbody

A rigid body state object with a quaternion attitude

euler2quat, quat2euler, quat2dcm

Conversions between quaternions, Euler angles, and DCMs

rotmat

Rotation matrices and rotational operations

Rigid Body Transformations

Background material on rotation matrices and frame conventions
Contents