# Path and Control Planning for Autonomous Vehicles in Restricted Space and Low Speed

^{*}

^{†}

## Abstract

**:**

## 1. Introduction

## 2. Kinematic Models of Autonomous Motion

#### 2.1. Single Vehicle

_{1}and l

_{1}.

#### 2.1.1. Passenger Car

**q**: x denotes vehicle longitudinal displacement, y denotes vehicle lateral displacement, φ denotes vehicle yaw angle, θ denotes vehicle’s front axle steering angle, and v denotes ego vehicle velocity. The derivatives are: v

_{i}_{x}denotes ego vehicle longitudinal velocity along global x-coordinate, v

_{y}denotes ego vehicle lateral velocity along global y-coordinate, and ω

_{φ}denotes ego vehicle yaw rate. The control parameters

**u**are: ω

_{i}_{θ}denotes vehicle’s front axle steering rate and a denotes ego vehicle longitudinal acceleration. Thus, the control parameters are the longitudinal acceleration and the angular velocity of the steered wheel rotation. Additionally, the input vector of model parameters is

**p**(

**p**= L), where L is the vehicle wheelbase. Then, in vector form,

**f**(

**q,u,p**) to a more convenient form, separating states and controls:

#### 2.1.2. Long Truck with Steered Rear Axle

_{1}is negative with respect to the intersection point of radius R, then

_{ζ}(vehicle’s rear axle steering rate) should be included to develop the state-space vector for a long single truck:

#### 2.2. Articulated Vehicles

_{1}and R

_{2}are crossing in the center of O. Moreover, in the same way, the radius R

_{2}passes through the semitrailer’s conditional middle axle (if 2 axles, then between them). The coupling point is shifted relative to the tractor’s rear axle on the offset e

_{1}. Since ψ is the articulation angle, R

_{1}and R

_{2}will also be located at the angle ψ to each other.

_{2}and the longitudinal axis of the semitrailer is determined by dividing the base L

_{2}onto L’

_{2}and l’

_{2}.

#### 2.2.1. Conventional Tractor-Semitrailer Vehicle

_{1}, and the angular velocity of the semitrailer ω

_{2}:

**q**: x denotes tractor longitudinal displacement, y denotes tractor lateral displacement, φ denotes tractor yaw angle, ψ denotes vehicle articulation angle, θ denotes vehicle’s front axle steering angle, and v denotes tractor velocity. The derivatives are: v

_{i}_{x}denotes tractor longitudinal velocity along global x-coordinate, v

_{y}denotes tractor lateral velocity along global y-coordinate, ω

_{φ}denotes tractor yaw rate, and ω

_{ψ}denotes vehicle articulation rate. The control parameters

**u**are: ω

_{i}_{θ}denotes tractor’s front axle steering rate and a denotes tractor longitudinal acceleration. Thus, the control parameters are the longitudinal acceleration and the angular velocity of the front axle’s steered wheel. The parameters

**p**are: L

_{1}denotes tractor wheelbase, e

_{1}denotes fifth wheel offset relative to the tractor’s rear axle (positive if within wheelbase, negative if shifted behind the rear axle), and L

_{2}denotes semitrailer wheelbase, which is the distance from the coupling center (kingpin) to the conditional middle axle. Considering that ψ = φ

_{1}– φ

_{2}, dψ/dt = ω

_{1}– ω

_{2}, then in vector form,

_{1}. The semitrailer position could be written as

#### 2.2.2. Tractor-Semitrailer Vehicle with Semitrailer’s Steered Axles

_{ζ}(semitrailer’s middle axle steering rate), as shown in Figure 1d. Considering Equation (28), The state-space components are given by

_{2}may be determined from two conditions, considering Equation (29) and the case of a long single truck:

_{2}is negative relative to a cross point of radius R

_{2}:

_{2}can be derived as:

## 3. Optimization Model

#### 3.1. Basic Model

_{0}, t

_{f}] can be written as:

**q**

_{i}denotes vector of state-space parameters at the

**i**th prediction horizon step, W

_{q}, W

_{u}, W

_{Δu}denote the matrices of weighting factors,

**u**

_{i}denotes control signals at the

**i**th prediction horizon step,

**z**

_{p}= (u

^{T}

_{0}, u

^{T}

_{i+1}, ··· u

^{T}

_{p-1}, ε

_{p}) denotes solution, ε denotes scalar dimensionless slack variable used for constraint softening, ρ

_{ε}denotes constraint violation penalty weight, i denotes current control interval, and p denotes prediction horizon (number of intervals).

_{j}

