1.2 自主导航定位技术

1.2.1 基于激光地图的长期视觉-惯性定位

1.2.1.1 视觉-惯性定位问题分析

高精度的定位是移动机器人在环境中实验自主导航的前提。传统的定位系统多是基于激光传感器实现的。然而，由于激光传感器价格昂贵、体积与质量较大，因此设计成本低且安装灵活的定位系统具有较高的研究价值与需求。其中，基于相机的定位系统则是目前的研究热点。近年来，随着有关基于视觉的同时定位与建图算法的研究不断深入，基于相机可以实现短期内的高精度位姿估计。但是对于长期视觉定位问题，由于天气、光照、动态物体等的影响，定位时传感器感知到的环境信息可能与地图构建时的环境信息不同，使得正确关联的数据较少，从而引起定位失败。

目前，有关长期视觉定位的研究从地图维护的角度可以划分为两个方向。第一个方向是积累地图的多样性，即当新一段数据中检测到定位失败时，地图维护算法将发生定位失败区域的特征累加入地图中。这种方法维护的地图占用存储空间较大，且索引时需要较多的计算资源与耗时。第二个方向多采用深度学习的方法，基于对环境的重复观测学习定位。这种方式目前仍未能取得较为稳定的长期定位结果，且可解释性较差，仍处于探索阶段。

分析基于激光的定位算法的长期稳定性，利用几何信息实现数据关联应是其克服环境外观变化的原因。这种数据关联方式使得激光定位不会受到外界气候、光照、季节等变化的影响，只有当环境的结构信息发生较大程度的改变时，激光定位算法才会受到影响。因此，目前也有学者尝试利用几何信息实现视觉定位。由于视觉三维重建在大范围场景下实现的难度较大，且部分场景的精度难以达到激光地图的精度，目前大部分基于几何信息的视觉定位方法借助于激光地图实现定位。这种定位模式下，视觉与激光地图信息的关联可以由两种方式实现。一种是利用稠密激光点云信息实时渲染并生成虚拟位姿的图像，将其与当前相机获取的图像相匹配，从而实现对当前位姿的估计。这种方式需要稠密地图进行渲染，对计算量要求较高，且需要环境具有含明显反射率的区域或者结构性较强。另一种则是通过连续图像信息重构局部视觉点云，利用点云匹配的方式将视觉点云与激光地图点云相匹配。这种关联方式的普适性更强，但是点云匹配的方式无法考虑到视觉点云不同于激光地图点云的不确定度与空间分布，从而造成定位误差。

结合动态环境下长期视觉定位的需求与目前学界的研究进展，本节介绍了一套针对长期视觉定位的激光地图滤波系统和与之匹配的基于激光地图的视觉定位方法。考虑到视觉与激光点云的分布不同，在构建地图时，首先根据视觉特征点的分布对激光数据进行降采样，随后基于对同段场景的不同时间观测，筛选出环境中的稳定点云数据。为进一步降低激光与视觉点云误匹配的概率，尤其是针对生成自无纹理地面区域的视觉特征点，对激光地图中的地面数据进行检测与标注。经实验验证，该方法可以基于短期内构建的激光地图，实现在室外大范围场景下时间跨度长达一年的鲁棒定位，且定位精度符合实际要求。

1.2.1.2 基于激光地图的长期视觉-惯性定位算法

该方法所研究的基于激光地图的长期视觉-惯性定位算法分为离线的激光地图构建模块与在线的视觉-惯性模块两个部分，如图1-22所示，下文将针对这两个模块分别进行详细的方法介绍。

（1）离线激光地图构建与滤波

为了提高激光地图与视觉地图点云数据关联的准确性，在构建激光地图时，基于视觉观测提取其中可稳定与视觉特征点云稳定关联的子集。文中将视觉点云地图表示为，激光点云地图表示为，其中n_v、n_l分别为视觉和激光点云地图的地图点数目，ξ∈SE（3）表示机器人位姿。若将视觉点云视为稠密激光点云的观测数据，则视觉定位的似然函数可以写为

引入c_i=j表示视觉特征点与激光地图点相关联，则该似然概率L可推导得

假设Q_i（c_i）表示c_i的密度函数，则似然函数L的下界可由杰森不等式推导得

当Q_i（c_i）是c_i的后验，即时，等号成立。参考Wang等人的工作，利用高斯假设将概率函数建模为

其中R（ξ）和t（ξ）表示位姿ξ的旋转与平移分量。则当c_i无先验时，后验概率为

若将此后验概率视为中心为c_i=j的单峰分布，则搜索视觉与激光点的数据关联步骤可转化为对在激光点云中的最近邻搜索。当且仅当ξ为真值时，似然函数达到最大值。为最大化正确匹配的概率，该后验概率应近似为

基于Kullback-Leibler散度可衡量该分布与实际后验概率的相似程度

