CN120010232B - 一种果实采摘机械系统控制器优化方法 - Google Patents

Abstract

本发明公开一种果实采摘机械系统控制器优化方法，属于自动化控制技术领域，包括：S1、将果实和机械爪位置建立在同一个三维空间，通过视觉传感器定位果实的三维位置，同时计算实时械臂爪的三维位置；S2、通过果实和实时机械爪的三维位置计算果实位置与机械爪位置的误差；S3、将误差输至加强型增量式PID算法模块进行处理，输出当前时刻机械臂调控信号增量；S4、将当前时刻机械臂调控信号增量迭加到上一时刻机械臂调控信号得到当前时刻机械臂调控信号调控机械臂运动，同时将实时械臂爪的三维位置反馈到S2，循环执行S2至S4达到调控信号增量为零。实验证明本发明方法加强了果实采摘机械系统控制器的控制精度和灵敏度。

Description

一种果实采摘机械系统控制器优化方法

技术领域

本发明涉及自动化控制技术领域，尤其涉及一种果实采摘机械系统控制器优化方法。

背景技术

苹果采摘机器人是一种广泛应用于农业领域的智能化设备，旨在替代人工完成苹果的精准采摘任务；这种机器人通常需要具备快速、准确的定位能力，以及对复杂农田环境的适应能力。在果实采摘过程中，机器人必须协调机械臂运动以及视觉识别功能，以实现高效采摘；然而，在实际应用中，由于农田环境复杂多变，采摘过程往往面临诸多技术挑战。

PID控制器（比例-积分-微分控制器）作为一种经典的控制算法，因其简单高效的结构被广泛应用于苹果采摘机器人中，用于机械臂的运动控制；然而，传统的PID控制器在复杂非线性环境中难以达到最优控制效果，机械臂的运动控制精度较低，运动速度较慢，主要体现在以下几个方面：1、PID控制器的性能严重依赖于比例、积分和微分参数（Kp、Ki、Kd）的设定。在复杂多变的农田环境中，这些参数往往需要频繁调节以适应不同的果实种类和采摘条件，手动调参既耗时又难以获得最优解；2、苹果采摘任务对控制系统的响应速度和稳定性有较高要求。然而，传统PID控制器在快速响应和抑制振荡之间难以实现理想的平衡，容易导致过冲、滞后或系统震荡，影响采摘效率和准确性；3、农田环境中存在各种动态干扰因素，如风力、枝叶晃动和光照变化，这些因素可能导致机械臂定位偏差，传统PID控制器难以实时适应这种动态变化。

增长优化算法（Growth Optimizer, GO）是一种新型元启发式算法，旨在解决连续全局优化问题，GO算法的设计灵感来源于个体在社会成长过程中的学习和反思机制，通过模拟个体的成长行为，GO算法旨在找到全局最优解，GO算法分为学习阶段和反思阶段建立个体位置更新数学模型，利用GO算法对果实采摘机械系统控制器的PID算法的控制参数优化可以提高控制器的灵敏度和精确度，但是需要对GO算法改进，解决其易陷入局部最优解的问题，才能保证在增长优化算法对果实采摘机械系统控制器的PID算法的控制参数优化过程中的绝对优越性。

发明内容

本发明的目的是提供一种果实采摘机械系统控制器优化方法，利用改进的增长优化算法通过对果实采摘机械系统控制器的控制算法的控制参数整定，加强果实采摘机械系统控制器的控制精度和灵敏度，解决上述背景技术中苹果采摘机器人机械臂的运动控制精度较低，运动速度较慢的问题，实现果实采摘机器人采摘精度最优鲁棒控制。

为了实现上述目的，本发明提供如下技术方案：一种果实采摘机械系统控制器优化方法，通过增长优化算法进行改进，利用改进的增长优化算法对果实采摘机械系统控制器的PID控制算法的控制参数优化，具体包括以下步骤。

S1、将果实和机械爪位置建立在同一个三维空间，通过视觉传感器定位果实的三 维位置，同时计算实时械臂爪的三维位置。

S2、通过果实和实时机械爪的三维位置计算果实位置与机械爪位置的误差坐标（Δx, Δy, Δz），建立果实与机械爪三维位置数据误差值。

S3、将所述误差值输至加强型增量式PID算法模块进行处理，输出当前时刻加强型增量式PID算法输出量Δu(t)；所述加强型增量式PID算法模块为改进的增长优化算法模块和增量式PID算法模块融合，具体方法为：利用改进的增长优化算法整定增量式PID算法的控制参数，得到加强型增量式PID算法，所述加强型增量式PID算法用于果实采摘机械系统控制器。

