慣性導(dǎo)航和輪速計組成的航跡推算定位系統(tǒng)誤差估算算法
【專利摘要】本發(fā)明提供一種慣性導(dǎo)航和輪速計組成的航跡推算定位系統(tǒng)誤差估算算法，通過將航跡推算坐標(biāo)和GPS經(jīng)緯度坐標(biāo)轉(zhuǎn)換到同一個局部坐標(biāo)系中，以GPS坐標(biāo)為標(biāo)準(zhǔn)值，航跡推算坐標(biāo)為參照值，計算航跡推算坐標(biāo)和GPS坐標(biāo)之間的誤差值，設(shè)定3個誤差參數(shù)，每個誤差參數(shù)設(shè)定一定數(shù)量級的誤差范圍，在該范圍內(nèi)隔段誤差選取一個誤差量，使用正交試驗(yàn)法篩選出部分典型的誤差組合，找出誤差最小的誤差參數(shù)組合作為誤差參數(shù)最優(yōu)值，并代入慣導(dǎo)航向和輪速計的左右輪數(shù)據(jù)中進(jìn)行修正。本發(fā)明通過簡單的算法，估算慣導(dǎo)的系統(tǒng)誤差，并一次性的給予修正，算法簡單，速度快，計算一次便可修正，簡化了慣性導(dǎo)航定位系統(tǒng)使用前的誤差校準(zhǔn)過程。
【專利說明】慣性導(dǎo)航和輪速計組成的航跡推算定位系統(tǒng)誤差估算算法

【技術(shù)領(lǐng)域】
[0001] 本發(fā)明涉及慣性導(dǎo)航系統(tǒng)【技術(shù)領(lǐng)域】，尤其涉及一種慣性導(dǎo)航和輪速計組成的航跡 推算定位系統(tǒng)誤差估算算法。

【背景技術(shù)】
[0002] 慣性導(dǎo)航系統(tǒng)依靠機(jī)械設(shè)備和相應(yīng)算法工作，能獨(dú)立完成導(dǎo)航任務(wù)。此外慣導(dǎo)系 統(tǒng)不受外界諸如天氣，電磁福射的環(huán)境因素的干擾，并且載體在小范圍內(nèi)的活動擁有非常 高的可靠性，因此慣性導(dǎo)航系統(tǒng)廣泛應(yīng)用于軍用和民用領(lǐng)域。
[0003] 地面車輛的慣導(dǎo)系統(tǒng)在長時間的使用之后，慣導(dǎo)的安裝有松動，會導(dǎo)致慣導(dǎo)有安 裝偏移角，并且慣導(dǎo)受外界影響如磁場時也會導(dǎo)致慣導(dǎo)初始方向角存在偏差，而車輪由于 車胎氣壓的變化，車輪的半徑相較于標(biāo)準(zhǔn)車輪半徑也會出現(xiàn)偏差，從而導(dǎo)致輪速計在計算 車輛移動距離時出現(xiàn)偏移誤差。由于該類誤差問題的存在，會導(dǎo)致慣導(dǎo)定位系統(tǒng)在進(jìn)行航 跡推算定位時，得到的坐標(biāo)和運(yùn)行軌跡出現(xiàn)較大的系統(tǒng)偏移誤差。因此慣導(dǎo)系統(tǒng)在開始工 作前，通常要進(jìn)行初始校準(zhǔn)W減小慣導(dǎo)初始安裝偏差角、慣導(dǎo)初始誤差、輪速計偏移誤差等 誤差導(dǎo)致的定位誤差。
[0004] 慣導(dǎo)的初始校準(zhǔn)通常分為靜基座分析和動態(tài)誤差分析兩種。靜基座測試主要測量 慣導(dǎo)和其他導(dǎo)航器件的系統(tǒng)誤差，因需要使用大量精密儀器，雖然精度較高但是需要耗費(fèi) 很長的時間并且成本也非常高。慣導(dǎo)的動態(tài)分析主要在慣導(dǎo)系統(tǒng)開始工作前進(jìn)行校準(zhǔn)，減 小安裝偏差角、慣導(dǎo)初始未對準(zhǔn)之類的問題導(dǎo)致慣導(dǎo)系統(tǒng)工作定位產(chǎn)生系統(tǒng)誤差。而且動 態(tài)誤差測試在室外場地就能進(jìn)行，不需要使用精密儀器，用GPS的坐標(biāo)和航向角作為參考 標(biāo)準(zhǔn)就行，不需要太長的時間，使用方便，成本也非常低。但動態(tài)誤差修正方法主要注重誤 差的實(shí)時修正，算法復(fù)雜的同時，不能一次性的估計系統(tǒng)誤差值并給予修正。


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

