CN113204236B - 一种智能体路径跟踪控制方法 - Google Patents

Abstract

本发明公开了一种智能体路径跟踪控制方法，属于路径跟踪控制领域，包括：在当前控制周期识别智能体在目标路径曲线(包括路径方程和参考路径点序列)上需经过的弯曲路径段；计算预瞄距离l_d，将智能体之前与其相距l_d的参考路径点定位为目标参考路径点，并计算偏差权重距离Δl；若Δl<l_min，速度差值Δv_r<Δv_rmax且转角差值Δδ_f<Δδ_fmax，则利用模型预测控制算法获得控制转角量δ₂；否则，利用纯跟踪算法获得控制转角量δ₁；根据智能体的横向位置偏差e_ld计算转角修正量Δδ，对所获得的控制转角量进行修正，得到控制转角量δ，并发送至智能体，实现路径跟踪；l_min、Δv_rmax和Δδ_fmax均为阈值。本发明能够使智能体可以在不同曲率的曲线道路上运动，实现高稳定性、高精度的路径跟踪。

Description

一种智能体路径跟踪控制方法

技术领域

本发明属于路径跟踪控制领域，更具体地，涉及一种智能体路径跟踪控制方法。

背景技术

移动智能体的路径跟踪技术广泛应用于自主AGV、智能驾驶车辆和无人机等多个领域。在工业工况协同作业的过程中存在避障绕行和换道曲线运动等场景，单一的控制方案往往无法同时满足智能体对曲线路径跟踪精度和行驶稳定性的要求，当存在一定初始位置偏差的情况下，智能体会存在难以收敛的航向误差。目前，国内外学者们在智能体曲线路径跟踪稳定性方面开展了大量的研究工作。

路径跟踪控制本质是一个多约束条件下的优化问题，模型预测控制 (ModelPredictive Control，MPC)能够显示地考虑系统的约束条件，在自动驾驶领域应用越来越广泛。但传统的MPC控制策略中参数均为常量，在跟踪曲线道路时，智能体跟踪稳定性和实时性较差。

现有技术中已经开展了一些基于模型预测算法的路径跟踪控制的研究，通过优化控制器参数以及结合模糊逻辑等其他控制策略等方法均能对跟踪效果有一定改善，但当存在一定初始偏差情况下智能体行驶在较大曲率变化的复杂弯道时，行驶的稳定性与路径跟踪精度难以很好的兼顾。

发明内容

针对现有技术的缺陷和改进需求，本发明提供了一种智能体路径跟踪控制方法，其目的在于，采用模型预测控制算法为主、纯跟踪算法为辅的混合控制方法，并综合考虑距离偏差、速度差值和转角差值设定切换条件，使智能体可以在不同曲率的曲线道路上运动，实现高稳定性、高精度的路径跟踪。

为实现上述目的，按照本发明的一个方面，提供了一种智能体路径跟踪控制方法，包括：

在当前控制周期，识别智能体在目标路径曲线上需经过的弯曲路径段；目标路径曲线包括路径方程f(x)和参考路径点序列；

计算预瞄距离l_d，将智能体之前与其相距l_d的参考路径点定位为当前控制周期的目标参考路径点，基于定位结果计算偏差权重距离Δl；

若Δl<l_min，Δv_r<Δv_rmax且Δδ_f<Δδ_fmax，则利用模型预测控制算法获得控制转角量δ₂，作为待修正控制转角量；否则，利用纯跟踪算法获得控制转角量δ₁，作为待修正控制转角量；

根据智能体与目标参考路径点之间的横向位置偏差e_ld计算转角修正量Δδ，对待修正控制转角量进行修正，得到控制转角量δ，并发送至智能体，以使智能体在当前控制周期沿目标路径移动，实现路径跟踪；

其中，偏差权重距离Δl用于衡量智能体与目标路径曲线之间的距离；Δv_r和Δδ_f分别表示智能体的速度差值和前轮转角差值；l_min表示横向跟踪误差阈值，Δv_rmax表示速度差值阈值，Δδ_fmax表示转角差值阈值。

本发明所提供的智能体路径跟踪控制方法，是一种混合控制方法，其以模型预测控制算法为主、纯跟踪算法为辅，综合考虑了智能体当前位置与目标路径曲线之间的距离、速度差值和前轮转角差值设定切换条件，具体来说，在三项指标均低于相应阈值时，由于智能体对目标路径具有较强的跟踪能力，此时切换至模型预测控制算法，能够在路径跟踪的同时，有效提高跟踪精度和稳定性；在任一项指标较高时，表明智能体已经或即将失去对目标路径的跟踪能力，此时切换至纯跟踪算法，能够使智能体快速收敛至目标路径附近。本发明在模型预测控制算法和纯跟踪算法所得出的控制转角量的基础上，基于横向位置偏差e_ld计算转角修正量Δδ对控制转角量进行修正，能够进一步路径跟踪的精度。总体而言，本发明能够使智能体可以在不同曲率的曲线道路上运动，实现高稳定性、高精度的路径跟踪。

进一步地，用于计算预瞄距离的预瞄模型表达式为：

其中，K_y表示弯曲路径段的曲率阈值，其绝对值越大，路径方程f(x) 相对于预瞄方向的曲率越大；l_d表示预瞄距离，V表示智能体的当前速度， C为常数，k_p和k_i分别为比例系数和积分系数；m和m'均为正整数。

纯跟踪算法的计算需要以预瞄距离作为输入参数；本发明所提出的预瞄模型中建立了预瞄距离与速度和路径曲率之间的关系方程，能够适应于不同曲线道路对车速和预瞄距离调整的需求，提高了纯跟踪算法跟踪控制过程的收敛速度和运动稳定性。根据智能体实际的运动特性，通过调整m 和m'的取值，能够使预瞄距离与速度之间相应呈现出线性或非线性关系。

