hector是一种结合了鲁棒性较好的扫描匹方法2D_SLAM方法和使用惯性传感系统的导航技术。传感器的要求较高，高更新频率小测量噪声的激光扫描仪，不需要里程计。使空中无人机与地面小车在不平坦区域运行存在运用的可能性。作者利用现代激光雷达的高更新率和低距离测量噪声，通过扫描匹配实时地对机器人运动进行估计。所以当只有低更新率的激光传感器时，即便测距估计很精确，对该系统都会出现一定的问题。

gmapping是一种基于粒子滤波的激光SLAM算法，它已经集成在ROS中，是移动机器人中使用最多的SLAM算法。基于粒子滤波的算法用许多加权粒子表示路径的后验概率，每个粒子都给出一个重要性因子。但是，它们通常需要大量的粒子才能获得比较好的的结果，从而增加该算法的的计算复杂性。此外，与PF重采样过程相关的粒子退化耗尽问题也降低了算法的准确性。

karto是基于图优化的SLAM算法，用高度优化和非迭代cholesky矩阵进行稀疏系统解耦作为解。图优化方法利用图的均值表示地图，每个节点表示机器人轨迹的一个位置点和传感器测量数据集，箭头的指向的连接表示连续机器人位置点的运动，每个新节点加入，地图就会依据空间中的节点箭头的约束进行计算更新。路标landmark越多,内存需求越大,然而图优化方式相比其他方法在大环境下制图优势更大。

cartographer是google开发的实时室内SLAM项目，cartographer采用基于google自家开发的ceres非线性优化的方法，cartographer的量点在于代码规范与工程化，非常适合于商业应用和再开发。并且cartographer基于submap子图构建全局地图的思想，能有效的避免建图过程中环境中移动物体的干扰。并且cartographer支持多传感器数据（odometry、IMU、LaserScan等）建图，支持2D_SLAM和3D_SLAM建图。所以，我果断采用cartographer来建图，我的树莓派3主板跑cartographer实时建图是十分的流畅，这一点很欣慰^_^

2.1.google-cartographer建图算法原理分析

cartographer采用的是主流的SLAM框架，也就是特征提取、闭环检测、后端优化的三段式。由一定数量的LaserScan组成一个submap子图，一系列的submap子图构成了全局地图。用LaserScan构建submap的短时间过程累计误差不大，但是用submap构建全局地图的长时间过程就会存在很大的累计误差，所以需要利用闭环检测来修正这些submap的位置，闭环检测的基本单元是submap，闭环检测采用scan_match策略。cartographer的重点内容就是融合多传感器数据（odometry、IMU、LaserScan等）的submap子图创建以及用于闭环检测的scan_match策略的实现。

（图18）cartographer算法系统框图

2.2.cartographer_ros安装

我们直接参考google-cartographer官方教程安装就行，官方教程分为cartographer和cartographer_ros，其实cartographer就是核心算法层、cartographer_ros是核心算法层的ros调用层。官方教程如下：

Cartographer — Cartographer documentation

Cartographer ROS Integration — Cartographer ROS documentation

直接按照第二个链接cartographer_ros的安装教程，就可将cartographer_ros、cartographer、以及各种依赖都安装了。不过特别说明一点，为了解决从官网下载ceres-solver速度慢的问题，我将ceres-solver的下载地址换到了github源；我需要将官方教程中生成的src/.rosinstall替换成了自己的内容，如图19。其余安装过程和官方教程一模一样。

（1）安装编译工具

我来编译cartographer_ros，我们需要用到wsool和rosdep。为了加快编译，我们使用ninja工具进行编译。


sudo apt-get update
sudo apt-get install -y python-wstool python-rosdep ninja-build

（2）创建存放cartographer_ros的专门工作空间


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

特别说明，在执行wstool update -t src之前，需要将src/.rosinstall文件修改成以下内容,以解决ceres-solver下载不了的问题，如图19。

（图19）我修改后的src/.rosinstall文件内容

（3）安装依赖项

安装cartographer_ros的依赖项proto3、deb包等。如果执行sudo rosdep init报错，可以直接忽略。


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整个项目工程了。

catkin_make_isolated --install --use-ninja

特别提醒，以后对cartographer_ros中的配置文件或源码有改动时，都需要执行这个编译命令使修改生效。

2.3.cartographer_ros使用

cartographer_ros整体代码结构分析：

