欢迎访问《控制理论与应用》期刊网站！

基于双动态规划和MPC算法的商用车路径规划

doi: 10.7641/CTA.2024.40048

闫立冰，冯健洧，熊嘉伟，申宗，冯海浩，吕宪勇

潍柴动力股份有限公司, 山东潍坊 261031

详细信息

作者简介

闫立冰硕士,高级工程师,目前研究方向为商用车电控系统策略开发,E-mail: yanlb@weichai.com;

冯健洧硕士,工程师,目前研究方向为商用车电控系统策略开发,E-mail: fengjianwei@weichai.com;

熊嘉伟硕士,工程师,目前研究方向为商用车电控系统策略开发,E-mail: xiongjiawei@weichai.com;

申宗本科,工程师,目前研究方向为商用车电控系统策略开发,E-mail: shenzong@weichai.com;

冯海浩硕士,高级工程师,目前研究方向为商用车电控系统策略开发,E-mail: fenghh@weichai.com;

吕宪勇硕士,高级工程师,目前研究方向为商用车电控系统策略开发,E-mail: lvxiany@weichai.com.

通信作者

冯健洧，E-mail: 925087094@qq.com;Tel.:+8613051921007.

Commercial vehicle route planning based on dual dynamic programming and MPC algorithm

YAN Li-bing ， FENG Jian-wei ， XIONG Jia-wei ， SHEN Zong ， FENG Hai-hao ， LÜ Xian-yong

Weichai Power Co., Ltd., Weifang Shandong 261031 , China

摘要

随着“碳达峰、碳中和”世纪目标的提出, 以商用车、乘用车为代表的能源消耗行业开始进行新一轮的技术革命, 随着自动驾驶、智能网联技术的发展, 融合道路信息的智能驾驶技术正在飞速发展. 通过合理地规划车辆速度, 减少车辆不必要的制动与换挡, 减少交通信号灯处的等待时间, 可以提高车辆的燃油经济性. 本文旨在开发具有高效性、经济性等优势的商用车路径规划算法, 提出了一种双动态规划+模型预测控制(MPC)的控制架构, 结合前方道路上的红绿灯、限速、坡度等信息对车速、车辆位置进行规划, 将车辆通行时间以及燃油消耗拆分求解, 使其耦合性减少, 保证车辆能够在快速通过前方道路的基础上, 进一步提高燃油经济性. 同时考虑到车辆跟踪规划路径时存在偏差, 通过MPC对车辆的实际位置进行闭环, 在考虑加速度限值的情况下, 使车辆完成对规划路径的跟踪, 最终实现车辆的节能降耗. 本文通过Simulink+GT-SUITE平台联合仿真验证了所提出的控制策略应用于商用车路径规划的优越性, 与单层动态规划的方案相比, 本方案可以省油2.13%.

关键词

路径规划 / 动态规划 / 模型预测控制 / 燃油经济性

Abstract

With the proposal of the century goal of “carbon peak and carbon neutrality”, the energy consumption industry represented by commercial vehicles and passenger vehicles has begun a new round of technological revolution. With the development of autonomous driving and intelligent network technology, intelligent driving technology that integrates road information is developing rapidly. By reasonably planning vehicle speed, reducing unnecessary braking and shifting of vehicles, and reducing waiting time at traffic lights, the fuel economy of vehicles can be improved. This article aims to develop a commercial vehicle route planning algorithm with advantages such as high efficiency and economy, and proposes a control architecture based on two-layer dynamic programming + model predictive control (MPC). It combines information such as traffic lights, speed limits, and slopes on the road ahead to plan the vehicle speed and position, and splits the vehicle travel time and fuel consumption to reduce coupling, ensuring that the vehicle can further improve fuel economy while quickly passing through the road ahead. At the same time, considering the deviation in the planned path of vehicle tracking, the actual position of the vehicle is closed-looped through MPC, and the acceleration limit is taken into account to enable the vehicle to complete the tracking of the planned path, ultimately achieving energy conservation and consumption reduction for the vehicle. This article verifies the superiority of the proposed control strategy applied to commercial vehicle path planning through joint simulation using the Simulink+GT-SUITE platform. Compared with the single-layer dynamic programming scheme, this scheme can save 2.13% fuel.

Keywords

path planning / dynamic programming / model predictive control / fuel economy

1 引言 2 基于双动态规划和MPC算法的商用车路径规划研究 2.1 第1层动态规划 2.2 车辆规划路径的边界求解 2.3 第2层动态规划 2.4 车辆位置闭环 3 基于Simulink+GT-SUITE联合仿真 3.1 车辆路径规划 3.2 车辆位置闭环 4 结论

1 引言

随着“碳达峰、碳中和”世纪目标 ^[1] 的提出，商用车作为全国燃油消耗总量中占比较高的行业引起了社会上广泛地关注，人们对于商用车驾驶的经济性需求也在逐步提高，自动驾驶在满足安全性的基础上，又迎来了新的时代命题–—节能降碳. 随着智能网联技术的不断发展，V2X技术已经逐渐迈向成熟，融合智能网联信息、有效提高商用车驾驶的经济性是当前研究的一大热点 ^[2] . 其中典型的需求场景包括在市区、市郊路段中进行车速、路径规划，结合道路限速信息、交通信号灯信息，减少不必要的停车; 结合道路坡度信息，有效地分配车速，在上坡前提前加速，下坡前提前减速，减少不必要的制动与换挡，提高燃油经济性 ^[3] .

