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

Abstract

This paper presents models of path and control planning for the parking, docking, and movement of autonomous vehicles at low speeds, considering space constraints. Given the low speed of motion, and in order to test and approve the proposed algorithms, vehicle kinematic models are used. Recent works on the development of parking algorithms for autonomous vehicles are reviewed. Bicycle kinematic models for vehicle motion are considered for three basic types of vehicles: passenger car, long wheelbase truck, and articulated vehicles with and without steered semitrailer axes. Mathematical descriptions of systems of differential equations in matrix form and expressions for determining the linearization elements of nonlinear motion equations that increase the speed of finding the optimal solution are presented. Options are proposed for describing the interaction of vehicle overall dimensions with the space boundaries, within which a maneuver should be performed. An original algorithm that considers numerous constraints is developed for determining vehicle permissible positions within the closed boundaries of the parking area, which are directly used in the iterative process of searching for the optimal plan solution using nonlinear model predictive control (NMPC). The process of using NMPC to find the best trajectories and control laws while moving in a semi-limited space of constant curvature (turnabouts, roundabouts) are described. Simulation tests were used to validate the proposed models for both constrained and unconstrained conditions and the output (state-space) and control parameters’ dependencies are shown. The proposed models represent an initial effort to model the movement of autonomous vehicles for parking and have the potential for other highway applications.

1. Introduction

Automated parking within the framework of the general task of autonomous vehicles aims to eliminate the influence of human factors, improving the quality and accuracy of control, and reducing the time and quantity of maneuvers by optimizing vehicle path in restricted parking zones [1,2,3]. The advantages of autonomous parking are not only eliminating routine driver actions, often requiring increased attention and responsibility, but also achieving significant economic benefits, especially for closed areas. The automation of vehicle parking allows one to reduce the lateral distances between parking spaces since there is no more need for opening car doors exactly on a parking spot (a car moves to a parking place autonomously without passengers). A narrow space contributes not only to an increase in parking places but also to a reduction in the unit cost for a parking spot during the construction of parking lots in buildings. The latter is especially relevant due to the promising technologies of vehicle to anything technology, which provides communication between a vehicle and a building through data exchange to find an unoccupied place, to generate the route, and to retain support while moving to a destination. For large-sized trucks, the exceptional importance lies in predicting stable and safe passing on road curved sections, forecasting precise control for docking, followed by discharging and minimizing total control in the case of multiple parameters.

Numerous approaches that consider different control strategies, sensory means, and prediction algorithms have been developed regarding automated parking. Those approaches demonstrate certain similarity in parking modeling. The study by Pérez-Morales et al. [4] is dedicated to perpendicular automated parking with a sensor-based control and weighted control scheme, positioning itself as one of the first attempts in this area. The research included real experiments using the Velodyne VLP-16 compact Lidar, and one part of the study was devoted to the data extraction with followed by determining the loose parking space. The two control methods were considered for kinematic vehicle model: geometric path planning and sensor-based control with evaluating the weighting factors. Despite the fact of using a Lidar and real-world data, some results were unclear, showing the duration of quite simple maneuver about 40 s and the use of steered wheels’ turning without vehicle motion. There was also a pretty big difference between the planned speed and the actual one according to the experiment. Lee et al. [2] proposed using the extended Kalman filter (EKF) with simultaneous localization and mapping (SLAM) algorithm and occupancy grid mapping method for the automated backward perpendicular parking. The authors assert this approach may increase the accuracy for estimating the radar positioning to form a grid map. An algorithm reducing the computational complexity by thresholding the landmark recognition and adaptive changing the state vector length is considered. The scattering extraction using the orthogonal matching pursuit from electric field data is utilized for making realistic simulation of car model’s radar measurements. The authors remark the real-time efficiency of the proposed algorithm improvements.

The study by Lee et al. [1] considers automated parking algorithms for self-driving cars equipped with a lidar such as HDL-32E. Based on 3D point cloud extraction, a parking zone is proposed to be preprocessed for defining the minimum parking space. The path prognosis is based on vehicle dynamics and collision constraints. The fuzzy-logic controller is proposed to be used for controlling the brakes and throttle to sustain stable vehicle speed. The test results obtained by engaging a self-driving car showed the feasibility and efficiency of proposed system for parallel and perpendicular variants of parking. Luca et al. [5] investigated the environment mapping for the case of robotic car parking. The laser scanner SICK LD1000 and ultrasonic sensors perform reliable data for map generation. Implementing evolutive algorithms, the data are being converted into lines denoting the edges of surrounding objects to simplify the parking zone environment. Due to the map’s dynamic evolution while vehicle moving, the data are being checked on merging and fitting by applying a shape correlation, followed by correction. The Embedded Matlab/Simulink Software and the PC104 system are used for testing the navigation and path determination in real-time.

The study by Zhou et al. [6] distinguishes the problem of parking spots’ detection reliability in semi-filled parking lots. The parking zone is supposed to be estimated using onboard laser scanner. The proposed approach aims identifying vehicle bumpers using a supervised learning technique. The classifier is to be trained for recognizing data segments as car bumpers. The developed algorithm creates a topological graph interpreting the parking space to be analyzed. The authors state the algorithm performance has been confirmed by a series of experimental tests. Heinen et al. [7] described a system for implementing the intelligent control for autonomous vehicles. The developed system allegedly can drive a vehicle providing a robust control for parallel automated parking. The sonar sensors are being read for processing the data by a neural network giving steering and accelerating signals. The authors state the proposed controller is perfectly working to park a vehicle in different situations.

Kiss and Tevesz [8] presented a combined approximate method for solving the path planning problem in narrow environments. The approach consists of a global planner that generates a preliminary path consisting of straight and turning-in-place primitives and a local planner that is used to make the preliminary path feasible to car-like vehicles. Lin and Zhu [3] developed a path planner based on a novel four-phase algorithm. By using some switching control laws to drive two virtual cars to a target line, the forward and reverse paths were obtained. Then, the two paths are connected along the target line. The proposed path planning algorithm was fast, highly autonomous, sufficiently general, and could be used in tight environments. Wang et al. [9] proposed a two-stage rapid random-tree (RRT) algorithm to improve computational efficiency. First, the proposed algorithm performs space exploration and establishes prior knowledge represented as waypoints, using cheap computation. Second, a waypoint guided RRT algorithm, with a sampling scheme biased by the waypoints, constructs a kinematic tree connecting the initial and goal configurations. Other researchers also made substantial contributions to the development of parking algorithms. Several ideas considering parking control can be found in such articles as Ballinas et al. [10], Petrov and Nashshibi [11], Gupta et al. [12], Tazaki at al. [13], and Suhr and Jung [14]. Some numerical methods implemented in this paper, such as nonlinear model predictive control (NMPC), have been used in other applications [15,16].

Most of the preceding studies are devoted to the identification of parking zone space and the use of geometric, fuzzy, neural, and other algorithms for predicting vehicle parking path. Many of the studies were accompanied by experiments based on small-scale models or real vehicles. However, most often, for completing a maneuver, the vehicle initial position is pre-set. Thus, clearly there is a lack of research in this field related to vehicle path and control optimization from any current position in restricted space. The purpose of this paper is to develop and test algorithms for parking and docking, based on kinematic vehicle models and nonlinear optimization within limited and unlimited spaces. Composing the unique technique for developing the nonlinear constraints of restricted parking was the main task for applying NMPC in this research.

The next section presents the modelling of autonomous motions of single and articulated vehicles. The following section presents the proposed optimization model, including the basic model and operational, and physical constraints. The implementation of the optimization model is then illustrated using simulation, followed by the conclusions.

2. Kinematic Models of Autonomous Motion

