一种考虑道路几何约束的地面无人车辆定位方法技术

本发明专利技术公开了一种考虑道路几何约束的地面无人车辆定位方法，包括以下步骤：首先对道路几何约束进行离线建模，利用搭载高精度传感器的采集车对道路信息进行采集，融合多个传感器的观测信息，利用拓展卡尔曼平滑算法进行平滑处理，对平滑处理后的数据进行重采样并利用三次B样条曲线进行拟合，将其作为道路几何约束；在线定位时，利用自适应卡尔曼滤波算法获得初步定位结果，借助牛顿法求解初步定位结果距离道路约束曲线最近的投影点，将其作为修正后的定位结果。本发明专利技术可以提高GNSS信号不佳时的地面无人车辆定位精度。的地面无人车辆定位精度。的地面无人车辆定位精度。

【技术实现步骤摘要】
一种考虑道路几何约束的地面无人车辆定位方法


[0001]本专利技术属于智能交通领域，具体涉及一种考虑道路几何约束的地面无人车辆定位方法。

技术介绍

[0002]地面无人驾驶车辆被认为在改善道路安全、提高交通运行效率等方面有着巨大的潜力，近年来得到了广泛关注与研究。目前利用RTK
‑
GNSS和高精度INS进行组合导航定位的无人车辆在理想工况下可以实现满足行驶要求的厘米级定位。但是高精度INS价格十分昂贵，同时在城市环境下，高大建筑物遮挡GNSS信号时，组合导航的定位精度变差，不足以保证车辆的安全行驶。

技术实现思路

[0003]针对现有技术的不足，本专利技术提供一种考虑道路几何约束的地面无人车辆定位方法。
[0004]本专利技术的目的是通过以下技术方案来实现的：
[0005]一种考虑道路几何约束的地面无人车辆定位方法，包括，首先道路几何约束建模，然后考虑道路几何约束的在线定位。
[0006]进一步的，包括以下步骤：首先对道路几何约束建模，利用搭载高精度传感器的采集车对道路信息进行采集，融合多个传感器的观测信息，利用拓展卡尔曼平滑算法进行平滑处理，对平滑处理后的数据进行重采样并利用三次B样条曲线进行拟合，将其作为道路几何约束；在线定位时，利用自适应卡尔曼滤波算法获得初步定位结果，借助牛顿法求解初步定位结果距离道路约束曲线最近的投影点，将其作为修正后的定位结果。
[0007]具体的，包括以下步骤：
[0008](1)道路几何约束离线建模，具体包括：<br/>[0009]利用搭载RTK
‑
GPS、IMU、轮速传感器的试验车采集道路信息，RTK
‑
GPS在信号良好的情况下提供厘米级全局定位结果，同时利用IMU和轮速传感器提供的车辆运动信息进行局部航迹推算，以校正RTK
‑
GPS信号不良时的定位结果。对采集得到的RTK
‑
GPS、IMU及轮速信息，离线利用离散拓展卡尔曼平滑算法进行平滑处理。
[0010]k时刻系统状态x
k
可以表示成上一时刻系统状态x
k
‑1、系统输入u
k
‑1及系统模型噪声w
k
‑1的非线性函数：
[0011]x
k
＝f
k
‑1(x
k
‑1,u
k
‑1,w
k
‑1)
[0012]具体的：
[0013][0014]式中，ψ
k
、ψ
k
‑1、X
k
、X
k
‑1、Y
k
、Y
k
‑1分别为k时刻及k
‑
1时刻车辆的航向、横向位置、纵向
位置；T为时间间隔；r、V分别为车辆的横摆角速度、速度；系统模型噪声w
k
‑1服从高斯分布w
k
‑1～(0,Q
k
‑1)。
[0015]k时刻系统状态的量测模型可以表示为：
[0016]y
k
＝H
k
x
k
+v
k
[0017]其中，y
k
为系统状态的量测值；H
k
为观测矩阵、v
k
为观测噪声，其服从高斯分布v
k
～(0,R
k
)。
[0018]具体的：
[0019][0020]式中，分别为GPS传感器关于航向、横向位置、纵向位置的测量值。
[0021]执行离散拓展卡尔曼平滑，得到平滑处理后行驶轨道的定位信息。以起始出发处为参考原点，将位置坐标从WGS
‑
84坐标系转换成NED坐标系，然后将位置坐标p
i
＝(x
i
,y
i
)转化成表示相对于参考原点位移的相对坐标(0,s
i
)。s
i
可以由下式计算得到：
[0022][0023]考虑到采集车辆在采集数据时不可能保持匀速运动，因此对相对坐标(0,s
i
)以0.1m为固定间隔进行重新采样，并利用三次B样条曲线进行拟合，得到一条可以描述道路几何结构的参数化曲线。
[0024]步骤2、考虑道路几何约束的在线定位，具体包括：
[0025]基于自适应卡尔曼滤波算法对车载RTK
‑
GPS、IMU、轮速传感器获取的信息进行融合处理，得到初步无约束定位结果自适应卡尔曼滤波算法包含预测、更新以及噪声自适应调整三个阶段，基于k
‑
1时刻的系统后验状态估计及协方差矩阵估计对k时刻的系统状态进行最优估计，并自适应调整k时刻噪声的协方差矩阵，其表达式为：
[0026][0027][0028][0029][0030][0031][0032][0033]式中，是k时刻系统的先验估计，是k
‑
1时刻系统的后验估计，A
k
‑1、B
k
‑1、u
k
‑1分别是k
‑
1时刻系统的状态转移矩阵、输入矩阵、输入向量，P
k
是k时刻系统先验估计的协方
差矩阵，是k
‑
1时刻系统后验估计的协方差矩阵，C
k
是k时刻系统观测矩阵，及分别是k
‑
1时刻系统噪声和观测噪声的后验估计，y
k
是k时刻的观测输入，分别是k时刻系统状态的后验估计及其协方差矩阵，分别是k时刻系统噪声和观测噪声的后验估计，分别是历元k的新息协方差矩阵和残差序列的协方差矩阵。
[0034]车辆行驶时道路几何结构施加在车辆状态x
k
上的约束信息可以表示为：
[0035]x
k
∈C
k
[0036]式中，C
k
是车辆系统状态需要满足的约束集，即步骤(1)中建立的表征道路几何结构的曲线。
[0037]将初步定位结果向表征道路几何约束的曲线上进行投影，利用牛顿法找到初步定位结果距离道路约束曲线最近的投影点，得出考虑道路约束的定位结果即：
[0038][0039]本专利技术的有益效果是：(1)考虑地面无人车辆在行驶时受到道路几何结构约束这一个特性，提高地面无人车辆在GNSS信号不佳时的定位精度。
附图说明
[0040]图1为本专利技术的道路几何约束离线建模流程图；
[0041]图2为本专利技术的考虑道路几何约束的在线定位示意图。
具体实施方式
[0042]下面结合附图进一步详细描述本专利技术的技术方案，但本专利技术的保护范围不局限于以下所述。
[0043]本专利技术提出的一种考虑道路几何约束的地面无人车辆定位方法，包括两个步骤：道路几何约束离线建模、考虑道路几何本文档来自技高网...