进一步地，曲率阈值K_y按照如下方式获取：

获得参考路径点序列中当前离智能体最近的参考路径点 P₀(x₀+l_pre1,f(x₀+l_pre1))，从参考路径点P₀前方的点(x₀+l_pre2,f(x₀+l_pre2))处开始，识别三个等间距的点P_A(x_PA,y_PA)、P_B(x_PB,y_PB)和P_C(x_PC,y_PC)，三个点所对应的侧向误差分别为s₀、s₁和s₂；

在智能体的纵向上预瞄一段路程，该路程的起点、中点和终点分别是 P_A ^*(x_PA,0)，P_B ^*(x_PB,0)和P_C ^*(x_PC,0)；

按照如下计算公式计算曲率阈值K_y：

其中，l_pre1和l_pre2是预瞄区域中两段不同的距离，且l_pre2≤l_pre1。

进一步地，模型预测控制算法中的预测时域由粒子群优化算法确定。

本发明在每个控制周期采用粒子群优化算法确定模型预测控制算法的预测时域，能够保证智能体在跟踪曲线道路时，仍然具有较好的跟踪稳定性和实时性。

进一步地，粒子群优化算法中，第k次迭代的惯性权重因子ω_k为：

其中，N_max为转折迭代次数，N_a为最大迭代次数，η为临时参数，ω_max和ω_min分别为惯性权重因子的最大值和最小值。

本发明在利用粒子群优化算法确定模型预测控制算法的预测时域时，惯性权重的线性变化过程分为两个部分，在达到转折迭代次数之前，更偏向于局部寻优；在转折迭代次数之后，则偏向于全局寻优，由此能够使所确定的预测时域自适应路径曲率的变化情况，进一步提高模型预测控制算法求解的速度和精度。

进一步地，偏差权重距离Δl的获取方式包括：

沿目标路径曲线，在参考路径点序列中从目标参考路径点开始向前、向后分别等间距采样n个参考路径点，连同目标参考路径点一起共得到 2n+1个参考路径点；

在局部坐标系下，按照如下公式计算偏差权重距离Δl：

其中，j表示2n+1个参考路径点中的参考路径点编号，(x_rj,y_rj)表示第j 个参考路径点的坐标；(x₀,y₀)表示智能体当前的坐标。

本发明在目标参考路径点周围(包括前向和后向)采样得到多个参考路径点，以采样得到的各参考路径点与智能体当前的距离均值作为偏差权重距离Δl，使得所计算的偏差权重距离Δl能够准确反映智能体与目标路径曲线之间的距离。

进一步地，转角修正量Δδ的计算公式为：

Δδ＝K_p·e_ld+K_i·∫e_lddt+K_d·de_ld/dt；

其中，K_p、K_i和K_d分别表示PID控制中的比例系数、积分系数和微分系数。

本发明将智能体与目标参考路径点之间的横向位置偏差作为优化行为的反馈信息，通过PID控制方法计算转角修正量，具有较高的计算速度，且计算得到的转角修正量精度较高。

进一步地，智能体与目标参考路径点之间的横向位置偏差e_ld为：

其中，(X_r(k+1),Y_r(k+1))为目标参考路径点在全局坐标系下的坐标，(X₀, Y₀)为智能体当前在全局坐标系下的坐标，

为目标参考路径点在全局坐标系下的期望航向。

进一步地，本发明提供的智能体控制方法，还包括：

按照V_ref＝min((1-e^-K/c)·V_max,V_s)实时计算目标控制速度V_ref，并实时判断智能体是否通过了当前的弯曲路径段；K表示弯曲路径段的曲率；V_max表示限制最大速度，

g表示重力加速度，μ表示摩擦系数；V_s为根据Sigmod规划曲线计算出的速度；c为与智能体运动性能相关的系数，与智能体受路径曲率变化的影响程度正相关；

若智能体当前未通过弯曲路径段，则根据目标控制速度V_ref使智能体执行减速；否则，根据速度V_s控制智能体的速度。

本发明在路径跟踪的同时，会实时根据路径曲率确定目标控制速度V_ref，并根据智能体的运动状态相应控制智能体的速度，由此能够进一步保证智能体运动的稳定性；智能体速度的控制结果会影响模型预测控制算法和纯跟踪算法的输出结果，进一步提高路径跟踪的精度。

进一步地，弯曲路径段的曲率K按照如下计算公式计算：

其中，

S和E分别为弯曲路径段的起点和终点，O'为弯曲路径段上除端点外的一个点，(x_S,y_S)、(x_E,y_E)和(x_O',y_O')分别表示起点S、终点E和点O'的在局部坐标系下的坐标，且x_O'＝(x_S+x_E)/2。

进一步地，识别智能体在目标路径曲线上需经过的弯曲路径段，包括：

(S1)以参考路径点序列中当前离智能体最近的参考路径点P₀为起点，沿目标路径曲线向前搜索N个参考路径点P_k(x_Pk,y_Pk)，k＝1,2,...,N，得到路径点集，将参考路径点P₀作为采样起点；

(S2)从采样起点开始，在路径点集中等间隔采样3个参考路径点P₁'、 P₂'和P₃'，计算直线段P₁'P₂'和P₂'P₃'之间的夹角α，若α<α₀，则将参考路径点P₂'作为弯曲路径点的起点，并转入步骤(S4)；否则，转入步骤(S3)；

(S3)将采样起点沿目标路径曲线滑动第一预设距离后，将当前的参考路径点作为新的采样起点，并转入步骤(S2)；

(S4)将采样起点沿目标路径曲线滑动第二预设距离后，将当前的参考路径点作为新的采样起点；

