Time-optimal trajectory planning for a rigid formation of nonholonomic mobile platforms

This paper presents a method for planning time-optimal trajectories for a formation of multiple nonholonomic (heavy duty) platforms (HDPs) to cooperatively transport an object to a specified pose. The first part addresses the mobile platforms themselves while the second part provides a trajectory planning approach derived from the well-known virtual leader approach. In order to ensure proper transport of the shared payload, the vehicles are modeled individually, resulting in a formation control problem. The goal of the optimization process is to minimize a cost function that balances time optimality, smooth control signals, and formation rigidity. The optimal control problem (OCP) takes into account the kinematics of the vehicles as well as their physical limitations. It is solved by using a multiple shooting method, which yields the desired trajectories for all vehicles while ensuring smooth control signals. The paper includes optimization results for several scenarios involving two and three HDPs together with various target poses, demonstrating the effectiveness of the proposed method.


Introduction
Autonomous ground vehicles are crucial for automating logistics systems.Cooperative transport involving multiple agents can reduce the payload of individual mobile platforms while enhancing the maneuverability of the objects being transported.Formation control, initially explored in UAV applications, is now gaining attention in AGV applications.A comprehensive survey on the fundamental problem and 578 Time-optimal trajectory planning for a rigid formation of nonholonomic mobile platforms K Fig. 1 Photo of the heavy duty platform solution approaches can be found in [1].The primary challenge lies in achieving virtual rigidity among the fleet of mobile robots, which is a key aspect of any formation control scheme.Two types of formations must be distinguished: fixed topology with switching geometry [2] and switching topology formations [3,4].Additionally, the rigidity property, particularly for 6D-agents like UAVs, needs to be investigated [5].Formation control schemes assume that agents can execute any desired motion, but non-holonomic mobile robots have kinematic constraints that limit their movement compared to omnidirectional platforms.This paper addresses the underlying path planning problem, specifically using special autonomous ground vehicles as mobile agents (see Fig. 1).The considered nonholonomic vehicle possesses unique kinematics, enabling car-like steered longitudinal and lateral motion, pure rotation, and wheel alignment even while stationary.
There are various approaches to cooperative transport, one of which is the virtual leader method.In [6], this method is applied to develop a distributed model predictive controller (DMPC) for a formation of differential-driven vehicles.Additionally, an adaptation for a car-like vehicle is proposed as the basis for the optimal control problem (OCP).A similar DMPC for omnidirectional vehicles is discussed in [7], which also employs the virtual leader approach and compares it with an algebraic graph theory method.Both approaches enable tracking of predefined setpoints or trajectories for the formation but do not offer a trajectory planning method considering the formation's kinematics.On the other hand, [8] utilizes the virtual leader approach to design a trajectory tracking controller with collision avoidance for a formation of robots equipped with differential drives.The advantage of treating the vehicles individually and formulating the issue as a formation control problem is the flexibility to accommodate any number of vehicles, regardless of their kinematics.This paper is organized as follows.At first, a brief overview of the HDP's hardware setup and kinematics along with a control strategy is given in Sect. 2. In Sect.3, a method for planning time optimal trajectories for a single vehicle is proposed, which is then extended to a formation of multiple mobile platforms cooperatively transporting an object.Additionally, a special case for two vehicles is considered.Sect. 4 presents optimization results for the cases of two and three vehicles.The paper closes with a summary and an outlook for the realization of the planned trajectories on the HDPs in Sect. 5. Note that this paper is an extension of [9].
Notation Since this paper focuses solely on planar problems, positional vectors are denoted as T , where K refers to the corresponding coordinate frame.For positional vectors expressed in the initial frame of reference, K = I , the frame indication is neglected, r = I r = (x r y r ) T .Regarding orientations, the chosen frame of reference is irrelevant and is therefore also neglected.