近年来，有众多学者围绕着车辆的节能车速规划问题进行了广泛地研究. 冷江昊等 ^[4] 设计了智能车辆多信号灯路口快速节能车速规划算法，通过建立车辆能耗模型和信号灯相位时刻数学模型，构建出基于巡航车速单位距离能耗的有向无环图，并使用动态规划算法求解最优绿灯通行周期，将最优绿灯通行周期作为约束条件，建立最优控制问题并转化为非线性优化问题，使用内点法进行优化求解. 韦麟等 ^[5] 设计了三阶段智能车辆多信号灯路口节能车速规划算法，为提高智能车辆在多信号灯场景通行时的能耗经济性，提出一种鲁棒三阶段的节能车速规划框架. 采用随机撒点的方式划分可通行区域，基于经济巡航车速构建加权有向无环图，利用Dijkstra算法求解加权有向无环图的粗略解; 利用三次样条插值模型平滑粗略解的轨迹，利用平滑后的轨迹作为内点法优化的初始迭代值，加快最优轨迹求解的速度. Nguyen等 ^[6] 以CO₂排放量和通行时间为代价函数，设计一种新型绿波车速引导（green light optimal speed advisory，GLOSA）系统，利用路测单元（road side units，RSUs）通信获得道路上交通信号灯的信息，从而提高自主汽车在行驶时间和等待时间方面的性能. Huang等 ^[7] 使用序列二次规划优化交叉路口的车速轨迹，以最大限度地减少油耗和行驶时间，并研究了车辆在转弯过程中的油耗和行驶时间. Sun等 ^[8] 考虑有效红灯时间，采用数据驱动的方法，根据经验抽样数据来应对交通信号灯时间不确定的问题，并采用动态规划算法来生成最佳车速参考轨迹，减少车辆燃油消耗.

上述论文虽然在车速规划中考虑了交通信号灯以及限速信息，但没有考虑道路坡度对车辆能耗的影响. 因此，本文针对商用车市区道路的车速规划问题，利用双动态规划算法，结合道路上的红绿灯、限速、坡度等信息，对车速、车辆位置进行规划，保证车辆能够在快速通过前方道路的同时，尽可能地提高燃油经济性.由于车辆跟踪规划路径时存在偏差，通过模型预测控制 ^[9] 算法对车辆的实际位置进行闭环，在考虑加速度限值的情况下，使得车辆完成对规划路径的跟踪，最终实现车辆的节能降耗. 本文通过仿真验证了所提出的控制策略用于实现商用车巡航节能目标的优越性.

2 基于双动态规划和MPC算法的商用车路径规划研究

车辆路径规划问题是一个经典的最优化问题，解决最优化问题有很多算法，比如模型预测控制（model predictive control，MPC）、动态规划、庞特里亚金等等 ^[10] . 动态规划作为一种非线性规划算法，具有全局最优性，目前已广泛应用于最优化问题的求解中，其求解结果往往作为其他算法求解精度的验证基准. 对于车辆路径规划问题，由于红绿灯信息具有时间、空间上的非连续特性，虽然其建模比较简单，但是不容易与求解连续空间的最优化算法结合，而动态规划最重要的是可以将求解问题从连续空间转化为离散空间，在离散空间下红绿灯信息的处理比较容易 ^[11] . 因此本文选择使用动态规划算法对车辆的速度、位置进行规划，结合前方道路上的红绿灯、限速、坡度等信息，保证车辆能够在快速通过前方道路的基础上，达到一定的燃油经济性. 由于道路含有限速、红绿灯等信息，所以实际需要求解的是带约束的最优化问题 ^[12] . 虽然动态规划算法对于约束的处理比较灵活，但求解过程中容易被约束条件影响，导致无解的计算结果. 本文为了提高动态规划算法的鲁棒性，对动态规划的求解方法进行了一定的改进: 采用双动态规划的算法，利用第1层动态规划对求解空间进行预处理，使得第2层动态规划可以更好地继承第1层动态规划的结果，提高算法鲁棒性、获得可行解的能力. 下面对双动态规划的原理进行介绍.

2.1 第1层动态规划

第1层动态规划主要是在充分考虑红绿灯通行时间窗口、道路最高限速、加速度限值的条件下，计算出最快到达终点的规划路径. 分别将前方距离和设定车速划分为离散空间. 首先是对前方距离进行离散，设车辆起始点位置是0，前方每隔一段距离设置一个位置状态，如果这段距离中存在限速区间的起始、终止位置，或者红绿灯，则在对应位置增加一个位置状态，位置空间一直从起点划分到终点. 接着对车速空间进行离散，车速离散空间从0 km/h开始，一直划分到对应位置的最高限速. 状态空间示意如图1所示.

对于动态规划，离散化程度对全局优化的求解效果起着至关重要的作用，需要根据实际问题进行仿真测试，确保离散化程度不会对计算结果造成影响. 完成求解空间的离散化之后，在考虑加速度限值、道路最高限速、红绿灯通行时间窗口的条件下，以车辆从起点到终点的通行时间为代价函数，进行第1层动态规划求解. 获得在满足上述条件下，最快到达终点的规划路径与规划车速.

图1动态规划状态点示意图

Fig.1Schematic diagram of dynamic programming solution

2.2 车辆规划路径的边界求解

规划路径的边界求解主要是计算最快通过终点时对应的通过各个红绿灯的时间窗口. 上一节提出的第 1层动态规划求解出在满足各项道路约束条件下最快到达终点所需要的时间，并同时获得通过每个红绿灯的时刻. 以通过最后一个红绿灯时对应的时间窗口为基础，反向计算出通过每个红绿灯所应当满足的时间窗口. 图2为求解过程的示意图.