(S5)从采样起点开始，在路径点集中等间隔采样3个参考路径点P₁'、 P₂'和P₃'，计算直线段P₁'P₂'和P₂'P₃'之间的夹角α，若α≥α₀，则将参考路径点P₂'作为弯曲路径的终点，并转入步骤(S7)；否则，转入步骤(S6)；

(S6)将采样起点沿目标路径曲线滑动第三预设距离后，将当前的参考路径点作为新的采样起点，并转入步骤(S5)；

(S7)将目标路径曲线上，由所识别的起点和终点所限定的路径段识别为当前控制周期内的弯曲路径段；

其中，α₀为预设的标准角度。

本发明，根据三个采样点间连线的夹角识别弯曲路径段的起点和终点，首先识别弯曲路径段的起点，将夹角较大时，对应的顶点作为弯曲路径段的起点；之后识别弯曲路径的终点，将夹角较小时，对应的顶点作为弯曲路径段的终点；按照这种方式，可以准确识别出对应控制周期内的弯曲路径段。

进一步地，步骤(S1)中，搜索得到的N个参考路径点P_k(x_Pk,y_Pk)均在预测距离D_p内；

预测距离D_p的计算公式为：

D_p＝r·N_p·V_ref·([10(1+e^-ξ·K′)+rev])；

其中，r为修正系数，N_p为模型预测算法中的预测时域，V_ref为目标控制速度，K'为上一控制周期的弯曲路径段的曲率，ξ为调整因子；rev为调整量，rev<1；[]表示取整。

本发明在识别当前控制周期内的弯曲路径段时，根据当前的控制目标速度、当前的预测时域以及上一控制周期的弯曲路径段的曲率计算当前控制周期的预测距离，该预测距离能够较为准确地反映当前控制周期内弯曲路径段所覆盖的范围，仅在该预测距离内搜索参考路径点，能够在当前控制周期不存在弯曲路径段时，及时终止参考路径点的处理，从而避免无效的计算，有效提高计算效率。

进一步地，目标路径曲线的获取方式包括：

生成智能体的路径曲线，包括路径方程和原始的参考路径点序列；

过滤掉原始的参考路径点序列中的毛刺点后，对剩余的参考路径点进行插值处理，得到新的参考路径点序列；

利用新的参考路径点拟合得到新的路径方程；

由新的参考路径点序列和新的路径方程构成智能体的目标路径曲线。

本发明通过对原始的参考路径点序列依次进行滤除毛刺点和插值的处理，能够得到更为密集的参考路径点序列，由该更为密集的参考路径点序列拟合得到的路径方程更为平滑，有利于提高路径跟踪的稳定性。

按照本发明的另一个方面，提供了一种计算机可读存储介质，包括存储的计算机程序；计算机程序被处理器执行时，控制计算机可读存储介质所在设备执行本发明提供的智能体路径跟踪控制方法。

总体而言，通过本发明所构思的以上技术方案，能够取得以下有益效果：

(1)本发明所提供的智能体路径跟踪控制方法，以模型预测控制算法为主、纯跟踪算法为辅，综合考虑了智能体当前位置与目标路径曲线之间的距离、速度差值和前轮转角差值设定切换条件，并基于智能体的横向位置偏差计算得到的转角修正量对控制转角量进行修正，能够实时、准确地捕捉智能体对目标路径的跟踪能力，并切换至相应的跟踪算法，充分发挥两种跟踪算法之间的互补性，使曲线运动的智能体不仅具有较高的跟踪精度，还保证了较好的行驶稳定性，尤其适用于需要经常发生换道曲线运动的移动对象实时路径跟踪的应用场合。

(2)本发明所提出的预瞄模型中，建立了预瞄距离与车速和道路曲率之间的关系方程，适应于不同曲线道路对车速和预瞄距离调整的需求，提高了纯跟踪控制算法跟踪控制过程的收敛速度和运动稳定性。

(3)本发明中在每个控制周期采用粒子群优化算法确定模型预测控制算法的预测时域，能够保证智能体在跟踪曲线道路时，仍然具有较好的跟踪稳定性和实时性；在其优选方案中，在利用粒子群优化算法确定模型预测控制算法的预测时域时，惯性权重的线性变化过程分为两个部分，在达到转折迭代次数之前，更偏向于局部寻优，在转折迭代次数之后，则偏向于全局寻优，由此能够使所确定的预测时域自适应路径曲率的变化情况，进一步提高模型预测控制算法求解的速度和精度。

附图说明

图1为本发明实施例提供的智能体路径跟踪控制方法流程图；

图2为本发明实施例提供的智能体路径跟踪控制方法示意图；

图3为本发明实施例提供的采用多点预瞄确定本控制周期曲线段路径的始终点判断流程图；

图4为本发明实施例提供的识别当前控制周期内的弯曲路径段的示意图；

图5为本发明实施例提供的采用多点预瞄进行路径曲率阈值计算的流程图；

图6为本发明实施例提供的采用路径模拟函数与智能体对称中心方向所形成预瞄距离与路径之间的位置关系图；

图7为本发明实施例提供的智能体路径跟踪各状态量标注及受力分析示意图；

图8为本发明实施例提供的通过粒子群算法对预测时域进行整定的流程示意图。

具体实施方式

为了使本发明的目的、技术方案及优点更加清楚明白，以下结合附图及实施例，对本发明进行进一步详细说明。应当理解，此处所描述的具体实施例仅仅用以解释本发明，并不用于限定本发明。此外，下面所描述的本发明各个实施方式中所涉及到的技术特征只要彼此之间未构成冲突就可以相互组合。

在本发明中，本发明及附图中的术语“第一”、“第二”等(如果存在)是用于区别类似的对象，而不必用于描述特定的顺序或先后次序。

