CN112193244B - 基于线性约束的自动驾驶车辆运动规划方法 - Google Patents

Abstract

本发明公开了一种基于线性约束的自动驾驶车辆运动规划方法。发明包括以下步骤：配置无人驾驶车辆及外围设备，获得自身车辆的当前运动状态，移动障碍物的当前运动状态以及静态障碍物位置，当前运动状态包括位置、速度和朝向；根据获得的自身车辆的当前运动状态和移动障碍物的当前运动状态利用基于带有行为决策模块以及约束简化器的运动规划算法，使无人驾驶车辆在存在潜在障碍物情况下完成车辆运动规划。本发明使无人驾驶车辆具有在高动态和多移动障碍物的复杂场景下实时运动规划能力。同时行为决策模块使算法不需要区分两种道路单独完成运动轨迹的规划，从而让无人驾驶车辆更好的适应结构化以及非结构化道路，提高了无人驾驶车辆的实时处理性能。

Description

基于线性约束的自动驾驶车辆运动规划方法

技术领域

本发明涉及一种自动驾驶车辆运动规划方法，具体涉及了一种基于线性约束的自动驾驶车辆运动规划方法。

背景技术

当前自动驾驶运动规划是保证无人驾驶车辆安全行驶的核心技术，其目的是为无人驾驶车辆规划到达目的地的无碰撞安全轨迹，同时该轨迹还应满足车辆动力学等约束。目前运动规划方法大多基于搜索，采样，离散优化与优化四种方法完成；基于优化的方法由于其能考虑多约束且在连续空间上求解，不依赖采样点等优点，在轨迹规划中有广泛的应用。但由于无人驾驶车辆行驶环境中有障碍物以及其他车辆参与，在运动规划中需要加入障碍物约束以及车辆自身运动学约束信息，以确保运动规划的安全性以及规划后的轨迹符合车辆运行动力学约束。由于障碍物约束形式表达一般较为复杂，多为非线性约束，在优化求解时会有很高的计算代价，若不对障碍物约束进行简化，基于优化的运动规划器很难达到自动驾驶车辆对于实时计算的时间要求。

在现有基于优化运动规划技术中，多障碍物约束处理在计算上非常昂贵，也由于障碍物约束有很强的非线性，求解空间多为非凸区域，会导致运动规划模块求解时间超过100ms，且非线性优化的效果非常依赖于初始值的选择，使自动驾驶车辆难以适应高动态环境。因此，基于优化的方法在目前较难在有较多动态障碍物参与的复杂环境下完成车辆运动规划任务。

发明内容

为了解决背景技术中存在的问题和需求，本发明提供了一种基于线性约束的自动驾驶车辆运动规划方法，方法为一种基于带有行为决策模块以及约束简化器的运动规划算法，使无人驾驶车辆在存在潜在障碍物情况下完成车辆运动规划，同时保证该算法的实时性能。

本发明方法包括以下步骤：

步骤1：配置无人驾驶车辆及外围设备，获得自身车辆的当前运动状态，移动障碍物的当前运动状态以及静态障碍物位置，当前运动状态包括位置、速度和朝向；

步骤2：

步骤201：根据自身车辆的当前运动状态预测处理获得自身车辆的轨迹，根据移动障碍物的当前运动状态预测处理获得移动障碍物的轨迹，逐一判断自身车辆的轨迹与每个移动障碍物的轨迹是否存在横向冲突，横向冲突的判断方法为在10s时间内自身车辆的轨迹与移动障碍物的轨迹是否在横向方向有相交；

若存在横向冲突，则需要紧急停车，进入步骤202；

若不存在横向冲突，则不需要紧急停车，进入步骤203；

步骤202：通过自身车辆的轨迹与移动障碍物的轨迹之间计算出横向冲突发生的位置和时间，自身车辆发出优先避让控制信号，并将优先避让控制信号以及横向冲突发生的位置和时间传递给约束生成器；进入步骤203；

步骤203：约束生成器实时根据自身车辆所在的位置，移动障碍物的位置和朝向以及静态障碍物的位置，生成车辆运动规划的障碍物约束；

步骤204：建立基于模型预测控制的运动规划器，运动规划器包括车辆运动学模型、目标函数和约束，将车辆运动学模型转化为线性模型，约束包括了车辆运动动力学约束和约束生成器生成的障碍物约束，求解获得了前轮转向角和加速度的控制量，对自身车辆进行自动驾驶的运动规划控制。

所述步骤1中，外围设备包括：