图2规划车速边界求解示意图

Fig.2Schematic diagram for solving the boundary of planned vehicle speed

以3个红绿灯为例，假设道路上有3个红绿灯，横轴为时间轴，纵轴为距离（位置），图中红绿灯处的红线代表此时刻当前红绿灯为红灯或黄灯，不可通行，绿色的点为绿灯时间，可以通行.

图中蓝线为第1层动态规划求解出的计算结果. 从图中可以看出，最快通过红绿灯3的时间点为a，则最慢通过当前红绿灯窗口的时间点为b. 从时间b开始，结合道路限速信息，倒推出车辆通过红绿灯2所对应的最慢时间b2. 其含义为，如果想要b时刻通过红绿灯 3，那么最晚需要在b2时刻通过红绿灯2. 以此类推通过红绿灯1的最晚时间b1，如果b1时刻是红灯时间，那么需要把通行时刻提前到本次红灯前最后一秒的绿灯时刻b1’. 以此类推便可以得到通过每个红绿灯最晚的时刻b_i. 由于第1层动态规划求解的就是最短时间通过终点的方案，因此第1层动态规划的求解结果即为通过每个红绿灯最早的时刻. 通过上述过程，便可以依次推导出通过每个红绿灯的时间窗口[a_i，b_i].

2.3 第2层动态规划

为了获得最终的规划路径，使用第2层动态规划算法进行最优路径的计算. 以前方位置、通行时间作为第2层动态规划的状态量，其中前方位置的离散与第1 层动态规划保持一致，然后对前方每个位置的通行时间进行离散化. 首先将红绿灯位置处的通行时间进行离散，由第2步计算得到的每个红绿灯的通行时间作为对应位置处通行时间的上下限，以固定的时间间隔进行离散化，得到每个红绿灯位置处的通行时间状态量. 红绿灯之间位置的通行时间状态量以相邻两个红绿灯通行时间的最大值与最小值为边界，按相同的时间间隔进行离散. 另外，考虑到车辆实际行驶与规划会有偏差，所以对红绿灯的通行时间、道路最高限速等都需要考虑余量. 具体表现在将红绿灯的通行时间窗口收紧，每个位置处离散得到的最高车速要低于区间限速的最大值，由于车辆减速需要一定的过程，所以如果下一区间的限定车速低于当前区间的限定车速，那么需要将当前区间的限速位置进行收紧，让车辆提前进行减速，防止车辆在两个区间过渡时由于减速不及时造成的超速问题. 图3为状态空间的示意图.

图3第2层动态规划状态示意图

Fig.3Schematic diagram of the second layer dynamic programming state

在此离散空间上进行第2层动态规划，此时代价函数需要考虑的因素与第1层动态规划差异较大，第2层动态规划需要同时考虑通行时间和油耗. 油耗的计算需要建立整车动力学模型，通过车轮端扭矩经过后桥速比和变速箱，折算出对应的发动机扭矩; 同时根据车速折算出发动机转速，通过转速和扭矩计算发动机喷油量，最终得到发动机这段行驶距离下所需要的油耗. 整车动力学模型如下:

m \dot{v} = F_{wheel} - F_{wind} - F_{roll} - F_{G},

(1a)

F_{w i n d} = \frac{1}{2} \cdot ρ \cdot C_{d} \cdot A \cdot v^{2}

(1b)

F_{roll} = μ \cdot m \cdot g \cdot c o s θ

(1c)

F_{G} = m \cdot g \cdot s i n θ,

(1d)

F_{wheel} = \frac{T_{wheel}}{R} = \frac{p t \cdot r}{R} T_{E n g},

(1e)

其中: m为整车质量，v是车辆加速度，F_wheel为车轮力，F_wind是风阻，F_roll是地面滚阻，F_G是重力沿坡度方向的分力，ρ是空气密度，C_d是风阻系数，A是迎风面积，v是车辆速度，µ是滚阻系数，g是重力加速度，θ是坡度，T_wheel是车轮端扭矩，R是轮胎半径，pt为传动比，r为后桥比，T_Eng为发动机扭矩. 通过采集数据，对模型中的C_d和µ进行辨识，其他参数由整车车辆信息获得.

为了提高整车动力学模型的精度，减小模型的偏差，引入扩张状态观测器. 基于上述辨识的整车动力学模型，搭建扩张状态观测器，确保扩张状态观测器中的模型参数与整车动力学模型的参数一致. 并对扩张状态观测器的观测带宽进行标定，确保扩张状态观测器估计的车速与实际车速接近，从而使得估计的扰动准确. 将上述整车动力学模型整理如下:

\begin{matrix} m \dot{v} = \frac{p t \cdot r}{R} T_{E n g} - \frac{1}{2} ρ C_{d} A v^{2} - \\ μ m g c o s θ - m g s i n θ, \end{matrix}

(2)

整理得

\begin{matrix} \dot{v} = \frac{p t \cdot r}{m R} T_{E n g} - \frac{ρ C_{d} A v^{2}}{2 m} - \\ μ g c o s θ - g s i n θ . \end{matrix}

(3)

整理成扩张状态观测器的一般形式

\dot{x} = b u + f_{0} + f

(4)

其中: x 代表状态，这里是车速; b 为输入增益，b =

\frac{p t \cdot r}{m R}; u

为系统输入，

u = T_{E n g}; f_{0} = - \frac{ρ C_{d} A v^{2}}{2 m} - μ g c o s θ - g s i n θ

