Camera Gimbals: A Robotics Approach

29. Sep. 2018 | Letzte Änderung 29. Nov. 2023 | Thema Brushless Gimbal, Grundlagen
Tag , , ,

When I started with the STorM32 gimbal controller project 5 years ago, I had some experience with controllers from my GA250 gyro firmware and GA250 FP&Coax gyro-mixer projects, but had zero idea about the control of a 3-axis direct-drive electro-mechanical system. I could fill some gaps in knowledge (here, here, here), but the controller design remained largely phenomenological. Two years ago I stumbled across the topics robotics and manipulators, realizing that this is what a gimbal is. So, I read a book on it and learned its techniques, and put it on my list of plans to rework the STorM32 gimbal controller along the robotics lines … but, I never did. Only recently I came back to this topic again, and found time to work out some results using the robotics mathematical tools … and realized that they essentially agree with what I had already implemented. 🙂

1. Basics
  1.1. Coordinate Frames, 1.2. Rotation Rate, 1.3. Fundamental Gimbal Laws
2. YRP Representation
3. Denavit Hartenberg
  3.1. Basics, 3.2. Yaw-Roll-Pitch Gimbal, 3.3. Yaw-Roll-Pitch Gimbal with Tilted Roll
4. Dynamics
  4.1. Basics, 4.2. Robotics, 4.3. Manipulator with Actuator, 4.4. Cartesian Space
5. Gimbal Models
  5.1. Gimbal Model I, 5.2. Gimbal Model II, 5.3. Gimbal Model III
References


1. Basics

Summary of some fundamental relations relevant for gimbals of all kinds. \boldsymbol{\theta}}_1 are the parameters describing the orientation of the camera with respect to the fixed frame and \boldsymbol{\theta}}_2 those describing the orientation of the gimbal base. The parameters \boldsymbol{\epsilon} describe the configuration of the gimbal, i.e., the orientation of the camera with respect to the gimbal base; they typically would be the motor angles.

1.1. Coordinate Frames

Earth fixed: K0, indexed with superscript 0

Camera fixed: K1, indexed with superscript 1

Gimbal base: K2, indexed with superscript 2

 
Coordinate Transformations: 

p^0 = R^0_1 p^1,    {\bf{R}}_1 \equiv R^0_1

p^0 = R^0_2 p^2,    {\bf{R}}_2 \equiv R^0_2

p^2 = R^2_1 p^1,    {\bf{R}}_\epsilon \equiv R^2_1

 
1.2. Rotation Rate

{\bf{R}}(\boldsymbol{\theta}})   \Rightarrow   \dot{{\bf{R}}}(\boldsymbol{\theta}}) = {\bf{S}}(\boldsymbol{\omega}}) {{\bf{R}}(\boldsymbol{\theta}})   with   \boldsymbol{\omega}} = {\bf{J}}_\omega(\boldsymbol{\theta}}) \dot{\boldsymbol{\theta}}}

{\bf{S}}(\boldsymbol{\omega}}) {{\bf{R}}(\boldsymbol{\theta}}) = {{\bf{R}}(\boldsymbol{\theta}}) {\bf{S}}(\boldsymbol{\omega}}')   with   \boldsymbol{\omega}}' = {{\bf{R}}(\boldsymbol{\theta}})^T \boldsymbol{\omega}} = {{\bf{R}}(\boldsymbol{\theta}})^T {\bf{J}}_\omega(\boldsymbol{\theta}}) \dot{\boldsymbol{\theta}}} = {\bf{X}}(\boldsymbol{\theta}}) \dot{\boldsymbol{\theta}}}

more precisely:

p^{K} = R^{K}_{K'}(\theta) p^{K'}   \Rightarrow   \dot{R}^{K}_{K'} = S(\omega^{K}_{K'}) R^{K}_{K'}   with   \omega^{K}_{K'} = J_\omega(\theta) \dot{\theta}

\omega^{K}_{K'} = R^{K}_{K'}(\theta) \omega^{K'}_{K'}   \Rightarrow   \omega^{K'}_{K'} = \left( R^{K}_{K'} \right)^T J_\omega(\theta) \dot{\theta}

\omega^{K}_{K'}: rotation rate of frame K', expressed in frame K

\omega^{K'}_{K'}: rotation rate of frame K', expressed in frame K'

 
1.3. Fundamental Gimbal Laws

{\bf{R}}_1 = {\bf{R}}_2 {\bf{R}}_\epsilon

If the same representation is used for the three orientations, it holds:   {\bf{R}}(\boldsymbol{\theta}}_1) = {\bf{R}}(\boldsymbol{\theta}}_2) {\bf{R}}(\boldsymbol{\epsilon}})

 
\boldsymbol{\omega}}_1 = \boldsymbol{\omega}}_2 + \boldsymbol{\omega}}_\epsilon

