CN104215249A - 一种行车轨迹的平滑方法 - Google Patents

Abstract

本发明公开一种行车轨迹的平滑方法，包括如下过程：步骤1：以一定的采样频率获取车辆的行驶坐标，作为原始定位坐标，记为{P₁,…P_k,…,P_q},k＝1,2，…，q；步骤2：对原始定位坐标{P₁,…P_k,…,P_q}进行筛选过滤，避免漂移的坐标点带来的影响；步骤3：对对经过步骤2筛选过滤后的定位坐标进行拐点补偿和插值；步骤4：根据步骤3获得的定位坐标进行路径拟合。本发明通过上述方法，该方法采用覆盖底层到上层的优化算法，在相同的坐标采样周期内，为用户提供最优的轨迹平滑及呈现方法。本发明结合定位漂移抑制算法以及拐点补偿和插值算法，具有很好的平滑效果，同时，其算法简单，易于实现，具有很好的实用性。

Description

一种行车轨迹的平滑方法

技术领域

本发明涉及一种轨迹平滑方法，尤其是行车轨迹的优化平滑方法。

背景技术

物品、车辆及行人运动轨迹的显示和回放的运用场景越来越多，用户对路径回放质量的要求也越来越高。其中，目前的行车路径回放，也即对车辆行驶轨迹(行车轨迹)的拟合，一般是通过以一定的频率获取其位置信息(例如通过GPS获得其定位坐标)，再通过这些采集的位置信息进行坐标描点，或者计算得出近似的连续行驶轨迹。这种过程可以看作是在离散数据的基础上补连续函数，通过函数在有限个点处的取值状况，估算出函数在其他点处的近似值，即插值或拟合过程。这种方式在采样频度高(小于1s)的场合下是基本满足需求的，但轨迹不平滑、体验差，在地图上常常伴随无效的折返曲线，同时过大的数据采集导致带来通信流量增大，数据的计算复杂性也增大。

另外，在采样过程中，经常会有定位坐标信息存在漂移问题，简称定位漂移。定位漂移一般在大气电离层介质发生变化或是楼宇多径效应影响时候发生，另外物品、车辆或是行人在发生微小位移或调整方向时也会有定位的漂移。由大气电离层介质发生变化或是多径效应影响带来的漂移是与物体运动状态无关的，是一种固有的漂移，而由微小位置移动或是方式变化带来的定位漂移往往是使用者或是观察者不关注的，因此定位坐标信息一旦显示在地图上，就会在真实的静止位置周围产生很多点，那么其实周围这些点就是漂移点。将这些漂移点均显示在轨迹曲线中不但会影响曲线的显示质量，且浪费服务器存储空间。为此，一篇申请号为201410050238.5的发明专利，公开了一种GPS轨迹曲线的生成方法，包括GPS位置点获取步骤、校正对象选择步骤、漂移点筛选步骤以及曲线生成步骤。在校正对象选择步骤中，根据预设的处理点条件进行判断，选择所需的校正对象；在漂移点筛选步骤中，根据预设的筛选条件进行判断，判断当前校正对象中的处理点是否为漂移点，在生成曲线时则将筛选出的所有处理点滤除。该发明对采样点进行筛选是通过对一组(三个)GPS位置点进行的，不但需要对坐标点两两之间的速度方向夹角进行计算，同时也要对每个点的速度进行加权平均，具有数据计算量大的缺陷，而且经过筛选后的数据采样点并不具备统一的采样周期，在后续的描点过程中容易出现不平滑的折线。

同时，当物品、车辆或运行行人轨迹斜率过大时(例如拐弯，车辆转弯)且速度较大时，由于位置坐标的采样间隔Δt不变，采样点前后两点的位置与当前运动速度成正比，当速度大于30KM/h时，若采样间隔为1s，那么相邻两个采样坐标的距离将超过8米，由转弯带来的问题就是采集的GPS坐标前后两点差异很大，导致出现斜率过大的折线，此时其定位数据一般不能显示真实的运行轨迹。为此，一篇申请号为201310650271.7的发明专利，公开了一种车辆轨迹的拐点补偿测量方法，其包括车辆状态判断步骤、拐点判断步骤和数据上传步骤，所述拐点判断步骤包括：连续测量每两点之间的拐弯角度，计算N个连续测量点的拐弯角度之和α，若|α|>β，则认为进入拐弯状态；所述数据上传步骤为：当进入拐弯状态后，进行频率为K1的间隔数据上传，当拐弯状态结束后，数据上传恢复默认频率K0，所述K1大于K0；所述β为预设的角度阈值，N为预设的判断个数。通过上述拐点补偿测量方法，在等时或等距采集基础上，增加拐点补偿机制，当检测到车辆拐弯时，才增加采集点，从而减小GPRS的传输数据量，通过对连续采样点角度累加统计，克服了小曲率大拐弯半径下可能出现的误报。该专利通过改变采样频率的方法规避了拐点过大时轨迹的曲折问题，但同时存在两个问题，一是但依照该专利拐点条件进行判断后，硬件设备需要立即采用变频模式来对GPS坐标进行采样，要满足这点需要硬件设备对满足在拐点条件进行实时判断，如果实时性不够那么就不可能在拐弯状态下建立变频的机制。其次是变采样的方法增加了数据量，而且这种方式只能在硬件终端中去完成，不能在云服务器中(不能满足实时性)，增加了硬件终端的计算处理压力。

