香蕉园机器人导航的激光与超声波组合测距方法研究

付根平 杨尘宇 张世昂 黄伟锋 陈天赐 朱立学

(1.仲恺农业工程学院自动化学院，广州 510225；2.仲恺农业工程学院机电工程学院，广州 510225)

0 引言

香蕉是岭南特色水果和主要经济作物之一。适收期的香蕉串质量普遍在25 kg左右，目前香蕉采收仍以传统人工方式为主，由多人协作完成，其劳动强度大、人工成本高、采摘效率低[1-2]，严重制约了香蕉的种植规模和经济效益。因此，迫切需要研制香蕉采摘机器人，以便提高采摘效率、降低采摘成本、解决农业劳动力短缺问题，实现香蕉采收的机械化和智能化[3]。

采摘机器人在香蕉园进行自主作业，必须具备定位和导航能力[4-5]。目前，常用的机器人定位和导航方法包括基于GPS[6-7]、机器视觉[8-9]、激光雷达[10-11]、惯性测量单元[12]和电机编码器[13]等。由于香蕉树种植密度较大、行距有限，且香蕉树叶繁茂不规整，使机器人容易被遮挡，导致携带的GPS模块难以稳定接收信号，甚至完全接收不到卫星定位信号[14-15]，因此，机器人仅依靠GPS不能在香蕉园实现可靠、连续的定位和导航。基于机器视觉[16]和激光雷达[17]的测距和导航方式容易受香蕉枝叶遮挡、香蕉园环境光照强度变化及机器人颠簸抖动等因素的影响，使摄像头采集的环境图像模糊不清，香蕉树干被遮挡导致难以准确识别机器人到香蕉树的距离；而激光雷达扫描的环境点云位置数据不准确，难以准确识别香蕉树干等目标的准确位置，使机器人难以进行准确定位和导航[18-19]。基于惯性测量单元的导航方式主要通过对测得的机器人实时加速度等运动状态数据进行积分求得机器人的相对位置，但累积位置误差会随着积分过程而不断增大[20]。根据编码器返回的驱动电机实时转速求解机器人相对运动位置的方法在机器人出现打滑和空转时定位误差会很大[21]。

上述机器人定位和导航方法在香蕉园复杂环境中效果不佳、甚至失效。显然，准确测出机器人与香蕉树的距离是实现定位和导航的关键。激光和超声波传感器使用方便、响应快、精度高、成本低，被广泛应用于汽车雷达和自动驾驶等领域[22]。激光测距响应速度快、精度高，但会受环境光线影响[23-24]；而超声波测距不受光线影响，但测距时间与待测距离有关，并且要求待测物体具有一定的反射面，否则会因反射波太弱导致测距失败[25]。综合考虑两者测距的特点，提出采用激光和超声波组合对香蕉树进行测距。为此，提出一种基于拟合滤波的激光和超声波香蕉树测距方法。首先由激光和超声波传感器组合测量机器人到香蕉树的距离，并通过相互校验融合成一组距离数据，再利用二次多项式以最小二乘法对该组距离数据进行拟合，基于拟合的二次多项式函数和设定阈值对该组距离数据滤波，求出滤波后该距离数组中最小3个距离数据的平均值，得到机器人到香蕉树的最短距离。

1 基于拟合滤波的激光和超声波测距

由于香蕉采摘机器人体积较大，而香蕉园机耕道宽度有限，且崎岖、泥泞，使得机器人运动中容易出现颠簸、侧滑，故需实时检测机器人位姿并据此调整其运动状态，防止撞上香蕉树。因此，通过安装在机器人两侧的激光和超声波传感器，测出机器人到两侧香蕉树的最短距离dl、dr，如图1所示。再结合几何关系，采用相应算法求解出机器人与机耕道中线的位置偏差，以及机器人中线与机耕道中线的夹角，实现机器人在机耕道的局部定位，并据此调节机器人的运动方向和速度，使其回到机耕道中间。

显然，准确测出到香蕉树的最短距离是机器人实现定位和导航的前提。然而，由于香蕉树旁有杂草、小灌木或香蕉叶等干扰物，以及香蕉园道路崎岖、泥泞导致机器人颠簸，使得某些时刻的测距数据不准确，因此，不能简单以距离数据中最小值或某个时刻的测距值作为机器人到香蕉树的最短距离。本文先通过激光和超声波传感器组合对香蕉树测距，再对二者的测距数据进行相互校验、拟合和滤波处理，进而求出机器人到香蕉树的最短距离。

