一种面向商用车车道保持辅助系统的精准测评方法

1.本发明涉及一种针对车道保持辅助系统的测评方法，尤其涉及一种面向商用车车道保持辅助系统的精准测评方法，属于智能驾驶测评领域。


背景技术：

2.车道保持辅助(lka，lane keeping assist)是应用于普通道路及高速公路等具有明确车道边界线车道上，辅助驾驶员保持车辆在车道内行驶的系统，该系统应具备车道偏离抑制或车道居中控制功能。当lka系统处于激活状态时，若车辆发生非驾驶意愿的车道偏离，lka系统应进行车辆横向运动控制以辅助驾驶员将车辆保持在原车道内行驶，从而修正行驶方向，避免异常驶出车道的情况，消除安全隐患，减少事故的发生。系统应用场景主要包括车道偏离抑制与车道居中控制两个方面，因而本发明对lka的两大应用的场景也进行了区分。
3.目前，对于上述应用已有标准《乘用车车道保持辅助(lka)系统性能要求及试验方法》(gb/t 39323-2020)以及《汽车转向系基本要求》(gb/t 17675-2021)，但其仅列出要求，未给出车道线距离等测评指标的计算方法，也没有针对大型商用车的具体测评方法，本发明方法的目的在于针对包含牵引车和拖挂车的大型商用车，为上述应用提供一种车道线拟合方法并完善标准相应的测评方法：利用高精度车载组合导航系统分别获取车道线以及测试车辆的基础参数数据，根据标准要求，结合牵引车和拖挂车的铰接特点，进行参数联合估计，在数据高精度处理的基础上，分析得到测评结果。本发明对上述标准进行补充，详细给出了测评过程中，评价车道保持行为所需的车道线方程、车轮边缘与车道线距离、铰接角、横摆稳定性、车道保持精度等参数的测评解算方法。


技术实现要素：

