大视场室内移动机器人高精度动态定位方法

田明锐 杨 皓 胡永彪

1.长安大学道路施工技术与装备教育部重点实验室，西安，7100642.广西柳工机械股份有限公司，柳州，545006

0 引言

室内移动机器人是运行在室内环境的自主移动机器人，如工业生产机器人、服务机器人、医疗机器人和仓储机器人等[1-3]。目前，关于室内移动机器人领域的研究主要集中在室内定位、自主导航、运动控制和协同作业几个方面，其中室内定位技术是移动机器人完成其他任务的基础与前提。相对于室外定位技术大都依赖卫星定位的情况，室内定位技术受到室内环境复杂性和动态性的影响，一直是机器人定位技术研究领域的难点与热点。

借助于不同的传感器技术，室内定位方法种类繁多，包括红外定位[4]、超声波定位[5]、无线网络定位[6]、蓝牙定位[7]、激光雷达定位[8]和视觉定位[9]等。机器视觉传感器对复杂环境的强感知性和图像处理技术获得的高精度定位使视觉定位方法受到了广泛的关注[10]。

目前，基于视觉的室内定位方法主要分为相对定位方法和绝对定位方法两类。

相对定位方法利用固定在移动机器人上的相机传感器采集周围环境图像并处理，通过检测环境中的人工标识来进行机器人的定位。谢家浩等[11]研究了一种基于视觉同步定位与建图(SLAM)的相对定位方法，采用ORB(oriented FAST and rotated BRIEF)算法对特征点进行提取和描述，实现了对目标物的实时定位。杨鹿情[12]设计了一种将颜色特征与形状特征相结合的人工信标，利用串联式组合误匹配剔除算法实现室内移动机器人的定位，减小了单目视觉SLAM系统前端存在的较多误匹配对位姿估计的影响。相对定位方法定位迅速，精度较高，但需安装大量机载设备，成本较高，同时该方法算法复杂，易受环境不确定因素的影响。

相比之下，绝对定位方法通过置于环境中的相机传感器对目标物及周围环境进行图像采集，通过图像处理进行移动机器人的定位，可有效降低机器人机载设备的复杂性，同时具有成本低、适应性强和算法简单等优点。黎安庆等[13]提出了一种基于分布式视觉技术的机器人绝对定位方法，在3 m×5 m的区域内该方法的平均定位精度为8.1 cm，方位角平均精度为8.05°。林剑冰等[14]采用定距双球作为标志物，设计了一种室内自动运输车的绝对定位系统，可同时实现其位置与方向的定位，相对误差一般情况下保持在1.5%以内，在距离系统3 m范围的短距离定位的绝对误差小于1 cm。李鹏等[15]提出了一种基于标定板绝对定位方案，利用高斯混合模型背景减除法检测机器人运动区域，缩小了标定板角点的提取范围，提高了定位的实时性，当机器人速度为0.3 m/s时，平均定位精度为6.86 cm。

上述绝对定位方法研究为保证定位精度，视场范围设置相对较小，在机器人低速移动时可满足较高的实时定位要求。目前，关于大视场范围与高速移动目标的绝对定位方法研究较少。本文对大视场范围移动机器人的高速动态定位方法进行了研究，将相机多参量非线性标定方法与基于光信标高速定位算法相结合，提高高速移动机器人的定位精度。

1 基于多相机的大视场传感器系统

1.1 定位系统硬件配置与软件构架

如图1所示，定位系统硬件由一组具有外部同步触发器的相机、一台工控机和LED光信标组成，定位面积即相机总视场可达43 m2。系统选用MER-131-75GX-P工业相机进行图像采集，相机内部采用CMOS感光芯片，采集图像分辨率为1280×1024。每台相机通过GigE数据传输线连接到处理节点，允许以每秒75帧的速度进行图像采集。处理节点为一个通用的多核PC平台，具有控制和处理来自多个相机信息的能力。实验中使用的所有机器人平台采用Traxxas-83024-4轮式机器人底盘，采用全轮驱动、前轮转向的方式运动，最高速度可达48.3 km/h。机器人上部安装220×20 mm的LED光信标，其方向与机器人几何中心线平行，光信标的几何中心与机器人的质心重合。

图1 定位系统方案示意图Fig.1 Schematic diagram of positioning system hardware

