一种基于机器视觉的移动机器人码盘定位修正方法与流程

本发明涉及属于机器人技术领域，具体涉及移动机器人定位技术与卡尔曼滤波的信息融合技术，特别是一种基于机器视觉的移动机器人码盘定位修正方法。

背景技术：

自主定位技术是移动机器人进行导航与运动控制的基础，是提高机器人自主能力的关键技术。对移动机器人而言，依靠里程计定位是一种最普遍的自主定位方式。但是，以安装在机器人车轮上的光电编码盘为基础的传统里程计定位方法中存在着许多问题，其中包括：机器人几何参数不准确、码盘脉冲计数的误差、车轮打滑造成实际位置和计算的不符而产生的误差，以及行驶路面的崎岖不平。这些误差会累积，一段时间之后会超出允许的误差范围而导致定位失败。为此，人们提出了视觉里程计技术。该技术通过计算前后两帧图像的光流场或者部分匹配特征点的三维坐标，实现对车体运动参数的估计。

在中国专利授权号为CN 101774170 B核电站作业机器人及其控制系统属于机器人及自动化设备领域。该核电站作业机器人为履带式移动机械手，由双履带驱动的移动平台及其搭载的四自由度的机械手组成，可以在核电站内部移动，并有人工遥控和自主控制两种控制模式，采用无线或有线方式对其进行远程控制。所述的核电站作业机器人的控制系统分为上位监测与规划控制系统和机器人控制系统两部分，二者配合使用控制机器人的运作；但是其依然没有解决在缺少目标点的情况下，通过自主校验来进行调整，以此来减小误差的产生。

中国专利授权号CN102538781公开了一种基于机器视觉和惯导融合的移动机器人运动姿态估计方法，其步骤为：同步采集移动机器人双目相机图像及三轴惯导数据；提取前后帧图像特征并匹配估计运动姿态；利用惯导计算俯仰角和横滚角；建立卡尔曼滤波器模型融合视觉和惯导姿态估计；根据估计方差自适应调整滤波器参数；姿态修正的累积航位推算。本发明提出实时扩展卡尔曼滤波器姿态估计模型，利用惯导结合重力加速度方向作为补充，对视觉里程计三个方向姿态估计进行解耦，修正姿态估计的累积误差；根据运动状态采用模糊逻辑调整滤波器参数，实现自适应的滤波估计，降低加速度噪声的影响，有效的提高了视觉里程计的定位精度和鲁棒性。

但是，视觉里程计也有一定的适用范围。一般来说，视觉里程计都需要大量的特征点。但是在有些场景下，比如月球表面，由于存在着大面积的弱纹理区域，使得特征点的提取本身就是一个难题。另外，特征点的匹配精度也是一个问题。大量的特征点可能会导致匹配结果中包含着大量的噪声点。这会在很大程度上降低方法的可靠性。

本发明所设计的方法同样是引入视觉信息，但是既不需要大量的特征点，也不需要去计算特征点的三维信息，而只需要计算车体到任意几个特征点之间的视线夹角，我们称之为视觉量角计；通过不同时刻机器人在不同位置看到的相同两个特征点之间的夹角变化，利用自适应无迹卡尔曼滤波技术使之与光电编码盘的定位信息相融合，实现了对光电编码盘定位误差的修正。

技术实现要素：

有鉴于此，本发明的目的是针对现有技术的不足，提供一种基于机器视觉的移动机器人码盘定位修正方法，通过设置光电编码盘与视觉测量角度相结合的方式，进行修正，从而能够在少量目标点的情况使得误差最大可能的减少。

为达到上述目的，本发明采用以下技术方案：

基于机器视觉的移动机器人码盘定位修正方法，其特征在于，包括以下步骤：

S1：通过光电编码盘建立机器人瞬时离散运动模型，对机器人实时坐标进行定位；

S2：通过安装在机器人本体上的摄像机采集两个目标点图像，建立机器人视觉测量角度模型；

S3：建立推广卡尔曼滤波器模型计算机器人瞬时位移变化量，并采用光电编码盘的统计特性得到位置变化的均值和方差；

S4：用差分的办法来代替偏导的值，计算机器人当前位置时的目标点的观测量；

S5：建立无迹卡尔曼滤波器模型对S1-S4获得的数据进行融合，首先通过光电编码盘得到大致的位置信息，在这个位置上通过差分估计出目标点观测量，即，这个位置坐标与目标点的夹角，并通过实际观测的夹角来修正光电编码盘的确定位置。

