雷鋒網(wǎng)按：本文作者qqfly，上海交通大學(xué)機(jī)器人所博士生，本科畢業(yè)于清華大學(xué)機(jī)械工程系，主要研究方向機(jī)器視覺(jué)與運(yùn)動(dòng)規(guī)劃，本文首發(fā)作者微信公眾號(hào)：Nao(ID：qRobotics)，雷鋒網(wǎng)已獲授權(quán)。

兩個(gè)月前給自己挖了個(gè)坑，說(shuō)要寫寫MoveIt，但一直沒(méi)動(dòng)手。主要有兩個(gè)原因： 

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

2）直接講MoveIt似乎需要挖更多坑，一直沒(méi)想好怎么寫比較好。 

主要是因?yàn)闄C(jī)器人運(yùn)動(dòng)規(guī)劃涉及太多基礎(chǔ)內(nèi)容，如果跳過(guò)不講就會(huì)變成新坑；一時(shí)半會(huì)又沒(méi)法講完。 

所以，這次就從初學(xué)者如何利用MoveIt快速搭建機(jī)器人運(yùn)動(dòng)規(guī)劃平臺(tái)來(lái)講吧，先展示Big Picture，其他細(xì)節(jié)內(nèi)容以后有空再慢慢填。但可能會(huì)坑的地方我會(huì)用（坑）標(biāo)注出來(lái)。 

什么是MoveIt

先看個(gè)視頻介紹吧：

   

看完視頻，大家應(yīng)該對(duì)MoveIt有一個(gè)大概的印象了。用MoveIt官網(wǎng)（moveit.ros.org）的說(shuō)法： 

它是目前最先進(jìn)的移動(dòng)（坑）操作機(jī)器人軟件，整合了最先進(jìn)的運(yùn)動(dòng)規(guī)劃、操作、3D感知、運(yùn)動(dòng)學(xué)、控制與導(dǎo)航算法。為這方面的開(kāi)發(fā)人員提供了一個(gè)十分便利的開(kāi)發(fā)平臺(tái)。 

這個(gè)說(shuō)法不太直觀，換個(gè)說(shuō)法就是MoveIt = RobotGo，翻譯成中文就是“機(jī)器人，走你！” 

所以，MoveIt的主要就是一款致力于讓機(jī)器人能夠自主運(yùn)動(dòng)及其相關(guān)技術(shù)的軟件，它的所有模塊都是圍繞著運(yùn)動(dòng)規(guī)劃的實(shí)現(xiàn)而設(shè)計(jì)的。 

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

• 運(yùn)動(dòng)規(guī)劃（Motion Planning）：運(yùn)動(dòng)規(guī)劃的介紹內(nèi)容之前公眾號(hào)已經(jīng)發(fā)過(guò)了，要讓一個(gè)機(jī)器人實(shí)現(xiàn)運(yùn)動(dòng)規(guī)劃，需要先將機(jī)器人抽象到構(gòu)形空間（C-Space）。MoveIt就可以幫大家把這些工作給做了，只需提供機(jī)器人URDF模型，就可以調(diào)用幾大運(yùn)動(dòng)規(guī)劃庫(kù)（坑）的規(guī)劃算法（如OMPL，SBPL，CHMOP），自動(dòng)生成機(jī)器人運(yùn)動(dòng)軌跡。 

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

• 3D感知（Perception）：這個(gè)并不是說(shuō)MoveIt整合了物體識(shí)別、環(huán)境建模等模塊，而是它可以利用傳感器（坑）采集的信息（點(diǎn)云或深度圖像）生成用于碰撞檢測(cè)的OctoMap。OctoMap這個(gè)東西挺好的，做SLAM的同學(xué)應(yīng)該了解，它就是以八叉樹(shù)形式表示點(diǎn)云，可以大大降低存儲(chǔ)空間，它看起來(lái)就跟你們玩的minecraft差不多。同時(shí)，這些3D OctoMap也可以依據(jù)貝葉斯準(zhǔn)則不斷實(shí)時(shí)更新。這樣，機(jī)器人就可以避開(kāi)真實(shí)世界的障礙物了。 

• 運(yùn)動(dòng)學(xué)（Kinematics）：運(yùn)動(dòng)學(xué)機(jī)器人工作空間與構(gòu)形空間（C-Space）的映射關(guān)系，所以MoveIt就它也包括在自己系統(tǒng)內(nèi)。目前它可以支持多種運(yùn)動(dòng)學(xué)求解器，如OpenRave的ikfast（封閉解）、Orocos的KDL（數(shù)值解）、Trac_ik（考慮關(guān)節(jié)極限的數(shù)值解）、基于service的求解器（用戶自己定義）。（坑） 

