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 期