Optimizationbased motion planning for multisteered articulated vehicles
Abstract
The task of maneuvering a multisteered articulated vehicle in confined environments is difficult even for experienced drivers. In this work, we present an optimizationbased trajectory planner targeting lowspeed maneuvers in unstructured environments for multisteered Ntrailer vehicles, which are comprised of a carlike 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 latticebased 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 optimizationbased trajectory planner is evaluated in a set of practically relevant scenarios for a multisteered 3trailer vehicle with a carlike tractor where the last trailer is steerable.
1 Introduction
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 [15] and long tractortrailer vehicle combinations [9]. 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 multisteered Ntrailer (MSNT) vehicles [18], which is a generalization of singlesteered Ntrailer (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 driverassistance system concepts have been proposed to automatically steer the extra steerable wheel(s) in order to increase lowspeed maneuverability or to reduce the so called offtracking 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 carlike tractor that: i) can handle various state and input constraints, ii) allows a mixture of onaxle/offaxle hitched and steerable/nonsteerable 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 carlike tractor targeting lowspeed maneuvers in confined and unstructured environments. The framework extends some techniques presented in our previous work in [14] and is inspired by [3, 4, 2]. Here, a latticebased 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 statespace discretization. In a second step, the output from the lattice planner is used to initialize a homotopybased 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 carlike 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 latticebased trajectory planner and the homotopybased optimization step are presented, respectively. Simulation results for an MS3T vehicle with carlike 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 carlike tractor considered in this work is illustrated in Figure 1. The multibody vehicle is composed of vehicle segments, including a carlike tractor and number of trailers that are equipped with steerable or nonsteerable wheels. Each vehicle segment is characterized by a segment length and a signed hitching offset . Since lowspeed maneuvers are considered in this work, a kinematic vehicle model is used. The model is based on the work in [15] 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 nonsteerable. The vehicle configuration consists of variables [15] where denotes the number of steerable trailers:

