CN113960921B - 果园履带车辆视觉导航控制方法与系统 - Google Patents

Abstract

本发明公开了一种果园履带车辆视觉导航控制方法与系统，利用微软Kinect cv2相机拍摄果园路况图像，将之传入语义分割模型中，生成路况掩码区并保存对应特征；从保存的路况掩码区中提取左右侧边缘信息点并拟合一系列中心点坐标；通过给定像素坐标权重表，加权平均得到每一幅图像的偏差；基于PID控制算法，设计模糊PID控制器；基于MATLAB软件的Simulink仿真环境，建立果园视觉导航控制的仿真模型；根据仿真结果，得出模糊PID控制器的输出量，将输出量作为电机的控制量，从而实现果园履带车辆自主行驶，具有能够准确的引导果园车辆在果园路径中进行行驶的优点。本发明简化了算法的计算能力，节省大量的图像处理时间，工作效率高，因此本发明方法适合在一些路径不规范的果园中使用。

Description

果园履带车辆视觉导航控制方法与系统

技术领域

本发明涉及果园机械控制的技术领域，尤其是指一种基于模糊PID和MATLAB仿真的果园履带车辆视觉导航控制方法与系统。

背景技术

现阶段果园大部分生产环节仍以人工作业为主，效率低、强度大、耗时费力，加之劳动力日益短缺,果园面临无人可用的生产管理困境，不能及时完成割草、施肥、采收等作业。据统计我国果园土壤整耕机械综合作业水平仅为31.2％，病虫防治综合机械作业水平仅24.8％，果品收获综合机械作业水平仅7.6％，综合机械化水平仅24.75％。此外，我国果树种植主要集中在丘陵地区，果园生产条件较差，农机农艺结合不紧密的问题广泛存在，使得我国果园机械化发展相对滞后。因此针对我国劳动力不足、果园机械化水平落后等现状,运用先进理论和技术手段开发适合果园作业的农业装备势在必行。

近年来，随着我国产业结构的不断优化和升级，果园机械化与智能化成为未来发展的必然趋势，自主导航技术更是当前国内外学者的研究重点，全球导航卫星系统GNSS、机器视觉、激光雷达、毫米波雷达、多传感器融合等都已经应用于各项农业作业活动中。但是相比GNSS导航系统和激光导航，机器视觉导航备受学者的青睐。机器视觉导航技术，它是基于数字图像处理的综合技术，具有灵活性大、信息获取丰富，对导航路径进行可靠、稳定地识别等优点，因此视觉导航在果园机械的自主导航领域领域还有很大发展空间。

传统导航控制方法都是以机器人运动的轨迹横向偏差和航向偏差作为输入量，经过路径追踪算法处理后输出机器人的转向指令，从而控制机器人沿着期望的路径运动。虽然可以达到要求，但是田间试验调试难度大，控制参数可调节范围小，容易导致车辆失控，无法完全达到安全的自主导航行驶。

发明内容

本发明的第一目的在于克服现有技术的缺点与不足，提出一种基于模糊PID和MATLAB仿真的果园履带车辆视觉导航控制方法，通过该方法能够准确地引导果园履带车辆在果园路径中进行行驶。

本发明的第二目的在于提出一种基于模糊PID和MATLAB仿真的果园履带车辆视觉导航控制系统。

本发明的第一目的通过下述技术方案实现：基于模糊PID和MATLAB仿真的果园履带车辆视觉导航控制方法，包括以下步骤：

S1、利用放置在果园履带车辆上的微软Kinect cv2相机进行数据采集，拍摄果园路况图像，通过USB数据线传入并存储于放置在果园履带车辆上的笔记本电脑中；

S2、将果园路况图像传入语义分割模型中，生成路况掩码区并保存对应特征，从保存的路况掩码区中提取左右侧边缘信息点，利用左右侧边缘信息点的特征拟合一系列中心点坐标；

S3、基于加权平均的方法提取图像偏差；其中，给定像素坐标权重表，让每一个中心点坐标乘以对应的权重，加权平均得到每一幅图像的偏差；

S4、基于图像偏差，利用PID控制算法，设计模糊PID控制器；其中，包括模糊比例、模糊积分和模糊微分，三者线性相加得到输出量；

S5、利用MATLAB软件的Simulink仿真环境，建立果园视觉导航控制的仿真模型，检测模糊PID控制器的仿真结果以及路径跟随效果；

S6、根据仿真结果，得出模糊PID控制器的输出量，通过放大器，将放大后的输出量作为电机的控制量，从而实现果园履带车辆自主行驶。

