一种轻量级计算下基于样条插值的TEB高速导航方法

本发明涉及一种轻量级计算下基于样条插值的teb高速导航方法，在计算资源受限的环境中，通过利用三次样条插值来优化teb算法生成的轨迹的方法，旨在提高自动驾驶车辆和移动机器人在复杂环境中的导航效率和平滑性，同时确保对计算资源的需求保持在最低限度，属于移动机器人导航。

背景技术：

1、随着自动驾驶技术的快速发展，对于移动机器人和自动驾驶车辆在各种环境下的高效、安全导航能力的要求日益增加。特别是在城市道路和复杂环境中，路径规划和避障算法必须能够处理障碍物，同时确保轨迹的平滑性，以尽可能避免避障失败现象发生，并减少对车辆动力系统的冲击。

2、teb(timed elastic band)算法作为一种流行的解决方案，为路径规划和避障提供了有效的框架，传统的teb算法通过动态优化一系列时间参数化的轨迹点来应对环境中的动态障碍物，但其在处理尖锐转角和高曲率轨迹时的性能限制，以及对计算资源的较高需求，成为了在资源受限环境下实现高速安全导航的主要障碍，teb算法在轨迹平滑性和计算效率方面的不足限制了其在资源受限条件下的应用。因此，亟需通过优化算法结构和计算流程，以显著提升智能车在室外自动驾驶场景中完成自动驾驶任务的效率和安全性。

技术实现思路

1、针对现有技术的不足，本发明提供一种轻量级计算下基于样条插值的teb高速导航方法，通过三次样条插值对teb算法输出的轨迹进行平滑处理，旨在优化轨迹的平滑性，降低计算负担，从而在不牺牲导航精度和响应速度的前提下，增强自动驾驶系统的实用性和效率，为自动驾驶车辆和移动机器人在资源受限环境下的高速安全导航提供了一种新的解决方案。

2、本发明采用如下技术方案：

3、一种轻量级计算下基于样条插值的teb高速导航方法，包括如下步骤：

4、s1：收集轨迹数据

5、利用自动驾驶车辆上的激光雷达与车载里程计收集周围环境的数据，并通过teb算法初步规划出从当前位置到目标位置的轨迹，该轨迹由一系列离散的数据点组成，每个点包含位置和可能的方向信息；

6、s2：对轨迹数据预处理：对teb算法产生的轨迹数据进行初步处理，包括去除异常点和进行必要的数据平滑，为后续插值准备干净、准确的数据基础；

7、s3：构建三次样条插值

8、将预处理后的轨迹点按照其在路径中的位置进行参数化，引入参数t，代表从起点到当前点的预估时间，将轨迹点转换为参数方程的形式，基于参数化的轨迹点构建一条平滑的路径，对于轨迹上的每一段，使用三次多项式s(t)＝at3+bt2+ct+d来近似这段轨迹，其中t为参数，a、b、c、d为多项式的系数，每两个相邻的轨迹点之间的路径由一个独立的三次样条段表示；多项式的系数通过最小化轨迹与s1得到的原始轨迹之间的偏差以及轨迹的总曲率来确定，使得插值曲线在每个分段和整体轨迹上均连续，并设置合适的边界条件，包括起点和终点处的速度和加速度，在连接点(即相邻两段三次样条曲线的交点)上，要求一阶和二阶导数连续，以确保轨迹的平滑性，这意味着车辆在沿着优化后的轨迹行驶时，速度和加速度不会发生突变；

9、s4：动态调整机制

10、根据自动驾驶车辆的实时运动状态和周围环境变化，动态调整三次样条曲线的参数，以优化轨迹的平滑性和适应性，降低因轨迹突变导致的不必要的加速和减速，通过以上步骤，可以优化计算复杂度，确保轨迹的实时计算和更新能够在有限的计算资源下完成。

11、本发明通过引入三次样条插值，意在生成更加平滑的轨迹。三次样条插值能够在关键路径点之间插入光滑曲线，有效减少路径的曲率变化，从而提高导航的成功率和安全性。考虑到自动驾驶系统往往在计算资源受限的环境中运行，通过算法优化，减少了对计算资源的需求。通过精心设计的数据结构和算法流程，减少了计算量，加快了轨迹规划的响应速度。在不牺牲路径规划精度的前提下，利用优化轨迹计算过程，实现了更高的导航效率。除此之外，本发明提供的方法易于集成到现有的自动驾驶和移动机器人系统中，且具有良好的扩展性，支持未来技术和算法的更新。通过标准化的接口和模块化的设计，本方法不仅可以与不同的传感器和控制系统兼容，还可以根据特定应用需求进行定制和优化，以适应不断发展的自动驾驶技术需求。

