Welcome to Smart Agriculture 中文
Special Issue--Artificial Intelligence and Robot Technology for Smart Agriculture

Path Planning and Motion Control Method for Sick and Dead Animal Transport Robots Integrating Improved A * Algorithm and Fuzzy PID

  • XU Jishuang , 1, 2 ,
  • JIAO Jun 1 ,
  • LI Miao 2 ,
  • LI Hualong , 2 ,
  • YANG Xuanjiang 2 ,
  • LIU Xianwang 2 ,
  • GUO Panpan 2 ,
  • MA Zhirun 2
Expand
  • 1. School of Information and Computer Science, Anhui Agricultural University, Hefei 230036, China
  • 2. Hefei Institute of Materials Chinese Academy of Sciences, Institute of Mechanical Intelligence, Hefei 230031, China
LI Hualong, E-mail:

Received date: 2023-07-27

  Online published: 2023-12-20

Supported by

National Key Research and Development Program of China(2022YFD2002104)

National Natural Science Foundation of China(31902205)

Copyright

copyright©2023 by the authors

Abstract

[Objective] A key challenge for the harmless treatment center of sick and dead animal is to prevent secondary environmental pollution, especially during the process of transporting the animals from cold storage to intelligent treatment facilities. In order to solve this problem and achieve the intelligent equipment process of transporting sick and dead animal from storage cold storage to harmless treatment equipment in the harmless treatment center, it is necessary to conduct in-depth research on the key technical problems of path planning and autonomous walking of transport robots. [Methods] A * algorithm is mainly adopted for the robot path planning algorithm for indoor environments, but traditional A * algorithms have some problems, such as having many inflection points, poor smoothness, long calculation time, and many traversal nodes. In order to solve these problems, a path planning method for the harmless treatment of diseased and dead animal using transport robots based on the improved A algorithm was constructed, as well as a motion control method based on fuzzy proportional integral differential (PID). The Manhattan distance method was used to replace the heuristic function of the traditional A * algorithm, improving the efficiency of calculating the distance between the starting and ending points in the path planning process. Referring to the actual location of the harmless treatment site for sick and dead animal, vector cross product calculation was performed based on the vector from the starting point to the target point and the vector from the current position to the endpoint target. Additional values were added and dynamic adjustments were implemented, thereby changing the value of the heuristic function. In order to further improve the efficiency of path planning and reduce the search for nodes in the planning process, a method of adding function weights to the heuristic function was studied based on the actual situation on site, to change the weights according to different paths. When the current location node was relatively open, the search efficiency was improved by increasing the weight. When encountering situations such as corners, the weight was reduced to improve the credibility of the path. By improving the heuristic function, a driving path from the starting point to the endpoint was quickly obtained, but the resulting path was not smooth enough. Meanwhile, during the tracking process, the robot needs to accelerate and decelerate frequently to adapt to the path, resulting in energy loss. Therefore, according to the different inflection points and control points of the path, different orders of Bessel functions were introduced to smooth the planning process for the path, in order to achieve practical application results. By analyzing the kinematics of robot, the differential motion method of the track type was clarified. On this basis, a walking control algorithm for the robot based on fuzzy PID control was studied and proposed. Based on the actual operation status of the robot, the fuzzy rule conditions were recorded into a fuzzy control rule table, achieving online identification of the characteristic parameters of the robot and adjusting the angular velocity deviation of robot. When the robot controller received a fuzzy PID control signal, the angular velocity output from the control signal was converted into a motor rotation signal, which changed the motor speed on both sides of the robot to achieve differential control and adjust the steering of the robot. [Results and Discussions] Simulation experiments were conducted using the constructed environmental map obtained, verifying the effectiveness of the path planning method for the harmless treatment of sick and dead animal using the improved A algorithm. The comparative experiments between traditional A * algorithm and improved algorithm were conducted. The experimental results showed that the average traversal nodes of the improved A * algorithm decreased from 3 067 to 1 968, and the average time of the algorithm decreased from 20.34 s to 7.26 s. Through on-site experiments, the effectiveness and reliability of the algorithm were further verified. Different colors were used to identify the planned paths, and optimization comparison experiments were conducted on large angle inflection points, U-shaped inflection points, and continuous inflection points in the paths, verifying the optimization effect of the Bessel function on path smoothness. The experimental results showed that the path optimized by the Bessel function was smoother and more suitable for the walking of robot in practical scenarios. Fuzzy PID path tracking experiment results showed that the loading truck can stay close to the original route during both straight and turning driving, demonstrating the good effect of fuzzy PID on path tracking. Further experiments were conducted on the harmless treatment center to verify the effectiveness and practical application of the improved algorithm. Based on the path planning algorithm, the driving path of robot was quickly planned, and the fuzzy PID control algorithm was combined to accurately output the angular velocity, driving the robot to move. The transport robots quickly realized the planning of the transportation path, and during the driving process, could always be close to the established path, and the deviation error was maintained within a controllable range. [Conclusions] A path planning method for the harmless treatment of sick and dead animal using an transport robots based on an improved A * algorithm combined with a fuzzy PID motion control was proposed in this study. This method could effectively shorten the path planning time, reduce traversal nodes, and improve the efficiency and smoothness of path planning.

