如何利用ROS MoveIt快速搭建機器人運動規劃平臺?

雷鋒網(公眾號：雷鋒網)按：本文作者qqfly，上海交通大學機器人所博士生，本科畢業於清華大學機械工程系，主要研究方向機器視覺與運動規劃，本文首發作者微信公眾號：Nao(ID：qRobotics)，雷鋒網已獲授權。

兩個月前給自己挖了個坑，說要寫寫MoveIt，但一直沒動手。主要有兩個原因： 

1）這兩個月主要在寫小論文，畢竟博士生要畢業還得看論文，不能靠公眾號閱讀量分享率； 

2）直接講MoveIt似乎需要挖更多坑，一直沒想好怎麼寫比較好。 

主要是因為機器人運動規劃涉及太多基礎內容，如果跳過不講就會變成新坑；一時半會又沒法講完。 

所以，這次就從初學者如何利用MoveIt快速搭建機器人運動規劃平臺來講吧，先展示Big Picture，其他細節內容以後有空再慢慢填。但可能會坑的地方我會用（坑）標註出來。 

什麼是MoveIt

先看個視頻介紹吧：

   

看完視頻，大家應該對MoveIt有一個大概的印象了。用MoveIt官網（moveit.ros.org）的說法： 

它是目前最先進的移動（坑）操作機器人軟體，整合了最先進的運動規劃、操作、3D感知、運動學、控制與導航算法。為這方面的開發人員提供了一個十分便利的開發平臺。 

這個說法不太直觀，換個說法就是MoveIt = RobotGo，翻譯成中文就是「機器人，走你！」 

所以，MoveIt的主要就是一款致力於讓機器人能夠自主運動及其相關技術的軟體，它的所有模塊都是圍繞著運動規劃的實現而設計的。 

下面大概介紹下它的一些功能模塊。 

• 運動規劃（Motion Planning）：運動規劃的介紹內容之前公眾號已經發過了，要讓一個機器人實現運動規劃，需要先將機器人抽象到構形空間（C-Space）。MoveIt就可以幫大家把這些工作給做了，只需提供機器人URDF模型，就可以調用幾大運動規劃庫（坑）的規划算法（如OMPL，SBPL，CHMOP），自動生成機器人運動軌跡。 

• 操作（Manipulation）：這個目前還比較弱，就是根據識別的物體生成一系列動作抓取物體（pick-and-place），不涉及任何反饋、動力學、re-grasp等操作問題，所以我一般都不用這個模塊。 

• 3D感知（Perception）：這個並不是說MoveIt整合了物體識別、環境建模等模塊，而是它可以利用傳感器（坑）採集的信息（點雲或深度圖像）生成用於碰撞檢測的OctoMap。OctoMap這個東西挺好的，做SLAM的同學應該了解，它就是以八叉樹形式表示點雲，可以大大降低存儲空間，它看起來就跟你們玩的minecraft差不多。同時，這些3D OctoMap也可以依據貝葉斯準則不斷實時更新。這樣，機器人就可以避開真實世界的障礙物了。 

• 運動學（Kinematics）：運動學機器人工作空間與構形空間（C-Space）的映射關係，所以MoveIt就它也包括在自己系統內。目前它可以支持多種運動學求解器，如OpenRave的ikfast（封閉解）、Orocos的KDL（數值解）、Trac_ik（考慮關節極限的數值解）、基於service的求解器（用戶自己定義）。（坑） 

• 碰撞檢測（Collision Checking）：碰撞檢測是運動規劃的一大難題，如果採用基於採用的規划算法，那麼我們需要對每個採樣點做有效性判斷，這時候就需要進行碰撞檢測。所以，運動規劃需要提供一個高效的碰撞檢測算法。幸好，香港城市大學的潘佳大神寫了個FCL（Flexible Collision Library），可以非常快速地實現各種幾何體（3D面片、OctoMap、基本幾何體）的碰撞檢測。（這個不是坑，直接用很好用，但以後有機會可以好好說說，反正潘大神不會看朋友圈，不至於班門弄斧）。 