12、优选的，步骤s1中，首先构建环境地图，采用gmapping算法作为基础框架，它通过粒子滤波技术实现，用于同时定位与地图构建(slam)的目的，该算法是一种广泛认可的现有算法；

13、在gmapping算法的实施过程中首先生成一组粒子，每个粒子代表自动驾驶车辆可能的位姿(位置和朝向)及对应的初始地图；并进行粒子初始化；

14、使用激光雷达与车载里程计采集到的数据，将时间戳对齐后打包成一帧激光里程计数据，输入到gmapping算法中，用于更新粒子的状态和估计机器人的位姿，这一更新包括：根据采集的数据调整每个粒子的位姿、使用新的位姿信息和环境数据更新每个粒子代表的地图，该过程基于标准的gmapping算法操作，在自动驾驶车辆和机器人导航中属于常规技术。

15、以上步骤在自动驾驶车辆移动过程中不断重复执行，逐步优化每个粒子的位姿估计和构建越来越精确的地图，直至整个环境被有效映射；在所有粒子对应的地图中，选择权重最高的粒子的地图作为最终的地图输出。

16、优选的，根据生成的地图，利用teb算法寻找最优位姿序列，具体来说，teb算法是在eb算法生成的路径上人为地划分时间点，从而规划出运动学轨迹，eb算法将整个路径想象成由多个节点连成的弹性带，在利用teb算法进行路径规划时，首先设定自动驾驶车辆的位置和方向，这一信息用位姿来表示：

17、si＝(xi,yi,θi)

18、位姿由三个部分组成，这里，si表示自动驾驶车辆在第i个时间点的位姿，xi,yi分别表示自动驾驶车辆在局部地图上的横向和纵向位置，而θi则是自动驾驶车辆在该局部坐标系下的航向角；

19、节点之间的相互作用力，包括相邻节点间的拉伸力和节点对路径的吸引力，共同作用使得弹性带呈现出收缩的性质，目的是为了让路径长度尽可能地减少，从而找到一个高效的行进路线。在这个过程中，我们将这个模型进行数学化处理，并定义一个用于评估路径效率的代价函数c：

20、c＝αclength+βcsmooth+γcobstacle+δckinematics

21、其中：

22、clength表示路径长度的代价，路径长度代价可以通过计算路径上所有相邻节点间距离的总和来得到；

23、csmooth表示路径平滑度的代价，用于减少转向角度的急剧变化，通过计算路径上所有连续节点间角度变化的平方和来估算；

24、cobstacle表示避障代价，用于避免与障碍物的碰撞，通过考虑路径上每个节点到最近障碍物的距离，并将这些距离的倒数进行累加计算得到；

25、ckinematics表示运动学约束的代价，确保路径的可行性。它可以基于路径是否满足特定运动学约束条件来评定，例如，通过检查路径上的每个转向是否超过了设备的最大转向角度；

26、α，β，γ，δ为权重系数，用于调整各项代价在总代价函数中的影响力度。这些权重系数通常通过优化过程获得，例如，可以基于特定应用的需求进行手动调整，或使用机器学习在一系列训练场景中自动优化，以平衡路径的长度、平滑度、安全性和运动学可行性；

27、代价函数c综合考虑了路径长度、偏离程度和运动学约束等因素，通过优化代价函数c，即寻求使其值最小的解，得到理论上的最佳路径，即得到s1初步规划的从当前位置到目标位置的轨迹。

28、优选的，粒子初始化过程中，所有粒子均被赋予等量的权重，随后的权重将基于粒子对环境的估计准确性进行调整；粒子权重的调整根据每个粒子对环境的估计准确性来进行，这一准确性的评估依赖于机器人通过激光雷达传感器与轮式里程计采集的数据。具体地，系统将采集到的实际数据与每个粒子所代表的地图进行比较，根据估计相似度的高低，相应地调整粒子的权重：估计准确性高的粒子权重增加，而估计准确性低的粒子权重减少。

29、根据粒子的权重决定它们是否被保留在新的粒子集中，权重较高的粒子将有更高的概率被选中复制，而权重较低的粒子可能被淘汰；同时，每个粒子的位姿和基于这些位姿的传感器数据被用来更新其对应的栅格地图。这个更新过程涉及根据传感器捕获的信息，如障碍物的存在，将新观测到障碍物位置投影到地图上，以此确保地图能够准确反映出当前环境的状态。

30、优选的，步骤s2中，为了确保后续插值处理使用的数据的准确性和可靠性，轨迹数据预处理是一个至关重要的步骤，定义一个序列q，序列q包含自动驾驶车辆在避障过程中的n个不同位姿，位姿依次排列，形成了自动驾驶车辆在空间中行进的轨迹：

