CN103983263A - 一种采用迭代扩展卡尔曼滤波与神经网络的惯性/视觉组合导航方法 - Google Patents

Abstract

本发明涉及一种采用迭代扩展卡尔曼滤波与神经网络的惯性/视觉组合导航方法，属于复杂环境下的组合导航技术领域。在视觉信号有效时，利用移动机器人搭载的摄像头采集动态视频，通过图像特征提取和最近邻匹配法来确定摄像头的速度；使用迭代扩展卡尔曼滤波最优估计移动机器人的速度、加速度；并利用神经网络建立惯性导航系统的导航速度误差模型；在视觉信号失锁时，依靠之前训练获得的神经网络误差模型对导航系统速度误差进行补偿。该方法克服了在视觉信号失锁时，惯性/视觉组合导航系统无法提供持久的高精度导航问题，可应用于移动机器人在弱光或是无光等复杂环境下的长航时远距离高精度导航定位。

Description

一种采用迭代扩展卡尔曼滤波与神经网络的惯性/视觉组合导航方法

技术领域

本发明涉及一种采用迭代扩展卡尔曼滤波与神经网络的惯性/视觉组合导航方法，属于复杂环境下组合导航技术领域。

背景技术

近年来，伴随着计算机技术、电子技术、通讯技术、先进控制以及人工智能的迅猛发展，有关移动机器人技术的研究以及应用取得了巨大的进步。智能移动机器人作为集环境感知、动态决策以及实时行为控制与执行等众多特性为一体的复杂综合系统，目前已经在军事、民用和科学研究以及工业生产等领域获得越来越普遍的应用，用以代替人类来执行一些需要在恶劣条件或是危险条件下开展的工作。定位与导航作为室内移动机器人完成任务的首要前提，逐渐成为该领域的研究热点。然而，在外界无线电信号微弱、电磁干扰强烈等一系列复杂室内环境中，室内机器人获取导航信息的准确性、实时性及鲁棒性会受到很大的影响。如何实现在室内环境下获取有效的导航信息以满足高精度导航智能移动机器人的要求且尽量避免使其受到外界环境的影响，具有十分重要的科学理论意义以及工程应用价值。

惯性导航系统(Inertial Navigation System，INS)以牛顿经典力学定律为理论基础，根据惯性测量单元(陀螺仪和加速度计)测量得到的载体的角运动和线运动信息，通过积分运算得到载体的姿态、速度和位置信息。惯性导航系统不依赖任何外部信息，也不向外辐射能量，仅靠系统本身就能在全天候、全球范围内、所有介质环境中自主的进行三维定位和定向，具有机动性好、环境适应力强、隐蔽性好、导航参数和载体运动信息完备以及短时精度高等优点。惯性导航系统的导航误差主要由初始对准的精度、惯性测量单元的误差和载体的运动特性决定，其误差值随时间累积，导航精度随时间发散，不适合单独长时间工作。因此，惯性导航系统单独工作时，不能满足远程、长航等情形下对导航精度和稳定性的要求。

随着计算机视觉技术以及数字图像处理技术的飞速发展，通过采用视觉传感器来对周围环境进行感知进而是实现自身的定位与导航的视觉导航方式被越来越普遍的运用到移动机器人的导航研究当中。视觉导航相较其它导航方式具有信号探测范围广、信息获取丰富的特点，而且视觉传感器属于被动式传感器，不受外界电磁环境等的影响亦不会对环境造成二次污染。但是视觉导航易受敏感器作用距离和光照条件的限制且导航数据更新频率低。

惯性导航和视觉导航有其各自的特点，并具有互补的特性，将两种导航方法结合，取长补短，利用信息融合技术进行组合而成的惯性/视觉组合导航系统，可以最大限度地提高导航的精度和可靠性。但是其导航精度依然受到外界环境的限制。比如当移动机器人需要长期的运动在弱光条件下或是无光的环境中时，视觉信号可能会出现长时间的失锁，而惯性导航系统不能提供长时间高精度导航的缺点会导致惯性/视觉组合导航系统出现严重的精度下降。因此针对视觉长期失锁条件下的惯性导航系统的误差补偿研究显得尤为重要。

发明内容

