随着电商的崛起和“工业4.0”的提出, 基于AGV(Automated Guided Vehicle, 自动巡航小车)的智能物流系统相较于传统的人工分拣具有精度高, 安全可靠, 运行成本低廉的优势. 并且AGV小车可以24小时无间断地工作, 大大提高了仓库的分拣效率^[1].

AGV按照驱动方式大概可以分为三种: 单舵轮, 差动轮, 双舵轮. 其中双舵轮式AGV具有非常高的灵活性, 可做出原地旋转, 平移, 侧移等全向的移动. 底盘模型如图1所示.

双舵轮的AGV小车具有良好的发展前景, 是目前研究的热点, 但是因为使用两个舵轮, 其运动模型复杂且导航定位精度不高. 双舵轮AGV小车还不能大规模、多领域地投入使用.

导航定位是AGV小车控制的核心部分^[2], 能实时地上传AGV小车精确的坐标位置, 是后续路径规划算法和避障算法的保障, 目前的导航方法主要有依赖环境的: 光学导航、磁带导航、二维码标志等, 和依赖于AGV自身传感器的: 激光导航、视觉导航等. 基于自身传感器的AGV小车使用范围更加广泛, 成为目前研究的热点. 其中激光导航技术相对成熟但是精密的激光雷达成本非常高昂商业运用价值低^[3]. 随着技术的发展, 视觉在各个领域的应用逐渐增多, 比如: 三维重建^[4]、SLAM^[5]等前沿技术, 而且其传感器价格低廉具有很好的前景.

基于自身传感器定位的AGV导航系统不管是视觉导航还是激光导航都有一个非常重要的功能模块——里程计. 其作用是记录小车每个时刻和前一微小时刻的运动增量, 然后将这个增量传递给后台来计算出AGV小车的运动轨迹, 得到小车的精确定位. 目前工业运用的两轮差速AGV大多采用轮式编码器来记录里程, 然后融合激光雷达进行定位. 但是双舵轮AGV小车运用编码器来记录里程会导致定位误差大, 在路面崎岖的情况下尤为明显. 本文用视觉里程计(Visual Odometry, VO)去计算里程, 避免对地盘的直接运动学建模提高双舵轮AGV小车的定位精度. 本文将详细地论述在实际工程中VO的原理和搭建过程, 选取的传感器是微软的R-GBD摄像头kinect1, 搭载在双舵轮的移动重载搬运平台上. 如图2所示为算法流程图.

1 相机模型 1.1 针孔相机模型

相机采集的数据是一帧一帧的数字图像, 它的本质是一个离散的数值矩阵, 这个矩阵记录了各个像素点灰度值(或者RGB值). 但是矩阵和图像都是二维的, 而相机周围的环境是三维的. 因此必须建立相机模型, 找到三维空间点和像素坐标点的对应关系. 如图3所示是针孔相机模型.

在针孔相机模型中存在着4个坐标系, 一个是世界坐标系, 另一个是以相机的光心O为原点的相机坐标系, 还有一个是以相机内部成像平面中心为原点的物理成像平面. 最后一个是像素坐标系, 像素和物理成像平面在一个平面上, 不过其原点在平面的左上角并且度量单位为像素.

现实世界中一个空间点P在如图所示的位置, 其通过相机的光心O投影在相机的物理成像平面上的P’点处, 于是得到了两个相似三角形, 可以得到以下关系:

$\frac{Z}{f} = - \frac{X}{{X'}} = - \frac{Y}{{Y'}}$

(1)

由上式可以看出成的是倒立的像, 相机会将图片自动翻转, 因此上式变为:

$\frac{Z}{f} = \frac{X}{{X'}} = \frac{Y}{{Y'}}$

(2)

这个公式反映了三维空间点和其在相机投影平面上的位置关系. 将点在相机坐标系(注意是相机坐标系, 和世界坐标系相差一个平移和旋转)中的坐标映射到物理成像平面, 但是相机操作的是像素, 因此需要将物理成像平面的坐标再转换成像素坐标(相差一个平移和缩放), 最后得到如下的关系:

$Z\left( {\begin{array}{*{20}{c}} u \\ v \\ 1 \end{array}} \right) = \left( {\begin{array}{*{20}{c}} {{f_x}}&0&{{c_x}} \\ 0&{{f_y}}&{{c_y}} \\ 0&0&1 \end{array}} \right)\left( {\begin{array}{*{20}{c}} X \\ Y \\ Z \end{array}} \right) \triangleq KP$

(3)

其中, (X,Y,Z)表示空间点在相机坐标系下的三维坐标, (u,v,1)表示投影点在像素坐标系下的齐次坐标. 参数K表示的是空间点和相机中投影点的对应关系, 这个参数在相机出厂之后是固定的, 称之为相机的内参(可由相机标定得到). 由此建立起3D-2D的对应关系.

