CN113625707A - 一种多传感器融合的温室自动跟随平台及其控制方法 - Google Patents

Abstract

本发明提供一种多传感器融合的温室自动跟随平台及其控制方法，包括定位装置、障碍物检测装置、主控制器、平台执行装置和辅助通讯模块；所述定位装置包括UWB基站和腰带；所述可穿戴式腰带上设有UWB标签、六轴陀螺仪、高度传感器、定位控制器、无线传输模块发送端和移动电源；首先通过UWB基站获取工作人员与平台的相对距离，并利用智能算法对工作人员进行测距定位，主控制器结合避障装置信息，采用改进的人工势场法进行路径规划，采用模糊控制结合平台运动学模型对路径进行跟随，从而完成平台的实时跟随效果。

Description

一种多传感器融合的温室自动跟随平台及其控制方法

技术领域

本发明属于智能农业装备领域，具体涉及一种多传感器融合的温室自动跟随平台及其控制方法。

背景技术

近些年来我国温室面积不断扩大，2016年末，全国温室占地面积334千公顷。但是温室内茄果类、瓜类、花菜类蔬菜等作物的中期管理包括打杈、打顶、打老叶、疏花疏果和后期的采摘运输人工劳动强度高、生产效率低。国外的现代化温室，结构规范，可采用轨道运输车提高生产效率，但是我国温室标准不一且空间狭小，建立轨道式运输平台成本较高。现有轮式、履带式作业平台的安全性、智能化程度都较低，工作人员操作十分不便。

穆元杰等发明公开了种温室大棚多功能作业平台及其控制方法专利号：202011348757.1，该发明在温室大棚内的种植垄间安装运行轨道，移动平台通过遥控器控制可在升降轨道上运行，也可通过安装在导轨上的位置定位装置获取目标路径信息并控制平台按照目标路径行进，该轨道式运输平台结构铺设复杂，成本较高，灵活性较差。

王新忠等发明公开了一种温室移动作业平台协同跟随作业导航方法及装置专利号：201710133282.6，该发明通过磁导航传感器模块结合超声波、红外传感器实现移动平台在温室垄间的自主导航，但是该发明要在温室内提前铺设磁性导轨，对温室环境要求高，成本较高；

汪小旵等发明公开一种基于体感感应器的温室省力化自动跟随作业平台专利号：201510815583.8，该发明通过体感传感器采集工作人员的图像信息，通过PC对图像进行处理获取人体骨骼深度数据，再把相应的执行命令发送给下位机控制电机完成实时跟随作业，深度相机可同时完成障碍物和工作人员识别功能，但是深度相机对光照要求苛刻、处理深度图像对处理器要求过高、温室内多存在植株遮挡相机视野情况，所以局限性较大。

简立玮基于GPS通信定位模块开发了能够自动跟随的智能购物车专利号：201920694581.1，GPS可以准确的解决定位问题且抗干扰能力较强，但GPS在室内无信号，不能适用于温室。

叶宇智利用小型麦克风阵作为超声波接收器发明了一种基于超声波测距的自动跟随系统实现自动跟随效果专利号：201821328969.1，超声波定位系统精度较高、结构简单、成本较低等优点。但是超声波受多径效应和非视距传播影响很大系统稳定性较低。

发明内容

针对上述技术问题，本发明提供一种多传感器融合的温室自动跟随平台及其控制方法，

以解决温室作业过程中作业工具和物料果实的装载和轻便运输问题，本发明通过多传感器融合和智能控制算法技术实现平台在温室平稳作业，改善目前温室内移动平台智能化程度低、劳动强度大、作业频次高、作业效率低等问题。

本发明的技术方案是：一种多传感器融合的温室自动跟随平台，包括定位装置、障碍物检测装置、主控制器、平台执行装置和辅助通讯模块；

所述定位装置包括UWB基站和腰带；所述可穿戴式腰带上设有UWB标签、六轴陀螺仪、高度传感器、定位控制器、无线传输模块发送端和移动电源；UWB基站安装在平台的四个角，所述所述UWB标签与定位控制器连接，六轴陀螺仪与定位控制器连接，无线传输模块发送端与定位控制器连接，高度传感器与定位控制器连接，UWB标签、六轴陀螺仪、高度传感器、定位控制器、无线传输模块发送端还分别与移动电源连接；

