Improvement of the TEB Algorithm for Local Path Planning of Car-like Mobile Robots Based on Fuzzy Logic Control

1. Introduction

Mobile robots are a type of robots capable of perceiving their surroundings and utilizing sensor information to autonomously or semi-autonomously navigate the environment and perform specific tasks, with autonomous motion planning being a key capability for task execution. The motion planning algorithms for mobile robots aim to find a path that satisfies geometric, kinematic, dynamic, and input constraints [1], encompassing path planning and path tracking. Path planning can be further divided into global path planning and local path planning, also referred to as path searching and trajectory optimization [2]. Global path planning typically searches for geometrically feasible routes, which are not directly suitable for the robot as robots cannot make sharp turns abruptly and need to decelerate [3]. Local path planning generates a trajectory with velocity and directional information, taking into account the control inputs along the trajectory to ensure smooth motion.

Local path planning for mobile robots is typically categorized into three types in the field of mobile robotics: artificial intelligence algorithms, bio-inspired algorithms, and classical algorithms. Classical algorithms can be further divided into sampling-based methods, curve interpolation-based methods, and optimization-based methods [4].

Artificial Intelligence (AI) algorithms use neural networks and reinforcement learning for path planning, such as Q-learning [5]. Currently, AI algorithms are primarily employed in the preprocessing stage of planning to provide prior knowledge, and are less frequently used as standalone path planners.

Bio-inspired algorithms refer to Intelligent Optimization Algorithms such as the Ant Colony Algorithm (ACO) [6] and Firefly Algorithm (FA) [7]. However, their imbalance between global and local search capabilities leads to significant variations in performance across different environments [8].

Classical path planning algorithms are based on explicit mathematical models and logical rules. Sampling-based methods randomly or semi-randomly sample in the solution space to find paths that satisfy specific conditions. The dynamic window approach (DWA) samples in the robot’s velocity space to generate optimal trajectories [9], offering fast computation and ease of solving. However, it exhibits unstable planning efficiency across different maps and is unsuitable for car-like robots. Rapidly-exploring Random Tree Star (RRT*) relies on extensive sampling expansions. While it has strong problem-solving capabilities, it demands high computational effort, converges slowly, and generates paths that are not sufficiently smooth [10].

Curve interpolation-based methods use mathematical functions to generate smooth paths, such as B-spline Curves [11] and high-order Bezier Curves [12]. However, these methods suffer from poor real-time performance and are more suitable for robots with simple constraints, showing suboptimal performance in car-like robots.

Optimization-based methods solve paths by defining a cost function to minimize the cost. The cost function typically considers multiple factors such as path length, smoothness, and safety. The Artificial Potential Field (APF) method is simple and efficient in computation but prone to path oscillation and local optima issues [13].

Achieving path planning with real-time performance, smoothness, and obstacle avoidance remains challenging for car-like mobile robots under non-holonomic constraints. Traditional methods based on sampling, curve interpolation, and optimization each have their advantages and disadvantages but fail to balance path smoothness and computational efficiency in complex environments. The Timed-elastic-band (TEB) method proposed by Rosmann et al. [14] transforms the path planning problem into a real-time multi-objective optimization problem, effectively integrating kinematic constraints and trajectory planning requirements. It efficiently generates optimal trajectories that conform to the motion characteristics of car-like robots, balancing trajectory smoothness, real-time performance, and obstacle avoidance. Compared to traditional Dubins curves and Reeds-Shepp curves, TEB not only exhibits comparable or equivalent results in trajectory optimization but also achieves superior performance in the time dimension [15].

Sun et al. improved TEB for a specific kinematic model of a collaborative robot and further developed a motion control function to ensure stability and safety during operation [16]. Hoang et al. incorporated human socio-temporal characteristics into the TEB algorithm to enable robots to plan trajectories more aligned with human intuition when avoiding people [17]. Wu et al. incorporated the heuristic function of the A-star algorithm into TEB's objective function to generate trajectories along walls and near the edges of curved paths, reducing planning time [18]. Zha et al. removed objective terms related to velocity and time information from TEB and added constraints for smoothness and curvature, subsequently applying fifth-order spline curves to smooth the trajectory, thereby improving trajectory smoothness [19]. Wang et al. introduced a two-step TEB algorithm to filter and optimize rough global paths generated by MH-RRT, reducing the number of homotopy class paths requiring optimization and thereby lowering TEB's computational burden [20]. Smith et al. addressed the decoupling between TEB's use of factor graphs as an optimization data structure in the G2O framework and the environmental data structures (e.g., costmap) in the navigation stack by proposing an Egocircle method for ego-centered obstacle representation and gap-based topological trajectory search to simplify TEB computations and enhance optimization efficiency [21]. Liu et al. utilized the Random Sample Consensus (RANSAC) algorithm to simplify the contours of complex obstacles, reducing computational load and enabling more responsive and faster optimization [22]. Nguyen et al. added potential collision information generated by the Hybrid Reciprocal Velocity Obstacle (HRVO) model as an objective term to TEB's objective function, enabling mobile robots to proactively predict the behavior of dynamic obstacles and enhancing TEB's optimization speed in dynamic scenarios [23].

