[发明专利]一种车辆意图和轨迹预测的方法

权利要求书

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)是未来行为推理概率；

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

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

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

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

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

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

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

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

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}，iN，其中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是观测变量，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的值的概率，ρ为与输出观测概率分布相关的参数，具体来说b_i(O_t)＝p(O_t|q_t＝s_i,ρ),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个高斯分布的权重系数，Y代表高斯分布，μ_im为高斯分布的均值向量，为高斯分布的方差矩阵，M是高斯混合分布的个数，O代表观测变量，输出观测概率的参数表示为c是权重系数向量，μ是均值矩阵，σ是方差矩阵集合；

54)利用隐马尔可夫高斯模型进行车辆行为的识别，是利用可观测变量去推测系统未知的状态；由于进行车辆意图预测时主要关注的是车辆在侧向上的行为，车辆在侧向上的行为可分为左换道，车道保持和右换道，将这三种行为作为高斯隐马尔可夫模型中隐藏变量可能的状态，那么模型中隐藏变量的取值数量R＝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的概率γ，采用初始递归的方法计算；

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

α_l(i)＝π_lb_i(o_l) i∈[1，3]

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

在其它时刻，则进行递归计算，具体含义表示在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步中，使用这组参数来计算隐藏变量的后验概率分布p(Q|O，)，其中Q是训练数据的隐藏变量集合，O是训练数据的观测变量集合；然后用该后验分布来估计完整数据似然函数，这时得到一个关于参数λ的函数，它定义为在M步中，最大化函数得到其估计值；E步和M步反复迭代直到满足要求得到估计结果λ^*；

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

57)利用步骤四中得到的序列信息，对换道行为参数进行学习之后，就可以进行车辆行为的识别，选择其中概率最大的车辆状态作为当前行为识别结果，这种行为识别基于历史轨迹，代表的是对现在行为的理解，相应的行为识别概率为p_l(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框架，是一种曲线坐标系，分为r方向和l方向；从而将车辆的二维运动问题解耦为两个一维问题，问题得到简化；

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

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

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

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

l₀＝d_#

r₀＝0

其中，d_#是初始时刻车辆与道路中心线上最近点间的距离，θ_T0是道路中心线上距离(x₀，y₀)最近点的切向量的角度值，l₀，是曲面坐标系下l方向的初始坐标，速度以及加速度，r₀，是曲面坐标系下r方向的初始坐标，速度以及加速度，θ₀是初始横摆角，v₀是初始速度，a₀是初始加速度，w₀是初始横摆角速度；

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

l₁＝d*

其中，d*取决于车辆预期意图，如果预期意图是车道保持那么d*＝0，如果预测的意图是车道变换，那么d*＝±d，d是车道的宽度，l₁，是曲面坐标系下l方向的终了坐标，速度以及加速度，是曲面坐标系下r方向的终了速度以及加速度，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内变化；

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

r是曲线坐标系下的坐标，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；

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

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

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

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

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

其中，和是观测车辆的位置，和是周围车辆的位置，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是车辆所有可选行为；

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