一种基于概率优化计算的惯导和地图组合定位方法技术

本发明专利技术公开了一种基于概率优化计算的惯导和地图组合定位方法，涉及导航定位领域，定义当地地理坐标系为导航系，记为n系，其x，y，z轴分别指向朝东，北，以及沿着重力反方向朝天，定义惯导坐标系为本体系，记作b系，利用光纤惯导位置更新结果与矢量地图进行地图匹配，能够实现卫星信号不可用条件下自动驾驶汽车的车道级高精度定位，另外通过预估惯导的定位误差来预筛候选路段，有效提升了匹配效率。有效提升了匹配效率。有效提升了匹配效率。

【技术实现步骤摘要】
一种基于概率优化计算的惯导和地图组合定位方法


[0001]本专利技术涉及导航定位领域
，更具体地说，它涉及一种基于概率优化计算的惯导和地图组合定位方法。

技术介绍

[0002]随着车载传感器的开发和大规模生产，无人驾驶技术已成为研究热点，这意味着车辆可以根据自己对周围环境条件的感知和理解自行进行运动控制，并达到人类驾驶员的水平。为了确定车辆在环境中的位置，定位模块在自主驾驶中起着非常重要的作用。传统的惯性导航方法，其典型组合之一是IMU(惯性测量单元)和里程计，可以在GNSS(全球导航卫星系统)拒止的环境中实现高精度定位，但长时间工作会导致误差累积。为了解决这个问题，利用其他信息辅助惯性的综合定位方法相继被提出。常见的传感器包括激光雷达、照相机和毫米波雷达。这些方案很容易受到环境的影响，导致定位结果的误差较大。

技术实现思路

[0003]针对现有技术存在的不足，本专利技术的目的在于提供一种基于概率优化计算的惯导和地图组合定位方法，解决现有组合定位方案在卫星信号不可用条件下定位误差大。本专利技术提出了一种基于概率优化计算的惯导和矢量地图的组合定位方法，利用光纤惯导位置更新结果与矢量地图进行地图匹配，能够实现卫星信号不可用条件下自动驾驶汽车的车道级高精度定位。
[0004]为实现上述目的，本专利技术提供了如下技术方案：
[0005]一种基于概率优化计算的惯导和地图组合定位方法，包括如下步骤：
[0006]步骤一：通过惯导的输出轨迹和本地矢量地图进行匹配定位，将当地地理坐标系标记为导航系，记为n系，将惯导坐标系标记为本体系，记为b系；
[0007]步骤二：标记惯导输出的轨迹为点的集合为O{o1,o2,o3
…
on}，其中，o1，o2
……
on为单个点；
[0008]步骤三：标记矢量地图为路段的集合为Q{q1,q2,q3
……
qn}，q1,q2
……
qn为单个路段；
[0009]步骤四：根据惯导输出的轨迹点来筛选候选路段，假设惯导在t时刻的输出点为ot，以其为圆心，半径r画圆，将圆内的路段或一部分在圆内的路段选为候选路段；
[0010]步骤五：计算定位点到每个候选路段的距离，计算方法为求解定位点到候选路段两个端点的距离以及垂足的距离，若垂足在候选路段的延长线上，则不予计算，记定位点到每个候选路段的最短距离为集合D{d1,d2
……
dn}，n为该定位点所对应的候选路段条数；
[0011]步骤六：定义初始分布概率为F1，计算方法为F1＝1/d
i
(i＝1,2
…
n)；
[0012]步骤七：定义转移概率为自动驾驶汽车从时刻t到时刻t+1从路段q
i
移动到路段q
j
的可能性，记为F2,计算方法为：当两个定位点观测值对应同一匹配路段时：
[0013]当两个定位点观测值直接相邻路段时：
[0014]步骤八：定义观测概率为自动驾驶汽车为t时刻在候选路段q
j
中观测到定位点o
t
的可能性，记为F3，计算方法为：
[0015]其中N表示候选路段集合中路段的数量，d(o
t
→
q
j
)指的是定位点观测值o
t
到候选路段q
j
的欧式距离；
[0016]步骤九：定义联合密度分布函数F，计算方法为：F＝F1*F2*F3，选取F最大值所对应的路段集合作为最优路径；
[0017]步骤十：在选取的最优路径上估计无人车真实位置，对比路段的两个端点，以及垂足到惯导轨迹输出点的距离，选取距离最短的点作为最终无人车的位置估计。
[0018]进一步的，所述惯导由三轴光纤陀螺仪和三轴加速度计构成，所述三轴光纤陀螺仪和三轴加速度计各轴指向相同。
[0019]进一步的，所述n系的x，y，z轴分别指向朝东，北，以及沿着重力反方向朝天。
[0020]进一步的，所述b系的x,y,z轴构成右手坐标系。
[0021]进一步的，步骤四中半径r的数值大小依据预估的惯导误差的平均值来确定。
[0022]进一步的，F1表示该定位点在第i条候选路段上的概率。
[0023]进一步的，其中D
mean
代表候选路段集合中路段的平均长度，d(o
t
→
o
t+1
)
mean
代表相邻两个定位点观测值之间的平均距离，定位点观测值是惯导输出的点对应到候选路段上的匹配点，即为候选路段的端点或者垂足。
[0024]与现有技术相比，本专利技术具备以下有益效果：
[0025]本专利技术提出了一种基于概率优化计算的惯导和矢量地图的组合定位方法，利用光纤惯导位置更新结果与矢量地图进行地图匹配，能够实现卫星信号不可用条件下自动驾驶汽车的车道级高精度定位，能够实现在GPS拒止条件下的无人车高精度定位，通过预估惯导的定位误差来预选候选路段，有效提升了筛选效率。
附图说明
[0026]图1为本专利技术的流程框图；
[0027]图2为本专利技术候选路段的筛选示意图。
具体实施方式
[0028]本专利技术在于提供一种基于概率优化计算的惯导和地图组合定位方法，目前随着车载传感器的开发和大规模生产，无人驾驶技术已成为研究热点，这意味着车辆可以根据自己对周围环境条件的感知和理解自行进行运动控制，并达到人类驾驶员的水平。为了确定
车辆在环境中的位置，定位模块在自主驾驶中起着非常重要的作用。传统的惯性导航方法，其典型组合之一是IMU(惯性测量单元)和里程计，可以在GNSS(全球导航卫星系统)拒止的环境中实现高精度定位，但长时间工作会导致误差累积。为了解决这个问题，利用其他信息辅助惯性的综合定位方法相继被提出。常见的传感器包括激光雷达、照相机和毫米波雷达。这些方案很容易受到环境的影响，导致定位结果的误差较大。本专利技术解决现有组合定位方案在卫星信号不可用条件下定位误差大的问题。本专利技术提出了一种基于概率优化计算的惯导和矢量地图的组合定位方法，利用光纤惯导位置更新结果与矢量地图进行地图匹配，能够实现卫星信号不可用条件下自动驾驶汽车的车道级高精度定位。
[0029]实施例1
[0030]参照图1至图2，一种基于概率优化计算的惯导和地图组合定位方法，方法定义当地地理坐标系为导航系，记为n系，其x，y，z轴分别指向朝东，北，以及沿着重力反方向朝天；所述惯导由三轴光纤陀螺仪和三轴加速度计构成；定义惯导坐标系为本体系，记作b系，其x,y,z轴构成右手坐标系；三轴光纤陀螺仪和三轴加速度计各轴指向相同。下面结合实施例对本专利技术作进一步说明。
[0031]方法具体包括如下步骤：
[0032]步骤一：通过惯导的输出轨迹和所处环境的地图进行匹配定位，流程如图1所示；
[0033]步骤二：记惯导输出的轨迹为本文档来自技高网...

