Linearized Equations of Motion

Linearization of Generalized Multibody Equations of Motion

This section has been adapted from Chapter 3 of Russ Tedrake, 2020.

For use with various linear controllers, the above given manipulator equation can be linearized about a (fixed) point. Before this, some prelimiary notation is given for describing the state of a manipulator system. The state of a given system can be described by its generzlied coordinates and their velocities. Thus, the state is written as ${\bf{x}} = [q,\ \dot{q}]^T$. From this, the derivative of the state can be written as $\dot{\bf x} = [\dot{q},\ \ddot{q}]^T$. The manipulator equation given in [41] can be adjusted for the case of free-floating system (no gravitational force) along with the following assumptions: no friction in the joints, and no contact operations. The equations of motion can then be written as:

\[M(q)\ddot{q} + C(q, \dot{q})\dot{q} = B(q)u \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ [42]\]

From the above equation, $\ddot{q}$ can be found by the rearrangement of terms and the derivative of the state can be now written as:

\[\dot{\bf x} = \begin{bmatrix} \dot{q} \\ M(q)^{-1}[B(q)u - C(q, \dot{q})\dot{q}] \end{bmatrix} \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ [43]\]

For the state derivative, the Taylor expansion about a fixed point $(\bf x^{\ast}, \bf u^{\ast})$ can be carried out as given below:

\[\dot{\bf x} = {\bf f}(\bf x,\bf u) \approx {\bf f}(\bf x^*,\bf u^*) + \left[\frac{\partial{\bf f}}{\partial \bf x}\right]_{\bf x=\bf x^*,\bf u=\bf u^*} (\bf x - \bf x^*) + \left[ \frac{\partial{\bf f}}{\partial \bf u}\right]_{\bf x=\bf x^*,\bf u=\bf u^*} (\bf u - \bf u^*) \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ [44]\]

Since, we are performing the linear approximation about a (stable or unstable) fixed point, ${\bf f}(\bf x^{\ast},\bf u^{\ast})$ is equal to zero and we can rewrite the remaining terms in the standard linear state-space form:

\[\dot{\bf x} = A_{lin} (\bf x - \bf x^*) + B_{lin} (\bf u - \bf u^*) \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ [45]\]

where $A_{lin}$ and $B_{lin}$ are constant matrices. We can furthermore define the following terms:

\[\bar{\bf x} = \bf x - \bf x^*\] \[\bar{\bf u} = \bf u - \bf u^*\]

Using these terms, the linear state-space equation can now be written as:

\[\dot{\bf x} = A_{lin} \bar{\bf x} + B_{lin} \bar{\bf u} \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ [46]\]

The matrices $A_{lin}$ and $B_{lin}$ can be found by differentiating equation [43] with respect to $\bf x$ and $\bf u$ and subtituting the fixed point $(\bf x^{\ast},\bf u^{\ast})$. As $\dot{q}$ is zero at the fixed point, the Coriolis terms drop out. Along with this, the $\frac{\partial M^{-1}}{\partial q}$ term also drops out as $B(q)u - C(q, \dot{q}\dot{q}) = 0$ at the fixed point. This results in the following state-space matrices:

\[A_{lin} = \begin{bmatrix} 0 & I \\ M^{-1} \frac{\partial B}{\partial q} u & 0 \end{bmatrix} \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ [47]\] \[B_{lin} = \begin{bmatrix} 0 \\ M^{-1} B \end{bmatrix} \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ [48]\]

Thus, the linearized equations of motion for a free-floating system about a (stable or unstable) fixed point can be described by equations [46], [47], and [48].

Quaternion Based Linearization of Single Rigid-Body Equations of Motion

The set of unit quaternions can be used to represent rotations in 3-D space. These represent a unit sphere $\mathbf{S}^3$ in $\mathbb{R}^4$. These form a Lie Group of 3 dimensions under the operation of quaternion multiplication. This group is isomorphic to the special unitary group $\mathbf{SU}(2)$.

