CN110987463B - 面向多场景的智能驾驶自主车道变换性能测试方法 - Google Patents

面向多场景的智能驾驶自主车道变换性能测试方法 Download PDF

Info

Publication number
CN110987463B
CN110987463B CN201911086483.0A CN201911086483A CN110987463B CN 110987463 B CN110987463 B CN 110987463B CN 201911086483 A CN201911086483 A CN 201911086483A CN 110987463 B CN110987463 B CN 110987463B
Authority
CN
China
Prior art keywords
vehicle
lane change
intelligent driving
formula
observation
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Active
Application number
CN201911086483.0A
Other languages
English (en)
Other versions
CN110987463A (zh
Inventor
李旭
胡玮明
徐启敏
胡锦超
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Southeast University
Original Assignee
Southeast University
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Southeast University filed Critical Southeast University
Priority to CN201911086483.0A priority Critical patent/CN110987463B/zh
Publication of CN110987463A publication Critical patent/CN110987463A/zh
Application granted granted Critical
Publication of CN110987463B publication Critical patent/CN110987463B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01M—TESTING STATIC OR DYNAMIC BALANCE OF MACHINES OR STRUCTURES; TESTING OF STRUCTURES OR APPARATUS, NOT OTHERWISE PROVIDED FOR
    • G01M17/00—Testing of vehicles
    • G01M17/007—Wheeled or endless-tracked vehicles
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01M—TESTING STATIC OR DYNAMIC BALANCE OF MACHINES OR STRUCTURES; TESTING OF STRUCTURES OR APPARATUS, NOT OTHERWISE PROVIDED FOR
    • G01M17/00—Testing of vehicles
    • G01M17/007—Wheeled or endless-tracked vehicles
    • G01M17/06—Steering behaviour; Rolling behaviour

Landscapes

  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Traffic Control Systems (AREA)

Abstract

本发明公开了一种面向多场景的智能驾驶自主车道变换性能测试方法。本方法首先根据自主车道变换过程中的运动特点，建立了基于运动学自行车模型的车道变换动态模型。其次，利用改进的无迹卡尔曼滤波算法对车辆位置、速度、方位角等状态变量进行滤波估计。最后，基于准确递推的车辆关键性基础性能参数，构建变道性能评价指标体系，量化并输出自主车道变换性能的评价指标：目标间隙、距离碰撞时间和并线横摆稳定性，从而实现智能驾驶自主车道变换性能优劣的高精度、高频率测量和科学定量评价。

Description

面向多场景的智能驾驶自主车道变换性能测试方法
技术领域
本发明涉及智能驾驶技术领域，尤其涉及一种面向多场景的智能驾驶自主车道变换性能测试方法。
背景技术
随着我国经济的不断发展，交通运输发展面临着既要提供覆盖面更广的均等化公共服务，又要保障道路交通安全、高效的双重压力。据统计，因汽车偏离正常行驶车道造成的交通事故占全部交通事故的比例高达百分之五十。为有效减少因变道不合时宜导致的交通事故的发生，智能驾驶自主车道变换(并线行驶)功能成为了智能驾驶技术的重要组成部分，其性能的优劣是保障智能驾驶汽车安全上路的前提。因此，对自主车道变换性能进行科学、合理的测评对提高道路交通驾驶安全具有重要意义，同时也是我国交通运输部发布的智能网联汽车自动驾驶测试中的必测项目。
ISO 17387-2008标准《智能运输系统—变道决策辅助系统—性能要求和试验过程》对变道决策辅助系统的性能要求和试验过程做出了明确规范，但该标准仅从安全角度出发，针对变道操作时的安全情况进行评估与警示，未针对变道时机等因素对车道变换性能做定量评价。工信部联装[2018]66号《智能网联汽车道路测试管理规范(试行)》对智能网联汽车道路测试中测试主体、测试驾驶人、测试车辆的基本要求做出了相关规定，但未涉及包括自主车道变换在内的各智能驾驶功能性能的测试方法和测试规程。具体针对智能驾驶自主车道变换性能的定量测试方法和试验要求，目前仍缺乏完善的测评规程和统一的国家标准。
目前，已有专利文献对智能驾驶高级驾驶辅助系统(Advanced DrivingAssistant System,ADAS)功能及其性能的测试评价进行研究，从测试方法角度，智能驾驶测评主要包括基于CarMaker/Prescan和Simulink的软件在环仿真测试、基于硬件在环测试台架的检测方法和基于实际道路试验的检测方法。虚拟仿真测试和硬件在环测试，多用于测试评判智能驾驶汽车的功能有无，在动力学模型不精确的情况下，难以对其性能进行定量测试，无法满足智能驾驶测试需求。而基于实际道路试验的检测方法是美欧日等国家和地区从技术发展和管理角度采取的普遍做法，其检验结果更具说服力。然而，针对智能驾驶自主车道变换性能的测试，目前尚缺乏基于实际道路试验的明确统一的测试方法和评价指标，特别是缺少量化自主车道变换性能指标的测试研究。
发明内容
发明目的：为了全面、准确、高频的获取变道过程中的车辆运动参数，量化并输出自主车道变换性能的评价指标，实现智能驾驶自主车道变换性能的可靠测试，以填补实际应用中自主车道变换性能测试技术的空白，本发明公开了一种面向多场景的智能驾驶自主车道变换性能测试方法。该测试方法基于实际道路试验，测量精度、测量频率高且允许离线处理，可面向开放道路场景、法规标准场景等多类测试场景以及夜晚、阴雨天等复杂工况进行测试。
技术方案：为实现本发明的目的，本发明所采用的技术方案是：一种面向多场景的智能驾驶自主车道变换性能测试方法。本方法首先根据自主车道变换过程中的运动特点，建立了基于运动学自行车模型的车道变换动态模型。其次，利用改进的无迹卡尔曼滤波算法对车辆位置、速度、方位角等状态变量进行滤波估计。最后，基于准确递推的车辆关键性基础性能参数，构建变道性能评价指标体系，量化并输出自主车道变换性能的评价指标：目标间隙、距离碰撞时间和并线横摆稳定性，从而实现智能驾驶自主车道变换性能优劣的高精度、高频率测量和科学定量评价。包括以下步骤：
步骤一：建立智能驾驶汽车自主车道变换的动态模型
在智能驾驶自主车道变换性能测试过程中，需要高频且精确的获取每一时刻被测车辆的位置、速度、方位角等众多信息并进行存储。为了满足信息全、精度和频率高的测量需求，建立能够准确描述智能驾驶汽车变道运动特性的动态模型。针对本发明的应用领域，本发明对于行驶在通常道路交通环境下的前轮转向的四轮车辆，做出以下合理假定：
1)忽略智能驾驶汽车在垂直方向的运动，将智能驾驶汽车在自主车道变换过程中的运动参数简化到二维平面内；
2)假定汽车前轴的两个轮胎具有相同的转向角和转速，类似地，假定汽车后轴的两个轮胎具有相同的转向角和转速；
3)假定车辆前轮的方向与车辆当前速度方向一致。
根据以上要求和假定，对汽车进行适当的运动学建模。本发明选取运动学自行车模型，建立如图2所示的自主车道变换过程的动态模型。
图2中定义了车辆所在的平面坐标系，其中，G点为车辆的质心点，将前轴的左侧车轮和右侧车轮合并为一个点，位于B点；将后轴的左侧车轮和右侧车轮合并为一个点，位于A点。O点为线段OA、OB的交点，是车辆的瞬时滚动中心，线段OA、OB分别垂直于两个滚动轮的方向。根据图2，车辆的运动学模型可描述为：
Figure GDA0002719703940000031
式(1)中，上标“'”表示微分，如m'表示m的微分，m,n分别为智能驾驶汽车的东向位置分量和北向位置分量，δ,β分别为汽车的方向盘转角和质心侧偏角，ve,vn分别为东向速度分量和北向速度分量，ae,an分别为东向加速度分量和北向加速度分量，v为车辆的对地平面速度且
Figure GDA0002719703940000032
ρ为转向系的传动比，本发明中取ρ＝10，lr,lf分别表示车辆前轮、后轮到车辆质心的距离。
Figure GDA0002719703940000033
为汽车的方位角，在本发明中，方位角是指汽车中间轴线方向与正北方向的夹角，具体含义如图3所示，即：
1)当汽车运动方向如S1时，相应的方位角区间为(0,π/2)；
2)当汽车运动方向如S2时，相应的方位角区间为(-π/2,0)；
3)当汽车运动方向如S3时，相应的方位角区间为(-π,-π/2)；
4)当汽车运动方向如S4时，相应的方位角区间为(π/2,π)。
根据图3可知，方位角
Figure GDA0002719703940000034
与东向速度ve、北向速度vn满足以下关系：
Figure GDA0002719703940000041
对于智能驾驶汽车的自主车道变换过程，取系统状态向量
Figure GDA0002719703940000042
本发明中矩阵上角标T表示对矩阵转置，T表示离散的周期。根据式(1)描述的运动学模型，建立系统状态方程：
Figure GDA0002719703940000043
式(3)中，f(·)为5维系统状态函数向量函数，W表示零均值的系统高斯白噪声向量，γ表示系统外输入对应的零均值高斯白噪声，U表示系统外部输入向量且U＝[ae an δ]T，其中，东向加速度分量ae和北向加速度分量an通过惯性测量单元获取，方向盘转角δ通过汽车CAN总线信息获取。
步骤二：基于改进无迹卡尔曼滤波的车辆运动状态估计
为推算智能驾驶自主车道变换过程中的系统各状态变量
Figure GDA0002719703940000044
可采用滤波递推估计的方法，利用较少的系统观测量实现多维度的参数递推。为处理步骤一中描述的非线性系统(式(3)描述的系统状态方程)，采用无迹卡尔曼滤波器，对车辆运动状态参数进行递推估计。
本发明选择厘米级高精度差分GPS作为车辆运动的测量传感器，以对地平面速度、东向位置分量和北向位置分量作为系统观测向量，则系统的观测方程可表示为：
Z(t)＝h(X(t),V(t)) (4)
式(4)中，h为观测方程，t表示时间，系统观测向量Z＝[mg ng vg]T，其中，mg,ng分别表示东向位置分量和北向位置分量的观测值，由厘米级高精度差分GPS采集的经纬度坐标转换得到；vg表示厘米级高精度差分GPS输出的对地平面速度，且满足
Figure GDA0002719703940000051
系统观测噪声V表示与系统过程噪声W互不相关的零均值高斯白噪声向量，X(t)表示t对应的X，V(t)表示t对应的V，Z(t)表示t对应的Z。
在实际的无迹卡尔曼滤波递推过程中，需要采用离散化的无迹卡尔曼滤波模型。为此，对系统状态方程(3)和观测方程(4)进行离散化处理，离散化后的系统状态方程和观测方程分别为：
Figure GDA0002719703940000052
式(5)中，k表示离散化时刻，系统过程噪声W＝[w1 w2 w3 w4 w5]T，其中，w1,w2,w3,w4,w5分别表示五个系统高斯白噪声分量，W(k-1)对应的系统高斯白噪声协方差阵
Figure GDA0002719703940000053
其中
Figure GDA0002719703940000054
分别表示系统高斯白噪声w1,w2,w3,w4,w5对应的方差。系统观测噪声V＝[v1 v2 v3]T，其中，v1,v2,v3分别表示两个系统高斯白噪声分量，V(k)对应的测量高斯白噪声协方差阵
Figure GDA0002719703940000055
其中，
Figure GDA0002719703940000056
分别表示系统高斯白噪声v1,v2,v3对应的方差，
Figure GDA0002719703940000057
可根据厘米级高精度差分GPS的位置测量噪声的统计特性来确定，
Figure GDA0002719703940000058
可根据厘米级高精度差分GPS的速度测量噪声的统计特性来确定。系统外输入噪声
Figure GDA0002719703940000059
其中，
Figure GDA00027197039400000510
wδ分别表示直测的系统外输入ae,an,δ对应的零均值高斯白噪声分量，这些白噪声隐含在系统状态函数f的三个系统外输入中。
观测方程h(X(k),V(k))和系统状态函数向量f(X,U,W,γ)分别为：
Figure GDA00027197039400000511
Figure GDA0002719703940000061
其中，
Figure GDA0002719703940000069
Figure GDA00027197039400000610
Figure GDA0002719703940000064
f4(X(k-1),U(k-1),W(k-1),γ(k-1))＝ve(k-1)+ae(k-1)·T+w4
f5(X(k-1),U(k-1),W(k-1),γ(k-1))＝vn(k-1)+an(k-1)·T+w5
根据公式(5)描述的系统状态方程和观测方程，建立无迹卡尔曼滤波的递推过程，通过时间更新和测量更新进行滤波递推：
1)对输入变量进行初始化并进行参数计算
Figure GDA0002719703940000065
式(6)中，X0为输入变量的初始值，
Figure GDA0002719703940000066
为X0的滤波估计值，本发明中，带有上标符号∧的变量均表示该变量的滤波估计值，P0为初始误差方差矩阵，为保证滤波算法收敛，本发明中，取P0为单位矩阵。
2)状态估计
计算Sigma点ξi(k-1)，并确定权重系数
Figure GDA0002719703940000067
Figure GDA0002719703940000068
式(7)中，
Figure GDA0002719703940000071
为加权协方差矩阵平方根的第i列，r为状态向量的维数，本发明中取r＝5。
Figure GDA0002719703940000072
式(8)中，λ表示调节Sigma点和
Figure GDA0002719703940000073
的距离参数，λ＝α2(r+κ)-r，α表示决定先验均值附近Sigma点分布广度的主要刻度因数，ε为用来强调后验协方差计算的零阶Sigma点权值的第二刻度因数，本发明中取ε＝2，
Figure GDA0002719703940000074
分别表示均值和方差的权重系数，κ表示第三刻度因数，本发明中取κ＝0。
3)时间更新方程计算
利用系统状态方程的传播计算Sigma点ξi(k,k-1)，系统状态方程对Sigma点做非线性变换：
ξi(k,k-1)＝f(ξi(k,k-1)),i＝0,1,...,2r (9)
计算函数映射后的均值
Figure GDA0002719703940000075
和方差P(k,k-1)，
Figure GDA0002719703940000076
Figure GDA0002719703940000077
4)观测更新方程计算
利用观测方程对变换后的Sigma点集进行变换，
χi(k,k-1)＝h(ξi(k,k-1)) (12)
式(12)中，χi(k,k-1)表示观测方程对Sigma点集变换后的值，
计算一步预测观测值，
Figure GDA0002719703940000078
式(13)中，
Figure GDA0002719703940000079
表示由k-1时刻递推的k时刻的一步预测观测值，
输出预测值协方差
Figure GDA00027197039400000710
Figure GDA0002719703940000081
计算状态值与测量值的协方差PXZ，
Figure GDA0002719703940000082
5)滤波更新
计算无迹卡尔曼滤波增益矩阵K(k)，
Figure GDA0002719703940000083
计算更新后的状态量估计值
Figure GDA0002719703940000084
和估计误差方差阵P(k)，
Figure GDA0002719703940000085
Figure GDA0002719703940000086
在实际测试过程中，不同智能驾驶车辆的运动特性存在差异，使得各被测车辆的初始状态无法保持一致。UKF算法虽然能够克服EKF等非线性滤波器存在的一些问题，但仍存在着跟踪能力较差、对滤波初值较敏感等不足。针对以上问题，引入自适应因子对UKF算法进行改进，以减小滤波初始值对系统状态方程和观测方程的影响，提高系统状态估计的精度。
引入自适应因子θk，利用改进的观测更新调整系统模型扰动对滤波递推的影响，将式(14)(15)(18)分别改写为：
Figure GDA0002719703940000087
Figure GDA0002719703940000088
Figure GDA0002719703940000089
式(19)中，R(k)表示k时刻的测量高斯白噪声协方差阵；
自适应因子θk的取值为：
Figure GDA0002719703940000091
式(22)中，预测误差向量
Figure GDA0002719703940000092
tr(g)表示计算矩阵的迹。
标准的无迹卡尔曼滤波递推的周期与观测向量的数据更新周期相同，在本发明中，由于厘米级高精度GPS的信息采集频率相对较低，观测向量的更新周期相对较长，若采用标准的滤波算法，将导致系统的输出频率较低。为保证系统具有较高的数据输出频率，满足高频率的测量要求，继续对UKF算法进行改进，即降低时间更新的周期，保持观测更新的周期不变，同时满足观测更新的周期是时间更新周期的整数倍(该整数可取20、10、5等)。
若厘米级高精度GPS数据的更新频率为TGPS，时间更新的周期T(即离散的周期T，该值可取0.01s、0.02s、0.05s等)应满足
Figure GDA0002719703940000093
其中，q表示正整数。因此，最终改进的无迹卡尔曼滤波过程可描述为：
1)当满足
Figure GDA0002719703940000094
时，滤波过程进行标准的无迹卡尔曼滤波递推过程，首先，利用式(9)～式(11)进行时间更新过程。其次，依次利用式(12)～式(16)、式(19)、式(20)进行观测更新过程。最后，利用式(17)、式(21)分别计算更新后的状态量估计值和估计误差方差阵。
2)当
Figure GDA0002719703940000095
时，首先，进行无迹卡尔曼滤波时间更新过程：
Figure GDA0002719703940000096
式(23)中，
Figure GDA0002719703940000097
表示权重系数，r表示状态向量的维数；
其次，计算更新后的状态量估计值
Figure GDA0002719703940000098
和估计误差方差阵P(k)：
Figure GDA0002719703940000099
P(k)＝P(k,k-1) (25)
上述算法的计算降低了滤波递推的周期，提高了系统状态变量的输出频率(根据传感器特性，TGPS＝0.05s，假设离散周期T＝0.01s，利用改进的无迹卡尔曼滤波进行滤波递推，系统输出频率则由20Hz提高到了100Hz)。
经过上述改进后的滤波递推计算，输出的状态估计
Figure GDA0002719703940000101
即为离散时刻k的车辆状态向量估计值，由此可以准确、高频的估计出智能驾驶汽车在自主车道变换过程中每个时刻的东向位置、北向位置、东向速度、北向速度等信息。
步骤三：量化自主车道变换性能的评价指标
基于步骤二输出的车辆关键性基础性能参数，构建变道性能评价指标体系，设置多尺度的自主车道变换性能评价指标：目标间隙、距离碰撞时间和并线横摆稳定性，并对以上指标进行量化，对自主车道变换性能进行多维度定量测评。
首先，对涉及的名词做如下定义和解释：
1)自车是指具备自主车道变换能力的被测车辆；
2)目标车道是指自车执行变道后将要到达的车道；目标车辆是指在自车前方行驶轨迹线上，距离自车最近的车辆，该车可能位于自车所在的车道前方或目标车道；
3)目标间隙是指自车与目标车辆的欧式距离；
4)距离碰撞时间是指在保持相对车速不变的情况下，自车与目标车辆发生碰撞所需的时间；
5)并线横摆稳定性是指在进行自主车道变换过程中，汽车绕车身坐标系垂向轴偏转大小的程度。
其次，根据上述指标的含义，基于步骤二输出的车辆位置、速度等运动学参数，根据式(26)、(27)、(28)分别推算汽车的目标间隙、距离碰撞时间、并线横摆稳定性，具体地：
将目标间隙定义为：
Figure GDA0002719703940000102
式(26)中，Lp表示目标间隙，单位为米，用以表征智能驾驶汽车实施变道的安全性，当目标间隙小于最小安全车距时，智能驾驶汽车若进行自主车道变换操作，将发生碰撞。msub,nsub分别为自车的东向位置分量和北向位置分量，
Figure GDA0002719703940000103
分别为目标车辆的东向位置分量和北向位置分量，位置信息可由步骤二输出得到，μp为目标间隙计算常数，通过测量车辆外廓到质心的距离获得。p表示目标车辆所在的车道，其中，1表示目标车辆位于自车所在的车道，2表示目标车辆位于目标车道，对应地，L1,L2分别表示自车与当前车道上目标车辆的间隙、自车与目标车道上目标车辆的间隙，单位为米。
借鉴车辆自动紧急制动系统(Automatic Emergency Braking System,AEBS)和前向碰撞预警系统(Forward Collision Warning System,FCWS)性能测试中的“距离碰撞时间”性能评价指标，用以描述智能驾驶汽车实施变道操作的安全性和选择变道时机的适宜性，在本发明中，利用两车的目标间隙和接近速度对距离碰撞时间进行估计，距离碰撞时间可表示为：
Figure GDA0002719703940000111
式(27)中，TTC表示距离碰撞时间，单位为秒，vcs表示接近速度，单位为米每秒，接近速度等于自车与目标车辆的速度之差，当计算结果为负值时，表明在上述条件下，碰撞不可能发生。
将并线横摆稳定性定义为：
Figure GDA0002719703940000112
式(28)中，σMSE表示并线横摆稳定性的量化值，反映了执行自主车道变换的激进程度，s为横摆角速度数据的数量，
Figure GDA0002719703940000113
为k时刻车辆的横摆角速度，可通过惯性测量单元获得，k时刻横摆角速度的期望值
Figure GDA0002719703940000114
σMSE,
Figure GDA0002719703940000115
的单位均为弧度每秒，曲率半径
Figure GDA0002719703940000116
式中变量B1,B2,B3,B4分别为：
Figure GDA0002719703940000117
式(29)中，
Figure GDA0002719703940000121
分别表示由步骤二输出的k时刻的东向速度和北向速度，mk,nk分别表示由步骤二输出的k时刻的东向位置和北向位置。
当进行自主车道变换性能测试时，首先，根据步骤一、步骤二对系统各状态变量进行滤波递推。其次，基于高频、精确输出的车辆运动状态参数，根据步骤三计算目标间隙、距离碰撞时间、并线横摆稳定性三个评价指标的量化值。最后，通过定量评价的方式分析智能驾驶汽车实施变道的合理性、选择变道时机的适宜性、变道过程的激进程度和安全性，从而实现了智能驾驶自主车道变换性能优劣的科学定量评价。
有益效果：与现有技术相比，本发明的技术方案具有以下有益技术效果：
(1)通过本发明提出的测试方法和构建的变道性能评价指标体系，实现了智能驾驶自主车道变换性能优劣的高精度测评。
(2)相比于虚拟仿真测试和硬件在环测试，本发明提出的基于实际道路和真实交通环境下的自主车道变换性能测试方法，能够保障智能驾驶汽车在各种道路交通状况下运行的安全性和可靠性，更具有准确性和说服力。
(3)自主车道变换性能指标的测量精度和频率高，位置测量精度可达0.0351m(RMS)，测量频率为100Hz，准确性和实时性能够满足变道性能高动态和高精度的测试要求。
(4)环境适应性强，在夜晚、阴雨天等复杂测试环境下，本发明能够完成与平坦、干燥路面同等精度的自主车道变换性能测试，同时适用于开放道路场景、法规标准场景等多类测试场景。
附图说明
图1是本发明的技术路线示意图；
图2是车辆的运动学模型示意图；
图3是车辆方位角的含义与取值范围的示意图；
图4是某次人工模拟实现的自主车道变换性能测试试验的车辆运动轨迹示意图；
图5是图4的局部放大图；
图6是整个试验过程的欧式距离误差相对于时间的变化曲线图。
具体实施方式
下面结合附图和实施例对本发明的技术方案作进一步的说明。
随着我国经济的不断发展，交通运输发展面临着既要提供覆盖面更广的均等化公共服务，又要保障道路交通安全、高效的双重压力。在这样的背景下，面向L1-L5级的智能驾驶技术成为近年来研究的热点之一，为保障智能驾驶汽车的安全性、敏捷性和智能性，公共道路测试是智能驾驶技术研发和应用过程不可或缺的步骤。然而，目前国内外缺乏明确统一的针对智能驾驶相关产品准入认证的集成测评技术与测试标准规范，智能驾驶乃至无人驾驶应用落地迫切需要研究测评技术，形成覆盖不同等级智能驾驶的测评体系。
据统计，因汽车偏离正常行驶车道造成的交通事故占全部交通事故的比例高达百分之五十。为有效减少因变道不合时宜导致的交通事故的发生，智能驾驶自主车道变换(并线行驶)功能成为了智能驾驶技术的重要组成部分，其性能的优劣是保障智能驾驶汽车安全上路的前提。因此，对自主车道变换性能进行科学、合理的测评对提高道路交通驾驶安全具有重要意义，同时也是我国交通运输部发布的智能网联汽车自动驾驶测试中的必测项目。
ISO 17387-2008标准《智能运输系统—变道决策辅助系统—性能要求和试验过程》对变道决策辅助系统的性能要求和试验过程做出了明确规范，但该标准仅从安全角度出发，针对变道操作时的安全情况进行评估与警示，未针对变道时机等因素对车道变换性能做定量评价。工信部联装[2018]66号《智能网联汽车道路测试管理规范(试行)》对智能网联汽车道路测试中测试主体、测试驾驶人、测试车辆的基本要求做出了相关规定，但未涉及包括自主车道变换在内的各智能驾驶功能性能的测试方法和测试规程。具体针对智能驾驶自主车道变换性能的定量测试方法和试验要求，目前仍缺乏完善的测评规程和统一的国家标准。
目前，已有专利文献对智能驾驶高级驾驶辅助系统(Advanced DrivingAssistant System,ADAS)功能及其性能的测试评价进行研究，从测试方法角度，智能驾驶测评主要包括基于CarMaker/Prescan和Simulink的软件在环仿真测试、基于硬件在环测试台架的检测方法和基于实际道路试验的检测方法。虚拟仿真测试和硬件在环测试，多用于测试评判智能驾驶汽车的功能有无，在动力学模型不精确的情况下，难以对其性能进行定量测试，无法满足智能驾驶测试需求。而基于实际道路试验的检测方法是美欧日等国家和地区从技术发展和管理角度采取的普遍做法，其检验结果更具说服力。然而，针对智能驾驶自主车道变换性能的测试，目前尚缺乏基于实际道路试验的明确统一的测试方法和评价指标，特别是缺少量化自主车道变换性能指标的测试研究。
为了全面、准确、高频的获取变道过程中的车辆运动参数，量化并输出自主车道变换性能的评价指标，实现智能驾驶自主车道变换性能的可靠测试，以填补实际应用中自主车道变换性能测试技术的空白，本发明提出了一种面向开放道路场景、法规标准场景等多类测试场景的智能驾驶自主车道变换性能测试方法。本方法首先根据自主车道变换过程中的运动特点，建立了基于运动学自行车模型的车道变换动态模型。其次，利用改进的无迹卡尔曼滤波算法对车辆位置、速度、方位角等状态变量进行滤波估计。最后，基于准确递推的车辆关键性基础性能参数，构建变道性能评价指标体系，量化并输出自主车道变换性能的评价指标：目标间隙、距离碰撞时间和并线横摆稳定性，从而实现智能驾驶自主车道变换性能优劣的高精度、高频率测量和科学定量评价。本发明的技术路线如图1所示，具体步骤如下：
步骤一：建立智能驾驶汽车自主车道变换的动态模型
在智能驾驶自主车道变换性能测试过程中，需要高频且精确的获取每一时刻被测车辆的位置、速度、方位角等众多信息并进行存储。为了满足信息全、精度和频率高的测量需求，建立能够准确描述智能驾驶汽车变道运动特性的动态模型。针对本发明的应用领域，本发明对于行驶在通常道路交通环境下的前轮转向的四轮车辆，做出以下合理假定：
1)忽略智能驾驶汽车在垂直方向的运动，将智能驾驶汽车在自主车道变换过程中的运动参数简化到二维平面内；
2)假定汽车前轴的两个轮胎具有相同的转向角和转速，类似地，假定汽车后轴的两个轮胎具有相同的转向角和转速；
3)假定车辆前轮的方向与车辆当前速度方向一致。
根据以上要求和假定，对汽车进行适当的运动学建模。常用的动载体动态模型包括恒定速度模型(Constant Velocity,CV)、恒定加速度模型(Constant Acceleration,CA)、恒定转率和速度模型(Constant Turn Rate and Velocity，CTRV)、恒定转率和加速度模型(Constant Turn Rate and Acceleration，CTRA)和基于自行车模型的运动学模型等。其中，CA模型和CT模型假定动载体在作匀速或匀加速直线运动，且未考虑动载体的转向过程。CTRV模型和CTRA模型假定动载体具有恒定的转向角度，无法准确描述智能驾驶汽车自主车道变换的运动过程。
运动学自行车模型在对车辆运动进行有效简化的同时，充分考虑了车辆运动的转向特性和加速度特性，能够较为真实的反映车辆的实际运动情况，更能准确的描述自主车道变换的运动过程。因此，本发明选取运动学自行车模型，建立如图2所示的自主车道变换过程的动态模型。
图2中定义了车辆所在的平面坐标系，其中，G点为车辆的质心点，将前轴的左侧车轮和右侧车轮合并为一个点，位于B点；将后轴的左侧车轮和右侧车轮合并为一个点，位于A点。O点为线段OA、OB的交点，是车辆的瞬时滚动中心，线段OA、OB分别垂直于两个滚动轮的方向。根据图2，车辆的运动学模型可描述为：
Figure GDA0002719703940000151
式(1)中，上标“'”表示微分，如m'表示m的微分，m,n分别为智能驾驶汽车的东向位置分量和北向位置分量，δ,β分别为汽车的方向盘转角和质心侧偏角，ve,vn分别为东向速度分量和北向速度分量，ae,an分别为东向加速度分量和北向加速度分量，v为车辆的对地平面速度且
Figure GDA0002719703940000152
ρ为转向系的传动比，本发明中取ρ＝10，lr,lf分别表示车辆前轮、后轮到车辆质心的距离。
Figure GDA0002719703940000153
为汽车的方位角，在本发明中，方位角是指汽车中间轴线方向与正北方向的夹角，具体含义如图3所示，即：
1)当汽车运动方向如S1时，相应的方位角区间为(0,π/2)；
2)当汽车运动方向如S2时，相应的方位角区间为(-π/2,0)；
3)当汽车运动方向如S3时，相应的方位角区间为(-π,-π/2)；
4)当汽车运动方向如S4时，相应的方位角区间为(π/2,π)。
根据图3可知，方位角
Figure GDA0002719703940000154
与东向速度ve、北向速度vn满足以下关系：
Figure GDA0002719703940000161
对于智能驾驶汽车的自主车道变换过程，取系统状态向量
Figure GDA0002719703940000162
本发明中矩阵上角标T表示对矩阵转置，T表示离散的周期。根据式(1)描述的运动学模型，建立系统状态方程：
Figure GDA0002719703940000163
式(3)中，f(·)为5维系统状态函数向量函数，W表示零均值的系统高斯白噪声向量，γ表示系统外输入对应的零均值高斯白噪声，U表示系统外部输入向量，且U＝[ae an δ]T，其中，东向加速度分量ae和北向加速度分量an通过惯性测量单元获取，方向盘转角δ通过汽车CAN总线信息获取。
步骤二：基于改进无迹卡尔曼滤波的车辆运动状态估计
为推算智能驾驶自主车道变换过程中的系统各状态变量
Figure GDA0002719703940000164
可采用滤波递推估计的方法，利用较少的系统观测量实现多维度的参数递推。为处理步骤一中描述的非线性系统(式(3)描述的系统状态方程)，需采用非线性卡尔曼滤波器。
在常用的非线性卡尔曼滤波器中，粒子滤波(Particle Filter,PF)计算复杂度较高，若减少粒子数量，估计精度将会下降。扩展卡尔曼滤波算法(Extended Kalman Filter,EKF)引入线性化误差，对于模型复杂的系统，容易导致滤波效果降低。无迹卡尔曼滤波(Unscented Kalman Filter,UKF)在计算复杂度上与扩展卡尔曼滤波同阶次，但参数估计精度高于扩展卡尔曼滤波。因此，采用无迹卡尔曼滤波器，对车辆运动状态参数进行递推估计。
本发明选择厘米级高精度差分GPS作为车辆运动的测量传感器，以对地平面速度、东向位置分量和北向位置分量作为系统观测向量，则系统的观测方程可表示为：
Z(t)＝h(X(t),V(t)) (4)
式(4)中，h为观测方程，t表示时间，系统观测向量Z＝[mg ng vg]T，其中，mg,ng分别表示东向位置分量和北向位置分量的观测值，由厘米级高精度差分GPS采集的经纬度坐标转换得到；vg表示厘米级高精度差分GPS输出的对地平面速度，且满足
Figure GDA0002719703940000171
系统观测噪声V表示与系统过程噪声W互不相关的零均值高斯白噪声向量，X(t)表示t对应的X，V(t)表示t对应的V，Z(t)表示t对应的Z。
在实际的无迹卡尔曼滤波递推过程中，需要采用离散化的无迹卡尔曼滤波模型。为此，对系统状态方程(3)和观测方程(4)进行离散化处理，离散化后的系统状态方程和观测方程分别为：
Figure GDA0002719703940000172
式(5)中，k表示离散化时刻，系统过程噪声W＝[w1 w2 w3 w4 w5]T，其中，w1,w2,w3,w4,w5分别表示五个系统高斯白噪声分量，W(k-1)对应的系统高斯白噪声协方差阵
Figure GDA0002719703940000173
其中
Figure GDA0002719703940000174
分别表示系统高斯白噪声w1,w2,w3,w4,w5对应的方差。系统观测噪声V＝[v1 v2 v3]T，其中，v1,v2,v3分别表示两个系统高斯白噪声分量，V(k)对应的测量高斯白噪声协方差阵
Figure GDA0002719703940000175
其中，
Figure GDA0002719703940000176
分别表示系统高斯白噪声v1,v2,v3对应的方差，
Figure GDA0002719703940000177
可根据厘米级高精度差分GPS的位置测量噪声的统计特性来确定，
Figure GDA0002719703940000178
可根据厘米级高精度差分GPS的速度测量噪声的统计特性来确定。系统外输入噪声
Figure GDA0002719703940000179
其中，
Figure GDA00027197039400001710
wδ分别表示直测的系统外输入ae,an,δ对应的零均值高斯白噪声分量，这些白噪声隐含在系统状态函数f的三个系统外输入中。
观测方程h(X(k),V(k))和系统状态函数向量f(X,U,W,γ)分别为：
Figure GDA00027197039400001711
Figure GDA0002719703940000181
其中，
Figure GDA00027197039400001810
Figure GDA00027197039400001811
Figure GDA0002719703940000184
f4(X(k-1),U(k-1),W(k-1),γ(k-1))＝ve(k-1)+ae(k-1)·T+w4
f5(X(k-1),U(k-1),W(k-1),γ(k-1))＝vn(k-1)+an(k-1)·T+w5
根据公式(5)描述的系统状态方程和观测方程，建立无迹卡尔曼滤波的递推过程，通过时间更新和测量更新进行滤波递推：
1)对输入变量进行初始化并进行参数计算
Figure GDA0002719703940000185
式(6)中，X0为输入变量的初始值，
Figure GDA0002719703940000186
为X0的滤波估计值，本发明中，带有上标符号∧的变量均表示该变量的滤波估计值，P0为初始误差方差矩阵，为保证滤波算法收敛，本发明中，取P0为单位矩阵。
2)状态估计
计算Sigma点ξi(k-1)，并确定权重系数
Figure GDA0002719703940000187
Figure GDA0002719703940000188
式(7)中，
Figure GDA0002719703940000189
为加权协方差矩阵平方根的第i列，r为状态向量的维数，本发明中取r＝5。
Figure GDA0002719703940000191
式(8)中，λ表示调节Sigma点和
Figure GDA0002719703940000192
的距离参数，λ＝α2(r+κ)-r，α表示决定先验均值附近Sigma点分布广度的主要刻度因数，ε为用来强调后验协方差计算的零阶Sigma点权值的第二刻度因数，本发明中取ε＝2，
Figure GDA0002719703940000193
分别表示均值和方差的权重系数，κ表示第三刻度因数，本发明中取κ＝0。
3)时间更新方程计算
利用系统状态方程的传播计算Sigma点ξi(k,k-1)，系统状态方程对Sigma点做非线性变换：
ξi(k,k-1)＝f(ξi(k,k-1)),i＝0,1,...,2r (9)
计算函数映射后的均值
Figure GDA0002719703940000194
和方差P(k,k-1)，
Figure GDA0002719703940000195
Figure GDA0002719703940000196
4)观测更新方程计算
利用观测方程对变换后的Sigma点集进行变换，
χi(k,k-1)＝h(ξi(k,k-1)) (12)
式(12)中，χi(k,k-1)表示观测方程对Sigma点集变换后的值，
计算一步预测观测值，
Figure GDA0002719703940000197
式(13)中，
Figure GDA0002719703940000198
表示由k-1时刻递推的k时刻的一步预测观测值，
输出预测值协方差
Figure GDA0002719703940000199
Figure GDA00027197039400001910
计算状态值与测量值的协方差PXZ，
Figure GDA0002719703940000201
5)滤波更新
计算无迹卡尔曼滤波增益矩阵K(k)，
Figure GDA0002719703940000202
计算更新后的状态量估计值
Figure GDA0002719703940000203
和估计误差方差阵P(k)，
Figure GDA0002719703940000204
Figure GDA0002719703940000205
在实际测试过程中，不同智能驾驶车辆的运动特性存在差异，使得各被测车辆的初始状态无法保持一致。UKF算法虽然能够克服EKF等非线性滤波器存在的一些问题，但仍存在着跟踪能力较差、对滤波初值较敏感等不足。针对以上问题，引入自适应因子对UKF算法进行改进，以减小滤波初始值对系统状态方程和观测方程的影响，提高系统状态估计的精度。
引入自适应因子θk，利用改进的观测更新调整系统模型扰动对滤波递推的影响，将式(14)(15)(18)分别改写为：
Figure GDA0002719703940000206
Figure GDA0002719703940000207
Figure GDA0002719703940000208
式(19)中，R(k)表示k时刻的测量高斯白噪声协方差阵；
自适应因子θk的取值为：
Figure GDA0002719703940000209
式(22)中，预测误差向量
Figure GDA00027197039400002010
tr(g)表示计算矩阵的迹。
标准的无迹卡尔曼滤波递推的周期与观测向量的数据更新周期相同，在本发明中，由于厘米级高精度GPS的信息采集频率相对较低，观测向量(东向位置分量mg、北向位置分量ng和对地平面速度vg)的更新周期相对较长，若采用标准的滤波算法，将导致系统的输出频率较低。为保证系统具有较高的数据输出频率，满足高频率的测量要求，继续对UKF算法进行改进，即降低时间更新的周期，保持观测更新的周期不变，同时满足观测更新的周期是时间更新周期的整数倍(该整数可取20、10、5等)。
若厘米级高精度GPS数据的更新频率为TGPS，时间更新的周期T(即离散的周期T，该值可取0.01s、0.02s、0.05s等)应满足
Figure GDA0002719703940000211
其中，q表示正整数。因此，最终改进的无迹卡尔曼滤波过程可描述为：
1)当满足
Figure GDA0002719703940000212
时，滤波过程进行标准的无迹卡尔曼滤波递推过程，首先，利用式(9)～式(11)进行时间更新过程。其次，依次利用式(12)～式(16)、式(19)、式(20)进行观测更新过程。最后，利用式(17)、式(21)分别计算更新后的状态量估计值和估计误差方差阵。
2)当
Figure GDA0002719703940000213
时，首先，进行无迹卡尔曼滤波时间更新过程：
Figure GDA0002719703940000214
式(23)中，
Figure GDA0002719703940000215
表示权重系数，r表示状态向量的维数；
其次，计算更新后的状态量估计值
Figure GDA0002719703940000216
和估计误差方差阵P(k)：
Figure GDA0002719703940000217
P(k)＝P(k,k-1) (25)
上述算法的计算降低了滤波递推的周期，提高了系统状态变量的输出频率(根据传感器特性，TGPS＝0.05s，假设离散周期T＝0.01s，利用改进的无迹卡尔曼滤波进行滤波递推，系统输出频率则由20Hz提高到了100Hz)。
经过上述改进后的滤波递推计算，输出的状态估计
Figure GDA0002719703940000218
即为离散时刻k的车辆状态向量估计值，由此可以准确、高频的估计出智能驾驶汽车在自主车道变换过程中每个时刻的东向位置、北向位置、东向速度、北向速度等信息。
步骤三：量化自主车道变换性能的评价指标
现有的国家标准及测试法规中，尚无明确、统一的自主车道变换性能评价指标。考虑到单一的评价指标难以涵盖测评对象的所有信息，无法较好的衡量自主车道变换性能的优劣。因此，基于步骤二输出的车辆关键性基础性能参数，构建变道性能评价指标体系，设置多尺度的自主车道变换性能评价指标：目标间隙、距离碰撞时间和并线横摆稳定性，并对以上指标进行量化，对自主车道变换性能进行多维度定量测评。
首先，对涉及的名词做如下定义和解释：
1)自车是指具备自主车道变换能力的被测车辆；
2)目标车道是指自车执行变道后将要到达的车道；目标车辆是指在自车前方行驶轨迹线上，距离自车最近的车辆，该车可能位于自车所在的车道前方或目标车道；
3)目标间隙是指自车与目标车辆的欧式距离；
4)距离碰撞时间是指在保持相对车速不变的情况下，自车与目标车辆发生碰撞所需的时间；
5)并线横摆稳定性是指在进行自主车道变换过程中，汽车绕车身坐标系垂向轴偏转大小的程度。
其次，根据上述指标的含义，基于步骤二输出的车辆位置、速度等运动学参数，根据式(26)、(27)、(28)分别推算汽车的目标间隙、距离碰撞时间、并线横摆稳定性，具体地：
将目标间隙定义为：
Figure GDA0002719703940000221
式(26)中，Lp表示目标间隙，单位为米，用以表征智能驾驶汽车实施变道的安全性，当目标间隙小于最小安全车距时，智能驾驶汽车若进行自主车道变换操作，将发生碰撞。msub,nsub分别为自车的东向位置分量和北向位置分量，
Figure GDA0002719703940000222
分别为目标车辆的东向位置分量和北向位置分量，位置信息可由步骤二输出得到，μp为目标间隙计算常数，通过测量车辆外廓到质心的距离获得。p表示目标车辆所在的车道，其中，1表示目标车辆位于自车所在的车道，2表示目标车辆位于目标车道，对应地，L1,L2分别表示自车与当前车道上目标车辆的间隙、自车与目标车道上目标车辆的间隙，单位为米。
借鉴车辆自动紧急制动系统(Automatic Emergency Braking System,AEBS)和前向碰撞预警系统(Forward Collision Warning System,FCWS)性能测试中的“距离碰撞时间”性能评价指标，用以描述智能驾驶汽车实施变道操作的安全性和选择变道时机的适宜性，在本发明中，利用两车的目标间隙和接近速度对距离碰撞时间进行估计，距离碰撞时间可表示为：
Figure GDA0002719703940000231
式(27)中，TTC表示距离碰撞时间，单位为秒，vcs表示接近速度，单位为米每秒，接近速度等于自车与目标车辆的速度之差，当计算结果为负值时，表明在上述条件下，碰撞不可能发生。
将并线横摆稳定性定义为：
Figure GDA0002719703940000232
式(28)中，σMSE表示并线横摆稳定性的量化值，反映了执行自主车道变换的激进程度，s为横摆角速度数据的数量，
Figure GDA0002719703940000233
为k时刻车辆的横摆角速度，可通过惯性测量单元获得，k时刻横摆角速度的期望值
Figure GDA0002719703940000234
σMSE,
Figure GDA0002719703940000235
的单位均为弧度每秒，曲率半径
Figure GDA0002719703940000236
式中变量B1,B2,B3,B4分别为：
Figure GDA0002719703940000237
式(29)中，
Figure GDA0002719703940000238
分别表示由步骤二输出的k时刻的东向速度和北向速度，mk,nk分别表示由步骤二输出的k时刻的东向位置和北向位置。
当进行自主车道变换性能测试评价时，首先，根据步骤一、步骤二对系统各状态变量进行滤波递推。其次，基于高频、精确输出的车辆运动状态参数，根据步骤三计算目标间隙、距离碰撞时间、并线横摆稳定性三个评价指标的量化值。最后，通过定量评价的方式分析智能驾驶汽车实施变道的合理性、选择变道时机的适宜性、变道过程的激进程度和安全性，从而实现了智能驾驶自主车道变换性能优劣的高精度、高频率测量和科学定量评价。
为检验本发明公开的自主车道变换性能测试方法的实际效果，进行了实际路测试验，试验基本情况说明如下：
试验目的：检验本发明公开的自主车道变换性能测试方法的实际效果。
试验系统组成：试验系统由硬件设备以及软件系统(数据采集系统)组成。硬件设备主要包括：研华工控机PPC6150(Core i5-3610ME 2.7GHz CPU、4G内存、320G硬盘)、NovAtel SPAN-IGM-A1高精度MEMS组合导航系统、陀螺仪、奇瑞瑞虎3试验车、固定支架、车载电源、逆变器等，传感器安装位置位于试验车的质心处，天线安装位置位于车顶中心。
软件系统利用课题组开发的智能驾驶测评管理系统中的数据采集模块，采用多线程技术开发，以保证实时同步的采集与存储各传感器信息。利用智能驾驶测评管理系统中的车道变换测试模块，对自主车道变换的试验数据进行采集。
试验设置：试验中，汽车以一定的初速度经过测试起点后开始采集数据，行驶过程中执行多次车道变换操作，到达终点后，测试结束。
试验方法：利用搭载综合测试系统的试验车，在试验路面上进行多次车道变换性能测试，对车辆运动的位置、速度等关键性基础性能参数进行采集。最后，分析并输出车道变换性能指标的量化值：目标间隙、距离碰撞时间和并线横摆稳定性。
试验路面与环境：实验地点位于江苏省南京市江宁区正方中路附近，属于智能驾驶技术和汽车功能测试场景中典型的高速公路场景，试验路面为平坦干燥的沥青路面。
试验结果：试验表明，本发明提出的自主车道变换性能评价检测方法具有良好的性能，具体而言：
(1)通过本发明提出的测试方法和构建的变道性能评价指标体系，实现了智能驾驶自主车道变换性能优劣的高精度测评；
(2)相比于虚拟仿真测试和硬件在环测试，本发明提出的基于实际道路和真实交通环境下的自主车道变换性能测试方法，能够保障智能驾驶汽车在各种道路交通状况下运行的安全性和可靠性，更具有准确性和说服力；
(3)自主车道变换性能指标的测量精度和频率高，位置测量精度可达0.0351m(RMS)，测量频率为100Hz，准确性和实时性能够满足变道性能高动态和高精度的测试要求；
(4)环境适应性强，在夜晚、阴雨天等复杂测试环境下，本发明能够完成与平坦、干燥路面同等精度的自主车道变换性能测试，同时适用于开放道路场景、法规标准场景等多类测试场景。
为说明本发明的实际效果，下面给出某次人工模拟实现的自主车道变换性能测试的试验结果，试验结果曲线如图4、图5、图6所示。图4中的密集点线是整个试验过程的车辆运动轨迹曲线，由于输出频率为100Hz，点线较为密集。图5是图4的局部放大图，图6是整个试验过程中车辆位置误差相对于时间的变化曲线图。通过计算可知，本发明提出的测量方法的位置测量精度为0.0351m(RMS)，平均绝对误差为0.0239m，具有高精度、高频率等优点，实现了精确、可靠的智能驾驶自主车道变换性能测试。

