CN112347567A - 一种车辆意图和轨迹预测的方法 - Google Patents

Abstract

本发是一种车辆意图和轨迹预测的方法。包括以下步骤：步骤一、获取驾驶交通数据集；步骤二、插入间隙的定义；步骤三、相关信息信息提取；步骤四、对交通数据集进行特征相关信息提取和状态标记；步骤五、利用隐马尔可夫模型进行车辆行为识别；步骤六、基于意图的轨迹预测；步骤七、基于车辆动力学模型的轨迹预测；步骤八、基于意图和基于车辆动力学模型的轨迹预测结合；步骤九、指标收益计算；步骤十、未来行为推理；步骤十一、最终意图预测结果。本发明能准确预测周边车辆的意图和轨迹信息，以提高自动驾驶车辆的行驶安全性。

Description

一种车辆意图和轨迹预测的方法

技术领域

本发明属于汽车技术领域，具体的说是一种车辆意图和轨迹预测的方法。

背景技术

智能汽车是未来交通工具的发展方向，它们在改善驾驶安全和交通通行效率方面有着巨大的潜力。其中智能车的决策模块接收外部感知系统获取的环境信息，并对周围车辆及环境中的障碍物的运动进行预测，从而决定车辆接下来的行为。准确预测周边车辆的意图和轨迹至关重要，因为它是有效避免碰撞，增加在复杂交通环境中行驶安全性的保证，同时也能保证决策出的行为安全且符合社会规范，从而在不过于保守的前提下也能保证行驶的安全性以及舒适性，避免交通事故的发生。

智能车辆所面临的一大挑战就是交通环境的高度复杂和不确定性，为了解决这个问题，智能车辆应该要具有预测未来交通环境变化的能力。准确的意图和轨迹预测可以帮助智能车辆更好地理解交通环境，同时有助于轨迹规划和轨迹跟踪。

现有的研究可以大概分为三类：基于物理的模型，基于机动的模型，感知交互模型。基于物理的模型认为车辆运动需要满足动力学和运动学的约束，利用特定的演化模型可以进行一定时间间隔内车辆运动的模拟。然而，这种预测方法限制于短期预测，对长期而言是不可靠的，因为车辆会受到周边车辆机动以及环境中各种不确定因素的影响。基于机动的模型，考虑了驾驶员打算执行的操作，一旦确定了驾驶意图之后，下一步的车辆运动将与之严格匹配。但是由于驾驶员会受到其他驾驶员的影响经常改变操作所以这种假设有可能失败。感知交互模型进一步考虑了两车之间的交互作用来对车辆运动进行更可靠的预测，但这种模型大多是人为建立，所以可能存在偏差，同时建模也比较困难。

预测其他交通参与者的轨迹也不是一个确定性任务，因为它取决于每个驾驶员的意图和驾驶习惯。但它仍是自动驾驶汽车的一项重要任务，指出其中最有可能的轨迹，从而避免在智能车辆规划轨迹上的碰撞。

故，如何准确预测周边车辆的意图和轨迹信息，以提高自动驾驶车辆的行驶安全性是亟待解决的技术问题。

发明内容

本发明提供了一种车辆意图和轨迹预测的方法，能准确预测周边车辆的意图和轨迹信息，以提高自动驾驶车辆的行驶安全性。

本发明技术方案结合附图说明如下：

一种车辆意图和轨迹预测的方法，包括以下步骤：

步骤一、获取驾驶交通数据集；

步骤二、插入间隙的定义；

通过语义的方法进行意图预测；人类驾驶员在驾驶过程中会为了完成一个个小目标，不断寻找并插入到不同的行驶区域中；因此预测人类驾驶行为可被简化为预测车辆会插入到哪个区域，从而实现车辆意图的语义表示。

步骤三、相关信息信息提取；

步骤四、对交通数据集进行特征相关信息提取和状态标记；

步骤五、利用隐马尔可夫模型进行车辆行为识别；

步骤六、基于意图的轨迹预测；

步骤七、基于车辆动力学模型的轨迹预测；

步骤八、基于意图和基于车辆动力学模型的轨迹预测结合；

步骤九、指标收益计算；

步骤十、未来行为推理；

步骤十一、最终意图预测结果；

最终意图预测结果p(m_j,k)是历史轨迹行为识别结果与未来行为推理结果的结合：

p(m_j,k)＝ω₁p₁(m_j,k)+ω₂p₂(m_j,k)

其中，ω₁和ω₂分别是行为识别与未来行为推理的加权系数，满足ω₁+ω₂＝1，取决于未来行为推理中保持车道的概率；p₁(m_j,k)是行为识别概率，p₂(m_j,k)是未来行为推理概率；

当未来行为推理中车道保持概率较高时，说明此时没有刺激换道发生的条件，满足安全条件，倾向于相信行为推理的结果；当车道保持概率较低时，表明存在刺激变道发生的条件，安全条件不满足，此时倾向于相信对历史轨迹的识别结果；

最终就能得到车辆的意图预测结果，车辆的轨迹预测结果按照前述的方法也能相应得到，最终插入区域也能够知道从而实现预测的语义化。

所述步骤一的具体方法如下：

数据集车辆信息和车辆运动轨迹信息，其中车辆信息包括车辆的种类、编号、长度、宽度和所在车道，车辆运动轨迹信息包括纵向位移、横向位移、速度、加速度以及与前车的车头间距、时距；

数据集中包含了高速公路以及城市干道场景，并且由于数据集中存在噪音，对横向位移、纵向位移、速度和加速度采用滤波处理去除噪音。

所述步骤三的具体方法如下：

车辆意图受主车自身行车状态以及周围交通环境的影响；如果当前车道的行驶条件不满足驾驶员的行驶需求，而目标车道又有足够的安全距离，那么驾驶员便会倾向于换道；

需要以下的特征变量用于车辆意图的预测，其中包括车辆自身的信息以及车辆的邻居信息两方面；

车辆自身的信息包括：主车自身速度，主车纵向位置，车辆相对于当前车道中心线的横向偏移；

车辆的邻居信息包括：周围其它车辆的纵向绝对速度，纵向位置，横向位置。

所述步骤四的具体方法如下：

41)进行换到序列的提取；

为了覆盖整个车道保持以及换道过程，每段序列的长度大于7s，间隔为0.1s；找到数据集中发生了车道变换的车辆，即所在车道标号发生了变化的车辆也就是主车之后，进行车辆自身信息的提取；然后根据前面步骤二定义的插入间隙在整个数据集中查找同一时刻主车的参考车辆，并根据参考车辆确定其它车辆，最后完成步骤三中相关信息序列的提取；

42)获取时间序列后对车辆状态进行标记；

在换道场景中主车有三个状态，分别是“向左变道”，“向右变道”和“沿着当前车道行驶”；标记方法是找到车辆中心点越过车道线的点，在其前后的一定时间范围内检查车辆运动轨迹，根据车辆的位置参数(x,y)计算其航向角

其中n代表当前时刻所对应的轨迹点，n-3代表当前时刻三个采样点前所对应的轨迹点，采用跨三点计算的原因是为了避免轨迹波动的影响；当θ到达设定的起始点边界值θ_s，即从换道点逆向遍历时间轴，第一次出现|θ|<θ_s时，该点便标记为变道轨迹的起点；同样的方法确定变道轨迹的终点，起点和终点之间是变道状态，其余则是车道保持状态。

所述步骤五的具体方法如下：

51)隐马尔可夫模型中的变量分成两部分；

第一部分为状态变量q，对应的状态序列为Q＝q₁q₂...q_T，q₁为1时刻的状态变量，q₂为2时刻的状态变量，q_T为T时刻的状态变量；它不能被直接观测得到也称为隐变量，其可能取值为一组离散的值，取值集合为S＝{s₁,s₂,s_i,...,s_N},i<N，其中s_i为对应的某种状态，s₁代表状态1，s₂代表状态2，s_N代表状态N；N为所有状态的数目；

第二部分为观测变量O,对应图中的观测序列O＝O₁O₂...O_T，O₁为1时刻的观测变量，O₂为2时刻的观测变量，O_T为T时刻的观测变量；其中t时刻的观测变量为

观测变量可以是多个，

分别代表观测量1，观测量2和观测量G，G是观测变量的个数；

隐马尔可夫模型具有马尔可夫性，系统任意时刻的状态仅与上一时刻的状态有关，因此所有变量的联合概率分布为：