所述障碍物检测装置与主控制器相连，用于检测平面障碍物，并传递给主控制器；

所述平台执行装置包括电机和电机驱动器，电机驱动器分别与电机和主控制器相连，电机与平台的履带底盘连接，驱动履带底盘的行驶；

所述辅助通讯模块包括蓝牙模块、无线传输模块接收端和移动通讯设备，蓝牙模块和主控制器连接，蓝牙模块与移动通讯设备无线通信，无线传输模块接收端主控制器连接；

所述平台主控制器接收来自无线传输模块接收端、蓝牙模块、二维激光雷达的信息并处理，主控制器设置有遥控模式和跟随模式，当选择遥控模式时，主控制器根据移动通讯设备的指令控制直流无刷电机实现平台履带底盘行驶，当选择跟随模式时，主控制器通过路径规划和模糊控制算法控制平台履带底盘行驶。

上述方案中，所述UWB基站的数量为四个，四个UWB基站呈长方形安装在平台四个角落；其中三个UWB基站安装于同一水平面，另外一个UWB基站的位置高于该水平面。

上述方案中，所述电机的数量为两个，电机为带霍尔传感器的直流无刷电机。

上述方案中，所述定位控制器为STM32F103核心板；所述平台主控制器为STM32H745。

上述方案中，所述障碍物检测装置为二维激光雷达，所述二维激光雷达与主控制器相连。

一种根据所述多传感器融合的温室自动跟随平台的控制方法，包括以下步骤：

平台运动学模型的建立：建立平台运动学模型，将运动学模型离散化处理并导入到主控制器中，平台开关打开后，主控制器进入工作模式选择界面，选择遥控模式或跟随模式；

遥控模式下：当选择遥控模式时，在遥控模式下所述蓝牙模块接收来自移动通信设备的指令，并将该指令发送给主控制器，主控制器对相应指令解析，通过定时器产生PWM波控制左、右电机调速，从而控制平台的履带底盘的行驶；

跟随模式下：当选择跟随模式时，工作人员佩戴腰带，通过定位控制器设定跟随距离D_S，定位装置的UWB标签利用TWR双边测距法开始采集工作人员与平台之间的距离信息并将其发送给定位控制器，定位控制器对测距信息进行卡尔曼滤波，运用三边定位算法和最小二乘法对工作人员进行定位，利用腰带上的六轴陀螺仪检测工作人员是否处于转身状态，高度传感器检测工作人员是否下蹲状态，根据不同状态对定位信息进行更新优化，定位装置通过无线传输模块发送端将更新优化过后的标签坐标发送给主控制器，主控制器以平台底盘建立坐标系，对更新优化后的标签坐标投影到坐标系平面，得到工作人员在平台底盘平面的投影坐标，计算出投影坐标到平台底盘的几何中心距离D_R，比较计算值与设定值，若当前工作人员距离D_R小于设定跟随距离D_S，则保持平台静止，如果当前工作人员距离D_R大于设定跟随距离D_S，则结合障碍物检测装置的信息，利用改进的人工势场法进行路径规划，利用模糊控制法控制电机从而控制平台履带底盘的行驶，实现实时跟随。

上述方案中，所述平台运动学模型建立中，对模型进行离散化处理如下：

Px_(t+1)＝Px_(t)+VT_scosθ

Py_(t+1)＝Py_(t)+VT_ssinθ

θ_(t+1)＝θ_(t)+WT_s

其中，V为平台中心处的线速度；

W为平台中心处的角速度；

V_r为平台的履带底盘的右轮线速度，V_l为平台的履带底盘的左轮线速度；

θ为方向角，即平台旋转的角度；

L为平台底盘宽度；

Px_(t)、Py_(t)为上一时刻坐标，Px_(t+1)、Py_(t+1)、为下一时刻坐标；

T_s为两时刻间隔时间；

θ_(t)为什么上一时刻方向角，θ_(t+1)为下一时刻方向角。

上述方案中，所述对定位信息进行更新优化具体为：设标签的坐标A₀(X₀,Y₀,Z₀)，利用

工作人员目前状态信息对标签坐标A₀(X₀,Y₀,Z₀)更新为(X′₀,Y′₀,Z′₀)：

