CN112484725A - 一种基于多传感器融合的智能汽车高精度定位与时空态势安全方法 - Google Patents
一种基于多传感器融合的智能汽车高精度定位与时空态势安全方法 Download PDFInfo
- Publication number
- CN112484725A CN112484725A CN202011318597.6A CN202011318597A CN112484725A CN 112484725 A CN112484725 A CN 112484725A CN 202011318597 A CN202011318597 A CN 202011318597A CN 112484725 A CN112484725 A CN 112484725A
- Authority
- CN
- China
- Prior art keywords
- error
- point
- camera
- imu
- feature
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Granted
Links
Images
Classifications
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
- G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
- G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
- G01C21/18—Stabilised platforms, e.g. by gyroscope
-
- Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
- Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
- Y02T—CLIMATE CHANGE MITIGATION TECHNOLOGIES RELATED TO TRANSPORTATION
- Y02T10/00—Road transport of goods or passengers
- Y02T10/10—Internal combustion engine [ICE] based vehicles
- Y02T10/40—Engine management systems
Landscapes
- Engineering & Computer Science (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Automation & Control Theory (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Image Analysis (AREA)
- Navigation (AREA)
Abstract
本发明公开了一种基于多传感器融合的智能汽车高精度定位与时空态势安全方法,其方法为:第一部分、高精度定位方法,其方法为:第一步、预处理;第二步、视觉惯性里程计前端及初始化;第三步、视觉惯性里程计后端优化;第四步、全局位姿图优化;第二部分、时空态势安全方法;有益效果:能够很好适配工作场景及研发的融合策略。本发明实现了一种关键帧选取算法,该算法可以根据不同的工作条件合理选择不同数量的关键帧,并且可以处理容易发生特征跟踪损失的工作条件,本发明的时空态势安全策略充分考虑道路、静态障碍物、动态物体的安全性及交通效率等,具有安全性高、效率高、计算量小、实时性高、技术难度低等优点。
Description
技术领域
本发明涉及一种智能汽车高精度定位与时空态势安全方法,特别涉及一种基于多传感器融合的智能汽车高精度定位与时空态势安全方法。
背景技术
目前,随着智能汽车的快速发展,对于高精度定位技术的需求越来越强。智能汽车定位技术是解决“在哪里”问题的核心技术,其在全局路径规划与局部路径规划中均具有重要意义,是决策规划与运动控制的基础,也是智能交通系统的核心组成部分。高精度定位技术的突破有利于提高交通效率、减少环境污染、改善行驶安全与舒适性等。同时,将智能汽车及障碍物的三维场景信息进行时空态势安全碰撞分析,并以此为决策规划提供数据支撑来提高行驶安全性。常见的智能汽车定位方法有全球卫星导航系统(Global NavigationSatellite System,GNSS)、惯性导航系统(Inertial Navigation System,INS)、航迹推算(Dead Reckoning,DR)、超宽带(Ultra Wide Band,UWB)、地图匹配、同时定位与建图(Simultaneous Localization and Mapping,SLAM)等,独立定位技术相对成熟,但是具有不可避免的缺陷且环境适应性差,智能汽车中很少仅采用单一技术实现准确定位。传感器融合利用技术互补来提高定位系统的精度与稳定性,是智能汽车定位技术的发展趋势。
在国内外研究中,诸多单位对传感器融合定位技术进行了相关研究并提出了诸如GNSS/UWB、UWB/INS、视觉SLAM、激光SLAM、视觉或激光雷达与地图匹配等定位策略。在多传感器融合领域,各个传感器的参数、物理模型及其联合标定结果等直接影响定位结果,同时需要考虑定位精度与实时性的相互制约。在安全避撞领域,国内外大多采用基于三维地理信息来设计相应的安全策略。
目前国内外的多传感器融合定位及安全策略研究有如下特点:
1)GNSS信号易被遮挡且精度低,相机受光照影响较大,惯性测量单元(InertialMeasurement Unit,IMU)误差不断累积且建模大多基于连续帧间bias不变,但仍为参与融合定位的主流定位方法;
2)精度与成本相互制约,基于激光雷达或高精地图或基站RTK成本高,大范围应用受限;
3)普遍对各传感器观测值进行高斯滤波,未考虑实际误差分布,并且对传感器的物理建模大部分基于连续时间内bias、灰度不变的假设,模型没有贴合传感器的实际工作状态;
4)主要通过离线标定传感器,标定繁琐且误差大,并且不能很好适配工作场景及研发的融合策略;
5)基于滤波的融合框架易产生误差累积并且对多传感器时钟同步要求高,基于优化的融合框架计算量大并且难以满足实时性要求;
6)视觉定位在大曲率行驶、特征变化不明显的直线行驶及动态物体影响行驶时易出现特征误匹配及特征追踪丢失;
7)通过相对位置来获取障碍物全局定位信息易引起误差累积,并且安全策略大多未考虑障碍物危险程度及交通效率等。
中国专利CN 109141410 B授权了一种AGV组合导航的多创安其融合定位方法,通过线性化融合陀螺仪、角速度计和磁力计来求解姿态。由于陀螺仪和角速度计的工作原理决定其定位误差会随时间不断累积,因此无法用于长时间定位。磁力计根据磁场强度进行定位,易受外部环境干扰且精度差,不适用于智能汽车。
中国专利CN 109737950 B授权了一种基于VIO和动力学模型的水下机器人定位系统及方法,通过试验法测得相关参数并求解动力学方程得到速度及加速度,将其和VIO的定位输出通过扩展卡尔曼滤波进行松耦合融合得到速度估计值,再通过积分得到位置估计值。由于VIO对IMU及视觉传感器的物理建模基于连续时间内bias、灰度不变的假设,模型没有贴合传感器的实际工作状态,将导致从IMU源头引入了误差。另外,通过离线标定IMU和视觉传感器,标定繁琐且误差大,并且不能很好适配工作场景及研发的融合策略。上述方法通过基于滤波的松耦合方法进行多传感器融合,易产生误差累积并且对多传感器时钟同步要求高。
中国专利CN 202010545062.6公开了一种基于点线特征融合激光的语义高精地图构建与定位方法。通过视觉和激光雷达分别对视觉图像和三维点云进行语义分割和点线特征提取,通过相机进行粗略估计并将估计值作为初始值用于迭代激光雷达定位来获得细致估计。视觉定位在大曲率行驶及特征变化不明显的直线行驶时易出现特征误匹配及特征追踪丢失,激光雷达成本高且计算量大,难以满足实时性,高精地图成本高且需要定时更新维护,难以应对复杂多变的场景。上述方法基于优化紧耦合融合视觉和IMU,虽然后端优化能够对其误差校正,但仍然缺乏全局一致性。另外,基于优化的融合框架计算量大且难以满足实时性要求。
中国专利CN 202010208367.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;
IMU和相机的采样频率不同,对两个连续帧之间的IMU测量值进行积分,以计算位置、速度和旋转四元数,转换方程如下:
每次更新和时,需要重新积分上式来计算位置、速度和旋转四元数,这将导致计算量很大并影响实时性,因此,将优化变量从IMU积分项的第i帧分离到第j帧。根据坐标系的旋转,将等式进行从word系到第i个body系的旋转,得到连续时刻的IMU预积分公式:
其中:
步骤四、视觉与惯性对齐:首先,根据两个连续帧之间的相机估计值和IMU预计分值相等来估计外参qbc,并基于旋转约束构造目标函数来估计bias:
然后,利用平移约束和重力大小来估计重力矢量、速度和尺度:
第一部分中第三步视觉惯性里程计后端优化步骤如下:
通过使先验误差和传感器测量值误差的Mahalanobis范数总和最小来求解最优位姿:
上式通过增量方程进行求解,具体如下:
构建观测值误差、雅可比矩阵及协方差矩阵,具体如下:
步骤一、IMU测量值误差:
对连续两帧间IMU的测量值、bias求导即可得到雅可比矩阵:
雅可比矩阵如下:
步骤二、视觉观测值误差:
视觉观测值误差主要包括特征点误差、特征线误差和特征面误差;
1)、特征点误差:
将第i帧的特征点投影到第j帧:
特征点重投影误差如下:
2)、特征线误差:
将滤除动态物体图像中提取的特征线表示为点-法线形式,其中为特征线上的任意一点,为特征线单位法向向量,通过三维空间特征线的起点坐标P1和终点坐标P2来参数化表示三维特征线Li=[P1,P2],通过点到直线距离构建特征线对应的残差:
3)、特征面误差:
基于上述点、线、面误差构建视觉观测值误差模型,并且在整体的误差模型中提高点、线特征所占的权重:
其中,点特征权重αp与线特征权重αl相等,并且都等于面特征权重αs的两倍。
步骤三、重定位误差:
用词袋BoW模型来检测回环并使用k-d树来表示字典,k-d树可以容纳kd个词,复杂度为O(kd),具有很高的搜索速度,当检测到回环后,计算当前帧和BoW之间的相似性得分,并将其与关键帧数据库中的所有帧进行比较以获得候选帧,然后,通过RANSAC提出误匹配,并通过PnP恢复位姿以进行闭环优化,如果匹配点的数量超过阈值,则将候选帧视为正确的闭环帧,最后,将前一帧,称为第v帧的位姿和相关特征点、线、面作为视觉约束添加到后端非线性优化的总体目标函数中,公式如下:
第一部分中第四步全局位姿图优化的步骤如下:
将虚拟基站的误差添加到主站的观测值中,卫星的相对误差和相位误差如下:
位姿图优化的本质是最大似然估计,该估计使用VIO和GPS/NRTK的旋转和平移作为先验去估计最大测量值:
其中:S是VIO和GPS/NRTK测量值的集合,X是一段时间内的车辆位姿:
位姿图包括两个因子: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建模及预积分
如图6所示,由于IMU和相机的采样频率不同,因此对两个连续帧之间的IMU测量值进行积分,以计算位置、速度和旋转四元数,转换方程如下:
每次更新和时,需要重新积分上式来计算位置、速度和旋转四元数,这将导致计算量很大并影响实时性。因此,将优化变量从IMU积分项的第i帧分离到第j帧。根据坐标系的旋转,将等式进行从word系到第i个body系的旋转,可以得到连续时刻的IMU预积分公式:
其中:
4.视觉与惯性对齐:
如图7所示,首先,根据两个连续帧之间的相机估计值和IMU预计分值相等来估计外参qbc,并基于旋转约束构造目标函数来估计bias:
然后,利用平移约束和重力大小来估计重力矢量、速度和尺度:
三、视觉惯性里程计后端优化如下:
通过使先验误差和传感器测量值误差的Mahalanobis范数总和最小来求解最优位姿:
上式通过增量方程进行求解,具体如下:
构建观测值误差、雅可比矩阵及协方差矩阵,具体如下:
1.IMU测量值误差
对连续两帧间IMU的测量值、bias求导即可得到雅可比矩阵:
雅可比矩阵如下:
2.视觉观测值误差
视觉观测值误差主要包括特征点误差、特征线误差、特征面误差。
2.1特征点误差
将第i帧的特征点投影到第j帧:
特征点重投影误差如下:
2.2特征线误差
将滤除动态物体图像中提取的特征线表示为点-法线形式,其中为特征线上的任意一点,为特征线单位法向向量。通过三维空间特征线的起点坐标P1和终点坐标P2来参数化表示三维特征线Li=[P1,P2]。通过点到直线距离构建特征线对应的残差:
2.3特征面误差
基于上述点、线、面误差构建视觉观测值误差模型,并且在整体的误差模型中提高点、线特征所占的权重:
其中,点特征权重αp与线特征权重αl相等,并且都等于面特征权重αs的两倍。
3.重定位误差
用词袋(BoW)模型来检测回环并使用k-d树来表示字典。k-d树可以容纳kd个词,复杂度为O(kd),具有很高的搜索速度。当检测到回环后,计算当前帧和BoW之间的相似性得分,并将其与关键帧数据库中的所有帧进行比较以获得候选帧。然后,通过RANSAC提出误匹配,并通过PnP恢复位姿以进行闭环优化。如果匹配点的数量超过阈值,则将候选帧视为正确的闭环帧。最后,将前一帧(称为第v帧)的位姿和相关特征点、线、面作为视觉约束添加到后端非线性优化的总体目标函数中,公式如下:
四、全局位姿图优化如下:
将虚拟基站的误差添加到主站的观测值中,卫星的相对误差和相位误差如下:
位姿图优化的本质是最大似然估计,该估计使用VIO和GPS/NRTK的旋转和平移作为先验去估计最大测量值:
其中S是VIO和GPS/NRTK测量值的集合,X是一段时间内的车辆位姿:
如图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,在安全的基础上可进行路径规划和行车安全等判断。
Claims (5)
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;
IMU和相机的采样频率不同,对两个连续帧之间的IMU测量值进行积分,以计算位置、速度和旋转四元数,转换方程如下:
每次更新和时,需要重新积分上式来计算位置、速度和旋转四元数,这将导致计算量很大并影响实时性,因此,将优化变量从IMU积分项的第i帧分离到第j帧,根据坐标系的旋转,将等式进行从word系到第i个body系的旋转,得到连续时刻的IMU预积分公式:
其中:
步骤四、视觉与惯性对齐:首先,根据两个连续帧之间的相机估计值和IMU预计分值相等来估计外参qbc,并基于旋转约束构造目标函数来估计bias:
然后,利用平移约束和重力大小来估计重力矢量、速度和尺度:
4.根据权利要求1所述的一种基于多传感器融合的智能汽车高精度定位与时空态势安全方法,其特征在于:所述的第一部分中第三步视觉惯性里程计后端优化步骤如下:
通过使先验误差和传感器测量值误差的Mahalanobis范数总和最小来求解最优位姿:
上式通过增量方程进行求解,具体如下:
构建观测值误差、雅可比矩阵及协方差矩阵,具体如下:
步骤一、IMU测量值误差:
对连续两帧间IMU的测量值、bias求导即可得到雅可比矩阵:
雅可比矩阵如下:
步骤二、视觉观测值误差:
视觉观测值误差主要包括特征点误差、特征线误差和特征面误差;
1)、特征点误差:
将第i帧的特征点投影到第j帧:
特征点重投影误差如下:
2)、特征线误差:
将滤除动态物体图像中提取的特征线表示为点-法线形式,其中为特征线上的任意一点,为特征线单位法向向量,通过三维空间特征线的起点坐标P1和终点坐标P2来参数化表示三维特征线Li=[P1,P2],通过点到直线距离构建特征线对应的残差:
3)、特征面误差:
基于上述点、线、面误差构建视觉观测值误差模型,并且在整体的误差模型中提高点、线特征所占的权重:
其中,点特征权重αp与线特征权重αl相等,并且都等于面特征权重αs的两倍;
步骤三、重定位误差:
用词袋BoW模型来检测回环并使用k-d树来表示字典,k-d树可以容纳kd个词,复杂度为O(kd),具有很高的搜索速度,当检测到回环后,计算当前帧和BoW之间的相似性得分,并将其与关键帧数据库中的所有帧进行比较以获得候选帧,然后,通过RANSAC提出误匹配,并通过PnP恢复位姿以进行闭环优化,如果匹配点的数量超过阈值,则将候选帧视为正确的闭环帧,最后,将前一帧,称为第v帧的位姿和相关特征点、线、面作为视觉约束添加到后端非线性优化的总体目标函数中,公式如下:
5.根据权利要求1所述的一种基于多传感器融合的智能汽车高精度定位与时空态势安全方法,其特征在于:所述的第一部分中第四步全局位姿图优化的步骤如下:
将虚拟基站的误差添加到主站的观测值中,卫星的相对误差和相位误差如下:
位姿图优化的本质是最大似然估计,该估计使用VIO和GPS/NRTK的旋转和平移作为先验去估计最大测量值:
其中:S是VIO和GPS/NRTK测量值的集合,X是一段时间内的车辆位姿:
位姿图包括两个因子:VIO因子和GPS/NRTK因子,分别如下:
(1)、VIO因子:
(2)、GPS/NRTK因子:
除了融合上述VIO及GPS/NRTK,还进一步融合UWB或磁力计传感器,并且通过考虑铺二维码、贴线、短时间内的转角及位置变化这些运动学特性来辅助定位得到最优定位结果。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202011318597.6A CN112484725B (zh) | 2020-11-23 | 2020-11-23 | 一种基于多传感器融合的智能汽车高精度定位与时空态势安全方法 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202011318597.6A CN112484725B (zh) | 2020-11-23 | 2020-11-23 | 一种基于多传感器融合的智能汽车高精度定位与时空态势安全方法 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN112484725A true CN112484725A (zh) | 2021-03-12 |
CN112484725B CN112484725B (zh) | 2023-03-21 |
Family
ID=74933412
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN202011318597.6A Active CN112484725B (zh) | 2020-11-23 | 2020-11-23 | 一种基于多传感器融合的智能汽车高精度定位与时空态势安全方法 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN112484725B (zh) |
Cited By (21)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN112945240A (zh) * | 2021-03-16 | 2021-06-11 | 北京三快在线科技有限公司 | 特征点位置的确定方法、装置、设备及可读存储介质 |
CN113124856A (zh) * | 2021-05-21 | 2021-07-16 | 天津大学 | 基于uwb在线锚点的视觉惯性紧耦合里程计及计量方法 |
CN113188557A (zh) * | 2021-04-28 | 2021-07-30 | 江苏方天电力技术有限公司 | 一种融合语义特征的视觉惯性组合导航方法 |
CN113204039A (zh) * | 2021-05-07 | 2021-08-03 | 深圳亿嘉和科技研发有限公司 | 一种应用于机器人的rtk-gnss外参标定方法 |
CN113359167A (zh) * | 2021-04-16 | 2021-09-07 | 电子科技大学 | 一种通过惯性测量参数将gps与激光雷达融合定位的方法 |
CN113375665A (zh) * | 2021-06-18 | 2021-09-10 | 西安电子科技大学 | 基于多传感器松紧耦合的无人机位姿估计方法 |
CN113418528A (zh) * | 2021-05-31 | 2021-09-21 | 江苏大学 | 一种面向智能汽车的交通场景语义建模装置、建模方法以及定位方法 |
CN113436270A (zh) * | 2021-06-18 | 2021-09-24 | 上海商汤临港智能科技有限公司 | 传感器标定方法及装置、电子设备和存储介质 |
CN113503873A (zh) * | 2021-07-14 | 2021-10-15 | 北京理工大学 | 一种多传感器融合的视觉定位方法 |
CN113706592A (zh) * | 2021-08-24 | 2021-11-26 | 北京百度网讯科技有限公司 | 一种定位信息的修正方法、装置、电子设备和存储介质 |
CN113776519A (zh) * | 2021-09-14 | 2021-12-10 | 西南科技大学 | 一种无光动态开放环境下agv车辆建图与自主导航避障方法 |
CN113790726A (zh) * | 2021-09-07 | 2021-12-14 | 中国科学院合肥物质科学研究院 | 一种融合相机、轮速计和单uwb信息的机器人室内定位方法 |
CN113865584A (zh) * | 2021-08-24 | 2021-12-31 | 知微空间智能科技(苏州)有限公司 | 一种基于视觉惯性里程计的uwb三维寻物方法和装置 |
CN113947639A (zh) * | 2021-10-27 | 2022-01-18 | 北京斯年智驾科技有限公司 | 基于多雷达点云线特征的自适应在线估计标定系统及方法 |
CN114222366A (zh) * | 2021-08-06 | 2022-03-22 | 深圳技术大学 | 一种基于单基站的室内定位方法及装置 |
CN114370870A (zh) * | 2022-01-05 | 2022-04-19 | 中国兵器工业计算机应用技术研究所 | 适用于位姿测量卡尔曼滤波的滤波器更新信息筛选方法 |
CN115272494A (zh) * | 2022-09-29 | 2022-11-01 | 腾讯科技(深圳)有限公司 | 相机与惯性测量单元的标定方法、装置和计算机设备 |
CN115406447A (zh) * | 2022-10-31 | 2022-11-29 | 南京理工大学 | 拒止环境下基于视觉惯性的四旋翼无人机自主定位方法 |
CN115571156A (zh) * | 2022-09-23 | 2023-01-06 | 东南大学 | 基于传感器融合的前车横向与纵向运动状态联合估计方法 |
CN115855117A (zh) * | 2023-02-16 | 2023-03-28 | 深圳佑驾创新科技有限公司 | 相机和惯性测量单元相对车体安装姿态的联合标定方法 |
CN117745777A (zh) * | 2024-01-05 | 2024-03-22 | 哈尔滨工业大学 | 一种基于时空配准的遥感检测地表密集异常元去除方法 |
Citations (6)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
US20180188032A1 (en) * | 2017-01-04 | 2018-07-05 | Qualcomm Incorporated | Systems and methods for using a global positioning system velocity in visual-inertial odometry |
US20180188384A1 (en) * | 2017-01-04 | 2018-07-05 | Qualcomm Incorporated | Systems and methods for using a sliding window of global positioning epochs in visual-inertial odometry |
CN110345944A (zh) * | 2019-05-27 | 2019-10-18 | 浙江工业大学 | 融合视觉特征和imu信息的机器人定位方法 |
CN111156984A (zh) * | 2019-12-18 | 2020-05-15 | 东南大学 | 一种面向动态场景的单目视觉惯性slam方法 |
CN111311680A (zh) * | 2020-02-13 | 2020-06-19 | 吉林大学 | 一种三维激光雷达与机械臂间的自动联合标定方法 |
CN111845754A (zh) * | 2020-07-27 | 2020-10-30 | 扬州大学 | 一种基于边缘计算和群智算法的自动驾驶车辆的决策预测方法 |
-
2020
- 2020-11-23 CN CN202011318597.6A patent/CN112484725B/zh active Active
Patent Citations (6)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
US20180188032A1 (en) * | 2017-01-04 | 2018-07-05 | Qualcomm Incorporated | Systems and methods for using a global positioning system velocity in visual-inertial odometry |
US20180188384A1 (en) * | 2017-01-04 | 2018-07-05 | Qualcomm Incorporated | Systems and methods for using a sliding window of global positioning epochs in visual-inertial odometry |
CN110345944A (zh) * | 2019-05-27 | 2019-10-18 | 浙江工业大学 | 融合视觉特征和imu信息的机器人定位方法 |
CN111156984A (zh) * | 2019-12-18 | 2020-05-15 | 东南大学 | 一种面向动态场景的单目视觉惯性slam方法 |
CN111311680A (zh) * | 2020-02-13 | 2020-06-19 | 吉林大学 | 一种三维激光雷达与机械臂间的自动联合标定方法 |
CN111845754A (zh) * | 2020-07-27 | 2020-10-30 | 扬州大学 | 一种基于边缘计算和群智算法的自动驾驶车辆的决策预测方法 |
Non-Patent Citations (2)
Title |
---|
BERTA BESCOS等: "Empty Cities: Image Inpainting for a Dynamic-Object-Invariant Space", 《2019 INTERNATIONAL CONFERENCE ON ROBOTICS AND AUTOMATION (ICRA)》 * |
TONG QIN等: "VINS-Mono: A Robust and Versatile Monocular Visual-Inertial State Estimator", 《IEEE TRANSACTIONS ON ROBOTICS 》 * |
Cited By (34)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN112945240B (zh) * | 2021-03-16 | 2022-06-07 | 北京三快在线科技有限公司 | 特征点位置的确定方法、装置、设备及可读存储介质 |
CN112945240A (zh) * | 2021-03-16 | 2021-06-11 | 北京三快在线科技有限公司 | 特征点位置的确定方法、装置、设备及可读存储介质 |
CN113359167A (zh) * | 2021-04-16 | 2021-09-07 | 电子科技大学 | 一种通过惯性测量参数将gps与激光雷达融合定位的方法 |
CN113188557A (zh) * | 2021-04-28 | 2021-07-30 | 江苏方天电力技术有限公司 | 一种融合语义特征的视觉惯性组合导航方法 |
CN113188557B (zh) * | 2021-04-28 | 2023-10-20 | 江苏方天电力技术有限公司 | 一种融合语义特征的视觉惯性组合导航方法 |
CN113204039A (zh) * | 2021-05-07 | 2021-08-03 | 深圳亿嘉和科技研发有限公司 | 一种应用于机器人的rtk-gnss外参标定方法 |
CN113124856A (zh) * | 2021-05-21 | 2021-07-16 | 天津大学 | 基于uwb在线锚点的视觉惯性紧耦合里程计及计量方法 |
CN113418528A (zh) * | 2021-05-31 | 2021-09-21 | 江苏大学 | 一种面向智能汽车的交通场景语义建模装置、建模方法以及定位方法 |
CN113375665A (zh) * | 2021-06-18 | 2021-09-10 | 西安电子科技大学 | 基于多传感器松紧耦合的无人机位姿估计方法 |
CN113436270A (zh) * | 2021-06-18 | 2021-09-24 | 上海商汤临港智能科技有限公司 | 传感器标定方法及装置、电子设备和存储介质 |
CN113436270B (zh) * | 2021-06-18 | 2023-04-25 | 上海商汤临港智能科技有限公司 | 传感器标定方法及装置、电子设备和存储介质 |
CN113375665B (zh) * | 2021-06-18 | 2022-12-02 | 西安电子科技大学 | 基于多传感器松紧耦合的无人机位姿估计方法 |
CN113503873A (zh) * | 2021-07-14 | 2021-10-15 | 北京理工大学 | 一种多传感器融合的视觉定位方法 |
CN113503873B (zh) * | 2021-07-14 | 2024-03-12 | 北京理工大学 | 一种多传感器融合的视觉定位方法 |
CN114222366A (zh) * | 2021-08-06 | 2022-03-22 | 深圳技术大学 | 一种基于单基站的室内定位方法及装置 |
CN114222366B (zh) * | 2021-08-06 | 2023-08-01 | 深圳技术大学 | 一种基于单基站的室内定位方法及装置 |
CN113865584A (zh) * | 2021-08-24 | 2021-12-31 | 知微空间智能科技(苏州)有限公司 | 一种基于视觉惯性里程计的uwb三维寻物方法和装置 |
CN113706592A (zh) * | 2021-08-24 | 2021-11-26 | 北京百度网讯科技有限公司 | 一种定位信息的修正方法、装置、电子设备和存储介质 |
CN113865584B (zh) * | 2021-08-24 | 2024-05-03 | 知微空间智能科技(苏州)有限公司 | 一种基于视觉惯性里程计的uwb三维寻物方法和装置 |
CN113790726B (zh) * | 2021-09-07 | 2024-03-29 | 中国科学院合肥物质科学研究院 | 一种融合相机、轮速计和单uwb信息的机器人室内定位方法 |
CN113790726A (zh) * | 2021-09-07 | 2021-12-14 | 中国科学院合肥物质科学研究院 | 一种融合相机、轮速计和单uwb信息的机器人室内定位方法 |
CN113776519A (zh) * | 2021-09-14 | 2021-12-10 | 西南科技大学 | 一种无光动态开放环境下agv车辆建图与自主导航避障方法 |
CN113947639B (zh) * | 2021-10-27 | 2023-08-18 | 北京斯年智驾科技有限公司 | 基于多雷达点云线特征的自适应在线估计标定系统及方法 |
CN113947639A (zh) * | 2021-10-27 | 2022-01-18 | 北京斯年智驾科技有限公司 | 基于多雷达点云线特征的自适应在线估计标定系统及方法 |
CN114370870B (zh) * | 2022-01-05 | 2024-04-12 | 中国兵器工业计算机应用技术研究所 | 适用于位姿测量卡尔曼滤波的滤波器更新信息筛选方法 |
CN114370870A (zh) * | 2022-01-05 | 2022-04-19 | 中国兵器工业计算机应用技术研究所 | 适用于位姿测量卡尔曼滤波的滤波器更新信息筛选方法 |
CN115571156A (zh) * | 2022-09-23 | 2023-01-06 | 东南大学 | 基于传感器融合的前车横向与纵向运动状态联合估计方法 |
CN115571156B (zh) * | 2022-09-23 | 2023-12-26 | 东南大学 | 基于传感器融合的前车横向与纵向运动状态联合估计方法 |
CN115272494A (zh) * | 2022-09-29 | 2022-11-01 | 腾讯科技(深圳)有限公司 | 相机与惯性测量单元的标定方法、装置和计算机设备 |
WO2024066816A1 (zh) * | 2022-09-29 | 2024-04-04 | 腾讯科技(深圳)有限公司 | 相机与惯性测量单元的标定方法、装置和计算机设备 |
CN115406447A (zh) * | 2022-10-31 | 2022-11-29 | 南京理工大学 | 拒止环境下基于视觉惯性的四旋翼无人机自主定位方法 |
CN115855117B (zh) * | 2023-02-16 | 2023-06-02 | 深圳佑驾创新科技有限公司 | 相机和惯性测量单元相对车体安装姿态的联合标定方法 |
CN115855117A (zh) * | 2023-02-16 | 2023-03-28 | 深圳佑驾创新科技有限公司 | 相机和惯性测量单元相对车体安装姿态的联合标定方法 |
CN117745777A (zh) * | 2024-01-05 | 2024-03-22 | 哈尔滨工业大学 | 一种基于时空配准的遥感检测地表密集异常元去除方法 |
Also Published As
Publication number | Publication date |
---|---|
CN112484725B (zh) | 2023-03-21 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN112484725B (zh) | 一种基于多传感器融合的智能汽车高精度定位与时空态势安全方法 | |
Heo et al. | EKF-based visual inertial navigation using sliding window nonlinear optimization | |
CN113781582A (zh) | 基于激光雷达和惯导联合标定的同步定位与地图创建方法 | |
CN114526745B (zh) | 一种紧耦合激光雷达和惯性里程计的建图方法及系统 | |
CN111862673B (zh) | 基于顶视图的停车场车辆自定位及地图构建方法 | |
Tao et al. | A multi-sensor fusion positioning strategy for intelligent vehicles using global pose graph optimization | |
CN110726406A (zh) | 一种改进的非线性优化单目惯导slam的方法 | |
CN104281148A (zh) | 基于双目立体视觉的移动机器人自主导航方法 | |
Zhou et al. | A lidar odometry for outdoor mobile robots using ndt based scan matching in gps-denied environments | |
Li et al. | Robust localization for intelligent vehicles based on compressed road scene map in urban environments | |
CN115574816B (zh) | 仿生视觉多源信息智能感知无人平台 | |
CN114529576A (zh) | 一种基于滑动窗口优化的rgbd和imu混合跟踪注册方法 | |
CN112781582A (zh) | 卫星弱观测条件下的多传感器融合高精度位姿估计算法 | |
CN115930977A (zh) | 特征退化场景的定位方法、系统、电子设备和可读存介质 | |
CN117367427A (zh) | 一种适用于室内环境中的视觉辅助激光融合IMU的多模态slam方法 | |
Wang et al. | Extraction of preview elevation information based on terrain mapping and trajectory prediction in real-time | |
Anousaki et al. | Simultaneous localization and map building of skid-steered robots | |
Liu et al. | A multi-sensor fusion with automatic vision-LiDAR calibration based on Factor graph joint optimization for SLAM | |
CN117782063A (zh) | 一种基于图优化的可变滑动窗口的多传感器融合定位方法 | |
Zhang et al. | An open-source, fiducial-based, underwater stereo visual-inertial localization method with refraction correction | |
Barrau et al. | Invariant filtering for pose ekf-slam aided by an imu | |
CN117075158A (zh) | 基于激光雷达的无人变形运动平台的位姿估计方法及系统 | |
Zhang et al. | A robust lidar slam system based on multi-sensor fusion | |
Lacroix et al. | Integration of concurrent localization algorithms for a planetary rover | |
Housein et al. | Extended Kalman filter sensor fusion in practice for mobile robot localization |
Legal Events
Date | Code | Title | Description |
---|---|---|---|
PB01 | Publication | ||
PB01 | Publication | ||
SE01 | Entry into force of request for substantive examination | ||
SE01 | Entry into force of request for substantive examination | ||
GR01 | Patent grant | ||
GR01 | Patent grant |