INS/GNSS组合导航（四）-INS误差模型

INS一个最大的特点就是其误差会随时间累计而导致误差漂移，因此与GNSS组合可以有效的对INS误差进行修正。INS的误差建模就非常重要，包括姿态误差、速度误差和位置误差等等。除此之外，还包括加速度计和陀螺零偏误差。

1 姿态误差

姿态误差存在的原因就是在车辆运动过程中，实际的导航坐标系是在不断的变化的，但是在INS进行采样这段时间内，并没有将采样期间的导航坐标系的移动考虑进去，因此导致了计算导航坐标系与实际导航坐标系在空间上产生的不对准。即会在角速度向的导航坐标系投影出现偏差。除此之外，IMU本身也存在测量误差，从而导致姿态角的计算误差。这两部分误差共同构成了姿态计算的误差。

常见的姿态误差方程包括phi-角误差和psi-角误差。对于小角度误差近似两者是等价的，其中psi-角误差模型基于假定计算机坐标系进行推导，形式简单，在现代INS多传感器组合定位中应用广泛。因此这里也给出的是psi-角误差方程。

这里不加证明的给出平台失准角微分方程：
$$\dot{\psi }=\delta {\omega }_{ie}^n+\delta {\omega }_{en}^n-({\omega }_{ie}^n+{\omega }_{en}^n)\times \psi +b_g$$
上式以经反映出了平台失准角的组成，前三项主要是由于计算的导航坐标系与实际的导航坐标系不对准造成的，最后一项是陀螺仪在运行过程中的零偏量，属于IMU自身的测量误差。

上节以经给出了$\delta {\omega }_{ie}^n$和$\delta {\omega }_{en}^n$的具体计算公式：
$$\delta {\omega }_{ie}^n=\left[\begin{array}{c}-{\omega }_{ie}\cdot sinL\\ 0\\ -{\omega }_{ie}\cdot cosL\end{array}\right]\delta L$$
$$\delta {\omega }_{en}^n=\left[\begin{array}{c}\frac{\delta v_y^n}{R_E+h}\\ -\frac{\delta v_x^n}{R_N+h}\\ -\frac{\delta v_y^{n}tanL}{R_E+h}-\frac{v_y^n}{R_E+h}se{c}^{e}L\cdot \delta L\end{array}\right]$$

将上两式带入平台失准角误差方程中得出：
$${\dot{\psi }}_x=-({\omega }_{ie}\cdot sinL+\frac{v_y^{n}tanL}{R_E+h}){\psi }_y+\frac{v_x^n}{R_N+h}{\psi }_z+\frac{1}{R_E+h}\delta v_y^n-{\omega }_{ie}sinL\cdot \delta L+b_{g,x}^n$$
$${\dot{\psi }}_y=({\omega }_{ie}\cdot sinL+\frac{v_y^{n}tanL}{R_E+h}){\psi }_x+({\omega }_{ie}\cdot cosL+\frac{v_y^n}{R_E+h}){\psi }_z-\frac{1}{R_N+h}\delta v_x^n+b_{g,y}^n$$
$${\dot{\psi }}_z=-\frac{v_x^n}{R_N+h}{\psi }_x-({\omega }_{ie}\cdot cosL+\frac{v_y^n}{R_E+h}){\psi }_y-\frac{tanL}{R_E+h}\delta v_y^n-({\omega }_{ie}cosL+\frac{v_y^n}{R_E+h}se{c}^{2}L)\delta L+b_{g,z}^n$$

一般来讲，平台失准角是个非常小的角度，如果用坐标变换矩阵和欧拉角来描述一个小角度的扰动时，把计算导航坐标系记作p系，把理想的导航坐标系记作n系，考虑到三角函数中对于小角度的近似关系，那么p系到n系的变换可以表示为：
$$C_p^n=\left[\begin{array}{ccc}1& {\psi }_z& -{\psi }_y\\ -{\psi }_z& 1& {\psi }_x\\ {\psi }_y& -{\psi }_x& 1\end{array}\right]=I+[\psi \times ]$$
根据上式。即可对姿态进行修正。

