一种汽车巡航系统关键目标识别方法

摘要

本发明公开了一种汽车巡航系统关键目标识别方法，基于模糊控制理论，设计了多目标车换道模糊逻辑控制器，预判危险换道车辆，确定了直道关键目标的有效识别。考虑车辆弯道行驶的侧倾运动影响，实现汽车质心侧偏角与道路曲率的扩展卡尔曼滤波在线实时估计，提出了弯道关键目标识别方法给定了关键目标判定依据，实现了弯道关键目标的有效识别。

权利要求

1.一种汽车巡航系统关键目标识别方法，其特征在于，包括：

在直道行驶中，采用模糊控制方法对前方车辆进行预换道预测：分别将相邻车道前方车辆与本车侧向相对距离、相对速度以及换道概率转换为模糊论域中的量化等级；

将所述相对距离以及相对速度输入模糊控制模型，所述相对距离分为5个等级，所述相对速度分为5个等级；

模糊控制模型输出为换道概率，将所述换道概率分为5个等级；

在本车车道内前方车辆和在相邻车道内前方车辆换道概率达到设定阈值的车辆中与本车纵向距离最近的车辆为关键目标；

在弯道行驶中，根据前方车辆相对本车车道中心侧向距离doff_i，确定前方车辆是否位于本车车道内，其判别依据如下：若doff_i>0且|doff_i|>dth，前方车辆位于本车左侧车道内；

若doff_i<0且|doff_i|>dth，前方车辆位于本车右侧车道内；

若|doff_i|≤dth，前方车辆位于本车道内；

式中，dth为设定的阈值，取半车道宽；

前方车辆中在本车车道内并且与本车纵向距离最近的车辆为关键目标。

2.如权利要求1所述的汽车巡航系统关键目标识别方法，其特征在于，所述相对距离的论域为{0，4}，所述相对速度的论域为{0，1.4}，所述换道概率的论域为{0，1}，所述阈值为

0.51。

3.如权利要求2所述的汽车巡航系统关键目标识别方法，其特征在于，所述相对距离分为5个等级，模糊集为{N，NM，M，ML，L}；所述相对速度分为5个等级，模糊集为{L，LM，M，MH，H}；所述换道概率分为5个等级，模糊集为{S，SM，M，MB，B}；隶属函数均选用梯形隶属函数。

4.如权利要求3所述的汽车巡航系统关键目标识别方法，其特征在于，所述模糊控制模型中控制规则为：当目标车与本车的相对距离输入为N，相对速度输入为MH，则目标车换道概率输出为B，即目标车换道；

当目标车与本车的相对距离输入为L，相对速度输入为LM，则目标车换道概率输出为S，即目标车未换道；

当目标车换道概率输出为S或SM，则该目标车未实现换道；当目标车换道概率输出为B或MB，则该目标车实现换道；当目标车换道概率输出为M，即该目标车换道概率为换道阈值。

5.如权利要求1所述的汽车巡航系统关键目标识别方法，其特征在于，包括如下步骤：建立非线性三自由度汽车运动微分方程；

将非线性三自由度微分方程进行线性化；

建立关于质心偏向角和横摆角速度的状态方程与量测方程，将质心侧偏角与横摆角速度进行线性化并迭代至扩展卡尔曼滤波方程，获得质心偏向角和横摆角速度的最优估计值，进而得到道路曲率半径；

根据所述道路曲率得到所述目标车辆相对本车车道中心侧向距离doff_i；

其中，所述目标车辆相对本车车道中心侧向距离 其中，

式中，dri为本车与目标车辆的雷达测量距离；θi

为本车与目标车辆各自中轴线间的夹角，设定θi在右转弯道时，雷达中轴线向回转中心一侧方向为正，背离回转中心一侧方向为负，左转弯道时与规定方向相反； 为巡航车所在道路曲率半径；dVW为巡航车宽度；dsi为目标车辆的行驶轨迹曲率半径。

