本发明涉及视觉slam导航，尤其涉及一种基于恒定加速度预测和pnp-epipolar优化的自动驾驶slam导航方法。
背景技术：
1、近年来，研究人员提出了不同类型的视觉slam导航方案，每种方法都有其特点，并且适用于不同的应用场景。大部分vslam算法都采用orb-slam2作为基本框架，在orb-slam2跟踪线程中，恒速模型属于跟踪模型的第一个阶段且是最常使用的跟踪方法，直接影响着整个系统的定位精度，其简单、快速的特点对实时性要求较高的slam系统具有重要意义。然而恒速模型过于理想化，忽略了对象位姿加速度且仅考虑了上一帧的信息，虽然在相邻帧间极短的时间内，可以合理的假设相机处于匀速运动状态，但面对自动驾驶车辆，在剧烈运动场景下，会经历剧烈变速和大角度转弯等运动，如果全程采用恒速模型跟踪，一方面车辆初始位姿估计不准确，另一方面3d与2d匹配数目减少，这些会共同导致slam定位精度下降甚至定位失败，尽管后续可以采用参考关键帧和重定位跟踪进行补救，但这也大大降低了slam系统的实时性。
2、因此，本发明基于orb-slam2框架，改进恒速跟踪模型，研究一种基于恒定加速度预测和pnp-epipolar优化的自动驾驶slam导航方法，该算法可以根据车辆所处的运动状态，在匀速、匀变速运动模型中进行切换，当车辆处于匀速运动时，采用匀速跟踪模型跟踪；当车辆处于变速或大角度转弯等运动变化较大场景时，采用鲁棒跟踪模型跟踪，该模型包含3个步骤：匀变速策略预测初始位姿、全局特征匹配和pnp-epipolar优化模型。首先根据上两帧信息得到的匀变速策略预测当前帧的初试位姿，解决初始位姿不准确的问题；然后在投影匹配的同时增加词袋匹配对同一相邻两帧之间进行3d-2d和2d-2d的全局特征点匹配，以增加特征匹配数量；最后在pnp约束的基础上融合极线约束设计了一种pnp-epipolar优化模型联合优化位姿。本发明提出的基于恒定加速度预测和pnp-epipolar优化的自动驾驶slam导航方法，为视觉slam应用于自动驾驶车辆提供了理论指导和解决方案。
技术实现思路
1、本发明针对现有技术的不足，提出了一种基于恒定加速度预测和pnp-epipolar优化的自动驾驶slam导航方法，提高导航的精度。
2、本发明采用下述技术方案实现：
3、基于恒定加速度预测和pnp-epipolar优化的自动驾驶slam导航方法，该方法采用下述步骤实现：
4、步骤一，车载传感器对输入图像进行预处理，然后通过参考关键帧跟踪方式对前几帧进行跟踪并获取图像帧的位姿速度；
5、步骤二，根据匀速运动假设，采用恒速运动模型估计当前帧车辆初始位姿，然后通过地图点投影进行特征匹配，最后根据pnp模型对位姿进行优化
6、步骤三，根据匹配数量判断车辆是否处于剧烈运动状态，若是，则采用本发明提出的鲁棒跟踪模型进行位姿跟踪，包含恒定加速度预测、全局特征匹配和pnp-epipolar优化三个阶段；若否，则直接进入局部地图跟踪阶段；
7、步骤四，当采用本发明提出的鲁棒跟踪模型进行位姿跟踪，首先采用匀加速运动估计车辆初始位姿，提升剧烈运动状态下车辆位姿估计的准确性和鲁棒性；
8、步骤五，其次采用全局特征匹配方法进行特征匹配，该匹配方法同时考虑了投影匹配和词袋匹配，能有效提高剧烈运动状态下特征匹配数量；
9、步骤六，然后采用pnp-epipolar模型优化位姿，该优化模型同时考虑了重投影约束与极线约束，提升了剧烈运动状态下车辆位姿优化的准确性和鲁棒性。
10、本发明的有效效果：一、本发明采用恒加速运动模型来估计当前帧初始位姿，有效的提高了剧烈运动状态下车辆位姿估计的准确性。二、本发明采用了全局特征匹配，显著提升了低重叠情况下连续帧间特征匹配数量。三、本发明根据全局特征匹配结果，采用pnp-epipolar模型优化位姿，对于处于剧烈运动状态下的自动驾驶车辆，该优化方案提高了slam定位的准确性和鲁棒性。
11、本发明的效果通过如下实验得到验证：
12、本发明提出的方法与orb-slam2、structure plp-slam和openvslam进行比较。本发明已经在公共kitti数据集和euroc mav数据集中评估了所提出的方法。为了使估计的轨迹与地面实况对齐，本发明使用了horn’s的方法。本发明通过绝对位姿误差和绝对轨迹误差来评估导航的准确性。绝对位姿误差和绝对轨迹误差由evo工具计算。本发明采用绝对轨迹均方根误差进行定量评估。绝对轨迹误差比较估计和地面实况轨迹的平移分量之间的绝对距离。在时间步i处的绝对轨迹误差计算公式如下：
13、
14、其中，t表示对齐两个轨迹的变换；g表示真实地面；e表示预估的轨迹。
15、对于n个姿势的序列，绝对轨迹误差的绝对轨迹均方根误差的计算公式如下：
16、
17、考虑到系统的不确定性，所有值都是通过五次实验获得的，并显示了估计轨迹准确性的中值结果。本发明使用kitti数据集来评估所提出的方法，该数据集适用于评估视觉惯性slam在自动驾驶车辆中的性能。在这个数据集中，为序列00-10提供了真实地面。为了提供准确的地面实况，对激光雷达、摄像头和gps进行校准和同步。kitti数据集是从现实世界中的各种场景数据中收集的，例如农村、高速公路和城市。
18、在kitti数据集和euroc mav数据集上的实验表明，本发明提出的方法与orb-slam2、structure plp-slam和openvslam相比，将定位精度提高了约27％，甚至在某些序列，定位精度提高了约50％。对比实验结果表明，本方法在车辆处于剧烈运动和低重叠时时提高了导航系统的准确性和鲁棒性。
1.一种基于恒定加速度预测和pnp-epipolar优化的自动驾驶slam导航方法，其特征在于，包括步骤：
2.根据权利要求1所述基于恒定加速度预测和pnp-epipolar优化的自动驾驶slam导航方法，其特征在于：所述步骤一中，具体步骤包括：
3.根据权利要求1所述基于恒定加速度预测和pnp-epipolar优化的自动驾驶slam导航方法，其特征在于：所述步骤二中，具体步骤包括：
4.根据权利要求1所述基于恒定加速度预测和pnp-epipolar优化的自动驾驶slam导航方法，其特征在于：所述步骤三中，具体步骤包括：
5.根据权利要求1所述基于恒定加速度预测和pnp-epipolar优化的自动驾驶slam导航方法，其特征在于：所述步骤四中，具体步骤包括：
6.根据权利要求1所述基于恒定加速度预测和pnp-epipolar优化的自动驾驶slam导航方法，其特征在于：所述步骤五中，具体步骤包括：
7.根据权利要求1所述基于恒定加速度预测和pnp-epipolar优化的自动驾驶slam导航方法，其特征在于：所述步骤六中，具体步骤包括：