定位系统软件使用C/C++语言实现程序框架和定位功能算法，利用MATLAB相机标定工具完成视觉系统标定，其构架如图2所示。该软件系统主要由相机标定、移动机器人定位、动态误差补偿三方面构成。

图2 定位系统软件构架Fig.2 Positioning system software architecture

1.2 大视场多相机非线性标定

1.2.1多参量非线性标定方法

相机标定旨在获得图像坐标(u,v)与地面坐标(xw,yw)的映射关系，是后续定位系统实现的前提。传统相机的线性标定方法不考虑图像的径向、切向的畸变，标定后的精度较低，且对噪声敏感。相机的非线性标定方法引入了相机的畸变参数，成像模型更加精确，可有效提高标定精度。

传统的相机非线性标定[16]借助一组靶标数据对相机参数进行标定，当视场范围增大时，该标定方法精度较低。为提高大视场环境下的相机标定精度，排除偶然误差的影响，本文选用多组靶标对相机进行非线性标定，靶标组数的选取与相机视场大小相关。

第k(k=1,2,…,n)组靶标中第i个靶标点的图像坐标与地面坐标满足如下关系：

(1)

式中，m′k11、m′k12、m′k14、m′k21、m′k22、m′k24、m′k31、m′k32为标定参量。

式(1)的矩阵方程形式为

Akm′k=Bk

(2)

Ak=

m′k=

利用k个单应矩阵计算平均单应矩阵：

(3)

将该平均单应矩阵作为最终将图像坐标映射至地面坐标的单应矩阵。

根据式(1)和式(3)可计算相机图像坐标(ui,vi)映射至地面的坐标(x′wi,y′wi)。相机的标定误差即定位系统的静态误差在地面坐标系xw、yw方向上分别为Ex=|xwi-x′wi|、Ey=|ywi-y′wi|。该标定方法和仅选用一组靶标点的标定方法相比可有效提高定位精度。

非线性标定实验利用MATLAB中的Camera Calibrator相机标定工具，对相机内部参数进行标定，结果如表1所示。

表1 相机内部参数

根据相机内参数据，对相机所采集的图像进行去畸变校正。由于单个相机视场约为25 m2，故在去畸变后的图像中共选取4组靶标点作为单应矩阵的标定点，即k=4。根据式(1)～式(3)计算左右两相机的平均单应矩阵：

(4)

(5)

根据式(1)、式(4)、式(5)计算地面所有靶标点的世界坐标，并利用MATLAB中Curve Fitting工具拟合出所有像素点静态误差，如图3所示，定位系统的平均静态定位精度为2.26 mm。若只选取单组靶标点，即k=1，则经计算得定位系统的平均静态定位精度为4.77 mm。基于多组靶标点的相机非线性标定方法使相机标定精度提高约52.6%。

1.2.2多相机视场融合

在大视场移动机器人定位之前，需将各相机成像平面坐标统一化，即多相机视场融合，作为后续同名目标检测的计算基础。同一平面目标点在不同成像平面上的映射关系可通过单应矩阵变换表达，因此本文基于不同视场图像坐标系的单应矩阵标定来实现多相机视场融合。

选取同时出现在相机p、相机q视场中的4个靶标点，组成两组图像坐标(upi,vpi)、(uqi,vqi)，i=1,2,3,4，代入下式：

(6)

根据1.2.1节所述方法，计算相机q映射至相机p的单应矩阵m′q-p，可得由右相机标定至左相机的单应矩阵：

(a)左相机Ex

(7)

2 动态定位系统算法设计

2.1 基于光信标的高速目标定位算法

模板匹配、光流法和帧差法等算法是基于图像的运动目标检测及定位的常用方法。模板匹配法能够适应较为复杂的背景，定位精度较高；光流法与帧差法可直接获得目标物的运动信息，易于进行实时处理。当定位目标增多，且目标物高速运动时，上述定位算法会非常复杂，图像处理时间长，造成定位系统实时性较差、动态定位误差大等问题。为解决高速多目标定位算法的上述问题，本文提出基于光信标的高速定位算法。该算法定位目标具有较高的移动速度，并涉及跨视场多目标的定位问题，算法流程图见图4。

图4 基于光信标的高速定位算法流程图Fig.4 Flow chart of high-speed positioning algorithmbased on optical beacon

