{'fraction_non_alphanumeric': 0.04241227050894725, 'fraction_numerical': 0.015919126191029514, 'mean_word_length': 4.288018433179723, 'pattern_counts': {'":': 0, '<': 1, '<?xml version=': 0, '>': 1, 'https://': 0, 'lorem ipsum': 0, 'www.': 0, 'xml': 0}, 'pii_count': 0, 'substrings_counts': 0, 'word_list_counts': {'cursed_substrings.json': 16, 'profanity_word_list.json': 0, 'sexual_word_list.json': 0, 'zh_pornsignals.json': 0}}
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 multisteered 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.


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 tractor-trailer 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 multi-steered N-trailer (MSNT) vehicles [18], 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., [9,15,18,21]). 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 [15,17,22,23].
Although a large amount of different motion planning techniques has been proposed in the literature for SSNT vehicles (see, e.g., [8,11,13,14,20]), there only exists a limited amount of work that consider the trajectory planning problem for special classes of MSNT vehicles (see e.g. [5,6,21,24,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 [14] and is inspired by [2][3][4]. 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.

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 N + 1 vehicle segments, including a car-like tractor and N number of trailers that are equipped with steerable or non-steerable wheels. Each vehicle segment is characterized by a segment length L i > 0 and a signed hitching offset M i . 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 non-steerable. The vehicle configuration consists of 4 + N + S variables [15] where S ∈ {1, . . . , N} denotes the number of steerable trailers: -the steering angle of the tractor's front wheel -the global position (x N , y N ) and orientation θ N of the Nth trailer in a fixed coordinate frame -for i = 1, . . . , N, a number of N constrained joint angles -and S ∈ {1, . . . , N} number of steering angles associated with steerable trailer wheels where index s ∈ I s ⊆ {1, . . . , N} specifies which trailers that have steerable wheels. The configuration vector for the MSNT with car-like tractor will be defined as where γ γ γ s represents a vector of trailer steering angles, and Figure 1: A schematic description of the geometric lengths and the vehicle configuration for a MSNT vehicle with car-like tractor in a global coordinate system (inspired and adapted from [15]).
The leading car-like tractor is described by a kinematic single-track vehicle model and its orientation θ 0 evolves asθ where κ 0 (β 0 ) = tan β 0 L 0 is the curvature of the tractor and v 0 is the longitudinal velocity of its rear axle. The recursive formula for the transformation of the angularθ i and longitudinal v i velocities between any two neighboring vehicle segments are given by [15,18]: where γ i denotes the steering angle of the ith trailer. For the car-like tractor, we have that γ 0 ≡ 0 since its rear axle is non-steerable. Note that if the jth trailer is non-steerable, it suffices to take γ j = 0 in (7). Each trailer steering angle γ s , s ∈ I s and the tractors steering angle β 0 are modeled as double integrator systemsγ where ω 0 , ω s , s ∈ I s and u ω 0 , u ω s , s ∈ I s 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 u ω 0 and u ω s , s ∈ I s are treated as control signals. Similarly, the longitudinal velocity of the tractor v 0 is constrained as v 0 ∈ Ω v = [−v,v] and its dynamics is modeled as a double integrator systeṁ v 0 = a 0 ,ȧ 0 = u v (10) in order to respect constraints on the longitudinal acceleration a 0 ∈ A = [−ā,ā] and jerk During the planning phase, the longitudinal jerk u v 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 kinematicṡ where its angular rateθ N and longitudinal velocity v N are given by which is derived by recursive usage of (7) N times together with (6). Combining (11) and (12), it is possible to compactly represent the model for the pose of the Nth trailer q N = [θ N x N y N ] T asq N = v 0 f q N (q). In analogy, by introducing the vector c = [1 0] T together with (6) and (7), the time derivative of (3) yields the joint-angle kinematicṡ where ∏ 1 j=0 (· ) I. Introduce the state vector as x = [q T ω 0 ω ω ω T s v 0 a 0 ] T ∈ X and denote the control signal vector as u = [u ω 0 u u u T ω s u v ] T ∈ U , where ω ω ω s and u u u ω s represent vectors of trailer steering angle rates and accelerations, respectively, and where dim(X ) = 7 + N + 2S n and dim(U ) = 2 + S m. 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 aṡ where f : R n × R m → R n is continuous and continuously differentiable with respect to x ∈ X and u ∈ U .

Problem formulation
The MSNT vehicle with car-like tractor is assumed to operate in a closed environment with only static obstacles X obs . The free-space where the vehicle is not in collision with any obstacle is defined as X free = X \ X obs . Here, it is assumed that the obstacle-occupied region X obs (hence also X free ) can be described analytically, e.g., using different bounding regions [12]. Since the free-space X free is defined as the complement set of X obs , 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 (x(t), u(t)), t ∈ [0,t G ] that moves the vehicle from its initial state x I ∈ X free to a desired goal state x G ∈ X free , while minimizing the cost functional J. This problem can be posed as a continuous-time optimal control problem (OCP) in the following general form where the final time t G and u are optimization variables, and l : R n × R m → R + is the cost function. In this work, the cost function is defined as where the weight matrices Q 0 and R 0. 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 X free 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].

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 [2][3][4]14]. 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 [2]. The main steps of the trajectory planning framework is summarized in Workflow 1, where the lattice planner (Step 0 and 1) and the optimization step (Step 2) are explained in detail in Section 3 and Section 4, respectively.

