基于线性迭代二次调节器的自动驾驶轨迹规划方法及系统

技术领域

本申请涉及智能辅助驾驶技术领域，尤其涉及基于线性迭代二次调节器的自动驾驶轨迹规划方法及系统。

背景技术

自动驾驶技术因其能够提高交通安全、效率和便利性，节约能源成为热点研究领域。轨迹规划是自动驾驶系统的核心功能模块。其性能的优异与否将直接决定自动驾驶系统的整体性能。目前轨迹规划主要有两种技术方案，分别为解耦轨迹规划和时空耦合轨迹规划。其中解耦轨迹规划采用路径、速度依次规划的方法。这种方案简化了轨迹规划问题，具有算法实时性好、易于部署等优点。但在应对复杂动态场景时，由于速度与路径解耦车辆行为会呈现出刻板、拟人性差等诸多不足。此外，该方法通常需要设计轨迹跟踪器来跟踪轨迹，会在规划与控制层之间引入不匹配问题。时空耦合轨迹规划方案因注重路径与速度之间的耦合关系，因此其具备同时协调车辆的横纵向运动能力，对复杂场景的处理能力更强。

常用的时空轨迹规划方法有基于采样、基于搜索、基于优化的方法。其中搜索和采样的方法虽然都具备在较大的时空范围内探索解的能力，但是也面临着各自的不足。如基于采样的方法其解的最优性与时空域采样分辨率之间存在难以协调的矛盾。较小的分辨率虽能提高探索解的能力，但会极大增加算法的复杂度，实时性难以保证。基于搜索的方法通常需要额外的先验知识或决策模型提供目标搜索点。受离散动作空间限制，方法难以覆盖时空域中所有可行解。基于优化的方法本质上是基于数值优化技术求解满足多约束的最优轨迹问题。其解虽天然具有时空性、最优性，但受限于问题的非凸性、非线性、及通用求解器的局限性，其难以广泛适应现实中复杂的交通场景。现有的时空轨迹规划方法都有其各种的优点与自身的局限性。此外，现有的时空轨迹规划研究大多以非失稳工况为前提。受限于车辆动力学特性及轮胎特性，车辆在高速低附、紧急避障等一些紧急工况下易出现失稳问题。

综上，相关技术中存在的技术问题有待得到改善。

发明内容

本申请实施例的主要目的在于提出一种基于线性迭代二次调节器的自动驾驶轨迹规划方法及系统，能够提高时空域的探索效率并增强自动驾驶汽车处理复杂场景的应对能力，从而提高自动驾驶轨迹规划的可靠性。

为实现上述目的，本申请实施例的一方面提出了一种基于线性迭代二次调节器的自动驾驶轨迹规划方法，所述方法包括：

根据自动驾驶汽车的当前时刻状态信息、障碍物的轨迹预测信息以及交通规则条件，通过可达状态集方法进行推演，构建自动驾驶汽车可行驶凸走廊；

根据所述自动驾驶汽车可行驶凸走廊，引入自动驾驶汽车运动约束条件与自动驾驶汽车成本控制项条件，构建自动驾驶汽车的轨迹优化目标函数；

通过指数障碍函数对所述自动驾驶汽车的轨迹优化目标函数进行转换处理，得到融合有约束成本项的自动驾驶汽车轨迹优化目标函数；

通过迭代线性二次调节器对所述融合有约束成本项的自动驾驶汽车轨迹优化目标函数进行迭代优化求解，得到自动驾驶汽车期望前轮转角与自动驾驶汽车期望加速度；

引入稳定性控制器并结合所述自动驾驶汽车期望前轮转角，获取自动驾驶汽车前轮转角的附加横摆力矩；

根据所述自动驾驶汽车期望加速度与所述自动驾驶汽车前轮转角的附加横摆力矩进行优化调节，输出自动驾驶汽车力矩控制量；

根据所述自动驾驶汽车力矩控制量对所述自动驾驶汽车进行驾驶轨迹规划，得到轨迹规划结果。

在一些实施例中，所述根据自动驾驶汽车的当前时刻状态信息、障碍物的轨迹预测信息以及交通规则条件，通过可达状态集方法进行推演，构建自动驾驶汽车可行驶凸走廊，包括：

根据所述自动驾驶汽车的当前时刻状态信息与所述交通规则条件，通过运动学模型获取车辆可达状态集；

基于所述车辆可达状态集进行均匀撒点采样处理，并结合所述障碍物的轨迹预测信息及碰撞风险评估模型，构建风险场；

