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

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\text{ m}\) in length, \(B=0.55\text{ m}\) in width and the wheel radii measure \(r=0.125\text{ m}\). Moreover, the off-center distance of each wheel equals \(a=0.11\text{ 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 \(\dot{\beta}_{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 \(\alpha_{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.

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}^{\circ}\) monitoring of the vehicle’s surroundings, two LiDAR laser scanners are positioned at opposite corners of the chassis. The entire setup communicates through a Powerlink bus system.

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 \(\pm\infty\) 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 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-wheel-steered 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, \(\mathbf{z}(t)=(\mathbf{r}^{T}(t)\ \theta(t)\ \boldsymbol{\alpha}^{T}(t)\ \boldsymbol{\beta}^{T}(t))^{T}\), which encompass the position \(\mathbf{r}=(x\ y)^{T}\) and orientation \(\theta\) of the chassis, as well as the steering angles \(\boldsymbol{\alpha}=(\alpha_{l}\ \alpha_{r})^{T}\) and wheel angles \(\boldsymbol{\beta}=(\beta_{l,R}\ \beta_{l,F}\ \beta_{r,R}\ \beta_{r,F})^{T}\). 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 \(\dot{\mathbf{s}}_{m}\), which can be mapped to the time derivative of the minimal coordinates, \(\dot{\mathbf{z}}=\mathbf{H}_{m}(\mathbf{q})\dot{\mathbf{s}}_{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 employed 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, \(\mathbf{q}=(x\ y\ \theta\ \varphi)^{T}\), and two minimal velocities, \({\dot{\mathbf{s}}=(v_{R}\ \omega_{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 \(\mathbf{g}_{1}(\mathbf{q})\), \(\mathbf{g}_{2}(\mathbf{q})\) in the state space \(\mathbb{R}^{4}\) will be relevant for trajectory planning in Sect. 3.

To control the HDP, given values for the bicycle model’s steering angle \(\varphi\) 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 using the abbreviations 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 \((\dot{\mathbf{s}}=\dot{\boldsymbol{\alpha}}, v_{R}=0, \omega_{F}=0)\) but is also evident, since (1) implies \(\dot{\alpha}_{i}=\dot{\alpha}_{i,F}=-\dot{\alpha}_{i,R}\) for \(i\in\{l,r\}\).

In order to move the HDP along planned trajectories \(\mathbf{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 \(\dot{\mathbf{s}}_{c}\), which are then used in the low-level control to compute the required feedback torque \(\mathbf{M}_{FB}\). Furthermore, a feed-forward controller incorporates the HDP’s dynamics yielding a feed-forward torque \(\mathbf{M}_{FF}\).

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 \(\mathbf{q}_{I}\) to a given final pose \(\mathbf{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 \(\mathbf{v}:=\ddot{\mathbf{s}}\) as the system’s new input, where \(\mathbf{I}\in\mathbb{R}^{2\times 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, \({\mathbf{x}_{k+1}=\mathbf{f}_{z}\left(\mathbf{q}_{k},\dot{\mathbf{s}}_{k},\mathbf{v}_{k}\right)}\). Furthermore, the OCP takes into account the physical limitations of the HDP, i.e. limits for the steering angles \(\boldsymbol{\alpha}_{\mathrm{min}}\leq\boldsymbol{\alpha}\leq\boldsymbol{\alpha}_{\mathrm{max}}\) and the angular wheel velocities \(\dot{\boldsymbol{\beta}}_{\mathrm{min}}\leq\dot{\boldsymbol{\beta}}\leq\dot{\boldsymbol{\beta}}_{\mathrm{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, \(\mathbf{q}_{\mathrm{min}}\leq\mathbf{q}\leq\mathbf{q}_{\mathrm{max}}\).

Concatenating the relevant minimal coordinates \(\bar{\mathbf{q}}=\left(\mathbf{q}_{1}^{T}\,\dots\,\mathbf{q}_{N}^{T}\right)^{T}\) as well as new inputs \(\bar{\mathbf{v}}=\left(\mathbf{v}_{0}^{T}\,\dots\,\mathbf{v}_{N-1}^{T}\right)^{T}\) at all sample times \(t_{k}=0,\,T/N,\,\dots,\,T\), the OCP can be denoted as The overall cost functional is composed of three terms \(J_{i}(\cdot)\) with respective weights \(w_{i}\), reading The first term, \(J_{T}(T)=\sum_{k=0}^{N}\Delta t_{k}=T\) with \(\Delta t_{k}:=t_{k+1}-t_{k}=T/N\), minimizes the overall time and the third one, \(J_{S}\left(\bar{\mathbf{v}}\right)=\sum_{k=0}^{N-1}\mathbf{v}_{k}^{T}\mathbf{v}_{k}\), ensures smooth input signals by minimizing the squared inputs \(\mathbf{v}\).

The second term, \(J_{A}\left(\cdot\right)=\sum_{k=1}^{N}l_{k}\left(\mathbf{q}_{k},{\mathbf{q}}_{F}\right)\) with determines how the setpoint \(\mathbf{q}_{F}\) is approached by projecting the deviations \(\Delta\mathbf{q}_{k}=\mathbf{q}_{k}-\mathbf{q}_{F}\) into the directions of the input vector fields \(\mathbf{g}_{1}\left(\mathbf{q}\right)\), \(\mathbf{g}_{2}\left(\mathbf{q}\right)\) and the subsequent Lie brackets Note that the system’s singularity at \(\varphi=\pm\pi/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 \(\left(\mathbf{q}_{N}=\mathbf{q}_{F}\right)\). 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 \(\mathbf{g}_{1}\left(\mathbf{q}\right),\dots\,,\mathbf{g}_{4}\left(\mathbf{q}\right)\) spanning the whole state space \(\mathbb{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}(\cdot)\) 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},\dots,w_{4}\) the respective behavior can be further adjusted.

The overall cost functional \(J(\cdot)\) 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 \(\mathbf{q}^{\ast}_{k}\) for the vehicle along with the required input signals \(\dot{\boldsymbol{\beta}}^{\ast}_{k}\) and \(\mathbf{v}_{k}^{\ast}\) for all sample times \(t_{k}^{\ast}=kT^{\ast}/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].

In this section, the proposed method for planning time optimal trajectories for a single vehicle will be adapted to a group of \(n\geq 2\) nonholonomic mobile platforms to cooperatively transport an object from an initial pose \(\hat{\mathbf{q}}_{I}\) to a desired final pose \(\hat{\mathbf{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 \(\mathbf{q}_{i}\) of all vehicles are concatenated, resulting in vectors denoted as \({\tilde{\mathbf{q}}=\left(\mathbf{q}_{1}^{T}\,\dots\,\mathbf{q}_{n}^{T}\right)^{T}}\) respectively. The individual discretized kinematic models of the robots are combined into one system of equations, \({\tilde{\mathbf{x}}_{k+1}=\tilde{\mathbf{f}}_{z}\left(\tilde{\mathbf{q}}_{k},\tilde{\dot{\mathbf{s}}}_{k},\tilde{\mathbf{v}}_{k}\right)}\). Furthermore, all relevant parameters that describe the payload’s geometry, particularly its mounting points, \(\mathbf{r}_{i}\) for \({i=1,\dots,n}\), are concatenated into the formation parameters \(\mathbf{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 \({\hat{\mathbf{q}}=(\hat{x}\ \hat{y}\ \hat{\theta})^{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, \(\hat{\mathbf{q}}=\hat{\mathbf{q}}\left(\tilde{\mathbf{q}}\right)\). 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 \(\vartheta_{ij}\) be the angle relative to the \({}_{I}\mathbf{y}\)-axis of the virtual line connecting the mounting points \(\mathbf{r}_{i}\) and \(\mathbf{r}_{j}\), By averaging the change of these angles compared to a reference configuration \(R\), where \(\hat{\theta}\left(\tilde{\mathbf{q}}_{R}\right):=0\), for all possible connecting lines, the estimation for the virtual leader’s orientation is obtained, Here, \(\mathcal{S}=\{(i,j)\in\{1,\dots,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, \(\mathbf{r}_{i}=\mathbf{r}_{i,d}\left(\tilde{\mathbf{q}},\mathbf{p}\right)\). 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 \(\boldsymbol{\epsilon}_{\mathbf{r}}> \mathbf{0}\) around their desired values, \(-\boldsymbol{\epsilon}_{\mathbf{r}}\leq\mathbf{r}_{i}-\mathbf{r}_{i,d}(\cdot)\leq\boldsymbol{\epsilon}_{\mathbf{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 (\(\sim\)) notation. The initial and final conditions for each robot are derived directly from the payload’s initial \(\hat{\mathbf{q}}_{I}\) and final pose \(\hat{\mathbf{q}}_{F}\) using (26), with \(\mathcal{P}\in\{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, is responsible for how the vehicles approach their setpoints, \(\mathbf{q}_{i,F}\) and is computed by applying the respective cost functional for a single vehicle, \(l_{k}(\cdot)\), 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 \(\mathbf{e}_{\mathbf{r},k}\left(\tilde{\mathbf{q}}_{k},\mathbf{p}\right)\) and can be expressed as with \({\mathbf{e}_{\mathbf{r},k}\left(\tilde{\mathbf{q}}_{k},\mathbf{p}\right)=\mathbf{r}_{i,k}-\mathbf{r}_{i,d}(\tilde{\mathbf{q}}_{k},\mathbf{p})}\) using (27). Again, the overall cost functional \(J(\cdot)\) 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.

In addition to the general case of a formation with \(n\geq 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, \(\theta_{1}=\theta_{2}=\hat{\theta}\) (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 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 \({-{\epsilon}_{\theta}\leq\theta_{i}-\hat{\theta}(\tilde{\mathbf{q}})\leq{\epsilon}_{\theta}}\), with the error limit \({\epsilon}_{\theta}> {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}_{\theta,k}(\cdot)\), resulting in with \({e}_{\theta,k}\left(\tilde{\mathbf{q}}_{k}\right)=\theta_{i,k}-\hat{\theta}(\tilde{\mathbf{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