最顶层的是cartographer_ros，作为rosj接口调用层，通过调用cartographer核心算法，订阅多传感器数据（/scan、/imu、/odom等），并发布地图、机器人位置信息（/map、/tf等）；其次是cartographer，作为SLAM算法的核心实现，特征提取、子图构建、闭环检测、全局优化都在这里实现，其中优化过程需要调用ceres-solver非线性优化库；最后是ceres-solver，是非线性优化库，用于求解SLAM中的优化问题。

（图20）cartographer_ros整体代码结构

在miiboo机器人上用cartographer_ros多传感器建图进行配置：

经过前面对cartographer_ros进行安装后，我们肯定迫不及待想在实际的miiboo机器人上使用cartographer_ros进行SLAM建图了。为了最大限度的提高SLAM建图的性能，我们的miiboo机器人提供了激光雷达、IMU、轮式里程计（/scan、/imu、/odom）这三种传感器的数据，所以我们需要先将cartographer_ros配置成对应的工作模式。

cartographer算法是一个非常通用和适应不同平台的开放框架算法，所以支持多种配置与工作模式。我们就来看看cartographer_ros如何进行配置。配置文件由*.lua书写被放在路径cartographer_ros/configuration_files/，我们需要建立一个我们自己的配置文件，取名就叫miiboo_mapbuild.lua吧，具体内容如图21。由于我们的miiboo机器人采用激光雷达、IMU、轮式里程计三种传感器融合建图，所以以下参数一定要设置正确：

参数tracking_frame设置为imu_link，因为我们使用/imu的数据；

参数published_frame设置为odom，因为我们使用/odom的数据；

参数provide_odom_frame设置为false，因为我们使用外部/odom，所以这里不需要内部提供；

参数use_odometry设置为true，因为我们使用外部/odom的数据；

参数use_imu_data设置为true，因为我们使用/imu的数据；

参数imu_gravity_time_constant设置为10，这个是IMU的重力加速度常数。

其余参数根据需要自行调整，由于cartographer是发展很迅速的算法，所以很多代码和文档一直在更新，所以参考官方文档来解读这些参数的含义是最好的选择，官方文档连接地址我贴在下面了。

Cartographer ROS Integration — Cartographer ROS documentation

（图21）我们miiboo机器人的建图配置文件miiboo_mapbuild.lua

然后需要配置*.launch文件，我们给miiboo机器人建立启动文件取名叫miiboo_mapbuild.launch，存放路径在cartographer_ros/launch/里面，具体内容如图22。

不难发现launch文件中包含三个node启动项，即urdf模型启动项、cartographer_node启动项、cartographer_occupancy_grid_node启动项。

第一个启动项是启动urdf模型，这个接口是提供给那些只使用cartographer单独建图的应用场景，由于我们miiboo机器人建立完地图后还需要继续进行自动导航任务，所以我们使用miiboo底盘提供的urdf模型，而不使用这里的urdf模型，所以这个启动项被注释掉了，这样建图和导航就更容易管理。

第二个启动项是启动cartographer_node建图节点，这个是SLAM建图主节点，我们建立的配置miiboo_mapbuild.lua将在这里被载入，同时这里可以对建图输入数据scan、imu、odom的topic名称做重映射。

第三个启动项是启动cartographer_occupancy_grid_node地图格式转换节点，由于cartographer_node建图节点提供的地图是submapList格式的，需要转换成GridMap格式才能在ROS中显示和使用。这里面有两个可配参数，resolution用来设置GridMap地图的分辨率，publish_period_sec用来设置GridMap地图发布的频率。

（图22）我们miiboo机器人的建图启动文件miiboo_mapbuild.launch

配置参数修改好后，不要忘了再编译一次整个catkin_ws_carto工作空间，切换到catkin_ws_carto目录，执行下面的编译命令。

catkin_make_isolated --install --use-ninja

启动cartographer_ros建图：

要在miiboo机器人上，启动cartographer_ros建图，分为这几个步骤：启动机器人上的各个传感器、启动cartographer_ros、在PC端启动键盘控制机器人运动并启动rviz观察地图（或者在Android手机端用miiboo机器人APP控制机器人运动和观察地图）。

首先，启动机器人上的各个传感器，为了操作方便，我已经将要启动的传感器都写入miiboo_bringup/launch/miiboo_all_sensor.launch这个启动文件了，文件内容如图23。这个启动文件包含机器人urdf启动项、miiboo底盘启动项、激光雷达启动项、IMU启动项、摄像头启动项、广播IP启动项。

（图23）各个传感器启动文件miiboo_all_sensor.launch

