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

1.一种基于运动状态与环境感知的多源融合导航定位方法，其特征在于，包括以下步骤：步骤1，使用相机采集图像帧，剔除图像帧中的动态点，获得每帧图像的静态特征点及其位置；步骤2，基于每帧图像的特征点位置以及视觉和IMU紧组合融合定位算法，获得局部相机位姿；步骤3，基于IMU测量数据，获得车辆运动状态；根据车辆运动状态进行自适应算法的切换；步骤4，基于局部相机位姿、自适应算法的切换结果、卡尔曼滤波以及GNSS、IMU和视觉松组合融合定位算法，获得最终定位结果；步骤1包括：步骤1.1，对图像帧中每一帧图像的行人和车辆进行语义分割，对于相机采集的图像帧合集OPic，先用KLT稀疏光流法分别提取并追踪每帧图像的特征点，获得特征点位置，并基于卷积神经网络进行车辆和行人的语义分割，获得每帧图像属于车辆或行人语义类别的特征点；步骤1.2，对IMU测量数据进行预积分，获得IMU预积分结果，即相邻两帧图像的相对位姿关系，包括相邻两帧图像的相对位置、速度、姿态变化关系，所述测量数据包括加速度计和陀螺仪输出的测量值；步骤1.3，动态点剔除，基于IMU预积分结果，将每帧图像属于车辆或行人语义类别的特征点基于IMU预积分结果投影到下一帧图像中，作为这部分特征点在所述下一帧图像中的预测位置；通过跟踪匹配到的所述下一帧图像中这些点的实际位置与预测位置的不一致的像素点即为动态点，实际位置与预测位置一致的像素点为静态点，将动态点进行剔除，从而获得所述下一帧图像的静态特征点及其位置；所述步骤1.1中卷积神经网络采用改进的U-net卷积神经网络，所述改进的U-net卷积神经网络编码器和解码器中包括Ghost模块，基于卷积神经网络进行车辆和行人的语义分割包括：步骤1.1.1，利用数据集对改进的U-net卷积神经网络模型进行训练；所述数据集预先通过车载摄像头采集的方式获取不同场景下的道路图像，建立车辆行人数据集，或直接下载现有的公开的道路数据集；对所述改进U-net卷积神经网络模型进行训练，获得训练后的改进U-net卷积神经网络；步骤1.1.2，将相机采集的图像帧合集OPic输入训练后的改进U-net卷积神经网络的编码器，获得相机采集的图像帧合集OPic的高层级特征向量；步骤1.1.3，将所述相机采集的图像帧合集OPic的高层级特征向量输入训练后的改进U-net卷积神经网络的解码器，获得用于语义类别分割的特征图像；步骤1.1.4，将用于语义类别分割的特征图像输入softmax层，获得相机采集的图像帧合集OPic中每帧图像属于车辆或行人语义类别的特征点；步骤3中，所述自适应算法的切换指根据车辆运动状态，分别用零速修正、鲁棒核函数、非完整约束对GNSS、IMU和视觉松组合融合定位算法以及视觉和IMU紧组合融合定位算法进行约束和修正；步骤3包括：步骤3.1，基于加速度计输出的测量值设计一个大小为N的滑动窗口求解加速度标准差因子，根据加速度标准差因子判断车辆状态是否处于静止状态，若状态处于静止状态，则在GNSS、IMU和视觉松组合融合定位算法中进行静态零速修正；步骤3.2，若检测到车辆处于非静止状态，则车辆处于转弯状态或直行状态；若判定车辆处于转弯状态，则在视觉和IMU紧组合融合定位算法中加入鲁棒核函数；步骤3.3，若判定车辆处于直行状态，则在GNSS、IMU和视觉松组合融合定位算法中加入非完整约束来修正惯导的累积误差。 2.根据权利要求1所述的一种基于运动状态与环境感知的多源融合导航定位方法，其特征在于，步骤2包括：步骤2.1，基于每帧图像的特征点位置和IMU预积分结果进行VIO初始化，获得相机和IMU之间的外参、特征点的深度、紧耦合的位置、速度、姿态的初始值；步骤2.2，所述视觉和IMU紧组合融合定位算法基于非线性优化方式，根据所述相机和IMU之间的外参、特征点的深度、紧耦合的位置、速度、姿态的初始值进行滑动窗口的视觉和IMU紧组合非线性优化，求解出滑动窗口中最优的相机位姿，即局部相机位姿。 3.根据权利要求2所述的一种基于运动状态与环境感知的多源融合导航定位方法，其特征在于，所述步骤2.1中，基于每帧图像的特征点位置通过运动恢复结构技术恢复图像帧的位姿变换，再用图像帧的位姿变换与IMU预积分结果对齐，恢复相机和IMU之间的外参，获得IMU坐标系和世界坐标系的转换关系；所述步骤2.2包括：获取视觉和IMU紧组合滑动窗口的状态向量X，运用最小化重投影误差算法，求解视觉和IMU紧组合非线性优化问题，运用非线性优化库Ceres solver库求解所述非线性优化问题，从而求解出滑动窗口中最优的相机位姿；视觉和IMU紧组合非线性优化问题的待优化误差函数为：式中，γ marg 是来自边缘化的先验信息，即滑动窗口中最老的帧与其相应的状态量被边缘化，作为先验信息；γ I 和γ L 分别是IMU残差项和视觉残差项；I是其中所有IMU测量的集合， 是第k帧图像到第k+1帧图像间IMU预积分噪声项的协方差矩阵；L是在当前滑动窗口中至少观察到两次的特征点的集合， 是集合L中第θ帧图像中第l个像素坐标的视觉观测噪声的协方差矩阵。 4.根据权利要求3所述的一种基于运动状态与环境感知的多源融合导航定位方法，其特征在于，步骤4包括：步骤4.1，解算GNSS位置信息；步骤4.2，进行VIO和GNSS初始化，将步骤2获得的局部相机位姿和步骤4.1解算获得的GNSS位置信息转化为地心地固坐标系下对应信息；步骤4.3，基于卡尔曼滤波以及GNSS、IMU和视觉松组合融合定位算法，获得全局估计位姿，即最终定位结果。 5.根据权利要求4所述的一种基于运动状态与环境感知的多源融合导航定位方法，其特征在于，步骤3.1包括：大小为N的滑动窗口加速度标准差 表示为如下公式：其中， 分别是t时刻加速度计xyz三轴的输出；i表示滑动窗口内加速度数据的时刻下标，i∈[t-N+1,t]；μ是滑动窗口内所有加速度的平均值，作为数学期望，此处使用无偏估计；在车辆运动过程中，通过设置车辆静止阈值 来判断车辆是否静止，当车辆的加速度标准差 时，判定车辆为零速静止状态，此时，为车辆添加零速修正，即在步骤4.3的卡尔曼滤波过程中，用惯导解算的速度作为系统速度误差的观测量，进行卡尔曼滤波估计，用更新后的状态估计量反馈校正INS子系统内部的导航参数结果，完成静态零速修正；其中车辆静止阈值 设置为动态阈值，再设置一个大小为M的滑动窗口，滑动窗口内为不同时刻的加速度标准差 车辆静止阈值 随滑动窗口M的滑动自适应地变化，能够更加灵活地检测出车辆的零速状态：其中，j表示滑动窗口M内的加速度标准差 的时刻下标，j∈[t-M+1,t]，η j 是各个加速度标准差的权重，离当前时刻t越远的加速度标准差的权重越小，越近的权重越大；定义滑动窗口M大小上限是M max ，下限是M min ，先取M min 当作滑动窗口长度，设定一个当这组 的均值σ小于等于加速度值平稳性阈值ε时，代表这段时间的加速度值较为平稳，增加滑动窗口长度M值，当大于加速度值平稳性阈值ε时，代表加速度值发生阶段性变化，减小滑动窗口长度M提高灵敏度；其中,M'∈[0,M max -M min ],σ为取M min 为窗口长度时，这组 的均值；采集多组车辆不同运动状态的IMU输出的加速度计算多组σ，选定加速度值平稳与否的临界值作为加速度值平稳性阈值ε。 6.根据权利要求5所述的一种基于运动状态与环境感知的多源融合导航定位方法，其特征在于，步骤3.2包括：基于角速度的测量值设计一个基于角速度均值的转弯检测因子，采用滑动窗口N时间内的角速率均值|ω|作为车辆转弯的判断依据，当角速率均值 时，判定车辆为转弯状态，否则判定为直行；式中，ω z 为陀螺仪z轴方向的输出；TH ω 为设定的角速率均值阈值；若判定车辆处于转弯状态，则在视觉和IMU紧组合融合定位算法中加入鲁棒核函数，在视觉和IMU紧组合非线性优化问题的待优化误差函数中的视觉测量的残差项中加入鲁棒核函数Huber核函数：其中，δ为判断视觉残差项是否过大的阈值，分别采集车辆直行质量好的数据和转弯发生图像模糊和误匹配的数据，比较所得的视觉残差项，从而确定判定视觉数据质量好坏的视觉残差项阈值δ；所述步骤3.3中，若判定车辆处于直行状态，则车辆在行驶过程中，不会发生横向侧滑和沿路面垂直方向的运动，则在步骤4.3的卡尔曼滤波过程中加入非完整约束来修正惯导的累积误差，即将横向和垂直方向的速度设为零。 7.根据权利要求6所述的一种基于运动状态与环境感知的多源融合导航定位方法，其特征在于，步骤4.1包括：去除卫星仰角低于卫星仰角阈值的卫星，获得筛选后的卫星观测数据；根据筛选后的卫星观测数据通过伪距单点定位算法，解算获得GNSS位置信息；步骤4.3包括：将步骤4.2获得的局部相机位姿和GNSS位置信息在地心地固坐标系下对应信息，以及步骤3的自适应算法的切换结果输入卡尔曼滤波器中，获得误差改正信息；再将误差改正信息输入到步骤2.2的基于滑动窗口的紧组合非线性优化中，对导航参数进行反馈修正；最后输出经过修正后的位姿信息作为全局估计位姿，即最终定位结果。