CN112937580A - 一种目标路径跟踪方法与装置 - Google Patents

Abstract

本发明涉及一种目标路径跟踪方法与装置，属于自动驾驶技术领域，方法包括获取期望跟踪的目标路径，确定至少两个预瞄距离，在所述目标路径中确定各预瞄距离对应的预瞄点；计算每个预瞄点的曲率，分别为各预瞄点的曲率分配曲率权重因子，利用所述曲率权重因子对相应预瞄点的曲率加权后求和，计算得到预瞄点融合之后的融合曲率；利用所述预瞄点融合之后的融合曲率，确定进行目标路径跟踪的方向盘目标转角，按照该方向盘目标转角跟踪所述目标路径，且跟踪目标路径的准确率较高，且方法原理简单、运算量较小。

Description

一种目标路径跟踪方法与装置

技术领域

本发明属于自动驾驶技术领域，具体涉及一种目标路径跟踪方法与装置。

背景技术

轨迹预测与跟踪是车道保持功能中的一个基本控制问题，它要求车辆在指定时间内到达给定的轨迹点。现有技术中，申请公布号为CN109515440A的中国发明专利申请提出了一种基于车速的变权重多点预瞄轨迹跟踪方法，该方法首先选出最佳预瞄距离，将预瞄距离范围均分三点，根据三点预瞄的各点预瞄距离，得到预瞄距离增益与车速的关系，即汽车正前方由近及远的三点预瞄距离权重系数，从而确定一个实时预瞄距离。

现有技术中，申请公布号为CN109214127A的中国发明专利申请提供了一种多点预瞄方法及其多点预瞄装置与目标路径跟踪方法，只考虑了车辆与实际目标路径在以下三个方面的横摆角速度，分别为距离偏差条件下的理想横摆角速度ωso、夹角偏差条件下的理想横摆角速度ωss、弯曲条件下的理想横摆角速度ωsw，最后叠加ωso、ωss、ωsw乘以一个预定系数就得到了车辆的理想方向盘转角δs。该方法的缺点在于当存在的预瞄点较多时，方法的计算会更加复杂，且方法是在设定的理想条件下实现的，目标路径跟踪的准确性较低。

发明内容

本发明的目的是提供一种目标路径跟踪方法与装置，用于实现目标路径跟踪问题。

基于上述目的，一种目标路径跟踪方法的技术方案如下：

获取期望跟踪的目标路径，确定至少两个预瞄距离，在所述目标路径中确定各预瞄距离对应的预瞄点；

计算每个预瞄点的曲率，分别为各预瞄点的曲率分配曲率权重因子，利用所述曲率权重因子对相应预瞄点的曲率加权后求和，计算得到预瞄点融合之后的融合曲率；

利用所述预瞄点融合之后的融合曲率，确定进行目标路径跟踪的方向盘目标转角，按照该方向盘目标转角跟踪所述目标路径。

基于上述目的，一种目标路径跟踪装置的技术方案如下：

包括存储器和处理器，以及存储在所述存储器上并在所述处理器上运行的计算机程序，所述处理器与所述存储器相耦合，所述处理器执行所述计算机程序时实现上述的目标路径跟踪方法。

上述两个技术方案的有益效果是：

本发明利用全新的目标路径跟踪原理，实现了目标路径的准确跟踪，即通过选取多个预瞄距离，在确定出与预瞄距离对应的预瞄点后，计算每个预瞄点的曲率，然后为每个曲率分配一个曲率权重因子，让曲率权重因子乘以对应的曲率之后进行求和，得到最终的融合曲率，利用此融合曲率计算方向盘目标转角，从而实现目标路径的跟踪，且跟踪目标路径的准确率较高，且方法原理简单、运算量较小。

为了提高跟踪目标路径的准确率，进一步的，所述每个预瞄点的曲率为最优曲率，通过以下步骤计算：

根据预瞄点的坐标，计算得到第一曲率；根据预瞄点的偏航角，计算得到第二曲率；分别为第一曲率和第二曲率分配相应的曲率增益因子；利用所述曲率增益因子对相应的第一曲率和第二曲率进行加权后求和，得到所述最优曲率。

具体的，第一曲率的计算式如下：

row1＝2×y/(x²+y²)

其中，row1为第一曲率，x为沿着目标路径下预瞄点的纵向坐标值，y为沿着目标路径下预瞄点的横向坐标值。

第二曲率的计算式如下：

其中，row2为第二曲率，angle为预瞄点的偏航角的正切值，x为沿着目标路径下预瞄点的纵向坐标值。

为了进一步提高跟踪目标路径的准确性，所述曲率权重因子是根据当前车速动态分配得到的，当车辆的当前车速高于设定高值时，距离车辆越远的预瞄点，为该预瞄点的最优曲率分配的最优曲率权重因子越大；当车辆的当前车速低于设定低值时，距离车辆越近的预瞄点，为该预瞄点的最优曲率分配的最优曲率权重因子越大。

所述预瞄距离的计算式如下：

l_i＝t_i*v+s_i

其中，l_i为预瞄点i的预瞄距离，t_i为该预瞄点的预瞄时间，v为车速，s_i为静态预瞄距离。