2 速度和位置误差

姿态误差的存在使得计算导航坐标系和实际导航坐标系产生偏离，这会引起加速度的不正确投影，加速度计的飘移会导致加速度测量值的误差， 这些误差的存在都会导致速度误差。速度的误差方程可以表示如下：
$$\delta {\dot{v}}^n=\delta f^n+\delta g-(2\delta {\omega }_{ie}^n+\delta {\omega }_{en}^n)\times v^n-(2{\omega }_{ie}^n+{\omega }_{en}^n)\times \delta v^n$$
坐标系偏离和加速度计偏移导致了上式中$\delta f^n$的存在。事实上，在车辆快速运动时，重力加速度也会发生变化，但是对于小范围运动的载体来说，重力加速度的偏差可以忽略。上式中。$\delta f^n$可以表示为：
$$\delta f^n=f^p-f^n+b_a$$
$$=C_n^p\delta f^n-f^n+b_a$$
$$=-\psi \times f^n+b_a$$
$$=f^n\times \psi +b_a$$
将$\delta f^n$代入速度误差方程，可以得到：
$$\delta {\dot{v}}^n=f^n\times \psi -(2\delta {\omega }_{ie}^n+\delta {\omega }_{en}^n)\times v^n-(2{\omega }_{ie}^n+{\omega }_{en}^n)\times \delta v^n+b_a$$
将已知量代入可得：
$$\delta {\dot{v}}_x^n=-f_z^n\cdot {\psi }_y+f_y^n\cdot {\psi }_z+\frac{v_z^n}{R_N+h}\delta v_x^n-(2{\omega }_{ie}\cdot sinL+2\frac{v_y^{n}tanL}{R_E+h})\delta v_y^n+\frac{v_x^n}{R_N+h}\delta v_z^n-(2{\omega }_{ie}{v}_y^{n}cosL+\frac{v^{n^2}}{R_E+h}se{c}^{2}L)\delta L+b_{\omega ,x}$$
$$\delta {\dot{v}}_y^n=f_z^n\cdot {\psi }_x-f_x^n\cdot {\psi }_z+(2{\omega }_{ie}\cdot sinL+\frac{v_y^{n}tanL}{R_E+h})\delta v_x^n+(\frac{v_x^{n}tanL}{R_E+h}+\frac{v_z^n}{R_E+h})\delta v_y^n+(2{\omega }_{ie}\cdot cosL+\frac{v_y^n}{R_E+h})\delta v_z^n+(2\omega ie{v}_{eb,x}^{n}cosL-2{\omega }_{ie}{v}_z^{n}sinL+\frac{v_x^n{v}_y^n}{R_E+h}se{c}^{2}L)\delta L+b_{a,y}$$
$$\delta {\dot{v}}_z^n=-f_y^n\cdot {\psi }_x+f_x^n\cdot {\psi }_y-\frac{2v_x^n}{R_N+h}\delta v_x^n-2({\omega }_{ie}\cdot cosL+\frac{v_y^n}{R_E+h})\delta v_y^n+2{\omega }_{ie}{v}_y^{n}sinL\cdot \delta L+b_{a,z}$$
速度误差微分方程反映了速度误差的变化情况，可以看出，速度微分误差微分方程是十分复杂的。位置误差微分方程则比较简单：
$$\delta \dot{L}=\frac{\delta v_x^n}{R_N+h}$$
$$\delta \dot{\lambda }=\frac{\delta v_y^n}{(R_E+h)cosL}+\frac{v_y^{n}sinL\delta L}{(R_E+h)co{s}^{2}L}$$
上式即为位置误差方程。对其得到的位置误差进行反馈就能得到正确的位置信息。