CN116882307B - 考虑不确定初始状态的车辆安全轨迹规划方法及系统 - Google Patents

Abstract

本发明公开了一种考虑不确定初始状态的车辆安全轨迹规划方法及系统，其技术方案主要是，基于本车初始状态的概率分布情况，通过基于数据驱动的密度函数深度神经网络推演本车状态沿参考轨迹的密度变化，结合环境车辆的轨迹预测结果构建可微概率栅格图，得到本车及环境障碍物的位姿状态分布重叠，作为量化碰撞概率的指标。综合考虑该指标以及车辆运动学、控制输入约束等因素进行轨迹规划，通过渐进优化及增量优化的方法，在复杂环境中生成安全的行驶轨迹。

Description

考虑不确定初始状态的车辆安全轨迹规划方法及系统

技术领域

本发明涉及自动驾驶车辆的轨迹规划技术领域，尤其涉及一种考虑不确定初始状态的车辆安全轨迹规划方法及系统。

背景技术

自动驾驶技术能够疏导交通拥堵，减少交通事故，对我国汽车制造业及交通出行行业的发展具有重要意义。由于行车环境和车辆系统难以精确建模及传感器感知局限将长期存在，复杂不确定环境下行车碰撞风险难以量化评估，初始车辆状态不确定时现有轨迹规划方法易失效、安全性难以保障。因此，自动驾驶规划系统需具备应对环境状态、车辆状态不确定性的能力。

不完整的环境表示和车辆状态的不确定性会影响轨迹规划，针对复杂不确定环境下行车碰撞风险难以量化评估的问题，现有行车风险评估模型通常是基于碰撞发生时间预测和基于轨迹冲突预测的。基于碰撞发生时间预测的行车风险评估模型由于与特定场景高度耦合，无法向其它场景拓展。基于轨迹冲突预测的行车风险评估模型通过预测动态车辆未来的行驶轨迹，可以一定程度上量化评估行车风险。但是，现有模型通常基于滤波处理后的平滑理想数据进行评估，在实际条件下由于传感器等装置的精度和可靠性、车辆模型建模误差的限制，所获取的数据特别是本车状态往往存在误差或噪声，导致本车状态不确定性和环境交通参与者状态不确定性相结合的碰撞风险难以量化，行驶安全性无法保障。

自动驾驶车辆轨迹规划作为多个技术模块之间的重要环节，其存在不确定性问题尤为突出，可以分为其他交通要素的意图不确定和本车的状态不确定，这种不确定性是复杂交通环境下行车碰撞风险的主要来源。为了降低行车碰撞风险，尽管现有部分轨迹规划发明考虑了其他交通要素的意图不确定，但他们通常基于滤波处理后的理想传感器数据进行轨迹规划，无法应对传感器噪声及模型误差较大的情况，容易引发轨迹规划失效。

发明内容

本发明主要目的在于为自动驾驶车辆提供一种基于考虑不确定初始状态的轨迹规划方法，该方法可以在环境感知不确定和存在模型误差的情况下，为自动驾驶车辆生成安全可靠的轨迹规划结果。

本发明所采用的技术方案是：

提供一种考虑不确定初始状态的车辆安全轨迹规划方法，包括以下步骤：

步骤一、基于车辆周围环境信息，将轨迹规划范围内的环境离散化，并将环境中的所有障碍物膨化处理，获取轨迹规划周期内每个环境栅格被占用的概率，构建可微概率栅格图；

步骤二、采样多组本车初始状态，通过最大似然估计方法建立多元高斯分布模型，得到车辆初始状态分布；

步骤三、获取目标车辆状态信息，根据车辆初始状态分布的均值，结合可微概率栅格图，考虑车辆尺寸初步量化行车碰撞风险，通过渐进优化方法计算轨迹规划初始解；

步骤四、根据从车辆初始状态分布中随机采样车辆初始状态、初始状态偏差以及初始概率密度值，预测沿轨迹规划初始解的车辆概率密度值，并进行轨迹规划增量优化，输出优化后的轨迹规划。

接上述技术方案，障碍物膨化处理具体为：根据本车车身尺寸，构造能够均匀覆盖本车矩形车身的D个等直径包络圆，获取包络半径，用于将环境中的障碍物的外轮廓向外膨化。

接上述技术方案，可微概率栅格图包含膨化障碍物位置的概率分布栅格及其梯度变化栅格两个部分，得到可微概率栅格图的步骤具体为：将车辆周围环境信息沿着轴分成数量为的均匀间隔的栅格单元，沿y轴分成数量为的栅格单元，共得到总数为的栅格单元，栅格的位置用()进行索引；

其中，二维环境中，每个膨化障碍物位置的概率分布栅格包含一个三维的张量，每个栅格单元(，)在时刻被障碍物占用的概率通过一定时间步长内环境中障碍物状态变化计算得到；

其中，二维环境中，每个梯度变化栅格包含一个四维的张量，通过计算相邻栅格的占用概率数值差，得到x方向和y方向上梯度变化栅格的梯度张量，计算梯度张量的公式为：

其中，是计算步长；为了提供完整的概率梯度，在对应膨化障碍物位置的概率分布栅格的障碍物占用的概率值不为0时，逐步增大步长，直到对应位置的梯度张量栅格的梯度张量，不为零。

接上述技术方案，步骤四中轨迹规划增量优化过程具体为：