Hardware
The HDP (Fig. 1) is a vehicle equipped with four wheels positioned at the corners of the chassis, each being driven by its own electric motor.It is designed to handle payloads weighing up to 1.5 tons.The HDP's chassis has dimensions of L = 1.18 m in length, B = 0.55 m in width and the wheel radii measure r = 0.125 m.Moreover, the off-center distance of each wheel equals a = 0.11 m.To differentiate between the front and rear parts of the vehicle, as well as the left and right sides (Fig. 2), the angular velocities of the wheels are denoted as βi,j , where i can take the values of either l or r to represent the left or right side, and j can be either F or R to represent the front or rear.The motor-wheel units are mounted in such a way that they can rotate around an off-center point, with the corresponding steering angles being denoted as α i , j .Due to mechanical coupling, the front and rear wheels on each side i of the vehicle can only pivot in opposite directions, Therefore, the vehicle is steered passively by appropriately driving the wheels.To enable proper control of the robot's motion, both the angular velocity and the steering angle of each wheel are measured.The HDP is also equipped with two additional motors located at the front and back, allowing it to lift the payload up to 0.3 m.However, in the considered two-dimensional scenario, this lifting functionality does not contribute to finding feasible trajectories for the vehicle and is thus not considered further.

K
Time-optimal trajectory planning for a rigid formation of nonholonomic mobile platforms 579 For the fundamental control of the system, hardware provided by B&R Industrial Automation is utilized.This includes a X20 PLC (Programmable Logic Controller) along with corresponding input and output modules.Two ACOPOS servo amplifiers are employed to drive three motors each.For comprehensive 360 • monitoring of the vehicle's surroundings, two Li-DAR laser scanners are positioned at opposite corners of the chassis.The entire setup communicates through a Powerlink bus system.

Kinematics
To enable a wheeled vehicle to move without wheel slippage, it is necessary for the lateral directions of all wheels to intersect at a single point known as the Instantaneous Center of Rotation (ICR).The motion of the robot can then be interpreted as a rotation around this particular point.The mechanical coupling of the wheels restricts the ICR to the body fixed R y axis as well as to ±∞ on the R x axis.Therefore, it is possible to distinguish between four different driving modes, illustrated in Fig. 3.In the Steering mode, the lateral directions of the wheels on the left and right sides do not intersect at a single point.Consequently, it is not possible to move the vehicle in this mode without wheel slippage, but it is still essential for transitioning between the other modes.In Fig. 3 this is illustrated by two ICRs for the left and right wheels.In contrast to the first mode, the remaining three modes enable the vehicle to drive in different ways.The Fig. 3 The ICR for the four driving modes of the HDP second mode, referred to as Sideways, permits lateral movement of the vehicle, resulting in a trivial linear motion along a straight line.When driving sideways, the ICR is positioned at the vehicle's longitudinal axis at infinity.In the third mode, the HDP can rotate around the geometric center of the wheels' mounting points, also leading to trivial motion.The final mode allows the vehicle to behave like an all-wheelsteered car, enabling nonlinear motion and facilitating tracking of more complex trajectories.This wheel alignment configuration is commonly known as Ackermann Kinematics.For this mode, the ICR can move along the robot's body fixed R y axis and is only restricted by the physical steering angle limits.Due to these four driving modes, the HDP can be categorized as quasi-omnidirectional, as it needs to rotate before it can move in any desired direction.This paper focuses solely on the Ackermann mode, as the primary emphasis is not on planning trajectories with automated mode switching.
The kinematics of the HDP can be fully described using a set of minimal coordinates, z(t) = (r T (t) θ(t) α T (t) β T (t)) T , which encompass the position r = (x y) T and orientation θ of the chassis, as well as the steering angles α = (α l α r ) T and wheel angles For brevity, time dependencies of the states, inputs and all variables depending on those are not further denoted.Each mode m is associated with a distinct set of minimal velocities ṡm , which can be mapped to the time derivative of the minimal coordinates, ż = H m (q)ṡ m .The number of minimal velocities for each mode is determined by the number of nonholonomic constraints, which impose restrictions solely on the vehicle's velocity, not its position.
The kinematics of the Ackermann mode can be represented by the bicycle model, featuring a fixed wheel at the center of the chassis and a steering wheel located between the HDP's two front wheels, as depicted in Fig. 2. Hence, this simplified model can be em-ployed in the design of an appropriate path tracking and feed-forward controller for the HDP, as well as in the planning of suitable trajectories.The chosen model, representing the bicycle's kinematics, involves four minimal coordinates, q = (x y θ ϕ) T , and two minimal velocities, ṡ = (v R ω F ) T .For simplicity, the parameter l = L/2 is introduced.Note that the interpretation of the kinematic model as a projection of the minimal velocities into the directions of the input vector fields g 1 (q), g 2 (q) in the state space R 4 will be relevant for trajectory planning in Sect.3.
To control the HDP, given values for the bicycle model's steering angle ϕ and minimal velocities need to be converted to the necessary steering angles and angular wheel velocities for the HDP.The conversion of the steering angles is derived using the ICR condition, The angular wheel velocities, corresponding to the input for the HDP's kinematic model, can be expressed by Moreover, to ensure proper steering of the vehicle, the correlation between the angular wheel and angular steering velocities, is required.This expression can be derived considering the HDP's steering configuration (ṡ = α, v R = 0,ω F = 0) but is also evident, since (1) implies αi = αi,F = − αi,R for i ∈ {l , r }.