在实际工程运用中空间点的坐标往往是世界坐标P_w, 和相机坐标系下的坐标P相差一个旋转R和一个平移t. 因此有如下等式:

$P = R{P_w} + t$

(4)

最后结合上面两式并化为齐次坐标:

${P_{uv}} = KT{P_w}$

(5)

1.2 相机标定

虽然通过相机模型可解出空间点和像素点的对应关系, 但是摄像头是光学原件, 存在着透视失真如: 径向畸变、切向畸变. 畸变会扭曲图像(比如现实中的一条直线因为畸变的原因在相机图像中变成了一条曲线)导致最后实验的结果产生很大的误差^[6].

对相机进行标定, 修正相机的参数是计算精确里程前必须要做的一步, 将像素坐标转换为极坐标并且引入5个畸变纠正参数, 可以得到如下关系.

径向畸变:

${x_{dis}} = x(1 + {k_1}{r^2} + {k_2}{r^4} + {k_3}{r^6})$

(6)

${y_{dis}} = y(1 + {k_1}{r^2} + {k_2}{r^4} + {k_3}{r^6})$

(7)

切向畸变:

${x_{dis}} = x + 2{p_1}xy + {p_2}({r^2} + 2{x^2})$

(8)

${y_{dis}} = y + 2{p_2}xy + {p_1}({r^2} + 2{y^2})$

(9)

联立上述方程可以解出这5个系数, 进而纠正相机的畸变找到空间点和图像投影点正确的对应关系. 本文采用OpenCV结合棋盘格对kinect1进行标定, 结果如图4所示.

通过棋盘格标定精确地得到了相机的内参数和5个纠正系数. 标定后的相机修正了畸变带来的误差, 为后面视觉里程计的搭建提供了条件.

2 特征提取和匹配 2.1 特征提取

相机模型可以得出三维空间点和图像投影点之间的对应关系, 但是一副图像中包含了大量的像素点, 而计算机的计算能力有限, 不足以对每个像素点都进行计算. 因此需要找到一些比较特殊的像素点代表这幅图像, 只需对这些特殊的点进行处理, 并计算对应空间点的相对运动^[7]. 这个过程叫做特征提取, 这些特殊的点叫做“特征点”, 大多数特征点都由两部分构成: 关键点和描述子.

常见的特征提取算子有很多, 比如经典的Harris角点检测算子、SIFT算子、SURF算子、FAST算子, 和根据FAST改进的ORB算子等等^[8]. 每种算子都有自己适合的运用场景, 如SIFT算子具有旋转不变性, 尺度不变性, 对光照和噪声不敏感等优点, 适合复杂的环境^[9]. 但是其对计算机性能要求高, 耗时长, 不适合本文的运用场景. 结合本文实际的运用场合选取ORB算子来进行特征提取, 其最大的特点是提取速度非常快, ORB算子由Oriented FAST关键点和BRIEF描述子构成. 如图5所示为Oriented FAST关键点的原理图:

Oriented FAST关键点是基于灰度检测的, 如果一个像素点与其周围的绝大多数的像素点的灰度值都差距较大, 则将这个点定义为关键点, 认为这个点是这幅图像的一个特征. 其具体的算法实现步骤如下:

(1)选取一个点P, 其灰度值为I_P, 并以P为圆形画一个半径为3的圆.

(2)设定一个阈值T(一般设为I_P的20%), 然后依次比较圆上16个点的灰度值和I_P的大小, 如果有连续12个点的灰度值大于I_P+T或者小于I_P−T, 则将P记为关键点.

(3)对每一个像素都进行以上两步操作, 得出所有的关键点.

(4)对得出的所有关键点进行非极大值抑制, 防止关键点扎堆.

本文通过OpenCV实现上述算法, 并验证了算法的可行性, 如图6所示, 对于kinect1采集的图像, 此算法能准确快速地提取出Oriented FAST关键点.

2.2 特征匹配

Oriented FAST可以找到每个关键点在图像中的位置, 视觉里程计需要通过两幅图像计算相机的运动增量. 因此要将两幅图像中的相同的关键点进行匹配, 需要给关键点加上一个独一无二的id: 描述子. 描述子描述了每个特征点和其周围像素点的一种关系, 因此不同特征点的描述子是不一样的. 而且在不同的图像中, 相同特征点的描述子差距不大, 计算机可以通过描述子对相同的特征点进行匹配. BRIEF描述子的基本原理如图7所示.