Rotation rate in frame K1 due to changes in angles {\boldsymbol{\theta}}_1:

  \omega^1_1 = \left( R^0_1 \right)^T J_w \dot{\theta}_1 = {{\bf{R}}_1^T {\bf{J}}_\omega(\boldsymbol{\theta}}_1) \dot{\boldsymbol{\theta}}_1 = {\bf{X}}(\boldsymbol{\theta}}_1) \dot{\boldsymbol{\theta}}_1

Rotation rate in frame K2 due to changes in angles {\boldsymbol{\theta}}_2:

  \omega^2_2 = \left( R^0_2 \right)^T J_w \dot{\theta}_2 = {{\bf{R}}_2^T {\bf{J}}_\omega(\boldsymbol{\theta}}_2) \dot{\boldsymbol{\theta}}_2 = {\bf{X}}(\boldsymbol{\theta}}_2) \dot{\boldsymbol{\theta}}_2

Rotation rate in frame K2 due to changes in angles {\boldsymbol{\epsilon}}:

  \omega^2_\epsilon = {\bf{J}}_\omega(\boldsymbol{\epsilon}}) \dot{\boldsymbol{\epsilon}}

Rotation rate in frame K1 due to changes in angles {\boldsymbol{\epsilon}}:

  \omega^1_\epsilon = \left( R^2_1 \right)^T \omega^2_\epsilon = {{\bf{R}}_\epsilon^T {\bf{J}}_\omega(\boldsymbol{\epsilon}}) \dot{\boldsymbol{\epsilon}} = {\bf{X}}(\boldsymbol{\epsilon}}) \dot{\boldsymbol{\epsilon}}

 
Comment: Strapdown gyros in frame K1 and K2 measure \omega^1_1 and \omega^2_2, respectively.


2. YRP Representation

The yaw-roll-pitch (YRP) representation of the angles is well suited for gimbals in Pitch-Roll-Yaw configuration.

\boldsymbol{\theta}} = \left(\begin{array}{c} \theta_y & \theta_r & \theta_p \end{array}\right)

 
Rotation Matrix Yaw-Roll-Pitch: 

{\bf{R}}(\boldsymbol{\theta}}) \equiv {\bf{R}}_z(\theta_y) {\bf{R}}_x(\theta_r) {\bf{R}}_y(\theta_p) = \left(\begin{array}{ccc} c_p c_y - s_p s_r s_y & - c_r s_y & s_p c_y + c_p s_r s_y \\ c_p s_y + s_p s_r c_y & c_r c_y & s_p s_y - c_p s_r c_y \\ -s_p c_r & s_r & c_p c_r \end{array}\right)

 
Jacobian Yaw-Roll-Pitch: 

{\bf{J}}_\omega(\boldsymbol{\theta}}) = \left(\begin{array}{ccc} 0 & c_y & - s_y c_r \\ 0 & s_y & c_y c_r \\ 1 & 0 & s_r \end{array}\right),    {\bf{J}}_\omega^{-1} = \dfrac{1}{c_r} \left(\begin{array}{ccc} s_y s_r & -c_y s_r & c_r \\ c_y c_r & s_y c_r & 0 \\ - s_y & c_y & 0 \end{array}\right)

{\bf{J}}^T_\omega {\bf{J}}_\omega = \left(\begin{array}{ccc} 1 & 0 & s_r \\ 0 & 1 & 0 \\ s_r & 0 & 1 \end{array}\right)

{\bf{X}} (\boldsymbol{\theta}}) \equiv {\bf{R}}^T {\bf{J}}_\omega = \left(\begin{array}{ccc} - c_r s_p & c_p & 0 \\ s_r & 0 & 1 \\ c_r c_p & s_p & 0 \end{array}\right) = {\bf{R}}^T_y(\theta_p) {\bf{Y}}(\theta_r)

{\bf{X}}^{-1} (\boldsymbol{\theta}}) \equiv {\bf{J}}_\omega^{-1} {\bf{R}} = \dfrac{1}{c_r} \left(\begin{array}{ccc} -s_p & 0 & c_p \\ c_r c_p & 0 & c_r s_p \\ s_r s_p & c_r & -s_r c_p \end{array}\right) = {\bf{Y}}^{-1}(\theta_r) {\bf{R}}_y(\theta_p)

 
Usefull: 