其中，π是初始状态概率，A是状态转移概率，

是与输出观测概率分布相关的参数，T是序列的长度，q是状态变量，O是观测变量，是状态变量，O是观测变量,q_i为i时刻的状态变量，q_i+1为i+1时刻的状态变量，O_j为j时刻的观测变量；

52)给定隐马尔可夫模型中的第三个部分；

一是状态转移概率，即系统在各个状态之间进行转移的概率，用概率转移矩阵A_N×N形式表示，其中的各个元素为a_ij＝p(q_t+1＝s_j|q_t＝s_i),i,j∈[1,N]，q是状态变量，s为具体状态，q_t为t时刻的状态变量，q_t+1为t+1时刻的状态变量，s_j和s_i分别代表状态j和状态i；二是输出观测概率，即系统在每个状态下输出观测值的概率，用b_i(O_t)表示系统在t时刻i状态下输出观测变量O的值的概率，

为控制输出观测概率分布的集合，具体来说

i∈[1,N]，q是状态变量，s为具体状态，O_t是t时刻的观测变量，s_i代表状态i；三是初始状态概率，即系统在初始时刻处于各状态的概率，记为π＝(π₁,π₂,...,π_N)，其中π_i＝p(q₁＝s_i),i∈[1,N]，s为具体状态；

53)采用连续隐马尔可夫模型，其输出观测概率通过连续的概率分布表示；利用高斯混合模型表示输出观测概率，其输出观测概率为

其中，c_im为i状态下GMM中第m个高斯分布的权重系数，N代表高斯分布，μ_im为高斯分布的均值向量，

为高斯分布的方差矩阵，M是高斯混合分布的个数，O代表观测变量，输出观测概率的参数表示为

c是权重系数向量，μ是均值矩阵，σ是方差矩阵集合；

54)利用隐马尔可夫高斯模型进行车辆行为的识别，是利用可观测变量去推测系统未知的状态；由于进行车辆意图预测时主要关注的是车辆在侧向上的行为，车辆在侧向上的行为可分为左换道，车道保持和右换道，将这三种行为作为高斯隐马尔可夫模型中隐藏变量可能的状态，那么模型中隐藏变量的取值数量N＝3；

55)对左换道，车道保持和右换道三种行为选取合适的特征；

选取车辆相对于车道中心线的侧向偏移d_y和侧向偏移速度v_y作为观测变量；车辆处于何种状态通过观测变量O＝[d_y,v_y]进行概率估计；

采用滑动时窗的方法来获取车辆的轨迹特征；在每个时刻都关注过去一段时间内的轨迹，据此来对车辆此刻的行为概率进行估计；在t时刻进行行为识别时，时间窗的宽度为△T，以△t为时间间隔采样轨迹上的n个特征点作为算法的输入，采样点的个数

从采集的这n个特征点得到t时刻观测变量的值O_t；

56)隐马尔可夫高斯模型记为元组

π是初始状态概率，A是状态转移矩阵，

是观测概率参数；其中有三个基本问题；

第一个问题是在已知模型情况下，若有观测序列，如何衡量观测序列与模型之间的匹配程度，即计算概率问题；在给定观测序列O，计算在各个时刻车辆状态取值q的概率分布，目标即计算γ_t(i)＝p(q_t＝s_i|O,λ),i∈[1,3]，q_t为t时刻的状态变量，s_i代表状态i，λ为模型参数，γ_t(i)代表t时刻状态变量取i值时的相关概率；

根据贝叶斯公式，上式改写为：

求解该问题时，定义前向向量α_t(i)和后向向量β_t(i):

α_t(i)＝p(O₁O₂...O_t,q_t＝s_i|λ)

β_t(i)＝p(O_t+1O_t+2...O_T|q_t＝s_i,λ)

根据全概率公式,得到

其中α_t(j)代表t时刻j状态的前向向量，β_t(j)代表t时刻j状态的后向向量。

变形后可得

其中α_t(j)代表t时刻j状态的前向向量，β_t(j)代表t时刻j状态的后向向量，α_t(i)代表t时刻i状态的前向向量，β_t(i)代表t时刻i状态的后向向量。计算出前向变量α和后向变量β就求出t时刻车辆状态为i的概率γ，采用初始递归的方法计算；

对于前向变量，在初始时刻，可以直接根据初始状态概率和输出观测概率计算；

α₁(i)＝π₁b_i(o₁) i∈[1,3]

其中α₁(i)代表初始时刻状态i的前向概率，π₁是初始状态概率，b_i(O₁)是初始状态的观测概率。

在其它时刻，则进行递归计算，具体含义表示在t时刻分别以三种可能的状态到达t+1时刻状态s_j的所有路径的求和；

i,j∈[1,3],t∈[1,T-1]

其中α_t(i)代表t时刻状态i的前向概率,a_ij代表从i状态向j状态转换的概率，b_j(O_t+1)代表观测概率。

后向变量β的计算思路与α一致，从T时刻开始向前递归计算；

第二个问题是已知观测序列O和模型结构，如何估计出使得该序列出现概率最大的模型参数；即在给定训练数据，即多组观测序列的情况下找出使得给定的观测序列出现概率最大的模型参数；通过最大似然方法来确定高斯隐马尔科夫模型中的参数

似然函数可以过边缘化式联合概率分布中的隐藏变量得到；

p(O|λ)＝∑_qp(O,q|λ)

其中q是状态变量，O是观测变量，λ是模型参数。

采用期望最大化方法框架来最大化似然函数，进而估计模型参数，给定n个数据序列ζ_1:n，最大似然估计方法就是找到使得似然参数最大的参数：

其中L(λ)是模型最大似然函数，λ是给定模型参数，ζ_t代表第t个数据序列，λ^*是最优模型参数；

EM算法首先为模型选择一组初始参数

在E步中，使用这组参数来计算隐藏变量的后验概率分布

其中Q是训练数据的隐藏变量集合，O是训练数据的观测变量集合；然后用该后验分布来估计完整数据似然函数，这时得到一个关于参数λ的函数，它定义为

在M步中，最大化函数

得到其估计值；E步和M步反复迭代直到满足要求得到估计结果λ^*；

第三个问题是已知模型和观测序列，如何推测系统可能的隐藏状态序列，即解码问题，采用维比特算法，求解概率最大的路径；从t＝1时刻开始，不断向后递推到下一个状态的路径的最大概率，直到在最后到达路径终点，然后依据终点回溯到起始点，得到最优路径；

57)利用步骤四中得到的序列信息，对换道行为参数进行学习之后，就可以进行车辆行为的识别，选择其中概率最大的车辆状态作为当前行为识别结果，这种行为识别基于历史轨迹，代表的是对现在行为的理解，相应的行为识别概率为p₁(m_j,k)，代表车辆j当前识别行为是k的概率。

所述步骤六的具体方法如下：

61)对于每一个需要进行轨迹预测的车辆，定义其状态向量ζ为：

ζ＝[x,y,θ,v,a,w]^T

其中x,y是纵向位置和横向位置，θ是横摆角，v是速度，a是加速度，w是横摆角速度；

在同一个笛卡尔坐标系下，道路的中心线采用二次函数进行拟合，形式如下：

y＝b₂x²+b₁x+b₀

其中，b₀,b₁,b₂都是系数，x,y分别是车道中心线的横纵坐标；

62)进行轨迹生成时采用以道路中心线为基础的Frenet框架,它是一种曲线坐标系，分为s方向和l方向；从而将车辆的二维运动问题解耦为两个一维问题，问题得到简化；

预测开始时刻车辆的状态ζ₀为

ζ₀＝[x₀,y₀,θ₀,v₀,a₀,w₀]^T

其中x₀,y₀是纵向初始位置和横向初始位置，θ₀是初始横摆角，v₀是初始速度，a₀是初始加速度，w₀是初始横摆角速度；

将其转换道曲线坐标系下，得到以下状态量

l₀＝d_#

s₀＝0

其中，d_#是初始时刻车辆与道路中心线上最近点间的距离，θ_T0是道路中心线上距离(x₀,y₀)最近点的切向量的角度值，

是曲面坐标系下l方向的初始坐标，速度以及加速度，

是曲面坐标系下s方向的初始坐标，速度以及加速度，θ₀是初始横摆角，v₀是初始速度，a₀是初始加速度，w₀是初始横摆角速度；

