CN110850895A - 一种路径跟踪方法、装置、设备及存储介质 - Google Patents

Abstract

本发明公开了一种路径跟踪方法、装置、设备及存储介质。该方法包括：获取目标状态方程和目标函数，其中，所述目标状态方程包括状态量；根据所述目标状态方程和目标函数利用LQR控制算法确定最优控制增益；根据所述最优控制增益与所述状态量的乘积确定最优控制量；根据所述最优控制量控制方向盘转角，以使车辆进行目标路径跟踪，通过本发明的技术方案，能够实时性更高，适合嵌入式低算力控制器使用，既可以利用优化理论权衡目标，寻求近似最优解，又适宜实车应用。

Description

一种路径跟踪方法、装置、设备及存储介质

技术领域

本发明实施例涉及计算机技术，尤其涉及一种路径跟踪方法、装置、设备及存储介质。

背景技术

路径跟踪作为自动驾驶系统中的关键执行层控制技术，通过控制车辆转向系统实现沿规划道路的精确行驶，影响着智能车辆的安全性与舒适性。常用的路径跟踪方法包括纯跟踪、Stanly等基于几何/运动模型的方法，将车辆简化为四轮机器人刚性结构，结构简单计算量小，但由于忽略了车辆的侧偏特性，在车速较高的场景下跟踪精度不高，而基于车辆动力学模型的路径跟踪方法，通常可以克服此问题，当前此类方法通常采用模型预测控制(MPC)的优化方法，通常其采用预测窗口内路径跟踪偏差和侧向加速度及侧向加速度变化率等指标综合最小等方式，保证跟踪精度及舒适性，但是其需要在线优化求解最优值，需要消耗大量的计算资源，不适宜在现有车辆嵌入式控制器中使用，因此，需要一种适宜实车嵌入式控制器使用的低计算量，同时可以综合优化跟踪偏差及跟踪舒适性的路径跟踪方法。

发明内容

本发明实施例提供一种路径跟踪方法、装置、设备及存储介质，以实现实时性更高，适合嵌入式低算力控制器使用，既可以利用优化理论权衡目标，寻求近似最优解，又适宜实车应用。

第一方面，本发明实施例提供了一种路径跟踪方法，包括：

获取目标状态方程和目标函数，其中，所述目标状态方程包括状态量；

根据所述目标状态方程和目标函数利用LQR控制算法确定最优控制增益；

根据所述最优控制增益与所述状态量的乘积确定最优控制量；

根据所述最优控制量控制方向盘转角，以使车辆进行目标路径跟踪。

第二方面，本发明实施例还提供了一种路径跟踪装置，该装置包括：

获取模块，用于获取目标状态方程和目标函数，其中，所述目标状态方程包括状态量；

第一确定模块，用于根据所述目标状态方程和目标函数利用LQR控制算法确定最优控制增益；

第二确定模块，用于根据所述最优控制增益与所述状态量的乘积确定最优控制量；

控制模块，用于根据所述最优控制量控制方向盘转角，以使车辆进行目标路径跟踪。

第三方面，本发明实施例还提供了一种计算机设备，包括存储器、处理器及存储在存储器上并可在处理器上运行的计算机程序，所述处理器执行所述程序时实现如本发明实施例中任一所述的路径跟踪方法。

第四方面，本发明实施例还提供了一种计算机可读存储介质，其上存储有计算机程序，该程序被处理器执行时实现如本发明实施例中任一所述的路径跟踪方法。

本发明实施例通过获取目标状态方程和目标函数，其中，所述目标状态方程包括状态量；根据所述目标状态方程和目标函数利用LQR控制算法确定最优控制增益；根据所述最优控制增益与所述状态量的乘积确定最优控制量；根据所述最优控制量控制方向盘转角，以使车辆进行目标路径跟踪，能够实时性更高，适合嵌入式低算力控制器使用，既可以利用优化理论权衡目标，寻求近似最优解，又适宜实车应用。

附图说明

为了更清楚地说明本发明实施例的技术方案，下面将对实施例中所需要使用的附图作简单地介绍，应当理解，以下附图仅示出了本发明的某些实施例，因此不应被看作是对范围的限定，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他相关的附图。

图1是本发明实施例一中的一种路径跟踪方法的流程图；

图2A是本发明实施例二中的一种路径跟踪方法的流程图；

