1、第 42 卷第 3 期重 庆 交 通 大 学 学 报(自 然 科 学 版)Vol 42No32023 年 3 月JOUNAL OF CHONGQING JIAOTONG UNIVESITY(NATUAL SCIENCE)Mar 2023DOI:103969/jissn1674-069620230320改进时间弹性带的动态避障轨迹规划系统研究谢春丽，刘斐灏(东北林业大学 交通学院，黑龙江 哈尔滨 150040)摘要:在动态障碍物存在的机器人导航场景下，时间弹性带算法(timed elastic bands，TEB)无法区分障碍物类型，易将动态障碍物视为静态类型去处理，致使导航过程出现碰撞而无法完
2、成导航任务。针对机器人运行真实动态环境，采用两个均值滤波器对激光点云进行滤波处理，实现动态障碍物的检测，而后增加动态障碍物代价地图层对检测到的障碍物进行聚类，利用卡尔曼滤波对动态障碍物的运动状态进行轨迹跟踪及状态预测，结合机器人当前运动状态作出动态避障的轨迹规划，采用阿克曼转向结构机器人进行仿真及真实动态环境下的重复实验。研究结果表明:改进 TEB 算法的动态避障轨迹规划系统能够在复杂动态环境中进行实时轨迹规划，生成安全平滑的局部轨迹，实现动态避障完成导航任务，能够满足移动机器人的动态避障要求。关键词:车辆与机电工程;时间弹性带算法;动态避障;卡尔曼滤波;轨迹规划中图分类号:TP242;U46
3、1文献标志码:A文章编号:1674-0696(2023)03-143-08Dynamic Obstacle Avoidance Trajectory Planning System withImproved Timed-Elastic-BandsXIE Chunli，LIU Feihao(School of Traffic Transportation，Northeast Forestry University，Harbin 150040，Heilongjiang，China)Abstract:In the robot navigation scene with dynamic obstacl
4、es，the timed elastic bands(TEB)algorithm cannot distinguishthe types of obstacles，and it is easy to treat dynamic obstacles as static types，resulting in collisions in the navigationprocess and unable to complete the navigation task Aiming at the real dynamic environment of the robot，two mean filters
5、were used to filter the laser point cloud to achieve dynamic obstacle detection，and then the dynamic obstacle cost map layerwas added to cluster the detected obstacles Kalman filtering was used to perform the trajectory tracking and state predictionon the motion state of dynamic obstacles Based on t
6、he current motion state of the robot，a dynamic obstacle avoidancetrajectory plan was carried out The Ackerman steering structure robot was used for simulation and the repeated experimentsin real dynamic environments The research results show that the dynamic obstacle avoidance trajectory planning sy
7、stem withimproved TEB algorithm can perform real-time trajectory planning in complex dynamic environments and generate safe andsmooth local trajectories，which realizes the dynamic obstacle avoidance to complete the navigation task and can meet thedynamic obstacle avoidance requirements of mobile rob
8、otsKey words:vehicle and mechatronics engineering;TEB algorithm;dynamic obstacle avoidance;Kalman filter;trajectoryplanning0引言移动机器人的自主导航中，避障是最关键的功能之一1。避障的实时性与稳定性是当前移动机器人路径规划的典型挑战2。随着场景的复杂性增加，动态障碍物对于算法的实时性要求增加3，为在动态环境下生成安全平滑的轨迹，需要轨迹规划算法能够在动态场景下对动态障碍物实时进行轨迹跟收稿日期:2021-11-11;修订日期:2022-03-19基金项目:黑龙江省自然科学
9、基金项目(LH2021F002);中央高校基本科研业务费专项资金项目(2572018BG02)第一作者:谢春丽(1978)，女，黑龙江哈尔滨人，副教授，博士，主要从事车辆工程、人工智能、故障诊断方面的研究。E-mail:xcl08 126com通信作者:刘斐灏(1998)，男，湖南娄底人，硕士研究生，主要从事智能汽车路径规划方面的研究。E-mail:liufei_hao0608 163com踪与运动状态预测4。现有机器人动态避障方法存在对复杂动态环境处理能力不够，易陷入局部最小值5，且实时性不满足实际应用的需要，且在混合场景下无法准确区分动态与静态障碍物类型6。因此动态避障轨迹规划系统需要对复
10、杂环境有较优的处理能力，并能实时求解出可行轨迹7。TEB 算 法8 是 基 于 弹 性 带(elastic bands，EB)9 方法改进，EB 算法使用两个分量来产生避撞路径，即排斥力与收缩力，障碍物施加排斥力在弹性带上形成安全距离，收缩力在避障条件满足的前提下让路径收缩。TEB 算法能够产生一条无碰撞的时间最优化运动路径，对环境中的障碍物进行实时的检测并作出在线规划。动态障碍物环境下，轨迹规划算法需要对环境信息有较优且快速的处理能力，并且能对动态障碍物进行轨迹跟踪与预测10。然而，TEB 算法在动态场景下会将动态障碍物视为静态去处理，导致机器人在导航过程中与动态障碍物发生碰撞。针对上述问题
11、，笔者采用不同频率的均值滤波器区分动、静态障碍物，增加动态障碍物代价地图，对障碍物进行聚类处理11，结合卡尔曼滤波12 对障碍物的运动轨迹进行跟踪预测，通过预测障碍物轨迹，结合 TEB 算法规划合理的轨迹进行避障。1阿克曼机器人的运动学模型采取阿克曼转向结构机器人进行仿真与真实实验，阿克曼转向结构机器人的运动学模型如图 1。图 1阿克曼转向结构机器人运动学模型Fig 1Kinematics model of Ackerman steering structure robot图中(Xw，Yw)为世界坐标系下坐标轴，(X，Y)为机器人基坐标系，坐标原点为机器人后轴中心 S点，其中车辆前进方向为 X
12、 轴正方向，垂直车身向左为 Y 轴正方向。I 点为阿克曼转向机器人瞬时旋转中心(instantaneous center of rotation，IC)，代表车辆前轮转向角，其中(/2，/2)，为车辆最小转弯半径，为车身与世界坐标系横轴夹角。左、右后轮的平移速度分别为 vl、vr，且满足机器人线速度 v=(vl+vr)/2。L 为轴距，其前轮转向角 与转弯半径、轴距 L 有关，需满足式(1):=L/tan=v/(1)式中:为机器人角速度。设在 t 时刻，机器人姿态由位置状态 s(t)和运动状态 u(t)=v(t)，(t)T表示，则阿克曼机器人运动学状态方程表示为式(2):s(t)=x(t)y(
13、t)(t)=v(t)cos(t)v(t)sin(t)v(t)/Ltan(t)(2)2TEB 算法原理与求解优化流程21TEB 算法基本原理TEB 算法最初是为移动基础机器人导航规划开发的。通过在弹性带的基础上引入时间因素，增加机器人相邻位姿的时间间隔。因此，机器人的离散运动轨迹 T 可描述为 N 个有序的位姿与时间间隔的集合，表示为式(3):T=S1，T1，Sk，Tk，TN1，SNT(3)Sk=Pk，kT，k 1，n，Pk=xk，yk式中:Sk表示机器人在世界坐标系下的位姿，其中Pk为机器人坐标。Tk为位姿 Sk过渡到 Sk+1的时间间隔。由连续机器人位姿组成的轨迹示例如图 2。图 2轨迹连续
14、位姿序列与时间间隔Fig 2Trajectory continuous pose sequence and time interval22TEB 求解优化流程在 TEB 中引入的时间因素将机器人控制系统与速度，加速度等动态约束进行拓展，能够为局部规划器提供一个持续优化的解决方案，在非完整约束441重 庆 交 通 大 学 学 报(自 然 科 学 版)第 42 卷移动机器人上得以适用13，如图 3。通过在局部代价地图中对全局路径的子集进行采样，创建局部姿态序列，将全局路径转变为弹性带进行优化，其生成的路径不一定是最短路径，但能够满足时间最优。在障碍物环境较多情况下，为防止在路径规划过程陷入局部极小
15、值，对具有不同拓扑结构的多条轨迹进行维护和优化，并提出两阶段优化算法解决轨迹局部最优问题14，而后针对阿克曼转向结构机器人进行拓展15。图 3TEB 算法的优化流程Fig 3The optimization process of TEB algorithm最优轨迹 b*是通过最小化代价函数获得，这些函数包括运动规划中的部分冲突目标与约束。这些目标包括 CPU 资源消耗、路径长度、总运动时间或以上目标函数的加权组合。通过对不符合移动机器人运动学约束的轨迹加以惩罚函数，将轨迹允许解限制在可行集内。TEB的优化问题被定义为一个加权非线性最小二乘代价函数，该代价函数考虑目标函数 J 和惩罚函数 P 的
16、冲突集，表示为:b*=argminb s1，sNiif2i(b)，i J，P(4)式中:b s1，sN 表示轨迹中的起始位姿 s1与目标位姿sN不受优化;i为每个目标函数的加权因子;fJ为目标函数的综合，目标函数 J 中包括计算资源，轨迹，运动时间等因素;fP为惩罚函数综合，惩罚函数 P 包括障碍物约束、非完整运动学约束、速度与加速度约束等。将 TEB 优化问题映射到一个超图上，构成解向量的机器人姿态 sk和时间间隔 Tk组成超图的顶点，超图的边由非线性规划的代价项组成。此外，将不受优化的机器人起始和目标位姿、静态障碍物等静态参数作为超图的固定顶点。得到的超图通过g2o 框架进行有效求解，该图优化框架利用系统矩阵的 稀 疏 结 构，采 用 了 鲁 棒 性 和 效 率 较 高 的Levenberg-Marquardt(LM)算法求解，计算出各个目标函数与惩罚函数的加权因子，通过最小化代价函数求解出最优轨迹。3基于卡尔曼滤波的障碍物运动估计在 OS 的运动规划框架中，代价地图为全局及局部规划器提供实时环境信息。代价地图用一个网格表示，其中每个单元格存在 3 种状态设定:即空闲(0)、占用(