1.1 激光和超声波测距值相互校验合成距离数组

由于激光传感器在强光环境中测距范围缩小、精度下降，甚至无法有效测距，故仅用激光传感器测距可靠性不高，精度难以保证。而超声波测距不受光照影响、测距精度高，但响应时间与所测距离有关，对反射面小的物体容易因接收不到反射波而导致测距失败。因此，采用激光和超声波传感器组合对香蕉树进行测距。

假设在一棵香蕉树的测距周期内机器人匀速前进，则激光或超声波传感器在不同时刻位于一条直线上，且发射的激光束或超声波与机器人运动方向垂直，如图2所示。将测距周期内n个时刻测到的与香蕉树的距离记为d1、d2、…、dn，其中：d1为传感器进入可测区域内第1次测得机器人与香蕉树距离；dn为传感器在可测区域内最后测得机器人与香蕉树距离。此外，若测距传感器不在香蕉树的可测区域内(如d0、dn+1)，将探测不到香蕉树而输出一个特定的时间脉冲或数值，此时传感器为测空状态。根据激光和超声波传感器是否同时测空来判断香蕉树测距周期的开始和结束。

正常情况下，激光和超声波传感器在同一时刻对香蕉树的测距值非常接近。然而，由于激光和超声波传感器测距信号的发射角不同，机器人颠簸，以及香蕉园光照、干扰物遮挡等因素都会影响测距精度，使得某些时刻二者所测距离可能存在较大偏差，甚至出现测空。因此，为了提高测距精度和可靠性，将二者的测距值进行相互校验。如图3所示，每个采样时刻测距完成后，若2种传感器没有同时测空，则将二者的测距值进行相互校验。如果2个距离数据的偏差小于校验阈值η，则认为2种传感器的测距值均有效，取二者的平均值作为本次的测距值；反之，若2个距离数据的偏差大于η，则取二者中小的距离数据作为本次的测距值。这样做有2个好处：当2个传感器中有1个测空时可以滤除测空值，而保留有效的测距值。当2个传感器的测距数据存在较大偏差时取小者对机器人的安全裕度更大。

1.2 基于二次多项式的香蕉树距离数据拟合

在测距周期内，香蕉树相对于激光和超声波传感器所在直线的位置不变，因此，香蕉树轮廓线到测距传感器所在直线的最短距离即为机器人到香蕉树的最短距离，如图2中d3。由于香蕉树干近似柱形，且测距传感器在不同时刻位于一条直线上，故理想情况下该组距离数据{d1,d2,…,dn}满足圆函数特征。然而，由于激光束和超声波测距信号都具有一定的发射角，呈喇叭状，使得激光和超声波的边界信号被香蕉树提前反射回来，导致实测距离比真实距离小，从而不再满足圆函数特征。为了找到适合实测距离数据的曲线，对多棵香蕉树实测的距离数据进行回归分析，发现香蕉树的实测距离数据比较符合二次多项式曲线特征，如图2中虚线，因此，选择二次多项式对香蕉树的实测距离数据进行拟合。

将激光和超声波传感器测得香蕉树的n个距离数据序列化为二维数组，即{(k,dk)|k=1，2，…，n}，其中dk表示第k个时刻的测距值。拟合该距离数组的二次多项式函数为

f(k)=ak2+bk+c

(1)

式中a、b、c——待定的拟合二次多项式系数

根据距离数组的特点，最小二乘法比较适合求解该二次多项式的系数，因此，基于最小二乘法可得系数a、b、c的计算表达式为

(2)

(3)

(4)

由式(2)～(4)即可求得系数a、b、c，从而确定拟合香蕉树距离数组的二次多项式(1)。

1.3 基于拟合多项式滤波的最短距离求取

二次多项式拟合曲线的顶点到测距传感器所在直线的距离即为机器人与香蕉树的最短距离，因此，可取满足拟合曲线的距离数据(图4)中最小值作为机器人到香蕉树的最短距离。然而，由前文分析可知，理想情况下测得香蕉树的距离数据满足二次多项式特征，但对杂草、小灌木、香蕉叶等不规则干扰物，以及香蕉园道路崎岖泥泞导致的测距数据发散、无规律，不满足二次多项式特征。因此，为了能准确求取机器人到待测香蕉树的最短距离，必须将偏差较大的测距点(图4)滤除。

对于设定的滤波阈值δ，若k时刻的测距值dk满足

|f(k)-dk|=|ak2+bk+c-dk|<δ

(5)