Control of a single platform
In order to move the HDP along planned trajectories q d (t), a suitable control strategy consisting of three components is implemented.A path tracking controller ensures that the vehicle follows the given path by providing desired minimal velocities ṡc , which are then used in the low-level control to compute the required feedback torque M F B .Furthermore, a feedforward controller incorporates the HDP's dynamics yielding a feed-forward torque M F F .

Path tracking control
For path tracking control, there are several options available.In [10] a flatness based control concept is proposed, originally designed for the kinematic model of a car but applicable to the bicycle model and thus the HDP.This controller requires knowledge about the desired trajectory up to its second derivative.Another option is the Stanley controller [11], which only requires the desired trajectory up to the first derivative.It's important to note that the Stanley controller is primarily designed for forward driving and would need slight modifications to handle reverse driving, as the generated paths may require it.Furthermore, a (D)MPC could be implemented for each individual vehicle, following the approach outlined in [6].This controller only requires the desired trajectory without any derivatives.However, implementing DMPC would necessitate communication between the robots, allowing them to consider other agents and respond effectively to potential disturbances.It should be emphasized that implementing such communication is a non-trivial task.Therefore, this control concept can be considered for future development, given the similarity between the optimization problem it is based on and the one used for trajectory planning in Sect.3. The chosen path tracking controller introduced in [12] computes the desired minimal velocities based on the vehicle's positional and orientational error with the former being expressed in the trajectory's current coordinate frame, ), (6) using the rotation matrix K Time-optimal trajectory planning for a rigid formation of nonholonomic mobile platforms 581 The error in the orientation, on the other hand, is independent of the coordinate frame, and reads e θ = θ d − θ.Using these error definitions, the path tracking control law (8) results in an asymptotically stable closed loop behavior for controller parameters k x , k y , k θ > 0. This statement can be proven via Lyapunov's indirect method along with the respective Lyapunov's function for |e θ | < π/2.Note that this concept also requires solely the desired trajectory, without any derivatives.
The computed minimal velocities leading to the vehicle tracking its desired path are consequently handled in the HDP's low level control generating the required feedback motor torque.

Low-level control
The HDP's steering angles and angular wheel velocities are governed by respective cascaded controllers.Initially, the desired angular steering velocity ω F,c , provided by the path tracking controller, is integrated to obtain the desired steering angle ϕ d .This steering angle is then transformed to the HDP's desired steering angles α l /r,d by making use of (3).Thus, the steering error e α := α d − α can be introduced.In order to achieve asymptotically stable steering control, it is essential that the related error dynamics take the form of ėα != −K P,α e α ⇔ ėα + K P,α e α = 0, (10) with K P,α > 0. Typically, while driving in the Ackermann mode, changes in the steering angle occur at a slow rate, allowing for the assumption αd ≈ 0. Therefore, the control law is sufficient to ensure asymptotically stable steering error dynamics.
Next, the desired angular wheel velocities are computed by combining (4) and ( 5), To maintain these desired angular velocities, a Proportional-Integral (PI) controller is implemented, which yields the feedback torque, with The proposed low-level control scheme for the HDP is illustrated in Fig. 4, taking the desired minimal velocities from the path tracking controller, as well as the current steering angles and angular velocities, and providing in the feedback torque.

