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;