激光雷达传感器，安装在车辆顶部中心，用于感知自身车辆前方的障碍物；

视觉传感器，安装在车辆前部，用于拍摄车辆前方图像进而对图像识别；

毫米波雷达传感器，安装在车辆前部和左右两个侧方，用于对前方障碍物进行感知；

定位装置安装在车辆中，用于实现车辆定位。

所述步骤201中，由自身车辆的当前运动状态中的位置、速度和朝向保持不变形成的直线轨迹作为自身车辆的轨迹；移动障碍物的轨迹是由激光雷达传感器所探测的障碍物的当前运动状态后进行分类判断，分为移动障碍物和静态障碍物，然后再对移动障碍物采用预测算法处理获得。

所述的步骤203中，生成车辆运动规划的障碍物约束，具体为：

将自身车辆用圆心沿同一直线布置且尺寸相同的三个圆相交组成表示，位于两侧的两个圆的圆心在位于中间的圆的圆弧上；将障碍物用矩形表示；移动障碍物的矩形移动障碍物的当前运动状态预测处理获得，静态障碍物的矩形由静态障碍物位置获得；从障碍物矩形的四个顶点分别各自向自身车辆三个圆的每个圆做切线，从所有切线中选取能将移动障碍物与自身车辆分别完整地划分在两侧的切线，每个移动障碍物共获得两条切线；

将两条切线均进行平移，使得切线经过自身车辆的位于车头的圆的圆心，以平移后的各条切线划分可通行区域：以平移后的各条切线为分界线所划分的两侧区域中不包含移动障碍物的一侧的区域作为可行区域，将所有可行区域求交集获得最终的各个可通行区域；取自身车辆前方沿全局轨迹的固定位置作为参考点，根据全局轨迹选取可通行区域中各条切线到参考点的距离的平均值最小所对应的可通行区域为最佳的可通行区域，即作为行驶区域，构成行驶区域中的切线作为障碍物约束。

当约束生成器接收到优先避让控制信号时，需要紧急停车，约束生成器为自身车辆生成减速约束，具体是根据横向冲突发生的位置生成沿自身车辆的当前行驶方向所在的直线作为一条匀减速轨迹，使自身车辆在距离横向冲突发生的位置前的预设安全距离处停止；若无法在预设安全距离前停止，则为自身车辆规划按最大减速度减速轨迹；

减速约束表示为：

其中，a表示匀减速的加速度，v表示自身车辆的当前速度，D_s表示自身车辆的当前位置到横向冲突发生的位置之间的距离，D_safe表示预设安全距离，a_min表示最大减速度对应的加速度；

当约束生成器未接收到优先避让控制信号时，不需要紧急停车，不生成减速约束。

所述步骤204中，目标函数表示为：

其中，x(t)表示t时刻的状态量，x(t+k|t)为在t时刻对t+k时刻状态量的估计，r(t+k|t)表示t+k时刻参考状态量，状态量包括自身车辆的位置，速度以及朝向；Δu(t)表示t时刻的控制量的变化量，Δu(t+k|t)表示t+k时刻的控制量的变化量，控制量是由前轮转向角和加速度构成；R、Q为第一、第二常数权重矩阵；N_p为预测时域，N_c为控制时域；

约束表示为：

u_min＜u(t+k|t)＜u_max

Δu_min＜Δu(t+k|t)＜Δu_max

x_min＜x(t+k|t)＜x_max

Eu+FX≤G

u＝[δ,a]^T

X＝[x,y,θ,v]^T

E＝zeros(2,4)

G＝[B_up,-B_down,I_up,I_down]

其中，u_min和u_max分别表示控制量的最小值限制以及最大值限制；Δu_min、Δu_max分别表示控制量的变化量的最小值限制于最大值限制；u表示控制量，由前轮转向角δ与加速度a构成；X表示车辆状态，由车辆横向位置x、纵向位置y、车辆朝向θ和速度v构成；E、F、G为第一、第二、第三约束矩阵，E为2*4的零矩阵；k_up为构成行驶区域上边界切线的斜率，k_down为构成行驶区域下边界切线的斜率；I_up为构成行驶区域上边界切线的截距，I_down为构成行驶区域下边界切线的截距；B_up，B_down分别为道路的上下边界；

最后按照将目标函数转化为二次规划(QP)形式，采用二次规划方法求解获得控制量u(t)。

