機(jī)器人操作系統(tǒng)（ROS）課件 10 ROS 機(jī)器人仿真實(shí)驗(yàn)

機(jī)器人操作系統(tǒng)（ROS）機(jī)器人操作系統(tǒng)（ROS）機(jī)器人模型構(gòu)建與仿真10.1機(jī)器人操作系統(tǒng)（ROS）10.1.1URDF介紹URDF（UnifiedRobotDescriptionFormat，統(tǒng)一機(jī)器人描述格式）是一個(gè)XML語(yǔ)法框架下用來(lái)描述機(jī)器人的語(yǔ)言格式，主要用于存儲(chǔ)模型的形狀、尺寸、顏色等基本屬性。在構(gòu)建機(jī)器人模型之前，我們需要了解URDF的語(yǔ)法和常用的標(biāo)簽。（1）URDF語(yǔ)法在URDF中編輯文件需要一定的編寫語(yǔ)法，語(yǔ)言要求包含本體、關(guān)節(jié)、節(jié)點(diǎn)的定義以及節(jié)點(diǎn)間各關(guān)節(jié)的連接關(guān)系。下面將詳細(xì)介紹URDF中幾種常用的標(biāo)簽。（2）常用標(biāo)簽<link>標(biāo)簽<link>標(biāo)簽描述機(jī)器人某個(gè)剛體部分的外觀和物理屬性，包括連桿尺寸（size）、顏色（color）、形狀（shape）、慣性矩陣（inertialmatrix）、碰撞屬性（collisionproperties）等。機(jī)器人中每個(gè)link都會(huì)成為一個(gè)坐標(biāo)系。link結(jié)構(gòu)機(jī)器人操作系統(tǒng)（ROS）10.1.1URDF介紹一個(gè)典型的link標(biāo)簽如下：<linkname="my_link"><inertial><originxyz="000.5"rpy="000"/><massvalue="1"/><inertiaixx="100"ixy="0"xz="0"iyy="100"iyz="0"izz="100"/></inertial><visual><originxyz="000"rpy="000"/><geometry><boxsize="111"/></geometry><materialname="Cyan"><colorrgba="01.01.01.0"/></material></visual><collision><originxyz="000"rpy="000"/><geometry><cylinderradius="1"length="0.5"/></geometry></collision></link>首先，定義該link的名字（linkname），<inertial>標(biāo)簽用于描述link部分的慣性參數(shù)，這個(gè)標(biāo)簽是可選的，如果未指定，則默認(rèn)為零質(zhì)量和零慣性。originxyz和rpy分別為為慣性參考系相對(duì)于link參考系的位置和姿態(tài)，rpy以弧度表示固定軸滾動(dòng)、俯仰和偏航角。此外，還可以設(shè)置質(zhì)量（massvalue）、慣性矩陣（inertia）。<visual>標(biāo)簽用于描述link的外觀參數(shù)，<visual>標(biāo)簽用于描述link的外觀參數(shù)（可選），<collision>標(biāo)簽用于描述link的碰撞屬性（可選），同一個(gè)link可以存在多個(gè)<collision>標(biāo)簽。它們所定義的幾何圖形的并集構(gòu)成了link的碰撞描述。通常，使用更簡(jiǎn)單的碰撞模型來(lái)減少計(jì)算時(shí)間。機(jī)器人操作系統(tǒng)（ROS）10.1.1URDF介紹<joint>標(biāo)簽與人的關(guān)節(jié)相似，<joint>標(biāo)簽用于描述機(jī)器人關(guān)節(jié)，包括關(guān)節(jié)運(yùn)動(dòng)的位置和速度限制。joint結(jié)構(gòu)一個(gè)典型的<joint>標(biāo)簽如下：<jointname="my_joint"type="floating"><originxyz="001"rpy="003.1416"/><parentlink="link1"/><childlink="link2"/><calibrationrising="0.0"/><dynamicsdamping="0.0"friction="0.0"/><limiteffort="30"velocity="1.0"lower="-2.2"upper="0.7"/></joint>每一個(gè)joint連接兩個(gè)link，其中的origin是從parentlink到childlink的轉(zhuǎn)換。接頭位于parentlink的原點(diǎn)，是相對(duì)于上一個(gè)joint的origin描述的，而上面的<link>標(biāo)簽中的origin是相對(duì)于joint坐標(biāo)系表達(dá)的。除了必須指定的兩個(gè)link，關(guān)節(jié)的其他屬性為可選屬性，如<calibration>，作用是設(shè)置joint的參考位置，用于校準(zhǔn)joint的絕對(duì)位置；<dynamics>的作用是描述關(guān)節(jié)的物理屬性，如阻尼值、物理靜摩擦力等。<dynamics>用于描述運(yùn)動(dòng)極限值，僅用于revolute（旋轉(zhuǎn)關(guān)節(jié)）prismatic（滑動(dòng)關(guān)節(jié)）的joint類型。機(jī)器人操作系統(tǒng)（ROS）<robot>標(biāo)簽<robot>是URDF中機(jī)器人描述文件的根標(biāo)簽，所有其他元素必須封裝在其中。一個(gè)完整的機(jī)器人模型由一系列的<link>和<joint>等標(biāo)簽組成，典型的robot結(jié)構(gòu)如圖。在<robot>標(biāo)簽里可以設(shè)置該機(jī)器人的名稱。robot結(jié)構(gòu)10.1.1URDF介紹機(jī)器人操作系統(tǒng)（ROS）10.1.2創(chuàng)建URDF模型并校驗(yàn)創(chuàng)建模型（本書配套的機(jī)器人模型放在myrobot_description功能包下）創(chuàng)建模型文件myrobot_description/urdf/myrobot.urdf：<?xmlversion="1.0"encoding="utf-8"?><robotname="myrobot_description"><linkname="base_link"><inertial><originxyz="000"rpy="000"/><inertiaixx="0.0029446"ixy="-1.0694E-05"ixz="8.6442E-05"iyy="0.0086395"iyz="-7.9718E-07"izz="0.0097467"/></inertial><visual><originxyz="000"rpy="000"/>……該機(jī)器人底盤模型包括6個(gè)link和5個(gè)joint。6個(gè)link包括1個(gè)機(jī)器人底板、一個(gè)與底盤連接的放置激光雷達(dá)的臺(tái)架和4個(gè)驅(qū)動(dòng)輪，4個(gè)joint負(fù)責(zé)將驅(qū)動(dòng)輪安裝到機(jī)器人底板上，并設(shè)計(jì)了fixed和continuous類型的連接方式，fixed是固定關(guān)節(jié)，continuous類型的關(guān)節(jié)圍繞單軸無(wú)限旋轉(zhuǎn)。這樣，一個(gè)比較基本的四輪實(shí)驗(yàn)小車模型就建立好了。機(jī)器人操作系統(tǒng)（ROS）URDF文件校驗(yàn)編程創(chuàng)建URDF文件后，必須進(jìn)行文件校驗(yàn)，查看是否存在語(yǔ)法錯(cuò)誤。對(duì)于我們這次創(chuàng)建的機(jī)器人模型，可以使用簡(jiǎn)單的命令工具來(lái)分析建立的結(jié)構(gòu)是否存在語(yǔ)法錯(cuò)誤，輸入以下命令在終端安裝工具：$sudoapt-getinstallliburdfdom-tools進(jìn)入myrobot_description/urdf目錄下，然后運(yùn)行如下命令對(duì)myrobot.urdf文件進(jìn)行檢查：$check_urdfmyrobot.urdfcheck_urdf命令將解析myrobot.urdf文件，并顯示在解析過(guò)程中檢查出的錯(cuò)誤。如果文件沒(méi)有錯(cuò)誤，終端將打印如圖所示的信息。終端信息10.1.2創(chuàng)建URDF模型并校驗(yàn)機(jī)器人操作系統(tǒng)（ROS）準(zhǔn)備工作在終端運(yùn)行如下命令，顯示所構(gòu)建的URDF結(jié)構(gòu)關(guān)系：$urdf_to_graphizmy_robot.Urdf終端執(zhí)行完畢后會(huì)得到一個(gè)PDF文件，展示了編程構(gòu)建的機(jī)器人模型的URDF關(guān)系文件：