其中b、c为固定值，α由六轴陀螺仪测量工作人员腰带初始位置与当前工作人员腰带位置的角度、H由高度传感器测量的腰带距离地面的高度；

将更新过的坐标(X′₀,Y′₀,Z′₀)进行投影到平台平面，得到坐标(X″₀,Y″₀),计算出投影坐标(X″₀,Y″₀)到坐标原点的距离D_R：

上述方案中，所述改进的人工势场法如下：

首先改进斥力场函数，并结合作业环境建立道路边界斥力场从而限制平台的行驶区域，改进障碍物的斥力场，并对人工势场法具体改进如下：

根据作业环境建立道路边界斥力场F_road，斥力如下：

其中，d_r为平台距离左侧道路边界距离，d_l为平台距离右侧边界距离，d₀为设定值；k_road为道路边界斥力增益系数，设置为固定值；

道路边界斥力场F_road方向为垂直于左右路面指向平台，为正时指向右侧，为负时指向左侧；

对障碍物的斥力场F_reqnew进行改进如下：

其中，F_req1由障碍物指向平台的斥力场，F_req2由平台指向目标点的斥力场，P(q,q_g)为一个矢量，方向是平台位置指向目标点位置，表示平台当前位置q与目标点位置q_g之间的欧几里德距离|q-q_g|，p₀为一个常数，表示障碍物对平台产生作用的最大距离；k_reqnew、k_req1、k_req1分别为斥力k_reqnew、F_req1、F_req2的增益系数，设定为固定值。

改进后的合力F_ff如下：

F_att＝k_attP(q,q_g)

其中，k_att为引力增益系数，设定为固定值，i为第i个斥力，n为斥力总个数

上述方案中，所述利用模糊控制法控制电机从而控制平台履带底盘的行驶的步骤如下：

已知改进后的人工势场法计算的合力的大小和方向，运用模糊控制，以人工势场法合力的大小和方向作为模糊控制输入，下一时刻平台位置坐标为模糊控制的输出，主控制器结合运动学模型求解左右轮速度，控制左右电机从而控制平台履带底盘的行驶，实现平台的自动跟随效果。

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

本发明以UWB基站、标签测得工作人员与平台的相对距离，利用智能算法计算出平台与工作人员相对位置关系，主控制器结合避障装置信息，采用改进的人工势场法进行路径规划，采用模糊控制结合平台运动学模型对路径进行跟随，从而完成平台的实时跟随效果。该平台不需要搭建轨道和支撑架，系统结构简单、成本较低、适用与不同结构温室。此外，本发明对人工势场法进行改进，结合避障装置信息平台运动学模型实现温室内移动平台平稳跟随作业功能，大大提高了作业效率。

附图说明

图1是本发明一实施方式的硬件结构示意图；

图2是本发明一实施方式的可穿戴式腰带示意图；

图3是本发明一实施方式的控制系统框架示意图；

图4是本发明一实施方式的工作流程图；

图5是本发明的改进人工势场法示意图；

图6是本发明一实施方式的模糊控制原理图。

1、UWB基站，2、二维激光雷达，3、电机，4、电机驱动器，5、蓝牙模块，6、无线传输模块接收端，7、平台主控制器，8、可穿戴式腰带，9、UWB标签，10、六轴陀螺仪，11、高度传感器，12、定位控制器，13、无线传输模块发送端，14、移动电源。

具体实施方式

下面详细描述本发明的实施例，所述实施例的示例在附图中示出，其中自始至终相同或类似的标号表示相同或类似的元件或具有相同或类似功能的元件。下面通过参考附图描述的实施例是示例性的，旨在用于解释本发明，而不能理解为对本发明的限制。

在本发明中，除非另有明确的规定和限定，术语“安装”、“相连”、“连接”、“固定”等术语应做广义理解，例如，可以是固定连接，也可以是可拆卸连接，或一体地连接；可以是机械连接，也可以是电连接；可以是直接相连，也可以通过中间媒介间接相连，可以是两个元件内部的连通。对于本领域的普通技术人员而言，可以根据具体情况理解上述术语在本发明中的具体含义。

图1和2所示为所述多传感器融合的温室自动跟随平台的一种较佳实施方式，所述多传感器融合的温室自动跟随平台包括定位装置、障碍物检测装置、平台主控制器、平台执行装置、辅助通讯模块。

