基于双目视觉的主从式果园作业车辆自主跟随系统设计

毕伟平，张　欢，瞿振林，丁永前*，余洪锋，王　波

(1.南京农业大学工学院，江苏 南京 210031；2.江苏省现代设施农业技术与装备工程实验室，江苏 南京 210031)



基于双目视觉的主从式果园作业车辆自主跟随系统设计

毕伟平1,2，张欢1,2，瞿振林1,2，丁永前1,2*，余洪锋1,2，王波1,2

(1.南京农业大学工学院，江苏 南京 210031；2.江苏省现代设施农业技术与装备工程实验室，江苏 南京 210031)

摘 要：将黑白棋盘格特征板固定于引导车辆作为跟随特征，通过固定于跟随车辆的双目视觉系统，获取黑白棋盘特征板上各角点的三维信息，分析该信息，得到引导车辆相对于跟随车辆的车辆间距、横向偏移及航向偏角，利用该导航信息实现跟随车辆的自动跟随，建立了主从式果园作业车辆的跟随系统。双目视觉系统在不同光强下静态性能测试结果表明，双目系统生成导航参数偏差与日间光强无明显相关性，体现出双目系统对外界光环境变化有良好的适应性。在果园环境下，对该系统进行性能测试，结果系统导航信息生成频率为4.0 Hz，横向偏移平均偏差为3.68 cm，车辆间距平均偏差为8.24 cm，航向偏角平均偏差为2.32°，表明所构建的跟随系统能实现对引导车辆的自动实时跟随。

关 键 词：主从式果园车辆；双目视觉；自动导航

投稿网址：http://xb.ijournal.cn

目前，农业车辆自主导航研究主要集中在单个车辆在农田环境中的自动导航[1–4]，但在很多情况下农业车辆作业需要2辆及以上车辆协同操作才能完成[5]，如在果园收获过程中，农业车辆作业采用主从式跟随方式，果实采摘车(引导车)与果实运输车(跟随车)之间跟随操作[6–8]。日本京都大学农业机械化研究生院研究了水稻收割过程中的协同车辆系统[9]，采用超声波模块测量车辆相对位置，方法较为简单，但超声波模块易受周围障碍物干扰，不具有分辨干扰物与目标车辆的能力；北海道大学农业车辆系统工程研究生院研究了在农田作业的主从式机器人系统[10]，其主要研究了跟随车辆跟随算法及到达特定位置的运行算法，但其跟随距离最小为12.5 m，近距离跟随时效果未知；上海交通大学智能车实验室实现了基于GPS的城市环境多智能车跟随协作系统[11]，主要针对城市道路环境下多车运行，无主从之分，当车辆间距离较远时各自运行，距离较近时车辆相互配合实现换道超车等操作。当前主从式车辆跟随研究中，主要采用红外测距、高精度GPS及激光扫描等方式[12–14]，成本高且易受环境影响。

用机器视觉方式实现主从式车辆跟随鲜有研究，因为不同车辆其特征不同且提取算法复杂，单纯通过识别车辆特征也较难判断其实际位置。黑白棋盘格作为一种标准的视觉系统参数标定对象[15–16]，其角点提取算法成熟，易于实现。笔者改装了2辆小型电动车辆使其构成主从车辆跟随系统，采用黑白棋盘格板固定于引导车辆后方，作为其特征标识物，同时利用固定于跟随车辆前方的双目视觉系统，依据双目立体视觉原理[17–19]提取特征板各角点三维信息，分析获取2车之间的相对位姿信息，指导跟随车辆的运行。在实际果园环境中，对建立的主从车辆跟随系统进行了验证试验，现将结果报道如下。

1 跟随车辆系统的构建

主从式果园车辆如图1所示。跟随车辆系统包括双目视觉系统与车辆运动平台。双目视觉系统采用双目相机与PC连接的方式构成，车辆运动平台在改装电动车基础上通过基于CAN总线的控制节点实现与双目视觉系统通信及车辆控制。

图1　主从式果园车辆Fig. 1 Master-slave vehicles in orchard

1.1 双目视觉系统

双目摄像头采用维视数字图像技术有限公司的 MV–VS030FC型工业数字相机。采用帧曝光CCD作为传感器，图像质量较高，颜色还原性较好。CCD相机最大分辨率640×480。该相机采用 IEEE 1394 作为输出接口。在安装有 1394采集卡的 PC平台同时连接2台该工业相机，2台工业相机平行固定于相机支架上，其成像中心间距离为90 mm，构成双目视觉系统。