为模型的已知信息; f为未知扰动，包括模型未建模或者参数不准确引起的扰动，以及外部环境造成的扰动. 通过构建扩张状态观测器，实现对未知扰动f的估计.

2.4 车辆位置闭环

由于实际场景下，车辆不可能完全按照规划的车速行驶，其实际车速与设定车速之间存在偏差，进而导致车辆的实际位置与规划路径出现偏差，为了保证车辆能够尽可能地在规划时间到达规划路径，需要考虑对车辆的位置进行闭环控制，由于车辆行驶过程中对加速度有限值，而且车辆前方的规划路径已经由双动态规划算法进行了求解，因此适合使用模型预测控制进行车辆位置的闭环.

模型预测控制是一种最优化控制算法 ^[13]，通过实时求解最小代价函数的方法来获得未来一段时间内的控制输入. 模型预测控制可以解决多输入多输出、耦合、约束等问题，首先在过程控制中发挥了重要的作用，近年来MPC在其他几个领域迅速扩张，例如汽车、航空航天、智能电网和金融工程等 ^[14] . 模型预测控制主要包含了3大部分: 模型、预测及反馈校正，三者的关系可以简单理解为通过建立能够用于描述被控对象状态的机理模型或是基于数据驱动的模型，来预测未来一段时间内被控对象可能出现的状态，并据此做出决策并输出当前时刻的控制动作，从而实现所需优化问题的求解.

上述建立了整车动力学模型，由于风阻与车速的平方成正比，导致风阻与车速的关系为非线性关系，因此在构建状态方程式，将风阻这一项进行了简单的线性化，建立车辆位置、车速的状态方程如下:

[\begin{matrix} \dot{x} \\ \dot{v} \end{matrix}] = [\begin{matrix} 0 & 1 \\ 0 & - \frac{ρ C_{d} A v}{2 m} \end{matrix}] [\begin{matrix} x \\ v \end{matrix}] + [\begin{matrix} 0 \\ \frac{p t \cdot r}{m R} \end{matrix}] T_{E n g} + [\begin{matrix} 0 \\ - μ g c o s θ - g s i n θ + f \end{matrix}] .

(5)

将车辆的位置和实际速度记为状态量X，则车辆的状态方程可以写为

\dot{X} = A X + B u + F,

(6)

其中: A矩阵为

[\begin{matrix} 0 & 1 \\ 0 & - \frac{ρ C_{d} A v}{2 m} \end{matrix}] ，

B矩阵为

[\begin{matrix} 0 \\ \frac{p t \cdot r}{m R} \end{matrix}] ，

输入u为T_Eng， F =

[\begin{matrix} 0 \\ - μ g c o s θ - g s i n θ + f \end{matrix}] .

本文采用增量式MPC对车辆的位置进行控制. 首先将连续状态方程进行离散化，得到下式:

X (k + 1) = (A T + I) X (k) + B T u (k) + T F (k),

(7)

整理形式为

X (k + 1) = A_{T} \cdot X (k) + B_{T} \cdot u (k) + T F (k),

(8)

其中: A_T =（AT + I），代表离散后的状态方程的转移矩阵; 同理，B_T为离散后的输入矩阵.

取相邻两个步长的离散状态方程进行做差，得到增量式的状态方程为

Δ X (k + 1) = A_{T} \cdot Δ X (k) + B_{T} \cdot Δ u (k) + T Δ F (k),

(9)

其中:

Δ X (k + 1) = X (k + 1) - x (k),

(10a)

Δ X (k) = X (k) - X (k - 1),

(10b)

Δ u (k) = u (k) - u (k - 1),

(10c)

Δ F (k) = F (k) - F (k - 1) .

(10d)

因此可以预测未来p步的输出为

Δ X (k + 1) = A_{T} \cdot Δ X (k) + B_{T} \cdot Δ u (k) + T Δ F (k),

(11a)

\begin{matrix} Δ X (k + 2) = A_{T}^{2} \cdot Δ X (k) + A_{T} B_{T} \cdot Δ u (k) + \\ B_{T} \cdot Δ u (k + 1) + A_{T} \cdot T Δ F (k), \end{matrix}

(11b)

\begin{matrix} Δ X (k + p) = A_{T}^{p} \cdot Δ X (k) + \\ A_{T}^{p - 1} \cdot B_{T} \cdot Δ u (k) + \\ A_{T}^{p - 2} \cdot B_{T} \cdot Δ u (k + 1) + \dots + \\ B_{T} \cdot Δ u (k + p - 1) + \\ A_{T}^{p - 1} \cdot T Δ F (k) \end{matrix}

(11c)

整理成紧凑的矩阵形式即为

[\begin{matrix} Δ X (k + 1) \\ Δ X (k + 2) \\ Δ X (k + 3) \\ ⋮ \\ Δ X (k + p) \end{matrix}] =

[\begin{matrix} A_{T} \\ A_{T}^{2} \\ A_{T}^{3} \\ ⋮ \\ A_{T}^{p} \end{matrix}] Δ X (k) + [\begin{matrix} B_{T} & 0 & \dots & 0 \\ A_{T} B_{T} & B_{T} & \dots & 0 \\ A_{T}^{2} B_{T} & A_{T} B_{T} & \dots & 0 \\ ⋮ & ⋮ & ⋮ \\ A_{T}^{p - 1} B_{T} & A_{T}^{p - 2} B_{T} & \dots & B_{T} \end{matrix}] \times

