-
高精度导航系统是水下机器人的关键技术之一[1]。目前，水下机器人常用的导航定位传感器包括超短基线定位系统、多普勒测速仪、电子罗盘、深度计、惯性导航系统等[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在水下无法工作，仅仅使用多普勒测速仪、电子罗盘和陀螺仪会存在较大的累积误差。
本文研究水下机器人的水下组合导航定位问题，提出了一种以超短基线定位系统、多普勒测速仪与电子罗盘相结合的组合导航系统，通过迭代无迹卡尔曼滤波算法，实现水下机器人的动态定位。通过水下机器人的水池实验与仿真分析验证了算法的有效性。
-
本文搭建的水下组合导航系统采用了3种类型的传感器：超短基线定位系统、多普勒测速仪和电子罗盘。其中，超短基线定位系统用于测量水下机器人相对于母船的位置；多普勒测速仪从3个方向捕捉水下机器人载体坐标系下的速度；电子罗盘用于获取水下机器人的艏向角、纵倾角和横摇角。基于UKF的水下组合导航系统流程图如图1所示。具体实现过程如下：
图 1 基于UKF的水下组合导航系统流程图
Figure 1. Flow chart of underwater navigation system based on UKF
（1）对于由多普勒测速仪测得的水下机器人3个方向的速度信息，利用UKF进行滤波处理。
（2）将处理后的速度信息与电子罗盘获得的角度信息相结合，将机器人的运动状态从载体坐标系转换到大地坐标系，计算出机器人在大地坐标系下的位移信息。
（3）对于超短基线定位系统实时获取的相对母船的位置坐标信息，通过UKF进行滤波处理。
（4）将处理后的机器人位移信息和位置坐标信息作为输入，使用UKF进行数据融合，输出定位信息。
（5）为提高定位精度，对输出定位信息进行3次迭代，以输出更高精度的定位信息。
-
UKF算法在处理状态方程时，首先进行UT变换，然后使用变换后的状态变量进行滤波估计，以减少估计误差。
UKF算法通过确定性采样得到一组Sigma点，从而获得更多的观测假设，对系统状态的均值和协方差的估计更为准确，同时由于该算法采用了非线性的状态方程或观测方程，避免了线性化误差[14]。首先使用UKF算法对多普勒传感器获取的速度信息和超短基线获取的位置坐标信息进行滤波，以提高初始数据的精度。然后在对多普勒测速仪、电子罗盘和超短基线定位系统获取的数据信息进行融合时，对图1中的位移估计和通过UKF滤波的位置坐标分别进行确定性采样。以图1位移估计中得到的大地坐标系下的X向位移信息和超短基线定位系统得到的X向位置信息为例，UKF数据融合算法的具体步骤如下：
(1)对于给定的
${\hat x_{k - 1,k - 1}}$ 、${\hat y_{k - 1,k - 1}}$ （设置初始值为0）、${{P}_{k - 1,k - 1}}$ （设置其初始值为单位矩阵）求出状态一步预测${\hat x_{k,k - 1}}$ 、${\hat y_{k,k - 1}}$ 以及预测误差的协方差阵${{P}_{k,k - 1}}$ 。其中${\hat x_{k - 1,k - 1}}$ 为图1位移估计中得到的大地坐标系下的X向位移信息的统计量，${\hat y_{k,k - 1}}$ 为超短基线定位系统得到的X向位置信息的统计量。在UKF算法中，一般选取2n+1个Sigma点，n为随机变量维数。本实验中，选择在一维条件下进行数据融合，故n=1。Sigma点$\chi _{k - 1,k - 1}^i$ 、$\gamma _{k - 1,k - 1}^i$ 均为随机变量，随机变量变化过程如下：①计算Sigma点
$\chi _{k - 1,k - 1}^i$ 、$\gamma _{k - 1,k - 1}^i$ ：
②计算Sigma点
$\chi _{k,k}^i$ 、$\gamma _{k,k}^i$ ，可以得到变换后的点集$\{ \chi _k^{\left( i \right)}\} $ 和${\rm{\{ }}\gamma _k^{\left( i \right)}{\rm{\} }}$ 。变换后的点集${\rm{\{ }}\chi _k^{\left( i \right)}{\rm{\} }}$ 可近似地表示为X向位移信息的分布，变换后的点集${\rm{\{ }}\gamma _k^{\left( i \right)}{\rm{\} }}$ 可近似地表示为X向位置信息的分布：
其中：
$\omega _i^{(m)}$ 为Sigma点期望的权值；$\omega _i^{(c)}$ 为Sigma点方差的权值；${{Q}_{k - 1}}$ 为过程激励噪声的协方差矩阵。(2)求Sigma点
${\hat x_{k,k - 1}}$ 、${\hat y_{k,k - 1}}$ 、${P_{k,k - 1}}$ 通过量测方程的传播。①计算Sigma点
${\hat x_{k,k - 1}}$ 、${\hat y_{k,k - 1}}$ 、${P_{k,k - 1}}$ 通过量测方程对${\rm{}}{\chi _k}$ 、${\rm{}}{\gamma _k}$ 传播：
②计算输出的一步提前预测，即观测预测均值：
其中：
$\xi _k^i$ 和$\zeta _k^i$ 为预测值；${\hat z_{k,k - 1}}$ 和${\hat w_{k,k - 1}}$ 为经过加权后得到的一步预测值即观测变量；${P_{{{\bar w}_k}}}$ 和${P_{{{\bar z}_k}}}$ 为均方误差的一步预测；${P_{{{\bar x}_k}{{\bar z}_k}}}$ 和${P_{{{\bar y}_k}{{\bar w}_k}}}$ 为互方差。③滤波更新以及数据融合，
${z_k}$ 为位移估计中大地坐标系下的X向位移信息，${w_k}$ 为超短基线定位系统得到的X向位置信息。即
其中：
${\hat x_k}$ 和${\hat y_k}$ 为$k$ 时刻的最优值；${{K}_k}$ 为$k$ 时刻的滤波增益；${{P}_k}$ 为$k$ 时刻的最优均方误差；${\hat x_k}={\hat x_k} + {\hat y_k}$ 实现大地坐标系下的X向位移信息和超短基线定位系统得到的X向位置信息的融合。 -
迭代无迹卡尔曼滤波（Iterative Unscented Kalman Filter，IUKF）是在原有的无迹卡尔曼滤波的基础上对量测更新的状态估计值采用多步迭代，从而减少由于泰勒展开带来的截断误差。本文应用IUKF算法减少误差，提高水下机器人在水下定位的精度。将已经通过无迹卡尔曼滤波输出的位置信息作为新的输入信号再次进行滤波，得到一个新的输出信号。
在任意时刻计算k迭代估计值的步骤如下：
（1）将
${x}\left( k \right)$ 的一步预测状态向量$\hat {{x}}{\left( k \right)^ - }$ 作为迭代的初始值${x^{\left( 0 \right)}}\left( k \right)$ ，依据观测方程线性化，计算滤波值与UKF相同，并把结果$\hat {{x}}\left( k \right)$ 作为${{{x}}^{\left( 1 \right)}}\left( k \right)$ 。${{{x}}^{\left( 1 \right)}}\left( k \right)$ 中的1代表第1次迭代完成。（2）在
${{{x}}^{\left( 1 \right)}}\left( k \right)$ 处重新将观测方程线性化，再一次进行UKF滤波，新估计值$\hat {{x}}{\left( k \right)^ + }$ 作为${{{x}}^{\left( 2 \right)}}\left( k \right)$ ，${{{x}}^{\left( 2 \right)}}\left( k \right)$ 中的2代表第2次迭代完成。（3）迭代过程中，当两步状态差值满足式(6)：
迭代终止，否则返回步骤（2）。其中
$\varepsilon $ 是预先给定的迭代阈值，i代表迭代次数[15]。 -
为验证水下组合导航算法的有效性，对水下机器人在水池环境下的数据进行了仿真实验分析，并将分析结果与机器人在湖泊和池塘环境下的数据分析结果进行对比，以确定最佳迭代次数。如图2所示，水下机器人自带电子罗盘，并在底部加装了多普勒测速仪。
图 2 机器人水池测试
Figure 2. Robot is testing in the pool
-
引入数据迭代不仅提高了UKF的收敛速度，而且收敛过程稳定，不会影响UKF的收敛性。为验证IUKF的有效性，基于多普勒测速仪测得的载体坐标系下的X向速度数据，分别利用UKF和IUKF进行处理，比较滤波效果和误差。
在UKF滤波器中，设置Q=0.03，R=0.01。UKF滤波结果及滤波误差分别如图3、图4所示，其中vx为x向速度。
图 3 UKF滤波结果
Figure 3. Results of UKF filtering
图 4 UKF的滤波误差
Figure 4. Error of UKF filtering
对UKF滤波后数据进行第一次迭代，并将图3中滤波处理后结果作为第一次IUKF滤波的输入。图5、图6分别示出了水池环境下不同迭代次数下的多普勒测速仪测得的载体坐标系下X向速度数据及经UKF后的滤波效果对比。
图 5 水池环境下UKF滤波效果图
Figure 5. Effect diagrams of UKF filtering under the swimming pool environme
图 6 水池环境下误差比对
Figure 6. Error comparison under the swimming pool environment
为了更直观地观察迭代次数对滤波效果的影响，计算每组数据经过UKF滤波后的误差绝对值均值。不同迭代次数下误差绝对值均值如表1所示。
Iteration Absolute value of error /(mm·s−1) 0 8.754 9 1 7.047 0 2 5.873 5 3 5.003 3 4 5.003 3 5 5.003 3 6 5.003 3 7 5.003 3 8 5.003 3 9 5.003 3 10 5.003 3 表 1 不同迭代次数下的误差绝对值均值
Table 1. Mean value of absolute error under different iterations
-
为验证3次迭代在其他环境应用时的有效性，在上海海事大学校内池塘与湖泊进行实验并采集了运动数据。
图7、图8分别示出了池塘环境下不同迭代次数下的多普勒测速仪测得的载体坐标系下X向速度数据以及经UKF后的滤波效果对比。
图 7 池塘环境下UKF滤波效果图
Figure 7. Effect diagrams of UKF filtering under the pond environment
图 8 池塘环境下误差比对
Figure 8. Error comparison under the pond environment
图 9 湖泊环境下UKF滤波效果图
Figure 9. Effect diagrams of UKF filtering under the lake environment
图9、图10分别示出了湖泊环境下不同迭代次数下的多普勒测速仪测得的载体坐标系下X向速度数据以及经UKF后的滤波效果对比。不同环境、不同迭代次数下误差绝对值均值如表2所示。
Iteration Mean value of absolute error /(mm·s−1) swimming pool Pond Lake 0 8.754 9 2.187 8.695 4 1 7.047 0 1.72 6.878 1 2 5.873 5 1.389 7 5.596 1 3 5.003 3 1.389 7 4.742 1 4 5.003 3 1.389 7 4.742 1 5 5.003 3 1.389 7 4.742 1 表 2 不同环境下不同迭代次数下的误差绝对值均值
Table 2. Mean value of absolute error under different iteration times in different environments
图 10 湖泊环境下误差比对
Figure 10. Error comparison under the lake environment
由图7~图10以及表2可以发现，在水池和湖泊环境下采集的数据经过3次迭代就可以达到预期效果，而在池塘环境下仅需要两次就可以达到预期效果。迭代次数的不同在一定程度上受环境影响，但影响不是很大，因此在水池环境下选择对输出的定位信息进行的迭代次数为3次。
-
图11示出了使用超短基线定位模拟数据做出的机器人轨迹与机器人运动的原始轨迹，超短基线定位模拟数据是在实际定位信息数据的基础上添加噪声产生的。图12示出了航位推算数据做出的机器人轨迹与机器人运动的原始轨迹。两者轨迹出现较大偏差是因为航位推算存在累积误差，从而使得机器人运动的轨迹出现较大偏差。图13示出了使用IUKF算法对超短基线定位、电子罗盘和多普勒测速仪的数据进行处理后的轨迹，与机器人运动的原始轨迹基本一致。比较图11、图12和图13可以发现，仅基于电子罗盘与多普勒测速仪的航位推算方法在后期具有较大的累积误差，加入超短基线定位系统的定位数据后，对位置信息进行了有效的修正，经过IUKF融合以后，取得了较好的融合效果。水池实验及仿真分析表明，本文提出的水下组合导航算法是可行、有效的。
图 11 模拟的超短基线路径
Figure 11. Simulate path of USBL
图 12 航位推算路径
Figure 12. Path of dead reckoning
图 13 IUKF融合路径
Figure 13. Path of IUKF
-
本文提出了一种基于迭代无迹卡尔曼滤波的水下多传感器组合导航融合算法，在对多传感器反馈的水下机器人运动信息进行UKF滤波处理后进行多次迭代。通过比较每次迭代后产生的均值误差，确定最合适的迭代次数为3次。通过对超短基线定位数据、仅基于电子罗盘与多普勒测速仪的航位推算数据和IUKF数据融合定位数据进行对比，验证了基于IUKF的水下组合导航系统的有效性。
基于迭代无迹卡尔曼滤波的水下组合导航
Underwater Integrated Navigation Based on IUKF
-
摘要: 研究了基于超短基线定位系统、多普勒测速仪以及电子罗盘相结合的水下机器人组合导航问题。提出了一种基于迭代无迹卡尔曼滤波（IVKF）的水下多传感器组合导航融合算法，降低了传统无迹卡尔曼滤波器中过程噪声协方差矩阵和测量噪声协方差矩阵对滤波精度和响应时间的影响。该方法通过对多传感器数据融合结果进行多次迭代，提高了水下多传感器组合导航精度。通过多传感器的水池实验和仿真分析，验证了水下组合导航算法的可行性和有效性。Abstract: Research based on the ultra-short baseline positioning system and Doppler velocimeter and electronic compass combination of underwater robot navigation problem, put forward a kind of based on iterative unscented Kalman filtering fusion algorithm of multi-sensor underwater integrated navigation, reduce traditional unscented Kalman filter process noise covariance matrix and measurement noise covariance matrix of the influence of the filtering precision and response time. The proposed method improves the precision of underwater multi-sensor combined navigation by iterating the results of multi-sensor data fusion several times. Firstly, the data obtained by the sensors were preprocessed. Secondly, plot the motion trajectory diagram simulated by the ultra-short baseline positioning system, the motion trajectory diagram calculated by dead reckoning and the motion trajectory diagram fused by the unscented Kalman filtering algorithm were drawn respectively. Thirdly, the unscented Kalman filtering algorithm iterates the output of multi-sensor data fusion for several times, which can achieve a better fusion effect. Finally, the feasibility and effectiveness of the combined navigation algorithm under the water are verified by multi-sensor pool test and simulation analysis. The effectiveness of the iterative unscented Kalman filtering algorithm based on underwater integrated navigation system was verified by comparing the simulated ultra-short baseline positioning data, the dead reckoning data based on the electronic compass and Doppler velocimeter and the position information of the positioning data fused with the iterative unscented Kalman filtering data.
-
Key words:
- integrated navigation /
- underwater /
- the unscented Kalman filter /
- iterative /
- data fusion
-
图 1 基于UKF的水下组合导航系统流程图
Figure 1. Flow chart of underwater navigation system based on UKF
图 5 水池环境下UKF滤波效果图
Figure 5. Effect diagrams of UKF filtering under the swimming pool environme
图 7 池塘环境下UKF滤波效果图
Figure 7. Effect diagrams of UKF filtering under the pond environment
图 9 湖泊环境下UKF滤波效果图
Figure 9. Effect diagrams of UKF filtering under the lake environment
表 1 不同迭代次数下的误差绝对值均值
Table 1. Mean value of absolute error under different iterations
Iteration Absolute value of error /(mm·s−1) 0 8.754 9 1 7.047 0 2 5.873 5 3 5.003 3 4 5.003 3 5 5.003 3 6 5.003 3 7 5.003 3 8 5.003 3 9 5.003 3 10 5.003 3 
下载: 导出CSV
表 2 不同环境下不同迭代次数下的误差绝对值均值
Table 2. Mean value of absolute error under different iteration times in different environments
Iteration Mean value of absolute error /(mm·s−1) swimming pool Pond Lake 0 8.754 9 2.187 8.695 4 1 7.047 0 1.72 6.878 1 2 5.873 5 1.389 7 5.596 1 3 5.003 3 1.389 7 4.742 1 4 5.003 3 1.389 7 4.742 1 5 5.003 3 1.389 7 4.742 1
下载: 导出CSV
-
[1] WANG L, WU W Q, GUO W, et al. Navigation information fusion in a redundant marine rotational inertial navigation system configuration[J]. Journal of Navigation, 2018, 71(6): 1531-1552. doi: 10.1017/S0373463318000322 [2] ANTONIO V, BRUNO B, ZORAN V. Underwater vehicle localization with complementary filter: Performance analysis in the shallow water environment[J]. Journal of Intelligent & Robotic Systems, 2012, 68(3/4): 373-386. [3] SUN Y D, YANG G L, CAI Q Z, et al. An on-line calibration method of SINS/Odometer integrated navigation system[C]//2017 4th International Conference on Information Science and Control Engineering (ICISCE). Changsha: IEEE, 2017: 1-25. [4] LIU J, NING X L, MA X, et al. Geometry error analysis in solar doppler difference navigation for the capture phase[J]. IEEE Transactions on Aerospace and Electronic Systems, 2019, 55(5): 2556-2567. doi: 10.1109/TAES.2019.2892659 [5] 魏延辉, 刘静, 郝晟功. 基于改进自适应滤波的SINS/DVL组合导航算法研究[J]. 自动化与仪表, 2019, 34(5): 95-100.
[6] TANG K H, WANG J L, LI W L, et al. A novel INS and Doppler sensors calibration method for long range underwater vehicle navigation[J]. Sensors, 2013, 13(11): 14583-14600. doi: 10.3390/s131114583 [7] 宋振华, 战兴群, 张炎华. 基于联邦卡尔曼滤波器的水下机器人自主导航[C]// 中国造船工程学会2006年船舶通讯导航学术会议. 宜昌: 中国造船工程学会, 中国仪器仪表学会, 2006: 263-269.
[8] HONG L, LI M J, HAN N. MEMS/GPS integrated navigation system based on EKF[J]. Applied Mechanics and Materials, 2014, 2933(964): 367-374. [9] 穆华, 吴志添, 吴美平. 水下地磁/惯性组合导航试验分析[J]. 中国惯性技术学报, 2013, 21(3): 386-391. doi: 10.3969/j.issn.1005-6734.2013.03.022
[10] 刘济, 顾幸生. 基于UKF的参数和状态联合估计[J]. 华东理工大学学报(自然科学版), 2009, 35(5): 762-767. doi: 10.3969/j.issn.1006-3080.2009.05.020
[11] 陈晶, 吴胜昔, 顾幸生. 一种针对火电厂流量计数据特点的数据处理算法[J]. 华东理工大学学报(自然科学版), 2009, 35(3): 475-480. doi: 10.3969/j.issn.1006-3080.2009.03.026
[12] LIU B, LIU K Z, WANG Y Y, et al. A hybrid deep sea navigation system of LBL/DR integration based on UKF and PSO-SVM[J]. 机器人, 2015, 37(5): 614-620.
[13] 史兴波, 曾俊宝. 水下机器人海洋资源目标探测导航仿真[J]. 计算机仿真, 2018, 35(4): 331-336. doi: 10.3969/j.issn.1006-9348.2018.04.071
[14] 徐成刚. 基于非线性预测滤波和UKF的状态估计方法[J]. 科技信息, 2013(19): 174-176. doi: 10.3969/j.issn.1001-9960.2013.19.130
[15] 龙慧, 胡利, 周宴宇. 迭代卡尔曼滤波在机器人定位中的应用[J]. 现代电子技术, 2010, 33(22): 123-1. doi: 10.3969/j.issn.1004-373X.2010.22.038
-
点击查看大图
图(13)表(2)
计量
- 文章访问数: 811
- HTML全文浏览量: 534
- PDF下载量: 7
- 被引次数: 0