CN114440881A - 一种融合多源传感器信息的无人车定位方法 - Google Patents

一种融合多源传感器信息的无人车定位方法 Download PDF

Info

Publication number
CN114440881A
CN114440881A CN202210109679.2A CN202210109679A CN114440881A CN 114440881 A CN114440881 A CN 114440881A CN 202210109679 A CN202210109679 A CN 202210109679A CN 114440881 A CN114440881 A CN 114440881A
Authority
CN
China
Prior art keywords
vehicle
model
matrix
algorithm
time
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
Application number
CN202210109679.2A
Other languages
English (en)
Other versions
CN114440881B (zh
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.)
Hainan University
Original Assignee
Hainan 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 Hainan University filed Critical Hainan University
Priority to CN202210109679.2A priority Critical patent/CN114440881B/zh
Publication of CN114440881A publication Critical patent/CN114440881A/zh
Application granted granted Critical
Publication of CN114440881B publication Critical patent/CN114440881B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

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/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中，线性模型包括恒定速度模型与恒定加速度模型，其中，恒定速度模型的状态向量的表达式为：
Figure BDA0003494729110000021
其中，x、y、θ和v分别为t时刻车辆位置的横坐标、纵坐标、车辆航向角和速度；
恒定速度模型的状态方程和观测方程的表达式分别为：
Figure BDA0003494729110000022
Figure BDA0003494729110000023
其中，Δt为时间间隔，
Figure BDA0003494729110000024
为经过Δt时间后的状态向量，
Figure BDA0003494729110000025
为由GPS得到的观测值，xgps为由GPS得到的车辆位置的横坐标，ygps为由GPS得到的车辆位置的纵坐标；
恒定加速度模型的状态向量的表达式为：
Figure BDA0003494729110000026
其中，s和v分别为t时刻车辆的位移和速度；
恒定加速度模型的状态方程和观测方程的表达式分别为：
Figure BDA0003494729110000027
Figure BDA0003494729110000028
其中，a为车辆的加速度，
Figure BDA0003494729110000029
为经过Δt时间后的状态向量，s(t)为t时刻车辆的位移，v(t)为t时刻车辆的速度，
Figure BDA0003494729110000031
为观测值，即GPS测量得到的位移；
恒定加速度模型将加速度计测量的数值作为***输入：
u(k)＝a(k)
其中，u(k)为***输入，a(k)为加速度计测量的数值。
所述的步骤3中，线性模型下的融合多源传感器信息的无人车定位方法具体为：
步骤301：根据卡尔曼滤波算法得到更为准确的无人车位置信息，卡尔曼滤波算法具体过程分为状态预测、误差协方差预测、卡尔曼增益更新、状态更新与误差协方差更新五个环节：
Figure BDA0003494729110000032
Figure BDA0003494729110000033
Figure BDA0003494729110000034
Figure BDA0003494729110000035
Figure BDA0003494729110000036
其中，
Figure BDA0003494729110000037
表示k时刻状态向量的预测值，
Figure BDA0003494729110000038
表示k时刻状态向量的最优估计值，
Figure BDA0003494729110000039
表示k时刻误差协方差的预测值，Pk表示k时刻误差协方差的最优估计值，Kk表示k时刻的卡尔曼增益，Q为过程噪声协方差矩阵，R为观测噪声协方差矩阵，uk-1表示k-1时刻的***输入，A表示***的状态转移矩阵，H表示***的观测矩阵，I表示单位矩阵，B为控制输入矩阵；
对于恒定速度模型，根据恒定速度模型的状态方程和观测方程得到观测矩阵H与状态转移矩阵A的表达式分别为：
Figure BDA00034947291100000310
Figure BDA00034947291100000311
其中，Δt为时间间隔，θ为车辆的航向角；
将协方差矩阵P初始化为单位矩阵，将过程噪声矩阵Q与观测噪声矩阵R初始化：
Figure BDA00034947291100000312
Figure BDA00034947291100000313
对于恒定加速度模型，根据恒定加速度模型的状态方程、观测值和***输入得到状态转移矩阵A、观测矩阵H和控制输入矩阵B的表达式分别为：
Figure BDA0003494729110000041
H＝[1 0]
Figure BDA0003494729110000042
其中，Δt为时间间隔；
将协方差矩阵P初始化为单位矩阵，过程噪声矩阵Q与观测噪声矩阵R初始化分别为：
Figure BDA0003494729110000043
R＝1；
步骤302：将得到的最优估计值与真值进行对比，并计算滤波前后的方差。
所述的步骤4中，非线性模型为基于恒定加速度与角速度的运动学模型，运动学模型的状态向量的表达式为：
Figure BDA0003494729110000044
其中，xk、yk、ψk、ωk、vk和ak分别为k时刻车辆的横坐标、纵坐标、航向角、角速度、速度以及加速度；
运动学模型的状态方程和观测方程的表达式分别为：
Figure BDA0003494729110000045
Figure BDA0003494729110000046
其中，
Figure BDA0003494729110000047
为k-1时刻车辆的状态向量，vk-1为k-1时刻车辆的速度，dt为时间间隔，
Figure BDA0003494729110000048
为观测值，xg,k、yg,k、aa,k和ωg,k分别为k时刻GPS观测得到的车辆横坐标、纵坐标、加速度计获得加速度以及陀螺仪测得的角速度.
所述的步骤5中，获取运动学模型下的融合多源传感器信息的无人车定位方法的过程具体包括以下步骤：
步骤501：分别采用拓展卡尔曼算法、无迹卡尔曼算法与粒子滤波算法进行融合定位；
步骤502：将三种滤波算法进行综合比较，计算RMS误差，以选取最优的定位算法，RMS误差的计算公式为：
errorrms＝|X-Xest|
其中，errorrms为经过滤波后的误差，X为真值，包括车辆的横坐标和纵坐标的真实值，Xest分别为经过滤波后车辆横坐标与纵坐标的最佳估计值；
步骤503：选取RMS误差最小的对应的滤波算法，并基于该滤波算法获取非线性模型下的融合定位结果。
所述的步骤501中，拓展卡尔曼滤波算法具体的公式包括：
Figure BDA0003494729110000051
Figure BDA0003494729110000052
其中，Fk-1与Hk分别为雅各比矩阵，uk为k时刻的***输入，xk-1为k-1时刻的状态向量；
雅各比矩阵Fk-1和Hk的表达式分别为：
Figure BDA0003494729110000053
Figure BDA0003494729110000054
其中，ψk-1为k-1时刻车辆的航向角，vk-1为k-1时刻车辆的速度，dt为时间间隔；
无迹卡尔曼滤波算法的具体滤波过程为：
步骤1a：初始化状态向量x0、状态估计误差协方差P0、过程噪声矩阵Q以及观测噪声矩阵R，对于运动学模型，状态向量的初值
Figure BDA0003494729110000055
为真值，过程噪声矩阵Q、观测噪声矩阵R以及初始误差协方差矩阵P0初始化的表达式为：
Figure BDA0003494729110000061
Figure BDA0003494729110000062
Figure BDA0003494729110000063
步骤2a：根据上一时刻的状态向量xk和误差协方差矩阵Pk选择当前时刻采样点，并分配权重；
步骤3a：选择2n+1个采样点，通过***的状态方程计算当前时刻所有采样点的均值和协方差，完成时间更新；
步骤4a：通过***的观测方程对采样点进行量测更新；
步骤5a：通过预测与量测更新计算出卡尔曼增益，完成滤波更新；
粒子滤波算法采用蒙特卡洛法解决非线性滤波问题，粒子滤波算法的滤波过程为：
步骤1b：初始化N个粒子
Figure BDA0003494729110000064
且将粒子的权值
Figure BDA0003494729110000065
统一设置为1/N；
步骤2b：更新粒子
Figure BDA0003494729110000066
计算每个粒子的似然概率
Figure BDA0003494729110000067
将每个粒子的权值
Figure BDA0003494729110000068
更新为
Figure BDA0003494729110000069
并进行归一化处理，粒子的权值更新公式为：
Figure BDA00034947291100000610
其中，
Figure BDA00034947291100000611
为粒子的权值，
Figure BDA00034947291100000612
为粒子的似然概率，zk为k时刻***的观测值，
Figure BDA00034947291100000613
为k时刻第i个粒子观测方程的输出值，Rk为k时刻的观测噪声协方差矩阵；
步骤3b：对
Figure BDA0003494729110000071
进行重采样，计算有效粒子数Neff，以此得到新的粒子群
Figure BDA0003494729110000072
且粒子的权值
Figure BDA0003494729110000073
为1/N；
步骤4b：采用经过重采样后的粒子的权值和状态计算当前时刻的滤波状态估计与方差估计。
所述的步骤6中，单车模型中的速度与角速度之间满足的关系为：
Figure BDA0003494729110000074
Figure BDA0003494729110000075
Figure BDA0003494729110000076
其中，β为滑移角，
Figure BDA0003494729110000077
为水平方向速度，
Figure BDA0003494729110000078
为垂直方向速度，
Figure BDA0003494729110000079
为航向角的角速度，v为质心车速，ψ为航向角大小，lr与lf分别为后悬长度与前悬长度，δf与δr分别为前轮偏角与后轮偏角；
滑移角β的计算公式为：
Figure BDA00034947291100000710
其中，lr与lf分别为后悬长度与前悬长度，δf与δr分别为前轮偏角与后轮偏角；
单车模型的状态向量的表达式为：
Figure BDA00034947291100000711
其中，xk、yk、ψk、r、b、N、δr,k和ωk分别为k时刻车辆的横坐标、纵坐标、航向角、车轮半径、车辆轴距、齿轮比、前轮偏角以及车轮转动角速度，车轮半径、车辆轴距和齿轮比保持不变，且将车辆的前轮偏角和车轮转动角速度为***输入，dt为时间间隔；
单车模型的状态方程为：
Figure BDA00034947291100000712
单车模型的观测值为：
Figure BDA0003494729110000081
其中，xg,k和yg,k分别为在k时刻GPS观测到的车辆的横坐标和纵坐标；
单车模型的观测方程为：
Figure BDA0003494729110000082
所述的步骤7中，获取单车模型下的融合多源传感器信息的无人车定位算法的过程具体包括以下步骤：
步骤701：基于公开数据集采用拓展卡尔曼算法、无迹卡尔曼算法与粒子滤波算法分别得到单车模型下的车辆的最佳估计值；
步骤702：针对车辆的坐标和航向角进行误差分析，计算车辆的横坐标、纵坐标以及航向角对应的RMS误差，RMS误差的计算公式为：
errorrms＝|X-Xest|
其中，errorrms为经过滤波后的误差，X为真值，包括车辆的横坐标和纵坐标的真实值，Xest分别为经过滤波后车辆的横坐标、纵坐标以及航向角的最佳估计值；
步骤703：将每个RMS误差进行均值处理，实现归一化；
步骤704：将每一种滤波算法的三个归一化后的RMS误差相加形成分值，通过比较滤波算法的分值以评估三种滤波算法的性能与效果，进而选取分值最小的作为最优的定位算法；
步骤705：基于最优的定位算法得到单车模型下的融合定位结果。
所述的步骤701中，对于单车模型采用滤波算法进行滤波时，初始状态向量
Figure BDA0003494729110000083
取真值，车轮半径r取0.425，车辆轴距b取0.8，齿轮比N取5，初始化过程噪声矩阵Q、观测噪声矩阵R以及初始误差协方差矩阵P0，其表达式分别为：
Figure BDA0003494729110000084
Figure BDA0003494729110000091
Figure BDA0003494729110000092
与现有技术相比，本发明具有以下优点：
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 (10)

