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

权利要求书

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₅]，其包括径向畸变系数和切向畸变系数，所述车载摄像头模型的归一化映射关系如下：

xnyn=XRZRYRZR---(1)]]>

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

xdyd=(1+k1r2+k2r4+k5r6)xnyntd---(2)]]>

其中r=xn2+yn2,]]>切向畸变向量td=2k3xnyn+k4(r2+2xn2)k3(r2+2yn2)+2k4xnyn,]]>点(c₁,c₂)为车载摄像头光轴与图像平面的交点，则像素点的最终坐标为：

xcyc=f100f2xdyd+c1c2---(3)]]>

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

xcyc=f1XRZRf2YRZR+c1c2---(4)]]>

（4）式对时间微分得：

VxVy=ZRvxf1+XRVzZRZRvyf2+YRVzZR---(5)]]>

考虑地面水平则Z_R为常值，对时间导数为0则有：

VxVy=ZRvxf1ZRvyf2---(6)]]>

由（4）式带入得：

VxVy=ZRvxf1+1f1(xc-c1)VzZRvyf2+1f2(yc-c2)Vz---(7)]]>

考虑地面水平则Z_R为常值，对时间导数为0则有：

VxVy=ZRvxf1ZRvyf2---(8)]]>

（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组合导航方法，其中所述异常角点为角点纵向位移超过预定阈值的角点。