[\begin{matrix} Δ u (k) \\ Δ u (k + 1) \\ Δ u (k + 2) \\ ⋮ \\ Δ u (k + p - 1) \end{matrix}] + [\begin{matrix} I \\ A_{T} \\ A_{T}^{2} \\ ⋮ \\ A_{T}^{p - 1} \end{matrix}] \cdot T Δ F (k) .

(12)

由于

[\begin{matrix} X (k + 1) \\ X (k + 2) \\ X (k + 3) \\ ⋮ \\ X (k + p) \end{matrix}] = [\begin{matrix} I & 0 & \dots & 0 \\ I & I & \dots & 0 \\ I & I & \dots & 0 \\ ⋮ & ⋮ & ⋮ \\ I & I & \dots & I \end{matrix}] [\begin{matrix} Δ X (k + 1) \\ Δ X (k + 2) \\ Δ X (k + 3) \\ ⋮ \\ Δ X (k + p) \end{matrix}] + [\begin{matrix} I \\ I \\ I \\ ⋮ \\ I \end{matrix}] X (k),

(13)

所以

[\begin{matrix} X (k + 1) \\ X (k + 2) \\ X (k + 3) \\ ⋮ \\ X (k + p) \end{matrix}] = [\begin{matrix} I & 0 & \dots & 0 \\ I & I & \dots & 0 \\ I & I & \dots & 0 \\ ⋮ & ⋮ & ⋮ \\ I & I & \dots & I \end{matrix}] [\begin{matrix} A_{T} \\ A_{T}^{2} \\ A_{T}^{3} \\ ⋮ \\ A_{T}^{p} \end{matrix}] \cdot Δ X (k) +

[\begin{matrix} I & 0 & \dots & 0 \\ I & I & \dots & 0 \\ I & I & \dots & 0 \\ ⋮ & ⋮ & ⋮ \\ I & I & \dots & I \end{matrix}] [\begin{matrix} B_{T} & 0 & \dots & 0 \\ A_{T} B_{T} & B_{T} & \dots & 0 \\ A_{T}^{2} B_{T} & A_{T} B_{T} & \dots & 0 \\ ⋮ & ⋮ & ⋮ \\ A_{T}^{p - 1} B_{T} & A_{T}^{p - 2} B_{T} & \dots & B_{T} \end{matrix}] \times

[\begin{matrix} Δ u (k) \\ Δ u (k + 1) \\ Δ u (k + 2) \\ ⋮ \\ Δ u (k + p - 1) \end{matrix}] + [\begin{matrix} I & 0 & \dots & 0 \\ I & I & \dots & 0 \\ I & I & \dots & 0 \\ ⋮ & ⋮ & ⋮ \\ I & I & \dots & I \end{matrix}] [\begin{matrix} I \\ A_{T}^{2} \\ A_{T}^{2} \\ ⋮ \\ A_{T}^{p - 1} \end{matrix}] \times T Δ F (k) + [\begin{matrix} I \\ I \\ I \\ ⋮ \\ I \end{matrix}] X (k)

(14)

整理可得

\begin{matrix} \vec{X} (k + 1) = S_{x} \cdot Δ X (k) + S_{u} \cdot Δ \vec{U} (k) + \\ S_{F} \cdot T Δ F (k) + \vec{I X} (k), \end{matrix}

(15)

其中:

S_{x} = [\begin{matrix} \sum_{i = 1}^{1} A_{T}^{i} \\ \sum_{i = 1}^{2} A_{T}^{i} \\ \sum_{i = 1}^{3} A_{T}^{i} \\ \sum_{i = 1}^{p} A_{T}^{i} \end{matrix}],

S_{u} = [\begin{matrix} \sum_{i = 1}^{1} A_{T}^{i - 1} B_{T} & 0 & \dots & 0 \\ \sum_{i = 1}^{2} A_{T}^{i - 1} B_{T} & \sum_{i = 1}^{1} A_{T}^{i - 1} B_{T} & \dots & 0 \\ \sum_{i = 1}^{3} A_{T}^{i - 1} B_{T} & \sum_{i = 1}^{2} A_{T}^{i - 1} B_{T} & \dots & 0 \\ ⋮ & ⋮ & ⋮ \\ \sum_{i = 1}^{p} A_{T}^{i - 1} B_{T} & \sum_{i = 1}^{p - 1} A_{T}^{i - 1} B_{T} & \dots & \sum_{i = 1}^{1} A_{T}^{i - 1} B_{T} \end{matrix}],

S_{F} = [\begin{matrix} I \\ \sum_{i = 1}^{1} A_{T}^{i} \\ \sum_{i = 1}^{2} A_{T}^{i} \\ ⋮ \\ \sum_{i = 1}^{p - 1} A_{T}^{i} \end{matrix}], \vec{I} = [\begin{matrix} I \\ I \\ I \\ ⋮ \\ I \end{matrix}],

\vec{X} = [\begin{matrix} X (k + 1) \\ X (k + 2) \\ X (k + 3) \\ ⋮ \\ X (k + p) \end{matrix}], Δ \vec{U} (k) = [\begin{matrix} Δ u (k) \\ Δ u (k + 1) \\ Δ u (k + 2) \\ ⋮ \\ Δ u (k + p - 1) \end{matrix}] .

定义代价函数J为车辆位置、车速的跟踪偏差以及发动机扭矩的变化率，其表达式为

\begin{matrix} J = \sum_{i = 1}^{p} q_{i} \cdot (X (k + i) - R (k + i))^{2} + \\ \sum_{i = 1}^{p} r_{i} \cdot (Δ U (k + i - 1))^{2}, \end{matrix}

(16)

