本发明涉及自动驾驶控制技术领域，具体地，涉及一种驾驶轨迹优化系统。
背景技术：
无人驾驶技术日益发展，现有技术自动驾驶方案的主要缺点是：
1)车辆运动速度非恒定，不同速度下相同距离间隔内采集的点数不同；
2)当车速较低或者静止时，由于传感器自身噪声及外界环境噪声的影响，会造成局部采样点过多且抖动；
3)当车速较快时，单位距离内的采样点太少，横向控制器可能无法精确预测下一时刻运动趋势，尤其是在大转弯的情况下；
4)计算得到的轨迹转向角，曲率等参数有跳变，会增加驾驶控制器设计负担。
如何让车辆运行更加平稳，提高乘客的舒适度，是无人驾驶决策控制要研究的问题，车辆稳定性成为评价无人驾驶控制性能的重要因素。现有的技术无人驾驶轨迹绘制，多以一定的采样频率采集GPS数据作为预设轨迹，驾驶控制器根据预设轨迹对车辆进行横向和纵向控制，预设轨迹中往往包含坐标点、方向角、曲率等信息。而对轨迹的优化多采用后续驾驶数据的采集反馈到控制系统中，对预设轨迹进行优化。
技术实现要素：
本发明的目的是提供一种驾驶轨迹优化系统，该驾驶轨迹优化系统，得到优化的驾驶轨迹，驾驶轨迹更加平滑，根据该优化的驾驶轨迹行驶可降低驾驶控制器设计难度，提高车辆运行性能。
为了实现上述目的，本发明提供一种驾驶轨迹优化方法，该方法包括：获取驾驶轨迹的初始点列信息；对所述初始点列信息进行计算，得到所述初始点列的坐标值、曲率k和方向角θ；基于计算得到的所述初始点列的曲率，确定曲率处于预设范围内的点列作为关键点列；以及，从所述关键点列的起点向终点对所述关键点列进行若干次曲线拟合运算，将所述若干次曲线拟合运算得到的终点与所述关键点列的终点比较，确定所述拟合后的曲线的终点与所述关键点列的终点误差最小的拟合曲线为最终的驾驶优化轨迹。
优选地，从所述关键点列的起点向终点对所述关键点列进行曲线拟合运算的函数为Spiralfit(start,goal,s)，包括如下公式：
k(s)＝u(s) (2)
u(s)＝a*s3+b*s2+c*s+d (1)
其中，start为所述关键点列的起点，start＝[xs,ys,θs,ks]，xs为所述起点的横坐标，ys为所述起点的纵坐标，θs为所述起点的方向角，ks为所述起点的曲率；goal为所述关键点列的终点，goal＝[xg,yg,θg,kg]，xg为所述终点的横坐标，yg为所述终点的纵坐标，θg为所述终点的方向角，kg为所述终点的曲率；s为所述关键点列内的点与所述关键点列的起点之间的距离；a、b、c和d为拟合运算参数。
优选地，初始点列的方向角其中，Δy＝yn-yn-1，Δx＝xn-xn-1。
优选地，初始点列的曲率k＝Δθ/Δs，其中，Δθ＝θn-θn-1。
优选地，基于计算得到的所述初始点列的曲率，确定关键点列包括：当曲率k在0.05-0.15之间时，确定该点列为关键点列。
本发明还提供一种驾驶轨迹优化系统，该系统包括：获取装置，用于获取所述驾驶轨迹的初始点列信息；计算装置，用于对所述初始点列信息进行计算，得到所述初始点列的坐标值、曲率k和方向角θ；处理装置，用于基于计算得到的所述初始点列的曲率，确定曲率处于预设范围内的点列作为关键点列；其中，所述计算装置还被配置为：从所述关键点列的起点向终点对所述关键点列进行若干次曲线拟合运算，将所述若干次曲线拟合运算得到的终点与所述关键点列的终点比较，确定所述拟合后的曲线的终点与所述关键点列的终点误差最小的拟合曲线为最终的驾驶优化轨迹。
优选地，所说计算装置从所述关键点列的起点向终点对所述关键点列进行曲线拟合运算的函数为Spiralfit(start,goal,s)，包括如下公式：
k(s)＝u(s) (2)
u(s)＝a*s3+b*s2+c*s+d (l)
其中，start为所述关键点列的起点，start＝[xs,ys,θs,ks]，xs为所述起点的横坐标，ys为所述起点的纵坐标，θs为所述起点的方向角，ks为所述起点的曲率；
goal为所述关键点列的终点，goal＝[xg,yg,θg,kg]，xg为所述终点的横坐标，yg为所述终点的纵坐标，θg为所述终点的方向角，kg为所述终点的曲率；
s为所述关键点列内的点与所述关键点列的起点之间的距离；
a、b、c和d为拟合运算参数。
优选地，所述计算装置对所述初始点列的方向角其中，Δy＝yn-yn-1，Δx＝xn-xn-1。
优选地，所述计算装置对所述初始点列的曲率k＝Δθ/Δs
其中，Δ6＝θn-θn-1。
优选地，所述处理装置基于计算得到的所述初始点列的曲率，确定关键点列包括：当曲率k在0.05-0.15之间时，确定该点列为关键点列。
通过上述技术方案，获取初始的驾驶轨迹，生成驾驶轨迹的初始点列信息，对上述初始点列信息进行计算，得到初始点列的坐标值、曲率k和方向角θ；基于计算得到的所述初始点列的曲率，确定曲率处于预设范围内的点列作为关键点列；以及，从所述关键点列的起点向终点对所述关键点列进行若干次曲线拟合运算，将所述若干次曲线拟合运算得到的终点与所述关键点列的终点比较，确定所述拟合后的曲线的终点与所述关键点列的终点误差最小的拟合曲线为最终的驾驶优化轨迹。通过上述方法，消除驾驶轨噪声，优化驾驶轨迹，为更加平稳的自动驾驶汽车提供前提。
本发明的其它特征和优点将在随后的具体实施方式部分予以详细说明。
附图说明
附图是用来提供对本发明的进一步理解，并且构成说明书的一部分，与下面的具体实施方式一起用于解释本发明，但并不构成对本发明的限制。在附图中：
图1是本发明一种实施方式的驾驶轨迹优化方法流程图；
图2是本发明一种实施方式的驾驶轨迹优化方法确定关键点列示意图；
图3是初始驾驶轨迹曲率曲线；
图4是根据本发明一种实施方式的驾驶轨迹优化方法优化后轨迹的曲率曲线；
图5是本发明一种实施方式的驾驶轨迹优化系统结构示意图。
具体实施方式
以下结合附图对本发明的具体实施方式进行详细说明。应当理解的是，此处所描述的具体实施方式仅用于说明和解释本发明，并不用于限制本发明。
图1是本发明一种实施方式的驾驶轨迹优化方法。如图1所示，该方法包括：在步骤S110中，获取驾驶轨迹的初始点列信息；在步骤S120中，对所述初始点列信息进行计算，得到所述初始点列的坐标值、曲率k和方向角θ；在步骤S130中，基于计算得到的所述初始点列的曲率，确定曲率处于预设范围内的点列作为关键点列；以及，在步骤S140中，从所述关键点列的起点向终点对所述关键点列进行若干次曲线拟合运算，将所述若干次曲线拟合运算得到的终点与所述关键点列的终点比较，确定所述拟合后的曲线的终点与所述关键点列的终点误差最小的拟合曲线为最终的驾驶优化轨迹。
上述方案中，通过某种技术手段，例如通过GPS定位或视觉定位获取初始驾驶轨迹，生成初始点列信息，对上述初始点列信息进行计算，得到初始点列的坐标值、曲率k和方向角θ，基于计算得到的所述初始点列的曲率k，确定曲率k处于预设范围内的点列作为关键点列；从所述关键点列的起点向终点对所述关键点列进行若干次曲线拟合运算，将所述若干次曲线拟合运算得到的终点与所述关键点列的终点比较，确定所述拟合后的曲线的终点与所述关键点列的终点误差最小的拟合曲线为最终的驾驶优化轨迹；例如可进行三次拟合运算，将三次拟合运算得到的终点坐标与初始点列的终点坐标进行比较，选取与初始点列的终点坐标相近的曲线作为最终优化的驾驶轨迹。
上述方案的步骤S140中，从所述关键点列的起点向终点对所述关键点列进行曲线拟合运算的函数为Spiralfit(start,goal,s)，包括如下公式：
k(5)＝u(5) (2)
u(s)＝a*s3+b*s2+c*s+d (l)
其中，start为所述关键点列的起点，start＝[xs,ys,θs,ks]，xs为所述起点的横坐标，ys为所述起点的纵坐标，θs为所述起点的方向角，ks为所述起点的曲率；goal为所述关键点列的终点，goal＝[xg,yg,θg,kg]，xg为所述终点的横坐标，yg为所述终点的纵坐标，θg为所述终点的方向角，kg为所述终点的曲率；s为所述关键点列内的点与所述关键点列的起点之间的距离；a、b、c和d为拟合运算参数。
上述方案步骤S140中，从起点开始，记为start＝[xs,ys,θs,ks]；到终点结束，记为goal＝[xg,yg,θg,kg]，对各个关键点列按照距离起点的距离逐一进行上述拟合运算，例如两个关键点列的间隔为0.1，则按照s＝0.1，0.2，0.3……逐一拟合运算直至终点goal，完成一次曲线拟合运算；对初始驾驶轨迹进行3次曲线拟合，曲线拟合函数Spiralfit(start,goal,s)，将三次拟合运算的终点与初始轨迹的终点进行比较，选取与初始轨迹的终点相差最小的轨迹作为优化后的驾驶轨迹，优化后的轨迹曲率更加平滑。
上述方案中，初始点列的方向角其中，Δy＝yn-yn-1，Δx＝xn-xn-1。
上述方案中，初始点列的曲率k＝Δθ/Δs，其中，Δ6＝θn-θn-1。
上述方案中，基于计算得到的所述初始点列的曲率，确定关键点列包括：当曲率k在0.05-0.15之间时，确定该点列为关键点列。
图2是本发明一种实施方式的驾驶轨迹优化方法确定关键点列示意图。如图2所示，在轨迹的起点1和终点4被确定为关键点列，曲率k在0.05-0.15之间的点2和3被确定为弯道的入弯点和出弯点，被确定为关键点列；在进行拟合运算时，以点1为起点，按照点2到点1的距离s，根据上述函数对点2进行拟合，而后按照点3到点1的距离s，根据上述函数对点3进行拟合，直至对点4的拟合运算结束，本次驾驶轨迹的拟合运算结束，实现对当前关键点列的根据s、θ、k进行拟合的运算。对该轨迹进行三次拟合运算，分别设置不同的a、b、c和d参数值，最终根据三次拟合运算得到的终点与初始轨迹的终点4进行比较，误差最小的一次拟合运算得到的轨迹最终确定为优化的驾驶轨迹。图4和图5分别为优化前和优化后的驾驶轨迹的曲率示意曲线，从图5可见，优化后的曲率更加平滑，有效消除抖动。
上述方案中，例如还包括：对轨迹起点和终点做均值处理，因在起点和终点的停车时间较长，采样的点数较多，传感器的误差将造成采集数据的噪声，对起点和终点进行均值处理，可有效消除传感器噪声对轨迹的影响。
图5是本发明一种实施方式的驾驶轨迹优化系统。如图2所示，该系统包括：获取装置10，用于获取所述驾驶轨迹的初始点列信息；计算装置20，用于对所述初始点列信息进行计算，得到所述初始点列的坐标值、曲率k和方向角θ；处理装置30，用于基于计算得到的所述初始点列的曲率，确定曲率处于预设范围内的点列作为关键点列；其中，所述计算装置20还被配置为：从所述关键点列的起点向终点对所述关键点列进行若干次曲线拟合运算，将所述若干次曲线拟合运算得到的终点与所述关键点列的终点比较，确定所述拟合后的曲线的终点与所述关键点列的终点误差最小的拟合曲线为最终的驾驶优化轨迹，其中误差包括位置误差和角度误差。
上述方案中，计算装置20从所述关键点列的起点向终点对所述关键点列进行曲线拟合运算的函数为Spiralfit(start,goal,s)，包括如下公式：
k(s)＝u(s) (2)
u(s)＝a*s3+b*s2+c*s+d (1)
其中，start为所述关键点列的起点，start＝[xs，ys，θs，ks]，xs为所述起点的横坐标，ys为所述起点的纵坐标，θs为所述起点的方向角，ks为所述起点的曲率；goal为所述关键点列的终点，goal＝[xg，yg，θg，kg]，xg为所述终点的横坐标，yg为所述终点的纵坐标，θg为所述终点的方向角，kg为所述终点的曲率；s为所述关键点列内的点与所述关键点列的起点之间的距离；a、b、c和d为拟合运算参数。
上述方案中，所述计算装置20计算所述初始点列的方向角θ的公式为：其中，Δy＝yn-yn-1，Δx＝xn-xn-1。
上述方案中，所述计算装置20计算所述初始点列的曲率k的公式为：k＝Δθ/Δs；其中，Δθ＝θn-θn-1。
上述方案中，所述处理装置30基于计算得到的所述初始点列的曲率，确定关键点列包括：当曲率k在0.05-0.15之间时，确定该点列为关键点列，当曲率在0.05-0.05之间的点列是为弯道，作为驾驶轨迹的关键点列。
上述方案的驾驶轨迹优化系统，消除测量数据的噪声，使驾驶轨迹平滑，根据得到的优化驾驶轨迹控制驾驶，有效降低驾驶控制器设计难度，提高车辆运行性能。
以上结合附图详细描述了本发明的优选实施方式，但是，本发明并不限于上述实施方式中的具体细节，在本发明的技术构思范围内，可以对本发明的技术方案进行多种简单变型，这些简单变型均属于本发明的保护范围。
另外需要说明的是，在上述具体实施方式中所描述的各个具体技术特征，在不矛盾的情况下，可以通过任何合适的方式进行组合。为了避免不必要的重复，本发明对各种可能的组合方式不再另行说明。
此外，本发明的各种不同的实施方式之间也可以进行任意组合，只要其不违背本发明的思想，其同样应当视为本发明所公开的内容。