该KL散度的衡量结果为基于视觉观测的地图滤波提供的指导，即使的近邻激光点具有较大的G（c_i），而远离的激光点G（c_i）值较小，据此可筛选出激光地图中适用于视觉定位的子集。但由于单次数据中存在大量高动态和缓慢变化的物体，仍需对该子集进行处理，寻找其中静态的成分用于最终的视觉定位。

在实际实验中，由于对视觉地图点进行三维重建的位置不确定度较大，在搜寻视觉点的最近邻激光点时，利用相机与激光的外参将激光点云投影到图像平面上，经深度滤波后保留距离视觉二维特征最近的激光点作为其匹配的激光地图点。单次采集的数据中包含大量动态以及缓变物体，需要通过多次观测予以剔除，该部分功能集成于统计滤波子模块中，通过统计地图点经多次数据采集后被观测的次数，分析地图中的静态成分。

当新地图被视觉滤波模块处理后，对于该地图中的每一个地图点，在已累积的地图M^vl中寻找与其空间位置最接近的点。如果二者的空间距离大于d^α，则认为新地图中的点未被观测过；否则，认为现存地图中的相应位置的点被重复观测。

当融合的地图数量较大，涉及的情景较为全面时，累积地图M^vl可被认为已包含来自不同情景的信息。环境中稳定的部分应能在大部分地图数据采集过程中被观测到，而不稳定物体的被观测次数则较少。因此，可以通过设定被观测次数的阈值对地图进行滤波。

虽然室外环境内的路面上大部分地区没有显著的纹理信息，但是在视觉里程计算法实时运行时，仍会有一些特征点从这些无纹理的地面上被提取。这些点被提取的位置较为随机，且极易随着光照和视角的变化而发生变化。因此，在地图构建时采集自这些地方的地图点几乎不被保留在地图中。但当定位算法实时运行，尤其当周围环境较为空旷时，大部分的特征被提取自地面，如果环境地图中不包含地面信息，则实时提取的视觉地图点极易关联上错误的地图点。为提供地面约束信息，并避免地面点提取较为随机的影响，从实时数据中按高度信息提取地面点，计算其表面法向量，并将其栅格化滤波后加入地图中。这些点只为定位算法提供平面约束，避免点到点匹配关联不准确的误差。该功能集成于地面提取模块中。

（2）在线视觉-惯性定位算法

在线视觉-惯性定位算法基于定长滑窗的视觉-惯性里程计算法研发，将定位的求解紧耦合于视觉-惯性里程计的联合优化中。当检测到新一帧的视觉关键帧建立后，滑窗内的视觉地图点与相关联的激光地图点之间的约束被加入到优化的目标函数中。

式中，是视觉点与匹配到的非地面点之间的误差函数；是视觉点与匹配到的地面点之间的误差函数；Ω是相应的信息矩阵；是激光点的法向量；是视觉里程坐标系与激光地图坐标系之间的外参。整体联合优化的紧耦合图模型如图1-23所示。

基于优化的结果，即可将视觉里程计的结果与激光地图的全局位置信息相关联，从而得到全局一致的定位结果。

上文所述的方法中，由于地图的约束紧耦合于视觉里程的联合优化中，所以激光地图高精度的几何信息可以作用于视觉点云的优化，从而提高定位的精度。然而该优化方式计算量较大，且对偏置和速度的优化易受到错误数据关联的影响。除这种方式外，也可以先优化视觉里程计的约束，再基于点云匹配的方式计算外参，即将地图的约束松耦合于定位框架内。为兼顾两种方法的优势，采用迭代进行两种优化的混合优化模型进行视觉定位。

（3）实验结果分析

此方法基于采集自浙江大学某校区的真实场景数据进行测试。用于构建地图的数据采集自2017年3月，其中激光传感器为VLP-16 Velodyne LiDAR，相机为ZED的双目相机。用于测试定位的数据分别采集自2017年8月与2018年1月，包含了与构建地图时相同行驶方向采集的数据与逆向的数据，且数据采集自不同的时间段与天气。实验算法基于C++实现，其中视觉-惯性里程计基于开源代码ORB-SLAM开发。具体的实验数据细节见表1-2。

利用采集自3天内的21条数据进行面向视觉定位的激光地图构建。每条数据利用基于激光的定位建图方法估计轨迹作为位姿真值。所有的数据按流程图示意的过程进行融合，最后滤波的结果如图1-24b所示。由图可见，经过滤波后的激光地图点云分布不同于原始激光点云图（图1-24a）的均匀分布，而更接近于传统的视觉点云地图。

为评估该定位算法的效果，利用采集自2017年8月的4条数据与2018年1月雪后的数据进行定位测试。各条数据在不同定位模型下的结果见表1-3。

由表中数据可见，混合优化模型的定位结果优于纯紧耦合与纯松耦合的定位结果，且在大部分的测试数据中，“5∶1”比例的松、紧耦合迭代方式效果最佳。每段定位数据的轨迹与定位过程中水平、偏航角的误差统计如图1-25所示。

其中第一列表示第二列轨迹图中矩形框位置的真实场景图，第二列中的箭头指示数据采集的方向，第三、四列分别统计了水平与偏航角的误差。

