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 期