构建预设风险等级指数，对所述风险场进行栅格化，得到若干子集风险场，且若干所述子集风险场的风险等级指数的极差均小于预设风险等级指数；

选取低于所述预设风险等级指数对应的若干所述子集风险场并通过动态规划算法进行搜索，构建自动驾驶汽车可行驶凸走廊。

在一些实施例中，所述根据所述自动驾驶汽车可行驶凸走廊，引入自动驾驶汽车运动约束条件与自动驾驶汽车成本控制项条件，构建自动驾驶汽车的轨迹优化目标函数，包括：

根据所述自动驾驶汽车的当前时刻状态信息，构建自动驾驶汽车动力学约束条件；

根据所述自动驾驶汽车的运动范围限制，构建自动驾驶汽车物理极限约束条件；

根据所述自动驾驶汽车可行驶凸走廊的边界，构建自动驾驶汽车走廊边界约束条件，所述自动驾驶汽车走廊边界约束条件包括走廊横向边界约束与走廊纵向边界约束；

整合所述自动驾驶汽车动力学约束条件、所述自动驾驶汽车物理极限约束条件和所述自动驾驶汽车走廊边界约束条件，构建自动驾驶汽车运动约束条件；

引入所述自动驾驶汽车的加速度权重系数，构建自动驾驶汽车控制成本项条件；

引入所述自动驾驶汽车的前后车距离权重系数，构建自动驾驶汽车跟踪成本项条件；

引入所述自动驾驶汽车的横摆角权重系数，构建自动驾驶汽车终端成本项条件；

整合所述自动驾驶汽车控制成本项条件、所述自动驾驶汽车跟踪成本项条件和所述自动驾驶汽车终端成本项条件，构建自动驾驶汽车成本控制项条件；

结合所述自动驾驶汽车运动约束条件与所述自动驾驶汽车成本控制项条件，得到所述自动驾驶汽车的轨迹优化目标函数。

在一些实施例中，所述通过指数障碍函数对所述自动驾驶汽车的轨迹优化目标函数进行转换处理，得到融合有约束成本项的自动驾驶汽车轨迹优化目标函数，包括：

基于指数障碍函数，将所述自动驾驶汽车的轨迹优化目标函数中的走廊横向边界约束进行转换处理，得到上边界约束成本与下边界约束成本；

将所述自动驾驶汽车的轨迹优化目标函数中的走廊纵向边界约束进行转换处理，得到前边界约束成本与后边界约束成本；

将所述自动驾驶汽车的轨迹优化目标函数中的自动驾驶汽车物理极限约束条件进行转换处理，得到控制量最大值约束成本与控制量最小值约束成本；

将所述上边界约束成本、所述下边界约束成本、所述前边界约束成本与所述后边界约束成本、所述控制量最大值约束成本与所述控制量最小值约束成本融合至所述自动驾驶汽车的轨迹优化目标函数，得到融合有约束成本项的自动驾驶汽车轨迹优化目标函数。

在一些实施例中，所述融合有约束成本项的自动驾驶汽车轨迹优化目标函数的表达式具体如下所示：

s.t.x

上式中，J(X,U)表示融合有约束成本项的自动驾驶汽车轨迹优化目标函数，c

在一些实施例中，所述通过迭代线性二次调节器对所述融合有约束成本项的自动驾驶汽车轨迹优化目标函数进行迭代优化求解，得到自动驾驶汽车期望前轮转角与自动驾驶汽车期望加速度，包括：

对所述融合有约束成本项的自动驾驶汽车轨迹优化目标函数进行简化处理，得到简化的融合有约束成本项的自动驾驶汽车轨迹优化目标函数；

根据所述自动驾驶汽车的当前时刻状态信息，给定初始控制序列，并结合所述自动驾驶汽车动力学约束条件进行前向递推处理，得到自动驾驶汽车初始状态序列；

通过所述迭代线性二次调节器，基于所述简化的融合有约束成本项的自动驾驶汽车轨迹优化目标函数对所述自动驾驶汽车初始状态序列进行求解计算，得到第一自动驾驶汽车初始状态序列代价；

根据所述自动驾驶汽车的当前时刻状态信息与所述给定初始控制序列进行后向递推处理，得到前向增益与反馈增益；

根据所述前向增益与所述反馈增益，获取自动驾驶汽车下一状态序列；

基于所述简化的融合有约束成本项的自动驾驶汽车轨迹优化目标函数对所述自动驾驶汽车下一状态序列进行求解计算，得到第二自动驾驶汽车初始状态序列代价；

