CN115435775A - 基于拓展卡尔曼滤波的多传感器融合slam方法 - Google Patents

基于拓展卡尔曼滤波的多传感器融合slam方法 Download PDF

Info

Publication number
CN115435775A
CN115435775A CN202211163166.6A CN202211163166A CN115435775A CN 115435775 A CN115435775 A CN 115435775A CN 202211163166 A CN202211163166 A CN 202211163166A CN 115435775 A CN115435775 A CN 115435775A
Authority
CN
China
Prior art keywords
pose
time
laser radar
data
sensor
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.)
Pending
Application number
CN202211163166.6A
Other languages
English (en)
Inventor
张卫波
温珍林
黄晓军
黄赐坤
封士宇
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Fuzhou University
Original Assignee
Fuzhou University
Priority date (The priority date 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 date listed.)
Filing date
Publication date
Application filed by Fuzhou University filed Critical Fuzhou University
Priority to CN202211163166.6A priority Critical patent/CN115435775A/zh
Publication of CN115435775A publication Critical patent/CN115435775A/zh
Pending legal-status Critical Current

Links

Images

Classifications

    • GPHYSICS
    • G01MEASURING; TESTING
    • G01CMEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/005Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 with correlation of navigation data from several sources, e.g. map or contour matching
    • GPHYSICS
    • G01MEASURING; TESTING
    • G01CMEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/10Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
    • G01C21/12Navigation; 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/16Navigation; 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/165Navigation; 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 combined with non-inertial navigation instruments
    • GPHYSICS
    • G01MEASURING; TESTING
    • G01CMEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/10Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
    • G01C21/12Navigation; 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/16Navigation; 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/165Navigation; 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 combined with non-inertial navigation instruments
    • G01C21/1652Navigation; 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 combined with non-inertial navigation instruments with ranging devices, e.g. LIDAR or RADAR
    • GPHYSICS
    • G01MEASURING; TESTING
    • G01CMEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/20Instruments for performing navigational calculations
    • GPHYSICS
    • G06COMPUTING; CALCULATING OR COUNTING
    • G06TIMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T7/00Image analysis
    • G06T7/20Analysis of motion
    • G06T7/277Analysis of motion involving stochastic approaches, e.g. using Kalman filters
    • GPHYSICS
    • G06COMPUTING; CALCULATING OR COUNTING
    • G06TIMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T7/00Image analysis
    • G06T7/70Determining position or orientation of objects or cameras
    • G06T7/73Determining position or orientation of objects or cameras using feature-based methods
    • GPHYSICS
    • G06COMPUTING; CALCULATING OR COUNTING
    • G06TIMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T2207/00Indexing scheme for image analysis or image enhancement
    • G06T2207/10Image acquisition modality
    • G06T2207/10032Satellite or aerial image; Remote sensing
    • G06T2207/10044Radar image
    • GPHYSICS
    • G06COMPUTING; CALCULATING OR COUNTING
    • G06TIMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T2207/00Indexing scheme for image analysis or image enhancement
    • G06T2207/30Subject of image; Context of image processing
    • G06T2207/30244Camera pose

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Automation & Control Theory (AREA)
  • Computer Vision & Pattern Recognition (AREA)
  • Theoretical Computer Science (AREA)
  • Multimedia (AREA)
  • Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)

Abstract

本发明公开了一种基于拓展卡尔曼滤波的多传感器融合SLAM方法,针对走廊相似场景下,激光SLAM算法由于观测数据的相似性,算法性能将严重劣化,甚至完全失效的问题,本发明将里程计和IMU的数据进行预处理,通过拓展卡尔曼滤波将两者的位姿信息融合,作为激光雷达扫描匹配更精确的迭代初始位姿;为验证本算法的性能,在Melodic版本的ROS(Robot Operating System)搭建了Gazebo仿真实验环境,通过仿真实验对比,验证了算法的鲁棒性和有效性。

Description

