：本发明属于自动驾驶领域，具体地说是一种激光导引agv的动态避障控制方法。
背景技术
0、
背景技术：
1、随着现代工厂生产自动化与智能化物流体系的持续升级，传统依赖人工运输及半自动化作业模式已难以满足高效、精准、灵活的工业需求。在智能制造与柔性生产成为主流趋势的背景下，自动化控制生产设备的广泛应用，不仅显著提升了工厂的生产效率与物流传输速度，更成为推动社会经济高质量发展的关键驱动力。在这一进程中，自动导引运输车(简称agv)作为核心物流装备，凭借其自主导航、灵活调度与高可靠性等特性，正逐步取代传统运输方式，广泛应用于仓储管理、生产线物料配送、成品搬运等场景。目前，agv的导引技术主要分为磁带导引、二维码导引和激光导引三种导引方式，其中激光导引agv通过搭载高精度激光雷达，能够实时扫描环境并构建地图，实现自主路径规划与定位导航，相较于传统导引方式具有显著优势：一方面，激光导引无需依赖磁条、二维码等物理标记，大幅降低了场地改造成本与维护复杂度；另一方面，激光雷达的高分辨率与快速响应特性，使得agv能够在复杂多变的工业环境中实现高精度定位，同时具备更强的环境适应性，为工厂的智能化升级与柔性化生产提供了坚实的技术支撑。
2、在agv执行物料搬运或其他任务的过程中，不可避免地会遇到人员走动、其他移动设备等动态障碍物，目前agv所采用的避障方式有两种：一种是通过调整全局路径，对动态障碍物进行避障，但其导致路径连贯性降低并增加计算复杂度，同时也会影响agv完成任务的时间和效率；另一种是局部路径规划控制器进行动态避障，时间弹性带算法(简称teb)是agv常用的局部路径规划的控制器，具有动态优化路径，同时兼顾避障效率与运动学约束的优势。然而，传统teb进行动态避障时依赖代价地图，由于动态障碍物无法实时更新且数据处理存在延迟，导致规划响应滞后，易频繁重规划甚至存在碰撞风险。
3、专利cn120043527a提出了一种结合teb动态避障与改进双向a*算法的路径规划方法，通过5邻域双向搜索提升全局路径效率，并利用teb实现局部避障，增强静态环境下路径连贯性。然而，专利cn120043527a未引入障碍物轨迹预测，仅基于当前位置进行teb优化，难以应对动态障碍物；此外，专利cn120043527a采用了全局与局部解耦架构，路径被阻断时需重启规划，鲁棒性较差。专利cn118938924a提出一种融合卡尔曼滤波与改进动态窗口算法(简称dwa)的动态避障方法，能预测障碍物轨迹并优化dwa评价函数，提升动态避障效果。然而，专利cn118938924a的预测结果未转化为时空约束，缺乏主动避障能力；此外，dwa评价函数以几何距离为主，未考虑速度矢量夹角威胁，动态密集场景下易发生碰撞。
技术实现思路
0、
技术实现要素：
1、针对现有技术的不足，本发明提出了一种激光导引agv的动态避障控制方法。该方法将安装在agv上的激光雷达点云信息经障碍物感知模块生成障碍物簇信息；障碍物预测模块根据障碍物簇预测障碍物未来运动轨迹；改进型teb规划控制器综合障碍物未来运动轨迹和agv输出的车辆状态信息，通过多目标优化方法求解局部路径并输出agv的角速度与线速度，控制agv执行避障动作，提升agv对动态障碍物的避障能力。
2、本发明解决技术问题所采取的技术方案如下：
3、本发明为一种激光导引agv的动态避障控制方法，该方法包括障碍物感知模块、障碍物预测模块和改进型teb规划控制器和agv。
4、该方法包括以下步骤：
5、步骤1、障碍物感知模块设计：
6、agv将激光雷达原始数据以极坐标形式(rn,θn)传入到障碍物感知模块，其中(rn,θn)表示第n个激光点的测距值和角度。通过xn＝rn cosθn，yn＝rn sinθn将激光雷达的极坐标数据转换为直角坐标数据。进一步地，利用坐标变换将激光雷达直角坐标点云数据映射到全局坐标系，实现点云位置的统一，以保证障碍物的位置一致性。
7、根据欧式距离d(p,q)采用基于密度的聚类算法对激光雷达点云信息进行聚类，如式(1)到式(2)：
8、
9、n(p)＝{q|d(p,q)≤ε} (2)
10、式中，p＝(xp,yp),q＝(xq,yq)分别是激光雷达两束激光点的坐标；n(p)是邻域集合；ε是邻域半径。
11、若某激光点在其邻域集合内包含不少于minpts的点，则此激光点被视为核心点，并扩展形成障碍物簇，实现障碍物的有效分离。其中，minpts表示激光点领域集合内构成高密度区域所需的最小点数。
12、步骤2、障碍物预测模块设计：
13、为区分激光雷达检测静态障碍物与动态障碍物的点云数据在时序上的位置变化，采用目标匹配和运动估计方法，跟踪障碍物的运动状态。
14、假设障碍物预测模块在t1和t2两个时刻分别得到两帧点云数据pclt1和pclt2，则障碍物的运动状态可以通过最近邻搜索方法进行匹配，如式(3)：
15、
16、式中，pz是点云数据pclt1中的障碍物中心点，pt2是点云数据pclt2中的任意一个激光点，是pz在点云数据pclt2中最近的障碍物中心匹配点。
17、若某个障碍物在两帧数据中的匹配误差大于设定的阈值dth，如式(4)：
18、
19、则判断该障碍物为动态障碍物，并计算障碍物中心的速度，如式(5)和式(6)：
20、
21、
22、式中，(x1,y1)是t1时刻的障碍物中心位置坐标，(x2,y2)是t2时刻的障碍物中心位置坐标，vx,yy分别是障碍物中心在x,y轴上的速度分量。
23、在障碍物匹配的基础上，采用卡尔曼滤波对移动障碍物进行目标跟踪，以平滑障碍物的运动轨迹，并预测障碍物未来运动轨迹，状态向量定义如式(7)：
24、xk＝[xk,yk,vx,vy]t (7)
25、卡尔曼滤波的状态更新方程，如式(8)和式(9)：
26、xk＝fxk-1+ωk (8)
27、zk＝hxk+vk (9)
28、式中，xk是k时刻障碍物的状态向量，包括障碍物中心的位置坐标和速度信息；f是状态转移矩阵，用于预测k+1时刻障碍物的位置；h是观测矩阵；zk表示k时刻激光雷达测量得到的障碍物状态向量；ωk和vk分别是过程噪声和测量噪声。
29、通过卡尔曼滤波，系统可平滑跟踪障碍物轨迹并抑制测量噪声。同时，提取动态障碍物的几何边界尺寸，作为teb避障模型的空间约束参数。在轨迹估计基础上，系统基于匀速模型预测障碍物未来位置，假设其在短时间内保持恒定速度运动，则预测的障碍物未来运动轨迹，如式(10)和式(11)：
30、xk+δk＝xk+vx·δk (10)
31、yk+δk＝yk+yy·δk (11)
32、式中，xk,yk是k时刻障碍物的横坐标和纵坐标，vx,yy分别是障碍物的水平和侧向速度，δk为预测步长。
33、步骤3、改进型teb规划控制器设计：
34、步骤3.1、信息输入：
35、改进型teb规划控制器进行动态避障，主要依赖障碍物未来运动轨迹和agv的车辆状态信息。
36、所述障碍物未来运动轨迹：包含未来t时间窗内所有障碍物的位置序列。
37、agv的车辆状态信息：由agv车辆的位姿、线速度与角速度组成，通过时间戳同步策略消除传感与规划间的时序偏差，确保数据时效性。
38、步骤3.2、改进型teb规划控制器根据障碍物未来运动轨迹和agv车辆状态信息构建一种融合距离约束、时间约束以及速度方向约束的动态避障约束函数，具体如下包括如下步骤：
39、步骤3.2.1、动态避障约束函数中的距离约束函数设计：
40、为提升agv的避障能力，引入距离约束函数，限制agv路径点与障碍物未来运动轨迹间的最小距离。在距离约束函数中障碍物以矩形近似建模，并通过宽度ωj和高度hj对agv路径点与障碍物未来运动轨迹间的距离进行归一化，如式(12)：
41、
42、式中，xi,yi是agv路径点上的横坐标和纵坐标；xj(k),yj(k)是障碍物在k时刻的未来运动轨迹点坐标；dij(k)是表征agv轨迹点与障碍物未来运动轨迹之间的最小距离；[k1,k2]是障碍物未来运动轨迹预测的时间段；i是agv路径点索引；j是障碍物未来运动轨迹点索引。
43、定义距离约束函数ed_p用于控制agv与障碍物间的安全距离，如式(13)：
44、
45、式中，λd是距离约束的惩罚权重，用于调节距离约束在teb规划中的影响程度；dsafe是安全距离；dm是基于预测不确定性调整的安全范围。
46、此外，为提升避障前瞻性，引入障碍物未来轨迹点索引的动态选取机制：
47、首先，计算agv当前位置pa＝(xa,ya)与障碍物当前位置po＝(xo,yo)的距离dagv-obs，如式(14)：
48、
49、其次，根据agv与障碍物的距离远近，动态调整障碍物未来运动轨迹点的索引j，如式(15)：
50、
51、式中，m是预测轨迹点序列的总长度，m＝tpredict/step_size，tpredict是预测时长，step_size是预测步长，dfar是agv与障碍物的距离阈值，argmin函数用于求取区间[1,m]范围内函数最小的s值。
52、步骤3.2.2、动态障碍约束函数中的避障时间约束函数设计：
53、在局部路径规划中，避障不仅需考虑空间约束，还需引入时间约束，以避免agv与动态障碍物同时到达同一位置。系统从障碍物未来运动轨迹中选取与agv最近的点作为预测点pt，分别计算agv和障碍物到达预测点pt的时间ta和to，如式(16)和式(17)：
54、
55、
56、计算agv和障碍物到达预测点的时间差δt，如式(18)：
57、δt＝|ta-to| (18)
58、式中，分别是预测点、agv当前位置和障碍物当前位置的位置向量，分别是agv和障碍物的速度向量。
59、为确保足够的时间间隔，引入避撞时间约束函数et，如式(19)：
60、et＝λt·max(0,tsafe-|δt|) (19)
61、式中，λt是时间约束的权重因子，tsafe是预定义的安全时间间隔，max函数用于选取0和tsafe-|δt|的最大值。
62、步骤3.2.3、动态避障约束函数中的速度方向约束函数设计：
63、引入速度方向约束，包括避障速度约束与目标对齐约束，用于保障避障安全和效率。避障速度约束通过预测障碍物的未来移动范围，提升动态避障效果。在避障速度约束函数中将障碍物建模为以当前速度方向为长轴的椭圆，用于覆盖障碍物的潜在移动区域，为teb规划器规划提供实时数据。首先计算agv与障碍物的相对速度vrel，如式(20)：
64、vrel＝va-vo (20)
65、式中，va是agv的速度，vo是障碍物的速度。
66、将障碍物建模为椭圆，椭圆中心为障碍物的当前估计位置，定义椭圆半长轴a与半短轴b，如式(21)和式(22)：
67、
68、
69、式中，hj是设定的障碍物的初始圆周直径，ka是半长轴膨胀系数，kb是半短轴膨胀系数。
70、为判断agv相较于障碍物建模椭圆的位置，建立障碍物范围距离函数g(pagv)，如式(23)：
71、
72、计算障碍物范围距离函数的梯度用于路径调整，并加入避障系数λavoid，构建避障速度约束函数eavoid，如式(24)：
73、
74、式中，λavoid是避障系数，表示函数g(pagv)的梯度向量，指向函数增加最快的方向；max函数用于求取0和的最大值。
75、所述目标对齐约束用于确保agv朝向目标前进，避免后退或偏离方向。进一步地，计算agv速度向量与目标方向的余弦值，如式(25)：
76、
77、引入了目标系数λalign，设计目标对齐约束函数ealign，如式(26)：
78、ealign＝λalign·(1-cosθ)2 (26)
79、步骤3.3、多目标优化求解：
80、teb通过多目标优化器实时求解包含动态避障约束的代价函数。同时优化路径控制点序列和时间间隔，最终输出agv的线速度和角速度，控制agv完成避障动作。
81、本发明的有益效果是：本发明涉及一种激光导引agv的动态避障控制方法，该方法基于agv上的激光雷达将点云信息通过障碍物感知模块实时提取为障碍物簇，融合障碍物预测模块输出的障碍物未来运动轨迹，采用集成动态避障约束函数的改进型teb规划控制器，结合agv的车辆状态信息进行多目标优化求解，确定agv的角速度与线速度，控制agv执行避障，从而提高agv的动态避障能力。