_{,min(i)}, q

_{j}

_{,max(i)}denote the minimum and maximum values of

**j**th output at the

**i**th prediction horizon step, respectively, u

_{j}

_{,min(i)}, u

_{j}

_{,max(i)}denote the minimum and maximum values of

**j**th input at the

**i**th prediction horizon step, respectively, Δu

_{j}

_{,min(i)}, Δu

_{j}

_{,max(i)}denote the minimum and maximum values of

**j**th input rate at the

**i**th prediction horizon step, respectively, h

^{(q)}

_{j}

_{,min(i)}, h

^{(q)}

_{j}

_{,max(i)}denote the minimum and maximum values of

**j**th output’s hard constraints at the

**i**th prediction horizon step, respectively, h

^{(u)}

_{j}

_{,min(i)}, h

^{(u)}

_{j}

_{,max(i)}denote the minimum and maximum values of

**j**th input’s hard constraints at the

**i**th prediction horizon step, respectively, h

^{(∆u)}

_{j}

_{,min(i)}, h

^{(∆u)}

_{j}

_{,max(i)}denote the minimum and maximum values of

**j**th input rates’ hard constraints at the

**i**th prediction horizon step, respectively, n

_{q}denotes the number of output parameters, n

_{u}denotes number of input parameters, and n

_{Δu}denotes the number of input rate parameters.

#### 3.2. Operational and Physical Constraints

#### 3.2.1. Case 1: Vehicle Yaw Rate

_{θ}:

_{cr}denotes the factor’s critical value.

#### 3.2.2. Case 2: Parking in Restricted Space

**S**of the local space boundaries may be tied by a virtual connection with the vehicle body’s conditional geometric center

_{i}**C**, in such a way that a segment

**CS**by means of a safety contour’s control point

_{i}**B**is being divided into two components:

_{i}**CB**(associated with the vehicle orientation relative to the initial position) and

_{i}**S**(which according to the conditions of maneuver’s safety and accuracy, should always remain positive). Thus, for each

_{i}B_{i}**i**at each predicted moment

**t**, the following condition must be satisfied:

_{k}**S**values in the nonlinear optimization algorithm is optional, since most of them will certainly be greater than zero, and at each iteration, the

_{i}B_{i}**S**combinations will be different. Therefore, within one iteration of the optimization search, it can be requested that only the minimum

_{i}B_{i}**S**value does not exceed zero. That is,

_{i}B_{i}**S**calculations. The main goal is to determine

_{i}B_{i}**CB**, since the vehicle contour changes its orientation relative to the initial one (Figure 2c,d). Even though

_{i}**CS**segments converging in the center

_{i}**C**(Figure 2c) do not ensure the perpendicularity to the vehicle’s safety contour, this drawback, however, may be compensated by increasing the grid density, considering that point that any violation of vehicle’s safety contour is fully sufficient for determining the constraints. Moreover, as approaching the most distinctive ledges of the parking zone perimeter, the

**CS**distances are reduced and their directions become more and more similar to perpendiculars passing through the safety contour.

_{i}**n**, an unambiguous determination in the vehicle’s initial position is established between an angle α

_{n0}and a point P

_{n0}. In this case, the internal points within the nodes can be determined using an interpolation (linear or spline, depending on priorities).

**XOY**system can be divided into translational and rotational (Figure 2c,d).

**S**of the parking space perimeter relative to the car contour’s initial position

_{i}**x**

_{0}**C**forms the point

_{0}y_{0}**B**on the angle

_{i0}**β**, and in the state

_{i0}**x**the point migrates to the position

_{k}C_{k}y_{k}**B**on the angle

_{ik}**β**. If one considers this situation from the car’s local coordinate system

_{ik}**xCy**, all the segments

**CS**will rotate relative to the safe contour. Therefore, using the interpolation approach in accordance with the prepared basis (Figure 2b), it is possible to recalculate the points’

_{i}**B**positions by the known angles

_{ik}**β**, which, in turn, are obtained based on the known coordinates of

_{ik}**S**and

_{i}**C**.

_{k}**C**is identical to the segment

_{0}S_{i}**C**, but due to the vehicle’s turn to an angle

_{k}S_{i}**φ**, the new angular position

_{k}**β**will be defined as:

_{ik}**q**are iterative during the optimization process, their current values are known, and thus, the segments’

**C**current angular directions relative to the local coordinate system

_{k}S_{i}**x**can be determined by the superposition of the parking boundary’s relative displacement and rotation. Thus, at each iterative step

_{0}C_{0}y_{0}**k**, the