Feed-forward control
The feed-forward control considers the HDP's dynamics and generates a feed-forward torque based on the desired trajectory.In the case of an ideal and undisturbed vehicle, this torque would be sufficient to keep it on its desired path.Consequently, the feedback control just needs to compensate deviations due to the real vehicles imperfections as well as potential disturbances.Note that, especially when transporting heavy payloads, feed-forward control is crucial to ensure sufficient path tracking and in this particular case the formation's rigidity.The HDP's equations of motion are transformed to fit the bicycle model, resulting in (14) with the mass matrix M(ϕ) ∈ R 2×2 , the input matrix B(ϕ) ∈ R 2×4 and the vector g ϕ, ṡ ∈ R 2 summarizing the remaining terms related to friction as well as centripetal and coriolis effects.The feed-forward torque is then derived by minimizing the squared motor torques with the vehicle's dynamics being considered as an equality constraint by using a Lagrange parameter λ, This minimization problem can then be solved using the right pseudo inverse Note that the feed-forward control requires the desired minimal velocities up to the first derivative, sd .
For further details regarding the vehicle's dynamics and the feed-forward control, see [13].

Overall control scheme
The comprehensive control scheme for the HDP consisting of the three discussed parts is depicted in Fig. 5.
The odometry is responsible for estimating the vehicles current state by utilizing sensor data, i.e. measurements of the steering angle and angular wheel velocities.While a detailed explanation of the odometry module is beyond the scope of this work, further information can be found in [13].
3 Time optimal formation trajectory planning

The bicycle model
Before addressing the problem of trajectory planning for a formation of multiple vehicles, a method for planning a path for a single vehicle, as depicted in Fig. 6, is proposed.The time optimal path, taking the mobile platform from an initial pose q I to a given final pose q F , will result from solving a time discrete OCP.Since the Feed-forward control requires the time derivative of the minimal velocities, the bicycle's kinematic model ( 2) is extended by introducing v := s as the system's new input, where I ∈ R 2×2 denotes the identity matrix.To incorporate this kinematic model into the discretized OCP, a fourth-order Runge-Kutta scheme (RK4) is employed to discretize the system, x k+1 = f z q k , ṡk , v k .Furthermore, the OCP takes into account the physical limitations of the HDP, i.e. limits for the steering angles α min ≤ α ≤ α max and the angular wheel velocities βmin ≤ β ≤ βmax .The constraints on these variables are incorporated into the OCP using ( 3) and ( 4).Without loss of generality, constraints are also imposed on the remaining states of the model, allowing for additional restrictions on the position and orientation of the vehicle, q min ≤ q ≤ q max .
Concatenating the relevant minimal coordinates q = q T 1 ... q T N T as well as new inputs T at all sample times t k = 0, T /N , ..., T , the OCP can be denoted as min J T, q, v, q F s.t.
The overall cost functional is composed of three terms J i (•) with respective weights w i , reading The first term, J T (T ) = N k=0 Δt k = T with Δt k := t k+1 − t k = T /N , minimizes the overall time and the third one,

