一種水下機器人雙冗餘姿態檢測系統的製作方法

2023-10-18 01:33:04 1

本發明屬於水下機器人技術領域，具體涉及一種水下機器人雙冗餘姿態檢測系統。

背景技術：

水下機器人姿態檢測系統可以為複雜未知環境下進行作業的水下機器人提供姿態、航向、速度、深度、高度等信息，這些信息對於水下機器人能否完成海洋環境作業任務至關重要。高可靠性的導航系統將能夠有效保障水下機器人安全生存和成功完成作業任務。

由於高精度慣性導航系統成本高昂，結構較為複雜。而基於MEMS慣性器件的慣性導航系統相比於高精度慣性導航系統具有成本低廉，結構簡單的優勢，同時姿態和航向精度指標能夠滿足作業級水下機器人工作需求。因此在工程應用領域，若考慮成本因素，同時精度指標要求適當，作業級水下機器人可以採用MEMS慣性器件與電子羅盤組合，為水下機器人提供穩定可靠的姿態和航向參考信息。

在水下機器人導航系統中，慣性器件的誤差對慣導系統精度影響很大。電子羅盤在實際應用中，易受到外界鐵磁性物質幹擾，導致輸出航向信息存在較大誤差。由於單一的導航系統都存在其各自的缺點，性能指標難以完全滿足作業級水下機器人的導航需求。為了使導航系統的精度和可靠性能夠滿水下機器人作業需求，需要結合各個單一導航系統的優勢，通過合理的數據融合方式，獲得精度和可靠性滿足需求的水下機器人導航系統。

在作業級水下機器人的導航系統中，通過組合導航的方式，可以實現導航原理、輸出數據的類型和特點都各不相同的導航系統的數據融合，實現各個導航系統之間的優勢互補，滿足作業級水下機器人對導航信息精度和高可靠性的需求。

技術實現要素：

本發明的目的在於提供一種多傳感器數據融合的姿態檢測系統。

本發明的目的是這樣實現的：

一種水下機器人雙冗餘姿態檢測系統，由數據融合處理器、MEMS姿態傳感器、電子羅盤、都卜勒計程儀、深度計、高度計傳感器構成，通過微控制器實現導航計算，輸出姿態、速度、深度、高度信息，都卜勒計程儀的輸出接口接至數據融合處理器的輸入接口，電子羅盤的輸出接口接至數據融合處理器的輸入接口，MEMS姿態傳感器的輸出接口接至數據融合處理器的輸入接口，深度計的輸出接口接至數據融合處理器的輸入接口，高度計的輸出接口接至數據融合處理器的輸入接口，數據融合處理器的輸出接口接至水下機器人中心控制器的輸入接口；數據融合處理器是將多種傳感器信息採集後，進行信息融合和處理，通過基於UD分解的EKF濾波和組合導航系統AHRS部分模型修正後，輸出姿態、速度、深度、高度信息；MEMS姿態傳感器是由三軸MEMS加速度計和三軸MEMS陀螺組成；

電子羅盤採用地球磁場信息來確定北極指向的導航設備；電子羅盤採用傳感器檢測所在位置地磁場強度，經過計算獲得載體航向信息，採用非鐵磁性金屬作為外殼；

基於UD分解的EKF濾波的運算步驟如下：

步驟1：狀態一步預測：

式中：Xk,k-1是系統的n×n維狀態觀測矩陣，是系統的狀態轉移矩陣，是狀態估計；

步驟2：UD分解：

式中：Pk是估計方差矩陣，Pk,k-1是誤差方差矩陣，Γk,k-1是n×p維噪聲輸入矩陣，Qk,k-1是系統過程噪聲方差陣；

步驟3：濾波增益矩陣的求解：

Gk＝Uk,k-1Fk

Sk＝HkGk+Rk

步驟4：狀態估計：

步驟5：估計方差矩陣：

組合導航系統AHRS部分模型修正方案的步驟如下所示：

步驟1：採用三軸磁力計和三軸加速度計原始數據，計算出地理坐標系下的航向角，作為導航系統航向的觀測信息；

對電子羅盤的三軸原始數據進行傾斜補償：

航向角的計算如下所示：

步驟2：組合導航系統姿態航向部分的模型構建：

令Z軸MEMS陀螺的角速度輸入值為陀螺零偏為εZ，陀螺噪聲序列為nr,z，Z軸MEMS陀螺的模型如下所示：

令k時刻陀螺的零偏為εZ，k-1時刻陀螺的零偏為零偏增量ΔεZ如下所示：

航向部分狀態方程和觀測方程如下所示：

系統的狀態量由航向角增量Δψ和ΔεZ構成，如下所示：