图2B是本发明实施例二中的车辆动力学模型；

图2C是本发明实施例二中的道路预瞄关系图；

图3是本发明实施例三中的一种路径跟踪装置的结构示意图；

图4是本发明实施例四中的一种计算机设备的结构示意图。

具体实施方式

下面结合附图和实施例对本发明作进一步的详细说明。可以理解的是，此处所描述的具体实施例仅仅用于解释本发明，而非对本发明的限定。另外还需要说明的是，为了便于描述，附图中仅示出了与本发明相关的部分而非全部结构。

应注意到：相似的标号和字母在下面的附图中表示类似项，因此，一旦某一项在一个附图中被定义，则在随后的附图中不需要对其进行进一步定义和解释。同时，在本发明的描述中，术语“第一”、“第二”等仅用于区分描述，而不能理解为指示或暗示相对重要性。

实施例一

图1为本发明实施例一提供的一种路径跟踪方法的流程图，本实施例可适用于路径跟踪的情况，该方法可以由本发明实施例中的路径跟踪装置来执行，该装置可采用软件和/或硬件的方式实现，如图1所示，该方法具体包括如下步骤：

S110，获取目标状态方程和目标函数，其中，所述目标状态方程包括状态量。

其中，所述目标状态方程为根据车辆动力学模型、道路预瞄模型以及平顺性模型构建的状态方程。

其中，所述目标函数能够优化预瞄窗口内的路径跟踪横向偏差，以及平顺性模型中前两个周期的控制量变化率。

具体的，获取目标状态方程和目标函数，例如可以是，预先建立车辆动力学模型、道路预瞄模型以及平顺性模型，然后根据车辆动力学模型、道路预瞄模型以及平顺性模型构建目标状态方程，构建目标函数，用来优化预瞄窗口内的路径跟踪横向偏差，以及平顺性模型中前两个周期的控制量变化率。

S120，根据所述目标状态方程和目标函数利用LQR控制算法确定最优控制增益。

具体的，根据目标状态方程和目标函数利用LQR控制算法确定最优控制增益的方式可以为，根据目标函数和目标状态方程构成标准的离散LQR控制问题，由离散LQR问题的标准形式，确定最优控制增益。

在一个具体的例子中，目标状态方程为：

X_opt(k+1)＝A_optX_opt(k)+B_optδ_sw(k)，

其中，

目标函数为：

其中，q₁,q₂,q₃分别为跟踪横向偏差、跟踪角度偏差、方向盘转角变化率的目标权重，R为控制量方向盘转角值得目标权重，关系到一定车速u下的车辆侧向加速度，由目标函数和目标状态方程，即可构成标准的离散LQR控制问题，由离散LQR问题的标准形式可知其最优控制增益为：K＝[R+B_opt ^TPB_opt]^-1B_opt ^TPA_opt。

S130，根据所述最优控制增益与所述状态量的乘积确定最优控制量。

具体的，由于离线优化部分已经计算得到了不同车速下的最优控制增益，在线实时控制部分仅需要根据当前车速查表插值获得当前最优控制增益。根据当前车辆位姿状态，道路状态，先前周期控制量状态更新当前k时刻的状态矩阵X_opt(k)中的状态量

通过最优增益状态量相乘即可获得当前最优控制量。

S140，根据所述最优控制量控制方向盘转角，以使车辆进行目标路径跟踪。

具体的，通过最优控制量控制方向盘转角使得车辆进行目标路径跟踪。

本发明实施例提出了一种基于LQR的路径跟踪方法，并能够综合考虑跟踪偏差及舒适度，包括离线优化部分中车辆动力学模型构建、道路预瞄模型构建、平顺性模型构建、优化目标构建、最优控制增益计算，还包括在线实时控制部分，基于本方法可以利用现代控制理论优化求解用于路径跟踪控制的最优目标方向盘转角控制量，兼顾跟踪精度与舒适度，同时通过离线求解的方式可大幅提升求解速度，适用于嵌入式控制器的实时应用。

本实施例的技术方案，通过获取目标状态方程和目标函数，其中，所述目标状态方程包括状态量；根据所述目标状态方程和目标函数利用LQR控制算法确定最优控制增益；根据所述最优控制增益与所述状态量的乘积确定最优控制量；根据所述最优控制量控制方向盘转角，以使车辆进行目标路径跟踪，能够实时性更高，适合嵌入式低算力控制器使用，既可以利用优化理论权衡目标，寻求近似最优解，又适宜实车应用。