Kinematic vehicle models (Figure 1) are used in this study instead of dynamic models. Kinematic models assume that no slip occurs between the wheels and the road. This assumption is accurate for vehicles moving at low speeds, which is the case for the parking cases developed in this paper. In addition, although dynamic models are generally more accurate, they involve many degrees of freedom that make the model more complex.

2.1. Single Vehicle

The motion of a single vehicle can be represented as a superposition of elementary rotations around the instantaneous center of velocities. In this case, the vectors of wheels’ translational speeds will be strictly placed in the planes of their rotation. The minimum turning radius for a passenger car model (Figure 1a) passes through the rotational axis of rear wheels and its place is fixed. Thus, the position of the center O is determined only by turning the front steered wheel. This, in turn, affects the excessive sensitivity of the angular velocity ω to the turning angle of the steered wheel and vehicle’s speed. In the case of an additional controlled axis (Figure 1b), the center O is obtained by crossing the perpendiculars to the steered wheels, the ratio of which angles may be different, and therefore the position of minimum turning radius R is not fixed and constantly changes the projection point on the vehicle longitudinal axis. In this regard, the maximum longitudinal base L is constantly divided into variable components L₁ and l₁.

2.1.1. Passenger Car

In the kinematic bicycle model of a biaxial vehicle, it is assumed that the center of rotation is formed by the intersection of the perpendiculars drawn to the planes of the wheels’ rotation (Figure 1a). In this case, the angular velocity of rotation relative to the instantaneous center of velocities O:

$$\omega =v/R$$

(1)

The minimum turning radius R can be determined using the right triangle with the vertex O from the ratio of the steering wheel’s rotation angle θ:

$$tan\left(\theta \right)=L/R$$

(2)

when

$$\omega =v·tan\left(\theta \right)/L$$

(3)

The following are model state parameters q_i: 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_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_i are: ω_θ 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,

$$\mathit{q}=\left(\begin{array}{c}x\\ y\\ \varphi \\ \theta \\ v\end{array}\right), \dot{\mathit{q}}=\left(\begin{array}{c}{v}_x\\ v_y\\ {\omega }_{\varphi }\\ \dot{\theta }\\ \dot{v}\end{array}\right), \dot{\mathit{q}}=\mathit{f}\left(\mathit{q}\left(t\right),\mathit{u}\left(t\right),\mathit{p}\right), u=\left(\begin{array}{c}{\omega }_{\theta }\\ a\end{array}\right), \mathit{f}\left(\mathit{q},\mathit{u},\mathit{p}\right)=\left(\begin{array}{c}v\cdot cos\left(\varphi \right)\\ v\cdot sin\left(\varphi \right)\\ v·tan\left(\theta \right)/L \\ {\omega }_{\theta }\\ a\end{array}\right)$$

(4)

Reduce the nonlinear function f(q,u,p) to a more convenient form, separating states and controls:

$$\mathit{f}\left(\mathit{q}\left(t\right),\mathit{u}\left(t\right),\mathit{p}\right)=\mathit{f}\left(\mathit{q},\mathit{u},\mathit{p}\right)=\mathit{\phi }\left(\mathit{q},\mathit{p}\right)+B\cdot \mathit{u}$$

(5)

where

$$\mathit{\phi }\left(\mathit{q},\mathit{p}\right)=v\cdot \left(\begin{array}{c}cos\left(\varphi \right)\\ sin\left(\varphi \right)\\ tan\left(\theta \right)/L\\ 0\\ 0\end{array}\right), B=\left(\begin{array}{cc}0& 0\\ 0& 0\\ 0& 0\\ 1& 0\\ 0& 1\end{array}\right)$$

(6)

Thus,

$$\dot{\mathit{q}}=\mathit{\phi }\left(\mathit{q},\mathit{p}\right)+B\cdot \mathit{u}$$

(7)

or

$$\frac{d}{dt}\left(\begin{array}{c}x\\ y\\ \varphi \\ \theta \\ v\end{array}\right)=v\cdot \left(\begin{array}{c}cos\left(\varphi \right)\\ sin\left(\varphi \right)\\ tan\left(\theta \right)/L\\ 0\\ 0\end{array}\right)+\left(\begin{array}{cc}0& 0\\ 0& 0\\ 0& 0\\ 1& 0\\ 0& 1\end{array}\right)\cdot \left(\begin{array}{c}{\omega }_{\theta }\\ a\end{array}\right)$$

(8)

To speed up the search for the optimal solution, as well as for the possibility of using adaptive MPC, consider the linearization of Equation (7), through the expansion in a Taylor series with the first linear terms in the vicinity of point 0. Then, in vector form

$$\begin{array}{ll}{\dot{\mathit{q}}}_0\left(t\right)+\Delta \dot{\mathit{q}}\left(t\right)=& f\left({\mathit{q}}_0\left(t\right),{\mathit{u}}_0\left(t\right),\mathit{p}\right)+{\frac{\partial \mathit{f}\left(\mathit{q}\left(t\right),\mathit{u}\left(t\right),\mathit{p}\right)}{\partial \mathit{q}}|}_{\begin{array}{c}{\mathit{q}}_0\\ {\mathit{u}}_0\end{array}}\cdot \Delta \mathit{q}\left(t\right)\\ & +{\frac{\partial \mathit{f}\left(\mathit{q}\left(t\right),\mathit{u}\left(t\right),\mathit{p}\right)}{\partial \mathit{u}}|}_{\begin{array}{c}{\mathit{q}}_0\\ {\mathit{u}}_0\end{array}}\cdot \Delta \mathit{u}\left(t\right)+O^2\end{array}$$

(9)

where

$${\dot{\mathit{q}}}_0\left(t\right)=\mathit{f}\left({\mathit{q}}_0\left(t\right),{\mathit{u}}_0\left(t\right),\mathit{p}\right)$$

(10)

Obtain,

$$\Delta \dot{\mathit{q}}\left(t\right)={\frac{\partial \mathit{f}\left(\mathit{q}\left(t\right),\mathit{u}\left(t\right),\mathit{p}\right)}{\partial \mathit{q}}|}_{\begin{array}{c}{\mathit{q}}_0\\ {\mathit{u}}_0\end{array}}\cdot \Delta \mathit{q}\left(t\right)+{\frac{\partial \mathit{f}\left(\mathit{q}\left(t\right),\mathit{u}\left(t\right),\mathit{p}\right)}{\partial \mathit{u}}|}_{\begin{array}{c}{\mathit{q}}_0\\ {\mathit{u}}_0\end{array}}\cdot \Delta \mathit{u}\left(t\right)$$

(11)

where

$$A\left(t\right)={\frac{\partial \mathit{f}\left(\mathit{q}\left(t\right),\mathit{u}\left(t\right),\mathit{p}\right)}{\partial \mathit{q}}|}_{\begin{array}{c}{\mathit{q}}_0\\ {\mathit{u}}_0\end{array}}, B\left(t\right)={\frac{\partial \mathit{f}\left(\mathit{q}\left(t\right),\mathit{u}\left(t\right),\mathit{p}\right)}{\partial \mathit{u}}|}_{\begin{array}{c}{\mathit{q}}_0\\ {\mathit{u}}_0\end{array}}$$

(12)

Then, the linearized equation in increments is given by

$$\Delta \dot{\mathit{q}}=A\cdot \Delta \mathit{q}+B\cdot \Delta \mathit{u}$$

(13)

The matrix A is the Jacobian, which is given by

$$J=\left(\begin{array}{ccc}\frac{\partial {\mathit{f}}_1}{\partial q_1}& \cdots & \frac{\partial {\mathit{f}}_1}{\partial q_n}\\ ⋮& \ddots & ⋮\\ \frac{\partial {\mathit{f}}_n}{\partial q_1}& \cdots & \frac{\partial {\mathit{f}}_n}{\partial q_n}\end{array}\right)$$