[0005] 本發(fā)明的目的是為了簡化慣性導(dǎo)航定位系統(tǒng)使用前的誤差校準(zhǔn)過程，提供一種慣 性導(dǎo)航和輪速計組成的航跡推算定位系統(tǒng)誤差估算算法，算法簡單，速度快，計算一次便可 修正。
[0006] 為解決上述技術(shù)問題，本發(fā)明采用如下技術(shù)方案：
[0007] -種慣性導(dǎo)航和輪速計組成的航跡推算定位系統(tǒng)誤差估算算法，包括如下步驟：
[000引 （1)試驗(yàn)車打開車載的慣導(dǎo)、輪速計和GPS行駛一段路程，分別記錄慣導(dǎo)航向、輪 速計和GI^經(jīng)締度坐標(biāo)數(shù)據(jù)；
[0009] (2)根據(jù)慣導(dǎo)航向數(shù)據(jù)和輪速計數(shù)據(jù)進(jìn)行航跡推算，得出n時刻航跡推算坐標(biāo) (Xn，Yn)；
[0010] (3)W試驗(yàn)車的起點(diǎn)為原點(diǎn)設(shè)置局部坐標(biāo)系，將GI^經(jīng)締度坐標(biāo)和步驟（2)中的航 跡推算坐標(biāo)轉(zhuǎn)換為局部坐標(biāo)系坐標(biāo)，并W正北方向作為Y軸正方向，正東方向作為X軸正方 向，兩個坐標(biāo)數(shù)據(jù)整合到同一坐標(biāo)系中進(jìn)行對比，WGI^經(jīng)締度坐標(biāo)點(diǎn)為標(biāo)準(zhǔn)參量，航跡推 算坐標(biāo)點(diǎn)為包含誤差的對比值，兩坐標(biāo)點(diǎn)之間的距離即為誤差值；
[0011] (4)設(shè)定慣導(dǎo)航向初始誤差角、左輪速計初始誤差和右輪速計初始誤差3個偏差 量為導(dǎo)致航跡推算得到的坐標(biāo)出現(xiàn)誤差的誤差參數(shù)；
[0012] (5)確定誤差參數(shù)值最優(yōu)值所在的范圍；首先對誤差參數(shù)組進(jìn)行篩選，當(dāng)誤差參 數(shù)值水平數(shù)為1或2時，通過作圖的辦法對誤差參數(shù)組進(jìn)行篩選，當(dāng)誤差參數(shù)值水平數(shù)大于 等于3時，通過正交試驗(yàn)法對誤差參數(shù)組進(jìn)行篩選；然后將篩選出的誤差參數(shù)組代入局部 坐標(biāo)系中的航跡推算坐標(biāo)中計算誤差大小來確定最優(yōu)值的取值區(qū)間；
[0013] (6)將篩選出的誤差參數(shù)組代入局部坐標(biāo)系中的航跡推算坐標(biāo)中計算誤差大小， 根據(jù)計算比較誤差最大值和均值能否滿足精度要求，判斷是否為誤差參數(shù)組的最優(yōu)解；
[0014] (7)在計算得到誤差參數(shù)組的最優(yōu)解后，將最優(yōu)解代入慣導(dǎo)航向和輪速計的左右 輪數(shù)據(jù)中進(jìn)行修正，使用修正后的慣導(dǎo)航向和輪速計數(shù)據(jù)進(jìn)行航跡推算便可W得到修正后 的慣導(dǎo)定位坐標(biāo)。
[0015] 步驟（2)中，所述航跡推算的具體方法為：根據(jù)試驗(yàn)車的慣導(dǎo)航向和輪速計的車 輛行駛距離數(shù)據(jù)，通過H角函數(shù)法計算車輛的位置坐標(biāo)（x"，y。)，如下式所示：
[0016] X。=Xn-I+ALs*cos(headingn-i) (:〇
[0017] y。二Yn-I+ ALs*sin(headingn-i)
[001引其中ALs為單位采樣時間內(nèi)試驗(yàn)車移動的距離，heading^為第n-1時刻試驗(yàn)車 的瞬時慣導(dǎo)航向。
[0019] 所述單位采樣時間內(nèi)試驗(yàn)車移動的距離ALs取左輪和右輪的行駛距離的平均 值，如下式所示：

【權(quán)利要求】
1. 一種慣性導(dǎo)航和輪速計組成的航跡推算定位系統(tǒng)誤差估算算法，其特征在于，包括 如下步驟： (1) 試驗(yàn)車打開車載的慣導(dǎo)、輪速計和GPS行駛一段路程，分別記錄慣導(dǎo)航向、輪速計 和GPS經(jīng)緯度坐標(biāo)數(shù)據(jù)； (2) 根據(jù)慣導(dǎo)航向數(shù)據(jù)和輪速計數(shù)據(jù)進(jìn)行航跡推算，得出n時刻航跡推算坐標(biāo) (xn，yn); (3) 以試驗(yàn)車的起點(diǎn)為原點(diǎn)設(shè)置局部坐標(biāo)系，將GPS經(jīng)緯度坐標(biāo)和步驟（2)中的航跡推 算坐標(biāo)轉(zhuǎn)換為局部坐標(biāo)系坐標(biāo)，并以正北方向作為Y軸正方向，正東方向作為X軸正方向， 兩個坐標(biāo)數(shù)據(jù)整合到同一坐標(biāo)系中進(jìn)行對比，以GPS經(jīng)緯度坐標(biāo)點(diǎn)為標(biāo)準(zhǔn)參量，航跡推算 坐標(biāo)點(diǎn)為包含誤差的對比值，兩坐標(biāo)點(diǎn)之間的距離即為誤差值； (4) 設(shè)定慣導(dǎo)航向初始誤差角、左輪速計初始誤差和右輪速計初始誤差3個偏差量為 導(dǎo)致航跡推算得到的坐標(biāo)出現(xiàn)誤差的誤差參數(shù)； (5) 確定誤差參數(shù)值最優(yōu)值所在的范圍：首先對誤差參數(shù)組進(jìn)行篩選，當(dāng)誤差參數(shù)值 水平數(shù)為1或2時，通過作圖的辦法對誤差參數(shù)組進(jìn)行篩選，當(dāng)誤差參數(shù)值水平數(shù)大于等于 3時，通過正交試驗(yàn)法對誤差參數(shù)組進(jìn)行篩選；然后將篩選出的誤差參數(shù)組代入局部坐標(biāo) 系中的航跡推算坐標(biāo)中計算誤差大小來確定最優(yōu)值的取值區(qū)間； (6) 將篩選出的誤差參數(shù)組代入局部坐標(biāo)系中的航跡推算坐標(biāo)中計算誤差大小，根據(jù) 計算比較誤差最大值和均值能否滿足精度要求，判斷是否為誤差參數(shù)組的最優(yōu)解； (7) 在計算得到誤差參數(shù)組的最優(yōu)解后，將最優(yōu)解代入慣導(dǎo)航向和輪速計的左右輪數(shù) 據(jù)中進(jìn)行修正，使用修正后的慣導(dǎo)航向和輪速計數(shù)據(jù)進(jìn)行航跡推算便可以得到修正后的慣 導(dǎo)定位坐標(biāo)。
2. 根據(jù)權(quán)利要求1所述的誤差估算算法，其特征在于，步驟（2)中，航跡推算具體為： 根據(jù)試驗(yàn)車的慣導(dǎo)航向和輪速計的車輛行駛距離數(shù)據(jù)，通過三角函數(shù)法計算車輛的位置坐 標(biāo)（xn，yn)，如下式所示： xn = Xlri+ A Ls*cos (headingn-i) (i) yn = yn-i+ A Ls*sin (heading^) 其中A Ls為單位采樣時間內(nèi)試驗(yàn)車移動的距離，heading^為第n-1時刻試驗(yàn)車的瞬 時慣導(dǎo)航向。
3. 根據(jù)權(quán)利要求2所述的誤差估算算法，其特征在于，所述單位采樣時間內(nèi)試驗(yàn)車移 動的距離A Ls取左輪和右輪的行駛距離的平均值，如下式所示：
其中Ls是輪速計記錄的行駛距離平均值，Ls^為左輪輪速計記錄的行駛距離，Lsk是右 輪輪速計記錄的行駛距離，單位采樣時間內(nèi)試驗(yàn)車移動的距離ALs = Lsn-Lslri，其中LsnS n時刻的左右輪速計記錄的行駛距離的平均值。
4. 根據(jù)權(quán)利要求1所述的誤差估算算法，其特征在于，步驟（3)中，GPS經(jīng)緯度坐標(biāo)轉(zhuǎn) 換成局部坐標(biāo)系坐標(biāo)的計算過程具體為：設(shè)n時刻GPS經(jīng)緯度坐標(biāo)轉(zhuǎn)化為局部坐標(biāo)系中的 坐標(biāo)（x'n，y'n)，如下式所示： X，n = X，n-i+R*!^，y，n = y，Qii) Ta =A CO * 31 /360 T4, =A 4) * 3i /360 其中R為地球半徑，A ?和A 為n時刻和n-1時刻之間的經(jīng)度和緯度差值，和 h為經(jīng)度和緯度差值轉(zhuǎn)化為弧度值。
5. 根據(jù)權(quán)利要求1所述的誤差估算算法，其特征在于，步驟（5)中，誤差參數(shù)誤差范圍 設(shè)定為慣性導(dǎo)航官方標(biāo)注誤差的兩倍，且正負(fù)方向都設(shè)定為誤差的分布范圍，在誤差正負(fù) 的分布范圍內(nèi)，設(shè)定同組等分的水平數(shù)，同時0也作為一組水平數(shù)，讓各個不同組的水平數(shù) 自由組合，構(gòu)成全部誤差參數(shù)組。
6. 根據(jù)權(quán)利要求1所述的誤差估算算法，其特征在于，步驟￠)中，若對誤差參數(shù)組有 更大的精度需求，需要進(jìn)一步精確計算最優(yōu)解時，可以使用響應(yīng)曲面法來擬合誤差曲面，找 到誤差曲面上誤差最小的點(diǎn)，其對應(yīng)的誤差參數(shù)即是誤差參數(shù)的最優(yōu)解。
【文檔編號】G01C25/00GK104359492SQ201410613529
【公開日】2015年2月18日 申請日期:2014年11月3日 優(yōu)先權(quán)日:2014年11月3日
【發(fā)明者】祝輝, 何筆華, 梁華為, 余彪 申請人:中國科學(xué)院合肥物質(zhì)科學(xué)研究院