从运动学到惯性测量单元IMU

Inertial Measurement Unit

1、旋转矩阵求导

考虑从世界坐标系 { W } \{W\} { W}原点出发的向量$\mathbf{p}$绕单位轴$\mathbf{u}$旋转，角速度矢量可以表示为$\mathbf{\omega }=\dot{\theta }\mathbf{u}$。易得向量$\mathbf{p}$末端点的速度矢量，即$\mathbf{p}$对时间的一阶导数为
$$\frac{\mathrm{d}\mathbf{p}}{\mathrm{d}t}=\mathbf{\omega }\times \mathbf{p}$$
事实上，当角速度大小和方向随时间改变时，上式仍然成立。

设机体坐标系 { B } \{B\} { B}到世界坐标系 { W } \{W\} { W}的旋转矩阵${\mathbf{R}}_{wb}=\left[{\mathbf{i}}_B,{\mathbf{j}}_B,{\mathbf{k}}_B\right]$，若三个轴${\mathbf{i}}_B,{\mathbf{j}}_B,{\mathbf{k}}_B$以角速度$\mathbf{\omega }$旋转，则根据上式，三个轴对时间的一阶导数为
$$\frac{\mathrm{d}{\mathbf{i}}_B}{\mathrm{d}t}=\mathbf{\omega }\times {\mathbf{i}}_B,\frac{\mathrm{d}{\mathbf{j}}_B}{\mathrm{d}t}=\mathbf{\omega }\times {\mathbf{j}}_B,\frac{\mathrm{d}{\mathbf{k}}_B}{\mathrm{d}t}=\mathbf{\omega }\times {\mathbf{k}}_B$$
故${\mathbf{R}}_{wb}$对时间的一阶导数为
$${\dot{\mathbf{R}}}_{wb}=[\mathbf{\omega }\times {\mathbf{i}}_B,\mathbf{\omega }\times {\mathbf{j}}_B,\mathbf{\omega }\times {\mathbf{k}}_B]=\mathbf{\omega }\times {\mathbf{R}}_{wb}$$
这里的角速度$\mathbf{\omega }$为机体坐标系 { B } \{B\} { B}相对于世界坐标系 { W } \{W\} { W}的角速度，且是在世界坐标系下表达的，故也记为${\mathbf{\omega }}_{wb}^w$

由叉乘运算和反对称矩阵乘法的对应关系得到
$${\dot{\mathbf{R}}}_{wb}=({\mathbf{\omega }}_{wb}^w{)}^{\wedge }{\mathbf{R}}_{wb}\text{\hspace{1em}(1)}$$
机体坐标系 { B } \{B\} { B}相对于世界坐标系 { W } \{W\} { W}的角速度，在机体坐标系下的表达为${\mathbf{\omega }}_{wb}^b={\mathbf{R}}_{bw}{\mathbf{\omega }}_{wb}^w$，故有${\mathbf{\omega }}_{wb}^w={\mathbf{R}}_{wb}{\mathbf{\omega }}_{wb}^b$
$${\dot{\mathbf{R}}}_{wb}=({\mathbf{R}}_{wb}{\mathbf{\omega }}_{wb}^b{)}^{\wedge }{\mathbf{R}}_{wb}\text{\hspace{1em}(2)}$$
根据§1，$(2)$可写为
$${\dot{\mathbf{R}}}_{wb}={\mathbf{R}}_{wb}({\mathbf{\omega }}_{wb}^b{)}^{\wedge }\text{\hspace{1em}(3)}$$
若${\mathbf{\omega }}_b$为定值，给定$t_0$时刻初始旋转矩阵为$\mathbf{R}(t_0)$，则微分方程$(3)$的解析解为
$$\mathbf{R}(t)=\mathbf{R}(t_0)\exp \left(({\mathbf{\omega }}_{wb}^b{)}^{\wedge }(t-t_0)\right)$$
简便起见，可以将上式写成
$$\mathbf{R}(t)=\mathbf{R}(t_0)\mathrm{Exp}\left({\mathbf{\omega }}_{wb}^b(t-t_0)\right)$$

2、车体坐标系与世界坐标系的速度，加速度转换关系

车的运动速度指的是车体坐标系 { B } \{B\} { B}相对于世界坐标系 { W } \{W\} { W}的速度在世界坐标系下的表示，记作${\mathbf{v}}_{wb}^w$，车体坐标系原点在车体坐标系下一直为原点，速度为零，所以不谈论该点在车体坐标系下的速度，我们谈论的是车体坐标系 { B } \{B\} { B}相对于世界坐标系 { W } \{W\} { W}的速度在车体坐标系下的表示

$${\mathbf{v}}_{wb}^b={\mathbf{R}}_{bw}{\mathbf{v}}_{wb}^w$$
这个速度可以通过轮子、电机等设备测到。实际上，${\mathbf{v}}_{wb}^b={\dot{\mathbf{p}}}_{wb}^b$的值与世界坐标系的选取无关，${\mathbf{v}}_{wb}^w={\dot{\mathbf{p}}}_{wb}^w$的取值依赖于世界坐标系的选取

为方便推导，可将上式改写为
$${\mathbf{v}}_{wb}^w={\mathbf{R}}_{wb}{\mathbf{v}}_{wb}^b$$
对上式进行求导，可以得到
$$\begin{array}{rl}{\dot{\mathbf{v}}}_{wb}^w& ={\dot{\mathbf{R}}}_{wb}{\mathbf{v}}_{wb}^b+{\mathbf{R}}_{wb}{\dot{\mathbf{v}}}_{wb}^b\\ & ={\mathbf{R}}_{wb}({\mathbf{\omega }}_{wb}^b{)}^{\wedge }{\mathbf{v}}_{wb}^b+{\mathbf{R}}_{wb}{\dot{\mathbf{v}}}_{wb}^b\end{array}\text{\hspace{1em}(1)}$$
记${\mathbf{a}}_{wb}^b$为车体坐标系下的位移加速度，而车体坐标系下的总加速度${\dot{\mathbf{v}}}_{wb}^b$除位移加速度外，还包含了因车体坐标系以角速度${\mathbf{\omega }}_{wb}^b$旋转而产生的加速度，该旋转亦可看作${\mathbf{v}}_{wb}^b$以$(-{\mathbf{\omega }}_{wb}^b)$的角速度绕车体坐标系旋转，故车体坐标系下总加速度为
$${\dot{\mathbf{v}}}_{wb}^b={\mathbf{a}}_{wb}^b-{\mathbf{\omega }}_{wb}^b\times {\mathbf{v}}_{wb}^b\text{\hspace{1em}(2)}$$
故$(1)$式可以写为
v ˙ w b w = R w b ( ω w b b ) ∧ v w b b + R w b ( a w b b − ω w b b × v w b b ) = R w b