一种基于固定摄像头视觉的移动机器人定位方法与流程

本发明属于移动机器人定位技术领域，尤其涉及一种固定摄像头视觉的移动机器人定位方法。

背景技术：

随着对室内定位需求的不断扩大，有效性和适应性强的室内的定位方法已成为研究热点。而在移动机器人领域，获取移动机器人的准确实时位置也是一个关键问题。

现有的定位技术有惯性导航定位、无线信号定位、激光雷达定位和视觉定位等。其中惯性导航定位由于存在漂移会产生较大的累计误差，而无线信号也容易被干扰导致无法准确定位，激光雷达则是设备成本高。相比之下，视觉定位的成本较低，但可以得到一个相对高的精度。

视觉定位方法又可以分为相对定位和全局定位。相对定位中使用比较多的就是视觉slam(simultaneouslocalizationandmapping)，目前的slam主要采用基于路标的方法，也即通过摄像头获得移动机器人周围场景的关键特征，作为推算相对位置的环境路标。该方法虽然能够达到较好的定位效果，且应用的场景范围可以比较大，但是当移动机器人处于变化的环境中时，地图信息的变化会对定位产生影响。同时摄像头需要搭载在移动机器人上，会让机器人结构外形更加复杂。且需要额外增加处理器或是占用机器人本身处理器资源。全局定位则是摄像头固定在场景中，能够获取整个场景中的视野，在不考虑遮挡的情况下，不受场景中物体变化的影响，独立于机器人，不占用移动机器人处理器资源。其中常用的方法就是目标特征匹配方式的定位方法，经典的特征检测方法有sift(scaleinvariantfeaturetransform)算法、surf(speeded-uprobustfeatures)算法、orb(orientedfastandrotatedbrief)算法。sift算法得到的特征点数较多，也具有较好的旋转不变性和尺度不变性，但是算法效率较低。surf算法相比于sift的效率有一定提升，其他性能也与sift大体上持平，但是也难以达到实时性要求。orb算法效率较前两者高，但也存在尺度不变性差的问题。

技术实现要素：

发明目的：针对移动机器人现有定位方法精度较低，orb算法尺度不变性差的特点，提出一种基于固定摄像头视觉的移动机器人定位方法。

技术方案：为实现本发明的目的，本发明所采用的技术方案是：一种基于固定摄像头视觉的移动机器人定位方法，步骤包括：

第一步，首先选择一张二维码标志作为模板图像，然后在移动机器人上粘贴该二维码标志，并将通过固定摄像头拍摄到的移动机器人图像作为场景图像。使用orb算法进行模板图像中的二维码标志和场景图像中的二维码标志的特征点提取，根据模板图像中二维码标志的特征点坐标向量和场景图像中二维码标志的特征点坐标向量间的汉明距离，来计算特征点间的相似度，通过比较特征点两两之间的相似度，为每个在模板图像中的特征点在场景图像中找到最相似的特征点；

第二步，采用半径滤波算法除去第一步中经过特征点初步匹配后的场景图像的点群中的离群值，得到集中分布在场景图像中的二维码标志上的特征点群，再将集中分布的特征点群坐标平均值作为点群中心，从而得到场景图像中的二维码标志中心的图像像素坐标，也即场景图像像素坐标系中的移动机器人的图像像素坐标，进而实现移动机器人在场景图像像素坐标系中的定位；

第三步，在定位出移动机器人在场景图像像素坐标系中的图像像素坐标后，根据摄像机成像模型，建立全局坐标转换关系，基于摄像机标定，将得到的移动机器人的图像像素坐标转换为移动机器人实际的世界坐标，从而实现移动机器人的全局视觉定位。

进一步地，在所述第一步中，使用orb算法提取出模板图像和场景图像中的特征点，再根据汉明距离进行特征点匹配找到距离最小的特征点匹配对集合，也即为每个在模板图像中的特征点在场景图像中找到与之最相似的特征点，其中汉明距离定义为两个等长字符串对应位置的不同字符的个数，特征向量间的这两种距离越小，则相似度越高，设模板图像和场景图像中的两个特征点对应的图像像素点坐标分别为和则这两个特征点对应的像素坐标的汉明距离为：

进一步地，在所述第二步中，在场景图像中，然后选择合适的滤波半径，对于场景图像中的某一个特征点，如果其他特征点与该特征点之间的二维坐标距离小于或等于滤波半径，则称其他特征点为该特征点的近邻，遍历场景图像中的所有特征点，对每个特征点的近邻数量进行统计，设定合适的阈值数量，近邻数量达到阈值数量的特征点进行保留，没有达到的则剔除。再求点群的平均坐标作为点群中心，从而得到场景图像像素坐标系中的二维码标志中心的图像像素坐标，也即场景图像像素坐标系中的移动机器人的图像像素坐标，进而实现移动机器人在场景图像像素坐标系中的定位。

进一步地：在所述第三步中，在定位出移动机器人在场景图像中的图像像素坐标后，根据摄像机成像模型，建立坐标转换关系，建立经典的全局坐标转换模型，这个全局坐标转换模型中的关键参数就是相机参数，通常这些参数要通过实验的方式进行计算得到，这个过程就是摄像机标定；整个全局坐标转换模型涉及四个坐标系，分别是图像像素坐标系、图像物理坐标系、相机坐标系、世界坐标系。将得到的移动机器人在图像像素坐标系里的图像像素坐标转换为移动机器人在世界坐标系里的实际的世界坐标，从而实现移动机器人的全局视觉定位。

(1)图像物理坐标系到图像像素坐标系的转换

