车辆安装角标定

---

前言

纯IMU解算位置发散太快，IMU/ODO能够一定程度上减缓发散的速度，本质是利用IMU的姿态和里程计的速度进行航位推算，但实际中惯组坐标系与车体坐标系存在安装偏角，必须进行安装角的标定。

一、基本流程图

最初的想法是在直线段标定安装角，IMU可以获得自身姿态，全站仪全程跟踪获得轨迹不同时刻的航向角、俯仰角，然后将两者的数值相减得到安装角。但这样算出来的结果很粗糙，误差很大。最后查找文献，确定了现在的安装角标定方法，基本流程如下图。

二、基本原理

算法部分原理参考陈起金的论文，

CHEN Q, ZHANG Q, NIU X. Estimate the Pitch and Heading Mounting Angles of the IMU for Land Vehicular GNSS/INS Integrated System[J]. IEEE Transactions on Intelligent Transportation Systems, 2021, 22(10): 6503-6515.

论文中有开源代码地址。开源代码仅有估计安装角的部分，双向平滑部分需要自己编写，可以参考PSINS工具箱。

http://www.psins.org.cn/

这里我将估计的维数降为15维。另外两部分开源代码坐标系是不同的，需要进行调整，这里我按照自己的习惯将导航系统一转换到了东北天坐标系。调整部分可以参考这篇博文：

https://zhuanlan.zhihu.com/p/464385688

下面我参考论文给出东北天坐标系下具体公式。

1.安装角的卡尔曼滤波器

待估的状态向量共有9维：

$\delta \mathbf{x}={\left[\begin{array}{cccc}\delta {\mathbf{r}}^n& \alpha & \varphi & \delta k\end{array}\right]}^{\mathrm{T}}$

其中，$\delta {\mathbf{r}}^n={\left[\begin{array}{ccc}\delta r_E& \delta r_N& \delta r_U\end{array}\right]}^{\mathrm{T}}$表示东、北、天方向上的位置误差，单位为米。这里定义为航位推算位置向量${\hat{\mathbf{r}}}^n$与真实位置向量 $r^n$之间的差异。$\varphi ={\left[\begin{array}{ccc}{\varphi }_p& {\varphi }_r& {\varphi }_y\end{array}\right]}^T$表示GNSS/INS姿态解的偏差。其中，${\varphi }_p、{\varphi }_r、{\varphi }_y$分别表示俯仰、横滚和航向角的误差。类似地，$\alpha ={\left[\begin{array}{cc}\delta \Delta \theta & \delta \Delta \psi \end{array}\right]}^T$是IMU安装角度的残差。其实，$\alpha ={\left[\begin{array}{ccc}\delta \Delta \theta & \delta \Delta \gamma & \delta \Delta \psi \end{array}\right]}^T$只是最后发现误差$\delta \Delta \gamma$与无关，去掉了$\delta \Delta \gamma$。 $\delta k$是行进距离测量的比例因子误差。
滤波器的设计需要描述误差状态动态的方程。接下来，将介绍航位推算算法，然后推导并详细讨论误差状态的动态特性。

注：$C_b^n$表示从$b$系到$n$系的旋转矩阵，如果仅存在右上角，表示在某个坐标下的变量，如$v^n$表示$n$系下的速度向量；$\Delta s_k^v$表示$k$时刻$v$系下的里程增量。大体如此。

2.航位推算

航位推算位置可以用一组关于地理纬度φ、经度λ和高度h的微分方程描述。

$\dot{\phi }=\frac{v_N}{R_M+h}$ $\dot{\lambda }=\frac{v_E}{(R_N+h)\cos \phi }$ $\dot{h}=v_U$

其中，$R_M$ 和$R_N$分别指代沿着经度和纬度线的曲率半径。$v_E、v_N、v_U$分别是在$n$系中$v^n$东、北、天方向上速度分量。
在实际应用中，里程计的车辆速度测量参考系为$v$系，然后通过以下转换将其转换为$n$系，

$v^n=C_b^n{C}_v^b{v}^v$

其中，车辆参考速度 $v^v$的第二个元素（y/前向分量）具有非零值$v$,其他元素为零，即，$v^v=[0v0{]}^T$
由里程计测量或由GNSS/INS轨迹导出的增量行驶距离 $\Delta s_k^v$参考系为$v$系，并定义为

$\Delta s_k^v={\int }_{t_{k-1}}^{t_k}{v}^v(t)dt$

其中，$t_{k-1}$和$t_k$表示离散时间戳。$\Delta s_k^v$的第二个元素（y分量）$∆s$ 也具有非零值,其他元素为零；即，$\Delta s_k^v=[0∆s0{]}^T$
航位推算位置在通过数值积分使用地理速度分量进行更新，