进一步，在步骤S1，微软Kinect cv2相机通过支架安装于果园履带车辆的上盖，横向位于果园履带车辆中轴线，拍摄方向为机体前方；通过施加固定的PWM脉冲于果园履带车辆的控制电机，让果园履带车辆匀速行驶，模拟果园履带车辆的正常作业状态；果园履带车辆在果园行驶过程中，微软Kinect cv2相机进行图像采集，拍摄果园路况图像；微软Kinectcv2相机通过USB数据线与放置在果园履带车辆上的笔记本连接，在果园履带车辆匀速行驶过程中，及时将采集的果园路况图像传入并存储于笔记本电脑中。

进一步，所述步骤S2包括以下步骤：

S201、在Win10 64位操作系统下，搭建并训练语义分割模型；训练前设置合理的模型参数，多次训练，使预测模型的验证准确率提高，从而得到成功的语义分割模型；

S202、将步骤S1中得到的图像传入语义分割模型，识别果园路况特征，将图像中周围背景与果园路况区域分离，得到需要的路况掩码区域，最后将对应的路况掩码区域保存；

S203、对路况掩码区域进行二值化，滤波处理；基于掩码区域边界提取左右侧边缘信息点，将左右边界对应的坐标求和平均得到拟合一系列中心点坐标。

进一步，所述步骤S3包括以下步骤：

S301、设置像素坐标权重表，对于不同的图像中心点坐标需要进行取舍，视觉图像存在畸变，远离图像坐标系原点的图像特征需要舍去，图像坐标系原点近端的图像特征需要保留，从而让像素坐标权重表前面数据数值大，后面数据数值小或者为零；

S302、基于像素坐标权重表和图像中心点坐标，采用加权平均的方式求得每幅图像的计算平均中心点；其中，所述像素坐标权重表和图像中心点坐标都是一维数组，两者的数据个数相同，像素坐标权重表的每一个数据都需要乘以图像中心点坐标的每一个数据，所得结果相加并除以像素坐标权重表的数据和，从而得到每幅图像的计算平均中心点；

每幅图像的计算平均中心点表达式为：

其中，为每幅图像的计算平均中心点，w[i]表示像素坐标权重表第i个数据，x[i]表示图像中心点中第i个横坐标数据，y[i]表示图像中心点中第i个纵坐标数据，n表示像素坐标权重表中数据的总个数；

S303、对于每一幅图像都有其理论中心坐标，将图像的计算平均中心点和理论中心坐标对应相减，即可得到每一幅图像的图像偏差，处理完一幅图像，就能得到一组偏差，随后将之保存；

每一幅图像的图像偏差的表达式为：

其中，(x_m,y_m)为每一幅图像的理论中心坐标，为每幅图像的计算平均中心点，x_e为横坐标偏差，y_e为纵坐标偏差。

进一步，所述步骤S4包括以下步骤：

S401、图像纵坐标偏差与给定的目标期望值构造控制偏差，利用PID控制算法，设计PID控制器，通过比例、积分和微分三部分线性组合以形成控制变量并控制，具体为：

其中，e_t作为PID控制器的输入，u_t作为PID控制器的输出，K_p为控制器的比例系数，为控制器的积分部分，T_i为控制器的积分系数，/>为控制器的微分部分，T_d为控制器的微分系数；

S402、基于PID控制器，设计模糊PID,使比例系数K_p、积分系数T_i和微分系数T_d三部分模糊化；其中，模糊化包括以下步骤：

S4021、根据2维模糊控制器的原理，将图像纵坐标偏差y_e分为13个等级，其量化等级为：{-6,-5,-4,-3,-2,-1,0,1,2,3,4,5,6}；将图像纵坐标偏差变化率d_ye分为13个等级，其量化等级为：{-6,-5,-4,-3,-2,-1,0,1,2,3,4,5,6}，其中d_ye＝y_e2-y_e1，y_e2为当前的图像纵坐标偏差，y_e1为上次的图像纵坐标偏差；

S4022、定义图像纵坐标偏差y_e的模糊集合为：左大LB、左中LM、左小LS、零Z0、右小RS、右中RM、右大RB；图像纵坐标偏差变化率d_ye模糊集合为：左大LB、左中LM、左小LS、零Z0、右小RS、右中RM、右大RB；输入变量y_e、d_ye均选用三角型隶属度函数；

S4023、确定模糊控制规则，根据PID控制器调试经验可知，当图像纵坐标偏差y_e和纵坐标偏差变化率d_ye取值大时，使用小的比例系数K_p、积分系数T_i和微分系数T_d，使快速逼近期望目标值，减小调节时间，提高响应速度；当图像纵坐标偏差y_e和纵坐标偏差变化率d_ye取值小时，使用大的比例系数K_p、积分系数T_i和微分系数T_d，防止超调，提高稳定性；

