一種簡單、直接的無人車船 鐳射雷達 導航、路徑規劃方法

如何使用鐳射雷達（或者深度相機）進行路徑規劃是很多同學比較關心的（比如我），我前一段時間恰好把這件事做了一下，此帖也記錄一下自己的工作流程，思路。程式碼的話，感興趣的話可以私聊，不過我覺得整個工作流程要是明白了，程式碼也沒啥難的。

整體的思路

對於無人車（船）而言，路徑規劃中常用的是二維的佔據柵格地圖，對應ros中的資料格式nav_msgs/OccupancyGrid。對於室內機器人而言，使用單線鐳射雷達（2D鐳射SLAM），可以直接構建柵格地圖，這是很方便的。但對於室外執行的機器人，使用多線鐳射雷達，其SLAM構建的是點雲地圖，然後點雲地圖這個東西是沒辦法直接用於路徑規劃的，需要繞個彎，這也是使用多線鐳射雷達進行路徑規劃的核心：

Lidar點雲生成佔據柵格地圖流程圖

在構建了佔據柵格（nav_msgs/OccupancyGrid）之後，想進行路徑規劃，還需要完成依靠感測器在先驗地圖中的重定位工作，完成這項工作也有幾種方法，這個後文再仔細展開。

1、關於slam演算法

我自己有的感測器包含（Lidar + 差分GPS（輸出經緯度+航向角））。在此處選用A-LOAM這個入門級的鐳射SLAM演算法來給Lidar進行定位。打算之後再加入IMU。

在開原始碼的基礎上進行了小部分的修改，主要是因為GPS的加入，在使用GPS時使用GeographicLib：：LocalCartesian，生成的是ENU系，而SLAM的座標系是第一幀鐳射對應的Lidar系，它們之間有一個變換矩陣需要標定，這樣GPS輸出的經緯度資訊才能與slam的map系進行轉換，這個標定方法也比較簡單，其實就是將兩端軌跡對齊，可以參考這篇文章：Intermittent GPS-aided VIO： Online Initialization and Calibration。哈哈哈哈哈哈不過在我自己做的時候我偷懶了，僅僅使用差分GPS輸出的航向角完成ENU與map系的標定（這個小範圍場景玩玩還行，主要是旋轉不準的話，場景一大GPS對應的ENU系座標透過變換矩陣變到map系就差很多。。。。。挖個坑之後把LIO整好就整）。

2、關於octomap的維護以及更新

鐳射點雲生成佔據柵格地圖核心就是知道哪些地方被佔據，哪些地方空閒。這個東西最簡單的實現方式就是點雲的體素化。如圖：

點雲的體素化

但簡單的點雲的體素化是不夠優雅的，實際使用的東西是八叉樹地圖（octomap）這個東西。八叉樹地圖的介紹可以參考高翔博士的部落格。同時也有一個開源的c++ 庫octomap，不過這個庫有點坑，沒有文件，基本只能是看看開原始碼咋寫然後依葫蘆畫瓢。

八叉樹地圖的維護中有一個比較重要的東西：sensor_origin到（已經轉換到世界系的）當前幀的每一個點進行光線追蹤，末尾的柵格為hit，其餘的柵格為miss。

ray-casting

每一個柵格都會儲存一個value，hit的柵格對應的 value += A，miss的柵格對應的value -= B。

如果一個柵格的 value > occupied_threshold，則認為被佔據，value

void Gen_grid_map：：ray_tracing（）{

free_cells 。clear（）；

occupied_cells。clear（）；

octomap：：KeyRay key_ray；

for（int i = 0；i < non_ground_cloud_in_map->size（）；i++）{

octomap：：point3d non_ground_point = octomap：：point3d（non_ground_cloud_in_map->points［i］。x，non_ground_cloud_in_map->points［i］。y，non_ground_cloud_in_map->points［i］。z）；

if（tree->computeRayKeys（sensor_origin，non_ground_point，key_ray））{

free_cells。insert（key_ray。begin（），key_ray。end（））；

}

octomap：：OcTreeKey end_key；

if（tree->coordToKeyChecked（non_ground_point，end_key））{

occupied_cells。insert（end_key）；

}

}

for（int i = 0；i < ground_cloud_in_map->size（）；i++）{

octomap：：point3d ground_point = octomap：：point3d（ground_cloud_in_map->points［i］。x，ground_cloud_in_map->points［i］。y，ground_cloud_in_map->points［i］。z）；

if（tree->computeRayKeys（sensor_origin，ground_point，key_ray））{

free_cells。insert（key_ray。begin（），key_ray。end（））；

}

}

for（octomap：：KeySet：：iterator it = occupied_cells。begin（） ； it ！= occupied_cells。end（）；it++）{

tree->updateNode（*it， true）；

}

for（octomap：：KeySet：：iterator it = free_cells。begin（） ； it ！= free_cells。end（）；it++）{

//對於本來沒有被佔據的柵格才更新為free

//octomap：：OcTreeNode* node = tree->search（*it）；

if（occupied_cells。find（*it） == occupied_cells。end（））

{

tree->updateNode（*it， false）；

}

}

//tree->prune（）；

tree->updateInnerOccupancy（）；

}

以上是octomap不斷維護的過程，接下來就是如何將octomap向下生成佔據柵格地圖（nav_msgs/OccupancyGrid）。其實這個東西原理比較簡單：選取合適高度的Ocomap柵格向下投影。不過實現起來還是有點麻煩，詳細程式碼可以參考程式碼（連結在末尾）中的octomap_multi_scan。cpp，對應的launch檔案入口octomap_multi_scan_updated。launch，主要用了octomap這個庫。整個從鐳射點雲生成nav_msgs/OccupancyGrid 的過程也都在程式碼裡。最後寫了一個service 將nav_msgs/Occupancy儲存起來（建圖階段的佔據柵格地圖儲存、以供之後路徑規劃階段呼叫），這裡需要儲存的東西包含1、（frame_id \resolution\width\height\origin） 這會在導航時用來恢復地圖。2、data 佔據柵格地圖的值。

哈哈哈終端花裡胡哨

在吉林大學中心校區晏湖（無人船）建圖效果

3、重定位，將當前機器人位置對映到柵格地圖(或者說slam中的map系)

事實上之前儲存的柵格地圖nav_msgs/OccupancyGrid中的座標 與 slam中的map系座標是完全相通的，因為nav_msgs/OccupancyGrid中有 frame_id（實際上就是map系），還有origin（佔據柵格原點在map系中的座標），根據resolution就可以計算佔據柵格地圖中的座標對應的map系座標。

所以這個問題就是slam中的重定位問題。這個就方法比較多了，其中最簡單的方法就是直接將差分GPS輸出的經緯度、以及航向角直接根據建圖時儲存的map系原點對應的經緯度對映到map系中（過於暴力）。更加優秀的做法是融合點雲以及GPS的位置、航向角共同完成重定位，當然沒有GPS的話，那就只能使用scan_contex這種描述子來進行place recognition了。

最後，octomap部分對應的程式碼地址：

superfastyyj/octomap_draft