CN103411621A - 一种面向室内移动机器人的光流场视觉/ins组合导航方法 - Google Patents

Abstract

一种面向室内移动机器人的光流场视觉导航/INS组合导航方法，其采用基于光流场的视觉导航和INS两种导航方式，移动机器人通过车载摄像头采集车辆的动态视频，在视频中获取视频光流特征，根据特征计算移动机器人在载体坐标系下的横向速度（V_x）、纵向速度（V_y），并与INS测量得到的航向角计算机器人的东向速度（V_E）和北向速度（V_N）。将计算得到的V_E、V_N和INS测量得到的航向角变化量ω_Z共同输入到扩展卡尔曼滤波器中进行数据融合，得到比基于光流场导航和INS两种导航方式精度更高的移动机器人的东向和北向的速度和加速度的最优预估。通过本发明的方法可为移动机器人提供更加精确的导航信息，扩大了室内机器人定位的范围。

Description

一种面向室内移动机器人的光流场视觉/INS组合导航方法

技术领域

一种面向室内移动机器人的光流场视觉/INS组合导航方法，属于复杂环境下组合定位技术领域。

背景技术

近年来，随着计算机技术、信息技术、通讯技术、微电子技术和机器人技术的飞速发展，移动机器人技术的研究与应用取得了长足的进步。室内机器人或运动目标的导航与定位作为实现生产过程智能化和完全自主化的关键技术，逐渐成为目前该领域的研究热点。然而，在外界无线电信号微弱、电磁干扰强烈等一系列复杂室内环境中，对移动目标导航信息获取的准确性、实时性及鲁棒性有很大的影响。如何将室内环境下获取的有限信息进行有效的融合以满足智能移动机器人或运动目标高导航精度的要求，消除外界环境的影响，具有重要的科学理论意义和实际应用价值。

目前移动机器人导航的方式按照传感器的类别大致可分为两大类：基于计算机视觉的导航技术和基于非视觉传感器（如超声波、激光等）的导航技术。非视觉传感器具有感知测量直接，对传感器数据处理能力要求低等特点；但不幸的是，这类传感器的使用将会对环境产生“污染”，特别是在有多个传感器共同工作的情况下，相互之间将会产生严重的干扰。但视觉传感器，它们的采样过程不会对环境产生污染，多个传感器之间也不会产生相互干扰。这种传感器采集的数据往往需要大量的运算，早期很难做到实时。近几年来由于图像处理技术和计算机运算能力的大大增强以及大量数字图像处理设备性价比的提高，加之视觉系统具有信号探测范围宽、目标信息完整等优势，基于视觉传感器的机器人导航己引起人们的极大关注。但是视觉导航系统易受敏感器作用距离和光照条件的限制且导航数据更新频率低。

惯性导航系统以牛顿力学定律为基础，通过测量载体内部载体运动的加速度，经积分运算得到载体的速度和位置等导航信息，实现导航。惯性导航的优点是不需要外部参数作为参考，自身可以提供完全的自主性的和完备的导航信息(机器人的位置、速度及姿态等)，缺点是随时间的增长，移动机器人运动路程逐渐增加，定位的准确度会逐渐的下降。所以惯性传感器适用于短时间的移动机器人精确导航定位。

发明内容

为了解决上述问题，本发明提出了一种面向室内移动机器人的光流场视觉/惯性导航系统(INS)组合导航方法，属于复杂环境下组合定位技术领域。该方法采用基于光流场的视觉导航和INS两种导航方式，移动机器人通过车载摄像头采集车辆的动态视频，在视频中的相邻图片帧之间获取视频光流特征，根据光流特征计算移动机器人在载体坐标系下的横向速度V_x、纵向速度V_y，并与INS测量得到的航向角计算机器人的东向速度V_E和北向速度V_N。将计算得到的V_E、V_N和INS测量得到的航向角变化量ω_Z共同输入到扩展卡尔曼滤波器中进行数据融合，得到比基于光流场视觉导航和INS两种导航方式精度更高的移动机器人的东向和北向的速度和加速度的最优预估。上述方法的提出，为移动机器人提供更加精确的导航信息，扩大了室内机器人定位的范围。