(14)

Substituting Equation (3) into Equation (14) yields

$$A=\left(\begin{array}{ccccc}0& 0& -v\cdot sin\left(\varphi \right)& 0& cos\left(\varphi \right)\\ 0& 0& v\cdot cos\left(\varphi \right)& 0& sin\left(\varphi \right)\\ 0& 0& 0& \frac{v\cdot \left(ta{n}^2\left(\theta \right)+1\right)}{L}& \frac{tg\left(\theta \right)}{L}\\ 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 0\end{array}\right), B=\left(\begin{array}{cc}0& 0\\ 0& 0\\ 0& 0\\ 1& 0\\ 0& 1\end{array}\right)$$

(15)

2.1.2. Long Truck with Steered Rear Axle

Unlike the kinematic bicycle model of a biaxial vehicle, it is assumed that in the case of an auxiliary rear steered axle, the perpendicular dropped from the center O to the vehicle’s longitudinal axis and containing the minimum turning radius is floating relative to an intersection point and depends on the ratio of rotation angles of the front and rear axles’ steered wheels (Figure 1b). The angular velocity of rotation relative to the instantaneous center O is determined to be similar to Equation (1). The minimum turning radius R can be determined in two ways, via right angle triangles with a vertex O. From the ratio of the steering angle θ of the front steered wheel:

$$tan\left(\theta \right)=L_1/R$$

(16)

From the ratio of the steering angle ζ of the rear steered wheel:

$$tan\left(\zeta \right)=l_1/R$$

(17)

Then

$$L_1=R\cdot tan\left(\theta \right), l_1=R\cdot tan\left(\zeta \right), L_1-l_1=R\cdot \left(tan\left(\theta \right)-tan\left(\zeta \right)\right)$$

(18)

Given that the coordinate l₁ is negative with respect to the intersection point of radius R, then

$$L_1-l_1=L_1-\left(-\left|l_1\right|\right)=L_1+l_1=L$$

(19)

Thus,

$$R=\frac{L}{\left(tan\left(\theta \right)-tan\left(\zeta \right)\right)}$$

(20)

The expression for the angular velocity is written as

$$\omega =\frac{v}{R}=\frac{v\cdot \left(tan\left(\theta \right)-tan\left(\zeta \right)\right)}{L}$$

(21)

Based on the described and established parameters in Equation (4) for a passenger car, one state ζ (vehicle’s rear axle steering angle) and one control parameter ω_ζ (vehicle’s rear axle steering rate) should be included to develop the state-space vector for a long single truck:

$$\mathit{q}=\left(\begin{array}{c}x\\ y\\ \varphi \\ \theta \\ \zeta \\ v\end{array}\right), \dot{\mathit{q}}=\left(\begin{array}{c}{v}_x\\ v_y\\ {\omega }_{\varphi }\\ \dot{\theta }\\ \dot{\zeta }\\ \dot{v}\end{array}\right), \mathit{u}=\left(\begin{array}{c}{\omega }_{\theta }\\ {\omega }_{\zeta }\\ a\end{array}\right), \dot{\mathit{q}}=\mathit{f}\left(\mathit{q}\left(t\right),\mathit{u}\left(t\right),\mathit{p}\right)$$

(22)

According to Equation (5),

$$\mathit{\phi }\left(\mathit{q},\mathit{p}\right)=v\cdot \left(\begin{array}{c}cos\left(\varphi \right)\\ sin\left(\varphi \right)\\ \frac{\left(tan\left(\theta \right)-tan\left(\zeta \right)\right)}{L}\\ 0\\ 0\\ 0\end{array}\right), B=\left(\begin{array}{ccc}0& 0& 0\\ 0& 0& 0\\ 0& 0& 0\\ 1& 0& 0\\ 0& 1& 0\\ 0& 0& 1\end{array}\right)$$

(23)

Thus,

$$\frac{d}{dt}\left(\begin{array}{c}x\\ y\\ \varphi \\ \theta \\ \zeta \\ v\end{array}\right)=v\cdot \left(\begin{array}{c}cos\left(\varphi \right)\\ sin\left(\varphi \right)\\ \frac{\left(tan\left(\theta \right)-tan\left(\zeta \right)\right)}{L}\\ 0\\ 0\\ 0\end{array}\right)+\left(\begin{array}{ccc}0& 0& 0\\ 0& 0& 0\\ 0& 0& 0\\ 1& 0& 0\\ 0& 1& 0\\ 0& 0& 1\end{array}\right)\cdot \left(\begin{array}{c}{\omega }_{\theta }\\ {\omega }_{\zeta }\\ a\end{array}\right)$$

(24)

To obtain the Jacobian, denote,

$$c_{\varphi }=cos\left(\varphi \right), s_{\varphi }=sin\left(\varphi \right), t_{\theta }=tan\left(\theta \right), t_{\zeta }=tan\left(\zeta \right)$$

(25)

Then, the matrix A (6×6) has the following nonzero elements:

$$\begin{array}{c}{A}_{1,3}=-v\cdot s_{\varphi }, A_{2,3}=v\cdot c_{\varphi }, A_{3,4}=\frac{v\cdot \left(t_{\theta }^2+1\right)}{L},\\ A_{3,5}=\frac{-v\cdot \left(t_{\zeta }^2+1\right)}{L}, A_{1,6}=c_{\varphi }, A_{2,6}=s_{\varphi }, A_{3,6}=\frac{\left(t_{\theta }-t_{\zeta }\right)}{L}\end{array}$$

(26)

The matrix B remains unchanged.

2.2. Articulated Vehicles

In the case of a conventional articulated vehicle (CAV), by an analogy described in the previous paragraph, two links are being considered whose minimum radii R₁ and R₂ are crossing in the center of O. Moreover, in the same way, the radius R₂ 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₁. Since ψ is the articulation angle, R₁ and R₂ will also be located at the angle ψ to each other.

While controlling the semitrailer axles, as in the case of a long-base single lorry, the position of the floating point of intersection of the radius R₂ and the longitudinal axis of the semitrailer is determined by dividing the base L₂ onto L’₂ and l’₂.

2.2.1. Conventional Tractor-Semitrailer Vehicle

Consider the kinematic bicycle model of a tractor-semitrailer vehicle (TSV). The rotation center is assumed to be formed by the intersection of perpendiculars drawn to the rotational planes of the wheels (Figure 1c). In this case, the angular velocity of the tractor’s rotation relative to the instantaneous center of velocities O will be ω₁, and the angular velocity of the semitrailer ω₂:

$${\omega }_1=v_1/R_1, {\omega }_2=v_2/R_2$$

(27)

The following are model state parameters q_i: 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_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_i are: ω_θ 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₁ denotes tractor wheelbase, e₁ denotes fifth wheel offset relative to the tractor’s rear axle (positive if within wheelbase, negative if shifted behind the rear axle), and L₂ denotes semitrailer wheelbase, which is the distance from the coupling center (kingpin) to the conditional middle axle. Considering that ψ = φ₁ – φ₂, dψ/dt = ω₁ – ω₂, then in vector form,

$$\mathit{q}=\left(\begin{array}{c}x\\ y\\ \varphi \\ \psi \\ \theta \\ v\end{array}\right), \mathit{p}=\left(\begin{array}{c}{L}_1\\ e_1\\ L_2\end{array}\right), \dot{\mathit{q}}=\left(\begin{array}{c}{v}_x\\ v_y\\ {\omega }_{\varphi }\\ \dot{\psi }\\ \dot{\theta }\\ \dot{v}\end{array}\right), \mathit{u}=\left(\begin{array}{c}{\omega }_{\theta }\\ a\end{array}\right), \dot{\mathit{q}}=\mathit{f}\left(\mathit{q}\left(t\right),\mathit{u}\left(t\right),\mathit{p}\right)$$

