一种基于混合A*与TEB融合的起重机动态路径规划与避障方法及系统与流程

摘要: 本发明属于工程机械自动化与智能控制,具体涉及一种基于混合与时间弹性带融合的起重机动态路径规划与避障方法及系统。背景技术:1、随着智能建造的发展,起重机作为建筑工...
一种基于混合A*与TEB融合的起重机动态路径规划与避障方法及系统与流程

本发明属于工程机械自动化与智能控制,具体涉及一种基于混合与时间弹性带融合的起重机动态路径规划与避障方法及系统。

背景技术:

1、随着智能建造的发展,起重机作为建筑工地核心的物料搬运设备,其自主导航与智能化作业水平至关重要。真实的施工现场往往是高度非结构化且动态的复杂环境,存在大量静态障碍物和不可预测的动态障碍物。

2、目前,现有的路径规划与避障算法在应用于起重机这一特殊的大型工程机械时,存在以下显著的技术缺陷:

3、未充分考虑大型机械的非完整运动学约束。传统的全局规划算法,传统算法、dijkstra算法,生成的路径多为折线,存在尖锐折角。起重机因受最小转弯半径等约束,无法直接跟踪执行此类路径,强行执行会导致频繁原地转向及严重的机械磨损。

4、应对人机混杂动态环境的能力不足。 传统的局部避障算法在复杂工地中极易陷入局部最优导致起重机“死锁”停滞;同时,现有算法缺乏对动态障碍物未来轨迹的长期预测能力,面对突发状况极易因制动距离不足发生碰撞。

5、忽略了带载作业的“柔性悬吊”动力学特性。 现有算法多将设备视为刚体,未对运行轨迹的曲率与加减速度进行严格约束。起重机在急转弯或急变加速时,极易激发悬吊重物的剧烈摆动,导致作业失准甚至整机失稳倾覆。

6、缺乏对吊臂三维空间伸展的有效防碰约束。传统的二维代价地图仅能描述起重机底盘的投影轮廓,无法表达高耸且向外大幅伸缩的吊臂,极易导致起重机在底盘无碰撞风险时,吊臂却与高空障碍物发生刮蹭。

技术实现思路

1、本发明针对现有技术难以在保证全局路径最优的同时,兼顾大型起重机的非完整运动学约束、复杂动态环境下的前瞻性避让,且未能有效解决带载防摇摆与吊臂空间防碰撞的问题,提供一种基于混合与teb融合的起重机动态路径规划与避障方法,通过在全局混合算法中剥离单纯的网格搜索,利用双重启发式函数深度融合起重机非完整运动学约束,生成的路径从源头消除尖锐折角,将防摇摆横向加速度极限化为代价约束,并建立吊臂独立动态投影包络,引入多模态动态目标预测与自适应时空安全边界,不再对所有动态障碍物一刀切地避让,显著降低了底层机械机构的磨损与转向延迟,消除了盲区内的高空吊臂剐蹭风险,实现了带载防摇摆与吊臂防剐蹭的立体主动安全防护。

2、根据本发明说明书的一方面,提供一种基于混合与teb融合的起重机动态路径规划与避障方法,包括:

3、步骤s1、基于实时获取的施工现场的环境点云与图像数据,构建包含静态障碍物层和动态障碍物层的多层代价地图,并根据起重机的物理外廓尺寸对所述静态障碍物层和动态障碍物层进行膨胀处理,生成起重机作业安全边界层;

4、步骤s2、基于起重机作业安全边界层,结合起重机底盘的非完整运动学约束建立运动学模型,采用混合算法在所述运动学模型的连续状态空间中进行全局节点扩展搜索,生成初始全局平滑路径;

5、步骤s3、基于初始全局平滑路径得到初始状态集合,结合多层代价地图中的动态障碍物层,利用时间弹性带算法在预设多目标代价函数的约束下,对所述初始状态集合进行局部轨迹形变与时间参数优化,生成局部避障轨迹;所述局部避障轨迹用于生成驱动起重机的控制指令。

6、作为进一步的技术方案,所述混合算法采用双重启发式函数,表达式为:

7、

8、其中,为双重启发式函数,为当前搜索节点,为目标节点;为忽略环境障碍物前提下,当前节点到目标节点的reeds-shepp曲线距离;中的下标rs表示reeds-shepp曲线距离;为忽略起重机运动学约束前提下,在二维栅格地图上当前搜索节点到目标节点的最短无碰撞路径距离;中的下标2d表示二维平面路径距离。

9、作为进一步的技术方案,所述预设的多目标代价函数的表达式为:

10、

11、其中,为预设的多目标代价函数,为轨迹跟随惩罚项,为轨迹跟随惩罚项的权重系数,为运动学极限惩罚项,为运动学极限惩罚项的权重系数,为常规避障惩罚项,为常规避障惩罚项的权重系数,为起重机带载防摇摆惩罚项,为起重机带载防摇摆惩罚项的权重系数,为起重机吊臂防碰撞惩罚项,为起重机吊臂防碰撞惩罚项的权重系数。

