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

说明书

技术领域

本发明涉及移动机器人、导航定位和数据融合领域，具体涉及一种利用光纤陀螺和智能车的里程计等传感器计算无人车局部姿态信息、利用GPS获得全局位姿、将无人车局部位姿和GPS位置进行配准得到修订的全局位姿的方法。

背景技术

随着科学技术的不断发展，无人驾驶车辆（无人车）在采矿业，货物运输，农业自动化和军事领域有着越来越广泛的应用。导航定位系统为无人车提供实时的位置和姿态信息，保证无人车按照正确路线完成自主导航和精确控制，是无人车系统的重要部分。目前的无人车定位方式主要有四类，GPS系统、航迹推算系统（Dead Reckoning System，DRS），将两者结合起来的组合导航系统，还有基于视觉/激光的定位系统。

第一类，全球定位系统。GPS系统能够在全球范围内为用户提供精确的、实时的位置和速度信息，是目前使用最为广泛的导航定位系统。GPS定位基于卫星测距原理，不存在累积误差，长时间范围内的性能优越。然而，在城区、峡谷、隧道和多树木的环境中，卫星信号容易受到干扰，GPS定位性能下降，甚至失去定位功能。

第二类，航迹推算系统。DRS属于局部定位系统，是陆地轮式车辆特有的低成本局部定位技术，主要通过里程计的速度信息和陀螺的航向信息推算车体的位置和姿态。DRS具有高度自治性，几乎不受外界环境干扰，更新频率快，短时间内定位精度高。然而，受传感器固有测量误差的影响，DRS的误差随时间迅速累积，无法保证长时间的定位精度。

第三类，组合导航系统。无人车要求导航系统提供连续不间断的高精度定位信息，这是目前任何一种单一的定位系统无法满足的，融合GPS和其他定位系统的组合导航方法已经成为定位技术研究的热点。

由于成本低、短时间内精度高，航迹推算系统常与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的定位性能。但滤波方法在处理随机噪声时只能加入单一的高斯噪声模型，难以对噪声进行多方面的估计和处理。

另外，也有GPS/INS一体式封装设备可供选择，具有紧耦合和松耦合两种组合方式。一体式在安装、操作等方面都是比较方便的。但目前，中高精度的GPS/INS组合导航系统大多售价昂贵，让用户望而却步。

第四类，利用视觉和激光等传感器也可以对载体的位置和姿态信息进行估计。基于视觉/激光的定位系统成本较低、信息量大，已经成为人工智能和定位导航领域研究的一个热点。然而，激光/视觉定位系统容易受外界环境的干扰，定位效果不稳定，目前主要应用于环境较为简单的室内机器人定位和辅助GPS、DRS等定位技术中。

发明内容

本发明的目的在于克服现有GPS定位方法的缺点，提供一种基于光纤陀螺、速度传感器和GPS的实时精确位姿估计方法。

为达到上述目的，本发明采用了以下技术方案。

1）获取车体的局部位姿

基于二维平面的航迹推算原理，通过由速度传感器获取的信息以及由光纤陀螺得到的航向信息估计车体当前时刻的局部位姿；

2）从GPS得到车体的全局位姿

实时解析出WGS84坐标系下车体的位置和三维速度，将解析出的位置和三维速度转换到东北天坐标系下，然后利用转换后的三维速度计算并滤波得到航向信息；

3）修正全局位姿

将历史时刻的全局轨迹（全局位姿的数据点集）与局部轨迹（局部位姿的数据点集）进行配准，然后将当前时刻的局部位姿数据投射到全局位姿所在的坐标系下，得到当前时刻车体的全局位姿的预测值，利用预测值完成局部位姿数据对全局位姿数据的修正。

所述步骤1）的具体步骤为：实时获取光纤陀螺输出的局部航向角θ；从速度传感器获取实时的前轮速度以及前轮转角，根据前后两帧的速度信息计算相对位移，利用所述相对位移进行实时航迹推算获得车体的局部位置信息，所述速度信息为前轮速度以及前轮转角。