Despite its excellent tunability and optimization capabilities, the TEB method, widely applied in local path planning, still has certain limitations in practical applications:

• Insufficient adaptability to different scenarios. The algorithm lacks the ability to self-adjust across varying environments, leading to suboptimal performance in dynamic or complex scenarios.
• Conflicts in multi-objective optimization. Potential conflicts among multiple optimization objectives may cause the overall optimization result to deviate from the optimal solution, or even break certain soft constraints.
• Lack of smooth control inputs. Frequent oscillations in orientation angles, velocity, and acceleration along the trajectory impact the robot's motion stability.

To address these issues, this paper proposes an improved TEB method based on fuzzy logic control (FLC-TEB, Fuzzy Logic Controller TEB), introducing new objective terms for trajectory smoothness and jerkiness in the TEB to reduce oscillations and dynamically adjusting the weights of objective terms based on environmental characteristics to enhance trajectory quality and algorithm robustness. The main contributions of this paper include:

• Introducing new objective terms for trajectory smoothness and jerkiness into the classic TEB objective function. These improvements aim to enhance the overall smoothness of the trajectory, reduce drastic changes in velocity control, and thereby improve the robot's motion stability.
• Designing fuzzy logic control rules to dynamically adjust the weights of objective terms in the TEB objective function through a fuzzy controller. This enables the improved TEB algorithm to optimize the priorities of different objectives in real time according to environmental changes, thereby significantly improving the quality of the overall trajectory.

The content of this paper is organized as follows: Section 2 briefly introduces the classic Timed-Elastic-Band (TEB) method, analyzes the kinematic model of car-like mobile robots, and explains how the TEB algorithm incorporates kinematic constraints into the objective function. Section 3 focuses on the improvements of adding trajectory smoothness and jerkiness objective terms into the classic TEB, along with the design of a fuzzy logic controller to dynamically adjust the weights of the objective terms to adapt to environmental changes. Section 4 validates the effectiveness of the proposed FLC-TEB algorithm in local path planning for car-like robots through simulations and real-world experiments. Finally, Section 5 summarizes the main conclusions of this paper and outlines future research directions.

2. TEB for Car-Like Robots

The classic Timed Elastic Band (TEB) method incorporates the kinematic constraints of car-like robots into the objective function for optimization, enhancing the algorithm's applicability to car-like robots.

2.1. Classic Timed Elastic Band

The Timed-Elastic-Band (TEB) method treats pose points as variables to be optimized while adding objective terms as soft constraints into the optimizer [24]. Its overall goal is to obtain a trajectory for a car-like mobile robot that moves from the starting point $S_s$ to the endpoint $S_f$ in the shortest time. The pose points consist of a series of discrete sequences ${\left(S_k\right)}_{k=1,2,\dots ,n}$along the path. TEB further introduces time intervals ${\left(\Delta T_k\right)}_{k=1,2,\dots ,n-1}\hspace{0.33em}\in \hspace{0.33em}{ℝ}^{+}$, where each $\Delta T_k$ represents the time interval for the robot to move from $S_k$ to $S_{k+1}$. At this point, the optimization variables are expressed as:

$$ℬ\hspace{0.33em}:=\hspace{0.33em}\left\{S_1,\hspace{0.33em}\Delta T_1,\hspace{0.33em}{S}_2,\hspace{0.33em}\Delta T_2,\hspace{0.33em}\dots \hspace{0.33em}{S}_{n-1},\hspace{0.33em}\Delta T_{n-1},\hspace{0.33em}{S}_n\right\}$$

(1)

At this point, the TEB problem is formulated as:

$$\underset{ℬ}{{\displaystyle \min }}\hspace{0.33em}\hspace{0.33em}{\displaystyle \sum _{k=1}^{n-1}\Delta T_{\hspace{0.17em}\hspace{0.17em}k}^2}$$

(2)

