[發明專利]一種結合航位推算和多車道路網地圖的車輛定位方法

有效

• 申請號: 202011531431.2
• 申請日: 2020-12-22
• 公開（公告）號: CN112747744B
• 公開（公告）日: 2022-11-18
• 發明（設計）人: 項志宇;王天樂;錢琦;張至可
• 申請（專利權）人: 浙江大學
• 主分類號: G01C21/16
• 分類號: G01C21/16;G01C21/14;G01C21/28
• 代理公司: 杭州求是專利事務所有限公司 33200
• 代理人: 林超

1.一種基于航位推算和多車道路網地圖的車輛定位方法，其特征在于，包括如下步驟：

(1)建立由路網節點、邊和包含各車道中心位置的多車道路網地圖，給定車輛行駛起點在多車道路網地圖中的起點位置范圍以及起始航向角范圍；

(2)通過航位推算方法實時獲得當前車輛相對于車輛行駛起點的位置和車輛行駛軌跡，并進行變道檢測獲得變道錨點，同時進行道路錨點檢測獲得道路錨點，道路錨點和變道錨點共同構成了錨點，將各錨點相連形成初步分段軌跡；根據道路錨點的實時檢測結果確定多車道路網地圖的起點位置與起始航向角；

(3)在確定起點位置與起始航向角后，對于后續的車輛行駛軌跡進行實時錨點檢測，利用多位置聯合濾波對錨點進行修正，并且將車輛行駛軌跡的其余位置投影到多車道路網地圖上完成軌跡修正。

2.根據權利要求1所述的一種基于航位推算和多車道路網地圖的車輛定位方法，其特征在于：所述步驟(1)中，用包含路網節點、邊以及其對應車道信息的拓撲圖結構來構建多車道路網地圖：以路網地圖中的路口中心點、彎曲道路上的間隔分段點為路網節點，以路網節點間連接的道路中心線作為邊；路網節點n＝(lat,lon)的位置由經緯度表示，lat、lon分別表示緯度、經度，邊e＝(n₁,n₂,lane)表示路網節點n₁和路網節點n₂之間的連接關系，每個邊包含的道路信息由lane＝(head,num,length)表示，head、num和length分別表示兩個路網節點之間的道路中車道的朝向、車道序號和車道的橫向長度。

3.根據權利要求1所述的一種基于航位推算和多車道路網地圖的車輛定位方法，其特征在于：所述步驟(2)中，通過航位推算方法實時得到當前車輛初始位置，利用變道前后的車輛的航向角變化、橫向距離變化與時間變化來進行變道檢測，并在變道處設置一個變道錨點。

4.根據權利要求1所述的一種基于航位推算和多車道路網地圖的車輛定位方法，其特征在于：在步驟(2)進行道路錨點檢測時：

當由拐彎產生的第一個道路錨點被檢測到時，根據車輛行駛起點的起點位置范圍與起始航向角范圍推算第一個道路錨點的位置范圍，在第一個錨點的位置范圍內的各個路網節點上均使用粒子堆；

當檢測得到由拐彎產生的后續道路錨點后，通過粒子濾波確定第一個道路錨點對應的路網節點，然后在起點位置范圍內的各車道中心線上沿自身均勻生成粒子，再與第一個道路錨點和第二個道路錨點進行聯合粒子濾波，選取權重最大的粒子作為多車道路網地圖中的起點位置，并根據道路方向確定多車道路網地圖中的起始航向角。

5.根據權利要求4所述的一種基于航位推算和多車道路網地圖的車輛定位方法，其特征在于：在步驟(2)中的聯合粒子濾波過程中，粒子按照以下初始化：

(1)在當前道路錨點有對應的一個或者多個路網節點時，

若路網節點為彎曲道路上的間隔分段點，則將路網節點垂直投影到所在彎曲道路的各個彎曲車道中心上的點分別作為混合高斯分布的中心，以車道的橫向長度作為方差生成粒子；

