一种智能网联汽车的路径实时规划与分布式控制方法与流程

本发明属于智能网联汽车路径规划与分布式控制技术领域，特别涉及一种智能网联汽车路径实时规划与分布式控制方法。

背景技术：

汽车智能化、网联化技术的应用，可以减少事故、提高交通效率。车联网允许车辆共享信息，在此基础上，车辆的交互行为如跟车、超车、避撞可以借助分布式最优控制实现；车辆的局部路径规划在无需考虑车车碰撞的前提下，可以显著降低复杂度从而提升计算实时性。

现有的车辆规划方法，主要使用数值优化、搜索或采样插值的方式。然而，其可能存在以下缺点：需要网格化、轨迹不光滑等。特别是在处理存在动态避障约束的任务时，无法保证实时性。

现有的车辆控制方法，仅能依赖于自车决策阶段给出光滑且符合动力学约束的轨迹，各自独立地进行跟踪。在没有分享通讯信息的情况下，多车的交互行为可能彼此制约，甚至出现死锁现象。

技术实现要素：

本发明的目的是解决智能网联汽车在结构化道路场景下高效通行的规划与控制问题，提出一种智能网联汽车路径的实时规划和分布式控制方法，本发明能够快速求解局部轨迹点，将轨迹点信息作为参考、网联他车信息作为条件，输入分布式控制器，独立求解约束最优控制问题，具有计算和通信需求低、可扩展性强、规划速度快、通行效率高的优点。

为实现上述目的，本发明采用如下技术方案：

本发明提出的一种智能网联汽车的路径实时规划和分布式控制方法，其特征在于，包括以下步骤：

1)车辆出发时刻，每辆智能网联汽车根据已知的车道线位置和自车的全局路径，确定道路特征点；

2)规划阶段，通过贝塞尔曲线规划自车当前位置到距自车最近的道路特征点的路径，在此基础上，根据已知的信号灯相位和限速标志牌信息，规划自车速度曲线，并在自车路径曲线上按照该规划自车速度曲线进行离散采样，得到固定时间间隔的自车规划轨迹点，形成自车规划轨迹点序列；

3)控制阶段，每辆智能网联汽车首先借助车联网获取其通信范围内他车上一步控制输出的预测信息，并结合步骤2)得到的自车规划轨迹点，在未来一段时域内求解本地的最优控制问题，向自车输出控制量、向通信范围内他车输出预测信息；

4)不断执行步骤2)～3)，直至达到全局路径终点。

本发明的特点及有益效果如下：

本发明可以保证求解问题的规模较小，求解过程彼此独立，从而降低了对车载计算单元的计算能力、通信频率要求，适用于多车共存的结构化道路交通场景。本发明的局部路径规划由于无需考虑动态交通参与者，有效保证实时性。

本发明提出分布式控制方法，共享车辆状态信息。对于网联汽车，可以共享准确的预测状态；对于非网联汽车，也可以借助本地观测进行预测，从而适用于混合交通场景。本发明的控制方法将避撞作为约束显式地考虑，能够实现无冲突通行的效果，从而提升交通效率。

附图说明

图1是本发明实施例的路径规划示意图；

图2是本发明的分布式控制使用的车辆预测模型及全局坐标图。

具体实施方式

为使本发明的目的、技术方案及优点更加清楚明白，以下结合附图及实施例，对本发明进行进一步的详细说明。应当理解，此处所描述的具体实施方式仅仅用以解释本发明，并不限定本发明的保护范围。

为了更好地理解本发明，以下详细阐述一个本发明一种智能网联汽车路径的实时规划和分布式控制方法的应用实例。

参见图1，本发明提出的一种智能网联汽车的路径实时规划方法，根据智能网联汽车的当前位置和车道线信息、信号灯信息、限速信息，求解贝塞尔曲线表达式和速度表达式，获得每辆汽车的局部参考轨迹点。该方法计算过程与动态交通参与者无关，能够保证路径规划的实时性。

本发明实施例的一种智能网联汽车的路径实时规划与分布式控制方法。本方法具体包括以下步骤：

1)车辆出发时刻，每辆智能网联汽车根据已知的车道线位置和自车的全局路径，确定道路特征点。

2)规划阶段，通过贝塞尔曲线规划自车当前位置到距自车最近的道路特征点的路径，在此基础上，根据已知的信号灯相位和限速标志牌信息，规划自车速度曲线，并在自车路径曲线上按照该规划自车速度曲线进行离散采样，得到固定时间间隔的自车规划轨迹点，形成自车规划轨迹点序列；

3)控制阶段，每辆智能网联汽车首先借助车联网获取其通信范围内他车上一步控制输出的预测信息，并结合步骤2)得到的自车规划轨迹点，在未来一段时域内求解本地的最优控制问题，向自车输出控制量、向通信范围内他车输出预测信息；

