融合改进A*算法与模糊PID的病死畜禽运输机器人路径规划与运动控制方法

2023 年 12 月 第 5 卷 第 4 期 智 慧 农 业 （ 中 英 文 ） Smart Agriculture Dec. 2023 Vol. 5, No. 4
融 合 改 进 A 算 法 与 模 糊 PID 的 病 死 畜 禽 运 输
机 器 人 路 径 规 划 与 运 动 控 制 方 法

1 ， 2 1 2 2 2 2 2 2
徐 济 双 ， 焦 俊 ， 李 淼 ， 李 华 龙 ， 杨 选 将 ， 刘 先 旺 ， 郭 盼 盼 ， 麻 之 润
（1. 安 徽 农 业 大 学 信 息 与 计 算 机 学 院 ， 安 徽 合 肥 230036 ， 中 国 ； 2. 中 国 科 学 院 合 肥 物 质 科 学 研 究 院 智 能 机 械 研 究
所 ， 安 徽 合 肥 230031 ， 中 国 ）
摘 要 ： ［ ［ 目 目 的 的 / 意 意 义 义 ］ ］ 为 了 实 现 病 死 畜 禽 无 害 化 处 理 中 心 将 病 死 畜 禽 从 存 储 冷 库 运 输 并 上 料 至 无 害 化 处 理 设 备 的
智 能 化 装 备 过 程 无 人 化 ， 对 运 输 机 器 人 的 路 径 规 划 与 自 主 行 走 的 关 键 技 术 难 题 进 行 研 究 。 ［ ［ 方 方 法 法 ］ ］ 目 前 室 内 环 境 路
径 规 划 算 法 主 要 采 用 的 是 A 算 法 ， 但 该 算 法 拐 点 大 、 平 滑 性 差 、 算 法 计 算 时 间 长 、 遍 历 节 点 多 ， 为 此 提 出 基 于 改
进 的 A 算 法 的 病 死 畜 禽 无 害 化 处 理 运 输 机 器 人 路 径 规 划 方 法 和 基 于 模 糊 比 例 积 分 微 分 （Proportional Integral Deriv?
ative ，PID ） 的 运 动 控 制 方 法 ， 利 用 曼 哈 顿 距 离 算 法 并 增 设 附 加 值 和 权 值 改 进 启 发 函 数 ， 引 入 贝 塞 尔 曲 线 函 数 优 化
路 径 ； 在 规 划 路 径 后 结 合 模 糊 PID 算 法 控 制 运 输 机 器 人 底 盘 的 线 速 度 与 角 速 度 实 现 追 踪 行 走 。 ［ ［ 结 结 果 果 和 和 讨 讨 论 论 ］ ］ 开 展
传 统 A 算 法 与 改 进 A 算 法 的 对 比 实 验 以 及 PID 追 踪 实 验 。 结 果 显 示 ， 改 进 后 的 A 算 法 节 平 均 遍 历 节 点 由 3 067 个
降 至 1 968 个 ， 算 法 平 均 时 间 由 20.34 s 减 少 到 7.26 s ， 开 展 现 场 试 验 验 证 了 该 算 法 的 有 效 性 和 可 靠 性 。 ［ ［ 结 结 论 论 ］ ］ 本
研 究 提 出 的 方 法 有 效 的 缩 短 了 病 死 畜 禽 运 输 机 器 人 的 路 径 规 划 时 间 且 减 少 了 遍 历 节 点 ， 提 高 了 路 径 规 划 效 率 和 路
径 平 滑 性 ， 结 合 模 糊 PID 算 法 可 以 实 现 运 输 机 器 人 的 稳 定 寻 迹 控 制 ， 有 效 解 决 传 统 的 A 算 法 在 运 输 机 器 人 路 径 规
划 过 程 中 存 在 的 路 径 规 划 拐 点 大 、 平 滑 性 差 、 算 法 计 算 时 间 长 、 遍 历 节 点 多 等 问 题 ， 满 足 病 死 畜 禽 无 人 上 料 技 术
需 求 。
关 键 词 ： A 算 法 ； 运 输 机 器 人 ； 贝 塞 尔 曲 线 ； 路 径 规 划 ； 模 糊 PID ； 病 死 畜 禽
中 图 分 类 号 ： TP391 文 献 标 志 码 ： A 文 章 编 号 ： SA202308001
引 用 格 式 ： 徐 济 双, 焦 俊, 李 淼, 李 华 龙, 杨 选 将, 刘 先 旺, 郭 盼 盼, 麻 之 润 . 融 合 改 进 A 算 法 与 模 糊 PID 的 病 死 畜 禽 运
输 机 器 人 路 径 规 划 与 运 动 控 制 方 法 [J]. 智 慧 农 业 ( 中 英 文), 2023, 5(4): 127-136. DOI ： 10.12133/j. smartag.
SA202308001
XU Jishuang, JIAO Jun, LI Miao, LI Hualong, YANG Xuanjiang, LIU Xianwang, GUO Panpan, MA Zhirun. Path plan ‐
ning 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 (in Chinese with English ab ‐
stract)
［1 ］ ［2 ］
只 。 目 前 ， 每 年 畜 禽 死 亡 量 约 8 亿 多 头 （ 只 ） 。
0 引 言
为 实 现 病 死 畜 禽 的 无 害 化 处 理 与 资 源 化 利 用 ，2022
中 国 是 世 界 上 较 大 的 畜 牧 国 ， 每 年 病 死 畜 禽 数
年 4 月 22 日 ， 农 业 农 村 部 发 布 《 病 死 畜 禽 和 病 害 畜
量 非 常 大 。 根 据 国 家 统 计 局 发 布 的 最 新 数 据 ，2021
禽 产 品 无 害 化 处 理 管 理 办 法 》 ， 全 国 各 地 响 应 号 召
年 肉 牛 出 栏 量 达 4 707 万 头 ； 生 猪 出 栏 67 128 万 头 ；
积 极 建 设 病 死 畜 禽 无 害 化 处 理 中 心 ， 病 死 畜 禽 在 从
全 国 羊 出 栏 33 045 万 只 ； 全 国 家 禽 出 栏 157.4 亿 储 藏 冷 库 搬 运 至 无 害 化 处 理 设 备 上 料 操 作 过 程 中 极
收 稿 日 期 ：2023-07-27
基 金 项 目 ： 国 家 重 点 研 发 计 划 （2022YFD2002104 ） ； 国 家 自 然 科 学 基 金 项 目 （31902205 ）
作 者 简 介 ： 徐 济 双 ， 硕 士 研 究 生 ， 研 究 方 向 为 机 器 人 导 航 。E-mail ：[e-mail]

