0 引　言

自主水下机器人(AUV)具有较高的智能化水平，是一种凭借自身控制系统实现自主运动控制、智能化决策的水下作业机器人^[1-3]。导航系统是水下机器人的必备部分，融合多传感器信息实现水下机器人导航是其导航方式的一种重要形式，而多传感器信息的融合效果直接对水下机器人导航精度产生影响。

邱海洋等^[4]设计基于MOOS的水下机器人导航系统，该导航系统利用卡尔曼滤波器对各传感器信息进行处理，难以取得较好的估计效果。夏楠等^[5]设计的AUV对接导航系统通过多尺度无迹卡尔曼滤波器实现多传感器数据的融合，解决了采样频率差异造成的估计结果误差大的问题，但该系统在避障过程中难以实现最优路径规划^[5]。鉴于以上系统存在的问题，本文设计基于多传感器采集信息的水下机器人导航系统，以提升水下机器人导航精度，使其更准确完成水下任务。

1 水下机器人导航系统 1.1 水下机器人导航系统总体框架

水下机器人导航系统基本结构如图1所示。该系统由传感器采集模块、数据处理模块、串口通信模块、运动控制模块、以太网通信模块、导航模块组成。数据采集模块在水下机器人运动环境与信息处理、控制间搭建沟通桥梁，利用安装在水下机器人上的各类型传感器完成其运动姿态、速度、位置以及环境等信息的采集。采集信息经过数据处理模块的融合处理后，通过串口通信模块传输给运动控制模块，由主控制器实现水下机器人的运动控制。利用以太网通信模块实现融合后数据的上传，上位机中的导航模块通过避障算法和路径规划算法实现水下机器人的导航，并呈现导航结果。

1.2 传感器采集模块设计

对水下机器人的运动、姿态等数据进行采集是实现其精准导航的前提，为提高传感器数据采集效率，降低计算难度，本文设计的传感器采集模块，通过配置在水下机器人上的若干个传感器分别获取其姿态、深度、密封舱温湿度、GPS等数据，在完成各传感器数据的实时处理后，提供给水下机器人的主控制器，实现其运动的闭环控制。传感器采集模块结构如图2所示。

1.3 主控制器设计

水下机器人主控制器结构如图3所示，包含5个主要模块，分别为主微控制单元(MCU)、无刷直流电机、电子调速器、电力线载波通信模块(PLC)、电压测试模块。其中将STM32F4芯片作为主控制MCU，其内核是基于ARM Cortex-M4设计，最高主频率可达到168 MHz，支持UATR，SPI等多种数据传输接口，为其配备的外部设备有定时器、ADC等。主MCU以间接方式对无刷直流电机进行控制，需借助定时器生成4路PWM信号，将其传输至电子调速器中，实现无刷直流电机的控制，从而完成实现机器人运动姿态的控制。主MCU依靠PLC获取外部控制指令，并实现水下机器人运动状态信息的传输。

1.4 基于联合卡尔曼滤波的水下机器人导航

利用联合卡尔曼滤波器对水下机器人IMU、水温水压传感器、GPS传感器、视觉传感器采集数据进行融合处理，其基本原理是设定系统内配置了若干个传感器，针对各个传感器，联合卡尔曼滤波器为其生成一个独立滤波器，在各局部滤波器并行运作下，获取局部滤波最优估计结果，再利用主滤波器实现各局部最优结果的全局融合处理，从而输出全局最优估计结果。水下机器人导航系统具有任意性、离散性特点，其卡尔曼滤波状态方程可通过下式进行描述：

$ X\left( {k + 1} \right) = \varPhi \left( k \right)X\left( k \right) + B\left( k \right)U\left( k \right) + W\left( k \right) ,$

(1)

式中：X(k)为水下机器人的状态向量， $\varPhi \left( k \right)$ 为状态转移矩阵，B(k)为输入控制矩阵，U(k)为控制变量，W(k)为噪声。

设定水下机器人中配置的传感器数量为 $ N $ ,则需要的局部滤波器数量也为 $ N $ ，每个滤波器都能完成其对应传感器的滤波处理。对于第 $ i $ 个局部滤波器 $ {X_i}\left( {k + 1} \right) $ ，可描述为：

