基于三节臂最优路径的智能挖掘机轨迹规划研究

罗映　张学强　王成涛　勾俊贺　张振

摘 要：以智能挖掘机三节臂的快速、安全运动规划为研究目的，建立了挖掘机三维几何模型，并在机器人操作系统（ROS）平台中进行运动配置，建立运动规划分析模型。模拟对比分析了快速扩展随机树算法（RRT）和双向扩展平衡的结构型双树算法（RRT-connect）的路径搜索能力，并量化了具体参数指标。进一步分别采用RRT和RRT-connect算法完成具體复杂约束环境下挖掘机臂的运动规划，结果表明，RRT-connect算法，搜索能力强，更适用于复杂环境下挖掘机臂的运动规划计算求解。

关键词：机器人操作系统 机械臂 运动规划

1 引言

挖掘机是工程机械的最主要机种，其功能典型，在各类在基础工程建设、抢险救灾等任务中发挥着不可替代的作用。目前，人工智能已进入2.0时代，人工智能技术与各细分产业融合是时代和行业发展的必然。在这一时代背景下，为应对精度、效率等方面越来越高的作业要求，适应危险、恶劣的作业环境，智能化、自主化已成为挖掘机未来主要发展趋势[1]。

依据研究技术的应用与面向功能的实现，智能挖掘机的研究可以分为三个阶段：基于传感器辅助的自动控制、基于轨迹规划的自动挖掘、面向现场工况的自主作业[2]。基于传感器辅助的自动控制相关技术研究是目前工程机械厂商进行改造升级应用的主要技术，其相关技术相对较为筒单且成熟，可以满足确定工况下筒单自动化需求，辅助操作人员完成指定任务，但其应用场景与功能不易更迭。相对于第一阶段的自动控制，基于轨迹规划的自动挖掘阶段的研究重点在于更加复杂的控制算法与工作装置的轨迹规划，一定程度上实现挖掘机器人的柔性控制，能够初步实现相对筒单的作业环节的自主控制，如自动装载、直沟挖掘等任务。面向现场工况的自主作业阶段的研究重点在于非确定环境下的自主作业，要求挖掘机具有感知复杂工况进行实时建模的能力，并据此自主规划作业流程。在施工过程中，自主完成任务分解、轨迹规划、驱动控制、自主避障等工作，并检测作业效果，最终实现开放环境中，多机协调合作完成工程任务。

在智能挖掘机械研究领域，挖掘机械臂的运动规划是一项关键技术，是保证挖掘机高效、准确自主作业的重要前提。研究中，基于对挖掘机臂工作装置驱动空间、关节空间和位姿空间的运动关系分析，在ROS环境下，建立了挖掘机三维模型，完成对机械臂模型的运动学配置，利用Moveit功能包，采用快速扩展随机树算法（RRT）和双向扩展平衡的结构型双树（RRT-connect）路径规划算法进行挖掘机臂挖掘作业的的运动规划仿真，对比分析两种运动规划算法的规划能力。

2 规划算法：RRT和RRT-connect

2.1 RRT和RRT-connect算法原理

RRT算法（Rapidly-exploring Random Tree）是基于采样的运动规划算法，是Steven M. LaValle和James J. Kuffner Jr.提出的一种通过随机构建空间填充树（Space Filling Tree）实现对非凸高维空间快速搜索的算法。RRT 算法是一种单树型的路径规划算法，其基本思想是通过随机采样和逐步扩展树的方式，快速探索配置空间（configuration space）。RRT 算法优势是搜索速度快，但生成的路径可能不是最优路径。如图1中左图所示，RRT算法将起点作为树的唯一节点，在搜索空间中以一定概率选择一个目标点作为采样点。从树中选择距离采样点最近的节点，计算该节点到采样点的路径。将计算得到的路径上的下一个点添加到树中，形成一个新的节点。同时，检测新节点与环境中的障碍物是否发生碰撞。如果发生碰撞，则舍弃该节点。如果新节点没有碰撞，将其与树中距离最近的节点进行连接。重复随机采样，直到找到一条路径连接起点和目标点。最后从树的末端节点向根节点回溯，得到从起点到目标点的最优路径[3]。

RRT-Connect（Rapidly Exploring Random Tree Connect）是RRT算法的一种变体，用于解决从起点到目标点的路径规划问题。如图1中右图所示，与传统的RRT算法不同，RRT-Connect算法通过在两个树之间建立连接来搜索路径，此算法可以检查两个树的最新节点是否可以连接。如果两个树的最新节点不发生碰撞且路径之间没有障碍物，则连接这两个节点。如果连接成功，交换两个树的角色。即以第一个树为目标树，第二个树为起始树。当两个树连接的节点之间的距离小于某个阈值时，路径被认为可行[4-5]。