Cite this article

XU Jishuang , JIAO Jun , LI Miao , LI Hualong , YANG Xuanjiang , LIU Xianwang , GUO Panpan , MA Zhirun . Path Planning and Motion Control Method for Sick and Dead Animal Transport Robots Integrating Improved A * Algorithm and Fuzzy PID[J]. Smart Agriculture, 2023 , 5(4) : 127 -136 . DOI: 10.12133/j.smartag.SA202308001

0 引 言

中国是世界上较大的畜牧国,每年病死畜禽数量非常大。根据国家统计局发布的最新数据,2021年肉牛出栏量达4 707万头;生猪出栏67 128万头;全国羊出栏33 045万只;全国家禽出栏157.4亿只1。目前,每年畜禽死亡量约8亿多头(只)2。为实现病死畜禽的无害化处理与资源化利用,2022年4月22日,农业农村部发布《病死畜禽和病害畜禽产品无害化处理管理办法》,全国各地响应号召积极建设病死畜禽无害化处理中心,病死畜禽在从储藏冷库搬运至无害化处理设备上料操作过程中极易产生环境二次污染,研究病死畜禽运输机器人,实现病死畜禽的无人搬运和上料可以有效解决这一问题。
运输机器人路径规划与控制方法是运输机器人重要的研究领域,它的主要目的是协助机器人自主寻找到一条安全、有效、精确的最优路径。通过这条路径,机器人可以快速到达指定目标地点,并且在行驶过程中自主避开障碍物。实现高效的路径规划算法可以极大地提高机器人的实际工作效率,完成预定任务。目前A*算法、Dijkstra算法、遗传算法、蚁群算法、DWA(Dynamic window approach)算法以及人工势场法等已经被大量地应用于路径规划领域技术的研究中。沈斯杰等3在传统A*算法中引入梯度下降思想,通过减少冗余点提高路径平滑性;张辉等4通过选取栅格地图的关键点,在传统A*算法的基础上引入安全距离碰撞模型,利用B样条曲线提升路径平滑性;陈艺文等5提出了一种基于运动约束的A*路径规划算法,保证机器人平稳、高效、安全地移动。姜龙腾等6提出一种基于滑模变结构的路径跟踪控制算法,减少了农机车辆转弯的横向偏差;Yang等7研究了基于双闭环策略的轮式移动机器人的轨迹跟踪控制,设计具有时变增益的扩展状态观测器来提高控制的精准度和稳定性;Ryota等8提出了一种基于随机模型预测控制的纵向和横向组合控制器方法,提高了制动鲁棒性。
本研究基于改进A*算法,提出病死畜禽无害化处理运输机器人路径规划方法。首先分析传统A*算法的实现原理,以曼哈顿距离9算法为基础,改进启发函数10并加入贝塞尔曲线11函数对规划的路径进行轨迹优化,实现对A*算法模型的改进。在此基础上,对运输机器人进行运动学分析12,在规划路径后结合模糊比例积分微分(Proportional Integral Derivative,PID)算法控制运输机器人线速度与角速度实现最优化的追踪行走,以期为病死畜禽无害化处理运输机器人的路径规划提供新的技术参考。

1 改进A*算法

A*算法是在静态环境中求解最短路径的全局规划算法,是一种快速高效的启发式最佳优先算法13。但A*算法通常给出的路径规划路线需要较长时间的计算,不利于病死畜禽运输机器人的高效工作。因此,本研究利用曼哈顿距离算法并增设附加值和权值改进启发函数,引入贝塞尔曲线函数优化路径,提高运输机器人的运动效率,有效解决A*算法在转角规划时路径尖锐问题,实现运输机器人在病死畜禽无害化处理现场环境中的最优全局路径规划。

1.1 传统A*算法原理