通 信 作 者 ： 李 华 龙 ， 博 士 ， 副 研 究 员 ， 研 究 方 向 为 农 牧 废 弃 物 资 源 化 利 用 技 术 与 智 能 装 备 。E-mail ：[e-mail]
copyright?2023 by the authors128 智 慧 农 业 （ 中 英 文 ） Smart Agriculture Vol. 5, No. 4
易 产 生 环 境 二 次 污 染 ， 研 究 病 死 畜 禽 运 输 机 器 人 ， 规 划 算 法 ， 是 一 种 快 速 高 效 的 启 发 式 最 佳 优 先 算
［13 ］
实 现 病 死 畜 禽 的 无 人 搬 运 和 上 料 可 以 有 效 解 决 这 一
法 。 但 A 算 法 通 常 给 出 的 路 径 规 划 路 线 需 要 较
问 题 。
长 时 间 的 计 算 ， 不 利 于 病 死 畜 禽 运 输 机 器 人 的 高 效
运 输 机 器 人 路 径 规 划 与 控 制 方 法 是 运 输 机 器 人
工 作 。 因 此 ， 本 研 究 利 用 曼 哈 顿 距 离 算 法 并 增 设 附
重 要 的 研 究 领 域 ， 它 的 主 要 目 的 是 协 助 机 器 人 自 主
加 值 和 权 值 改 进 启 发 函 数 ， 引 入 贝 塞 尔 曲 线 函 数 优
寻 找 到 一 条 安 全 、 有 效 、 精 确 的 最 优 路 径 。 通 过 这
化 路 径 ， 提 高 运 输 机 器 人 的 运 动 效 率 ， 有 效 解 决
条 路 径 ， 机 器 人 可 以 快 速 到 达 指 定 目 标 地 点 ， 并 且
A 算 法 在 转 角 规 划 时 路 径 尖 锐 问 题 ， 实 现 运 输 机
在 行 驶 过 程 中 自 主 避 开 障 碍 物 。 实 现 高 效 的 路 径 规
器 人 在 病 死 畜 禽 无 害 化 处 理 现 场 环 境 中 的 最 优 全 局
划 算 法 可 以 极 大 地 提 高 机 器 人 的 实 际 工 作 效 率 ， 完
路 径 规 划 。
成 预 定 任 务 。 目 前 A 算 法 、Dijkstra 算 法 、 遗 传 算
1.1 　 传 统 A 算 法 原 理 　
法 、 蚁 群 算 法 、DWA （Dynamic window approach ）
算 法 以 及 人 工 势 场 法 等 已 经 被 大 量 地 应 用 于 路 径 规
A 算 法 是 以 Dijkstra 算 法 和 广 度 优 先 算 法
［3 ］
划 领 域 技 术 的 研 究 中 。 沈 斯 杰 等 在 传 统 A 算 法 ［14 ］
（Breadth-First-Search ，BFS ） 为 基 础 ， 加 入 启 发
［15 ］
中 引 入 梯 度 下 降 思 想 ， 通 过 减 少 冗 余 点 提 高 路 径 平
式 的 算 法 规 则 ， 是 一 种 经 典 的 启 发 式 搜 索 算 法 。
［4 ］
滑 性 ； 张 辉 等 通 过 选 取 栅 格 地 图 的 关 键 点 ， 在
该 算 法 在 保 证 最 短 路 径 的 条 件 下 实 现 快 速 计 算 。
传 统 A 算 法 的 基 础 上 引 入 安 全 距 离 碰 撞 模 型 ， 利
A 算 法 的 核 心 计 算 方 法 如 公 式 （1 ） 所 示 。
［5 ］
用 B 样 条 曲 线 提 升 路 径 平 滑 性 ； 陈 艺 文 等 提 出 了
f n = g n + h n （1 ）
( ) ( ) ( )
一 种 基 于 运 动 约 束 的 A 路 径 规 划 算 法 ， 保 证 机 器
式 中 ：f n 表 示 全 局 节 点 的 优 先 级 ， 在 进 行 下
( )
［6 ］
人 平 稳 、 高 效 、 安 全 地 移 动 。 姜 龙 腾 等 提 出 一
一 次 前 进 选 择 前 考 虑 该 节 点 的 全 局 化 优 先 情 况 进 行
种 基 于 滑 模 变 结 构 的 路 径 跟 踪 控 制 算 法 ， 减 少 了 农
路 径 抉 择 ；g n 表 示 开 始 出 发 位 置 到 当 前 节 点 位 置
( )
［7 ］
机 车 辆 转 弯 的 横 向 偏 差 ；Yang 等 研 究 了 基 于 双
的 行 驶 代 价 ；h n 表 示 估 算 值 ， 即 遍 历 算 法 的 启 发
( )
闭 环 策 略 的 轮 式 移 动 机 器 人 的 轨 迹 跟 踪 控 制 ， 设 计
函 数 ， 实 现 对 当 前 位 置 到 目 标 节 点 的 行 驶 代 价 估
具 有 时 变 增 益 的 扩 展 状 态 观 测 器 来 提 高 控 制 的 精 准
算 ， 是 A 算 法 的 核 心 ， 可 以 实 现 引 导 路 径 不 断 接
［8 ］
度 和 稳 定 性 ；Ryota 等 提 出 了 一 种 基 于 随 机 模 型
近 目 标 点 的 功 能 。
预 测 控 制 的 纵 向 和 横 向 组 合 控 制 器 方 法 ， 提 高 了 制
A 算 法 是 通 过 从 起 点 的 栅 格 开 始 进 行 搜 索 ，
动 鲁 棒 性 。
依 次 完 成 与 它 相 邻 的 g n 、f n 、h n 的 计 算 ， 选
( ) ( ) ( )
本 研 究 基 于 改 进 A 算 法 ， 提 出 病 死 畜 禽 无 害
取 f n 值 最 小 的 栅 格 作 为 下 一 跳 进 行 新 的 搜 索 起
( )
化 处 理 运 输 机 器 人 路 径 规 划 方 法 。 首 先 分 析 传 统
［9 ］
点 ， 继 续 向 周 边 扩 展 ， 直 到 找 到 终 点 栅 格 ， 并 保 存
A 算 法 的 实 现 原 理 ， 以 曼 哈 顿 距 离 算 法 为 基 础 ，
［10 ］ ［11 ］
路 径 （ 图 1 ） 。
改 进 启 发 函 数 并 加 入 贝 塞 尔 曲 线 函 数 对 规 划
的 路 径 进 行 轨 迹 优 化 ， 实 现 对 A 算 法 模 型 的 改 进 。
［12 ］
在 此 基 础 上 ， 对 运 输 机 器 人 进 行 运 动 学 分 析 ，
在 规 划 路 径 后 结 合 模 糊 比 例 积 分 微 分 （Proportional
Integral Derivative ，PID ） 算 法 控 制 运 输 机 器 人 线 速
度 与 角 速 度 实 现 最 优 化 的 追 踪 行 走 ， 以 期 为 病 死 畜
禽 无 害 化 处 理 运 输 机 器 人 的 路 径 规 划 提 供 新 的 技 术
参 考 。
1 改 进 A 算 法
图 1 　 传 统 A 算 法 原 理 图
A 算 法 是 在 静 态 环 境 中 求 解 最 短 路 径 的 全 局 Fig. 1 Schematic diagram of traditional A algorithmVol. 5, No. 4 徐 济 双 等 ： 融 合 改 进 A 算 法 与 模 糊 PID 的 病 死 畜 禽 运 输 机 器 人 路 径 规 划 与 运 动 控 制 方 法 129
式 中 ：H 表 示 出 发 点 坐 标 指 向 目 标 点 向 量 和 当
1.2 　 改 进 启 发 函 数 　
前 位 置 与 目 标 点 向 量 的 向 量 叉 积 ； 向 量 下 标 c 表 示
高 效 的 启 发 函 数 是 实 现 A 算 法 路 径 规 划 的 关
当 前 节 点 ；e 表 示 目 标 终 点 ；s 表 示 出 发 点 ； Μc_e
键 。 启 发 函 数 主 要 作 用 是 估 算 当 前 坐 标 点 到 达 终 点
表 示 出 发 点 坐 标 指 向 目 标 点 向 量 ； Μs_e 表 示 当 前
的 代 价 ， 不 同 启 发 函 数 的 设 计 选 择 对 应 A 算 法 不
位 置 与 目 标 点 向 量 。
［16 ］
同 的 运 行 效 率 ， 如 表 1 所 示 。 启 发 函 数 等 于 0
在 病 死 畜 禽 无 害 化 处 理 的 实 际 环 境 中 ， 路 径 规
时 ，A 算 法 退 化 为 Dijkstra 算 法 ， 能 够 寻 得 最 优 路
划 算 法 的 重 点 在 根 据 病 死 畜 禽 存 储 冷 库 起 始 位 置 点
径 ， 但 遍 历 节 点 特 别 多 ， 花 费 时 间 长 ； 当 启 发 函 数
和 无 害 化 处 理 设 备 终 止 位 置 点 的 快 速 定 位 路 径 。
数 值 非 常 大 时 ，A 算 法 时 间 效 率 提 升 ， 但 无 法 保
［17 ］
在 正 常 情 况 下 ，A 算 法 代 价 函 数 与 估 计 函 数
证 规 划 的 路 径 是 最 短 、 最 优 路 径 。
的 比 值 为 1 ∶1 ， 在 启 发 函 数 中 添 加 函 数 权 重 w ， 研
表 1 不 同 启 发 函 数 比 较
究 为 了 实 现 A 算 法 可 控 性 ， 与 实 际 情 况 相 结 合 ，
Table 1 Comparison of different heuristic functions
［18 ］
根 据 不 同 路 径 改 变 权 值 ， 以 传 统 的 A 算 法 的 启
情 况 函 数 结 果
发 函 数 作 为 参 照 ， 记 录 参 考 值 n ， 在 当 前 位 置 节 点
A 算 法 演 变 成 Dijkstra
h （n ） =0 保 证 能 找 到 最 短 路 径
算 法
较 开 阔 时 ， 增 大 权 重 ， 提 高 搜 索 效 率 ； 在 遇 到 转 角
h （n ） 越 小 ， A 算 法 扩 展 保 证 能 找 到 一 条 最 短 路
等 情 况 时 ， 降 低 权 重 ， 提 高 路 径 可 信 度 。 最 终 可 得
h （n ） < 实 际 代 价
节 点 越 多 径 且 运 算 快
改 进 A 算 法 核 心 计 算 函 数 如 公 式 （4 ） 所 示 。
仅 寻 找 最 佳 路 径 而 不 扩 保 证 能 找 到 一 条 最 短 路
h （n ） = 实 际 代 价
展 任 何 节 点 径 并 且 运 算 非 常 快 f n = g n + h n H + w （4 ）
( ) ( ) ( ) ( )
式 中 ：f n 表 示 全 局 节 点 的 优 先 级 ；g n 表 示
寻 找 最 佳 路 径 且 扩 展 别 不 能 保 证 找 到 一 条 最 短 ( ) ( )
h （n ） > 实 际 代 价
的 节 点 路 径 ， 但 运 算 快
开 始 出 发 位 置 到 当 前 节 点 位 置 的 行 驶 代 价 ；H 表 示
不 能 保 证 找 到 一 条 最 短
h （n ） >> 实 际 代 价 A 算 法 演 变 成 BFS 算 法
指 向 向 量 叉 积 ；w 表 示 路 径 权 值 。
路 径 ， 但 运 算 非 常 快
1.3 　 路 径 轨 迹 优 化 　
为 实 现 将 病 死 畜 禽 从 存 储 冷 库 无 人 运 输 至 无 害
化 处 理 设 备 并 上 料 ， 本 研 究 将 A 算 法 启 发 函 数
规 划 的 路 径 在 转 角 处 不 够 平 滑 ， 在 实 际 应 用 中
［19 ］
h n 进 行 优 化 改 进 ， 在 确 保 完 成 运 输 机 器 人 最 短 路
( )
不 利 于 机 器 人 转 向 。 这 种 情 况 需 要 继 续 对 轨 迹
径 规 划 的 前 提 下 ， 尽 可 能 地 减 少 规 划 过 程 中 的 路 径
进 行 优 化 。
搜 索 时 间 ， 以 此 实 现 快 速 运 输 、 提 高 工 作 效 率 的
由 于 采 用 改 进 A 算 法 函 数 的 运 输 机 器 人 在 经
目 的 。
过 转 角 实 现 转 向 过 程 中 ， 速 度 和 加 速 度 等 力 学 状 态
研 究 使 用 曼 哈 顿 距 离 替 代 传 统 A 算 法 的 启 发
无 法 突 变 ， 使 得 运 输 机 器 人 必 须 在 经 过 尖 锐 转 角 时
函 数 ， 计 算 当 起 点 与 终 点 间 的 距 离 ， 如 公 式 （2 ）
完 成 减 速 和 加 速 ， 这 样 会 增 加 移 动 时 间 以 及 能 量 消
所 示 。
［20 ］
耗 。 针 对 这 一 问 题 ， 本 研 究 采 用 贝 塞 尔 曲 线 算
d x ， y = x - y | + x - y | （2 ）
|
( ) | 1 1 2 2
法 对 路 径 轨 迹 进 行 优 化 ， 进 一 步 提 升 运 输 机 器 人 行
驶 的 平 滑 性 以 及 移 动 效 率 。
式 中 ： x ，y 表 示 出 发 点 坐 标 ； x ，y 表 示
( ) ( )
1 1 2 2
贝 塞 尔 曲 线 根 据 数 据 点 和 控 制 点 进 行 优 化 。 数
目 标 点 坐 标 。 然 后 对 曼 哈 顿 距 离 添 加 附 加 值 ， 结 合
据 点 表 示 为 路 径 的 出 发 位 置 和 目 标 位 置 ； 控 制 点 表
病 死 畜 禽 无 害 化 处 理 现 场 实 际 位 置 ， 依 据 出 发 点 指
示 为 路 径 的 转 弯 过 程 中 轨 迹 的 点 。 按 照 控 制 点 个 数
向 目 标 点 向 量 以 及 当 前 位 置 指 向 终 点 目 标 向 量 ， 进
的 不 同 ， 可 以 将 贝 塞 尔 曲 线 划 分 为 一 阶 贝 塞 尔 曲 线
行 向 量 叉 积 计 算 ， 实 现 附 加 值 动 态 调 整 ， 以 此 改 变
启 发 式 函 数 h n 的 值 ， 实 现 从 初 始 点 到 目 标 点 的 连 （0 个 控 制 点 ） ； 二 阶 贝 塞 尔 曲 线 （1 个 控 制 点 ） ； 三
( )
线 路 径 。 具 体 描 述 如 公 式 （3 ） 所 示 。
阶 贝 塞 尔 曲 线 （2 个 控 制 点 ） 等 。 贝 塞 尔 曲 线 如 公
H = Μc_e - Μs_e （3 ） 式 （5 ） 所 示 。130 智 慧 农 业 （ 中 英 文 ） Smart Agriculture Vol. 5, No. 4
n
则 表 示 路 径 规 划 失 败 ， 跳 转 到 step10 ， 路 径 规 划
n -i i n 0
B (t ) = ( n ) P (1 - t ) t =( n ) P (1 - t ) t +
∑ i 0
i 0
i = 0
结 束 ；
n - 1 1
( n ) P (1 - t ) t + ...+( n ) P (1 -
1 n -1
1 n - 1
Step 5 ： 算 法 遍 历 open 列 表 ， 利 用 改 进 后 的
1 n - 1 0 n
t ) t +( n ) P (1 - t ) t （5 ）
n
f n 函 数 计 算 出 函 数 值 最 小 点 的 坐 标 加 入 close 列
( )
n
式 中 ：B 表 示 贝 塞 尔 函 数 ；P 表 示 第 i 个 控 制
表 中 ；
(t ) i
点 坐 标 信 息 ；n 表 示 控 制 的 阶 数 ；t 表 示 将 曲 线 范 围
Step 6 ： 清 空 当 前 open 列 表 ， 为 下 一 节 点 遍 历
等 分 后 占 有 的 比 例 值 。
提 供 列 表 空 间 ， 避 免 列 表 过 大 影 响 搜 索 速 度 ；
Step 7 ： 利 用 启 发 函 数 遍 历 最 新 加 入 close 节 点
1.4 　 运 输 机 器 人 路 径 规 划 方 法 　
的 周 围 ， 将 周 围 节 点 加 入 到 open 列 表 中 ；
在 上 述 研 究 基 础 上 ， 提 出 的 基 于 改 进 A 算 法
Step 8 ： 判 断 open 列 表 中 是 否 存 在 上 料 运 输 终
的 病 死 畜 禽 无 害 化 处 理 运 输 机 器 人 路 径 规 划 方 法 的
点 坐 标 ， 如 果 存 在 ， 则 表 示 到 达 运 输 机 器 人 的 运 输
具 体 算 法 流 程 如 图 2 所 示 。
终 点 位 置 ， 将 终 止 节 点 放 入 close 列 表 中 ， 依 次 输
出 close 列 表 中 的 节 点 ， 根 据 输 出 节 点 坐 标 的 连 线
获 得 的 路 径 即 为 起 始 点 到 终 止 点 最 优 路 径 ； 否 则 跳
转 至 step 3 循 环 搜 索 ， 直 至 搜 索 到 运 输 机 器 人 的 运
输 终 点 坐 标 或 open 列 表 为 空 结 束 循 环 ；
Step 9 ： 路 径 规 划 成 功 后 利 用 贝 塞 尔 曲 线 函 数
对 输 出 的 最 优 路 径 进 行 平 滑 性 处 理 ， 获 得 最 终 可 实
际 应 用 的 最 优 路 径 ；
Step 10 ： 结 束 。
2 运 输 机 器 人 控 制 方 法
2.1 　 运 输 机 器 人 运 动 学 分 析 　
［21 ］
本 研 究 提 出 的 运 输 机 器 人 采 用 履 带 机 为 底
盘 基 础 ， 履 带 结 构 通 过 性 强 ， 重 心 较 低 且 传 动 效 率
高 ， 符 合 重 型 货 物 运 输 。 在 此 基 础 上 加 装 能 够 模 仿
［22 ］
人 类 驾 驶 行 为 的 智 能 驾 驶 结 构 。 该 驾 驶 结 构 由
图 2 　 运 输 机 器 人 路 径 规 划 算 法 流 程 图
机 器 人 环 境 定 位 、 行 驶 路 径 规 划 、 驾 驶 行 为 决 策 等
Fig. 2 Flow chart of path planning algorithm for transport robot
系 统 组 成 。
病 死 畜 禽 运 输 机 器 人 采 用 履 带 运 动 方 式 ， 运 动
基 于 改 进 A 算 法 的 病 死 畜 禽 无 害 化 处 理 运 输
［23 ］
特 性 为 两 轮 差 速 驱 动 。 动 力 电 机 安 装 在 左 右 作
机 器 人 路 径 规 划 算 法 的 伪 代 码 如 下 ：
为 驱 动 。 左 右 驱 动 轮 距 与 中 心 线 间 的 距 离 分 别 为 W
Step 1 ： 开 始 ； l
和 W （m ） 。 这 两 点 相 对 于 车 体 坐 标 系 中 的 惯 性 坐 标
Step 2 ： 建 立 并 导 入 病 死 畜 禽 现 场 环 境 二 值 r
系中移动的线速度为 v 和 v （m/s ） 。 运动过程中两台
地 图 ； l r
动 力 电 机 运 行 情 况 均 为 圆 周 运 动 的 直 线 转 速 。 利 用
Step 3 ： 给 定 冷 库 坐 标 位 置 作 为 运 输 机 器 人 的
电 机 驱 动 接 口 ， 输 出 的 角 速 度 为 Φ 、 Φ （rad/s ） ， 以
运 输 起 点 ， 无 害 化 处 理 设 备 坐 标 位 置 为 运 输 机 器 人
l r
的 运 输 终 点 ， 建 立 open 列 表 和 close 列 表 ， 将 冷 库 及 启 动 电 机 齿 轮 半 径 r （m ） ， 可 得 出 线 速 度 、 角 速
坐 标 点 加 入 open 列 表 中 ； 度 以 及 半 径 的 关 系 ， 如 公 式 （6 ） 所 示 。
Step 4 ： 判 断 open 列 表 是 否 为 空 ， 如 果 为 空 ， v = r ? Φ （6 ）
l lVol. 5, No. 4 徐 济 双 等 ： 融 合 改 进 A 算 法 与 模 糊 PID 的 病 死 畜 禽 运 输 机 器 人 路 径 规 划 与 运 动 控 制 方 法 131
运 输 机 器 人 以 左 右 两 侧 动 力 轮 的 中 心 点 作 为 线 间 的 速 度 差 实 现 运 输 机 器 人 3 种 不 同 的 运 动 状 态 。
段 的 端 点 做 一 条 线 段 h ， 该 线 段 中 点 记 为 C ， 以 此 当 左 右 轮 速 不 同 时 ， 运 输 机 器 人 进 行 转 弯 ； 当 左 右
轮 速 度 相 同 时 ， 即 v = v ， 运 输 机 器 人 向 前 运 动 ；
点 作 为 运 输 机 器 人 的 中 心 点 x ，y 。 将 运 输 机 器 人
r l
( )
当 v = -v 时 ， 运 输 机 器 人 中 心 O 进 行 原 地 旋 转 。
r l c
产 生 运 动 行 为 的 瞬 时 线 速 度 为 v （m/s ） ； 瞬 时 角 速
c
度 为 ω （rad/s ） ； 姿 态 角 即 运 输 机 器 人 前 进 方 向 与 x
c
2.2 　 模 糊 PID 控 制 方 法 　
轴 的 夹 角 标 记 为 θ 。 运 输 机 器 人 的 姿 态 信 息 可 以 表
为 实 现 运 输 机 器 人 沿 规 划 好 的 路 径 自 主 行 走 ，
示 为 公 式 （7 ） 。
T 研 究 提 出 基 于 模 糊 PID 控 制 的 运 输 机 器 人 的 行 走 控
P = [ x ， y ， θ ] （7 ）
制 算 法 。
式 中 ：P 为 运 输 机 器 人 的 姿 态 信 息 ；x ，y 表 示
［24 ］
PID 控 制 是 最 早 发 展 起 来 的 控 制 策 略 之 一 ，
运 输 机 器 人 中 心 点 坐 标 ； θ 表 示 运 输 机 器 人 前 进 方
具 有 算 法 简 单 、 鲁 棒 性 好 和 可 靠 性 高 等 优 点 ， 被 广
向 与 x 轴 的 夹 角 ， （° ） 。
泛 使 用 于 各 种 电 机 控 制 设 备 。 模 糊 PID 控 制 结 合 模
则 运 输 机 器 人 的 瞬 时 线 速 度 v （m/s ） ， 用 公
c
糊 控 制 理 论 与 传 统 PID 控 制 算 法 ， 具 有 响 应 速 度 更
式 （8 ） 计 算 。
快 、 控 制 精 度 更 高 的 优 点 。
v + v
r l
v = （8 ） 本 研 究 采 用 模 糊 PID 算 法 ， 如 公 式 （13 ） 所
c
2
示 ， 根 据 运 输 机 器 人 的 实 际 运 行 状 态 ， 将 模 糊 后 的
计 运 输 机 器 人 左 右 两 侧 动 力 履 带 之 间 长 度 为
规 则 条 件 记 录 成 模 糊 控 制 规 则 表 ， 从 而 实 现 在 线 辨
l （m ） ； 运 输 机 器 人 在 进 行 转 向 过 程 中 ， 由 于 差 速
识 运 输 机 器 人 的 特 征 参 数 ， 实 时 调 整 运 输 机 器 人 的
前 进 ， 左 右 轮 转 动 距 离 不 同 ， 转 动 距 离 线 段 与 过 运
角 速 度 偏 差 ， 使 得 其 可 以 准 确 按 照 规 划 的 运 输 路 径
输 机 器 人 中 心 点 C 的 直 线 h 相 交 的 点 ， 即 为 旋 转 中
自 主 按 迹 行 走 。
心 O ； 此 时 运 输 机 器 人 绕 O 转 动 的 转 弯 半 径 为 中
c c
k
心 点 C 到 旋 转 中 心 O 的 距 离 R 。 公 式 （9 ） 表 示 为 u ( k ) = ( k + Δk ) × e ( k ) + ( k + Δk ) × e ( k )
c p p i i ∑
i = 0
运 输 机 器 人 在 进 行 圆 周 运 动 时 ， 以 中 心 点 C 为 固 定
+( k + Δk ) × (e ( k ) - e ( k - 1) )
d d
点 ， 中 心 点 C 指 向 车 头 中 心 位 置 的 连 线 偏 移 旋 转 角
（13 ）
度 方 向 上 的 点 作 为 基 点 ， 左 右 驱 动 履 带 及 基 点 的 所
式 中 ：u ( k ) 表 示 在 第 k 个 时 刻 PID 控 制 器 对 运
处 方 位 必 须 与 该 圆 周 内 运 动 问 题 中 的 角 速 度 方 位
输 机 器 人 的 信 号 控 制 ；k 、k 、k 分 别 表 示 在 控 制
p i d
相 同 。
过 程 中 的 误 差 比 例 系 数 、 积 分 系 数 以 及 微 分 系 数 ；
ω = ω = ω （9 ）
l r c
Δk 、 Δk 、 Δk 分 别 为 实 时 角 速 度 ω 、 预 期 角 速 度
p i d
式 中 ： ω 、 ω 、 ω 分 别 表 示 左 轮 、 右 轮 以 及 运
l r c
ω 的 差 值 ，e 以 及 变 化 结 果 作 为 输 入 量 得 到 的 模 糊
R
输 机 器 人 中 心 点 在 转 向 过 程 中 的 角 度 ， （° ） 。
PID 控 制 器 的 输 出 参 数 。 再 经 过 PID 控 制 器 对 运 输
根 据 到 转 向 中 心 半 径 的 不 同 ， 可 得 公 式 （10 ） 。
机 器 人 角 速 度 差 值 e 进 行 运 算 ， 输 出 控 制 信 号 。 控
v v
r l
制 器 接 收 模 糊 PID 控 制 信 号 ， 将 控 制 信 号 输 出 的 角
l = - （10 ）
ω ω
r l
速 度 换 算 为 电 机 旋 转 信 号 ， 改 变 履 带 车 两 边 电 机 转
则 运 输 机 器 人 的 瞬 时 角 速 度 ω 可 以 表 示 为 公
c
速 ， 实 现 差 速 控 制 履 带 车 转 向 。
式 （11 ） 。
v - v
r l 3 仿 真 与 验 证
ω = （11 ）
c
l
由 公 式 （8 ） 和 公 式 （11 ） 两 式 ， 并 利 用 v 和 v
r l
3.1 　 改 进 算 法 对 比 试 验 　
求 出 机 器 转 动 半 径 R m ， 如 公 式 （12 ） 。
为 了 验 证 基 于 改 进 A 算 法 的 病 死 畜 禽 无 害 化
v l v + v
c r l
R = = （12 ）
ω 2 v - v 处 理 运 输 机 器 人 路 径 规 划 方 法 的 有 效 性 ， 利 用 构 建
c r l
依 据 运 输 机 器 人 运 动 模 型 ， 可 以 通 过 v 和 v 之 好 的 环 境 地 图 对 算 法 进 行 仿 真 实 验 。 地 图 中 黑 色 部
r l132 智 慧 农 业 （ 中 英 文 ） Smart Agriculture Vol. 5, No. 4
分 为 静 态 障 碍 物 ， 空 白 部 分 表 示 可 行 驶 区 域 ， 计 了 规 划 路 径 的 优 化 试 验 如 图 4 所 示 ， 红 色 路 径 为
如 图 3 。 起 始 点 A 到 终 止 点 B 的 完 整 路 径 ； 蓝 色 路 径 为 贝 塞
尔 曲 线 优 化 后 路 径 。
（a ） 传 统 A 算 法 （b ） 无 权 值 算 法 （c ） 改 进 A 算 法
图 3 　 不 同 路 径 规 划 算 法 试 验 的 结 果
Fig. 3 Results of experiments on different path planning
algorithms （a ） 贝 塞 尔 函 数 优 化 （b ） 优 化 路 径 C 处 拐 点
为 验 证 本 研 究 提 出 的 路 径 规 划 算 法 在 病 死 畜 禽
无 人 上 料 过 程 中 能 够 找 到 路 程 最 短 ， 且 搜 索 时 间 最
短 的 目 标 要 求 ， 在 未 进 行 轨 迹 优 化 情 况 下 ， 将 本 研
究 提 出 的 算 法 与 传 统 的 A 算 法 、 未 添 加 权 值 算 法
进 行 对 比 试 验 ， 并 记 录 结 果 。 图 3 中 ， 蓝 色 像 素 点
表 示 遍 历 的 节 点 ； 红 色 代 表 输 出 由 起 始 点 A 到 终 止
（c ） 优 化 D 处 拐 角 （d ） 优 化 E 处 反 复 拐 折
图 4 　 改 进 A 算 法 轨 迹 优 化 实 验
点 B 的 路 径 。
Fig. 4 Algorithm trajectory optimization experiment of the
图 3 （a ） 为 传 统 A 算 法 ； 图 3 （b ） 为 曼 哈 顿
improved A algorithm
距 离 算 法 替 换 传 统 启 发 函 数 ， 但 未 添 加 权 值 的 算
从 图 4 的 轨 迹 优 化 算 法 试 验 中 可 以 看 到 ， 加 入
法 ； 图 3 （c ） 为 本 研 究 的 完 整 改 进 算 法 。 由 图 3 可
贝 塞 尔 曲 线 函 数 后 ， 路 径 轨 迹 的 平 滑 度 得 到 了 提
以 看 出 ， 传 统 A 算 法 需 要 遍 历 地 图 中 的 大 部 分 节
升 ， 点 C 处 的 尖 锐 拐 角 更 加 平 滑 ， 满 足 运 输 机 器 人
点 ， 无 权 值 算 法 中 靠 近 终 止 点 的 遍 历 节 点 相 对 减
的 实 际 应 用 需 要 ； 点 D 处 的 大 拐 角 问 题 也 得 到 了 解
少 ， 但 是 在 出 发 点 附 近 依 旧 需 要 遍 历 大 部 分 节 点 。
决 ， 减 少 了 实 际 应 用 中 转 弯 角 度 大 的 问 题 ； 在 路 径
本 研 究 算 法 在 起 始 点 和 终 止 点 附 近 遍 历 节 点 最 少 ，
E 处 出 现 的 连 续 拐 点 问 题 也 得 到 了 有 效 解 决 ， 避 免
在 路 径 中 端 也 相 对 减 少 了 遍 历 节 点 数 。
了 连 续 转 折 带 来 的 能 量 消 耗 ， 提 高 了 运 输 机 器 人 的
结 合 图 3 和 表 2 的 对 比 实 验 数 据 发 现 ， 本 研 究
运 输 行 驶 效 率 。
改 进 后 的 算 法 在 不 改 变 最 优 路 径 的 前 提 下 遍 历 节 点
最 少 ， 时 间 最 短 ， 相 对 于 传 统 A 算 法 遍 历 节 点 由
3.3 　 模 糊 PID 的 路 径 追 踪 试 验 　
3 067 个 减 少 到 1 968 个 ， 规 划 所 需 要 时 间 由 20.34 s
为 了 验 证 模 糊 PID 对 规 划 后 路 径 的 追 踪 情 况 ，
减 少 到 7.26 s ， 遍 历 节 点 数 与 规 划 时 间 都 显 著
设 计 了 模 糊 PID 路 径 追 踪 试 验 ， 蓝 线 为 设 计 路 径 ，
减 少 。
红 线 为 模 糊 PID 算 法 对 蓝 线 实 现 寻 迹 追 踪 。 具 体 结
表 2 路 径 规 划 算 法 实 验 的 数 据 对 比
果 如 图 5 所 示 。
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
（a ） 路 径 跟 踪 1 （b ） 路 径 跟 踪 2
3.2 　 改 进 算 法 的 路 径 平 滑 试 验 　
图 5 　 PID 路 径 跟 踪 巡 线 实 验
为 了 验 证 贝 塞 尔 函 数 对 路 径 拐 点 优 化 效 果 ， 设 Fig. 5 PID path tracking line patrol experimentVol. 5, No. 4 徐 济 双 等 ： 融 合 改 进 A 算 法 与 模 糊 PID 的 病 死 畜 禽 运 输 机 器 人 路 径 规 划 与 运 动 控 制 方 法 133
从 图 5 的 追 踪 巡 线 实 验 结 果 中 可 以 看 出 ， 红 色
追 踪 线 路 在 直 线 路 径 中 ， 可 以 紧 贴 既 定 路 线 行 驶 ，
在 进 行 大 角 度 转 向 时 ， 可 以 根 据 模 糊 PID 控 制 算
法 ， 快 速 做 出 响 应 ， 实 现 精 准 转 向 。
3.4 　 病 死 畜 禽 运 输 机 器 人 现 场 试 验 　
（ a ） 路 径 规 划 与 运 动 状 态 1 （ b ） 路 径 规 划 与 运 动 状 态 2
为 了 进 一 步 验 证 基 于 改 进 的 A 算 法 的 病 死 畜
禽 无 害 化 处 理 运 输 机 器 人 路 径 规 划 方 法 和 基 于 模 糊
PID 的 运 动 控 制 方 法 的 实 际 应 用 效 果 ， 选 取 安 徽 临
泉 县 某 无 害 化 处 理 中 心 为 试 验 基 地 ， 基 地 为 水 泥 地
面 ， 路 面 平 整 度 一 般 ， 主 要 以 平 路 以 及 5o 左 右 倾 角
（ c ） 路 径 规 划 与 运 动 状 态 3 （ d ） 路 径 规 划 与 运 动 状 态 4
的 上 坡 路 两 部 分 组 成 。 现 场 环 境 大 小 约 为 15 m ×
图 7 　 路 径 规 划 与 运 动 控 制 方 法 中 运 输 机 器 人 行 驶 状 态
10 m 。 利 用 构 建 好 的 该 中 心 的 病 死 畜 禽 无 害 化 处 理
Fig. 7 Travel status of transport robots in path planning and mo ‐
现 场 环 境 地 图 对 算 法 进 行 现 场 试 验 。
tion control methods
如 图 6 所 示 ， 无 人 上 料 试 验 车 采 用 山 东 晟 德 机
械 有 限 公 司 履 带 式 电 动 滑 移 装 载 机 器 人 ， 采 用 Jet ‐
过 程 中 对 追 踪 精 度 影 响 不 大 ， 运 输 机 器 人 沿 着 规 划
son NX 作 为 运 输 机 器 人 的 控 制 终 端 搭 建 ROS 平 台 ，
好 的 路 径 匀 速 前 进 偏 差 可 以 控 制 在 0.02 m 左 右 ， 但
并 移 植 本 研 究 提 出 的 基 于 改 进 A 算 法 的 路 径 规 划
在 转 角 过 程 中 随 着 线 速 度 增 大 ， 在 转 向 过 程 中 会 增
算 法 和 基 于 模 糊 PID 算 法 的 运 动 控 制 算 法 ， 在 无 害
加 角 速 度 导 致 偏 移 量 增 大 。 当 线 速 度 在 2 m/s 以 内 ，
化 处 理 中 心 进 行 实 际 行 走 试 验 。
运 输 机 器 人 的 偏 移 量 为 0.05 m 左 右 ； 当 线 速 度 超 过
10 m/s 时 ， 转 角 最 大 处 偏 移 量 为 0.30 m 左 右 。 采 用
2 m/s 的 速 度 进 行 试 验 ， 运 行 时 间 约 为 10.07 min ，
整 体 运 行 稳 。 为 防 止 运 输 机 器 人 在 行 驶 过 程 中 的 偏
移 导 致 碰 撞 ， 在 导 入 地 图 时 对 障 碍 物 进 行 膨 胀 处
理 ， 即 在 障 碍 物 周 围 设 定 不 可 达 区 域 。 当 运 输 机 器
人 中 心 点 坐 标 与 不 可 达 区 域 距 离 小 于 中 心 点 到 运 输
机 器 人 两 边 距 离 时 ， 给 予 车 辆 反 向 角 速 度 控 制 车 辆
图 6 　 病 死 畜 禽 无 害 化 处 理 运 输 机 器 人
转 向 偏 离 障 碍 物 ； 在 运 输 机 器 人 以 2 m/s 的 速 度 前
Fig6 Transport robot for harmless treatment of dead livestock
进 时 ， 设 置 膨 胀 范 围 为 0.05 m ， 运 输 机 器 人 在 行 驶
and poultry
过 程 中 未 发 生 与 墙 体 碰 撞 情 况 ， 进 一 步 验 证 了 本 算
如 图 7 所 示 ， 搭 建 ROS （Robot Operating Sys ‐
法 的 有 效 性 。
tem ） 平 台 进 行 由 起 点 A 到 终 点 B 的 路 径 规 划 与 行
走 控 制 试 验 ， 其 中 C 表 示 运 输 机 器 人 在 行 驶 过 程 中
4 结 论
的 位 置 。
病 死 畜 禽 无 害 化 处 理 过 程 中 容 易 滋 生 病 菌 ， 并
图 7 为 试 验 过 程 中 ROS 平 台 的 rviz 可 视 化 界
产 生 恶 臭 气 味 ， 不 利 于 人 工 现 场 作 业 。 采 用 运 输 机
面 ， 可 以 看 出 在 病 死 畜 禽 无 害 化 处 理 中 心 地 图 中 给
器 人 对 病 死 畜 禽 进 行 运 输 作 业 ， 可 以 有 效 避 免 病 死
定 起 点 A 和 终 点 B 后 ， 根 据 路 径 规 划 算 法 快 速 显 示
畜 禽 在 从 储 藏 冷 库 搬 运 至 无 害 化 处 理 设 备 进 行 上 料
运 输 机 器 人 的 行 驶 路 径 （ 图 7 中 蓝 线 表 示 规 划 的 运
操 作 过 程 中 所 带 来 的 二 次 环 境 污 染 问 题 。
动 路 径 ） 。 路 径 长 度 约 为 18.76 m ， 结 合 模 糊 PID 控
制 算 法 输 出 角 速 度 对 运 输 机 器 人 的 电 机 进 行 驱 动 ， 由 于 病 死 畜 禽 无 害 化 处 理 车 间 是 一 个 相 对 固 定
经 过 多 次 试 验 显 示 ， 线 速 度 不 断 增 大 ， 在 直 线 行 驶 的 静 态 环 境 ， 为 防 止 无 害 化 处 理 过 程 中 病 菌 扩 散 ，134 智 慧 农 业 （ 中 英 文 ） Smart Agriculture Vol. 5, No. 4
JIANG W C. Harmless treatment and resource utilization
会 经 常 使 用 消 毒 水 对 厂 区 进 行 清 洗 消 毒 ， 相 对 于 传
of sick and dead livestock and poultry[J]. Veterinary orien ‐
统 的 移 动 机 器 人 （Automatic Guided Vehicle ，
tation, 2020(13): 48.

