1. Introduction
Mobile robots have become an integral part of modern applications, including cleaning, industrial automation, and warehousing [
1]. They are also well suited for replacing humans in tasks that are monotonous or hazardous or require operation in challenging environments. To achieve autonomy, a mobile robot must possess the ability to perceive its surroundings and plan a collision-free trajectory from the starting position to a designated target [
2]. Path planning plays a crucial role in this process and is generally categorized into global and local path planning, depending on the availability of complete environmental information [
3].
Global path planning algorithms encompass well-established techniques such as Rapidly-Exploring Random Tree (RRT), the ant colony algorithm [
4], the bee colony algorithm [
5], particle swarm optimization [
6], Dijkstra’s algorithm [
7], and the A* algorithm [
8]. In contrast, local path planning methods primarily include the Dynamic Window Approach (DWA) [
8], the Timed Elastic Band (TEB) [
9], and the artificial potential field (APF) method [
10]. The paths generated by these algorithms are typically composed of discrete straight-line segments and abrupt turns [
11], which are suboptimal for direct execution due to inefficiencies in motion smoothness and control feasibility.
Trajectory planning refines a reference path to produce a dynamically feasible and executable trajectory for a mobile robot. This process must account for kinematic constraints to ensure smooth motion, stability, and feasibility during execution. While traditional path planning techniques provide collision-free paths, they often overlook the robot’s dynamic characteristics, leading to sudden turns or abrupt acceleration changes that degrade navigation performance. Consequently, optimizing the trajectory to conform to the robot’s kinematic constraints while enhancing smoothness has become a critical research focus in mobile robot navigation.
In practical applications, mobile robots frequently encounter complex system challenges such as nonlinear dynamics, input constraints, and actuator faults. Existing research predominantly centers on maintaining system stability and ensuring reliable task execution. For instance, fault-tolerant control strategies that integrate control barrier functions (CBFs) with reinforcement learning have demonstrated the ability to uphold safety under unknown disturbances [
12]. Similarly, robust approaches grounded in differential game theory have proven effective for modeling intricate tasks with state constraints [
13]. Additionally, lightweight neural network architectures designed for edge deployment have gained traction in visual defect detection tasks within resource-limited environments [
14,
15]. While these works do not directly address trajectory planning, they collectively underscore the critical importance of stability and physical executability during the task execution phase.
This study primarily focuses on generating smooth trajectories with continuous curvature. A widely adopted approach in this domain is path smoothing, which refines an initially coarse reference path to ensure curvature continuity and meet smoothness requirements. Among the various path smoothing techniques, curve-based methods are frequently employed to interpolate sparse waypoints and generate continuous and feasible trajectories.
One popular category within this approach is spline interpolation, such as Catmull–Rom splines, which have attracted significant attention due to their ability to construct smooth and continuous curves using Lagrange interpolation and B-spline basis functions [
16]. In particular, cubic Catmull–Rom splines have been used to generate trajectory segments at intersections while preserving continuity at junction points [
17]. However, a key limitation of spline-based methods lies in their restricted local control, which makes it difficult to finely adjust curve shapes to meet specific trajectory constraints.
Beyond spline-based approaches, Dubins curves and Reeds–Shepp curves provide alternative solutions for computing the shortest path between two configurations while respecting vehicle kinematic constraints [
18,
19]. Despite their effectiveness in satisfying motion constraints, these methods often require additional collision-checking steps, as the resulting paths may deviate substantially from the original reference trajectory.
Bézier curves offer another flexible trajectory generation strategy by defining paths through a series of control points [
20]. They have been extensively studied for path smoothing [
21,
22,
23,
24], owing to their capability to produce smooth trajectories. However, controlling the curvature of Bézier curves remains challenging, particularly when compared to clothoid curves, which feature a linear change in curvature and are thus well suited for generating gradual and smooth transitions during motion planning.
Originally proposed by the French mathematician Euler, the clothoid curve has been widely applied in autonomous vehicle applications, particularly in automated parking scenarios [
25,
26]. It has also been adopted in many studies for generating smooth and feasible trajectories [
27]. Nevertheless, existing implementations often fail to consider the acceleration constraints specific to two-wheeled differential-drive robots, limiting their ability to fully satisfy the practical motion requirements of such systems.
Beyond path smoothness, practical trajectory planning must also consider dynamic constraints, such as velocity, acceleration, and jerk, to ensure smooth motion while maintaining system feasibility. Additionally, minimizing the trajectory execution time is crucial for improving the responsiveness and overall efficiency of the system. However, enforcing velocity, acceleration, and jerk constraints inherently limits the extent to which the trajectory duration can be minimized. To address this issue, time-optimal trajectory planning techniques have been developed to strike a balance between execution time minimization and adherence to motion feasibility constraints.
As a result, ongoing research is increasingly focused on developing advanced trajectory planning algorithms that can adapt to various dynamic scenarios while optimizing overall vehicle performance [
28]. Among the local trajectory planning methods, several approaches have been extensively studied, including techniques based on lateral acceleration [
29], network search algorithms [
30], sampling-based motion planning [
31,
32], and artificial potential field (APF) methods [
33,
34]. To enhance solution space flexibility, Ref. [
35] proposed an optimization framework that utilizes trapezoidal prism-shaped corridors, offering significant improvements over traditional cuboidal corridor-based methods. Similarly, Ref. [
36] introduced a spatial traversability mapping approach tailored for urban driving, though its high computational complexity remains a limitation for large-scale trajectory generation.
Further advancements in trajectory generation have introduced second-order Bézier curve-based techniques [
37], which, although effective in specific applications, face limitations in handling complex and dynamic environments. To overcome the constraints associated with predefined reference trajectories, Ref. [
38] proposed a trajectory generation framework that accounts for system constraints and produces multiple candidate paths. However, the performance of such methods remains sensitive to factors such as model accuracy, computational overhead, and real-time adaptability. In parallel, Ref. [
39] developed an improved artificial potential field (APF)-based trajectory planning method that integrates adaptive distance modulation, dynamic road repulsion, and velocity-constrained planning. While these enhancements improve trajectory feasibility, APF-based methods are inherently susceptible to local minima, which may result in suboptimal paths or navigation deadlocks. The TubeSTRRT* algorithm notably improves the physical executability of trajectories by incorporating a trajectory optimization mechanism [
40]. Moreover, the integration of Bézier curves with time parametric frameworks allows for compliance with speed constraints while ensuring smooth transitions in control commands [
41]. Additional approaches such as eliminating pseudo-acceleration discontinuities via quadratic polynomial interpolation [
42] and enhancing trajectory continuity using spline interpolation strategies, as demonstrated in the NC-BRRT algorithm [
43], provide valuable perspectives on optimizing trajectory smoothness.
To address these challenges, this paper proposes an optimized trajectory generation method based on clothoid curves for two-wheeled differential-drive mobile robots. The proposed approach integrates both trajectory and velocity planning, aiming to minimize the execution time while ensuring smooth transitions. Ultimately, it generates an optimal and dynamically feasible trajectory tailored to two-wheeled differential-drive systems. A flowchart illustrating the proposed method is presented in
Figure 1. The remainder of this paper is organized as follows:
Section 2 presents the fundamental modeling of the clothoid curve.
Section 3 introduces the proposed trajectory generation and parameter optimization modules.
Section 4 provides the experimental results, and
Section 5 concludes the paper.
3. Proposed Algorithm
3.1. Trajectory Generation Module
In this section, we focus on modeling the trajectory planning scheme based on clothoid curves. The proposed approach constructs a trajectory using a straight-line segment + composite curve structure, where the composite curve consists of clothoid segments and a circular arc. Due to the relative orientation difference between the initial and target stations, the trajectory must satisfy the heading angle constraints at the connection points. Therefore, we first design the composite curve to meet these angular requirements. The length of the straight segment can be easily computed by subtracting the coordinates once the composite curve is determined. Hence, we limit our discussion here to the modeling of the composite curve.
As shown in
Figure 3, the green solid line represents the circular arc, while the red solid line denotes the clothoid segment. The composite curve starts at point
, where the tangent angle is zero, and it ends at point
, where the tangent angle is
, representing the orientation difference between the start and target stations. The center of the circular arc is denoted by point
, and its radius is
R. The clothoid is tangentially connected to the arc at point
.
The clothoid segment from to is symmetrically mirrored about the line segment , resulting in a mirrored clothoid curve from point Q to point . The complete composite curve, composed of a clothoid–arc–clothoid structure, thus satisfies both the orientation constraints at the endpoints and the curvature continuity.
By adjusting the a parameter of the clothoid and the radius R of the arc, the horizontal and vertical coordinate differences between and can be modified, enabling the precise alignment of the curve with the desired initial and terminal positions.
Next, the clothoid curve segment from point to point is discussed. According to Formula (1), the parameter of the clothoid curve is denoted as a, the total length of the curve is , and the azimuth angle of the mobile robot at point is . The variable represents the difference in the abscissa between point and point , while is the difference in the ordinate between point and point . The variable denotes the difference in the abscissa between point and point , and represents the difference in the ordinate between point and point .
By introducing the above formula for the clothoid curve, we can obtain the following equations:
where
N is the order of the approximation for the clothoid curve. For simplicity in the calculations, we adopt
uniformly in this experiment. The radius
R can be obtained by solving the following nonlinear equation:
Therefore, once the a parameter of the clothoid curve is specified, the radius R of the arc segment can be computed using the method described above. At this stage, all necessary parameters for constructing the composite clothoid–arc–clothoid curve are determined. The next step is to consider how to represent this combined curve using control points and the associated parameters.
As previously described, the clothoid segment begins at point
and is tangent to the arc segment at point
. The curve then transitions into the arc segment from
to
, followed by a second clothoid segment with identical parameters extending from
to
. The full composite curve consists of a clothoid from
to
, followed by an arc from
to point
Q, and it is then symmetrically reflected about the line segment
. This reflection can be achieved using an affine transformation, with the transformation matrix denoted as
. Consequently, the coordinates of each control point along the trajectory, as well as the parameters of the respective curve segments, can be expressed through this affine mapping.
The meanings of the parameters in are as follows: is the curvature at the starting position, is the azimuth at the starting position, is the curvature at the target position, is the azimuth at the target position, and a is the curvature change rate of this curve. Additionally, a is the parameter of the clothoid curve, and for the arc curve, .
At this stage, the composite clothoid–arc–clothoid curve has been fully constructed, and the coordinates of the control points, along with the parameters for each corresponding segment, can be stored. With these parameters, the planned trajectory can be readily reconstructed using the derived clothoid curve equations. Within this composite curve, the curvature of the arc segment is given by . Since the curvature of a clothoid increases linearly with the arc length, also represents the maximum curvature of the entire trajectory.
Assuming that the two-wheeled differential mobile robot moves with a uniform speed during the tracking of the entire combined curve, its maximum speed
must satisfy the following relation:
where
is the safety factor set during the operation of the two-wheeled differential mobile robot. After the
a parameter is given and the maximum curvature
is calculated, the total running time of the two-wheeled differential mobile robot is also determined by the following equation:
where
represents the total length of the combined curve, and
is the translation length of the starting point or the target point, which is considered the linear motion of the mobile robot.
So far, a transition curve consisting of a clothoid curve–arc curve–clothoid curve has been constructed between the starting point and the target point after the coordinate transformation preprocessing. After the complete planning of the entire path, it is necessary to further discuss the influence of the clothoid curve’s parameter setting on the running time and trajectory. By solving Equation (
36), the correspondence between the parameters can be obtained. Based on this, the running time becomes a function related only to the variable, allowing for a theoretical analysis of the monotonicity of the running time with respect to this variable. Experimental verification demonstrates that there is a monotonically decreasing relationship between the parameter and the running time: as the given parameter of the clothoid curve increases, the total running time of the entire path decreases. The results of a Matlab simulation and verification experiment are shown in
Figure 4, with the parameters in the figure ranging from 0.5 m to 30 m. This relationship is based on the initial point, azimuth angle, target point, safety factor, and target azimuth angle. Since the physical conditions are not strictly limited and the actual two-wheeled differential mobile robot needs to undergo acceleration and deceleration phases, this conclusion can only serve as a reference for selecting the curvature change rate parameters.
3.2. Kinematic Modeling of Two-Wheel Differential Robot
Given that the curvature of a clothoid curve changes linearly with the path length, we consider a four-wheeled vehicle as an example. In this case, if the vehicle maintains a constant linear speed while turning and its steering wheel rotates at a uniform rate, the resulting trajectory follows a clothoid curve. This practical analogy highlights a significant limitation of the clothoid curve when applied to a two-wheeled differential-drive robot.
Unlike four-wheeled vehicles, a two-wheeled differential-drive robot relies on the independent speed control of its two wheels for both steering and forward motion. Since turning is achieved through the speed ratio between the two wheels and the overall velocity is also jointly determined by them, steering and forward movement are inherently coupled. Consequently, changes in direction necessitate variations in wheel speed, which are constrained by physical limits on velocity and acceleration.
If the curvature change rate of the clothoid curve is too large, the planned trajectory may violate the kinematic constraints of the robot, making it infeasible to execute. Therefore, it is essential to impose restrictions and optimize the parameter settings to ensure that the planned trajectory remains within the robot’s operational limits.
Assume that a two-wheeled differential-drive robot executes a left-turn maneuver. Let the speed of the left wheel be
, the speed of the right wheel be
, the wheelbase (the distance between the two wheels) be
d, the linear velocity of the robot be
, and the turning radius be
R, as shown in
Figure 5. The turning radius and linear velocity can be expressed in terms of
,
, and
d as follows:
The curvature
k of the trajectory is then given by
Since parameter
a of the clothoid curve characterizes the rate of curvature change with respect to the arc length, it is necessary to compute the derivative of the curvature
. For convenience, we denote this derivative as
.
where
and
denote the derivatives of
and
with respect to the path length. If the accelerations of the left and right wheels are denoted as
and
, respectively, then
and
are given by
Using these expressions,
can be rewritten in terms of
,
,
, and
. By setting
, we obtain the relationship between the accelerations of the left and right wheels:
This equation implies that, when the robot follows a clothoid curve during turning, the accelerations of the left and right wheels must satisfy a specific relationship. Moreover, this relationship depends not only on the accelerations but also on the current speeds of the wheels.
3.3. Velocity Constraints in the Critical State
To further analyze a complete trajectory, we consider a scenario in which the two-wheeled differential robot initially moves in a straight line before executing a turn, with its velocity at points
and
being zero. The planned trajectory for this case is illustrated in
Figure 6.
Point is the starting point, and point is the endpoint. The trajectory consists of four segments: the - segment is a straight line, the - segment is a clothoid transition curve, the - segment is an arc, and the - segment is a symmetrical clothoid transition curve.
When the two-wheeled differential-drive robot reaches point
through linear motion, its wheel velocities satisfy
. At this point, the robot enters the clothoid segment, where the accelerations of the left and right wheels must satisfy Equation (49). Substituting
into this equation yields the following:
Since
a,
d, and
are all non-negative, it follows that
. When
is large,
decreases, and, given that the wheel acceleration of the two-wheeled differential-drive robot cannot be infinite, the constraint
must be satisfied. To ensure that the robot adheres to this acceleration constraint at point
, we obtain the following:
During the execution of the clothoid curve trajectory, if there exists a velocity pair
and a corresponding acceleration
such that
, the system is said to be in a critical state. If
, increasing
allows
to exceed
, thereby escaping the critical state. However, if
gradually increases until it reaches
, no further adjustments can be made, and the system remains in the critical state. At this point, with
and
, after an infinitesimal time interval
, the system satisfies the following:
Since , the above equation holds only at . For any , the right-hand side is greater than , meaning that the system naturally escapes the critical state. Therefore, concerns about acceleration constraints along the clothoid curve trajectory are unnecessary, and only the acceleration at the starting point (where the curvature is zero) needs to be considered.
Once the constraint on the curvature change rate parameter of the clothoid curve is determined, the velocity profile of the two-wheeled differential-drive robot along the entire trajectory can be planned efficiently, allowing for the rapid generation of feasible motion plans.
3.4. Velocity Planning of Two-Wheel Differential Robot
In the clothoid curve model, the linear velocities along the trajectory from to are denoted as to , while the corresponding velocities of the left and right wheels are represented by to and to , respectively. The complete trajectory consists of consecutive segments: a straight line, a clothoid curve, an arc segment, and another clothoid curve, starting from . In some cases, the trajectory may begin directly with a clothoid curve instead of a straight segment. When this occurs, the planning sequence can be adjusted accordingly, allowing trajectory generation to start from the target point. This adjustment facilitates the construction of a more unified mathematical model.
The velocity of the arc segment is primarily determined by the velocities at points and , making it essential to consider these speeds first in the velocity planning process. Once these velocities are determined, the feasibility and optimality of the velocities at other points should be evaluated. Typically, a mobile robot travels faster along straight-line segments and must decelerate when approaching curves to avoid instability or the risk of rollover caused by excessive speed. Therefore, to minimize the total traversal time, the robot should maintain the maximum allowable speed at each point while ensuring safety.
At point
, the velocities of the left and right wheels of a two-wheeled differential robot are computed based on safety constraints. The maximum allowable linear velocity at
is determined by
From Equation (43), the expressions for
and
can be derived accordingly.
The above derivation indicates that the two-wheeled differential robot can achieve the maximum possible linear velocity at point
. Once the velocities of the two wheels at
are determined, it is further necessary to consider how to set the acceleration of the wheels along the clothoid curve trajectory. Given the objective of minimizing the travel time from
to
, the fundamental kinematic equation
suggests that, for a given distance and velocity, increasing the acceleration reduces the travel time. Since the velocity at
is predetermined, the linear velocity should be maximized throughout the transition from
to
. This implies that the left wheel of the robot remains in a deceleration state throughout this process.
Denoting the average acceleration of the left wheel during the movement from
to
as
, the following relationship between distance and time can be established:
From this, it follows that the travel time t is inversely related to the average acceleration . Therefore, to minimize the travel time, should be as small as possible, leading to the choice . Given that , the above equation holds only if .
Thus, the velocities of the left and right wheels at
can be computed as follows:
where
represents the path length of the left wheel in the clothoid curve segment.
In order to obtain
, it is necessary to analyze the relationship between the velocities of the left and right wheels and the linear velocity of the two-wheeled differential robot. The specific derivation process is as follows:
The above equations establish the ratio between the micro-components of the left wheel displacement and the overall movement of the two-wheeled differential robot. Specifically, in the clothoid curve segment, given that
the expression for
can be derived by substituting the above equation and applying calculus principles:
When
, we have
, which leads to
Since the clothoid curve segment satisfies
, it follows that
At this point, the velocities of the two wheels at point have also been determined.
Through the above derivation, it can be observed that the velocity of the two-wheeled differential robot at point is greater than zero, whereas the linear velocity at point is exactly zero. This results in an asymmetry between the speed planning of the clothoid curve segments - and -.
Similar to the segment
-
, the segment
-
should ensure that the left wheel undergoes maximum deceleration at point
. Consequently, the velocity at
can be derived from the velocity at
:
Since there is no explicit size relationship between and , it is necessary to determine the required operations for the two-wheeled differential robot along the arc curve segment - in order to meet the velocity constraints at . Given that the initial velocity, target velocity, and path length along the arc curve are known, a physical analysis suggests that a higher acceleration results in a shorter traversal time.
Therefore, the shortest travel time can be achieved by setting the acceleration of the right wheel to in the variable-speed motion along the arc when and by setting the acceleration of the left wheel to when .
On the straight-line segment
-
, the left and right wheels maintain the same velocity and acceleration. A physical analysis shows that the acceleration along this segment should be
which corresponds to either the maximum or minimum acceleration required for the shortest travel time. At this point, the velocity at each key point and the acceleration profile for each segment of the two-wheeled differential robot’s trajectory have been determined, allowing for the next step: the construction of an optimization model.
3.5. Optimization Module
In addition, the left wheel acceleration
is maintained during the acceleration process along the straight line, and the left wheel acceleration
is maintained during the deceleration process, as well as in the clothoid curve segment. In the variable-speed process of the arc curve segment, different accelerations are selected based on the relationship between
and
:
It is assumed that the shortest path for the two-wheeled differential robot to accelerate from 0 to
in a straight-line segment is
, and the shortest path for the robot to accelerate from 0 to
is
. Based on the relationship between these two distances, different strategies should be chosen for the line segment’s motion process. The core strategy is to accelerate directly from point
to the maximum speed
supported by the car and then to decelerate to point
after traveling a distance at a constant speed. Since the length of the straight track
may not allow the car to reach the maximum speed, the above core strategy can be expressed as follows:
According to the varying speeds of the two-wheel differential robot at points and , distinct acceleration and deceleration strategies must be employed. The core objective is to maximize the portion of the trajectory traveled at the maximum allowable speed. The primary approach involves accelerating the robot as quickly as possible to reach this maximum speed.
Assuming that the left wheel follows a circular arc, where it accelerates over a distance
, maintains a constant speed over
, and decelerates over
, the process can be formulated as follows:
where
represents the length of the circular arc, given by
where
is the target azimuth after preprocessing the initial conditions. At this point, the velocity at each point along the trajectory is explicitly defined, allowing for the computation of the total traversal time,
, as follows:
The total traversal time is then obtained as
At this stage, the objective function for minimizing the total traversal time can be formulated. The next step involves optimizing the selection of the curvature rate parameter of the cyclotron curve using an optimization algorithm. The specific optimization model is formulated as
4. Experimentation
The experiment sets the simulation parameters according to the actual physical parameters of the two-wheeled differential-drive robot in the experimental field. Specifically, the parameters are
Figure 7,
Figure 8,
Figure 9 and
Figure 10 present comparison graphs of the optimization algorithms under different scenarios, where the distance is measured in meters, the time is measured in seconds, and the speed is measured in meters per second. Due to the strong correlation between the optimization effect and the actual path, the degree of optimization cannot be precisely quantified. Based on the experimental comparison diagram, it is evident that the two-wheel differential robot achieves a higher linear velocity on the optimized trajectory, particularly along the clothoid curve and arc segments, while consistently maintaining high acceleration. Specifically, the initial pose in
Figure 8 is consistent with that in
Figure 7, while the target position is obtained by shifting the target point in
Figure 7 forward by 0.5 m along its orientation. A comparison between
Figure 7 and
Figure 8 reveals that, due to the shorter straight-line segment in
Figure 7, the robot must begin decelerating before reaching the maximum velocity
, thus limiting the potential for higher speeds. Although the maximum velocity is still not fully achieved after parameter optimization, both the peak velocity and the average velocity are improved.
In contrast,
Figure 8 features an extended straight-line segment, which provides the robot with a sufficient acceleration distance to reach
. Moreover, after parameter optimization, the robot is able to maintain this maximum speed for a longer period, thereby increasing the overall average velocity.
Figure 9 and
Figure 10 illustrate scenarios involving obtuse turning angles, under the same experimental setup as in
Figure 7 and
Figure 8. These comparative experiments demonstrate that, after trajectory parameter optimization, the robot is capable of sustaining high-speed motion for a longer duration, which significantly improves the average velocity along the trajectory and effectively reduces the task execution time. We conducted additional comparative experiments but only present the most representative cases here. The results show that, after parameter optimization, the overall execution time is reduced by approximately 15%.