CN112525207B - 一种基于车辆俯仰角地图匹配的无人驾驶汽车定位方法 - Google Patents

一种基于车辆俯仰角地图匹配的无人驾驶汽车定位方法 Download PDF

Info

Publication number
CN112525207B
CN112525207B CN202011346584.XA CN202011346584A CN112525207B CN 112525207 B CN112525207 B CN 112525207B CN 202011346584 A CN202011346584 A CN 202011346584A CN 112525207 B CN112525207 B CN 112525207B
Authority
CN
China
Prior art keywords
vehicle
pitch angle
map
time
angle
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
Application number
CN202011346584.XA
Other languages
English (en)
Other versions
CN112525207A (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.)
Wuhan University WHU
Original Assignee
Wuhan University WHU
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 Wuhan University WHU filed Critical Wuhan University WHU
Priority to CN202011346584.XA priority Critical patent/CN112525207B/zh
Publication of CN112525207A publication Critical patent/CN112525207A/zh
Application granted granted Critical
Publication of CN112525207B publication Critical patent/CN112525207B/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/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
    • 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/34—Route searching; Route guidance
    • G01C21/3407—Route searching; Route guidance specially adapted for specific applications
    • G01C21/343—Calculating itineraries, i.e. routes leading from a starting point to a series of categorical destinations using a global route restraint, round trips, touristic trips
    • 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/34—Route searching; Route guidance
    • G01C21/3446—Details of route searching algorithms, e.g. Dijkstra, A*, arc-flags, using precalculated routes

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Automation & Control Theory (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Navigation (AREA)

Abstract

本发明提出了一种基于车辆俯仰角地图匹配的无人驾驶汽车定位方法。本发明使用航位推算模型综合车轮编码器数据、光纤陀螺数据和MEMS加速度计数据来计算车辆的预测位置；然后利用R树结构的车辆俯仰角地图来提高空间搜索效率；再将实时测量的车辆俯仰角数据与俯仰角地图中存储的数据进行匹配，获得更精确的车辆估计位置；利用扩展卡尔曼滤波模型对车辆预测位置和车辆估计位置进行融合。本发明采用先进的动态时间规整算法，将包含车辆俯仰角信息的路线图存储在R树中，加快了实时定位过程，使系统真正做到了实时性；在保证定位精度的同时，提出了融合车内多传感器数据和时间序列数据匹配结果的集成定位模型；且对单俯仰角数据的突变具有鲁棒性。

Description

一种基于车辆俯仰角地图匹配的无人驾驶汽车定位方法
技术领域
本发明属于导航定位技术领域，尤其涉及一种基于车辆俯仰角地图匹配的无人驾驶汽车定位方法。
背景技术
近年来，由于计算机技术，卫星定位导航技术，人工智能技术等的发展，无人驾驶汽车得到了蓬勃的发展。无人驾驶汽车中的一个关键问题就是车辆如何进行精确定位。全球导航卫星系统(GNSS)通常用于车辆定位，在开阔场地，卫星数量足够多的情况下，这些系统可以利用多基站网络RTK技术建立的连续运行(卫星定位服务)参考站(CORS)来达到厘米级的精度。特别是北斗导航卫星系统(BDS)近年来发展迅速，在定位和定时方面显示出巨大的潜力。目前车辆定位的精度很大程度上取决于全球导航卫星系统的收星数量与信号强弱，然而，在城市高楼林立的地区，卫星信号经常出现的丢失现象，以及卫星信号在高楼下产生的多路径问题极大降低了车辆定位精度，甚至可能导致定位失败。惯性导航系统(INS)是卫星信号的一个有吸引力的扩充，可以解决这个问题。惯性导航系统具有航位推算能力，即使在全球导航卫星系统信号丢失时也能提供位置、速度和姿态估计。同时，在开阔区域，利用卫星数据的位置和速度更新可以减少INS误差。然而，惯性导航系统存在累积误差，误差会随时间的增加而无限增大，使得惯性导航系统很难在成本和定位精度之间取得平衡。战术级的惯性测量单元(IMU)可以实现较高的定位精度，但成本太高，无法大规模用于商业。为了提高低成本IMU的定位性能，近年来，IMU与其他传感器相结合，如转向编码器、车轮转速表和车轮编码器等。然而，这些传感器会受到车轮打滑、路面打滑和车轮直径变化等的影响，从而增加定位结果的不确定性和累积误差。
目前，基于地形地图的定位手段逐渐发展起来，因为IMU可以通过车辆俯仰来测量道路坡度，通过车辆滚动来测量道路坡度，或者通过车辆横摆角速度来测量道路曲率。在基于地形的定位中，将驾驶区域的地形信息存储在预定地图中，并将来自车载IMU传感器的俯仰、旋转或航向角的新测量值与预定地图数据进行比较，以定位车辆。目前，使用粒子滤波算法，在没有卫星信号设备的情况下，利用空间俯仰角测量可以获得亚米级的纵向位置精度。并且证明了低频的俯仰数据与速度无关，因此利用俯仰地形变动来定位车辆是可行的。
车内传感器测量的地形信息包括俯仰角、航向角和旋转角信息都有典型的时间序列特征。利用地形信息对车辆进行定位成为一项模式识别任务，特别是在时间序列数据库中基于相似性的模式查询任务。在相似性比较中使用距离的动态时间规整算法是一种经典而先进的解决方案，它广泛应用于音频/视频处理、手势识别、手写识别、工业、天文学、医学、地质学和金融。在过去的十年里，人们提出了几十种替代方法，如符号聚合近似法、基于趋势的时间序列相似性、基于形状的时间序列相似性、基于事件的时间序列相似性和兴趣点方法等，时间序列匹配方法的快速发展使得基于地形的车辆定位更加可靠和实用。然而，所有这些方法中，都存在时间复杂度过高的问题。
以往的研究都试图利用地形的信息，采用基于粒子滤波的方式或者单纯的基于时间序列的匹配方法对车辆进行定位，但都存在时间复杂度过高等局限性。
动态时间规整算法已经被证明是在大多数领域中时间序列数据查询的最精确的方法。然而在以往研究中所讨论的定位过程完全依赖于时间序列匹配，而不考虑精度，这是不鲁棒的，因为车辆的俯仰角数据会跟随速度变化，增加了匹配失败的几率。
基于俯仰角的定位方法的另一个未解决的问题是忽略了IMU的误差。战术级的IMU设备的累积漂移误差是0.1度/小时，而较低成本的IMU设备的累积误差可以达到100度/小时。单靠战术的IMU设备可以完成精确的定位，但是成本太高，无法广泛应用于商用车辆。针对较低成本的IMU，需要设计一种新的定位系统。
发明内容
有鉴于此，本发明提出了一种新的实时定位系统，它结合了光纤陀螺得到的车辆航向角、横滚角和俯仰角信息，车轮编码器得到的速度、行驶里程信息，使用先进的动态时间规整方法，在弱卫星信号以及无卫星信号的情况下为无人驾驶汽车提供高精度的实时定位。
本发明系统包括：光纤陀螺、MEMS加速度计、车轮编码器、差分北斗卫星导航系统、微处理器；
所述微处理器分别与所述的光纤陀螺、MEMS加速度计、车轮编码器、差分北斗卫星导航系统依次连接；
所述光纤陀螺用于采集车辆的姿态信息、车辆的航向角、车辆的横滚角、车辆的俯仰角，并传输至所述微处理器；
所述MEMS加速度计用于采集车辆的航向角加速度、车辆的横滚角加速度、车辆的俯仰角加速度，并传输至所述微处理器；
所述车轮编码器用于采集车辆的速度、车辆的行驶里程，并传输至所述微处理器；
所述差分北斗卫星导航系统用于采集车辆的经度、车辆的纬度、车辆的高度信息，并传输至所述微处理器；
所述微处理器用于实现基于车辆俯仰角地图匹配的无人驾驶汽车定位方法。
基于车辆俯仰角地图匹配的无人驾驶汽车定位方法，包括以下步骤：
步骤1：微处理器通过所述差分北斗卫星导航系统采集车辆的经度、车辆的纬度、车辆的高度信息，通过所述光纤陀螺采集车辆的姿态信息、车辆的航向角、车辆的横滚角、车辆的俯仰角，通过所述车轮编码器采集车辆的速度、车辆的行驶里程，利用R树结构，构建包含道路点精确位置和道路点俯仰角的地图；
步骤2：微处理器通过所述光纤陀螺采集车辆的航向角、车辆的横滚角、车辆的俯仰角，通过所述MEMS加速度计采集车辆的航向角加速度、车辆的横滚角加速度、车辆的俯仰角加速度，通过所述车轮编码器采集车辆的速度、车辆的行驶里程，使用扩展卡尔曼滤波算法对车辆的航向角、车辆的横滚角、车辆的俯仰角、车辆的航向角加速度、车辆的横滚角加速度、车辆的俯仰角加速度、车辆的速度、车辆的行驶里程进行融合，构建航位推算模型；
步骤3：微处理器通过所述光纤陀螺采集当前时刻车辆的航向角、当前时刻车辆的横滚角、当前时刻车辆的俯仰角，通过所述MEMS加速度计采集当前时刻车辆的航向角加速度、车辆的横滚角加速度、车辆的俯仰角加速度，通过所述车轮编码器采当前时刻车辆的速度、车辆的行驶里程，当前时刻车辆的纬度、当前时刻车辆的经度、当前时刻车辆的高度，利用步骤1所述航位推算模型预测下一时刻的车辆的纬度、下一时刻的车辆的经度、下一时刻的车辆的高度、下一时刻车辆的航向角、下一时刻车辆的横滚角、下一时刻车辆的俯仰角；
步骤4：微处理器根据步骤3中车辆下一时刻的经度、下一时刻的纬度、下一时刻的高度、下一时刻的俯仰角，搜索得到车辆在t-d+1到t时刻内车辆的纬度、经度、高度、俯仰角，进一步构建车辆行驶过的车辆位置时间序列；
步骤5：微处理器根据在t-d+1到t时刻内车轮编码器采集的车辆的行驶里程，计算得到d个时刻的车辆行驶的总里程；
步骤6：微处理器将d个时刻的辆行驶的里程与里程阈值比较，若d个时刻的辆行驶的里程小于里程阈值则返回到步骤3；
步骤7：微处理器根据步骤3所述的车辆下一时刻的车辆纬度、下一时刻的车辆经度、下一时刻的车辆高度、下一时刻的车辆俯仰角以及步骤1中得到的包含道路点精确位置和道路点俯仰角的地图，使用R树空间索引，搜索得到包含道路点精确位置和道路点俯仰角的地图中距离车辆当前位置最近的道路点纬度，最近的道路点经度、最近的道路点高度、最近的道路点俯仰角；
步骤8：微处理器通过一个步长为L的搜索窗口、地图中距离车辆当前位置最近的道路点纬度、地图中距离车辆当前位置最近的道路点经度、地图中距离车辆当前位置最近的道路点高度以及地图中距离车辆当前位置最近的道路点俯仰角，在步骤1中得到的道路点精确位置和道路点俯仰角的地图中，使用空间搜索算法，计算得到地图上在所述搜索窗口内的道路点纬度集合、道路点经度集合、道路点高度集合、道路点俯仰角集合。
步骤9：微处理器根据地图中的道路点纬度集合度、地图中的道路点经度集合、地图中的道路点高度集合、地图中的道路点俯仰角集合，构建地图中包含精确位置信息和俯仰角的地图时间序列；
步骤10：微处理器根据步骤4中所述的车辆时间序列、步骤9中所述的地图时间序列，使用动态时间规整算法，输出最优子序列，作为估算的车辆位置集合；根据所述最优子序列、车辆时间序列，通过欧几里得距离，计算最优子序列与车辆时间序列之间的差值序列；在最优子序列与车辆时间序列之间的差值序列中搜素出最优子序列与车辆时间序列之间的差值最小值；根据最优子序列与车辆时间序列之间的差值最小值，搜索该差值最小值所对应的地图时间序列中的道路点，作为最优的车辆估计位置；根据上述最优的车辆估计位置、和步骤3中得到的车辆预测位置，进一步使用扩展卡尔曼滤波算法进行融合，输出最终的车辆位置。
作为优选，步骤3所述当前时刻车辆的航向角为：t时刻车辆的航向角，定义为Yt；
步骤3所述当前时刻车辆的横滚角为：t时刻车辆的横滚角，定义为Rt；
步骤3所述当前时刻车辆的俯仰角为：t时刻车辆的俯仰角，定义为Pt；
步骤3所述当前时刻车辆的航向角加速度为：t时刻车辆的航向角加速度，定义为Yat；
步骤3所述当前时刻车辆的横滚角加速度为：t时刻车辆的横滚角加速度，定义为Rat；
步骤3所述当前时刻车辆的俯仰角加速度为：t时刻车辆的俯仰角加速度，定义为Pat；
步骤3所述当前时刻车辆的速度为：t时刻车辆的速度，定义为vt；
步骤3所述车辆的行驶里程为：t时刻车辆的行驶里程，定义为St；
步骤3所述当前时刻车辆的纬度为：t时刻车辆的纬度，定义为Lat；
步骤3所述当前时刻车辆的经度为：t时刻车辆的经度，定义为Lot；
步骤3所述当前时刻车辆的高度为：t时刻车辆的高度，定义为Ht；
步骤3所述下一时刻车辆的纬度为：t+1时刻车辆的纬度，定义为Lat+1；
步骤3所述下一时刻车辆的经度为：t+1时刻车辆的经度，定义为Lot+1；
步骤3所述下一时刻车辆的高度为：t+1时刻车辆的高度，定义为Ht+1；
步骤3所述下一时刻车辆的航向角为：t+1时刻车辆的航向角，定义为Yt+1；
步骤3所述下一时刻车辆的横滚角为：t+1时刻车辆的横滚角，定义为Rt+1；
步骤3所述下一时刻为车辆的俯仰角：t+1时刻车辆的俯仰角，定义为Pt+1；
作为优选，步骤4所述车辆在t-d+1到t时刻内的车辆的纬度为：
Lat-d+1,Lat-d+2,Lat-d+3....Lat；
步骤4所述车辆在t-d+1到t时刻内的车辆的经度为：
Lot-d+1,Lot-d+2,Lot-d+3....Lot；
步骤4所述车辆在t-d+1到t时刻内的车辆的高度为：
Ht-d+1,Ht-d+2,Ht-d+3....Ht；
步骤4所述车辆在t-d+1到t时刻内的车辆的俯仰角为：
Pt-d+1,Pt-d+2,Pt-d+3....Pt
步骤4所述车辆时间序列具体为：
Figure BDA0002799992340000061
作为优选，步骤5所述d个时刻累加的车辆行驶的总里程为：
Sall＝St-d+1+St-d+2+.....+St-1+St
其中，St为t时刻车辆的行驶里程；
作为优选，步骤7所述下一时刻车辆的纬度为：t+1时刻车辆的纬度，定义为Lat+1；
步骤7所述下一时刻车辆的经度为：t+1时刻车辆的经度，定义为Lot+1；
步骤7所述下一时刻车辆的高度为：t+1时刻车辆的高度，定义为Ht+1；
步骤7所述下一时刻为车辆的俯仰角：t+1时刻车辆的俯仰角，定义为Pt+1；
步骤7所述地图中距离车辆当前位置最近的道路点纬度定义为Lanear；
步骤7所述地图中距离车辆当前位置最近的道路点经度定义为Lonear；
步骤7所述地图中距离车辆当前位置最近的道路点高度定义为Hnear；
步骤7所述地图中距离车辆当前位置最近的道路点俯仰角定义为Pnear；
作为优选，步骤8所述地图中的道路点纬度集合，定义为
Lanear-L,Lanear-L+1......Lanear+L-1,Lanear+L；
步骤8所述地图中的道路点经度集合，定义为
Lonear-L,Lonear-L+1......Lonear+L-1,Lonear+L；
步骤8所述地图中的道路点高度集合，定义为
Hnear-L,Hnear-L+1......Hnear+L-1,Hnear+L；
步骤8所述地图中的道路点俯仰角集合，定义为
Pnear-L,Pnear-L+1......Pnear+L-1,Pnear+L；
作为优选，步骤9所述地图时间序列为：
Figure BDA0002799992340000071
本发明优点在于：
采用先进的动态时间规整算法，将包含车辆俯仰角信息的路线图存储在R树中，加快了实时定位过程，使系统真正做到了实时性；
在保证定位精度的同时，提出了融合车内多传感器数据和时间序列数据匹配结果的集成定位模型；
提出的模型对车辆速度的动态不敏感，而现有的方法大多要求车速稳定；
由于使用时间序列的子序列俯仰角数据来代替单时间点的俯仰角数据，因此对单俯仰角数据的突变具有鲁棒性。
附图说明
图1：为本发明方法流程图；
图2：本发明实施场景示意图；
图3：使用设备参数图。
具体实施方式
为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。
下面结合图1至图3介绍本发明的具体实施方式：
基于车辆俯仰角地图匹配的无人驾驶汽车定位系统包括：光纤陀螺、MEMS加速度计、车轮编码器、差分北斗卫星导航系统、微处理器；
所述微处理器分别与所述的光纤陀螺、MEMS加速度计、车轮编码器、差分北斗卫星导航系统依次连接。
所述光纤陀螺的型号为Novatel SPAN-CPT中内嵌的FOG，用于采集车辆的姿态信息、车辆的航向角、车辆的横滚角、车辆的俯仰角，并传输至所述微处理器，其技术指标见图3所示；
所述MEMS加速度计的型号为Novatel SPAN-CPT中内嵌的加速度计，用于采集车辆的航向角加速度、车辆的横滚角加速度、车辆的俯仰角加速度，并传输至所述微处理器，其技术指标见图3所示；
所述车轮编码器的型号为华测定制的车轮编码器，内部核心是SICK DFS60E，用于采集车辆的速度、车辆的行驶里程，并传输至所述微处理器；
所述差分北斗卫星导航系统由Novatel的GNSS接收天线、北斗信号处理模块以及连续运行参考站组成，用于采集车辆的经度、车辆的纬度、车辆的高度信息，并传输至所述微处理器；
所述微处理器包含3.70GHz CPU和16GB RAM，用于基于车辆俯仰角地图匹配的无人驾驶汽车定位方法。
基于车辆俯仰角地图匹配的无人驾驶汽车定位方法，包括以下步骤：
步骤1：微处理器通过所述差分北斗卫星导航系统采集车辆的经度、车辆的纬度、车辆的高度信息，通过所述光纤陀螺采集车辆的姿态信息、车辆的航向角、车辆的横滚角、车辆的俯仰角，通过所述车轮编码器采集车辆的速度、车辆的行驶里程，利用R树结构，构建包含道路点精确位置和道路点俯仰角的地图；
步骤2：微处理器通过所述光纤陀螺采集车辆的航向角、车辆的横滚角、车辆的俯仰角，通过所述MEMS加速度计采集车辆的航向角加速度、车辆的横滚角加速度、车辆的俯仰角加速度，通过所述车轮编码器采集车辆的速度、车辆的行驶里程，使用扩展卡尔曼滤波算法对车辆的航向角、车辆的横滚角、车辆的俯仰角、车辆的航向角加速度、车辆的横滚角加速度、车辆的俯仰角加速度、车辆的速度、车辆的行驶里程进行融合，构建航位推算模型；
步骤3：微处理器通过所述光纤陀螺采集当前时刻车辆的航向角、当前时刻车辆的横滚角、当前时刻车辆的俯仰角，通过所述MEMS加速度计采集当前时刻车辆的航向角加速度、车辆的横滚角加速度、车辆的俯仰角加速度，通过所述车轮编码器采当前时刻车辆的速度、车辆的行驶里程，当前时刻车辆的纬度、当前时刻车辆的经度、当前时刻车辆的高度，利用步骤1所述航位推算模型预测下一时刻的车辆的纬度、下一时刻的车辆的经度、下一时刻的车辆的高度、下一时刻车辆的航向角、下一时刻车辆的横滚角、下一时刻车辆的俯仰角；
步骤3所述当前时刻车辆的航向角为：t时刻车辆的航向角，定义为Yt；
步骤3所述当前时刻车辆的横滚角为：t时刻车辆的横滚角，定义为Rt；
步骤3所述当前时刻车辆的俯仰角为：t时刻车辆的俯仰角，定义为Pt；
步骤3所述当前时刻车辆的航向角加速度为：t时刻车辆的航向角加速度，定义为Yat；
步骤3所述当前时刻车辆的横滚角加速度为：t时刻车辆的横滚角加速度，定义为Rat；
步骤3所述当前时刻车辆的俯仰角加速度为：t时刻车辆的俯仰角加速度，定义为Pat；
步骤3所述当前时刻车辆的速度为：t时刻车辆的速度，定义为vt；
步骤3所述车辆的行驶里程为：t时刻车辆的行驶里程，定义为St；
步骤3所述当前时刻车辆的纬度为：t时刻车辆的纬度，定义为Lat；
步骤3所述当前时刻车辆的经度为：t时刻车辆的经度，定义为Lot；
步骤3所述当前时刻车辆的高度为：t时刻车辆的高度，定义为Ht；
步骤3所述下一时刻车辆的纬度为：t+1时刻车辆的纬度，定义为Lat+1；
步骤3所述下一时刻车辆的经度为：t+1时刻车辆的经度，定义为Lot+1；
步骤3所述下一时刻车辆的高度为：t+1时刻车辆的高度，定义为Ht+1；
步骤3所述下一时刻车辆的航向角为：t+1时刻车辆的航向角，定义为Yt+1；
步骤3所述下一时刻车辆的横滚角为：t+1时刻车辆的横滚角，定义为Rt+1；
步骤3所述下一时刻为车辆的俯仰角：t+1时刻车辆的俯仰角，定义为Pt+1；
步骤4：微处理器根据步骤3中车辆下一时刻的经度、下一时刻的纬度、下一时刻的高度、下一时刻的俯仰角，搜索得到车辆在t-d+1到t时刻内车辆的纬度、经度、高度、俯仰角，进一步构建车辆行驶过的车辆位置时间序列；
步骤4所述车辆在t-d+1到t时刻内的车辆的纬度为：
Lat-d+1,Lat-d+2,Lat-d+3....Lat；
步骤4所述车辆在t-d+1到t时刻内的车辆的经度为：
Lot-d+1,Lot-d+2,Lot-d+3....Lot；
步骤4所述车辆在t-d+1到t时刻内的车辆的高度为：
Ht-d+1,Ht-d+2,Ht-d+3....Ht；
步骤4所述车辆在t-d+1到t时刻内的车辆的俯仰角为：
Pt-d+1,Pt-d+2,Pt-d+3....Pt
步骤4所述车辆时间序列具体为：
Figure BDA0002799992340000101
步骤5：微处理器根据在t-d+1到t时刻内车轮编码器采集的车辆的行驶里程，计算得到d个时刻的车辆行驶的总里程；
步骤5所述d个时刻累加的车辆行驶的总里程为：
Sall＝St-d+1+St-d+2+.....+St-1+St
其中，St为t时刻车辆的行驶里程；
步骤6：微处理器将d个时刻的辆行驶的里程与里程阈值比较，若d个时刻的辆行驶的里程小于里程阈值则返回到步骤3；
步骤7：微处理器根据步骤3所述的车辆下一时刻的车辆纬度、下一时刻的车辆经度、下一时刻的车辆高度、下一时刻的车辆俯仰角以及步骤1中得到的包含道路点精确位置和道路点俯仰角的地图，使用R树空间索引，搜索得到包含道路点精确位置和道路点俯仰角的地图中距离车辆当前位置最近的道路点纬度，最近的道路点经度、最近的道路点高度、最近的道路点俯仰角；
步骤7所述下一时刻车辆的纬度为：t+1时刻车辆的纬度，定义为Lat+1；
步骤7所述下一时刻车辆的经度为：t+1时刻车辆的经度，定义为Lot+1；
步骤7所述下一时刻车辆的高度为：t+1时刻车辆的高度，定义为Ht+1；
步骤7所述下一时刻为车辆的俯仰角：t+1时刻车辆的俯仰角，定义为Pt+1；
步骤7所述地图中距离车辆当前位置最近的道路点纬度定义为Lanear；
步骤7所述地图中距离车辆当前位置最近的道路点经度定义为Lonear；
步骤7所述地图中距离车辆当前位置最近的道路点高度定义为Hnear；
步骤7所述地图中距离车辆当前位置最近的道路点俯仰角定义为Pnear；
步骤8：微处理器通过一个步长为L的搜索窗口、地图中距离车辆当前位置最近的道路点纬度、地图中距离车辆当前位置最近的道路点经度、地图中距离车辆当前位置最近的道路点高度以及地图中距离车辆当前位置最近的道路点俯仰角，在步骤1中得到的道路点精确位置和道路点俯仰角的地图中，使用空间搜索算法，计算得到地图上在所述搜索窗口内的道路点纬度集合、道路点经度集合、道路点高度集合、道路点俯仰角集合。
步骤8所述地图中的道路点纬度集合，定义为
Lanear-L,Lanear-L+1......Lanear+L-1,Lanear+L；
步骤8所述地图中的道路点经度集合，定义为
Lonear-L,Lonear-L+1......Lonear+L-1,Lonear+L；
步骤8所述地图中的道路点高度集合，定义为
Hnear-L,Hnear-L+1......Hnear+L-1,Hnear+L；
步骤8所述地图中的道路点俯仰角集合，定义为
Pnear-L,Pnear-L+1......Pnear+L-1,Pnear+L；
步骤9：微处理器根据地图中的道路点纬度集合度、地图中的道路点经度集合、地图中的道路点高度集合、地图中的道路点俯仰角集合，构建地图中包含精确位置信息和俯仰角的地图时间序列；
步骤9所述地图时间序列为：
Figure BDA0002799992340000111
步骤10：微处理器根据步骤4中所述的车辆时间序列、步骤9中所述的地图时间序列，使用动态时间规整算法，输出最优子序列，作为估算的车辆位置集合；根据所述最优子序列、车辆时间序列，通过欧几里得距离，计算最优子序列与车辆时间序列之间的差值序列；在最优子序列与车辆时间序列之间的差值序列中搜素出最优子序列与车辆时间序列之间的差值最小值；根据最优子序列与车辆时间序列之间的差值最小值，搜索该差值最小值所对应的地图时间序列中的道路点，作为最优的车辆估计位置；根据上述最优的车辆估计位置、和步骤3中得到的车辆预测位置，进一步使用扩展卡尔曼滤波算法进行融合，输出最终的车辆位置。
步骤10所述最优子序列为地图时间序列中与车辆时间序列最为接近的序列，定义为Toptimal；
步骤10所述差值序列为最优子序列与车辆时间序列之间的差值，定义为Tdif；
本方法的技术流程图如图1所示。
本发明进行了野外实验以测试的实时导航系统的精度和运行效率。的系统和算法是在Windows10 PC上以C++实现的，该PC具有3.70GHz CPU和16GB RAM。使用的测试车是一辆改装自奇瑞瑞虎7的无人驾驶汽车“途联号”，配备了Novatel SPAN-CPT，该车将卫星信号和光纤陀螺、MEMS加速度计耦合在一起进行组合导航。使用了厘米级精度的差分北斗卫星数据用于收集地面真实位置进行算法评估。使用到的无人驾驶汽车如图2所示。
使用到车轮编码器用于测量车辆的速度以及行驶里程。在武汉的花山路上进行测试，在的测试区域中，高度既有急剧的变化，也有轻微的变化，有助于测试定位系统的稳定性。首先使用高精度的差分北斗卫星数据以大约20km/h的速度沿路线行驶(称为参考行程)以生成参考地图，其中包含位置信息(纬度，经度，高度)，姿态信息(航向角，横滚角和俯仰角)和车轮编码器信息，并以100Hz的频率记录。然后，将该包含了俯仰角数据的参考地图存储在R树中。在测试过程中，车辆以各种速度在路线上行驶，依旧以100hz的频率记录原始的光纤陀螺和MEMS加速度计测量值(角度增量，速度增量)，位置信息，姿态信息和距离。然后将车轮编码器的速度值与光纤陀螺和MEMS加速度计得数据集成在一起，生成航位推算模型。
在实验中，以100Hz的频率进行航位推算计算，以1Hz的频率进行动态时间规整匹配，并以1Hz的频率记录最终的状态信息。实验结果表明，纯粹的航位推算模型容易受到陀螺仪，加速度计和车轮编码器的偏差的影响导致误差累积过快。而我们的系统通过基于俯仰角地图匹配算法可以补偿这些偏差，以得到更高精度的位置数据。工作中的动态时间规整算法是通过快速动态时间规整来实现的，最终得到中位运行时间仅12ms，满足100hz的航位推算频率。系统得到的最终的中位位置误差为0.3m，满足了无人驾驶汽车在城市区域所需要的定位需求。
应当理解的是，本说明书未详细阐述的部分均属于现有技术。
应当理解的是，上述针对较佳实施例的描述较为详细，并不能因此而认为是对本发明专利保护范围的限制，本领域的普通技术人员在本发明的启示下，在不脱离本发明权利要求所保护的范围情况下，还可以做出替换或变形，均落入本发明的保护范围之内，本发明的请求保护范围应以所附权利要求为准。

Claims (7)

1.一种基于车辆俯仰角地图匹配的无人驾驶汽车定位方法，其特征在于，包括：
车辆俯仰角地图匹配的无人驾驶汽车系统；
所述车辆俯仰角地图匹配的无人驾驶汽车系统包括：光纤陀螺、MEMS加速度计、车轮编码器、差分北斗卫星导航系统、微处理器；
所述微处理器分别与所述的光纤陀螺、MEMS加速度计、车轮编码器、差分北斗卫星导航系统依次连接；
所述光纤陀螺用于采集车辆的姿态信息、车辆的航向角、车辆的横滚角、车辆的俯仰角，并传输至所述微处理器；
所述MEMS加速度计用于采集车辆的航向角加速度、车辆的横滚角加速度、车辆的俯仰角加速度，并传输至所述微处理器；
所述车轮编码器用于采集车辆的速度、车辆的行驶里程，并传输至所述微处理器；
所述差分北斗卫星导航系统用于采集车辆的经度、车辆的纬度、车辆的高度信息，并传输至所述微处理器；
所述车辆俯仰角地图匹配的无人驾驶汽车定位方法包括以下步骤：
步骤1：微处理器通过所述差分北斗卫星导航系统采集车辆的经度、车辆的纬度、车辆的高度信息，通过所述光纤陀螺采集车辆的姿态信息、车辆的航向角、车辆的横滚角、车辆的俯仰角，通过所述车轮编码器采集车辆的速度、车辆的行驶里程，利用R树结构，构建包含道路点精确位置和道路点俯仰角的地图；
步骤2：微处理器通过所述光纤陀螺采集车辆的航向角、车辆的横滚角、车辆的俯仰角，通过所述MEMS加速度计采集车辆的航向角加速度、车辆的横滚角加速度、车辆的俯仰角加速度，通过所述车轮编码器采集车辆的速度、车辆的行驶里程，使用扩展卡尔曼滤波算法对车辆的航向角、车辆的横滚角、车辆的俯仰角、车辆的航向角加速度、车辆的横滚角加速度、车辆的俯仰角加速度、车辆的速度、车辆的行驶里程进行融合，构建航位推算模型；
步骤3：微处理器通过所述光纤陀螺采集当前时刻车辆的航向角、当前时刻车辆的横滚角、当前时刻车辆的俯仰角，通过所述MEMS加速度计采集当前时刻车辆的航向角加速度、车辆的横滚角加速度、车辆的俯仰角加速度，通过所述车轮编码器采当前时刻车辆的速度、车辆的行驶里程，当前时刻车辆的纬度、当前时刻车辆的经度、当前时刻车辆的高度，利用步骤1所述航位推算模型预测下一时刻的车辆的纬度、下一时刻的车辆的经度、下一时刻的车辆的高度、下一时刻车辆的航向角、下一时刻车辆的横滚角、下一时刻车辆的俯仰角；
步骤4：微处理器根据步骤3中车辆下一时刻的经度、下一时刻的纬度、下一时刻的高度、下一时刻的俯仰角，搜索得到车辆在t-d+1到t时刻内车辆的纬度、经度、高度、俯仰角，进一步构建车辆行驶过的车辆位置时间序列；
步骤5：微处理器根据在t-d+1到t时刻内车轮编码器采集的车辆的行驶里程，计算得到d个时刻的车辆行驶的总里程；
步骤6：微处理器将d个时刻的辆行驶的里程与里程阈值比较，若d个时刻的辆行驶的里程小于里程阈值则返回到步骤3；
步骤7：微处理器根据步骤3所述的车辆下一时刻的车辆纬度、下一时刻的车辆经度、下一时刻的车辆高度、下一时刻的车辆俯仰角以及步骤1中得到的包含道路点精确位置和道路点俯仰角的地图，使用R树空间索引，搜索得到包含道路点精确位置和道路点俯仰角的地图中距离车辆当前位置最近的道路点纬度，最近的道路点经度、最近的道路点高度、最近的道路点俯仰角；
步骤8：微处理器通过一个步长为L的搜索窗口、地图中距离车辆当前位置最近的道路点纬度、地图中距离车辆当前位置最近的道路点经度、地图中距离车辆当前位置最近的道路点高度以及地图中距离车辆当前位置最近的道路点俯仰角，在步骤1中得到的道路点精确位置和道路点俯仰角的地图中，使用空间搜索算法，计算得到地图上在所述搜索窗口内的道路点纬度集合、道路点经度集合、道路点高度集合、道路点俯仰角集合；
步骤9：微处理器根据地图中的道路点纬度集合度、地图中的道路点经度集合、地图中的道路点高度集合、地图中的道路点俯仰角集合，构建地图中包含精确位置信息和俯仰角的地图时间序列；
步骤10：微处理器根据步骤4中所述的车辆时间序列、步骤9中所述的地图时间序列，使用动态时间规整算法，输出最优子序列，作为估算的车辆位置集合；根据所述最优子序列、车辆时间序列，通过欧几里得距离，计算最优子序列与车辆时间序列之间的差值序列；在最优子序列与车辆时间序列之间的差值序列中搜素出最优子序列与车辆时间序列之间的差值最小值；根据最优子序列与车辆时间序列之间的差值最小值，搜索该差值最小值所对应的地图时间序列中的道路点，作为最优的车辆估计位置；根据上述最优的车辆估计位置、和步骤3中得到的车辆预测位置，进一步使用扩展卡尔曼滤波算法进行融合，输出最终的车辆位置。
2.根据权利要求1所述的基于车辆俯仰角地图匹配的无人驾驶汽车定位方法，其特征在于：
步骤3所述当前时刻车辆的航向角为：t时刻车辆的航向角，定义为Yt；
步骤3所述当前时刻车辆的横滚角为：t时刻车辆的横滚角，定义为Rt；
步骤3所述当前时刻车辆的俯仰角为：t时刻车辆的俯仰角，定义为Pt；
步骤3所述当前时刻车辆的航向角加速度为：t时刻车辆的航向角加速度，定义为Yat；
步骤3所述当前时刻车辆的横滚角加速度为：t时刻车辆的横滚角加速度，定义为Rat；
步骤3所述当前时刻车辆的俯仰角加速度为：t时刻车辆的俯仰角加速度，定义为Pat；
步骤3所述当前时刻车辆的速度为：t时刻车辆的速度，定义为vt；
步骤3所述车辆的行驶里程为：t时刻车辆的行驶里程，定义为St；
步骤3所述当前时刻车辆的纬度为：t时刻车辆的纬度，定义为Lat；
步骤3所述当前时刻车辆的经度为：t时刻车辆的经度，定义为Lot；
步骤3所述当前时刻车辆的高度为：t时刻车辆的高度，定义为Ht；
步骤3所述下一时刻车辆的纬度为：t+1时刻车辆的纬度，定义为Lat+1；
步骤3所述下一时刻车辆的经度为：t+1时刻车辆的经度，定义为Lot+1；
步骤3所述下一时刻车辆的高度为：t+1时刻车辆的高度，定义为Ht+1；
步骤3所述下一时刻车辆的航向角为：t+1时刻车辆的航向角，定义为Yt+1；
步骤3所述下一时刻车辆的横滚角为：t+1时刻车辆的横滚角，定义为Rt+1；
步骤3所述下一时刻为车辆的俯仰角：t+1时刻车辆的俯仰角，定义为Pt+1。
3.根据权利要求1所述的基于车辆俯仰角地图匹配的无人驾驶汽车定位方法，其特征在于：
步骤4所述车辆在t-d+1到t时刻内的车辆的纬度为：
Lat-d+1,Lat-d+2,Lat-d+3....Lat；
步骤4所述车辆在t-d+1到t时刻内的车辆的经度为：
Lot-d+1,Lot-d+2,Lot-d+3....Lot；
步骤4所述车辆在t-d+1到t时刻内的车辆的高度为：
Ht-d+1,Ht-d+2,Ht-d+3....Ht；
步骤4所述车辆在t-d+1到t时刻内的车辆的俯仰角为：
Pt-d+1,Pt-d+2,Pt-d+3....Pt
步骤4所述车辆时间序列具体为：
Figure FDA0002799992330000041
4.根据权利要求1所述的基于车辆俯仰角地图匹配的无人驾驶汽车定位方法，其特征在于：
步骤5所述d个时刻累加的车辆行驶的总里程为：
Sall＝St-d+1+St-d+2+.....+St-1+St
其中，St为t时刻车辆的行驶里程。
5.根据权利要求1所述的基于车辆俯仰角地图匹配的无人驾驶汽车定位方法，其特征在于：
步骤7所述下一时刻车辆的纬度为：t+1时刻车辆的纬度，定义为Lat+1；
步骤7所述下一时刻车辆的经度为：t+1时刻车辆的经度，定义为Lot+1；
步骤7所述下一时刻车辆的高度为：t+1时刻车辆的高度，定义为Ht+1；
步骤7所述下一时刻为车辆的俯仰角：t+1时刻车辆的俯仰角，定义为Pt+1；
步骤7所述地图中距离车辆当前位置最近的道路点纬度定义为Lanear；
步骤7所述地图中距离车辆当前位置最近的道路点经度定义为Lonear；
步骤7所述地图中距离车辆当前位置最近的道路点高度定义为Hnear；
步骤7所述地图中距离车辆当前位置最近的道路点俯仰角定义为Pnear。
6.根据权利要求1所述的基于车辆俯仰角地图匹配的无人驾驶汽车定位方法，其特征在于：
步骤8所述地图中的道路点纬度集合，定义为
Lanear-L,Lanear-L+1......Lanear+L-1,Lanear+L；
步骤8所述地图中的道路点经度集合，定义为
Lonear-L,Lonear-L+1......Lonear+L-1,Lonear+L；
步骤8所述地图中的道路点高度集合，定义为
Hnear-L,Hnear-L+1......Hnear+L-1,Hnear+L；
步骤8所述地图中的道路点俯仰角集合，定义为
Pnear-L,Pnear-L+1......Pnear+L-1,Pnear+L。
7.根据权利要求1所述的基于车辆俯仰角地图匹配的无人驾驶汽车定位方法，其特征在于：
步骤9所述地图时间序列为：
Figure FDA0002799992330000051
CN202011346584.XA 2020-11-26 2020-11-26 一种基于车辆俯仰角地图匹配的无人驾驶汽车定位方法 Active CN112525207B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202011346584.XA CN112525207B (zh) 2020-11-26 2020-11-26 一种基于车辆俯仰角地图匹配的无人驾驶汽车定位方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202011346584.XA CN112525207B (zh) 2020-11-26 2020-11-26 一种基于车辆俯仰角地图匹配的无人驾驶汽车定位方法

Publications (2)

Publication Number Publication Date
CN112525207A CN112525207A (zh) 2021-03-19
CN112525207B true CN112525207B (zh) 2022-04-29

Family

ID=74993706

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202011346584.XA Active CN112525207B (zh) 2020-11-26 2020-11-26 一种基于车辆俯仰角地图匹配的无人驾驶汽车定位方法

Country Status (1)

Country Link
CN (1) CN112525207B (zh)

Families Citing this family (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN113503887B (zh) * 2021-06-22 2024-04-16 西安理工大学 单台站地震旋转量和平移量共点测量的运动车辆追踪方法
CN116660952B (zh) * 2023-07-31 2023-10-24 北京斯年智驾科技有限公司 多轴车辆的角度标定补偿方法、装置、设备及介质

Citations (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN107860399A (zh) * 2017-09-22 2018-03-30 北京机械设备研究所 一种基于地图匹配的车载捷联惯导行进间精对准方法
JP2020071122A (ja) * 2018-10-31 2020-05-07 トヨタ自動車株式会社 自車位置推定装置

Family Cites Families (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US11428537B2 (en) * 2019-03-28 2022-08-30 Nexar, Ltd. Localization and mapping methods using vast imagery and sensory data collected from land and air vehicles

Patent Citations (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN107860399A (zh) * 2017-09-22 2018-03-30 北京机械设备研究所 一种基于地图匹配的车载捷联惯导行进间精对准方法
JP2020071122A (ja) * 2018-10-31 2020-05-07 トヨタ自動車株式会社 自車位置推定装置

Non-Patent Citations (7)

* Cited by examiner, † Cited by third party
Title
一种新型的城市道路地图匹配方法;王建培 等;《测绘通报》;20191231(第06期);第81-84页 *
制药上料移动机器人室内地图构建与导航技术研究;朱朔凌;《中国优秀硕士学位论文全文数据库 信息科技辑》;20200731(第07期);第22-25,35-36页 *
城市拥堵环境智能车辆定位方法研究与实践;翁理洪;《中国优秀硕士学位论文全文数据库 工程科技Ⅱ辑》;20200215(第02期);第16-26页 *
基于R树空间索引的植保无人机与植保作业匹配算法;杨泽 等;《农业工程学报》;20171231;第33卷(第S1期);第92-98页 *
基于全景图的车道级高精细地图生成方法研究;路昊;《中国优秀硕士学位论文全文数据库 基础科学辑》;20200315(第03期);第18-21/41-48页 *
基于卡尔曼数据融合提高组合定位算法研究;李男男;《中国优秀硕士学位论文全文数据库 信息科技辑》;20200715(第07期);第43-52页 *
面向复杂城市道路网络的GPS轨迹匹配算法;刘张 等;《电子科技大学学报》;20161231;第45卷(第06期);第1008-1013页 *

Also Published As

Publication number Publication date
CN112525207A (zh) 2021-03-19

Similar Documents

Publication Publication Date Title
CN101907714B (zh) 基于多传感器数据融合的gps辅助定位方法
Bevly Global positioning system (GPS): A low-cost velocity sensor for correcting inertial sensor errors on ground vehicles
CN102538781B (zh) 基于机器视觉和惯导融合的移动机器人运动姿态估计方法
CN111307162A (zh) 用于自动驾驶场景的多传感器融合定位方法
US11022445B2 (en) Segmented path coordinate system
JP2007333385A (ja) 不動物位置記録装置
CN112525207B (zh) 一种基于车辆俯仰角地图匹配的无人驾驶汽车定位方法
CN101201255A (zh) 基于智能导航算法的车辆组合导航系统
CN111562603B (zh) 基于航位推算的导航定位方法、设备及存储介质
CN112923931A (zh) 一种基于固定路线下的特征地图匹配与gps定位信息融合方法
US20210173093A1 (en) Method and system for real-time vehicle location and in-vehicle tracking device
CN110631579A (zh) 一种用于农业机械导航的组合定位方法
Gao et al. Development of precise GPS/INS/wheel speed sensor/yaw rate sensor integrated vehicular positioning system
CN110006421A (zh) 一种基于mems和gps的车载导航方法及系统
CN104864874A (zh) 一种低成本单陀螺航位推算导航方法及系统
CN102538790A (zh) 惯性导航中陀螺仪参数的差异性解决方法
Iqbal et al. Experimental results on an integrated GPS and multisensor system for land vehicle positioning
Liu et al. Interacting multiple model UAV navigation algorithm based on a robust cubature Kalman filter
CN114323003A (zh) 一种基于umb、imu及激光雷达的井工矿融合定位方法
Wang et al. Land vehicle navigation using odometry/INS/vision integrated system
Phuyal Method and use of aggregated dead reckoning sensor and GPS data for map matching
Holzapfel et al. Road profile recognition for autonomous car navigation and Navstar GPS support
Kauffman et al. Navigation via H-field signature map correlation and INS integration
Dean et al. Terrain-based road vehicle localization on multi-lane highways
CN114639263B (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