基于3D视觉的青饲机拖车车斗自动识别与定位方法

苗中华 陈苏跃 何创新 金称雄 马世伟 徐双喜

(1.上海大学机电工程与自动化学院， 上海 200072； 2.勇猛机械股份有限公司， 天津 301802)

0 引言

随着计算机、自动控制以及人工智能技术的不断发展，农业机械作业的智能化升级受到越来越多的关注。农机作业智能化对提升劳动生产效率和舒适性、节约生产成本、改善农产品生产质量具有重要作用[1]。青饲机是一种青贮饲料收获机械，主要用于收获大麦、燕麦、牧草、玉米和高粱等作物，可一次完成对作物的收割、切碎、揉搓，并将碎作物抛送至运输车。近年来，随着我国规模化、自动化、集约化畜牧养殖业的快速发展，青饲机具有广阔的市场需求，然而，国内青饲机技术水平较低，尤其在自动化、智能化方面，与国外相比差距较大[2-4]。

机器视觉技术是人工智能快速发展的一个重要分支，与图像处理、模式识别等技术结合，可实现特定物体的识别，以及多物体间的相互定位与位置跟踪。基于机器视觉的目标辨识、空间定位与位置跟踪技术已成为农机装备智能控制领域竞相探究的热点和难点，具有广泛的应用前景[5-15]。

目前，我国青饲机的青饲料收集作业主要为人工遥控操作，操作员需要通过肉眼识别车斗的位置，手动控制喷头的高度和角度。这种操作存在喷射不精准、反应不及时、物料浪费、人力劳顿等各种缺陷。因此，研究青饲机和拖车之间的协同作业和青饲料自动精准喷装技术具有实用价值。

青饲机和拖车之间的自动物料填充过程是一个双机协同作业控制问题[3-4]。首先需要自动识别行进中的拖车车斗外框边沿，获取车框与收获车喷料机械臂的相对位置，在此基础上自动控制机械臂和喷头的高度和角度，使抛出的青饲料恰好落入车斗中。

针对青饲机拖车车斗自动识别与定位问题，本文利用车斗外框上沿位于世界坐标系的局部最高处的特点，提出一种基于RGB-D相机三维视觉信息的运动车斗识别与定位方法。

1 运动车斗自动识别与定位方法

1.1 方法概述

本文所提方法采用“眼在手”工作模式[16]，通过在机械臂上安装相机，采集拖车区域的视频信息。在地面保持水平的理想条件下(实际操作中可以通过加载车体陀螺仪信息来近似达到水平)，利用车斗外框上沿位于世界坐标系局部最高处(垂直地面方向)的特点，获取车斗所在位置。因此，为了实现“定位”，即获取机械臂喷头与车斗的相对位置关系，需选择一个合适的世界坐标系原点。本文所建立的世界坐标系与相机坐标系如图1所示。

图1 坐标系示意图Fig.1 Schematic of coordinate system

世界坐标系中Oxy平面平行于地面，坐标系原点为机械臂基座旋转中心，z轴垂直地面向上，y轴指向机械臂在Oxy平面上的投影方向，即机械臂中轴线位于Oyz平面。饲料填充过程中世界坐标系跟随机械臂绕z轴转动，因此，饲料落点一直维持在Oyz平面。

车斗角点与饲料理想落点的世界坐标系坐标将通过定位算法获得，又由于机械臂喷头在该世界坐标系中位置已知，饲料的喷射速度已知，因此可获取饲料实际落点。当获取喷头与车斗的相对位置以及理想落点位置后，通过规划机械臂运动控制实际落点去接近理想落点位置。

识别与定位方法主要步骤如下：

(1)选择世界坐标系，并用奇异值分解(Singular value decomposition, SVD)求解其与相机坐标系之间的旋转平移矩阵。

(2)读入一帧三维点云数据、一帧深度图像，并对点云的格式进行转换。

(3)点云降采样。实际并不需要如此稠密的点云数据，降采样可成倍提升后续步骤的执行效率。

(4)使用步骤(1)所得旋转平移矩阵将相机坐标系下点云变换到世界坐标系下。

(5)对点云数据进行阈值操作获取车斗所在大致区域，并将保留值投射至二维平面。

(6)用随机抽样一致性算法(Random sample consensus，RANSAC)找出二维平面下的车斗角点定位。

(7)在像素坐标系下找到车斗角点，生成可视化界面。

识别与定位方法流程图如图2所示。

图2 拖车车斗识别与定位方法流程图Fig.2 Flow chart of method for trailer hopper positioning

1.2 位姿变换矩阵求解

图3 青饲机与拖车协同作业示意图Fig.3 Cooperative operation diagram of green forage harvester and trailer1.运输车 2.车斗 3.青饲料 4.喷头 5.机械臂 6.深度相机 7.青饲机 8.前方割台