Claims (1)

1.一种面向多场景的智能驾驶自主车道变换性能测试方法，其特征在于：首先根据自主车道变换过程中的运动特点，建立了基于运动学自行车模型的车道变换动态模型；其次，利用改进的无迹卡尔曼滤波算法对车辆位置、速度、方位角进行滤波估计；最后，基于准确递推的车辆关键性基础性能参数，构建变道性能评价指标体系，量化并输出自主车道变换性能的评价指标：目标间隙、距离碰撞时间和并线横摆稳定性；具体步骤包括：
步骤一：建立智能驾驶汽车自主车道变换的动态模型
在智能驾驶自主车道变换性能测试过程中，需要高频且精确的获取每一时刻被测车辆的位置、速度、方位角并进行存储；为了满足信息全、精度和频率高的测量需求，建立能够准确描述智能驾驶汽车变道运动特性的动态模型；对于行驶在通常道路交通环境下的前轮转向的四轮车辆，做出以下合理假定：
1)忽略智能驾驶汽车在垂直方向的运动，将智能驾驶汽车在自主车道变换过程中的运动参数简化到二维平面内；
2)假定汽车前轴的两个轮胎具有相同的转向角和转速，假定汽车后轴的两个轮胎具有相同的转向角和转速；
3)假定车辆前轮的方向与车辆当前速度方向一致；
根据以上要求和假定，对汽车进行适当的运动学建模；其中选取运动学自行车模型，建立自主车道变换过程的动态模型；
对车辆所在的平面坐标系进行定义，原点位于车辆质心附近的地球表面的一点，X轴指向正东方向，Y轴指向正北方向，车辆的质心点为G点；将前轴的左侧车轮和右侧车轮合并为一个点，位于B点；将后轴的左侧车轮和右侧车轮合并为一个点，位于A点；O点为线段OA、OB的交点，是车辆的瞬时滚动中心，线段OA、OB分别垂直于两个滚动轮的方向，车身轴线与正北方向的夹角为
Figure FDA0002719703930000011
车身轴线与速度方向夹角为β；车辆的运动学模型可描述为：
Figure FDA0002719703930000021
式(1)中，上标“'”表示微分，如m'表示m的微分，m,n分别为智能驾驶汽车的东向位置分量和北向位置分量，δ,β分别为汽车的方向盘转角和质心侧偏角，ve,vn分别为东向速度分量和北向速度分量，ae,an分别为东向加速度分量和北向加速度分量，v为车辆的对地平面速度且
Figure FDA0002719703930000022
ρ为转向系的传动比，其中取ρ＝10，lr,lf分别表示车辆前轮、后轮到车辆质心的距离；
Figure FDA0002719703930000025
为汽车的方位角，其中，方位角是指汽车中间轴线方向与正北方向的夹角，方位角
Figure FDA0002719703930000026
与东向速度ve、北向速度vn满足以下关系：
Figure FDA0002719703930000023
对于智能驾驶汽车的自主车道变换过程，取系统状态向量
Figure FDA0002719703930000027
其中矩阵上角标T表示对矩阵转置，T表示离散的周期；根据式(1)描述的运动学模型，建立系统状态方程：
Figure FDA0002719703930000024
式(3)中，f(·)为5维系统状态函数向量函数，W表示零均值的系统高斯白噪声向量，γ表示系统外输入对应的零均值高斯白噪声，U表示系统外部输入向量且U＝[ae an δ]T，其中，东向加速度分量ae和北向加速度分量an通过惯性测量单元获取，方向盘转角δ通过汽车CAN总线信息获取；
步骤二：基于改进无迹卡尔曼滤波的车辆运动状态估计
为推算智能驾驶自主车道变换过程中的系统各状态变量
Figure FDA0002719703930000037
可采用滤波递推估计的方法，利用较少的系统观测量实现多维度的参数递推；为处理步骤一中描述的非线性系统，采用无迹卡尔曼滤波器，对车辆运动状态参数进行递推估计；
选择厘米级高精度差分GPS作为车辆运动的测量传感器，以对地平面速度、东向位置分量和北向位置分量作为系统观测向量，则系统的观测方程可表示为：
Z(t)＝h(X(t),V(t)) (4)
式(4)中，h为观测方程，t表示时间，系统观测向量Z＝[mg ng vg]T，其中，mg,ng分别表示东向位置分量和北向位置分量的观测值，由厘米级高精度差分GPS采集的经纬度坐标转换得到；vg表示厘米级高精度差分GPS输出的对地平面速度，且满足
Figure FDA0002719703930000031
系统观测噪声V表示与系统过程噪声W互不相关的零均值高斯白噪声向量；
在实际的无迹卡尔曼滤波递推过程中，需要采用离散化的无迹卡尔曼滤波模型；为此，对系统状态方程(3)和观测方程(4)进行离散化处理，离散化后的系统状态方程和观测方程分别为：
Figure FDA0002719703930000032
式(5)中，k表示离散化时刻，系统过程噪声W＝[w1 w2 w3 w4 w5]T，其中，w1,w2,w3,w4,w5分别表示五个系统高斯白噪声分量，W(k-1)对应的系统高斯白噪声协方差阵
Figure FDA0002719703930000033
其中
Figure FDA0002719703930000034
分别表示系统高斯白噪声w1,w2,w3,w4,w5对应的方差；系统观测噪声V＝[v1 v2 v3]T，其中，v1,v2,v3分别表示两个系统高斯白噪声分量，V(k)对应的测量高斯白噪声协方差阵
Figure FDA0002719703930000035
其中，
Figure FDA0002719703930000036
分别表示系统高斯白噪声v1,v2,v3对应的方差，
Figure FDA0002719703930000041
可根据厘米级高精度差分GPS的位置测量噪声的统计特性来确定，
Figure FDA0002719703930000042
可根据厘米级高精度差分GPS的速度测量噪声的统计特性来确定；系统外输入噪声
Figure FDA0002719703930000043
其中，
Figure FDA0002719703930000044
wδ分别表示直测的系统外输入ae,an,δ对应的零均值高斯白噪声分量，这些白噪声隐含在系统状态函数f的三个系统外输入中；
观测方程h(X(k),V(k))和系统状态函数向量f(X,U,W,γ)分别为：
Figure FDA0002719703930000045
Figure FDA0002719703930000046
其中，
Figure FDA0002719703930000047
Figure FDA0002719703930000048
Figure FDA0002719703930000049
f4(X(k-1),U(k-1),W(k-1),γ(k-1))＝ve(k-1)+ae(k-1)·T+w4
f5(X(k-1),U(k-1),W(k-1),γ(k-1))＝vn(k-1)+an(k-1)·T+w5
根据公式(5)描述的系统状态方程和观测方程，建立无迹卡尔曼滤波的递推过程，通过时间更新和测量更新进行滤波递推：
1)对输入变量进行初始化并进行参数计算
Figure FDA00027197039300000410
式(6)中，X0为输入变量的初始值，
Figure FDA00027197039300000411
为X0的滤波估计值，带有上标符号^的变量均表示该变量的滤波估计值，P0为初始误差方差矩阵，为保证滤波算法收敛，其中，取P0为单位矩阵；
2)状态估计
计算Sigma点ξi(k-1)，并确定权重系数
Figure FDA0002719703930000051
Figure FDA0002719703930000052
式(7)中，
Figure FDA0002719703930000053
为加权协方差矩阵平方根的第i列，r为状态向量的维数，其中取r＝5；
Figure FDA0002719703930000054
式(8)中，λ表示调节Sigma点和
Figure FDA0002719703930000055
的距离参数，λ＝α2(r+κ)-r，α表示决定先验均值附近Sigma点分布广度的主要刻度因数，ε为用来强调后验协方差计算的零阶Sigma点权值的第二刻度因数，其中取ε＝2，
Figure FDA0002719703930000056
分别表示均值和方差的权重系数，κ表示第三刻度因数，其中取κ＝0；
3)时间更新方程计算
利用系统状态方程的传播计算Sigma点ξi(k,k-1)，系统状态方程对Sigma点做非线性变换：
ξi(k,k-1)＝f(ξi(k,k-1)),i＝0,1,...,2r (9)
计算函数映射后的均值
Figure FDA0002719703930000057
和方差P(k,k-1)，
Figure FDA0002719703930000058
Figure FDA0002719703930000059
4)观测更新方程计算
利用观测方程对变换后的Sigma点集进行变换，
χi(k,k-1)＝h(ξi(k,k-1)) (12)
式(12)中，χi(k,k-1)表示观测方程对Sigma点集变换后的值，
计算一步预测观测值，
Figure FDA0002719703930000061
式(13)中，
Figure FDA0002719703930000062
表示由k-1时刻递推的k时刻的一步预测观测值，
输出预测值协方差
Figure FDA0002719703930000063
Figure FDA0002719703930000064
计算状态值与测量值的协方差PXZ，
Figure FDA0002719703930000065
5)滤波更新
计算无迹卡尔曼滤波增益矩阵K(k)，
Figure FDA0002719703930000066
计算更新后的状态量估计值
Figure FDA0002719703930000067
和估计误差方差阵P(k)，
Figure FDA0002719703930000069
Figure FDA00027197039300000610
在实际测试过程中，不同智能驾驶车辆的运动特性存在差异，使得各被测车辆的初始状态无法保持一致；UKF算法虽然能够克服非线性滤波器存在的一些问题，但仍存在着跟踪能力较差、对滤波初值较敏感；针对以上问题，引入自适应因子对UKF算法进行改进，以减小滤波初始值对系统状态方程和观测方程的影响，提高系统状态估计的精度；
引入自适应因子θk，利用改进的观测更新调整系统模型扰动对滤波递推的影响，将式(14)(15)(18)分别改写为：
Figure FDA00027197039300000611
Figure FDA0002719703930000071
Figure FDA0002719703930000072
式(19)中，R(k)表示k时刻的测量高斯白噪声协方差阵；
自适应因子θk的取值为：
Figure FDA0002719703930000073
式(22)中，预测误差向量
Figure FDA0002719703930000074
tr(g)表示计算矩阵的迹；
标准的无迹卡尔曼滤波递推的周期与观测向量的数据更新周期相同，其中，由于厘米级高精度GPS的信息采集频率相对较低，观测向量的更新周期相对较长，若采用标准的滤波算法，将导致系统的输出频率较低；为保证系统具有较高的数据输出频率，满足高频率的测量要求，继续对UKF算法进行改进，即降低时间更新的周期，保持观测更新的周期不变，同时满足观测更新的周期是时间更新周期的整数倍；
若厘米级高精度GPS数据的更新频率为TGPS，时间更新的周期T，应满足
Figure FDA0002719703930000075
其中，q表示正整数；因此，最终改进的无迹卡尔曼滤波过程可描述为：
1)当满足
Figure FDA0002719703930000076
时，滤波过程进行标准的无迹卡尔曼滤波递推过程，首先，利用式(9)～式(11)进行时间更新过程；其次，依次利用式(12)～式(16)、式(19)、式(20)进行观测更新过程；最后，利用式(17)、式(21)分别计算更新后的状态量估计值和估计误差方差阵；
2)当
Figure FDA0002719703930000077
时，首先，进行无迹卡尔曼滤波时间更新过程：
Figure FDA0002719703930000078
式(23)中，
Figure FDA0002719703930000079
表示权重系数，r表示状态向量的维数；
其次，计算更新后的状态量估计值
Figure FDA0002719703930000081
和估计误差方差阵P(k)：
Figure FDA0002719703930000082
P(k)＝P(k,k-1) (25)
上述算法的计算降低了滤波递推的周期，提高了系统状态变量的输出频率；
经过上述改进后的滤波递推计算，输出的状态估计
Figure FDA0002719703930000083
即为离散时刻k的车辆状态向量估计值，由此可以准确、高频的估计出智能驾驶汽车在自主车道变换过程中每个时刻的东向位置、北向位置、东向速度、北向速度等信息；
步骤三：量化自主车道变换性能的评价指标
基于步骤二输出的车辆关键性基础性能参数，构建变道性能评价指标体系，设置多尺度的自主车道变换性能评价指标：目标间隙、距离碰撞时间和并线横摆稳定性，并对以上指标进行量化，对自主车道变换性能进行多维度定量测评；
首先，对涉及的名词做如下定义和解释：
1)自车是指具备自主车道变换能力的被测车辆；
2)目标车道是指自车执行变道后将要到达的车道；目标车辆是指在自车前方行驶轨迹线上，距离自车最近的车辆，该车可能位于自车所在的车道前方或目标车道；
3)目标间隙是指自车与目标车辆的欧式距离；
4)距离碰撞时间是指在保持相对车速不变的情况下，自车与目标车辆发生碰撞所需的时间；
5)并线横摆稳定性是指在进行自主车道变换过程中，汽车绕车身坐标系垂向轴偏转大小的程度；
其次，根据上述指标的含义，基于步骤二输出的车辆位置、速度，根据式(26)、(27)、(28)分别推算汽车的目标间隙、距离碰撞时间、并线横摆稳定性，具体地：
将目标间隙定义为：
Figure FDA0002719703930000084
式(26)中，Lp表示目标间隙，单位为米，用以表征智能驾驶汽车实施变道的安全性，当目标间隙小于最小安全车距时，智能驾驶汽车若进行自主车道变换操作，将发生碰撞；msub,nsub分别为自车的东向位置分量和北向位置分量，
Figure FDA0002719703930000085
分别为目标车辆的东向位置分量和北向位置分量，位置信息可由步骤二输出得到，μp为目标间隙计算常数，通过测量车辆外廓到质心的距离获得；p表示目标车辆所在的车道，其中，1表示目标车辆位于自车所在的车道，2表示目标车辆位于目标车道，对应地，L1,L2分别表示自车与当前车道上目标车辆的间隙、自车与目标车道上目标车辆的间隙，单位为米；
借鉴车辆自动紧急制动系统性能测试中的“距离碰撞时间”性能评价指标，用以描述智能驾驶汽车实施变道操作的安全性和选择变道时机的适宜性，其中，利用两车的目标间隙和接近速度对距离碰撞时间进行估计，距离碰撞时间可表示为：
Figure FDA0002719703930000091
式(27)中，TTC表示距离碰撞时间，单位为秒，vcs表示接近速度，单位为米每秒，接近速度等于自车与目标车辆的速度之差，当计算结果为负值时，表明在上述条件下，碰撞不可能发生；
将并线横摆稳定性定义为：
Figure FDA0002719703930000092
式(28)中，σMSE表示并线横摆稳定性的量化值，反映了执行自主车道变换的激进程度，s为横摆角速度数据的数量，
Figure FDA0002719703930000093
为k时刻车辆的横摆角速度，可通过惯性测量单元获得，k时刻横摆角速度的期望值
Figure FDA0002719703930000094
σMSE,
Figure FDA0002719703930000095
的单位均为弧度每秒，曲率半径
Figure FDA0002719703930000096
式中变量B1,B2,B3,B4分别为：
Figure FDA0002719703930000097
式(29)中，
Figure FDA0002719703930000098
分别表示由步骤二输出的k时刻的东向速度和北向速度，mk,nk分别表示由步骤二输出的k时刻的东向位置和北向位置；
当进行自主车道变换性能测试时，首先，根据步骤一、步骤二对系统各状态变量进行滤波递推；其次，基于高频、精确输出的车辆运动状态参数，根据步骤三计算目标间隙、距离碰撞时间、并线横摆稳定性三个评价指标的量化值；最后，通过定量评价的方式分析智能驾驶汽车实施变道的合理性、选择变道时机的适宜性、变道过程的激进程度和安全性，从而实现了智能驾驶自主车道变换性能优劣的科学定量评价。
CN201911086483.0A 2019-11-08 2019-11-08 面向多场景的智能驾驶自主车道变换性能测试方法 Active CN110987463B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201911086483.0A CN110987463B (zh) 2019-11-08 2019-11-08 面向多场景的智能驾驶自主车道变换性能测试方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201911086483.0A CN110987463B (zh) 2019-11-08 2019-11-08 面向多场景的智能驾驶自主车道变换性能测试方法