其中Q和R分别为跟踪偏差和发动机扭矩变化率的权重，在车辆路径规划场景中，需要考虑约束问题，约束主要包括加速度约束与最大车速限值. 其中车速是状态量，所以最大车速限值相当于对预测步长下的状态量进行限值，而加速度约束可以通过离散转化为对车速的约束. 最终通过求解

Δ \vec{U} （ k ）

，使得代价函数J在满足约束的条件下达到最小.

3 基于Simulink+GT-SUITE联合仿真

本文基于MATLAB-Simulink，GT-SUITE平台进行联合仿真，其中被控对象通过GT-SUITE进行搭建，主要包括车辆、变速箱、发动机模型; 车辆路径规划以及位置闭环策略在 Simulink 中开发. 首先介绍被控对象模型. 发动机为直喷发动机，当量燃烧（空燃比 14.7），主要特性参数见表1，外特性曲线见表2.

表1发动机主要特性参数

Table1Main characteristic parameters of the engine

表2发动机外特性

Table2External characteristics of engine

发动机模型示意图如图4所示.

图4发动机模型示意图

Fig.4Schematic diagram of engine model

仿真车辆的基本参数如表3所示.

表3车辆参数

Table3Vehicle parameters

为了便于Simulink仿真，将上述被控对象模型封装成FMU格式. 模型的输入主要包括发动机需求扭矩，模型通过控制节气门开度闭环需求扭矩. 仿真测试的道路信息为: 道路全长12 km，其中包含6段限速区、9段坡以及12个红绿灯，具体信息如表4所示.

本文目标为开发车辆预测性路径规划以及位置闭环控制算法，要求实现车辆的综合目标函数最优，并且限制车辆在起点、终点的车速为0，不能闯红灯、超速，且加减速度必须在±5 m/s²以内，最终停下来时的位置距离终点不能超过±5 m. 定义综合目标函数为

J = a \cdot Time + b \cdot Fuel,

(17)

其中: Time为通行时间; Fuel为燃油消耗量; a为通行时间的权重; b为油耗的权重，权重的大小会直接影响路径规划的结果. 本文基于商用车的运输需求进行考虑，商用车运输往往会在运输效率与油耗之间权衡，综合目标函数中如果没有时间项，那么会影响运输效率; 没有油耗项，则体现不出省油的目标. 本文综合考虑运输效率和油耗的影响，经实车数据分析，将a定为 0.7， b为0.3.

表4道路信息

Table4Road information

3.1 车辆路径规划

设计动态规划算法，首先需要将求解空间进行离散化，网格离散化的程度，即网格划分的疏密程度对动态规划算法的求解速度和求解精度影响较大. 针对同一连续问题求解时，网格划分的越少即离散化越稀疏，则动态规划算法的求解速度越快，同时得到的最终优化结果同实际最优值偏差越大; 离散化越密集，则动态规划算法得到的最终优化结果同实际最优值偏差越小，同时求解速度会明显变慢. 因此需要合理选取网格离散化的程度来兼顾求解速度和优化结果精度 ^[15] .

首先进行第1层动态规划算法，分别将前方距离和设定车速划分为离散空间. 首先是前方距离，设车辆起始点位置是0，前方每100 m设置一个位置状态，如果前方100 m内存在限速区间的起始、终止位置，或者红绿灯，则在对应位置增加一个位置状态，位置空间一直从起点划分到终点. 接着对车速空间进行离散，车速离散空间从0 km/h开始，以1 km/h作为离散间隔，一直划分到当前位置的最高限速. 由于要求起点和终点的车速为零，因此起点、终点的车速离散状态只有 0 km/h.

然后进行第1层动态规划求解，方法如第2部分介绍. 从图5–6可以看出，车辆基本是在满足加速度限制的条件下，按照道路最高限速移动，其中几处红绿灯基本是急减速到0 m/s通过.

接下来计算通过每个红绿灯的时间窗口，计算结果如图7所示. 从图中可以看出，在6150 m之前，通过每个红绿灯的时间窗口与第1层动态规划的计算出的时间窗口基本是一致的; 在6150 m的红路灯处，最慢方案可以暂时比第1层动态规划计算出的红绿灯通行窗口晚一个红绿灯的时间，但随后在7340 m处，两者通过红绿灯的时间窗口又恢复成一致，在这一段过程中，最慢方案的车速变化比第1层动态规划的结果更加平缓.

图5第1层动态规划车速示意图

Fig.5Schematic diagram of the first layer dynamic programming velocity

图6第1层动态规划位置示意图

Fig.6Schematic diagram of the first layer dynamic programming location

图7规划路径边界示意图

Fig.7Schematic diagram of planned path boundary

最后进行第2层动态规划，得到车辆最终的规划位置如图8所示. 从图8中可以看出，第2层动态规划到达终点的时间与第1层动态规划的到达时间非常接近，但第2层动态规划的车速变化更加平稳，且避免了在红绿灯附近的急加速与急减速，避免了车辆动能的损失.

3.2 车辆位置闭环

基于上述整车物理模型以及Simulink进行联合仿真，对上述双动态规划计算得到的规划路径进行闭环控制. 图9为分别使用比例积分微分（proportional integration differentiation，PID）和MPC闭环车辆位置的示意图. 在进行位置闭环时，由于PID是基于偏差的控制且无法对前方的路况进行预测，因此PID在位置跟踪时存在超调和静差的情况，且PID在跟踪出现超调时，由于车辆无法后退，因此无法对位置的偏差进行修正，应用MPC控制可以对前方速度以及路况进行预测，使位置控制的精度更高且不易超调，跟踪性更好.

