CN111750854A - 车辆定位方法、装置、系统及存储介质 - Google Patents
车辆定位方法、装置、系统及存储介质 Download PDFInfo
- Publication number
- CN111750854A CN111750854A CN202010713275.5A CN202010713275A CN111750854A CN 111750854 A CN111750854 A CN 111750854A CN 202010713275 A CN202010713275 A CN 202010713275A CN 111750854 A CN111750854 A CN 111750854A
- Authority
- CN
- China
- Prior art keywords
- vehicle
- vehicle positioning
- information
- particle
- positioning information
- 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
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
- G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
- G01S19/00—Satellite radio beacon positioning systems; Determining position, velocity or attitude using signals transmitted by such systems
- G01S19/38—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system
- G01S19/39—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system the satellite radio beacon positioning system transmitting time-stamped messages, e.g. GPS [Global Positioning System], GLONASS [Global Orbiting Navigation Satellite System] or GALILEO
- G01S19/40—Correcting position, velocity or attitude
- G01S19/41—Differential correction, e.g. DGPS [differential GPS]
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
- G01S19/00—Satellite radio beacon positioning systems; Determining position, velocity or attitude using signals transmitted by such systems
- G01S19/38—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system
- G01S19/39—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system the satellite radio beacon positioning system transmitting time-stamped messages, e.g. GPS [Global Positioning System], GLONASS [Global Orbiting Navigation Satellite System] or GALILEO
- G01S19/42—Determining position
- G01S19/48—Determining position by combining or switching between position solutions derived from the satellite radio beacon positioning system and position solutions derived from a further system
- G01S19/49—Determining position by combining or switching between position solutions derived from the satellite radio beacon positioning system and position solutions derived from a further system whereby the further system is an inertial position system, e.g. loosely-coupled
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06N—COMPUTING ARRANGEMENTS BASED ON SPECIFIC COMPUTATIONAL MODELS
- G06N3/00—Computing arrangements based on biological models
- G06N3/004—Artificial life, i.e. computing arrangements simulating life
- G06N3/006—Artificial life, i.e. computing arrangements simulating life based on simulated virtual individual or collective life forms, e.g. social simulations or particle swarm optimisation [PSO]
Landscapes
- Engineering & Computer Science (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Computer Networks & Wireless Communication (AREA)
- Theoretical Computer Science (AREA)
- Biophysics (AREA)
- General Health & Medical Sciences (AREA)
- Biomedical Technology (AREA)
- Life Sciences & Earth Sciences (AREA)
- Computational Linguistics (AREA)
- Data Mining & Analysis (AREA)
- Evolutionary Computation (AREA)
- Artificial Intelligence (AREA)
- Molecular Biology (AREA)
- Computing Systems (AREA)
- General Engineering & Computer Science (AREA)
- Mathematical Physics (AREA)
- Software Systems (AREA)
- Health & Medical Sciences (AREA)
- Automation & Control Theory (AREA)
- Navigation (AREA)
Abstract
本发明公开了一种车辆定位方法、装置、系统及存储介质,一种车辆定位方法,包括:获取设置于车辆中的RTK设备采集的车辆定位信息;获取设置于车辆中的IMU采集的车辆的惯性测量信息;将车辆定位信息和惯性测量信息进行数据融合处理,得到优化后的车辆定位信息。本发明实施例公开的车辆定位方法、装置、系统及存储介质,可以提高对于车辆的定位精度,为自动驾驶提供精确地车辆定位信息。
Description
技术领域
本发明实施例涉及自动驾驶技术,尤其涉及一种车辆定位方法、装置、系统及存储介质。
背景技术
随着人类文明的发展、科技的进步以及人类生活质量水平的提高,汽车也越来越成为人类生活中必不可少的工具。自动驾驶是新一轮汽车科技革命的下的新兴技术,代表着未来汽车技术的趋势,是汽车产业转型升级的关键。自动驾驶技术也开始逐渐成为热点,而车辆定位技术是自动驾驶最核心的技术之一。
目前的自动驾驶技术依靠激光及毫米波雷达、计算机视觉、全球定位系统等技术,自动捕获车辆四周的环境信息,并给与一定的程序使之对获取到的环境信息进行决策和路径规划,以此达到一个完全不需要人类操作的自动驾驶。
差分实时动态(Real-time Kinematic,RTK)是目前使用最广泛的高精度定位方法,在理想试验情况下定位精度可以达到2cm。但在实际车辆行驶过程中会存在信号遮挡、干扰、无法锁定等情况,此时定位结果可能会出现较大偏差。因此,为了保证车辆自动驾驶的安全性,如何提高车辆定位精度,是目前亟待解决的问题。
发明内容
本发明提供一种车辆定位方法、装置、系统及存储介质,可以提高对于车辆的定位精度,为自动驾驶提供精确地车辆定位信息。
第一方面,本发明实施例提供一种车辆定位方法,包括:
获取设置于车辆中的RTK设备采集的车辆定位信息;
获取设置于车辆中的IMU采集的车辆的惯性测量信息;
将车辆定位信息和惯性测量信息进行数据融合处理,得到优化后的车辆定位信息。
在第一方面一种可能的实现方式中,获取设置于车辆中的IMU采集的车辆的惯性测量信息之后,还包括:
基于RLS算法对惯性测量信息进行滤波处理,得到滤波后的惯性测量信息;
将车辆定位信息和惯性测量信息进行数据融合处理,得到优化后的车辆定位信息,包括:
将车辆定位信息和滤波后的惯性测量信息进行数据融合处理,得到优化后的车辆定位信息。
在第一方面一种可能的实现方式中,基于RLS算法对惯性测量信息进行滤波处理,得到滤波后的惯性测量信息,包括:
采用粒子群算法对RLS算法进行优化;
基于优化后的RLS算法对惯性测量信息进行滤波处理,得到滤波后的惯性测量信息。
在第一方面一种可能的实现方式中,采用粒子群算法对RLS算法进行优化,包括:
对粒子的位置和速度随机初始化;
计算每个粒子的适应值,将每个粒子的适应值作为历史最优值并将粒子的当前位置作为历史最好位置,同时,将所有粒子中的最好适应度作为粒子群的全局最优解;
更新所有粒子的位置和速度;
重新计算每个粒子的适应值,并将重新计算的每个粒子的适应值与历史最优值进行比较,如果重新计算的每个粒子的适应值超过粒子的历史最优值,则将粒子的当前位置更新为历史最优位置;
将每个粒子的历史最优值与粒子群的历史最优值进行比较,如果粒子的历史最优值超过群体的历史最优值,则将粒子的当前位置更新为该粒子的历史最优位置,并将所有粒子中的最好适应度作为粒子群当前的全局最优解;
若满足优化结束条件则输出结果,得到全局最优值,否则重新更新所有粒子的位置和速度。
在第一方面一种可能的实现方式中,将车辆定位信息和惯性测量信息进行数据融合处理,得到优化后的车辆定位信息,包括:
采用卡尔曼算法将车辆定位信息和惯性测量信息进行数据融合处理,得到优化后的车辆定位信息。
在第一方面一种可能的实现方式中,将车辆定位信息和惯性测量信息进行数据融合处理,得到优化后的车辆定位信息,包括:
对数据融合的误差纠正方程进行学习,得到学习结果;
若RTK设备的信号强度低于预设阈值,则将学习结果和惯性测量信息进行数据融合处理,得到优化后的车辆定位信息。
在第一方面一种可能的实现方式中,获取设置于车辆中的RTK设备采集的车辆定位信息,包括:
获取设置于车辆中的RTK设备接收的差分定位数据;
根据差分定位数据确定车辆定位信息。
第二方面,本发明实施例提供一种车辆定位装置,包括:
RTK信息获取模块,用于获取设置于车辆中的RTK设备采集的车辆定位信息;
惯性测量信息获取模块,用于获取设置于车辆中的IMU采集的车辆的惯性测量信息;
数据融合模块,用于将车辆定位信息和惯性测量信息进行数据融合处理,得到优化后的车辆定位信息。
第三方面,本发明实施例提供一种智能驾驶控制系统,包括:
一个或多个处理器;
存储器,用于存储一个或多个程序;
RTK设备;
IMU;
当一个或多个程序被一个或多个处理器执行,使得一个或多个处理器实现如第一方面任一种实现方式的车辆定位方法。
第四方面,本发明实施例提供一种计算机可读存储介质,其上存储有计算机程序,该程序被处理器执行时实现如第一方面任一种实现方式的车辆定位方法。
本发明实施例提供的车辆定位方法、装置、系统及存储介质,在获取设置于车辆中的RTK设备采集的车辆定位信息和获取设置于车辆中的IMU采集的车辆的惯性测量信息之后,将车辆定位信息和惯性测量信息进行数据融合处理,得到优化后的车辆定位信息,使得车辆定位信息出现较大误差时,能够采用惯性测量信息对车辆定位信息进行优化,从而提高了对车辆进行定位的精度,为自动驾驶提供精确地车辆定位信息。
附图说明
图1为本发明实施例提供的车辆定位方法的流程图;
图2为粒子群优化处理的流程示意图;
图3为本发明实施例提供的车辆定位方法的信息处理示意图;
图4为本发明实施例提供的车辆定位装置的结构示意图;
图5为本发明提供的车辆定位系统的结构示意图;
图6为本发明实施例提供的车辆定位系统安装于车辆中的示意图。
具体实施方式
下面结合附图和实施例对本发明作进一步的详细说明。可以理解的是,此处所描述的具体实施例仅仅用于解释本发明,而非对本发明的限定。另外还需要说明的是,为了便于描述,附图中仅示出了与本发明相关的部分而非全部结构。
图1为本发明实施例提供的车辆定位方法的流程图,如图1所示,本实施例提供的车辆定位方法包括:
步骤S101,获取设置于车辆中的RTK设备采集的车辆定位信息。
本实施例提供的车辆定位方法应用于任一种车辆中,其中车辆上需要设置RTK设备和惯性测量单元(Inertial Measurement Unit,IMU)。
其中,RTK设备和IMU均设置于车辆中。RTK设备用于采集车辆的定位信息。RTK是基于载波相位观测值的实时动态定位技术,它能够实时地提供测站点在指定坐标系中的三维定位结果。RTK设备包括两个RTK天线,可以将两个RTK天线设置于车辆的顶部,且两个RTK天线间隔预设距离,例如间隔超过1米。RTK设备能够输出车辆的定位信息,车辆的定位信息例如是三维坐标数据。通过设置于车辆中的RTK设备可以获取车辆的定位信息。
RTK设备接收的数据可以为差分定位数据,根据差分定位数据确定车辆定位信息。
例如,在车辆中设置两个智能实时差分仪,其中,一个智能实时差分仪可以安装在车头处,另一个智能实时差分仪可以安装在车尾处,具体两个智能实时差分仪可以安装在左右的终点处的顶部,且相距不小于1m。车载测试的卫星数据部分采用差分数据,差分技术采用了RTK载波相位差分技术,该种技术能够实时地提供测站点在指定坐标系中的三维定位结果,并达到厘米级精度,在RTK作业模式下,基准站通过数据链将其观测值和测站坐标信息一起传送给流动站,流动站不仅通过数据链接受来自基准站的数据,还要采集GPS观测数据,并在系统内组成差分观测值进行实时处理,同时给出厘米级定位结果,历时不到一秒钟。流动站可处于静止状态,也可以处于运动状态;可在固定点上线进行初始化后再进入动态作业,也可在动态条件下直接开机,并在动态环境下完成周模糊度的搜索求解。在整周未知数解固定后,即可进行每个历元的实时处理,只要能保持四颗星以上卫星相位观测值的跟踪和必要的几何图形,则流动站可随时给出厘米级定位结果。RTK技术的关键在于数据处理技术和数据传输技术,RTK定位时,要求基准站接收机实时的把观测数据及一直数据传输给流动站接收机。两个智能实时差分仪可以等同于两个移动基站,两个移动基站分别接受、观测卫星导航信号并进行差分处理,生成差分数据。移动基站1接受到来自移动基站2的差分数据并进行解读,从而获得实时的、动态的、厘米级定位精度。
步骤S102,获取设置于车辆中的IMU采集的车辆的惯性测量信息。
IMU例如安装在车辆的重心处,IMU例如包含三个单轴的加速度计和三个单轴的陀螺仪,加速度计检测车辆在载体坐标体系独立三轴的加速度信号,而陀螺检测载体相对于导航坐标系的角速度信号,丈量物体在三维空间中的角速度和加速度,并以此解算出车辆的姿势。为了进步可靠性,还可以在每个轴配备更多的传感器。IMU惯性测量设备归于捷联式惯导,该体系有两个加速度传感器与三个方向的角速率传感器(陀螺仪)组成。以当地水平指北体系为例,惯性渠道始终保持地平坐标系,设备在渠道上的3个相互正交的加速度计别离测出沿东西、南北和笔直方向的加速度重量,并输入处理器。在消除加速度计差错、重力加速度和因为地球自转发生的科里奥利加速度影响后,得出运载体相对地平坐标系的位移加速度重量,再就t(从起始点到待测点的时刻)进行两次积分,并考虑初始速度值,就可解算出相对前一起始点的坐标改变量,同相应起始点的经度、纬度和高程累加,就得到待定点的坐标。通过设置于车辆中的IMU可以获取车辆的惯性测量信息,车辆的惯性测量信息例如是车辆的三轴姿态角以及加速度。
进一步地,在获取设置于车辆中的IMU采集的车辆的惯性测量信息之后,还包括:基于递归最小二乘(Recursive Least Squares,RLS)算法对惯性测量信息进行滤波处理,得到滤波后的惯性测量信息。其中,RLS算法包括:面对增益向量中的矩阵,利用无损变换(Unscented Transformation,UT)和输出变量,得到它们的均值和协方差,最终获得RLS算法的非线性系统模型。
RLS算法进行滤波的基本原理就是利用n时刻(指当前时刻)得到的新的观测数据,在已知或已计算得到n-1时刻(指前一时刻)的滤波器权向量的最小二乘估计w-1的情况下,用迭代的方法计算出n时刻的滤波器权向量的最小二乘估计值w。而所谓自适应实现是指利用前一时刻获得的滤波器参数,根据估计误差自动调节现时刻的参数,使得某个代价函数达到最小,从而实现最优滤波。即观察一个平稳输入信号输入的自适应系数在一点时间内输出误差信号的平均功率(时间平均),将该平均功率是否为最小作为测量自适应系统是否最佳的准则。RLS估计从每次获得的测量值中提取出被估计的信息,用于修正上一步所得的估计,量测次数越多,修正次数越多,估计精度也就越高。
现有的RLS算法输入信号、计算误差以及更新滤波器权系数的公式如下:
1、数据初始化:w(0)=0,R(0)=σI
2、滤波处理:y(n)=wT(n-1)x(n)
4、估计误差:e(n)=d(n)-wT(n-1)x(n)
5、更新权向量:w(n)=w(n-1)+k(n)e(n)
6、更新协方差:p(n)=[p(n-1)-k(n)w(n)p(n-1)]/λ
上述6个公式当中,w(n)是滤波器在n时刻的权重向量,d(n)表示滤波器输入的量测信号,y(n)表示滤波后的输出信号量,x(n)表示RLS算法的估计量,e(n)表示滤波前后的信号误差量,λ则为滤波器的遗忘因子。
RLS算法开始之前,首先要进行第一步,对数据进行初始化处理,σ一般取0.001或自己拟定的较小值,因为在没有得到确定的信噪比的情况下,均默认将需要滤波的信号量信噪比处于较高,也就是希望相关矩阵初始值R(0)在R(n)中占非常小比重,甚至能忽略不计,即:
上式中,x(i)表示第i时刻的RLS算法的估计量。
遗忘因子λ是误差测度函数中的加权因子,引入它的目的是为了赋予原来数据与新数据以不同的权值,以使该算法具有对输入过程特性变化的快速反应能力。所以,一般的改进就是在RLS算法的代价函数当中引入遗忘因子,再对遗忘因子进行指数加权处理,RSL算法的最终求取准则为指数加权下的误差平方和最小即可,从而实现最优滤波目的,代价函数即如下式:
J(n)=∑λn-i|e(i)|2
其中,λn-i表示第n-1时刻的滤波器遗忘因子;e(i)表示第i时刻的滤波器输入前后的信号误差量;λ为遗忘因子,取值为0<λ<1。引入遗忘因子作用是离n时刻近的误差附较大权重,离n时刻远的误差赋较小权重,确保在过去某一段时间的观测数据被“遗忘”,从而使滤波器可以工作在非平稳状态下确定环境当中。
新误差平方的系数是1,而前面几次的误差平方的系数是λ的相应次幂,λ是个小于1的数,如果λ越小,能量信号就越接近最新的误差平方,对前面的误差遗忘的越快,跟踪效果就越好。但是,递推RLS算法中的误差是由期望信号决定的,如果λ很小则误差信号对期望信号的依赖性就会很大,所以,输出信号就很接近期望信号,这就导致了如果期望信号是错误的,那么输出信号也就错误,从而就起不到滤波器的效果,所以现有技术中也有要求0.95≤λ≤0.995。但是,在实际的非平稳环境当中,不仅有误差还具有野值,所以对于遗忘因子的要求是随机的加权和可变,希望在非平稳环境下λ的值可以足够的小,这样可以对非平稳环境更加精确的适应,对于有限的最近的时刻的误差或是野值进行遗忘,使得算法在非平稳环境下对于每一个趋势都有所跟踪、适应。同时,对于平稳环境则不需要较小的遗忘因子,希望λ能够根据环境增长到一个合适的值,以此减小参数的估计误差并且提高算法的运算速度。因此,考虑引入随机加权的可变遗忘因子以加强对于复杂非平稳环境的自适应程度:
F(n)=-[ρε(n)2]
其中,ρ代表了敏感增益系数,用来控制λ接近1或者减小到足够小的速率,随机加权的系数向下取整。随机加权方法的最大的优点就是具有较大的自适应性,如此便不再需要假定ε(n)的分布具有某种形式;同时还可以有效地提高对于误差的逼近,对野值也增加了较好的抵抗性。由上述过程可以看出,野值的限制并非以往的方法,判断出来后就将其直接剔除,这样做会损失滤波过程当中的平滑性,甚至有可能出现数据断层从而失去对于滤波系统的驱动作用,所以改进后的算法是通过上式来实现对于数据的一个自适应修正。
所以,更新好的抗野值的RLS自适应滤波的代价方程如下:
J(n)=∑λ(i)|e(i)|2
其中,λ(i)为上述讨论的随机加权的可变的遗忘因子,同时,更新k(n)和更新P(n)的遗忘因子也要进行相应的改变。
自适应滤波是在维纳滤波,Kalman滤波等现行滤波基础上发展起来的一种最有滤波方法;如果数据输出的运行环境的特性不变,自适应算法滤波器会找到最佳的自由参数和参数集,并在滤波器性能最优的时候停止参数的调整,一般将该过程称为参数捕获过程;如果数据输出的运行环境会随时间而变化,自适应滤波器在发现变化会重新调整自身的参数,以适应参数使自身的滤波性能继续最优化,一般讲该过程称为性能跟踪过程。
进一步地,本发明实施例中在RLS算法的基础上对其加入了粒子群优化算法,自适应调整遗忘因子,同时也增强了RLS算法自身的适应性。
粒子群算法(Particle Swarm Optimization,PSO)是群智能优化算法的一种,具有原理简单、初始参数较少、容易实现、前期收敛速度快等优点。但是PSO算法中粒子主要依靠个体经验和种群经验来调节其飞行方向,后期种群逐渐呈现同一性,在求解复杂函数优化问题时会出现后期收敛速度缓慢,且易陷入局部最优等问题。大多学者对PSO算法进行改进研究,并且改进后的算法常被用来解决工程中的优化问题。例如,文献的改进粒子群算法采用重组、种群规模调整以及探测三个模块对算法进行改进,有效提高了算法的探测和开采能力。粒子群算法也算一种并行算法,在车辆组合定位过程中加入它可以在优化实验结果的同时有极大的实效性。
遗忘因子是误差测度函数中的加权因子,引入它的目的是为了赋予原来数据与新数据以不同的权值,以使该算法具有对输入过程特性变化的快速反应能力。一般情况下,在平稳环境中希望λ较大,可以较好地保持之前的对误差的处理效果;而在非平稳的环境中,则希望λ较小,针对有限区域内或最佳时刻的误差以此来更加精准的适应非平稳环境,使得算法在非平稳环境下对信号的每一个趋势都有所跟踪、适应。希望在非平稳环境下λ的值可以足够的小,这样可以对非平稳环境更加精确的适应,对于有限的最近的时刻的误差或是野值进行遗忘,野值表示测量过程中由于一些不明原因所造成的错误测量值。使得算法在非平稳环境下对于每一个趋势都有所跟踪、适应。同时,对于平稳环境则不需要较小的遗忘因子,希望λ能够根据环境增长到一个合适的值,以此减小参数的估计误差并且提高算法的运算速度。
因为遗忘因子λ对整个系统的性能起很大的作用,所以可以考虑对λ进行修改,使它既能使系统的跟踪能力加强,又能使系统收敛时估计误差小,于是考虑到采用变遗忘因子的思想,当误差变小时,λ就接近1,这样会使参数的误差减小;反之当误差变大时,λ就变小,我们将它设一个最小值λmin,这样会使系统跟踪能力加强。
所以,在平缓环境不需要较小的遗忘因子,希望λ可以根据环境增长到一个合适的值来减小参数的估计误差。
粒子群优化算法中,加速因子设置为2.0,伪随机数采用随机函数rand(),范围是[0,1],粒子能达到的最大速度为10,结束条件为优化代数,此优化代数是动态代数,每个数与结束条件对应的代数一一对应,粒子群优化流程如图2所示,图2为粒子群优化处理的流程示意图,如图2所示,粒子群优化包括如下步骤
1、对粒子的位置和速度随机初始化;
2、计算每个粒子的适应值,将每个粒子的适应值作为历史最优值并将粒子的当前位置作为历史最好位置,同时,将所有粒子中的最好适应度作为粒子群的全局最优解;
3、更新每个粒子的位置和速度;
4、重新计算每个粒子的适应值,并将重新计算的每个粒子的适应值与历史最优值进行比较,如果重新计算的每个粒子的适应值超过粒子的历史最优值,则将粒子的当前位置更新为历史最优位置;
5、将每个粒子的历史最优值与粒子群的历史最优值进行比较,如果粒子的历史最优值超过群体的历史最优值,则将粒子的当前位置更新为该粒子的历史最优位置,并将所有粒子中的最好适应度作为粒子群当前的全局最优解;
6、若满足优化结束条件则输出结果,得到全局最优值,否则重新更新所有粒子的位置和速度,即返回步骤3。
vi=vi+c1×rand()×(pbesti-xi)+c2×rand()×(gbesti-xi)
xi=xi+vi
其中,i=1,2,...,N,N是此群中粒子的总数;vi是粒子的速度;rand()是介于[0,1]的随机数;xi是粒子当前的位置;c1和c2是学习因子,通常c1=c2=2.0;vi的最大值为Vmax,即此时粒子的速度最大值为Vmax,且Vmax必须大于0,如果vi大于Vmax,则vi=Vmax;pbesti为粒子i个体经历过的最好的位置;gbesti为种群所经历过的最好位置。
针对增益向量中的矩阵P(n),增强RLS算法自适应度的能力。首先基于当前状态x的均值和方差Px,通过采样,得到一组固定数目的点集{xi},其相对应的权重分别为{Wn i};然后根据这组采样点其中的样本均值和样本方差非线性变换后加权处理,最后得到输出变量y的均值如下:
上式中,Wn (i)表示第i时刻对应点集的权重;
上式中,Wc (i)表示在非线性变换中第i时刻对应点集的权重;
x(n)=x(n-1)+V(n|n-1)
其中,V(n|n-1)表示,条件为n-1时刻的一步预测矩阵和n时刻的一步预测矩阵的条件概率模型下的测量修正矩阵,w(n-1)表示针对n-1时刻输出量的权重向量,W(n-1)表示n-1时刻权重矩阵;h表示观测系统的参数矩阵。
步骤S103,将车辆定位信息和惯性测量信息进行数据融合处理,得到优化后的车辆定位信息。
由于RTK设备采集的车辆定位信息是基于卫星数据计算的,而车辆行驶过程中会存在信号遮挡、干扰、无法锁定等情况,因此根据RTK设备采集的车辆定位信息作为车辆的定位结果可能会出现较大偏差。同时,卫星数据输出频率一般不超过20Hz,无法满足精确车辆控制精度的要求。
因此,本实施例针对上述问题,在车辆中加入IMU,使用IMU采集的车辆的惯性测量信息与RTK设备采集的车辆定位信息进行数据融合,从而对车辆定位信息进行优化,从而提高车辆定位的精度。将车辆定位信息和惯性测量信息进行数据融合处理,实际就是使用IMU采集的车辆的惯性测量信息对RTK设备采集的车辆定位信息进行修正。由于IMU采集的车辆的惯性测量信息表示了车辆的姿态和加速度,那么可以根据惯性测量信息计算出车辆的运动趋势。再结合RTK设备采集的车辆定位信息,当RTK设备由于遮挡等原因导致在某一时刻无法正确接收卫星数据时,即可根据惯性测量信息和上一时刻的车辆定位信息推算出当前时刻的车辆定位信息。或者当RTK设备由于干扰等原因导致在某一时刻采集的车辆定位信息误差较大,不符合根据惯性测量信息推算出的车辆运动趋势时,即可根据惯性测量信息和上一时刻的车辆定位信息推算出当前时刻的车辆定位信息。
在将车辆定位信息和惯性测量信息进行数据融合处理后,得到的优化后的车辆定位信息将比仅根据RTK设备采集的车辆定位信息的精度更高,且在RTK设备由于遮挡、干扰的原因导致车辆定位信息出现较大偏差时,仍然能够获取精确地车辆定位信息,从而提高了车辆定位的精度,同时,IMU采集的惯性测量数据还弥补了RTK设备采集的车辆定位信息更新频率较低的缺陷。另一方面,RTK设备采集的车辆定位信息又能纠正IMU采集的惯性测量信息的运动误差。
进一步地,可以采用卡尔曼算法将车辆定位信息和惯性测量信息进行数据融合处理,得到优化后的车辆定位信息。这样可以提高数据的输出频率值100Hz,可以平滑定向结果。
优选地,将车辆定位信息和惯性测量信息进行数据融合处理,包括:对数据融合的误差纠正方程进行学习,得到学习结果;若RTK设备的信号强度低于预设阈值,则将学习结果和惯性测量信息进行数据融合处理,得到优化后的车辆定位信息。也就是采用学习的方法,对数据融合的误差进行学习,从而当RTK设备的信号强度低于预设阈值时,根据学习结果和惯性测量信息进行数据融合处理。
本实施例提供的车辆定位方法,在获取设置于车辆中的RTK设备采集的车辆定位信息和获取设置于车辆中的IMU采集的车辆的惯性测量信息之后,将车辆定位信息和惯性测量信息进行数据融合处理,得到优化后的车辆定位信息,使得车辆定位信息出现较大误差时,能够采用惯性测量信息对车辆定位信息进行优化,从而提高了对车辆进行定位的精度,为自动驾驶提供精确地车辆定位信息。
图3为本发明实施例提供的车辆定位方法的信息处理示意图,图3中以RTK设备接收卫星信息,且卫星信息为差分数据为例。如图3所示,首先同时通过RTK设备和IMU获取卫星信息和IMU信息(即通过IMU获取的惯性测量信息),对获取的卫星信息进行信号检测,若接收的卫星信息的信号弱(信号强度低于预设阈值),则判断卫星信息不宜作为融合数据,那么就可以对数据融合的误差纠正方程进行学习,得到学习结果,并将学习结果和IMU信息进行数据融合处理。若接收的卫星信息的信号正常(信号强度高于或等于预设阈值),那么确定卫星信息对应的RTK差分结果,并与IMU信息进行数据融合,最终实现定位信息的优化。
图4为本发明实施例提供的车辆定位装置的结构示意图,如图4所示,本实施例提供的车辆定位装置包括:
RTK信息获取模块41,用于获取设置于车辆中的RTK设备采集的车辆定位信息;惯性测量信息获取模块42,用于获取设置于车辆中的IMU采集的车辆的惯性测量信息;数据融合模块43,用于将车辆定位信息和惯性测量信息进行数据融合处理,得到优化后的车辆定位信息。
本实施例提供的车辆定位装置用于实现图1所示车辆定位方法的技术方案,其实现原理和技术效果类似,此处不再赘述。
进一步地,在图4所示实施例的基础上,车辆定位装置还包括:滤波模块,用于基于RLS算法对惯性测量信息进行滤波处理,得到滤波后的惯性测量信息;数据融合模块43,具体用于将车辆定位信息和滤波后的惯性测量信息进行数据融合处理,得到优化后的车辆定位信息。
进一步地,在图4所示实施例的基础上,滤波模块具体用于采用粒子群算法对RLS算法进行优化;基于优化后的RLS算法对惯性测量信息进行滤波处理,得到滤波后的惯性测量信息。
进一步地,在图4所示实施例的基础上,滤波模块具体用于对粒子的位置和速度随机初始化;计算每个粒子的适应值,将每个粒子的适应值作为历史最优值并将粒子的当前位置作为历史最好位置,同时,将所有粒子中的最好适应度作为粒子群的全局最优解;更新所有粒子的位置和速度;重新计算每个粒子的适应值,并将重新计算的每个粒子的适应值与历史最优值进行比较,如果重新计算的每个粒子的适应值超过粒子的历史最优值,则将粒子的当前位置更新为历史最优位置;将每个粒子的历史最优值与粒子群的历史最优值进行比较,如果粒子的历史最优值超过群体的历史最优值,则将粒子的当前位置更新为该粒子的历史最优位置,并将所有粒子中的最好适应度作为粒子群当前的全局最优解;若满足优化结束条件则输出结果,得到全局最优值,否则重新更新所有粒子的位置和速度。
进一步地,在图4所示实施例的基础上,数据融合模块43,具体用于采用卡尔曼算法将车辆定位信息和惯性测量信息进行数据融合处理,得到优化后的车辆定位信息。
进一步地,在图4所示实施例的基础上,数据融合模块43,具体用于对数据融合的误差纠正方程进行学习,得到学习结果;若RTK设备的信号强度低于预设阈值,则将学习结果和惯性测量信息进行数据融合处理,得到优化后的车辆定位信息。
进一步地,在图4所示实施例的基础上,RTK信息获取模块41,具体用于获取设置于车辆中的RTK设备接收的差分定位数据;根据差分定位数据确定车辆定位信息。
图5为本发明提供的车辆定位系统的结构示意图。如图5所示,该车辆定位系统包括处理器51、存储器52、RTK设备53、IMU 54。该车辆定位系统中处理器51的数量可以是一个或多个,图5中以一个处理器51为例;该车辆定位系统的处理器51、存储器52、RTK设备53以及IMU 54可以通过总线或其他方式连接,图5中以通过总线连接为例。
存储器52作为一种计算机可读存储介质,可用于存储软件程序、计算机可执行程序以及模块,如本发明实施例中的车辆定位方法对应的程序指令以及模块(例如,车辆装置中的RTK信息获取模块41、惯性测量信息获取模块42、数据融合模块43)。处理器51通过运行存储在存储器52中的软件程序、指令以及模块,从而执行车辆定位系统的各种功能应用以及数据处理,即实现上述的车辆定位方法。
存储器52可主要包括存储程序区和存储数据区,其中,存储程序区可存储操作系统、至少一个功能所需的应用程序;存储数据区可存储根据车辆定位系统的使用所创建的数据等。此外,存储器52可以包括高速随机存取存储器,还可以包括非易失性存储器,例如至少一个磁盘存储器件、闪存器件、或其他非易失性固态存储器件。在一些实施例中,存储器52可进一步包括相对于处理器51远程设置的存储器,这些远程存储器可以通过网络连接至车辆定位系统。上述网络的实施例包括但不限于互联网、企业内部网、局域网、移动通信网及其组合。
RTK设备53为任一种能够获取RTK信号的设备,IMU 54为任一种能够进行惯性测量的设备。
图6为本发明实施例提供的车辆定位系统安装于车辆中的示意图,图6中仅示出车辆定位系统的RTK设备和IMU在车辆中的安装示意图。如图6所示,在车辆60中,安装有IMU61,IMU 61安装于车辆60的重心处,车辆60中还安装有RTK设备62,RTK设备62可以为一个集成的设备,RTK设备62的两个天线分别设置于车辆60顶部的前后两端。或者RTK设备62还可以为两个独立的设备,每个RTK设备62例如为一个智能实时差分仪,两个智能实时差分仪协同工作,共同实现RTK设备62所需实现的功能。
另外,本发明实施例还提供一种车辆,该车辆包括如图5所示的车辆定位系统。
本发明还提供一种包含计算机可执行指令的存储介质,计算机可执行指令在由计算机处理器执行时用于执行一种车辆定位方法,该方法包括:
获取设置于车辆中的RTK设备采集的车辆定位信息;获取设置于车辆中的IMU采集的车辆的惯性测量信息;将车辆定位信息和惯性测量信息进行数据融合处理,得到优化后的车辆定位信息。
注意,上述仅为本发明的较佳实施例及所运用技术原理。本领域技术人员会理解,本发明不限于这里所述的特定实施例,对本领域技术人员来说能够进行各种明显的变化、重新调整和替代而不会脱离本发明的保护范围。因此,虽然通过以上实施例对本发明进行了较为详细的说明,但是本发明不仅仅限于以上实施例,在不脱离本发明构思的情况下,还可以包括更多其他等效实施例,而本发明的范围由所附的权利要求范围决定。
Claims (10)
1.一种车辆定位方法,其特征在于,包括:
获取设置于车辆中的实时动态RTK设备采集的车辆定位信息;
获取设置于车辆中的惯性测量单元IMU采集的车辆的惯性测量信息;
将所述车辆定位信息和所述惯性测量信息进行数据融合处理,得到优化后的车辆定位信息。
2.根据权利要求1所述的方法,其特征在于,所述获取设置于车辆中的IMU采集的车辆的惯性测量信息之后,还包括:
基于递归最小二乘RLS算法对所述惯性测量信息进行滤波处理,得到滤波后的惯性测量信息;
所述将所述车辆定位信息和所述惯性测量信息进行数据融合处理,得到优化后的车辆定位信息,包括:
将所述车辆定位信息和所述滤波后的惯性测量信息进行数据融合处理,得到优化后的车辆定位信息。
3.根据权利要求2所述的方法,其特征在于,所述基于RLS算法对所述惯性测量信息进行滤波处理,得到滤波后的惯性测量信息,包括:
采用粒子群算法对所述RLS算法进行优化;
基于优化后的RLS算法对所述惯性测量信息进行滤波处理,得到滤波后的惯性测量信息。
4.根据权利要求3所述的方法,其特征在于,所述采用粒子群算法对所述RLS算法进行优化,包括:
对粒子的位置和速度随机初始化;
计算每个粒子的适应值,将每个粒子的适应值作为历史最优值并将粒子的当前位置作为历史最好位置,同时,将所有粒子中的最好适应度作为粒子群的全局最优解;
更新所有粒子的位置和速度;
重新计算每个粒子的适应值,并将重新计算的每个粒子的适应值与历史最优值进行比较,如果重新计算的每个粒子的适应值超过粒子的历史最优值,则将粒子的当前位置更新为历史最优位置;
将每个粒子的历史最优值与粒子群的历史最优值进行比较,如果粒子的历史最优值超过群体的历史最优值,则将粒子的当前位置更新为该粒子的历史最优位置,并将所有粒子中的最好适应度作为粒子群当前的全局最优解;
若满足优化结束条件则输出结果,得到全局最优值,否则重新更新所有粒子的位置和速度。
5.根据权利要求1~4任一项所述的方法,其特征在于,所述将所述车辆定位信息和所述惯性测量信息进行数据融合处理,得到优化后的车辆定位信息,包括:
采用卡尔曼算法将所述车辆定位信息和所述惯性测量信息进行数据融合处理,得到优化后的车辆定位信息。
6.根据权利要求1~4任一项所述的方法,其特征在于,所述将所述车辆定位信息和所述惯性测量信息进行数据融合处理,得到优化后的车辆定位信息,包括:
对数据融合的误差纠正方程进行学习,得到学习结果;
若所述RTK设备的信号强度低于预设阈值,则将所述学习结果和所述惯性测量信息进行数据融合处理,得到优化后的车辆定位信息。
7.根据权利要求1~4任一项所述的方法,其特征在于,所述获取设置于车辆中的RTK设备采集的车辆定位信息,包括:
获取设置于车辆中的RTK设备接收的差分定位数据;
根据所述差分定位数据确定所述车辆定位信息。
8.一种车辆定位装置,其特征在于,包括:
实时动态RTK信息获取模块,用于获取设置于车辆中的RTK设备采集的车辆定位信息;
惯性测量信息获取模块,用于获取设置于车辆中的惯性测量单元IMU采集的车辆的惯性测量信息;
数据融合模块,用于将所述车辆定位信息和所述惯性测量信息进行数据融合处理,得到优化后的车辆定位信息。
9.一种车辆定位系统,其特征在于,包括:
一个或多个处理器;
存储器,用于存储一个或多个程序;
实时动态RTK设备;
惯性测量单元IMU;
当所述一个或多个程序被所述一个或多个处理器执行,使得所述一个或多个处理器实现如权利要求1-7中任一所述的车辆定位方法。
10.一种计算机可读存储介质,其上存储有计算机程序,其特征在于,该程序被处理器执行时实现如权利要求1-7中任一所述的车辆定位方法。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202010713275.5A CN111750854A (zh) | 2020-07-22 | 2020-07-22 | 车辆定位方法、装置、系统及存储介质 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202010713275.5A CN111750854A (zh) | 2020-07-22 | 2020-07-22 | 车辆定位方法、装置、系统及存储介质 |
Publications (1)
Publication Number | Publication Date |
---|---|
CN111750854A true CN111750854A (zh) | 2020-10-09 |
Family
ID=72710470
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN202010713275.5A Pending CN111750854A (zh) | 2020-07-22 | 2020-07-22 | 车辆定位方法、装置、系统及存储介质 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN111750854A (zh) |
Cited By (5)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN112558130A (zh) * | 2020-12-07 | 2021-03-26 | 安徽江淮汽车集团股份有限公司 | 定位数据的同步方法、装置、设备及存储介质 |
CN113218389A (zh) * | 2021-05-24 | 2021-08-06 | 北京航迹科技有限公司 | 一种车辆定位方法、装置、存储介质及计算机程序产品 |
CN113899374A (zh) * | 2021-12-10 | 2022-01-07 | 智道网联科技(北京)有限公司 | 自动驾驶车辆定位方法、装置及电子设备、存储介质 |
CN114413929A (zh) * | 2021-12-06 | 2022-04-29 | 阿波罗智能技术(北京)有限公司 | 定位信息的校验方法、装置、系统、无人车及存储介质 |
CN115508875A (zh) * | 2022-09-21 | 2022-12-23 | 中国第一汽车股份有限公司 | 目标车辆的定位方法、装置及车辆 |
-
2020
- 2020-07-22 CN CN202010713275.5A patent/CN111750854A/zh active Pending
Cited By (7)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN112558130A (zh) * | 2020-12-07 | 2021-03-26 | 安徽江淮汽车集团股份有限公司 | 定位数据的同步方法、装置、设备及存储介质 |
CN112558130B (zh) * | 2020-12-07 | 2023-12-19 | 安徽江淮汽车集团股份有限公司 | 定位数据的同步方法、装置、设备及存储介质 |
CN113218389A (zh) * | 2021-05-24 | 2021-08-06 | 北京航迹科技有限公司 | 一种车辆定位方法、装置、存储介质及计算机程序产品 |
CN113218389B (zh) * | 2021-05-24 | 2024-05-17 | 北京航迹科技有限公司 | 一种车辆定位方法、装置、存储介质及计算机程序产品 |
CN114413929A (zh) * | 2021-12-06 | 2022-04-29 | 阿波罗智能技术(北京)有限公司 | 定位信息的校验方法、装置、系统、无人车及存储介质 |
CN113899374A (zh) * | 2021-12-10 | 2022-01-07 | 智道网联科技(北京)有限公司 | 自动驾驶车辆定位方法、装置及电子设备、存储介质 |
CN115508875A (zh) * | 2022-09-21 | 2022-12-23 | 中国第一汽车股份有限公司 | 目标车辆的定位方法、装置及车辆 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN111750854A (zh) | 车辆定位方法、装置、系统及存储介质 | |
CN111780755B (zh) | 一种基于因子图和可观测度分析的多源融合导航方法 | |
CN101285686B (zh) | 一种农业机械导航分级定位的方法和系统 | |
CN111880207B (zh) | 一种基于小波神经网络的视觉惯性卫星紧耦合定位方法 | |
CN110954132B (zh) | Grnn辅助自适应卡尔曼滤波进行导航故障识别的方法 | |
US10557711B2 (en) | Apparatus for inferring pedestrian position based on pedestrian movement detection, and method therefor | |
US20230358541A1 (en) | Inertial navigation system capable of dead reckoning in vehicles | |
CN113063425B (zh) | 车辆定位方法、装置、电子设备及存储介质 | |
WO2012145792A1 (en) | Communications | |
CN109141412B (zh) | 面向有数据缺失ins/uwb组合行人导航的ufir滤波算法及系统 | |
CN115060257B (zh) | 一种基于民用级惯性测量单元的车辆变道检测方法 | |
CN114593735B (zh) | 一种位姿预测方法及装置 | |
CN109769206B (zh) | 一种室内定位融合方法、装置、存储介质及终端设备 | |
CN113029173A (zh) | 车辆导航方法及装置 | |
CN111708008B (zh) | 一种基于imu和tof的水下机器人单信标导航方法 | |
CN116772837A (zh) | 基于交互式多模型的gnss/sins组合导航方法 | |
CN117269989A (zh) | 一种基于ins辅助的GNSS欺骗的检测方法及系统 | |
CN114894222B (zh) | Imu-gnss天线的外参数标定方法和相关方法、设备 | |
CN116310991A (zh) | 一种基于强化学习的篮板落点预测方法及系统 | |
CN116295414A (zh) | 基于鲸鱼算法优化的AUV-UFastSLAM算法 | |
CN115540854A (zh) | 一种基于uwb辅助的主动定位方法、设备和介质 | |
CN114660641B (zh) | 一种自适应gps融合定位系统、方法及介质 | |
CN114018250B (zh) | 惯性导航方法、电子设备、存储介质和计算机程序产品 | |
CN114035154B (zh) | 一种运动参数辅助的单站射频信号定位方法 | |
WO2022230449A1 (ja) | 移動量推定装置、移動量推定方法、および移動量推定プログラム |
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 |