基于云台相机的番茄主茎跟踪控制方法*

张万豪， 张文爱， 冯青春， 成伟， 王博文

1. 太原理工大学 信息与计算机学院， 山西 晋中 030600；2. 北京农业智能装备技术研究中心， 北京100097； 3. 国家农业智能装备工程技术研究中心， 北京100097

中国是世界上番茄栽培面积最大、 生产总量最多的国家， 常年产量在5 000 t以上， 并呈现增长趋势[1]． 目前， 番茄已经成为我国种植面积和产量最大的蔬菜品种之一， 2020年番茄产量为6 515万t， 占全国蔬菜总产量的7.1%[2]． 在番茄种植周期中， 整枝打叶、 授粉、 采摘以及喷药等环节人工作业频次高、 劳动强度大． 近年来， 随着从业劳动力的流失， 番茄种植管理的人工费用已达到总生产成本的35%以上， 成为制约番茄种植产业效益增长的重要因素[3]． 因此， 研发替代或者辅助人工作业的番茄机器人， 对于我国番茄安全供应、 种植产业健康发展具有重要意义．

由于种植管理作业对象(果、 叶、 花)都是交错生长在番茄主茎上[4]， 这类作业有着相似的作业路径， 都是沿着番茄主茎的生长方向进行作业． 因此对番茄植株主茎视觉信息的获取是实现对番茄智能化管理作业的重要前提， 针对这类作业环节， 借鉴人工作业方式， 沿着番茄主茎生长方向自下而上主动搜索作业目标， 是提高对作业对象定位效率和识别精度的有效方法．

国内外学者针对果蔬的茎秆形态跟踪定位问题做了很多研究． Botterill等[5]设计的葡萄藤修剪机器人采用3个围绕葡萄藤固定安装在箱体内部的立体相机获取葡萄藤的视觉信息以实现修剪工作． 黄彪[6]设计的枇杷剪枝机器人采用2个超广角相机在3个固定高度对枇杷树做3轮扫描获取枇杷枝条的视觉信息， 摄像头的畸变导致采集的图像存在较大失真． Cuevas等[7]设计的玫瑰整枝机器人采用立体相机在9个固定位姿对玫瑰灌木进行扫描获取整株玫瑰的点云图， 根据相邻作业点的实时更新实现对整株玫瑰的作业． 冯青春等[8]提出的离散小视场番茄主茎测量方法， 实现了对番茄主茎的测量和信息获取． 荷兰Tomas[9]团队设计的Kompano Deleaf-Line机器人采用可控的Chameleon 3相机实现了对番茄侧枝的修剪． Jon等[10]借助安装于机械手末端的深度相机获取视觉信息设计了一款番茄采摘机器人 ． Arad等[11]设计的甜椒收获机器人采用随末端移动的深度相机识别目标和果梗． 这些研究极大促进了玫瑰、 番茄和果树等作物整枝和收获技术的进步， 但这些研究中， 以固定视角被动搜索目标的设计方案使得设备的体积过大， 得到大量的冗余信息； 以可控视角主动搜索的方式， 设备轻巧灵活， 获取信息可用性更大， 可以提高信息的处理效率．

针对番茄智能化作业需要， 本文设计了基于云台相机的番茄主茎形态视觉信息获取系统， 提出了对番茄主茎的视觉跟踪控制方法． 根据摄像机视场内的番茄主茎形态， 实时调整云台姿态使番茄主茎上的跟踪参考点始终位于相机视场中心． 该研究可以为番茄整枝、 授粉、 喷药和采摘等智能化作业环节提供技术支撑．

1 温室环境与跟踪原理

1.1 温室番茄栽培环境