对所述第一自动驾驶汽车初始状态序列代价与所述第二自动驾驶汽车初始状态序列代价进行差值计算处理，得到差值计算结果；

对所述差值计算结果进行判断，若所述差值计算结果满足预设收敛条件，则输出自动驾驶汽车下一状态序列中的元素，所述元素包括自动驾驶汽车期望前轮转角与自动驾驶汽车期望加速度；

若所述差值计算结果不满足预设收敛条件，则循环前向递推处理步骤、求解计算步骤、后向递推处理步骤与差值计算处理步骤，直至所述差值计算结果满足预设收敛条件，输出自动驾驶汽车期望前轮转角与自动驾驶汽车期望加速度。

在一些实施例中，所述根据所述自动驾驶汽车的当前时刻状态信息与所述给定初始控制序列进行后向递推处理，得到前向增益与反馈增益，包括：

通过扰动函数对所述自动驾驶汽车的当前时刻状态信息与所述给定初始控制序列进行求偏导计算处理，得到当前时刻状态信息一阶偏导数、控制序列一阶偏导数、当前时刻状态信息二阶偏导数和控制序列二阶偏导数；

通过价值函数对所述当前时刻状态信息一阶偏导数、所述控制序列一阶偏导数、所述当前时刻状态信息二阶偏导数和所述控制序列二阶偏导数进行计算，得到当前时刻状态信息价值函数与控制序列价值函数；

分别对所述当前时刻状态信息价值函数与所述控制序列价值函数进行求解，得到前向增益与反馈增益。

在一些实施例中，所述引入稳定性控制器并结合所述自动驾驶汽车期望前轮转角，获取自动驾驶汽车前轮转角的附加横摆力矩，包括：

获取所述自动驾驶汽车的横摆角速度、所述自动驾驶汽车的质心侧偏角稳态值、所述自动驾驶汽车的横摆角速度极限值，所述自动驾驶汽车的质心侧偏角极限值；

根据所述质心侧偏角极限值与所述质心侧偏角稳态值，确定所述自动驾驶汽车的期望质心侧偏角；

根据所述横摆角速度与所述横摆角速度极限值，确定所述自动驾驶汽车的期望横摆角速度并进行求偏导计算，得到横摆角速度的一阶导数；

根据所述期望质心侧偏角与所述期望横摆角速度，构建滑模面模型并进行求导计算，得到求导后的滑模面模型；

引入常速趋近率，并结合所述横摆角速度的一阶导数与所述求导后的滑模面模型，构建所述自动驾驶汽车的初步附加横摆力矩；

引入车辆失稳程度评估因子与附加横摆力矩调节因子，并结合所述稳定性控制器对所述初步附加横摆力矩进行调节处理，得到所述自动驾驶汽车前轮转角的附加横摆力矩。

在一些实施例中，所述根据所述自动驾驶汽车期望加速度与所述自动驾驶汽车前轮转角的附加横摆力矩进行优化调节，输出自动驾驶汽车力矩控制量，包括：

引入所述自动驾驶汽车与路面的附着系数，构建附着利用率最小化性能指标；

根据所述附着利用率最小化性能指标，构建自动驾驶汽车力矩控制优化目标函数；

将所述自动驾驶汽车期望加速度与所述自动驾驶汽车前轮转角的附加横摆力矩输入至所述自动驾驶汽车力矩控制优化目标函数进行求解，得到所述自动驾驶汽车力矩控制量。

为实现上述目的，本申请实施例的另一方面提出了一种基于线性迭代二次调节器的自动驾驶轨迹规划系统，所述系统包括：

第一模块，用于根据自动驾驶汽车的当前时刻状态信息、障碍物的轨迹预测信息以及交通规则条件，通过可达状态集方法进行推演，构建自动驾驶汽车可行驶凸走廊；

第二模块，用于根据所述自动驾驶汽车可行驶凸走廊，引入自动驾驶汽车运动约束条件与自动驾驶汽车成本控制项条件，构建自动驾驶汽车的轨迹优化目标函数；

第三模块，用于通过指数障碍函数对所述自动驾驶汽车的轨迹优化目标函数进行转换处理，得到融合有约束成本项的自动驾驶汽车轨迹优化目标函数；

第四模块，用于通过迭代线性二次调节器对所述融合有约束成本项的自动驾驶汽车轨迹优化目标函数进行迭代优化求解，得到自动驾驶汽车期望前轮转角与自动驾驶汽车期望加速度；

第五模块，用于引入稳定性控制器并结合所述自动驾驶汽车期望前轮转角，获取自动驾驶汽车前轮转角的附加横摆力矩；