所述定位装置包括四个UWB基站1、可穿戴式腰带8、一个UWB标签9、六轴陀螺仪10、高度传感器11、定位控制器12、无线传输模块发送端13、移动电源14，四个UWB基站1呈长方形安装在平台四个角落，其中三个基站安装于同一水平面，优选的，另外一个基站安装在高于该水平面50-100厘米处，所述定位控制器12优选为STM32F103核心板，UWB标签9通过串口和STM32F103核心板连接，六轴陀螺仪10通过IIC协议与STM32F103核心板通讯，无线传输模块发送端13通过SPI协议与STM32F103核心板通讯，高度传感器11与STM32F103核心板普通IO连接，UWB标签9、六轴陀螺仪10、高度传感器11、STM32F103核心板、无线传输模块发送端13均安装在可穿戴式腰带8中由移动电源14供电。

所述障碍物检测装置优选的，为二维激光雷达2，二维激光雷达2通过RS232接口与主控制器7相连。

所述平台执行装置包括两个带霍尔传感器的直流无刷电机3、电机驱动器4，电机驱动器4与平台主控制器相连；

所述辅助通讯模块包括蓝牙模块5、无线传输模块接收端6、移动通讯设备，优选的，所述移动通讯设备为手机，手机内配套APP软件，蓝牙模块5通过串口与主控制器7通讯，无线传输模块接收端6通过SPI协议与主控制器7通讯。

所述平台主控制器7为STM32H745，所述平台主控制器7接收来自无线传输模块接收端6、蓝牙模块5、二维激光雷达2的信息并处理，主控制器7设置有遥控模式和跟随模式，当选择遥控模式时，主控制器7根据移动通讯设备的指令控制直流无刷电机3实现平台履带底盘行驶，当选择跟随模式时，主控制器7通过路径规划和模糊控制算法控制平台履带底盘行驶。

本发明工作原理为：首先通过UWB基站9获取工作人员与平台的相对距离，并利用智能算法对工作人员进行测距定位，主控制器7结合避障装置信息，采用改进的人工势场法进行路径规划，采用模糊控制结合平台运动学模型对路径进行跟随，从而完成平台的实时跟随效果。

结合图3和图4，本发明具体工作流程如下：

首先为了更好的规划和控制，不考虑带轮打滑和地面不平导致的抖动情况对平台进行运动学建模，对建立的模型离散化处理，将处理后的运动学模型导入到主控制器6中。

升降平台开关打开后，控制系统进入工作模式选择界面，工作人员可选择模式为遥控模式和跟随模式。

遥控模式下蓝牙模块5接收来自手机蓝牙APP的指令，并将该指令发送给主控制器7，主控制器7对相应指令解析，通过定时器产生PWM波控制左右直流无刷电机3调速，从而实现平台的行驶功能；

跟随模式下工作人员正确佩戴腰带8，设定跟随距离D_S,定位装置的UWB基站9利用TWR双边测距法开始采集工作人员与平台之间的距离信息并将其发送给STM32F103核心板，STM32F103核心板对测距信息进行卡尔曼滤波，运用三边定位算法和最小二乘法对工作人员进行定位，利用腰带8上的六轴陀螺仪10和高度传感器11检测出工作人员目前是否转身、下蹲状态，根据不同状态对定位信息进行更新优化，定位装置通过无线传输模块发送端13将更新优化过后的标签坐标发送给主控制器7。

所述主控制器7以平台底盘建立坐标系，对更新优化后的标签坐标投影到坐标系平面，得到工作人员在平台底盘平面的投影坐标，计算出投影坐标到平台底盘的几何中心距离D_R。

比较计算值与设定值，若当前工作人员距离D_R小于设定跟随距离D_S，则保持平台静止，如果当前工作人员距离D_R大于设定跟随距离D_S，则结合障碍物检测装置的信息，利用改进的人工势场法进行路径规划，图5为改进的人工势场法示意图，利用模糊控制法控制左、右电机3实现实时跟随，图6为模糊控制原理图。

一种根据所述多传感器融合的温室自动跟随平台的控制方法，具体的，包括以下步骤：

平台运动学模型的建立：建立平台运动学模型，将运动学模型离散化处理并导入到主控制器7中，平台开关打开后，主控制器7进入工作模式选择界面，选择遥控模式或跟随模式；