S4、果实采摘机械系统控制器将所述Δu(t)迭加到上一时刻机械臂调控信号u(t- 1)输出当前时刻机械臂调控信号u(t)；所述当前时刻机械臂调控信号调控机械臂运动；同 时将实时械臂爪的三维位置反馈到所述S2，循环执行S2至S4达到Δu(t)为 零，完成果实采摘机械系统控制器的优化。

优选的，果实采摘机械系统包括果实采摘机器人的机械臂和机械爪两个部分，机械爪与机械臂的末端相接，通过果实采摘机械臂调整机械爪的角度和距离；果实采摘机械系统控制器包括果实和机械爪位置数据输入模块、增量式PID算法模块、果实与机械爪三维位置数据差值计算模块、机械爪实时位置数据反馈模块、机械臂调控输出模块，其中，机械臂调控输出模块输出当前时刻机械臂调控信号u(t)，所述调控信号u(t)输入机器臂三自由度电调模块，其中电调模块将调控信号u(t)处理后分别控制机械臂三个自由度电机运动。

优选的，通过计算果实位置与机械爪实时位置的误差（Δx, Δy, Δz）确定机械爪需要调整的角度和距离，其中，机械爪与目标果实之间的直线距离数学模型为：

（1）；

式（1）中，为横轴位置误差，为纵轴位置误差，为Z轴位置误差； 为机械爪与目标果实之间的直线距离，该直线距离是机械爪沿直线轨迹移动所需调整的距 离；

更具体地，实验过程中机械爪需要调整的角度和距离即为果实采摘机械臂需要调整的角度，机械臂为三自由度机械臂，通过机械臂电调驱动三个伺服电机调整机械臂的角度；其中，机械爪调整所需的角度在笛卡尔坐标系中转换为极坐标系表示，定义目标果实点的方向角，包括俯仰角和偏航角；其中，俯仰角为垂直方向调整角度在四象限内的分布，数学模型为：

（2）；

偏航角是果实与机械爪在水平面上的夹角，数学模型为：

（3）。

优选地，果实采摘机械系统控制器的控制算法采用增量式PID算法，对机械臂调控信号的增量进行调整，相对于位置式PID，其仅计算当前时刻的控制信号增量，基于当前误差的变化量调整，对计算误差的累积影响较小，具有较好的鲁棒性，数学模型为：

（4）；

式（4）中，为增量式PID算法的比例系数、为增量式PID算法的积分系数、为增量式PID算法的微分系数，、、分别为t、t-1和t-2时刻 果实与机械爪三维位置数据误差值；

更具体地，果实位置与机械爪位置的误差坐标（Δx, Δy, Δz），建立果实与机械爪三维位置数据误差值，数学模型为：

（5）；

更具体地，通过加强型增量式PID算法输出量Δu(t)改变果实采摘机械系统控制器的输出机械臂调控信号，直到果实与机械爪三维位置数据误差值趋于零，即加强型增量式PID算法输出量Δu(t)趋于零，果实采摘机械系统控制器反馈结束，数学模型为：

（6）；

式中，u(t-1)为上一时刻机械臂调控信号，u(t)为当前时刻机械臂调控信号。

优选地，机器臂三自由度电调模块是一种现有的控制模块，可以将单一控制信号转换为多组控制信号，将机械臂调控信号转换为一个三维数组，数组的第一维数据为机械臂第一自由度的调控信号增量，调控偏航角，第二维数据为机械臂第二自由度的调控信号增量，调控伸缩长度，第三维数据为机械臂第三自由度的调控信号增量，调控俯仰角。

优选的，利用改进的增长优化算法对增量式PID算法的比例系数、积分系数和微分系数整定，得到的最佳的控制参数输入到公式（4）中得到加强型增量式PID算法，包括：比例系数、积分系数和微分系数；将加强型增量式PID算法用于果实采摘机械系统控制器的增量式PID算法模块，提高果实采摘机械系统控制器对机械臂的控制精度和灵敏度。

优选的，增长优化算法包括学习阶段和反思阶段两个阶段的数学模型，其中，学习阶段模拟了个体在社会中的学习过程，通过从其他个体之间的差距中学习来提升自己的知识水平，为增长优化算法的全局搜索阶段；反思阶段模拟了个体在自我反思过程中的行为，通过检查和弥补自身的不足来提升自己的知识水平，为增长优化算法的局部开发阶段；增长优化算法通过迭代更新个体位置，找到最佳的个体位置，其迭代过程即为增量式PID算法最佳控制参数的寻优过程，最佳的个体位置即为最佳控制参数的值。