图8第2层动态规划位置示意图

Fig.8Schematic diagram of the second layer dynamic programming location

图9PID以及MPC位置跟踪图

Fig.9PID and MPC position tracking diagram

从图9中可以看出，PID 在上坡或者下坡的时候，由于没有预见性，即使在上坡过程中扭矩到达最大扭矩，仍然无法实现位置闭环，一直存在静态偏差. 而 MPC在闭环车辆位置时有预见性，在上坡前可以合理调整车速，使车辆提前加速，不会出现由于最大扭矩限制导致位置跟踪不上; 在下坡时，可以合理调整车速使超调更少. 从PID以及MPC位置闭环的对比图中可以看出，MPC的跟踪效果明显优于PID.

图10为对比使用单层动态规划的车辆轨迹与双动态规划的车辆轨迹的综合目标函数，从图10中可以看出，双动态规划的综合目标函数比起单动态规划的综合目标函数下降了2.08%.

为了充分验证算法的可靠性，设计不同的路况进行测试，增加3条不同的道路对算法进行验证，其道路信息如表5（a）–（c）所示.

图10不同规划的综合目标函数示意图

Fig.10Schematic diagram of comprehensive objective function for different plans

经测试，表6展示了双动态规划算法的综合目标函数与单层动态规划算法的综合目标函数，从表6可以看出，应用双动态规划算法可以有效降低油耗，与单层动态规划算法相比，大约提高了2.13%的燃油经济性.

4 结论

针对商用车节能减排的时代要求，本文设计了双动态规划算法，基于道路交通信号灯、限速、坡度等信息，对车辆的车速、位置进行了规划. 针对车辆实际位置与规划路径的偏差，使用模型预测控制算法对车辆位置进行闭环，提高了车辆跟踪规划路径的精度. 通过设计不同的仿真道路，对本文提出的算法进行了验证，结果证明，本文提出的算法可以有效地利用道路信息进行车辆路径规划，在考虑通行时间的同时也能显著降低油耗. 本文提出的算法与单层动态规划算法相比，在油耗方面上能优化2.13%，且规划的车速可以避免交通信号灯前的急刹，提高了驾驶的舒适性.

表5（a）测试路况1信息

Table5(a) Test road condition 1 information

表5（b）测试路况2信息

Table5(b) Test road condition 2 information

表5（c）测试路况3信息

Table5(c) Test road condition 3 information

表6测试路况结果

Table6Test road condition results

图1动态规划状态点示意图

Fig.1Schematic diagram of dynamic programming solution

下载: 全尺寸图片

图2规划车速边界求解示意图

Fig.2Schematic diagram for solving the boundary of planned vehicle speed

下载: 全尺寸图片

图3第2层动态规划状态示意图

Fig.3Schematic diagram of the second layer dynamic programming state

下载: 全尺寸图片

图4发动机模型示意图

Fig.4Schematic diagram of engine model

下载: 全尺寸图片

图5第1层动态规划车速示意图

Fig.5Schematic diagram of the first layer dynamic programming velocity

下载: 全尺寸图片

图6第1层动态规划位置示意图

Fig.6Schematic diagram of the first layer dynamic programming location

下载: 全尺寸图片

图7规划路径边界示意图

Fig.7Schematic diagram of planned path boundary

下载: 全尺寸图片

图8第2层动态规划位置示意图

Fig.8Schematic diagram of the second layer dynamic programming location

下载: 全尺寸图片

图9PID以及MPC位置跟踪图

Fig.9PID and MPC position tracking diagram

下载: 全尺寸图片

图10不同规划的综合目标函数示意图

Fig.10Schematic diagram of comprehensive objective function for different plans

下载: 全尺寸图片

表1发动机主要特性参数

Table1Main characteristic parameters of the engine

下载: 全尺寸图片

表2发动机外特性

Table2External characteristics of engine

下载: 全尺寸图片

表3车辆参数

Table3Vehicle parameters

下载: 全尺寸图片

表4道路信息

Table4Road information

下载: 全尺寸图片

表5（c）测试路况3信息

Table5(c) Test road condition 3 information

下载: 全尺寸图片

表6测试路况结果

Table6Test road condition results

下载: 全尺寸图片

图1动态规划状态点示意图

Fig.1Schematic diagram of dynamic programming solution

图2规划车速边界求解示意图

Fig.2Schematic diagram for solving the boundary of planned vehicle speed

图3第2层动态规划状态示意图

Fig.3Schematic diagram of the second layer dynamic programming state

图4发动机模型示意图

Fig.4Schematic diagram of engine model

图5第1层动态规划车速示意图

Fig.5Schematic diagram of the first layer dynamic programming velocity

图6第1层动态规划位置示意图

Fig.6Schematic diagram of the first layer dynamic programming location

图7规划路径边界示意图

Fig.7Schematic diagram of planned path boundary

图8第2层动态规划位置示意图

Fig.8Schematic diagram of the second layer dynamic programming location

图9PID以及MPC位置跟踪图

Fig.9PID and MPC position tracking diagram

图10不同规划的综合目标函数示意图

Fig.10Schematic diagram of comprehensive objective function for different plans

表1发动机主要特性参数

Table1Main characteristic parameters of the engine

表2发动机外特性

Table2External characteristics of engine

表3车辆参数

Table3Vehicle parameters

表4道路信息

Table4Road information

表5（c）测试路况3信息

Table5(c) Test road condition 3 information

表6测试路况结果

Table6Test road condition results

图1动态规划状态点示意图

Fig.1Schematic diagram of dynamic programming solution

