CN103761737A - 基于稠密光流的机器人运动估计方法 - Google Patents

Abstract

本发明涉及一种基于稠密光流的机器人运动估计方法，包括：对相机的内参和外参进行标定；采集图像序列并畸变校正；通过多项式展开的方法求解稠密光流；光流提纯；图像坐标与机器人坐标之间的坐标变换；估计机器人的运动速度。本发明针对现有技术中存在的运动估计精度不高，在光照不均匀、运动模糊情况下鲁棒性较差以及相机的作用受到局限等问题，提供一种基于稠密光流的机器人运动速度估计方法，引入基于多项式分解的稠密光流算法，提高了运动估计的精度；将RANSAC算法应用于光流提纯过程，提高了算法的鲁棒性；采用单目相机，且镜头斜向下放置，使机器人可以实时地检测前方的情况，为完成避障、路径规划等其他任务提供了便利。

Description

基于稠密光流的机器人运动估计方法

技术领域

本发明属于机器人视觉领域，涉及利用视觉信息实现对机器人进行运动速度估计的方法。

背景技术

运动状态的测量在移动机器人的自定位、路径规划、自主导航以及其它应用领域至关重要。近年来，基于视觉的方法成为移动机器人运动估计的研究热点。视觉里程计(visionodometry)是一种利用相机采集到的图像序列来估计机器人相对运动的方法。与传统的机器人运动估计方法相比，基于视觉运动估计方法具有结构简单、精度较高等优点。而且，对于全向运动机器人，传统基于里程计的航位推算难以实现，基于视觉的方法的优势更为明显。

申请号为201110449925.0的专利公开了一种移动机器人双目视觉里程计实现方法。该方法使用双目摄像机采集室内天花板图像对，通过特征提取、匹配获得点的三维对应关系，最后采用分段优化算法获得机器人的运动参数。但是此方法中相机镜头为垂直向上放置，检测不到机器人前方的情况，无法完成避障、路径规划等其他任务。而且采用双目视觉提高了机器人运动估计的系统复杂度及成本。

申请号为201310201122.2的专利公开了一种融合光流与特征点匹配的单目视觉里程计的设计方法。该方法将SURF特征匹配与光流相融合，然后对匹配结果进行卡尔曼滤波，获得车体运动估计的结果。但是该方法采用稀疏光流跟踪，定位精度略差，而且没有对得到的光流结果进行提纯，在光照不均匀或运动模糊时不具有较好的鲁棒性。

发明内容

针对现有技术中存在的运动估计精度不高，在光照不均匀、运动模糊情况下鲁棒性较差以及相机的作用受到局限等问题，本发明提供一种基于稠密光流的机器人运动速度估计方法，引入一种基于多项式分解的稠密光流算法，提高了运动估计的精度；将RANSAC算法应用于光流提纯过程，提高了算法的鲁棒性；将相机镜头斜向下放置，使机器人可以实时地检测前方的情况，为完成避障、路径规划等其他任务提供了便利。

基于稠密光流的机器人运动估计方法步骤如下：

步骤1，对相机的内参和外参进行标定。

假定世界坐标系平行于机器人坐标系，并且调整相机的位置使得相机的偏航角和侧倾角为零。在图像测量和机器视觉应用中，相机参数的标定是非常关键的环节，本发明采用张正友的平面标定法对相机进行标定。

步骤2，采集连续图像序列作为输入，并对获得的图像序列进行畸变校正。

首先使机器人平台沿着一定的轨迹移动，通过数字相机采集连续的图像序列，然后对输入的图像序列进行径向畸变校正和切向畸变校正，其中用到的相机参数可以通过步骤1获得。

步骤3，计算相邻两帧图像之间的稠密光流。

首先对灰度图像进行多项式展开，然后对每一个像素点邻域附加权重，最后通过最小二乘法求得稠密的光流结果。

步骤4，对获得的光流估计结果进行提纯。

本发明采用随机抽样一致性（RANSAC）算法来剔除光照不均匀以及运动模糊产生的错误光流矢量。首先通过3D运动场与2D速度场之间的关系建立平面片光流二次模型，然后将光流模型带入到RANSAC算法中即可实现光流的提纯。

步骤5，进行图像坐标与机器人坐标之间的坐标变换。

