CN117268409A - 基于迭代扩展卡尔曼滤波的无人车局部定位方法 - Google Patents
基于迭代扩展卡尔曼滤波的无人车局部定位方法 Download PDFInfo
- Publication number
- CN117268409A CN117268409A CN202311107923.2A CN202311107923A CN117268409A CN 117268409 A CN117268409 A CN 117268409A CN 202311107923 A CN202311107923 A CN 202311107923A CN 117268409 A CN117268409 A CN 117268409A
- Authority
- CN
- China
- Prior art keywords
- value
- unmanned vehicle
- moment
- state
- 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.)
- Pending
Links
- 238000000034 method Methods 0.000 title claims abstract description 23
- 238000001914 filtration Methods 0.000 title claims abstract description 11
- 239000011159 matrix material Substances 0.000 claims abstract description 27
- 238000005259 measurement Methods 0.000 claims description 16
- 230000017105 transposition Effects 0.000 claims description 3
- 238000004364 calculation method Methods 0.000 description 2
- 230000009286 beneficial effect Effects 0.000 description 1
- 230000004927 fusion Effects 0.000 description 1
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/26—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 specially adapted for navigation in a road network
- G01C21/28—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 specially adapted for navigation in a road network with correlation of data from several navigational instruments
- G01C21/30—Map- or contour-matching
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06F—ELECTRIC DIGITAL DATA PROCESSING
- G06F17/00—Digital computing or data processing equipment or methods, specially adapted for specific functions
- G06F17/10—Complex mathematical operations
- G06F17/11—Complex mathematical operations for solving equations, e.g. nonlinear equations, general mathematical optimization problems
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06F—ELECTRIC DIGITAL DATA PROCESSING
- G06F17/00—Digital computing or data processing equipment or methods, specially adapted for specific functions
- G06F17/10—Complex mathematical operations
- G06F17/16—Matrix or vector computation, e.g. matrix-matrix or matrix-vector multiplication, matrix factorization
Landscapes
- Engineering & Computer Science (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Mathematical Physics (AREA)
- Data Mining & Analysis (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Theoretical Computer Science (AREA)
- Computational Mathematics (AREA)
- Mathematical Analysis (AREA)
- Mathematical Optimization (AREA)
- Pure & Applied Mathematics (AREA)
- Software Systems (AREA)
- Algebra (AREA)
- Databases & Information Systems (AREA)
- General Engineering & Computer Science (AREA)
- Operations Research (AREA)
- Automation & Control Theory (AREA)
- Computing Systems (AREA)
- Navigation (AREA)
Abstract
本发明公开了一种基于迭代扩展卡尔曼滤波的无人车局部定位方法，包括：S1，初始化车辆位置值与协方差矩阵；S2，获得传感器测量值并建立状态估计模型；S3，计算雅可比矩阵；S4，判断k时刻是否有测量值，如无，直接输出所述状态一步预测，给到规控模块并返回S3；如有，执行S5；S5，根据所述测量值进行迭代，计算卡尔曼增益矩阵；S6，进行状态更新；S7，判断S6得到的状态估计值是否满足预设的精度或是否超出迭代步数，如否，则返回S5进行迭代；如是，则该值为k时刻的最优状态估计值，输出该最优状态估计值给到规控模块，同时返回S3，并令k＝k+1。该方法提高了无人车局部定位的精度，其收敛速度更快、误差更小。
Description
技术领域
本发明涉及无人车技术领域，尤其涉及一种基于迭代扩展卡尔曼滤波的无人车局部定位方法。
背景技术
在无人车的局部定位中，通常是将激光雷达的数据和IMU的预积分数据通过扩展卡尔曼滤波算法进行数据融合，从而得到无人车的精确位置。但是，由于扩展卡尔曼滤波算法的主要原理是在估计值的均值附近进行泰勒一阶近似，忽略了高阶项，因此，使用扩展卡尔曼滤波在局部定位中存在估计精度较低的问题。
发明内容
为解决现有技术存在的问题，本发明提供一种精度较高的基于迭代扩展卡尔曼滤波的无人车局部定位方法。
为此，本发明采用以下技术方案：
一种基于迭代扩展卡尔曼滤波的无人车局部定位方法，包括以下步骤：
S1，初始化车辆位置值与协方差矩阵：
给待估计值和协方差矩阵/>赋初值，所述待估计值/>为无人车的位置；
S2，获得传感器测量值并建立状态估计模型：
实时获取无人车的位置数据；同时将该位置数据与先验地图的位置数据进行匹配，得到先验地图上的无人车位置；
建立状态估计模型，所述状态估计模型由一个运动方程和一个观测方程构成：
式中，f(·)为运动模型；g(·)为观测模型；xk为无人车在k时刻的位置，xk-1为无人车在k-1时刻的位置；yk为k时刻的测量值；vk为k时刻的过程噪声；wk为k时刻的运动噪声；nk为k时刻的测量噪声；
S3，计算雅可比矩阵：
其中，为k时刻状态一步预测，Bk为k时刻线性化后的雅可比矩阵；
观测模型和雅可比矩阵分别为：
其中表示k时刻的一步测量预测，Gk表示k时刻对测量预测的雅可比矩阵；
S4，判断k时刻是否有测量值，如没有，则直接输出所述状态一步预测给到规控模块并返回S3；如有测量值，则执行步骤S5；
S5，根据所述测量值进行迭代，计算卡尔曼增益矩阵：
其中Kk为k时刻的卡尔曼滤波增益，为k时刻的协方差矩阵；上标T表示矩阵转置；R是测量噪声的方差；
S6，进行状态更新，有：
其中，为k+1时刻的状态估计值；
S7，判断S6得到的状态估计值是否满足预设的精度或是否超出迭代步数，如果否，则返回S5进行迭代；如果是，则该值为k时刻的最优状态估计值，输出该最优状态估计值给到规控模块，同时返回S3，并令k＝k+1，进行下一时刻的位置预测。
优选的是，步骤S2中，通过将第一次得到的测量值求平均值，以此为参考来赋初值。
在步骤S2中，还可以根据对不同传感器的数据自定义一个权值来赋初值，使其更接近真实值。
在步骤S2中，假设nk、wk和vk均满足零均值的高斯分布。
本发明在扩展卡尔曼滤波算法框架基础上，对无人车状态的一步预测进行判断，如果状态预测值符合要求，即在阈值的范围内或者已经达到最大迭代步数，则退出迭代计算，将最终迭代计算结果作为滤波器的状态预测值。
与现有技术相比，本发明具有以下有益效果：
1.该方法减小了无人车状态估计值的误差、提高了无人车局部定位的估计精度，具有二阶泰勒近似精度；
2.该方法增加了迭代计算，提高了精度，因此增强了局部定位的鲁棒性；
3.与现有方法相比，本发明的方法收敛速度更快、误差更小。
附图说明
图1为本发明的基于迭代扩展卡尔曼滤波的无人车局部定位方法流程图。
具体实施方式
以下结合附图和实施例对本发明的技术方案进行详细说明。
参见图1，本发明基于迭代扩展卡尔曼滤波的无人车局部定位方法包括以下步骤：
S1，初始化状态值与协方差矩阵：
给待估计值和协方差矩阵/>赋初值。其中，所述待估计值/>为无人车的位置。所述协方差矩阵/>用于无人车位置数据的相关性。
为了减少工作量，所赋初值越接近真实值越好。可以通过将第一次得到的测量值求平均值，以此为参考赋初值；也可以根据对不同传感器的数据自定义一个权值来赋初值，使其更接近真实值。
S2，获得传感器测量值并建立状态估计模型：
实时获取无人车的位置数据；同时将该位置数据与先验地图的位置数据进行匹配，得到先验地图上的无人车位置；
建立状态估计模型。所述状态估计模型由一个运动方程和一个观测方程构成，如式：
式中，f(·)为运动模型；g(·)为观测模型；xk为无人车在k时刻的位置，xk-1为无人车在k-1时刻的位置；yk为k时刻测量值(包括实时获取无人车的位置数据和先验地图上的无人车位置)；vk为k时刻的过程噪声；wk为k时刻的运动噪声；nk为k时刻的测量噪声；
通常假设nk、wk和vk都满足零均值的高斯分布。
S3，计算雅可比矩阵：
其中，为k时刻状态一步预测，Bk为k时刻线性化后的雅可比矩阵。
观测模型和雅可比矩阵可以写成：
其中表示k时刻的一步测量预测，Gk表示k时刻对测量预测的雅可比矩阵。
S4，判断k时刻是否有测量值，如没有，则直接输出所述状态一步预测如有测量值，则执行步骤S4；
S5，根据所述测量值进行迭代，计算卡尔曼增益矩阵：
其中Kk为k时刻的卡尔曼滤波增益，为k时刻的协方差矩阵；上标T表示矩阵转置；R是测量噪声的方差。
S6，进行状态更新：
其中，为k+1时刻的状态估计值。
S7，判断S6得到的状态估计值是否满足预设的精度或是否超出迭代步数，如果否，则返回S5进行迭代；如果是，则该值为k时刻的最优状态估计值，输出该最优状态估计值给到规控模块，同时返回S3，并令k＝k+1，进行下一时刻的位置预测。
Claims (4)
1.一种基于迭代扩展卡尔曼滤波的无人车局部定位方法，其特征在于，包括以下步骤：
S1，初始化车辆位置值与协方差矩阵：
给待估计值和协方差矩阵/>赋初值，所述待估计值/>为无人车的位置；
S2，获得传感器测量值并建立状态估计模型：
实时获取无人车的位置数据；同时将该位置数据与先验地图的位置数据进行匹配，得到先验地图上的无人车位置；
建立状态估计模型，所述状态估计模型由一个运动方程和一个观测方程构成：
式中，f(·)为运动模型；g(·)为观测模型；xk为无人车在k时刻的位置，xk-1为无人车在k-1时刻的位置；yk为k时刻的测量值；vk为k时刻的过程噪声；wk为k时刻的运动噪声；nk为k时刻的测量噪声；
S3，计算雅可比矩阵：
其中，为k时刻状态一步预测，Bk为k时刻线性化后的雅可比矩阵；
观测模型和雅可比矩阵分别为：
其中表示k时刻的一步测量预测，Gk表示k时刻对测量预测的雅可比矩阵；
S4，判断k时刻是否有测量值，如没有，则直接输出所述状态一步预测给到规控模块并返回S3；如有测量值，则执行步骤S5；
S5，根据所述测量值进行迭代，计算卡尔曼增益矩阵：
其中Kk为k时刻的卡尔曼滤波增益，为k时刻的协方差矩阵；上标T表示矩阵转置；R是测量噪声的方差；
S6，进行状态更新，有：
其中，为k+1时刻的状态估计值；
S7，判断S6得到的状态估计值是否满足预设的精度或是否超出迭代步数，如果否，则返回S5进行迭代；如果是，则该值为k时刻的最优状态估计值，输出该最优状态估计值给到规控模块，同时返回S3，并令k＝k+1，进行下一时刻的位置预测。
2.根据权利要求1所述的无人车局部定位方法，其特征在于：步骤S2中，通过将第一次得到的测量值求平均值，以此为参考来赋初值。
3.根据权利要求1所述的无人车局部定位方法，其特征在于：步骤S2中，根据对不同传感器的数据自定义一个权值来赋初值，使其更接近真实值。
4.根据权利要求1所述的无人车局部定位方法，其特征在于：步骤S2中，假设nk、wk和vk均满足零均值的高斯分布。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202311107923.2A CN117268409A (zh) | 2023-08-30 | 2023-08-30 | 基于迭代扩展卡尔曼滤波的无人车局部定位方法 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202311107923.2A CN117268409A (zh) | 2023-08-30 | 2023-08-30 | 基于迭代扩展卡尔曼滤波的无人车局部定位方法 |
Publications (1)
| Publication Number | Publication Date |
|---|---|
| CN117268409A true CN117268409A (zh) | 2023-12-22 |
Family
ID=89201716
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202311107923.2A Pending CN117268409A (zh) | 2023-08-30 | 2023-08-30 | 基于迭代扩展卡尔曼滤波的无人车局部定位方法 |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN117268409A (zh) |
Cited By (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN118655472A (zh) * | 2024-08-20 | 2024-09-17 | 宁德时代新能源科技股份有限公司 | 电池参数的估计方法、设备和存储介质 |
Citations (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN115033844A (zh) * | 2022-05-12 | 2022-09-09 | 合肥赛为智能有限公司 | 一种无人机状态估计方法、系统、设备及可读存储介质 |
| CN115951369A (zh) * | 2022-12-14 | 2023-04-11 | 畅加风行(苏州)智能科技有限公司 | 面向复杂港口环境的多传感器融合定位方法 |
-
2023
- 2023-08-30 CN CN202311107923.2A patent/CN117268409A/zh active Pending
Patent Citations (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN115033844A (zh) * | 2022-05-12 | 2022-09-09 | 合肥赛为智能有限公司 | 一种无人机状态估计方法、系统、设备及可读存储介质 |
| CN115951369A (zh) * | 2022-12-14 | 2023-04-11 | 畅加风行(苏州)智能科技有限公司 | 面向复杂港口环境的多传感器融合定位方法 |
Non-Patent Citations (3)
| Title |
|---|
| 强敏利: "IEKF滤波在移动机器人定位中的应用", 《测控技术与仪器仪表》, vol. 39, no. 2, 28 February 2013 (2013-02-28), pages 74 - 76 * |
| 王丽佳等: "《基于openCV的移动机器人目标跟踪实现》", 31 October 2021, 天津大学出版社, pages: 52 - 59 * |
| 龙慧;胡利;周宴宇;: "迭代卡尔曼滤波在机器人定位中的应用", 现代电子技术, no. 22, 15 November 2010 (2010-11-15) * |
Cited By (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN118655472A (zh) * | 2024-08-20 | 2024-09-17 | 宁德时代新能源科技股份有限公司 | 电池参数的估计方法、设备和存储介质 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN109186601A (zh) | 一种基于自适应无迹卡尔曼滤波的激光slam算法 | |
| CN105043388A (zh) | 基于惯性/重力匹配组合导航的向量搜索迭代匹配方法 | |
| CN111189454A (zh) | 基于秩卡尔曼滤波的无人车slam导航方法 | |
| CN113466890A (zh) | 基于关键特征提取的轻量化激光雷达惯性组合定位方法和系统 | |
| CN117268409A (zh) | 基于迭代扩展卡尔曼滤波的无人车局部定位方法 | |
| CN110895146A (zh) | 一种移动机器人同步定位与地图构建方法 | |
| CN115046540A (zh) | 一种点云地图构建方法、系统、设备和存储介质 | |
| CN108134640A (zh) | 一种基于节点运动状态约束的协作定位系统和方法 | |
| CN112034445B (zh) | 基于毫米波雷达的车辆运动轨迹跟踪方法和系统 | |
| CN108444479A (zh) | 基于自适应鲁棒无迹卡尔曼滤波的重力匹配方法 | |
| CN108226887B (zh) | 一种观测量短暂丢失情况下的水面目标救援状态估计方法 | |
| CN111256727B (zh) | 一种基于Augmented EKF的提高里程计精度的方法 | |
| CN114265047B (zh) | 一种大潜深auv的定位基阵联合标校方法 | |
| CN114637956B (zh) | 一种基于双卡尔曼滤波器实现目标位置预测的方法 | |
| CN113306573B (zh) | 一种自动驾驶车辆学习型路径跟踪预测控制方法 | |
| CN104331087B (zh) | 一种鲁棒的水下传感器网络目标跟踪方法 | |
| CN108680162B (zh) | 一种基于渐进无迹卡尔曼滤波的人体目标跟踪方法 | |
| CN109253727A (zh) | 一种基于改进迭代容积粒子滤波算法的定位方法 | |
| CN109655057B (zh) | 一种六推无人机加速器测量值的滤波优化方法及其系统 | |
| CN116680500B (zh) | 水下航行器在非高斯噪声干扰下的位置估计方法及系统 | |
| CN116127406B (zh) | 一种基于混合h∞自适应卡尔曼滤波的数据融合方法 | |
| CN115047505B (zh) | 基于载波相位差分辅助的gnss定位方法及导航方法 | |
| CN113932815B (zh) | 稳健性优化Kalman滤波相对导航方法、装置、设备和存储介质 | |
| CN114543799B (zh) | 一种抗差联邦卡尔曼滤波方法、设备与系统 | |
| CN110514209B (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 |