非完整约束移动机器人CRAB-RRT轨迹规划_宋云云.pdf

书书书第 卷第期 年月 收稿日期：基金项目：贵州省科学技术基金项目（黔科合基础 ）作者简介：宋云云（），女，贵州纳雍人，硕士研究生，研究方向为机器人路径规划与跟踪控制；杨盛毅（），男，贵州平塘人，博士，教授，研究方向为飞行器动力学与控制；朱力（），女，贵州织金人，硕士研究生，研究方向为机器人路径规划。非完整约束移动机器人 轨迹规划宋云云，杨盛毅，朱力，（贵州民族大学数据科学与信息工程学院，贵州 贵阳 ；贵州民族大学模式识别与智能系统重点实验室，贵州 贵阳 ；贵州民族大学机械电子工程学院，贵州 贵阳 ）摘要：针对快速扩展树算法在多障碍环境搜索效率低、冗余节点多的问题，提出一种 算法。该算法采用圆域概率偏向采样策略进行随机采样；将扩展树上节点分为类，通过度量邻近点间对角线距离、历史路径成本和扩展角度的加权和，在有效邻近点集中选出高质量最近点；在新节点扩展中添加了目标引力分量，并根据环境障碍信息实现扩展步长自适应变化；经过对非完整约束移动机器人运动学模型的分析，提出了极限角约束，通过考虑受极限角约束的路径裁剪策略剔除冗余节点，利用三次 样条平滑方法平滑轨迹。通过数值仿真分析和实车实验，验证了 算法的可行性、实用性与优越性，平滑后的路径更利于机器人跟踪，且适用于多障碍环境。关键词：非完整约束；目标偏向性；自适应步长；样条平滑中图分类号：文献标志码：文章编号：（），（，；，；，）：，：；（）引言随着自主移动机器人在现代物流运输、餐饮服务和智慧工业等领域应用日渐增加，路径规划成为实现自主机器人的关键技术之一。常用的机器人路径规划方法有算法、人工势场法、粒子群算法和快速扩展随机树算法等。（）算法因具有概率完备性、结构简单、不需要对环境进行建模、易与其他算法融合、搜索速度快和搜索能力强等优势，得到了广泛应用。传统 算法搜索具有盲目性，缺乏目标偏向策略，容易导致算法收敛速度缓慢。文献文献 引入概率目标偏向策略，引导节点朝着目标点采样，减少了无效节点；文献文献 分析障碍物与目标物对机器人产生的引、斥力效果，引入人工势场引力思想改进新节点的生成方式，以此增大节点向目标点偏向扩展的能力。某些研究者提出了多种有界连通域，在一定程度上促进偏向目标点进行采样，减少算法在无用区域的采样次数 。现有改进算法中使用了加权欧氏距离 和添加角度约束的距离 等度量函数，可减弱欧氏距离带来的消极影响。在扩展节点过程中，扩展步长不具有自适应性，需要人为调整参数，在不同环境下无法变换阈值，缺乏对环境的适应性，适用于简单环境下的路径规划，文献 文献 使用动态步长实现自适应扩展。部分 改进算法未考虑机器人运动学约束，得到的路径冗余节点多，路径点转角过大，路径曲折，不适用于非完整约束移动机器人的实际运行过程。为解决快速扩展随机树算法在多障碍环境中搜索效率低、冗余节点多的问题，本文提出了一种 （）算法。该算法通过障碍物安全距离约束策略、圆域概率偏向采样策略、优化的最近点选取策略和自适应目标偏置扩展策略，搜索出一条起始点到目标点的可行路径。通过分析非完整约束移动机器人运动学模型，提出了受极限角约束的路径裁剪策略，剔除可行路径中的冗余节点；对规划后的可行路径利用三次 样条平滑方法，在结构化障碍环境中优化出一条满足非完整约束移动机器人运动学约束的移动轨迹，可减少采样点数目，简化路径，同时减少搜索范围，提高算法计算效率。非完整约束移动机器人约束非完整约束移动机器人的特点在于移动机器人只做纯滚动的运动，不能进行滑动，常见的例子有轮式移动机器人。因此，本文主要分析以四轮移动机器人为例的自主移动路径规划问题，其简化模型如图所示。图四轮移动机器人简化模型四轮移动机器人在运动时，转弯半径、转向角、转折曲率、四轮移动机器人的轴距之间的关系为 （）因为轮式移动机器人只能通过前轮转向，所以根据阿克曼转向条件，存在一个最大转向角，本文称其为极限角 。由式（）可分析出，任意时刻的转向角应满足式（）的极限角约束，即 （）算法 障碍物安全距离约束策略为防止机器人在跟踪路径过程时车身与障碍物发生“擦边”事故，本文对机器人作业环境中的障碍物进行膨胀化处理，实现障碍物安全距离约束。即将每个障碍物膨胀单位。考虑到实际情况，应该满足如式（）的约束关系，即（）为移动机器人机身宽度。圆域概率偏向采样策略本文提出了圆域概率偏向采样策略，以期减少无效采样节点数。该策略主要是以地图中心（，）为圆心、短边长为直径构建地图内切圆，该圆域即为圆形采样区域。同时，在该圆域下，结合概率偏向采样方法确定 （，），采样点 生成公式为 其他（）宋云云等：非完整约束移动机器人 轨迹规划研究与设计 为 随 机 概 率；为 设 定 的 概 率 阈 值；（，）为路径的目标点；（，）为圆域采样点，求解公式为 （）（）（）最近点选取策略 算法及部分改进算法中，对最近点的选取是通过比较邻近点集中的各邻近点与随机采样点之间的欧氏距离，选择距离最小的邻近点为最近点。这种选取方法没有考虑到邻近点的历史路径成本和扩展角度，导致最终搜索出的路径代价相对较大，路径点转折角度不满足机器人极限角约束。为 了 提 高 路 径 质 量，本 文 综 合 考 虑 了 邻 近 点 （，）和随机采样点 （，）的距离，邻近点 到起点 （，）的历史路径成本，随机采样点 、与邻近点的父节点（，）之间连线的角度等个方面，根据式（）得到各邻近点的度量值（，）。选择最小度量值对应的节点为最近点 。（，）（）（）（）（）（，）（）（，）（）（，）（）（）为邻近节点 和随机采样点 之间 归 一 化 后 的 对 角 线 距 离 度 量 函 数（，），如式（）；（）为邻近节点 与起始点 归一化后的历史路径成本（，），如式（）；（，）为随机采样点 、邻近节点 、邻近节点父节点 之间归一化后的夹角关系；、是个分量上的权重系数，并且满足。（，）（，）.（，）（）（，）（，）.（，）（）自适应目标偏置扩展策略 新节点的生成为了提高本文算法的搜索时间，借鉴人工势场引力思想，提出了一种自适应目标偏置扩展策略，以引导随机树向目标点扩展。传统的新节点 生成公式为 （，）（）（，）为采样点 与最近点 的欧氏距离；为扩展步长。自适应目标偏置扩展的新节点受最近点和目标点的合力影响，新节点生成公式为 （）（，）（，）（）（，）为采样点 与最近点 的欧氏距离；（，）为目标点 与最近点 的欧氏距离；为扩展步长；为目标引力系数。自适应步长通过对传统的 算法扩展固定步长，使得路径穿越障碍的可能性增大，但是极易导致延伸失败，同时也可能会远离目标点扩展，导致随机树进行无效扩展。因此，根据式（）实现对步长的自适应调整，扩展步长与周边障碍物数量相关。（）为扩展步长阈值；为最近点与随机采样点连线方向上的障碍物数量参数；为设定的障碍物数量阈值；为采样点 与目标点 的距离。路径裁剪与平滑 考虑受极限角约束的裁剪策略非完整约束移动机器人在跟踪路径时，由于路径转折角度过大、转向不连续，导致机器人在移动过程中消耗大量的能量，不满足非完整移动机器人的运动学模型。结合上述问题，说明规划出的路径点中仍存在部分冗余节点，为提高路径质量，减少路径转折次数，本文根据相邻且非共线的个节点之间存在的三角不等关系和夹角关系设计路径剪枝策略。沿着规划路径方向，将各路径点（不包括目标点）放入待剪裁点集。从起点 开始，根据三角不等关系筛选规 则 和 极 限 角 筛 选 规 则，遍 历 待 剪 裁 点 集（，），其中 ，为路径节点总个数，筛选出规划路径中的有效节点。（）三角不等关系筛选规则：根据三角不等式关系，非共线的个点、之间相互连线的线段长度存在关系为 （）当且仅当、共线时满足等式关系时，。若节点 与节点 的连线没有与障碍物发生碰撞，且满足极限角筛选条件，则剔除这个节点之间的所有节点，否则保留这个节点之间的所有节点，将其添加至新路径点集中，并从 的父节点开始重复上述过程，直到将节点 添加至新路径点集，为剪枝后的路径点个数，且。极限角筛选规则：由机器人运动学分析可知，轮式移动机器人航向角存在一个最大值，本文称其为极限角 ，且 。当路径点之间的夹角大于极限角时，机器人需要经过多次转动才能正确朝着下一个路径点前进，此过程会消耗大量的能量。因此，本文在删除冗余节点的同时还考虑了相邻节点的角度关系，提出了极限角筛选规则。若节点与其父节点 连线 同构成的夹角 满足式（）的不等关系，则满足筛选图 算法流程条件，可删除节点；否则保留节点。（）三次样条曲线平滑为了便于移动机器人的路径跟踪，减少运动过程中由于路径曲率较大带来的抖动或急转弯问题，采用三次 样条曲线方程对路径进行优化，即（），（），（），（），（）（）（）为平滑后路径点；、为控 制 曲 线 的 特 征 点；为 时 间 参数，；，（）、，（）、，（）、，（）为各控制点的三阶样条基函数，具体计算式为，（）（），（）（），（）（），（）（）在使用三次样条时，若需要曲线的个端点与起始点、目标点重合，则需要构造个节点、，加入需要平滑的路径点集中。节点、生成方式为（）算法 算法流程如图所示。为起始位置，为目标位置，为概率阈值，为自适应扩展步长，为障碍物位置信息，为最大迭代次数，为 生长的随机树，为优化路径。从起点开始进行路径搜索，首先将障碍物膨胀化 （）；然后随机生成概率，当小于 时，随机采样点 落于 处，否则，在 障 碍 环 境 中 通 过 圆 域 概 率 偏 向 采 样 （）得到 ；通过最近点选取函数 （）选出优质最近点 ；通过自适应 偏 向 扩 展 函 数 （）得 到 新 节 点 ，并直线连接 与 形成扩展分支；通过 （）检测 （，）是否与障碍发生碰撞，若未碰撞则添加 和至随机树上，完成次成功的随机树扩展搜索；直到新节点距离目标位置距离 （，）小于误差阈值 时，路径搜索成功并返回从 到 的随机树；最后通过反向搜索、受极限角约束的宋云云等：非完整约束移动机器人 轨迹规划研究与设计路径裁剪、样条曲线平滑等处理后，得到一条优化后的路径 。实验结果与分析为了验证本文提出的 算法可行性，在 及 和 的环境下开展仿真验证；在 支持下，使用轮式移动机器人完成实车实验。数值仿真结果与分析简单稀疏障碍环境尺寸为 ，设有个圆柱状障碍和个矩形障碍；路径起始点坐标和目标点坐标分别为（，）、（，），坐标单位为；算法最大迭代次数为 ，概率阈值为，目标区域误差阈值为。本节仿真图中图例说明如表所示。在同一环境下，分别运行 算法和本文算法 ，实际运行结果如图所示。表图例说明图例样式图例名称起始点目标点随机采样点矩形障碍物圆柱状障碍物随机树分支最终搜索路径搜索路径节点图简单稀疏障碍环境下 仿真结果由图的结果可分析，本文算法 在稀疏环境中的路径搜索效果比 算法好，采样点数减少，搜索效率提高；随机树的分支明显减少，节点利用率高；迭代次数与路径转折次数减少，路径搜索效果更好。为了进一步验证算法的可行性，在多障碍环境中进行仿真实验。环境中设有个圆柱状障碍和个方形障碍；算法最大迭代次数为 ，其余参数保持不变。在同一环境下，分别运行 算法、算 法、和 本 文 算法 次，结果如表所示。各算法随机的实际运行结果如图所示。表多障碍环境下路径规划实验数据算法名称平均搜索时长平均路径长度 采样点数 图多障碍环境下 仿真结果由实验数据和仿真结果可看出，算法在多障碍环境中的路径搜索效果优于 、。其中，随机采样点数（迭代次数）减少 ，节点利用率高；平均搜索时间缩短 以上，收敛速度更快；路径长度缩短 左右，搜索效果相对较好。路径优化效果分析本文通过对随机路径搜索结果进行路径裁剪和平滑处理，得到优化后的路径。在简单环境和复 （）杂环境下，路径点数、转折次数和路径长度在路径优化前后的比实验数据如表所示，运行结果如图所示，图例补充说明如表所示。表不同环境下路径优化前后对比实验数据实验场景处理路径点数转折次数路径长度 简单环境优化前 优化后 复杂环境优化前 优化后 图不同环境下路径优化效果表图例说明图例样式图例名称前路径及路径节点剪枝后路径最终优化路径由表可知，优化后的路径点数与路径长度有效减少。简