一种履带式果园割草机路径规划和跟踪控制方法

本发明涉及果园割草机，特别是涉及一种履带式果园割草机路径规划和跟踪控制方法。

背景技术：

1、在土地流转的大趋势下，规模化种植为农业机械智能化发展创造了条件。基于global positioning system、glonass、galileo和beidou navigation satellite system的定位导航系统越来越多的应用于精准农业。自动驾驶系统由信息感知模块、定位测姿模块、路径规划模块、跟踪控制模块组成。而路径规划和跟踪控制是自动驾驶在工程应用领域实现落地的有效支撑。作为常见农业机器人的一种，履带式果园割草机可以自主地感知外界的环境信息，对自身的姿态和路径作出决策和规划，并自主地控制自身的行为进行作业。

2、但是，在土壤特性多变的高度非结构化果园环境中，割草机会受到许多不确定性因素干扰，这给精确地按照既定轨迹自动驾驶带来了极大困难。因此，设计一种履带式果园割草机路径规划和跟踪控制方法是十分有必要的。

技术实现思路

1、本发明的目的是提供一种履带式果园割草机路径规划和跟踪控制方法，能够实现割草机路径规划及跟踪控制，能够解决我国果园割草机自动驾驶易跑偏，导致漏割面积较大的问题。

2、为实现上述目的，本发明提供了如下方案：

3、一种履带式果园割草机路径规划和跟踪控制方法，应用于一种履带式果园割草机路径规划和跟踪控制系统，包括：路径规划模块及跟踪控制模块；

4、该方法包括如下步骤：

5、步骤1：路径规划模块对传统a*算法进行优化处理，基于优化处理后的算法进行路径规划；

6、步骤2：跟踪控制模块以履带式果园割草机为控制对象建立运动学模型，基于模糊控制思想及滑膜变结构控制理论，构建模糊滑膜趋近律。

7、可选的，步骤1中，路径规划模块对传统a*算法进行优化处理，具体包括如下步骤：

8、步骤101：路径规划模块基于传统a*算法引入变权重的启发式函数、惩罚函数和五次贝塞尔曲线，对传统a*算法进行平滑化处理；

9、步骤102：在平滑化处理的基础上，引入蚁群算法，对传统a*算法进行优化处理。

10、可选的，步骤101中，路径规划模块基于传统a*算法引入变权重的启发式函数、惩罚函数和五次贝塞尔曲线，对传统a*算法进行平滑化处理，具体为：

11、基于传统a*算法，利用五次贝塞尔曲线对传统a*算法规划的路径进行平滑处理，其中，五次贝塞尔曲线的表达式为：

12、p(u)＝(1-u)5p0+5(1-u)4up1+10(1-u)3u2p2+10(1-u)3p3+5(1-u)4p4+u5p5 (1)

13、将其转换为矩阵形式，为：

14、

15、式中，pi(i＝1,2,…,5)为五次贝塞尔曲线的6个控制点，ub为贝塞尔曲线参数，ub∈[0,1]，中间的常数矩阵为贝塞尔曲线的系数矩阵，将贝塞尔曲线在任意一点的曲率k表示为：

16、

17、设传统a*算法规划的路径为：

18、ppath＝(px,py) (4)

19、式中，(px,py)为路径的节点坐标；

20、将ppath作为五次贝塞尔曲线的控制点并计算对应的贝塞尔曲线，进行路径平滑处理，计算优化后的路径曲线曲率c为：

21、c(ub)＝p′x(ub)p"y(ub)-p′y(ub)p"x(ub)/||p′x(ub)+p′y(ub)||3/2 (5)

22、式中，p′x和p′y为对应控制节点p处五次贝塞尔曲线关于ub的一阶偏导，p″x和p″y为对应控制节点p处五次贝塞尔曲线关于u的二阶偏导；

23、设计评估代价值计算方式为：

24、

25、式中，α、β为常系数；

26、设计评价函数为：

27、f(n)＝g(n)+h(n)+q(n) (7)

