农用机器人行驶路径控制方法及系统

1.本技术涉及机器人控制技术领域，尤其涉及一种农用机器人行驶路径控制方法及系统。


背景技术：

2.目前，对果树的施肥、采摘等工序多采用人工方式，而人工成本高且效率低。比如：农户大面积种植果树并采用人工方式进行施肥时，使得施肥的成本高且效率低，严重制约了种植业的发展。
3.为此，农业机械应运而生，比如：能够施肥的农用机器人、能够采摘的农用机器人等。农业机械的应用能够加快种植业机械化的进程，且对现代农业生产具有积极的促进作用。
4.农用机器人的行驶路径是预先规划好的，但是，由于种植园环境复杂多变，导致农用机器人的实际行驶路径与预设行驶路径之间偏差较大。


技术实现要素：

5.本技术提供一种农用机器人行驶路径控制方法及系统，技术方案如下：
6.第一方面，本技术提供一种农用机器人行驶路径控制方法，该方法包括：获取目标机器人预设行驶路径中的第一标记点位置和第二标记点位置，并分别获取相邻两时刻中每一时刻目标机器人的实际质心位置、实际航向角及实际前轮转角，相邻两时刻分别为：当前时刻和上一时刻；根据第一标记点位置、第二标记点位置、相邻两时刻的实际质心位置、相邻两时刻的实际航向角、相邻两时刻的实际前轮转角及预设公式分别确定相邻两时刻中每一个时刻的实际前轮转角误差和相邻两时刻之间的实际前轮转角误差变化；根据当前时刻的实际前轮转角误差、相邻两时刻之间的实际前轮转角误差变化及变论域模糊规则确定下一时刻的转角修正参数，并根据当前时刻的实际前轮转角误差、上一时刻的实际前轮转角误差及下一时刻的转角修正参数确定下一时刻目标机器人上电机的转向量，以调整目标机器人的航向。
7.在一些实施例中，根据当前时刻的实际前轮转角误差、相邻两时刻之间的实际前轮转角误差变化及变论域模糊规则确定下一时刻的转角修正参数，包括：根据当前时刻的实际前轮转角误差、相邻两时刻之间的实际前轮转角误差变化确定伸缩因子；根据当前时刻的实际前轮转角误差、相邻两时刻之间的实际前轮转角误差变化、伸缩因子及变论域模糊规则确定下一时刻的转角修正参数。
8.在一些实施例中，根据第一标记点位置、第二标记点位置、相邻两时刻的实际质心位置、相邻两时刻的实际航向角、相邻两时刻的实际前轮转角及预设公式分别确定相邻两时刻中每一个时刻的实际前轮转角误差和相邻两时刻之间的实际前轮转角误差变化，包括：根据第一标记点位置的坐标、第二标记点位置的坐标、上一时刻的实际质心位置的坐标、上一时刻的实际航向角确定上一时刻的前轮期望转角，并根据上一时刻的前轮期望转
角和上一时刻的实际前轮转角确定上一时刻的实际前轮转角误差；根据第一标记点位置的坐标、第二标记点位置的坐标、当前时刻的实际质心位置的坐标、当前时刻的实际航向角确定当前时刻的前轮期望转角，并根据当前时刻的前轮期望转角和当前时刻的实际前轮转角确定当前时刻的实际前轮转角误差；根据上一时刻的实际前轮转角误差和当前时刻的实际前轮转角误差确定相邻两时刻之间的实际前轮转角误差变化。
9.在一些实施例中，根据第一标记点位置的坐标、第二标记点位置的坐标、某一时刻的实际质心位置的坐标、某一时刻的实际航向角及如下公式确定某一时刻的前轮期望转角：
[0010][0011][0012]dθ
＝(1-λ1)k1ed λ1k2e
θ
[0013]
其中，ed为某一时刻的横向偏差，(x1,y1)为第一标记点位置的坐标，(x2,y2)为第二标记点位置的坐标，(x3,y3)为某一时刻的实际质心位置的坐标，e
θ
为某一时刻的航向角偏差，θ为某一时刻的实际航向角，d
θ
为某一时刻的前轮期望转角，λ1为偏差权重，k1为某一时刻的横向偏差增益，k2为某一时刻的航向角偏差增益。
[0014]
在一些实施例中，根据上一时刻的前轮期望转角、上一时刻的实际前轮转角及如下公式确定上一时刻的实际前轮转角误差：
[0015]
e(t-1)＝d
θ
(t-1)-m
θ
(t-1)
[0016]
根据当前时刻的前轮期望转角、当前时刻的实际前轮转角及如下公式确定当前时刻的实际前轮转角误差：
[0017]
e(t)＝d
θ
(t)-m
θ
(t)
[0018]
根据上一时刻的实际前轮转角误差、当前时刻的实际前轮转角误差及如下公式确定相邻两时刻之间的实际前轮转角误差变化：
[0019]
ec(t)＝e(t)-e(t-1)
[0020]
其中，e(t-1)为上一时刻的实际前轮转角误差，d
θ
(t-1)为上一时刻的前轮期望转角，m
θ
(t-1)为上一时刻的实际前轮转角，e(t)为当前时刻的实际前轮转角误差，d
θ
(t)为当前时刻的前轮期望转角，m
θ
(t)为当前时刻的实际前轮转角，ec(t)为相邻两时刻之间的实际前轮转角误差变化。
[0021]
在一些实施例中，根据当前时刻的实际前轮转角误差、相邻两时刻之间的实际前轮转角误差变化及变论域模糊规则确定下一时刻的转角修正参数，并根据所述当前时刻的实际前轮转角误差、所述上一时刻的实际前轮转角误差及所述下一时刻的转角修正参数确定下一时刻所述目标机器人上电机的转向量，以调整所述目标机器人的航向，包括：分别确定当前时刻的实际前轮转角误差、相邻两时刻之间的实际前轮转角误差变化、第一转角参数、第二转角参数及第三转角参数的基本论域；根据当前时刻的实际前轮转角误差和相邻两时刻之间的实际前轮转角误差变化确定第一伸缩因子和第二伸缩因子；根据当前时刻的
实际前轮转角误差的基本论域、相邻两时刻之间的实际前轮转角误差变化的基本论域、第一转角参数的基本论域、第二转角参数的基本论域、第三转角参数的基本论域、第一伸缩因子、第二伸缩因子及变论域模糊规则分别确定下一时刻的转角修正参数中的第一转角修正参数、第二转角修正参数和第三转角修正参数；根据当前时刻的实际前轮转角误差、相邻两时刻之间的实际前轮转角误差变化、第一转角修正参数、第二转角修正参数及第三转角修正参数确定下一时刻目标机器人上电机的转向量。
[0022]
在一些实施例中，根据当前时刻的实际前轮转角误差、相邻两时刻之间的实际前轮转角误差变化及如下公式确定第一伸缩因子和第二伸缩因子：
[0023][0024][0025]
根据当前时刻的实际前轮转角误差、相邻两时刻之间的实际前轮转角误差变化、第一转角修正参数、第二转角修正参数、第三转角修正参数及以下公式确定下一时刻目标机器人上电机的转向量：
[0026][0027]
其中，α(e
t
)为第一伸缩因子，λ2为调整权重，0＜λ2＜1，e为自然常数，k3为调整系数，β(e
ct
)为第二伸缩因子，λ3为调整权重，0＜λ3＜1，k4为调整系数，u为下一时刻目标机器人上电机的转向量，δk
p
、δki及δkd分别为第一转角修正参数、第二转角修正参数、第三转角修正参数。
[0028]
第二方面，本技术提供一种农用机器人行驶路径控制系统，采用上述中任一项的方法，系统包括：数据采集模块、数据融合模块及数据输出模块，数据采集模块用于获取目标机器人预设行驶路径中的第一标记点位置和第二标记点位置，并分别获取相邻两时刻目标机器人的实际质心位置、实际航向角及实际前轮转角，相邻两时刻分别为：当前时刻和上一时刻；数据融合模块用于根据第一标记点位置、第二标记点位置、相邻两时刻的实际质心位置、相邻两时刻的实际航向角及相邻两时刻的实际前轮转角分别确定相邻两时刻中每一个时刻的实际前轮转角误差和相邻两时刻之间的实际前轮转角误差变化；数据输出模块用于根据当前时刻的实际前轮转角误差、相邻两时刻之间的实际前轮转角误差变化及变论域模糊规则确定下一时刻的转角修正参数，并根据下一时刻的转角修正参数确定下一时刻目标机器人上电机的转向量，以调整目标机器人的航向。
[0029]
第三方面，本技术提供一种电子设备，电子设备包括：处理器、存储器、总线；其中，处理器、存储器通过总线完成相互间的通信；处理器用于调用存储器中的程序指令，以执行如上述中任一项的方法。
[0030]
第四方面，本技术提供一种计算机可读存储介质，计算机可读存储介质包括：存储的程序；其中，在程序运行时控制存储介质所在设备执行如上述中任一项的方法。
[0031]
相较于现有技术，本技术第一方面提供的农用机器人行驶路径控制方法，通过将相邻两时刻的实际前轮转角误差及下一时刻的转角修正参数结合起来去确定下一时刻目标机器人上电机的转向量，从而能够根据实时变化中的前轮转角误差对下一时刻目标机器
人上电机的转向量进行实时调整，以能够减小实际行驶路径和预设行驶路径之间的偏差。
[0032]
本技术第二方面提供的系统、第三方面提供的电子设备、第四方面提供的计算机可读存储介质，与第一方面提供的方法具有相同或相似的有益效果。
附图说明
[0033]
通过参考附图阅读下文的详细描述，本技术示例性实施方式的上述以及其他目的、特征和优点将变得易于理解。在附图中，以示例性而非限制性的方式示出了本技术的若干实施方式，相同或对应的标号表示相同或对应的部分，其中：
[0034]
图1为本技术一些实施例的农用机器人行驶路径控制方法的流程图；
[0035]
图2为本技术一些实施例的某一时刻下第一标记点位置、第二标记点位置及目标机器人的实际质心位置的坐标图；
[0036]
图3为本技术一些实施例的变论域模糊规则表；
[0037]
图4为本技术一些实施例的变论域模糊规则的系统框图。
具体实施方式
[0038]
下面将参照附图更详细地描述本技术的示例性实施方式。虽然附图中显示了本技术的示例性实施方式，然而应当理解，可以以各种形式实现本技术而不应被这里阐述的实施方式所限制。相反，提供这些实施方式是为了能够更透彻地理解本技术，并且能够将本技术的范围完整的传达给本领域的技术人员。
[0039]
需要注意的是，除非另有说明，本技术使用的技术术语或者科学术语应当为本技术所属领域技术人员所理解的通常意义。
[0040]
第一方面
[0041]
本技术实施例提供一种农用机器人行驶路径控制方法，参见图1所示，该方法包括：
[0042]
s101：获取目标机器人预设行驶路径中的第一标记点位置和第二标记点位置，并分别获取相邻两时刻中每一时刻目标机器人的实际质心位置、实际航向角及实际前轮转角，相邻两时刻分别为：当前时刻和上一时刻；
[0043]
s102：根据第一标记点位置、第二标记点位置、相邻两时刻的实际质心位置、相邻两时刻的实际航向角、相邻两时刻的实际前轮转角及预设公式分别确定相邻两时刻中每一个时刻的实际前轮转角误差和相邻两时刻之间的实际前轮转角误差变化；
[0044]
s103：根据当前时刻的实际前轮转角误差、相邻两时刻之间的实际前轮转角误差变化及变论域模糊规则确定下一时刻的转角修正参数，并根据当前时刻的实际前轮转角误差、上一时刻的实际前轮转角误差及下一时刻的转角修正参数确定下一时刻目标机器人上电机的转向量，以调整目标机器人的航向。
[0045]
具体来讲，上述中的农用机器人可以是能够喷洒药物的机器人，也可以是能够除草的机器人，也可以是能够施肥的机器人，也可以是能够采摘果实的机器人，还可以是具有其他功能的机器人。
[0046]
上述中的预设行驶路径可以为上层控制器规划出的行驶路径，也可以是人为规划出的行驶路径。上述中的第一标记点和第二标记点分别为目标机器人预设行驶路径中两个
不同的点，即第一标记点位置和第二标记点位置分别为目标机器人预设行驶路径中两个不同的位置，比如：参见图2所示，预设行驶路径中的一部分为线段ab，a为该段路径中的起始点，b为该段路径中的终止点，而第一标记点为a点，第二标记点为b点，第一标记点位置可以用a点的坐标(x1,y1)表示，第二标记点位置可以用b点的坐标(x1,y2)表示。上述中的分别获取相邻两时刻中每一时刻目标机器人的实际质心位置、实际航向角及实际前轮转角，相邻两时刻分别为：当前时刻和上一时刻，也就是说，获取当前时刻目标机器人的实际质心位置、实际航向角及实际前轮转角，并获取上一时刻目标机器人的实际质心位置、实际航向角及实际前轮转角，这里，实际质心位置即目标机器人的质量中心所在的位置，比如：参见图2所示，当前时刻目标机器人的实际质心位置为c点所在的位置，而实际质心位置可以用c点的坐标表示。需要说明的是，实际航向角、实际前轮转角可以通过角度传感器等仪器获取。
[0047]
上述中的根据第一标记点位置、第二标记点位置、相邻两时刻的实际质心位置、相邻两时刻的实际航向角、相邻两时刻的实际前轮转角及预设公式分别确定相邻两时刻中每一个时刻的实际前轮转角误差和相邻两时刻之间的实际前轮转角误差变化，也就是说，根据第一标记点位置、第二标记点位置、相邻两时刻中每一时刻的实际质心位置、相邻两时刻中每一时刻的实际航向角、相邻两时刻中每一时刻的实际前轮转角及预设公式能够分别确定当前时刻的实际前轮转角误差、上一时刻的实际前轮转角误差及当前时刻和上一时刻之间的实际前轮转角误差变化。本技术中提出两种预设公式，当然在具体实施时也可以是其他预设公式，此处不作特别限定。
[0048]
预设公式一，参考图2：
[0049][0050][0051][0052]dθ
＝(1-λ1)ed λ1e
θ
[0053]
e(t-1)＝d
θ
(t-1)-m
θ
(t-1)
[0054]
e(t)＝d
θ
(t)-m
θ
(t)
[0055]
ec(t)＝e(t)-e(t-1)
[0056]
其中，ed为某一时刻的横向偏差，(x1,y1)为第一标记点位置的坐标，(x2,y2)为第二标记点位置的坐标，(x3,y3)为某一时刻的实际质心位置的坐标，e
θ
为某一时刻的航向角偏差，θ为某一时刻的实际航向角，d
θ
为某一时刻的前轮期望转角，λ1为偏差权重，e(t-1)为上一时刻的实际前轮转角误差，d
θ
(t-1)为上一时刻的前轮期望转角，m
θ
(t-1)为上一时刻的实际前轮转角，e(t)为当前时刻的实际前轮转角误差，d
θ
(t)为当前时刻的前轮期望转角，m
θ
(t)为当前时刻的实际前轮转角，ec(t)为相邻两时刻之间的实际前轮转角误差变化。需要说明的是，λ1可以是数据库内记录的多次实验的最优值，也可以是根据经验设定的值。
[0057]
预设公式二，参考图2：
[0058][0059][0060][0061]dθ
＝(1-λ1)k1ed λ1k2e
θ
[0062]
e(t-1)＝d
θ
(t-1)-m
θ
(t-1)
[0063]
e(t)＝d
θ
(t)-m
θ
(t)
[0064]
ec(t)＝e(t)-e(t-1)
[0065]
其中，ed为某一时刻的横向偏差，(x1,y1)为第一标记点位置的坐标，(x2,y2)为第二标记点位置的坐标，(x3,y3)为某一时刻的实际质心位置的坐标，e
θ
为某一时刻的航向角偏差，θ为某一时刻的实际航向角，d
θ
为某一时刻的前轮期望转角，λ1为偏差权重，k1为某一时刻的横向偏差增益，k2为某一时刻的航向角偏差增益，e(t-1)为上一时刻的实际前轮转角误差，d
θ
(t-1)为上一时刻的前轮期望转角，m
θ
(t-1)为上一时刻的实际前轮转角，e(t)为当前时刻的实际前轮转角误差，d
θ
(t)为当前时刻的前轮期望转角，m
θ
(t)为当前时刻的实际前轮转角，ec(t)为相邻两时刻之间的实际前轮转角误差变化。需要说明的是，λ1、k1、k2可以是数据库内记录的多次实验的最优值，也可以是根据经验设定的值。
[0066]
上述中的根据当前时刻的实际前轮转角误差、相邻两时刻之间的实际前轮转角误差变化及变论域模糊规则确定下一时刻的转角修正参数，这里，变论域模糊规则即遵循变论域模糊pid思想的规则，而所确定的转角修正参数能够结合当前时刻的实际前轮转角误差和相邻两时刻之间的实际前轮转角误差变化以对下一时刻目标机器人上电机的转向量进行确定，从而来调整目标机器人的航向以使目标机器人的航向与种植园道路的实际情况相适配。
[0067]
本实施例中，其一，通过将相邻两时刻的实际前轮转角误差及下一时刻的转角修正参数结合起来去确定下一时刻目标机器人上电机的转向量，从而能够根据实时变化中的前轮转角误差对下一时刻目标机器人上电机的转向量进行实时调整，以能够减小实际行驶路径和预设行驶路径之间的偏差。其二，在常规模糊pid控制器基础上，引入了变论域模糊pid的思想，大大提高了抗外部干扰的性能，解决了常规路径跟踪模型在存在外部干扰时，pid算法会引入外部干扰导致控制系统不稳定的问题，从而对农用机器人的运动进行更加精确控制。
[0068]
在一些实施例中，根据当前时刻的实际前轮转角误差、相邻两时刻之间的实际前轮转角误差变化及变论域模糊规则确定下一时刻的转角修正参数，包括：
[0069]
根据当前时刻的实际前轮转角误差、相邻两时刻之间的实际前轮转角误差变化确定伸缩因子；
[0070]
根据当前时刻的实际前轮转角误差、相邻两时刻之间的实际前轮转角误差变化、伸缩因子及变论域模糊规则确定下一时刻的转角修正参数。
[0071]
具体来讲，上述中的根据当前时刻的实际前轮转角误差、相邻两时刻之间的实际前轮转角误差变化确定伸缩因子，其可以是根据当前时刻的实际前轮转角误差、相邻两时刻之间的实际前轮转角误差变化与以往实验数据比对，以在以往数据中找到与当前时刻的实际前轮转角误差、相邻两时刻之间的实际前轮转角误差变化这两者接近的数实验，并将最接近的实验中的伸缩因子作为本实施例的伸缩因子。
[0072]
上述中的根据当前时刻的实际前轮转角误差、相邻两时刻之间的实际前轮转角误差变化确定伸缩因子，其也可以是通过下方公式确定，公式中的α(e
t
)为第一伸缩因子，λ2为调整权重，0＜λ2＜1，e为自然常数，k3为调整系数，β(e
ct
)为第二伸缩因子，λ3为调整系数，0＜λ3＜1，k4为调整系数，λ2、λ3、k3及k4可以是以往的实验数据，也可以是根据经验设定的：
[0073][0074][0075]
本实施例中，其一，通过伸缩因子能够进一步提升所确定的下一时刻的转角修正参数的准确性，由此，使得最终确定的下一时刻目标机器人上电机的转向量精准度更高，以能够进一步减小目标机器人的实际行驶路径与预设行驶路径之间的偏差；其二，在常规模糊pid控制器基础上，引入了变论域模糊pid的思想，通过当前时刻实际前轮转角误差e(t)、当前时刻和上一时刻之间的实际前轮转角误差变化ec(t)来伸缩控制下一时刻目标机器人上电机的转向量，从而使系统具有更好的鲁棒性。
[0076]
在一些实施例中，根据第一标记点位置、第二标记点位置、相邻两时刻的实际质心位置、相邻两时刻的实际航向角、相邻两时刻的实际前轮转角及预设公式分别确定相邻两时刻中每一个时刻的实际前轮转角误差和相邻两时刻之间的实际前轮转角误差变化，包括：
[0077]
根据第一标记点位置的坐标、第二标记点位置的坐标、上一时刻的实际质心位置的坐标、上一时刻的实际航向角确定上一时刻的前轮期望转角，并根据上一时刻的前轮期望转角和上一时刻的实际前轮转角确定上一时刻的实际前轮转角误差；
[0078]
根据第一标记点位置的坐标、第二标记点位置的坐标、当前时刻的实际质心位置的坐标、当前时刻的实际航向角确定当前时刻的前轮期望转角，并根据当前时刻的前轮期望转角和当前时刻的实际前轮转角确定当前时刻的实际前轮转角误差；
[0079]
根据上一时刻的实际前轮转角误差和当前时刻的实际前轮转角误差确定相邻两时刻之间的实际前轮转角误差变化。
[0080]
这里，上一时刻的实际前轮转角和当前时刻的实际前轮转角可以通过角度传感器等设备获取。
[0081]
本实施例中，通过上一时刻的数据确定上一时刻的实际前轮转角误差，并通过当前时刻的数据确定当前时刻的实际前轮转角误差，进而来确定相邻两时刻之间的实际前轮转角误差变化，从而使得所确定的实际前轮转角误差变化更精确。
[0082]
在本技术一些实施例中，可以根据第一标记点位置的坐标、第二标记点位置的坐标、某一时刻的实际质心位置的坐标、某一时刻的实际航向角及如下公式确定某一时刻的前轮期望转角：
[0083][0084][0085]dθ
＝(1-λ1)k1ed λ1k2e
θ
[0086]
其中，ed为某一时刻的横向偏差，(x1,y1)为第一标记点位置的坐标，(x2,y2)为第二标记点位置的坐标，(x3,y3)为某一时刻的实际质心位置的坐标，e
θ
为某一时刻的航向角偏差，θ为某一时刻的实际航向角，d
θ
为某一时刻的前轮期望转角，λ1为偏差权重，k1为某一时刻的横向偏差增益，k2为某一时刻的航向角偏差增益。
[0087]
在本技术另一些实施例中，可以根据第一标记点位置的坐标、第二标记点位置的坐标、某一时刻的实际质心位置的坐标、某一时刻的实际航向角及如下公式确定某一时刻的前轮期望转角：
[0088][0089][0090]dθ
＝(1-λ1)ed λ1e
θ
[0091]
其中，ed为某一时刻的横向偏差，(x1,y1)为第一标记点位置的坐标，(x2,y2)为第二标记点位置的坐标，(x3,y3)为某一时刻的实际质心位置的坐标，e
θ
为某一时刻的航向角偏差，θ为某一时刻的实际航向角，d
θ
为某一时刻的前轮期望转角，λ1为偏差权重。
[0092]
在一些实施例中，根据上一时刻的前轮期望转角、上一时刻的实际前轮转角及如下公式确定上一时刻的实际前轮转角误差：
[0093]
e(t-1)＝d
θ
(t-1)-m
θ
(t-1)
[0094]
根据当前时刻的前轮期望转角、当前时刻的实际前轮转角及如下公式确定当前时刻的实际前轮转角误差：
[0095]
e(t)＝d
θ
(t)-m
θ
(t)
[0096]
根据上一时刻的实际前轮转角误差、当前时刻的实际前轮转角误差及如下公式确定相邻两时刻之间的实际前轮转角误差变化：
[0097]
ec(t)＝e(t)-e(t-1)
[0098]
其中，e(t-1)为上一时刻的实际前轮转角误差，d
θ
(t-1)为上一时刻的前轮期望转角，m
θ
(t-1)为上一时刻的实际前轮转角，e(t)为当前时刻的实际前轮转角误差，d
θ
(t)为当前时刻的前轮期望转角，m
θ
(t)为当前时刻的实际前轮转角，ec(t)为相邻两时刻之间的实际前轮转角误差变化。
[0099]
在一些实施例中，根据当前时刻的实际前轮转角误差、相邻两时刻之间的实际前
轮转角误差变化及变论域模糊规则确定下一时刻的转角修正参数，并根据当前时刻的实际前轮转角误差、上一时刻的实际前轮转角误差及下一时刻的转角修正参数确定下一时刻目标机器人上电机的转向量，以调整目标机器人的航向，包括：
[0100]
分别确定当前时刻的实际前轮转角误差、相邻两时刻之间的实际前轮转角误差变化、第一转角参数、第二转角参数及第三转角参数的基本论域；
[0101]
根据当前时刻的实际前轮转角误差和相邻两时刻之间的实际前轮转角误差变化确定第一伸缩因子和第二伸缩因子；
[0102]
根据当前时刻的实际前轮转角误差的基本论域、相邻两时刻之间的实际前轮转角误差变化的基本论域、第一转角参数的基本论域、第二转角参数的基本论域、第三转角参数的基本论域、第一伸缩因子、第二伸缩因子及变论域模糊规则分别确定下一时刻的转角修正参数中的第一转角修正参数、第二转角修正参数和第三转角修正参数；
[0103]
根据当前时刻的实际前轮转角误差、相邻两时刻之间的实际前轮转角误差变化、第一转角修正参数、第二转角修正参数及第三转角修正参数确定下一时刻目标机器人上电机的转向量。
[0104]
本实施例中，根据当前时刻的实际前轮转角误差和相邻两时刻之间的实际前轮转角误差变化确定第一伸缩因子和第二伸缩因子，并根据第一伸缩因子和第二伸缩因子对当前时刻的实际前轮转角误差、相邻两时刻之间的实际前轮转角误差变化、第一转角参数、第二转角参数及第三转角参数的基本论域的基本论域进行调整，即在常规模糊pid控制器基础上，引入了变论域模糊pid的思想，通过伸缩因子基于函数类型的变论域模糊控制器，大大提高了抗外部干扰的性能，解决了常规路径跟踪模型在存在外部干扰时，pid算法会引入外部干扰导致控制系统不稳定的问题，从而对农用机器人的运动进行更加精确控制。
[0105]
在一些实施例中，根据当前时刻的实际前轮转角误差、相邻两时刻之间的实际前轮转角误差变化及如下公式确定第一伸缩因子和第二伸缩因子：
[0106][0107][0108]
根据当前时刻的实际前轮转角误差、相邻两时刻之间的实际前轮转角误差变化、第一转角修正参数、第二转角修正参数、第三转角修正参数及以下公式确定下一时刻目标机器人上电机的转向量：
[0109][0110]
其中，α(e
t
)为第一伸缩因子，λ2为调整权重，0＜λ2＜1，e为自然常数，k3为调整系数，β(e
ct
)为第二伸缩因子，λ3为调整权重，0＜λ3＜1，k4为调整系数，u为下一时刻目标机器人上电机的转向量，δk
p
、δki及δkd分别为第一转角修正参数、第二转角修正参数、第三转角修正参数。
[0111]
一些实施例中，参见图2至4所示，农用机器人行驶路径控制方法如下：
[0112]
获取预设行驶路径中第一标记点位置a的坐标(x1,y1)和预设行驶路径中第二标记点位置b的坐标(x2,y2)，并获取目标机器人地球坐标系下当前时刻的质心坐标(x3,y3)、当
前时刻的实际航向角θ及当前时刻的实际前轮转角m
θ
(t)。其中，ab为预设行驶路径的最小单元段，a点为该最小单元段的起点，b点为该最小单元短的终点，实际前轮转角通过角度传感器测量。
[0113]
根据第一标记点位置a的坐标(x1,y1)、第二标记点位置b的坐标(x2,y2)、当前时刻的质心坐标(x3,y3)及如下公式确定当前时刻目标机器人的横向偏差ed(t)：
[0114][0115]
根据目标机器人当前时刻的实际航向角θ、第一标记点位置a的坐标(x1,y1)、第二标记点位置b的坐标(x2,y2)及如下公式确定当前时刻目标机器人的航向角偏差e
θ
(t)：
[0116][0117][0118]
根据偏差权重λ1、当前时刻的实际横向偏差增益k1、当前时刻的实际航向角偏差增益k2、当前时刻目标机器人的横向偏差ed(t)、当前时刻目标机器人的航向角偏差e
θ
(t)及如下公式确定当前时刻的前轮期望转角d
θ
(t)：
[0119]dθ
(t)＝(1-λ1)k1ed(t) λ1k2e
θ
(t)
[0120]
根据当前时刻的前轮期望转角d
θ
(t)、实际前轮转角m
θ
(t)及以下公式确定当前时刻实际前轮转角误差e(t)：
[0121]
e(t)＝d
θ
(t)-m
θ
(t)
[0122]
根据当前时刻实际前轮转角误差e(t)、上一时刻实际前轮转角误差e(t-1)及以下公式确定当前时刻和上一时刻之间的实际前轮转角误差变化ec(t)，上一时刻实际前轮转角误差e(t-1)的计算与本实施例中当前时刻实际前轮转角误差e(t)的计算方式一致，此处不再赘述：
[0123]
ec(t)＝e(t)-e(t-1)
[0124]
根据当前时刻实际前轮转角误差e(t)、当前时刻和上一时刻之间的实际前轮转角误差变化ec(t)、调整权重λ2和λ3，0＜λ2＜1，0＜λ3＜1、自然常数e、调整系数k3和k4，k3＞0，k4＞0及以下公式确定第一伸缩因子α(e
t
)和第二伸缩因子β(e
ct
)；
[0125][0126][0127]
如图4所示，根据变论域模糊pid算法(即变论域模糊规则)，上层规划控制器进行预设路径的输入，经由数据融合期望转角得到当前时刻实际前轮转角误差e(t)及当前时刻和上一时刻之间的实际前轮转角误差变化ec(t)，进而以当前时刻实际前轮转角误差e(t)及当前时刻和上一时刻之间的实际前轮转角误差变化ec(t)作为t时刻变论域模糊pid算法的输入信号输入，经控制器i整定后输出第一伸缩因子和第二伸缩因子至控制器ⅱ。
[0128]
设当前时刻的实际前轮转角误差的基本论域为[-ε1,ε1]，相邻两时刻之间的实际
前轮转角误差变化的基本论域为[-ε2,ε2]，第一转角参数的基本论域为[-u1,u1]，第二转角参数的基本论域为[-u2,u2]，第三转角参数的基本论域为[-u3,u3]。这里，基本论域根据经验进行确定。
[0129]
根据当前时刻的实际前轮转角误差的基本论域为[-ε1,ε1]，相邻两时刻之间的实际前轮转角误差变化的基本论域为[-ε2,ε2]，第一转角参数的基本论域为[-u1,u1]，第二转角参数的基本论域为[-u2,u2]，第三转角参数的基本论域为[-u3,u3]、第一伸缩因子α(e
t
)及第二伸缩因子β(e
ct
)确定当前时刻的实际前轮转角误差的调整论域e
t
、相邻两时刻之间的实际前轮转角误差变化的调整论域e
ct
、第一转角参数的调整论域k
p
、第二转角参数的调整论域ki、第三转角参数的调整论域kd；
[0130]et
＝[-α(e
t
)ε1,α(e
t
)ε1]
[0131]ect
＝[-α(e
t
)ε2,α(e
t
)ε2]
[0132]kp
＝[-β(e
ct
)u1,β(e
ct
)u1]
[0133]ki
＝[-β(e
ct
)u2,β(e
ct
)u2]
[0134]
kd＝[-β(e
ct
)u3,β(e
ct
)u3]
[0135]
设定当前时刻的实际前轮转角误差的调整论域e
t
、相邻两时刻之间的实际前轮转角误差变化的调整论域e
ct
、第一转角参数的调整论域k
p
、第二转角参数的调整论域ki、第三转角参数的调整论域kd的模糊子集均为{nb nm ns z ps pm pb}，[nb]负大、[nm]负中、[ns]负小、[zo]零、[ps]正小、[pm]正中、[pb]正大，将调整论域范围内连续变化的量分级离散化，然后进行模糊处理。选用三角形函数将论域分为其隶属度函数，根据隶属度函数求取录属度，模糊规则如图3所示。示例性地，当前时刻的实际前轮转角误差e(t)的调整论域e
t
[-3,3]、相邻两时刻之间的实际前轮转角误差变化ec(t)的调整论域e
ct
[-3,3]，则图3中的nb＝-3，nm＝-2，ns＝-1，z＝0，ps＝1，pm＝2，pb＝3，而e(t)＝-3，ec(t)＝-3，则相交点为[pb、nb、ps]，继而在第一转角参数的调整论域k
p
的模糊子集中找对应pb的值即为第一转角修正参数δk
p
、在第二转角参数的调整论域ki的模糊子集中找对应nb的值即为第二转角修正参数δki、在第三转角参数的调整论域kd的模糊子集中找对应ps的值即为第三转角修正参数δkd。
[0136]
经由控制器ⅱ模糊算法后输出δk
p
、δki、δkd，进而将进入pid控制器，通过以下公式确定下一时刻目标机器人上电机的转向量u。
[0137][0138]
同时，对输出量进行限幅，积分项饱和时，误差仍在积分作用下累计，一旦误差开始反向变化，系统需要时间从饱和区退出。因而输出到达最小最大时，应停止积分作用，进行积分限幅和输出限幅，δ，为限幅边界。
[0139][0140][0141]
该判决结果通过输出伸缩因子整定后得到修正参数δk
p
、δki、δkd输入至pid控制器进而得到该作业状态下的工况参数输出的转向电机转向速度数字量。
[0142]
示例性地，果园施肥机器人在果园中接收到上层控制器规划出的期望的规划路径，与通过传感器测出的实际路线进行做差值，得出横向偏差和航向角偏差，针对pid控制器对模型依赖性强、参数整定困难，动态调整范围较小等不足，在增益调整型模糊pid控制器的基础上，引入变论域思想，根据系统响应各个阶段的e
t
、e
ct
运行轨迹，设计了一个论域伸缩因子自适应调整机构，在线调整系统运行各个阶段的输入输出变量与模糊子集的映射关系，实现论域随着系统的控制要求进行伸缩变化，改善了系统的动态特性和稳态精度，提高了系统的动态调节能力，最终实现机器人的轨迹跟踪的进度和机器人行驶的稳定性。
[0143]
本实施例中，在常规模糊pid控制器基础上，引入了变论域模糊pid的思想，通过伸缩因子基于函数类型的变论域模糊控制器，大大提高了抗外部干扰的性能，解决了常规路径跟踪模型在存在外部干扰时，pid算法会引入外部干扰导致控制系统不稳定的问题，从而对果园智能施肥小车等农用机器人的运动进行更加精确控制，采用变论域自适应模糊控制实现果园机器人一类非线性系统的稳定自适应控制，具有良好的稳定性和鲁棒性，以能够准确实现控制目标。
[0144]
第二方面
[0145]
基于同一发明构思，参见图4所示，本技术提供一种农用机器人行驶路径控制系统，采用上述中任一项的方法。系统包括：数据采集模块、数据融合模块及数据输出模块。数据采集模块用于获取目标机器人预设行驶路径中的第一标记点位置和第二标记点位置，并分别获取相邻两时刻目标机器人的实际质心位置、实际航向角及实际前轮转角，相邻两时刻分别为：当前时刻和上一时刻。数据融合模块用于根据第一标记点位置、第二标记点位置、相邻两时刻的实际质心位置、相邻两时刻的实际航向角及相邻两时刻的实际前轮转角分别确定相邻两时刻中每一个时刻的实际前轮转角误差和相邻两时刻之间的实际前轮转角误差变化。数据输出模块用于根据当前时刻的实际前轮转角误差、相邻两时刻之间的实际前轮转角误差变化及变论域模糊规则确定下一时刻的转角修正参数，并根据下一时刻的转角修正参数确定下一时刻目标机器人上电机的转向量，以调整目标机器人的航向。
[0146]
本实施例中，通过将相邻两时刻的实际前轮转角误差及下一时刻的转角修正参数结合起来去确定下一时刻目标机器人上电机的转向量，从而能够根据实时变化中的前轮转角误差对下一时刻目标机器人上电机的转向量进行实时调整，以能够减小实际行驶路径和预设行驶路径之间的偏差。其二，在常规模糊pid控制器基础上，引入了变论域模糊pid的思想，大大提高了抗外部干扰的性能，解决了常规路径跟踪模型在存在外部干扰时，pid算法
会引入外部干扰导致控制系统不稳定的问题，从而对农用机器人的运动进行更加精确控制。
[0147]
这里需要指出的是，以上系统实施例的描述，与上述方法实施例的描述是类似的，具有同方法实施例相似的有益效果。对于本技术系统实施例中未披露的技术细节，请参照本技术方法实施例的描述而理解。
[0148]
第三方面
[0149]
基于同一发明构思，本技术实施例提供一种电子设备，电子设备包括：处理器、存储器、总线；其中，处理器、存储器通过总线完成相互间的通信；处理器用于调用存储器中的程序指令，以执行如上述中任一项的方法。
[0150]
这里需要指出的是，以上电子设备实施例的描述，与上述方法实施例的描述是类似的，具有同方法实施例相似的有益效果。对于本技术电子设备实施例中未披露的技术细节，请参照本技术方法实施例的描述而理解。
[0151]
第四方面
[0152]
基于同一发明构思，本技术实施例提供一种计算机可读存储介质，计算机可读存储介质包括：存储的程序；其中，在程序运行时控制存储介质所在设备执行如上述中任一项的方法。
[0153]
这里需要指出的是，以上存储介质实施例的描述，与上述方法实施例的描述是类似的，具有同方法实施例相似的有益效果。对于本技术存储介质实施例中未披露的技术细节，请参照本技术方法实施例的描述而理解。
[0154]
以上所述，仅为本技术的具体实施方式，但本技术的保护范围并不局限于此，任何熟悉本技术领域的技术人员在本技术揭露的技术范围内，可轻易想到变化或替换，都应涵盖在本技术的保护范围之内。因此，本技术的保护范围应以所述权利要求的保护范围为准。