使用光信标能够简化图像运动目标检测算法，有利于提高定位算法的运行速度。该定位算法设置较短的相机曝光时间，借助固定阈值方法快速提取图像中安装光信标的移动检测物体，得到无其他干扰的目标物二值图像。设原图像像素点灰度级为f(u,v)，二值化后图像像素点灰度级为g(u,v)，固定阈值为T0，则二值化计算公式为

(8)

针对二值化处理后的图像，利用连通域分析与循环遍历算法，获得连通域外接矩形的长h、宽w、质心坐标(uc,vc)和左上角点坐标(u0,v0)等参数。由于光信标的中心与机器人质心重合，连通域外接矩形的图像质心坐标(uc,vc)为机器人质心的图像坐标，故可根据式(1)、式(4)和式(5)计算机器人质心的位置坐标(xc,yc)。

移动机器人的姿态角计算示意图见图5。首先，根据外接矩形长宽比的不同(图5a为w>h，图5b为w

(9)

(a)w>h(b)h>w图5 机器人姿态角计算示意图Fig.5 Schematic diagram of robot attitudeangle calculation

式中，ucl、uc(l-1)分别为第l与第l-1时刻机器人质心的图像横坐标值。

移动机器人的位姿信息包含其质心坐标与姿态角，向量形式表示为(xc,yc,θ)T。

2.2 跨视场同名目标检测算法

考虑相机布置中存在一定的视场重合区域，当物体位于重合区域时将同时出现在两个相机的图像中，如何区分不同视场中的同名目标物是多相机定位方法中的一个关键。在目标定位计算前，可对相邻相机视场图像中的同名目标物进行判别与剔除，避免同名目标的重复定位计算。

跨视场同名目标检测算法将同一时刻相邻相机采集的图像进行处理，分别获得左相机、右相机图像中所有连通域目标的质心参数(uLcj,vLcj)和(uRci,vRci)，结合1.2.2节中得到的单应矩阵，将右相机图像中所有质心点(uRci,vRci)映射至左相机图像中，得到(u′Rci、v′Rci)。计算映射后各质心点坐标(u′Rci、v′Rci)与左相机图像中原质心点坐标(uLcj,vLcj)间的距离：

(10)

将其与距离阈值Te比较，进而判定右相机中对应目标物是否为同名目标物，并将其排除，Te的选取与相机参数、相机的布置以及移动机器人几何尺寸参数有关，可通过实验方法确定。设命题Q(i)为右相机中第i个目标物为同名目标物，当Q(i)=TRUE时该目标为同名目标物，当Q(i)=FALSE时该目标为非同名目标物，其判别公式如下：

(11)

该算法通过比对左右相机中每个连通域目标质心坐标，利用式(11)实现同名目标的判别和剔除，其流程如图6所示。

图6 同名目标检测算法流程图Fig.6 Flow chart of same-named targetdetection algorithm

2.3 动态定位误差补偿算法

尽管上述高速定位算法程序运行周期较短，但系统仍会产生一定的时延，尤其目标物处于高速移动时，系统输出的位姿信息滞后较为严重，即定位系统存在一定的动态误差，因此，需进行动态定位误差的补偿以提高系统定位精度。

由于程序运行周期较短，故假设机器人行驶速度v在同一运行周期内保持不变。选取机器人质心(x,y)为第一特征点，建立该特征点的运动学方程[17]：

(12)

(13)

式中，下标m表示时刻；T为程序运行周期；vx、vy为第一特征点在图像空间中x、y方向上的速度。

同理，选取机器人姿态角θ为第二特征点，建立该特征点的运动学方程：

θm+1=θm+vθT

(14)

vθ=(θm-θm-1)/T

(15)

式中，vθ为第二特征点的变化率，即角速度。

动态定位误差补偿算法基于上述运动学模型，在C/C++语言编程环境中开发，分析计算机器人的移动轨迹、速度和方向等信息，预测定位系统存在时延时机器人的真实位姿，完成误差补偿。该算法流程图见图7。

图7 动态定位误差补偿算法流程图Fig.7 Flow chart of dynamic positioning errorcompensation algorithm

3 定位系统实验

3.1 移动机器人位姿定位实验

根据2.1节提出的定位算法，以左相机为例，设置合适的曝光参数，使图像能够清晰完整地显示移动机器人上安装的光信标，程序运行结果如图8所示。由输出结果可知，共检测到三台移动机器人，分别为Car_0、Car_1和Car_2，它们对应的位姿向量分别为(579.28 mm,385.67 mm,36.87°)T、(583.28 mm,482.33 mm,-36.87°)、(692.84 mm,569.38 mm,124.13°)T。

图8 移动机器人位姿计算程序运行结果Fig.8 Running results of mobile robot position andattitude angle calculation program

3.2 跨视场同名目标物检测实验

首先进行距离阈值Te的设定。选取三组位于相机视场重叠区域的地面坐标点(xLcj,yLcj)、(xRci,yRci)，i,j=1,2,3(即每组包含3对坐标点)，分别获得它们位于左图、右图中的图像坐标(uLcj,vLcj)、(uRci,vRci)。同时根据1.2.2得到的单应矩阵m′R-L，将位于右图中的三组坐标点映射至左图并统计映射误差eui=|u′Rci-uLcj|、evi=|v′Rci-vLcj|，结果如表2所示。

表2 坐标点映射误差

根据表2中数据，结合移动机器人几何尺寸，将距离阈值Te设定为25 pixel。而后，根据2.2节中算法，对视场范围中图9所示的移动机器人分布进行同名目标的检测与排除，该视场范围内共有四台移动机器人，其中2号、3号为出现在视场重叠区域的同一移动机器人。

图9 两视场中移动机器人分布Fig.9 Distribution of mobile robots in twofields of view

该算法的输出结果如图10所示，成功检测出3号机器人为同名目标物并对其进行了剔除，最终获得三个有效目标，分别用Car_0、Car_1与Car_2表示。由结果可知，该算法能够准确地判断有效目标并去除重复同名目标，为定位系统检测多目标的准确性提供保障。

图10 同名目标检测程序运行结果Fig.10 Running results of same-named targetdetection algorithm

3.3 移动机器人定位动态误差补偿实验

在获得移动机器人静态位姿且进行了同名目标检测与排除的前提下，通过实验验证机器人在0.3 m/s、1.2 m/s、2.3 m/s、3.2 m/s四组不同平均速度下动态误差补偿算法的有效性。当机器人处于最高平均速度3.2m/s时，其质心坐标实际值与估计值对比图、姿态角实际值与估计值对比图见图11和图12。

图11 平均速度值为3.2 m/s时机器人质心轨迹实际值与估计值对比Fig.11 Comparison of the actual and estimated valuesof the robot’s center of mass position with the averagespeed is 3.2 m/s

图12 平均速度值为3.2 m/s时机器人姿态角实际值与估计值对比Fig.12 Comparison of the actual and estimated valuesof the robot’s attitude angle with the averagespeed is 3.2 m/s

根据不同速度下的实际值与估计值对比图得质心坐标误差补偿平均精度与姿态角误差补偿平均精度，如表3所示。

表3 动态误差补偿后定位系统精度

由表3可得，在移动机器人最高平均速度可达3.2 m/s时，系统位置平均动态定位精度为5 cm、姿态角平均动态定位精度为0.6°。

4 结论

本文针对室内高速移动机器人实时定位问题，提出了一种基于视觉的高精度绝对定位方法。该方法利用多相机视场融合，实现了43 m2的大视场绝对定位，结合同名目标物排除算法，解决了多相机视场重叠区域机器人定位问题；利用多参量非线性标定方法，提高平均静态定位精度至2.26 mm；采用光信标简化图像处理算法提高定位系统运行速度，并借助线性误差补偿算法，使机器人在平均速度为3.2 m/s时，定位系统位置平均动态定位精度达5 cm，姿态角平均动态定位精度达0.6°。实验结果证明，该定位系统可实现室内大范围下高速移动机器人的实时定位，定位系统的静态与动态定位精度满足要求。本文方法的局限性主要在于，大视场平面靶标点布置过程导致单应矩阵标定工作量较大，例如当移动机器人光信标到地面的垂直距离发生变化时，需要重新标定与地面平行的另一平面的单应矩阵，此时需要在目标平面内布置(x,y)坐标已知的若干靶标点，当精度要求较高时，靶标点布置难度和工作量较大。这也限定了各移动机器人光信标到地面的垂直距离最好相等。因此，后续工作还需研究大视场平面的快速标定方法。