Considering the actual motion trajectory of the robot, additional objective conditions are incorporated when solving Equation (2), such as velocity $v_k$ and acceleration $a_k$ being constrained to the intervals $\left(0,\hspace{0.33em}{v}_{\max }\right)$ and $\left(0,\hspace{0.33em}{a}_{\max }\right)$, the distance to obstacles$o_k\left(S_k\right)\hspace{0.33em}\ge \hspace{0.33em}0$, non-holonomic constraints $h_k\left(s_{k+1},\hspace{0.33em}{s}_k\right)\hspace{0.33em}=\hspace{0.33em}0$ for car-like robots, and minimum turning radius ${\rho }_{\min }$ which is constrained such that ${\rho }_k\hspace{0.33em}\ge \hspace{0.33em}{\rho }_{\min }$, and ${\rho }_k$ represents the radius corresponding to each arc segment in the trajectory. To improve the solving speed, the constraints are incorporated into the total objective term in the form of penalty functions, transforming the original exact nonlinear programming (NLP) problem into an approximate nonlinear least-squares optimization problem, expressed as:

$${ℬ}^{*}\hspace{0.33em}=\hspace{0.33em}\arg \underset{ℬ}{{\displaystyle \min }}\hspace{0.33em} \mathrm{V} ˜\left(ℬ\right)$$

(3)

$$\mathrm{V} ˜\left(ℬ\right)\hspace{0.33em}=\hspace{0.33em}{\displaystyle \sum _{k=1}^{n-1}f\left(ℬ\right)}$$

(4)

The $f\left(ℬ\right)$ represents various objective terms, including the robot's velocity $f_v$, acceleration $f_a$, distance to obstacles $f_{obs}$, shortest path length $f_{path}$, optimal trajectory time $f_t$, and kinematic constraints $f_{nh}$, among others. Velocity and acceleration are treated as constraints, bounded through penalty functions, while path length and trajectory time are treated as optimization objectives to achieve the optimal solution.

As shown in Figure 1, TEB employs a hypergraph to depict the process of solving a nonlinear optimization problem. In the figure, $S_k$ represents poses along the trajectory, $P_k$ represents waypoints, and $O_k$ represents associated obstacles. TEB employs different objective functions as hyperedges in the hypergraph, treating all connected poses $S_k$ and their time intervals $\Delta T_k$ as nodes in the hypergraph for optimization. The trajectory with the minimum cost under the total objective function is selected as the optimal trajectory. The TEB Optimizer section in Figure 2 illustrates the process by which TEB uses a hypergraph to optimize the trajectory.

2.2. Kinematic Model of a Car-Like Robot

At low speeds, the local path planning of car-like mobile robots is based on their kinematics without considering dynamics. The car-like robot is modeled as a rigid body with four wheels in contact with the ground, with its direction determined by the front wheels [25], as shown in Figure 3:

Figure 3 illustrates the kinematic model of a car-like mobile robot in the world coordinate system $\left(x_w,\hspace{0.17em}{y}_w\right)$. The continuous poses of the car-like mobile robot during motion are represented as $S\hspace{0.33em}=\hspace{0.33em}{\left[x,\hspace{0.17em}y,\hspace{0.17em}\beta \right]}^T\hspace{0.33em}\in \hspace{0.33em}{ℝ}^2\hspace{0.33em}\times \hspace{0.33em}{S}^1$. The robot's kinematic center is the point S, located at the midpoint of the rear wheel axis. The robot's principal axis is aligned with the x-axis.

When turning, the car-like mobile robot has a rotational motion radius around the center of rotation (ICR), labeled as $\rho$ in the diagram; The equivalent front wheel steering angle $f\hspace{0.33em}\in \hspace{0.33em}\left(-f_{\max },\hspace{0.33em}{f}_{\max }\right)$, lies within the range$\left(0,\hspace{0.33em}\frac{\pi }{2}\right)$. When $f\hspace{0.33em}=\hspace{0.33em}{f}_{\max }$, the robot's turning radius is equal to the minimum turning radius, $\rho \hspace{0.33em}=\hspace{0.33em}{\rho }_{\min }$. The robot's signed translational velocity along its principal axis is denoted by $v\hspace{0.33em}\in \hspace{0.33em}ℝ$. The ordinary differential motion equations of the robot in the world coordinate system are as follows:

$$\dot{S}\left(t\right)\hspace{0.33em}=\hspace{0.33em}\left[\begin{array}{c}\dot{x}\left(t\right)\\ \dot{y}\left(t\right)\\ \dot{\beta }\left(t\right)\end{array}\right]\hspace{0.33em}=\hspace{0.33em}\left[\begin{array}{c}v\left(t\right)\cos \left(\beta \left(t\right)\right)\\ v\left(t\right)\sin \left(\beta \left(t\right)\right)\\ {\displaystyle \frac{v\left(t\right)}{L}}\tan \left(f\left(t\right)\right)\end{array}\right]$$

(5)