• 碰撞檢測(cè)（Collision Checking）：碰撞檢測(cè)是運(yùn)動(dòng)規(guī)劃的一大難題，如果采用基于采用的規(guī)劃算法，那么我們需要對(duì)每個(gè)采樣點(diǎn)做有效性判斷，這時(shí)候就需要進(jìn)行碰撞檢測(cè)。所以，運(yùn)動(dòng)規(guī)劃需要提供一個(gè)高效的碰撞檢測(cè)算法。幸好，香港城市大學(xué)的潘佳大神寫了個(gè)FCL（Flexible Collision Library），可以非?？焖俚貙?shí)現(xiàn)各種幾何體（3D面片、OctoMap、基本幾何體）的碰撞檢測(cè)。（這個(gè)不是坑，直接用很好用，但以后有機(jī)會(huì)可以好好說(shuō)說(shuō)，反正潘大神不會(huì)看朋友圈，不至于班門弄斧）。 

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

• 控制（Controll）：這個(gè)其實(shí)不能算控制，只是一個(gè)機(jī)器人控制接口問(wèn)題。由于不同機(jī)器人的控制接口都不一樣，開(kāi)發(fā)者只需簡(jiǎn)單修改配置文件，就可以讓MoveIt發(fā)布出機(jī)器人相應(yīng)的控制指令（只是修改action名字而已）。 

• 導(dǎo)航（Navigation）：這是個(gè)大坑，MoveIt雖然原理上可以進(jìn)行移動(dòng)機(jī)器人的導(dǎo)航，但是它沒(méi)提供針對(duì)移動(dòng)機(jī)器人的規(guī)劃器。也就是說(shuō)目前它的Navigation功能是不能直接使用的（自己寫規(guī)劃器和碰撞檢測(cè)已經(jīng)超出初學(xué)者的要求了）。 

• 交互（Interaction）：MoveIt給開(kāi)發(fā)者提供了三種方式交互方式，Rviz圖形界面（直觀）、Python（快速編程）、C++（豐富的高級(jí)功能）。 

目前而言，MoveIt還只是一個(gè)針對(duì)機(jī)械臂運(yùn)動(dòng)規(guī)劃問(wèn)題的軟件平臺(tái)，暫時(shí)還不是適合用在Navigation、manipulation、control、perception等領(lǐng)域。 

為什么選擇MoveIt

其實(shí)，要做運(yùn)動(dòng)規(guī)劃可以用很多方法：自己寫、OMPL、OpenRave等。我個(gè)人認(rèn)為選擇MoveIt大概有三個(gè)原因： 

| 對(duì)初學(xué)者很友好 

前面我們已經(jīng)知道，要想自己從頭建立一個(gè)運(yùn)動(dòng)規(guī)劃的軟件平臺(tái)需要花費(fèi)非常多力氣：運(yùn)動(dòng)學(xué)正逆解、碰撞檢測(cè)算法、環(huán)境識(shí)別、規(guī)劃算法，任何一點(diǎn)都是需要一段不短的時(shí)間積累才可能親手實(shí)現(xiàn)的。初學(xué)者可能一開(kāi)始就被這些次要問(wèn)題打退了。 

自己寫：對(duì)初學(xué)者簡(jiǎn)直是災(zāi)難，尤其是沒(méi)人帶的情況； 

OMPL：完全沒(méi)有機(jī)器人的概念，需要用戶自己集成運(yùn)動(dòng)學(xué)、碰撞檢測(cè)算法，它的官方文檔也不涉及如何利用OMPL做機(jī)器人的運(yùn)動(dòng)規(guī)劃問(wèn)題； 

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

用MoveIt的話，初學(xué)者只需準(zhǔn)備機(jī)器人的模型，跟著教程走，便可以在半小時(shí)內(nèi)實(shí)現(xiàn)仿真環(huán)境中的機(jī)器人運(yùn)動(dòng)規(guī)劃演示。 

| 方便研究 

這個(gè)應(yīng)該是更重要的因素。運(yùn)動(dòng)規(guī)劃由很多子問(wèn)題構(gòu)成，每個(gè)子問(wèn)題都可以成為一個(gè)研究點(diǎn)。MoveIt幾乎所有組件都是以Plugin的形式工作的，也就是說(shuō)我們可以隨時(shí)更換它的任一模塊。目前它支持以下組件的修改： 