機(jī)器人模型的URDF關(guān)系10.1.2創(chuàng)建URDF模型并校驗(yàn)機(jī)器人操作系統(tǒng)（ROS）10.1.3在rviz中顯示模型可以使用rviz將已創(chuàng)建的機(jī)器人模型可視化顯示出來(lái)，用來(lái)檢查構(gòu)建你的模型是否符合設(shè)計(jì)預(yù)期。啟動(dòng)文件目錄為myrobot_description/launch/display_urdf.launch，launch文件詳細(xì)內(nèi)容如下：<launch><paramname="robot_description"textfile="$(findlingao_description)/urdf/myrobot.urdf"/><!—設(shè)置GUI參數(shù)，顯示關(guān)節(jié)控制插件--><paramname="use_gui"value="true"/><!—設(shè)置joint_state_publisher節(jié)點(diǎn)，發(fā)布機(jī)器人的關(guān)節(jié)狀態(tài)--><nodename="joint_state_publisher"pkg="joint_state_publisher"type="joint_state_publisher"><paramname="rate"value="20.0"/></node><!—設(shè)置robot_state_publisher節(jié)點(diǎn)，發(fā)布TF轉(zhuǎn)換--><nodename="robot_state_publisher"pkg="robot_state_publisher"type="robot_state_publisher"> <paramname="rate"value="20.0"/></node><!--運(yùn)行rviz可視化界面--><nodename="rviz"pkg="rviz"type="rviz"args="/></launch>創(chuàng)建啟動(dòng)文件機(jī)器人操作系統(tǒng)（ROS）運(yùn)行啟動(dòng)文件在終端輸入以下命令運(yùn)行該啟動(dòng)文件：$roslaunchmyrobot_descriptiondisplay_urdf.launch若出現(xiàn)如下報(bào)錯(cuò)：CouldnotfindtheGUI,installthe‘joint_state_publisher_gui’package則使用下面命令進(jìn)行安裝：$sudoapt-getinstallros-melodic-joint-state-publisher-gui若運(yùn)行成功，則會(huì)出現(xiàn)如下界面：10.1.3在rviz中顯示模型rviz界面機(jī)器人操作系統(tǒng)（ROS）10.1.3在rviz中顯示模型在rviz界面中點(diǎn)擊Add，添加RobotModel和TF，如圖所示，并將FixedFrame修改為base_link，此時(shí)將會(huì)在rviz中顯示之前構(gòu)建好的機(jī)器人模型。添加機(jī)器人模型添加TF機(jī)器人操作系統(tǒng)（ROS）10.1.3在rviz中顯示模型rviz界面中的機(jī)器人模型機(jī)器人操作系統(tǒng)（ROS）10.1.4在仿真環(huán)境中控制小車運(yùn)動(dòng)在本節(jié)中，我們使用rviz界面來(lái)實(shí)現(xiàn)機(jī)器人運(yùn)動(dòng)的仿真控制。需要安裝軟件包rbx1和ArbotiX功能包。rbx1軟件包（來(lái)源于《ROSbyExample》），包括路徑規(guī)劃，視覺(jué)，語(yǔ)音識(shí)別和其他功能的一些程序包。ArbotiX是一款控制電機(jī)、舵機(jī)的控制板，提供相應(yīng)的ROS功能包arbotix_ros，該功能包不僅可以驅(qū)動(dòng)真實(shí)的驅(qū)動(dòng)板，還提供了一個(gè)控制器，通過(guò)接收速度控制指令更新機(jī)器人的joint狀態(tài)，從而幫助我們實(shí)現(xiàn)仿真機(jī)器人的運(yùn)動(dòng)控制。機(jī)器人操作系統(tǒng)（ROS）首先安裝rbx1的軟件包，通過(guò)執(zhí)行如下命令將rbx1克隆到本地或拷貝本書配套的源碼到工作空間：$gitclone/pirobot/rbx1.git$cdrbx1$gitcheckoutmelodic-devel通過(guò)以下命令下載arbotix_ros的源碼或復(fù)制本書源碼至工作空間中：$gitclone/vanadiumlabs/arbotix_ros.git下載完成后編譯功能包。軟件基礎(chǔ)10.1.4在仿真環(huán)境中控制小車運(yùn)動(dòng)機(jī)器人操作系統(tǒng)（ROS）接下來(lái)，需要將虛擬機(jī)器人啟動(dòng)文件（fake_turtlebot.launch）中修改成自己的urdf文件，修改后的文件如下所示：<launch><paramname="/use_sim_time"value="false"/><!--LoadtheURDF/Xacromodelofourrobot--><!--argname="urdf_file"default="$(findxacro)/xacro.py'$(findrbx1_description)/urdf/turtlebot.urdf.xacro'"/--><argname="urdf_file"default="$(findxacro)/xacro.py'$(findmyrobot_description)/urdf/myrobot.urdf'"/><paramname="robot_description"command="$(argurdf_file)"/><nodename="arbotix"pkg="arbotix_python"type="arbotix_driver"output="screen"clear_params="true"><rosparamfile="$(findrbx1_bringup)/config/fake_turtlebot_arbotix.yaml"command="load"/><paramname="sim"value="true"/></node><nodename="robot_state_publisher"pkg="robot_state_publisher"type="state_publisher"><paramname="publish_frequency"type="double"value="20.0"/></node><!--Weneedastatictransformsforthewheels--><nodepkg="tf"type="static_transform_publisher"name="base_link_to_footprint"args="000000/base_footprint/base_link100"/></launch>軟件基礎(chǔ)10.1.4在仿真環(huán)境中控制小車運(yùn)動(dòng)機(jī)器人操作系統(tǒng)（ROS）運(yùn)行如下指令啟動(dòng)仿真機(jī)器人和rviz：$roslaunchrbx1_bringupfake_turtlebot.launch$rosrunrvizrviz-d`rospackfindrbx1_nav`/sim.rviz在rviz界面中點(diǎn)擊Add，添加RobotModel，將FixedFrame修改為odom，會(huì)出現(xiàn)如圖界面。rviz界面中的機(jī)器人模型10.1.4在仿真環(huán)境中控制小車運(yùn)動(dòng)機(jī)器人操作系統(tǒng)（ROS）使用以下指令通過(guò)命令行發(fā)送機(jī)器人控制命令：$rostopicpub-r10/cmd_velgeometry_msgs/Twist{linear:{x:0.1,y:0,z:0},angular:{x:0,y:0,z:-0.5}}”若想可視化機(jī)器人的運(yùn)動(dòng)，點(diǎn)擊Add，添加Odometry，將Topic修改為/odom。這時(shí)，可以看到機(jī)器人在做圓周運(yùn)動(dòng)。發(fā)送機(jī)器人控制命令可視化機(jī)器人圓周運(yùn)動(dòng)10.1.4在仿真環(huán)境中控制小車運(yùn)動(dòng)機(jī)器人操作系統(tǒng)（ROS）除了命令行的方式外，我們還可以通過(guò)編寫腳本的方式控制機(jī)器人移動(dòng)。創(chuàng)建Python腳本文件timed_out_and_back.py，該節(jié)點(diǎn)實(shí)現(xiàn)的功能是讓小車模型先前進(jìn)1米，再旋轉(zhuǎn)180度，重復(fù)兩次。部分內(nèi)容如下：#-*-coding:utf-8-*-#!/usr/bin/envpythonimportrospyfromgeometry_msgs.msgimportTwistfrommathimportpiclassOutAndBack():def__init__(self):rospy.init_node('out_and_back',anonymous=False)#節(jié)點(diǎn)的名字為out_and_backrospy.on_shutdown(self.shutdown)#設(shè)置rospy在程序退出時(shí)執(zhí)行的關(guān)機(jī)函數(shù)

self.cmd_vel=rospy.Publisher('/cmd_vel',Twist,queue_size=5)#發(fā)布機(jī)器人運(yùn)動(dòng)的速度

rate=50#以50hz的頻率更新控制機(jī)器人運(yùn)動(dòng)的命令

r=rospy.Rate(rate)#設(shè)定相同的值給rospy.Rate()……10.1.4在仿真環(huán)境中控制小車運(yùn)動(dòng)機(jī)器人操作系統(tǒng)（ROS）輸入以下命令啟動(dòng)虛擬機(jī)器人和仿真環(huán)境：$roslaunchrbx1_bringupfake_turtlebot.launch$rosrunrvizrviz-d`rospackfindrbx1_nav`/sim.rviz在rviz界面中點(diǎn)擊Add，添加RobotModel，將FixedFrame修改為odom，點(diǎn)擊Add，添加Odometry，將Topic修改為/odom。運(yùn)行如下命令啟動(dòng)控制節(jié)點(diǎn)：$rosrunrbx1_navtimed_out_and_back.py機(jī)器人仿真運(yùn)行10.1.4在仿真環(huán)境中控制小車運(yùn)動(dòng)機(jī)器人操作系統(tǒng)（ROS）10.1.5本節(jié)小結(jié)本節(jié)學(xué)習(xí)了如何使用URDF創(chuàng)建一個(gè)真實(shí)的機(jī)器人模型，并通過(guò)rviz可視化工具和Arbotix仿真平臺(tái)對(duì)構(gòu)建好的機(jī)器人模型進(jìn)行顯示，最后建立機(jī)器人控制節(jié)點(diǎn)控制機(jī)器人移動(dòng)。通過(guò)本章的學(xué)習(xí)，讀者可以動(dòng)手搭建自己的機(jī)器人平臺(tái)，為后續(xù)機(jī)器人的開(kāi)發(fā)提供更好的基礎(chǔ)。機(jī)器人操作系統(tǒng)（ROS）機(jī)器人操作系統(tǒng)（ROS）MoveIt!的使用10.2機(jī)器人操作系統(tǒng)（ROS）10.2.1MoveIt!系統(tǒng)架構(gòu)MoveIt!系統(tǒng)架構(gòu)如圖所示，內(nèi)容參考官方文檔概念說(shuō)明。move_group是MoveIt!的核心節(jié)點(diǎn)，能夠?qū)⑵渌墓δ芙M件綜合在一起為用戶提供ROS中的動(dòng)作指令和服務(wù)，其本身并不具備強(qiáng)大豐富的功能，依靠各種功能包和插件的集成，通過(guò)服務(wù)或消息的方式接收機(jī)器人發(fā)出的消息和機(jī)器人的TF坐標(biāo)變換。MoveIt!系統(tǒng)架構(gòu)機(jī)器人操作系統(tǒng)（ROS）10.2.1MoveIt!系統(tǒng)架構(gòu)