实施例二

图2A为本发明实施例二中的一种路径跟踪方法的流程图，本实施例以上述实施例为基础进行优化，在本实施例中，获取目标状态方程包括：构建车辆动力学模型、道路预瞄模型以及平顺性模型；根据所述车辆动力学模型、道路预瞄模型以及平顺性模型确定目标状态方程。

如图2A所示，本实施例的方法具体包括如下步骤：

S210，构建车辆动力学模型、道路预瞄模型以及平顺性模型。

可选的，所述车辆动力学模型如下：

其中，

y为车辆横向位置；a、b分别为车辆质心到前后轴的距离；ψ为车辆航向角；u为车辆纵向速度；δ_sw为方向盘转角；i_steer为转向传动比；C_f、C_r为车辆前后轴的侧偏刚度；M为整车质量，I_z为车辆绕Z轴横摆的转动惯量。

可选的，所述道路预瞄模型如下：

y_r(k+1)＝Dy_r(k)+Ey_{r_n+1}；

其中，y_r＝[y_r0 y_r1 y_r2 ... y_rn]^T，

y_r0为第一个道路状态量，y_r1为第二个道路状态量，y_rn为第n个道路状态量，y_{r_n+1}为最后一时刻的道路状态。

可选的，所述平顺性模型如下：

Z(k+1)＝FZ(k)+Gδ_sw

其中，δ_sw(k-1)为系统状态量当前时刻前一周期的控制量，δ_sw(k-2)为系统状态量当前时刻前两个周期的控制量。

S220，根据所述车辆动力学模型、道路预瞄模型以及平顺性模型确定目标状态方程。

可选的，所述目标状态方程如下：

X_opt(k+1)＝A_optX_opt(k)+B_optδ_sw(k)

其中：A_d＝(I+T·A/2)/(I-T·A/2),B_d＝T·B。

S230，获取目标函数。

可选的，所述目标函数如下：

其中，

q₁,q₂,q₃分别为跟踪横向偏差、跟踪角度偏差、方向盘转角变化率的目标权重，R为控制量方向盘转角值的目标权重。

S240，根据所述目标状态方程和目标函数利用LQR控制算法确定最优控制增益。

S250，根据所述最优控制增益与所述状态量的乘积确定最优控制量。

S260，根据所述最优控制量控制方向盘转角，以使车辆进行目标路径跟踪。

在一个具体的例子中，本发明实施例提供一种路径跟踪方法，分为离线优化部分和在线实时控制部分。其中，离线优化部分包括车辆动力学模型构建、道路预瞄模型构建、平顺性模型构建、优化目标构建、最优控制增益计算。车辆动力学模型构建，本发明实施例中的车辆动力学模型主要用于根据路径跟踪的控制量方向盘转角输入量，估计车辆的位姿变化状态，用于优化较为合理的控制量输入。假设车辆是一个在平面内沿一定速度向前行驶的刚体，可通过前轮转角进行横摆旋转和侧向平移运动，通过如图2B所示的车辆动力学模型，即可确定车辆在一定速度下以方向盘转角作为控制量，对车辆横向位置及横摆角速度状态产生的影响。

所构建的车辆状态方程如下：

其中，

式中y为车辆横向位置；a、b分别为车辆质心到前后轴的距离；ψ为车辆航向角；u为车辆纵向速度；δ_sw为方向盘转角；i_steer为转向传动比；Cf、Cr为车辆前后轴的侧偏刚度；M为整车质量，I_z为车辆绕Z轴横摆的转动惯量，上式此状态方程为连续量，在控制器中采用的是离散数字控制，需要根据实际采样时间T对状态方程进行离散化处理，如下式所示。

X(k+1)＝A_d·X(k)+B_d·δ_sw

其中，A_d＝(I+T·A/2)/(I-T·A/2)，B_d＝T·B