63)假设车辆在最终状态下沿着预定车道的中心线行驶并在整个过程中保持恒定的纵向加速度，得到以下状态信息：

l₁＝d^*

其中，d^*取决于车辆预期意图，如果预期意图是车道保持那么d^*＝0，如果预测的意图是车道变换，那么d^*＝±d，d是车道的宽度，

是曲面坐标系下l方向的终了坐标，速度以及加速度，

是曲面坐标系下s方向的终了速度以及加速度，v₀是初始速度，a₀是初始加速度，t₁是终了时刻；

65)对于横向轨迹，由于有6个已知量，所以采用五次多项式进行拟合

l(t)＝c₅t⁵+c₄t⁴+c₃t³+c₂t²+c₁t+c₀

其中，t是时间，l(t)是t时刻的横向坐标，c₀,c₁,c₂,c₃,c₄,c₅都是相关系数，通过下面的方程组进行求解：

其中，t₀是初始时刻，t₁是终了时刻；

对于纵向轨迹，由于有5个已知量，所以采用四次多项式进行拟合

s(t)＝f₄t⁴+f₃t³+f₂t²+f₁t+f₀

其中，t是时间，s(t)是t时刻的纵向坐标，f₀,f₁,f₂,f₃,f₄都是系数，通过下面的方程组进行求解：

令两个方程组中t₀＝0，这样对应于每一个特定的t₁都可以求解得到一组特定的解；这样对终了时间t₁进行采样，得到一系列不同的轨迹；由于通常一个完整的换道过程不会超过8s，所以采样时间t₁在2到8s内变化；

在获得一系列不同的轨迹后，需要将其从曲面坐标系中转换到笛卡尔坐标系中，转换公式如下：

s是曲线坐标系下的坐标，x_a是笛卡尔坐标系下的坐标，对于每一个s，都有唯一的一个x_a与之一一对应，x₀是笛卡尔坐标系下起始点的横向坐标；X是预测点，O点是某个参考点，A是起始点，d是横向距离，

是法向向量，y'(x_a)代表笛卡尔坐标系下车道中心线x_a处的导数；从而转换成笛卡尔坐标系下的轨迹；

65)从这一系列轨迹中选择最优轨迹作为预测轨迹，定义的损失函数形式如下：

Loss(Tⁱ)＝max(a(t))+r·t₁ ⁽ⁱ⁾

其中Loss(Tⁱ)代表第i条轨迹的损失函数值，a是加速度，r是权重系数，t₁ ⁽ⁱ⁾代表第i条轨迹持续时间；第一项是在该轨迹上行驶时的最大加速度值，是对舒适性的保证，第二项是对那些长持续时间轨迹的惩罚，权重系数根据实际情况调整；这样具有最小损失函数的轨迹就作为基于意图的轨迹预测结果，该轨迹定义为T_intention。

所述步骤七的具体方法如下：

基于车辆动力学模型的轨迹预测方法采用的是CTRA模型，它是一个横加速度和恒转率的模型；

具体的预测求解过程如下：

其中

是状态向量，△t为时间间隔，f(△t)是转移方程，θ是横摆角，v是速度，a是加速度，w是横摆角速度；

由此基于车辆的初始状态信息，得到基于车辆动力学模型的轨迹预测结果，该轨迹定义为T_model。

所述步骤八的具体方法如下：

采用一个权重函数将两者结合得到最终的轨迹预测结果；

T(t)＝Q(t)T_model(t)+(1-Q(t))T_intention(t)

其中，T(t)是最终轨迹预测结果，Q(t)是权重函数，Q(t)在0到1范围内变化，开始时它接近于1，最后接近于0；基于车辆动力学模型的轨迹预测结果为T_model，基于意图的轨迹预测结果为T_intention；这样就能得到一个综合的轨迹预测结果，这个结果作为后续未来行为推理中的未来交通态势预测。

所述步骤九的具体方法如下：

在未来行为推理的过程中，考虑了三个相关指标，分别是前向可行驶空间，碰撞安全系数和舒适性系数；

具体来说，前向可行驶空间α是观测车辆与其目标车道上前车的距离，即

其中，d_e是可视距离，它与车速有关，d_f是观测车辆目标车道上的可行驶空间；

碰撞安全系数β是观测车辆与其周围车辆在一段预测时域内距离最近值的负倒数之和，具体为：

其中，

和x_y0是观测车辆的位置，

和x_yi是周围车辆的位置，p是周围车辆的数目，d_min,i代表观测车辆与周围车辆i在预测时域内的最近距离，T是预测时域；

舒适性系数γ是预测时域内加速度平方积分值的负数，具体为：

其中a_x(t)和a_y(t)分别是横纵向加速度，T是预测时域。

计算碰撞安全系数和舒适性系数时运用了前面所介绍的确定性轨迹预测方法，最终的收益是三个指标的加权和：

u＝ε₁α+ε₂β+ε₃γ

其中，u是最终收益，ε₁，ε₂和ε₃是相应的加权系数，α是前向可行驶空间，β是碰撞安全系数，γ是舒适性系数。

所述步骤十的具体方法如下：

车辆未来行为推理是通过最大期望效用理论来实现的；在换道场景中，每个车辆有三种可选的行为，行为集为Ω＝{LK,LLC,RLC},分别是车道保持LK，左车道变换LLC，右车道变换RLC；但是由于实际道路的限制，一些行为被限制，相应行为的概率为0；

每个车辆都执行一个特定的行为，组成一个特定场景；根据最大期望效用理论，人们在风险的情况下做决策时会平衡考虑所有的结果；期望效用U是行为概率与相应场景收益的乘积之和，每个行为的期望效用计算如下：

其中，U₁(RLC)代表车辆1采取右车道变换的期望效用，U₁(LLC)代表车辆1采取左车道变换的期望效用，U₁(LK)代表车辆1保持车道的期望效用，m_j,i代表车j采取行为i，Ω为行为集,车辆每个行为期望效用是所有可能场景的收益与相应场景概率的乘积；

未来行为推理概率p₂(m_j,k)通过将期望效用进行归一化得到：

其中m_j,i代表车j采取行为i，U_j(k)是车辆j采取k行为的期望效用，Ω为行为集，z是车辆所有可选行为。

人类驾驶员的决策过程是一个追求收益最大化的过程，用收益函数和最大期望效用理论的结合来对其进行建模；在进行未来行为推理时考虑了未来交通态势的演变，代表的是对未来行为的理解。

本发明的有益效果为：

本发明能准确预测周边车辆的意图和轨迹信息，以提高自动驾驶车辆的行驶安全性。

附图说明

图1为典型插车间隙场景图；

图2为滑动时窗示意图；

图3为高斯隐马尔可夫模型行为识别框架示意图；

图4为Frenet框架示意图；

图5为轨迹预测流程图；

图6为换道场景示意图；

图7为未来行为推理框架图；

图8为权重方程示意图；

图9为整体模块框架图。

具体实施方式

下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。

一种车辆意图和轨迹预测的方法，包括以下步骤：

步骤一、获取驾驶交通数据集；

本发明使用了基于数据的概率图模型方法，因此需要一定数目的样本数据进行学习，以便充分学习到数据之间的内在联系。本发明采用的数据集是美国联邦高速公路管理局的ngsim数据集，其中详尽记录了收集了车辆信息和车辆轨迹信息，其中车辆信息包括车辆的种类、编号、长度、宽度和所在车道，车辆运动轨迹包括纵向位移，横向位移，速度，加速度以及与前车的车头间距、时距等。数据集中包含了高速公路以及城市干道场景。

由于该数据集是由视频分析而来，数据集中存在噪音，因此对位移，速度和加速度等采用了滤波处理以去除噪音

步骤二、插入间隙的定义；

在进行意图预测的时候，使用的是基于语义的方法。人类驾驶员在驾驶过程中会为了完成一个个小目标，不断寻找并插入到不同的行驶区域中。因此预测人类驾驶行为可被简化为预测车辆会插入到哪个区域，从而实现车辆意图的语义表示。