本发明为了解决移动机器人运动在弱光或是无光环境时，惯性导航系统不能提供长时间高精度导航从而导致惯性/视觉组合导航系统出现严重的精度下降的缺点，提出一种采用迭代扩展卡尔曼滤波与神经网络的惯性/视觉组合导航方法。

本发明解决其技术问题所采用的技术方案为：在视觉信号有效时，利用移动机器人搭载的摄像头采集动态视频，通过图像特征提取和最近邻匹配法来确定摄像头的速度；根据惯性导航系统测得的航向角并结合摄像头的速度计算出移动机器人的速度；使用迭代扩展卡尔曼滤波最优估计移动机器人的速度、加速度；并利用神经网络建立惯性导航系统的导航速度误差模型；在视觉信号失锁时，依靠之前训练获得的神经网络误差模型对导航系统速度误差进行补偿得到移动机器人速度的最优估计。

进一步的，当视觉信号有效时，利用车载摄像头采集动态视频，采用SURF算法分别提取视频中的相邻两个图像帧中的SURF特征点，并根据最近邻匹配法对两帧图像上的SURF特征点进行匹配，确定摄像头在水平面上的速度V_x、V_y。

进一步的，由惯性导航系统测量得到的航向角结合摄像头在水平面上的速度V_x、V_y，计算得到移动机器人的东向速度和北向速度计算公式如下：

进一步的，利用迭代扩展卡尔曼滤波器，将移动机器人的速度和惯性导航系统测量得到的航向角变化量ω_Z进行数据融合，得到移动机器人的东向和北向的速度以及加速度的最优估计；

迭代扩展卡尔曼滤波器的系统方程以移动机器人的东向和北向的速度和加速度作为状态变量，则滤波器的系统方程如式(2)所示：

$\left[\begin{array}{c}{V}_{E,k+1}\\ V_{N,k+1}\\ {\mathrm{Acc}}_{E,k+1}\\ {\mathrm{Acc}}_{N,k+1}\end{array}\right]=\left[\begin{array}{cccc}1& 0& T& 0\\ 0& 1& 0& T\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right]\left[\begin{array}{c}{V}_{E,k}\\ V_{N,k}\\ {\mathrm{Acc}}_{E,k}\\ {\mathrm{Acc}}_{N,k}\end{array}\right]+{\mathrm{\ω}}_{4\×1}^{*}---\left(2\right)$

式中，V_E,k,V_N,k,Acc_E,k,Acc_N,k分别代表k时刻移动机器人的东向速度、北向速度、东向加速度和北向加速度，V_E,k+1,V_N,k+1,Acc_E,k+1,Acc_N,k+1分别代表k+1时刻移动机器人的东向速度、北向速度、东向加速度和北向加速度，T为相邻两帧图片的采样时间，也即数据的采样周期，为系统随机噪声；

滤波器的观测方程如式(3)所示：

$\left[\begin{array}{c}{\tilde{V}}_E\\ {\tilde{V}}_N\\ {\mathrm{\ω}}_Z\end{array}\right]=\left[\begin{array}{c}{V}_{E,k}\\ V_{N,k}\\ \frac{V_{N,k}\mathrm{Ac}{c}_{E,k}-V_{E,k}{\mathrm{Acc}}_{N,k}}{V_{E,k}^2+V_{N,k}^2}\end{array}\right]+{\mathrm{\υ}}_{3\×1}---\left(3\right)$

式中，υ_3×1为观测方程随机噪声。

进一步的，训练误差模型将惯性导航系统中测量得到东向速度和北向速度作为输入，将移动机器人东向和北向速度的最优估计与惯性导航系统测量得到的东向速度和北向速度作差，并将差值作为理想输出，对误差模型进行训练建模。

进一步的，所使用的训练误差模型为RBF神经网络模型。

本发明与现有技术相比的有益效果在于：

1、通过视觉获取室内移动机器人的东向和北向速度，仅需要车载摄像头，不需要借助其他外部设备，具有较好的独立性。

2、采用迭代扩展卡尔曼滤波器将视觉导航信息与惯性导航系统的导航信息进行数据融合，可有效的对系统线性化过程中所产生的截断误差进行补偿，得到的导航信息比上述任何单一导航方法得到的信息精度更高。