若路網節點為路口中心點，則將路網節點所在路口的各個車道中心線間的交叉點作為混合高斯分布的各個中心，以車道的橫向長度作為方差生成粒子；

(2)在當前道路錨點沒有對應的路網節點時，將當前道路錨點與上一個道路錨點分別投影到各條車道中心線上，從而在各條車道中心線上均獲得兩個投影點，每條車道中心線上的兩個投影點間按照混合高斯分布生成粒子。

6.根據權利要求1所述的一種基于航位推算和多車道路網地圖的車輛定位方法，其特征在于：所述步驟(3)中，將車輛行駛軌跡的其余位置投影到多車道路網地圖上完成軌跡修正，具體為：

根據步驟(2)獲得的各個錨點與多車道路網地圖，將車輛行駛軌跡的其余位置投影到多車道路網地圖上再依次相連作為修正后的車輛行駛軌跡，根據錨點和車輛行駛軌跡中車輛航向角的變化，確定車輛拐彎前后點的時刻，將車輛行駛軌跡劃分為拐彎段與非拐彎段，劃分同時獲得拐彎前點和拐彎后點的位置，對于非拐彎段和非拐彎段的車輛行駛軌跡采用不同的投影方式進行處理：

(A)對于非拐彎段的軌跡：

利用原始的車輛行駛軌跡以及錨點分別修正確定車輛沿道路的縱向位置與橫向位置：

(A.1)沿道路縱向位置

對于錨點AP1和錨點AP2之間的某一時刻t，通過計算該時刻t的車輛位置到錨點AP1的縱向距離以及錨點AP1到錨點AP2之間的縱向距離的比值來計算獲得時刻t定位修正后的縱向位置p_tx，然后將縱向位置p_tx垂直投影到道路上獲得車輛沿道路的縱向位置：

其中分別代表原始的車輛行駛軌跡中的錨點AP1位置、錨點AP2位置與時刻t車輛在錨點AP1和錨點AP2連線方向上的位置；

(A.2)沿道路橫向位置

若錨點為由拐彎產生的錨點，則根據在錨點附近的原始的車輛行駛軌跡與道路邊沿確定在拐彎前車輛所在的車道，并直接以該車道的車道中心線作為橫向位置；

若錨點為由變道產生的錨點，則根據每個時刻的車輛位置到變道前的位置之間的距離與每個時刻的車輛位置到變道后的位置之間的距離的比值，根據比值將車輛位置垂直投影到垂直于車道方向的直線上，作為橫向位置；

(B)對于拐彎段的軌跡：

以修正后的錨點、拐彎前點和拐彎后點位置為基準，在保持原始的車輛行駛軌跡的局部形狀情況下，將原始的車輛行駛軌跡投影到多車道路網地圖上：

先利用拐彎前點和錨點對處于拐彎前點和錨點之間的車輛行駛軌跡進行修正：根據原始的車輛行駛軌跡確定拐彎前點所在車道，并采用和(A.1)、(A.2)相同方式處理獲得拐彎前點的沿所在車道的縱向位置和沿所在車道的橫向位置，進而將拐彎前點修正到該車道中心線上，進行了拐彎前點的修正；

然后，根據修正前的拐彎前點相對于修正前的錨點的位置相比于修正后的拐彎前點相對于修正前的錨點的位置的變化對車輛行駛軌跡進行修正，具體是計算修正前的拐彎前點繞修正前的錨點旋轉、距離縮放后變換到修正后的拐彎前點相對于修正后的錨點的相對位置的轉化矩陣，將修正前的處于拐彎前點和錨點之間的車輛行駛軌跡利用轉化矩陣進行變換得到修正后的處于拐彎前點和錨點之間的車輛行駛軌跡；

最后利用拐彎后點和錨點對處于錨點和拐彎后點之間的車輛行駛軌跡進行修正，處理方式和處于拐彎前點和錨點之間的車輛行駛軌跡的處理方式相同。