0 引　言

激光惯导（Inertial Navigation System，INS）主要由3个激光陀螺和3个加速度计构成，目前已广泛应用于舰船、飞机以及汽车等运载体。受导航原理的限制，激光惯导的位置误差会随着惯性器件误差的时间积累而迅速发散^[1]。激光惯导/测速仪组合导航系统可以通过测速设备对惯性器件误差进行实时校正，具有不依赖外界信号，能够自主导航定位的特点，通常作为卫导不可用情况下军舰、小型舰艇、水下AUV等运载体的主要导航系统。

根据测量原理的不同，常用的测速设备主要包括里程计^[2]（Odometer, ODO）、激光多普勒测速仪^[3-4]（Laser Doppler Velocimeter, LDV）、多普勒测速仪^[5]（DVL）以及电磁里程仪^[6]（Electromagnetic Log, EML）等。其中，前两种测速设备主要用于车载环境，测速设备的速度输出为对地速度。而后两种测速设备主要用于船载环境，DVL根据探测距离不同可以输出对底速度和对水速度。电磁里程仪受限于测量原理，仅能输出对水速度。因此，激光惯导/里程仪导航系统需要克服水速对导航结果的影响，才能实现高精度导航定位。而洋流速度在时间、空间上会发生变化，可以在相关海域对洋流参数进行提前测绘，并通过预置模型从而对洋流速度进行补偿^[7-9]。

激光惯导与测速设备之间的标定是进行导航解算的基础。文献[2-4]针对陆用组合导航系统中的一维、二维激光多普勒测速仪以及里程计的标定问题进行了研究。标定过程中利用差分GPS的位置、速度作为观测量，对LDV的比例因子、安装误差等参数进行标定。车载试验结果表明相关参数标定精度较高，能有效保障自主导航精度。文献[10-11]设计了一种分立式的海上测速仪参数标定方法，通过设计AUV的轨迹实现对安装误差、比例因子的精确标定，而对标定过程中出现的测向流情况，通过不断调整AUV的艏向，使AUV能够沿着既定路线完成标定，从而降低测向流对标定结果的影响。

标定工作也可以在导航Kalman滤波中完成。文献[12-13]基于可观测度反馈设计了组合导航算法，将DVL的刻度因子、安装误差角以及杆臂误差作为状态量用于DVL/INS组合导航系统，仿真及试验结果表明，该方法具有优于传统Kalman滤波的性能。水跟踪模式下，DVL的速度输出会受到洋流的影响，可以通过引入额外的辅助定位手段，例如USBL、PS等，可以对洋流速度进行估计^[14-16]。考虑到这些方法应用的局限性，文献[17]中在对准模式下将洋流速度建模为SINS/DVL组合导航系统中的状态量进行估计，然后在导航模式下将估计出来的洋流速度进行实时补偿，从而提高导航解算精度。对于DVL速度输出中的异常值也可以通过该方法进行剥离^[18]。由于EML的精度较低，针对EML的应用相对较少，Ben等人设计了双状态滤波器来避免未知洋流速度对EML/SINS组合导航解算结果的影响，仿真及实验结果验证了该方法的有效性^[6]。

论文针对船载试验环境中激光惯导/里程仪组合导航系统标定容易受到洋流速度干扰的影响，在进行标定滤波器设计时在原有标定参数基础上将洋流速度建模为额外的Kalman滤波状态量，并采用GNSS位置、速度信息作为Kalman滤波观测量来对相关参数进行估计。论文从里程仪的标定观测方程出发，分析了洋流速度对里程仪比例因子、安装误差角等误差参数的影响以及不同误差参数之间的耦合关系；基于可观性分析理论研究了洋流速度的可观测性，并确定了适用于洋流速度估计的标定路径。仿真及试验结果验证了论文方法的正确性。

1 原理实验分析

1.1 坐标系定义

激光惯导与里程仪之间的标定涉及三个坐标系，分别是惯导载体坐标系b系，里程仪坐标系d系以及导航坐标系n系。三个坐标系的定义如下：

b系：即惯导载体坐标系 ${o_b} - {x_b}{y_b}{z_b}$，将船前行的方向定义为y轴，向上的方向定义为z轴，x轴构成右手坐标系。

