CN109459047B - 车辆高精度定位数据与导航地图精确匹配及变道行为探测方法 - Google Patents

Abstract

本发明涉及一种车辆高精度定位数据与导航地图精确匹配及变道行为探测方法，基于车道边界线组织的车道级导航地图数据结构模型；首先对导航地图数据进行组织并导入定位轨迹数据，采用射线法对每个定位轨迹点与车道进行初步匹配；进一步地，发展基于Delaunay三角网的匹配方法对复杂道路区域匹配失败的定位轨迹点进行二次匹配探测；最后，依据每个定位轨迹点的匹配车道信息探测‘一次变道’、‘连续变道’、‘驾驶偏离’三种不同类型的车辆变道行为。本发明通过数据结构组织、匹配算法构建、变道行为分类及识别三个方面的创新，构建定位轨迹数据精确匹配与变道行为识别模型，对车辆行为监测、交通事故分析及理赔等应用有意义。

Description

车辆高精度定位数据与导航地图精确匹配及变道行为探测 方法

技术领域

本发明属于地理信息领域，尤其涉及车辆高精度定位数据与导航地图间的精确匹配，以及在此基础上探测移动车辆的变道行为。

背景技术

随着移动互联网及各种类型移动传感器载体(如车辆、手机等)的普及，描述各种移动对象(如行人、车辆、野生动物)的轨迹数据成为当前测绘地理大数据的重要组成部分。车辆行驶轨迹线通常由一系列按时间序列组织的位置坐标组成，描述移动车辆在一定时间范围内的运动轨迹。车辆轨迹数据不仅记录了车辆在地理空间中的几何移动位置，而且蕴含了速度、加速度、转向、变道等时空行为特征，是交通导航领域关注的重要数据对象。利用轨迹数据分析移动车辆的时空行为，首先需要解决定位轨迹数据与交通道路数据间的匹配问题。这是由于定位轨迹数据本身受定位系统软硬件设备条件、车辆行驶环境(如障碍物物遮挡)影响存在定位误差，同时道路数据采集建库本身也存在空间误差，从而导致定位轨迹点与道路对象发生偏离，影响轨迹数据时空分析的准确性。常规的定位轨迹数据匹配方法通常基于单线道路实体，表现为定位轨迹点与表达道路的单线目标间的匹配。随着无人驾驶等新型交通导航应用的不断发展，高精度定位轨迹数据及车道级导航地图越来越受到重视，在此背景下车道级分辨率下的定位轨迹数据匹配问题亟需解决。针对这一问题，本发明通过构建新的车道级道路数据组织模型、定位轨迹点与车道间精确匹配方法、以及变道行为分类及探测三个方面的创新，构建定位轨迹数据精确匹配与变道行为识别模型。本发明对车辆行为监测、交通事故分析及理赔等应用有意义。

发明内容

针对上述存在的应用问题，本发明设计了一种高精度定位数据和导航地图间的精确匹配方法，以及在此基础上实现探测车辆变道行为。本发明的核心思想是依据定位点与车道线的相对位置进行快速车道匹配，对于局部复杂路段通过由道路线构造的道路面进行点位匹配，最后基于匹配结果探测车辆的变道行为。

本发明所采用的技术方案是：一种车辆高精度定位数据与导航地图精确匹配及车辆变道行为探测方法，其特征在于由车道级导航地图的数据组织模型、定位数据与导航地图要素间的精确匹配、变道行为探测三个方面。具体是基于车道级导航地图数据组织模型，具体定义是道路实体组织为车道边界线集合L＝{l₁,l₂,…,l_m}(m≥2)；对于每条车道边界线l_i(1≤i≤m)，采用9元组结构表示：

{Id,Path,Layer,LLane_No,L_Name,L_Function,RLane_No,R_Name,R_Function}

其中，Id表示为车道边界线在数据库系统的唯一编号；

Path记录车道边界线的坐标点信息；

Layer表示车道边界线的功能类型；

LLane_No表示车道边界线左侧车道编号；

L_Name表示车道边界线左侧车道名称；

L_Function表示车道边界线左侧车道的功能；

RLane_No表示车道边界线右侧车道编号；

R_Name表示车道边界线右侧车道名称；

R_Function表示车道边界线右侧车道的功能；

具体包括以下步骤：

步骤1：建立车道级导航地图数据结构模型，导入车辆高精度定位轨迹数据；