打开终端，通过下面的命令直接启动就行了。


source ~/catkin_ws/devel/setup.bash 
roslaunch miiboo_bringup miiboo_all_sensor.launch

然后，启动cartographer_ros，由于前面已经做好了相应的配置，所以直接使用命令启动就行了。


source ~/catkin_ws_carto/install_isolated/setup.bash
roslaunch cartographer_ros miiboo_mapbuild.launch

这里给个小提示，为了查看cartographer_ros建图算法有没有正常开始工作，我们可以用rosrun rqt_tf_tree rqt_tf_tree查看整个tf树的结构，正常的tf树如图24。map->odom之间的关系由cartographer建图节点提供，odom->base_footprint之间的关系由miiboo底盘的轮式里程计提供，base_footprint->imu_link和base_link和base_laser_link之间的关系由miiboo机器人的urdf模型提供。从tf树不难看出整个建图过程中机器人定位的实现原理，cartographer建图节点通过维护map->odom之间的关系最终实现全局定位，miiboo底盘的轮式里程计通过维护odom->base_footprint之间的关系来实现局部定位，传感器之间的安装关系由urdf模型提供，这个静态关系主要用于多传感器数据融合。

（图24）cartographer运行时正常的tf树

最后，在PC端启动键盘控制机器人运动并启动rviz观察地图（或者在Android手机端用miiboo机器人APP控制机器人运动和观察地图）。如果用PC端控制和观察，启动命令如下。

在PC端打开一个新终端，运行rviz启动命令。

rosrun rviz rviz

在rviz窗口中添加订阅/map，就可以看到建图效果了，如图25。

（图25）在PC端用rviz观察地图

在PC端再打开一个新终端，运行键盘控制启动命令。

rosrun teleop_twist_keyboard teleop_twist_keyboard.py

在该终端下，用键盘就可以控制机器人前进、后退、左转、右转了。

如果是在Android手机端用miiboo机器人APP控制机器人运动和观察地图，直接就能使用，如图26。

（图26）在Android手机端用miiboo机器人APP控制机器人运动和观察地图

保存cartographer_ros建图结果：

当我们在房间里面扫描一圈，地图建立的差不多了，就可以将建图结果保存下来了，cartographer_ros提供了将建图结果保存为*.pbstream专门的方法，其实就是一条命令。


source ~/catkin_ws_carto/install_isolated/setup.bash
rosservice call /write_state  /home/ubuntu/map/carto_map.pbstream

其实就是调用cartographer_ros提供的叫/write_state这个名字的服务，服务传入参数/home/ubuntu/map/carto_map.pbstream为地图的保存路径。保存成功后，会返回相应的状态信息，如图27。

（图27）调用/write_state服务保存建图结果

地图格式转换：

由于用cartographer_ros提供的/write_state方法保存的地图是*.pbstream的格式，而要在后续的自主导航中使用这个地图，我们需要将其转换为ROS中通用的GridMap格式。其实很简单，cartographer_ros已经跟我们提供了cartographer_pbstream_to_ros_map这个节点用于转换的实现。所以，我们只需要写一个启动文件启动这个节点就行了，我给这个启动文件取名miiboo_pbstream2rosmap.launch，存放路径是cartographer_ros/launch/，启动文件的内容如图28。在使用这个启动文件进行启动时，需要从外部传入两个参数，参数pbstream_filename为待转换的*.pbstream文件路径，参数map_filestem为转换后存放结果的文件路径。

（图28）pbstream转GridMap启动文件

配置参数修改好后，不要忘了再编译一次整个catkin_ws_carto工作空间，切换到catkin_ws_carto目录，执行下面的编译命令。

catkin_make_isolated --install --use-ninja

最后，就可以打开终端，使用启动这个启动文件，对地图格式进行转换了，命令如下。

roslaunch cartographer_ros miiboo_pbstream2rosmap.launch pbstream_filename:=/home/ubuntu/map/carto_map.pbstream map_filestem:=/home/ubuntu/map/carto_map

保存结束后，节点会自动退出，这时我们可以得到转换后的地图，转换后的GridMap地图由*.pgm和*.yaml两部分构成，这时标准的ROS格式地图，可以被ROS导航框架中的map_server节点直接调用，转换后的地图结果如图29。

（图29）地图格式转换后的结果

后记

如果大家对博文的相关类容感兴趣，或有什么技术疑问，欢迎加QQ技术交流群（117698356）

参考文献