一个典型的三车道多车辆场景如图1所示，Car0是需要预测意图的车辆，即中主车。Car4是与主车同一车道，距离主车最近的前车，由于安全问题与责任主要取决于后车，因此没有考虑后车的影响。Car2和Car6是主车相邻两车道上纵向距离最近的车辆。Car2，Car4，Car6属于参考车辆，与两辆参考车辆Car2和Car6同一车道距离最近前后车分别定义为Car3，Car1，Car7和Car5，由此可以形成5个车辆可以最终进入的动态插入间隙，预测车辆下一时刻的意图就可以通过预测车辆将插入哪个区域实现。如果车辆最终插入的是区域1-4，那么车辆将进行的就是车道变换操作；如果车辆最终插入的是区域5，那么车辆将进行的就是车道保持操作，从而实现了车辆意图的语义化。

步骤三、相关信息提取；

车辆意图受主车自身行车状态以及周围交通环境的影响。如果当前车道的行驶条件不满足驾驶员的行驶需求，而目标车道又有足够的安全距离，那么驾驶员便会倾向于换道。

需要以下的特征变量用于车辆意图的预测，其中包括车辆自身的信息以及车辆的邻居信息两方面。

车辆自身的信息包括：主车自身速度，主车纵向位置，车辆相对于当前车道中心线的横向偏移等。

车辆的邻居信息包括：周围其它车辆的纵向绝对速度，纵向位置，横向位置等。

步骤四、对交通数据集进行特征相关信息提取和状态标记；

41)对数据集进行提取和处理，首先进行换道序列的提取，为了覆盖整个车道保持以及换道过程，每段序列的长度大于7s，间隔为0.1s。找到数据集中发生了车道变换的车辆，即所在车道标号发生了变化的车辆也就是主车之后，进行车辆自身信息的提取。然后根据前面步骤二定义的插入间隙在整个数据集中查找同一时刻主车的参考车辆，并根据参考车辆确定其它车辆，最后完成步骤三中相关信息序列的提取。

42)获取时间序列后需要对车辆状态进行标记。在换道场景中主车有三个状态，分别是“向左变道”，“向右变道”和“沿着当前车道行驶”。标记方法是找到车辆中心点越过车道线的点，在其前后的一定时间范围内检查车辆运动轨迹，根据车辆的位置参数(x,y)计算其航向角

其中n代表当前时刻所对应的轨迹点，n-3代表当前时刻三个采样点前所对应的轨迹点，采用跨三点计算的原因是为了避免轨迹波动的影响。当θ到达设定的起始点边界值θ_s，即从换道点逆向遍历时间轴，第一次出现|θ|<θ_s时，该点便标记为变道轨迹的起点。同样的方法可以确定变道轨迹的终点，起点和终点之间是变道状态，其余则是车道保持状态。这样就完成了意图状态的标记，用于后续的学习训练和验证。

步骤五、利用隐马尔可夫模型进行车辆行为识别；

隐马尔可夫模型(HMM)是用离散随机变量描述过程状态的时序概率模型，用来描述一个含有隐含未知参数的马尔可夫过程，是一个典型的图模型；

51)模型中的变量分成两部分，第一部分为状态变量q，图中对应的状态序列为Q＝q₁q₂...q_T，q₁为1时刻的状态变量，q₂为2时刻的状态变量，q_T为T时刻的状态变量；它不能被直接观测得到也称为隐变量，其可能取值为一组离散的值，取值集合为S＝{s₁,s₂,s_i,...,s_N},i<N，其中s_i为对应的某种状态，s₁代表状态1，s₂代表状态2，s_N代表状态N；N为所有状态的数目。第二部分是观测变量O,对应图中的观测序列O＝O₁O₂...O_T，其中t时刻的状态变量为

状态变量可以是多个，

分别代表观测量1，观测量2和观测量G；G是观测变量的个数。隐马尔可夫模型具有马尔可夫性，系统任意时刻的状态仅与上一时刻的状态有关，因此所有变量的联合概率分布为：

其中π是初始状态概率，A是状态转移概率，

是与输出观测概率分布相关的参数，T是序列的长度，q是状态变量，O是观测变量，q_i为i时刻的状态变量，q_i+1为i+1时刻的状态变量，O_j为j时刻的观测变量；

52)由上式可以看出，要想确定一个隐马尔可夫模型需要给定三个部分。一是状态转移概率，即系统在各个状态之间进行转移的概率，用概率转移矩阵A_N×N形式表示，其中的各个元素为a_ij＝p(q_t+1＝s_j|q_t＝s_i),i,j∈[1,N]，q是状态变量，s为具体状态，q_t为t时刻的状态变量，q_t+1为t+1时刻的状态变量，s_j和s_i分别代表状态j和状态i。二是输出观测概率，即系统在每个状态下输出观测值的概率。用b_i(O_t)表示系统在t时刻i状态下输出观测变量O的值的概率，

为控制输出观测概率分布的集合，具体来说

i∈[1,N]，q是状态变量，s为具体状态，O_t是t时刻的观测变量，s_i代表状态i。三是初始状态概率，即系统在初始时刻处于各状态的概率，记为π＝(π₁,π₂,...,π_N)，其中π_i＝p(q₁＝s_i),i∈[1,N]，s为具体状态。

53)隐马尔可夫模型按观测量是离散的还是连续的可以分为离散隐马尔可夫模型和连续隐马尔可夫模型。在进行车辆的行为识别时，通过传感器获得的车辆的运动状态都是连续的变量，因此采用连续隐马尔可夫模型，其输出观测概率通过连续的概率分布表示。高斯混合模型(GMM)理论上可以用来表示任何形式的连续概率分布，而且具有其它一些优良的计算方面的特性，因此利用高斯混合模型表示输出观测概率，其输出观测概率为

其中，c_im为i状态下GMM中第m个高斯分布的权重系数，N代表高斯分布，μ_im为高斯分布的均值向量，

为高斯分布的方差矩阵，M是高斯混合分布的个数，O代表观测变量，输出观测概率的参数可以一表示为

c是权重系数向量，μ是均值矩阵，σ是方差矩阵集合；

54)利用隐马尔可夫高斯模型进行车辆行为的识别，是利用可观测变量去推测系统未知的状态。由于进行车辆意图预测时主要关注的是车辆在侧向上的行为，车辆在侧向上的行为可分为左换道，车道保持和右换道，将这三种行为作为高斯隐马尔可夫模型中隐藏变量可能的状态，那么模型中隐藏变量可能的取值数量N＝3。

55)对于上述三种行为的识别问题而言，需要选取合适的特征。具体来说，选取车辆相对于车道中心线的侧向偏移d_y和侧向偏移速度v_y作为观测变量。车辆处于何种状态通过观测变量O＝[d_y,v_y]进行概率估计。

因为车辆任何的行为都是针对于一段连续的驾驶过程而言，仅凭单一时刻的观测变量数据无法对车辆的行为进行判断，因此采用滑动时窗的方法来获取车辆的轨迹特征。如图2所示，其基本思想是在每个时刻都关注过去一段时间内的轨迹，据此来对车辆此刻的行为概率进行估计。在t时刻进行行为识别时，时间窗的宽度为△T，以△t为时间间隔采样轨迹上的n个特征点作为算法的输入，采样点的个数

从采集的这n个特征点就可以得到t时刻观测变量的值O_t。

56)隐马尔可夫高斯模型记为元组

π是初始状态概率，A是状态转移矩阵，

是观测概率参数；有三个基本问题。第一个是在已知模型情况下，若有观测序列，如何衡量观测序列与模型之间的匹配程度，即计算概率问题。在给定观测序列O，计算在各个时刻车辆状态取值q的概率分布，目标即计算γ_t(i)＝p(q_t＝s_i|O,λ),i∈[1,3]，q_t为t时刻的状态变量，s_i代表状态i，λ为模型参数，γ_t(i)代表t时刻状态变量取i值时的相关概率。

根据贝叶斯公式，上式可以改写为：

求解该问题时，为了提高计算效率、降低算法复杂度使用前向后向算法。定义前向向量α_t(i)和后向向量β_t(i):

α_t(i)＝p(O₁O₂...O_t,q_t＝s_i|λ)

β_t(i)＝p(O_t+1O_t+2...O_T|q_t＝s_i,λ)

根据全概率公式,可以得到

其中α_t(j)代表t时刻j状态的前向向量，β_t(j)代表t时刻j状态的后向向量。变形后可得

其中α_t(j)代表t时刻j状态的前向向量，β_t(j)代表t时刻j状态的后向向量，α_t(i)代表t时刻i状态的前向向量，β_t(i)代表t时刻i状态的后向向量。只要计算出前向变量α和后向变量β就可以求出t时刻车辆状态为i的概率γ，采用初始递归的方法计算。