4)不断执行步骤2)～3)，直至达到全局路径终点。

进一步地，步骤1)包括以下步骤：

根据自车全局路径和含车道线信息的地图，基于规则地确定道路特征点。规则如下：定义沿车道中心线上至智能网联汽车行驶方向为前方，在连续的直道或弯道，智能网联汽车i下一个道路特征点pm，i取为距离其上一道路特征点前方lp处，lp为自定义距离，一般取智能网联汽车按平均车速行驶1秒的长度，脚标m代表道路特征点的序号，m＝0，1，2，...，ntem，ntem为自车全局路径的终点对应的道路特征点序号。在路口附近，即若按照前述规则产生的道路特征点在路口区域内，则将智能网联汽车的下一个道路特征点pm，i替换为当前所在车道中心线与路口停止线的交点，并将智能网联汽车的再下一个道路特征点pm+1，i取为自车驶出路口时进入车道的中心线起点。因此，按照所述规则，只要车辆在结构化道路的某条车道中心线上出发，即可作为起点，并根据自车实时位置，依次确定各道路特征点直至全局终点。道路特征点pm，i的坐标包含横坐标、纵坐标、方向角坐标，即[xp，m，i，yp，m，i，φp，m，i]^t。

进一步地，步骤2)包括以下步骤：

2-1)通过贝塞尔曲线规划自车当前位置到距自车最近的道路特征点的路径，同时通过直线规划该道路特征点之后的自车路径。具体为：

选取智能网联汽车i路径的两个辅助点，该两个辅助点为二维向量；根据智能网联汽车i的当前时刻质心pi坐标[xi，yi，φi]^t，以及下一个道路特征点pm，i的坐标[xp，m，i，yp，m，i，φp，m，i]^t、第一辅助点pa1，i的坐标[xa1，i，ya1，i]^t和第二辅助点pa2，i的坐标[xa2，i，ya2，i]^t，确定一条由贝塞尔曲线和直线组合而成的参数方程，其中，x(·)和y(·)分别为横、纵坐标，φ(·)为方向角坐标；脚标i和m分别为智能网联汽车和道路特征点的序号，脚标a1和a2分别为取智能网联汽车路径的两个辅助点序号。所述参数方程为：

式中，

τ是确定的贝塞尔曲线和直线的自变量；当自车由当前位置驶向距自车最近的道路特征点时，即τ∈[0，1]时，利用贝塞尔曲线规划自车路径；当自车由距离其最近的道路特征点驶向后续道路特征点时，即τ∈(1，+∞)，利用直线规划自车路径；xr，i(τ)和yr，i(τ)分别是为智能网联汽车i规划的路径的横纵坐标。

智能网联汽车i在行驶过程中，会先驶向最近的下一个道路特征点pm，i，然后驶向再下一个道路特征点pm+1，i。在智能网联汽车i驶离最近的下一个道路特征点pm，i、驶向再下一个道路特征点pm+1，i的过程中，利用式(2)判定是否将下一个道路特征点pm，i更新为pm+1，i。确定当前时刻下一个道路特征点的规则如下：

式中，τi，-1，1代表智能网联汽车i在上一时刻(用脚标-1表示)所规划的轨迹点序列中，第1个轨迹点对应的参数τ值。τbound为(0，1)内的道路特征点更新阈值。当τi，-1，1＜τbound时，表明智能网联汽车i尚未充分接近该下一个道路特征点pm，i，无需更新下一个道路特征点，即令m＝m。当τi，-1，1≥τbound时，表明智能网联汽车i马上达到或已经到达该下一个道路特征点pm，i，因此需要更新下一个道路特征点，即令m＝m+1。道路特征点更新阈值τbound按照实际需要设定，若其τbound的取值越趋近于1，车辆规划路径越逼近路径特征点，但可能因为避让他车导致实际行驶轨迹曲率过大；若τbound的取值越趋近于0，车辆实际行驶轨迹与路径特征点的偏离程度越大，但在避让他车后逐渐回归自车规划路径的轨迹更为光滑。

式(1)中第一、第二辅助点的坐标由式(3)确定：

式中，l0，i，l3，i分别为智能网联汽车i的近端辅助距离和远端辅助距离，定义如下：

式中，xr，i，-1(τi，-1，1)和yr，i，-1(τi，-1，1)分别是上一时刻(用脚标-1表示)智能网联汽车i所规划出的横纵坐标满足的函数在τi，-1，1处的取值，即上一时刻规划出的第一个轨迹点的横纵坐标。c1，c2为自定义常数值，取值范围均为(1，+∞)，且(1/c1+1/c2)＜1。一般而言，c1，c2取值越大，规划路径越接近直线；反之，规划路径曲率越小。xtemp1，i，xtemp2，i，ytemp1，i，ytemp2，i分别为智能网联汽车i的上一时刻规划路径所采用的第一、第二辅助点的横纵坐标，定义如下：