{\bf{Y}}(\theta_r)= \left(\begin{array}{ccc} 0 & 1 & 0 \\ s_r & 0 & 1 \\ c_r & 0 & 0 \end{array}\right)

{\bf{Y}}^{-1}(\theta_r)= \dfrac{1}{c_r} \left(\begin{array}{ccc} 0 & 0 & 1 \\ c_r & 0 & 0 \\ 0 & c_r & -s_r \end{array}\right)

 
{\bf{R}}_y(\theta) \left(\begin{array}{ccc} A & 0 & 0 \\ 0 & B & 0 \\ 0 & 0 & C \end{array}\right) {\bf{R}}^T_y(\theta) = \left(\begin{array}{ccc} c^2 A + s^2 C & 0 & s c (A-C) \\ 0 & B & 0 \\ s c (A - C) & 0 & s^2 A + c^2 C \end{array}\right)

               = \left(\begin{array}{ccc} A - s^2 (A-C) & 0 & s c (A-C) \\ 0 & B & 0 \\ s c (A-C) & 0 & C + s^2 (A-C) \end{array}\right)


3. Denavit Hartenberg

3.1. Basics

p^0 = R^0_n p^n + o^0_n

T^0_n = A_1 A_2\cdots A_n = \left[\begin{array}{cc} R^0_n & o^0_n \\ 0 & 1 \end{array}\right]

w^0_n = J_\omega \dot{q}

J_\omega = \left[ z^0_0, z^0_1, z^0_2 \right] = \left[ e_z, A_1 e_z, A_1 A_2 e_z \right]   (only revolute joints)

v^0_n = J_v \dot{q}

J_v = \left[ z^0_0 \times (o^0_n-o^0_0), z^0_1 \times (o^0_n-o^0_1), z^0_2 \times (o^0_n-o^0_2) \right]   (only revolute joints)

 
DH Conditions:

DH1: axis x_i is perpendicular to axis z_{i-1}

DH2: axis x_i intersects axis z_{i-1}

(): axis z_i is along rotation or slide axis

A_i(q_i) = \left(\begin{array}{cccc} c_{\theta_i} & -s_{\theta_i} c_{\alpha_i} & s_{\theta_i} s_{\alpha_i} & a_i c_{\alpha_i} \\ s_{\theta_i} & c_{\theta_i} c_{\alpha_i} & -c_{\theta_i} s_{\alpha_i} & a_i s_{\alpha_i} \\ 0 & s_{\alpha_i} & c_{\alpha_i} & d_i \\ 0 & 0 & 0 & 1 \end{array}\right)

\alpha_i: angle to rotate z_{i-1} to z_i around x_i axis
\theta_i: angle to rotate x_{i-1} to x_i around z_{i-1} axis
a_i: displacement along axis x_i
d_i: displacement along axis z_{i-1}

 
3.2. Yaw-Roll-Pitch Gimbal

DH Matrices:

\alpha_1 = 90^\circ,   \theta_1 = 90^\circ + \epsilon_y,   a_1 = 0,   d_1 < 0:   A_1(\epsilon_y) = \left(\begin{array}{cccc} -s_y & 0 & c_y & 0 \\ c_y & 0 & s_y & 0 \\ 0 & 1 & 0 & d_1 \\ 0 & 0 & 0 & 1 \end{array}\right)

\alpha_2 = 90^\circ,   \theta_2 = 90^\circ + \epsilon_r,   a_2 = 0,   d_2 > 0:   A_2(\epsilon_r) = \left(\begin{array}{cccc} -s_r & 0 & c_r & 0 \\ c_r & 0 & s_r & 0 \\ 0 & 1 & 0 & d_2 \\ 0 & 0 & 0 & 1 \end{array}\right)

\alpha_3 = 90^\circ,   \theta_3 = 90^\circ + \epsilon_p,   a_3 = 0,   d_3 < 0:   A_3(\epsilon_p) = \left(\begin{array}{cccc} -s_p & 0 & c_p & 0 \\ c_p & 0 & s_p & 0 \\ 0 & 1 & 0 & d_3 \\ 0 & 0 & 0 & 1 \end{array}\right)

Comment: With respect to rotation, the matrices correspond to R_z(\epsilon) R_z(90^\circ) R_x(90^\circ).

Comment: The offset d_1 can be set to zero without restriction of generality, and for a balanced gimbal d_3 should be zero. Thus, only d_2 should be of relevance.

 
Manipulator Matrices:

T^0_1 = A_1 = \left(\begin{array}{cccc} -s_y & 0 & c_y & 0 \\ c_y & 0 & s_y & 0 \\ 0 & 1 & 0 & d_1 \\ 0 & 0 & 0 & 1 \end{array}\right)

