CN110596694B - 一种复杂环境雷达多目标跟踪和道路行驶环境预测方法 - Google Patents

Abstract

本发明属于智能汽车技术领域，具体的说是一种复杂环境雷达多目标跟踪和道路行驶环境预测方法，特别是针对具有自适应控制功能的汽车在弯道中或者具有自主代客泊车功能的智能汽车在进出地下停车场弯曲坡道的过程中解决基于雷达原始目标量测值判别目标车辆所在车道位置关系识别不准确和目标跟踪算法的鲁棒性和精度不高的问题。主要由本车运动状态估计、毫米波雷达信号转换、时间同步、目标运动补偿、数据合理性判断、目标量测值降噪、道路曲率估计、目标聚合、目标运动属性运动状态识别、改进自适应扩展卡尔曼滤波算法跟踪和数据关联、道路行驶环境预测、关键目标生成共同完成。

Description

一种复杂环境雷达多目标跟踪和道路行驶环境预测方法

技术领域

本发明属于汽车技术领域，具体的说是一种复杂环境雷达多目标跟踪和道路行驶环境预测方法。

背景技术

具有自适应巡航控制、自主代客泊车系统功能的智能车辆通常采用毫米波雷达作为传感器感知前方障碍物的距离、速度等目标运动状态信息。车载毫米波雷达在真实行驶环境中会受到传感器本身热噪声和外界环境的干扰，使得探测到的目标状态信息含有噪声。通常将受到噪声污染的目标状态信息称之为目标量测值。由于雷达原始目标量测信息不能反映目标运动的历史状态信息、因此无法直接进行目标跟踪。目前国内绝大多数整车制造商主要是通过购买经过数据预处理和有效目标跟踪后直接输出面向特定高级辅助驾驶功能(例如：自适应巡航控制系统ACC)的毫米波雷达，在汽车上实现车辆的巡航控制、自主代客泊车等功能，国内整车厂更关注所实现功能的鲁棒性而对雷达的原始数据处理和目标跟踪算法的研究非常少。同时有很多技术人员通过使用软件构建简单毫米波雷达模型和虚拟场景来验证毫米波雷达目标检测和跟踪性能，例如中国专利公开号CN110031813A，申请公开日2019年05月21日，发明创造专利的名称为“用于多目标动态模拟的车载毫米波雷达测试系统”，该发明专利公布了一种用于多目标动态模拟的车载毫米波雷达测试系统，该系统包含了天线转台、雷达云台、雷达回波模拟模块、控制模块、信号采集模块和显示器组成。但是难以真实的模拟实际环境对毫米波雷达所引起的各种干扰以及雷达本身产生的噪声，因此在这种场景中的目标跟踪算法是不能直接应用到实际雷达在真实环境中的目标跟踪的。同时还有一些技术人员针对雷达目标运动信息开展算法研究。例如中国专利公开号CN108944929A，申请公开日2018年12月07日，发明创造专利的名称为“一种用于车辆自适应巡航控制系统的目标提取方法”，该发明专利公布了一种用于车辆自适应巡航控制系统的目标提取方法。公开专利采用的是在车辆上安装GPS定位系统获得车辆所在车道上的道路曲率信息，但是这种方法在城市高架桥上由于GPS信号不能准确获得不同高度的高架桥的曲率，因此算法的鲁棒性较差，甚至对不同高度得高架桥曲率估计失败。对于近几年推出的具有自主代客泊车系统的无人车在进出地下停车场弯曲坡道的过程中行驶的弯道半径远远小于自适应巡航控制系统的弯道半径。车辆在弯道中行驶的过程中可能会造成雷达跟踪车辆的误判，可能出现车辆的急加速情况，此工况是极度危险的。同时，现有的毫米波雷达目标跟踪方法没有对道路行驶环境进行预测，目标跟踪方法的鲁棒性和精度较低，不能满足近距离跟踪车辆的需求。并且在较多单目标车辆的跟踪方法中，在目标车辆位置发生改变的时候，存在建立新目标航迹需要的时间比较长从而影响控制算发鲁棒性的问题。

发明内容

本发明提供了一种复杂交通环境中雷达多目标跟踪和道路行驶环境预测方法，解决基于雷达原始目标量测值判别目标车辆所在车道位置关系识别不准确和目标跟踪算法的鲁棒性和精度不高的问题。

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

一种复杂环境雷达多目标跟踪和道路行驶环境预测方法，该预测方法包括以下步骤：

步骤一、建立车辆二自由度动力学模型；具体如下：

其中，M₁汽车质量；a_y为侧向加速度；F_y1为前轮所受地面侧向力；F_y2为后轮所受地面侧向力；I_z为车辆惯性横摆转矩；

为车辆绕z轴旋转的横摆角加速度；a为质心到前轴的距离；b为质心到后轴的距离；

步骤二、将非普通CAN网络传输的信号转换为普通CAN网络传输信号；

所述非普通CAN网络传输的信号转换为普通CAN网络传输信号，具体是指对于雷达目标量测值以多路复用信号形式转换为非多路复用信号形式；判断依据为：

其中，Number为量测值中目标的数目；A为标志位；B为多路复用信号占用的字节数；C表示偏移量；Multiplexor为CAN多路复用信号标记；

步骤三、将来源于车辆状态估计的信号和雷达的目标测量值信号进行时间同步；

所述时间同步是根据雷达一周期结束后的标志位Multiplexor来触发算法目标跟踪算法执行。

步骤四、使用数据合理性判断规则对雷达原始目标量测值进行判断、消除野点以及超出感兴趣距离、速度、方向角范围的目标量测值；

所述感兴趣距离的距离范围采用ISO22179标准中目标检测区间进行识别d_max＝τ_max(v_{set_max})*v_{set_max}，其中，τ_max(v_{set_max})为车辆可提供的自适应巡航系统最高设定车速下可获得的最大稳态车间时距；v_{set_max}为自适应巡航系统提供的最高设定车速；速度和方向角参考雷达厂商说明书中给定的范围来确定速度的最小和最大值分别为V_min和V_max，方向角的最小值和最大值分别为θ_min和θ_max；

步骤五、对毫米波雷达探测到的目标量测值进行运动补偿；

步骤六、消除毫米波雷达目标量测值中的随机噪声，对目标量测值进行匹配降噪处理；

步骤七、使用本车状态估计获得本车的道路曲率，以及使用雷达探测目标车辆的纵向速度和横向速度推算得出的目标车辆所在车道的道路曲率，通过建立的道路曲率识别模型辨别车辆所在车道的位置关系模型；

步骤八、采用曼哈顿距离进行目标聚类，消除鬼影的影响；

步骤九、依据目标和主车运动的速度对目标运动状态进行辨识，以及计算目标碰撞预警参数TTC；

步骤十、采用最近邻数据关联和改进自适应扩展卡尔曼滤波以及起始跟踪逻辑对目标航迹进行跟踪；

步骤十一、根据计算静止障碍物到本车的横向距离，横向距离落入特定范围的静止目标个数以及目标运动状态和目标所在车道的位置关系对道路环境预测；所述道路环境预测包括道路隔离带识别和道路属性识别；

步骤十二、依据目标碰撞预警参数TTC、车道辨识关系、以及道路环境预测信息筛选出关键的目标。

所述步骤四中使用数据合理性判断规则对雷达原始目标量测值进行判断的具体依据为：

(1)目标相对于雷达的相对径向距离在雷达探测范围内，即：D_r≤d_max；

(2)目标相对于雷达的相对径向速度在雷达探测范围内，即：V_min≤V_r≤V_max；

(3)目标量测角度值在雷达探测范围内，即：θ_min≤θ_r≤θ_max。

