[發明專利]一種附有GNSS速度矢量輔助的車載姿態估計方法

有效

• 申請號: 202011373852.7
• 申請日: 2020-11-30
• 公開（公告）號: CN112683267B
• 公開（公告）日: 2022-06-03
• 發明（設計）人: 周澤波;張澤亮;吳藎
• 申請（專利權）人: 電子科技大學
• 主分類號: G01C21/20
• 分類號: G01C21/20
• 代理公司: 電子科技大學專利中心 51203
• 代理人: 陳一鑫

1.一種附有GNSS速度矢量輔助的車載姿態估計方法，包括以下步驟：

步驟1：利用陀螺儀數據進行姿態推算；

步驟2：利用加速度計和陀螺儀對GNSS速度進行檢測：

步驟2-1：利用長度為l的滑動遞推窗口對GNSS速度進行p階多項式擬合，進一步對其求導，獲得運動加速度擬合多項式；對GNSS速度得到的運動加速度與陀螺儀和加速度計計算的運動加速度進行比較，檢驗GNSS速度是否異常；

步驟2-2：利用GNSS速度進行俯仰角求解，將其與陀螺儀獲得的俯仰角進行比較，檢驗GNSS速度是否異常；

步驟3：利用GNSS速度對運動加速度進行補償，進行陀螺儀/加速度計一級濾波；

取3維姿態誤差q_ξ，陀螺漂移誤差b_g和加速度計零偏誤差b_a構建濾波狀態矢量并建立傳感器漂移噪聲模型：

其中，頂標·表示一階導數運算，表示陀螺漂移模型噪聲，表示噪聲服從0均值方差為的高斯分布；表示加速度計零偏模型噪聲，表示噪聲服從0均值方差為的高斯分布；

記狀態轉移模型噪聲可建立系統狀態模型：

其中，轉移矩陣B與系統噪聲方差Q分別為：

其中，R_g表示陀螺隨機噪聲的方差；表示陀螺漂移模型噪聲的方差；表示加速度計零偏模型的噪聲方差；[·×]表示反對稱矩陣運算；進一步地，在系統狀態模型的基礎上，離散化的系統狀態模型可寫為：

其中，x_k表示k時刻的狀態矢量，表示離散化的系統狀態模型噪聲，狀態轉移矩陣為Φ_k,k-1＝I_9×9+BΔt_s；

利用加速度計建立量測模型：

其中，表示k時刻的量測信息，H_a,k表示k時刻的加速度計量測模型矩陣，z_a,k表示k時刻的加速度計輸出，表示利用陀螺估計的姿態四元數，表示k時刻的運動加速度估計；基于線性卡爾曼濾波進行狀態估計，獲得融合加速度計信息后的狀態及方差P_a,k；將一級濾波的結果更新至姿態四元數并做歸一化處理：

其中，表示一級濾波k時刻融合加速度計信息后的姿態誤差，表示一級濾波k時刻融合加速度計信息后的狀態估計量，表示取矢量的第1至第3個分量；表示k時刻利用陀螺估計的姿態四元數；表示一級濾波k時刻融合加速度計信息后的姿態四元數；用更新并對狀態矢量中q_ξ置零；

步驟4：利用GNSS速度檢測排除磁力計模型中的磁場干擾；

步驟5：進行二級濾波，融合磁力計數據；

進行二級濾波狀態及其方差一步預測：

其中，表示k時刻融合加速度計信息后的狀態估計，在步驟3中獲得，其方差為P_a,k；表示k時刻融合磁力計信息后的狀態一步預測，表示狀態方差，P_a,k(1:3,1:3)表示取矩陣P_a,k第1至3行，1至3列的矩陣；

建立磁力計量測模型：

其中，z_m表示磁力計輸出，mⁿ表示n系下的地磁參考矢量；ε_m為磁力計噪聲，將模型記為

其中，量測信息量測矩陣融合加速度計后的姿態四元數通過步驟3中(23)獲得；為保證磁力計數據僅用于偏航角修正，而不影響俯仰和滾轉，引入約束矩陣計算系統濾波增益K_m,k：

其中，R_m表示磁力計的噪聲方差，一步預測方差由公式(8)獲得；約束矩陣Λ_m,k為：

