第二十一篇 [IMU+RTK+WHEEL]组合定位18维状态设计

参考这篇论文第四章

1. 字符含义

$b$: 载体坐标系
$i$: 惯性坐标系
$D$:odometer
$n$:导航坐标系(东北天坐标系)
$m$:IMU坐标系
$C_b^n$:表示b到n旋转矩阵或者是姿态矩阵
$q_b^n$:表示b到n旋转四元素
$w_{nb}^b$:表示b系相对n系的旋转在b系中的表达

2. 系统建模

状态量：位置(东北天)误差、速度误差、姿态误差(失准角)、陀螺仪零偏误差、加速计零偏误差、安装误差角误差（heading,pitch）、轮速计尺度因子误差，共18维。
$$[\delta p_x,\delta p_y,\delta p_z,\delta v_x,\delta v_y,\delta v_z,{\varphi }_x,{\varphi }_y,{\varphi }_z,\delta {\nabla }_x,\delta {\nabla }_y,\delta {\nabla }_z,\delta {ϵ}_x,\delta {ϵ}_y,\delta {ϵ}_z,\delta {\alpha }_y,\delta {\alpha }_z,\delta s]$$
观测量1：GPS位置、速度和航向。
观测量2：[轮速，0，0]。采用非完整性约束。
特别注意1：此处失准角的定义为左扰动模型，定义为真值n系旋转到计算机推算得到n’系之间的旋转角度。必须与后面的correct相对应。因为姿态角计算不是简单的加减法。
$$C_{n^{\prime }}^n=I+[\varphi {]}_{\times }$$
因此加上右扰动的姿态求导如下式，这个式子是后面求误差传播方程的基础。
$$\delta C_b^n=C_n^{n^{\prime }}\ast C_b^n-C_b^n=-[\varphi {]}_{\times }{C}_b^n$$
特别注意2：此处安装角的失准角使用了右扰动模型，定义为真值m系旋转到计算机推算得到的m’系之间的旋转角。必须与后面的correct相对应。因为姿态角计算不是简单的加减法。
$$C_{m^{\prime }}^m=I+[\delta \alpha {]}_{\times }$$
因此加上左扰动的姿态求导如下式，这个式子是后面求误差传播方程的基础。
$$\delta C_m^b=C_m^b\ast C_{m^{\prime }}^m-C_m^b=C_m^b\ast [\delta \alpha {]}_{\times }$$
特别注意3
使用eigen求等效旋转向量时

1. IMU测量了一个$b_k$系到$b_{k+1}$系的角度旋转量$\Delta \theta$，则将$b_{k+1}$系的坐标转到$b_k$系的坐标的等效旋转矢量可以Eigen::AngleAxisd(theta.norm(), theta.normalized()).toRotationMatrix()求出。
2. 1中所述也可以和失准角联系起来。

3 状态微分方程

$$\begin{gathered} {\dot{C}}_b^n=C_b^n\ast {\omega }_{nb}^b \\ {\dot{v}}^n=C_b^n\ast f^b+g^n \\ {\dot{p}}^n=v^n \end{gathered}$$

4. 误差传播方程

4.1 姿态误差传播方程

推导过程从教科书中15维姿态误差微分方程推出。
$$\begin{gathered} {\dot{\varphi }}^n=-C_b^n\ast \delta {\omega }_{nb}^b \\ {\omega }_{nb}^b=C_m^b\ast ({\omega }_{nm}^m-{ϵ}_{nm}^m) \\ \delta {\omega }_{nb}^b=\delta C_m^b\ast ({\omega }_{nm}^m-{ϵ}_{nm}^m)-C_m^b\ast \delta {ϵ}_{nm}^m \\ \delta {\omega }_{nb}^b=C_m^b\ast [\delta \alpha ]\ast ({\omega }_{nm}^m-{ϵ}_{nm}^m)-C_m^b\ast \delta {ϵ}_{nm}^m \end{gathered}$$
因此：
$${\dot{\varphi }}^n=C_b^n\ast C_m^b\ast [{\omega }_{nm}^m-{ϵ}_{nm}^m]\ast \delta \alpha +C_b^n\ast C_m^b\ast \delta {ϵ}_{nm}^m$$

4.2 速度误差传播方程