从初始多元高斯分布不为0的车辆初始状态分布中随机采样多组车辆初始状态；

获取轨迹规划初始解、多组初始状态中的初始状态偏差、初始参考状态以及采样点的初始密度值，使用预先训练好的密度函数深度神经网络求解沿轨迹规划初始解的车辆状态预测值及其密度函数预测值；

并行计算所有车辆状态预测值-参考输入对的代价函数并做代价求和，计算整体代价函数的梯度，使用Adam梯度下降算法更新轨迹规划初始解，直到最大迭代次数；

输出优化后的轨迹规划，以交由跟踪控制器执行。

接上述技术方案，密度函数深度神经网络的训练样本的制作方法具体为：

基于车载传感器，通过评估其数据不确定性程度，得到初始密度分布，从初始密度分布不为0的区域中随机采样初始参考状态、轨迹参数集和车辆初始状态偏差，是车辆参考输入基于时间的五次多项式系数或分段函数，两者具有映射关系，；

基于初始参考状态、轨迹参数集，计算车辆参考状态轨迹，使用线控转向实车平台的车辆控制器跟踪参考状态轨迹，输出控制指令，是车辆状态向量；

采集包括车辆位置信息、方向盘转角、纵向车速的车辆状态信息试验真实数据，并通过对系统动力学方程进行积分获得模拟车辆状态信息；

按照一定比例混合真实及模拟车辆状态信息得到合成车辆状态，及为权重系数，依据实际情况指定；求解刘维尔方程，获得时刻的车辆状态偏差对数密度函数值：

接上述技术方案，步骤四中预测沿轨迹规划初始解的车辆概率密度值的过程具体为：

从初始密度分布不为0的区域中采样数量为的初始状态及相应的初始状态偏差；

使用训练好的密度函数深度神经网络接收输入张量，同步推演本车的状态沿行驶轨迹的概率演化，其时刻的输出为轨迹中车辆状态预测偏差及对数密度函数预测变化值；

使用计算车辆状态预测值；

使用计算预测密度值，并对进行插值和归一化，得到近似总体车辆概率密度分布。

本发明还提供一种考虑不确定初始状态的车辆安全轨迹规划系统，包括：

环境栅格化模块，用于基于车辆周围环境信息，将轨迹规划范围内的环境离散化，将环境中的所有障碍物膨化，获取轨迹规划周期内每个环境栅格被占用的概率，构建可微概率栅格图；

初始状态分布获取模块，用于采样多组本车初始状态，通过最大似然估计方法建立多元高斯分布模型，得到车辆初始状态分布；

轨迹规划处理模块，用于获取目标车辆状态信息，根据车辆初始状态分布的均值，结合可微概率栅格图，初步量化行车碰撞风险，通过渐进优化方法计算轨迹规划初始解；

轨迹优化模块，用于根据从车辆初始状态分布中随机采样车辆初始状态、初始状态偏差以及初始概率密度值，预测沿轨迹规划初始解的车辆概率密度值，并进行轨迹规划增量优化，输出优化后的轨迹规划。

接上述技术方案，环境中的障碍物外轮廓根据本车车身包络圆尺寸向外膨化后，构建可微概率栅格图，包含膨化障碍物位置的概率分布栅格及其梯度变化栅格两个部分，可微概率栅格图构建模块在构建可微概率栅格图时具体用于：将车辆周围环境信息沿着轴分成数量为的均匀间隔的栅格单元，沿y轴分为数量为的栅格单元，共得到总数为的栅格单元，栅格的位置用()进行索引；

其中，二维环境中，每个膨化障碍物位置的概率分布栅格包含一个三维的张量，每个栅格单元(，)在时刻被障碍物占用的概率通过一定时间步长内环境中障碍物状态变化计算得到；

其中，二维环境中，每个梯度变化栅格包含一个四维的张量，通过计算相邻栅格的占用概率数值差，得到x方向和y方向上梯度变化栅格的梯度张量，计算梯度张量的公式为：

其中，是计算步长；为了提供完整的概率梯度，在对应膨化障碍物位置的概率分布栅格的障碍物占用的概率值不为0时，逐步增大步长，直到对应位置的梯度张量栅格的梯度张量，不为零。

接上述技术方案，轨迹优化模块在求解轨迹规划增量优化时，具体用于：

从初始多元高斯分布不为0的部分随机采样多组车辆初始状态；

获取轨迹规划初始解、多组初始状态中的初始状态偏差、初始参考状态以及采样点的初始密度值，使用预先训练好的密度函数深度神经网络预测沿轨迹规划初始解的车辆状态预测值及其密度函数预测值；

并行计算所有车辆状态预测值-参考输入对的代价函数并做代价求和，计算整体代价函数的梯度，使用Adam梯度下降算法更新轨迹规划初始解，直到最大迭代次数；

输出优化后的轨迹规划，以交由跟踪控制器执行。

本发明还提供一种计算机存储介质，其内存储有可被处理器执行的计算机程序，该计算机程序执行上述技术方案所述的考虑不确定初始状态的车辆安全轨迹规划方法。

本发明产生的有益效果是： 本发明基于环境障碍物状态概率分布的预测以及本车初始状态的概率分布情况，通过基于数据驱动的密度函数深度神经网络推演本车状态沿参考轨迹的密度变化，构建可微概率栅格图，得到本车及环境障碍物的状态分布重叠，作为量化碰撞概率的指标。

