[发明专利]基于改进快速扩展随机树的冗余机械臂避障轨迹规划方法

说明书

本发明属于自动控制的技术领域，公开了一种基于改进快速扩展随机树的冗余机械臂避障轨迹规划方法，包括步骤一、建立冗余机械臂的D‑H模型以及障碍物的简化包络模型；步骤二、根据冗余机械臂末端执行器的目标位姿，利用冗余机械臂逆运动学解析法求解满足需求的目标关节角，并设为q_goal；步骤三、将障碍物的简化包络模型向冗余机械臂的关节空间映射，进而得到冗余机械臂在关节空间内的无碰撞自由空间；步骤四、以目标关节角q_goal对应的节点为目标节点，在无碰撞自由空间内建立搜索树，根据选取规则有条件地选取随机采样节点，并引入搜索树节点筛选机制，最终在无碰撞自由空间内规划出一条无碰撞路径。

技术领域

本发明属于自动控制的技术领域，具体涉及一种基于改进快速扩展随机树的冗余机械臂避障轨迹规划方法。

背景技术

轨迹规划是机械臂轨迹控制的基础，其性能对机械臂工作效率、运动平稳性和能量消耗具有非常重要的意义。随着机械臂应用领域的发展，工作环境愈为复杂，冗余机械臂运动时可能会与环境中的障碍物发生碰撞，导致工作任务无法完成，极大限制了机械臂的应用，因此开展冗余机械臂避障轨迹规划研究是十分必要的。

冗余机械臂运动范围大，灵活度高，能够适应多种工作环境，并完成各种复杂任务。在执行工作任务时，需要为冗余机械臂规划在给定约束条件下从起始位姿运动到目标位姿的路径，但其高自由度的特点也导致了传统路径规划算法在对于冗余机械臂的应用中存在计算耗时长，收敛速度慢，避障效率低，生成路径难以在实际工作中满足要求的缺点。

目前针对机械臂避障路径规划方法，有自由空间法，人工势场法，基于采样的路径搜索算法等，如Lazona-Perze提出的一种基于C空间法的自由空间法，先根据机械臂的各个关节约束范围建立机械臂所有可行C空间，再将障碍物映射到C空间内，进而求解C空间内除去障碍物的补集空间，即机械臂不会发生碰撞的自由空间，在得到的自由空间内，利用启发式搜索算法(A*算法，蚁群算法等)搜寻机械臂的运动路径；贾庆轩,陈钢等基于A*算法在机械臂无碰撞的自由C空间内搜索避障路径；齐志刚等根据空间冗余机械臂臂型角参数化完善逆运动学算法，提出了基于臂型角逆运动学的优化A*路径规划算法等等，上述几种算法对于冗余机械臂都存在路径求解计算速率低的问题，基于采样的路径搜索算法的提出，解决了冗余机械臂自由度过高而难以搜索到关节空间解的问题，其代表算法有快速扩展随机树法(rapidly-exploring random trees,RRT)和随机路图法(probabilistic roadmapmethod,PRM)，由于RRT算法和PRM算法都是直接在机械臂关节空间搜索，并不需要构造求解可自由运动的C空间，因此其适合高维空间的规划，但其采样过于平均，搜索效率低且耗时长，并且在复杂环境下可能搜索不到解。

发明内容

本发明提供了一种基于改进快速扩展随机树的冗余机械臂避障轨迹规划方法，解决了传统路径规划算法在对于冗余机械臂的应用中存在计算耗时长，生成路径难以在实际工作中满足要求等问题。

本发明可通过以下技术方案实现：

一种基于改进快速扩展随机树的冗余机械臂避障轨迹规划方法，包括以下步骤：

步骤一、建立冗余机械臂的D-H模型以及障碍物的简化包络模型；

步骤二、根据冗余机械臂末端执行器的目标位姿，利用冗余机械臂逆运动学解析法求解满足需求的目标关节角，并设为q_goal；

步骤三、将障碍物的简化包络模型向冗余机械臂的关节空间映射，进而得到冗余机械臂在关节空间内的无碰撞自由空间；

步骤四、以目标关节角q_goal对应的节点为目标节点，在无碰撞自由空间内建立搜索树，根据选取规则有条件地选取随机采样节点，并引入搜索树节点筛选机制，最终在无碰撞自由空间内规划出一条无碰撞路径。