d系：即里程仪坐标系 ${o_d} - {x_d}{y_d}{z_d}$，将船前行的方向定义为y轴，向上的方向定义为z轴，x轴构成右手坐标系。

n系：即导航坐标系 ${o_n} - {x_n}{y_n}{z_n}$，将地理坐标系的东向（E）定义为x轴，北向（N）定义为y轴，天向（U）定义为z轴。

图1所示为d系与b系之间的安装关系，图中 ${L_b}$表示里程仪与惯导之间的安装杆臂， $\left[ {\begin{array}{*{20}{c}} {{\alpha _x}}&{{\alpha _y}}&{{\alpha _z}} \end{array}} \right]$表示二者之间的安装误差角。在标定之前，里程仪和惯导之间的杆臂可以先通过全站仪等光学设备进行精确测量。

图 1. 惯导b系和里程仪d系之间的安装关系

Fig. 1. Relationship between b-frame and d-frame

下载图片 查看所有图片

1.2 GNSS/激光惯导/里程仪组合导航系统标定模型

标定中，以GNSS的位置速度输出作为Kalman滤波观测量。与陆上标定不同，状态量增加了洋流速度项。标定观测方程、状态方程以及状态量如下。

1.2.1 标定观测方程

设激光惯导与里程仪之间的安装矩阵为 $ {\boldsymbol{C}}_d^b $，则里程仪在导航系下的速度输出表示为：

$ {\boldsymbol{v}}_n^d = {\boldsymbol{C}}_b^n{\boldsymbol{C}}_d^b{{\boldsymbol{v}}_d} $ (1)

式中： ${{\boldsymbol{v}}_d} = {[\begin{array}{*{20}{c}} 0&{{v_d}}&0 \end{array}]^{\rm{T}}}$。

假设惯导的姿态误差 ${\boldsymbol{\phi}} = {\left[ {\begin{array}{*{20}{c}} {{\phi _E}}\;\;\;{{\phi _N}}\;\;\;{{\phi _U}} \end{array}} \right]^{\rm{T}}}$，则公式（1）可以表示为：

$ {{\boldsymbol{v}}_n} = \left( {{\boldsymbol{I}} - \left[ {{\boldsymbol{\phi}} \times } \right]} \right){\boldsymbol{C}}_b^n{\boldsymbol{C}}_d^b{{\boldsymbol{v}}_d} $ (2)

考虑惯导与里程仪之间的安装误差角 ${\boldsymbol{\alpha}}$，则公式（2）可以进一步表示为：

$ {{\boldsymbol{v}}_n} = \left( {{\boldsymbol{I}} - \left[ {{\boldsymbol{\phi}} \times } \right]} \right){\boldsymbol{C}}_b^n\left( {{\boldsymbol{I}} - \left[ {{\boldsymbol{\alpha}} \times } \right]} \right){\boldsymbol{C}}_d^b{{\boldsymbol{v}}_d} $ (3)

假设里程仪的比例因子误差为 $\delta k$，公式（3）可以表示为：

$ {{\boldsymbol{v}}_n} = \left( {{\boldsymbol{I}} - \left[ {{\boldsymbol{\phi}} \times } \right]} \right){\boldsymbol{C}}_b^n\left( {{\boldsymbol{I}} - \left[ {{\boldsymbol{\alpha}} \times } \right]} \right){\boldsymbol{C}}_d^b\left( {1 + \delta k} \right){{\boldsymbol{v}}_d} $ (4)

不考虑天向速度的影响，n系下的洋流速度可以表示为 ${\boldsymbol{v}}_n^C = \left[ {\begin{array}{*{20}{c}} {{\boldsymbol{v}}_{nE}^C}&{{\boldsymbol{v}}_{nN}^C}&0 \end{array}} \right]$，则公式（4）可以表示为：

$ {{\boldsymbol{v}}_n} = \left( {{\boldsymbol{I}} - \left[ {{\boldsymbol{\phi}} \times } \right]} \right){\boldsymbol{C}}_b^n\left( {{\boldsymbol{I}} - \left[ {{\boldsymbol{\alpha}} \times } \right]} \right){\boldsymbol{C}}_d^b\left( {1 + \delta k} \right){{\boldsymbol{v}}_d} + {\boldsymbol{v}}_n^C $ (5)