则视该测距值dk有效；否则，视为偏差较大的测距值而舍弃。由式(5)对所测距离数据进行滤波，得到一组满足拟合二次多项式的距离数据。为了提高可靠性，取滤波后距离数组中最小3个的平均值作为机器人到香蕉树的最短距离。

综上所述，基于拟合滤波的激光和超声波香蕉树测距流程如图5所示，激光和超声波传感器各时刻测得机器人到香蕉树的距离数据经相互校验后，采用二次多项式以最小二乘法进行拟合，然后基于该拟合二次多项式和设定阈值对其滤波，去除偏差较大的距离，再求取其中最小3个距离的平均值，作为机器人到香蕉树的最短距离。

1.4 测距效果性能指标

为了衡量本文测距方法在不同环境下对香蕉树的测距精度和稳定性，定义以下参数指标：

对于香蕉树的一个测距周期，测距误差e为

e=|dt-da|

(6)

式中dt——机器人到香蕉树的目标最短距离，cm

da——机器人到香蕉树的实测最短距离，cm

测距误差率σ为

(7)

对于目标最短距离dt，若第i次的测距误差为e(i)，则重复测量p次中最大测距误差emax为

emax=max{e(i)|i=1,2,…,p}

(8)

最大测距误差率σmax为

(9)

在某种测距环境下设定q个目标最短距离，第j个目标最短距离的最大测距误差为emax(j)，则最大误差均值em为

(10)

其中，测距误差e、测距误差率σ越小，表明机器人对香蕉树的测距精度越高；而最大测距误差emax、最大测距误差率σmax、最大误差均值em越小，表明测距方法越稳定，适用性越好。

2 硬件设计

2.1 激光和超声波传感器选型

如图6所示，在履带式香蕉采摘机器人的两侧对称、等间距、并列安装3组激光和超声波传感器。由于激光和超声波传感器的安装间距比香蕉树直径大，所以一棵香蕉树的测距周期内只有1组激光和超声波传感器起作用。

香蕉园的行距为1.5～3 m，为了确保机器人处在行间任何位置都能有效测距，要求激光和超声波传感器的测距范围超过3 m。因此，选择测距范围为0.04～4 m的VL53L1X型激光传感器，它采用STToF技术、物理红外滤光片和光学元件，故受测距目标的颜色和反射率影响小，可在各种环境光照下实现有效测距，而且测距精度高、响应速度快、体积小、使用方便、探测灵敏性高。而超声波传感器选择US-015型，其测距范围为0.02～4 m，测距精度达0.5 mm，稳定性强。显然，组合使用VL53L1X型激光传感器和US-015型超声波传感器，满足机器人在香蕉园行间对两侧香蕉树的测距要求。

2.2 激光和超声波传感器的控制电路

机器人底层采用分布式控制系统，由STM32控制3组传感器的测距周期和距离数据处理，激光和超声波传感器与控制器STM32的电路连接方式如图7所示。所选VL53L1X型激光传感器在测距结束后以IIC协议输出距离数据，因此，多个激光传感器通过IIC总线与控制器STM32的IIC模块SDA、SCL引脚连接，以不同的地址码实现多机通信，读取各传感器的测距数据。而US-015型超声波传感器测距完成后输出一个与距离成正比的时间脉冲，STM32先准确捕获该脉冲时间，再换算成对应的距离，所以通过STM32中定时器模块的脉冲捕获通道来检测超声波传感器输出的测距时间脉冲。因此，超声波传感器的时间脉冲输出引脚与STM32相应定时器的捕获通道引脚连接。此外，超声波传感器测距启动端Trig可与控制器STM32的任一IO口连接。

进入一棵香蕉树的测距区域后，在每个采样时刻由控制器STM32分别发射激光和超声波信号对香蕉树进行测距，然后根据接收到的返回数据换算出2种传感器的测距值，再通过相互校验得出该时刻的有效距离。重复上述步骤，即可测得不同时刻机器人到香蕉树的距离。

3 实验与结果分析

为了验证本文所述香蕉树测距方法的有效性和鲁棒性，选择不同直径和高度的香蕉树模型，参照实际香蕉树的种植株距、行距在实验室搭建图8所示的模拟香蕉园环境。由于测距实验不需要用到香蕉采摘机构，并且出于安全考虑，选用不带香蕉采摘机构的履带机器人(长1.0 m、宽0.76 m)，其两侧等间距(0.25 m)支撑杆上并列安装激光和超声波测距传感器，如图9所示。

