[發明專利]一種微型水下探測機器人自主導航系統

有效

• 申請號: 202210292210.7
• 申請日: 2022-03-24
• 公開（公告）號: CN114384939B
• 公開（公告）日: 2022-07-29
• 發明（設計）人: 徐盛果;白云;李娟;張仕穎;陶樹建;陳科祥;陸雪
• 申請（專利權）人: 江蘇深瑞光學技術有限公司
• 主分類號: G05D1/10
• 分類號: G05D1/10
• 代理公司: 南京鑫之航知識產權代理事務所(特殊普通合伙) 32410
• 代理人: 姚蘭蘭

1.一種微型水下探測機器人自主導航系統，其特征在于：包括：

第一處理器；

避障聲吶傳感單元，用于探測水下地貌和在水下機器人進行自主導航時掃描水下物體進行避碰；

磁力計，用于獲取磁場強度和方向；

慣性測量單元，用于獲取三軸姿態角、角速率以及加速度；

數據記錄儀，用于將所述避障聲吶傳感單元、磁力計、慣性測量單元獲取測量結果存儲；

模數轉換器，用于將模擬信號轉換成數字信號；

開入開出模塊，用于輸入輸出數據；

第二處理器；

與第二處理器連接的晶振；

與第二處理器連接的復位模塊；

其中，所述第二處理器與所述第一處理器通過EMIF總線連接；

第三處理器；

與第三處理器連接的晶振；

與第三處理器連接的復位模塊；

其中，所述第一處理器與所述第三處理器通過EMIF總線連接；

所述導航系統還包括最小方差估計模塊，所述最小方差估計模塊用于通過濾波器為系統誤差提供線性最小方差估計，并通過誤差的估計值修正導航系統；所述濾波器的輸入值獲取包括：通過磁位公式獲取各地磁場的標量磁位，

式中，a為地球半徑，r為地球徑向距離，g、h為地磁場球諧系數，為伴隨勒讓德函數，i、j為函數的階次，、分別為余緯和經度，=90-λ，λ為緯度，k為模型的截斷水平，n是地磁模型類型階數，選取1-15之間；

通過地磁公式獲取地磁分量；

根據機器人目前所處的經緯高得出當地的地磁分量M_N，M_E,M_H

同時從地理坐標系轉換至載體坐標系，可通過以下旋轉矩陣：

式中、、為機器人的滾轉角，俯仰角，航向角，通過上式即可得到安裝在機器人上的磁力計的理論地磁值，同時磁力計獲得實際測量值，將此值作為觀測輸入量輸入濾波器中；

所述導航系統通電時先進行初始化程序，檢測應用層程序的完整性，若應用層程序完整，則進入應用層程序與監控軟件數據交互并給第一處理器發送配置參數，否則進入底層BOOT循環等待監控軟件發送正確應用層程序，所述第一處理器在通電后等待DSP發送端口配置參數，則接收到應用層程序的配置參數時則建立數據連接；

所述濾波器在通電時，執行初始化，并初始對準指令，初始對準完畢，檢測導航解算標志是否有效，若有效捷聯導航解算，計算濾波器狀態轉移矩陣，同時計算噪聲矩陣，并離散化系統和噪聲矩陣，再檢測濾波器更新標志是否有效，若有效則計算量測量，更新濾波器數據，否則更新濾波器時間。

2.根據權利要求1所述的一種微型水下探測機器人自主導航系統，其特征在于：所述第二處理器通過共享RAM與第三處理器交互數據。

3.根據權利要求1所述的一種微型水下探測機器人自主導航系統，其特征在于：所述第二處理器和第三處理器為DSP架構，所述第一處理器為FPGA架構。

4.根據權利要求3所述的一種微型水下探測機器人自主導航系統，其特征在于：所述第一處理器通過SPI協議分別與慣性測量單元和磁力計連接，通過UART總線與避障聲吶傳感單元數據連接。