一种基于视觉信息校正的室内移动机器人实时导航方法.pdf

本发明公开了一种基于视觉信息校正的室内移动机器人实时导航方法，包括步骤一：初始化机器人导航系统，启动里程计和视觉传感器；步骤二：里程计和视觉传感器得到机器人当前位资，构造卡尔曼滤波器，获得里程计滤波估计值和视觉传感器滤波估计值；步骤三：里程计滤波估计值和视觉传感器滤波估计值融合；步骤四：参数重置；本发明充分利用了视觉信息和里程计信息的各自优点，将视觉信息的精确性和里程计信息的实时性相结合起来，在大部分时间下，利用里程计自身信息进行递推计算，获得导航数据，保证了导航系统的实时性要求，同时利用视觉信息来矫正里程计航迹推算过程中产生的累积误差，从而大大提高了导航系统的准确性。

1、  一种基于视觉信息校正的室内移动机器人实时导航方法，其特征在于，包含以下步骤：
步骤一：初始化机器人导航系统，启动里程计和视觉传感器；
初始化包括确定移动机器人的初始位置、初始化导航系统中所有滤波器的参数；
步骤二：里程计和视觉传感器得到机器人当前位姿，构造卡尔曼滤波器，获得里程计滤波估计值和视觉传感器滤波估计值；
(1)里程计测量通道；
利用里程计获得移动机器人左、右轮的移动距离和移动速度，采用航迹递推的方法获得当前机器人位姿；
(2)视觉传感器测量通道；
通过全局运动目标检测算法检测移动机器人在图像中的位置，并通过离线的标定建立图像位置与实际空间的映射关系，获得移动机器人当前位姿；
(3)构造卡尔曼滤波器，获得里程计滤波估计值和视觉传感器滤波估计值；
结合机器人运动学方程，构造卡尔曼滤波器，将视觉传感器测量通道和里程计测量通道所得到的测量值，通过卡尔曼滤波得到里程计滤波估计值和视觉传感器滤波估计值；
步骤三：里程计滤波估计值和视觉传感器滤波估计值融合；
(1)在未获得视觉传感器滤波器滤波估计值时；
利用里程计滤波器滤波估计值与移动机器人模型公共参考系统进行信息融合，获得最优滤波值
(2)当获得视觉传感器滤波器滤波估计值后；
利用里程计滤波器滤波估计值，视觉传感器滤波器滤波估计值与移动机器人模型公共参考系统进行信息融合，获得最优滤波值
按照最优融合公式(1)，将各滤波器的滤波估计值进行融合：
x^g=PgΣi=1NPii-1x^iPg-1=Σi=1NPii-1---(1)]]>
式中：表示最优滤波值，P_g为融合后的估计误差方差矩阵，P_ii为第i个滤波器的估计误差方差矩阵，为第i个滤波器的滤波估计值，N为自然数，表示包括滤波器的个数；
步骤四：参数重置；
判断导航是否结束，若导航未结束，则将获得的最优滤波估计值或者重置到里程计滤波器和视觉传感器滤波器，返回步骤二，进行下一次的滤波；
同时根据获得的最优滤波估计值或者利用移动机器人运动模型，预测下一步移动机器人的位姿信息，从而确定在下一帧图像中移动机器人目标所在的位置，以该位置作为下一次运动目标检测时的搜索启发点；
若导航结束，则本方法结束。

