付凤婷， 褚振忠， 朱大奇

（上海海事大学物流工程学院，上海智能海事搜救与水下机器人工程技术研究中心，上海 201306）

高精度导航系统是水下机器人的关键技术之一[1]。目前，水下机器人常用的导航定位传感器包括超短基线定位系统（USBL）、多普勒测速仪（DVL）、电子罗盘（EC）、深度计、惯性导航系统等[2-5]。为了提高水下机器人导航定位的精度，需要对多传感器的输出数据进行融合处理以实现多传感器的组合导航。

卡尔曼滤波是一种常用的数据融合算法，该算法的滤波和融合精度直接影响着运动系统定位信息的精度和实时性[6]。宋振华等[7]提出了一种以捷联惯导、电子罗盘和深度计为观测量，通过联邦卡尔曼滤波器组合实现自主导航的方法。扩展卡尔曼滤波（Extended Kalman Filter，EKF）的基本思想是利用泰勒展开直接线性化非线性方程组，其优点是不需要额外的计算，计算结果与基本卡尔曼滤波的计算结果相似。Hong 等[8]提出了基于EKF 的MEMS/GPS组合导航系统。穆华等[9]提出了地磁/惯性组合导航的方法，通过点群滤波和扩展卡尔曼滤波相结合的两级滤波，实现了大观测噪声地磁测量的有效融合。然而，水下机器人运动具有较强的非线性，而扩展卡尔曼滤波算法的基本思想是利用泰勒展开，将非线性方程直接线性化，线性化后的模型与实际系统存在很大差别。无迹卡尔曼滤波（Unscented Kalman Filter，UKF）利用无迹变换（Unscented Transform，UT）处理单步预测方程的均值和协方差的非线性传递，其非线性分布统计量的计算精度高于EKF[10-11]。Liu等[12]提出了一种基于UKF 和支持向量机的混合导航方法。史兴波等[13]提出了基于UKF 算法的水下机器人组合导航系统，但是该导航系统的精度依赖于GPS 信号。由于GPS 在水下无法工作，仅仅使用多普勒测速仪、电子罗盘和陀螺仪会存在较大的累积误差。

本文研究水下机器人的水下组合导航定位问题，提出了一种超短基线定位系统、多普勒测速仪与电子罗盘相结合的组合导航系统，通过迭代无迹卡尔曼滤波（Iterative Unscented Kalman Filter, IUKF）算法，实现水下机器人的动态定位。通过水下机器人的水池实验与仿真分析验证了算法的有效性。

1 组合导航系统

本文搭建的水下组合导航系统采用了3 种类型的传感器：超短基线定位系统、多普勒测速仪和电子罗盘。其中，超短基线定位系统用于测量水下机器人相对于母船的位置；多普勒测速仪从3 个方向捕捉水下机器人载体坐标系下的速度；电子罗盘用于获取水下机器人的艏向角、纵倾角和横摇角。基于UKF 的水下组合导航系统流程图如图1 所示。具体实现过程如下：

（1）对于由多普勒测速仪测得的水下机器人3 个方向的速度信息，利用UKF 进行滤波处理。

（2）将处理后的速度信息与电子罗盘获得的角度信息相结合，将机器人的运动状态从载体坐标系转换到大地坐标系，计算出机器人在大地坐标系下的位移信息。

（3）对于超短基线定位系统实时获取的相对母船的位置坐标信息，通过UKF 进行滤波处理。

（4）将处理后的机器人位移信息和位置坐标信息作为输入，使用UKF 进行数据融合，输出定位信息。

（5）为提高定位精度，对输出定位信息进行3 次迭代，以输出更高精度的定位信息。

2 UKF 数据融合

UKF 算法在处理状态方程时，首先进行UT 变换，然后使用变换后的状态变量进行滤波估计，以减少估计误差。

UKF 算法通过确定性采样得到一组Sigma 点，从而获得更多的观测假设，对系统状态的均值和协方差的估计更为准确，同时由于该算法采用了非线性的状态方程或观测方程，避免了线性化误差[14]。首先使用UKF 算法对多普勒传感器获取的速度信息和超短基线获取的位置坐标信息进行滤波，以提高初始数据的精度。然后在对多普勒测速仪、电子罗盘和超短基线定位系统获取的数据信息进行融合时，对图1 中的位移估计和通过UKF 的位置坐标分别进行确定性采样。以图1 位移估计中得到的大地坐标系下的X 方向位移信息和超短基线定位系统得到的X 方向位置信息为例，UKF 数据融合算法的具体步骤如下：

图1基于UKF 的水下组合导航系统流程图Fig.1Flow chart of underwater navigation system based on UKF

②计算输出的一步提前预测，即观测预测均值：

③滤波更新以及数据融合， zk为位移估计中大地坐标系下的X 方向位移信息， wk为超短基线定位系统得到的X 方向位置信息。即

3 IUKF 算法

IUKF 是在原有的UKF 基础上对量测更新的状态估计值采用多步迭代，从而减少由于泰勒展开带来的截断误差。本文应用IUKF 算法减少误差，提高水下机器人在水下定位的精度。将已经通过UKF 输出的位置信息作为新的输入信号再次进行滤波，得到一个新的输出信号。