運(yùn)動(dòng)學(xué)求解器 

規(guī)劃算法 同時(shí)OMPL自身也支持用戶自己編寫規(guī)劃器 

規(guī)劃器初始化方法 

控制器接口 

傳感器接口 

規(guī)劃器的采樣算法 

碰撞檢測(cè)算法 

OctoMap更新算法 

如果是做這些課題研究的人，完全可以先用MoveIt建立一個(gè)環(huán)境，之后修改相應(yīng)Plugin，換成自己的算法。這樣可以讓我們將側(cè)重點(diǎn)放在主要矛盾上。 

| 活躍的社群 

這點(diǎn)其實(shí)就是ROS相對(duì)于其他機(jī)器人開(kāi)發(fā)平臺(tái)的優(yōu)點(diǎn)。MoveIt依托于ROS，也擁有很高的人氣（去年的調(diào)研結(jié)果看，MoveIt是ROS中使用度排名第三的package）。 

活躍的社群對(duì)于學(xué)習(xí)是大有裨益的： 

• ①遇到問(wèn)題很容易問(wèn)到能解決的人，剛開(kāi)始MoveIt還沒(méi)出文檔，我就是靠著MoveIt的mail lists入門的； 

• ②網(wǎng)站、教程、代碼維護(hù)更新很好，MoveIt剛推出時(shí)，總是有一大堆Bug，現(xiàn)在才過(guò)幾年，已經(jīng)非常好用了，官方教程也已經(jīng)非常人性化了。 

怎么樣使用MoveIt

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

建立機(jī)器人URDF模型（必須） 

建立機(jī)器人ROS驅(qū)動(dòng) 

生成MoveIt配置文件（必須） 

標(biāo)定相機(jī) 

修改MoveIt配置文件與launch文件 

機(jī)器人，走你！（必須） 

其中，上面未標(biāo)明“（必須）”字樣的只有在使用實(shí)際機(jī)器人時(shí)才需要。初學(xué)者如果只想在仿真里看看的話，可以先跳過(guò)。 

| 建立機(jī)器人URDF模型（必須） 

URDF（Unified Robot Description Format）是ROS中使用的一種機(jī)器人描述文件，它以HTML的形式定義一個(gè)機(jī)器人。包含的內(nèi)容有：連桿、關(guān)節(jié)名稱，運(yùn)動(dòng)學(xué)參數(shù)、動(dòng)力學(xué)參數(shù)、可視化模型、碰撞檢測(cè)模型等。

后續(xù)碰撞檢測(cè)、運(yùn)動(dòng)學(xué)求解、規(guī)劃等都依賴于URDF文件。 

那么，要如何建立URDF文件呢？如果你用的是單臂、串聯(lián)機(jī)器人，并且你本人沒(méi)有強(qiáng)迫癥的話，可以使用ROS官方發(fā)布的sw_urdf_exporter，它可以幫你從SolidWorks中導(dǎo)出URDF文件。

但如果不幸你使用的是雙臂（雙臂機(jī)器人用這個(gè)插件經(jīng)常出問(wèn)題）或者非并聯(lián)機(jī)器人（需要自己用mimic_joint改成串聯(lián)形式），又或者你有強(qiáng)迫癥（想要盡量簡(jiǎn)潔、漂亮的模型）的話，可以考慮自己手寫URDF或者xacro文件（坑）。 

這點(diǎn)我就不具體說(shuō)了，簡(jiǎn)單寫幾個(gè)要點(diǎn)： 

多臂用xacro來(lái)減少工作量； 

坐標(biāo)系設(shè)置盡量滿足所有關(guān)節(jié)為0°時(shí)候，所有坐標(biāo)系同姿態(tài)（這樣可以避免引入pi）； 

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

可視化模型采用漂亮、精細(xì)的模型，碰撞模型可以使用簡(jiǎn)化的模型。 

| 建立機(jī)器人ROS驅(qū)動(dòng) 

如果你不用真實(shí)機(jī)器人，這步可以先跳過(guò)。 

機(jī)器人的ROS驅(qū)動(dòng)并沒(méi)有什么標(biāo)準(zhǔn)的格式或者規(guī)定。對(duì)于MoveIt而言，只要求你有個(gè)ROS node，它有兩個(gè)功能： 

1）發(fā)布關(guān)節(jié)角度/joint_states 