进一步地，本发明的轨迹规划方法考虑了本车状态和环境状态的不确定性相结合的碰撞风险对轨迹规划的影响，通过渐进优化方法及增量优化方法，综合考虑碰撞概率指标以及车辆运动学、控制输入约束等因素进行轨迹规划，保证在复杂环境中生成安全的行驶轨迹。

相比传统的轨迹规划方法，本发明考虑了车辆初始状态的不确定性，即存在传感器误差及模型误差的情况， 从而为自动驾驶车辆规划最优的安全行驶轨迹。相比基于学习的轨迹规划方法，本发明具备更强的可解释性，更适用于安全关键的自动驾驶应用，能够实现自动驾驶车辆的高效安全运作。

进一步地，本发明通过对大量初始状态进行采样并预测其在特定时刻的密度值，基于刘维尔方程(Liouville Equation， LE)密度函数值表示车辆动力学系统处于特定状态的概率，通过密度函数深度神经网络推断车辆系统状态的不确定性演化，描述状态空间中状态分布情况，在此基础上进行更精细和准确的轨迹规划。

附图说明

为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。

图1 为本发明实施例基于数据驱动的密度函数深度神经网络训练示意图；

图2为本发明实施例基于数据驱动的密度函数深度神经网络预测流程图；

图3为本发明实施例障碍物概率分布栅格示例图；

图4为本发明实施例某时刻的梯度栅格示例图；

图5为本发明实施例考虑不确定初始状态的车辆安全轨迹规划方法流程图；

图6为本发明实施例轨迹规划初始解并行计算流程图；

图7为本发明实施例渐进优化方法流程图；

图8为本发明实施例轨迹规划增量优化方法流程图

图9为本发明实施例密度函数深度神经网络训练样本的制作方法流程图；

图10为本发明实施例使用D个车身包络圆覆盖车身示例图（D=2）；

图11为本发明实施例轨迹规划中考虑车身尺寸的膨化等效转化。

具体实施方式

为了使本发明的目的、技术方案及优点更加清楚明白，以下结合附图及实施例，对本发明进行进一步详细说明。应当理解，此处所描述的具体实施例仅用以解释本发明，并不用于限定本发明。

本发明主要为自动驾驶车辆提供一种基于考虑不确定初始状态的轨迹规划方法，该方法可以在环境感知不确定和存在模型误差的情况下，为自动驾驶车辆生成安全可靠的轨迹规划结果。

实施例1

本发明实施例考虑不确定初始状态的车辆安全轨迹规划方法，包括以下步骤：

步骤一、获取环境车辆等障碍物未来轨迹预测及意图不确定性信息，构建可微概率栅格图；

基于车辆周围环境信息（道路信息，障碍物信息等），将轨迹规划范围内的环境离散化，并将环境中的所有障碍物膨化，获取轨迹规划周期内每个环境栅格被占用的概率，构建可微概率栅格图。

步骤二、采样多组本车初始状态，通过最大似然估计等方法建立多元高斯分布模型；其中，初始状态分布为一组概率密度函数。

采样N组初始状态，每组初始状态包含本车位置、速度、加速度、车身航向角等数据，假设N组初始状态符合多元高斯分布，用最大似然估计方法估计这个多元高斯分布的参数和，建立多元高斯分布模型。

步骤三、获取目标车辆状态信息，根据本车初始状态的均值，结合可微概率栅格图，初步量化行车碰撞风险，通过渐进优化方法计算轨迹规划初始解；

基于车辆目标状态（目标位置，车辆状态等），随机采样M组参考轨迹，取初始多元高斯分布的均值状态为车辆初始参考状态。基于车辆的系统动力学方程、轨迹参数集及代价函数，通过渐进优化方法，多线程同步优化M组参考轨迹，并返回成本最低的作为轨迹规划初始解。

步骤四、从本车初始状态分布中随机采样车辆初始状态以及概率密度值，预测沿轨迹规划初始解的车辆状态及车辆状态概率密度值，对行车碰撞风险进行进一步量化，推动轨迹规划增量优化，输出最终结果。初始概率密度值是指随机采样的车辆初始状态对应的概率密度函数值。

步骤一中，所述可微概率栅格图包含障碍物位置的概率分布栅格及其梯度变化栅格两个部分。如图3、4所示，得到二维栅格的步骤为：将环境沿着x轴分成数量为的均匀间隔的栅格单元，沿y轴分为数量为的栅格单元，共得到总数为的栅格单元，栅格的位置用()进行索引。所述可微概率栅格图不局限于二维环境，本发明可以很容易拓展到更高维度。

步骤一中，对于二维环境的所述可微概率栅格图，每个障碍物均执行了膨化处理，如图10、11所示，膨化处理的步骤是：根据本车车身尺寸参数：前轴中心与车头的距离，轴距，后轴中心与车尾的距离，车宽，构造能够均匀覆盖本车矩形车身的2个等径包络圆，其圆心为、，获取半径数值

将所有障碍物的外轮廓向外扩大。因为“减小包络圆覆盖区域与障碍物的碰撞概率”与“减小质点、占据的栅格区域与膨化障碍物的碰撞概率”是等价问题，在膨化场景中，将车身视为两个相对位置固定的质点能够一定程度上考虑本车的车身尺寸参数。