为验证地图滤波算法对视觉定位性能提升的有效性，进一步在原始与滤波后的激光地图上进行定位测试，测试结果见表1-4。

由表中结果可见，经滤波的地图可以显著提升视觉定位算法的精度。在对比实验中，我们也测试了在未滤波的地图中松耦合定位结果的性能。经测试，在一些数据中，于未滤波的地图中进行松耦合定位也可以取得较好的定位精度，但是由于未对地图中动态物体进行处理，且未滤除地图中与视觉地图点无关的点集，在一些数据段上无法成功进行整条路径的定位，从而进一步验证了地图滤波算法对提升定位鲁棒性的作用。

1.2.2 基于点、线特征的视觉定位

1.2.2.1 视觉定位问题分析

定位对于机器人自主导航是至关重要的。与基于激光雷达（Light Detection And Ranging，LiDAR）的定位方法相比，视觉定位在许多应用中是有利的，因为低成本且稳定的相机可以被广泛使用。视觉定位的典型工作流程是检测图像中的特征点，将图像特征点与地图点匹配，并根据一组匹配估计位姿。在此过程中几乎无法避免错误的匹配，这会导致不准确的位姿估计。随机抽样一致（Random Sample Consensus，RANSAC）是一种常用于实现鲁棒的位姿估计的方法。然而，RANSAC被限制用于外观严重变化的环境中，在该环境中特征误匹配比例可能会显著增长。因此，对天气、光照或季节变化鲁棒的可靠视觉定位仍然是一个具有挑战性的问题。

两个因素对于RANSAC找到内点是至关重要的，即集合中的内点数量（越多越好）以及估计姿势所需的最小匹配数量（越少越好）。对于第一个因素，一些先前的工作通过利用多种类型的特征（例如点、线和圆）来增加内点的数量，因为在更大图像区域上定义的特征通常会导致对光照变化更高的鲁棒性。对于第二个因素，现有的3D-2D定位方法通常需要3或4对特征匹配，例如三点法（Perspective-Three-Points，P3P）和多点法（Efficient Perspective-n-Point Camera Pose Estimation，EPnP）。本节的主要研究点是：在机器人导航场景中，点和线特征的匹配数可以进一步减少，从而导致对误匹配更高的鲁棒性。

具体而言，在惯性测量单元（Inertial Measurement Unit，IMU）的辅助下，先将预先构建的地图与当前查询图像之间的重力方向进行对准。之后的目标是利用直接观测到的俯仰角和滚转角来将解析估计需要的点特征匹配数量从3减少到2。此外，对于1点和1线的情况也推导出解析解，从而可以考虑线特征匹配。因此，本节提出了一种2实体RANSAC用于持久视觉定位，如图1-26所示，其中绿线表示在黑色预建3D地图定位。理论上，由于2个观测值（2D）可以提供4个自由度（Degree of Freedom，DOF）的约束，因此所提出的解决方案应该是能得出解析解的情况下需要观测值最少的。与现有的2点RANSAC方法相比，2点RANSAC适用于视觉惯性测距或同时定位与映射中的连续姿态估计，本节的方法侧重于3D-2D匹配，而不涉及相对偏航角的测量，使其适用于定位。

这些解决方案用于所提出的点线定位方法，其对于误匹配更加鲁棒，因为线和点特征可以同时使用并且位姿估计所需的匹配数量也会减少。此外，我们引入了三种具有不同优势的特征采样策略，实现了自动选择机制。通过该机制，我们的2实体RANSAC可以适应不同特征类型分布的环境。

1.2.2.2 基于点、线特征的视觉定位算法

本节提出的定位方法包括以下步骤：

Step1：获取当前场景的先验三维地图，其中地图是事先构建的。

Step2：获取机器人的当前图像和惯性传感器的测量数据。

Step3：根据当前惯性传感器数据与所述先验地图中的惯性传感器数据计算当前机器人位姿的俯仰角和翻滚角。

Step4：根据当前图像检测到的二维点、线特征与先验地图中的三维点、线特征进行匹配。

Step5：根据所述匹配到的两对点特征或者一对点特征加一对线特征计算当前机器人位姿的其余未知量。

（1）重力对齐

本节所提的方法对先验三维地图有如下要求：要求包含重力加速度在所述地图坐标系下的测量数据，一般由IMU测量所得，并且建立的地图还要包含三维点、线特征。在定位过程中，机器人采集的当前IMU数据包含重力加速度在当前机器人坐标系下的测量值。根据重力加速度在所述先验地图坐标系下的测量值，将重力加速度在两个坐标下进行对齐，从而得到地图坐标系相对于当前机器人坐标系的俯仰角和翻滚角，具体根据以下算式进行计算

式中，[x_wy_wz_w]^T是重力加速度在地图坐标系下的测量值；[x_cy_cz_c]^T是重力坐标系在当前机器人坐标系下的测量值；β、γ分别是所求的当前机器人坐标系相对于地图坐标系的俯仰角和翻滚角。

（2）位姿解算—2点

