CN103777220A - 基于光纤陀螺、速度传感器和gps的实时精确位姿估计方法 - Google Patents

Abstract

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

Description

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

所述局部位置信息根据前后两帧的速度信息和局部航向角θ求均值后实时累加积分获得。

所述航迹推算的公式为：

$X_{k/k-1}=f(X_{k-1},{\mathrm{\θ}}_k)=\left(\begin{array}{c}{x}_{k-1}+\mathrm{\Δ}{D}_k\cos \left(\frac{{\mathrm{\θ}}_{k-1}+{\mathrm{\θ}}_k}{2}\right)\\ y_{k-1}+\mathrm{\Δ}{D}_k\sin \left(\frac{{\mathrm{\θ}}_{k-1}+{\mathrm{\θ}}_k}{2}\right)\\ {\mathrm{\θ}}_k\end{array}\right)$

其中，X表示局部位姿，k-1表示前一帧、k表示当前帧，x表示局部位姿在x方向的位置坐标，y表示局部位姿在y方向的位置坐标，ΔD_k表示[k-1,k]时间内的相对位移，θ表示局部航向角。

ΔD_k=ΔT_[k-1,k](v_k-1cosα_k-1+v_kcosα_k)/2

其中，ΔT_[k-1,k]为获得的前后两帧数据的时间差，v_k-1,v_k为前轮速度，α_k-1,α_k为前轮转角。

所述步骤3）中，用迭代最近点算法完成已知的有效的全局轨迹与局部轨迹的配准，从而获得全局轨迹与局部轨迹的变换关系，用所述变换关系进行全局位姿预测，利用预测的全局位姿判断当前获得的GPS数据是否有效，如果有效，则使用根据当前获得的GPS数据确定的全局位姿，如果无效，则使用预测的全局位姿。

所述步骤3）的具体步骤为：

a）全局轨迹与局部轨迹配准

以当前时刻为起点在已经产生的轨迹中选择对应的一段全局轨迹与局部轨迹，确定全局轨迹与局部轨迹后，使用迭代最近点算法计算使局部轨迹经过旋转平移与全局轨迹重合的旋转平移量，此旋转平移量为全局位姿预测的基准；

b）全局位姿预测

使用旋转平移量对当前时刻的局部位姿数据进行旋转平移，得到当前时刻全局位姿的预测值；

c）GPS数据有效性判断及处理

计算根据当前时刻GPS数据确定的全局位姿与步骤b）中所述预测值的距离偏差，如果距离偏差小于阈值，则将所述根据当前时刻GPS数据确定的全局位姿作为用于计算旋转平移量的全局轨迹中一点（即有效的全局轨迹）；如果距离偏差大于等于阈值，则将根据当前时刻GPS数据确定的全局位姿以及当前时刻的局部位姿数据标记为噪声信号，使用步骤b）中所述预测值。

所述阈值是由采用的GPS接收机精度决定。

若连续出现距离偏差大于等于阈值的情况，则使用噪声信号重新进行旋转平移量的计算。

本发明通过融合光纤陀螺、速度传感器和GPS的数据，可以将局部位姿和全局位姿有效地结合起来，其中局部位姿采用了基于车体运动模型的无人车局部定位算法获得，该算法主要基于二维平面的航迹推算原理，通过前一时刻的车体传感器的速度、航向信息估计车体当前的局部位姿，提高了局部定位***的精度；利用迭代最近点（ICP）算法配准全局和局部位姿，能够校正局部定位的累积误差，有效消除GPS的随机噪声，并在GPS完全失效或者出现故障的情况下保持位姿的精度。跑车试验结果表明，本发明提出的算法能够很好的融合多传感信息，基于航迹推算及ICP配准对GPS定位获得的位姿数据进行修正，能够实时获得连续平滑的全局位姿信息，在多树木、多建筑物的城区环境中提供可靠连续的定位信息。

本发明有以下几个特点：

1）根据车体运动模型，由速度传感器给出的车辆速度及前轮转角，获得车辆运动方向的速度（vcosα，v表示前轮速度，α表示前轮转角），计算相对位移；运用基于二维平面的航迹推算原理，通过前一时刻的车体传感器的速度、航向信息计算车体当前的局部位姿；

2）采用迭代最近点算法，计算旋转量和平移量，对局部位姿和全局位姿数据进行配准，将数据融合；GPS数据对局部定位的累积进行校正，而局部信息又可以消除GPS的随机噪声，两者相互弥补促进，使得该位姿估计方法在复杂环境及GPS失锁、故障的情况下仍然保持有效的位姿输出；

3）光纤陀螺受环境影响小，抗干扰能力强；速度传感器即为里程计，无须额外增加设备；因此该***具有稳定性好，可靠性高，成本低的特点。

附图说明

图1是本发明的流程示意图；

图2是计算局部位置信息的车体运动模型和二维航迹推算原理图，其中，（a）为车体运动模型；（b）为二维航迹推算原理；