对于前向变量，在初始时刻，可以直接根据初始状态概率和输出观测概率计算。

α₁(i)＝π₁b_i(o₁) i∈[1,3]

其中α₁(i)代表初始时刻状态i的前向概率，π₁是初始状态概率，b_i(O₁)是初始状态的观测概率。

在其它时刻，则进行递归计算，具体含义表示在t时刻分别以三种可能的状态到达t+1时刻状态s_j的所有路径的求和。

i,j∈[1,3],t∈[1,T-1]

其中α_t(i)代表t时刻状态i的前向概率,a_ij代表从i状态向j状态转换的概率，b_j(O_t+1)代表观测概率。

后向变量β的计算思路与α一致，从T时刻开始向前递归计算。

第二个问题是已知观测序列O和模型结构，如何估计出使得该序列出现概率最大的模型参数。即在给定训练数据，即多组观测序列的情况下找出使得给定的观测序列出现概率最大的模型参数。可以通过最大似然方法来确定高斯隐马尔科夫模型中的参数

似然函数可以通过边缘化式联合概率分布中的隐藏变量得到。

p(O|λ)＝∑_qp(O,q|λ)

其中q是状态变量，O是观测变量，λ是模型参数。

采用期望最大化方法框架来最大化似然函数，进而估计模型参数，给定n个数据序列ζ_1:n，最大似然估计方法就是找到使得似然参数最大的参数：

其中L(λ)是模型最大似然函数，λ是给定模型参数，ζ_t代表第t个数据序列，λ^*是最优模型参数。

EM算法首先为初始参数

在E步中，使用这组参数来计算隐藏变量的后验概率分布

其中Q是训练数据的隐藏变量集合，O是训练数据的观测变量集合。然后用该后验分布来估计完整数据似然函数，这时可以得到一个关于参数λ的函数，它定义为

在M步中，最大化函数

就可以得到其估计值。E步和M步反复迭代直到满足要求就可以得到估计结果λ^*。

第三个问题是已知模型和观测序列，如何推测系统可能的隐藏状态序列，即解码问题，采用维比特算法，求解概率最大的路径。基于原理是，如果最优路径在某时刻t通过节点i，那么这条路径从节点i到终点的部分，在节点i到终点的路径中，必然是最优的。通过这种原理就可以从t＝1时刻开始，不断向后递推到下一个状态的路径的最大概率，直到在最后到达路径终点，然后依据终点回溯到起始点，这样就能得到最优路径。

57)根据以上三个问题，利用步骤四中得到的序列信息，对换道行为参数进行学习之后，就可以进行车辆行为的识别，选择其中概率最大的车辆状态作为当前行为识别结果，这种行为识别基于历史轨迹，代表的是对现在行为的理解，相应的行为识别概率为p₁(m_j,k)，代表车辆j当前识别行为是k的概率。高斯隐马尔科夫模型行为识别的框架流程图如图3所示。

步骤六、基于意图的轨迹预测；

在给定车辆预期意图的情况下，可以通过基于意图的方法预测车辆的轨迹。

首先对于每一个需要进行轨迹预测的车辆，定义其状态向量ζ为：

ζ＝[x,y,θ,v,a,w]^T

其中x,y是纵向位置和横向位置，θ是横摆角，v是速度，a是加速度，w是横摆角速度；在同一个笛卡尔坐标系下，道路的中心线采用二次函数进行拟合，形式如下：

y＝b₂x²+b₁x+b₀

其中，b₀,b₁,b₂都是系数，x,y分别是车道中心线的横纵坐标；

进行轨迹生成时采用以道路中心线为基础的Frenet框架,如图4所示，它是一种曲线坐标系，分为s方向和l方向。从而将车辆的二维运动问题解耦为两个一维问题，问题得到简化。

预测开始时刻车辆的状态ζ₀为

ζ₀＝[x₀,y₀,θ₀,v₀,a₀,w₀]^T

其中x₀,y₀是纵向初始位置和横向初始位置，θ₀是初始横摆角，v₀是初始速度，a₀是初始加速度，w₀是初始横摆角速度；

将其转换道曲线坐标系下，可以得到以下状态量

l₀＝d_#

s₀＝0

其中，d_#是初始时刻车辆与道路中心线上最近点间的距离，

是道路中心线上距离(x₀,y₀)最近点的切向量的角度值，

是曲面坐标系下l方向的初始坐标，速度以及加速度，

是曲面坐标系下s方向的初始坐标，速度以及加速度，θ₀是初始横摆角，v₀是初始速度，a₀是初始加速度，w₀是初始横摆角速度；

确实好车辆初始状态以后，下一步是确定车辆的终止状态。

假设车辆在最终状态下沿着预定车道的中心线行驶并在整个过程中保持恒定的纵向加速度，可以得到以下状态信息：

l₁＝d^*

其中，d^*取决于车辆预期意图，如果预期意图是车道保持那么d^*＝0，如果预测的意图是车道变换，那么d^*＝±d，d是车道的宽度，

是曲面坐标系下l方向的终了坐标，速度以及加速度，

是曲面坐标系下s方向的终了速度以及加速度，v₀是初始速度，a₀是初始加速度，t₁是终了时刻；

始态和终态确定以后，接下来便是确定具体的横向轨迹和纵向轨迹。

对于横向轨迹，由于有6个已知量，所以采用五次多项式进行拟合

l(t)＝c₅t⁵+c₄t⁴+c₃t³+c₂t²+c₁t+c₀

其中，t是时间，l(t)是t时刻的横向坐标，c₀,c₁,c₂,c₃,c₄,c₅都是相关系数，通过下面的方程组进行求解：

其中，t₀是初始时刻，t₁是终了时刻。

对于纵向轨迹，由于有5个已知量，所以采用四次多项式进行拟合

s(t)＝f₄t⁴+f₃t³+f₂t²+f₁t+f₀

其中，t是时间，s(t)是t时刻的纵向坐标，f₀,f₁,f₂,f₃,f₄都是系数，通过下面的方程组进行求解：

可以令两个方程组中t₀＝0，这样对应于每一个特定的t₁都可以求解得到一组特定的解。这样对终了时间t₁进行采样，就可以得到一系列不同的轨迹。由于通常一个完整的换道过程不会超过8s，所以采样时间t₁在2到8s内变化。

在获得一系列不同的轨迹后，需要将其从曲面坐标系中转换到笛卡尔坐标系中，转换公式如下：

s是曲线坐标系下的坐标，x_a是笛卡尔坐标系下的坐标，对于每一个s，都有唯一的一个x_a与之一一对应，x₀是笛卡尔坐标系下起始点的横向坐标；X是预测点，O点是某个参考点，A是起始点，d是横向距离，

是法向向量，y'(x_a)代表笛卡尔坐标系下车道中心线x_a处的导数；从而转换成笛卡尔坐标系下的轨迹；

下一步就是从这一系列轨迹中选择最优轨迹作为预测轨迹，定义的损失函数形式如下：

Loss(Tⁱ)＝max(a(t))+r·t₁ ⁽ⁱ⁾

其中Loss(Tⁱ)代表第i条轨迹的损失函数值，a是加速度，r是权重系数，t₁ ⁽ⁱ⁾代表第i条轨迹持续时间；第一项是在该轨迹上行驶时的最大加速度值，是对舒适性的保证，第二项是对那些长持续时间轨迹的惩罚，权重系数根据实际情况调整；这样具有最小损失函数的轨迹就作为基于意图的轨迹预测结果，该轨迹定义为T_intention。

步骤七、基于车辆动力学模型的轨迹预测；

基于车辆动力学模型的轨迹预测方法采用的是CTRA模型，它是一个横加速度和恒转率的模型。

具体的预测求解过程如下：

x(t+△t)＝x(t)+f(△t)

其中

是状态向量，△t为时间间隔，f(△t)是转移方程，θ是横摆角，v是速度，a是加速度，w是横摆角速度；

由此基于车辆的初始状态信息，可以得到基于车辆动力学模型的轨迹预测结果，该轨迹定义为T_model。

步骤八、基于意图和基于车辆动力学模型的轨迹预测结合；

基于车辆动力学模型生产的轨迹在短时间内准确度较高，但从长期来看，基于意图生成的模型更加符合驾驶员行为模式。采用一个权重函数将两者结合得到最终的轨迹预测结果。