本节所提出的方法根据当前图像和地图中的三维点、线特征可以进行二维到三维的点、线特征的匹配。先从获取的当前图像提取多个二维特征点和多条二维线段，然后将当前图像的二维点、线特征与地图中的三维点、线特征进行匹配。衡量匹配成功与否的标准是计算相应描述子的欧式距离，小于一定阈值则认为匹配成功，之后迭代获取多组二维与三维的匹配点、线特征。

根据匹配到的2组二维到三维匹配的点特征，可以计算地图相对于当前机器人坐标系的位姿的其余4DOF未知量，具体步骤如下。

根据地图坐标系{W₀}中匹配到的两个三维点，引入一个中间坐标系{W₁}，其中两个三维点在所述中间坐标系下表示为P₁=（0 0 0）^T、P₂=（X₂Y₂Z₂）^T，而{W₀}到{W₁}的变换则表示为的简单平移，具体为

然后根据当前图像中匹配到的二维点和相机标定的内参矩阵K，可得二维点在机器人相机坐标系{C}下的归一化平面上的三维坐标为

式中，C是相机的光心。

根据图1-27所示的投影几何约束可得三点（C，D₁，RP₁+t）共线，且三点（C，D₂，RP₂+t）共线，其中R为{W₁}相对于{C}的旋转变换矩阵，t为{W₁}相对于{C}的平移变换向量；根据前述所得的{C}相对于{W₀}的俯仰角和翻滚角，可将R简化为

式中，α是未知量，表示为机器人当前相机坐标系相对于地图坐标系的偏航角；设t=[t₁t₂t₃]^T，则全部未知量一共有四个，分别为α、t₁、t₂、t₃。

根据三点（C，D₁，RP₁+t）共线，可得两个方程如下

根据三点（C，D₂，RP₂+t）共线，可得两个方程如下

联立以上四个方程即可求得四个未知量，从而求得机器人在地图中的当前位姿。

（3）位姿解算—1点1线

根据匹配到的1组点特征加1组线特征，可以计算地图相对于当前机器人坐标系的位姿的其余4DOF未知量，具体步骤如下。

根据地图坐标系{W₀}中匹配到的一个三维点和一条三维线段，引入一个中间坐标系{W₁}，其中三维点在所述中间坐标系下表示为P₁=（0 0 0）^T，{W₀}到{W₁}的变换则表示为的简单平移，易得三维线段的端点在所述中间坐标系下表示为L₂=（X₂Y₂Z₂）^T和L₃=（X₃Y₃Z₃）^T。

然后根据当前图像中匹配到的一个二维点和一条二维线段，以及相机标定的内参矩阵K可得二维点和二维线段的端点在相机坐标系{C₀}下的归一化平面上的三维坐标和；引入一个中间坐标系{C₁}，使得相机光心C以及匹配到的点D₁和线段端点的表示如下

中间坐标系{C₁}到{C₀}的变换可由（C⁰，）→（C，D₂，D₃）的变换求得，坐标系变换示意图如图1-28所示；接下来求解中间坐标系中D₁的具体坐标，将投影射线d₁经过变换到中间坐标系下，并延长取其与XY平面的交点即得D₁

根据图1-28所示的投影几何约束可得三点（C，D₁，RP₁+t）共线，四点（C，D₂，D₃，RL₂+t）以及（C，D₂，D₃，RL₃+t）共面，其中R为{W₁}相对于{C₁}的旋转变换矩阵，t为{W₁}相对于{C₁}的平移变换向量。

根据三点（C，D₁，RP₁+t）共线，可得两个方程如下

根据四点（C，D₂，D₃，RL₂+t）共面，可得方程

根据四点（C，D₂，D₃，RL₃+t）共面，可得方程

联立以上四个方程即可求得四个未知量，从而求得机器人在地图中的当前位姿。

1.2.2.3 实验结果

对所提出的方法在仿真数据上进行评估，以证明在视觉定位中优于其他方法。本节验证了本方法对图像特征噪声的精度，对于不准确的俯仰角和滚转角的灵敏度，以及对误匹配的鲁棒性。在实物实验数据上，我们还进行了成功率的比较，以显示所提出方法的有效性。

（1）精度

在模拟实验中，我们在一个立方体中生成了一些3D点和线，并计算了不同相机位姿的2D投影，以获得3D-2D特征匹配。对于每种方法，执行100次RANSAC迭代。在优化中，有两种可能的方式：4DOF或6DOF优化。4DOF优化意味着我们固定VINS提供的俯仰角和滚转角，并且仅优化姿势中的其他四个变量。当内点很少时，4DOF优化十分有效。

在仿真实验评估的最小的解决方案是EPnP、P3P、2P1L和提出的解决方案，包括1P1L-6DOF、1P1L-4DOF、2P-6DOF、2P-4DOF。mixed-6DOF和mixed-4DOF也在对抗误匹配鲁棒性方面进行了评估。

