基于多傳感器采集信息的水下機器人導航系統

冷 芳，裴洲奇

(大連海洋大學，遼寧 大連 116300)

0 引 言

自主水下機器人(AUV)具有較高的智能化水平，是一種憑借自身控制系統實現自主運動控制、智能化決策的水下作業機器人[1–3]。導航系統是水下機器人的必備部分，融合多傳感器信息實現水下機器人導航是其導航方式的一種重要形式，而多傳感器信息的融合效果直接對水下機器人導航精度產生影響。

邱海洋等[4]設計基于MOOS 的水下機器人導航系統，該導航系統利用卡爾曼濾波器對各傳感器信息進行處理，難以取得較好的估計效果。夏楠等[5]設計的AUV 對接導航系統通過多尺度無跡卡爾曼濾波器實現多傳感器數據的融合，解決了采樣頻率差異造成的估計結果誤差大的問題，但該系統在避障過程中難以實現最優路徑規劃[5]。鑒于以上系統存在的問題，本文設計基于多傳感器采集信息的水下機器人導航系統，以提升水下機器人導航精度，使其更準確完成水下任務。

1 水下機器人導航系統

1.1 水下機器人導航系統總體框架

水下機器人導航系統基本結構如圖1 所示。該系統由傳感器采集模塊、數據處理模塊、串口通信模塊、運動控制模塊、以太網通信模塊、導航模塊組成。數據采集模塊在水下機器人運動環境與信息處理、控制間搭建溝通橋梁，利用安裝在水下機器人上的各類型傳感器完成其運動姿態、速度、位置以及環境等信息的采集。采集信息經過數據處理模塊的融合處理后，通過串口通信模塊傳輸給運動控制模塊，由主控制器實現水下機器人的運動控制。利用以太網通信模塊實現融合后數據的上傳，上位機中的導航模塊通過避障算法和路徑規劃算法實現水下機器人的導航，并呈現導航結果。

圖 1 水下機器人導航系統基本結構Fig. 1 Basic structure of underwater robot navigation system

1.2 傳感器采集模塊設計

對水下機器人的運動、姿態等數據進行采集是實現其精準導航的前提，為提高傳感器數據采集效率，降低計算難度，本文設計的傳感器采集模塊，通過配置在水下機器人上的若干個傳感器分別獲取其姿態、深度、密封艙溫濕度、GPS 等數據，在完成各傳感器數據的實時處理后，提供給水下機器人的主控制器，實現其運動的閉環控制。傳感器采集模塊結構如圖2 所示。

圖 2 傳感器采集模塊Fig. 2 Sensor acquisition module

1.3 主控制器設計

水下機器人主控制器結構如圖3 所示，包含5 個主要模塊，分別為主微控制單元(MCU)、無刷直流電機、電子調速器、電力線載波通信模塊(PLC)、電壓測試模塊。其中將STM32F4 芯片作為主控制MCU，其內核是基于ARM Cortex-M4 設計，最高主頻率可達到168 MHz，支持UATR，SPI 等多種數據傳輸接口，為其配備的外部設備有定時器、ADC 等。主MCU 以間接方式對無刷直流電機進行控制，需借助定時器生成4 路PWM 信號，將其傳輸至電子調速器中，實現無刷直流電機的控制，從而完成實現機器人運動姿態的控制。主MCU 依靠PLC 獲取外部控制指令，并實現水下機器人運動狀態信息的傳輸。

圖 3 水下機器人主控制器結構Fig. 3 Main controller structure of underwater vehicle

1.4 基于聯合卡爾曼濾波的水下機器人導航

利用聯合卡爾曼濾波器對水下機器人IMU、水溫水壓傳感器、GPS 傳感器、視覺傳感器采集數據進行融合處理，其基本原理是設定系統內配置了若干個傳感器，針對各個傳感器，聯合卡爾曼濾波器為其生成一個獨立濾波器，在各局部濾波器并行運作下，獲取局部濾波最優估計結果，再利用主濾波器實現各局部最優結果的全局融合處理，從而輸出全局最優估計結果。水下機器人導航系統具有任意性、離散性特點，其卡爾曼濾波狀態方程可通過下式進行描述：