以关键点P为中心选取一个固定大小的正方形区域, 然后根据一定的概率分布随机地选取两个点A和B组成一个点对, 比较这两个点灰度值的大小, 如果A大于B则输出1, 反之输出0. 按照此方法重复地选取n个点对, 将最后得到的结果记录在一个相应大小的数组中(采用这样类似于二进制编码的描述方式, 计算机读取描述子的速度非常快). 然后重复以上操作对每一个关键点都求其描述子.

上述方法得到的特征点对旋转, 尺度这两个因素敏感, 当场景发生改变时容易丢失. 通过搭建图像金字塔来改善尺度不变性, 通过求图像的质心来解决旋转不变性. 最后通过计算汉明距离结合快速近似最近邻(FLANN)算法对两幅有微小旋转和平移的图像进行特征匹配. 以上算法全在OpenCV下实现, 然后在kinect1上去检测算法的效果, 结果如图8所示.

ORB算子能有效地提取两幅图像中的特征点, 并将相同的特征点进行特征匹配.

2.3 视觉里程计

通过前文的步骤, 可以准确快速地提取相机图像中的特征点, 并对相邻两幅图像中的特征点进行匹配, 然后找到与之对应的空间点坐标. 最后, 可以通过这些空间点的几何关系去求解出相机在这两幅图片中的运动增量. 本文采用ICP(Iterative Closest Point, 迭代最近点)的方法来求解这个运动R和t^[10,11].

选取一对已经经过特征匹配的空间点:

$P = ({p_1}, \cdots ,{p_n}),P = ({p_1}', \cdots ,p_n')$

定义一个误差项e_i, 可以建立一个最小二乘的问题:

${p_i} = R{p_i}' + t$

(10)

${e_i} = {p_i} - (R{p_i}' + t)$

(11)

对于这个最小二乘的问题, 求解R, t使误差平方和最小.

$\mathop {\min }\limits_{R,t} J = \frac{1}{2}\sum\limits_{i = 1}^n {\parallel ({p_i} - (R{p_i}' + t))\parallel _2^2} $

(12)

至此, 一个基于RGB-D摄像头的视觉里程计就搭建完成了. 最后在Linux+ROS平台下对上述算法进行代码的实现. 如图9是视觉里程计效果图.

左边的箭头表示相机当前的位姿, 始终指向相机坐标系的正前方, 右边的箭头实时地接收里程计返回的数据并可视化的表示出来.

3 实验结果与分析

将视觉里程计装载上图10所示的双舵轮AGV小车台架上和激光雷达数据融合测试并对比定位精度.

本文采用重复定位的方式来测试定位精度, 在系统运行稳定后, 分别重复20次定位并绘制出散点图, 和基校点比较得出定位精度. 图11为路面平坦的情况下编码器做里程和视觉做里程的定位精度对比图, 编码器的定位精度为19.46 mm, 而视觉里程计的定位精度为16.7 mm, 可以看出视觉里程计的定位精度更高.

但是实际工厂中的环境往往更加复杂, AGV小车一般会在路面崎岖的环境下工作, 针对崎岖的路面对比两种方案的定位精度, 结果如图12所示, 可以看出在崎岖的路面中轮子颠簸和打滑加上运动模型复杂, 编码器不能很好的记录里程信息. 定位误差增大为31.46 mm. 视觉里程计避开了直接求解运动模型而且路面对其影响较小, 定位效果有了明显的改善, 重复定位精度为19.65 mm, 相对于平坦路面的情况下定位精度变化不大.

最后为了对比系统的鲁棒性, 在崎岖的路面上分别应用两种方案连续运行8小时并绘制时间轴. 可以看出视觉做里程的情况下, 系统的鲁棒性相对稳定, 在连续运行8小时的情况下定位精度几乎保持在一个稳定的范围类, 而编码器做里程其对路面非常敏感, 定位精度不高累计误差大.

通过图13、图14对比可以看出在双舵轮平台上, 运用视觉做里程定位精度较高, 在不同路面环境下都能保持在一个稳定的范围内, 同时鲁棒性较好, 和现在传统的编码器做里程相比, 避免了对底盘的直接运动学建模并且价格低廉具有很好的发展潜力.

4 结论

针对双舵轮AGV在地面崎岖不平的情况下编码器失效的问题. 本文提出一种使用价格低廉的RGB-D相机做视觉里程计的方案, 避免对双舵轮AGV的直接运动学建模来解决编码器航迹推算累积误差过大的问题. 并在不同路面条件下对视觉里程计和编码器里程计两种方案的定位精度做了对比, 视觉里程计的定位精度更高特别是在崎岖路面的条件下, 而且系统的鲁棒性相对稳定, 在连续运行8小时的情况下定位精度保持在一个稳定的范围类. 而编码器做里程对路面非常敏感, 定位精度不高累计误差大. 结果表明使用RGB-D相机做视觉里程计运用于双舵轮AGV具有很好的实际运用价值.