步骤2：采用垂线法通过相交分析搜索每个定位轨迹点所在的车道，建立两者间的匹配关系；

步骤3：采用基于Delaunay三角网的方法对步骤2执行后匹配对应车道为空的轨迹点进行二次匹配探测；

步骤4：依据每个轨迹点的匹配车道信息，实施变道行为探测。

在上述的车辆高精度定位数据与导航地图精确匹配及变道行为探测方法，步骤2中，采用垂线法构建定位轨迹点与车道匹配关系，定义定位轨迹点为{p₁,p₂,…,p_n}(n≥2)，集合{r₁,r₂,…,r_n}用于记录轨迹点匹配的车道编号，车道边界线集合为{l₁,l₂,…,l_m}(m≥2)；与p_i(1≤i≤n)匹配的车道计算如下：

步骤2.1：取p_i的前一时刻轨迹点p_i-1，两点连线得到直线段p_i-1p_i；

步骤2.2：p_i处分别向两侧作长度为Len₀的射线rr和rl，射线与p_i-1p_i垂直；

步骤2.3：探测与rl相交的车道边界线，若探测得到2条及以上数量的不同车道边界线，取其中到p_i最短距离最小的车道边界线，记作l_L；

步骤2.4：利用步骤2.3同样方法计算与rr相交且到p_i最短距离最小的车道边界线，记作l_R；

步骤2.5：提取左侧车道边界线l_L记录的右侧车道编号(即l_L.RLane_No)和右侧车道边界线l_R记录的左侧车道编号l_R.LLane_No；

步骤2.6：若l_L.RLane_No＝l_R.LLane_No，则将对应的车道作为与p_i相匹配的车道，即r_i＝l_L.RLane_No；否则，pi的匹配车道信息标识为空，即r_i＝Φ；

步骤2.7：对下一个轨迹点pi+1重复上述步骤，直至完成对所有定位轨迹点的遍历。

在上述的车辆高精度定位数据与导航地图精确匹配及变道行为探测方法，步骤3中，采用基于Delaunay三角网的方法对步骤2执行后匹配对应车道为空的轨迹点进行二次匹配探测，具体是依次遍历轨迹点集合，对于当前轨迹点pk(1≤k≤n)，若r_k＝Φ(即匹配车道编号为空)，执行以下步骤：

步骤3.1，搜索轨迹点p_k周围分布的车道边界线，搜索半径为Len₁，搜索结果组织为集合L’＝{l₁,l₂,…,l_m}；

步骤3.2，对集合L’包含的车道边界线进行线性加密，使相邻节点间距离不大于阈值μ；

步骤3.3，对集合L’包含车道边界线构造约束Delaunay三角网，得到三角面集合{tri₁,tri₂,…,tri_q}(q≥3)，每个三角面标记为

每个三角面顶点组织为

其中

表示

的位置坐标，

表示顶点

所在的车道边界线编号；

步骤3.4，对于每个三角面tri_k(1≤k≤q)，按顺时针方向依据三个顶点所在车道边界线关联的车道信息，判断该三角面区域的车道编号；分为以下三种情况：

情况一，若三个顶点处于两条不同车道边界线，且两条车道边界线记录的车道编号逻辑一致，则将相应的车道编号赋予该三角面；

情况二，若三个顶点分别处于三条不同车道边界线，且三条车道边界线记录的车道编号逻辑一致，则将相应的车道编号赋予该三角面；

情况三，对于不满足以上两种情况的三角面，其车道信息赋值为空；

步骤3.5，对相邻分布且车道编号一致的三角面进行合并，形成车道面多边形集合；

步骤3.6，将当前轨迹点pk与构建的车道面进行叠置分析，若p_k处于某一车道面多边形范围内，则将该车道面关联的车道编号作为p_k的匹配车道编号。

在上述的车辆高精度定位数据与导航地图精确匹配及变道行为探测方法，步骤4中，将变道行为划分为‘一次变道’、‘连续变道’、‘驾驶偏离’三种类型，然后基于定位轨迹点匹配车道信息，按如下步骤探测车辆变道行为；定义行车轨迹点按时间次序表示为{p₁,p₂,…,p_n}，对应匹配得到的车道编号为{r₁,r₂,…,r_n}；依次遍历所有轨迹点，对于当前轨迹点p_i(1≤i≤n-1)，

