清洁机器人规划控制方案

作者	联系方式
Forrest	709335543@qq.com

文章目录

清洁机器人规划控制方案

方案简介

室内商用扫地/洗地机器人规划控制（PNC）方案。有以下功能包组成：

clean_msg：功能消息定义
clean_pnc：规划控制算法
clean_rviz：rviz仿真测试插件

目前PNC方案支持以下功能：

定点自主导航
固定路线清洁
区域覆盖清洁
贴边沿墙清洁
自主返航回充

PNC方案在满足清洁机器人常见场景需求，除此之外，还支持以下特性：

断点续接清洁
清洁区域补漏
虚拟墙设置
禁止清洁区域设置
动/静态覆盖清洁设置
左/右沿墙清扫设置
动态调节速度（根据距离障碍物远近调节）
清洁数据统计（清洁轨迹、清洁率、面积、耗时等）
不同底盘形状碰撞支持
扫地/洗地清洁设备控制支持
...

方案设计

模块链路

lidar/camera/odom lidar/camera/odom robot pose clean task clean report ctrl vel clean ctrl sensor localization pnc map application robot

PNC模块相关输出/输出关系如上图所示：

输入
- 传感器数据、如lidar、camera等
- 定位位姿
- 地图数据
- TF变化
输出
- 机器人控制指令
- 清洁设备控制指令
- 清洁任务数据，如状态、报告等

坐标变换

整体方案运行，需要相关TF支持，相关TF关系定义如下：
10hz mapping or lolicazation module 30hz chassis module 30hz static tf 30hz static tf 30hz static tf 30hz static tf map odom odom_link base_link lidar camera imu

注意：这里引入odom_linkframe主要是有些底盘运动电机没有安装在底盘中心

算法框架

tasker task pnc task_manager decider global_plan_decider local_plan_decider path_follow_decider planner astar_path_planner dwa_path_planner clean_path_planner contours_path_planner planners deciders navi_logic pose_navigator path_follower area_cleaner wall_follower auto_charger clean_system sweep_clean_system wash_clean_system function_map forbidden_clean_area task_cleaned_area cost_map footprint_model global_cost_map local_cost_map task_list clean_tasks

task_manager：清洁任务执行管理器。接收上层发过来的任务队列，挨个进行执行。
- task：导航任务定义描述，现支持以下五个导航任务：
  - pose_navigator：定点自主导航任务。可实现A到B点功能。
  - path_follower：固定路线清洁任务。可用于预定义路线清洁。
  - area_cleaner：区域覆盖清洁任务。可用于固定区域覆盖式清洁。
  - wall_follower：贴边沿墙清洁任务。可用于沿墙贴边清洁。
  - auto_charger：自主返航回充任务。机器人低电量时自主返航回充。
- 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点功能。功能设计如下图所示：
pose_navigator pnc task_manager decider global_plan_decider local_plan_decider planner astar_path_planner dwa_path_planner planners deciders navi_logic navi_goal cost_map clean_task

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

固定路线清洁

固定路线清洁任务，可用于预定义路线清洁。功能设计如下图所示：
path_follower pnc task_manager decider global_plan_decider local_plan_decider path_follow_decider planner astar_path_planner dwa_path_planner planners deciders navi_logic clean_path cost_map function_map clean_system clean_task

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

区域覆盖清洁

区域覆盖清洁任务，可用于固定区域覆盖式清洁。功能设计如下图所示：
area_cleaner pnc task_manager decider global_plan_decider local_plan_decider path_follow_decider planner astar_path_planner dwa_path_planner clean_path_planner planners deciders navi_logic clean_zone cost_map function_map clean_system clean_task

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

贴边沿墙清洁

贴边沿墙清洁任务，可用于沿墙贴边清洁。功能设计如下图所示：
wall_follower pnc task_manager decider global_plan_decider local_plan_decider planner astar_path_planner dwa_path_planner contours_path_planner planners deciders navi_logic along_wall_path cost_map function_map clean_system clean_task

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

自主返航回充

自主返航回充任务，机器人低电量时自主返航回充。功能设计如下图所示：
auto_charger pnc task_manager decider global_plan_decider local_plan_decider planner astar_path_planner dwa_path_planner planners deciders navi_logic navi_goal cost_map clean_task

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

仿真测试

功能仿真测试视频:

清洁机器人PNC方案演示

仿真测试准备

安装部署好工程
仿真地图确认：仿真地图放在cleanbot/clean_pnc/data/map目录下，目前该目录下有测试地图demo，如果想更换地图可按如下步骤：
- 将要更换的地图放到cleanbot/clean_pnc/data/map目录下
- 更改仿真器中cleanbot/clean_pnc/launch/pnc_simulator.launch的test_map_name参数：
  xml 复制代码
```
<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界面：
shell 复制代码
```
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按钮在地图上相应位置进行障碍物设置：

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

贴边沿墙清洁测试

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

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