近年来，随着人工智能技术迅速发展，移动机器人已经被广泛应用于工业、农业和服务业等诸多领域^[1-3]. 路径规划作为移动机器人的核心技术之一，一直是机器人研究领域的重要课题,关系到机器人能否顺利作业^[4]. 路径规划的目标是在周围环境存在障碍物的情况下，规划一条从起始点到终点的最优路径^[5].

路径规划算法根据环境信息的不同分为全局路径算法和局部路径算法^[6]. 目前，常见的全局路径算法包括A*算法^[7]、RRT算法^[8]、遗传算法^[9]等. 为了满足机器人在移动过程中做到实时避障，须将全局路径算法与局部路径算法配合使用.

局部路径规划算法是一种实时的规划方法，主要用在动态或未知的环境中，根据机器人感知到的环境信息来调整机器人的运动速度和方向，完成避障和到达目标点^[10-12]. Rösmann等^[13-15]提出的时间弹性带（time elastic band，TEB）算法是一种典型的局部路径规划算法，该算法基于优化理论将机器人的运动轨迹建模为一个弹性带（elastic band，EB），并在弹性带上加入时间信息，通过优化算法调整弹性带的形状和时间插值分配，使其满足动力学约束和环境约束，从而生成平滑且可行的路径. Fox等^[16]提出动态窗口法（dynamic window approach，DWA）算法，DWA算法通过机器人的运动学模型，以及当前速度、加速度和环境信息，生成一个动态窗口，该动态窗口的边界由机器人的各种运动学约束来决定，然后通过评估函数对生成的每条轨迹进行评估，将最佳速度向量作为机器人的控制输入，以实现在动态环境中的避障和路径优化^[17-19]. 人工势场法（artificial potential field approach，APF）^[20-22]通过模拟物理场，将目标位置设定为引力源，障碍物设定为排斥源，使得机器人在这些虚拟力的共同作用下进行移动. TEB算法将路径规划问题建模为时间参数化的优化问题，并引入了速度、加速度及路径连续性的约束，相较DWA算法能够生成更加平滑且动态可行的轨迹；同时，TEB通过全局优化的弹性带结构，改善了APF算法容易陷入局部极小值的问题，具备更强的鲁棒性与环境适应能力.

传统TEB算法在复杂环境下存在速度跳变和运行不平滑问题^[23]. Chen等 ^[24]提出模糊逻辑控制-TEB算法，增加了平滑度和加加速度目标，使轨迹更平滑，控制更稳定. 代婉玉等^[25]对不规则障碍物实施膨胀化处理并采用区域分级方法，同时将障碍物距离纳入速度的约束中. 陈奕梅等^[26]将TEB算法与速度障碍法相融合，提出自适应参数动态调节机制，该机制可动态调整轨迹离散间隔及机器人最大允许线速度.

本研究针对TEB算法在复杂环境中出现的加速度曲线突变和算法调节路径节点数量与环境动态需求不匹配导致机器人运动不稳定的问题，在原始TEB算法的基础上引入加加速度（jerk）约束来平滑速度和加速度曲线，同时提出一种时间分辨率自适应调整方法，使弹性带节点密度能够根据不同环境特性动态变化，减少了机器人运行过程中的震荡、抖动现象，并通过实验验证方法的有效性.

基于四轮两驱差速小车开展实验研究. 为了简化移动机器人的控制算法的实现，作出如下假设.

1) 刚体假设：忽略结构弹性变形，将机器人视为刚体系统;

2) 纯滚动约束：假设驱动轮与地面接触点满足纯滚动条件，无相对滑动；

3) 平面运动假设：限定机器人在二维平面内运动，忽略地面高度变化及垂直方向动力学影响.

差速机器人模型如图1所示，其前侧2个车轮为驱动轮，后侧2个车轮为全向轮，可视作两轮差速模型，($ {X}_{{\mathrm{w}} } $, $ {Y}_{{\mathrm{w}}} $, $ {Z}_{{\mathrm{w}} } $)为世界坐标系,$ l $为2个驱动轮中心距离的一半，ICR为机器人的瞬时旋转中心，R为转弯半径，$ {v}_{{\mathrm{l}}} $为左轮速度，$ {v}_{{\mathrm{r}}} $为右轮速度，$ v $为机器人线速度，$ \theta $为航向角.

