讓機器人平滑移動以及增加應用彈性的方法

有沒有一個經驗,為什麼我做的機器人移動上永遠無法像 PR2 或者吸塵器機器人一樣平滑?為什麼我的機器人每次移動都好像一瞬間花了洪荒之力,而機身就是被不情願的往前脫去,這樣久了不只對馬達是種損害,對電力系統也宣判了慢性癌症,總有一天會讓電力系統中某個元件壞掉(如果你的電力系統跟我設計的一樣永遠都不完美)。

 

因此,我們需要為自己的導航系統加點控制系統,讓機器人移動上不會這麼魯莽。我們使用 Yujin robot (他們的 GitHub) 開發的 kobuki_safety_controllerkobuki_velocity_smoother 。其實這兩個 packages 是包在yujin_yocs 這個 stack 中。詳細的官方資訊可以參考這裡。有一點稍微提下,就是這些 Packages 裡面裝的,並不是節點,而是 nodelets (不知道怎麼翻XDD)。但這之後我們會再聊到,本篇先從略。我們來看一下整個驅動控制架構:

si_Ih7qGws9qsRJwX5xTb_g
圖一:Kobuki 整體控制架構

Source: kobukiTutorialsKobuki’s Control System

 

 

 

由圖一中可以觀察到,Kobuki 這台吸塵器機器人(加上上面的架子後就進化成 Turtlebot了,看倌可以花三萬塊台幣去收服它!)所採用的,就是這一套馬達驅動控制架構。yocs_safety_controller 其實由三個程式組成:kobuki_node 、cmd_vel_mux、kobuki:_safety_controller。其中,kobuki_node 其實就是 kobuki 吸塵器機器人的控制驅動節點。cmd_vel_mux 是一種多進單出的速度切換器。kobuki_safety_controller 則是檢查機器人是否碰撞到障礙物、輪子脫落、有沒有跑到懸崖上的檢查機制。

 

啟動速度切換器!

 

所以,我們可以利用這三個節點中的 cmd_vel_mux 來作切換器,這個節點確認就算有不同的速度指令同時要操控這台機器人,只有一種速度指令會傳送給馬達。它有10個 /cmd_vel 的接口,可以接上十種速度指令,並指定每個速度指令的優先性先後順序,由最優先的10到最後一位的第0位。預設是讓 ROS Nav stack 的優先性設在最後一位,第零位,而讓 safety controller 輸出的速度指令設在第十位,使用者遙控輸出的指令排在前述兩者之間。這樣子,多進單出的 cmd_vel_mux 切換器就完善了。另外,這個切換器還會輸出一個總輸出速度 /cmd_vre_mux/output 作為閉路回饋控制迴圈的輸入依據。

這樣講有點抽象,讓我舉個例子:

優先性:

10 . (highest priority) safety controller

  1.   keyboard teleop
  2.   joystick teleop
  3.   android teleop
  4.  (Lowest priority) nav cmd vel

 

以這個方式,我們一次接上了5個速度指令,而一次只有一個總輸出給馬達控制器。但在輸出到馬達之前,我們要在加上一個控制器。

 

那要如何實做呢?既然我們都有節點了,那麼現在要擔心的,是怎麼用 Launch file 來開啟每個節點以及輸出的 topics。其實不只是要學會用 launch,具體的速度指令優先順序,必須另外寫個社檔宣告,然後匯入到 launch 檔。範例如下:

 

<launch>
 <arg name="nodelet_manager_name" default="nodelet_manager" />
 <!–Velocity Smoother’s settings–>  <arg name="smoother_node_name"    default="velocity_smoother" />

 <arg name="config_file"           default="$(find mybot_bringup)/launch/velocity_smoother_param.yaml"/>

 <arg name="raw_cmd_vel_topic"     default="cmd_vel_mux/output"/>                <!–subscribed topic –>

 <arg name="smooth_cmd_vel_topic"  default="/smooth_vel"/>                          <!–publish topic–>

 <arg name="robot_cmd_vel_topic"   default="cmd_vel_mux/output"/>               <!–subscribed topic–>

 <arg name="odom_topic"            default="odom"/>                                             <!–subscribed topic–>
 <!– ****** Nodelet manager ******** –>

 <node pkg="nodelet" type="nodelet" name="$(arg nodelet_manager_name)" args="manager" />
 <!– ***** cmd_vel_mux ************* –>

 <node pkg="nodelet" type="nodelet" name="cmd_vel_mux" args="load yocs_cmd_vel_mux/CmdVelMuxNodelet $(arg nodelet_manager_name)">

    <param name="yaml_cfg_file" value="$(find mybot_bringup)/launch/cmd_vel_mux_minimal_with_safety.yaml" />

 </node>

  <!– ****Velocity Smoother (just regulate the overall output from cmd_vel_mux) ***** –>

 <node pkg="nodelet" type="nodelet" name="$(arg smoother_node_name)"

       args="load yocs_velocity_smoother/VelocitySmootherNodelet $(arg nodelet_manager_name)">

   <!– parameters –>

   <rosparam file="$(arg config_file)" command="load"/>
   <!– velocity commands I/O –>

   <remap from="$(arg smoother_node_name)/raw_cmd_vel"        to="$(arg raw_cmd_vel_topic)"/>

   <remap from="$(arg smoother_node_name)/smooth_cmd_vel"  to="$(arg smooth_cmd_vel_topic)"/>
   <!– Robot velocity feedbacks –>

   <remap from="$(arg smoother_node_name)/robot_cmd_vel"  to="$(arg robot_cmd_vel_topic)"/>

   <remap from="$(arg smoother_node_name)/odometry"          to="$(arg odom_topic)"/>

 </node>