图3是局部位姿与全局位姿数据融合的ICP（迭代最近点）算法流程框图；

图4是本发明实际应用的数据对比图，其中，（a）为城区环境跑车数据的整体比较；（b）和（c）为（a）中两处的局部放大图，分别展示无GPS数据及GPS数据有跳变和毛刺情况下的修正结果。

具体实施方式

下面结合附图对本发明作进一步说明。

参见图1，基于光纤陀螺、速度传感器和GPS的实时精确位姿估计方法分为三部分，包括的各个步骤如下：

1）由光纤陀螺和速度传感器得到局部位姿，具体步骤如下：

（1a）陀螺输出航向角θ信息；

（1b）从UDP（用户数据协议）包中解析出实时的前轮速度和前轮转角，从而得到速度信息；

（1c）根据前后两帧的速度信息，计算相对位移；

（1d）实时航迹推算获得局部位置信息；

根据图2（a），可以获得实时的后轮车速（车辆运动方向的速度）：速度传感器读取的是车体前轮转速信息，选取车体坐标系原点位于车体后轴中心，车转弯时后轮速度是前轮速度沿车体方向的分量；由传感器信息可以读取车前轮转角α

v^m=[vsinα  vcosα]

其中，v^m表示车体前轮的二维速度向量，vsinα表示前轮速度的侧向分量，vcosα表示前轮速度的纵向分量，即实时的后轮车速数据。根据图2（b）可以计算出智能车的局部位姿：通过陀螺得到当前时刻的航向信息；车体实时位置信息根据前后两帧的速度信息和航向角θ求均值后实时累加积分获得。[k-1,k]时间内相对位移表示为：

ΔD_k=ΔT_[k-1,k](v_k-1cosα_k-1+v_kcosα_k)/2

其中，ΔT_[k-1,k]由获得前后两帧数据的时间差得到，前轮速度v_k-1,v_k以及前轮转角α_k-1,α_k由速度传感器给出。航向角θ由陀螺实时给出，航迹推算公式为：

$X_{k/k-1}=f(X_{k-1},{\mathrm{\θ}}_k)=\left(\begin{array}{c}{x}_{k-1}+\mathrm{\Δ}{D}_k\cos \left(\frac{{\mathrm{\θ}}_{k-1}+{\mathrm{\θ}}_k}{2}\right)\\ y_{k-1}+\mathrm{\Δ}{D}_k\sin \left(\frac{{\mathrm{\θ}}_{k-1}+{\mathrm{\θ}}_k}{2}\right)\\ {\mathrm{\θ}}_k\end{array}\right)$

其中，X表示局部位姿，k-1表示前一帧、k表示当前帧，x表示局部位姿在x方向的位置坐标，y表示局部位姿在y方向的位置坐标，ΔD_k表示[k-1,k]时间内的相对位移，θ表示局部航向角。

2）从GPS得到全局位姿，具体步骤如下：

（2a）通过UDP包实时解析出WGS84坐标系下的车体的位置和三维速度；

（2b）将解析出的位置和三维速度转换到东北天坐标系下；

（2c）用三维速度进行正切计算及均值滤波得到航向信息，结合位置得到GPS全局位姿。

3）将局部位姿和GPS全局位姿进行融合，得到修订的全局位姿，具体步骤如下：

（3a）全局轨迹与局部轨迹配准

第一步，以当前时刻为起点在已经产生轨迹中选择对应的一段全局轨迹与局部轨迹；

第二步，确定全局与局部轨迹后，则使用ICP（迭代最近点）配准算法计算可以使局部轨迹经过旋转平移与全局轨迹重合的旋转平移量，此旋转平移量为全局位姿预测的基准。

ICP配准是一个逐步迭代的循环过程，由图3可知，ICP算法主要包括以下两个基本步骤：

首先，根据第二步的变换来建立当前两个点集（局部位姿点集P和全局位姿点集M）之间的对应关系：

$c_k\left(i\right)=\underset{j\∈\{\mathrm{1,2},...,N_m\}}{\arg \min }\left(\right||(R_{k-1}{\stackrel{\→}{p}}_i+{\stackrel{\→}{t}}_{k-1})-{\stackrel{\→}{m}}_j{\left|\right|}_2^2),i=\mathrm{1,2},...,N_p$

其次，计算两个点集和

之间新的变换：

$(R^{*},{\stackrel{\→}{t}}^{*})=\underset{R^{T}R=I_m,\det \left(R\right)=1,\stackrel{\→}{t}}{\arg \min }\left(\underset{i=1}{\overset{N_p}{\mathrm{\Σ}}}\right||R(R_{k-1}{\stackrel{\→}{p}}_i+{\stackrel{\→}{t}}_{k-1})+\stackrel{\→}{t}-{\stackrel{\→}{m}}_{c_k\left(i\right)}{\left|\right|}_2^2)$

更新第k步的变换R_k和