ensures smooth input signals by minimizing the squared inputs v.
The second term, determines how the setpoint q F is approached by projecting the deviations Δq k = q k − q F into the directions of the input vector fields g 1 q , g 2 q and the subsequent Lie brackets K Time-optimal trajectory planning for a rigid formation of nonholonomic mobile platforms 583 Originalarbeit Note that the system's singularity at ϕ = ±π/2 is also present in these Lie-Brackets.This part of the cost functional plays a crucial role in the OCP for the MPC presented in [6].In that case, it is essential for finding feasible trajectories without terminal deviations for a (more or less) arbitrary setpoint, due to the absence of terminal conditions.In contrast, when seeking feasible solutions of (18) this term is not mandatory, because of the here present terminal condition q N = q F .Nevertheless, the term is included in the overall cost functional, since it can be beneficial for finding more direct paths to the desired setpoint, although not necessarily optimal in terms of time.Furthermore, the OCP can be easily adapted to fit the requirements of an MPC approach.
From an illustrative point of view, the input vector fields correspond to the instantaneously controllable directions of the vehicle in the configuration space, i.e. driving straight and turning the front wheel.The results from the Lie brackets on the other hand correspond to the not instantaneously controllable directions, i.e. turning the whole vehicle and moving it sideways.Note that these are the two only linear independent Lie brackets of the input vector fields and subsequent Lie brackets, with g 1 q ,... ,g 4 q spanning the whole state space R 4 , since the vehicle is controllable.Due to the choice for the exponents k 1 = k 2 = 12, k 3 = 6, and k 4 = 4, as proposed in [6], deviations in the not instantaneously controllable directions of the vehicle near the setpoint are penalized more than deviations in the remaining directions, with the ones in the lateral direction being penalized the most.In the case of a MPC, this specific choice for J A (•) and its exponents ultimately results in the vehicle approaching the desired setpoint along its R x direction.For this use case, it enables a more direct setpoint approach, eliminating the need for extra distancing and reducing the overall space required.With the coefficients w 1 ,..., w 4 the respective behavior can be further adjusted.
The overall cost functional J (•) balances three factors: achieving a time-optimal trajectory, ensuring smooth input signals, and determining how the setpoint is approached.Thus, the coefficients w T , w A , and w S provide the flexibility to finely adjust the optimization result.Solving the proposed OCP yields a feasible, time optimal trajectory q * k for the vehicle along with the required input signals β * k and v * k for all sample times t * k = kT * /N .It is important to note, that the resulting trajectory is time optimal only in terms of the kinematic constraints, since the vehicle dynamics have not been considered in the OCP.Moreover, the time optimal part of the cost functional is balanced with the two other terms, which is another reason for why the trajectories are not purely time optimal.
To solve the OCP, the direct Multiple Shooting method [14] is employed with the N discretization intervals as shooting intervals.This method is implemented within the CASADI framework [15], and solved using the Interior Point method with the integrated IPOpt solver [16].