(28)

The angular velocity of the leading unit (tractor) is determined to be similar to Equation (3), replacing L with L₁. The semitrailer position could be written as

$$tan\left(\psi \right)=\frac{L_2-e_1/cos\left(\psi \right)}{R_2}$$

(29)

Then,

$${\omega }_2=\frac{v_2}{R_2}=\frac{v_2\cdot tan\left(\psi \right)}{L_2-e_1/cos\left(\psi \right)}=\frac{v_1\cdot cos\left(\psi \right)\cdot tan\left(\psi \right)}{L_2-e_1/cos\left(\psi \right)}=\frac{v\cdot sin\left(\psi \right)}{L_2-e_1/cos\left(\psi \right)}$$

(30)

As a result, similarly to Equations (23) and (24),

$$\frac{d}{dt}\underset{\mathit{q}}{\underbrace{\left(\begin{array}{c}x\\ y\\ \varphi \\ \psi \\ \theta \\ v\end{array}\right)}}=\underset{\mathit{\phi }\left(\mathit{q},\mathit{p}\right)}{\underbrace{v\cdot \left(\begin{array}{c}cos\left(\varphi \right)\\ sin\left(\varphi \right)\\ tan\left(\theta \right)/L_1\\ \frac{tan\left(\theta \right)}{L_1}-\frac{sin\left(\psi \right)}{L_2-e_1/cos\left(\psi \right)}\\ 0\\ 0\end{array}\right)}}+\underset{B}{\underbrace{\left(\begin{array}{cc}0& 0\\ 0& 0\\ 0& 0\\ 0& 0\\ 1& 0\\ 0& 1\end{array}\right)}}\cdot \underset{\mathit{u}}{\underbrace{\left(\begin{array}{c}{\omega }_{\theta }\\ a\end{array}\right)}}$$

(31)

To obtain the Jacobian, similar to Equations (9)–(14), the linearization of Equation (31) is first obtained by letting

$$c_{\varphi }=cos\left(\varphi \right), s_{\varphi }=sin\left(\varphi \right), c_s=cos\left(\psi \right), s_s=sin\left(\psi \right), t_{\theta }=tan\left(\theta \right)$$

(32)

Then, the matrix A (6×6) has the following nonzero elements:

$$\begin{array}{c}{A}_{1,3}=-v\cdot s_{\varphi }, A_{2,3}=v\cdot c_{\varphi }, A_{3,5}=A_{4,5}=\frac{v\cdot \left(t_{\theta }^2+1\right)}{L_1}, A_{1,6}=c_{\varphi }, A_{2,6}=s_{\varphi },\\ A_{4,4}=-v\cdot \left(\frac{c_{\psi }}{L_2-\frac{e_1}{c_{\psi }}}+\frac{e_1\cdot s_{\psi }^2}{c_{\psi }^2\cdot {\left(L_2-\frac{e_1}{c_{\psi }}\right)}^2}\right), A_{3,6}=\frac{t_{\theta }}{L_1}, A_{4,6}=\frac{t_{\theta }}{L_1}-\frac{s_{\psi }}{\left(L_2-\frac{e_1}{c_{\psi }}\right)}\end{array}$$

(33)

The matrix B remains unchanged.

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

The case of the tractor-semitrailer vehicle with semitrailer’s steered axles (TSV-SSA) is similar to the previous case of TSV, except that two parameters are added: the state parameter ζ - (semitrailer’s middle axle steering angle) and control parameter ω_ζ (semitrailer’s middle axle steering rate), as shown in Figure 1d. Considering Equation (28), The state-space components are given by

$$\mathit{q}=\left(\begin{array}{c}x\\ y\\ \varphi \\ \psi \\ \theta \\ \zeta \\ v\end{array}\right), \mathit{p}=\left(\begin{array}{c}{L}_1\\ e_1\\ L_2\end{array}\right), \dot{\mathit{q}}=\left(\begin{array}{c}{v}_x\\ v_y\\ {\omega }_{\varphi }\\ \dot{\psi }\\ \dot{\theta }\\ \dot{\zeta }\\ \dot{v}\end{array}\right), \mathit{u}=\left(\begin{array}{c}{\omega }_{\theta }\\ {\omega }_{\zeta }\\ a\end{array}\right), \dot{\mathit{q}}=\mathit{f}\left(\mathit{q}\left(t\right),\mathit{u}\left(t\right),\mathit{p}\right)=\mathit{\phi }\left(\mathit{q},\mathit{p}\right)+B\cdot \mathit{u}$$

(34)

The radius R₂ may be determined from two conditions, considering Equation (29) and the case of a long single truck:

$$l_2^{’}=R_2\cdot tan\left(\zeta \right), L_2^{’}-e_1/cos\left(\psi \right)=R_2\cdot tan\left(\psi \right)$$

(35)

Then

$$L_2^{’}-l_2^{’}-e_1/cos\left(\psi \right)=R_2\cdot \left(tan\left(\psi \right)-tan\left(\zeta \right)\right)$$

(36)

Considering the coordinate l’₂ is negative relative to a cross point of radius R₂:

$$L_2^{’}-l_2^{’}=L_2^{’}-\left(-\left|l_2^{’}\right|\right)=L_2^{’}+l_2^{’}=L_2$$

(37)

Thus,

$$R_2=\frac{L_2-e_1/cos\left(\psi \right)}{tan\left(\psi \right)-tan\left(\zeta \right)}$$

(38)

The expression for angular velocity ω₂ can be derived as:

$${\omega }_2=\frac{u_2}{R_2}=\frac{v_1\cdot cos\left(\psi \right)}{R_2}=\frac{v\cdot cos\left(\psi \right)\cdot \left(tan\left(\psi \right)-tan\left(\zeta \right)\right)}{L_2-e_1/cos\left(\psi \right)}=\frac{v\cdot sin\left(\psi -\zeta \right)}{\left(L_2-e_1/cos\left(\psi \right)\right)\cdot cos\left(\zeta \right)}$$

(39)

As a result, similar to Equations (23) and (24),

$$\frac{d}{dt}\underset{\mathit{q}}{\underbrace{\left(\begin{array}{c}x\\ y\\ \varphi \\ \psi \\ \theta \\ \zeta \\ v\end{array}\right)}}=\underset{\mathit{\phi }\left(\mathit{q},\mathit{p}\right)}{\underbrace{v\cdot \left(\begin{array}{c}cos\left(\varphi \right)\\ sin\left(\varphi \right)\\ tan\left(\theta \right)/L_1\\ \frac{tan\left(\theta \right)}{L_1}-\frac{cos\left(\psi \right)\cdot \left(tan\left(\psi \right)-tan\left(\zeta \right)\right)}{L_2-e_1/cos\left(\psi \right)}\\ 0\\ 0\\ 0\end{array}\right)}}+\underset{B}{\underbrace{\left(\begin{array}{ccc}0& 0& 0\\ 0& 0& 0\\ 0& 0& 0\\ 0& 0& 0\\ 1& 0& 0\\ 0& 1& 0\\ 0& 0& 1\end{array}\right)}}\cdot \underset{\mathit{u}}{\underbrace{\left(\begin{array}{c}{\omega }_{\theta }\\ {\omega }_{\zeta }\\ a\end{array}\right)}}$$

(40)

To obtain the Jacobian, similar to Equations (9)–(14), the linearization of Equation (40) is first obtained by letting,

$$c_{\varphi }=cos\left(\varphi \right), s_{\varphi }=sin\left(\varphi \right), c_s=cos\left(\psi \right), s_s=sin\left(\psi \right), t_{\theta }=tan\left(\theta \right), t_{\psi }=tan\left(\psi \right), t_{\zeta }=tan\left(\zeta \right)$$