遥控模式下：当选择遥控模式时，在遥控模式下所述蓝牙模块5接收来自移动通信设备的指令，并将该指令发送给主控制器7，主控制器7对相应指令解析，通过定时器产生PWM波控制左、右电机3调速，从而控制平台的履带底盘的行驶；

跟随模式下：当选择跟随模式时，工作人员佩戴腰带8，通过定位控制器12设定跟随距离D_S，定位装置的UWB标签9利用TWR双边测距法开始采集工作人员与平台之间的距离信息并将其发送给定位控制器12，定位控制器12对测距信息进行卡尔曼滤波，运用三边定位算法和最小二乘法对工作人员进行定位，利用腰带8上的六轴陀螺仪10检测工作人员是否处于转身状态，高度传感器11检测工作人员是否下蹲状态，根据工作人员不同状态对定位信息进行更新优化，定位装置通过无线传输模块发送端13将更新优化过后的标签坐标发送给主控制器7，主控制器7以平台底盘建立坐标系，对更新优化后的标签坐标投影到坐标系平面，得到工作人员在平台底盘平面的投影坐标，计算出投影坐标到平台底盘的几何中心距离D_R，比较计算值与设定值，若当前工作人员距离D_R小于设定跟随距离D_S，则保持平台静止，如果当前工作人员距离D_R大于设定跟随距离D_S，则结合障碍物检测装置的信息，利用改进的人工势场法进行路径规划，利用模糊控制法控制电机3从而控制平台履带底盘的行驶，实现实时跟随。

上述方案中，所述平台运动学模型建立中，对模型进行离散化处理如下：

Px_(t+1)＝Px_(t)+VT_scosθ

Py_(t+1)＝Py_(t)+VT_ssinθ

θ_(t+1)＝θ_(t)+W(k)T_s

其中，V为平台中心处的线速度；

W为平台中心处的角速度；

V_r为平台的履带底盘的右轮线速度，V_l为平台的履带底盘的左轮线速度；

θ为方向角，即平台旋转的角度；

L为平台底盘宽度；

Px_(t)、Py_(t)为上一时刻坐标，Px_(t+1)、Py_(t+1)、为下一时刻坐标；

T_s为两时刻间隔时间；

θ_(t)为什么上一时刻方向角，θ_(t+1)为下一时刻方向角。

TWR双边测距流程如下：)

由于四基站测距原理相同，以基站一为例说明，首先由标签向基站发出信息并记录发出信息时刻T1,基站在T2时刻接收到信息，然后在T3时刻返回一段信息给标签，标签在T4时刻收到信息并在T5时刻再次返回信息，基站在T6时刻接收到信息。则测距计算如下：

D＝T*C

其中，D为标签和基站的距离；T为信号飞行时间，C为光速；

卡尔曼滤波流程如下：

状态预测方程：

状态更新方程：

上式中：

为上一时刻的测距距离，由uwb基站1、标签9上一时刻测得，

为下一时刻的预估距离，设定为固定值，

为当前时刻真实距离；

F_t为状态转移矩阵，设定为固定值，B_t为控制矩阵，设定为固定值，u_t为控制量即人携带标签走的距离，为当前时刻uwb基站、标签测得距离与上一时刻距离差值；

表示当前时刻平台不确定性，设定为固定值，为协方差矩阵，设定为固定值；

Q为预测模型的噪声，设定为固定值；

Z_t为UWB基站1测量的距离值；

H表示实际距离x_t和测量距离Z_t相关关系，设定为固定值；

Y表示测量噪声，设定为固定值，R为测量噪声的协方差矩阵，设定为固定值；

K_t为卡尔曼系数，设定为固定值；

P_t是下一时刻的平台不确定性，设定为固定值；

x_t为实际距离，Z_t为uwb基站1、uwb标签9测得的当前时刻测距距离；

由

上一时刻的测距距离，和Z_t当前时刻测距距离代入以上方程可得，x_t实际距离；三边定位算法如下：

已知四基站坐标A₁(x₁,y₁,z₁),A₂(x₂,y₂,z₂),A₃(x₃,y₃,z₃),A₄(x₄,y₄,z₄)和各个基站距标签A₀距离d₁,d₂,d₃,d₄。利用最小二乘法可求得标签坐标A₀(x₀,y₀,z₀)，具体如下：

