ipa 覆盖算法测试

相关文章

ipa 功能包测试

ipa 分区算法 ipa 分区算法总结，部分算法图解

ipa 覆盖算法分析（一）

ipa 覆盖算法分析（二）

测试

网上找的地图：

fig.1 测试地图

opencv

fig.2 opencv 显示的覆盖路径

rviz

fig.3 rviz 显示的覆盖路径

rviz 中 path 话题只是将路径点连线起来而没有显示路径点，单看话题还以为在障碍物和未知区域内规划了路径。cv 图片虽然区分了路径点和路线，但也只有灰度图像不方便看。所以最好在 rviz 中再补个路径点的话题。

为了方便观察，补充 posearray 话题显示所有路径点和起点终点。

效果如下，绿线是路径，红色箭头是路径点，蓝色箭头是起点和终点。

fig.4 rviz 显示的覆盖路径和路径点

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 计算覆盖栅格的大小，如下图，绿色区域是有效覆盖半径，蓝色正方形是其覆盖栅格。

fig.5 覆盖栅格

调整参数测试，左边 0.25m，右边 0.5m。可以看出该参数直接影响行距。

fig.6 coverage_radius 参数测试

• 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 显示。

fig.7 display_trajectory 参数测试

仅仅是服务端多显示一步 “cell path” ，把路径点转到栅格地图显示而已。

• map_correction_closing_neighborhood_size

设置地图预处理中的“腐蚀-膨胀”的开操作（虽然源码注释是闭操作）迭代次数，开操作主要用于过滤噪音。

源码中使用 3*3 的核对地图图片进行处理，迭代 n 次意味着能够过滤 n 个像素的噪声。

需要注意到 opencv 中腐蚀和膨胀都是对白色像素的操作！

所以源码的处理实际上是对自由空间的噪音的过滤，可以根据需要修改代码选择开闭操作。

给测试地图增加噪点像素：

fig.8(a) 增加噪点的测试地图

fig.8(b) 增加噪点的障碍物

fig.8(c) 增加噪点的障碍物

用 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 效果：

fig.9(a) tsp_solver 参数测试

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。

fig.10 min_cell_area 参数测试

看了下代码，源码的实现是跳过面积小于 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。

fig.11 path_eps 参数测试

• 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 向上取整。

fig.12 grid_obstacle_offset 参数意义图示

左图 grid_obstacle_offset = 0.251，右图 grid_obstacle_offset = 0.1。右图单元边界附近的路径更多一些。

fig.13(a) grid_obstacle_offset 参数测试

为什么图 fig.13(a) 中地图上方路径点距离边界远一点，但是地图下方路径距离边界却很近？增加测试代码显示旋转后的地图如图 fig.13(b)，旋转后的地图边界（一圈障碍物）确实发生了改变，实际上右侧已经没有黑色像素了。而原图 fig.13(c) 的地图边界原本就是整齐的，这说明 ipa 的旋转步骤会引入噪声。

fig.13(b) 旋转后的地图

fig.13(c) 测试原地图

为什么图 fig.13(a) 中 0 号单元区域左侧空隙较大？

源码是在被腐蚀自由区域后的地图上（增加路径安全距离），再在单元区域内计算牛耕覆盖路径。被腐蚀区域大小是 half_grid_spacing_as_int+grid_obstacle_offset ，也就是上面提到的第二个应用。

fig.13(d) 第 0 号单元区域

图 fig.13(e) 上方就是第 0 号单元区域。可以看到腐蚀后自由空间已经不多了。

fig.13(e) 腐蚀掉半个机身的地图

总的来说，地图旋转导致了误差， 安全距离腐蚀自由区域再次放大了误差，导致最后得到的路径异常。

• max_deviation_from_track

遇到障碍物时候，牛耕路径最大偏移量，单位：地图像素。设置为负值会自动调整。

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

fig.14 max_deviation_from_track 参数测试

发现单元相接的位置，牛耕路径是会有重叠的！原因是牛耕路径规划是单个单元区域进行的，各个单元并不知道相邻单元的规划情况，导致有大量路径重叠。

• cell_visiting_order

单元遍历顺序， cell_visiting_order = 1 是 TSP 规划， cell_visiting_order = 2 是从左到右遍历。

左图是顺序遍历，右图是 TSP 遍历。

TSP 的起点以机器所在单元开始，这里单元区域被简化为单元的中心点也就是单元编号所在的位置来计算的。

fig.15 cell_visiting_order 参数测试

****************** 神经网络规划器 ******************

神经网络规划器有些抽象，应该要像论文那样动态地观察网络是如何更新的才好理解，源码中大概扫了一眼没有找到动态观察的调试功能，下面就是简单地测试 ipa 参数。