$r_k^n=r_{k-1}^n+{\int }_{t_{k-1}}^{t_k}{C}_b^n(t)C_v^b(t)v^v(t)dt$

略去一些参数介绍…

$r_k^n=r_{k-1}^n+C_b^n(t_{k-1})C_v^b{\int }_{t_{k-1}}^{t_k}{v}^v(t)dt=r_{k-1}^n+C_b^n(t_{k-1})C_v^b\Delta s_k^v$

略去一些参数介绍…

$\Delta s_k^n=C_b^n(t_{k-1})C_v^b\Delta s_k^v$

其中，$\Delta s_k^n={\left[\begin{array}{c}\Delta s_{E,k}\Delta s_{N,k}\Delta s_{U,k}\end{array}\right]}^T$

${\varphi }_k={\varphi }_{k-1}+\frac{\Delta s_{N,k}}{R_M+h}$

${\lambda }_k={\lambda }_{k-1}+\frac{\Delta s_{E,k}}{(R_N+h)\cos \varphi }$

$h_k=h_{k-1}+\Delta s_{U,k}$

接下来，推导航位推算位置误差的离散时间传播模型。
航位推算与初始位置误差或前一时段的误差、姿态误差、补偿后的安装角度的残余误差以及距离测量误差有关。因此，航位推算的位置表示为，

${\hat{r}}_k^n={\hat{r}}_{k-1}^n+{\hat{C}}_{b,k-1}^n{\hat{C}}_v^b\Delta {\hat{s}}_k^v$

车辆增量距离测量误差被假设为由比例因子误差$\delta k$引起，

$\Delta {\hat{s}}^v=(1+{\delta }_k)\Delta s^v=\Delta s^v+\Delta s^v{\delta }_k$

结合上两式，并考虑到
$\delta r^n={\hat{r}}^n-r^n$

${\hat{C}}_b^n=[I-(\varphi \times )]C_b^n$

${\hat{C}}_b^v=[I-(\alpha \times )]C_b^v$
可得

$\delta r_k^n=\delta r_{k-1}^n-C_b^n{C}_v^b(\Delta s^v\times \alpha )+\Delta s^n\times \varphi +\Delta s^n{\delta }_k$

$\Delta s^v\times \alpha =\left[\begin{array}{ccc}0& 0& \Delta s\\ 0& 0& 0\\ -\Delta s& 0& 0\end{array}\right]\left[\begin{array}{c}\delta \Delta \theta \\ \delta \Delta \gamma \\ \delta \Delta \psi \end{array}\right]=\Delta s\left[\begin{array}{c}\delta \Delta \psi \\ 0\\ -\delta \Delta \theta \end{array}\right]$

上述方程表明位置误差与$\delta \Delta \gamma$无关,整理一下：

$\delta r_k^n=\delta r_{k-1}^n-C_b^n{C}_v^{b}M\left[\begin{array}{c}\delta \Delta \theta \\ \delta \Delta \psi \end{array}\right]+\Delta s^n\times \varphi +\Delta s^n{\delta }_k$
$M=\left[\begin{array}{cc}0& \Delta s\\ 0& 0\\ -\Delta s& 0\end{array}\right]$

只要以一定的精度观测到航位推算定位误差，就可以估计俯仰和航向安装角的残差

3.状态模型

在离散时间下，安装角度估计滤波器的系统模型,

$\delta x_k={\Phi }_{k/k-1}\delta x_{k-1}+G_{k-1}{w}_{k-1}$

其中，$w_{k-1}$表示状态转移矩阵，$\Phi$是状态转移矩阵，$G$是离散时间系统噪声分布矩阵，$w$ 是系统噪声向量。

${\Phi }_{k/k-1}=\left[\begin{array}{cccc}{I}_{3\times 3}& -C_b^n{C}_v^{b}M& (\Delta s^n\times )& \Delta s^n\\ 0& I_{2\times 2}& 0& 0\\ 0& 0& I_{3\times 3}& 0\\ 0& 0& 0& 1\end{array}\right]$

$G_{k-1}=\left[\begin{array}{c}{0}_{3\times 6}\\ I_{6\times 6}\end{array}\right]$

$w_{k-1}={\left[\begin{array}{cccccc}{w}_{\Delta \theta }& w_{\Delta \psi }& w_{\theta }& w_{\varphi }& w_{\psi }& w_{SF}\end{array}\right]}^T$

变量说明参考上文，写公式真累……

4.量测模型

KF滤波器的量测是航位推算位置和GNSS/INS位置之间的差异

实在懒得打公式了，大家凑合看吧。

三、结果

取了某次的实测数据，往返采集了两次数据。
第一次

第二次

结果基本吻合。

总结

希望对大家有所帮助，有所纰漏在所难免，望斧正。
ps：矩阵不清楚怎么加粗，并未与普通变量区分，注意辨识。