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

有效

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

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

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

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

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

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

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

式中表示在已知t時刻AUV的預測位置與t時刻AUV的觀測位置時AUV在t時刻的真實位置的概率分布情況；

(3)由于觀測存在誤差，其觀測位置亦與X_t-1，U_t-1相關，由觀測獲得目標分布為：

(4)由于每個粒子的位姿不一，則其重要程度也會不一致，因此在對粒子的預測位姿進行更新的同時，對t時刻第j個粒子計算其權重

(5)得到每個粒子在加入觀測值后的一個權重之后，根據其權重大小，將其中權重小于0.4的粒子丟棄，并對剩下的粒子進行歸一化：

式中是歸一化后的例子權重，M是剩下粒子數；

(6)將粒子歸一化之后，對粒子進行加權求和得到AUV的最優估計位置：

式中是最優估計位置，第j(j＝1，2，…，m)個粒子在t時刻的位置；

同時利用聲吶或超聲波測量掃描周圍所有未知節點的位置，獲得AUV與所有能掃描到的未知節點的距離；

(7)由于在步驟(5)中丟棄了部分粒子，會導致隨著計算過程中粒子數目逐漸減少；為保證粒子的有效數目，設定判斷變量N，N＝m/2，m為初始粒子總數；當M＜N時則表明粒子數太少，需要對機器人的位置進行重采樣；重采樣過程為：

粒子數：M→m；粒子分布：即粒子分布不變；

式中表示重采樣前M個粒子的分布情況，表示重采樣后N個粒子的分布情況；

(8)利用凸優化方法，根據AUV在每個時刻采集的隨機信標節點的信息，進行節點位置計算；

假設已知節點a_k與未知節點x_j之間的測距誤差為w_kj，測量距離為d_kj，真實距離為d(a_k，x_j)；那么就有：

||a_k-x_j||²＝d(a_k，x_j)，d(a_k，x_j)+w_kj＝d_kj (1-8)

基于實際的測量情況，測量誤差利用正態分布表示，即：

式中表示測量方差；

由于每次測量之間是獨立的，所以利用每次測量信息得到的X的最大似然函數L表示為：

其似然估計表示為：

所以利用最大似然估計結果建立數學規劃模型：

將上述式子松弛為半定規劃得到：

其中定義e_i∈Rⁿ代表R^n×n中第i列，并且定義對稱矩陣0∈R^d是一個全零向量；I_d是d階單位陣；Z_{(1：d，1：d)}表示矩陣Z的左上角d階主子陣，矩陣之間的點乘代表每個元素的乘積之和A·B＝∑_ijA_ijB_ij，Z≥0表示對稱矩陣Z是半定矩陣；

其中矩陣Z∈R^(d+n)×(d+n)能分解成子矩陣：

式中X表示未知節點的坐標位置，Y表示一個矩陣塊，無特殊意義；

(9)對水下機器人進行自主定位與節點地圖構建：

A、在AUV初始位置周圍布放4～6個已知位置的節點，對AUV的初期運動進行導航；將通過步驟(6)獲得的AUV的最優估計位置視為已知節點位置a_k，即AUV初始位置為a₁，下一時刻為a₂，以此類推；對依次掃描到的未知節點進行編號并視為x_j，即AUV掃描測距到的第一個未知節點x₁，下一個為x₂，以此類推；在a_k所測得AUV距未知節點x_j的距離為d_kj；在移動中不斷檢測所剩粒子，當剩余粒子少于初始粒子數50％時按步驟(7)所述內容進行重采樣；

B、在初始已知位置節點的定位下，AUV通過一段時間的移動獲得足量的關于未知節點x_j的距離信息d_kj；按照步驟(8)中公式(1-13)進行求解，解得半定矩陣Z；矩陣Z中的X分量即為未知節點位置，即X＝[x₁，x₂，…，x_j]，將求得的這部分節點的位置加入到地圖中，實現節點地圖構建；

C、根據前一步驟B所構建的地圖，利用該步驟中所求的節點位置對AUV進行后續導航，實現水下機器人的自主定位；同時利用通信，獲得AUV與所有能掃描到的未知節點的位置；

D、重復步驟B、C，直至節點地圖構建完成，或者水下機器人到達目標位置。