Workflow 1 The proposed trajectory planning framework for a MSNT vehicle with car-like tractor
Step 0 (offline) -State lattice construction: a) State-space discretization: Specify the resolution of the discretized state-space X d b) Motion primitive generation: Compute the set of motion primitives P by specifying a set of desired maneuvers and solve (20) using an OCP solver c) Heuristic function: Precompute a HLUT by calculating the optimal cost-to-go on a grid in an obstacle-free environment Step 1 -Online planning: a) Initialization: Project the vehicle's initial state x I and desired goal state x G to X d b) Graph search: Solve the discrete graph search problem (19) using a graph search algorithm Step 2 -Homotopy-based optimization step: The idea with lattice-based trajectory planning is to restrict the solutions of the trajectory planning problem (16) to a lattice graph G = V, E , 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 X d ⊂ X and precompute a set of motion primitives P.
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 |P| = M and the motion primitives that can be used from x[k] is denoted P(x[k]) ⊆ P. Moreover, since the MSNT vehicle is position invariant, the motion primitive set P can be computed from the position of the Nth trailer at the origin. Each motion primitive m i can then be translated and reused for all other positions on the grid.
, m i ) denote the successor state when motion primitive m i ∈ P is applied from x[k] and denote J p (m i ) as the stage-cost associated to this transition, which is given by where l(x i , u i ) is defined in (17). The resulting trajectory taken by the vehicle when motion primitive m i ∈ P is applied from x[k], is collision-free if it does not collide with any obstacle . . , M} as a discrete and integer-valued decision variable that is selected by the lattice planner, where u p [k] specifies which motion primitive that is applied at stage k. Now, the continuous-time trajectory planning problem (16) is approximated by the following discrete-time OCP [14]: wherex I andx G are obtained by projecting the actual initial state x I and desired goal state x G to the their closest neighboring state in X d . The decision variables to the problem in (19) are the motion primitive sequence {u p [k]} N p −1 k=0 and its length N p ∈ Z + . A feasible solution is an ordered sequence of collision-free motion primitives {m u p [k] } N p −1 k=0 , i.e., a trajectory (x(t), u(t)), t ∈ [0,t G ] that connects the projected initial state x(0) =x I with the projected goal state x(t G ) = x G . Given the set of all feasible solutions to (19), an optimal solution is one that minimizes the cost function J D . 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 freespace heuristic look-up table (HLUT) as heuristic function [10]. A HLUT significantly reduces the online planning time, as it takes the vehicles nonholonomic constraints into account and enables perfect estimation of cost-to-go in free-space scenarios with no obstacles.

State-space discretization
It is important that the resolution of the discretized state space X d and the cardinality of the motion primitive set P 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 G, both the resolution of X d and the cardinality of P have to be chosen carefully in order to maintain a reasonable search time during online planning [19]. Motivated by this, the position of the ) is discretized to a uniform grid with resolution r and its orientation is irregularly 1 discretized θ N [k] ∈ Θ into |Θ| = 16 different orientations as proposed in [19]. Additionally, the longitudinal velocity of the tractor is discretized as The proposed discretization will impose restrictions, but is motivated to enable fast online planning. Moreover, since the output from the lattice planner will be used to warm start a second optimization step, it will improve the initial guess computed by the lattice planner such that the finally computed trajectory is a locally optimal solution to the original trajectory planning problem (16).

