基于双目视觉的农业机器人运动定位与避障研究

马耀锋,李红丽

(郑州工程技术学院,郑州 450044)

0 引言

随着社会经济和无线通信、嵌入式、机械等技术的快速发展,研究人员研究出了各类机器人替代人类完成繁琐和危险的工作,不仅降低了人类作业风险,还大大提高了工作效率。农业机器人是一种集外界环境感知、定位和行走规划于一体的综合系统,被广泛应用于各种农业生产中。农业机器人的工作环境复杂、多变,因此需要其具备较高的环境识别、定位和避障的能力。农业机器人定位和避障往往是研究中的重难点问题,本文提出了一种基于双目视觉的农业机器人运动定位和避障系统,可通过双目视觉采集农业机器人周边的环境信息,实现农业机器人运动定位和避障。

1 双目视觉定位原理

1.1 双目系统定位原理

双目视觉系统根据人眼立体成像原理,利用两个摄像机从不同的角度形成两幅图像,而同一点在两幅图像中会形成非常明显的视觉差异,根据这些差异,利用三角测量法,可以计算出目标点的三维立体信息。双目视觉系统成像示意图如图1所示。

图1 双目视觉系统成像示意图

从图1可以看出:两个摄像机距离为B,水平放置,相机焦距都为f,空间上P点在左右相机成像分别为P1(Xleft,Yleft)和P2(Xright,Yright)。由于两个相机水平放置,目标物体在两幅图像中的Y坐标相同,因此只需要求出X坐标,便可以得到P点在空间上的三维坐标。双目系统视差测量示意如图2所示。

图2 双目系统视差测量示意图

图2中,CL和CR分别为两个摄像机的光心,相机焦距为f,P点在左右相机成像分别为PL(xL,yL)和PR(xR,yR),ALPL=XL,ARPR=XR,PRB=a,由几何中三角相似定理可知

(1)

(2)

联合式(1)和式(2)可得

(3)

另外,可以得出d的值,即

(4)

双目视觉系统三维成像坐标计算如图3所示。

图3 双目视觉系统三维成像坐标计算示意图

假设P(X,Y,Z),在左右图像平面坐标中成像分别为(xL,y)和(xR,y),利用前面介绍的几何三角相似定理,可以计算出P在空间坐标系中的坐标,即

(5)

(6)

1.2 双目视觉系统标定

本文研究的农业机器人将双目摄像机安装在车身本体,摄像机跟随着机器人一起运动,可以较好地捕捉到工作场地周边环境状况,实现机器人的定位和避障。农业机器人结构简单示意图如图4所示。

图4 农业机器人结构简单示意图

双目视觉系统标定是实现目标物体三维坐标计算的前提,其准确率直接影响立体视觉精度。本文研究的双目视觉系统两个摄像机位置和方向跟随着采摘机器人变化,视觉参数相对稳定。根据前文双目系统测距原理介绍及图2,空间P(Xw,Yw,Zw)点在两幅图像中的坐标分别为P1(u1,v1)和P2(u1,v1),可以得到左右相机获取P点图像的模型为

(7)

(8)

(9)

(10)

其中,旋转和平移矩阵分别为R和T,其分解后为

(11)

式(11)给出了两个相机在方位上的关系,可以参考棋盘标定的方法,建立相机内外参数的方程,从而完成相机的标定。

2 农业机器人避障

2.1 障碍物特征提取

在对目标障碍物提供特征时,农业机器人视觉边界采用直线模型,主要是采用Hough变换进行特征提取。Hough变换采用以下两种方法实现障碍物的特征提取:

1)角度约束关系。Hough变换会根据障碍物特征分布情况对其进行检测,该方法在判定障碍物特征时会对其最大特征进行查询,其一般要求临近的边界满足以下角度约束关系,即

(12)

其中,σleft和σright为障碍物特征左右界限角度值,使用该式可以除去没有作用的特征。

2)障碍物边界特征提取。本文采用消失点分析hough算法检测到的结果,从而避免Hough算法的“虚峰”和“漏检”状况的出现,消失点指检测到障碍物特征两侧边界相交处。