Non-holonomic constraints require that the robot's consecutive poses $S_k$ and $S_{k+1}$ lie on a common arc with the same curvature during motion [15]. As shown in Figure 4, the direction vector of the robot at pose $S_k$ is $d_k\hspace{0.33em}=\hspace{0.33em}\left[x_{k+1}-x_k,\hspace{0.33em}{y}_{k+1}-y_k,\hspace{0.33em}0\right]$, which forms an angle $J_{k,\hspace{0.33em}k}$ with the velocity $x_k$ at $S_k$, and an angle $J_{k,\hspace{0.33em}k+1}$ with the velocity $x_{k+1}$ at $S_{k+1}$. The non-holonomic constraint is expressed using the following equations:

$$J_{k,\hspace{0.33em}k}\hspace{0.33em}=\hspace{0.33em}{J}_{k,\hspace{0.33em}k+1}$$

(6)

$$h_k\left(s_{k+1},\hspace{0.33em}{s}_k\right)\hspace{0.33em}=\hspace{0.33em}\left(\left[\begin{array}{c}\cos \left({\beta }_k\right)\\ \sin \left({\beta }_k\right)\\ 0\end{array}\right]+\left[\begin{array}{c}\cos \left({\beta }_{k+1}\right)\\ \sin \left({\beta }_{k+1}\right)\\ 0\end{array}\right]\right)\hspace{0.33em}\times \hspace{0.33em}{d}_k\hspace{0.33em}=\hspace{0.33em}0$$

(7)

The front wheel steering angle of a car-like mobile robot has a maximum limit, beyond which the radius of the turn is minimized, referred to as the minimum turning radius. In Figure 4, the relationship between two adjacent poses can be described as:

$${\rho }_k\hspace{0.33em}=\hspace{0.33em}{\displaystyle \frac{{‖d_k‖}_2}{\left|2\sin \left(\frac{\Delta {\beta }_k}{2}\right)\right|}}\hspace{0.33em}\stackrel{\Delta {\beta }_k\ll 1}{\approx }\hspace{0.33em}{\displaystyle \frac{{‖d_k‖}_2}{\left|\Delta {\beta }_k\right|}}$$

(8)

The turning radius ${\rho }_k$ of a car-like mobile robot must satisfy ${\rho }_k\hspace{0.33em}\ge \hspace{0.33em}{\rho }_{\min }$.

TEB incorporates the non-holonomic constraint $f_{nh}\hspace{0.33em}=\hspace{0.33em}{‖h_k‖}_2^2$ and the minimum turning radius constraint $f_r\hspace{0.33em}=\hspace{0.33em}{‖\min \left\{0,\hspace{0.33em}\left({\rho }_{\min }\hspace{0.33em}-\hspace{0.33em}{\rho }_k\right)\right\}‖}_2^2$ from the kinematic model of car-like robots into the total objective function in the form of weighted penalty functions, as shown in Equation:

$$\mathrm{V} ˜\left(ℬ\right)\hspace{0.33em}=\hspace{0.33em}{\displaystyle \sum _{k=1}^{n-1}f\left(ℬ\right)\hspace{0.33em}}=\hspace{0.33em}{\displaystyle \sum _{k=1}^{n-1}\left(f_v\hspace{0.33em}+\hspace{0.33em}{f}_a\hspace{0.33em}+\hspace{0.33em}{f}_{obs}\hspace{0.33em}+\hspace{0.33em}\cdots \hspace{0.33em}+\hspace{0.33em}{f}_{nh}\hspace{0.33em}+\hspace{0.33em}{f}_r\right)\hspace{0.33em}}$$

(9)

3. Improved TEB Algorithm with Fuzzy Logic Controller

During local path optimization, the weights of the objective terms in the TEB algorithm significantly influence the actual optimization results. According to the quadratic penalty theory, the optimal solution ${ℬ}^{*}$ for the original nonlinear problem can only be achieved when all the objective term weights approach infinity($\sigma \hspace{0.33em}\to \hspace{0.33em}\infty$) [15]. Since excessively large weights can cause the solver to struggle with convergence, TEB employs a weight coefficient of 1 as the standard weight for each objective term. However, in different scenarios, the robot must consider varying priority objectives for the trajectory to reduce energy consumption and improve mobility efficiency.

Therefore, new objective for trajectory smoothness and jerkiness are added to the objective function to achieve smoother trajectories and more stable velocity changes, and a fuzzy logic control method is introduced to enhance TEB's adaptability. The flowchart for dynamically adjusting the weights of TEB objective terms using a fuzzy controller is shown in Figure 5:

3.1. TEB Objective Function Considering Robot Motion Smoothness

To address issues such as oscillations in velocity, angular velocity, and steering angles in TEB-based local path planning, smoothness and jerk [26,27] are incorporated into the TEB objective terms to reduce the robot's motion instability.

Trajectory smoothness is represented by the angular changes between adjacent pose points, as shown in Equation (10):