Publications (2)

Publication Number Publication Date
CN110987463A CN110987463A (zh) 2020-04-10
CN110987463B true CN110987463B (zh) 2020-12-01

Family

ID=70083598

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201911086483.0A Active CN110987463B (zh) 2019-11-08 2019-11-08 面向多场景的智能驾驶自主车道变换性能测试方法

Country Status (1)

Country Link
CN (1) CN110987463B (zh)

Families Citing this family (12)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN111780981B (zh) 2020-05-21 2022-02-18 东南大学 一种智能车辆编队变道性能测评方法
CN111783536A (zh) * 2020-05-29 2020-10-16 北京百度网讯科技有限公司 变道合理性评估方法、装置、设备以及存储介质
CN112101709B (zh) * 2020-07-31 2022-11-11 东南大学 用于商用车辆自动紧急制动系统的全面准确的测评方法
CN112162563B (zh) * 2020-09-15 2023-01-31 郑州轻工业大学 基于自适应弱敏无迹Kalman滤波的直升机状态估计方法
CN112464564A (zh) * 2020-11-27 2021-03-09 北京罗克维尔斯科技有限公司 车辆动力学参数的确定方法和装置
US20220227397A1 (en) * 2021-01-19 2022-07-21 Baidu Usa Llc Dynamic model evaluation package for autonomous driving vehicles
CN113487910B (zh) * 2021-06-10 2022-12-09 中国汽车技术研究中心有限公司 一种用于跟车场景的汽车数据采集系统性能分析方法
CN113484040A (zh) * 2021-06-21 2021-10-08 上汽通用五菱汽车股份有限公司 一种紧急车道保持功能测试方法、装置、系统及存储介质
CN113705802B (zh) * 2021-07-26 2023-09-08 深圳市易成自动驾驶技术有限公司 自动驾驶的同步计算方法、装置、系统、程序产品及介质
CN115099599B (zh) * 2022-06-17 2024-04-30 东南大学 一种面向公交信号优先控制的测试评价方法
CN115266131A (zh) * 2022-07-04 2022-11-01 一汽奔腾轿车有限公司 一种具有四激光雷达定位的自主换道功能实车测试系统及方法
CN115440027A (zh) * 2022-07-21 2022-12-06 交通运输部公路科学研究所 面向多场景的智能车辆编队功能测评方法及装置