进一步的，所述S1中建立机器人瞬时离散运动模型方法如下：

1)根据安装在机器人左轮(L)和右轮(R)上的光电编码盘的脉冲频率，计算左右车轮的线速度v^L、v^R为：

2)进而得出机器人的角速度为：

3)机器人的离散运动学方程为：

定义机器人初始位置的两个驱动轮轴线中点的坐标系为世界坐标系，机器人在k时刻的坐标为(x_k,y_k)。式中：f_L和f_R分别是位于机器人左右两侧驱动轮的光电编码盘的脉冲频率，d为驱动轮的直径，L为机器人两个驱动轮的轮距，n为光电编码盘的线数即驱动轮转一圈光电编码盘输出的脉冲数，Δt为时间变量，当两侧的驱动轮转速不相同时，机器人的运动轨迹即为绕着其中一个驱动轮为中心做圆周运动。

进一步的，所述S2中建立机器人视觉测量角度模型的方法为：设立两个目标点A、B，测出目标点A、B与摄像机镜头的光心O之间的夹角AOB的角度，∠AOB余弦的计算公式为：

通过安装在机器人本体上的摄像机采集两个假设的目标点A、B的成像图像，式中，f为摄像机焦距，Sa、Sb分别为目标点A、B在成像图像平面的投影，So为光心O在成像图像平面的投影。

进一步的，所述步骤三中通过推广卡尔曼滤波器计算得到机器人的运动学方程和视觉观测方程分别如下：

式中，f(x_k,u_k)与h(x_k)分别是机器人在k时刻的非线性运动学模型与测量模型，w_k与v_k为互不相关的高斯白噪声，x_k＝[x,y,θ]^T，

这里通过式(3)得到的机器人位置的变化u_k＝[Δx_k,Δy_k]^T，并由光电编码盘的融合计算得到机器人位置变化的均值和方差。

进一步的，所述机器人视觉的观测方程z_k＝h(x_k)+v_k，在这里，z_k＝[cosα1_k,cosα2_k,…,cosαn_k]^T是由机器人位置坐标到各个特征点的向量之间的夹角余弦组成的向量，如图2中的α_AB1。

真正的观测方程为：

但是由于不知道A、B点的坐标，因而这个方程不能直接用于滤波。在扩展卡尔曼滤波中，使用这个公式的目的是在一个估计的位置得到估计的位置的观测量z_k+1|k通过这个量与实际观测量z_k+1(摄像机直接测量得到)之间的差别来修正估计的位置，使修正后得到的位置在统计意义下更接近于实际位置。同时，因为这个方程明显是非线性的，在递推过程中还要用到它对于x_k的偏导，在没有特征点坐标的情况下，此偏导无法得到实际的值，因而无法用于计算。在这里用差分的办法来代替偏导的值，然后在x_k点做泰勒级数展开，估算在x_k+1|k位置时的观测量z_k+1|k。具体公式如下：

进一步的，步骤五中无迹卡尔曼滤波器采用标准无迹卡尔曼滤波器，信息融合方法为：由公式(8)与公式(9)和两个初始点的信息就可以利用自适应无迹卡尔曼滤波算法进行滤波计算。设计引入自适应机制的无迹卡尔曼滤波器为了进一步提高估计精度。自适应无迹卡尔曼滤波算法的具体过程描述如下。

1)标准无迹卡尔曼滤波器的计算步骤

(1)初始状态定义如下(k＝0):

式中，是初始状态的期望值，P₀是初始的协方差。

增广的状态包含原点，参数，与过程噪声，被定义为

(2)当k＝1,2,…,∞

(a)计算sigma点，可得

(b)预测的步骤为

其中，Q_k是过程噪声的协方差矩阵，权值与的计算公式为

式(14)中，n是增广状态的维数；参数α可以控制sigma点的分布区域的大小，尤其当系统的非线性化程度较强时，通过选择一个理想的小数可以避免采样周期的非局部效应；β是一个非负的权值项，用于确认分布的更高阶矩信息。对于一个高斯先验模型参数β优先选择为β＝2，为了保证协方差矩阵的半正定性，协方差调整参数选择为κ≥0。其余的新参数定义如下：

(c)更新步骤为

式中，R_k是测量噪声的协方差矩阵。