道路预瞄模型构建，本发明实施例中的道路预瞄模型构建主要用于与车辆模型结合预测未来车辆跟踪位置，衡量车辆的总体跟踪偏差。如图2C所示，以道路在局部坐标系下的未来n个周期的参考车辆横向偏移作为道路状态量，当前车辆在车辆坐标系中的横向坐标为0，朝向角为0，假设车辆纵向速度u恒定，当前车辆坐标为0，第一个道路状态量即为y_r0，第二个状态量即车辆沿车辆坐标系x轴方向移动uT后，对应的道路y坐标，即为图2C中的y_r1，以此类推再下一周期同样x向移动uT，状态量变为y_r2,y_r3…y_rn。

对于车辆而言，k时刻的道路状态y_rn即为k+1时刻的道路状态y_rn-1，只需将前一时刻的道路状态整体向前移动一个周期，同时补充最后一时刻的状态y_{r_n+1}，可将道路状态整理为如下矩阵形式：

y_r(k+1)＝Dy_r(k)+Ey_{r_n+1}

其中，y_r＝[y_r0 y_r1 y_r2 ... y_rn]^T

平顺性模型构建，本发明实施例中平顺性方面主要考虑控制量的变化率，即方向盘转角变化率，其影响着车辆行驶过程中人体较为敏感的侧向加加速度，因此在模型中考虑前两周期的控制量作为状态，用于衡量控制量的变化，其模型如下：

其中，系统状态量为当前时刻前一周期的控制量δ_sw(k-1)，及前两个周期的控制量δ_sw(k-2)，可用这两个状态的变化衡量系统控制量的变化率，从而优化系统的平顺性，将其整理为标准状态方程的形式如下：

Z(k+1)＝FZ(k)+Gδ_sw；

其中，

优化目标构建，构建优化目标前首先将车辆模型、道路预瞄模型、平顺性模型的状态方程进行整合，可得到如下目标状态方程：

X_opt(k+1)＝A_optX_opt(k)+B_optδ_sw(k)；

其中：

本发明实施例中的优化目标为结合车辆模型及道路预瞄模型，优化预瞄窗口(n)内的路径跟踪横向偏差，以及平顺性模型中前两个周期的控制量变化率，因此构建的目标函数如下：

其中，

q₁,q₂,q₃分别为跟踪横向偏差、跟踪角度偏差、方向盘转角变化率的目标权重，R为控制量方向盘转角值得目标权重，关系到一定车速u下的车辆侧向加速度。最优增益计算：由目标函数和目标状态方程，即可构成标准的离散LQR控制问题，由离散LQR问题的标准形式可知其最优控制增益为：

K＝[R+B_opt ^TPB_opt]^-1B_opt ^TPA_opt；

其中，P矩阵可通过求解如下代数黎卡提方程获得：

通过观察上述矩阵中的变量可以发现同一车辆，在控制参数目标权重确定的情况下，仅有车速u动态变化，因此在实施本方法的时候可通过计算不同离散车速u下最优控制增益K，然后在线控制时直接根据车速进行查表获得当前车速下的对应最优控制增益，即可大幅减小在线计算量，提升算法实时性。

在线实时控制部分：由于离线优化部分已经计算得到了不同车速下的最优控制增益，在线实时控制部分仅需要根据当前车速查表插值获得当前最优控制增益K，然后根据当前车辆位姿状态，道路状态，先前周期控制量状态更新当前k时刻的状态矩阵X_opt(k)中的状态量

通过最有增益与状态量相乘即可获得当前最优控制量，用于控制方向盘转角使得车辆进行目标路径跟踪即可，δ_sw(k)＝-K·X_opt(k)。

本实施例的技术方案，通过获取目标状态方程和目标函数，其中，所述目标状态方程包括状态量；根据所述目标状态方程和目标函数利用LQR控制算法确定最优控制增益；根据所述最优控制增益与所述状态量的乘积确定最优控制量；根据所述最优控制量控制方向盘转角，以使车辆进行目标路径跟踪，能够实时性更高，适合嵌入式低算力控制器使用，既可以利用优化理论权衡目标，寻求近似最优解，又适宜实车应用。

实施例三

图3为本发明实施例三提供的一种路径跟踪装置的结构示意图。本实施例可适用于路径跟踪的情况，该装置可采用软件和/或硬件的方式实现，该装置可集成在任何提供路径跟踪功能的设备中，如图3所示，所述路径跟踪装置具体包括：获取模块310、第一确定模块320、第二确定模块330和控制模块340。

其中，获取模块，用于获取目标状态方程和目标函数，其中，所述目标状态方程包括状态量；

