一种周围车辆驾驶行为意图的推断方法、系统及设备

本发明属于车辆智能驾驶，具体地说涉及一种周围车辆驾驶行为意图的推断方法、系统及设备。

背景技术：

1、由于经济快速发展，道路上的车辆越来越多，交通事故频发，驾驶员以及周围车辆驾驶员的安全难以得到有效保障。根据驾驶员的行为意图，车辆驾驶行为可分为：左换道、直行和右换道，而换道不仅改变了自车的驾驶行为，还会对周围车辆的驾驶行为产生影响，在换道时因失误或失察而导致的事故较多。因此，如何准确推断出周围车辆驾驶行为意图，从而提高自车换道的安全性、避免发生交通事故成为了亟需解决的问题。

技术实现思路

1、本发明设计了一种周围车辆驾驶行为意图的推断方法，包括以下步骤：

2、步骤1：建立以s轴、q轴为坐标轴的frenet坐标系，将目标车辆的驾驶路径建模为三次样条曲线；

3、步骤2：获取目标车辆在坐标系中的位置(st,qt)和初始航向角θt，确定初始路径公式(1)：

4、q(s)＝a(s-st)3+b(s-st)2+c(s-st)+d，(st≤s≤sf)  (1)

5、所述路径的起点坐标为(st，qt)、终点坐标为(sf，qf)，其中，qf＝qi，i＝1,2,3，所述qi为每个车道中心线对应的q值，所述s的取值范围为[st,sf]，所述a、b、c、d表示系数；

6、步骤3：获取所述终点sf的计算公式(3)：

7、sf＝vttprev+st  (3)

8、其中，vt表示目标车辆的纵向速度，tprev表示换道时间，sf-st表示目标车辆从换道开始至换道完成所行驶的纵向距离；

9、预设初始换道时间tprev，

10、根据初始换道时间tprev和终点sf的计算公式，获取路径的预设终点sf；

11、步骤4：获取系数a、b、c、d的计算公式(4)，

12、

13、根据系数a、b、c、d的计算公式，确定初始路径的唯一形状；

14、步骤5：获取当前周期k，利用线性递归最小二乘估计算法，根据公式(1)、公式(3)和公式(4)，估计当前周期k自适应换道时间tprev,k，

15、根据公式(3)和tprev,k，获得路径的实际终点sf；

16、步骤6：根据路径的实际终点sf得到三条路径，计算每条路径的概率，

17、根据概率最高的路径，推断目标车辆的驾驶行为意图。

18、与现有技术相比，本发明的有益效果在于：获取当前周期k的三条路径及其概率后，在以后的每个周期更新一次三条路径及其概率，并获取其中概率最高的一条路径，重新推断目标车辆的驾驶行为意图，有利于提高推断结果的准确性。通过本发明的推断方法，能够对周围车辆的驾驶行为意图进行推断，提前预判周围车辆的驾驶轨迹(左换道或直行或右换道)，从而对自车的换道轨迹进行规划，有利于提高自车换道时的安全性，避免发生交通事故，保障道路安全。

19、优选的，获取初始路径的四个边界条件，根据所述边界条件，获取系数a、b、c、d的计算公式；

20、所述边界条件为：

21、

22、其中，q(st)＝qt表示初始路径的起点边界；表示初始路径起点的斜率，θt表示目标车辆的初始航向角；q(sf)＝qi表示初始路径的终点边界，qi表示每个车道中心到s轴的距离；表示初始路径终点的斜率；

23、根据所述四个边界条件获取系数a、b、c、d的计算公式。

24、优选的，根据公式(1)、公式(3)和公式(4)，获取系数a的变形公式(5)和b的变形公式(6)：

25、

26、

27、根据公式(5)和公式(6)，将公式(1)变形得到公式(7a)：

28、

29、所述公式(7a)表示为与换道时间tprev相关的路径，θk为换道时间的倒数，其中，ak、bk、ck、dk分别对应系数a、b、c、d，根据公式(4)、(5)和公式(6)，得到ak、bk、ck、dk的计算公式：

30、

31、

32、

33、dk＝c(sk-st)+d  (7e)

34、其中，vt为目标车辆在初始路径起点处的纵向速度值，所述vt为恒定值，(sk,qk)为当前周期k时目标车辆的坐标，qf为路径终点的q值，st路径起点的s值。

35、优选的，利用递归最小二乘法对参数θ进行自适应估计，递归公式为：

36、

37、其中，为公式(7a)中θk的最小二乘估计，为上个周期θ的最小二乘估计，φ(k)为当前周期k可供测量的信号，φt(k)为φ(k)的转置，φt(k)上个周期预测的q值，y(k)为当前周期k实际的q值，l(k)为当前周期k时的递归系数；

38、所述l(k)的计算公式为：

39、l(k)＝p(k)φ(k)＝p(k-1)φ(k)(λ+φt(k)p(k-1)φ(k))-1  (9)

40、p(k)＝(i-l(k)φt(k))λ-1p(k-1)  (10)

41、其中，p(k)为误差协方差矩阵，λ为遗忘因子，λ∈(0，1]。

