(19)中华 人民共和国 国家知识产权局 (12)发明 专利申请 (10)申请公布号 (43)申请公布日 (21)申请 号 202111396447.1 (22)申请日 2021.11.23 (71)申请人 北京科技大 学顺德研究生院 地址 528300 广东省佛山市顺德区大良致 慧路2号 (72)发明人 顾青　 (74)专利代理 机构 北京市广友专利事务所有限 责任公司 1 1237 代理人 张仲波　邓琳 (51)Int.Cl. G06Q 10/04(2012.01) G06F 17/18(2006.01) (54)发明名称 一种铰接式采矿装备交叉口转弯最优轨迹 规划方法及系统 (57)摘要 本发明公开了一种铰接式采矿装备交叉口 转弯最优轨迹规划方法及系统， 所述方法包括： 搭建用于轨迹规划算法的运动学模 型； 设计环境 感知模块， 检测装备是否进入到待转区域； 设计 基于二次规划的轨迹规划模块， 将 轨迹规划问题 解耦为纵向和横向轨迹规划； 设计基于三维搜索 的关键参数确定模块， 将行驶时间、 纵向行驶距 离和横向行驶距离作为搜索策略的关键因素， 分 别设计基于行驶时间的搜索外循环， 基于纵向行 驶距离的搜索中循环和基于横向行驶距离的搜 索内循环， 并在此基础上通过轨迹规划算法获得 最优解； 设计约束检查模块， 对得到的轨迹进行 约束检查。 本发 明能够减少自主驾驶系统调试和 部署的时间， 有利于自动化设备性能的充分发 挥， 提高生产效率。 权利要求书6页 说明书16页 附图6页 CN 114139787 A 2022.03.04 CN 114139787 A 1.一种铰接式采 矿装备交叉口转弯最优轨 迹规划方法， 其特 征在于， 包括以下步骤： 搭建用于 轨迹规划算法的铰接式移动采 矿装备的运动学模型； 设计环境感知模块， 检测铰接式移动采矿装备是否进入到待转区域， 以及为轨迹规划 模块输入 入口速度、 位置以及姿态； 设计基于二次规划的轨迹规划模块， 将轨迹规划问题解耦为纵向和横向轨迹规划， 首 先设计纵向轨 迹规划模型生成纵向轨 迹， 然后设计横向轨 迹规划模型生成横向轨 迹； 设计基于三维搜索的关键参数确定模块， 将行驶时间、 纵向行驶距离和横向行驶距离 作为搜索策略的关键因素， 分别 设计基于行驶时间的搜索外循环， 基于纵向行驶距离的搜 索中循环和基于横向行驶距离的搜索内循环， 当基于二次规划的轨迹规划算法以给定的初 始行驶时间和行驶距离进行求解出现无解时， 依 次通过内循环、 中循环和外循环不断调整 行驶时间、 纵向行驶距离和横向行驶距离直到找到合适的行驶时间和行驶距离， 并在此基 础上通过轨 迹规划算法获得最优解； 设计约束检查模块， 对基于二次规划的轨迹规划算法得到的轨迹进行约束检查， 保证 轨迹符合铰接角及铰接角速度约束， 若当前轨迹不满足约束条件， 根据三维搜索策略依 次 在内循环、 中循环和外循环中不断迭代规划 新的轨迹直到约束检查得到满足， 并将该轨迹 作为最优轨迹。 2.根据权利要求1所述的铰接式采矿装备交叉口转弯最优轨迹规划方法， 其特征在于， 所述铰接式移动采 矿装备的运动学模型的表达式为： 其中， xf和yf分别是前车体轴心位置的纵坐标和横坐标， θf是铲运机 前车体的航向角， Lf 是铰接角到前车体轴心位置的距离， Lr是铰接角到后车体轴心位置的距离， γ是铰接角， 是铰接角γ的角速度。 3.根据权利要求1所述的铰接式采矿装备交叉口转弯最优轨迹规划方法， 其特征在于， 所述设计 基于二次规划的轨 迹规划模块具体包括： 设计纵向轨迹规划模型， 纵向轨迹规划模型的目标函数由纵向加速度及其变化率所组 成， 表示为： 并满足如下约束： 权　利　要　求　书 1/6 页 2 CN 114139787 A 2xmin≤xt≤xmax,t＝1,......,N           (6) x0＝xin                                 (9) xN＝xend                                 (12) 其中， xt、 和 为决策变量， N是离散间隔数目， Δt是时间间隔， t表示时间间隔索 引， xt是前车体轴心位置的纵向位置， xmin和xmax是xt的边界， 是前车体轴心位置的纵向速 度， vxmin和vxmax是 的边界， 是前车体轴心位置的纵向加速度， axmin和axmax是 的边界， Δaxmax和Δaxmin是 的边界， λ1和 λ2是权重系数， λ1+λ2＝1， λ1＞0, λ2≥ 0， xin和xend分别为xt的起点和终点约束， 和 分别是 和 的起点约束， α为转弯角 度； 设计横向轨迹规划模型， 横向轨迹规划模型的目标函数由横向加速度及其变化率所组 成， 表示为： ymin≤yt≤ymax,t＝1,......,N                 (18) y0＝yin                                         (21) yN＝yend                                        (24) 其中， yt、 和 为决策变量， yt是前车体轴心位置的横向位置， ymin和ymax是yt的边界， 是前车体轴心位置的横向速度， vymin和vymax是 的边界， 是前车体轴心位置的横向权　利　要　求　书 2/6 页 3 CN 114139787 A 3