AGV ） 导 航 控 制 方 法 ， 本 研 究 提 出 的 基 于 改 进 的
[ 3 ] 沈 斯 杰, 田 昕, 袁 千 贺 . 融 合 改 进 A 和 时 间 弹 性 带 的 移
动 机 器 人 路 径 规 划 算 法[J]. 智 能 计 算 机 与 应 用, 2022, 12
A 算 法 的 病 死 畜 禽 无 害 化 处 理 运 输 机 器 人 路 径 规
(11): 96-102.
划 方 法 和 基 于 模 糊 PID 的 运 动 控 制 方 法 ， 不 需 要 在
SHEN S J, TIAN X, YUAN Q H. Mobile robot path plan ‐

ning algorithm integrating improved A algorithm and
地 面 配 套 铺 设 第 三 方 引 导 磁 条 、 磁 钉 等 设 备 ， 不 影
time elastic band[J]. Intelligent computer and applica ‐
响 病 死 畜 禽 的 无 害 化 处 理 作 业 。 同 时 ， 由 于 病 死 畜
tions, 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 un ‐
环 境 对 信 号 的 干 扰 ， 更 符 合 病 死 畜 禽 无 害 化 处 理 车 manned 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]. Comput ‐
PID 算 法 可 以 实 现 运 输 机 器 人 的 稳 定 寻 迹 控 制 ， 有 er integrated manufacturing systems, 2023, 29(4): 1186-
1193.
效 解 决 传 统 的 A 算 法 在 运 输 机 器 人 路 径 规 划 过 程
[ 6 ] 姜 龙 腾, 迟 瑞 娟, 李 青 君, 等 . 基 于 滑 模 变 结 构 的 农 机 路
中 ， 存 在 的 路 径 规 划 拐 点 大 、 平 滑 性 差 、 算 法 计 算
径 跟 踪 控 制 算 法 研 究 [J]. 农 机 化 研 究 , 2024, 46(2):
229-234.
时 间 长 、 遍 历 节 点 多 等 问 题 ， 满 足 病 死 畜 禽 无 人 上
JIANG L T, CHI R J, LI Q J, et al. Research on path track ‐
料 技 术 需 求 。 该 研 究 为 病 死 畜 禽 无 害 化 处 理 运 输 机
ing 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 track ‐
目 前 ， 研 究 的 病 死 畜 禽 运 输 机 器 人 路 径 规 划 与
ing for a wheeled mobile robot with an omnidirectional
wheel on uneven ground[J]. IET control theory & applica ‐
运 动 控 制 方 法 是 基 于 预 先 已 知 运 输 机 器 人 作 业 现 场
tions, 2020, 14(7): 921-929.
地 图 的 基 础 上 提 出 的 。 下 一 步 将 针 对 不 同 的 病 死 畜
[ 8 ] RYOTA N, KAZUMA S, KENICHIRO N, et al. Stochas ‐
禽 无 害 化 处 理 现 场 环 境 ， 开 展 未 知 地 图 环 境 下 运 输
tic model predictive braking control for heavy-duty com ‐
mercial 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 se ‐
mantic computing, 2019, 13(1): 5-23.
参 参 考 考 文 文 献 献 ：
[11] 杨 洋, 温 兴, 马 强 龙, 等 . 基 于 贝 塞 尔 曲 线 的 动 态 识 别 区
农 机 避 障 路 径 实 时 规 划[J]. 农 业 工 程 学 报, 2022, 38(6):
[ 1 ] 嵇 少 泽, 勾 长 龙, 张 喜 庆, 等 . 我 国 病 死 畜 禽 无 害 化 处 理
34-43.
简 述[J]. 家 畜 生 态 学 报, 2019, 40(7): 87-90.
JI S Z, GOU C L, ZHANG X Q, et al. Brief introduction YANG Y, WEN X, MA Q L, et al. Real time planning of
to the harmless treatment of dead animals and poultry in the obstacle avoidance path of agricultural machinery in
China[J]. Journal of domestic animal ecology, 2019, 40 dynamic recognition areas based on Bezier curve[J].
(7): 87-90. Transactions of the Chinese society of agricultural engi ‐
[ 2 ] 蒋 晚 成 . 病 死 畜 禽 无 害 化 处 理 及 资 源 化 利 用[J]. 兽 医 导 neering, 2022, 38(6): 34-43.
刊, 2020(13): 48. [12] 郭 凯 文 . 轻 型 农 用 履 带 底 盘 通 过 性 研 究[D]. 长 沙: 湖 南Vol. 5, No. 4 徐 济 双 等 ： 融 合 改 进 A 算 法 与 模 糊 PID 的 病 死 畜 禽 运 输 机 器 人 路 径 规 划 与 运 动 控 制 方 法 135
农 业 大 学, 2020. [19] 于 蕾 艳, 王 显 宇, 吴 宝 贵, 等 . 无 人 驾 驶 汽 车 平 行 泊 车 路
GUO K. Research on mobility of light-agricultural track 径 规 划 优 化 与 跟 踪 [J]. 江 苏 大 学 学 报 ( 自 然 科 学 版),
chassis[D]. Changsha: Hunan Agricultural University, 2022, 43(5): 519-523.
2020. YU L Y, WANG X Y, WU B G, et al. Path planning opti ‐
[13] KIADI M, GARCíA E, VILLAR J R, et al. A-based co- mization and tracking of parallel parking for driverless ve ‐
evolutionary approach for multi-robot path planning with hicle[J]. Journal of Jiangsu university (natural science edi ‐
collision avoidance[J]. Cybernetics and systems, 2023, 54 tion), 2022, 43(5): 519-523.
(3): 339-354. [20] JIN Y R, WEN S J, SHI Z Y, et al. Target recognition and
[14] 徐 小 强, 刘 芃 辉, 冒 燕 . 改 进 人 工 势 场 法 和 ID-BFS 融 合 navigation path optimization based on NAO robot[J]. Ap ‐
算 法 的 无 人 艇 路 径 规 划 研 究[J]. 武 汉 理 工 大 学 学 报, plied sciences, 2022, 12(17): ID 8466.
2021, 43(5): 85-91. [21] 张 彬, 邹 渊, 张 旭 东, 等 . 混 动 履 带 式 无 人 平 台 轨 迹 跟 踪
XU X Q, LIU P H, MAO Y. Research on path planning of 控 制 研 究[J]. 汽 车 工 程, 2023, 45(4): 579-587.
USV based on improved artificial potential field method ZHANG B, ZOU Y, ZHANG X D, et al. Research on tra ‐
and ID-BFS fusion algorithm[J]. Journal of Wuhan univer ‐ jectory tracking control of hybrid tracked unmanned plat ‐
sity of technology, 2021, 43(5): 85-91. form[J]. Automotive engineering, 2023, 45(4): 579-587.
[15] NAGATANI Y, SAWADA K, SHIN S. Self-triggered opti ‐ [22] 吴 应 新, 吴 剑 桥, 杨 雨 航, 等 . 油 电 混 合 果 园 自 动 导 航 车
mal control based on path search algorithm[J]. SICE jour ‐ 控 制 器 硬 件 在 环 仿 真 平 台 设 计 与 应 用[J]. 智 慧 农 业( 中
nal of control, measurement, and system integration, 英 文), 2020, 2(4): 149-164.
2019, 12(3): 85-93. WU Y X, WU J Q, YANG Y H, et al. Design and applica ‐
[16] WANG H W, QI X Y, LOU S J, et al. An efficient and ro ‐ tion of hardware-in-the-loop simulation platform for AGV
bust improved A algorithm for path planning[J]. Symme ‐ controller in hybrid orchard[J]. Smart agriculture, 2020, 2
try, 2021, 13(11): ID 2213. (4): 149-164.
[17] TSARDOULIAS E G, ILIAKOPOULOU A, KARGA ‐ [23] 康 翌 婷, 张 煜, 曾 日 芽 . 地 面 不 平 条 件 下 考 虑 滑 动 转 向 特
KOS A, et al. A review of global path planning methods 性 的 履 带 车 辆 路 径 跟 踪 控 制[J]. 中 南 大 学 学 报( 自 然 科
for occupancy grid maps regardless of obstacle density[J]. 学 版), 2022, 53(2): 491-501.
Journal of intelligent & robotic systems, 2016, 84(1/2/3/ KANG Y T, ZHANG Y, ZENG R Y. Path tracking control
4): 829-858. of tracked vehicles considering skid-steer characteristics

