一种轨迹点与交通标志位置优化方法及系统与流程

本发明涉及自动驾驶领域，尤其涉及一种轨迹点与交通标志位置优化方法及系统。

背景技术：

在自动驾驶中，高精度地图的制作是一项耗时长、成本高的工程任务，传统的地图制作方法已难以满足现代高精度地图的驾驶需求。在大数据环境下，采用低成本硬件进行众包采集从而生成高精度地图的方式能较好的匹配高精度地图的成本要求，但大量低成本采集的众包数据很多达不到自动驾驶中的高精度地图精度要求。

针对众包数据庞杂且精度难以达到要求的问题，当前多采用计算大量低成本众包数据的密度分布来逼近真值的方式，这种方式对众包采集数据的数据量有较高的要求。在相对偏僻的道路，众包车采集难以完全覆盖，数据量难以达到要求，从而会导致高精度地图的精度降低，难以满足自动驾驶的市场要求。

技术实现要素：

有鉴于此，本发明实施例提供了一种轨迹点与交通标志位置优化方法及系统，用于解决在采集的数据量未达到要求时，高精度地图数据的精度降低的问题。

在本发明实施例的第一方面，提供了一种轨迹点与交通标志位置优化方法，包括：

对众包采集源数据中的轨迹数据及交通标志数据进行预处理，并去除所述轨迹数据及交通标志数据中的噪声；

基于轨迹点运动规律及观测值，通过建立轨迹点的图优化模型，优化所述轨迹数据中的轨迹点位置；

获取所述轨迹数据中轨迹点对交通标志的观测数据，所述观测数据至少包括观测距离和观测角度，基于优化后的轨迹点位置，通过ba算法建立图优化模型优化所述交通标志数据中的交通标志位置。

在本发明实施例的第二方面，提供了一种轨迹点与交通标志位置优化系统，包括：

预处理模块，用于对众包采集源数据中的轨迹数据及交通标志数据进行预处理，并去除所述轨迹数据及交通标志数据中的噪声；

轨迹点优化模块，用于基于轨迹点运动规律及观测值，通过建立轨迹点的图优化模型，优化所述轨迹数据中的轨迹点位置；

交通标志优化模块，用于获取所述轨迹数据中轨迹点对交通标志的观测数据，所述观测数据至少包括观测距离和观测角度，基于优化后的轨迹点位置，通过ba算法建立图优化模型优化所述交通标志数据中的交通标志位置。

本发明实施例中，对采集的众包数据预处理去除噪声后，基于轨迹点运动规律及观测值，构建轨迹点的图优化模型，优化轨迹点位置，再获取交通标志位置、观测点位置及车辆航向角，基于优化后的轨迹点通过ba算法建立图优化模型优化交通标志位置。在实施例的技术方案中，对于众包数据，基于轨迹运动模型及ba算法，形成图优化模型，进而可以实现准确快速的对轨迹点及交通标志位置进行优化，可以提高高精度地图数据的精度，减少对采集众包数据量的要求，同时可以降低优化的时耗及计算量，进而大大降低高精度地图的众包数据采集及更新优化成本，实现对大量级的众包数据进行优化处理，便于高精度地图制作更新。

附图说明

图1为本发明实施例提供的一种轨迹点与交通标志位置优化方法的流程示意图；

图2为本发明实施例提供的基于轨迹点优化交通标志的原理示意图；

图3为本发明实施例提供的一种轨迹点与交通标志位置优化系统的结构示意图。

具体实施方式

本发明实施例提供了一种轨迹点与交通标志位置优化方法及系统，可以实现对基于众包低成本设备采集的相对低精度的轨迹点及交通标志位置进行整体优化，从而提高高精度地图的精度，并降低制作成本。

为使得本发明的发明目的、特征、优点能够更加的明显和易懂，下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，下面所描述的实施例仅仅是本发明一部分实施例，而非全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其它实施例，都属于本发明保护的范围。

实施例一：

在自动驾驶的高精度地图制作过程中，传统高精度测绘设备采集高精度地图原始数据进行产线工具集中式构图成本太高，且采集的众包数据量也要求较高，通过大量低成本众包采集车的方式来进行分布式大数据构图可以有效降低成本，但由于存在大量低精度的众包数据，需要在不增加硬件成本的前提下有效提升众包数据中轨迹点及交通标志等关键地图参数的精度，同时降低对采集数据量的要求。