令Z軸陀螺陀螺隨機遊走噪聲序列為nωw，系統的噪聲激勵序列Wyaw如下所示：

Wyaw＝[nωr nωw]T；

系統的狀態轉移矩陣系統噪聲驅動陣Γyaw如下所示：

系統的觀測量由電子羅盤航向角ψMag與上一時刻系統輸出航向角ψ差值構成，如下所示：

系統的觀測轉移矩陣Hyaw如下所示：

Hyaw＝[1 0]；

系統狀態噪聲方差陣Qyaw、量測噪聲方差陣Ryaw如下所示：

步驟3：將舊AHRS模型中的航向修正部分移除，只保留姿態部分，如下所示：

步驟4：求取濾波結果後，通過四元數求取橫滾和俯仰角，與修正後的航向角求取濾波後得到的四元數，完成本次濾波器觀測更新。

本發明的有益效果在於：

(1)採用多種類型導航傳感器提供的信息，通過數據融合實現高可靠導航信息輸出的方法。

(2)採用的傳感器都是低成本，但是相互信息具有很強的互補性，能夠有效滿足水下機器人實際需要，克服單一傳感器的缺陷，通過對典型海況下的分析，設計出有效地補償方法，避免外界幹擾和傳感器自身缺點對水下機器人姿態信息影響。

(3)基於多傳感器的姿態檢測系統為水下機器人姿態測量提供解決方案，具有一定通用性和實用性。

附圖說明

圖1為水下機器人雙冗餘姿態檢測系統結構圖；

圖2為組合導航系統AHRS模型優化處理後信號流程圖；

圖3為組合導航系統AHRS模型優化前後姿態波形圖；

圖4為水下機器人導航系統動態實驗姿態波形；

圖5為水下機器人導航系統動態實驗速度波形；

圖6為水下機器人導航系統動態實驗位置波形。

具體實施方式

下面結合附圖對本發明做更詳細的描述。

本發明涉及水下作業機器人(簡稱水下機器人)控制領域，提供了一種具有雙冗餘的姿態檢測系統。本發明設計雙冗餘水下機器人姿態檢測系統由數據融合處理器、MEMS姿態傳感器、電子羅盤、都卜勒計程儀、深度計、高度計傳感器構成。通過微控制器實現導航計算，輸出姿態、速度、深度、高度信息。本發明解決單一形式姿態傳感器精度低、易受外界幹擾等問題，通過多種傳感器信息融合和相互補償，實現水下機器人姿態檢測系統的低成本、高可靠性和高精度，且具有通用性和實用性，滿足水下機器人在不同工作環境下對姿態檢測要求。該系統克服單一傳感器檢測精度低，易受外界幹擾等問題，提高系統的可靠性和測量精度，滿足實際作業需要。

本發明採用的技術方案是：

雙冗餘水下機器人姿態檢測系統由數據融合處理器、MEMS姿態傳感器、電子羅盤、都卜勒計程儀、深度計、高度計傳感器構成。通過微控制器實現導航計算，輸出姿態、速度、深度、高度信息。都卜勒計程儀的輸出接口接至數據融合處理器的輸入接口，電子羅盤的輸出接口接至數據融合處理器的輸入接口，MEMS姿態傳感器的輸出接口接至數據融合處理器的輸入接口，深度計的輸出接口接至數據融合處理器的輸入接口，高度計的輸出接口接至數據融合處理器的輸入接口，數據融合處理器的輸出接口接至水下機器人中心控制器的輸入接口。

MEMS姿態傳感器是由三軸MEMS加速度計和三軸MEMS陀螺組成，能夠測量載體的姿態，該傳感器已經為每個傳感器的靈敏度、偏置等參數進行工廠校準，並且對傳感器的安裝誤差進行了嚴格的正交對準，使其能夠提供精確的測量信息。同時該傳感器針對MEMS器件的溫度漂移也進行了校準，使該傳感器能夠有更寬的溫度應用環境。

電子羅盤是採用地球磁場信息來確定北極指向的導航設備。電子羅盤採用傳感器檢測所在位置地磁場強度，經過計算獲得載體航向信息。採用非鐵磁性金屬作為外殼，具有低功耗、小體積和易於安裝的優點，但是易受外界磁場幹擾。

都卜勒計程儀是一種利用都卜勒效應，實現航行器在運動中絕對或相對水層速度測量的傳感器，深度計是測量載體到水面深度的傳感器，高度計是利用聲學測量載體到海底的高度傳感器。

本發明解決了單一傳感器檢測精度低，易受外界幹擾等問題，通過多種傳感器信息融合實現信息的融合和互補，克服MEMS姿態傳感器長時間的累積誤差，以及電子羅盤容受外界電磁幹擾的缺點，提高系統的可靠性和測量精度。