• 軌跡插值（Trajectory Processing）：由於大多數規劃器只能返回一系列路徑點，MoveIt可以根據機器人的控制參數（速度、加速度限制等）重新處理路徑，生成一條帶有時間戳、位置、速度、加速度信息的完整軌跡。 

• 控制（Controll）：這個其實不能算控制，只是一個機器人控制接口問題。由於不同機器人的控制接口都不一樣，開發者只需簡單修改配置文件，就可以讓MoveIt發布出機器人相應的控制指令（只是修改action名字而已）。 

• 導航（Navigation）：這是個大坑，MoveIt雖然原理上可以進行移動機器人的導航，但是它沒提供針對移動機器人的規劃器。也就是說目前它的Navigation功能是不能直接使用的（自己寫規劃器和碰撞檢測已經超出初學者的要求了）。 

目前而言，MoveIt還只是一個針對機械臂運動規劃問題的軟體平臺，暫時還不是適合用在Navigation、manipulation、control、perception等領域。 

為什麼選擇MoveIt

其實，要做運動規劃可以用很多方法：自己寫、OMPL、OpenRave等。我個人認為選擇MoveIt大概有三個原因： 

| 對初學者很友好 

前面我們已經知道，要想自己從頭建立一個運動規劃的軟體平臺需要花費非常多力氣：運動學正逆解、碰撞檢測算法、環境識別、規划算法，任何一點都是需要一段不短的時間積累才可能親手實現的。初學者可能一開始就被這些次要問題打退了。 

自己寫：對初學者簡直是災難，尤其是沒人帶的情況； 

OMPL：完全沒有機器人的概念，需要用戶自己集成運動學、碰撞檢測算法，它的官方文檔也不涉及如何利用OMPL做機器人的運動規劃問題； 

OpenRave：雖然OpenRave已經有自己的一套機器人描述方法，但是它的文檔不太友好。 

用MoveIt的話，初學者只需準備機器人的模型，跟著教程走，便可以在半小時內實現仿真環境中的機器人運動規劃演示。 

| 方便研究 

這個應該是更重要的因素。運動規劃由很多子問題構成，每個子問題都可以成為一個研究點。MoveIt幾乎所有組件都是以Plugin的形式工作的，也就是說我們可以隨時更換它的任一模塊。目前它支持以下組件的修改： 

運動學求解器 

規划算法同時OMPL自身也支持用戶自己編寫規劃器 

規劃器初始化方法 

控制器接口 

傳感器接口 

規劃器的採樣算法 

碰撞檢測算法 

OctoMap更新算法 

如果是做這些課題研究的人，完全可以先用MoveIt建立一個環境，之後修改相應Plugin，換成自己的算法。這樣可以讓我們將側重點放在主要矛盾上。 

| 活躍的社群 

這點其實就是ROS相對於其他機器人開發平臺的優點。MoveIt依託於ROS，也擁有很高的人氣（去年的調研結果看，MoveIt是ROS中使用度排名第三的package）。 

活躍的社群對於學習是大有裨益的： 

怎麼樣使用MoveIt

要用MoveIt控制機器人大概分為以下幾步： 

建立機器人URDF模型（必須） 

建立機器人ROS驅動 

生成MoveIt配置文件（必須） 

標定相機 

修改MoveIt配置文件與launch文件 

機器人，走你！（必須） 

其中，上面未標明「（必須）」字樣的只有在使用實際機器人時才需要。初學者如果只想在仿真裡看看的話，可以先跳過。 

| 建立機器人URDF模型（必須） 

URDF（Unified Robot Description Format）是ROS中使用的一種機器人描述文件，它以HTML的形式定義一個機器人。包含的內容有：連杆、關節名稱，運動學參數、動力學參數、可視化模型、碰撞檢測模型等。