T^0_2 = A_1 A_2 = \left(\begin{array}{cccc} s_y s_r & c_y & -s_y c_r & c_y d_2 \\ -c_y s_r & s_y & c_y c_r & s_y d_2 \\ c_r & 0 & s_r & d_1 \\ 0 & 0 & 0 & 1 \end{array}\right)

T^0_3 = A_1 A_2 A_3 = \left(\begin{array}{cccc} c_y c_p - s_y s_r s_p & -s_y c_r & c_y s_p + s_y s_r c_p & c_y d_2 - s_y c_r d_3 \\ s_y c_p + c_y s_r s_p & c_y c_r & s_y s_p - c_y s_r c_p & s_y d_2 + c_y c_r d_3 \\ -c_r s_p & s_r & c_r c_p & d_1 + s_r d_3 \\ 0 & 0 & 0 & 1 \end{array}\right) \equiv {\bf{R}}(\boldsymbol{\epsilon}})

 
Manipulator Jacobian:

J_\omega = \left(\begin{array}{ccc} 0 & c_y & - s_y c_r \\ 0 & s_y & c_y c_r \\ 1 & 0 & s_r \end{array}\right) \equiv {\bf{J}}_\omega(\boldsymbol{\epsilon}})

J_v = \left[ z^0_0 \times o^0_n, z^0_1 \times o^0_n, z^0_2 \times o^0_n \right] - \left(\begin{array}{ccc} 0 & s_y d_1 & c_y c_r d_1 - s_y s_r d_2 \\ 0 & -c_y d_1 & s_y c_r d_1 + c_y s_r d_2 \\ 0 & 0 & -c_r d_2 \end{array}\right)

 
3.3. Yaw-Roll-Pitch Gimbal with Tilted Roll

\alpha_1 = 90^\circ + \bar\alpha,   \theta_1 = 90^\circ + \epsilon_y,   a_1 = 0,   d_1 < 0

\alpha_2 = 90^\circ,   \theta_2 = 90^\circ + \epsilon_r,   a_2 = 0,   d_2 > 0

\alpha_3 = 90^\circ,   \theta_3 = 90^\circ + \epsilon_p - \bar\alpha,   a_3 = 0,   d_3 < 0

A'_1(\epsilon_y) = A_1(\epsilon_y) R_x(\bar\alpha) = \left(\begin{array}{cccc} -s_y & c_y s_\bar\alpha & c_y c_\bar\alpha & 0 \\ c_y & s_y s_\bar\alpha & s_y c_\bar\alpha & 0 \\ 0 & c_\bar\alpha & -s_\bar\alpha & d_1 \\ 0 & 0 & 0 & 1 \end{array}\right),   A'_2(\epsilon_r) = A_2(\epsilon_r),   A'_3(\epsilon_p) = A_3(\epsilon_p - \bar\alpha)


4. Dynamics

4.1. Basics

\mathcal{L} = K - P

\dfrac{d}{dt} \dfrac{\partial }{\partial \dot{q}_k} \mathcal{L} - \dfrac{\partial }{\partial q_k} \mathcal{L} = Q_k

p_k = \dfrac{\partial }{\partial \dot{q}_k} \mathcal{L}

 
4.2. Robotics

Lagrange: 

