一、目標(biāo)
搭載Robosense 16線激光雷達(dá)和6軸IMU傳感器的履帶機(jī)器人，通過(guò)使用ros的navigation自主導(dǎo)航框架完成點(diǎn)到點(diǎn)的自主導(dǎo)航。
二、總體方案設(shè)計(jì)
2.1 功能架構(gòu)設(shè)計(jì)
Navigation導(dǎo)航框架如下圖所示，其主要完成了全局路徑規(guī)劃器、局部路徑規(guī)劃器、全局代價(jià)地圖、局部代價(jià)地圖和行為決策（recovery_behaviors）的實(shí)現(xiàn)。
由上圖可知，為實(shí)現(xiàn)自主導(dǎo)航功能，還需要地圖、感知、定位、控制這四個(gè)模塊。
2.2 地圖模塊
地圖模塊需要提供全局地圖，以便全局路徑規(guī)劃器進(jìn)行全局路徑規(guī)劃。
全局地圖一般為二維占據(jù)柵格地圖，可由圖像(jpg等格式)表示，其中白色表示自由，黑色表示占據(jù)，灰色代表未知區(qū)域。
navigation框架提供了一個(gè)可選的地圖模塊package：map_server。
該package通過(guò)讀取地圖配置文件和地圖，然后發(fā)布/map話題，以供全局規(guī)劃器進(jìn)行全局路徑規(guī)劃
地圖配置文件為.yaml文件，主要由6個(gè)參數(shù)描述：
- 1.地圖路徑
- 2.地圖分辨率（單位：米）
- 3.地圖坐標(biāo)系原點(diǎn)（x，y，theta）
- 4.是否翻轉(zhuǎn)（黑色占據(jù)，白色自由 -> 黑色自由，白色占據(jù)）
- 5.占據(jù)閾值
- 6.自由閾值
image: result.jpg
resolution: 0.20000
origin: [-61, -149.5, 0.000]
negate: 0
occupied_thresh: 0.65
free_thresh: 0.196
地圖為.jpg文件，其中白色表示自由，黑色表示占據(jù)，一般需要提前讓機(jī)器人用SLAM算法為工作區(qū)域建立二維柵格地圖。。
地圖模塊實(shí)踐操作：
- 1.建圖
生成二維地圖和yaml配置文件
在實(shí)際工程中一般用3D SLAM算法先建立三維點(diǎn)云地圖，然后設(shè)置地圖分辨率參數(shù)，占據(jù)/自由閾值，將其投影為二維柵格地圖，同時(shí)輸出三維點(diǎn)云地圖中坐標(biāo)原點(diǎn)對(duì)應(yīng)二維柵格地圖中的像素坐標(biāo)。
在使用地圖模塊時(shí)，需要分清楚機(jī)器人在地圖坐標(biāo)系下的坐標(biāo)和定位坐標(biāo)系下坐標(biāo)的區(qū)別。
機(jī)器人的二維柵格地圖坐標(biāo)系為像素坐標(biāo)系，圖片的左頂點(diǎn)為原點(diǎn)，圖片寬度為x，圖片高度為y，定位數(shù)據(jù)通過(guò)地圖分辨率轉(zhuǎn)換為像素坐標(biāo)。
- 2.定位
若在定位時(shí)，使用二維地圖對(duì)應(yīng)的三維點(diǎn)云地圖進(jìn)行定位，則二維地圖坐標(biāo)系與三維點(diǎn)云地圖坐標(biāo)系完成對(duì)應(yīng)，機(jī)器人在三維點(diǎn)云地圖坐標(biāo)系下的定位坐標(biāo)可直接作為二維地圖坐標(biāo)系下坐標(biāo)。
若在定位時(shí)，未使用二維地圖對(duì)應(yīng)的三維點(diǎn)云地圖進(jìn)行定位，而是使用其它方式，如RTK、UWB等作為定位數(shù)據(jù)，則需要在建圖的同時(shí)記錄三維點(diǎn)云地圖中坐標(biāo)原點(diǎn)對(duì)應(yīng)的RTK、UWB值和原點(diǎn)對(duì)應(yīng)二維柵格地圖中的像素坐標(biāo)。機(jī)器人在二維地圖坐標(biāo)系下的坐標(biāo)為，實(shí)時(shí)的RTK、UWB值 減去 三維點(diǎn)云地圖中坐標(biāo)原點(diǎn)對(duì)應(yīng)的RTK、UWB值。
2.3 定位模塊
定位方式有多種：①輪速里程計(jì)+imu；②激光+imu；③GPS(RTK)+激光+imu；④GPS(RTK)+激光+imu+輪速里程計(jì)；⑤相機(jī)+imu；⑥GPS(RTK)+相機(jī)+imu；⑦GPS(RTK)+相機(jī)+imu+輪速里程計(jì)；⑧GPS(RTK)+相機(jī)+激光+imu+輪速里程計(jì)；⑨...
以上不同定位方式的定位精度和成本不同，其中⑧的成本最高，定位效果最好；③的應(yīng)用最廣泛，不依賴特定的平臺(tái)，同時(shí)環(huán)境適應(yīng)性很強(qiáng)。其它定位方式與具體的場(chǎng)景有關(guān)，如室內(nèi)環(huán)境、輪式平臺(tái)等。
Navigation要求定位模塊發(fā)出/odom的Topic名稱，消息類型是nav_msgs/Odometry，包含機(jī)器人的位姿和速度。
GPS(RTK)設(shè)備不斷的重新接收和計(jì)算世界坐標(biāo)系中機(jī)器人的位姿，但連續(xù)兩幀的GPS(RTK)坐標(biāo)是不連續(xù)的，可能會(huì)發(fā)生跳變。這意味著在map坐標(biāo)系中機(jī)器人的姿態(tài)再兩幀之間會(huì)發(fā)生離散的跳變。
GPS(RTK)作為長(zhǎng)期的全局坐標(biāo)是很有用的，但是位姿的跳變極大影響運(yùn)動(dòng)規(guī)劃和控制模塊的性能，所以一般不直接使用GPS(RTK)坐標(biāo)進(jìn)行運(yùn)動(dòng)規(guī)劃和控制。
機(jī)器人的里程計(jì)坐標(biāo)是能夠保證連續(xù)的，這意味著在里程計(jì)坐標(biāo)系中的機(jī)器人的姿態(tài)總是平滑變化，沒(méi)有跳變。因此適用于進(jìn)行運(yùn)動(dòng)規(guī)劃和控制。
激光和imu組合成激光里程計(jì)，然后通過(guò)GPS(RTK)進(jìn)行校正是常用的定位方法。
定位模塊實(shí)踐操作：
1.讓負(fù)責(zé)定位的同學(xué)發(fā)出里程計(jì)坐標(biāo)，Topic的名稱是/odom，消息類型是nav_msgs/Odometry，包含機(jī)器人的位姿和速度。
2.如果定位模塊中有tf廣播，需要注釋掉，避免和自己寫的tf沖突。
3.定位模塊發(fā)出的里程計(jì)坐標(biāo)的topic的frame_id要綁定到odom固定坐標(biāo)系下（或者直接綁定到map下），不能綁定到base_link坐標(biāo)系下。
2.4 感知模塊
Navigation使用的sensro sources是激光雷達(dá)點(diǎn)云信息，話題名稱是/scan，數(shù)據(jù)類型是LaserScan。
單線激光雷達(dá)可以室內(nèi)滿足要求，但無(wú)法在室外環(huán)境下為機(jī)器人提供足夠的環(huán)境信息。通常使用多線激光雷達(dá)在室外環(huán)境下進(jìn)行環(huán)境感知。
在Navigation中，采用多線激光雷達(dá)在室外環(huán)境下進(jìn)行環(huán)境感知，需要將多線激光雷達(dá)數(shù)據(jù)格式轉(zhuǎn)換為L(zhǎng)aserScan數(shù)據(jù)類型。
推薦用pointcloud_to_laserscan包進(jìn)行數(shù)據(jù)轉(zhuǎn)換。該包是將多線激光雷達(dá)投影為二維數(shù)據(jù)，并不是只提取多線激光雷達(dá)中的一條線。
<group ns="bigrobot2">
<node pkg="pointcloud_to_laserscan" type="pointcloud_to_laserscan_node" name="pointcloud_to_laserscan">
<rosparam>
<remap from="cloud_in" to="/lidar_cloud_origin"/>
target_frame: bigrobot2/base_footprint # Leave disabled to output scan in pointcloud frame
transform_tolerance: 0.01
min_height: -0.4
max_height: 1.0
angle_min: -3.1415926 # -M_PI
angle_max: 3.1415926 # M_PI
angle_increment: 0.003 # 0.17degree
scan_time: 0.1
range_min: 0.1
range_max: 100
use_inf: true
inf_epsilon: 1.0
# Concurrency level, affects number of pointclouds queued for processing and number of threads used
# 0 : Detect number of cores
# 1 : Single threaded
# 2->inf : Parallelism level
concurrency_level: 1
</rosparam>
</node>
</group>
注意1： target_frame: bigrobot2/base_footprint 這個(gè)參數(shù)要求轉(zhuǎn)換前的點(diǎn)云frame_id為bigrobot2/base_footprint，而該點(diǎn)云是直接讀取速騰激光雷達(dá)點(diǎn)云topic，所以需要在速騰激光雷達(dá)點(diǎn)云的配置文件中直接修改發(fā)出點(diǎn)云topic的frame_id為bigrobot2/base_footprint。
注意2：rqt_graph顯示pointcloud_to_laserscan沒(méi)有訂閱上points_raw，是因?yàn)闆](méi)有節(jié)點(diǎn)訂閱 scan。必須有節(jié)點(diǎn)訂閱scan，pointcloud_to_laserscan節(jié)點(diǎn)才會(huì)訂閱points_raw
感知模塊實(shí)踐操作
- 1.修改速騰激光雷達(dá)驅(qū)動(dòng)配置文件，修改雷達(dá)發(fā)出點(diǎn)云
topic的frame_id為bigrobot2/ lidar_cloud_origin - 2.修改
pointcloud_to_laserscan的launch文件中的target_frame為已有tf 樹上的frame_id。
注意是否需要remap相應(yīng)的topic，可以通過(guò)rosnode info pointcloud_to_laserscan_node查看發(fā)出和接收的topic。
remap和group的規(guī)則：先remap，再group。
remap的規(guī)則：<remap from="old_topic" to="new_topic">
2.5 tf樹
此模塊非常重要！
需要自己創(chuàng)建節(jié)點(diǎn)進(jìn)行tf樹的廣播，以便激光雷達(dá)點(diǎn)云數(shù)據(jù)、里程計(jì)的topic綁定對(duì)應(yīng)的frame_id，必須確保這些topic綁定到對(duì)應(yīng)的frame_id，否則Navigation無(wú)法正確運(yùn)行。
Navigation是通過(guò)ROS中的tf 樹來(lái)維護(hù)機(jī)器人中不同數(shù)據(jù)之間的坐標(biāo)變換，如將激光雷達(dá)點(diǎn)云從激光雷達(dá)坐標(biāo)系(base_laser)變換到車體坐標(biāo)系(base_link)。
一個(gè)機(jī)器人通常tf 樹的結(jié)構(gòu)如下:
每個(gè)坐標(biāo)系都有一個(gè)父坐標(biāo)系和任意子坐標(biāo)系，map坐標(biāo)系是odom坐標(biāo)系的父，odom坐標(biāo)系是base_link的父，每個(gè)坐標(biāo)系只能有一個(gè)父類。
map下可以有多個(gè)定位分支odom，gps(uwb)，fusion_localization，這些分支與map原點(diǎn)可以設(shè)定是重合的或者由固定坐標(biāo)轉(zhuǎn)換，采用不同定位方式得到的定位結(jié)果?？梢愿鶕?jù)實(shí)際定位效果，把base_link綁定在不同的定位坐標(biāo)系下。
①world->map->gps(uwb)->base_link->base_laser
②world->map->odom->base_link->base_laser
③world->map->fusion_localization->base_link->base_laser
注意，如果slam定位模塊中有tf廣播，需要注釋掉，避免和navigation的tf沖突，ROS只允許一個(gè)tf樹存在。
Rviz中的fixed_frame也是根據(jù)tf樹構(gòu)建的坐標(biāo)樹顯示相應(yīng)的topic。
坐標(biāo)系簡(jiǎn)析
- 1.世界坐標(biāo)(earth)
固定坐標(biāo)系（fixed frame），用來(lái)描述某點(diǎn)在地球上的絕對(duì)位置，通常用經(jīng)緯度和高度，或者WGS84表示。
引入世界坐標(biāo)系的背景是：
1.可以將機(jī)器人在地圖中的坐標(biāo)通過(guò)earth直接轉(zhuǎn)換為經(jīng)緯高。
2.機(jī)器人建圖時(shí)會(huì)因?yàn)榈貓D過(guò)大(TB甚至PB數(shù)量級(jí)）無(wú)法一次性完成建圖，需要將地圖分為多個(gè)子圖（自動(dòng)駕駛地圖就是由多個(gè)子圖組成），可以利用經(jīng)緯高完成不同的子圖拼接。
如果只有一張地圖，一般而言，世界坐標(biāo)系原點(diǎn)與地圖坐標(biāo)系原點(diǎn)重合
-
2.map坐標(biāo)系
固定坐標(biāo)系（fixed frame），地圖坐標(biāo)系
作為長(zhǎng)期的全局參考是很有用的，但是跳變使得對(duì)于本地傳感和執(zhí)行器來(lái)說(shuō)，其實(shí)是一個(gè)不好的參考坐標(biāo)。
- 3.里程計(jì)坐標(biāo)系(odom)
odom 坐標(biāo)系是一個(gè)固定坐標(biāo)系（fixed frame）。
odom和map坐標(biāo)系再機(jī)器人運(yùn)動(dòng)開始是重合的。但是，隨著時(shí)間的推移是不重合的，而出現(xiàn)的偏差就是里程計(jì)的累積誤差。
那map–>odom的tf怎么得到?就是在一些校正傳感器合作校正的package比如amcl會(huì)給出一個(gè)位置估計(jì)（localization），這可以得到map–>base_link的tf，所以估計(jì)位置和里程計(jì)位置的偏差也就是odom與map的坐標(biāo)系偏差。所以，如果你的odom計(jì)算沒(méi)有錯(cuò)誤，那么map–>odom的tf就是0。
- 4.車體坐標(biāo)系(base_link)
該base_link坐標(biāo)剛性地連接到移動(dòng)機(jī)器人基座。base_link可以安裝在基座中的任意方位；對(duì)于每個(gè)硬件平臺(tái)，在基座上的不同地方都會(huì)提供一個(gè)明顯的參考點(diǎn)。
機(jī)器人本體坐標(biāo)系，與機(jī)器人中心重合，當(dāng)然有些機(jī)器人(PR 2)是base_footprint,其實(shí)是一個(gè)意思。
-
5.激光雷達(dá)的坐標(biāo)系(base_laser)
與激光雷達(dá)的安裝點(diǎn)有關(guān)，其與base_link的tf為固定的。
2.6 規(guī)劃模塊
如果以播放bag包形式運(yùn)行move_base，costmap會(huì)報(bào)錯(cuò)，costmap的tolorence時(shí)間超過(guò)1s閾值，此時(shí)需要設(shè)置時(shí)間為仿真時(shí)間。
首先啟動(dòng)bag包的播放，讓其發(fā)布/clock話題，這樣其他包就會(huì)從這個(gè)包的時(shí)間戳讀時(shí)間
1 設(shè)置采用仿真時(shí)間
rosparam set /use_sim_time true
2 仿真時(shí)間來(lái)源為bag的時(shí)間戳
rosbag play --pause --clock -k a.bag
--pause：停止播放（按空格取消）
--clock：以bag時(shí)間為clock加，使用仿真時(shí)間
-k, --keep-alive：包的數(shù)據(jù)結(jié)束后，仍然提供時(shí)間服務(wù)，避免一些程序讀時(shí)間產(chǎn)生bug
2.7 控制模塊
三、Navigation配置
3.1 move_base.launch
navigation實(shí)際使用的核心在于參數(shù)配置文件!
Navigation由于參數(shù)配置文件不同，在實(shí)際使用過(guò)程中的導(dǎo)航效果也不同，本節(jié)將從launch文件與配置參數(shù)文件進(jìn)行分析，move_base.launch為啟動(dòng)文件，啟動(dòng)的節(jié)點(diǎn)包含map_server、move_base以及pointcloud_to_laserscan。
該launch文件不包含定位節(jié)點(diǎn)，需要額外單獨(dú)啟動(dòng)，詳情參考另一篇博客navigation定位未加鏈接！
3.2 map_server節(jié)點(diǎn)配置參數(shù)
move_base.launch首先啟動(dòng)map_server節(jié)點(diǎn)，加載了104andtest.yaml的地圖配置文件，并設(shè)置map_server發(fā)布的map的frame_id為map。
<?xml version="1.0"?>
<launch>
<node name="map_server" pkg="map_server" type="map_server" args="$(find move_control)/maps/104andtest.yaml">
<param name="frame_id" value="map"/>
</node>
104andtest.yaml文件如下,主要是制定了地圖文件和地圖參數(shù)如原點(diǎn),分辨率等.
image: 104andtest.pgm
resolution: 0.050000
origin: [-141.372849, -52.350185, 0.000000]
negate: 0
occupied_thresh: 0.65
free_thresh: 0.196
104andtest.pgm為slam建立的二維地圖,類似
3.3 pointcloud_to_laserscan節(jié)點(diǎn)配置參數(shù)
接著啟動(dòng)pointcloud_to_laserscan節(jié)點(diǎn)
<node pkg="pointcloud_to_laserscan" type="pointcloud_to_laserscan_node" name="pointcloud_to_laserscan">
<remap from="/cloud_in" to="/points_raw"/>
<rosparam>
target_frame: velodyne # Leave disabled to output scan in pointcloud frame
<!-- outputxx_frame: base_link -->
transform_tolerance: 0.01
min_height: -0.4
max_height: 1.0
angle_min: -3.1415926 # -M_PI
angle_max: 3.1415926 # M_PI
angle_increment: 0.003 # 0.17degree
scan_time: 0.1
range_min: 0.1
range_max: 100
use_inf: true
inf_epsilon: 1.0
# Concurrency level, affects number of pointclouds queued for processing and number of threads used
# 0 : Detect number of cores
# 1 : Single threaded
# 2->inf : Parallelism level
concurrency_level: 1
</rosparam>
<remap from="/scan" to="/scan"/>
</node>
該節(jié)點(diǎn)需要訂閱雷達(dá)點(diǎn)云信息,因?yàn)樵掝}名稱不匹配,需要remap訂閱的話題名稱<remap from="/cloud_in" to="/points_raw"/>
根據(jù)tf關(guān)系決定是否調(diào)整輸出的點(diǎn)云信息的target_frame``,target_frame決定了點(diǎn)云是掛在哪個(gè)tf坐標(biāo)系下的。
min_height:為-0.4,若為0,則會(huì)把地面當(dāng)成障礙物,出現(xiàn)一圈環(huán)形障礙物
max_height:為1.0,高于機(jī)器人本體的高度即可,低于該高度的點(diǎn)云都會(huì)被投影下來(lái)
scan_time:為發(fā)出點(diǎn)云的間隔時(shí)間,0.1s,對(duì)應(yīng)10Hz
range_min:點(diǎn)云距離雷達(dá)最小的距離
range_max:點(diǎn)云距離雷達(dá)最大的距離
use_inf:設(shè)置為true,若為false,可能導(dǎo)致costmap地圖更新會(huì)有問(wèn)題,與costmap_2D的障礙層的inf_is_valid參數(shù)相關(guān)聯(lián),對(duì)于無(wú)窮遠(yuǎn)的點(diǎn)要求其值為inf
3.4 move_base節(jié)點(diǎn)配置參數(shù)
啟動(dòng)核心節(jié)點(diǎn)move_base
<launch>
...
<node pkg="move_base" type="move_base" respawn="false" name="move_base" output="screen">
<param name="controller_frequency" value="10.0"/>
<param name="controller_patiente" value="15.0"/>
<param name="planner_frequnency" value="0.0"/>
<param name="planner_patience" value="5.0" />
<rosparam command="load" file="$(find move_control)/config/global_planner_params.yaml"/>
<param name="base_global_planner" value="global_planner/GlobalPlanner"/>
<rosparam command="load" file="$(find move_control)/config/teb_local_planner_params.yaml" />
<param name="base_local_planner" value="teb_local_planner/TebLocalPlannerROS" />
<rosparam file="$(find move_control)/config/costmap_common_params.yaml" command="load" ns="global_costmap" />
<rosparam file="$(find move_control)/config/costmap_common_params.yaml" command="load" ns="local_costmap" />
<rosparam file="$(find move_control)/config/local_costmap_params.yaml" command="load" />
<rosparam file="$(find move_control)/config/global_costmap_params.yaml" command="load" />
<rosparam file="$(find move_control)/config/costmap_converter_params.yaml" command="load" />
</launch>
controller_frequency:以 Hz 為單位運(yùn)行控制回路并向地盤發(fā)送速度命令的速率。
controller_patiente:在執(zhí)行空間清理操作之前，控制器將等待多長(zhǎng)時(shí)間（以秒為單位）而沒(méi)有收到有效的控制。
planner_patience:在執(zhí)行空間清理操作之前，規(guī)劃器將在幾秒鐘內(nèi)等待多長(zhǎng)時(shí)間以嘗試找到有效規(guī)劃結(jié)果。
planner_frequnency:以 Hz 為單位運(yùn)行全局規(guī)劃循環(huán)的速率。如果頻率設(shè)置為 0.0，則全局規(guī)劃器將僅在收到新目標(biāo)或本地規(guī)劃器報(bào)告其路徑被阻塞時(shí)運(yùn)行。該參數(shù)是Navigation 1.6.0 中的新功能
3.4.1 base_global_planner節(jié)點(diǎn)配置參數(shù)
global_planner/GlobalPlanner參數(shù)為navigation自帶的全局規(guī)劃器,包含多種規(guī)劃算法.
<rosparam command="load" file="$(find move_control)/config/global_planner_params.yaml"/>
<param name="base_global_planner" value="global_planner/GlobalPlanner"/>
global_planner_params.yaml文件如下,其中use_dijkstra若為false,則采用A*算法.
GlobalPlanner:
allow_unknown: false #true
default_tolerance: 0.2 # If goal in obstacle, plan to the closest point in radius default_tolerance, default 0.0
visualize_potential: false
use_dijkstra: true #If true, use dijkstra's algorithm. Otherwise, A*.
use_quadratic: true
use_grid_path: false
old_navfn_behavior: false # Exactly mirror behavior of navfn, use defaults for other boolean parameters, default false
3.4.2 base_local_planner節(jié)點(diǎn)配置參數(shù)
teb_local_planner/TebLocalPlannerROS為teb局部路徑規(guī)劃器,需要額外安裝,可以參考teb規(guī)劃器未加鏈接
<rosparam command="load" file="$(find move_control)/config/teb_local_planner_params.yaml" />
<param name="base_local_planner" value="teb_local_planner/TebLocalPlannerROS" />
teb_local_planner_params.yaml文件如下
TebLocalPlannerROS:
odom_topic: odom
# Trajectory
teb_autosize: True
dt_ref: 0.34
dt_hysteresis: 0.15
max_samples: 500
global_plan_overwrite_orientation: True
allow_init_with_backwards_motion: False # false
max_global_plan_lookahead_dist: 1.5
global_plan_viapoint_sep: -1
global_plan_prune_distance: 1
exact_arc_length: True
feasibility_check_no_poses: 5
publish_feedback: False
# Robot
max_vel_x: 2.0 # 0.8
max_vel_x_backwards: 0.8
max_vel_y: 0.0
max_vel_theta: 0.6
acc_lim_x: 0.4 # 0.2
acc_lim_theta: 0.6
min_turning_radius: 0.0 # diff-drive robot (can turn on place!)
footprint_model:
# type: "polygon"
# vertices: [[-1.223, 0.42], [-1.223, -0.42], [0.3, -0.42], [0.3, 0.42]]
type: "two_circles"
front_offset: 0.0
front_radius: 0.28
rear_offset: 0.3
rear_radius: 0.25
# GoalTolerance
xy_goal_tolerance: 0.2
yaw_goal_tolerance: 0.1
free_goal_vel: False
complete_global_plan: True
# Obstacles
min_obstacle_dist: 0.1 # 0.25 # This value must also include our robot radius, since footprint_model is set to "point".
inflation_dist: 0.2 # 0.6
include_costmap_obstacles: True
costmap_obstacles_behind_robot_dist: 1.5
obstacle_poses_affected: 15
dynamic_obstacle_inflation_dist: 0.2 # 0.6
include_dynamic_obstacles: True
costmap_converter_plugin: ""
costmap_converter_spin_thread: True
costmap_converter_rate: 5
# Optimization
no_inner_iterations: 5
no_outer_iterations: 4
optimization_activate: True
optimization_verbose: False
penalty_epsilon: 0.001 # 0.1
obstacle_cost_exponent: 4
weight_max_vel_x: 2
weight_max_vel_theta: 1
weight_acc_lim_x: 1
weight_acc_lim_theta: 1
weight_kinematics_nh: 1000
weight_kinematics_forward_drive: 1
weight_kinematics_turning_radius: 1
weight_optimaltime: 1 # must be > 0
weight_shortest_path: 0
weight_obstacle: 100
weight_inflation: 0.2
weight_dynamic_obstacle: 10
weight_dynamic_obstacle_inflation: 0.2
weight_viapoint: 1
weight_adapt_factor: 2
# Homotopy Class Planner
enable_homotopy_class_planning: True
enable_multithreading: True
max_number_classes: 4
selection_cost_hysteresis: 1.0
selection_prefer_initial_plan: 0.9
selection_obst_cost_scale: 100.0
selection_alternative_time_cost: False
roadmap_graph_no_samples: 15
roadmap_graph_area_width: 5
roadmap_graph_area_length_scale: 1.0
h_signature_prescaler: 0.5
h_signature_threshold: 0.1
obstacle_heading_threshold: 0.45
switching_blocking_period: 0.0
viapoints_all_candidates: True
delete_detours_backwards: True
max_ratio_detours_duration_best_duration: 3.0
visualize_hc_graph: False
visualize_with_time_as_z_axis_scale: False
# Recovery
shrink_horizon_backup: True
shrink_horizon_min_duration: 10
oscillation_recovery: True
oscillation_v_eps: 0.1
oscillation_omega_eps: 0.1
oscillation_recovery_min_duration: 10
oscillation_filter_duration: 10
teb的參數(shù)可分為七類:
Trajectory:局部軌跡的參數(shù),如軌跡長(zhǎng)度,間隔時(shí)間,采樣數(shù)量,是否允許倒退等
Robot:機(jī)器人最大速度,加速度和機(jī)器人外形尺寸
GoalTolerance:到達(dá)目標(biāo)位置的誤差參數(shù)
Obstacles:障礙物距離與膨脹參數(shù)問(wèn)題!!!與costmap中的障礙物膨脹有什么區(qū)別
Optimization:目標(biāo)函數(shù)權(quán)重參數(shù),優(yōu)化求解器參數(shù)
Homotopy Class Planner:同倫類規(guī)劃器,teb中兩種類實(shí)例化局部規(guī)劃器,teb與Homotopy Class Planner.后者是一系列規(guī)劃器的組合.參考
Recovery:類似navigation的recovery模塊,陷入困境時(shí)脫困使用.
teb各個(gè)參數(shù)具體含義及調(diào)整可以參考這篇文章
3.4.3 costmap節(jié)點(diǎn)配置參數(shù)
navigation采用costmap_2D作為地圖層,通過(guò)加載不同參數(shù)生成全局地圖和局部地圖.
<rosparam file="$(find move_control)/config/costmap_common_params.yaml" command="load" ns="global_costmap" />
<rosparam file="$(find move_control)/config/costmap_common_params.yaml" command="load" ns="local_costmap" />
<rosparam file="$(find move_control)/config/local_costmap_params.yaml" command="load" />
<rosparam file="$(find move_control)/config/global_costmap_params.yaml" command="load" />
<rosparam file="$(find move_control)/config/costmap_converter_params.yaml" command="load" />
通過(guò)這些參數(shù)文件在ROS的參數(shù)服務(wù)器上注冊(cè)了global_costmap/xx與local_costmap/xx等參數(shù),move_base中會(huì)調(diào)用costmap_2D的API接口,根據(jù)這些參數(shù)實(shí)例化兩個(gè)costmap對(duì)象,即global_costmap和local_costmap.
costmap_common_params.yaml文件,其中參數(shù)是全局地圖和局部地圖共有的參數(shù).通過(guò)設(shè)置namespace參數(shù)ns="global_costmap"與ns="local_costmap"將global_costmap和local_costmap相同的參數(shù)一同初始化,避免改同一個(gè)參數(shù),要修改兩次.
當(dāng)然也可以不用costmap_common_params.yaml文件,把這些參數(shù)全部放到global_costmap_params.yaml和local_costmap_params.yaml文件中.
costmap_common_params.yaml文件如下
robot_radius: 0.3
obstacle_layer:
enabled: true
combination_method: 1
track_unknown_space: true
obstacle_range: 6
raytrace_range: 6.1
observation_sources: scan
scan:
sensor_frame: velodyne
data_type: LaserScan
topic: scan
marking: true
clearing: true
inf_is_valid: true
expected_update_rate: 0
# inflation_layer:
# enabled: true
# cost_scaling_factor: 10.0 # 0.1
# inflation_radius: 0.8 # 0.01
static_layer:
enabled: true
global_costmap和local_costmap的機(jī)器人尺寸為robot_radius,兩者都包含static_layer和obstacle_layer,而inflation_layer層的配置參數(shù)則由global_costmap和local_costmap分別配置,其中障礙物層參數(shù)解析可見添加鏈接.
global_costmap_params.yaml文件如下,
global_costmap:
global_frame: map
robot_base_frame: base_link
update_frequency: 5.0
publish_frequency: 5.0
static_map: true
rolling_window: false
resolution: 0.1
robot_radius: 0.3
transform_tolerance: 1.0
map_type: costmap
plugins:
- {name: static_layer, type: "costmap_2d::StaticLayer"}
- {name: obstacle_layer, type: "costmap_2d::ObstacleLayer"}
- {name: inflation_layer, type: "costmap_2d::InflationLayer"}
inflation_layer:
enabled: true
cost_scaling_factor: 10.0 # 0.1
inflation_radius: 0.4 # 0.01
所有參數(shù)都是global_costmap:的二級(jí)子參數(shù),類似global_costmap/global_frame,參數(shù)解析添加鏈接
local_costmap_params.yaml文件如下
local_costmap:
global_frame: map
robot_base_frame: base_link
update_frequency: 10.0
publish_frequency: 10.0
static_map: false
rolling_window: true
width: 6.0
height: 6.0
resolution: 0.1
robot_radius: 0.2
transform_tolerance: 1.0
plugins:
- {name: obstacle_layer, type: "costmap_2d::ObstacleLayer"}
- {name: inflation_layer, type: "costmap_2d::InflationLayer"}
inflation_layer:
enabled: true
cost_scaling_factor: 10.0 # 0.1
inflation_radius: 0.2 # 0.01
costmap_converter_params.yaml文件如下
###########################################################################################
## NOTE: Costmap conversion is experimental. Its purpose is to combine many point ##
## obstales into clusters, computed in a separate thread in order to improve the overall ##
## efficiency of local planning. However, the implemented conversion algorithms are in a ##
## very early stage of development. Contributions are welcome! ##
###########################################################################################
TebLocalPlannerROS:
## Costmap converter plugin
#costmap_converter_plugin: "costmap_converter::CostmapToPolygonsDBSMCCH"
costmap_converter_plugin: "costmap_converter::CostmapToLinesDBSRANSAC"
#costmap_converter_plugin: "costmap_converter::CostmapToLinesDBSMCCH"
#costmap_converter_plugin: "costmap_converter::CostmapToPolygonsDBSConcaveHull"
costmap_converter_spin_thread: True
costmap_converter_rate: 5
## Configure plugins (namespace move_base/costmap_to_lines or move_base/costmap_to_polygons)
## costmap_converter/CostmapToLinesDBSRANSAC, costmap_converter/CostmapToLinesDBSMCCH, costmap_converter/CostmapToPolygonsDBSMCCH
costmap_converter/CostmapToLinesDBSRANSAC:
cluster_max_distance: 0.4
cluster_min_pts: 2
ransac_inlier_distance: 0.15
ransac_min_inliers: 10
ransac_no_iterations: 1500
ransac_remainig_outliers: 3
ransac_convert_outlier_pts: True
ransac_filter_remaining_outlier_pts: False
convex_hull_min_pt_separation: 0.1
costmap_converter誠(chéng)如代碼中的note所說(shuō),還處于very eary stage,早期不穩(wěn)定版本,該參數(shù)不是必須的!
costmap_converter是teb作者提供了一個(gè)插件用于轉(zhuǎn)換代價(jià)地圖，原始的代價(jià)地圖是由柵格地圖中單元格組成，用于表示障礙物，但單元格占用的計(jì)算資源較大，故采用插件將單元格轉(zhuǎn)換成點(diǎn)，線，多邊形表示。
可以[參考]這篇文章了解convert(https://blog.csdn.net/weixin_42621524/article/details/120721270)
地圖拼接，新地圖坐標(biāo)系原點(diǎn)，相對(duì)舊地圖坐標(biāo)系的變換矩陣。
把odom發(fā)布的cpp文件放到autoware中.
6。問(wèn)題解決
注意pointcloud_to_laserscan是接收到cloud in的 topic后才會(huì)創(chuàng)建去訂閱
remap不成功，在cloudin前面加了個(gè)/，不知道能否起作用
https://blog.csdn.net/qq_23670601/article/details/88529739
參考：
坐標(biāo)系：https://blog.csdn.net/lovely_yoshino/article/details/99699321