首先通过求解单应性矩阵将点的图像坐标转化到世界坐标系下，然后通过标定的外参矩阵将点的世界坐标转化到机器人坐标系下。

步骤6，通过非线性迭代的方法计算机器人帧间角速度和线速度的测量值，并对测量结果进行卡尔曼滤波，最终获得机器人运动速度的估计值。

与传统的视觉里程计相比，本发明具有以下优势：

（1）采用单目相机，且镜头为斜向下放置，可以实时地检测到机器人前方的情况，为完成避障、路径规划等其他任务提供了便利。

（2）机器人运动估计模型中世界坐标系平行于机器人坐标系，并且调整相机的偏航角和侧倾角近似为零，简化了坐标系间的转化关系，提高了运动估计算法的精度。

（3）本发明采用了稠密光流算法，与传统的稀疏匹配算法相比，提高了运动估计的精度。

（4）引入RANSAC算法来剔除错误的光流矢量，提高了运动估计算法的鲁棒性。

附图说明

图1为基于稠密光流的机器人运动速度测量方法流程图；

图2为本发明用到的三种坐标系示意图：O_c-X_cY_cZ_c为相机坐标系，O_r-X_rY_rZ_r为机器人坐标系，O_w-X_wY_wZ_w为世界坐标系；

图3为本发明实施例得到的稠密光流：（a）为相机采集到的真实场景图像，（b）为基于多项式分解获得的稠密光流。

图4为本发明实施例光流提纯结果：（a）为真实场景图，（b）为RANSAC排异之前的光流矢量图，（c）为RANSAC排异之后的光流矢量图。

图5为实验环境：（a）为实验的真实场景，（b）为实验环境的二维几何示意图。

图6为机器人速度估计结果：（a）为X轴方向线速度估计结果，（b）为Y轴方向线速度估计结果，（c）为角速度估计结果，（d）为估计速度的模值与真值的误差。

具体实施方式

下面结合附图和具体实施方式对本发明作进一步说明。

基于稠密光流的机器人运动速度测量方法流程图如图1所示，具体包括以下几个步骤。

步骤1，对相机的内参和外参进行标定。

（1）打印一张尺寸为A2的靶标模板，靶标上特征点的间隔为30mm。

（2）从多个角度拍摄靶标。拍摄时，应尽量让靶标占满屏幕，并保证靶标的每一个角都在屏幕中，一共拍摄9张靶标图像。

（3）检测出图像中的特征点，即靶标的每一个黑色交叉点。

（4）建立相机坐标系、机器人坐标系和世界坐标系，如附图2所示，相机镜头为斜向下放置，其中：O_c-X_cY_cZ_c表示相机坐标系，原点固定于相机的光心，Z_c轴沿相机的光轴方向，X_c和Y_c轴与成像面的x和y轴平行；O_r-X_rY_rZ_r表示机器人坐标系，原点位于机器人的中心，Z_r轴垂直于地面向上，Y_r轴沿机器人前进的方向，X_r垂直于Y_rO_rZ_r所在平面；O_w-X_wY_wZ_w表示世界坐标系，原点建立在机器人视野范围内的一个靶标的左上角上，Z_w轴垂直于地面向上，X_w和Y_w轴分别与靶标上的两直角边平行。

（5）求出相机标定的内参矩阵A：

$A=\left[\begin{array}{ccc}{f}_x& \mathrm{\γ}& C_x\\ 0& f_y& C_y\\ 0& 0& 1\end{array}\right]$

其中，（f_x,f_y）是相机的归一化焦距，(C_x,C_y)是相机像平面中心坐标，γ为像平面横轴与纵轴之间的倾斜因子。

（6）为了简化运动估计的数学模型，首先将平面靶标放置在机器人的正前方，沿机器人坐标轴Y_R引出一条辅助直线，调整平面靶标的位置使得它的坐标轴Y_W垂直于辅助直线，而X_W平行于辅助直线。然后调整相机的位置使得相机的偏航角和侧倾角为零。

（7）拍摄第10幅靶标图像。

（8）通过（7）中拍摄的图像计算相机与靶标之间的相对位置关系。

外参矩阵表达式如下：

$\left[R_w^c\right|t_w^c]$

其中，

和

