[發明專利]一種基于多傳感器數據融合的無人車定位方法

有效

• 申請號: 201910202376.3
• 申請日: 2019-03-18
• 公開（公告）號: CN109946732B
• 公開（公告）日: 2020-12-01
• 發明（設計）人: 李子月
• 申請（專利權）人: 李子月
• 主分類號: G01S19/48
• 分類號: G01S19/48;G01C21/16;G01C21/00
• 代理公司: 杭州求是專利事務所有限公司 33200
• 代理人: 邱啟旺

1.一種基于多傳感器數據融合的無人車定位方法，其特征在于，包括如下步驟：

步驟(1)通過衛星導航接收機輸出的定位數據，實時監測數據有效性，當出現PDOP5、可視星數目小于等于4兩種情況中任意一種時，判別為衛星定位數據可用性下降；

步驟(2)當衛星定位數據可用性下降時，無人車進行制動，制動過程中，通過里程計和慣導數據融合，對定位位置進行更新，車輛靜止后對慣導設備的俯仰角誤差、橫滾角誤差、航向角誤差、加速度計和陀螺儀器件常值誤差進行修正；

步驟(3)利用激光雷達和視覺傳感器分別獲取道路邊界和車道線在車體坐標系中的二次函數表示，計算車頭方向相對于厘米級高精度地圖中對應目標點航向的偏移角，并據此求取基于厘米級高精度地圖的車輛航向角，對步驟(2)中的航向角進行更新；

步驟(4)通過激光雷達、視覺傳感器、厘米級高精度地圖對無人車航向、俯仰、橫滾、定位信息更新后，無人車恢復運行，通過里程計和慣導數據融合進行實時位置更新，同時激光雷達、視覺傳感器、厘米級高精度地圖數據融合獲取車輛航向，對慣導航向角參數進行修正；

所述步驟(2)中車輛靜止后對慣導設備的俯仰角誤差、橫滾角誤差、航向角誤差、加速度計和陀螺儀器件常值誤差進行修正，具體過程如下：

車輛靜止后真實的速度為0，慣導設備輸出的速度即為誤差，并以此建立誤差模型，利用kalman濾波器對姿態失準角、水平速度誤差、加速度計和陀螺儀器件常值進行估計，狀態向量X如下所示：

式中，φ_E、φ_N、φ_U分別為俯仰、橫滾和航向失準角，δV_E、δV_N分別為東向、北向速度誤差，ε_x、ε_y、ε_z分別為三個陀螺儀常值誤差，分別為三個加速度計常值誤差；

姿態失準角和速度誤差對應的狀態方程如下：

公式(2)-(6)中，ω_ie為地球自轉角速度，L為緯度，V_E為東向速度，R_M為子午圈曲率半徑、R_N為卯酉圈曲率半徑，V_N為北向速度，h為高度，f_i(i＝E,N,U)為加速度計輸出的比力在導航系下的投影；

T為姿態矩陣，具體表達式如下：

其中γ為橫滾角，φ為航向角，θ為俯仰角；

所述步驟(3)具體計算過程如下：

1)利用視覺傳感器獲取當前車道線的二次函數表示，并投影到車體坐標系O-XY平面，計算二次函數和X軸交點的切線斜率k₀，進而計算得到切線和Y軸夾角β₀；

2)利用激光雷達掃描周邊環境，提取出邊界特征點，然后投影到車體坐標系O-XY平面，剔除野值點，并利用最小二乘算法擬合出道路邊界在二維平面中的二次函數表示，計算該二次函數和的交點坐標A₀(x₀,y₀)，該點切線和Y軸夾角β₁；

3)根據夾角β₁和厘米級高精度地圖對應坐標點航向可計算基于定位和厘米級高精度地圖的當前航向角，公式如下：

式中，表示基于定位和厘米級高精度地圖的車輛航向。