用戶可以通過(guò)如下三種方式訪問(wèn)move_group提供的操作和服務(wù)：基于move_group_interface的C++接口，基于moveit_commander的Python接口和使用rviz插件的GUI接口。move_group需要使用ROS參數(shù)服務(wù)器來(lái)獲取以下三種信息：URDF：在ROS參數(shù)服務(wù)器上查找robot_description參數(shù)，以獲取機(jī)器人模型的描述信息。SRDF：在ROS參數(shù)服務(wù)器上查找robot_description_semantic參數(shù)，以獲取機(jī)器人模型的配置信息，配置信息通常由用戶使用MoveIt!SetupAssistant創(chuàng)建。Config：機(jī)器人的其他配置信息，包括關(guān)節(jié)限制、運(yùn)動(dòng)學(xué)、運(yùn)動(dòng)規(guī)劃、感知和其他信息。機(jī)器人操作系統(tǒng)（ROS）10.2.1MoveIt!系統(tǒng)架構(gòu)運(yùn)動(dòng)規(guī)劃（Motionplanning）就是在將機(jī)器人從初始姿態(tài)和位置移動(dòng)到目標(biāo)姿態(tài)和位置的過(guò)程中，避開(kāi)環(huán)境中的障礙物并防止自身碰撞的一種算法MoveIt!通過(guò)插件機(jī)制（plugininterface）與運(yùn)動(dòng)規(guī)劃器（motionplanner）進(jìn)行交互，可以使用多個(gè)庫(kù)的不同運(yùn)動(dòng)規(guī)劃器，使得MoveIt!擴(kuò)展性更強(qiáng)。MoveIt!中包含有多種運(yùn)動(dòng)規(guī)劃器，包括基于采樣的運(yùn)動(dòng)規(guī)劃器OMPL（move_group默認(rèn)使用），基于搜索的運(yùn)動(dòng)規(guī)劃器SBPL和基于最優(yōu)化的運(yùn)動(dòng)規(guī)劃器CHOMP等。運(yùn)動(dòng)規(guī)劃器結(jié)構(gòu)如圖所示，運(yùn)動(dòng)規(guī)劃請(qǐng)求需要根據(jù)實(shí)際情況設(shè)置一些約束條件：①位置約束：限制link的空間區(qū)域。②方向約束：限制link的運(yùn)動(dòng)方向（滾轉(zhuǎn)、俯仰和偏航）。③可見(jiàn)性約束：限制link上的某個(gè)點(diǎn)在某個(gè)區(qū)域的可見(jiàn)性。④關(guān)節(jié)約束：限制關(guān)節(jié)的運(yùn)動(dòng)范圍。⑤用戶指定約束：用戶通過(guò)回調(diào)函數(shù)定義約束條件。運(yùn)動(dòng)規(guī)劃器結(jié)構(gòu)機(jī)器人操作系統(tǒng)（ROS）10.2.1MoveIt!系統(tǒng)架構(gòu)規(guī)劃場(chǎng)景用于重現(xiàn)機(jī)器人的周圍狀態(tài)以及機(jī)器人的自身姿態(tài)。這一功能主要由規(guī)劃場(chǎng)景監(jiān)聽(tīng)器（PlanningSceneMonitor）實(shí)現(xiàn)，如圖所示：規(guī)劃場(chǎng)景框圖機(jī)器人操作系統(tǒng)（ROS）10.2.2如何使用MoveIt!配置助手配置機(jī)械臂運(yùn)行如下命令安裝MoveIt!，如圖所示：$sudoapt-getinstallros-melodic-moveit*安裝MoveIt!機(jī)器人操作系統(tǒng)（ROS）10.2.2如何使用MoveIt!配置助手配置機(jī)械臂

MoveIt!配置助手（SetupAssistant）是一個(gè)使用MoveIt!配置機(jī)器人的圖形界面，主要功能是產(chǎn)生機(jī)器人的SRDF文件，另外還產(chǎn)生其他配置文件，從而創(chuàng)建一個(gè)MoveIt!配置的功能包，完成機(jī)器人的配置、可視化和仿真等工作。運(yùn)行如下命令，啟動(dòng)設(shè)置助手，如圖所示：$roslaunchmoveit_setup_assistantsetup_assistant.launchMoveIt!配置助手其中，按鍵“CreateNewMoveItConfigurationPackage”功能為新建配置功能包，按鍵“EditExistingMoveItConfigurationPackage”功能為使用已有的配置功能包。機(jī)器人操作系統(tǒng)（ROS）10.2.2如何使用MoveIt!配置助手配置機(jī)械臂點(diǎn)擊按鍵“CreateNewMoveItConfigurationPackage”后，點(diǎn)擊“Browse”按鈕，加入相關(guān)模型，模型位置：robot_arm/robot_arm_description/urdf（需將本書對(duì)應(yīng)代碼拷貝至工作空間并進(jìn)行編譯），點(diǎn)擊“LoadFiles”完成模型加載，如圖所示。加載機(jī)械臂模型機(jī)器人操作系統(tǒng)（ROS）10.2.2如何使用MoveIt!配置助手配置機(jī)械臂點(diǎn)擊“Self-Collisions”，配置自碰撞矩陣，如圖所示。默認(rèn)的自碰撞矩陣生成器搜索機(jī)械臂所有關(guān)節(jié)，這個(gè)碰撞免檢矩陣是可以安全地關(guān)閉檢查，從而減少行動(dòng)規(guī)劃的處理時(shí)間。采樣密度指定了多少個(gè)隨機(jī)機(jī)械臂位置來(lái)檢查碰撞，默認(rèn)10000個(gè)碰撞檢查?？牲c(diǎn)擊“GenerateCollisionMatrix”自動(dòng)完成設(shè)置。配置自碰撞矩陣機(jī)器人操作系統(tǒng)（ROS）10.2.2如何使用MoveIt!配置助手配置機(jī)械臂點(diǎn)擊“VirtualJoints”，分配機(jī)械臂的虛擬關(guān)節(jié)，如圖所示。這里定義機(jī)械臂與世界坐標(biāo)系的關(guān)系，把機(jī)械臂關(guān)節(jié)固定到某個(gè)物體上，例如機(jī)械臂有一個(gè)滑動(dòng)底座，可以將機(jī)械臂的底座與里程計(jì)（odom）通過(guò)一個(gè)關(guān)節(jié)連接，此時(shí)機(jī)械臂就可以在二維平面滑動(dòng)。這里用不上，直接跳過(guò)。分配機(jī)械臂虛擬關(guān)節(jié)機(jī)器人操作系統(tǒng)（ROS）10.2.2如何使用MoveIt!配置助手配置機(jī)械臂點(diǎn)擊“PlanningGroups”，配置機(jī)械臂的規(guī)劃群組，如圖所示。規(guī)劃組可以將機(jī)械臂劃分為不同的組，例如機(jī)械臂本身和夾爪部分。運(yùn)動(dòng)規(guī)劃會(huì)針對(duì)這一個(gè)組完成運(yùn)動(dòng)規(guī)劃，在配置過(guò)程中還可以選擇運(yùn)動(dòng)學(xué)解析器。配置機(jī)械臂的規(guī)劃群組機(jī)器人操作系統(tǒng)（ROS）10.2.2如何使用MoveIt!配置助手配置機(jī)械臂點(diǎn)擊“AddGroup”，按照?qǐng)D所示內(nèi)容進(jìn)行配置，其中：GroupName：規(guī)劃組的名稱。KinematicSolver：配置針對(duì)運(yùn)動(dòng)規(guī)劃的運(yùn)動(dòng)學(xué)求解器，選擇的是kdl的運(yùn)動(dòng)規(guī)劃器插件。Kin.SearchResolution：關(guān)節(jié)空間的采樣密度。Kin.SearchTimeout（sec）：超時(shí)時(shí)間。創(chuàng)建機(jī)械臂arm組機(jī)器人操作系統(tǒng)（ROS）10.2.2如何使用MoveIt!配置助手配置機(jī)械臂點(diǎn)擊“AddKin.Chain”，設(shè)置運(yùn)動(dòng)學(xué)計(jì)算中需包含的link，如圖所示。具體設(shè)置如下：BaseLink：base_linkTipLink：grasping_frame添加運(yùn)動(dòng)學(xué)工具鏈機(jī)器人操作系統(tǒng)（ROS）10.2.2如何使用MoveIt!配置助手配置機(jī)械臂點(diǎn)擊“AddGroup”，為機(jī)械臂夾爪創(chuàng)建如圖所示的的gripper組。