[1] 张虎，机器人SLAM导航核心技术与实战[M]. 机械工业出版社，2022.

购书链接：https://item.jd.com/13041503.html

下载更多资料：www.xiihoo.com

QQ技术讨论群： 117698356

B站视频教程：https://space.bilibili.com/66815220

Github源码：https://github.com/xiihoo/Books_Robot_SLAM_Navigation

Gitee源码（国内访问速度快）：https://gitee.com/xiihoo-robot/Books_Robot_SLAM_Navigation

序

前言

编程基础篇

第1章 ROS入门必备知识

1.1 ROS简介 2

1.1.1 ROS的性能特色 2

1.1.2 ROS的发行版本 3

1.1.3 ROS的学习方法 3

1.2 ROS开发环境的搭建 3

1.2.1 ROS的安装 4

1.2.2 ROS文件的组织方式 4

1.2.3 ROS网络通信配置 5

1.2.4 集成开发工具 5

1.3 ROS系统架构 5

1.3.1 从计算图视角理解ROS架构 6

1.3.2 从文件系统视角理解ROS架构 7

1.3.3 从开源社区视角理解ROS架构 8

1.4 ROS调试工具 8

1.4.1 命令行工具 9

1.4.2 可视化工具 9

1.5 ROS节点通信 10

1.5.1 话题通信方式 12

1.5.2 服务通信方式 15

1.5.3 动作通信方式 19

1.6 ROS的其他重要概念 25

1.7 ROS 2.0展望 28

1.8 本章小结 28

第2章 C++编程范式

2.1 C++工程的组织结构 29

2.1.1 C++工程的一般组织结构 29

2.1.2 C++工程在机器人中的组织结构 29

2.2 C++代码的编译方法 30

2.2.1 使用g++编译代码 31

2.2.2 使用make编译代码 32

2.2.3 使用CMake编译代码 32

2.3 C++编程风格指南 33

2.4 本章小结 34

第3章 OpenCV图像处理

3.1 认识图像数据 35

3.1.1 获取图像数据 35

3.1.2 访问图像数据 36

3.2 图像滤波 37

3.2.1 线性滤波 37

3.2.2 非线性滤波 38

3.2.3 形态学滤波 39

3.3 图像变换 40

3.3.1 射影变换 40

3.3.2 霍夫变换 42

3.3.3 边缘检测 42

3.3.4 直方图均衡 43

3.4 图像特征点提取 44

3.4.1 SIFT特征点 44

3.4.2 SURF特征点 50

3.4.3 ORB特征点 52

3.5 本章小结 54

硬件基础篇

第4章机器人传感器

4.1 惯性测量单元 56

4.1.1 工作原理 56

4.1.2 原始数据采集 60

4.1.3 参数标定 65

4.1.4 数据滤波 73

4.1.5 姿态融合 75

4.2 激光雷达 91

4.2.1 工作原理 92

4.2.2 性能参数 94

4.2.3 数据处理 96

4.3 相机 100

4.3.1 单目相机 101

4.3.2 双目相机 107

4.3.3 RGB-D相机 109

4.4 带编码器的减速电机 111

4.4.1 电机 111

4.4.2 电机驱动电路 112

4.4.3 电机控制主板 113

4.4.4 轮式里程计 117

4.5 本章小结 118

第5章机器人主机

5.1 X86与ARM主机对比 119

5.2 ARM主机树莓派3B+ 120

5.2.1 安装Ubuntu MATE 18.04 120

5.2.2 安装ROS melodic 122

5.2.3 装机软件与系统设置 122

5.3 ARM主机RK3399 127

5.4 ARM主机Jetson-tx2 128

5.5 分布式架构主机 129

5.5.1 ROS网络通信 130

5.5.2 机器人程序的远程开发 130

5.6 本章小结 131

第6章机器人底盘

6.1 底盘运动学模型 132

6.1.1 两轮差速模型 132

6.1.2 四轮差速模型 136

6.1.3 阿克曼模型 140

6.1.4 全向模型 144

6.1.5 其他模型 148

6.2 底盘性能指标 148

6.2.1 载重能力 148

6.2.2 动力性能 148

6.2.3 控制精度 150

6.2.4 里程计精度 150

6.3 典型机器人底盘搭建 151

6.3.1 底盘运动学模型选择 152

6.3.2 传感器选择 152

6.3.3 主机选择 153

6.4 本章小结 155

SLAM篇

第7章 SLAM中的数学基础

7.1 SLAM发展简史 158

