CN102980555B - 一种光学成像式轮式移动机器人方位的检测方法及其装置 - Google Patents

Abstract

本发明涉及一种光学成像式轮式移动机器人方位的检测方法及其装置，属于运动定位技术领域。本发明在轮式移动机器人车体安装光源、摄像机和图像处理器。摄像机拍摄地面图像，并将拍摄的图像传输给图像处理器，图像处理器接收摄像机输出的地面图像，测量相邻两帧图像的相对位置和方向的变化值，换算为移动机器人相对地面的位置和方向的变化值，最后将所有变化值分别累加，获得移动机器人相对起始点的位置和方向。本发明的检测方法和装置结构简单、使用方便、价格低廉，在原理上不受时间或机器人结构参数估值的影响，具有很高的长期精度，具有实际推广应用价值。

Description

一种光学成像式轮式移动机器人方位的检测方法及其装置

技术领域

本发明涉及一种光学成像式轮式移动机器人方位的检测方法及其装置，属于运动定位技术领域。

背景技术

随着社会发展和科技进步，机器人在当前生产生活中得到了越来越广泛的应用。轮式移动机器人由于其具有自重轻、承载大、机构简单、驱动和控制相对方便、行走速度快、机动灵活、工作效率高等优点，被大量应用于工业、农业、军事、医院、家庭、空间探测等领域。

轮式移动机器人要完成指定的任务，首先要在移动过程中实时感知当前的位置和方位，并不断与目标位置和方位比较，控制移动机构进行相应的调整，最终到达目标位置，从而有效完成任务。检测移动机器人位置和方位的方法分为两类：自主检测法和参照物检测法。参照物检测方法要依赖磁条、路标、信标（如GPS信号）等外部参照物或参照信号来确定位置和方位，需要建立和维护参照物或参照信号；自主检测方法不需要在外部设立参照物或参照信号，利用机器人相对地球表面的运动来确定位置和方位，检测装置全部安装在机器人内部，工作时不依赖外部信息，因此不易收到外界干扰的影响。

常用自主检测法主要有两种：

（1）基于惯性传感器的测量方法。依据牛顿惯性原理，利用安装在移动机器人内部的加速度计和陀螺，分别测量机器人本身相对地球表面的线加速度和角加速度，对测量结果进行积分，从而计算出机器人移动的距离及方位变化。其缺点是：由于加速度的常值误差将引起与时间平方成正比的测量误差，因此任何小的常值误差都会随时间而无限增大，长期精度很差；每次使用之前需要较长的初始对准时间；惯性测量设备的结构复杂，价格较昂贵。

（2）基于编码器的测量方法。利用安装在轮式移动机器人驱动系统中的编码器分别测量左、右驱动轮的转动角度，然后根据左、右驱动轮的转动角度推算出左、右驱动轮走过的距离，最后估算出机器人的位置和方位。基于编码器的测量方法结构相对简单、成本较低，但由于左、右驱动轮的实际直径不完全相等、有效轮距也在随时变化，因此基于编码器的测量方法的常值误差和测量误差也会随时间不断积累，长期精度也很差。如果出现驱动轮打滑现象，基于编码器的测量方法完全失效。

上述两种位置和方位自主测量方法获得的测量结果是间接测量值。基于惯性传感器的测量结果是根据惯性传感器的加速度直接测量值间接计算出来的，测量结果不仅与惯性传感器加速度有关，而且还与时间的平方成正比；基于编码器的测量结果是根据编码器角位移直接测量值间接计算出来的，测量结果不仅与编码器角位移有关，而且还与驱动系统传动比、驱动轮直径及轮间距等参数有关。因此，时间、驱动系统传动比、驱动轮直径及轮间距等参数的估算误差都会传递到位置和方位的测量结果中，这些估算误差会随时间不断积累，导致上述两种测量方法的测量结果误差越来越大，长期精度变低。

发明内容