31、q＝{s0,s1,...sn}

32、其中，对于i＝0到n，每个位姿si描述了自动驾驶车辆在特定时间点的位置和方向；

33、在teb算法中，路径规划是基于时间的，将连续位姿之间的转换划分为一系列时间步长，为了表示这种时间上的划分，引入δti(对于i＝0到n-1)，代表了从位姿si到位姿si+1之间的时间间隔，得到一个时间步长序列：

34、τ＝{δt0,δt1,...,δtn-1}

35、时间步长序列τ与位姿序q结合起来，形成了一个基于时间的位姿表示，这种表示不仅包含了汽车的空间位置和方向，还融入了时间维度，为避障过程提供了一个完整的时间-空间框架：

36、b(q,τ)

37、在得到一系列包含自动驾驶车辆在某一时刻的位置和方向信息的位姿后，对位姿信息进行预处理，使用高斯滤波器处理原始数据，有助于去除因传感器误差或环境因素引入的随机噪声。

38、优选的，高斯滤波器中根据标准差σ确定高斯核的大小和权重，将高斯核的大小选择为奇数，以确保有一个中心点；

39、标准差σ控制着权重随距离减少的速度，影响滤波的平滑程度。σ的值决定了高斯函数的宽度。较小的σ值会产生一个窄而尖的高斯核，仅有与中心点非常接近的数据点才会对滤波结果有较大贡献，保留更多细节，但去噪效果较弱。较大的σ值会生成一个宽而平的高斯核，增强了去噪效果，但会模糊一些细节。对于高斯核中的每个点，其权重由高斯函数g(m)给出：

40、

41、其中，m为当前点到核中心点的距离(对于时间序列，m表示时间步的差值)；

42、对于轨迹中的每个位姿点，使用高斯权重对其及其邻近点进行加权平均，对于位姿序列中的每个点si＝(xi,yi,θi)，其滤波后的值为：

43、

44、其中，si为滤波后的位姿，k为高斯核一侧的长度，确保总核大小为2k+1。

45、本发明将高斯滤波器与时间弹性带(teb)算法相结合用于高速导航的场景中，从而在特定的应用背景下展示了创新性。通过调整高斯核的标准差和大小，本发明可以根据车辆周围环境的变化(例如，障碍物密集区域或开放空间)灵活调整滤波程度。这种调整使得轨迹规划在不同环境条件下都能保持良好的性能，特别是在teb算法需要快速调整路径时效果优异。

46、本发明通过将高斯滤波器与teb算法相结合，不仅优化了车辆位姿数据的处理，提高了路径规划的准确性和适应性，而且还考虑了实时性和计算资源的限制，展示了在高速导航应用中的创新应用。

47、对于边界部分的处理，即位姿序列的开始和结束部分，由于缺少足够的邻近点，可以直接简单地复制边界点；

48、通过上述步骤，高斯滤波器能够有效地平滑基于时间的位姿轨迹数据，减少噪声影响，同时保持轨迹的整体形状，有助于提高自动驾驶系统的导航准确性和鲁棒性。

49、优选的，步骤s3中，通过teb算法生成了一系列时间参数化的轨迹点后，这些点不仅包含空间位置信息(如x、y坐标)，还包含到达这些位置的预估时间，参数t代表从起点到当前点的预估时间，将这个时间序列记为{t0,t1,t2,...tn}，其中t0是起始时间(通常为0)，每个ti代表到达第i个点所需的预估时间；

50、基于teb算法输出的时间戳，可以构造关于时间t的三次参数方程，从点i到i+1的每一段轨迹，定义一个局部的三次多项式：

51、

52、其中，x(t)、y(t)分别表示在时间t点上的横向、纵向位置坐标，即对应于车辆在二维空间中的x轴和y轴位置；ax,bx,cx,dx,ay,by,cy,dy为该段轨迹的多项式系数，需要根据每段轨迹的起始和结束条件(位置、速度、加速度)来确定。这种方式可以使轨迹在每一段都保持连续和光滑，同时能够根据时间戳适应动态约束；

53、为了适配teb算法输出，参数t的范围需要根据每一段轨迹的起始和结束时间戳来调整：

54、从teb算法的输出中提取每一段轨迹的起始时间戳ti和结束时间戳ti+1，将时间参数t的范围对应规范调整到每段轨迹的实际时间范围ti和ti+1。这意味着对于轨迹中的每一段，参数t都从ti开始，到ti+1结束。具体的，规范化的时间t通过以下转换公式计算：

55、

56、其中，t′表示原始时间戳。