4.本发明的目的是提出了一种面向商用车车道保持辅助系统的精准测评方法，利用高精度车载组合导航系统分别获取测评所需的车道线以及测试车辆基础参数数据，采用曲线拟合、参数联合估计和数据插值等处理方法，对被测商用车与车道线距离、偏离速度、横摆稳定性、车道保持精度等指标进行测评。
5.具体步骤包括：
6.步骤一：建立面向车道保持辅助测试场景的道路模型；
7.子步骤1：采集原始数据，利用高精度车载组合导航系统采集需要测评路段的车道线轨迹数据信息；
8.子步骤2：对采集到的道路信息进行投影变换，以交叉路口为端点对车道进行分段，再根据分段的车道为直道或弯道，将其细化为独立的车道数据块，利用坐标变换，将分段后数据的经纬度坐标转换为局部切平面直角坐标系坐标，转换后得到的某段独立道路全部轨迹数据为t
line
＝{t0(c0,z0),t1(c1,z1),
…
,tn(cn,zn)},其中，cn，zn，分别为道路轨迹点的北向位置坐标和东向位置坐标，n为轨迹数据点的个数；
9.子步骤3：建立道路模型；利用kochanek-bartels样条曲线对子步骤2中每个道路数据块的轨迹数据进行插值，样条曲线受坐标集t
line
＝{t0(c0,z0),t1(c1,z1),
…
,tn(cn,zn)}中的控制点控制，每个控制点处的切线值由与该控制点相邻的轨迹控制点计算得出。独立的单侧车道数据块中，除去两端的控制点t0(c0,z0)、tn(c
n-1
,z
n-1
)、tn(cn,zn)后，对剩余的n-3个控制点，三次kochanek-bartels样条曲线可由任意4个连续控制点确定，设p(u)是两控制点tk和t
k+1
间的三次函数式，其中u为参变量，0≤u≤1，＝2,3,
…
,n-2，则从t
k-1
到t
k+2
间的四个控制点用于建立拟合车道线的边界条件：
10.p(0)＝tk11.p(1)＝t
k+1
[0012][0013][0014]
式中p(0)和p(1)分别为p(u)在节点tk和t
k+1
之间曲线段两个端点的位置向量，p
′
(0)
in
和p
′
(1)
out
分别为曲线段在两端点处的切向量，曲线参变量u是在两个端点取值0和1之间变化。参数t是张量参数，控制拟合轨迹的松紧程度；参数b是偏离参数，调整曲线段在端点处弯曲的数值；参数c是连续性参数，控制切向量在曲线边界处的连续性。
[0015]
记两端点处切向量为和即：
[0016][0017][0018]
将上述边界条件转换成矩阵形式：
[0019][0020]
其中hermite矩阵是
[0021][0022]
将上述矩阵方程展开成多项式形式，可以有
[0023][0024]
经由上述过程，得到了独立的单侧车道数据t1(c1,z1)到t
n-2
(c
n-2
,z
n-2
)之间，kochanek-bartels样条插值的车道线拟合方程。
[0025]
对于单独测评路段，通过上述子步骤可以获得该路段车道线的拟合曲线c1、c2，分别对应目标路段的左右车道边界，另外定义车道中线为车道中央的曲线，该曲线上的点到左、右车道边界线的距离相等，一般作为车道保持跟踪目标曲线，由左右侧车道线的数据取均值得到。
[0026]
步骤二：测试过程中车辆运动参数的卡尔曼滤波联合估计
[0027]
为牵引车和拖挂车安装组合导航系统：对于拖挂车，沿其纵轴安装有两个天线(即
主天线和从天线)的接收器用于测量主天线所在观测点的基础参数，包括位置、速度、加速度、航向角和横摆角速度；对于牵引车，只需在其纵轴上安装一个单天线接收器，由于单天线能记录的参数只有位置、速度、加速度，其航向角和横摆角速度需另外计算得出。组合导航系统的时间以北斗时间为参考基准，在选定测评路段上采集对应的基础参数以及对应的采样时刻；
[0028]
子步骤1：拖挂车基础参数卡尔曼滤波处理
[0029]
首先，定义拖挂车的系统位置状态向量为：
[0030]
x
1_p
＝[p
1_ev1_ea1_e
p
1_nv1_na1_n
]
′
[0031]
其中，p
1_e
，v
1_e
，a
1_e
分别代表p1点(主天线)的东向位置、东向速度分量与东向加速度分量，同样的，p
1_n
，v
1_n
，a
1_n
分别代表其的北向位置、北向速度分量与北向加速度分量。选取gps接收机作为车辆运动的测量传感器，其输出的位置和速度作为系统的观测量。
[0032]
对应离散状态为：
[0033]
x
1_p
(k)＝φ
1_p
·
x
1_p
(k-1)+u
1_p
(k-1)+w
1_p
(k-1)
[0034]
其中k为离散时间间隔，φ
1_p
为位置状态转移矩阵，u
1_p
为位置输入向量，w
1_p
为位置噪声向量且
[0035][0036]
其中t为离散周期，和分别是在东向上与北向上的相关时间常数，和分别是当前东向上和北向上加速度分量均值。
[0037]
类似的，拖挂车的角度离散状态为
[0038]
x
1_φ
(k)＝φ
1_φ
·
x
1_φ
(k-1)+w
1_φ
(k-1)
[0039]
其中φ
1_φ
为角度状态转移矩阵，w
1_φ
为角度噪声向量且
[0040][0041]
其中φ1是拖挂车的朝向角，ω1是拖挂车的横摆角速度。
[0042]
结合位置与角度离散状态，整个拖挂车的卡尔曼滤波状态方程为：
[0043]
x1(k)＝φ1·
x1(k-1)+u1(k-1)+w1(k-1)
[0044]
其中其中
[0045]
拖挂车的观测信息来源于包含gps接收机与陀螺仪的组合导航系统，gps接收机可以输出观测点的位置、速度与方位角信息，对应观测方程为：
[0046]z1_gps
(k)＝h
1_gps
·
x
1_p
(k)+n
1_gps
(k)
[0047]
其中，z
1_gps
是两天线的测量信息，h
1_gps
是其观测矩阵，n
1_gps
是协方差矩阵为r
1_gps
的观测噪声向量，且：
[0048][0049]
其中，z
p1_e
，z
p1_n
，z
1_v
，z
1_φ
分别是天线测得的东向位置，北向位置，速度，航向角；n
p1_e
，n
p1_n
，n
1_v
，n
1_φ
分别是对应的观测白噪声。
[0050]
陀螺仪可以输出横摆角速度，观测方程为：
[0051]z1_gryo
(k)＝h
1_gryo
·
x
1_φ
(k)+n
1_gryo
(k)
[0052]
其中，z
1_gryo
(k)＝[z
1_ω
]是陀螺仪的横摆角速度测量值，h
1_gryo
＝[0 1]是其观测矩阵，n
1_gryo
是其观测白噪声。
[0053]
结合所有观测量，观测方程为：
[0054]
z1(k)＝h1·
x1(k)+n1(k)
[0055]
其中噪声向量n1的协方差矩阵是r1，且，且
[0056]
根据以上状态方程与观测方程，在k时刻拖挂车基础参数卡尔曼滤波处理过程如下：
[0057]
(1)状态量的先验估计
[0058][0059]
(2)计算先验误差协方差矩阵
[0060][0061]
其中q1为过程噪声的协方差矩阵。
[0062]
(3)计算卡尔曼增益
[0063][0064]
其中r1为测量噪声的协方差矩阵。
[0065]
(4)计算状态量后验估计
[0066][0067]
(5)更新误差协方差矩阵
[0068][0069]
子步骤2：牵引车基础参数卡尔曼滤波处理
[0070]
对于牵引车，只需对位置状态相关数据进行滤波处理，系统状态方程为：
[0071]
x2(k)＝φ2·
x2(k-1)+u2(k-1)+w2(k-1)
[0072]
其中，x2是系统状态向量，φ2是其状态转移矩阵，u2为系统输入，w2是噪声向量，且：
[0073]
φ2＝φ
1_p
，w2＝w
1_p
[0074]
其中，p
2_e
，v
2_e
，a
2_e
分别代表p2点(天线安装位置)东向位置、东向速度分量与东向加速度分量，同样的，p
2_n
，v
2_n
，a
2_n
分别代表其的北向位置、北向速度分量与北向加速度分量；和分别是当前东向上和北向上加速度分量均值。
[0075]
牵引车的观测信息来源于单天线gps接收机，输出位置，速度信息，对应的观测方程为：
[0076]
z2(k)＝h2·
x2(k)+n2(k)
[0077]
其中，z2是单天线的测量信息，h2是观测矩阵，n2是对应的测量噪声向量，且：
[0078][0079]
其中，z
p2_e
，z
p2_n
，z
p2_v
分别是天线测得的东向位置，北向位置，速度；n
p2_e
，n
p2_n
，n
2_v
分别是对应的观测白噪声。
[0080]
根据以上状态方程与观测方程，在k时刻牵引车基础参数卡尔曼滤波处理过程与拖挂车基础参数卡尔曼滤波处理过程类似，同样包括态量的先验估计、计算先验误差协方差矩阵、计算卡尔曼增益、计算状态量后验估计、更新误差协方差矩阵等步骤。
[0081]
子步骤3：牵引车航向角与横摆角速度估计
[0082]
牵引车的航向角与横摆角速度可由拖挂车滤波后的基础参数计算获得，根据二者几何约束可得：
[0083]
φ
2-α
2-β2＝π
[0084]
φ1+γ
1-α2＝π
[0085]
则牵引车的航向角为：
[0086]
φ2＝φ1+β2+γ1[0087]
其中，β2＝arccos(m)，＝arccos(m)，点o为铰接点。
[0088]
为计算横摆角速度，记则牵引车横摆角速度可由以下微分方程得出：
[0089][0090]
结合dz/dt＝((p
2_e-p
1_e
)(v
2_e-v
1_e
)+(p
2_e-p
1_e
)(v
2_e-v
1_e
))/z，可以得到
[0091]
[0092]
至此，测试过程中拖挂车与牵引车的主天线处(p1点和p2点)的位置、速度、加速度、航向角和横摆角速度均为已知。
[0093]
步骤三：车道保持辅助系统性能测评
[0094]
子步骤1：车辆与车道线的距离测评
[0095]
(1)特定点位置推算：对于车道保持辅助测评场景，测评过程中需要车辆某一特定点位置推算，以车道偏离的界定标准为例，车辆轮胎的外缘是否将要超出车道边线为判断偏离的标准，而组合导航系统记录的车辆位置实际为系统主天线的安装位置，因此需要先将记录的主天线运动轨迹转换成大型商用车特定轮胎外缘的运动轨迹，选定牵引车前轮外缘为特定点，推算过程如下：
[0096]
假定该时刻为tc，tc时刻对应的经纬度坐标为(xc,yc)，若组合导航信息的采集时刻没有的tc对应值，可采用线性插值计算得出经纬度坐标为(xc,yc)，xc为纬度、yc为经度，单位为度；其余测量参数也应进行插值处理。然后将其转换为高斯投影坐标(xu,yu)(东向位置、北向位置确定的平面投影坐标系)，单位为m；
[0097]
主天线安装在牵引车的纵轴上，与两前轮外缘处的纵向距离为d，横向距离为l，车辆航向角为φ2。局部切平面直角坐标系坐标下，左前轮外缘处的位置坐标为：
[0098][0099]
其中β＝π-α，d2＝d4
·
cosβ，d4＝d-d3，d3＝d1
·
cosβ。
[0100]
类似的，可以根据几何关系计算出右前轮外缘处的位置坐标pr。
[0101][0102]
对组合导航系统中记录的牵引车位置均作以上处理，即可得到任意时刻牵引车前轮外缘的运动轨迹；类似的，可以用相同的解算方法得出拖挂车的末端外边缘(或其他任意部位在地面的投影)所划过的轨迹，作为后续测评的数据基础。
[0103]
(2)距离计算：车道保持辅助测评过程中，车辆与车道线的距离为车辆特定部位与车道边界之间的横向距离，由于大型商用车的铰接特性，通常牵引车与拖挂车在此步骤中应视为单独的测评对象分别进行距离测评。以两前轮外缘位置为例，分别计算与两侧车道边界的距离，假定发生左偏，需计算牵引车左前车轮外缘与左侧车道线的距离，计算过程如下：
[0104]
假定该时刻仍为tc，局部切平面直角坐标系坐标左前轮外缘处的位置坐标已知，为p
l
；左侧车道线拟合c1上，p
l
位于控制点tc和t
c+1
之间确定路段的横向区域内，该路段对应拟合曲线：
[0105][0106]
其本质上对应一个关于u∈[0,1]的多项式：
[0107]
[0108]
考虑到拟合实际车道线的情况下，两控制点tc和t
c+1
之间拟合曲线斜率变化不大且较平缓，用点p
l
到三次曲线p(u)的最短距离代替横向距离，记为表示车辆上左前轮外缘点p
l
与车道线的距离；特别的，当车辆轮胎外缘位于车道边界线外侧时，取负值。
[0109]
子步骤2：铰接角测评
[0110]
铰接角是指行驶过程中牵引车和拖挂车轴线之间出现的相对夹角，计算方法如下：
[0111]
δ＝φ
1-φ2[0112]
其中φ1表示拖挂车航向角，φ2表示牵引车的航向角。
[0113]
子步骤3：横摆稳定性测评
[0114]
横摆稳定性是指汽车车身坐标系垂向轴偏转大小的程度，综合考虑牵引车和挂车的横摆角速度，横摆稳定性测评需利用牵引车和拖挂车的速度、横摆角速度以及对应车道的曲率数据，以评价lka的控制稳定程度，计算方法如下：
[0115][0116]
其中η表示大型商用车横摆稳定性的量化值，sum为一次测试中横摆角速度数据的数量，由于牵引车横摆角速度由拖挂车横摆角速度等数据推算所得，同一次测试中二者数据量相同；η1表示拖挂车横摆稳定性，η2表示牵引车横摆稳定性，计算方法如下：
[0117][0118]
ω
t
表示t时刻的车辆横摆角速度，t时刻横摆角速度期望值ωe＝v
t
/r
l
，其中v
t
是t时刻车辆速度，r
l
是道路曲率半径，即车辆与车道线距离测评中迭代出的插值点处的曲率半径。
[0119]
子步骤4：车道保持精度测评
[0120]
车道保持精度是指车道保持辅助测评过程中，车辆纵轴与车道中线距离的均方差。大型商用车由于其铰接特性，应分别考虑牵引车和拖挂车的车道保持精度，以综合评价大型商用车的车道保持精度，计算方法如下：
[0121][0122]
其中，ε表示车道保持精度量化值；ε1表示拖挂车车道保持精度，ε2表示牵引车车道保持精度，计算方法如下：
[0123][0124]
其中，sumi为测试过程中位置数据的数量，d
it
表示t时刻牵引车或拖挂车主天线与车道中线的距离(主天线安装在牵引车和拖挂车的纵轴上)，可由子步骤1中的距离计算方法获得。i＝1表示拖挂车的相关参数，i＝2表示牵引车的相关参数。
[0125]
本发明的优点及显著效果：
[0126]
(1)本发明的车道线拟合原理基于kochanek-bartels样条曲线，能够较好的适应
道路曲率变化大的特点，可调控性较好，获取的道路模型能够更真实的反应测试车道情况，作为lka测评的可靠基础数据。
[0127]
(2)本发明所提出的测评方法与lka系统的具体实现原理和技术无关，根据牵引车与拖挂车组成的大型商用车的行驶特点，从第三方的角度开展测评，完善相关标准的测评细节，测评项目全面。
附图说明
[0128]
图1是测评流程示意图；
[0129]
图2是测评车道模型建立流程图；
[0130]
图3是大型商用车天线安装与几何关系示意图；
[0131]
图4是牵引车前轮胎外缘位置坐标计算示意图；
[0132]
图5是大型商用车纵轴与车道中线距离示意图。
具体实施方式
[0133]
车道保持辅助(lka，lane keeping assist)是应用于普通道路及高速公路等具有明确车道边界线车道上，辅助驾驶员保持车辆在车道内行驶的系统，该系统应具备车道偏离抑制或车道居中控制功能。当lka系统处于激活状态时，若车辆发生非驾驶意愿的车道偏离，lka系统应进行车辆横向运动控制以辅助驾驶员将车辆保持在原车道内行驶，从而修正行驶方向，避免异常驶出车道的情况，消除安全隐患，减少事故的发生。系统应用场景主要包括车道偏离抑制与车道居中控制两个方面，因而本发明对lka的两大应用的场景也进行了区分。
[0134]
目前，对于上述应用已有标准《乘用车车道保持辅助(lka)系统性能要求及试验方法》(gb/t 39323-2020)以及《汽车转向系基本要求》(gb/t 17675-2021)，但其仅列出要求，未给出车道线距离等测评指标的计算方法，也没有针对大型商用车的具体测评方法，本发明方法的目的在于针对包含牵引车和拖挂车的大型商用车，为上述应用提供一种车道线拟合方法并完善标准相应的测评方法：利用高精度差分gps以及车载组合导航分别从车道线以及测试车辆上获取测评所需基础参数数据，根据标准要求，结合牵引车和拖挂车的铰接特点，进行参数联合估计，在数据高精度处理的基础上，分析得到测评结果。本发明对上述标准进行补充，详细给出了测评过程中，评价车道保持行为的优劣所需的车道线方程、车轮边缘与车道线距离、铰接角、横摆稳定性、车道保持精度等参数的测评解算方法。
[0135]
本发明的目的是提出了一种面向商用车车道保持辅助系统的精准测评方法，包括车道线拟合部分和车道保持辅助测评部分；其中，车道线拟合部分，对道路进行分段建模，将交叉路口之间的路段且不包含交叉路口部分定义为独立的目标道路，使用三次kochanek-bartels样条插值算法作为单独的测评路段；车道保持辅助测评部分，大型商用车上布置三处天线即观测点，通过卡尔曼滤波进行参数联合估计，精准地获取牵引车和拖挂车在行驶过程中的基础参数信息，实现对车道线距离、铰接角、横摆稳定性、车道保持精度指标的测评，测评流程如图1所示，具体步骤包括：
[0136]
步骤一：建立面向车道保持辅助测试场景的道路模型，建立流程如图2所示；
[0137]
子步骤1：采集原始数据，利用高精度车载组合导航系统采集需要测评路段的车道
线轨迹数据信息；
[0138]
子步骤2：对采集到的道路信息进行投影变换，以交叉路口为端点对车道进行分段，再根据分段的车道为直道或弯道，将其细化为独立的车道数据块，利用坐标变换，将分段后数据的经纬度坐标转换为局部切平面直角坐标系坐标，转换后得到的某段独立道路全部轨迹数据为t
line
＝{t0(c0,z0),t1(c1,z1),
…
,tn(cn,zn)},其中，cn，zn，分别为道路轨迹点的北向位置坐标和东向位置坐标，n为轨迹数据点的个数；
[0139]
子步骤3：建立道路模型；利用kochanek-bartels样条曲线对子步骤2中每个道路数据块的轨迹数据进行插值，样条曲线受坐标集t
line
＝{t0(c0,z0),t1(c1,z1),
…
,tn(cn,zn)}中的控制点控制，每个控制点处的切线值由与该控制点相邻的轨迹控制点计算得出。独立的单侧车道数据块中，除去两端的控制点t0(c0,z0)、tn(c
n-1
,z
n-1
)、tn(cn,zn)后，对剩余的n-3个控制点，三次kochanek-bartels样条曲线可由任意4个连续控制点确定，设p(u)是两控制点tk和t
k+1
间的三次函数式，其中u为参变量，0≤u≤1，k＝2,3,
…
,n-2，则从t
k-1
到t
k+2
间的四个控制点用于建立拟合车道线的边界条件：
[0140]
p(0)＝tk[0141]
p(1)＝t
k+1
[0142][0143][0144]
式中p(0)和p(1)分别为p(u)在节点tk和t
k+1
之间曲线段两个端点的位置向量，p
′
(0)
in
和p
′
(1)
out
分别为曲线段在两端点处的切向量，曲线参变量u是在两个端点取值0和1之间变化。参数t是张量参数，控制拟合轨迹的松紧程度；参数b是偏离参数，调整曲线段在端点处弯曲的数值；参数c是连续性参数，控制切向量在曲线边界处的连续性。
[0145]
记两端点处切向量为和即：
[0146][0147][0148]
将上述边界条件转换成矩阵形式：
[0149][0150]
其中hermite矩阵是
[0151][0152]
将上述矩阵方程展开成多项式形式，可以有
[0153][0154]
经由上述过程，得到了独立的单侧车道数据t1(c1,z1)到t
n-2
(c
n-2
,z
n-2
)之间，kochanek-bartels样条插值的车道线拟合方程。
[0155]
对于单独测评路段，通过上述子步骤可以获得该路段车道线的拟合曲线c1、c2，分别对应目标路段的左右车道边界，另外定义车道中线为车道中央的曲线，该曲线上的点到左、右车道边界线的距离相等，一般作为车道保持跟踪目标曲线，由左右侧车道线的数据取均值得到。
[0156]
步骤二：测试过程中车辆运动参数的卡尔曼滤波联合估计
[0157]
为牵引车和拖挂车安装组合导航系统，如图3所示：对于拖挂车，沿其纵轴安装有两个天线(即主天线和从天线)的接收器用于测量主天线所在观测点的基础参数，包括位置、速度、加速度、航向角和横摆角速度；对于牵引车，只需在其纵轴上安装一个单天线接收器，由于单天线能记录的参数只有位置、速度、加速度，其航向角和横摆角速度需另外计算得出。组合导航系统的时间以北斗时间为参考基准，在选定测评路段上采集对应的基础参数以及对应的采样时刻；
[0158]
子步骤1：拖挂车基础参数卡尔曼滤波处理
[0159]
首先，定义拖挂车的系统位置状态向量为：
[0160]
x
1_p
＝[p
1_ev1_ea1_e
p
1_ev1_ea1_n
]
′
[0161]
其中，p
1_e
，v
1_e
，a
1_e
分别代表p1点(主天线)的东向位置、东向速度分量与东向加速度分量，同样的，p
1_n
，v
1_n
，a
1_n
分别代表其的北向位置、北向速度分量与北向加速度分量。选取gps接收机作为车辆运动的测量传感器，其输出的位置和速度作为系统的观测量。
[0162]
对应离散状态为：
[0163]
x
1_p
(k)＝φ
1_p
·
x
1_p
(k-1)+u
1_p
(k-1)+w
1_p
(k-1)
[0164]
其中k为离散时间间隔，φ
1_p
为位置状态转移矩阵，u
1_p
为位置输入向量，w
1_p
为位置噪声向量且
[0165][0166]
其中t为离散周期，和分别是在东向上与北向上的相关时间常数，和分别是当前东向上和北向上加速度分量均值。
[0167]
类似的，拖挂车的角度离散状态为
[0168]
x
1_φ
(k)＝φ
1_φ
·
x
1_φ
(k-1)+w
1_φ
(k-1)
[0169]
其中φ
1_φ
为角度状态转移矩阵，w
1_φ
为角度噪声向量且
[0170][0171]
其中φ1是拖挂车的朝向角，ω1是拖挂车的横摆角速度。
[0172]
结合位置与角度离散状态，整个拖挂车的卡尔曼滤波状态方程为：
[0173]
x1(k)＝φ1·
x1(k-1)+u1(k-1)+w1(k-1)
[0174]
其中
[0175]
拖挂车的观测信息来源于包含gps接收机与陀螺仪的组合导航系统，gps接收机可以输出观测点的位置、速度与方位角信息，对应观测方程为：
[0176]z1_gps
(k)＝h
1_gps
·
x
1_p
(k)+n
1_gps
(k)
[0177]
其中，z
1_gps
是两天线的测量信息，h
1_gps
是其观测矩阵，n
1_gps
是协方差矩阵为r
1_gps
的观测噪声向量，且：
[0178][0179]
其中，z
p1_e
，z
p1_n
，z
1_v
，z
1_φ
分别是天线测得的东向位置，北向位置，速度，航向角；n
p1_e
，n
p1_n
，n
1_v
，n
1_φ
分别是对应的观测白噪声。
[0180]
陀螺仪可以输出横摆角速度，观测方程为：
[0181]z1_gryo
(k)＝h
1_gryo
·
x
1_φ
(k)+n
1_gryo
(k)
[0182]
其中，z
1_gryo
(k)＝[z
1_ω
]是陀螺仪的横摆角速度测量值，h
1_gryo
＝[0 1]是其观测矩阵，n
1_gryo
是其观测白噪声。
[0183]
结合所有观测量，观测方程为：
[0184]
z1(k)＝h1·
x1(k)+n1(k)
[0185]
其中噪声向量n1的协方差矩阵是r1，且，且
[0186]
根据以上状态方程与观测方程，在k时刻拖挂车基础参数卡尔曼滤波处理过程如下：
[0187]
(1)状态量的先验估计
[0188][0189]
(2)计算先验误差协方差矩阵
[0190][0191]
其中q1为过程噪声的协方差矩阵。
[0192]
(3)计算卡尔曼增益
[0193][0194]
其中r1为测量噪声的协方差矩阵。
[0195]
(4)计算状态量后验估计
[0196][0197]
(5)更新误差协方差矩阵
[0198]
[0199]
子步骤2：牵引车基础参数卡尔曼滤波处理
[0200]
对于牵引车，只需对位置状态相关数据进行滤波处理，系统状态方程为：
[0201]
x2(k)＝φ2·
x2(k-1)+u2(k-1)+w2(k-1)
[0202]
其中，x2是系统状态向量，φ2是其状态转移矩阵，u2为系统输入，w2是噪声向量，且：
[0203]
φ2＝φ
1_p
，w2＝w
1_p
[0204]
其中，p
2_e
，v
2_e
，a
2_e
分别代表p2点(天线安装位置)东向位置、东向速度分量与东向加速度分量，同样的，p
2_n
，v
2_n
，a
2_n
分别代表其的北向位置、北向速度分量与北向加速度分量；和分别是当前东向上和北向上加速度分量均值。
[0205]
牵引车的观测信息来源于单天线gps接收机，输出位置，速度信息，对应的观测方程为：
[0206]
z2(k)＝h2·
x2(k)+n2(k)
[0207]
其中，z2是单天线的测量信息，g2是观测矩阵，n2是对应的测量噪声向量，且：
[0208][0209]
其中，z
p2_e
，z
p2_n
，z
p2_v
分别是天线测得的东向位置，北向位置，速度；n
p2_e
，n
p2_n
，n
2_v
分别是对应的观测白噪声。
[0210]
根据以上状态方程与观测方程，在k时刻牵引车基础参数卡尔曼滤波处理过程与拖挂车基础参数卡尔曼滤波处理过程类似，同样包括态量的先验估计、计算先验误差协方差矩阵、计算卡尔曼增益、计算状态量后验估计、更新误差协方差矩阵等步骤。
[0211]
子步骤3：牵引车航向角与横摆角速度估计
[0212]
牵引车的航向角与横摆角速度可由拖挂车滤波后的基础参数计算获得，如图3所示，根据二者几何约束可得：
[0213]
φ
2-α
2-β2＝π
[0214]
φ1+γ
1-α2＝π
[0215]
则牵引车的航向角为：
[0216]
φ2＝φ1+β2+γ1[0217]
其中，β2＝arccos(m)，＝arccos(m)，点o为铰接点。
[0218]
为计算横摆角速度，记则牵引车横摆角速度可由以下微分方程得出：
[0219]
[0220]
结合dz/dt＝((p
2_e-p
1_e
)(v
2_e-v
1_e
)+(p
2_e-p
1_e
)(v
2_e-v
1_e
))/z，可以得到
[0221][0222]
至此，测试过程中拖挂车与牵引车的主天线处(p1点和p2点)的位置、速度、加速度、航向角和横摆角速度均为已知。
[0223]
步骤三：车道保持辅助系统性能测评
[0224]
子步骤1：车辆与车道线的距离测评
[0225]
(1)特定点位置推算：对于车道保持辅助测评场景，测评过程中需要车辆某一特定点位置推算，以车道偏离的界定标准为例，车辆轮胎的外缘是否将要超出车道边线为判断偏离的标准，而组合导航系统记录的车辆位置实际为系统主天线的安装位置，因此需要先将记录的主天线运动轨迹转换成大型商用车特定轮胎外缘的运动轨迹，选定牵引车前轮外缘为特定点，如图4所示，推算过程如下：
[0226]
假定该时刻为tc，tc时刻对应的经纬度坐标为(xc,yc)，若组合导航信息的采集时刻没有的tc对应值，可采用线性插值计算得出经纬度坐标为(xc,yc)，xc为纬度、yc为经度，单位为度；其余测量参数也应进行插值处理。然后将其转换为高斯投影坐标(xu,yu)(东向位置、北向位置确定的平面投影坐标系)，单位为m；
[0227]
主天线安装在牵引车的纵轴上，与两前轮外缘处的纵向距离为d，横向距离为l，车辆航向角为φ2。局部切平面直角坐标系坐标下，左前轮外缘处的位置坐标为：
[0228][0229]
其中β＝π-α，d2＝d4
·
cosβ，d4＝d-d3，d3＝d1
·
cosβ。
[0230]
类似的，可以根据几何关系计算出右前轮外缘处的位置坐标pr。
[0231][0232]
对组合导航系统中记录的牵引车位置均作以上处理，即可得到任意时刻牵引车前轮外缘的运动轨迹；类似的，可以用相同的解算方法得出拖挂车的末端外边缘(或其他任意部位在地面的投影)所划过的轨迹，作为后续测评的数据基础。
[0233]
(2)距离计算：车道保持辅助测评过程中，车辆与车道线的距离为车辆特定部位与车道边界之间的横向距离，由于大型商用车的铰接特性，通常牵引车与拖挂车在此步骤中应视为单独的测评对象分别进行距离测评。以两前轮外缘位置为例，分别计算与两侧车道边界的距离，假定发生左偏，需计算牵引车左前车轮外缘与左侧车道线的距离，计算过程如下：
[0234]
假定该时刻仍为tc，局部切平面直角坐标系坐标左前轮外缘处的位置坐标已知，
为p
l
；左侧车道线拟合c1上，p
l
位于控制点tc和t
c+1
之间确定路段的横向区域内，该路段对应拟合曲线：
[0235][0236]
其本质上对应一个关于u∈[0,1]的多项式：
[0237][0238]
考虑到拟合实际车道线的情况下，两控制点tc和t
c+1
之间拟合曲线斜率变化不大且较平缓，用点p
l
到三次曲线p(u)的最短距离代替横向距离，记为表示车辆上左前轮外缘点p
l
与车道线的距离；特别的，当车辆轮胎外缘位于车道边界线外侧时，取负值。
[0239]
子步骤2：铰接角测评
[0240]
铰接角是指行驶过程中牵引车和拖挂车轴线之间出现的相对夹角，计算方法如下：
[0241]
δ＝φ
1-φ2[0242]
其中φ1表示拖挂车航向角，φ2表示牵引车的航向角。
[0243]
子步骤3：横摆稳定性测评
[0244]
横摆稳定性是指汽车车身坐标系垂向轴偏转大小的程度，综合考虑牵引车和挂车的横摆角速度，横摆稳定性测评需利用牵引车和拖挂车的速度、横摆角速度以及对应车道的曲率数据，以评价lka的控制稳定程度，计算方法如下：
[0245][0246]
其中η表示大型商用车横摆稳定性的量化值，sum为一次测试中横摆角速度数据的数量，由于牵引车横摆角速度由拖挂车横摆角速度等数据推算所得，同一次测试中二者数据量相同；η1表示拖挂车横摆稳定性，η2表示牵引车横摆稳定性，计算方法如下：
[0247][0248]
ω
t
表示t时刻的车辆横摆角速度，t时刻横摆角速度期望值ωe＝v
t
/r
l
，其中v
t
是t时刻车辆速度，r
l
是道路曲率半径，即车辆与车道线距离测评中迭代出的插值点处的曲率半径。
[0249]
子步骤4：车道保持精度测评
[0250]
车道保持精度是指车道保持辅助测评过程中，车辆纵轴与车道中线距离的均方差。大型商用车由于其铰接特性，应分别考虑牵引车和拖挂车的车道保持精度，以综合评价大型商用车的车道保持精度，计算方法如下：
[0251][0252]
其中，ε表示车道保持精度量化值；ε1表示拖挂车车道保持精度，ε2表示牵引车车道保持精度，计算方法如下：
[0253]
[0254]
其中，sumi为测试过程中位置数据的数量，d
it
表示t时刻牵引车或拖挂车主天线与车道中线的距离(主天线安装在牵引车和拖挂车的纵轴上)，如图5所示，可由子步骤1中的距离计算方法获得。i＝1表示拖挂车的相关参数，i＝2表示牵引车的相关参数。