温室番茄栽培逐渐趋于工厂化和标准化， 这将有利于机器人的智能作业． 选择试验的温室栽培环境如图1所示． 番茄栽培在离地面600 mm高的培养槽中， 番茄种植行间距约为1 200 mm， 株距为300 mm． 距地面600～1 500 mm的区域为番茄整枝、 打叶、 授粉和收获的作业区域． 随着番茄植株的生长， 通过释放缠绕于主茎的吊线进行落蔓， 使得作业区域相对地面高度保持不变， 因此本文以此高度区域内的番茄植株为跟踪的对象．

图1 温室番茄栽培环境

1.2 主茎跟踪原理

如图2所示， 果实和侧枝是番茄整枝和采摘环节的作业对象． 番茄的侧枝和果实是沿着主茎生长的， 由于作业对象交错生长在番茄主茎两侧， 大视场下会出现信息冗余和遮挡， 采用离散小视场的方法可以获取更加精确的信息． 借鉴人工作业方式， 沿着番茄主茎自下而上地搜索可以提高作业对象(花、 果和侧枝)的探测效率． 主动跟踪就是不断调整相机姿态使番茄主茎上变化的跟踪参考点始终位于视场中心进而实现模仿人工作业时获取视觉信息的一种目标跟踪方法．

图2 目标生长规律

2 云台视觉系统设计

2.1 硬件构成

视觉信息是支撑机器人作业的重要依据， 为了避免大视场带来的信息冗余以及对目标定位和识别精度的影响， 采用离散小视场方式采集信息． 本次试验设计的视觉获取系统如图3所示．

① Realsense D435i相机； ② 俯仰机构； ③ 摆动机构； ④ 控制模块与电源．图3 视觉获取系统

视觉获取系统主要由一个二自由度的云台和Intel公司的Realsense D435i深度相机组成． 云台有左右摆动和上下俯仰两个自由度， 其范围分别为(-75°～+75°)和(-30°～+60°)， 其精度均为0.01°． Realsense D435i相机是一款深度相机， 采用即用型USB供电形式， 搭载Intel公司的D400系列深度模块， 具备完整的深度光学解决方案． 它主要由两个红外传感器、 一个1 080 P的色彩传感器和一个红外激光投射器3个部分组成[12]． D435i模仿人眼的视觉成像原理， 通过两个红外传感器同时追踪由红外激光投射器发出的红外光位置， 然后利用三角定位原理计算得到图像中的深度信息． 主要参数[13]如表1所示．

表1 Realsense D435i相机参数

2.2 结构参数标定

明确云台与相机的结构位置关系是根据图像信息控制云台姿态的重要依据[14]． 通过D435i相机可以直接得到目标点在相机坐标系下的坐标， 但为了实现对目标的跟踪和云台的控制， 必须将其坐标转换到云台坐标系下． 如图3所示， 云台坐标系为OT-XTYTZT， 相机坐标系为OC-XCYCZC， 则云台坐标与相机坐标存在式(1)的转换关系[15]．

(1)

T为云台与相机坐标系之间的转换矩阵． 由式(1)可知， 对云台相机做标定就是求解旋转矩阵R和平移分量t．

如图3所示， 以云台垂直自由度的转轴与云台上平面的交点O0为原点， 并以左右摆动转轴向上的方向作为Z轴的正方向， 以水平指向目标的方向作为X轴正方向， 建立基坐标系O0-X0Y0Z0． 以云台两个旋转轴的交点OT为原点， 以俯仰旋转轴为Y轴， 以与相机光轴平行的方向为X轴， 建立云台坐标系OT-XTYTZT． 以左红外相机的光心为原点OC建立相机坐标系OC-XCYCZC，ZC轴与光轴重合． 根据坐标系建立规则， 基坐标系O0-X0Y0Z0与云台坐标系OT-XTYTZT的原点共线．

已知， 坐标系绕X轴和Z轴旋转具有式(2)的旋转矩阵关系[16]．

(2)

θ，γ分别是坐标系绕X轴和Z轴的旋转角度．

(3)

设空间中一点q在基坐标系下为q0， 在云台坐标系下为qT， 根据坐标系转换原理可得式(4)的关系．