本發明提供的是一種水下機器人雙冗餘姿態檢測系統，該系統解決單一形式姿態傳感器精度低、易受外界幹擾等問題，通過多種傳感器信息融合和相互補償，實現水下機器人姿態檢測系統的低成本、高可靠性和高精度，滿足控制系統需要和實際工程需要，具有通用性和實用性，滿足水下機器人在不同工作環境下對姿態檢測要求。

實施1，結合附圖1，本發明的雙冗餘水下機器人姿態檢測系統由數據融合處理器、MEMS姿態傳感器、電子羅盤、都卜勒計程儀、深度計、高度計傳感器構成。通過微控制器實現導航計算，輸出姿態、速度、深度、高度信息。都卜勒計程儀的輸出接口1接至數據融合處理器的輸入接口2，電子羅盤的輸出接口3接至數據融合處理器的輸入接口4，MEMS姿態傳感器的輸出接口5接至數據融合處理器的輸入接口6，深度計的輸出接口7接至數據融合處理器的輸入接口8，高度計的輸出接口9接至數據融合處理器的輸入接口10，數據融合處理器的輸出接口11接至水下機器人中心控制器的輸入接口12。

實施2，基於UD分解的EKF濾波的運算步驟如下：

步驟1：狀態一步預測：

步驟2：UD分解：

步驟3：濾波增益矩陣的求解：

由式(3)可得：

步驟4：狀態估計：

步驟5：估計方差矩陣：

通過基於UD分解的EKF濾波算法，能夠避免EKF的運算發散，有效提高了導航系統數據融合算法的穩定性。

實施3，結合附圖2，組合導航系統AHRS部分模型修正方案的步驟如下所示：

步驟1：採用三軸磁力計和三軸加速度計原始數據，計算出地理坐標系下的航向角，作為導航系統航向的觀測信息；

對電子羅盤的三軸原始數據進行傾斜補償，如式(7)所示：

航向角的計算如式(8)所示：

步驟2：組合導航系統姿態航向部分的模型構建

令Z軸MEMS陀螺的角速度輸入值為陀螺零偏為εZ，陀螺噪聲序列為nr,z。Z軸MEMS陀螺的模型如式(9)所示：

令k時刻陀螺的零偏為εZ，k-1時刻陀螺的零偏為零偏增量ΔεZ如式(10)所示：

航向部分狀態方程和觀測方程如式(11)所示：

系統的狀態量由航向角增量Δψ和ΔεZ構成，如式(12)所示：

令Z軸陀螺陀螺隨機遊走噪聲序列為nωw，系統的噪聲激勵序列Wyaw如式(13)所示：

Wyaw＝[nωr nωw]T (13)

系統的狀態轉移矩陣系統噪聲驅動陣Γyaw如式(14)所示：

系統的觀測量由電子羅盤航向角ψMag與上一時刻系統輸出航向角ψ差值構成，如式(15)所示：

系統的觀測轉移矩陣Hyaw如式(16)所示：

Hyaw＝[1 0] (16)

系統狀態噪聲方差陣Qyaw、量測噪聲方差陣Ryaw如式(17)所示：

步驟3：將舊AHRS模型中的航向修正部分移除，只保留姿態部分，如式(18)所示：

步驟4：求取濾波結果後，通過四元數求取橫滾和俯仰角，與修正後的航向角求取濾波後得到的四元數，完成本次濾波器觀測更新。

實施4，驗證試驗設計上，將導航系統靜止放置，在1.1秒至4.6秒、對電子羅盤進行鐵磁幹擾，記錄模型優化前後導航系統的姿態和航向信息，對方案的有效性進行驗證。模型優化前後的系統歐拉角波形附圖3所示。由附圖3可知，在電子羅盤受到鐵磁幹擾時，模型未優化前系統得橫滾角、俯仰角在磁幹擾穩定後分別產生了-13°和28°的穩態誤差。模型優化後，系統橫滾角、俯仰角在系統在電子羅盤受到鐵磁幹擾時，仍然維持初始角度。通過該實驗，驗證了AHRS模型優化後方案的有效性。

實施5，水下機器人在進行水下作業時，主要有定高、定深、定航向、勻速巡檢等模式，需要水下機器人導航系統提供滿足水下機器人控制精度需求的姿態、航向、速度、深度和高度信息。水下機器人在水中移動速度較慢，液壓系統和推進器運轉也會使水下機器人存在一定程度震動。考慮到測試環境限制，在本實驗中，將水下機器人導航系統安裝至運動載體，進行導航系統測試。採用GPS導航系統為組合導航系統的速度和水平位置提供參考，利用氣壓計傳感器提供高度參考。系統姿態、速度、位置數據輸出附圖4、附圖5、附圖6所示。