**S**nodes’ coordinates due to translation in the vehicle’s coordinate system

_{i}**x**are:

_{0}C_{0}y_{0}**x**

_{Sk},

**y**

_{Sk}denote vectors of zone contour nodes’ coordinates in the coordinate system

**x**,

_{0}C_{0}y_{0}**x**

_{S0},

**y**

_{S0}denote vectors of zone contour nodes’ coordinates in the global coordinate system

**XOY**, and

**x**

_{Ck},

**y**

_{Ck}denote displacements of the safe contour’s center

**C**in the global coordinate system

_{k}**XOY**. The distances r

_{Sik}from the nodes

**S**to the center

_{ik}**C**and the angles

_{k}**β**at time interval

_{ik}**k**in the coordinate system

**x**:

_{k}C_{k}y_{k}**β**and the base angles α

_{ik}_{n0}with the corresponding x

_{n0}, y

_{n0}, the coordinates of points

**B**can be obtained:

_{ik}**x**

_{Bk},

**y**

_{Bk}denote vectors of safety contour points’ coordinates at time step

**k**, f

_{x}, f

_{y}denote parametric interpolation functions for x

_{k}and y

_{k}coordinates, respectively,

**α**denotes vector of segments’

_{0}**C**angles at initial state,

_{0}S_{i}**x**

_{0},

**y**

_{0}denote vectors of contour control points’ coordinates at initial state, and

**β**denotes vector of current segments’

_{k}**C**angles, Equation (47).

_{k}S_{i}**C**to points

_{k}**B**in the coordinate system

_{ik}**x**at time interval

_{0}C_{0}y_{0}**k**are:

**k**is given by

#### 3.2.3. Case 3: Circular Motion

**n**given points must lay within the considered boundaries. Each such a

**B**point (like Figure 2b) is distanced by a radius

_{n}**r**from the contour’s center and compose an angle

_{Bn}**α**with the vehicle local coordinate system’s longitudinal axis. Thus, in the global coordinate system

_{Bn}**XOY**, the

**B**points’ coordinates

_{n}**X**,

_{Bnk}**Y**for

_{Bnk}**k**th prediction horizon step yield:

**XOY**:

**k**th prediction horizon step is:

_{in}and R

_{out}denote the inner and outer radii of a roundabout, respectively.

## 4. Simulation

#### 4.1. Simulation of Passenger Car

**L**which equals 2.8 m. Equations (8)–(15) are used for model prediction. For the parking maneuver (Figure 3, Figure 4a,b), the quadratic linear form of the cost function works well, while optimizing without restrictions when the minimum of a cost function is absolute. Moreover, due to the quadraticity, the results give the smoothest functions of the state parameters, which rarely change their sign within the prediction horizon. However, the imposition of restrictions narrows the area of optimum search and complicates the task, where conditional optimality may also be acceptable. In the case of parking, the priority is not so much the optimality of solution as the maneuver accuracy with the possibility of arbitrarily using the space and directions of movement (forward and backward). In view of the latter, it is proposed that one uses a linear function as the target one that relaxes the search and reduces the time of iterations. The inequality constraints are based on Equation (54).

_{q}, e

_{u}denote unit vectors of the same dimension as

**q**and

**u**, respectively, W

_{q}, W

_{u}denote weighting factors, and z

_{p}denotes solution vector.

#### 4.2. Simulation of Long Single Truck with Steered Rear Axle

**L**=

**L**

_{10}+

**l**

_{10}, consisting of longitudinal wheelbase

**L**

_{10}= 6.65 m between steering and driving axles and spread axles’ wheelbase

**l**

_{10}= 1.4 m. Equations (24)–(26) are used for model prediction. Consider first the parking maneuver (Figure 5a,b). In the case of perpendicular reverse parking, the following objectives are set for optimizing the maneuver: reducing the use of space, ensuring the smoothness of the control functions, and redistributing the control between the vehicle’s steered axles to provide the minimal total steering control. In particular, the vehicle maneuver is better to be oriented in a way that there are the perpendicular and the parallel phases resembling the letter “L” relative to an unoccupied parking place. In this regard, it is expedient to minimize the use of corresponding x and y coordinates. Then, considering constraints set in Equation (54), the cost function can be derived as a combination:

_{u}denotes the control weighting factor and W

_{θζ}denotes weighting factor of mutual influence between θ and ζ.

_{q}= diag(1, 1, 1, 0, 0, 1) denotes the diagonal matrix of states’ weighting factors, e

_{q}denotes unit vector of the same dimension as

**q**, e

