The Ordinary Differential Equati

2020-07-16  本文已影响0人  轻骑兵1390

1. The Description of Rotation

R_{cw} can project vectors from global to camera frame.
\mathbf{v}_c = R_{cw} \cdot \mathbf{v}_w \tag{1}
where \mathbf{v}_c and \mathbf{v}_w represent the vector in camera and global frame, respectively. There are two descriptions of (1) as following:

  1. The rotation matrix from global to camera frame.
  2. The rotation matrix representing global direction with respect to camera frame.

Translation works the same way.

2. Possion Equation

2.1 Rotation Matrix Version

A vector of angular velocity \mathbf{\omega} = \theta\mathbf{u} is the product of an angular \theta and an axis \mathbf{u}.
The velocity of vector \mathbf{n} is defined by the first derivative with repect to time:
\frac{d\mathbf{n}}{dt} = \mathbf{\omega} \times \mathbf{n} \tag{2}
We define that axes of the body frame are (\overrightarrow{i}, \overrightarrow{j}, \overrightarrow{k}). Usually the axes in body frame are \mathbf{i}_b = [1,0,0]^T, \mathbf{j}_b = [0,1,0]^T, \mathbf{k}_b = [0,0,1]^T. And the axes in the world frame is computed as following:
\begin{aligned} \begin{bmatrix} \mathbf{i}_w & \mathbf{j}_w & \mathbf{k}_w \end{bmatrix} &= R_{wb} \cdot \begin{bmatrix} \mathbf{i}_b & \mathbf{j}_b & \mathbf{k}_b \end{bmatrix} \\ &= R_{wb} \cdot \mathbf{I}_3 \\ \end{aligned}

R_{wb} = \begin{bmatrix} \mathbf{i}_w & \mathbf{j}_w & \mathbf{k}_w \end{bmatrix} \tag{3}
where \mathbf{I}_3 is a 3x3 identity matrix. The equation (3) means columns are vectors representing body axes with respect to global frame.
We can decompose the rotation about \mathbf{n} into three axes. \mathbf{\omega} in (2) is with respect to global frame. Then the ordinary differential equation is
\begin{matrix} \frac{d\mathbf{i}_w}{dt} = \mathbf{\omega} \times \mathbf{i}_w & \frac{d\mathbf{j}_w}{dt} = \mathbf{\omega} \times \mathbf{j}_w & \frac{d\mathbf{k}_w}{dt} = \mathbf{\omega} \times \mathbf{k}_w \end{matrix}

\dot{R}_{wb} = \mathbf{\omega} \times \begin{bmatrix} \mathbf{i}_w & \mathbf{j}_w & \mathbf{k}_w \end{bmatrix}

\dot{R}_{wb} = \mathbf{\omega}_w \times R_{wb} = [\mathbf{\omega}_w]_\times R_{wb} \tag{4}
Sensors usually are mounted on the robot and measure the angular velocity in body frame. Thus, we need to express (4) with \mathbf{\omega}_b using equations \mathbf{\omega}_b = R_{wb}^T\mathbf{\omega}_w and [Rv]_\times = R[v]_\times R^T
\dot{R}_{wb} = R_{wb} [\mathbf{\omega}_b]_\times \tag{5}
The equation (5) is consistent with the formula (6.45) in State Estimation for Robotics,
\begin{aligned} \dot{R}_{bw} &= [\mathbf{\omega}_b]^T_\times R_{bw} \\ &= -[\mathbf{\omega}_b]_\times R_{bw} \\ \end{aligned} \tag{6}
where the coordinate \underrightarrow{2} is the body frame in the book and \underrightarrow{1} is the global frame.
Equations (5) and (6) are Poisson's equation.

2.2 Quaternion Version

The relationship between rotation vector and quaternion can be expressed as:
q = \begin{bmatrix} cos(\theta/2)\\ \mathbf{u}sin(\theta/2) \end{bmatrix}
where the rotation vector is \mathbf{v} = \theta\mathbf{u}, \theta is the angular and \mathbf{u} is the unit vector.
In the limit, as \delta t \rightarrow 0, the angle of the rotation will become very small. Quaternion can be simplified as:
q_{t+\Delta t|t} = \lim_{\delta\theta \rightarrow 0} {\begin{bmatrix} cos(\delta\theta/2)\\ \mathbf{u}sin(\delta\theta/2) \end{bmatrix}} = \begin{bmatrix} 1 \\ \frac{1}{2} \delta\theta \mathbf{u} \end{bmatrix}
The derivative of \delta \theta with respect to \Delta t represents the angular velocity \mathbf{\omega}_b in the body frame.
\omega = \lim_{\Delta t \rightarrow 0} \frac{\delta \theta}{\Delta t}
\delta \theta is the angle between times t and \Delta t, thus it represent the local angular velocity.
We now deduce the quaternion derivative as
\begin{aligned} \dot{q}_{wb} &= \lim_{\Delta t \rightarrow 0} \frac{q_{w|{t+1}}- q_{w|t}}{\Delta t} \\ &= \lim_{\Delta t \rightarrow 0} \frac{q_{w|{t}}\otimes q_{t|t+1}- q_{w|t}}{\Delta t} \\ &= \lim_{\Delta t \rightarrow 0} \frac{q_{w|{t}}\otimes (q_{t|t+1} - q_0)}{\Delta t} \\ &= \lim_{\Delta t \rightarrow 0} \frac{q_{wb}\otimes ( \begin{bmatrix} 1 \\ \frac{1}{2}\delta\theta\mathbf{u}\end{bmatrix} - \begin{bmatrix} 1 \\ 0 \end{bmatrix} )}{\Delta t} \\ &= \frac{1}{2} q_{wb}\otimes \begin{bmatrix} 0 \\ \omega_b\mathbf{u}\end{bmatrix} \end{aligned}

\begin{aligned} \dot{q}_{wb} &= \frac{1}{2} q_{wb} \otimes \overline{\mathbf{\omega}}_b \\ &= \frac{1}{2} [\overline{\mathbf{\omega}}_b]_R \cdot q_{wb} \end{aligned} \tag{7}
where \overline{\mathbf{\omega}}_b is a pure quaternion and [\overline{\mathbf{\omega}}_b]_L is the left-quaternion-product matrix, which can be writen by:
[\overline{\mathbf{\omega}}_b]_L = \begin{bmatrix} 0 & -\mathbf{\omega}_b^T \\ \mathbf{\omega}_b & -[\mathbf{\omega}_b]_\times \end{bmatrix}
where \mathbf{\omega}_b is the imaginary unit. The mathematical deduction in the page can be found in section 4.5 of Quaternion kinematics for the error-state Kalman filter.

上一篇下一篇

猜你喜欢

热点阅读