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

摘要

本发明提供一种基于光纤陀螺、速度传感器和GPS的实时精确位姿估计方法，其中基于车体运动模型的无人车局部定位算法主要基于二维平面的航迹推算原理，通过前一时刻的车体传感器的速度、航向信息估计车体当前的局部位姿，并利用线性模型补偿陀螺航向的累积误差，提高了局部定位系统的精度；而利用迭代最近点算法配准GPS和局部位姿测量系统的位姿估计方法能够校正局部定位的累积误差，有效消除GPS的随机噪声，并在GPS完全失效或者出现故障的情况下保持位姿的精度，跑车试验结果表明，本发明提出的方法能够很好的融合多传感信息，在多树木、多建筑物的城区环境中可以为无人车提供可靠连续的定位信息，此外，该方法还具有很好的实时性。

主权项

一种基于光纤陀螺、速度传感器和GPS的实时精确位姿估计方法，其特征在于：包括以下步骤：1）获取车体的局部位姿基于二维平面的航迹推算原理，通过由速度传感器获取的信息以及由光纤陀螺得到的航向信息估计车体当前时刻的局部位姿；2）从GPS得到车体的全局位姿实时解析出WGS84坐标系下车体的位置和三维速度，将解析出的位置和三维速度转换到东北天坐标系下，然后利用转换后的三维速度计算并滤波得到航向信息；3）修正全局位姿将历史时刻的全局轨迹与局部轨迹进行配准，然后将当前时刻的局部位姿数据投射到全局位姿所在的坐标系下，得到当前时刻车体的全局位姿的预测值，利用预测值完成局部位姿数据对全局位姿数据的修正。