CN109001789B - 一种基于互相关熵配准的无人车定位融合方法 - Google Patents
一种基于互相关熵配准的无人车定位融合方法 Download PDFInfo
- Publication number
- CN109001789B CN109001789B CN201810570035.7A CN201810570035A CN109001789B CN 109001789 B CN109001789 B CN 109001789B CN 201810570035 A CN201810570035 A CN 201810570035A CN 109001789 B CN109001789 B CN 109001789B
- Authority
- CN
- China
- Prior art keywords
- positioning
- data
- dead reckoning
- point
- gps
- 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
- 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
- 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
Landscapes
- Engineering & Computer Science (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Computer Networks & Wireless Communication (AREA)
- Automation & Control Theory (AREA)
- Navigation (AREA)
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
- Position Fixing By Use Of Radio Waves (AREA)
Abstract
本发明公开了一种基于互相关熵配准的无人车定位融合方法,首先使用轮速传感器和陀螺仪的数据进行无人车航迹推算;然后将GPS定位数据与航迹推算数据使用基于互相关熵的ICP算法进行配准,得到经过长时修正的航迹推算定位结果;进而根据融合长时修正的航迹推算结果,给定新的可能带有较大误差的GPS定位结果,输出融合后的定位结果。本发明利用到的传感设备要求简单,没有使用到无人车的环境感知信息。轮速传感器的使用给***提供了稳定的车速信息,相对于基于IMU等惯性器件测量车体速度的方案更为鲁棒和简练,减弱了因速度的观测不准确而导致的积分漂移现象。本发明计算复杂度低,适应性强,能够在绝大多数无人驾驶汽车中应用,实现实时稳定的定位融合。
Description
技术领域
本发明属于无人驾驶汽车技术中的定位与导航领域,具体涉及一种基于互相关熵配准的无人车定位融合方法。
背景技术
近年来随着汽车产业的高速发展,交通事故已经成为全球性的问题,全世界每年交通事故的死伤人数估计超过50多万人,因此集自动控制、人工智能、模式识别等技术于一体的无人驾驶应用而生。无人车定位技术是无人驾驶技术中关键组成部分之一。为了实时得到无人驾驶汽车的当前位置,无人驾驶汽车上装备了各种主动传感器与被动传感器,包括相机、轮速编码器、GPS和IMU等。在早期的研究中,使用GPS进行定位的方法得到了大量的应用。但是,GPS的使用受周围环境因素的影响巨大。当周围环境出现GPS信号遮挡甚至屏蔽时,其定位结果就会十分不可靠,特别是有可能出现的GPS定位跳变问题,其将会对无人车的运动规划和控制带来不可预估的影响。以IMU、陀螺仪等为代表的惯性器件则不会出现此问题,然而,利用惯性器件进行定位和导航时,定位结果会极大的受到累计误差的影响,当***较长时间工作时,惯性定位***的定位误差极大,失去利用价值。而使用相机或激光等传感器进行定位的方法大部分需要提前建立相应的场景地图,并且此类方法将消耗较大的计算量。
发明内容
本发明的目的在于克服上述现有技术的缺点,提供一种高效、实时、鲁棒的基于互相关熵配准的无人车定位融合方法,无人车能够通过轮速传感器和陀螺仪的数据将GPS定位数据平滑化,得到更为可靠的融合定位结果
为达到上述目的,本发明采用以下技术方案予以实现:
一种基于互相关熵配准的无人车定位融合方法,包括以下步骤:
首先使用轮速传感器和陀螺仪的数据进行无人车航迹推算;然后将GPS定位数据与航迹推算数据使用基于互相关熵的ICP算法进行配准,得到经过长时修正的航迹推算定位结果;进而根据融合长时修正的航迹推算结果,给定新的带有较大误差的GPS定位结果,输出融合后的定位结果。
本发明进一步的改进在于:
具体包括以下步骤:
1)使用无人驾驶汽车的轮速编码器数据和陀螺仪的车辆航向数据进行航迹推算;以此得到车辆积分推演的定位结果;
2)将通过GPS得到的定位序列点和航迹推算的定位序列点使用基于互相关熵的ICP算法进行配准,得到经过长时修正的航迹推算定位结果;
3)当有新的GPS数据输入时,选取在航迹推算位置序列中的对应点作为融合后的定位结果进行输出。
步骤1)的具体方法如下:
1.1)确定航迹推算更新间隔;根据输出频率较低的传感器确定航迹推算的更新间隔T;对于另一个输出频率较高的传感器,在更新时取最新的输出结果作为当前帧数据;
1.2)设置航迹推算起点为无人车定位程序启动时的GPS位置P0=(x0,y0);并设置陀螺仪的起始航向值为0或GPS给出的航向;
1.3)对于从轮速传感器和陀螺仪获得的每一帧车体速度数据vk和航向数据θk,在上一帧的位置Pk-1=(xk-1,yk-1)的基础上,得到第k帧的位置Pk=(xk,yk),计算公式为:
步骤2)的具体方法如下:
2.3)经过步骤2.2)后,利用2.2)求得的对应关系,最大化下述的使用互相关熵定义的目标函数,求取刚体变换:
其中,σ是一个自由变量;
2.4)判断当前的配准误差是否已经符合要求,若已符合要求,则输出刚体变换R和若尚未符合要求,则继续判断当前迭代次数k是否已经达到预设的迭代最大值,若已经达到,则输出刚体变换R和若尚未达到,则返回到步骤2.2),继续迭代计算刚体变换。
步骤3)的具体方法如下:
3.1)当无人车的新的GPS坐标点得到时,将其顺延加入到点集Y的最后;新的航迹推算坐标点顺延加入到点集X的最后;
3.2)使用点集配准算法将两个点集进行配准,取配准后X点集中最新加入的数据点的位置作为融合后的定位数据进行输出。
与现有技术相比,本发明具有以下有益效果:
本发明利用轮速传感器和陀螺仪得到无人车的航迹推算定位结果,将GPS位置点序列与航迹推算位置点序列进行配准以消除航迹推算的累积误差的影响,最后输出配准后的航迹推算位置点作为融合定位结果。
本发明利用到的传感设备要求简单,可靠性高,没有使用到无人车的环境感知信息。轮速传感器的使用给***提供了稳定的车速信息,相对于基于IMU等惯性器件测量车体速度的方案更为鲁棒和简练,减弱了因速度的观测不准确而导致的积分漂移现象。另外,步骤2)中使用基于互相关熵的配准方法将两种定位序列点进行配准,极大的抑制了GPS中可能出现的跳变点对定位融合带来的影响。同时,本发明计算复杂度低,适应性强,能够在绝大多数无人驾驶汽车中应用,实现实时稳定的定位融合。
附图说明
图1为基于互相关熵配准的无人车定位融合方法总体框图;
图2为定位融合示例,针对GPS跳变情况;
图3为定位融合示例,针对GPS定位误差较大的情况。
具体实施方式
下面结合附图对本发明做进一步详细描述:
参见图1-3,本发明基于互相关熵配准的无人车定位融合方法,首先使用轮速传感器和陀螺仪的数据进行无人车航迹推算;然后将GPS定位数据与航迹推算数据使用基于互相关熵的ICP算法进行配准,得到经过长时修正的航迹推算定位结果;进而根据融合长时修正的航迹推算结果,给定新的可能带有较大误差的GPS定位结果,输出融合后的定位结果。如图1所示。具体按以下步骤进行:
步骤1)使用无人驾驶汽车的轮速编码器数据和陀螺仪的车辆航向数据进行航迹推算。以此得到车辆积分推演的定位结果。与通过惯性器件得到的定位数据相似,本步骤中的航迹推算结果也将受到积分的累积误差的影响。
步骤1.1)确定航迹推算更新间隔。由于是使用轮速传感器和陀螺仪两种传感器,不同的传感器的更新频率并不相同,因此需要根据输出频率较低的传感器确定航迹推算的更新间隔T。对于另一个输出频率较高的传感器,在更新时取最新的输出结果作为当前帧数据。
步骤1.2)***初始化。将航迹推算的起点设置为程序启动时GPS的位置P0=(x0,y0),并设置陀螺仪的起始航向值为0或GPS给出的航向。由于本专利提出的配准算法基于迭代最近点(Iterative Closest Point,ICP)算法改进而来,而ICP算法是一个局部收敛算法,将***按照此步骤进行初始化将能够极大的简化算法收敛难度,特别是将航向值的初始值设置为由多帧GPS位置估计出的车体航向时,算法的迭代次数将进一步减少,提高本发明运行的速度。
步骤1.3)对于从轮速传感器和陀螺仪获得的每一帧车体速度数据vk(m/s)和航向数据θk(θk∈(-180°,180°]),在上一帧的位置Pk-1=(xk-1,yk-1)的基础上,得到第k帧的位置Pk=(xk,yk),计算公式为:
步骤2)将通过GPS得到的定位序列点和航迹推算的定位序列点使用基于互相关熵的ICP算法进行配准,得到经过长时修正的航迹推算定位结果。通过本步骤,可以减弱航迹推算的累积误差。
步骤2.3)经过步骤2.2)后,利用2.2)求得的对应关系,最大化下述的使用互相关熵定义的目标函数,求取刚体变换:
其中,σ是一个自由变量。
步骤2.4)判断当前的配准误差是否已经符合要求,若已符合要求,则输出刚体变换R和若尚未符合要求,则继续判断当前迭代次数k是否已经达到预设的迭代最大值,若已经达到,则输出刚体变换R和若尚未达到,则返回到步骤2.2),继续迭代计算刚体变换。
步骤3)当有新的GPS数据输入时,选取在航迹推算位置序列中的对应点作为融合后的定位结果进行输出。
步骤3.1)当有新的航迹推算结果时,把该坐标点加入到点集X中;当有新的GPS位置数据输入时,把新的位置坐标点加入到点集Y中。根据步骤2求得的刚体变换,得到航迹推算点在经过该刚体变换后的位置。将此位置作为融合结果输出。
步骤3.2)本***是一个迭代求解的***,因此执行完输出结果后需要返回步骤1.3循环进行。
经过上述步骤,根据离散的甚至可能存在跳变的GPS定位数据,配合以从轮速传感器和陀螺仪的数据,即可融合得到光滑性较好的定位数据,满足无人车的规划和控制对定位数据平滑性的需要。
图2和图3给出了两个本算法应用的例子。图2显示了当GPS位置突然发生跳变时,本算法的融合性能,其中蓝色的点位航迹推算的点,红色的点位GPS位置,黑色的点为融合后的定位结果。从图中可以看出虽然GPS的定位结果发生了跳变,但是融合的结果并没有发生严重跳变,而是依着GPS变化的趋势慢慢地向GPS位置靠拢,保证了输出结果的平滑性。
图3显示了当GPS定位方差比较大时的融合结果。可以从图中看出GPS定位结果存在较大的高斯噪声。经过本专利的算法融合航迹推算结果之后,定位结果也变得更加平滑。
以上内容仅为说明本发明的技术思想,不能以此限定本发明的保护范围,凡是按照本发明提出的技术思想,在技术方案基础上所做的任何改动,均落入本发明权利要求书的保护范围之内。
Claims (3)
1.一种基于互相关熵配准的无人车定位融合方法,其特征在于,包括以下步骤:
首先使用轮速传感器和陀螺仪的数据进行无人车航迹推算;然后将GPS定位数据与航迹推算数据使用基于互相关熵的ICP算法进行配准,得到经过长时修正的航迹推算定位结果;进而根据融合长时修正的航迹推算结果,给定新的带有较大误差的GPS定位结果,输出融合后的定位结果;
具体包括以下步骤:
1)使用无人驾驶汽车的轮速传感器数据和陀螺仪的车辆航向数据进行航迹推算;以此得到车辆积分推演的定位结果;
2)将通过GPS得到的定位序列点和航迹推算的定位序列点使用基于互相关熵的ICP算法进行配准,得到经过长时修正的航迹推算定位结果;具体方法如下:
2.3)经过步骤2.2)后,利用2.2)求得的对应关系,最大化下述的使用互相关熵定义的目标函数,求取刚体变换:
其中,σ是一个自由变量;
2.4)判断当前的配准误差是否已经符合要求,若已符合要求,则输出刚体变换R和若尚未符合要求,则继续判断当前迭代次数k是否已经达到预设的迭代最大值,若已经达到,则输出刚体变换R和若尚未达到,则返回到步骤2.2),继续迭代计算刚体变换;
3)当有新的GPS数据输入时,选取在航迹推算位置序列中的对应点作为融合后的定位结果进行输出。
2.根据权利要求1所述的基于互相关熵配准的无人车定位融合方法,其特征在于,步骤1)的具体方法如下:
1.1)确定航迹推算更新间隔;根据输出频率较低的轮速传感器确定航迹推算的更新间隔T;对于另一个输出频率较高的轮速传感器,在更新时取最新的输出结果作为当前帧数据;
1.2)设置航迹推算起点为无人车定位程序启动时的GPS位置P0=(x0,y0);并设置陀螺仪的起始航向值为0或GPS给出的航向;
1.3)对于从轮速传感器和陀螺仪获得的每一帧车体速度数据vk和航向数据θk,在上一帧的位置Pk-1=(xk-1,yk-1)的基础上,得到第k帧的位置Pk=(xk,yk),计算公式为:
3.根据权利要求1所述的基于互相关熵配准的无人车定位融合方法,其特征在于,步骤3)的具体方法如下:
3.1)当无人车的新的GPS坐标点得到时,将其顺延加入到点集Y的最后;新的航迹推算坐标点顺延加入到点集X的最后;
3.2)使用点集配准算法将两个点集进行配准,取配准后X点集中最新加入的数据点的位置作为融合后的定位数据进行输出。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201810570035.7A CN109001789B (zh) | 2018-06-05 | 2018-06-05 | 一种基于互相关熵配准的无人车定位融合方法 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201810570035.7A CN109001789B (zh) | 2018-06-05 | 2018-06-05 | 一种基于互相关熵配准的无人车定位融合方法 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN109001789A CN109001789A (zh) | 2018-12-14 |
CN109001789B true CN109001789B (zh) | 2020-05-22 |
Family
ID=64574248
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN201810570035.7A Active CN109001789B (zh) | 2018-06-05 | 2018-06-05 | 一种基于互相关熵配准的无人车定位融合方法 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN109001789B (zh) |
Families Citing this family (5)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN110085013A (zh) * | 2019-04-10 | 2019-08-02 | 同济大学 | 一种汽车轮速无线传感测量*** |
CN110363800B (zh) * | 2019-06-19 | 2021-08-13 | 西安交通大学 | 一种基于点集数据与特征信息相融合的精确刚体配准方法 |
CN112543409B (zh) * | 2019-09-05 | 2021-11-12 | 大唐移动通信设备有限公司 | 一种到达时间toa估计方法及基站 |
CN111258218B (zh) * | 2020-01-17 | 2022-08-12 | 成都信息工程大学 | 基于最大相关熵准则的智能车辆路径跟踪方法 |
CN116400694A (zh) * | 2020-06-28 | 2023-07-07 | 苏州宝时得电动工具有限公司 | 自移动设备 |
Family Cites Families (11)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN100580379C (zh) * | 2006-08-18 | 2010-01-13 | 湖南大学 | Gps/dr车载组合定位***及定位方法 |
CN101907714B (zh) * | 2010-06-25 | 2013-04-03 | 陶洋 | 基于多传感器数据融合的gps辅助定位方法 |
CN102654582A (zh) * | 2012-04-16 | 2012-09-05 | 东莞市泰斗微电子科技有限公司 | 一种联合导航***及方法 |
CN105300395A (zh) * | 2014-07-11 | 2016-02-03 | 北京协进科技发展有限公司 | 一种导航定位方法及装置 |
KR101697645B1 (ko) * | 2014-10-06 | 2017-01-18 | 현대모비스 주식회사 | 추측 항법과 gps를 이용한 복합 항법 시스템 및 그 방법 |
CN104360366B (zh) * | 2014-11-05 | 2017-02-08 | 中国科学院嘉兴微电子与***工程中心 | 航位推算和全球定位***的组合定位方法 |
CN104764457B (zh) * | 2015-04-21 | 2017-11-17 | 北京理工大学 | 一种用于无人车的城市环境构图方法 |
CN105871356B (zh) * | 2016-03-23 | 2018-07-17 | 西安交通大学 | 基于最大混合互相关熵准则的自适应滤波方法 |
CN106482728B (zh) * | 2016-09-14 | 2019-07-23 | 西安交通大学 | 基于最大互相关熵准则无迹卡尔曼滤波的通信保障航天器相对状态估计方法 |
CN107339990B (zh) * | 2017-06-27 | 2020-05-08 | 北京邮电大学 | 多模式融合定位***及方法 |
CN107357761A (zh) * | 2017-06-28 | 2017-11-17 | 西安交通大学 | 一种量化的最小误差熵计算方法 |
-
2018
- 2018-06-05 CN CN201810570035.7A patent/CN109001789B/zh active Active
Also Published As
Publication number | Publication date |
---|---|
CN109001789A (zh) | 2018-12-14 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN109001789B (zh) | 一种基于互相关熵配准的无人车定位融合方法 | |
CN109520497B (zh) | 基于视觉和imu的无人机自主定位方法 | |
KR101628427B1 (ko) | 카메라를 이용한 추측항법 기반 네비게이션 시스템 및 그 제어방법 | |
JP2022113746A (ja) | 判定装置 | |
KR101214143B1 (ko) | 이동체의 위치 및 방향 인식 장치 및 그 방법 | |
KR20200119920A (ko) | 자동 발렛 파킹 시스템의 위치 추정 장치 및 방법 | |
JP2016162013A (ja) | 自己位置推定装置および移動体 | |
CN113405545B (zh) | 定位方法、装置、电子设备及计算机存储介质 | |
TWI522258B (zh) | Based on electronic map, global navigation satellite system and vehicle motion detection technology Lane identification method | |
KR101985344B1 (ko) | 관성 및 단일 광학 센서를 이용한 슬라이딩 윈도우 기반 비-구조 위치 인식 방법, 이를 수행하기 위한 기록 매체 및 장치 | |
CN112415558B (zh) | 行进轨迹的处理方法及相关设备 | |
CN109708632A (zh) | 一种面向移动机器人的激光雷达/ins/地标松组合导航***及方法 | |
KR102331312B1 (ko) | 차량 내부 센서, 카메라, 및 gnss 단말기를 이용한 3차원 차량 항법 시스템 | |
CN114111774B (zh) | 车辆的定位方法、***、设备及计算机可读存储介质 | |
CN114136315B (zh) | 一种基于单目视觉辅助惯性组合导航方法及*** | |
CN112433531A (zh) | 一种自动驾驶车辆的轨迹跟踪方法、装置及计算机设备 | |
JP7418196B2 (ja) | 走行軌跡推定方法及び走行軌跡推定装置 | |
CN112362044A (zh) | 室内定位方法、装置、设备和*** | |
CN112556699B (zh) | 导航定位方法、装置、电子设备及可读存储介质 | |
CN109737957B (zh) | 一种采用级联FIR滤波的INS/LiDAR组合导航方法及*** | |
US20230304802A1 (en) | Passive combined indoor positioning system and method based on intelligent terminal sensor | |
Yang et al. | Real-time infrastructureless indoor tracking for pedestrian using a smartphone | |
CN115451946A (zh) | 一种MEMS-IMU和Wi-Fi组合的室内行人定位方法 | |
CN109074407A (zh) | 多源数据建图方法、相关装置及计算机可读存储介质 | |
CN113155156A (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 |