激光和超声波传感器原始距离数据相互校验阈值η=3，基于拟合函数的滤波阈值δ=1.8。在Matlab中编写机器人对香蕉树测距数据的拟合、滤波和可视化等处理程序，并由式(6)～(10)计算出衡量测距效果的相关参数指标。

3.1 激光和超声波传感器测距性能对比

为了比较激光传感器和超声波传感器对香蕉树的测距性能，控制机器人在模拟香蕉园中匀速前进，由激光和超声波传感器同时对一棵香蕉树进行测距，得到图10所示的距离数据。

由图10可知，在二者均有效测距时，大部分时刻激光和超声波传感器的测距数据相同，只有3个时刻的测距数据不同，且偏差只有1 cm，主要考虑到STM32控制器数据处理方便，将测距值进行了四舍五入处理，而实测距离偏差在1 cm以内，这表明正常情况下激光和超声波的测距精度非常接近。另外，开始2个时刻和最后3个时刻的测距点仅超声波测距有效，这是因为超声波的发射角比激光大，在香蕉树可测区域的边界只有超声波传感器能接收到反射波完成测距，而激光传感器由于发射角小导致探测不到香蕉树。实验结果表明，激光和超声波传感器测距数据相互校验滤波的方法可行，能获得一组有效的距离数据，为后续二次多项式拟合和阈值滤波做准备。

3.2 测距方法有效性验证

设定机器人与香蕉树的目标距离分别为30、50、80、100 cm，机器人在模拟香蕉园机耕道上匀速前进。在每个目标距离对香蕉树重复测量5次。

每个目标距离取一组测距数据，经过对激光和超声波传感器的原始距离数据进行校验、最小二乘拟合和阈值滤波后，得到图11所示4种不同目标距离的测距点和二次多项式拟合曲线。显然，二次多项式曲线较好地拟合了大部分距离数据，且满足式(5)阈值滤波条件，而误差较大的测距点很少。此外，对每个目标距离进行5次重复测量的距离数据，经校验、拟合、滤波、排序及求平均值后得出机器人到香蕉树的最短距离如表1所示，各实测最短距离都很接近目标最短距离，且最大测距误差emax为0.7 cm，最大测距误差率σmax均不超过1.0%。实验结果表明，该测距方法能够有效测量机器人到香蕉树的距离，且测距精度较高、稳定性好。

3.3 测距方法鲁棒性验证

3.3.1小灌木干扰下的香蕉树测距模拟

在模拟香蕉园中放置一棵小灌木，并让机器人对其进行测距，如图12所示。由于小灌木细长弯曲、枝叶疏密不均且形状不规整，测得分散、无规律的距离数据如图13所示。显然，测得小灌木的距离数据不符合二次多项式特征，据此，可基于香蕉树距离数据的拟合二次多项式函数和设定阈值将探测到香蕉树旁的杂草、小灌木等干扰距离数据滤除。

为了验证本文方法对有小灌木等干扰物的香蕉树也能有效、准确测距，在香蕉树旁放置一棵小灌木，如图14所示，让机器人距离该香蕉树50 cm匀速直线驶过，重复进行4次测距，通过对激光和超声波所测的距离数据进行校验、拟合、滤波等处理，得到的二次多项式拟合曲线如图15所示。

图15中，分散、无规律，且与拟合二次多项式曲线偏差较大，不满足滤波条件的测距点，主要原因是由小灌木枝叶干扰香蕉树形成的。显然，基于拟合函数和设定阈值可将其有效滤除，从而减少对求取最短距离的影响。通过对4次重复测量的距离数据拟合、滤波后求得机器人与待测香蕉树的最短距离如表2所示。由表2可知，测距误差e都不超过1.0 cm，最大测距误差率σmax为2.0%，表明本文测距方法能较好地消除小灌木对香蕉树测距的影响，且测距精度较高。

表2 小灌木干扰下实测最短距离及误差率

3.3.2崎岖不平的香蕉园机耕道测距模拟

机器人在由小木块模拟的崎岖不平香蕉园机耕道中前行(图16)，通过激光和超声波传感器测量机器人到两侧香蕉树的距离。重复测量4次，所测的距离数据经拟合、滤波后如图17所示，与拟合二次多项式曲线偏差较大的测距点，主要由机器人驶过小木堆时颠簸较大引起，显然，偏差较大的测距点均已被滤除。

此外，通过对4次重复测距的数据拟合、滤波后求得机器人与待测香蕉树的最短距离如表3所示。由表3可知，测距误差e2次为零，另2次为0.3 cm，最大测距误差率σmax为0.6%。实验结果表明，在崎岖不平的机耕道上也能较准确地测出香蕉树的距离。

