SINS/DR组合导航(一)
1.1 关于SINS/DR组合导航
SINS/DR组合导航是一种很常见的，也是很传统的一种组合导航方式，这里的DR叫做航位推算，定义是利用姿态、航向和行驶里程信息来推算机器人相对于起始点的相对位置。对于移动机器人来说，里程计可以是车轮里程计，或者视觉里程计；对于空中机器人来说，像对于军用直升机来说，可能是多普勒雷达；对于水下机器人来说，用的最多的就是多普勒计程仪（DVL）了。所以下文我采用一种广义上的里程计定义，这种里程计能够测量得到载体坐标系上的三维速度信息。
这里我主要基于严恭敏老师提出的轨迹相似性原理去补偿SINS/DR组合导航中的若干主要误差，像SINS和里程计之间的安误差，DVL刻度因子等误差。在轨迹相似性原理的推导过程中，能看出该算法的适用条件，然后会给出轨迹相似性原理的工程应用，最后的组合导航效果。
1.2 适用场景
下文要讨论的SINS/DR组合导航算法适用的场景主要特点如下：
1.俯仰，滚转角变化较小，或者可以理解为机器人近似在一个二维平面内运动，像无人船，或者仓库AGV,这一假设是完全成立的；
2.使用低成本惯导：像无人车，低速移动机器人使用的惯导，从量产角度考虑，用的也是不到万元的MEMS惯导产品。
1.3 SINS/DR组合导航主要误差项
这里可以先摆上些结论，对整个系统的理解很有帮助，就是以上场景下的SINS/DR组合导航，从速度误差方程和位置误差方程可以分析得到，惯导和里程计之间的偏航角，以及DVL刻度因子对组合导航精度影响较大，惯导和里程计之间的俯仰角主要影响高度漂移，在二维状态下可以忽略，惯导和里程计之间的滚转角不影响组合导航精度。
所以能够在线估计出这些主要的误差项，或者能够准确标定出这些误差项，对于SINS/DR组合导航精度有很大的提升。在线估计的方法主要是基于卡尔曼滤波，优点是可以实现一种自适应，即使参数发生变化，也不受影响，缺点算法复杂，估计精度和惯导的精度有关；标定方法的优点是利用一些优化算法可以实现比较精准的标定，缺点是当参数变化时，例如惯导的拆装，参数要重新标定，不太方便。
1.4 轨迹相似性原理
1.4.1 航位推算速度误差方程
设惯导坐标系为 b b b系，车体坐标系为 m m m系，假设从 m m m到 b b b系存在小量的安装偏差角，绕机器人 X X X轴 o x m o x_{m} oxm、 Y Y Y轴 o y m o y_{m} oym及 Z Z Z轴 o z m o z_{m} ozm分别存在俯仰偏角 α θ \alpha_{\theta} αθ，滚动偏角 α γ \alpha_{\gamma} αγ和方位偏角 α ψ \alpha_{\psi} αψ，机器人 X , Y , Z X, Y, Z X,Y,Z轴分别对应右、前，上。记偏差角矢量 α = [ α θ α γ α ψ ] T \alpha=\left[\begin{array}{lll} \alpha_{\theta} & \alpha_{\gamma} & \alpha_{\psi} \end{array}\right]^{\mathrm{T}} α=[αθαγαψ]T，可得到如下公式：
C b m = I + ( α × ) = [ 1 − α ψ α γ α ψ 1 − α θ − α γ α θ 1 ] C_{b}^{m}=I+(\alpha \times)=\left[\begin{array}{ccc} 1 & -\alpha_{\psi} & \alpha_{\gamma} \\ \alpha_{\psi} & 1 & -\alpha_{\theta} \\ -\alpha_{\gamma} & \alpha_{\theta} & 1 \end{array}\right] Cbm=I+(α×)=⎣⎡1αψ−αγ−αψ1αθαγ−αθ1⎦⎤
另外，里程计的测量中还可能存在刻度系数误差 δ K D \delta K_{D} δKD，其输出速度大小 v ~ D \tilde{v}_{D} v~D与理论速度大小 U D \mathcal{U}_{D} UD之间的关系为：
v ~ D m = ( 1 + δ K D ) v D m \tilde{v}_{D}^{m}=\left(1+\delta K_{D}\right) v_{D}^{m} v~Dm=(1+δKD)vDm
所以，在导航坐标系中里程计的实际速度输出应该为：
v ~ D n = C ~ b n ( C b m ) T v ~ D m = ( I − ϕ D × ) C b n ( I − α × ) ( 1 + δ K D ) v D m ≈ \tilde{v}_{D}^{n}=\tilde{C}_{b}^{n}\left(C_{b}^{m}\right)^{\mathrm{T}} \tilde{v}_{D}^{m}=\left(I-\phi_{D} \times\right) C_{b}^{n}(I-\alpha \times)\left(1+\delta K_{D}\right) v_{D}^{m} \approx v~Dn=C~bn(Cbm)Tv~Dm=(I−ϕD×)Cbn(I−α×)(1+δKD)vDm≈
v D n − ( ϕ D × ) C b n v D m − C b n ( α × ) v D m + C b n δ K D v D m = v_{D}^{n}-\left(\phi_{D} \times\right) C_{b}^{n} v_{D}^{m}-C_{b}^{n}(\alpha \times) v_{D}^{m}+C_{b}^{n} \delta K_{D} v_{D}^{m}= vDn−(ϕD×)CbnvDm−Cbn(α×)vDm+CbnδKDvDm=
v D n + v D n × ϕ D + C b n ( v D m × ) α + C b n v D m δ K D v_{D}^{n}+v_{D}^{n} \times \phi_{D}+C_{b}^{n}\left(v_{D}^{m} \times\right) \alpha+C_{b}^{n} v_{D}^{m} \delta K_{D} vDn+vDn×ϕD+Cbn(vDm×)α+CbnvDmδKD
其中， ϕ D \phi_{D} ϕD是航位推算的姿态失准角，可以理解为姿态偏差角，这里的里程计是移动机器人中的车轮里程计，即车体 Y Y Y轴方向的速度 U D \mathcal{U}_{D} UD， C b n = C i j ( i , j = 1 , 2 , 3 ) C_{b}^{n}=C_{i j}(i, j=1,2,3) Cbn=Cij(i,j=1,2,3)，将 v D m = [ 0 v D 0 ] T v_{D}^{m}=\left[\begin{array}{lll} 0 & v_{D} & 0 \end{array}\right]^{\mathrm{T}} vDm=[0vD0]T带入上式得：
v ~ D n = v D n + v D n × ϕ D + [ C 11 C 12 C 13 C 21 C 22 C 23 C 31 C 32 C 33 ] [ 0 0 v D 0 0 0 − v D 0 0 ] α + \tilde{v}_{D}^{n}=v_{D}^{n}+v_{D}^{n} \times \phi_{D}+\left[\begin{array}{ccc} C_{11} & C_{12} & C_{13} \\ C_{21} & C_{22} & C_{23} \\ C_{31} & C_{32} & C_{33} \end{array}\right]\left[\begin{array}{ccc} 0 & 0 & v_{D} \\ 0 & 0 & 0 \\ -v_{D} & 0 & 0 \end{array}\right] \alpha+ v~Dn=vDn+vDn×ϕD+⎣⎡C11C21C31C12C22C32C13C23C33⎦⎤⎣⎡00−vD000vD00⎦⎤α+
[ C 11 C 12 C 13 C 21 C 22 C 23 C 31 C 32 C 33 ] [ 0 v D 0 ] δ K D \left[\begin{array}{ccc} C_{11} & C_{12} & C_{13} \\ C_{21} & C_{22} & C_{23} \\ C_{31} & C_{32} & C_{33} \end{array}\right]\left[\begin{array}{c} 0 \\ v_{D} \\ 0 \end{array}\right] \delta K_{D} ⎣⎡C11C21C31C12C22C32C13C23C33⎦⎤⎣⎡0vD0⎦⎤δKD
v D n + v D n × ϕ D + v D [ − C 13 0 C 11 − C 23 0 C 21 − C 33 0 C 31 ] α + v D [ C 12 C 22 C 32 ] δ K D v_{D}^{n}+v_{D}^{n} \times \phi_{D}+v_{D}\left[\begin{array}{ccc} -C_{13} & 0 & C_{11} \\ -C_{23} & 0 & C_{21} \\ -C_{33} & 0 & C_{31} \end{array}\right] \alpha+v_{D}\left[\begin{array}{c} C_{12} \\ C_{22} \\ C_{32} \end{array}\right] \delta K_{D} vDn+vDn×ϕD+vD⎣⎡−C13−C23−C33000C11C21C31⎦⎤α+vD⎣⎡C12C22C32⎦⎤δKD
从上式可以看出，滚动偏角 α γ \alpha_{\gamma} αγ不影响里程计的速度测量值。
1.4.2 轨迹相似性原理
假设在载车行驶过程中，一般水平姿态角都比较小（这个假设很重要），近似有：
C b n ≈ [ cos ψ − sin ψ 0 sin ψ cos ψ 0 0 0 1 ] C_{b}^{n} \approx\left[\begin{array}{ccc} \cos \psi & -\sin \psi & 0 \\ \sin \psi & \cos \psi & 0 \\ 0 & 0 & 1 \end{array}\right] Cbn≈⎣⎡cosψsinψ0−sinψcosψ0001⎦⎤
容易得到如下公式，该公式在后面会用到：
v D n × u U = ( C b n v D m ) × u U ≈ v_{D}^{n} \times u_{U}=\left(C_{b}^{n} v_{D}^{m}\right) \times u_{U} \approx vDn×uU=(CbnvDm)×uU≈
[ cos ψ − sin ψ 0 sin ψ cos ψ 0 0 0 1 ] [ 0 v D 0 ] × [ 0 0 1 ] = v D [ − sin ψ cos ψ 0 ] × [ 0 0 1 ] = v D [ cos ψ sin ψ 0 ] \left[\begin{array}{ccc} \cos \psi & -\sin \psi & 0 \\ \sin \psi & \cos \psi & 0 \\ 0 & 0 & 1 \end{array}\right]\left[\begin{array}{c} 0 \\ v_{D} \\ 0 \end{array}\right] \times\left[\begin{array}{c} 0 \\ 0 \\ 1 \end{array}\right]=v_{D}\left[\begin{array}{c} -\sin \psi \\ \cos \psi \\ 0 \end{array}\right] \times\left[\begin{array}{c} 0 \\ 0 \\ 1 \end{array}\right]=v_{D}\left[\begin{array}{c} \cos \psi \\ \sin \psi \\ 0 \end{array}\right] ⎣⎡cosψsinψ0−sinψcosψ0001⎦⎤⎣⎡0vD0⎦⎤×⎣⎡001⎦⎤=vD⎣⎡−sinψcosψ0⎦⎤×⎣⎡001⎦⎤=vD⎣⎡cosψsinψ0⎦⎤
上公式中 u U = [ 0 0 1 ] T u_{U}=\left[\begin{array}{lll} 0 & 0 & 1 \end{array}\right]^{\mathrm{T}} uU=[001]T为天向单位矢量。
在姿态误差矢量 ϕ D = [ ϕ D E ϕ D N ϕ D U ] T \phi_{D}=\left[\begin{array}{lll} \phi_{D E} & \phi_{D N} & \phi_{D U} \end{array}\right]^{\mathrm{T}} ϕD=[ϕDEϕDNϕDU]T中，若忽略水平姿态误差影响，即近似 ϕ D E ≈ ϕ D N ≈ 0 \phi_{D E} \approx \phi_{D N} \approx 0 ϕDE≈ϕDN≈0，根据速度误差方程推导出来的 v ~ D n \tilde{v}_{D}^{n} v~Dn可得：
v ~ D n = v D n + v D n × [ 0 0 ϕ D U ] + v D [ 0 0 cos ψ 0 0 sin ψ − 1 0 0 ] [ α θ α γ α ψ ] + v D n δ K D = \tilde{v}_{D}^{n}=v_{D}^{n}+v_{D}^{n} \times\left[\begin{array}{c} 0 \\ 0 \\ \phi_{D U} \end{array}\right]+v_{D}\left[\begin{array}{ccc} 0 & 0 & \cos \psi \\ 0 & 0 & \sin \psi \\ -1 & 0 & 0 \end{array}\right]\left[\begin{array}{c} \alpha_{\theta} \\ \alpha_{\gamma} \\ \alpha_{\psi} \end{array}\right]+v_{D}^{n} \delta K_{D}= v~Dn=vDn+vDn×⎣⎡00ϕDU⎦⎤+vD⎣⎡00−1000cosψsinψ0⎦⎤⎣⎡αθαγαψ⎦⎤+vDnδKD=
v D n + ϕ D U v D n × [ 0 0 1 ] + α ψ v D [ cos ψ sin ψ 0 ] − α θ v D [ 0 0 1 ] + v D n δ K D = v_{D}^{n}+\phi_{D U} v_{D}^{n} \times\left[\begin{array}{l} 0 \\ 0 \\ 1 \end{array}\right]+\alpha_{\psi} v_{D}\left[\begin{array}{c} \cos \psi \\ \sin \psi \\ 0 \end{array}\right]-\alpha_{\theta} v_{D}\left[\begin{array}{l} 0 \\ 0 \\ 1 \end{array}\right]+v_{D}^{n} \delta K_{D}= vDn+ϕDUvDn×⎣⎡001⎦⎤+αψvD⎣⎡cosψsinψ0⎦⎤−αθvD⎣⎡001⎦⎤+vDnδKD=
v D n + ϕ D U v D n × u U + α ψ v D n × u U − α θ v D u U + v D n δ K D = v_{D}^{n}+\phi_{D U} v_{D}^{n} \times u_{U}+\alpha_{\psi} v_{D}^{n} \times u_{U}-\alpha_{\theta} v_{D} u_{U}+v_{D}^{n} \delta K_{D}= vDn+ϕDUvDn×uU+αψvDn×uU−αθvDuU+vDnδKD=
[ I − ( ϕ D U + α ψ ) u U × ] v D n + δ K D v D n − α θ v D u U ≈ \left[I-\left(\phi_{D U}+\alpha_{\psi}\right) u_{U} \times\right] v_{D}^{n}+\delta K_{D} v_{D}^{n}-\alpha_{\theta} v_{D} u_{U} \approx [I−(ϕDU+αψ)uU×]vDn+δKDvDn−αθvDuU≈
( 1 + δ K D ) [ I − ( ϕ D U + α ψ ) u U × ] v D n − α θ v D u U \left(1+\delta K_{D}\right)\left[I-\left(\phi_{D U}+\alpha_{\psi}\right) u_{U} \times\right] v_{D}^{n}-\alpha_{\theta} v_{D} u_{U} (1+δKD)[I−(ϕDU+αψ)uU×]vDn−αθvDuU
假设 α θ , ϕ D U + α ψ , δ K D \alpha_{\theta}, \phi_{D U}+\alpha_{\psi}, \delta K_{D} αθ,ϕDU+αψ,δKD，且载车在地理位置变化不大的范围内行驶，即整个导航过程汇总导航坐标系的旋转变化不大，可近似当成平面看待，将上式积分可以得到位置方程，即为：
S ~ D n = ( 1 + δ K D ) [ I − ( ϕ D U + α ψ ) u U × ] S D n − α θ S D u U \tilde{S}_{D}^{n}=\left(1+\delta K_{D}\right)\left[I-\left(\phi_{D U}+\alpha_{\psi}\right) u_{U} \times\right] S_{D}^{n}-\alpha_{\theta} S_{D} u_{U} S~Dn=(1+δKD)[I−(ϕDU+αψ)uU×]SDn−αθSDuU
其中 S D n = ∫ 0 T v D n d t , S ~ D n = ∫ 0 T v ~ D n d t , S D = ∫ 0 T v D d t S_{D}^{n}=\int_{0}^{\mathrm{T}} v_{D}^{n} d t, \tilde{S}_{D}^{n}=\int_{0}^{\mathrm{T}} \tilde{v}_{D}^{n} d t, S_{D}=\int_{0}^{\mathrm{T}} v_{D} d t SDn=∫0TvDndt,S~Dn=∫0Tv~Dndt,SD=∫0TvDdt，分别表示在时间段 [ 0 , T ] [0, T] [0,T]内的载车真实位移矢量、计算位移矢量和行驶里程。
上公式可以分解为水平和垂直量部分，可得：
S ~ D H n = ( 1 + δ K D ) [ I − ( ϕ D U + α ψ ) u U × ] S D H n \tilde{S}_{D H}^{n}=\left(1+\delta K_{D}\right)\left[I-\left(\phi_{D U}+\alpha_{\psi}\right) u_{U} \times\right] S_{D H}^{n} S~DHn=(1+δKD)[I−(ϕDU+αψ)uU×]SDHn
S ~ D U = ( 1 + δ K D ) S D U − α θ S D \tilde{S}_{D U}=\left(1+\delta K_{D}\right) S_{D U}-\alpha_{\theta} S_{D} S~DU=(1+δKD)SDU−αθSD
记 S ~ D n = [ S ~ D E S ~ D N S ~ D U ] T , S D n = [ S D E S D N S D U ] T , S ~ D H n = [ S ~ D E S ~ D N 0 ] T \tilde{S}_{D}^{n}=\left[\tilde{S}_{D E} \quad \tilde{S}_{D N} \quad \tilde{S}_{D U}\right]^{\mathrm{T}}, S_{D}^{n}=\left[S_{D E} \quad S_{D N} \quad S_{D U}\right]^{\mathrm{T}}, \tilde{S}_{D H}^{n}=\left[\tilde{S}_{D E} \quad \tilde{S}_{D N} \quad 0\right]^{\mathrm{T}} S~Dn=[S~DES~DNS~DU]T,SDn=[SDESDNSDU]T,S~DHn=[S~DES~DN0]T， S D H n = [ S D E S D N 0 ] T S_{D H}^{n}=\left[\begin{array}{lll} S_{D E} & S_{D N} & 0 \end{array}\right]^{\mathrm{T}} SDHn=[SDESDN0]T，右下标字符中的“H”表示在水平面的投影分量。
如下图所示，假设载车从A点沿某线路行驶一圈又回到A点。在行驶线路上任取一点B。图示 S D H n = A B → S_{D H}^{n}=\overrightarrow{A B} SDHn=AB为水平面上的真实位移， S ~ D H n = A C → \tilde{S}_{D H}^{n}=\overrightarrow{A C} S~DHn=AC，为相应的计算位移，若做辅助线段BD使 B D ⊥ A C B D \perp A C BD⊥AC，上面公式的几何含义是：真实位移 S D H n S_{D H}^{n} SDHn绕天向轴 u U u_{U} uU转动角度 ( ϕ D U + α ψ ) \left(\phi_{D U}+\alpha_{\psi}\right) (ϕDU+αψ)得到 A D → \overrightarrow{A D} AD，再扩大 ( 1 + δ K D ) \left(1+\delta K_{D}\right) (1+δKD)倍，得计算位移 S ~ D H n \tilde{S}_{D H}^{n} S~DHn，。由于在行驶路线上的每一点都满足以上几何规律，因此，导航解算路线和真实路线是几何相似的，即：以起始点A为中心点，解算路线在整体上转动了 ( ϕ D U + α ψ ) \left(\phi_{D U}+\alpha_{\psi}\right) (ϕDU+αψ)，角度并扩大了 ( 1 + δ K D ) \left(1+\delta K_{D}\right) (1+δKD)倍，这一特点称为航位推算轨迹与真实轨迹相似性原理。从图中还可以看出， ( ϕ D U + α ψ ) \left(\phi_{D U}+\alpha_{\psi}\right) (ϕDU+αψ)将引起垂直于位移方向的误差 B D → \overrightarrow{B D} BD，而 δ K D \delta K_{D} δKD会引起沿着位移方向的误差 D C → \overrightarrow{D C} DC，这两项误差总和为 B C → \overrightarrow{B C} BC，显然，载车行驶距起点越远误差越大，但是在返回起始点过程中，误差又会逐渐减少甚至消失。
航位推算的高度误差如下：
δ S D U = S ~ D U − S D U = δ K D S D U − α θ S D \delta S_{D U}=\tilde{S}_{D U}-S_{D U}=\delta K_{D} S_{D U}-\alpha_{\theta} S_{D} δSDU=S~DU−SDU=δKDSDU−αθSD
载车在行驶过程中，一般情况下行驶里程远大于其高度变化，即有 S D ≫ S D U S_{D} \gg S_{D U} SD≫SDU，当里程计刻度系数误差 δ K D \delta K_{D} δKD较小时，近似有：
δ S D U ≈ − α θ S D \delta S_{D U} \approx-\alpha_{\theta} S_{D} δSDU≈−αθSD
这表明，航位推算的高度误差和俯仰安装误差角及行驶里程成正比，不论行驶路线如何，随着行驶里程增加，高度误差都会不断积累。
最后总结一下，如果不关注高度通道，组合导航轨迹和真实轨迹之间的偏航角是由于惯导器件偏航角误差和里程计和惯导之间的安装偏航角误差两方面引起的，由于这两个偏航角误差是耦合在一起的，所以标定出来的偏航角实际是两个误差累积结果，假定能通过其他观测数据相对精确辨识出其中一个，另外一个也就能解出来。
1.5 SINS/DR组合导航中的参数标定
以上场景下的SINS/DR组合导航，可以采用纯航位推算，并且标定安装误差和里程计刻度因子的方法。我使用的惯导是高精度光纤惯导，按照经验里程计和惯导之间的偏航安装角在1°以上，高精度光纤惯导的方位角误差和偏航安装角相比较小，可忽略，所以可以认为标定出来的偏航角误差即为它们之间的安装偏航角。因为不关注高度数据，所以需要标定的数据就只有两个，即里程计和SINS之间的安装偏航角和里程计的刻度因子。
参数标定过程类似加速度计误差的标定，常用非线性优化算法，例如高斯牛顿方法，LM算法等。优化函数采用平方根形式的导航定位精度，真值使用GPS数据，GPS的动态定位精度大约2m左右。
下图是导航精度和两个待标定参数的变化规律，从图中可以看出只有一个极小值，极小值点的导航精度大约0.2%左右，由于不存在多个极值的情况，另外梯度方向比较清晰，所以对于标定来说是有利的。
下图是没有标定误差的航位推算结果，计算出来的定位精度为2.31%，从图中可以看出很明显的两条轨迹之间的角度偏差。
下图是通过非线性优化方法标定误差之后的航位推算结果，从图中可以看出很明显两条轨迹几乎重合，标定出来的误差如下：安装偏航角为1.44°，DVL刻度因子为0.995，从中也可以看出，两个误差中安装误差角所占比重较大。
然后按照这个已经标定好的两个误差参数，测试如下轨迹，没有经过误差标定的轨迹和经过误差标定的轨迹如下所示,其中经过误差标定的轨迹的定位精度为0.27%。
参考文献
- 车载自主定位定向系统研究（博士论文） 严恭敏
- 捷联惯导算法与组合导航原理 严恭敏等编著
- A robust and easy to implement method for IMU calibration without external equipments
- 惯性导航精度评定方法 GJB 729-89