$$f_{sm}\hspace{0.33em}=\hspace{0.33em}{m}_k\left(S_{k+1},\hspace{0.33em}{S}_k\right)\hspace{0.33em}=\hspace{0.33em}{\displaystyle \sum _{i=1}^{n-1}{\left(\Delta {\beta }_{i+1}\hspace{0.33em}-\hspace{0.33em}\Delta {\beta }_i\right)}^2}$$

(10)

The Jerk is the derivative of acceleration and is calculated using the differences between four consecutive poses, as shown in Equation (11):

$$f_{jerk}\hspace{0.33em}=\hspace{0.33em}{j}_k\left(S_{k+3},\hspace{0.33em}{S}_{k+2},\hspace{0.33em}{S}_{k+1},\hspace{0.33em}{S}_k,\hspace{0.33em}\Delta T_{k+2},\hspace{0.33em}\Delta T_{k+1},\hspace{0.33em}\Delta T_k\right)\hspace{0.33em}=\hspace{0.33em}{\displaystyle \sum _{i=1}^{n-3}\frac{a_{i+1}\hspace{0.33em}-\hspace{0.33em}{a}_i}{\frac{1}{4}\Delta T_i\hspace{0.33em}+\hspace{0.33em}\frac{1}{2}\Delta T_{i+1}\hspace{0.33em}+\frac{1}{4}\Delta T_{i+2}}}$$

(11)

Therefore, penalty terms for trajectory smoothness and jerk, along with their corresponding weights, are added to Equation (9). These penalty terms are incorporated as hyperedges into the optimization framework of TEB, forming a new objective function:

$$\mathrm{V} ˜\left(ℬ\right)\hspace{0.33em}=\hspace{0.33em}{\displaystyle \sum _{k=1}^{n-1}{f}^{\prime }\left(ℬ\right)\hspace{0.33em}}=\hspace{0.33em}{\displaystyle \sum _{k=1}^{n-1}\left(f\left(ℬ\right)\hspace{0.33em}+\hspace{0.33em}{f}_{sm}\hspace{0.33em}+\hspace{0.33em}{f}_{jerk}\right)\hspace{0.33em}}$$

(12)

3.2. Establishment of Fuzzy Rule Controller

The key to local path planning for car-like robots lies in effectively incorporating the constraints introduced by the kinematic model into the planning process. Non-holonomic constraints require replacing lateral translational motion with curved motion, while the minimum turning radius constraint further reduces the solution space for the path.

To improve the local path planning performance of car-like robots, this paper proposes using "freedom" and "directionality" to evaluate the trajectory characteristics of car-like mobile robots. Under non-holonomic and minimum turning radius constraints, the "freedom" of car-like mobile robot motion refers to the flexibility of pose adjustment, while "directionality" refers to the complexity of curved motion in the trajectory caused by non-holonomic constraints, replacing the need for lateral translation. To evaluate freedom, this paper uses the ratio of the average distance between the trajectory and obstacles to the minimum turning radius to measure the Narrowness ($N_t$) of the passage, which serves as an intuitive indicator of trajectory freedom; To evaluate directionality, this paper uses the proportion of different curvature arcs in the entire trajectory to quantify the turning complexity ($T_c$) of curved motion, which reflects the trajectory's directionality.

3.2.1. Freedom Evaluation

Due to the minimum turning radius constraint, car-like mobile robots have an inaccessible circular area on the side, and the planned path must circumvent this area. Within this area, the distance from any point to the kinematic center is $d$, the point furthest from the robot's kinematic center is $M$, with a distance of $2{\rho }_{\min }$ from the kinematic center. As shown in Figure 6, when there are obstacles outside point $M$, the robot cannot directly turn around to move toward a target point behind it. In this situation, the robot can only adjust its initial pose and move forward in the other direction before turning to indirectly approach the target point, or adopt a reversing strategy. However, to avoid instability, reversing behavior is usually subject to strict constraints [28]. Therefore, when the distance on both sides of the robot in the passage is less than $d\hspace{0.33em}<\hspace{0.33em}2{\rho }_{\min }$, the solution space for navigable paths is significantly reduced.

When the orientation of the target point's pose is opposite to the current pose, car-like mobile robots usually use curved motion to adjust the heading angle. As shown in Figure 7, when the space around the robot is too narrow to plan a single curved trajectory to adjust the heading angle, the robot needs to use a combination of forward and backward movements to achieve heading angle adjustment. When the distance between the obstacle and the robot, $d_{obs}$, is less than ${\displaystyle \frac{L}{2}}\hspace{0.33em}+\hspace{0.33em}{\rho }_{\min }$, the robot cannot adjust its heading angle by 180° using a single forward and backward combination movement, significantly increasing the complexity of pose adjustment. This limitation further reduces the solution space for path planning, constraining effective motion options.