利用第四个方程减去前三个方程得到如下：

AX＝b

其中：

l_i ²＝x_i ²+y_i ²+z_i ²；(i取1、2、3、4)l²为x_i、y_i、z_i的平方和；

所述对定位信息进行更新优化具体为：设标签的坐标A₀(X₀,Y₀,Z₀)，利用工作人员目前

状态信息对标签坐标A₀(X₀,Y₀,Z₀)更新为(X′₀,Y′₀,Z′₀)：

其中b、c为固定值，α由六轴陀螺仪10测量工作人员腰带8初始位置与当前工作人员腰带8位置的角度、H由高度传感器11测量的腰带8距离地面的高度；

将更新过的坐标(X′₀,Y′₀,Z′₀)进行投影到平台平面，得到坐标(X″₀,Y″₀),计算出投影坐标(X″₀,Y″₀)到坐标原点的距离D_R：

比较计算值与设定值，若当前工作人员距离D_R小于设定跟随距离D_S，则保持平台静止，

如果当前工作人员距离D_R大于设定跟随距离D_S，则结合障碍物检测装置，利用改进的人工势场法进行路径规划，利用模糊控制对工作人员完成实时跟随。

上述方案中，所述改进的人工势场法如下：

传统人工势场法引力势场函数U_att和引力F_att分别为：

U_att＝k_attP(q,q_g)

F_att＝k_attP(q,q_g)

其中k_att为引力增益系数；

P(q,q_g)为一个矢量，方向是平台位置指向目标点位置，表示平台当前位置q与目标点位置q_g之间的欧几里德距离|q-q_g|；

传统人工势场法斥力势场函数U_req和斥力F_req分别为：

其中k_req为斥力增益系数，设置为固定值；P(q,q₀)为一个矢量，方向是障碍物指向平台，表示平台当前位置q与障碍物位置q₀之间的欧几里德距离|q-q₀|；p₀为一个常数，表示障碍物对平台产生作用的最大距离；

合力势场U和合力F分别为：

其中，i为第几个斥力，n为斥力总个数

由于传统人工势场法易出现局部最优解、目标点不可达问题，为了解决该问题，对传统人工势场法进行改进：首先改进斥力场函数，并结合作业环境建立道路边界斥力场从而限制平台的行驶区域，改进障碍物的斥力场，并对人工势场法具体改进如下：

根据作业环境建立道路边界斥力场F_road，斥力如下：

其中，d_r为平台距离左侧道路边界距离，d_l为平台距离右侧边界距离，d₀为设定值；k_road为道路边界斥力增益系数；

道路边界斥力场F_road方向为垂直于左右路面指向平台，为正时指向右侧，为负时指向左侧；

对障碍物的斥力场F_reqnew进行改进如下：

其中，F_req1由障碍物指向平台的斥力场，F_req2由平台指向目标点的斥力场，P(q,q_g)为一个矢量，方向是平台位置指向目标点位置，表示平台当前位置q与目标点位置q_g之间的欧几里德距离|q-q_g|，p₀为一个常数，表示障碍物对平台产生作用的最大距离；k_reqnew、k_req1、k_req1分别为斥力F_reqnew、F_req1、F_req2的增益系数，设定为固定值。

改进后的合力F_ff如下：

F_att＝k_attP(q,q_g)

其中，k_att为引力增益系数，设定为固定值，，i为第几个斥力，n为斥力总个数。

上述方案中，所述利用模糊控制法控制电机3从而控制平台履带底盘的行驶的步骤如下：

已知改进后的人工势场法计算的合力的大小和方向，运用模糊控制，以人工势场法合力的大小和方向作为模糊控制输入，下一时刻平台位置坐标为模糊控制的输出，主控制器7结合运动学模型求解左右轮速度，控制左右电机3从而控制平台履带底盘的行驶，实现平台的自动跟随效果。

应当理解，虽然本说明书是按照各个实施例描述的，但并非每个实施例仅包含一个独立的技术方案，说明书的这种叙述方式仅仅是为清楚起见，本领域技术人员应当将说明书作为一个整体，各实施例中的技术方案也可以经适当组合，形成本领域技术人员可以理解的其他实施方式。