1.2 车辆跟随系统

车辆控制系统采用易于扩展的CAN总线结构，各CAN总线节点以STM32F102VET6为核心；主节点通过串口与PC端双目视觉导航系统传输导航信息，各个从节点通过编码器实现测量前轮转角和车辆前进速度、通过控制步进电机配合链轮传动机构带动方向盘旋转实现车辆转向、通过控制后轮差速电机实现车辆驱动和差速转弯。

1.3 车辆控制方法

果园路面状况较为复杂，因此跟随车辆在双目视觉系统获取车辆的位姿参数后，需要经过相关控制算法才能实现稳定运行。转向控制系统根据航向偏角θ和横向偏差f 与目标控制期望值之间的偏差,通过航向控制器输出控制转向电机的PWM脉冲，从而控制车辆转向轮的偏转[20]。车速控制系统根据车辆间距d与目标控制期望偏差通过速度控制器输出相关模拟电压从而控制车辆前进差速电机，实现车辆行驶速度的调整，以保持设定的车辆间距[21]。2个控制系统同时作用，实现车辆跟随操作。车辆跟踪控制系统如图2所示。

图2　跟随控制系统Fig. 2 Diagram of follow-control system

2 车辆跟随方法

2.1 车辆运动学模型

跟随车辆运动学分析如图3(左)所示，跟随车辆与引导车辆相对位置如图3(右)所示。当车辆前后跟随时，理想情况为保持设定的跟随距离的同时，2车中轴线相互重合，因此，当引导车辆运行状态变化或运行环境受到影响时，2车间距d(m)发生变化，同时产生距离为f(m)的横向偏移及角度为θ(°)的航向偏角，跟随车辆控制系统需要通过控制车辆前进速度及前轮偏转角度，使2车间距趋于目标间距、横向偏移趋于0和相对航向偏角趋向于0，从而实现跟随。图中2车轮中间浅色阴影区域为2车轮的等效模型，O为跟随车转向运行时的速度瞬心。

图3　主从式果园作业车辆跟随系统运动学分析Fig.3 Kinematical analysis of autonomous following system for master-slave vehicles in orchard

根据运动学理论可以得出以下计算关系：

式中：L为跟随车辆前后车轮轮轴间距离(m)；θ 为引导车和跟随车相对航向偏转角(°)；δ为当前跟随车辆前轮的偏转角度(rad)；vω为当前跟随车辆的转向运行时的角速度(rad/s)；v 为当前跟随车辆的前进方向速度(m/s)；u为当前跟随车辆的转向时横向速度(m/s)；d为引导车与跟随车间距(m)；f为引导车与跟随车相对横向偏移(m)。

2.2 车辆导航参数确定方法

主从车辆跟随系统的主要特点是黑白棋盘格板固定于引导车辆作为车辆特征，运用黑白棋盘格板内角点提取算法及双目视觉原理，可以得到黑白棋盘格各个内角点在双目系统坐标系下的三维信息，而双目系统固定于跟随车辆，因此其角点三维信息代表了引导车辆与跟随车辆之间的相对位姿，通过建立整个系统坐标系分析各个角点三维信息，可以得到2车间距、横向偏移及航向偏角。

如图4–a所示，建立主从系统的坐标系，以双目系统左相机图像中心O为坐标原点，横向偏移方向标记为OX，2车间距方向即深度方向标记为OZ，垂直于XOZ平面的轴记为OY，则空间点坐标可表示为(x,y,z)。

黑白棋盘格被固定在引导车车尾并垂直于地面，2车同向且无横向偏差行驶时，OZ轴与黑白棋盘格平面垂直，其交点与黑白棋盘格中心点A连线垂直于地面。引导车和跟随车的相对位置关系可以由黑白棋盘格角点的三维信息确定。如图4–b所示，记A点为黑白棋盘格的中心点，其在OX和OZ方向的坐标记为(c，d)，则c、d分别为引导车辆与跟随车辆的横向偏移与车辆间距。设黑白棋盘格角点在OX和OZ方向的三维坐标为(xi，zj)，角点总数为n，则车辆间距及横向偏移可以表示为：

图4　引导车与跟随车位置Fig. 4 The position between two vehicles

图4–b中，BC为黑白棋盘特征板载XOZ平面的投影位置，假设点B、C为黑白棋盘格上处于同一水平线的2个特征点，B、C 2点在坐标系下的横向距离为b，深度距离为a，航向偏角θ可由a、b的三角关系得到，若 B、C2点坐标分别为(x1，z1) 和(x2，z2)，则a、b 可表示为：