T(t)＝Q(t)T_model(t)+(1-Q(t))T_intention(t)

其中，T(t)是最终轨迹预测结果，Q(t)是权重函数，Q(t)在0到1范围内变化，开始时它接近于1，最后接近于0；基于车辆动力学模型的轨迹预测结果为T_model，基于意图的轨迹预测结果为T_intention。这样就能得到一个综合的轨迹预测结果，这个结果作为后续未来行为推理中的未来交通态势预测。

这样就能得到一个综合的轨迹预测结果，这个结果作为后续未来行为推理中的未来交通态势预测。轨迹预测流程框图如图5所示。

步骤九、指标收益计算；

在未来行为推理的过程中，考虑了三个相关指标，分别是前向可行驶空间，碰撞安全系数和舒适性系数。

具体来说，前向可行驶空间α是观测车辆与其目标车道上前车的距离，即

其中d_e是可视距离，它与车速有关，d_f是观测车辆目标车道上的可行驶空间。

碰撞安全系数β是观测车辆与其周围车辆在一段预测时域内距离最近值的负倒数之和，具体为：

其中，

和x_y0是观测车辆的位置，

和x_yi是周围车辆的位置，p是周围车辆的数目，d_min,i代表观测车辆与周围车辆i在预测时域内的最近距离，T是预测时域；

舒适性系数γ是预测时域内加速度平方积分值的负数，具体为：

其中a_x(t)和a_y(t)分别是横纵向加速度，T是预测时域。

计算碰撞安全系数和舒适性系数时运用了前面所介绍的确定性轨迹预测方法，最终的收益是三个指标的加权和：

u＝ε₁α+ε₂β+ε₃γ

其中，u是最终收益，ε₁，ε₂和ε₃是相应的加权系数，α是前向可行驶空间，β是碰撞安全系数，γ是舒适性系数。

步骤十、未来行为推理；

车辆未来行为推理是通过最大期望效用理论来实现的。如图6所示，在换道场景中，每个车辆有三种可选的行为，行为集为Ω＝{LK,LLC,RLC},分别是车道保持LK，左车道变换LLC，右车道变换RLC。但是由于实际道路的限制，一些行为被限制，Car2和Car3不能进行左换道行为，Car4和Car5不能进行右换道行为，相应行为的概率为0。

每个车辆都执行一个特定的行为，组成一个特定场景。去除特定的不合理行为，六辆车的可能组合为144种。根据最大期望效用理论，人们在风险的情况下做决策时会平衡考虑所有的结果。期望效用U是行为概率与相应场景收益的乘积之和，以1车为例，每个行为的期望效用计算如下：

其中，U₁(RLC)代表车辆1采取右车道变换的期望效用，U₁(LLC)代表车辆1采取左车道变换的期望效用，U₁(LK)代表车辆1保持车道的期望效用，m_j,i代表车j采取行为i，Ω为行为集,车辆每个行为期望效用是所有可能场景的收益与相应场景概率的乘积；

未来行为推理概率p₂(m_j,k)可以通过将期望效用进行归一化得到：

其中m_j,i代表车j采取行为i，U_j(k)是车辆j采取k行为的期望效用，Ω为行为集，z是车辆所有可选行为。

我们认为人类驾驶员的决策过程是一个追求收益最大化的过程，用收益函数和最大期望效用理论的结合来对其进行建模。在进行未来行为推理时考虑了未来交通态势的演变，代表的是对未来行为的理解。未来行为推理的框架流程图如图7所示。

步骤十一、最终意图预测结果；

最终意图预测结果p(m_j,k)是历史轨迹行为识别结果与未来行为推理结果的结合：

p(m_j,k)＝ω₁p₁(m_j,k)+ω₂p₂(m_j,k)

其中，ω₁和ω₂分别是行为识别与未来行为推理的加权系数，满足ω₁+ω₂＝1，取决于未来行为推理中保持车道的概率；p₁(m_j,k)是行为识别概率，p₂(m_j,k)是未来行为推理概率。

当未来行为推理中车道保持概率较高时，说明此时没有刺激换道发生的条件，满足安全条件，倾向于相信行为推理的结果。当车道保持概率较低时，表明存在刺激变道发生的条件，安全条件不满足，此时倾向于相信对历史轨迹的识别结果。

最终就能得到车辆的意图预测结果，车辆的轨迹预测结果按照前述的方法也能相应得到，最终插入区域也能够知道从而实现预测的语义化。整体框架流程图如图9所示。

Claims (10)

1.一种车辆意图和轨迹预测的方法，其特征在于，包括以下步骤：

步骤一、获取驾驶交通数据集；

步骤二、插入间隙的定义；

通过语义的方法进行意图预测；人类驾驶员在驾驶过程中会为了完成一个个小目标，不断寻找并插入到不同的行驶区域中；因此预测人类驾驶行为可被简化为预测车辆会插入到哪个区域，从而实现车辆意图的语义表示。

步骤三、相关信息信息提取；

步骤四、对交通数据集进行特征相关信息提取和状态标记；

步骤五、利用隐马尔可夫模型进行车辆行为识别；

步骤六、基于意图的轨迹预测；

步骤七、基于车辆动力学模型的轨迹预测；

步骤八、基于意图和基于车辆动力学模型的轨迹预测结合；

步骤九、指标收益计算；

步骤十、未来行为推理；

步骤十一、最终意图预测结果；

最终意图预测结果p(m_j,k)是历史轨迹行为识别结果与未来行为推理结果的结合：

p(m_j,k)＝ω₁p₁(m_j,k)+ω₂p₂(m_j,k)

其中，ω₁和ω₂分别是行为识别与未来行为推理的加权系数，满足ω₁+ω₂＝1，取决于未来行为推理中保持车道的概率；p₁(m_j,k)是行为识别概率，p₂(m_j,k)是未来行为推理概率；

当未来行为推理中车道保持概率较高时，说明此时没有刺激换道发生的条件，满足安全条件，倾向于相信行为推理的结果；当车道保持概率较低时，表明存在刺激变道发生的条件，安全条件不满足，此时倾向于相信对历史轨迹的识别结果；

最终就能得到车辆的意图预测结果，车辆的轨迹预测结果按照前述的方法也能相应得到，最终插入区域也能够知道从而实现预测的语义化。

2.根据权利要求1所述的一种车辆意图和轨迹预测的方法，其特征在于，所述步骤一的具体方法如下：

数据集车辆信息和车辆运动轨迹信息，其中车辆信息包括车辆的种类、编号、长度、宽度和所在车道，车辆运动轨迹信息包括纵向位移、横向位移、速度、加速度以及与前车的车头间距、时距；

数据集中包含了高速公路以及城市干道场景，并且由于数据集中存在噪音，对横向位移、纵向位移、速度和加速度采用滤波处理去除噪音。

3.根据权利要求1所述的一种车辆意图和轨迹预测的方法，其特征在于，所述步骤三的具体方法如下：

车辆意图受主车自身行车状态以及周围交通环境的影响；如果当前车道的行驶条件不满足驾驶员的行驶需求，而目标车道又有足够的安全距离，那么驾驶员便会倾向于换道；

需要以下的特征变量用于车辆意图的预测，其中包括车辆自身的信息以及车辆的邻居信息两方面；

车辆自身的信息包括：主车自身速度，主车纵向位置，车辆相对于当前车道中心线的横向偏移；

车辆的邻居信息包括：周围其它车辆的纵向绝对速度，纵向位置，横向位置。

4.根据权利要求1所述的一种车辆意图和轨迹预测的方法，其特征在于，所述步骤四的具体方法如下：

41)进行换到序列的提取；

为了覆盖整个车道保持以及换道过程，每段序列的长度大于7s，间隔为0.1s；找到数据集中发生了车道变换的车辆，即所在车道标号发生了变化的车辆也就是主车之后，进行车辆自身信息的提取；然后根据前面步骤二定义的插入间隙在整个数据集中查找同一时刻主车的参考车辆，并根据参考车辆确定其它车辆，最后完成步骤三中相关信息序列的提取；

42)获取时间序列后对车辆状态进行标记；

在换道场景中主车有三个状态，分别是“向左变道”，“向右变道”和“沿着当前车道行驶”；标记方法是找到车辆中心点越过车道线的点，在其前后的一定时间范围内检查车辆运动轨迹，根据车辆的位置参数(x,y)计算其航向角

