一种基于运动状态与环境感知的多源融合导航定位方法

1.本发明属于卫星定位导航技术领域，尤其涉及一种基于运动状态与环境感知的多源融合导航定位方法。


背景技术：

2.城市环境中智能交通正在飞速发展，自动驾驶技术已经为人类生活提供了诸多便利。导航定位技术是决定车辆定位高精度、连续性、可靠性的关键，也是路径规划和车辆控制的重要前提。gnss是导航定位的重要传感器，在空旷环境可以提供高精度的时空信息，但在城市峡谷、高架桥、隧道等城市复杂环境下，gnss信号阻塞和遮挡会导致定位精度下降甚至缺失。借助单一的传感器难以在复杂多变的环境中保持良好的定位精度，因此需要通过利用多种传感器各自的优势，弥补缺陷与不足。当前，多传感器融合导航是解决城市环境中gnss性能下降的主要研究方向，gnss/ins组合导航是应用最广泛的组合导航系统，但长时间gnss处于遮挡区域仅依靠惯性导航系统仍会产生较大误差，因此需要其他传感器来减小短时间内惯导的漂移，相机能够通过获取周围环境的图像进行环境感知和视觉信息获取，且具有不易受到干扰、价格低廉等优点，是理想的车载传感器。张奕然等人提出了一种通过双目视觉利用路标的地理位置建立路标数据库，对gnss定位精度进行局部改良的方法，但只是用相机辅助gnss，没有将相机作为主要传感器进行组合导航，薛伟罗晨等人提出了一种基于联邦滤波器的视觉辅助的ins/gnss导航系统，用ins分别和gnss和视觉进行松组合，但当观测信息和数据质量不佳时，组合导航的性能将会受到影响。因此，进行具备可靠性和鲁棒性的gnss/imu/视觉组合导航系统的研究有巨大的发展前景。
3.术语解释：gnss
ꢀꢀꢀꢀ
全球导航卫星系统 （global navigation satellite system）imu
ꢀꢀꢀꢀꢀ
惯性测量元件 （inertial measurement unit）ins
ꢀꢀꢀꢀꢀꢀ
惯性导航系统（inertial navigation system）vio
ꢀꢀꢀꢀꢀꢀ
视觉惯性里程计（visual inertial odometry）slam
ꢀꢀꢀꢀ
同步定位与建图（simultaneous localization and mapping）vo
ꢀꢀꢀꢀꢀꢀꢀ
视觉里程计（visual odometry）bn
ꢀꢀꢀꢀꢀꢀꢀ
批量归一化（batch normalization）ecef
ꢀꢀꢀꢀꢀ
地心地固（earth-centered, earth-fixed）klt
ꢀꢀꢀꢀꢀꢀ
稀疏光流跟踪法（kanade-lucas-tomasi tracking method）ekf
ꢀꢀꢀꢀꢀꢀ
扩展卡尔曼滤波 (extended kalman filter)。


技术实现要素：