A*算法是以Dijkstra算法和广度优先算法(Breadth-First-Search,BFS)14为基础,加入启发式的算法规则,是一种经典的启发式搜索算法15。该算法在保证最短路径的条件下实现快速计算。A*算法的核心计算方法如公式(1)所示。
f n = g n + h n
式中: f n表示全局节点的优先级,在进行下一次前进选择前考虑该节点的全局化优先情况进行路径抉择; g n表示开始出发位置到当前节点位置的行驶代价; h n表示估算值,即遍历算法的启发函数,实现对当前位置到目标节点的行驶代价估算,是A*算法的核心,可以实现引导路径不断接近目标点的功能。
A*算法是通过从起点的栅格开始进行搜索,依次完成与它相邻的 g n f n h n的计算,选取 f n值最小的栅格作为下一跳进行新的搜索起点,继续向周边扩展,直到找到终点栅格,并保存路径(图1)。
图1 传统A*算法原理图

Fig. 1 Schematic diagram of traditional A* algorithm

1.2 改进启发函数

高效的启发函数是实现A*算法路径规划的关键。启发函数主要作用是估算当前坐标点到达终点的代价,不同启发函数的设计选择对应A*算法不同的运行效率16,如表1所示。启发函数等于0时,A*算法退化为Dijkstra算法,能够寻得最优路径,但遍历节点特别多,花费时间长;当启发函数数值非常大时,A*算法时间效率提升,但无法保证规划的路径是最短、最优路径17
表1 不同启发函数比较

Table 1 Comparison of different heuristic functions

情况 函数 结果
h n)=0 A*算法演变成Dijkstra算法 保证能找到最短路径
h n)<实际代价 h n)越小,A*算法扩展节点越多 保证能找到一条最短路径且运算快
h n)=实际代价 仅寻找最佳路径而不扩展任何节点 保证能找到一条最短路径并且运算非常快
h n)>实际代价 寻找最佳路径且扩展别的节点 不能保证找到一条最短路径,但运算快
h n)>>实际代价 A*算法演变成BFS算法 不能保证找到一条最短路径,但运算非常快
为实现将病死畜禽从存储冷库无人运输至无害化处理设备并上料,本研究将A*算法启发函数 h n进行优化改进,在确保完成运输机器人最短路径规划的前提下,尽可能地减少规划过程中的路径搜索时间,以此实现快速运输、提高工作效率的目的。
研究使用曼哈顿距离替代传统A*算法的启发函数,计算当起点与终点间的距离,如公式(2)所示。
d x , y = x 1 - y 1 + x 2 - y 2
式中: x 1 , y 1表示出发点坐标; x 2 , y 2表示目标点坐标。然后对曼哈顿距离添加附加值,结合病死畜禽无害化处理现场实际位置,依据出发点指向目标点向量以及当前位置指向终点目标向量,进行向量叉积计算,实现附加值动态调整,以此改变启发式函数 h n的值,实现从初始点到目标点的连线路径。具体描述如公式(3)所示。
H = Μ c _ e - Μ s _ e
式中: H表示出发点坐标指向目标点向量和当前位置与目标点向量的向量叉积;向量下标 c表示当前节点; e表示目标终点; s表示出发点; Μ c _ e表示出发点坐标指向目标点向量; Μ s _ e表示当前位置与目标点向量。
在病死畜禽无害化处理的实际环境中,路径规划算法的重点在根据病死畜禽存储冷库起始位置点和无害化处理设备终止位置点的快速定位路径。
在正常情况下,A*算法代价函数与估计函数的比值为1∶1,在启发函数中添加函数权重 w,研究为了实现A*算法可控性,与实际情况相结合,根据不同路径改变权值18,以传统的A*算法的启发函数作为参照,记录参考值 n,在当前位置节点较开阔时,增大权重,提高搜索效率;在遇到转角等情况时,降低权重,提高路径可信度。最终可得改进A*算法核心计算函数如公式(4)所示。
f n = g n + h n H + w
式中: f n表示全局节点的优先级; g n表示开始出发位置到当前节点位置的行驶代价; H表示指向向量叉积; w表示路径权值。

1.3 路径轨迹优化

