Optimization-based motion planning for multi-steered articulated vehicles
The task of maneuvering a multi-steered articulated vehicle in confined environments is difficult even for experienced drivers. In this work, we present an optimization-based trajectory planner targeting low-speed maneuvers in unstructured environments for multi-steered N-trailer vehicles, which are comprised of a car-like tractor and an arbitrary number of interconnected trailers with fixed or steerable wheels. The proposed trajectory planning framework is divided into two steps, where a lattice-based trajectory planner is used in a first step to compute a resolution optimal solution to a discretized version of the trajectory planning problem. The output from the lattice planner is then used in a second step to initialize an optimal control problem solver, which enables the framework to compute locally optimal trajectories that start at the vehicle’s initial state and reaches the goal state exactly. The performance of the proposed optimization-based trajectory planner is evaluated in a set of practically relevant scenarios for a multi-steered 3-trailer vehicle with a car-like tractor where the last trailer is steerable.
In recent years, there has been a growing demand within the transportation sector to increase efficiency and reduce environmental impact related to transportation of both people and goods. This has lead to an increased interest for large capacity (multi-) articulated buses  and long tractor-trailer vehicle combinations . In order to improve these long vehicles ability to maneuver in confined environments, some trailers (or wagons) can be equipped with steerable wheels. In the literature, these vehicles are commonly referred to as multi-steered N-trailer (MSNT) vehicles , which is a generalization of single-steered N-trailer (SSNT) vehicles. Compared to a SSNT vehicle, the additional steering capability enables an MSNT vehicle to be more flexible and agile, on the expense of increased difficulty in manually maneuvering the vehicle by a human driver. These difficulties partially arise due to the vehicle’s increased degrees of freedom which are hard to successfully cope with for a human operator, and because of the specific kinematic and dynamic properties of an MSNT vehicle (see, e.g., [21, 15, 18, 9]). To aid the driver, several advanced driver-assistance system concepts have been proposed to automatically steer the extra steerable wheel(s) in order to increase low-speed maneuverability or to reduce the so called off-tracking during tight cornering [17, 22, 15, 23].
Although a large amount of different motion planning techniques has been proposed in the literature for SSNT vehicles (see, e.g., [20, 11, 8, 13, 14]), there only exists a limited amount of work that consider the trajectory planning problem for special classes of MSNT vehicles (see e.g. [6, 21, 24, 5, 26]). As a consequence, there is still a need to develop a trajectory planner that is able to solve the trajectory planning problem for a generic MSNT vehicle with car-like tractor that: i) can handle various state and input constraints, ii) allows a mixture of on-axle/off-axle hitched and steerable/non-steerable trailers, and iii) computes locally optimal trajectories by combining forward and backward motion.
The contribution of this work is a trajectory planning framework for an MSNT vehicle with car-like tractor targeting low-speed maneuvers in confined and unstructured environments. The framework extends some techniques presented in our previous work in  and is inspired by [3, 4, 2]. Here, a lattice-based trajectory planner is developed and used in a first step to compute a resolution optimal solution to a discretized version of the trajectory planning problem. The lattice planner uses a finite library of precomputed optimized maneuvers restricted to move the MSNT vehicle within a specified state-space discretization. In a second step, the output from the lattice planner is used to initialize a homotopy-based optimization step enabling the framework to compute locally optimal trajectories that starts at the vehicle’s initial state and reaches desired goal states exactly. To the best of the authors knowledge, this paper presents the first trajectory planning framework for a generic MSNT vehicle with car-like tractor.
The reminder of the paper is organized as follows. In Section 2, the kinematic vehicle model and the trajectory planning problem for the considered MSNT vehicle are presented, as well as an overview of the proposed trajectory planning framework. In Section 3 and Section 4, the lattice-based trajectory planner and the homotopy-based optimization step are presented, respectively. Simulation results for an MS3T vehicle with car-like tractor are presented in Section 5 and the paper is concluded in Section 6 by summarizing the contributions and discussing the directions for future work.
2 Kinematic vehicle model and problem formulation
The MSNT vehicle with car-like tractor considered in this work is illustrated in Figure 1. The multi-body vehicle is composed of vehicle segments, including a car-like tractor and number of trailers that are equipped with steerable or non-steerable wheels. Each vehicle segment is characterized by a segment length and a signed hitching offset . Since low-speed maneuvers are considered in this work, a kinematic vehicle model is used. The model is based on the work in  and is derived based on various assumptions such as the wheels are rolling without slipping and that the vehicle is operating on a flat surface. Moreover, it is assumed that the front wheel of the tractor is steerable and its rear wheel is non-steerable. The vehicle configuration consists of variables  where denotes the number of steerable trailers:
the steering angle of the tractor’s front wheel
the global position and orientation of the th trailer in a fixed coordinate frame
for a number of constrained joint angles
and number of steering angles associated with steerable trailer wheels
where index specifies which trailers that have steerable wheels. The configuration vector for the MSNT with car-like tractor will be defined as
where represents a vector of trailer steering angles, and .
The leading car-like tractor is described by a kinematic single-track vehicle model and its orientation evolves as
where is the curvature of the tractor and is the longitudinal velocity of its rear axle. The recursive formula for the transformation of the angular and longitudinal velocities between any two neighboring vehicle segments are given by [15, 18]:
where denotes the steering angle of the th trailer. For the car-like tractor, we have that since its rear axle is non-steerable. Note that if the th trailer is non-steerable, it suffices to take in (7).
Each trailer steering angle , and the tractors steering angle are modeled as double integrator systems
where , , and , , denote steering angle rates and accelerations, respectively. This modeling is used to be able to penalize large rates and accelerations, and to enforce constraints in the form
where the steering angle accelerations and , are treated as control signals. Similarly, the longitudinal velocity of the tractor is constrained as and its dynamics is modeled as a double integrator system
in order to respect constraints on the longitudinal acceleration and jerk . During the planning phase, the longitudinal jerk is treated as a control signal.
Since rolling without slipping of the wheels is assumed, the position of the last trailer evolves according to standard unicycle kinematics
where its angular rate and longitudinal velocity are given by
which is derived by recursive usage of (7) times together with (6). Combining (11) and (12), it is possible to compactly represent the model for the pose of the th trailer as . In analogy, by introducing the vector together with (6) and (7), the time derivative of (3) yields the joint-angle kinematics
Introduce the state vector as and denote the control signal vector as , where and represent vectors of trailer steering angle rates and accelerations, respectively, and
where and . The constraints in (14) will be referred to the vehicle’s physical constraints arising from, e.g., actuator, mechanical or sensing limitations. Finally, the kinematic model of the MSNT vehicle with car-like tractor is given in (8), (10) and (11)–(13), which can compactly be represented as
where is continuous and continuously differentiable with respect to and .
2.1 Problem formulation
The MSNT vehicle with car-like tractor is assumed to operate in a closed environment with only static obstacles . The free-space where the vehicle is not in collision with any obstacle is defined as . Here, it is assumed that the obstacle-occupied region (hence also ) can be described analytically, e.g., using different bounding regions . Since the free-space is defined as the complement set of , it is in general a non-convex set.
The trajectory planning problem considered in this work is defined as follows: Compute a feasible and collision-free state and control signal trajectory , that moves the vehicle from its initial state to a desired goal state , while minimizing the cost functional . This problem can be posed as a continuous-time optimal control problem (OCP) in the following general form
where the final time and are optimization variables, and is the cost function. In this work, the cost function is defined as
where the weight matrices and . It is well-known that the OCP in (16) is in general hard to solve by directly invoking a state-of-the-art OCP solver [1, 25]. This is mainly because the vehicle model is nonlinear and the free-space is in many applications non-convex. Hence, a proper initialization strategy for any OCP solver is often a necessity in order to converge to a good locally optimal (or even feasible) solution [4, 27].
2.2 Trajectory planning framework
To efficiently and reliably solve the trajectory planning problem (16) for an MSNT vehicle with car-like tractor, we propose a framework that combines a lattice-based trajectory planner and an online optimization step. The framework is based on and extends the previous works in [14, 3, 4, 2]. The extensions are made to account for the specific properties of an MSNT vehicle with car-like tractor. The general idea is that a lattice-based trajectory planner is used in a first step to compute an optimal solution to a discrete version of the trajectory planning problem (16) using a discretized state-space and a library of precomputed trajectories. The lattice planner is responsible for solving the combinatorial aspects of the trajectory planning problem, e.g., taking left or right around obstacles, and thus provides the latter optimization step with a proper initial guess. While keeping the combinatorial parts fixed, the objective of the optimization step is to further improve the initial guess computed by the lattice planner such that the resulting trajectory is a locally optimal solution to the original trajectory planning problem (16). However, since the lattice planner uses a discretized state space, in general its computed trajectory does not satisfies the initial and goal state constraints in (16). Therefore, the optimization step is also responsible for modifying the initial guess computed by the lattice planner such that the final optimized trajectory starts at the vehicle’s initial state and reaches the goal state exactly. To handle this in a structured and numerically stable way, a homotopy-based optimization strategy is proposed that is inspired by the work in .
3 Lattice-based trajectory planner
The idea with lattice-based trajectory planning is to restrict the solutions of the trajectory planning problem (16) to a lattice graph , which is a graph embedded in an Euclidean space that forms a regular and repeated pattern . The lattice graph is constructed offline by discretizing the vehicle’s state space and precompute a set of motion primitives . Each vertex is a vehicle state and each edge represents of a motion primitive . A motion primitive is a feasible trajectory , that moves the vehicle from an initial state to a final state while satisfying and . A motion primitive is in this way designed to connect two vertices in the graph and the kinematic constraints (15) and the physical constraints (14) are satisfied offline. The cardinality of the motion primitive set is and the motion primitives that can be used from is denoted . Moreover, since the MSNT vehicle is position invariant, the motion primitive set can be computed from the position of the th trailer at the origin. Each motion primitive can then be translated and reused for all other positions on the grid.
Let denote the successor state when motion primitive is applied from and denote as the stage-cost associated to this transition, which is given by
where is defined in (17). The resulting trajectory taken by the vehicle when motion primitive is applied from , is collision-free if it does not collide with any obstacle . Define as a discrete and integer-valued decision variable that is selected by the lattice planner, where specifies which motion primitive that is applied at stage . Now, the continuous-time trajectory planning problem (16) is approximated by the following discrete-time OCP :
where and are obtained by projecting the actual initial state and desired goal state to the their closest neighboring state in . The decision variables to the problem in (19) are the motion primitive sequence and its length . A feasible solution is an ordered sequence of collision-free motion primitives , i.e., a trajectory , that connects the projected initial state with the projected goal state . Given the set of all feasible solutions to (19), an optimal solution is one that minimizes the cost function . During online planning, the discrete-time OCP in (19) can be solved using classical graph-search algorithms such as A together with an informative precomputed free-space heuristic look-up table (HLUT) as heuristic function . A HLUT significantly reduces the online planning time, as it takes the vehicleâs nonholonomic constraints into account and enables perfect estimation of cost-to-go in free-space scenarios with no obstacles.
3.1 State-space discretization
It is important that the resolution of the discretized state space and the cardinality of the motion primitive set are chosen such that the vehicle is sufficiently agile to maneuver in confined environments. However, as they also define the size of the lattice graph , both the resolution of and the cardinality of have to be chosen carefully in order to maintain a reasonable search time during online planning . Motivated by this, the position of the th trailer is discretized to a uniform grid with resolution and its orientation is irregularly
3.2 Motion primitive generation
The set of motion primitives is computed offline by solving a finite set of OCPs from all initial states with the position of the th trailer at the origin, to a set of final states in a bounded neighborhood in an obstacle-free environment. This procedure can be performed manually as in  or using exhaustive search together with pruning strategies as proposed in [19, 7]. In both cases, the motion primitive generation procedure will become time consuming or requires a designer with high system knowledge. Therefore, here we use the maneuver-based motion primitive generation framework introduced in . Instead of selecting pairs of discrete vehicle state to connect, a set of desired maneuvers from each initial state is selected and an OCP solver together with a rounding heuristic are used to automatically select the optimal final state . Each maneuver is defined with a terminal manifold in the form where and , where is the degrees of freedom for the terminal state constraint. To compute a maneuver-specified motion primitive , the following continuous-time OCP is first solved
Here it is not required that . To ensure that the final state , a rounding heuristic is used and the closest neighboring states represented in the discretized state-space from are evaluated and the solution with lowest objective functional value is selected as the resulting motion primitive . Finally, since the MSNT with car-like tractor is orientation invariant, rotational symmetries are exploited to reduce the number of OCPs needed to be solved . For more details of the motion primitive generation framework, the reader is referred to . Note that the vehicle’s physical constraints and in (20) are defined to be maneuver dependent, which is not the case in . This extension is made to automatically generate similar maneuvers, i.e., same terminal manifold , but resulting in different optimal final states and final times . This additional freedom can be used to design a more flexible lattice planner or, e.g., to adapt to a change in available trailer steering angles , during different maneuvers.
As proposed in , the motion primitive set is build upon optimized straight, heading change and parallel maneuvers. The heading change and parallel maneuvers are only possible to use from states where the tractor has nonzero velocity, i.e. , and are designed to end up in the same final velocity . Additionally, short straight maneuvers from to some are also optimized to enable the tractor to reduce, increase and keep a constant longitudinal velocity. The heading change and parallel maneuvers are computed using the following terminal manifolds.
Heading change maneuvers: By specifying the vehicle’s physical constraints and , a heading change maneuver from an initial state with pose and , to a user-defined adjacent orientation is optimized by solving (20) using the following terminal constraint
which implies that and are free variables for the OCP solver to select. Note that the vehicle states that are left out from the argument to are all constrained to zero to guarantee that . Examples of computed heading change maneuvers from are depicted in Figure 2 for an MS3T vehicle with car-like tractor where the last trailer has steerable wheels, i.e., . Here, the allowed trailer steering angle is alternated using , and rad, resulting in different types of optimal trajectories.
Parallel maneuvers: A parallel maneuver from initial state with pose and , is defined with a user-defined lateral displacement in with respect to the initial orientation . This maneuver can be optimized by solving (20) using the following terminal constraint
Here, the final position of the th trailer is restricted to a line defined by the first row in . Examples of computed parallel maneuvers for an MS3T vehicle with car-like tractor using rad from can be seen in Figure 2.
When the motion primitive set is computed, a free-space heuristic look-up table (HLUT) is computed using techniques presented in . The HLUT is computed offline by solving several obstacle-free graph-search problems (19) from all initial states with , to final states with positions on a bounded grid around the origin. This computation can be done efficiently using Dijkstra’s search, as the optimal cost-to-come is simply recorded and stored in the HLUT . Moreover, in analogy to the motion primitive generation, the size of the HLUT is kept small by exploiting the position and orientation invariance properties of .
4 Homotopy-based optimization step
Similar to , the optimization step is used to improve the initial guess , computed by the lattice planner such that the final trajectory , is a locally optimal solution to (16). Since the lattice planner uses a discretized state space , in general its computed state trajectory does not satisfy the initial and goal state constraints in (19). Thus, the optimization step should not only improve the initial guess but also make it feasible in the original problem formulation (16). To handle this in a structured way, a homotopy-based initialization strategy is used that is inspired by the work in . The idea is to start from a relaxed problem that is easy to solve and then gradually transform the relaxed problem to the original one. Here, these ideas are applied on the initial and goal state constraints in (16) such that the solution obtained from the lattice planner is feasible to the relaxed problem. By letting denote the homotopy parameters , the initial and goal state constraints in (16) are relaxed to
When , the initial guess from the lattice planner is feasible to the relaxed version of (16) and when , the original problem in (16) is obtained. One possibility is to start with and repeatedly solve the relaxed version of (16) using an OCP solver where is gradually decreased using a fixed step-size until is reached . In this work, the idea is instead to let the OCP solver automatically modify the homotopy parameters using a penalty method . Define the linear penalty as , where and define the relaxed version of the trajectory-planning problem (16) as
which is initialized with the solution from the lattice planner and . By choosing sufficiently large, the OCP solver will automatically adjust the step size of and converge to if a feasible solution to (16) exists in the homotopy class selected by the lattice planner (19) . As previously mentioned, if is obtained, a locally optimal solution , to the original trajectory planning problem (16) is obtained which can then be sent to a trajectory-tracking controller for plan execution.
Note that if is not chosen sufficiently large, a solution with may not be obtained even though one exists . In that case, one may need to increase and continue the solution process. However, in extensive simulation trials presented in Section 5, it is shown that the proposed homotopy-based optimization step is able to reliably compute locally optimal solutions to (24) with without modifying in all problem instances.
5 Simulation results
In this section, the proposed trajectory planning framework is evaluated in two complicated parking problem scenarios for an MS3T with car-like tractor where only trailer is steerable, i.e., , and a mixture of off-axle () and on-axle hitching (). Using the recursive model presented in Section 2 it is now straightforward to derive the vehicle model (15) for this specific MS3T vehicle with configuration , augmented state vector and control signals . The vehicle model is presented in Appendix A and the values of the vehicle’s parameters used in this section are summarized in Table 1.
|Tractor’s wheelbase||4.6 m|
|Length of the off-hitch||1.6 m|
|Length of trailer 1||2.5 m|
|Length of trailer 2||7.0 m|
|Length of trailer 3||7.0 m|
|Maximum joint angles ,||0.87 rad|
|Maximum steering angle tractor||0.73 rad|
|Maximum steering-angle rate tractor||rad/s|
|Maximum steering-angle acceleration tractor||rad/s|
|Maximum steering angle trailer 3||0.35 rad|
|Maximum steering-angle rate trailer 3||rad/s|
|Maximum steering-angle acceleration trailer 3||rad/s|
|Maximum longitudinal speed tractor||1 m/s|
|Maximum longitudinal acceleration tractor||m/s|
|Maximum longitudinal jerk tractor||40 m/s|
The cost function is chosen as
which is used in all steps of the trajectory planning framework. The linear cost on the homotopy parameters in the optimization step is chosen as . The lattice planner is implemented in C++, whereas the motion primitive generation and the homotopy-based optimization step are both implemented in Python using CasADi , where IPOPT is used as nonlinear programming problem solver. All simulations are performed on a laptop computer with an Intel Core i7-4600U@2.1GHz CPU.
The motion primitive set consists of heading change, parallel and straight trajectories where and a subset of the motion primitive set () can be seen in Figure 2. From each initial heading with nonzero longitudinal velocity, there are parallel and heading change maneuvers. The heading change maneuvers are computed using three different limits on the trailer steering angle , and rad, respectively, and the parallel maneuvers using rad. After the motion primitive set is computed, a free-space HLUT is computed on a square grid m centered around the origin. To evaluate if the trajectory planner is able to exploit the additional trailer steering, it is compared with an SS3T vehicle, i.e., , with the same vehicle parameters and the difference that only heading change maneuvers exist in the motion primitive set ().
The first planning scenario is a loading-bay parking problem that is illustrated in Figure 3. The obstacles and vehicle bodies are described by bounding circles , where in total, the vehicle bodies are described using 8 bounding circles of radius 2 m. The objective of the trajectory planner is to plan a trajectory from 32 different initial states (see Figure 3) to the goal state . One solution example is provided for the lattice planner (dashed line) and optimization step (solid line) for SS3T (orange) and MS3T (green), respectively, for symmetric planning problems. The results show that the planned trajectory for MS3T is purely in backward motion, as apposed to SS3T which needs to combine forward and backward motion due to less steering capability. A summary of the simulation results are provided in Table 2. The average computation time for the lattice planner is only s for both SS3T and MS3T, whereas the optimization step takes in average s for MS3T and s for SS3T. However, a signification reduction of both average cost and time improvement of the solutions are obtained. Note that the average time improvement of the solutions computed by the optimization step is significantly larger compared to the optimization step’s average computation time . Thus, when the optimization step is added, the combined average computation and execution time becomes significantly lower.
To evaluate how the homotopy-based optimization step handles an infeasible initialization, 1029 perturbed initial states for both SS3T and MS3T are evaluated (see Figure 3), where blue and red trajectories are related to MS3T and SS3T, respectively. The initial joint angles are perturbed with , and initial orientation with . In all cases, the used optimization step is able to handle the infeasible initial guess obtained from the lattice planner, i.e., the value of homotopy parameter in all cases. That is, in all cases, a solution to the original trajectory planning problem (16) is obtained. Moreover, the average computation time of the optimization step is s for MS3T and s for SS3T. Hence, the active trailer steering also reduces the computation load of the OCP solver.
The second planning scenario is a parallel parking problem with 18 different problems that is illustrated in Figure 4 and the results are summarized in Table 3. This scenario is a confined environment which affects the average computation time of the lattice planner , which is s for MS3T and s for SS3T. This is because the HLUT is drastically underestimating the cost-to-go in this confined environment. Therefore, both the average cost improvement (MS3T: , SS3T: ) and time improvement (MS3T: s, SS3T: s) of the optimization step are significant. The confined environment does however not affect the average computation time of the optimization step which is 2.4 s for MS3T and 8.4 s for SS3T. Moreover, as can be seen in the three highlighted planning problems in Figure 4, the final optimized solutions for the MS3T only needs to reverse, as opposed to the SS3T which needs to combine forward and backward motion in two cases. Finally, Figure 5 shows the difference between the trajectories from the lattice planner and the optimization step for the selected planning problem from position in Figure 4. As can be seen, the trajectories for the two steering angles, the longitudinal velocity and the joint angles are significantly smoother after the optimization step, at the same time as the final time is decreased from s to s.
An optimization-based trajectory planner for multi-steered articulated vehicles is proposed that targets low-speed maneuvers in unstructured environments. The proposed trajectory planner is divided into two steps, where a lattice planner is used in a first step to compute an optimal solution to a discretized version of the trajectory planning problem using a library of precomputed trajectories. In a second step, the output from the lattice planner is then used to initialize a homotopy-based optimization step, which enables the framework to compute a locally optimal solution that starts at the vehicle’s initial state and reaches the goal state exactly. The performance of the proposed optimization-based trajectory planner is evaluated in a set of practically relevant scenarios for a multi-steered 3-trailer vehicle where the last trailer is steerable. In the simulations, it is shown that the framework can solve challenging trajectory planning problems and that the proposed optimization step provides a significant improvement in terms of reduced objective functional value and final time, at the same time as it enables the framework to plan from and to a larger set of different vehicle states.
As future work we would like to develop a trajectory-tracking controller and evaluate the framework in real-world experiments on a full-scale test vehicle.
In this section, the model (15) for the specific MS3T vehicle used in the simulation trails is presented. The matrices , and describing the longitudinal and angular velocity transformations between neighboring vehicle segments (7) are