优选的，增长优化算法的第i个个体的位置向量包括D个维度，其中D为算法问题维 度，增长优化算法对增量式PID算法优化过程中，针对增量式PID算法的控制参数整定，得到 在当前环境下的最佳控制参数，其中，控制参数的个数记为算法问题维度D，其中，增量式 PID算法的控制参数包括比例参数、微分参数和积分参数；第i个个体的位置向量为，第i个个体的位置向量的第1维值映射增量式PID算法的比例参 数值，第i个个体的位置向量的第2维值映射增量式PID算法的积分参数值，第i个个体的位 置向量的第3维值映射增量式PID算法的微分参数值。

优选的，增长优化算法学习阶段通过构建四种不同标准个体间的差异，模拟个体之间的协作学习，其中四种不同标准个体包括：当前种群的最优个体、当前种群中的精英个体、当前种群中的较差个体和当前种群中的随机个体，其中，四种不同标准个体间的差异的数学模型为：

（7）；

式（7）中，为全局种群的最优个体位置向量的第j维值，为适 应度值排名前的个体，记为当前种群中的精英个体的位置向量的第j维值，为最大种 群规模，为适应度值排名后的个体，记为当前种群中的较差个体的位置向量 的第j维值，和为种群中的随机的两个个体的位置向量的第j维值， 为当前种群的最优个体与精英个体的距离，为当前种群中的精英个体与较差个体 的距离，为当前种群的最优个体与较差个体的距离，为种群中的随机的两 个个体之间的距离。

优选的，增长优化算法学习阶段根据知识获取量更新第i个个体位置，数学模型：

（8）；

式（8）中，为第t+1次迭代第i个个体的位置向量，为第t次迭代第i个个 体的位置向量，为第k个知识获取量，通过四种不同标准个体间的欧几里得距离得 到。

优选的，当前标准增长优化算法主要依赖于欧几里得距离，目标方向单一，影响对果实采摘机械系统控制的精度；对标准增长优化算法的学习阶段个体位置更新策略改进，在学习阶段，引入模仿高适应值个体的多方向探索策略得到改进的学习阶段个体位置更新策略，具体步骤为：

步骤一、计算当前个体与其他N-1个个体的欧几里得距离，其中距离小于R的作为当前个体的邻域个体，根据邻域个体的位置更新当前个体的位置，数学模型为：

（9）；

式（9）中，为当前个体邻域R内的个体数；为第i个个体根据第y个邻域个 体生成的位置向量，其中y=1,2,...,n；为第t次迭代的邻域解集内的第y个邻域个 体位置向量；为第t次迭代第i个个体的位置向量；

步骤二、模仿种群中适应值小的个体，数学模型为：

（10）；

式（10）中，为第t+1次迭代第i个个体的位置向量，为第i个个体根 据第y个邻域个体生成的位置向量，其中y=1,2,...,n，和为动态权重因子，确保不同 来源的信息被合理整合，为全局种群的最优个体位置向量，从适应值最接 近的个体位置向量；

步骤三、基于多方向的搜索结果取最优，改进单个方向引导的方式，具体方式为：

第i个个体生成的n个候选解 的适应度值分别为，选择最优候选解更新第i个个体位置向量，数学模型为：

；其中，为第t+ 1次迭代的第i个个体的第1个候选解，为第t+1次迭代的第i个个体的第2个候选解，为第t+1次迭代的第i个个体的第n个候选解，为个体位置的适应度值，为最小值取值函数。

优选的，增长优化算法（GO）中的衰减因子（AF）设计以当前迭代次数和最大迭代次数为依据，逐渐降低探索力度，专注于局部开发；本发明提出的方法在标准增长优化算法（GO）中的衰减因子（AF）设计基础上，利用种群当前迭代个体与上一代个体位置变化的平均幅度动态调整衰减因子，在局部开发阶段，通过降低探索力度并专注于局部搜索，能够更精细地寻找最优解，同时在不同阶段自适应地调整策略，避免陷入局部最优或过早收敛的问题，提高增长优化算法在局部开发阶段对增量式PID算法控制参数的整定优化效率和精度；改进的衰减因子数学模型为：

（11）；

式中，为第t次迭代的改进的衰减因子值，为最大迭代次数， 为第t次迭代的种群活跃度，数学模型为：

（12）；

式中，为种群初始化时的最大活跃度，为种群最大规模，为第t 次迭代第i个个体第j维的位置值，为第t-1次迭代第i个个体第j维的位置值。