本发明为解决其技术问题，提出如下技术方案：

（1）车载摄像头垂直向下放置，从车载摄像头拍摄的图像帧中选取一个矩形区域作为感兴趣区域ROI（Region of interest，感兴趣区域），后续的处理均在ROI中进行。

（2）在上述ROI中根据Shi-Tomasi角点检测方法提取N个角点（图像中的梯度值和梯度方向变化速率都很高的点），记录角点在图像坐标系下的坐标；

（3）采用图像金字塔Lucas-Kanade光流法对选取的步骤（2）中选取的角点进行跟踪，获取所有角点在下一帧中的图像坐标系中的坐标；

（4）采用随机抽样一致性算法（RANSAC）剔除跟踪异常的角点比如说在一帧图像中检测出20个角点，通过光流法跟踪得到角点在下一帧位置，如果有19个角点纵向位移在10±5个像素点之间，另外一个远大于这个范围，那么这个点就认为跟踪异常），只对跟踪正常的角点进行数据处理；

（5）根据所有跟踪正常的角点在相邻两帧图像中对应的坐标，计算所有正常角点在横向和纵向的位置变化(△x,△y)；

（6）假设车载摄像头的投影中心距离地面的垂直距离为Z_R，焦距为f₁,f₂，主光轴点坐标(c₁,c₂)，车载摄像头在水平面上的速度分量分别为V_x,V_y，镜头畸变系数为[k₁,k₂,k₃,k₄,k₅]，其包括径向畸变系数和切向畸变系数，所述车载摄像头模型的归一化映射关系如下：

$\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(1\right)$

其中(X_R,Y_R,Z_R)为与图像坐标系上跟踪正常的角点相对应的地面上的真实点在相机坐标系中的坐标，考虑镜头畸变系数，则相应的图像坐标系上的点可以表示为：

$\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(2\right)$

其中 $r=\sqrt{x_n^2+y_n^2},$ 切向畸变向量 $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],$ 点(c₁,c₂)为车载摄像头光轴与图像平面的交点，则像素点的最终坐标为：