其中，t1，t2为自定义常数，取值范围均为(0，1)，且t1≠t2，t1和t2表征当前时刻所规划的路径与上一时刻规划路径上的两个不同的公共点所对应的参数τ的值。τtemp1，i和τtemp2，i表征上一时刻所规划的路径与当前时刻规划路径上的两个不同的公共点所对应的参数τ的值。t1，t2与τtemp1，i，τtemp2，i对应关系为τtemp1，i＝τi，-1，1+t1(1-τi，-1，1)，τtemp2，i＝τi，-1，1+t2(1-τi，-1，1)。

2-2)根据信号灯相位和限速标志牌信息，基于规则地确定速度曲线，并在前述贝塞尔曲线和直线上按速度采样。车辆在连续直道或弯道行驶时，速度可取为低于道路限速ulimit的任意车速，例如可设为0.9ulimit。车辆在驶入路口前，速度取值要求如下：

表1智能网联汽车i驶入路口的规划速度约束

表1列出了驶入路口前，智能网联汽车i的速度需满足的条件，其中t为当前时刻到下一次信号灯跳变之间的时间间隔，为表述简便，这里不考虑黄灯。si为当前时刻智能网联汽车i到路口停止线的距离，ur，i(t)为智能网联汽车i的规划车速。表1第2列给出了速度规划的要求，实际车速曲线可以有不同的确定方式。例如，若车辆始终进行匀加速/匀减速/匀速运动，则速度可以按表2显式给出：

表2匀变速车辆的规划速度规则

表2中，si＜0代表智能网联汽车i越过路口停止线，进入路口。ui为当前时刻智能网联汽车i的纵向车速观测值。当其驶出该路口时，si更新为智能网联汽车i到下一路口停止线的距离。lbound为路口距离阈值，lbound≤tphaseulimit，tphase为信号灯的单相位持续时间(即红灯时间或绿灯时间)。ai，max为智能网联汽车i的最大纵向加速度值。ur，i(t)为智能网联汽车i的规划车速。

对表2确定的参考车速ur，i(t)进行离散，得到离散的规划车速ur，i(k)。

式中，ts为离散步长，可取为0.1s；ntraj为每次规划轨迹点的总数目，例如可取为20。

按照下式确定智能网联汽车i的规划轨迹点：

τi，k＝{τ|xr，i(τ)＝xr，i(k)，yr，i(τ)＝yr，i(k)}

式中，xr，i(k)，yr，i(k)为离散后的智能网联汽车i的规划轨迹点，且xr，i(0)＝xi，yr，i(0)＝yi。

根据式(7)，可以依次计算当前时刻智能网联汽车i规划的全部ntraj个规划轨迹点(xi(k)，yi(k))，1≤k≤ntraj。τi，k为当前时刻所规划的智能网联汽车i的第k个轨迹点对应的参数τ的值，即未来第k步参数τ的值，且τi，0＝0。当智能网联汽车i开始规划下一步的轨迹时，上一步全部的τi，k记为τi，-1，k，得到自车规划轨迹点序列。

进一步地，步骤3)包括以下步骤：

3-1)建立最优控制模型

构建车辆求解的最优控制模型，如式(8)-(13)所示：

xi(k+1)＝fi(xi(k)，ui(k))(9)

xi(0)＝xi，current(10)

xi，min≤xi(k)≤xi，max，1≤k≤np(11)

ui，min≤ui(k)≤ui，max，0≤k≤np(12)

(xi(k)-xj(k))^tqi，s(xi(k)-xj(k))≥di，j∈ni(t)(13)

其中：

式(8)为目标函数，xi(k)为智能网联汽车i在未来第k步的状态向量，为变量；ui(k)为智能网联汽车i在未来第k步的输入向量，为变量；xr，i(k)为步骤2-2)得到的智能网联汽车i在未来第k步的规划轨迹点坐标向量；ac为对角矩阵，ac＝diag{1，1，0，0，1，0}，qi为跟踪项对角矩阵，ri为输入项对角矩阵，分别用于限制跟踪误差、输入幅值，要求对角元素非负。对角元素越大，对应变量的优化权重越大，对其限制作用也越大。

式(9)为动力学约束，其中fi(·)为非线性映射车辆模型，反映了智能网联汽车i从k时刻状态到k+1时刻状态的转移关系。

式(10)为初值约束，xi(t)为智能网联汽车i当前时刻的状态观测值。

式(11)为状态约束，xi，min和xi，max分别为智能网联汽车i的状态值下限和状态值上限。

式(12)为输入约束，ui，min和ui，max分别为智能网联汽车i的输入值下限和输入值上限。