本发明在基于MPC的运动规划器中将以运动学模型为参考模型，加入障碍物避碰的不等式约束，以及加入对于速度、加速度、前轮转向角限制、角速度限制车辆等动力学约束，所有的约束均为不等式约束行驶，全部为线性约束，目标函数求解可以采用二次优化(QP)求解，保证算法实时性能。

本发明采用一种带有行为决策以及约束简化器的运动规划方法，优化运动规划算法的实时性以及对于多障碍物约束的处理能力，优化运动规划算法的实时性能以及对复杂环境的适应能力，在原有运动规划算法基础上，提升了原有运动规划算法在有较多障碍物且有移动障碍物的高动态环境下较差的实时性能。

本发明的有益效果为：

在硬件设备及计算能力一定的情况下，本发明通过带有行为决策模块以及障碍物约束简化模块的运动规划器构建，使无人驾驶车辆具有在高动态以及带有多移动障碍物的复杂场景下实时运动规划能力。同时行为决策模块使算法不需要区分两种道路单独完成运动轨迹的规划，从而让无人驾驶车辆更好的适应结构化以及非结构化道路，提高了无人驾驶车辆的实时处理性能。

附图说明

图1为运动规划方法的系统框图；

图2为横向冲突的判断方法框图；

图3为车辆躲避障碍物的示意图；

图4为障碍物约束的流程图；

图5为结构化场景1仿真结果图；

图6为结构化场景1仿真的状态与控制量示意图；

图7为结构化场景2仿真结果图；

图8为结构化场景2仿真的状态与控制量示意图。

图9为非结构化场景仿真结果图；

图10为非结构化场景仿真的状态与控制量示意图。

具体实施方式

下面结合具体实时例，进一步阐述本发明。应理解，这些实施例仅用于说明本发明而不用于限制本发明的范围。

如图1所示，本发明方法的实施例包括以下步骤：

本实施例基于软件仿真平台完成运动规划方法验证工作，为无人驾驶车辆完成实时运动规划功能。本实施例只设计运动规划部分，在仿真环境中，无人驾驶车辆能感知其感知范围内所有障碍物信息，并根据此时速度信息对移动障碍物进行预测。在本实验中，车辆长宽分别为4.8m和1.8m。

步骤1：配置无人驾驶车辆及外围设备，获得自身车辆的当前运动状态，移动障碍物的当前运动状态以及静态障碍物位置，当前运动状态包括位置、速度和朝向；

步骤1中，外围设备包括：

激光雷达传感器，安装在车辆顶部中心，用于感知自身车辆前方的障碍物；

视觉传感器，安装在车辆前部，用于拍摄车辆前方图像进而对图像识别交通信号灯等交通标识；

毫米波雷达传感器，安装在车辆前部和左右两个侧方，用于对前方5m内近距离的障碍物进行感知，弥补激光雷达传感器存在的盲区；

定位装置安装在车辆中，定位装置带有差分GPS及惯性导航系统，用于实现车辆定位。

将激光雷达传感器所在位置设定为世界坐标系原点。

自身车辆的位置和朝向是由定位装置检测获得，自身车辆的速度是由自身车辆本身自带的传感器检测获得。

障碍物的位置和朝向是由激光雷达传感器和毫米波雷达传感器检测获得，对多帧数据进行匹配处理获得障碍物的速度。

步骤2：

系统根据感知预测提供的障碍物信息基于规则完成车辆行为的判断，处理包括停车制动、跟随等。流程图见图2，基于自身车辆状态以及预测障碍物信息，对横向冲突进行判断。

步骤201：根据自身车辆的当前运动状态预测处理获得自身车辆的轨迹，根据移动障碍物的当前运动状态预测处理获得移动障碍物的轨迹，逐一判断自身车辆的轨迹与每个移动障碍物的轨迹是否存在横向冲突，横向冲突为沿垂直于自身车辆行驶方向两个轨迹是否相交的冲突，横向冲突的判断方法为在10s时间内自身车辆的轨迹与移动障碍物的轨迹是否在横向方向有相交，横向方向为沿垂直于自身车辆行驶方向；

若存在横向冲突，则需要紧急停车，自身车辆需要进行减速避让，给出停车避让的行为，进入步骤202；

若不存在横向冲突，则不需要紧急停车，根据参考轨迹数量判断车辆行驶的道路为结构化道路(由多车道构成)或是非结构化道路(不区分车道或仅有一个车道构成)，进入步骤203；