分别为世界坐标系到相机坐标系的旋转矩阵和平移向量。

步骤2，采集图像序列并畸变校正，具体方法如下：

（1）使机器人平台沿着一定的轨迹移动，移动的速度为30-60mm/s，通过Point Grey Flea2相机采集连续的图像序列，相机的采样频率为1-10Hz。

（2）对获得的原始图像序列进行畸变校正。

通过双线性插值来计算输出图像的像素值，公式如下：

x′＝x(1+k₁r²+k₂r⁴)+p₁(r²+2x²)+2p₁xy

y′＝y(1+k₁r²+k₂r⁴)+p₁(r²+2y²)+2p₂xy

其中，(x,y)和(x′,y′)分别是图像校正前和校正后的像素点的坐标，k₁和k₂是径向形变系数，p₁和p₂是切向形变系数，且有：r²=x²+y²。

步骤3，通过多项式展开的方法求解稠密光流，具体方法如下：

本发明引入全局光流算法，可以得到致密的光流场从而提高机器人速度估计的精度。对相邻两帧灰度图像在空间上逐点进行多项式展开，即：

I₁(x)＝x^TA₁x+b₁ ^Tx+c₁

I₂(x)＝x^TA₂x+b₂ ^Tx+c₂

其中，A_i是一个对称阵，b_i是一个向量，c_i是一个标量。

第二幅图像序列的灰度由光流矢量表示如下：

由灰度守恒约束可以得到：b₂＝b₁-2A₁u，当A1可逆时可求得光流值

为了提升光流算法的鲁棒性，对每一个像素点的邻域附加权重。

建立求解光流的目标函数：

$E\left(u\left(x\right)\right)=\underset{\mathrm{\Δx}\∈I}{\mathrm{\Σ}}\mathrm{\ω}\left(\mathrm{\Δx}\right)\left|\right|A(x+\mathrm{\Δx})u\left(x\right)-\mathrm{\Δb}(x+\mathrm{\Δx})\left|\right|$

其中，ω(Δx)表示像素点在邻域内的权重系数，表示邻域上各个像素点对约束产生的影响的大小分配。

由最小二乘法求解光流：

u＝(∑w A^T A)^-1∑w A^TΔb

稠密光流计算结果见图3，其中（a）为相机采集到的真实场景图像，（b）为基于多项式展开获得的稠密光流。由图3（b）可以得到：本发明引入的光流算法能够获得平滑、稠密的光流，具有较高的精度。

步骤4，光流提纯，具体方法如下：

（1）建立平面片光流模型

设刚体上有一点P=(X,Y,Z)，它相对于原点的平移速度t=(t_x,t_y,t_z)^T，同时伴有一旋转速度R=(R_x,R_y,R_z)^T，则可得P处的瞬时速度矢量V=(V_x,V_y,V_z)^T：

V＝R×P+t

设透视投影模型中有一运动的平面片，在某一时刻该平面片在3D坐标系下的方程为：

k_XX+k_YY+k_ZZ＝1

由于焦距是常数，可以令f=1，根据透镜成像的基本原理和简单的几何知识可以推导出点P的3D坐标(X,Y,Z)和在成像平面上与这对应的像点p的图像坐标(x,y)满足如下关系：

$x=\frac{X}{Z},y=\frac{Y}{Z}$

在上述方程两边分别对时间t求导可得平面片光流方程：

$\left\{\begin{array}{c}u={\mathrm{\ψ}}_x(x,y)=a_1+a_{2}x+a_{3}y+a_7{x}^2+a_8\mathrm{xy}\\ v={\mathrm{\ψ}}_y(x,y)=a_4+a_{5}x+a_{6}y+a_7\mathrm{xy}+a_8{y}^2\end{array}\right.$

其中：

（2）基于RANSAC算法的光流提纯，具体过程如下：

1)随机选取一组光流矢量作为样本，通过最小二乘法来估计平面片光流模型系数ψ⁰。

2)将剩余的所有光流矢量带入到ψ⁰中，计算它们与ψ⁰之间的误差，如果误差较小，则这个矢量被划分为内点光流；否则，划分为异常光流。记录这一组样本获得的内点光流数量，如果内点数大于设定的内点数阈值，则对这一组样本进行精提纯。