发明内容

因此，针对上述的问题，本发明提出一种行车轨迹的平滑方法，该方法采用覆盖底层到上层的优化算法，在相同的坐标采样周期内，为用户提供最优的轨迹平滑及呈现方法，从而解决现有技术之不足。

为了解决上述技术问题，本发明思路是，首先，本发明对漂移点筛除采用概率分析的方法，也就是在上一采样点确定的条件下给出下一个点坐标最大置信区间，通俗地说来就是预估计下一个最有可能坐标点坐落的范围，并通过比较进行筛选。该算法的优势是算法复杂度低，仅涉及到前后两个采样点的坐标运算，并且运算所需的几个变量都可直接GPS中获取。再次，本发明在对拐弯的处理中采用补偿的方式进行，在不需要硬件设备提供更多的坐标采样数据的条件下，通过预估采样点和插值的方法，使得原本较为稀疏的坐标点之间构造出更多符合实际运动规律的坐标，从而将轨迹平滑化。该算法的优势是不需要对硬件设备进行改进，维持原有的采样频率即可，因此原始的数据量不会由于变采样而增加。更有价值的是数据的所以运算都可以用集中在服务器，也就是云端，不会对硬件设备带来运算能力或是功耗方面的压力。

具体的，本发明所采用的技术方案是，一种行车轨迹的平滑方法，包括如下过程：

步骤1：以一定的采样频率获取车辆的行驶坐标，作为原始定位坐标，记为{P₁,…P_k,…,P_q},k＝1,2，…，q；

步骤2：对原始定位坐标{P₁,…P_k,…,P_q}进行筛选过滤，避免漂移的坐标点带来的影响；具体筛选过程如下：

步骤21：在固定采样时间窗Δt内，若平均速度记相邻两个采样坐标P_i(x_i,y_i,z_i)，P_j(x_j,y_j,z_j)的距离为P_ij，即P_ij＝︱＝︱P_i(x_i,y_i,z_i)-P_j(x_j,y_j,z_j)︱，同时令其中R为下一坐标点可能坐落的最远区域的半径，通过平均速度和采样间隔的乘积得到；r为下一坐标点可能坐落的最近区域半径，由实验值得到。此外考虑到速度方向不可能在采样周期内急剧变化，因此内半径为r、外半径为R的圆环区域(圆区域R内和r外)与速度矢量的法线形成的半圆区域是下一坐标点应落入的最大置信范围，概率约95％；

步骤22：判断P_ij是否落在内半径为r、外半径为R且与速度矢量的法线形成的半圆环区域内：如果P_ij落在该半圆环区域外，则P_t(x，y，z)＝P_j(x，y，z)，此时过滤掉采样坐标P_j(x_j,y_j,z_j)；如果P_ij落在该半圆环区域内，则保留采样坐标P_j(x_j,y_j,z_j)；一般情况下，取V_ε＝3m/s，P_ε＝1m。

步骤23：判断P_i(x_i,y_i,z_i)与P_j(x_j,y_j,z_j)的下一点坐标P_k(x_k,y_k,z_k)之间的距离P_ik是否落在内半径为r、外半径为R且与速度矢量的法线形成的半圆环区域内，此时如果P_ik落在该半圆环区域外，则过滤掉采样坐标P_k(x_k,y_k,z_k)，如果P_ik落在该半圆环区域内，则保留采样坐标P_k(x_k,y_k,z_k)；

步骤24：重复步骤23，每次过滤掉一点，将采样的间隔时间增加一个Δt，那么限定外半径R变为n为此次比较中过滤的坐标点数量，直到所有的坐标比对完毕。

