一种基于角度光流法的机器人运动信息估计方法与流程

本发明属于计算机视觉领域，特别涉及一种无人机悬停或移动机器人的运动信息估计方法。

背景技术：

虽然现在已有许多方法使用多个相机或多个主动传感器来提供精确测量进行导航，但它们的感知范围相对比较局限，并且对于小型机器人来说笨重而且成本高，因此，使用单目相机是更好的选择，可以实现减轻重量以及降低功耗的目的。

早期的光流法之一是horn-schunck光流法，它是基于相同速率的明亮图案几乎在图像中的所有地方都平滑变化的假设，使用相位相关法，通过块匹配计算光流，接着通过一个额外的优化步骤在几个候选之间找到更平滑的运动场。为了减少噪声影响，提出了一些基于特征跟踪的方法，例如，fast算法。benosman表明，通过估计时空域中的平面，可以以线性方式从每个事件周围的局部窗口估计光流。这些算法都需要大量手工计算的异常值抑制方案，它们并不能正确地对传感器的输出进行建模。

因此，可以使用基于图像生成帧的事件相机采集的数据来训练网络，例如由brandli等人开发的动态和主动感知像素视觉传感器(davis)，避免了对昂贵的数据标记的需要。但是，对于嵌入式系统，昂贵的davis并不适用。虽然事件相机可以实现高帧率和低功率性能，但目前它太昂贵，而且这种相机已经失去了基本的照相能力，并且现有方法只能提供两个自由度的相机信息，即在x轴和y轴上的位移信息，不能提供相机的旋转角度信息。

现有的光流法主要通过寻找图像序列中的像素点在时域上的变化，相邻帧之间的相关性来找到上一帧跟当前帧之间存在的对应关系，从而计算出相邻帧之间物体的运动信息。在实际应用光流法进行运动物体检测中，由于其具有计算量很大，计算耗时长，并且对于变化的光线敏感的缺点，无法保证其实时性和实用性，并不能有效地应用到运动信息的估计中。

技术实现要素：

本发明的目的，在于提供一种基于角度光流法的机器人运动信息估计方法，其可直接计算角度偏移量，最终获得准确的摄像机位置和角度估计，减少运动信息估计的计算强度，更适用于工程实践。

为了达成上述目的，本发明的解决方案是：

一种机器人运动信息估计方法，包括如下步骤：

步骤1，获取机器人移动的连续两帧图像i，j，提取特征点；

步骤2，计算特征点在对应两帧图像i，j时刻的角点方向θ；

步骤3，基于灰度不变性假设计算x，y，θ，t与u，v，ω的关系式；

步骤4，在特征点周围选择一个w×w块，得到w²对特征点，然后假设窗口中的所有像素具有相同的运动，考虑一个大小为w×w的窗口，其中包含w²个像素，则窗口中的所有像素具有相同的运动，从而得到w²个方程式，求解后，得到机器人此刻x，y方向的速度u，v以及航向角速度ω，得到此时机器人的位置状态信息。

上述步骤1中，利用机器人上的单目相机获取图像。

上述步骤1中，利用orientedfast方法提取特征点。

上述步骤2中，利用灰度质心法确定角点方向θ，具体过程是：首先定义块的力矩：

mpq＝∑x，yx^py^qi(x，y)

其中，mpq表示特征点领域的(p+q)阶矩，i(x，y)表示特征点坐标，x，y分别表示特征点在坐标轴x，y上的坐标，x^p指x的p次方，y^q指y的q次方；

通过这些力矩，找到质心为：

构造一个向量从角的中心o到质心则块的方向简化为：

θ＝atan2(m01，m10)

得到每个特征点在图像坐标系中的坐标。

上述步骤3的具体内容是：

假设第一帧图像i有一个图像点u＝[uxuyuθ]^t，当i(u)和j(v)相同时，第二帧图像j上的位置v＝u+d＝[ux+dxuy+dyuθ+dθ]^t，向量是d＝[dxdydθ]^t图像偏移量，对其进行一阶泰勒级数展开的公式：

基于灰度不变性的假设：

i(x+dx，y+dy，θ+dθ，t+dt)＝i(x，y，θ，t)

i(x，y，θ，t)表示在时间为t，位置为(x，y)，角度为θ的像素位置；

从而得到：

其中，ix，iy，iθ表示图像点k处i(x，y，θ，t)对x，y，θ的偏导；u，v，ω表示x，y，θ方向的速度。

上述步骤4中，w²个方程式的表达式是：

等式就变成：

其中：

使用最小二乘法来求解这个超定线性方程组：