創(chuàng)建機(jī)械臂夾爪的gripper組機(jī)器人操作系統(tǒng)（ROS）10.2.2如何使用MoveIt!配置助手配置機(jī)械臂將“finger_joint1”和“finger_joint2”加入到右側(cè)的列表中，如圖所示。選中“gripper”目錄下的“Joints”，點(diǎn)擊“EditSelected”，如圖所示。進(jìn)入gripper組中的joint進(jìn)行配置設(shè)置gripper組中的joint機(jī)器人操作系統(tǒng)（ROS）10.2.2如何使用MoveIt!配置助手配置機(jī)械臂配置完成后，主配置界面如圖所示。點(diǎn)擊“RobotPoses”，定義機(jī)械臂的位姿。點(diǎn)擊“AddPose”，首先設(shè)置機(jī)械臂的初始位姿，如圖所示。主配置界面設(shè)置機(jī)械臂位姿機(jī)器人操作系統(tǒng)（ROS）10.2.2如何使用MoveIt!配置助手配置機(jī)械臂位姿設(shè)置完成后會(huì)出現(xiàn)如圖所示的界面。通過(guò)更改相關(guān)的joint參數(shù)，設(shè)置機(jī)械臂的第二個(gè)位姿，如圖所示。設(shè)置機(jī)械臂第二個(gè)位姿機(jī)械臂位姿配置完成機(jī)器人操作系統(tǒng)（ROS）10.2.2如何使用MoveIt!配置助手配置機(jī)械臂點(diǎn)擊“EndEffectors”，配置機(jī)械臂的夾爪，如圖所示。機(jī)械臂夾爪配置

點(diǎn)擊“PassiveJoints”，如圖所示，這里是配置不能驅(qū)動(dòng)的關(guān)節(jié)，這些關(guān)節(jié)不需要MoveIt！對(duì)其進(jìn)行規(guī)劃和控制。由于本實(shí)驗(yàn)沒(méi)有類似關(guān)節(jié)，所以無(wú)需配置。PassiveJoints配置機(jī)器人操作系統(tǒng)（ROS）10.2.2如何使用MoveIt!配置助手配置機(jī)械臂點(diǎn)擊“AuthorInformation”，設(shè)置作者信息。點(diǎn)擊“ConfigurationFiles”，生成配置文件。選擇一個(gè)保存路徑，配置助手將所有配置的文件打包成一個(gè)功能包（自行命名）進(jìn)行保存，保存成功后點(diǎn)擊“ExitSetupAssistant”即可退出配置助手，如圖10-36所示。生成配置文件機(jī)器人操作系統(tǒng)（ROS）10.2.3啟動(dòng)MoveIt!

使用MoveIt!配置助手配置完成后，會(huì)在相應(yīng)的路徑下生成名為“robot_arm_config”的功能包，包含了大部分控制機(jī)械臂所需的啟動(dòng)和配置文件。運(yùn)行如下命令，測(cè)試配置是否成功，如圖所示：

$roslaunchrobot_arm_configdemo.launch運(yùn)行demo.launch通過(guò)運(yùn)動(dòng)規(guī)劃面板可以控制機(jī)械臂完成拖動(dòng)規(guī)劃、隨機(jī)目標(biāo)規(guī)劃等功能。機(jī)器人操作系統(tǒng)（ROS）10.2.3啟動(dòng)MoveIt!

拖動(dòng)機(jī)械臂的前端改變機(jī)械臂的姿態(tài)，點(diǎn)擊“Planning”下的“Plan&Execute”，Moveit!開(kāi)始規(guī)劃路徑，控制機(jī)械臂向目標(biāo)位置移動(dòng)，如圖所示。拖動(dòng)規(guī)劃?rùn)C(jī)器人操作系統(tǒng)（ROS）10.2.3啟動(dòng)MoveIt!

點(diǎn)擊“Query”下的“GoalState”的下拉選項(xiàng)，選擇“randomvalid”，在機(jī)械臂的工作范圍內(nèi)隨機(jī)出現(xiàn)一個(gè)目標(biāo)位姿，點(diǎn)擊“Plan&Execute”，機(jī)械臂將從當(dāng)前位姿開(kāi)始運(yùn)動(dòng)，直至達(dá)到目標(biāo)位姿，如圖所示。

隨機(jī)目標(biāo)規(guī)劃?rùn)C(jī)器人操作系統(tǒng)（ROS）10.2.4本節(jié)小結(jié)

