[發明專利]基于光纖陀螺、速度傳感器和GPS的實時精確位姿估計方法

有效

• 申請號: 201410022601.2
• 申請日: 2014-01-17
• 公開（公告）號: CN103777220A
• 公開（公告）日: 2014-05-07
• 發明（設計）人: 杜少毅;宋曄;劉娟;薛建儒;張春家;祝繼華
• 申請（專利權）人: 西安交通大學
• 主分類號: G01S19/53
• 分類號: G01S19/53;G01C21/00
• 代理公司: 西安通大專利代理有限責任公司 61200
• 代理人: 蔡和平

1.一種基于光纖陀螺、速度傳感器和GPS的實時精確位姿估計方法，其特征在于：包括以下步驟：

1）獲取車體的局部位姿

基于二維平面的航跡推算原理，通過由速度傳感器獲取的信息以及由光纖陀螺得到的航向信息估計車體當前時刻的局部位姿；

2）從GPS得到車體的全局位姿

實時解析出WGS84坐標系下車體的位置和三維速度，將解析出的位置和三維速度轉換到東北天坐標系下，然后利用轉換后的三維速度計算并濾波得到航向信息；

3）修正全局位姿

將歷史時刻的全局軌跡與局部軌跡進行配準，然后將當前時刻的局部位姿數據投射到全局位姿所在的坐標系下，得到當前時刻車體的全局位姿的預測值，利用預測值完成局部位姿數據對全局位姿數據的修正。

2.根據權利要求1所述一種基于光纖陀螺、速度傳感器和GPS的實時精確位姿估計方法，其特征在于：所述步驟1）的具體步驟為：實時獲取光纖陀螺輸出的局部航向角θ；從速度傳感器獲取實時的前輪速度以及前輪轉角，根據前后兩幀的速度信息計算相對位移，利用所述相對位移進行實時航跡推算獲得車體的局部位置信息，所述速度信息為前輪速度以及前輪轉角。

3.根據權利要求2所述一種基于光纖陀螺、速度傳感器和GPS的實時精確位姿估計方法，其特征在于：所述局部位置信息根據前后兩幀的速度信息和局部航向角θ求均值后實時累加積分獲得。

4.根據權利要求1所述一種基于光纖陀螺、速度傳感器和GPS的實時精確位姿估計方法，其特征在于：所述航跡推算的公式為：

Xk/k-1=f(Xk-1,θk)=xk-1+ΔDkcos(θk-1+θk2)yk-1+ΔDksin(θk-1+θk2)θk]]>

其中，X表示局部位姿，k-1表示前一幀、k表示當前幀，x表示局部位姿在x方向的位置坐標，y表示局部位姿在y方向的位置坐標，ΔD_k表示[k-1,k]時間內的相對位移，θ表示局部航向角。