本发明涉及一种智能汽车高精度定位与时空态势安全方法，特别涉及一种基于多传感器融合的智能汽车高精度定位与时空态势安全方法。
背景技术：
目前，随着智能汽车的快速发展，对于高精度定位技术的需求越来越强。智能汽车定位技术是解决“在哪里”问题的核心技术，其在全局路径规划与局部路径规划中均具有重要意义，是决策规划与运动控制的基础，也是智能交通系统的核心组成部分。高精度定位技术的突破有利于提高交通效率、减少环境污染、改善行驶安全与舒适性等。同时，将智能汽车及障碍物的三维场景信息进行时空态势安全碰撞分析，并以此为决策规划提供数据支撑来提高行驶安全性。常见的智能汽车定位方法有全球卫星导航系统(globalnavigationsatellitesystem,gnss)、惯性导航系统(inertialnavigationsystem,ins)、航迹推算(deadreckoning,dr)、超宽带(ultrawideband,uwb)、地图匹配、同时定位与建图(simultaneouslocalizationandmapping,slam)等，独立定位技术相对成熟，但是具有不可避免的缺陷且环境适应性差，智能汽车中很少仅采用单一技术实现准确定位。传感器融合利用技术互补来提高定位系统的精度与稳定性，是智能汽车定位技术的发展趋势。
在国内外研究中，诸多单位对传感器融合定位技术进行了相关研究并提出了诸如gnss/uwb、uwb/ins、视觉slam、激光slam、视觉或激光雷达与地图匹配等定位策略。在多传感器融合领域，各个传感器的参数、物理模型及其联合标定结果等直接影响定位结果，同时需要考虑定位精度与实时性的相互制约。在安全避撞领域，国内外大多采用基于三维地理信息来设计相应的安全策略。
目前国内外的多传感器融合定位及安全策略研究有如下特点：
1)gnss信号易被遮挡且精度低，相机受光照影响较大，惯性测量单元(inertialmeasurementunit,imu)误差不断累积且建模大多基于连续帧间bias不变，但仍为参与融合定位的主流定位方法；
2)精度与成本相互制约，基于激光雷达或高精地图或基站rtk成本高，大范围应用受限；
3)普遍对各传感器观测值进行高斯滤波，未考虑实际误差分布，并且对传感器的物理建模大部分基于连续时间内bias、灰度不变的假设，模型没有贴合传感器的实际工作状态；
4)主要通过离线标定传感器，标定繁琐且误差大，并且不能很好适配工作场景及研发的融合策略；
5)基于滤波的融合框架易产生误差累积并且对多传感器时钟同步要求高，基于优化的融合框架计算量大并且难以满足实时性要求；
6)视觉定位在大曲率行驶、特征变化不明显的直线行驶及动态物体影响行驶时易出现特征误匹配及特征追踪丢失；
7)通过相对位置来获取障碍物全局定位信息易引起误差累积，并且安全策略大多未考虑障碍物危险程度及交通效率等。
中国专利cn109141410b授权了一种agv组合导航的多创安其融合定位方法，通过线性化融合陀螺仪、角速度计和磁力计来求解姿态。由于陀螺仪和角速度计的工作原理决定其定位误差会随时间不断累积，因此无法用于长时间定位。磁力计根据磁场强度进行定位，易受外部环境干扰且精度差，不适用于智能汽车。
中国专利cn109737950b授权了一种基于vio和动力学模型的水下机器人定位系统及方法，通过试验法测得相关参数并求解动力学方程得到速度及加速度，将其和vio的定位输出通过扩展卡尔曼滤波进行松耦合融合得到速度估计值，再通过积分得到位置估计值。由于vio对imu及视觉传感器的物理建模基于连续时间内bias、灰度不变的假设，模型没有贴合传感器的实际工作状态，将导致从imu源头引入了误差。另外，通过离线标定imu和视觉传感器，标定繁琐且误差大，并且不能很好适配工作场景及研发的融合策略。上述方法通过基于滤波的松耦合方法进行多传感器融合，易产生误差累积并且对多传感器时钟同步要求高。
中国专利cn202010545062.6公开了一种基于点线特征融合激光的语义高精地图构建与定位方法。通过视觉和激光雷达分别对视觉图像和三维点云进行语义分割和点线特征提取，通过相机进行粗略估计并将估计值作为初始值用于迭代激光雷达定位来获得细致估计。视觉定位在大曲率行驶及特征变化不明显的直线行驶时易出现特征误匹配及特征追踪丢失，激光雷达成本高且计算量大，难以满足实时性，高精地图成本高且需要定时更新维护，难以应对复杂多变的场景。上述方法基于优化紧耦合融合视觉和imu，虽然后端优化能够对其误差校正，但仍然缺乏全局一致性。另外，基于优化的融合框架计算量大且难以满足实时性要求。
中国专利cn202010208367.8公开了一种基于三维地理信息的时空态势感知方法和系统，通过对三维地理数据进行虚实场景融合得到三维地理融合场景，并对其进行时空位置智能碰撞检测分析。上述方法需要提前预知三维地理信息，方法适用性受到限制，并且实时在线进行虚实场景融合计算量大，难以满足实时性。
技术实现要素：
本发明的目的是为了解决现有的智能汽车定位技术成本高、精度差、实时性差及表征行驶安全风险等问题而提供的一种基于多传感器融合的智能汽车高精度定位与时空态势安全方法。
本发明提供的基于多传感器融合的智能汽车高精度定位与时空态势安全方法,其方法如下所述:
第一部分、高精度定位方法，具体方法如下：
第一步、预处理；
第二步、视觉惯性里程计前端及初始化；
第三步、视觉惯性里程计后端优化；
第四步、全局位姿图优化；
第二部分、时空态势安全方法：
时空态势安全方法以大数据为基础，在设定的时间和空间内对复杂交互式环境中的各组成成分进行感知、理解，进而预知未来变化，从而动态、整体地洞悉风险，同时，基于道路场el考虑道路行驶安全性、基于静态场es考虑静态障碍物的安全性、基于动态场ek考虑动态物体的安全性，构建了主车附近的交通安全模型e＝wl·el ws·es wk·ek，在安全的基础上进行路径规划和行车安全的判断。
第一部分中第一步预处理的步骤如下：
步骤一、相机标定：以已知大小的平面黑白棋盘格作为标定参照物，通过调节相机与棋盘格的距离和角度提取一系列像素点，找到黑白线交点对应像素点在图像中的坐标，假设黑白线交点的世界坐标为p＝(x，y，z，1)，该交点在相机投影平面的坐标为：p′＝(u，v，1)，根据针孔相机模型有zp′＝kp，其中k为相机内参矩阵，形式如下：
同时，考虑到相机的切向畸变和径向畸变，对相机进行去畸变操作，其归一化坐标写成极坐标形式(r，θ)，有
经过标定后，得到像素点在图像上的正确位置：
其中k1，k2，k3是径向畸变系数，p1，p2是切向畸变系数；
步骤二、动态物体剔除：通过深度学习的方法对图像中的动态物体进行剔除，对相机得到的原始图像进行像素级语义分割并且计算得到的语义图与地面真值之间的损失函数l1，通过卷积神经网络dynss得到仅包括动态物体的语义图，将其和原始灰度图通过对抗生成网络u-net滤除动态物体来得到静态图像并计算其损失函数l2，同时和l1一起进行向后传播，对抗生成网络的工作如下：
其中:
第一部分中第二步视觉惯性里程计前端及初始化的步骤如下：
步骤一、特征提取、追踪、匹配：对滤除完动态物体的图像进行点、线、面特征的提取，并对连续帧间的特征进行追踪，通过ransac剔除误匹配得到最终特征匹配结果，其中特征点通过fast角点进行提取，特征线通过lsd进行提取，利用金字塔光流法进行特性追踪和特征匹配，金字塔光流法是将原始图像作为底层，通过固定的缩放比例获得具有不同分辨率的图像作为其他层，在计算光流时，首先计算第一层的图像，然后将上一层的跟踪结果用作下一层的初始值，当最后一层的像素运动较大时，金字塔顶部的图像显示的像素运动仍会在较小范围内，从而避免局部最小值或特征追踪丢失的问题；
步骤二、关键帧选取策略：根据不同的工作条件合理地选择不同数量的关键帧，并且应对易发生特征跟踪丢失的工况，采用三种原则来选择第k个帧是否为关键帧：
(1)、特征匹配点数nm与总特征点数ntotal的比率小于比率sthr的阈值；
(2)、在无明显变化的直线行驶中，世界坐标系下的相机估计值和imu估计值之间的位置差超过空间位置差阈值δdthr；
(3)、当以大曲率转弯行驶时，世界坐标系下相机估计值和imu估计值之间的航向角偏差超过航向角偏差阈值
步骤三、imu建模及预积分：考虑到bias随机游走和噪声，imu的加速度
imu的加速度偏差
imu和相机的采样频率不同，对两个连续帧之间的imu测量值进行积分，以计算位置、速度和旋转四元数，转换方程如下：
每次更新
其中：
步骤四、视觉与惯性对齐：首先，根据两个连续帧之间的相机估计值和imu预计分值相等来估计外参qbc，并基于旋转约束构造目标函数来估计bias：
然后，利用平移约束和重力大小来估计重力矢量、速度和尺度：
最后，将第0个相机系下重力向量
第一部分中第三步视觉惯性里程计后端优化步骤如下：
经过视觉和惯性对齐后，世界坐标系中的位置、速度和旋转四元数，bias外参以及特征点的3d信息
通过使先验误差和传感器测量值误差的mahalanobis范数总和最小来求解最优位姿：
上式通过增量方程进行求解，具体如下：
构建观测值误差、雅可比矩阵及协方差矩阵，具体如下：
步骤一、imu测量值误差：
对连续两帧间imu的测量值、bias求导即可得到雅可比矩阵：
雅可比矩阵如下：
步骤二、视觉观测值误差：
视觉观测值误差主要包括特征点误差、特征线误差和特征面误差；
1)、特征点误差：
将第i帧的特征点投影到第j帧：
特征点重投影误差如下：
2)、特征线误差：
将滤除动态物体图像中提取的特征线表示为点-法线形式，
3)、特征面误差：
将滤除动态物体图像中提取的特征面表示为点-法线形式
基于上述点、线、面误差构建视觉观测值误差模型，并且在整体的误差模型中提高点、线特征所占的权重：
其中，点特征权重αp与线特征权重αl相等，并且都等于面特征权重αs的两倍。
步骤三、重定位误差：
用词袋bow模型来检测回环并使用k-d树来表示字典，k-d树可以容纳kd个词，复杂度为o(kd)，具有很高的搜索速度，当检测到回环后，计算当前帧和bow之间的相似性得分，并将其与关键帧数据库中的所有帧进行比较以获得候选帧，然后，通过ransac提出误匹配，并通过pnp恢复位姿以进行闭环优化，如果匹配点的数量超过阈值，则将候选帧视为正确的闭环帧，最后，将前一帧，称为第v帧的位姿和相关特征点、线、面作为视觉约束添加到后端非线性优化的总体目标函数中，公式如下：
第一部分中第四步全局位姿图优化的步骤如下：
网络rtk及nrtk采用虚拟基站的方式，虚拟基站与卫星之间的距离
将虚拟基站的误差添加到主站的观测值中，卫星的相对误差和相位误差如下：
其中
位姿图优化的本质是最大似然估计，该估计使用vio和gps/nrtk的旋转和平移作为先验去估计最大测量值：
其中：s是vio和gps/nrtk测量值的集合，x是一段时间内的车辆位姿：
x＝[x0，x1，…xn]，
其中：
其中
位姿图包括两个因子：vio因子和gps/nrtk因子，分别如下：
(1)、vio因子：
其中
(2)、gps/nrtk因子：
除了融合上述vio及gps/nrtk，还进一步融合uwb或磁力计传感器，并且通过考虑铺二维码、贴线、短时间内的转角及位置变化这些运动学特性来辅助定位得到最优定位结果。
本发明的有益效果：
本发明提供的技术方案是基于滑动窗口优化实现紧耦合融合视觉和imu传感器，基于全局位姿图优化根据vio和gps/nrtk的定位精度自适应融合vio和gps/nrtk得到全局最优定位结果，具有全局一致性，能够应对大尺度复杂场景。同时，避免了相对定位的误差累积，具有高精度、低成本、高实时性等优点。本发明对imu实现细致化建模，考虑连续相机帧间bias的变化，从源头上减少了imu的定位误差来提高vio的定位精度，从而提高全局定位精度。本发明实现了视觉传感器和imu在线自动联合标定及在线估计imu参数，标定简单且精度高，能够很好适配工作场景及研发的融合策略。本发明实现了一种关键帧选取算法，该算法可以根据不同的工作条件合理选择不同数量的关键帧，并且可以处理容易发生特征跟踪损失的工作条件，例如无变化的直线行驶和大曲率的转弯行驶等。本发明的时空态势安全策略充分考虑道路、静态障碍物、动态物体的安全性及交通效率等，具有安全性高、效率高、计算量小、实时性高、技术难度低等优点。
附图说明
图1为本发明所述的基于多传感器融合的智能汽车高精度定位方法架构示意图。
图2为本发明所述的动态物体滤除示意图。
图3为本发明所述的金字塔光流法示意图。
图4为本发明所述的关键帧选取策略示意图。
图5为本发明所述的关键帧选取策略算法图。
图6为本发明所述的imu与视觉采样频率对齐示意图。
图7为本发明所述的视觉与惯性对齐算法图。
图8为本发明所述的网络rkt示意图。
图9为本发明所述的位姿图示意图。
图10为本发明与最先进的多传感器融合定位策略定位结果对比图。
图11为本发明所述的时空态势安全策略中道路场示意图。
图12为本发明所述的时空态势安全策略中静态场示意图。
图13为本发明所述的时空态势安全策略中动态场示意图。
图14为本发明所述的时空态势安全策略中交通安全场模型示意图。
具体实施方式
请参阅图1至图14所示：
本发明提供的基于多传感器融合的智能汽车高精度定位与时空态势安全策略，其策略包括高精度定位策略和时空态势安全策略两部分策略。高精度定位策略包括预处理、视觉惯性里程计前端及初始化、视觉惯性里程计后端优化和全局位姿图优化四部分。
一、预处理如下：
1.相机标定
以已知大小的平面黑白棋盘格作为标定参照物，通过调节相机与棋盘格的距离、角度等提取一系列像素点，这样可以找到黑白线交点对应像素点在图像中的坐标。假设黑白线交点的世界坐标为p＝(x，y，z，1)，该交点在相机投影平面的坐标为p′＝(u，v，1)，根据针孔相机模型有zp′＝kp，其中k为相机内参矩阵，形式如下：
同时，考虑到相机的切向畸变和径向畸变，对相机进行去畸变操作，其归一化坐标写成极坐标形式(r，θ)，有
经过标定后，可以得到像素点在图像上的正确位置：
其中k1，k2，k3是径向畸变系数，p1，p2是切向畸变系数，
2.动态物体滤除
如图2所示，通过深度学习的方法对图像中的动态物体进行滤除。对相机得到的原始图像进行像素级语义分割并且计算得到的语义图与地面真值之间的损失函数l1，通过卷积神经网络dynss得到仅包括动态物体的语义图，将其和原始灰度图通过对抗生成网络u-net滤除动态物体来得到静态图像并计算其损失函数l2，同时和l1一起进行向后传播。对抗生成网络的工作如下：
其中:
二、视觉惯性里程计前端及初始化如下：
1.特征提取、追踪、匹配
对滤除完动态物体的图像进行点、线、面特征的提取，并对连续帧间的特征进行追踪，通过ransac剔除误匹配得到最终特征匹配结果，其中特征点通过速度特别快的fast角点进行提取，特征线通过lsd进行提取，利用金字塔光流法进行特性追踪和特征匹配，如图3所示。金字塔光流法是将原始图像作为底层，通过固定的缩放比例获得具有不同分辨率的图像作为其他层。在计算光流时，首先计算第一层的图像，然后将上一层的跟踪结果用作下一层的初始值。当最后一层的像素运动较大时，金字塔顶部的图像显示的像素运动仍会在较小范围内，从而避免局部最小值或特征追踪丢失的问题。
2.关键帧选取策略
根据不同的工作条件合理地选择不同数量的关键帧，并且可以应对易发生特征跟踪丢失的工况，例如特征变化不明显的直线行驶和大曲率转弯行驶等。如图4-5所示，本文拟采用三种原则来选择第k个帧是否为关键帧：
(1)特征匹配点数nm与总特征点数ntotal的比率小于比率sthr的阈值；
(2)在无明显变化的直线行驶中，世界坐标系下的相机估计值和imu估计值之间的位置差超过空间位置差阈值δdthr；
(3)当以大曲率转弯行驶时，世界坐标系下相机估计值和imu估计值之间的航向角偏差超过航向角偏差阈值
3.imu建模及预积分
考虑到bias随机游走和噪声，imu的加速度
imu的加速度偏差
如图6所示，由于imu和相机的采样频率不同，因此对两个连续帧之间的imu测量值进行积分，以计算位置、速度和旋转四元数，转换方程如下：
每次更新
其中：
4.视觉与惯性对齐：
如图7所示，首先，根据两个连续帧之间的相机估计值和imu预计分值相等来估计外参qbc，并基于旋转约束构造目标函数来估计bias：
然后，利用平移约束和重力大小来估计重力矢量、速度和尺度：
最后，将第0个相机系下重力向量
三、视觉惯性里程计后端优化如下：
经过视觉和惯性对齐后，世界坐标系中的位置、速度和旋转四元数，bias，外参以及特征点的3d信息
通过使先验误差和传感器测量值误差的mahalanobis范数总和最小来求解最优位姿：
上式通过增量方程进行求解，具体如下：
构建观测值误差、雅可比矩阵及协方差矩阵，具体如下：
1.imu测量值误差
对连续两帧间imu的测量值、bias求导即可得到雅可比矩阵：
雅可比矩阵如下：
2.视觉观测值误差
视觉观测值误差主要包括特征点误差、特征线误差、特征面误差。
2.1特征点误差
将第i帧的特征点投影到第j帧：
特征点重投影误差如下：
2.2特征线误差
将滤除动态物体图像中提取的特征线表示为点-法线形式，
2.3特征面误差
将滤除动态物体图像中提取的特征面表示为点-法线形式
基于上述点、线、面误差构建视觉观测值误差模型，并且在整体的误差模型中提高点、线特征所占的权重：
其中，点特征权重αp与线特征权重αl相等，并且都等于面特征权重αs的两倍。
3.重定位误差
用词袋(bow)模型来检测回环并使用k-d树来表示字典。k-d树可以容纳kd个词，复杂度为o(kd)，具有很高的搜索速度。当检测到回环后，计算当前帧和bow之间的相似性得分，并将其与关键帧数据库中的所有帧进行比较以获得候选帧。然后，通过ransac提出误匹配，并通过pnp恢复位姿以进行闭环优化。如果匹配点的数量超过阈值，则将候选帧视为正确的闭环帧。最后，将前一帧(称为第v帧)的位姿和相关特征点、线、面作为视觉约束添加到后端非线性优化的总体目标函数中，公式如下：
四、全局位姿图优化如下：
如图8所示，网络rtk(nrtk)采用虚拟基站的方式，虚拟基站与卫星之间的距离
将虚拟基站的误差添加到主站的观测值中，卫星的相对误差和相位误差如下：
其中
位姿图优化的本质是最大似然估计，该估计使用vio和gps/nrtk的旋转和平移作为先验去估计最大测量值：
其中s是vio和gps/nrtk测量值的集合，x是一段时间内的车辆位姿：
x＝[x0，x1，…xn]，
其中
其中
如图9所示，本发明采用的位姿图包括两个因子：vio因子和gps/nrtk因子，分别如下：
(1)vio因子：
其中
(2)gps/nrtk因子：
除了融合上述vio及gps/nrtk等，融合例如uwb、磁力计等传感器也属于本发明提出的多传感器融合策略。同时本发明还通过考虑铺二维码、贴线、短时间内的转角及位置变化等运动学特性等来辅助定位得到最优定位结果。将本发明的定位结果与目前最先进的传感器融合定位方法进行对比，如图10所示。
本发明的时空态势安全策略以大数据为基础，在一定的时间和空间内对复杂交互式环境中的各组成成分进行感知、理解，进而预知未来变化，从而动态、整体地洞悉风险。同时，如图11-14所示，基于道路场el考虑道路行驶安全性、基于静态场es考虑静态障碍物的安全性、基于动态场ek考虑动态物体的安全性，构建了主车附近的交通安全模型e＝wl·el ws·es wk·ek，在安全的基础上可进行路径规划和行车安全等判断。
1.一种基于多传感器融合的智能汽车高精度定位与时空态势安全方法，其特征在于：其方法如下所述：
第一部分、高精度定位方法，具体方法如下：
第一步、预处理；
第二步、视觉惯性里程计前端及初始化；
第三步、视觉惯性里程计后端优化；
第四步、全局位姿图优化；
第二部分、时空态势安全方法：
时空态势安全方法以大数据为基础，在设定的时间和空间内对复杂交互式环境中的各组成成分进行感知、理解，进而预知未来变化，从而动态、整体地洞悉风险，同时，基于道路场el考虑道路行驶安全性、基于静态场es考虑静态障碍物的安全性、基于动态场ek考虑动态物体的安全性，构建了主车附近的交通安全模型e＝wl·el ws·es wk·ek，在安全的基础上进行路径规划和行车安全的判断。
2.根据权利要求1所述的一种基于多传感器融合的智能汽车高精度定位与时空态势安全方法，其特征在于：所述的第一部分中第一步预处理的步骤如下：
步骤一、相机标定：以已知大小的平面黑白棋盘格作为标定参照物，通过调节相机与棋盘格的距离和角度提取一系列像素点，找到黑白线交点对应像素点在图像中的坐标，假设黑白线交点的世界坐标为p＝(x，y，z，1)，该交点在相机投影平面的坐标为：p′＝(u，v，1)，根据针孔相机模型有zp′＝kp，其中k为相机内参矩阵，形式如下：
同时，考虑到相机的切向畸变和径向畸变，对相机进行去畸变操作，其归一化坐标写成极坐标形式(r，θ)，有
经过标定后，得到像素点在图像上的正确位置：
其中k1，k2，k3是径向畸变系数，p1，p2是切向畸变系数；
步骤二、动态物体剔除：通过深度学习的方法对图像中的动态物体进行剔除，对相机得到的原始图像进行像素级语义分割并且计算得到的语义图与地面真值之间的损失函数l1，通过卷积神经网络dynss得到仅包括动态物体的语义图，将其和原始灰度图通过对抗生成网络u-net滤除动态物体来得到静态图像并计算其损失函数l2，同时和l1一起进行向后传播，对抗生成网络的工作如下：
其中：
3.根据权利要求1所述的一种基于多传感器融合的智能汽车高精度定位与时空态势安全方法，其特征在于：所述的第一部分中第二步视觉惯性里程计前端及初始化的步骤如下：
步骤一、特征提取、追踪、匹配：对滤除完动态物体的图像进行点、线、面特征的提取，并对连续帧间的特征进行追踪，通过ransac剔除误匹配得到最终特征匹配结果，其中特征点通过fast角点进行提取，特征线通过lsd进行提取，利用金字塔光流法进行特性追踪和特征匹配，金字塔光流法是将原始图像作为底层，通过固定的缩放比例获得具有不同分辨率的图像作为其他层，在计算光流时，首先计算第一层的图像，然后将上一层的跟踪结果用作下一层的初始值，当最后一层的像素运动较大时，金字塔顶部的图像显示的像素运动仍会在较小范围内，从而避免局部最小值或特征追踪丢失的问题；
步骤二、关键帧选取策略：根据不同的工作条件合理地选择不同数量的关键帧，并且应对易发生特征跟踪丢失的工况，采用三种原则来选择第k个帧是否为关键帧：
(1)、特征匹配点数nm与总特征点数ntotal的比率小于比率sthr的阈值；
(2)、在无明显变化的直线行驶中，世界坐标系下的相机估计值和imu估计值之间的位置差超过空间位置差阈值δdthr；
(3)、当以大曲率转弯行驶时，世界坐标系下相机估计值和imu估计值之间的航向角偏差超过航向角偏差阈值
步骤三、imu建模及预积分：考虑到bias随机游走和噪声，imu的加速度
imu的加速度偏差
imu和相机的采样频率不同，对两个连续帧之间的imu测量值进行积分，以计算位置、速度和旋转四元数，转换方程如下：
每次更新
其中：
步骤四、视觉与惯性对齐：首先，根据两个连续帧之间的相机估计值和imu预计分值相等来估计外参qbc，并基于旋转约束构造目标函数来估计bias：
然后，利用平移约束和重力大小来估计重力矢量、速度和尺度：
最后，将第0个相机系下重力向量
4.根据权利要求1所述的一种基于多传感器融合的智能汽车高精度定位与时空态势安全方法，其特征在于：所述的第一部分中第三步视觉惯性里程计后端优化步骤如下：
经过视觉和惯性对齐后，世界坐标系中的位置、速度和旋转四元数，bias外参以及特征点的3d信息
通过使先验误差和传感器测量值误差的mahalanobis范数总和最小来求解最优位姿：
上式通过增量方程进行求解，具体如下：
构建观测值误差、雅可比矩阵及协方差矩阵，具体如下：
步骤一、imu测量值误差：
对连续两帧间imu的测量值、bias求导即可得到雅可比矩阵：
雅可比矩阵如下：
步骤二、视觉观测值误差：
视觉观测值误差主要包括特征点误差、特征线误差和特征面误差；
1)、特征点误差：
将第i帧的特征点投影到第j帧：
特征点重投影误差如下：
2)、特征线误差：
将滤除动态物体图像中提取的特征线表示为点-法线形式，
3)、特征面误差：
将滤除动态物体图像中提取的特征面表示为点-法线形式
基于上述点、线、面误差构建视觉观测值误差模型，并且在整体的误差模型中提高点、线特征所占的权重：
其中，点特征权重αp与线特征权重αl相等，并且都等于面特征权重αs的两倍；
步骤三、重定位误差：
用词袋bow模型来检测回环并使用k-d树来表示字典，k-d树可以容纳kd个词，复杂度为o(kd)，具有很高的搜索速度，当检测到回环后，计算当前帧和bow之间的相似性得分，并将其与关键帧数据库中的所有帧进行比较以获得候选帧，然后，通过ransac提出误匹配，并通过pnp恢复位姿以进行闭环优化，如果匹配点的数量超过阈值，则将候选帧视为正确的闭环帧，最后，将前一帧，称为第v帧的位姿和相关特征点、线、面作为视觉约束添加到后端非线性优化的总体目标函数中，公式如下：
5.根据权利要求1所述的一种基于多传感器融合的智能汽车高精度定位与时空态势安全方法，其特征在于：所述的第一部分中第四步全局位姿图优化的步骤如下：
网络rtk及nrtk采用虚拟基站的方式，虚拟基站与卫星之间的距离
将虚拟基站的误差添加到主站的观测值中，卫星的相对误差和相位误差如下：
其中
位姿图优化的本质是最大似然估计，该估计使用vio和gps/nrtk的旋转和平移作为先验去估计最大测量值：
其中：s是vio和gps/nrtk测量值的集合，x是一段时间内的车辆位姿：
其中：
其中
位姿图包括两个因子：vio因子和gps/nrtk因子，分别如下：
(1)、vio因子：
其中
(2)、gps/nrtk因子：
除了融合上述vio及gps/nrtk，还进一步融合uwb或磁力计传感器，并且通过考虑铺二维码、贴线、短时间内的转角及位置变化这些运动学特性来辅助定位得到最优定位结果。
技术总结