进一步的，步骤五中无迹卡尔曼滤波器采用标准无迹卡尔曼滤波器，信息融合方法为：为了进一步地提高估计精度，在无迹卡尔曼滤波器估计过程中，应用一个协方差匹配技术，引入一个噪声协方差自适应校正机制。更确切地说，基于移动机器人的位姿序列，考虑过程噪声协方差Q_k与测量噪声协方差R_k的自适应估计。因此，Q_k与R_k被估计与反复更新：

式中，是移动机器人被测量的位姿，Ck被定义如下：

式中，是移动机器人的在第k步位姿估计误差，C_k是机器人第k步位姿协方差矩阵的近似值，L是与协方差匹相配的滑动窗大小。

本发明将视觉的信息通过适应无迹卡尔曼滤波与光电码盘信息相融合，首先码盘自身可以得到大致的位置信息，在这个位置上通过差分估计出观测量—就是这个位置坐标与目标点的夹角，然后通过实际观测的夹角来修正码盘的确定位置。

另外，本发明针对移动机器人在崎岖路面光电码盘定位精度差的特点，利用视觉进行角度测量的方法，并利用自适应无迹卡尔曼滤波方法将该信息与光电码盘定位结果相融合，并设计了实验与装置；该方法与传统的视觉里程计技术相比主要具有三个优点：一是无需大量特征点，所以适用于类似月球表面这样的弱纹理区域；二是视觉测量值准确可靠。其测量精度只取决于图像的分辨率；三是自适应无迹卡尔曼滤波器的估计精度远高于标准卡尔曼滤波器、扩展卡尔曼滤波器与标准无迹卡尔曼滤波器；值得注意的是，该方法不能彻底消除定位误差的累积，但是本发明所设计方法对于提高轮式移动机器人系统定位精度是十分简单有效的，在降低成本的情况下，通过视觉数据和光电码盘数据的相互协同作用下，计算出定位的误差，进行及时的修正，极大的减少了误差累积。

附图说明

图1为视觉角度测量原理；

图2为本发明应用视觉进行轨迹修正示意图；

图3为本发明工作原理图。

具体实施方式

为使本发明实施例的目的、技术方案和优点更加清楚，下面将结合本发明实施例的附图1-3，对本发明实施例的技术方案进行清楚、完整地描述。显然，所描述的实施例是本发明的一部分实施例，而不是全部的实施例。基于所描述的本发明的实施例，本领域普通技术人员所获得的所有其他实施例，都属于本发明保护的范围。

实施例一

一，光电编码盘定位

光电编码盘定位是通过机器人两侧码盘读数及其读数差来实现；机器人在k时刻用(x_k,y_k)表示，x、y为机器人两驱动轮轴线中点的坐标；以机器人初始位置的车体坐标系为世界坐标系，建立机器人的运动学方程。当两侧车轮转速不相同时，车就绕着一个中心做圆周运动，设两轮轮距为L，码盘的线数为n(轮子转一圈码盘输出的脉冲数)，车轮直径为d通过左右码盘的脉冲频率f_L和f_R可以计算出左右轮子的线速度v^L、v^R为：

机器人的角速度为

机器人的离散运动学方程为

二，机器人视觉测量角度原理

机器人视觉测量角度其实就是在移动机器人本体上安装一个摄像机，只要标志点匹配准确而且相机的象素足够高，就可以认为这个角度值有足够的精度。利用摄像机进行角度测量的基本原理如图1所示；其中O是光心，A、B是两个目标点；所谓视觉量角计，就是通过A、B两点在摄像机上的成像，测出AOB的角度。

设A、B在图像平面的投影分别为S_a、S_b，光心O在图像平面的投影为S_o，f为摄像机焦距，则角度∠AOB余弦的计算公式为：

显然，该角度余弦值的准确性只与图像分辨率有关。因此用摄像机进行角度测量，简单可靠。

三，滤波方程

推广卡尔曼滤波要求得到机器人的运动学方程和观测方程以及它们的误差分布信息；考虑机器人的非线性运动学方程和观测方程如下：

式中，f(x_k,u_k)与h(x_k)分别是机器人的非线性运动学模型与测量模型，x_k＝[x,y,θ]^T，w_k与v_k为互不相关的高斯白噪声。

这里通过式(3)得到的机器人位置的变化u_k＝[Δx_k,Δy_k]^T，并由光电编码盘的统计特性得到位置变化的均值和方差。

四，机器人视觉测量信息