式中：X(k)為水下機器人的狀態向量， Φ(k)為狀態轉移矩陣，B(k) 為輸入控制矩陣，U(k) 為控制變量，W(k)為噪聲。

設定水下機器人中配置的傳感器數量為N,則需要的局部濾波器數量也為N，每個濾波器都能完成其對應傳感器的濾波處理。對于第i個局部濾波器Xi(k+1)，可描述為：

其中：ZVi(k)，Hi(k)分別為觀測量及觀測矩陣，Vi(k)為噪聲。

對于卡爾曼濾波器，其時間更新方程為：

測量更新方程的描述公式為：

其中：Si(k+1) 為協方差矩陣；di(k+1)=Zi(k+1)?Hi(k+1)X?i(k+1|k)為信息，經一步預測后，水下機器人狀態值表示為X?i(k+1|k)， 其協方差表示為Pi(k+1|k)，卡爾曼增益表示為Ki(k+1)，調整后的水下機器人狀態表示為X?i(k+1|k+1)， 誤差協方差表示為Pi(k+1|k+1)，單位矩陣表示為I。

實現水下機器人多傳感器采集信息的融合處理后，建立其自身及運動環境模型，識別運動環境中的障礙物以及可能存在的碰撞風險，尋找環境模型中全部運動路徑，在其中找出最優路徑完成避障，從而實現水下機器人的導航，水下機器人導航流程如圖4 所示。

圖 4 水下機器人導航過程Fig. 4 Navigation process of underwater robot

2 實驗分析

以自主水下機器人(AUV)為研究對象，將本文系統應用于AUV 導航中，AUV 上布置的傳感器參數如表1 所示。設定AUV 在240 m×240 m×240 m 的水下環境中運動，通過對AUV 導航過程的模擬分析，驗證本文系統的應用性能。

設定AUC 向東北30°角方向運動，同時作下潛航行，航行時長為1200 s，航行過程中未遇到任何障礙物，初始位置為經度120°、緯度40°，終止位置為經度120.016°、緯度為40.006°，采用本文系統獲取AUV 上各傳感器采集信息，并對其航行過程進行導航，將實際運動軌跡作為對比，通過對比分析軌跡導航結果，驗證本文系統的導航性能，實驗結果如圖5所示。可知，采用本文系統對AUV 進行導航，導航軌跡與實際真實軌跡曲線基本保持相同的走勢規律，導航軌跡僅在局部區域存在微小偏差，經過導航，AUV能夠準確達到目標位置。實驗結果表明，本文系統具有較好的導航效果。

表 1 各傳感器參數Tab. 1 Parameters of each sensor

圖 5 本文系統導航結果分析Fig. 5 Analysis of system navigation results in this article

為進一步分析本文系統的導航效果，對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 的導航，且能達到導航精度提升目的。

圖 6 傳感器信息融合前后速度誤差對比Fig. 6 Comparison of velocity errors before and after sensor information fusion

圖 7 傳感器信息融合前后位置誤差對比Fig. 7 Comparison of position errors before and after sensor information fusion

設定AUV 航行環境中存在3 個移動障礙物，采用本文系統對其進行導航，通過對AUV 的導航軌跡進行分析，驗證本文系統的避障能力以及路徑規劃能力，實驗結果如圖8 所示。可知，將本文系統應用于AUV 導航中，可有效規避AUV 航行環境中存在的移動障礙物，能夠不斷對其運動軌跡進行規劃，設計出適合當下環境的最優路徑。實驗結果表明，本文系統可實現避障功能，并完成航行軌跡的設計。

圖 8 本文系統導航結果分析Fig. 8 Analysis of system navigation results in this article

3 結 語

應用本文系統對水下機器人AUV 進行導航，通過分析多傳感器信息融合前后水下機器人位置、速度誤差、導航軌跡以及避障性能驗證本文系統的應用效果。實驗結果表明：利用該系統對AUV 進行導航，速度、位置誤差較低；導航軌跡與實際軌跡曲線走勢相似，偏差不大；可實現避障，并規劃出避障路線。