步骤3：对经过步骤2筛选过滤后的定位坐标进行拐点补偿和插值，为了防止出现斜率过大的折线，通过预估插值的方法在采样点之间多生产几个点，使轨迹平滑，具体包括如下步骤：

步骤31：取估计间隔因子δt＝Δt/n,估计的坐标点P_m(m＝1,2，…，n)可以根据下算式计算：

$P_m-P_{m-1}+\∫{\stackrel{\→}{V}}_{m-1}\mathrm{\δt}+\∫\∫{\stackrel{\→}{a}}_{m-1}\mathrm{\δt\δt};$

上式子中，P_m-1是上一个有效的坐标点，是物体在坐标点P_m-1处的速度瞬时值，由GPS模块计算得出；是物体在坐标点P_m-1处的加速度瞬时值，由加速度计输出得到。

步骤32：得到坐标点P_m后，利用牛顿多项式插值的方法做线路拟合，其中牛顿多项式阶数由下式来确定：

|P_m-P_j|＜P_ε；----式(*)

即满足上式的最小m为某段路径拟合需要的最小阶数，取n＝m即可。

但通过坐标点剔除后的坐标来进行插值时，插值的两点采样间隔不一定是Δt，可能是T＝kΔt，这里k是一个大于2的自然数(k＝2,3,4，…)，这时估计间隔因子δt＝T/k汝先进行一次插值，如果精度满足式(*)，再进行分段后的二次插值，此时取估计间隔因子δt＝Δt/n；

插值公式给出如下：

f(P)＝f[P₀，P₁](p-P₀)+f[P₀，P₁，P₂](p-P₀)(p-P₁)+…+f[P₀，P₁，...，P_n](p-P₀)(p-P₁)...(p-Pn)

其中，

$f[P,P_1]=\frac{f\left(P\right)-f\left(P_0\right)}{P-P_0};$

……；

$f[P,P_1,...,P_n]=\frac{f[P,P_0,...,P_{n-1}]-f[P_0,P_1,...,P_n]}{P-P_n};$

f(P)为经过拟合后的路径，实际应用中我们不需要将f(P)计算出来，只需要计算出P_m即可。

步骤4：根据步骤3获得的定位坐标进行路径拟合，即将步骤3计算得到的坐标点呈现在地图上，进行最近路径匹配，目前地图厂商均提供API接口，调用地图的API接口，并根据计算上传的坐标对路径进行拟合匹配，最终将结果显示在地图上即可。

本发明通过上述方法，该方法采用覆盖底层到上层的优化算法，在相同的坐标采样周期内，为用户提供最优的轨迹平滑及呈现方法。其中，本发明对采样点进行筛选是通过排除落在内半径为r、外半径为R且与速度矢量的法线形成的半圆环区域，而进行筛选，其数据计算简单，易于实现。另外，本发明对原始定位坐标进行拐点补偿和插值，其通过预估插值的方法在采样点之间多生产几个点，使轨迹平滑，同时满足实时性的要求，且不需要采用变频模式来对GPS坐标进行采样，大大减少了数据量，从而降低了硬件终端的压力。本发明结合定位漂移抑制算法以及拐点补偿和插值算法，具有很好的平滑效果，同时，其算法简单，易于实现，具有很好的实用性。

附图说明

图1为本发明的算法的流程图；

图2为本发明的半圆环示意图；

图3为本发明采用定位漂移抑制算法与未采用抑制算法的曲线对比示意图；

图4为本发明的拐点折线示意图。

具体实施方式

现结合附图和具体实施方式对本发明进一步说明。

作为一个较佳的实施方式，参见图1，本发明的一种行车轨迹的平滑方法，包括如下过程：

步骤1：以一定的采样频率获取车辆的行驶坐标，作为原始定位坐标，记为{P₁,…P_k,…,P_q}，k＝1,2，…，q；

步骤2：通过定位漂移抑制对原始定位坐标{P₁,…P_k,…,P_q}进行筛选过滤；定位漂移一般在大气电离层介质发生变化或是楼宇多径效应影响时候发生，另外物品、车辆或是行人在发生微小位移或调整方向时也会有定位的漂移。由大气电离层介质发生变化或是多径效应影响带来的漂移是与物体运动状态无关的，是一种固有的漂移；而由微小位置移动或是方式变化带来的定位漂移往往是使用者或是观察者不关注的，因此在进行地图回放前需要将这两种漂移滤除，以免造成地图轨迹上混乱。综合这两种情况，本发明对原始坐标数据进行筛选过滤。具体过程如下：