为了量化不同最小解的精度，我们在2D投影上添加了零均值和标准差的高斯噪声，并在四个级别中改变了特征匹配对的数量：10、5、4、3（10组数据意味着在场景中有10对点匹配和10对线匹配），结果如图1-29所示。可以发现，当特征匹配足够时（参见10组数据），随着噪声的标准偏差增加，所提出的方法可以实现与其他方法相同的精度。但特征匹配对的数量减少，与2P1L、EPnP和P3P相比，2实体方法的优势变得更大。

（2）灵敏度

借助IMU的观测，我们利用传感器提供的俯仰角和翻滚角可以将定位问题降低到4DOF。因此，有必要研究俯仰角和翻滚角对最终定位精度的影响。我们在俯仰角和翻滚角上添加了零均值和标准差的高斯噪声，并研究了所提出的方法在三个特征匹配级别中的性能：10、5、3，结果如图1-30所示。当有足够的特征匹配时（参见10组数据），所提出的方法可以在俯仰角和滚转角上容忍近25°的噪声。在这种情况下，6DOF优化的定位精度优于4DOF。但是，随着特征匹配对的数量减少，6DOF和4DOF方法之间的差异变小。这是合理的，因为当内点数很少时，由优化中的附加DOF引起的误差可能大于由噪声位姿估计引起的误差。因此，如果在长期定位的情况下，可靠的特征匹配非常有限，4DOF优化是一个不错的选择。

（3）鲁棒性

为了验证所提方法的鲁棒性，设计了一些实验，通过将内点率从0到80%改变为不同的内部数量级别：10、5、4、3。在每个级别，尽管误匹配率在增加，但内点数量不变。误匹配是通过错误地关联原始数据中的像素而生成的。当估计的位姿结果的平移误差小于10%且旋转误差小于5°时，任务定位成功。为每种方法做了200次实验来平均成功率，结果如图1-31所示。正如预期的那样，当误匹配增加时，所提出的2实体RANSAC优于其他最小解，当内点数减少时更为明显。注意，在具有足够数量内点的情况下，当误匹配率为80%时，所提出的方法可以实现超过90%的成功率。

1.2.3 基于拓扑局部尺度地图的定位

1.2.3.1 基于拓扑局部尺度地图的定位问题分析

对机器人的可靠移动的研究已经有很多年了，其中一个最主要的难点在于机器人定位。为了解决这个问题，其中一类方法是构建一个全局一致的尺度地图，然后利用机器人观测到的传感器数据与地图进行匹配，从而得到机器人位置。由于环境的动态变化，这些方法在机器人的长期运行中存在全局一致性问题。对此，大量研究尝试融合不同时间段的传感器数据，形成一个全局地图，为机器人提供定位服务。这些方法依赖于不同时间段数据之间对齐的精度，这在室外大尺度环境是难以保证的。除此以外，有学者研究利用拓扑结构进行建图和定位，降低了对匹配精度的要求。然而，这些方法没有提供多段数据之间的尺度信息。在某些研究中，尺度信息保存在拓扑结构中，但这类方法依然建立在全局一致性上，当有太多观测误差时仍然存在问题。

在具体实现时，将机器人运行中采集到的数据保存在拓扑地图上。定位时，实时传感器数据与地图邻域内的数据进行匹配，得到的定位结果是相对于局部而言的，从而避免了全局一致性要求。如果定位失败了，则将该数据插入到地图中，并与已有的数据建立拓扑联系。在导航中，通过在拓扑地图上搜索出一条能到达目标点的路径并依此到达，可以保证局部定位误差保持在一定范围内。主要创新点包括：

1）提出了一个基于拓扑结构的局部尺度框架（Topological Local-metric Framework，TLF），将不同时间段的传感器数据组织在一个地图下。

2）设计了针对TLF的定位和导航算法，让机器人在无全局坐标系的前提下自主运行。

3）提出了一个记忆机制，通过在TLF上增加或删除信息，让机器人记录未经过或改变了的地方，从而使机器人拥有不同时间段的地图信息。

4）为了验证算法的有效性和性能，我们将TLF应用于激光雷达和相机，在室外环境中不同时间段进行了21次导航试验。

1.2.3.2 基于拓扑局部尺度地图的定位算法

（1）地图表示

地图可以用图M={N，E}表示，其中N是节点的集合，E是边的集合。对于节点n_i∈N，其属性定义为

n_i={s_i，l_i，N_i}

式中，s_i是由传感器构建的子地图，如点云累积得到的局部激光地图或一帧图像数据；l_i是与传感相关的定位器，可视为函数p_t，i=l_i（d_t，s_i），用于计算子地图s_i和当前传感器测量值d_t之间的相对位姿，如激光中的ICP和图像中的RANSAC；N_i是其他关于该节点的属性，如GPS数据、采集日期等，这些属性可以帮助定位和导航。

边e_i∈E定义为

e_i={p_i，j，E_i，j}