规划的路径在转角处不够平滑,在实际应用中不利于机器人转向19。这种情况需要继续对轨迹进行优化。
由于采用改进A*算法函数的运输机器人在经过转角实现转向过程中,速度和加速度等力学状态无法突变,使得运输机器人必须在经过尖锐转角时完成减速和加速,这样会增加移动时间以及能量消耗20。针对这一问题,本研究采用贝塞尔曲线算法对路径轨迹进行优化,进一步提升运输机器人行驶的平滑性以及移动效率。
贝塞尔曲线根据数据点和控制点进行优化。数据点表示为路径的出发位置和目标位置;控制点表示为路径的转弯过程中轨迹的点。按照控制点个数的不同,可以将贝塞尔曲线划分为一阶贝塞尔曲线(0个控制点);二阶贝塞尔曲线(1个控制点);三阶贝塞尔曲线(2个控制点)等。贝塞尔曲线如公式(5)所示。
B ( t ) = i = 0 n ( n i ) P i ( 1 - t ) n - i t i = ( n 0 ) P 0 ( 1 - t ) n t 0 +                          ( n 1 ) P 1 ( 1 - t ) n - 1 t 1 + . . . + ( n n - 1 ) P n - 1 ( 1 -                           t ) 1 t n - 1 + ( n n ) P n ( 1 - t ) 0 t n
式中: B ( t )表示贝塞尔函数; P i表示第i个控制点坐标信息; n表示控制的阶数; t表示将曲线范围等分后占有的比例值。

1.4 运输机器人路径规划方法

在上述研究基础上,提出的基于改进A*算法的病死畜禽无害化处理运输机器人路径规划方法的具体算法流程如图2所示。
图2 运输机器人路径规划算法流程图

Fig. 2 Flow chart of path planning algorithm for transport robot

基于改进A*算法的病死畜禽无害化处理运输机器人路径规划算法的伪代码如下:
Step 1:开始;
Step 2:建立并导入病死畜禽现场环境二值地图;
Step 3:给定冷库坐标位置作为运输机器人的运输起点,无害化处理设备坐标位置为运输机器人的运输终点,建立open列表和close列表,将冷库坐标点加入open列表中;
Step 4:判断open列表是否为空,如果为空,则表示路径规划失败,跳转到step10,路径规划结束;
Step 5:算法遍历open列表,利用改进后的 f n函数计算出函数值最小点的坐标加入close列表中;
Step 6:清空当前open列表,为下一节点遍历提供列表空间,避免列表过大影响搜索速度;
Step 7:利用启发函数遍历最新加入close节点的周围,将周围节点加入到open列表中;
Step 8:判断open列表中是否存在上料运输终点坐标,如果存在,则表示到达运输机器人的运输终点位置,将终止节点放入close列表中,依次输出close列表中的节点,根据输出节点坐标的连线获得的路径即为起始点到终止点最优路径;否则跳转至step 3循环搜索,直至搜索到运输机器人的运输终点坐标或open列表为空结束循环;
Step 9:路径规划成功后利用贝塞尔曲线函数对输出的最优路径进行平滑性处理,获得最终可实际应用的最优路径;
Step 10:结束。

2 运输机器人控制方法

2.1 运输机器人运动学分析

本研究提出的运输机器人采用履带机21为底盘基础,履带结构通过性强,重心较低且传动效率高,符合重型货物运输。在此基础上加装能够模仿人类驾驶行为的智能驾驶结构22。该驾驶结构由机器人环境定位、行驶路径规划、驾驶行为决策等系统组成。
病死畜禽运输机器人采用履带运动方式,运动特性为两轮差速驱动23。动力电机安装在左右作为驱动。左右驱动轮距与中心线间的距离分别为 W l W r(m)。这两点相对于车体坐标系中的惯性坐标系中移动的线速度为 v l v r(m/s)。运动过程中两台动力电机运行情况均为圆周运动的直线转速。利用电机驱动接口,输出的角速度为 Φ l Φ r(rad/s),以及启动电机齿轮半径 r(m),可得出线速度、角速度以及半径的关系,如公式(6)所示。
v l = r Φ l
运输机器人以左右两侧动力轮的中心点作为线段的端点做一条线段 h,该线段中点记为 C,以此点作为运输机器人的中心点 x , y。将运输机器人产生运动行为的瞬时线速度为 v c(m/s);瞬时角速度为 ω c(rad/s);姿态角即运输机器人前进方向与x轴的夹角标记为 θ。运输机器人的姿态信息可以表示为公式(7)
P = x , y , θ T
式中:P为运输机器人的姿态信息; x y表示运输机器人中心点坐标; θ表示运输机器人前进方向与x轴的夹角,(°)。
则运输机器人的瞬时线速度 v c(m/s),用公式(8)计算。
v c = v r + v l 2
计运输机器人左右两侧动力履带之间长度为 l (m);运输机器人在进行转向过程中,由于差速前进,左右轮转动距离不同,转动距离线段与过运输机器人中心点C的直线 h相交的点,即为旋转中心 O c;此时运输机器人绕 O c转动的转弯半径为中心点 C到旋转中心 O c的距离 R公式(9)表示为运输机器人在进行圆周运动时,以中心点 C为固定点,中心点 C指向车头中心位置的连线偏移旋转角度方向上的点作为基点,左右驱动履带及基点的所处方位必须与该圆周内运动问题中的角速度方位相同。
ω l = ω r = ω c
式中: ω l ω r ω c分别表示左轮、右轮以及运输机器人中心点在转向过程中的角度,(°)。
根据到转向中心半径的不同,可得公式(10)
l = v r ω r - v l ω l
则运输机器人的瞬时角速度 ω c可以表示为公式(11)
ω c = v r - v l l
公式(8)公式(11)两式,并利用 v r v l求出机器转动半径R m,如公式(12)
R = v c ω c = l 2 v r + v l v r - v l
依据运输机器人运动模型,可以通过 v r v l之间的速度差实现运输机器人3种不同的运动状态。当左右轮速不同时,运输机器人进行转弯;当左右轮速度相同时,即 v r = v l,运输机器人向前运动;当 v r = - v l时,运输机器人中心 O c进行原地旋转。

