ipa 覆蓋算法測試

ipa 功能包測試

ipa 分區算法?ipa 分區算法總結，部分算法圖解

ipa 覆蓋算法分析（一）

ipa 覆蓋算法分析（二）

測試

網上找的地圖：

opencv

rviz

rviz 中 path 話題只是將路徑點連線起來而沒有顯示路徑點，單看話題還以為在障礙物和未知區域內規劃了路徑。cv 圖片雖然區分了路徑點和路線，但也只有灰度圖像不方便看。所以最好在 rviz 中再補個路徑點的話題。

為了方便觀察，補充 posearray 話題顯示所有路徑點和起點終點。

效果如下，綠線是路徑，紅色箭頭是路徑點，藍色箭頭是起點和終點。

Ipa 參數測試

覆蓋算法

action 數據結構

客戶端發送任務參數對算法也有影響，下面是 action.goal 數據結構

# goal definition
sensor_msgs/Image input_map # 地圖,# 8bit 8UC1 灰度圖， 0 (黑) 表示障礙物， 255 (白) 表示自由空間#            todo: the image needs to be vertically mirrored compared to the map in RViz for using right coordinate systems#                  OccupancyGrid map = origin lower left corner, image = origin upper left corner#            todo: take the OccupanyGrid message here instead to avoid confusion and deal with map coordinates in server
float32 map_resolution # 地圖分辨率，單位：米/柵格
geometry_msgs/Pose map_origin # 地圖原點位置，暫不支持角度
float32 robot_radius # 沒有用到，看注釋是要考慮機器半徑來檢查碰撞的
float32 coverage_radius # 下面 planning_mode = 1 時候用的 footprint 覆蓋面積
geometry_msgs/Point32[] field_of_view # 用 4 個點表示機器感知區域，x 軸朝前，y 朝左。下面 planning_mode = 2 時候使用
geometry_msgs/Point32 field_of_view_origin # 上面 field_of_view 的原點在機器坐標系中的位置
geometry_msgs/Pose2D starting_position # starting pose of the robot in the room coordinate system [meter,meter,rad]
int32 planning_mode # @1:機器 footprint 計算覆蓋面積 @2：傳感器感知計算覆蓋面積

參數測試

在 ipa_room_exploration/ros/launch 中覆蓋算法有 3 個參數服務器配置文件

cob_map_accessibility_analysis_params 文件

5 個參數都沒用到

coverage_monitor_server_params 文件

map_frame

Map 坐標名，tf 轉換用

robot_frame

機器坐標名，tf 轉換用

coverage_radius

有效覆蓋半徑，單位米，以實際覆蓋區域的最大內接圓計算。然后用 coverage_radius 計算覆蓋柵格的大小，如下圖，綠色區域是有效覆蓋半徑，藍色正方形是其覆蓋柵格。

調整參數測試，左邊 0.25m，右邊 0.5m。可以看出該參數直接影響行距。

coverage_circle_offset_transform

??沒用

robot_trajectory_recording_active

??coverage_monitor_server.cpp 中監測覆蓋路徑執行情況，記錄，顯示機器運行軌跡。

room_exploration_action_server_params 文件

room_exploration_algorithm

覆蓋算法選擇。

display_trajectory

是否顯示算法步驟。

算法選擇 boustrophedon explorator ，coverage_radius 是 0.25m。

左邊是 cell path 步驟，中間是 path 步驟，右邊是最后的 rviz 顯示。

僅僅是服務端多顯示一步 “cell path” ，把路徑點轉到柵格地圖顯示而已。

map_correction_closing_neighborhood_size

設置地圖預處理中的“腐蝕-膨脹”的開操作（雖然源碼注釋是閉操作）迭代次數，開操作主要用于過濾噪音。

源碼中使用 3*3 的核對地圖圖片進行處理，迭代 n 次意味著能夠過濾 n 個像素的噪聲。

需要注意到 opencv 中腐蝕和膨脹都是對白色像素的操作！

所以源碼的處理實際上是對自由空間的噪音的過濾，可以根據需要修改代碼選擇開閉操作。

給測試地圖增加噪點像素：

用 map_correction_closing_neighborhood_size = 2 測試：

從左到右分別是：腐蝕步驟，膨脹步驟，規劃路徑，rviz顯示

fig.9 map_correction_closing_neighborhood_size 參數測試

return_path

返回路徑到覆蓋 action 的 result 中，同時發布 rviz 話題。