优选的，利用改进的衰减因子，同时引入当前种群的最优个体位置引导个体位置更新对增长优化算法的反思阶段的个体位置更新策略数学模型改进，数学模型为：

（13）；

式中，为第t+1次迭代第i个个体的位置向量，为第t次迭代第i个个体 的位置向量，UB和LB为个体位置向量上限和下限，r为0到1内的随机数，为第t次迭 代的种群的最优个体位置向量，p为控制参数。

优选的，利用改进的学习阶段和反思阶段个体位置更新策略，完成对标准增长优化算法的改进；利用改进的增长优化算法对增量式PID算法的比例系数、积分系数和微分系数寻优，具体步骤为：

S31、将果实采摘机械系统控制器的增量式PID算法的控制参数编码为空间向量U， 维度为3，将所述空间向量与改进的增长优化算法的第i个个体的位置向量建立映射；

S32、设置改进的增长优化算法的最大迭代次数T，问题维度D，种群最大规模N，个体位置向量上限UB和下限LB，随机初始化每个个体的位置向量值；

S33、利用目标函数计算当前每个个体的位置的适应度值，保留当前最小适应度值对应的个体位置，将其作为第t次迭代的种群的最优个体位置向量；按照公 式（14）更新全局种群的最优个体位置向量；

（14）；

式中，为全局种群的最优个体位置向量，为全局种群的最优 个体位置向量的适应度值；

S34、若当前迭代次数t满足t>T，则将当前全局种群的最优个体位置向量输出解析赋值于为果实采摘机械系统控制器的增量式PID算法的控制参数；否则执行改进的增长优化算法的个体位置更新数学模型；

S35、利用改进的学习阶段个体位置更新策略对N个个体的位置更新，计算第t次迭代的改进的衰减因子值，根据所述衰减因子值建立反思阶段的个体位置更新策略更新N个个体的位置；

S36、将更新后的个体位置向量限制在个体位置向量上限UB和下限LB范围内；迭代次数自加一，返回执行S33。

优选的，适应度值的作用是评价解的优越性，适应度值的大小反映了当前果实采摘机械系统控制器的控制精度，即果实采摘机械系统控制器的增量式PID算法的比例系数、积分系数和微分系数精度，通过将改进的增长优化算法的个体位置解析为增量式PID算法的比例系数、积分系数和微分系数用于果实采摘机械系统控制器，通过目标函数计算果实采摘机械系统控制器在当前比例系数、积分系数和微分系数下的控制误差大小得到适应度值，适应度值越小则果实采摘机械系统控制器的当前控制误差越小，反之控制误差越大；目标函数数学模型为：

（15）；

式中，为目标函数，e(t)为第t时刻果实与机械爪三维位置数据误差值。

相比现有技术，本发明的技术方案具有如下优点和有益效果：本发明利用改进的增长优化算法对果实采摘机械系统控制器的PID控制算法的控制参数进行优化，显著提高了控制器的控制精度和灵敏度；通过精确调整机械臂的运动，实现了对果实的精准采摘，减少了定位偏差和采摘过程中的误差，改进的增长优化算法通过全局搜索和局部开发两个阶段，能够更精细地寻找最优解，避免了陷入局部最优或过早收敛的问题，因此，本发明的方法在不同果实种类和采摘条件下均能保持较好的控制性能，增强了系统的适应性和鲁棒性；传统的PID控制器需要频繁调节比例、积分和微分参数以适应不同的采摘条件，这不仅耗时而且难以获得最优解，而本发明的方法通过自动优化控制参数，降低了人工调参的成本和难度。

附图说明

图1为果实采摘机械系统控制器优化的整体技术框架图；

图2为改进的增长优化算法对增量式PID算法的比例系数、积分系数和微分系数寻优的流程图；

图3为改进的增长优化算法与标准的增长优化算法寻优过程中每次迭代的最小适应度值的对比图；

图4为果实采摘机械系统控制器的增量式PID控制算法的控制参数寻优过程变化图；

图5为本发明方法和现有方法对果实采摘机械系统控制器优化的效果对比图。

具体实施方式

以下实施例进一步说明本发明的内容，但不应理解为对本发明的限制。在不背离本发明精神和实质的情况下，对本发明方法、步骤或条件所作的修改或替换，均属于本发明的范围。

如图1所示，本发明提供了一种果实采摘机械系统控制器优化方法，通过对增长优化算法进行改进，利用改进的增长优化算法对果实采摘机械系统控制器的增量式PID控制算法的控制参数寻优，将改进的增量式PID控制算法用于果实采摘机械系统控制器，实施过程中包括Matlab代码和Simulink系统仿真模型，包括步骤S1至S4。