第六模块，用于根据所述自动驾驶汽车期望加速度与所述自动驾驶汽车前轮转角的附加横摆力矩进行优化调节，输出自动驾驶汽车力矩控制量；

第七模块，用于根据所述自动驾驶汽车力矩控制量对所述自动驾驶汽车进行驾驶轨迹规划，得到轨迹规划结果。

本申请实施例至少包括以下有益效果：本申请提供一种基于线性迭代二次调节器的自动驾驶轨迹规划方法及系统,该方案通过可达状态集方法进行推演，构建自动驾驶汽车可行驶凸走廊，减小无效时空域的探索范围，提高了时空域的探索效率，基于自动驾驶汽车可行驶凸走廊构建自动驾驶汽车的轨迹优化目标函数并通过迭代线性二次调节器进行求解，虑车辆横纵向耦合的动力学特性，有效增强了车辆处理复杂场景的应对能力及提升车辆的轨迹质量，进一步获取自动驾驶汽车前轮转角的附加横摆力矩，结合稳定性控制器输出自动驾驶汽车力矩控制量，扩展自动驾驶汽车轨迹规划的工况适应范围。

附图说明

图1是本申请实施例提供的基于线性迭代二次调节器的自动驾驶轨迹规划方法的流程图；

图2是本申请实施例进行自动驾驶轨迹规划的流程示意图；

图3是本申请实施例构建的复杂场景的示意图；

图4是本申请实施例基于复杂场景下通过本申请实施例方法与现有的RS+QP方法进行对比的车辆速度示意图；

图5是本申请实施例基于复杂场景下通过本申请实施例方法与现有的RS+QP方法进行对比的车辆轨迹示意图；

图6是本申请实施例稳定性控制器有无参与协同工作下的对比测试结果示意图；

图7是本申请实施例提供的基于线性迭代二次调节器的自动驾驶轨迹规划系统的结构示意图。

具体实施方式

为了使本申请的目的、技术方案及优点更加清楚明白，以下结合附图及实施例，对本申请进行进一步详细说明。应当理解，此处所描述的具体实施例仅仅用以解释本申请，并不用于限定本申请。下面的描述涉及附图时，除非另有表示，不同附图中的相同数字表示相同或相似的要素。以下示例性实施例中所描述的实施方式并不代表与本申请实施例相一致的所有实施方式，它们仅是与如所附权利要求书中所详述的、本申请实施例的一些方面相一致的系统和方法的例子。

可以理解，本申请所使用的术语“第一”、“第二”等可在本文中用于描述各种概念，但除非特别说明，这些概念不受这些术语限制。这些术语仅用于将一个概念与另一个概念区分。例如，在不脱离本申请实施例范围的情况下，第一信息也可以被称为第二信息，类似地，第二信息也可以被称为第一信息。取决于语境，如在此所使用的词语“若”、“如果”可以被解释成为“在……时”或“当……时”或“响应于确定”。

本申请所使用的术语“至少一个”、“多个”、“每个”、“任一”等，至少一个包括一个、两个或两个以上，多个包括两个或两个以上，每个是指对应的多个中的每一个，任一是指多个中的任意一个。

除非另有定义，本文所使用的所有的技术和科学术语与属于本申请的技术领域的技术人员通常理解的含义相同。本文中所使用的术语只是为了描述本申请实施例的目的，不是旨在限制本申请。

在对本申请实施例进行详细说明之前，首先对本申请实施例中涉及的部分名词和术语进行说明，本申请实施例中涉及的名词和术语适用于如下的解释。

1)自动驾驶汽车可行驶凸走廊(Convex Hallway)，是指一种特定的道路结构，其中车辆可以沿着凸形的走廊行驶，这种走廊的形状可以保证自动驾驶汽车在行驶过程中始终保持安全。在可行驶凸走廊中，道路的形状和结构可以由多个不同的元素组成，包括直线、曲线、交叉口、道路边界等等。这些元素都被设计成凸形的，以确保自动驾驶汽车在行驶过程中始终处于安全的状态。可行驶凸走廊的主要优点是它可以提供更高的安全性。由于凸形的走廊形状，自动驾驶汽车在行驶过程中可以更好地控制车辆的轨迹和速度，避免与其他车辆或障碍物的碰撞。此外，凸形的走廊还可以提供更好的视野和感知能力，使自动驾驶汽车能够更好地识别和避让其他道路使用者。

相关技术中，存在的一些不足，如拟人性差、轨迹规划与控制之间的不匹配问题、解的最优性与分辨率的矛盾、工况适应范围窄等。