在智能体的路径跟踪中，当存在一定初始偏差情况下或者智能体行驶在较大曲率变化的复杂弯道时，行驶的稳定性与路径跟踪精度难以很好地兼顾，为了解决该技术问题，本发明提供了一种智能体路径跟踪控制方法，其整体思路在于：挖掘智能体在路径跟踪中的运动特性，采用以模型预测控制算法为主、纯跟踪算法为辅的混合控制方法，并综合考虑智能体当前位置与目标路径曲线之间的距离、速度差值和前轮转角差值设定切换条件，以实时、准确地捕捉智能体对目标路径的跟踪能力，并切换至相应的跟踪算法，充分发挥两种跟踪算法之间的互补性，使曲线运动的智能体不仅具有较高的跟踪精度，还保证了较好的行驶稳定性。

在智能体路径跟踪过程中，涉及到两个坐标系，即全局坐标系和局部坐标系，二者可相互变换；为便于描述，在没有特殊说明的情况下，以大写字母X和Y表示全局坐标系下的坐标，以小写字母x和y表示局部坐标系下的坐标；例如，智能体当前在全局坐标系下的坐标表示为(X₀,Y₀)，智能体在局部坐标系下的坐标表示为(x₀,y₀)。

以下为实施例。

实施例1：

一种智能体路径跟踪控制方法，如图1和图2所示，具体包括：

在当前控制周期，识别智能体在目标路径曲线上需经过的弯曲路径段；目标路径曲线包括路径方程f(x)和参考路径点序列；

计算预瞄距离l_d，将智能体之前与其相距l_d的参考路径点定位为当前控制周期的目标参考路径点，基于定位结果计算偏差权重距离Δl；

若Δl<l_min，Δv_r<Δv_rmax且Δδ_f<Δδ_fmax，则利用模型预测控制算法获得控制转角量δ₂，作为待修正控制转角量；否则，利用纯跟踪算法获得控制转角量δ₁，作为待修正控制转角量；

根据智能体与目标参考路径点之间的横向位置偏差e_ld计算转角修正量Δδ，对待修正控制转角量进行修正，得到控制转角量δ，并发送至智能体，以使智能体在当前控制周期沿目标路径移动，实现路径跟踪；

其中，偏差权重距离Δl用于衡量智能体与目标路径曲线之间的距离；Δv_r和Δδ_f分别表示智能体的速度差值和前轮转角差值；l_min表示横向跟踪误差阈值，Δv_rmax表示速度差值阈值，表示Δδ_fmax转角差值阈值。

本实施例中，在进行路径跟踪控制之前，还会先采集环境信息并对目标路径曲线进行预处理；在采集环境信息时，首先通过传感器设备检测周围环境信息并进行完成自身的定位，传感器设备可以是深度相机、激光雷达、超声波传感器等；目标路径曲线的预处理具体包括：

生成智能体的路径曲线，包括路径方程和原始的参考路径点序列；

过滤掉原始的参考路径点序列中的毛刺点后，对剩余的参考路径点进行插值处理，得到新的参考路径点序列；在实际应用中，具体可采用高斯滤波、双边滤波和中值滤波等方式过滤毛刺点，可采用分段线性插值、任意阶多项式插值和自然邻点插值等插值方式对参考路径点进行插值；新的参考路径点序列中的参考路径点更为密集，可表示为点序列(x_r(nT),y_r(nT),θ_r(nT))，其中，T为采样时间，x_r(nT),y_r(nT)和θ_r(nT)分别表示第 nT时刻智能体期望的位姿横坐标、纵坐标和横摆角度；

利用新的参考路径点拟合得到新的路径方程；在实际应用中，可采用贝塞尔曲线、三次样条曲线和基数样条曲线等拟合新的路径方程；拟合得到路径方程更为平滑；

由新的参考路径点序列和新的路径方程构成智能体的目标路径曲线；

本实施例通过对原始的参考路径点序列依次进行滤除毛刺点和插值的处理，能够得到更为密集的参考路径点序列，由该更为密集的参考路径点序列拟合得到的路径方程更为平滑，有利于提高路径跟踪的稳定性。

识别弯曲路径段，可将其简化为识别出弯曲路径段的起点和终点，作为一种可选的实施方式，本实施例中，识别智能体在目标路径曲线上需经过的弯曲路径段，如图3所示，包括：

(S1)以参考路径点序列中当前离智能体最近的参考路径点P₀为起点，沿目标路径曲线向前搜索N个参考路径点P_k(x_Pk,y_Pk)，k＝1,2,...,N，得到路径点集，将参考路径点P₀作为采样起点；

参考路径点P₀的坐标可表示为(x₀+l_pre1,f(x₀+l_pre1))，其中，l_pre1是预瞄区域内的一段距离；

当前控制周期的弯曲路径段的起点和终点仅在搜索到的N个参考路径点中确定；

在实际的路径跟踪过程中，可能存在在当前控制周期中，智能体并不经过弯曲路径段的情况，这也表明智能体在当前控制周期中的移动路径是直线路径，此时，弯曲路径段的曲率为K＝0；

为了合理限定弯曲路径段起点、终点的搜索范围，可选地，本实施例在沿目标路径向前搜索参考路径点时，还会先计算一个预测距离D_p，用于预测弯曲路径段所覆盖的范围，步骤(S1)中，搜索得到的N个参考路径点P_k(x_Pk,y_Pk)均在预测距离D_p内；本实施例中，预测距离D_p的计算公式为：

D_p＝r·N_p·V_ref·([10(1+e^-ξ·K′)+rev])；