1.一种融合多源传感器信息的无人车定位方法，其特征在于，该方法包括以下步骤：
步骤1：获取待控制车辆的GPS位置信息和IMU信息，并将待控制车辆结合惯性导航***与GPS信号获得的厘米级精度定位作为真值，以验证融合后的定位精度；
步骤2：根据GPS位置信息和IMU信息建立线性模型；
步骤3：获取线性模型下的融合多源传感器信息的无人车定位算法，并得到线性模型下的融合定位结果；
步骤4：根据GPS位置信息和IMU信息建立非线性模型；
步骤5：获取非线性模型下的融合多源传感器信息的无人车定位算法，并得到非线性模型下的融合定位结果；
步骤6：对非线性模型进一步加深非线性并重新建模得到单车模型；
步骤7：获取单车模型下的融合多源传感器信息的无人车定位算法，并得到单车模型下的融合定位结果。
2.根据权利要求1所述的一种融合多源传感器信息的无人车定位方法，其特征在于，所述的步骤1中，通过安装在车顶的GNSS天线收集GPS信息，通过惯性导航单元收集IMU信息，所述的GPS信息包括车辆的经纬度信息，且车辆的经纬度信可转换为车辆的横坐标和纵坐标，所述的IMU信息包括车辆的三轴角速度和加速度。
3.根据权利要求1所述的一种融合多源传感器信息的无人车定位方法，其特征在于，所述的步骤2中，线性模型包括恒定速度模型与恒定加速度模型，其中，恒定速度模型的状态向量的表达式为：
Figure FDA0003494729100000011
其中，x、y、θ和v分别为t时刻车辆位置的横坐标、纵坐标、车辆航向角和速度；
恒定速度模型的状态方程和观测方程的表达式分别为：
Figure FDA0003494729100000021
Figure FDA0003494729100000022
其中，Δt为时间间隔，
Figure FDA0003494729100000023
为经过Δt时间后的状态向量，
Figure FDA0003494729100000024
为由GPS得到的观测值，xgps为由GPS得到的车辆位置的横坐标，ygps为由GPS得到的车辆位置的纵坐标；
恒定加速度模型的状态向量的表达式为：
Figure FDA0003494729100000025
其中，s和v分别为t时刻车辆的位移和速度；
恒定加速度模型的状态方程和观测方程的表达式分别为：
Figure FDA0003494729100000026
Figure FDA0003494729100000027
其中，a为车辆的加速度，
Figure FDA0003494729100000028
为经过Δt时间后的状态向量，s(t)为t时刻车辆的位移，v(t)为t时刻车辆的速度，
Figure FDA0003494729100000029
为观测值，即GPS测量得到的位移；
恒定加速度模型将加速度计测量的数值作为***输入：
u(k)＝a(k)
其中，u(k)为***输入，a(k)为加速度计测量的数值。
4.根据权利要求3所述的一种融合多源传感器信息的无人车定位方法，其特征在于，所述的步骤3中，线性模型下的融合多源传感器信息的无人车定位方法具体为：
步骤301：根据卡尔曼滤波算法得到更为准确的无人车位置信息，卡尔曼滤波算法具体过程分为状态预测、误差协方差预测、卡尔曼增益更新、状态更新与误差协方差更新五个环节：
Figure FDA00034947291000000210
Figure FDA00034947291000000211
Figure FDA00034947291000000212
Figure FDA00034947291000000213
Figure FDA0003494729100000031
其中，
Figure FDA0003494729100000032
表示k时刻状态向量的预测值，
Figure FDA0003494729100000033
表示k时刻状态向量的最优估计值，
Figure FDA0003494729100000034
表示k时刻误差协方差的预测值，Pk表示k时刻误差协方差的最优估计值，Kk表示k时刻的卡尔曼增益，Q为过程噪声协方差矩阵，R为观测噪声协方差矩阵，uk-1表示k-1时刻的***输入，A表示***的状态转移矩阵，H表示***的观测矩阵，I表示单位矩阵，B为控制输入矩阵；
对于恒定速度模型，根据恒定速度模型的状态方程和观测方程得到观测矩阵H与状态转移矩阵A的表达式分别为：
Figure FDA0003494729100000035
Figure FDA0003494729100000036
其中，Δt为时间间隔，θ为车辆的航向角；
将协方差矩阵P初始化为单位矩阵，将过程噪声矩阵Q与观测噪声矩阵R初始化：
Figure FDA0003494729100000037
Figure FDA0003494729100000038
对于恒定加速度模型，根据恒定加速度模型的状态方程、观测值和***输入得到状态转移矩阵A、观测矩阵H和控制输入矩阵B的表达式分别为：
Figure FDA0003494729100000039
H＝[1 0]
Figure FDA00034947291000000310
其中，Δt为时间间隔；
将协方差矩阵P初始化为单位矩阵，过程噪声矩阵Q与观测噪声矩阵R初始化分别为：
Figure FDA00034947291000000311
R＝1；
步骤302：将得到的最优估计值与真值进行对比，并计算滤波前后的方差。
5.根据权利要求1所述的一种融合多源传感器信息的无人车定位方法，其特征在于，所述的步骤4中，非线性模型为基于恒定加速度与角速度的运动学模型，运动学模型的状态向量的表达式为：
Figure FDA0003494729100000041
其中，xk、yk、ψk、ωk、vk和ak分别为k时刻车辆的横坐标、纵坐标、航向角、角速度、速度以及加速度；
运动学模型的状态方程和观测方程的表达式分别为：
Figure FDA0003494729100000042
Figure FDA0003494729100000043
其中，
Figure FDA0003494729100000044
为k-1时刻车辆的状态向量，vk-1为k-1时刻车辆的速度，dt为时间间隔，
Figure FDA0003494729100000045
为观测值，xg,k、yg,k、aa,k和ωg,k分别为k时刻GPS观测得到的车辆横坐标、纵坐标、加速度计获得加速度以及陀螺仪测得的角速度。
6.根据权利要求1所述的一种融合多源传感器信息的无人车定位方法，其特征在于，所述的步骤5中，获取运动学模型下的融合多源传感器信息的无人车定位方法的过程具体包括以下步骤：
步骤501：分别采用拓展卡尔曼算法、无迹卡尔曼算法与粒子滤波算法进行融合定位；
步骤502：将三种滤波算法进行综合比较，计算RMS误差，以选取最优的定位算法，RMS误差的计算公式为：
errorrms＝|X-Xest|
其中，errorrms为经过滤波后的误差，X为真值，包括车辆的横坐标和纵坐标的真实值，Xest分别为经过滤波后车辆横坐标与纵坐标的最佳估计值；
步骤503：选取RMS误差最小的对应的滤波算法，并基于该滤波算法获取非线性模型下的融合定位结果。
7.根据权利要求6所述的一种融合多源传感器信息的无人车定位方法，其特征在于，所述的步骤501中，拓展卡尔曼滤波算法具体的公式包括：
Figure FDA0003494729100000051
Figure FDA0003494729100000052
其中，Fk-1与Hk分别为雅各比矩阵，uk为k时刻的***输入，xk-1为k-1时刻的状态向量；
雅各比矩阵Fk-1和Hk的表达式分别为：
Figure FDA0003494729100000053
Figure FDA0003494729100000054
其中，ψk-1为k-1时刻车辆的航向角，vk-1为k-1时刻车辆的速度，dt为时间间隔；
无迹卡尔曼滤波算法的具体滤波过程为：
步骤1a：初始化状态向量x0、状态估计误差协方差P0、过程噪声矩阵Q以及观测噪声矩阵R，对于运动学模型，状态向量的初值
Figure FDA0003494729100000055
为真值，过程噪声矩阵Q、观测噪声矩阵R以及初始误差协方差矩阵P0初始化的表达式为：
Figure FDA0003494729100000056
Figure FDA0003494729100000057
Figure FDA0003494729100000061
步骤2a：根据上一时刻的状态向量xk和误差协方差矩阵Pk选择当前时刻采样点，并分配权重；
步骤3a：选择2n+1个采样点，通过***的状态方程计算当前时刻所有采样点的均值和协方差，完成时间更新；
步骤4a：通过***的观测方程对采样点进行量测更新；
步骤5a：通过预测与量测更新计算出卡尔曼增益，完成滤波更新；
粒子滤波算法采用蒙特卡洛法解决非线性滤波问题，粒子滤波算法的滤波过程为：
步骤1b：初始化N个粒子
Figure FDA0003494729100000062
且将粒子的权值
Figure FDA0003494729100000063
统一设置为1/N；
步骤2b：更新粒子
Figure FDA0003494729100000064
计算每个粒子的似然概率
Figure FDA0003494729100000065
将每个粒子的权值
Figure FDA0003494729100000066
更新为
Figure FDA0003494729100000067
并进行归一化处理，粒子的权值更新公式为：
Figure FDA0003494729100000068
其中，
Figure FDA0003494729100000069
为粒子的权值，
Figure FDA00034947291000000610
为粒子的似然概率，zk为k时刻***的观测值，
Figure FDA00034947291000000611
为k时刻第i个粒子观测方程的输出值，Rk为k时刻的观测噪声协方差矩阵；
步骤3b：对
Figure FDA00034947291000000612
进行重采样，计算有效粒子数Neff，以此得到新的粒子群
Figure FDA00034947291000000613
且粒子的权值
Figure FDA00034947291000000614
为1/N；
步骤4b：采用经过重采样后的粒子的权值和状态计算当前时刻的滤波状态估计与方差估计。
8.根据权利要求1所述的一种融合多源传感器信息的无人车定位方法，其特征在于，所述的步骤6中，单车模型中的速度与角速度之间满足的关系为：
Figure FDA0003494729100000071
Figure FDA0003494729100000072
Figure FDA0003494729100000073
其中，β为滑移角，
Figure FDA0003494729100000074
为水平方向速度，
Figure FDA0003494729100000075
为垂直方向速度，
Figure FDA0003494729100000076
为航向角的角速度，v为质心车速，ψ为航向角大小，lr与lf分别为后悬长度与前悬长度，δf与δr分别为前轮偏角与后轮偏角；
滑移角β的计算公式为：
Figure FDA0003494729100000077
其中，lr与lf分别为后悬长度与前悬长度，δf与δr分别为前轮偏角与后轮偏角；
单车模型的状态向量的表达式为：
Figure FDA0003494729100000078
其中，xk、yk、ψk、r、b、N、δr,k和ωk分别为k时刻车辆的横坐标、纵坐标、航向角、车轮半径、车辆轴距、齿轮比、前轮偏角以及车轮转动角速度，车轮半径、车辆轴距和齿轮比保持不变，且将车辆的前轮偏角和车轮转动角速度为***输入，dt为时间间隔；
单车模型的状态方程为：
Figure FDA0003494729100000079
单车模型的观测值为：
Figure FDA00034947291000000710
其中，xg,k和yg,k分别为在k时刻GPS观测到的车辆的横坐标和纵坐标；
单车模型的观测方程为：
Figure FDA0003494729100000081
9.根据权利要求8所述的一种融合多源传感器信息的无人车定位方法，其特征在于，所述的步骤7中，获取单车模型下的融合多源传感器信息的无人车定位算法的过程具体包括以下步骤：
步骤701：基于公开数据集采用拓展卡尔曼算法、无迹卡尔曼算法与粒子滤波算法分别得到单车模型下的车辆的最佳估计值；
步骤702：针对车辆的坐标和航向角进行误差分析，计算车辆的横坐标、纵坐标以及航向角对应的RMS误差，RMS误差的计算公式为：
errorrms＝|X-Xest|
其中，errorrms为经过滤波后的误差，X为真值，包括车辆的横坐标和纵坐标的真实值，Xest分别为经过滤波后车辆的横坐标、纵坐标以及航向角的最佳估计值；
步骤703：将每个RMS误差进行均值处理，实现归一化；
步骤704：将每一种滤波算法的三个归一化后的RMS误差相加形成分值，通过比较滤波算法的分值以评估三种滤波算法的性能与效果，进而选取分值最小的作为最优的定位算法；
步骤705：基于最优的定位算法得到单车模型下的融合定位结果。
10.根据权利要求9所述的一种融合多源传感器信息的无人车定位方法，其特征在于，所述的步骤701中，对于单车模型采用滤波算法进行滤波时，初始状态向量
Figure FDA0003494729100000082
取真值，车轮半径r取0.425，车辆轴距b取0.8，齿轮比N取5，初始化过程噪声矩阵Q、观测噪声矩阵R以及初始误差协方差矩阵P0，其表达式分别为：
Figure FDA0003494729100000083
Figure FDA0003494729100000084
Figure FDA0003494729100000091
CN202210109679.2A 2022-01-29 2022-01-29 一种融合多源传感器信息的无人车定位方法 Active CN114440881B (zh)

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 true CN114440881A (zh) 2022-05-06
CN114440881B 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)