_{u}denotes unit vector of the same dimension as

**u**, W

_{u}denotes control weighting factor, W

_{θζ}denotes weighting factor of mutual influence between θ and ζ.

#### 4.3. Simulation of Articulated Vehicles

_{1}= 3.8 m, L

_{2}= 7.57 m, e

_{1}= 0.47 m. Equations (28), (31), (33), (34), (40), and (42) are used for model-based prediction, and all the symbols correspond to those ones in figures.

_{q}= diag(1,1,1,1,0,1) denotes diagonal matrix of states’ weighting factors, and other symbols are as previously defined.

_{q}= diag(1,1,1,0,0,0,0) denotes diagonal matrix of states’ weighting factors, and W

_{θζ}denotes weighting factor of mutual influence between θ and ζ.

#### 4.4. Results and Discussion

**q**, however, it is more expedient to choose a position at which the distance to the destination is minimal and the complexity of the maneuver is quite high. Due to the linear form of the objective function Equation (58) the car speed v (Figure 3b) may change a sign, which corresponds to shifting vehicle direction for better adapting to local space. When approaching a sharp edge of the pocket (Figure 3a) at the sixth second, the speed module value decreases. The graph of the steering angle θ shows an intensive adaptation of the vehicle angular position using the full range of steering angle. Nevertheless, the resulting output parameters of the car angular and linear displacements X, Y, φ show smooth properties (Figure 3b), which in general can characterize the parking process as stable.

_{0}_{C0}, β

_{Cf}in Table 1. In this example, there is no hard restriction on the desired speed value and therefore the speed v varies in the range of 5-6 m/s, with practically zero acceleration a. The average value of the steering angle is kept at the level of 18°, and the fluctuations are stipulated by the deterministic tie with a speed change. Nevertheless, the output characteristic of the yaw angle is almost linear, which demonstrates the stability and constancy of the yaw rate.

_{θ}, ω

_{ζ}of steering wheels, as well as their turning angles θ and ζ, are in antiphase within a wide range of admissible values.

_{θ}, ω

_{ζ}and the steering angles θ and ζ are explained by the influence of the truck long wheelbase and by the simultaneous control of two axles, which brings an oversteer tendency to the kinematic model. Nevertheless, the output linear displacements X, Y, φ are stable and represented by smooth curves, without any ambiguity in curvature changes.

_{θ}fluctuates, however, the average value of steered wheels’ angle is about θ = 16 °, and the output linear and angular displacements X, Y, φ are represented by smooth curves, including a stable folding angle ψ close to a constant value. In the case of TSV-SSA, the situation is tougher. The vehicle is situated in a very narrow corridor to be accommodated. The swept path should be minimal. As seen in Figure 8d, in this case, all the control parameters ω

_{θ}, ω

_{ζ}and the states θ, ζ, and ψ are practically invariable within the prediction horizon.

## 5. Conclusions

- Based on the positive results in all the simulations, the use of kinematic models’ trajectories for the tracking is quite suitable for low speeds, when the trajectories are supposed to be represented by smooth curves. However, the shape of control signals reflects, to a greater measure, the disadvantages of kinematic models’ indirect control (by acceleration) and to a lesser measure reflects the direct control parameter (throttle position and power).
- The presented original algorithms consider the indirect parameter - the intrusion into a vehicle safety contour (or the excess of a pre-set level by control points) to model the inequality constraints. The proposed idea has shown the adequate accuracy in assessing the inadmissible distances to a vehicle body. The simplicity and versatility can be marked as an advantage of the proposed method, and the fact that it is a part of the optimization process and not just a geometric technique. The proposed technique can also be easily used for simulating the avoidance of moving and stationary obstacles.
- The cost function form significantly affects the forecast, depending on the accepted optimality criteria. The advantage of the specified NMPC is the ability to use any functions, both linear and non-linear, and their combinations. Unlike the lane change at high speeds, where the smoothness is required and quadratic forms are frequently used, the linear functions and quasi-optimal solutions are often quite adequate at low speeds. Thus, it was revealed that the cost function’s linear components work better where changes of vehicle model speed’s signs are expected, and a shorter maneuvering path is needed. Quadratic forms provide more smoothed control and allow better coordination of combined control (the case of several steered axles).
- This project may be considered as a test phase of a comprehensive study of parking/docking algorithms for autonomous vehicles. The results have argued the applicability of kinematic models and the quality of forecast in general. Within the expansion of elaborating the automated parking algorithms, it is planned to include the following issues: mapping the parking space using the SLAM methods, improving the constraint evaluation algorithm to an adaptive level, creating and testing the alternative algorithms for constraints, developing dynamic vehicle models with real-world control parameters, implementing nonlinear and adaptive MPC methods for the tracking task, and combining the parking computing techniques into one automated option for HIL-testing.