步骤4.1，比较p_i与p_i+1的匹配车道信息；若r_i＝r_i+1，将当前轨迹点设为p_i+1，重复步骤4.1；否则，执行步骤4.2；

步骤4.2，若i+k>n，停止遍历；否则，获取p_i的后续k个轨迹点的匹配车道编号，

步骤4.2.1，若r_i+1＝r_i+2＝…＝r_i+k，则记录轨迹线段

并标识为‘一次变道’类型；将当前轨迹点设为r_i+k，转步骤4.1；

步骤4.2.2，若r_i+1＝…＝r_i+j(j<k)&&r_i+j+1＝…＝r_i+k＝r_i，即第一次变道后短时间内又进行了一次变道并回到原来的车道，记录轨迹线段

并标识为‘驾驶偏离’类型；将当前轨迹点设为r_i+k，转步骤4.1；

步骤4.2.3，若r_i+1＝…＝r_i+j(j<k)&&r_i+j+1＝…＝r_i+k&&r_i+j+1≠r_i&&r_i+j+1≠r_i+1，即第一次变道后短时间内又进行了一次变道并继续偏离原来的车道，记录轨迹线段

并标识为‘连续变道’类型；将当前轨迹点设为r_i+k，转步骤4.1；

步骤4.2.4，将当前轨迹点设为pi+1，执行步骤4.1。

本发明通过数据结构组织、匹配算法构建、变道行为分类及识别三个方面的创新，构建定位轨迹数据精确匹配与变道行为识别模型，对车辆行为监测、交通事故分析及理赔等应用有意义。

附图说明

图1是本发明实施例的流程图。

图2是本发明实施例的车道级导航地图数据组织示意图。

图3是本发明实施例的定位数据组织及与导航地图叠加示意图。

图4是本发明实施例的基于射线法的匹配过程示意图。

图5是本发明实施例的基于Delaunay三角网的匹配方法示意图。

图6是本发明实施例的变道行为示意图。

图7是本发明实施例的车辆变道行为探测统计结果图。

具体实施方式

为了便于本领域普通技术人员理解和实施本发明，下面结合附图及实施例对本发明作进一步的详细描述，应当理解，此处所描述的实施示例仅用于说明和解释本发明，并不用于限定本发明。

请见图1，本发明提供的一种车辆高精度定位数据与导航地图精确匹配及车辆变道行为探测方法，包括以下步骤：

步骤1：建立车道级导航地图数据结构模型，导入车辆高精度定位轨迹数据。

步骤1.1：车道级导航地图数据的组织模型。本发明中道路实体并不是简单地表达为单线目标，而是通过车道边界线集合L＝{l₁,l₂,…,l_m}(m≥2)进行描述组织。对于每条车道边界线l_i(1≤i≤m)，采用9元组结构表示

{Id,Path,Layer,LLane_No,L_Name,L_Function,RLane_No,R_Name,R_Function}

上述9元组的具体说明见发明内容部分，此处不再详述。在数据预处理时，需要根据车道的几何或功能上发生的变化，对其相关的车道边界线进行分割处理。例如，在图2示例中，由下侧的四车道变为上侧的三车道，所以需要在功能分割处A将相关的车道边界线打断为l₃、l₄、l₇表示，同样地，最右侧车道功能可能由“直行”变为“右转”，因此需要在功能分割处B将相应车道边界线打断为l₂、l₆。

步骤1.2，高精度定位数据组织及导入。如图3所示的一条轨迹线T，按轨迹点产生的时间次序组织为T＝{p₁,p₂,…,p₈}，一共包含8个轨迹点。每个轨迹点p_i表示为三元组信息<x_i,y_i,t_i>(1≤i≤8)，x_i和y_i为轨迹点p_i的空间位置坐标，t_i表示p_i产生的时间信息。按上述结构将车辆的定位信息进行组织，并导入系统与车道信息实施匹配。

步骤2：采用垂线法通过相交分析搜索每个定位轨迹点所在的车道，建立两者间的匹配关系。假设定位轨迹点为{p₁,p₂,…,p_n}(n≥2)，集合{r₁,r₂,…,r_n}用于记录轨迹点匹配的车道编号，车道边界线集合为{l₁,l₂,…,l_m}(m≥2)。如图4所示，与p_i(1≤i≤n)匹配的车道计算如下：

步骤2.1：取p_i的前一时刻轨迹点p_i-1，两点连线得到直线段p_i-1p_i。