$ \left\{ \begin{gathered} {X_i}\left( {k + 1} \right) = A\left( k \right){X_i}\left( k \right) + B\left( k \right)U\left( k \right), \\ {Z_i}\left( k \right) = {H_i}\left( k \right){X_i}\left( k \right) + {V_i}\left( k \right)。\\ \end{gathered} \right. $

(2)

其中：ZV_i(k)，H_i(k)分别为观测量及观测矩阵，V_i(k)为噪声。

对于卡尔曼滤波器，其时间更新方程为：

$ {\hat X_i}\left( {k + 1\left| k \right.} \right) = \varPhi \left( k \right){\hat X_i}\left( {k\left| k \right.} \right) ,$

(3)

$ {P_i}\left( {k + 1\left| k \right.} \right) = \varPhi \left( k \right){P_i}\left( {k\left| k \right.} \right){\varPhi ^{\rm{T}}}\left( k \right) + \varPhi \left( k \right)W\left( k \right){\varPhi ^{\rm{T}}}\left( k \right)。$

(4)

测量更新方程的描述公式为：

$ {K_i}\left( {k + 1} \right) = \frac{{{P_i}\left( {k + 1\left| k \right.} \right)H_i^{\rm{T}}\left( {k + 1} \right)}}{{S_i^{}\left( {k + 1} \right)}} ,$

(5)

$ {\hat X_i}\left( {k + 1\left| {k + 1} \right.} \right) = {\hat X_i}\left( {k + 1\left| k \right.} \right) + {K_i}\left( {k + 1} \right){d_i}\left( {k + 1} \right) ,$

(6)

$ \begin{gathered} {P_i}\left( {k + 1\left| {k + 1} \right.} \right) = I \cdot {P_i}\left( {k + 1\left| k \right.} \right) - \\ {K_i}\left( {k + 1} \right)H_i^{}\left( {k + 1} \right){P_i}\left( {k + 1\left| k \right.} \right)。\\ \end{gathered} $

(7)

其中： $S_i^{}\left( {k + 1} \right)$ 为协方差矩阵； ${d_i}\left( {k + 1} \right) = {Z_i} \left( {k + 1} \right) - H_i^{} \left( {k + 1} \right){\hat X_i}\left( {k + 1\left| k \right.} \right)$ 为信息，经一步预测后，水下机器人状态值表示为 $ {\hat X_i}\left( {k + 1\left| k \right.} \right) $ ，其协方差表示为 $ {P_i}\left( {k + 1\left| k \right.} \right) $ ，卡尔曼增益表示为 $ {K_i}\left( {k + 1} \right) $ ，调整后的水下机器人状态表示为 $ {\hat X_i}\left( {k + 1\left| {k + 1} \right.} \right) $ ，误差协方差表示为 $ {P_i}\left( {k + 1\left| {k + 1} \right.} \right) $ ，单位矩阵表示为 $ I $ 。

在卡尔曼滤波时，以 $ {\hat X_i}\left( {0\left| 0 \right.} \right) $ ， $ {P_i}\left( {0\left| 0 \right.} \right) $ 为初始点，通过不断推进形式展开，各局部滤波器对传感器采集数据进行局部状态估计，获得局部最佳估计结果 $ {\hat X_i} $ ，当 $ W\left( k \right) $ 和 $ {V_i}\left( k \right) $ 为取值为0的均值白噪声，且局部估计相互独立时，即可获得多传感器信息融合结果。

实现水下机器人多传感器采集信息的融合处理后，建立其自身及运动环境模型，识别运动环境中的障碍物以及可能存在的碰撞风险，寻找环境模型中全部运动路径，在其中找出最优路径完成避障，从而实现水下机器人的导航，水下机器人导航流程如图4所示。

2 实验分析

以自主水下机器人(AUV)为研究对象，将本文系统应用于AUV导航中，AUV上布置的传感器参数如表1所示。设定AUV在240 m×240 m×240 m的水下环境中运动，通过对AUV导航过程的模拟分析，验证本文系统的应用性能。

设定AUC向东北30°角方向运动，同时作下潜航行，航行时长为1 200 s，航行过程中未遇到任何障碍物，初始位置为经度120°、纬度40°，终止位置为经度120.016°、纬度为40.006°，采用本文系统获取AUV上各传感器采集信息，并对其航行过程进行导航，将实际运动轨迹作为对比，通过对比分析轨迹导航结果，验证本文系统的导航性能，实验结果如图5所示。可知，采用本文系统对AUV进行导航，导航轨迹与实际真实轨迹曲线基本保持相同的走势规律，导航轨迹仅在局部区域存在微小偏差，经过导航，AUV能够准确达到目标位置。实验结果表明，本文系统具有较好的导航效果。

为进一步分析本文系统的导航效果，对AUV航行过程中的速度、位置误差进行分析，并与多传感器采集数据融合前的速度、位置误差进行对比，传感器信息融合前后的速度误差对比结果如图6所示，位置误差对比结果如图7所示。可知，本文系统对AUV的多传感器采集信息进行融合处理，实现了多传感器信息的综合利用，使得AUV导航过程中速度、位置误差获得大幅降低，融合前速度、位置误差分别在(−7 m/s，10 m/s)，(−0.15°,0.2°)范围内波动，融合后速度、位置误差分别为(−1 m/s,1 m/s)，(−0.02 m,0.02 m),区间波动。实验结果表明，本文系统可实现AUV的导航，且能达到导航精度提升目的。

设定AUV航行环境中存在3个移动障碍物，采用本文系统对其进行导航，通过对AUV的导航轨迹进行分析，验证本文系统的避障能力以及路径规划能力，实验结果如图8所示。可知，将本文系统应用于AUV导航中，可有效规避AUV航行环境中存在的移动障碍物，能够不断对其运动轨迹进行规划，设计出适合当下环境的最优路径。实验结果表明，本文系统可实现避障功能，并完成航行轨迹的设计。

3 结　语

应用本文系统对水下机器人AUV进行导航，通过分析多传感器信息融合前后水下机器人位置、速度误差、导航轨迹以及避障性能验证本文系统的应用效果。实验结果表明：利用该系统对AUV进行导航，速度、位置误差较低；导航轨迹与实际轨迹曲线走势相似，偏差不大；可实现避障，并规划出避障路线。