後續碰撞檢測、運動學求解、規劃等都依賴於URDF文件。 

那麼，要如何建立URDF文件呢？如果你用的是單臂、串聯機器人，並且你本人沒有強迫症的話，可以使用ROS官方發布的sw_urdf_exporter，它可以幫你從SolidWorks中導出URDF文件。

但如果不幸你使用的是雙臂（雙臂機器人用這個插件經常出問題）或者非並聯機器人（需要自己用mimic_joint改成串聯形式），又或者你有強迫症（想要儘量簡潔、漂亮的模型）的話，可以考慮自己手寫URDF或者xacro文件（坑）。 

這點我就不具體說了，簡單寫幾個要點： 

多臂用xacro來減少工作量； 

坐標系設置儘量滿足所有關節為0°時候，所有坐標系同姿態（這樣可以避免引入pi）； 

如果想要有顏色的模型，可以自己生成每個零件的dae模型，而不使用stl模型； 

可視化模型採用漂亮、精細的模型，碰撞模型可以使用簡化的模型。 

| 建立機器人ROS驅動 

如果你不用真實機器人，這步可以先跳過。 

機器人的ROS驅動並沒有什麼標準的格式或者規定。對於MoveIt而言，只要求你有個ROS node，它有兩個功能： 

1）發布關節角度/joint_states 

如果連接實際機器人，MoveIt需要從機器人當前狀態開始規劃，因此這個ROS驅動需要能夠實時獲取機器人的各關節信息（如角度），並用過/joint_states消息發布； 

2）接收規劃結果，並下發給機器人 

由於MoveIt規劃的結果會以一個action的形式發布，所以我們的ROS驅動就應該提供一個action server，這個功能就是接收規劃結果，下發給機器人，並反饋執行情況。action的類型是control_msgs/FollowJointTrajectory。具體action的寫法可以參照ROS官網（坑）。

簡單而言，一個action有五個部分： 

• action_name/goal：這個就是規劃的路徑，我們需要接收這個路徑，並將所有路徑點解析成機器人控制器可以識別的形式，之後下發給機器人，必須要有； 

• action_name/cancel：這個指令可以隨時中斷正在執行的動作，但並不是必須的功能； 

• action_name/feedback：這個是實時反饋執行狀態，最簡單的就是將機器人當前關節角度等信息反饋回去，非必須； 

• action_name/status：這個用於顯示機器人狀態，如正在執行動作、等待、執行結束等待，非必須； 

• action_name/result：這個就是在動作執行完之後給MoveIt反饋一個執行結果，這個是必須要有的，當然，為了簡單，可以已接收到goal就反饋執行成功。 

這部分在MoveIt部分是看不到文檔的，所以也是阻礙初學者使用MoveIt控制自己機器人的最大問題之一。但是了解了它的機理之後，就比較簡單了。如果你是第一次使用MoveIt，極力推薦你先試試UR、Baxter等已經寫好這部分驅動的機器人。 

| 生成MoveIt配置文件（必須） 

這個利用MoveIt的setup assistant界面，按照教程很容易就能配置好。 

這步做完，就可以直接在仿真裡面看運動規劃效果了。這是我覺得MoveIt對初學者最友善的地方，不用寫一行代碼就可以看到運動規劃。 

| 標定相機 

這個主要涉及相機模型與AX=XB求解問題，不多贅述。這步就是為了讓機器人知道攝像頭放在機器人的哪個位置。大家可以看看我實驗室師弟寫的自動標定演示： 

一個launch文件就能完成標定，不知比我當年寫的手動標定方法高多少。這個標定程序我們之後可能會開源出來。如果有機會我也會順便講講它的原理（坑）。 

| 修改MoveIt配置文件與launch文件 

因為前面生成的文件都是針對虛擬機器人的，如果需要連接實際機器人，需要修改一些配置文件，我可能記不太清具體要修改幾個文件了，請以官方教程為主： 

