本书介绍了导航拒止环境下无人机自主导航与运动规划技术的研究现状，着重描述了无人机自主导航、运动规划的方法与应用。根据国内外在该领域的最新研究成果及课题组最新研究进展，凝练其中的关键问题与设计方法。针对导航拒止环境下的无人机自主导航问题，分别提出了基于视觉的无人机自主导航方法、基于激光雷达的无人机自主导航方法、基于多传感器融合的无人机自主导航方法。针对复杂多障碍环境下的无人机自主运动规划问题，分别描述了感知受限条件下的无人机实时局部运动规划方法、面向未知环境的无人机自主探测运动规划方法。针对真实导航拒止环境，设计了一套面向室外复杂环境的无人机自主飞行系统。最后，基于搭建的实物平台展开闭环飞行实验，验证了本书提出的无人机自主导航与运动规划方法的有效性。 本书适合自动化相关专业的本科生及研究生阅读，同时可供对无人机自主导航与运动规划感兴趣的高校师生、科研工作者和工程技术人员阅读参考。

第1章　绪论
　　无人机(unmanned aerial vehicle，UAV)诞生于**次世界大战期间，相关技术发展至今已有一百多年历史。无人机的定义即不载有操作人员、可以自主飞行或遥控驾驶、可以一次使用也可回收使用、携带致命或非致命有效载荷的有动力飞行器。相对有人作战系统，无人机具有成本低廉、结构轻巧、机动灵活且隐蔽性好等优势。其研究过程大致经历了三个发展阶段，从**次世界大战的萌芽期，到20世纪80年代以色列*创的有人-无人机协同作战引起的发展期，再到当前伴随人工智能相关技术快速发展的蓬勃期，其种类不断增多，性能日益提升，应用领域也逐步拓展。无人机可以分为固定翼与旋翼两大类别，固定翼无人机是*早得以研究与应用于战场的，其在航速、续航时间、飞行载荷等方面具有明显的优势，可以胜任大多侦察、监视等军事任务；旋翼无人机具备定点悬停、垂直起降、机动灵活等优势，能够执行各类低空任务，更适用于山区、丛林和城市等复杂环境，而在旋翼无人机中，多旋翼无人机由于其机械结构简单、飞行原理简单、故障率低等特点，成为无人机家族中研究与应用*广泛的一类。
　　当前阶段，多旋翼无人机系统的自动化研究已经解决了基本的自主飞行控制问题，通过机载动力系统与导航定位模块结合自身的自主控制系统，具备实现姿态控制与轨迹跟踪的能力。伴随着相关技术的发展，多旋翼无人机平台被广泛应用于探测、巡检或搜寻等应用场景。然而，当面对丛林、楼宇等复杂场景时，简单的自主控制无法满足日益增长的实际应用需求。近年来，导航拒止环境下的无人机自主导航、复杂多障碍环境下的无人机实时运动规划、面向导航拒止环境下的无人机自主飞行演示验证等成为推动无人机自主化的关键技术。本书重点针对无人机在导航拒止环境下自主飞行中面临的关键问题进行介绍。为解决导航拒止环境下无人机自主导航方法鲁棒性不足、感知受限条件下无人机运动规划算法难以求解等技术问题，针对丛林、楼宇、地下空间等典型导航拒止环境，提出了无人机自主导航与实时运动规划方法。同时，搭建面向导航拒止环境的演示验证平台，测试无人机在导航拒止环境下的自主导航与运动规划能力，从而提升无人机的自主任务执行能力。
　　1.1　无人机自主导航与运动规划研究现状
　　近年来，无人机在情报探测、搜索救援、侦察预警、目标捕捉等方面发挥越来越重要的作用，已经成为未来军事战场中不可或缺的力量。无人机由于其成本低、灵活性强，被普遍认为是未来应用于实际军事作战的核心装备之一，因此受到了世界各国尤其是美国的广泛关注。
　　1.1.1　相关研究项目概述
　　自海湾战争中无人机正式投入作战以来，美国陆军无人机在军事作战中发挥越来越重要的作用。此后历次局部战争或军事行动中，无人机的能力和任务进一步拓展，成为战场上不可或缺的力量，并赢得了“陆军之眼”的美誉。目前，美国陆军拥有超过4000架各种型号的无人机，编制在陆军的多个层级中，在战场全纵深执行各种作战任务，负责在联合行动和在全谱军事行动中支持陆军作战。但直至目前无人机作战场景多局限于通过全球导航卫星系统(global navigation satellite system， GNSS)为无人机导航定位的作战环境，针对导航定位拒止环境下的无人机作战仍存在极大的挑战性，特别是当士兵在建筑物内、城市峡谷、地下或森林中操作无人机作业时，导航定位拒止环境会严重影响无人机的操作性能。近年来，无人机在任务执行过程中坠毁的情况时有发生。据统计，2010～2020年超过175架美军无人机因电磁干扰、卫星信号拒止等技术原因而坠毁。考虑到导航拒止等复杂环境在未来战争中的重要作用，为提高无人机在导航拒止环境中的自主化水平，美国国防高级研究计划局(Defense Advanced Research Projects Agency， DARPA)先后实施了拒止环境中协同作战(collaborative operations in denied environment， CODE)项目及快速轻量自主(fast lightweight autonomy， FLA)项目，重点开发针对导航拒止等复杂环境的无人机快速安全自主飞行算法。由于充分意识到导航拒止的复杂环境在未来战争中的重要作用，2018年DARPA开始着手启动导航拒止环境下的地下空间(subterranean， SUBT)挑战赛，DARPA在SUBT挑战赛官网中引用了Clausewitz在《战争论》中的一句话“The relationship between warfare and terrain demands the faculty of quickly and accurately grasping the topography of any area”(战争与地形之间的关系要求我们具备快速、准确掌握任何区域地形的能力)(图1.1)。从中不难看出，未来无人系统作战将极有可能下沉到地下，形成“海陆空+地道战”的新型作战形式，借助地下空间进行隐藏、机动、休整、支援保障的军事行动。DARPA通过比赛牵引，使无人系统初步形成了具备对导航拒止等复杂环境的快速探测的能力，为后续复杂环境下无人系统的自主化发展奠定了基础。下面将对DARPA针对导航拒止环境开展的两个项目，即DARPA-FLA项目与DARPA-SUBT挑战赛进行详细介绍。
　　1. DARPA-FLA项目
　　2015年，DARPA启动FLA项目(图1.2)。FLA项目旨在探索一种非传统的感知及自主化方法，通过该方法实现多旋翼无人机在复杂环境下的高速自主飞行，且该过程不需要与操作员进行通信，也不需要GNSS的外界定位辅助。该项目展示出了一系列具备卓越飞行能力的多旋翼无人机设计方案，其潜在应用包括：在作战部队进入未知建筑之前，快速完成对内部威胁区域的扫描；在无法通过高空图像观测的茂密的丛林地区或敌对地区搜寻伤员；进入位于灾后区域的一些危险建筑物内搜寻幸存者等。该项目聚焦于导航、规划以及控制等方面的自主化，并要求自主飞行功能能够通过消费级传感器应用在小型化商用平台上。该项目的**阶段于2017年进行，研究团队平衡无人机的尺寸、重量以及感知能力等因素，使无人机利用有限的机载计算能力完全自主地执行简单任务。
　　DARPA在2018年完成了FLA项目的第二阶段飞行试验，在**阶段飞行测试的基础上，科研团队对软件与硬件系统进行优化，使用消费级传感器达到更高的性能水平，以便应用于尺寸更小、重量更轻的多旋翼无人机，如图1.3所示。其所设计的无人机在模拟城镇环境中进行了飞行试验，实现了在多层建筑之间高速飞行，并在飞行过程中确认感兴趣的目标；穿越窄窗飞进建筑物，沿走廊寻找房间并构建三维室内环境地图；*后识别走廊或楼梯场景，并沿该走廊从敞开的门退出建筑物。
　　图1.1 DARPA地下空间挑战赛
　　图1.2 DARPA-FLA项目**阶段
　　图1.3 DARPA-FLA项目第二阶段
　　2. DARPA-SUBT挑战赛
　　2018年，DARPA启动SUBT挑战赛，SUBT是DARPA大型机器人挑战赛中*新的一系列，该挑战赛旨在提升可在黑暗、密闭和危险空间中工作的移动机器人的自主化水平。挑战赛分为三个*立的阶段，每个阶段都在不同的地下环境(隧道、城市地下和地下洞穴)中进行。复杂的地下环境给军事和民用急救人员带来了巨大挑战，不同领域伴随着差异性风险，且该环境随时间动态变化，给进入该环境的作业人员带来了极大的风险，来自世界各地的团队应邀参与到挑战赛中，提出了各类新颖的解决办法，以解决在未知恶劣环境中的快速探索问题。
　　2019年，该挑战赛进行了**阶段——隧道环境挑战，如图1.4所示，挑战赛选用位于宾夕法尼亚州匹兹堡的一个矿洞，科研团队通过多样化的方案、多类型的机器人平台，在封闭的隧道环境中实现自主导航，并协同作业完成任务，以及通过多节点间的分布式通信技术，将探测到的信息传送给隧道外的工作人员。2020年，该挑战赛进行了第二阶段——城市地下环境挑战，如图1.5所示。该阶段的挑战赛在华盛顿奥林匹亚以西的Satsop商业园中进行，相比**阶段的隧道环境而言，更为复杂的城市地下环境给参赛团队带来了更大的挑战，挑战赛需要研究团队派出的机器人在规定时间内，在错综复杂的地下环境中搜寻特定目标，并需要通过无线通信手段将具体的目标信息传送出作业空间。
　　图1.4 DARPA地下空间挑战赛**阶段——隧道环境挑战
　　2021年该挑战赛进行了第三阶段——地下洞穴环境挑战，如图1.6所示。该阶段的挑战赛在路易斯维尔巨型洞穴举行，经过激烈的角逐，CERBERUS团队通过高度自主化的导航与规划算法设计，完成了对未知地下洞穴的快速探测，并*终赢得200万美元的奖金。
　　综上所述，无人机作为改变未来战争模式的颠覆性技术装备，在世界范围内得到飞速发展，已经成为国家间军事博弈的重要力量。然而，传统无人机在执行任务时，往往需要依赖GNSS，难以在楼宇、丛林及地下空间等复杂环境下进行全自主飞行，而这些典型复杂环境对未来城市巷战及野外低空作战的全方位信息获取至关重要。因此，作为无人机在复杂未知多障碍环境下实现自主飞行的核心技术，无人机自主导航与运动规划技术受到国内外的广泛关注。
　　图1.5 DARPA地下空间挑战赛第二阶段——城市地下环境挑战
　　图1.6 DARPA地下空间挑战赛第三阶段——地下洞穴环境挑战
　　1.1.2 导航拒止环境下无人机自主导航方法研究现状
　　导航拒止环境下无人机的自主导航方法是指无人机在不利用卫星及外部传感器(如超宽带(ultra-wide bandwidth， UWB)设备、运动捕捉系统等)信息的情况下实现无人机自主定位与建图的方法。导航拒止环境下无人机的自主导航方法按照传感器类型分类，可以分为基于视觉的自主导航方法、基于激光雷达的自主导航方法、基于视觉与激光雷达融合的自主导航方法。在此，针对上述三类主流导航拒止环境下的自主导航方法展开讨论。
　　

