工程科学学报 Chinese Journal of Engineering 基于双维度搜索的地下自主铲运机最优转弯轨迹规划 顾青刘立白国星孟宇 Optimal turning trajectory planning of an LHD based on a bidimensional search GU Qing,LIU Li,BAI Guo-xing.MENG Yu 引用本文： 顾青，刘立，白国星，孟宇.基于双维度搜索的地下自主铲运机最优转弯轨迹规划工程科学学报，2021,432)：289-298. doi10.13374j.issn2095-9389.2020.11.09.002 GU Qing.LIU Li.BAI Guo-xing.MENG Yu.Optimal turning trajectory planning of an LHD based on a bidimensional search [J]. Chinese Journal of Engineering,.2021,43(2:289-298.doi:10.13374j.issn2095-9389.2020.11.09.002 在线阅读View online::htps/ldoi.org10.13374/.issn2095-9389.2020.11.09.002 您可能感兴趣的其他文章 Articles you may be interested in 高速公路绿篱修剪机器人手臂避障路径规划 Obstacle avoidance path planning for expressway hedgerow pruning robot manipulator 工程科学学报.2019.41(1)：134htps:1doi.org/10.13374.issn2095-9389.2019.01.015 集总干扰下六旋翼飞行器的轨迹跟踪控制 Trajectory tracking control for an unmanned hexrotor with lumped disturbance 工程科学学报.2018,40(5：622 https:/1doi.org10.13374j.issn2095-9389.2018.05.013 无人直升机自抗扰自适应轨迹跟踪混合控制 Trajectory-tracking hybrid controller based on ADRC and adaptive control for unmanned helicopters 工程科学学报.2017,3911：1743htps1doi.0rg/10.13374j.issn2095-9389.2017.11.018 基于自适应搜索的免疫粒子群算法 Immune particle swarm optimization algorithm based on the adaptive search strategy 工程科学学报.2017,391：125 https:1doi.org10.13374.issn2095-9389.2017.01.016 矿区废弃地移动机器人全覆盖路径规划 Complete coverage path planning of mobile robot on abandoned mine land 工程科学学报.2020,42(9：1220 https:/doi.org10.13374.issn2095-9389.2019.09.09.004 基于半自主导航与运动想象的多旋翼飞行器二维空间目标搜索 Two-dimensional space target searching based on semi-autonomous navigation and motor imagery for multi-rotor aircraft 工程科学学报.2017,398：1261 https:/doi.org/10.13374.issn2095-9389.2017.08.017
基于双维度搜索的地下自主铲运机最优转弯轨迹规划 顾青 刘立 白国星 孟宇 Optimal turning trajectory planning of an LHD based on a bidimensional search GU Qing, LIU Li, BAI Guo-xing, MENG Yu 引用本文: 顾青, 刘立, 白国星, 孟宇. 基于双维度搜索的地下自主铲运机最优转弯轨迹规划[J]. 工程科学学报, 2021, 43(2): 289-298. doi: 10.13374/j.issn2095-9389.2020.11.09.002 GU Qing, LIU Li, BAI Guo-xing, MENG Yu. Optimal turning trajectory planning of an LHD based on a bidimensional search [J]. Chinese Journal of Engineering, 2021, 43(2): 289-298. doi: 10.13374/j.issn2095-9389.2020.11.09.002 在线阅读 View online: https://doi.org/10.13374/j.issn2095-9389.2020.11.09.002 您可能感兴趣的其他文章 Articles you may be interested in 高速公路绿篱修剪机器人手臂避障路径规划 Obstacle avoidance path planning for expressway hedgerow pruning robot manipulator 工程科学学报. 2019, 41(1): 134 https://doi.org/10.13374/j.issn2095-9389.2019.01.015 集总干扰下六旋翼飞行器的轨迹跟踪控制 Trajectory tracking control for an unmanned hexrotor with lumped disturbance 工程科学学报. 2018, 40(5): 622 https://doi.org/10.13374/j.issn2095-9389.2018.05.013 无人直升机自抗扰自适应轨迹跟踪混合控制 Trajectory-tracking hybrid controller based on ADRC and adaptive control for unmanned helicopters 工程科学学报. 2017, 39(11): 1743 https://doi.org/10.13374/j.issn2095-9389.2017.11.018 基于自适应搜索的免疫粒子群算法 Immune particle swarm optimization algorithm based on the adaptive search strategy 工程科学学报. 2017, 39(1): 125 https://doi.org/10.13374/j.issn2095-9389.2017.01.016 矿区废弃地移动机器人全覆盖路径规划 Complete coverage path planning of mobile robot on abandoned mine land 工程科学学报. 2020, 42(9): 1220 https://doi.org/10.13374/j.issn2095-9389.2019.09.09.004 基于半自主导航与运动想象的多旋翼飞行器二维空间目标搜索 Two-dimensional space target searching based on semi-autonomous navigation and motor imagery for multi-rotor aircraft 工程科学学报. 2017, 39(8): 1261 https://doi.org/10.13374/j.issn2095-9389.2017.08.017
工程科学学报.第43卷，第2期：289-298.2021年2月 Chinese Journal of Engineering,Vol.43,No.2:289-298,February 2021 https://doi.org/10.13374/j.issn2095-9389.2020.11.09.002;http://cje.ustb.edu.cn 基于双维度搜索的地下自主铲运机最优转弯轨迹规划 顾 青12，刘立”，白国星”，孟宇)四 1)北京科技大学机械工程学院，北京1000832)北京科技大学顺德研究生院.佛山528300 ☒通信作者，E-mail:myu@ustb.edu.cn 摘要提出了一种基于双维度搜索的实时轨迹规划方法，用来解决自主地下铲运机转弯轨迹规划问题.该方法是一种结合 采样思想和最优化算法的复合轨迹规划方法，包含三个主要步骤：基于双维度搜索策略的优化模型参数生成，基于二次规划 的轨迹计算，以及基于约束检查的最优轨迹确定.该方法新颖之处在于提出的基于转弯区域行驶时间和里程的双维度搜索 策略，以及基于平稳目标的轨迹最优化模型，可根据弯道区域入口速度和位置，快速生成纵横向都有最优性保证的最优轨迹 该方法结构简单、易于实施，可通过关键参数的调整满足控制器对轨迹生成速度的实时性要求.基于该轨迹规划方法的特 点，使其不仅适用于实时轨迹规划，还可为未来智慧矿山的智能管控与优化调度提供底层约束.多组算例验证了该方法的有 效性和优越性 关键词地下铲运机：自主行驶：轨迹规划：纵横向轨迹规划：搜索策略 分类号TP202.7 Optimal turning trajectory planning of an LHD based on a bidimensional search GU Qing2,LIU L),BAI Guo-xing,MENG Yu 1)School of Mechanical Engineering.University of Science and Technology Beijing.Beijing 100083,China 2)Shunde Graduate School,University of Science and Technology Beijing,Foshan 528300,China Corresponding author,E-mail:myu@ustb.edu.cn ABSTRACT To solve the problem of smooth turning of an autonomous underground load-haul-dump loader (LHD),in this paper,a method for turning trajectory planning of an LHD was proposed.This method is a type of hybrid trajectory planning method based on a bidimensional search.According to the characteristic of the problem,the longitudinal and lateral decomposition method was applied,and the basic algorithms are a sampling method and an optimization algorithm.The algorithm consists of three main steps that are parameter generation of the optimal model based on a bidimensional search strategy,trajectory calculation based on quadratic programming models,and determination of the optimal trajectory based on an articulated angle and collision avoidance constraints check.The novelty of this method lies in the proposed two-dimensional search strategy and trajectory optimization models.The two dimensions are the driving time and mileage of the trajectory in the turing area;the trajectory optimization model is based on the quadratic programming that can quickly generate the optimal trajectory in both dimensions according to the turning area entering speed and position of the LHD. This trajectory planning method is simple in structure and easy to implement.Moreover,it can satisfy the real-time requirement of the controller on the trajectory generation time by adjusting the key parameters.Based on the characteristics of the trajectory planning method,it is not only suitable for real-time trajectory planning but can also provide basic constraints for intelligent control and optimal scheduling of intelligent mines.A series of case studies was conducted to show the effectiveness and superiority of the proposed method. The case studies show that the optimal trajectories according to different entering speeds and positions can be obtained through the proposed method.A prototype experiment was performed to show the feasibility of the proposed trajectory planning method.This 收稿日期：2020-11-09 基金项目：国家重点研发计划资助项目(2018YFC0810500,2019YFC0605300):广东省基础与应用基础研究基金资助项目(2019A1515111015)
基于双维度搜索的地下自主铲运机最优转弯轨迹规划 顾 青1,2)，刘 立1)，白国星1)，孟 宇1) 苣 1) 北京科技大学机械工程学院，北京 100083 2) 北京科技大学顺德研究生院，佛山 528300 苣通信作者，E-mail: myu@ustb.edu.cn 摘 要 提出了一种基于双维度搜索的实时轨迹规划方法，用来解决自主地下铲运机转弯轨迹规划问题. 该方法是一种结合 采样思想和最优化算法的复合轨迹规划方法，包含三个主要步骤：基于双维度搜索策略的优化模型参数生成，基于二次规划 的轨迹计算，以及基于约束检查的最优轨迹确定. 该方法新颖之处在于提出的基于转弯区域行驶时间和里程的双维度搜索 策略，以及基于平稳目标的轨迹最优化模型，可根据弯道区域入口速度和位置，快速生成纵横向都有最优性保证的最优轨迹. 该方法结构简单、易于实施，可通过关键参数的调整满足控制器对轨迹生成速度的实时性要求. 基于该轨迹规划方法的特 点，使其不仅适用于实时轨迹规划，还可为未来智慧矿山的智能管控与优化调度提供底层约束. 多组算例验证了该方法的有 效性和优越性. 关键词 地下铲运机；自主行驶；轨迹规划；纵横向轨迹规划；搜索策略 分类号 TP202.7 Optimal turning trajectory planning of an LHD based on a bidimensional search GU Qing1,2) ，LIU Li1) ，BAI Guo-xing1) ，MENG Yu1) 苣 1) School of Mechanical Engineering, University of Science and Technology Beijing, Beijing 100083, China 2) Shunde Graduate School, University of Science and Technology Beijing, Foshan 528300, China 苣 Corresponding author, E-mail: myu@ustb.edu.cn ABSTRACT To solve the problem of smooth turning of an autonomous underground load-haul-dump loader (LHD), in this paper, a method for turning trajectory planning of an LHD was proposed. This method is a type of hybrid trajectory planning method based on a bidimensional search. According to the characteristic of the problem, the longitudinal and lateral decomposition method was applied, and the basic algorithms are a sampling method and an optimization algorithm. The algorithm consists of three main steps that are parameter generation of the optimal model based on a bidimensional search strategy, trajectory calculation based on quadratic programming models, and determination of the optimal trajectory based on an articulated angle and collision avoidance constraints check. The novelty of this method lies in the proposed two-dimensional search strategy and trajectory optimization models. The two dimensions are the driving time and mileage of the trajectory in the turning area; the trajectory optimization model is based on the quadratic programming that can quickly generate the optimal trajectory in both dimensions according to the turning area entering speed and position of the LHD. This trajectory planning method is simple in structure and easy to implement. Moreover, it can satisfy the real-time requirement of the controller on the trajectory generation time by adjusting the key parameters. Based on the characteristics of the trajectory planning method, it is not only suitable for real-time trajectory planning but can also provide basic constraints for intelligent control and optimal scheduling of intelligent mines. A series of case studies was conducted to show the effectiveness and superiority of the proposed method. The case studies show that the optimal trajectories according to different entering speeds and positions can be obtained through the proposed method. A prototype experiment was performed to show the feasibility of the proposed trajectory planning method. This 收稿日期: 2020−11−09 基金项目: 国家重点研发计划资助项目（2018YFC0810500, 2019YFC0605300）；广东省基础与应用基础研究基金资助项目（2019A1515111015） 工程科学学报，第 43 卷，第 2 期：289−298，2021 年 2 月 Chinese Journal of Engineering, Vol. 43, No. 2: 289−298, February 2021 https://doi.org/10.13374/j.issn2095-9389.2020.11.09.002; http://cje.ustb.edu.cn
290 工程科学学报，第43卷，第2期 method generates trajectories that are easy to track and control because the velocity,articulated angle,and angular velocity change gently. KEY WORDS load haul dump;autonomous driving;trajectory planning;vertical and horizontal trajectory planning;search strategy 地下铲运机是一种铰接式结构的地下无轨移 研究发现，当转弯角度过大时，现有的约束并不能 动采矿装备，具有转弯半径小，地形适应能力强的 约束铰接角及其角速度.究其原因，还是没有将铰 特点，广泛应用于矿产开采和运输.随着采矿行业 接角及其角速度作为决策变量加入到优化模型之 的发展，采矿深度不断增加，地下高温高湿环境对 中，无法对其直接约束.然而，若是直接将铰接角 驾驶员身体有极大伤害，因此，自主行驶成为新一 及其角速度作为决策变量，则根据其运动状态方 代智能移动采矿装备的必备功能. 程，轨迹优化模型一定是一个复杂的非线性优化 为了实现铲运机的自主行驶，轨迹规划与控 模型，能否得到最优解或可行解都没有保障 制方法被广泛的研究-.文献[]~[8]研究了地 为了解决上述问题，提高自主铲运机的弯道 下铲运机的导航控制方法，进而实现铲运机可以 行驶性能，根据铲运机及其行驶环境特点，本文提 在地下巷道内采用“沿壁法”安全避障行驶.文献[8)] 出了一种基于双维度搜索的地下铲运机最优转弯 在无约束小范围内，可在铲装点和卸载点之间生 轨迹规划方法.本文的贡献及新颖之处在于设计 成平滑行驶路径.文献[9提出了一种基于蚁群算 了一种基于行驶里程和时间的双维度搜索策略， 法的路径规划方法，在地下巷道环境内，可生成全 在轨迹计算阶段，建立了一种以平稳行驶为目标的 局最优路径.文献[10]提出一种自主导航控制框 纵横向解耦二次规划模型，其中，铰接角及铰接角 架：首先人工示教生成导航地图和驾驶提示信息， 速度约束并不在优化模型中直接考虑，而是通过对 然后在自主行驶时依照驾驶提示信息行驶.基于 生成轨迹进行约束条件检查来筛选得到满足约束的 这些研究，目前铲运机自主行驶技术可分为两类： 最优轨迹.与现有规划方法不同的是，使用本文提 一类是路径规划和跟踪，此类方法没有考虑到速 出的方法可以快速得到在纵向横向两个维度都具 度因素，因此对于行铲运机工作效率无法考虑，同 有最优性保证的轨迹，不需要后备策略，保障了导 时行驶速度较慢：另一类是先人工示教，然后自主 航控制系统中轨迹规划层的可靠性，提高了生成轨 行驶.此类方法受到驾驶员经验影响较大，一般在 迹的质量和速度，使铲运机在转弯时可以合理调整 直道中可以以较高的速度行驶，而接近转弯区域 行驶速度和位置，实现高效转弯，提高生产效率 时，为了避免与巷道壁碰撞，驾驶员通常会提前减 本文提出的方法具有以下特点：能够生成平 速，以很低的速度通过转弯区域，也就是说，通过 稳的转弯轨迹，模型简单易于实时求解，所得到的 转弯区域的行驶时间较长.因为铲运机的地下金 轨迹具有最优性保证，不需要后备策略 属开采工作主要是出矿，通常每个“铲一运一返” 文章结构如下，第一章介绍铲运机运动学模型、 工作循环都要转弯，所以，转弯区域通过时间长会 转弯区域模型及所研究的具体问题，第二章介绍轨 影响铲运机工作效率.总的来说，目前铲运机的自 迹规划方法框架，第三章介绍轨迹规划方法数学模 主行驶水平还处于初级阶段 型，第四章和第五章是算例分析与样机试验，验证本 对于采用跟踪目标要来完成自主行驶功能的 文提出的方法有效性和优越性，第五章总结结论. 控制框架来说，除了研究高精度的跟踪控制算 1对象、场景模型与问题描述 法-，进行高质量的轨迹规划也是提高自主行 驶水平的有效途径.目前针对乘用车轨迹规划法 1.1铲运机运动学模型 方法的研究很多，总体可以分为直接法，、基于 由于铲运机(Load haul dump,LHD)具有铰接 解耦的方法20-习，较好的解决了乘用车在边界曲 结构，对其运动姿态的描述与刚性车辆不同，在运 率平缓的公路上的轨迹规划问题.文献[31]研究 动规划时需要同时考虑其航向角和铰接角.LHD 了铲运机在地下巷道内典型循环工况的轨迹规划 的整体结构如图1所示，车体由两部分组成.PA 问题，可视为一种转弯轨迹规划问题.其中，纵向 是前后车体的交接点，P(,y)是前桥中心点， 轨迹规划是一个二次规划问题，易于求解.然而横 P(x,y)是后桥中心点，A和0，分别是前后车体的航 向轨迹还是一个非线性优化模型，因此求解时间 向角.P和P与PA之间的距离分别为Lr和Lr,定义 较长（而且有可能得不到最优解或可行解）：此外， 铰接角y为0r与0，之差
method generates trajectories that are easy to track and control because the velocity, articulated angle, and angular velocity change gently. KEY WORDS load haul dump；autonomous driving；trajectory planning；vertical and horizontal trajectory planning；search strategy 地下铲运机是一种铰接式结构的地下无轨移 动采矿装备，具有转弯半径小，地形适应能力强的 特点，广泛应用于矿产开采和运输. 随着采矿行业 的发展，采矿深度不断增加，地下高温高湿环境对 驾驶员身体有极大伤害，因此，自主行驶成为新一 代智能移动采矿装备的必备功能. 为了实现铲运机的自主行驶，轨迹规划与控 制方法被广泛的研究[1−11] . 文献 [1]～[8] 研究了地 下铲运机的导航控制方法，进而实现铲运机可以 在地下巷道内采用“沿壁法”安全避障行驶. 文献 [8] 在无约束小范围内，可在铲装点和卸载点之间生 成平滑行驶路径. 文献 [9] 提出了一种基于蚁群算 法的路径规划方法，在地下巷道环境内，可生成全 局最优路径. 文献 [10] 提出一种自主导航控制框 架：首先人工示教生成导航地图和驾驶提示信息， 然后在自主行驶时依照驾驶提示信息行驶. 基于 这些研究，目前铲运机自主行驶技术可分为两类： 一类是路径规划和跟踪，此类方法没有考虑到速 度因素，因此对于行铲运机工作效率无法考虑，同 时行驶速度较慢；另一类是先人工示教，然后自主 行驶. 此类方法受到驾驶员经验影响较大，一般在 直道中可以以较高的速度行驶，而接近转弯区域 时，为了避免与巷道壁碰撞，驾驶员通常会提前减 速，以很低的速度通过转弯区域，也就是说，通过 转弯区域的行驶时间较长. 因为铲运机的地下金 属开采工作主要是出矿，通常每个“铲—运—返” 工作循环都要转弯，所以，转弯区域通过时间长会 影响铲运机工作效率. 总的来说，目前铲运机的自 主行驶水平还处于初级阶段. 对于采用跟踪目标要来完成自主行驶功能的 控制框架来说，除了研究高精度的跟踪控制算 法[11−12] ，进行高质量的轨迹规划也是提高自主行 驶水平的有效途径. 目前针对乘用车轨迹规划法 方法的研究很多，总体可以分为直接法[13−19]、基于 解耦的方法[20−32] ，较好的解决了乘用车在边界曲 率平缓的公路上的轨迹规划问题. 文献 [31] 研究 了铲运机在地下巷道内典型循环工况的轨迹规划 问题，可视为一种转弯轨迹规划问题. 其中，纵向 轨迹规划是一个二次规划问题，易于求解. 然而横 向轨迹还是一个非线性优化模型，因此求解时间 较长（而且有可能得不到最优解或可行解）；此外， 研究发现，当转弯角度过大时，现有的约束并不能 约束铰接角及其角速度. 究其原因，还是没有将铰 接角及其角速度作为决策变量加入到优化模型之 中，无法对其直接约束. 然而，若是直接将铰接角 及其角速度作为决策变量，则根据其运动状态方 程，轨迹优化模型一定是一个复杂的非线性优化 模型，能否得到最优解或可行解都没有保障. 为了解决上述问题，提高自主铲运机的弯道 行驶性能，根据铲运机及其行驶环境特点，本文提 出了一种基于双维度搜索的地下铲运机最优转弯 轨迹规划方法. 本文的贡献及新颖之处在于设计 了一种基于行驶里程和时间的双维度搜索策略， 在轨迹计算阶段，建立了一种以平稳行驶为目标的 纵横向解耦二次规划模型，其中，铰接角及铰接角 速度约束并不在优化模型中直接考虑，而是通过对 生成轨迹进行约束条件检查来筛选得到满足约束的 最优轨迹. 与现有规划方法不同的是，使用本文提 出的方法可以快速得到在纵向横向两个维度都具 有最优性保证的轨迹，不需要后备策略，保障了导 航控制系统中轨迹规划层的可靠性，提高了生成轨 迹的质量和速度，使铲运机在转弯时可以合理调整 行驶速度和位置，实现高效转弯，提高生产效率. 本文提出的方法具有以下特点：能够生成平 稳的转弯轨迹，模型简单易于实时求解，所得到的 轨迹具有最优性保证，不需要后备策略. 文章结构如下，第一章介绍铲运机运动学模型、 转弯区域模型及所研究的具体问题，第二章介绍轨 迹规划方法框架，第三章介绍轨迹规划方法数学模 型，第四章和第五章是算例分析与样机试验，验证本 文提出的方法有效性和优越性，第五章总结结论. 1 对象、场景模型与问题描述 1.1 铲运机运动学模型 PA Pf(xf , yf) Pr(xr , yr) θf θr Pf Pr PA Lf Lr γ θf θr 由于铲运机（Load haul dump，LHD）具有铰接 结构，对其运动姿态的描述与刚性车辆不同，在运 动规划时需要同时考虑其航向角和铰接角. LHD 的整体结构如图 1 所示，车体由两部分组成. 是前后车体的交接点 ， 是前桥中心点 ， 是后桥中心点， 和 分别是前后车体的航 向角. 和 与 之间的距离分别为 和 . 定义 铰接角 为 与 之差 · 290 · 工程科学学报，第 43 卷，第 2 期
顾青等：基于双维度搜索的地下自主铲运机最优转弯轨迹规划 291· B相交，铲运机从巷道B向巷道A转弯.在路口建 立直角坐标系，O为转弯区域起点位置，位于巷道 A,弯道区域直角坐标系Y轴垂直于巷道B的边 P(ey 界：红色虚线为转弯区域结束的位置，垂直于巷道 A的边界.Lse是安全距离长度，黑色虚线之间的 蓝色阴影部分是除去安全边界的可行区域：α为两 巷道夹角：LA和LB分别为巷道A和B在转弯区域 的长度.(xout,yout)是弯道出口处左边界点位置坐 0 标.地下巷道转弯处都会根据铰接式采矿装备的 图1LHD结构 转弯半径进行有磨角，目的是为了使这类移动采 Fig.1 Structure of an LHD 矿装备能够安全通过路口.磨角后，转弯处可行区 y=-0 (1) 域增大，如图2(b)所示，和是磨角操作后巷 P和P之间的位置关系可以表示为 道A和巷道B的剩余长度 xr=xE-Lfcos0f-Lr cos0 (2) 1.3问题描述 当铲运机进入转弯区域时，根据铲运机当前 yr=yf-Lf sine-Lr sine (3) 速度、位置，实时生成一条平稳轨迹，使铲运机能 铰接车辆的运动学模型30可以描述为 够在安全转弯的情况下，平稳的行驶出转弯区域. cosr 0 f 在轨迹规划时，要满足下列约束条件： sinOf 0 siny L (1)前后车体都不与巷道壁发生碰撞： Lfcosy+L Lfcosy+L (2)铲运机速度不超过限速； 0 (3)铰接角和铰接角速度不超过设计约束值； (4) (4)在驶出转弯区域时，前车体方向与巷道壁 其中，为前车体速度，y为铰接角y的角速度.可 平行 知，前车体航向角的角速度为 乐-smy+L 2轨迹规划算法框架 (5) Licosy+L 2.1最优轨迹存在性分析 铰接角及其角速度的物理约束为 在矿山建设中，巷道的结构和尺寸与矿产种 Ymin≤y≤ymax (6) 类、分布、采矿工艺相关，也就是说，在建设之初， ymin≤y≤Ymax (7) 巷道尺寸和拟选用的采矿装备是相互匹配的.而 其中，Ymin为铰接角最小值，ymax为铰接角最大值，min 且，在地下巷道的路口处，都要根据采矿装备的转 为铰接角速度最小值，ma为铰接角速度最大值. 弯半径进行磨角，这是一种标准操作，目的就是为 1.2弯道区域模型 了能使铰接式的移动采矿装备可以安全无碰撞通 转弯区域如图2所示.图2(a)中，巷道A与 过路口.因此，对于给定铲运机以及符合其标准的 (a) (b) (Xou:Yo) Tunnel A Tunnel B Tunnel B 0 图2地下巷道转弯区域.(a)磨角之前的转弯区域：(b)磨角之后的转弯区域 Fig.2 Roadway tuning area:(a)before grinding;(b)after grinding
γ = θf −θr （1） Pf 和 Pr之间的位置关系可以表示为 xr = xf − Lf cos θf − Lr cos θr （2） yr = yf − Lf sinθf − Lrsinθr （3） 铰接车辆的运动学模型[30] 可以描述为 x˙f y˙f θ˙ f γ˙ = cos θf sinθf sinγ Lf cosγ+ Lr 0 0 0 Lr Lf cosγ+ Lr 1 [ vf γ˙ ] （4） 其中， vf 为前车体速度， γ˙为铰接角 γ 的角速度. 可 知，前车体航向角的角速度为 θ˙ f = vf sinγ+ Lrγ˙ Lf cosγ+ Lr （5） 铰接角及其角速度的物理约束为 γmin ⩽ γ ⩽ γmax （6） γ˙min ⩽ γ˙ ⩽ γ˙max （7） γmin γmax γ˙min γ˙max 其中， 为铰接角最小值， 为铰接角最大值， 为铰接角速度最小值， 为铰接角速度最大值. 1.2 弯道区域模型 转弯区域如图 2 所示. 图 2（a）中，巷道 A 与 O Lsafe α LA LB (xout, yout) L ′ A L ′ B B 相交，铲运机从巷道 B 向巷道 A 转弯. 在路口建 立直角坐标系， 为转弯区域起点位置，位于巷道 A，弯道区域直角坐标系 Y 轴垂直于巷道 B 的边 界；红色虚线为转弯区域结束的位置，垂直于巷道 A 的边界. 是安全距离长度，黑色虚线之间的 蓝色阴影部分是除去安全边界的可行区域； 为两 巷道夹角； 和 分别为巷道 A 和 B 在转弯区域 的长度. 是弯道出口处左边界点位置坐 标. 地下巷道转弯处都会根据铰接式采矿装备的 转弯半径进行有磨角，目的是为了使这类移动采 矿装备能够安全通过路口. 磨角后，转弯处可行区 域增大，如图 2（b）所示， 和 是磨角操作后巷 道 A 和巷道 B 的剩余长度. 1.3 问题描述 当铲运机进入转弯区域时，根据铲运机当前 速度、位置，实时生成一条平稳轨迹，使铲运机能 够在安全转弯的情况下，平稳的行驶出转弯区域. 在轨迹规划时，要满足下列约束条件： （1）前后车体都不与巷道壁发生碰撞； （2）铲运机速度不超过限速； （3）铰接角和铰接角速度不超过设计约束值； （4）在驶出转弯区域时，前车体方向与巷道壁 平行. 2 轨迹规划算法框架 2.1 最优轨迹存在性分析 在矿山建设中，巷道的结构和尺寸与矿产种 类、分布、采矿工艺相关. 也就是说，在建设之初， 巷道尺寸和拟选用的采矿装备是相互匹配的. 而 且，在地下巷道的路口处，都要根据采矿装备的转 弯半径进行磨角，这是一种标准操作，目的就是为 了能使铰接式的移动采矿装备可以安全无碰撞通 过路口. 因此，对于给定铲运机以及符合其标准的 Pr (xr , yr ) Pf (xf , yf ) θf γ θr Lr Lf O X Y 图 1 LHD 结构 Fig.1 Structure of an LHD O X Y Tunnel A LA LB Pf Lsafe Lsafe (a) α (xout, yout) Tunnel B (b) O X Y Tunnel A L′A L′B Pf α Lsafe (xout, yout) Tunnel B Lsafe 图 2 地下巷道转弯区域. （a）磨角之前的转弯区域；（b）磨角之后的转弯区域 Fig.2 Roadway tuning area: (a) before grinding; (b) after grinding 顾 青等： 基于双维度搜索的地下自主铲运机最优转弯轨迹规划 · 291 ·
292 工程科学学报，第43卷，第2期 路口，铲运机一定能够顺利通过 通过，则最优轨迹找到：否则，回到第一个步骤进 轨迹同时包含速度和位置信息，通常来说，可以 入下一次迭代 通过路口的轨迹不止一条.当铲运机采用不同的速 使用本算法，针对一个转弯路口，基于铲运机 度，沿不同曲率的路径曲线，都可以安全通过路口.例 进入转弯区域的位置和速度，本文提出的方法能 如：对于一条比较平缓的位置曲线，采用不同的速度 够快速的找到一条纵向和横向都具有最优性保证 都可以很好的沿此位置曲线行驶，但是每种速度对应 的最优轨迹，不需要后备策略.第三章介绍规划算 了不同的轨迹.因此，当对轨迹设置一个的评价指标 法的具体细节 后，则在可行轨迹之中，存在一个最优指标值最小的 3 轨迹规划算法 最优解，本文的目标正是寻找这一最优轨迹 2.2算法思想及框架 3.1基于双维度搜索的终端位置和行驶时间确定 本质上，弯道区域中每条可行轨迹可以通过 本算法中利用二次规划构建轨迹生成模型， 不同行驶里程和行驶时间表征.因此，为了能够根 设计基于双维度搜索的策略来确定优化问题的具 据入口速度和位置快速的生成最优轨迹，本文提 体条件.其中，外循环调整轨迹的行驶时间，内循 出一种基于双维度搜索策略的轨迹优化方法.此 环调整轨迹终端位置.在每次循环中，首先计算纵 方法核心思想是在弯道行驶区域内，通过调整轨 向轨迹，然后计算横向轨迹 迹的里程和时间，找到最优轨迹.本方法采用最优 记为外循环中的循环变量，为内循环中的循 化算法进行构建，总体过程是：根据里程和时间迭 环变量，每次循环中行驶时间T通过方程(8)设置， 代策略给出的终端位置和行驶时间，计算轨迹并 Tiy=(LA+LB)/(vm-(i-1)△） (8) 检查是否符合铲运机铰接角物理约束，最终得到 其中：ym是铲运机进人转弯区域的人口速度：△v为 最优轨迹.因此，算法主要包含三步： 速度递减间隔 (1)确定行驶里程和行驶时间 每次循环中轨迹终端位置(xendiYendi),通过如 此步骤的作用是确定当前轨迹规划问题的行 下方程设置， 驶里程与时间.设计了一种双维度搜索策略，每次 Xendij=xout+(j-1).Ad.cosa (9) 在轨迹计算前，首先通过搜索策略，从终端位置和 行驶时间两个维度进行调整，确定当前迭代的终 yendij=yout +(j-1).Ad.sina (10) 端位置和行驶时间. 其中：Ws为巷道B宽度；△d=(WB-2Lsae)/m为位置 (2)轨迹生成 递增间隔，即将巷道B的可行宽度进行等间隔离 在转弯区域直角坐标系下，将轨迹分解为纵向和 散后的离散长度，m为间隔离散个数 横向两个维度，分别采用二次规划生成最优轨迹（此 3.2基于二次规划的转弯轨迹计算 时并不考虑铰接角约束).本算法中，横纵向轨迹都建 本算法中轨迹计算是基于纵向横向解耦和二 模为二次规划问题，因此，所得到的轨迹在纵向和横 次规划完成的.首先生成纵向行驶轨迹，然后生成 向两个维度都具有最优性保证，而且求解速度快 横向行驶轨迹 (3)轨迹铰接角约束检查及最优轨迹确定 (1)纵向轨迹规划模型 对于生成的轨迹，根据运动学方程计算铰接 将转弯轨迹离散为N段，纵向轨迹规划建模为一 角和铰接角速度序列，若满足约束，且防碰撞检查 个标准的二次规划问题，如公式(11-1)到(11-9)所示 min Jo ∑△d2a+s2, (11-1) k=l k= △2 Xk+I=k+Vx△f+ 2a, k=0,…,N-1, (11-2) V+i=V+a4△1， k=0,…,N-1. (11-3) XN Xendij" (11-4) XN-1 XN, whena=π/2 (11-5) xmin一e1≤Xk≤xmax+El, k=1,…,W, (11-6) Vxmin-El≤Vx≤Vxmax+e1, k=1,…,N, (11-7) dxmin-S1≤aw≤axmx+8l, k=1,…,W, (11-8) △axmn-e1≤△a≤△dxmx+1, k=1,…,N (11-9)
路口，铲运机一定能够顺利通过. 轨迹同时包含速度和位置信息，通常来说，可以 通过路口的轨迹不止一条. 当铲运机采用不同的速 度，沿不同曲率的路径曲线，都可以安全通过路口. 例 如：对于一条比较平缓的位置曲线，采用不同的速度 都可以很好的沿此位置曲线行驶，但是每种速度对应 了不同的轨迹. 因此，当对轨迹设置一个的评价指标 后，则在可行轨迹之中，存在一个最优指标值最小的 最优解，本文的目标正是寻找这一最优轨迹. 2.2 算法思想及框架 本质上，弯道区域中每条可行轨迹可以通过 不同行驶里程和行驶时间表征. 因此，为了能够根 据入口速度和位置快速的生成最优轨迹，本文提 出一种基于双维度搜索策略的轨迹优化方法. 此 方法核心思想是在弯道行驶区域内，通过调整轨 迹的里程和时间，找到最优轨迹. 本方法采用最优 化算法进行构建，总体过程是：根据里程和时间迭 代策略给出的终端位置和行驶时间，计算轨迹并 检查是否符合铲运机铰接角物理约束，最终得到 最优轨迹. 因此，算法主要包含三步： （1）确定行驶里程和行驶时间. 此步骤的作用是确定当前轨迹规划问题的行 驶里程与时间. 设计了一种双维度搜索策略，每次 在轨迹计算前，首先通过搜索策略，从终端位置和 行驶时间两个维度进行调整，确定当前迭代的终 端位置和行驶时间. （2）轨迹生成. 在转弯区域直角坐标系下，将轨迹分解为纵向和 横向两个维度，分别采用二次规划生成最优轨迹（此 时并不考虑铰接角约束）. 本算法中，横纵向轨迹都建 模为二次规划问题，因此，所得到的轨迹在纵向和横 向两个维度都具有最优性保证，而且求解速度快. （3）轨迹铰接角约束检查及最优轨迹确定. 对于生成的轨迹，根据运动学方程计算铰接 角和铰接角速度序列，若满足约束，且防碰撞检查 通过，则最优轨迹找到；否则，回到第一个步骤进 入下一次迭代. 使用本算法，针对一个转弯路口，基于铲运机 进入转弯区域的位置和速度，本文提出的方法能 够快速的找到一条纵向和横向都具有最优性保证 的最优轨迹，不需要后备策略. 第三章介绍规划算 法的具体细节. 3 轨迹规划算法 3.1 基于双维度搜索的终端位置和行驶时间确定 本算法中利用二次规划构建轨迹生成模型， 设计基于双维度搜索的策略来确定优化问题的具 体条件. 其中，外循环调整轨迹的行驶时间，内循 环调整轨迹终端位置. 在每次循环中，首先计算纵 向轨迹，然后计算横向轨迹. i j Ti j 记为 外循环中的循环变量， 为内循环中的循 环变量，每次循环中行驶时间 通过方程（8）设置， Ti j = (LA + LB)/(vin −(i−1)·∆v) （8） 其中： vin 是铲运机进入转弯区域的入口速度； ∆v 为 速度递减间隔. (xendi j , yendi j 每次循环中轨迹终端位置 ) ，通过如 下方程设置， xendi j = xout +(j−1)·∆d · cosα （9） yendi j = yout +(j−1)·∆d ·sinα （10） WB ∆d = (WB −2Lsafe)/m m 其中： 为巷道 B 宽度； 为位置 递增间隔，即将巷道 B 的可行宽度进行等间隔离 散后的离散长度， 为间隔离散个数. 3.2 基于二次规划的转弯轨迹计算 本算法中轨迹计算是基于纵向横向解耦和二 次规划完成的. 首先生成纵向行驶轨迹，然后生成 横向行驶轨迹. （1）纵向轨迹规划模型. 将转弯轨迹离散为 N 段，纵向轨迹规划建模为一 个标准的二次规划问题，如公式（11-1）到（11-9）所示. min Jlo = ∑ N k=1 λ1a 2 xk + ∑ N k=1 λ2∆a 2 xk +ε1 2 , (11-1) xk+1 = xk +vxk∆t+ ∆t 2 2 axk , k = 0,······ ,N −1, (11-2) vxk+1 = vxk +axk∆t, k = 0,······ ,N −1, (11-3) xN = xendi j , (11-4) xN−1 = xN, whenα = π/2 (11-5) xmin −ε1 ⩽ xk ⩽ xmax+ε1, k = 1,······ ,N, (11-6) vxmin −ε1 ⩽ vxk ⩽ vxmax+ε1, k = 1,······ ,N, (11-7) axmin −ε1 ⩽ axk ⩽ axmax +ε1, k = 1,······ ,N, (11-8) ∆axmin −ε1 ⩽ ∆axk ⩽ ∆axmax+ε1, k = 1,······ ,N (11-9) · 292 · 工程科学学报，第 43 卷，第 2 期