基于光纖陀螺、速度傳感器和gps的實時精確位姿估計方法
【專利摘要】本發(fā)明提供一種基于光纖陀螺、速度傳感器和GPS的實時精確位姿估計方法，其中基于車體運動模型的無人車局部定位算法主要基于二維平面的航跡推算原理，通過前一時刻的車體傳感器的速度、航向信息估計車體當(dāng)前的局部位姿，并利用線性模型補償陀螺航向的累積誤差，提高了局部定位系統(tǒng)的精度；而利用迭代最近點算法配準(zhǔn)GPS和局部位姿測量系統(tǒng)的位姿估計方法能夠校正局部定位的累積誤差，有效消除GPS的隨機(jī)噪聲，并在GPS完全失效或者出現(xiàn)故障的情況下保持位姿的精度，跑車試驗結(jié)果表明，本發(fā)明提出的方法能夠很好的融合多傳感信息，在多樹木、多建筑物的城區(qū)環(huán)境中可以為無人車提供可靠連續(xù)的定位信息，此外，該方法還具有很好的實時性。
【專利說明】基于光纖陀螺、速度傳感器和GPS的實時精確位姿估計方法
【技術(shù)領(lǐng)域】
[0001]本發(fā)明涉及移動機(jī)器人、導(dǎo)航定位和數(shù)據(jù)融合領(lǐng)域，具體涉及一種利用光纖陀螺和智能車的里程計等傳感器計算無人車局部姿態(tài)信息、利用GPS獲得全局位姿、將無人車局部位姿和GPS位置進(jìn)行配準(zhǔn)得到修訂的全局位姿的方法。
【背景技術(shù)】
[0002]隨著科學(xué)技術(shù)的不斷發(fā)展，無人駕駛車輛(無人車)在采礦業(yè)，貨物運輸，農(nóng)業(yè)自動化和軍事領(lǐng)域有著越來越廣泛的應(yīng)用。導(dǎo)航定位系統(tǒng)為無人車提供實時的位置和姿態(tài)信息，保證無人車按照正確路線完成自主導(dǎo)航和精確控制，是無人車系統(tǒng)的重要部分。目前的無人車定位方式主要有四類，GPS系統(tǒng)、航跡推算系統(tǒng)(Dead Reckoning System,DRS),將兩者結(jié)合起來的組合導(dǎo)航系統(tǒng)，還有基于視覺/激光的定位系統(tǒng)。
[0003]第一類，全球定位系統(tǒng)。GPS系統(tǒng)能夠在全球范圍內(nèi)為用戶提供精確的、實時的位置和速度信息，是目前使用最為廣泛的導(dǎo)航定位系統(tǒng)。GPS定位基于衛(wèi)星測距原理，不存在累積誤差，長時間范圍內(nèi)的性能優(yōu)越。然而，在城區(qū)、峽谷、隧道和多樹木的環(huán)境中，衛(wèi)星信號容易受到干擾，GPS定位性能下降，甚至失去定位功能。
[0004]第二類，航跡推算系統(tǒng)。DRS屬于局部定位系統(tǒng)，是陸地輪式車輛特有的低成本局部定位技術(shù)，主要通過里程計的速度信息和陀螺的航向信息推算車體的位置和姿態(tài)。DRS具有高度自治性，幾乎不受外界環(huán)境干擾，更新頻率快，短時間內(nèi)定位精度高。然而，受傳感器固有測量誤差的影響，DRS的誤差隨時間迅速累積，無法保證長時間的定位精度。
[0005]第三類，組合導(dǎo)航系統(tǒng)。無人車要求導(dǎo)航系統(tǒng)提供連續(xù)不間斷的高精度定位信息，這是目前任何一種單一的定位系統(tǒng)無法滿足的，融合GPS和其他定位系統(tǒng)的組合導(dǎo)航方法已經(jīng)成為定位技術(shù)研究的熱點。
[0006]由于成本低、短時間內(nèi)精度高，航跡推算系統(tǒng)常與GPS構(gòu)成組合導(dǎo)航系統(tǒng)，改善原有定位系統(tǒng)的性能。卡爾曼濾波和擴(kuò)展卡爾曼濾波是GPS/DR組合導(dǎo)航采用的主要融合算法。例如，Rezaei S和Sengupta R提出了一種基于EKF融合GPS、里程計和陀螺的組合導(dǎo)航算法，通過加入車體運動學(xué)模型的約束提高對GPS的噪聲估計精度；Lu WC和Zhang W則提出運用聯(lián)邦濾波結(jié)構(gòu)融合GPS和DR的信息，分別用KF和EKF算法設(shè)計GPS和DR局部濾波器，主濾波器只做信息融合，相比集中式的濾波結(jié)構(gòu)，提高了計算效率和系統(tǒng)的容錯性；Obradovic D, Lenz H和Schupfner M介紹了西門子公司設(shè)計的GPS/DR車載組合導(dǎo)航系統(tǒng)，該系統(tǒng)采用分布式卡爾曼濾波算法進(jìn)行融合，并依靠地圖信息改進(jìn)GPS失效時DR的定位性能。但濾波方法在處理隨機(jī)噪聲時只能加入單一的高斯噪聲模型，難以對噪聲進(jìn)行多方面的估計和處理。
[0007]另外，也有GPS/INS —體式封裝設(shè)備可供選擇，具有緊耦合和松耦合兩種組合方式。一體式在安裝、操作等方面都是比較方便的。但目前，中高精度的GPS/INS組合導(dǎo)航系統(tǒng)大多售價昂貴，讓用戶望而卻步。[0008]第四類，利用視覺和激光等傳感器也可以對載體的位置和姿態(tài)信息進(jìn)行估計?；谝曈X/激光的定位系統(tǒng)成本較低、信息量大，已經(jīng)成為人工智能和定位導(dǎo)航領(lǐng)域研究的一個熱點。然而，激光/視覺定位系統(tǒng)容易受外界環(huán)境的干擾，定位效果不穩(wěn)定，目前主要應(yīng)用于環(huán)境較為簡單的室內(nèi)機(jī)器人定位和輔助GPS、DRS等定位技術(shù)中。

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