其中，r为修正系数，用于将预测距离取值到合理范围，本实施例中，其值具体为0.01；N_p为模型预测算法中的预测时域，V_ref为目标控制速度， K'为上一控制周期的弯曲路径段的曲率；ξ为调整因子，其取值可依据实验数据调整获取；rev为调整量，rev<1，本实施例中，其取值具体为0.5；[] 表示取整；预测距离D_p是根据当前的控制目标速度、当前的预测时域以及上一控制周期的弯曲路径段的曲率计算得到的，该预测距离能够较为准确地反映当前控制周期内弯曲路径段所覆盖的范围，仅在该预测距离内搜索参考路径点，能够在当前控制周期不存在弯曲路径段时，及时终止参考路径点的处理，从而避免无效的计算，有效提高计算效率；

(S2)从采样起点开始，在路径点集中等间隔采样3个参考路径点P₁'、 P₂'和P₃'，计算直线段P₁'P₂'和P₂'P₃'之间的夹角α，若α<α₀，则将参考路径点P₂'作为弯曲路径点的起点，并转入步骤(S4)；否则，转入步骤(S3)；

采样间隔可跟据实验数据调整获取；

以(x_P1',y_P1')、(x_P2',y_P2')和(x_P3',y_P3')分别表示参考路径点P₁'、P₂'和P₃'的坐标，直线段P₁'P₂'和P₂'P₃'之间的夹角α的计算公式可表示为：

(S3)将采样起点沿目标路径曲线滑动第一预设距离后，将当前的参考路径点作为新的采样起点，并转入步骤(S2)；

第一预设距离与采样间隔之间的比例可根据实验数据调整获取；

(S4)将采样起点沿目标路径曲线滑动第二预设距离后，将当前的参考路径点作为新的采样起点；

第二预设距离与采样间隔之间的比例可根据实验数据调整获取；

(S5)从采样起点开始，在路径点集中等间隔采样3个参考路径点P₁'、 P₂'和P₃'，计算直线段P₁'P₂'和P₂'P₃'之间的夹角α，若α≥α₀，则将参考路径点作为P₁'弯曲路径的终点，并转入步骤(S7)；否则，转入步骤(S6)；

(S6)将采样起点沿目标路径曲线滑动第三预设距离后，将当前的参考路径点作为新的采样起点，并转入步骤(S5)；

第三预设距离与采样间隔之间的比例可根据实验数据调整获取；

(S7)将目标路径曲线上，由所识别的起点和终点所限定的路径段识别为当前控制周期内的弯曲路径段；

其中，α₀为预设的标准角度，其值可根据智能体结构参数和用户需求综合设定，可选地，本实施例中，α₀的取值小于车轮最大转向角度的机械结构参数限制；可选地，本实施例中，在确定弯曲路径段起点和终点时，采样间隔相同，且第一预设阈值、第二预设阈值和第三预设阈值相等；

图4所示为一个具体的示例，搜索得到的N个参考路径点P₁～P_N以及个参考路径点在目标路径曲线上的分布如图4所示，经过上述识别过程后，其中第 2个参考路径点P₂被识别为弯曲路径段起点，第6个参考路径点P₆被识别为完全路径段终点。

识别出当前控制周期的弯曲路径段后，可相应计算该弯曲路径段的曲率K和曲率阈值K_y；曲率阈值K_y用于衡量智能体与目标路径的接近程度；

弯曲路径段的曲率K按照如下计算公式计算：

其中，

S和E分别为弯曲路径段的起点和终点，O'为弯曲路径段上除端点外的一个点，(x_S,y_S)、(x_E,y_E)和(x_O',y_O')分别表示起点S、终点E和点O'的在局部坐标系下的坐标，且x_O'＝(x_S+x_E)/2；d₁、d₂、d₃分别代表直线段SO'、SE和O'E的长度。

如图5和图6所示，曲率阈值K_y按照如下方式获取：

获得参考路径点序列中当前离智能体最近的参考路径点 P₀(x₀+l_pre1,f(x₀+l_pre1))，从参考路径点P₀前方的点(x₀+l_pre2,f(x₀+l_pre2))处开始，识别三个等间距的点P_A(x_PA,y_PA)、P_B(x_PB,y_PB)和P_C(x_PC,y_PC)，三个点所对应的侧向误差分别为s₀、s₁和s₂；

在智能体的纵向(即图6中的x₁方向)上预瞄一段路程，该路程的起点、中点和终点分别是P_A ^*(x_PA,0)，P_B ^*(x_PB,0)和P_C ^*(x_PC,0)；

按照如下计算公式计算率阈值K_y：

其中，l_pre1和l_pre2是预瞄区域中两段不同的距离，且l_pre2≤l_pre1，通过设定l_pre2≤l_pre1，可以在计算预瞄距离时对预瞄距离进行细致调整，同时在智能体经过弯曲路径段时提前减速，从而进一步提高路径跟踪的稳定性和精度；在以上计算式中，

表示 P_AP_BP_CP_A ^*P_B ^*P_C ^*曲线所围成的面积，

表示以点P_A、P_B、P_A ^*、 P_B ^*为顶点的梯形面积，

表示以点P_B、P_C、P_B ^*、P_C ^*为顶点的梯形面积；

当f(x)为图6中的凸路段弧P_AP_BP_C时，K_y>0；当f(x)为图6中的直线路段P_AP_B'P_C时，K_y＝0；当f(x)为图6中的凹路段弧P_AP_B”P_C时，K_y<0；|K_y| 越大，f(x)相对于预瞄方向的曲率越大。

为了能够适应于不同曲线道路对车速和预瞄距离调整的需求，本实施例提出了一种新的用于计算预瞄距离的预瞄模型，该模型中建立了预瞄距离与速度和路径曲率之间的关系方程，具体表达式如下：

其中，K_y表示弯曲路径段的曲率阈值，其绝对值越大，路径方程f(x) 相对于预瞄方向的曲率越大；l_d表示预瞄距离，V表示智能体的当前速度， C为常数，k_p和k_i分别为比例系数和积分系数；m和m'均为正整数；

