[發明專利]一種基于基線幾何結構約束的水下航行器定位方法有效
| 申請號： | 201811596305.8 | 申請日： | 2018-12-26 |
| 公開（公告）號： | CN109782289B | 公開（公告）日： | 2022-07-05 |
| 發明（設計）人： | 馬朋;胡安平;康建榮;王偉 | 申請（專利權）人： | 中國電子科技集團公司第二十研究所 |
| 主分類號： | G01S15/46 | 分類號： | G01S15/46;G01S7/52 |
| 代理公司： | 西北工業大學專利中心 61204 | 代理人： | 顧潮琪 |
| 地址： | 710068 *** | 國省代碼： | 陜西;61 |
| 權利要求書： | 查看更多 | 說明書： | 查看更多 |
| 摘要： | |||
| 搜索關鍵詞： | 一種 基于 基線 幾何 結構 約束 水下 航行 定位 方法 | ||
1.一種基于基線幾何結構約束的水下航行器定位方法，其特征在于包括下述步驟：
步驟1，三個具有自定位功能的水聲信標以固定時間間隔廣播自身位置(xi,yi)，i＝1,2,3，水下航行器利用接受到的水聲信號延遲時間Δti與區域水下平均聲速的乘積，來計算得到各個信標間的3D量測距離利用所攜帶深度計精確測量水下航行器深度值h，將3D量測距離li轉換為平面2D量測距離
步驟2，已知信標位置，計算兩兩信標間的距離Lij，即定位基線長度，j＝1,2,3且i≠j；利用定位基線間的幾何結構關系，構造信標與水下航行器當前位置(x,y)間的2D計算距離的等式約束條件S(ri2)＝0與不等式約束條件h(ri)≥J(ri)；
步驟3，在第k-1次定位結果基礎上，水下航行器利用慣性傳感器所測前向速度vk-1、航向角速度wk-1以及俯仰角速度υk-1，設定各傳感器定位采樣周期為δt，從而求得第k次預測定位信息包括位置、航向角以及俯仰角，即有
其對應于狀態估計與控制輸入uk-1＝[vk-1 wk-1 υk-1]T的雅可比矩陣和分別為：
利用所接收到的三個水面聲信標位置(xi,k,yi,k)以及測量得到的相對距離在距離量測誤差為εk的基礎上，建立水面信標與水下航行器間的相對距離量測方程
其對應于預測信息的雅可比矩陣
選擇最大后驗估計方法，實現等式約束條件與不等式約束條件h(ri,k)≥J(ri,)與相對距離相融合，并通過求解以下約束優化問題得到水下航行器位置、航向及俯仰估計值，即有
s.t.Σk＝Diag(Pk/k-1,Rk)
h(ri,k)≥J(ri,k)
i＝1,2,3
其中，Rk為距離量測誤差協方差，而狀態估計協方差的預測值Pk/k-1則借助于擴展Kalman濾波進行遞推求解，即
該專利技術資料僅供研究查看技術是否侵權等信息，商用須獲得專利權人授權。該專利全部權利屬于中國電子科技集團公司第二十研究所，未經中國電子科技集團公司第二十研究所許可，擅自商用是侵權行為。如果您想購買此專利、獲得商業授權和技術合作，請聯系【客服】
本文鏈接：http://www.szxzyx.cn/pat/books/201811596305.8/1.html，轉載請聲明來源鉆瓜專利網。
