CN108759833B - 一种基于先验地图的智能车辆定位方法 - Google Patents
一种基于先验地图的智能车辆定位方法 Download PDFInfo
- Publication number
- CN108759833B CN108759833B CN201810382229.4A CN201810382229A CN108759833B CN 108759833 B CN108759833 B CN 108759833B CN 201810382229 A CN201810382229 A CN 201810382229A CN 108759833 B CN108759833 B CN 108759833B
- Authority
- CN
- China
- Prior art keywords
- pose
- intelligent vehicle
- information
- coordinate system
- map
- 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
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/20—Instruments for performing navigational calculations
-
- 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/45—Determining position by combining measurements of signals from the satellite radio beacon positioning system with a supplementary measurement
Abstract
本发明公开一种基于先验地图的智能车辆定位方法,包括通过智能车辆的里程计、车载雷达,分别获取智能车辆的位姿信息、观测信息;利用SLAM算法对智能车辆的位姿信息、观测信息进行处理,得到整个运行区域的全局地图、局部运行区域的局部地图以及智能车辆SLAM坐标系下的位姿信息;获取智能车辆的GPS位姿信息,并将GPS位姿信息转换至SLAM坐标系下,得到智能车辆在SLAM坐标系下的GPS位姿信息;利用卡尔曼滤波技术,将SLAM坐标系下的位姿信息和SLAM坐标系下的GPS位姿信息进行融合,得到智能车辆的初步位姿估计;将全局地图和局部地图进行匹配,利用匹配结果对初步位姿估计进行优化,得到智能车辆的最终位姿。本发明可极大提高智能车定位结果的精度和准确性。
Description
技术领域
本发明涉及智能车辆定位技术领域,特别涉及一种基于先验地图的智能车辆定位方法。
背景技术
智能车辆自主驾驶或辅助驾驶的前提之一就是要准确知道车辆的位姿,当前智能车辆自主驾驶或辅助驾驶中,定位方法一般包括:基于GPS定位、基于GPS与里程计融合进行定位以及同步定位和地图构件技术。
其中,对于基于GPS技术进行定位,其容易受到外界环境的干扰,特别高楼林立的城市环境对GPS信号的干扰很大,会导致定位结果产生很大误差。
对于GPS和里程计融合的定位方法,由于车辆里程计在车辆行驶过程中本身会累积较大的误差,因此得到的定位结果不准确。
对于同步定位与地图构建技术,其使用雷达数据对里程计数据进行校正,得到智能车辆局部的位姿估计,再结合GPS定位,通过坐标转换和卡尔曼滤波,将二者进行融合,得到智能车辆的一个较为准确的位姿。但是受到先有计算机的运算能力的限制,需舍弃掉一部分的观测信息,导致地图精度下降,进而影响定位精度。
发明内容
本发明的目的在于提供一种基于先验地图的智能车辆定位方法,以提高智能车辆的定位精度。
为实现以上目的,本发明采用一种基于先验地图的智能车辆定位方法,智能车辆在预先设定的运行区域内移动,该方法包括如下步骤:
通过智能车辆的里程计、车载雷达,分别获取智能车辆的位姿信息、观测信息,所述位姿信息包括智能车辆在整个运行区域的位姿信息以及在局部运行区域的位姿信息,所述观测信息包括覆盖整个运行区域的观测信息以及覆盖局部运行区域的观测信息;
利用SLAM算法对所述智能车辆的位姿信息、观测信息进行处理,得到整个运行区域的全局地图、局部运行区域的局部地图以及智能车辆SLAM坐标系下的位姿信息;
获取智能车辆在GPS地理坐标系下的位姿信息,并将GPS地理坐标系下的位姿信息转换至所述SLAM坐标系下,得到智能车辆在SLAM坐标系下的GPS位姿信息;
利用卡尔曼滤波技术,将智能车辆SLAM坐标系下的位姿信息和智能车辆在SLAM坐标系下的GPS位姿信息进行融合,得到智能车辆的初步位姿估计;
利用所述全局地图和局部地图进行匹配,并利用匹配结果对所述初步位姿估计进行优化,得到智能车辆的最终位姿。
优选地,所述通过智能车辆的里程计、车载雷达,分别获取智能车辆的位姿信息、观测信息,包括:
构建里程计坐标系以及里程计坐标系下的智能车辆位姿模型,并结合智能车辆自带的里程计,获得智能车辆的位姿信息;
利用所述车载雷达获取并记录智能车辆的观测信息
优选地,所述利用SLAM算法对所述智能车辆的位姿信息、观测信息进行处理,得到整个运行区域的全局地图、局部运行区域的局部地图以及智能车辆SLAM坐标系下的位姿信息,包括:
建立与所述里程计坐标系同原点同轴的所述SLAM坐标系;
在所述SLAM坐标系下,将所述整个运行区域的位姿信息作为SLAM算法的控制输入信息,将所述覆盖整个运行区域的观测信息作为SLAM算法的观测信息,得到整个运行区域的全局地图;
在所述SLAM坐标系下,将所述局部运行区域的位姿信息作为SLAM算法的控制输入信息,将所述覆盖局部运行区域的观测信息作为SLAM算法的观测信息,得到局部运行区域的局部地图和智能车辆SLAM坐标系下的位姿信息。
优选地,还包括:
将所述全局地图中相对于同一个观测节点的两个位姿节点之间的差值作为全局误差项,利用高斯牛顿法对所述全局误差项进行迭代优化;
将优化完成后全局误差项的位姿节点、观测节点添加至所述全局地图中,并在所述全局地图中将相应的位姿节点、观测节点及节点之间边剔除以得到优化后的全局地图;
将所述局部地图中相对于同一个观测节点的两个位姿节点之间的差值作为局部误差项,利用高斯牛顿法对所述局部误差项进行迭代优化;
将优化完成后局部误差项的位姿节点、观测节点添加至所述局部地图中,并在所述局部地图中将相应的位姿节点、观测节点及节点之间的边剔除以得到优化后的局部地图。
优选地,该方法还包括:
在离线状态下,采用高斯牛顿法对所述优化后的全局地图进行离线优化,得到所述离线优化的全局地图。
优选地,所述获取智能车辆在GPS地理坐标系下的位姿信息,并将GPS地理坐标系下的位姿信息转换至所述SLAM坐标系下,得到智能车辆在SLAM坐标系下的GPS位姿信息,包括:
在SLAM系统启动时,获取智能车辆在GPS坐标系中的位姿信息,GPS坐标系中的位姿信息包括原始经度、纬度信息;
采用高斯投影,将所述GPS坐标系中的位姿信息转换到平面坐标系中,得到平面坐标系下的位姿信息;
将平面坐标系中的位姿信息转换到所述SLAM坐标系下,得到智能车辆在SLAM坐标系下的GPS位姿信息。
优选地,所述利用卡尔曼滤波技术,将智能车辆SLAM坐标系下的位姿信息和智能车辆在SLAM坐标系下的GPS位姿信息进行融合,得到智能车辆的初步位姿估计,包括:
根据状态空间转移方程得到(k-1)时刻后验位姿状态向量的估计值,并通过设定的预测方程组对(k-1)时刻后验位姿状态向量的估计值处理,得到当前时刻k的先验位姿状态估计量;
根据状态空间的观测方程,得到当前时刻k位姿向量的观测值,并通过设定的更新方程组对当前时刻k位姿向量的观测值进行处理,得到当前时刻k位姿状态向量的预测值。
优选地,所述利用所述全局地图和局部地图进行匹配,并利用匹配结果对所述初步位姿估计进行优化,得到智能车辆的最终位姿,包括:
根据所述k时刻的的先验位姿和(k-1)时刻的后验位姿,得到智能车辆在k时刻的里程信息uk、k时刻的观测信息zk;
在粒子滤波器中,对智能车辆在k时刻的里程信息uk、k时刻的观测信息zk进行处理,得到智能车辆的最终位姿。
优选地,所述在粒子滤波器中,对智能车辆在k时刻的里程信息uk、k时刻的观测信息zk进行处理,得到智能车辆的最终位姿,包括:
优选地,还包括:
对每个粒子权重信息进行采样,并根据采样结果增加权值高的粒子,删除权值低的粒子,对所述当前智能车辆的最终位姿进行优化。
与现有技术相比,本发明存在以下技术效果:本发明首先通过同步定位与地图构建方法(simultaneous localization and mapping,SLAM)构建信号智能车辆在整个行驶区域的先验全局地图,由于在构建先验全局地图的过程中,可以不考虑实时性,因此可以对整个先验全局地图进行优化,得到更为准确的全局地图信息。同时,通过SLAM实时构建局部地图以及智能车辆的位姿信息,通过卡尔曼滤波器对智能车辆的GPS定位信息以及智能车辆的位姿信息进行融合处理,得到一个较优的位姿,然后将局部地图与全局地图进行匹配,并通过粒子滤波器对智能车辆进行定位,得到智能车辆的准确位姿。由于采用了多个传感器,可以有效的克服GPS信号弱,以及车辆里程漂移的问题。通过局部地图与全局地图匹配的方式,可以有效的解决计算机计算能力的限制造成的实时定位与地图构建中精度不高的问题。
附图说明
下面结合附图,对本发明的具体实施方式进行详细描述:
图1是一种基于先验地图的智能车辆定位方法的流程示意图;
图2是一种基于先验地图的智能车辆定位方法的整体流程示意图。
具体实施方式
为了更进一步说明本发明的特征,请参阅以下有关本发明的详细说明与附图。所附图仅供参考与说明之用,并非用来对本发明的保护范围加以限制。
参阅图1至图2所示,本实施例公开了一种基于先验地图的智能车辆定位方法,智能车辆在预先设定的运行区域内移动,该方法包括如下步骤:
S1、通过智能车辆的里程计、车载雷达,分别获取智能车辆的位姿信息、观测信息,所述位姿信息包括智能车辆在整个运行区域的位姿信息以及在局部运行区域的位姿信息,所述观测信息包括覆盖整个运行区域的观测信息以及覆盖局部运行区域的观测信息;
需要说明的是,智能车辆在整个运行区域的位姿信息以及观测信息是不变的,而智能车辆在局部运行区域的位姿信息以及观测信息随着智能车辆的移动实时发生变化。
S2、利用SLAM算法对所述智能车辆的位姿信息、观测信息进行处理,得到整个运行区域的全局地图、局部运行区域的局部地图以及智能车辆SLAM坐标系下的位姿信息;
其中,智能车辆在整个运行区域内的位姿信息和覆盖整个运行区域的观测信息作为先验数据,该先验数据用于构建智能车辆在整个运行区域的先验全局地图,该先验全局地图不考虑实时性。智能车辆在局部运行区域的位姿信息以及覆盖局部运行区域的观测信息用于构建智能车辆在运行区域内行驶时的局部地图,该局部地图在智能车辆的行驶过程中是实时更新的。
S3、获取智能车辆在GPS地理坐标系下的位姿信息,并将GPS地理坐标系下的位姿信息转换至所述SLAM坐标系下,得到智能车辆在SLAM坐标系下的GPS位姿信息;
其中,智能车辆在GPS地理坐标系下的位姿信息包括智能车辆在GPS坐标系中的原始经度、纬度。
S4、利用卡尔曼滤波技术,将智能车辆SLAM坐标系下的位姿信息和智能车辆在SLAM坐标系下的GPS位姿信息进行融合,得到智能车辆的初步位姿估计;
需要说明的是,智能车辆在SLAM坐标系下的位姿信息为智能车辆在运行区域内行驶时的实时定位信息,即在局部地图构建的同时所获取的定位信息,智能车辆全局地图构建过程中仅进行地图构件不进行车辆定位。
利用卡尔曼滤波器将智能车辆的两种位姿信息进行融合的过程与现有技术中卡尔曼滤波器对位姿的融合过程相同。
S5、利用所述全局地图和局部地图进行匹配,并利用匹配结果对所述初步位姿估计进行优化,得到智能车辆的最终位姿。
需要说明的是,现有技术中在进行同步定位与地图构建时,使用雷达数据对里程计数据进行校正,由于受到计算机运算能力的限制,在同步定位与地图构建过程中需要舍弃掉一部分的观测信息,导致构建的地图精度下降,影响定位精度。本实施例中将实时更新的局部地图与在先构建的全局地图进行匹配,虽然局部地图在构建过程中也会丢弃掉部分观测信息,但是通过将局部地图与全局地图进行实时匹配,可以有效的弥补局部地图丢失的观测信息,解决了由于计算机运算能力不足导致的构建的地图精度低的问题,从而提高了智能车辆定位的精度。
作为进一步优选的方案,步骤S1:通过智能车辆的里程计、车载雷达,分别获取智能车辆的位姿信息、观测信息,包括:
构建里程计坐标系以及里程计坐标系下的智能车辆位姿模型,并结合智能车辆自带的里程计,获得智能车辆的位姿信息;
利用所述车载雷达获取并记录时刻k智能车辆周边的观测信息。
其中,里程计坐标系构建的过程为:以智能车辆启动时车体中心为原点O,将沿智能车辆前进方向做为X轴、垂直X轴指向车体左侧为Y轴方向、垂直X轴和Y轴的方向作为Z轴。假设智能车辆在Z轴上的运动始终为0,则智能车辆的位姿模型为(x,y,θ),其中,(x,y)表示每个障碍物的位姿信息,θ表示角度信息,然后结合智能车辆自带的里程计即可获取智能车辆的实时位姿信息。
作为进一步优选的方案,步骤S2:利用SLAM算法对所述智能车辆的位姿信息、观测信息进行处理,得到整个运行区域的全局地图、局部运行区域的局部地图以及智能车辆SLAM坐标系下的位姿信息,包括:
建立与所述里程计坐标系同原点同轴的所述SLAM坐标系;
在所述SLAM坐标系下,将所述整个运行区域的位姿信息作为SLAM算法的控制输入信息,将所述覆盖整个运行区域的观测信息作为SLAM算法的观测信息,得到整个运行区域的全局地图;
在所述SLAM坐标系下,将所述局部运行区域的位姿信息作为SLAM算法的控制输入信息,将所述覆盖局部运行区域的观测信息作为SLAM算法的观测信息,得到局部运行区域的局部地图和智能车辆SLAM坐标系下的位姿信息。
其中,采用SLAM算法构建整个运行区域的全局地图的过程为:
(1)信息输入:将整个运行区域的位姿信息作为SLAM算法的控制输入信息,将所述覆盖整个运行区域的观测信息作为SLAM算法的观测信息;
(2)SLAM算法进行地图构建:将所述整个运行区域的位姿信息和覆盖整个运行区域的观测信息分别转换在SLAM坐标系下,然后将所述位姿信息和观测信息作为图的节点,将节点之间的约束信息作为图的边,将节点和边添加至图中;
(3)高斯迭代优化:将添加了全部的节点和边的信息后的地图作为待优化地图,通过高斯迭代的方法对待优化地图进行优化,具体为:
在k时刻下,将里程计当前获取的智能车辆位姿信息作为新的位姿节点添加至待优化的地图中,该位姿节点包含了智能车辆当前的位姿和角度信息(x,y,θ)。
在k时刻下,将车载雷达当前获取的智能车辆的n个观测信息作为新的观测节点0k添加至待优化地图中,其中k∈[1,n]。该观测节点中包含了智能车辆周边每个障碍物的位姿和角度信息(x,y,θ)。
将每个新的位姿节点及其对应的新的观测节点之间在x、y、θ三个方向的差值作为约束节点和节点之间的边添加至待优化的地图中。该节点和节点之间的约束包括同一个观测节点和不同的位姿节点之间的约束,两个位姿节点之间的约束。
将对于同一个观测节点相对于两个位姿节点会有所不同,将两个位姿节点之间的差值作为待优化的误差项,当误差项累计到一定程度的时候,比如已经有了7-8帧的观测数据所包含的所有误差项。采用高斯牛顿法对待优化的地图进行优化处理,以降低整个地图的误差项。
在地图优化完成之后,将已经优化的位姿节点和观测节点加入到全局地图中,并且在待优化的地图中将这些节点和边剔除掉。
当智能车辆完成对整个运行区域的探索后,即可构建出整个运行区域的全局地图。
需要说明的是,局部地图的构建过程与全局地图的构建过程相似,只是SLAM算法的输入数据不同,构建局部地图输入的是局部运行区域内的位姿信息和覆盖局部运行区域即智能车辆周边的观测信息。将局部运行区域内的位姿信息和覆盖局部运行区域的观测信息分别转换在SLAM坐标系下,实时的得到局部运行区域中智能车辆在SLAM坐标系下的位姿信息。
作为进一步优选的方案,本实施例中在构建上述全局地图过程中,将位姿信息、观测信息等添加在第三方提供的地图框架中构建全局地图,同时将位姿信息、观测信息作为节点、以及节点之间的约束条件均存储在数据库中,在全局地图构建完成后,还采用离线的方法对全局地图进行离线优化,具体过程为:
将上述存储在数据库中的所有节点添加至上述全局地图中;
将上述存储在数据库中的节点间约束条件添加至上述全局地图中对应的节点和节点之间;
将添加了数据库中的节点和边的信息的全局地图作为待二次优化地图,并在离线状态下对待二次优化地图进行高斯牛顿迭代优化,得到二次优化后的全局地图。
需要说明的是,在实际应用中,由于系统对实时性的要求导致了记性迭代优化的次数会受到限制,待优化地图的大小也会受到限制。导致两相邻两次迭代优化得到的地图间的约束信息损失,以及整个全局地图的约束信息损失。本实施例中通过在离线状态下对得到的全局地图进行离线优化,降低了地图的约束信息损失量,确保了全局地图构建的准确性和精度。
作为进一步优选的方案,上述步骤S3:获取智能车辆在GPS地理坐标系下的位姿信息,并将GPS地理坐标系下的位姿信息转换至所述SLAM坐标系下,得到智能车辆在SLAM坐标系下的GPS位姿信息,包括:
在SLAM系统启动时,获取智能车辆在GPS坐标系中的位姿信息,GPS坐标系中的位姿信息包括原始经度、纬度信息;
采用高斯投影,将所述GPS坐标系中的位姿信息转换到平面坐标系中,得到平面坐标系下的位姿信息;
将平面坐标系中的位姿信息转换到所述SLAM坐标系下,得到智能车辆在SLAM坐标系下的GPS位姿信息。
作为进一步优选的方案,步骤S4:利用卡尔曼滤波技术,将智能车辆SLAM坐标系下的位姿信息和智能车辆在SLAM坐标系下的GPS位姿信息进行融合,得到智能车辆的初步位姿估计,包括:
定义状态空间转移方程和状态空间的观测方程如下:
X(k)=AX(k-1)+W(k),
Z(k)=HX(k)+V(k),
其中,X(k)为k时刻的位姿状态向量,A(k)为过程矩阵,H为测量矩阵,W(k)为过程噪声矩阵,Z(k)表示观测矩阵,其协方差为Q,V(k)为测量噪声,其协方差为R,A表示状态转移矩阵。
状态空间转移方程用于通过上一时刻系统状态X(k-1)和状态转移矩阵的乘积加上高斯噪声得到当前时刻系统的状态。
状态空间的观测方程用于通过当前时刻系统状态X(k)和测量矩阵的乘积加上一个观测的高斯噪声得到当前系统的观测量。
卡尔曼滤波的过程如下:
预测方程组:
X(k|k-1)=AX(k-1|k-1),
P(k|k-1)=AP(k-1|k-1)A′+Q,
更新方程组:
X(k|k)=X(k|k-1)+Kg(k)(Z(k)-HX(k|k-1)),
P(k|k)=(1-Kg(k)H)P(k|k-1),
其中:
X(k|k-1)是k时刻的预估位姿状态向量;
X(k-1|k-1)是k-1时刻的最优位姿状态;
Kg(k)是k时刻的卡尔曼增益;
X(k|k)是k时刻的最优位姿状态估计;
Q是系统过程噪声W(k)的协方差;
R是系统测量噪声V(k)的协方差;
P代表协方差矩阵;
P(k|k-1)、P(k-1|k-1)、P(k|k)分别表示k时刻的先验协方差矩阵,k-1时刻的后验协方差矩阵,k时刻的后验协方差矩阵;
A′表示状态转移矩阵A的逆矩阵,H′表示测量矩阵H的逆矩阵。
需要说明的是,本实施例中采用卡尔曼滤波状态估计过程中的经典公式,通过将
需要说明的是,根据状态空间转移方程得到(k-1)时刻后验位姿状态向量的估计值,并通过设定的预测方程组对(k-1)时刻后验位姿状态向量的估计值处理,得到当前时刻k的先验位姿状态估计量,通过对比k时刻的先验位姿和(k-1)时刻的后验位姿,可以得到k时刻的控制信息即里程信息uk,k时刻的观测信息zk。
作为进一步优选的方案,上述步骤S5:在粒子滤波器中,对智能车辆在k时刻的里程信息uk、k时刻的观测信息zk进行处理,得到智能车辆的最终位姿,包括:
需要说明的是,初始化过程中粒子的位姿是直接赋值的,不需要推算。
作为进一步优选的方案,在计算出k时刻智能车辆的后验位姿后还包括:
对每个粒子权重信息进行重采样,并根据采样结果增加权值高的粒子,删除权值低的粒子,对所述当前智能车辆的后验位姿即最终位姿进行优化。
需要说明的是,本实施例中通过在粒子滤波器中采用序贯重要性采样算法和采样重要性重采样算法来获得粒子的后验概率,通过对参考分布的采样粒子进行加权来近似得到后验概率分布。
本方案通过使用卡尔曼滤波器对SLAM坐标和GPS坐标进行融合再配合粒子滤波器将局部地图和先验地图进行匹配从而进一步对上述定位结果进行优化,最后可以得到一个更为精确的定位结果。对比别的定位方案,本方案可以更有效的对智能车辆进行定位,特别是可以有效克服由于GPS漂移而带来的定位误差。同时由于使用了经过离线优化的先验地图进行匹配,本方案对比别的定位方案会更为鲁棒。
需要说明的是,本方案的总体思路为:通过构建智能车辆在整个行驶区域的先验全局地图,对全局地图进行离线优化得到一个更为准确的地图信息。通过结合多传感器来有效克服GPS信号弱以及车辆里程计漂移的问题。通过局部地图与全局地图匹配的方式,解决计算机计算能力的限制造成实时定位与地图构建精度不高的问题。
以上所述仅为本发明的较佳实施例,并不用以限制本发明,凡在本发明的精神和原则之内,所作的任何修改、等同替换、改进等,均应包含在本发明的保护范围之内。
Claims (9)
1.一种基于先验地图的智能车辆定位方法,智能车辆在预先设定的运行区域内移动,其特征在于,包括:
通过智能车辆的里程计、车载雷达,分别获取智能车辆的位姿信息、观测信息,所述位姿信息包括智能车辆在整个运行区域的位姿信息以及在局部运行区域的位姿信息,所述观测信息包括覆盖整个运行区域的观测信息以及覆盖局部运行区域的观测信息;
利用SLAM算法对所述智能车辆的位姿信息、观测信息进行处理,得到整个运行区域的全局地图、局部运行区域的局部地图以及智能车辆SLAM坐标系下的位姿信息;
获取智能车辆在GPS地理坐标系下的位姿信息,并将GPS地理坐标系下的位姿信息转换至所述SLAM坐标系下,得到智能车辆在SLAM坐标系下的GPS位姿信息;
利用卡尔曼滤波技术,将智能车辆SLAM坐标系下的位姿信息和智能车辆在SLAM坐标系下的GPS位姿信息进行融合,得到智能车辆的初步位姿估计;
利用所述全局地图和局部地图进行匹配,并利用匹配结果对所述初步位姿估计进行优化,得到智能车辆的最终位姿;
还包括:
将所述全局地图中相对于同一个观测节点的两个位姿节点之间的差值作为全局误差项,利用高斯牛顿法对所述全局误差项进行迭代优化;
将优化完成后全局误差项的位姿节点、观测节点添加至所述全局地图中,并在所述全局地图中将相应的位姿节点、观测节点及节点之间边剔除以得到优化后的全局地图;
将所述局部地图中相对于同一个观测节点的两个位姿节点之间的差值作为局部误差项,利用高斯牛顿法对所述局部误差项进行迭代优化;
将优化完成后局部误差项的位姿节点、观测节点添加至所述局部地图中,并在所述局部地图中将相应的位姿节点、观测节点及节点之间的边剔除以得到优化后的局部地图。
2.如权利要求1所述的基于先验地图的智能车辆定位方法,其特征在于,所述通过智能车辆的里程计、车载雷达,分别获取智能车辆的位姿信息、观测信息,包括:
构建里程计坐标系以及里程计坐标系下的智能车辆位姿模型,并结合智能车辆自带的里程计,获得智能车辆的位姿信息;
利用所述车载雷达获取并记录智能车辆的观测信息。
3.如权利要求2所述的基于先验地图的智能车辆定位方法,其特征在于,所述利用SLAM算法对所述智能车辆的位姿信息、观测信息进行处理,得到整个运行区域的全局地图、局部运行区域的局部地图以及智能车辆SLAM坐标系下的位姿信息,包括:
建立与所述里程计坐标系同原点同轴的所述SLAM坐标系;
在所述SLAM坐标系下,将所述整个运行区域的位姿信息作为SLAM算法的控制输入信息,将所述覆盖整个运行区域的观测信息作为SLAM算法的观测信息,得到整个运行区域的全局地图;
在所述SLAM坐标系下,将所述局部运行区域的位姿信息作为SLAM算法的控制输入信息,将所述覆盖局部运行区域的观测信息作为SLAM算法的观测信息,得到局部运行区域的局部地图和智能车辆SLAM坐标系下的位姿信息。
4.如权利要求1所述的基于先验地图的智能车辆定位方法,其特征在于,还包括:
在离线状态下,采用高斯牛顿法对所述优化后的全局地图进行离线优化,得到所述离线优化的全局地图。
5.如权利要求1所述的基于先验地图的智能车辆定位方法,其特征在于,所述获取智能车辆在GPS地理坐标系下的位姿信息,并将GPS地理坐标系下的位姿信息转换至所述SLAM坐标系下,得到智能车辆在SLAM坐标系下的GPS位姿信息,包括:
在SLAM系统启动时,获取智能车辆在GPS坐标系中的位姿信息,GPS坐标系中的位姿信息包括原始经度、纬度信息;
采用高斯投影,将所述GPS坐标系中的位姿信息转换到平面坐标系中,得到平面坐标系下的位姿信息;
将平面坐标系中的位姿信息转换到所述SLAM坐标系下,得到智能车辆在SLAM坐标系下的GPS位姿信息。
6.如权利要求1所述的基于先验地图的智能车辆定位方法,其特征在于,所述利用卡尔曼滤波技术,将智能车辆SLAM坐标系下的位姿信息和智能车辆在SLAM坐标系下的GPS位姿信息进行融合,得到智能车辆的初步位姿估计,包括:
根据状态空间转移方程得到(k-1)时刻后验位姿状态向量的估计值,并通过设定的预测方程组对(k-1)时刻后验位姿状态向量的估计值处理,得到当前时刻k的先验位姿状态估计量;
根据状态空间的观测方程,得到当前时刻k位姿向量的观测值,并通过设定的更新方程组对当前时刻k位姿向量的观测值进行处理,得到当前时刻k位姿状态向量的预测值。
7.如权利要求6所述的基于先验地图的智能车辆定位方法,其特征在于,所述利用所述全局地图和局部地图进行匹配,并利用匹配结果对所述初步位姿估计进行优化,得到智能车辆的最终位姿,包括:
根据所述k时刻的的先验位姿和(k-1)时刻的后验位姿,得到智能车辆在k时刻的里程信息uk、k时刻的观测信息zk;
在粒子滤波器中,对智能车辆在k时刻的里程信息uk、k时刻的观测信息zk进行处理,得到智能车辆的最终位姿。
9.如权利要求8所述的基于先验地图的智能车辆定位方法,其特征在于,还包括:
对每个粒子权重信息进行重采样,并根据采样结果增加权值高的粒子,删除权值低的粒子,对所述当前智能车辆的最终位姿进行优化。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201810382229.4A CN108759833B (zh) | 2018-04-25 | 2018-04-25 | 一种基于先验地图的智能车辆定位方法 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201810382229.4A CN108759833B (zh) | 2018-04-25 | 2018-04-25 | 一种基于先验地图的智能车辆定位方法 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN108759833A CN108759833A (zh) | 2018-11-06 |
CN108759833B true CN108759833B (zh) | 2021-05-25 |
Family
ID=64011929
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN201810382229.4A Active CN108759833B (zh) | 2018-04-25 | 2018-04-25 | 一种基于先验地图的智能车辆定位方法 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN108759833B (zh) |
Families Citing this family (24)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN109545072B (zh) * | 2018-11-14 | 2021-03-16 | 广州广电研究院有限公司 | 地图构建的位姿计算方法、装置、存储介质和系统 |
CN111323004B (zh) * | 2018-12-16 | 2022-05-13 | 北京魔门塔科技有限公司 | 初始位置的确定方法及车载终端 |
CN111323029B (zh) * | 2018-12-16 | 2022-05-27 | 北京魔门塔科技有限公司 | 导航方法及车载终端 |
CN111383324B (zh) * | 2018-12-29 | 2023-03-28 | 广州文远知行科技有限公司 | 点云地图的构建方法、装置、计算机设备和存储介质 |
DE102019101569A1 (de) * | 2019-01-23 | 2020-07-23 | Bayerische Motoren Werke Aktiengesellschaft | Verfahren zum fahrerlosen Umsetzen eines Fahrzeugs über eine Strecke innerhalb eines abgeschlossenen Geländes |
CN109917790A (zh) * | 2019-03-21 | 2019-06-21 | 上海赛摩物流科技有限公司 | 一种自主导引车辆及其行驶控制方法和控制装置 |
CN110246182B (zh) * | 2019-05-29 | 2021-07-30 | 达闼机器人有限公司 | 基于视觉的全局地图定位方法、装置、存储介质和设备 |
CN110954113B (zh) * | 2019-05-30 | 2021-10-15 | 北京初速度科技有限公司 | 一种车辆位姿的修正方法和装置 |
CN110490809B (zh) * | 2019-08-28 | 2021-12-24 | 清华大学 | 多智能体协同定位与建图方法及装置 |
CN110617825B (zh) * | 2019-09-29 | 2022-01-18 | 阿波罗智联(北京)科技有限公司 | 一种车辆定位方法、装置、电子设备和介质 |
CN110794434B (zh) * | 2019-11-29 | 2022-11-15 | 广州视源电子科技股份有限公司 | 一种位姿的确定方法、装置、设备及存储介质 |
CN111735451B (zh) * | 2020-04-16 | 2022-06-07 | 中国北方车辆研究所 | 一种基于多源先验信息的点云匹配高精度定位方法 |
CN111624618A (zh) * | 2020-06-09 | 2020-09-04 | 安徽意欧斯物流机器人有限公司 | 融合激光slam和二维码导航的定位方法及搬运平台 |
CN111949943B (zh) * | 2020-07-24 | 2022-08-30 | 北京航空航天大学 | 一种面向高级自动驾驶的v2x和激光点云配准的车辆融合定位方法 |
CN111983636A (zh) * | 2020-08-12 | 2020-11-24 | 深圳华芯信息技术股份有限公司 | 位姿融合方法、系统、终端、介质以及移动机器人 |
CN112230211A (zh) * | 2020-10-15 | 2021-01-15 | 长城汽车股份有限公司 | 车辆定位方法、装置、存储介质及车辆 |
CN112904396A (zh) * | 2021-02-03 | 2021-06-04 | 深圳亿嘉和科技研发有限公司 | 一种基于多传感器融合的高精度定位方法及系统 |
CN113094462B (zh) * | 2021-04-30 | 2023-10-24 | 腾讯科技(深圳)有限公司 | 数据处理方法和装置及存储介质 |
CN113218385B (zh) * | 2021-05-24 | 2022-05-27 | 周口师范学院 | 基于slam的高精度车辆定位方法 |
CN113763560B (zh) * | 2021-08-02 | 2024-02-09 | 纵目科技(上海)股份有限公司 | 点云数据的生成方法、系统、设备及计算机可读存储介质 |
CN113838129B (zh) * | 2021-08-12 | 2024-03-15 | 高德软件有限公司 | 一种获得位姿信息的方法、装置以及系统 |
CN114660589B (zh) * | 2022-03-25 | 2023-03-10 | 中国铁建重工集团股份有限公司 | 一种地下隧道的定位方法、系统及装置 |
CN115468569A (zh) * | 2022-09-16 | 2022-12-13 | 海南大学 | 一种基于双定位的语音控制车辆导航方法 |
CN117278941A (zh) * | 2023-09-15 | 2023-12-22 | 广东省机场管理集团有限公司工程建设指挥部 | 基于5g网络和数据融合的车辆驾驶辅助定位方法及装置 |
Citations (5)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN104764457A (zh) * | 2015-04-21 | 2015-07-08 | 北京理工大学 | 一种用于无人车的城市环境构图方法 |
CN105928505A (zh) * | 2016-04-19 | 2016-09-07 | 深圳市神州云海智能科技有限公司 | 移动机器人的位姿确定方法和设备 |
CN106767853A (zh) * | 2016-12-30 | 2017-05-31 | 中国科学院合肥物质科学研究院 | 一种基于多信息融合的无人驾驶车辆高精度定位方法 |
CN106840179A (zh) * | 2017-03-07 | 2017-06-13 | 中国科学院合肥物质科学研究院 | 一种基于多传感器信息融合的智能车定位方法 |
CN107144285A (zh) * | 2017-05-08 | 2017-09-08 | 深圳地平线机器人科技有限公司 | 位姿信息确定方法、装置和可移动设备 |
Family Cites Families (2)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN104850615A (zh) * | 2015-05-14 | 2015-08-19 | 西安电子科技大学 | 一种基于g2o的SLAM后端优化算法方法 |
CN105809687B (zh) * | 2016-03-08 | 2019-09-27 | 清华大学 | 一种基于图像中边沿点信息的单目视觉测程方法 |
-
2018
- 2018-04-25 CN CN201810382229.4A patent/CN108759833B/zh active Active
Patent Citations (5)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN104764457A (zh) * | 2015-04-21 | 2015-07-08 | 北京理工大学 | 一种用于无人车的城市环境构图方法 |
CN105928505A (zh) * | 2016-04-19 | 2016-09-07 | 深圳市神州云海智能科技有限公司 | 移动机器人的位姿确定方法和设备 |
CN106767853A (zh) * | 2016-12-30 | 2017-05-31 | 中国科学院合肥物质科学研究院 | 一种基于多信息融合的无人驾驶车辆高精度定位方法 |
CN106840179A (zh) * | 2017-03-07 | 2017-06-13 | 中国科学院合肥物质科学研究院 | 一种基于多传感器信息融合的智能车定位方法 |
CN107144285A (zh) * | 2017-05-08 | 2017-09-08 | 深圳地平线机器人科技有限公司 | 位姿信息确定方法、装置和可移动设备 |
Non-Patent Citations (2)
Title |
---|
Lidar Scan Matching EKF-SLAM Using the Differential Model of Vehicle Motion;Daobin Wang 等;《2013 IEEE Intelligent Vehicles Symposium (IV)》;20130623;第908-912页 * |
基于高斯牛顿的局部优化SLAM系统;郝亚东 等;《传感器世界》;20180331;第24卷(第3期);第7-11页 * |
Also Published As
Publication number | Publication date |
---|---|
CN108759833A (zh) | 2018-11-06 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN108759833B (zh) | 一种基于先验地图的智能车辆定位方法 | |
US20230194266A1 (en) | Vision-aided inertial navigation | |
CN109211251B (zh) | 一种基于激光和二维码融合的即时定位与地图构建方法 | |
CN110412635B (zh) | 一种环境信标支持下的gnss/sins/视觉紧组合方法 | |
CN112268559B (zh) | 复杂环境下融合slam技术的移动测量方法 | |
CN110930495A (zh) | 基于多无人机协作的icp点云地图融合方法、系统、装置及存储介质 | |
CN112639502A (zh) | 机器人位姿估计 | |
Wen et al. | Performance comparison of GNSS/INS integrations based on EKF and factor graph optimization | |
JP4984659B2 (ja) | 自車両位置推定装置 | |
CN112762957B (zh) | 一种基于多传感器融合的环境建模及路径规划方法 | |
CN108387236B (zh) | 一种基于扩展卡尔曼滤波的偏振光slam方法 | |
US11580688B2 (en) | High-definition city mapping | |
CN112965063B (zh) | 一种机器人建图定位方法 | |
CN114001733B (zh) | 一种基于地图的一致性高效视觉惯性定位算法 | |
CN114964212B (zh) | 面向未知空间探索的多机协同融合定位与建图方法 | |
Zhang et al. | Vision-aided localization for ground robots | |
CN115980765A (zh) | 一种基于环境感知辅助粒子滤波的车辆定位方法 | |
CN116839600A (zh) | 一种基于轻量化点云地图的视觉测绘导航定位方法 | |
Han et al. | Precise positioning with machine learning based Kalman filter using GNSS/IMU measurements from android smartphone | |
WO2023226154A1 (zh) | 自主定位方法、装置、设备及计算机可读存储介质 | |
Poddar et al. | Tuning of GPS aided attitude estimation using evolutionary algorithms | |
CN114264301A (zh) | 车载多传感器融合定位方法、装置、芯片及终端 | |
Das et al. | Pose-graph based crowdsourced mapping framework | |
Aditya | Implementation of a 4D fast SLAM including volumetric sum of the UAV | |
CN112991400A (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 |