(41)

Then, the matrix A (7×7) has the following nonzero elements:

$$\begin{array}{c}{A}_{1,3}=-v\cdot s_{\varphi }, A_{2,3}=v\cdot c_{\varphi }, A_{1,7}=c_{\varphi }, A_{2,7}=s_{\varphi }, A_{3,7}=t_{\theta }/L_1,\\ A_{4,4}=-v\cdot \frac{\frac{L_2\cdot cos\left(3\cdot \psi -\zeta \right)}{4}+e_1\cdot cos\left(2\cdot \psi -\zeta \right)+\frac{L_2\cdot cos\left(\psi +\zeta \right)}{4}+\frac{L_2\cdot cos\left(\psi -\zeta \right)}{2}}{c_{\zeta }\cdot {\left(e_1-L_2\cdot c_{\psi }\right)}^2},\\ A_{3,5}=A_{4,5}=\frac{v\cdot \left(t_{\theta }^2+1\right)}{L_1}, A_{4,6}=\frac{v\cdot c_{\psi }^2}{c_{\zeta }^2\cdot \left(L_2\cdot c_{\psi }-e_1\right)}, A_{4,7}=\frac{t_{\theta }}{L_1}-\frac{c_{\psi }\cdot \left(t_{\psi }-t_{\zeta }\right)}{L_2-\frac{e_1}{c_{\psi }}}\end{array}$$

(42)

The matrix B remains unchanged.

3. Optimization Model

3.1. Basic Model

In the general case, for a continuous system, the search condition for optimal control over a finite time interval [t₀, t_f] can be written as:

$$\underset{u}{min}\hspace{1em}J\left(\mathit{u}\right)={\rho }_{\epsilon }\cdot {\epsilon }^2+{{\displaystyle \int }}_0^{t_f}\left({\mathit{q}}^T\cdot W_{\mathit{q}}\cdot \mathit{q}+{\mathit{u}}^T\cdot W_u\cdot \mathit{u}+\Delta {\mathit{u}}^T\cdot W_u\cdot \Delta \mathit{u}\right)\cdot dt$$

(43)

Subject to

$$\dot{\mathit{q}}=\mathit{f}\left(\mathit{q}\left(t\right),\mathit{u}\left(t\right),\mathit{p}\right),t\in \left[t_0,t_f\right]$$

The function Equation (43) can be represented in discrete form as

$$\underset{u}{min} J\left({\mathit{z}}_p\right)={\rho }_{\epsilon }\cdot {\epsilon }_p^2+{\displaystyle \sum }_{i=1}^{p-1}\left({\mathit{q}}_i^T\cdot W_q\cdot {\mathit{q}}_i+{\mathit{u}}_i^T\cdot W_u\cdot {\mathit{u}}_i+\Delta {\mathit{u}}_i^T\cdot W_{\Delta u}\cdot \Delta {\mathit{u}}_i\right)$$

(44)

where q_i denotes vector of state-space parameters at the ith prediction horizon step, W_q, W_u, W_Δu denote the matrices of weighting factors, u_i denotes control signals at the ith prediction horizon step, z_p = (u^T₀, 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).

The system of constraints is written as:

$$\{\begin{array}{l}{q}_{j,\min \left(i\right)}-\epsilon \cdot h_{j,\min \left(i\right)}^{\left(q\right)}\le q_{j,i}\le q_{j,\max \left(i\right)}+\epsilon \cdot h_{j,\max \left(i\right)}^{\left(q\right)},i=1\dots p,j=1\dots n_q\\ u_{j,\min \left(i\right)}-\epsilon \cdot h_{j,\min \left(i\right)}^{\left(u\right)}\le u_{j,i-1}\le u_{j,\max \left(i\right)}+\epsilon \cdot h_{j,\max \left(i\right)}^{\left(u\right)},i=1\dots p,j=1\dots n_u\\ \Delta u_{j,\min \left(i\right)}-\epsilon \cdot h_{j,\min \left(i\right)}^{\left(\Delta u\right)}\le \Delta u_{j,i-1}\le \Delta u_{j,\max \left(i\right)}+\epsilon \cdot h_{j,\max \left(i\right)}^{\left(\Delta u\right)},i=1\dots p,j=1\dots n_{\Delta u}\end{array}$$

(45)

where q_j,min(i), q_j,max(i) denote the minimum and maximum values of jth output at the ith prediction horizon step, respectively, u_j,min(i), u_j,max(i) denote the minimum and maximum values of jth input at the ith prediction horizon step, respectively, Δu_j,min(i), Δu_j,max(i) denote the minimum and maximum values of jth input rate at the ith prediction horizon step, respectively, h^(q)_j,min(i), h^(q)_j,max(i) denote the minimum and maximum values of jth output’s hard constraints at the ith prediction horizon step, respectively, h^(u)_j,min(i), h^(u)_j,max(i) denote the minimum and maximum values of jth input’s hard constraints at the ith prediction horizon step, respectively, h^(∆u)_j,min(i), h^(∆u)_j,max(i) denote the minimum and maximum values of jth input rates’ hard constraints at the ith 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

As noted, a clear drawback of the kinematic models’ operation at low speeds is the fact that for obtaining the vehicle’s yaw rate ω, the product of the longitudinal speed v and the steering angle θ function is used. This may lead to the case when, if insufficient longitudinal speed, the angular speed is compensated by the intensive changing the turning velocity of steered wheels. This is quite stable from a mathematical point of view but does not correspond to the real nature of the vehicle movement. In turn, it is impossible to directly impose restrictions on the angular rotation velocity in a region of low translational speeds. In this regard, it is assumed that the best solution is to limit the product between the longitudinal speed v and the turning angular velocity of steered wheels ω_θ:

$$\left|v\cdot {\omega }_{\theta }\right|\le f_{cr}$$

(46)

where f_cr denotes the factor’s critical value.

In this case, at high speeds, the steered wheels’ abrupt turns are absent, and at low values of the vehicle’s longitudinal speed, the wheels’ control turning velocities are limited, which normalizes the nature of the vehicle model movement, especially if the vehicle’s longitudinal speed is close to zero while changing the movement direction.

3.2.2. Case 2: Parking in Restricted Space

Consider the scheme in Figure 2a illustrating a general approach of determining the permissible boundaries between an enclosed space and a passenger car safety contour. In this article, the question of the boundary contour obtaining is omitted but, note, it may be identified by using the SLAM and sensor fusion technologies. As a car moves in an area of potential parking, the surrounding space is scanned by in-vehicle sensors, according to measurements of which a parking loose space shape may be evaluated relative to the vehicle local coordinate system. At the initial position prior to predicting the maneuver, the part of the space that is supposed to be used may be limited by appending the virtual boundaries, which will focus the search for state-space and reduce the optimization time.

Suppose that a car is pre-oriented relative to the desirable final position. Then, a closed perimeter may be virtually represented by a discrete grid with a necessary step along the border (Figure 2a). Note, it is more expedient that the grid density be variable according to space priority, concentrating near the destination. Each node S_i of the local space boundaries may be tied by a virtual connection with the vehicle body’s conditional geometric center C, in such a way that a segment CS_i by means of a safety contour’s control point B_i is being divided into two components: CB_i (associated with the vehicle orientation relative to the initial position) and S_iB_i (which according to the conditions of maneuver’s safety and accuracy, should always remain positive). Thus, for each i at each predicted moment t_k, the following condition must be satisfied:

$$C{S}_i-C{B}_i=S_i{B}_i\ge 0$$

(47)

However, the use of all S_iB_i values in the nonlinear optimization algorithm is optional, since most of them will certainly be greater than zero, and at each iteration, the S_iB_i combinations will be different. Therefore, within one iteration of the optimization search, it can be requested that only the minimum S_iB_i value does not exceed zero. That is,

$$\underset{i}{min} S_i{B}_i\ge 0$$

(48)

Now, consider the actual S_iB_i calculations. The main goal is to determine CB_i, since the vehicle contour changes its orientation relative to the initial one (Figure 2c,d). Even though CS_i segments converging in the center 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_i distances are reduced and their directions become more and more similar to perpendiculars passing through the safety contour.

The virtual security points of vehicle contour can also be set in discrete form. Any form that outlines the overall vehicle dimensions along its perimeter with some safety margin may be represented in polar coordinates (Figure 2b) in such a way that, for any 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).