Therefore, the adjustability of car-like mobile robot poses is evaluated using the narrowness ($N_t$) of the passage where the trajectory lies. $N_t$ is determined by the relationship between the average distance, $d_{obs}$, between all robot poses along the trajectory and the nearest obstacle, and the minimum turning radius ${\rho }_{\min }$. Assuming the semi-axle length of the car-like mobile robot satisfies ${\displaystyle \frac{L}{2}}\hspace{0.33em}<\hspace{0.33em}{\rho }_{\min }$, and there are n trajectory points ${\left(S_k\right)}_{k=1,2,\dots ,n}$ in the trajectory, with the average distance to the nearest obstacle ${\overline{d}}_{obs}\hspace{0.33em}=\hspace{0.33em}{\displaystyle \frac{1}{n}}{\displaystyle \sum _{i=1}^n{\left(d_{obs}\right)}_i}$, $N_t$ is calculated as:

$$N_t\hspace{0.33em}=\hspace{0.33em}{\displaystyle \frac{{\overline{d}}_{obs}}{{\rho }_{\min }}}$$

(53)

3.2.2. Directionality Evaluation

Due to non-holonomic kinematic constraints, the motion trajectory of car-like mobile robots requires curved motion instead of lateral translation, and an increase in curved motion raises the complexity of the trajectory. Additionally, the greater the curvature of the curved trajectory, the higher the demands on the robot's motion control inputs. Therefore, the complexity of curved motion for car-like mobile robots is evaluated using the Turning Complexity ($T_c$). $T_c$ is derived from the proportion of trajectory segments with different curvatures relative to the total.

For each local trajectory generated by the local planner, the curvature change${\kappa }_i\hspace{0.33em}=\hspace{0.33em}{\displaystyle \frac{\Delta {\theta }_i}{\Delta s_i}}$ between each pair of consecutive poses is first calculated, where $\Delta {\theta }_i$ is the heading angle change between adjacent poses $S_i$ and $S_{i+1}$, and $\Delta s_i$ is the length of the corresponding trajectory segment. If the curvature ${\kappa }_i$ exceeds the turning threshold ${\kappa }_{turn}$, the trajectory between $S_i$ and $S_{i+1}$ is labeled as a turning segment; otherwise, it is labeled as a non-turning segment.

Next, turning segments are identified as sharp turning segments. If the curvature${\kappa }_i$ of a turning segment exceeds the sharp turning threshold ${\kappa }_{sharp}$ but is less than${\kappa }_{\max }$, the segment is classified as a sharp turning segment, where ${\kappa }_{\max }$ is the curvature corresponding to the minimum turning radius ${\rho }_{\min }$ of the car-like robot, given by ${\kappa }_{\max }\hspace{0.33em}=\hspace{0.33em}{\displaystyle \frac{1}{{\rho }_{\min }}}$. In this paper, ${\kappa }_{turn}\hspace{0.33em}=\hspace{0.33em}0.2\hspace{0.33em}\times \hspace{0.33em}{\kappa }_{\max }$ and ${\kappa }_{sharp}\hspace{0.33em}=\hspace{0.33em}0.8\hspace{0.33em}\times \hspace{0.33em}{\kappa }_{\max }$. Trajectory segments identified as sharp turning segments are not counted again as turning segments. If the trajectory contains n trajectory points ${\left(S_k\right)}_{k=1,2,\dots ,n}$, it consists of n-1 segments. Let the number of turning segments be $n_{turn}$ and the number of sharp turning segments be $n_{sharp}$. The turning complexity $T_c$ is defined as:

$$T_c\hspace{0.33em}=\hspace{0.33em}{\displaystyle \frac{n_{turn}\hspace{0.33em}+\hspace{0.33em}2n_{sharp}}{n-1}}$$

(64)

3.2.3. Establishment of Fuzzy Rules

Fuzzy logic control is a rule-based control method. Compared to other control methods, it can simulate human reasoning through linguistic control without requiring a precise mathematical model, offering greater fault tolerance, robustness, and simplicity [29]. This paper considers the freedom and directionality of robot motion to set corresponding fuzzy logic control rules and establish a fuzzy controller for dynamically adjusting parameters.

The domain of narrowness is [0, 4], with a fuzzy set of [S, M, L]. The membership function of narrowness ($N_t$) is shown in Figure 9(a). The domain of turning complexity is [0, 2], with a fuzzy set of [S, M, L]. The membership function of turning complexity ($T_c$) is shown in Figure 9(b).); (b) Membership function graph for the complexity of turns ($T_c$).

Based on the above fuzzy logic control rules, using fuzzy logic control to build TEB graph edges is as follows:

3.3. Nonlinear Mapping Strategy for Output Weight Values

The increase in penalty function weight coefficients has a nonlinear effect on optimization results, typically following a pattern close to logarithmic growth. Therefore, to simplify computation and improve the performance of the fuzzy controller, the inputs to the fuzzy controller are represented as a linear set, and a piecewise function is used to map the inputs to weight values. As shown in Figure 10, the output range of the fuzzy controller, denoted as ${\sigma }^{\prime }$, is [-1,3], and the real weight values, denoted as $\sigma$, obtained through piecewise function mapping, fall within the range [0, 1000].

The membership function for the output weights of the fuzzy controller is shown in Figure 11. The weight ranges for objectives related to obstacle distance, smoothness, velocity, acceleration, and jerk are mapped using piecewise functions. Specifically, the fuzzy output range for obstacle distance is [-1, 3], corresponding to a weight range of $\sigma \in \left[0,\hspace{0.33em}1000\right]$, with a fuzzy set of [VL,L,M,H,VH]. Since the weights for optimal time and shortest path length should not be too large, these objectives are not linearized, and their corresponding weight range is set to $\sigma \in \left[0,\hspace{0.33em}5\right]$, with a fuzzy set of [VL, L, H, VH].

4. Simulation and Experimental Results

To validate the performance of the proposed FLC-TEB algorithm, modifications and adjustments were made to the TEB algorithm using the C++ programming language in the Ubuntu 18.04 ROS melodic environment, and comparisons were conducted with the original algorithm. First, simulation tests were conducted. The StageRos simulation environment was built to test the algorithm's ability to adaptively adjust weights in different environments, comparing the motion performance improvements brought by the new method. Next, the proposed algorithm was tested on a real car-like robot platform to evaluate its adaptive capabilities and practical improvements in robot performance. The experimental results presented in this paper are reproducible under the same robot structure and experimental parameters. In this paper, the speed smoothness metric is defined as the variance of the speed distribution along the entire trajectory. Lower variance indicates more stable speed and smoother trajectories, while higher variance suggests greater speed fluctuations and less smooth paths.

4.1. Simulation Experiment

The workstation running the program was equipped with an AMD Ryzen 5 5600H processor with Radeon Graphics, featuring 6 cores and a base frequency of approximately 3.3 GHz, and was equipped with 8 GB of RAM. The simulation employed a quadrilateral robot model to simulate a car-like robot. The robot body dimensions were 0.6m in length and 0.25m in width, with a front-to-rear axle distance of 0.4m, and a minimum turning radius of 0.5m. The kinematic parameters are provided in Table 2.

Table 3 shows the default weights for the classic TEB algorithm, the TEB algorithm optimized with smoothness and jerkiness (referred to as "Smooth-TEB"), and the weights computed for FLC-TEB in the narrow continuous turning section of Simulation Map:

Figure 12 shows Simulation Map used in this simulation. Simulation Map is a channel with continuous turns, where the width of the channel is 1.0m throughout. The car-like robot starts from the channel entrance, with the goal located at the position near the exit of the channel. The trajectory planned by the planner in a single execution on this map (red curve in the figure) is analyzed below. The kinematic characteristic curves obtained by the traditional TEB, Smooth-TEB, and FLC-TEB algorithms are shown in Figure 13.

As shown in Figure 13, in the channel environment of Simulation Map, all three Timed-Elastic-Band (TEB) algorithms generated feasible and consistent trajectories. From the perspective of trajectory length and time consumption, the TEB algorithm produced the longest path but the shortest execution time. The Smooth-TEB algorithm shortened the path by 6.25% compared to TEB, but the execution time increased by 20%. The FLC-TEB algorithm generated a path almost identical in length to Smooth-TEB but with a further increase of 23% in execution time. The comparison of data for each algorithm is presented in Table 4.

The data in Table 4 shows that, compared to TEB, FLC-TEB shortened the trajectory length by 8%, but increased the trajectory execution time by 49%. This is because the channel width in Simulation Map is very narrow, with two consecutive sharp turns near the entrance, causing FLC-TEB to increase the weight of the speed constraint, resulting in lower speeds. However, FLC-TEB reduced the change in turning angles by 50% compared to TEB and by 24% compared to Smooth-TEB. The smoothness of linear and angular velocities improved by 56%. FLC-TEB increased the weights for obstacle distance, speed, and acceleration while reducing the weights for trajectory length and execution time after considering the characteristics of the area. Notably, despite the reduced weight for trajectory length, FLC-TEB shortened the trajectory length due to better speed smoothing, indicating that FLC-TEB's weight adjustments improved the overall quality of the trajectory. Next, we analyze the motion performance of the robot moving along the complete trajectory to the destination. The kinematic characteristic curves obtained from real-time planning and full motion using traditional TEB, Smooth-TEB, and FLC-TEB are shown in Figure 14.

