# squ_robot
**Repository Path**: suqian_robot_college/squ_robot
## Basic Information
- **Project Name**: squ_robot
- **Description**: developing
- **Primary Language**: Python
- **License**: Not specified
- **Default Branch**: master
- **Homepage**: None
- **GVP Project**: No
## Statistics
- **Stars**: 1
- **Forks**: 0
- **Created**: 2022-10-01
- **Last Updated**: 2025-04-10
## Categories & Tags
**Categories**: Uncategorized
**Tags**: None
## README
# squ_robot——室外低速自动导航车设计
#### 介绍
SQU 松灵自动驾驶项目
**定位**
Autoware.Ai的定位依赖于3D高精地图数据与NDT算法。并且使用从CAN信息和GNSS/IMU传感器中获得的里程计信息，通过Kalman滤波算法对定位结果进行补充。
**检测**
Autoware.AI通过相机和激光雷达设备结合3D高精地图数据进行检测。检测模块使用深度学习和传感器融合方法。
**跟踪与预测**
Autoware.AI利用卡尔曼滤波算法和三维高清地图数据提供的车道网络信息实现跟踪和预测。
**规划**
Autoware.AI的规划基于概率机器人与基于规则的系统，部分使用深度学习方法。
**控制**
Autoware.AI的控制通过速度和角度(也是曲率)的变化来定义车辆的运动。控制模块分为为两个Autoware软件堆栈(MPC和纯追踪)和车辆端接口(PID变体)。
#### 软件架构
workspace 为主工作空间
分为 navigation_ws(导航模块)、vision_ws(视觉模块)、voice_ws(语音模块)、simulation_ws(仿真模块)、calibration_ws(标定模块)
catkin build 每个单独工作空间，不要写入到BASHRC中，每次使用先source 下
## 导航
目前开源的导航框架有 百度的 Applo，autoware和move_base。move_base的框架资料较多，上手比较容易，适用于激光传感器在小车上实现自主导航，move_base相比其他两个框架，其本身没有融合有感知模块，需要自己添加模块和定义接口。这一部分中我们首先介绍自主导航小车的软件框架，随后介绍每一个部分的配置过程。
导航是机器人系统中最重要的模块之一，比如现在较为流行的服务型室内机器人，就是依赖于机器人导航来实现室内自主移动的，本章主要就是介绍仿真环境下的导航实现，主要内容有:
导航相关概念
导航实现:机器人建图(SLAM)、地图服务、定位、路径规划....以可视化操作为主。
导航消息:了解地图、里程计、雷达、摄像头等相关消息格式。
1.获得运动模型，tf树，激光雷达的数据。
2.调用ros进行建图，比如我目前采用的gmapping算法
3.定位
4.进行全局路径规划
5.局部路径规划
#### navigation
在ROS中机器人导航(Navigation)由多个功能包组合实现，ROS 中又称之为导航功能包集，关于导航模块，官方介绍如下:
一个二维导航堆栈，它接收来自里程计、传感器流和目标姿态的信息，并输出发送到移动底盘的安全速度命令。
更通俗的讲: 导航其实就是机器人自主的从 A 点移动到 B 点的过程。
ROS的二维导航功能包，简单来说，就是根据输入的里程计等传感器的信息流和机器人的全局位置，通过导航算法，计算得出安全可靠的机器人速度控制指令。但是，如何在特定的机器人上实现导航功能包的功能，却是一件较为复杂的工程。作为导航功能包使用的必要先决条件，机器人必须运行ROS，发布tf变换树，并发布使用ROS消息类型的传感器数据。同时，为了让机器人更好的完成导航任务，开发者还要根据机器人的外形尺寸和性能，配置导航功能包的一些参数。
**里程计:** 机器人相对出发点坐标系的位姿状态(X 坐标 Y 坐标 Z坐标以及朝向)。
# 二、总体方案设计
## 2.1 功能架构设计
Navigation导航框架如下图所示，其主要完成了全局路径规划器、局部路径规划器、全局代价地图、局部代价地图和行为决策（recovery_behaviors）的实现。
由上图可知，为实现自主导航功能，还需要地图、感知、定位、控制这四个模块。
## 2.2 地图模块
地图模块需要提供全局地图，以便全局路径规划器进行全局路径规划。
全局地图一般为二维占据栅格地图，可由图像(jpg等格式)表示，其中白色表示自由，黑色表示占据，灰色代表未知区域。
navigation框架提供了一个可选的地图模块`package`：**map_server**。
该`package`通过读取**地图配置文件**和**地图**，然后发布`/map`话题，以供全局规划器进行全局路径规划
**地图配置文件为`.yaml`文件，主要由6个参数描述：**
- 1.地图路径
- 2.地图分辨率（单位：米）
- 3.地图坐标系原点（x，y，theta）
- 4.是否翻转（黑色占据，白色自由 -> 黑色自由，白色占据）
- 5.占据阈值
- 6.自由阈值
```yaml
image: result.jpg
resolution: 0.20000
origin: [-61, -149.5, 0.000]
negate: 0
occupied_thresh: 0.65
free_thresh: 0.196
```
**地图**为`.jpg`文件，其中白色表示自由，黑色表示占据，一般需要提前让机器人用SLAM算法为工作区域建立二维栅格地图。
**地图模块实践操作：**
- 1.建图
**生成二维地图和yaml配置文件**
在实际工程中一般用3D SLAM算法先建立三维点云地图，然后设置**地图分辨率参数**，**占据/自由阈值**，将其投影为二维栅格地图，**同时输出三维点云地图中坐标原点对应二维栅格地图中的像素坐标**。
在使用地图模块时，需要分清楚机器人在地图坐标系下的坐标和定位坐标系下坐标的区别。
机器人的二维栅格地图坐标系为像素坐标系，图片的左顶点为原点，图片宽度为x，图片高度为y，定位数据通过地图分辨率转换为像素坐标。
- 2.定位
若在定位时，使用二维地图对应的三维点云地图进行定位，则二维地图坐标系与三维点云地图坐标系完成对应，**机器人在三维点云地图坐标系下的定位坐标可直接作为二维地图坐标系下坐标**。
若在定位时，未使用二维地图对应的三维点云地图进行定位，而是使用其它方式，如RTK、UWB等作为定位数据，则需要在建图的同时**记录三维点云地图中坐标原点对应的RTK、UWB值**和**原点对应二维栅格地图中的像素坐标**。机器人在二维地图坐标系下的坐标为，**实时的RTK、UWB值 减去 三维点云地图中坐标原点对应的RTK、UWB值**。
## 2.3 定位模块
定位方式有多种：①轮速里程计+imu；②激光+imu；**③GPS(RTK)+激光+imu**；④GPS(RTK)+激光+imu+轮速里程计；⑤相机+imu；⑥GPS(RTK)+相机+imu；⑦GPS(RTK)+相机+imu+轮速里程计；⑧GPS(RTK)+相机+激光+imu+轮速里程计；⑨...
以上不同定位方式的定位精度和成本不同，其中⑧的成本最高，定位效果最好；**③的应用最广泛，不依赖特定的平台，同时环境适应性很强。**其它定位方式与具体的场景有关，如室内环境、轮式平台等。
Navigation要求定位模块发出`/odom`的`Topic`名称，消息类型是`nav_msgs/Odometry`，包含机器人的位姿和速度。
GPS(RTK)设备不断的重新接收和计算世界坐标系中机器人的位姿，但连续两帧的GPS(RTK)坐标是不连续的，可能会发生跳变。这意味着在map坐标系中机器人的姿态再两帧之间会发生离散的跳变。
GPS(RTK)作为长期的全局坐标是很有用的，但是位姿的跳变极大影响运动规划和控制模块的性能，所以一般不直接使用GPS(RTK)坐标进行运动规划和控制。
机器人的里程计坐标是能够保证连续的，这意味着在里程计坐标系中的机器人的姿态总是平滑变化，没有跳变。因此适用于进行运动规划和控制。
激光和imu组合成激光里程计，然后通过GPS(RTK)进行校正是常用的定位方法。
**定位模块实践操作**：
1.让负责定位的同学发出里程计坐标，`Topic`的名称是`/odom`，消息类型是`nav_msgs/Odometry`，包含机器人的位姿和速度。
2.如果定位模块中有tf广播，需要注释掉，避免和自己写的tf冲突。
3.定位模块发出的里程计坐标的`topic`的`frame_id`要绑定到`odom`固定坐标系下（或者直接绑定到`map`下），不能绑定到`base_link`坐标系下。
## 2.4 感知模块
Navigation使用的sensro sources是激光雷达点云信息，话题名称是`/scan`，数据类型是`LaserScan`。
单线激光雷达可以室内满足要求，但无法在室外环境下为机器人提供足够的环境信息。通常使用多线激光雷达在室外环境下进行环境感知。
在Navigation中，采用多线激光雷达在室外环境下进行环境感知，需要将多线激光雷达数据格式转换为LaserScan数据类型。
推荐用`pointcloud_to_laserscan`包进行数据转换。该包是将多线激光雷达投影为二维数据，并不是只提取多线激光雷达中的一条线。
```yaml
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
```
**注意1： `target_frame: bigrobot2/base_footprint` 这个参数要求转换前的点云`frame_id`为`bigrobot2/base_footprint`，而该点云是直接读取速腾激光雷达点云`topic`，所以需要在速腾激光雷达点云的配置文件中直接修改发出点云`topic`的`frame_id`为`bigrobot2/base_footprint`。**
**注意2：rqt_graph显示pointcloud_to_laserscan没有订阅上points_raw，是因为没有节点订阅 scan。必须有节点订阅scan，pointcloud_to_laserscan节点才会订阅points_raw**
**感知模块实践操作**
- 1.修改速腾激光雷达驱动配置文件，修改雷达发出点云`topic`的`frame_id为bigrobot2/ lidar_cloud_origin`
- 2.修改`pointcloud_to_laserscan`的`launch`文件中的`target_frame`为已有tf 树上的`frame_id。`
注意是否需要`remap`相应的`topic`，可以通过`rosnode info pointcloud_to_laserscan_node`查看发出和接收的`topic`。
`remap`和`group`的规则：先`remap`，再`group`。
`remap`的规则：``
## 2.5 tf树
此模块非常重要！
**需要自己创建节点进行tf树的广播**，以便激光雷达点云数据、里程计的`topic`绑定对应的`frame_id`，必须确保这些`topic`绑定到对应的`frame_id`，否则Navigation无法正确运行。
Navigation是通过ROS中的tf 树来维护机器人中不同数据之间的坐标变换，如将激光雷达点云从激光雷达坐标系(base_laser)变换到车体坐标系(base_link)。
一个机器人通常tf 树的结构如下:
tf 树
每个坐标系都有一个父坐标系和任意子坐标系，map坐标系是odom坐标系的父，odom坐标系是base_link的父，每个坐标系只能有一个父类。
`map`下可以有多个定位分支`odom`，`gps(uwb)`，`fusion_localization`，这些分支与`map`原点可以设定是重合的或者由固定坐标转换，采用不同定位方式得到的定位结果。可以根据实际定位效果，把`base_link`绑定在不同的定位坐标系下。
base_link可以绑定在不同分支下
①`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只允许一个tf树存在。**
`Rviz`中的`fixed_frame`也是根据tf树构建的坐标树显示相应的`topic`。
#### 坐标系简析
- **1.世界坐标(earth)**
固定坐标系（fixed frame），用来描述某点在地球上的绝对位置，通常用经纬度和高度，或者WGS84表示。
引入世界坐标系的背景是：
1.可以将机器人在地图中的坐标通过earth直接转换为经纬高。
2.机器人建图时会因为地图过大(TB甚至PB数量级）无法一次性完成建图，需要将地图分为多个子图（自动驾驶地图就是由多个子图组成），可以利用经纬高完成不同的子图拼接。
如果只有一张地图，一般而言，世界坐标系原点与地图坐标系原点重合
- **2.map坐标系**
固定坐标系（fixed frame），地图坐标系
作为长期的全局参考是很有用的，但是跳变使得对于本地传感和执行器来说，其实是一个不好的参考坐标。
- **3.里程计坐标系(odom)**
odom 坐标系是一个固定坐标系（fixed frame）。
odom和map坐标系再机器人运动开始是重合的。但是，随着时间的推移是不重合的，而出现的偏差就是里程计的累积误差。
那map–>odom的tf怎么得到?就是在一些校正传感器合作校正的package比如amcl会给出一个位置估计（localization），这可以得到map–>base_link的tf，所以估计位置和里程计位置的偏差也就是odom与map的坐标系偏差。所以，如果你的odom计算没有错误，那么map–>odom的tf就是0。
- **4.车体坐标系(base_link)**
该base_link坐标刚性地连接到移动机器人基座。base_link可以安装在基座中的任意方位；对于每个硬件平台，在基座上的不同地方都会提供一个明显的参考点。
机器人本体坐标系，与机器人中心重合，当然有些机器人(PR 2)是base_footprint,其实是一个意思。
- **5.激光雷达的坐标系(base_laser)**
与激光雷达的安装点有关，其与base_link的tf为固定的。
## 2.6 规划模块
如果以播放bag包形式运行move_base，costmap会报错，costmap的tolorence时间超过1s阈值，此时需要设置时间为仿真时间。
首先启动bag包的播放，让其发布/clock话题，这样其他包就会从这个包的时间戳读时间
1 设置采用仿真时间
`rosparam set /use_sim_time true`
2 仿真时间来源为bag的时间戳
`rosbag play --pause --clock -k a.bag`
`--pause`：停止播放（按空格取消）
`--clock`：以bag时间为clock加，使用仿真时间
`-k, --keep-alive`：包的数据结束后，仍然提供时间服务，避免一些程序读时间产生bug
## 2.7 控制模块
# 三、Navigation配置
## 3.1 move_base.launch
**navigation实际使用的核心在于参数配置文件!**
Navigation由于参数配置文件不同，在实际使用过程中的导航效果也不同，本节将从launch文件与配置参数文件进行分析，move_base.launch为启动文件，启动的节点包含map_server、move_base以及pointcloud_to_laserscan。
该launch文件不包含定位节点，需要额外单独启动，
## 3.2 map_server节点配置参数
move_base.launch首先启动map_server节点，加载了`104andtest.yaml`的地图配置文件，并设置map_server发布的map的`frame_id`为`map`。
```xml
```
`104andtest.yaml`文件如下,主要是制定了地图文件和地图参数如原点,分辨率等.
```css
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节点配置参数
接着启动`pointcloud_to_laserscan`节点
```xml
target_frame: velodyne # 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
```
该节点需要订阅雷达点云信息,因为话题名称不匹配,需要remap订阅的话题名称``
根据tf关系决定是否调整输出的点云信息的`target_frame``,target_frame`决定了点云是挂在哪个tf坐标系下的。
`min_height`:为-0.4,若为0,则会把地面当成障碍物,出现一圈环形障碍物
`max_height`:为1.0,高于机器人本体的高度即可,低于该高度的点云都会被投影下来
`scan_time`:为发出点云的间隔时间,0.1s,对应10Hz
`range_min`:点云距离雷达最小的距离
`range_max`:点云距离雷达最大的距离
`use_inf`:设置为true,若为false,可能导致costmap地图更新会有问题,与costmap_2D的障碍层的`inf_is_valid`参数相关联,对于无穷远的点要求其值为inf
## 3.4 move_base节点配置参数
启动核心节点`move_base`
```xml
...
```
`controller_frequency`:以 Hz 为单位运行控制回路并向地盘发送速度命令的速率。
`controller_patiente`:在执行空间清理操作之前，控制器将等待多长时间（以秒为单位）而没有收到有效的控制。
`planner_patience`:在执行空间清理操作之前，规划器将在几秒钟内等待多长时间以尝试找到有效规划结果。
`planner_frequnency`:以 Hz 为单位运行全局规划循环的速率。如果频率设置为 0.0，则全局规划器将仅在收到新目标或本地规划器报告其路径被阻塞时运行。**该参数是Navigation 1.6.0 中的新功能**
## 3.4.1 base_global_planner节点配置参数
`global_planner/GlobalPlanner`参数为navigation自带的全局规划器,包含多种规划算法.
```bash
```
`global_planner_params.yaml`文件如下,其中`use_dijkstra`若为`false`,则采用`A*`算法.
```yaml
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节点配置参数
`teb_local_planner/TebLocalPlannerROS`为teb局部路径规划器,需要额外安装,可以参考[teb规划器未加链接]()
```bash
```
`teb_local_planner_params.yaml`文件如下
```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的参数可分为七类:
Trajectory:局部轨迹的参数,如轨迹长度,间隔时间,采样数量,是否允许倒退等
Robot:机器人最大速度,加速度和机器人外形尺寸
GoalTolerance:到达目标位置的误差参数
Obstacles:障碍物距离与膨胀参数[问题!!!与costmap中的障碍物膨胀有什么区别]()
Optimization:目标函数权重参数,优化求解器参数
Homotopy Class Planner:同伦类规划器,teb中两种类实例化局部规划器,teb与Homotopy Class Planner.后者是一系列规划器的组合.[参考](https://links.jianshu.com/go?to=https%3A%2F%2Fblog.csdn.net%2Fshoufei403%2Farticle%2Fdetails%2F104268893)
Recovery:类似navigation的recovery模块,陷入困境时脱困使用.
teb各个参数具体含义及调整可以参考[这篇文章]()
## 3.4.3 costmap节点配置参数
navigation采用costmap_2D作为地图层,通过加载不同参数生成全局地图和局部地图.
```bash
```
通过这些参数文件在ROS的参数服务器上注册了`global_costmap/xx`与`local_costmap/xx`等参数,move_base中会调用costmap_2D的API接口,根据这些参数实例化两个costmap对象,即global_costmap和local_costmap.
`costmap_common_params.yaml`文件,其中参数是全局地图和局部地图共有的参数.通过设置namespace参数`ns="global_costmap"`与`ns="local_costmap"`将global_costmap和local_costmap相同的参数一同初始化,避免改同一个参数,要修改两次.
当然也可以不用`costmap_common_params.yaml`文件,把这些参数全部放到`global_costmap_params.yaml`和`local_costmap_params.yaml`文件中.
`costmap_common_params.yaml`文件如下
```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的机器人尺寸为robot_radius,两者都包含static_layer和obstacle_layer,而inflation_layer层的配置参数则由global_costmap和local_costmap分别配置,其中障碍物层参数解析可见[添加链接]().
`global_costmap_params.yaml`文件如下,
```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
```
所有参数都是`global_costmap:`的二级子参数,类似`global_costmap/global_frame`,[参数解析添加链接]()
`local_costmap_params.yaml`文件如下
```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`文件如下
```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诚如代码中的note所说,还处于very eary stage,早期不稳定版本,**该参数不是必须的**!
costmap_converter是teb作者提供了一个插件用于转换代价地图，原始的代价地图是由栅格地图中单元格组成，用于表示障碍物，但单元格占用的计算资源较大，故采用插件将单元格转换成点，线，多边形表示。
可以[参考]这篇文章了解convert([https://blog.csdn.net/weixin_42621524/article/details/120721270](https://links.jianshu.com/go?to=https%3A%2F%2Fblog.csdn.net%2Fweixin_42621524%2Farticle%2Fdetails%2F120721270))
地图拼接，新地图坐标系原点，相对旧地图坐标系的变换矩阵。
```
把odom发布的cpp文件放到autoware中.
```
6。问题解决
注意pointcloud_to_laserscan是接收到cloud in的 topic后才会创建去订阅
remap不成功，在cloudin前面加了个/，不知道能否起作用
[https://blog.csdn.net/qq_23670601/article/details/88529739](https://links.jianshu.com/go?to=https%3A%2F%2Fblog.csdn.net%2Fqq_23670601%2Farticle%2Fdetails%2F88529739)
总结下来，涉及的关键技术有如下五点:
1. 全局地图
2. 自身定位
3. 路径规划
4. 运动控制
5. 环境感知
机器人导航实现与无人驾驶类似，关键技术也是由上述五点组成，只是无人驾驶是基于室外的，而我们当前介绍的机器人导航更多是基于室内的。
1.全局地图
在现实生活中，当我们需要实现导航时，可能会首先参考一张全局性质的地图，然后根据地图来确定自身的位置、目的地位置，并且也会根据地图显示来规划一条大致的路线.... 对于机器人导航而言，也是如此，在机器人导航中地图是一个重要的组成元素，当然如果要使用地图，首先需要绘制地图。关于地图建模技术不断涌现，这其中有一门称之为 SLAM 的理论脱颖而出:
SLAM(simultaneous localization and mapping),也称为CML (Concurrent Mapping and Localization), 即时定位与地图构建，或并发建图与定位。SLAM问题可以描述为: 机器人在未知环境中从一个未知位置开始移动,在移动过程中根据位置估计和地图进行自身定位，同时在自身定位的基础上建造增量式地图，以绘制出外部环境的完全地图。
在 ROS 中，较为常用的 SLAM 实现也比较多，比如: gmapping、hector_slam、cartographer、rgbdslam、ORB_SLAM ....
当然如果要完成 SLAM ，机器人必须要具备感知外界环境的能力，尤其是要具备获取周围环境深度信息的能力。感知的实现需要依赖于传感器，比如: 激光雷达、摄像头、RGB-D摄像头...
SLAM 可以用于地图生成，而生成的地图还需要被保存以待后续使用，在 ROS 中保存地图的功能包是 map_server
另外注意: SLAM 虽然是机器人导航的重要技术之一，但是 二者并不等价，确切的讲，SLAM 只是实现地图构建和即时定位。
2.自身定位
导航伊始和导航过程中，机器人都需要确定当前自身的位置，如果在室外，那么 GPS 是一个不错的选择，而如果室内、隧道、地下或一些特殊的屏蔽 GPS 信号的区域，由于 GPS 信号弱化甚至完全不可用，那么就必须另辟蹊径了，比如前面的 SLAM 就可以实现自身定位，除此之外，ROS 中还提供了一个用于定位的功能包: amcl
amcl(adaptiveMonteCarloLocalization)自适应的蒙特卡洛定位,是用于2D移动机器人的概率定位系统。它实现了自适应（或KLD采样）蒙特卡洛定位方法，该方法使用粒子过滤器根据已知地图跟踪机器人的姿态。
3.路径规划
导航就是机器人从A点运动至B点的过程，在这一过程中，机器人需要根据目标位置计算全局运动路线，并且在运动过程中，还需要时时根据出现的一些动态障碍物调整运动路线，直至到达目标点，该过程就称之为路径规划。在 ROS 中提供了 move_base 包来实现路径规则,该功能包主要由两大规划器组成:
全局路径规划(gloable_planner)
根据给定的目标点和全局地图实现总体的路径规划，使用 Dijkstra 或 A* 算法进行全局路径规划，计算最优路线，作为全局路线
本地时时规划(local_planner)
在实际导航过程中，机器人可能无法按照给定的全局最优路线运行，比如:机器人在运行中，可能会随时出现一定的障碍物... 本地规划的作用就是使用一定算法(Dynamic Window Approaches) 来实现障碍物的规避，并选取当前最优路径以尽量符合全局最优路径
全局路径规划与本地路径规划是相对的，全局路径规划侧重于全局、宏观实现，而本地路径规划侧重与当前、微观实现。
4.运动控制
导航功能包集假定它可以通过话题"cmd_vel"发布geometry_msgs/Twist类型的消息，这个消息基于机器人的基座坐标系，它传递的是运动命令。这意味着必须有一个节点订阅"cmd_vel"话题， 将该话题上的速度命令转换为电机命令并发送。
5.环境感知
感知周围环境信息，比如: 摄像头、激光雷达、编码器...，摄像头、激光雷达可以用于感知外界环境的深度信息，编码器可以感知电机的转速信息，进而可以获取速度信息并生成里程计信息。
在导航功能包集中，环境感知也是一重要模块实现，它为其他模块提供了支持。其他模块诸如: SLAM、amcl、move_base 都需要依赖于环境感知。
### 导航坐标系
1.简介
定位是导航中的重要实现之一，所谓定位，就是参考某个坐标系(比如:以机器人的出发点为原点创建坐标系)在该坐标系中标注机器人。定位原理看似简单，但是这个这个坐标系不是客观存在的，我们也无法以上帝视角确定机器人的位姿，定位实现需要依赖于机器人自身，机器人需要逆向推导参考系原点并计算坐标系相对关系，该过程实现常用方式有两种:
通过里程计定位:时时收集机器人的速度信息计算并发布机器人坐标系与父级参考系的相对关系。
通过传感器定位:通过传感器收集外界环境信息通过匹配计算并发布机器人坐标系与父级参考系的相对关系。
两种方式在导航中都会经常使用。
在ROS中，进行导航需要使用到的三个包是：
（1） move_base：根据参照的消息进行路径规划，使移动机器人到达指定的位置；
（2） gmapping：根据激光数据（或者深度数据模拟的激光数据）建立地图；
（3） amcl：根据已经有的地图进行定位。
在总体框架图中可以看到，move_base提供了ROS导航的配置、运行、交互接口，它主要包括两个部分：
（1） 全局路径规划（global planner）：根据给定的目标位置进行总体路径的规划.
在ROS的导航中，首先会通过全局路径规划，计算出机器人到目标位置的全局路线。这一功能是navfn这个包实现的。
navfn通过Dijkstra或A*最优路径的算法，计算costmap上的最小花费路径，作为机器人的全局路线。
（2） 本地实时规划（local planner）：根据附近的障碍物进行躲避路线规划。
本地的实时规划是利用base_local_planner包实现的。该包使用Trajectory Rollout 和Dynamic Window approaches算法计算机器人每个周期内应该行驶的速度和角度（dx，dy，dtheta velocities）。
base_local_planner这个包通过地图数据，通过算法搜索到达目标的多条路经，利用一些评价标准（是否会撞击障碍物，所需要的时间等等）选取最优的路径，并且计算所需要的实时速度和角度。
其中，Trajectory Rollout 和Dynamic Window approaches算法的主要思路如下：
（1） 采样机器人当前的状态（dx,dy,dtheta）；
（2） 针对每个采样的速度，计算机器人以该速度行驶一段时间后的状态，得出一条行驶的路线。
（3） 利用一些评价标准为多条路线打分。
（4） 根据打分，选择最优路径。
（5） 重复上面过程。
## catorgrapher
cartographer是google开发的实时室内SLAM项目，cartographer采用基于google自家开发的ceres非线性优化的方法，cartographer的量点在于代码规范与工程化，非常适合于商业应用和再开发。并且cartographer基于submap子图构建全局地图的思想，能有效的避免建图过程中环境中移动物体的干扰。并且cartographer支持多传感器数据（odometry、IMU、LaserScan等）建图，支持2D_SLAM和3D_SLAM建图。
cartographer采用的是主流的SLAM框架，也就是特征提取、闭环检测、后端优化的三段式。由一定数量的LaserScan组成一个submap子图，一系列的submap子图构成了全局地图。用LaserScan构建submap的短时间过程累计误差不大，但是用submap构建全局地图的长时间过程就会存在很大的累计误差，所以需要利用闭环检测来修正这些submap的位置，闭环检测的基本单元是submap，闭环检测采用scan_match策略。cartographer的重点内容就是融合多传感器数据（odometry、IMU、LaserScan等）的submap子图创建以及用于闭环检测的scan_match策略的实现。
我们直接参考google-cartographer官方教程安装就行，官方教程分为cartographer和cartographer_ros，其实cartographer就是核心算法层、cartographer_ros是核心算法层的ros调用层。
1）安装编译工具
我来编译cartographer_ros，我们需要用到wsool和rosdep。为了加快编译，我们使用ninja工具进行编译。
```bash
sudo apt-get update
sudo apt-get install -y python-wstool python-rosdep ninja-build
```
（2）创建存放cartographer_ros的专门工作空间
```bash
mkdir catkin_ws_carto
cd catkin_ws_carto
wstool init src
wstool merge -t src https://raw.githubusercontent.com/googlecartographer/cartographer_ros/master/cartographer_ros.rosinstall
wstool update -t src
```
（3）安装依赖项
安装cartographer_ros的依赖项proto3、deb包等。如果执行sudo rosdep init
报错，可以直接忽略。
```bash
src/cartographer/scripts/install_proto3.sh
sudo rosdep init
rosdep update
rosdep install --from-paths src --ignore-src --rosdistro=${ROS_DISTRO} -y
```
（4）编译和安装
上面的配置和依赖都完成后，就可以开始编译和安装cartographer_ros整个项目工程了。
```bash
catkin_make_isolated --install --use-ninja
```
特别提醒，以后对cartographer_ros中的配置文件或源码有改动时，都需要执行这个编译命令使修改生效。
#### cartographer_ros使用
cartographer_ros整体代码结构分析：
最顶层的是cartographer_ros，作为ros接口调用层，通过调用cartographer核心算法，订阅多传感器数据（/scan、/imu、/odom等），并发布地图、机器人位置信息（/map、/tf等）；其次是cartographer，作为SLAM算法的核心实现，特征提取、子图构建、闭环检测、全局优化都在这里实现，其中优化过程需要调用ceres-solver非线性优化库；最后是ceres-solver，是非线性优化库，用于求解SLAM中的优化问题。
## LeGO-LOAM
实际场景与机器人（ROS）地图的对应关系：
我们来看一下下面这副图片，这是一个机器人在实际环境中绘制的二维地图，在图片的左下角是地图原点（map所在的位置），在原点上已经标明了这个地图的三维坐标轴，其中红色的代表x轴方向，绿色代表y轴方向。
#### [](https://img-blog.csdnimg.cn/20200226153325912.png?x-oss-process=image/watermark,type_ZmFuZ3poZW5naGVpdGk,shadow_10,text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3o4MjQwNzQ5ODl5,size_16,color_FFFFFF,t_70)
在ROS的地图中，地图同样是以像素标记的，每一个像素代表0.05m，也就是一个像素是5厘米。比如下图上的星星，他的位置在像素层面上的坐标为：x：400像素，y：150像素；那么这个机器人实际的横坐标就 应该是：400*0.05=20米，纵坐标是150*0.05=7.5m。再换句话说，如果一个地图的场合宽都是800像素，那么它能够代表的实际长和宽度为800*0.05=40米。 [](https://img-blog.csdnimg.cn/20200226153923648.png?x-oss-process=image/watermark,type_ZmFuZ3poZW5naGVpdGk,shadow_10,text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3o4MjQwNzQ5ODl5,size_16,color_FFFFFF,t_70) 当然，有时候地图的原点并不一定在左下角，它的原点可以在地图的任意一个地方。比如说下面这张图： [](https://img-blog.csdnimg.cn/20200226160029843.png?x-oss-process=image/watermark,type_ZmFuZ3poZW5naGVpdGk,shadow_10,text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3o4MjQwNzQ5ODl5,size_16,color_FFFFFF,t_70) 这张图的地图原点就跑到了地图图片里，那么假如机器人在原点左下角的位置上，那么机器人的X和Y坐标就会为负值。 在ROS中，地图的类型是nav_msgs/OccupancyGrid，每个像素代表的长度以及地图的原点都被定义在了消息结构体之中： std_msgs/Header header nav_msgs/MapMetaData info time map_load_time float32 resolution 分辨率 geometry_msgs/Pose origin原点坐标 ...... geometry_msgs/Quaternion orientation ...... int8[] data 地图像素信息
## 2. 地图坐标系与Python canvas鼠标点击位置之间的对应关系：
我们使用python编写客户端的时候（Java于此类似），地图是要被绘制到canvas，也就是虚拟画布上面。**虚拟画布是以其左上角为坐标原点（0，0）**（如下图）。我们举一个例子，现在有一个800x800的地图，我们需要使用鼠标或触摸选择了APP中地图的左上角，那么显然的，在APP中，我们得到的点击位置是（0，0）。 [](https://img-blog.csdnimg.cn/20200226161543620.png?x-oss-process=image/watermark,type_ZmFuZ3poZW5naGVpdGk,shadow_10,text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3o4MjQwNzQ5ODl5,size_16,color_FFFFFF,t_70) 但是如果我们把这个数据输入ROS的地图中，可以看到，在ROS中，（0，0）点位置对应的是左下角。所以造成的问题就会如下图： [](https://img-blog.csdnimg.cn/2020022616184530.png?x-oss-process=image/watermark,type_ZmFuZ3poZW5naGVpdGk,shadow_10,text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3o4MjQwNzQ5ODl5,size_16,color_FFFFFF,t_70) 这个问题解决的方法很简单，就是在用户端软件（如APP）中，将点击的点做一些处理（坐标系变换），再将其数据传输到机器人上即可。
## 3. Python Canvas与机器人内ROS之间的数据传输（http）
一般的机器人与客户端的通讯方式采用http方式传输信息，http协议的详细内容请参见新安浅滩的博文： https://blog.csdn.net/hu694028833/article/details/80862695 整个机器人与Python之间地图与坐标系的传递过程中，Python要向机器人获取三个信息（已知地图每个像素为0.05米）： 1）机器人中当前地图的图片 2）地图图片的右下角在机器人坐标系中所处的位置（**或者说地图的原点在地图图片中所处的像素位置**） 3）机器人相对于**地图坐标系原点**的位置 由于传输数据量很大，所以Python客户端向机器人请求地图的频率不能太高（基本为数秒一次，或只在加载时请求）。而因为机器人在时刻移动，机器人相对地图坐标原点的位置我们必须要频繁的请求。所以在数据通讯上，我们将以上三个信息分为两类： 1）当前机器人内部地图图片以及地图原点信息（加载地图，以及绘制地图时请求） 2）机器人相对地图原点的位置（数百毫秒，轮询请求） 例如，Python在机器人加载地图后，向机器人发送一个名为： GET /robot_map HTTP/1.1的消息 机器人向Python反馈jpeg压缩格式的地图图片 Python程序向机器人发送一个名为：GET /robot_map_origin HTTP/1.1的消息 机器人反馈地图的原点坐标： { "x": 1.0 "y":1.0 } 这样就实现了地图与地图坐标原点的传输。 同时，Python每间隔1000ms向机器人发送：GET /robot_state HTTP/1.1的消息 机器人反馈机器人实时位置： { “navigation_state ”: { “x ”:1.23 //注意：这里指的是机器人相对地图原点的实际位置，带小数点的。 “y ” :2.34 } } 这样就获取到了机器人的位置。
## 4. Python上显示机器人位置以及正确发送点击坐标
现在我们得到了地图，地图原点，机器人在实际环境中相对于地图原点的位置。那么我们就要在地图中正确显示出机器人所在位置，具体的方法如下：
1）采用比例变换，将机器人相对于原点的实际位置坐标转换为像素坐标
2）在已知地图原点在图片中像素的前提下，将机器人相对于地图原点的像素坐标，映射到相对地图图片的像素中坐标。
3）在地图图片上，按照机器人的像素坐标叠加机器人图片。
4）显示如果我们想向机器人输出一个鼠标或触摸点击的坐标，我们要执行以下步骤：
1）获得鼠标点击的位置像素坐标（x,y）
2）对Python Canvas的坐标进行Y轴反转，获得临时像素坐标（例如地图高度为800，点击了（100，700）位置，那么发送的Y坐标为800-700=100）
3）在获取地图原点的基础上，利用坐标系变换，将临时像素坐标转换为相对于原点的像素坐标
4）根据像素-实际尺寸的比例变换，将像素坐标变为实际坐标
5）通过http的POST结合json数据格式将点击数据下发至机器人。 下一小节我们将介绍机器人图片与原点坐标系的变换方法。
### 多目标点导航及任务调度
通过前面的学习，我们已经可以通过点击地图的方式来命令机器人运动到目标点。其实，ros-navigation导航框架就是为我们提供了一个最基本的机器人自动导航接口，即单点导航。然而，在实际的机器人应用中，机器人往往要完成复杂的任务，这些复杂的任务都是由一个个基本的任务组合而成的。一般的，机器人通过状态机的形式将一个个基本任务组合在一起来进行复杂任务的调度实现。
4.1.状态机
这里我们只讨论有限状态机,也称为FSM(Finite State Machine)，其在任意时刻都处于有限状态集合中的某一状态。当其获得一个输入条件时，将从当前状态转换到另一个状态，或者仍然保持在当前状态。任何一个FSM都可以用状态转换图来描述，图中的节点表示FSM中的一个状态，有向加权边表示输入条件时状态的变化。如图49，以一个上班族的生活场景来举例说明状态机的状态转换图。矩形框表示FSM中的一个状态，有向边表示在输入条件下的状态转换过程。
## 4.2.多目标点巡航
机器人多目标点巡航，特别是按特定巡逻路径进行巡航是很实用的功能。这里将利用前面学到的ros-navigation单点导航、状态机、状态机任务调度的知识。我们来编写一个应用功能包实现机器人多目标点巡航。
到这里，我们慢慢清楚了miiboo机器人编程的框架思路，我们将传感器相关的底层驱动包放在~/catkin_ws/工作空间统一管理，将基于google-cartographer的SLAM建图程序包放在~/catkin_ws_carto/工作空间统一管理，将基于ros-navigation的导航程序包放在~/catkin_ws_nav/工作空间统一管理，将高层应用功能包放在~/catkin_ws_apps/工作空间统一管理。miiboo机器人编程的框架思路，如图50。