在实际应用中，根据智能体实际的运动特性，通过调整m和m'的取值，能够使预瞄距离与速度之间相应呈现出线性或非线性关系，例如，预瞄距离l_d与速度V呈线性关系时，相应设置m与m'相等；预瞄距离l_d与速度V 呈非线性关系时，相应设置m与m'不相等；可选地，本实施例中，m＝3、 m'＝2，即

计算得到预瞄距离l_d之后，以智能体当前的后轴中点为原点，以预瞄距离为半径作圆，将该圆与路径方程f(x)在智能体前方的交点作为目标参考路径点；智能体当前的后轴中点可通过传感器获取；当无前向交点时，以该目标路径曲线末端点切线方向延长3L(L表示智能体的长度)长度作为预泊车距离进行跟踪，并停止期望速度的计算而采取减速操作。

为了使偏差权重距离Δl能够准确地衡量智能体与目标路径曲线之间的距离，作为一种可选的实施方式，本实施例中，偏差权重距离Δl的获取方式包括：

沿目标路径曲线，在参考路径点序列中从目标参考路径点开始向前、向后分别等间距采样n个参考路径点，连同目标参考路径点一起共得到 2n+1个参考路径点；采样间隔可根据智能体的实际移动情况相应设定；

在局部坐标系下，按照如下公式计算偏差权重距离Δl：

其中，j表示2n+1个参考路径点中的参考路径点编号，(x_rj,y_rj)表示第j 个参考路径点的坐标；(x₀,y₀)表示智能体当前的坐标；

在目标参考路径点周围(包括前向和后向)采样得到多个参考路径点，以采样得到的各参考路径点与智能体当前的距离均值作为偏差权重距离Δl，使得所计算的偏差权重距离Δl能够准确反映智能体与目标路径曲线之间的距离。

计算得到偏差权重距离Δl之后，即可结合智能体的速度信息和前路转角信息判断智能体当前对目标路径的跟踪能力；

具体地，在本实施例中，在同时满足Δl<l_min，Δv_r<Δv_rmax且Δδ_f<Δδ_fmax时，判定智能体能够很好地跟踪目标路径，此时采用模型预测控制算法，能够在路径跟踪的同时，有效提高跟踪精度和稳定性；在其他情况下，则说明此时智能体已经或即将失去对目标路径的跟踪能力，此时采用纯跟踪算法，能够使智能体快速收敛至目标路径附近。本实施例综合考虑智能体当前位置与目标路径曲线之间的距离、速度差值和前轮转角差值设定切换条件，能够实时、准确地捕捉智能体的运动状态。

横向跟踪误差阈值l_min，速度差值阈值Δv_rmax，转角差值阈值Δδ_fmax可通过实验获取；例如，可在不同车速和横向偏差参数下对智能体的直线跟踪效果进行测试，选取当智能体失去对目标路径跟踪能力时的横向跟踪误差作为l_min。

以上混合控制方法输出的控制转角量δ可以表示为：

δ＝λ₁·δ₁+λ₂·δ₂+Δδ；

其中λ₁,λ₂均为控制器切换开关系数，取值为0或1，具体地，当满足Δl<l_min，Δv_r<Δv_rmax且Δδ_f<Δδ_fmax时，λ₁＝0且λ₂＝1，其中l_min为横向偏差阈值；其他情况均切换至λ₁＝1且λ₂＝0。

模型预测控制算法包括误差模型、目标函数和约束条件，具体可根据动力学模型预测算法得到误差模型和目标函数，如图7所示，建立纵向、横向及横摆3个自由度的智能体动力学模型为

其中，假设预瞄点为A，m为智能体质量；l_f、l_r分别为前后轴到智能体质心的距离；δ为智能体前轮有效转向角；I_z是车体绕Z轴的转动惯量；α_f、α_r为前后轮侧偏角；φ,

为智能体横摆角及加速度；

是x、y轴方向的速度及加速度；C_xf、C_xr为前后轮纵向滑移刚度，C_αf、C_αr为前后轮侧偏刚度；i_f、i_r为前后轮胎纵向滑移率；

为全局坐标系X、Y轴方向的速度；

对模型进行线性化处理并进行近似离散化处理得到出系统的预测输出表达式为

Y(t)＝F_tγ(t|t)+G_tΔU(t)+H_tI

A_k,t＝A_t,t,B_k,t＝B_t,t

其中，F_t、G_t代表变换矩阵，N_u、N_y为输入输出量个数；

本实施例中，模型预测控制算法的目标函数为：

其中，N_p为预测时域，N_c为控制时域，Q、R、S为权重矩阵，ε为松弛因子，ρ为其权重系数，η(k|t)为系统状态量(位姿)；

表示系统状态参考量，

U(k|t)为系统控制量(控制转角量、速度)，ΔU(k|t)为系统控制量的变化；X_r、Y_r和θ_r分别表示智能体参考横坐标、参考纵坐标和参考航向角；

控制过程中满足以下约束条件：

U_min≤E·ΔU₀+U₀≤U_max

其中，构成E左矩阵的行数和列数均为N_c，I_m代表一个维数为m的单位矩阵，

表示克罗内克积；

考虑到传统的模型预测控制中参数均为常量，在跟踪曲线道路时，智能体跟踪稳定性和实时性较差；本实施例中，模型预测控制算法中的预测时域由粒子群优化算法确定，具体如图8所示，以预测时域N_p为粒子，搜索空间为D维，粒子的运动路径简化为一维运动方程如下：

其中，j＝1,2,…n，n是群体规模。

为第k次更新第j个粒子时更新的速度；

为第k次更新时第j个粒子所在位置；ω为惯性权重因子；c₁、 c₂是学习因子常数；r₁、r₂是0到1之间的随机数；P_k为第k次更新时的个体极值；G_k是第k次整个种群中的极值。惯性权重的线性变化分为两个部分，通过临时参数η，则第k次迭代的惯性权重因子ω_k(开始迭代时偏向于局部寻优；之后偏向于全局寻优)为：