6.如权利要求5所述的汽车巡航系统关键目标识别方法，其特征在于，所述卡尔曼滤波器的状态方程为：xk＝xk-1+[M]-1[K]xk-1·△k+[M]-1[N]δk-1△k+[M]-1wk-1·△k，式中，xk为实际的状态变量；wk为过程噪声，Iz为整车绕z轴的转动惯量，Ixz为悬架质量绕x、z两轴的惯性积；V为汽车车速；M为整车质量；Ms为悬架质量；Q为过程噪声协方差矩阵；W表示非线性函数与对状态变量求偏导的雅克比矩阵；h为侧倾力臂；Lp＝-(Cφf+Cφr)；Lφ＝Msgh-(Kφf+Kφr)；Cφf、Cφr分别为前、后悬架侧倾角阻尼；Kφf、Kφr分别为前、后悬架侧倾角刚度。

7.如权利要求6所述的汽车巡航系统关键目标识别方法，其特征在于，所述卡尔曼滤波方程的量测方程为yk＝Cxk-1+Iνk-1，式中，yk为观测变量，vk为观测噪声，wk和vk二者为相互独立且具有正态分布的高斯白噪声序列，I为单位矩阵，C＝I4×4。

说明书

一种汽车巡航系统关键目标识别方法

技术领域

[0001] 本发明涉及汽车系统动力学操纵稳定性控制领域，具体涉及一种汽车巡航系统关键目标识别方法。

背景技术

[0002] 汽车自适应巡航控制系统(ACC)主要依靠雷达或激光传感器作为信息输入，可自动调整巡航车车速并维持与其车道前方车辆的安全距离，是较典型的汽车主动安全辅助驾驶系统之一。该系统在一定程度上实现了安全跟车的目的，大大降低了交通事故率。但该系统仍存在与前车发生碰撞危险的可能性。比如：系统对相邻车道的危险车辆预汇入或突然闯入本车道的情形不能提前做出预判，仍跟随本车道原有目标行驶，必然会存在巡航车与汇入车辆发生碰撞的危险。尤其是针对城市道路行车环境，车流密集且相互交织与换道行为较频繁且存在转弯路段，所以，发生碰撞的危险性会更大。从车辆在道路上的运行过程来看，巡航车前方通常会存在多个目标车辆，从多个目标车辆中确定出唯一的危险目标车(被视为关键目标)对于系统的控制策略来讲是非常关键的。而且，当自车在车道内的位置发生波动，而前方车辆也相应的发生波动时，预准确识别关键目标，其识别难度会大大提高。

[0003] 在弯道路段，汽车自适应巡航控制系统经常会出现目标车混乱或丢失的现象。在转弯路段准确识别并跟踪关键目标，对于完善或改进ACC系统性能具有重要指导意义和实际应用价值。如果能够准确估计出前方弯道路段的道路曲率，就能判别弯道目标相对本车行驶轨迹中心线的侧向距离，进而确定本车道前方的关键目标。而汽车的侧向运动特征在实际中是不能通过传感器等直接测量得到的，但可以通过汽车质心侧偏角进行转换得到汽车的侧向速度。因此，对汽车质心侧偏角与横摆角速度进行准确估计是十分必要的。

发明内容

[0004] 本发明设计开发了一种汽车巡航系统关键目标识别方法，目的是在汽车直道行驶使用自适应巡航控制系统的过程中，系统对相邻车道的危险车辆预汇入或突然闯入本车道的情形提前做出预判，重新确定巡航跟踪车辆。

[0005] 本发明的另一个发明目的是在汽车弯道行驶使用自适应巡航控制系统的过程中，解决在弯道路段出现目标车辆混乱或者丢失的现象，通过本发明能够使本车在转弯路段对前方多个目标车辆中进行准确识别巡航跟踪车辆。

[0006] 本发明提供的技术方案为：

[0007] 一种汽车巡航系统关键目标识别方法，包括：

[0008] 在直道行驶中，采用模糊控制方法对前方车辆进行预换道预测：分别将相邻车道前方车辆与本车侧向相对距离、相对速度以及换道概率转换为模糊论域中的量化等级；

