[發明專利]基于多傳感器融合的智能網聯汽車復合定位方法及裝置

在審

• 申請號: 202211343799.5
• 申請日: 2022-10-31
• 公開（公告）號: CN115540882A
• 公開（公告）日: 2022-12-30
• 發明（設計）人: 王龐偉;劉程;俞宏勝;張名芳;葉榮盛
• 申請（專利權）人: 北方工業大學
• 主分類號: G01C21/30
• 分類號: G01C21/30;G01S17/86;G01S19/48;G06F17/16;G08G1/01
• 代理公司: 北京德崇智捷知識產權代理有限公司 11467
• 代理人: 房婉瓊

1.一種基于多傳感器融合的智能網聯汽車復合定位方法，其特征在于，該方法包括如下步驟：

步驟1：智能網聯汽車狀態定義

激光雷達與攝像機分別為單個局部濾波器，定義每個局部濾波器定位智能網聯汽車的狀態如下：

X(t)＝[δx,δy,δv_x,δv_y]^T (1)

其中，δx,δy為智能網聯汽車橫縱向位置偏差；δv_x,δv_y為智能網聯汽車橫縱向速度偏差，其標準狀態方程如下：

其中，f(·)是具有離散形式的非線性狀態轉移函數；w(k)為零均值高斯白噪聲，假設其協方差為Q(k)；B(t)u(t)為局部濾波器的控制輸入，為獲取智能網聯汽車的位置與速度的觀測值，構建激光雷達與攝像機的量測模型，如下：

其中，Z_C(k)表示攝像機的量測模式，Z_L(k)表示激光雷達的量測模式；x_m、y_m、v_xm與v_ym分別表示由高精地圖獲取的智能網聯汽車位置與速度數據；x_Camera、y_Camera、v_xCamera與v_yCamera分別表示由攝像機獲取的智能網聯汽車位置與速度數據；x_Lidar、y_Lidar、v_xLidar與v_yLidar分別表示由激光雷達獲取的智能網聯汽車位置與速度數據；

步驟2：基于無跡卡爾曼濾波的智能網聯汽車定位數據融合

初始化激光雷達與攝像機代表的局部濾波器X_s，如公式(4)所示。

其中，表示局部濾波器X_s的初始均值；P_s(0)表示局部濾波器X_s的初始方差；

分別獲取激光雷達與攝像機定位智能網聯汽車的結果作為觀測值，通過無跡變換得到k-1時刻2n+1個σ點的狀態，如公式(5)所示。定義無跡變換采用的參數如公式(6)所示。

其中，分別表示第0個和第s個σ點的狀態；α是正極小量；是通過矩陣P_s(k-1)的下三角分解獲得的平方根的第j列，其向量維數為n。

在k時刻計算下一步復合定位方法的預測結果，如公式(7)所示。

通過復合定位方法預測的結果更新激光雷達與攝像機傳感器的量測結果，如公式(8)所示。更新方程中權重值定義如公式(9)所示。

其中，H_s(k)表示測量矩陣；表示局部濾波器X_s第k次觀測的中間變量；表示局部濾波器X_s第k次觀測結果；表示局部濾波器X_s第k次觀測結果的協方差；R_s(k)表示系統噪聲；

最后計算各局部濾波器的最優狀態估計和協方差矩陣并將每個局部濾波器的結果輸入至補償融合層，如公式(10)所示。

其中，K_s(k)表示卡爾曼濾波增益；

步驟3：智能網聯汽車定位數據融合補償

智能網聯汽車安裝的GNSS接收機實時獲取經緯度數據，并分別計算激光雷達、攝像機傳感器與接收機的相對定位誤差平均值，將高精度地圖下智能網聯汽車的位置數據P_m＝[x_m,y_m,v_xm,v_ym]作為局部濾波器的參考，分別與激光雷達與攝像機的觀測結果做差，獲取路側定位平臺中單傳感器定位智能網聯汽車的偏差量，如公式(11)所示。

其中，ΔY＝[Δx,Δy,Δv_x,Δv_y]^T為激光雷達與攝像機分別定位車輛坐標的狀態誤差補償值；分別為激光雷達與攝像機復合定位智能網聯汽車的結果；為智能網聯汽車的經緯度數據；

采用均值理論處理所有局部濾波器的結果，如公式(12)所示。

其中，與表示第1個與第2個濾波器結果，在此分別表示激光雷達與攝像機對智能網聯汽車的量測結果；表示補償前智能網聯汽車的位置與速度狀態，補償方法如公式(13)、(14)所示。

Y^*_Sensors＝[x^*,y^*,v^*_x,v^*_y]^T (13)