如图2，图像像素坐标系是一个二维直角坐标系，反映了相机芯片中像素的排列情况。其原点o’位于图像的左上角，u坐标轴与v坐标轴分别与图像的两条边重合。像素坐标为离散值，以像素为单位，图像物理坐标系在理想状态下，图像中心o为坐标系原点，x坐标轴与y坐标轴分别与u坐标轴与v坐标轴平行。两个坐标系是平移量为(u0,v0)的平移关系。

设相机感光元件中单个像素点的物理尺寸为dx×dy，(x,y)为移动机器人在图像物理坐标系里的图像物理坐标，则第二步中得到的移动机器人在图像像素坐标系里的图像像素坐标(u,v)满足：

将上式写成齐次形式，有：

上式完成了图像物理坐标系到图像像素坐标系的转换；

(2)相机坐标系到图像物理坐标系的转换

相机坐标系是一个三维直角坐标系，其原点oc位于镜头的光心处，xc轴与yc轴分别与图像平面的两边平行，zc轴为镜头光轴，与像平面垂直。

如图3所示，设移动机器人在相机坐标系下坐标为p(xc,yc,zc)，点p通过投影中心的光线投影到像平面上，在图像物理坐标平面上的投影点p’(x,y)即为移动机器人在图像物理坐标系下的坐标,投影点p’在相机坐标系下的坐标为(x,y,f)，其中f为摄像头光心oc到图像物理坐标系原点o’的垂直距离，根据相似三角形原理，有：

同样地，将上式写成齐次形式：

上式完成了从相机坐标系到图像物理坐标系的转换；

(3)世界坐标系到相机坐标系的转换

世界坐标系描述的是物体在实际空间中的位置，相机坐标系可以通过对世界坐标系做旋转操作r3×3和平移操作t3×1得到，设移动机器人在世界坐标系下的世界坐标为(xw,yw,zw)，则转换关系可以表示为：

上式完成了从世界坐标系到相机坐标系的转换；

(4)世界坐标系到图像像素坐标系的转换

综合(1)(2)(3)中的公式，可得世界坐标系到像素坐标系之间的转换关系，有：

根据上式，令：

k1表达式中的fx、fy为摄像头在x轴和y轴方向上的归一化焦距，单位为像素；

同样地，令：

k2＝[r3×3t3×1]

其中，k1被称为相机的内参矩阵，k2被称为相机的外参矩阵，令k＝k1k2，k则被称为投影矩阵。

世界坐标系与像素坐标系之间的转换关系公式可以写成：

由此，关键问题就是求取相机的内参矩阵和外参矩阵。这里采用张正友标定法，采集20-30幅图像进行标定得到。于是就得到了移动机器人在图像像素坐标系下的图像像素坐标与移动机器人在世界坐标系下的世界坐标间的转换关系。所以在得到移动机器人在图像像素坐标系下的图像像素坐标后，经过上述转换，即可得到移动机器人在世界坐标系下的世界坐标，进而完成移动机器人的全局视觉定位。

有益效果：与现有技术相比，本发明的技术方案具有以下有益技术效果：

(1)使用二维码作为标志物，既能够作为定位标志物，后续又能够用于存储其他信息。

(2)利用orb算法结合半径滤波方法，直接根据特征匹配集中分布的特点进行定位，不受尺度不变性差所带来的影响，有效提高了检测和定位速率。

附图说明

图1为半径滤波算法示意图；

图2为图像像素坐标系与图像物理坐标系示意图；

图3为全局视觉模型示意图。

具体实施方式

下面结合附图对本发明作更进一步的说明。

第一步，首先选择一张二维码标志作为模板图像，然后在移动机器人上粘贴该二维码标志，并将通过固定摄像头拍摄到的移动机器人图像作为场景图像。使用orb算法进行模板图像中的二维码标志和场景图像中的二维码标志的特征点提取，根据模板图像中二维码标志的特征点坐标向量和场景图像中二维码标志的特征点坐标向量间的汉明距离，来计算特征点间的相似度，通过比较特征点两两之间的相似度，为每个在模板图像中的特征点在场景图像中找到最相似的特征点；

第二步，采用半径滤波算法除去第一步中经过特征点初步匹配后的场景图像的点群中的离群值，得到集中分布在场景图像中的二维码标志上的特征点群，再将集中分布的特征点群坐标平均值作为点群中心，从而得到场景图像中的二维码标志中心的图像像素坐标，也即场景图像像素坐标系中的移动机器人的图像像素坐标，进而实现移动机器人在场景图像像素坐标系中的定位；

第三步，在定位出移动机器人在场景图像像素坐标系中的图像像素坐标后，根据摄像机成像模型，建立全局坐标转换关系，基于摄像机标定，将得到的移动机器人的图像像素坐标转换为移动机器人实际的世界坐标，从而实现移动机器人的全局视觉定位。

为验证本发明所提出的基于固定摄像头视觉的移动机器人定位方法在实际工程应用中的有效性，基于自行搭建的实验平台对定位效果从轨迹效果和静态定位精度两个方面进行实验。

在轨迹效果实验中，控制移动机器人走大致的矩形路线并使用该方法进行定位，在图像中绘制出行进轨迹，从结果来看，轨迹的连续性较好，这也反映出方法的实时性较好。

在定位精度实验中，在场景范围的两条对角线上等距离间隔地取9个点，将移动机器人移动到这九个点处进行定位，统计算法输出坐标与全局实际坐标数据，绘制在坐标图上。移动机器人坐标定义为其上搭载的二维码标志物的中心坐标，误差定义为移动机器人实际测量坐标与算法定位坐标的两点间欧氏距离。实验得到的平均静态定位误差为1.28cm，精度较高。

以上实施只为阐释本发明的技术思想，不可因此限定本发明的保护范围。值得注意的是，在本发明的技术思想上对技术方案做出的任何改进，均属于本发明的保护范围。