Cited By (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN116222544A (zh) * 2023-05-09 2023-06-06 浙江大学湖州研究院 一种面向饲养场的投料车自动导航定位方法及装置

Citations (9)

* Cited by examiner, † Cited by third party
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 长沙海格北斗信息技术有限公司 基于多传感器融合的定位方法、定位***及机器人

Patent Citations (9)

* Cited by examiner, † Cited by third party
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)

* Cited by examiner, † Cited by third party
Title
LI XU等: "Multi-sensor fusion methodology for enhanced land vehicle positioning", 《INFORMATION FUSION》 *
VICENT GIRBÉS-JUAN等: "Asynchronous Sensor Fusion of GPS, IMU and CAN-Based Odometry for Heavy-Duty Vehicles", 《IEEE TRANSACTIONS ON VEHICULAR TECHNOLOGY 》 *
周群等: "基于融合算法的GPS/UWB/MARG协同定位***研究", 《现代电子技术》 *

Cited By (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN116222544A (zh) * 2023-05-09 2023-06-06 浙江大学湖州研究院 一种面向饲养场的投料车自动导航定位方法及装置
CN116222544B (zh) * 2023-05-09 2023-08-04 浙江大学湖州研究院 一种面向饲养场的投料车自动导航定位方法及装置

Also Published As

Publication number Publication date
CN114440881B (zh) 2023-05-30

Similar Documents

Publication Publication Date Title
CN111307162B (zh) 用于自动驾驶场景的多传感器融合定位方法
CN112083726B (zh) 一种面向园区自动驾驶的双滤波器融合定位***
CN111426318B (zh) 基于四元数-扩展卡尔曼滤波的低成本ahrs航向角补偿方法
CN108731670B (zh) 基于量测模型优化的惯性/视觉里程计组合导航定位方法
CN106840179B (zh) 一种基于多传感器信息融合的智能车定位方法
CN104061899B (zh) 一种基于卡尔曼滤波的车辆侧倾角与俯仰角估计方法
CN111595592B (zh) 一种自适应巡航控制***性能测评方法
CN110307836B (zh) 一种用于无人清扫车辆贴边清扫的精确定位方法
CN112505737B (zh) 一种gnss/ins组合导航方法
CN111426320B (zh) 一种基于图像匹配/惯导/里程计的车辆自主导航方法
CN104198765A (zh) 车辆运动加速度检测的坐标系转换方法
CN113063429B (zh) 一种自适应车载组合导航定位方法
CN108362288A (zh) 一种基于无迹卡尔曼滤波的偏振光slam方法
CN110702113B (zh) 基于mems传感器的捷联惯导***数据预处理和姿态解算的方法
CN107600073B (zh) 一种基于多源信息融合的车辆质心侧偏角估计***及方法
CN113340298B (zh) 一种惯导和双天线gnss外参标定方法
CN108387236A (zh) 一种基于扩展卡尔曼滤波的偏振光slam方法
CN114966629A (zh) 一种基于ekf算法框架的车体激光雷达外参标定方法
CN111751857A (zh) 一种车辆位姿的估算方法、装置、存储介质及***
CN103123487B (zh) 一种航天器姿态确定方法
CN114440881B (zh) 一种融合多源传感器信息的无人车定位方法
CN113008229B (zh) 一种基于低成本车载传感器的分布式自主组合导航方法
CN114935345A (zh) 一种基于模式识别的车载惯导安装角误差补偿方法
CN108507587B (zh) 一种基于坐标变换的三维车载定位导航方法
CN113029173A (zh) 车辆导航方法及装置

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