Orthogonal & Non-Orthogonal Gimbal Kinematics Derivation

Nikolai Nekrutenko, Freefly Systems

Using the axis convention as shown below, we derive the equations from translating motion from the gimbal body frame to that of the camera frame.

Using the following variable convention:

$\left[\begin{array}{c}x\\ y\\ z\end{array}\right]=\left[\begin{array}{c}\text{roll}\\ \text{tilt}\\ \text{pan}\end{array}\right]$

We define the following variables:

$\theta {}_x$ = roll joint angle [rad]

$\theta {}_y$ = tilt joint angle [rad]

$\theta {}_z$ = pan joint angle [rad]

$s{}_x$ = roll joint rate [rad/s]

$s{}_y$ = tilt joint rate [rad/s]

$s{}_z$ = pan join rate [rad/s]

$\omega {}_x$ = roll IMU rate [rad/s]

$\omega {}_y$ = tilt IMU rate [rad/s]

$\omega {}_z$ = pan IMU rate [rad/s]

Orthogonal Gimbal Kinematics Derivation

The jacobian is defined as:

$$J=R_y({\theta }_y)\cdot \left[\begin{array}{c}1\\ 0\\ 0\end{array}\right]+\left[\begin{array}{c}0\\ 1\\ 0\end{array}\right]+R_y({\theta }_y)\cdot R_x({\theta }_x)\cdot \left[\begin{array}{c}0\\ 0\\ 1\end{array}\right]$$

Derivation from pg. 49 of the ETH Zurich lecture notes, found in resources on rotation matricies.

However, the standard rotational matricies do not work with our coordinate axes as the z-axis is flipped in the Freefly convention. As a result the rotation matricies are modified to take this into account:

$$R_x({\theta }_x)=\left[\begin{array}{ccc}1& 0& 0\\ 0& cos({\theta }_x)& -sin({\theta }_x)\\ 0& sin({\theta }_x)& cos({\theta }_x)\end{array}\right]\Rightarrow \left[\begin{array}{ccc}1& 0& 0\\ 0& cos({\theta }_x)& sin({\theta }_x)\\ 0& -sin({\theta }_x)& cos({\theta }_x)\end{array}\right]$$

$$R_y({\theta }_y)=\left[\begin{array}{ccc}cos({\theta }_y)& 0& sin({\theta }_y)\\ 0& 1& 0\\ -sin({\theta }_y)& 0& cos({\theta }_y)\end{array}\right]\Rightarrow \left[\begin{array}{ccc}cos({\theta }_y)& 0& -sin({\theta }_y)\\ 0& 1& 0\\ sin({\theta }_y)& 0& cos({\theta }_y)\end{array}\right]$$

$$R_z({\theta }_z)=\left[\begin{array}{ccc}cos({\theta }_z)& -sin({\theta }_z)& 0\\ sin({\theta }_z)& cos({\theta }_z)& 0\\ 0& 0& 1\end{array}\right]\Rightarrow \left[\begin{array}{ccc}cos({\theta }_z)& sin({\theta }_z)& 0\\ -sin({\theta }_z)& cos({\theta }_z)& 0\\ 0& 0& 1\end{array}\right]$$

We multiply

$R_y({\theta }_y)$ by

$R_x({\theta }_x)$ and get the following:

$$R_y({\theta }_y)\cdot R_x({\theta }_x)=\left[\begin{array}{ccc}cos({\theta }_y)& sin({\theta }_y)sin({\theta }_x)& -sin({\theta }_y)cos({\theta }_x)\\ 0& cos({\theta }_x)& sin({\theta }_x)\\ sin({\theta }_y)& -cos({\theta }_y)sin({\theta }_x)& cos({\theta }_y)cos({\theta }_x)\end{array}\right]$$

Then, we evaluate our expression for the jacobian:

$$J=\left[\begin{array}{ccc}cos({\theta }_y)& 0& -sin({\theta }_y)\\ 0& 1& 0\\ sin({\theta }_y)& 0& cos({\theta }_y)\end{array}\right]\cdot \left[\begin{array}{c}1\\ 0\\ 0\end{array}\right]+\left[\begin{array}{c}0\\ 1\\ 0\end{array}\right]+\left[\begin{array}{ccc}cos({\theta }_y)& sin({\theta }_y)sin({\theta }_x)& -sin({\theta }_y)cos({\theta }_x)\\ 0& cos({\theta }_x)& sin({\theta }_x)\\ sin({\theta }_y)& -cos({\theta }_y)sin({\theta }_x)& cos({\theta }_y)cos({\theta }_x)\end{array}\right]\cdot \left[\begin{array}{c}0\\ 0\\ 1\end{array}\right]$$

$$J=\left[\begin{array}{ccc}cos({\theta }_y)& 0& -sin({\theta }_y)cos({\theta }_x)\\ 0& 1& sin({\theta }_x)\\ sin({\theta }_y)& 0& cos({\theta }_y)cos({\theta }_x)\end{array}\right]$$

The following relationship can be established between the rate from the camera and world frames using the jacobian:

$$\left[\begin{array}{c}{w}_x\\ w_y\\ w_z\end{array}\right]=\left[\begin{array}{ccc}cos({\theta }_y)& 0& -sin({\theta }_y)cos({\theta }_x)\\ 0& 1& sin({\theta }_x)\\ sin({\theta }_y)& 0& cos({\theta }_y)cos({\theta }_x)\end{array}\right]\cdot \left[\begin{array}{c}{s}_x\\ s_y\\ s_z\end{array}\right]$$

Evaluating the matrix expression results in the following system of equations:

$${\omega }_x=s_{x}cos({\theta }_y)-s_{z}sin({\theta }_y)cos({\theta }_x)$$

$${\omega }_y=s_y+s_{z}sin({\theta }_x)$$

$${\omega }_z=s_{x}cos({\theta }_y)+s_{y}cos({\theta }_y)cos({\theta }_x)$$

---

Non-Orthogonal Kinematics Derivation

To account for the non-orthogonality of the gimbal, a rotation matrix is introduced, with the angle from normal defined as

$\varphi$.

This makes the sequence of going from the body frame to the camera frame look like this:

$$\text{1. pan}\to \varphi \to \text{roll}\to \text{tilt}$$

Going from camera frame to body frame is accomplished with the following:

$$\text{1. tilt}\to \text{roll}\to \varphi \to \text{pan}$$

Thus, the non-orthogonal Jacobian would look like the following:

$$J_1=R_y({\theta }_y)\cdot \left[\begin{array}{c}1\\ 0\\ 0\end{array}\right]+\left[\begin{array}{c}0\\ 1\\ 0\end{array}\right]+R_y({\theta }_y)\cdot R_x({\theta }_x)\cdot R_{\varphi }(\varphi )\cdot \left[\begin{array}{c}0\\ 0\\ 1\end{array}\right]$$

Where

$R_{\varphi }(\varphi )$ is the transformation that accounts for the non-orthogonality of the x-axis is dervied from a rotation about the y axis in the -

$\varphi$ direction:

$$R_{\varphi }(\varphi )=R_y(-\varphi )=\left[\begin{array}{ccc}cos(-\varphi )& 0& -sin(-\varphi )\\ 0& 1& 0\\ sin(-\varphi )& 0& cos(-\varphi )\end{array}\right]=\left[\begin{array}{ccc}cos(\varphi )& 0& sin(\varphi )\\ 0& 1& 0\\ -sin(\varphi )& 0& cos(\varphi )\end{array}\right]$$

Now, we move onto computing the Jacobians:

$$R_y({\theta }_y)\cdot R_x({\theta }_x)\cdot R_{\varphi }(\varphi )=\left[\begin{array}{ccc}cos({\theta }_y)& sin({\theta }_y)sin({\theta }_x)& -sin({\theta }_y)cos({\theta }_x)\\ 0& cos({\theta }_x)& sin({\theta }_x)\\ sin({\theta }_y)& -cos({\theta }_y)sin({\theta }_x)& cos({\theta }_y)cos({\theta }_x)\end{array}\right]\cdot \left[\begin{array}{ccc}cos(\varphi )& 0& sin(\varphi )\\ 0& 1& 0\\ -sin(\varphi )& 0& cos(\varphi )\end{array}\right]$$

$$=\left[\begin{array}{ccc}cos({\theta }_y)cos(\varphi )+sin({\theta }_y)cos({\theta }_x)sin(\varphi )& sin({\theta }_y)sin({\theta }_x)& cos({\theta }_y)sin(\varphi )-sin({\theta }_y)cos({\theta }_x)cos(\varphi )\\ -sin({\theta }_y)sin(\varphi )& cos({\theta }_x)& sin({\theta }_x)cos(\varphi )\\ sin({\theta }_y)cos(\varphi )-cos({\theta }_y)cos({\theta }_x)sin(\varphi )& -cos({\theta }_y)sin({\theta }_x)& sin({\theta }_y)sin(\varphi )+cos({\theta }_y)cos({\theta }_x)cos(\varphi )\end{array}\right]$$

Thus,

$J_1$ can be expressed as:

$$J_1=\left[\begin{array}{ccc}cos({\theta }_y)& 0& cos({\theta }_y)sin(\varphi )-sin({\theta }_y)cos({\theta }_x)cos(\varphi )\\ 0& 1& sin({\theta }_x)cos(\varphi )\\ sin({\theta }_y)& 0& sin({\theta }_y)sin(\varphi )+cos({\theta }_y)cos({\theta }_x)cos(\varphi )\end{array}\right]$$

The resulting equations of motion for J1 is as follows:

$$\left[\begin{array}{c}{w}_x\\ w_y\\ w_z\end{array}\right]=J_1\cdot \left[\begin{array}{c}{s}_x\\ s_y\\ s_z\end{array}\right]$$

$$\left[\begin{array}{c}{w}_x\\ w_y\\ w_z\end{array}\right]=\left[\begin{array}{ccc}cos({\theta }_y)& 0& cos({\theta }_y)sin(\varphi )-sin({\theta }_y)cos({\theta }_x)cos(\varphi )\\ 0& 1& sin({\theta }_x)cos(\varphi )\\ sin({\theta }_y)& 0& sin({\theta }_y)sin(\varphi )+cos({\theta }_y)cos({\theta }_x)cos(\varphi )\end{array}\right]\cdot \left[\begin{array}{c}{s}_x\\ s_y\\ s_z\end{array}\right]$$

The resulting equations are:

$$w_x=s_{x}cos({\theta }_y)+s_z[cos({\theta }_y)sin(\varphi )-sin({\theta }_y)cos({\theta }_x)cos(\varphi )]$$

$$w_y=s_y+s_{z}sin({\theta }_x)cos(\varphi )$$

$$w_z=s_{x}sin({\theta }_y)+s_z[sin({\theta }_y)sin(\varphi )+cos({\theta }_y)cos({\theta }_x)cos(\varphi )]$$

---

Resources:

• Medium post about applying rotation matricies: https://dominicplein.medium.com/extrinsic-intrinsic-rotation-do-i-multiply-from-right-or-left-357c38c1abfd
• NASA paper and 3 and 4 axis gimbals:
  https://ntrs.nasa.gov/api/citations/19680020485/downloads/19680020485.pdf
• Robot dynamics lecture notes from ETC Zurich, goes into Jacobians and how to apply rotation matricies in the context of robots:
  https://ethz.ch/content/dam/ethz/special-interest/mavt/robotics-n-intelligent-systems/rsl-dam/documents/RobotDynamics2016/RD2016script.pdf