28、当搜索过程节点偏离理论最短路线时，对该节点n实施惩罚，惩罚函数为：

29、ax+by+c＝0 (8)

30、

31、可选的，步骤102中，在平滑化处理的基础上，引入蚁群算法，对传统a*算法进行优化处理，具体为：

32、基于蚁群算法对优化后的a*算法进行优化，设有ρ个节点，割草机按照编号顺序x＝(x1，x2，...，xρ)进行行走，行走距离为v(x)，构建数学表达式，求解一组顺序x，使得行走距离v(x)最小化，为：

33、

34、

35、式中，d为节点间的距离矩阵，满足d＝dt，dij为节点编号i到j的可行路径距离，为dij＝dji，i，j∈[1,ρ]，d(xi，xj)为以编号xi为起点、xj为终点的距离dxixj，在距离矩阵d的基础上，基于蚁群算法求解一组顺序x，使得距离函数v(x)最小化，得到最终的优化路径。

36、5、根据权利要求1所述的履带式果园割草机路径规划和跟踪控制方法，其特征在于，步骤2中，跟踪控制模块以履带式果园割草机为控制对象建立运动学模型，基于模糊控制思想及滑膜变结构控制理论，构建模糊滑膜趋近律，具体为：

37、基于跟踪控制模块通过履带式果园割草机机身与期望路径之间的几何关系，设计f-smc跟踪控制器，以履带式果园割草机为控制对象建立运动学模型，根据运动学模型设计位置控制律及姿态控制律，并在姿态控制律的基础上进行模糊控制的设计。

38、根据本发明提供的具体实施例，本发明公开了以下技术效果：本发明提供的履带式果园割草机路径规划和跟踪控制方法，该方法包括路径规划模块对传统a*算法进行优化处理，基于优化处理后的算法进行路径规划，跟踪控制模块以履带式果园割草机为控制对象建立运动学模型，基于模糊控制思想及滑膜变结构控制理论，构建模糊滑膜趋近律，该方法能够实现割草机路径规划及跟踪控制，能够解决我国果园割草机自动驾驶易跑偏，导致漏割面积较大的问题。

技术特征：

1.一种履带式果园割草机路径规划和跟踪控制方法，应用于一种履带式果园割草机路径规划和跟踪控制系统，包括：路径规划模块及跟踪控制模块，其特征在于，该方法包括如下步骤：

2.根据权利要求1所述的履带式果园割草机路径规划和跟踪控制方法，其特征在于，步骤1中，路径规划模块对传统a*算法进行优化处理，具体包括如下步骤：

3.根据权利要求2所述的履带式果园割草机路径规划和跟踪控制方法，其特征在于，步骤101中，路径规划模块基于传统a*算法引入变权重的启发式函数、惩罚函数和五次贝塞尔曲线，对传统a*算法进行平滑化处理，具体为：

4.根据权利要求2所述的履带式果园割草机路径规划和跟踪控制方法，其特征在于，步骤102中，在平滑化处理的基础上，引入蚁群算法，对传统a*算法进行优化处理，具体为：

5.根据权利要求1所述的履带式果园割草机路径规划和跟踪控制方法，其特征在于，步骤2中，跟踪控制模块以履带式果园割草机为控制对象建立运动学模型，基于模糊控制思想及滑膜变结构控制理论，构建模糊滑膜趋近律，具体为：

技术总结本发明提供了一种履带式果园割草机路径规划和跟踪控制方法，包括：路径规划模块对传统A*算法进行优化处理，基于优化处理后的算法进行路径规划，跟踪控制模块以履带式果园割草机为控制对象建立运动学模型，基于模糊控制思想及滑膜变结构控制理论，构建模糊滑膜趋近律。本发明提供的履带式果园割草机路径规划和跟踪控制方法，能够实现割草机路径规划及跟踪控制，能够解决我国果园割草机自动驾驶易跑偏，导致漏割面积较大的问题。技术研发人员：刘丽星,王旭,刘洪杰,杨欣受保护的技术使用者：河北农业大学技术研发日：技术公布日：2024/6/18