S1、将果实和机械爪位置建立在同一个三维空间，通过视觉传感器定位果实的三 维位置，同时计算实时械臂爪的三维位置。

具体地，在本实施步骤中，将整个三维空间的坐标系作为全球坐标系，根据机械爪 初始位置和果实的位置确定局部坐标系，果实的位置通过视觉传感器进行实时定位，将图 像数据转化为三维坐标系中的点，通过机械臂的三自由度电机的编码器计算 实时械臂爪的三维位置。

S2、通过果实和实时机械爪的三维位置计算果实位置与机械爪位置的误差坐标（Δx, Δy, Δz），建立果实与机械爪三维位置数据误差值。

具体地，在本实施步骤中，计算果实位置与机械爪实时位置的误差（Δx, Δy, Δz）确定机械爪需要调整的角度和距离，其中，机械爪与目标果实之间的直线距离数学模型为：

（1）；

式（1）中，为横轴位置误差，为纵轴位置误差，为Z轴位置误差； 为机械爪与目标果实之间的直线距离，该直线距离是机械爪沿直线轨迹移动所需调整的距 离。

进一步地，实验过程中机械爪需要调整的角度和距离即为果实采摘机械臂需要调整的角度，机械臂为三自由度机械臂，通过机械臂电调驱动三个伺服电机调整机械臂的角度；其中，机械爪调整所需的角度在笛卡尔坐标系中转换为极坐标系表示，定义目标果实点的方向角，包括俯仰角和偏航角；其中，俯仰角为垂直方向调整角度在四象限内的分布，数学模型为：

（2）；

偏航角是果实与机械爪在水平面上的夹角，数学模型为：

（3）。

S3、将所述误差值输至加强型增量式PID算法模块进行处理，输出当前时刻加强型增量式PID算法输出量Δu(t)；所述加强型增量式PID算法模块为改进的增长优化算法模块和增量式PID算法模块融合，具体方法为：利用改进的增长优化算法整定增量式PID算法的控制参数，得到加强型增量式PID算法，所述加强型增量式PID算法用于果实采摘机械系统控制器。

具体地，在本实施步骤中，定义果实采摘机械系统控制器的控制算法，采用增量式PID算法用于Simulink中的果实采摘机械系统控制器仿真模型的增量式PID算法模块，数学模型为：

（4）；

式（4）中，为增量式PID算法的比例系数、为增量式PID算法的积分系数、为增量式PID算法的微分系数，、、分别为t、t-1和t-2时刻 果实与机械爪三维位置数据误差值；其中，果实位置与机械爪位置的误差坐标（Δx, Δy, Δz），建立果实与机械爪三维位置数据误差值，输入Simulink果实采摘机械系统控制器仿 真模型的果实与机械爪三维位置数据差值计算模块，数学模型为：

（5）。

具体地，在本实施步骤中，对标准增长优化算法的学习阶段个体位置更新策略改进，在学习阶段，引入模仿高适应值个体的多方向探索策略得到改进的学习阶段个体位置更新策略，具体步骤为：

步骤一、计算当前个体与其他N-1个个体的欧几里得距离，其中距离小于R的作为当前个体的邻域个体，根据邻域个体的位置更新当前个体的位置，数学模型为：

（9）；

式（9）中，为当前个体邻域R内的个体数；为第i个个体根据第y个邻域个 体生成的位置向量，其中y=1,2,...,n；为第t次迭代的邻域解集内的第y个邻域个 体位置向量；为第t次迭代第i个个体的位置向量；

步骤二、模仿种群中适应值小的个体，数学模型为：

（10）；

式（10）中，为第t+1次迭代第i个个体的位置向量，为第i个个体根 据第y个邻域个体生成的位置向量，其中y=1,2,...,n，和为动态权重因子，确保不同 来源的信息被合理整合，为全局种群的最优个体位置向量，从适应值最接 近的个体位置向量；

步骤三、基于多方向的搜索结果取最优，改进单个方向引导的方式，具体方式为：

第i个个体生成的n个候选解 的适应度值分别为，选择最优候选解更新第i个个体位置向量，数学模型为：

；其中，为第t+ 1次迭代的第i个个体的第1个候选解，为第t+1次迭代的第i个个体的第2个候选解，为第t+1次迭代的第i个个体的第n个候选解，为个体位置的适应度值，为最小值取值函数。

进一步地，在标准增长优化算法（GO）中的衰减因子（AF）设计基础上，利用种群当前迭代个体与上一代个体位置变化的平均幅度动态调整衰减因子，改进的衰减因子数学模型为：