Because of plane representation (bird’s-eye view) of the vehicle movement in a parking lot, the safe contour’s motion relative to the absolute XOY system can be divided into translational and rotational (Figure 2c,d).

In the case of translational motion (Figure 2c), the point S_i of the parking space perimeter relative to the car contour’s initial position x₀C₀y₀ forms the point B_i0 on the angle β_i0, and in the state x_kC_ky_k the point migrates to the position B_ik on the angle β_ik. If one considers this situation from the car’s local coordinate system xCy, all the segments CS_i 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’ B_ik positions by the known angles β_ik, which, in turn, are obtained based on the known coordinates of S_i and C_k.

In the case of the car contour’s rotational movement (Figure 2d), the segment C₀S_i is identical to the segment C_kS_i, but due to the vehicle’s turn to an angle φ_k, the new angular position β_ik will be defined as:

$${\beta }_{ik}={\beta }_{0k}-{\varphi }_k$$

(49)

Considering that the state-space parameters q are iterative during the optimization process, their current values are known, and thus, the segments’ C_kS_i current angular directions relative to the local coordinate system x₀C₀y₀ can be determined by the superposition of the parking boundary’s relative displacement and rotation. Thus, at each iterative step k, the S_i nodes’ coordinates due to translation in the vehicle’s coordinate system x₀C₀y₀ are:

$${\mathit{x}}_{Sk}={\mathit{x}}_{S0}-x_{Ck},\hspace{1em}{\mathit{y}}_{Sk}={\mathit{y}}_{S0}-y_{Ck}$$

(50)

where x_Sk, y_Sk denote vectors of zone contour nodes’ coordinates in the coordinate system x₀C₀y₀, 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_k in the global coordinate system XOY. The distances r_Sik from the nodes S_ik to the center C_k and the angles β_ik at time interval k in the coordinate system x_kC_ky_k:

$$r_{Sik}=\sqrt{x_{Sik}^2+y_{Sik}^2},\hspace{1em}{\beta }_{ik}=\mathit{arctan}\left(\frac{y_{Sik}}{x_{Sik}}\right)-{\varphi }_k$$

(51)

Then, knowing the new angles β_ik and the base angles α_n0 with the corresponding x_n0, y_n0, the coordinates of points B_ik can be obtained:

$${\mathit{x}}_{Bk}=f_x\left({\mathit{\alpha }}_0,{\mathit{x}}_0,{\mathit{\beta }}_k\right),\hspace{1em}{\mathit{y}}_{Bk}=f_y\left({\mathit{\alpha }}_0,{\mathit{y}}_0,{\mathit{\beta }}_k\right)$$

(52)

where 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’ C₀S_i angles at initial state, x₀, y₀ denote vectors of contour control points’ coordinates at initial state, and β_k denotes vector of current segments’ C_kS_i angles, Equation (47).

The distances from center C_k to points B_ik in the coordinate system x₀C₀y₀ at time interval k are:

$$r_{Bik}=\sqrt{x_{Bik}^2+y_{Bik}^2}$$

(53)

Then, the condition for the car safety contour’s violation absence at time interval k is given by

$$d_k=\mathit{min} \left(r_{Sik}-r_{Bik}\right)\ge 0$$

(54)

The last condition is added to the vector of inequality constrictions of optimization conditions.

3.2.3. Case 3: Circular Motion

The idea of constraints with constant curvature is that the trajectories of the vehicle contour’s n given points must lay within the considered boundaries. Each such a B_n point (like Figure 2b) is distanced by a radius r_Bn 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 XOY, the B_n points’ coordinates X_Bnk, Y_Bnk for kth prediction horizon step yield:

$$X_{Bnk}=x_{Ck}+r_{Bn}\cdot cos\left({\alpha }_{Bn}+{\varphi }_i\right), Y_{Bnk}=y_{Ck}+r_{Bn}\cdot sin\left({\alpha }_{Bn}+{\varphi }_i\right)$$

(55)

Correspondingly, the radii of controlling points in the global coordinate system XOY:

$$R_{Bnk}=\sqrt{X_{Bnk}^2+Y_{Bnk}^2}$$

(56)

Then, the condition of nonlinear restrictions for each kth prediction horizon step is:

$$R_{in}\le R_{Bnk}\le R_{out}$$

(57)

where R_in and R_out denote the inner and outer radii of a roundabout, respectively.

4. Simulation

There are various schemes of the model predictive control method that provide optimal solutions for guaranteeing the robustness under conditions of complex restrictions; see Garcia et al. [17] and Lu and Arkun [18]. Basically, for each horizon interval, the scheme solves an optimization problem with respect to the constraints. The NMPC provided by MATLAB software [19] was used for predicting vehicle behavior and control on a finite-time interval. To plan optimal trajectories, the NMPC controllers solve an open loop constrained nonlinear optimization problem using the SQP algorithm. For each design vehicle, an NMPC object combining model-based prediction and constrained optimization was created. Accordingly, all vehicle models, equality and inequality constraints are nonlinear. The cost function may not necessarily be a quadratic function, which gives more flexibility in finding an appropriate solution. Restrictions may be imposed on inputs (control), outputs, and states. Three types of design vehicles were simulated: passenger car, long single truck with steered rear axle, and articulated vehicles (conventional AV and AV with steered axle of semitrailer). The restrictions and initial conditions for various types of motions are shown in

