Cleaning robot planning control scheme

author	Contact information
Forrest	[email protected]

Article directory

Cleaning robot planning control scheme

Program introduction

Planning control (PNC) solution for indoor commercial sweeping/washing robots. Consists of the following functional packages:

clean_msg: function message definition
clean_pnc: planning control algorithm
clean_rviz: rviz simulation test plug-in

The current PNC solution supports the following functions:

fixed-point autonomous navigation
fixed route cleaning
Area Coverage Cleaning
Cleaning along the wall
Autonomous return and recharging

The PNC solution meets the common scene requirements of cleaning robots. In addition, it also supports the following features:

Breakpoint cleaning
Clean Area Trapping
Virtual Wall Settings
Prohibited Cleaning Area Settings
Dynamic/static override cleaning settings
Left/Right Sweeping Settings along the Wall
Dynamically adjust the speed (according to the distance from the obstacle)
Cleaning data statistics (cleaning track, cleaning rate, area, time-consuming, etc.)
Collision support for different chassis shapes
Sweeping/scrubbing cleaning equipment control support
…

Design

module link

The relevant output/output relationship of the PNC module is shown in the figure above:

enter
- Sensor data, such as lidar, camera, etc.
- positioning pose
- map data
- TF change
output
- Robot Control Instructions
- Cleaning Equipment Control Instructions
- Cleaning task data such as status, reports, etc.

coordinate transformation

The operation of the overall solution requires the support of relevant TFs. The relevant TF relationships are defined as follows:

Note: The frame introduced here odom_linkis mainly because some chassis motion motors are not installed in the center of the chassis

Algorithm framework

task_manager: Clean task execution manager. Receive the task queue sent by the upper layer and execute them one by one.
- task: Navigation task definition description, currently supports the following five navigation tasks:
  - pose_navigator: Fixed-point autonomous navigation task. It can realize the function of point A to B.
  - path_follower: Fixed route cleaning tasks. Can be used for predefined route cleaning.
  - area_cleaner: Area coverage cleaning task. Can be used for fixed area coverage cleaning.
  - wall_follower: Follow the task of cleaning along the wall. Can be used for cleaning along wall welts.
  - auto_charger: autonomous return and recharging tasks. When the robot is low on power, it will automatically return to the voyage and recharge.
- tasker：导航任务实现抽象。上述每个导航任务都是一个tasker，一般每个tasker由以下三部分组成：
  - planner：规划器。一个tasker可根据要实现的功能搭配多个planner，比如实现pose_navigator要搭配astar_path_planner和dwa_path_planner。现支持的planner有：
    - astar_path_planner：A*全局路径规划算法。用于A到B点路径生成
    - dwa_path_planner：DWA局部路径规划算法。用于路径跟踪。
    - clean_path_planner：覆盖路径规划算法。用于区域覆盖清洁路线生成。
    - contours_path_planner：沿墙路径规划算法。用于沿墙贴边清洁路径生成。
  - decider：决策器。一个tasker可根据要实现的功能搭配多个decider，比如实现pose_navigator要搭配global_plan_decider和local_plan_decider。现支持的decider有：
    - global_plan_decider：全局规划决策器。用于判断是否需要进行全局路径重规划。
    - local_plan_decider：局部规划决策器。用于局部路径规划失败处理。
    - path_follow_decider：路径跟踪决策器。用于路径跟踪停障等待和绕障处理。
  - navi_logic：导航逻辑。一个tasker可根据要实现的功能配合planner和decider进行调用逻辑组合，从而实现对应功能。
cost_map：代价地图。用于障碍物碰撞检测。
- footprint_model：机器人碰撞模型描述。可根据机器人形状进行定义。
- global_cost_map：全局代价地图。可用于全局规划路径和清洁规划路径生成。
- local_cost_map：局部代价地图。可用于局部规划路径生成。
fuction_map：功能地图。用于相关清洁功能实现，比如禁止清洁区域。
- forbidden_clean_area：禁止清洁区域。限定清洁环境中，某些区域只允许经过，不可以清洁。
- task_cleaned_area：已清洁区域。用于清洁任务中，已经清洁区域统计，生成相关清洁报告。
clean_system：清洁设备控制器。用于扫地/洗地清洁设备开关控制。

功能设计

定点自主导航

定点自主导航任务，可实现A到B点功能。功能设计如下图所示：

由astar_path_planner生成机器人当前位置到导航目标点全局路径。
得到全局路径后，dwa_path_planner则进行跟踪控制。
在导航过程中，global_plan_decider会判断全局路径可达性，如果当前全局路径不可达，则重新进行全局路径规划。
在导航过程中，local_plan_decider会根据dwa_path_planner规划状态，进行局部路径规划失败处理。

