一种温室移动作业平台协同跟随作业导航方法及装置与流程

技术特征：

1.一种温室移动作业平台协同跟随作业导航装置，包括：运载平台(1)、履带行走装置及探测控制装置；所述运载平台(1)用于装载物料果实和作业工具；所述履带行走装置用于驱动运载平台(1)的行驶；所述探测控制装置用于检测平台的行驶路径偏差以及移动平台与工作人员之间的距离，并控制移动平台沿着导引路径跟随作业人员行驶。

2.根据权利要求1所述的一种温室移动作业平台协同跟随作业导航装置，其特征在于：所述履带行走装置由左伺服电机驱动器(11)、右伺服电机驱动器(12)、左伺服电机(10)、右伺服电机(9)、蓄电池组(18)、左齿轮减速器(14)、右齿轮减速器(13)、左履带(8)、右履带(7)、左驱动轮(16)、右驱动轮(15)、左诱导轮(4)、右诱导轮(3)、左负重轮(6)、右负重轮(5)组成；所述左伺服电机(10)和右伺服电机(9)的输出轴分别经左齿轮减速器(14)和右齿轮减速器(13)与左驱动轮(16)和右驱动轮(15)相互连接；所述左驱动轮(16)和右驱动轮(15)与左诱导轮(4)和右诱导轮(3)大小相等并通过左履带(8)和右履带(7)分别与位于运载平台(1)前面左侧的左诱导轮(4)和前面右侧的右诱导轮(3)连接。

3.根据权利要求1所述的一种温室移动作业平台协同跟随作业导航装置，其特征在于：履带行走装置的左负重轮(6)安装在左驱动轮(16)和左诱导轮(4)之间，右负重轮(5)安装在右驱动轮(15)和右诱导轮(3)之间，并分别与移动平台左右两侧的固定轴相连。

4.根据权利要求1所述的一种温室移动作业平台协同跟随作业导航装置，其特征在于：所述探测控制装置包括测距传感器模块、磁导航传感器模块(20)、温度检测模块、速度检测模块、无线发射模块和主控制器；所述履带行走机构的前测距传感器模块(21)和后测距传感器模块(22)分别安装在移动平台的车体纵向中心面上；测距传感器模块包括超声波测距传感器和红外传感器，每个测距传感器模块的超声波测距传感器安装在红外传感器的正上方并相距1cm；所述运载平台(1)的底部正中间安装磁导航传感器模块(20)，磁＝导航传感器模块(20)距离地面4cm且关于移动平台的车体纵向中心面对称；所述速度检测模块包括左编码器和右编码器，左编码器和右编码器分别与左伺服电机(10)和右伺服电机(9)连接；所述无线发射模块包括红外遥控器和红外接收头，红外接收头经导线与主控制器连接；所述温度检测模块通过导线与主控制器连接；所述主控制器安装在所述运载平台(1)内部，通过导线与左伺服电机驱动器(11)和右伺服电机驱动器(12)相连，左伺服电机驱动器(11)和右伺服电机驱动器(12)均引出导线分别连接左伺服电机(10)、左编码器和右伺服电机(9)、右编码器；所述主控制器经导线分别连接前测距传感器模块(21)和后测距传感器模块(22)的超声波测距传感器，再通过内置的模数转换模块经导线分别连接前后测距传感器模块(21)和后测距传感器模块(22)的红外传感器；所述左编码器和右编码器的外壳分别与左伺服电机(10)和右伺服电机(9)的外壳固定，左编码器转子与左伺服电机(10)的转子连接，右编码器转子与右伺服电机(9)的转子连接。

5.一种温室移动作业平台协同跟随作业导航方法，其特征在于：利用权利要求1所述的导航装置，具体包括以下步骤：

步骤一，导航装置接收到启动信号后，主控制器进入初始化程序，先判断程序是否有异常，若程序正常则选择工作模式，在手动模式下由红外遥控器遥控移动平台行驶，在跟随作业模式下则启动测距子程序，通过测距传感器模块的超声波传感器和红外传感器检测行进方向上障碍物的距离；