[0009] 将所述相对距离以及相对速度输入模糊控制模型，所述相对距离分为5个等级，所述相对速度分为5个等级；

[0010] 模糊控制模型输出为换道概率，将所述换道概率分为5个等级；

[0011] 在本车车道内前方车辆和在相邻车道内前方车辆换道概率达到设定阈值的车辆中与本车纵向距离最近的车辆为关键目标；

[0012] 在弯道行驶中，根据前方车辆相对本车车道中心侧向距离doff_i，确定前方车辆是否位于本车车道内，其判别依据如下：

[0013] 若doff_i＞0且|doff_i|＞dth，前方车辆位于本车左侧车道内；

[0014] 若doff_i＜0且|doff_i|＞dth，前方车辆位于本车右侧车道内；

[0015] 若|doff_i|≤dth，前方车辆位于本车道内；

[0016] 式中，dth为设定的阈值，取半车道宽；

[0017] 前方车辆中在本车车道内并且与本车纵向距离最近的车辆为关键目标。

[0018] 优选的是，所述相对距离的论域为{0，4}，所述相对速度的论域为{0，1.4}，所述换道概率的论域为{0，1}，所述阈值为0.51。

[0019] 优选的是，所述相对距离分为5个等级，模糊集为{N，NM，M，ML，L}；所述相对速度分为5个等级，模糊集为{L，LM，M，MH，H}；所述换道概率分为5个等级，模糊集为{S，SM，M，MB，B}；隶属函数均选用梯形隶属函数。

[0020] 优选的是，所述模糊控制模型中控制规则为：

[0021] 当目标车与本车的相对距离输入为N，相对速度输入为MH，则目标车换道概率输出为B，即目标车换道；

[0022] 当目标车与本车的相对距离输入为L，相对速度输入为LM，则目标车换道概率输出为S，即目标车未换道；

[0023] 当目标车换道概率输出为S或SM，则该目标车未实现换道；当目标车换道概率输出为B或MB，则该目标车实现换道；当目标车换道概率输出为M，即该目标车换道概率为换道阈值。

[0024] 优选的是，包括如下步骤：

[0025] 建立非线性三自由度汽车运动微分方程；

[0026] 将非线性三自由度微分方程进行线性化；

[0027] 建立关于质心偏向角和横摆角速度的状态方程与量测方程，将质心侧偏角与横摆角速度进行线性化并迭代至扩展卡尔曼滤波方程，获得质心偏向角和横摆角速度的最优估计值，进而得到道路曲率半径；

[0028] 根据所述道路曲率得到所述目标车辆相对本车车道中心侧向距离doff_i；

[0029] 其中，所述目标车辆相对本车车道中心侧向距离 其中，式中，dri为本车与目标车辆的雷达测量距离；θi
为本车与目标车辆各自中轴线间的夹角，设定θi在右转弯道时，雷达中轴线向回转中心一侧方向为正，背离回转中心一侧方向为负，左转弯道时与规定方向相反； 为巡航车所在道路曲率半径；dVW为巡航车宽度。

[0030] 优选的是，所述卡尔曼滤波器的状态方程为：xk＝xk-1+[M]-1[K]xk-1·Δk+[M]-1[N]δk-1Δk+[M]-1wk-1·Δk，式中，xk为实际的状态变量；wk为过程噪声，Iz为整车
绕z轴的转动惯量，Ixz为悬架质量绕x、z两轴的惯性积、Δk为采样时间。

[0031] 优选的是，所述卡尔曼滤波方程的量测方程为yk＝Cxk-1+Iνk-1，式中，yk为观测变量，vk为观测噪声，wk和vk二者为相互独立且具有正态分布的高斯白噪声序列，I为单位矩阵，C＝I4×4。

[0032] 优选的是，所述卡尔曼滤波方程中还包括：

[0033] 状态预测方程：

[0034] 误差协方差预测方程：

[0035] 式中，Ak为k时刻的状态转移矩阵，uk-1为控制变量，Ak＝[M]-1[K]，Bk＝[M]-1[N]，Qk为k时刻过程噪声的协方差矩阵。

