《机器人自主智能导航》面向培养导航工程、机器人工程和人工智能复合型创新人才的需求，以移动机器人等无人系统为对象，系统讲述自主智能导航的概念内涵、技术框架和研究方法。《机器人自主智能导航》内容主要包括机器人自主导航框架、环境语义感知、状态估计、同步定位与建图、视觉语义融合、导航规划与决策以及认知导航、多足机器人导航等方面的代表性技术和最新研究成果。《机器人自主智能导航》力求反映智能导航技术的最新理论成果和应用案例，突出深度学习等AI技术在导航中的应用，并在各章节安排了相关工程实践教程。

介绍人工智能技术在环境感知任务中的应用

目录
序一
序二
前言
第1章 机器人自主智能导航概述 1
1.1 技术的发展和趋势 2
1.2 迈向智能导航 9
1.3 本书内容简介 13
参考文献 14
第2章 机器人自主导航框架 17
2.1 机器人操作系统简介 17
2.1.1 概述 17
2.1.2 ROS的基础概念与设计思想 18
2.2 自主导航框架 24
2.2.1 建图与地图发布模块 26
2.2.2 状态估计模块 29
2.2.3 全局路线规划模块 31
2.2.4 局部规划模块 31
2.2.5 抽象控制模块 32
2.3 用于机器人导航的硬件 34
2.3.1 主动传感器 34
2.3.2 被动传感器 39
2.3.3 电机 41
2.3.4 常见的机器人底盘及运动模型 42
2.3.5 融合北斗和AI的实验教学系统 51
2.4 工程实践：差速轮机器人的远程操控 52
2.4.1 环境配置与安装 53
2.4.2 代码解析 54
2.4.3 实验 55
2.5 工程实践：ROS：：Movebase 58
2.5.1 环境配置与安装 58
2.5.2 实验框图 61
2.5.3 仿真实验 63
参考文献 65
第3章 机器人环境语义感知 67
3.1 目标检测 67
3.1.1 概述 67
3.1.2 常用数据集及评估指标 69
3.1.3 二阶段目标检测模型 71
3.1.4 一阶段目标检测模型 74
3.2 图像分割 79
3.2.1 概述 79
3.2.2 常用数据集及评估指标 81
3.2.3 语义分割 83
3.2.4 实例分割 90
3.2.5 全景分割 93
3.2.6 融合深度信息的分割 95
3.3 目标跟踪 98
3.3.1 概述 98
3.3.2 常用数据集与评估指标 101
3.3.3 单目标跟踪 104
3.3.4 多目标跟踪 111
3.4 视觉注意力 113
3.4.1 概述 113
3.4.2 基于Transformer的目标检测 120
3.4.3 基于Transformer的图像分割 123
3.4.4 基于Transformer的目标跟踪 125
3.5 基于深度学习的3D点云语义感知 127
3.5.1 概述 127
3.5.2 常用数据集与评估指标 130
3.5.3 3D点云目标检测 130
3.5.4 3D点云分割 134
3.5.5 3D点云目标跟踪 137
3.6 图像和点云的融合语义感知 138
3.6.1 概述 138
3.6.2 前期融合 139
3.6.3 后期融合 139
3.7 典型交通对象的语义感知 140
3.8 工程实践：YOLO v4 142
3.8.1 环境配置与安装 142
3.8.2 代码解析 145
3.8.3 实验 146
3.9 工程实践：DeepLab v3 + 149
3.9.1 环境配置与安装 149
3.9.2 代码解析 151
3.9.3 实验 153
3.10 工程实践：SiamRPN 161
3.10.1 环境配置 162
3.10.2 代码解析 163
3.10.3 实验 164
参考文献 168
第4章 机器人状态估计 175
4.1 GNSS高精度定位定姿技术 175
4.1.1 全球卫星导航系统 175
4.1.2 GNSS快速定位 184
4.1.3 GNSS高精度定位方法 185
4.1.4 GNSS测向定姿 189
4.2 基于IMU的主动姿态估计 191
4.2.1 捷联惯性导航涉及的坐标系 191
4.2.2 捷联惯性导航中IMU传感器误差建模 193
4.2.3 捷联惯性导航姿态表示方法 195
4.2.4 捷联惯导算法中的微分方程及其解算更新 197
4.2.5 捷联惯导的误差微分方程 201
4.3 基于激光雷达的定位 204
4.4 基于视觉的定位 204
4.5 基于机会信号的定位 204
4.6 GNSS/SINS组合导航定位 206
4.6.1 卡尔曼滤波用于数据融合 208
4.6.2 GNSS/SINS松组合 210
4.6.3 RTK/SINS紧组合 213
4.7 因子图用于数据融合 214
4.8 深度神经网络用于数据融合 219
4.8.1 非端到端学习下的惯导定位方法 219
4.8.2 端到端学习下的惯导定位方法 223
4.8.3 监督学习下的视觉定位方法 227
4.8.4 无监督学习下的视觉定位方法 230
4.8.5 惯性传感器与视觉传感器的数据融合 234
4.9 工程实践：利用网络RTK技术定位机器人 237
4.9.1 实验准备 237
4.9.2 GNSS数据格式 238
4.9.3 实验 239
4.10 工程实践：GNSS/INS松组合导航定位 242
4.10.1 数据集介绍 242
4.10.2 代码解析 243
4.10.3 实验 244
4.11 工程实践：RoNIN 247
4.11.1 环境配置 247
4.11.2 代码解析 248
4.11.3 实验 250
参考文献 252
第5章 机器人同步定位与建图 255
5.1 激光SLAM方法 255
5.1.1 概述 255
5.1.2 基于优化的激光SLAM 258
5.1.3 基于滤波的激光SLAM 266
5.2 视觉SLAM方法 268
5.2.1 概述 268
5.2.2 光流估计 275
5.2.3 间接法视觉SLAM与经典方案 280
5.2.4 直接法视觉SLAM与经典方案 289
5.3 视觉惯性SLAM方法 293
5.3.1 概述 293
5.3.2 基于滤波的视觉惯性SLAM方法 294
5.3.3 基于优化的视觉惯性SLAM方法 302
5.4 激光惯性SLAM方法 307
5.4.1 概述 307
5.4.2 基于滤波的激光惯性SLAM 308
5.4.3 基于优化的激光惯性SLAM 313
5.5 事件相机SLAM方法 315
5.6 工程实践：Cartographer 319
5.6.1 环境配置 319
5.6.2 代码解析 323
5.6.3 Turtlebot3仿真实验 324
5.7 工程实践：ORB-SLAM2 326
5.7.1 环境配置 327
5.7.2 代码解析 329
5.7.3 实验 331
5.8 工程实践：ORB-SLAM3 333
5.8.1 环境配置 333
5.8.2 代码解析 336
5.8.3 实验 337
参考文献 339
第6章 机器人视觉语义融合 344
6.1 语义辅助前端特征筛选 344
6.1.1 概述 344
6.1.2 语义分割辅助特征筛选 345
6.1.3 实例分割辅助特征筛选 347
6.2 语义辅助后端定位优化 350
6.2.1 概述 350
6.2.2 像素级语义辅助后端 350
6.2.3 物体级语义辅助后端 353
6.3 语义辅助回环检测 356
6.3.1 概述 356
6.3.2 基于特征的回环检测 357
6.3.3 基于场景的回环检测 359
6.4 语义融合环境建模 361
6.4.1 概述 361
6.4.2 像素级语义建模 364
6.4.3 物体级语义建模 368
6.5 工程实践：DS-SLAM 369
6.5.1 环境配置 370
6.5.2 代码解析 372
6.5.3 实验 373
参考文献 374
第7章 机器人导航规划与决策 378
7.1 全局路线的规划 378
7.1.1 基于图搜索的路线规划 378
7.1.2 基于采样的路线规划 383
7.1.3 面向任务的路线规划 384
7.2 动态目标的距离探测和运动预测 386
7.2.1 传感器测距 386
7.2.2 基于动态SLAM的多目标运动预测 388
7.3 基于几何模型的避障 393
7.3.1 概述 393
7.3.2 速度障碍模型 394
7.3.3 快速几何避障模型 396
7.4 基于势场模型的避障 399
7.4.1 概述 399
7.4.2 人工势场模型 400
7.4.3 旋转矢量场模型 401
7.4.4 混合势场法模型 403
7.5 基于优化思想的避障 405
7.5.1 概述 405
7.5.2 动态窗口算法 405
7.5.3 时变松紧带算法 410
7.6 基于深度学习的局部规划 414
7.6.1 端对端局部规划 414
7.6.2 融合仿生LGMD的局部规划 418
7.7 更高级的行动决策 421
7.8 工程实践：TEB_Local_planner 424
7.8.1 环境配置与安装 424
7.8.2 文件介绍 425
7.8.3 仿真实验 426
参考文献 433
第8章 基于强化学习的认知导航 436
8.1 认知导航的任务描述与建模 436
8.1.1 基本定义与马尔可夫决策过程建模 436
8.1.2 强化学习A2C/A3C算法 442
8.1.3 常见的任务形式 445
8.2 目标驱动导航 446
8.2.1 孪生网络模型 447
8.2.2 自适应视觉导航模型 449
8.3 视觉语言导航 452
8.3.1 增强型跨模态匹配和自监督模仿学习的视觉语言导航模型 452
8.3.2 对象-动作感知模型 458
8.4 视觉对话导航 461
8.5 导航知识图谱的构建与应用 466
8.5.1 知识图谱概述 466
8.5.2 认知导航中知识图谱的构建 467
8.5.3 认知导航中知识图谱的应用 470
8.6 拓展到仿生导航 476
8.6.1 仿生导航传感器 476
8.6.2 使用类网格细胞网络模拟生物的矢量导航 477
8.7 工程实践：SAVN 481
8.7.1 数据集 481
8.7.2 环境配置与安装 481
8.7.3 代码解析 484
8.7.4 实验 485
参考文献 486
第9章 多足机器人导航 489
9.1 多足机器人的发展及应用 489
9.2 多足机器人上的足式里程计 490
9.2.1 多足机器人的D-H建模方法 491
9.2.2 多足机器人的正向运动学模型 492
9.2.3 多足机器人的足式里程计的构建 493
9.3 多足机器人上的不变卡尔曼滤波器 495
9.3.1 不变卡尔曼滤波器简介 495
9.3.2 组合导航中的不变卡尔曼滤波器 498
9.3.3 足式里程计/INS组合导航 501
9.4 多足机器人上的最优平滑算法 503
9.4.1 双向滤波算法 503
9.4.2 RTS平滑 505
9.4.3 分段平滑 506
9.5 多足机器人导航的发展趋势 507
参考文献 509
附录 部分常用数据集 511
参考文献 515