$\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(3\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(4\right)$

（4）式对时间微分得：

$\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(5\right)$

考虑地面水平则Z_R为常值，对时间导数为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(6\right)$

由（4）式带入得：

$\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(7\right)$

考虑地面水平则Z_R为常值，对时间导数为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(8\right)$

（7）与惯性导航系统（INS）测量得到的航向角

，计算机器人的东向速度V_E和北向速度V_N，计算公式如下：

（8）将计算得到的V_E、V_N和INS测量得到的航向角变化量ω_Z共同输入到卡尔曼滤波器中进行数据融合；

（9）扩展卡尔曼滤波器的系统方程以每一时刻视觉测量得到V_E和V_N以及INS每一时刻东向和北向的加速度作为状态变量，以每一时刻视觉测量得到的V_E、V_N和INS测量得到的航向角变化量ω_Z作为观测量，滤波器的系统方程如式(11)所示：

其中

为系统噪声，观测方程如式(12)所示：

其中υ_3×1为观测方程噪声,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为相邻时刻间隔周期。

本发明的有益效果如下：

1、通过光流场视觉获取室内移动机器人的东向和北向速度，仅需要车载摄像头，不需要借助其他外部设备，具有较好的独立性。与传统的通过码盘获取移动机器人速度的方法相比，克服了机器人在车轮打滑时车速测量不准确的缺点，具有较高的精度。

2、将计算得到的V_E、V_N和INS测量得到的航向角变化量ω_Z共同输入到扩展卡尔曼滤波器中进行数据融合，得到的导航信息，比上述任何单一导航方法得到的信息精度更高。

3、可用于室内，地下矿井等密闭复杂环境下的长距离高精度目标定位跟踪。

附图说明

图1为用于一种面向室内移动机器人的光流场视觉/INS组合导航方法的系统示意图。

图2为用于一种面向室内移动机器人的光流场视觉/INS组合导航方法的控制方法示意图。

图3为本发明的方法流程图。

具体实施方式

下面结合附图对本发明创造做进一步详细说明。

如图1所示，一种面向室内移动机器人的光流场视觉/INS的组合导航方法的系统，包括视觉摄像头、惯性测量单元(IMU)导航模块和数据处理计算机。

扩展卡尔曼滤波器的系统方程以INS每一时刻东向和北向的速度和加速度作为状态变量，以每一时刻视觉测量得到的V_E、V_N和INS测量得到的航向角变化量ω_Z作为观测量。滤波器的系统方程如式(13)所示：

其中为系统噪声，观测方程如式(14)所示：

其中υ_3×1为观测方程噪声,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为相邻时刻间隔周期。

通过扩展卡尔曼滤波器中的数据融合，得到比基于光流场导航和INS两种导航方式精度更高的移动机器人的东向和北向的速度和加速度的最优预估。

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

Claims (2)

1.一种面向室内移动机器人的光流场视觉/INS组合导航方法，其包括下列步骤：

（1）车载摄像头垂直向下放置，从车载摄像头拍摄的图像帧中选取一个矩形区域作为感兴趣区域ROI，后续的处理均在ROI中进行；

（2）在上述ROI中根据Shi-Tomasi角点检测方法提取N个角点，记录所有角点在图像坐标系下的坐标；

（3）采用图像金字塔Lucas-Kanade光流法对选取的步骤（2）中选取的角点进行跟踪，获取所有角点在下一帧中的图像坐标系中的坐标；

（4）采用随机抽样一致性算法RANSAC剔除跟踪异常的角点，只对跟踪正常的角点进行数据处理；

（5）根据所有跟踪正常的角点在相邻两帧图像中对应的坐标，计算所有正常角点在横向和纵向的位置变化(△x,△y)；

（6）假设车载摄像头的投影中心距离地面的垂直距离为Z_R，焦距为f₁,f₂，主光轴点坐标(c₁,c₂)，车载摄像头在水平面上的速度分量分别为V_x,V_y，镜头畸变系数为[k₁,k₂,k₃,k₄,k₅]，其包括径向畸变系数和切向畸变系数，所述车载摄像头模型的归一化映射关系如下：

$\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(1\right)$

其中(X_R,Y_R,Z_R)为与图像坐标系上跟踪正常的角点相对应的地面上的真实点在相机坐标系中的坐标，考虑镜头畸变系数，则相应的图像坐标系上的点可以表示为：

$\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(2\right)$

其中 $r=\sqrt{x_n^2+y_n^2},$ 切向畸变向量 $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],$ 点(c₁,c₂)为车载摄像头光轴与图像平面的交点，则像素点的最终坐标为：

$\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(3\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(4\right)$

（4）式对时间微分得：

$\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(5\right)$

考虑地面水平则Z_R为常值，对时间导数为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(6\right)$

由（4）式带入得：

$\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(7\right)$

考虑地面水平则Z_R为常值，对时间导数为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(8\right)$

（7）与惯性导航系统（INS）测量得到的航向角

，计算机器人的东向速度V_E和北向速度V_N，计算公式如下：

（8）将计算得到的V_E、V_N和INS测量得到的航向角变化量ω_Z共同输入到卡尔曼滤波器中进行数据融合；

（9）扩展卡尔曼滤波器的系统方程以每一时刻视觉测量得到V_E和V_N以及INS每一时刻东向和北向的加速度作为状态变量，以每一时刻视觉测量得到的V_E、V_N和INS测量得到的航向角变化量ω_Z作为观测量，滤波器的系统方程如式(11)所示：

其中

为系统噪声，观测方程如式(12)所示：

其中υ_3×1为观测方程噪声，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为相邻时刻间隔周期。

2.如权利要求1所述一种面向室内移动机器人的光流场视觉/INS组合导航方法，其中所述异常角点为角点纵向位移超过预定阈值的角点。

Images