[发明专利]一种考虑扰动的无人驾驶框架车轨迹跟踪控制方法

权利要求书

1.一种考虑扰动的无人驾驶框架车轨迹跟踪控制方法，其特征在于，该方法包括期望跟踪轨迹预处理、预瞄点搜索、轨迹跟踪误差计算和考虑扰动的跟踪误差修正；期望跟踪轨迹预处理用于获取跟踪点系与确定跟踪点信息；预瞄点搜索通过求解预瞄距离，并搜索预瞄距离内跟踪点，进而得到期望预瞄点；轨迹跟踪误差计算通过获取当前的坐标以及期望预瞄点，计算得到当前的车速偏差与横向偏差；考虑扰动的跟踪误差修正通过获取车速偏差与横向偏差，求解出当前的踏板开度与转向角控制量，进而控制框架车实现轨迹跟踪控制；

该方法包括以下步骤：

步骤1、期望跟踪轨迹预处理，包括期望轨迹离散化与跟踪信息设计；

所述步骤1中期望轨迹离散化具体通过将期望跟踪轨迹离散化，得到离散的跟踪点系；

所述步骤1中跟踪信息设计通过确定跟踪点系下每一个跟踪点p的轨迹坐标、期望车速和跟踪顺序，进而确定每一个跟踪点p的跟踪信息(Xp,Yp,ve_des,seq)，其中Xp为跟踪点在大地坐标系OXY下的横坐标、Yp为跟踪点在大地坐标系OXY下的纵坐标、ve_des为跟踪点的期望跟踪车速、seq为跟踪点通过顺序；

步骤2、预瞄点搜索，包括预瞄距离计算与预瞄点搜寻，进而得到当前无人驾驶框架车的期望跟踪点；

所述步骤2中预瞄距离计算具体步骤包括：

步骤2A-1、通过传感器获取当前车速ve；

步骤2A-2、根据预瞄距离S与车速ve的关系式计算预瞄距离S：

所述步骤2中预瞄点搜寻具体步骤包括：

步骤2B-1、通过磁钉定位得到当前车辆在大地坐标系下的坐标位置(Xv,Yv)；

步骤2B-2、以车辆当前位置为中心，获取步骤2A-2中计算的得到的预瞄距离S，搜寻预瞄距离S内的所有跟踪点；

步骤2B-3、将步骤2B-2中搜寻到的所有跟踪点中通过顺序seq最靠后的跟踪点作为当前预瞄点；

步骤3、轨迹跟踪误差计算，包括横向误差计算与车速误差计算，根据步骤2中搜索到的预瞄点，计算横向偏差dx与车速偏差dv；

所述步骤3中横向偏差dx计算包括以下步骤：

步骤3A-1、进行相对坐标变化，以当前车辆在大地坐标系下的横坐标Xv、纵坐标Yv为相对坐标原点o，当前车辆在大地坐标系下的航向方向θ为相对y坐标轴，建立二维右手相对坐标系oxy；

步骤3A-2、计算预瞄点在相对坐标系下的坐标(xt,yt)，并把当前相对坐标系下预瞄点的横坐标x_t作为横向误差dx；

dx＝x_t (2)

所述步骤3中车速误差计算，具体方法为获取当前预瞄点的期望车速ve_des，并把期望车速ve_des与当前车速ve的差值作为车速误差dv：

dv＝ve_des-ve (3)

步骤4、考虑扰动的跟踪误差修正，包括车速修正与考虑外界扰动的轨迹修正,通过控制系统的踏板开度以及转向角减小跟踪误差；

所述步骤4中车速误差修正设计，具体方法为将步骤3中计算得到的车速误差dv作为反馈误差e输入给PID控制器，得到当前偏差下的踏板开度决策：

其中e_v为车速误差、K_p为PID比例系数、K_i为PID积分系数、K_d为PID微分系数，pedal为踏板开度，其值为正代表驱动踏板的踏板开度值，此时制动踏板开度为零；其值为负代表制动踏板的踏板开度值，此时驱动踏板开度为零；

所述步骤4中考虑外界扰动的轨迹修正，具体步骤包括快速跟踪微分器建立、扩展状态观测器建立以及考虑扰动的线性组合控制，得到考虑扰动的转向角控制量；其具体实施步骤包括：

步骤4A-1、建立快速跟踪微分器，根据步骤3中计算得到的横向误差dx，结合当前车辆在大地坐标系下的横坐标Xv得到期望跟踪值，采用快速跟踪微分器对期望跟踪值进行处理，防止其微分超调；快速跟踪微分器离散型控制函数如下所示：

其中k为离散的时刻点，v₀为期望跟踪横坐标，v₁为经过快速跟踪微分器处理的期望跟踪横坐标值、v₂为经过快速跟踪微分器处理的期望跟踪横坐标微分值、fh为fhan函数的计算值，r0为fhan函数速度因子，h为fhan函数滤波因子，fhan函数的计算式如下所示：

fhan(v₁-v₀,v₂,r₀,h)

步骤4A-2、建立扩张状态观测器，通过系统当前状态，对系统的扰动项进行观测，其离散计算式如下：

其中e_x为横向距离观测值偏差，z₁为车辆横坐标观测值、z₂为车辆横坐标微分观测值、z₃为车辆横坐标扰动观测值，β₀₁、β₀₂与β₀₃为标准正定参数，b₀为扰动补偿系数，u为考虑扰动的转向角控制量；

步骤4A-3、考虑扰动的线性组合控制，结合步骤4A-2中计算的扩张状态观测器得到的观测值与步骤4A-1中计算得到的快速跟踪微分器处理值进行线性组合，得到考虑扰动的转向角控制量，其组合率如下式所示：

其中e₁为车辆横坐标偏差，e₂为车辆横坐标微分偏差，e₃为车辆横坐标累积误差，K₁、K₂、K₃为基于误差的控制系数，b₀为扰动补偿系数，u为考虑扰动的转向角控制量；

将步骤4中计算得到的踏板开度量与考虑扰动的转向角控制量u作为无人驾驶框架车当前时刻的控制量，进而实现轨迹跟踪控制。