2.2 模糊PID控制方法

为实现运输机器人沿规划好的路径自主行走,研究提出基于模糊PID控制的运输机器人的行走控制算法。
PID控制24是最早发展起来的控制策略之一,具有算法简单、鲁棒性好和可靠性高等优点,被广泛使用于各种电机控制设备。模糊PID控制结合模糊控制理论与传统PID控制算法,具有响应速度更快、控制精度更高的优点。
本研究采用模糊PID算法,如公式(13)所示,根据运输机器人的实际运行状态,将模糊后的规则条件记录成模糊控制规则表,从而实现在线辨识运输机器人的特征参数,实时调整运输机器人的角速度偏差,使得其可以准确按照规划的运输路径自主按迹行走。
u ( k ) = ( k p + Δ k p ) × e ( k ) + ( k i + Δ k i ) × i = 0 k e ( k ) + ( k d + Δ k d ) × ( e ( k ) - e ( k - 1 ) )
式中: u ( k )表示在第 k个时刻PID控制器对运输机器人的信号控制; k p k i k d分别表示在控制过程中的误差比例系数、积分系数以及微分系数; Δ k p Δ k i Δ k d分别为实时角速度 ω、预期角速度 ω R的差值, e以及变化结果作为输入量得到的模糊PID控制器的输出参数。再经过PID控制器对运输机器人角速度差值 e进行运算,输出控制信号。控制器接收模糊PID控制信号,将控制信号输出的角速度换算为电机旋转信号,改变履带车两边电机转速,实现差速控制履带车转向。

3 仿真与验证

3.1 改进算法对比试验

为了验证基于改进A*算法的病死畜禽无害化处理运输机器人路径规划方法的有效性,利用构建好的环境地图对算法进行仿真实验。地图中黑色部分为静态障碍物,空白部分表示可行驶区域,如图3
图3 不同路径规划算法试验的结果

Fig. 3 Results of experiments on different path planning algorithms

为验证本研究提出的路径规划算法在病死畜禽无人上料过程中能够找到路程最短,且搜索时间最短的目标要求,在未进行轨迹优化情况下,将本研究提出的算法与传统的A*算法、未添加权值算法进行对比试验,并记录结果。图3中,蓝色像素点表示遍历的节点;红色代表输出由起始点A到终止点B的路径。
图3(a)为传统A*算法;图3(b)为曼哈顿距离算法替换传统启发函数,但未添加权值的算法;图3(c)为本研究的完整改进算法。由图3可以看出,传统A*算法需要遍历地图中的大部分节点,无权值算法中靠近终止点的遍历节点相对减少,但是在出发点附近依旧需要遍历大部分节点。本研究算法在起始点和终止点附近遍历节点最少,在路径中端也相对减少了遍历节点数。
结合图3表2的对比实验数据发现,本研究改进后的算法在不改变最优路径的前提下遍历节点最少,时间最短,相对于传统A*算法遍历节点由3 067个减少到1 968个,规划所需要时间由20.34 s减少到7.26 s,遍历节点数与规划时间都显著减少。
表2 路径规划算法实验的数据对比

Table 2 Data comparison of path planning algorithm experiments

算法类型 平均路径长度/mm 遍历节点数/个 平均运行时间/s
传统A*算法 1 406 3 067 20.34
无权值A*算法 1 395 2 843 15.36
本研究改进算法 1 360 1 968 7.26

3.2 改进算法的路径平滑试验