在任意时刻计算迭代估计值的步骤如下：

（3）迭代过程中，当两步状态差值满足式(6)：

迭代终止，否则返回步骤（2）。其中 ε 是预先给定的迭代阈值，i 代表迭代次数[15]。

4 数据分析

为验证水下组合导航算法的有效性，对水下机器人在水池环境下的数据进行了仿真实验分析，并将分析结果与机器人在湖泊和池塘环境下的数据分析结果进行对比，以确定最佳迭代次数。如图2 所示，水下机器人自带电子罗盘，并在底部加装了多普勒测速仪。

4.1 IUKF 与UKF 对比

引入数据迭代不仅提高了UKF 的收敛速度，而且收敛过程稳定，不会影响UKF 的收敛性。为验证IUKF 的有效性，基于多普勒测速仪测得的载体坐标系下的X 方向速度数据，分别利用UKF 和IUKF 进行处理，比较滤波效果和误差。

图2机器人水池测试Fig.2Robot is testing in the pool

图3UKF 滤波结果（a）和误差（b）Fig.3Results (a) and error (b) of UKF filtering

对UKF 滤波后的数据进行第1 次迭代，并将图3(a)中滤波处理后的结果作为第1 次IUKF 滤波的输入。图4、图5 分别示出了水池环境下不同迭代次数的多普勒测速仪测得的载体坐标系下X 方向速度数据及经UKF 滤波后的效果对比。

为了更直观地观察迭代次数对滤波效果的影响，计算每组数据经过UKF 滤波后的误差绝对值均值。不同迭代次数下误差绝对值均值如表1 所示。

由表1 以及图4、图5 可知，对由传感器获取的信息进行3 次迭代后，误差绝对值均值基本保持不变，可以达到预期效果。

4.2 不同场景对迭代次数的影响

图4水池环境下 UKF 滤波效果图Fig.4Effect diagrams of UKF filtering under the swimming pool environment

图5水池环境下误差比对Fig.5Error comparison under the swimming pool environment

表1不同迭代次数下的误差绝对值均值Table1Mean value of absolute error under different iterations

为验证3 次迭代在其他环境应用时的有效性，在上海海事大学校内的池塘和湖泊进行实验并采集了运动数据。图6、图7 分别示出了池塘环境不同迭代次数下的多普勒测速仪测得的载体坐标系下X 方向速度数据以及经UKF 滤波后的效果对比。图8、图9 分别示出了湖泊环境不同迭代次数下的多普勒测速仪测得的载体坐标系下X 方向速度数据以及经UKF 滤波后的效果对比。不同环境、不同迭代次数下的误差绝对值均值如表2 所示。

由图6～图9 以及表2 可以看出，在水池和湖泊环境下采集的数据经过3 次迭代可以达到预期效果，而在池塘环境下仅需要两次就可以达到预期效果。迭代次数的不同在一定程度上受环境影响，但影响不是很大，因此在水池环境下选择对输出的定位信息进行的迭代次数为3 次。

4.3 IUKF 数据融合

图6池塘环境下 UKF 滤波效果图Fig.6Effect diagrams of UKF filtering under the pond environment

图7池塘环境下误差比对Fig.7Error comparison under the pond environment

图8湖泊环境下UKF 滤波效果图Fig.8Effect diagrams of UKF filtering under the lake environment

图9湖泊环境下误差比对Fig.9Error comparison under the lake environment

表2不同环境不同迭代次数下的误差绝对值均值Table2Mean value of absolute error under different iteration times in different environments

图10 示出了使用超短基线定位模拟数据作出的机器人轨迹与机器人运动的原始轨迹，超短基线定位模拟数据是在实际定位信息数据的基础上添加噪声产生的。图11 示出了根据航位推算数据作出的机器人轨迹与机器人运动的原始轨迹，两者轨迹出现较大偏差是因为航位推算存在累积误差所致，图12示出了使用IUKF 算法对超短基线定位、电子罗盘和多普勒测速仪的数据进行处理后的轨迹，可见该轨迹与机器人运动的原始轨迹基本一致。比较图10～图12可以看出，仅基于电子罗盘与多普勒测速仪的航位推算方法得到的结果在后期具有较大的累积误差，加入超短基线定位系统的定位数据后，对位置信息进行了有效的修正，经过IUKF 融合以后，取得了较好的融合效果。水池实验及仿真分析表明，本文提出的水下组合导航算法是可行、有效的。

图10模拟的超短基线路径Fig.10Simulate path of USBL

图11航位推算路径Fig.11Path of dead reckoning

图12IUKF 融合路径Fig.12Path of IUKF

5 结 论

本文提出了一种基于IUKF 的水下多传感器组合导航融合算法，在对多传感器反馈的水下机器人运动信息进行UKF 滤波处理后进行多次迭代。通过比较每次迭代后产生的均值误差，确定最合适的迭代次数为3 次。通过对超短基线定位数据、仅基于电子罗盘与多普勒测速仪的航位推算数据和IUKF数据融合定位数据进行对比，验证了基于IUKF 的水下组合导航系统的有效性。