由于预瞄距离与车速强相关，速度低时，预瞄距离近，速度高时，预瞄距离远；当车速低时，预瞄距离短，跟踪的是离车辆较近的路径点，跟踪精度较高；当车速高时，预瞄距离长，跟踪的是离车辆较远的路径点，跟踪精度会变差，但跟踪稳定性会增大，非常符合驾驶员路径跟踪特性。

上述方向盘目标转角是根据方向盘目标转角与曲率之间的关系计算得到的，方向盘目标转角的关系式如下：

δ＝k_pIrow^bestL+δ₀

其中，δ为方向盘目标转角，I为转向速比，L为轴距，k_p为增益系数，row^best为所有预瞄点融合之后的最优曲率，δ₀为方向盘静态偏差角度。

附图说明

图1是本发明的目标路径跟踪方法流程图；

图2是本发明的预瞄点确定示意图；

图3是本发明的实现预瞄点的最优曲率计算示意图。

具体实施方式

下面结合附图对本发明的具体实施方式作进一步的说明。

方法实施例：

本发明的一种目标路径跟踪方法，如图1所示，包括以下步骤：

1)计算预瞄距离：

本实施例中，预瞄距离与车速强相关，在车速低时，预瞄距离近，车速高时，预瞄距离远，具体的，预瞄距离的计算式如下：

l_i＝t_i*v+s_i

其中，l_i为预瞄点i的预瞄距离，t_i为该预瞄点的预瞄时间，v为车速，s_i为静态预瞄距离(为标定量，主要是为了防止车速接近于零时，预瞄距离太短，造成控制抖动)，i越大，则预瞄距离越远。

2)确定预瞄点：

基于步骤1)求出的预瞄距离，确定对应的预瞄点，确定方式分为以下两种：

a)直线预瞄，目标路径中的纵向坐标X上等于预瞄距离l_i的点即为该预瞄点，如图2中的预瞄点1；

b)弧线预瞄，首先找到离车辆最近的点O，从O点计算目标路径的累积长度，累积长度为预瞄距离l_i的点即为该预瞄点，如图2中的预瞄点2。

3)计算单个预瞄点的最优曲率：

图3中，α为预瞄点偏航角，β为偏航角(α、β均表示偏航角，仅是定义方式不同，定义方式见图3，α通过直线AB和AY的夹角确定，通过点A、B间轨迹的切线与直线AY的夹角确定)，B为后轴中心，A为目标路径上某一预瞄点A(y,x)；O为AB弧线的圆心，R表示对应的曲率半径，X/Y为直角坐标系的纵轴和横轴。

a)由预瞄点的横向误差计算第一曲率row1，根据图3中显示的关系，列取如下关系式：

sinα＝y/AB

sinα＝(0.5×AB)/R

对上式进行联立，求出：

row1＝1/R＝2×y/(x²+y²)

b)由预瞄点偏航角计算第二曲率row2：

由图3中显示的关系，列取如下关系式：

sinβ＝x/R

sinβ＝x/R

由上面两个公式可得：

row2＝sinβ/x

结合下式：

tanβ＝angle

可得：

由上式可求出第二曲率row2：

c)将两种最优曲率按照以下公式融合：

rowⁱ＝k×row1+(1-k)×row2

其中，i为预瞄点的序号，rowⁱ为预瞄点的最优曲率，k为权重系数(即曲率增益因子)，0<k<1。

本实施例中，k的分配依据如下：根据控制目标(位置与姿态)确定k的大小，如果希望位置跟踪误差重要(体现在第一曲率的求取上)，则这方面给予较大的权重，即将k值设置大些；如果控制目标对姿态敏感(体现在第二曲率的求取上)，则这个方面给予较大的权重，即将k值设置小些。

4)多个预瞄点最优曲率融合及预瞄点的曲率权重因子分配：

其中row^best为多个预瞄点融合之后的融合曲率，n为预瞄点个数，rowⁱ为预瞄点i的最优曲率，qⁱ为预瞄点i的曲率权重因子，

其中qⁱ是车速v的函数，即qⁱ＝f(v)，该曲率权重因子是根据当前车速动态分配得到的，当车辆的当前车速高于设定高值时，距离车辆越远的预瞄点，为该预瞄点的最优曲率分配的最优曲率权重因子越大；当车辆的当前车速低于设定低值时，距离车辆越近的预瞄点，为该预瞄点的最优曲率分配的最优曲率权重因子越大。

本实施例中，采用如下公式计算曲率权重因子：

s＝(v-v₁)/v₂

其中n为预瞄点个数，v为当前车速，v₁，v₂为设定车速，s为速度影响因子，Q为归一化因子，

5)计算方向盘目标转角：

根据阿克曼转向原理可得：

δ＝k_pIrow^bestL+δ₀

其中，δ为方向盘目标转角，L为轴距，k_p为增益系数，row^best为多个预瞄点融合之后的融合曲率，I为转向速比，δ₀为方向盘静态偏差角度。