本发明的目的是提出一种光学成像式轮式移动机器人方位的检测方法及其装置，以克服现有技术中存在的定位测量误差随时间累计而无限增大导致的长期测量后定位精度变低的问题，以提高轮式移动机器人位置和方位的长期检测精度。

本发明提出的光学成像式轮式移动机器人方位的检测方法，包括以下步骤：

（1）在轮式移动机器人车体的底部安装一个摄像机，用于按设定的采样周期拍摄地面图像，并将拍摄的图像传输给图像处理器；

（2）设定一个地面坐标系xoy，设定一个移动机器人的坐标系XOY，初始位置时两个坐标系重合；

（3）图像处理器连续接收摄像机输出的地面图像，在第一帧地面图像中设定两个特征点M和N，则两个特征点M和N在坐标系XOY中的坐标分别为（X_m0，Y_m0）和（X_n0，Y_n0），两个特征点M和N在第二帧图像中的坐标系XOY中的坐标分别为（X_m1，Y_m1）和（X_n1，Y_n1），通过计算，得到机器人相对地面坐标系的旋转角度△θ₁：

△θ₁=arc tan[（|tanα-tanβ|）/（1+tanα×tanβ）]，

其中，α为点（X_m0，Y_m0）和点（X_n0，Y_n0）之间的连线与坐标系XOY的X轴的夹角，β为点（X_m1，Y_m1）和点（X_n1，Y_n1）之间的连线与坐标系XOY的Y轴的夹角，tanα=（Y_n0-Y_m0）/（X_n0-X_m0），tanβ=（Y_n1-Y_m1）/（X_n1-X_m1）；

（4）根据上述旋转角度△θ₁，得到机器人在地面坐标系中沿x轴方向的移动增量△g₁和沿y轴方向的移动增量△h₁：

△g₁=-△X₁×cosθ+△Y₁×sinθ₁+（X_m0×sinθ₁+Y_m0×cosθ₁）×△θ₁

△h₁=-△X₁×sinθ₁—△Y₁×cosθ₁+（X_m0×cosθ₁—Y_m0×sinθ₁）×△θ₁，

其中△X₁=X_m1-X_m0，△Y₁=Y_m1-Y_m0；

（5）重复步骤（3）和（4），连续接收摄像机输出的地面图像，得到机器人沿地面坐标系的x轴方向的移动量g、沿y轴方向的移动量h以及相对地面坐标系xoy旋转θ角：

g=△g₁+△g₂+…+△g_i

h=△h₁+△h₂+…+△h_i

θ=△θ₁+△θ₂+…+△θ_i。

本发明提出的光学成像式轮式移动机器人方位的检测装置，包括：

光源，用于地面照明，所述的光源固定在轮式移动机器人的车体上；

摄像机，用于拍摄地面图像，并将图像信号传送给图像处理器，所述的摄像机固定在轮式移动机器人的车体上；

图像处理器，用于接收摄像机输出的图像信号，并进行预处理，测量相邻两帧图像的相对位置和方向的变化值，然后换算为移动机器人相对地面的位置和方向的变化值，最后将所有位置和方向变化值分别累加，获得移动机器人相对起始点的位置和方向，所述的图像处理器与摄像机相连接。

本发明提出的光学成像式轮式移动机器人方位的检测方法及其装置，其优点是：利用本发明方法得到的测量结果，仅与机器人相对地面的相对位置和方向变化有关，与测量时间或移动机器人的结构参数估值无关，因此从理论上消除了众多误差因素的影响；移动机器人位置和方位的测量结果是多次直接测量值的累加，由于直接测量值的测量误差为随机误差，因此随着机器人移动距离的增大，直接测量值的数量也不断增加，测量误差的累加值将趋于零，因此本发明测量方法具有较高的长期测量精度，解决了现有检测技术测量误差随时间累计增大而导致的长期精度变低的问题。而且本发明的轮式移动机器人位置和方向的检测装置，结构简单、使用方便、价格低廉，在原理上不受时间或机器人结构参数估值的影响，有很高的长期精度，具有实际推广应用价值。

附图说明

图1是本发明检测装置的结构原理示意图。