Family Cites Families (9)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
DE102008039400A1 (de) * 2008-08-22 2010-02-25 Iav Gmbh Ingenieurgesellschaft Auto Und Verkehr Vorrichtung zum reproduzierbaren Testen einer Umfeldsensorik
US9090263B2 (en) * 2010-07-20 2015-07-28 GM Global Technology Operations LLC Lane fusion system using forward-view and rear-view cameras
CN104751151B (zh) * 2015-04-28 2017-12-26 苏州安智汽车零部件有限公司 一种实时多车道识别及跟踪方法
US9921581B2 (en) * 2016-01-04 2018-03-20 Ford Global Technologies, Llc Autonomous vehicle emergency operating mode
CN108983219B (zh) * 2018-08-17 2020-04-07 北京航空航天大学 一种交通场景的图像信息和雷达信息的融合方法及系统
CN109684921B (zh) * 2018-11-20 2022-05-27 吉林大学 一种基于三维激光雷达的道路边界检测与跟踪方法
US11169532B2 (en) * 2019-03-26 2021-11-09 Intel Corporation Computer-assisted (CA)/autonomous driving (AD) vehicle inference model creation
CN110147382B (zh) * 2019-05-28 2022-04-12 北京百度网讯科技有限公司 车道线更新方法、装置、设备、系统及可读存储介质
CN110413942B (zh) * 2019-06-04 2023-08-08 上海汽车工业(集团)总公司 车道线方程筛选方法及其筛选模块