12、作为进一步的技术方案,所述起重机带载防摇摆惩罚项的约束条件为:

13、对局部避障轨迹的线速度和轨迹曲率进行约束,使得起重机在局部避障轨迹上任意时间节点的线速度与该时间节点处的轨迹曲率满足横向加速度约束。

14、作为进一步的技术方案,所述起重机吊臂防碰撞惩罚项的处理过程,包括:

15、将起重机的底盘与起重机上方伸出的吊臂进行空间物理隔离建模;

16、获取吊臂的实时伸出长度和仰角数据,将吊臂三维姿态投影至所述多层代价地图的二维平面,生成动态包络框;

17、利用时间弹性带算法在未来位姿序列推演中预测所述动态包络框与静态或动态障碍物的安全膨胀区,若两者发生几何重叠,则向预设的多目标代价函数中加入防碰撞代价值以触发起重机吊臂防碰撞惩罚;其中,所述安全膨胀区为在障碍物物理轮廓基础上向外扩展预设安全缓冲距离所形成的区域;所述防碰撞代价值为在多目标代价函数中设定的趋于无穷大的惩罚权重。

18、作为进一步的技术方案,结合多层代价地图中的动态障碍物层,包括以下动态障碍物轨迹预测步骤:

19、基于环境点云与图像数据感知到的动态障碍物进行目标检测与属性分类;

20、对于具有非完整运动学约束的车辆类目标,采用恒定转弯率和速度模型预测车辆类目标在未来时间窗口内的移动轨迹;

21、对于高机动且无规律的行人类目标,采用引入高斯噪声的恒定速度模型,预测行人类目标在未来时间窗口内随时间呈放射状膨胀的位置概率分布区域。

22、作为进一步的技术方案,在预测得到动态障碍物在未来时间窗口内的移动轨迹或位置概率分布区域后,还包括引入自适应时空安全边界,具体为:

23、当起重机的动态包络框与动态障碍物预测位置之间的最短欧式距离小于所述自适应时空安全边界时,压缩相邻位姿节点之间的时间差以生成主动减速控制指令;

24、当只减速无法避免碰撞且满足横向加速度约束时,触发局部避障轨迹的横向形变以生成变道避让指令。

25、作为进一步的技术方案,所述自适应时空安全边界的表达式为:

26、

27、其中,为自适应时空安全边界,为动态障碍物的基础物理半径,为动态障碍物当前时刻的速度向量,为速度敏感系数,为时间不确定性膨胀系数,为当前时刻,为预测的时间节点。

28、作为进一步的技术方案,所述初始全局平滑路径与所述起重机作业安全边界层不发生干涉。

29、根据本发明说明书的一方面,提供一种基于混合与teb融合的起重机动态路径规划与避障系统,包括:

30、多层代价地图构建模块,用于基于实时获取的施工现场的环境点云与图像数据,构建包含静态障碍物层和动态障碍物层的多层代价地图,并根据起重机的物理外廓尺寸对所述静态障碍物层和动态障碍物层进行膨胀处理,生成起重机作业安全边界层;

31、全局先验规划模块,用于基于起重机作业安全边界层,结合起重机底盘的非完整运动学约束建立运动学模型,采用混合算法在所述运动学模型的连续状态空间中进行全局节点扩展搜索,生成初始全局平滑路径;

32、局部动态优化模块,用于基于初始全局平滑路径得到初始状态集合,结合多层代价地图中的动态障碍物层,利用时间弹性带算法在预设多目标代价函数的约束下,对所述初始状态集合进行局部轨迹形变与时间参数优化,生成局部避障轨迹;所述局部避障轨迹用于生成驱动起重机的控制指令。

33、与现有技术相比,本发明的有益效果是:

34、1、本发明通过在全局混合算法中剥离单纯的网格搜索,转而利用双重启发式函数深度融合起重机非完整运动学约束,生成的路径从源头消除尖锐折角,无需繁琐的后期平滑处理,显著降低了底层机械机构的磨损与转向延迟,改善了传统算法的理论可行但物理不可执行困境。

35、2、不同于传统teb算法仅视机器人为二维质点或简单多边形,本发明创新性地将防摇摆横向加速度极限化为代价约束,并建立吊臂独立动态投影包络,这使得起重机在进行局部紧急避障时,不会因惯性引发致命的重物钟摆效应,同时消除了盲区内的高空吊臂剐蹭风险,实现了带载防摇摆与吊臂防剐蹭的立体主动安全防护。

36、3、本发明通过引入多模态动态目标预测与自适应时空安全边界,不再对所有动态障碍物一刀切地避让,配合“减速优先、变道次之”的类人驾驶控制策略,在保障绝对安全的前提下,大幅减少了工地环境中由于工人穿梭引发的“假死锁”和无效急刹停滞,提升了全周期的转运效率,提高了起重机应对人机混杂环境的高阶博弈能力。