execute_path

是否執行路徑，該功能會在覆蓋路徑上選擇目標發布給到 move_base，同時記錄機器軌跡。

goal_eps

執行路徑時候，選擇的目標點與機器最小距離。

use_dyn_goal_eps

啟用該功能的話當路徑曲率越大，目標選擇最近，goal_eps 作為選點最遠距離。

interrupt_navigation_publishing

revisit_areas

因為計算或者動態障礙物干擾忽略過的區域是否要回去訪問。

left_sections_min_area

重新訪問時候，最小的未訪問區域大小。

global_costmap_topic

全局地圖話題名

coverage_check_service_name

沒用到

map_frame

map 坐標系名

camera_frame

相機坐標系名

============================== 規劃器 ==============================

****************** TSP 規劃器 ******************?

tsp_solver

根據注釋

tsp 規劃器第一類求解器 Nearest Neighbor 效果：

tsp 規劃器第三類求解器 Concorde solver 需要到官網下載，注釋說這個規劃算法更加費時我就不折騰看了。

??略...

tsp_solver_timeout

Tsp 規劃超時時間，單位秒

??****************** 牛耕法規劃器 ******************

min_cell_area

為了方便查看效果，rviz 增加了單元區域顯示，序號是單元遍歷順序。

測試圖片是 200 pixel * 200 pixel = 40000 pixel^2。

下圖左邊是 min_cell_area = 200 效果，右圖是 min_cell_area = 10000 效果。可以看到右圖中 No.0 單元面積非常大，遠不止 10000 pixel^2。

看了下代碼，源碼的實現是跳過面積小于 min_cell_area 的單元，當遇到面積滿足條件的單元后，再合并前面被忽略的相鄰單元給到當前單元。由于 0 到 6 號單元面積都不滿足條件，且都相鄰，所以都被合并進 7 號單元了。

測試了 room_exploration_algorithm = 8 的改進牛耕算法，這部分單元融合效果是一樣的。

path_eps

覆蓋路徑點間距，單位是像素。

測試 room_exploration_algorithm = 2 牛耕法，左邊 2 圖是 path_eps = 6.0，右邊 2 圖是 path_eps = 10.0。

grid_obstacle_offset

第一個應用：計算 min_cell_width，在單元合并時候，寬高小于 min_cell_width 的單元會被相鄰的大單元合并。

const int min_cell_width = half_grid_spacing_as_int + 2.*grid_obstacle_offset/map_resolution;

第二個應用：單元區域內計算牛耕路徑之前，會腐蝕迭代 (半個覆蓋柵格+grid_obstacle_offset) 次。

cv::erode(room_map, inflated_room_map, cv::Mat(), cv::Point(-1, -1), half_grid_spacing_as_int+grid_obstacle_offset);

這里腐蝕的 grid_obstacle_offset 個柵格可以認為是對障礙物的膨脹，增加了路徑安全保障。因為規劃的路徑認為是機器中心跟隨的，為保障路徑可行，需要腐蝕掉大于半個機器半徑的自由柵格空間。

如圖 fig.12 所示，藍色圓圈表示機器（如果是幾何外形，可以用最大外接圓代替），中間棕色正方形 ABCD 就是覆蓋柵格區域，girdObstacleOffset 距離就是最小的安全障礙物膨脹距離 grid_obstacle_offset 。由于源碼的計算得到的是 int 類型，小于 1 地圖柵格還會變為 0 個柵格，所以 grid_obstacle_offset 還得大于等于地圖分辨率。另一種做法是用 std::ceil 向上取整。

左圖 grid_obstacle_offset = 0.251，右圖 grid_obstacle_offset = 0.1。右圖單元邊界附近的路徑更多一些。

為什么圖 fig.13(a) 中地圖上方路徑點距離邊界遠一點，但是地圖下方路徑距離邊界卻很近？增加測試代碼顯示旋轉后的地圖如圖 fig.13(b)，旋轉后的地圖邊界（一圈障礙物）確實發生了改變，實際上右側已經沒有黑色像素了。而原圖 fig.13(c) 的地圖邊界原本就是整齊的，這說明 ipa 的旋轉步驟會引入噪聲。

為什么圖 fig.13(a) 中 0 號單元區域左側空隙較大？

源碼是在被腐蝕自由區域后的地圖上（增加路徑安全距離），再在單元區域內計算牛耕覆蓋路徑。被腐蝕區域大小是 half_grid_spacing_as_int+grid_obstacle_offset ，也就是上面提到的第二個應用。