（11）；

式中，为第t次迭代的改进的衰减因子值，为最大迭代次数，为 第t次迭代的种群活跃度，数学模型为：

（12）；

式中，为种群初始化时的最大活跃度，为种群最大规模，为第t次 迭代第i个个体第j维的位置值，为第t-1次迭代第i个个体第j维的位置值。

更进一步地，利用改进的衰减因子，同时引入当前种群的最优个体位置引导个体位置更新对增长优化算法的反思阶段的个体位置更新策略数学模型改进，数学模型为：

（13）；

式中，为第t+1次迭代第i个个体的位置向量，为第t次迭代第i个个体 的位置向量，UB和LB为个体位置向量上限和下限，r为0到1内的随机数，为第t次迭代 的种群的最优个体位置向量，p为控制参数。

进一步地，如图2所示，利用改进的学习阶段个体位置更新策略，完成对标准增长优化算法的改进；利用改进的增长优化算法对增量式PID算法的比例系数、积分系数和微分系数寻优，具体步骤为：

S31、将果实采摘机械系统控制器的增量式PID算法的控制参数编码为空间向量U， 维度为3，将所述空间向量与改进的增长优化算法的第i个个体的位置向量建立映射；

S32、设置改进的增长优化算法的最大迭代次数T，问题维度D，种群最大规模N，个体位置向量上限UB和下限LB，随机初始化每个个体的位置向量值；

S33、 利用目标函数计算当前每个个体的位置的适应度值，保留当前最小适应度 值对应的个体位置，将其作为第t次迭代的种群的最优个体位置向量；按照公 式（14）更新全局种群的最优个体位置向量；

（14）；

式中，为全局种群的最优个体位置向量，为全局种群的最优个 体位置向量的适应度值；

其中， 目标函数数学模型为：

（15）；

式中，为目标函数，e(t)为第t时刻果实与机械爪三维位置数据误差值；

S34、若当前迭代次数t满足t>T，则将当前全局种群的最优个体位置向量输出解析赋值于为果实采摘机械系统控制器的增量式PID算法的控制参数；否则执行改进的增长优化算法的个体位置更新数学模型；

S35、利用改进的学习阶段个体位置更新策略对N个个体的位置更新，计算第t次迭代的改进的衰减因子值，根据所述衰减因子值建立反思阶段的个体位置更新策略更新N个个体的位置；

S36、将更新后的个体位置向量限制在个体位置向量上限UB和下限LB范围内；迭代次数自加一，返回执行S33。

S4、果实采摘机械系统控制器将所述Δu(t)迭加到上一时刻机械臂调控信号u(t- 1)输出当前时刻机械臂调控信号u(t)；所述当前时刻机械臂调控信号调控机械臂运动；同 时将实时械臂爪的三维位置反馈到所述S2，循环执行S2至S4达到Δu(t)为 零，完成果实采摘机械系统控制器的优化。

具体地，在本实施步骤中，通过加强型增量式PID算法输出量Δu(t)改变果实采摘机械系统控制器的输出机械臂调控信号，直到果实与机械爪三维位置数据误差值趋于零，即加强型增量式PID算法输出量Δu(t)趋于零，果实采摘机械系统控制器反馈结束，数学模型为：

（6）；

式中，u(t-1)为上一时刻机械臂调控信号，u(t)为当前时刻机械臂调控信号。

进一步地，在Matlab中建立机器臂三自由度电调模型将单一控制信号转换为多组控制信号，将机械臂调控信号转换为一个三维数组，数组的第一维数据为机械臂第一自由度的调控信号增量，调控偏航角，第二维数据为机械臂第二自由度的调控信号增量，调控伸缩长度，第三维数据为机械臂第三自由度的调控信号增量，调控俯仰角。

进一步地，在Matlab中对本发明方法的数学模型完成代码设计，初始化最大迭代次数T=100，问题维度D=3，种群最大规模N=20，个体位置向量上限UB=110和下限LB=0.001，适应度值调用目标函数，代码如下：

Dim = 3;

Pop =20;

f = @(x) AGO_PID(x);

Max_iter = 100;

ub = 110;

lb = 0.001;

然后，将改进的增长优化算法与果实采摘机械系统控制器的增量式PID控制算法建立映射，代码如下：

[Best_f,Best_X,val,curve_f]=AGO(Max_iter,lb,ub,Dim,f);其中，Best_f代表最小适应度值， curve_f代表适应度值，val代表三维增量式PID控制算法的控制参数，Best_X代表个体最佳位置。

