[發明專利]一種水下無人航行器浮出水面檢測方法

在審

• 申請號: 202010963840.3
• 申請日: 2020-09-14
• 公開（公告）號: CN112050808A
• 公開（公告）日: 2020-12-08
• 發明（設計）人: 崔欣;高明;金長新
• 申請（專利權）人: 濟南浪潮高新科技投資發展有限公司
• 主分類號: G01C21/16
• 分類號: G01C21/16;G01D21/02
• 代理公司: 濟南泉城專利商標事務所 37218
• 代理人: 李桂存

1.一種水下無人航行器浮出水面檢測方法，水下無人航行器中安裝有溫鹽深傳感器、紅外傳感器、氧含量傳感器、六軸陀螺儀加速度傳感器以及用于采集各個傳感器數據的FPGA處理器，其特征在于，包括如下步驟：

a)當水下無人航行器上浮至離海面10米距離時，溫鹽深傳感器測量深度值為d1；

b)無人航行器在t1時刻內上浮至離海面小于1米時，溫鹽深傳感器測量深度值為d2，通過公式x₁＝10-at1²計算無人航行器的位置估計值x₁，式中a為無人航行器在t1時刻內上浮時的加速度；

c)通過公式計算位置估計值x₁的概率分布x₁′，式中N為正態分布，為位置方差，利用表示無人航行器的速度概率分布，v₁為加速度a積分解算出的速度，為六軸陀螺儀加速度傳感器的噪聲方差；

d)在t1時刻后的t2時刻，通過公式計算位置估計值x₁概率分布x₂′，式中a＝x₁-v₁，通過公式計算溫鹽深傳感器測量的t2時刻的深度值h₂的正態分布h₂′，式中為溫鹽深傳感器中的深度傳感器噪聲誤差方差；

e)跟卡爾曼濾波算法利用公式計算最優估計的概率分布b₂；

f)采用概率分布x₂′和正態分布h₂′的方差作為對數據的可相信權重，利用公式對概率分布b₂進行加權處理得到無人航行器當前位置的最優估計的概率分布c₂′

g)根據公式計算得到無人航行器當前位置的最優估計值c₂；

h)以最優估計值c₂作為當前位置，以nms為采樣時間間隔，重復執行步驟d)至步驟g)對接下來的若干時刻進行迭代運算得到若干位置預測的最優估計值，根據六軸陀螺儀加速度傳感器和溫鹽深傳感器測得的Z軸位置與若干位置預測的最優估計值進行加權求和得到綜合位置預測值d_n；

i)當根據公式f_n＝p×d_n+q₁×k+q₂×k+q₃×k+q₄×k計算f_n，式中p＝1-q₁-q₂-q₃-q₄，q₁為紅外傳感器的權重，q₂為氧含量傳感器的權重，q₃為溫鹽深傳感器中的溫度傳感器的權重，q₄為溫鹽深傳感器中的鹽度傳感器的權重，k為常數，k≤10cm；

j)當-0.001≤f_n＜0時判定為無人航行器已浮出水面。

2.根據權利要求1所述的水下無人航行器浮出水面檢測方法，其特征在于：步驟d)中t2時刻為t1時刻之后1ms的時刻。

3.根據權利要求1所述的水下無人航行器浮出水面檢測方法，其特征在于：步驟h)中n取值為1。