$R_k=R^{*}{R}_{k-1},t_k=R^{*}{\stackrel{\→}{t}}_{k-1}+{\stackrel{\→}{t}}^{*}$

其中，N_p、N_m分别为局部位姿点集P和全局位姿点集M中点的数量，R、

分别为点集变换关系中的旋转矩阵和平移矩阵，

为点集P和M的数据矩阵。

图3中T即为局部位姿点集P和全局位姿点集M的旋转平移关系，当点集P做T变换的结果与点集M相似程度达到阈值要求，或迭代次数超过所设极限时，停止迭代，得到最优T，此时可用局部位姿根据此变换关系T预测全局位姿。

（3b）全局位姿预测

由（3a）可以得到局部轨迹变换到全局轨迹上的旋转平移量，使用该旋转平移量对当前时刻的局部数据进行旋转平移，作为当前全局位姿的预测值。

（3c）GPS数据有效性判断及处理

第一步，根据（3b）预测的当前的全局位姿，此时需要确定当前时刻GPS数据确定的全局位姿与预测值的距离偏差，以此来判断GPS数据是否有效；

第二步，如果所述根据当前时刻GPS数据确定的全局位姿与当前预测值的距离偏差小于阈值，则让其参加（3a）中的旋转平移量计算；如果所述根据当前时刻GPS数据确定的全局位姿与当前预测值的距离偏差大于等于阈值，则将根据当前时刻GPS数据确定的全局位姿与由光纤陀螺和速度传感器得到的局部位姿数据标记为噪声。噪声处理对于***的及时纠错非常重要，一旦全局位姿预测值连续出错，则使用噪声信号重新进行旋转平移量的计算。所述阈值是由采用的GPS接收机精度决定，例如，若其精度为2米，在半径为2米的圆内，最大偏差为4米，因此该阈值为4。

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

根据图4，可以看出本发明在实际应用中的效果。该数据是无人车在城区环境中跑车结果，行车环境有较严重的树木、高楼遮档。图4（a）为数据的整体比较，GPS原始数据有缺失、跳变及毛刺，而修正后的全局位姿数据则十分连续平滑。图4（b）为图4（a）中左侧数据局部放大，这一段中，GPS原始数据缺失；图4（c）为图4（a）中下方数据局部放大，这一段中，GPS原始数据出现跳变和毛刺；但经过本发明中算法的修正后，能给出正确的行车路线，在很大程度上消除了GPS数据缺失、跳变和毛刺引起的偏差，是十分可靠的。

Claims (9)

1.一种基于光纤陀螺、速度传感器和GPS的实时精确位姿估计方法，其特征在于：包括以下步骤：

1）获取车体的局部位姿

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

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

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

3）修正全局位姿

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

2.根据权利要求1所述一种基于光纤陀螺、速度传感器和GPS的实时精确位姿估计方法，其特征在于：所述步骤1）的具体步骤为：实时获取光纤陀螺输出的局部航向角θ；从速度传感器获取实时的前轮速度以及前轮转角，根据前后两帧的速度信息计算相对位移，利用所述相对位移进行实时航迹推算获得车体的局部位置信息，所述速度信息为前轮速度以及前轮转角。

3.根据权利要求2所述一种基于光纤陀螺、速度传感器和GPS的实时精确位姿估计方法，其特征在于：所述局部位置信息根据前后两帧的速度信息和局部航向角θ求均值后实时累加积分获得。

4.根据权利要求1所述一种基于光纤陀螺、速度传感器和GPS的实时精确位姿估计方法，其特征在于：所述航迹推算的公式为：

$X_{k/k-1}=f(X_{k-1},{\mathrm{\θ}}_k)=\left(\begin{array}{c}{x}_{k-1}+\mathrm{\Δ}{D}_k\cos \left(\frac{{\mathrm{\θ}}_{k-1}+{\mathrm{\θ}}_k}{2}\right)\\ y_{k-1}+\mathrm{\Δ}{D}_k\sin \left(\frac{{\mathrm{\θ}}_{k-1}+{\mathrm{\θ}}_k}{2}\right)\\ {\mathrm{\θ}}_k\end{array}\right)$

其中，X表示局部位姿，k-1表示前一帧、k表示当前帧，x表示局部位姿在x方向的位置坐标，y表示局部位姿在y方向的位置坐标，ΔD_k表示[k-1,k]时间内的相对位移，θ表示局部航向角。

5.根据权利要求4所述一种基于光纤陀螺、速度传感器和GPS的实时精确位姿估计方法，其特征在于：

ΔD_k=ΔT_[k-1,k](v_k-1cosα_k-1+v_kcosα_k)/2

其中，ΔT_[k-1,k]为获得的前后两帧数据的时间差，v_k-1,v_k为前轮速度，α_k-1,α_k为前轮转角。

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的实时精确位姿估计方法，其特征在于：若连续出现距离偏差大于等于阈值的情况，则使用噪声信号重新进行旋转平移量的计算。

Images