3)用获得的所有内点光流矢量重新估计平面片光流模型系数ψ^k，然后将所有的光流矢量带入到更新的光流模型ψ^k中并计算误差率。

4)重复步骤1）～3)50次，找到误差率最低的光流模型ψ^*，也就是最优的光流模型。

5)用最优的光流模型ψ^*划分内点光流和异常光流，并将异常光流排除。光流提纯的结果见图4。其中（a）为真实场景图，（b）为RANSAC排异之前的光流矢量图，（c）为RANSAC排异之后的光流矢量图。由图4可得：RANSAC算法可以排除光照不均匀以及运动模糊产生的错误光流，提升了运动估计的鲁棒性。

步骤5，图像坐标与机器人坐标之间的坐标变换。具体方法如下：

（1）求解单应矩阵H，将点的图像坐标转化到世界坐标系下。

假设图像坐标系中一点的齐次坐标为m=(u,v,1)^T，在世界坐标系中与之相对应的一点的齐次坐标为M=(X,Y,Z,1)^T。可得如下的投影方程：

sm＝A[R,T]M

其中s是任意比例因子，[R,T]为外参矩阵，A为内参矩阵。模型中靶标上点的Z坐标为0，可得单应矩阵的定义：

H＝A[r₁,r₂,T]

其中，r₁和r₂分别表示旋转矩阵R的第一列和第二列。然后将靶标上特征点的图像坐标以及设定的世界坐标作为输入，通过最优化反投影误差得到两坐标系之间的透视变换H，最终获得点的世界坐标M’。最优化反投影误差为：

${\mathrm{\Σ}}_i[{(X_i-\frac{h_{11}{u}_i+h_{12}{v}_i+h_{13}}{h_{31}{u}_i+h_{32}{v}_i+h_{33}})}^2+{(Y_i-\frac{h_{21}{u}_i+h_{22}{v}_i+h_{23}}{h_{31}{u}_i+h_{32}{v}_i+h_{33}})}^2]$

（2）通过标定的外参矩阵将点的世界坐标转化到机器人坐标系。

首先计算相机光心到世界坐标系原点的距离

世界坐标系的原点到机器人中心之间的距离

由于运动估计的数学模型在步骤一中已简化，因此将点从世界坐标系转化到机器人坐标系的旋转矩阵和平移向量可由下式表示：

$R_w^r=I,t_w^r=D_w^r$

其中，I为单位矩阵。

步骤6，估计机器人的运动速度，具体方法如下：

(1)通过Levenberg-Marquardt算法恢复运动参数(旋转矩阵R和平移向量t)

机器人的运动可以看成是刚体运动，满足X_k=R*X_k+1+t，即通过旋转矩阵R和平移向量t把机器人坐标系下t+1时刻的点坐标变换到t时刻，R和t分别为：

$R=\left(\begin{array}{cc}\cos \mathrm{\θ}& -\sin \\ \sin \mathrm{\θ}& \cos \mathrm{\θ}\end{array}\right),t={(t_x,t_y)}^T$

然后，通过最小化如下的目标函数来求解R,t：

$\begin{array}{c}E(R,t)=\underset{i=1}{\overset{n}{\mathrm{\Σ}}}{\left[{\mathrm{\ω}}_i(X_{k_i}-{\mathrm{RX}}_{{(k+1)}_i}-t)\right]}^2\\ ={(X_{k_i}-{\mathrm{RX}}_{{(k+1)}_i}-t)}^{2}W(X_{k_i}-{\mathrm{RX}}_{{(k+1)}_i}-t)\end{array}$

通过LM算法求取机器人相对位姿，公式如下：

[J^TWJ+λI]h_lm＝J^TW(X_k-RX_(k+1)-t)

其中，J为相机的雅各比矩阵。

然后通过如下公式求解机器人速度的测量值：

$v_k\≈\frac{1}{T_s}{\left[\begin{array}{ccc}{t}_x& t_y& \mathrm{\θ}\end{array}\right]}^T$

(2)进行卡尔曼滤波

设定KF模型的状态变量S=[a,v]^T，其中a=[a_x,a_y,a_θ]^T，v=[v_x,v_y,v_θ]^T分别表示机器人帧间的速度与加速度。状态变量的一步预测为：