所述步骤五中运动补偿包括目标位置补偿和目标速度补偿两部分；所述目标位置补偿是将毫米波雷达坐标系中目标历史状态信息转换到当前毫米波雷达坐标系中；

r(k|k-1)＝[(x(k-1)-x_v(k))²+(y(k-1)-y_v(k))²]_1/2

式中：

x_v(k)＝vx(k-1)*Δt

y_v(k)＝v_y(k-1)*Δt

其中，r(k|k-1)为目标T＝(k-1)*Δt时刻的历史位置在T＝(k)*Δt时刻的毫米波雷达极坐标系中的相对径向距离；

为目标T＝(k-1)*Δt时刻的历史位置在T＝(k)*Δt时刻的毫米波雷达极坐标系中的相对径向距离和方位角；

r(k-1)为T＝(k-1)*Δt时刻目标在毫米波雷达极坐标系中的相对径向距离；

为T＝(k-1)*Δt时刻目标在毫米波雷达极坐标系中的方位角；

x(k-1)为T＝(k-1)*Δt时刻目标在毫米波雷达笛卡尔坐标系中的纵向距离；

y(k-1)为T＝(k-1)*Δt时刻目标在毫米波雷达笛卡尔坐标系中的横向距离；

x_v(k)为主车从T＝(k-1)*Δt时刻到T＝(k)*Δt时刻纵向位移；

y_v(k)为主车从T＝(k-1)*Δt时刻到T＝(k)*Δt时刻横向位移；

为主车从T＝(k-1)*Δt时刻到T＝(k)*Δt时刻横摆角；

v_x(k-1)为主车T＝(k-1)*Δt时刻纵轴方向的速度；

v_y(k-1)为主车T＝(k-1)*Δt时刻横轴方向的速度；

ω(k-1)为主车T＝(k-1)*Δt时刻横摆角速度；

所述目标速度补偿是为了获得目标在大地坐标系中的实际速度；

其中，v_{Tar_x}(k)为目标在T＝(k)*Δt时刻在大地坐标系中的纵向速度；

v_{Tar_y}(k)为目标在T＝(k)*Δt时刻在大地坐标系中的横向速度；

v_x(k)为主车T＝(k)*Δt时刻纵轴方向的速度；

v_y(k)为主车T＝(k)*Δt时刻横轴方向的速度；

ω(k)为主车T＝(k)*Δt时刻横摆角速度；

v_{Tar_rx}(k)为目标在T＝(k)*Δt时刻在毫米波雷达坐标系中的纵向速度；

v_{Tar_ry}(k)为目标在T＝(k)*Δt时刻在毫米波雷达坐标系中的横向速度。

所述步骤六中目标量测值匹配降噪处理依据同一个真实目标量测信息在时间历程上是连续的，但是目标量测值中的噪声干扰信息具有随机性，不具有连续性；因此通过记录一定周期内的原始目标量测值信息，分析这段点迹的连续性来区分真实目标和杂波；现将伪算法过程描述如下：

(1)初始化；

Step1：初始记录时间片段长度为N，目标量测值对应的二进制位标识对应的权值表bit_mark，以及长度位为N*M₂的表示索引号；其中M₂表示目标数目；

Step2：初始化目标量测属性值序列，分别包含相对径向距离、相对径向速度、方位角、置信度、生命周期列表Track_lifecycle、目标的ID列表，可信标记列表。；

(2)历史列表维护；

Step1：删除上一周期确定的目标属性值；

Step2：历史目标列表周期性运动补偿，包括目标量测值的位置补偿和速度补偿，并更新生命周期列表Track_lifecycle的值；

Step3：采用二进制编码维护每个目标临近周期匹配状态，通过移位运算更新该标记；其中，二进制标记长度为记录时间片段的长度N，最高位表示当前周期，由高位到低位进行周期推算；

(3)新目标点存入列表；

Step1：更新当前标识位C_mark，如果值为N，更新该值为1；否则，C_mark的数值自增1；

Step2：遍历输入的属性序列，如果有效性标志位为0，转到Step2a；否则，转到Step2b；

Step2a：对于第k个输入，将生命周期列表Track_lifecycle的第(C_mark-1)*M₂+k个位置的数值置为0；对于Track位标识列表Tracklist_histrack同样处理；

Step2b：对于第k个输入，分别对每个列表的第(C_mark-1)*M₂+k个位置赋新值；

(4)目标量测值点对匹配；

Step1：初始化用于记录每个历史目标的配对点位置的变量标记match_id；

Step2：目标点配对准测为径向距离绝对差值的加权、侧向距离绝对差值的加权、径向速度绝对差值的加权、侧向速度绝对差值的加权分别与设定阈值的比较；若加权值小于设定的阈值，则将历史目标对应的match_id值置为当前目标点序号，并对目标量测值对应的其它属性值进行更新；否则，将match_id值留空；

(5)降噪目标点列表生成；

Step1：将bit_mark的值由大到小进行排序，对大于设定阈值的项，检测目标点的Id是否为设定初值0；如果是，则将全局可信目标计数自增1，并赋予该目标的Id；否则，沿用原来的Id号；

Step2：对于列表中的所有置信度为0.1并且距离小于空降目标阈值以及目标绝对速度即目标量测值的速度-本车速度的绝对值为0的项属性值置为0；

Step3：将历史列表中的目标属性值输出。

所述步骤七中道路曲率的估计首先通过本车方向盘转角来识别行驶道路为直道还是弯道，方向盘转角δ连续三个周期大于设定阈值的时候将认为弯道，否则将是直道；

所述弯道道路曲率的估计包括本车所在道路曲率的估计和目标车所在道路曲率的估计两部分；

所述本车所在道路曲率K₁的估计是依据横摆角速度ω、本车纵向车速v_x、方向盘转角δ、轴距L、转向系传动比is_g进行加权估计：

其中，P为权值，P的数值和车辆行驶的速度有关，车速越高、P的数值越大；

所述目标车所在道路曲率的估计是依据经过运动补偿后目标车辆的纵向速度v′_x，横向速度v_y进行估计：

其中，

为目标车的航向角，由物理关系可得目标车辆所在的曲率K₂为：

所述弯道目标车辆和本车之间的横向距离D1为：

所述直道目标车辆和本车之间的横向距离D₂为：D₂＝d*sinθ；

所述道路曲率识别模型目的为辨别车辆所在车道的位置关系；

(1)同车道内的目标辨识模型

(2)目标处于左车道内的辨识模型

(3)目标处于右车道内的辨识模型

其中：W为标准车道宽度；d_L为主车与左车道线之间的距离；d_R为主车与右车道线之间的距离；B为本车的宽度；M₃为车道标识线的宽度；D为D₁或者D₂；

所述步骤八中曼哈顿距离进行目标聚类为：

式中，(x_i，y_i)为雷达获得的i点的目标纵向距离和侧向距离量测值；

(x_j，y_j)为雷达获得的j点的目标纵向距离和侧向距离量测值；

为雷达获得的i点的目标纵向速度、侧向速度量测值；

(V_{Tar_rx_}j，V_{Tar_ry_j})为雷达获得的j点的目标纵向速度、侧向速度量测值；

Threshol_d_x为纵向距离聚类阈值；

Threshol_d_y为横向距离聚类阈值；

Threshol_Vx为纵向速度聚类阈值；

Threshol_V_y为横向速度聚类阈值。

所述步骤九中目标运动状态的辨识是依据经过运动补偿和聚类分析后得到的目标纵向速度值和本车纵向速度值之差ΔV，在连续时间片段内进行的状态进行分析辨别目标为静止、同向、反向、起停状态；

所述目标纵向速度值和本车纵向速度值之差ΔV在连续三个采样周期内在-1～1m/s之间，目标运动状态为静止，或者由同向/反向运动状态切换为起停状态；