Table 1. Restrictions and initial conditions for simulating passenger cars.

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/s2 ≤ a ≤ 1 m/s2	Ts = 1 s; p = 14; q0 = (0, 0, 0, 0, 0)T; qf = (−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/s2 ≤ a ≤ 1 m/s2	Ts = 0.5 s; p = 14; q0 = (0, 0, 0, 0, 0)T; qf = (−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/s2 ≤ a ≤ 1 m/s2	Ts = 1 s; p = 16; q0 = (0, 0, 0, 0, 0)T; qf = (−5.5, −6.8, −π/2, 0, 0)T
Circular motion 2,3	−40 ° ≤ θ ≤ 40 °; Rout = 10 m, H = 2.3 m; Rin = Rout – H; −28 °/s ≤ ωθ ≤ 28 °/s; 0.95·vdes m/s ≤ v ≤ 1.25·vdes m/s; −2 m/s2 ≤ a ≤ 2.5 m/s2	Ts = 1 s; p = 16; βC0 = −π·7/9; βCf = π·5/6; q0 = (Rav·cos(βC0), Rav·sin(βC0), π/2 +βC0, arctan(2·L/Rav), vdes)T; qf = (Rav·cos(βCf), Rav·sin(βCf), π/2 +βCf, arctan(2·L/Rav), vdes)T

¹v_des = 5 m/s (desirable circulating speed). ² R_av = (R_out + R_in)/2. ³ R_av = (R_out + R_in)/k, where k = 2.15.

for a passenger car,

Table 2. Restrictions and initial conditions for simulating long single trucks.

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/s2 ≤ a ≤ 0.7 m/s2	Ts = 3 s; p = 6; q0 = (0, 0, 0, 0, 0, 0)T; qf = (−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/s2 ≤ a ≤ 1.5 m/s2	Ts = 1 s; p = 18; q0 = (0, 0, 0, 0, 0, 0)T; qf = (0, 0, π/2, 0, 0, 0)T
Circular motion 2,3	−40° ≤ θ ≤ 40°; −30°≤ ζ ≤ 30°; 0.95·vdes m/s ≤ v ≤ 1.25·vdes m/s; Rout = 15 m, H = 4.5 m; Rin = Rout – H; −28 °/s ≤ ωθ ≤ 28 °/s; −28 °/s ≤ ωζ ≤ 28 °/s; −2 m/s2 ≤ a ≤ 2.0 m/s2	Ts = 1 s; p = 13; βC0 = −135°; βCf = 140°; q0 = (Rav·cos(βC0), Rav·sin(βC0), π/2 +βC0, arctan(2·L10/Rav), −arctan(2·l10/Rav), vdes)T; qf = (Rav·cos(βCf), Rav·sin(βCf), π/2 +βCf, arctan(2· L10/Rav), −arctan(2·l10/Rav), vdes)T

¹v_des = 5 m/s (desirable circulating speed). ² R_av = (R_out + R_in)/2. ³ R_av = (R_out + R_in)/k, where k = 2.15.

for long single trucks and in

Table 3. Restrictions and initial conditions for simulating articulated vehicles.

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/s2 ≤ a ≤ 2.5 m/s2	Ts = 1 s; p = 12; q0 = (0, 0, π/2, 0, 0, 0)T; qf = (−5, −35, π, 0, 0, 0)T
	Circular Motion 2	Inequality constraint of Equation (56); Rout = 15 m, H = 5 m; Rin = Rout – H; −40° ≤ ψ ≤ 40°; −45° ≤ θ ≤ 45°; 0.95·vdes m/s ≤ v ≤ 1.25·vdes m/s; −34 °/s ≤ ωθ ≤ 34 °/s; −0.5 m/s2 ≤ a ≤ 0.5 m/s2	Ts = 1 s; p = 9; βC0 = −160°; βCf = 140°; q0 = (Rav·cos(βC0), Rav·sin(βC0), π/2 +βC0, π·11/60, arctan(2·L1/Rav), vdes)T; qf = (Rav·cos(βCf), Rav·sin(βCf), π/2+βCf, π·11/60, arctan(2·L1/Rav), vdes)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/s2 ≤ a ≤ 2.5 m/s2	Ts = 1 s; p = 12; q0 = (0, 0, π/2, 0, 0, 0, 0)T; qf = (−5, −35, π, 0, 0, 0, 0)T
	Circular motion 3	Inequality constraint of Equation (56); Rout = 15 m, H = 4 m; Rin = Rout – H; 30° ≤ ψ ≤ 35°; 15° ≤ θ ≤ 20°; −15° ≤ ζ ≤ −10°; 0.95·vdes m/s ≤ v ≤ 1.05·vdes m/s; −34 °/s ≤ ωθ ≤ 34 °/s; −28 °/s ≤ ωζ ≤ 28 °/s; −0.5 m/s2 ≤ a ≤ 0.5 m/s2	Ts = 1 s; p = 9; βC0 = -100; βCf = 200°; q0 = (Rav·cos(βC0), Rav·sin(βC0), π/2 +βC0, π/6, arctan(2·L1/Rav), -arctan(2·L1/Rav), vdes)T; qf = (Rav·cos(βCf), Rav·sin(βCf), π/2 +βCf, π/6, arctan(2·L1/Rav), -arctan(2·L1/Rav), vdes)T

¹v_des = 8 m/s (desirable circulating speed). ² R_av = (R_out + R_in)/k, where k = 1.95. ³ R_av = (R_out + R_in)/k, where k = 2.025.

for articulated vehicles. For simplicity, all the weighting factors of the objective functions were set as equal to 1.

4.1. Simulation of Passenger Car

For a passenger car, the only parameter is the longitudinal wheelbase 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).

$$\underset{u}{min} J\left(z_p\right)={\displaystyle \sum }_{i=1}^{p-1}\left(q_i^T\cdot W_q\cdot e_q+u_i^T\cdot W_u\cdot e_u\right)$$

(58)

where e_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.

For the circular motion (Figure 4b,c), control is a priori the simplest, due to retaining the steering angle θ value of a narrow range. The cost function has the form:

$$\underset{u}{min} J\left(z_p\right)={\displaystyle \sum }_{i=1}^{p-1}{\theta }_i^2$$

(59)

This is subject to an inequality constraint given by Equation (57).

4.2. Simulation of Long Single Truck with Steered Rear Axle

The parameter for this model is the full wheelbase L = L₁₀ + l₁₀, consisting of longitudinal wheelbase L₁₀ = 6.65 m between steering and driving axles and spread axles’ wheelbase l₁₀ = 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:

$$\underset{u}{min} J\left(z_p\right)={\displaystyle \sum }_{i=1}^{p-1}\left(x_i+y_i+u_i^T\cdot W_u\cdot u_i+{\theta }_i\cdot W_{\theta \zeta }\cdot {\zeta }_i\right)$$

(60)

where W_u denotes the control weighting factor and W_θζ denotes weighting factor of mutual influence between θ and ζ.

In the case of changing position on the spot (Figure 5c,d), the task is complicated with the fact that the initial and final coordinates of the vehicle’s mass center are coincident. To realize the maneuver, the cost function needs to be relaxed by allowing the controller searching for a solution in both positive and negative zones. The linear cost function can be represented by the sum of Cartesian coordinates x and y, Equation (22):

$$\underset{u}{min} J\left(z_p\right)={\displaystyle \sum }_{i=1}^{p-1}\left(x_i+y_i\right)$$

(61)

For the circular motion (Figure 6); on the one hand, there is a need for softening the inequality constraints set in Equation (57) within the boundaries of a roundabout’s lane width. On the other hand, it is desirable to ensure the minimum space occupied by the vehicle with the smooth and minimal total steering control. Thus, a combination of linear and non-linear cost function elements may be used:

$$\underset{u}{min} J\left(z_p\right)={\displaystyle \sum }_{i=1}^{p-1}\left(q_i^T\cdot W_q\cdot e_q+u_i^T\cdot W_u\cdot e_u+{\theta }_i\cdot W_{\theta \zeta }\cdot {\zeta }_i\right)$$

(62)

where W_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

According to the designation in Figure 1c,d, the models are characterized by three parameters: L₁ = 3.8 m, L₂ = 7.57 m, e₁ = 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.

Consider first docking at unconstrained space (Figure 7). Usually, the loading and unloading of articulated vehicles are carried out from the side of the warehouses’ docks, where there is a lot of space for the maneuvering of long vehicles. In this regard, the spatial restrictions may be omitted. In the case of conventional TSV (Figure 7a,b), considering the space between initial and final positions is not being restricted, a relatively symmetric distribution of coordinates, speed, and control may be satisfactory, which mitigate the search with a linear cost function, avoiding the redundant smoothness. Hence, it may be presented as:

$$\underset{u}{min} J\left(z_p\right)={\displaystyle \sum }_{i=1}^{p-1}\left(q_i^T\cdot W_q\cdot e_q+u_i^T\cdot W_u\cdot e_u\right)$$

(63)

where W_q = diag(1,1,1,1,0,1) denotes diagonal matrix of states’ weighting factors, and other symbols are as previously defined.

In the case of TSV-SSA (Figure 7c,d), the vehicle may occupy as much space as needed for the maneuver. As the vehicle is charged and in order to prevent significant tires’ sideslip, it is undesirable for its links to be folded on a big angle. Meanwhile, there is no strict necessity that the tractor center’s trajectory be highly smoothed. Thus, a linear function of the tractor’s translational and rotational states would be enough. The control must provide smooth motion in general and total steering action should be reduced as well. Consequently, the cost function may be written in a form:

$$\underset{u}{min} J\left(z_p\right)={\displaystyle \sum }_{i=1}^{p-1}\left(q_i^T\cdot W_q\cdot e_q+u_i^T\cdot W_u\cdot u_i+{\theta }_i\cdot W_{\theta \zeta }\cdot {\zeta }_i\right)$$

(64)

where W_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 ζ.

For the circular motion, in the case of conventional TSV (Figure 8a,b), the controlling of the vehicle links’ mutual orientation (articulation angle ψ) is possible only by the tractor’s steered wheels. However, according to the tractor movement conditions relative to the center of a roundabout, the wheels’ position θ is determined in a narrow range.

Within the specified lane width H, certain control is feasible, ensuring the minimum possible articulation angle. Then, considering Equation (31), the cost function can be written in the linear-quadratic form:

$$\underset{u}{min} J\left(z_p\right)={\displaystyle \sum }_{i=1}^{p-1}\left({\psi }_i^2+u_i^T\cdot W_u\cdot u_i\right)$$

(65)

In the case of TSV-SSA (Figure 8c,d), the idea is that the vehicle should occupy the minimum space at a roundabout, which corresponds to keeping the articulation angle ψ in the region of the smallest possible value for a given circle with the narrowest corridor H. In addition, it is desirable that the control signals be as smoothed as possible, and the total control of tractor’s θ and semitrailer’s ζ wheels is synchronized and minimal as well. Thus, considering Equation (34), the cost functional can be written as:

$$\underset{u}{min} J\left(z_p\right)={\displaystyle \sum }_{i=1}^{p-1}\left({\psi }_i^2+u_i^T\cdot W_u\cdot u_i+{\theta }_i\cdot W_{\theta \zeta }\cdot {\zeta }_i\right)$$

(66)

4.4. Results and Discussion

Based on the optimized paths for the vehicles’ motion plans, the visualizations of predicted trajectories are depicted, as well as graphs of the main output parameters (linear, angular displacements, ego speeds, and steering angles) and control laws (accelerations and steered wheels’ angular speeds). The technique presented in paragraph 4, in general, has shown its effectiveness, allowing one to plan successful maneuvers with various combinations of initial and final conditions on a finite prediction horizon. The obvious advantage of the optimization approach is that the steering angles evolve with a change in vehicle speed, i.e., in fact, there is no wheel turn on the spot. The aim of forecasting the circular motion is to work out the restrictions for any type of curvilinear motion with variable road curvature, for cases of assigning a lane to an articulated vehicle, as well as when narrowing the space allocated to a vehicle. The hard restrictions lead to some fluctuations in solutions. To have smooth gentle output curves, it is necessary to narrow the range of constraints of state parameters within the framework of vehicle kinematic models.

The simulation results for the car parallel reverse parking are presented in Figure 3a,b. The initial conditions are given in Table 1. In this example, the task consists of predicting the maneuver and the control factors for placing a car in a parking spot from the parallel initial position in a finite time. In fact, the car initial position relative to the parking pocket can be specified by an arbitrary initial vector 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.

The results for car perpendicular reverse parking are presented in Figure 3c,d. This maneuver is the simplest in terms of control. In some works, the necessary trajectory is determined geometrically by an arc of constant radius. However, this is acceptable if the initial position of the vehicle is determined in the vicinity of an acute angle of the parking pocket. In the case of an arbitrary position, it is important to orient the vehicle in phase 2 as coaxially as possible to the pocket axis when a car may maintain an approximately permanent value of ego velocity v. In this regard, at the initial moment, the steered wheels have a positive angle of rotation θ, which first leads the car’s rear to the outer boundary. It is noteworthy that the rotation angle and speed signals are of a general tendency but not synchronous. The results for car perpendicular forward parking are presented in Figure 4. Such a maneuver is possible with sufficient space outside a parking pocket. It consists of two phases: partial reverse turn (1–2) and forward turning (2–3). The maneuver is quite long, 16 seconds, however, NMPC successfully built the forecast for it. The control parameters a and ω begin and end with zero values; the values of the steering angle θ are used in the full range, which indicates the need for a high degree of maneuverability in the given constraint conditions. It can be noted that the final phase is characterized by almost constant values of the yaw angle and the X coordinate, and the speed v decreases to the utmost, which indicates a stable and safe car movement relative to the destination and the parking spot borders.

The results for car circular motion are presented in Figure 4c,d. When planning a circular motion, it should be considered that the closer values of vectors of initial and final conditions, especially the coordinates in the restrictions of tight boundaries, the more the solution instability. The space of a road curved section defined by lidar or camera locks up on the prediction horizon. Therefore, the planning makes sense only within the framework of an arc defined by the values of initial and final angular coordinates β_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.

The results for truck perpendicular reverse parking are presented in Figure 5a,b. This maneuver is modeled for the case when the same type long wheelbase vehicles are placed in a row and there is a free spot. The use of an auxiliary steered axle allows increasing vehicle maneuverability and ensuring the best control accuracy and space use. The truck has zero initial data in the initial position. The approach of describing the space represented in Figure 2. As can be seen, the output and control parameters are smooth curves, clearly reflecting the maneuver phases. The angular velocities ω_θ, ω_ζ of steering wheels, as well as their turning angles θ and ζ, are in antiphase within a wide range of admissible values.

The results for changing truck position on the spot are presented in Figure 5c,d. The maneuver corresponds to the case of the most limited space if there is a need for changing the truck position. The linear form of the objective function Equation (61) is focused on the minimum use of coordinates, which is enhanced by the simultaneous control of two axles with the maximum ranges of steering angles θ and ζ. First, the vehicle moves backwards from state 1 to state 2, and then forward to the final position 3. The vehicle speed module does not exceed 2 m/s, and the whole process takes about 17 s. The graph of accelerations clearly shows the number of sign changes of longitudinal accelerations, which indicates the nature of the acceleration-deceleration control. As can be seen from the graphs, the initial and final values of linear displacements coincide, and the angular coordinates have a difference of 90 °. The results for truck circular motion are presented in Figure 6. The need for such a maneuver is explained by the emerging need for the accurate prediction of curvilinear motion in conditions of minimal swept path. The output yaw angle demonstrates an almost linear increase. Some fluctuations in the steering rates ω_θ, ω_ζ 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.

The results for an articulated vehicle docking in an unconstrained space for TSV and TSV-SSA docking are presented in Figure 7. The purpose of the maneuver is to obtain the simplest control while minimizing the use of space and state parameters. However, there are no restrictions on the use of space. The task is to perform a maneuver in reverse with a turn on 90 ° to the place of supposed unloading. As can be seen in Figure 7b,d, the distribution of acceleration and speed over time is almost identical for the TSV and TSV-SSA, however, for the control signals of the steering rates and, as a consequence, the steering angles—there is a significant difference. When using the TSV-SSA, the required control is both more stable and smaller in range due to oversteer. Moreover, TSV-SSA has a much lesser range of articulation angle. The results for articulated vehicle circular motion for TSV and TSV-SSA are presented in Figure 8. The maneuver purpose is to optimize the disposition of an articulated vehicle in conditions of movement along a lane of a roundabout (turnabout) or other arched road sections with a small curvature radius. Since the acceleration a of TSV in Figure 8b is variable, the control signal of the steering rate ω_θ 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

This paper has presented nonlinear kinematic models for vehicle path and control forecasting using an open-loop optimization technique for predicting vehicle behavior in the low-speed range and space-limited areas. The study has proposed a parking algorithm and has evaluated the implementation of NMPC for such objectives, in general, and for the possible use in the closed-loop tracking combined with dynamic vehicle models. In addition, the study has revealed the modeling nuances, advantages, and disadvantages of applying kinematic models. Based on this study, the following comments are offered:

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