目录
前言
第1章 绪论 1
1.1 无人机自主导航与运动规划研究现状 1
1.1.1 相关研究项目概述 2
1.1.2 导航拒止环境下无人机自主导航方法研究现状 6
1.1.3 多障碍环境下无人机运动规划方法研究现状 13
1.2 导航拒止环境下无人机自主导航与运动规划难点分析 18
1.2.1 不依赖外部传感器信息的无人机自主导航 18
1.2.2 考虑避障与传感器视场约束的无人机运动规划 19
1.2.3 面向复杂导航拒止环境的无人机自主飞行 19
1.3 本书主要内容 19
参考文献 21
第2章 坐标系定义与坐标变换 27
2.1 无人机坐标系定义 27
2.1.1 参考坐标系 27
2.1.2 机体坐标系 27
2.1.3 相机坐标系 28
2.1.4 激光雷达坐标系 28
2.2 坐标系间的变换关系 29
2.2.1 旋转矩阵 29
2.2.2 旋转向量 30
2.2.3 欧拉角 30
2.2.4 四元数 32
2.2.5 变换矩阵 34
2.3 李群与李代数 35
2.3.1 李群 35
2.3.2 李代数 37
2.3.3 李群与李代数间的映射关系 40
2.3.4 李代数雅可比 43
2.4 本章小结 47
参考文献 47
第3章 基于视觉的无人机自主导航方法 48
3.1 概述 48
3.1.1 相关工作 48
3.1.2 基于视觉的无人机自主导航方法结构设计 49
3.1.3 问题描述 50
3.2 传感器工作原理 52
3.2.1 针孔相机模型与畸变校正方法 52
3.2.2 双目相机模型 57
3.2.3 惯性测量单元 61
3.3 基于视觉的无人机自主导航前端数据处理 63
3.3.1 图像特征提取 64
3.3.2 图像特征匹配 66
3.3.3 特征点深度恢复 69
3.4 基于滤波后端的无人机自主导航方法 71
3.4.1 状态变量定义 71
3.4.2 滤波预测模型 73
3.4.3 滤波更新模型 77
3.5 基于优化后端的无人机自主导航方法 83
3.5.1 状态变量定义 84
3.5.2 非线性优化问题构建 85
3.5.3 非线性优化问题求解 90
3.6 实验验证 92
3.7 本章小结 94
参考文献 94
第4章 基于激光雷达的无人机自主导航方法 97
4.1 概述 97
4.1.1 相关工作 97
4.1.2 基于激光雷达的无人机自主导航方法结构设计 98
4.1.3 问题描述 99
4.2 基于激光雷达的无人机自主导航前端数据处理 103
4.2.1 传感器模型 103
4.2.2 点云畸变产生的原因与畸变校正方法 105
4.2.3 点云特征提取 108
4.3 基于激光雷达点云地图后端的无人机自主导航方法 111
4.3.1 特征点云地图构建 111
4.3.2 残差函数构建 112
4.3.3 非线性优化问题求解 116
4.4 基于激光雷达体素地图后端的无人机自主导航方法 123
4.4.1 体素地图构建 124
4.4.2 残差函数构建 126
4.4.3 非线性优化问题求解 128
4.5 实验验证 130
4.6 本章小结 132
参考文献 133
第5章 基于多传感器融合的无人机自主导航方法 135
5.1 概述 135
5.1.1 相关工作 135
5.1.2 基于多传感器融合的无人机自主导航方法结构设计 136
5.1.3 问题描述 137
5.2 基于视觉雷达融合的无人机自主导航前端数据处理 138
5.2.1 视觉与激光雷达融合的时间同步方法 138
5.2.2 视觉与激光雷达融合的空间同步方法 140
5.2.3 视觉特征点深度恢复 145
5.3 基于视觉雷达融合的无人机自主导航方法 147
5.3.1 基于视觉雷达松耦合的无人机自主导航方法 147
5.3.2 基于视觉雷达紧耦合的无人机自主导航方法 156
5.4 实验验证 164
5.4.1 基于视觉雷达松耦合的无人机自主导航方法实验验证 164
5.4.2 基于视觉雷达紧耦合的无人机自主导航方法实验验证 166
5.5 本章小结 167
参考文献 167
第6章 感知受限条件下的无人机实时局部运动规划方法 169
6.1 概述 169
6.1.1 相关工作 169
6.1.2 无人机局部运动规划方法结构设计 170
6.1.3 无人机局部运动规划问题广义表达形式 171
6.1.4 并行化地图更新 176
6.2 基于模型预测路径积分控制的轨迹规划 178
6.2.1 路径积分控制理论推导 178
6.2.2 路径积分控制具体步骤 182
6.2.3 模型预测控制实现 185
6.3 有限视场角范围约束的自主飞行 188
6.3.1 有限视场角约束 189
6.3.2 未探索区域安全性约束 191
6.3.3 实验验证 193
6.4 稀疏特征场景下的自主感知飞行 198
6.4.1 兴趣点观测约束 199
6.4.2 实验验证 201
6.5 本章小结 204
参考文献 204
第7章 面向未知环境的无人机自主探测运动规划方法 206
7.1 概述 206
7.1.1 相关工作 206
7.1.2 无人机自主探测方法结构设计 207
7.2 基于前端边界的探测引导路径生成 208
7.2.1 相关概念定义 209
7.2.2 边界增量式更新 212
7.2.3 信息结构更新 214
7.2.4 旅行商问题构建与求解 219
7.3 基于边界增益约束的后端局部运动规划 224
7.3.1 边界体素滤波 226
7.3.2 边界约束设计 227
7.4 实验验证 230
7.4.1 仿真分析 230
7.4.2 实物验证 236
7.5 本章小结 238
参考文献 238
第8章 面向室外复杂环境的无人机自主飞行系统 240
8.1 概述 240
8.1.1 相关工作 240
8.1.2 面向室外复杂环境的无人机自主飞行系统结构设计 241
8.2 感知系统设计 244
8.2.1 多传感器融合状态估计 244
8.2.2 全局-局部地图构建 249
8.3 运动规划系统设计 250
8.3.1 全局-局部运动规划策略 251
8.3.2 无人机局部初始轨迹解算 252
8.3.3 无人机局部轨迹规划 256
8.4 控制系统设计 259
8.4.1 多旋翼无人机动力学模型建立 260
8.4.2 外环轨迹跟踪控制器设计 261
8.4.3 内环姿态控制器设计 264
8.5 实验验证 265
8.5.1 感知系统验证实验 265
8.5.2 运动规划系统验证实验 267
8.5.3 无人机控制系统验证实验 269
8.5.4 导航拒止环境下无人机自主飞行验证实验 272
8.6 本章小结 275
参考文献 275