其中，N_max为转折迭代次数，N_a为最大迭代次数，η为临时参数，ω_max和ω_min分别为惯性权重因子的最大值和最小值；

本实施例在利用粒子群优化算法确定模型预测控制算法的预测时域时，惯性权重的线性变化过程分为两个部分，在达到转折迭代次数之前，更偏向于局部寻优；在转折迭代次数之后，则偏向于全局寻优；采用这种方式设置惯性权重参数，能够使所确定的预测时域自适应路径曲率的变化情况，使得在不同路径(直线、曲线)和不同运行速度(低速、中速、高速)下，都能提高模型的预测精度，使智能体更好地跟踪目标路径。

纯跟踪算法输出的控制转角量δ₁的计算式如下：

其中，L表示智能体的长度；本实施例中，智能体与目标参考路径点之间的横向位置偏差e_ld为：

其中，(X_r(k+1),Y_r(k+1))为目标参考路径点在全局坐标系下的坐标，(X₀, Y₀)为智能体当前在全局坐标系下的坐标，

为目标参考路径点在全局坐标系下的期望航向。

传统的路径跟踪控制方法中，往往利用模糊控制算法计算控制转角量的修正量，耗时较长且精度得不到保证；为了进一步提高转角修正量的计算速度和计算精度，作为一种可选的实施方式，本实施例中，转角修正量Δδ的计算公式为：

Δδ＝K_p·e_ld+K_i·∫e_lddt+K_d·de_ld/dt；

其中，K_p、K_i和K_d分别表示PID控制中的比例系数、积分系数和微分系数。

本实施例将智能体与目标参考路径点之间的横向位置偏差作为优化行为的反馈信息，通过PID控制方法计算转角修正量，具有较高的计算速度，且计算得到的转角修正量精度较高。

为了进一步提高智能体路径跟踪的稳定性和跟踪精度，如图1所示，本实施例提供的智能体路径跟踪控制方法，还包括：

按照V_ref＝min((1-e^-K/c)·V_max,V_s)实时计算目标控制速度V_ref，并实时判断智能体是否通过了当前的弯曲路径段；K表示弯曲路径段的曲率；V_max表示限制最大速度，

g表示重力加速度，μ表示摩擦系数；V_s为根据Sigmod规划曲线计算出的速度；c为与智能体运动性能相关的系数，与智能体受路径曲率变化的影响程度正相关，具体地，智能体受路径曲率变化影响越小，c的值越小，且

若智能体当前未通过弯曲路径段，则根据目标控制速度V_ref使智能体执行减速；否则，根据速度V_s控制智能体的速度；

在判断智能体是否通过了当前的弯曲路径段时，可根据智能体的传感器获取相关信息；

本实施例在路径跟踪的同时，会实时根据路径曲率确定目标控制速度 V_ref，并根据智能体的运动状态相应控制智能体的速度，由此能够进一步保证智能体运动的稳定性；智能体速度的控制结果会影响模型预测控制算法和纯跟踪算法的输出结果，进一步提高路径跟踪的精度。

为了减少内存消耗，本实施例在当前控制周期结束，进入下一个控制周期之前，会记录当控制周期的弯曲路径段的曲率K和曲率阈值K_y并删除掉当前控制周期位于智能体后方已处理的路径点及相关几何信息。

总体而言，本实施例所提供的智能体路径跟踪控制方法，是一种混合控制方法，其以模型预测控制算法为主、纯跟踪算法为辅，综合考虑了智能体当前位置与目标路径曲线之间的距离、速度差值和前轮转角差值设定切换条件，能够使智能体可以在不同曲率的曲线道路上运动，实现高稳定性、高精度的路径跟踪。

实施例2：

一种计算机可读存储介质，包括存储的计算机程序；计算机程序被处理器执行时，控制计算机可读存储介质所在设备执行上述实施例1提供的智能体路径跟踪控制方法。

本领域的技术人员容易理解，以上所述仅为本发明的较佳实施例而已，并不用以限制本发明，凡在本发明的精神和原则之内所作的任何修改、等同替换和改进等，均应包含在本发明的保护范围之内。

Claims (9)

1.一种智能体路径跟踪控制方法，其特征在于，包括：

在当前控制周期，识别所述智能体在目标路径曲线上需经过的弯曲路径段；所述目标路径曲线包括路径方程f(x)和参考路径点序列；

计算预瞄距离l_d，将所述智能体之前与其相距l_d的参考路径点定位为当前控制周期的目标参考路径点，基于定位结果计算偏差权重距离Δl；

若Δl<l_min，Δv_r<Δv_rmax且Δδ_f<Δδ_fmax，则利用模型预测控制算法获得控制转角量δ₂，作为待修正控制转角量；否则，利用纯跟踪算法获得控制转角量δ₁，作为待修正控制转角量；

根据所述智能体与所述目标参考路径点之间的横向位置偏差e_ld计算转角修正量△δ，对待修正控制转角量进行修正，得到控制转角量δ，并发送至所述智能体，以使所述智能体在当前控制周期沿所述目标路径移动，实现路径跟踪；

其中，偏差权重距离Δl用于衡量所述智能体与所述目标路径曲线之间的距离；Δv_r和Δδ_f分别表示所述智能体的速度差值和前轮转角差值；l_min表示横向跟踪误差阈值，Δv_rmax表示速度差值阈值，Δδ_fmax表示转角差值阈值；

识别所述智能体在目标路径曲线上需经过的弯曲路径段，包括：

(S1)以所述参考路径点序列中当前离所述智能体最近的参考路径点P₀为起点，沿所述目标路径曲线向前搜索N个参考路径点P_k(x_Pk,y_Pk)，k＝1,2,...,N，得到路径点集，将所述参考路径点P₀作为采样起点；