圖 fig.13(e) 上方就是第 0 號單元區域。可以看到腐蝕后自由空間已經不多了。

總的來說，地圖旋轉導致了誤差，安全距離腐蝕自由區域再次放大了誤差，導致最后得到的路徑異常。

max_deviation_from_track

遇到障礙物時候，牛耕路徑最大偏移量，單位：地圖像素。設置為負值會自動調整。

左圖 max_deviation_from_track = 0，右圖 max_deviation_from_track = 8，地圖分辨率是 0.05cm。

發現單元相接的位置，牛耕路徑是會有重疊的！原因是牛耕路徑規劃是單個單元區域進行的，各個單元并不知道相鄰單元的規劃情況，導致有大量路徑重疊。

cell_visiting_order

單元遍歷順序， cell_visiting_order = 1 是 TSP 規劃， cell_visiting_order = 2 是從左到右遍歷。

左圖是順序遍歷，右圖是 TSP 遍歷。

TSP 的起點以機器所在單元開始，這里單元區域被簡化為單元的中心點也就是單元編號所在的位置來計算的。

****************** 神經網絡規劃器 ******************

神經網絡規劃器有些抽象，應該要像論文那樣動態地觀察網絡是如何更新的才好理解，源碼中大概掃了一眼沒有找到動態觀察的調試功能，下面就是簡單地測試 ipa 參數。

神經元定義（部分）：

class Neuron
{
protected:// 外部刺激double I(){if(obstacle_ == true)return -1.0*E_;else if(visited_ == false)return E_;elsereturn 0.0;}// 神經信號值更新void updateState(){// get external inputconst double input = I();// get the current sum of weights times the state of the neighbordouble weight_sum = 0;for(size_t neighbor=0; neighbor<neighbors_.size(); ++neighbor)weight_sum += weights_[neighbor]*std::max(neighbors_[neighbor]->getState(true), 0.0);// calculate current gradient --> see stated paper from the beginningdouble gradient = -A_*state_ + (B_-state_)*(std::max(input, 0.0) + weight_sum) - (D_+state_)*std::max(-1.0*input, 0.0);// update state using euler methodstate_ += step_size_*gradient;}
}

step_size

神經信號增益。

左圖 step_size = 0.0008，中圖 step_size = 0.008，右圖 step_size = 0.012。

神經信號衰減值。

左圖 step_size = 0.008，A = 17，右圖 step_size = 0.008，A = 10。

神經信號的最高預期值。當神經元信號高于該值，下次迭代中信號會被拉低。

左圖 step_size = 0.008，A = 17，B = 5，右圖 step_size = 0.008，A = 17，B = 10。

神經信號的最低預期值。當神經元信號低于該值，下次迭代中信號會被拉高。

左圖 step_size = 0.008，A = 17，B = 5，D = 7，右圖 step_size = 0.008，A = 17，B = 5，D = 3。

外部刺激信號大小，E 來自障礙物，未訪問區域或者已訪問區域。

左圖 step_size = 0.008，A = 17，B = 5，D = 7，E = 80。

右圖 step_size = 0.008，A = 17，B = 5，D = 3，E = 200。

相鄰神經元的影響權重，該權重會被距離稀釋。

左圖 step_size = 0.008，A = 17，B = 5，D = 7，E = 80，mu = 1.03。

右圖 step_size = 0.008，A = 17，B = 5，D = 3，E = 200，mu = 1.3。

delta_theta_weight

相鄰神經元的角度影響權重

左圖 step_size = 0.008，A = 17，B = 5，D = 7，E = 80，mu = 1.03，delta_theta_weight = 0.15。

右圖 step_size = 0.008，A = 17，B = 5，D = 3，E = 200，mu = 1.03，delta_theta_weight = 0.3。

****************** 凸感知放置規劃器 ******************

在覆蓋規劃客戶端修改 goal 的 planning_mode=2 用傳感器感知區域計算覆蓋，同時將傳感器感知范圍 field_of_view 設置如下，大概模擬下扇形感知：

std::vector<geometry_msgs::Point32> fov_points(4);
fov_points[0].x = 0.0;
fov_points[0].y = 0.0;
fov_points[1].x = 0.5;
fov_points[1].y = 0.4;
fov_points[2].x = 0.5;
fov_points[2].y = -0.4;
fov_points[3].x = 0.6;
fov_points[3].y = 0.0;