[0036] 优选的是，还包括误差校正过程：

[0037] 增益矩阵：

[0038] 由观测变量更新估计：

[0039] 误差协方差更新方程：

[0040] 式中，Hk和Vk是k时刻测量的雅可比矩阵，Rk为k时刻观测噪声的协方差矩阵。

[0041] 本发明与现有技术相比较所具有的有益效果：发明考虑侧倾运动影响，建立汽车质心运动、横摆运动及侧倾运动具有非线性特征的运动微分方程，设计汽车质心侧偏角与横摆角速度扩展卡尔曼滤波估计器，实现了汽车质心侧偏角与道路曲率的在线实时估计，建立了弯道关键目标识别模型并给定了关键目标判定依据，实现了弯道关键目标的有效识别，并且还采用模糊控制理论，设计了在直道行驶时多目标车换道模糊逻辑控制模型，预判危险换道车辆，确定了直道行驶的关键目标，通过对关键目标的确定使车辆对相邻的危险车辆预汇入或者突然闯入本车道的情形能够提前做出预判，减少碰撞危险，并且，在转弯路段同样能够准确识别并跟踪关键目标，因此对自适应巡航控制系统做出了更好的改进。

附图说明

[0042] 图1a为本发明所述的侧向相对距离的隶属函数图。

[0043] 图1b为本发明所述的侧向相对速度的隶属函数图。

[0044] 图1c为本发明所述的换道概率的隶属函数图。

[0045] 图2为本发明所述的目标车辆距本车道中心线的距离仿真结构。

[0046] 图3为本发明所述的目标车辆换入本车道概率仿真结果。

[0047] 图4为本发明所述的换道概率临界值。

[0048] 图5为本发明所述的考虑侧倾运动的非线性三自由度汽车操纵稳定性模型。

[0049] 图6为本发明所述的考虑侧倾运动的非线性三自由度汽车操纵稳定性模型。

[0050] 图7为本发明所述的非线性三自由度汽车操纵稳定性模块。

[0051] 图8a为本发明所述的横摆角速度结果对比图。

[0052] 图8b为本发明所述的质心偏向角结果对比图。

[0053] 图9为本发明所述的本车道内弯道目标车辆识别。

[0054] 图10为本发明所述的目标车距本车行驶轨迹中心线距离仿真结果。

[0055] 图11为本发明所述的弯道目标车换道后侧向距离变化仿真结果。

具体实施方式

[0056] 下面结合附图对本发明做进一步的详细说明，以令本领域技术人员参照说明书文字能够据以实施。

[0057] 实施例1

[0058] 本发明提供了一种汽车巡航系统关键目标识别方法，包括：对前方车辆进行预换道判断，如果前方车辆中不存在预换道车辆，则关键目标为本车道前方的车辆，如果前方车辆中存在预换道车辆，则关键目标为预换道车辆与本车道前方的车辆中与本车纵向相对距离最近的车辆。

[0059] 汽车在直道上行驶时，主要分为两种情况，一是巡航车(称本车)前方车辆如果均按照各自车道运行，不存在预换道车辆，则关键目标就是本车道前方的车辆。二是前方相邻车道车辆具有换入本车道的趋势，则关键目标要从预换道的多个目标车辆及本车道前方目标车辆中产生，则关键目标应为上述多目标车中距本车纵向相对距离最小者。

[0060] 在本发明中，采用模糊控制方法对预换道车辆进行预测并确定关键目标车辆：模糊控制模型分别以相邻车道前方目标车与本车侧向相对距离(LD)及侧向相对速度(LV)为输入变量，以换道概率(CP)为输出变量，实现关键目标的确定；如果换道概率达到设定阈值，判断为换入本车道；在本实施例中阈值为0.51。