S4024、通过重心法反模糊化，将模糊规则转换为驱动器或执行机构能够理解的语言，设计比例系数K_p、积分系数T_i和微分系数T_d的模糊控制查询表。

进一步，所述步骤S5包括以下步骤：

S501、基于MATLAB软件搭建Simulink仿真模型，在Simulink可视化仿真环境下构建导航车辆控制模型，模型函数使用MATLAB语言编写，完成相关参数之间的连线并保存；

S502、利用模糊逻辑工具箱搭建模糊PID控制器,模糊PID控制器参数以及过程按照步骤S4，完成相关参数之间的连线并保存；

S503、运行仿真，在脚本编辑器中设置仿真参数，其中车辆速度设置为0.5m/s，模糊PID控制器的比例系数K_p、积分系数T_i和微分系数T_d赋初始值，仿真周期为100s,选择直线和曲线两种跟踪路径，在MATLAB环境和当前路径下开始运行，检测模糊PID控制器的仿真结果以及路径跟随效果。

本发明的第二目的通过下述技术方案实现：基于模糊PID和MATLAB仿真的果园履带车辆视觉导航控制系统，包括：

图像接收模块，用于笔记本电脑实时接收微软Kinect cv2相机采集的图像数据；其中微软Kinect cv2相机固定在果园履带车辆上，通过USB数据线和笔记本电脑通讯，电脑端通过调用Python程序将图像数据以图片格式保存下来，并且设置时间间隔，让微软Kinect cv2相机每隔设定的采样时间保存一幅果园路况图像；

图像偏差处理模块，基于语义分割模型，识别果园路况特征，得到需要的路况掩码区域；在路况掩码区域中基于边界提取左右侧边缘信息点，将左右边界对应的坐标求和平均得到一系列图像中心点坐标；基于坐标权重表和图像中心点坐标，采用加权平均的方式求得每幅图像的计算平均中心点；将图像的计算平均中心点和理论中心坐标对应相减，即可得到每一幅图像的偏差；

模糊PID控制器模块，用于控制果园履带车辆的运动状态；其中，模糊PID控制器模块中的模糊PID控制器的输入为图像纵坐标偏差y_e和纵坐标偏差变化率d_ye，输出为控制量；通过将输入量分为13个等级，定义模糊集合，确定模糊控制规则过程，使得比例系数K_p、积分系数T_i和微分系数T_d模糊化，扩大控制器调节的范围，最后通过重心法反模糊化，将模糊规则转换为驱动器或执行机构能够理解的语言，达到最终的控制要求；

MATLAB仿真模块，搭建仿真模型，用于验证模糊PID控制器的效果；在Win10 64位操作系统下，利用MATLAB软件的Simulink可视化模块，基于模糊逻辑工具箱搭建模糊PID控制器，仿真前设置模型初始参数，其中包括车辆速度、比例系数K_p、积分系数T_i、微分系数T_d和仿真周期；选择直线和曲线两种跟踪路径，在MATLAB环境和当前路径下开始运行，检测模糊PID控制器的仿真结果以及路径跟随效果；仿真过程中，模糊PID控制器三个参数根据图像纵坐标偏差y_e和纵坐标偏差变化率d_ye的大小合理选择数值，打破传统PID控制器参数固定的局面，达到最佳的控制目标；

实地试验验证模块，利用果园履带车辆测试系统的精度和稳定性；将模糊PID控制器的控制程序移植到STM32硬件设备中，通过输入PWM脉宽信号，利用反馈控制直流电机转动；微软Kinect cv2相机拍摄的图像经过处理后得到输出量，经过放大器作用和STM32硬件设备PWM端口，将处理后的输出量以PWM脉宽信号的方式传入直流电机，作为直流电机的控制量，果园履带车辆左电机加上控制量，右电机减去控制量，从而实现左右电机以不同转速运转，达到电子差速的效果，完成果园视觉导航。

本发明与现有技术相比，具有如下优点与有益效果：

1、本发明方法利用微软Kinect cv2相机拍摄果园路况图像，将之传入语义分割模型中，生成路况掩码区并保存对应特征；从保存的路况掩码区中提取左右侧边缘信息点并拟合一系列中心点坐标；通过给定像素坐标权重表，加权平均得到每一幅图像的偏差；基于PID控制算法，设计模糊PID控制器，包括模糊比例，模糊积分和模糊微分，三者线性相加得到输出量；基于MATLAB软件的Simulink仿真环境，建立果园视觉导航控制的仿真模型；根据MATLAB仿真结果，得出模糊PID控制器的输出量，经过放大器作用，将输出量作为电机的控制量，从而实现果园履带车辆自主行驶，具有能够准确的引导果园车辆在果园路径中进行行驶的优点。

