[發明專利]水下機器人自主定位與節點地圖構建方法

有效

• 申請號: 201710851937.3
• 申請日: 2017-09-19
• 公開（公告）號: CN107589749B
• 公開（公告）日: 2019-09-17
• 發明（設計）人: 冀大雄;孫明哲;宋偉;朱世強
• 申請（專利權）人: 浙江大學
• 主分類號: G05D1/06
• 分類號: G05D1/06
• 代理公司: 杭州中成專利事務所有限公司 33212
• 代理人: 周世駿

本發明涉及水下機器人導航技術領域，旨在提供一種水下機器人自主定位與節點地圖構建方法。本發明提出一種新型的節點定位與水下機器人自主定位方法，即在移動節點只能獲得自身與隨機節點的位置關系時，對所有節點進行定位，獲得節點地圖并同時對AUV進行定位。該方法能夠確定較大范圍內隨機布放的節點位置，獲得節點地圖；并保證AUV的位置坐標誤差在一個較小的范圍內。本發明只需獲得AUV在每個最優估計位置上與未知節點的距離即可，無需獲得二者之間的角度關系；相較于傳統方法，只利用聲吶進行測距，大大降低測量成本。能滿足在大范圍海域內的工作需求，可保證自身定位誤差，以及保證節點地圖的節點定位平均誤差小。

技術領域

本發明屬于水下機器人導航技術領域，設計一種利用水下機器人(AutonomousUnderwater Vehicle，AUV；以下簡稱AUV)對無線傳感器網絡節點進行定位并同時確定自身位置的方法。

背景技術

隨著海洋的開發，無線傳感器網絡技術被廣泛的應用于海洋生態狀況監控、海洋地質信息采集以及航行器定位等領域中。由于布設成本所限，無法對網絡中每個節點進行事先預設以精確布置，但實際使用中有要求所有節點位置已知，因此解決無線傳感器網絡節點定位問題是無線傳感器網絡應用的關鍵。

為了解決此問題，研究人員提出包括凸優化、MDS-MAP、DV-Hop等多種傳統的無線傳感器網絡節點定位方法，利用信標間的通信關系構建整體無線傳感器網絡節點位置關系。這種方法在通信質量高的環境下具有較高的準確度，但由于海洋中的特殊環境導致各種通信干擾，使得節點定位問題面臨兩難情況：1)各節點間在保證足夠的通信質量下需求較高的通信成本；2)由于無線傳感器網絡節點數量多，無法滿足較高的通信成本。這些直接導致這些傳統的無線傳感器網絡節點定位方法無法直接用于海洋中。

目前為了解決上述問題，研究人員提出引入即時定位與地圖構建(simultaneouslocalization and mapping，SLAM；以下簡稱SLAM)方法對節點進行定位。即引入移動節點(如AUV)，采用SLAM方法對移動節點進行即時定位，利用移動節點獲取隨機節點相對于自身的距離和角度，對無線傳感器網絡節點進行定位，得到節點地圖，再利用地圖對移動節點進行后續定位。這種方法從理論上基本是可行的，但是在實際使用情況中移動節點只能獲得自身與隨機節點的位置關系而無法獲得角度關系，直接導致無法確定隨機節點位置，導致方法整體不可行。

發明內容

本發明要解決的技術問題是，克服現有技術中的不足，提供一種水下機器人自主定位與節點地圖構建方法。

為解決技術問題，本發明的解決方案是：

提供一種水下機器人自主定位與節點地圖構建方法，包括以下步驟：

(1)通過水下機器人AUV搭載的傳感器獲取其自身角度、距離參數，用于計算移動誤差小，并作出下一時刻的位置預測：

上式中的遞歸公式是利用貝葉斯濾波的方式得到機器人在t時刻的位姿的后驗概率；式中是t時刻AUV的預測位置，X_t-1是t-1時刻的AUV的真實位置，P(·)表示隨機變量的概率密度函數，P(·|·)表示條件概率密度函數(后文P(·|·)均表示條件概率，不再贅述)；

利用粒子濾波器對后驗概率進行近似估計，在t時刻時每個粒子位置的后驗概率分布滿足：

上式中，為第j(j＝1,2,…,m)個粒子在t時刻的位置，U_t-1是t-1時刻的AUV的控制量；

(2)在狀態預測方程中加入觀測值Z_t-1，利用觀測量修正AUV的預測值得到更為精確的概率分布并作為提議分布位置：