(4)

(5)

Δθ和Δγ分别是云台左右摆动和上下俯仰两个自由度的增量．

(6)

(7)

(8)

采用Tsai两步法[17]求解式(8)； 已知矩阵A和H的旋转轴和旋转角可以求出转换矩阵T的旋转矩阵R， 再将R代入式(8)则可求出平移分量t， 最后求得相机坐标系与云台坐标系的转换矩阵T．

3 主茎跟踪控制算法

3.1 跟踪参考点坐标转化

根据番茄主茎生长趋势实时调整云台姿态使番茄主茎上的跟踪参考点始终位于视场中心， 是实现对番茄主茎主动跟踪的基本思路． 如何确定跟踪参考点BC，i和跟踪点的期望位置BC，i+1是实现对番茄主茎主动跟踪的关键． 在番茄工厂化种植中， 番茄主茎都做了吊绳落蔓处理， 根据实地考察后发现， 这些番茄植株除在根部有较大弯曲外， 主茎其他部分生长趋势比较规则， 在较小视场下可近似为一条空间曲线． 依据这些， 提出了跟踪参考点BC，i和跟踪点期望位置BC，i+1的确定方法．

(9)

式中， 向量l(m，n，p)为直线L的单位方向向量．

(10)

根据式(10)可以求得期望点BC，i+1(0， 0，zc，i+1)在相机坐标系下的三维坐标．

根据图4的跟踪方案， 已经确定了跟踪参考点BC，i和期望点BC，i+1在相机坐标系下的空间坐标．

图4 番茄主茎主动跟踪原理

3.2 云台姿态伺服控制

(11)

将式(11)求得的云台坐标点代入到式(6)并作齐次化处理可得：

(12)

将参考点BT，i和BT，i+1的坐标代入到式(12)并化简可得：

(13)

其中c代表cos函数，s代表sin函数．

方程(13)中包含的未知数Δθ，Δγ是云台依次绕X轴和Z轴旋转的角度增量． 解得：

(14)

θ是每一次位姿变换前云台坐标系OT-XTYTZT相对于基坐标系O0-X0Y0Z0的上下俯仰角度， 可以通过云台驱动电机编码器实时反馈得到．

4 试验与结果分析

4.1 现场试验

为了验证对番茄主茎主动跟踪算法的实际效果， 在北京大兴区宏福国际农业生产基地番茄工厂化温室内开展了现场试验． 选择5株番茄植株， 分别在跟踪步长Δl为50 mm， 100 mm， 150 mm和200 mm下完成一次跟踪试验． 将安装有相机的云台固定在与番茄植株种植槽等高的可移动工作台上， 云台与试验对象的水平距离为400～600 mm， 跟踪的番茄主茎区域为离地600～1 500 mm高度范围内． 根据相机视场大小和试验对象的高度， 需要调整云台姿态4～5次以完成对单株番茄主茎的跟踪． 为了避免引入主茎识别误差， 本文以人眼对摄像机视场主茎的实时识别结果为依据， 在主茎中心线上选择跟踪起始点． 基于python 3开发了云台相机的跟踪交互程序， 将相机的视频流实时显示在界面上， 如图5所示， 人工通过鼠标在靠近视场中心的番茄主茎上选择一个点AC，i作为跟踪起始点， 沿着主茎的中心线在视场上边缘主茎上再次选取一个点AC，i+1． 由深度相机可得点AC，i，AC，i+1空间坐标， 以两点连线AC，iAC，i+1的立体单位向量作为当前视场主茎的单位方向向量l． 设定跟踪步长Δl下， 根据式(9)可以求得跟踪参考点BC，i在当前视场下的坐标． 求解云台转角位移， 调整云台相机的姿态后， 比较相机的视场中心点与跟踪期望点的像素偏差．

图5 人工选取主茎跟踪起始点