进一步地，在Simulink中建立果实采摘机械系统控制器仿真模型，包括果实和机械爪位置数据输入模块、增量式PID算法模块、果实与机械爪三维位置数据差值计算模块、机械爪实时位置数据反馈模块、机械臂调控输出模块以及目标函数模型；将最小适应度值时的增量式PID控制算法的控制参数输入增量式PID算法模块，运行代码sim('AGO_Model')完成果实采摘机械系统控制器的闭环控制。

如图3所示，从标准增长优化算法（GO）与改进的增长优化算法（AGO）寻优过程中每次迭代的最小适应度值的对比图可以发现，在迭代初期，两种算法均表现出快速的适应度值下降，但GO-PID的下降的速度略低于AGO-PID，且在迭代初期改进的增长优化算法的最小适应度值低于标准增长优化算法（GO）的适应度值，说明改进的增长优化算法（AGO）寻优前期就表现出较快的速度和较高的精度；在迭代48次时，标准增长优化算法（GO）的适应度值保持稳定不在继续减小，达到了其能寻优的最小适应度值；在迭代52次时，改进的增长优化算法（AGO）达到了其能寻优的最小适应度值，对比可以发现，改进的增长优化算法（AGO）寻优的适应度值比标准增长优化算法（GO）更小，说明本发明提出的改进的增长优化算法（AGO）对果实采摘机械系统控制器的PID控制算法的控制参数寻优效果更好。

如图4所示，从果实采摘机械系统控制器的增量式PID控制算法的控制参数寻优过程变化图可以发现，改进的增长优化算法（AGO）寻优过程中增量式PID控制算法的Kp、Ki、Kd参数的变化，在迭代52次左右时达到稳定，最佳的Kp、Ki、Kd参数分别为：4.3058、0.530887、1.23167；将最佳的控制参数输入果实采摘机械系统控制器Simulink仿真模型的增量式PID算法模块，设置目标值为5倍单位阶跃信号，运行20秒输出果实采摘机械系统控制器的控制效果，如图5所示，实验过程中加入标准PID控制算法对比，从三种控制算法用于果实采摘机械系统控制器的控制效果可以发现，本发明提出的改进的增长优化算法优化增量式PID控制算法（AGO-PID）的方法用于果实采摘机械系统控制器时，在控制速度和控制精度相比于其他两种方式更好，表现在前期的控制超调量，本发明的方法不存在超调量，而其他方法的超调量达到6到7左右；在控制速度上，本发明的控制方法在7秒左右便达到目标值5个单位，且几乎无波动，说明本发明提出的果实采摘机械系统控制器优化方法更佳。

Claims (4)

1.一种果实采摘机械系统控制器优化方法，其特征在于，具体包括：

S1、将果实和机械爪位置建立在同一个三维空间，通过视觉传感器定位果实的三维位置 ，同时计算实时械臂爪的三维位置；

S2、通过果实和实时机械爪的三维位置计算果实位置与机械爪位置的误差坐标（Δx,Δy, Δz），建立果实与机械爪三维位置数据误差值；

S3、将所述误差值输至加强型增量式PID算法模块进行处理，输出当前时刻加强型增量式PID算法输出量Δu(t)；所述加强型增量式PID算法模块为改进的增长优化算法模块和增量式PID算法模块融合，具体方法为：利用改进的增长优化算法整定增量式PID算法的控制参数，得到加强型增量式PID算法，所述加强型增量式PID算法用于果实采摘机械系统控制器；所述改进的增长优化算法，具体方法为：在学习阶段，引入模仿高适应值个体的多方向探索策略得到改进的学习阶段个体位置更新策略，具体步骤为：

步骤一、计算当前个体与其他N-1个个体的欧几里得距离，其中距离小于R的作为当前个体的邻域个体，根据n个邻域个体的位置更新当前个体的位置向量，所述位置向量共有n个，对应n个邻域个体，数学模型为：

（9）；

式（9）中，为当前个体邻域R内的个体数；为第i个个体根据第y个邻域个体生成的位置向量，其中y=1,2,...,n；为第t次迭代的邻域解集内的第y个邻域个体位置向量；为第t次迭代第i个个体的位置向量；

步骤二、模仿种群中适应值小的个体对第i个个体位置向量的n个值更新，数学模型为：

（10）；

式（10）中，为第t+1次迭代第i个个体的第y个位置向量，其中y=1,2,...,n，和为动态权重因子，确保不同来源的信息被合理整合，为全局种群的最优个体位置向量，从适应值最接近的个体位置向量，为第t次迭代第i个个体的第y个位置向量；

