[发明专利]基于光纤陀螺、速度传感器和GPS的实时精确位姿估计方法

权利要求书

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]时间内的相对位移，θ表示局部航向角。