步骤二，为了补偿温室温度对声波在空气中传播速度的影响，通过温度检测模块实时测量当前温室的温度值T，得超声波的传播速度V为：

$V=331.4*\sqrt{1+T/273}\≈331.4+0.607T(m/s)---\left(1\right)$

步骤三，测量时每组测距传感器模块的超声波传感器和红外传感器分别采集移动平台与作业人员之间的距离三次，通过自适应加权融合算法对测距传感器模块采集的数据进行处理，通过权值的计算和分配得到计算后的测量距离；

步骤四，当测距传感器模块的实际测量值小于设置的安全跟随距离时，主控制器控制左伺服电机和右伺服电机停止转动；当移动平台的测距传感器模块的实际测量距离大于设置的安全跟随距离时，主控制器控制左伺服电机和右伺服电机运行；

步骤五，磁导航传感器每个探测点之间距离1cm，移动平台在行驶过程中通过16位数字式磁导航传感器检测铺设在行驶路径上的导引磁条2产生的磁性，主控制器通过中断方式每150ms采集一次磁导航传感器的偏移量，通过磁导航传感器左右两侧探测点输出的不同位置判断移动平台偏离行驶路径的程度，主控制器根据移动平台的偏移量以及偏移量的变化率控制对左右伺服电机和右伺服电机进行PWM调速，当导引磁条在移动平台的正中间时，磁导航传感器中间的四个输出点导通，当磁导航传感器的输出点左移，移动平台重心偏向右垄，主控制器分别给左伺服电机和右伺服电机减小和增加相同的占空比，从而实现作业平台的左转调速；当磁导航传感器的输出点右移，移动平台重心偏向左垄，主控制器分别给左伺服电机和右伺服电机增加和减小相同的占空比，从而实现作业平台的右转调速，通过不断调整行驶位姿，实现移动平台沿着导引路径行走。

6.根据权利要求5所述的一种温室移动作业平台协同跟随作业导航方法，其特征在于：步骤三中，所述的自适应加权融合算法，具体计算过程如下：

过程一，为超声波传感器进行3次测量的测距均值，为超声波测距传感器的测距方差，计算公式如下

${\stackrel{\‾}{X}}_p=\frac{1}{3}{\mathrm{\Σ}}_{i=1}^3{X}_p\left(i\right)---\left(2\right)$

${\stackrel{\‾}{\mathrm{\σ}}}_p=\frac{1}{3}{\mathrm{\Σ}}_{i=1}^3{\{X_p\left(i\right)-{\stackrel{\‾}{X}}_p\}}^2---\left(3\right)$

过程二，为红外传感器进行3次测量的测距均值，为红外传感器的测距方差，计算公式如下

${\stackrel{\‾}{X}}_q=\frac{1}{3}{\mathrm{\Σ}}_{i=1}^3{X}_p\left(i\right)---\left(4\right)$

${\stackrel{\‾}{\mathrm{\σ}}}_q=\frac{1}{3}{\mathrm{\Σ}}_{i=1}^3{\left\{X_q\left(i\right)-{\stackrel{\‾}{X}}_q\right\}}^2---\left(5\right)$

过程三，超声波传感器和红外传感器的加权因子分别为W_p、W_q，计算公式如下

$w_p={\left\{{\mathrm{\σ}}_p^2(\frac{1}{{\mathrm{\σ}}_p^2}+\frac{1}{{\mathrm{\σ}}_q^2})\right\}}^{-1}---\left(6\right)$

$w_q={\left\{{\mathrm{\σ}}_q^2(\frac{1}{{\mathrm{\σ}}_p^2}+\frac{1}{{\mathrm{\σ}}_q^2})\right\}}^{-1}---\left(7\right)$

过程四，通过自适应加权融合后的测距均值计算公式如下

$\stackrel{\‾}{X}=W_p{\stackrel{\‾}{X}}_p+W_q{\stackrel{\‾}{X}}_q---\left(8\right).$