2、本发明方法在图像处理方面，基于语义分割模型和加权平均就得到图像偏差，提高了复杂多变的果园路况图像的识别率，简化了算法的计算能力，节省大量的图像处理时间，工作效率高，因此本发明方法适合在一些路径不规范的果园中使用。

3、本发明方法设计了模糊PID控制器，实现参数模糊化，扩大参数调节范围，改变传统PID控制器调试困难的局面，提高了控制器的准确性和稳定性，进一步提高果园视觉导航的鲁棒性。

4、本发明系统包括图像接收模块、图像偏差处理模块、模糊PID控制器模块、MATLAB仿真模块、实地试验验证模块；其中图像接收模块获得Kinect cv2相机采集的图像数据，传送到图像偏差处理模块加工处理，得到图像偏差，然后作为模糊PID控制器模块的输入量，经过模糊PID控制算法计算得到控制量，最后通过MATLAB仿真和实地试验验证模块，利用STM32硬件设备测试系统的准确性，本发明系统具有结构简单的优点，提高了导航的工作效率，为农业机器人在果园中导航行驶提供参考。

附图说明

图1为本发明方法的流程图。

图2为本发明系统的架构图。

图3为本发明中模糊PID控制器原理图。

图4为本发明图像偏差隶属函数图。

图5为本发明图像偏差变化率隶属函数图。

图6为本发明输出量隶属函数图。

图7为本发明软件系统设计流程图。

具体实施方式

下面结合实施例及附图对本发明作进一步详细的描述，但本发明的实施方式不限于此。

参见图1所示，本实施例所提供的基于模糊PID和MATLAB仿真的果园履带车辆视觉导航控制方法，包括以下步骤：

S1、利用微软Kinect cv2相机进行数据采集，拍摄果园路况图像，通过USB数据线传入并存储于笔记本电脑中；在本实施例中，用于拍摄果园果园路况图像的相机通过棋盘标定板进行校准标定，针对该相机的多种功能，本实施例只使用RGB彩色图像的工作模式；

微软Kinect cv2相机通过支架安装于果园履带车辆的上盖，横向位于果园履带车辆中轴线，拍摄方向为机体前方；通过施加固定的PWM脉冲于果园履带车辆的控制电机，让果园履带车辆匀速行驶，模拟果园履带车辆的正常作业状态；果园履带车辆在果园行驶过程中，微软Kinect cv2相机进行图像采集，拍摄果园路况图像；微软Kinect cv2相机通过USB数据线与放置在果园履带车辆上的笔记本连接，在果园履带车辆匀速行驶过程中，及时将采集的果园路况图像传入并存储于笔记本电脑中。

S2、将果园路况图像传入语义分割模型中，生成路况掩码区并保存对应特征。从保存的路况掩码区中提取左右侧边缘信息点，利用左右侧边缘信息点的特征拟合一系列中心点坐标；包括以下步骤：

S201、在Win10 64位操作系统下，搭建并训练语义分割模型；训练前设置合理的模型参数，多次训练，使预测模型的验证准确率提高，从而得到成功的语义分割模型。

S202、将步骤S1中得到的图像传入语义分割模型，识别果园路况特征，将图像中周围背景与果园路况区域分离，得到需要的路况掩码区域，最后将对应的路况掩码区域保存。

S203、对路况掩码区域进行二值化，滤波处理；基于掩码区域边界提取左右侧边缘信息点，将左右边界对应的坐标求和平均得到拟合一系列中心点坐标。

S3、基于加权平均的方法提取视觉图像偏差；其中，给定像素坐标权重表，让每一个中心点坐标乘以对应的权重，加权平均得到每一幅图像的偏差；包括以下步骤：

S301、设置像素坐标权重表，对于不同的图像中心点坐标需要进行取舍。视觉图像存在畸变，远离图像坐标系原点的图像特征需要舍去，图像坐标系原点近端的图像特征需要保留，从而让像素坐标权重表前面数据数值大，后面数据数值小或者为零。

S302、基于像素坐标权重表和图像中心点坐标，采用加权平均的方式求得每幅图像的计算平均中心点；其中，所述的像素坐标权重表和图像中心点坐标都是一维数据，两者的数据个数相同，像素坐标权重表的每一个数据都需要乘以图像中心点坐标的每一个数据，所得结果相加并除以像素坐标权重表的数据和，从而得到每幅图像的计算平均中心点。

每幅图像的计算平均中心点表达式为：

