基于柔索测距装置的农机定位方法研究

夏聪明，喻擎苍，贾 靖，赵 晖

(浙江理工大学 信息学院，杭州 310018)

0 引言

促进农业发展、农民增收一直是近几年国家关注的重点。2004-2015年的11年时间，全国农作物耕种收综合机械化水平增长了26.3%[1]。实践表明：提高农业机械化水平，可以促进农产品的增产增收，提高农民收入，还能减少农民劳作时间，提高劳动效率[1-2]。农机自主行走可以提升田间作业的效率，降低劳动者的工作强度[3]，有利于我国农业的智能化和自动化；还能提高机械作业精度，增加机械作业的时间，让劳动者免于机械重复的劳动，确保了劳动者的人身安全。传统的定位方式中，GPS定位的精度低，差分GPS价格比较昂贵，惯性导航定位存在累加误差问题，激光测距存在对准问题，视觉定位难以适应田间复杂环境。为此，研究设计了柔索长度测量装置和基于激光测距的缓冲机构，利用三角法对农机进行定位。

1 三角法定位原理分析

在田间地头固定两个立杆(定点A与定点B)。在农机上选取一固定点，用柔索将农机上的固定点分别与两个立杆相连接，且将农机与两杆抽象成质点，这样农机与地头的两个杆之间便形成一个三角形，如图1所示。

先测出两个杆之间的距离L,通过柔索测距装置测出农机固定点到两个杆之间的距离L1和L2，据此可计算出农机在田间的坐标。

图1 三角定位原理图Fig.1 The principle of triangulation

2 柔索长度测量装置设计研究

2.1 长度测量原理

柔索装置采用的柔索是直径为0.5mm的不锈钢钢丝绳，强度高、质量小。实验测量出100m的钢丝绳平衡力为16N，拉断力为215N。在长度测量装置中，柔索的一端连接在地头一定点，另一端经过长度测量装置与农机相连。本文采用的是增量式光电旋转编码器，在编码器的输出轴上套上一个固定直径的滚轮，将柔索缠绕在滚轮上；农机在田间作业时，滚轮会随着机器的行走带动编码器旋转，对编码器的脉冲进行计数，即可得出机器在田间行走的距离。

2.2 长度测量核心部件

本装置采用的是600线编码器，旋转1圈可输出600个脉冲，可通过ArduinoMega 2560对编码器输出的脉冲进行计数。测试前，将Arduino的数字引脚2与3连接编码器的A相与B相输出线，如图2所示。

图2 Mega 2560读取编码器脉冲Fig.2 Using Mega2560 to read the encoder pulse

在Arduino程序[4]中，用中断的方式对脉冲计数，使用中断的优点是中断响应时间快，不占用资源，主程序同时还可以完成其它任务。Mega2560的2号引脚和3号引脚可作为中断引脚。当Arduino检测到A通道有变化, 程序就立刻跳转到中断函数,中断的触发方式是电平变化，中断函数会在上升沿和下降沿都会被调用，B通道的变化也是同样的道理。中断函数的执行流程如图3(以A通道为例)所示。

图3 中断函数执行过程Fig.3 Interrupt function execution process

在实际的应用中，对于编码器脉冲的读取肯定会出现少读或误读的情况，为了检测脉冲的读取误差，用步进电机通过联轴器带动编码器转动固定的圈数，计算Arduino读取的脉冲数并与理论情况的脉冲数比较。进行50次测量，取平均值，如表1所示。

表1 编码器读取脉冲数与实际脉冲数

Table 1 The number of pulses read by the encoder and the number of actual pulses

当编码器转动500圈以下时，检丢的脉冲数逐渐变多；到了500圈以后，检丢的脉冲数稳定在15个左右，脉冲丢失率在0.002 5%以内。

2.3 纳线机构研究设计

完成距离的测量需要有一个纳线机构对柔索进行收放线的控制，其必须可以自动进行绕线，以保证柔索能均匀地缠绕在绕线轴上。

