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

权利要求书

1.一种高精度定位方法，其特征在于，所述高精度定位方法包括：

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

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

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

2.如权利要求1所述的高精度定位方法，其特征在于，所述获取自车当前时刻的观测状态向量，包括：

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

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

3.如权利要求2所述的高精度定位方法，其特征在于，所述根据自车当前时刻的视觉识别车道线信息和高精度地图车道线信息获取自车当前时刻的观测状态向量，包括：

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

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

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

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

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

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

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

4.如权利要求1所述的高精度定位方法，其特征在于，所述根据自车上一时刻的融合优化后的状态向量得到自车当前时刻的预测状态向量的步骤包括：

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

其中，v_t为自车上一时刻的融合优化后的速度，(x_t，y_t)为自车上一时刻的融合优化后的坐标，其中，x_t为自车上一时刻x方向融合优化后的坐标，y_t为自车上一时刻y方向融合优化后的坐标，heading_t为自车上一时的融合优化后的航向角，d_t为当前时刻与上一时刻的间隔时长，L为自车轴距，steerangle为自车方向盘转角，v_t+1为自车当前时刻的预测速度，heading_t+11为自车当前时刻的预测航向角，(x_t+1，y_t+1)为自车当前时刻的预测坐标，其中，x_t+1为自车当前时刻x方向预测坐标，y_t+1为自车当前时刻y方向预测坐标。