CN114440881B - 一种融合多源传感器信息的无人车定位方法 - Google Patents
一种融合多源传感器信息的无人车定位方法 Download PDFInfo
- Publication number
- CN114440881B CN114440881B CN202210109679.2A CN202210109679A CN114440881B CN 114440881 B CN114440881 B CN 114440881B CN 202210109679 A CN202210109679 A CN 202210109679A CN 114440881 B CN114440881 B CN 114440881B
- Authority
- CN
- China
- Prior art keywords
- vehicle
- model
- positioning
- matrix
- algorithm
- 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.)
- Active
Links
- 238000000034 method Methods 0.000 title claims abstract description 55
- 238000004422 calculation algorithm Methods 0.000 claims abstract description 74
- 230000004927 fusion Effects 0.000 claims abstract description 24
- 238000001914 filtration Methods 0.000 claims description 56
- 239000011159 matrix material Substances 0.000 claims description 53
- 230000001133 acceleration Effects 0.000 claims description 28
- 239000013598 vector Substances 0.000 claims description 24
- 230000014509 gene expression Effects 0.000 claims description 22
- 239000002245 particle Substances 0.000 claims description 21
- 230000008569 process Effects 0.000 claims description 18
- 230000000694 effects Effects 0.000 claims description 11
- 239000000725 suspension Substances 0.000 claims description 8
- 230000007704 transition Effects 0.000 claims description 6
- 238000006073 displacement reaction Methods 0.000 claims description 5
- 238000004364 calculation method Methods 0.000 claims description 4
- 238000004458 analytical method Methods 0.000 claims description 3
- 238000010606 normalization Methods 0.000 claims description 3
- 238000012545 processing Methods 0.000 claims description 3
- 238000010586 diagram Methods 0.000 description 5
- 238000005070 sampling Methods 0.000 description 4
- 238000012952 Resampling Methods 0.000 description 2
- 238000005259 measurement Methods 0.000 description 2
- 238000000342 Monte Carlo simulation Methods 0.000 description 1
- 230000003321 amplification Effects 0.000 description 1
- 238000005094 computer simulation Methods 0.000 description 1
- 230000007547 defect Effects 0.000 description 1
- 230000002708 enhancing effect Effects 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 230000006855 networking Effects 0.000 description 1
- 238000003199 nucleic acid amplification method Methods 0.000 description 1
- 230000008447 perception Effects 0.000 description 1
- 238000011160 research Methods 0.000 description 1
- 238000004088 simulation Methods 0.000 description 1
- 238000006467 substitution reaction Methods 0.000 description 1
- 238000012360 testing method Methods 0.000 description 1
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/165—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 combined with non-inertial navigation instruments
-
- 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/20—Instruments for performing navigational calculations
-
- 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)
- Position Fixing By Use Of Radio Waves (AREA)
- Navigation (AREA)
Abstract
本发明涉及一种融合多源传感器信息的无人车定位方法,该方法包括以下步骤:步骤1:获取待控制车辆的GPS位置信息和IMU信息,并将待控制车辆结合惯性导航***与GPS信号获得的厘米级精度定位作为真值;步骤2:建立线性模型;步骤3:获取线性模型下的融合多源传感器信息的无人车定位算法,并得到线性模型下的融合定位结果;步骤4:建立非线性模型;步骤5:获取运动学模型下的融合多源传感器信息的无人车定位算法,并得到非线性模型下的融合定位结果;步骤6:建立单车模型;步骤7:获取单车模型下的融合多源传感器信息的无人车定位算法,并得到单车模型下的融合定位结果,与现有技术相比,本发明具有提高无人车定位精度和适用性强等优点。
Description
技术领域
本发明涉及自动驾驶技术领域,尤其涉及一种融合多源传感器信息的无人车定位方法。
背景技术
近年随着中国经济增长,汽车已经不仅仅只是便捷的交通工具,汽车的智能网联化已经成为人们的另一种需求,因此无人车由于其便携性和可拓展性受到广大科研工作者和产业界的关注。其中无人车定位是研究无人车领域的基础模块,使用不同的感知传感器需要采用不同的研究方法。目前市面上单纯地使用GPS完成定位工作的方案,其特点是全天候,覆盖区域大,然而面对卫星信号受到干扰时,会产生很大误差,另外单纯使用IMU完成定位工作地方案由于此技术为相对定位技术,其定位误差会随着时间地累积越来越大。
发明内容
本发明的目的就是为了克服上述现有技术存在的缺陷而提供一种融合多源传感器信息的无人车定位方法。
本发明的目的可以通过以下技术方案来实现:
一种融合多源传感器信息的无人车定位方法,该方法包括以下步骤:
步骤1:获取待控制车辆的GPS位置信息和IMU信息,并将待控制车辆结合惯性导航***与GPS信号获得的厘米级精度定位作为真值,以验证融合后的定位精度;
步骤2:根据GPS位置信息和IMU信息建立线性模型;
步骤3:获取线性模型下的融合多源传感器信息的无人车定位算法,并得到线性模型下的融合定位结果;
步骤4:根据GPS位置信息和IMU信息建立非线性模型;
步骤5:获取非线性模型下的融合多源传感器信息的无人车定位算法,并得到非线性模型下的融合定位结果;
步骤6:对非线性模型进一步加深非线性并重新建模得到单车模型;
步骤7:获取单车模型下的融合多源传感器信息的无人车定位算法,并得到单车模型下的融合定位结果。
所述的步骤1中,通过安装在车顶的GNSS天线收集GPS信息,通过惯性导航单元收集IMU信息,所述的GPS信息包括车辆的经纬度信息,且车辆的经纬度信可转换为车辆的横坐标和纵坐标,所述的IMU信息包括车辆的三轴角速度和加速度。
所述的步骤2中,线性模型包括恒定速度模型与恒定加速度模型,其中,恒定速度模型的状态向量的表达式为:
其中,x、y、θ和v分别为t时刻车辆位置的横坐标、纵坐标、车辆航向角和速度;
恒定速度模型的状态方程和观测方程的表达式分别为:
恒定加速度模型的状态向量的表达式为:
其中,s和v分别为t时刻车辆的位移和速度;
恒定加速度模型的状态方程和观测方程的表达式分别为:
恒定加速度模型将加速度计测量的数值作为***输入:
u(k)=a(k)
其中,u(k)为***输入,a(k)为加速度计测量的数值。
所述的步骤3中,线性模型下的融合多源传感器信息的无人车定位方法具体为:
步骤301:根据卡尔曼滤波算法得到更为准确的无人车位置信息,卡尔曼滤波算法具体过程分为状态预测、误差协方差预测、卡尔曼增益更新、状态更新与误差协方差更新五个环节:
其中,表示k时刻状态向量的预测值,/>表示k时刻状态向量的最优估计值,/>表示k时刻误差协方差的预测值,Pk表示k时刻误差协方差的最优估计值,Kk表示k时刻的卡尔曼增益,Q为过程噪声协方差矩阵,R为观测噪声协方差矩阵,uk-1表示k-1时刻的***输入,A表示***的状态转移矩阵,H表示***的观测矩阵,I表示单位矩阵,B为控制输入矩阵;
对于恒定速度模型,根据恒定速度模型的状态方程和观测方程得到观测矩阵H与状态转移矩阵A的表达式分别为:
其中,Δt为时间间隔,θ为车辆的航向角;
将协方差矩阵P初始化为单位矩阵,将过程噪声矩阵Q与观测噪声矩阵R初始化:
对于恒定加速度模型,根据恒定加速度模型的状态方程、观测值和***输入得到状态转移矩阵A、观测矩阵H和控制输入矩阵B的表达式分别为:
H=[1 0]
其中,Δt为时间间隔;
将协方差矩阵P初始化为单位矩阵,过程噪声矩阵Q与观测噪声矩阵R初始化分别为:
R=1;
步骤302:将得到的最优估计值与真值进行对比,并计算滤波前后的方差。
所述的步骤4中,非线性模型为基于恒定加速度与角速度的运动学模型,运动学模型的状态向量的表达式为:
其中,xk、yk、ψk、ωk、vk和ak分别为k时刻车辆的横坐标、纵坐标、航向角、角速度、速度以及加速度;
运动学模型的状态方程和观测方程的表达式分别为:
其中,为k-1时刻车辆的状态向量,vk-1为k-1时刻车辆的速度,dt为时间间隔,为观测值,xg,k、yg,k、aa,k和ωg,k分别为k时刻GPS观测得到的车辆横坐标、纵坐标、加速度计获得加速度以及陀螺仪测得的角速度.
所述的步骤5中,获取运动学模型下的融合多源传感器信息的无人车定位方法的过程具体包括以下步骤:
步骤501:分别采用拓展卡尔曼算法、无迹卡尔曼算法与粒子滤波算法进行融合定位;
步骤502:将三种滤波算法进行综合比较,计算RMS误差,以选取最优的定位算法,RMS误差的计算公式为:
errorrms=|X-Xest|
其中,errorrms为经过滤波后的误差,X为真值,包括车辆的横坐标和纵坐标的真实值,Xest分别为经过滤波后车辆横坐标与纵坐标的最佳估计值;
步骤503:选取RMS误差最小的对应的滤波算法,并基于该滤波算法获取非线性模型下的融合定位结果。
所述的步骤501中,拓展卡尔曼滤波算法具体的公式包括:
其中,Fk-1与Hk分别为雅各比矩阵,uk为k时刻的***输入,xk-1为k-1时刻的状态向量;
雅各比矩阵Fk-1和Hk的表达式分别为:
其中,ψk-1为k-1时刻车辆的航向角,vk-1为k-1时刻车辆的速度,dt为时间间隔;
无迹卡尔曼滤波算法的具体滤波过程为:
步骤1a:初始化状态向量x0、状态估计误差协方差P0、过程噪声矩阵Q以及观测噪声矩阵R,对于运动学模型,状态向量的初值为真值,过程噪声矩阵Q、观测噪声矩阵R以及初始误差协方差矩阵P0初始化的表达式为:
步骤2a:根据上一时刻的状态向量xk和误差协方差矩阵Pk选择当前时刻采样点,并分配权重;
步骤3a:选择2n+1个采样点,通过***的状态方程计算当前时刻所有采样点的均值和协方差,完成时间更新;
步骤4a:通过***的观测方程对采样点进行量测更新;
步骤5a:通过预测与量测更新计算出卡尔曼增益,完成滤波更新;
粒子滤波算法采用蒙特卡洛法解决非线性滤波问题,粒子滤波算法的滤波过程为:
步骤4b:采用经过重采样后的粒子的权值和状态计算当前时刻的滤波状态估计与方差估计。
所述的步骤6中,单车模型中的速度与角速度之间满足的关系为:
滑移角β的计算公式为:
其中,lr与lf分别为后悬长度与前悬长度,δf与δr分别为前轮偏角与后轮偏角;
单车模型的状态向量的表达式为:
其中,xk、yk、ψk、r、b、N、δr,k和ωk分别为k时刻车辆的横坐标、纵坐标、航向角、车轮半径、车辆轴距、齿轮比、前轮偏角以及车轮转动角速度,车轮半径、车辆轴距和齿轮比保持不变,且将车辆的前轮偏角和车轮转动角速度为***输入,dt为时间间隔;
单车模型的状态方程为:
单车模型的观测值为:
其中,xg,k和yg,k分别为在k时刻GPS观测到的车辆的横坐标和纵坐标;
单车模型的观测方程为:
所述的步骤7中,获取单车模型下的融合多源传感器信息的无人车定位算法的过程具体包括以下步骤:
步骤701:基于公开数据集采用拓展卡尔曼算法、无迹卡尔曼算法与粒子滤波算法分别得到单车模型下的车辆的最佳估计值;
步骤702:针对车辆的坐标和航向角进行误差分析,计算车辆的横坐标、纵坐标以及航向角对应的RMS误差,RMS误差的计算公式为:
errorrms=|X-Xest|
其中,errorrms为经过滤波后的误差,X为真值,包括车辆的横坐标和纵坐标的真实值,Xest分别为经过滤波后车辆的横坐标、纵坐标以及航向角的最佳估计值;
步骤703:将每个RMS误差进行均值处理,实现归一化;
步骤704:将每一种滤波算法的三个归一化后的RMS误差相加形成分值,通过比较滤波算法的分值以评估三种滤波算法的性能与效果,进而选取分值最小的作为最优的定位算法;
步骤705:基于最优的定位算法得到单车模型下的融合定位结果。
所述的步骤701中,对于单车模型采用滤波算法进行滤波时,初始状态向量取真值,车轮半径r取0.425,车辆轴距b取0.8,齿轮比N取5,初始化过程噪声矩阵Q、观测噪声矩阵R以及初始误差协方差矩阵P0,其表达式分别为:/>
与现有技术相比,本发明具有以下优点:
1、本发明通过将GPS信息与IMU信息进行融合,利用两种传感器信息的互补性,获得更为精确的无人车定位结果,实现了提高在简单环境下的自动驾驶的定位精度,进而控制车辆安全行驶的技术效果;
2、本发明通过针对***的非线性程度选取最优的算法获取最优的定位结果,实现了增强无人车定位方法的适用性的技术效果。
附图说明
图1为本发明提出所述的一种融合多源传感器信息的无人车定位方法所使用的线性模型图。
图2为本发明提出所述的一种融合多源传感器信息的无人车定位方法实现的针对线性模型的融合结果放大图。
图3为本发明提出所述的一种融合多源传感器信息的无人车定位方法所使用的运动学模型图。
图4为本发明提出所述的一种融合多源传感器信息的无人车定位方法实现的针对运动学模型的融合结果放大图。
图5为本发明提出所述的一种融合多源传感器信息的无人车定位方法实现的针对运动学模型的三种不同滤波算法的滤波效果对比图。
图6为本发明提出所述的一种融合多源传感器信息的无人车定位方法所使用的单车模型图。
图7为本发明提出所述的一种融合多源传感器信息的无人车定位方法实现的针对单车模型的融合结果图。
图8为本发明提出所述的一种融合多源传感器信息的无人车定位方法实现的针对单车模型的三种不同滤波算法的滤波效果对比放大图。
图9为本发明的方法流程示意图。
具体实施方式
下面结合附图和具体实施例对本发明进行详细说明。
实施例
一种融合多源传感器信息的无人车定位方法,该方法包括以下步骤:
步骤1:获取待控制车辆的GPS位置信息和IMU信息,并将待控制车辆结合惯性导航***与GPS信号获得的厘米级精度定位作为真值,以验证融合后的定位精度;
步骤2:根据GPS位置信息和IMU信息建立线性模型;
步骤3:根据线性模型获得融合多源传感器信息的无人车定位方法,并得到线性模型下的融合定位结果;
步骤4:根据GPS位置信息和IMU信息建立非线性模型;
步骤5:根据非线性模型获取运动学模型下的融合多源传感器信息的无人车定位方法,并得到非线性模型下的融合定位结果;
步骤6:根据非线性模型对非线性模型进一步加深非线性并重新建模,以完成单车模型的建立;
步骤7:根据单车模型获取单车模型下的融合多源传感器信息的无人车定位方法,并得到单车模型下的融合定位结果。
在步骤1中,GPS信息通过安装在车顶的GNSS天线进行收集,角速度与加速度信息通过惯性导航单元进行收集。
在步骤2中,线性模型包括恒定速度模型与恒定加速度模型。
在步骤3中,线性模型下的融合多源传感器信息的无人车定位方法具体为:
将线性模型、IMU信息和GPS信息作为输入,根据卡尔曼滤波算法将GPS位置信息与IMU信息进行融合并得到更准确的无人车位置信息,与真值进行对比;
在步骤4中,非线性模型为恒定加速度与角速度的运动学模型。
在步骤5中,运动学模型下的融合多源传感器信息的无人车定位方法具体为:
分别采用拓展卡尔曼滤波算法、无迹卡尔曼滤波算法和粒子滤波算法完成融合定位,并进行综合比较,选取最优的定位算法。
在步骤6中,单车模型比运动学模型具有更强的非线性,且与实际生活中车辆模型更相似。
在步骤7中,单车模型下的融合多源传感器信息的无人车定位方法具体为:
针对公开数据集采用拓展卡尔曼滤波算法、无迹卡尔曼滤波算法和粒子滤波算法分析单车模型的非线性程度对无人车融合定位算法结果精确度的影响,并获取最优的定位算法。
本发明中,通过GPS与IMU对待控制车辆进行数据采集,针对***进行线性***建模,采用卡尔曼滤波算法融合得到更为精确的定位结果,针对***进行非线性建模并进行实车测试,在非线性***下采用拓展卡尔曼滤波、无迹卡尔曼滤波以及粒子滤波将无人车定位精度明显提高,进一步扩大模型的非线性,采用单车模型进一步进行实验仿真并挑选出最优的滤波算法,本发明将GPS信息与IMU信息进行融合,解决了在简单环境下如何在自动驾驶中提高定位精度以控制车辆行驶的技术问题,且能够在非线性程度不同的***中挑选最优的解决方式,适用性强。
如图1~图8所示,一种融合多源传感器信息的无人车定位方法,无人车定位方法包括以下步骤:获取待控制车辆的GPS信息与IMU信息,并获取待控制车辆结合惯性导航***与GPS信号所获得的厘米级精度定位作为真值,用以与融合定位结果进行对比,建立线性的恒定加速度模型,并采用卡尔曼滤波算法实现GPS位置信息与IMU位置信息融合,与真值进行对比并且计算滤波前后方差,建立恒定加速度与角加速度的运动学模型,采用拓展卡尔曼滤波算法、无迹卡尔曼滤波算法、粒子滤波算法实现GPS信息与IMU信息的融合,计算RMS误差,并判断三种算法效果的优劣,进一步加深非线性并建立单车模型,通过公开数据集完成GPS信息与IMU信息融合,并针对坐标值和航向角进行误差分析。
该发明能够将GPS信息和IMU信息进行优劣互补,实现在简单环境下获取更为精确的无人车定位效果,通过对非线性***下三种滤波算法效果的比较,得到能够实现最为准确的无人车定位效果,在非线性不强的环境中采用拓展卡尔曼滤波算法,当环境的非线性程度加强时,采用无迹卡尔曼滤波算法与粒子滤波算法得到的更优,因此本发明针对非线性程度不同的环境下都能够实现良好的融合定位效果,适用性强。
以上,仅为本发明的具体实施方式,但本发明的保护范围并不局限于此,任何熟悉本技术领域的工作人员在本发明揭露的技术范围内,可轻易想到各种等效的修改或替换,这些修改或替换都应涵盖在本发明的保护范围之内。因此,本发明的保护范围应以权利要求的保护范围为准。
Claims (7)
1.一种融合多源传感器信息的无人车定位方法,其特征在于,该方法包括以下步骤:
步骤1:获取待控制车辆的GPS位置信息和IMU信息,并将待控制车辆结合惯性导航***与GPS信号获得的厘米级精度定位作为真值,以验证融合后的定位精度;
步骤2:根据GPS位置信息和IMU信息建立线性模型;
步骤3:获取线性模型下的融合多源传感器信息的无人车定位算法,并得到线性模型下的融合定位结果;
步骤4:根据GPS位置信息和IMU信息建立非线性模型;
步骤5:获取非线性模型下的融合多源传感器信息的无人车定位算法,并得到非线性模型下的融合定位结果;
步骤6:对非线性模型进一步加深非线性并重新建模得到单车模型;
步骤7:获取单车模型下的融合多源传感器信息的无人车定位算法,并得到单车模型下的融合定位结果;
所述的步骤2中,线性模型包括恒定速度模型与恒定加速度模型,其中,恒定速度模型的状态向量的表达式为:
其中,x、y、θ和v分别为t时刻车辆位置的横坐标、纵坐标、车辆航向角和速度;
恒定速度模型的状态方程和观测方程的表达式分别为:
恒定加速度模型的状态向量的表达式为:
其中,s和v分别为t时刻车辆的位移和速度;
恒定加速度模型的状态方程和观测方程的表达式分别为:
恒定加速度模型将加速度计测量的数值作为***输入:
u(k)=a(k)
其中,u(k)为***输入,a(k)为加速度计测量的数值;
所述的步骤4中,非线性模型为基于恒定加速度与角速度的运动学模型,运动学模型的状态向量的表达式为:
其中,xk、yk、ψk、ωk、vk和ak分别为k时刻车辆的横坐标、纵坐标、航向角、角速度、速度以及加速度;
运动学模型的状态方程和观测方程的表达式分别为:
2.根据权利要求1所述的一种融合多源传感器信息的无人车定位方法,其特征在于,所述的步骤1中,通过安装在车顶的GNSS天线收集GPS信息,通过惯性导航单元收集IMU信息,所述的GPS信息包括车辆的经纬度信息,且车辆的经纬度信可转换为车辆的横坐标和纵坐标,所述的IMU信息包括车辆的三轴角速度和加速度。
3.根据权利要求1所述的一种融合多源传感器信息的无人车定位方法,其特征在于,所述的步骤3中,线性模型下的融合多源传感器信息的无人车定位方法具体为:
步骤301:根据卡尔曼滤波算法得到更为准确的无人车位置信息,卡尔曼滤波算法具体过程分为状态预测、误差协方差预测、卡尔曼增益更新、状态更新与误差协方差更新五个环节:
其中,表示k时刻状态向量的预测值,/>表示k时刻状态向量的最优估计值,/>表示k时刻误差协方差的预测值,Pk表示k时刻误差协方差的最优估计值,Kk表示k时刻的卡尔曼增益,Q为过程噪声协方差矩阵,R为观测噪声协方差矩阵,uk-1表示k-1时刻的***输入,A表示***的状态转移矩阵,H表示***的观测矩阵,I表示单位矩阵,B为控制输入矩阵;
对于恒定速度模型,根据恒定速度模型的状态方程和观测方程得到观测矩阵H与状态转移矩阵A的表达式分别为:
其中,Δt为时间间隔,θ为车辆的航向角;
将协方差矩阵P初始化为单位矩阵,将过程噪声矩阵Q与观测噪声矩阵R初始化:
对于恒定加速度模型,根据恒定加速度模型的状态方程、观测值和***输入得到状态转移矩阵A、观测矩阵H和控制输入矩阵B的表达式分别为:
H=[1 0]
其中,Δt为时间间隔;
将协方差矩阵P初始化为单位矩阵,过程噪声矩阵Q与观测噪声矩阵R初始化分别为:
R=1;
步骤302:将得到的最优估计值与真值进行对比,并计算滤波前后的方差。
4.根据权利要求1所述的一种融合多源传感器信息的无人车定位方法,其特征在于,所述的步骤5中,获取运动学模型下的融合多源传感器信息的无人车定位方法的过程具体包括以下步骤:
步骤501:分别采用拓展卡尔曼算法、无迹卡尔曼算法与粒子滤波算法进行融合定位;
步骤502:将三种滤波算法进行综合比较,计算RMS误差,以选取最优的定位算法,RMS误差的计算公式为:
errorrms=|X-Xest|
其中,errorrms为经过滤波后的误差,X为真值,包括车辆的横坐标和纵坐标的真实值,Xest分别为经过滤波后车辆横坐标与纵坐标的最佳估计值;
步骤503:选取RMS误差最小的对应的滤波算法,并基于该滤波算法获取非线性模型下的融合定位结果。
5.根据权利要求1所述的一种融合多源传感器信息的无人车定位方法,其特征在于,所述的步骤6中,单车模型中的速度与角速度之间满足的关系为:
滑移角β的计算公式为:
其中,lr与lf分别为后悬长度与前悬长度,δf与δr分别为前轮偏角与后轮偏角;
单车模型的状态向量的表达式为:
其中,xk、yk、ψk、r、b、N、δr,k和ωk分别为k时刻车辆的横坐标、纵坐标、航向角、车轮半径、车辆轴距、齿轮比、前轮偏角以及车轮转动角速度,车轮半径、车辆轴距和齿轮比保持不变,且将车辆的前轮偏角和车轮转动角速度为***输入,dt为时间间隔;
单车模型的状态方程为:
单车模型的观测值为:
其中,xg,k和yg,k分别为在k时刻GPS观测到的车辆的横坐标和纵坐标;
单车模型的观测方程为:
式中,B为控制输入矩阵。
6.根据权利要求5所述的一种融合多源传感器信息的无人车定位方法,其特征在于,所述的步骤7中,获取单车模型下的融合多源传感器信息的无人车定位算法的过程具体包括以下步骤:
步骤701:基于公开数据集采用拓展卡尔曼算法、无迹卡尔曼算法与粒子滤波算法分别得到单车模型下的车辆的最佳估计值;
步骤702:针对车辆的坐标和航向角进行误差分析,计算车辆的横坐标、纵坐标以及航向角对应的RMS误差,RMS误差的计算公式为:
errorrms=|X-Xest|
其中,errorrms为经过滤波后的误差,X为真值,包括车辆的横坐标和纵坐标的真实值,Xest分别为经过滤波后车辆的横坐标、纵坐标以及航向角的最佳估计值;
步骤703:将每个RMS误差进行均值处理,实现归一化;
步骤704:将每一种滤波算法的三个归一化后的RMS误差相加形成分值,通过比较滤波算法的分值以评估三种滤波算法的性能与效果,进而选取分值最小的作为最优的定位算法;
步骤705:基于最优的定位算法得到单车模型下的融合定位结果。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202210109679.2A CN114440881B (zh) | 2022-01-29 | 2022-01-29 | 一种融合多源传感器信息的无人车定位方法 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202210109679.2A CN114440881B (zh) | 2022-01-29 | 2022-01-29 | 一种融合多源传感器信息的无人车定位方法 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN114440881A CN114440881A (zh) | 2022-05-06 |
CN114440881B true CN114440881B (zh) | 2023-05-30 |
Family
ID=81372107
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN202210109679.2A Active CN114440881B (zh) | 2022-01-29 | 2022-01-29 | 一种融合多源传感器信息的无人车定位方法 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN114440881B (zh) |
Families Citing this family (1)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN116222544B (zh) * | 2023-05-09 | 2023-08-04 | 浙江大学湖州研究院 | 一种面向饲养场的投料车自动导航定位方法及装置 |
Citations (9)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CA2716597A1 (en) * | 2010-10-08 | 2012-04-08 | Canadian Space Agency | Apparatus and method for driftless attitude determination and reliable localization of vehicles |
JP2012173190A (ja) * | 2011-02-23 | 2012-09-10 | Seiko Epson Corp | 測位システム、測位方法 |
CN103439731A (zh) * | 2013-08-29 | 2013-12-11 | 北京空间飞行器总体设计部 | 基于无迹卡尔曼滤波的gps/ins组合导航方法 |
CN108931799A (zh) * | 2018-07-18 | 2018-12-04 | 兰州交通大学 | 基于递归快速正交搜索的列车组合定位方法及*** |
CN110243358A (zh) * | 2019-04-29 | 2019-09-17 | 武汉理工大学 | 多源融合的无人车室内外定位方法及*** |
CN110602647A (zh) * | 2019-09-11 | 2019-12-20 | 江南大学 | 基于扩展卡尔曼滤波和粒子滤波的室内融合定位方法 |
CN110956665A (zh) * | 2019-12-18 | 2020-04-03 | 中国科学院自动化研究所 | 车辆拐弯轨迹双向计算方法、***、装置 |
CN113063429A (zh) * | 2021-03-18 | 2021-07-02 | 苏州华米导航科技有限公司 | 一种自适应车载组合导航定位方法 |
CN113252033A (zh) * | 2021-06-29 | 2021-08-13 | 长沙海格北斗信息技术有限公司 | 基于多传感器融合的定位方法、定位***及机器人 |
-
2022
- 2022-01-29 CN CN202210109679.2A patent/CN114440881B/zh active Active
Patent Citations (9)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CA2716597A1 (en) * | 2010-10-08 | 2012-04-08 | Canadian Space Agency | Apparatus and method for driftless attitude determination and reliable localization of vehicles |
JP2012173190A (ja) * | 2011-02-23 | 2012-09-10 | Seiko Epson Corp | 測位システム、測位方法 |
CN103439731A (zh) * | 2013-08-29 | 2013-12-11 | 北京空间飞行器总体设计部 | 基于无迹卡尔曼滤波的gps/ins组合导航方法 |
CN108931799A (zh) * | 2018-07-18 | 2018-12-04 | 兰州交通大学 | 基于递归快速正交搜索的列车组合定位方法及*** |
CN110243358A (zh) * | 2019-04-29 | 2019-09-17 | 武汉理工大学 | 多源融合的无人车室内外定位方法及*** |
CN110602647A (zh) * | 2019-09-11 | 2019-12-20 | 江南大学 | 基于扩展卡尔曼滤波和粒子滤波的室内融合定位方法 |
CN110956665A (zh) * | 2019-12-18 | 2020-04-03 | 中国科学院自动化研究所 | 车辆拐弯轨迹双向计算方法、***、装置 |
CN113063429A (zh) * | 2021-03-18 | 2021-07-02 | 苏州华米导航科技有限公司 | 一种自适应车载组合导航定位方法 |
CN113252033A (zh) * | 2021-06-29 | 2021-08-13 | 长沙海格北斗信息技术有限公司 | 基于多传感器融合的定位方法、定位***及机器人 |
Non-Patent Citations (3)
Title |
---|
Asynchronous Sensor Fusion of GPS, IMU and CAN-Based Odometry for Heavy-Duty Vehicles;Vicent Girbés-Juan等;《IEEE Transactions on Vehicular Technology 》;第70卷(第9期);第8617-8626页 * |
Multi-sensor fusion methodology for enhanced land vehicle positioning;Li Xu等;《Information Fusion》;第46卷(第3期);第51-62页 * |
基于融合算法的GPS/UWB/MARG协同定位***研究;周群等;《现代电子技术》;第41卷(第6期);第82-86页 * |
Also Published As
Publication number | Publication date |
---|---|
CN114440881A (zh) | 2022-05-06 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN111307162B (zh) | 用于自动驾驶场景的多传感器融合定位方法 | |
CN112083726B (zh) | 一种面向园区自动驾驶的双滤波器融合定位*** | |
CN111426318B (zh) | 基于四元数-扩展卡尔曼滤波的低成本ahrs航向角补偿方法 | |
CN104061899B (zh) | 一种基于卡尔曼滤波的车辆侧倾角与俯仰角估计方法 | |
CN113819914B (zh) | 一种地图构建方法及装置 | |
CN111595592B (zh) | 一种自适应巡航控制***性能测评方法 | |
CN109466558B (zh) | 一种基于ekf和bp神经网络的路面附着系数估计方法 | |
CN112505737B (zh) | 一种gnss/ins组合导航方法 | |
CN108362288B (zh) | 一种基于无迹卡尔曼滤波的偏振光slam方法 | |
CN113386781A (zh) | 一种基于数据驱动车辆动力学模型的智能车辆轨迹跟踪控制方法 | |
CN104198765A (zh) | 车辆运动加速度检测的坐标系转换方法 | |
CN108387236B (zh) | 一种基于扩展卡尔曼滤波的偏振光slam方法 | |
JP7036080B2 (ja) | 慣性航法装置 | |
CN113063429B (zh) | 一种自适应车载组合导航定位方法 | |
CN113340298B (zh) | 一种惯导和双天线gnss外参标定方法 | |
CN114966629A (zh) | 一种基于ekf算法框架的车体激光雷达外参标定方法 | |
CN112099378B (zh) | 考虑随机测量时滞的前车侧向运动状态实时估计方法 | |
CN111751857A (zh) | 一种车辆位姿的估算方法、装置、存储介质及*** | |
CN114440881B (zh) | 一种融合多源传感器信息的无人车定位方法 | |
CN114912061B (zh) | 一种面向商用车车道保持辅助***的精准测评方法 | |
CN113008229B (zh) | 一种基于低成本车载传感器的分布式自主组合导航方法 | |
CN109033017B (zh) | 一种丢包环境下的车辆侧倾角与俯仰角估计方法 | |
CN114935345A (zh) | 一种基于模式识别的车载惯导安装角误差补偿方法 | |
CN113029173A (zh) | 车辆导航方法及装置 | |
CN114475581B (zh) | 基于轮速脉冲和imu卡尔曼滤波融合的自动泊车定位方法 |
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 |