[發明專利]里程計標度誤差標定方法、系統及計算機存儲介質

有效

• 申請號: 202011120011.5
• 申請日: 2020-10-19
• 公開（公告）號: CN112284415B
• 公開（公告）日: 2023-01-03
• 發明（設計）人: 李慧鵬;潘雄;王已熏;邵振華
• 申請（專利權）人: 株洲菲斯羅克光電科技股份有限公司
• 主分類號: G01C25/00
• 分類號: G01C25/00
• 代理公司: 長沙朕揚知識產權代理事務所(普通合伙) 43213
• 代理人: 鄧宇

1.一種里程計標度誤差標定方法，其特征在于，包括：

通過慣導和RTK組合獲得基準軌跡；

通過里程計計算載車水平軌跡；

對比基準軌跡和水平軌跡，估算出所述里程計的標度誤差；

還包括：

分別建立慣導坐標系、導航坐標系和里程儀測量坐標系，并將慣導坐標系和里程儀測量坐標系中的數據映射到導航坐標系進行基準軌跡、水平軌跡及標度誤差的求解；

在獲得基準軌跡的過程中，基于卡爾曼濾波進行最優估計；

所述基準軌跡的求解過程包括：

定義慣導載體橫滾軸為y_b軸，指向慣導正前方，俯仰軸為x_b軸，指向慣導右方，航向軸為z_b軸，沿重力方向指向朝天，x_b、y_b、z_b三個軸符合右手坐標系，將x_b、y_b、z_b記為b系；

定義導航坐標系x_n軸指向東，y_n軸指向北，z_n軸指向朝天，將x_n、y_n、z_n記為n系；

里程儀測量坐標系是一個與車體固聯的“右-前-上”右手直角坐標系并記為m系，oy_m軸在和載車車輪相接觸的地平面內，并且指向車體的正前方；oz_m軸垂直于地平面向上為正；ox_m軸指向右方；

選取系統狀態參數，將狀態量記為：

δr為慣導位置誤差，δv為慣導速度誤差，Φ是慣導姿態角誤差，是陀螺零偏，δf^b是加速度計零偏；

建立慣導姿態誤差方程為：其中，是φ的導數，為導航坐標系下陀螺測量誤差，為導航坐標系計算誤差，表示導航坐標系相對于慣導坐標系的旋轉；

為地球自轉引起的導航坐標系旋轉，為系統在地球表面附近移動因地球表面彎曲引起的導航坐標系旋轉；

速度誤差方程為：其中，vⁿ、δvⁿ分別為導航坐標系下速度及速度誤差，是δvⁿ的導數，為導航坐標系下加速度計測量值，為導航坐標系下加速度計測量誤差，δgⁿ分別為導航坐標系下地球自轉角速度計算誤差、旋轉計算誤差和重力誤差；

慣導位置誤差方程為：

L、λ和h分別表示緯度、經度和高度，δL、δλ和δh分別表示緯度誤差、經度誤差和高度誤差，和分別為δL、δλ和δh的導數，R_M為子午圈主曲率半徑；R_N為卯酉圈主曲率半徑；

記慣導速度分量vⁿ＝[v_E v_N v_U]^T；

速度誤差分量δvⁿ＝[δv_E δv_N δv_U]^T；

其中，下標E、N、U分別代表東向、北向和天向；

在導航坐標系下，慣導和RTK組合導航的狀態方程表示為：

X(t)為所選的組合導航坐標系統狀態向量，是X(t)的導數，F(t)為系統狀態轉移矩陣，W(t)為系統噪聲向量，G(t)為系統噪聲驅動矩陣；

其中，為b系下加速度計的測量值，表示n系到b系的姿態變換矩陣，( )×表示反對稱矩陣；

其中，W_a和W_g分別表示陀螺儀和加速計的噪聲，R為地球視為圓球時的半徑，w_ie為地球自轉速率；

量測方程由RTK測得的位置和速度與慣導測得的位置和速度分別做差得到，即為：

上式中Z(t)為t時刻的量測矩陣，B代表經度，L代表緯度，H代表高度，V_E代表東向速度，V_N代表北向速度，V_U代表天向速度，下標RTK和INS分別代表RTK和慣導；V(t)代表RTK的位置和速度的噪聲序列；

采用卡爾曼濾波來進行最優估計，其計算步驟為：

一步預測方程：X_k,k-1＝Φ_k,k-1X_k-1；其中，Φ_k,k-1是k-1時刻至k時刻的系統轉移矩陣，X_k-1是k-1時刻的系統狀態矢量；

狀態估計方程為：X_k＝X_k,k-1+K_k(Z_k-Z_k,k-1)；其中，X_k是k時刻的系統狀態矢量，K_k是k時刻的增益矩陣，Z_k是k時刻的量測量，Z_k,k-1是k-1時刻至k時刻的量測量；

增益矩陣的求取方法為：其中，P_k,k-1是k-1時刻至k時刻的均方誤差矩陣，H_k為k時刻的量測矩陣，R_k為系統k時刻量測噪聲方差矩陣；

一步預測均方誤差：其中，Γ_k-1為k-1時刻的噪聲驅動矩陣，Γ_k,k-1是k-1時刻至k時刻的噪聲驅動矩陣，Q_k-1為系統k-1時刻噪聲方差矩陣；

估計均方誤差：P_k＝(I-K_kH_k)P_k,k-1；其中，p_k為k時刻的均方誤差矩陣；

聯立上述各方程解算出來的位置軌跡作為基準軌跡，記為S1；

所述載車水平軌跡的求解包括：

里程儀的速度輸出在里程儀坐標系上表示為：其中，v_D為里程儀測得的前向速度大小，右向和天向速度均為零，視為載車正常行駛時的速度約束條件，上標m、n、b分別表示對應的坐標系；

在導航坐標系下里程儀速度輸出為：

考慮到m系至b系存在小量的安裝偏差角，記偏差角矢量α＝[α_θ α_γ α_ψ]^T，以及刻度系數誤差δK_D，則實際在導航坐標系上里程計的輸出為：

其中，φ_D為航位推算的姿態失準角，下標ψ、γ、θ分別代表m系到b系的俯仰角誤差、橫滾角誤差以及航向角誤差，且φ_D＝[φ_DE φ_DN φ_DU]^T；忽略水平姿態誤差影響，即作近似φ_DE≈φ_DN≈0，得到：

α_θ、φ_DU+α_ψ和δK_D均為常值小量，且載車在地理位置變化不大的范圍內行駛，即整個導航過程中導航坐標系的旋轉變化不大，當作平面處理，上式兩邊同時積分得：

其中，分別表示在時間段[0,T]內的載車真實位移矢量、計算位移矢量和行駛里程；u_U＝[0 0 1]^T為天向單位向量；

將上式分解為水平和垂直兩部分，得：

其中，下標H表示在水平面上的投影；

真實位移繞天向軸u_U轉動角度φ_DU+α_ψ，再擴大1+δK_D倍，得到計算位移從而計算出水平軌跡S2；進而通過對比軌跡S1和S2估算出里程計的標度誤差δK_D。