In Zhou, Z., & Colgren, R., 2005, it is shown that a full state-space linearization of the quaternion representation is uncontrollable. Yang, Y., 2012 demonstrates a method for linearization and control in $\mathbf{SO}(3)$ using a quaternion-based linearization by using only the vector part of the quaternion to derive a controllable linearization.

This section extends the work done in Yang, Y., 2012 to full $\mathbf{SE}(3)$ linearization and control using a quaternion-based linearization. This allows the complete pipeline of trajectory optimization and control of such a system to be in quaternion-based representation thus eliminating potential sources of errors caused by representation changes.

To perform a quaternion-based linearization the mapping between angular velocities and quaternion derivatives is used. Let $\bar{q}$ represent a unit-quaternion which represents rotation $\alpha$ about the unit rotation axis $\hat{e}$. The scalar and vector part of the quaternion can be represented as follows:

\[q_0 = cos(\frac{\alpha}{2}) \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ [49]\] \[q = [q_1, q_2, q_3]^T = \hat{e} sin(\frac{\alpha}{2}) \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ [50]\]

This results in the following unit-quaternion representation for rotation:

\[\bar{q} = (q_0, q^T)^T = [cos(\frac{\alpha}{2}), \hat{e}^T sin(\frac{\alpha}{2})]^T \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ [51]\]

The mapping between angular velocity and quaternion derivatives, i.e. quaternion kinematics, is given by the following relation (from Andrle, M. S & Crassidis, J. L., 2012):

\[\dot{\bar{q}} = \frac{1}{2} \Omega (\omega) \bar{q} \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ [52]\]

This is a differential equation on $\mathbf{SU}(2)$. Here, the matrix operator $\frac{1}{2} \Omega (\omega)$ is given as:

\[\frac{1}{2} \Omega (\omega) = \frac{1}{2} \begin{bmatrix} 0 & -\omega^T \\ \omega & -[\omega]^\times \end{bmatrix} \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ [53]\]

Here, the $[\omega]^\times$ operator is defined as:

\[[\omega]^\times = \begin{bmatrix} 0 & -\omega_3 & \omega_2 \\ \omega_3 & 0 & -\omega_1 \\ -\omega_2 & \omega_1 & 0 \end{bmatrix} \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ [54]\]

The quaternion kinematic equation can be expanded to its full matrix-form as follows:

\[\begin{bmatrix} \dot{q_0} \\ \dot{q_1} \\ \dot{q_2} \\ \dot{q_3} \end{bmatrix} = \frac{1}{2} \begin{bmatrix} 0 & -\omega_1 & -\omega_2 & -\omega_3 \\ \omega_1 & 0 & \omega_3 & -\omega_2 \\ \omega_2 & -\omega_3 & 0 & \omega_1 \\ \omega_3 & \omega_2 & -\omega_1 & 0 \end{bmatrix} \begin{bmatrix} q_0 \\ q_1 \\ q_2 \\ q_3 \end{bmatrix} = \frac{1}{2} \begin{bmatrix} q_0 & -q_1 & -q_2 & -q_3 \\ 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{bmatrix} \begin{bmatrix} 0 \\ \omega_1 \\ \omega_2 \\ \omega_3 \end{bmatrix} \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ [55]\]

For any given quaternion $\bar{q}$, if $\alpha \neq \pm \pi$, the scalar part of the quaternion can be written as:

\[q_0 = \sqrt{1 - q_1^2 - q_2^2 - q_3^2} \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ [56]\]

We can substitute this in equation [55] to get:

\[\begin{bmatrix} \dot{q_1} \\ \dot{q_2} \\ \dot{q_3} \end{bmatrix} = \frac{1}{2} \begin{bmatrix} \sqrt{1 - q_1^2 - q_2^2 - q_3^2} & -q_3 & q_2 \\ q_3 & \sqrt{1 - q_1^2 - q_2^2 - q_3^2} & -q_1 \\ -q_2 & q_1 & \sqrt{1 - q_1^2 - q_2^2 - q_3^2} \end{bmatrix} \begin{bmatrix} \omega_1 \\ \omega_2 \\ \omega_3 \end{bmatrix} \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ [57]\]

This gives a one-to-one mapping between angular velocities and quaternion derivatives which can written as:

\[\dot{q} = \frac{1}{2} Q \omega \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ [58]\]

The singularities of this map can be found by looking at the determinant of the mapping matrix $Q$:

\[det(Q) = \frac{1}{\sqrt{1 - q_1^2 - q_2^2 - q_3^2}} \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ [59]\]

As seen here, the one-to-one map has singularities (also as written before) at $\alpha = \pm \pi$. However, we use the following mapping for a linearized version of the dynamic system, and assume that the linear system will not be used for such large angles away from the point of linearization. The linear system is only used for control in the local neighbourhood of the linearization point where the linearization assumption is valid for the non-linear system. Therefore, the singularities can be safely ignored for the purpose of this derivation.

Parameterising $q_0$ using [56], we can write the full-state of of the single rigid body as following:

\[X = \begin{bmatrix} q \\ r \\ \omega \\ v \end{bmatrix} \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ [60]\]

Here, $r$ is the position vector, and $v$ is the translational velocity of the rigid body. The derivative of the state vector can now be written as:

\[\dot{X} = \begin{bmatrix} \dot{q} \\ v \\ \dot{\omega} \\ \dot{v} \end{bmatrix} \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ [61]\]

The multibody generalized equation of motion in matrix form (see derivation here) can be adapted for a single rigid body as follows:

\[M\ddot{q} + C\dot{q} = u \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ [62]\]

Here, $M$ is the mass-inertia matrix for a single rigid body containing the inertia term and the diagonal matrix of total mass, C $C(q, \dot{q})$ is the coriolis term, and $u$ is the generalized torque-force vector. $\ddot{q}$ is the vector of generalized accelerations and $\dot{q}$ the vector of generalized velocities. $\ddot{q}$, $M$ and $u$ can be written as:

\[\ddot{q} = \begin{bmatrix} \dot{\omega} \\ \dot{v} \end{bmatrix} \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ [63]\] \[M = \begin{bmatrix} I & 0 \\ 0 & mE \end{bmatrix} \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ [64]\] \[u = \begin{bmatrix} \tau \\ F \end{bmatrix} \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ [65]\]

The equation of motion can be evaluated at a fixed point ($\dot{q}=0$) to obtain an expression for the generalized accelerations:

\[\ddot{q} = \begin{bmatrix} \dot{\omega} \\ \dot{v} \end{bmatrix} = M^{-1}u = M^{-1}\begin{bmatrix} \tau \\ F \end{bmatrix} = \begin{bmatrix} I & 0 \\ 0 & mE \end{bmatrix}^{-1} \begin{bmatrix} \tau \\ F \end{bmatrix} = \begin{bmatrix} I^{-1} & 0 \\ 0 & \frac{1}{m}E \end{bmatrix} \begin{bmatrix} \tau \\ F \end{bmatrix} = \begin{bmatrix} I^{-1} \tau \\ \frac{1}{m} F \end{bmatrix} \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ [66]\]

Substituting results from [66] nad [58] in [61], we can re-write the state-space derivate as:

\[\dot{X} = \begin{bmatrix} \frac{1}{2}Q\omega \\ v \\ I^{-1} \tau \\ \frac{1}{m} F \end{bmatrix} \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ [67]\]

We can then perform a linear approximation using the taylor expansion as shown in the previous section.

The fixed point state of the system (at quaternion $\bar{q} = (1, 0, 0, 0)^T$) can be written as:

\[X = 0_{12 \times 1}\]

At a fixed point, ${\bf f}(\bf x^{\ast},\bf u^{\ast}) = 0$, and the partial derivatives with respect to $X$ can be evaluated as follows:

$\frac{\partial{f}_1}{\partial{X_1}} = \frac{\partial{\frac{1}{2}Q\omega}}{\partial{q}} = 0^{3 \times 3}$	$\frac{\partial{f}_1}{\partial{X}_2} = \frac{\partial{\frac{1}{2}Q\omega}}{\partial{r}} = 0^{3 \times 3}$	$\frac{\partial{f}_1}{\partial{X}_3} = \frac{\partial{\frac{1}{2}Q\omega}}{\partial{\omega}} = \frac{1}{2} E^{3\times 3}$	$\frac{\partial{f}_1}{\partial{X}_4} = \frac{\partial{\frac{1}{2}Q\omega}}{\partial{v}} = 0^{3 \times 3}$
$\frac{\partial{f}_2}{\partial{X}_1} = \frac{\partial{v}}{\partial{q}} = 0^{3 \times 3}$	$\frac{\partial{f}_2}{\partial{X}_2} = \frac{\partial{v}}{\partial{r}} = 0^{3 \times 3}$	$\frac{\partial{f}_2}{\partial{X}_3} = \frac{\partial{v}}{\partial{\omega}} = 0^{3 \times 3}$	$\frac{\partial{f}_2}{\partial{X}_4} = \frac{\partial{v}}{\partial{v}} = E^{3 \times 3}$
$\frac{\partial{f}_3}{\partial{X}_1} = \frac{\partial{I^{-1} \tau}}{\partial{q}} = 0^{3 \times 3}$	$\frac{\partial{f}_3}{\partial{X}_2} = \frac{\partial{I^{-1} \tau}}{\partial{r}} = 0^{3 \times 3}$	$\frac{\partial{f}_3}{\partial{X}_3} = \frac{\partial{I^{-1} \tau}}{\partial{\omega}} = 0^{3 \times 3}$	$\frac{\partial{f}_3}{\partial{X}_4} = \frac{\partial{I^{-1} \tau}}{\partial{v}} = 0^{3 \times 3}$
$\frac{\partial{f}_4}{\partial{X}_1} = \frac{\partial{\frac{1}{m} F}}{\partial{q}} = 0^{3 \times 3}$	$\frac{\partial{f}_4}{\partial{X}_2} = \frac{\partial{\frac{1}{m} F}}{\partial{r}} = 0^{3 \times 3}$	$\frac{\partial{f}_4}{\partial{X}_3} = \frac{\partial{\frac{1}{m} F}}{\partial{\omega}} = 0^{3 \times 3}$	$\frac{\partial{f}_4}{\partial{X}_4} = \frac{\partial{\frac{1}{m} F}}{\partial{v}} = 0^{3 \times 3}$

Using the above equations, the matrix $A_{lin}$ can be written as:

\[A_{lin} = \begin{bmatrix} \frac{\partial{f}_1}{\partial{X}_1} & \frac{\partial{f}_1}{\partial{X}_2} & \frac{\partial{f}_1}{\partial{X}_3} & \frac{\partial{f}_1}{\partial{X}_4} \\ \frac{\partial{f}_2}{\partial{X}_1} & \frac{\partial{f}_2}{\partial{X}_2} & \frac{\partial{f}_2}{\partial{X}_3} & \frac{\partial{f}_2}{\partial{X}_4} \\ \frac{\partial{f}_3}{\partial{X}_1} & \frac{\partial{f}_3}{\partial{X}_2} & \frac{\partial{f}_3}{\partial{X}_3} & \frac{\partial{f}_3}{\partial{X}_4} \\ \frac{\partial{f}_4}{\partial{X}_1} & \frac{\partial{f}_4}{\partial{X}_2} & \frac{\partial{f}_4}{\partial{X}_3} & \frac{\partial{f}_4}{\partial{X}_4} \end{bmatrix} = \begin{bmatrix} 0_{3\times3} & 0_{3\times3} & \frac{1}{2}E_{3\times3} & 0_{3\times3} \\ 0_{3\times3} & 0_{3\times3} & 0_{3\times3} & E_{3\times3} \\ 0_{3\times3} & 0_{3\times3} & 0_{3\times3} & 0_{3\times3} \\ 0_{3\times3} & 0_{3\times3} & 0_{3\times3} & 0_{3\times3} \end{bmatrix} \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ [68]\]

Similarly, the partial derivatives with respect to $u$ can be written as:

$\frac{\partial{f}_1}{\partial{u}_1} = \frac{\partial{\frac{1}{2}Q\omega}}{\partial{\tau}} = 0^{3\times3}$	$\frac{\partial{f}_1}{\partial{u}_2} = \frac{\partial{\frac{1}{2}Q\omega}}{\partial{F}} = 0^{3\times3}$
$\frac{\partial{f}_2}{\partial{u}_1} = \frac{\partial{v}}{\partial{\tau}} = 0^{3\times3}$	$\frac{\partial{f}_2}{\partial{u}_2} = \frac{\partial{v}}{\partial{F}} = 0^{3\times3}$
$\frac{\partial{f}_3}{\partial{u}_1} = \frac{\partial{I^{-1} \tau}}{\partial{\tau}} = I^{-1^{3\times3}}$	$\frac{\partial{f}_3}{\partial{u}_2} = \frac{\partial{I^{-1} \tau}}{\partial{F}} = 0^{3\times3}$
$\frac{\partial{f}_4}{\partial{u}_1} = \frac{\partial{\frac{1}{m} F}}{\partial{\tau}} = 0^{3\times3}$	$\frac{\partial{f}_4}{\partial{u}_2} = \frac{\partial{\frac{1}{m} F}}{\partial{F}} = \frac{1}{m} E^{3\times3}$

Using the above equations, the matrix $B_{lin}$ can be written as:

\[B_{lin} = \begin{bmatrix} \frac{\partial{f_1}}{\partial{u_1}} & \frac{\partial{f_1}}{\partial{u_2}} \\ \frac{\partial{f_2}}{\partial{u_1}} & \frac{\partial{f_2}}{\partial{u_2}} \\ \frac{\partial{f_3}}{\partial{u_1}} & \frac{\partial{f_3}}{\partial{u_2}} \\ \frac{\partial{f_4}}{\partial{u_1}} & \frac{\partial{f_4}}{\partial{u_2}} \\ \end{bmatrix} = \begin{bmatrix} 0_{3\times3} & 0_{3\times3} \\ 0_{3\times3} & 0_{3\times3} \\ I^{-1}_{3\times3} & 0_{3\times3} \\ 0_{3\times3} & \frac{1}{m} E_{3\times3} \end{bmatrix} \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ [69]\]

The linearized system can now be written as following:

\[\dot{X} = A_{lin} X + B_{lin} u\] \[\begin{bmatrix} \dot{q} \\ v \\ \dot{\omega} \\ \dot{v} \end{bmatrix} = \begin{bmatrix} 0_{3\times3} & 0_{3\times3} & \frac{1}{2}E_{3\times3} & 0_{3\times3} \\ 0_{3\times3} & 0_{3\times3} & 0_{3\times3} & E_{3\times3} \\ 0_{3\times3} & 0_{3\times3} & 0_{3\times3} & 0_{3\times3} \\ 0_{3\times3} & 0_{3\times3} & 0_{3\times3} & 0_{3\times3} \end{bmatrix} \begin{bmatrix} q \\ r \\ \omega \\ v \end{bmatrix} + \begin{bmatrix} 0_{3\times3} & 0_{3\times3} \\ 0_{3\times3} & 0_{3\times3} \\ I^{-1}_{3\times3} & 0_{3\times3} \\ 0_{3\times3} & \frac{1}{m} E_{3\times3} \end{bmatrix} \begin{bmatrix} \tau \\ F \end{bmatrix} \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ [70]\]

Quaternion Based Linearization of Multi-Body System with a Free-Floating Base

The equations of motion for a multi-body system with a free-floating base can be written as (see the full derivation here):

\[M(q)\ddot{q} + C(q, \dot{q})\dot{q} = u \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ [71]\]

The state of such a system can be written as follows:

\[X = [q_b, r_b, q_m, \omega_b, v_b, \dot{q}_m]^T \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ [72]\]

here, $q_b$ is the vector part of the rotation quaternion of the base, $r_b$ is the position vector of the base, $q_m$ the position vector of the joint angles for the multi-body system, $\omega_b$ the base angular velocity, $v_b$ the base linear velocity, and $\dot{q}_m$ the angular velocity of the joints.

The derivative of the state vector is given as follows:

\[\dot{X} = [\dot{q}_b, v_b, \dot{q}_m, \dot{\omega}_b, \dot{v}_b, \ddot{q}_m]^T \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ [73]\]