## Author Contributions

## Funding

## Acknowledgments

## Conflicts of Interest

## Abbreviations

AV | Autonomous vehicle |

CAV | Conventional articulated vehicle |

EKF | Extended Kalman filter |

HIL | Hardware-in-the-loop |

NMPC | Nonlinear model predictive control |

SLAM | Simultaneous localization and mapping |

SQP | Sequential quadratic programming |

TSV | Tractor-semitrailer vehicle |

TSV-SSA | Tractor- semitrailer vehicle with semitrailer’s steered axles |

## References

- Lee, B.; Wei, Y.; Guo, I.Y. Automatic parking of self-driving car based on Lidar. International Archives of the Photogrammetry. Remote Sens. Spat. Inf. Sci.
**2017**, 42, 241–246. [Google Scholar] - Lee, H.; Chun, J.; Jeon, K. Autonomous back-in parking based on occupancy grid map and EKF SLAM with W-band radar. In Proceedings of the International Conference on Radar (RADAR), Brisbane, Australia, 27–30 August 2018; pp. 1–4. [Google Scholar]
- Lin, L.; Zhu, J.J. Path planning for autonomous car parking. In Proceedings of the ASME Dynamic Systems and Control Conference, Atlanta, GA, USA, 30 September–October 3 2018; Volume 3. [Google Scholar]
- Pérez-Morales, D.; Domínguez-Quijada, S.; Kermorgant, O.; Martinet, P. Autonomous parking using a sensor-based approach. In Proceedings of the IEEE 19th International Conference on Intelligent Transportation Systems (ITSC), Rio de Janeiro, Brazil, 1–4 November 2016. [Google Scholar]
- Luca, R.; Troester, F.; Gall, R.; Simion, C. Environment mapping for autonomous driving into parking lots. In Proceedings of the IEEE International Conference on Automation, Quality and Testing, Robotics (AQTR), Cluj-Napoca, Romania, 28–30 May 2010; pp. 1–6. [Google Scholar]
- Zhou, J.; Navarro-Serment, L.E.; Hebert, M. Detection of parking spots using 2D range data. In Proceedings of the IEEE International Conference on Intelligent Transportation Systems, Anchorage, AK, USA, 16–19 September 2012; pp. 1280–1287. [Google Scholar]
- Heinen, M.R.; Osorio, F.S.; Heinen, F.J.; Kelber, C. SEVA3D: Using artificial neural networks to autonomous vehicle parking control. In Proceedings of the IEEE International Joint Conference on Neural Network, Vancouver, BC, Canada, 16–21 July 2006; pp. 4704–4711. [Google Scholar]
- Kiss, D.; Tevesz, G. Autonomous path planning for road vehicles in narrow environments: An efficient continuous curvature approach. J. Adv. Transp. Intell. Auton. Transp. Syst. Des. Simul.
**2017**. [Google Scholar] [CrossRef] [Green Version] - Wang, Y.; Jha, D.K.; Akemi, Y. A two-stage RRT path planner for automated parking. In Proceedings of the 13th IEEE Conference on Automation Science and Engineering, Xi’anChina, 20–23 August 2017; pp. 496–502. [Google Scholar]
- Ballinas, E.; Montiel, O.; Castillo, O.; Rubio, Y.; Aguilar, L.T. Automatic parallel parking algorithm for a car-like robot using fuzzy PD+1. Control. Eng. Lett.
**2018**, 26, 447–454. [Google Scholar] - Petrov, P.; Fawzi, N. Automatic vehicle perpendicular parking design using saturated control. In Proceedings of the IEEE Jordan Conference on Applied Electrical Engineering and Computing Technologies, Amman, Jordan, 3–5 November 2015; pp. 1–6. [Google Scholar]
- Gupta, A.; Rohan, D.; Mohit, A. Autonomous parallel parking system for Ackerman steering four wheelers. In Proceedings of the IEEE International Conference on Computational Intelligence and Computing Research, Coimbatore, India, 28–29 December 2010; pp. 1–6. [Google Scholar]
- Tazaki, Y.; Okuda, H.; Suzuki, T. Parking Trajectory Planning Using Multiresolution State Roadmaps. IEEE Trans. Intell. Veh.
**2017**, 2, 298–307. [Google Scholar] [CrossRef] - Suhr, J.K.; Jung, H.G. Automatic parking space detection and tracking for underground and indoor environments. IEEE Trans. Ind. Electron.
**2016**, 63, 5687–5698. [Google Scholar] [CrossRef] - Diachuk, M.; Easa, S.M.; Hassein, U.; Shihundu, D. Modeling passing maneuver based on vehicle characteristics for in-vehicle collision warning systems on two-lane highways. Transp. Res. Record
**2019**, 2673, 165–178. [Google Scholar] [CrossRef] - Diachuk, M.; Easa, S.M. Guidelines for roundabout circulatory and entry widths based on vehicle dynamics. J. Traffic Transp. Eng.
**2018**, 5, 361–371. [Google Scholar] [CrossRef] - Garcia, C.E.; Prett, D.M.; Morari, M. Model predictive control: Theory and practice—A survey. Automatica
**1989**, 25, 335–348. [Google Scholar] [CrossRef] - Lu, Y.; Arkun, Y. Quasi-Min-Max MPC algorithms for LPV systems. Automatica
**2000**, 36, 527–540. [Google Scholar] [CrossRef] - MathWorks. Available online: www.mathworks.com (accessed on 1 August 2019).