在固定采样时间窗Δt内，若平均速度记相邻两个采样坐标P_i(x_i,y_i,z_i)，P_j(x_j,y_j,z_j)的距离为P_ij，即P_ij＝︱P_i(x_i,y_i,z_i)-P_j(x_j,y_j,z_j)_︱，同时记其中R为下一坐标点可能坐落的最远区域的半径，通过平均速度和采样间隔的乘积得到；r为下一坐标点可能坐落的最近区域半径，由实验值得到。此外考虑到速度方向不可能在采样周期内急剧变化，因此内半径为r、外半径为R的圆环区域(圆区域R内和r外)与速度矢量的法线形成的半圆区域是下一坐标点应落入的最大置信范围，概率约95％；

若P_ij落在图2所示的内半径为r、外半径为R且与速度矢量的法线形成的半圆环区域外时，认为P_i(x，y，z)＝P_j(x，y，z)，即过滤掉采样坐标P_j(x_j,y_j,z_j)，同时将P_i(x_i,y_i,z_i)与P_j(x_j,y_j,z_j)的下一点坐标P_k(x_k,y_k,z_k)相比较，此时，取重复上述过程直到条件不能满足。一般情况下，取V_ε＝3m/s，P_ε＝1m。参见图3，图中的曲线1是未进过坐标点过滤的，根据上述抑制算法剔除的坐标偏移点有P2，P5，按算法拟合的曲线如曲线2所示。

步骤3：拐点补偿和插值。当物品、车辆或运行行人轨迹斜率过大时(例如拐弯，车辆转弯)且速度较大时，由于位置坐标的采样间隔Δt不变，采样点前后两点的位置与当前运动速度成正比，当速度大于30KM/h时，若采样间隔为1s，那么相邻两个采样坐标的距离将超过8米，由转弯带来的问题就是上传前后坐标差异很大，导致出现斜率过大的折线。

图3中，P_i，P_j和P_k是按间隔Δt采样得到的坐标位置，蓝色线条为描绘的路径轨迹，实际的轨迹却如红色部分所示。可以看出当速度过快，拐弯斜率变化过大时描绘的行径轨迹与实际线路差别很大。因此需要通过预估插值的方法在采样点间多生产几个点，使轨迹平滑。

取估计间隔因子δt＝Δt/n，估计的坐标点P_m(m＝1,2，…，n)可以根据下算式计算：

$P_m-P_{m-1}+\∫{\stackrel{\→}{V}}_{m-1}\mathrm{\δt}+\∫\∫{\stackrel{\→}{a}}_{m-1}\mathrm{\δt\δt}$

得到坐标点P_m后，利用牛顿多项式插值的方法做线路拟合，其中牛顿多项式阶数由下式来确定：

|P_m-P_j|＜P_ε

即满足上式的最小m为某段路径拟合需要的最小阶数，取n＝m即可。

但通过坐标点剔除后的坐标来进行插值时，插值的两点采样间隔不一定是Δt，可能是T＝kΔt，这里k是一个大于2的自然数(k＝2,3,4，…)，这时估计间隔因子δt＝T/k缸先进行一次插值，如果精度满足式(*)，再进行分段后的二次插值，此时取估计间隔因子δt＝Δt/n；

插值公式给出如下：

f(P)＝f[P₀，P₁](p-P₀)+f[P₀，P₁，P₂](p-P₀)(p-P₁)+…+f[P₀，p₁，...，P_n](p-P₀)(p-P₁)...(p-P_n)

其中，

$f[P,P_1]=\frac{f\left(P\right)-f\left(P_0\right)}{P-P_0};$

……；

$f[P,P_1,...,P_n]=\frac{f[P,P_0,...,P_{n-1}]-f[P_0,P_1,...,P_n]}{P-P_n};$

f(P)为经过拟合后的路径，实际应用中我们不需要将f(P)计算出来，只需要计算出Pm即可。

步骤4：地图最近路径匹配：调用地图的API接口，并根据计算上传的坐标对路径进行拟合匹配，最终显示在地图上。

本发明的算法优化集中在服务器端，主要是对用户行驶轨迹进行平滑优化，主要通过三种方式对用户轨迹进行平滑，定位漂移抑制，拐点补偿和插值，以及地图最近路径匹配。

尽管结合优选实施方案具体展示和介绍了本发明，但所属领域的技术人员应该明白，在不脱离所附权利要求书所限定的本发明的精神和范围内，在形式上和细节上可以对本发明做出各种变化，均为本发明的保护范围。