2、  根据权利要求1所述的一种基于视觉信息校正的室内移动机器人实时导航方法，其特征在于，步骤二(2)中视觉传感器测量通道的具体步骤如下：
①采用基于混合高斯背景建模的运动目标检测方法获得移动机器人在图像中的位置；
用K个不同均值与方差的高斯分布描述非完全静止条件下的背景图像；设像素点(u，v)的噪声强度z(u，v)服从混合高斯分布，则该点的概率密度函数为：
P(zuv)=Σj=1Kwj,uvη(zuv|μj,uv,Σj,uv)---(2)]]>
其中：η(z_uv|μ_j，uv，∑_j，uv)表示采集到的图像中(u，v)点的第j个高斯分布，其均值为μ_j，uv，方差为∑_j，uv，该点当前灰度值为z_uv，w_j，uv为第j个高斯分布的加权权重，P(z_uv)为图像中(u，v)点的概率分布函数；
基于混合高斯背景建模的过程包括以下步骤：
i)初始化模型；
视觉传感器采集的第一幅图像中每个像素点的像素值作为均值，方差和权值任取，建立一个高斯模型；
ii)模型学习；
视觉传感器采集到新图像，将新图像中的每个像素与该像素的已有的k个高斯模型相比较，其中k≤K；
若满足|z_uv-μ_j，uv|＜2.5∑_j，uv，采用EM算法估计第j个高斯参数和权重；
若不满足，且k＜K，则增加一个高斯模型，若k＝K，则用新的高斯分布代替优先级最低的高斯分布，新的高斯分布取z_uv的值为均值、并赋予新的方差和权值；
视觉传感器继续采集图像，不断地训练初始化模型，最终得到K个|z_uv-μ_j，uv|＜2.5∑_j，uv的高斯分布函数时，停止训练过程，得到高斯混合分布的背景图像模型；
iii)前景图像评估；
去除背景模型中用噪声或运动目标建立的高斯分布模型；将K个高斯分布模型按照优先级高低次序排列，前B个分布作为背景模型，B的定义如下：
B=minb(Σj=1bwj,uv>M)---(3)]]>
M是预先定义的阈值，其中b＜K；
对每一幅采集到的新图像，将图像的每一个像素点与该像素点对应的高斯混合模型的各个高斯模型进行比较，若有|z_uv-μ_j，uv|＜2.5∑_j，uv，则该点属于背景，否则属于前景；所有属于前景的点组成了图像中运动目标，从而获得了移动机器人在图像中的位置；
②离线的标定建立图像位置与实际空间的映射关系，从而测量获得移动机器人当前位姿；
根据步骤①获得移动机器人在图像中的位置，利用全局摄像机的离线标定所获得的图像坐标系到实际空间坐标系的映射关系，计算得到移动机器人在实际空间中的位姿信息；
其中映射关系的如下：
Xi=a0+a1xi+a2yi+a3xi2+a4yi2+a5xiyi+a6xi3+a7yi3+a8xi2yi+a9xiyi2Yi=b0+b1xi+b2yi+b3xi2+b4yi2+b5xiyi+b6xi3+b7yi3+b8xi2yi+b9xiyi2---(4)]]>
式中(X_i，Y_i)为机器人在空间中的实际位置，(x_i，y_i)为机器人在图像中的坐标位置，在地面上选取48个控制点，利用最小二乘法对式(4)进行标定，获得多项式系数a_i，b_i，i＝1，2...9；得到了该映射关系后，每次检测到移动机器人在图像中的位置后，都将它映射到实际空间中，得到其在实际空间中的位置，从而获得视觉传感器测量的位姿。

3、  根据权利要求1所述的一种基于视觉信息校正的室内移动机器人实时导航方法，其特征在于，步骤二(3)中构造卡尔曼滤波器，获得里程计滤波估计值和视觉传感器滤波估计值的具体步骤如下：
根据移动机器人运动模型以及各个噪声统计信息构造扩展卡尔曼滤波器，结合移动机器人的非完整运动学方程，卡尔曼滤波器的滤波方程组如式(5)～式(9)：
X^k+1/k=f[X^k/k,uk]---(5)]]>
X^k+1=X^k+1/k+Kk+1[Zk+1-h[X^k+1,uk+1]]---(6)]]>
Kk+1=Pk+1/kHkT[Hk+1Pk+1/kHkT+R]-1---(7)]]>
P_k+1/k＝ΦP_k/kΦ^T+Q     (8)
P_k+1＝[I-K_k+1H_k+1]P_k+1/k(9)
其中：Q为导航系统噪声的方差矩阵，R为测量噪声的方差矩阵，两者均为零均值时的高斯噪声，P为估计误差的协方差矩阵，P_k/k为k时刻P的估计值，P_k+1/k为k+1时刻P的预测值；为系统状态，为k时刻的状态估计值，为k+1时刻的状态预测值，K为卡尔曼滤波增益，K_k为k时刻的卡尔曼滤波增益，K_k+1为k+1时刻的卡尔曼滤波增益，Φ为线性化的状态转移矩阵；
通过式(5)～式(9)得到的滤波估计值为