机器人车轮速度、角速度和转弯半径存在以下关系：

(1)$ {v}_{{\mathrm{l}}}=\omega (R-l),\;{v}_{{\mathrm{r}}}=\omega (R+l). $

式中：$\omega $为角速度.

进而，差动移动机器人线速度$ {v}_{} $和角速度$ {\omega }_{} $，可以通过以下公式进行计算：

(2)$ v=\frac{{v}_{{\mathrm{r}}}+{v}_{{\mathrm{l}}}}{2},\;\omega =\frac{{v}_{{\mathrm{r}}}-{v}_{{\mathrm{l}}}}{2l} .$

弹性带算法是由n个中间机器人的位姿$ {\boldsymbol{s}}_{i}= {\left[{x}_{i},{y}_{i},{\beta }_{i}\right]}^{\mathrm{T }}\in {\bf{R}}^{\text{2}}\times {{\bf{S}}}^{\text{1}} $序列组成的，其中$ {x}_{i},{y}_{i} $为机器人在全局地图中的位置，$ {\beta }_{i} $为在全局地图中机器人与坐标系$ x $轴的夹角. 位姿序列表达式如下：

(3)$ Q={\left\{{\boldsymbol{s}}_{i}\right\}}_{i=0,\cdots,n}; \; \begin{array}{c}\end{array}n\in \bf{N}. $

式中：$ \textit{Q} $为位姿序列，$ {\boldsymbol{s}}_{i} $为位姿点.

TEB算法在EB算法基础上进行了改进，通过引入2个连续位姿之间的时间信息$ \Delta T $，如图2所示，从而产生了n−1个时间间隔的序列，表达式如下：

(4)$ \tau ={\left\{\Delta {T}_{i}\right\}}_{i=0,\cdots,n-\text{1}} . $

式中：$ \tau $为时间序列，$ \Delta {T}_{i} $为位姿点2个相邻位姿之间的时间间隔.

将位姿序列与时间序列合并，在TEB算法中将待优化的变量定义为

(5)$ \boldsymbol{B}: =\left(Q,\tau \right) . $

TEB算法的核心思想是使用加权和模型，通过标量化多目标优化方法，对TEB算法的位姿序列进行实时适应性调整和优化：

(6)$ f\left(\boldsymbol{B}\right)=\sum\limits_{k}{\gamma }_{k}{f}_{k}\left(\boldsymbol{B}\right) ,$

(7)$ {\boldsymbol{B}}^{\mathbf{*}}=\underset{{\boldsymbol{B}}}{\arg \min }\;f\left(\boldsymbol{B}\right) . $

式中： $ f(\boldsymbol{B}) $为加权后基础的全局目标函数，$ {\gamma }_{k} $为权重值，$ {f}_{k}(\boldsymbol{B}) $为各种约束条件下的目标函数，$ {\boldsymbol{B }}^{*} $为最优轨迹.

在TEB算法中，目标函数的约束主要由2种基本类型构成. 第1类是基于惩罚函数的约束，核心是对机器人的速度与加速度实施边界限制；第2种是涉及轨迹优化的目标函数约束，例如最短路径、最短执行时间以及与障碍物的距离等. 稀疏约束在机器人框架中难以自由使用，因此这些约束被转化为目标函数的形式，并通过分段连续且具备可微分的成本函数来评估违反约束的惩罚值. 误差函数表达式如下：