其中为每幅图像的计算平均中心点，w[i]表示像素坐标权重表第i个数据，x[i]表示图像中心点中第i个横坐标数据，y[i]表示图像中心点中第i个纵坐标数据，n表示像素坐标权重表中数据的总个数。

S303、对于每一幅图像都有其理论中心坐标，将图像的计算平均中心点和理论中心坐标对应相减，即可得到每一幅图像的图像偏差。处理完一幅图像，就能得到一组偏差，随后将之保存。

每一幅图像的图像偏差的表达式为：

其中(x_m,y_m)为每一幅图像的理论中心坐标，为每幅图像的计算平均中心点，x_e为横坐标偏差，y_e为纵坐标偏差。

S4、基于PID控制算法，设计模糊PID控制器；其中，包括模糊比例，模糊积分和模糊微分，三者线性相加得到输出量；包括以下步骤：

S401、图像纵坐标偏差与给定的目标期望值构造控制偏差，利用PID控制算法，设计PID控制器，如图3所示。通过比例、积分和微分三部分线性组合以形成控制变量并控制，具体为：

其中，e_t作为PID控制器的输入，u_t作为PID控制器的输出，K_p为控制器的比例系数，为控制器的积分部分，T_i为控制器的积分系数，/>为控制器的微分部分，T_d为控制器的微分系数。

S402、基于PID控制器，设计模糊PID,使比例系数K_p、积分系数T_i和微分系数T_d三部分模糊化；其中，模糊化包括以下步骤：

S4021、根据2维模糊控制器的原理，将图像纵坐标偏差y_e分为13个等级，其量化等级为：{-6,-5,-4,-3,-2,-1,0,1,2,3,4,5,6}；将图像纵坐标偏差变化率d_ye分为13个等级，其量化等级为：{-6,-5,-4,-3,-2,-1,0,1,2,3,4,5,6}，其中d_ye＝y_e2-y_e1，y_e2为当前的图像纵坐标偏差，y_e1为上次的图像纵坐标偏差。

S4022、定义图像纵坐标偏差y_e的模糊集合为：左大(LB)、左中(LM)、左小(LS)、零(Z0)、右小(RS)、右中(RM)、右大(RB)；图像纵坐标偏差变化率d_ye模糊集合为：左大(LB)、左中(LM)、左小(LS)、零(Z0)、右小(RS)、右中(RM)、右大(RB)；输入变量y_e、d_ye均选用三角型隶属度函数，最后的结果如图4、图5所示，模糊输出量的三角型隶属度函数如图6所示。

S4023、确定模糊控制规则，根据PID控制器调试经验可知，当图像纵坐标偏差y_e和纵坐标偏差变化率d_ye取值较大时，使用较小的比例系数K_p、积分系数T_i和微分系数T_d，使快速逼近跟踪路径，减小系统调节时间，提高系统响应速度；当图像纵坐标偏差y_e和纵坐标偏差变化率d_ye取值较小时，使用较大的比例系数K_p、积分系数T_i和微分系数T_d，防止超调，提高稳定性；本实施例中，经过多次仿真试验，确定了最优的49条模糊控制规则，模糊控制规则如表1所示：

表1模糊控制规则表

S4024、通过重心法反模糊化，将模糊规则转换为驱动器或执行机构可以理解的语言，设计比例系数K_p、积分系数T_i和微分系数T_d的模糊控制查询表。

S5、利用MATLAB软件的Simulink仿真环境，建立果园视觉导航控制系统的仿真模型；包括以下步骤：

S501、基于MATLAB软件搭建Simulink仿真模型，在Simulink可视化仿真环境下构建导航车辆控制模型，模型函数使用MATLAB语言编写，完成相关参数之间的连线并保存。

S502、利用模糊逻辑工具箱(Fuzzy Logic Toolbox)搭建模糊PID控制器,模糊PID控制器参数以及过程按照步骤S4，完成相关参数之间的连线并保存。

S503、运行仿真，在脚本编辑器中设置仿真参数，其中车辆速度设置为0.5m/s，模糊PID控制器的比例系数K_p、积分系数T_i和微分系数T_d赋初始值，仿真周期为100s,选择直线和曲线两种跟踪路径，在MATLAB环境和当前路径下开始运行，检测模糊PID控制器的仿真结果以及路径跟随效果。

S6、根据MATLAB仿真结果，得出模糊PID控制器的输出量，经过放大器作用，将输出量作为电机的控制量，从而实现果园履带车辆自主行驶。

参见图2所示，本实施例也提供了一种基于模糊PID和MATLAB仿真的果园履带车辆视觉导航控制系统，包括以下功能模块：