式(13)为碰撞约束，xj(k)为车辆j在未来第k步的坐标，qi，s＝diag{1，1，0，0，0，0}，di为自定义正实数常量，一般不小于车辆i俯视对角线长度的平方值。ni(t)为当前时刻车辆i的邻域内全部其他车辆集合。

0≤k≤np，np为预测步数，np＝ntraj。

智能网联汽车i在未来第k步的状态向量xi(k)和输入向量ui(k)，智能网联汽车i当前时刻的状态观测值xi，current，智能网联汽车i的状态值下限和状态值上限xi，min和xi，max，以及智能网联汽车i的输入值下限和输入值上限ui，min和ui，max分别按照式(14)～(17)计算：

式(14)中，xi(k)为智能网联汽车i未来第k步的横坐标，为变量；yi(k)为智能网联汽车i未来第k步的纵坐标，为变量；ui(k)为智能网联汽车i未来第k步的纵向车速，为变量；vi(k)为智能网联汽车i未来第k步的横向车速，为变量；为智能网联汽车i未来第k步的横摆角，为变量；为智能网联汽车i未来第k步的横摆角速度，为变量。式(14)中，ai(k)为智能网联汽车i未来第k步输入的纵向加速度，为变量；δi(k)为智能网联汽车i未来第k步输入的前轮转角，为变量，具体参见图2。

式(15)中，xi为智能网联汽车i当前时刻的质心横坐标观测值；yi为智能网联汽车i当前时刻的质心纵坐标观测值；ui为智能网联汽车i当前时刻的纵向车速观测值；vi为智能网联汽车i当前时刻的纵向车速观测值；φi为智能网联汽车i当前时刻横摆角的观测值；ωi为智能网联汽车i当前时刻横摆角速度的观测值。

式(16)中，xi，min为智能网联汽车i质心横坐标的最小值，yi，min为智能网联汽车i质心纵坐标的最小值；ui，min为智能网联汽车i纵向车速的最小值，vi，min为智能网联汽车i横向车速的最小值；φi，min为智能网联汽车i横摆角的最小值；ωi，min为智能网联汽车i横摆角速度的最小值。式(17)中，xi，max为智能网联汽车i质心横坐标的最大值；yi，max为智能网联汽车i质心纵坐标的最大值；ui，max为智能网联汽车i纵向车速的最大值；vi，max为智能网联汽车i横向车速的最大值；φi，max为智能网联汽车i横摆角的最大值；ωi，max为智能网联汽车i横摆角速度的最大值。ai，min为智能网联汽车i输入的纵向加速度最小值；δi，min为智能网联汽车i输入的前轮转角最小值。ai，max为智能网联汽车i输入的纵向加速度最大值；δi，max为智能网联汽车i输入的前轮转角最大值。式(16)、(17)中的各最小值和最大值分别由智能网联汽车相应实测确定。

式(18)中，mveh为全部智能网联汽车组成的集合；dcomm，i为智能网联汽车i能够建立通信的最大距离。式(18)表示，以智能网联汽车i质心为中心，以dcomm，i为半径的圆内的全部其他智能网联汽车，构成了t时刻的智能网联汽车i的最优控制他车集合ni(t)。

3-2)求解最优控制模型

步骤3-1)构建的最优控制模型(8)-(13)可通过标准非线性规划算法求解，如内点法。求解优化变量为ui(k)，xi(k)，k＝0，1，...，np，np为模型预测控制的预测步数，取值越大，预测时间越长，但求解耗时也越长。

3-3)输出最优控制量

将ui(0)输出至车辆底层控制器，完成决策和上层控制。在离散步长ts内保持ui(0)不变，之后循环执行步骤2-1)、2-2)、3-1)、3-2)。

进一步地，步骤3-1)中式(9)采用的非线性映射车辆模型为：

式(19)中，lf，i为智能网联汽车i质心到前轴的距离；lr，i为智能网联汽车i质心到后轴的距离；cf，i为智能网联汽车i当前时刻的等效前轮侧偏刚度，cf，i＝2cw，f，i，cw，f，i为智能网联汽车i的单个转向轮轮胎侧偏刚度；cr，i为智能网联汽车i的等效后轮侧偏刚度，cr，i＝2cw，r，i，cw，r，i为智能网联汽车i的单个后轮轮胎侧偏刚度；mi为智能网联汽车i的总质量；izz，i为智能网联汽车i绕z轴的转动惯量。

式(19)可以适用于车速为零的工况，并且相较于运动学模型具有更高的精度。

以上示意性地对本发明及其实施方式进行了描述，该描述没有限制性，附图中所示的也只是本发明的实施方式之一，实际并不局限于此。所以，如果本领域的普通技术人员受其启示，在不脱离本发明创造宗旨的情况下，不经创造性地设计出与该技术方案相似的方式及实施例，均应属于本发明的保护范围。