如果連接實(shí)際機(jī)器人，MoveIt需要從機(jī)器人當(dāng)前狀態(tài)開(kāi)始規(guī)劃，因此這個(gè)ROS驅(qū)動(dòng)需要能夠?qū)崟r(shí)獲取機(jī)器人的各關(guān)節(jié)信息（如角度），并用過(guò)/joint_states消息發(fā)布； 

2）接收規(guī)劃結(jié)果，并下發(fā)給機(jī)器人 

由于MoveIt規(guī)劃的結(jié)果會(huì)以一個(gè)action的形式發(fā)布，所以我們的ROS驅(qū)動(dòng)就應(yīng)該提供一個(gè)action server，這個(gè)功能就是接收規(guī)劃結(jié)果，下發(fā)給機(jī)器人，并反饋執(zhí)行情況。action的類型是control_msgs/FollowJointTrajectory。 具體action的寫法可以參照ROS官網(wǎng)（坑）。

簡(jiǎn)單而言，一個(gè)action有五個(gè)部分： 

• action_name/goal：這個(gè)就是規(guī)劃的路徑，我們需要接收這個(gè)路徑，并將所有路徑點(diǎn)解析成機(jī)器人控制器可以識(shí)別的形式，之后下發(fā)給機(jī)器人，必須要有； 

• action_name/cancel：這個(gè)指令可以隨時(shí)中斷正在執(zhí)行的動(dòng)作，但并不是必須的功能； 

• action_name/feedback：這個(gè)是實(shí)時(shí)反饋執(zhí)行狀態(tài)，最簡(jiǎn)單的就是將機(jī)器人當(dāng)前關(guān)節(jié)角度等信息反饋回去，非必須； 

• action_name/status：這個(gè)用于顯示機(jī)器人狀態(tài)，如正在執(zhí)行動(dòng)作、等待、執(zhí)行結(jié)束等待，非必須； 

• action_name/result：這個(gè)就是在動(dòng)作執(zhí)行完之后給MoveIt反饋一個(gè)執(zhí)行結(jié)果，這個(gè)是必須要有的，當(dāng)然，為了簡(jiǎn)單，可以已接收到goal就反饋執(zhí)行成功。 

這部分在MoveIt部分是看不到文檔的，所以也是阻礙初學(xué)者使用MoveIt控制自己機(jī)器人的最大問(wèn)題之一。但是了解了它的機(jī)理之后，就比較簡(jiǎn)單了。 如果你是第一次使用MoveIt，極力推薦你先試試UR、Baxter等已經(jīng)寫好這部分驅(qū)動(dòng)的機(jī)器人。 

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

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

這步做完，就可以直接在仿真里面看運(yùn)動(dòng)規(guī)劃效果了。這是我覺(jué)得MoveIt對(duì)初學(xué)者最友善的地方，不用寫一行代碼就可以看到運(yùn)動(dòng)規(guī)劃。 

| 標(biāo)定相機(jī) 

這個(gè)主要涉及相機(jī)模型與AX=XB求解問(wèn)題，不多贅述。 這步就是為了讓機(jī)器人知道攝像頭放在機(jī)器人的哪個(gè)位置。大家可以看看我實(shí)驗(yàn)室?guī)煹軐懙淖詣?dòng)標(biāo)定演示： 

一個(gè)launch文件就能完成標(biāo)定，不知比我當(dāng)年寫的手動(dòng)標(biāo)定方法高多少。這個(gè)標(biāo)定程序我們之后可能會(huì)開(kāi)源出來(lái)。如果有機(jī)會(huì)我也會(huì)順便講講它的原理（坑）。 

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

因?yàn)榍懊嫔傻奈募际轻槍?duì)虛擬機(jī)器人的，如果需要連接實(shí)際機(jī)器人，需要修改一些配置文件，我可能記不太清具體要修改幾個(gè)文件了，請(qǐng)以官方教程為主： 

• controllers.yaml：這個(gè)就是要根據(jù)你的ROS驅(qū)動(dòng)中的action來(lái)修改，MoveIt可以根據(jù)這個(gè)配置文件發(fā)布出與機(jī)器人驅(qū)動(dòng)相匹配的action。簡(jiǎn)單地說(shuō)，就是action的名字、類型、關(guān)節(jié)名字幾個(gè)信息。 

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

• sensors.yaml：這個(gè)需要額外增加，它主要定義了點(diǎn)云的消息名稱、OctoMap屬性等。 

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