步骤2.2：p_i处分别在两侧作长度为Len的射线rr和rl，射线与p_i-1p_i垂直。本发明实例中射线长度Len取车道宽度最大值的1.5倍。

步骤2.3：探测与rl相交的车道边界线，若探测得到2条及以上数量的不同车道边界线，取其中到p_i最短距离最小的车道边界线，记作l_L。

步骤2.4：利用步骤2.3同样方法计算与rr相交且到p_i最短距离最小的车道边界线，记作l_R。

步骤2.5：提取左侧车道分界线l_L记录的右侧车道编号(即l_L.RLane_No)和右侧车道分界线l_R记录的左侧车道编号l_R.LLane_No。

步骤2.6：若l_L.RLane_No＝l_R.LLane_No，则将对应的车道作为与p_i相匹配的车道，即r_i＝l_L.RLane_No；否则，p_i的匹配车道信息标识为空，即r_i＝Φ。本发明实例示意图4中，l_L.RLane_No＝＝l_R.LLane_No＝＝3，因此轨迹点p_i匹配在编号为3的车道上。

步骤2.7：对下一个轨迹点p_i+1重复上述步骤，直至完成对所有定位轨迹点的遍历。

步骤3，没有搜索到匹配车道的轨迹点，即该轨迹点相关联的匹配车道信息为空，原因可能处于道路交叉口、出口等复杂区段。例如在本发明实施例图5中轨迹点p₃，其左侧是出入口，并不存在车道边界线，因此上述射线法难以进行准确的匹配探测。针对此情况，本发明引入计算几何领域的Delaunay三角网模型进行如下处理。依次遍历轨迹点集合，对于当前轨迹点p_k(1≤k≤n)，若r_k＝Φ(即匹配车道信息为空)，执行以下步骤：

步骤3.1，搜索轨迹点p_k周围分布的车道边界线，搜索半径为Len₁，搜索结果组织为集合L’＝{l₁,l₂,…,l_m}。本发明实例中搜索半径Len₁设置为车道最大宽度值的5倍。

步骤3.2，对集合L’包含的车道边界线进行线性加密，使相邻节点间距离不大于阈值μ。本发明实例中μ取车道平均宽度。

步骤3.3，对集合L’包含车道边界线构造约束Delaunay三角网，得到三角面集合{tri₁,tri₂,…,tri_q}(q≥3)，每个三角面标记为

每个三角面顶点组织为

其中

表示

的位置坐标，

表示顶点

所在的车道边界线编号；

步骤3.4，对于每个三角面tri_k(1≤k≤q)，按顺时针方向依据三个顶点所在车道边界线关联的车道信息，判断该三角面区域的车道编号。分为以下三种情况：

①若三个顶点处于两条不同车道边界线，且两条车道边界线记录的车道编号逻辑一致，则将相应的车道编号赋予该三角面。如图5中三角面tri₁的顶点有两个落在车道线l₂，一个落在车道线l₅上，l₅.RLane_No＝＝l₂.LLane_No＝＝3，因此三角面tri₁所在区域车道编号为3。

②若三个顶点分别处于三条不同车道边界线，且三条车道边界线记录的车道编号逻辑一致，则将相应的车道编号赋予该三角面。如图5中tri₂三个顶点分别处于车道线l₅、l₁、l₂，且l₅.RLane_No＝＝l₁.RLane_No＝＝l₂.LLane_No＝3，所以三角面tri₂对应车道编号为3。

③对于不满足以上两种情况的三角面，其车道信息赋值为空。

步骤3.5，对相邻分布且车道编号一致的三角面进行合并，形成车道面多边形集合；

步骤3.6，将当前轨迹点p_k与构建的车道面进行叠置分析，若p_k处于某一车道面多边形范围内，则将该车道面关联的车道编号作为p_k的匹配车道编号。

步骤4，依据每个轨迹点的匹配车道信息，实施变道行为探测。本发明将变道行为划分为‘一次变道’、‘连续变道’、‘驾驶偏离’三种类型。假设行车轨迹点按时间次序表示为{p₁,p₂,…,p_n}，对应匹配得到的车道编号为{r₁,r₂,…,r_n}。依次遍历所有轨迹点，对于当前轨迹点p_i(1≤i≤n-1)，步骤4.1，比较p_i与p_i+1的匹配车道信息。若r_i＝r_i+1，将当前轨迹点设为p_i+1，重复步骤4.1；否则，执行步骤4.2。

