[發明專利]一種基于激光定位的動態障礙物剔除算法

在審

• 申請號: 201811029593.9
• 申請日: 2018-09-05
• 公開（公告）號: CN109085838A
• 公開（公告）日: 2018-12-25
• 發明（設計）人: 李勝;朱禹璇;郭健;吳益飛;龔勛;袁佳泉;施佳偉;危海明;趙超
• 申請（專利權）人: 南京理工大學
• 主分類號: G05D1/02
• 分類號: G05D1/02
• 代理公司: 南京理工大學專利中心 32203
• 代理人: 封睿

1.一種基于激光定位的動態障礙物剔除算法，其特征在于，包括如下步驟：

步驟1、激光雷達測距：利用激光雷達測量環境障礙物的距離；

步驟2、建立柵格地圖：將激光雷達檢測到的數據轉換為全局坐標，并映射到柵格地圖上，標記柵格單元格的障礙物占據情況；

步驟3、潛在動態障礙物的檢測：通過不同時刻柵格單元格的標記情況，確定靜態障礙物和潛在動態障礙物；

步驟4、不確定性處理：根據不同時刻潛在動態障礙物柵格鄰域的標記情況，確定動態障礙物；

步驟5、剔除動態障礙物：根據動態障礙物的位置信息，剔除動態障礙物。

2.根據權利要求1所述的基于激光定位的動態障礙物剔除算法，其特征在于，步驟1中，采用SICK公司生產的激光雷達LMS511，從右往左掃描障礙物，每隔0.5°返回一個距離值，掃描范圍為180°，共有361個距離數據。

3.根據權利要求1所述的基于激光定位的動態障礙物剔除算法，其特征在于，步驟2中，設激光雷達檢測的障礙物距離數據以極坐標的形式返回(ρ_i，α_i)，ρ_i是第i個障礙點到激光雷達的距離，α_i＝i×0.5°是第i個障礙點與機器人坐標系X^R軸的夾角，將極坐標映射到局坐標系中的公式為：

式中，(x_g,y_g)為障礙物在全局坐標系中的坐標，(x_r，y_r)為機器人在全局坐標系中的坐標；ρ為障礙物相對于激光雷達的距離；α為當前障礙點相對于機器人坐標系X^R軸的角度；θ為機器人坐標系X^R軸與全局坐標系X^G軸的夾角。

4.根據權利要求1所述的基于激光定位的動態障礙物剔除算法，其特征在于，步驟2中，將障礙物映射到柵格地圖上的具體公式為：

式中，(x_gm,y_gm)為柵格在全局坐標系中的坐標，(x_g,y_g)為障礙物在全局坐標系中的坐標，ω為單個柵格的寬度，通過上式即將激光雷達獲取的障礙物距離信息映射到柵格地圖上。

5.根據權利要求1所述的基于激光定位的動態障礙物剔除算法，其特征在于，步驟2中，柵格地圖將環境劃分成大小相等的柵格單元格c_i,j；每個柵格單元表示機器人工作環境中的一小塊區域，如果某個柵格單元格被障礙物占據，則標記c_i,j＝1；如果是自由區域，則標記c_i,j＝0。

6.根據權利要求1所述的基于激光定位的動態障礙物剔除算法，其特征在于，步驟3中，建立三個相鄰時刻傳感器的三幅柵格地圖，設t時刻的柵格地圖為M_t，t-1時刻的柵格地圖為M_t-1,t+1時刻的柵格地圖為M_t+1，根據同一對應柵格單元c_i,j在M_t-1，M_t，M_t+1上的占用情況判定靜態障礙物，如果都被占據，則確定該處為靜態障礙物，如果同一對應柵格單元c_i,j在M_t-1，M_t，M_t+1中任意一個或者兩個時刻被占據，則為潛在的動態障礙物，如果同一對應柵格單元c_i,j在M_t-1，M_t，M_t+1中均未被占據，則為自由區。

7.根據權利要求6所述的基于激光定位的動態障礙物剔除算法，其特征在于，步驟4中，采用8鄰域滾動窗口的方法從潛在動態障礙物中剔除靜態障礙物，即考慮三幅柵格地圖某個對應柵格單元是否被障礙物占有時，同時兼顧到周圍與其相鄰的8個柵格單元的占用狀態，如果相鄰三個時刻同一對應柵格單元c_i,j被占用，同時其周圍相鄰的八個柵格單元存在被占用的情況，則判定c_i,j為靜態障礙物，否則為動態障礙物。