本節(jié)介紹了MoveIt!的系統(tǒng)架構(gòu)，并學(xué)習(xí)了如何使用MoveIt!配置助手生成配置文件，最終配置了一個(gè)機(jī)械臂，并能通過(guò)MoveIt!實(shí)現(xiàn)了機(jī)械臂的運(yùn)動(dòng)規(guī)劃。機(jī)械臂還可實(shí)現(xiàn)自主避障、抓取放置等功能，讀者可自行學(xué)習(xí)感興趣的部分。機(jī)器人操作系統(tǒng)（ROS）機(jī)器人操作系統(tǒng)（ROS）Hector四旋翼無(wú)人機(jī)仿真10.3機(jī)器人操作系統(tǒng)（ROS）10.3Hector四旋翼無(wú)人機(jī)仿真四旋翼無(wú)人機(jī)是一種非共軸式碟形飛行器，在平面上呈十字對(duì)稱結(jié)構(gòu)并均勻分布有4個(gè)旋翼。通過(guò)調(diào)節(jié)四個(gè)旋翼旋轉(zhuǎn)產(chǎn)生的升力來(lái)控制6個(gè)自由度方向的動(dòng)作。它除了具有一般無(wú)人機(jī)的優(yōu)點(diǎn)，如不懼傷亡、制造成本低廉、隱蔽性好、操作靈活等，還具有多旋翼無(wú)人機(jī)的獨(dú)特優(yōu)勢(shì)：（1）能夠在狹小的空間內(nèi)實(shí)現(xiàn)垂直升降、定點(diǎn)懸停、低速飛行、旋轉(zhuǎn)、側(cè)飛及倒飛等，機(jī)動(dòng)靈活，可控性較強(qiáng)；（2）結(jié)構(gòu)簡(jiǎn)單，拆卸方便，且易于維護(hù)；（3）四個(gè)旋翼同時(shí)旋轉(zhuǎn)提供升力，相對(duì)一般無(wú)人機(jī)，可以使用較小的旋翼和較低的轉(zhuǎn)速，安全性提高。機(jī)器人操作系統(tǒng)（ROS）10.3.1四旋翼無(wú)人機(jī)簡(jiǎn)介四旋翼無(wú)人機(jī)是無(wú)人飛行器（UAV）的一種，其旋翼對(duì)稱分布在機(jī)體的前后、左右四個(gè)方向，四個(gè)旋翼處于同一高度平面，且四個(gè)旋翼的結(jié)構(gòu)和半徑都相同，四個(gè)電機(jī)對(duì)稱的安裝在飛行器的支架端，支架中間部分安放飛行控制計(jì)算機(jī)和外部設(shè)備。結(jié)構(gòu)形式如圖所示。四旋翼無(wú)人機(jī)的結(jié)構(gòu)形式四旋翼飛行器通過(guò)調(diào)節(jié)四個(gè)電機(jī)轉(zhuǎn)速來(lái)改變旋翼轉(zhuǎn)速，實(shí)現(xiàn)升力的變化，從而控制飛行器的姿態(tài)和位置。四旋翼飛行器是一種六自由度的垂直升降機(jī)，有四個(gè)輸入力，同時(shí)有六個(gè)狀態(tài)輸出。機(jī)器人操作系統(tǒng)（ROS）10.3.1四旋翼無(wú)人機(jī)簡(jiǎn)介如圖所示，無(wú)人機(jī)繞三個(gè)坐標(biāo)軸旋轉(zhuǎn)的角度分別由滾轉(zhuǎn)角（roll）、俯仰角（pitch）和偏航角（yaw）表示。滾轉(zhuǎn)角是飛機(jī)對(duì)稱平面與通過(guò)飛機(jī)機(jī)體縱軸的鉛垂平面間的夾角，右滾為正。俯仰角是機(jī)體軸與地平面（水平面）之間的夾角，飛機(jī)抬頭為正。偏航角是機(jī)體軸在水平面上的投影與地軸之間的夾角，以機(jī)頭右偏為正。飛機(jī)滾轉(zhuǎn)角、俯仰角和偏航角機(jī)器人操作系統(tǒng)（ROS）10.3.2Hector四旋翼無(wú)人機(jī)仿真本實(shí)驗(yàn)采用的是塔姆斯塔特工業(yè)大學(xué)的HectorDarmstadt團(tuán)隊(duì)開(kāi)源的hector仿真旋翼UAV項(xiàng)目，其開(kāi)源代碼網(wǎng)站為：/tu-darmstadt-ros-pkg/hector_quadrotor。該功能包包含了UAV的URDF描述建模、飛行控制及在Gazebo中運(yùn)行四旋翼無(wú)人機(jī)的啟動(dòng)文件等。https://www.teamhector.de/open-source機(jī)器人操作系統(tǒng)（ROS）10.3.2Hector四旋翼無(wú)人機(jī)仿真hector_quadrotorhector_quadrotor_description包含了基礎(chǔ)四旋翼及搭載各種傳感器的urdf文件；hector_quadrotor_gazebo包含了四旋翼在gazebo仿真中的驅(qū)動(dòng)、插件；hector_quadrotor_teleop包含了四旋翼遙控器接口文件；hector_quadrotor_gazebo_plugins包含了gazebo仿真中氣動(dòng)、推力、控制等模擬文件；hector_quadrotor_demo包含了官網(wǎng)例程的啟動(dòng)文件；hector_quadrotor_actions包含了支持的action(起飛、降落、航點(diǎn))文件；hector_quadrotor_controller_gazebo包含了gazebo仿真中的控制器接口插件；hector_quadrotor_controllers包含了姿態(tài)、位置、速度控制器文件；hector_quadrotor_interface包含了四旋翼的接口文件；hector_quadrotor_model包含了四旋翼的動(dòng)力學(xué)模型文件；hector_quadrotor_pose_estimation包含了四旋翼的位姿解算文件；hector_uav_msgs包含了四旋翼的消息、動(dòng)作、服務(wù)匯總；hector_quadrotor包含了hector_quadrotor功能包的管理文件；整個(gè)tu-darmstadt-ros-pkg功能包的各個(gè)子功能包的結(jié)構(gòu)如下所示機(jī)器人操作系統(tǒng)（ROS）10.3.2Hector四旋翼無(wú)人機(jī)仿真hector_modelshector_sensors_description包含了sonar/laser/camera/rgb-camera等xacro宏文件及gazeboplugin；hector_xacro_tools包含了轉(zhuǎn)動(dòng)慣量計(jì)算、關(guān)節(jié)驅(qū)動(dòng)、傳感器安裝的xacro宏文件；hector_components_description包含了一些例程中采用的傳感器搭配xacro文件；hector_model包含了hector_model功能包的管理文件；hector_gazebohector_gazebo_plugins包含了GPS/IMU/MAG/SONAR的gazebo插件(噪聲、飄移、頻率)；hector_gazebo_thermal_camera包含了熱成像相機(jī)的gazebo插件；hector_gazebo_worlds包含了例程中應(yīng)用的gazebo場(chǎng)景和啟動(dòng)文件；hector_sensors_gazebo關(guān)聯(lián)hector_sensors_description包；hector_gazebo包含了hector_gazebo功能包的管理文件；hector_localizationhector_pose_estimation包含了傳感器數(shù)據(jù)匯總、位姿估計(jì)發(fā)布文件；hector_pose_estimation_core包含了EKF核心算法文件；message_to_tf包含了傳感器信息的坐標(biāo)轉(zhuǎn)換文件；hector_localization包含了hector_localization功能包的管理文件；機(jī)器人操作系統(tǒng)（ROS）10.3.2Hector四旋翼無(wú)人機(jī)仿真hector_slamhector_mapping包含了建圖、定位節(jié)點(diǎn)文件；hector_geotiff包含了存儲(chǔ)二位柵格地圖及機(jī)器人軌跡的節(jié)點(diǎn)文件；hector_trajectory_server包含了存儲(chǔ)tf軌跡的節(jié)點(diǎn)文件；hector_compressed_map_transport包含了地圖轉(zhuǎn)換成圖片的節(jié)點(diǎn)文件；hector_geotiff_plugins包含了擴(kuò)展柵格地圖的插件；hector_imu_attitude_to_tf包含了將姿態(tài)角發(fā)布到tf的節(jié)點(diǎn)文件；hector_imu_tools包含了IMU的位姿方向角解算；hector_map_server包含了地圖檢索及障礙物檢測(cè)文件；hector_map_tools包含了一個(gè)地圖構(gòu)建的頭文件；hector_marker_drawing包含了可視化標(biāo)記的函數(shù)文件;hector_nav_msgs包含了hector_slam包用到的消息、服務(wù)文件；hector_slam_launch包含了例程中關(guān)于不同配置hector_slam的啟動(dòng)文件；hector_slam包含了hector_slam功能包的管理文件；機(jī)器人操作系統(tǒng)（ROS）10.3.3Hector仿真環(huán)境搭建為了方便讀者調(diào)試，我們提供了HectorDarmstadt的源碼。本實(shí)驗(yàn)使用到的功能包有：hector_quadrotor、hector_models、hector_gazebo、hector_localization、hector_slam、gazebo_ros_pkgs、geographic_info，其中前五個(gè)是源碼，其余為依賴安裝包。首先我們需要對(duì)功能包進(jìn)行編譯：$cd~/catkin_ws$catkin_make可能會(huì)出現(xiàn)“Couldnotfindxxxpackage”的報(bào)錯(cuò)，這是因?yàn)槿鄙傧嚓P(guān)的依賴，運(yùn)行以下命令安裝相應(yīng)的依賴包，其中xxx代表所缺少依賴的名稱，如圖所示：$sudoapt-getinstallros-melodic-xxx此外還可以通過(guò)rosdep指令安裝所需依賴：$rosdepinstall--from-pathssrc--ignore-src-r-y依賴安裝完成后，重新編譯功能包。編譯運(yùn)行截圖機(jī)器人操作系統(tǒng)（ROS）10.3.4啟動(dòng)Hector仿真實(shí)驗(yàn)（2）室內(nèi)飛行同樣在hector_quradrotor功能包包含四旋翼飛行器的室內(nèi)模擬飛行例程。運(yùn)行以下命令啟動(dòng)節(jié)點(diǎn)：$roslaunchhector_quadrotor_demoindoor_slam_gazebo.launch節(jié)點(diǎn)啟動(dòng)后，Gazebo中將顯示W(wǎng)illowGarage室內(nèi)辦公室的模擬環(huán)境，若沒(méi)有加載出環(huán)境，需要將Gazebo的官方模型庫(kù)（/osrf/gazebo_models）拷貝到路徑為~/.gazebo（如若不行，放置在/share/gazebo-9/models）的文件夾里，可以通過(guò)以下命令查看是否裝好Gazebo的庫(kù)結(jié)果如圖所示：$roslaunchgazebo_roswillowgarage_world.launch運(yùn)行指令可以調(diào)出Gazebo窗口并加載模擬辦公室的環(huán)境。成功配置Gazebo庫(kù)后，運(yùn)行如下命令啟動(dòng)室內(nèi)時(shí)SLAM節(jié)點(diǎn)，運(yùn)行結(jié)果如圖所示：$roslaunchgazebo_rosindoor_slam_gazebo.launch機(jī)器人操作系統(tǒng)（ROS）10.3.4啟動(dòng)Hector仿真實(shí)驗(yàn)Hector無(wú)人機(jī)室內(nèi)Gazebo圖像Hector無(wú)人機(jī)室內(nèi)Rviz圖像機(jī)器人操作系統(tǒng)（ROS）10.3.4啟動(dòng)Hector仿真實(shí)驗(yàn)調(diào)用使能電機(jī)的服務(wù)，控制無(wú)人機(jī)起飛，命令如下：$rosservicecall/enable_motors"enable:true"運(yùn)行如下命令啟動(dòng)Xbox手柄控制節(jié)點(diǎn)：$roslaunchhector_quadrotor_teleopxbox_controller.launch通過(guò)手柄控制無(wú)人機(jī)移動(dòng)，并實(shí)時(shí)建立環(huán)境地圖，如圖所示。使用rqt_graph工具查看節(jié)點(diǎn)通信關(guān)系圖，其結(jié)果如圖所示。Hector在室內(nèi)環(huán)境進(jìn)行地圖構(gòu)建的Rviz圖Hectorquadrotor室內(nèi)SLAM的節(jié)點(diǎn)通信關(guān)系圖機(jī)器人操作系統(tǒng)（ROS）10.3.5本節(jié)小結(jié)本節(jié)介紹了無(wú)人機(jī)飛行原理及相關(guān)概念，并通過(guò)搭建Hector無(wú)人機(jī)仿真環(huán)境進(jìn)行室內(nèi)外SLAM仿真，向讀者展示了無(wú)人機(jī)的控制過(guò)程。同時(shí)Hector無(wú)人機(jī)仿真項(xiàng)目提供了較為完善的功能，方便讀者進(jìn)行導(dǎo)航和路徑規(guī)劃的進(jìn)一步研究。機(jī)器人操作系統(tǒng)（ROS）機(jī)器人操作系統(tǒng)（ROS）機(jī)器人SLAM及自主導(dǎo)航10.4機(jī)器人操作系統(tǒng)（ROS）10.4機(jī)器人SLAM及自主導(dǎo)航