图像接收模块，用于笔记本电脑实时接收Kinect cv2相机采集的图像数据；其中微软Kinect cv2相机固定在果园履带车辆上，通过USB数据线和笔记本电脑通讯，电脑端通过调用Python程序将图像数据以图片格式保存下来，并且设置时间间隔，让所述相机每隔五毫秒的采样时间保存一幅果园路况图像；

图像偏差处理模块，基于语义分割模型，识别果园路况特征，得到需要的路况掩码区域；在路况掩码区域中基于边界提取左右侧边缘信息点，将左右边界对应的坐标求和平均得到拟合一系列图像中心点坐标；基于坐标权重表和图像中心点坐标，采用加权平均的方式求得每幅图像的计算平均中心点；将图像的计算平均中心点和理论中心坐标对应相减，即可得到每一幅图像的图像偏差；

模糊PID控制器模块，用于控制果园履带车辆的运动状态。其中控制器的输入为图像纵坐标偏差y_e和纵坐标偏差变化率d_ye，输出为控制量。通过将输入量分为13个等级，定义模糊集合，确定模糊控制规则等过程，使得比例系数K_p、积分系数T_i和微分系数T_d模糊化，扩大控制器调节的范围，最后通过重心法反模糊化，将模糊规则转换为驱动器或执行机构可以理解的语言，达到最终的控制要求；

MATLAB仿真模块，搭建仿真模型，用于验证模糊PID控制器的效果。在Win10 64位操作系统下，利用MATLAB软件的Simulink可视化模块，基于模糊逻辑工具箱(Fuzzy LogicToolbox)搭建模糊PID控制器，仿真前设置模型初始参数，其中包括车辆速度，比例系数K_p、积分系数T_i和微分系数T_d，仿真周期。选择直线和曲线两种跟踪路径，其中直线路径为一次函数y＝x，曲线路径为正弦函数y＝sinx，在MATLAB环境和当前路径下开始运行，检测模糊PID的仿真结果以及路径跟随效果。仿真过程中，模糊PID控制器三个参数根据图像纵坐标偏差y_e和纵坐标偏差变化率d_ye的大小合理选择数值，打破传统PID控制器参数固定的局面，达到最佳的控制目标；

实地试验验证模块，利用果园履带车辆测试系统的精度和稳定性。将模糊PID控制程序移植到STM32硬件设备中，通过输入PWM脉宽信号，利用反馈控制直流电机转动。参照图7所示，上位机PC接收微软Kinect cv2相机拍摄的图像，处理后得到图像偏差，通过串口传输给嵌入式下位机。下位机控制系统接收到指令后，经过模糊PID控制器处理得到输出量，经过放大器作用和STM32硬件设备PWM端口，将处理后的输出量以PWM脉宽信号的方式传入直流电机，作为直流电机的控制量，履带车辆左电机加上控制量，右电机减去控制量，从而实现左右电机以不同转速运转，达到电子差速的效果，完成果园视觉导航。如果左右电机的转速分别为：n₁,n₂，则有：

表2电子差速原理

电机转速比较	导航车辆运行状态
		若n1＜n2	左电机转速慢，右电机转速快，左转弯。
若n1＞n2	左电机转速快，右电机转速慢，右转弯。
		若n1＝n2	左电机转速和右电机转速相等，直线行驶。

上述实施例为本发明较佳的实施方式，但本发明的实施方式并不受上述实施例的限制，其他的任何未背离本发明的精神实质与原理下所作的改变、修饰、替代、组合、简化，均应为等效的置换方式，都包含在本发明的保护范围之内。

Claims (6)

1.基于模糊PID和MATLAB仿真的果园履带车辆视觉导航控制方法，其特征在于，包括以下步骤：

S1、利用放置在果园履带车辆上的微软Kinect cv2相机进行数据采集，拍摄果园路况图像，通过USB数据线传入并存储于放置在果园履带车辆上的笔记本电脑中；

S2、将果园路况图像传入语义分割模型中，生成路况掩码区并保存对应特征，从保存的路况掩码区中提取左右侧边缘信息点，利用左右侧边缘信息点的特征拟合一系列中心点坐标；

S3、基于加权平均的方法提取图像偏差；其中，给定像素坐标权重表，让每一个中心点坐标乘以对应的权重，加权平均得到每一幅图像的偏差；

S4、基于图像偏差，利用PID控制算法，设计模糊PID控制器；其中，包括模糊比例、模糊积分和模糊微分，三者线性相加得到输出量；包括以下步骤：

S401、图像纵坐标偏差与给定的目标期望值构造控制偏差，利用PID控制算法，设计PID控制器，通过比例、积分和微分三部分线性组合以形成控制变量并控制，具体为：