Also Published As

Publication number Publication date
CN110987463A (zh) 2020-04-10

Similar Documents

Publication Publication Date Title
CN110987463B (zh) 面向多场景的智能驾驶自主车道变换性能测试方法
CN111780981B (zh) 一种智能车辆编队变道性能测评方法
CN111595592B (zh) 一种自适应巡航控制系统性能测评方法
Liu et al. Automated vehicle sideslip angle estimation considering signal measurement characteristic
Xiong et al. IMU-based automated vehicle body sideslip angle and attitude estimation aided by GNSS using parallel adaptive Kalman filters
Guo et al. A review of estimation for vehicle tire-road interactions toward automated driving
Xia et al. Vehicle sideslip angle estimation by fusing inertial measurement unit and global navigation satellite system with heading alignment
CN110532636B (zh) 一种面向多场景的智能驾驶自主车道保持性能检测方法
Jauch et al. Road grade estimation with vehicle-based inertial measurement unit and orientation filter
Liu et al. Sideslip angle estimation of ground vehicles: a comparative study
CN111142091B (zh) 一种融合车载信息的自动驾驶系统激光雷达在线标定方法
CN104635233A (zh) 基于车载毫米波雷达的前方物体运动状态估计及分类方法
CN110296709B (zh) 基于自适应里程计模型的车载定位导航方法
CN105890595A (zh) 基于信息滤波的车载组合导航系统
CN108345021A (zh) 一种多普勒雷达辅助gps/ins车辆测速方法
CN112346103A (zh) 基于v2x的智能网联汽车动态协同定位方法与装置
CN114912061B (zh) 一种面向商用车车道保持辅助系统的精准测评方法
Wang et al. A review of dynamic state estimation for the neighborhood system of connected vehicles
Choi et al. Position estimation in urban u-turn section for autonomous vehicles using multiple vehicle model and interacting multiple model filter
Parra-Tsunekawa et al. A kalman-filtering-based approach for improving terrain mapping in off-road autonomous vehicles
Junqueira et al. A Model-less Approach for Estimating Vehicles Sideslip Angle by a Neural Network Concept
Chen et al. On-board estimation of road adhesion coefficient based on ANFIS and UKF
Chen et al. Estimation of vehicle sideslip angle via pseudo-multisensor information fusion method
Chaichaowarat et al. Kinematics-based analytical solution for wheel slip angle estimation of a RWD vehicle with drift
Kim et al. Vehicle localization using radar calibration with disconnected GPS

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination
GR01 Patent grant
GR01 Patent grant