[0061] 输入变量LD与LV论域分别为{0,1.8,2.15,2.5,2.85,3.2,4}与{0,0.2,0.45,0.7,0.95,1.2,1.4}，输出变量CP论域为{0,0.2,0.35,0.5,0.65,0.8,1}；输入变量LD的模糊语言集为：N(近)，NM(较近)，M(中等)，ML(较远)，L(远)，输入变量LV的模糊语言集为：L(低)，LM(较低)，M(中等)，MH(较高)，H(高)，输出变量CP的模糊语言集为：S(小)，SM(较小)，M(中等)，MB(较大)，B(大)；LD的量化因子αLD＝1.0，LV的量化因子αLV＝1.0/3.6，CP的量化因子αCP＝1.0，LD与LV的隶属函数采用梯形隶属函数(如图1a、1b所示)；CP的隶属函数也采用梯形隶属函数(如图1c所示)

[0062] 如表1所示，给出了目标车辆换道模糊逻辑控制模型的模糊控制规则，即：用模糊语言描述控制器输入变量(LD与LV)与输出变量(CP)间的关系。

[0063] 表1模糊控制规则

[0064]

[0065] 模糊控制模型中控制规则为：

[0066] 如果目标车与本车的侧向相对距离(LD)为“近(N)”，侧向相对速度(LV)为“较高(MH)”，则目标车换道概率为“大(B)”，即目标车一定实现换道。

[0067] 如果目标车与本车的侧向相对距离(LD)为“远(L)”，侧向相对速度(LV)为“较低(LM)”，则目标车换道概率为“小(S)”，即目标车未换道。

[0068] 也就是说，如果目标车换道概率为“小(S)或较小(SM)”，则该车目标未实现换道；如果目标车换道概率为“大(B)或较大(MB)”，则该目标车实现换道；如果目标车换道概率为“中等(M)”，即：该目标车换道概率为换道阈值，此种情况，如果目标车的运行状态稍有变化，则必然会形成换道或未进行换道这两种情况的切换。

[0069] 结果分析

[0070] 在Carsim中构建了直道工况的仿真环境：车道宽4m，本车以20m/s的速度匀速行驶。前方存在与本车同速行驶的三个目标车，其中目标车辆1距本车纵向距离50m，位于左侧车道；目标车辆2距本车纵向距离80m，位于右侧车道；目标车辆3距本车纵向距离90m，位于本车道内。

[0071] 初始状态5s时，目标车辆1开始以1m/s的横向速度向右切入本车道，9s时完成换道；目标车辆2以0.5m/s的速度向左运动持续时间2s但最终没有换道。

[0072] 如图2、图3所示，通过仿真结果可以得知，目标车辆1在5s时开始发生侧向位移的变化，与本车道中心线的距离逐渐缩小，在8s左右换入到本车道行驶，目标车2虽然也发生了侧向位移的变化，但最终距离本车道中心线的距离2m以上，所以该目标只是靠近本车道但并未换入本车道行驶。图3给出了目标车1(切入车辆)与目标车2(未切入车辆)的换道概率，目标1的换道概率大于临界值0.51，所以进行了换道，而目标车2的换道概率小于临界值0.51，所以未进行换道；该结果与图2的结论及仿真环境是吻合的，说明所设计的目标车换道模糊控制器是合理的。

[0073] 关于换道概率临界值，本发明是通过多次仿真实验获得该值，图4为换道概率临界值仿真结果，从图4中可以得知，当目标车换道概率达到阈值0.51时便实现了换道，当目标车换道概率小于0.51时未能实现换道。

[0074] 鉴于此，本发明采用模糊控制方法，对直道行驶工况预换道车辆进行提前判断。将相邻车道前方目标车与本车侧向相对距离(LD)及相对速度(LV)为基础，设定目标车换道的模糊规则，以换道阈值为依据，有效地确定了关键目标车辆。

[0075] 实施例2

[0076] 本发明提供了一种汽车巡航系统关键目标识别方法，包括：判断前方弯道中的车辆是否位于本车车道内，当前方弯道内的车辆有且只有一辆车位于本车车道内时，所述目标车辆为该车辆，当前方弯道内的车辆有且多于一辆位于本车车道内时，所述目标车辆为与本车纵向距离最近的车辆；