Claims (5)

1.一种行车轨迹的平滑方法，包括如下过程：

步骤1：以一定的采样频率获取车辆的行驶坐标，作为原始定位坐标，记为{P₁,…P_k,…,P_q},k＝1,2，…，q；

步骤2：对原始定位坐标{P₁,…P_k,…,P_q}进行筛选过滤，避免漂移的坐标点带来的影响；

步骤3：对经过步骤2筛选过滤后的定位坐标进行拐点补偿和插值；

步骤4：根据步骤3获得的定位坐标进行路径拟合。

2.根据权利要求1所述的行车轨迹的平滑方法，其特征在于：所述步骤2中，通过定位漂移抑制对原始定位坐标{P₁,…P_k,…,P_q}进行筛选过滤；具体筛选过程如下：

步骤21：在固定采样时间窗Δt内，若平均速度记相邻两个采样坐标P_i(x_i,y_i,z_i)，P_j(x_j,y_j,z_j)的距离为P_ij，即P_ij＝︱＝︱P_i(x_i,y_i,z_i)-P_j(x_j,y_j,z_j)︱，同时令其中R为下一坐标点可能坐落的最远区域的半径，通过平均速度和采样间隔的乘积得到；r为下一坐标点可能坐落的最近区域半径；

步骤22：判断P_ij是否落在内半径为r、外半径为R且与速度矢量的法线形成的半圆环区域内，如果P_ij落在该半圆环区域外，则过滤掉采样坐标P_j(x_j,y_j,z_j)，如果P_ij落在该半圆环区域内，则保留采样坐标P_j(x_j,y_j,z_j)；

步骤23：判断P_i(x_i,y_i,z_i)与P_j(x_j,y_j,z_j)的下一点坐标P_k(x_k,y_k,z_k)之间的距离P_ik是否落在内半径为r、外半径为R且与速度矢量的法线形成的半圆环区域内，此时如果P_ik落在该半圆环区域外，则过滤掉采样坐标P_k(x_k,y_k,z_k)，如果P_ik落在该半圆环区域内，则保留采样坐标P_k(x_k,y_k,z_k)；

步骤24：重复步骤23，每次过滤掉一点，将采样的间隔时间增加一个Δt，那么限定外半径R变为直到所有的坐标比对完毕。

3.根据权利要求2所述的行车轨迹的平滑方法，其特征在于：所述V_ε＝3m/s，P_ε＝1m。

4.根据权利要求1所述的行车轨迹的平滑方法，其特征在于：所述步骤3中，对原始定位坐标进行拐点补偿和插值，具体包括如下步骤：

步骤31：取估计间隔因子δt＝Δt/n，估计的坐标点P_m(m＝1,2，…，n)根据下算式计算：

$P_m-P_{m-1}+\∫{\stackrel{\→}{V}}_{m-1}\mathrm{\δt}+\∫\∫{\stackrel{\→}{a}}_{m-1}\mathrm{\δt\δt};$

其中，P_m-1是上一个有效的坐标点，是物体在坐标点P_m-1处的速度瞬时值，由GPS模块计算得出；是物体在坐标点P_m-1处的加速度瞬时值，由加速度计输出得到；

步骤32：得到坐标点P_m后，利用牛顿多项式插值的方法做线路拟合，其中牛顿多项式阶数由下式来确定：

|P_m-P_j|＜P_ε；

即满足上式的最小m为某段路径拟合需要的最小阶数，取n＝m。

5.根据权利要求4所述的行车轨迹的平滑方法，其特征在于：所述步骤3还包括如下步骤：

步骤33：通过坐标点剔除后的坐标来进行插值时，插值的两点采样间隔是T＝kΔt，k是一个大于2的自然数，这时估计间隔因子δt＝T/k先进行一次插值，如果精度满足式|P_m-P_j|＜P_ε，再进行分段后的二次插值，此时取估计间隔因子δt＝Δt/n；

插值公式给出如下：

f(P)＝f[P₀，P₁](p-P₀)+f[P₀，P₁，P₂](p-P₀)(p-P₁)+…+f[P₀，P₁，...，P_n](p-P₀)(p-P₁)...(p-P_n)

其中，

$f[P,P_1]=\frac{f\left(P\right)-f\left(P_0\right)}{P-P_0};$

……；

$f[P,P_1,...,P_n]=\frac{f[P,P_0,...,P_{n-1}]-f[P_0,P_1,...,P_n]}{P-P_n};$

f(P)为经过拟合后的路径。