所述目标纵向速度值和本车纵向速度值之差ΔV在连续三个采样周期内大于2m/s，目标运动状态为同向，或者由静止/起停状态切换为同向状态；

所述目标纵向速度值和本车纵向速度值之差ΔV在连续三个采样周期内小于-2m/s，目标运动状态为反向，或者由静止/起停状态切换为反向状态；

所述目标碰撞预警参数TTC为：

其中，d_x为本车与目标车辆之间的相对纵向距离；V_x为本车与目标车辆之间的相对纵向速度。

所述步骤十中改进自适应扩展卡尔曼滤波是为了避免参数中的减法运算，使得出现参数值违反约束而导致算法无法继续运行的故障，降低鲁棒性；改进后的自适应扩展卡尔曼滤波算法计算过程如下：

Step1：状态预测

P(k|k-1)＝A(k)*P(k-1)*A^T(k)+Q(k)

Step2：状态校正

S(k|k-1)＝H(k)*P(k|k-1)*H^T(k)+R(k)

K(k)＝P(k|k-1)*H^T(k)*S^-1(k|k-1)

P(K)＝P(k|k-1)-K(k)*H(k)*P(k|k-1)

Step3：计算改进后的噪声统计估计器

其中，R₁(k+1)为改进前的测量噪声协方差；Q₁(k+1)为改进前的过程噪声协方差；d(k)是k时刻的遗忘因子；

式中：

d(k)＝(1-b)/(1-b^k+1)

R₁(k+1)＝(1-d(k))R(k)+d(k)[ε(k)*ε^T(k)-H(k)*P(k|k-1)*H^T(K)]

Q₁(k+1)＝(1-d(k))Q(k)+d(k)[K(k)ε(k)ε^T(k)K^T(k)+P(k)-A(k)P(k-1)A^T(k)]

其中，d(k)是k时刻的遗忘因子；b为[0.95，0.995]范围内的遗忘因子；ε(k)为k时刻的新息，z(k)为k时刻的系统观测值，

为k-1时刻系统状态预测的观测值，R(k)为k时刻的测量噪声协方差；H(k)为K时刻观测矩阵，P(k|k-1)为k时刻误差协方差矩阵预测值，Q(k)为k时刻过程噪声协方差矩阵，P(k)为k时刻误差协方差，P(k-1)为k-1时刻误差协方差，A(k)k时刻状态转移矩阵，K(k)为k时刻自适应扩展卡尔曼增益矩阵；

所述目标航迹进行跟踪规则是指以被跟踪目标预测位置为中心，用来确定来自该目标的量测可能出现的区域，采用最近邻椭圆形跟踪门实现目标量测值和航迹之间的数据关联；如下所述构建椭圆形跟踪门最近邻数据关联方法：

Step1：计算新息的协方差矩阵

S_k＝E(ε(k)ε^T(k))

Step2：计算统计距离

式中：ε(k)为k时刻的新息，

为k-1时刻系统状态预测的观测值，S_k为k时刻新息协方差矩阵；

若统计距离满足下面的式子，则表明目标量测值可以用来和航迹进行匹配关联，否则不能用来和航迹进行数据关联；

式中：

为统计距离，λ为标定参数；

所述跟踪起始逻辑是依据目标的生命周期判断目标量测值进入自适应扩展卡尔曼滤波迭代时，状态属性值取值的依据；

所述状态属性值包括目标运动的横向距离、纵向距离、横向速度、纵向速度、横向加速度、纵向加速度；

若目标的生命周期为0，则将目标运动状态属性值初始为0；

若目标的生命周期为1，表明该目标为新目标，目标运动状态属性值中横向距离、纵向距离、横向速度、纵向速度为当前时刻目标量测值，横向加速度和纵向加速度为0；

若目标的生命周期不为0或者1，则目标运动状态属性值中横向距离、纵向距离、横向速度、纵向速度为上一时刻目标量测值，横向加速度和纵向加速度为当前时刻和上一时刻的速度差和时间的比值。

所述步骤十一中所述道路隔离带识别是通过计算每一个静止障碍物到本车的横向距离D_i，如果D_min≤D_i≤D_max则认为该静止障碍物可能是道路隔离带的一部分；同时记录落入设定区间内的障碍物的个数T，当T超过了设定的阈值时，认为静止的障碍物为道路隔离带；通过计算落入特定横向距离范围内的静止目标个数T，来判别道路隔离带，将道路隔离带所在的位置标识为不可行驶区域；

所述道路属性识别是通过目标车辆的绝对运动速度识别道路为同向车道、反向车道、不可行使区域；所述道路属性识别在每一次转动方向盘之后将会置为初始状态；

如果所述目标车辆的绝对运动速度在连续三个采样周期内大于2m/s，认为目标车辆为同向运动，同时结合道路曲率识别模型辨识得到的目标所在道路位置将对应的车道标识为同向车道；如果所述目标车辆绝对运动速度在连续三个采样周期内小于-2m/s，认为目标车辆为反向运动，同时结合道路曲率识别模型辨识得到的目标所在道路位置将对应的车道标识为反向车道；通过计算落入特定横向距离范围内的静止目标个数T，可以判别道路隔离带，将道路隔离带所在的位置标识为不可行驶区域；

所述步骤十二中关键的目标的筛选是指依据本车和目标车之间的横向距离D，分别在本车道、左车道、右车道内将目标运动状态为同向或者起停状态的目标依据相对距离最近和TTC最小原则，筛选出自适应巡航控制系统或者自主代客泊车系统跟踪的目标。

本发明的有益效果为：

(1)本发明通过使用本车的运动状态估计信息对雷达获得的目标运动状态进行运动补偿，可以获得目标相对于大地坐标系真实位置和运动速度，有利于提高目标运动状态的辨识准确性，提高目标跟踪的动态响应能力；

(2)本发明提出了基于事件的触发同步机制，可以实现雷达数据和车辆运动状态信息精确时间同步；

(3)本发明通过记录连续一段时间片段(N个周期)内的原始目标量测数据，依据相邻周期目标径向距离及相对速度匹配准测，实现过滤和消除目标量测值中地面杂波/随机扰动噪声的影响，提高了目标量测值的稳定程度；

(4)本发明通过使用曼哈顿距离进行目标聚类，消除鬼影的影响；

(5)本发明改进了自适应扩展卡尔曼滤波和最近邻数据关联方法，在数学方法中消除了矩阵的非负定性并通过试验验证了算法的收敛性和精度，算法的鲁棒性得到了较大的提高；

(6)本发明提出的道路曲率计算和辨识模型，能够适应弯道、直道行驶过程中的多目标跟踪并且具有较高的目标跟踪精度；

(7)本发明提出的通过计算每一个静止障碍物到本车的横向距离D_i，然后计算横向距离落入D_min≤D_i≤D_max这个范围内静止障碍物的个数T，当T超过了设定的阈值时，认为静止的障碍物为道路隔离带的方法可以有效的识别道路隔离带。提出了识别同向车道、反向车道、不可行使区域的道路行驶环境预测方法，有助于主车在路径规划过程中分析可行使区域。提出了针对自适应巡航控制系统或者自主代客泊车系统的多目标跟踪筛选规则，有效避免单目标车辆的跟踪方法中，在目标车辆位置发生改变的时候，建立新目标航迹需要的时间比较长从而影响控制算发鲁棒性的问题。

附图说明

图1为本发明方法流程图；

图2为本发明目标量测值匹配降噪流程图；

图3为本发明弯道多目标跟踪场景图；

图4为本发明未运动补偿时弯道多目标跟踪识别和道路环境预测效果图；

图5为本发明运动补偿后弯道多目标跟踪识别和道路环境预测效果图；

图6为本发明未运动补偿时弯道多目标跟踪效果图；