**Figure 1.**Kinematics of curvilinear motion for different types of vehicle design: (

**a**) Passenger car; (

**b**) Single truck with steered rear axle; (

**c**) Conventional tractor-semitrailer vehicle (TSV); (

**d**) Tractor- semitrailer vehicle with semitrailer’s steered axles (TSV-SSA).

**Figure 2.**Determining mutual disposition between vehicle safe contour and parking area boundary: (

**a**) Scheme of the general idea; (

**b**) Vehicle’s safe contour control points; (

**c**) Control points due to vehicle translational motion; (

**d**) Control points due to vehicle rotation.

**Figure 3.**Simulation results for reverse parking of a passenger car: (

**a**) Position and trajectory (parallel reverse); (

**b**) Basic parameters (parallel reverse); (

**c**) Position and trajectory (perpendicular reverse); (

**d**) Basic parameters (perpendicular reverse).

**Figure 4.**Simulation results for perpendicular forward parking and circular motion of a passenger car: (

**a**) Position and trajectory (perpendicular forward); (

**b**) Basic parameters (perpendicular forward); (

**c**) Car positions and planned trajectories (circular); (

**d**) Car’s output and control parameters (circular).

**Figure 5.**Single truck automated maneuvering simulation in a parking area with restricted space: (

**a**) Vehicle positions and planned trajectories for perpendicular reverse parking; (

**b**) Basic output and control parameters for perpendicular reverse parking; (

**c**) Vehicle positions and planned trajectories for changing position on the spot; (

**d**) Basic output and control parameters for changing position on the spot.

**Figure 6.**Simulation results for the single truck circular motion: (

**a**) Truck positions and planned trajectories; (

**b**) Truck output and control parameters.

**Figure 7.**Simulation results for the docking of articulated vehicles: (

**a**) Position and trajectory (conventional TSV); (

**b**) Basic parameters (conventional TSV); (

**c**) Position and trajectory (TSV-SSA); (

**d**) Basic parameters (TSV-SSA).

**Figure 8.**Simulation results for the circular motion of articulated vehicles: (

**a**) Position and trajectory (conventional TSV); (

**b**) Basic parameters (conventional TSV); (

**c**) Position and trajectory (TSV-SSA); (

**d**) Basic parameters (TSV-SSA).

Type of Motion | Restrictions ^{1} | Initial Conditions |
---|---|---|

Parallel reverse parking | −40 ° ≤ θ ≤ 40 °; −2 m/s ≤ v ≤ 2 m/s; −34 °/s ≤ ω _{θ} ≤ 34 °/s; −1 m/s^{2} ≤ a ≤ 1 m/s^{2} | T_{s} = 1 s; p = 14; q_{0} = (0, 0, 0, 0, 0)^{T};q _{f} = (−7.65, −5, 0, 0, 0)^{T} |

Perpendicular reverse parking | −40 ° ≤ θ ≤ 40 °; −2 m/s ≤ v ≤ 2 m/s; −34 °/s ≤ ω _{θ} ≤ 34 °/s; −1 m/s^{2} ≤ a ≤ 1 m/s^{2} | T_{s} = 0.5 s; p = 14; q_{0} = (0, 0, 0, 0, 0)^{T};q _{f} = (−5.5, −6.8, π/2, 0, 0)^{T} |