障碍物边界特征提取的主要步骤为:

1)结合角度约束表达式,利用Hough变换在图像左右边界获取P组的边界线;

2)获得P×P个边界线交点;

3)根据交点位置,计算出障碍物边界线,防止“虚峰”和“漏检”状况的出现,提交障碍物计算精度。障碍物边界特征提取简图如图5所示。

图5 障碍物边界特征提取简图

2.2 求解避障最优路径

根据Hough算法提取到障碍物特征后,可以引入约束条件,融入A*(A-Star)算法可以让农业机器人在避障过程中获取最优路径。A*算法是结合Dijkstra算法和估计函数,根据计算出的估计值确定最优路径预备节点,可提高求解效率。算法的估计函数为

f(p)=g(p)+h(p)

(13)

其中,g(p)为农业机器人出发点到p点的代价,h(p)为当前预备节点到期望节点的代价,A*算法根据极小节点作为最优路径节点。

A*算法求解避障最优路径的主要流程为:

1)构建OPEN和CLOSE的节点表单。其中,OPEN为暂未进行代价估计的节点,CLOSE为已经完成代价估计的节点。

2)采用广度搜索原理,设置农业机器人起点,并保存至OPEN节点。

3)将OPEN节点中的第一个节点,保存到CLOSE节点表单中,根据估计值,查询和确定所有的预备节点,保存于OPEN节点表单,并利用估计函数对所有节点进行代价计算和排序。

4)重复步骤3),直到农业机器人达到目标地点。

3 农业机器人避障的实现

3.1 计算目标避障角度

根据双目视觉系统获取障碍物的位置后,便可以调整农业机器人的移动方向,从而实现避障。农业机器人避障模型如图6所示。

图6 农业机器人避障模型图

图6中,XOY为大地坐标系,P(x,y)为目标路径上一点,R为轨道半径,L为农业机器人前轮距离。避障模型目标角度会根据运动学方程和农业机器人的几何关系确定,重点是确定在当前路径需要前进多少。由此得到农业机器人避障模型表达式,即

(14)

农业机器人两个运动轮子的运动学表达式为

(15)

因此,在避障过程中,农业机器人的目标角度为

(16)

其中,δe为农业机器人目标角度,αc为角度偏差值,dc为横向位置偏差。确定这些参数以后,便可以计算出农业机器人方向盘转动角度,从而可以实现对农业机器人转向的控制。

3.2 农业机器人车轮控制

农业机器人车轮控制采用PID闭环控制,控制器根据目标路径、避障角度和采样周期T计算出偏差。离散化PID控制表达式为

(17)

其中,u(k)为农业机器人控制器输出,Kp为比率系数,TD为微分时间常数,TI积分时间常数。PID控制增量为

Δu(k)=u(k)-u(k-1)

(18)

农业机器人车轮PID控制程序如图7所示。

图7 农业机器人车轮PID控制程序

由图7可以看出:农业机器人车轮PID控制程序主要包括PID增量计算、PID输出计算和控制判断。

4 实验结果与分析

为了验证农业机器人运动定位和避障的可行性,采用MatLab仿真软件进行了仿真与分析。农业机器人在实际作业过程中,对自身定位和障碍物检测是实时的,所以本文以前面5m范围的障碍物对避障方位。试验仿真结果如图8所示。

图8 Matlab仿真结果图

由图8可以看出:农业机器人的上方障碍物较多,因此机器人向右转弯,计算到障碍物外切圆的圆心位置后,农业机器人以起点、障碍物外切圆和终点的连接线作为农业机器人的避障路线。实验结果表明:农业机器人从起点成功到达终点,算法路径为最优避障路径,证明了系统的准确性和可行性。

5结论

提出了一种基于双目视觉的农业机器人运动定位和避障系统。该系统通过双目视觉采集农业机器人周边的环境信息,运用Hough变换提取障碍物特征信息,结合A*算法进行避障最优路径的选择,并采用PID闭环控制,实现对机器人方向盘的控制,实现了农业机器人运动定位和避障。实验结果表明:农业机器人从起点成功到达终点,算法路径为最优避障路径,证明了系统的准确性和可行性。