固定路线清洁

固定路线清洁任务，可用于预定义路线清洁。功能设计如下图所示：

首先机器人会根据pose_navigator一样的逻辑导航到离清洁路线最近点，然后开始清洁路线跟踪。
清洁路线跟踪使用dwa_path_planner。
在开始清洁路线跟踪后，path_follow_decider判断跟踪路线是否需要停障。
- 如果遇到障碍物则进行停障等待。
- 停障等待超时后，进行绕障继续跟踪清洁路线。
在清洁路线跟踪过程中，local_plan_decider会根据dwa_path_planner规划状态，进行局部路径规划失败处理。

区域覆盖清洁

区域覆盖清洁任务，可用于固定区域覆盖式清洁。功能设计如下图所示：

首先机器人会根据pose_navigator一样的逻辑导航到离清洁区域内最近点。
到达清洁区域内后，使用clean_path_planner进行清洁路线覆盖规划，并进行跟踪。
清洁路线跟踪使用dwa_path_planner。
在开始清洁路线跟踪后，path_follow_decider判断跟踪路线是否需要停障
- 如果遇到障碍物则进行停障等待
- 停障等待超时后，进行绕障继续跟踪清洁路线
在清洁路线跟踪过程中，local_plan_decider会根据dwa_path_planner规划状态，进行局部路径规划失败处理。

贴边沿墙清洁

贴边沿墙清洁任务，可用于沿墙贴边清洁。功能设计如下图所示：

首先机器人会根据pose_navigator一样的逻辑导航到离沿墙清洁路径上最近点。
到达最近后，使用contours_path_planner进行沿墙清洁路径规划，并进行跟踪。
沿墙清洁路线跟踪使用dwa_path_planner。
在清洁路线跟踪过程中，local_plan_decider会根据dwa_path_planner规划状态，进行局部路径规划失败处理。

自主返航回充

自主返航回充任务，机器人低电量时自主返航回充。功能设计如下图所示：

首先机器人会根据pose_navigator一样的逻辑导航到充电桩前方位置。
然后进行充电桩对接控制。

仿真测试

功能仿真测试视频:

清洁机器人PNC方案演示

仿真测试准备

安装部署好工程
仿真地图确认：仿真地图放在cleanbot/clean_pnc/data/map目录下，目前该目录下有测试地图demo，如果想更换地图可按如下步骤：
- 将要更换的地图放到cleanbot/clean_pnc/data/map目录下
- 更改仿真器中cleanbot/clean_pnc/launch/pnc_simulator.launch的test_map_name参数：
```
<launch>
  <node name="clean_pnc_simulator" pkg="clean_pnc" type="clean_pnc_simulator" output="screen">
    <param name="map_file_dir"    value="$(find clean_pnc)/data/map/"/>
    <param name="test_map_name"   value="demo"/>
  </node>
</launch>
```
启动仿真程序：启动成功后，可看到如下rviz界面：
```
roslaunch clean_pnc simulation_wash_robot.launch
```
初始化机器人位姿：在rviz软件显示窗口上，点击2D Pose Estimate按钮在地图上相应位置进行机器人位姿初始化
任务暂停/取消：在rviz软件显示窗口左下角，可进行任务暂停/取消操作

定点自主导航测试

下发导航目标点：在rviz软件显示窗口上，点击2D Nav Goal按钮在地图上相应位置进行导航目标点下发

在这里插入图片描述

下发成功后可在rviz看到如下界面：
如果想在测试过程中增加障碍物，可在rviz软件显示窗口上，点击Publish Point按钮在地图上相应位置进行障碍物设置：

固定路线清洁测试

设定清洁路线：在rviz软件显示窗口上，点击custom_path按钮在地图上相应位置进行清洁路线标注
- 按下键盘上的Enter按键下发任务
- 按下键盘上的d按键重新绘制路线
下发成功后可在rviz看到如下界面：
如果想在测试过程中增加障碍物，可在rviz软件显示窗口上，点击Publish Point按钮在地图上相应位置进行障碍物设置：

注意：任务完成后，机器人会主动导航返回任务起始点

区域覆盖清洁测试

设定清洁区域：在rviz软件显示窗口上，点击clean_polygon按钮在地图上相应位置进行清洁区域标注
- 按下键盘上的Enter按键下发任务
- 按下键盘上的d按键重新绘制区域
下发成功后可在rviz看到如下界面：
如果想在测试过程中增加障碍物，可在rviz软件显示窗口上，点击Publish Point按钮在地图上相应位置进行障碍物设置：