式中，p_i，j是节点n_i和n_j之间的相对位姿，在建图过程中，该位姿一方面可以由里程计数据获得，在局部是精确的；另一方面可以由定位器或全局闭环得到，在局部也是精确的；E_i，j是边的其他属性，例如在视觉导航中，由于相机的观测具有方向性，需要记录节点访问的顺序，即边的遍历方向。当有多段数据或者多个机器人时，这些地图可以通过寻找闭环融合到一起。TLF中会保存不同地图上节点间的相对关系。

（2）相对定位

本文提出的定位算法基于滑窗优化。在定位误差不大的假设下，可以在拓扑地图上寻找出离当前机器人最近的一个节点Q（t），以Q（t）为中心在拓扑地图上找出邻域C_Q。同时，由历史里程计可得到里程计滑窗C_odom。C_Q和C_odom可由定位器给出首位相对位姿，如图1-32所示。在该优化图中，三种边的代价函数定义如下

式中，ρ（·）是robust kernel，用于减少观测误差带来的影响；p是待估计位姿；∑表示加权矩阵，即。E_odom是里程计数据；E_loc是由定位器给出的相对位姿；E_Q是拓扑地图中保存的相对位姿。待估计位姿可由下式给出

（3）基于流形的导航

地图是保存在三维空间中的，但实际上，该拓扑地图可视为局部平滑的二维流形。因此，基于TLF的机器人无法在整个三维空间中运动。基于此，我们提出一个双层的导航算法。在顶层，从机器人当前位置出发，我们可以在拓扑地图上寻找出一条到达目标地点的路径，实际上是一个节点序列H= {h_i}。在下层，每当机器人到达了H中的一个节点，该节点就从H中移除，并按顺序确定下一个目标点。不难证明，该方法能保证对最终目标点的估计误差会越来越小。流形导航示意图如图1-33所示。

（4）记忆机制

为了提高在不同时间的定位成功率，需要不断丰富地图的信息。本书提出的记忆机制如图1-34所示。当重新定位后（定位失败一段时间后全局定位成功），无法与地图匹配的数据会被加入到地图中（包括定位失败前最后一次定位成功的节点和最新一次定位成功的节点）。下次运行时，新增的数据即可用于定位，从而提高定位成功率。

图1-34中，灰色点是地图点，绿色点是定位成功的点，红色点是定位失败的点，绿色边则表示一次成功的定位。为了保持地图规模在合理范围内，还需要对地图进行删减。地图维护针对的目标是动态变化的，如移动的车辆等。这类变化是不可持续的，在以后的定位中无法提供有效信息，因此需要删除。我们可以统计每个节点定位成功和失败的次数，算出该节点定位成功率的变化趋势。定位成功率稳定下降的节点对应的就是该类节点，应该从地图上删除。

1.2.3.3 实验结果

实验采用一个四轮移动机器人，机器人上安装了一个ZED双目相机和一个16线激光雷达VLP-16，如图1-35所示。在三天中的不同时间段，我们用该平台在校园里同一段路上运行了21次，每次运行约1km（图1-36），共23km。

为了验证记忆机制的作用，我们分别统计了有/无记忆机制下的定位成功率，如图1-37所示。

在记忆机制的作用下，定位成功率明显增加。图中s1～s8、s9～s16、s17～s21分别采集于第一、二、三天。没有记忆机制时，每天第一次运行的定位成功率是最高的，说明该时段与原地图最相似。s16的定位成功率最低，这是因为已经进入晚上，光照变化最大。在三天中，没有记忆机制的定位成功率呈现了类似的趋势，与我们的采集时间吻合。

每次运行结束后地图节点数如图1-38所示。在第一天结束后，地图维护机制开始生效。随着机器人运行次数增加，每次增加的节点数呈下降趋势，偶尔会不增反减。在s16结束后，节点出现了较大的增加，这是因为s16观测到了晚上的图像，这是之前没有经历过的。总体而言，地图规模缓慢增加，并趋于稳定。

图1-39所示为算法的计算时间。除了首次运行，运行时间均在每帧0.1s左右，该速度能满足机器人的导航需求。

1.2.4 机器人可行域检测

1.2.4.1 可行域检测问题分析

无结构化环境中的可行域分割是自动驾驶中不可或缺的技术，也是安全导航和行为控制的基础。在现有方案中，昂贵的激光雷达等主动传感器由于数据稀疏，难以检测出较小的障碍物。因此，在路面感知研究中，低成本的相机受到了越来越多的关注。近年来，深度神经网络在各个领域均取得了不小的突破。其中，用于语义分割的卷积神经网络进入了实用阶段，为无结构化环境中的可行域分割带来了可能性。

当前，大部分高级驾驶辅助系统（ADAS）局限在路面环境，主要完成车道线保持任务。这些系统依赖车道线检测、交通标志识别等技术，无法直接用于非结构化环境。基于监督学习的语义分割方法在可行域分割上取得了不错的效果，但由于这类方法需要大量带标签数据以及大量人力，难以获得海量数据。为了减少人工过程，有人提出了一种自学习的方法，通过给定初始地面假设，不断扩大可行域，但这种在线学习的方法无法利用已有数据。之后又有人提出了一种弱监督方法来生成标签数据，但他们标注的可行域太窄，限制了机器人的运动空间。同时，在有不同前进方向的分岔路口处，该方法会失效。