7.1.1 数据关联、收敛和一致性 160

7.1.2 SLAM的基本理论 161

7.2 SLAM中的概率理论 163

7.2.1 状态估计问题 164

7.2.2 概率运动模型 166

7.2.3 概率观测模型 171

7.2.4 概率图模型 173

7.3 估计理论 182

7.3.1 估计量的性质 182

7.3.2 估计量的构建 183

7.3.3 各估计量对比 190

7.4 基于贝叶斯网络的状态估计 193

7.4.1 贝叶斯估计 194

7.4.2 参数化实现 196

7.4.3 非参数化实现 202

7.5 基于因子图的状态估计 206

7.5.1 非线性最小二乘估计 206

7.5.2 直接求解方法 206

7.5.3 优化方法 208

7.5.4 各优化方法对比 218

7.5.5 常用优化工具 219

7.6 典型SLAM算法 221

7.7 本章小结 221

第8章激光SLAM系统

8.1 Gmapping算法 223

8.1.1 原理分析 223

8.1.2 源码解读 228

8.1.3 安装与运行 233

8.2 Cartographer算法 240

8.2.1 原理分析 240

8.2.2 源码解读 247

8.2.3 安装与运行 258

8.3 LOAM算法 266

8.3.1 原理分析 266

8.3.2 源码解读 267

8.3.3 安装与运行 270

8.4 本章小结 270

第9章视觉SLAM系统

9.1 ORB-SLAM2算法 274

9.1.1 原理分析 274

9.1.2 源码解读 310

9.1.3 安装与运行 319

9.1.4 拓展 327

9.2 LSD-SLAM算法 329

9.2.1 原理分析 329

9.2.2 源码解读 334

9.2.3 安装与运行 337

9.3 SVO算法 338

9.3.1 原理分析 338

9.3.2 源码解读 341

9.4 本章小结 341

第10章其他SLAM系统

10.1 RTABMAP算法 344

10.1.1 原理分析 344

10.1.2 源码解读 351

10.1.3 安装与运行 357

10.2 VINS算法 362

10.2.1 原理分析 364

10.2.2 源码解读 373

10.2.3 安装与运行 376

10.3 机器学习与SLAM 379

10.3.1 机器学习 379

10.3.2 CNN-SLAM算法 411

10.3.3 DeepVO算法 413

10.4 本章小结 414

自主导航篇

第11章自主导航中的数学基础

11.1 自主导航 418

11.2 环境感知 420

11.2.1 实时定位 420

11.2.2 环境建模 421

11.2.3 语义理解 422

11.3 路径规划 422

11.3.1 常见的路径规划算法 423

11.3.2 带约束的路径规划算法 430

11.3.3 覆盖的路径规划算法 434

11.4 运动控制 435

11.4.1 基于PID的运动控制 437

11.4.2 基于MPC的运动控制 438

11.4.3 基于强化学习的运动控制 441

11.5 强化学习与自主导航 442

11.5.1 强化学习 443

11.5.2 基于强化学习的自主导航 465

11.6 本章小结 467

第12章典型自主导航系统

12.1 ros-navigation导航系统 470

12.1.1 原理分析 470

12.1.2 源码解读 475

12.1.3 安装与运行 479

12.1.4 路径规划改进 492

12.1.5 环境探索 496

12.2 riskrrt导航系统 498

12.3 autoware导航系统 499

12.4 导航系统面临的一些挑战 500

12.5 本章小结 500

第13章机器人SLAM导航综合实战

13.1 运行机器人上的传感器 502

13.1.1 运行底盘的ROS驱动 503

13.1.2 运行激光雷达的ROS驱动 503

13.1.3 运行IMU的ROS驱动 504

13.1.4 运行相机的ROS驱动 504

13.1.5 运行底盘的urdf模型 505

13.1.6 传感器一键启动 506

13.2 运行SLAM建图功能 506

13.2.1 运行激光SLAM建图功能 507

13.2.2 运行视觉SLAM建图功能 508

13.2.3 运行激光与视觉联合建图功能 508

13.3 运行自主导航 509

13.4 基于自主导航的应用 510

13.5 本章小结 511

附录A Linux与SLAM性能优化的探讨

附录B 习题

声明：本文内容由网友自发贡献，不代表【wpsshop博客】立场，版权归原作者所有，本站不承担相应法律责任。如您发现有侵权的内容，请联系我们。转载请注明出处：https://www.wpsshop.cn/w/你好赵伟/article/detail/390296?site