Minimum-Time Trajectory Generation for Wheeled Mobile Systems Using Bézier Curves with Constraints on Velocity, Acceleration and Jerk

This paper considers the problem of minimum-time smooth trajectory planning for wheeled mobile robots. The smooth path is defined by several Bézier curves and the calculated velocity profiles on individual segments are minimum-time with continuous velocity and acceleration in the joints. We describe a novel solution for the construction of a 5th order Bézier curve that enables a simple and intuitive parameterization. The proposed trajectory optimization considers environment space constraints and constraints on the velocity, acceleration, and jerk. The operation of the trajectory planning algorithm has been demonstrated in two simulations: on a racetrack and in a warehouse environment. Therefore, we have shown that the proposed path construction and trajectory generation algorithm can be applied to a constrained environment and can also be used in real-world driving scenarios.


Introduction
Path planning and trajectory planning are fundamental topics in autonomous mobile robotics and even more broadly in the context of automation [1]. Path planning algorithms generate a path through predefined points with the main goal of finding a continuous path that minimizes the distance between the starting point and an end point without colliding with obstacles [2,3]. While path planning is a geometric problem, trajectory planning additionally parameterizes the resulting path by time. Consequently, defining time moments along a path affects the kinematic and dynamic properties of the motion of a mobile system. Forces and torques depend on acceleration along a trajectory, while vibrations of its mechanical structure are mainly determined by values of jerk, the time derivative of acceleration [4].
The aim of our work was to solve the problem of minimum-time trajectory generation for wheeled mobile systems with constraints on velocity, acceleration, and jerk in a limited planar space without obstacles. The idea we propose is to apply an optimization method to determine the construction parameters of a Bézier curve primitive such that the resulting travel time on a complete smooth path is minimal. The algorithm we use to compute the minimum-time velocity profile is presented in Ref. [5]. It computes the velocity profile on a predefined path under specified constraints on velocity, radial and tangential acceleration, and radial and tangential jerk.
This paper is organized as follows. Section 2 gives a general overview of the related work. Section 3 introduces the research problem and our main objectives, and Section 4 briefly lists the main contributions. The novel methodology of constructing and parameterizing the fifth-order Bézier curves that make up the resulting geometric path is detailed in Section 5.1. In Section 6, we present two applications of our proposed trajectory generation algorithm, namely the computation of the minimum-time trajectory of a wheeled mobile measured from a given fixed origin. The velocity vector v(t), the acceleration vector a(t), and the jerk vector j(t) in the tangential-normal form are: whereT andN are the unit tangential and the unit normal vector, respectively, and κ is the curvature of the path at time t. In Equation (1a), v is called speed, and the tangential and normal components of the acceleration (Equation (1b)) are called acceleration along the path and centripetal acceleration (also called radial acceleration), respectively. The expression (1c) is obtained by differentiating Equation (1b) and applying the Frenet-Serret formulas for movement in two-dimensional Euclidean space R 2 [41]. The maximum allowable value of velocity v MAX is determined by the capabilities of the robot actuators and also the environmental conditions (e.g., surface type). Driving in the reverse direction is not permitted. The maximum values of radial acceleration a R MAX and tangential acceleration a T MAX can be set based on the dynamic constraints of a mobile robot (e.g., maximum centripetal force and rolling resistance in a turn) [6]. Similarly, we imposed third-order constraints j R MAX and j T MAX , whose values can be derived from ride comfort criteria [9]. Although we could limit the acceleration and jerk components separately (and treat them individually), we additionally restricted their values to the range within an ellipse (similar to researchers in [5,6,35,42]: Treating the tangential and radial components of acceleration (Equation (2b)) and jerk (Equation (2c)) together is more rigorous than limiting the individual components. It also provides greater ride comfort by constraining the overall norms. The goal of this research was to develop a trajectory planning method for a mobile system operating in a constrained, obstacle-free, planar environment while subject to kinematic constraints. Although motion planning algorithms have been the subject of extensive research, dealing with third-order constraints still proves challenging.

Contributions
The main contributions of this paper can be summarized as follows: • We describe an innovative construction method for 5th order Bézier curves. The proposed parameterization is simple and intuitive, yet effective for generating smooth paths consisting of multiple splines (Section 5); • The above smooth path generation basis is coupled with an algorithm that computes a minimum-time velocity profile with velocity, acceleration, and jerk constraints on a predefined path (see Ref. [5]). Together they form a powerful trajectory generation algorithm (Section 6). The resulting trajectories thus provide continuous velocity and acceleration profiles; • To prove the applicability of our approach to trajectory optimization, we performed simulation experiments on a racetrack and in a warehouse environment (Sections 6.1 and 6.2).
In the warehouse simulation, we identified and analyzed realistic situations with different dynamic constraints to investigate and propose the most appropriate driving scenarios.

Curve Primitives
A Bernstein-Bézier curve (or Bézier curve) is defined by a set of control points P 0 , P 1 , . . . P b , b ∈ N : where λ is a normalized time variable (0 ≤ λ ≤ 1) and p i denotes the position vector of a control point P i . The polynomials B i,b (λ): are known as Bernstein basis polynomials of degree b. Bézier curves can be defined for N-dimensional space, N ∈ N. In planar space, the curve r b (λ) and the vectors p i are two-element vectors: These curves have several useful properties for path planning. The first and last points of the Bézier curves introduced in Equation (3) are their endpoints: The N-dimensional, b-th order Bézier curve also lies within the convex hull defined by its control points. Furthermore, the beginning and the end of the curve are tangent to the first and the last section of the convex polygon, respectively ( Figure 1).
P 0 P 1 P 2 P 3 P 4 P 5 Figure 1. Fifth order Bernstein-Bézier curve within its convex hull (dashed lines). The curve is tangent to the sides of the convex hull, line segments P 0 P 1 and P 4 P 5 .
Other properties of Bernstein polynomials (derivatives, calculation of definite integrals, the de Casteljau algorithm, degree elevation, etc.) fall outside the scope of this article; more details on this topic can be found in Ref. [28].
Bézier curves constructed by a large number of control points are computationally intensive. For this reason, in path planning, it is desirable to construct a smooth path by connecting low-degree Bézier curves [6]. The authors in Ref. [43] proposed a new parameterization of motion primitives based on Bézier curves for path planning applications of wheeled mobile robots. However, the method was presented for third-order polynomials and the algorithm does not guarantee the existence of the curve for all possible parameterizations. We used fifth-order Bézier curves because this is the degree of Bézier curves that always satisfies the curvature continuity requirement (C 2 ) in the joints. The 5th order Bézier curve r 5 (λ) is defined by six control points P i :

Construction of 5th Order Bézier Curves
It is very important to choose the appropriate construction parameters that would efficiently define the Bézier curves and facilitate the search for the minimum-time trajectory.
With the above notation, let us mark the distances between consecutive control points d(P i , P i+1 ) as d i+1 and the angles between (P i , P i+1 ) and the positive direction of the x-axis as ϕ i+1 (Figure 2), i = {0, 1 . . . , 4}. For the coordinates of two consecutive control points, it follows that: We evaluate (9a) and (9b) for i ∈ {0, 1} using the sum and difference formulas for sine and cosine. This gives the following expression for the value of the curvature in P 0 : The derivative of curvature κ 0 in P 0 with respect to λ is: We choose the parameters of the curve so that the second term in Equation (11) becomes 0. This happens when: The curvature κ 0 from Equation (10) and its derivative κ 0 in P 0 from Equation (11) then become: The purpose of introducing notations for d i and ϕ i and deriving expressions for κ 0 and κ 0 is to make the process of path construction as efficient and intuitive as possible. This also takes into account that the path ultimately consists of several Bézier curves. Thus, the parameters needed to generate a 5th order Bézier curve are P 0 , P 5 , ϕ 1 , ϕ 5 , κ 0 , κ 5 , d 1 , d 5 , and κ 0 . However, how would one set the value of κ 0 ? It could be simply set to zero, but perhaps it is also useful to examine Equation (1c) and choose such a value for κ 0 that the value of the radial component (in P 0 ) of the jerk vector is zero.
A 5th order Bézier curve is therefore constructed in the following steps ( Figure 2): 1.
Outline the first control point and mark it as P 0 . In the direction of ϕ 1 , measure out the distance d 1 and mark the second point as P 1 .
Measure in the perpendicular direction the distance d ⊥ 2 (from Equation (10)): and mark the third point as P 2 . 4.
All points away from P 2 for d ⊥ 3 (Equation (11)) in the same direction (perpendicular to the line segment P 0 P 1 ) lie on the red dashed line.

5.
Mark the last point as P 5 . Measure out the distance d 5 in the opposite direction from ϕ 5 and mark the fifth point as P 4 . 6.
All points away from P 4 for d ⊥ 4 (Equations (9a), (9b) and (10) for i = 4) in the same direction (perpendicular to the line segment P 4 P 5 ) lie on the green dashed line: 7.
The fourth control point P 3 lies on the intersection of the red and green dashed lines. The Bézier curve is now completely defined.

Generation of Minimum-Time Trajectories
We have shown the use of the proposed trajectory planning algorithm in two environments. On a racetrack, the focus was on demonstrating path construction and ensuring that it is within the corridor boundaries. In a warehouse, we demonstrated the benefits of our proposed methods in a real-world application. All simulations were performed using the Matlab programming environment on a computer with an Intel(R) Core(TM) i7-8700 CPU 3.2 GHz processor with 16 GB RAM memory.
A minimum-time trajectory is computed by applying an algorithm that generates a minimum-time velocity profile (proposed in Ref. [5]) to Bézier curve splines. The algorithm, which considers velocity, acceleration, and jerk constraints along a given path, consists of two steps. In the first step of the algorithm, the velocity and acceleration constraints are considered. In the second step, the algorithm modifies the original velocity profile to include the jerk constraints, with the process varying depending on the type of violation (single-point or interval jerk violations). The simulation methodology for computing the minimum-time velocity profile, described in detail in Ref. [5], can essentially be described as solving the presented ordinary differential equation with a given initial value. In our own implementation, numerical methods (Euler's method and trapezoidal integration) were used to calculate the required values in the discrete time samples.
We then used a nonlinear gradient optimization method, an optimization routine built into Matlab, to change the construction parameters of the Bézier curves. Using this method, we found a solution where the travel time reached a minimum. Since the simulated environments were static and free of obstacles, we divided the environments into several individual sections. This was done primarily to reduce the number of optimization parameters and consequently speed up the minimization process.

Racetrack Environment
The model of a racetrack that we used in our simulations is shown in Figure 3. It is defined by the centerline. The left and right edges of the racetrack are at a distance w/2 from the centerline and represent the corridor boundaries. The shape of the racetrack can in general be arbitrary complex and is therefore divided into segments. This is done by analyzing the curvature of the centerline. Points where the curvature reaches local extrema are denoted by the sequence  Thus, each curve in a segment is completely defined by the positions, angles, and curvatures in the first and last control points (p 0 , ϕ 1 , κ 0 , p 5 , ϕ 5 , κ 5 ), d 1 , and d 5 . Note that the angles are measured from a tangent to the centerline in C i (Figure 4). To find a reasonable starting point for the optimization, we devised a simple heuristics. In each segment, a line is drawn from P i 0 through the outermost edge of the inner side of the corridor at the end of the (i + 1)th segment. The intersection of lines and m i is the initial estimate for the position p i 5,init of the last control point P i 5 . If the intersection point is outside the corridor (as in Figure 5), p i 5,init is set to its edge. The initial estimate for the angle ϕ i 5,init is the angle between the line and the line perpendicular to m i+1 , while κ i 5,init is the curvature of the centerline in C i+1 . The values of d i 1 and d i 5 were set to the value of a certain fraction of the distance between P i 0 and P i 5 . The heuristic procedure described is shown in Figure 5. To simplify the notation, we will from now on omit the superscript i.  We devised a series of separate simulation experiments to demonstrate the operation and efficiency of the proposed Bézier curve construction and trajectory planning algorithm.
Let N free denote the number of optimization parameters on each corridor segment. It is expected that the higher the number of (free) curve construction parameters N free , the more versatile a curve is. Thus a better solution can be provided. However, in this way the optimization problem becomes more computationally expensive and the solution more difficult to obtain due to the complex form of the objective function.
Another problem is the gradual generation of the final trajectory. A Bézier curve can be constructed for each segment separately and have the criterion assigned to it. Alternatively, multiple Bézier curves spanning several segments can be constructed together, and the objective is the travel time along all of them. Then only the solution on the first segment is kept and this procedure is repeated in a receding horizon manner. Let us denote by N seg the number of segments that are treated simultaneously. If only a one-segment optimization is performed (N seg = 1), the solution does not take into account the corridor shape in the following segment, e.g., when a sharp turn follows. On the other extreme, the complete curve (on all corridor segments) can be generated in each run of the optimization, but since the dimension of the optimization problem is the product of the construction parameters, namely N free × N seg , it is reasonable not to exaggerate the two values and to find a sensible compromise.
Simulations were performed for N free ∈ {2, 5} and N seg ∈ {1, 2, 3}. Therefore, in one group of experiments, the two optimization parameters are d 1 and d 5 , while the other three necessary parameters for curve construction (p 5 , ϕ 5 , and κ 5 ) are given by the heuristics discussed above and shown in Figure 5. In the other set of simulations, there are five optimization parameters, namely d 1 , d 5 , p 5 , ϕ 5 , and κ 5 . Certainly, both simulation scenarios also include cases with different N seg . The data obtained from the simulation experiments are compiled in Table 1, where t i represents the travel time at the end of a given corridor segment and ∑ 6 i=1 t i is the total travel time on a resulting path. Table 1. Resulting travel times on segments and total travel times. Simulations were performed for different numbers of optimization parameters N free ∈ {2, 5}, and different numbers of segments N seg ∈ {1, 2, 3}, whose geometry was taken into account when calculating the solution for the current segment.        As expected, the results in Table 1 show that the travel times decrease when either N seg or N free increases. The shortest travel time is calculated for the case where N seg = 3 and N free = 5.

Warehouse Environment
The enormous technological capabilities of automated guided vehicles (AGVs) and other autonomous mobile robots (AMRs) are facilitating the launch of fully automated warehouses. Common warehouse tasks performed by mobile robots include loading and unloading goods, stacking and retrieving items, picking and sorting orders, inventory tracking, and warehouse maintenance.
We tested the proposed trajectory planning algorithm in a simple warehouse environment by simulating the task of moving between three rows of storage racks, picking up and dropping off loads from specific locations (Figure 12). Warehouses are usually very confined environments, so we assumed that movement in the two aisles is restricted to a straight line. To avoid collisions of AGVs with storage racks, the straight segments on both sides protrude slightly beyond the edges (black solid dots in Figure 13). The optimization problem is to find the most suitable path shape between the aisles. The simulation experiment was designed as follows. An AGV travels clockwise along a circular route from the pick-up point (PUP) to the drop-off point (DOP) and back to the starting point. At the pick-up and drop-off points, the speed is set to zero. As the load is delicate, the dynamic constraints on the mobile system are more severe in the first part of the path, as shown in Table 2. Table 2. Constraints on velocity, acceleration, and jerk for a fully loaded ( ) and an unloaded (×) mobile system. We proposed that the continuous curvature path between a pick-up point and a dropoff point (and vice versa) consists of two straight lines and two 5th order Bézier curves. The coordinates of the control points were determined by an optimization process that minimizes travel time. Since the velocity is set to zero at the symmetrically placed drop-off point A , the optimizations can be performed only on one half of the circular route (thus on four segments instead of eight). The free optimization parameters for the construction of the Bézier curves were d 1 and d 5 (Section 5.1 for a full explanation) and the x coordinate of the joint between them, which is P 5 of the first Bézier curve and P 0 of the second Bézier curve.
First, we computed the minimum-time trajectories for symmetrically placed pair of pick-up and drop-off point (A and A ). Let us denote the path representing the full-load optimization solution for the symmetrically placed pair A and A by F . Similarly, let N be the path representing the no-load optimization solution for the symmetrically placed pair A and A (Figure 13). We then conducted a comparative travel time analysis. The main question was whether travel times differ in cases where a fully loaded/unloaded AGV travels along a path that is not optimized for a load of the same type. Normally, AGVs in warehouses travel along predefined trajectories. So with the simulation experiment, we wanted to test whether it is possible to reduce travel time if each curve segment is optimized for the actual load being carried. We also included examples with different ratios of travel times (or path lengths) of fully loaded or unloaded mobile systems by adding drop points B and C. Essentially, we calculated travel times for the three pick-up and drop-off pairs where the AGV was fully loaded on the first part (PUP→DOP) and unloaded on the second part (DOP→PUP) of the circular path, but traveling on either F or N . The travel times are given in Table 3, where the subscripts indicate the load type of the AGV. By µ, we denote the relative increase (in percent) in travel time in a given route case scenario compared to the travel time when the AGV travels on a path optimized for a load of the same type (the last three rows in Table 3).
The results in Table 3 show that the travel time is indeed the shortest when the mobile system travels along the route optimized for the actual load (PUP→DOP: F F , and DOP→PUP: N N ). Moreover, it can be seen that when the default path is F (rows 4-6 in Table 3), the corresponding travel times are always shorter than in the case when the default path is N (rows 1-3 in Table 3). However, generally, the travel times are not radically different and this observation is not entirely unexpected. We could achieve more obvious travel time differences if we increased the ratio of fully loaded to unloaded constraint values (see Table 2) or chose a more complex arrangement of pick-up and drop-off points spanning multiple rows of storage racks. Nevertheless, the selected values for velocity, acceleration, and jerk constraints (and the ratio between the two load types) should reflect reality. Additionally, the examples presented can be viewed as the smallest units for which this analysis can be performed. These subtle differences in travel times (approximately 1% reduction) imply significant absolute time differences when the presented trajectories are combined into larger trajectories. Or if one considers that a warehouse robot would traverse the same trajectories over and over again during its entire operation.  Figure 14 shows the velocity profiles for all three pairs of pick-up and drop-off points for the case where F F is on the first part and N N is on the second part of the circular route.

Conclusions
In this paper, we present a new minimum-time optimization-based approach for planning the trajectory of a mobile robot in a planar constrained environment. We assumed that a mobile system has constraints on velocity, acceleration, and jerk. The resulting smooth path consists of 5th order Bézier curves, for whose construction we propose a new method that allows efficient parameterization.
We analyzed the results of the proposed approach for generating trajectories in a simulated automated warehouse. Different sets of dynamic constraints led to different solutions for trajectories. We have shown that it is possible to achieve noticeable improvements in travel times by choosing the appropriate trajectories. The approach is applicable for trajectory and velocity planning of a single wheeled robot, but could be extended for the use of multiple robots to take into account evasive maneuvers or cooperation on a given task. It can also be used by various other mobile systems moving in a plane (e.g., track robots, robotic manipulators), especially non-holonomic systems.
Our findings may be especially useful and have great potential for determining minimum-time trajectories in automated warehouses, where the dynamic constraints imposed on autonomous mobile robots may depend on the type of load the mobile system is transporting. Our approach could also be applied to other planar environments with similar requirements.
The values of the constraints in the warehouse environment were conservatively estimated to ensure the vertical stability of a mobile system. However, the stability of the system (mobile robot with load) itself was not the subject of our research. Future studies should aim to describe the characteristics of the load in more detail, as this could impose additional or more demanding constraints on a mobile system. For a specific mobile system with known load characteristics (mass, mass distribution, dimensions, contact area conditions) it would be possible to calculate the tipping angle and consequently determine the allowable accelerations. The use of higher order Bézier curves or other curve primitives would also be of particular interest. More broadly, research is needed to apply the proposed trajectory planning approach to environments with static or dynamic obstacles to demonstrate the proposed idea using global or local path planning methods.