第一确定模块，用于根据所述目标状态方程和目标函数利用LQR控制算法确定最优控制增益；

第二确定模块，用于根据所述最优控制增益与所述状态量的乘积确定最优控制量；

控制模块，用于根据所述最优控制量控制方向盘转角，以使车辆进行目标路径跟踪。

可选的，获取模块用于：

构建车辆动力学模型、道路预瞄模型以及平顺性模型；

根据所述车辆动力学模型、道路预瞄模型以及平顺性模型确定目标状态方程。

可选的，所述车辆动力学模型如下：

其中，

y为车辆横向位置；a、b分别为车辆质心到前后轴的距离；ψ为车辆航向角；u为车辆纵向速度；δ_sw为方向盘转角；i_steer为转向传动比；C_f、C_r为车辆前后轴的侧偏刚度；M为整车质量，I_z为车辆绕Z轴横摆的转动惯量。

可选的，所述道路预瞄模型如下：

y_r(k+1)＝Dy_r(k)+Ey_{r_n+1}；

其中，y_r＝[y_r0 y_r1 y_r2 ... y_rn]^T，

y_r0为第一个道路状态量，y_r1为第二个道路状态量，y_rn为第n个道路状态量，y_{r_n+1}为最后一时刻的道路状态。

可选的，所述平顺性模型如下：

Z(k+1)＝FZ(k)+Gδ_sw；

其中，δ_sw(k-1)为系统状态量当前时刻前一周期的控制量，δ_sw(k-2)为系统状态量当前时刻前两个周期的控制量。

可选的，所述目标状态方程如下：

X_opt(k+1)＝A_optX_opt(k)+B_optδ_sw(k)

其中：

A_d＝(I+T·A/2)/(I-T·A/2),B_d＝T·B。

可选的，所述目标函数如下：

其中，

q₁,q₂,q₃分别为跟踪横向偏差、跟踪角度偏差、方向盘转角变化率的目标权重，R为控制量方向盘转角值的目标权重。

上述产品可执行本发明任意实施例所提供的方法，具备执行方法相应的功能模块和有益效果。

本实施例的技术方案，通过获取目标状态方程和目标函数，其中，所述目标状态方程包括状态量；根据所述目标状态方程和目标函数利用LQR控制算法确定最优控制增益；根据所述最优控制增益与所述状态量的乘积确定最优控制量；根据所述最优控制量控制方向盘转角，以使车辆进行目标路径跟踪，能够实时性更高，适合嵌入式低算力控制器使用，既可以利用优化理论权衡目标，寻求近似最优解，又适宜实车应用。

实施例四

图4为本发明实施例四中的一种计算机设备的结构示意图。图4示出了适于用来实现本发明实施方式的示例性计算机设备12的框图。图4显示的计算机设备12仅仅是一个示例，不应对本发明实施例的功能和使用范围带来任何限制。

如图4所示，计算机设备12以通用计算设备的形式表现。计算机设备12的组件可以包括但不限于：一个或者多个处理器或者处理单元16，系统存储器28，连接不同系统组件(包括系统存储器28和处理单元16)的总线18。

总线18表示几类总线结构中的一种或多种，包括存储器总线或者存储器控制器，外围总线，图形加速端口，处理器或者使用多种总线结构中的任意总线结构的局域总线。举例来说，这些体系结构包括但不限于工业标准体系结构(ISA)总线，微通道体系结构(MAC)总线，增强型ISA总线、视频电子标准协会(VESA)局域总线以及外围组件互连(PCI)总线。

计算机设备12典型地包括多种计算机系统可读介质。这些介质可以是任何能够被计算机设备12访问的可用介质，包括易失性和非易失性介质，可移动的和不可移动的介质。

系统存储器28可以包括易失性存储器形式的计算机系统可读介质，例如随机存取存储器(RAM)30和/或高速缓存存储器32。计算机设备12可以进一步包括其它可移动/不可移动的、易失性/非易失性计算机系统存储介质。仅作为举例，存储系统34可以用于读写不可移动的、非易失性磁介质(图4未显示，通常称为“硬盘驱动器”)。尽管图4中未示出，可以提供用于对可移动非易失性磁盘(例如“软盘”)读写的磁盘驱动器，以及对可移动非易失性光盘(例如CD-ROM,DVD-ROM或者其它光介质)读写的光盘驱动器。在这些情况下，每个驱动器可以通过一个或者多个数据介质接口与总线18相连。存储器28可以包括至少一个程序产品，该程序产品具有一组(例如至少一个)程序模块，这些程序模块被配置以执行本发明各实施例的功能。