步骤三、基于多方向的搜索结果取最优，改进单个方向引导的方式，更新第i个个体位置向量，具体方式为：

第i个个体生成的n个位置向量 的适应度值分别为，选择最优候选解更新第i个个体位置向量，数学模型为：

；其中，为第t+1次迭代的第i个个体的第1个候选解，为第t+1次迭代的第i个个体的第2个候选解，为第t+1次迭代的第i个个体的第n个候选解，为适应度函数，为最小值取值函数；

所述改进的增长优化算法，包括利用种群当前迭代个体与上一代个体位置变化的平均幅度动态调整衰减因子，改进的衰减因子数学模型为：

（11）；

式中，为第t次迭代的改进的衰减因子值，为最大迭代次数，为第t次迭代的种群活跃度，数学模型为：

（12）；

式中，为种群初始化时的最大活跃度，为种群最大规模，为第t次迭代第i个个体第j维的位置值，为第t-1次迭代第i个个体第j维的位置值；

然后，利用改进的衰减因子，同时引入当前种群的最优个体位置引导个体位置更新对增长优化算法的反思阶段的个体位置更新策略数学模型改进；

S4、果实采摘机械系统控制器将所述Δu(t)迭加到上一时刻机械臂调控信号u(t-1)输出当前时刻机械臂调控信号u(t)；所述当前时刻机械臂调控信号调控机械臂运动；同时将实时械臂爪的三维位置反馈到所述S2，循环执行S2至S4达到所述Δu(t)为零，完成果实采摘机械系统控制器的优化。

2.根据权利要求1所述的一种果实采摘机械系统控制器优化方法，其特征在于，所述果实采摘机械系统包括果实采摘机器人的机械臂和机械爪两个部分，机械爪与机械臂的末端相接，通过果实采摘机械臂调整机械爪的角度和距离；果实采摘机械系统控制器包括果实和机械爪位置数据输入模块、增量式PID算法模块、果实与机械爪三维位置数据差值计算模块、机械爪实时位置数据反馈模块、机械臂调控输出模块，其中，机械臂调控输出模块输出当前时刻机械臂调控信号u(t)，所述调控信号u(t)控制机械臂运动，数学模型为：

（6）；

式中，u(t-1)为上一时刻机械臂调控信号，u(t)为当前时刻机械臂调控信号。

3.根据权利要求1所述的一种果实采摘机械系统控制器优化方法，其特征在于，改进后的反思阶段的个体位置更新策略数学模型为：

（13）；

式中，为第t+1次迭代第i个个体的位置向量，为第t次迭代第i个个体的位置向量，UB和LB为个体位置向量上限和下限，r为0到1内的随机数，为第t次迭代的种群的最优个体位置向量，p为控制参数。

4.根据权利要求3所述的一种果实采摘机械系统控制器优化方法，其特征在于，所述得到加强型增量式PID算法，包括：利用改进的学习阶段个体位置更新策略和反思阶段的个体位置更新策略完成对标准增长优化算法的改进，利用改进的增长优化算法对增量式PID算法的比例系数、积分系数和微分系数寻优，具体步骤为：

S31、将果实采摘机械系统控制器的增量式PID算法的控制参数编码为空间向量U，维度为3，将所述空间向量与改进的增长优化算法的第i个个体的位置向量建立映射；

S32、设置改进的增长优化算法的最大迭代次数T，问题维度D，种群最大规模N，个体位置向量上限UB和下限LB，随机初始化每个个体的位置向量值；

S33、 利用目标函数计算当前每个个体的位置的适应度值，保留当前最小适应度值对应的个体位置，将其作为第t次迭代的种群的最优个体位置向量；按照公式（14）更新全局种群的最优个体位置向量；

（14）；

式中，为全局种群的最优个体位置向量，为全局种群的最优个体位置向量的适应度值；

其中， 目标函数数学模型为：

（15）；

式中，为目标函数，e(t)为第t时刻果实与机械爪三维位置数据误差值；

S34、若当前迭代次数t满足t>T，则将当前全局种群的最优个体位置向量输出解析赋值于为果实采摘机械系统控制器的增量式PID算法的控制参数；否则执行改进的增长优化算法的个体位置更新数学模型；

S35、利用改进的学习阶段个体位置更新策略对N个个体的位置更新，计算第t次迭代的改进的衰减因子值，根据所述衰减因子值建立反思阶段的个体位置更新策略更新N个个体的位置；

S36、将更新后的个体位置向量限制在个体位置向量上限UB和下限LB范围内；迭代次数自加一，返回执行S33。