推导过程从教科书中15维姿态误差微分方程推出。
$$\begin{gathered} \delta {\dot{v}}^n=C_b^n\ast f^b\times {\varphi }^n+C_b^n\ast \delta f^b \\ {f}^b=C_m^b\ast (f^m-{\nabla }^m) \\ \delta f^b=\delta C_m^b\ast (f^m-{\nabla }^m)-C_m^b\ast \delta {\nabla }^m \\ \delta f^b=C_m^b\ast [\delta \alpha ]\ast (f^m-{\nabla }^m)-C_m^b\ast \delta {\nabla }^m \end{gathered}$$
因此：
$$\delta {\dot{v}}^n=C_b^n\ast C_m^b\ast (f^m-{\nabla }^m)\times {\varphi }^n-C_b^n\ast C_m^b\ast [f^m-{\nabla }^m]\ast \delta \alpha -C_b^n\ast C_m^b\ast \delta {\nabla }_{nm}^m$$

4.3 位置误差传播方程

推导过程从教科书中15维姿态误差微分方程推出。
$$\delta {\dot{p}}^n=\delta v^n$$

5.误差传播矩阵

$$\dot{X}=FX+GW$$
令
$$\begin{gathered} F_{\varphi }=C_b^n\ast C_m^b\ast [{\omega }_{nm}^m-{ϵ}_{nm}^m] \\ {F}_{\varphi }=[F_{\varphi }(:,2:3);0_{3\times 1}] \\ {F}_v=-C_b^n\ast C_m^b\ast [f^m-{\nabla }_{nm}^m] \\ {F}_v=[F_v(:,2:3);0_{3\times 1}] \end{gathered}$$

5.1 误差传播矩阵F

$$F=\left[\begin{array}{cccccc}{0}_{3\times 3}& I_{3\times 3}& 0_{3\times 3}& 0_{3\times 3}& 0_{3\times 3}& 0_{3\times 3}\\ 0_{3\times 3}& 0_{3\times 3}& [C_b^n{C}_m^b{f}^m]& C_b^n{C}_m^b& 0_{3\times 3}& F_v\\ 0_{3\times 3}& 0_{3\times 3}& 0_{3\times 3}& 0_{3\times 3}& -C_b^n{C}_m^b& F_{\varphi }\\ 0_{3\times 3}& 0_{3\times 3}& 0_{3\times 3}& 0_{3\times 3}& 0_{3\times 3}& 0_{3\times 3}\\ 0_{3\times 3}& 0_{3\times 3}& 0_{3\times 3}& 0_{3\times 3}& 0_{3\times 3}& 0_{3\times 3}\\ 0_{3\times 3}& 0_{3\times 3}& 0_{3\times 3}& 0_{3\times 3}& 0_{3\times 3}& 0_{3\times 3}\end{array}\right]$$

5.2 噪声分配矩阵G

系统噪声输入
$${\omega }_{noise}=\left[\begin{array}{cccccccccccc}w{a}_{walk\_x}& w{a}_{walk\_y}& w{a}_{walk\_z}& w{g}_{walk\_x}& w{g}_{walk\_y}& w{g}_{walk\_z}& w{a}_{offset\_x}& w{a}_{offset\_y}& w_{offset\_z}& w{g}_{offset\_x}& w{g}_{offset\_y}& w{g}_{offset\_z}\end{array}\right]$$
噪声分配矩阵
$$G=\left[\begin{array}{ccccc}{0}_{3\times 3}& 0_{3\times 3}& 0_{3\times 3}& 0_{3\times 3}& 0_{3\times 1}\\ C_b^n& 0_{3\times 3}& 0_{3\times 3}& 0_{3\times 3}& 0_{3\times 1}\\ 0_{3\times 3}& C_b^n& 0_{3\times 3}& 0_{3\times 3}& 0_{3\times 1}\\ 0_{3\times 3}& 0_{3\times 3}& E_{3\times 3}& 0_{3\times 3}& 0_{3\times 1}\\ 0_{3\times 3}& 0_{3\times 3}& 0_{3\times 3}& E_{3\times 3}& 0_{3\times 1}\\ 0_{3\times 3}& 0_{3\times 3}& 0_{3\times 3}& 0_{3\times 3}& 0_{3\times 1}\end{array}\right]$$

6.误差传播离散化

$$X_{k+1}={\Phi }_{k+1,k}{X}_k+{\Gamma }_{k+1,k}W$$
其中
$$\begin{gathered} {\Phi }_{k+1,k}=1+F\ast T \\ {\Gamma }_{k+1,k}=G\ast T \end{gathered}$$

7.状态更新方程