上文所列出的一系列的详细说明仅仅是针对本发明的可行性实施例的具体说明，它们并非用以限制本发明的保护范围，凡未脱离本发明技艺精神所作的等效实施例或变更均应包含在本发明的保护范围之内。

Claims (10)

1.一种多传感器融合的温室自动跟随平台，其特征在于，包括定位装置、障碍物检测装置、主控制器(7)、平台执行装置和辅助通讯模块；

所述定位装置包括UWB基站(1)和腰带(8)；所述可穿戴式腰带(8)上设有UWB标签(9)、六轴陀螺仪(10)、高度传感器(11)、定位控制器(12)、无线传输模块发送端(13)和移动电源(14)；UWB基站(1)安装在平台的四个角，所述UWB标签(9)与定位控制器(12)连接，六轴陀螺仪(10)与定位控制器(12)连接，无线传输模块发送端(13)与定位控制器(12)连接，高度传感器(11)与定位控制器(12)连接，UWB标签(9)、六轴陀螺仪(10)、高度传感器(11)、定位控制器(12)、无线传输模块发送端(13)还分别与移动电源(14)连接；

所述障碍物检测装置与主控制器(7)相连，用于检测平面障碍物，并传递给主控制器(7)；

所述平台执行装置包括电机(3)和电机驱动器(4)，电机驱动器(4)分别与电机(3)和主控制器(7)相连，电机(3)与平台的履带底盘连接，驱动履带底盘的行驶；

所述辅助通讯模块包括蓝牙模块(5)、无线传输模块接收端(6)和移动通讯设备，蓝牙模块(5)和主控制器(7)连接、蓝牙模块(5)与移动通讯设备通讯，无线传输模块接收端(6)和主控制器(7)连接；

所述平台主控制器(7)接收来自无线传输模块接收端(6)、蓝牙模块(5)、二维激光雷达(2)的信息并处理，主控制器(7)设置有遥控模式和跟随模式，当选择遥控模式时，主控制器(7)根据移动通讯设备的指令控制直流无刷电机(3)实现平台履带底盘行驶，当选择跟随模式时，主控制器(7)通过路径规划和模糊控制算法控制平台履带底盘行驶。

2.根据权利要求1所述的多传感器融合的温室自动跟随平台，其特征在于，所述UWB基站(1)的数量为四个，四个UWB基站(1)呈长方形安装在平台四个角落，其中三个UWB基站(1)安装于同一水平面，另外一个UWB基站(1)的位置高于该水平面。

3.根据权利要求1所述的多传感器融合的温室自动跟随平台，其特征在于，所述电机(3)的数量为两个，电机(3)为带霍尔传感器的直流无刷电机。

4.根据权利要求1所述的多传感器融合的温室自动跟随平台，其特征在于，所述定位控制器(12)为STM32F103核心板；所述平台主控制器(7)为STM32H745。

5.根据权利要求1所述的多传感器融合的温室自动跟随平台，其特征在于，所述障碍物检测装置为二维激光雷达(2)，所述二维激光雷达(2)与主控制器(7)相连。

6.一种根据权利要求1-5任意一项所述多传感器融合的温室自动跟随平台的控制方法，其特征在于，包括以下步骤：

平台运动学模型的建立：建立平台运动学模型，将运动学模型离散化处理并导入到主控制器(7)中，平台开关打开后，主控制器(7)进入工作模式选择界面，选择遥控模式或跟随模式；

遥控模式下：当选择遥控模式时，在遥控模式下所述蓝牙模块(5)接收来自移动通信设备的指令，并将该指令发送给主控制器(7)，主控制器(7)对相应指令解析，通过定时器产生PWM波控制左、右电机(3)调速，从而控制平台的履带底盘的行驶；