于是可得：

至此，2车间距、横向偏移、航向偏角3个导航参数均得到。

3 试验验证及结果分析

3.1 静态精度试验

为验证系统得到导航参数的准确性，设计了静态导航参数的生成试验。制作内角点为4×4、单个方格边长为10 cm的黑白棋盘格特征板，固定在可随垂直于地面轴360°旋转的工作台上，通过移动工作台，双目视觉系统获取导航参数。

纵向移动：以10 cm为间隔纵向移动工作台，移动范围为1.5～2.0 m。

横向移动：在1.5～2.0 m各间距点上，以10 cm为间隔横向移动工作台，移动范围为–500～500 mm。

航向旋转：在 1.5～2.0 m各间距点上，以 10°为步进，分别按顺时针和逆时针方向旋转工作台，旋转范围为–60°～60°。

在以上试验条件下，通过双目系统测量相应间距、横向偏移及航向偏角，每个位置测量持续时间为3 min。通过观察测量结果的波动情况，评价系统稳定性，通过分析双目系统与实际固定间距、横向偏移及偏角偏差，评价系统准确性。

静态精度试验结果列于表 1。结果表明，在固定测量位置时，本系统实时生成的位置信息与实际位置偏差较小，说明系统具有较高的准确性，同时系统输出参数波动率较低，说明系统具有较高的稳定性。

表1　静态试验结果Table 1 Static test results

3.2 光环境适应性试验

为验证系统的光环境适应性，白天在室外进行了不同时段自然光下的导航参数生成的静态试验。设置车辆间距为1.6 m，横向偏移为0 cm，航向偏角0°，在 08:00、12:00、17:00，分别通过双目系统测量3个导航参数，通过计算不同光照条件下双目系统获得导航参数与实际设置参数的偏差，分析双目系统对光环境的适应性。光照度装置采用希玛高精度数字光照度计AS823，测量范围为1～200 000 lx，天气晴朗，导航参数平均偏差见表2。

表2　不同光环境下的导航参数Table 2 The experimental results under different light conditions

在不同光照度下，双目视觉系统生成的车辆间距、横向偏移与航向偏角与实际设置参数的平均偏差较小，表明双目视觉系统对不同光环境有较好的适应性。

由于黑白棋盘格板制作采用普通KT板，表面为塑料覆膜，在阳光强烈及特定角度下会产生一定反光，因此棋盘格需要注意采用磨砂或纸面材料。

3.3 果园跟随导航试验

于2015年11月下旬，在南京农业大学江浦农场果园路径中进行了果园导航试验。路径长25 m，宽2.6 m，引导车采用人工控制，沿果园路径中心线运行，目标运行速度为2 km/h。跟随车辆自动跟随引导车，目标跟随距离为1.6 m，目标横向偏移0 cm，目标航向偏角为0(°)。各导航参数变化曲线如图5所示。

图5　实际试验车辆导航参数Fig. 5 Navigation parameters of the actual test

导航参数稳定性分析如表3。果园环境下实时导航运行过程中，系统输出性能稳定，偏差较小，说明在整个过程中跟随车辆能够稳定跟随引导车辆。

表3　导航参数试验结果Table 3 The test results for navigation parameters

参考文献：

[1] Reid J F，Zhang Q，Noguchi N，et al．Agricultural automatic guidance research in North America[J]. Computers and Electronics in Agriculture，2000，25 (1/2)：155–167．

[2] Keicher R，Seufer T H．Automatic guidance for agricultural vehicles in Europe[J]．Computers and Electronics in Agriculture，2000，25(1/2)：169–194．

[3] Torii T．Research in autonomous agriculture vehicles in Japan[J]．Computers and Electronics in Agriculture，2000，25(1/2)：133–153．

[4] Li M，Imou K，Wakabayayashi K，et al．Review of research on agricultural vehicle autonomous guidance[J]. Int J Agric and Biol Eng，2009，2(3)：1–16．

[5] Stavros G V．A distributed control framework for motion coordination of teams of autonomous agricultural vehicles[J]．Biosys Tems Engineering，2012，113(3)：284–297．

[6] Goragl A，Amatyas S，Karkee M．Sensors and systems for fruit detection and localization：a review[J]. Computers and Electronics in Agriculture，2015，116：8–19．