步骤一中，对于二维环境的所述可微概率栅格图，每个障碍物位置的概率分布栅格包含一个三维的张量，每个栅格单元(,)在时刻被障碍物占用的概率通过深度神经网络，马尔科夫链或可达性分析预测一定时间步长内环境中障碍物状态变化获得（障碍物可以是车道分隔线、行人、其他环境车辆或者是交通标志等物体）。

步骤一中，对于二维环境的所述可微概率栅格图，每个梯度变化栅格包含一个四维的张量，通过计算相邻栅格的占用概率数值差，得到x方向和y方向上梯度变化栅格的梯度张量。用于计算梯度张量的公式为：

其中，是计算步长；为了提供完整的概率梯度，在对应膨化障碍物位置的概率分布栅格的障碍物占用的概率值不为0时，逐步增大步长，直到对应位置的梯度张量栅格的梯度张量,不为零。

步骤三及步骤四中，所采用的车辆系统动力学方程是

其中，是车辆状态向量，是一种车辆控制器的输出，是状态转移函数，是参考状态轨迹-参考输入对，参考控制输入向量与有简单映射关系，是当前位置和目标状态之间轨迹参数集。

步骤三及步骤四中，所采用的代价函数是：

是惩罚项，是权重项，计算每个轨迹的代价并使用Adam梯度下降算法更新轨迹参数集。其中，各代价项计算方法如下：

第一项惩罚状态轨迹的最终状态到目标轨迹的欧几里得距离

其中，是最终时间，是自主车辆应该到达的目标状态，而Q则是加权矩阵。第二项用于最小化控制输入

第三项的目标是将状态轨迹保持在有效状态空间中

其中，是状态空间的最小边界，是状态空间的最大边界，而和分别是具有对角线元素的对角矩阵。

最后一项表示碰撞风险的代价，基于环境栅格化建模进行计算

其中，是指本车状态对应的现实世界的状态轨迹点坐标，对应本车车辆后轴中点，而是相应的期望轨迹点坐标，两者分别通过栅格化环境中的坐标以及转换得到的。将车辆状态通过操作符及映射到对应的栅格化环境中的栅格点，检索梯度变化栅格，如果至少有一个梯度向量不为0，意味着当前车辆状态能够进一步优化。根据梯度下降的方向得更安全的计算期望网格坐标以进一步降低碰撞概率：

其中，是计算步长。在此基础上，通过期望状态轨迹点坐标和当前状态轨迹点坐标之间的欧几里得距离量化状态下的碰撞成本。则是状态引起的碰撞概率，在起到了加权的作用，可以通过以下方式进行计算

其中，由障碍物位置的概率分布栅格提供，是本车状态密度预测，)是等径包络圆的圆心坐标，当D=2时，这两个质点是车身纵轴上的四分位点，如图，，，，分别为车身参数：前轴中心到车头的距离，轴距，后轴距中心到车尾的距离以及车宽，两个质点坐标通过后轴中点坐标计算得到。

本车状态密度预测是通过基于数据驱动的密度函数深度神经网络计算沿参考轨迹的密度函数变化值来实现的。深度神经网络的输入为某时刻的初始车辆状态及轨迹参数集，深度神经网络的输出为车辆状态在某时刻的偏差和对数密度函数的估计值。所述轨迹参数集是预定义多项式结构的系数(如五次多项式，贝塞尔曲线，B样条曲线)或离散化参数值(每个时间步长的输入信号设定为不同的固定值)等。参考控制输入向量与有简单映射关系

所述密度函数是指基于刘维尔方程的密度函数封闭式解，对于(2)中车辆系统动力学方程有

其中，是密度集中函数，反映了车辆状态对应的密度沿参考轨迹的变化。是矢量场的散度。为系统动力学方程的积分形式。为了提高计算效率，将密度集中函数转化为对数形式：

因此，所述基于数据驱动的密度函数深度神经网络训练，其训练样本包括：初始参考状态、轨迹参数集、车辆状态初始偏差、时序信息、车辆状态在时刻的真实偏差及其对数密度函数的真实值。用于深度神经网络训练的损失函数为：

其中，为时刻状态偏差的预测值，为对数密度函数的预测值。

基于数据驱动的密度函数深度神经网络，其训练样本的获取方法为：

基于车载传感器，通过评估其数据不确定性程度，得到初始密度分布，从初始密度分布不为0的区域中随机采样初始参考状态、轨迹参数集和车辆状态初始偏差，通常是车辆参考输入基于时间的五次多项式系数或分段函数，两者具有简单映射关系（12）；

基于初始参考状态、轨迹参数集，计算满足车辆动力学的参考状态轨迹，使用线控转向实车平台的车辆控制器跟踪参考状态轨迹，输出控制指令，是车辆状态向量，是车辆参考轨迹，是车辆参考输入；

采集包括车辆位置信息、方向盘转角、纵向车速的车辆状态信息试验真实数据，并通过对系统动力学方程进行积分获得模拟车辆状态信息；

按照一定比例混合真实及模拟车辆状态信息得到合成车辆状态，及为权重系数，依据实际情况指定；求解刘维尔方程，获得时刻的车辆状态偏差对数密度函数值：