[0009]本發(fā)明的目的在于克服現(xiàn)有GPS定位方法的缺點,提供一種基于光纖陀螺、速度傳感器和GPS的實時精確位姿估計方法。
[0010]為達(dá)到上述目的，本發(fā)明采用了以下技術(shù)方案。
[0011]1)獲取車體的局部位姿
[0012]基于二維平面的航跡推算原理，通過由速度傳感器獲取的信息以及由光纖陀螺得到的航向信息估計車體當(dāng)前時刻的局部位姿；
[0013]2)從GPS得到車體的全局位姿
[0014]實時解析出WGS84坐標(biāo)系下車體的位置和三維速度，將解析出的位置和三維速度轉(zhuǎn)換到東北天坐標(biāo)系下，然后利用轉(zhuǎn)換后的三維速度計算并濾波得到航向信息；
[0015]3)修正全局位姿
[0016]將歷史時刻的全局軌跡(全局位姿的數(shù)據(jù)點集)與局部軌跡(局部位姿的數(shù)據(jù)點集)進(jìn)行配準(zhǔn)，然后將當(dāng)前時刻的局部位姿數(shù)據(jù)投射到全局位姿所在的坐標(biāo)系下，得到當(dāng)前時刻車體的全局位姿的預(yù)測值，利用預(yù)測值完成局部位姿數(shù)據(jù)對全局位姿數(shù)據(jù)的修正。
[0017]所述步驟I)的具體步驟為:實時獲取光纖陀螺輸出的局部航向角Θ ;從速度傳感器獲取實時的前輪速度以及前輪轉(zhuǎn)角，根據(jù)前后兩幀的速度信息計算相對位移，利用所述相對位移進(jìn)行實時航跡推算獲得車體的局部位置信息，所述速度信息為前輪速度以及前輪轉(zhuǎn)角。
[0018]所述局部位置信息根據(jù)前后兩幀的速度信息和局部航向角Θ求均值后實時累加積分獲得。
[0019]所述航跡推算的公式為:
[0020]
【權(quán)利要求】
1.一種基于光纖陀螺、速度傳感器和GPS的實時精確位姿估計方法,其特征在于:包括以下步驟: O獲取車體的局部位姿 基于二維平面的航跡推算原理，通過由速度傳感器獲取的信息以及由光纖陀螺得到的航向信息估計車體當(dāng)前時刻的局部位姿； 2)從GPS得到車體的全局位姿 實時解析出WGS84坐標(biāo)系下車體的位置和三維速度，將解析出的位置和三維速度轉(zhuǎn)換到東北天坐標(biāo)系下，然后利用轉(zhuǎn)換后的三維速度計算并濾波得到航向信息； 3)修正全局位姿 將歷史時刻的全局軌跡與局部軌跡進(jìn)行配準(zhǔn)，然后將當(dāng)前時刻的局部位姿數(shù)據(jù)投射到全局位姿所在的坐標(biāo)系下，得到當(dāng)前時刻車體的全局位姿的預(yù)測值，利用預(yù)測值完成局部位姿數(shù)據(jù)對全局位姿數(shù)據(jù)的修正。`
2.根據(jù)權(quán)利要求1所述一種基于光纖陀螺、速度傳感器和GPS的實時精確位姿估計方法，其特征在于:所述步驟I)的具體步驟為:實時獲取光纖陀螺輸出的局部航向角Θ ;從速度傳感器獲取實時的前輪速度以及前輪轉(zhuǎn)角，根據(jù)前后兩幀的速度信息計算相對位移，利用所述相對位移進(jìn)行實時航跡推算獲得車體的局部位置信息，所述速度信息為前輪速度以及前輪轉(zhuǎn)角。
3.根據(jù)權(quán)利要求2所述一種基于光纖陀螺、速度傳感器和GPS的實時精確位姿估計方法，其特征在于:所述局部位置信息根據(jù)前后兩幀的速度信息和局部航向角Θ求均值后實時累加積分獲得。
4.根據(jù)權(quán)利要求1所述一種基于光纖陀螺、速度傳感器和GPS的實時精確位姿估計方法，其特征在于:所述航跡推算的公式為:
5.根據(jù)權(quán)利要求4所述一種基于光纖陀螺、速度傳感器和GPS的實時精確位姿估計方法，其特征在于:
6.根據(jù)權(quán)利要求1所述一種基于光纖陀螺、速度傳感器和GPS的實時精確位姿估計方法，其特征在于:所述步驟3)中，用迭代最近點算法完成已知的有效的全局軌跡與局部軌跡的配準(zhǔn)，從而獲得全局軌跡與局部軌跡的變換關(guān)系，用所述變換關(guān)系進(jìn)行全局位姿預(yù)測，利用預(yù)測的全局位姿判斷當(dāng)前獲得的GPS數(shù)據(jù)是否有效，如果有效，則使用根據(jù)當(dāng)前獲得的GPS數(shù)據(jù)確定的全局位姿，如果無效，則使用預(yù)測的全局位姿。
7.根據(jù)權(quán)利要求6所述一種基于光纖陀螺、速度傳感器和GPS的實時精確位姿估計方法，其特征在于:所述步驟3)的具體步驟為: a)全局軌跡與局部軌跡配準(zhǔn) 以當(dāng)前時刻為起點在已經(jīng)產(chǎn)生的軌跡中選擇對應(yīng)的一段全局軌跡與局部軌跡，確定全局軌跡與局部軌跡后，使用迭代最近點算法計算使局部軌跡經(jīng)過旋轉(zhuǎn)平移與全局軌跡重合的旋轉(zhuǎn)平移量，此旋轉(zhuǎn)平移量為全局位姿預(yù)測的基準(zhǔn)； b)全局位姿預(yù)測 使用旋轉(zhuǎn)平移量對當(dāng)前時刻的局部位姿數(shù)據(jù)進(jìn)行旋轉(zhuǎn)平移，得到當(dāng)前時刻全局位姿的預(yù)測值； c)GPS數(shù)據(jù)有效性判斷及處理 計算根據(jù)當(dāng)前時刻GPS數(shù)據(jù)確定的全局位姿與步驟b)中所述預(yù)測值的距離偏差，如果距離偏差小于閾值，則將所述根據(jù)當(dāng)前時刻GPS數(shù)據(jù)確定的全局位姿作為用于計算旋轉(zhuǎn)平移量的全局軌跡中一點；如果距離偏差大于等于閾值，則將根據(jù)當(dāng)前時刻GPS數(shù)據(jù)確定的全局位姿以及當(dāng)前時刻的局部位姿數(shù)據(jù)標(biāo)記為噪聲信號，使用步驟b)中所述預(yù)測值。
8.根據(jù)權(quán)利要求7所述一種基于光纖陀螺、速度傳感器和GPS的實時精確位姿估計方法，其特征在于:所述閾值是由采用的GPS接收機(jī)精度決定。`
9.根據(jù)權(quán)利要求7所述一種基于光纖陀螺、速度傳感器和GPS的實時精確位姿估計方法，其特征在于:若連續(xù)出現(xiàn)距離偏差大于等于閾值的情況，則使用噪聲信號重新進(jìn)行旋轉(zhuǎn)平移量的計算。
【文檔編號】G01S19/53GK103777220SQ201410022601
【公開日】2014年5月7日 申請日期:2014年1月17日 優(yōu)先權(quán)日:2014年1月17日
【發(fā)明者】杜少毅, 宋曄, 劉娟, 薛建儒, 張春家, 祝繼華 申請人:西安交通大學(xué)