The linerization is carried out around a fixed point i.e $\dot{q}=0$. The equations of motion can then be written as:

\[M(q)\ddot{q}= u \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ [74]\] \[\ddot{q}= M(q)^{-1}u = M^{-1} u = \begin{bmatrix} \dot{\omega}_b \\ \dot{v}_b \\ \ddot{q}_m \end{bmatrix} \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ [75]\]

Substituting equations [58] and [75] in [73], we can now write the state derivative as:

\[\dot{X} = [\frac{1}{2} Q \omega_b, v_b, \dot{q}_m, M^{-1}u]^T \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ [76]\]

The generalized forces in the multibody system with a free-floating base can be written as:

\[u = [\tau_b, F_b, \tau_m]^T\]

Here, $\tau_b$ are the torques applied at the base, $F_b$ are the forces applied at the base, and $\tau_m$ are the manipulator joint torques.

We can then perform a linear approximation using the taylor expansion as shown in the previous section.

The fixed point sytate of the system (at quaternion $\bar{q}=(1, 0, 0, 0)^T$) and joint positions $q_m$ can be written as:

\[X = [0_{6 \times 1}, q_m, 0_{(6+n) \times 1}]^T_{(12+2n) \times 1}\]

At a fixed point, ${\bf f}(\bf x^{\ast},\bf u^{\ast}) = 0$, and the partial derivatives with respect to $X$ can be evaluated as follows:

$\frac{\partial{f}_1}{\partial{X}_1}=\frac{\partial{\frac{1}{2} Q \omega_b}}{\partial{q_b}}= 0^{3 \times 3}$	$\frac{\partial{f}_1}{\partial{X}_2}=\frac{\partial{\frac{1}{2} Q \omega_b}}{\partial{r_b}}= 0^{3 \times 3}$	$\frac{\partial{f}_1}{\partial{X}_3}=\frac{\partial{\frac{1}{2} Q \omega_b}}{\partial{q_m}}= 0^{3 \times n}$
$\frac{\partial{f}_1}{\partial{X}_4}=\frac{\partial{\frac{1}{2} Q \omega_b}}{\partial{\omega_b}}= \frac{1}{2}E^{3 \times 3}$	$\frac{\partial{f}_1}{\partial{X}_5}=\frac{\partial{\frac{1}{2} Q \omega_b}}{\partial{v_b}}= 0^{3 \times 3}$	$\frac{\partial{f}_1}{\partial{X}_6}=\frac{\partial{\frac{1}{2} Q \omega_b}}{\partial{\dot{q}_m}}= 0^{3 \times n}$

$\frac{\partial{f}_2}{\partial{X}_1}=\frac{\partial{v_b}}{\partial{q_b}}=0^{3 \times 3}$	$\frac{\partial{f}_2}{\partial{X}_2}=\frac{\partial{v_b}}{\partial{r_b}}=0^{3 \times 3}$	$\frac{\partial{f}_2}{\partial{X}_3}=\frac{\partial{q_m}}{\partial{q_b}}=0^{3 \times n}$
$\frac{\partial{f}_2}{\partial{X}_4}=\frac{\partial{v_b}}{\partial{\omega_b}}=0^{3 \times 3}$	$\frac{\partial{f}_2}{\partial{X}_5}=\frac{\partial{v_b}}{\partial{v_b}}=E^{3 \times 3}$	$\frac{\partial{f}_2}{\partial{X}_6}=\frac{\partial{v_b}}{\partial{\dot{q}_b}}=0^{3 \times n}$

$\frac{\partial{f}_3}{\partial{X}_1}=\frac{\partial{\dot{q}_m}}{\partial{q_b}}=0^{n \times 3}$	$\frac{\partial{f}_3}{\partial{X}_2}=\frac{\partial{\dot{q}_m}}{\partial{r_b}}=0^{n \times 3}$	$\frac{\partial{f}_3}{\partial{X}_3}=\frac{\partial{\dot{q}_m}}{\partial{q_m}}=0^{n \times n}$
$\frac{\partial{f}_3}{\partial{X}_4}=\frac{\partial{\dot{q}_m}}{\partial{\omega_b}}=0^{n \times 3}$	$\frac{\partial{f}_3}{\partial{X}_5}=\frac{\partial{\dot{q}_m}}{\partial{v_b}}=0^{n \times 3}$	$\frac{\partial{f}_3}{\partial{X}_6}=\frac{\partial{\dot{q}_m}}{\partial{\dot{q}_m}}=E^{n \times n}$