跟随模式下：当选择跟随模式时，工作人员佩戴腰带(8)，通过定位控制器(12)设定跟随距离D_S，定位装置的UWB标签(9)利用TWR双边测距法开始采集工作人员与平台之间的距离信息并将其发送给定位控制器(12)，定位控制器(12)对测距信息进行卡尔曼滤波，运用三边定位算法和最小二乘法对工作人员进行定位，利用腰带(8)上的六轴陀螺仪(10)检测工作人员是否处于转身状态，高度传感器(11)检测工作人员是否下蹲状态，根据工作人员不同工作状态对定位信息进行更新优化，定位装置通过无线传输模块发送端(13)将更新优化过后的标签坐标发送给主控制器(7)，主控制器(7)以平台底盘建立坐标系，对更新优化后的标签坐标投影到坐标系平面，得到工作人员在平台底盘平面的投影坐标，计算出投影坐标到平台底盘的几何中心距离D_R，比较计算值与设定值，若当前工作人员距离D_R小于设定跟随距离D_S，则保持平台静止，如果当前工作人员距离D_R大于设定跟随距离D_S，则结合障碍物检测装置的信息，利用改进的人工势场法进行路径规划，利用模糊控制法控制电机(3)从而控制平台履带底盘的行驶，实现实时跟随。

7.根据权利要求6所述多传感器融合的温室自动跟随平台的控制方法，其特征在于，所述平台运动学模型建立中，对模型进行离散化处理如下：

Px_(t+1)＝Px_(t)+VT_scosθ

Py_(t+1)＝Py_(t)+VT_ssinθ

θ_(t+1)＝θ_(t)+WT_s

其中，V为平台中心处的线速度；

W为平台中心处的角速度；

V_r为平台的履带底盘的右轮线速度，V_l为平台的履带底盘的左轮线速度；

θ为方向角，即平台旋转的角度；

L为平台底盘宽度；

Px_(t)、Py_(t)为上一时刻坐标，Px_(t+1)、Py_(t+1)、为下一时刻坐标；

T_s为两时刻间隔时间；

θ_(t)为什么上一时刻方向角，θ_(t+1)为下一时刻方向角。

8.根据权利要求6所述多传感器融合的温室自动跟随平台的控制方法，其特征在于，所述对定位信息进行更新优化具体为：设标签的坐标A₀(X₀，Y₀，Z₀)，利用工作人员目前状态信息对标签坐标A₀(X₀，Y₀，Z₀)更新为(X′₀，Y′₀，Z′₀)：

其中b、c为固定值，α由六轴陀螺仪(10)测量工作人员腰带(8)初始位置与当前工作人员腰带(8)位置的角度、H由高度传感器(11)测量的腰带(8)距离地面的高度；

将更新过的坐标(X′₀，Y′₀，Z′₀)进行投影到平台平面，得到坐标(X″₀，Y″₀)，计算出投影坐标(X″₀，Y″₀)到坐标原点的距离D_R：

9.根据权利要求6所述多传感器融合的温室自动跟随平台的控制方法，其特征在于，所述改进的人工势场法如下：

首先改进斥力场函数，并结合作业环境建立道路边界斥力场从而限制平台的行驶区域，改进障碍物的斥力场，并对人工势场法具体改进如下：

根据作业环境建立道路边界斥力场F_road，斥力如下：

其中，d_r为平台距离左侧道路边界距离，d_l为平台距离右侧边界距离，d₀为设定值；k_road为道路边界斥力增益系数；

道路边界斥力场F_road方向为垂直于左右路面指向平台，为正时指向右侧，为负时指向左侧；

对障碍物的斥力场F_reqnew进行改进如下：

其中，F_req1由障碍物指向平台的斥力场，F_req2由平台指向目标点的斥力场，P(q，q_g)为一个矢量，方向是平台位置指向目标点位置，表示平台当前位置q与目标点位置q_g之间的欧几里德距离|q-q_g|，p₀为一个常数，表示障碍物对平台产生作用的最大距离；k_reqnew、k_req1、k_req1分别为斥力F_reqnew、F_req1、F_req2的增益系数，设定为固定值。改进后的合力F_ff如下：

其中，F_att＝k_attP(q，q_g)，k_att为引力增益系数，设定为固定值，i为第几个斥力，n为斥力总个数。

10.根据权利要求7所述多传感器融合的温室自动跟随平台的控制方法，其特征在于，所述利用模糊控制法控制电机(3)从而控制平台履带底盘的行驶的步骤如下：

已知改进后的人工势场法计算的合力的大小和方向，运用模糊控制，以人工势场法合力的大小和方向作为模糊控制输入，下一时刻平台位置坐标为模糊控制的输出，主控制器(7)结合运动学模型求解左右轮速度，控制左右电机(3)从而控制平台履带底盘的行驶，实现平台的自动跟随效果。

Images