一种路径跟踪控制方法及设备与流程

1.本发明涉及车辆控制技术领域，尤其涉及一种路径跟踪控制方法及设备。


背景技术：

2.随着科技的进步，车辆的自动驾驶技术也得到了快速发展，路径跟踪控制技术是实现车辆自动驾驶的关键技术之一。现有技术中，采用模型预测控制或者线性二次型调节器进行路径跟踪控制，但是在路径曲率不连续且变化较大的场景下控制精准度较差，也不适用于算力有限的量产实车控制器；现有技术还采用pid算法、纯追踪算法来进行路径跟踪控制，但是精准度较差。


技术实现要素：

3.本发明实施例的目的是提供一种路径跟踪控制方法及设备，能够通过对预瞄距离进行实时调整补偿，以适应实际行驶过程中不断变化的外部环境约束，适用于算力有限的量产实车控制器，并提高了路径跟踪控制的精准度。
4.为实现上述目的，本发明实施例提供了一种路径跟踪控制方法，包括：
5.基于模糊控制算法，根据获取到的当前车速和当前路径曲率计算得到基准预瞄距离，并根据获取到的车辆行驶偏差和车辆行驶偏差变化率计算得到调整预瞄距离；
6.根据所述调整预瞄距离对所述基准预瞄距离进行补偿计算，得到目标预瞄距离。
7.作为上述方案的改进，所述车辆行驶偏差包括航向角偏差，所述车辆行驶偏差变化率包括航向角偏差变化率，所述调整预瞄距离包括航向角调整预瞄距离。
8.作为上述方案的改进，所述车辆行驶偏差包括横向偏差，所述车辆行驶偏差变化率包括横向偏差变化率，所述调整预瞄距离包括横向调整预瞄距离。
9.作为上述方案的改进，还包括：
10.根据获取的执行器响应时间、预设角速度、当前预瞄转角、当前转角和所述当前车速，计算得到最小预瞄距离；
11.当所述目标预瞄距离小于所述最小预瞄距离时，将所述最小预瞄距离作为新的目标预瞄距离。
12.作为上述方案的改进，还包括：
13.根据所述目标预瞄距离和获取的参考轨迹，计算得到目标预瞄点；
14.根据获取的轴距、车辆当前姿态到所述目标预瞄点的朝向角和所述目标预瞄距离，计算基准预瞄转角；
15.根据所述参考轨迹、所述目标预瞄点和所述轴距，计算前馈转角；
16.根据所述前馈转角对所述基准预瞄转角进行修正，得到目标预瞄转角。
17.作为上述方案的改进，所述目标预瞄转角通过以下方式计算：
18.δ(t)＝(1-k)*δ
基准
k*δ0；
19.其中，δ(t)表示所述目标预瞄转角，k表示前馈系数，δ
基准
表示所述基准预瞄转角，
δ0表示所述前馈转角。
20.作为上述方案的改进，还包括：
21.将所述参考轨迹划分为前期参考轨迹和后期参考轨迹；
22.则，当所述当前位置在所述前期参考轨迹中时，所述前馈系数为第一前馈系数；当所述当前位置在所述后期参考轨迹中时，所述前馈系数为第二前馈系数；其中，所述第一前馈系数小于所述第二前馈系数。
23.作为上述方案的改进，还包括：
24.当检测到车辆换挡时，采用运动协调控制策略进行车辆控制；
25.所述运动协调控制策略具体包括：
26.控制所述当前车速为预设爬行速度，并根据预设的上限方向盘转速，控制前轮转角为所述当前预瞄转角；
27.当所述前轮转角为所述当前预瞄转角时，退出运动协调控制策略。
28.作为上述方案的改进，所述方法应用于自动泊车辅助系统、遥控泊车辅助系统、自动代客泊车系统和自动下线系统中的至少一种。
29.为实现上述目的，本发明实施例还提供了一种路径跟踪控制设备，包括处理器、存储器以及存储在所述存储器中且被配置为由所述处理器执行的计算机程序，所述处理器执行所述计算机程序时实现如上述任一实施例所述的路径跟踪控制方法。
30.与现有技术相比，本发明实施例公开的路径跟踪控制方法及设备，基于模糊控制算法，通过根据获取当前车速和当前路径曲率来计算基准预瞄距离，通过根据获取到的车辆行驶偏差和车辆行驶偏差变化率来计算得到调整预瞄距离，进而根据调整预瞄距离对基准预瞄距离进行补偿计算，以得到目标预瞄距离。由此可知，本发明实施例考虑了车辆行驶偏差，以对预瞄距离进行实时的调整补偿，以适应实际行驶过程中不断变化的外部环境约束，适用于算力有限的量产实车控制器，并提高了路径跟踪控制精度。
附图说明
31.图1是本发明一实施例提供的一种路径跟踪控制方法的流程图；
32.图2是本发明一实施例提供的一种模糊控制器框架图；
33.图3是本发明一实施例提供的另一种路径跟踪控制方法的流程图；
34.图4是本发明一实施例提供的一种低速泊车场景路径跟踪的示意图；
35.图5是本发明一实施例提供的一种低速avp场景路径跟踪的示意图；
36.图6是本发明一实施例提供的一种自动下线场景路径跟踪的示意图；
37.图7是本发明一实施例提供的一种低速泊车场景轨迹跟踪偏差的曲线图；
38.图8是本发明一实施例提供的一种低速avp场景轨迹跟踪偏差的曲线图；
39.图9是本发明一实施例提供的一种低速hvp场景轨迹跟踪偏差的曲线图；
40.图10是本发明实施例提供的一种路径跟踪控制设备的结构示意图。
具体实施方式
41.下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于
本发明中的实施例，本领域普通技术人员在没有作出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。
42.参见图1，是本发明一实施例提供的一种路径跟踪控制方法的流程图，所述路径跟踪控制方法包括：
43.s1、基于模糊控制算法，根据获取到的当前车速和当前路径曲率计算得到基准预瞄距离，并根据获取到的车辆行驶偏差和车辆行驶偏差变化率计算得到调整预瞄距离；
44.s2、根据所述调整预瞄距离对所述基准预瞄距离进行补偿计算，得到目标预瞄距离。
45.值得说明的是，参考轨迹由一系列参考点组成，因此以距离车辆最近的参考点的路径曲率作为车辆所在位置的当前路径曲率。
46.具体地，步骤s1中的当前路径曲率是通过根据获取的参考轨迹和车辆的当前位置计算得到的，具体为：根据车辆的当前位置在参考轨迹上找到距离最近的参考点，获取该参考点的曲率信息。
47.具体地，通过设置主模糊控制器和副模糊控制器，来进行预瞄距离的计算。将获取的车辆的当前车速和当前路径曲率输入到主模糊控制器中，基于模糊控制算法，计算得到基准预瞄距离，以当前车速和当前路径曲率作为输入量，保证计算得到的基准预瞄距离能够适应车辆当前的行驶状态以及参考轨迹的特征；进一步地，将获取的车辆行驶偏差和车辆行驶偏差变化率输入到副模糊控制器中，基于模糊控制算法，计算调整预瞄距离，以用来对基准预瞄距离进行补偿，使得车辆行驶偏差和车辆行驶偏差变化率趋于平稳状态，适应实际行驶过程中不断变化的外部环境约束，提高了路径跟踪控制精度，同时对于实车控制器算力资源的要求较小，适用于算力有限的量产实车控制器。
48.优选的，该方法适用于低速场景下的路径跟踪控制。示例性的，低速场景指的是车辆行驶速度为15km/h以下的场景，例如自动泊车辅助(auto parking asist，apa)、远程遥控泊车辅助(remote parking asist，rpa)、自动代客泊车(automated valet parking，avp)、家庭区域泊车辅助(home zone parking assist，hpa)、家庭区域记忆式泊车(hzp)、自动下线(hvp)以及园区低速自动驾驶等场景。
49.与现有技术相比，本发明实施例考虑了车辆行驶偏差，以对预瞄距离进行实时的调整补偿，以适应实际行驶过程中不断变化的外部环境约束，适用于算力有限的量产实车控制器，并提高了路径跟踪控制精度。
50.在一种实施方式中，所述车辆行驶偏差包括航向角偏差，所述车辆行驶偏差变化率包括航向角偏差变化率，所述调整预瞄距离包括航向角调整预瞄距离。
51.具体地，在预瞄距离计算过程中，车辆行驶偏差可以是航向角偏差，车辆行驶偏差变化率为航向角偏差变化率，将航向角偏差和航向角偏差变化率输入到副模糊控制器，利用模糊控制算法，计算出航向角调整预瞄距离，以航向角调整预瞄距离对基准预瞄距离进行补偿，当航向角偏差以及航向角偏差变化率较大时，需要及时修正预瞄距离，适当增大预瞄距离，以降低航向角偏差和航向角偏差变化率，减小车辆姿态振荡。
52.具体地，航向角偏差和航向角偏差变化率通过以下方式计算得到：根据实时获取的车辆的当前航向角和参考轨迹计算得到航向角偏差，计算航向角偏差在单元时间内的变化以得到航向角偏差变化率；其中，当前航向角表示车辆的本地坐标系的x轴与预设的定位
坐标系的x轴的夹角，定位坐标系为相对于大地坐标系的一个局部坐标系，定位坐标系在路径跟踪控制之前设定并在路径跟踪控制过程中保持不变。
53.在又一种实施方式中，所述车辆行驶偏差包括横向偏差，所述车辆行驶偏差变化率包括横向偏差变化率，所述调整预瞄距离包括横向调整预瞄距离。
54.具体地，在预瞄距离计算过程中，车辆行驶偏差可以是横向偏差，车辆行驶偏差变化率为横向偏差变化率，将横向偏差和横向偏差变化率输入到副模糊控制器，利用模糊控制算法，计算出横向调整预瞄距离，以横向调整预瞄距离对基准预瞄距离进行补偿，当横向偏差以及横向偏差变化率较大时，需要及时修正预瞄距离，适当缩小预瞄距离，以尽快减小横向偏差。
55.具体地，横向偏差指的是实时获取的车辆定位的实际位置和离车辆最近的参考点的位置在车辆前进方向的垂直方向上的距离偏差，计算横向偏差在单元时间内的变化以得到横向偏差变化率。
56.值得说明的是，可同时利用航向角调整预瞄距离和横向调整预瞄距离来调整基准预瞄距离，以得到目标预瞄距离。
57.示例性的，目标预瞄距离的计算公式为：d＝k1*d1 k2*d2 k3*d3，其中，k1、k2、k3分别表示基准系数、横向系数、航向角系数，d1、d2、d3分别表示基准预瞄距离、横向调整预瞄距离、航向角调整预瞄距离，可选的，k1＝k3＝1，k2＝-1，具体的系数取值可根据实际情况设定，不局限于上述具体数值。
58.具体的模糊控制过程可参见图2所示的模糊控制器框架。
59.在又一种实施方式中，还包括步骤s3～s4：
60.s3、根据获取的执行器响应时间、预设角速度、当前预瞄转角、当前转角和所述当前车速，计算得到最小预瞄距离；
61.s4、当所述目标预瞄距离小于所述最小预瞄距离时，将所述最小预瞄距离作为新的目标预瞄距离。
62.具体地，执行器响应时间为控制方向盘转角变化的执行器的响应时间；可选的，预设角速度可以是方向盘转动的最大角速度，由车辆本身的性能决定，也可以是用户设定的角速度，可以理解的，用户设定的角速度小于等于最大角速度；所述最小预瞄距离通过以下公式计算得到：
63.d0＝t*u abs(a-a0)/w*u；
64.其中，d0表示所述最小预瞄距离，t表示所述执行器响应时间，u表示当前车速，abs表示绝对值，a表示目标方向盘转角，a0表示当前方向盘转角，w表示所述预设角速度。所述目标方向盘转角通过根据所述当前预瞄转角计算得到，所述当前方向盘转角通过根据所述当前转角计算得到。
65.可以理解的，最小预瞄距离考虑了执行器响应时间、方向盘转角时间(方向盘转角由当前方向盘转角到目标方向盘转角所需的时间)以及当前车速，最小预瞄距离对目标预瞄距离进行限幅，当目标预瞄距离小于最小预瞄距离时，将最小预瞄距离作为新的目标预瞄距离，保证最终的目标预瞄距离不小于执行器响应控制需求所需要的行驶距离，从而保证跟踪控制指令的有效执行。
66.在又一种实施方式中，还包括步骤s5～s8：
67.s5、根据所述目标预瞄距离和获取的参考轨迹，计算得到目标预瞄点；
68.s6、根据获取的轴距、车辆当前姿态到所述目标预瞄点的朝向角和所述目标预瞄距离，计算基准预瞄转角；
69.s7、根据所述参考轨迹、所述目标预瞄点和所述轴距，计算前馈转角；
70.s8、根据所述前馈转角对所述基准预瞄转角进行修正，得到目标预瞄转角。
71.具体地，步骤s7中的根据所述参考轨迹、所述目标预瞄点和所述轴距，计算前馈转角，具体包括步骤s71～s72：
72.s71、根据所述参考轨迹和所述目标预瞄点，计算预瞄路径曲率；
73.s72、根据所述轴距、所述预瞄路径曲率和所述目标预瞄点，计算前馈转角。
74.具体地，根据目标预瞄距离在获取的参考轨迹中找到目标预瞄点，获取车辆当前姿态到目标预瞄点的朝向角(目标预瞄点与车辆的当前位置的连线和与车辆的本地坐标系的x轴的夹角，即目标预瞄点与车辆的当前位置的连线与车辆中轴线的夹角)，从车辆的配置文件中获取轴距(轴距是指前轴(前轮轮轴)到后轴(后轮轮轴)的距离)，根据轴距、车辆当前姿态到目标预瞄点的朝向角和目标预瞄距离，计算基准预瞄转角；计算参考轨迹中的目标预瞄点的曲率，得到预瞄路径曲率，根据轴距、预瞄路径曲率和目标预瞄点，计算前馈转角，具体的前馈转角的计算方式可参考现有技术，在此不再赘述。
75.具体地，步骤s6中的基准预瞄转角的计算公式如下：
[0076][0077]
其中，δ
基准
表示基准预瞄转角，l表示车辆的轴距，α(t)表示实时获取的车辆当前姿态到目标预瞄点的朝向角，d表示目标预瞄距离。
[0078]
在一种实施方式中，所述目标预瞄转角通过以下方式计算：
[0079]
δ(t)＝(1-k)*δ
基准
k*δ0；
[0080]
其中，δ(t)表示所述目标预瞄转角，k表示前馈系数，δ
基准
表示所述基准预瞄转角，δ0表示所述前馈转角。
[0081]
进一步地，所述方法还包括：将所述参考轨迹划分为前期参考轨迹和后期参考轨迹；
[0082]
则，当所述当前位置在所述前期参考轨迹中时，所述前馈系数为第一前馈系数；当所述当前位置在所述后期参考轨迹中时，所述前馈系数为第二前馈系数；其中，所述第一前馈系数小于所述第二前馈系数。
[0083]
具体地，为了提高路径跟踪初期的跟踪精度，尽可能减小跟踪偏差，并在跟踪后期提高车辆姿态稳定，在跟踪初期采用较小的前馈系数(其中，跟踪前期的前馈系数还与预瞄点处的路径曲率有关)，在跟踪后期根据预瞄点处的路径曲率采用较大的前馈系数，减小停车段路径的位姿不稳定性。
[0084]
示例性的，将参考轨迹划分为前期参考轨迹和后期参考轨迹，前期参考轨迹为车辆从开始跟踪到跟踪结束前5米以内的轨迹，后期参考轨迹为停车前距离终点5米以内的轨迹，在前期参考轨迹中跟踪时，前馈系数为0.5，在后期参考轨迹中跟踪时，前馈系数为0.8。
[0085]
值得说明的是，参考轨迹的划分并不局限于上述具体的划分，前馈系数的取值也不局限于上述具体数值，可根据实际情况进行设定。
[0086]
在又一种实施方式中，所述方法还包括步骤s9：
[0087]
s9、当检测到车辆换挡时，采用运动协调控制策略进行车辆控制；
[0088]
所述运动协调控制策略具体包括步骤s91～s92：
[0089]
s91、控制所述当前车速为预设的爬行速度，并根据预设的上限方向盘转速，控制车辆的前轮转角为所述当前预瞄转角；
[0090]
s92、当所述前轮转角为所述当前预瞄转角时，退出运动协调控制策略。并将所述车辆的车速调整为预设车速。
[0091]
值得说明的是，对于车辆换挡操作，为了保证跟踪精度提前打方向盘，同时减少轮胎与地面磨损，并保证控制的流畅性，在检测到车辆换挡时，采用运动协调控制策略作为过渡性的控制策略来对车辆进行控制。
[0092]
示例性的，当检测到车辆换挡时，控制车辆的当前车速为预设的爬行速度(一般情况下，爬行速度不超过1km/h)，并用预设的方向盘转速控制车辆的前轮转角为目标转角(当前预瞄转角)，当前轮转角为目标转角时退出运动协调控制策略；其中，预设的方向盘转速一般被设置为方向盘所能达到的最大转速。
[0093]
值得说明的是，上述具体实施例的步骤过程还可以参考图3。
[0094]
值得说明的是，所述路径跟踪控制方法可以在快速原型开发控制器autobox中实施，也可以在mcu中实施，如tc297，tc397等。为了进一步说明所述路径跟踪控制方法的路径跟踪控制效果，对本方案进行matlab/simulink仿真及实车测试，参见图4～图9，图4是低速泊车场景路径跟踪的示意图，图5是低速avp场景路径跟踪的示意图，图6是自动下线(hvp)场景路径跟踪的示意图，图7是低速泊车场景轨迹跟踪偏差的曲线图，图8是低速avp场景轨迹跟踪偏差的曲线图，图9是低速hvp场景轨迹跟踪偏差的曲线图，从中可以看出，三类低速场景下的轨迹整体跟踪偏差较小，整体横向偏差低于15cm，航向角偏差低于5度，停车横向偏差低于5cm，停车航向角偏差低于2度，相比目前各场景下的实际跟踪误差有了明显减小，大大提高了低速场景下的路径跟踪控制精度。
[0095]
所述方法应用于自动泊车辅助系统、遥控泊车辅助系统、自动代客泊车系统和自动下线系统中的至少一种。
[0096]
与现有技术相比，本发明实施例公开的路径跟踪控制方法，基于模糊控制算法，通过根据获取当前车速和当前路径曲率来计算基准预瞄距离，通过根据获取到的车辆行驶偏差和车辆行驶偏差变化率来计算得到调整预瞄距离，进而根据调整预瞄距离对基准预瞄距离进行补偿计算，以得到目标预瞄距离。由此可知，本发明实施例考虑了车辆行驶偏差，以对预瞄距离进行实时的调整补偿，以适应实际行驶过程中不断变化的外部环境约束，适用于算力有限的量产实车控制器，并提高了路径跟踪控制精度。
[0097]
参见图10，图10是本发明实施例提供的一种路径跟踪控制设备的结构示意图。所述路径跟踪控制设备包括处理器21、存储器22以及存储在所述存储器中且被配置为由所述处理器执行的计算机程序，例如路径跟踪控制程序。所述处理器21执行所述计算机程序时实现上述路径跟踪控制方法实施例中的步骤，例如图1中所示的步骤s1～s2。
[0098]
所述路径跟踪控制设备可以是桌上型计算机、笔记本、掌上电脑及云端服务器等计算设备。所述路径跟踪控制设备可包括，但不仅限于，处理器21、存储器22。本领域技术人员可以理解，所述示意图仅仅是路径跟踪控制设备的示例，并不构成对路径跟踪控制设备
的限定，可以包括比图示更多或更少的部件，或者组合某些部件，或者不同的部件，例如所述路径跟踪控制设备还可以包括输入输出设备、网络接入设备、总线等。
[0099]
所述处理器21可以是中央处理单元(central processing unit，cpu)，还可以是其他通用处理器、数字信号处理器(digital signal processor，dsp)、专用集成电路(application specific integrated circuit，asic)、现成可编程门阵列(field-programmable gate array，fpga)或者其他可编程逻辑器件、分立门或者晶体管逻辑器件、分立硬件组件等。通用处理器可以是微处理器或者该处理器也可以是任何常规的处理器等，所述处理器21是所述路径跟踪控制设备的控制中心，利用各种接口和线路连接整个路径跟踪控制设备的各个部分。
[0100]
所述存储器22可用于存储所述计算机程序和/或模块，所述处理器21通过运行或执行存储在所述存储器22内的计算机程序和/或模块，以及调用存储在存储器22内的数据，实现所述路径跟踪控制设备的各种功能。所述存储器22可主要包括存储程序区和存储数据区，其中，存储程序区可存储操作系统、至少一个功能所需的应用程序等；存储数据区可存储根据控制器的使用所创建的数据等。此外，存储器22可以包括高速随机存取存储器，还可以包括非易失性存储器，例如硬盘、内存、插接式硬盘，智能存储卡(smart media card,smc)，安全数字(secure digital,sd)卡，闪存卡(flash card)、至少一个磁盘存储器件、闪存器件、或其他易失性固态存储器件。
[0101]
其中，所述路径跟踪控制设备集成的模块如果以软件功能单元的形式实现并作为独立的产品销售或使用时，可以存储在一个计算机可读取存储介质中。基于这样的理解，本发明实现上述实施例方法中的全部或部分流程，也可以通过计算机程序来指令相关的硬件来完成，所述的计算机程序可存储于一计算机可读存储介质中，该计算机程序在被处理器执行时，可实现上述各个方法实施例的步骤。其中，所述计算机程序包括计算机程序代码，所述计算机程序代码可以为源代码形式、对象代码形式、可执行文件或某些中间形式等。所述计算机可读介质可以包括：能够携带所述计算机程序代码的任何实体或装置、记录介质、u盘、移动硬盘、磁碟、光盘、计算机存储器、只读存储器(rom，read-only memory)、随机存取存储器(ram，random access memory)、电载波信号、电信信号以及软件分发介质等。
[0102]
以上所述是本发明的优选实施方式，应当指出，对于本技术领域的普通技术人员来说，在不脱离本发明原理的前提下，还可以做出若干改进和润饰，这些改进和润饰也视为本发明的保护范围。