CN112904352A - 基于拟合滤波的激光和超声波香蕉树测距方法 - Google Patents

Abstract

本发明公开了基于拟合滤波的激光和超声波香蕉树测距方法，通过激光和超声波传感器组合对香蕉树进行测距，并由二者的测距数据相互校验滤除单个传感器由于光线等因素导致误差较大的测距值，采用最小二乘法以二次多项式能够较好地拟合一个周期内各采样时刻所测香蕉树的距离数据，基于拟合二次多项式和设定阈值有效滤除小灌木等干扰物和道路颠簸产生的偏差较大的距离数据，再求取滤波后距离数组中三个最小值的平均值，即为机器人与香蕉树的最短距离。本发明测距方法的误差率为1.0％‑2.0％，相应的最大测距误差为1.0cm，其测距稳定性好，能为机器人在自然香蕉园环境中实现局部定位和导航提供准确、可靠的距离数据。

Description

基于拟合滤波的激光和超声波香蕉树测距方法

技术领域

本发明涉及香蕉采摘机器人测距技术领域，具体为基于拟合滤波的激光和超声波香蕉树测距方法。

背景技术

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

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

上述机器人定位和导航方法在香蕉园复杂环境中效果不佳甚至可能失效，因此，提出通过激光和超声波传感器组合测量机器人与机耕道两侧香蕉树的距离，再利用相关融合算法实现机器人在香蕉园的局部定位和导航。显然，准确测出机器人与香蕉树的距离是实现定位和导航的关键。激光和超声波传感器都是常用的测距方式，因其使用简单、响应快、精度高、成本低而被广泛应用于汽车雷达和自动驾驶等领域。虽然激光测距响应速度更快、精度更高、但会受环境光线影响；而超声波测距不受光线影响，但测距时间与待测距离有关，并且要求待测物体具有一定的反射面，否则反射波太弱可能导致测距失败。

发明内容

本发明的目的在于提供基于拟合滤波的激光和超声波香蕉树测距方法，能够在理想环境下对香蕉树的最大测距误差率为1.0％，而在有小灌木等干扰物或者道路颠簸的香蕉园环境以及室外自然场景下的最大测距误差率为2.0％，相应的最大测距误差为1.0cm，且测距稳定性好,能为机器人在自然香蕉园环境中实现局部定位和导航提供准确、可靠的距离数据,以解决上述背景技术中提出的问题。

为实现上述目的，本发明提供如下技术方案：

基于拟合滤波的激光和超声波香蕉树测距方法，包括以下步骤：

S1：在一棵香蕉树测距周期内的各采样时刻，通过激光传感器和超声波传感器组合的方式测量机器人到香蕉树的距离，利用相互校验法将二者的测距值融合并生成一个距离数组；

S2：用二次多项式以最小二乘法对S1中的距离数据组进行拟合；

S3：基于拟合二次多项式函数和设定阈值对S1中的距离数据组进行滤波，并对滤波后的距离数组排序以便求出最小三个距离数据的平均值，从而得出机器人到待测香蕉树的最短距离。

更进一步地，S1中具体方法为：测距开始后，由激光传感器和超声波传感器同时测距，在每个采样时刻测距完成后，获取激光传感器和超声波传感器的测距数据，若两个传感器没有同时测空，则将二者的测距值进行相互校验；若两个传感器的测距数据的偏差小于校验阈值η，则两种传感器的测距值均有效，取二者的平均值作为本次的测距值；反之，若两个测距数据的偏差大于η，则取二者中小的距离数据作为本次的测距值。

更进一步地，S2中具体方法为：将激光传感器和超声波传感器测得香蕉树的n个距离数据序列化为二维数组，即{(k,d_k)|k＝1、2、…、n}，其中d_k表示第k个时刻的测距值，若拟合该距离数组的二次多项式函数为：

f(k)＝ak²+bk+c (1)

式(1)中a、b、c为待定的拟合二次多项式系数。

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

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

更进一步地，S3中具体方法为：对于设定的滤波阈值δ，若第k个时刻的测距值d_k满足：

|f(k)-d_k|＝|ak²+bk+c-d_k|＜δ (5)

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

更进一步地，所述激光传感器和超声波传感器设有三组等间距、并列对称安装在履带式香蕉采摘机器人的两侧。

更进一步地，三组激光传感器和超声波传感器的安装间距大于香蕉树的直径。

更进一步地，激光传感器选择测距范围为0.04-4m的VL53L1X，其采用STToF技术、物理红外滤光片和光学元件；超声波传感器选择US-015，其测距范围为0.02-4m，测距精度达0.5mm。