42、优选的，对公式(8)进行非线性变形，得到公式(11)：

43、y(k)＝f(k，θ)+v  (11)

44、其中，y(k)为当前周期k实际的q值，f(k,θ)为非线性系统q值的计算公式，v为建模时的噪声；

45、设定v＝0，对公式(11)的y(k)进行以下近似：

46、

47、其中，表示当前周期为k，获取k-1时刻计算的q值，f(k)表示为斜率，

48、

49、将所述公式(12)变形为公式(7a)的形式，将q值的计算公式与换道时间进行关联：

50、

51、其中，为使用预测模型计算的q值；

52、将公式(13)展开得到：

53、

54、其中，为斜率f(k)，

55、设定当前周期k测量的信号为路径的斜率，即φt(k)＝f(k)，将公式(10)变形为：

56、

57、其中，pk为当前周期k的误差协方差矩阵，pk-1为上个周期的误差协方差矩阵，λ的取值范围为(0,1]；

58、根据公式(14)、(15)、(16)，得到参数θ的最小二乘估计：

59、

60、其中，qk,m为当前周期k实际的q值，为当前周期k计算的q值，设定预测开始时

61、根据公式(17)，计算得到当前周期k的自适应换道时间为：

62、

63、优选的，预设换道时间阈值tth，

64、判断所述自适应换道时间tprev,k是否小于tth，

65、若是，则根据公式(3)和tprev,k，获得路径的实际终点sf，根据路径的实际终点sf得到三条路径，计算每条路径的概率，根据概率最高的路径，推断目标车辆的驾驶行为意图；

66、否则，推断目标车辆的驾驶行为意图为保持当前车道直行。

67、优选的，根据路径的实际终点sf得到三条路径，计算每条路径的概率，包括以下步骤：

68、获取系统建模公式：

69、

70、

71、其中，z(tk)为x(tk)的测量值，m(tk)表示为当前周期k目标车辆所处的路径模式，w[tk,m(tk)]表示为当前周期k路径模式m(tk)的测量噪声，v[tk-1,m(tk)]表示为零均值高斯白噪声，f[m(tk)]表示为系统建模的线性化方程x(tk)的斜率，h[m(tk)]表示为z(tk)的斜率；

72、获取ti至tk时刻的测量数据，

73、根据公式(19)和(20)，得到路径模式mj的后验概率：

74、

75、其中，表示ti至tk时间内的测量值序列；z(tk)表示tk时刻的测量值；j＝1,2,3；当j取不同的值时，表示不同的路径模式mj；

76、根据公式(21)，得到每条路径的模式概率ak[j]：

77、

78、其中，ak[j]表示为当前周期k第j条路径的模式概率，ak-1[j]表示为上个周期第j条路径的模式概率；

79、根据其中模式概率最高的一条路径，推断目标车辆的驾驶行为意图为左换道或直行或右换道。

80、优选的，获取路径模式mj在tk时刻的似然函数：

81、

82、其中，vj表示为在路径模式mj下的测量值的线性组合，pj为其协方差，为vj(tk)的概率密度函数，vj(tk)为高斯随机变量，均值设为0，pj(tk)为vj(tk)的协方差；

83、获取每个路径模式的估计误差协方差pk[j]以及测量值与实际值之间差值的均值rk[j]，根据pk[j]和rk[j]，计算得到每条路径的似然性p(rk[j]|mj)，

84、设定似然性p(rk[j]|mj)具有高斯分布，根据公式(22)，得到似然性p(rk[j]|mj)的计算公式：

85、

86、其中，rk[j]表示为当前周期k、第j条路径模式下测量值与实际值之间差值的均值，pk[j]表示为当前周期k、第j条路径模式的估计误差协方差，n为测量值序列的范数；

87、将公式(23)代入公式(21)，得到每条路径的模式概率ak[j]。

88、本发明还设计了一种周围车辆驾驶行为意图的推断系统，包括：

89、控制模块，配置用于

90、建立以s轴、q轴为坐标轴的frenet坐标系，将目标车辆的驾驶路径建模为三次样条曲线，

91、预设初始换道时间tprev，

92、信息采集模块，配置用于

93、获取目标车辆在坐标系中的位置(st,qt)和初始航向角θt，确定初始路径公式(1)，

94、获取所述终点sf的计算公式(3)；

95、根据初始换道时间tprev和终点sf的计算公式(3)，获取路径的预设终点sf，

96、获取系数a、b、c、d的计算公式(4)；

97、计算模块，配置用于

98、利用线性递归最小二乘估计算法，根据公式(1)、公式(3)和公式(4)，估计当前周期k自适应换道时间tprev,k，

99、根据公式(3)和tprev,k，获得路径的实际终点sf，

100、根据路径的实际终点sf得到三条路径，计算每条路径的概率，

101、根据概率最高的路径，推断目标车辆的驾驶行为意图。

102、本发明还设计了一种计算机设备，包括存储器和处理器；

103、所述存储器，用于存储计算机程序；

104、所述处理器，用于当执行所述计算机程序时，实现上述的推断方法。