步骤4.2，给定常数k，若i+k>n，停止遍历；否则，获取p_i的后续k个轨迹点的匹配车道编号，

步骤4.2.1，若r_i+1＝r_i+2＝…＝r_i+k(如图6中的(a)所示)，则记录轨迹线段

并标识为‘一次变道’类型；将当前轨迹点设为r_i+k，转步骤4.1。

步骤4.2.2，若r_i+1＝…＝r_i+j(j<k)&&r_i+j+1＝…＝r_i+k＝r_i(如图6中的(b)所示)，即第一次变道后短时间内又进行了一次变道并回到原来的车道，记录轨迹线段

并标识为‘驾驶偏离’类型；将当前轨迹点设为r_i+k，转步骤4.1。

步骤4.2.3，若r_i+1＝…＝r_i+j(j<k)&&r_i+j+1＝…＝r_i+k&&r_i+j+1≠r_i&&r_i+j+1≠r_i+1(如图6中的(c)所示)，即第一次变道后短时间内又进行了一次变道并继续偏离原来的车道，记录轨迹线段

并标识为‘连续变道’类型；将当前轨迹点设为r_i+k，转步骤4.1。

步骤4.2.4，将当前轨迹点设为p_i+1，执行步骤4.1。

上述步骤4.2中，k取值需要根据轨迹点采集频率和常规行车速度等确定，即驾驶偏移和连续变道行为中两次变道应该在较短时间内完成，否则就是两次独立的一次变道行为。本发明样例中，轨迹点采样频率为1Hz，行车速度平均为60km/h，k取4，最终轨迹探测结果如图7所示。

应当理解的是，本说明书未详细阐述的部分均属于现有技术。

应当理解的是，上述针对实施例的描述较为详细，并不能因此而认为是对本发明专利保护范围的限制，本领域的普通技术人员在本发明的启示下，在不脱离本发明权利要求所保护的范围情况下，还可以做出替换或变形，均落入本发明的保护范围之内，本发明的请求保护范围应以所附权利要求为准。

Claims (3)

1.一种车辆高精度定位数据与导航地图精确匹配及变道行为探测方法，其特征在于，基于车道级导航地图数据组织模型，具体定义是道路实体组织为车道边界线集合L＝{l₁,l₂,…,l_m}，m≥2；对于每条车道边界线l_i，1≤i≤m，采用9元组结构表示：

{Id,Path,Layer,LLane_No,L_Name,L_Function,RLane_No,R_Name,R_Function}

其中，Id表示为车道边界线在数据库系统的唯一编号；

Path记录车道边界线的坐标点信息；

Layer表示车道边界线的功能类型；

LLane_No表示车道边界线左侧车道编号；

L_Name表示车道边界线左侧车道名称；

L_Function表示车道边界线左侧车道的功能；

RLane_No表示车道边界线右侧车道编号；

R_Name表示车道边界线右侧车道名称；

R_Function表示车道边界线右侧车道的功能；

具体包括以下步骤：

步骤1：建立车道级导航地图数据结构模型，导入车辆高精度定位轨迹数据；

步骤2：采用垂线法通过相交分析搜索每个定位轨迹点所在的车道，建立两者间的匹配关系；

步骤3：采用基于Delaunay三角网的方法对步骤2执行后匹配对应车道为空的轨迹点进行二次匹配探测；

步骤4：依据每个轨迹点的匹配车道信息，实施变道行为探测；

步骤2中，采用垂线法构建定位轨迹点与车道匹配关系，定义定位轨迹点为{p₁,p₂,…,p_n}，n≥2，集合{r₁,r₂,…,r_n}用于记录轨迹点匹配的车道编号，车道边界线集合为{l₁,l₂,…,l_m}，m≥2；与p_i匹配的车道计算如下：

步骤2.1：取p_i的前一时刻轨迹点p_i-1，两点连线得到直线段p_i-1p_i，1≤i≤n；

步骤2.2：p_i处分别向两侧作长度为Len₀的射线rr和rl，射线与p_i-1p_i垂直；

步骤2.3：探测与rl相交的车道边界线，若探测得到2条及以上数量的不同车道边界线，取其中到p_i最短距离最小的车道边界线，记作l_L；

步骤2.4：利用步骤2.3同样方法计算与rr相交且到p_i最短距离最小的车道边界线，记作l_R；