57、优选的，轨迹的多项式系数根据每段轨迹的边界条件确定，边界条件包括位置、速度和加速度；

58、位置边界条件包括：起点、中间控制点(即起点和终点中间的点)、终点的坐标(即关键点)，分别表示为x(ti)、y(ti)、x(ti+1)和y(ti+1)；

59、速度边界条件：为了使轨迹平滑且动态，使用条件限制在起点和终点处速度，起点和终点的速度分别为vxi,vyi和vxi+1,vyi+1；

60、加速度边界条件：加速度条件进一步控制轨迹的平滑度和车辆的动态响应。在起点和终点处指定加速度分别表示为axi,ayi和axi+1,ayi+1；

61、根据插值条件、连续性条件和边界条件建立线性方程求解多项式系数。

62、优选的，考虑一个简单的情况，假设需要规划一条从起点到终点的轨迹，并且已知起点和终点的位置、速度、加速度。为了简化，仅展示一维(如x方向)的求解过程，y方向可以通过相同的方法求解。可以通过已知条件来求解四个未知系数，具体过程如下：

63、(1)插值条件：确保在每个给定的时间点ti，样条函数xi(t)与对应的轨迹点值相等，即：

64、

65、(2)连续性条件：确保样条在每个连接点上的一阶导数和二阶导数连续，即：

66、

67、(3)边界条件：根据实际问题需求设置。一般情况下，可以选择以下一种：

68、第一类边界条件：给定端点处的一阶导数值：

69、

70、第二类边界条件：给定端点处的二阶导数值：

71、

72、自然边界条件为x′0′＝x′n′＝0；

73、第三类边界条件：周期性边界条件，适用于周期函数，要求：

74、

75、根据上述(1)、(2)、(3)的条件，建立一个线性方程组来求解每个区间上的系数ax,bx,cx,dx。对于非边界点，有n-1个区间和4(n-1)个系数需要求解，而上述条件提供了足够的方程来求解这些未知数。一旦系数被确定，就可以计算出给定时间点t的插值，同时，通过这些三次多项式，可以直接绘制出各区间段的拟合曲线图，得到优化后的轨迹。

76、优选的，步骤s4的核心在于基于车辆的实时运动状态和周围环境条件的变化，灵活地调整用于路径规划的三次样条曲线参数。通过这种方法，不仅能够生成更加平滑的行驶轨迹，而且还能够使轨迹在面对环境变化时表现出高度的适应性。

77、步骤s4中，利用激光雷达与车载里程计持续监测自动驾驶车辆当前的运动状态和周围环境条件，包括车速、加速度、周围障碍物的位置和移动趋势，基于收集到的数据，实时评估环境条件的变化，teb算法判断这些变化是否对当前轨迹产生影响，是否需要进行轨迹调整从而生成一系列新的时间参数化的轨迹点；若需要调整轨迹，将根据新的时间参数化的轨迹点重复进行三次样条插值过程，从而生成调整后的样条曲线，以适应环境变化并优化轨迹的平滑性。使用调整后的样条曲线，重新生成一条优化后的轨迹。这一轨迹不仅更平滑，而且能够有效避开新出现的障碍物，适应路况变化。动态调整是一个持续的过程，系统会不断地反馈当前行驶状况和环境信息，根据最新的情况调整轨迹。这种快速迭代确保了轨迹在整个行驶过程中始终保持最优状态。

78、本发明未详尽之处，均可参见现有技术。

79、本发明的有益效果为：

80、本发明利用三次样条插值的高阶平滑特性，结合动态参数调整，实现在各种行驶速度和计算能力受限的条件下，无人车辆的精确定位、高效建图和平滑导航，进而提升自动驾驶车辆的路径规划性能。本发明不仅优化了轨迹的平滑性、提高了路径规划的灵活性和鲁棒性，还降低了对计算资源的依赖，著提升了车辆应对复杂环境的能力。

81、本发明将三次样条插值方法与时间弹性带(teb)导航方法相结合，与传统的三次样条插值方法相比，本发明在样条曲线的参数选择上引入了动态调整机制，自动调整以生成最优化的导航路径。这种动态调整能够使得导航轨迹不仅更加平滑，而且具有更高的适应性，能够灵活应对环境变化。通过引入三次样条插值，优化了时间弹性带中轨迹的生成过程，使得路径规划在满足所有动态约束的同时，还能保证轨迹的平滑性和连续性，降低因轨迹突变引起的运动不稳定性。并且减少了teb算法在处理复杂环境下的计算负担，三次样条的平滑特性减少了对高频噪声的敏感度，减轻了后续优化阶段的计算压力，提高了路径规划的整体效率和实时性。