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

【發明內容】

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