基于拓展卡尔曼滤波的多传感器融合SLAM方法
技术领域
本发明涉及无人驾驶汽车和移动机器人在未知环境中运行时定位导航与地图构建技术领域,具体涉及一种基于拓展卡尔曼滤波的多传感器融合SLAM(同时定位和建图)方法。
背景技术
无人驾驶技术和辅助驾驶技术都离不开SLAM的发展,同时SLAM也是实现自动驾驶的关键一环。当地图和机器人的位置都事先未知时,在这种情况下,要求机器人在一个完全未知的环境中从一个未知的位置出发,在递增地建立环境的导航地图同时,利用已建立的地图来同步刷新自身的位置。在上述问题中,机器人位置和地图两者的估算是高度相关的,任何一方都无法独立获取。这样一种相辅相生、不断迭代的过程,简称为SLAM问题。
尽管SLAM近些年取得了巨大的进步,但有一些问题仍然困扰着无人驾驶界,SLAM是一个估算问题,考虑到环境的复杂性和传感器测量的不确定性,目前还没有一个比较完整的解决方案。通常的SLAM方法都是局限于用单一传感器收集环境信息,而使用单一的传感器容易出现各种问题,达不到智能驾驶的要求。
发明内容
本发明目的在于针对环境特征高度相似场景容易出现SLAM匹配失效的问题以及单一传感器建图精度不高等问题,提出了一种基于拓展卡尔曼滤波的多传感器融合SLAM(同时定位和建图)方法。
本发明属于无人驾驶汽车在未知环境中运行时定位导航与地图构建领域,具体为一种基于拓展卡尔曼滤波的多传感器融合SLAM(Simultaneous Localization andMapping)方法,本发明为了解决SLAM(同步定位与地图创建)过程中单一传感器容易出现的定位精度不高、激光点云匹配计算量大、轨迹闭合效果差、位姿累积误差大、以及各环节传感器观测数据利用不充分等问题。提出了一种基于拓展卡尔曼滤波的惯性里程计(IMU)、轮速计、单线激光雷达多传感器信息融合智能车SLAM方法。针对走廊相似场景下,激光SLAM算法由于观测数据的相似性,算法性能将严重劣化,甚至完全失效的问题,本发明将里程计和IMU的数据进行预处理,通过拓展卡尔曼滤波将两者的位姿信息融合,作为激光雷达扫描匹配更精确的迭代初始位姿;为验证本算法的性能,在Melodic版本的ROS(Robot OperatingSystem)搭建了Gazebo仿真实验环境,通过仿真实验对比,验证了算法的鲁棒性和有效性。
为实现上述目的,本发明采取如下技术方案:
一种基于拓展卡尔曼滤波的多传感器融合SLAM方法,其特征在于,包括以下步骤:
步骤S1:读取IMU、轮速计的数据信息并进行预处理;
步骤S2:采用拓展卡尔曼滤波融合IMU、轮速计预处理的位姿得到融合位姿;
步骤S3:激光雷达数据预处理,将融合后的位姿为激光雷达数据做线性插值去运动畸变;
步骤S4:将融合处理之后的位姿作为激光雷达扫描匹配的迭代初始位姿,开始SLAM前端建立局部子图;
步骤S5:对于构建完成的子图判断是否形成回环,进行SLAM后端回环检测以减少累计误差,并将当前时刻对应的位姿约束加入到非线性优化中;
步骤S6:当没有新的激光雷达Scan产生,输出全局优化后的轨迹位姿,并将产生的全局地图可视化。
进一步地,步骤S1具体包括:
对惯性测量单元IMU预积分求得惯性里程计;
IMU每个时刻进行自身3轴加速度和角速度的测量,角速度
Figure BDA0003860717460000021
与加速度
Figure BDA0003860717460000022
的测量模型分别为:
Figure BDA0003860717460000023
Figure BDA0003860717460000024
其中
Figure BDA0003860717460000025
为实际角速度,bg是角速度随时间缓慢变化的偏差bias,ηg是角速度白噪声,
Figure BDA0003860717460000026
为旋转矩阵,aw为实际加速度,gw表示重力矢量,w表示世界坐标系作为惯性系,ba是加速度随时间变化的偏差,ηa是加速度白噪声;
运动方程为:
Figure BDA0003860717460000027
Figure BDA0003860717460000028
Figure BDA0003860717460000029
公式中∧表示反对称矩阵;
在区间[t,t+Δt]里进行欧拉积分有:
Figure BDA0003860717460000031
Figure BDA0003860717460000032
Figure BDA0003860717460000033
Figure BDA0003860717460000034
为所求旋转矩阵,pw(t+Δt)为所求位移,从而求得惯性里程计;
计算轮速计数据求得轮边里程计信息:
通过轮速计得到的两轮角速度和两轮线速度求解横向位移、纵向位移和航向角;
根据运动模型计算出小车的速度v和角速度ω:
Figure BDA0003860717460000035
Figure BDA0003860717460000036
公式vL、vR分别表示左右轮的线速度,d为轮胎离底盘中心的距离;
对时间积分得到里程信息:
θ(t)=∫ω(t)dt
x(t)=∫v(t)cos(θ(t))dt
y(t)=∫v(t)sin(θ(t))dt
式中θ(t)为航行角,x(t)为横轴位移,y(t)为纵轴位移;
进行位姿数据融合前时间上和空间上的统一;
以激光雷达发布的时间为基准进行时间上的统一:当每一帧激光传感器数据到来时,各传感器的时间取比基准时间小且离的最近的时间作为初始时间,当下一帧数据到来时,取比基准时间大且最近的时间作为终止时间,计算这两个时刻的时间差作为积分时间;
将得到的数据进行坐标系的变换以得到空间上的统一:根据各传感器的安装位姿,利用Eigen库进行坐标系的转换,得到变换的平移矩阵和旋转矩阵,再将其他传感器坐标系统一到小车的基座标系,W为基座标系,C为需要对齐的坐标系,X、Y、Z分别为空间坐标系下的坐标;
变换公式为:
Figure BDA0003860717460000037
为了方便计算改写成增广矩阵:
Figure BDA0003860717460000041
公式中Rc为3*3的旋转矩阵,Tc为3*1的平移矩阵:
Figure BDA0003860717460000042
公式中RX(α)为绕X轴旋转α角度的旋转矩阵,公式中RY(β)为绕Y轴旋转β角度的旋转矩阵,公式中RZ(θ)为绕Z轴旋转θ角度的旋转矩阵。
进一步地,步骤S2当中:
所述拓展卡尔曼滤波是标准卡尔曼滤波EKF在非线性情形下的一种扩展形式,即,将非线性函数通过一阶泰勒展开近似线性函数,然后采用卡尔曼滤波框架得出准确的下一时刻状态真值;
记无人车在各时刻的状态为x1,x2....,xk,表示为xk=[x,y,z,pitch,roll,yaw]T,其中k是离散时间下标,x,y,z表示在空间中的位置,roll,pitch,yaw分别表示绕x,y,z轴的旋转;采用运动方程和观测方程来描述状态估计问题:
xt=g(xt-1,ut)+εt
zt=h(xt)+δt
公式中:g为运动方程;u为输入;εt输入噪声;h为观测方程;x为观测数据,δt为观测噪声;
标准卡尔曼滤波EKF在工作点附近,对非线性系统进行线性近似化:
Figure BDA0003860717460000043
Figure BDA0003860717460000044
g(xt-1,ut)在上一状态估计的最优值μt-1处取一阶导数得到Gt,h(xt)在当前时刻预测值
Figure BDA0003860717460000045
处取一阶导数得到Rt
预测过程:
Figure BDA0003860717460000046
Figure BDA0003860717460000047
式中
Figure BDA0003860717460000048
为期望,
Figure BDA0003860717460000049
为协方差;
更新过程:
Figure BDA0003860717460000051
Figure BDA0003860717460000052
Figure BDA0003860717460000053
式中K为卡尔曼增益,Ht为转换矩阵,h为观测方程,;u为输入;
多传感器系统模型如下:
Figure BDA0003860717460000054
y1(k)=C1x(k)+δ1
Figure BDA0003860717460000055
yn(k)=Cnx(k)+δn
多传感器系统的状态方程
Figure BDA00038607174600000510
只有一个,观测方程y有多个,ε为运动误差,δ为单个传感器观测误差,第一个传感器的观测方程更新后得到系统的状态量x(k)及系统协方差矩阵∑(k);将二者作为下一个传感器更新过程的系统预测状态量
Figure BDA0003860717460000056
和系统预测协方差矩阵
Figure BDA0003860717460000057
进行状态更新;将最后一个传感器更新后得到的系统的状态量x(k)及系统协方差矩阵∑(k)作为融合后输出,并将二者用于预测过程进行下一时刻的迭代,最终得到融合之后的位姿。
进一步地,在步骤S3中:
考虑激光雷达旋转一周所需时间为0.05-0.1S,即激光雷达旋转一周获取一帧数据的时间,故激光雷达产生的第一个激光点与最后一个点之间存在时间间隔,当智能车运动时,激光雷达对于同一个物体的检测距离会发生变化,即激光点所对应的激光雷达坐标系发生了变化;通过多传感器位姿对齐后经过拓展卡尔曼滤波得到各时刻的位姿估计,分别做线性插值将每个激光雷达点对应时刻做坐标变换,以实现去激光雷达运动畸变。
进一步地,在步骤S4中:
SLAM确定位姿的过程是一个非线性最小二乘问题,即将求解位姿最大估计概率问题转化为观测与估计的误差平方和最小;智能驾驶汽车的运动方程和观测方程为:
Figure BDA0003860717460000058
式中wk为运动噪声,vk,j为观测噪声,xk为k时刻对应的汽车位置,yj表示观测到的路标位置,uk为传感器读数或者输入,zk,j为观测数据;
对于某次观测:zk,j=h(yj,xk)+vk,j,噪声满足高斯分布:vk~N(0,Qk,j),一般的高斯分布为:
Figure BDA0003860717460000059
其中μ为期望,即最大概率的取值,∑-1表示协方差矩阵的逆矩阵;对其取负对数:
Figure BDA0003860717460000061
Figure BDA0003860717460000062
故原问题可能取值的最大概率的值问题就等价于该负数问题的最小值问题,即求原问题的最大似然:P(zj,k|xk,yj)=N(h(yj,xk),Qk,j),相当于最小化:
Figure BDA0003860717460000063
对于最小二乘问题采用裂纹伯格-马夸尔特(L-M)迭代法求解,采用步骤S2求解得到的融合位姿作为求解汽车位姿的激光雷达迭代初始位姿,以减小当前激光雷达数据在子图内扫描匹配的区域和迭代收敛的次数;最后在匹配得分最高的位姿中插入当前激光雷达数据Scan,并做实时车辆位姿轨迹和子图输出。
进一步地,在步骤S5中:
所述回环检测是当前激光雷达数据与已经构建完成的子图进行扫描匹配,将步骤S2得到的估计位姿信息提供给SLAM后端的扫描匹配,以减少需要匹配的子图数量和降低后端优化的时间;
当匹配得分大于设定的阈值时,认为当前回环成功,并将当前位姿约束加入到非线性优化中,通过构建最小二乘问题迭代求解,给匹配进行打分,并且前端匹配是在子图内优化单次误差,后端匹配是在子图之间优化减少累计误差,只要回环一次就做一次全局优化消除累计误差。
进一步地,在步骤S6中,当不再有新的激光雷达扫描数据Scan产生时,将优化后的全局地图输出。
相比于现有技术,本发明及其优选方案所完成的工作主要包括:第一,针对单一激光雷达SLAM过程中扫描匹配耗时并且精度低的缺陷,利用拓展卡尔曼滤波融合了IMU、论速计的位姿信息,为激光雷达扫描匹配提供初始位姿,减少了迭代的次数,加速了计算时间;并且提供了更加精确的初始位姿,避免了迭代发散的问题,使得迭代之后的位姿更加精确。第二,在仿真试验中设计了走廊这种特征高度相似的场景,在这种环境下做回环检测极易出现匹配出错导致优化失效,将融合之后的信息作为位姿约束,加入到SLAM后端非线性优化中,从而减小了累计误差,最后通过实验验证了本发明融合SLAM算法的鲁棒性和有效性。
附图说明
图1为本发明实施例两轮差速底盘的运动学示意图;
图2为本发明实施例坐标变换图;
图3为本发明实施例激光雷达去运动畸变效果图;
图4为本发明实施例Gazebo实验环境示意图;
图5为本发明实施例单一激光雷达SLAM结果图1;
图6为本发明实施例单一激光雷达SLAM结果图2;
图7为本发明实施例方法SLAM结果图1;
图8为本发明实施例方法最终保存的地图;
图9为本发明实施例环境地图和本发明方法SLAM结果图2;
图10为本发明实施例方法总体流程示意图。
具体实施方式
为让本专利的特征和优点能更明显易懂,下文特举实施例,作详细说明如下:
应该指出,以下详细说明都是例示性的,旨在对本申请提供进一步的说明。除非另有指明,本说明书使用的所有技术和科学术语具有与本申请所属技术领域的普通技术人员通常理解的相同含义。
需要注意的是,这里所使用的术语仅是为了描述具体实施方式,而非意图限制根据本申请的示例性实施方式。如在这里所使用的,除非上下文另外明确指出,否则单数形式也意图包括复数形式,此外,还应当理解的是,当在本说明书中使用术语“包含”和/或“包括”时,其指明存在特征、步骤、操作、器件、组件和/或它们的组合。
如图10所示,本实施例提供的方案的步骤和流程如下:
步骤S1:读取IMU、轮速计的数据信息并进行预处理;
包括:对惯性测量单元(IMU)预积分求得惯性里程计;
IMU每个时刻可实现自身3轴加速度和角速度的测量,角速度
Figure BDA0003860717460000071
与加速度
Figure BDA0003860717460000072
的测量模型分别为:
Figure BDA0003860717460000073
Figure BDA0003860717460000074
其中
Figure BDA0003860717460000081
为实际角速度,bg是角速度随时间缓慢变化的偏差bias,ηg是角速度白噪声,
Figure BDA0003860717460000082
为旋转矩阵,aw为实际加速度,gw表示重力矢量,w表示世界坐标系作为惯性系,ba是加速度随时间变化的偏差,ηa是加速度白噪声;
运动方程为:
Figure BDA0003860717460000083
Figure BDA0003860717460000084
Figure BDA0003860717460000085
公式中∧表示反对称矩阵;
在区间[t,t+Δt]里进行欧拉积分有:
Figure BDA0003860717460000086
vw(t+Δt)=vw(t)+aw(t)·Δt
Figure BDA0003860717460000087
Figure BDA0003860717460000088
为所求旋转矩阵,pw(t+Δt)为所求位移,这样便求得惯性里程计;
计算轮速计数据求得轮边里程计信息;
通过轮速计得到的两轮角速度和两轮线速度求解横向位移、纵向位移和航向角,如图1所示;
根据运动模型计算出小车的速度v和角速度ω:
Figure BDA0003860717460000089
Figure BDA00038607174600000810
公式vL、vR分别表示左右轮的线速度,d为轮胎离底盘中心的距离;
对时间积分得到里程信息:
θ(t)=∫ω(t)dt
x(t)=∫v(t)cos(θ(t))dt
y(t)=∫v(t)sin(θ(t))dt
式中θ(t)为航行角,x(t)为横轴位移,y(t)为纵轴位移;
进行位姿数据融合前时间上和空间上的统一;
由于每个传感器发布数据的频率不同,需要将各传感器数据进行时间上和空间的统一,而激光雷达作为建图的核心传感器,故以激光雷达发布的时间为基准进行时间上的统一。
时间上的统一方案:当每一帧激光传感器数据到来时,其他传感器的时间取比这个时间小且离的最近的时间作为初始时间,当下一帧数据到来时,取比这个时间大且最近的时间作为终止时间,计算这两个时刻的时间差作为积分时间。
将得到的数据进行坐标系的变换以得到空间上的统一。根据各传感器的安装位姿,利用Eigen库进行坐标系的转换,得到变换的平移矩阵和旋转矩阵,再将其他传感器坐标系统一到小车的基座标系,如图2所示,W为基座标系,C为需要对齐的坐标系,X、Y、Z分别为空间坐标系下的坐标。
变换公式为:
Figure BDA0003860717460000091
为了方便计算改写成增广矩阵:
Figure BDA0003860717460000093
公式中Rc为3*3的旋转矩阵,Tc为3*1的平移矩阵。
Figure BDA0003860717460000092
公式中RX(α)为绕X轴旋转α角度的旋转矩阵,公式中RY(β)为绕Y轴旋转β角度的旋转矩阵,公式中RZ(θ)为绕Z轴旋转θ角度的旋转矩阵。
步骤S2:拓展卡尔曼滤波融合IMU、轮速计预处理的位姿得到融合位姿;
融合数据问题是一个非线性最小二乘问题(又称最小平方法),它通过最小化误差的平方和寻找数据的最佳函数匹配。利用最小二乘法可以简便地求得未知的数据,并使得这些求得的数据与实际数据之间误差的平方和为最小。而拓展卡尔曼滤波是标准卡尔曼滤波(EKF)在非线性情形下的一种扩展形式,实质就是将非线性函数通过一阶泰勒展开近似线性函数,然后采用卡尔曼滤波框架得出准确的下一时刻状态真值,其核心思想为估计和观测反馈。
记无人车在各时刻的状态为x1,x2....,xk,表示为xk=[x,y,z,pitch,roll,yaw]T,其中k是离散时间下标,x,y,z表示在空间中的位置,roll,pitch,yaw分别表示绕x,y,z轴的旋转。在SLAM中,通常要估计机器人的位置,那么系统的状态就指的是机器人的位姿。用运动方程和观测方程来描述状态估计问题:
xt=g(xt-1,ut)+εt
zt=h(xt)+δt
公式中:g-运动方程;u-输入;εt输入噪声;h-观测方程;x-观测数据,δt-观测噪声;
运动方程描述了状态xk-1是怎么推导到xk的,而观测方程描述的是从xk是怎么得到观察数据的yk,当有多个传感器数据时,观测方程就有多个,然而这种性质只适用线性高斯系统。EKF的做法是在工作点附近,对非线性系统进行线性近似化:
Figure BDA0003860717460000101
Figure BDA0003860717460000102
g(xt-1,ut)在上一状态估计的最优值μt-1处取一阶导数得到Gt,h(xt)在当前时刻预测值
Figure BDA00038607174600001015
处取一阶导数得到Rt
预测过程:
Figure BDA0003860717460000103
Figure BDA0003860717460000104
式中
Figure BDA0003860717460000105
为期望,
Figure BDA0003860717460000106
为协方差;
更新过程:
Figure BDA0003860717460000107
Figure BDA0003860717460000108
Figure BDA0003860717460000109
式中K-卡尔曼增益,Ht-转换矩阵,h-观测方程,;u-输入;
多传感器系统模型如下:
Figure BDA00038607174600001010
y1(k)=C1x(k)+δ1
Figure BDA00038607174600001011
yn(k)=Cnx(k)+δn
多传感器系统的状态方程
Figure BDA00038607174600001012
只有一个,观测方程y有多个,ε为运动误差,δ为单个传感器观测误差,第一个传感器的观测方程更新后得到系统的状态量x(k)及系统协方差矩阵∑(k)。将二者作为下一个传感器更新过程的系统预测状态量
Figure BDA00038607174600001013
和系统预测协方差矩阵
Figure BDA00038607174600001014
进行状态更新。将最后一个传感器更新后得到的系统的状态量x(k)及系统协方差矩阵∑(k)作为融合后输出,并将二者用于预测过程进行下一时刻的迭代,最终得到融合之后的位姿。
步骤S3:激光雷达数据预处理,将融合后的位姿为激光雷达数据做线性插值去运动畸变;
由于激光雷达旋转一周所需时间为0.05-0.1S(10-20HZ),即激光雷达旋转一周获取一帧数据的时间,故激光雷达产生的第一个激光点与最后一个点之间存在时间间隔,当智能车运动时,激光雷达对于同一个物体的检测距离会发生变化,即激光点所对应的激光雷达坐标系发生了变化。如果可以将所有的激光点与初始点坐标变化关系求出来,那么就可以解决激光雷达运动畸变问题。多传感器位姿对齐后经过拓展卡尔曼滤波得到各时刻的位姿估计,分别做线性插值将每个激光雷达点对应时刻做坐标变换,即可去激光雷达运动畸变,去运动畸变效果如图3所示。
步骤S4:将融合处理之后的位姿作为激光雷达扫描匹配的迭代初始位姿,开始SLAM前端建立局部子图;
SLAM确定位姿的过程是一个非线性最小二乘问题,即将求解位姿最大估计概率问题转化为观测与估计的误差平方和最小;智能驾驶汽车的运动方程和观测方程为:
Figure BDA0003860717460000111
式中wk为运动噪声,vk,j为观测噪声,xk为k时刻对应的汽车位置,yj表示观测到的路标位置,uk为传感器读数或者输入,zk,j为观测数据;
对于某次观测:zk,j=h(yj,xk)+vk,j,噪声满足高斯分布:vk~N(0,Qk,j),一般的高斯分布为:
Figure BDA0003860717460000112
其中μ为期望,即最大概率的取值,∑-1表示协方差矩阵的逆矩阵;对其取负对数:
Figure BDA0003860717460000113
Figure BDA0003860717460000114
故原问题可能取值的最大概率的值问题就等价于该负数问题的最小值问题,即求原问题的最大似然:P(zj,k|xk,yj)=N(h(yj,xk),Qk,j),相当于最小化:
Figure BDA0003860717460000115
对于最小二乘问题采用裂纹伯格-马夸尔特(L-M)迭代法求解,本算法采用步骤S2求解得到的融合位姿作为求解汽车位姿的激光雷达迭代初始位姿,从而减小了当前激光雷达数据在子图内扫描匹配的区域和迭代收敛的次数,加速了迭代效率,为获得更精确的位姿提供了指引。最后在匹配得分最高的位姿中插入当前激光雷达数据Scan,并做实时车辆位姿轨迹和子图输出。
步骤S5:对于构建完成的子图判断是否形成回环,进行SLAM后端回环检测以减少累计误差,并将这时刻对应的位姿约束加入到非线性优化中;
回环检测是当前激光雷达数据与已经构建完成的子图进行扫描匹配,同时本算法将S2得到的估计位姿信息提供给SLAM后端的扫描匹配,从而减少了需要匹配的子图数量,大大降低了后端优化的时间。当匹配得分大于设定的阈值时,认为当前回环成功,并将当前位姿约束加入到非线性优化中,此时的优化与S4相似,依然是构建最小二乘问题迭代求解,不同的是需要给匹配进行打分,并且前端匹配是在子图内优化单次误差,后端匹配是在子图之间优化减少累计误差,只要回环一次就做一次全局优化消除一些累计误差。
步骤S6:当没有新的激光雷达Scan产生,输出全局优化后的轨迹位姿,并将产生的全局地图可视化;
当不再有新的激光雷达扫描数据Scan产生时,算法开始将优化后的全局地图输出。
本实施例在Ubuntu18.04+ROS Melodic版本的Gazebo环境中进行实验验证,图4为特征高度相似走廊实验环境,该环境为50x10(米)的回环走廊,图5、6为单一激光雷达的SLAM结果,从图片中可以看出在环境特征高度形似场景中单一的激光雷达匹配出现问题,最后导致地图的畸变甚至重叠;图7、8、9为本发明方法的实验结果,由图可以看出,本发明的方法融合了多传感器位姿信息后减少了匹配出错的问题,并大幅度减少了位姿误差,在特征高度相似场景下鲁棒性强,至此改善了单一激光雷达对于长走廊等高度相似场景的SLAM失效问题,通过多次实验验证了本方法的有效性。
以上所述,仅是本发明的较佳实施例而已,并非是对本发明作其它形式的限制,任何熟悉本专业的技术人员可能利用上述揭示的技术内容加以变更或改型为等同变化的等效实施例。但是凡是未脱离本发明技术方案内容,依据本发明的技术实质对以上实施例所作的任何简单修改、等同变化与改型,仍属于本发明技术方案的保护范围。
本专利不局限于上述最佳实施方式,任何人在本专利的启示下都可以得出其它各种形式的基于拓展卡尔曼滤波的多传感器融合SLAM方法,凡依本发明申请专利范围所做的均等变化与修饰,皆应属本专利的涵盖范围。