\mathcal{L} = \dfrac{1}{2} \dot{{\bf{q}}}^T {\bf{M}}({{\bf{q}}) \dot{{\bf{q}}} - P({{\bf{q}})

 
Equation of Motion:

{\bf{M}}({\bf{q}}) \ddot{\bf{q}} + {\bf{N}}({\bf{q}},\dot{\bf{q}}) + {\bf{G}}({\bf{q}}) = {\boldsymbol{\tau}}

\sum_i m_{ki} \ddot{q}_i + \sum_{i,j} c_{ij,k} \dot{q}_i \dot{q}_j + \dfrac{\partial P}{\partial q_k} = \tau_k

c_{ij,k} = c_{ji,k} = \dfrac{1}{2} \left[ \dfrac{\partial m_{ki}}{\partial q_j} + \dfrac{\partial m_{kj}}{\partial q_i} - \dfrac{\partial m_{ij}}{\partial q_k} \right]

Note: The matrix \dfrac{\partial m_{kj}}{\partial q_i} is the transpose of \dfrac{\partial m_{ki}}{\partial q_j}.

 
{\bf{M}}({\bf{q}}) \ddot{\bf{q}} + {\bf{C}}({\bf{q}},\dot{\bf{q}}) \dot{\bf{q}} + {\bf{G}}({\bf{q}}) = {\boldsymbol{\tau}}

\sum_i m_{ki} \ddot{q}_i + \sum_{i} c_{ki} \dot{q}_i + \dfrac{\partial P}{\partial q_k} = \tau_k

c_{ki} = \sum_j c_{ij,k} \dot{q}_j

 
Gimbals are balanced, thus P({{\bf{q}}) = 0 = {\bf{G}}({\bf{q}}).

 
Kinetic Energy:

The energy in the Lagrange function is additive. Therefore each arm contributes additively to the Lagrange function as well as to the equation of motion.

K_{rot} = \dfrac{1}{2} {\boldsymbol{\omega}}^T {{\bf{I}}' {\boldsymbol{\omega}} = \dfrac{1}{2} \left( {{\bf{R}}^T {\boldsymbol{\omega}} \right)^T {{\bf{I}} \left({{\bf{R}}^T {\boldsymbol{\omega}} \right) = \dfrac{1}{2} \dot{{\bf{q}}}^T \left[ \left( {{\bf{R}}^T {\bf{J}}_\omega \right)^T {{\bf{I}} \left( {{\bf{R}}^T {\bf{J}}_\omega \right) \right] \dot{{\bf{q}}} = \dfrac{1}{2} \dot{{\bf{q}}}^T \left( {{\bf{X}}^T {{\bf{I}} {{\bf{X}} \right) \dot{{\bf{q}}}

K_s = \dfrac{1}{2} {\bf{v}}_s^T m_s {\bf{v}}_s = \dfrac{1}{2} \dot{{\bf{q}}}^T {\bf{J}}_{v_s}^T m_s {\bf{J}}_{v_s} \dot{{\bf{q}}}

 
Generalized Inertia Matrix:

K_i = \dfrac{1}{2} \sum_{j,k} Tr \left[ \dfrac{\partial T^0_i}{\partial q_j} \bar{I}_i \dfrac{\partial T^0_i^T}{\partial q_k} \right] \dot{\bf{q}}_j \dot{\bf{q}}_k

\bar{I} = \left(\begin{array}{cccc} \frac{1}{2}(-I_x + I_y + I_z) & I_{xy} & I_{xz} & m x_s\\ I_{xy} & \frac{1}{2}(I_x - I_y + I_z) & I_{yz} & m y_s\\ I_{xz} & I_{yz} & \frac{1}{2}(I_x + I_y - I_z) & m z_s \\ m x_s & m y_s & m z_s & m \end{array}\right)

 
4.3. Manipulator with Actuator

Actuator:

\tau_M = K u - b_M \dot{\epsilon}

J_M \ddot{\epsilon} = \tau_M - \tau_{load} - b \dot{\epsilon} - \tau_{fric} - \tau_{cog}(\epsilon)

\Rightarrow  

\tau_{load} = K u - J_M \ddot{\epsilon} - B_M \dot{\epsilon} - \tau_{fric} - \tau_{cog}(\epsilon)

 
Equation of Motion:

{\bf{M}}({\bf{q}}) \ddot{\bf{q}} + {\bf{N}}({\bf{q}},\dot{\bf{q}}) = {\bf{K}} {\bf{u}} - {\bf{J}}_M \ddot{\bf{q}} - {\bf{B}}_M \dot{\bf{q}} - {\boldsymbol{\tau}}_{fric} - {\boldsymbol{\tau}}_{cog}({\bf{q}})

\Rightarrow  

\left[ {\bf{M}}({\bf{q}}) + {\bf{J}}_M \right] \ddot{\bf{q}} + \left[ {\bf{N}}({\bf{q}},\dot{\bf{q}}) + {\bf{B}}_M \dot{\bf{q}} \right] = {\bf{K}} {\bf{u}} - {\boldsymbol{\tau}}_{fric} - {\boldsymbol{\tau}}_{cog}

 
4.4. Cartesian Space

{\bf{y}} = h({\bf{q}})
\Rightarrow   \dot{\bf{y}} = {\bf{J}}_y \dot{\bf{q}}
\Rightarrow   \ddot{\bf{y}} = {\bf{J}}_y \ddot{\bf{q}} + \dot{\bf{J}}_y \dot{\bf{q}}   \Rightarrow   \ddot{\bf{q}} = {\bf{J}}_y^{-1} \left( \ddot{\bf{y}} - \dot{\bf{J}}_y \dot{\bf{q}} \right)   (if {\bf{J}}_y is invertible, otherwise use pseudoinverse)

\Rightarrow  

{\bf{M}} {\bf{J}}_y^{-1} \ddot{\bf{y}} + \left[ {\bf{N}} - {\bf{M}} {\bf{J}}_y^{-1} \dot{\bf{J}}_y \right] = {\boldsymbol{\tau}}

{\bf{J}}_y^{-T} {\bf{M}} {\bf{J}}_y^{-1} \ddot{\bf{y}} + \left[ {\bf{J}}_y^{-T} {\bf{N}} - {\bf{J}}_y^{-T} {\bf{M}} {\bf{J}}_y^{-1} \dot{\bf{J}}_y \right] = {\bf{J}}_y^{-T} {\boldsymbol{\tau}}

\tilde{\bf{M}} ({\bf{q}}) \ddot{\bf{y}} + \tilde{\bf{N}}({\bf{q}},\dot{\bf{q}}) = {\bf{f}} \equiv {\bf{J}}_y^{-T}{\boldsymbol{\tau}}

Comment: \tilde{\bf{M}} is symmetric and positive definite as \bf{M}} is. For {\bf{M}} {\bf{J}}_y^{-1} this doesn’t hold true.


5. Gimbal Models

The following gimbal models assume a gimbal in pitch-roll-yaw configuration, i.e., the matrices given in the above chapters Denavit Hartenberg or YRP Representation do apply.

 
5.1. Gimbal Model I

It is assumed that all mass is concentrated in the camera, and that the camera’s moment of inertia matrix is proportional to unit.

{{\bf{I}} = I {{\bf{1}}

\mathcal{L} = K_{rot} = \dfrac{1}{2} I \dot{\boldsymbol{\epsilon}}^T \left[ \left( {\bf{R}}^T {\bf{J}}_\omega \right)^T \left( {\bf{R}}^T {\bf{J}}_\omega \right) \right] \dot{\boldsymbol{\epsilon}} = \dfrac{1}{2} I \dot{\boldsymbol{\epsilon}}^T \left( {\bf{J}}_\omega^T {\bf{J}}_\omega \right) \dot{\boldsymbol{\epsilon}}

 
\mathcal{L} = \dfrac{1}{2} I \dot{\boldsymbol{\epsilon}}^T \left(\begin{array}{ccc} 1 & 0 & s_r \\ 0 & 1 & 0 \\ s_r & 0 & 1 \end{array}\right) \dot{\boldsymbol{\epsilon}} = \dfrac{1}{2} I \left( \dot{\epsilon}^2_y + \dot{\epsilon}^2_r + \dot{\epsilon}^2_p + 2 s_r \dot{\epsilon}_y \dot{\epsilon}_p \right)

{\boldsymbol{\tau}} = I \left(\begin{array}{c} \ddot{\epsilon}_y + s_r \ddot{\epsilon}_p + c_r \dot{\epsilon}_r \dot{\epsilon}_p \\ \ddot{\epsilon}_r - c_r \dot{\epsilon}_y \dot{\epsilon}_p \\ \ddot{\epsilon}_p + s_r \ddot{\epsilon}_y + c_r \dot{\epsilon}_y \dot{\epsilon}_r \end{array}\right)

 
5.2. Gimbal Model II

It is assumed that all mass is concentrated in the camera, and that the camera’s moment of inertia matrix is diagonal.

{{\bf{I}} = \left(\begin{array}{ccc} I_1 & 0 & 0 \\ 0 & I_2 & 0 \\ 0 & 0 & I_3 \end{array}\right)

\mathcal{L} = K_{rot} = \dfrac{1}{2} \dot{\boldsymbol{\epsilon}}^T \left( {\bf{X}}^T {\bf{I}} {\bf{X}} \right) \dot{\boldsymbol{\epsilon}} = \dfrac{1}{2} \dot{\boldsymbol{\epsilon}}^T \left[ \left(\begin{array}{ccc} 0 & s_r & c_r \\ 1 & 0 & 0 \\ 0 & 1 & 0 \end{array}\right) {\bf{R}}_y(\epsilon_p) {\bf{I}} {\bf{R}}^T_y(\epsilon_p) \left(\begin{array}{ccc} 0 & 1 & 0 \\ s_r & 0 & 1 \\ c_r & 0 & 0 \end{array}\right) \right] \dot{\boldsymbol{\epsilon}}

{\bf{R}}_y(\epsilon_p) {\bf{I}} {\bf{R}}^T_y(\epsilon_p) = \left(\begin{array}{ccc} I_1 c^2_p + I_3 s^2_p & 0 & (I_3-I_1) s_p c_p \\ 0 & I_2 & 0 \\ (I_3-I_1) s_p c_p & 0 & I_1 s^2_p + I_3 c^2_p \end{array}\right) \equiv \left(\begin{array}{ccc} I'_1 & 0 & I_\gamma \\ 0 & I_2 & 0 \\ I_\gamma & 0 & I'_3 \end{array}\right)

 
\mathcal{L} = \dfrac{1}{2} \dot{\boldsymbol{\epsilon}}^T \left(\begin{array}{ccc} I'_3 c^2_r + I_2 s^2_r & I_\gamma c_r & I_2 s_r \\ I_\gamma c_r & I'_1 & 0 \\ I_2 s_r & 0 & I_2 \end{array}\right) \dot{\boldsymbol{\epsilon}} = \dfrac{1}{2} \left[ \left(I'_3 c^2_r + I_2 s^2_r \right) \dot{\epsilon}^2_y + I'_1 \dot{\epsilon}^2_r + I_2 \dot{\epsilon}^2_p + 2 I_\gamma c_r \dot{\epsilon}_y \dot{\epsilon}_r + 2 I_2 s_r \dot{\epsilon}_y \dot{\epsilon}_p \right]

 
Comment: I_1 \leftrightarrow I_r, I_2 \leftrightarrow I_p, I_3 \leftrightarrow I_y.

Comment: To minimize axis coupling one should attempt I_1 \approx I_3 and I_2 \ll I_1,I_3. In this regard GoPro-style cameras are better than Mobius-style cameras.

Comment: I'_3 c^2_r + I_2 s^2_r = I'_3 - (I'_3-I_2) s^2_r

{\bf{M}}(y,r,p) \equiv \left(\begin{array}{ccc} I_a(r,p) & I_\gamma(p) c_r & I_2 s_r \\ I_\gamma(p) c_r & I_b(p) & 0 \\ I_2 s_r & 0 & I_2 \end{array}\right)

\tau_y = I_a \ddot{\epsilon}_y + I_\gamma c_r \ddot{\epsilon}_r + I_2 s_r \ddot{\epsilon}_p + I_{a/r} \dot{\epsilon}_y \dot{\epsilon}_r + I_{a/p} \dot{\epsilon}_y \dot{\epsilon}_p + ( I_{\gamma/p} c_r + I_2 s_r) \dot{\epsilon}_r \dot{\epsilon}_p - I_\gamma s_r \dot{\epsilon}_r^2
\tau_r = I_b \ddot{\epsilon}_r + I_\gamma c_r \ddot{\epsilon}_y + ( I_{\gamma/p} - I_2) c_r \dot{\epsilon}_y \dot{\epsilon}_p + I_{b/p} \dot{\epsilon}_r \dot{\epsilon}_p - \frac{1}{2} I_{a/r} \dot{\epsilon}_y^2
\tau_p = I_2 \ddot{\epsilon}_p + I_2 s_r \ddot{\epsilon}_y + (-I_{\gamma/p} + I_2)c_r \dot{\epsilon}_y \dot{\epsilon}_r - \frac{1}{2} I_{a/p} \dot{\epsilon}_y^2 - \frac{1}{2} I_{b/p} \dot{\epsilon}_r^2

I_a = \left( I_1 s^2_p + I_3 c^2_p \right) c^2_r + I_2 s^2_r
I_b = I_1 c^2_p + I_3 s^2_p
I_\gamma = (I_3 - I_1) s_p c_p

I_{a/r} = - 2 \left[ \left( I_1 s^2_p + I_3 c^2_p \right) - I_2 \right] c_r s_r
I_{a/p} = - 2 ( I_3 - I_1 ) s_p c_p c^2_r = - 2 I_\gamma c^2_r
I_{b/p} = - 2 ( I_3 - I_1 ) s_p c_p = - 2 I_\gamma
I_{\gamma/p} = (I_3 - I_1) ( c_p^2 - s_p^2 )

 
5.3. Gimbal Model III

It is assumed that the moment of inertia matrices of the camera and the roll and yaw arms are diagonal, and that the center of masses of the camera and roll arm coincide and fall onto the yaw rotation axis (d_2 = d_3 = 0). The Lagrangian consists then of only the rotational energies of the camera, roll and yaw arms.

{{\bf{I}}_{cam} = \left(\begin{array}{ccc} I^{cam}_1 & 0 & 0 \\ 0 & I^{cam}_2 & 0 \\ 0 & 0 & I^{cam}_3 \end{array}\right)   {{\bf{I}}_{roll} = \left(\begin{array}{ccc} I^{roll}_1 & 0 & 0 \\ 0 & I^{roll}_2 & 0 \\ 0 & 0 & I^{roll}_3 \end{array}\right)   {{\bf{I}}_{yaw} = \left(\begin{array}{ccc} I^{yaw}_1 & 0 & 0 \\ 0 & I^{yaw}_2 & 0 \\ 0 & 0 & I^{yaw}_3 \end{array}\right)

\mathcal{L} = K^{cam}_{rot} + K^{roll}_{rot} + K^{yaw}_{rot} = \dfrac{1}{2} \dot{{\bf{q}}}^T \left( {{\bf{M}}_{cam} + {{\bf{M}}_{roll} + {{\bf{M}}_{yaw} \right)\dot{{\bf{q}}}

{{\bf{M}}_{cam} = \left( {T^0_3}^T J^3_\omega \right)^T {{\bf{I}}_{cam} \left( {T^0_3}^T J^3_\omega \right) = \left(\begin{array}{ccc} I_a & I_\gamma c_r & I_2 s_r \\ I_\gamma c_r & I_b & 0 \\ I_2 s_r & 0 & I_2 \end{array}\right)

{{\bf{M}}_{roll} =\left( {T^0_2}^T J^2_\omega \right)^T {{\bf{I}}_{roll} \left( {T^0_2}^T J^2_\omega \right) = \left(\begin{array}{ccc} I^{roll}_1 c_r^2 + I^{roll}_3 s_r^2 & 0 & 0 \\ 0 & I^{roll}_2 & 0 \\ 0 & 0 & 0 \end{array}\right)

{{\bf{M}}_{yaw} = \left( {T^0_1}^T J^1_\omega \right)^T {{\bf{I}}_{yaw} \left( {T^0_1}^T J^1_\omega \right) = \left(\begin{array}{ccc} I^{yaw}_2 & 0 & 0 \\ 0 & 0 & 0 \\ 0 & 0 & 0 \end{array}\right)

\tau_y = \tau^{cam}_y + ( I^{roll}_1 c_r^2 + I^{roll}_3 s_r^2 + I^{yaw}_2 ) \ddot{\epsilon}_y
\tau_r = \tau^{cam}_r + I^{roll}_2 \ddot{\epsilon}_r + (I^{roll}_3 - I^{roll}_1) s_r c_r \dot{\epsilon}_y^2
\tau_p = \tau^{cam}_p

Comment: The result for {{\bf{M}}_{cam} had been calculated already in gimbal model II.

Comment: It holds {T^0_3}^T J^3_\omega = {{\bf{X}},   {T^0_2}^T J^2_\omega = \left(\begin{array}{ccc} c_r & 0 & 0 \\ 0 & 1 & 0 \\ s_r & 0 & 0 \end{array}\right),   {T^0_1}^T J^1_\omega = \left(\begin{array}{ccc} 0 & 0 & 0 \\ 1 & 0 & 0 \\ 0 & 0 & 0 \end{array}\right)

Comment: The moment of inertia matrices are with respect to the respective joint frames, and not the manipulator base frame.

With a more natural labeling of the moment of inertia values, the diagonals in the total {{\bf{M}} matrix can be written as:

M_{pp} = I^{cam}_y

M_{rr} = \left( I^{cam}_x c_p^2 + I^{cam}_z s_p^2 \right) + I^{roll}_x

M_{yy} = \left[ \left( I^{cam}_z c_p^2 + I^{cam}_x s_p^2 \right) c_r^2 + I^{cam}_y s_r^2 \right] + \left( I^{roll}_z c_r^2 + I^{roll}_y s_r^2 \right) + I^{yaw}_z

Comment: Both d_1 and d_3 should be zero, see a comment in the above. Only d_2 is of relevance. which should however add a constant moment of inertia to the yaw axis dynamics, and thus can be absorbed into I^{yaw}_z. The major assumption in the gimbal model III is thus that the moment of inertia matrices are diagonal, which often won’t be true for the roll arm, and is unrealistic for the yaw arm.


Appendix: References

[MS] Robot Dynamics and Control [.pdf] – by M.W. Spong, S. Hutchinson, and M. Vidyasaga.
[RJ] Theory of Applied Robotics[.pdf] – by R.N. Jazar.
[FL] Robot Manipulator Control: Theory and Practice [.pdf] – by F.L. Lewis, D.M. Dawson, and C.T. Abdallah
[RM] A Mathematical Introduction to Robotic Manipulation, A Mathematical Introduction to Robotic Manipulation [.pdf] – by R. M. Murray, Z. Li, and S. S. Sastry

Hinterlasse einen Kommentar