一种航点拟合和分段航线拼接的方法与流程

本发明涉及图像处理领域，特别是涉及一种航点拟合和分段航线拼接的方法。

背景技术：

近年来随着智能驾驶领域的飞速发展，大量的汽车企业开始着手对智能驾驶展开研究。通过建立航线库管理系统可管理上传的航线并再次调用，在航线调用的时候需要将分段航线组合成完整航线，但现如今没有很好的方法来连接分段航线。除此以外，车辆行驶过程中通常是固定时间间隔采集航点坐标，而上传时需要将此数据转换为分段航线存储于航线数据库中，因此有必要研究一种将航点归类、拟合成航线的方法。

三次样条插值曲线属于分段光滑插值，是应用于二维图形应用程序的数学曲线三次样条曲线的数学模型如公式(1)所示。

当x∈[xj-1,xj]时，s(x)的表达式为：

其中xj-1、xj、yj-1和yj为两个控制点的坐标，hj＝xj-xj-1，因此只要确定m0，m1…，mn这n+1个待定参数就可得到三次样条插值函数在各个子区间上的表达式sj(x)。

自然边界条件的m0＝mn＝0,未知元m1，m2…，mn-1可通过下列方程求解：

其中

式中f[xj-1,xj,xj+1]为关于xj-1、xj、xj+1三点的二阶差商。

贝赛尔曲线，是应用于二维图形应用程序的数学曲线，三次贝赛尔曲线的数学模型如公式(4)所示：

b(t)＝(1-t²)p0+2t(1-t)p1+t²p2,t∈[0,1](4)

式中p0、p1和p2分别表示起始点、控制点和终点。

技术实现要素：

本发明要解决的技术问题是，提供一种将航点分类、去噪并拟合成分段航线进行存储的方法，同时要解决调用时分段航线的拼接问题。

本发明为解决上述技术问题所采用的技术方案包括以下步骤：

1)将采集到的航点数据转换为二维平面的点集{p1、p2……pn}，点集包含相应点的坐标；

2)根据相邻航点间距离的大小，利用聚类算法将航点分类l1、l2……lm、g，其中l1、l2……lm为第1至第m条路段上的航点集，g为孤立点集，不参与后续航线的拟合；

3)对于任意航点集li中的所有航点，按其位置关系排序，分别将起点至终点的航点坐标依次记为p'i1、p'i2……p'ij，排序完成后航点集更新为l'i；

4)利用三次样条插值函数拟合航点集l'i中p'i1至p'ij间的航线，选取自然边界条件；

5)拼接航线时，利用贝塞尔曲线拟合两段航线的间隙。以前一段航线的终点p'(i-1)j为贝塞尔曲线的起点，后一段航线的起点p'i1为贝赛尔曲线的终点，两点处切线的交点为控制点做贝赛尔曲线。

作为本发明的优选实施例，所述步骤2中聚类算法的具体步骤为：

1)初始化类别数n＝0，i＝0，孤立集

2)判断是否存在未分类采样点，若存在，执行第3步，若不存在则该算法结束；

3)搜索未分类采样点pi的邻域uε(pi)内是否存在其他未分类的采样点，其中聚类阈值ε＝3·min{d(pi,pj)}。若存在，继续执行第4步；若不存在，则将pi归类为孤立点集g，令g＝gu{pi}，i＝i+1，回到第2步搜索未分类采样点；

4)n＝n+1，令pn1＝pi，建立新的航点集ln＝{pn1}，初始化航点集中的点数m＝1；

5)统计新加入的采样点的邻域内其他未分类采样点的个数k，建立点集{pn(m+1),pn(m+2)…,pn(m+k)}。分别将采样点pi邻域内k个点赋值给pn(m+1)，pn(m+2)…，pn(m+k)，令ln＝lnu{pn(m+1),pn(m+2)…,pn(m+k)}，并更新点数m＝m+k；

6)搜索新加入的采样点pn(m-k+1)，pn(m-k+2)…，pnm的邻域内是否有未分类的采样点，若存在，回到第5步将这些点加入航点集ln中，若不存在，则该分类完成，i＝i+1，回到第2步；

步骤3中排序算法的具体步骤为：

1)初始化i＝1

2)搜某个航点集ln中的航点pni的邻域uε(pni)内所有属于该航点集的航点；

3)若航点pni的邻域uε(pni)内只有一个其他航点，则该点为端点，转至步骤5；若航点pni的邻域uε(pni)内存在的其他航点数大于1，转至步骤4；

4)判断是否满足对有若满足，则该点为端点，转至步骤5；若不满足则该点不是端点，令i＝i+1，转至步骤2搜索下一个点；

5)判断p'n1是否已存在，若不存在则令p'n1＝pni，i＝i+1并转至步骤1；若已存在p'n1，则令p'nm＝pni，端点搜索过程结束，转至步骤6。其中m为航点集的点数，p'n1和p'nm分别表示该航点集的起点和终点；

6)初始化j＝1，

7)搜索p'nj的邻域uε(p'nj)内所有的其他未分类采样点，若点pk满足对于有d(p'ni,pk)≤d(p'ni,px)，则pk为下一个点，令j＝j+1，p'nj＝pk；

8)判断是否有j＝m-1，若是，则排序完成，转到步骤9，若不是，返回第7步继续搜索下一个点；