(8)$ {e}_{\Gamma }\left(x,{x}_{\text{r}},\varepsilon ,S,n\right)\simeq \left\{\begin{matrix}{\left(\dfrac{x-\left({x}_{\text{r}}-\varepsilon \right)}{S}\right)}^{n}， & x \gt {x}_{\text{r}}-\varepsilon； \\0, &其他 .\end{matrix}\right. $

式中：$ {x}_{\text{r}} $表示边界值，$ S $表示调整比例，$ n $为多项式的阶数，$ \varepsilon $为值很小的误差.

路径点约束与障碍物约束的主要机理可类比于人工势场方法. 其中，路径点对机器人产生吸引作用，引导其朝向目标运动；障碍物则产生排斥作用，迫使机器人远离危险区域，从而生成安全路径. 路径跟随目标函数和障碍物目标函数表达式分别为

(9)$ {f}_{{\mathrm{path}}}={e}_{\Gamma }\left({d}_{\min ,j},{r}_{{{\text{p}}_{\text{max}}}},\varepsilon ,S,n\right) , $

(10)$ {f}_{{\mathrm{ob}}}={e}_{\Gamma }\left(-{d}_{\min ,k},-{r}_{{{\text{o}}_{\text{min}}}},\varepsilon ,S,n\right) . $

式中：$ {d}_{\min ,j} $为机器人位姿点与第$ j $个路径点的距离，$ {r}_{\text{p}_{{\mathrm{max}}}} $为机器人与路径点距离的阈值，$ \varepsilon $为偏移因子，$ S $为缩放比例，$ n $为阶数，$ {d}_{\min ,k} $为机器人位姿点与第$ k $个障碍物的最小距离，$ {r}_{\text{o}_{{\mathrm{min}}}} $为机器人与障碍物距离的阈值.

机器人的速度和加速度的动态约束通过惩罚函数来进行描述. 根据连续的2个位姿点$ {\boldsymbol{s}}_{i} $和$ {\boldsymbol{s}}_{i+1} $之间的欧氏距离和角度偏移值以及2个位姿之间的时间间隔$ \Delta {T}_{i} $来计算平均的移动速度和旋转速度. 移动速度和转速表达式分别如下：

(11)$ {v}_{i}\simeq \frac{\text{1}}{\Delta {T}_{i}}\left|\left|\left(\begin{array}{c}{x}_{i+\text{1}}-{x}_{i}\\{y}_{i+\text{1}}-{y}_{i}\end{array}\right)\right|\right|， $

(12)$ {\omega }_{i}\simeq \left({{\beta }_{i+1}-{\beta }_{i}}\right)/{\Delta {T}_{i}} . $

加速度与2个连续的平均速度有关，公式如下：

(13)$ {a}_{i,v}={\text{2}\left({v}_{i+\text{1}}-{v}_{i}\right)}/\left({\Delta {T}_{i}+\Delta {T}_{i\text{+1}}}\right) , $

(14)$ {a}_{i,\omega }={\text{2}\left({\omega }_{i+\text{1}}-{\omega }_{i}\right)}/\left({\Delta {T}_{i}+\Delta {T}_{i\text{+1}}}\right) . $

差分驱动的机器人只有2个局部的自由度，即只能在机器人当前运动朝向的方向上移动，这种约束使得路径展示出由弧形构成的平滑特性. 因此，相邻的2个位姿必须位于一个曲率近似恒定的公共弧形上，如图3所示. 其中，初始位姿$ {\boldsymbol{s}}_{i} $与方向$ {\boldsymbol{d}}_{i,i+\text{1}} $的角度$ {\theta }_{i} $必须与$ {{{{\boldsymbol{s}}}}}_{i+\text{1}} $处的角度$ {\theta }_{i+\text{1}} $相等. $ {\beta }_{i} $为机器人在第$ i $个位姿处相对于世界坐标系的航向角，则弧形条件为

(15)$ {\theta }_{i}={\theta }_{i+\text{1}}\Leftrightarrow \left[\begin{array}{c}\cos \;{\beta }_{i}\\\sin \;{\beta }_{i}\\0\end{array}\right]\times {\boldsymbol{d}}_{i,i+\text{1}}={\boldsymbol{d}}_{i,i+\text{1}}\times \left[\begin{array}{c}\cos \;{\beta }_{i+\text{1}}\\\sin \;{\beta }_{i+\text{1}}\\0\end{array}\right]. $

其中，$ {\boldsymbol{d}}_{i,i+\text{1}} $可表示为

(16)$ {\boldsymbol{d}}_{i,i+\text{1}}=\left[\begin{array}{c}{x}_{i+\text{1}}-{x}_{i}\\{y}_{i+\text{1}}-{y}_{i}\\0\end{array}\right] . $

相应的目标函数表达式如下：

(17)$ {f}_{k}\left({x}_{i},{x}_{i+\text{1}}\right)={\left|\left|\left(\left[ \begin{array}{c}\cos \;{\beta }_{i}\\\sin \;{\beta }_{i}\\0\end{array} \right]+\left[ \begin{array}{c}\cos \;{\beta }_{i+\text{1}}\\\sin \;{\beta }_{i+\text{1}}\\0\end{array} \right]\right)\times {\boldsymbol{d}}_{i,i+\text{1}}\right|\right|}^{\text{2}}. $

EB方法通过对路径进行收缩和调整，实现对原始路径的优化. 在此基础上，TEB算法引入了时间维度信息，将仅以最短路径为优化目标扩展为以最快路径为目标，或将最短路径与最快路径这2类目标进行联合优化. 其中，最快路径目标通常通过最小化相邻路径点之间时间间隔的平方和实现.

(18)$ {f}_{k}={\left(\sum\limits_{i=\text{1}}^{n}\Delta {T}_{i}\right)}^{2} . $

TEB 算法的核心在于采用加权和模型，对包含时间间隔信息的机器人位姿序列进行联合优化，将各类约束统一建模为变量化的多目标优化问题，从而生成满足动力学约束的最优轨迹，如式（7）所示.

TEB算法将机器人的位姿、相邻位姿的时间间隔以及障碍物等作为节点，然后通过g2o框架，将目标函数与受其影响的节点经由边连接起来，由此构建出超图（Hyper-graph）. 之后借助g2o框架中的求解器进行求解.

超图是一种对传统图结构的扩展，其边不再局限于连接2个节点. 超图中的边（hyper-edge）可同时关联多个节点，用以描述多变量之间的约束关系. 在 TEB 框架中，每个目标函数依赖于TEB元组中的一个子集，并由一个超边表示，该超边连接所有与该目标函数相关的节点，这些节点以参数形式参与优化. 如图4所示，展示了较多目标函数实现的TEB超图. 单圈表示待优化的状态变量，双圈表示固定或受约束的状态节点，双圈节点值固定，不受优化改变，方框表示超边或目标函数节点，节点将每条多边形内的目标函数按照其权重添加到总体的目标函数中. 从图中可以看出，除了固定障碍节点外，路径点${\boldsymbol{ P}}_1 $和初始状态$ {{\boldsymbol{s}}}_{0} $都为双圈.

TEB算法求解的流程图如图5所示. 初始路径$ \left\{{{z}}_{j}\right\} $被初始化处理为离散的位姿序列和时间间隔序列构成的几何结构$ \boldsymbol{B}\left({Q},{\tau }\right) $. 在每次迭代过程中，以特定频率动态更新位姿的序列或移除已经处理过的位姿，且仅保留当前设置间隔内的位姿，确保了处理优化问题的速度，具有良好的实时性.

TEB 算法的整体流程如下：1）轨迹初始化：根据输入的初始路径点初始化TEB 控制元组，并对状态节点进行初始插入或删除. 2）超图构建：融合传感器获取的障碍物信息，构建包含状态节点与约束项的TEB超图.3）轨迹优化：对超图进行优化，得到满足动力学与安全约束的轨迹. 4）节点更新与检验：根据优化结果调整TEB状态节点，并对轨迹可行性进行检验. 5）控制输出与循环规划：基于运动学模型计算控制量并发送至控制器执行；同时随机器人运动和环境变化持续更新超图并重复优化，直至到达目标点.

传统TEB算法的运动规划优化框架主要针对速度与加速度边界运动学约束条件，但这类传统约束对机器人运动轨迹的调节能力有限. 本研究通过在优化框架中引入位置三阶导数（jerk）约束，将误差项形式的刚性约束转化为目标函数中的惩罚型软约束. 该改进方案能在保证数值稳定性的同时，显著提升运动控制的可控性.

jerk是对加速度的导数，约束jerk可以平滑速度和加速度曲线，使机器人运动更顺滑. 速度约束的实现需要2个位姿点以及1个对应的时间间隔，加速度的约束需要3个位姿点以及2个对应的时间间隔，加加速度jerk的约束则需要4个位姿点以及3个对应的时间间隔. 故设4个连续的位姿点为$ {\boldsymbol{s}}_{0} $、$ {\boldsymbol{s}}_{1} $、$ {\boldsymbol{s}}_{2} $、$ {\boldsymbol{s}}_{3} $，其对应的时间间隔为$ \Delta {T}_{0} $、$ \Delta {T}_{1} $和$ \Delta {T}_{2} $，由式（13）、（14）可知，其对应的线加速度值分别为$ {a}_{0} $和$ {a}_{1} $. jerk的值可以直接利用相邻的线加速度值$ a $与其对应的3个时间间隔$ \Delta T $通过三阶差分法求得：

(19)$ {j}_{v}=\frac{\left({a}_{1,v}-{a}_{0,v}\right)}{\alpha \Delta {T}_{0}+\beta \Delta {T}_{1}+\gamma \Delta {T}_{2}}， $

(20)$ {j}_{\omega }=\frac{\left({a}_{1,\omega }-{a}_{0,\omega }\right)}{\alpha \Delta {T}_{0}+\beta \Delta {T}_{1}+\lambda \Delta {T}_{2}} . $

为了获得精确结果，须确保$ \alpha +\beta +\gamma =\text{1.00} $，又因$ \Delta {T}_{1} $与$ {a}_{0} $和$ {a}_{1} $均相关，而$ \Delta {T}_{0} $和$ \Delta {T}_{2} $仅分别与$ {a}_{0} $、$ {a}_{1} $有关，因此，须赋予$ \Delta {T}_{1} $更大的权重以保障计算精确性. 本研究将权重值设为$ \alpha =\gamma =0.25 $，$ \beta =0.50 $.

线jerk和角jerk的约束误差函数为

(21)$ {f}_{{{j}_{v}}}={e}_{\Gamma }\left({j}_{{{v}_{i}}},{j}_{{{v}_{\max }}},\varepsilon ,S,n\right) , $

(22)$ {f}_{{{j}_{\omega }}}={e}_{\Gamma }\left({j}_{{{\omega }_{i}}},{j}_{{{\omega }_{\max }}},\varepsilon ,S,n\right) . $

式中：$ {j}_{{{v}_{\max }}} $为线jerk设定的最大值，$ {j}_{{{\omega }_{\max }}} $为角jerk设定的最大值.

添加jerk约束后的超图如图6所示.

在传统 TEB 算法中，轨迹节点基于固定时间分辨率均匀配置. 在路径平直且距离较长的区域，节点间的距离较大，导致时间间隔较大；而在障碍密集或路径转弯区域，为了确保机器人能够平稳、可靠地跟踪轨迹，须适当缩短时间间隔，从而提高节点密度.

在TEB算法中，$ \mathrm{autoResize} $函数的核心功能是结合路径特性与环境信息动态调整轨迹节点的数量. 该函数通过时间分辨率$ {\text{dt}}_{\text{ref}} $与时间迟滞$ {\text{dt}}_{\text{hysteresis}} $这2个参数对节点密度进行控制. 当相邻节点之间的时间差大于“时间分辨率与时间迟滞之和”，且节点数量未超过设定的最大值时，在两节点之间插入新的状态节点；当时间差小于“时间分辨率与时间迟滞之差”，且当前节点数量高于下限时，则执行节点删除操作. 若时间差分小于时间分辨率减去时间迟滞之差，且节点数高于下限，则删除节点. 插入操作将时间差均分，删除操作则合并时间差，从而动态维持合适的时间分布和节点数量.

尽管该方法可满足固定时间分辨率的要求，但在某些场景下表现仍有不足，固定分辨率可能导致路径出现抖动. 为此，本研究提出自适应改变时间分辨率的方法，以实现轨迹节点密度对环境变化的动态适应.

引入jerk约束后的TEB算法，为了实现对机器人的精准控制，须对路径曲率进行实时判断. 在路径转弯曲率较大处，相较于正常运行工况，须额外插入更多节点，从而更精确地调控机器人的速度与角速度，确保运动过程中加速度变化率满足约束条件，提升运动的平稳性与安全性，曲率计算表达式如下：

(23)$ k=\frac{\left| \left({v}_{0}\times {v}_{1}\right)\right| }{\left| {v}_{0}\right| \cdot \left| {v}_{1}\right| } . $

式中：$ {v}_{0} $和$ {v}_{1} $分别为3个位姿点之间的平均速度.

当针对动态障碍物时，TEB算法会在每次更新优化时获取移动障碍物的速度、方向和位置，进而线性预测障碍物后续位置. 此时TEB算法不断调整轨迹节点及时间分配，插入更多节点以精确快速地调整机器人运动方向和速度，在避免与障碍物相撞的前提下，实现安全、顺利、快速通过.

针对狭窄的走廊环境，通常须增加位姿点，通过更多控制使机器人沿全局路径点运动，并避免与障碍物发生碰撞. 但由于狭窄区域一般较长，且障碍物分布不规则，插入大量节点会导致路径在局部区域出现锯齿状或抖动，影响机器人的连续性运动，导致机器人无法顺利地到达下一个节点. 因此，须在狭窄区域内减少一定数量的节点.

综上，在曲率较大时，增大曲率因子$ \alpha $，曲率较小时则减小曲率因子$ \alpha $，使其趋向于1.0；当移动障碍物速度较大时，增大动态障碍物因子$ \beta $的大小，当障碍物速度较慢或没有移动障碍物时，动态障碍物因子$ \beta $趋向于1.0；当路径两旁障碍物较多时，减小狭窄走廊因子$ \gamma $，当障碍物较少时，使障碍物因子趋向于1.0，保持默认的$ {\text{dt}}_{\text{ref}} $. 最后，将曲率因子$ \alpha $、动态障碍物因子$ \beta $和狭窄走廊因子$ \gamma $相乘，得到综合调整因子，将其代入$ \text{autoResize} $函数以调整相邻位姿点间的时间间隔大小$ {\text{dt}}_{\text{ref}} $. 表达式如下：

(24)$ \left.\begin{split}&{\rm{adjusted\_dt\_ref}}=\dfrac{\rm{dt\_ref}}{\alpha \cdot \beta \cdot \gamma }；\\& \alpha ,\beta \in \left[{1.0,1.2}\right],\quad \gamma \in \left[{0.8,1.0}\right].\end{split}\right\}$