一种基于视觉信息校正的室内移动机器人实时导航方法
技术领域
本发明涉及一种基于视觉信息校正的室内移动机器人实时导航方法，属于机器人导航领域。
背景技术
导航系统是移动机器人的重要组成部分之一，它能够提供给机器人正确的位置和姿态信息，从而使得机器人能在一定的环境中进行工作，完成指定的任务。移动机器人的导航系统也是实现移动机器人路径规划，避障避碰，智能自主控制的基础。
目前常见的机器人导航系统的方式有：电磁导航、光反射导航、GPS导航、环境地图模型匹配导航、路标导航、视觉导航等。
(1)电磁导航也称地下埋线导航，其原理是在路径上连续的埋设多条引导电缆，分别流过不同频率的电流，通过感应线圈对电流的检测来获得感知信息。该技术简单实用，但是成本高、改造和维护困难。
(2)光反射导航的原理是在路径上连续的铺设光反射条，同电磁导航一样，该技术也相当的成熟，目前国内制造行业使用的移动机器人大多是基于光反射导航与电磁导航这两种导航方式的，但是它们应用范围比较窄，不适合在动态、变换工作环境下的移动机器人导航。
(3)GPS导航：全球定位系统(GPS)在陆地车辆导航系统中得到了广泛的应用。但是在诸如城市地区，高楼大厦GPS信号经常阻塞，这就意味着有一大部分空间无法获得GPS信号，在室内也同样存在同样的问题，所以不能采用逐点定位的导航系统。
(4)环境地图模型匹配导航：是机器人通过自身的各种传感器探测周围环境，利用感知到的局部环境信息进行局部的地图构造，并与其内部事先存储的完整地图进行匹配。通过匹配，机器人可确定自身的位置，并根据预先规划的一条全局路线，采用路径跟踪和相关避障技术来实现导航。
(5)路标导航：是指路标在环境中的坐标、形状等特征已知的前提下，机器人通过对路标的探测来确定自身的位置，同时将全局路线分解成为路标与路标间的片段，逐段行进完成导航。虽然人工路标导航比较容易实现，但它人为地改变了机器人工作的环境，自然路标导航不改变工作环境，但机器人需要通过对工作环境中的自然特征进行识别以完成导航。路标探测的稳定性和鲁棒性是采用该方法时需要研究的主要问题。
(6)视觉导航：具有信号探测范围宽、获取信息完整等优点，将成为未来机器人导航的一个主要发展方向。在视觉导航方式中，目前应用最多的还是采用在机器人上安装车载摄像机的基于局部视觉的导航方式，如D.L.Boley等研制的移动机器人，利用车载摄像机和较少的传感器通过识别路标进行导航，比直接采用卡尔曼滤波器具有更好的实时性，并可有效抑制噪声。采用局部视觉这种导航方式，所有的计算设备和传感器都装载在机器人车体上。图像识别、路径规划等高层决策都由车载计算机完成，所以车载计算机的工作量较大，延迟问题也较为明显。
(7)基于传感器数据的机器人导航方式：一般机器人都安装了一些非视觉传感器，如里程计，磁惯传感器，超声传感器、红外传感器、接触传感器等。这类传感器可以实现对机器人的定位与导航。比如，当机器人在光线很暗的环境中的时候，视觉导航方式将会失效。在这种情况下，可以利用里程计或磁惯传感器来进行导航。
根据室内移动机器人导航的要求，目前最常见的是基于里程计或磁惯传感器的导航系统以及基于视觉的导航系统，这两类系统本身都存在着一定的局限性，首先里程计或磁惯传感器通常处理速度较快，但是由于它们都是利用航迹递推的方法进行导航定位，因此必然会引入较大的累积误差。而基于视觉的导航系统通常精度较高，但是图像的获取以及处理所消耗的时间较大，因此实时性较差。
发明内容
本发明的目的是为了克服现有技术的不足，提出一种基于视觉信息校正的室内移动机器人实时导航方法，该方法通过综合集成视觉信息以及里程计信息的优势和特点，使得该导航系统同时兼顾实时性要求和导航精度要求。
本发明的一种基于视觉信息校正的室内移动机器人实时导航方法，其特征在于采用将视觉信息与里程计信息相融合的方式，通过对各滤波器估计位姿的最优融合，获得机器人的最优估计位姿，从而提高系统的实时性和精确度。其主要步骤为：
步骤一：初始化机器人导航系统，启动里程计和视觉传感器；
步骤二：里程计和视觉传感器得到机器人当前位资，构造卡尔曼滤波器，获得里程计滤波估计值和视觉传感器滤波估计值；
步骤三：里程计滤波估计值和视觉传感器滤波估计值融合；
步骤四：参数重置；
本发明的优点在于：
(1)本发明充分利用了视觉信息和里程计信息的各自优点，将视觉信息的精确性和里程计信息的实时性相结合起来，在大部分时间下，利用里程计自身信息进行递推计算，获得导航数据，保证了导航系统的实时性要求。同时利用视觉信息来矫正里程计航迹推算过程中产生的累积误差，从而大大提高了导航系统的准确性；
(2)本发明采用了基于混合高斯背景建模的全局运动目标检测方法，大大提高了运动目标检测的抗干扰能力，对光照的变化以及缓慢变化的背景噪声有抑制作用；
(3)本发明在获得最优滤波估计之后，加入了参数的重置，使得各滤波器始终以最优滤波估计值为基础进行递推估计，加快了滤波器的收敛速度；
(4)本发明利用当前获得的最优滤波估计值，对下一帧图像中目标可能出现的位置进行了预测，大大提高了目标检测的速度。
附图说明
图1是本发明所述方法的流程图；
图2是本发明卡尔曼滤波器的示意图。
具体实施方式
下面将结合附图和实施例对本发明作进一步的详细说明。
本发明是一种基于视觉信息校正的室内移动机器人实时导航方法，流程如图1所示，主要包括以下步骤：
步骤一：初始化机器人导航系统，启动里程计和视觉传感器；
对导航系统进行初始化，包括确定移动机器人的初始位置以及初始化所有滤波器的各个参数，其中滤波器的主要参数为移动机器人运动模型的系统噪声方差矩阵和各传感器的测量噪声方差矩阵，启动里程计和视觉传感器。
步骤二：里程计和视觉传感器得到机器人当前位资，构造卡尔曼滤波器，获得里程计滤波估计值和视觉传感器滤波估计值；
(1)里程计测量通道；
利用里程计获得移动机器人左、右轮的移动距离和移动速度，采用航迹递推获的方法获得当前机器人位姿。
(2)视觉传感器测量通道；
通过全局运动目标检测算法检测移动机器人在图像中的位置，并通过离线的标定建立图像位置与实际空间的映射关系，从而测量获得移动机器人当前位姿，其具体步骤如下：
①采用基于混合高斯背景建模的运动目标检测方法获得移动机器人在图像中的位置；
采用基于混合高斯背景建模方法进行移动机器人目标的运动检测，用K个不同均值与方差的高斯分布来描述非完全静止条件下的背景图像。设像素点(u，v)的噪声强度z(u，v)服从混合高斯分布，则该点的概率密度函数可用式(1)表示：
P(zuv)=Σj=1Kwj,uvη(zuv|μj,uv,Σj,uv)---(1)]]>
其中：η(z_uv|μ_j，uv，∑_j，uv)表示用于描述采集到的图像中(u，v)点的第j个高斯分布，其均值为μ_j，uv，方差为∑_j，uv，该点当前灰度值为z_uv，而w_j，uv为第j个高斯分布的加权权重，P(z_uv)为图像中(u，v)点的概率分布函数。
基于混合高斯背景建模的过程包括以下步骤：
i)初始化模型；
用视觉传感器采集的第一幅图像中每个像素点的像素值作为均值、方差和权值任取，建立一个高斯模型。
ii)模型学习；
视觉传感器采集到新图像，将新图像中的每个像素与该像素的已有的k个高斯模型相比较，其中k≤K；
若满足|z_uv-μ_j，uv|＜2.5∑_j，uv，则利用EM算法估计第j个高斯参数和权重；
若不满足，且k＜K，则增加一个高斯模型，若k＝K，则用新的高斯分布代替优先级最低的高斯分布，新的高斯分布取z_uv的值为均值、并赋予新的方差和权值；
视觉传感器继续采集图像，不断地训练初始化模型，最终得到K个|z_uv-μ_j，uv|＜2.5∑_j，uv的高斯分布函数时，停止训练过程，从而得到高斯混合分布的背景图像模型。
iii)前景图像评估；
由于有噪声的影响或运动目标的存在，背景图像模型中些像素点并不能真正地代表背景，用这类像素建立的高斯分布模型应该被去除。假设运动目标及噪声在场景区域中不会长时间停留在某一位置上，这样，噪声和运动目标相对应的高斯模型的权值和优先级非常小，将K个高斯分布按照优先级高低次序排列，前B个分布作为背景模型，B的定义如下：
B=minb(Σj=1bwj,uv>M)---(2)]]>
M是预先定义的阈值，它表示能真正反映背景的数据占总数据的最小比重，如果M＝1，就是个单高斯分布的背景模型；M＞1，就是高斯混合分布的背景模型；其中b＜K。
对每一幅采集到的新图像，将图像的每一个像素点与该像素点对应的高斯混合模型的各个高斯模型进行比较，若有|z_uv-μ_j，uv|＜2.5∑_j，uv，则该点属于背景，否则属于前景。所有属于前景的点组成了图像中运动目标，从而获得了移动机器人在图像中的位置。
②离线的标定建立图像位置与实际空间的映射关系，从而测量获得移动机器人当前位姿；
根据步骤①获得移动机器人在图像中的位置，利用全局摄像机的离线标定所获得的图像坐标系到实际空间坐标系的映射关系，计算得到移动机器人在实际空间中的位姿信息。
其中映射关系的标定过程如下：
将畸变的因素考虑到从图像平面到实际空间平面的映射关系中去，建立三次多项式，如式(3)所示，获得校正后的空间坐标位置：
Xi=a0+a1xi+a2yi+a3xi2+a4yi2+a5xiyi+a6xi3+a7yi3+a8xi2yi+a9xiyi2Yi=b0+b1xi+b2yi+b3xi2+b4yi2+b5xiyi+b6xi3+b7yi3+b8xi2yi+b9xiyi2---(3)]]>
式中(X_i，Y_i)为机器人在空间中的实际位置，(x_i，y_i)为机器人在图像中的坐标位置，在地面上选取48个控制点，利用最小二乘法对式(3)进行标定，可以获得多项式系数a_i，b_i，i＝1，2...9。得到了该映射关系后，每次检测到移动机器人在图像中的位置后，都可以将它映射到实际空间中，得到其在实际空间中的位置，从而获得视觉传感器测量的位姿。
(3)构造卡尔曼滤波器，获得里程计滤波估计值和视觉传感器滤波估计值；
结合机器人运动学方程，为两个测量通道构造卡尔曼滤波器，将视觉传感器测量通道和里程计测量通道所得到的测量值，通过卡尔曼滤波得到里程计滤波估计值和视觉传感器滤波估计值。
根据移动机器人运动模型以及各个噪声统计信息构造扩展卡尔曼滤波器，结合移动机器人的非完整运动学方程，卡尔曼滤波器的滤波方程组如式(4)～式(8)：
X^k+1/k=f[X^k/k,uk]---(4)]]>
X^k+1=X^k+1/k+Kk+1[Zk+1-h[X^k+1,uk+1]]---(5)]]>
Kk+1=Pk+1/kHkT[Hk+1Pk+1/kHkT+R]-1---(6)]]>
P_k+1/k＝ΦP_k/kΦ^T+Q         (7)
P_k+1＝[I-K_k+1H_k+1]P_k+1/k    (8)
其中：Q为导航系统噪声的方差矩阵，R为测量噪声的方差矩阵，两者均为零均值时的高斯噪声，P为估计误差的协方差矩阵，P_k/k为k时刻P的估计值，P_k+1/k为k+1时刻P的预测值。为系统状态，为k时刻的状态估计值，为k+1时刻的状态预测值，K为卡尔曼滤波增益，K_k为k时刻的卡尔曼滤波增益，K_k+1为k+1时刻的卡尔曼滤波增益，Φ为线性化的状态转移矩阵，通过式(5)～式(9)得到的滤波估计值为其工作结构示意图如图2所示，在的基础上，利用式(4)获得同时根据P_k，利用式(7)获得P_k+1/k，利用式(6)获得K_k+1，利用K_k+1和根据式(5)就可以获得同时利用K_k+1和P_k+1/k，根据式(8)就可以获得P_k+1，为第k+1步的滤波估计值，即获得里程计滤波估计值和视觉传感器滤波估计值。
步骤三：里程计滤波估计值和视觉传感器滤波估计值融合；
由于视觉传感器的处理速度远远小于里程计的处理速度，可能出现里程计已获得测量数据，其滤波器计算获得了滤波估计值，而视觉传感器还未完成其图像处理的过程，没有获得测量数据，也没有获得滤波估计值，因此滤波估计值的融合有以下两种情况：
(1)在未获得视觉传感器滤波器滤波估计值时；
利用里程计滤波器滤波估计值与移动机器人模型公共参考系统进行信息的融合，获得最优滤波值
(2)当获得视觉传感器滤波器滤波估计值后；
利用里程计滤波器滤波估计值，视觉传感器滤波器滤波估计值与移动机器人模型公共参考系统进行信息的融合，获得最优滤波值
按照最优融合公式(9)，将各滤波器的滤波估计值进行融合：
x^g=PgΣi=1NPii-1x^iPg-1=Σi=1NPii-1---(9)]]>
式中：表示最优滤波值，P_g为融合后的估计误差方差矩阵，P_ii为第i个滤波器的估计误差方差矩阵，为第i个滤波器的滤波估计值，N为自然数，表示包括滤波器的个数；
步骤四：参数重置；
判断导航是否结束，若未结束导航，则将获得的最优滤波估计值，重置到里程计滤波器和视觉传感器滤波器，代替各滤波器的滤波估计值，返回步骤二，进行下一次的滤波；
同时根据获得的最优位姿估计值，利用移动机器人运动模型，预测下一步移动机器人的位姿信息，从而确定在下一帧图像中移动机器人目标所在的大概位置，以该位置作为下一次运动目标检测时的搜索启发点，加快整个目标检测的速度。
若导航结束，则本方法结束。