[發明專利]基于虛擬掃描與測距匹配的AGV激光SLAM方法

有效

• 申請號: 201710504910.7
• 申請日: 2017-06-28
• 公開（公告）號: CN107239076B
• 公開（公告）日: 2020-06-23
• 發明（設計）人: 仲訓昱
• 申請（專利權）人: 仲訓昱
• 主分類號: G05D1/02
• 分類號: G05D1/02
• 代理公司: 廈門南強之路專利事務所(普通合伙) 35200
• 代理人: 馬應森

1.基于虛擬掃描與測距匹配的AGV激光SLAM方法，其特征在于包括以下步驟：

1)柵格地圖表示與創建方法：采用柵格表示柵格地圖M，每個柵格為正方形，其長寬為Δ≤定位精度指標，對于某個柵格m_i為障礙時，定義其標志位s_i＝1，為非障礙時s_i＝-1、不確定時s_i＝0，其列行編號為(x_i,y_i)，柵格地圖M表示為：

M＝{m₁(x₁,y₁,s₁),m₂(x₂,y₂,s₂),...,m_i(x_i,y_i,s_i),...,m_N(x_N,y_N,s_N)}

2)虛擬掃描與匹配定位方法：AGV機器人每運行一步后，通過激光雷達獲得當前的測距數據L，再結合之前建立的柵格地圖M，進行當前位姿的估算，即定位；

虛擬激光雷達掃描如下：

采用局部遍歷的方式，先設置一定的位置和角度遍歷范圍Ω，在所有的遍歷位姿，假設為可能的激光雷達位姿，模擬激光雷達，逐一對柵格地圖M進行虛擬掃描；在某一遍歷位姿(vx_i,vy_i,vθ_i)，獲得虛擬掃描數據L_i如下：

L_i＝{vl_i1(vd_i1,vθ_i1,va_i1),...,vl_ij(vd_ij,vθ_ij,va_ij),...,vl_in(vd_in,vθ_in,va_in)}

在虛擬掃描的vθ_i1方向獲得測距vd_i1，若vd_i1d_max，則令va_i1＝1；否則va_i1＝-1；在vθ_i2方向獲得測距vd_i2，若vd_i2d_max，則令va_i2＝1；否則va_i2＝-1；...；在vθ_in方向獲得測距vd_in，若vd_ind_max，則令va_in＝1；否則va_in＝-1；

以方向vθ_ij時，采用逐步推進的方式獲得測距vd_ij，其過程如下：從起始點(vx_i,vy_i)開始，沿相對于vθ_i角度為vθ_ij的方向，以長度Δ，或更小，為增量向前推進，每推進一次計算一次到達點對應的柵格，直到所到達點柵格的標志位為1則停止推進，若推進距離dmax，則同樣停止推進，此時起始點(vx_i,vy_i)到該柵格的推進距離即為vd_ij，若vd_ijd_max，則令va_ij＝1；否則va_ij＝-1；

遍歷方式時，逐一遍歷Ω中的每個柵格點，遍歷到每個柵格點時再逐一遍歷每個角度值，若Ω的位置范圍為10×10＝100個柵格，角度范圍為5°/0.1°＝50個角度值，則總的遍歷次數為100×50＝5000＝K，遍歷完后獲得K個虛擬掃描數據VSs(Ω)＝{L₁,L₂,...,L_i,...,L_K}；

基于測距的輪廓匹配為：

采用基于測距的輪廓匹配方式進行定位，將VSs(Ω)中所有的虛擬掃描數據與當前的真實測距數據L進行比較，找出與L的測距數據最接近的虛擬掃描數據L*，即：

其中d_min≥0，根據具體應用環境來設定；L*所在的遍歷位姿(vx*,vy*,vθ*)即為激光雷達當前位姿的最優估計值，即：

x_r＝vx*,y_r＝vy*,θ_r＝vθ*；

3)提高算法實時性的方法：基于虛擬掃描與測距匹配的SLAM方法先用于離線建圖，然后用于AGV機器人工作時的即時定位，其中，虛擬激光雷達掃描的計算量最大，每次遍歷時都需要模擬激光雷達在其n個掃描方向上進行推進測距，采取以下措施：

(1)采用多GPU并行處理方式，每個GPU運行一個掃描方向上模擬測距的推進測距算法；

(2)對于第i個掃描方向，改變虛擬測距方向vθ_i上的初始推進位置，從小于真實測距離d_i的某個位置開始，即不從遍歷位置開始，推進時遇到障礙柵格或到距離大于d_i的某個位置則停止推進；

所述虛擬掃描方向vθ_i對應的GPU推進測距算法如下：

算法輸入：遍歷位姿(vx,vy,vθ)，柵格地圖M，測距數據L

算法輸出：虛擬掃描方向上的測距vd_i，標志位va_i