从零手写vio基础（1）-旋转运动学

1 线速度与角速度

如上图所示：假设一个粒子在坐标中的$z=h$平面做圆周运动，坐标为：$\stackrel{.}{r}=(acos\theta ,asin\theta ,h{)}^T$，对坐标求导得：
$\stackrel{.}{r}=(-a\stackrel{.}{\theta }sin\theta ,a\stackrel{.}{\theta }cos\theta ,0{)}^T$
$=\left[\begin{array}{ccc}0& -\stackrel{.}{\theta }& 0\\ \stackrel{.}{\theta }& 0& 0\\ 0& 0& 0\end{array}\right]\left[\begin{array}{c}acos\theta \\ asin\theta \\ h\end{array}\right]$
$=\hat{w}r$
可以看出左边矩阵为反对称矩阵，这样其实就是$w=\left[\begin{array}{c}0\\ 0\\ \stackrel{.}{\theta }\end{array}\right]$。
$|\stackrel{.}{\theta }|$就是角速度的大小。
我们知道，线速度大小等于角速度乘以圆的半径。即，$|\stackrel{.}{r}|=a|\stackrel{.}{\theta }|$.

2 旋转坐标系下的运动学

上述简单例子是在静止坐标系下的，粒子的所有运动分析都是符合简单的牛顿定律的，那么在旋转坐标系下该怎么分析呢？如下图。

假设一个质量快在$body$坐标系(其实就是$imu$坐标系)下的坐标为：$r_B=(x_1,x_2,x_3{)}^T$
旋转到惯性系下有：
$r_I(t)=x_1(t)i+x_2(t)j+x_3(t)k=R_{IB}{r}_B$
解决旋转坐标系下这个问题$r_I(t)=R_{IB}{r}_B$对时间求导为：
${\stackrel{.}{r}}_I=R_{IB}{\stackrel{.}{r}}_B+\stackrel{.}{R_{IB}}{r}_B$

这里用到了旋转矩阵的求导：(采用右扰动模型)
$\stackrel{.}{R_{IB}}={\lim }_{\Delta t\to 0}\frac{R_{IB}exp(\widehat{w_b}\Delta t)-R_{IB}}{\Delta t}$
$\approx {\lim }_{\Delta t\to 0}\frac{R_{IB}(I+\widehat{w_b}\Delta t)-R_{IB}}{\Delta t}$
$={\lim }_{\Delta t\to 0}\frac{R_{IB}[\widehat{w_b}]\Delta t}{\Delta t}$
$={\lim }_{\Delta t\to 0}\frac{\widehat{[R_{IB}{w}_b]}{R}_{IB}\Delta t}{\Delta t}$
$=\widehat{w_I}{R}_{IB}$

注意：上面第二、三步用到了一个性质：$R_{IB}[\widehat{w_b}]=\widehat{[R_{IB}{w}_b]}{R}_{IB}$
完成上述推导，我们再来看看求导问题：

${\stackrel{.}{r}}_I=R_{IB}{\stackrel{.}{r}}_B+\stackrel{.}{R_{IB}}{r}_B$
$=R_{IB}{v}_B+\widehat{w_b}{R}_{IB}{r}_B$
$=v_I+\widehat{w_b}{r}_I$

推导到这里，我们会发现：在旋转坐标系下对$r$求导不只是会得到一个$v_I$,而是还多了一项$\widehat{w_b}{r}_I$。

接下来对速度继续求导。

我关于红框内的疑惑，是忘记了${\omega }_I=R_{IB}{\omega }_b$。
至此，旋转坐标系的运动学问题推导完毕，
结论：在旋转坐标系下运动的物体（运动方向和旋转轴不为同一个轴时）会受到科氏力的作用。利用这个性质就可以测量旋转坐标系的旋转角速度

3 目的

很多同学肯定疑惑，推导这个旋转坐标系下的运动模型有啥用？
其实，从上面的结论来看，在旋转坐标系下运动的物体（运动方向和旋转轴不为同一个轴时）会受到科氏力的作用。利用科氏力这个性质就可以测量旋转坐标系的旋转角速度，在后面的陀螺仪测量原理中可以看出这一点。科氏力=2$wv$,只要计算得到运动物体所受的科氏力以及运动物体速度，就可以得出角速度。

4 六轴imu的yaw角不可观

陀螺仪的构成如下图所示：

从3中我们知道：在旋转坐标系中，运动的物体受到科氏力作用。这里的旋转坐标系就是陀螺仪的坐标系，而运动的物体就是指陀螺主轴！
就是说，陀螺主轴在$x$方向上做高速往复运动。当陀螺仪在非主轴方向上发生旋转时，陀螺主轴会受到科氏力的作用，可以通过公式得到角速度；而在主轴方向上发生旋转，陀螺主轴不会受到科氏力影响，也就是说在俯仰角（Pitch）和翻滚角（Roll）不变的情况下，无论偏航角是多少，加速度计测出来的值是一样的。