其中，e_t作为PID控制器的输入，u_t作为PID控制器的输出，K_p为控制器的比例系数，为控制器的积分部分，T_i为控制器的积分系数，/>为控制器的微分部分，T_d为控制器的微分系数；

S402、基于PID控制器，设计模糊PID,使比例系数K_p、积分系数T_i和微分系数T_d三部分模糊化；其中，模糊化包括以下步骤：

S4021、根据2维模糊控制器的原理，将图像纵坐标偏差y_e分为13个等级，其量化等级为：{-6,-5,-4,-3,-2,-1,0,1,2,3,4,5,6}；将图像纵坐标偏差变化率d_ye分为13个等级，其量化等级为：{-6,-5,-4,-3,-2,-1,0,1,2,3,4,5,6}，其中d_ye＝y_e2-y_e1，y_e2为当前的图像纵坐标偏差，y_e1为上次的图像纵坐标偏差；

S4022、定义图像纵坐标偏差y_e的模糊集合为：左大LB、左中LM、左小LS、零Z0、右小RS、右中RM、右大RB；图像纵坐标偏差变化率d_ye模糊集合为：左大LB、左中LM、左小LS、零Z0、右小RS、右中RM、右大RB；输入变量y_e、d_ye均选用三角型隶属度函数；

S4023、确定模糊控制规则，根据PID控制器调试经验可知，当图像纵坐标偏差y_e和纵坐标偏差变化率d_ye取值大时，使用小的比例系数K_p、积分系数T_i和微分系数T_d，使快速逼近期望目标值，减小调节时间，提高响应速度；当图像纵坐标偏差y_e和纵坐标偏差变化率d_ye取值小时，使用大的比例系数K_p、积分系数T_i和微分系数T_d，防止超调，提高稳定性；

S4024、通过重心法反模糊化，将模糊规则转换为驱动器或执行机构能够理解的语言，设计比例系数K_p、积分系数T_i和微分系数T_d的模糊控制查询表；

S5、利用MATLAB软件的Simulink仿真环境，建立果园视觉导航控制的仿真模型，检测模糊PID控制器的仿真结果以及路径跟随效果；

S6、根据仿真结果，得出模糊PID控制器的输出量，通过放大器，将放大后的输出量作为电机的控制量，从而实现果园履带车辆自主行驶。

2.根据权利要求1所述的基于模糊PID和MATLAB仿真的果园履带车辆视觉导航控制方法，其特征在于，在步骤S1，微软Kinect cv2相机通过支架安装于果园履带车辆的上盖，横向位于果园履带车辆中轴线，拍摄方向为机体前方；通过施加固定的PWM脉冲于果园履带车辆的控制电机，让果园履带车辆匀速行驶，模拟果园履带车辆的正常作业状态；果园履带车辆在果园行驶过程中，微软Kinect cv2相机进行图像采集，拍摄果园路况图像；微软Kinectcv2相机通过USB数据线与放置在果园履带车辆上的笔记本连接，在果园履带车辆匀速行驶过程中，及时将采集的果园路况图像传入并存储于笔记本电脑中。

3.根据权利要求1所述的基于模糊PID和MATLAB仿真的果园履带车辆视觉导航控制方法，其特征在于，所述步骤S2包括以下步骤：

S201、在Win10 64位操作系统下，搭建并训练语义分割模型；训练前设置合理的模型参数，多次训练，使预测模型的验证准确率提高，从而得到成功的语义分割模型；

S202、将步骤S1中得到的图像传入语义分割模型，识别果园路况特征，将图像中周围背景与果园路况区域分离，得到需要的路况掩码区域，最后将对应的路况掩码区域保存；

S203、对路况掩码区域进行二值化，滤波处理；基于掩码区域边界提取左右侧边缘信息点，将左右边界对应的坐标求和平均得到拟合一系列中心点坐标。

4.根据权利要求1所述的基于模糊PID和MATLAB仿真的果园履带车辆视觉导航控制方法，其特征在于，所述步骤S3包括以下步骤：

S301、设置像素坐标权重表，对于不同的图像中心点坐标需要进行取舍，视觉图像存在畸变，远离图像坐标系原点的图像特征需要舍去，图像坐标系原点近端的图像特征需要保留，从而让像素坐标权重表前面数据数值大，后面数据数值小或者为零；

S302、基于像素坐标权重表和图像中心点坐标，采用加权平均的方式求得每幅图像的计算平均中心点；其中，所述像素坐标权重表和图像中心点坐标都是一维数组，两者的数据个数相同，像素坐标权重表的每一个数据都需要乘以图像中心点坐标的每一个数据，所得结果相加并除以像素坐标权重表的数据和，从而得到每幅图像的计算平均中心点；