3、通过人工智能算法来对惯性导航系统的导航速度误差进行建模，从而实现在视觉信息发生失锁后对惯性导航系统的导航速度误差进行有效的补偿，进而提高了惯性/视觉组合导航的适用范围。

4、使用RBF神经网络对惯性导航系统的导航速度误差进行建模，可以获得精度较高的预测模型。

附图说明

图1为一种采用迭代扩展卡尔曼滤波与神经网络的惯性/视觉组合导航方法的速度误差训练框图。

图2为采用RBF神经网络的惯性/视觉组合导航方法的速度误差补偿框图。

具体实施方式

下面结合附图对本发明做更进一步的解释。

一种采用迭代扩展卡尔曼滤波与神经网络的惯性/视觉组合导航方法包括以下几个步骤：

步骤1：如图1所示，当视觉信号良好时，车载摄像头垂直向下放置，获取移动机器人运动过程中所经过路面的动态视频，采用SURF算法分别提取视频中的相邻两个图像帧中的SURF特征点，并记录特征点在图像坐标系中的位置坐标并根据最近邻匹配法对两帧图像上的SURF特征点进行匹配，来确定摄像头在水平面上的速度V_x、V_y；

设车载摄像头的投影中心距离地面的垂直距离为Z_R，摄像头成像平面上横轴与纵轴的归一化焦距分别为f₁＝f/dx,f₂＝f/dy，f为摄像头焦距，dx和dy分别为成像平面的横轴与纵轴上单位像素的尺寸大小，主光轴点坐标(c₁,c₂)，针孔摄像头模型的归一化映射关系如下：

$\left[\begin{array}{c}{x}_n\\ y_n\end{array}\right]=\left[\begin{array}{c}\frac{X_R}{Z_R}\\ \frac{Y_R}{Z_R}\end{array}\right]---\left(4\right)$

式中，(X_R,Y_R,Z_R)为真实的地面上的某一特定点在相机坐标系中的坐标，此特定点投影在图像坐标系上就是SURF特征点。考虑镜头畸变系数为[k₁,k₂,k₃,k₄,k₅]，其包括径向畸变系数和切向畸变系数，则相应的图像坐标系上的点可以表示为：

$\left[\begin{array}{c}{x}_d\\ y_d\end{array}\right]=(1+k_1{r}^2+k_2{r}^4+k_5{r}^6)\left[\begin{array}{c}{x}_n\\ y_n\end{array}\right]+t_d---\left(5\right)$

其中切向畸变向量 $t_d=\left[\begin{array}{c}{2k}_3{x}_n{y}_n+k_4(r^2+2x_n^2)\\ k_3(r^2+2y_n^2)+{2k}_4{x}_n{y}_n\end{array}\right],$ 则相应的像素点的最终坐标为：

$\left[\begin{array}{c}{x}_c\\ y_c\end{array}\right]=\left[\begin{array}{cc}{f}_1& 0\\ 0& f_2\end{array}\right]\left[\begin{array}{c}{x}_d\\ y_d\end{array}\right]+\left[\begin{array}{c}{c}_1\\ c_2\end{array}\right]---\left(6\right)$

假设镜头的畸变系数[k₁,k₂,k₃,k₄,k₅]可忽略，均为零，则：

$\left[\begin{array}{c}{x}_c\\ y_c\end{array}\right]=\left[\begin{array}{c}{f}_1\frac{X_R}{Z_R}\\ f_2\frac{Y_R}{Z_R}\end{array}\right]+\left[\begin{array}{c}{c}_1\\ c_2\end{array}\right]---\left(7\right)$

将(7)式对时间微分得：

$\left[\begin{array}{c}{V}_x\\ V_y\end{array}\right]=\left[\begin{array}{c}\frac{Z_R{v}_x}{f_1}+\frac{X_R{V}_z}{Z_R}\\ \frac{Z_R{v}_y}{f_2}+\frac{Y_R{V}_z}{Z_R}\end{array}\right]---\left(8\right)$

式中，V_x、V_y、V_z分别为坐标点X_R、Y_R、Z_R关于时间的导数，表示摄像头在相机坐标系x、y、z三个轴方向上的即时速度；v_x、v_y分别为坐标点x_c、y_c关于时间的导数，表示SURF特征点在图像坐标系x、y两个轴方向上的即时速度，将(7)式带入(8)式得：