[0077] 其中，根据目标车辆相对本车车道中心侧向距离doff_i，确定目标车辆是否位于本车车道内，其判别依据如下：

[0078] 若doff_i＞0且|doff_i|＞dth，则：目标车辆位于本车左侧车道内；

[0079] 若doff_i＜0且|doff_i|＞dth，则：目标车辆位于本车右侧车道内；

[0080] 若|doff_i|≤dth，则：目标车辆位于本车道内；

[0081] 式中，dth为设定的阈值，取半车道宽。

[0082] 为了如实反映车辆在弯道路段的行驶状态，本发明考虑车辆弯道行驶的侧倾运动影响，建立了反映汽车质心运动、横摆运动及侧倾运动等具有非线性特征的运动微分方程，设计了横摆角速度与质心侧偏角扩展卡尔曼滤波估计器，对横摆角速度进行实时估计，从而获得巡航车前方弯道路段道路曲率半径，结合关键目标判定依据进行弯道关键目标的有效识别。

[0083] 如图5、图6所示，V为汽车车速；M为整车质量；Ms为悬架质量；ωr为横摆角速度；β为质心侧偏角；βf、βr分别为前后轮轮胎的侧偏角(假定左、右轮胎侧偏角相等)；δ为前轮转向角；φ为悬架的侧倾角；kf、kr分别为前后轮胎的侧偏刚度；lf、lr分别为前后轴至质心的距离；df、dr分别为前后轮的轮距；Cφf、Cφr分别为前、后悬架侧倾角阻尼；Kφf、Kφr分别为前、后悬架侧倾角刚度；Iz为整车绕Z轴的转动惯量；Ixc为悬架质量绕车身重心纵轴的转动惯量；Ixz为悬架质量绕X、Z两轴的惯性积；hs为侧倾力臂，αf、αr为前后轮的侧倾转向角(假定其与车身侧倾角成正比)； 为车轮的滚动阻力系数。

[0084] 基于非线性三自由度汽车运动模型，得到包含汽车质心运动、横摆运动及侧倾运动的微分方程如下：

[0085] Y方向的受力平衡方程

[0086] 绕Z轴的力矩方程为

[0087] 绕X轴的力矩方程为

[0088] 其中，在(1)、(2)、(3)各式中：

[0089] Wβ＝2(kf+kr)， Wδ＝-2kf，Lp＝-(Cφf+Cφr)，Lφ＝Msgh-(Kφf+Kφr)，其中，
Fyr＝krβr，
Ixc+Msh2＝Ix；

[0090] 在上述各式中， 为前轮外倾侧倾刚度系数；Ef、Er分别为前、后轴侧倾转向系数， Fyf分别为左前轮、右前轮及前轮等效侧向力； Fyr分别为左后轮、右后轮及后轮等效侧向力；h为轮轴距离地面的高度；hf、hr分别为前后轴距离地面的高度；V为汽车车速；M为整车质量；Ms为悬架质量；ωr为横摆角速度；β为质心侧偏角；βf、βr分别为前后轮轮胎的侧偏角(假定左、右轮胎侧偏角相等)；δ为前轮转向角；φ为悬架的侧倾角；kf、kr分别为前后轮胎的侧偏刚度；lf、lr分别为前后轴至质心的距离；df、dr分别为前轮以及后轮之间的轮距；Uf、Ur分别为前后回正力矩刚度系数；Cφf、Cφr分别为前、后悬架侧倾角阻尼；
Kφf、Kφr分别为前、后悬架侧倾角刚度；Iz为整车绕Z轴的转动惯量；Ixc为悬架质量绕车身重心纵轴的转动惯量；Ixz为悬架质量绕X、Z两轴的惯性积；hs为侧倾力臂，g为重力加速度，αf、αr为前后轮的侧倾转向角(假定其与车身侧倾角成正比)； 为车轮的滚动阻力系数。

[0091] 为便于进行扩展卡尔曼滤波处理，需要将三自由度模型的汽车运动微分方程整理成状态方程形式，如下所示。

[0092]

[0093] 式中：

[0094]

[0095]