• 其他（可選）：industrial_robot_simulator、warehouse、joystick、規(guī)劃器、規(guī)劃算法庫(kù)…… 

• 機(jī)器人，走你！（必須） 

上述內(nèi)容完成后，就可以Enjoy Yourself了，無(wú)論是Rviz, Python 還是C++，都可以用來(lái)進(jìn)行運(yùn)動(dòng)規(guī)劃，如果連接了真實(shí)機(jī)器人，那么也可以在實(shí)際機(jī)器人上完成運(yùn)動(dòng)規(guī)劃。 

其他 

雖然寫了這么多，但感覺(jué)還是沒(méi)寫清楚，最后隨便列點(diǎn)之前大家在后臺(tái)問(wèn)過(guò)的比較多的問(wèn)題： 

| 什么機(jī)器人能用MoveIt 

MoveIt其實(shí)跟機(jī)器人關(guān)系不大，只要你有URDF文件，能接受控制指令，那么就能用MoveIt，移動(dòng)機(jī)器人的話也可以，只是MoveIt現(xiàn)在沒(méi)有針對(duì)Navigation做規(guī)劃器。 

| 怎么用MoveIt做移動(dòng)機(jī)器人3D Navigation 

相對(duì)于傳統(tǒng)Navigation包，MoveIt中可以做3D的碰撞檢測(cè)，但是它尚未加入適合移動(dòng)平臺(tái)的規(guī)劃算法。大概做法如下： 

• 修改FCL，開(kāi)放出碰撞檢測(cè)函數(shù)（最新版本好像已經(jīng)可以直接調(diào)用了）； 

• 寫一個(gè)規(guī)劃器：最簡(jiǎn)單的就是自己寫一個(gè)A*或Dijkstra，也可以想辦法將SBPL用起來(lái)（我沒(méi)在移動(dòng)機(jī)器人上試過(guò)），這樣就可以進(jìn)行全局規(guī)劃了； 

• 寫一個(gè)action server，接受規(guī)劃結(jié)果，同時(shí)將其轉(zhuǎn)換成Navigation包的gloal_planner相同格式，利用Navigation的local_planner完成路徑跟蹤；當(dāng)然，這一步也可以自己寫local_planner。 

| 怎么用MoveIt做飛行器或潛艇的路徑規(guī)劃 

這個(gè)與上個(gè)問(wèn)題類似，MoveIt沒(méi)有針對(duì)剛體的規(guī)劃算法，如果可以接受RRT的規(guī)劃結(jié)果，那么理論上講是可以直接使用的。 

| 如何在MoveIt上使用自己的規(guī)劃算法 

我只嘗試過(guò)先在OMPL中寫自己的規(guī)劃器，之后通過(guò)修改moveit_planner中的ompl_interface，將自己的規(guī)劃器用到MoveIt中； 如果是非Sampling-based方法，那就要去看看MoveIt的Plugin怎么改了，這部分我沒(méi)經(jīng)驗(yàn)。 

| 如何學(xué)習(xí)MoveIt 

關(guān)注我的公眾號(hào)（劃重點(diǎn)）； 

• 學(xué)ROS基本概念：三種消息機(jī)制等； 

• 學(xué)教程：按照官網(wǎng)教程走一遍； 

• 遇到問(wèn)題，先在ROS問(wèn)答區(qū)或MoveIt的mail lists搜索是否有同類問(wèn)題，如沒(méi)有，則自己在上述平臺(tái)提問(wèn)； （至此， 你已經(jīng)會(huì)用MoveIt了，但用得效果肯定不好） 

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

• 根據(jù)自己需要，修改部分源碼（例如開(kāi)放出FCL的各種功能），之后再M(fèi)oveIt官方github上提出修改源碼請(qǐng)求（PR）； （至此，你已經(jīng)掌握了MoveIt這個(gè)工具，可以充分發(fā)揮MoveIt的功能） 

• 根據(jù)自己的研究?jī)?nèi)容，寫自己的Plugin，充分發(fā)掘MoveIt的潛力； 

• 如果效果好，那么在IROS/ICRA發(fā)paper，會(huì)議中找MoveIt的作者們聊天，回家后到github上將自己的代碼開(kāi)源，并PR到MoveIt上。 

 

機(jī)器人，走你！                        

雷峰網(wǎng)版權(quán)文章，未經(jīng)授權(quán)禁止轉(zhuǎn)載。詳情見(jiàn)轉(zhuǎn)載須知。