本节在先前工作的基础上提出了一个用于多时段多传感器的融合框架，并应用于自动生成带标签数据，为可行域分割模型提供训练数据。我们可以使用这些标签数据训练一个语义分割深度卷积神经网络。在训练阶段，该方法只需要一个激光和单目相机；在部署阶段，该方法只需要一个单目相机。

在采集到多段数据后，通过构建全局激光地图，定义一个全局坐标，并将数据组织在该框架下。每帧激光的位姿由激光在地图中定位给出，通过矩阵的链式法则合一得到其他传感器的位姿。

1.2.4.2 可行域检测算法

我们提出了一个数据融合框架与地图，该框架可以融合不同空间、不同时间、不同传感器的数据，如图1-40所示。融合的关键在于如何将数据表达在同一个框架下。首先，借助某个时段采集到的激光数据，构建一个全局地图M。对于从激光传感器L在t_i刻采集到的激光数据，可以用ICP定位算法得到该帧数据在M中的全局位姿。其中，t_i包含两部分信息：一是该帧数据来自于哪段数据；二是来自该段的哪个时间点。

该框架还支持多传感器配置。框架还可以融合激光数据以外的信息，如相机得到的图像数据。本方法假设相机C和激光L的相对位姿固定，可以由标定给出。后续算法中，常常要知道不同时刻的图像数据间的相对位姿。对于t_i刻和t_j刻的图像数据和，它们之间的相对位姿可以由对应激光的定位结果和激光-相机标定结果给出，即

算法涉及的其余定义如下：在t_i时刻采集到的点云上的第k个点记为；t_j刻车轮和地面的接触点记为，其中l和r分别表示左轮和右轮，接触点的位置可以由标定或者测量给出。

本方法将路面分为三种语义，即可行域、障碍物区域和未知区域。我们用一个移动机器人在校园里的同一条道路上采集了不同时间段的传感器数据。方法的核心即假设机器人走过的地方是可行域；地面以上的激光点是障碍物。标签数据生成过程如下：

1）可行域：本方法提出的可行域是基于先前提出的“候选道路”的概念的。在第s段数据上，t_i刻的“候选道路”P_s是未来机器人将要经过的路径在当前图像上的投影。这条路径是由机器人轮子与地面接触点形成的轨迹决定的，并且是在相机坐标系下的，它在图像上的投影可以表示为

式中，是上文提到的激光定位结果；是激光-相机标定结果；K是相机内参。

在此定义下，“候选道路”P_s可以表示为一个顶点如下的多边形

对于每段数据，我们可以得到一条“候选道路”P_s。在采集到多段数据后，通过融合多条“候选道路”，我们可以得到更宽的可行域。可行域融合采用图像上的“并”操作。图1-40是该融合过程的一个例子，分别展示了融合1、2、3、4、5和13段数据后的可行域。可以看到，可行域越来越宽阔。我们的融合方法收益与统一的数据框架，先前提出的基于局部的算法无法有效利用多段数据。

2）障碍物区域：障碍物区域表示车辆无法通过的地方。上面提出的可行域标签生成方法存在一定瑕疵，在边界的地方容易产生歧义。如果直接将可行域以外的地方标记为障碍物，会影响实际避障效果。因此，找出障碍物区域尤为重要。

在我们的传感器配置中，激光可以检测出周围环境的结构，因此可以通过激光检测障碍物。激光点投影为，像素点上方的像素标记为障碍物。为了避免将地面点云标记为障碍物，还需要对地面进行平面拟合。我们在当前点云上进行平面拟合，将距离平面小于0.25m的点从点云中删除，删除后的点云按照上述步骤在图像上标记障碍物。由于激光的稀疏性，我们还对图像进行了形态学膨胀操作，以填补空洞。为了行驶安全，障碍物优先级高于可行域，即如果一个点同时被标记为二者，则该点视为障碍物。

3）未知区域：上述自动标记算法完成后，有一些像素会没有标签。这是因为有些地面既没有被车辆行驶过，也没有激光点扫描到。对于这些区域，我们将其标记为未知区域。

上述方法可以产生大量标签数据，数据的数量取决于采集车行走的路程。理论上这些数据适用于任意的语义分割网络。在实际部署中，将单目相机得到的图像数据输入网络，即可获得可行域预测结果。本文采用DeepLab作为像素级别分类器，该网络在精度和效率上均有不错的表现。与先前类似的我们还对角度进行了直方图采样，来保证数据的平衡性。

1.2.5 面向智能巡航的多传感器复合导航

1.2.5.1 多传感器复合导航问题分析

广义定义导航是监督和控制一架航空器或者车辆从一个地方到另一个地方移动的过程。对于旋翼飞行机器人系统来说，导航可以定义为数据获取、数据分析和提取以及关于机体状态和其周边环境信息的推测，并且成功和安全地完成分配的任务。根据一个导航系统的自主等级功能，从低到高可以分为：