步骤201中，由自身车辆的当前运动状态中的位置、速度和朝向保持不变形成的直线轨迹作为自身车辆的轨迹；移动障碍物的轨迹是由激光雷达传感器所探测的障碍物的当前运动状态后进行分类判断，分为移动障碍物和静态障碍物，然后再对移动障碍物采用预测算法处理获得。具体由移动障碍物的当前运动状态中的位置、速度和朝向保持不变形成的直线轨迹。

步骤202：当自身车辆与移动障碍物存在横向冲突时，通过自身车辆的轨迹与移动障碍物的轨迹之间计算出横向冲突发生的位置和时间，自身车辆发出优先避让控制信号，并将优先避让控制信号以及横向冲突发生的位置和时间传递给约束生成器；进入步骤203；

步骤203：采用基于车辆几何约束的障碍物约束处理，在没有横向冲突的情况下，约束生成器实时根据自身车辆所在的位置以及移动障碍物的位置和朝向，生成车辆运动规划的障碍物约束；

步骤203中，生成车辆运动规划的障碍物约束，具体为：

如图2和图3所示，将自身车辆用圆心沿同一直线布置且尺寸相同的三个圆相交组成表示，位于两侧的两个圆的圆心在位于中间的圆的圆弧上；将移动障碍物用矩形表示；从移动障碍物矩形的四个顶点分别各自向自身车辆三个圆的每个圆做切线，从所有切线中选取能将移动障碍物与自身车辆分别完整地划分在两侧的切线作为当前移动障碍物的线性约束，每个移动障碍物共获得两条切线；

在单移动障碍物或者多移动障碍物情况下，将两条切线均进行平移，使得切线经过自身车辆的位于车头的圆的圆心，以平移后的各条切线划分可通行区域：以平移后的各条切线为分界线所划分的两侧区域中不包含移动障碍物的一侧的区域作为可行区域，将所有可行区域求交集获得最终的各个可通行区域；这样移动障碍物的线性约束会构成若干个可通行区域，取自身车辆前方沿全局轨迹的固定位置作为参考点，根据全局轨迹选取可通行区域中各条切线到参考点的距离的平均值最小所对应的可通行区域为最佳的可通行区域，即作为行驶区域，构成行驶区域中的切线作为障碍物约束，实际为运动规划的线性约束。

实施结果如图3中所示，在此情况下，标号为2的区间会作为该时刻的约束求解空间，构成该空间的两条切线，作为该情况下两个线性约束。

全局轨迹是由车辆所行驶的地图和目的地进行导航事先确定获得。

当约束生成器接收到优先避让控制信号时，需要紧急停车，在此情况下，约束生成器为自身车辆生成减速约束，具体是根据横向冲突发生的位置生成沿自身车辆的当前行驶方向所在的直线作为一条匀减速轨迹，并作为约束，使自身车辆在距离横向冲突发生的位置前的预设安全距离处停止；若无法在预设安全距离前停止，则为自身车辆规划按最大减速度减速轨迹，同时在距离横向冲突发生的位置前的预设安全距离处为车辆设置停止墙约束；

减速约束表示为：

其中，a表示匀减速的加速度，v表示自身车辆的当前速度，D_s表示自身车辆的当前位置到横向冲突发生的位置之间的距离，D_safe表示预设安全距离，a_min表示最大减速度对应的加速度；

当约束生成器未接收到优先避让控制信号时，不需要紧急停车，在此情况下，不生成减速约束。

步骤204：车辆运动学模型可以采用自行车模型，但不限于此。

建立基于模型预测控制的运动规划器，运动规划器包括车辆运动学模型、目标函数和约束，将车辆运动学模型转化为线性模型，约束包括了车辆运动动力学约束和约束生成器生成的障碍物约束，可能包括了停止墙约束；求解获得了前轮转向角和加速度的控制量，对自身车辆进行自动驾驶的运动规划控制。

优化目标函数为对参考轨迹的跟踪，参考轨迹根据决策模块给出的参考轨迹进行设定。

步骤204中，目标函数表示为：

其中，x(t)表示t时刻的状态量，x(t+k|t)为在t时刻对t+k时刻状态量的估计,r(t+k|t)表示t+k时刻参考状态量,状态量包括自身车辆的位置，速度以及朝向；Δu(t)表示t时刻的控制量的变化量，Δu(t+k|t)表示t+k时刻的控制量的变化量，控制量是由前轮转向角和加速度构成；R，Q为常数构成的权重矩阵；N_p为预测时域，N_c为控制时域。

约束表示为：

u_min＜u(t+k|t)＜u_max

Δu_min＜Δu(t+k|t)＜Δu_max

x_min＜x(t+k|t)＜x_max

Eu+FX≤G