有鉴于此，本申请实施例中提供了一种基于线性迭代二次调节器的自动驾驶轨迹规划方法，通过可达集来提供可行驶凸走廊，减小无效时空域的探索范围。其次，在凸走廊内利用迭代线性二次调节器理论综合考虑车辆动力学约束、走廊边界约束、物理约束等，保证轨迹最优性同时兼顾轨迹规划与控制的耦合。此外，利用协同工作的稳定性控制器扩展轨迹规划的工况适应范围。

参照图1和图2，图1为本发明实施例提供的一种基于线性迭代二次调节器的自动驾驶轨迹规划方法的流程图，参照图1，该方法包括以下步骤：

S100、根据自动驾驶汽车的当前时刻状态信息、障碍物的轨迹预测信息以及交通规则条件，通过可达状态集方法进行推演，构建自动驾驶汽车可行驶凸走廊；

需要说明的是，一些实施例中，步骤S100可以包括：S110、根据所述自动驾驶汽车的当前时刻状态信息与所述交通规则条件，通过运动学模型获取车辆可达状态集；S120、基于所述车辆可达状态集进行均匀撒点采样处理，并结合所述障碍物的轨迹预测信息及碰撞风险评估模型，构建风险场；S130、构建预设风险等级指数，对所述风险场进行栅格化，得到若干子集风险场，且若干所述子集风险场的风险等级指数的极差均小于预设风险等级指数；S140、选取低于所述预设风险等级指数对应的若干所述子集风险场并通过动态规划算法进行搜索，构建自动驾驶汽车可行驶凸走廊。

一些具体实施例中，根据车辆当前时刻的状态信息、交通法规信息，依据运动学模型计算轨迹规划时域内不同时间步的车辆可达状态集，在可达状态集的凸包络内部均匀撒点采样，结合障碍物预测轨迹及碰撞风险评估模型，在可达集凸包内部生成风险场，对生成的风险场栅格化生成若干子集，若子集内部的风险水平的极差高于设定的风险阈值，则对基础子集进一步精细化分割直至无法分割为止，将低于风险阈值的子集标记为低风险子集，高于风险阈值的子集标记为高风险子集。并将低风险子集内部的中心点的状态视为该子集的特征状态予以记录，通过动态规划算法，依据效率指标、安全性、中心性指标在搜索相邻时刻间的最优低风险子集，以包含最优低风险子集的最大安全矩形区域为拓展目标，获取每一时刻的安全走廊边界。

S200、根据所述自动驾驶汽车可行驶凸走廊，引入自动驾驶汽车运动约束条件与自动驾驶汽车成本控制项条件，构建自动驾驶汽车的轨迹优化目标函数；

需要说明的是，一些实施例中，步骤S200可以包括：S210、根据所述自动驾驶汽车的当前时刻状态信息，构建自动驾驶汽车动力学约束条件；S220、根据所述自动驾驶汽车的运动范围限制，构建自动驾驶汽车物理极限约束条件；S230、根据所述自动驾驶汽车可行驶凸走廊的边界，构建自动驾驶汽车走廊边界约束条件，所述自动驾驶汽车走廊边界约束条件包括走廊横向边界约束与走廊纵向边界约束；S240、整合所述自动驾驶汽车动力学约束条件、所述自动驾驶汽车物理极限约束条件和所述自动驾驶汽车走廊边界约束条件，构建自动驾驶汽车运动约束条件；S250、引入所述自动驾驶汽车的加速度权重系数，构建自动驾驶汽车控制成本项条件；S260、引入所述自动驾驶汽车的前后车距离权重系数，构建自动驾驶汽车跟踪成本项条件；S270、引入所述自动驾驶汽车的横摆角权重系数，构建自动驾驶汽车终端成本项条件；S280、整合所述自动驾驶汽车控制成本项条件、所述自动驾驶汽车跟踪成本项条件和所述自动驾驶汽车终端成本项条件，构建自动驾驶汽车成本控制项条件；S290、结合所述自动驾驶汽车运动约束条件与所述自动驾驶汽车成本控制项条件，得到所述自动驾驶汽车的轨迹优化目标函数。

一些具体实施例中，自动驾驶汽车物理极限约束条件的表达式具体如下所示：

上式中，x为系统状态量，且

一些实施例中，自动驾驶汽车物理极限约束条件的表达式为：

上式中，u

一些实施例中，自动驾驶汽车走廊边界约束条件中的走廊横向边界约束的表达式为：

上式中，x

一些实施例中，自动驾驶汽车走廊边界约束条件中的走廊纵向边界约束的表达式为：