其中，Y^*_Sensors表示補償后智能網聯汽車狀態；x^*,y^*,v^*_x,v^*_y表示補償后的智能網聯汽車期望橫縱向位置與速度；

步驟4：結合路側定位平臺經緯度數據的智能網聯汽車定位聯合優化

將補償后的傳感器定位數據與路側定位平臺的經緯度數據相結合，通過UKF進行異構優化。聯合優化層中智能網聯汽車狀態向量X'(t)與離散的時間模型X'(k)如式(14)、(15)所示。

X'(t)＝[δx',δy',δv'_x,δv'_y]^T (14)

X′(k)＝f′(X′(k-1))+B′(k-1)u′(k-1)+w′(k-1) (15)

其中，X'(t)表示聯合優化層中智能網聯汽車的狀態；與局部濾波器的量測類似，狀態向量表達式中的f′(·)是具有離散形式的非線性狀態轉移函數；w′(k-1)為零均值高斯白噪聲，假設其協方差為Q(k-1)；B′(k-1)u′(k-1)為局部濾波器的控制輸入；聯合優化層的量測模型如公式(16)、(17)所示。

Z′(k)＝H′(k)X′(k)+v′(k) (17)

其中，Z′(k)表示聯合優化層智能網聯汽車的量測模式，測量矩陣H'(k)為diag([ones(1,3),R_M,R_NcosL,1])；R_M，R_N，L分別表示智能網聯汽車所在的中值半徑、法向半徑、緯度，v′(k)表示系統噪聲；

其次，將由公式(15)、(17)組成的狀態空間模型代入步驟2中以獲得全局最優狀態估計相應智能網聯汽車復合定位結果如公式(18)所示。

其中，表示測量的定位結果。

步驟5：局部坐標系與大地坐標系變換

將局部坐標系下智能網聯汽車的相對定位結果變換為大地坐標下的定位結果，如公式(19)所示。

p_wc＝R^r-＞w·p_car+c^r-＞w (19)

其中，以路側定位平臺為圓心的局部坐標系與大地坐標系間的旋轉矩陣及平移矩陣分別為R^r-＞w、c^r-＞w；目標車輛的局部坐標為p_car＝(x^car,y^car)；智能網聯汽車在大地坐標系中的坐標為p_wc＝(x^w_car,y^w_car)；

最后，再將復合定位系統輸出的數據與車載端獲取的經緯度數據作差分，得到智能網聯汽車位置差分數據Y_difference，具體表達式如式(20)所示。

步驟6：路側定位平臺與車載平臺V2X通信

路側定位平臺的RSU將智能網聯汽車的經緯度數據發送至車載平臺的OBU；

步驟7：智能網聯汽車經緯度數據修正與濾波

智能網聯汽車定位數據來源包括t時刻路側定位平臺通過V2X發送的經緯度數據以及車載GNSS接收機采集到的經緯度數據本發明設計了濾波算法以去除異常的經緯度值，包含以下兩個約束。

其中，Xⁿ、Yⁿ分別表示交叉口區域內所有可能取到的經緯度數值。

經緯度異常值被過濾后，通過兩個不同的權重值融合多源的經緯度數據，如公式(23)所示。

其中，w₁、w₂分別表示經度與緯度的權重值；x_t、y_t分別表示修正后的智能網聯汽車經緯度數據。

2.一種應用于如權利要求1所述的基于多傳感器融合的智能網聯汽車復合定位方法的裝置，其特征在于，該裝置包括

車載平臺，其將車載的擴展主板封裝；

車載GNSS接收機，其位于智能網聯汽車頂部，通過GNSS天線接收不同衛星以及基站下發的經緯度等信息，其通過RJ45接口與擴展主板連接；

地平線征程系列AI芯片，其安裝至智能網聯汽車后備箱中，用于運行復合定位方法中經緯度數據修正以及濾波部分，通過IO與擴展主板相連；

車載單元OBU，其位于車內后備箱中，作為車路通信的車端設備，用于發送與接收經緯度數據，通過串口連接至擴展主板；

路側單元RSU，其位于燈桿側邊，作為車路通信的路側端設備，用于發送與接收經緯度數據，通過串口連接至擴展主板；

地平線旭日系列AI芯片，其架設于城市交叉口中，用于運行復合定位方法中數據融合以及深度學習方法部分，定位感知區域內的智能網聯汽車，通過IO與擴展主板相連；

視頻傳感器和激光雷達傳感器，其分別位于燈桿頂部的防雨罩下，通過RJ45接口與擴展主板連接，用于獲取交叉口中智能網聯汽車的原始點云與圖像數據。