一種基于車輛里程計輔助的姿態(tài)測量方法
【技術領域】
[0001] 本發(fā)明設及一種車輛姿態(tài)測量系統(tǒng)，具體設及一種基于車輛里程計輔助的姿態(tài)測 量方法，屬于車輛姿態(tài)測量技術領域。
【背景技術】
[0002] 目前測量車輛姿態(tài)基本采用W下Ξ種方法:方法1采用高精度慣導，直接用慣性導 航方法測量車輛姿態(tài)角；方法2如圖1所示，采用慣性導航單元（INS)與全球衛(wèi)星導航單元 (GNSS)的組合導航形式，全球衛(wèi)星導航單元(GNSS)包括GPS、GL0NASS、北斗等，通過GNSS修 正慣導，得到較高精度姿態(tài)角;方法3采用垂直巧螺測量，一般用于要求低精度的情況;但在 現(xiàn)有技術方法中，方法1價格昂貴，而且由于慣性器件漂移，定時需要停車修正;方法2是最 常用的方法，但由于采用了外部信息，當信號丟失或受到干擾條件下，精度會變差，長時間 就和純慣性導航一樣;方法3種方法適合姿態(tài)測量精度低而且是車輛運動勻速，顯然車輛不 適合；另外，其它方法還有采用慣性導航與GNSSW及里程計等組合，都是WGNSS為主，里程 計在GNSS信號丟失時作為輔助，上述的其他方法適用前提是必須是慣導和GNSS組合同時存 在，里程計才能輔助。

【發(fā)明內(nèi)容】

