工程科学学报 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 期
顾青等：基于双维度搜索的地下自主铲运机最优转弯轨迹规划 293· 其中：o是纵向轨迹目标函数；△t=T;/N,T是第 在本算法中，在目标函数是最小化加速度及 i次外循环时铲运机行驶时间；k是轨迹点序号； 其变化量.没有将目标速度考虑在优化模型之中， d1和2为权重系数，1是松弛变量，x是铲运机前桥 这是因为在弯道区域，理想的目标速度很难设定，设 中心点纵向位置；yx是前桥中心点纵向速度，ax和 置不好会造成实际速度曲线发生突变，难以控制. △a,分别是前桥中心点纵向加速度和加速度变化 (2)横向轨迹规划模型 量，0=0，Vo=Va,am=aa:公式(1l-5)表示的约 横向轨迹规划也采用二次规划模型，与文献[30] 束是为了令轨迹终端姿态与巷道壁平行，"和 中方法不同的是，此时并不考虑铰接角相关约束， a分别是铲运机进入转弯区域的入口纵向速度和 因此可以将横向轨迹规划模型也建立为一个标准 加速度 二次规划模型，如公式(12-1)到(12-9)所示 min a P2Aa+号 (12-1) k=1 k= yk+1=收+z△t+ AR 2a4, k=0,…,N, (12-2) V+1=u+a△t, k=0,…,N (12-3) yN yendij (12-4) Vyw/VxN tana, when0≤a<π/2 (12-5) ymin-E2≤Jyk≤ymax+82, k=1,…,N, (12-6 yyan-e2≤Vyg≤'max+E2, k=1,…,NW, (12-7) aym血-E2≤az≤aymx+E2, k=1,…,N, (12-8) △dymin-82≤△az≤△ayms+E2, k=1,…,N (12-9) 其中：Ja是横向轨迹目标函数；p1和p2为权重系数， 行的轨迹，有 2是松弛变量，y是铲运机前桥中心点横向位置； ymin≤yi-max≤ymax (11) ,是前桥中心点横向速度，a,和△a,分别是前桥中 ymin≤yi-max≤ymax (12) 心点纵向加速度和加速度变化量，o=m,o=” 同时，对于最优轨迹来说，松弛变量应该为0，即 a%=am;公式(12-5)表示的约束是为了令轨迹终 E1=82=0 (13) 端姿态与巷道壁平行.和a分别是铲运机进入 此外，还需进行碰撞检查，本文采用的是最为 转弯区域的入口横向速度和加速度.与纵向轨迹 简单的膨胀法，此处不赘述 规划模型设计思想相同的是，目标函数是最小化 若公式(13)、(14)和(15)同成立，且铲运机与 加速度及其变化量 边界没有碰撞，则迭代停止，当前轨迹为最优轨 3.3基于约束检查的最优轨迹确定 迹；否则j=j+1,返回公式(9)和(10)进行内循环； 上面的模型并未考虑铰接角约束，为了保证 若j=m时都没有获得最优解，则i=i+1,返回公式 铰接角及其角速度符合约束，本算法设计了约束 (8)进行外循环.由于外循环相当于降低行驶速 检查环节.根据公式(4)可以计算得到此轨迹对应 度，所以在延长转弯行驶时间的情况下，一定能找 的铰接角和铰接角速度序列.记本条轨迹中最大 到符合约束的轨迹，此时迭代停止，当前轨迹为最 铰接角为y-max、最大铰接角速度为i-max,对于可 优轨迹.具体流程如图3所示. Setting iteration Calculating the conditions by optimal Formula Optimal Start trajectory by (13-(15) trajectory is End equation model ture? (8-(10) obtained (11)12) N +1 产计1 图3双维度搜索轨迹规划方法流程图 Fig.3 Flow chart for the two-dimensional search-based trajectory planning method
Jlo ∆t = Ti/N Ti k λ1 λ2 ε1 xi vxi axi ∆axi x0 = 0 vx0 = vxin ax0 = axin vxin axin 其中： 是纵向轨迹目标函数； ， 是第 i 次外循环时铲运机行驶时间； 是轨迹点序号； 和 为权重系数， 是松弛变量， 是铲运机前桥 中心点纵向位置； 是前桥中心点纵向速度， 和 分别是前桥中心点纵向加速度和加速度变化 量， ， ， ；公式（11-5）表示的约 束是为了令轨迹终端姿态与巷道壁平行. 和 分别是铲运机进入转弯区域的入口纵向速度和 加速度. 在本算法中，在目标函数是最小化加速度及 其变化量. 没有将目标速度考虑在优化模型之中， 这是因为在弯道区域，理想的目标速度很难设定，设 置不好会造成实际速度曲线发生突变，难以控制. （2）横向轨迹规划模型. 横向轨迹规划也采用二次规划模型，与文献 [30] 中方法不同的是，此时并不考虑铰接角相关约束， 因此可以将横向轨迹规划模型也建立为一个标准 二次规划模型，如公式（12-1）到（12-9）所示. min Jla = ∑ N k=1 ρ1a 2 yk + ∑ N k=1 ρ2∆a 2 yk +ε 2 2 , (12-1) yk+1 = yk +vyk∆t+ ∆t 2 2 ayk , k = 0,······ ,N, (12-2) vyk+1 = vyk +ayk∆t, k = 0,······ ,N, (12-3) yN = yendi j , (12-4) vyN /vxN = tanα, when 0 ⩽ α < π/2 (12-5) ymin −ε2 ⩽ yk ⩽ ymax +ε2, k = 1,······ ,N, (12-6) vymin −ε2 ⩽ vyk ⩽ vymax +ε2, k = 1,······ ,N, (12-7) aymin −ε2 ⩽ ayk ⩽ aymax +ε2, k = 1,······ ,N, (12-8) ∆aymin −ε2 ⩽ ∆ayk ⩽ ∆aymax +ε2, k = 1,······ ,N (12-9) Jla ρ1 ρ2 ε2 yi vyi ayi ∆ayi y0 = yin vy0 = vyin ay0 = ayin vyin ayin 其中： 是横向轨迹目标函数； 和 为权重系数， 是松弛变量， 是铲运机前桥中心点横向位置； 是前桥中心点横向速度， 和 分别是前桥中 心点纵向加速度和加速度变化量， ， ， ；公式（12-5）表示的约束是为了令轨迹终 端姿态与巷道壁平行. 和 分别是铲运机进入 转弯区域的入口横向速度和加速度. 与纵向轨迹 规划模型设计思想相同的是，目标函数是最小化 加速度及其变化量. 3.3 基于约束检查的最优轨迹确定 γi−max γ˙i−max 上面的模型并未考虑铰接角约束，为了保证 铰接角及其角速度符合约束，本算法设计了约束 检查环节. 根据公式（4）可以计算得到此轨迹对应 的铰接角和铰接角速度序列. 记本条轨迹中最大 铰接角为 、最大铰接角速度为 ，对于可 行的轨迹，有 γmin ⩽ γi−max ⩽ γmax （11） γ˙min ⩽ γ˙i−max ⩽ γ˙max （12） 同时，对于最优轨迹来说，松弛变量应该为 0，即 ε1 = ε2 = 0 （13） 此外，还需进行碰撞检查，本文采用的是最为 简单的膨胀法，此处不赘述. j = j+1 j = m i = i+1 若公式（13）、（14）和（15）同成立，且铲运机与 边界没有碰撞，则迭代停止，当前轨迹为最优轨 迹；否则 ，返回公式（9）和（10）进行内循环； 若 时都没有获得最优解，则 ，返回公式 （8）进行外循环. 由于外循环相当于降低行驶速 度，所以在延长转弯行驶时间的情况下，一定能找 到符合约束的轨迹，此时迭代停止，当前轨迹为最 优轨迹. 具体流程如图 3 所示. Start End i=1 j=1 j=j+1 j=m? N N Y Y i=i+1 Setting iteration conditions by equation (8)−(10) Calculating the optimal trajectory by model (11)−(12) Formula (13)−(15) ture? Optimal trajectory is obtained 图 3 双维度搜索轨迹规划方法流程图 Fig.3 Flow chart for the two-dimensional search-based trajectory planning method 顾 青等： 基于双维度搜索的地下自主铲运机最优转弯轨迹规划 · 293 ·
294 工程科学学报，第43卷，第2期 4算例分析与讨论 转弯区域.将入口位置分为三个等级，由于铲运机 在地下平直巷道工作时限速一般为15kmh,也 为了验证本文提出的轨迹规划方法，设计了 就是4ms左右，因此将入口速度分为四个等级，入 三组算例.设置了一种难度较高的转弯场景，转弯 口方向都与巷道B保持平行.然后，针对每个入口 角度为直角，且铲运机从宽巷道向窄巷道转弯行 速度，求解其最优轨迹，求解采用Matlab quadprog 驶，描述了一种铲运机从运输巷道向开采巷道内 函数，并将得到最优轨迹时内循环次数i、外循环 转弯的场景.算例参数如表1所示.三组算例分别 的次数j、最优轨迹对应的行驶时间Tbst,以及此 设计为：以相同的入口位置，不同的入口速度进入 条轨迹中的最大铰接角ymax和最大角速度max列出 表1算例参数表 Table 1 Parameters for case studies WA/m WB/m LA/m Lg/m a/rad Lsate/m △v/(ms) △d/(ms) 5 4.5 30 30 π/2 1.5 0.1 0.5 4 Li/m L/m Ymin/rad Ymax/rad Ymin/(rad.s) Ymin/(rad-s-) LA血 m 15 2 -0.69 0.69 -0.17 0.17 24 24 33 (1)第一组算例，入口位置为(0,1.5)，不同入口 从上面的算例可以看出，针对不同的入口速度和 速度的最优轨迹计算结果列表如表2所示 位置，采用所提出的规划方法都可以得到最优轨迹 表2第一组算例结果 在相同的位置，入口速度越高，迭代次数越 Table 2 Results of the first group 多.根据本算例中的问题规模，单次轨迹生成时间 Vin/(m's-) 为0.005s左右，若是迭代上百次，则有可能超过控 Thest/s ymax/rad ymax/(rad-s) 制周期（例如，当入口速度为4ms时，迭代次数 1 2 3 66.67 0.51 0.06 2 4 42.84 0.51 0.06 为17×3×2=102次，则得到轨迹的时间超过了0.5s), 3 13 4 33.33 0.47 0.06 这就会影响轨迹跟跟踪性能.因此，可以通过调整 4 17 4 28.54 0.42 0.06 速度减小间隔△v这一迭代关键参数的方法来减少 迭代次数.例如，当△v=0.2ms时，迭代次数减 (2)第二组算例，入口位置为(0,2.5)，不同入口 少一半.因此，本方法可以根据控制器性能，调整 速度的最优轨迹计算结果列表如表3所示 间隔，满足实际控制需求 为了更直观显示轨迹，以入口坐标为(0,2.5)， 表3第二组算例结果 Table 3 Results of the second group 入口速度为2ms1的情况为例，图4~6给出了最 优轨迹的位置曲线，速度曲线、铰接角、航向角和 vin/(m's-) Tbest/s Ymax/rad Ymax/(rad-s) 相应的角速度曲线.图6中，y、v,和va,分别是铰 1 3 66.67 0.51 0.06 1 接角和前后车体的航向角速度.可以看出，沿行驶 2 4 42.84 0.53 0.06 方向速度、铰接角和铰接角速度变化平缓，适合跟 3 13 4 33.33 0.5 0.07 1 踪控制 4 28.54 0.45 0.08 35 Trajectory of the front body (3)第二组算例，入口位置为(0,3.5)，不同入口 30 --Trajectory of the rear body 速度的最优轨迹计算结果列表如表4所示 25 Upper bound Lower bound 20 表4第三组算例结果 Table 4 Results of the third group 10 m/(s) i Ymax/rad ymax/(rad-s) 23 66.67 0.53 0.06 2 7 4 42.84 0.56 0.07 510152025 30 35 Longitudinal distance/m 3 13 4 33.33 0.53 0.08 17 4 28.54 0.48 0.09 图4位置曲线(n=2.5,V=2) Fig.4 Position trajectory (yin =2.5.vx =2)
4 算例分析与讨论 为了验证本文提出的轨迹规划方法，设计了 三组算例. 设置了一种难度较高的转弯场景，转弯 角度为直角，且铲运机从宽巷道向窄巷道转弯行 驶，描述了一种铲运机从运输巷道向开采巷道内 转弯的场景. 算例参数如表 1 所示. 三组算例分别 设计为：以相同的入口位置，不同的入口速度进入 i j Tbest γmax γ˙max 转弯区域. 将入口位置分为三个等级，由于铲运机 在地下平直巷道工作时限速一般为 15 km·h−1，也 就是 4 m·s−1 左右，因此将入口速度分为四个等级，入 口方向都与巷道 B 保持平行. 然后，针对每个入口 速度，求解其最优轨迹，求解采用 Matlab quadprog 函数，并将得到最优轨迹时内循环次数 、外循环 的次数 、最优轨迹对应的行驶时间 ，以及此 条轨迹中的最大铰接角 和最大角速度 列出. 表 1 算例参数表 Table 1 Parameters for case studies WA/m WB/m LA/m LB/m α/rad Lsafe /m ∆v/（m·s−1） ∆d /（m·s−1） m 5 4.5 30 30 π/2 1.5 0.1 0.5 4 Lf /m Lr /m γmin/rad γmax/rad γ˙min/（rad·s−1） γ˙min/（rad·s−1） L ′ A /m L ′ B /m N 1.5 2 −0.69 0.69 −0.17 0.17 24 24 33 （1）第一组算例，入口位置为（0,1.5）, 不同入口 速度的最优轨迹计算结果列表如表 2 所示. 表 2 第一组算例结果 Table 2 Results of the first group vin /（m·s−1） i j Tbest /s γmax/rad γ˙max/（rad·s−1） 1 2 3 66.67 0.51 0.06 2 7 4 42.84 0.51 0.06 3 13 4 33.33 0.47 0.06 4 17 4 28.54 0.42 0.06 （2）第二组算例，入口位置为（0,2.5）, 不同入口 速度的最优轨迹计算结果列表如表 3 所示. 表 3 第二组算例结果 Table 3 Results of the second group vin /（m·s−1） i j Tbest /s γmax/rad γ˙max/（rad·s−1） 1 2 3 66.67 0.51 0.06 2 7 4 42.84 0.53 0.06 3 13 4 33.33 0.5 0.07 4 17 4 28.54 0.45 0.08 （3）第二组算例，入口位置为（0,3.5）, 不同入口 速度的最优轨迹计算结果列表如表 4 所示. 表 4 第三组算例结果 Table 4 Results of the third group vin /（m·s−1） i j Tbest /s γmax/rad γ˙max/（rad·s−1） 1 2 3 66.67 0.53 0.06 2 7 4 42.84 0.56 0.07 3 13 4 33.33 0.53 0.08 4 17 4 28.54 0.48 0.09 从上面的算例可以看出，针对不同的入口速度和 位置，采用所提出的规划方法都可以得到最优轨迹. ∆v ∆v = 0.2 在相同的位置，入口速度越高，迭代次数越 多. 根据本算例中的问题规模，单次轨迹生成时间 为 0.005 s 左右，若是迭代上百次，则有可能超过控 制周期（例如，当入口速度为 4 m·s−1 时，迭代次数 为 17×3×2=102 次，则得到轨迹的时间超过了 0.5 s）， 这就会影响轨迹跟跟踪性能. 因此，可以通过调整 速度减小间隔 这一迭代关键参数的方法来减少 迭代次数. 例如，当 m·s−1 时，迭代次数减 少一半. 因此，本方法可以根据控制器性能，调整 间隔，满足实际控制需求. vγ vθf vθr 为了更直观显示轨迹，以入口坐标为（0, 2.5）， 入口速度为 2 m·s−1 的情况为例，图 4～6 给出了最 优轨迹的位置曲线，速度曲线、铰接角、航向角和 相应的角速度曲线. 图 6 中， 、 和 分别是铰 接角和前后车体的航向角速度. 可以看出，沿行驶 方向速度、铰接角和铰接角速度变化平缓，适合跟 踪控制. Longitudinal distance/m −5 0 5 10 15 20 25 30 35 Lateral distance/m 0 5 10 15 20 25 30 35 Trajectory of the front body Trajectory of the rear body Upper bound Lower bound 图 4 位置曲线（ yin = 2.5, vxin = 2 ） Fig.4 Position trajectory ( yin = 2.5, vxin = 2 ) · 294 · 工程科学学报，第 43 卷，第 2 期
顾青等：基于双维度搜索的地下自主铲运机最优转弯轨迹规划 295· 35 Trajectory of the front body 吃 ---Trajectory of the rear body 25 Upper bound Lower bound 0 10 2030405060 Distance/m 0 510152025 30 35 5 10152025303540 Longitudinal distance/m Time/s 图7位置曲线(T:=70s) 图5行驶方向速度曲线(ym=2.5,vn=2) Fig.7 Position trajectory (Ti=70s) Fig.5 Heading velocity trajectory (yin=2.5.v=2) 0 510152025 30354045 Time/s 1020304050 0.1 0.1 Distance/m 0 51015202530354045 Time/s /PA 2 0 510152025303540 45 0 10 20 3040 506070 Time/s Time/s 0.2E -0.2E 图8行驶方向速度曲线(T:=70s) 0 5101520253035 40 45 Fig.8 Heading velocity trajectory (Ti=70s) Time/s 是05日 -0.5上 0 510152025303540 45 0 10 2030405060 70 Time/s Time/s 0.2F 0.1日 -0.2E -0.1E 0 1015202530354045 102030405060 Time/s Time/s 图6铰接角、前后车体航向角及角速度(m=2.5,'m=2) pei/o Fig.6 Angle and angular velocity for y,r and r(yin=2.5.=2) 0 102030405060 0 Time/s 通过本文提出的方法可知，Tbe可视为以当前 (s-pery 0.2E 初始状态及平稳目标下铲运机通过弯道的最短时 -0.2 0 10 20 30405060 70 间.因此，当找到最优轨迹之后，若将行驶时间继 Time/s 续延长，也就是继续外循环，也能找到满足新的行 驶时间的解.同理，对于一个固定的终端位置，只 04 0 20 3040 5060 70 采用外循环延长时间，也可得到满足要求的最优 Time/s 轨迹，下面分别举例 0.2 -0.2E 对于入口位置为(0,2.5)，入口速度为2ms1 0 10 20 3040 5060 70 Time/s 的情况，得到最优轨迹时，=7，广4，Tb=42.84s,轨 图9铰接角、前后车体航向角及角速度(T:=70s) 迹在出口的终端位置为(33,35).此时，若保持此 Fig.9 Angle and angular velocity for y,0r,and (T;=70s) 终端位置，将行驶时间设置为70S,通过本文提出 的方法得到最优转弯轨迹如图7~9所示 位于巷道中心线，也就是出口位置为(32.25,35)，采用 还是以入口位置为(0,2.5)，入口速度为2ms1 本文提出的方法，只进行外循环，得到的最优轨迹 的情况为例，当要求铲运机行驶出转弯区域时，位置 如图10~12所示.此时=9，弯道行驶时间为46.15s
通过本文提出的方法可知， Tbest 可视为以当前 初始状态及平稳目标下铲运机通过弯道的最短时 间. 因此，当找到最优轨迹之后，若将行驶时间继 续延长，也就是继续外循环，也能找到满足新的行 驶时间的解. 同理，对于一个固定的终端位置，只 采用外循环延长时间，也可得到满足要求的最优 轨迹，下面分别举例. 对于入口位置为（0, 2.5），入口速度为 2 m·s−1 的情况，得到最优轨迹时，i=7，j=4，Tbest=42.84 s，轨 迹在出口的终端位置为（33, 35）. 此时，若保持此 终端位置，将行驶时间设置为 70 s，通过本文提出 的方法得到最优转弯轨迹如图 7～9 所示. 还是以入口位置为（0, 2.5），入口速度为 2 m·s−1 的情况为例，当要求铲运机行驶出转弯区域时，位置 位于巷道中心线，也就是出口位置为（32.25, 35），采用 本文提出的方法，只进行外循环，得到的最优轨迹 如图 10～12 所示. 此时 i=9，弯道行驶时间为 46.15 s. Distance/m 0 10 20 30 40 50 60 Velocity/(m·s−1 ) Velocity/(m·s−1 ) 0 1 2 3 Time/s 0 5 10 15 20 25 30 35 40 0 1 2 3 图 5 行驶方向速度曲线（ yin = 2.5, vxin = 2 ） Fig.5 Heading velocity trajectory ( yin = 2.5, vxin = 2 ) Time/s 0 5 10 15 20 25 30 35 40 45 γ/rad vγ/(rad·s−1 ) θf/rad vθ /(rad·s f −1 ) θr/rad vθ /(rad·s r −1 ) −0.5 0.5 −0.1 0.1 Time/s 0 5 10 15 20 25 30 35 40 45 Time/s 0 5 10 15 20 25 30 35 40 45 Time/s 0 5 10 15 20 25 30 35 40 45 Time/s 0 5 10 15 20 25 30 35 40 45 Time/s 0 5 10 15 20 25 30 35 40 45 0 1 −0.2 0.2 0 1 −0.2 0.2 图 6 铰接角、前后车体航向角及角速度（ yin = 2.5, vxin = 2 ） Fig.6 Angle and angular velocity for γ, θf and θr( yin = 2.5, vxin = 2 ) Longitudinal distance/m −5 0 5 10 15 20 25 30 35 Lateral distance/m 0 5 10 15 20 25 30 35 Trajectory of the front body Trajectory of the rear body Upper bound Lower bound 图 7 位置曲线（ Ti = 70 s ） Fig.7 Position trajectory ( Ti = 70 s ) Distance/m 0 10 20 30 40 50 10 20 30 40 70 50 60 Velocity/(m·s−1 ) Velocity/(m·s−1 ) 0 1 2 3 Time/s 0 0 1 2 3 图 8 行驶方向速度曲线（ Ti = 70 s ） Fig.8 Heading velocity trajectory ( Ti = 70 s ) Time/s 0 γ/rad vγ/(rad·s−1 ) θf/rad vθ /(rad·s f −1 ) θr/rad vθ /(rad·s r −1 ) −0.5 0.5 −0.1 0.1 Time/s 0 Time/s 0 Time/s 0 Time/s 0 Time/s 0 0 1 −0.2 0.2 0 1 −0.2 0.2 10 20 30 40 50 60 70 10 20 30 40 50 60 70 10 20 30 40 50 60 70 10 20 30 40 50 60 70 10 20 30 40 50 60 70 10 20 30 40 50 60 70 图 9 铰接角、前后车体航向角及角速度（ Ti = 70 s ） Fig.9 Angle and angular velocity for γ, θf , and θr( Ti = 70 s ) 顾 青等： 基于双维度搜索的地下自主铲运机最优转弯轨迹规划 · 295 ·
296 工程科学学报，第43卷，第2期 Trajectory of the front body 机如图13所示.采用激光雷达和电子地图进行定 30 ---Trajectory of the rear body 位.工控机为研华ARK-3500,Intel Core is5-3610ME, 25 Upper bound Lower bound 2.7GHz.试验环境为楼道内的一个直角转弯处， 楼道宽度为2.2m.由于是楼道试验，为了保证安 全，最高速度设置为1ms.试验参数设置如表5 所示.样机从转弯区域外开始启动并加速到1ms, 保持此速度进入转弯区域，车辆控制周期为0.2s -5 0 5 1015202530 35 要求行驶出转弯区域时车辆位于巷道中线，基于 Longitudinal distance/m 图10位置曲线（出口位置为(32.25,35）) 本文提出的轨迹规划方法，得到最优解时，=1，因 Fig.10 Position trajectory (exit position is(32.25,35)) 此轨迹生成时间在一个控制周期之内.所生成的 轨迹时间为7.2s,速度和角度曲线如图14所示 路径采用LTV-MPC进行控制，速度采用PID控 制，样机也平稳安全的通过了直角弯道，图15显 示了速度和航向角误差，其中，E4为前车体航向角 0 0 10 20 30.4050 60 Distance/m 跟踪误差，E为铲运机速度跟踪误差.可以看出， 误差都保持在一个很小的水平 051015202530354045 Time/s 图11行驶方向速度曲线（出口位置为(32.25,35）) Fig.11 Heading velocity trajectory (exit position is(32.25,35)) 0 51015202530354045 Time/s 图13铰接车试验平台 -0.1 Fig.13 Articulated vehicle 51015202530354045 lme/s 6 结论 01 0 5101520253035 40 45 (1)本文提出的方法可以生成纵向和横向都 Time/s 具有最优性保证的轨迹，理论上保证了生成轨迹 0.2E 的最优性 -0.2E 0 510 1520253035 40 45 (2)本文提出的方法优化得到的轨迹，其纵向 Time/s 速度、铰接角及角速度变化平缓，易于跟踪控制 (3)本文提出的算法可根据底层控制器的控 0 5 1015202530354045 Time/s 制周期灵活调整迭代关键参数，进而调整轨迹生 0.2 成时间，满足控制器对目标轨迹生成时间的要求 -0.2E (4)本文提出的算法适用于宽窄不同的、角度 0 5 1015 2025 30354045 Time/s 不同的转弯路口，不需要复杂的边界处理与坐标 图12较接角、前后车体航向角及角速度（出口位置为(32.25,35）) 转换，因此便于实际部署实施 Fig.12 Angle and angular velocity for y.r,and (exit position is (5)本文提出的方法既可以根据入口速度和位 (32.25,35)） 置生成安全平稳的最优转弯参考轨迹；也可用于根 5样机试验 据具体条件确定弯道最短行驶时间，因此，不仅适用 于实时轨迹规划，也可为多装备协同运行，甚至未 为了表明算法的可行性，进行了样机试验.样 来矿山的智能管控和调度等问题提供系统约束
5 样机试验 为了表明算法的可行性，进行了样机试验. 样 Eθf Ev 机如图 13 所示. 采用激光雷达和电子地图进行定 位. 工控机为研华 ARK-3500，Intel Core i5-3610ME， 2.7 GHz. 试验环境为楼道内的一个直角转弯处， 楼道宽度为 2.2 m. 由于是楼道试验，为了保证安 全，最高速度设置为 1 m·s−1 . 试验参数设置如表 5 所示. 样机从转弯区域外开始启动并加速到 1 m·s−1 ， 保持此速度进入转弯区域，车辆控制周期为 0.2 s. 要求行驶出转弯区域时车辆位于巷道中线，基于 本文提出的轨迹规划方法，得到最优解时，i=1，因 此轨迹生成时间在一个控制周期之内. 所生成的 轨迹时间为 7.2 s，速度和角度曲线如图 14 所示. 路径采用 LTV-MPC 进行控制 ，速度采用 PID 控 制，样机也平稳安全的通过了直角弯道，图 15 显 示了速度和航向角误差，其中， 为前车体航向角 跟踪误差， 为铲运机速度跟踪误差. 可以看出， 误差都保持在一个很小的水平. 图 13 铰接车试验平台 Fig.13 Articulated vehicle 6 结论 （1）本文提出的方法可以生成纵向和横向都 具有最优性保证的轨迹，理论上保证了生成轨迹 的最优性. （2）本文提出的方法优化得到的轨迹，其纵向 速度、铰接角及角速度变化平缓，易于跟踪控制. （3）本文提出的算法可根据底层控制器的控 制周期灵活调整迭代关键参数，进而调整轨迹生 成时间，满足控制器对目标轨迹生成时间的要求. （4）本文提出的算法适用于宽窄不同的、角度 不同的转弯路口，不需要复杂的边界处理与坐标 转换，因此便于实际部署实施. （5）本文提出的方法既可以根据入口速度和位 置生成安全平稳的最优转弯参考轨迹；也可用于根 据具体条件确定弯道最短行驶时间，因此，不仅适用 于实时轨迹规划，也可为多装备协同运行，甚至未 来矿山的智能管控和调度等问题提供系统约束. Longitudinal distance/m −5 0 5 10 15 20 25 30 35 Lateral distance/m 0 5 10 15 20 25 30 35 Trajectory of the front body Trajectory of the rear body Upper bound Lower bound 图 10 位置曲线（出口位置为（32.25, 35）） Fig.10 Position trajectory (exit position is (32.25, 35)) Distance/m 0 10 20 30 40 50 Velocity/(m·s−1 ) Velocity/(m·s−1 ) 0 1 2 3 Time/s 0 0 1 2 3 60 5 10 15 20 25 30 35 40 45 图 11 行驶方向速度曲线（出口位置为（32.25, 35）） Fig.11 Heading velocity trajectory (exit position is (32.25, 35)) Time/s 0 γ/rad vγ/(rad·s−1 ) θf/rad vθ /(rad·s f −1 ) θr/rad vθ /(rad·s r −1 ) −0.5 0.5 −0.1 0.1 Time/s 0 Time/s 0 Time/s 0 Time/s Time/s 0 1 −0.2 0.2 0 1 −0.2 0.2 5 10 15 20 25 30 35 40 45 5 10 15 20 25 30 35 40 45 5 10 15 20 25 30 35 40 45 5 10 15 20 25 30 35 40 45 0 0 5 10 15 20 25 30 35 40 45 5 10 15 20 25 30 35 40 45 图 12 铰接角、前后车体航向角及角速度（出口位置为（32.25, 35）） Fig.12 γ θf θr Angle and angular velocity for , , and (exit position is (32.25, 35)) · 296 · 工程科学学报，第 43 卷，第 2 期
顾青等：基于双维度搜索的地下自主铲运机最优转弯轨迹规划 297· 表5试验参数表 Table 5 Parameters for experiments WA/m Wg/m LA/m LB/m a/rad Lsafe/m △v/(ms) △d/(ms) 2.2 2.2 3.6 3.6 2 0.3 0.1 0.8 3 Li/m L/m Ymin/rad Ymax/rad ymin/(rad.s) imin/(rad-s) LA/m 名m 0.6 0.6 0.69 0.69 -0.17 0.17 3.6 3.6 33 分201 (石峰，顾洪枢，战凯，等.地下铲运机自主行驶与避障控制方法 研究.有色金属（矿山部分），2015,67(5)：68) 1.0 [6] Yang C,Chen S X,Liu L,et al.Reactive navigation for undergro- 0.5 0 und autonomous scraper.JChina Coal Soc,2011,36(11):1943 2 3 4 56 (杨超，陈树新，刘立，等.反应式导航在地下自主行驶铲运机中 Time/s 的应用.煤炭学报，2011,36(11)：1943) 1.5 [1 Long ZZ,Zhan K,Gu H S,et al.The control method based on 31.0 improved fuzzy-PID algorithm for the autonomous driving of 0.5 intelligent LHD.Nonferrous Met (Mine Sect),2015,67(5):76 0 1 2 3 (龙智卓，战凯，顾洪枢，等.基于改进模糊PD算法的智能铲运 5 6 Time/s 机自主行驶控制方法.有色金属（矿山部分），2015,67(5)：76) 因14参考速度和航向角曲线 [8] Andersson U,Mrozek K.Hyyppa K.et al.Path design and control Fig.14 Reference trajectory for velocity and heading algorithms for articulated mobile robots /Field and Service Robotics.London,1998:390 0.05 [9] Long ZZ,Zhan K,Gu H S,et al.Global path planning of 是0S intelligent load-haul-dump based on improved ant colony algorithm.Nonferrous Met (Mine Sect),2013,65(2):6 0 12 34 (龙智卓，战凯，顾洪枢，等.基于改进蚊群算法的智能铲运机全 Time/s 局路径规划.有色金属（矿山部分），2013,65(2)：6) 0.05 [10]Shi F,Gu H S,Zhan K,et al.The basic method study on the 0 location-navigation and control strategy for the independent LHD unit.Nonferrous Met (Mine Sect),2009,61(2):65 -0.05 0 1 34567 (石峰，顾洪枢，战凯，等.自主铲运机的定位导航和控制策略基 Time/s 本思路.有色金属（矿山部分，2009,61(2）：65) 图15速度和航向角跟踪误差 [11]Jiang C,Wang H W,Li J K,et al.Trajectory-tracking hybrid Fig.15 Tracking Errors trajectory for velocity and heading controller based on ADRC and adaptive control for unmanned 参考文献 helicopters.Chin J Eng,2017,39(11)：片1743 (姜辰，王浩文，李健珂，等.无人直升机白抗扰自适应轨迹跟踪 [1]Makela H,Lehtinen H,Rintanen K,et al.Navigation system for 混合控制.工程科学学报，2017,39(11)：1743) LHD machines.IFA4C Proc Vol,1995,28(11):295 [12]Invernizzi D,Lovera M,Zaccarian L Dynamic attitude planning [2]Roberts J M,Duff E S,Corke P I.Reactive navigation and for trajectory tracking in thrust-vectoring UAVs.IEEE Trans opportunistic localization for autonomous underground mining Autom Control,2020,65(1):453 vehicles.Inf Sci,2002,145(1-2):127 [13]Ziegler J.Bender P,Dang T,et al.Trajectory planning for Bert [3]Dragt B J,Camisani-Calzolari F R,Craig I K.An overview of the ha-A local,continuous method /2014 IEEE Intelligent Vehicles automation of load-haul-dump vehicles in an underground mining Symposium Proceedings.Dearborn,2014:450 environment.IFA4C Proc Vol,2005,38(1):37 [14]Ziegler J,Bender P,Schreiber M,et al.Making bertha drive-an [4]Larsson J,Broxvall M,Saffiotti A.A navigation system for autonomous journey on a historic route.IEEE Intell Transp Syst automated loaders in underground mines /Proceedings of the 5th Mag,2014,6(2:8 International Conference on Field and Service Robotics (FSR- [15]Liu CL,Lin C Y,Tomizuka M.The convex feasible set algorithm 2005).Port Douglas,2005:1 for real time optimization in motion planning.SIAMJControl [5]Shi F,Gu H S,Zhan K,et al.Study on the control method of Optim,.2018,56(4):2712 underground loader autonomous driving and obstacle avoidance. [16]Liu C L,Lin C Y,Wang Y Z,et al.Convex feasible set algorithm Nonferrous Met (Mine Sect),2015,67(5):68 for constrained trajectory smoothing /2017 American Control
参 考 文 献 Mäkelä H, Lehtinen H, Rintanen K, et al. Navigation system for LHD machines. IFAC Proc Vol, 1995, 28（11）: 295 [1] Roberts J M, Duff E S, Corke P I. Reactive navigation and opportunistic localization for autonomous underground mining vehicles. Inf Sci, 2002, 145（1-2）: 127 [2] Dragt B J, Camisani-Calzolari F R, Craig I K. An overview of the automation of load-haul-dump vehicles in an underground mining environment. IFAC Proc Vol, 2005, 38（1）: 37 [3] Larsson J, Broxvall M, Saffiotti A. A navigation system for automated loaders in underground mines // Proceedings of the 5th International Conference on Field and Service Robotics (FSR- 2005). Port Douglas, 2005: 1 [4] Shi F, Gu H S, Zhan K, et al. Study on the control method of underground loader autonomous driving and obstacle avoidance. Nonferrous Met (Mine Sect), 2015, 67（5）: 68 [5] （石峰, 顾洪枢, 战凯, 等. 地下铲运机自主行驶与避障控制方法 研究. 有色金属(矿山部分), 2015, 67（5）：68） Yang C, Chen S X, Liu L, et al. Reactive navigation for underground autonomous scraper. J China Coal Soc, 2011, 36（11）: 1943 （杨超, 陈树新, 刘立, 等. 反应式导航在地下自主行驶铲运机中 的应用. 煤炭学报, 2011, 36（11）：1943） [6] Long Z Z, Zhan K, Gu H S, et al. The control method based on improved fuzzy-PID algorithm for the autonomous driving of intelligent LHD. Nonferrous Met (Mine Sect), 2015, 67（5）: 76 （龙智卓, 战凯, 顾洪枢, 等. 基于改进模糊PID算法的智能铲运 机自主行驶控制方法. 有色金属(矿山部分), 2015, 67（5）：76） [7] Andersson U, Mrozek K, Hyyppä K, et al. Path design and control algorithms for articulated mobile robots // Field and Service Robotics. London, 1998: 390 [8] Long Z Z, Zhan K, Gu H S, et al. Global path planning of intelligent load-haul-dump based on improved ant colony algorithm. Nonferrous Met (Mine Sect), 2013, 65（2）: 6 （龙智卓, 战凯, 顾洪枢, 等. 基于改进蚁群算法的智能铲运机全 局路径规划. 有色金属(矿山部分), 2013, 65（2）：6） [9] Shi F, Gu H S, Zhan K, et al. The basic method study on the location-navigation and control strategy for the independent LHD unit. Nonferrous Met (Mine Sect), 2009, 61（2）: 65 （石峰, 顾洪枢, 战凯, 等. 自主铲运机的定位导航和控制策略基 本思路. 有色金属(矿山部分), 2009, 61（2）：65） [10] Jiang C, Wang H W, Li J K, et al. Trajectory-tracking hybrid controller based on ADRC and adaptive control for unmanned helicopters. Chin J Eng, 2017, 39（11）: 1743 （姜辰, 王浩文, 李健珂, 等. 无人直升机自抗扰自适应轨迹跟踪 混合控制. 工程科学学报, 2017, 39（11）：1743） [11] Invernizzi D, Lovera M, Zaccarian L. Dynamic attitude planning for trajectory tracking in thrust-vectoring UAVs. IEEE Trans Autom Control, 2020, 65（1）: 453 [12] Ziegler J, Bender P, Dang T, et al. Trajectory planning for Bertha — A local, continuous method // 2014 IEEE Intelligent Vehicles Symposium Proceedings. Dearborn, 2014: 450 [13] Ziegler J, Bender P, Schreiber M, et al. Making bertha drive—an autonomous journey on a historic route. IEEE Intell Transp Syst Mag, 2014, 6（2）: 8 [14] Liu C L, Lin C Y, Tomizuka M. The convex feasible set algorithm for real time optimization in motion planning. SIAM J Control Optim, 2018, 56（4）: 2712 [15] Liu C L, Lin C Y, Wang Y Z, et al. Convex feasible set algorithm for constrained trajectory smoothing // 2017 American Control [16] 表 5 试验参数表 Table 5 Parameters for experiments WA/m WB/m LA/m LB/m α/rad Lsafe /m ∆v/（m·s−1） ∆d /（m·s−1） m 2.2 2.2 3.6 3.6 π/2 0.3 0.1 0.8 3 Lf /m Lr /m γmin/rad γmax/rad γ˙min/（rad·s−1） γ˙min/（rad·s−1） L ′ A /m L ′ B /m N 0.6 0.6 −0.69 0.69 −0.17 0.17 3.6 3.6 33 Time/s 0 0 0 0 1 2 3 4 5 6 7 Time/s 1 2 3 4 5 6 7 Velocity/(m·s−1 ) 0.5 1.0 1.5 2.0 θf/rad 0.5 1.0 1.5 图 14 参考速度和航向角曲线 Fig.14 Reference trajectory for velocity and heading Time/s 0 0 1 2 3 4 5 6 7 Time/s 1 2 3 4 5 6 7 Ev/(m·s−1 ) 0.05 Eθ /rad f −0.05 0 −0.05 0 0.05 图 15 速度和航向角跟踪误差 Fig.15 Tracking Errors trajectory for velocity and heading 顾 青等： 基于双维度搜索的地下自主铲运机最优转弯轨迹规划 · 297 ·