其中，是对数密度函数，其输入的其中一是，为时刻的车辆状态。显示了车辆状态是由真实数据及模拟数据混合而成。及为权重系数，依据实际情况来指定。是车辆动力学模拟得到的车辆状态。参考轨迹是一组车辆参考状态（理想情况下的车辆轨迹），由车辆动力学模拟初始车辆参考状态得到，初始输入是初始参考状态及初始参考输入。是一个函数符号。是轨迹参数集（与的区别在于不包含车辆状态，仅包含参考轨迹的曲线信息，可以是五次多项式、分段函数）。是一种函数符号，将时刻的信息转化为参考输入。的具体过程有很多种，公式（3）为其中一种例子。

步骤三中，随机采样的M组参考轨迹是指M组在当前位置和目标状态之间轨迹参数集。所有采集到的组参数集，经系统动力学方程计算相应的后，通过代价函数同步执行渐进优化。优化结束时，轨迹规划初始解是指在所有参数集中代价最低的路径参数集U _init。

步骤三中，本车状态对应的密度函数值都为定值1。

步骤三中，计算轨迹规划初始解的具体过程为，取初始多元高斯分布的均值状态为车辆初始参考状态，随机采样组轨迹参数集，通过(12)得到，积分系统动力学方程，得到参考状态轨迹，通过下式计算每组参考状态轨迹的代价函数：

首先初始化所有轨迹代价函数的及为零。当目标点的偏差距离代价小于一定阈值后，相应的将被设置为非0值。对于所有的轨迹，其状态空间的约束代价得到满足后，相应的设置为非0值来考虑碰撞代价，进一步迭代提高参考轨迹的安全性，更新轨迹参数集，直到最大迭代。最后，对比M组轨迹参数集，输出代价最小的作为轨迹规划初始解U _init。

步骤四中，轨迹规划增量优化的具体过程为：

首先从初始多元高斯分布不为0的部分随机采样组初始状态以及初始车辆状态密度，计算初始偏差。根据轨迹规划初始解U _init、初始参考状态，计算参考输入及参考状态轨迹；使用密度函数深度神经网络得到车辆状态预测值及其密度函数值。之后并行计算所有代价函数并做代价求和，计算整体代价函数的梯度，使用Adam梯度下降算法迭代优化轨迹规划初始解U _init，直到最大迭代次数。输出轨迹规划结果，交由跟踪控制器执行。

步骤四中，基于密度函数深度神经网络的状态分布预测器的预测过程为：从初始密度分布不为0的区域中采样数量为 S 的初始状态及相应的初始状态偏差，使用训练好的密度函数深度神经网络接受输入张量，同步推演本车的状态沿行驶轨迹的概率演化，其时刻的输出为轨迹中车辆状态预测偏差及对数密度函数预测变化值。最后，使用(18)计算车辆状态预测值，使用(19)计算预测密度值，并对进行插值和归一化，近似总体密度分布。

本发明充分利用带有噪声的数据，而不是简单地进行滤波处理，具体是将车辆初始状态的不确定性建模为多元高斯分布，基于本车初始状态的概率分布情况，通过基于数据驱动的密度函数深度神经网络推演本车状态沿参考轨迹的密度变化，结合环境车辆的轨迹预测结果及膨化处理，构建可微概率栅格图，得到本车及环境障碍物的位置状态分布重叠，作为量化行车碰撞风险的指标。与现有方法相比，本发明可以在本车初始车辆状态的不确定性较大的情况下完成轨迹规划，更准确地量化了本车状态不确定性和交通要素意图不确定性相结合的行车碰撞风险，此外，为了避免局部次优解，本发明综合考虑行车碰撞风险指标以及车辆运动学、控制输入约束等因素进行轨迹规划，通过渐进优化及增量优化的方法计算轨迹规划结果，能够生成安全鲁棒的行驶轨迹。相比完全基于学习的轨迹规划方法，本发明具备更强的可解释性，更适用于安全关键的自动驾驶应用，能够实现自动驾驶车辆的高效安全运作。

实施例2

该实施例基于刘维尔方程及密度函数，提出了基于密度函数深度神经网络的车辆状态密度分布预测方法。通过密度函数深度神经网络预测本车的状态沿行驶轨迹的概率演化，结合环境障碍物状态概率分布得到量化碰撞概率的指标，综合考虑该指标以及车辆运动学约束、控制输入约束等因素，提出了适用于保证在复杂环境中生成安全的行驶轨迹的代价函数。此外，为了避免轨迹优化结果陷入局部极小值，本发明提出了渐进优化方法以及轨迹规划增量优化方法。

本实施例提供了一种考虑不确定初始状态的车辆安全轨迹规划方法，如附图5所示，包括以下步骤：

步骤一、获取环境车辆等障碍物未来轨迹预测及意图不确定性信息，构建可微概率栅格图

接收到车辆周围环境信息后，将环境沿着x轴分成数量为的均匀间隔的栅格单元，沿y轴分为数量为的栅格单元，共得到总数为的栅格单元，栅格的位置用(,)进行索引。环境中每个障碍物根据本车车身尺寸进行膨化处理，附图10、11作为膨化示例。每个栅格单元(，)在时刻被障碍物占用的概率通过深度神经网络，马尔科夫链或可达性分析预测一定时间步长内环境中障碍物状态变化获得。附图3作为障碍物概率分布栅格示例，颜色越深表示数值越大。通过计算相邻栅格的占用概率数值差，得到x方向和y方向上梯度变化栅格的梯度张量。