2.2 RRT和RRT-connect算法模拟对比分析

建立50cm×30cm的仿真环境，设立若干个障碍，由于每一次运行结果存在差异，分别对两种算法运行60次，并计算路径规划时间、路径节点数量和节点利用率，列于表1，图2显示了其中一组计算结果对比。

根据仿真结果可以很直观的观察到，传统的RRT算法规划时间约为RRT-connect算法的4倍。同时，与RRT-connect算法相比，RRT算法采样带点数量多，存在盲目搜索的问题，最优路径曲率过大，存在这大量拐点，因此在对机械臂进行运动规划时，容易造成机械臂抖动；相比之下，RRT-connect算法在规划时间和采样点数量上有明显优势，但其规划路径平滑性稍差，并且节点的利用效率略低于RRT算法。

3 基于两种算法的挖掘机臂运动规划对比分析

基于三维建模软件Solidworks建立挖掘机的三维实体模型，并设定正确的基准轴和坐标系。完成三维实体建模后，在ROS系统中设置各个连接关节和相应的父连杆、子连杆，并定义各个关节的转动角度范围[6]，如图3所示，并建立挖掘机的三维模型，如图4所示。

为了对比分析RRT和RRT-connect算法在挖掘机臂运动规划中的计算性能和稳定性，在rviz中搭建三维复杂环境，设置多个障碍物，规划时间设置为三秒，不采用循环规划。图5为所搭建简单环境的正视图（a）和俯视图（b），绿色柱体为障碍物，图中1代表初始位姿，2代表终点位姿。

基于搭建的环境，在相同初始条件下，分别采用RRT算法及RRT-connect，对挖掘机臂进行运动规划计算求解，计算运动规划完成时间、节点总数、节点利用率、最優路径长度等参数。计算结果如图6所示，性能指标均值计算结果列于表2。

可以看出，复杂环境条件下，RRT与RRT-connect算法计算结果存在差异。为了定量分析两种算法的计算性能，避免随机误差，在简单环境下进行60次仿真实验，得到计算结果均值，列于表3。根据参数指标计算结果， RRT算法在60次实验中与障碍物发生了3次碰撞，功率降为95%。相比之下，RRT-connect 算法具有100%成功率。两种算法的最优路径平均节点数分别为148、106，平均规划时间分别为1.17s和0.90s。相比之下，RRT-connect 算法具更高的成功率、更快的计算时间和更高的可靠性、更高的节点利用率。综合各指标可知，在复杂环境下，与RRT算法相比，RRT-connect计算性能优势显著。

4 总结

利用三维建模软件，建立了挖掘机臂的三维几何模型。基于ROS平台，生成XML格式的URDF 文件，并利用moveit功能包完成对挖掘机臂的运动配置。预设挖掘机臂运动规划的六个主要位姿，在仿真环境添加障碍物模型，使挖掘机机械臂处于一定复杂的环境下。分别采用 RRT 算法和RRT-connect算法，实现对挖掘机臂的避障轨迹规划计算求解，并对规划成功率、路径平均节点数、规划时间、平均节点利用率等指标进行定量对比分析。结果说明，RRT-connect在挖掘机机械臂自动避障的安全运动规划中综合性能较为优异。

基金项目：2022年度省重点扶持区域引进急需紧缺人才项目“工程机械智能网联控制平台研究及产业化”。

参考文献：

[1]张良壮. 智能挖掘机器人轨迹规划与任务决策研究[D]. 浙江大学，2019.

[2]李运华， 范茹军，杨丽曼. 智能化挖掘机的研究现状与发展趋势[J].机械工程学报，2020，56（13）：165-178.

[3]丁健. 基于ROS 的深海机械臂运动规划研究[D]. 上海海洋大学，2022.

[4]智晋宁，贾旭峰，刘超，谢虎，史青录. 平整工况下三节臂挖掘机时间最优运动规划[J]. 中国农机化学报， 2022， 43（11）：146-154.

[5]唐家朋. 无人操纵挖掘机轨迹规划与控制[D]. 浙江大学， 2021.

[6]朱萌，孟婥，张豪，孙以泽. 基于ROS的6自由度机械臂运动轨迹规划[J]. 组合机床与自动化加工技术， 2021， 000（004）：1-3，9.