图7为本发明运动补偿时弯道多目标跟踪效果图；

图8为本发明未运动补偿时弯道多目标侧向距离跟踪效果图；

图9为本发明运动补偿时弯道多目标侧向距离跟踪效果图；

图10为本发明未运动补偿时弯道多目标纵向距离跟踪效果图；

图11为本发明运动补偿时弯道多目标纵向距离跟踪效果图。

图中：1、左车道目标车辆；2、本车道目标车辆；3、试验车辆。

具体实施方式

参阅图1，本发明提出的复杂环境雷达多目标跟踪和道路行驶环境预测方法由本车运动状态估计、毫米波雷达信号转换、时间同步、数据和理性判断、目标运动补偿、目标量测值降噪、道路曲率估计、目标聚合、目标运动属性运动状态识别、改进自适应扩展卡尔曼滤波算法跟踪和数据关联、道路行驶环境预测、关键目标生成共同完成。下面对该方法的各个步骤描述如下：

步骤一、建立车辆二自由度动力学模型，以前轮转角δ、纵向速度v_x为输入，使用卡尔曼滤波对纵向速度和横摆角速度进行滤波处理并结合车辆二自由度动力学模型对侧向速度进行观测，得到车辆的侧向速度v_y；

其中

式中，M₁汽车质量；a_y为侧向加速度；I_z车辆惯性横摆转矩；θ为车辆绕z轴旋转的横摆角，

为车辆绕z轴旋转的横摆角加速度，

为车辆绕z轴旋转的横摆角速度，F_y1为前轮所受地面侧向力，F_y2为后轮所受地面侧向力；a为质心到前轴的距离、b为质心到后轴的距离，β为航向角，ω_r为横摆角速度，v_y为车辆的横向速度，v_x为车辆的纵向速度；

步骤二、对非普通CAN网络(例如：多路复用信号)传输的信号转换为普通CAN网络传输信号。

所述非普通CAN网络传输的信号转换为普通CAN网络传输信号，具体是指对于雷达目标量测值以多路复用信号形式转换为非多路复用信号形式；判断依据为：

其中，Number为量测值中目标的数目，A为标志位，B为多路复用信号占用的字节数，C表示偏移量；Multiplexor为CAN多路复用信号标记；

步骤三、将来源于车辆状态估计的信号和雷达的目标测量值信号进行时间同步。

所述时间同步是根据雷达一周期结束后的标志位Multiplexor来触发算法目标跟踪算法执行。

步骤四、使用数据合理性判断规则对雷达原始目标量测值进行判断、消除野点以及超出感兴趣距离、速度、方向角范围的目标量测值。

所述感兴趣距离的范围采用ISO22179标准中目标检测区间进行识别：

d_max＝τ_max(v_{set_max})*v_{set_max}；

其中，τ_max(v_{set_max})为车辆可提供的自适应巡航系统最高设定车速下可获得的最大稳态车间时距；v_{set_max}为自适应巡航系统提供的最高设定车速；速度和方向角参考雷达厂商说明书中给定的范围来确定速度的最小和最大值分别为V_min和V_max，方向角的最小值和最大值分别为θ_min和θ_max。

(1)、目标相对于雷达的相对径向距离在雷达探测范围内，即：D_r≤d_max；

(2)、目标相对于雷达的相对径向速度在雷达探测范围内，即：V_min≤V_r≤V_max；

(3)、目标量测角度值应该在雷达探测范围内，即：θ_min≤θ_r≤θ_max。

步骤五、使用本车运动状态估计信息对毫米波雷达探测到的目标量测值进行运动补偿。

所述运动补偿包括目标位置补偿和目标速度补偿两部分。

所述目标位置补偿是将毫米波雷达坐标系中目标历史状态信息转换到当前毫米波雷达坐标系中。

r(k|k-1)＝[(x(k-1)-x_v(k))²+(y(k-1)-y_v(k))²]^1/2

式中，

x_v(k)＝v_x(k-1)*Δt

y_v(k)＝v_y(k-1)*Δt

其中，r(k|k-1)为目标T＝(k-1)*Δt时刻的历史位置在T＝(k)*Δt时刻的毫米波雷达极坐标系中的相对径向距离；

为目标T＝(k-1)*Δt时刻的历史位置在T＝(k)*Δt时刻的毫米波雷达极坐标系中的相对径向距离和方位角；

r(k-1)为T＝(k-1)*Δt时刻目标在毫米波雷达极坐标系中的相对径向距离；

为T＝(k-1)*Δt时刻目标在毫米波雷达极坐标系中的方位角；

x(k-1)为T＝(k-1)*Δt时刻目标在毫米波雷达笛卡尔坐标系中的纵向距离；

y(k-1)为T＝(k-1)*Δt时刻目标在毫米波雷达笛卡尔坐标系中的横向距离；

x_v(k)为主车从T＝(k-1)*Δt时刻到T＝(k)*Δt时刻纵向位移；

y_v(k)为主车从T＝(k-1)*Δt时刻到T＝(k)*Δt时刻横向位移；

为主车从T＝(k-1)*Δt时刻到T＝(k)*Δt时刻横摆角；

v_x(k-1)为主车T＝(k-1)*Δt时刻纵轴方向的速度；

v_y(k-1)为主车T＝(k-1)*Δt时刻横轴方向的速度；

ω(k-1)为主车T＝(k-1)*Δt时刻横摆角速度；

所述目标速度补偿是为了获得目标在大地坐标系中的实际速度；

其中，v_{Tar_x}(k)为目标在T＝(k)*Δt时刻在大地坐标系中的纵向速度；

v_{Tar_y}(k)为目标在T＝(k)*Δt时刻在大地坐标系中的横向速度；

v_x(k)为主车T＝(k)*Δt时刻纵轴方向的速度；

v_y(k)为主车T＝(k)*Δt时刻横轴方向的速度；

ω(k)为主车T＝(k)*Δt时刻横摆角速度；

v_{Tar_rx}(k)为目标在T＝(k)*Δt时刻在毫米波雷达坐标系中的纵向速度；

v_{Tar_ry}(k)为目标在T＝(k)*Δt时刻在毫米波雷达坐标系中的横向速度。

步骤六、消除毫米波雷达目标量测值中的随机噪声，对目标量测值进行匹配降噪处理。

参阅图2，所述目标量测值匹配降噪处理依据同一个真实目标量测信息在时间历程上是连续的，但是目标量测值中的噪声干扰信息具有随机性，不具有连续性。因此通过记录一定周期内的原始目标量测值信息，分析这段点迹的连续性来区分真实目标和杂波。现将伪算法过程描述如下：

(1)初始化：

Step1：初始记录时间片段长度为N，目标量测值对应的二进制位标识对应的权值表bit_mark，以及长度位为N*M₂的表示索引号；其中M₂表示目标数目。

Step2：初始化目标量测属性值序列，分别包含相对径向距离、相对径向速度、方位角、置信度、生命周期列表Track_lifecycle、目标的ID列表，可信标记列表。

(2)历史列表维护

Step1：删除上一周期确定的目标属性值；

Step2：历史目标列表周期性运动补偿，包括目标量测值的位置补偿和速度补偿，并更新生命周期列表Track_lifecycle的值；

Step3：采用二进制编码维护每个目标临近周期匹配状态，通过移位运算更新该标记。其中二进制标记长度为记录时间片段的长度N，最高位表示当前周期，由高位到低位进行周期推算；

(3)新目标点存入列表

Step1：更新当前标识位C_mark，如果值为N，更新该值为1；否则，C_mark的数值自增1；

Step2：遍历输入的属性序列，如果有效性标志位为0，转到Step2a；否则，转到Step2b；