其中，是计算步长；为了提供完整的概率梯度，在对应膨化障碍物位置的概率分布栅格的障碍物占用的概率值不为0时，逐步增大步长，直到对应位置梯度张量栅格的梯度张量，不为零。附图4为x轴正方向梯度栅格示例。

步骤二、采样多组本车初始状态，通过最大似然估计等方法建立多元高斯分布模型；

采样N组初始状态，每组初始状态包含本车位置、速度、加速度、车身航向角等数据，假设N组初始状态符合多元高斯分布，用最大似然估计方法估计这个多元高斯分布的参数μ和Σ，建立多元高斯分布模型。

步骤三、获取目标车辆状态信息，根据本车初始状态的均值，结合可微概率栅格图，初步量化行车碰撞风险，通过渐进优化方法计算轨迹规划初始解；

为了量化行车碰撞风险，需对于特定的动力学模型训练密度函数深度神经网络。训练样本包括：初始参考状态、轨迹参数集、车辆状态初始偏差、时序信息、车辆状态在时刻的真实偏差及其对数密度函数的真实值。首先，基于Dubins车辆模型建立系统动力学方程

其中，是车身后轴中点在全局坐标系中x轴上的位置，是y轴上的位置，是t时间的车辆航向角，是车辆的纵向速度，是引入的不确定性误差，是控制器输出，是测量状态，是参考状态。通过轨迹参数集矩阵对参考输入进行参数化

其中，是矩阵的第列。训练过程如附图1所示，随机采样初始参考状态、轨迹参数集和车辆状态初始偏差作为深度神经网络的输入；并基于初始参考状态、轨迹参数集，计算参考状态轨迹，通过分别积分系统动力学方程(1)和求解刘维尔方程(4)获得车辆状态在时刻的真实偏差及其对数密度函数的真实值，批量生成以上数据样本用于训练深度神经网络，通过最小化损失函数优化网络权重。预测过程如图2所示，深度神经网络的输出为时刻的车辆状态偏差和对数密度函数的估计值。

轨迹规划初始解的计算流程如附图6所示，接受目标状态及栅格化环境信息输入后，系统随机采样M组连接当前位置和目标状态之间的轨迹参数集，车辆的初始参考状态是多元高斯分布的均值。然后，所有组轨迹参数集将使用渐进优化方法并行优化。

在渐进优化的初始化阶段，基于特定初始密度分布均值选定的初始状态以及参数集积分系统动力学方程，可以得到参考状态轨迹，通过下式计算代价函数：

渐进优化的迭代优化阶段流程如附图7所示，通过代价函数更新轨迹参数集，其中相关权重参数取值如表格1所示。首先初始化所有轨迹代价函数的及为零，当目标点的偏差距离代价小于一定阈值后，相应的将被设置为非0值。对于所有的轨迹，其状态空间的约束代价得到满足后，相应的设置为非0值来考虑碰撞代价，进一步迭代提高参考轨迹的安全性，更新轨迹参数集，直到最大迭代。其中相关权重参数取值如下表1所示，完整代价函数如(6)，各项释义与实施例1中相同。

表1 相关权重参数取值

最后，排序M组轨迹参数集，输出代价最小的作为轨迹规划初始解。

步骤四、从初始状态的多元高斯分布模型中随机采样车辆初始状态以及概率密度值，预测沿轨迹规划初始解的车辆概率密度值，对行车碰撞风险进行进一步量化，推动轨迹规划增量优化，输出最终结果。

轨迹规划增量优化如附图8所示，基于本车状态及其密度分布信息，从初初始多元高斯分布不为0的区域中随机采样数量为 S 的初始状态及相应的初始状态偏差、初始车辆状态密度，根据轨迹规划初始解、初始参考状态，计算参考输入及参考状态轨迹使用训练好的密度函数深度神经网络接受输入张量，同步推演本车的状态沿行驶轨迹的概率演化。预测过程如附图2所示，其时刻的输出为轨迹中车辆状态预测偏差及对数密度函数预测变化值。最后，使用(7)计算车辆状态预测值，使用(8)计算预测密度值，对进行插值和归一化，近似总体密度分布

之后，并行计算所有代价函数并做代价求和，计算整体代价函数的梯度，使用Adam梯度下降算法迭代优化轨迹规划初始解，直到最大迭代次数。输出轨迹规划结果，交由跟踪控制器执行。

其中，密度函数深度神经网络训练样本的制作方法如下：

如附图9所示，首先，基于车载传感器随机采样初始参考状态、车辆状态初始偏差及轨迹参数集；基于初始参考状态、轨迹参数集，计算参考状态轨迹，使用线控转向实车平台的车辆控制器跟踪参考轨迹，输出控制指令。一方面，采集包括车辆位置信息，方向盘转角，纵向车速等车辆状态信息试验数据对时间序列中缺失数据点进行线性插值获得真实车辆状态信息；另一方面，通过对系统动力学方程进行积分并添加随机噪声获得模拟车辆状态信息。进行数据归一化处理后按照一定比例混合真实及模拟车辆状态信息，求解刘维尔方程，获得车辆状态在时刻包含真实及模拟数据的状态偏差及其对数密度函数：

实施例3

该实施例主要用于实现上述方法实施例。该实施例考虑不确定初始状态的车辆安全轨迹规划系统，所述系统包括：