[18] 郭 志 军, 尹 亚 昆, 李 亦 轩, 等 . 融 合 改 进 A 和 TEB 算 法 的 on uneven terrain[J]. Journal of central south university
移 动 机 器 人 路 径 规 划[J]. 河 南 科 技 大 学 学 报( 自 然 科 学 (science and technology), 2022, 53(2): 491-501.
版), 2023, 44(4): 57-65, 7. [24] YU X Y, FAN Y H, XU S Y, et al. A self-adaptive SAC-
GUO Z J, YIN Y K, LI Y X, et al. Mobile robot path plan ‐ PID control approach based on reinforcement learning for

ning based on fusion of improved A and TEB algo ‐ mobile robots[J]. International journal of robust and non ‐
rithms[J]. Journal of Henan university of science and tech ‐ linear control, 2022, 32(18): 9625-9643.
nology (natural science), 2023, 44(4): 57-65, 7.
Path Planning and Motion Control Method for Sick and
Dead Animal Transport Robots Integrating Improved A
Algorithm and Fuzzy PID
1,2 1 2 2 2
XU Jishuang , JIAO Jun , LI Miao , LI Hualong , YANG Xuanjiang ,
2 2 2
LIU Xianwang , GUO Panpan , MA Zhirun
（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 ）
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 prob ‐
lem and achieve the intelligent equipment process of transporting sick and dead animal from storage cold storage to harmless treat ‐
ment 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.136 智 慧 农 业 （ 中 英 文 ） Smart Agriculture Vol. 5, No. 4
[Methods] 　 A algorithm is mainly adopted for the robot path planning algorithm for indoor environments, but traditional A algo ‐
rithms 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 ro ‐
bots based on the improved A algorithm was constructed, as well as a motion control method based on fuzzy proportional integral dif ‐
ferential (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 lo ‐
cation 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 effi ‐
ciency 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 loca ‐
tion node was relatively open, the search efficiency was improved by increasing the weight. When encountering situations such as cor ‐
ners, the weight was reduced to improve the credibility of the path. By improving the heuristic function, a driving path from the start ‐
ing 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 differ ‐
ent inflection points and control points of the path, different orders of Bessel functions were introduced to smooth the planning pro ‐
cess 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 ef ‐
fectiveness of the path planning method for the harmless treatment of sick and dead animal using the improved A algorithm. The com ‐
parative 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 de ‐
creased 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 in ‐
flection 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 im ‐
proved algorithm. Based on the path planning algorithm, the driving path of robot was quickly planned, and the fuzzy PID control al ‐
gorithm 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 er ‐
ror 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 im ‐
proved 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.
Key words: A algorithm; transport robots; Bezier curve; path planning; fuzzy PID; sick and dead animal
Foundation items: National Key Research and Development Program of China (2022YFD2002104); National Natural Science Founda ‐
tion of China (31902205)
Biography: XU Jishuang, E-mail: [e-mail]
Corresponding author: LI Hualong, E-mail: [e-mail]
（ 登 陆 www.smartag.net.cn 免 费 获 取 电 子 版 全 文 ）