the steering angle of the tractor’s front wheel
(1) 
the global position and orientation of the th trailer in a fixed coordinate frame
(2) 
for a number of constrained joint angles
(3) 
and number of steering angles associated with steerable trailer wheels
(4) where index specifies which trailers that have steerable wheels. The configuration vector for the MSNT with carlike tractor will be defined as
(5) where represents a vector of trailer steering angles, and .
The leading carlike tractor is described by a kinematic singletrack vehicle model and its orientation evolves as
(6) 
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]:
(7) 
where denotes the steering angle of the th trailer. For the carlike tractor, we have that since its rear axle is nonsteerable. Note that if the th trailer is nonsteerable, it suffices to take in (7).
Each trailer steering angle , and the tractors steering angle are modeled as double integrator systems
(8) 
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
(9) 
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
(10) 
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
(11) 
where its angular rate and longitudinal velocity are given by
(12) 
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 jointangle kinematics
(13) 
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
(14) 
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 carlike tractor is given in (8), (10) and (11)–(13), which can compactly be represented as
(15) 
where is continuous and continuously differentiable with respect to and .
2.1 Problem formulation
The MSNT vehicle with carlike tractor is assumed to operate in a closed environment with only static obstacles . The freespace where the vehicle is not in collision with any obstacle is defined as . Here, it is assumed that the obstacleoccupied region (hence also ) can be described analytically, e.g., using different bounding regions [12]. Since the freespace is defined as the complement set of , it is in general a nonconvex set.
The trajectory planning problem considered in this work is defined as follows: Compute a feasible and collisionfree 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 continuoustime optimal control problem (OCP) in the following general form
(16) 
where the final time and are optimization variables, and is the cost function. In this work, the cost function is defined as
(17) 
where the weight matrices and . It is wellknown that the OCP in (16) is in general hard to solve by directly invoking a stateoftheart OCP solver [1, 25]. This is mainly because the vehicle model is nonlinear and the freespace is in many applications nonconvex. 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 carlike tractor, we propose a framework that combines a latticebased 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 carlike tractor. The general idea is that a latticebased 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 statespace 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 homotopybased optimization strategy is proposed that is inspired by the work in [2].
3 Latticebased trajectory planner
The idea with latticebased 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 [19]. 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 stagecost associated to this transition, which is given by
(18) 
where is defined in (17). The resulting trajectory taken by the vehicle when motion primitive is applied from , is collisionfree if it does not collide with any obstacle . Define as a discrete and integervalued decision variable that is selected by the lattice planner, where specifies which motion primitive that is applied at stage . Now, the continuoustime trajectory planning problem (16) is approximated by the following discretetime OCP [14]:
(19) 
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 collisionfree 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 discretetime OCP in (19) can be solved using classical graphsearch algorithms such as A together with an informative precomputed freespace heuristic lookup table (HLUT) as heuristic function [10]. A HLUT significantly reduces the online planning time, as it takes the vehicleâs nonholonomic constraints into account and enables perfect estimation of costtogo in freespace scenarios with no obstacles.
3.1 Statespace 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 [19]. 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 obstaclefree environment. This procedure can be performed manually as in [14] 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 maneuverbased motion primitive generation framework introduced in [3]. 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 maneuverspecified motion primitive , the following continuoustime OCP is first solved
(20) 
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 statespace from are evaluated and the solution with lowest objective functional value is selected as the resulting motion primitive . Finally, since the MSNT with carlike tractor is orientation invariant, rotational symmetries are exploited to reduce the number of OCPs needed to be solved [19]. For more details of the motion primitive generation framework, the reader is referred to [3]. Note that the vehicle’s physical constraints and in (20) are defined to be maneuver dependent, which is not the case in [3]. 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 [3], 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 userdefined adjacent orientation is optimized by solving (20) using the following terminal constraint
(21) 
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 carlike 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 userdefined lateral displacement in with respect to the initial orientation . This maneuver can be optimized by solving (20) using the following terminal constraint
(22) 
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 carlike tractor using rad from can be seen in Figure 2.
When the motion primitive set is computed, a freespace heuristic lookup table (HLUT) is computed using techniques presented in [10]. The HLUT is computed offline by solving several obstaclefree graphsearch 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 costtocome is simply recorded and stored in the HLUT [10]. 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 [7].
4 Homotopybased optimization step
Similar to [4], 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 homotopybased initialization strategy is used that is inspired by the work in [2]. 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 [2], the initial and goal state constraints in (16) are relaxed to
(23) 
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 stepsize until is reached [2]. In this work, the idea is instead to let the OCP solver automatically modify the homotopy parameters using a penalty method [16]. Define the linear penalty as , where and define the relaxed version of the trajectoryplanning problem (16) as
(24) 
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) [2]. 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 trajectorytracking controller for plan execution.
Note that if is not chosen sufficiently large, a solution with may not be obtained even though one exists [16]. 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 homotopybased 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 carlike tractor where only trailer is steerable, i.e., , and a mixture of offaxle () and onaxle 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.
Vehicle parameter  Value 

Tractor’s wheelbase  4.6 m 
Length of the offhitch  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 steeringangle rate tractor  rad/s 
Maximum steeringangle acceleration tractor  rad/s 
Maximum steering angle trailer 3  0.35 rad 
Maximum steeringangle rate trailer 3  rad/s 
Maximum steeringangle 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
(25) 
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 homotopybased optimization step are both implemented in Python using CasADi [1], where IPOPT is used as nonlinear programming problem solver. All simulations are performed on a laptop computer with an Intel Core i74600U@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 freespace 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 ().
Vehicle  [s]  [s]  [s]  

SS3T  0.11  9.3  170.8  128.4  25%  26.8 
MS3T  0.09  3.0  126.8  100.8  21%  14.4 
The first planning scenario is a loadingbay parking problem that is illustrated in Figure 3. The obstacles and vehicle bodies are described by bounding circles [12], 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.
Vehicle  [s]  [s]  [s]  

SS3T  5.6  8.4  270.6  122.4  55%  109.6 
MS3T  11.3  2.4  207.9  98.1  52%  80.4 
To evaluate how the homotopybased 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 costtogo 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.
6 Conclusions
An optimizationbased trajectory planner for multisteered articulated vehicles is proposed that targets lowspeed 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 homotopybased 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 optimizationbased trajectory planner is evaluated in a set of practically relevant scenarios for a multisteered 3trailer 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 trajectorytracking controller and evaluate the framework in realworld experiments on a fullscale test vehicle.
Appendix A
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
(26) 
since and . Thus, by inserting (26) in (12) the longitudinal velocity of trailer 3 can be written as
(27) 
Finally, using (26) and (27) in (11)–(13), the complete model (15) for the MS3T vehicle becomes