请参阅图1，本发明实施例提供的一种轨迹点与交通标志位置优化方法的流程示意图，包括：

s101、对众包采集源数据中的轨迹数据及交通标志数据进行预处理，并去除所述轨迹数据及交通标志数据中的噪声；

所述众包数据为多辆汽车采集的地图数据，所述众包数据中包括轨迹数据及交通标志数据。所述轨迹数据为采集车辆的运动轨迹数据，包括轨迹点位置、轨迹点运动方向、轨迹点运动速度及加速度，所述交通标志数据包含有轨迹点对交通标志的观测数据，需要注意的是，所述轨迹数据与所述交通标志数据对应为同一车辆采集，每一个众包数据由单独采集车辆采集形成，同一车辆可以采集多个众包数据。

其中，所述交通标志一般包括交通信号灯和交通标志牌，所述交通信号灯和交通标志牌的观测角度存在差异。

在通过摄像头或雷达等设备采集的轨迹数据及交通标志数据中，存在有噪声干扰，通过噪声滤除可有效提升轨迹数据处理的精确度。

具体的，对所述众包数据中小于预定速度的轨迹点数据进行零值处理，对小于预定时间间隔采集的轨迹点进行归一化处理，即低时间间隔的轨迹点进行归一化，并滤除所述轨迹数据及交通标志数据中大于预定频率的高频噪声或小于预定频率的低频噪声，即滤除高频噪声和低频噪声。

s102、基于轨迹点运动规律及观测值，通过建立轨迹点的图优化模型，优化所述轨迹数据中的轨迹点位置；

所述轨迹点运动规律为符合物体运动定律，根据物体运动定律可以计算物体运动的位置及速度变化，所述观测值为轨迹点某一位置的采样数据，采样数据中一般有轨迹点位置、速度、加速度、方向等。在本实施中，基于轨迹点运动规律及观测值，可以构建运动方程及观测方程。

具体的，分别建立轨迹点的运动方程及观测方程；设定观测误差为轨迹点与观测点差值，通过最小二乘法求取所有观测误差的最小值；根据所述所有观测误差的最小值，建立图优化模型，优化轨迹点位置。

示例性的，假设轨迹点为匀速运动模型(以时间为序)，可将轨迹点运动分解为x和y方向，并设定

vx：t时刻轨迹点x方向的速度

vy：t时刻轨迹点y方向的速度

ax：t时刻轨迹点x方向加速度

ay：t时刻轨迹点y方向加速度

px：t时刻轨迹点x方向位置

py：t时刻轨迹点y方向位置

v0：轨迹的起始位置

vk：轨迹点在k处的速度

pk：轨迹点在k处的位置(观测值)

则待优化轨迹点变量为sx和sy。

根据运动和观测方程建立方法：

运动方程：vx,k+1＝vx,k+ax,k*t；vy,k+1＝vy,k+ay,k*t；

观测方程：sx,k+1＝sx,k+1/2*t*(vx,k+vx,k+1)

sy,k+1＝sy,k+1/2*t*(vy,k+vy,k+1)

此时，观测点误差为：e＝px–s；

将所有的误差项利用最小二乘的方法求最小值：

进而可以得到优化后的轨迹点位置(sx，sy)。

s103、获取所述轨迹数据中轨迹点对交通标志的观测数据，基于优化后的轨迹点位置，通过ba算法建立图优化模型优化所述交通标志数据中的交通标志位置。

所述观测数据至少包括观测距离和观测角度，观测距离即采集车辆与交通标志间的距离，观测角度即车辆航向与交通标志间的角度。当交通标志出现在车辆视野中，则记录该轨迹点对应的观测数据。

具体的，预先设定交通标志位置、车辆的观测角度及观测距离，当交通标志出现在车辆视野中，形成一组观测数据；一般的，可预先根据采集车辆对既定位置的交通标志采集观测数据，构建观测方程。

根据多组观测数据，建立车辆对交通标志的观测方程：

其中，ri,j,k为轨迹点在k处的观测距离，pi,x,k为轨迹点在x方向的位置，pi,y,k为轨迹点在y方向的位置，sj,x为待优化轨迹点在x方向的位置，sj,y为待优化轨迹点在y方向的位置，bi,j,k为轨迹点在k处的航向角，θ为初始航向角，vy,k为轨迹点在y方向的速度，vx,k为轨迹点在x方向的速度。