Table 5 shows the real-time feedback data during the complete motion process on this map. The TEB algorithm generated a longer path but had the shortest execution time. Smooth-TEB produced a path length almost identical to TEB but increased execution time by 4%. FLC-TEB generated a slightly shorter path than the other two but increased execution time by 6% compared to TEB. The results in Figure 14(a,b) shows that compared to the TEB algorithm, FLC-TEB reduced the linear velocity smoothness of the robot by 50% but increased the angular velocity smoothness by 55%. The trajectory and angular velocity smoothness optimized by the FLC-TEB planner were superior to those of TEB, enhancing motion stability and safety. This allowed the robot to reach the target point with minimal pose corrections, improving its motion efficiency.

4.2. Experiment in Real Environment

Simulation experiments cannot fully replicate the movement of robots on real road surfaces; therefore, we conducted experiments using a self-developed car-like robot platform. Figure 15 shows the robot we used, with its components listed in Table 6 and its kinematic parameters provided in Table 7.

The experiment was conducted outdoors, with multiple barriers used as obstacles. The map configuration is shown in Figure 16.

On the constructed map, TEB, Smooth TEB, and FLC-TEB were used for planning. The resulting kinematic characteristic curves are shown in Figure 17

Table 8 presents the results of this real-world experiment. From the data, it can be concluded that the FLC-TEB algorithm demonstrated significant advantages. In terms of trajectory length, FLC-TEB generated the shortest trajectory (9.41 meters), significantly outperforming TEB (11.19 meters) and Smooth-TEB (10.63 meters). This is because both TEB and Smooth-TEB required reversing to adjust the forward heading angle, whereas FLC-TEB did not encounter such situations. In contrast, FLC-TEB's trajectory duration was longer than TEB's but shorter than Smooth-TEB's. Additionally, both FLC-TEB and Smooth-TEB exhibited lower linear and angular velocities compared to TEB, indicating that FLC-TEB achieved better time results while ensuring smoother paths and velocity controls.

In terms of trajectory smoothness, FLC-TEB also demonstrated excellent performance. The average heading angle variation on its trajectory was 0.0358 rad, a 39% reduction compared to TEB (0.0504 rad). The linear velocity smoothness (0.0541 m²/s²) and angular velocity smoothness (0.0382 rad²/s²) of FLC-TEB's generated trajectory significantly outperformed TEB. It was slightly inferior to Smooth-TEB only in angular velocity smoothness. This indicates that FLC-TEB effectively reduces speed and acceleration fluctuations, improving trajectory smoothness and motion stability.

Overall, FLC-TEB not only provided shorter and smoother trajectories in path optimization but also enhanced the robot's motion stability and efficiency through reasonable time control and velocity fluctuation management, demonstrating significant advantages in practical applications.

5. Conclusions

This paper proposes a fuzzy logic-based improved TEB algorithm (FLC-TEB) for local path planning of car-like robots. The algorithm introduces constraints on trajectory smoothness and jerkiness to improve the smoothness of trajectories and the stability of velocity control inputs. It dynamically adjusts the objective function weights based on real-time evaluations of the narrowness and turning complexity of the environment, thereby enhancing trajectory quality.

• Without the fuzzy controller, the introduction of trajectory smoothness and jerkiness constraints can improve trajectory smoothness and velocity stability but at the cost of increased trajectory length and execution time;
• The introduction of the fuzzy controller significantly improves trajectory smoothness and velocity stability while adaptively adjusting the weights of the objectives based on the region's characteristics. In narrow or highly curved areas, it prioritizes improving trajectory smoothness and velocity stability, whereas in open or near-linear areas, it reduces trajectory length and execution time to achieve a better overall trajectory quality;
• Simulation results show that in narrow and continuously curved channels, FLC-TEB achieved a path length nearly identical to the classic TEB, with trajectory duration increasing by 6%, but turning angle variations decreased by 38%. Real-world experimental results further validated the effectiveness of the algorithm. In an open outdoor map with regions formed by discrete obstacles, FLC-TEB increased trajectory duration by 16% compared to classic TEB, but shortened the trajectory length by 16%. Turning angle variations decreased by 39%, linear velocity smoothness improved by 71%, angular velocity smoothness improved by 38%, and no reversing behavior was observed. These results indicate that for trajectory planning of car-like mobile robots, FLC-TEB significantly enhanced the adaptability and robustness of the TEB algorithm in complex environments.

In future work, more precise models will be considered to identify the robot's regional context, and the fuzzy sets of the fuzzy controller will be further refined to enhance the adaptability of FLC-TEB.