清潔機(jī)器人規(guī)劃控制方案

作者	聯(lián)系方式
Forrest	709335543@qq.com

方案簡介

室內(nèi)商用掃地/洗地機(jī)器人規(guī)劃控制（PNC）方案。有以下功能包組成：

• clean_msg：功能消息定義
• clean_pnc：規(guī)劃控制算法
• clean_rviz：rviz仿真測試插件

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

• 定點(diǎn)自主導(dǎo)航
• 固定路線清潔
• 區(qū)域覆蓋清潔
• 貼邊沿墻清潔
• 自主返航回充

PNC方案在滿足清潔機(jī)器人常見場景需求，除此之外，還支持以下特性：

• 斷點(diǎn)續(xù)接清潔
• 清潔區(qū)域補(bǔ)漏
• 虛擬墻設(shè)置
• 禁止清潔區(qū)域設(shè)置
• 動/靜態(tài)覆蓋清潔設(shè)置
• 左/右沿墻清掃設(shè)置
• 動態(tài)調(diào)節(jié)速度（根據(jù)距離障礙物遠(yuǎn)近調(diào)節(jié)）
• 清潔數(shù)據(jù)統(tǒng)計(jì)（清潔軌跡、清潔率、面積、耗時等）
• 不同底盤形狀碰撞支持
• 掃地/洗地清潔設(shè)備控制支持
• …

方案設(shè)計(jì)

模塊鏈路

PNC模塊相關(guān)輸出/輸出關(guān)系如上圖所示：

• 輸入
  • 傳感器數(shù)據(jù)、如lidar、camera等
  • 定位位姿
  • 地圖數(shù)據(jù)
  • TF變化
• 輸出
  • 機(jī)器人控制指令
  • 清潔設(shè)備控制指令
  • 清潔任務(wù)數(shù)據(jù)，如狀態(tài)、報告等

坐標(biāo)變換

整體方案運(yùn)行，需要相關(guān)TF支持，相關(guān)TF關(guān)系定義如下：

注意：這里引入odom_linkframe主要是有些底盤運(yùn)動電機(jī)沒有安裝在底盤中心

算法框架

• task_manager：清潔任務(wù)執(zhí)行管理器。接收上層發(fā)過來的任務(wù)隊(duì)列，挨個進(jìn)行執(zhí)行。
  • task：導(dǎo)航任務(wù)定義描述，現(xiàn)支持以下五個導(dǎo)航任務(wù)：
    • pose_navigator：定點(diǎn)自主導(dǎo)航任務(wù)。可實(shí)現(xiàn)A到B點(diǎn)功能。
    • path_follower：固定路線清潔任務(wù)?？捎糜陬A(yù)定義路線清潔。
    • area_cleaner：區(qū)域覆蓋清潔任務(wù)。可用于固定區(qū)域覆蓋式清潔。
    • wall_follower：貼邊沿墻清潔任務(wù)?？捎糜谘貕N邊清潔。
    • auto_charger：自主返航回充任務(wù)。機(jī)器人低電量時自主返航回充。
  • tasker：導(dǎo)航任務(wù)實(shí)現(xiàn)抽象。上述每個導(dǎo)航任務(wù)都是一個tasker，一般每個tasker由以下三部分組成：
    • planner：規(guī)劃器。一個tasker可根據(jù)要實(shí)現(xiàn)的功能搭配多個planner，比如實(shí)現(xiàn)pose_navigator要搭配astar_path_planner和dwa_path_planner。現(xiàn)支持的planner有：
      • astar_path_planner：A*全局路徑規(guī)劃算法。用于A到B點(diǎn)路徑生成
      • dwa_path_planner：DWA局部路徑規(guī)劃算法。用于路徑跟蹤。
      • clean_path_planner：覆蓋路徑規(guī)劃算法。用于區(qū)域覆蓋清潔路線生成。
      • contours_path_planner：沿墻路徑規(guī)劃算法。用于沿墻貼邊清潔路徑生成。
    • decider：決策器。一個tasker可根據(jù)要實(shí)現(xiàn)的功能搭配多個decider，比如實(shí)現(xiàn)pose_navigator要搭配global_plan_decider和local_plan_decider?，F(xiàn)支持的decider有：
      • global_plan_decider：全局規(guī)劃決策器。用于判斷是否需要進(jìn)行全局路徑重規(guī)劃。
      • local_plan_decider：局部規(guī)劃決策器。用于局部路徑規(guī)劃失敗處理。
      • path_follow_decider：路徑跟蹤決策器。用于路徑跟蹤停障等待和繞障處理。
    • navi_logic：導(dǎo)航邏輯。一個tasker可根據(jù)要實(shí)現(xiàn)的功能配合planner和decider進(jìn)行調(diào)用邏輯組合，從而實(shí)現(xiàn)對應(yīng)功能。
