[發明專利]一種3D激光slam建圖中動態障礙物的后處理方法

在審

• 申請號: 202210216255.6
• 申請日: 2022-03-07
• 公開（公告）號: CN114638934A
• 公開（公告）日: 2022-06-17
• 發明（設計）人: 楚明龍;趙萬忠;王春燕;張恒于;張自宇;劉津強;劉利峰;周宇寧;張從余;朱耀鎏
• 申請（專利權）人: 南京航空航天大學
• 主分類號: G06T17/05
• 分類號: G06T17/05;G06T19/20
• 代理公司: 江蘇圣典律師事務所 32237
• 代理人: 賀翔

1.一種3D激光slam建圖中動態障礙物的后處理方法，其特征在于，步驟如下：

1)利用3D激光slam方法得到先驗地圖和建圖過程中激光雷達的行駛位姿；

2)對所述得到的先驗地圖中的點云信息進行重定義；

3)根據所述步驟1)中得到的激光雷達的行駛位姿和步驟2)中重定義后的點云信息對步驟1)中得到的先驗地圖進行網格劃分，得到可能存在動態點的區域；

4)根據所述步驟3)中網格劃分得到的可能存在動態點的區域篩選出動態障礙物點云；

5)根據所述步驟4)中的動態障礙物點云，區分動態點和地面點，將動態點移除同時對地面點進行擬合；

6)重復所述步驟5)逐步完成對整個地圖的動態障礙物剔除，最終得到不含動態障礙物的靜態地圖。

2.根據權利要求1所述的3D激光slam建圖中動態障礙物的后處理方法，其特征在于，所述步驟1)中先驗地圖為激光雷達掃描的點云數據逐幀累積構建而成的地圖；所述激光雷達的行駛位姿為行駛過程中激光雷達的位置和姿態。

3.根據權利要求1所述的3D激光slam建圖中動態障礙物的后處理方法，其特征在于，所述步驟2)中的先驗地圖中的點云信息為激光雷達掃描得到的周圍環境的三維位置信息的集合；

所述步驟2)中的點云信息重定義方法為：設P_t是一組在時間t處包含n個點的點云，P_t^M是先驗地圖中時間t處的點云，即全局坐標系下時間t處包含的點云信息，P_t^Q是時間t處的查詢幀上的查詢點云，即激光雷達坐標下時間t處包含的點云信息，T_t^Q是與P_t^Q關聯的SE(3)姿態變換矩陣；因此將先驗地圖中的點云信息進行重定義如下：

式中，T為總時間步長，[T]等于{1,2，...，T-1，T}，M為在世界框架上先驗地圖中所有被測量的點云信息；同時，P_t^M為全局坐標系下時間t處包含的點云信息，意味著P_t^M可通過T_t^Q的逆變換得到P_t^Q，即P_t^Q和P_t^M存在聯系，P_t^Q是查詢，P_t^M是映射。

4.根據權利要求3所述的3D激光slam建圖中動態障礙物的后處理方法，其特征在于，所述步驟3)中的先驗地圖網格劃分的方法為：以激光雷達為中心，ρ為半徑，角度為θ劃分先驗地圖，將其劃分為多個網格，將這些網格作為篩選動態障礙物的可能存在動態點的區域V_t；

V_t＝{P_k|P_k∈P_t,ρ_k-1＜ρ(P_k)＜ρ_k,θ_k-1＜θ(P_k)＜θ_k,ρ_k＜ρ_max}

式中，ρ_max為最大半徑，ρk-1，ρ_k分別為k-1和k處的半徑，θ_k-1，θ_k分別為k-1和k處的角度，P_k是k處的一組點云，ρ(P_k)為P_k的半徑，θ(P_k)為P_k的角度，P_t是一組在時間t處包含n個點的點云。