9)建立排序完成的航点集l'n＝{p'n1,p'n2…,p'nm}。

在步骤4用三次样条插值曲线绘制任一航点集l'i的航线时选取自然边界条件，即在起点p'i1和终点p'im处曲线的二阶导数为0。根据公式(1)、(2)和(3)分别计算p'i1与p'i2、p'i2与p'i3……p'i(m-1)与p'im之间的三次样条函数。计算完成后将曲线存储至航线数据库。

当需要调用航线时，从航线库中选取相应的航线段，通过步骤5的贝塞尔曲线拟合航线段之间的间隙。

本发明的有益效果是，通过聚类算法可以将航点分段归类，扫描或上传过程中产生的噪点也可以通过此方法去除；自然边界条件下三次样条拟合的曲线具有良好的数学性状，曲线具有连续的二阶导数；调用航线时，利用贝赛尔曲线拼接两段航线只需要拼接点的坐标和切线斜率，不需要调用原始的航点信息，可有效减少计算量，提高效率。

附图说明

图1为本发明航点拟合和分段航线拼接方法的步骤；

图2为本发明航点聚类算法的流程图；

图3为本发明航点集排序算法的流程图。

具体实施方式

以下通过特定的具体实施例说明本发明的实施方式，本领域的技术人员可由本说明书所揭示的内容轻易地了解本发明的其他优点及功效。

下面结合附图和具体实施例对本发明进行详细说明。

如图1所示：

1.将采集到的航点数据转换为二维平面点集{p1、p2……pn}；

2.利用聚类算法将航点分类l1、l2……lm、g，其中l1、l2……lm为第1至第m条路段上的航点集，g为孤立点集，不参与后续航线的拟合。具体算法如下(如图2所示)：

1)初始化类别数n＝0，孤立集

2)判断是否存在未分类采样点，若存在，执行第3步，若不存在则该算法结束；

3)搜索未分类采样点pi的邻域uε(pi)内是否存在其他未分类的采样点，其中聚类阈值ε＝3·min{d(pi,pj)}。若存在，继续执行第4步；若不存在，则将pi归类为孤立点集g，令g＝gu{pi}，回到第2步搜索未分类采样点；

4)n＝n+1，令pn1＝pi，建立新的航点集ln＝{pn1}，初始化航点集中的点数m＝1；

5)统计新加入的采样点的邻域内其他未分类采样点的个数k，建立点集{pn(m+1),pn(m+2)…,pn(m+k)}。分别将采样点pi邻域内k个点赋值给pn(m+1)，pn(m+2)…，pn(m+k)，令ln＝lnu{pn(m+1),pn(m+2)…,pn(m+k)}，并更新点数m＝m+k；

6)搜索新加入的采样点pn(m-k+1)，pn(m-k+2)…，pnm的邻域内是否有未分类的采样点，若存在，回到第5步将这些点加入航点集ln中，若不存在，则该分类完成，回到第2步；

3.对于任意航点集li中的所有航点，按其位置关系排序，分别将起点至终点的航点坐标依次记为p'i1、p'i2……p'ij，排序完成后航点集更新为l'i。具体算法如下(如图3所示)：

1)初始化i＝1；

2)搜索某个航点集ln中的航点pni的邻域uε(pni)内属于该航点集的所有航点；

3)若航点pni的邻域uε(pni)内只有一个其他航点，则该点为端点，转至步骤5；若航点pni的邻域uε(pni)内存在的其他航点数大于1，转至步骤4；

4)判断是否满足对有若满足，则该点为端点，转至步骤5；若不满足则该点不是端点，令i＝i+1，转至步骤2搜索下一个点；

5)判断p'n1是否已存在，若不存在则令p'n1＝pni，i＝i+1并转至步骤1；若已存在p'n1，则令p'nm＝pni，端点搜索过程结束，转至步骤6。其中m为航点集的点数，p'n1和p'nm分别表示该航点集的起点和终点；

6)初始化j＝1；

7)搜索p'nj的邻域uε(p'nj)内所有的其他未分类采样点，若点pk满足对于有d(p'ni,pk)≤d(p'ni,px)，则pk为下一个点，令j＝j+1，p'nj＝pk；

8)判断j＝m-1是否成立，若是，则排序完成，转到步骤9，若不是，返回第7步继续搜索下一个点；

9)建立排序完成的航点集l'n＝{p'n1,p'n2…,p'nm}。

4.对排序完成后的航点集中的航点用三次样条函数插值，选取自然边界条件。例如拟合航点集l'i中的航点时，令起点p'i1和终点p'im处曲线的二阶导数为0，根据公式(1)、(2)和(3)分别计算p'i1与p'i2、p'i2与p'i3……p'i(m-1)与p'im之间的三次样条函数，计算完成后将分段函数整合成完整的函数并存储于航线数据库中。

5.当需要调用航线时，从航线库中选取相应的航线段，以前一段航线的终点p'(i-1)j为贝塞尔曲线的起点，后一段航线的起点p'i1为贝赛尔曲线的终点，两点处切线的交点为控制点，参照公式4中贝赛尔曲线的数学模型拟合两航线段之间的空隙。

以上所述仅为本发明的较佳实施例而已，并不用以限制本发明，凡在本发明的精神和原则之内所作的任何修改、等同替换和改进等，均应包含在本发明的保护范围之内。