具有一组(至少一个)程序模块42的程序/实用工具40，可以存储在例如存储器28中，这样的程序模块42包括——但不限于——操作系统、一个或者多个应用程序、其它程序模块以及程序数据，这些示例中的每一个或某种组合中可能包括网络环境的实现。程序模块42通常执行本发明所描述的实施例中的功能和/或方法。

计算机设备12也可以与一个或多个外部设备14(例如键盘、指向设备、显示器24等)通信，还可与一个或者多个使得用户能与该计算机设备12交互的设备通信，和/或与使得该计算机设备12能与一个或多个其它计算设备进行通信的任何设备(例如网卡，调制解调器等等)通信。这种通信可以通过输入/输出(I/O)接口22进行。另外，本实施例中的计算机设备12，显示器24不是作为独立个体存在，而是嵌入镜面中，在显示器24的显示面不予显示时，显示器24的显示面与镜面从视觉上融为一体。并且，计算机设备12还可以通过网络适配器20与一个或者多个网络(例如局域网(LAN)，广域网(WAN)和/或公共网络，例如因特网)通信。如图所示，网络适配器20通过总线18与计算机设备12的其它模块通信。应当明白，尽管图中未示出，可以结合计算机设备12使用其它硬件和/或软件模块，包括但不限于：微代码、设备驱动器、冗余处理单元、外部磁盘驱动阵列、RAID系统、磁带驱动器以及数据备份存储系统等。

处理单元16通过运行存储在系统存储器28中的程序，从而执行各种功能应用以及数据处理，例如实现本发明实施例所提供的路径跟踪方法：获取目标状态方程和目标函数，其中，所述目标状态方程包括状态量；根据所述目标状态方程和目标函数利用LQR控制算法确定最优控制增益；根据所述最优控制增益与所述状态量的乘积确定最优控制量；根据所述最优控制量控制方向盘转角，以使车辆进行目标路径跟踪。

实施例五

本发明实施例五提供了一种计算机可读存储介质，其上存储有计算机程序，该程序被处理器执行时实现如本申请所有发明实施例提供的路径跟踪方法：获取目标状态方程和目标函数，其中，所述目标状态方程包括状态量；根据所述目标状态方程和目标函数利用LQR控制算法确定最优控制增益；根据所述最优控制增益与所述状态量的乘积确定最优控制量；根据所述最优控制量控制方向盘转角，以使车辆进行目标路径跟踪。

可以采用一个或多个计算机可读的介质的任意组合。计算机可读介质可以是计算机可读信号介质或者计算机可读存储介质。计算机可读存储介质例如可以是——但不限于——电、磁、光、电磁、红外线、或半导体的系统、装置或器件，或者任意以上的组合。计算机可读存储介质的更具体的例子(非穷举的列表)包括：具有一个或多个导线的电连接、便携式计算机磁盘、硬盘、随机存取存储器(RAM)、只读存储器(ROM)、可擦式可编程只读存储器(EPROM或闪存)、光纤、便携式紧凑磁盘只读存储器(CD-ROM)、光存储器件、磁存储器件、或者上述的任意合适的组合。在本文件中，计算机可读存储介质可以是任何包含或存储程序的有形介质，该程序可以被指令执行系统、装置或者器件使用或者与其结合使用。

计算机可读的信号介质可以包括在基带中或者作为载波一部分传播的数据信号，其中承载了计算机可读的程序代码。这种传播的数据信号可以采用多种形式，包括——但不限于——电磁信号、光信号或上述的任意合适的组合。计算机可读的信号介质还可以是计算机可读存储介质以外的任何计算机可读介质，该计算机可读介质可以发送、传播或者传输用于由指令执行系统、装置或者器件使用或者与其结合使用的程序。

计算机可读介质上包含的程序代码可以用任何适当的介质传输，包括——但不限于——无线、电线、光缆、RF等等，或者上述的任意合适的组合。