每幅图像的计算平均中心点表达式为：

其中，为每幅图像的计算平均中心点，w[i]表示像素坐标权重表第i个数据，x[i]表示图像中心点中第i个横坐标数据，y[i]表示图像中心点中第i个纵坐标数据，n表示像素坐标权重表中数据的总个数；

S303、对于每一幅图像都有其理论中心坐标，将图像的计算平均中心点和理论中心坐标对应相减，即可得到每一幅图像的图像偏差，处理完一幅图像，就能得到一组偏差，随后将之保存；

每一幅图像的图像偏差的表达式为：

其中，(x_m,y_m)为每一幅图像的理论中心坐标，为每幅图像的计算平均中心点，x_e为横坐标偏差，y_e为纵坐标偏差。

5.根据权利要求1所述的基于模糊PID和MATLAB仿真的果园履带车辆视觉导航控制方法，其特征在于，所述步骤S5包括以下步骤：

S501、基于MATLAB软件搭建Simulink仿真模型，在Simulink可视化仿真环境下构建导航车辆控制模型，模型函数使用MATLAB语言编写，完成相关参数之间的连线并保存；

S502、利用模糊逻辑工具箱搭建模糊PID控制器,模糊PID控制器参数以及过程按照步骤S4，完成相关参数之间的连线并保存；

S503、运行仿真，在脚本编辑器中设置仿真参数，其中车辆速度设置为0.5m/s，模糊PID控制器的比例系数K_p、积分系数T_i和微分系数T_d赋初始值，仿真周期为100s,选择直线和曲线两种跟踪路径，在MATLAB环境和当前路径下开始运行，检测模糊PID控制器的仿真结果以及路径跟随效果。

6.基于模糊PID和MATLAB仿真的果园履带车辆视觉导航控制系统，其特征在于，用于实现权利要求1-5任意一项所述的果园履带车辆视觉导航控制方法，包括：

图像接收模块，用于笔记本电脑实时接收微软Kinect cv2相机采集的图像数据；其中微软Kinect cv2相机固定在果园履带车辆上，通过USB数据线和笔记本电脑通讯，电脑端通过调用Python程序将图像数据以图片格式保存下来，并且设置时间间隔，让微软Kinectcv2相机每隔设定的采样时间保存一幅果园路况图像；

图像偏差处理模块，基于语义分割模型，识别果园路况特征，得到需要的路况掩码区域；在路况掩码区域中基于边界提取左右侧边缘信息点，将左右边界对应的坐标求和平均得到一系列图像中心点坐标；基于坐标权重表和图像中心点坐标，采用加权平均的方式求得每幅图像的计算平均中心点；将图像的计算平均中心点和理论中心坐标对应相减，即可得到每一幅图像的偏差；

模糊PID控制器模块，用于控制果园履带车辆的运动状态；其中，模糊PID控制器模块中的模糊PID控制器的输入为图像纵坐标偏差y_e和纵坐标偏差变化率d_ye，输出为控制量；通过将输入量分为13个等级，定义模糊集合，确定模糊控制规则过程，使得比例系数K_p、积分系数T_i和微分系数T_d模糊化，扩大控制器调节的范围，最后通过重心法反模糊化，将模糊规则转换为驱动器或执行机构能够理解的语言，达到最终的控制要求；

MATLAB仿真模块，搭建仿真模型，用于验证模糊PID控制器的效果；在Win10 64位操作系统下，利用MATLAB软件的Simulink可视化模块，基于模糊逻辑工具箱搭建模糊PID控制器，仿真前设置模型初始参数，其中包括车辆速度、比例系数K_p、积分系数T_i、微分系数T_d和仿真周期；选择直线和曲线两种跟踪路径，在MATLAB环境和当前路径下开始运行，检测模糊PID控制器的仿真结果以及路径跟随效果；仿真过程中，模糊PID控制器三个参数根据图像纵坐标偏差y_e和纵坐标偏差变化率d_ye的大小合理选择数值，打破传统PID控制器参数固定的局面，达到最佳的控制目标；

实地试验验证模块，利用果园履带车辆测试系统的精度和稳定性；将模糊PID控制器的控制程序移植到STM32硬件设备中，通过输入PWM脉宽信号，利用反馈控制直流电机转动；微软Kinect cv2相机拍摄的图像经过处理后得到输出量，经过放大器作用和STM32硬件设备PWM端口，将处理后的输出量以PWM脉宽信号的方式传入直流电机，作为直流电机的控制量，果园履带车辆左电机加上控制量，右电机减去控制量，从而实现左右电机以不同转速运转，达到电子差速的效果，完成果园视觉导航。