步骤2.5：提取左侧车道边界线l_L记录的右侧车道编号l_L.RLane_No和右侧车道边界线l_R记录的左侧车道编号l_R.LLane_No；

步骤2.6：若l_L.RLane_No＝l_R.LLane_No，则将对应的车道作为与p_i相匹配的车道，即r_i＝l_L.RLane_No；否则，p_i的匹配车道信息标识为空，即r_i＝Φ；

步骤2.7：对下一个轨迹点p_i+1重复上述步骤，直至完成对所有定位轨迹点的遍历。

2.根据权利要求1所述的车辆高精度定位数据与导航地图精确匹配及变道行为探测方法，其特征在于：步骤3中，采用基于Delaunay三角网的方法对步骤2执行后匹配对应车道为空的轨迹点进行二次匹配探测，具体是依次遍历轨迹点集合，对于当前轨迹点p_k，1≤k≤n，若r_k＝Φ，即匹配车道编号为空，执行以下步骤：

步骤3.1，搜索轨迹点p_k周围分布的车道边界线，搜索半径为Len₁，搜索结果组织为集合L’＝{l₁,l₂,…,l_m}；

步骤3.2，对集合L’包含的车道边界线进行线性加密，使相邻节点间距离不大于阈值μ；

步骤3.3，对集合L’包含车道边界线构造约束Delaunay三角网，得到三角面集合{tri₁,tri₂,…,tri_q}，q≥3，每个三角面标记为

每个三角面顶点组织为

其中

表示

的位置坐标，

表示顶点

所在的车道边界线编号；

步骤3.4，对于每个三角面tri_k，1≤k≤q，按顺时针方向依据三个顶点所在车道边界线关联的车道信息，判断该三角面区域的车道编号；分为以下三种情况：

情况一，若三个顶点处于两条不同车道边界线，且两条车道边界线记录的车道编号逻辑一致，则将相应的车道编号赋予该三角面；

情况二，若三个顶点分别处于三条不同车道边界线，且三条车道边界线记录的车道编号逻辑一致，则将相应的车道编号赋予该三角面；

情况三，对于不满足以上两种情况的三角面，其车道信息赋值为空；

步骤3.5，对相邻分布且车道编号一致的三角面进行合并，形成车道面多边形集合；

步骤3.6，将当前轨迹点p_k与构建的车道面进行叠置分析，若p_k处于某一车道面多边形范围内，则将该车道面关联的车道编号作为p_k的匹配车道编号。

3.根据权利要求1所述的车辆高精度定位数据与导航地图精确匹配及变道行为探测方法，其特征在于：步骤4中，将变道行为划分为‘一次变道’、‘连续变道’、‘驾驶偏离’三种类型，然后基于定位轨迹点匹配车道信息，按如下步骤探测车辆变道行为；定义行车轨迹点按时间次序表示为{p₁,p₂,…,p_n}，对应匹配得到的车道编号为{r₁,r₂,…,r_n}；依次遍历所有轨迹点，对于当前轨迹点p_i，1≤i≤n-1，

步骤4.1，比较p_i与p_i+1的匹配车道信息；若r_i＝r_i+1，将当前轨迹点设为p_i+1，重复步骤4.1；否则，执行步骤4.2；

步骤4.2，若i+k>n，停止遍历；否则，获取p_i的后续k个轨迹点的匹配车道编号，

步骤4.2.1，若r_i+1＝r_i+2＝…＝r_i+k，则记录轨迹线段

并标识为‘一次变道’类型；将当前轨迹点设为r_i+k，转步骤4.1；

步骤4.2.2，若r_i+1＝…＝r_i+j&&r_i+j+1＝…＝r_i+k＝r_i，j<k，即第一次变道后短时间内又进行了一次变道并回到原来的车道，记录轨迹线段

并标识为‘驾驶偏离’类型；将当前轨迹点设为r_i+k，转步骤4.1；

步骤4.2.3，若r_i+1＝…＝r_i+j&&r_i+j+1＝…＝r_i+k&&r_i+j+1≠r_i&&r_i+j+1≠r_i+1，j<k，即第一次变道后短时间内又进行了一次变道并继续偏离原来的车道，记录轨迹线段

并标识为‘连续变道’类型；将当前轨迹点设为r_i+k，转步骤4.1；

步骤4.2.4，将当前轨迹点设为p_i+1，执行步骤4.1。

Images