将公式（5）展开，忽略二阶小量，可得：

$ \delta {\boldsymbol{v}}_n^d = \left[ {{\boldsymbol{v}}_n^d \times } \right]{\boldsymbol{\phi}} + {\boldsymbol{C}}_b^n\left[ {{\boldsymbol{v}}_b^d \times } \right]{\boldsymbol{\alpha}} + \delta k{\boldsymbol{v}}_n^d + {\boldsymbol{v}}_n^C $ (6)

式中： ${\boldsymbol{v}}_b^d = {\boldsymbol{C}}_d^b{{\boldsymbol{v}}_d}$为里程仪在b系下的速度投影。方程左边为GNSS速度与里程仪n系下的速度之差，可表示为：

$ \delta {\boldsymbol{v}}_n^d = {\boldsymbol{v}}_n^g - {\boldsymbol{v}}_n^d $ (7)

公式（6）为里程仪的标定方程，与陆用测速设备相比多了一个洋流速度项。

为保证姿态估计精度，引入GNSS/INS组合导航解算模型，其中位置和速度作为组合导航系统的观测量，即：

$ \left\{ {\begin{array}{*{20}{c}} {\delta {\boldsymbol{P}}_n^I = {\boldsymbol{P}}_n^g - {\boldsymbol{P}}_n^I} \\ {\delta {\boldsymbol{v}}_n^I = {\boldsymbol{v}}_n^g - {\boldsymbol{v}}_n^I} \end{array} } \right. $ (8)

式中： ${\boldsymbol{P}}_n^g$、 ${\boldsymbol{v}}_n^g$GPS在n系下的位置速度输出； ${\boldsymbol{P}}_n^I$、 ${\boldsymbol{v}}_n^I$为惯导在n系下的位置速度输出。

联立方程（7）、（8），系统观测矩阵可以写为：

$ {\boldsymbol{H}} = \left[ {\begin{array}{*{20}{c}} {{{\boldsymbol{I}}_{3 \times 3}}}\;\;{{{\boldsymbol{0}}_{3 \times 3}}}\;\;{{{\boldsymbol{0}}_{3 \times 3}}}\;\;{{{\boldsymbol{0}}_{3 \times 3}}}\;\;{{{\boldsymbol{0}}_{3 \times 3}}}\;\;{{{\boldsymbol{0}}_{3 \times 3}}}\;\;{{{\boldsymbol{0}}_{3 \times 1}}}\;\;{{{\boldsymbol{0}}_{3 \times 2}}} \\ {{{\boldsymbol{0}}_{3 \times 3}}}\;\;{{{\boldsymbol{0}}_{3 \times 3}}}\;\;{{I_{3 \times 3}}}\;\;{{{\boldsymbol{0}}_{3 \times 3}}}\;\;{{{\boldsymbol{0}}_{3 \times 3}}}\;\;{{{\boldsymbol{0}}_{3 \times 3}}}\;\;{{{\boldsymbol{0}}_{3 \times 1}}}\;\;{{{\boldsymbol{0}}_{3 \times 2}}} \\ {{{\boldsymbol{0}}_{3 \times 3}}}\;\;{\left[ {v_n^d \times } \right]}\;\;{{{\boldsymbol{0}}_{3 \times 3}}}\;\;{{{\boldsymbol{0}}_{3 \times 3}}}\;\;{{{\boldsymbol{0}}_{3 \times 3}}}\;\;{{\boldsymbol{C}}_b^n\left[ {{\boldsymbol{v}}_b^d \times } \right]}\;\;{{\boldsymbol{v}}_n^d}{{\boldsymbol{H}}_v^C} \end{array} } \right] $ (9)

其中：

$ {\boldsymbol{H}}_v^C = \left[ {\begin{array}{*{20}{c}} 1\;\;\;0 \\ 0\;\;\;1 \\ 0\;\;\;0 \end{array} } \right] $ (10)

系统的观测量为 ${\boldsymbol{z}} = {\left[ {\begin{array}{*{20}{c}} {\delta {\boldsymbol{v}}_n^I}&{\delta {\boldsymbol{P}}_n^I}&{\delta {\boldsymbol{v}}_n^d} \end{array}} \right]^{\rm{T}}}$，观测噪声则依据卫导的位置、速度测量精度确定。

1.2.2 系统状态量

系统状态量主要包括惯导相关状态误差量以及标定相关状态量，可以表示为：

$ \begin{split} {\boldsymbol{x}} =& \left[ {\begin{array}{*{20}{c}} {\delta {v_E}}\;\;{\delta {v_N}}\;\;{\delta {v_U}}\;\;{{\phi _E}}\;\;{{\phi _N}}\;\;{{\phi _U}}\;\;{\delta L}\;\;{\delta \lambda }\;\;{\delta h} \end{array} } \right. \\ &{\left. {\begin{array}{*{20}{c}} {{\varepsilon _x}}\;\;{{\varepsilon _y}}\;\;{{\varepsilon _z}}\;\;{{\nabla _x}}\;\;{{\nabla _y}}\;\;{{\nabla _z}}\;\;{{\alpha _x}}\;\;{{\alpha _y}}\;\;{{\alpha _z}}\;\;{\delta k}\;\;{v_E^C}\;\;{v_N^C} \end{array} } \right]^{\rm{T}}} \\ \end{split} $ (11)

式中： $ \delta {{\boldsymbol{v}}_{E,N,U}} $为东向、北向、天向速度误差； $ \delta L $为纬度误差； $ \delta \lambda $为经度误差； $ \delta h $为高度误差； $ {\varepsilon _{x,y,z}} $为三个陀螺零偏； $ {\nabla _{x,y,z}} $为三个加速度计零偏。

1.2.3 系统状态方程

惯导相关状态量的状态方程与GNSS/INS组合导航系统状态方程一致，此处不再赘述。测速仪相关状态量建模为随机常数，如公式（12）所示：

$ \left\{ {\begin{array}{*{20}{c}} {\dot {\boldsymbol{\alpha}} = {\boldsymbol{0}}} \\ {\delta \dot k = 0} \\ {\dot {\boldsymbol{v}}_n^C = {\boldsymbol{0}}} \end{array}} \right. $ (12)

1.3 标定参数可观性分析

为进一步分析各标定参数之间的耦合关系，对其可观性进行分析。由于船在行驶过程中水平方向的姿态变化较小，这里仅考虑水平运动情况。当Kalman滤波完全收敛时，姿态误差为0，此处不予考虑。则姿态变换矩阵可以表示为：

$ {\boldsymbol{C}}_b^n = \left[ {\begin{array}{*{20}{c}} {\cos \left( {{\omega _c}t} \right)}&{ - \sin\left( {{\omega _c}t} \right)}&0 \\ {\sin\left( {{\omega _c}t} \right)}&{\cos \left( {{\omega _c}t} \right)}&0 \\ 0&0&1 \end{array}} \right] $ (13)

式中： $ {\omega _c} $为水平方向的角速度。

里程仪在b系下的速度输出可以表示为：

$ {\boldsymbol{v}}_b^d = \left( {1 + \delta k} \right)\left[ {\begin{array}{*{20}{c}} 1&{{\alpha _z}}&{ - {\alpha _y}} \\ { - {\alpha _z}}&1&{{\alpha _x}} \\ {{\alpha _y}}&{ - {\alpha _x}}&1 \end{array}} \right]\left[ {\begin{array}{*{20}{c}} 0 \\ {{v_d}} \\ 0 \end{array}} \right] $ (14)

将公式（14）展开，并忽略二阶小量，可以得到如下表达式：

$ {\boldsymbol{v}}_b^d = \left( {1 + \delta k} \right)\left[ {\begin{array}{*{20}{c}} {{v_d}{\alpha _z}} \\ {{v_d}} \\ { - {v_d}{\alpha _x}} \end{array}} \right] \approx \left[ {\begin{array}{*{20}{c}} {{v_d}{\alpha _z}} \\ {\left( {1 + \delta k} \right){v_d}} \\ { - {v_d}{\alpha _x}} \end{array}} \right] $ (15)

联立公式（13）、（15），里程仪在n系下的速度可以表示为：

$ \begin{split} {\boldsymbol{v}}_n^d = & \left[ {\begin{array}{*{20}{c}} {\cos \left( {{\omega _c}t} \right)}&{ - \sin\left( {{\omega _c}t} \right)}&0 \\ {\sin\left( {{\omega _c}t} \right)}&{\cos \left( {{\omega _c}t} \right)}&0 \\ 0&0&1 \end{array}} \right]\left[ {\begin{array}{*{20}{c}} {{v_d}{\alpha _z}} \\ {\left( {1 + \delta k} \right){v_d}} \\ { - {v_d}{\alpha _x}} \end{array}} \right] =\\ & \left[ {\begin{array}{*{20}{c}} { - \left( {1 + \delta k} \right)\sin \left( {{\omega _c}t} \right){v_d} + \cos \left( {{\omega _c}t} \right){v_d}{\alpha _z}} \\ {\left( {1 + \delta k} \right)\cos \left( {{\omega _c}t} \right){v_d} + \sin \left( {{\omega _c}t} \right){v_d}{\alpha _z}} \\ { - {v_d}{\alpha _x}} \end{array}} \right] \\ \end{split} $ (16)

不考虑姿态误差的影响，公式（6）可以写为：

$ {\boldsymbol{v}}_n^g - {\boldsymbol{v}}_n^d = {\boldsymbol{C}}_b^n\left[ {{{\boldsymbol{v}}_b} \times } \right]{\boldsymbol{\alpha}} + \delta k{\boldsymbol{v}}_n^d + {\boldsymbol{v}}_n^C $ (17)

结合公式（16），公式（17）左边可以展开为：

$ {\boldsymbol{v}}_n^g - {\boldsymbol{v}}_n^d = \left[ {\begin{array}{*{20}{c}} {{\boldsymbol{v}}_{nE}^g + \left( {1 + \delta k} \right)\sin \left( {{\omega _c}t} \right){v_d} - \cos \left( {{\omega _c}t} \right){v_d}{\alpha _z}} \\ {{\boldsymbol{v}}_{nN}^g - \left( {1 + \delta k} \right)\cos \left( {{\omega _c}t} \right){v_d} - \sin \left( {{\omega _c}t} \right){v_d}{\alpha _z}} \\ {{\boldsymbol{v}}_{nU}^g + {v_d}{\alpha _x}} \end{array}} \right] $ (18)

公式（17）右边第一项可以表示为：

$\begin{split} \;{{\boldsymbol{C}} }_b^n\left[{\boldsymbol{v}}_b \times\right] {\boldsymbol{\alpha}}= & {\left[\begin{array}{ccc} \cos \left(\omega_c t\right) & -\sin \left(\omega_c t\right) & 0 \\ \sin \left(\omega_c t\right) & \cos \left(\omega_c t\right) & 0 \\ 0 & 0 & 1 \end{array}\right] . } \\ & {\left[\begin{array}{lll} 0 & v_d \alpha_x & (1+\delta k) v_d \\ -v_d \alpha_x & 0 & -v_d \alpha_z \\ -(1+\delta k) v_d & v_d \alpha_z & 0 \end{array}\right]\left[\begin{array}{c} \alpha_x \\ 0 \\ \alpha_z \end{array}\right]=} \\ & {\left[ \begin{array}{lll} \cos \left(\omega_c t\right) & -\sin \left(\omega_c t\right) & 0 \\ \sin \left(\omega_c t\right) & \cos \left(\omega_c t\right) & 0 \\ 0 & 0 & 1 \end{array} \right]\left[ \begin{array}{c} (1+\delta k) v_d \alpha_z \\ -v_d \alpha_x^2-v_d \alpha_z^2 \\ -(1+\delta k) v_d \alpha_x \end{array} \right] \approx } \\ & {\left[\begin{array}{ll} \cos \left(\omega_c t\right) v_d \alpha_z \\ \sin \left(\omega_c t\right) v_d \alpha_z \\ -v_d \alpha_x \end{array}\right] } \end{split} $ (19)

公式（17）右边第二项可以表示为：

$ \begin{split} \delta k{\boldsymbol{v}}_n^d =& [ { - \left( {1 + \delta k} \right)\sin \left( {{\omega _c}t} \right){v_d} + \cos \left( {{\omega _c}t} \right){v_d}{\alpha _z}} \cdot \\ & {\left( {1 + \delta k} \right)\cos \left( {{\omega _c}t} \right){v_d} + \sin \left( {{\omega _c}t} \right){v_d}{\alpha _z}}- \\ & { {v_d}{\alpha _x}} ]\delta k \end{split}$ (20)

公式（17）右边第三项可以表示为：

$ {\boldsymbol{v}}_n^C = \left[ {\begin{array}{*{20}{c}} {{\boldsymbol{v}}_E^C} \\ {{\boldsymbol{v}}_N^C} \\ 0 \end{array}} \right] $ (21)

将公式（18）~（21）代入公式（17）可得：

$ \left\{ {\begin{array}{*{20}{c}} \begin{split} & v_{nE}^g = - {\left( {1 + \delta k} \right)^2}\sin \left( {{\omega _c}t} \right){v_d} + 2\cos \left( {{\omega _c}t} \right){v_d}{\alpha _z} + \\ & \qquad \;\; \cos \left( {{\omega _c}t} \right){v_d}{\alpha _z}\delta k + v_E^C \\ \end{split} \\ \begin{split} & v_{nN}^g = {\left( {1 + \delta k} \right)^2}\cos \left( {{\omega _c}t} \right){v_d} + 2\sin \left( {{\omega _c}t} \right){v_d}{\alpha _z} + \\ & \qquad \;\; \sin \left( {{\omega _c}t} \right){v_d}{\alpha _z}\delta k + v_N^C \\ \end{split} \\ {v_{nU}^g = - 2{v_d}{\alpha _x} - {v_d}{\alpha _x}\delta k} \end{array} } \right. $ (22)

公式（22）即为标定参数解算方程，从公式（22）中可以得出以下结论：

（1）在陆用环境中，由于不需要考虑洋流速度的影响，比例因子误差、安装误差完全可观。由于不同误差状态之间存在耦合关系，当初始标定误差较大的时候，一次滤波不能得到实现状态的完全估计，可以通过加长滤波周期或多次迭代的方法提高Kalman滤波估计精度；

（2）在海用环境中，如果不考虑洋流速度的影响，洋流速度会以速度误差的形式出现在观测方程中。在此情况下，速度误差会耦合在标定参数中，从而估计出一个错误的值；

（3） $ {\alpha _x} $的估计与天向速度直接相关，屏蔽天向通道不能估计 $ {\alpha _x} $； $ {\alpha _y} $完全不可观；

（4）公式（22）的推导过程未考虑姿态误差的影响，根据惯导可观性分析可得，GNSS/INS组合导航系统的航向可观性较差，需经过机动才能快速收敛。相关标定参数只有在航向完全收敛后，才能得到很好的估计；

（5）将洋流速度做为状态量在Kalman滤波估计后，公式（22）变得不完全可观，需要通过机动改变航向才能对洋流速度等参数进行有效估计。这里仅考虑两个最简单的情况：

设转弯之前的航向角为0°，则公式（22）可以变化为：

$ \left\{ {\begin{array}{*{20}{c}} {{\boldsymbol{v}}_{nE1}^g = 2{v_d}{\alpha _z} + {v_d}{\alpha _z}\delta k + v_E^C} \\ {{\boldsymbol{v}}_{nN1}^g = {{\left( {1 + \delta k} \right)}^2}{v_d} + v_N^C} \end{array} } \right. $ (23)

式中： $ {\boldsymbol{v}}_{nE1}^g $和 $ {\boldsymbol{v}}_{nN1}^g $表示航向角等于0°时的GNSS速度。

设转弯之前的航向角为90°，则公式（22）可以变化为：

$ \left\{ {\begin{array}{*{20}{c}} {{\boldsymbol{v}}_{nE2}^g = - {{\left( {1 + \delta k} \right)}^2}{v_d} + v_E^C} \\ {{\boldsymbol{v}}_{nN2}^g = 2{v_d}{\alpha _z} + {v_d}{\alpha _z}\delta k + v_N^C} \end{array} } \right. $ (24)

式中： $ {\boldsymbol{v}}_{nE2}^g $和 $ {\boldsymbol{v}}_{nN2}^g $表示航向角为90°时的GNSS速度信息。

联立公式（23）、（24），可得：

$ \left\{ {\begin{array}{*{20}{c}} {{\boldsymbol{v}}_E^C = \dfrac{{v_{nE1}^g + v_{nE2}^g + v_{nN1}^g - v_{nN2}^g}}{2}} \\ {{\boldsymbol{v}}_N^C = \dfrac{{v_{nN1}^g + v_{nE2}^g + v_{nN2}^g - v_{nE1}^g}}{2}} \\ {{\alpha _z} = \dfrac{{v_{nE1}^g - v_{nE2}^g - v_{nN1}^g + v_{nN2}^g}}{{2{v_d} + {v_d}\delta k}}} \\ {\delta k = \sqrt {\dfrac{{v_{nN1}^g - v_{nE2}^g - v_{nN2}^g + v_{nE1}^g}}{{{v_d}}}} } \end{array} } \right. $ (25)

从公式（25）可以看出，经过转向后， $ {\boldsymbol{v}}_E^C $、 $ {\boldsymbol{v}}_N^C $、 $ {\alpha _z} $和 $ \delta k $变得完全可解。而 $ {\alpha _z} $与 $ \delta k $相关，其收敛速度会受到比例因子误差 $ \delta k $的影响。

受解算原理限制，惯导初始姿态误差较大。而在滤波初始阶段或者第一次迭代滤波时，标定参数未完全收敛，标定参数误差过大会对姿态估计造成影响，反过来影响标定结果。因此，可以考虑略去标定方程中与惯导姿态误差有关的项。在此情况下，观测矩阵可以改写为：

${\boldsymbol{ H}} = \left[ {\begin{array}{*{20}{c}} {{{\boldsymbol{I}}_{3 \times 3}}}\;\;\;{{{\boldsymbol{0}}_{3 \times 3}}}\;\;\;{{{\boldsymbol{0}}_{3 \times 3}}}\;\;\;{{{\boldsymbol{0}}_{3 \times 3}}}\;\;\;{{{\boldsymbol{0}}_{3 \times 3}}}\;\;\;{{{\boldsymbol{0}}_{3 \times 3}}}\;\;\;{{{\boldsymbol{0}}_{3 \times 1}}}\;\;\;{{{\boldsymbol{0}}_{3 \times 2}}} \\ {{{\boldsymbol{0}}_{3 \times 3}}}\;\;\;{{{\boldsymbol{0}}_{3 \times 3}}}\;\;\;{{I_{3 \times 3}}}\;\;\;{{{\boldsymbol{0}}_{3 \times 3}}}\;\;\;{{{\boldsymbol{0}}_{3 \times 3}}}\;\;\;{{{\boldsymbol{0}}_{3 \times 3}}}\;\;\;{{{\boldsymbol{0}}_{3 \times 1}}}\;\;\;{{{\boldsymbol{0}}_{3 \times 2}}} \\ {{{\boldsymbol{0}}_{3 \times 3}}}\;\;\;{{{\boldsymbol{0}}_{3 \times 3}}}\;\;\;{{{\boldsymbol{0}}_{3 \times 3}}}\;\;\;{{{\boldsymbol{0}}_{3 \times 3}}}\;\;\;{{{\boldsymbol{0}}_{3 \times 3}}}\;\;\;{{\boldsymbol{C}}_b^n\left[ {{\boldsymbol{v}}_b^d \times } \right]}\;\;\;{{\boldsymbol{v}}_n^d}\;\;\;{{\boldsymbol{H}}_v^C} \end{array} } \right] $ (26)

2 仿真试验分析

2.1 仿真试验分析

2.1.1 仿真试验设置

以某次航海试验数据为基础对所提方法的可行性及精度进行仿真分析。试验轨迹如图2所示。从图中可以看出，试验过程中有两次明显转弯，根据可观性分析，转弯有助于相关标定参数的收敛。

图 2. 仿真用标定试验轨迹

Fig. 2. Trajectory of calibration experiment in simulation

下载图片 查看所有图片

试验中使用的惯导为一90型捷联激光惯导，双频卫导接收机作为位置、速度输出设备。试验中所用惯导及卫导精度如表1所示。

由于该次海试试验未搭载里程仪，里程仪速度通过将卫导在n系下的速度投影到b系下而仿真生成。初始标定参数设置如表2所示。

2.1.2 标定结果

利用文中的方法对激光惯导/里程仪组合导航系统的参数进行标定。采用了多次标定法，其中第一次滤波后的结果如图3~图5所示。

图 3. 一次滤波后的洋流速度估计结果

Fig. 3. Current velocity estimation after first filtering

下载图片 查看所有图片

图 4. 一次滤波后的安装误差角估计结果

Fig. 4. Installation error estimation after first filtering

下载图片 查看所有图片

图 5. 一次滤波后的比例因子误差估计结果

Fig. 5. Scale factor estimation after first filtering

下载图片 查看所有图片

从图3~图5中可以看出，将洋流速度建模为状态量估计之后，经过一次滤波所有的标定参数都可以收敛到一个较为准确的值。而在1.5~2.2 h之间船经过两次转弯，可以看到标定参数收敛明显加快。

经过三次迭代之后的标定参数残差如图6~图8所示。

图 6. 三次滤波后的洋流速度残差

Fig. 6. Current velocity residual after third filtering

下载图片 查看所有图片

从图中可以看出，经过三次迭代滤波之后，标定参数基本收敛到真值，标定残差较小，洋流速度对标定参数的影响可以完全消除，验证了所提方法的有效性。

图 7. 三次滤波后的安装误差估计残差

Fig. 7. Installation error residual after third filtering

下载图片 查看所有图片

图 8. 三次滤波后的比例因子估计残差

Fig. 8. Scale factor residual after third filtering

下载图片 查看所有图片

2.2 试验验证

2.2.1 试验及设备简介

以中国南海的某次海试为基础对文中的标定方法进行验证，试验中用到了50型激光陀螺捷联惯导以及电磁里程仪。卫导接收机输出作为速度及位置观测量。试验中所用设备的精度如表3所示。

试验总时长9.4 h，取前面的2.4 h作为标定路径，如图9中小图所示。从图中可以看出，标定过程中有几次明显的转弯，根据可观性分析，转弯有助于提高相关参数的收敛速度，对于提高参数估计精度具有重要作用。经过多次迭代后，计算得到的标定结果如表4所示。

图 9. 带里程仪的标定试验轨迹

Fig. 9. Trajectory of calibration experiment with EML

下载图片 查看所有图片

相关参数的残差如图10、图11和图12所示。从图中可以看出，使用论文中的标定方法，经过多次迭代后，标定参数可以收敛到一个准确值，残差接近于0，相关参数的收敛速度也在转弯之后明显加快，与仿真结果类似。

图 10. 带里程仪的海上洋流速度估计残差

Fig. 10. Current velocity residual in the marine experiment with EML

下载图片 查看所有图片

图 11. 带里程仪的海上安装误差角估计残差

Fig. 11. Installation error residual in the marine experiment with EML

下载图片 查看所有图片

图 12. 带里程仪的海上比例因子估计残差

Fig. 12. Scale factor residual in the marine experiment with EML

下载图片 查看所有图片

2.2.2 海上试验结果

通过海试组合导航数据进一步验证标定参数的准确性，利用上节标定出的参数对里程仪及激光惯导之间的误差参数进行校正。海试试验总时长为9.4 h，试验轨迹如图9中的大图所示。图13所示为误差参数标定与未标定时的水平位置误差对比。

图 13. 有无标定的海上水平位置误差试验结果对比

Fig. 13. Position error comparison between with and without calibration in the marine experiment

下载图片 查看所有图片

从图中可以看出，标定参数补偿之后，水平误差得到了显著抑制，水平定位精度有了显著提升。试验结果证明了标定参数的准确性以及标定方法的可行性。

3 结　论

目前，船载测速设备主要基于分立式标定，分立式标定对海域、航行条件等要求较高。文中基于Kalman滤波方法设计了里程仪标定模型，将洋流速度建模为滤波状态量进行估计，有效避免了洋流速度对标定参数的影响。基于可观测性分析理论，分析了不同误差之间的耦合关系以及洋流速度的可观性，并据此设计了误差参数标定路径。试验结果表明，经过转弯之后洋流速度完全可观，其对其他参数的影响可以完全消除，验证了文中方法的有效性。