根據(jù)機(jī)器人所使用的傳感器不同，可以將SLAM（SimultaneousLocalizationandMapping，同步定位與建圖）分為激光SLAM和視覺(jué)SLAM。激光SLAM是使用激光雷達(dá)進(jìn)行環(huán)境感知進(jìn)而定位建圖；視覺(jué)SLAM通過(guò)視覺(jué)傳感器進(jìn)行定位建圖。本章我們將使用單線激光雷達(dá)實(shí)現(xiàn)環(huán)境地圖的構(gòu)建，并在已構(gòu)建完成的地圖中實(shí)現(xiàn)自主導(dǎo)航。自主導(dǎo)航機(jī)器人操作系統(tǒng)（ROS）10.4.1使用gmapping構(gòu)建地圖gmapping原理gmapping算法是基于RBPF(Rao-BlackwellisedParticleFilter)的激光2DSLAM算法。該算法將機(jī)器人的里程計(jì)位姿信息和激光雷達(dá)的點(diǎn)云數(shù)據(jù)進(jìn)行結(jié)合。RBPFSLAM的核心是通過(guò)激光雷達(dá)感知環(huán)境信息，并在RBPF的基礎(chǔ)上引入了改進(jìn)的建議分布（Proposaldistribution）和自適應(yīng)重采樣技術(shù)，在估計(jì)粒子分布時(shí)，同時(shí)考慮里程計(jì)位姿信息和最新的激光雷達(dá)觀測(cè)值，從而一定程度上減少了粒子數(shù)目和計(jì)算量，保證了建圖的準(zhǔn)確性，有效改善了RBPF的粒子耗散和計(jì)算量大的劣勢(shì)。gmapping算法流程圖機(jī)器人操作系統(tǒng)（ROS）10.4.1使用gmapping構(gòu)建地圖gmapping原理當(dāng)節(jié)點(diǎn)獲取到激光雷達(dá)和里程計(jì)的數(shù)據(jù)時(shí)，將最新時(shí)刻獲取的激光雷達(dá)觀測(cè)數(shù)據(jù)與之前構(gòu)建的點(diǎn)云地圖掃描匹配，確定當(dāng)前機(jī)器人的位置。同時(shí)，算法會(huì)根據(jù)匹配的程度來(lái)計(jì)算得分，若得分在設(shè)定值的范圍內(nèi)，則匹配成功，采用改進(jìn)的建議分布（即觀測(cè)模型）進(jìn)行粒子采樣。若失敗，則粒子采樣使用運(yùn)動(dòng)模型x_t^((i))～p(x_t∣x_(t-1),u_t)，其中x_t、x_(t-1)、u_t分別是當(dāng)前時(shí)刻的機(jī)器人位姿，上一時(shí)刻機(jī)器人位姿，當(dāng)前時(shí)刻里程計(jì)信息。通過(guò)當(dāng)前時(shí)刻里程計(jì)數(shù)據(jù)和上一時(shí)刻機(jī)器人位姿推算當(dāng)前時(shí)刻位姿并計(jì)算粒子權(quán)值。根據(jù)機(jī)器人當(dāng)前位置和已構(gòu)建的地圖，通過(guò)計(jì)算來(lái)構(gòu)建下一時(shí)刻地圖。改進(jìn)前的RBPF使用運(yùn)動(dòng)學(xué)模型作為粒子采樣的建議分布，由于方差較大，只有少數(shù)粒子符合真實(shí)分布，因此必須進(jìn)行重采樣來(lái)使粒子數(shù)符合實(shí)際分布。改進(jìn)后的建議分布在運(yùn)動(dòng)學(xué)模型的基礎(chǔ)上根據(jù)觀測(cè)值和上一時(shí)刻地圖信息對(duì)采樣的粒子進(jìn)行加權(quán)，選用權(quán)重大的粒子進(jìn)而更新地圖，改進(jìn)建議分布如下式所示：機(jī)器人操作系統(tǒng)（ROS）10.4.1使用gmapping構(gòu)建地圖gmapping原理