Step2a：对于第k个输入，将生命周期列表Track_lifecycle的第(C_mark-1)*M₂+k个位置的数值置为0；对于Track位标识列表Tracklist_histrack同样处理；

Step2b：对于第k个输入，分别对每个列表的第(C_mark-1)*M₂+k个位置赋新值；

(4)目标量测值点对匹配；

Step1：初始化用于记录每个历史目标的配对点位置的变量标记match_id；

Step2：目标点配对准测为径向距离绝对差值的加权、侧向距离绝对差值的加权、径向速度绝对差值的加权、侧向速度绝对差值的加权分别与设定阈值的比较；若加权值小于设定的阈值，则将历史目标对应的match_id值置为当前目标点序号，并对目标量测值对应的其它属性值进行更新。否则，将match_id值留空；

(5)降噪目标点列表生成；

Step1：将bit_mark的值由大到小进行排序，对大于设定阈值的项，检测目标点的Id是否为设定初值0。如果是，则将全局可信目标计数自增1，并赋予该目标的Id；否则，沿用原来的Id号；

Step2：对于列表中的所有置信度为0.1并且距离小于空降目标阈值以及目标绝对速度(目标量测值的速度-本车速度的绝对值)为0的项属性值置为0；

Step3：将历史列表中的目标属性值输出。

步骤七、使用本车状态估计获得本车的道路曲率，以及使用雷达探测目标车辆的纵向速度和横向速度推算得出的目标车辆所在车道的道路曲率，通过建立的道路曲率识别模型辨别车辆所在车道的位置关系模型。

所述道路曲率的估计首先通过本车方向盘转角来识别行驶道路为直道还是弯道；方向盘转角δ连续三个周期大于设定阈值的时候将认为弯道，否则将是直道；

所述弯道道路曲率估计由本车所在道路曲率的估计和目标车所在道路曲率的估计两部分组成；

所述本车所在道路曲率K₁的估计是依据横摆角速度ω、本车纵向车速v_x、方向盘转角δ、轴距L、转向系传动比i_sg进行加权估计，具体如下：

其中，P为权值，P的数值和车辆行驶的速度有关，车速越高、P的数值越大；

所述目标车所在道路曲率的估计是依据经过运动补偿后目标车辆的纵向速度v′_x，横向速度v_y进行估计，具体如下：

其中，

为目标车的航向角，由物理关系可得目标车辆所在的曲率K₂为：

所述弯道目标车辆和本车之间的横向距离D₁为：

所述直道目标车辆和本车之间的横向距离D₂为：D₂＝d*sinθ；

所述道路曲率识别模型目的为辨别车辆所在车道的位置关系；

(1)同车道内的目标辨识模型；

(2)目标处于左车道内的辨识模型

(3)目标处于右车道内的辨识模型

其中，W为标准车道宽度；d_L为主车与左车道线之间的距离；d_R为主车与右车道线之间的距离；B为本车的宽度；M₃为车道标识线的宽度；D为D₁或者D₂；

步骤八、采用曼哈顿距离进行目标聚类，消除鬼影的影响；

所述曼哈顿距离进行目标聚类为：

其中，(x_i，y_i)为雷达获得的i点的目标纵向距离和侧向距离量测值；

(x_j，y_j)为雷达获得的j点的目标纵向距离和侧向距离量测值；

(V_{Tar_rx_i}，v_{Tar_ry_i})为雷达获得的i点的目标纵向速度、侧向速度量测值；

(V_{Tar_rx_j}，V_{Tar_ry_j})为雷达获得的j点的目标纵向速度、侧向速度量测值；

Threshol_d_x为纵向距离聚类阈值；

Threshol_d_y为横向距离聚类阈值；

Threshol_Vx为纵向速度聚类阈值；

Threshol_Vy为横向速度聚类阈值。

步骤九、依据目标和主车运动的速度对目标运动状态进行辨识，以及计算目标碰撞预警参数TTC。

所述目标运动状态的辨识是依据经过运动补偿和聚类分析后得到的目标纵向速度值和本车纵向速度值之差ΔV，在连续时间片段内进行的状态进行分析辨别目标为静止、同向、反向、起停状态；

所述目标纵向速度值和本车纵向速度值之差ΔV在连续三个采样周期内在-1～1m/s之间，目标运动状态为静止，或者由同向/反向运动状态切换为起停状态；

所述目标纵向速度值和本车纵向速度值之差ΔV在连续三个采样周期内大于2m/s，目标运动状态为同向，或者由静止/起停状态切换为同向状态；

所述目标纵向速度值和本车纵向速度值之差ΔV在连续三个采样周期内小于-2m/s，目标运动状态为反向，或者由静止/起停状态切换为反向状态；

所述目标碰撞预警参数TTC为：

其中，d_x为本车与目标车辆之间的相对纵向距离；V_x为本车与目标车辆之间的相对纵向速度。

步骤十、采用最近邻数据关联和改进自适应扩展卡尔曼滤波以及起始跟踪逻辑对目标航迹进行跟踪。

所述改进自适应扩展卡尔曼滤波是为了避免参数中的减法运算，使得出现参数值违反约束而导致算法无法继续运行的故障，降低鲁棒性；改进后的自适应扩展卡尔曼滤波算法计算过程如下：

Step1：状态预测

P(k|k-1)＝A(k)*P(k-1)*A^T(k)+Q(k)

Step2：状态校正

S(k|k-1)＝H(k)*P(k|k-1)*H^T(k)+R(k)

K(k)＝P(k|k-1)*H^T(k)*S^-1(k|k-1)

P(K)＝P(k|k-1)-K(k)*H(k)*P(k|k-1)

Step3：计算改进后的噪声统计估计器

其中，R₁(k+1)为改进前的测量噪声协方差；Q₁(k+1)为过程噪声协方差；d(k)是k时刻的遗忘因子；

式中：

d(k)＝(1-b)/(1-b^k+1)

R₁(k+1)＝(1-d(k))R(k)+d(k)[ε(k)*ε^T(k)-H(k)*P(k|k-1)*H^T(K)]

Q₁(k+1)＝(1-d(k))Q(k)+d(k)[K(k)ε(k)ε^T(k)K^T(k)+P(k)-A(k)P(k-1)A^T(k)]

式中：d(k)是k时刻的遗忘因子；b为[0.95，0.995]范围内的遗忘因子；ε(k)为k时刻的新息，z(k)为k时刻的系统观测值，

为k-1时刻系统状态预测的观测值，R(k)为k时刻的测量噪声协方差；H(k)为k时刻观测矩阵，P(k|k-1)为k时刻误差协方差矩阵预测值，Q(k)为k时刻过程噪声协方差矩阵，P(k)为k时刻误差协方差，P(k-1)为k-1时刻误差协方差，A(k)k时刻状态转移矩阵，K(k)为k时刻自适应扩展卡尔曼增益矩阵；

所述改进后自适应扩展卡尔曼滤波算法中，新容错噪声统计估计器中保留了大部分无偏噪声统计估计器里面的协方差项，具有提高算法精度的功能。

所述跟踪门规则是指以被跟踪目标预测位置为中心，用来确定来自该目标的量测可能出现的区域，采用最近邻椭圆形跟踪门实现目标量测值和航迹之间的数据关联；如下所述构建椭圆形跟踪门最近邻数据关联方法：

Step1：计算新息的协方差矩阵

S_k＝E(ε(k)ε^T(k))

Step2：计算统计距离

式中：ε(k)为k时刻的新息，

为k-1时刻系统状态预测的观测值，S_k为k时刻新息协方差矩阵；

若统计距离满足下面的式子，则表明目标量测值可以用来和航迹进行匹配关联，否则不能用来和航迹进行数据关联；

式中：

为统计距离，λ为标定参数；