图2是本发明检测方法的坐标系与地面坐标系关系示意图。

图3是本发明测量装置中摄像机采集的图像，其中（a）是一个采样间隔中的第一帧图像，（b）是一个采样间隔的第二帧图像。

图1中，1是轮式移动机器人车体、2是图像处理器、3是摄像机、4是光源。

具体实施方式

本发明提出的光学成像式轮式移动机器人方位的检测方法，包括以下步骤：

（1）在轮式移动机器人车体1的底部安装一个摄像机3，用于按设定的采样周期拍摄地面图像，并将拍摄的图像传输给图像处理器。

（2）设定一个地面坐标系xoy，设定一个移动机器人的坐标系XOY，初始位置时两个坐标系重合，如图2中所示，其中xoy直角坐标系为地面坐标系，XOY为轮式移动机器人检测装置坐标系，g是XOY坐标系（车体）沿x轴平移的距离，h是XOY坐标系（车体）沿y轴平移的距离，θ是XOY坐标系（车体）相对xoy坐标系的转动角度。

（3）图像处理器2连续接收摄像机3输出的地面图像，如图3所示，在第一帧地面图像中设定两个特征点M和N，则两个特征点M和N在坐标系XOY中的坐标分别为（X_m0，Y_m0）和（X_n0，Y_n0），两个特征点M和N在第二帧图像中的坐标系XOY中的坐标分别为（X_m1，Y_m1）和（X_n1，Y_n1），通过计算，得到机器人相对地面坐标系的旋转角度△θ₁：

△θ₁=arc tan[（|tanα-tanβ|）/（1+tanα×tanβ）]，

其中，α为点（X_m0，Y_m0）和点（X_n0，Y_n0）之间的连线与坐标系XOY的X轴的夹角，β为点（X_m1，Y_m1）和点（X_n1，Y_n1）之间的连线与坐标系XOY的Y轴的夹角，tanα=（Y_n0-Y_m0）/（X_n0-X_m0），tanβ=（Y_n1-Y_m1）/（X_n1-X_m1）；

（4）根据上述旋转角度△θ₁，得到机器人在地面坐标系中沿x轴方向的移动增量△g₁和沿y轴方向的移动增量△h₁：

△g₁=-△X₁×cosθ+△Y₁×sinθ₁+（X_m0×sinθ₁+Y_m0×cosθ₁）×△θ₁

△h₁=-△X₁×sinθ₁—△Y₁×cosθ₁+（X_m0×cosθ₁-Y_m0×sinθ₁）×△θ₁，

其中△X₁=X_m1-X_m0，△Y₁=Y_m1-Y_m0；

（5）重复步骤（3）和（4），连续接收摄像机输出的地面图像，得到机器人沿地面坐标系的x轴方向的移动量g、沿y轴方向的移动量h以及相对地面坐标系xoy旋转θ角：

g=△g₁+△g₂+…+△g_i

h=△h₁+△h₂+…+△h_i

θ=△θ₁+△θ₂+…+△θ_i。

本发明提出的光学成像式轮式移动机器人方位的检测装置，其结构如图1所示，包括：

光源4，用于地面照明，所述的光源固定在轮式移动机器人的车体1上；

摄像机3，用于拍摄地面图像，并将图像信号传送给图像处理器，所述的摄像机固定在轮式移动机器人的车体上；

图像处理器2，用于接收摄像机3输出的图像信号，并进行预处理，测量相邻两帧图像的相对位置和方向的变化值，然后换算为移动机器人相对地面的位置和方向的变化值，最后将所有位置和方向变化值分别累加，获得移动机器人相对起始点的位置和方向，所述的图像处理器与摄像机相连接。

