顾青等：基于双维度搜索的地下自主铲运机最优转弯轨迹规划 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 methodJlo ∆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 ·