1.一种基于道路与障碍物建模的模型预测控制车辆避障方法,其特征在于:其步骤是:S1、建立车辆路径跟踪和道路及障碍物的模型:基于阿克曼转向几何与路径曲率前馈补偿,构建了状态变量为横向误差、航向误差及实际转向角的三阶车辆路径跟踪模型;选取横向误差ev和航向误差作为系统的输出,同时选取期望转向角作为系统的控制量,则状态向量,其中T为矩阵的转置,描述车辆路径跟踪模型;建立车辆与车道边界的最小欧几里得距离模型,确保车辆的当前位置始终位于道路的边界范围内;针对场景中出现的静态障碍物、动态行人及车辆等,建立基于实时位置的欧几里得距离模型,确保车辆与障碍物始终保持安全距离;
S2、构建基于道路与障碍物建模的模型预测控制车辆避障方法:将车辆路径跟踪模型离散化,预测时域以内的状态轨迹,得到预测向量,定义系统状态的预测序列、系统预测输出序列和控制输入序列后得到系统状态的预测方程和系统输出的预测方程;设计优化目标函数以实现车辆的全局路径跟踪;
其中优化目标函数为:
(12)
式中,为状态权重矩阵;是基于A*规划的原始路径经过平滑处理后计算得到的前馈转向角序列;为控制权重矩阵;为的权重,为速度的倒数,为的矩阵转置,为系统输出序列;其中,,为时刻的参考转向角,为时刻的参考转向角,为时刻的参考转向角;
S3、为避免将道路边界作为硬约束,设计一种基于Sigmoid函数的连续可微惩罚项:其中连续可微惩罚项包含两个惩罚项:(1)车辆横向位置接近边界的惩罚项:(15)
式中,为指数部分的线性组合为的欧拉数,为道路惩罚系数,为含有车辆与道路间的安全范围内函数的道路惩罚系数;
(2)车辆与障碍物的距离过近惩罚项:(18)
式中,为指数部分的线性组合为的欧拉数,为障碍物惩罚系数,为含有车辆与障碍物间的安全范围内函数的障碍物惩罚系数。
2.根据权利要求1所述的基于道路与障碍物建模的模型预测控制车辆避障方法,其特征在于:所述的三阶车辆路径跟踪模型为:
(1)
(2)
式中,为轴距;为车速;ev为横向偏差,即车辆当前位置到期望路径的垂直距离;为车辆横向误差的变化速率;为航向误差;为航向误差的变化速率;为实际转向角;为转向角的变化速率;为参考转向角,为期望转向角,为转向系统的时间常数;
车辆路径跟踪模型:
(3)
式中,为扰动项;为输出向量,为输出矩阵;为状态变量的瞬时变化速率,为状态矩阵,为控制输入矩阵;
式(3)中各个矩阵分别为:
车辆与车道边界的最小欧几里得距离模型:(4)
式中,为车辆的横向位置;为道路左右边界,为车辆与车道之间的最小距离;为车辆与道路之间的预设安全距离;
车辆与障碍物的实时位置的欧几里得距离模型:(5)
式中,为车辆的纵向位置;为障碍物纵向和横向位置;为车辆与障碍物之间的距离;为车辆与障碍物之间的预设安全距离。
3.根据权利要求1所述的基于道路与障碍物建模的模型预测控制车辆避障方法,其特征在于:离散化后的车辆路径跟踪模型为:
(6)
式中,为时刻的状态量,为时刻的状态量,为时刻的控制量,为时刻系统的输出状态量,为离散状态转移矩阵,为离散控制输入矩阵,为扰动矩阵;
离散状态转移矩阵和离散控制输入矩阵为:;
式中,为单位矩阵,为采样周期;
预测时域以内的状态轨迹为,
(7)
式中,为时刻的状态量,为时刻的控制量,为预测时域后时刻的状态量,为当预测时域为时的离散状态转移矩阵,为当预测时域为时的离散状态转移矩阵,为时刻的控制量,为求和项;
根据状态轨迹得到预测向量:
(8)
式中,为当预测时域为2时的离散状态转移矩阵,为当预测时域为时的离散状态转移矩阵,为时刻的控制量;
定义系统状态的预测序列、系统预测输出序列和控制输入序列分别为:(9)
式中,为采样时刻对下一时刻的系统状态向量的预测值,为在采样时刻对下一时刻的系统输出向量的预测矩阵,为时刻的预测输出值,为时刻的预测输出值,为预测时域后时刻的预测输出值,为控制输入序列;
系统状态的预测方程和系统输出的预测方程为:(10)
(11)
式中,、、和分别为预测时域内的状态转移矩阵、控制输入矩阵、扰动矩阵和输出矩阵;
状态转移矩阵、控制输入矩阵、扰动矩阵和输出矩阵如下:;
由此获得优化目标函数。
4.根据权利要求1所述的基于道路与障碍物建模的模型预测控制车辆避障方法,其特征在于:所述连续可微惩罚项的设计过程为:当车辆横向位置接近边界时,惩罚项定义为:(13)
(14)
式中,为输入值是时的Sigmoid函数,为道路边界惩罚函数,为判断车辆与道路之间的距离是否处于安全范围内的函数,为道路惩罚项;
将车辆与道路间的安全范围内的函数代入道路惩罚项获得车辆横向位置接近边界的惩罚项;
当车辆与障碍物的距离过近时,惩罚项定义为:(16)
(17)
式中,为障碍物距离惩罚函数;为判断车辆与障碍物之间的距离是否处于安全范围内的函数,为车辆与障碍物之间的预设安全距离,为障碍物惩罚项,为输入值是时的Sigmoid函数;
将车辆与障碍物间的安全范围内的函数代入障碍物惩罚项获得车辆与障碍物的距离过近惩罚项;
在优化目标函数中加入道路边界惩罚和障碍物惩罚后得到代价目标函数:(19) ;
将代价目标函数转换为标准二次规划形式:(20)
式中,为二次项系数矩阵,为一次项系数向量;
式(20)中二次项系数矩阵和一次项系数向量矩阵如下:;
式中,为预测时域内的输出矩阵与控制输入矩阵相乘后的矩阵转置,和分别为的结果矩阵和的矩阵转置,和分别为参考转向角矩阵和的矩阵转置,为道路边界惩罚函数的矩阵转置,为障碍物距离惩罚函数的矩阵转置;
已知时刻的状态量和前一时刻的控制量,通过求解公式(20)的QP问题得到控制时域内的最优控制量序列:(21)
式中,为在当前采样时刻计算出的最优控制量,为当前采样时刻计算出的下一时刻的最优控制量,为当前采样时刻计算出的时刻的最优控制量。