本发明提出的光学成像式轮式移动机器人方位的检测方法中，图像处理器接收摄像机输出的地面图像，在第一帧地面图像中设定两个特征点M和N，并测量M和N在第一帧图像中的坐标值（X_m0，Y_m0）和（X_n0，Y_n0），再测量M和N在第二帧图像中的坐标值（X_m1，Y_m1）和（X_n1，Y_n1），计算直线[（X_m0，Y_m0），（X_n0，Y_n0）]和直线[（X_m1，Y_m1），（X_n1，Yn1）]之间的夹角△θ1，以及车体相对地面M点的位移△X2=Xm1-Xm0和△Y2=Ym1-Ym0，最后可根据△θ1、△X1和△Y1，计算出移动机器人在第二帧图像时车体相对地面的平移距离增量△g1和△h1，最后将连续i个采样周期的移动机器人平移距离增量△gi和△hi，以及车体相对地面的转动角度变化△θi分别累加，可得到第i个采样时刻时车体相对地面坐标系原点的位置g=△g1+△g2+…+△gi和h=△h1+△h2+…+△hi，以及方位角θ=△θ1+△θ2+…+△θi。车体平移距离g和h的增量△g和△h与地面M点在XOY坐标系中前一时刻的位置X、Y及位置变化△X、△Y，以及车体XOY坐标系相对地面xoy坐标系前一时刻的转动角度θ和转动角度变化△θ有关。

图3中，（a）是一个采样间隔的第一帧图像，（b）是一个采样间隔的第二帧图像。其中的M、N是地面上的两个特征点，在相邻的前后两帧数字图像中，M、N点的坐标值分别从（X_m0，Y_m0）和（X_n0，Y_n0）变化为（X_m1，Y_m1）和（X_n1，Y_n1）。α为MN两点连线在第一帧图像中的倾角，β为MN两点连线在第二帧图像中的倾角，MN两点连线在车体XOY坐标系中的偏转角度△θ₁就是车体相对地面的转动角度变化△θ₁。△θ₁可利用两直线夹角计算公式计算：

△θ₁=arc tan[（|tanα-tanβ|）/（1+tanα*tanβ）]

其中tanα=（Y_n0-Y_m0）/（X_n0-X_m0），tanβ=（Y_n1-Y_m1）/（X_n1-X_m1）。

在一个采样间隔内，M点的X轴坐标变化为△X₁=X_m1-X_m0，Y轴坐标变化为△Y₁=Y_m1-Y_m0。

设初始位置轮式移动机器人车体XOY坐标系与地面xoy坐标系重合，图像处理器根据X_m0、△X₁、Y_m0、△Y₁和△θ₁计算出车体平移距离的增量△g₁和△h₁。连续测量并计算△g_i和△h_i（i=1、2、3…），则第i个采样时刻时的轮式移动机器人车体位置及方位为：

g=△g₁+△g₂+…+△g_i

h=△h₁+△h₂+…+△h_i

θ=△θ₁+△θ₂+…+△θ_i

本发明的轮式移动机器人位置和方向的检测装置中，照明光源4采用威世半导体（Vishay Semiconductors）公司的TLHG520红外线发光二极管，摄像机3采用分辨率为2048×1536的CCD（Charge Coupled Device）数字摄像机，图像处理器2采用TI公司的TMS320数字信号处理器，图像采样速率设为6帧/秒。

本发明轮式移动机器人位置和方向检测装置的工作原理是：

照明光源4对地面照明，突出地面纹理特征，增强地面细节。

由于摄像机3的图像位移变化与真实地面上的位移变化存在一个比例关系，因此首先对摄像机3进行标定。在地面上标识5个已知坐标值的参考点，使5个参考点位于摄像机3视场的四个角及中央；图像处理器2通过图像处理提取各参考点在摄像机3坐标系中的坐标值，与参考点在地面坐标系中的坐标值对比，获得摄像机3坐标系与地面坐标系的比例关系。

摄像机3以6帧/秒的速率拍摄地面图像，并将数字图像信号传送给图像处理器2。图像处理器2首先对数字图像进行滤波处理，去除图像中的噪声，对数字数字图像进行增强处理，突出地面上的特征点；然后设定地面特征点，测量前后两帧图像中相同特征点的位置变化△X_i和△Y_i，以及车体相对地面坐标系的方位角变化△θ_i，计算车体相对地面平移距离增量△g_i和△h_i。分别将△g_i、△h_i和△θ_i累加，可获得当前移动机器人相对地面坐标系原点的位置（g，h）和方位θ。