表3 崎岖机耕道上实测最短距离及误差率

3.4 与图像测距方法对比

为了进一步说明本文测距方法的优点，与基于深度相机的图像测距方法进行对比。先利用Intel的realsense D435i型深度相机采集机器人两侧环境的RGB图像与深度信息图像，然后在RGB图像中对香蕉树进行识别并获取其位置，再将香蕉树在RGB图像中的位置映射到深度信息图像，从而得出机器人与香蕉树的距离。由于图像测距法需要先对RGB图像中待测香蕉树进行识别与位置判断，但在不同距离与光照条件下，香蕉树在RGB图像中呈现出不同的尺寸、表型、颜色等特征，增加了对香蕉树识别和测距的计算成本。

将机器人与香蕉树的4个目标距离分别设定为30、50、80、100 cm，在模拟香蕉园环境中利用realsense D435i型深度相机对每个目标距离重复测量5次，从图像中识别出的香蕉树干及其深度信息如图18所示，香蕉树的实测最短距离和相关参数指标如表4所示。与本文测距方法的实验结果(表1)中相关参数指标进行对比，如表5所示，两者的最大测距误差emax相同，但图像测距方法的最大误差均值em和最大测距误差率σmax更大，这表明本文测距方法的精度更高、稳定性更好。

表4 基于图像的实测最短距离及其最大误差率

表5 本文测距方法和图像测距方法相关性能对比

虽然基于深度相机的图像测距法也可以较为准确地测量香蕉树的距离，但与本文基于激光和超声波的测距方法相比，还存在以下不足之处：计算成本高，在获取距离信息前，需要对待测香蕉树进行识别并判断其位置，难以在一些边缘计算设备上进行部署。若有其他光源干扰，容易出现噪声，导致深度信息测量不准确。当待测香蕉树与深度相机的距离较短时，无法计算出深度信息，例如上述设备在实验中测得，在20 cm内无法输出深度信息。

3.5 室外自然场景测距适用性验证

在校园内室外自然场景中选取外形轮廓与香蕉树相似的植株进行实验，以此验证本文测距方法在实际环境中的适用性。如图19所示，自然光照条件下控制机器人在柔软的草地上匀速驶过待测植株，由激光与超声波传感器同时对待测植株测距，再通过本文所述方法得出机器人到待测植株的最短距离。出于一般性考虑，实验中选取4个目标距离进行测试，分别为50、60、80、100 cm。

每个目标距离取一组测距数据，经过对激光和超声波传感器的原始距离数据进行校验、最小二乘拟合和阈值滤波后得到图20所示4种不同目标距离的测距数据点和二次多项式拟合曲线，显然，二次多项式曲线能够较好地拟合距离数据，且满足式(5)阈值滤波条件，而误差较大的测距点很少，仅在50 cm和100 cm中存在，说明该测距方法在室外自然环境下性能稳定，受光照等因素影响小。

对每个目标距离进行5次重复测量的距离数据，经校验、拟合、滤波、求平均值后得出机器人到待测植株的实测最短距离da如表6所示，其中测距误差e都较小，最大测距误差emax为1.0 cm，最大误差均值em为0.68 cm，最大测距误差率σmax为1.4%。对比表1所示室内模拟香蕉园测距结果，室外有3个目标距离的最大测距误差率σmax超过1.0%，最大测距误差emax稍微偏大，可能是草地松软和不平整导致机器人测距过程中有一定的颠簸，从而影响测距精度。实验结果表明，本文测距方法在室外自然场景下也具有较高的精度和良好的稳定性。

表6 室外场景下的不同实测最短距离及最大误差率

4 结论

(1)由激光和超声波传感器组合测量机器人到香蕉树的距离，通过2种传感器原始测距数据的相互校验可以滤除单个传感器由于光线和发射角等因素导致误差较大的测距值。

(2)采用最小二乘法选择二次多项式能较好地拟合所测香蕉树的距离数据，而基于拟合的二次多项式和设定阈值可以有效滤除小灌木等干扰物和道路颠簸产生的干扰距离数据，从而获得机器人与香蕉树的最短距离。

(3)实验表明，本文测距方法在理想环境下对香蕉树的最大测距误差率为1.0%，而在有小灌木等干扰物或者道路颠簸的模拟香蕉园环境以及室外自然场景下的最大测距误差率为2.0%，相应的最大测距误差为1.0 cm，且测距稳定性良好。本文方法能为机器人在实际香蕉园环境中实现局部定位和导航提供准确、可靠的距离数据。