其中n代表当前时刻所对应的轨迹点，n-3代表当前时刻三个采样点前所对应的轨迹点，采用跨三点计算的原因是为了避免轨迹波动的影响；当θ到达设定的起始点边界值θ_s，即从换道点逆向遍历时间轴，第一次出现|θ|<θ_s时，该点便标记为变道轨迹的起点；同样的方法确定变道轨迹的终点，起点和终点之间是变道状态，其余则是车道保持状态。

5.根据权利要求1所述的一种车辆意图和轨迹预测的方法，其特征在于，所述步骤五的具体方法如下：

51)隐马尔可夫模型中的变量分成两部分；

第一部分为状态变量q，对应的状态序列为Q＝q₁q₂...q_T，q₁为1时刻的状态变量，q₂为2时刻的状态变量，q_T为T时刻的状态变量；它不能被直接观测得到也称为隐变量，其可能取值为一组离散的值，取值集合为S＝{s₁,s₂,s_i,...,s_N},i<N，其中s_i为对应的某种状态，s₁代表状态1，s₂代表状态2，s_N代表状态N；N为所有状态的数目；

第二部分为观测变量O,对应图中的观测序列O＝O₁O₂...O_T，O₁为1时刻的观测变量，O₂为2时刻的观测变量，O_T为T时刻的观测变量；其中t时刻的观测变量为

观测变量可以是多个，

分别代表观测量1，观测量2和观测量G，G是观测变量的个数；

隐马尔可夫模型具有马尔可夫性，系统任意时刻的状态仅与上一时刻的状态有关，因此所有变量的联合概率分布为：

其中，π是初始状态概率，A是状态转移概率，

是与输出观测概率分布相关的参数，T是序列的长度，q是状态变量，O是观测变量，是状态变量，O是观测变量,q_i为i时刻的状态变量，q_i+1为i+1时刻的状态变量，O_j为j时刻的观测变量；

52)给定隐马尔可夫模型中的第三个部分；

一是状态转移概率，即系统在各个状态之间进行转移的概率，用概率转移矩阵A_N×N形式表示，其中的各个元素为a_ij＝p(q_t+1＝s_j|q_t＝s_i),i,j∈[1,N]，q是状态变量，s为具体状态，q_t为t时刻的状态变量，q_t+1为t+1时刻的状态变量，s_j和s_i分别代表状态j和状态i；二是输出观测概率，即系统在每个状态下输出观测值的概率，用b_i(O_t)表示系统在t时刻i状态下输出观测变量O的值的概率，

为控制输出观测概率分布的集合，具体来说

q是状态变量，s为具体状态，O_t是t时刻的观测变量，s_i代表状态i；三是初始状态概率，即系统在初始时刻处于各状态的概率，记为π＝(π₁,π₂,...,π_N)，其中π_i＝p(q₁＝s_i),i∈[1,N]，s为具体状态；

53)采用连续隐马尔可夫模型，其输出观测概率通过连续的概率分布表示；利用高斯混合模型表示输出观测概率，其输出观测概率为

其中，c_im为i状态下GMM中第m个高斯分布的权重系数，N代表高斯分布，μ_im为高斯分布的均值向量，

为高斯分布的方差矩阵，M是高斯混合分布的个数，O代表观测变量，输出观测概率的参数表示为

c是权重系数向量，μ是均值矩阵，σ是方差矩阵集合；

54)利用隐马尔可夫高斯模型进行车辆行为的识别，是利用可观测变量去推测系统未知的状态；由于进行车辆意图预测时主要关注的是车辆在侧向上的行为，车辆在侧向上的行为可分为左换道，车道保持和右换道，将这三种行为作为高斯隐马尔可夫模型中隐藏变量可能的状态，那么模型中隐藏变量的取值数量N＝3；

55)对左换道，车道保持和右换道三种行为选取合适的特征；

选取车辆相对于车道中心线的侧向偏移d_y和侧向偏移速度v_y作为观测变量；车辆处于何种状态通过观测变量O＝[d_y,v_y]进行概率估计；

采用滑动时窗的方法来获取车辆的轨迹特征；在每个时刻都关注过去一段时间内的轨迹，据此来对车辆此刻的行为概率进行估计；在t时刻进行行为识别时，时间窗的宽度为△T，以△t为时间间隔采样轨迹上的n个特征点作为算法的输入，采样点的个数

从采集的这n个特征点得到t时刻观测变量的值O_t；

56)隐马尔可夫高斯模型记为元组

π是初始状态概率，A是状态转移矩阵，

是观测概率参数；其中有三个基本问题；

第一个问题是在已知模型情况下，若有观测序列，如何衡量观测序列与模型之间的匹配程度，即计算概率问题；在给定观测序列O，计算在各个时刻车辆状态取值q的概率分布，目标即计算γ_t(i)＝p(q_t＝s_i|O,λ),i∈[1,3]，q_t为t时刻的状态变量，s_i代表状态i，λ为模型参数，γ_t(i)代表t时刻状态变量取i值时的相关概率。

根据贝叶斯公式，上式改写为：

求解该问题时，定义前向向量α_t(i)和后向向量β_t(i):

α_t(i)＝p(O₁O₂...O_t,q_t＝s_i|λ)

β_t(i)＝p(O_t+1O_t+2...O_T|q_t＝s_i,λ)

根据全概率公式,得到

其中α_t(j)代表t时刻j状态的前向向量，β_t(j)代表t时刻j状态的后向向量。

变形后可得

其中α_t(j)代表t时刻j状态的前向向量，β_t(j)代表t时刻j状态的后向向量，α_t(i)代表t时刻i状态的前向向量，β_t(i)代表t时刻i状态的后向向量。计算出前向变量α和后向变量β就求出t时刻车辆状态为i的概率γ，采用初始递归的方法计算；

对于前向变量，在初始时刻，可以直接根据初始状态概率和输出观测概率计算；

α₁(i)＝π₁b_i(o₁)i∈[1,3]

其中α₁(i)代表初始时刻状态i的前向概率，π₁是初始状态概率，b_i(O₁)是初始状态的观测概率。

在其它时刻，则进行递归计算，具体含义表示在t时刻分别以三种可能的状态到达t+1时刻状态s_j的所有路径的求和；

其中α_t(i)代表t时刻状态i的前向概率,a_ij代表从i状态向j状态转换的概率，b_j(O_t+1)代表观测概率。

后向变量β的计算思路与α一致，从T时刻开始向前递归计算；

第二个问题是已知观测序列O和模型结构，如何估计出使得该序列出现概率最大的模型参数；即在给定训练数据，即多组观测序列的情况下找出使得给定的观测序列出现概率最大的模型参数；通过最大似然方法来确定高斯隐马尔科夫模型中的参数

似然函数可以过边缘化式联合概率分布中的隐藏变量得到；

p(O|λ)＝∑_qp(O,q|λ)

其中q是状态变量，O是观测变量，λ是模型参数。

采用期望最大化方法框架来最大化似然函数，进而估计模型参数，给定n个数据序列ζ_1:n，最大似然估计方法就是找到使得似然参数最大的参数：

其中L(λ)是模型最大似然函数，λ是给定模型参数，ζ_t代表第t个数据序列，λ^*是最优模型参数；

EM算法首先为模型选择一组初始参数

在E步中，使用这组参数来计算隐藏变量的后验概率分布

其中Q是训练数据的隐藏变量集合，O是训练数据的观测变量集合；然后用该后验分布来估计完整数据似然函数，这时得到一个关于参数λ的函数，它定义为

在M步中，最大化函数

得到其估计值；E步和M步反复迭代直到满足要求得到估计结果λ^*；

第三个问题是已知模型和观测序列，如何推测系统可能的隐藏状态序列，即解码问题，采用维比特算法，求解概率最大的路径；从t＝1时刻开始，不断向后递推到下一个状态的路径的最大概率，直到在最后到达路径终点，然后依据终点回溯到起始点，得到最优路径；

57)利用步骤四中得到的序列信息，对换道行为参数进行学习之后，就可以进行车辆行为的识别，选择其中概率最大的车辆状态作为当前行为识别结果，这种行为识别基于历史轨迹，代表的是对现在行为的理解，相应的行为识别概率为p₁(m_j,k)，代表车辆j当前识别行为是k的概率。

6.根据权利要求1所述的一种车辆意图和轨迹预测的方法，其特征在于，所述步骤六的具体方法如下：