進行二級濾波觀測修正：

其中，表示k時刻融合磁力計信息后的狀態估計，P_m,k表示的方差；一步預測值由(7)計算獲得，增益K_m,k利用(11)計算獲得；利用二級濾波的結果更新姿態四元數并做歸一化處理：

其中，表示二級濾波k時刻融合磁力計信息后的姿態誤差，表示二級濾波k時刻融合磁力計信息后的狀態估計量，見(13)；為矢量的第1至第3個分量；表示一級濾波k時刻融合加速度計信息后的姿態四元數，見(6)；表示二級濾波k時刻融合磁力計信息后的姿態四元數；用更新并對狀態矢量中q_ξ置零；

步驟6：進行三級濾波，融合GNSS速度信息；具體步驟如下：

步驟6-1：計算三級濾波狀態及其方差的一步預測：

對于三級濾波，計算狀態及其方差的一步預測：

其中，表示k時刻融合GNSS速度信息后的狀態一步預測，表示k時刻融合磁力計信息后的狀態估計量；和P_m,k分別為和的方差；

步驟6-2：利用GNSS速度建立載體速度矢量對方程，進一步建立三級濾波量測方程；

利用GNSS速度建立載體速度矢量對方程：

其中，表示b系下的速度矢量估計，v^e表示e系下的速度矢量，表示由e系到b系的旋轉矩陣，ε₁表示噪聲，b系下載體的速度可表示為

式(37)中，噪聲ε₁包含兩類影響：一為車輪軌跡和車輛b系X軸的定義方向存在偏差；二為由路面引起車輛滑動或跳躍，導致在Y軸或Z軸存在速度；GNSS速度觀測模型有：

其中，表示GNSS速度觀測，ε₂表示噪聲，將(39)帶入(37)，得GNSS速度矢量對方程：

記式(40)可寫作

根據誤差傳播定律確定ε_s的方差R_s：

其中，R₁和R₂分別表示噪聲ε₁和ε₂的方差；利用GNSS速度建立三級濾波量測方程

其中，觀測信息為觀測矩陣為z_s,k表示k時刻的z_s；為步驟5中融合磁力計信息后的姿態四元數；

步驟6-3：建立三級濾波滾轉/偏航-滾轉約束方程：

利用Z軸角速率輸出z_g(3)(t)檢測載體是否存在轉彎：

由于GNSS速度無法測量載體滾轉角，在三級濾波中，GNSS速度不對二級濾波的姿態滾轉角修正；據此可建立滾轉約束方程：

其中，矩陣下標(2,3)表示取矩陣第2行第3列的元素；(3,3)表示取矩陣第3行第3列的元素；

其中，表示三級濾波中融合GNSS速度后的姿態四元數估計；表示與姿態真值間的誤差四元數；表示三級濾波中融合GNSS速度后的狀態矢量估計；向量下標(1)，(2)，(3)，以及下文出現的(4)分別表示取向量的第1、第2、第3、第4個元素；

將滾轉約束方程改寫成如下形式

其中，約束矩陣為

在車輛轉彎時，GNSS速度方向與載體偏航角方向存在偏差；為抑制其對偏航角的影響，在轉彎情況下建立偏航-滾轉約束方程，約束矩陣為

步驟6-4：基于貝葉斯理論進行最優估計，得到含滾轉/偏航-滾轉約束的三級濾波狀態估計值，完成姿態估計：

基于貝葉斯理論建立如下最優問題：

其中，V_k和分別表示觀測和預測的殘差，

引入拉格朗日算子，

對其求一階偏導數，有

其中，表示求偏導數運算；最終得有無滾轉約束的狀態估計和間的關系

其中，表示未進行滾轉約束的濾波估計值，其方差為表示進行滾轉約束的濾波估計值，其方差為P_s,k；利用三級濾波的結果更新姿態四元數并做歸一化處理：

其中，表示三級濾波中融合GNSS速度后的姿態四元數估計；表示三級濾波姿態誤差四元數；表示三級濾波中融合GNSS速度后的狀態矢量估計；為的第1至第3個元素；表示二級濾波中融合磁力計信息后的姿態四元數。