本发明将路径跟踪问题分解为位置跟踪问题与方向跟踪问题(路径跟踪包括两个控制目标，位置控制与姿态控制，通过将这两个目标解耦，可以根据需要灵活调整)，并根据阿克曼转向原理，分别计算出对应的最优曲率，最后对各最优曲率进行融合，得到融合曲率，进而求得方向盘目标转角，从而实现目标路径跟踪，好处是可以根据不同的应用场景，能够灵活调整路径跟踪的倾向。其中一种应用场景为：在车辆避障时，且可行驶空间较小时，需要提高位置控制精度，通过设置较大的k值，保证不碰撞。另一种应用场景为：当车辆高速行驶时，为了保证车辆横向稳定性，容许较大的跟踪误差，但要求更好的跟踪稳定性，可通过设置较小的k值来实现。

并且，本发明的目标路径跟踪方法通过采用多点预瞄，并根据车速动态分配各个预瞄点的最优曲率权重因子，最终融合得到融合曲率，能够保证路径跟踪方法的适应性，即低速保证路径跟踪的精度，高速保证路径跟踪的稳定性。

以上所述仅为本发明的优选实施例，并不用于限制本发明，对于本领域的技术人员来说，本发明可以有各种更改和变化。例如，本实施例中，在计算融合曲率之前的最优曲率是通过对第一曲率和第二曲率进行加权求和得到的，作为其他实施方式，还可将第一曲率或第二曲率作为最优曲率，去计算融合曲率。第一曲率作为最优曲率时，跟踪精度高，大曲率、高速时容易抖动；第二曲率作为最后曲率时，跟踪精度差，稳定性相对高，特别是在车辆高速行驶的时候。

装置实施例：

本实施例提出一种目标路径跟踪装置，包括存储器和处理器，以及存储在存储器上并在处理器上运行的计算机程序，处理器与存储器相耦合，处理器在执行计算机程序时用于实现上述方法实施例中的目标路径跟踪方法。本实施例中的目标路径跟踪装置为与方法流程相对应的处理进程，由于对上述方法的介绍已经足够清楚完整，故不再详细进行描述。

另外，本实施例中的处理器既可以是微处理器，如ARM等，也可以是可编程芯片，如FPGA、DSP等。

Claims (8)

1.一种目标路径跟踪方法，其特征在于，包括以下步骤：

获取期望跟踪的目标路径，确定至少两个预瞄距离，在所述目标路径中确定各预瞄距离对应的预瞄点；

计算每个预瞄点的曲率，分别为各预瞄点的曲率分配曲率权重因子，利用所述曲率权重因子对相应预瞄点的曲率加权后求和，计算得到预瞄点融合之后的融合曲率；

利用所述预瞄点融合之后的融合曲率，确定进行目标路径跟踪的方向盘目标转角，按照该方向盘目标转角跟踪所述目标路径。

2.根据权利要求1所述的目标路径跟踪方法，其特征在于，所述每个预瞄点的曲率为最优曲率，通过以下步骤计算：

根据预瞄点的坐标，计算得到第一曲率；根据预瞄点的偏航角，计算得到第二曲率；分别为第一曲率和第二曲率分配相应的曲率增益因子；利用所述曲率增益因子对相应的第一曲率和第二曲率进行加权后求和，得到所述最优曲率。

3.根据权利要求1或2所述的目标路径跟踪方法，其特征在于，所述曲率权重因子是根据当前车速动态分配得到的，当车辆的当前车速高于设定高值时，距离车辆越远的预瞄点，为该预瞄点的最优曲率分配的最优曲率权重因子越大；当车辆的当前车速低于设定低值时，距离车辆越近的预瞄点，为该预瞄点的最优曲率分配的最优曲率权重因子越大。

4.根据权利要求1或2所述的目标路径跟踪方法，其特征在于，所述预瞄距离的计算式如下：

l_i＝t_i*v+s_i

其中，l_i为预瞄点i的预瞄距离，t_i为该预瞄点的预瞄时间，v为车速，s_i为静态预瞄距离。

5.根据权利要求2所述的目标路径跟踪方法，其特征在于，第一曲率的计算式如下：

row1＝2×y/(x²+y²)

其中，row1为第一曲率，x为沿着目标路径下预瞄点的纵向坐标值，y为沿着目标路径下预瞄点的横向坐标值。

6.根据权利要求2或5所述的目标路径跟踪方法，其特征在于，第二曲率的计算式如下：

其中，row2为第二曲率，angle为预瞄点的偏航角的正切值，x为沿着目标路径下预瞄点的纵向坐标值。

7.根据权利要求1或2所述的目标路径跟踪方法，其特征在于，所述确定进行目标路径跟踪的方向盘目标转角的关系式如下：

δ＝k_pIrow^bestL+δ₀

其中，δ为方向盘目标转角，I为转向速比，L为轴距，k_p为增益系数，row^best为所有预瞄点融合之后的融合曲率，δ₀为方向盘静态偏差角度。

8.一种目标路径跟踪装置，其特征在于，包括存储器和处理器，以及存储在所述存储器上并在所述处理器上运行的计算机程序，所述处理器与所述存储器相耦合，所述处理器执行所述计算机程序时实现如权利要求1-7任一项所述的目标路径跟踪方法。

Images