4.2 结果分析

(15)

式(15)中，D0为番茄主茎的像素宽度， 根据试验环境测得番茄主茎像素宽度平均为15个像素．

跟踪结果如图6所示， 是同一植株在相同姿态下不同跟踪步长的跟踪结果． 当跟踪步长Δl=200 mm时， 跟踪参考点不在当前视场， 为了直观体现误差， 试验中可人为标记．

图6 同一姿态下不同步长跟踪结果

表2 跟踪偏差

图7和图8分别是不同跟踪步长下平均水平偏差和平均垂直偏差． 观察每组跟踪步长下的跟踪偏差， 随着云台姿态调整次数的增加， 跟踪的平均偏差也在变大， 这是由于随着跟踪的进行， 跟踪对象与云台相机的距离在变大， 相机的畸变变大， 导致相机测距产生偏差． 比较同一次云台姿态调整后的跟踪偏差可以发现， 随着跟踪步长的增加， 同等姿态下跟踪偏差也在变大， 因为随着跟踪步长的变大， 对拟合的主茎直线L的准确率要求增加． 但是番茄主茎客观上不是一条规则的空间直线， 随着跟踪步长的增加， 依赖于拟合直线确定的跟踪参考点误差增大， 导致跟踪的偏差也会变大． 比较每一组数据的跟踪偏差变化曲线可以发现在云台姿态第4次调整后， 跟踪偏差较前3次有明显的增加， 这是由于第4次姿态调整后， 跟踪距离变化较大， 相机畸变变大， 导致跟踪效果变差．

图7 不同步长跟踪平均水平偏差

图8 不同步长跟踪平均垂直偏差

图9是同一株番茄植株在不同跟踪步长下的跟踪偏差， 在跟踪步长为50 mm， 100 mm， 150 mm时， 跟踪偏差增加幅度较小， 但在跟踪步长为200 mm时， 跟踪偏差增加幅度较大． 说明跟踪步长在不超过150 mm时可以保证跟踪精度， 当跟踪步长达到或超过200 mm时， 跟踪效果变差， 精度下降严重．

图9 同一植株不同步长跟踪结果

图10是不同跟踪步长下， 水平平均偏差占番茄主茎像素宽度的多少， 20组数据的最大水平平均偏差占主茎像宽的46.7%． 在跟踪步长为150 mm时， 最大水平平均偏差占主茎像宽的40%； 在跟踪步长为200 mm时， 最大水平平均偏差占主茎像宽的74.7%， 远高于平均水平； 说明当跟踪步长Δl超过200 mm时该算法不适合跟踪番茄主茎． 可以发现当跟踪步长越小时， 跟踪的精度就越高， 但是跟踪的效率也会降低， 综合考虑将跟踪步长Δl设置为100 mm到150 mm之间最为合适．

图10 不同步长水平偏差与主茎像宽之比

5 结论

为了提高对番茄智能管理作业对象的探测效率， 本研究提出了一种基于云台相机的番茄主茎主动跟踪控制方法． 采用二自由度云台相机， 实现了对主茎水平和垂直方向的跟踪； 通过视觉系统结构参数标定， 建立了云台与摄像机坐标之间的关系； 提出了以主茎形态为引导的主茎跟踪控制方法， 实现了摄像机视场中心对主茎的定向跟踪． 试验结果表明， 在番茄植株离地600～1 500 mm高度范围内， 当跟踪步长Δl≤150 mm时， 平均水平偏差和平均垂直偏差不超过6个像素， 水平偏差占番茄主茎像宽不超过40%； 当跟踪步长Δl≥200 mm时， 跟踪偏差变大， 水平偏差占番茄主茎像宽超过70%． 综合考虑跟踪精度和跟踪效率， 将跟踪步长设置为100～150 mm可以满足对番茄主茎的跟踪． 本研究可以为番茄的收获、 打叶和授粉提供视觉信息参考．