u＝[δ,a]^T

X＝[x,y,θ,v]^T

E＝zeros(2,4)

G＝[B_up,-B_down,I_up,I_down]

其中，u_min和u_max分别表示控制量的最小值限制以及最大值限制；Δu_min，Δu_max分别表示控制量的变化量的最小值限制于最大值限制；u表示控制量，由前轮转向角δ与加速度a构成；X表示车辆状态，由车辆横向位置x,纵向位置y,车辆朝向θ和速度v构成；E，F，G为不等式约束矩阵，E为2*4的零矩阵；k_up为构成行驶区域上边界切线的斜率，k_down为构成行驶区域下边界切线的斜率；I_up为构成行驶区域上边界切线的截距，I_down为构成行驶区域下边界切线的截距；B_up，B_down分别为道路的上下边界。

最后按照将目标函数转化为二次规划(QP)形式，采用二次规划(QP)求解获得控制量u(t)，保证优化求解的速度和实时性能。

本发明方法在仿真平台上进行了验证，共设计三个场景对该方法进行验证，第一个场景为在结构化道路中，该结构化道路由三条车道构成，设置一个横穿道路的移动障碍物，与自身车辆有横向轨迹冲突，两个移动障碍物分别占据一条车道。自身车辆需先进行制动停车，对第一个横穿障碍物进行避让，然后重新启动完成变道，超越两个移动障碍物。该仿真结果如图5所示，该仿真的状态量与控制量如图6所示。第二个场景也为结构化道路场景，该结构化场景由两条车道构成，两条车道上均有移动障碍物，方法需寻找合适的超车机会，完成对移动障碍物的超车行为。仿真结果如图7所示，该仿真的状态量与控制量如图8所示。第三个场景为非结构化道路场景，只有一条车道，设置一个横穿道路的移动障碍物，与自身车辆有横向轨迹冲突，设置一个静态障碍物，自身车辆需先进行制动停车，对第一个横穿障碍物进行避让，然后重新启动完成超越静态障碍物。仿真结果如图9所示，该仿真的状态量与控制量如图10所示。

仿真结果表明，该方法在结构化道路以及非结构化道路中都能较好的完成自身车辆的运动规划，保证通行效率以及安全性，同时，该方法的平均求解时间也远小于100ms，能满足高动态环境下对方法实时性的要求。

记录本方法求解的所花费的时间数据，平均耗时为5ms，最大求解时间为38ms，最小求解时间3ms。该方法所消耗时间远小于对决策规划模块实时性要求100ms，且相对传统方法，在求解效率上有很大提升。

Claims (5)

1.一种基于线性约束的自动驾驶车辆运动规划方法，其特征在于：包括以下步骤：

步骤1：配置无人驾驶车辆及外围设备，获得自身车辆的当前运动状态，移动障碍物的当前运动状态以及静态障碍物位置，当前运动状态包括位置、速度和朝向；

步骤2：

步骤201：根据自身车辆的当前运动状态预测处理获得自身车辆的轨迹，根据移动障碍物的当前运动状态预测处理获得移动障碍物的轨迹，逐一判断自身车辆的轨迹与每个移动障碍物的轨迹是否存在横向冲突，横向冲突的判断方法为在10s时间内自身车辆的轨迹与移动障碍物的轨迹是否在横向方向有相交；

若存在横向冲突，则需要紧急停车，进入步骤202；

若不存在横向冲突，则不需要紧急停车，进入步骤203；

步骤202：通过自身车辆的轨迹与移动障碍物的轨迹之间计算出横向冲突发生的位置和时间，自身车辆发出优先避让控制信号，并将优先避让控制信号以及横向冲突发生的位置和时间传递给约束生成器；进入步骤203；

步骤203：约束生成器实时根据自身车辆所在的位置，移动障碍物的位置和朝向以及静态障碍物的位置，生成车辆运动规划的障碍物约束；

步骤204：建立基于模型预测控制的运动规划器，运动规划器包括车辆运动学模型、目标函数和约束，将车辆运动学模型转化为线性模型，约束包括了车辆运动动力学约束和约束生成器生成的障碍物约束，求解获得了前轮转向角和加速度的控制量，对自身车辆进行自动驾驶的运动规划控制。

2.根据权利要求1所述的一种基于线性约束的自动驾驶车辆运动规划方法，其特征在于：所述步骤1中，外围设备包括：

激光雷达传感器，安装在车辆顶部中心，用于感知自身车辆前方的障碍物；

