本发明涉及无人驾驶汽车轨迹跟踪控制技术领域，特别涉及一种考虑扰动的无人驾驶框架车轨迹跟踪控制方法。
背景技术：
在港口运输领域，由于货物运送量较大，往往需要耗费大量人力操作框架车进行货物运输，导致运送成本居高不下。与此同时，港口的货物运输工况具有往返路程固定、避障要求较低等特点，近些年随着无人驾驶技术的发展，采用基于磁钉定位的无人驾驶框架车进行港口货物自动化运输已成为了当下发展方向。
目前，在进行无人驾驶汽车的控制研究时，通常假设车辆的定位准确，控制过程中不考虑定位误差与外界扰动的影响。然而，由于外界天气的干扰以及磁钉老化等原因，在实际的使用中部分工况下车辆定位会发生失准现象；此外，从增大单次货物运输量的角度考虑，目前港口运输使用的无人驾驶框架车大部分为多轴驱动构型，轴距较长，进而导致整车易受到外界干扰。因此，无论是定位误差干扰或是环境的干扰，均给无人驾驶框架车的轨迹跟踪控制效果带来影响。
技术实现要素：
为了解决无人驾驶框架车在有定位与环境干扰下的轨迹跟踪控制，本发明提供一种考虑扰动的无人驾驶框架车轨迹跟踪控制方法。
该方法具体包括以下步骤：
步骤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)，并把当前相对坐标系下预瞄点的横坐标xt作为横向误差dx；
dx＝xt(2)
所述步骤3中车速误差计算，具体方法为获取当前预瞄点的期望车速ve_des，并把期望车速ve_des与当前车速ve的差值作为车速误差dv：
dv＝ve_des-ve(3)
步骤4、考虑扰动的跟踪误差修正，包括车速修正与考虑外界扰动的轨迹修正,通过控制系统的踏板开度以及转向角减小跟踪误差；
所述步骤4中车速误差修正设计，具体方法为将步骤3中计算得到的车速误差dv作为反馈误差e输入给pid控制器，得到当前偏差下的踏板开度决策：
其中ev为车速误差、kp为pid比例系数、ki为pid积分系数、kd为pid微分系数，pedal为踏板开度，其值为正代表驱动踏板的踏板开度值(此时制动踏板开度为零)，其值为负代表制动踏板的踏板开度值(此时驱动踏板开度为零)；
所述步骤4中考虑外界扰动的轨迹修正，具体步骤包括快速跟踪微分器建立、扩展状态观测器建立以及考虑扰动的线性组合控制，得到考虑扰动的转向角控制量；其具体实施步骤包括：
步骤4a-1、建立快速跟踪微分器，根据步骤3中计算得到的横向误差dx，结合当前车辆在大地坐标系下的横坐标xv得到期望跟踪值，采用快速跟踪微分器对期望跟踪值进行处理，防止其微分超调；快速跟踪微分器离散型控制函数如下所示：
其中k为离散的时刻点，v0为期望跟踪横坐标，v1为经过快速跟踪微分器处理的期望跟踪横坐标值、v2为经过快速跟踪微分器处理的期望跟踪横坐标微分值、fh为fhan函数的计算值，r0为fhan函数速度因子，h为fhan函数滤波因子，fhan函数的计算式如下所示：
步骤4a-2、建立扩张状态观测器，通过系统当前状态，对系统的扰动项进行观测，其离散计算式如下：
其中ex为横向距离观测值偏差，z1为车辆横坐标观测值、z2为车辆横坐标微分观测值、z3为车辆横坐标扰动观测值，β01、β02与β03为标准正定参数，b0为补偿因子，u为上一时刻的转向角控制量；
步骤4a-3、考虑扰动的线性组合控制，结合步骤4a-2中计算的扩张状态观测器得到的观测值与步骤4a-1中计算得到的快速跟踪微分器处理值进行线性组合，得到考虑扰动的转向角控制量，其组合率如下式所示：
其中e1为车辆横坐标偏差，e2为车辆横坐标微分偏差，e3为车辆横坐标累积误差，k1、k2、k3为基于误差的控制系数，b0为扰动补偿系数，u为考虑扰动的转向角控制量；
将步骤4中计算得到的踏板开度量与考虑扰动的转向角控制量u作为无人驾驶框架车当前时刻的控制量，进而实现轨迹跟踪控制。
本发明与现有技术相比的有益效果为：本发明考虑了外界干扰对无人驾驶框架车的控制影响，能够保证框架车在有干扰环境下的精确轨迹跟踪控制；此外，该控制方法控制流程简洁，易于工程实现。
附图说明
图1为本发明所述的一种考虑扰动的无人驾驶框架车轨迹跟踪控制方法示意图；
图2为本发明所述的相对坐标系转换示意图；
图3为本发明所述的考虑扰动的转向角控制决策示意图。
具体实施方式
下面结合附图对本发明作详细的描述。
参阅附图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；
参阅附图2，所述步骤3中横向偏差dx计算包括以下步骤：
步骤3a-1、进行相对坐标变化，以当前车辆在大地坐标系下的横坐标xv、纵坐标yv为相对坐标原点o，当前车辆在大地坐标系下的航向方向θ为相对y坐标轴，建立二维右手相对坐标系oxy；若当前车辆在大地坐标系下的坐标为(xv,yv)，预瞄点在绝对坐标系下的坐标为(xt,yt)，其预瞄点在相对坐标系下的坐标(xt,yt)转换计算式为：
步骤3a-2、计算预瞄点在相对坐标系下的坐标(xt,yt)，并把当前相对坐标系下预瞄点的横坐标xt作为横向误差dx；
dx＝xt(3)
所述步骤3中车速误差计算，通过获取当前预瞄点的期望车速ve_des，并把期望车速ve_des与当前车速ve的差值作为车速误差dv：
dv＝ve_des-ve(4)
步骤4、考虑扰动的跟踪误差修正，包括车速修正与考虑外界扰动的轨迹修正,通过控制系统的踏板开度以及转向角减小跟踪误差；
所述步骤4中车速误差修正设计，具体步骤为将步骤3中计算得到的车速误差dv作为反馈误差e输入给pid控制器，得到当前偏差下的踏板开度决策：
其中ev为车速误差、kp为pid比例系数、ki为pid积分系数、kd为pid微分系数，pedal为踏板开度，其值为正代表驱动踏板的踏板开度值(此时制动踏板开度为零)，其值为负代表制动踏板的踏板开度值(此时驱动踏板开度为零)；
参阅附图3，所述步骤4中考虑外界扰动的轨迹修正，其具体步骤包括快速跟踪微分器建立、扩展状态观测器建立以及考虑扰动的线性组合控制，得到考虑扰动的转向角控制量；其具体实施步骤包括：
步骤4a-1、建立快速跟踪微分器，根据步骤3中计算得到的横向误差dx，结合当前车辆在大地坐标系下的横坐标xv得到期望跟踪值，采用快速跟踪微分器对期望跟踪值进行处理，防止其微分超调；快速跟踪微分器离散型控制函数如下所示：
其中k为离散的时刻点，v0为期望跟踪横坐标，v1为经过快速跟踪微分器处理的期望跟踪横坐标值、v2为经过快速跟踪微分器处理的期望跟踪横坐标微分值、fh为fhan函数的计算值，r0为fhan函数速度因子，h为fhan函数滤波因子，fhan函数的计算式如下所示：
步骤4a-2、建立扩张状态观测器，通过系统当前状态，对系统的扰动项进行观测，其离散计算式如下：
其中ex为横向距离观测值偏差，z1为车辆横坐标观测值、z2为车辆横坐标微分观测值、z3为车辆横坐标扰动观测值，β01、β02与β03为标准正定参数，b0为补偿因子，u为上一时刻的转向角控制量；
步骤4a-3、考虑扰动的线性组合控制，结合步骤4a-2中计算的扩张状态观测器得到的观测值与步骤4a-1中计算得到的快速跟踪微分器处理值进行线性组合，得到考虑扰动的转向角控制量，其组合率如下式所示：
其中e1为车辆横坐标偏差，e2为车辆横坐标微分偏差，e3为车辆横坐标累积误差，k1、k2、k3为基于误差的控制系数，b0为补偿因子，u为考虑扰动的转向角控制量；
将步骤4中计算得到的踏板开度量与考虑扰动的转向角控制量u作为无人驾驶框架车当前时刻的控制量，进而实现轨迹跟踪控制。
本发明中未述及的部分采用或借鉴已有技术即可实现。
以上所述仅为本发明的一个实施例，并不用于限制本发明，凡在本发明的精神和原则之内，所做的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。
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)，并把当前相对坐标系下预瞄点的横坐标xt作为横向误差dx；
dx＝xt(2)
所述步骤3中车速误差计算，具体方法为获取当前预瞄点的期望车速ve_des，并把期望车速ve_des与当前车速ve的差值作为车速误差dv：
dv＝ve_des-ve(3)
步骤4、考虑扰动的跟踪误差修正，包括车速修正与考虑外界扰动的轨迹修正,通过控制系统的踏板开度以及转向角减小跟踪误差；
所述步骤4中车速误差修正设计，具体方法为将步骤3中计算得到的车速误差dv作为反馈误差e输入给pid控制器，得到当前偏差下的踏板开度决策：
其中ev为车速误差、kp为pid比例系数、ki为pid积分系数、kd为pid微分系数，pedal为踏板开度，其值为正代表驱动踏板的踏板开度值(此时制动踏板开度为零)，其值为负代表制动踏板的踏板开度值(此时驱动踏板开度为零)；
所述步骤4中考虑外界扰动的轨迹修正，具体步骤包括快速跟踪微分器建立、扩展状态观测器建立以及考虑扰动的线性组合控制，得到考虑扰动的转向角控制量；其具体实施步骤包括：
步骤4a-1、建立快速跟踪微分器，根据步骤3中计算得到的横向误差dx，结合当前车辆在大地坐标系下的横坐标xv得到期望跟踪值，采用快速跟踪微分器对期望跟踪值进行处理，防止其微分超调；快速跟踪微分器离散型控制函数如下所示：
其中k为离散的时刻点，v0为期望跟踪横坐标，v1为经过快速跟踪微分器处理的期望跟踪横坐标值、v2为经过快速跟踪微分器处理的期望跟踪横坐标微分值、fh为fhan函数的计算值，r0为fhan函数速度因子，h为fhan函数滤波因子，fhan函数的计算式如下所示：
步骤4a-2、建立扩张状态观测器，通过系统当前状态，对系统的扰动项进行观测，其离散计算式如下：
其中ex为横向距离观测值偏差，z1为车辆横坐标观测值、z2为车辆横坐标微分观测值、z3为车辆横坐标扰动观测值，β01、β02与β03为标准正定参数，b0为补偿因子，u为上一时刻的转向角控制量；
步骤4a-3、考虑扰动的线性组合控制，结合步骤4a-2中计算的扩张状态观测器得到的观测值与步骤4a-1中计算得到的快速跟踪微分器处理值进行线性组合，得到考虑扰动的转向角控制量，其组合率如下式所示：
其中e1为车辆横坐标偏差，e2为车辆横坐标微分偏差，e3为车辆横坐标累积误差，k1、k2、k3为基于误差的控制系数，b0为扰动补偿系数，u为考虑扰动的转向角控制量；
将步骤4中计算得到的踏板开度量与考虑扰动的转向角控制量u作为无人驾驶框架车当前时刻的控制量，进而实现轨迹跟踪控制。
技术总结