所述跟踪起始逻辑是依据目标的生命周期判断目标量测值进入自适应扩展卡尔曼滤波迭代时，状态属性值取值的依据；

所述目标运动状态属性值包括目标运动的横向距离、纵向距离、横向速度、纵向速度、横向加速度、纵向加速度；

若目标的生命周期为0，则将目标运动状态属性值初始为0；

若目标的生命周期为1，表明该目标为新目标，目标运动状态属性值中横向距离、纵向距离、横向速度、纵向速度为当前时刻目标量测值，横向加速度和纵向加速度为0；

若目标的生命周期不为0或者1，则目标运动状态属性值中横向距离、纵向距离、横向速度、纵向速度为上一时刻目标量测值，横向加速度和纵向加速度为当前时刻和上一时刻的速度差和时间的比值；

步骤十一、根据计算静止障碍物到本车的横向距离，横向距离落入特定范围的静止目标个数以及目标运动状态和目标所在车道的位置关系对道路环境预测；道路环境预测主要包括道路隔离带识别和道路属性识别；

所述道路隔离带识别是通过计算每一个静止障碍物到本车的横向距离Di，如果D_min≤D_i≤D_max则认为该静止障碍物可能是道路隔离带的一部分；同时记录落入设定区间内的障碍物的个数T，当T超过了设定的阈值时，认为静止的障碍物为道路隔离带；

所述道路属性识别是通过目标车辆的绝对运动速度识别道路为同向车道、反向车道、不可行使区域；

所述道路属性识别在每一次转动方向盘之后将会置为初始状态；

所述目标车辆绝对运动速度在连续三个采样周期内大于2m/s，认为目标车辆为同向运动，同时结合道路曲率识别模型辨识得到的目标所在道路位置将对应的车道标识为同向车道；

所述目标车辆绝对运动速度在连续三个采样周期内小于-2m/s，认为目标车辆为反向运动，同时结合道路曲率识别模型辨识得到的目标所在道路位置将对应的车道标识为反向车道；

通过计算落入特定横向距离范围内的静止目标个数T，可以判别道路隔离带，将道路隔离带所在的位置标识为不可行驶区域；

步骤十二、依据目标碰撞预警参数TTC、车道辨识关系、以及道路环境预测信息筛选出关键的目标；

所述关键的目标筛选是指：依据本车和目标车之间的横向距离D，分别在在本车道、左车道、右车道内将目标运动状态为同向或者起停状态的目标依据相对距离最近和TTC最小原则，筛选出自适应巡航控制系统或者自主代客泊车系统跟踪的目标。

参阅图3为本发明实施例中的弯道多目标跟踪场景图，弯道多目标跟踪场景中包括左车道目标车辆1、本车道目标车辆2、试验车辆3、道路隔离带组成。

参阅图4为本发明实施例中依据图3交通场景开展弯道多目标跟踪识别和道路环境预测的时候，未进行目标运动补偿时真实道路试验效果图，图中雷达测量值为单个周期内的雷达原始数据；真实目标值为经过本发明算法处理后识别出来的真实车辆；不可行使区域为本发明中提出依据横向距离落入特定范围的静止目标个数来识别的道路隔离带；同向车道为本发明中提出的依据目标车辆运动的绝对速度来判别车道行驶环境效果；车道线为本发明中为了判别目标运动，依据常规道路宽度设定的数值；由图可以看出在没有进行弯道目标运动补偿的时候，误将本车道内的目标2经过车道辨识模型后识别为右侧车道的目标，使得本车突然加速行驶，引发交通事故；

参阅图5为本发明实施例中依据图3交通场景开展弯道多目标跟踪识别和道路环境预测的时候，进行目标运动补偿后真实道路试验效果图，由图可以正确的看出经过本发明目标运动补偿后，准确的识别出目标所在的车道位置关系；

参阅图6、图7分别为本发明未运动补偿时弯道多目标跟踪效果图和本发明经过运动补偿后弯道多目标跟踪效果图；所述的图6目标跟踪轨迹图可以看出，没有经过运动补偿的时候，误将同车道内的前方目标识别为右车道的前方目标；所述的图7目标跟踪轨迹图可以看出，经过运动补偿之后，可以正确的识别在同车道内的前方目标和左车道内的前方目标；

参阅图8、图9分别为本发明未运动补偿时侧向距离跟踪效果图和本发明经过运动补偿后侧向距离跟踪效果图；图8目标侧向距离跟踪效果图可以看出，未经过运动补偿的时候计算得到的目标侧向距离较大，因此经过车道辨识模型后误将目标识别为右车道内的前方目标车；图9目标侧向距离跟踪效果图可以看出，经过运动补偿的时候计算得到的目标侧向距离较小，因此经过车道辨识模型后可以正确的将目标识别为本车道内的前方目标车；

参阅图10、图11分别为本发明未运动补偿时纵向距离跟踪效果图和本发明经过运动补偿后纵向距离跟踪效果图；图10未运动补偿时目标纵向距离跟踪效果图可以看出，0.2秒之前会误将本车道内的目标识别为右车道内的目标车辆；图11经过运动补偿后纵向距离跟踪效果图可以看出，整个识别周期内可以将本车道内的目标车辆正确的识别为本车道内的目标。

表1为本发明弯道行驶环境中本车道和临车道目标车辆在未进行目标运动补偿和运动补偿之后识别率对比效果表；弯道行驶环境中本车道和临车道目标车辆在运动补偿前后识别率是指在弯道中开展50组不同车速行驶状态的时候，本车道目标和临车道目标正确识别的概率。

表1

	弯道未补偿	弯道补偿后
			本车道识别率	82％	94％
临车道目标车辆识别率	81％	93％

表2为本发明改进自适应EKF算法相比于传统算法误差协方差对比表；本方法中的自适应EKF算法相比于传统的自适应EKF和EKF算法具有较高的目标跟踪精度。

表2

	EKF算法	传统自适应EKF	本方法中的自适应EKF
				误差协方差均值	35％	28％	15％

Claims (7)

1.一种复杂环境雷达多目标跟踪和道路行驶环境预测方法，其特征在于，该预测方法包括以下步骤：

步骤一、建立车辆二自由度动力学模型；具体如下：

其中，M₁汽车质量；a_y为侧向加速度；F_y1为前轮所受地面侧向力；F_y2为后轮所受地面侧向力；I_z为车辆惯性横摆转矩；

为车辆绕z轴旋转的横摆角加速度；a为质心到前轴的距离；b为质心到后轴的距离；

步骤二、将非普通CAN网络传输的信号转换为普通CAN网络传输信号；

所述非普通CAN网络传输的信号转换为普通CAN网络传输信号，具体是指对于雷达目标量测值以多路复用信号形式转换为非多路复用信号形式；判断依据为：

其中，Number为量测值中目标的数目；A为标志位；B为多路复用信号占用的字节数；C表示偏移量；Multiplexor为CAN多路复用信号标记；

步骤三、将来源于车辆状态估计的信号和雷达的目标测量值信号进行时间同步；

所述时间同步是根据雷达一周期结束后的标志位Multiplexor来触发算法目标跟踪算法执行；

步骤四、使用数据合理性判断规则对雷达原始目标量测值进行判断、消除野点以及超出感兴趣距离、速度、方向角范围的目标量测值；

所述感兴趣距离的距离范围采用ISO22179标准中目标检测区间进行识别d_max＝τ_max(v_{set_max})*v_{set_max}，其中，τ_max(v_{set_max})为车辆可提供的自适应巡航系统最高设定车速下可获得的最大稳态车间时距；v_{set_max}为自适应巡航系统提供的最高设定车速；速度和方向角参考雷达厂商说明书中给定的范围来确定速度的最小和最大值分别为V_min和V_max，方向角的最小值和最大值分别为θ_min和θ_max；

