我们致力于基于环境认知地图的具有类生物导航能力的“类脑导航”研究，专注于“自主环境感知”、“空间定位推理”和“面向目标的主动导航”等关键技术，重点突破在陌生环境识别、大范围地图构建及环境主动探索方面的瓶颈问题。通过模仿生物对环境的认知和定位导航行为，基于大脑对抽象知识编码与空间位置的表示方式构建场景认知地图，实现陌生环境中不同尺度地理位置的快速探索和认知，形成高效率、高稳健的主动定位导航系统。
主动探索与认知的类脑导航系统架构
目前实验室类脑导航方面的成果如下：
1. 多源融合类脑定位与导航
为实现主动感知和场景认知，需深入理解导航系统的地图特性及导航策略，因此以传统导航系统为切入点开展了多源融合定位与导航方法的研究。针对不同类型的应用场景，在定位与导航硬件平台、算法运行效率、定位精度和导航鲁棒性等方面开展了大量研究，具体包括视觉导航平台、视觉定位与导航、水下视觉惯性融合导航、视觉-惯性-卫星融合导航以及激光定位与导航等。
1.1 定位与导航硬件平台
基于自主机器人的高精度定位数据采集平台的设计与研究
在定位算法的研究中，数据采集与标定往往会消耗大量人力与时间，为解决该问题，自主研发了基于自主机器人的高精度定位数据采集平台。该平台利用多源传感器融合定位算法、全局路径规划技术等，实现了对室内外位置环境的建图、自主导航数据采集以及高精度轨迹真值生成等功能，达到复杂场景下小于5cm的定位精度以及自主数据采集的功能。平台使用了基于SLAM轨迹拓扑地图和球形VFH避障系统以实现在复杂环境下的自主导航避障以及数据采集任务，并开发了人机界面方便开发人员进行数据采集。
自主机器人高精度定位数据采集平台 人机操作界面
可重配置载体的多传感器数据采集平台
在自动驾驶、地面机器人等研究领域中，数据集采集测试是研究的重要组成。针对这种需求，我们设计了一种可适用于多种载体的多传感器数据采集平台，本平台具有以下特点:
（1）多载体适应性：可用于手持、机载、车载等方式进行数据采集，提升采集效率，亦可方便研究人员针对不同平台同种传感器表现进行研究。
（2）自持性：本测试设备具备不依赖任何外部设备即可启动、采集与与运算的能力，提升了数据采集的效率与灵活性。
（3）高本地计算能力：搭载了桌面级i7处理器与GTX1070显卡，可实现实时神经网络计算，提高算法调试能力。
本系统的顶层设计架构
1.2 定位精度和导航鲁棒性
考虑空气-水-防水壳折射的水下相机模型
考虑水下环境特点，在预处理数据的时候，摄像头始终放置在防水外壳中，因此由水、防水外壳和空气之间的折射引起的额外失真被引入系统。在标定水下相机时，传统的标定方法将水-空气折射畸变视为引入额外误差的镜头畸变。我们设计了一种新的水下图像校正水下相机标定模型，利用近似的单视角相机模型分别校正水气折射畸变和镜头畸变。
水下相机去畸变模型
针孔相机模型和我们提出的水下相机模型的平均重投影误差如下图所示，提出的水下相机模型的总体平均误差为0.15像素，比针孔相机模型的总体平均误差小11.7%。
普通针孔相机模型和提出的水下相机模型的反投影误差对比
高精度水下视觉-惯性里程计数据集
视觉-惯性里程计被越来越多的研究者应用于水下定位研究，然而由于在开放海域获取轨迹真值难度较高，现有的水下定位数据集通常缺乏高精度且完整的轨迹真值，一定程度上限制了视觉惯性里程计在水下定位领域的应用。为解决这一问题，我们提出了一个拥有高精度、完整轨迹真值的水下数据集。
数据集采集环境示意图
车载行人定位技术
该技术的实现包括行人检测部分、行人定位部分及地图映射部分。行人检测部分中，系统将采集的图像输入到网络中并利用深度学习算法计算出道路中行人的边界框坐标及行人的数量。行人定位算法接收行人检测算法的结果，结合原始图像计算得到行人相对于车辆的深度距离及角度。最后，地图映射算法接收到行人定位算法的结果并且结合车辆的GPS信息计算出行人的GPS信息并将其信息映射到车载地图上，具体的架构图如下。
系统架构图
两种图像情况下的实验结果如下图所示：
普通摄像头的行人识别与深度估计结果
行车记录仪的行人识别与深度估计结果
视觉地点识别技术
视觉地点识别所面临的两大挑战分别是环境变化和视角变化问题，为此我们使用神经网络作为特征描述子，同时通过物体检测算法构建局部标志物拓扑关系检测，将两者的输出进行融合比对，从而增加算法识别的鲁棒性。训练得到的网络用作提取视觉特征的描述器，因此首先需要训练出准确率较高的网络模型，然后再将此模型应用到定位导航的系统中，最终训练得到的网络用于构建定位系统。
整体架构分为离线和在线两部分，离线与在线模式的算法架构一致。离线模式下，通过网络前向传播与编码压缩的过程，将地图库中的图片提取为特征向量语义库；在线模式下，将输入的查询图片提取为特征向量，并与地图库中的数据进行匹配，筛选出最接近的若干输出。
单点定位功能算法框架
为验证算法的有效性，选取上海交通大学构建了完整的数据集，包括1700多个点总计6万多张图片，覆盖了交大校园的每个角落。
交大测试结果示意图
1.3 视觉-惯性-卫星融合定位
面向智能手机的惯导融合差分定位技术
GNSS导航得到了很大的发展，但相比于专业级设备，低成本设备的性能并不能满足市场需求。对于米级精密定位，广泛应用是利用预定数据库的wifi信号，但理想的解决方案是采用智能手机原始GNSS数据和惯性测量单元（IMU）等传感器。在本工作中，我们使用智能手机IMU来协助定位系统。基于短基线假设，流动站基线方向矢量可以被视为平行线。在不超过10公里的短基线上忽略了电离层和对流层。智能手机时钟错误是一个不稳定的因素，因此采用双重差异来消除智能手机时钟错误。为了交换，RTD将在观察中牺牲一个方程，5个可见卫星是必不可少的。为使运动位置解收敛，探索了一种辅助RTD方法，通过使用基于MEMS的IMU来更新速度状态。实验平台如下，参考溶液由u-blox m8p RTK模型提供。智能手机和u-blox m8n安装在手持平台上：
实验手持平台
比较结果表明，我们的PDD+IMU的方案可以有效提高定位精度。实验结果如下：
地图映射定位结果
高程误差曲线 水平误差曲线
PDR/GNSS融合定位算法研究
基于智能手机的行人航位推算（PDR）由于受限于MEMS器件的低性能，其航向与步长估计会随着时间增长产生显著的积累误差，从而限制了PDR的实际应用。考虑到室外应用时，手机的GNSS能够提供绝对位置信息，而且两种不同的定位方式的航向与步长误差具有不同的特征，因此，本研究充分分析了两种不同类型误差的特性，并提出一种可以实时融合PDR和GNSS信息的融合定位框架，从而给出最优的航向与步长估计。实验结果表明，融合算法相比于PDR或GNSS在偏航程度、抗干扰以及噪声性能等方面都具有优越性。
基于图优化的GNSS与视觉紧耦合算法
在定位导航功能问题中，传感器融合是常见的方法之一。传感器的互补特性可以提升实际应用的适应性及准确性。在所有的常用传感器中，GNSS与视觉传感器具有显着的互补特性。因此研究GNSS与视觉传感器的融合问题对提升导航精度与稳定性有着显著的意义。我们提出了将GNSS与视觉SLAM系统进行紧耦合联合解算的方法，将GNSS卫星与视觉特征点约束应用于整体的代价函数。算法示意图如下。
视觉特征点与卫星点的共同约束下的定位解算 GNSS视觉联合解算图优化框架
相比于传统单纯的视觉算法与GNSS定位算法，本方法提供了对大尺度下对于视觉尺寸漂移的修正，并且由于视觉的局部高精度提升了GNSS的定位精度。实验平台及测试结果如下：
试验平台及测试场景 实验数据结果
2. 语义地图构建技术
针对机器人定位、路径规划和避障过程中对不同尺度地图的特定要求，探索全局尺度和局部尺度的语义地图构建技术。在全局尺度下，研究了基于视觉特征点 SLAM 的拓扑地图构建方法；同时，研究了基于物体三维包裹盒的语义地图构建方法如图，大幅提高了视角变化条件下回环与重定位准确率。在局部尺度下，研究了基于语义高程信息的局部可通行地图构建和避障算法，融合物体的三维空间信息和材质语义信息实现了区域可通行度的可靠评估。另外，还研究了基于航拍图像的三维语义重建系统，以提高大尺度场景下建图效率。
基于语义物体建图的鲁棒闭环检测算法
视觉同步定位和建图（SLAM）被广泛用于机器人领域的相机定位和三维重建。由于相机成像系统的噪声和物体外观的变化，要实现准确、鲁棒的定位存在一些关键性的问题。特别在相机视角变化较大的场景下，对于基于局部外观特征的视觉SLAM仍然是巨大挑战。我们提出了基于三维物体层面语义建图的闭环检测方法。首先利用基于卷积神经网络的目标检测算法提取环境中物体层面特征并将其表示为三维立方体；然后利用物体空间约束生成高质量的三维立方体语义地图。在此基础上，提出了一种基于邻域拓扑关系的三维立方体匹配方法，以此在相机大视角变化下实现鲁棒的闭环检测算法。最后将该算法集成在经典的视觉SLAM框架中，并进行定位漂移的校正。在KITTI数据集上进行实验，表明提出的方法比其他现有方法具有更好的定位精度。此外，所提出的方法在自采集的数据集上也实现了更准确、更鲁棒且召回率更高的闭环检测。
基于语义物体建图的闭环检测系统框架 语义物体地图
与其他方法的定位估计误差对比
基于航拍图像的三维语义重建系统
为提高大尺度场景下建图效率，使用无人机重建三维地图并将其直接应用于地面无人车进行导航，构建了无人机-无人车空地一体化的建图-导航系统。整体系统分为两大部分：一部分是基于航拍图像的三维重建系统；另一部分是地面无人车导航系统，整个系统实现了基于无人机建图的无人车导航功能。其中，基于航拍图像的三维语义重建系统由稀疏重建、稠密重建、语义融合、拓扑提取四部分组成。无人车导航系统包括局部规划、全局规划和局部避障三部分模块。
空地一体化建图-导航系统结构
系统实机测试环境如下图所示，屏幕左上角为RGBD相机的RGB图像以及局部高程地图模块构造的实时地图，左下角为局部规划模块计算出的局部目的地，右上角为实验实时录像，右下角为三维语义拓扑地图。
实机测试场景
3.主动探索建图与面向目标的导航
主动探索建图和面向目标的导航具有相似性，但它们的驱动力不同，主动环境探索受认知范围的扩充驱动，面向目标的导航受设定的目标驱动。目前实验室在这一领域开展的研究如下：
基于先验拓扑图的主动探索建图
针对地下矿洞探索、废墟救援、外星登陆等任务，对移动机器人高效探索的迫切需求，采用基于环境先验拓扑图有全局规划的探索策略。面对未知起点无法预先进行全局规划的挑战，研究了基于拓扑图匹配的主动定位与建图技术，提出了 1.5 阶图匹配算法和基于降维状态的主动定位策略，将拓扑图匹配定位和基于拓扑图的主动定位决策运行时间分别降至约 20%和 10%。
（a）分析先验拓扑图（左：先验拓扑图；右：待探索环境）
（b）实时决策最优主动定位行动
基于先验拓扑图的主动探索建图策略
基于激光的强化学习避障算法
基于强化学习模型Deep Deterministic Policy Gradient(DDPG)构建避障算法。模型输入是维度压缩的单线激光、目的地坐标和小车速度，输出小车线速度和角速度。首先，引入一个辅助训练机制保证训练速度，利用DDPG的critic网络选择网络输出的行为和简单控制器的输出行为中Q值较高的行为，增加训练过程中好行为的出现，提高训练效果；然后，提出镜像经验回放机制提高训练效果。考虑到单线激光、目的地、速度、小车控制量等信息都是左右对称的，可根据这一特性将训练的经验镜像复制，提高经验获得效率。最后，添加了一个子目标生成模块以降低避障难度。如果目的地隐藏在一个大型障碍物后，可能模型会陷入某些局部最低点而不移动，我们用一个简化传统避障算法给出一个可视范围内的目标点替代原目的地，可有效降低陷入局部最低点的情况，提高避障成功率。
模型框图
实验场景