一種用于水下自主導航與定位的方法與流程
本發明屬于水下機器人自主導航領域，特別涉及一種用于水下自主導航與定位的方法。
背景技術：
水下自主導航與定位主要是通過機器人實現，目前機器人中均采用的傳統SLAM(即時定位與地圖構建，下文簡稱SLAM)方法在密集雜波和特征數目多且變化劇烈的水下環境中存在數據關聯精度低、計算復雜的問題，且在SLAM實現過程中，由于機器人運動過程中的誤差、傳感器的觀測誤差、以及傳感器不確定性造成地圖特征漏檢等問題，都將影響數據關聯過程中的準確性，所以傳統SLAM方法的準確性對地圖特征信息的數據關聯非常敏感。
基于隨機有限集合理論的概率假設密度SLAM方法是一種在估計地圖特征時不依賴數據關聯，并將傳統SLAM方法中所忽略的由于傳感器不確定性造成的地圖特征漏檢概率帶入地圖特征濾波的SLAM新方法，非常適用于數據關聯模糊、目標特征多且變化劇烈的水下環境中。但由于概率假設密度(Probability Hypothesis Density,下文簡稱PHD)算法的特性，需要提供一定先驗信息才能遞歸進行濾波運算，而SLAM過程是在未知環境中從未知位置進行運動，無法提供先驗信息，為此傳統方法是將移動機器人上一時刻的觀測集合作為當前時刻的先驗信息集合，帶入PHD濾波當中從而估計出新出現地圖特征的位置。然而，由于新生目標集合僅僅采用上一時刻的觀測集合，因此在地圖特征位置及數目的估計精度上存在不足。
技術實現要素：
發明目的：為了克服現有傳統SLAM方法在密集雜波和特征數目多且變化劇烈的水下環境中存在數據關聯精度低、計算復雜的不足以及現有隨機有限集SLAM方法中先驗信息不足的問題，本發明提供了一種數據關聯精度高，計算簡單的用于水下自主導航與定位的方法。
技術方案：本發明提供了一種用于水下自主導航與定位的方法，包括以下步驟：
步驟1：將機器人反饋的每一時刻地圖特征信息、地圖特征觀測信息以及傳感器不確定造成的漏檢概率都建模為隨機有限集形式；
步驟2：使用Rao-Blackwellised粒子濾波器進行機器人位姿估計得到機器人位姿先驗估計的粒子集合；
步驟3：根據步驟2中獲得的機器人先驗估計的粒子集合對每個粒子所對應的機器人位姿進行地圖估計，完成對地圖信息的預測和更新；
步驟4：根據步驟3獲得的PHD濾波器對每個粒子地圖估計結果調整粒子權重,得到調整過后的粒子集后，通過采樣粒子的加權平均獲得k時刻的機器人位姿，從而實現導航和定位。
進一步，所述步驟3中對每個粒子所對應的機器人位姿進行地圖估計的方法為：包括以下步驟：
步驟301：地圖預測，根據公式Vk|k-1(m|Xk)＝Vk-1|k-1(m|Xk-1)+b(m|Xk)得到k時刻預測可能出現的地圖特征隨機有限集合(Random Finite Set,下文簡稱RFS)的PHD；其中，Vk-1|k-1(m|Xk-1)表示k-1時刻根據機器人位姿Xk-1得到的后驗地圖特征信息RFS的PHD，b(m|Xk)表示隨著機器人運動可能進入機器人視域內新生地圖特征信息RFS的PHD；初始時刻，使用k-1時刻觀測集合作為k時刻可能出現新地圖特征信息；當機器人經過PHD-SLAM更新出已探測區域地圖信息后，將已探測地圖信息中靠近機器人位姿的地圖特征作為先驗信息，加入上一時刻觀測集合中一起作為當前時刻的b(m Xk)帶入當前時刻的PHD預測階段；
步驟302：更新階段，根據公式：
更新地圖信息，式中Vk|k(m|Xk)表示k時刻后驗地圖特征信息RFS的PHD，Λ(m|Xk)＝PD(m|Xk)gk(z|m,Xk)，PD(m|Xk)表示位姿為Xk的機器人能夠探測到位于m處地圖特征的概率，ck(z)表示k時刻由雜波引起的雜波RFS的PHD，gk(z|m,Xk)表示機器人攜帶的傳感器模型；
步驟303：對更新階段PHD濾波器的結果進行剪切與合并。這樣有效提高了對地圖特征點位置和數目的估計精度及地圖濾波運算速度。
進一步，所述步驟4中調整每個粒子權重大小的方法為對粒子的重要性采樣，對每個粒子對應的地圖特征進行濾波，將得到的結果與機器人采集到的地圖特征進行比較，如果比較的結果誤差越小，就表示粒子與機器人的位置越準確，從而增加對應粒子的權重；如果相反，就減小對應粒子的權重。這樣得到的定位更加的精準。
進一步，所述步驟4中還包括粒子重采樣。這樣有效解決了粒子退化的問題。
有益效果：與現有技術相比，本發明使用隨機有限集方法建模實現SLAM方法，解決了傳統SLAM方法在密集雜波和特征數目多且變化劇烈的水下環境中存在數據關聯精度低、計算復雜的問題。使用了混合新生地圖信息方法，增加了地圖預測更新過程中PHD濾波器的先驗信息，當機器人重新回到已探測過的環境中時，通過增加先驗信息，改進PHD-SLAM方法對地圖特征數目估計及地圖特征位置精度估計。使用空地圖特征策略提高機器人位姿后驗估計的運算速度。從而使SLAM方法能夠在更復雜的環境中得到應用。
附圖說明
圖1為本發明工作流程圖；
圖2為本發明中混合新生地圖信息預測階段流程圖；
圖3為本發明中提供的實施例的具體實施方式流程圖。
具體實施方式
下面結合附圖對本發明做更進一步的解釋。
如圖1所示，本發明公開的一種用于水下自主導航與定位的方法，主要包括以下步驟：
步驟1、將機器人反饋的k時刻地圖特征信息、地圖特征觀測信息以及傳感器不確定造成的漏檢概率都建模為隨機有限集形式。地圖特征集合的數目從零到任意大小隨意變化，隨著在機器人不斷出現在機器人視域內的地圖特征而單調增加，每個地圖特征之間是相互獨立的。
M＝φ(沒有地圖特征)
M＝{m1}(一個狀態為m1的地圖特征)
M＝{m1,m2}(兩個地圖特征m1≠m2)
M＝{m1,...,mn}(n個地圖特征m1≠...≠mn)
SLAM過程中地圖特征觀測信息集合不僅包括實際地圖特征還包括雜波、虛警，因此觀測集合的數目同樣是從零到任意大小隨意變化。每個觀測信息包含傳感器獲得地圖特征的距離及角度。
Z＝φ(沒有地圖特征被探測)
Z＝{z1}(一個狀態為z1被探測)
Z＝{z1,z2}(兩個地圖特征被探測)
(個地圖特征被探測)
步驟2、在隨機有限集模型下，將k時刻SLAM聯合后驗概率估計轉換為兩個“獨立”的估計過程，即機器人位姿估計，以及該狀態估計條件下的環境地圖估計。機器人位姿估計使用RB粒子濾波器進行位姿估計，根據機器人速度v，k-1時刻的控制量Uk-1，前一狀態位置Xk-1，及機器人運動方程得到機器人先驗估計
步驟3、得到每個粒子的先驗估計后，對每個粒子所對應的機器人位姿進行地圖估計，完成對地圖信息的預測和更新。地圖估計包括三個階段，1.地圖預測階段，2.更新階段，3.PHD濾波器剪切與合并。
(1)地圖預測階段：
Vk|k-1(m|Xk)＝Vk-1|k-1(m|Xk-1)+b(m|Xk) (1)
式中Vk-1|k-1(m|Xk-1)表示k-1時刻根據機器人位姿Xk-1得到的后驗地圖特征信息RFS的PHD，Vk|k-1(m|Xk)表示k時刻預測可能出現的地圖特征RFS的PHD，b(m|Xk)表示隨著機器人運動可能進入機器人視域內新生地圖特征信息RFS的PHD，m表示機器人位姿為Xk狀態下的地圖特征。
在估計地圖特征目標數目以及地圖特征精度中，地圖預測階段起重要作用，本發明使用的混合新生目標信息的方法，增加預測階段中的新生地圖特征RFS的PHD。如圖2所示，當機器人從初始時刻出發時由于缺乏先驗信息，使用k-1時刻的觀測信息集合作為k時刻可能出現新地圖特征信息。當機器人經過PHD-SLAM更新出已探測區域地圖信息后，將已探測地圖信息中靠近機器人位姿的地圖特征作為先驗信息，加入上一時刻觀測集合中一起作為當前時刻的b(m|Xk)帶入當前時刻的PHD預測階段。
b(m|Xk)＝Zk-1+Mf (2)
式中，Zk-1表示k-1時刻的地圖特征觀測集合，如果當前時刻為k時刻，k-1則表示上一時刻；Mf表示已探測地圖中靠近機器人位姿的地圖特征的RFS的PHD，ν表示機器人的速度，R表示機器人所攜帶傳感器掃描半徑，M表示經過PHD-SLAM方法得出的已探測地圖特征信息。根據機器人速度ν和傳感器掃描半徑R，確定出一個閾值T，閾值T為機器人速度ν和傳感器掃描半徑R的和；當已探索過的地圖特征集合內有地圖特征位置與機器人位置之間距離小于這一閾值時，將該地圖特征作為可能出現在當前時刻機器人視域內的新地圖特征，γ(m|Xk-1)表示已探測地圖特征中小于閾值的地圖特征形成的RFS的PHD。
混合新生地圖信息策略中的b(m|Xk)不僅包括k-1時刻的觀測集合還加入了已探測區域內地圖信息中靠近機器人位姿的地圖特征集合，當機器人視域內已探測過的地圖特征重新出現時，加入b(m|Xk)中作為先驗信息的這部分地圖特征，經過PHD-SLAM地圖濾波后，重新出現的地圖特征的權重會增加，從而提高了對地圖特征點位置和數目的估計精度。
(2)更新階段：
式中Vk|k(m|Xk)表示k時刻后驗地圖特征信息RFS的PHD，
Λ(m|Xk)＝PD(m|Xk)gk(z|m,Xk)，PD(m|Xk)表示位姿為Xk的機器人能夠探測到位于m處地圖特征的概率，ck(z)表示k時刻由雜波引起的雜波RFS的PHD，gk(z|m,Xk)表示機器人攜帶的傳感器模型，Zk表示k時刻的地圖特征觀測集合，ξ為一個中間變量，沒有具體含義。
更新階段用以確定真實地圖特征所在位置，利用傳感器獲得地圖特征觀測集合與預測階段的地圖特征進行后驗概率估計確定地圖特征所在的位置。
(3)PHD濾波器剪切與合并
預測階段及地圖特征觀測集合中包含很多虛警及雜波，這些虛警及雜波在更新階段會產生大量權重很小無效的RFS，從而增加下一步SLAM過程的運算速度，因此需要對更新階段PHD濾波器的結果進行剪切與合并。
步驟4、根據步驟3PHD濾波器對每個粒子地圖估計結果，調整每個粒子權重大小。得到調整過后的粒子集后，k時刻的機器人位姿通過采樣粒子的加權平均獲得，從而實現導航和定位。
如圖3所示，本發明提供的實施例中采用Rao-Blackwellised粒子濾波器實現機器人位姿后驗估計。地圖信息采用高斯混合概率假設密度進行預測和更新。具體包括如下步驟：
步驟1：將SLAM問題描述建模為隨機有限集形式。
1)地圖特征信息集合
PHD-SLAM方法的觀測信息以及地圖特征不再采用向量序列的形式表示，而是使用隨機有限集合形式來表示。
式中，Mk表示k時刻機器人探索過的地圖特征集合，Mk-1表示截止到k-1時刻機器人已經探索過的地圖特征集合，FoV(Xk)表示在k時刻，位姿為Xk的機器人的視域范圍內地圖特征的集合，表示不在Mk-1地圖內的地圖特征集合。
2)地圖特征觀測集合
在真實情況中，由于傳感器的漏檢以及傳感器自身產生的虛假測量，真實地圖特征觀測的產生是無序的，特征與特征之間沒有內在關聯，特征觀測的數目也是無法確定的，因此使用隨機有限集合的形式可以更準確的表示地圖特征觀測，在k時刻，位姿為Xk的機器人獲得的特征觀測集合表示為Zk，其RFS模型為
式中Dk(m,Xk)表示位于m處的位姿為Xk的機器人由真實地圖特征產生的測量RFS，Ck(Xk)表示雜波產生的測量RFS。中測量數目隨意變化，且每個測量值都是無序排列。
Dk(m,Xk)建模為一個Bernoulli型RFS，Dk(m,Xk)＝φ的概率為1-PD(m|Xk)，即真實地圖特征沒有被傳感器檢測到的概率為1-PD(m|Xk)；Dk(m,Xk)＝{z}的概率密度為PD(m|Xk)gk(z|m,Xk)，即真實地圖特征能夠被傳感器檢測到的概率為PD(m|Xk)gk(z|m,Xk)；其中PD(m|Xk)表示當機器人位姿為Xk時，能夠探測到位于m處的地圖特征概率。gk(z|m,Xk)表示機器人攜帶的傳感器模型。
步驟2：PHD-SLAM使用RB粒子濾波器實現機器人先驗位姿估計：
機器人位姿由不同權值的粒子集表示，即其中，表示第i個粒子在k時刻的權重，表示第i個粒子在k時刻所代表的機器人位姿，N表示粒子集中粒子的總數，i表示粒子的編號；機器人的狀態服從狀態轉移方程，則每個粒子位姿的先驗分布表示為其中表示第i個粒子從0時刻到k-1時刻所表示的所有機器人位姿，U0:k-1表示從0時刻到k-1時刻所有的控制量，X0表示機器人初始位姿。
步驟3：使用GMPHD濾波器實現每個粒子對地圖信息預測更新：
1)預測階段
根據每個粒子采樣位置，可以得到k時刻新出現特征的高斯混合項：
表示機器人新探測出現的特征的高斯量的數量，分別表示第i個粒子中的第j個高斯量的權重，均值和方差；b表示機器人新探測出現的特征。
對預測階段的采用混合新生地圖信息策略，不僅包括上一時刻的觀測集合形成的高斯項，還包括已探測地圖信息中地圖特征位置與機器人位置距離小于閾值T的地圖特征的高斯項。
第i個粒子所對應的先驗地圖的PHD表示為其高斯混合表達式為：
式中表示上一時刻經過PHD濾波后得到高斯量個數，分別表示高斯量對應的權重均值和方差，表示第i個粒子k-1時刻的位姿。
根據公式(1)可知，地圖預測的高斯混合PHD表示為:
其中，表示k時刻地圖特征預測的高斯混合項包括k時刻新出現的地圖特征高斯混合項，以及k-1時刻PHD濾波后得到的高斯混合項。
2)更新階段
每個粒子的觀測似然函數也采用高斯混合的形式，更新后地圖的后驗PHD同樣具有高斯混合的形式
其中μk|k,Pk|k,Sk通過基于EKF-PHD濾波器更新獲得。
3)PHD的剪切、合并與地圖特征的提取
觀測集合中存在大量虛假觀測，地圖特征經過PHD濾波后虛假觀測將產生許多權重極小的高斯分量，這些高斯分量對地圖的估計沒有幫助，但會大大影響整個地圖濾波的計算速度，因此對于高斯權重小于Tp的高斯分量丟棄，本實施例中Tp＝10-5，另一方面每個粒子由于自身位置的誤差對每個真實地圖特征的估計也會產生偏差，因此在狀態提取之前需對經過剪切后，高斯分量相互位置距離小于門限Tm的高斯分量進行合并，提高地圖估計的精度。PHD剪切、合并完成后，對高斯分量權重大于地圖特征提取門限Tf的高斯分量進行地圖特征提取，其高斯項的均值表示地圖特征的位置，滿足門限Tf高斯分量的個數表示地圖特征的數目。
步驟4：利用步驟3獲得的PHD濾波器對每個粒子地圖估計結果調整粒子權重,得到調整過后的粒子集后，通過采樣粒子的加權平均獲得k時刻的機器人位姿。
1)粒子重要性采樣：
機器人位姿的后驗分布由不同權值的粒子集表示，即表示k時刻第i個粒子的權重，表示第i個粒子對應的位姿。權重的遞推公式為：
是權重遞歸的關鍵，對于計算使用結構簡單，計算量小的空地圖策略，的計算公式為：
式中表示k時刻雜波形成PHD，表示更新后的地圖特征數目，表示預測地圖特征數目，λc表示雜波的個數。
2)粒子重采樣：
為解決粒子退化問題，需要進行粒子重采樣，粒子重采樣完成后得到新的粒子集
以上所述僅為本發明的較佳實施例而已，并不用以限制本發明，凡在本發明的精神和原則之內，所做的任何修改、等同替換、改進等，均應包含在本發明保護的范圍之內。
- 還沒有人留言評論。精彩留言會獲得點贊！