$\left[\begin{array}{c}{V}_x\\ V_y\end{array}\right]=\left[\begin{array}{c}\frac{Z_R{v}_x}{f_1}+\frac{1}{f_1}(x_c-c_1)V_z\\ \frac{Z_R{v}_y}{f_2}+\frac{1}{f_2}(y_c-c_2)V_z\end{array}\right]---\left(9\right)$

若考虑地面水平则Z_R为常值，关于时间的导数为0，即V_z＝0，则有：

$\left[\begin{array}{c}{V}_x\\ V_y\end{array}\right]=\left[\begin{array}{c}\frac{Z_R{v}_x}{f_1}\\ \frac{Z_R{v}_y}{f_2}\end{array}\right]---\left(10\right)$

根据最近邻匹配法对两帧图像上的SURF特征点进行匹配，得到SURF特征点在图像上的横向位移变化Δx和纵向位移变化Δy，则有：

$\left[\begin{array}{c}{V}_x\\ V_y\end{array}\right]=\left[\begin{array}{c}\frac{Z_R\mathrm{\Δx}}{f_{1}T}\\ \frac{Z_R\mathrm{\Δy}}{f_{2}T}\end{array}\right]---\left(11\right)$

式中，T为相邻两帧图片的采样时间，也即数据的采样周期；

步骤2：根据步骤1获得的摄像头在水平面上的速度V_x,V_y，结合惯性导航系统测量得到的航向角计算得到移动机器人的东向速度和北向速度计算公式如下：

步骤3：利用迭代扩展卡尔曼滤波器，结合步骤2获得的和惯性导航系统测量得到的航向角变化量ω_Z进行数据融合，得到移动机器人的东向和北向的速度以及加速度的最优估计，记东向速度和加速度的最优估计分别为北向速度和加速度的最优估计分别为

迭代扩展卡尔曼滤波器的系统方程以移动机器人的东向和北向的速度和加速度作为状态变量，则滤波器的系统方程如式(13)所示：

$\left[\begin{array}{c}{V}_{E,k+1}\\ V_{N,k+1}\\ {\mathrm{Acc}}_{E,k+1}\\ {\mathrm{Acc}}_{N,k+1}\end{array}\right]=\left[\begin{array}{cccc}1& 0& T& 0\\ 0& 1& 0& T\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right]\left[\begin{array}{c}{V}_{E,k}\\ V_{N,k}\\ {\mathrm{Acc}}_{E,k}\\ {\mathrm{Acc}}_{N,k}\end{array}\right]+{\mathrm{\ω}}_{4\×1}^{*}---\left(13\right)$

式中，V_E,k,V_N,k,Acc_E,k,Acc_N,k分别代表k时刻移动机器人的东向速度、北向速度、东向加速度和北向加速度，V_E,k+1,V_N,k+1,Acc_E,k+1,Acc_N,k+1分别代表k+1时刻移动机器人的东向速度、北向速度、东向加速度和北向加速度，为系统随机噪声，且系统方程中的状态量初始值为实际测量所得的实验数据，通常均取为0；

观测方程如式(14)所示：

$\left[\begin{array}{c}{\tilde{V}}_E\\ {\tilde{V}}_N\\ {\mathrm{\ω}}_Z\end{array}\right]=\left[\begin{array}{c}{V}_{E,k}\\ V_{N,k}\\ \frac{V_{N,k}\mathrm{Ac}{c}_{E,k}-V_{E,k}{\mathrm{Acc}}_{N,k}}{V_{E,k}^2+V_{N,k}^2}\end{array}\right]+{\mathrm{\υ}}_{3\×1}---\left(14\right)$

式中，υ_3×1为观测方程随机噪声，且滤波协方差矩阵初始值 $P_0=\left[\begin{array}{cccc}1& 0& 0& 0\\ 0& 1& 0& 0\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right],$ 系统噪声协方差矩阵 $Q=\left[\begin{array}{cccc}0.02& 0& 0& 0\\ 0& 0.02& 0& 0\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right],$ 观测噪声协方差矩阵 $R=\left[\begin{array}{ccc}0.1& 0& 0\\ 0& 0.1& 0\\ 0& 0& 0.01\end{array}\right];$