步骤五、对毫米波雷达探测到的目标量测值进行运动补偿；

步骤六、消除毫米波雷达目标量测值中的随机噪声，对目标量测值进行匹配降噪处理；

步骤七、使用本车状态估计获得本车的道路曲率，以及使用雷达探测目标车辆的纵向速度和横向速度推算得出的目标车辆所在车道的道路曲率，通过建立的道路曲率识别模型辨别车辆所在车道的位置关系模型；

步骤八、采用曼哈顿距离进行目标聚类，消除鬼影的影响；

步骤九、依据目标和主车运动的速度对目标运动状态进行辨识，以及计算目标碰撞预警参数TTC；

步骤十、采用最近邻数据关联和改进自适应扩展卡尔曼滤波以及起始跟踪逻辑对目标航迹进行跟踪；

步骤十一、根据计算静止障碍物到本车的横向距离，横向距离落入特定范围的静止目标个数以及目标运动状态和目标所在车道的位置关系对道路环境预测；所述道路环境预测包括道路隔离带识别和道路属性识别；

步骤十二、依据目标碰撞预警参数TTC、车道辨识关系、以及道路环境预测信息筛选出关键的目标；

所述步骤五中运动补偿包括目标位置补偿和目标速度补偿两部分；所述目标位置补偿是将毫米波雷达坐标系中目标历史状态信息转换到当前毫米波雷达坐标系中；

r(k|k-1)＝[(x(k-1)-x_v(k))²+(y(k-1)-y_v(k))²]^1/2

式中：

x_v(k)＝v_x(k-1)*Δt

y_v(k)＝v_y(k-1)*Δt

其中，r(k|k-1)为目标T＝(k-1)*Δt时刻的历史位置在T＝(k)*Δt时刻的毫米波雷达极坐标系中的相对径向距离；

为目标T＝(k-1)*Δt时刻的历史位置在T＝(k)*Δt时刻的毫米波雷达极坐标系中的相对径向距离和方位角；

r(k-1)为T＝(k-1)*Δt时刻目标在毫米波雷达极坐标系中的相对径向距离；

为T＝(k-1)*Δt时刻目标在毫米波雷达极坐标系中的方位角；

x(k-1)为T＝(k-1)*Δt时刻目标在毫米波雷达笛卡尔坐标系中的纵向距离；

y(k-1)为T＝(k-1)*Δt时刻目标在毫米波雷达笛卡尔坐标系中的横向距离；

x_v(k)为主车从T＝(k-1)*Δt时刻到T＝(k)*Δt时刻纵向位移；

y_v(k)为主车从T＝(k-1)*Δt时刻到T＝(k)*Δt时刻横向位移；

为主车从T＝(k-1)*Δt时刻到T＝(k)*Δt时刻横摆角；

v_x(k-1)为主车T＝(k-1)*Δt时刻纵轴方向的速度；

v_y(k-1)为主车T＝(k-1)*Δt时刻横轴方向的速度；

ω(k-1)为主车T＝(k-1)*Δt时刻横摆角速度；

所述目标速度补偿是为了获得目标在大地坐标系中的实际速度；

其中，v_{Tar_x}(k)为目标在T＝(k)*Δt时刻在大地坐标系中的纵向速度；

v_{Tar_y}(k)为目标在T＝(k)*Δt时刻在大地坐标系中的横向速度；

v_x(k)为主车T＝(k)*Δt时刻纵轴方向的速度；

v_y(k)为主车T＝(k)*Δt时刻横轴方向的速度；

ω(k)为主车T＝(k)*Δt时刻横摆角速度；

v_{Tar_rx}(k)为目标在T＝(k)*Δt时刻在毫米波雷达坐标系中的纵向速度；

v_{Tar_ry}(k)为目标在T＝(k)*Δt时刻在毫米波雷达坐标系中的横向速度；

所述步骤六中目标量测值匹配降噪处理依据同一个真实目标量测信息在时间历程上是连续的，但是目标量测值中的噪声干扰信息具有随机性，不具有连续性；因此通过记录一定周期内的原始目标量测值信息，分析这段点迹的连续性来区分真实目标和杂波；现将伪算法过程描述如下：

(1)初始化；

Step1：初始记录时间片段长度为N，目标量测值对应的二进制位标识对应的权值表bit_mark，以及长度位为N*M₂的表示索引号；其中M₂表示目标数目；

Step2：初始化目标量测属性值序列，分别包含相对径向距离、相对径向速度、方位角、置信度、生命周期列表Track_lifecycle、目标的ID列表，可信标记列表；

(2)历史列表维护；

Step1：删除上一周期确定的目标属性值；

Step2：历史目标列表周期性运动补偿，包括目标量测值的位置补偿和速度补偿，并更新生命周期列表Track_lifecycle的值；

Step3：采用二进制编码维护每个目标临近周期匹配状态，通过移位运算更新该标记；其中，二进制标记长度为记录时间片段的长度N，最高位表示当前周期，由高位到低位进行周期推算；

(3)新目标点存入列表；

Step1：更新当前标识位C_mark，如果值为N，更新该值为1；否则，C_mark的数值自增1；

Step2：遍历输入的属性序列，如果有效性标志位为0，转到Step2a；否则，转到Step2b；

Step2a：对于第k个输入，将生命周期列表Track_lifecycle的第(C_mark-1)*M₂+k个位置的数值置为0；对于Track位标识列表Tracklist_histrack同样处理；

Step2b：对于第k个输入，分别对每个列表的第(C_mark-1)*M₂+k个位置赋新值；

(4)目标量测值点对匹配；

Step1：初始化用于记录每个历史目标的配对点位置的变量标记match_id；

Step2：目标点配对准测为径向距离绝对差值的加权、侧向距离绝对差值的加权、径向速度绝对差值的加权、侧向速度绝对差值的加权分别与设定阈值的比较；若加权值小于设定的阈值，则将历史目标对应的match_id值置为当前目标点序号，并对目标量测值对应的其它属性值进行更新；否则，将match_id值留空；

(5)降噪目标点列表生成；

Step1：将bit_mark的值由大到小进行排序，对大于设定阈值的项，检测目标点的Id是否为设定初值0；如果是，则将全局可信目标计数自增1，并赋予该目标的Id；否则，沿用原来的Id号；

Step2：对于列表中的所有置信度为0.1并且距离小于空降目标阈值以及目标绝对速度即目标量测值的速度-本车速度的绝对值为0的项属性值置为0；

Step3：将历史列表中的目标属性值输出。

2.根据权利要求1所述一种复杂环境雷达多目标跟踪和道路行驶环境预测方法，其特征在于，所述步骤四中使用数据合理性判断规则对雷达原始目标量测值进行判断的具体依据为：

(1)目标相对于雷达的相对径向距离在雷达探测范围内，即：D_r≤d_max；

(2)目标相对于雷达的相对径向速度在雷达探测范围内，即：V_min≤V_r≤V_max；

(3)目标量测角度值在雷达探测范围内，即：θ_min≤θ_r≤θ_max。

3.根据权利要求1所述一种复杂环境雷达多目标跟踪和道路行驶环境预测方法，其特征在于，所述步骤七中道路曲率的估计首先通过本车方向盘转角来识别行驶道路为直道还是弯道，方向盘转角δ连续三个周期大于设定阈值的时候将认为弯道，否则将是直道；

弯道道路曲率的估计包括本车所在道路曲率的估计和目标车所在道路曲率的估计两部分；

所述本车所在道路曲率K₁的估计是依据横摆角速度ω、本车纵向车速v_x、方向盘转角δ、轴距L、转向系传动比i_sg进行加权估计：

其中，P为权值，P的数值和车辆行驶的速度有关，车速越高、P的数值越大；