[7] Thomas R，Marco K．Robotic fruit harvesting：a stateof-the-art review[J]．Journal of Agricultural Engineering Research，1993，54(3)：265–280．

[8] Barawid O C，Akira M，Kazunobu I，et al．Development of an autonomous navigation system using a twodimensional laser scanner in an orchard application[J]. Biosystems Engineering，2007，96(2)：139–149．

[9] Iida M，Kudou M，Ono K，et al．Automatic following control for Agricutural vehicle[C]//6th International Workshop on Advanced Motion Control，Proceedings. Nagoya：AMC，2000：158–162．

[10] Noboru N，Jeff W．Development of a master-slave robot system for farm operations[J]．Computers and Electronics in Agriculture，2004，44(1)：1–19．

[11] 彭新荣．基于智能小车平台的多车协作研究[D]．上海：上海交通大学，2010．

[12] 沈文龙，薛金林，张永．农业车辆电控液压转向系统的设计[J]．湖南农业大学学报(自然科学版)，2014，40(3)：325–329．

[13] 周俊，姬长英．自主车辆导航系统中的多传感器融合技术[J]．农业机械学报，2002，33(5)：113–116．

[14] 陈无畏，施文武，王启瑞，等．新型自动引导车导航与控制系统[J]．农业机械学报，2002，33(2)：70–73．

[15] 冯焕飞．三维重建中的相机标定方法研究[D]．重庆：重庆交通大学，2013．

[16] 舒娜．摄像机标定方法的研究[D]．南京：南京理工大学，2014．

[17] Rovira M F，Reid J F，Zhang Q．Stereovision data processing with 3D density maps for agricultural vehicles[J]. Transactions of the ASABE，2006，49(4)：1213–1222．

[18] Kise M，Zhang Q，Rovira F．A stereovision-based crop row detection method for tractor-automated guidance [J]. Biosystems Engineering，2005，90(4)：357–367．

[19] Rovira M F．Applications of stereoscopic vision to agriculture[D]．Urbana：University of Illinois at Urbana-Champaign，2003．

[20] 丁永前，王致情，林相泽，等．自主跟随车辆航向控制系统[J]．农业机械学报，2015，46(1)：8–13．

[21] 王致情．农业车辆自主跟随控制系统的设计与实现[D]．南京：南京农业大学，2014．

责任编辑：罗慧敏

英文编辑：吴志立

Design of autonomous following system for master-slave vehicles operating in orchard based on binocular stereo vision

Bi Weiping1,2, Zhang Huan1,2, Qu Zhenlin1,2, Ding Yongqian1,2*, Yu Hongfeng1,2, Wang Bo1,2
(1.College of Engineering, Nanjing Agricultural University, Nanjing 210031, China; 2.Jiangsu Province Engineering Laboratory for Modern Facility Agriculture Technology & Equipment, Nanjing 210031, China)

Abstract:A checkerboard plate was fixed on the back part of the master-vehicle as a guidance feature for the slave-vehicle. The three-dimension(3D) information of each corner point on the checkerboard plate can be easily obtained by the stereo vision system which was fixed on the front of the slave-vehicle. Through the analysis of the 3D information of each corner point, the distance, the relative lateral offset and heading angle can be obtained between the two vehicles. These information can be used to guide the slave-vehicle to follow the master-vehicle autonomously. According to this method, a master-slave following system was built for vehicles operating in orchard. The test results of stereo vision system performance in different light environment showed that the navigation parameter deviation is not associated with the light intensity during the day, so the stereo vision system have good adaptability to the natural light environment. The performance of the master-slave vehicles system was tested in an orchard. The update rate of the navigation information was 4.0 Hz. During the operation, the average deviation of lateral offset, the distance, heading angle was 3.68 cm, 8.24 cm and 2.32° respectively. The experimental results showed that the slave vehicle can follow the master-vehicle in real time successfully.

Keywords:master-slave vehicles in orchard; binocular stereovision; autonomous navigation

中图分类号：S225.93

文献标志码：A

文章编号：1007−1032(2016)03−0344−05

收稿日期：2016–01–15 修回日期：2016–03–16

基金项目：国家“十二·五”科技支撑计划项目(2013BAD08B04–1)；江苏省农机局–南京农业大学科研启动基金项目(6KH–0603)

作者简介：毕伟平(1989—)，男，山西太原人，硕士研究生，主要从事车辆导航与信息融合技术研究，bwp2012@163.com；*通信作者，丁永前，博士，副教授，主要从事农业信息和自动化研究，yongqiand@njau.edu.cn