【技术保护点】

【技术特征摘要】
1.一种考虑道路几何约束的地面无人车辆定位方法，其特征在于，包括，首先道路几何约束建模，然后考虑道路几何约束的在线定位。2.根据权利要求1所述的一种考虑道路几何约束的地面无人车辆定位方法，其特征在于，包括以下步骤：首先对道路几何约束建模，利用搭载高精度传感器的采集车对道路信息进行采集，融合多个传感器的观测信息，利用拓展卡尔曼平滑算法进行平滑处理，对平滑处理后的数据进行重采样并利用三次B样条曲线进行拟合，将其作为道路几何约束；在线定位时，利用自适应卡尔曼滤波算法获得初步定位结果，借助牛顿法求解初步定位结果距离道路约束曲线最近的投影点，将其作为修正后的定位结果。3.根据权利要求1或2所述的一种考虑道路几何约束的地面无人车辆定位方法，其特征在于，所述的道路几何约束建模，具体包括：利用搭载RTK
‑
GPS、IMU、轮速传感器的试验车采集道路信息，RTK
‑
GPS在信号良好的情况下提供厘米级全局定位结果，同时利用IMU和轮速传感器提供的车辆运动信息进行局部航迹推算，以校正RTK
‑
GPS信号不良时的定位结果；对采集得到的RTK
‑
GPS、IMU及轮速信息，离线利用离散拓展卡尔曼平滑算法进行平滑处理；k时刻系统状态x
k
表示成上一时刻系统状态x
k
‑1、系统输入u
k
‑1及系统模型噪声w
k
‑1的非线性函数：x
k
＝f
k
‑1(x
k
‑1,u
k
‑1,w
k
‑1)具体的：式中，ψ
k
、ψ
k
‑1、X
k
、X
k
‑1、Y
k
、Y
k
‑1分别为k时刻及k
‑
1时刻车辆的航向、横向位置、纵向位置；T为时间间隔；r、V分别为车辆的横摆角速度、速度；系统模型噪声w
k
‑1服从高斯分布w
k
‑1～(0,Q
k
‑1)；k时刻系统状态的量测模型表示为：y
k
＝H
k
x
k
+v
k
其中，y
k
为系统状态的量测值；H
k
为观测矩阵、v
k
为观测噪声，其服从高斯分布v
k
～(0,R
k
)；具体的：式中，分别为GPS传感器关于航向、横向位置、纵向位置的测量值；执行离散拓展卡尔曼平滑，得到平滑处理后行驶轨道的定...

【专利技术属性】
技术研发人员：蒋朝阳，盛树轩，徐言杰，王星琦，荆崇波，
申请(专利权)人：北京理工大学，
类型：发明
国别省市：