Formation trajectory planning for multiple vehicles
In this section, the proposed method for planning time optimal trajectories for a single vehicle will be adapted to a group of n ≥ 2 nonholonomic mobile platforms to cooperatively transport an object from an initial pose q I to a desired final pose q F .Extending the OCP (18) to multiple robots is straightforward.However, in order to ensure proper payload transport, the vehicles must also maintain their relative distances while following the resulting trajectories.This requirement needs to be mathematically formulated and incorporated into the OCP.
To simplify notation, the states q i of all vehicles are concatenated, resulting in vectors denoted as q = q T 1 ... q T n T respectively.The individual discretized kinematic models of the robots are combined into one system of equations, x k+1 = f z q k , ṡk , v k .Furthermore, all relevant parameters that describe the payload's geometry, particularly its mounting points, r i for i = 1,...,n, are concatenated into the formation parameters p.The formation is described by its geometric center as well as the minimal coordinates of all mobile platforms.The formation center serves as a virtual leader and is represented by its pose q = ( x y θ) T including its position and an assigned orientation.In Fig. 7, a formation consisting of three vehicles along with the virtual leader is depicted as an example.The leader's configuration can be estimated based solely on the other vehicles' positions, q = q q .The location of the virtual leader is given by For estimating the leader's orientation, we consider the orientations of the virtual connection lines of the payload's mounting points.Without loss of generality, let ϑ i j be the angle relative to the I y-axis of the virtual line connecting the mounting points r i and r j , 584 Time-optimal trajectory planning for a rigid formation of nonholonomic mobile platforms K Originalarbeit Fig. 7 Formation of n = 3 vehicles and its representation by a virtual leader By averaging the change of these angles compared to a reference configuration R, where θ q R := 0, for all possible connecting lines, the estimation for the virtual leader's orientation is obtained, Here, S = {(i , j ) ∈ {1,...,n} 2 : j > i } represents the set of all possible connection lines, with each line included exactly once.Following that, the desired position of each robot can be expressed either with respect to the virtual leader, or directly through the vehicles' minimal coordinates and the formation parameters using ( 23) and (25), In order to maintain the formation exactly, the positions of all robots have to coincide with their respective desired position, r i = r i ,d q, p .Including this rigidity condition in the extended OCP for all n mobile platforms as an additional equality constraint at every sample time t k would ensure admissible relative trajectories of the robots.However, it would also lead to a problem that is hard to solve numerically.
To address this issue, considering that the robots will track the resulting trajectories using the proposed tracking controller and therefore will have small tracking errors, the strict requirement of exact formation maintenance can be relaxed.Instead, the constraint can be modified to allow the vehicles' positions to be within a certain margin of error r > 0 around their desired values, − r ≤ r i − r i ,d (•) ≤ r .This modification significantly improves the solvability of the OCP and reduces computation time, making it already sufficient for finding feasible trajectories for the given task.However, further improvement in solvability and computation time can be achieved by minimizing the margins between the desired and actual positions of the vehicles, in addition to just constraining them.
Finally, the general OCP for planning time optimal trajectories for multiple vehicles reads In this formulation, various variables, including vehicle states, inputs, position vectors, and error limits, are concatenated using the tilde (∼) notation.The initial and final conditions for each robot are derived directly from the payload's initial q I and final pose q F using (26), with P ∈ {I , F }. Note that all robots are required to have the same orientation as the leader at the start and end of the trajectory to ensure that the formation can continue moving seamlessly after completing the computed trajectory.The OCP incorporates state and input restrictions for all agents, extending the analogous constraints from (18) to encompass all vehicles, while also incorporating the new formation constraints.
The overall cost functional, is now composed of four terms that account for different aspects of the optimization problem, in contrast to the three aspects from the single vehicle from Sect.3.1.The first term, J T (T ) = T , leads to time optimality and remains the same for both single and multiple vehicle OCPs.The second term, K Time-optimal trajectory planning for a rigid formation of nonholonomic mobile platforms 585 is responsible for how the vehicles approach their setpoints, q i ,F and is computed by applying the respective cost functional for a single vehicle, l k (•), to all agents and summing up the results.The third term ensures smooth input signals and is obtained in the same way by simply extending the cost functional for one robot to all agents, The forth term in the cost functional minimizes the formation error e r,k q k , p and can be expressed as with e r,k q k , p = r i ,k − r i ,d ( q k , p) using ( 27).Again, the overall cost functional J (•) is a trade off between now four aspects, being time optimality, smooth input signals, the way the setpoint is approached and additionally the magnitude of formation errors.

Trajectory planning for 2 vehicles with equal orientation
In addition to the general case of a formation with n ≥ 2 robots, we want to consider a special case with n = 2 mobile platforms maintaining the same orientation as the payload throughout the entire transport, θ 1 = θ 2 = θ (see Fig. 8).Note that for the case n > 2, the vehicles must also be aligned laterally in order to enable the formation to move along a trajectory.Otherwise the vehicles' movements would be restricted significantly, allowing only for linear, longitudinal motion.This setup simplifies the mounting of the object Fig. 8 Special case with n = 2 vehicles on the vehicles, as no rotatable connections are necessary.
The additional requirement is included in the OCP in two ways.First, all vehicle orientations are constrained, similar to their restricted positions, by enforcing − θ ≤ θ i − θ( q) ≤ θ , with the error limit θ > 0. Second, the formation part of the cost functional is extended by adding a second term that minimizes the robots' orientation errors at each time step, e θ,k (•), resulting in with e θ,k q k = θ i ,k − θ( q k ).The leader's orientation is computed using (25).Hence, the resulting OCP for the case of two vehicles transporting an object while maintaining the same orientation can be written as