</launch>

 

接著我們來看一下設定檔,範例如下,:

 

cmd_vel_mux_with_safety_priority.yaml

subscribers:

 – name:        “Default input"

   topic:       “def_cmd_vel"

   timeout:     0.1

   priority:    0

   short_desc:  “Default velocity topic; controllers unaware that we are multiplexing cmd_vel will come here"
 – name:        “Navigation stack"

   topic:       “nav_cmd_vel"

   timeout:     0.5

   priority:    1

   short_desc:  “ROS navigation stack controller"
 – name:        “Safety Controller"

   topic:       “cmd_vel_safety"

   timeout:     0.2

   priority:    10

   short_desc:  “Kobuki’s safety controller"
 – name:        “Keyboard operation"

   topic:       “key_cmd_vel"

   timeout:     0.1

   priority:    9
 – name:        “Remote control"

   topic:       “rem_cmd_vel"

   timeout:     0.1

   priority:    8
 – name:        “Onboard joystick"

   topic:       “joy_cmd_vel"

   timeout:     0.1

   priority:    7
 – name:        “Web application"

   topic:       “web_cmd_vel"

   timeout:     0.3

   priority:    6

 

平滑輸出速率!

 

儘管 ros_control 架構下的控制程式本身就有速度平滑化的特點。但萬一我們使用的平台並非遵守這個架構,或者只想雙倍確認速度控制不會出錯,那麼我們可以使用 Yujin Robot 「出品」的 yocs_velocity_smoother 。方法很簡單,把 cmd_vel_mux 輸出的速度指令直接接到 yocs_velocity_smoother 的入口,這樣子,以目前 Yujin 給的 PID 參數,應該就可以應付您的機器人控制了。

 

從官網的ROS Wiki網頁上可以看得出來,Velocity Smoother Nodelet 的輸入與輸出:

輸入:

~raw_cmd_vel (geometry_msgs/Twist)

~odometry (nav_msgs/Odometry)

~robot_cmd_vel (geometry_msgs/Twist)

 

其中,如果沒有額外付上其他閉路控制設計,那麼 raw_cmd_vel 的輸入 topic 可以和輸入 robot_cmd_vel 的一樣。

 

輸出:

~smooth_cmd_vel (geometry_msgs/Twist):即平滑化後的速度指令,直接給 base_controller 節點。

此外,對於網頁上提及的參數,也可以另外寫成一個設定檔,由 launch 載入,讓其自動存進 parameter server 裡面。範例如下:

 

# Example configuration:

# – velocity limits are around a 10% above the physical limits

# – acceleration limits are just low enough to avoid jerking
# Mandatory parameters

speed_lim_v: 0.8

speed_lim_w: 5.4
accel_lim_v: 0.3

accel_lim_w: 3.5
# Optional parameters

frequency: 20.0

decel_factor: 1.0
# Robot velocity feedback type:

#  0 – none

#  1 – odometry

#  2 – end robot commands

robot_feedback: 2

 

在 launch 檔載入的方式其實已經出現在上述的範例裡面,我們節錄出來:

 

  <!– ****Velocity Smoother (just regulate the overall output from cmd_vel_mux) ***** –>

 <node pkg="nodelet" type="nodelet" name="$(arg smoother_node_name)"

       args="load yocs_velocity_smoother/VelocitySmootherNodelet $(arg nodelet_manager_name)">

   <!– parameters –>

   <rosparam file="$(arg config_file)" command="load"/>

 </node>

 

好了,如果把所有的節點搞定,輸出與輸入或對接的 Topics 都有接對(我還真的自己拿筆出來畫圖!),那麼,現在就可以打開你的機器人,遠端遙控一下!光是只給一個 Twist 都可以發現機器人的移動行為變了!這就表示,你成功了!

 

對於 Navigation Stack 的基本了解

呼!從何談起?

Navigation Stack 在 ROS 整個架構中佔相當重要的份量,但是截至目前為止,我個人認為網路上能找到的資料雜亂不堪,官方教學有些部分並沒有寫得太清楚,對於剛要入門的新手而言,反而會起更大的心理壓力,不知從何學起。那我就以我自己的理解盡量寫,希望能幫助各位了解 Navigation Stack,並附上學理部分的講解,希望可以讓大家更加熟悉其徵的道理。

楔子

