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

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

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

173
Then, the linearized equation in increments is given by The matrix A is the Jacobian, which is given by

187
From the ratio of the steering angle θ of the front steered wheel: 203 According to Eq. (5), To obtain the Jacobian, denote,

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

212
The matrix B remains unchanged.

214
In the case of a conventional articulated vehicle (CAV), by analogy described in the previous the offset e1. Since ψ is the articulation angle, R1 and R2 will also be located at the angle ψ to each 219 other.

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

240
The angular velocity of the leading unit (tractor) is determined similarly Eq.

253
The matrix B remains unchanged.

280
The matrix B remains unchanged.
In the general case, for a continuous system, the search condition for optimal control over a finite

295
The system of constraints is written as:

335
Suppose that a car is pre-oriented relative to the desirable final position. Then, a closed perimeter with the vehicle bodyʹs conditional geometric center C in such a way that a segment СSi by means of them will certainly be greater than zero, and at each iteration, the SiBi combinations will be different.

347
Therefore, within one iteration of the optimization search, it can be requested that only the minimum , , , , , ,

456
For the circular motion (Figure 4b, c), control is a priori the simplest due to retaining the steering 457 angle θ value of a narrow range. Cost function gets the form: Inequality constraints correspond to Eq. (56).

508
Consider first docking at unconstrained space (Figure 7). Usually the loading and unloading of 509 articulated vehicles are carried out from the side of the warehousesʹ docks where there is a lot of 510 space for the maneuvering of long vehicles. In this regard, the spatial restrictions may be omitted. In

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

531
For the circular motion, in the case of conventional TSV (Figure 8a, b), the controlling of the 532 vehicle linksʹ mutual orientation (articulation angle ψ) is possible only by the tractorʹs steered wheels.

533
However, according to the tractor movement conditions relative to the center of a roundabout, the 534 wheelsʹ position θ is determined in a narrow range.
In the case of TSV-SSA (Figure 8c, d), the idea is that the vehicle should occupy the minimum 543 space at a roundabout, which corresponds to keeping the articulation angle ψ in the region of the 544 smallest possible value for a given circle with the narrowest corridor H. In addition, it is desirable 545 that the control signals be as smoothed as possible, and the total control of tractorʹs θ and semitrailerʹs 546 ζ wheels is synchronized and minimal as well. Thus, considering Eq. (33), the cost functional can be 547 written as: where Wθζ = weighting factor of mutual influence.

563
The simulation results for the car parallel reverse parking are presented in Figure 3a-b. The initial 564 conditions are given in the Table 1. In this example, the task consists of predicting the maneuver and 565 the control factors for placing a car in a parking spot from the parallel initial position in a finite time.

566
In fact, the car initial position relative to the parking pocket can be specified by an arbitrary initial

583
The results for car perpendicular forward parking are presented in Figure 4. Such a maneuver is possible 584 with sufficient space outside a parking pocket. It consists of two phases: partial reverse turn (1-2) and maneuverability in the given constraint conditions. It can be noted that the final phase is decreases to the utmost, which indicates a stable and safe car movement relative to the destination planning makes sense only within the framework of an arc defined by the values of initial and final 597 angular coordinates βC0, βCf in Table 1

626
The results for Articulated vehicle docking at unconstrained space for TSV and TSV-SSA docking are 627 presented in Figure 7. The purpose of the maneuver is to obtain the simplest control while minimizing 628 the use of space and state parameters. However, there are no restrictions on the use of space. The task 629 is to perform a maneuver in reverse with a turn on 90 degrees to the place of supposed unloading.

630
As can be seen in Figure 7b, d, the distribution of acceleration and speed over time is almost identical 631 for the TSV and TSV-SSA, however, for the control signals of the steering rates and, as a consequence, 632 the steering angles -there is a significant difference. When using the TSV-SSA, the required control 633 is both more stable and smaller in range due to oversteer. Moreover, TSV-SSA has a much lesser