从而得到机器人此刻x,y方向的速度u,v以及航向角速度ω。

采用上述方案后，本发明进行物体运动信息估计时，仅使用一个单目相机通过orientedfast方法使每个特征点包含具有位置与时间的信息为i(x,y,θ，t)，利用灰度不变性假设最终获得物体此刻运动信息u，v,ω，实现这一时刻物体的运动信息估计。本发明将基于特征方法丰富的特征信息与光流法的精度和速度相结合，通过更便捷的计算得到运动物体在任意水平面上的航向角信息，减少运动信息估计的计算强度，更适用于工程实践。与经典光流法相比，本发明可以直接计算角度偏移量，最终获得准确的摄像机位置和角度估计，主要可应用于无人机的悬停技术以及移动机器人的定位技术中去。

附图说明

图1是本发明的流程图；

图2是角度光流示意图。

具体实施方式

以下将结合附图，对本发明的技术方案及有益效果进行详细说明。

如图1所示，本发明提供一种基于角度光流法的机器人运动信息估计方法，包括如下步骤：

步骤1，通过机器人上的单目相机获得连续两帧图像i，j，利用orb的作者rublee提出的orientedfast方法(具有旋转不变性的特征点检测方法)提取特征点；

步骤2，计算特征点在对应两帧图像i，j时刻的角点方向θ；

具体来说，利用灰度质心法来确定角点方向θ，灰度质心假设角的强度可以与其中心的强度相抵消，并且该向量可用于估算方向。rosin将块的力矩定义为：

mpq＝∑x，yx^py^qi(x，y)(1)

mpq表示特征点领域的(p+q)阶矩，例如图像的0阶矩为：m00＝∑x，yi(x，y)，1阶矩为m10＝∑x，vxi(x，y)。其中，i(x，y)表示特征点坐标，x，y分别表示特征点在坐标轴x，y上的坐标，x^p指x的p次方，y^q指y的q次方。

通过这些力矩，我们可以找到质心：

其中，代表了图像像素在x方向上的偏重，这就是重心x的坐标，m10＝∑x，yxi(x，y)表示像素在x方向上的累加，m00＝∑x，yi(x，y)表示像素块总和；代表了图像像素在y方向上的偏重，这就是重心y的坐标。m01＝∑x，yyi(x，y)表示像素在y方向上的累加

我们构造一个向量从角的中心o到质心则块的方向可以简化为：

θ＝atan2(m01，m10)(3)

其中，atan2表示特征点i(x，y)到点(m10，m01)的方位角。

因此，每个orientedfast特征点在图像坐标系中都有一个坐标。

步骤3，基于灰度不变性假设计算x，y，θ，t与u，v，ω关系式；

由于每个orientedfast特征点在图像坐标系中都有一个对应的坐标，也即每个特征点具有4维信息(x，y，θ，t)，其中，t为时间，(x，y)为位置，θ为角度。

假设第一帧图像i有一个图像点k＝[uxuyuθ]^t。当i(k)和j(l)相同时，我们的目标是找到第二帧图像j上的位置l＝k+d＝[ux+dxuy+dyuθ+dθ]^t，向量是d＝[dxdydθ]^t图像偏移量。对其进行一阶泰勒级数展开的公式：

基于灰度不变性的假设：

i(x+dx，y+dy，θ+dθ，t+dt)＝i(x，y，θ，t)(5)

i(x，y，θ，t)表示在时间为t，位置为(x，y)，角度为θ的像素位置。

因此，我们可以得到：

其中，ix,iy，iθ表示图像点k处i(x，y，θ，t)对x，y，θ的偏导；u,v,ω表示x,y,θ方向的速度。

步骤4，如果只有一个点时，我们只能得到运动的弧度。我们在特征点周围选择一个w×w块，这样就有w²对特征点。然而只有三个变量的一阶方程，不足以计算像素移动量(u,v,ω)，因此，必须引入额外的约束来计算(u,v,ω)。在角度光流中，我们假设窗口中的所有像素具有相同的运动。考虑一个大小为w×w的窗口，其中包含w²个像素，则窗口中的所有像素具有相同的运动，因此我们总共可以得到w²个方程式：

所以，等式就变成：

其中：

使用最小二乘法来求解这个超定线性方程组：

最终，我们通过从单目相机获取的机器人运动信息得到了机器人此刻x,y方向的速度u,v以及航向角速度ω，得到此时机器人的位置状态信息。

以上实施例仅为说明本发明的技术思想，不能以此限定本发明的保护范围，凡是按照本发明提出的技术思想，在技术方案基础上所做的任何改动，均落入本发明保护范围之内。