• cost_map：代價地圖。用于障礙物碰撞檢測。
  • footprint_model：機(jī)器人碰撞模型描述?？筛鶕?jù)機(jī)器人形狀進(jìn)行定義。
  • global_cost_map：全局代價地圖?？捎糜谌忠?guī)劃路徑和清潔規(guī)劃路徑生成。
  • local_cost_map：局部代價地圖?？捎糜诰植恳?guī)劃路徑生成。
• fuction_map：功能地圖。用于相關(guān)清潔功能實(shí)現(xiàn)，比如禁止清潔區(qū)域。
  • forbidden_clean_area：禁止清潔區(qū)域。限定清潔環(huán)境中，某些區(qū)域只允許經(jīng)過，不可以清潔。
  • task_cleaned_area：已清潔區(qū)域。用于清潔任務(wù)中，已經(jīng)清潔區(qū)域統(tǒng)計(jì)，生成相關(guān)清潔報告。
• clean_system：清潔設(shè)備控制器。用于掃地/洗地清潔設(shè)備開關(guān)控制。

功能設(shè)計(jì)

定點(diǎn)自主導(dǎo)航

定點(diǎn)自主導(dǎo)航任務(wù)，可實(shí)現(xiàn)A到B點(diǎn)功能。功能設(shè)計(jì)如下圖所示：

• 由astar_path_planner生成機(jī)器人當(dāng)前位置到導(dǎo)航目標(biāo)點(diǎn)全局路徑。
• 得到全局路徑后，dwa_path_planner則進(jìn)行跟蹤控制。
• 在導(dǎo)航過程中，global_plan_decider會判斷全局路徑可達(dá)性，如果當(dāng)前全局路徑不可達(dá)，則重新進(jìn)行全局路徑規(guī)劃。
• 在導(dǎo)航過程中，local_plan_decider會根據(jù)dwa_path_planner規(guī)劃狀態(tài)，進(jìn)行局部路徑規(guī)劃失敗處理。

固定路線清潔

固定路線清潔任務(wù)，可用于預(yù)定義路線清潔。功能設(shè)計(jì)如下圖所示：

• 首先機(jī)器人會根據(jù)pose_navigator一樣的邏輯導(dǎo)航到離清潔路線最近點(diǎn)，然后開始清潔路線跟蹤。
• 清潔路線跟蹤使用dwa_path_planner。
• 在開始清潔路線跟蹤后，path_follow_decider判斷跟蹤路線是否需要停障。
  • 如果遇到障礙物則進(jìn)行停障等待。
  • 停障等待超時后，進(jìn)行繞障繼續(xù)跟蹤清潔路線。
• 在清潔路線跟蹤過程中，local_plan_decider會根據(jù)dwa_path_planner規(guī)劃狀態(tài)，進(jìn)行局部路徑規(guī)劃失敗處理。

區(qū)域覆蓋清潔

區(qū)域覆蓋清潔任務(wù)，可用于固定區(qū)域覆蓋式清潔。功能設(shè)計(jì)如下圖所示：

• 首先機(jī)器人會根據(jù)pose_navigator一樣的邏輯導(dǎo)航到離清潔區(qū)域內(nèi)最近點(diǎn)。
• 到達(dá)清潔區(qū)域內(nèi)后，使用clean_path_planner進(jìn)行清潔路線覆蓋規(guī)劃，并進(jìn)行跟蹤。
• 清潔路線跟蹤使用dwa_path_planner。
• 在開始清潔路線跟蹤后，path_follow_decider判斷跟蹤路線是否需要停障
  • 如果遇到障礙物則進(jìn)行停障等待
  • 停障等待超時后，進(jìn)行繞障繼續(xù)跟蹤清潔路線
• 在清潔路線跟蹤過程中，local_plan_decider會根據(jù)dwa_path_planner規(guī)劃狀態(tài)，進(jìn)行局部路徑規(guī)劃失敗處理。

貼邊沿墻清潔

貼邊沿墻清潔任務(wù)，可用于沿墻貼邊清潔。功能設(shè)計(jì)如下圖所示：