關於Navigation Stack 的介紹,我建議先看過我的良師益友賴柏任的部落格,他整理得很清楚。此外,如果要以更實際的方向來看,可以參考這位網友的文章。而對我而言,Navigation Stack 是一整組可以讓機器人或自動化載具可以在空間中全穩定的移動的相關程式。這些程式可以大致上分為:

  • 感測器、馬達編碼器、各種感測器的輸入輸出
  • 世界、機器人各關節之座標轉換
  • 即時建圖與定位
  • 分層導航(路徑規劃)
  • 上層傳動機制

別緊張,我們下面會娓娓道來。

overview_tf_small

相信大家對上面這張圖並不陌生。這一大組程式大致上就符合上面五個種類。

感測器、馬達編碼器、各種感測器的輸入輸出

這包括:odometry source, sensor sources

每個感測器都有一個驅動程式負責擷取底層的資料,並轉換成 ROS 格式的 messages,例如在途中可以看到的 senosr_msgs/LaserScan, sensor_msgs/PointCloud, nav_msgs/Odometry。ROS 的特有應用就是,其他節點可以收到這些資料,並做後續的處理。

世界、機器人各關節之座標的轉換

這包括:sensors transform

這跟機器人的定位有著密切的關係。可以參考這邊。機器人內部個關節的定義通常寫在 URDF 內,由最中心的關節 base_link 連接到其他元件,有其相對的位置與幾何關係。此外,馬達的傳動也會關係到 odom 相對於虛擬位於機器人正中心的座標 base_footprint的關係,最後,odometry 座標到 map 座標之間的關係,則會由 SLAM 的節點提供。

目前可能看得一頭霧水,沒關係,我們只要知道,必須有程式隨時關心各元件相對於世界的位置關係。

即時建圖與定位

這包括:amcl, map_server, gmapping

讓機器人能自動定位自己在空間中的位置,對於對於環境的感知,以及規劃到達目的地的路徑都相當重要。大家耳熟能詳的 Simultaneous Localization and Mapping (SLAM)演算法實際上可以直接使用 ROS 中的 gmapping 包寡達成,而裡面的定位,仍然是使用 AMCL實作的 Adaptive Monte Carlo Localization 定位演算法,依照編碼器推斷出 odometry 給出自己相對於周遭空間的關係。

分層導航(路徑規劃)

這包括:global planner, local planner, global costmap, local costmap, recovery_behaviour

ROS 架構採取分層導航的方式,使得機器人的導航更安全。系統一方面會用較低更新率的全域路徑規劃演算法計算出從現在位置到終點位置的路徑,另一方面,會使用更高頻的節點做路徑規劃。move_base 建立在 nav_core 基礎上,把這幾個程式,或節點,包起來,讓使用者只需呼叫一個節點,就可以包辦全部。其中運作的機制,其實就是上圖的架構。此外,move_base 把所得到的地圖轉換成象棋盤一樣,一格一格的 grid map,上面用數值標定障礙物與可通行空間的數值(如果障礙物的數值為1、可通行的空間為0,則整份地圖就成為二元地圖 binary map)(更多學理可以參考這裡)這樣的地圖被稱之為 Occupancy Map,這種格式尚無法拿來做導航,所以會再轉成 cost map 的格式,方能實行路經規劃。

當地圖有了,Global Planer 便根據這張 costmap、座標關係、位置座標作為依據,在有限的時間內規劃出安全的路徑。路徑規劃的演算法則從上個世紀60年代以來第一次提出路徑規劃演算法,便不斷精益求精,隨著經驗的積累,更貼近真實應用的能力。但是通常這樣的路徑規劃並沒有顧及實際機器模型在空間中要怎麼移動,這即是所謂的"The piano moving problem"。想像你搬家時,好幾個搬運工辛苦的將你家的平台式鋼琴左僑右僑避開你家的家具搬到電梯口。在機器人導航中,這牽涉到機器人動力運動學(kinodynamics)。由於這攸關於瞬息萬變在空間中移動,所以區域規劃計算的更新率必須比全域規劃頻率還要高,譬如說全域頻率為 20 Hz,但區域頻率就必須提高到 50 Hz甚至更高。

從官方的文件中可以得知,move_base 的 global planner 預設使用 Dijstra 演算法。大家可能會問,欸?那 A* 呢?可能 Willow Garrage 真的太忙了,所以直到 2013年,David Lu! 才把後者寫成一個 Global Planner Plug-in,方才能供大家使用。

抱歉,八卦說的有點多,請先別擔心。

上層傳動機制

這包括:base_control

當區域規劃已經算出一條區域路徑後,便會輸出一個虛擬層的速度 /cmd_vel (原來的名稱:command velocity)。這個速度再透過 base_control轉成真正對每顆馬達輸出訊號,使馬達轉動,讓機體到達目的地。這不只是輪型機器人而已,也可能是機器手臂上每顆馬達的轉動速度。

 

所以…move_base怎麼用?

一般來說,要用的時候必須要啟動建圖、座標轉換、sensor_transform, sensor_source, move_base 等節點節點。如果要使用不同的規劃演算法,可以把演算法寫成 plug-in 加進 move_base 中啟動,這部分之後會再解說。整體怎麼啟動,也會用實際範例解說。