机器人视觉的观测方程z_k＝h(x_k)+v_k，在这里，z_k＝[cosα1_k,cosα2_k,…,cosαn_k]^T是由机器人位置坐标到各个特征点的向量之间的夹角余弦组成的向量，如图2中的与

真正的观测方程为

但是由于不知道A、B点的坐标，因而这个方程不能直接用于滤波.在扩展卡尔曼滤波中，使用这个公式的目的是在一个估计的位置得到估计的观测量z_k+1|k通过这个量与实际方向角观测量z_k+1之间的差别来修正估计的位置，所述方向角度观测量z_k+1，即，摄像机直接测量得到，使修正后得到的位置在统计意义下更接近于实际位置；同时，因为这个方程明显是非线性的，在递推过程中还要用到它对于x_k的偏导，在没有特征点坐标的情况下，此偏导无法得到实际的值，因而无法用于计算。在这里用差分的办法来代替偏导的值，然后在x_k点做一阶泰勒级数展开，估算在x_k+1|k位置时的观测量z_k+1|k。具体公式如下：

五，运用自适应无迹卡尔曼滤波器进行信息融合

由公式(8)与公式(9)和两个初始点的信息就可以利用自适应无迹卡尔曼滤波算法进行滤波计算；设计引入自适应机制的无迹卡尔曼滤波器为了进一步提高估计精度。自适应无迹卡尔曼滤波算法的具体过程描述如下。

1)标准无迹卡尔曼滤波器的计算步骤

(1)初始状态定义如下(k＝0):

式中，是初始状态的期望值，P₀是初始的协方差。

增广的状态包含原点，参数，与过程噪声，被定义为

(2)当k＝1,2,…,∞

(a)计算sigma点，可得

(b)预测的步骤为

其中，Q_k是过程噪声的协方差矩阵，权值与的计算公式为

式(14)中，n是增广状态的维数；参数α可以控制sigma点的分布区域的大小，尤其当系统的非线性化程度较强时，通过选择一个理想的小数可以避免采样周期的非局部效应；β是一个非负的权值项，用于确认分布的更高阶矩信息。对于一个高斯先验模型参数β优先选择为β＝2，为了保证协方差矩阵的半正定性，协方差调整参数选择为κ≥0；其余的新参数定义如下：

(c)更新步骤为

式中，R_k是测量噪声的协方差矩阵。

2)自适应无迹卡尔曼滤波器

为了进一步地提高估计精度，在无迹卡尔曼滤波器估计过程中，应用一个协方差匹配技术，引入一个噪声协方差自适应校正机制。更确切地说，基于移动机器人的位姿序列，考虑过程噪声协方差Q_k与测量噪声协方差R_k的自适应估计；因此，Q_k与R_k被估计与反复更新：

式中，是移动机器人被测量的位姿，C_k被定义如下：

式中，是移动机器人的在第k步位姿估计误差，C_k是机器人第k步位姿协方差矩阵的近似值，L是与协方差匹相配的滑动窗大小。

本发明将视觉的信息通过适应无迹卡尔曼滤波与光电码盘信息相融合，首先码盘自身可以得到大致的位置信息，在这个位置上通过差分估计出观测量—就是这个位置坐标与目标点的夹角，然后通过实际观测的夹角来修正码盘的确定位置。

另外，本发明针对移动机器人在崎岖路面光电码盘定位精度差的特点，利用视觉进行角度测量的方法，并利用自适应无迹卡尔曼滤波方法将该信息与光电码盘定位结果相融合，并设计了实验与装置；该方法与传统的视觉里程计技术相比主要具有三个优点：一是无需大量特征点，所以适用于类似月球表面这样的弱纹理区域；二是视觉测量值准确可靠。其测量精度只取决于图像的分辨率；三是自适应无迹卡尔曼滤波器的估计精度远高于标准卡尔曼滤波器、扩展卡尔曼滤波器与标准无迹卡尔曼滤波器；值得注意的是，该方法不能彻底消除定位误差的累积，但是本发明所设计方法对于提高轮式移动机器人系统定位精度是十分简单有效的，在降低成本的情况下，通过视觉数据和光电码盘数据的相互协同作用下，计算出定位的误差，进行及时的修正，极大的减少了误差累积。

最后说明的是，以上实施例仅用以说明本发明的技术方案而非限制，本领域普通技术人员对本发明的技术方案所做的其他修改或者等同替换，只要不脱离本发明技术方案的精神和范围，均应涵盖在本发明的权利要求范围当中。