Motion primitive generation
The set of motion primitives P is computed offline by solving a finite set of OCPs from all initial states x i s ∈ X d with the position of the Nth trailer at the origin, to a set of final states x i f ∈ X d in a bounded neighborhood in an obstacle-free environment. This procedure can be performed manually as in [14] or using exhaustive search together with pruning strategies as proposed in [7,19]. 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 x i s ∈ X d is selected and an OCP solver together with a rounding heuristic are used to automatically select the optimal final state x i f ∈ X d . Each maneuver is defined with a terminal manifold in the form g i (x i (t i f )) = 0 where g : R n → R l and l < n, where n − l is the degrees of freedom for the terminal state constraint. To compute a maneuver-specified motion primitive m i ∈ P, the following continuous-time OCP is first solved Here it is not required that x i (t i f ) ∈ X d . To ensure that the final state x i f = x i (t i f ) ∈ X d , a rounding heuristic is used and the closest neighboring states represented in the discretized state-space X d from x i (t i f ) are evaluated and the solution with lowest objective functional value is selected as the resulting motion primitive m i . 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 [19]. For more details of the motion primitive generation framework, the reader is referred to [3]. Note that the vehicle's physical constraints X i ⊆ X and U i ⊆ U 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 g i (x i (t i f )) = 0, but resulting in different optimal final states x i f and final times t i f . 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 γ s , s ∈ I s 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. v 0,s = ±v, and are designed to end up in the same final velocity v 0, f = v 0,s . Additionally, short straight maneuvers from v 0,s ∈ V to some v 0, f ∈ V 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 X i ⊆ X and U i ⊆ U , a heading change maneuver from an initial state x i s ∈ X d with pose p i N,s = [0 0 θ i N,s ] T and v i 0,s = ±v, to a user-defined adjacent orientation θ i N, f ∈ Θ \ θ i N,s is optimized by solving (20) using the following terminal constraint which implies that x i N (t f ) and y i N (t f ) are free variables for the OCP solver to select. Note that the vehicle states that are left out from the argument to g i are all constrained to zero to guarantee that x i f ∈ X d . Examples of computed heading change maneuvers from (θ 3,s , v 0,s ) = (π/2, ±1) are depicted in Figure 2 for an MS3T vehicle with car-like tractor where the last trailer has steerable wheels, i.e., I s = {3}. Here, the allowed trailer steering angle |γ 3 | ≤γ 3 is alternated usingγ 3 = 0, 0.175 and 0.35 rad, resulting in different types of optimal trajectories.
Parallel maneuvers: A parallel maneuver from initial state x i s ∈ X d with pose p i N,s = [0 0 θ i N,s ] T and v i 0,s = ±v, is defined with a user-defined lateral displacement z i lat in (x i N, f , y i N, f ) with respect to the initial orientation θ i N,s . This maneuver can be optimized by solving (20) using the following terminal constraint Here, the final position of the Nth trailer (x N (t i f ), y N (t i f )) is restricted to a line defined by the first row in (22). Examples of computed parallel maneuvers for an MS3T vehicle with car-like tractor usingγ 3 = 0.35 rad from (θ 3,s , v 0,s ) = (0, ±1) can be seen in Figure 2.
When the motion primitive set P is computed, a free-space heuristic look-up table (HLUT) is computed using techniques presented in [10]. The HLUT is computed offline by solving several obstacle-free graph-search problems (19) from all initial statesx I ∈ X d with (x N,I , y N,I ) = (0, 0), to final statesx G ∈ X d 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 [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 P [7].

Homotopy-based optimization step
Similar to [4], the optimization step is used to improve the initial guess (x(t), u(t)), t ∈ [0,t G ] computed by the lattice planner such that the final trajectory (x * (t), u * (t)), t ∈ [0,t * G ] is a locally optimal solution to (16). Since the lattice planner uses a discretized state space X d , 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 [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 ε T p = [ε p,I ε p,G ] ∈ [0, 1] 2 denote the homotopy parameters [2], the initial and goal state constraints in (16) are relaxed to When ε T p = [1 1], the initial guess from the lattice planner is feasible to the relaxed version of (16) and when ε T p = [0 0], the original problem in (16) is obtained. One possibility is to start with ε 0 p = [1 1] T and repeatedly solve the relaxed version of (16) using an OCP solver where ε k p is gradually decreased using a fixed step-size ∆ε p until ε k p = [0 0] T 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 c T p ε p , where c p ∈ R 2 ++ and define the relaxed version of the trajectory-planning problem (16) as minimize u(·), t G , ε p J H = J + c T p ε p subject toẋ(t) = f (x(t), u(t)), which is initialized with the solution from the lattice planner and ε T p = [1 1]. By choosing c p sufficiently large, the OCP solver will automatically adjust the step size of ε p and converge to ε T p = [0 0] if a feasible solution to (16) exists in the homotopy class selected by the lattice planner (19) [2]. As previously mentioned, if ε * p = [0 0] T is obtained, a locally optimal solution (x * (t), u * (t)), t ∈ [0,t * G ] 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 c p is not chosen sufficiently large, a solution with ε T p = [0 0] may not be obtained even though one exists [16]. In that case, one may need to increase c p 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 ε T p = [0 0] without modifying c p in all problem instances.

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 N = 3 is steerable, i.e., I s = {3}, and a mixture of off-axle (M 1 = 0) and on-axle hitching (M 2 = M 3 = 0). 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 q = [β 0 β 1 β 2 β 3 γ 3 θ 3 x 3 y 3 ] T , augmented state vector x = [q T ω 0 ω 3 v 0 a 0 ] T and control signals u = [u ω 0 u ω 3 u v ] T . The vehicle modelẋ = f (x, u) is presented in Appendix A and the values of the vehicle's parameters used in this section are summarized in Table 1. 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 c T p = [1000 1000]. The lattice planner is implemented in C++, whereas the motion primitive generation and the homotopy-based optimization   Figure 2. From each initial heading with nonzero longitudinal velocity, there are 20 parallel and 24 heading change maneuvers. The heading change maneuvers are computed using three different limits on the trailer steering angleγ 3 = 0, 0.175 and 0.35 rad, respectively, and the parallel maneuvers usingγ 3 = 0.35 rad. After the motion primitive set is computed, a free-space HLUT is computed on a square grid 80 × 80 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.,γ 3 = 0, with the same vehicle parameters and the difference that only 8 heading change maneuvers exist in the motion primitive set P SS3T (|P SS3T | = 1124).
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 [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 x I ∈ X d (see Figure 3) to the goal state x G . One solution example is provided for the lattice planner (dashed line) and x G Figure 3: Loading-bay parking scenario using the proposed trajectory planner for SS3T and MS3T from two initial positions (p A I and p B I ), symmetric with respect to the loading bay, initial orientations θ 3,I ∈ Θ and zero initial joint angles, to the goal state x G . The solutions for the position (x * 3 (· ), y * 3 (· )) from 1029 perturbed initial states for SS3T (p A I ) and MS3T (p B I ) are displayed by blue (red) solid lines for MS3T (SS3T). The initial guess computed by the lattice planner for (x 3 (· ), y 3 (· )) and optimization step for (x * 3 (· ), y * 3 (· )) from straight configuration is marked with green (orange) dashed line and green (orange) solid line, respectively, for MS3T (SS3T). 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 0.1 s for both SS3T and MS3T, whereas the optimization step takes in average 3.0 s for MS3T and 9.0 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 stept G,imp is Table 2: Results from loading-bay parking scenario in Figure 3 for 32 problems.t lat andt ocp is average computation time for lattice planner and optimization step, respectively.J D andJ H is average objective functional value for the solutions from lattice planner and optimization step, respectively.r imp andt G,imp is average cost and time improvement between the lattice planner's and the optimization step's solutions, respectively.  Figure 4: Parallel parking scenario using the proposed trajectory planner for SS3T and MS3T from three different initial positions (p 1 I , p 2 I and p 3 I ), six different initial orientations θ 3,I and zero initial joint angles, to the goal state x G . The blue (red) lines illustrate the planned trajectories after the optimization step for the position of trailer 3 (x * 3 (· ), y * 3 (· )) for MS3T (SS3T) from three selected initial states.
significantly larger compared to the optimization step's average computation timet G,ocp . 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 β i = −30 • , −20 • , . . . , 30 • , i = 1, 2, 3 and initial orientation with θ 3,I = −10 • , 0 • , 10 • . 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 ε * p,I = 0 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 3.4 s for MS3T and 26.2 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 Table 3: Results from parallel parking scenario in Figure 4 for 18 problems. See Table 2  (a) The joint angle between tractor and trailer 1 β 1 (· ) (black), joint angle between the trailer 1 and trailer 2 β 2 (· ) (blue), and joint angle between the trailer 2 and trailer 3 β 3 (· ) (red).   Figure 4 from initial state p 3 I with MS3T for optimized (solid lines) and lattice initial guess (dotted lines). environment which affects the average computation time of the lattice plannert lat , which is 11.3 s for MS3T and 5.6 s for SS3T. This is because the HLUT is drastically underestimating the cost-to-go in this confined environment. Therefore, both the average cost improvementr imp (MS3T: −52%, SS3T: −55%) and time improvementt G,imp (MS3T: −80.4 s, SS3T: −109.6 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 p 3 I 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 96 s to 70 s.

Conclusions
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.