[发明专利]一种基于视觉里程计的移动机器人定位方法

权利要求书

1.一种基于视觉里程计的移动机器人定位方法，其特征在于：包括以下步骤：

S1、采集移动机器人周围环境图像信息；

S2、通过光流法计算图像的先验运动估计，得到基于光流法的移动机器人的运动轨迹；

S3、将至少由前100帧图像得到的基于光流法的移动机器人运动轨迹进行三维地图化，得到局部地图；

S4、通过局部地图获取实时图像中的下一个关键帧，并获取该关键帧中的特征点；

S5、通过特征点获取对应图像的运动轨迹，即基于特征点的移动机器人的运动轨迹；

S6、将基于光流法的移动机器人的运动轨迹和基于特征点的移动机器人的运动轨迹进行卡尔曼融合，得到该关键帧对应的移动机器人的位置；

S7、判断是否继续定位，若是则进入步骤S8，否则结束定位；

S8、根据该关键帧更新局部地图，采用与步骤S2相同的方法持续获取基于光流法的移动机器人的运动轨迹并返回步骤S4；

所述步骤S2的具体方法包括以下步骤：

S2-1、采用光流法获取每两帧图像之间的旋转矩阵R_i和平移向量t_i；

S2-2、将从每两帧图像之间得到的旋转矩阵R_i和平移向量t_i进行累计得到先验运动估计，即位姿偏移量u_k-1＝[Δx，Δy，Δθ]^T，其中Δx为x方向的偏移量，Δy为y方向的偏移量，Δθ为行进方向的偏移量；

S2-3、根据离散卡尔曼滤波器的预测方程：

得到基于光流法的移动机器人的运动轨迹其中A是状态转移矩阵，为常数；B是输入增益，为常数；u_k-1是位置输入函数，即位姿偏移量；w_k和Q分别是运动过程噪声和噪声协方差矩阵，为常数；P_k-1是上一时刻的协方差矩阵；(·)T为转置矩阵；为当前时刻的协方差矩阵估计；为当前的预测位置，即基于光流法得到的移动机器人位置；为上一时刻的预测位置；

所述步骤S4的具体方法包括以下步骤：

S4-1、将局部地图中每一帧的特征点作为局部地图的特征点；

S4-2、以至少20帧为间隔帧数，将局部地图后第一个特征点数量少于局部地图的特征点数量60％的帧作为局部地图的下一个关键帧；

S4-3、判断该关键帧中是否有30％以上的特征点未匹配至局部地图中，若是则忽略该关键帧并进入步骤S4-2，否则进入步骤S5。

2.根据权利要求1所述的基于视觉里程计的移动机器人定位方法，其特征在于：所述步骤S5的具体方法为：

根据公式

z_k＝Hx_k+v_k

通过特征点匹配得到基于特征点的对应图像的运动轨迹z_k，即基于特征点的移动机器人的运动轨迹；其中H为单位矩阵；v_k为观测噪声；x_k为移动机器人的位姿数据。

3.根据权利要求2所述的基于视觉里程计的移动机器人定位方法，其特征在于：所述步骤S6的具体方法包括以下步骤：

根据公式

将基于光流法的移动机器人的运动轨迹和基于特征点的移动机器人的运动轨迹进行卡尔曼融合，得到该关键帧对应的移动机器人的运动估计即该关键帧对应的移动机器人位置；其中I为单位矩阵。

4.根据权利要求3所述的基于视觉里程计的移动机器人定位方法，其特征在于：所述步骤S8中根据该关键帧更新局部地图的具体方法包括以下步骤：

S8-1、获取局部地图中每个特征点出现的次数；

S8-2、将局部地图中的所有特征点投影到该局部地图的下一个关键帧上，将未在该局部地图的下一个关键帧上出现的现有特征点的出现次数减1，将在该局部地图的下一个关键帧上出现的现有特征点的出现次数加1；

S8-3、将该局部地图的下一个关键帧中未出现在当前局部地图中的特征点加入当前局部地图并将其出现次数记为1，且去掉出现次数小于0的特征点，完成该关键帧对局部地图的更新。