$\frac{\partial{f}_4}{\partial{X}_1}=\frac{\partial{M^{-1}u}}{\partial{q_b}}=0^{(6+n) \times 3}$	$\frac{\partial{f}_4}{\partial{X}_2}=\frac{\partial{M^{-1}u}}{\partial{r_b}}=0^{(6+n) \times 3}$	$\frac{\partial{f}_4}{\partial{X}_3}=\frac{\partial{M^{-1}u}}{\partial{q_m}}=0^{(6+n) \times n}$
$\frac{\partial{f}_4}{\partial{X}_4}=\frac{\partial{M^{-1}u}}{\partial{\omega_b}}=0^{(6+n) \times 3}$	$\frac{\partial{f}_4}{\partial{X}_4}=\frac{\partial{M^{-1}u}}{\partial{v_b}}=0^{(6+n) \times 3}$	$\frac{\partial{f}_4}{\partial{X}_6}=\frac{\partial{M^{-1}u}}{\partial{\dot{q}_m}}=0^{(6+n) \times n}$

Using the above equations, the matrix $A_{lin}$ can be written as:

\[A_{lin} = \begin{bmatrix} 0_{3 \times 3} & 0_{3 \times 3} & 0_{3 \times n} & \frac{1}{2} E_{3 \times 3} & 0_{3 \times 3} & 0_{3 \times n} \\ 0_{3 \times 3} & 0_{3 \times 3} & 0_{3 \times n} & 0_{3 \times 3} & E_{3 \times 3} & 0_{3 \times n} \\ 0_{n \times 3} & 0_{n \times 3} & 0_{n \times n} & 0_{n \times 3} & 0_{n \times 3} & E_{n \times n} \\ 0_{(6+n) \times 3} & 0_{(6+n) \times 3} & 0_{(6+n) \times n} & 0_{(6+n) \times 3} & 0_{(6+n) \times 3} & 0_{(6+n) \times n} \end{bmatrix}\]

Similarly, the partial derivatives with respect to $u$ can be written as:

\[\frac{\partial{f_1}}{\partial{u}}=\begin{bmatrix} \frac{\partial{f_1}}{\partial{u_1}} & \frac{\partial{f_1}}{\partial{u_2}} & \frac{\partial{f_1}}{\partial{u_3}} \end{bmatrix}= \begin{bmatrix} \frac{\partial{f_1}}{\partial{\tau_b}} & \frac{\partial{f_1}}{\partial{F_b}} & \frac{\partial{f_1}}{\partial{\tau_m}} \end{bmatrix}\]

$\frac{\partial{f_1}}{\partial{u_1}}=\frac{\partial{\frac{1}{2}Q\omega_b}}{\partial{\tau_b}}=0^{3 \times 3}$	$\frac{\partial{f_1}}{\partial{u_2}}=\frac{\partial{\frac{1}{2}Q\omega_b}}{\partial{F_b}}=0^{3 \times 3}$	$\frac{\partial{f_1}}{\partial{u_3}}=\frac{\partial{\frac{1}{2}Q\omega_b}}{\partial{\tau_m}}=0^{3 \times n}$
$\frac{\partial{f_2}}{\partial{u_1}}=\frac{\partial{v_b}}{\partial{\tau_b}}=0^{3 \times 3}$	$\frac{\partial{f_2}}{\partial{u_2}}=\frac{\partial{v_b}}{\partial{F_b}}=0^{3 \times 3}$	$\frac{\partial{f_2}}{\partial{u_3}}=\frac{\partial{v_b}}{\partial{\tau_m}}=0^{3 \times n}$
$\frac{\partial{f_3}}{\partial{u_1}}=\frac{\partial{\dot{q}_m}}{\tau_b}=0^{n \times 3}$	$\frac{\partial{f_3}}{\partial{u_2}}=\frac{\partial{\dot{q}_m}}{F_b}=0^{n \times 3}$	$\frac{\partial{f_3}}{\partial{u_3}}=\frac{\partial{\dot{q}_m}}{\tau_m}=0^{n \times n}$