所述目标车所在道路曲率的估计是依据经过运动补偿后目标车辆的纵向速度v′_x，横向速度v_y进行估计：

其中，

为目标车的航向角，由物理关系可得目标车辆所在的曲率K₂为：

弯道目标车辆和本车之间的横向距离D₁为：

直道目标车辆和本车之间的横向距离D₂为：D₂＝d*sinθ；

所述道路曲率识别模型目的为辨别车辆所在车道的位置关系；

(1)同车道内的目标辨识模型

(2)目标处于左车道内的辨识模型

(3)目标处于右车道内的辨识模型

其中：W为标准车道宽度；d_L为主车与左车道线之间的距离；d_R为主车与右车道线之间的距离；B为本车的宽度；M₃为车道标识线的宽度；D为D₁或者D₂。

4.根据权利要求1所述一种复杂环境雷达多目标跟踪和道路行驶环境预测方法，其特征在于，所述步骤八中曼哈顿距离进行目标聚类为：

式中，(x_i，y_i)为雷达获得的i点的目标纵向距离和侧向距离量测值；

(x_j，y_j)为雷达获得的j点的目标纵向距离和侧向距离量测值；

为雷达获得的i点的目标纵向速度、侧向速度量测值；

(V_{Tar_rx_j}，V_{Tar_ry_j})为雷达获得的j点的目标纵向速度、侧向速度量测值；

Threshol_d_x为纵向距离聚类阈值；

Threshol_d_y为横向距离聚类阈值；

Threshol_Vx为纵向速度聚类阈值；

Threshol_Vy为横向速度聚类阈值。

5.根据权利要求1所述一种复杂环境雷达多目标跟踪和道路行驶环境预测方法，其特征在于，所述步骤九中目标运动状态的辨识是依据经过运动补偿和聚类分析后得到的目标纵向速度值和本车纵向速度值之差ΔV，在连续时间片段内进行的状态进行分析辨别目标为静止、同向、反向、起停状态；

所述目标纵向速度值和本车纵向速度值之差ΔV在连续三个采样周期内在-1～1m/s之间，目标运动状态为静止，或者由同向/反向运动状态切换为起停状态；

所述目标纵向速度值和本车纵向速度值之差ΔV在连续三个采样周期内大于2m/s，目标运动状态为同向，或者由静止/起停状态切换为同向状态；

所述目标纵向速度值和本车纵向速度值之差ΔV在连续三个采样周期内小于-2m/s，目标运动状态为反向，或者由静止/起停状态切换为反向状态；

所述目标碰撞预警参数TTC为：

其中，d_x为本车与目标车辆之间的相对纵向距离；V_x为本车与目标车辆之间的相对纵向速度。

6.根据权利要求1所述一种复杂环境雷达多目标跟踪和道路行驶环境预测方法，其特征在于，所述步骤十中改进自适应扩展卡尔曼滤波是为了避免参数中的减法运算，使得出现参数值违反约束而导致算法无法继续运行的故障，降低鲁棒性；改进后的自适应扩展卡尔曼滤波算法计算过程如下：

Step1：状态预测

P(k|k-1)＝A(k)*P(k-1)*A^T(k)+Q(k)

Step2：状态校正

S(k|k-1)＝H(k)*P(k|k-1)*H^T(k)+R(k)

K(k)＝P(k|k-1)*H^T(k)*S^-1(k|k-1)

Step3：计算改进后的噪声统计估计器

其中，R₁(k+1)为改进前的测量噪声协方差；Q₁(k+1)为改进前的过程噪声协方差；d(k)是k时刻的遗忘因子；

式中：

d(k)＝(1-b)/(1-b^k+1)

R₁(k+1)＝(1-d(k))R(k)+d(k)[ε(k)*ε^T(k)-H(k)*P(k|k-1)*H^T(K)]

Q₁(k+1)＝(1-d(k))Q(k)+d(k)[K(k)ε(k)ε^T(k)K^T(k)+P(k)-A(k)P(k-1)A^T(k)]

其中，d(k)是k时刻的遗忘因子；b为[0.95，0.995]范围内的遗忘因子；ε(k)为k时刻的新息，z(k)为k时刻的系统观测值，

为k-1时刻系统状态预测的观测值，R(k)为k时刻的测量噪声协方差；H(k)为K时刻观测矩阵，P(k|k-1)为k时刻误差协方差矩阵预测值，Q(k)为k时刻过程噪声协方差矩阵，P(k)为k时刻误差协方差，P(k-1)为k-1时刻误差协方差，A(k)k时刻状态转移矩阵，K(k)为k时刻自适应扩展卡尔曼增益矩阵；

所述目标航迹进行跟踪规则是指以被跟踪目标预测位置为中心，用来确定来自该目标的量测可能出现的区域，采用最近邻椭圆形跟踪门实现目标量测值和航迹之间的数据关联；如下构建椭圆形跟踪门最近邻数据关联方法：

Step1：计算新息的协方差矩阵

S_k＝E(ε(k)ε^T(k))

Step2：计算统计距离

式中：ε(k)为k时刻的新息，

为k-1时刻系统状态预测的观测值，S_k为k时刻新息协方差矩阵；

若统计距离满足下面的式子，则表明目标量测值可以用来和航迹进行匹配关联，否则不能用来和航迹进行数据关联；

式中：

为统计距离，λ为标定参数；

跟踪起始逻辑是依据目标的生命周期判断目标量测值进入自适应扩展卡尔曼滤波迭代时，状态属性值取值的依据；

所述状态属性值包括目标运动的横向距离、纵向距离、横向速度、纵向速度、横向加速度、纵向加速度；

若目标的生命周期为0，则将目标运动状态属性值初始为0；

若目标的生命周期为1，表明该目标为新目标，目标运动状态属性值中横向距离、纵向距离、横向速度、纵向速度为当前时刻目标量测值，横向加速度和纵向加速度为0；

若目标的生命周期不为0或者1，则目标运动状态属性值中横向距离、纵向距离、横向速度、纵向速度为上一时刻目标量测值，横向加速度和纵向加速度为当前时刻和上一时刻的速度差和时间的比值。

7.根据权利要求1所述一种复杂环境雷达多目标跟踪和道路行驶环境预测方法，其特征在于，所述步骤十一中所述道路隔离带识别是通过计算每一个静止障碍物到本车的横向距离D_i，如果D_min≤D_i≤D_max则认为该静止障碍物可能是道路隔离带的一部分；同时记录落入设定区间内的障碍物的个数T，当T超过了设定的阈值时，认为静止的障碍物为道路隔离带；通过计算落入特定横向距离范围内的静止目标个数T，来判别道路隔离带，将道路隔离带所在的位置标识为不可行驶区域；

所述道路属性识别是通过目标车辆的绝对运动速度识别道路为同向车道、反向车道、不可行使区域；所述道路属性识别在每一次转动方向盘之后将会置为初始状态；

如果所述目标车辆的绝对运动速度在连续三个采样周期内大于2m/s，认为目标车辆为同向运动，同时结合道路曲率识别模型辨识得到的目标所在道路位置将对应的车道标识为同向车道；如果所述目标车辆绝对运动速度在连续三个采样周期内小于-2m/s，认为目标车辆为反向运动，同时结合道路曲率识别模型辨识得到的目标所在道路位置将对应的车道标识为反向车道；通过计算落入特定横向距离范围内的静止目标个数T，可以判别道路隔离带，将道路隔离带所在的位置标识为不可行驶区域；

所述步骤十二中关键的目标的筛选是指依据本车和目标车之间的横向距离D，分别在本车道、左车道、右车道内将目标运动状态为同向或者起停状态的目标依据相对距离最近和TTC最小原则，筛选出自适应巡航控制系统或者自主代客泊车系统跟踪的目标。

Images