(S2)从采样起点开始，在所述路径点集中等间隔采样3个参考路径点P₁'、P₂'和P₃'，计算直线段P₁'P₂'和P₂'P₃'之间的夹角α，若α<α₀，则将参考路径点P₂'作为弯曲路径点的起点，并转入步骤(S4)；否则，转入步骤(S3)；

(S3)将采样起点沿所述目标路径曲线滑动第一预设距离后，将当前的参考路径点作为新的采样起点，并转入步骤(S2)；

(S4)将采样起点沿所述目标路径曲线滑动第二预设距离后，将当前的参考路径点作为新的采样起点；

(S5)从采样起点开始，在所述路径点集中等间隔采样3个参考路径点P₁'、P₂'和P₃'，计算直线段P₁'P₂'和P₂'P₃'之间的夹角α，若α≥α₀，则将参考路径点作为弯曲路径的终点，并转入步骤(S7)；否则，转入步骤(S6)；

(S6)将采样起点沿所述目标路径曲线滑动第三预设距离后，将当前的参考路径点P₂'作为新的采样起点，并转入步骤(S5)；

(S7)将所述目标路径曲线上，由所识别的起点和终点所限定的路径段识别为当前控制周期内的弯曲路径段；

其中，α₀为预设的标准角度。

2.如权利要求1所述的智能体路径跟踪控制方法，其特征在于，用于计算所述预瞄距离的预瞄模型表达式为：

其中，K_y表示所述弯曲路径段的曲率阈值，其绝对值越大，所述路径方程f(x)相对于预瞄方向的曲率越大；l_d表示所述预瞄距离，V表示所述智能体的当前速度，C为常数，k_p和k_i分别为比例系数和积分系数；m和m'均为正整数。

3.如权利要求2所述的智能体路径跟踪控制方法，其特征在于，所述曲率阈值K_y按照如下方式获取：

获得所述参考路径点序列中当前离所述智能体最近的参考路径点P₀(x₀+l_pre1,f(x₀+l_pre1))，从所述参考路径点P₀前方的点(x₀+l_pre2,f(x₀+l_pre2))处开始，识别三个等间距的点P_A(x_PA,y_PA)、P_B(x_PB,y_PB)和P_C(x_PC,y_PC)，三个点所对应的侧向误差分别为s₀、s₁和s₂；

在所述智能体的纵向上预瞄一段路程，该路程的起点、中点和终点分别是P_A ^*(x_PA,0)，P_B ^*(x_PB,0)和P_C ^*(x_PC,0)；

按照如下计算公式计算所述曲率阈值K_y：

其中，l_pre1和l_pre2是预瞄区域中两段不同的距离，且l_pre2≤l_pre1。

4.如权利要求1所述的智能体路径跟踪控制方法，其特征在于，所述模型预测控制算法中的预测时域由粒子群优化算法确定，并且，所述粒子群优化算法中，第k次迭代的惯性权重因子ω_k为：

其中，N_max为转折迭代次数，N_a为最大迭代次数，η为临时参数，ω_max和ω_min分别为惯性权重因子的最大值和最小值。

5.如权利要求1所述的智能体路径跟踪控制方法，其特征在于，所述转角修正量△δ的计算公式为：

Δδ＝K_p·e_ld+K_i·∫e_lddt+K_d·de_ld/dt；

并且，所述智能体与所述目标参考路径点之间的横向位置偏差e_ld为：

其中，K_p、K_i和K_d分别表示PID控制中的比例系数、积分系数和微分系数；(X_r(k+1),Y_r(k+1))为所述目标参考路径点在全局坐标系下的坐标，(X₀,Y₀)为所述智能体当前在全局坐标系下的坐标，

为所述目标参考路径点在全局坐标系下的期望航向。

6.如权利要求1-5任一项所述的智能体路径跟踪控制方法，其特征在于，还包括：

按照V_ref＝min((1-e^-K/c)·V_max,V_s)实时计算目标控制速度V_ref，并实时判断所述智能体是否通过了当前的弯曲路径段；K表示弯曲路径段的曲率；V_max表示限制最大速度，

g表示重力加速度，μ表示摩擦系数；V_s为根据Sigmod规划曲线计算出的速度；c为与所述智能体运动性能相关的系数，与所述智能体受路径曲率变化的影响程度正相关；

若所述智能体当前未通过弯曲路径段，则根据目标控制速度V_ref使所述智能体执行减速；否则，根据速度V_s控制所述智能体的速度。

7.如权利要求6所述的智能体路径跟踪控制方法，其特征在于，弯曲路径段的曲率K按照如下计算公式计算：

其中，

S和E分别为弯曲路径段的起点和终点，O'为弯曲路径段上除端点外的一个点，(x_S,y_S)、(x_E,y_E)和(x_O',y_O')分别表示起点S、终点E和点O'的在局部坐标系下的坐标，且x_O'＝(x_S+x_E)/2。

8.如权利要求1-5任一项所述的智能体路径跟踪控制方法，其特征在于，所述步骤(S1)中，搜索得到的N个参考路径点P_k(x_Pk,y_Pk)均在预测距离D_p内；

所述预测距离D_p的计算公式为：

D_p＝r·N_p·V_ref·([10(1+e^-ξ·K′)+rev])；

其中，r为修正系数，N_p为模型预测算法中的预测时域，V_ref为目标控制速度，K'为上一控制周期的弯曲路径段的曲率，ξ为调整因子；rev为调整量，rev<1；[]表示取整。

9.一种计算机可读存储介质，其特征在于，包括存储的计算机程序；所述计算机程序被处理器执行时，控制所述计算机可读存储介质所在设备执行权利要求1-8任一项所述的智能体路径跟踪控制方法。

Images