【技术保护点】

【技术特征摘要】
1.一种基于概率优化计算的惯导和地图组合定位方法，其特征在于，包括如下步骤：步骤一：通过惯导的输出轨迹和本地矢量地图进行匹配定位，将当地地理坐标系标记为导航系，记为n系，将惯导坐标系标记为本体系，记为b系；步骤二：标记惯导输出的轨迹为点的集合为O{o1,o2,o3
…
on}，其中，o1，o2
……
on为单个点；步骤三：标记矢量地图为路段的集合为Q{q1,q2,q3
……
qn}，q1,q2
……
qn为单个路段；步骤四：根据惯导输出的轨迹点来筛选候选路段，假设惯导在t时刻的输出点为ot，以其为圆心，半径r画圆，将圆内的路段或一部分在圆内的路段选为候选路段；步骤五：计算定位点到每个候选路段的距离，计算方法为求解定位点到候选路段两个端点的距离以及垂足的距离，若垂足在候选路段的延长线上，则不予计算，记定位点到每个候选路段的最短距离为集合D{d1,d2
……
dn}，n为该定位点所对应的候选路段条数；步骤六：定义初始分布概率为F1，计算方法为F1＝1/d
i
(i＝1,2
…
n)；步骤七：定义转移概率为自动驾驶汽车从时刻t到时刻t+1从路段q
i
移动到路段q
j
的可能性，记为F2,计算方法为：当两个定位点观测值对应同一匹配路段时：当两个定位点观测值直接相邻路段时：步骤八：定义观测概率为自动驾驶汽车为t时刻在候选路段q
j
中观测到定位点o
t
的可能性，记为F3，计算方法为：其中N表示候选路段集合中路段的数量，d(o
t
→...

【专利技术属性】
技术研发人员：于振华，孟利平，曹中心，丁国良，李坤，赵渊，
申请(专利权)人：无锡恺韵来机器人有限公司，
类型：发明
国别省市：