${\hat{S}}_{(k+1/k)}={\mathrm{\Φ}}_{k+1,k}{\hat{S}}_k$

然后计算状态向量的后验估计：

${\hat{S}}_{(k+1)}={\hat{S}}_{(k+1/k)}+K_k[S_k-H{\hat{S}}_{(k+1|k)}]$

其中，H为系统的观测矩阵；S_k为状态变量的测量值，由前面计算得到；K_k为卡尔曼增益，由下式获得：

K_k＝P_k/k-1H^T(HP_k/k-1H^T+R_k)^-1

其中，P_k为预测误差的协方差，由下式计算获得：

P_k＝(I-K_kH_k)P_k/k-1(I-K_kH_k)^T+K_kR_kK_k ^T

最终，通过KF模型的后验状态变量得到了机器人运动速度估计值。

下面给出应用本发明所述方法在室内真实环境下进行速度估计一个实验实例。

实验采用Point Grey Flea2数字相机，图像分辨率为640×480。室内环境如图5所示，图5（a）为实验的真实场景，图5（b）为实验环境的二维几何示意图。实验时机器人平台沿图示的路径移动，分别通过轮式里程计和基于光流的运动估计方法对机器人的速度进行估计，速度的真值由基于激光测距仪的SLAM提供。速度估计结果见图6所示。（a）（b）（c）中的实线表示基于光流的速度估计结果，点线表示轮式里程计估计结果，虚线表示速度的真值；（d）中实线为基于光流的速度估计结果与真值的误差，虚线为轮式里程计估计结果与真值的误差。由图可知，基于光流的运动速度估计的精度优于轮式里程计。

实验结果表明，本发明所述方法可对机器人的速度进行估计，具有较好的精度和鲁棒性。

Claims (2)

1.一种基于稠密光流的机器人运动速度估计方法，其特征在于包括以下步骤：

步骤1，对相机的内参和外参进行标定；

（1）打印一张靶标模板；

（2）从多个角度拍摄靶标；拍摄时，应尽量让靶标占满屏幕，并保证靶标的每一个角都在屏幕中，一共拍摄9张靶标图像；

（3）检测出图像中的特征点，即靶标的每一个黑色交叉点；

（4）建立相机坐标系、机器人坐标系和世界坐标系：相机镜头为斜向下放置，其中O_c-X_cY_cZ_c表示相机坐标系，原点固定于相机的光心，Z_c轴沿相机的光轴方向，X_c和Y_c轴与成像面的x和y轴平行；O_r-X_rY_rZ_r表示机器人坐标系，原点位于机器人的中心，Z_r轴垂直于地面向上，Y_r轴沿机器人前进的方向，X_r垂直于Y_rO_rZ_r所在的平面；O_w-X_wY_wZ_w表示世界坐标系，原点建立在机器人视野范围内的一个靶标的左上角上，Z_w轴垂直于地面向上，X_w和Y_w轴分别与靶标上的两条直角边平行；

（5）求出相机标定的内参矩阵A：

$A=\left[\begin{array}{ccc}{f}_x& \mathrm{\γ}& C_x\\ 0& f_y& C_y\\ 0& 0& 1\end{array}\right]$

其中，（f_x,f_y）是相机的归一化焦距，(C_x,C_y)是相机像平面中心坐标，γ为像平面横轴与纵轴之间的倾斜因子；

（6）首先将平面靶标放置在机器人的正前方，沿机器人坐标轴Y_R引出一条辅助直线，调整平面靶标的位置使得它的坐标轴Y_W垂直于辅助直线，而X_W平行于辅助直线；然后调整相机的位置使得相机的偏航角和侧倾角为零；

（7）拍摄第10幅靶标图像；

（8）通过（7）中拍摄的图像计算相机与靶标之间的相对位置关系；

外参矩阵表达式如下：

$\left[R_w^c\right|t_w^c]$

其中，

和

分别为世界坐标系到相机坐标系的旋转矩阵和平移向量；

步骤2，采集图像序列并畸变校正，具体方法如下：

（1）使机器人平台沿着一定的轨迹移动，移动的速度为30-60mm/s，通过相机采集连续的图像序列，相机的采样频率为1-10Hz；

（2）对获得的原始图像序列进行畸变校正；