1）传感：一种传感系统涉及一个或者一组传感器，这些传感器对特定的物理现象或者激励做出反应并且产生反应目标物或者物理现象的一些特征或者信息。旋翼飞行机器人系统机载常用的传感器有陀螺仪、加速度计、地磁计、静态和动态压力传感器、摄像头、激光雷达等。这些传感器可以为状态估计和感知算法提供原始的测量信息。

2）状态估计：状态估计主要涉及原始传感测量值的处理，从而估计与机体相关的状态变量，特别是其相关的位置和运动，例如姿态、位置和速度。这些估计可以是绝对的或者相对的，定位是一种局限于关联地图或其他地点的位置估计的特殊情况。

3）感知：旋翼移动类机器人系统感知是能够使用来自传感器的输入信息构建机体操作环境的内部模型，并且可以分配实体、事件，能够根据感知的环境状况来分类。分类/识别处理涉及旋翼飞行机器人系统观测到的信息与先验信息进行对比。感知根据不同的层级进一步可以分为多种功能，例如建图、避障、目标检测和目标识别。

4）航行态势感知：态势感知的概念通常用于航空领域并且关于态势感知的定义有很多。在本次调研报告中，我们采用“一定时间和空间环境中的元素的感知，对它们的含义的理解，并对他们稍后状态的投影（恩兹利，1999）”作为态势感知的定义。态势感知比感知更高一层，因为它需要对形势进行理解然后对未来一段时间的信息进行推断或者投影来决定将如何影响操作环境的未来状态。

上述分级导航内容的整体结构图如图1-41所示。

按照导航采用传感器的不同，将导航分为惯性导航、全球导航卫星系统（GNSS）、惯性导航系统（INS）和GNSS复合导航、视觉导航、激光导航、多传感器融合导航、感知导航系统等。基于IMU/GNSS、IMU/激光雷达的传统导航方式应用最为普遍，目前已基本成熟。高校、科研机构及各大飞行机器人企业把研发重心放在了视觉、多传感器融合导航等其他导航方式上。

1.2.5.2 多传感器融合导航

单一的导航方式都有着各自的优缺点，多传感器融合才能得到更加可靠的导航性能。典型的多传感器融合导航系统由IMU、GPS、视觉、激光雷达等构成，可以实现各种复杂环境下的自主飞行。千叶大学在该种导航系统的基础上，实现了核泄漏现场的飞行机器人侦察。麻省理工学院（MIT）也多次公开发布了室内外自由移动的飞行机器人融合导航科研成果。此外，宾夕法尼亚大学、密歇根理工大学、卡内基·梅隆大学、哈尔滨工程大学等高校也在研究应用于飞行机器人导航的多传感器融合技术，如图1-42和图1-43所示。国际飞行机器人赛事也把该项内容列为主要比赛项目。

（1）复合导航飞行实验平台

本项研究定制的复合导航飞行实验平台（图1-44）搭载有三维激光雷达、视觉传感器、GNSS接收机/实时动态（RTK）载波相位差分技术、地磁计、惯性测量单元（IMU）、气压计等传感器，通过融合以上多种传感器来实现位置复杂多种环境下的自主导航。

（2）硬件系统

复合导航板微处理器采用STM32F4系列处理器，提供了工作频率为168MHz的Cortex^TM_M4内核（具有浮点单元）的性能，闪存高达1MB、具有先进互联功能和加密功能，可用于复合导航多种传感器信号中转、计算，可接入IMU、RGB-D相机、激光雷达、光流传感器等多种传感器，能同时实现4路串口数据收发，2路CAN接口数据收发，另外预留了1 路IIC接口。支持DC5～12V的电源输入，支持DC3.3V/1A的电源输出。每路的数据输入输出口均有指示灯提示。为了避免误插，接口采用防接错设计。复合导航印制电路板（PCB）如图1-45所示。

（3）多传感器数据融合架构

多传感器数据融合架构图如图1-46所示。一级模块包含模块1、模块2，一级融合模块1融合加速度计、陀螺仪、地磁计、光流传感器和单线激光雷达输出Roll、Pitch、Yaw及三轴加速度、角速度数据、位置、速度和高度数据；一级融合模块2使用3D激光雷达/视觉传感器通过融合估计得到的三维位置数据。二级融合模块的数据来源于一级融合模块，采用EKF多传感器融合方法，程序独立运行于多传感器数据融合板（MSDFB）中。

（4）GNSS/IMU融合导航

通过EKF融合GNSS（BDS/GPS）接收机、姿态角、加速度等数据输出位姿信息，该位姿信息可以用于室外环境下的自主导航，通过将融合速度数据与原始速度数据对比来验证算法的有效性，如图1-47所示。其中，红色实线为融合后的，蓝色虚线为原始数据。

对比融合经纬度数据与原始经纬度数据来验证算法的有效性，如图1-48所示，其中红色实线为融合后的，蓝色虚线为原始数据。