機(jī)器人操作系統(tǒng)（ROS）10.4.1使用gmapping構(gòu)建地圖PLICP模擬里程計(jì)mapping是一個(gè)依賴?yán)锍逃?jì)的算法，除了激光掃描數(shù)據(jù)，也需要有里程計(jì)信息(Odometry)輸入作為運(yùn)算的前提，比如輪式里程計(jì)，視覺(jué)里程計(jì)等。laser_scan_matcher功能包是基于ROS的增量式激光掃描配準(zhǔn)工具，可以通過(guò)掃描連續(xù)的兩幀sensor_msgs/LaserScan消息之間匹配完成位姿估計(jì)，并將估計(jì)完成的位姿信息以geometry_msgs/Pose2D類型的話題進(jìn)行發(fā)布。在僅有激光雷達(dá)傳感器的情況下，該功能包可以作為單獨(dú)的里程計(jì)估計(jì)器來(lái)使用。laser_scan_matcher功能包的核心是PLICP（點(diǎn)對(duì)線迭代最近點(diǎn)）掃描匹配算法。PLICP算法流程和ICP流程基本一樣，不同之處在于ICP是找最近鄰的一點(diǎn)，以點(diǎn)到點(diǎn)之間的距離作為誤差，而PLICP是找到最近鄰的兩點(diǎn)，兩點(diǎn)連線，是以點(diǎn)到線的距離作為誤差，因此PLICP的匹配誤差比ICP的匹配誤差要小的多。機(jī)器人操作系統(tǒng)（ROS）安裝步驟首先，運(yùn)行如下命令，安裝本次實(shí)驗(yàn)相關(guān)依賴庫(kù)：$sudoapt-getinstalllibsdl1.2-dev$sudoaptinstalllibsdl-image1.2-devROS中已經(jīng)集成了gmapping相關(guān)功能包的二進(jìn)制文件，可以使用如下命令進(jìn)行安裝：$sudoapt-getinstallros-melodic-slam-gmapping下面通過(guò)源碼安裝scan_tools功能包，作用是使用激光數(shù)據(jù)為gmapping提供里程計(jì)。首先，進(jìn)入工作空間的src文件夾下：$cd~/catkin_ws/src/使用如下命令克隆源碼：$gitclone/ccny-ros-pkg/scan_tools.git10.4.1使用gmapping構(gòu)建地圖機(jī)器人操作系統(tǒng)（ROS）源碼下載完成后，運(yùn)行如下命令進(jìn)行編譯，也可將本書提供的功能包（scan_tools）拷貝至工作空間并進(jìn)行編譯：$cd~/catkin_ws/$catkin_make本次實(shí)驗(yàn)使用的激光雷達(dá)為鐳神LS01B，這是一款價(jià)格低廉的二維雷達(dá)，能夠?qū)崿F(xiàn)在25米范圍內(nèi)360度的二維平面掃描。將本書配套代碼中的激光雷達(dá)驅(qū)動(dòng)功能包（ls01b_v2）復(fù)制到當(dāng)前工作空間的src文件夾下，并進(jìn)行編譯。LS01B激光雷達(dá)安裝步驟10.4.1使用gmapping構(gòu)建地圖機(jī)器人操作系統(tǒng)（ROS）如果在編譯過(guò)程中出現(xiàn)無(wú)法找到csm功能包的錯(cuò)誤時(shí)，運(yùn)行如下命令安裝csm功能包，并重新編譯：$sudoapt-getinstallros-melodic-csm無(wú)法找到csm功能包10.4.1使用gmapping構(gòu)建地圖機(jī)器人操作系統(tǒng)（ROS）編譯成功后，修改scan_tools/laser_scan_matcher/demo文件夾中的demo_gmapping.launch文件，其中<paramname=“serial_port”value=“/dev/ttyUSB0”/>，ttyUSB0為激光雷達(dá)端口號(hào)，讀者根據(jù)實(shí)際情況修改，修改后的文件如下（部分）：<launch>##setupleishenlidar#################<nodename="ls01b_v2"pkg="ls01b_v2"type="ls01b_v2"output="screen"><paramname="scan_topic"value="scan"/><paramname="frame_id"value="laser_link"/><paramname="serial_port"value="/dev/ttyUSB0"/>……10.4.1使用gmapping構(gòu)建地圖機(jī)器人操作系統(tǒng)（ROS）配置參數(shù)講解map_udpate_interval：每次更新地圖的時(shí)間間隔（數(shù)據(jù)類型：float，默認(rèn)值：5.0，單位：秒）。該值越小，節(jié)點(diǎn)將更頻繁的更新地圖，代價(jià)是計(jì)算負(fù)荷變大。maxUrange：激光的最大可用范圍。光束被裁剪為該值（數(shù)據(jù)類型：float，默認(rèn)值：80.0，單位：米）。sigma：掃描匹配過(guò)程中cell的標(biāo)準(zhǔn)差（數(shù)據(jù)類型：float，默認(rèn)值：0.05）。kernelSize：掃描匹配過(guò)程的搜索窗口大?。〝?shù)據(jù)類型：int，默認(rèn)值：1）。lstep和astep分別是掃描匹配的初始距離步長(zhǎng)和掃描匹配的初始角度步長(zhǎng)（數(shù)據(jù)類型：float，默認(rèn)值：0.05）。iterations：掃描匹配器的迭代次數(shù)（數(shù)據(jù)類型：int，默認(rèn)值：5）。lsigma：掃描匹配過(guò)程中單個(gè)激光掃描束的標(biāo)準(zhǔn)差（數(shù)據(jù)類型：float,，默認(rèn)值:0.075）。ogain：似然估計(jì)時(shí)使用的增益，用于平滑重采樣效果（數(shù)據(jù)類型：默認(rèn)值:：3.0）。lskip：每個(gè)n+1次掃描進(jìn)行一次掃描匹配，取值為0時(shí)表示每次掃描之后都進(jìn)行一次匹配(數(shù)據(jù)類型：int,默認(rèn)值：0)。srr：位置的噪聲項(xiàng)（數(shù)據(jù)類型：float，默認(rèn)值：0.1）。srt：方位角的噪聲項(xiàng)（數(shù)據(jù)類型：float，默認(rèn)值：0.2）。str：位置到方位角的協(xié)方差項(xiàng)（數(shù)據(jù)類型：float，默認(rèn)值：0.1）。stt：方位角到位置的協(xié)方差項(xiàng)（數(shù)據(jù)類型：float，默認(rèn)值：0.2）。10.4.1使用gmapping構(gòu)建地圖機(jī)器人操作系統(tǒng)（ROS）linearUpdate：只有當(dāng)機(jī)器人至少運(yùn)動(dòng)了linearUpdate的距離之后才進(jìn)行一次新的測(cè)量（數(shù)據(jù)類型：float,默認(rèn)值:1.0）angularUpdate：只有當(dāng)機(jī)器人至少轉(zhuǎn)動(dòng)了angularUpdate的角度之后才進(jìn)行一次新的測(cè)量（數(shù)據(jù)類型：float,默認(rèn)值:0.5）temporalUpdate：如果上次掃描處理的時(shí)間早于更新時(shí)間（秒），則處理掃描。小于零的值將關(guān)閉基于時(shí)間的更新(數(shù)據(jù)類型：float,默認(rèn)值：-1.0)。resampleThreshold：粒子重采樣的閾值。只有當(dāng)評(píng)價(jià)粒子相似度的指標(biāo)Neff小于該閾值時(shí)才進(jìn)行重采樣，所以降低該值意味著提高重采樣的頻率(數(shù)據(jù)類型：float,默認(rèn)值：0.5)。particles：濾波器中的粒子數(shù)，粒子數(shù)越多，定位精度越高，計(jì)算代價(jià)越大（數(shù)據(jù)類型：int，默認(rèn)值：30）。xmin、ymin、xmax和ymax分別是初始地圖大小中X的最小值（數(shù)據(jù)類型：float，默認(rèn)值：-100.0）、Y的最小值（數(shù)據(jù)類型：float，默認(rèn)值：-100.0）、X的最大值（數(shù)據(jù)類型：float，默認(rèn)值：100.0）以及Y的最大值（數(shù)據(jù)類型：float，默認(rèn)值：100.0）。delta：地圖的分辨率（數(shù)據(jù)類型：float，默認(rèn)值：0.05）。llsamplerange：似然估計(jì)的距離采樣范圍(數(shù)據(jù)類型：float，default:0.01)。llsamplestep似然估計(jì)的距離采樣步長(zhǎng)（數(shù)據(jù)類型：float,默認(rèn)值：0.01）。lasamplerange：似然估計(jì)的旋轉(zhuǎn)采樣范圍。（數(shù)據(jù)類型：float,默認(rèn)值：0.005）。lasamplestep：似然估計(jì)的旋轉(zhuǎn)采樣步長(zhǎng)（數(shù)據(jù)類型：float,默認(rèn)值：0.005）。occ_thresh：占用概率閾值（數(shù)據(jù)類型：float，默認(rèn)值：default:0.25）。10.4.1使用gmapping構(gòu)建地圖機(jī)器人操作系統(tǒng)（ROS）實(shí)驗(yàn)測(cè)試首先，將激光雷達(dá)接入工控機(jī)，通過(guò)以下命令查看設(shè)備是否正常接入：$ls/dev若出現(xiàn)ttyUSB*（*代表0-9中的某個(gè)數(shù)，本節(jié)中激光雷達(dá)的設(shè)備是dev/ttyUSB0），則激光雷達(dá)驅(qū)動(dòng)成功。通過(guò)以下命令更改相應(yīng)串口權(quán)限，允許串口進(jìn)行數(shù)據(jù)讀寫：$sudochmod777/dev/ttyUSB0運(yùn)行如下命令啟動(dòng)SLAM節(jié)點(diǎn)，并移動(dòng)實(shí)驗(yàn)小車進(jìn)行地圖構(gòu)建，實(shí)驗(yàn)結(jié)果如圖。$roslaunchlaser_scan_matcherdemo_gmapping.launch實(shí)驗(yàn)結(jié)果10.4.1使用gmapping構(gòu)建地圖機(jī)器人操作系統(tǒng)（ROS）在工作空間目錄下創(chuàng)建一個(gè)map文件夾，通過(guò)如下命令保存當(dāng)前構(gòu)建的地圖，保存后的地圖如圖所示：$rosrunmap_servermap_saver-f~/catkin_ws/map/mymap保存地圖若未安裝map_server，可運(yùn)行如下命令安裝功能包：$sudo

apt-get

install

