[發明專利]基于航位推算的導航定位方法、設備及存儲介質

有效

• 申請號: 202010612025.2
• 申請日: 2020-06-30
• 公開（公告）號: CN111562603B
• 公開（公告）日: 2022-10-04
• 發明（設計）人: 王振飛
• 申請（專利權）人: 深圳摩吉智行科技有限公司
• 主分類號: G01S19/45
• 分類號: G01S19/45;G01S19/47;G01C21/28
• 代理公司: 深圳協成知識產權代理事務所(普通合伙) 44458
• 代理人: 章小燕

1.一種基于航位推算的導航定位方法，其特征在于，所述基于航位推算的導航定位方法包括：

實時采集車輛運行過程中的車載信息，基于采集的所述車載信息，利用姿態算法，計算實時姿態角，得到方向余弦矩陣；

利用所述車載信息，判斷車載診斷系統OBD中里程速度的可信度，并根據判斷結果獲取對應的車輛速度；

利用位置精度因子和可見衛星數，判斷GPS輸出定位信息的可信度；

根據所述GPS接收到的導航定位信息的可信度，并結合所述方向余弦矩陣以及獲取的所述車輛速度，實時計算并獲取所述車輛的定位信息，其中，若所述GPS接收到的所述導航定位信息不可信，則利用所述方向余弦矩陣和所述車輛速度，實時計算并獲取所述車輛的定位信息；

所述若所述GPS接收到的所述導航定位信息不可信，則利用所述方向余弦矩陣和所述車輛速度，實時計算并獲取所述車輛的定位信息，包括：

當所述車輛速度采用加速度計獲取得到對應的速度信息時，滿足：

且V_t＝V_t-1+a_nΔt；

其中，gⁿ＝[0 0 g]，g為當地重力加速度；a_n為當前時刻車輛加速度信息；V_t-1為所述車輛前一時刻對應的速度信息；V_t為獲取的當前時刻對應的車輛速度；為所述方向余弦矩陣，地理坐標系記為n系，車輛載體坐標系記為b系；

當所述車輛速度采用車載診斷系統OBD中的里程速度時，滿足：

其中，所述V_OBD為所述車載診斷系統OBD的里程速度；

根據獲取得到的所述車輛速度V_t，計算得到所述車輛的定位信息P_t滿足：

P_t＝P_t-1+M_pv(v_t-1+v_t)Δt/2；

其中，P_t為當前時刻位置信息，包括經度、維度和高度；P_t-1為前一時刻位置信息；h為高度信息；R_MD和R_ND分別為利用地理信息計算的子午圈和卯酉圈對應的主曲率半徑。

2.如權利要求1所述的基于航位推算的導航定位方法，其特征在于，所述實時采集車輛運行過程中的車載信息，基于采集的所述車載信息，利用姿態算法，計算實時姿態角，得到方向余弦矩陣，包括：

實時采集車輛運行過程中包含GPS信號信息、加速度計信息和陀螺儀信息的車載信息；

基于獲取的所述車載信息，采用姿態算法，根據初始姿態角計算余弦矩陣的初始值，并利用陀螺儀實時更新的數據，計算實時姿態角，獲取地理坐標系與車輛載體坐標系之間的方向余弦矩陣。

3.如權利要求2所述的基于航位推算的導航定位方法，其特征在于，所述基于獲取的所述車載信息，采用姿態算法，根據初始姿態角計算余弦矩陣的初始值，并利用陀螺儀實時更新的數據，計算實時姿態角，獲取地理坐標系與車輛載體坐標系之間的方向余弦矩陣，包括：

基于獲取的所述車載信息中的航向角信息，對GPS輸出的航跡角進行初始化，得到其中，為航向角，GPS_heading為GPS輸出的航跡角信息；

利用獲取的所述車載信息中的加速度計信息，對GPS輸出的俯仰角和橫滾角進行初始化，即：

其中，為橫滾角，為俯仰角，f_i^b(i＝x,y,z)為所述車載信息中包含的三軸加速度計輸出的相應信息；

對所述初始姿態角進行初始化后，根據所述初始姿態角計算方向余弦矩陣利用所述車載信息中的陀螺儀信息實時更新所述方向余弦矩陣計算得到實時姿態角，即：

其中，為三軸陀螺儀對應輸出的信息，Δt為所述陀螺儀對應的數據采樣時間間隔；地理坐標系記為n系，車輛載體坐標系記為b系。