环境栅格化模块，用于基于车辆周围环境信息，将轨迹规划范围内的环境离散化，将环境中的所有障碍物膨化，获取轨迹规划周期内每个环境栅格被占用的概率，构建可微概率栅格图；

初始状态分布获取模块，用于采样多组本车初始状态，通过最大似然估计方法建立多元高斯分布模型，得到车辆初始状态分布；

轨迹规划处理模块，用于获取目标车辆状态信息，根据车辆初始状态分布的均值，结合可微概率栅格图，初步量化行车碰撞风险，通过渐进优化方法计算轨迹规划初始解；

轨迹优化模块，用于根据从车辆初始状态分布中随机采样车辆初始状态、初始状态偏差以及初始概率密度值，预测沿轨迹规划初始解的车辆概率密度值，并进行轨迹规划增量优化，输出优化后的轨迹规划，交由控制器执行；车辆跟踪控制器搭载车辆轨迹跟踪算法，控制前轮转角，执行轨迹跟踪。

该系统还包括数据采集和密度函数深度神经网络训练模块，基于线控转向实车平台，在保证安全的前提下跟踪连续转弯道路，每隔一定时间步长记录初始参考状态、车辆状态初始偏差、时序信息、车辆状态在特定时刻的真实状态偏差及感知信息，包括但不限于点云信息，视觉传感器信息处理结果，训练密度函数深度神经网络。

各个模块用于实现上述方法实施例的各个步骤，在此不赘述。

本发明可以在环境感知不确定和存在模型误差的情况下，为自动驾驶车辆生成安全可靠的轨迹规划。本发明基于环境障碍物状态概率分布的预测以及本车初始状态的概率分布情况，通过基于数据驱动的密度函数深度神经网络推演本车状态沿参考轨迹的密度变化，得到本车及环境障碍物的状态分布重叠，作为量化碰撞概率的指标。综合考虑该指标以及车辆运动学、控制输入约束等因素进行轨迹规划，保证在复杂环境中生成安全的行驶轨迹。

相比传统的轨迹规划方法，本发明考虑了车辆初始状态的不确定性，即存在传感器误差及模型误差的情况，从而为自动驾驶车辆规划最优的安全行驶轨迹。相比基于学习的轨迹规划方法，本发明具备更强的可解释性，更适用于安全关键的自动驾驶应用，能够实现自动驾驶车辆的高效安全运作。

实施例4

本申请还提供一种计算机可读存储介质，如闪存、硬盘、多媒体卡、卡型存储器（例如，SD或DX存储器等）、随机访问存储器（RAM）、静态随机访问存储器（SRAM）、只读存储器（ROM）、电可擦除可编程只读存储器（EEPROM）、可编程只读存储器（PROM）、磁性存储器、磁盘、光盘、服务器、App应用商城等等，其上存储有计算机程序，程序被处理器执行时实现相应功能。本实施例的计算机可读存储介质被处理器执行时实现方法实施例的考虑不确定初始状态的车辆安全轨迹规划方法。

需要指出，根据实施的需要，可将本申请中描述的各个步骤/部件拆分为更多步骤/部件，也可将两个或多个步骤/部件或者步骤/部件的部分操作组合成新的步骤/部件，以实现本发明的目的。

上述实施例中各步骤的序号的大小并不意味着执行顺序的先后，各过程的执行顺序应以其功能和内在逻辑确定，而不应对本申请实施例的实施过程构成任何限定。

应当理解的是，对本领域普通技术人员来说，可以根据上述说明加以改进或变换，而所有这些改进和变换都应属于本发明所附权利要求的保护范围。

Claims (7)

1.一种考虑不确定初始状态的车辆安全轨迹规划方法，其特征在于，包括以下步骤：

步骤一、基于车辆周围环境信息，将轨迹规划范围内的环境离散化，并将环境中的所有障碍物膨化处理，获取轨迹规划周期内每个环境栅格被占用的概率，构建可微概率栅格图；

步骤二、采样多组本车初始状态，通过最大似然估计方法建立多元高斯分布模型，得到车辆初始状态分布；

步骤三、获取目标车辆状态信息，根据车辆初始状态分布的均值，结合可微概率栅格图，考虑车辆尺寸初步量化行车碰撞风险，通过渐进优化方法计算轨迹规划初始解；

步骤四、根据从车辆初始状态分布中随机采样车辆初始状态、初始状态偏差以及初始概率密度值，预测沿轨迹规划初始解的车辆概率密度值，并进行轨迹规划增量优化，输出优化后的轨迹规划；

其中，步骤四中轨迹规划增量优化过程具体为：

从初始多元高斯分布不为0的车辆初始状态分布中随机采样多组车辆初始状态；

获取轨迹规划初始解、多组初始状态中的初始状态偏差、初始参考状态以及采样点的初始密度值，使用预先训练好的密度函数深度神经网络求解沿轨迹规划初始解的车辆状态预测值及其密度函数预测值；

并行计算所有车辆状态预测值-参考输入对的代价函数并做代价求和，计算整体代价函数的梯度，使用Adam梯度下降算法更新轨迹规划初始解，直到最大迭代次数；

输出优化后的轨迹规划，以交由跟踪控制器执行。

2.根据权利要求1所述的考虑不确定初始状态的车辆安全轨迹规划方法，其特征在于，障碍物膨化处理具体为：根据本车车身尺寸，构造能够均匀覆盖本车矩形车身的D个等直径包络圆，获取包络半径，用于将环境中的障碍物的外轮廓向外膨化。