为了验证贝塞尔函数对路径拐点优化效果,设计了规划路径的优化试验如图4所示,红色路径为起始点A到终止点B的完整路径;蓝色路径为贝塞尔曲线优化后路径。
图4 改进A*算法轨迹优化实验

Fig. 4 Algorithm trajectory optimization experiment of the improved A* algorithm

图4的轨迹优化算法试验中可以看到,加入贝塞尔曲线函数后,路径轨迹的平滑度得到了提升,点C处的尖锐拐角更加平滑,满足运输机器人的实际应用需要;点D处的大拐角问题也得到了解决,减少了实际应用中转弯角度大的问题;在路径E处出现的连续拐点问题也得到了有效解决,避免了连续转折带来的能量消耗,提高了运输机器人的运输行驶效率。

3.3 模糊PID的路径追踪试验

为了验证模糊PID对规划后路径的追踪情况,设计了模糊PID路径追踪试验,蓝线为设计路径,红线为模糊PID算法对蓝线实现寻迹追踪。具体结果如图5所示。
图5 PID路径跟踪巡线实验

Fig. 5 PID path tracking line patrol experiment

图5的追踪巡线实验结果中可以看出,红色追踪线路在直线路径中,可以紧贴既定路线行驶,在进行大角度转向时,可以根据模糊PID控制算法,快速做出响应,实现精准转向。

3.4 病死畜禽运输机器人现场试验

为了进一步验证基于改进的A*算法的病死畜禽无害化处理运输机器人路径规划方法和基于模糊PID的运动控制方法的实际应用效果,选取安徽临泉县某无害化处理中心为试验基地,基地为水泥地面,路面平整度一般,主要以平路以及5º左右倾角的上坡路两部分组成。现场环境大小约为15 m×10 m。利用构建好的该中心的病死畜禽无害化处理现场环境地图对算法进行现场试验。
图6所示,无人上料试验车采用山东晟德机械有限公司履带式电动滑移装载机器人,采用Jetson NX作为运输机器人的控制终端搭建ROS平台,并移植本研究提出的基于改进A*算法的路径规划算法和基于模糊PID算法的运动控制算法,在无害化处理中心进行实际行走试验。
图6 病死畜禽无害化处理运输机器人

Fig6 Transport robot for harmless treatment of dead livestock and poultry

图7所示,搭建ROS(Robot Operating System)平台进行由起点A到终点B的路径规划与行走控制试验,其中C表示运输机器人在行驶过程中的位置。
图7 路径规划与运动控制方法中运输机器人行驶状态

Fig. 7 Travel status of transport robots in path planning and motion control methods

图7为试验过程中ROS平台的rviz可视化界面,可以看出在病死畜禽无害化处理中心地图中给定起点A和终点B后,根据路径规划算法快速显示运输机器人的行驶路径(图7中蓝线表示规划的运动路径)。路径长度约为18.76 m,结合模糊PID控制算法输出角速度对运输机器人的电机进行驱动,经过多次试验显示,线速度不断增大,在直线行驶过程中对追踪精度影响不大,运输机器人沿着规划好的路径匀速前进偏差可以控制在0.02 m左右,但在转角过程中随着线速度增大,在转向过程中会增加角速度导致偏移量增大。当线速度在2 m/s以内,运输机器人的偏移量为0.05 m左右;当线速度超过10 m/s时,转角最大处偏移量为0.30 m左右。采用2 m/s的速度进行试验,运行时间约为10.07 min,整体运行稳。为防止运输机器人在行驶过程中的偏移导致碰撞,在导入地图时对障碍物进行膨胀处理,即在障碍物周围设定不可达区域。当运输机器人中心点坐标与不可达区域距离小于中心点到运输机器人两边距离时,给予车辆反向角速度控制车辆转向偏离障碍物;在运输机器人以2 m/s的速度前进时,设置膨胀范围为0.05 m,运输机器人在行驶过程中未发生与墙体碰撞情况,进一步验证了本算法的有效性。

4 结 论