更进一步地，所述机器人的底层采用分布式控制系统，由控制器STM32控制三组激光传感器和超声波传感器的测距周期以及距离数据处理，其中，激光传感器VL53L1X在测距结束后以IIC协议输出距离数据，多个激光传感器通过IIC总线与控制器STM32的IIC模块SDA、SCL引脚连接，以不同的地址码实现多机通信，读取各传感器的测距数据；超声波传感器US-015测距完成后输出一个与距离成正比的时间脉冲，通过STM32中定时器模块的脉冲捕获通道来检测超声波传感器输出的测距时间脉冲，超声波传感器的时间脉冲输出引脚与STM32相应定时器的捕获通道引脚连接，超声波传感器测距启动端Trig可与控制器STM32的任一IO口连接。

与现有技术相比，本发明的有益效果是：

本发明提供的基于拟合滤波的激光和超声波香蕉树测距方法，采用将激光传感器和超声波传感器组合的方式测量机器人到香蕉树的距离，通过两种传感器的原始测距数据相互校验滤除单个传感器由于光线和发射角等因素导致误差较大的测距值，采用最小二乘法以二次多项式能够较好地拟合所测香蕉树的距离数据，另外，基于拟合二次多项式和设定阈值可以有效滤除小灌木等干扰物和道路颠簸产生的偏差较大的距离数据，进而求得机器人与香蕉树的最短距离；实验结果表明，本发明测距方法在理想环境下对香蕉树的最大测距误差率为1.0％，而在有小灌木等干扰物或者道路颠簸的香蕉园环境以及室外自然场景下的最大测距误差率为2.0％，相应的最大测距误差为1.0cm，且测距稳定性好，能为机器人在自然香蕉园环境中实现局部定位和导航提供准确、可靠的距离数据。

附图说明

图1为本发明的机器人在香蕉园机耕道俯视图；

图2为本发明的机器人基于激光和超声波传感器对香蕉树测距图；

图3为本发明的基于相互校验的香蕉树距离数组采集图；

图4为本发明的复杂环境下的测距数据及拟合曲线图；

图5为本发明的基于拟合与滤波的香蕉树测距流程图；

图6为本发明的激光和超声波传感器在履带式机器人侧面分布图；

图7为本发明的激光和超声波传感器与控制器STM32连接图。

图中：1、激光传感器；2、超声波传感器；3、机器人；4、香蕉树。

具体实施方式

下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。

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

由上可知，准确测出到香蕉树4的最短距离是机器人3实现定位和导航的前提，然而，由于香蕉树4旁有杂草、小灌木或香蕉叶等干扰物，以及香蕉园道路崎岖、泥泞导致机器人3颠簸，使得某些时刻的测距数据不准确，因此，不能简单以距离数据中最小值或某个时刻的测距值作为机器人3到香蕉树4的最短距离。基于此，本发明提供了基于拟合滤波的激光和超声波香蕉树测距方法，包括以下步骤：

第一步：在一棵香蕉树4测距周期内的各采样时刻，采用激光传感器1和超声波传感器2组合的方式测量机器人3到香蕉树4的距离，利用相互校验法将二者的测距值融合并生成一组距离数据，实现准确、可靠的香蕉树4测距；具体方法如下：

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

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

第二步：用二次多项式以最小二乘法对第一步中的距离数据组进行拟合；具体方法原理为：

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

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

f(k)＝ak²+bk+c (1)

式(1)中a、b、c为待定的拟合二次多项式系数。

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

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

第三步：基于拟合二次多项式函数和设定阈值对距离数据组进行滤波，并对滤波后的距离数组排序以便求出最小三个距离数据的平均值，从而得出机器人3到待测香蕉树4的最短距离，具体方法原理为：

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

对于设定的滤波阈值δ，若第k个时刻的测距值d_k满足：

|f(k)-d_k|＝|ak²+bk+c-d_k|＜δ (5)

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

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

为了进一步更好的解释说明本发明，还提供如下具体测距的结构原理：

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

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

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

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

综上所述：本发明提供的基于拟合滤波的激光和超声波香蕉树测距方法，针对GPS、激光雷达、图像等机器人定位导航方法在香蕉园复杂环境中的缺点和不足，采用将激光传感器1和超声波传感器2组合测量机器人3到香蕉树4的距离，通过两种传感器的原始测距数据相互校验滤除单个传感器由于光线和发射角等因素导致误差较大的测距值，采用最小二乘法以二次多项式能够较好地拟合所测香蕉树4的距离数据，另外，基于拟合二次多项式和设定阈值可以有效滤除小灌木等干扰物和道路颠簸产生的干扰距离数据，进而求得机器人3与香蕉树4的最短距离；实验结果表明，本发明测距方法在理想环境下对香蕉树4的最大测距误差率为1.0％，而在有小灌木等干扰物或者道路颠簸的香蕉园环境以及室外自然场景下的最大测距误差率为2.0％，相应的最大测距误差为1.0cm，且测距稳定性好；能为机器人3在实际香蕉园自然环境中实现局部定位和导航提供准确、可靠的距离数据。