\[\frac{\partial{f_4}}{\partial{u}}=\frac{\partial{M^{-1}u}}{\partial{u}}=M^{-1^{(6+n) \times (6+n)}}\]

Using the above equations, the matrix $B_{lin}$ can be written as:

\[B_{lin}=\begin{bmatrix} 0_{3 \times 3} & 0_{3 \times 3} & 0_{3 \times n} \\ 0_{3 \times 3} & 0_{3 \times 3} & 0_{3 \times n} \\ 0_{n \times 3} & 0_{n \times 3} & 0_{n \times n} \\ & M^{-1}_{(6+n) \times (6+n)} & \end{bmatrix}\]

The linearized system can now be written as following:

\[\dot{X} = A_{lin} X + B_{lin} u\] \[\begin{bmatrix} \dot{q}_b \\ v_b \\ \dot{q}_m \\ \dot{\omega}_b \\ \dot{v}_b \\ \ddot{q}_m \end{bmatrix}_{(12+2n) \times 1}= \begin{bmatrix} 0_{3 \times 3} & 0_{3 \times 3} & 0_{3 \times n} & \frac{1}{2} E_{3 \times 3} & 0_{3 \times 3} & 0_{3 \times n} \\ 0_{3 \times 3} & 0_{3 \times 3} & 0_{3 \times n} & 0_{3 \times 3} & E_{3 \times 3} & 0_{3 \times n} \\ 0_{n \times 3} & 0_{n \times 3} & 0_{n \times n} & 0_{n \times 3} & 0_{n \times 3} & E_{n \times n} \\ 0_{(6+n) \times 3} & 0_{(6+n) \times 3} & 0_{(6+n) \times n} & 0_{(6+n) \times 3} & 0_{(6+n) \times 3} & 0_{(6+n) \times n} \end{bmatrix}_{(12+2n) \times (12+2n)} \begin{bmatrix} q_b \\ r_b \\ q_m \\ \omega_b \\ v_b \\ \dot{q}_m \end{bmatrix}_{(12+2n) \times 1} + \\ \begin{bmatrix} 0_{3 \times 3} & 0_{3 \times 3} & 0_{3 \times n} \\ 0_{3 \times 3} & 0_{3 \times 3} & 0_{3 \times n} \\ 0_{n \times 3} & 0_{n \times 3} & 0_{n \times n} \\ & M^{-1}_{(6+n) \times (6+n)} & \end{bmatrix}_{(12+2n) \times (6+n)} \begin{bmatrix} \tau_b \\ F_b \\ \tau_m \end{bmatrix}_{(6+n) \times 1}\]

References

[Russ Tedrake, 2020] Russ Tedrake. Underactuated Robotics: Algorithms for Walking, Running, Swimming, Flying, and Manipulation (Course Notes for MIT 6.832). Downloaded on 4 November 2020 from http://underactuated.mit.edu/

[Yang, Y., 2012] Yang, Y. (2012). Analytic LQR Design for Spacecraft Control System Based on Quaternion Model. Journal of Aerospace Engineering, 25(3), 448–453. https://doi.org/10.1061/(asce)as.1943-5525.0000142

[Andrle, M. S & Crassidis, J. L., 2012] Andrle, M. S., & Crassidis, J. L. (2012). Geometric integration of quaternions. AIAA/AAS Astrodynamics Specialist Conference 2012, 1–10. https://doi.org/10.2514/6.2012-4421

[Zhou, Z., & Colgren, R., 2005]Zhou, Z., & Colgren, R. (2005). A non-linear spacecraft attitude tracking controller for large non-constant rate commands. International Journal of Control, 78(5), 311–325. https://doi.org/10.1080/00207170500079779