Perpendicular forward parking | −40 ° ≤ θ ≤ 40 °; −2 m/s ≤ v ≤ 2 m/s; −34 °/s ≤ ω _{θ} ≤ 34 °/s; −1 m/s^{2} ≤ a ≤ 1 m/s^{2} | T_{s} = 1 s; p = 16; q_{0} = (0, 0, 0, 0, 0)^{T};q _{f} = (−5.5, −6.8, −π/2, 0, 0)^{T} |

Circular motion ^{2,3} | −40 ° ≤ θ ≤ 40 °; R _{out} = 10 m, H = 2.3 m;R _{in} = R_{out} – H;−28 °/s ≤ ω _{θ} ≤ 28 °/s;0.95·v _{des} m/s ≤ v ≤ 1.25·v_{des} m/s;−2 m/s ^{2} ≤ a ≤ 2.5 m/s^{2} | T_{s} = 1 s; p = 16; β_{C0} = −π·7/9;β _{Cf} = π·5/6;q_{0} = (R_{av}·cos(β_{C0}), R_{av}·sin(β_{C0}),π/2 +β _{C0}, arctan(2·L/R_{av}), v_{des})^{T};q_{f} = (R_{av}·cos(β_{Cf}), R_{av}·sin(β_{Cf}),π/2 +β _{Cf}, arctan(2·L/R_{av}), v_{des})^{T} |

^{1}v

_{des}= 5 m/s (desirable circulating speed).

^{2}R

_{av}= (R

_{out}+ R

_{in})/2.

^{3}R

_{av}= (R

_{out}+ R

_{in})/k, where k = 2.15.

Type of Motion | Restrictions ^{1} | Initial Conditions |
---|---|---|

Perpendicular reverse parking | −40° ≤ θ ≤ 40°; −30° ≤ ζ ≤ 30; −2 m/s ≤ v ≤ 2 m/s; −6 °/s ≤ ω _{θ} ≤ 6 °/s;−6 °/s ≤ ω _{ζ} ≤ 6 °/s; −0.7 m/s ^{2} ≤ a ≤ 0.7 m/s^{2} | T_{s} = 3 s; p = 6;q_{0} = (0, 0, 0, 0, 0, 0)^{T};q_{f} = (−18, −15, π/2, 0, 0, 0)^{T} |

Parking with changing position on the spot | −40° ≤ θ ≤ 40°; −30° ≤ ζ ≤ 30; −2 m/s ≤ v ≤ 2 m/s; −28 °/s ≤ ω _{θ} ≤ 28 °/s;−28 °/s ≤ ω _{ζ} ≤ 28 °/s;−1.5 m/s ^{2} ≤ a ≤ 1.5 m/s^{2} | T_{s} = 1 s; p = 18;q_{0} = (0, 0, 0, 0, 0, 0)^{T};q_{f} = (0, 0, π/2, 0, 0, 0)^{T} |

Circular motion ^{2,3} | −40° ≤ θ ≤ 40°; −30°≤ ζ ≤ 30°; 0.95·v _{des} m/s ≤ v ≤ 1.25·v_{des} m/s;R _{out} = 15 m, H = 4.5 m;R _{in} = R_{out} – H;−28 °/s ≤ ω _{θ} ≤ 28 °/s;−28 °/s ≤ ω _{ζ} ≤ 28 °/s;−2 m/s ^{2} ≤ a ≤ 2.0 m/s^{2} | T_{s} = 1 s; p = 13;β _{C0} = −135°; β_{Cf} = 140°;q_{0} = (R_{av}·cos(β_{C0}), R_{av}·sin(β_{C0}), π/2 +β_{C0}, arctan(2·L_{10}/R_{av}),−arctan(2·l _{10}/R_{av}), v_{des})^{T};q_{f} = (R_{av}·cos(β_{Cf}), R_{av}·sin(β_{Cf}),π/2 +β _{Cf}, arctan(2· L_{10}/R_{av}),−arctan(2·l _{10}/R_{av}), v_{des})^{T} |

^{1}v

_{des}= 5 m/s (desirable circulating speed).

^{2}R

_{av}= (R

_{out}+ R

_{in})/2.

^{3}R

_{av}= (R

_{out}+ R

_{in})/k, where k = 2.15.

Type of Automated Vehicle (AV) | Type of Motion | Restrictions ^{1} | Initial Conditions |
---|---|---|---|

