[发明专利]高精度定位方法、装置、设备及可读存储介质

说明书

本发明提供一种高精度定位方法、装置、设备及可读存储介质。该方法包括：获取自车当前时刻的观测状态向量以及自车上一时刻的融合优化后的状态向量，其中，所述状态向量包括自车的坐标、航向角和/或速度；根据自车上一时刻的融合优化后的状态向量得到自车当前时刻的预测状态向量；将自车当前时刻的观测状态向量与自车当前时刻的预测状态向量进行融合，得到自车当前时刻的融合优化后的状态向量。通过本发明，解决了高精度惯导实现车道级定位的成本高且稳定性和精度较低的问题。

技术领域

本发明涉及汽车电子技术领域，尤其涉及一种高精度定位方法、装置、设备及可读存储介质。

背景技术

随着汽车数量的不断增加，道路安全问题越来越不容忽视。因此，高级驾驶辅助系统已成为研究热点。在众多高级驾驶辅助系统应用中，车辆的车道级定位非常重要。

现有技术中，采用高精度惯导以及载波相位差分技术(RTK)设备能实现车道级定位，但是高精度惯导成本高，并且在长隧道中或高架桥下或信号较弱的地方，高精度惯导无法实现实现车道级定位，因此，高精度惯导还具有受道路地形的影响，实现车道级定位的稳定性和精度较低的缺陷。

发明内容

本发明的主要目的在于提供一种高精度定位方法、装置、设备及可读存储介质，旨在解决高精度惯导实现车道级定位的成本高且稳定性和精度较低的问题。

第一方面，本发明提供一种高精度定位方法，所述高精度定位方法包括：

获取自车当前时刻的观测状态向量以及自车上一时刻的融合优化后的状态向量，其中，所述状态向量包括自车的坐标、航向角和/或速度；

根据自车上一时刻的融合优化后的状态向量得到自车当前时刻的预测状态向量；

将自车当前时刻的观测状态向量与自车当前时刻的预测状态向量进行融合，得到自车当前时刻的融合优化后的状态向量。

可选的，所述获取自车当前时刻的观测状态向量，包括：

根据全球卫星导航系统GNSS获取自车当前时刻的观测状态向量；

或，根据自车当前时刻的视觉识别车道线信息和高精度地图车道线信息获取自车当前时刻的观测状态向量。

可选的，所述根据自车当前时刻的视觉识别车道线信息和高精度地图车道线信息获取自车当前时刻的观测状态向量，包括：

根据自车当前时刻的视觉识别车道线信息和高精度地图车道线信息确定自车当前时刻的观测坐标；

确定自车当前时刻的观测坐标后，分别在自车所处车道线左、右边线以及车道中心线上获取自车观测坐标前后各5个点，其中，自车所处车道线位置即为高精度地图车道线信息对应的车道线位置；

分别对车道线左、右边线以及中心线上取的点进行5次多项式拟合，得到3个多项式；

分别根据3个多项式计算得到3个车道线航向角，求取3个车道线航向角的平均值，所述平均值即为高精度地图车道线的航向角；

通过视觉控制器获取自车与视觉车道线的相对夹角；

计算高精度地图车道线的航向角和自车与视觉车道线的相对夹角之和，得到自车当前时刻的观测航向角；

根据自车上一时刻的融合优化后的速度确定自车当前时刻的观测速度。

可选的，所述根据自车上一时刻的融合优化后的状态向量得到自车当前时刻的预测状态向量的步骤包括：

根据自车上一时刻的融合优化后的坐标、航向角和/或速度通过扩展卡尔曼滤波算法第一递推公式得到自车当前时刻的预测状态向量，第一递推公式如下：