[0096] 进一步整理得到：

[0097]

[0098] 整理成状态方程形式

[0099]

[0100] 其中，[A]＝[M]-1[K]，[B]＝[M]-1[N]，[C]＝I4×4，[D]＝I4×1，[E]＝[M]-1I，δ为前轮转向角矩阵；

[0101] 根据估计对象建立状态方程与量测方程，对非线性模型进行线性化并赋初值进行递推估计，主要包括预测过程与校正过程，其具体过程如下：

[0102] (1)建立状态方程与量测方程：

[0103] xk＝xk-1+[M]-1[K]xk-1·Δk+[M]-1[N]δk-1Δk+[M]-1wk-1·Δk

[0104] yk＝Cxk-1+Iνk-1

[0105] 式中，xk为实际的状态变量，yk为观测变量；wk为过程噪声，vk为观测噪声，Δk为采样时间，wk和vk二者为相互独立且具有正态分布的高斯白噪声序列。

[0106] 假定过程噪声wk和观测噪声vk为零，从而估计状态向量和观测向量，得到：

[0107]

[0108]

[0109] 式中，是状态相对于前一时刻的后验估计。

[0110] (2)非线性三自由度微分方程进行线性化，得到新的控制方程：

[0111]

[0112]

[0113] 其中，

[0114] 式中，F、W、H、V分别表示非线性函数f(xk,uk,wk)与g(xk,uk)对状态变量求偏导的雅可比矩阵；

[0115] (3)扩展卡尔曼滤波算法递推步骤

[0116] 扩展卡尔曼滤波算法递推步骤主要包括预测与校正两个过程，并给定初值即可实现。

[0117] ①预测过程

[0118] (a)状态预测方程： 式中， 为k时刻状态变量的估计值， 为k+1时刻状态变量的预估计值，Ak为状态转移矩阵；

[0119] 求解状态转移矩阵：Ak＝eF(k)*Δk≈I+F(k)*Δk，式中，Δk为采样时间，I为单位矩阵；

[0120] 计算预测误差：

[0121] 观测变量的残差为：

[0122] 式中，εk和ηk分别为具有零均值，协方差矩阵为WQWT和VRVT的独立所及变量，Q为过程噪声协方差矩阵，R为观测噪声协方差矩阵。

[0123] 将估计结果记为 获得后验状态估计，即 令估计值为 得到式中，Kk为卡尔曼增益。

[0124] (b)误差协方差预测方程： 式中，Qk为k时刻过程噪声的协方差矩阵。

[0125] ②校正过程，测量更新方程为：

[0126] (a)增益矩阵：

[0127] (b)由观测变量更新估计：

[0128] (c)误差协方差更新方程：

[0129] 式中，Hk和Vk是k时刻测量的雅可比矩阵，Rk为k时刻观测噪声的协方差矩阵。

[0130] 算法初值设置：车速为120km/h，采样时间间隔Δk＝0.01s。状态变量X(0)＝[0，0，0，0]，误差协方差矩阵P(0)＝eye(4)，过程噪声协方差矩阵Q＝eye(4)*0.001，量测噪声协方差矩阵R＝eye(4)*0.01；方向盘转角信号采用双移线输入，通过Carsim进行导入。

[0131] 鉴于此，本发明考虑车辆在实际弯道行驶过程中侧倾运动的影响，建立非线性三自由度汽车模型(如图7所示)，设计了汽车质心侧偏角与横摆角速度的扩展卡尔曼滤波器模型，利用Carsim导入方向盘转角信号δ(如：双移线输入)的实测输入数据，进行了汽车横摆角速度的准确估计，本发明得到的汽车横摆角速度的估计值可代替实测值，可消除传感器测量噪声的影响而实现弯道曲率的准确估计。根据本发明给定的弯道关键目标识别模型及其判别依据，可有效地识别弯道关键目标。

[0132] 结果分析