如图3所示，相机固定于机械臂前端，确保较好的视角以便能够拍摄到收获车侧后方来车。相机固定后，相机坐标系与世界坐标系之间的相对关系得到确定。定位过程最终需得到机械臂喷头与拖车车斗之间的位置关系，所以需解算以上两个坐标系之间的旋转平移矩阵，即“手眼”标定过程。

为此需研究3D-3D的位姿估计问题，该问题的数学描述如下：

假设有一组配对好的3D点

(1)

需要找到一个欧氏变换R、t，使得

(2)

式中pi——点阵P中一个点的坐标

针对此类问题，视觉即时定位与视觉构建(Simultaneous localization and mapping，SLAM)中涉及较多相关应用，借助其思想求解本方案中所需的位姿变换矩阵。在视觉SLAM问题的研究中，构造视觉里程计(Visual odometry, VO)[17]是至关重要的步骤之一。VO的基本任务是通过图像估计相机运动。其中基于特征提取的视觉里程计较为成熟，是应用比较广泛的主流方法[18]。

此类视觉里程计需通过对前后两帧图像进行特征提取和匹配，从而得到两帧图像间对应特征点。通过特征匹配获取两组对应点，并通过三角化计算或者直接通过RGB-D相机的深度图确定特征点的3D位置，此时得到的两组点即为P与P′，最终根据相机类型选择求解运动估计的方法解决式(2)提出的问题。此处求得两组特征点间R、t矩阵为相机的姿态变换矩阵，即两个相机坐标系之间的旋转、平移矩阵。

本文相当于构建一个两帧间视觉里程计，得到世界坐标系与相机坐标系之间的变换关系。此处需要进行匹配的两组特征点分别是参考物在世界坐标系下的参考点与相机坐标系下的参考点。世界坐标系下参考点坐标通过实际测量获得，相机坐标系下的参考点坐标通过编写相应的程序在点云中指定。通过应用视觉SLAM中求取位姿变换矩阵的核心算法，解算两个三维坐标系之间的R、t矩阵。

视觉SLAM求解运动估计矩阵有两种方法：利用线性代数求解(主要是SVD)，以及利用非线性优化方式求解(类似Bundle adjustment)，本文将采用SVD算法完成解算。

通过构建最小二乘问题，求使得误差平方和达到极小值的R、t[19]。

(3)

式中J——两个点阵的误差平方和

位姿估计3D-3D求解过程如下

Algorithm 1 pose_estimation_3d3d

∥center of mass

∥remove the center

∥compute q1*q2^T

∥SVD onW

W=UΣVT,R=UVT,t=p-Rp′

至此，解得两组点阵之间的旋转矩阵R与平移矩阵t。所得结果为相机坐标系与世界坐标系之间的转换关系。

1.3 点云处理

考虑到课题的实际需求，处理点云数据之前进行数据降采样可成倍提升后续步骤的执行效率。降采样使用体素化网格的方法实现，在减少点云数据量的同时保留点云的形状特征，能有效提升算法速度。降采样时对输入的点云数据创建一个三维体素栅格，即把点云所在的三维空间划分成一些微小的空间三维立方体的集合，然后在每个体素中用该体素内所有点的重心来近似代表体素中所有的点，最终单个体素只剩一个重心点。

随后，在对实际情况进行合理分析的基础上进行阈值操作。如图3所示，左侧车辆为农用拖车，其高大的车斗为整车中最高的部分，利用这个限制条件可进行阈值处理，提取出车斗所在区域的点阵。此时需要用到世界坐标系下点云，即要求点云z轴垂直于地面，其值表示车斗高度。因此，使用1.2节所求R、t矩阵对相机坐标系下的点云数据进行旋转平移。计算公式为

(4)

式中Pw——世界坐标系下点云坐标

Pc——相机坐标系下点云坐标

Rcw——相机坐标系到世界坐标系的旋转矩阵

tcw——相机坐标系到世界坐标系的平移矩阵

X、Y、Z——Pc点云中点的x、y、z轴坐标值

然后根据Z通道提供的车斗高度信息进行阈值操作，取出车斗顶部的点云，并将Z值去除，仅保留X、Y值组成的Mat矩阵，完成降维处理。图4为同一物体在同一时刻相机坐标系下的点云图、像素坐标系(相机成相平面对应的坐标系)下的depth图及世界坐标系下阈值操作后的2D点阵图。

图4 参考物在同一时刻下的3种图像Fig.4 Three kinds of images of reference at the same time

1.4 定位算法

利用2D点阵图进行车斗角点定位，从而确定车斗的中轴线，车斗中轴线即饲料喷洒的落点所在直线。该直线与机械臂垂直投影所在直线(世界坐标系y轴)的交点即饲料理想落点。车斗角点与饲料落点关系如图5所示。

图5 理想饲料落点定位示意图Fig.5 Schematic of ideal point location

图6 RANSAC算法车斗边沿拟合图Fig.6 Edge fitting of bucket by RANSAC

根据车斗实际情况，需要从图4c中拟合出多条直线，找到车斗边沿。此时最小二乘法显然不适用。对于每条边而言，都必须排除一些局外点，才能拟合出合理的直线。因此引入随机抽样一致性算法[20-21]来完成这个工作。针对车斗点阵进行边沿直线拟合示意图如图6所示，具体步骤为：