第1章 机器人自主智能导航概述
　　导航学是一门古老的科学。导航能力源自生物的本能和人类对大自然的探索。最早人们通过山川河流、日月星辰来确定位置和方向。指南针的发明促进了大航海时代的兴盛。全球卫星导航系统（Global Navigation Satellite System，GNSS）的建成推动了现代导航工程的快速发展。围绕着导航的三个基本问题“我在哪”、“我要去哪”以及“如何去”，经过不断探索与实践，导航工程已经形成了较为完备的技术体系。而伴随着机器人、无人系统的快速发展，如何为机器人导航成为导航工程、机器人学以及人工智能等学科共同关注的热点。
　　机器人学（Robotic）是一门综合运用机械装置、传感器、驱动器以及计算设备来实现人类在某些方面能力的学科[1， 2]。该学科综合了力学、仿生学、自动控制、机械设计、传感器技术以及导航学等领域的知识，是一门研究机器认知理论与制造技术的综合性学科。早在机器人出现之前，Norbert Wiener便发表了《控制论》一书，向人们介绍了反馈技术的应用和日常生活中的意义，普及了反馈控制的概念，为机器人学的产生和发展奠定了理论基础。自20世纪60年代第一台工业机器人问世以来，机器人在工业生产、国防建设、国民经济及人民生活等诸多领域扮演着越来越重要的角色。1972年美国斯坦福国际研究所第一次在机器人中引入了人工智能技术，通过在机器人上搭载相机、测距仪等传感器研发出了能够对周围环境进行测量的移动机器人Shakey[3]。尽管Shakey的实际表现并不理想，但它的出现是人工智能和机器人融合感知环境的一个重要里程碑。1980年前后，美国斯坦福大学研制的Stanford Cart机器人利用机载摄像机实现了机器人在周围环境中的导航，这一成果首次将导航技术与机器人技术结合，促进了机器人学中导航分支的发展。作为机器人学的重要分支之一，导航技术可以为机器人提供位置信息、路线规划、环境感知等诸多服务，是机器人工作的基础。人工智能技术则为如何获取精确、可靠和智能的导航信息提供了新的方法和思路。
　　人工智能（Artificial Intelligence，AI）的概念于1956年在达特茅斯会议上被首次提出。由于受到基础理论、计算能力、工业水平等诸多因素的影响，人工智能技术的发展几经波折，呈现出螺旋式上升的态势。2010年前后，随着计算硬件技术的快速发展，以深度学习（Deep Learning）为代表的人工智能迎来又一个爆发期。2018年的A.M.图灵奖（A.M. Turing Award）颁发给了Yoshua Bengio、Geoffrey Hinton和Yann LeCun三位科学家，这是人工智能史上具有划时代意义的重要时刻，代表着机器的智能化趋势得到普遍认可。国务院2017年发布《新一代人工智能发展规划》中也明确提出，要重点突破自主无人系统计算架构、复杂动态场景感知与理解、实时精准定位、面向复杂环境的适应性智能导航等共性技术，为无人机、服务机器人、特种机器人、无人驾驶等提供核心技术，支撑整个无人系统应用和产业发展[4]。导航技术、机器人技术和人工智能三者已经紧密结合在一起。
　　1.1 技术的发展和趋势
　　机器人智能导航可以分为几何导航（Geometric Navigation）、自主导航（Autonomous Navigation）和智能导航（Intelligent navigation）几个阶段，并朝着超越生物导航、定位和授时（Positioning，Navigation，and Timing，PNT）智能的方向发展[5]，越来越精准、越来越可靠、越来越具有弹性（Resilience）[6]。机器人的“几何导航”是以获得空间几何信息为主要目标，通过搭载的测量传感器进行坐标化的度量和距离性规划，辅助人进行决策，不考虑无人化运行；“自主导航”以机器人无人操控、自主运动为目标，主要由各种人工建立的模型驱动（Model Driving），具备初步的机器智能；“智能导航”则在“自主导航”的基础上，在方法层面超越“人工植入的模型”，从感知向认知发展，实现模型和数据共同驱动（Model & Data Driving）。在功能层面则让机器人拥有或部分拥有生物导航的特性，能适应复杂而陌生的环境，理解高级的任务意图，见图1.1。
　　以近几年来受到学术界、工业界和技术市场广泛关注的自动驾驶汽车（Autonomous Vehicle）为例，可以充分认识机器人智能导航技术的发展趋势。从导航角度看，自动驾驶汽车是一种典型的具有网络支持、集成多种服务的轮式机器人[7]。作为人工智能、汽车制造、自动控制、网络通信等多种先进技术集成的产物，自动驾驶汽车通过网络和车载传感器感知车辆周边环境，然后由智能驾驶系统进行路线规划与决策控制，实现车辆在道路上的自主智能行驶。国外早在20世纪中叶，便对自动驾驶技术进行了初步探索。进入21世纪后，自动驾驶技术迅速发展。2004年美国国防部组织了著名的自动驾驶DARPA挑战赛，旨在推动自动驾驶技术的发展。自此以后，全球开始对自动驾驶技术投入大量研究。我国相比国外起步较晚，但发展速度很快。2008年国家自然科学基金委员会发起“视听觉信息的认知计算”计划，对自动驾驶汽车研究项目进行资助，并连续十几年主办中国智能车未来挑战赛（Intelligent Vehicle Future Challenge，IVFC）。以百度为代表的新兴技术企业更是对自动驾驶投入全力，到2015年已经实现了城市复杂交通场景下最高速度100km/h的无人驾驶，2017年向整个汽车行业提供了开放、统一的开发平台Apollo[8]。在车辆智能化分级方面，目前工业界也主要采用美国的两套标准，一套是由美国国家公路交通安全管理局（National Highway Traffic Safety Administration，NHTSA）制定的，另一套是由美国汽车工程师学会（Society of Automotive Engineers，SAE）制定的。SAE分级标准中的L0级表示无自动化，L1级表示驾驶辅助，包含自适应巡航控制等辅助功能。L2级表示部分自动化，包含紧急制动、避免碰撞等高级辅助功能。从L3级开始，实时环境感知的主体由人类驾驶员变为驾驶系统，车辆可以在相对简单的限定环境（如高速公路、封闭园区等）中自主行驶，但特殊情况下仍需人工干预。L4级与L5级无须任何的人工干预，但L4级只能在有限的环境中运行，而L5级可以在任意环境下完全自动驾驶。目前全球都在积极研制具有L4级或L5级的自动驾驶汽车，其主要的挑战之一就是智能导航[9-11]。
　　图1.1 机器人自主智能导航的技术
　　移动机器人涉及感知、规划、控制三部分[12]。①感知包括对自身位姿的感知（这一问题通常被称为机器人状态估计（State Estimation for Robotic）[13]和对环境目标的感知。测量技术是感知的基础。智能导航要在测量的基础上开展对环境和目标的识别、预测以及推理，将感知发展为认知。②导航规划可以分为全局性的规划和局部性的规划，解决“如何去”的问题。最普通的全局规划是以距离为基础的导航路线规划。发展到智能阶段，则需要增加对除距离外的各种交通语义的理解，如路况、流量、通行成本等，让机器人思考如何在“盲环境”下开展路线规划。局部规划通常又被称为“决策”，如“如何避开障碍物”、“如何让机器人平稳舒适行驶”、“当前时刻是否应该切换车道”、“是否还应该往前继续搜索”等都属于决策的范畴，是最能体现机器人导航智能的部分。③控制本身是机器人学的重要内容。例如，一个串联的关节型机器人（机械臂），如果已知各个关节的状态去求机器人末端夹具的位姿，这个过程为正运动学（Forward Kinematic）。反之，如果知道末端的运动去求各个关节的状态，这个过程就是逆运动学（Inverse Kinematic）。机器人导航也有类似的概念。我们将机器人看作运动质点或刚体，计算出当前时刻它在各个方向的运动速度，是前述规划部分的任务。同样的，如将规划得到的各个方向的速度转化为轮式底盘、机器狗各关节、无人机各旋翼电机的转速与执行序列，则属于控制部分要考虑的事情。传统导航学认为导航不涉及控制。但具体到机器人导航，需要关注局部规划所做出的决策是否可行。如果因为电机功率等原因无法实现决策行为，或者机器人在控制约束下无法做出规划的动作，都说明导航规划是无效的。因此控制问题也要被包含在导航范畴下一起考虑[14]。至于更加底层的机电控制、材料及机械、硬件实现等问题，不在本书讨论范围内。机器人自主智能导航的技术发展趋势包括以下几个方面。
　　1. 多源传感器协同定位
　　定位技术是自主智能导航的重要基础。尽管GNSS带来了定位技术的革新，然而单一传感源的导航系统无论是精度还是适用范围都很难满足用户的需求。GNSS/INS组合导航系统（GNSS/Inertial Integrated Navigation System）虽然在一定程度上解决了单源导航存在的问题，但对惯性器件的使用要求较高且环境适应性仍然存在缺陷。因此多源融合导航系统应运而生。该系统利用协同化的数据融合算法将来自不同数据源的定位信息进行融合，进而得到最佳的导航结果[15]。常见的传感器源包括GNSS接收机、惯性测量单元（Inertial Measurement Unit，IMU）、气压计、磁力计、里程计、相机、雷达等，见图1.2。
　　多源传感器融合定位的重点不在于传感器源的数量和种类，而是融合方法与手段。因此多源融合又被称为协同精密定位（Precise Cooperative Real-Time Positioning），指多个用户通过信息交互和通信，共享位置服务技术和资源，融合各类定位手段以突破位置服务中的各种时空障碍与信息缺乏，协作完成各自或共同的高精度定位的一种技术 。当前最成熟的融合思路之一是基于卡尔曼滤波（Kalman Filter，KF）展开的。但是传统运动对象的动力学模型大多建立在欧几里得空间中，而大量研究和工程实践表明，机器人运动状态是在黎曼空间中的矩阵流形（Matrix Manifold）上变化的[16]，尤其是无人机、多足机器人等具有复杂动力学特性的机器人。经典系统动力学方程无法自然地描述机器人在这些非欧氏空间中的系统动力学过程。矩阵流形作为一种以矩阵为元素、保持光滑可微属性的流形，非常适合对空间中非线性运动状态的建模。如机器人刚体连续旋转的特殊正交群SO（3）、表示位姿（包含平移和旋转）的特殊欧几里得群SE（3）和广泛应用于姿态表示的四元数群S3都存在于矩阵流形上。因此针对机器人状态估计的滤波可以考虑在流形上设计，通过李群（Lie Group）和李代数理论将运算复杂度为O(n2)的矩阵运算转移到运算复杂度为O(n)的向量空间，提升精度和降低复杂度[17]。
　　图1.2 传感器融合模式的发展与变化[15]
　　基于滤波的融合方法以迭代的形式处理时间序列数据，这对多源传感器之间的时空标定提出了极高要求。近几年来针对机器人等运动载体，因子图（Factor Graph）优化方法也被广泛应用[18]。该方法可以实现各个传感器的即插即用，不要求严格的时间同步，实现传感器之间外参的实时在线估计。最重要的是，因子图优化方法能同时处理多个时间窗口的数据，在提高状态估计的鲁棒性的基础上有效地对数据进行增量平滑，提高状态估计的精度。因子图优化方法将状态估计问题建模成贝叶斯推断问题，然后通过概率建模将多个状态的联合概率密度函数进行分解，最后进行因子图上的最大后验概率推断，目前已取得了非常优越的性能，但是其运算成本比滤波方法要高。
　　值得关注的是，随着人工神经网络（Artificial Neural Network，ANN）在高维数据特征提取中变得越来越有效，人们也开始使用深度神经网络处理多源传感器数据的状态估计问题。这一类方法不再考虑不精确的系统动力学模型，而是直接对传感器高维数据进行训练学习。人们利用深度神经网