病死畜禽无害化处理过程中容易滋生病菌,并产生恶臭气味,不利于人工现场作业。采用运输机器人对病死畜禽进行运输作业,可以有效避免病死畜禽在从储藏冷库搬运至无害化处理设备进行上料操作过程中所带来的二次环境污染问题。
由于病死畜禽无害化处理车间是一个相对固定的静态环境,为防止无害化处理过程中病菌扩散,会经常使用消毒水对厂区进行清洗消毒,相对于传统的移动机器人(Automatic Guided Vehicle,AGV)导航控制方法,本研究提出的基于改进的A*算法的病死畜禽无害化处理运输机器人路径规划方法和基于模糊PID的运动控制方法,不需要在地面配套铺设第三方引导磁条、磁钉等设备,不影响病死畜禽的无害化处理作业。同时,由于病死畜禽无害化处理现场安装的处理设备较多(多为金属材质),对信号传输影响较大,相对于采用卫星等其他信号导航技术,本研究提出的方法可有效避免环境对信号的干扰,更符合病死畜禽无害化处理车间环境的特殊性。
实验结果表明,本研究提出的方法可有效缩短病死畜禽运输机器人的路径规划时间且减少遍历节点,提高路径规划效率和路径平滑性,结合模糊PID算法可以实现运输机器人的稳定寻迹控制,有效解决传统的A*算法在运输机器人路径规划过程中,存在的路径规划拐点大、平滑性差、算法计算时间长、遍历节点多等问题,满足病死畜禽无人上料技术需求。该研究为病死畜禽无害化处理运输机器人的路径规划与运动控制提供了新的方法参考,具有广泛的应用前景。
目前,研究的病死畜禽运输机器人路径规划与运动控制方法是基于预先已知运输机器人作业现场地图的基础上提出的。下一步将针对不同的病死畜禽无害化处理现场环境,开展未知地图环境下运输机器人的自动路径规划与运动控制技术研究,进一步拓宽运输机器人在未知环境下的应用领域,更加快捷高效地将运输机器人应用于不同的病死畜禽无害化处理现场。

利益冲突声明

本研究不存在研究者以及与公开研究成果有关的利益冲突。

1
嵇少泽, 勾长龙, 张喜庆, 等. 我国病死畜禽无害化处理简述[J]. 家畜生态学报, 2019, 40(7): 87-90.

JI S Z, GOU C L, ZHANG X Q, et al. Brief introduction to the harmless treatment of dead animals and poultry in China[J]. Journal of domestic animal ecology, 2019, 40(7): 87-90.

2
蒋晚成. 病死畜禽无害化处理及资源化利用[J]. 兽医导刊, 2020(13): 48.

JIANG W C. Harmless treatment and resource utilization of sick and dead livestock and poultry[J]. Veterinary orientation, 2020(13): 48.

3
沈斯杰, 田昕, 袁千贺. 融合改进A*和时间弹性带的移动机器人路径规划算法[J]. 智能计算机与应用, 2022, 12(11): 96-102.

SHEN S J, TIAN X, YUAN Q H. Mobile robot path planning algorithm integrating improved A* algorithm and time elastic band[J]. Intelligent computer and applications, 2022, 12(11): 96-102.

4
张辉, 张瑞亮, 许小庆, 等. 基于关键节点的改进A*无人车路径规划算法[J]. 汽车技术, 2023(3): 10-18.

ZHANG H, ZHANG R L, XU X Q, et al. Key nodes-based improved A* algorithm for path planning of unmanned vehicle[J]. Automobile technology, 2023(3): 10-18.

5
陈艺文, 江文松, 杨力, 等. 基于运动约束的移动机器人路径规划[J]. 计算机集成制造系统, 2023, 29(4): 1186-1193.

CHEN Y W, JIANG W S, YANG L, et al. Path planning based on motion constraints for mobile robot[J]. Computer integrated manufacturing systems, 2023, 29(4): 1186-1193.

6
姜龙腾, 迟瑞娟, 李青君, 等. 基于滑模变结构的农机路径跟踪控制算法研究[J]. 农机化研究, 2024, 46(2): 229-234.

JIANG L T, CHI R J, LI Q J, et al. Research on path tracking control algorithm of agricultural machinery based on sliding mode variable structure[J]. Journal of agricultural mechanization research, 2024, 46(2): 229-234.

7
YANG H J, WANG S Z, ZUO Z Q, et al. Trajectory tracking for a wheeled mobile robot with an omnidirectional wheel on uneven ground[J]. IET control theory & applications, 2020, 14(7): 921-929.

8
RYOTA N, KAZUMA S, KENICHIRO N, et al. Stochastic model predictive braking control for heavy-duty commercial vehicles during uncertain brake pressure and road profile conditions[J]. Control theory and technology, 2022, 20(2): 248-262.

9
GUTTMANN-BECK N, BRUMER H O, SHALEV Y, et al. Two dimensional maximum weight matching using Manhattan topology[J]. Operations research letters, 2022, 50(3): 281-286.

10
YIU Y F, DU J, MAHAPATRA R. Evolutionary heuristic A* search: Pathfinding algorithm with self-designed and optimized heuristic function[J]. International journal of semantic computing, 2019, 13(1): 5-23.