ros-melodic-map-server保存后的地圖10.4.1使用gmapping構(gòu)建地圖機(jī)器人操作系統(tǒng)（ROS）10.4.2基于地圖的定位與自主導(dǎo)航介紹導(dǎo)航框架（1）amclamcl的英文全稱是adaptiveMonteCarlolocalization，是在蒙特卡洛定位的基礎(chǔ)上，使用自適應(yīng)的KLD（kullback-leiblerdivergence）方法來(lái)更新粒子。蒙特卡洛定位使用粒子濾波的方法進(jìn)行定位，粒子濾波用通俗的話說(shuō)就是一開(kāi)始在地圖空間均勻的撒一把粒子，然后通過(guò)獲取機(jī)器人的移動(dòng)位姿來(lái)移動(dòng)粒子，比如機(jī)器人向前移動(dòng)了一米，所有的粒子也就向前移動(dòng)一米，不管現(xiàn)在這個(gè)粒子的位置對(duì)不對(duì)，使用每個(gè)粒子所處位置模擬一個(gè)傳感器信息與觀察到的傳感器信息（一般是激光信息）作對(duì)比，從而賦給每個(gè)粒子一個(gè)權(quán)重。之后根據(jù)生成的權(quán)重來(lái)重新生成粒子，權(quán)重越高的生成的概率越大。這樣的迭代之后，所有的粒子會(huì)慢慢地收斂到一起，機(jī)器人在地圖上的確切位置也就被推算出來(lái)了。（2）move_basemove_base功能包的作用是將全局路徑規(guī)劃和局部路徑規(guī)劃結(jié)合，從而使機(jī)器人完成基于地圖的導(dǎo)航任務(wù)中的最優(yōu)路徑規(guī)劃，全局路徑規(guī)劃用于生成地圖上機(jī)器人的起始點(diǎn)到設(shè)置的目標(biāo)點(diǎn)的路徑，局部路徑規(guī)劃用于生成到近距離目標(biāo)和為了臨時(shí)躲避障礙物的路徑。機(jī)器人操作系統(tǒng)（ROS）ROS中的導(dǎo)航框架10.4.2基于地圖的定位與自主導(dǎo)航機(jī)器人操作系統(tǒng)（ROS）在機(jī)器人的導(dǎo)航任務(wù)中，首先，機(jī)器人需要發(fā)布必要的傳感器話題（sensortopic），消息類型為sensor_msgs/LaserScan或sensor_msgs/PointCloud，導(dǎo)航目標(biāo)位置信息（move_base_simle/goal），消息類型為geometry_msgs/PoseStamped。其次，要求機(jī)器人發(fā)布里程計(jì)信息及相應(yīng)的TF變換。導(dǎo)航功能包用tf功能包來(lái)確定機(jī)器人在世界坐標(biāo)系中的位置和相對(duì)于靜態(tài)地圖的相關(guān)傳感器信息，但是tf功能包不提供與機(jī)器人速度相關(guān)的任何信息，所以導(dǎo)航功能包要求里程計(jì)源程序發(fā)布一個(gè)變換和一個(gè)包含速度信息的nav_msgs/Odometry消息。最后，該導(dǎo)航功能包輸出控制機(jī)器人移動(dòng)的指令（cmd_vel），并通過(guò)geometry_msgs/Twist類型的消息來(lái)和底層驅(qū)動(dòng)板通信，從而控制電機(jī)運(yùn)轉(zhuǎn)，使機(jī)器人完成相應(yīng)的移動(dòng)。ROS中的導(dǎo)航框架10.4.2基于地圖的定位與自主導(dǎo)航機(jī)器人操作系統(tǒng)（ROS）在導(dǎo)航框架中，機(jī)器人的路徑規(guī)劃包括全局路徑規(guī)劃（globalplanner）和局部實(shí)時(shí)規(guī)劃（localplanner）。前者根據(jù)給定的目標(biāo)位置進(jìn)行總體路徑規(guī)劃，后者根據(jù)所在位置附近的障礙物進(jìn)行躲避規(guī)劃。全局路徑規(guī)劃器使用了A*算法，A*算法是一種高效的路徑搜索算法，采用啟發(fā)函數(shù)來(lái)估計(jì)地圖上機(jī)器人當(dāng)前的位置到目標(biāo)位置之間的距離，并以此選擇最優(yōu)的方向進(jìn)行搜索，如果失敗會(huì)選擇其他路徑繼續(xù)搜索直到得到最優(yōu)路徑。局部路徑實(shí)時(shí)規(guī)劃是利用base_local_plann包實(shí)現(xiàn)的，該包使用DWA（DynamicWindowapproaches，規(guī)劃推理和動(dòng)態(tài)窗口）算法，計(jì)算機(jī)器人每個(gè)周期內(nèi)應(yīng)該行駛的速度和角度（dx，dy，dthetavelocities）。DWA算法中先離散采樣機(jī)器人控制空間（dx,dy,dtheta），再對(duì)于每個(gè)采樣速度，從機(jī)器人當(dāng)前的狀態(tài)，進(jìn)行模擬預(yù)測(cè)。ROS中的導(dǎo)航框架10.4.2基于地圖的定位與自主導(dǎo)航機(jī)器人操作系統(tǒng)（ROS）（3）costmap_2d代價(jià)地圖（costmap）是機(jī)器人收集傳感器信息建立和更新的二維或三維地圖。在move_base的框架下，costmap_2d為全局路徑規(guī)劃和局部路徑規(guī)劃提供了2D的代價(jià)地圖。costmap_2d使用的是占用柵格地圖，通過(guò)多個(gè)圖層描述環(huán)境信息。每個(gè)圖層描述了一種類型的信息，最終的代價(jià)是這些圖層疊加的結(jié)果。比如說(shuō)靜態(tài)地圖層(staticmaplayer)描述的是導(dǎo)航的地圖信息，障礙物層(obstaclelayer)則記錄了環(huán)境中的障礙物，膨脹層(inflationlayer)根據(jù)用戶指定的參數(shù)和機(jī)器人的尺寸將障礙物的占用柵格區(qū)域放大一部分，以防止碰撞。根據(jù)導(dǎo)航類型，代價(jià)地圖又被分成兩部分。一個(gè)是全局地圖（global_costmap），在全局移動(dòng)路徑規(guī)劃中以整個(gè)區(qū)域?yàn)閷?duì)象建立移動(dòng)計(jì)劃。而另一個(gè)被稱為局部地圖（local_costmap），這是在局部移動(dòng)路徑規(guī)劃中，在以機(jī)器人為中心的部分限定區(qū)域中規(guī)劃移動(dòng)路徑時(shí)，或在躲避障礙物時(shí)用到的地圖。然而，盡管兩種地圖的目的不同，但表示方法是相同的。costmap用0到255之間的值來(lái)表示。簡(jiǎn)單地說(shuō)，根據(jù)該值可以知道機(jī)器人是位于可移動(dòng)區(qū)域還是位于可能與障礙物碰撞的區(qū)域。000：機(jī)器人可以自由移動(dòng)的freearea（自由區(qū)域）001~127：碰撞概率低的區(qū)域128~252：碰撞概率高的區(qū)域253~254：碰撞區(qū)域255：機(jī)器人不能移動(dòng)的占用區(qū)域（occupiedarea）10.4.2基于地圖的定位與自主導(dǎo)航機(jī)器人操作系統(tǒng)（ROS）障礙距離與costmap值的關(guān)系10.4.2基于地圖的定位與自主導(dǎo)航機(jī)器人操作系統(tǒng)（ROS）10.4.3安裝步驟導(dǎo)航框架中包含amcl、move_base等很多功能包，可以通過(guò)源碼安裝，也可以使用如下命令安裝：$sudoapt-getinstallros-melodic-navigation用源碼安裝的方法如下，在工作空間的src文件夾下克隆源碼：$gitclone/ros-planning/navigation進(jìn)入navigation功能包查看版本：$cdnavigation

$gitbranch選擇melodic版本的navigation：$gitcheckoutmelodic-devel回到工作空間下進(jìn)行編譯：$cdcatkin_ws/

$catkin_make若編譯出現(xiàn)如圖所示的問(wèn)題，說(shuō)明缺少相應(yīng)功能包，運(yùn)行如下命令安裝相應(yīng)功能包：$sudoapt-getinstallros-melodic-tf2-sensor-msgs編譯報(bào)錯(cuò)機(jī)器人操作系統(tǒng)（ROS）10.4.4參數(shù)配置文件講解代價(jià)地圖配置障礙物信息通過(guò)兩種代價(jià)地圖儲(chǔ)存：一種是global_costmap，用于全局路徑規(guī)劃；一種是local_costmap，用于本地路徑規(guī)劃和實(shí)時(shí)避障。兩種代價(jià)地圖的正常使用需要三個(gè)配置文件，分別是通用配置文件（CommonConfiguration）、全局規(guī)劃配置文件（GlobalConfiguration）和局部規(guī)劃配置文件（LocalConfiguration）。1.通用配置文件obstacle_range:2.5#設(shè)置地圖中檢測(cè)障礙物的最大范圍（m）。raytrace_range:3.0#設(shè)置機(jī)器人檢測(cè)自由空間的最大范圍（m）。footprint:[[0.165,0.165],[-0.165,0.165],[-0.165,-0.165],[0.165,-0.165]]#設(shè)置機(jī)器人在地圖上的占用面積，以機(jī)器人的中心作為原點(diǎn)。若機(jī)器人外形為圓形，則設(shè)置robot_radius（圓形半徑）。這里我們?cè)O(shè)置機(jī)器人外形為矩形。#robot_radius:0.165inflation_radius:0.1#機(jī)器人的膨脹參數(shù)（m），參數(shù)為0.1表示機(jī)器人規(guī)劃的路徑應(yīng)與障礙物保持大于0.1m的安全距離。max_obstacle_height:0.6#障礙物的最大高度（m）。min_obstacle_height:0.0#障礙物的最小高度（m）。observation_sources:scan#代價(jià)地圖需要關(guān)注的傳感器信息scan:{data_type:LaserScan,topic:/scan,marking:true,clearing:true,expected_update_rate:0}#分別為傳感器的消息類型、話題、是否使用傳感器的實(shí)時(shí)信息來(lái)添加或清除代價(jià)地圖的障礙物信息以及根據(jù)傳感器實(shí)際發(fā)布的速率為每個(gè)觀測(cè)源設(shè)置預(yù)期更新速率，當(dāng)傳感器低于預(yù)期速率時(shí)，會(huì)在終端中發(fā)出警告。地圖的更新，來(lái)源于機(jī)器人發(fā)布的傳感器消息，代價(jià)地圖儲(chǔ)存由傳感器獲取的障礙物信息。配置文件名為costmap_common_params.yaml，文件內(nèi)容與解釋如下：機(jī)器人操作系統(tǒng)（ROS）全局規(guī)劃配置文件用于全局代價(jià)地圖參數(shù)的配置，配置文件名為global_costmap_params.yaml,文件內(nèi)容及解釋如下：2.全局規(guī)劃配置文件global_costmap:global_frame:/map#表示全局代價(jià)地圖在哪個(gè)坐標(biāo)系下運(yùn)行，這里選擇map參考系。robot_base_frame:/base_footprint#表示全局地圖參考的機(jī)器人坐標(biāo)系。update_frequency:1.0#設(shè)置全局地圖信息更新的頻率（HZ）。publish_frequency:1.0#設(shè)置全局地圖信息發(fā)布的頻率（HZ）。static_map:true#用來(lái)決定代價(jià)地圖是否需要根據(jù)map_server提供的地圖信息進(jìn)行初始化，若不需要已知地圖或map_server，將該參數(shù)設(shè)為false。rolling_window:false#用來(lái)設(shè)置機(jī)器人移動(dòng)過(guò)程中是否需要滾動(dòng)窗口來(lái)保持機(jī)器人處于中心位置。resolution:0.01#