为了确定$ \alpha $、$ \beta $和$ \gamma $的取值，须计算机器人运动过程中的路径曲率$ k $、动态障碍物的移动速度$ {v}_{\text{ang}} $以及每个位姿到最近障碍物的距离$ {d}_{{{\mathrm{obs}},x}} $.

(25)$ d_{\mathrm{obs}, {x}}=\min _{o \in \text { obstacles }}\left\|x-\operatorname{closest}_o(x)\right\| . $

式中：$ x $代表位姿点，${o \in \text { obstacles }} $表示障碍物o属于障碍物集合，$\operatorname{closest}_o(x) $表示障碍物o到机器人位姿点x距离最近的点.

定义$ {k}_{\max } $为轨迹最大曲率、$ {k}_{1} $、$ {k}_{2} $为路径曲率阈值，通过判断$ {k}_{\max } $与阈值之间的关系来确定$ \alpha $的取值，$ \alpha $的取值规则如下：

(26)$ \alpha =\left\{\begin{array}{ll}1.0，&{k}_{\max }\leqslant {k}_{1}；\\1.1，&{k}_{1} \lt {k}_{\max }\leqslant {k}_{2}；\\1.2，&{k}_{\max } \gt {k}_{2}.\end{array}\right. $

式中：根据经验，$ {k}_{1} $、$ {k}_{2} $分别取0.05、0.10 m⁻¹.

同理可得，定义$ {v}_{{\mathrm{an{g}}},\,{{\max }}} $为最大动态障碍物速度，$ {v}_{1} $、$ {v}_{2} $分别为动态障碍物低速阈值和高速阈值，可以得到$ \beta $的取值规则:

(27)$ \beta =\left\{\begin{array}{ll} 1.0，&{v}_{{{\text{ang}},\,{\text{max}}}}\leqslant {v}_{1}；\\\text{1.1}，&{v}_{1} \lt {v}_{{{\text{ang}},\,{\text{max}}}}\leqslant {v}_{2}；\\\text{1.2}，&{v}_{{{\text{ang}},\,{\text{max}}}} \gt {v}_{2}.\end{array}\right. $

式中：根据经验，$ {v}_{1} $、$ {v}_{2} $分别取0.5、1.0 m/s.

定义$ {d}_{{{\text{obs}},\,{\text{min}}}} $为位姿到最近障碍物的最小距离，$ {d}_{1} $、$ {d}_{2} $分别为开阔距离阈值和狭窄距离阈值，$ \gamma $的取值规则如下：

(28)$ \gamma =\left\{\begin{array}{ll} 1.0，&{d}_{{{\text{obs}},\,{\text{min}}}}\geqslant {d}_{1}；\\0.9，&{d}_{2}\leqslant {d}_{{{\text{obs}},\,{\text{min}}}} \lt {d}_{1}；\\0.8，&{d}_{{{\text{obs}},\,{\text{min}}}} \lt {d}_{2}.\end{array}\right. $

式中：根据经验，$ {d}_{1} $、$ {d}_{2} $分别取值1.0 m、0.5 m.

为了验证本研究提出的改进TEB算法的有效性，分别对DWA算法、APF算法、传统的TEB算法和改进后的TEB算法进行仿真实验. 仿真平台基于Ubuntu20.04系统和系统下的Rviz与Stage软件，用于仿真的部分参数如表1所示. 其中,$ {v}_{\max } $为最大线速度，$ {\omega }_{\max } $为最大角速度，$ {a}_{\max } $为最大线加速度，$ {\alpha }_{\max } $为最大角加速度，$ {j}_{\max } $为最大jerk值，$ {v}_{\text{bw},\max } $为最大后退速度，$ {d}_{\min } $为避障最小距离，$ {r}_{\text{infl}} $为障碍膨胀半径.

为了验证改进算法的有效性，实验设计2类不同路径：路线1为远距离长狭窄走廊环境，路线2为多转弯包含反向停车环境. 路线1中不同算法的规划路径及转弯局部放大图如图7所示. 图7（a）为不同算法下机器人的实际行驶路径，图7（b）为转弯时的局部放大图. 可以看出，APF路径呈现出剧烈震荡与回拉现象，可见该算法易受虚拟势场极值影响，不适合对线速度、角速度平滑性要求高的场景；DWA、传统TEB和改进TEB算法表现较好，DWA算法跟随全局路径的效果较好，但也存在轻微抖动，传统TEB算法在拐角出路径存在外扩现象，说明其对高曲率区域的控制不足，改进后的TEB算法路径贴近理想最短路径，转弯过渡自然，曲率连续，在路径平滑性方面表现优异.

如图8、9所示为路线1中4种算法线速度和角速度对比图. 对于APF算法，当最大角速度设置为0.5 m²/s²时机器人无法通过狭窄走廊环境，故此处并未对最大角速度进行限制；其在线速度上波动较小，但在角速度上存在剧烈波动. 对于DWA算法，其线速度在路径上容易出现小波动，且角速度波动幅度大于传统TEB和改进TEB算法. 改进后的TEB算法相比于传统TEB算法，线速度曲线和角速度曲线的波动程度更小，验证了改进算法可有效优化线速度和角速度，减小波动并使变化更平滑.

路线1中4种算法的仿真数据如表2所示. 其中,$ \overline{v} $为平均线速度,$ \sigma _{v}^{2} $为线速度方差,$ \overline{\omega } $为平均角速度,$ \sigma _{\omega }^{2} $为角速度方差. APF算法在线速度上存在明显优势，但在角速度方面存在较大劣势，且生成的路径在该环境下剧烈震荡，不具备适用性；DWA算法的线速度方差和角速度方差均大于改进后的TEB算法；改进后的TEB算法相较于传统TEB算法，平均线速度升了4.37%、平均角速度、线速度方差和角速度方差分别减小了16.67%、7.38%、12.84%，规划路径更契合机器人实际较优运动轨迹.

路线2中不同算法的规划路径及转弯局部放大图如图10所示. 图10（a）为不同算法下机器人实际行驶路径，图10（b）为转弯局部放大图. 由转弯局部放大图可知，APF规划路径存在大弯绕行现象，导致路径长度变长，反映出人工势场法在障碍物边界附近易受斥力场非线性叠加的影响，易引发局部极值问题；DWA算法路径较短，但拐角处平滑性欠佳；传统TEB算法与改进TEB算法的曲率变化更加自然.

如图11、12所示为路线2中4种算法线速度和角速度对比图. 从路径2的运动特性分析可知，该路径包含较多转弯区域，导致线速度出现显著下降. 转弯密集区域伴随角速度的频繁跳变调整，反映出路径曲率变化对机器人运动控制参数的显著影响. 从图中可以看出，转弯时所有算法的线速度均出现不同程度的波动，而改进后TEB算法的波动最小，其角速度波动也最小.

路线2中4种算法的仿真数据如表3所示. 可以看出，改进TEB算法在线速度方差、平均角速度和角速度方差上相较于APF算法值更低；改进TEB算法相较于DWA算法具有更大的平均线速度，同时线速度方差、平均角速度以及角速度方差更小；改进TEB算法的线速度方差、平均角速度和角速度方差分别比传统TEB算法下降了8.61%、4.34%和8.58%，使得速度与角速度更加平滑，波动减少，进一步验证了改进TEB算法的有效性.

为了验证改进TEB算法的有效性，采用搭载Jetson Nano主控板、STM32底层驱动板和镭神M10P激光雷达的四轮两驱差速移动机器人平台，小车主机使用ubuntu18.04版本，在如图13所示的简单结构化环境与复杂动态环境中进行系统性实验验证.

如图14、15所示分别为简单、复杂环境下的TEB算法和改进TEB算法对比. 实验结果表明，在简单实验环境下，小车遵循改进TEB算法生成的路径行驶时，转弯时的线速度更平稳，没有出现较大波动并且导航时间更短，角速度曲线更加平滑没有较大突刺，运动性能更好；在复杂环境下，采用TEB算法规划路径的小车与纸箱有轻微碰撞，线速度和角速度出现较大波动，而遵循改进TEB算法路径行驶的小车则安全平滑地运行至目标点. 上述结果充分验证了改进TEB算法的有效性.

本研究介绍了传统TEB算法的基本原理、数学模型构建、轨迹约束条件以及TEB模型求解方法. 针对复杂环境下加速度变化率突变导致机器人速度曲线异常、运动过程震荡抖动问题，提出基于TEB算法的改进策略，引入加速度的变化率jerk约束和自适应调节弹性带节点密度的方法. 在TEB算法的基础上增加jerk约束，其中，jerk 约束的融入有效平滑了路径全程的速度变化，保障运动连贯性；自适应调整弹性带节点密度方法，则依据环境中障碍物分布情况，动态调整时间间隔，灵活改变节点密度，增强了算法对复杂环境的适应性，显著提升了机器人运动控制的精准度与稳定性. 仿真实验与真实实验验证了改进算法的有效性. 时间分辨率自适应调整策略主要依据环境复杂度进行调节，其参数设置仍具有一定的经验性. 在高度动态或不确定环境中，节点调整的鲁棒性仍有改进余地. 未来计划考虑引入学习方法或自适应参数调节机制，实现更加智能的节点密度调整策略.