[0133] 如图8a、图8b所示，用双移线输入验证了车速为80km/h的汽车横摆角速度与质心侧偏角估计，得到横摆角速度和质心侧偏角的估计值(EKF value)与实际值(Actual value)对比结果；从图中的对比结果可以看出，利用扩展卡尔曼滤波估计的车辆横摆角速度和质心侧偏角与实际值基本是吻合的，可以获得理想的估计效果，这表明：考虑侧倾运动的非线性三自由度汽车模型而建立的汽车行驶状态扩展卡尔曼滤波估计器，可以有效地实现汽车行驶过程中横摆角速度和质心侧偏角的实时估计，该研究方法和结论对于汽车智能巡航控制或避撞控制系统关键目标识别中弯道道路曲率估计具有重要参考价值。

[0134] 如图9所示，巡航车雷达检测到前方弯道上存在多个目标车，需要从中判定哪些车辆位于本车道内，进而确定关键目标。

[0135] 巡航车在弯道上行驶，安装于车上的惯性传感器实时输出横摆角速度数据，而该值是由道路的曲率ρ和车速u共同确定的，理想状态下，车辆在接近圆弧状态的道路行驶，则存在如下关系： 以本车前方的目标车辆i为研究，已知车辆右转弯行驶，目标车辆i的行驶轨迹曲率半径dsi可由式(3)得到：
巡航车相对于目标车辆i的侧向偏移距离doff_i为 式中，式中：dri
为巡航车与目标车辆i的雷达测量距离(m)；θi为巡航车与目标车辆i各自中轴线间的夹角(可由雷达测量的方向角得到)，设定θi在右转弯道时，雷达中轴线向回转中心一侧方向为正，背离回转中心一侧方向为负，左转弯道时与上述规定方向相反； 为巡航车所在道路曲率半径；u为汽车纵向车速；dVW为巡航车宽度。

[0136] 根据目标车辆i相对本车道中心线侧向距离doff_i，确定目标车辆i是否位于本车道内，其判别依据如下：

[0137] (1)若doff_i＞0且|doff_i|＞dth，则：目标车辆i位于本车左侧车道内；

[0138] (2)若doff_i＜0且|doff_i|＞dth，则：目标车辆i位于本车右侧车道内；

[0139] (3)若|doff_i|≤dth，则：目标车辆i位于本车道内；

[0140] 其中：dth为设定的阈值(m)，取半车道宽，也可以根据实际情况设定其它值。

[0141] 结果分析

[0142] 在CarSim中建立实验车辆与道路模型，雷达可测取前方100m处目标，车道宽4m，巡航车沿本车道中心线附近以20m/s的速度匀速运行。目标车1、目标车2、目标车3分别以20m/s的速度距离巡航车60m、80m与90m匀速行驶，其中目标车1位于左侧车道，目标车2位于右侧车道，目标车3位于本车道，在目标车2行驶一段时间后进行了换道，而目标车1与目标车3均按原车道行驶。

[0143] 从图10中得知：由弯道关键目标识别方法得出目标车1与目标车2距本车行驶轨迹中心线的侧向距离分别为3.75m与3.2m，目标3距本车道中心线的侧向距离为0.9m左右，结果表明：只有目标3位于本车道内，为关键目标。而目标车1与目标车2均不在本车道内，即：目标车1位于左侧车道，目标车2位于右侧车道，这个结果与道路模型结果是吻合的。

[0144] 从图11中得知：在5s内，目标识别方法得出目标车1、2、3距本车行驶轨迹中心线侧向距离分别为3.75m、3.2m和0.8m左右，此时的关键目标为目标车3。5s时，目标车2开始换道，距本车行驶轨迹中心线距离逐渐变小，4s后完成换道。目标车距本车行驶轨迹中心线0.8m左右，此时的目标车2被判定为关键目标(在本车道内且距离本车纵向距离最近)。

[0145] 尽管本发明的实施方案已公开如上，但其并不仅仅限于说明书和实施方式中所列运用，它完全可以被适用于各种适合本发明的领域，对于熟悉本领域的人员而言，可容易地实现另外的修改，因此在不背离权利要求及等同范围所限定的一般概念下，本发明并不限于特定的细节和这里示出与描述的图例。