[0003] (一)要解決的技術問題
[0004] 為解決上述問題，本發(fā)明提出了一種基于車輛里程計輔助的姿態(tài)測量方法，將里 程計模塊與慣性導航模塊結合，利用測量里程計與慣性導航組合，完成車輛姿態(tài)測量。
[000引（二)技術方案
[0006] 本發(fā)明的基于車輛里程計輔助的姿態(tài)測量系統(tǒng)，包括慣性導航模塊，及與慣性導 航模塊電連接的卡爾曼濾波器，及與卡爾曼濾波器電連接的里程計模塊;所述慣性導航模 塊輸出端輸出至用戶端。
[0007] 進一步地，所述慣性導航模塊由Ξ軸巧螺、Ξ軸加速度計、Ξ軸加速度計信號采集 器和計算電路組成;所述Ξ軸巧螺包括機械巧螺、激光巧螺、光纖巧螺及MEMS巧螺;所述Ξ 軸加速度計包括擺式加速度計和振梁式加速度計。
[0008] 進一步地，所述里程計模塊包括速度傳感器;所述速度傳感器安裝于車輪處;所述 里程計模塊通過車輛0BD接口與車輛電連接。
[0009] -種基于車輛里程計輔助的姿態(tài)測量方法，包括W下步驟：
[0010] 第一步:慣性導航模塊將Ξ軸巧螺和Ξ軸加速度計測量信號數(shù)字化，設定隨動的 導航坐標系，即車前進方向為導航坐標系X軸，天向為y軸，Z向根據(jù)右手法則確定，并進行導 航計算，計算周期為5ms~30ms，得到姿態(tài)角和位置信息后輸出至接口；
[0011] 第二步:若導航時間到修正周期即Is~300s，則執(zhí)行W下步驟，否則返回第一步；
[0012] 第Ξ步:卡爾曼濾波器讀取里程計信息，根據(jù)卡爾曼濾波器計算公式計算觀測矩 陣，計算公式如下：
[0013] X(k+l/k) = 〇 化巧化）；
[0014] P(k+l/k)=巫化)P化A)巫T(k)+GQ(k)Gdt;
[0015] Γ(k+l) = [R化+l)+H(k+l)P(k+l/k)HT化+l)]-i;
[0016]K(k+l)=P(k+l/k)HT化+1)Γ化+1);
[0017]X(k+l)=X(k+l/k)+K化+1)(Ζ(Κ+1))-Η化+1 巧化+l/k));
[0018]P(k+l/k+l) = (Ι-Κ化+1 化化+1))P(k+l/k);
[0019] 公式中，
[0020 ]X化+1 /k)為根據(jù)k時刻預測k+1時刻的狀態(tài)向量值；
[0021 ]K(k+1)為增益矩陣；
[0022]P化+1 /k)為根據(jù)k時刻預測k+1時刻的狀態(tài)向量協(xié)方差矩陣；
[0023]X化+1)為k+1時刻的狀態(tài)向量最佳估計值；
[0024]P化+l/k+l)為k時刻的狀態(tài)向量協(xié)方差估計矩陣；
[0025] Φ化)為狀態(tài)轉移矩陣；
[0026]
[0027] 觀測矩陣求解公式中，
[002引Vy車輛前進方向；
[0029]Vic為里程計速度；
[0030] ΔΦ為修正間隔車輛轉彎角度，從慣性導航模塊得到；
[0031 ]第四步:卡爾曼濾波器計算；
[0032] 第五步:將卡爾曼濾波器計算得到姿態(tài)角誤差和速度誤差結果反饋至慣性導航模 塊進行導航結果的修正。
[0033] 進一步地，所述第一步中的導航計算為四元數(shù)計算，所述四元數(shù)計算中微分方程 計算方法為四階龍格-庫達方法或旋轉矢量方法。
[0034] (立)有益效果
[0035] 與現(xiàn)有技術相比，本發(fā)明的基于車輛里程計輔助的姿態(tài)測量方法，將里程計模塊 與慣性導航模塊結合，利用測量里程計與慣性導航組合，完成車輛姿態(tài)測量，采用了隨動坐 標系，根據(jù)車輛運動導航坐標系隨動，根據(jù)車輛行駛過程任意時刻橫向速度為零的原理，實 現(xiàn)自主(不需要外部信息)姿態(tài)測量，在無GNSS或在GNSS失鎖W及GNSS受到干擾無法使用情 況下，仍能很好的得到車輛姿態(tài)。
【附圖說明】
[0036] 圖1是現(xiàn)有技術中方法2的車輛姿態(tài)測量系統(tǒng)的結構框圖；
[0037] 圖2是本發(fā)明的基于車輛里程計輔助的姿態(tài)測量系統(tǒng)的結構框圖。
[0038] 附圖中的部件標注為：1-慣性導航單元，2-全球衛(wèi)星導航單元，3-卡爾曼濾波單 元，4-慣性導航模塊，5-里程計模塊、6-卡爾曼濾波器。
【具體實施方式】
[0039] 如圖2所示的基于車輛里程計輔助的姿態(tài)測量系統(tǒng)，包括慣性導航模塊4,及與慣 性導航模塊4電連接的卡爾曼濾波器6,及與卡爾曼濾波器6電連接的里程計模塊5;所述慣 性導航模塊4輸出端輸出至用戶端(未圖示）。
[0040]其中，所述慣性導航模塊4由Ξ軸巧螺、Ξ軸加速度計、Ξ軸加速度計信號采集器 和計算電路組成;所述Ξ軸巧螺包括機械巧螺、激光巧螺、光纖巧螺及MEMS巧螺;所述Ξ軸 加速度計包括擺式加速度計和振梁式加速度計。
[0041 ]所述里程計模塊5包括速度傳感器;所述速度傳感器安裝于車輪處;所述里程計模 塊通過車輛0BD接口與車輛電連接。
[0042] -種基于車輛里程計輔助的姿態(tài)測量方法，包括W下步驟：
[0043] 第一步:慣性導航模塊4將Ξ軸巧螺和Ξ軸加速度計測量信號數(shù)字化，設定隨動的 導航坐標系，即車前進方向為導航坐標系X軸，天向為y軸，Z向根據(jù)右手法則確定，并進行導 航計算，計算周期為5ms~30ms，得到姿態(tài)角和位置信息后輸出至接口；
[0044] 第二步:若導航時間到修正周期即Is~300s，則執(zhí)行W下步驟，否則返回第一步；
[0045] 第Ξ步:卡爾曼濾波器6讀取里程計信息，根據(jù)卡爾曼濾波器計算公式計算觀測矩 陣，計算公式如下：
[0046] X化+l/k) = 〇 化巧化）；
[0047] P(k+l/k) =Φ化)P化A) 〇T(k)+GQ(k)Gdt;
[004引 Γ(k+1) = [R化+1 )+H(k+l)P(k+l/k)HT化+1) ]-1;
[0049] K(k+1) =P(k+l/k)HT化+1)?；?1);
[0050] X(k+l)=X(k+l/k)+K化+1)(Z(K+1))-H化+1 巧化+l/k));
[0051 ] P(k+l/k+l) = (I-K化+1 化化+1))P(k+l/k);
[0052] 公式中，
[0053 ] X化+1/k)為根據(jù)k時刻預測k+1時刻的狀態(tài)向量值；
[0054] K(k+1)為增益矩陣；
[0055] P化+1/k)為根據(jù)k時刻預測k+1時刻的狀態(tài)向量協(xié)方差矩陣；
[0056 ] X化+1)為k+1時刻的狀態(tài)向量最佳估計值；
[0057] P化+l/k+l)為k時刻的狀態(tài)向量協(xié)方差估計矩陣；
[005引 Φ化)為狀態(tài)轉移矩陣；
[0059] 觀測矩陣為
[0060] 觀測矩陣求解公式中，
[0061] Vy車輛前進方向；
[0062] Vic為里程計速度；
[0063] ΔΦ為修正間隔車輛轉彎角度，從慣性導航模塊4得到；
[0064] 第四步:卡爾曼濾波器計算；
[0065] 第五步:將卡爾曼濾波器6計算得到姿態(tài)角誤差和速度誤差結果反饋至慣性導航 模塊4進行導航結果的修正。
[0066] 所述第一步中的導航計算為四元數(shù)計算，所述四元數(shù)計算中微分方程計算方法為 四階龍格-庫達方法或旋轉矢量方法。
[0067] 上面所述的實施例僅僅是對本發(fā)明的優(yōu)選實施方式進行描述，并非對本發(fā)明的構 思和范圍進行限定。在不脫離本發(fā)明設計構思的前提下，本領域普通人員對本發(fā)明的技術 方案做出的各種變型和改進，均應落入到本發(fā)明的保護范圍，本發(fā)明請求保護的技術內(nèi)容， 已經(jīng)全部記載在權利要求書中。
【主權項】
1. 一種基于車輛里程計輔助的姿態(tài)測量系統(tǒng)，其特征在于:包括慣性導航模塊，及與慣 性導航模塊電連接的卡爾曼濾波器，及與卡爾曼濾波器電連接的里程計模塊;所述慣性導 航模塊輸出端輸出至用戶端。2. 根據(jù)權利要求1所述的基于車輛里程計輔助的姿態(tài)測量系統(tǒng)，其特征在于:所述慣性 導航模塊由三軸陀螺、三軸加速度計、三軸加速度計信號采集器和計算電路組成;所述三軸 陀螺包括機械陀螺、激光陀螺、光纖陀螺及MEMS陀螺;所述三軸加速度計包括擺式加速度計 和振梁式加速度計。3. 根據(jù)權利要求1所述的基于車輛里程計輔助的姿態(tài)測量系統(tǒng)，其特征在于:所述里程 計模塊包括速度傳感器;所述速度傳感器安裝于車輪處;所述里程計模塊通過車輛OBD接口 與車輛電連接。4. 一種基于車輛里程計輔助的姿態(tài)測量方法，其特征在于:包括以下步驟： 第一步:慣性導航模塊將三軸陀螺和三軸加速度計測量信號數(shù)字化，設定隨動的導航 坐標系，車前進方向為導航坐標系X軸，天向為y軸，z向根據(jù)右手法則確定，并進行導航計 算，計算周期為5ms~30ms，得到姿態(tài)角和位置信息后輸出至接口； 第二步:若導航時間到修正周期即Is~300s，則執(zhí)行以下步驟，否則返回第一步； 第三步:卡爾曼濾波器讀取里程計信息，根據(jù)卡爾曼濾波器計算公式計算觀測矩陣，計 算公式如下： X(k+l/k) = ?(k)X(k)； P(k+l/k) = ?(k)P(k/k)?T(k)+GQ(k)Gdt； Γ (k+l) = [R(k+l)+H(k+l)P(k+l/k)HT(k+l)]-S K(k+l)=P(k+l/k)HT(k+l) Γ (k+1)； X(k+l)=X(k+l/k)+K(k+l)(Z(K+l))-H(k+l)X(k+l/k))； P(k+l/k+l) = (I-K(k+l)H(k+l))P(k+l/k)； 公式中， X(k+l/k)為根據(jù)k時刻預測k+1時刻的狀態(tài)向量值； K(k+1)為增益矩陣； P(k+l/k)為根據(jù)k時刻預測k+Ι時刻的狀態(tài)向量協(xié)方差矩陣； X (k+1)為k+1時刻的狀態(tài)向量最佳估計值； P (k+1/k+1)為k時刻的狀態(tài)向量協(xié)方差估計矩陣； Φ (k)為狀態(tài)轉移矩陣； 觀測矩陣為：觀測矩陣求解公式中， Vy車輛前進方向； Vic為里程計速度； Α Φ為修正間隔車輛轉彎角度，從慣性導航模塊得到； 第四步:卡爾曼濾波器計算； 第五步:將卡爾曼濾波器計算得到姿態(tài)角誤差和速度誤差結果反饋至慣性導航模塊進 行導航結果的修正。5.根據(jù)權利要求4所述的基于車輛里程計輔助的姿態(tài)測量方法，其特征在于:所述第一 步中的導航計算為四元數(shù)計算，所述四元數(shù)計算中微分方程計算方法為四階龍格-庫達方 法或旋轉矢量方法。
【專利摘要】本發(fā)明公開了一種基于車輛里程計輔助的姿態(tài)測量方法，包括以下步驟：第一步：慣性導航模塊將三軸陀螺和三軸加速度計測量信號數(shù)字化，并進行導航計算，得到姿態(tài)角和位置信息后輸出到接口；第二步：若導航時間到修正周期執(zhí)行以下步驟，否則返回第一步；第三步：卡爾曼濾波器讀取里程計信息，并計算觀測矩陣；第四步：卡爾曼濾波器計算；第五步：將卡爾曼濾波器計算得到姿態(tài)角誤差和速度誤差結果修正導航結果。本發(fā)明的基于車輛里程計輔助的姿態(tài)測量方法，利用測量里程計與慣性導航組合，完成車輛姿態(tài)測量，根據(jù)車輛運動導航坐標系隨動，車輛行駛過程任意時刻橫向速度為零的原理，實現(xiàn)自主姿態(tài)測量。
【IPC分類】G01C22/00, G01C21/18
【公開號】CN105444764
【申請?zhí)枴緾N201510829174
【發(fā)明人】田雨農(nóng), 周秀田
【申請人】大連樓蘭科技股份有限公司
【公開日】2016年3月30日
【申請日】2015年11月24日