11
杨洋, 温兴, 马强龙, 等. 基于贝塞尔曲线的动态识别区农机避障路径实时规划[J]. 农业工程学报, 2022, 38(6): 34-43.

YANG Y, WEN X, MA Q L, et al. Real time planning of the obstacle avoidance path of agricultural machinery in dynamic recognition areas based on Bezier curve[J]. Transactions of the Chinese society of agricultural engineering, 2022, 38(6): 34-43.

12
郭凯文. 轻型农用履带底盘通过性研究[D]. 长沙: 湖南农业大学, 2020.

GUO K. Research on mobility of light-agricultural track chassis[D]. Changsha: Hunan Agricultural University, 2020.

13
KIADI M, GARCÍA E, VILLAR J R, et al. A*-based co-evolutionary approach for multi-robot path planning with collision avoidance[J]. Cybernetics and systems, 2023, 54(3): 339-354.

14
徐小强, 刘芃辉, 冒燕. 改进人工势场法和ID-BFS融合算法的无人艇路径规划研究[J]. 武汉理工大学学报, 2021, 43(5): 85-91.

XU X Q, LIU P H, MAO Y. Research on path planning of USV based on improved artificial potential field method and ID-BFS fusion algorithm[J]. Journal of Wuhan university of technology, 2021, 43(5): 85-91.

15
NAGATANI Y, SAWADA K, SHIN S. Self-triggered optimal control based on path search algorithm[J]. SICE journal of control, measurement, and system integration, 2019, 12(3): 85-93.

16
WANG H W, QI X Y, LOU S J, et al. An efficient and robust improved A* algorithm for path planning[J]. Symmetry, 2021, 13(11): ID 2213.

17
TSARDOULIAS E G, ILIAKOPOULOU A, KARGAKOS A, et al. A review of global path planning methods for occupancy grid maps regardless of obstacle density[J]. Journal of intelligent & robotic systems, 2016, 84(1/2/3/4): 829-858.

18
郭志军, 尹亚昆, 李亦轩, 等. 融合改进A*和TEB算法的移动机器人路径规划[J]. 河南科技大学学报(自然科学版), 2023, 44(4): 57-65, 7.

GUO Z J, YIN Y K, LI Y X, et al. Mobile robot path planning based on fusion of improved A* and TEB algorithms[J]. Journal of Henan university of science and technology (natural science), 2023, 44(4): 57-65, 7.

19
于蕾艳, 王显宇, 吴宝贵, 等. 无人驾驶汽车平行泊车路径规划优化与跟踪[J]. 江苏大学学报(自然科学版), 2022, 43(5): 519-523.

YU L Y, WANG X Y, WU B G, et al. Path planning optimization and tracking of parallel parking for driverless vehicle[J]. Journal of Jiangsu university (natural science edition), 2022, 43(5): 519-523.

20
JIN Y R, WEN S J, SHI Z Y, et al. Target recognition and navigation path optimization based on NAO robot[J]. Applied sciences, 2022, 12(17): ID 8466.

21
张彬, 邹渊, 张旭东, 等. 混动履带式无人平台轨迹跟踪控制研究[J]. 汽车工程, 2023, 45(4): 579-587.

ZHANG B, ZOU Y, ZHANG X D, et al. Research on trajectory tracking control of hybrid tracked unmanned platform[J]. Automotive engineering, 2023, 45(4): 579-587.

22
吴应新, 吴剑桥, 杨雨航, 等. 油电混合果园自动导航车控制器硬件在环仿真平台设计与应用[J]. 智慧农业(中英文), 2020, 2(4): 149-164.

WU Y X, WU J Q, YANG Y H, et al. Design and application of hardware-in-the-loop simulation platform for AGV controller in hybrid orchard[J]. Smart agriculture, 2020, 2(4): 149-164.

23
康翌婷, 张煜, 曾日芽. 地面不平条件下考虑滑动转向特性的履带车辆路径跟踪控制[J]. 中南大学学报(自然科学版), 2022, 53(2): 491-501.

KANG Y T, ZHANG Y, ZENG R Y. Path tracking control of tracked vehicles considering skid-steer characteristics on uneven terrain[J]. Journal of central south university (science and technology), 2022, 53(2): 491-501.

24
YU X Y, FAN Y H, XU S Y, et al. A self-adaptive SAC-PID control approach based on reinforcement learning for mobile robots[J]. International journal of robust and nonlinear control, 2022, 32(18): 9625-9643.

Outlines

/