如图4所示：57步进电机旋转带动连接在其上面的绕线轴转动，同时42步进电机会协同转动，通过联轴器带动其上面的丝杆转动，进而推动其上面的螺母在丝杆上来回运动。为此，设计了一个推动装置，套在螺母上，随着螺母的来回移动而移动，此装置负责将线来回推动，使得绳索在绕线轴上缠绕1圈，装置将绳头向前推动，使绕线均匀缠绕在绕线轴上。为了防止推动装置倾斜，在旁侧用一固定光杆使推动装置保持稳定。控制电机的芯片是Arduino Mega2560, 步进电机驱动模块采用的是双路输出的DRV8825步进电机驱动器。纳线机构工作流程如图5所示。

图4 收纳线装置结构示意图Fig.4 The structure of the receiving line device

图5 纳线机构工作流程图Fig.5 Working process of nanowire mechanism

3 基于激光测距的缓冲机构设计

3.1 缓冲机构总体设计

田间作业时，机器摆动导致柔索突然张紧或松弛，造成柔索断裂或对装置拉力过度。增加弹簧缓冲，用激光测距传感器测量绕线点的偏移。柔索测距如图6所示。柔索的一端固定在立杆上，在编码器上套上带有凹槽的滚轮，柔索在滚轮上饶1圈后，通过缓冲的滚轮，纳线机构上缓冲滚轮用铝杆支撑。

3.2 缓冲位移测量

激光测距传感器VL53L0X采用飞行时间原理实现测距应用，使用I2C接口进行设备控制和数据传输,激光测距传感器一次测量可在30ms内完成。对VL53L0X进行测试，将Arduino的[5]SDA与SCL和VL53L0X的SDA、SCL相连接，Arduino的3.3V引脚连VL53L0X的VIN,两者的GND连接。

图6 柔索测距详细图Fig.6 Flexible cable distance measuring structure

为了测试激光测距传感器的准确性及稳定性，在50、100、150mm的距离上每隔1s进行1次测量，得到500组数据，从得到的数据中选取100组进行分析，如图7所示。

(a) 50mm

(b) 100mm

(c) 150mm

(d) 200mm图7 不同距离下VL53L0X测试数据Fig.7 VL53L0X test data at different distances

对上述数据进行分析，统计结果如表2所示。由表2发现：测量距离为200mm时最大误差达到了10mm，50mm时平均相对误差为7.9%，难以满足田间定位高精度的要求，因此要用卡尔曼滤波的方法进一步减少误差，提高测量精度。

表2 VL53L0X测试数据统计结果Table 2 VL53L0X test data statistical results

对上面测量的数据进行卡尔曼滤波处理后，如图8所示。

对卡尔曼滤波后的数据进行分析，结果如表3所示。由表3可见：无论是均值还是平均相对误差较卡尔曼滤波之前的数据都更加精确；测量误差控制在3%以内，满足对缓冲装置伸缩位移进行测量的需求。

(a) 50mm

(b) 100mm

(c) 150mm

(d) 200mm图8 卡尔曼滤波后数据Fig.8 Data after Calman filtering表3 卡尔曼滤波后VL53L0X测试数统计结果Table 3 Statistical results of VL53L0X test number after Calman filtering

测量距离/mm均值/mm方差平均相对误差/%5051.36731.20382.73100102.11002.32742.11150151.97001.86641.31200202.44003.02491.22

4 定位试验结果与分析

长度测量装置得到了农机与定点之间的距离，缓冲机构测量出了冲击带来的偏移的位移，将以上数据通过Arduino得到，通过I2C将数据传给树莓派，由树莓派根据建立的坐标系计算出农机的坐标。坐标系以定点A与定点B的中心点作为坐标原点，如图9所示。

图9 定位坐标系Fig.9 Positioning coordinate system

试验时，手动拉着装置沿Y轴行走，得到的距离即为Y轴坐标。在1～10m的距离上每隔1m输出一次坐标值，进行50次试验，结果如表4所示。

表4 不同测量点距离值Table 4 3Distance values of different measuring points mm

通过表4可以看出，该方法的定位误差在20mm以内。这表明该方法可靠、精确。

5 结论

研究了基于柔索测距装置的农机定位方法。通过读取编码器脉冲计算柔索长度，设计纳线机构进行收放线的控制。利用激光测距芯片对缓冲机构伸缩位移进行测量。编写了Arduino程序，读取了激光测距传感器的测量数据，用卡尔曼滤波的方法对数据进行了处理，滤波后的数据最大平均相对误差为2.73%。对该方法进行了定位试验，结果表明：定位误差小于20mm，适用于农机田间定位。