上式中，x

一些实施例中，定义diag(a

上式中，w

一些实施例中，建立跟踪成本项的表达式具体为：

上式中，

一些实施例中，建立终端成本项的表达式具体为：

上式中，

一些实施例中，构建自动驾驶汽车的轨迹优化目标函数的表达式为：

s.t.x

g

g

g

g

上式中，n为规划时域总的时间步数，j(·)为轨迹优化目标函数。

S300、通过指数障碍函数对所述自动驾驶汽车的轨迹优化目标函数进行转换处理，得到融合有约束成本项的自动驾驶汽车轨迹优化目标函数；

需要说明的是，一些实施例中，步骤S300可以包括：S310、基于指数障碍函数，将所述自动驾驶汽车的轨迹优化目标函数中的走廊横向边界约束进行转换处理，得到上边界约束成本与下边界约束成本；S320、将所述自动驾驶汽车的轨迹优化目标函数中的走廊纵向边界约束进行转换处理，得到前边界约束成本与后边界约束成本；S330、将所述自动驾驶汽车的轨迹优化目标函数中的自动驾驶汽车物理极限约束条件进行转换处理，得到控制量最大值约束成本与控制量最小值约束成本；S340、将所述上边界约束成本、所述下边界约束成本、所述前边界约束成本与所述后边界约束成本、所述控制量最大值约束成本与所述控制量最小值约束成本融合至所述自动驾驶汽车的轨迹优化目标函数，得到融合有约束成本项的自动驾驶汽车轨迹优化目标函数。

一些实施例中，将不等式约束g

上式中，q1

进一步，将约束g

一些实施例中，建立融合有约束成本项的轨迹优化目标函数的表达式为：

s.t.x

上式中，j(X,U)表示融合有约束成本项的自动驾驶汽车轨迹优化目标函数，c

S400、通过迭代线性二次调节器对所述融合有约束成本项的自动驾驶汽车轨迹优化目标函数进行迭代优化求解，得到自动驾驶汽车期望前轮转角与自动驾驶汽车期望加速度；

需要说明的是，一些实施例中，步骤S400可以包括：S410、对所述融合有约束成本项的自动驾驶汽车轨迹优化目标函数进行简化处理，得到简化的融合有约束成本项的自动驾驶汽车轨迹优化目标函数；S420、根据所述自动驾驶汽车的当前时刻状态信息，给定初始控制序列，并结合所述自动驾驶汽车动力学约束条件进行前向递推处理，得到自动驾驶汽车初始状态序列；S430、通过所述迭代线性二次调节器，基于所述简化的融合有约束成本项的自动驾驶汽车轨迹优化目标函数对所述自动驾驶汽车初始状态序列进行求解计算，得到第一自动驾驶汽车初始状态序列代价；S440、根据所述自动驾驶汽车的当前时刻状态信息与所述给定初始控制序列进行后向递推处理，得到前向增益与反馈增益；S450、根据所述前向增益与所述反馈增益，获取自动驾驶汽车下一状态序列；S460、基于所述简化的融合有约束成本项的自动驾驶汽车轨迹优化目标函数对所述自动驾驶汽车下一状态序列进行求解计算，得到第二自动驾驶汽车初始状态序列代价；S470、对所述第一自动驾驶汽车初始状态序列代价与所述第二自动驾驶汽车初始状态序列代价进行差值计算处理，得到差值计算结果；S480、对所述差值计算结果进行判断，若所述差值计算结果满足预设收敛条件，则输出自动驾驶汽车下一状态序列中的元素，所述元素包括自动驾驶汽车期望前轮转角与自动驾驶汽车期望加速度；S490、若所述差值计算结果不满足预设收敛条件，则循环前向递推处理步骤、求解计算步骤、后向递推处理步骤与差值计算处理步骤，直至所述差值计算结果满足预设收敛条件，输出自动驾驶汽车期望前轮转角与自动驾驶汽车期望加速度。

一些具体实施例中，定义

s.t.x

一些实施例中，获取车辆当前时刻初始状态x

需要说明的是，计算第k个时间步的状态变化量δx

进一步计算第k个时间步的控制量变化量δu

δu

进一步，获取第k个时间步的新控制量

进一步，计算获取第k+1个时间步的新状态量

最后，从k＝0开始，重复上述计算第k个时间步的状态变化量步骤、计算第k个时间步的控制量变化量步骤、获取第k个时间步的新控制量步骤和计算获取第k+1个时间步的新状态量，直至k＝n-1，获取新的状态序列X1和新的控制序列U

进一步，根据简化的融合有约束成本项的自动驾驶汽车轨迹优化目标函数计算初始状态序列与初始控制序列的代价J(X

一些实施例中，判断代价变化是否在收敛阈值范围内，其判断的表达式为：

上式中，ε中为收敛判断阈值。

若上述的判断的表达式的判断条件成立，则轨迹优化过程终止，输出新控制序列U

一些具体实施例中，步骤S440可以包括，S441、通过扰动函数对所述自动驾驶汽车的当前时刻状态信息与所述给定初始控制序列进行求偏导计算处理，得到当前时刻状态信息一阶偏导数、控制序列一阶偏导数、当前时刻状态信息二阶偏导数和控制序列二阶偏导数；S442、通过价值函数对所述当前时刻状态信息一阶偏导数、所述控制序列一阶偏导数、所述当前时刻状态信息二阶偏导数和所述控制序列二阶偏导数进行计算，得到当前时刻状态信息价值函数与控制序列价值函数；S443、分别对所述当前时刻状态信息价值函数与所述控制序列价值函数进行求解，得到前向增益与反馈增益。

一些具体实施例中，通过扰动函数Q对每一个状态与控制对(x

上式中，l

进一步，通过价值函数V的一阶偏导数与二阶偏导数计算，其计算的过程为：

最后，计算每一个状态与控制对(x

上式中，H

S500、引入稳定性控制器并结合所述自动驾驶汽车期望前轮转角，获取自动驾驶汽车前轮转角的附加横摆力矩；

需要说明的是，一些实施例中，步骤S500可以包括：S510、获取所述自动驾驶汽车的横摆角速度、所述自动驾驶汽车的质心侧偏角稳态值、所述自动驾驶汽车的横摆角速度极限值，所述自动驾驶汽车的质心侧偏角极限值；S520、根据所述质心侧偏角极限值与所述质心侧偏角稳态值，确定所述自动驾驶汽车的期望质心侧偏角；S530、根据所述横摆角速度与所述横摆角速度极限值，确定所述自动驾驶汽车的期望横摆角速度并进行求偏导计算，得到横摆角速度的一阶导数；S540、根据所述期望质心侧偏角与所述期望横摆角速度，构建滑模面模型并进行求导计算，得到求导后的滑模面模型；S550、引入常速趋近率，并结合所述横摆角速度的一阶导数与所述求导后的滑模面模型，构建所述自动驾驶汽车的初步附加横摆力矩；S560、引入车辆失稳程度评估因子与附加横摆力矩调节因子，并结合所述稳定性控制器对所述初步附加横摆力矩进行调节处理，得到所述自动驾驶汽车前轮转角的附加横摆力矩。

一些具体实施例中，计算稳态转向下的横摆角速度和质心侧偏角的稳态值，其表达式为：

进一步，计算轮胎力饱和时横摆角速度的极限值，其计算的表达式为：

进一步，计算在稳态范围内质心侧偏角的极限值，其计算的表达式为：

上式中，C为刷子轮胎模型的侧偏刚度。

进一步，考虑横摆角速度、质心侧偏角的极限值约束，计算期望的稳态质心侧偏角和横摆角速度，其计算的表达式为：

进一步，构建滑模面s，滑模面的表达式为：

s＝(w-w

上式中，ξ为归一化权重系数，

进一步，获取横摆角速度的一阶导数，其表达式为：

上式中，ΔM为附加横摆力矩。

进一步，对滑模面的表达式进行求导得到：

上式中，

进一步，引入常速趋近率，其表达式为：

上式中，k

进一步，结合横摆角速度的一阶导数的表达式、滑模面求导表达式和常速趋近率的表达式，得到附加横摆表达式为:

进一步，引入车辆失稳程度评估因子，其表达式为：

上式中，q为权重因子，且q>1。

进一步，引入附加横摆力矩调节因子，其表达式为：

上式中，λ

进一步，获取稳定性控制器输出的避免车辆失稳的附加横摆力矩ΔM

ΔM

进一步，判断车辆是否处于失稳边界，依据判断公式决定稳定性控制器是否参与协同工作，并输出相对应的附加横摆力矩指令，判断公式为：

/>

上式中，ω

S600、引入稳定性控制器并结合所述自动驾驶汽车期望前轮转角，获取自动驾驶汽车前轮转角的附加横摆力矩；

需要说明的是，一些实施例中，步骤S600可以包括：S610、引入所述自动驾驶汽车与路面的附着系数，构建附着利用率最小化性能指标；S620、根据所述附着利用率最小化性能指标，构建自动驾驶汽车力矩控制优化目标函数；S630、将所述自动驾驶汽车期望加速度与所述自动驾驶汽车前轮转角的附加横摆力矩输入至所述自动驾驶汽车力矩控制优化目标函数进行求解，得到所述自动驾驶汽车力矩控制量。

一些具体实施例中，根据目标函数求解出各车轮的扭矩U

f(U)＝‖W

s.t.U

U

U

T

其中V＝[ma

上式中，U

S700、引入稳定性控制器并结合所述自动驾驶汽车期望前轮转角，获取自动驾驶汽车前轮转角的附加横摆力矩。

需要说明的是，参照图3、图4和图5，可以看出，本申请实施例的方法能有效应对高密度复杂交通流场景。相比RS+QP算法，本申请实施例提出的方法在处理相同场景时车辆表现显得更加类人。车辆整体的通行效率更高、舒适性更好，轨迹质量提升显著。

参照图6，图6为车辆以70km/h行驶在道路附着系数只有0.3路面上连续遭遇“鬼探头”，稳定性控制器有无参与协同工作下的对比测试结果，可以看出，在无稳定性控制器协同工作仅依靠轨迹规划器在高速低附工况下紧急避障时车辆发生失稳，而本申请实施例的方法能够安全应对极限工况下的复杂场景，有效扩展了智能车辆的轨迹规划边界。

下面，结合具体的应用例子，对本发明实施例的方案作详细介绍和说明：

具体地，本申请实施例中，利用车辆状态、交通规则、轨迹预测信息，采用可达状态集的方式推演生成复杂环境下的自车可行驶凸走廊，设计动力学约束、物理极限约束、走廊边界约束及控制成本项、跟踪成本项、终端成本项，建立用于轨迹优化的目标函数，将约束中的不等式约束，利用指数障碍函数转化为目标函数中的代价项，建立融合有约束成本项的轨迹优化目标函数，利用迭代线性二次调节器理论对目标函数进行迭代优化求解最优轨迹，输出期望前轮转角与期望加速度，以前轮转角控制量、车辆状态为输入，若车辆横摆角速度与期望的横摆角速度之间的差异大于预设阈值，则判定为车辆处于失稳边界，则稳定性控制器激活介入，输出与前轮转角控制量相匹配的附加横摆力矩，以附加横摆力矩和期望的加速度为目标，兼顾轮胎附着利用率最小化的性能指标，对四轮的纵向驱动力矩采用二次规划进行优化协调，得到各轮的力矩控制量。

请参阅图7，本申请实施例还提供一种基于线性迭代二次调节器的自动驾驶轨迹规划系统，可以实现上述基于线性迭代二次调节器的自动驾驶轨迹规划方法，该系统包括：

第一模块，用于根据自动驾驶汽车的当前时刻状态信息、障碍物的轨迹预测信息以及交通规则条件，通过可达状态集方法进行推演，构建自动驾驶汽车可行驶凸走廊；

第二模块，用于根据所述自动驾驶汽车可行驶凸走廊，引入自动驾驶汽车运动约束条件与自动驾驶汽车成本控制项条件，构建自动驾驶汽车的轨迹优化目标函数；

第三模块，用于通过指数障碍函数对所述自动驾驶汽车的轨迹优化目标函数进行转换处理，得到融合有约束成本项的自动驾驶汽车轨迹优化目标函数；

第四模块，用于通过迭代线性二次调节器对所述融合有约束成本项的自动驾驶汽车轨迹优化目标函数进行迭代优化求解，得到自动驾驶汽车期望前轮转角与自动驾驶汽车期望加速度；

第五模块，用于引入稳定性控制器并结合所述自动驾驶汽车期望前轮转角，获取自动驾驶汽车前轮转角的附加横摆力矩；

第六模块，用于根据所述自动驾驶汽车期望加速度与所述自动驾驶汽车前轮转角的附加横摆力矩进行优化调节，输出自动驾驶汽车力矩控制量；

第七模块，用于根据所述自动驾驶汽车力矩控制量对所述自动驾驶汽车进行驾驶轨迹规划，得到轨迹规划结果。

可以理解的是，上述方法实施例中的内容均适用于本系统实施例中，本系统实施例所具体实现的功能与上述方法实施例相同，并且达到的有益效果与上述方法实施例所达到的有益效果也相同。

以上参照附图说明了本申请实施例的优选实施例，并非因此局限本申请实施例的权利范围。本领域技术人员不脱离本申请实施例的范围和实质内所作的任何修改、等同替换和改进，均应在本申请实施例的权利范围之内。