通过双线性插值来计算输出图像的像素值，公式如下：

x′＝x(1+k₁r²+k₂r⁴)+p₁(r²+2x²)+2p₁xy

y′＝y(1+k₁r²+k₂r⁴)+p₁(r²+2y²)+2p₂xy

其中，(x,y)和(x’,y’)分别是图像校正前和校正后的像素点的坐标，k₁和k₂是径向形变系数，p₁和p₂是切向形变系数，且有：r²=x²+y²；

步骤3，通过多项式展开的方法求解稠密光流，具体方法如下：

引入全局光流算法，可以得到致密的光流场从而提高机器人速度估计的精度；对相邻两帧灰度图像在空间上逐点进行多项式展开，即：

I₁(x)＝x^TA₁x+b₁ ^Tx+c₁

I₂(x)＝x^TA₂x+b₂ ^Tx+c₂

其中，A_i是一个对称阵，b_i是一个向量，c_i是一个标量；

第二幅图像序列的灰度由光流矢量表示如下：

由灰度守恒约束可以得到：b₂＝b₁-2A₁u，当A₁可逆时可求得光流值

为了提升光流算法的鲁棒性，对每一个像素点的邻域附加权重；

建立求解光流的目标函数：

$E\left(u\left(x\right)\right)=\underset{\mathrm{\Δx}\∈I}{\mathrm{\Σ}}\mathrm{\ω}\left(\mathrm{\Δx}\right)\left|\right|A(x+\mathrm{\Δx})u\left(x\right)-\mathrm{\Δb}(x+\mathrm{\Δx})\left|\right|$

其中，ω(Δx)表示像素点在邻域内的权重系数，表示邻域上各个像素点对约束产生的影响的大小分配；

由最小二乘法求解光流：

u＝(∑w A^T A)^-1∑w A^T Δb

步骤4，光流提纯，具体方法如下：

（1）建立平面片光流模型

设刚体上有一点P=(X,Y,Z)，它相对于原点的平移速度t=(t_x,t_y,t_z)^T，同时伴有一旋转速度R=(R_x,R_y,R_z)^T，则可得P处的瞬时速度矢量V=(V_x,V_y,V_z)^T：

V＝R×P+t

设透视投影模型中有一运动的平面片，在某一时刻该平面片在3D坐标系下的方程为：

k_XX+k_YY+k_ZZ＝1

由于焦距是常数，令f=1，点P的3D坐标(X,Y,Z)和在成像面上与之对应的像点p的图像坐标(x, y)满足如下关系：

$x=\frac{X}{Z},y=\frac{Y}{Z}$

在上述方程两边分别对时间t求导可得平面片光流方程：

