农业采摘机器人的路径规划

REF：基于 ROS 的采摘机器人底盘的设计和路径规划研究

1. 农业机器人

澳大利亚悉尼大学的果实检测及产量预估的果园巡检机器人

1be4dd9ebc9868e5150dae93a31a57f0.png

马来西亚柔佛工艺大学温室喷药机器人

c3452f04eeb8948b724dfaec70c24dba.png

智能果园巡检机器人

e31fa7ecc64a6040c3f4d11b3859988e.png

黄瓜采摘机器人

3e4cd7fada37e945d74bdc3a94c0b7d3.png

番茄采摘机器人

36c8ca5a1b8e1113f663f4bc00cd85a5.png

2. 全局路径规划

• Astar 算法：通过启发函数预估当前状态到目标的最优代价，高效缩小搜索空间并加速决策过程，从而找到从起点到终点的最优路径，若邻接节点不在开放列表和关闭列表中，则将其加入开放列表，通过启发函数计算当前点的运动搜索方向各点的代价值，然后从开放列表中选择代价值最小的点作为下一个目标，重复节点评估与选择、节点扩展、路径更新等步骤，直至到达目标点，核心步骤可分解如下：
  1. 参数初始化：预定义地图尺寸、障碍物密度和启发式函数权重等基础参数
  2. 环境建模：构建二维栅格地图，设定起点与终点的坐标位置
  3. 数据容器：建立 Open 表（待探索）、Closed 表（已处理）及关联的代价矩阵（实际成本、启发式估值等）
  4. 节点选择：在 Open 表未包含终点时，循环选综合代价f最小的节点，移入Closed表并更新相关矩阵数据
  5. 邻域扩展：以当前最优节点为父节点搜索八邻域，对无障碍邻域节点计算实际移动成本（障碍为无穷大），将符合条件的子节点及其代价值存入Open 表和相关矩阵。
  6. 终止判定：当 Open 表为空时，宣告寻路失败；若检测到终点进入 Open 表，则逆向回溯父节点生成最优路径。
• 启发函数：A-star默认为：f(n)=g(n)+h(n)，代表当前节点的总评估值
  • g(n)：从起点到当前节点 n 的实际路径成本，通过逐步累加计算
  • ℎ(n)：从当前节点n到目标节点的估计成本
  • 如果ℎ(n)=0：算法演变为 Dijkstra 算法
  • 如果g(n)=0：算法演变为 BFS 算法
  • 当ℎ(n)比g(n)小：路径搜索的栅格地图就越少，效率比较高，但是难以找到最短的路径
  • 当ℎ(n)比g(n)大：找到最短路径的概率比较高，但是存在栅格节点多，效率低的问题
  • 当ℎ(n) 等于g(n)：是最理想的状态，但在实际作业中无法达到
  • 优化启发函数：f(n)=g(n)+(1+3r2R)h(n)，其中R是起点到目标点的距离，r是当前点与目标点的距离。若当前点离目标点较远时，增大估计值的权重，减少搜索节点；若当前点离目标点较近时，此时估计值逐渐近似实际值，应减小估计值权重。
• Floyd 算法：用于求解所有点对之间的最短路径的动态规划算法，也称插点法，核心思想是逐步考虑图中每一个顶点作为“中间点”，尝试通过该中间点来缩短任意两点之间的路径。

0632e8f353cc0abeb35b496f4c5eda9e.png

• 路径搜索方式：传统A-star算法默认为 8 邻域，表示机器人可能运动到 1 到 8 中的任意一个方向，会造成一些无用的额外运算，为避免无关节点浪费时间同时进一步提高搜索效率，根据当前节点与目标节点的连线与各方向的夹角α，决定保留正向搜索方向，舍弃反向方向。

4d22d9ab18a2d62944ab8d95eae1ba25.png

3. 局部路径规划

• DWA 算法：通过对机器人的线速度和角速度进行动态采样，生成一组可行的运动轨迹，并通过多目标评价函数筛选出安全性最高、效率最优且贴近全局路径方向的轨迹，核心原理是通过构建包含机器人运动学特性的速度窗口，在可行速度组合中生成预测轨迹，并基于多维度评估体系筛选最优运动方案，包含三个关键模块
  1. 建立符合机器人运动特性的动力学模型，精确描述其位移与速度的数学关系

  

  ff672691b48b150284069c7d14ecb700.png

  1. 在速度向量空间实施动态采样策略，生成满足实时约束条件的候选速度对，约束条件包括自身速度的限制（机器人能够达到的最小、大线速度和角速度）、电机力矩的限制（最大加速度、减速度）以及预测轨迹与与最近障碍物之间的最小间隔距离限制（机器人以特定线速度 v 和角速度 w 运动，遇到障碍物需要有足够的时间做出反应）
  2. 构建综合评估函数，对预测轨迹的安全性、效率性和目标趋近性进行量化评分：

  

  其中ρ为平滑系数、α、β、γ为权重系数、heading是当前速度下轨迹末端点与目标点的方位角偏差，来评估轨迹的方向适应性，dist为轨迹与障碍物的最近距离，反应机器人运行的安全性，velocity是当前速度大小的评价函数，该值越大时说明机器人的速度越快

4. 路径规划融合

affddcb385d2ed349a74933e5c4f14c2.png

a16915a8613efbbf2c6c782596dcc8f7.png