清洁机器人规划控制方案

清洁机器人规划控制方案

作者	联系方式
Forrest	[email protected]

方案简介

室内商用扫地/洗地机器人规划控制（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参数：

    <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按钮在地图上相应位置进行障碍物设置：

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

贴边沿墙清洁测试

• 设定清洁区域：在rviz软件显示窗口上，点击along_wall按钮在地图上相应墙体位置进行沿墙启点下发

  

• 下发成功后可在rviz看到如下界面：

  

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

  

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