$\left\{\begin{array}{c}u={\mathrm{\ψ}}_x(x,y)=a_1+a_{2}x+a_{3}y+a_7{x}^2+a_8\mathrm{xy}\\ v={\mathrm{\ψ}}_y(x,y)=a_4+a_{5}x+a_{6}y+a_7\mathrm{xy}+a_8{y}^2\end{array}\right.$

其中：

（2）基于RANSAC算法的光流提纯；

步骤5，图像坐标与机器人坐标之间的坐标变换；具体方法如下：

（1）求解单应矩阵H，将点的图像坐标转化到世界坐标系下；

假设图像坐标系中一点的齐次坐标为m=(u,v,1)^T，在世界坐标系中与之相对应的一点的齐次坐标为M=(X,Y,Z,1)^T；得到如下的投影方程：

sm＝A[R,T]M

其中s是任意比例因子，[R,T]为外参矩阵，A为内参矩阵；模型中靶标上点的Z坐标为0，可得单应矩阵的定义：

H＝A[r₁,r₂,T]

其中，r1和r2分别表示旋转矩阵R的第一列和第二列；然后将靶标上特征点的图像坐标以及设定的世界坐标作为输入，通过最优化反投影误差得到两坐标系之间的透视变换H，最终获得点的世界坐标M’；最优化反投影误差为：

${\mathrm{\Σ}}_i[{(X_i-\frac{h_{11}{u}_i+h_{12}{v}_i+h_{13}}{h_{31}{u}_i+h_{32}{v}_i+h_{33}})}^2+{(Y_i-\frac{h_{21}{u}_i+h_{22}{v}_i+h_{23}}{h_{31}{u}_i+h_{32}{v}_i+h_{33}})}^2]$

（2）通过标定的外参矩阵将点的世界坐标转化到机器人坐标系；

首先计算相机光心到世界坐标系原点的距离世界坐标系的原点到机器人中心之间的距离

由于运动估计的数学模型在步骤一中已简化，因此将点从世界坐标系转化到机器人坐标系的旋转矩阵和平移向量由下式表示：

$R_w^r=I,t_w^r=D_w^r$

其中，I为单位矩阵；

步骤6，估计机器人的运动速度，具体方法如下：

(1)通过Levenberg-Marquardt算法恢复运动参数：旋转矩阵R和平移向量t；

机器人的运动可以看成是刚体运动，满足X_k=R*X_k+1+t，即通过旋转矩阵R和平移向量t把机器人坐标系下t+1时刻的点坐标变换到t时刻，R和t分别为：

$R=\left(\begin{array}{cc}\cos \mathrm{\θ}& -\sin \\ \sin \mathrm{\θ}& \cos \mathrm{\θ}\end{array}\right),t={(t_x,t_y)}^T$

然后，通过最小化如下的目标函数来求解R,t：

$\begin{array}{c}E(R,t)=\underset{i=1}{\overset{n}{\mathrm{\Σ}}}{\left[{\mathrm{\ω}}_i(X_{k_i}-{\mathrm{RX}}_{{(k+1)}_i}-t)\right]}^2\\ ={(X_{k_i}-{\mathrm{RX}}_{{(k+1)}_i}-t)}^{2}W(X_{k_i}-{\mathrm{RX}}_{{(k+1)}_i}-t)\end{array}$

通过LM算法求取机器人相对位姿，公式如下：

[J^TWJ+λI]h_lm＝J^TW(X_k-RX_(k+1)-t)

其中，J为相机的雅各比矩阵；

然后通过如下公式求解机器人速度的测量值：

$v_k\≈\frac{1}{T_s}{\left[\begin{array}{ccc}{t}_x& t_y& \mathrm{\θ}\end{array}\right]}^T$

(2)进行卡尔曼滤波；

设定KF模型的状态变量S=[a,v]^T，其中a=[a_x,a_y,a_θ]^T，v=[v_x,v_y,v_θ]^T分别表示机器人帧间的速度与加速度；状态变量的一步预测为：

${\hat{S}}_{(k+1/k)}={\mathrm{\Φ}}_{k+1,k}{\hat{S}}_k$

然后计算状态向量的后验估计：

${\hat{S}}_{(k+1)}={\hat{S}}_{(k+1/k)}+K_k[S_k-H{\hat{S}}_{(k+1|k)}]$

其中，H为系统的观测矩阵；S_k为状态变量的测量值，由前面计算得到；K_k为卡尔曼增益，由下式获得：

K_k＝P_k/k-1H^T(HP_k/k-1H^T+R_k)^-1

其中，P_k为预测误差的协方差，由下式计算获得：

$P_k=(I-K_k{H}_k)P_{k/k-1}{(I-K_k{H}_k)}^T+K_k{R}_k{K}_k^T$

最终，通过KF模型的后验状态变量得到了机器人运动速度估计值。

2.根据权利要求1所述的一种基于稠密光流的单目视觉运动估计方法，其特征在于，所述步骤4基于RANSAC算法的光流提纯还包括以下步骤：

（1）随机选取一组光流矢量作为样本，通过最小二乘法来估计平面片光流模型系数ψ⁰；

（2）将剩余的所有光流矢量带入到ψ⁰中，计算它们与ψ⁰之间的误差，如果误差较小，则这个矢量被划分为内点光流；否则，划分为异常光流；记录这一组样本获得的内点光流数量，如果内点数大于设定的内点数阈值，则对这一组样本进行精提纯；

（3）用获得的所有内点光流矢量重新估计平面片光流模型系数ψ^k，然后将所有的光流矢量带入到更新的光流模型ψ^k中并计算误差率；

（4）重复步骤（1）～（3)50次，找到误差率最低的光流模型ψ^*，也就是最优的光流模型；

（5）用最优的光流模型ψ^*划分内点光流和异常光流，并将异常光流排除。

Images