Claims (7)

1.一种基于拓展卡尔曼滤波的多传感器融合SLAM方法,其特征在于,包括以下步骤:
步骤S1:读取IMU、轮速计的数据信息并进行预处理;
步骤S2:采用拓展卡尔曼滤波融合IMU、轮速计预处理的位姿得到融合位姿;
步骤S3:激光雷达数据预处理,将融合后的位姿为激光雷达数据做线性插值去运动畸变;
步骤S4:将融合处理之后的位姿作为激光雷达扫描匹配的迭代初始位姿,开始SLAM前端建立局部子图;
步骤S5:对于构建完成的子图判断是否形成回环,进行SLAM后端回环检测以减少累计误差,并将当前时刻对应的位姿约束加入到非线性优化中;
步骤S6:当没有新的激光雷达Scan产生,输出全局优化后的轨迹位姿,并将产生的全局地图可视化。
2.根据权利要求1所述的基于拓展卡尔曼滤波的多传感器融合SLAM方法,其特征在于:步骤S1具体包括:
对惯性测量单元IMU预积分求得惯性里程计;
IMU每个时刻进行自身3轴加速度和角速度的测量,角速度
Figure FDA0003860717450000011
与加速度
Figure FDA0003860717450000012
的测量模型分别为:
Figure FDA0003860717450000013
Figure FDA0003860717450000014
其中
Figure FDA0003860717450000015
为实际角速度,bg是角速度随时间缓慢变化的偏差bias,ηg是角速度白噪声,
Figure FDA0003860717450000016
为旋转矩阵,aw为实际加速度,gw表示重力矢量,w表示世界坐标系作为惯性系,ba是加速度随时间变化的偏差,ηa是加速度白噪声;
运动方程为:
Figure FDA0003860717450000017
Figure FDA0003860717450000018
Figure FDA0003860717450000019
公式中∧表示反对称矩阵;
在区间[t,t+Δt]里进行欧拉积分有:
Figure FDA0003860717450000021
vw(t+Δt)=vw(t)+aw(t)·Δt
Figure FDA0003860717450000022
Figure FDA0003860717450000023
为所求旋转矩阵,pw(t+Δt)为所求位移,从而求得惯性里程计;
计算轮速计数据求得轮边里程计信息:
通过轮速计得到的两轮角速度和两轮线速度求解横向位移、纵向位移和航向角;
根据运动模型计算出小车的速度v和角速度ω:
Figure FDA0003860717450000024
Figure FDA0003860717450000025
公式vL、vR分别表示左右轮的线速度,d为轮胎离底盘中心的距离;
对时间积分得到里程信息:
θ(t)=∫ω(t)dt
x(t)=∫v(t)cos(θ(t))dt
y(t)=∫v(t)sin(θ(t))dt
式中θ(t)为航行角,x(t)为横轴位移,y(t)为纵轴位移;
进行位姿数据融合前时间上和空间上的统一;
以激光雷达发布的时间为基准进行时间上的统一:当每一帧激光传感器数据到来时,各传感器的时间取比基准时间小且离的最近的时间作为初始时间,当下一帧数据到来时,取比基准时间大且最近的时间作为终止时间,计算这两个时刻的时间差作为积分时间;
将得到的数据进行坐标系的变换以得到空间上的统一:根据各传感器的安装位姿,利用Eigen库进行坐标系的转换,得到变换的平移矩阵和旋转矩阵,再将其他传感器坐标系统一到小车的基座标系,W为基座标系,C为需要对齐的坐标系,X、Y、Z分别为空间坐标系下的坐标;
变换公式为:
Figure FDA0003860717450000026
为了方便计算改写成增广矩阵:
Figure FDA0003860717450000031
公式中Rc为3*3的旋转矩阵,Tc为3*1的平移矩阵:
Figure FDA0003860717450000032
公式中RX(α)为绕X轴旋转α角度的旋转矩阵,公式中RY(β)为绕Y轴旋转β角度的旋转矩阵,公式中RZ(θ)为绕Z轴旋转θ角度的旋转矩阵。
3.根据权利要求2所述的基于拓展卡尔曼滤波的多传感器融合SLAM方法,其特征在于:步骤S2当中:
所述拓展卡尔曼滤波是标准卡尔曼滤波EKF在非线性情形下的一种扩展形式,即,将非线性函数通过一阶泰勒展开近似线性函数,然后采用卡尔曼滤波框架得出准确的下一时刻状态真值;
记无人车在各时刻的状态为x1,x2....,xk,表示为xk=[x,y,z,pitch,roll,yaw]T,其中k是离散时间下标,x,y,z表示在空间中的位置,roll,pitch,yaw分别表示绕x,y,z轴的旋转;采用运动方程和观测方程来描述状态估计问题:
xt=g(xt-1,ut)+εt
zt=h(xt)+δt
公式中:g为运动方程;u为输入;εt输入噪声;h为观测方程;x为观测数据,δt为观测噪声;
标准卡尔曼滤波EKF在工作点附近,对非线性系统进行线性近似化:
Figure FDA0003860717450000033
Figure FDA0003860717450000034
g(xt-1,ut)在上一状态估计的最优值μt-1处取一阶导数得到Gt,h(xt)在当前时刻预测值
Figure FDA0003860717450000035
处取一阶导数得到Rt
预测过程:
Figure FDA0003860717450000036
Figure FDA0003860717450000037
式中
Figure FDA0003860717450000038
为期望,
Figure FDA0003860717450000039
为协方差;
更新过程:
Figure FDA0003860717450000041
Figure FDA0003860717450000042
Figure FDA0003860717450000043
式中K为卡尔曼增益,Ht为转换矩阵,h为观测方程,;u为输入;
多传感器系统模型如下:
Figure FDA0003860717450000044
y1(k)=C1x(k)+δ1
Figure FDA0003860717450000045
yn(k)=Cnx(k)+δn
多传感器系统的状态方程
Figure FDA0003860717450000046
只有一个,观测方程y有多个,ε为运动误差,δ为单个传感器观测误差,第一个传感器的观测方程更新后得到系统的状态量x(k)及系统协方差矩阵Σ(k);将二者作为下一个传感器更新过程的系统预测状态量
Figure FDA0003860717450000047
和系统预测协方差矩阵
Figure FDA0003860717450000048
进行状态更新;将最后一个传感器更新后得到的系统的状态量x(k)及系统协方差矩阵Σ(k)作为融合后输出,并将二者用于预测过程进行下一时刻的迭代,最终得到融合之后的位姿。
4.根据权利要求3所述的基于拓展卡尔曼滤波的多传感器融合SLAM方法,其特征在于:在步骤S3中:
考虑激光雷达旋转一周所需时间为0.05-0.1S,即激光雷达旋转一周获取一帧数据的时间,故激光雷达产生的第一个激光点与最后一个点之间存在时间间隔,当智能车运动时,激光雷达对于同一个物体的检测距离会发生变化,即激光点所对应的激光雷达坐标系发生了变化;通过多传感器位姿对齐后经过拓展卡尔曼滤波得到各时刻的位姿估计,分别做线性插值将每个激光雷达点对应时刻做坐标变换,以实现去激光雷达运动畸变。
5.根据权利要求4所述的基于拓展卡尔曼滤波的多传感器融合SLAM方法,其特征在于:在步骤S4中:
SLAM确定位姿的过程是一个非线性最小二乘问题,即将求解位姿最大估计概率问题转化为观测与估计的误差平方和最小;智能驾驶汽车的运动方程和观测方程为:
Figure FDA0003860717450000049
式中wk为运动噪声,vk,j为观测噪声,xk为k时刻对应的汽车位置,yj表示观测到的路标位置,uk为传感器读数或者输入,zk,j为观测数据;
对于某次观测:zk,j=h(yj,xk)+vk,j,噪声满足高斯分布:vk~N(0,Qk,j),一般的高斯分布为:
Figure FDA0003860717450000051
其中μ为期望,即最大概率的取值,∑-1表示协方差矩阵的逆矩阵;对其取负对数:
Figure FDA0003860717450000052
Figure FDA0003860717450000053
故原问题可能取值的最大概率的值问题就等价于该负数问题的最小值问题,即求原问题的最大似然:P(zj,k|xk,yj)=N(h(yj,xk),Qk,j),相当于最小化:
Figure FDA0003860717450000054
对于最小二乘问题采用裂纹伯格-马夸尔特(L-M)迭代法求解,采用步骤S2求解得到的融合位姿作为求解汽车位姿的激光雷达迭代初始位姿,以减小当前激光雷达数据在子图内扫描匹配的区域和迭代收敛的次数;最后在匹配得分最高的位姿中插入当前激光雷达数据Scan,并做实时车辆位姿轨迹和子图输出。
6.根据权利要求5所述的基于拓展卡尔曼滤波的多传感器融合SLAM方法,其特征在于:在步骤S5中:
所述回环检测是当前激光雷达数据与已经构建完成的子图进行扫描匹配,将步骤S2得到的估计位姿信息提供给SLAM后端的扫描匹配,以减少需要匹配的子图数量和降低后端优化的时间;
当匹配得分大于设定的阈值时,认为当前回环成功,并将当前位姿约束加入到非线性优化中,通过构建最小二乘问题迭代求解,给匹配进行打分,并且前端匹配是在子图内优化单次误差,后端匹配是在子图之间优化减少累计误差,只要回环一次就做一次全局优化消除累计误差。
7.根据权利要求6所述的基于拓展卡尔曼滤波的多传感器融合SLAM方法,其特征在于:在步骤S6中,当不再有新的激光雷达扫描数据Scan产生时,将优化后的全局地图输出。
CN202211163166.6A 2022-09-23 2022-09-23 基于拓展卡尔曼滤波的多传感器融合slam方法 Pending CN115435775A (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202211163166.6A CN115435775A (zh) 2022-09-23 2022-09-23 基于拓展卡尔曼滤波的多传感器融合slam方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202211163166.6A CN115435775A (zh) 2022-09-23 2022-09-23 基于拓展卡尔曼滤波的多传感器融合slam方法

Publications (1)

Publication Number Publication Date
CN115435775A true CN115435775A (zh) 2022-12-06

Family

ID=84248702

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202211163166.6A Pending CN115435775A (zh) 2022-09-23 2022-09-23 基于拓展卡尔曼滤波的多传感器融合slam方法

Country Status (1)

Country Link
CN (1) CN115435775A (zh)

Cited By (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN116541663A (zh) * 2023-06-21 2023-08-04 四川信息职业技术学院 一种基于卡尔曼滤波提高双套站传感器数据质量的方法
CN116878488A (zh) * 2023-09-07 2023-10-13 湘江实验室 一种建图方法、装置、存储介质及电子装置
CN117848332A (zh) * 2024-03-07 2024-04-09 北京理工大学前沿技术研究院 一种车载多源融合高精度定位系统的imu噪声消除方法

Citations (10)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US20040073360A1 (en) * 2002-08-09 2004-04-15 Eric Foxlin Tracking, auto-calibration, and map-building system
US20120086598A1 (en) * 2010-10-08 2012-04-12 Canadian Space Agency Apparatus and methods for driftless attitude determination and reliable localization of vehicles
WO2017177533A1 (zh) * 2016-04-12 2017-10-19 深圳市龙云创新航空科技有限公司 基于激光雷达的微型无人机操控方法及系统
CN109186601A (zh) * 2018-07-05 2019-01-11 南京理工大学 一种基于自适应无迹卡尔曼滤波的激光slam算法
US20190323845A1 (en) * 2016-11-09 2019-10-24 The Texas A&M University System Method and System for Accurate Long Term Simultaneous Localization and Mapping with Absolute Orientation Sensing
CN112734841A (zh) * 2020-12-31 2021-04-30 华南理工大学 一种用轮式里程计-imu和单目相机实现定位的方法
US11002859B1 (en) * 2020-02-27 2021-05-11 Tsinghua University Intelligent vehicle positioning method based on feature point calibration
CN113111443A (zh) * 2021-05-06 2021-07-13 江西昊絮智辕科技有限公司 智能运输小车及运动模型构建方法
CN114993298A (zh) * 2022-04-29 2022-09-02 南京航空航天大学 基于ekf的模板匹配vo与轮式里程计融合定位方法
CN115046540A (zh) * 2022-05-25 2022-09-13 新驱动重庆智能汽车有限公司 一种点云地图构建方法、系统、设备和存储介质

Patent Citations (10)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US20040073360A1 (en) * 2002-08-09 2004-04-15 Eric Foxlin Tracking, auto-calibration, and map-building system
US20120086598A1 (en) * 2010-10-08 2012-04-12 Canadian Space Agency Apparatus and methods for driftless attitude determination and reliable localization of vehicles
WO2017177533A1 (zh) * 2016-04-12 2017-10-19 深圳市龙云创新航空科技有限公司 基于激光雷达的微型无人机操控方法及系统
US20190323845A1 (en) * 2016-11-09 2019-10-24 The Texas A&M University System Method and System for Accurate Long Term Simultaneous Localization and Mapping with Absolute Orientation Sensing
CN109186601A (zh) * 2018-07-05 2019-01-11 南京理工大学 一种基于自适应无迹卡尔曼滤波的激光slam算法
US11002859B1 (en) * 2020-02-27 2021-05-11 Tsinghua University Intelligent vehicle positioning method based on feature point calibration
CN112734841A (zh) * 2020-12-31 2021-04-30 华南理工大学 一种用轮式里程计-imu和单目相机实现定位的方法
CN113111443A (zh) * 2021-05-06 2021-07-13 江西昊絮智辕科技有限公司 智能运输小车及运动模型构建方法
CN114993298A (zh) * 2022-04-29 2022-09-02 南京航空航天大学 基于ekf的模板匹配vo与轮式里程计融合定位方法
CN115046540A (zh) * 2022-05-25 2022-09-13 新驱动重庆智能汽车有限公司 一种点云地图构建方法、系统、设备和存储介质

Non-Patent Citations (6)

* Cited by examiner, † Cited by third party
Title
刘仕琦等: "基于多传感器融合的视觉SLAM算法研究", 《中国优秀硕士学位论文全文数据库 信息科技辑》, no. 2, 15 February 2021 (2021-02-15), pages 138 - 2309 *
吴佳鑫等: "带有回环约束的无人车多传感器融合SLAM系统", 《中国优秀硕士学位论文全文数据库 信息科技辑》, no. 01, 15 February 2021 (2021-02-15), pages 140 - 653 *
夏卫星等: "基于自适应四参数估计的电罗经航向运动建模", 《舰船科学技术 》, vol. 38, no. 19, 31 October 2016 (2016-10-31), pages 125 - 128 *
胡尊刚: "基于单目视觉的移动机器人SLAM技术与目标检测技术研究", 《中国优秀硕士学位论文全文数据库 信息科技辑》, no. 02, 15 February 2021 (2021-02-15), pages 138 - 936 *
陈光祯等: "《智能无线机器人:人工智能算法与应用》", 31 July 2022, 机械工业出版社, pages: 152 - 156 *
陈靖等: "《增强现实技术及其应用案例》", 31 January 2022, 北京理工大学出版社 , pages: 122 - 126 *

Cited By (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN116541663A (zh) * 2023-06-21 2023-08-04 四川信息职业技术学院 一种基于卡尔曼滤波提高双套站传感器数据质量的方法
CN116541663B (zh) * 2023-06-21 2023-09-19 四川信息职业技术学院 一种基于卡尔曼滤波提高双套站传感器数据质量的方法
CN116878488A (zh) * 2023-09-07 2023-10-13 湘江实验室 一种建图方法、装置、存储介质及电子装置
CN116878488B (zh) * 2023-09-07 2023-11-28 湘江实验室 一种建图方法、装置、存储介质及电子装置
CN117848332A (zh) * 2024-03-07 2024-04-09 北京理工大学前沿技术研究院 一种车载多源融合高精度定位系统的imu噪声消除方法
CN117848332B (zh) * 2024-03-07 2024-05-03 北京理工大学前沿技术研究院 一种车载多源融合高精度定位系统的imu噪声消除方法

Similar Documents

Publication Publication Date Title
CN112347840B (zh) 视觉传感器激光雷达融合无人机定位与建图装置和方法
Xu et al. Fast-lio: A fast, robust lidar-inertial odometry package by tightly-coupled iterated kalman filter
CN107063280B (zh) 一种基于控制采样的智能车辆路径规划系统及方法
CN109211251B (zh) 一种基于激光和二维码融合的即时定位与地图构建方法
CN115435775A (zh) 基于拓展卡尔曼滤波的多传感器融合slam方法
CN110160542B (zh) 车道线的定位方法和装置、存储介质、电子装置
CN112083726B (zh) 一种面向园区自动驾驶的双滤波器融合定位系统
Li et al. Collaborative mapping and autonomous parking for multi-story parking garage
Hasberg et al. Simultaneous localization and mapping for path-constrained motion
US11158065B2 (en) Localization of a mobile unit by means of a multi hypothesis kalman filter method
CN114018248B (zh) 一种融合码盘和激光雷达的里程计方法与建图方法
CN112965063B (zh) 一种机器人建图定位方法
CN111707272A (zh) 一种地下车库自动驾驶激光定位系统
Kang et al. Vins-vehicle: A tightly-coupled vehicle dynamics extension to visual-inertial state estimator
CN114001733B (zh) 一种基于地图的一致性高效视觉惯性定位算法
CN113920198B (zh) 一种基于语义边缘对齐的由粗到精的多传感器融合定位方法
CN111060099A (zh) 一种无人驾驶汽车实时定位方法
CN114323033A (zh) 基于车道线和特征点的定位方法、设备及自动驾驶车辆
CN111812669B (zh) 绕机检查装置及其定位方法、存储介质
CN116337045A (zh) 一种基于karto和teb的高速建图导航方法
CN111257853B (zh) 一种基于imu预积分的自动驾驶系统激光雷达在线标定方法
Peng et al. Vehicle odometry with camera-lidar-IMU information fusion and factor-graph optimization
CN115577320A (zh) 一种基于数据插值的多传感器异步数据融合方法
Wang et al. Research on AGV task path planning based on improved A* algorithm
CN116774247A (zh) 基于ekf的多源信息融合的slam前端策略

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