• controllers.yaml：這個就是要根據你的ROS驅動中的action來修改，MoveIt可以根據這個配置文件發布出與機器人驅動相匹配的action。簡單地說，就是action的名字、類型、關節名字幾個信息。 

• robot_moveit_controller_manager.launch：這個額外新增，就是在不適用fake controller的時候能找到上述controller.yaml文件，發布出正確的action類型。 

• sensors.yaml：這個需要額外增加，它主要定義了點雲的消息名稱、OctoMap屬性等。 

• moveit_sensor_manager.launch：同樣的，增加傳感器配置文件後，我們也需要在launch文件中增加對配置文件的讀取。 

• 其他（可選）：industrial_robot_simulator、warehouse、joystick、規劃器、規划算法庫…… 

• 機器人，走你！（必須） 

上述內容完成後，就可以Enjoy Yourself了，無論是Rviz, Python 還是C++，都可以用來進行運動規劃，如果連接了真實機器人，那麼也可以在實際機器人上完成運動規劃。 

其他 

雖然寫了這麼多，但感覺還是沒寫清楚，最後隨便列點之前大家在後臺問過的比較多的問題： 

| 什麼機器人能用MoveIt 

MoveIt其實跟機器人關係不大，只要你有URDF文件，能接受控制指令，那麼就能用MoveIt，移動機器人的話也可以，只是MoveIt現在沒有針對Navigation做規劃器。 

| 怎麼用MoveIt做移動機器人3D Navigation 

相對於傳統Navigation包，MoveIt中可以做3D的碰撞檢測，但是它尚未加入適合移動平臺的規划算法。大概做法如下： 

• 修改FCL，開放出碰撞檢測函數（最新版本好像已經可以直接調用了）； 

• 寫一個規劃器：最簡單的就是自己寫一個A*或Dijkstra，也可以想辦法將SBPL用起來（我沒在移動機器人上試過），這樣就可以進行全局規劃了； 

• 寫一個action server，接受規劃結果，同時將其轉換成Navigation包的gloal_planner相同格式，利用Navigation的local_planner完成路徑跟蹤；當然，這一步也可以自己寫local_planner。 

| 怎麼用MoveIt做飛行器或潛艇的路徑規劃 

這個與上個問題類似，MoveIt沒有針對剛體的規划算法，如果可以接受RRT的規劃結果，那麼理論上講是可以直接使用的。 

| 如何在MoveIt上使用自己的規划算法 

我只嘗試過先在OMPL中寫自己的規劃器，之後通過修改moveit_planner中的ompl_interface，將自己的規劃器用到MoveIt中；如果是非Sampling-based方法，那就要去看看MoveIt的Plugin怎麼改了，這部分我沒經驗。 

| 如何學習MoveIt 

關注我的公眾號（劃重點）； 

• 學ROS基本概念：三種消息機制等； 

• 學教程：按照官網教程走一遍； 

• 遇到問題，先在ROS問答區或MoveIt的mail lists搜索是否有同類問題，如沒有，則自己在上述平臺提問；（至此， 你已經會用MoveIt了，但用得效果肯定不好） 

• 看MoveIt各部分API，闡釋其高級功能； 

• 根據自己需要，修改部分源碼（例如開放出FCL的各種功能），之後再MoveIt官方github上提出修改源碼請求（PR）；（至此，你已經掌握了MoveIt這個工具，可以充分發揮MoveIt的功能） 

• 根據自己的研究內容，寫自己的Plugin，充分發掘MoveIt的潛力； 

• 如果效果好，那麼在IROS/ICRA發paper，會議中找MoveIt的作者們聊天，回家後到github上將自己的代碼開源，並PR到MoveIt上。 

 

機器人，走你！                        

雷鋒網版權文章，未經授權禁止轉載。詳情見轉載須知。