第一步对IMU测量去零偏
$$\begin{gathered} f_{unbias}^m=f^m-f_{bias} \\ {g}_{unbias}^m=g^m-g_{bias} \end{gathered}$$
第二步对校正安装误差角
$$\begin{gathered} f^b=C_m^b\ast f_{unbias}^m \\ {g}^b=C_m^b\ast g_{unbias}^m \end{gathered}$$
第三步进行状态量更新
$$\begin{gathered} p(k+1)=p(k)+v(k)\ast \Delta T+\frac{1}{2}\ast (C_b^n\ast f^b+g^n)T^2 \\ v(k+1)=v(k)+(C_b^n\ast f^b+g^n)\ast T \\ {C}_{b(k+1)}^n=C_{b(k)}^n\ast C_{b(k+1)}^{b(k)}=C_{b(k)}^n\ast C(\Delta \theta ) \\ {a}_{offset\_k+1}=a_{offset\_k} \\ {w}_{offset\_k+1}=w_{offset\_k} \\ {\alpha }_{k+1}={\alpha }_k \\ {s}_{k+1}=s_k \end{gathered}$$

8. 观测方程

观测量采用序贯处理的办法

8.1 GPS 的观测如下

$$\begin{gathered} \delta p_{obs}=\delta (p_{gps}-p(k))=-E_{3\ast 3}\ast \delta p \\ \delta v_{obs}=\delta (v_{gps}-v(k))=-E_{3\ast 3}\ast \delta v \\ \delta ya{w}_{obs}=\delta (ya{w}_{gps}-yaw(k))=-E_{1\ast 1}\ast {\varphi }_z \end{gathered}$$

8.2 轮速的观测如下

$$\begin{gathered} v_D^b=[s\ast v_{wheel}00{]}^{\prime } \\ {v}_d^b=[v_{wheel}00{]}^{\prime } \\ {v}_D^n=C_b^n\ast v_D^b \end{gathered}$$
因此，可以求得里程计速度的误差表达式。
$$\begin{gathered} \delta v_D^n=\delta C_b^n\ast v_D^b+C_b^n\ast \delta v_D^b \\ \delta v_D^n=C_b^n\ast v_D^b\times \varphi +C_b^n\ast \delta v_D^b \\ \delta v_D^b=[v_{wheel}00{]}^{\prime }\ast \delta s=v_d\ast \delta s \end{gathered}$$
于是得到轮速的观测误差方程如下式。
$$\begin{gathered} \delta v_{obs}=\delta (v_D^n-v(k))=\delta v_D^n-\delta v(k) \\ =C_b^n\ast v_D^b\times \varphi +C_b^n\ast v_d\ast \delta s-\delta v(k) \end{gathered}$$
因此，观测矩阵可以表达为下式。
$$H=\left[\begin{array}{ccccccc}{0}_{3\times 3}& -E_{3\times 3}& [C_b^n\ast v_D^b]& 0_{3\times 3}& 0_{3\times 3}& 0_{3\times 2}& C_b^n\ast v_d\end{array}\right]$$

注1：按照上面观测的方程写法，求状态量更新的时候是X=X_pre-delta_X。如果想使用加法需要加个负号。
注2：由于姿态correct的时候不是简单的加减法，所以此处不要改变失准角对应的观测矩阵项的符号，按照失准角的定义进行correct。
如果想用X=X_pre+delta_X，GPS观测矩阵是单位矩阵E，轮速的观测矩阵如下。
$$H=\left[\begin{array}{ccccccc}{0}_{3\times 3}& E_{3\times 3}& [C_b^n\ast v_D^b]& 0_{3\times 3}& 0_{3\times 3}& 0_{3\times 2}& －C_b^n\ast v_d\end{array}\right]$$
姿态correct都是下式,必须和模型建立时的右扰动定义和失准角定义相同。
$$\begin{gathered} C_b^n=C_{n^{\prime }}^n\ast C_b^{n^{\prime }}=(I+[\varphi ])C_b^{n^{\prime }} \\ {C}_m^b=C_{m^{\prime }}^b\ast C_m^{m^{\prime }}=C_{m^{\prime }}^b\ast (I-[\delta \alpha ]) \end{gathered}$$

7. 杆臂补偿

假设IMU安装在后轴中心，是车体的原点位置。
$P_g$：代表gnss观测在导航系的位置
$v_g$：代表gnss观测在导航系的速度
$C_b^n$: 代表当前车体在导航系的姿态
$L$:代表杆臂向量，gnss天线在车体系下的位置
$P_{car}$：待求的GNSS观测的车体在导航系得位置。
因此位置杆臂补偿如下式。
$$\begin{gathered} P_g=C_b^n\ast L+P_{car} \\ {P}_{car}=P_g-C_b^n\ast L \end{gathered}$$
车体为刚体，天线和车体的转动角速度是一致的，因此速度的杆臂补偿公式如下
$$V_{car}=V_g-C_b^n\ast （\omega \times L）$$
$\omega$为IM U测量的角速度。

8. 完整性约束

轮速观测中已经带入了完整性约束。

9. 零速修正&零航向修正

10. 时间同步

11. 误差标定处理

系统建模时已经带入了安装误差和轮速误差建模。