3.根据权利要求1所述的考虑不确定初始状态的车辆安全轨迹规划方法，其特征在于，可微概率栅格图包含膨化障碍物位置的概率分布栅格及其梯度变化栅格两个部分，得到可微概率栅格图的步骤具体为：将车辆周围环境信息沿着轴分成数量为的均匀间隔的栅格单元，沿y轴分成数量为的栅格单元，共得到总数为的栅格单元，栅格的位置用()进行索引；

其中，二维环境中，每个膨化障碍物位置的概率分布栅格包含一个三维的张量，每个栅格单元(，)在时刻被障碍物占用的概率通过一定时间步长内环境中障碍物状态变化计算得到；

其中，二维环境中，每个梯度变化栅格包含一个四维的张量，通过计算相邻栅格的占用概率数值差，得到x方向和y方向上梯度变化栅格的梯度张量，计算梯度张量的公式为：

其中，是计算步长；为了提供完整的概率梯度，在对应膨化障碍物位置的概率分布栅格的障碍物占用的概率值不为0时，逐步增大步长，直到对应位置的梯度张量栅格的梯度张量，不为零。

4.根据权利要求1所述的考虑不确定初始状态的车辆安全轨迹规划方法，其特征在于，密度函数深度神经网络的训练样本的制作方法具体为：

基于车载传感器，通过评估其数据不确定性程度，得到初始密度分布，从初始密度分布不为0的区域中随机采样初始参考状态、轨迹参数集和车辆初始状态偏差，是车辆参考输入基于时间的五次多项式系数或分段函数，两者具有映射关系，；

基于初始参考状态、轨迹参数集，计算车辆参考状态轨迹，使用线控转向实车平台的车辆控制器跟踪参考状态轨迹，输出控制指令，是车辆状态向量；

采集包括车辆位置信息、方向盘转角、纵向车速的车辆状态信息试验真实数据，并通过对系统动力学方程进行积分获得模拟车辆状态信息；

按照一定比例混合真实及模拟车辆状态信息得到合成车辆状态，及为权重系数，依据实际情况指定；求解刘维尔方程，获得时刻的车辆状态偏差对数密度函数值：

5.一种考虑不确定初始状态的车辆安全轨迹规划系统，其特征在于，包括：

环境栅格化模块，用于基于车辆周围环境信息，将轨迹规划范围内的环境离散化，将环境中的所有障碍物膨化，获取轨迹规划周期内每个环境栅格被占用的概率，构建可微概率栅格图；

初始状态分布获取模块，用于采样多组本车初始状态，通过最大似然估计方法建立多元高斯分布模型，得到车辆初始状态分布；

轨迹规划处理模块，用于获取目标车辆状态信息，根据车辆初始状态分布的均值，结合可微概率栅格图，初步量化行车碰撞风险，通过渐进优化方法计算轨迹规划初始解；

轨迹优化模块，用于根据从车辆初始状态分布中随机采样车辆初始状态、初始状态偏差以及初始概率密度值，预测沿轨迹规划初始解的车辆概率密度值，并进行轨迹规划增量优化，输出优化后的轨迹规划；

其中，轨迹优化模块在求解轨迹规划增量优化时，具体用于：从初始多元高斯分布不为0的部分随机采样多组车辆初始状态；获取轨迹规划初始解、多组初始状态中的初始状态偏差、初始参考状态以及采样点的初始密度值，使用预先训练好的密度函数深度神经网络预测沿轨迹规划初始解的车辆状态预测值及其密度函数预测值；并行计算所有车辆状态预测值-参考输入对的代价函数并做代价求和，计算整体代价函数的梯度，使用Adam梯度下降算法更新轨迹规划初始解，直到最大迭代次数；输出优化后的轨迹规划，以交由跟踪控制器执行。

6.根据权利要求5所述的考虑不确定初始状态的车辆安全轨迹规划系统，其特征在于，环境中的障碍物外轮廓根据本车车身包络圆尺寸向外膨化后，构建可微概率栅格图，包含膨化障碍物位置的概率分布栅格及其梯度变化栅格两个部分，可微概率栅格图构建模块在构建可微概率栅格图时具体用于：将车辆周围环境信息沿着轴分成数量为的均匀间隔的栅格单元，沿y轴分为数量为的栅格单元，共得到总数为的栅格单元，栅格的位置用()进行索引；

其中，二维环境中，每个膨化障碍物位置的概率分布栅格包含一个三维的张量，每个栅格单元(，)在时刻被障碍物占用的概率通过一定时间步长内环境中障碍物状态变化计算得到；

其中，二维环境中，每个梯度变化栅格包含一个四维的张量，通过计算相邻栅格的占用概率数值差，得到x方向和y方向上梯度变化栅格的梯度张量，计算梯度张量的公式为：

其中，是计算步长；为了提供完整的概率梯度，在对应膨化障碍物位置的概率分布栅格的障碍物占用的概率值不为0时，逐步增大步长，直到对应位置的梯度张量栅格的梯度张量，不为零。

7.一种计算机存储介质，其特征在于，其内存储有可被处理器执行的计算机程序，该计算机程序执行权利要求1-4中任一项所述的考虑不确定初始状态的车辆安全轨迹规划方法。