可以以一种或多种程序设计语言或其组合来编写用于执行本发明操作的计算机程序代码，所述程序设计语言包括面向对象的程序设计语言—诸如Java、Smalltalk、C++，还包括常规的过程式程序设计语言—诸如“C”语言或类似的程序设计语言。程序代码可以完全地在用户计算机上执行、部分地在用户计算机上执行、作为一个独立的软件包执行、部分在用户计算机上部分在远程计算机上执行、或者完全在远程计算机或服务器上执行。在涉及远程计算机的情形中，远程计算机可以通过任意种类的网络——包括局域网(LAN)或广域网(WAN)—连接到用户计算机，或者，可以连接到外部计算机(例如利用因特网服务提供商来通过因特网连接)。

注意，上述仅为本发明的较佳实施例及所运用技术原理。本领域技术人员会理解，本发明不限于这里所述的特定实施例，对本领域技术人员来说能够进行各种明显的变化、重新调整和替代而不会脱离本发明的保护范围。因此，虽然通过以上实施例对本发明进行了较为详细的说明，但是本发明不仅仅限于以上实施例，在不脱离本发明构思的情况下，还可以包括更多其他等效实施例，而本发明的范围由所附的权利要求范围决定。

Claims (10)

1.一种路径跟踪方法，其特征在于，包括：

获取目标状态方程和目标函数，其中，所述目标状态方程包括状态量；

根据所述目标状态方程和目标函数利用LQR控制算法确定最优控制增益；

根据所述最优控制增益与所述状态量的乘积确定最优控制量；

根据所述最优控制量控制方向盘转角，以使车辆进行目标路径跟踪。

2.根据权利要求1所述的方法，其特征在于，获取目标状态方程包括：

构建车辆动力学模型、道路预瞄模型以及平顺性模型；

根据所述车辆动力学模型、道路预瞄模型以及平顺性模型确定目标状态方程。

3.根据权利要求2所述的方法，其特征在于，所述车辆动力学模型如下：

其中，

y为车辆横向位置；a、b分别为车辆质心到前后轴的距离；ψ为车辆航向角；u为车辆纵向速度；δ_sw为方向盘转角；i_steer为转向传动比；C_f、C_r为车辆前后轴的侧偏刚度；M为整车质量，I_z为车辆绕Z轴横摆的转动惯量。

4.根据权利要求3所述的方法，其特征在于，所述道路预瞄模型如下：

y_r(k+1)＝Dy_r(k)+Ey_{r_n+1}；

其中，y_r＝[y_r0 y_r1 y_r2... y_rn]^T，

y_r0为第一个道路状态量，y_r1为第二个道路状态量，y_rn为第n个道路状态量，y_{r_n+1}为最后一时刻的道路状态。

5.根据权利要求4所述的方法，其特征在于，所述平顺性模型如下：

Z(k+1)＝FZ(k)+Gδ_sw；

其中，

δ_sw(k-1)为系统状态量当前时刻前一周期的控制量，δ_sw(k-2)为系统状态量当前时刻前两个周期的控制量。

6.根据权利要求5所述的方法，其特征在于，所述目标状态方程如下：

X_opt(k+1)＝A_optX_opt(k)+B_optδ_sw(k)

其中：A_d＝(I+T·A/2)/(I-T·A/2),B_d＝T·B。

7.根据权利要求6所述的方法，其特征在于，所述目标函数如下：

其中，

q₁,q₂,q₃分别为跟踪横向偏差、跟踪角度偏差、方向盘转角变化率的目标权重，R为控制量方向盘转角值的目标权重。

8.一种路径跟踪装置，其特征在于，包括：

获取模块，用于获取目标状态方程和目标函数，其中，所述目标状态方程包括状态量；

第一确定模块，用于根据所述目标状态方程和目标函数利用LQR控制算法确定最优控制增益；

第二确定模块，用于根据所述最优控制增益与所述状态量的乘积确定最优控制量；

控制模块，用于根据所述最优控制量控制方向盘转角，以使车辆进行目标路径跟踪。

9.一种计算机设备，包括存储器、处理器及存储在存储器上并可在处理器上运行的计算机程序，其特征在于，所述处理器执行所述程序时实现如权利要求1-7中任一所述的方法。

10.一种计算机可读存储介质，其上存储有计算机程序，其特征在于，该程序被处理器执行时实现如权利要求1-7中任一所述的方法。

Images