4.发明目的：本发明所要解决的技术问题是针对现有技术的不足，提供一种基于运动状态与环境感知的多源融合导航定位方法。
5.为了解决上述技术问题，本发明公开了一种基于运动状态与环境感知的多源融合
导航定位方法，包括以下步骤。
6.步骤1，使用相机采集图像帧，剔除图像帧中的动态点，获得每帧图像的静态特征点及其位置。
7.步骤2，基于每帧图像的特征点位置以及视觉和imu紧组合融合定位算法，获得局部相机位姿。
8.步骤3，基于imu测量数据，获得车辆运动状态；根据车辆运动状态进行自适应算法的切换。
9.步骤4，基于局部相机位姿、自适应算法的切换结果、卡尔曼滤波以及gnss、imu和视觉松组合融合定位算法，获得最终定位结果。
10.进一步地，步骤1包括步骤1.1，对图像帧中每一帧图像的行人和车辆进行语义分割，对于相机采集的图像帧合集opic，先用klt稀疏光流法分别提取并追踪每帧图像的特征点，获得特征点位置，即特征点的像素坐标，并基于卷积神经网络进行车辆和行人的语义分割，获得每帧图像属于车辆或行人语义类别的特征点。
11.klt稀疏光流法提取特征点，并跟踪同一像素点在像素坐标中的位置变化，是后续位姿估计的基础。而通过语义分割与动态点剔除，可以去掉那些“人”和“车辆”的动态的特征点，一是减小动态点对最终位姿估计的负面影响，二是可以减小稀疏光流法跟踪的点的个数，提高运算效率。
12.步骤1.2，对imu测量数据进行预积分，获得imu预积分结果，即相邻两帧图像的相对位姿关系，包括相邻两帧图像的相对位置、速度、姿态变化关系，所述测量数据包括加速度计和陀螺仪输出的测量值。
13.步骤1.3，动态点剔除，基于imu预积分结果，将每帧图像属于车辆或行人语义类别的特征点基于imu预积分结果投影到下一帧图像中，作为这部分特征点在所述下一帧图像中的预测位置；通过跟踪匹配到的所述下一帧图像中这些点的实际位置与预测位置的不一致的像素点即为动态点，实际位置与预测位置一致的像素点为静态点，将动态点进行剔除，从而获得所述下一帧图像的静态特征点及其位置。
14.步骤1.1分割出所有“车辆”和“行人”的动态点，但并不是所有的车辆和行人都是在运动的，所以需要再通过imu预积分找出步骤1.1得到的车辆行人特征点中运动的那部分。先进行分割的作用是不必对所有的特征点进行预积分投影，因为例如树木，路灯这些物体是一定静止的，所以先找出车辆和行人这些有可能动的，只投影这一部分，可以减少运算量。
15.进一步地，所述步骤1.1中卷积神经网络采用改进的u-net卷积神经网络，所述改进的u-net卷积神经网络将编码器和解码器中卷积层替换为ghost模块，ghost模块与传统卷积模块功能相同，但它属于轻量级操作，其所用的线性变换操作在保证精度的同时更加成本低廉，计算量小。
16.基于卷积神经网络进行车辆和行人的语义分割包括步骤1.1.1，利用数据集对改进的u-net卷积神经网络模型进行训练；所述数据集预先通过车载摄像头采集的方式获取不同场景下的道路图像，建立车辆行人数据集，或直接下载现有的公开的道路数据集；对所述改进u-net卷积神经网络模型进行训练，获得训练后的改进u-net卷积神经网络。
17.步骤1.1.2，将相机采集的图像帧合集opic输入训练后的改进u-net卷积神经网络
的编码器，获得相机采集的图像帧合集opic的高层级特征向量。
18.步骤1.1.3，将所述相机采集的图像帧合集opic的高层级特征向量输入训练后的改进u-net卷积神经网络的解码器，获得用于语义类别分割的特征图像。
19.步骤1.1.4，将用于语义类别分割的特征图像输入softmax层，获得相机采集的图像帧合集opic中每帧图像属于车辆或行人语义类别的特征点。
20.进一步地，步骤2.1，基于每帧图像的特征点位置和imu预积分结果进行vio初始化，获得相机和imu之间的外参、特征点的深度、紧耦合的位置、速度、姿态的初始值。
21.步骤2.2，所述视觉和imu紧组合融合定位算法基于非线性优化方式，根据所述相机和imu之间的外参、特征点的深度、紧耦合的位置、速度、姿态的初始值进行滑动窗口的视觉和imu紧组合非线性优化，求解出滑动窗口中最优的相机位姿，即局部相机位姿。
22.进一步地，所述步骤2.1中，基于每帧图像的特征点位置通过运动恢复结构技术恢复图像帧的位姿变换，再用图像帧的位姿变换与imu预积分结果对齐，恢复相机和imu之间的外参，获得imu坐标系和世界坐标系的转换关系。
23.所述步骤2.2包括：获取视觉和imu紧组合滑动窗口的状态向量x，运用最小化重投影误差算法，求解视觉和imu紧组合非线性优化问题，运用非线性优化库ceres solver库求解所述非线性优化问题，从而求解出滑动窗口中最优的相机位姿。
24.视觉和imu紧组合非线性优化问题的待优化误差函数为如下公式。
25.式中，γ
marg
是来自边缘化的先验信息，即滑动窗口中最老的帧与其相应的状态量被边缘化，作为先验信息；γi和γ
l
分别是imu残差项和视觉残差项；i是其中所有imu测量的集合，是第k帧图像到第k+1帧图像间imu预积分噪声项的协方差矩阵；l是在当前滑动窗口中至少观察到两次的特征点的集合，是集合l中第θ帧图像中第l个像素坐标的视觉观测噪声的协方差矩阵。
26.进一步地，步骤3中，所述自适应算法的切换指根据车辆运动状态，分别用零速修正、鲁棒核函数、非完整约束对gnss、imu和视觉松组合融合定位算法以及视觉和imu紧组合融合定位算法进行约束和修正。
27.步骤3包括步骤3.1，基于加速度计输出的测量值设计一个大小为n的滑动窗口求解加速度标准差因子，根据加速度标准差因子判断车辆状态是否处于静止状态，若状态处于静止状态，则在gnss、imu和视觉松组合融合定位算法中进行静态零速修正。
28.步骤3.2，若检测到车辆处于非静止状态，则车辆处于转弯状态或直行状态；若判定车辆处于转弯状态，则在视觉和imu紧组合融合定位算法中加入鲁棒核函数。
29.步骤3.3，若判定车辆处于直行状态，则在gnss、imu和视觉松组合融合定位算法中加入非完整约束来修正惯导的累积误差。
30.车辆运动状态对于定位精度的影响很大，通过判定车辆运动状态的不同，可以在
算法中添加不同的约束和修正，提高定位精度。本发明设置了三种运动状态的判定，当车辆静止时，传感器imu和相机可能由于自身的误差导致二者的输出并不是静止的，这就带来了误差，那么准确的判定车辆静止，进行静态零速修正，就修正了这一误差。当车辆直行时，不可能发生侧移和垂直移动，符合非完整约束的使用条件。当车辆拐弯时，相机拍到的图像会发生模糊，相机在特征点匹配时会产生误匹配，大大降低其精度，因此加入鲁棒核函数，减小误差影响。
31.进一步地，步骤4包括步骤4.1，解算gnss位置信息。
32.步骤4.2，进行vio和gnss初始化，将步骤2获得的局部相机位姿和步骤4.1解算获得的gnss位置信息转化为地心地固坐标系下对应信息。
33.步骤4.3，基于卡尔曼滤波以及gnss、imu和视觉松组合融合定位算法，获得全局估计位姿，即最终定位结果。
34.进一步地，步骤3.1包括大小为n的滑动窗口加速度标准差表示为如下公式。表示为如下公式。
35.其中，分别是t时刻加速度计xyz三轴的输出；i表示滑动窗口内加速度数据的时刻下标，i∈[t-n+1,t]；μ是滑动窗口内所有加速度的平均值，作为数学期望，此处使用无偏估计。
[0036]
在车辆运动过程中，通过设置车辆静止阈值来判断车辆是否静止，当车辆的加速度标准差时，判定车辆为零速静止状态，此时，为车辆添加零速修正，即在步骤4.3的卡尔曼滤波过程中，用惯导解算的速度作为系统速度误差的观测量，进行卡尔曼滤波估计，用更新后的状态估计量反馈校正ins子系统内部的导航参数结果，完成静态零速修正。
[0037]
其中车辆静止阈值设置为动态阈值，再设置一个大小为m的滑动窗口，滑动窗口内为不同时刻的加速度标准差，车辆静止阈值随滑动窗口m的滑动自适应地变化，能够更加灵活地检测出车辆的零速状态。
[0038]
其中，j表示滑动窗口m内的加速度标准差的时刻下标，j∈[t-m+1,t]，ηj是各个加速度标准差的权重，离当前时刻t越远的加速度标准差的权重越小，越近的权重越大。
[0039]
定义滑动窗口m大小上限是m
max
，下限是m
min
，先取m
min
当作滑动窗口长度，设定一个当这组的均值σ小于等于加速度值平稳性阈值ε时，代表这段时间的加速度值较为平稳，应选择较大的滑动窗口长度m值，当大于加速度值平稳性阈值ε时，代表加速度值发生阶段性变化，应选择较小的滑动窗口长度m提高灵敏度。应选择较小的滑动窗口长度m提高灵敏度。
[0040]
其中, ,σ为取m
min
为窗口长度时，这组的均值；采集多组车辆不同运动状态的imu输出的加速度计算多组σ，选定加速度值平稳与否的临界值作为加速度值平稳性阈值ε。
[0041]
在车辆静止时，加速度计的输出较为平稳，而非静止时，数据就会有比较大的起伏波动，而标准差能够反映一组数据的平稳性。而不同的静止的场景时间，由于车身本身的抖动等外因影响，加速度值平稳性阈值应当是不同的，所以附近一些的时间段的标准差对于加速度值平稳性阈值的确定有参考价值，因此设置动态加速度值平稳性阈值。
[0042]
进一步地，步骤3.2包括基于角速度的测量值设计一个基于角速度均值的转弯检测因子，采用滑动窗口n时间内的角速率均值|ω|作为车辆转弯的判断依据，当角速率均值时，判定车辆为转弯状态，否则判定为直行。
[0043]
式中，ωz为陀螺仪z轴方向的输出；th
ω
为设定的角速率均值阈值。
[0044]
若判定车辆处于转弯状态，则在视觉和imu紧组合融合定位算法中加入鲁棒核函数，在视觉和imu紧组合非线性优化问题的待优化误差函数中的视觉测量的残差项中加入鲁棒核函数huber核函数。
[0045]
其中，δ为判断视觉残差项是否过大的阈值，分别采集车辆直行质量较好的数据和转弯发生图像模糊和误匹配的数据，比较所得的视觉残差项，从而确定判定视觉数据质量好坏的视觉残差项阈值δ。
[0046]
所述步骤3.3中，若判定车辆处于直行状态，则车辆在行驶过程中，不会发生横向侧滑和沿路面垂直方向的运动，则在步骤4.3的卡尔曼滤波过程中加入非完整约束来修正惯导的累积误差，即将横向和垂直方向的速度设为零。
[0047]
当车辆直行时，不可能发生侧移和垂直移动，符合非完整约束的使用条件。当车辆拐弯时，相机拍到的图像会发生模糊，相机在特征点匹配时会产生误匹配，大大降低其精度，因此将入鲁棒核函数，减小误差影响。
[0048]
进一步地，步骤4.1包括：去除卫星仰角低于卫星仰角阈值的卫星，获得筛选后的卫星观测数据；根据筛选后的卫星观测数据通过伪距单点定位算法，解算获得gnss位置信息。
[0049]
步骤4.3包括：将步骤4.2获得的局部相机位姿和gnss位置信息在地心地固坐标系下对应信息，以及步骤3的自适应算法的切换结果输入卡尔曼滤波器中，获得误差改正信息；再将误差改正信息输入到步骤2.2的基于滑动窗口的紧组合非线性优化中，对导航参数进行反馈修正；最后输出经过修正后的位姿信息作为全局估计位姿，即最终定位结果。
[0050]
如果gnss本身的定位就有问题，那么会极大影响它和vio的组合效果。仰角低于卫星仰角阈值的卫星的信号容易经过楼等障碍物的反射导致测量信息有较大误差，因此必须剔除仰角低的卫星。
[0051]
gnss能够提供车辆的绝对定位位置，且在信号良好的情况下，gnss定位精度很高，且不存在累积误差，因此将它与vio进行组合导航能够获得良好的全局位姿结果。
[0052]
有益效果：1、针对gnss/imu组合导航在gnss长时间失效的情况下会产生漂移，本发明采用相机与gnss/imu进行组合导航，先将视觉信息和imu进行基于滑动窗口优化的紧组合，再将输出的局部位姿与gnss的定位结果输入卡尔曼滤波进行松组合，有效的发挥了视觉在组合导航的作用，提高了多源融合定位的精度及鲁棒性。
[0053]
2、针对组合导航数据预处理中动态点剔除问题，本发明采用改进的u-net卷积神经网络，即用ghost模块替代原有的传统卷积层，对相机拍摄图片进行语义分割，对行人车辆特征点进行提取，再基于imu预积分结果将这部分特征点投影到下一帧图像中，找出位置不一样的点即为动态点进行剔除有效的实现了高效的动点剔除，优化了计算效率。
[0054]
3、针对车辆运动状态的变化带来的精度下降的问题，本发明构建基于imu加速度和角速度的检测因子，检测车辆的运动状态属于静止、转弯还是直行，针对三种状态分别用
零速修正、鲁棒核函数、非完整约束对滤波过程和非线性优化进行约束和修正。
附图说明
[0055]
下面结合附图和具体实施方式对本发明做更进一步的具体说明，本发明的上述和/或其他方面的优点将会变得更加清楚。
[0056]
图1是本技术实施例提出的一种基于运动状态与环境感知的多源融合导航定位方法的流程示意图。
[0057]
图2是ghost模块的示意图。
[0058]
图3是改进u-net卷积神经网络的网络结构示意图。
[0059]
图4是相机采集的图像帧。
[0060]
图5是现有技术对图4的动态点剔除效果示意图。
[0061]
图6是本技术实施例提供的一种结合车辆运动状态和环境感知的多源融合导航定位方法中动态点剔除效果示意图。
[0062]
图7是现有技术和本技术实施例提供的一种结合车辆运动状态和环境感知的多源融合导航定位方法导航定位的东北天坐标系中三维位置误差比较示意图。
[0063]
图8是现有技术和本技术实施例提供的一种结合车辆运动状态和环境感知的多源融合导航定位方法导航定位的姿态误差比较示意图。
具体实施方式
[0064]
下面将结合附图，对本发明的实施例进行描述。
[0065]
现有技术存在如下问题。
[0066]
1、当前应用最广泛的gps/ins组合导航，在gnss长时间失效的情况下，imu会产生误差累积和漂移，且ins的造价高、生产周期长。同时现有的加入视觉的组合导航方法大多是vo或视觉辅助gnss定位，视觉在定位作用没有得到充分发挥。
[0067]
2、由于城市道路中车辆和行人这些运动点较多，且在图像中会呈现不同的大小、姿态以及视角变化，会导致视觉解算精度下降，现有的gnss/imu/视觉组合导航存在缺乏动态环境自适应的问题，在视觉数据预处理时没有精确高效剔除动态点，存在对所有特征点进行处理带来较大计算量，及剔除全部状态的行人车辆特征点的弊端。
[0068]
3、现有的gnss/imu/视觉组合导航在车辆运动状态多变时，定位的精度会有所下降，存在着性能下降及可靠性不足的问题。
[0069]
为满足车辆全天候全路况条件下的高精度、连续性、可靠性导航定位需求，提高车辆在复杂环境和运动状态变化中的自适应性，本发明实施例提供了一种结合车辆运动状态和环境感知的多源融合导航定位方法，如图1所示，包括步骤1，使用相机采集图像帧，剔除图像帧中的动态点，获得每帧图像的静态特征点及其位置。
[0070]
步骤1.1，基于改进卷积神经网络的行人、车辆语义分割对于相机采集的图像帧合集opic，先用klt稀疏光流法分别提取并追踪每帧图像的特征点，获得特征点位置，即特征点的像素坐标，并将相机采集的图像帧合集opic输入卷积神经网络进行“车辆”和“行人”的语义分割，最后利用imu预积分结果进行动态点剔除。
[0071]
klt稀疏光流法分别提取并追踪每帧图像的特征点，具体参见参考文献：周磊,马
立.基于稀疏光流法的orb特征匹配优化[j].应用光学,2019,40(04):583-588.所述步骤1.1中卷积神经网络采用改进的u-net卷积神经网络，如图3所示，将常规的卷积层替换为ghost模块进行特征提取，再对图像进行语义分割。所述改进的u-net卷积神经网络为编码器-解码器结构，在编码器部分使用ghost模块替代传统卷积层进行特征提取，解码器部分是采用上采样和卷积组合的方法实现反卷积，同样使用ghost模块替代传统卷积层。
[0072]
步骤1.1.1，利用数据集对改进的u-net卷积神经网络模型进行训练。所述数据集通过车载摄像头采集的方式获取不同场景下的道路图像，建立车辆行人数据集，或直接下载现有的公开的道路数据集，例如kitti数据集。对所述改进u-net卷积神经网络模型进行训练，获得训练后的改进u-net卷积神经网络。
[0073]
步骤1.1.2，将相机采集的图片合集opic输入练后的改进u-net卷积神经网络的编码器，获得相机采集的图像帧合集opic的高层级特征向量。
[0074]
利用ghost模块增多特征图的输出，分为两部分，如图2所示，第一部分是输出少量特征图的传统卷积层，如公式(1)所示。
[0075]
y’=opic*f’(1)其中，y’是输出特征图合集，f’是该卷积操作的卷积核，opic是相机采集的图像帧合集，*是常规卷积。这个卷积操作的输出特征图通道数要小于传统卷积层的通道数。第二部分是用于产生冗余特征图的轻量级线性变换层，如公式(2)所示。
[0076]
(2)其中,为y’的m个特征图像，y’中的每个特征图，都通过轻量级线性操作，得到s个特征图像，此处可以是3
×
3或者5
×
5的卷积操作。因此，m个特征图通过这个线性操作共得到m
×
s个特征图像。设定ghost模块个数为5，每一个ghost模块之后，通过最大池化对图片进行下采样对特征进行降维。经过5个ghost模块后输出图像的高层级特征向量，传入解码器。
[0077]
步骤1.1.3，进行解码，采用4个解码模块对提取的特征进行解码，每一个解码模块由一个反卷积模块跟随一个ghost模块组成，同一层级的编码与解码部分的卷积模块相同。上一层级的输出和对应编码器输出的特征图作为该解码模块的输入，将编码器输出的低等级的特征进行融合，充分利用所有特征，获得用于语义类别分割的特征图像。
[0078]
步骤1.1.4，将用于语义类别分割的特征图像输入到softmax层，从而得到相机采集的图像帧合集opic中每帧图像每个像素属于“车辆”或“行人”语义类别的概率，如公式(3)所示。
[0079]
(3)其中，sd(x)是每一像素点x对应语义类别d的得分，d是语义类别的数量，此处为“行人”和“车辆”两类,d=2。qd(x)是语义类别d对像素点x的分类结果，使得最有可能的结果最大化，同时抑制其他类别的概率。根据多次试验，采集多组“行人”与“车辆”的图像，经过改进的u-net网络可得到多组该语义类别的概率，例如，图中行人特征点得到的属于行人类别的概率、车辆特征点得到的属于车辆类别的概率均大于0.8，则概率经验阈值q
thr
的取值为0.8。对于某一特征点，它会求得两个概率，一是它属于“行人”的概率q1（x），二是它属于“车辆”的概率q2（x）将得到的概率与确定的概率经验阈值q
thr
相比较，若q1（x）大于概率经验阈值q
thr
，则认定该像素属于“行人”类别，若q2（x）大于q
thr
,则认定它属于“车辆”类别。
[0080]
步骤1.2，对imu测量数据进行预积分，获得imu预积分结果，即相邻两帧图像的相对位姿关系，包括相邻两帧图像的相对位置、速度、姿态变化关系，所述测量数据包括加速度计和陀螺仪输出的测量值。
[0081]
加速度计和陀螺仪的测量数据如公式(4)和公式(5)所示。
[0082]
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(4)
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(5)其中，是t时刻imu输出的加速度计输出的测量值；a
t
是加速度真实值；ba是加速度计的偏置；na是加速度计噪声项；w表示世界坐标系，gw是世界坐标系中的重力加速度；是世界坐标系到载体坐标系的旋转矩阵，通过惯导的力学编排可求得；是陀螺仪测量值；ω
t
是角速度真实值；bg是陀螺仪偏置；ng是陀螺仪噪声项。
[0083]
对于图像帧k和k+1，对上面两个式子进行积分，可以计算出时刻tk和tk+1之间的相对位置、速度、姿态变化。预积分公式如公式(6)至公式(11)所示。
[0084]
ꢀꢀꢀꢀꢀꢀꢀ
(6)
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(7)
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(8)
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
（9）
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(10)
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(11)其中，δtk是tk和t
k+1
之间的时间间隔，b表示载体坐标系，是载体坐标系到世界坐标系的旋转矩阵；、、和、、分别是世界坐标系下图像帧k和图像帧k+1的位置、速度和姿态；表示t时刻相机相对于第k帧的姿态变化量；ω是表示姿态的四元数；、和分别表示t时刻角速度真实值ω
t
在载体坐标系的xyz三轴上的分量，即。
[0085]
步骤1.3，动态点剔除，基于imu预积分结果，即连续两帧间的相对位姿关系，包括相邻两帧图像的相对位置、速度、姿态变化关系，将语义类别为“车辆”和“行人”的那部分特征点基于这个姿态变化投影到下一帧图像中，作为这部分特征点在所述下一帧图像中的预测位置。通过跟踪匹配到的所述下一帧图像中这些点的实际位置与预测位置的不一致的像素点即为动态点，实际位置与预测位置一致的像素点为静态点，将动态点进行剔除，获得所述下一帧图像的静态特征点及其位置。
[0086]
图4是相机采集的图像帧，图5是现有技术对图4的动态点剔除效果，图6是本技术实施例的动态点剔除效果，从图6中可以看出本技术实施例的动态点剔除可以完整分割“人”这部分的像素，并将正在运动的人的像素点全部剔除，而现有技术不能完全剔除所有动态点，图5中运动的“人”仍有部分动态特征点残余。
[0087]
步骤2，基于每帧图像的特征点位置以及视觉和imu紧组合融合定位算法，获得局部相机位姿。
[0088]
步骤2.1，vio初始化。
[0089]
动态点剔除之后的特征点匹配结果通过运动恢复结构技术恢复图像帧的位姿变换，恢复相机外参特征点深度和位置速度姿态的初始值，具体参见参考文献yang z , shen s . monocular visual
–
inertial state estimation with online initialization and camera
–
imu extrinsic calibration[j]. ieee transactions on automation science and engineering, 2017, 14(1):1-13.。初始化过程完成，所有度量值输入到下一步骤。
[0090]
步骤2.2，基于滑动窗口的非线性优化，求解出滑动窗口中最优的相机位姿，即局部相机位姿。
[0091]
紧组合滑动窗口的状态向量如公式(12)至公式(14)所示。
[0092]
ꢀꢀꢀꢀ
(12)
ꢀꢀꢀꢀ
(13)
ꢀꢀꢀ
(14)其中，x是状态向量；χh是滑动窗口内第h个时刻imu的状态，包含第h帧图像时imu在世界坐标系中的位置、速度和姿态，此处h∈[0,kf]，kf是从步骤1.1中相机采集的图像帧中提取的关键帧的总数；fp是滑动窗口中的特征总数；λ
l
是第一次观测到第l个特征点时该点在相机坐标系中的逆深度，为深度的倒数，所述深度通过初始化过程获得，此处l∈[0, fp]；是相机的外参，c是相机坐标系，和分别是相机坐标系到载体坐标系之间的原点平移向量和旋转姿态向量。
[0093]
运用最小化重投影误差方法求解出最优的相机位姿，此时该非线性优化问题的待优化误差函数如公式(15)所示。
[0094]
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(15)式中，γ
marg
是来自边缘化的先验信息，即滑动窗口中最老的帧与其相应的状态量被边缘化，作为先验信息；γi和γ
l
分别是imu残差项和视觉残差项；i是其中所有imu测量的集合，是第k帧图像到第k+1帧图像间imu预积分噪声项的协方差矩阵；l是在当前滑动窗口中至少观察到两次的特征点的集合，是集合l中第θ帧图像中第l个像素坐标的视觉观测噪声的协方差矩阵。
[0095]
imu残差项如公式(16)所示。
[0096]
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(16)式中，和分别是第k帧图像到第k+1帧图像间位置和速度的三维误差状态表示；是第k帧图像到第k+1帧图像间姿态四元数的三维误差状态表示；
δba和δbg分别是加速度计和陀螺仪偏置的误差状态。
[0097]
假设第l个特征点在第υ帧图像中第一次被观测到，第θ帧图像（θ＞υ）的视觉残差项如公式(17)所示。
[0098]
ꢀꢀꢀ
(17)
ꢀꢀꢀ
(18)
ꢀꢀ
(19)其中，是第一次观测到出现在第υ帧图像中的第l个特征像素点坐标；是第θ帧图像中对相同特征点的观测的像素坐标；是反投影前的像素坐标；是反投影函数，它将残差向量投影到单位球上，再除以逆深度得到像素点在相机坐标系中的坐标，再投影到归一化切平面上，φ1和φ2是在切平面上的两个任意选择的正交基；是相机坐标系到载体坐标系的旋转矩阵，即为已知的相机的外参。和是第υ帧图像时的载体坐标系到世界坐标系的旋转矩阵和原点平移向量，和是世界坐标系到第θ帧图像时的载体坐标系的旋转矩阵和原点平移向量，二者由该时刻的相机姿态可得。
[0099]
运用非线性优化库ceres solver库求解该非线性优化问题，使得待优化误差函数式（15）取得最小值，即为得到最优的载体位姿估计p
vio
和q
vio
，输入到步骤4与gnss进行松组合。
[0100]
步骤3，基于imu测量数据，获得车辆运动状态；根据车辆运动状态进行自适应算法的切换。
[0101]
imu测量得到的加速度和角速度输入到步骤3，进行自适应算法的切换。
[0102]
步骤3.1，基于加速度的测量值设计一个大小为n的滑动窗口加速度标准差因子，根据加速度标准差因子判断车辆状态是否处于静止状态，若状态处于静止状态，则在gnss、imu和视觉松组合融合定位算法中进行静态零速修正。
[0103]
大小为n的滑动窗口加速度标准差表示为如公式(20)和公式(21)所示。
[0104]
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(20)
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(21)其中，分别是t时刻加速度计xyz三轴的输出；n是滑动窗口的长度，此处窗口长度设置为100个数据，n=100；i表示滑动窗口内加速度数据的时刻下标，i∈[t-n+1,t]；μ是滑动窗口内所有加速度的平均值，作为数学期望，此处使用无偏估计，是t时刻滑动窗口计算出的加速度标准差。
[0105]
在车辆运动过程中，可以通过设置车辆静止阈值来判断车辆是否静止。
[0106]
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(22)其中车辆静止则不设置为固定阈值，再设置一个大小为m的窗口。窗口内为不同时刻的加速度标准差，车辆静止阈值随窗口的滑动自适应地变化，能够更加灵活地检测出车辆的零速状态。
[0107]
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(23)
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(24)其中，j表示滑动窗口m内的加速度标准差的时刻下标，j∈[t-m+1,t]，ηj是各个标准差的权重，离当前时刻越远的标准差的权重越小，越近的权重越大。
[0108]
选择合适大小的m十分重要。因此，设计了一种窗口大小自适应的方法，解决滑动窗口灵敏度和精度的要求。定义滑动窗口大小上限是m
max
，下限是m
min
，先取m
min
当作窗口长度。设定一个当这组的均值σ小于等于加速度值平稳性阈值ε时，代表这段时间的加速度值较为平稳，应选择较大的m值，当大于阈值时，代表加速度值发生阶段性变化，应选择较小的窗口长度提高灵敏度。
[0109]
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(25)
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(26)其中, ，σ为取m
min
为窗口长度时，这组的均值；采集多组车辆不同运动状态的imu输出的加速度计算多组σ，选定加速度值平稳与否的临界
值作为加速度值平稳性阈值ε。经过测试实验，窗口长度取20-50的结果差异较小，因此m
max
取50，m
min
取20；加速度值平稳性阈值ε设置为0.05。
[0110]
当车辆的加速度值满足式（22）时，判定车辆为零速静止状态，此时，为车辆添加零速修正，即在步骤4.3的卡尔曼滤波过程中，用惯导解算的速度作为系统速度误差的观测量，进行卡尔曼滤波估计，用更新后的状态估计量反馈校正ins子系统内部的导航参数结果，完成静态零速修正。
[0111]
步骤3.2，若检测到车辆处于非静止状态，则进入直线运动检测部分，此处基于角速度的测量值设计一个基于角速度均值的转弯检测因子，采用滑动窗口n时间内的角速率均值|ω|作为车辆转弯的判断依据。
[0112]
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(27)式中，|ω|是角速度的均值，n为滑动窗口内的数据数量；ωz为陀螺仪z轴方向的输出；th
ω
为设定的角速率均值阈值，此处设置为2
°
/s。当z轴角速率超过th
ω
时，即满足式（27）时，判定车辆为转弯状态，否则判定为直行。
[0113]
在车辆转弯时由于视觉会由于图像模糊产生较大误差，特征点匹配易发生错误，因此在视觉与imu紧组合过程中加入鲁棒核函数提高视觉导航的抗差性，在式（15）中的视觉残差项中加入鲁棒核函数huber核函数，在残差项较大时，由原来的二次函数变为线性函数，降低错误数据的影响。
[0114]
ꢀꢀꢀꢀ
(28)
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(29)其中，δ为判断视觉残差项是否过大的阈值，分别采集车辆直行质量较好的数据和转弯发生图像模糊和误匹配的数据，比较所得的视觉残差项，即可确定判定视觉数据质量好坏的视觉残差项阈值δ。例如，质量较好的数据得到的残差项值都小于0.5，而图像模糊时的残差项值均大于0.5，则设置残差项值δ的值为0.5。
[0115]
步骤3.3，若算法判定车辆处于直行状态，则车辆在行驶过程中，不会发生横向侧滑和沿路面垂直方向的运动，则在步骤4.3的卡尔曼滤波过程中加入非完整约束来修正惯导的累积误差，即将横向和垂直方向的速度设为零。
[0116]
步骤4，基于局部相机位姿、自适应算法的切换结果、卡尔曼滤波以及gnss、imu和视觉松组合融合定位算法，获得最终定位结果。
[0117]
步骤4.1，gnss位置信息解算去除卫星仰角低于卫星仰角阈值的卫星。根据经验设定卫星仰角阈值为α
thre
=15
°
，遍历获得的卫星仰角α
sat
(sat=1,2,
…
,num
vis
)，num
vis
为当前历元可见星的数目，若满
足α
sat
》α
thre
，则保留卫星sat相关的观测数据，否则删除卫星sat相关的观测数据。筛选后的观测信息经过伪距单点定位算法求解获得gnss位置信息。
[0118]
步骤4.2，vio/gnss初始化，将步骤2获得的局部相机位姿和步骤4.1解算获得的gnss位置信息转化为地心地固坐标系下对应信息。
[0119]
vio的位姿估计是在局部坐标系中，因此，须将它转换成地心地固ecef坐标系。记步骤4.1中起点处静止的初始一分钟的gnss位置信息，即东北天坐标系和载体坐标系原点（二者原点一样）在ecef坐标系中的位置。再用低噪声的多普勒测量校正载体坐标系和东北天坐标系之间的航向角偏置ψ。从东北天到ecef坐标系的旋转矩阵如公式(30)所示。
[0120]
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(30)其中，n是东北天坐标系；e是ecef坐标系；α和β分别是东北天坐标系原点的纬度和经度，从载体坐标系到东北天坐标系的旋转矩阵 如公式(31)所示。
[0121]
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(31)因此，载体坐标系到ecef的转换关系如公式(32)所示。
[0122]
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(32)其中，是接收机即gnss在ecef中的坐标，是接收机和载体坐标系原点在载体坐标系中的位置之差。至此，gnss位置信息和vio输出的导航参数p
vio
和q
vio
统一到了ecef坐标系下。
[0123]
步骤4.3，基于卡尔曼滤波的gnss/imu/视觉松组合导航将步骤4.2获得的局部相机位姿和gnss位置信息在地心地固坐标系下对应信息，以及步骤3的自适应算法的切换结果一同输入卡尔曼滤波器中，输出得到相应的误差改正信息。再将输出的误差改正信息输入到步骤2.2的imu/视觉紧组合过程的基于滑动窗口的紧组合非线性优化中对导航参数进行反馈修正，最后输出经过修正后的位姿信息作为最终的定位结果。
[0124]
图7、图8和表1、表2是现有技术和本发明实施例提供的一种结合车辆运动状态和环境感知的多源融合导航定位方法导航定位的东北天坐标系中三维位置误差和姿态误差比较示意图和均方根误差表，从图表中可以看出本技术得到的三维位置和姿态误差均小于gnss/imu组合导航的结果，这证明了本技术的位姿精度的优越性。
[0125]
表1 本技术和现有技术的三维位置均方根误差表
表2 本技术和现有技术的姿态均方根误差表本发明提供了一种基于运动状态与环境感知的多源融合导航定位方法，具体实现该技术方案的方法和途径很多，以上所述仅是本发明的具体实施方式，应当指出，对于本技术领域的普通技术人员来说，在不脱离本发明原理的前提下，还可以做出若干改进和润饰，这些改进和润饰也应视为本发明的保护范围。本实施例中未明确的各组成部分均可用现有技术加以实现。