神经元定义（部分）：

class Neuron
{
protected:
  // 外部刺激
  double I()
  {
    if(obstacle_ == true)
      return -1.0*E_;
    else if(visited_ == false)
      return E_;
    else
      return 0.0;
  }

  // 神经信号值更新
  void updateState()
  {
    // get external input
    const double input = I();

    // get the current sum of weights times the state of the neighbor
    double 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 beginning
    double 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 method
    state_ += step_size_*gradient;
  }
}

• step_size

神经信号增益。

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

fig.16 step_size 参数测试

• A

神经信号衰减值。

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

fig.17 A 参数测试

• B

神经信号的最高预期值。当神经元信号高于该值，下次迭代中信号会被拉低。

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

fig.18 B 参数测试

• D

神经信号的最低预期值。当神经元信号低于该值，下次迭代中信号会被拉高。

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

fig.19 D 参数测试

• E

外部刺激信号大小，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。

fig.20 E 参数测试

• mu

相邻神经元的影响权重，该权重会被距离稀释。

左图 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。

fig.21 mu 参数测试

• 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。

fig.22 delta_theta_weight 参数测试

****************** 凸感知放置规划器 ******************

在覆盖规划客户端修改 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;

fig.23 凸感知范围简化几何

• cell_size

凸感知使用的地图分辨率，单位是地图像素。cell_size <= 0 会自动检测分辨率，十分费时且效果不佳。分辨率越小规划越精确，但是求解时间越久。

左边是 cell_size = 0 自动检测分辨率和规划都十分耗时，右边是 cell_size = 7，计算快多了。

fig.24 cell_size 参数测试

• delta_theta

求解传感器放置位姿时候，传感器放置角度的采样步进，单位是弧度。

delta_theta = 0.78539816339 和 delta_theta = 1.570796。

fig.25 delta_theta 参数测试

****************** 流网络规划器 ******************

程序陷入死循环，代码未完成。

• curvature_factor

• max_distance_factor

缺陷

• TSP 规划十分耗时！

• 牛耕算法中，旋转地图引入了噪声。

为什么图 fig.13 中地图上方路径点距离边界远一点，但是地图下方路径距离边界却很近？增加测试代码显示旋转后的地图如图 fig.26，旋转后的地图边界（一圈障碍物）确实发生了改变，实际上右侧已经没有黑色像素了。而原图 fig.27 的地图边界原本就是整齐的，这说明 ipa 的旋转步骤会引入噪声。

fig.26 旋转后的地图

fig.27 测试原地图

为什么在牛耕算法测试中 0 号单元区域左侧空隙较大？

源码是在被腐蚀自由区域后的地图上（增加路径安全距离），再在单元区域内计算牛耕覆盖路径。被腐蚀区域大小是 half_grid_spacing_as_int+grid_obstacle_offset ，也就是上面提到的第二个应用。

fig.28 第 0 号单元区域

  图 fig.29 上方就是第 0 号单元区域。可以看到腐蚀后自由空间已经不多了。

fig.29 腐蚀掉半个机身的地图

总的来说，地图旋转导致了误差， 安全距离腐蚀自由区域再次放大了误差，导致最后得到的路径异常。

• 单元内部存在重叠路径。

在 max_deviation_from_track 参数测试步骤中，可以发现在 0 号单元内存在重叠路径（图 fig.30 左侧），当提高 max_deviation_from_track 偏移值后，路径别没有重叠了（图 fig.30 右侧）。

fig.30 单元内路径重叠

• 牛耕法单元之间路径没有连接。

Ipa 功能包是为 move_base 框架服务的，ipa 负责规划覆盖路径，然后根据机器定位和感知情况，选择路径点提供给 move_base，move_base 再进行点到点规划执行任务。

• 单元区间之间存在重叠的路径。

因为每个单元区是单独计算的！

• 单元合并不合理。

应该是小面积单元先尝试与之前所有相邻的小单元合并检查面积是否达标，不达标则暂时忽略跳过。

• 单元遍历顺序不合理。

就算是 TSP 规划遍历顺序，用简化的单元中心点计算也不合适。可以做成实时的规划，因为有时候因为动态障碍物干扰导致单元访问完毕后，终点位置会有变化。

• 凸感知放置算法中，感知范围设置只支持 4 个点的多边形。

改进

• 单元分解中，对于单个超大面积单元，可以增加分解。就是说单元分解不仅有最小面积限制，还有最大面积限制，超过最大单元面积需要分解为小单元。

• 障碍物边缘可以稍微离远点，提高路径执行效率。另外执行沿边规划来访问地图边缘。