以上所述，仅为本发明较佳的具体实施方式，但本发明的保护范围并不局限于此，任何熟悉本技术领域的技术人员在本发明披露的技术范围内，根据本发明的技术方案及其发明构思加以等同替换或改变，都应涵盖在本发明的保护范围之内。

Claims (8)

1.基于拟合滤波的激光和超声波香蕉树测距方法，其特征在于，包括以下步骤：

S1：在一棵香蕉树4测距周期内的各采样时刻，通过激光传感器1和超声波传感器2组合的方式测量机器人3到香蕉树4的距离，并利用相互校验法将二者的测距数据融合并累积生成一个距离数组；

S2：采用二次多项式以最小二乘法对S1中的距离数组进行拟合；

S3：基于拟合的二次多项式函数和设定阈值对S1中的距离数组进行滤波，并对滤波后的距离数组排序以便求出其中最小三个距离数据的平均值，从而得出机器人3到待测香蕉树4的最短距离。

2.如权利要求1所述的基于拟合滤波的激光和超声波香蕉树测距方法，其特征在于，S1中具体方法为：测距开始后，由激光传感器1和超声波传感器2同时测距，在每个采样时刻测距完成后，获取激光传感器1和超声波传感器2的测距数据，若两种传感器没有同时测空，则将二者的测距值进行相互校验；若两个测距数据的偏差小于校验阈值η，则两种传感器的测距值均有效，取二者的平均值作为本次的测距值；反之，若两个测距数据的偏差大于η，则取二者中小的距离数据作为本次的测距值。

3.如权利要求1所述的基于拟合滤波的激光和超声波香蕉树测距方法，其特征在于，S2中具体方法为：将激光传感器1和超声波传感器2测得香蕉树4的n个距离数据序列化为二维数组，即{(k,d_k)|k＝1、2、…、n}，其中，d_k表示第k个时刻的测距值，若拟合该距离数组的二次多项式函数为：

f(k)＝ak²+bk+c (1)

式(1)中a、b、c为待定的拟合二次多项式系数。

根据香蕉树距离数组的特点，利用最小二乘法求解该二次多项式的系数，基于最小二乘法可得系数a、b、c的计算表达式为：

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

4.如权利要求1所述的基于拟合滤波的激光和超声波香蕉树测距方法，其特征在于，S3中具体方法为：对于设定的滤波阈值δ，若第k个时刻的测距值d_k满足：

|f(k)-d_k|＝|ak²+bk+c-d_k|＜δ (5)

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

5.如权利要求1所述的基于拟合滤波的激光和超声波香蕉树测距方法，其特征在于，所述激光传感器1和超声波传感器2设有三组等间距、并列对称安装在履带式香蕉采摘机器人3的两侧。

6.如权利要求5所述的基于拟合滤波的激光和超声波香蕉树测距方法，其特征在于，三组激光传感器1和超声波传感器2的安装间距大于香蕉树4的直径。

7.如权利要求6所述的基于拟合滤波的激光和超声波香蕉树测距方法，其特征在于，激光传感器1选择测距范围为0.04-4m的VL53L1X，其采用STToF技术、物理红外滤光片和光学元件；超声波传感器2选择US-015，其测距范围为0.02-4m，测距精度达0.5mm。

8.如权利要求5所述的基于拟合滤波的激光和超声波香蕉树测距方法，其特征在于，所述机器人3的底层采用分布式控制系统，由控制器STM32控制三组激光传感器1和超声波传感器2的测距周期以及距离数据处理，其中，激光传感器1(VL53L1X)在测距结束后以IIC协议输出距离数据，多个激光传感器通过IIC总线与控制器STM32的IIC模块SDA、SCL引脚连接，以不同的地址码实现多机通信，读取各传感器的测距数据；超声波传感器2(US-015)测距完成后输出一个与距离成正比的时间脉冲，通过STM32中定时器模块的脉冲捕获通道来检测超声波传感器输出的测距时间脉冲，超声波传感器的时间脉冲输出引脚与STM32相应定时器的捕获通道引脚连接，超声波传感器测距启动端Trig可与控制器STM32的任一IO口连接。

Images