图2规划车速边界求解示意图

Fig.2Schematic diagram for solving the boundary of planned vehicle speed

图3第2层动态规划状态示意图

Fig.3Schematic diagram of the second layer dynamic programming state

图4发动机模型示意图

Fig.4Schematic diagram of engine model

图5第1层动态规划车速示意图

Fig.5Schematic diagram of the first layer dynamic programming velocity

图6第1层动态规划位置示意图

Fig.6Schematic diagram of the first layer dynamic programming location

图7规划路径边界示意图

Fig.7Schematic diagram of planned path boundary

图8第2层动态规划位置示意图

Fig.8Schematic diagram of the second layer dynamic programming location

图9PID以及MPC位置跟踪图

Fig.9PID and MPC position tracking diagram

图10不同规划的综合目标函数示意图

Fig.10Schematic diagram of comprehensive objective function for different plans

表1发动机主要特性参数

Table1Main characteristic parameters of the engine

表2发动机外特性

Table2External characteristics of engine

表3车辆参数

Table3Vehicle parameters

表4道路信息

Table4Road information

表5（c）测试路况3信息

Table5(c) Test road condition 3 information

表6测试路况结果

Table6Test road condition results

HU Angang. China’s goal of peaking carbon emissions before 2030 and its main approaches. Journal of Beijing Institute of Technology(Social Sciences Edition),2021,21(3):1-15.(胡鞍钢. 中国实现2030年前碳达峰目标及主要途径. 北京工业大学学报(社会科学版),2021,21(3):1-15.)

LI Keqiang, DAI Yifan, LI Shengbo,et al. Development status and trends of intelligent connected vehicle(icv)technology. Journal of Automotive Safety and Energy Conservation,2017,8(1):1-14.(李克强, 戴一凡, 李升波, 等. 智能网联汽车(ICV)技术的发展现状及趋势. 汽车安全与节能学报,2017,8(1):1-14.)

CHAO S, JACOPO G, FRANCESCO B,et al. Optimal eco-driving control of connected and autonomous vehicles through signalized intersections. IEEE Internet of Things Journal,2020,7(5):3759-3773.

LENG Jianghao, SUN Chao, LU Bing. Intelligent vehicle multi signal light intersection fast energy-saving speed planning. Automotive Engineering,2021,43(10):1442-1447,1478.(冷江昊, 孙超, 卢兵. 智能车辆多信号灯路口快速节能车速规划. 汽车工程,2021,43(10):1442-1447,1478.)

WEI Lin, ZHAO Qinghai. Three stage intelligent vehicle multi signal intersection energy-saving speed planning. Journal of Qingdao University(Natural Science Edition),2023,36(2):71-78,83.(韦麟, 赵清海. 三阶段智能车辆多信号灯路口节能车速规划. 青岛大学学报(自然科学版),2023,36(2):71-78,83.)

NGUYEN W D, TRAN T K O, NGUYEN T D,et al. An efficient and reliable green light optimal speed advisory system for autonomous cars. The 18th Asia-Pacific Network Operations and Management Symposium(APNOMS). Kanazawa, Japan: IEEE,2016, DOI:10.1109/APNOMS.2016.7737260.

LI C, WANG S, HUANG X,et al. Parked vehicular computing for energy-efficient internet of vehicles: A contract theoretic approach. IEEE Internet of Things Journal,2019,6(4):6079-6088.

SUN C, GUANETTI J, BORRELLI F,et al. Optimal eco-driving control of connected and autonomous vehicles through signalized intersections. IEEE Internet of Things Journal,2020,7(5):3759-3773.

SUN Yinjian. Research on trajectory tracking control algorithm for autonomous vehicles based on model predictive control. Beijing: Beijing Institute of Technology,2015.(孙银健. 基于模型预测控制的无人驾驶车辆轨迹跟踪控制算法研究. 北京: 北京理工大学,2015.)

HUO Hongwei. Algorithm Design and Analysis. Xi’an: Xi’an University of Electronic Science and Technology Press.2005.(霍红卫编著. 算法设计与分析. 西安: 西安电子科技大学出版社.2005.)

WU Shouzhang. Discrete time optimal control-comment on dynamic programming. Control Theory & Applications,2013,30(9):1165-1169.(吴受章. 离散时间最优控制-评论动态规划. 控制理论与应用,2013,30(9):1165-1169.)

ZHAO Dongbin, LIU Derong, YI Jianqiang. Overview of urban traffic signal optimization control methods based on adaptive dynamic programming. Journal of Automation,2009,35(6):676-681.(赵冬斌, 刘德荣, 易建强. 基于自适应动态规划的城市交通信号优化控制方法综述. 自动化学报,2009,35(6):676-681.)

XU Zuhua. Research on model predictive control theory and application. Hangzhou: Zhejiang University,2004.(徐祖华. 模型预测控制理论及应用研究. 杭州: 浙江大学,2004.)

XI Yugeng, LI Dewei, LIN Shu. Model predictive control-current status and challenges. Journal of Automation,2013,39(3):222-236.(席裕庚, 李德伟, 林姝. 模型预测控制-现状与挑战. 自动化学报,2013,39(3):222-236.)

ZHAO Guanghui, GAO Xin, FANG Jinfu,et al. Performance comparison and analysis of fixed step discretization methods for continuous control systems. Journal of Anshan Normal University,2018,20(2):33-39.(赵广辉, 高鑫, 方金福, 等. 连续控制系统固定步长离散化方法性能对比分析. 鞍山师范学院学报,2018,20(2):33-39.)