# Gimbal Control - 運動學
###### tags: `book_FW`
---
e: earth frame
a: plane frame
c: camera frame
$\vec{V}_{p}$ 為無人機速度，$\vec{V}_{v}$ 為目標物速度，$\vec{\omega}_{p}$ 為無人機角速度，$\vec{\omega}_{c/p}$ 為相機相對無人機的角速度，$\vec{r}_{c/p}$ 為相機相對無人機的位置，$\vec{r}_{v/c}$ 為目標物相對相機的位置
The relative velocity between the moving target and the camera
$\dot{\vec{r}}_{v / c} = \vec{V}_{v} - \vec{V}_{p} - (\vec{\omega}_{p} + \vec{\omega}_{c/p}) \times \vec{r}_{v / c} - \vec{\omega}_{p} \times \vec{r}_{c / p}$
The relative velocity can be written as
$\dot{r}_{v / c} = {}^{c}R_{e} \color{red}[{}^{e}V_{v} - {}^{e}V_{p} - \color{blue}{{}^{e}R_{c} ( ({}^{c}R_{a}{}^{a}\omega_{p} ) \times} {}^\color{red}{c} r_{v / c}) - {}^\color{red}{e}R_{\color{red}c}({}^{c} \omega_{c/p} \times {}^\color{red}{c} r_{v / c}) - {}^{\color{red}e}R_{a} ({}^{a} \omega_{p} \times {}^\color{red}{a} r_{c / p})\color{red}]$
which can be presented as
$\begin{align}
\dot{r}_{v / c} &= {}^{c}R_{e} [{}^{e}V_{v} - {}^{e}V_{p} - {}^{e}R_{c} ( \color{blue}{ {}^{c}\omega_{p} \times {}^{c} r_{v / c} } ) - {}^{e}R_{c}({}^{c} \omega_{c/p} \times {}^{c} r_{v / c}) - {}^{e}R_{a} \color{blue}{ {}^{a}R_{c} ({}^{c} \omega_{p} \times {}^{c} r_{c/p})} ] \\
&= {}^{c}V_{v} - {}^{c}V_{p} - {}^{c}\omega_{p} \times {}^{c} r_{v / c} - {}^{c} \omega_{c/p} \times {}^{c} r_{v / c} - {}^{c} \omega_{p} \times {}^{c} r_{c / p}
\end{align}$
:::success
<font color="blue">**Properties Used**</font>
${}^{e} \dot{R}_{b}={}^{e}{R}_{b}[\omega]_{\times}$
$\vec{r}_{A} =\vec{r}_{B}+\vec{r}_{A/B}$ (vector form)
${r}_{A} ={r}_{B}+{}^{e}R_{{b}}{r}_{A/B}$ (where ${r}_{A/B}$ is expressed in the body frame)
$\dot{r}_{A} =\dot{r}_{B}+\color{red}{{}^{e}{R}_{b}[\omega]_{\times}} {r}_{A/B}+{}^{e}R_{{b}}\color{blue}{\dot{r}_{A/B}}$ (convert to 3x1 form, $\color{blue}{\dot{r}_{A/B}}$ is similar to $\color{blue}{\dot{r}_{v/c}}$)
where $[\omega]_{\times} {r}_{A/B}$=$\omega\times{r}_{A/B}$
$R(a \times b) = (Ra) \times (Rb)$
:::
For PT camera, assume the order of rotation is (yaw -> pitch)
$\vec{\omega}_{c/p} = \dot\theta_{yaw} \vec{a}_{yaw}+ \dot\theta_{pitch} \vec{b}_{pitch}$
In the camera frame
${}^{c} \omega_{c/p} = {}^{c}R_{a}R_{z}(\theta_{yaw}) {}^{a}\omega_{yaw} + {}^{c}R_{b} R_{y}(\theta_{pitch}) {}^{b}\omega_{pitch}$
$\begin{align}
R_{z}(\theta_{yaw}) = \left[\begin{array}{c}
cos(\theta_{yaw}) & sin(\theta_{yaw}) & 0 \\
-sin(\theta_{yaw}) & cos(\theta_{yaw}) & 0 \\
0 & 0 &1
\end{array}\right] \end{align}$, $\begin{align}
R_{y}(\theta_{pitch}) = \left[\begin{array}{c}
cos(\theta_{pitch}) & 0 & -sin(\theta_{pitch}) \\
0 & 1 & 0 \\
sin(\theta_{pitch}) & 0 & cos(\theta_{pitch})
\end{array}\right] \end{align}$
expand the relative velocity
$\begin{align}
\dot{r}_{v / c}
&= {}^{c}V_{v} - {}^{c}V_{p} - {}^{c}\omega_{p} \times {}^{c} r_{v / c} - {}^{c} \omega_{c/p} \times {}^{c} r_{v / c} - {}^{c} \omega_{p} \times {}^{c} r_{c / p} \\
&=\left[\begin{array}{c}
{v}_{vx} - {v}_{px} - \omega_{2} Z + \omega_{3} Y - \omega_{c2} Z + \omega_{c3} Y - \omega_{2} m_{z} + \omega_{3} m_{y}\\
{v}_{vy} - {v}_{py} + \omega_{1} \color{red}{Z}- \omega_{3} \color{red}{X} + \omega_{c1} \color{red}{Z} - \omega_{c3} \color{red}{X} + \omega_{1} m_{z} - \omega_{3} m_{x}\\
{v}_{vz} - {v}_{pz} - \omega_{1} Y + \omega_{2} X - \omega_{c1} Y + \omega_{c2} X - \omega_{1} m_{y} + \omega_{2} m_{x}
\end{array}\right] = \left[\begin{array}{c}
\dot{X} \\
\dot{Y} \\
\dot{Z} \\
\end{array}\right]
\end{align}$
, where
${}^{c}{V}_{p} = \left[\begin{array}{c}
\ {v}_{px}, \ {v}_{py}, \ {v}_{pz}
\end{array}\right]^{T}$ (known term)*
${}^{c}{V}_{v} = \left[\begin{array}{c}
\ {v}_{vx}, \ {v}_{vy}, \ {v}_{vz}
\end{array}\right]^{T}$ *(assume it's a known term)**
${}^{c}\omega_{p} = \left[\begin{array}{c}
\ {\omega}_{1}, \ {\omega}_{2}, \ {\omega}_{3}
\end{array}\right]^{T}$ *(known term)*
${}^{c} \omega_{c/p} = \left[\begin{array}{c}
\ {\omega}_{c1}, \ {\omega}_{c2}, \ {\omega}_{c3}
\end{array}\right]^{T}$ *(to be control)*
${}^{c}r_{c/p} = \left[\begin{array}{c}
\ m_{x}, \ m_{y}, \ m_{z}
\end{array}\right]^{T}$ *(known term)*
${}^{c}{r}_{v / c}=\left[\begin{array}{c}
\ X, \ Y, \ Z
\end{array}\right]^{T}$ *(known term, assume Z is a known term)*
:::warning
<span class="blue"> **Computation** </span>
$\begin{aligned}
V_{v/c} &= V_{v} - V_{p} - {}^cR_{a}( {}^{a}\omega_{p} \times {}^{a}r_{c/p} ) - ( {}^{c}R_{a} {}^{a}\omega_{p} ) \times {}^{c}r_{v/c} - {}^{c}\omega_{c/p} \times {}^{c}r_{v/c} \\
&= V_{v} - V_{p} - {}^{c}\omega_{p} \times {}^{c}r_{c/p} - {}^{c}\omega_{p} \times {}^{c}r_{v/c} - {}^{c}\omega_{c/p} \times {}^{c}r_{v/c} \\
&= \left[\begin{array}{c}
V_{vx}-V_{px} \\ V_{vy}-V_{py} \\ V_{vz}-V_{pz}
\end{array}\right] -
\left[\begin{array}{c}
\omega_{2} m_{z} - \omega_{3} m_{y} \\
-\omega_{1} m_{z} + \omega_{3} m_{x} \\
\omega_{1} m_{y} - \omega_{2} m_{x}
\end{array}\right] -
\left[\begin{array}{c}
\omega_{2} Z - \omega_{3} Y \\
-\omega_{1} Z + \omega_{3} X \\
\omega_{1} Y - \omega_{2} X
\end{array}\right] -
\left[\begin{array}{c}
\omega_{c2} Z - \omega_{c3} Y \\
-\omega_{c1} Z + \omega_{c3} X \\
\omega_{c1} Y - \omega_{c2} X
\end{array}\right] \\
&= \left[\begin{array}{c}
V_{vx}-V_{px} - \omega_{c2} Z+\omega_{c3} Y - \omega_{2}( m_{z}+Z ) + \omega_{3} (m_{y}+Y) \\
V_{vy}-V_{py} + \omega_{c1} Z-\omega_{c3} X + \omega_{1}( m_{z}+Z ) - \omega_{3} (m_{x}+X) \\
V_{vz}-V_{pz} - \omega_{c1} Y+\omega_{c2} X - \omega_{1}( m_{y}+Y ) + \omega_{2} (m_{x}+X)
\end{array}\right]
\end{aligned}$
:::
---
System States
$\mathbf{x} = \left[\begin{array}{c}
\ x_{1}, \ x_{2}, \ x_{3}, \ r_{v}^{T}, \ V_{v}^{T}
\end{array}\right]^{T}$
, where $\left[\begin{array}{c}
\ x_{1}, \ x_{2}, \ x_{3}
\end{array}\right]^{T} = \left[\begin{array}{c}
\ \dfrac{X}{Z}, \dfrac{Y}{Z} \ , \dfrac{1}{Z} \
\end{array}\right]^{T}$
System Dynamics
$\dot{x} = \left[\begin{array}{c}
\dot {x}_1 \\
\dot{x}_2 \\
\dot{x}_3 \\
V_{v} \\
0_{3 \times 1}
\end{array}\right]=\left[\begin{array}{c}
\dot{(\frac{X}{Z})} \\
\dot{(\frac{Y}{Z})} \\
\dot{(\frac{1}{Z})} \\
V_{v} \\
0_{3 \times 1}
\end{array}\right]=\left[\begin{array}{ccc}
{(\frac{\dot{X}Z-X\dot{Z}}{Z^2})} \\
{(\frac{\dot{Y}Z-Y\dot{Z}}{Z^2})} \\
{(\frac{-\dot Z}{Z^2})} \\
V_{v} \\
0_{3 \times 1}
\end{array}\right]= \left[\begin{array}{c}
v_{vx} x_{3} - v_{vz} x_{1} x_{3} + \xi_{1} + \Omega_{1}+ \eta_{1} \\
v_{vy} x_{3} - v_{vz} x_{2} x_{3} + \xi_{2} + \Omega_{2}+ \eta_{2} \\
-v_{vz} x^{2}_{3} + \xi_{3} + \Omega_{3}+ \eta_{3}\\
V_{v} \\
0_{3 \times 1}
\end{array}\right]$
$\begin{aligned}
\xi_{1}(t, \mathbf{x}) &= \omega_{c1} x_{1} x_{2} -\omega_{c2} - \omega_{c2} x_{1}^{2} + \omega_{c3} x_{2} \\
\xi_{2}(t, \mathbf{x}) &= \omega_{c1} + \omega_{c1} x_{2}^{2} -\omega_{c2} x_{1} x_{2} - \omega_{c3} x_{1} \\
\xi_{3}(t, \mathbf{x}) &= \omega_{c1} x_{2} x_{3} - \omega_{c2} x_{1} x_{3} \\
\Omega_{1}(t, \mathbf{x}) &=\omega_{1} x_{1} (x_{2} + m_{y} x_{3}) - \omega_{2} - \omega_{2} x_{1}^{2} - \omega_{2} x_{3} (m_{z} + m_{x} x_{1}) + \omega_{3} (x_{2} + m_{y} x_{3}) \\
\Omega_{2}(t, \mathbf{x}) &=\omega_{1} + \omega_{1} x_{3} (m_{z} + m_{y} x_{2}) + \omega_{1} x_{2}^{2} - \omega_{2} x_{2} (x_{1} + m_{x} x_{3}) + \omega_{3} (-x_{1} - m_{x} x_{3}) \\
\Omega_{3}(t, \mathbf{x}) &= \omega_{1}( x^{2}_{3}m_{y}+x_{2}x_{3} ) + \omega_{2}( -x^{2}_{3}m_{x}-x_{1}x_{3} ) \\
\eta_{1}(t, \mathbf{x}) &=\left(v_{pz} x_{1} - v_{px}\right) x_{3} \\
\eta_{2}(t, \mathbf{x}) &=\left(v_{pz} x_{2} - v_{py}\right) x_{3} \\
\eta_{3}(t, \mathbf{x}) &=v_{pz} x^{2}_{3}
\end{aligned}$
:::warning
<span class="blue"> **Computation** </span>
$\begin{aligned}
\dot{x}_{1} &= \dot{(\frac{X}{Z})} = \frac{\dot{X}}{Z} - \frac{X}{Z} \frac{\dot{Z}}{Z} \\
\\
&= x_{3} (V_{vx}-V_{px}) - \omega_{c2} + \omega_{c3} x_{2} - \omega_{2} ( x_{3} m_{z}+1 ) + \omega_{3}(x_{3}m_{y}+x_{2}) \\
& - x_{1} \left[ x_{3}(V_{vz}-V_{pz}) - \omega_{c1}x_{2} + \omega_{c2}x_{1} - \omega_{1}( x_{3}m_{y}+x_{2} ) + \omega_{2}( x_{3}m_{x}+x_{1} ) \right] \\
\\
&= x_{3}( V_{vx}-V_{px} ) - x_{1}x_{3}( V_{vz}-V_{pz} ) + \omega_{c1} x_{1} x_{2} -\omega_{c2} - \omega_{c2} x^{2}_{1} + \omega_{c3} x_{2} \\
& + \omega_{1} x_{1}( x_{3}m_{y}+x_{2} ) - \omega_{2}( x_{3}m_{z}+1 ) - \omega_{2}x_{1}( x_{3}m_{x}+x_{1} ) + \omega_{3}( x_{3}m_{y}+x_{2} ) \\
\\
&= (V_{px}-V_{vx})( -x_{3} ) + (V_{pz}-V_{vz})( x_{1}x_{3} ) + \omega_{c1}( x_{1}x_{2} ) + \omega_{c2}\left[ -(1+x^{2}_{1}) \right] + \omega_{c3}( x_{2} ) \\
& + \omega_{1} \left[ x_{1}( x_{3}m_{y}+x_{2} ) \right] + \omega_{2} \left[ -( x_{3}m_{z}+1 ) - x_{1}( x_{3}m_{x}+x_{1} ) \right] + \omega_{3}( x_{3}m_{y}+x_{2} )
\end{aligned}$
...
$\begin{aligned}
\dot{x}_{2} &= \dot{(\frac{Y}{Z})} = \frac{\dot{Y}}{Z} - \frac{Y}{Z} \frac{\dot{Z}}{Z} \\
\\
&= x_{3}(V_{vy}-V_{py}) + \omega_{c1} - \omega_{c3} x_{1} + \omega_{1}( x_{3}m_{z}+1 ) - \omega_{3}( x_{3}m_{x}+x_{1} ) \\
& -x_{2} \left[ x_{3}( V_{vz}-V_{pz} ) - \omega_{c1} x_{2} + \omega_{c2} x_{1} - \omega_{1}( x_{3}m_{y} + x_{2} ) + \omega_{2}( x_{3}m_{x} + x_{1} ) \right] \\
\\
&= x_{3}( V_{vy}-V_{py} ) - x_{2}x_{3}( V_{vz}-V_{pz} ) + \omega_{c1} + \omega_{c1}x^{2}_{2} - \omega_{c2}x_{1}x_{2} - \omega_{c3}x_{1} \\
& +\omega_{1}( x_{3}m_{z}+1 ) + \omega_{1}x_{2}( x_{3}m_{y}+x_{2} ) - \omega_{2}x_{2}(x_{3}m_{x}+x_{1}) - \omega_{3}( x_{3}m_{x}+x_{1} ) \\
\\
&= (V_{py}-V_{vy})( -x_{3} ) + (V_{pz}-V_{vz})( x_{2}x_{3} ) + \omega_{c1}( 1+x^{2}_{2} ) + \omega_{c2}( -x_{1}x_{2} ) - \omega_{c3}( -x_{1} ) \\
& + \omega_{1}\left[ x_{3}m_{z}+1+x_{2}( x_{3}m_{y}+x_{2} ) \right] + \omega_{2}\left[ -x_{2}( x_{3}m_{x}+x_{1} ) \right] + \omega_{3}\left[ -( x_{3}m_{x}+x_{1} ) \right]
\end{aligned}$
...
$\begin{aligned}
\dot{x}_{3} &= \dot{(\frac{1}{Z})} = \frac{ - \dot{Z}}{Z^{2}} \\
\\
&= - x_{3} \left[ x_{3}( V_{vz}-V_{pz} ) - \omega_{c1}x_{2} + \omega_{c2}x_{1} - \omega_{1}( x_{3}m_{y}+x_{2} ) + \omega_{2}( x_{3}m_{x}+x_{1} ) \right] \\
\\
&= (V_{pz}-V_{vz})( x^{2}_{3} ) + \omega_{c1}( x_{2}x_{3} ) + \omega_{c2}( -x_{1}x_{3}) \\
& + \omega_{1}( x^{2}_{3}m_{y}+x_{2}x_{3} ) + \omega_{2}( -x^{2}_{3}m_{x}-x_{1}x_{3} )
\end{aligned}$
:::
---
Based on pin hole model, the states $x_1$ and $x_2$ can be obtained by projecting the position of the target onto the image plane as
$x_1 = \dfrac{u-c_u}{f_x} \\
x_2 = \dfrac{v-c_v}{f_y}$
---
The control objective
$s^* \triangleq \left[\begin{array}{c}
\ x_{1}, \ x_{2}
\end{array}\right] \triangleq \left[\begin{array}{c}
\ 0, \ 0
\end{array}\right]$
above implies $\left[\begin{array}{c}
\ r_{v/c, x}, \ r_{v/c, y}
\end{array}\right]^{T} \rightarrow \left[\begin{array}{c}
\ 0, \ 0
\end{array}\right]^{T}$
Define the tracking error as
$e(t) = s(t) - s^*$
Open-loop Error System
$\dot{e} = \dot{s}(t) = \left[\begin{array}{c}
\ \dot{x}_{1}, \dot{x}_{2}
\end{array}\right]^{T} = \left[\begin{array}{c}
v_{vx} x_{3} - v_{vz} x_{1} x_{3} + \xi_{1} + \Omega_{1}+ \eta_{1} \\
v_{vy} x_{3} - v_{vz} x_{2} x_{3} + \xi_{2} + \Omega_{2}+ \eta_{2}
\end{array}\right]$
$\dot{e} = \dot{s} = L_{e} \left[\begin{array}{c}
{}^{c}V_{p} - {}^{c}V_{v} \\ {}^{c}\omega_{c/p} \\ {}^{c}\omega_{p}
\end{array}\right]$
$L_{e} = \left[\begin{array}{c}
-x_{3} & 0 & x_{1}x_{3} & x_{1} x_{2} & -(1+x_{1}^{2}) & x_{2} & x_{1}(x_{2} + m_{y}x_{3}) & -(1 + x_{1}^{2} + x_{3}(m_{z} + m_{x} x_{1})) & x_{2} + m_{y}x_{3} \\
0 & -x_{3} & x_{2}x_{3} & (1+x_{2}^{2}) & -x_{1}x_{2} & -x_{1} & 1 + x_{2}^2 + x_{3}(m_{z} + m_{y} x_{2}) & -x_{2}(x_{1} + m_{x} x_{3}) & -x_{1} - m_{x} x_{3}
\end{array}\right]$
The pitch angular velocity and yaw angular velocity of camera would be controlled, the open-loop error system can be modified as
$\dot{e} = L_{2}
\left[\begin{array}{c}
{}^{c}V_{p} - {}^{c}V_{v} \\ {}^{c}\omega_{p}
\end{array}\right] + L_{1}
\left[\begin{array}{c}
{}^{c}\omega_{c/p}
\end{array}\right]$
, where
$\begin{align}
L_{1} = \left[\begin{array}{c}
x_{1} x_{2} & -(1 + x_{1}^{2}) & x_{2} \\
(1+x_{2}^{2}) & -x_{1} x_{2} & -x_{1}
\end{array}\right]
\end{align}$ , $\begin{align}
L_{2} = \left[\begin{array}{c}
-x_{3} & 0 & x_{1} x_{3} & x_{1}(x_{2} + m_{y}x_{3}) & -(1 + x_{1}^{2} + x_{3}(m_{z} + m_{x} x_{1})) & x_{2} + m_{y}x_{3} \\
0 & -x_{3} & x_{2} x_{3} & 1 + x_{2}^2 + x_{3}(m_{z} + m_{y} x_{2}) & -x_{2}(x_{1} + m_{x} x_{3}) & -x_{1} - m_{x} x_{3}
\end{array}\right]
\end{align}$
hope the error could be the exponentially decaded function ($\dot{e}^{-ax} = -a e^{-ax}$), which implies $\dot{e} = -\lambda e$.
Controller Design
$\left[\begin{array}{c}
{}^{c}\omega_{c/p}
\end{array}\right] = - \nu L_{1}^{+} L_{2}
\left[\begin{array}{c}
{}^{c}V_{p} - {}^{c}V_{v} \\ {}^{c}\omega_{p}
\end{array}\right] -\lambda L_{1}^{+} e$ $\space\space$ , where $L_{1}^{+} = L_{1}^{T}( L_{1} L_{1}^{T} )^{-1}$
, where $\nu$ and $\lambda$ are the control gains.
check the singularity
$\begin{align}
det( L_{1}L^{T}_{1} ) &= (1+x^{2}_{1}) ( 1+x^{2}_{2} ) ( 1+x^{2}_{1}+x^{2}_{2} )^{2} - x^{2}_{1} x^{2}_{2} ( 1 + x^{2}_{1} + x^{2}_{2})^{2} \\
&= ( 1 + x^{2}_{1} + x^{2}_{2})^{2} \left[ (1+x^{2}_{1}) ( 1+x^{2}_{2} ) - x^{2}_{1} x^{2}_{2} \right] \\
&= ( 1 + x^{2}_{1} + x^{2}_{2})^{3} \\
&> 0 \\
&\not= 0
\end{align}$
The closed-loop error system:
$\dot{e} = L_{2}
\left[\begin{array}{c}
{}^{c}V_{p} -{}^{c}V_{v} \\ {}^{c}\omega_{p}
\end{array}\right] - \nu L_{1} L_{1}^{+} L_{2} \left[\begin{array}{c}
{}^{c}V_{p} - {}^{c}V_{v} \\ {}^{c}\omega_{p}
\end{array}\right] -\lambda L_{1} L_{1}^{+} e$
<style>
.gray {
color: gray;
}
.blue {
color: blue;
}
</style>
Sign in with Wallet
Connect another wallet