将实际采集的多组观测数据代入方程，并基于优化后的轨迹点可以求取待优化的交通标志位置，如图2所示，交通标志01表示待优化的交通标志位置，一般一条轨迹上交通标志有多个，可以依次进行编号，对同一个交通标志01，存在多个观测点，即多个轨迹点可以观测到交通标志01，假设轨迹点201为第一个可以观测的位置，存在其他可以观测的轨迹点202和203，当然可以观测的轨迹点可以有多个，在此不够成任何限定。当轨迹点201、202和203均为优化后的轨迹点，则根据观测方程，通过ba算法可以建立图优化模型进行优化，所述图优化模型可以对多个交通标志位置进行优化。

示例性的，建立ba优化模型，且观测量为r和b，r为交通标志及观测点位置，b为车辆航向角。

设定待优化变量的交通标志位置坐标(x,y)，根据车辆运动及观测模型，存在观测方程：

根据ba算法原理建立图优化模型，求解最小二乘问题，利用图优化模型得到优化后的交通标志位置坐标(x,y)。

本实施例提供的方案中，采集观测数据，基于车辆运动规律建立数学模型，并通过图优化模型对轨迹点及交通标志的位置优化，能实现高效准确的数据优化，提升众包数据精度从而降低更新采集成本，而且基于二乘法的求解可以简化优化模型。

实施例二：

图3为本发明实施例二提供的轨迹点与交通标志位置优化系统的结构示意图，包括：

预处理模块310，用于对众包采集源数据中的轨迹数据及交通标志数据进行预处理，并去除所述轨迹数据及交通标志数据中的噪声；

轨迹点优化模块320，用于基于轨迹点运动规律及观测值，通过建立轨迹点的图优化模型，优化所述轨迹数据中的轨迹点位置；

可选的，所述去除所述轨迹数据及交通标志数据中的噪声包括：

对所述众包数据中小于预定速度的轨迹点进行零值处理、对小于预定时间间隔采集的轨迹点进行归一化处理以及滤除所述轨迹数据及交通标志数据中大于预定频率的高频噪声或小于预定频率的低频噪声。

可选的，所述轨迹点优化模块320包括：

构建单元，用于分别建立轨迹点的运动方程及观测方程；

计算单元，用于设定观测误差为轨迹点与观测点差值，通过最小二乘法求取所有观测误差的最小值；

优化单元，用于根据所述所有观测误差的最小值，建立图优化模型，优化轨迹点位置。

交通标志优化模块330，用于获取所述轨迹数据中轨迹点对交通标志的观测数据，所述观测数据至少包括观测距离和观测角度，基于优化后的轨迹点位置，通过ba算法建立图优化模型优化所述交通标志数据中的交通标志位置。

可选的，所述获取所述轨迹数据中轨迹点对交通标志的观测数据还包括：

预先设定交通标志位置、车辆的观测角度及观测距离，当交通标志出现在车辆视野中，形成一组观测数据；

根据多组观测数据，建立车辆对交通标志的观测方程：

其中，ri,j,k为轨迹点在k处的观测距离，pi,x,k为轨迹点在x方向的位置，pi,y,k为轨迹点在y方向的位置，sj,x为待优化轨迹点在x方向的位置，sj,y为待优化轨迹点在y方向的位置，bi,j,k为轨迹点在k处的航向角，θ为初始航向角，vy,k为轨迹点在y方向的速度，vx,k为轨迹点在x方向的速度。

本实施例系统中，基于轨迹点优化模块及交通标志优化模块，可以实现对众包数据中的轨迹点和交通标志进行优化，保障众包数据的精度，进而可以降低众包数据采集成本。

在上述实施例中，对各个实施例的描述都各有侧重，某个实施例中没有详述或记载的部分，可以参见其它实施例的相关描述。

以上所述，以上实施例仅用以说明本发明的技术方案，而非对其限制；尽管参照前述实施例对本发明进行了详细的说明，本领域的普通技术人员应当理解：其依然可以对前述各实施例所记载的技术方案进行修改，或者对其中部分技术特征进行等同替换；而这些修改或者替换，并不使相应技术方案的本质脱离本发明各实施例技术方案的精神和范围。