步骤4：将步骤3获得的移动机器人东向和北向速度的最优估计与惯性导航系统测量得到的东向速度和北向速度作差，并将差值作为训练误差模型的理想输出，将惯性导航系统中测量得到东向速度和北向速度作为训练误差模型的输入，对RBF神经网络误差模型进行训练建模；

步骤5：如图2所示，当载体进入视觉信息失锁区域时，惯性/视觉组合导航系统进入速度误差补偿区域，在此区域，系统无法获得视觉测量信息，只能通过惯性导航系统完成自主导航。此时惯性导航系统根据在视觉信号有效时得到的RBF神经网络模型对惯性导航系统测量的速度信息进行误差补偿，得到在视觉信息失锁区域移动机器人东向和北向的最优速度估计和进而避免了因视觉信息缺失而导致的导航精度迅速下降的问题，具有良好的可行性和前瞻性。

以上所述仅是本发明的优选实施方式，应当指出，对于本技术领域的普通技术人员来说，在不脱离本发明原理的前提下，还可以做出若干改进和润饰，这些改进和润饰也应视为本发明的保护范围。

Claims (6)

1.一种采用迭代扩展卡尔曼滤波与神经网络的惯性/视觉组合导航方法，其特征在于：在视觉信号有效时，利用移动机器人搭载的摄像头采集动态视频，通过图像特征提取和最近邻匹配法来确定摄像头的速度；根据惯性导航系统测得的航向角并结合摄像头的速度计算出移动机器人的速度；使用迭代扩展卡尔曼滤波最优估计移动机器人的速度、加速度；并利用神经网络建立惯性导航系统的导航速度误差模型；在视觉信号失锁时，依靠之前训练获得的神经网络误差模型对导航系统速度误差进行补偿得到移动机器人速度的最优估计。 

2.如权利要求1所述的一种采用迭代扩展卡尔曼滤波与神经网络的惯性/视觉组合导航方法，其特征在于：当视觉信号有效时，利用车载摄像头采集动态视频，采用SURF算法分别提取视频中的相邻两个图像帧中的SURF特征点，并根据最近邻匹配法对两帧图像上的SURF特征点进行匹配，确定摄像头在水平面上的速度V_x、V_y。 

3.如权利要求1所述的一种采用迭代扩展卡尔曼滤波与神经网络的惯性/视觉组合导航方法，其特征在于：由惯性导航系统测量得到的航向角结合摄像头在水平面上的速度V_x、V_y，计算得到移动机器人的东向速度和北向速度计算公式如下： 

。

4.根如权利要求1所述的一种采用迭代扩展卡尔曼滤波与神经网络的惯性/视觉组合导航方法，其特征在于：利用迭代扩展卡尔曼滤波器，将移动机器人的速度和惯性导航系统测量得到的航向角变化量ω_Z进行数据融合，得到移动机器人的东向和北向的速度以及加速度的最优估计： 

迭代扩展卡尔曼滤波器的系统方程以移动机器人的东向和北向的速度和加速度作为状态变量，则滤波器的系统方程如式(2)所示： 

式中，V_E,k,V_N,k,Acc_E,k,Acc_N,k分别代表k时刻移动机器人的东向速度、北向速度、东向加速度和北向加速度，V_E,k+1,V_N,k+1Acc_E,k+1,Acc_N,k+1分别代表k+1时刻移动机器人的东向速度、北向速度、东向加速度和北向加速度，T为相邻两帧图片的采样时间，也即数据的采样周期， 为系统随机噪声； 

滤波器的观测方程如式(3)所示： 

式中，υ_3×1为观测方程随机噪声。 

5.如权利要求1所述的一种采用迭代扩展卡尔曼滤波与神经网络的惯性/视觉组合导航方法，其特征在于：训练误差模型将惯性导航系统中测量得到东向速度和北向速度作为输入，将移动机器人东向和北向速度的最优估计与惯性导航系统测量得到的东向速度和北向速度作差，并将差值作为理想输出，对误差模型进行训练建模。 

6.如权利要求1或5所述的一种采用迭代扩展卡尔曼滤波与神经网络的惯性/视觉组合导航方法，其特征在于：所使用的训练误差模型为RBF神经网络模型。