Optimization results
In this section the optimization results for a general case (n = 3) as well as the special case (n = 2, Sect.3.3) will be presented.The distance between all of the payload's mounting points is set as L P = 1 m, in both scenarios.For the case of n = 3 the object resembles an equilateral triangle.The HDP's steering angles are limited to |α l /r | 0.6 ⇔ |ϕ| ≤ π/4 and the limits for the angular wheel velocities are | βl/r,R/F | ≤ 2 1/s.The desired bounds for formation errors are chosen as x = y = 10 −3 m for both cases and for the special case, the additional orientation bound is set to θ = 10 −3 rad.
As initial pose of the payload, q I = 0 is chosen.For the setpoint approach part of the cost functional, J A (•), the chosen weights are w 1 = 1, w 2 = 0.1, w 3 = 5 and w 4 = 50.Furthermore, the weights for the overall cost functional read w T = 10 4 , w A = 1/N , w F = 2•10 9 /N and w S = 10 9 /N , where N = 500 is the number of control intervals chosen for optimization.
The desired final pose for the general case (n = 3) is q F,1 = (L P L P π/2) T .The set of all possible mounting point connection lines reads S = {(1,2), (1,3), (2,3)}.Figure 9 shows the trajectories for all three vehicles as well as the payload's geometric center together with the longitudinal velocity v R and the steering angle ϕ of the representative bicycle models.Note that the color for all vehicles and the payload is consistent across all the plots.The optimal time is T * 1 = 29.61s and the optimization took about a minute on average.Figure 10 depicts the robots' inputs being the angular wheel velocities.Due to time optimality, at least one of these overall 12 input values is at its limit throughout most of the trajectory.Only towards the end, when approaching the setpoint, none of the input values reach their limits because of the setpoint approach part in the overall cost functional.For w A = 0, one of the angular wheel velocities would be at its limit at all times, but the formation would require more space during the maneuver.Moreover, Fig. 11 illustrates the formation errors e r (•) = (e x e y ) T , which always remain within their designated boundaries.Obviously, the chosen error limits could be reduced to a certain degree, which would result in more exact trajectories for the vehicles but for the cost of longer optimization times.
11 Formation related errors of the first scenario For the special case (n = 2, Sect.3.3), only the resulting trajectories are shown as the inputs and errors exhibit similar behavior to the general case.In this scenario, there is only one connecting line for the two robots' mounting points, S = {(1,2)}.Figure 12 shows trajectories for the desired endpoint q F,2 = (−L P −L P − π/2) T , comparing the results with and without the approach part in the cost functional.Consequently, the optimal time for the scenario with the approach part (w A = 0, T * 2,1 = 30.21s) is smaller than the one without it (w A = 0, T * 2,2 = 39.79 s).As already discussed, the trajectory with the approach part poses a more direct approach.Figure 13 presents the resulting trajectory for a parallel park scenario for the case of n = 2.The chosen endpoint reads q F,3 = (0 L P 0) T with an optimal time of T * 3 = 38.67 s.

K
Time-optimal trajectory planning for a rigid formation of nonholonomic mobile platforms 587

Summary and outlook
This paper proposes a method for generating time optimal trajectories for multiple HDPs in order to transport a shared payload from a initial to a given final pose.The next step involves implementing and testing these trajectories on physical robots.However, validating the absolute formation errors solely through the vehicle's odometry is insufficient due to its inaccuracies and limited ability, estimating only the position of a single robot relative to its initial configuration.Hence, an external measuring system, e.g. a laser tracker or motion capture system, has to be employed.Furthermore, a proper way of mounting the payload on the vehicles needs to be developed, allowing for minor displacement of the object relative to the robots mitigating non-rigidity of the formation.Future research will also focus on utilizing the remaining, unused drive modes of the HDP in addition to the Ackermann steering.Further, considerations such as collision avoidance and the payload's weight distribution can be incorporated into the optimization process.

Fig. 4
Fig. 4 Low-level control for the HDP

Fig. 5
Fig. 5 Control scheme for the HDP

Fig. 6
Fig. 6 Kinematics of the bicycle model

Fig. 9 Fig. 10
Fig. 9 Trajectories of the first scenario