Conventional TSV | Docking (unconstrained space) | −90° ≤ ψ ≤ 90°; −45° ≤ θ ≤ 45°; −4 m/s ≤ v ≤ 4 m/s; −34 °/s ≤ ω _{θ} ≤ 34 °/s;−2.0 m/s ^{2} ≤ a ≤ 2.5 m/s^{2} | T_{s} = 1 s; p = 12;q_{0} = (0, 0, π/2, 0, 0, 0)^{T};q_{f} = (−5, −35, π, 0, 0, 0)^{T} |

Circular Motion ^{2} | Inequality constraint of Equation (56); R _{out} = 15 m, H = 5 m;R _{in} = R_{out} – H;−40° ≤ ψ ≤ 40°; −45° ≤ θ ≤ 45°; 0.95·v _{des} m/s ≤ v ≤ 1.25·v_{des} m/s;−34 °/s ≤ ω _{θ} ≤ 34 °/s;−0.5 m/s ^{2} ≤ a ≤ 0.5 m/s^{2} | T_{s} = 1 s; p = 9; β_{C0} = −160°; β_{Cf} = 140°;q_{0} = (R_{av}·cos(β_{C0}), R_{av}·sin(β_{C0}),π/2 +β _{C0}, π·11/60,arctan(2·L _{1}/R_{av}), v_{des})^{T};q_{f} = (R_{av}·cos(β_{Cf}), R_{av}·sin(β_{Cf}),π/2+β _{Cf}, π·11/60,arctan(2·L _{1}/R_{av}), v_{des})^{T} | |

TSV-SSA | Docking (unconstrained space) | −90° ≤ ψ ≤ 90°; −40° ≤ θ ≤ 40°; −35° ≤ ζ ≤ 35°; −4 m/s ≤ v ≤ 4 m/s; −34 °/s ≤ ω _{θ} ≤ 34 °/s;−34 °/s ≤ ω _{ζ} ≤ 34 °/s;−2.0 m/s ^{2} ≤ a ≤ 2.5 m/s^{2} | T_{s} = 1 s;p = 12; q_{0} = (0, 0, π/2, 0, 0, 0, 0)^{T};q_{f} = (−5, −35, π, 0, 0, 0, 0)^{T} |

Circular motion ^{3} | Inequality constraint of Equation (56); R _{out} = 15 m, H = 4 m;R _{in} = R_{out} – H;30° ≤ ψ ≤ 35°; 15° ≤ θ ≤ 20°; −15° ≤ ζ ≤ −10°; 0.95·v _{des} m/s ≤ v ≤ 1.05·v_{des} m/s;−34 °/s ≤ ω _{θ} ≤ 34 °/s;−28 °/s ≤ ω _{ζ} ≤ 28 °/s;−0.5 m/s ^{2} ≤ a ≤ 0.5 m/s^{2} | T_{s} = 1 s; p = 9; β_{C0} = -100; β_{Cf} = 200°;q_{0} = (R_{av}·cos(β_{C0}), R_{av}·sin(β_{C0}),π/2 +β _{C0}, π/6, arctan(2·L_{1}/R_{av}),-arctan(2·L _{1}/R_{av}), v_{des})^{T};q_{f} = (R_{av}·cos(β_{Cf}), R_{av}·sin(β_{Cf}),π/2 +β _{Cf}, π/6, arctan(2·L_{1}/R_{av}),-arctan(2·L _{1}/R_{av}), v_{des})^{T} |

^{1}v

_{des}= 8 m/s (desirable circulating speed).

^{2}R

_{av}= (R

_{out}+ R

_{in})/k, where k = 1.95.

^{3}R

_{av}= (R

_{out}+ R

_{in})/k, where k = 2.025.

© 2020 by the authors. Licensee MDPI, Basel, Switzerland. This article is an open access article distributed under the terms and conditions of the Creative Commons Attribution (CC BY) license (http://creativecommons.org/licenses/by/4.0/).

## Share and Cite

**MDPI and ACS Style**

Diachuk, M.; Easa, S.M.; Bannis, J.
Path and Control Planning for Autonomous Vehicles in Restricted Space and Low Speed. *Infrastructures* **2020**, *5*, 42.
https://doi.org/10.3390/infrastructures5050042

**AMA Style**

Diachuk M, Easa SM, Bannis J.
Path and Control Planning for Autonomous Vehicles in Restricted Space and Low Speed. *Infrastructures*. 2020; 5(5):42.
https://doi.org/10.3390/infrastructures5050042

**Chicago/Turabian Style**

Diachuk, Maksym, Said M. Easa, and Joel Bannis.
2020. "Path and Control Planning for Autonomous Vehicles in Restricted Space and Low Speed" *Infrastructures* 5, no. 5: 42.
https://doi.org/10.3390/infrastructures5050042