• 首先機(jī)器人會根據(jù)pose_navigator一樣的邏輯導(dǎo)航到離沿墻清潔路徑上最近點(diǎn)。
• 到達(dá)最近后，使用contours_path_planner進(jìn)行沿墻清潔路徑規(guī)劃，并進(jìn)行跟蹤。
• 沿墻清潔路線跟蹤使用dwa_path_planner。
• 在清潔路線跟蹤過程中，local_plan_decider會根據(jù)dwa_path_planner規(guī)劃狀態(tài)，進(jìn)行局部路徑規(guī)劃失敗處理。

自主返航回充

自主返航回充任務(wù)，機(jī)器人低電量時自主返航回充。功能設(shè)計(jì)如下圖所示：

• 首先機(jī)器人會根據(jù)pose_navigator一樣的邏輯導(dǎo)航到充電樁前方位置。
• 然后進(jìn)行充電樁對接控制。

仿真測試

功能仿真測試視頻:

仿真測試準(zhǔn)備

• 安裝部署好工程

• 仿真地圖確認(rèn)：仿真地圖放在cleanbot/clean_pnc/data/map目錄下，目前該目錄下有測試地圖demo，如果想更換地圖可按如下步驟：

  • 將要更換的地圖放到cleanbot/clean_pnc/data/map目錄下

  • 更改仿真器中cleanbot/clean_pnc/launch/pnc_simulator.launch的test_map_name參數(shù)：

    <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
  

  

• 初始化機(jī)器人位姿：在rviz軟件顯示窗口上，點(diǎn)擊2D Pose Estimate按鈕在地圖上相應(yīng)位置進(jìn)行機(jī)器人位姿初始化

  

• 任務(wù)暫停/取消：在rviz軟件顯示窗口左下角，可進(jìn)行任務(wù)暫停/取消操作

  

定點(diǎn)自主導(dǎo)航測試

• 下發(fā)導(dǎo)航目標(biāo)點(diǎn)：在rviz軟件顯示窗口上，點(diǎn)擊2D Nav Goal按鈕在地圖上相應(yīng)位置進(jìn)行導(dǎo)航目標(biāo)點(diǎn)下發(fā)

• 下發(fā)成功后可在rviz看到如下界面：

  

• 如果想在測試過程中增加障礙物，可在rviz軟件顯示窗口上，點(diǎn)擊Publish Point按鈕在地圖上相應(yīng)位置進(jìn)行障礙物設(shè)置：

  

固定路線清潔測試

• 設(shè)定清潔路線：在rviz軟件顯示窗口上，點(diǎn)擊custom_path按鈕在地圖上相應(yīng)位置進(jìn)行清潔路線標(biāo)注

  • 按下鍵盤上的Enter按鍵下發(fā)任務(wù)
  • 按下鍵盤上的d按鍵重新繪制路線

  

• 下發(fā)成功后可在rviz看到如下界面：

  

• 如果想在測試過程中增加障礙物，可在rviz軟件顯示窗口上，點(diǎn)擊Publish Point按鈕在地圖上相應(yīng)位置進(jìn)行障礙物設(shè)置：

  

注意：任務(wù)完成后，機(jī)器人會主動導(dǎo)航返回任務(wù)起始點(diǎn)

區(qū)域覆蓋清潔測試

• 設(shè)定清潔區(qū)域：在rviz軟件顯示窗口上，點(diǎn)擊clean_polygon按鈕在地圖上相應(yīng)位置進(jìn)行清潔區(qū)域標(biāo)注

  • 按下鍵盤上的Enter按鍵下發(fā)任務(wù)
  • 按下鍵盤上的d按鍵重新繪制區(qū)域

  

• 下發(fā)成功后可在rviz看到如下界面：

  

• 如果想在測試過程中增加障礙物，可在rviz軟件顯示窗口上，點(diǎn)擊Publish Point按鈕在地圖上相應(yīng)位置進(jìn)行障礙物設(shè)置：

注意：任務(wù)完成后，機(jī)器人會主動導(dǎo)航返回任務(wù)起始點(diǎn)

貼邊沿墻清潔測試

• 設(shè)定清潔區(qū)域：在rviz軟件顯示窗口上，點(diǎn)擊along_wall按鈕在地圖上相應(yīng)墻體位置進(jìn)行沿墻啟點(diǎn)下發(fā)

  

• 下發(fā)成功后可在rviz看到如下界面：

  

• 如果想在測試過程中增加障礙物，可在rviz軟件顯示窗口上，點(diǎn)擊Publish Point按鈕在地圖上相應(yīng)位置進(jìn)行障礙物設(shè)置：

  

注意：任務(wù)完成后，機(jī)器人會主動導(dǎo)航返回任務(wù)起始點(diǎn)