(1)使用RANSAC算法编程，拟合出4条直线，即车斗边沿所在直线。

(2)对4条直线产生的6个交点进行筛选(车斗虽然为矩形，但由于直线由算法拟合产生，拟合所得的矩形对边不可能平行)。筛选依据为交点所在坐标的绝对值，即交点偏移坐标原点的距离，将距离最大的两个点排除(矩形车斗两组对边拟合直线的交点距原点最远)。

(3)根据矩形特征，将保留的4个交点进行排序。将同一边沿的交点连结，得到车斗边沿。

二维平面上的定位得到车斗角点在世界坐标系下的x、y轴坐标值，另外通过拖车车斗上沿与机械臂基座的高度差获取角点z轴坐标值，完成三维世界坐标系下的定位。

现在已经获得世界坐标系下的车斗角点坐标，进一步根据图5车斗角点与饲料落点关系，可以获取饲料理想落点所在范围(即车斗中轴线)，而实际落点坐标则由喷头挡板角度和喷射速度决定。自动喷装系统的控制算法需要根据车斗的位置信息以及落点的满溢程度进行合理的喷装路径规划。该部分内容不在本文的讨论范围。

1.5 可视化反馈

本文重点研究运动车斗的识别与定位，自动喷装系统的控制算法将利用所检测信息进行机械臂运动规划。为了可视化识别与定位效果，进行逆向坐标变换，将世界坐标系下找到的角点坐标变换到相机坐标系，然后通过相机内参数将相机坐标系下的角点坐标映射到像素坐标系下，从而在RGB图或者depth图中标记出车斗所在区域，形成直观的可视化反馈，便于操作人员监测青饲料填充情况。

将世界坐标系下找到的角点坐标变换到相机坐标系下，进行逆变换

(5)

然后通过相机内参数将相机坐标系下的角点坐标变换到像素坐标系下，此时依据相机的针孔模型[22-23]做转换

(6)

式中Puv——像素坐标系下角点的齐次坐标

K——相机内参矩阵

fx、fy、cx、cy——像素坐标系与成像平面间缩放平移参数

通过以上步骤，将世界坐标系下定位的角点显示在二维图像上，方便工作人员观察车斗识别的实际情况，及时做出必要的调整。

2 现场实验

2.1 实验设计

实验采用图漾公司的PERCIPIO.XYZ系列RGB-D相机，如图7所示。该相机支持0.5～6 m测试距离，误差达到测试距离的0.25%。测试用计算机型号为ThinkPad X230i，Intel内核(2.4 GHz)，10 GB内存。测试系统为Ubuntu 14.04 LTS，编程语言为C/C++。

图7 RGB-D相机结构示意图Fig.7 Schematic of camera structure1、3.红外摄像头 2.红外激光发射器 4.彩色摄像头 5.定位孔

车斗识别和自动定位系统在天津勇猛机械股份有限公司生产的青饲机上进行了田间实验，实验平台如图8所示，相机固定于机械臂上，拍摄车斗所在区域，并通过USB向计算机传递视觉信号。

图8 现场实验平台Fig.8 Experiment platform1.车斗 2.青饲料 3.机械臂 4.RGB-D相机

2.2 实验结果分析

在算法检测效果评估中，采用交并比(Intersection over union，IoU)量化实验结果。IoU表示算法得出的候选框与原标记框的交叠率，最理想的情况是完全重叠，即IoU为1。交并比计算公式如下

(7)

式中 area()——图像区域面积函数

C——候选区G——原标记区

现场实验检测效果如图9a所示，对应的RGB参考图如图9b所示，图9c为IoU计算区域。其中绿色框线区域为检测算法获取的目标区域，即候选区；蓝色框线区域为手动标记的目标区域，即原标记区；紫色填充区域为以上两个区域的重叠区。根据以上信息可计算出每次检测结果的IoU，由此定量评判算法精度，具体数据如表1所示。表1中呈现了6帧图像的交并比，并得出其平均值为0.931，且总体检测效果稳定，无明显检测不到车斗的帧。

图9 现场实验结果量化分析示意图Fig.9 Quantitative analysis schematic of field experiment results

帧号area(C)∩area(G)/像素area(C)∪area(G)/像素IoU12430572697400.90122653782900760.91532576702599980.99142900983299710.87953115233311190.94162587192698710.959平均值0.931

3 结论

(1)本文方法能对不同高度、不同类型拖车车斗进行准确识别，无需使用专用的配套拖车或人工添加标记点。

(2)通过“手眼”标定建立各坐标系之间的关系，不仅能从图像中识别运动车斗，而且能在空间上定位车斗角点与饲料落点，作为机械臂控制的输入信号。

(3)本文方法降低了点云计算量，能满足现场工作的实时性需求。对连续6帧检测结果进行IoU分析表明，该算法满足现场工作的精度需求。