视觉传感器，安装在车辆前部，用于拍摄车辆前方图像进而对图像识别；

毫米波雷达传感器，安装在车辆前部和左右两个侧方，用于对前方障碍物进行感知；

定位装置安装在车辆中，用于实现车辆定位。

3.根据权利要求1所述的一种基于线性约束的自动驾驶车辆运动规划方法，其特征在于：所述步骤201中，由自身车辆的当前运动状态中的位置、速度和朝向保持不变形成的直线轨迹作为自身车辆的轨迹；移动障碍物的轨迹是由激光雷达传感器所探测的障碍物的当前运动状态后进行分类判断，分为移动障碍物和静态障碍物，然后再对移动障碍物采用预测算法处理获得。

4.根据权利要求1所述的一种基于线性约束的自动驾驶车辆运动规划方法，其特征在于：所述的步骤203中，生成车辆运动规划的障碍物约束，具体为：

将自身车辆用圆心沿同一直线布置且尺寸相同的三个圆相交组成表示，位于两侧的两个圆的圆心在位于中间的圆的圆弧上；将障碍物用矩形表示；移动障碍物的矩形移动障碍物的当前运动状态预测处理获得，静态障碍物的矩形由静态障碍物位置获得；从障碍物矩形的四个顶点分别各自向自身车辆三个圆的每个圆做切线，从所有切线中选取能将移动障碍物与自身车辆分别完整地划分在两侧的切线，每个移动障碍物共获得两条切线；

将两条切线均进行平移，使得切线经过自身车辆的位于车头的圆的圆心，以平移后的各条切线划分可通行区域：以平移后的各条切线为分界线所划分的两侧区域中不包含移动障碍物的一侧的区域作为可行区域，将所有可行区域求交集获得最终的各个可通行区域；取自身车辆前方沿全局轨迹的固定位置作为参考点，根据全局轨迹选取可通行区域中各条切线到参考点的距离的平均值最小所对应的可通行区域为最佳的可通行区域，即作为行驶区域，构成行驶区域中的切线作为障碍物约束；

当约束生成器接收到优先避让控制信号时，需要紧急停车，约束生成器为自身车辆生成减速约束，具体是根据横向冲突发生的位置生成沿自身车辆的当前行驶方向所在的直线作为一条匀减速轨迹，使自身车辆在距离横向冲突发生的位置前的预设安全距离处停止；若无法在预设安全距离前停止，则为自身车辆规划按最大减速度减速轨迹；

减速约束表示为：

其中，a表示匀减速的加速度，v表示自身车辆的当前速度，D_s表示自身车辆的当前位置到横向冲突发生的位置之间的距离，D_safe表示预设安全距离，a_min表示最大减速度对应的加速度；

当约束生成器未接收到优先避让控制信号时，不需要紧急停车，不生成减速约束。

5.根据权利要求1所述的一种基于线性约束的自动驾驶车辆运动规划方法，其特征在于：所述步骤204中，目标函数表示为：

其中，x(t)表示t时刻的状态量，x(t+k|t)为在t时刻对t+k时刻状态量的估计，r(t+k|t)表示t+k时刻参考状态量，状态量包括自身车辆的位置，速度以及朝向；Δu(t)表示t时刻的控制量的变化量，Δu(t+k|t)表示t+k时刻的控制量的变化量，控制量是由前轮转向角和加速度构成；R、Q为第一、第二常数权重矩阵；N_p为预测时域，N_c为控制时域；

约束表示为：

u_min<u(t+k|t)<u_max

Δu_min<Δu(t+k|t)<Δu_max

x_min<x(t+k|t)<x_max

Eu+FX≤G

u＝[δ,a]^T

X＝[x,y,θ,v]^T

E＝zeros(2,4)

G＝[B_up,-B_down,I_up,I_down]

其中，u_min和u_max分别表示控制量的最小值限制以及最大值限制；Δu_min、Δu_max分别表示控制量的变化量的最小值限制于最大值限制；u表示控制量，由前轮转向角δ与加速度a构成；X表示车辆状态，由车辆横向位置x、纵向位置y、车辆朝向θ和速度v构成；E、F、G为第一、第二、第三约束矩阵；k_up为构成行驶区域上边界切线的斜率，k_down为构成行驶区域下边界切线的斜率；I_up为构成行驶区域上边界切线的截距，I_down为构成行驶区域下边界切线的截距；B_up，B_down分别为道路的上下边界；

最后按照将目标函数转化为二次规划(QP)形式，采用二次规划方法求解获得控制量u(t)。

Images