61)对于每一个需要进行轨迹预测的车辆，定义其状态向量ζ为：

ζ＝[x,y,θ,v,a,w]^T

其中x,y是纵向位置和横向位置，θ是横摆角，v是速度，a是加速度，w是横摆角速度；

在同一个笛卡尔坐标系下，道路的中心线采用二次函数进行拟合，形式如下：

y＝b₂x²+b₁x+b₀

其中，b₀,b₁,b₂都是系数，x,y分别是车道中心线的横纵坐标；

62)进行轨迹生成时采用以道路中心线为基础的Frenet框架,它是一种曲线坐标系，分为s方向和l方向；从而将车辆的二维运动问题解耦为两个一维问题，问题得到简化；

预测开始时刻车辆的状态ζ₀为

ζ₀＝[x₀,y₀,θ₀,v₀,a₀,w₀]^T

其中x₀,y₀是纵向初始位置和横向初始位置，θ₀是初始横摆角，v₀是初始速度，a₀是初始加速度，w₀是初始横摆角速度；

将其转换道曲线坐标系下，得到以下状态量

l₀＝d_#

s₀＝0

其中，d_#是初始时刻车辆与道路中心线上最近点间的距离，

是道路中心线上距离(x₀,y₀)最近点的切向量的角度值，l₀,

是曲面坐标系下l方向的初始坐标，速度以及加速度，s₀,

是曲面坐标系下s方向的初始坐标，速度以及加速度，θ₀是初始横摆角，v₀是初始速度，a₀是初始加速度，w₀是初始横摆角速度；

63)假设车辆在最终状态下沿着预定车道的中心线行驶并在整个过程中保持恒定的纵向加速度，得到以下状态信息：

l₁＝d^*

其中，d^*取决于车辆预期意图，如果预期意图是车道保持那么d^*＝0，如果预测的意图是车道变换，那么d^*＝±d，d是车道的宽度，l₁,

是曲面坐标系下l方向的终了坐标，速度以及加速度，

是曲面坐标系下s方向的终了速度以及加速度，v₀是初始速度，a₀是初始加速度，t₁是终了时刻；

65)对于横向轨迹，由于有6个已知量，所以采用五次多项式进行拟合

l(t)＝c₅t⁵+c₄t⁴+c₃t³+c₂t²+c₁t+c₀

其中，t是时间，l(t)是t时刻的横向坐标，c₀,c₁,c₂,c₃,c₄,c₅都是相关系数，通过下面的方程组进行求解：

其中，t₀是初始时刻，t₁是终了时刻；

对于纵向轨迹，由于有5个已知量，所以采用四次多项式进行拟合

s(t)＝f₄t⁴+f₃t³+f₂t²+f₁t+f₀

其中，t是时间，s(t)是t时刻的纵向坐标，f₀,f₁,f₂,f₃,f₄都是系数，通过下面的方程组进行求解：

令两个方程组中t₀＝0，这样对应于每一个特定的t₁都可以求解得到一组特定的解；这样对终了时间t₁进行采样，得到一系列不同的轨迹；由于通常一个完整的换道过程不会超过8s，所以采样时间t₁在2到8s内变化；

在获得一系列不同的轨迹后，需要将其从曲面坐标系中转换到笛卡尔坐标系中，转换公式如下：

s是曲线坐标系下的坐标，x_a是笛卡尔坐标系下的坐标，对于每一个s，都有唯一的一个x_a与之一一对应，x₀是笛卡尔坐标系下起始点的横向坐标；X是预测点，O点是某个参考点，A是起始点，d是横向距离，

是法向向量，y'(x_a)代表笛卡尔坐标系下车道中心线x_a处的导数；从而转换成笛卡尔坐标系下的轨迹；

65)从这一系列轨迹中选择最优轨迹作为预测轨迹，定义的损失函数形式如下：

Loss(Tⁱ)＝max(a(t))+r·t₁ ⁽ⁱ⁾

其中Loss(Tⁱ)代表第i条轨迹的损失函数值，a是加速度，r是权重系数，t₁ ⁽ⁱ⁾代表第i条轨迹持续时间；第一项是在该轨迹上行驶时的最大加速度值，是对舒适性的保证，第二项是对那些长持续时间轨迹的惩罚，权重系数根据实际情况调整；这样具有最小损失函数的轨迹就作为基于意图的轨迹预测结果，该轨迹定义为T_intention。

7.根据权利要求1所述的一种车辆意图和轨迹预测的方法，其特征在于，所述步骤七的具体方法如下：

基于车辆动力学模型的轨迹预测方法采用的是CTRA模型，它是一个横加速度和恒转率的模型；

具体的预测求解过程如下：

其中

是状态向量，△t为时间间隔，f(△t)是转移方程，θ是横摆角，v是速度，a是加速度，w是横摆角速度；

由此基于车辆的初始状态信息，得到基于车辆动力学模型的轨迹预测结果，该轨迹定义为T_model。

8.根据权利要求1所述的一种车辆意图和轨迹预测的方法，其特征在于，所述步骤八的具体方法如下：

采用一个权重函数将两者结合得到最终的轨迹预测结果；

T(t)＝Q(t)T_model(t)+(1-Q(t))T_intention(t)

其中，T(t)是最终轨迹预测结果，Q(t)是权重函数，Q(t)在0到1范围内变化，开始时它接近于1，最后接近于0；基于车辆动力学模型的轨迹预测结果为T_model，基于意图的轨迹预测结果为T_intention；这样就能得到一个综合的轨迹预测结果，这个结果作为后续未来行为推理中的未来交通态势预测。

9.根据权利要求1所述的一种车辆意图和轨迹预测的方法，其特征在于，所述步骤九的具体方法如下：

在未来行为推理的过程中，考虑了三个相关指标，分别是前向可行驶空间，碰撞安全系数和舒适性系数；

具体来说，前向可行驶空间α是观测车辆与其目标车道上前车的距离，即

其中，d_e是可视距离，它与车速有关，d_f是观测车辆目标车道上的可行驶空间；

碰撞安全系数β是观测车辆与其周围车辆在一段预测时域内距离最近值的负倒数之和，具体为：

其中，

和

是观测车辆的位置，

和

是周围车辆的位置，p是周围车辆的数目，d_min,i代表观测车辆与周围车辆i在预测时域内的最近距离，T是预测时域；

舒适性系数γ是预测时域内加速度平方积分值的负数，具体为：

其中a_x(t)和a_y(t)分别是横纵向加速度，T是预测时域。

计算碰撞安全系数和舒适性系数时运用了前面所介绍的确定性轨迹预测方法，最终的收益是三个指标的加权和：

u＝ε₁α+ε₂β+ε₃γ

其中，u是最终收益，ε₁，ε₂和ε₃是相应的加权系数，α是前向可行驶空间，β是碰撞安全系数，γ是舒适性系数。

10.根据权利要求1所述的一种车辆意图和轨迹预测的方法，其特征在于，所述步骤十的具体方法如下：

车辆未来行为推理是通过最大期望效用理论来实现的；在换道场景中，每个车辆有三种可选的行为，行为集为Ω＝{LK,LLC,RLC},分别是车道保持LK，左车道变换LLC，右车道变换RLC；但是由于实际道路的限制，一些行为被限制，相应行为的概率为0；

每个车辆都执行一个特定的行为，组成一个特定场景；根据最大期望效用理论，人们在风险的情况下做决策时会平衡考虑所有的结果；期望效用U是行为概率与相应场景收益的乘积之和，每个行为的期望效用计算如下：

其中，U₁(RLC)代表车辆1采取右车道变换的期望效用，U₁(LLC)代表车辆1采取左车道变换的期望效用，U₁(LK)代表车辆1保持车道的期望效用，m_j,i代表车j采取行为i，Ω为行为集,车辆每个行为期望效用是所有可能场景的收益与相应场景概率的乘积；

未来行为推理概率p₂(m_j,k)通过将期望效用进行归一化得到：

其中m_j,i代表车j采取行为i，U_j(k)是车辆j采取k行为的期望效用，Ω为行为集，z是车辆所有可选行为。

人类驾驶员的决策过程是一个追求收益最大化的过程，用收益函数和最大期望效用理论的结合来对其进行建模；在进行未来行为推理时考虑了未来交通态势的演变，代表的是对未来行为的理解。

Images