Claims (2)

1.一种光学成像式轮式移动机器人方位的检测方法，其特征在于该检测方法包括以下步骤：

(1)在轮式移动机器人车体的底部安装一个摄像机，用于按设定的采样周期拍摄地面图像，并将拍摄的图像传输给图像处理器；

(2)设定一个地面坐标系xoy，设定一个移动机器人的坐标系XOY，初始位置时两个坐标系重合；

(3)图像处理器连续接收摄像机输出的地面图像，在第一帧地面图像中设定两个特征点M和N，则两个特征点M和N在坐标系XOY中的坐标分别为(X_m0，Y_m0)和(X_n0，Y_n0)，两个特征点M和N在第二帧图像中的坐标系XOY中的坐标分别为(X_m1，Y_m1)和(X_n1，Y_n1)，通过计算，得到机器人相对地面坐标系的旋转角度△θ₁：

△θ₁＝arc tan[(|tanα‐tanβ|)/(1+tanα×tanβ)]，

其中，α为点(X_m0，Y_m0)和点(X_n0，Y_n0)之间的连线与坐标系XOY的X轴的夹角，β为点(X_m1，Y_m1)和点(X_n1，Y_n1)之间的连线与坐标系XOY的Y轴的夹角，tanα＝(Y_n0‐Y_m0)/(X_n0‐X_m0)，tanβ＝(Y_n1‐Y_m1)/(X_n1‐X_m1)；

(4)根据上述旋转角度△θ₁，得到机器人在地面坐标系中沿x轴方向的移动增量△g₁和沿y轴方向的移动增量△h₁：

△g₁＝‐△X₁×cosθ+△Y₁×sinθ₁+(X_m0×sinθ₁+Y_m0×cosθ₁)×△θ₁

△h₁＝‐△X₁×sinθ₁—△Y₁×cosθ₁+(X_m0×cosθ₁—Y_m0×sinθ₁)×△θ₁，

其中△X₁＝X_m1‐X_m0，△Y₁＝Y_m1‐Y_m0；

(5)重复步骤(3)和(4)，连续接收摄像机输出的地面图像，得到图像中机器人沿地面坐标系的x轴方向的移动量g、沿y轴方向的移动量h以及相对地面坐标系xoy旋转θ角：

g＝△g₁+△g₂+…+△g_i

h＝△h₁+△h₂+…+△h_i

θ＝△θ₁+△θ₂+…+△θ_i；

其中，i是移动机器人平移距离的连续采样周期数；

(6)对摄像机进行标定，在地面上标识5个已知坐标值的参考点，使5个参考点位于摄像机视场的四个角及中央；图像处理器通过图像处理提取各参考点在摄像机图像中地面坐标系中的坐标值，与参考点在真实地面坐标系中的坐标值对比，获得摄像机图像中地 面坐标系与真实地面坐标系的比例关系a，根据该比例关系a和上述图像中机器人沿地面坐标系的x轴方向的移动量g和沿y轴方向的移动量h，得到机器人沿地面坐标系的x轴方向的移动量G和沿y轴方向的移动量H，G＝a×g，H＝a×h。

2.一种用于执行如权利要求1所述的光学成像式轮式移动机器人方位的检测方法的检测装置，其特征在于该装置包括：

光源，用于地面照明，所述的光源固定在轮式移动机器人的车体上；

摄像机，用于拍摄地面图像，并将图像信号传送给图像处理器，所述的摄像机固定在轮式移动机器人的车体上；

图像处理器，用于接收摄像机输出的图像信号，并进行预处理，测量相邻两帧图像的相对位置和方向的变化值，然后换算为移动机器人相对地面的位置和方向的变化值，最后将所有位置和方向变化值分别累加，获得移动机器人相对起始点的位置和方向，所述的图像处理器与摄像机相连接。