[發明專利]高精度定位方法、裝置、設備及可讀存儲介質

在審

• 申請號: 202111441906.3
• 申請日: 2021-11-30
• 公開（公告）號: CN114168900A
• 公開（公告）日: 2022-03-11
• 發明（設計）人: 聶文;劉杏;楊月
• 申請（專利權）人: 東風商用車有限公司
• 主分類號: G06F17/16
• 分類號: G06F17/16;G06F17/18;G06K9/62;G01D21/02;G01S19/42
• 代理公司: 湖北竟弘律師事務所 42230
• 代理人: 沈莉

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方向預測坐標。