TRAJECTORY PLANNING ALGORITHMS IN TWO-DIMENSIONAL ENVIRONMENT WITH OBSTACLES

. This article proposes algorithms for planning and controlling the movement of a mobile robot in a two-dimensional stationary environment with obstacles. The task is to reduce the length of the planned path, take into account the dynamic constraints of the robot and obtain a smooth trajectory. To take into account the dynamic constraints of the mobile robot, virtual obstacles are added to the map to cover the unfeasible sectors of the movement. This way of accounting for dynamic constraints allows the use of map-oriented methods without increasing their complexity. An improved version of the rapidly exploring random tree algorithm (multi-parent nodes RRT – MPN-RRT) is proposed as a global planning algorithm. Several parent nodes decrease the length of the planned path in comprise with the original one-node version of RRT. The shortest path on the constructed graph is found using the ant colony optimization algorithm. It is shown that the use of two-parent nodes can reduce the average path length for an urban environment with a low building density. To solve the problem of slow convergence of algorithms based on random search and path smoothing, the RRT algorithm is supplemented with a local optimization algorithm. The RRT algorithm searches for a global path, which is smoothed and optimized by an iterative local algorithm. The lower-level control algorithms developed in this article automatically decrease the robot’s velocity when approaching obstacles or turning. The overall efficiency of the developed algorithms is demonstrated by numerical simulation methods using a large number of experiments.


Introduction.
Path planning is a key problem in designing motion planning systems. The problem is to determine the trajectory that connects the initial and final position of the robot and ensures that there are no collisions with obstacles. In this case, the problem can be solved by taking into account the dynamics of the robot, the uncertainty and non-stationarity of the environment, the time for calculating the path, and the physical feasibility of the trajectory. The planning problem is often formulated as the problem of optimizing the state of the robot's current position relative to the target position. Most often, this problem is solved in the configuration space [1 -3], which consists of a set of obstacles, kinematic and dynamic constraints, and a set of robot points. In this case, a configuration is understood as a set of variables that uniquely determine the robot's position in space. Planning methods are divided into global and local. Global methods build a route based on a known map, while local methods correct the path when unmapped obstacles are detected. Planning can be carried out in discrete and continuous spaces.
Cell decomposition methods [2] are the basis of a significant number of path-planning algorithms, which implies decomposing the original space into discrete cells, for example, according to the procedure [4]. After decomposition, the search for a path is already carried out in a discrete space that describes the original space with certain accuracy. In general, the division of space into cells that is optimal in terms of accuracy requires different resolutions for different regions of this space. In this regard, the methods of adaptive cell decomposition are widely used today [5]. The main problem of this method is the increase in computational complexity with the growth of requirements for the accuracy of planning the robot's path, which makes this problem remaining relevant [6].
Cell decomposition allows applying discrete search methods based on graph theory. The most popular methods are the A* and D* algorithms [7,8].
A* algorithm is a development of Dijkstra's algorithm, in which the computational complexity is reduced due to the heuristic function for estimating the path cost. At the same time, the algorithm demands on memory, and its computational complexity increases significantly with the number of cells on the map. In this regard, there are modifications of A* aimed at eliminating these disadvantages. Such modifications include A* algorithms with iterative deepening [9], A* with memory constraints [10], hierarchical A* [11], and A* with dynamic change in edge weights [12,13]. D* algorithm [8] provides for path planning under conditions when information about the environment can be updated while the robot is moving, for example, when new obstacles are detected.
It should be noted that the considered path planning algorithms give non-smooth paths. In this regard, these methods are supplemented by additional methods that are used to smooth the path [14]. Taking into account the dynamic constraints imposed by the inertia of the robot is also a separate laborious task.
Smooth paths can be obtained using the potential field method [15], which has been studied in detail in the works of various authors [16 -19]. The potential field method is computationally efficient and allows path planning in uncertain environments. However, taking into account the dynamic properties of the target and obstacles is a separate problem since it is required to form the functions of attraction and repulsion depending on the positions and velocities. This problem is considered in [20].
The main disadvantage that limits the application of the potential field method in practice is the possible presence of local minima. The proposed approaches to solving this drawback solve the problem in particular cases. There is currently no good solution to this problem. In addition, the coefficients of attractive and repulsive forces are usually chosen heuristically, and the potential field method experiences difficulties when planning in a complex environment with narrow corridors, passages, and non-convex obstacles. Despite the partial solution of problems, for example, using vector histograms [19], it is required to involve other planning algorithms.
One way to solve the problem of local minima is to use random forces in them that push the robot out of the indicated minima. This idea has been developed in sampling methods, which include probabilistic planning methods.
At present, probabilistic path-planning algorithms in discrete space are widely used [21]. Algorithms of this type include Probabilistic Roadmap (PRM) [3,22,23] and rapidly-exploring random trees (RRT) [2, 24 -26]. The PRM algorithm usually does not take into account the dynamics of the mobile robot. To consider it, significant computational costs are required [22,27]. The RRT algorithm can take into account the dynamics of a mobile robot [24,25] since it builds a feasible path in free space.
The main limitations of these methods are slow convergence to the optimal solution, high requirements for the memory used [22,27], and limitations in planning under conditions of incomplete certainty of the environment and robot model [22]. In solving these problems, algorithms based on a probabilistic approach can be effectively used as global path planners.
In this regard, let us consider in more detail the work aimed at improving the efficiency of algorithms based on a probabilistic approach.
1.1. Related work. The problem of using the RRT and RPM algorithms under conditions of partial uncertainty is considered in [22], in which it is proposed to divide the entire area of operation into separate regions. Links between regions are searched using the RRT or RPM methods. Planning a local path within a particular region is carried out using a simple straight-line planning algorithm. In this case, the uncertainty of the robot model is taken into account by introducing an additive disturbing input such as white noise. The uncertainty of the map is taken into account by the probability that the cell is occupied by an obstacle. The main result of the method is a path that guarantees the successful achievement of the target point with a probability not less than a given p min . The work [22] presents the results of analyzing and modeling the motion of the robot that is represented by the kinematic equations of a material point in a static environment. The main disadvantage of the method is a significant increase in computational costs for large values of the probability p min . The problem of reducing computational costs and memory occupied is considered in [26,27], which combines the RRT and potential field (P-RRT*) methods. In this method, a random tree is built in the direction of decreasing the potential field value, which makes it possible to increase the rate of convergence to the optimal trajectory. In [27], an improved version of the algorithm RRT* [28] is used, in which the initial path is calculated similarly to the basic RRT algorithm. Then the number of nodes in the configuration space is increased to optimize the initial path. In [28], the analysis of the convergence of the proposed algorithm is carried out and the results of numerical simulation are presented. The disadvantages of the method presented in [27,28] are the non-smoothness of the trajectories and the still high computational complexity.
Note that the idea of using potential fields to improve the convergence and computational efficiency is not limited only to the RRT algorithms family, but it is also used with the PRM algorithms. Thus, in [23], a hybrid method is used that includes the PRM algorithm and artificial potential fields. The node distribution uses a segmented map to create areas of low and high potentials. To eliminate the problem of local minima, a reactive approach to planning local paths in the presence of static and dynamic obstacles is used. The proposed algorithms have been tested using numerical and field experiments.
In [26], the P-RRT* method is developed for the case of bidirectional search. In this method, nodes are generated from both the starting and the target points. Both trees create attractive fields for each other, which improves the convergence of the P-RRT* method, especially for finding narrow passages. The work [29] also investigates the problem of increasing the rate of convergence of the path planning process using the bidirectional search RRT algorithm. The outcomes of [26,29] indicate that the bidirectional search in a combination with potential fields makes it possible to reduce the number of iterations for finding the shortest path. Also, such algorithms allow moving in narrow corridors without hitting local minima. To reduce the memory used, the calculation of two random trees is carried out sequentially in time, which can increase the calculation time.
The paper [30] also uses a two-step planning procedure for a dynamic environment. The global planner plans the original first approximation path using the RRT method. Next, a dynamic planner is used, which changes the nodes by using repulsive forces from obstacles and inertial forces. To account for moving obstacles, this method uses a forecasting procedure. The effectiveness of this method is confirmed by the results of numerical experiments. In this work, a heuristic optimization function is used, the choice of which depends on various requirements and constraints. The correct formation of such function is the central problem of the presented method.
In [31], the problem of constructing an optimal path with a visit to the set of given target points is considered [32]. To solve this problem in an environment with obstacles, a forest of trees is built [33], where the trees start from the given target points. A forest of trees is built before they are connected to each other. This work is a development of [34], in which the construction of a forest of trees is optimized due to the priorities of the queue of nodes. The proposed method is studied in a two-dimensional and three-dimensional environment using numerical simulation methods. The reduction in computational costs and memory requirements is due to the fact that each tree is built separately from each other. In this regard, the nodes of individual trees are stored in separate arrays.
In [35], a modification of the RRT algorithm is considered, taking into account the limitations imposed by the vision system and the speed limitations. An algorithm based on computer vision that functions under conditions of an unknown image depth is proposed. A model of a robot in the form of kinematics of a material point is considered. The main contribution of the method proposed in [31] is the presence of a virtual target, which is located so that the robot is on a line that provides its specified orientation angle at the target point. The advantages of the proposed algorithm are confirmed by the results of the experiments. This article shows the efficient operation of the RRT algorithm in real conditions. However, the experiments were carried out in a simple environment.
1.2. Contribution of the article. The performed analysis allows us to conclude that currently the typical issues of path planning using the RRT algorithm are: 1) Consideration of restrictions in terms of the formation of feasible paths that take into account the dynamic properties of robots.
2) Optimization of computational complexity and convergence of algorithms to optimal trajectories.
3) Consideration of the uncertainty associated with the lack of a complete map of the environment and optimization of the processes of replanning paths in dynamic environments.
The purpose of this article is to develop planning algorithms that reduce the length of the planned trajectory, reduce its calculation time and take into account the restrictions imposed by the robots' properties. We propose a two-stage procedure, during which the MPN-RRT method is used to find the first approximation path which is optimized using the developed iteration algorithm. To take into account the dynamic restrictions on the turns of a mobile robot, it is proposed to add virtual obstacles to the map, which block unfeasible sectors of the movement. This makes it possible to take into account these limitations without changing the developed MPN-RRT algorithm.
In summary, the contributions of this paper are listed as follows. 1) In this work, virtual obstacles are introduced on the map, located in such a way that they overlap the unfeasible sectors of the mobile robot's movement. This makes it possible to take into account the restrictions on maneuvers that depend on the linear speed of the robot and its maximum permissible angular accelerations and velocity. At the same time, the virtual obstacle method does not change the planning algorithm and allows the use of any map-oriented planning algorithm.
2) This article proposes the use of several parent nodes when building a random tree using the RRT algorithm. On the one hand, this complicates the structure of the constructed graph. On the other hand, it gives more possibilities to optimize the resulting motion trajectory. As will be shown below, the introduction of additional parent nodes makes it possible to reduce the length of the planned path with increasing computational complexity.
3) The proposed method includes global and local planning stages. The use of a two-stage procedure makes it possible to reduce the computational costs for finding an optimal solution compared to a one-stage global search. A proposed MPN-RRT algorithm with two-parent nodes is used as a global planner. An optimization algorithm is used as a local planner, taking into account the path length and deviation from the points planned by the global planner.
4) An algorithm of local optimization and path smoothing described by segments connecting the points of the first approximation path is proposed. A quality criterion is proposed, which is a convolution of two indicators, which are the path length and the deviation from the global path points. This procedure allows optimizing the path's length while limiting the deviation from the original trajectory of the first approximation. This method is based on the outcomes of [36,37]. Its difference lies in the application of a new optimization functional. The function that was used in these papers included the probability of successful passage of a section of the trajectory and the deviation from the trajectory of the first approximation.
2. Problem statement. Let us consider a rectangular stationary twodimensional environment of size L x ×L y , shown in Figure 1, which simulates a low-density urban development.
The lower-left corner of the map coincides with the origin of the fixed coordinate system OX g Y g . The length of the environment along the OX g axis is L x , and along the OY g axis is L y . The position of the robot in the environment is determined by the coordinate vector (x, y) and the orientation angle ϕ. The obstacles are the rectangles which are r pi wide and b pi high. The position of an obstacle is determined by the coordinates of its lower-left corner (x pi , y pi ). The target position is specified by a vector of coordinates (x t , y t ).
The mobile robot's design features, inertial properties, and requirements for stability and controllability do not allow it to move to an arbitrary point in space [39]. In this regard, the following restriction is introduced on the maximum turning angle depending on: where ∆ϕ min is the minimum rotation angle, independent of the robot's velocity, determined by the rolling resistance; V min , V max are the minimum and maximum values of the robot's velocity, respectively; α is the coefficient depending on critical velocity, robot length, friction, and rolling resistance. The function (3) is shown in Figure 2.
where r is the straight distance to the target; r s is the distance that determines the moment when the robot starts braking; k ϕ is the function of velocity reduction when turning around; k obs is the function of decreasing velocity when approaching obstacles.
The function k ϕ is shown in Figure 3, according to which the robot's velocity decreases linearly with an increase in the difference between the current ϕ and the desired ϕ* angle of the movement of the robot.
The k obs function is shown in Figure 4, according to which, when the robot approaches an obstacle at distance smaller than r s , its velocity begins to decrease in proportion to the specified distance: i.e. have no breaks. The quality criterion of the planned trajectory is its length, i.e.:

Planning system algorithms.
This section considers the planning procedure, which includes the following: 1) Introduction of a virtual obstacle into the map to take into account the constraints (3).
2) Construction of a random tree for planning the first approximation path using the modified RRT method.
3) Finding the shortest first approximation path using the ant colony algorithm. 4) Local optimization and smoothing of the first approximation path.
3.1. Adding virtual obstacles to the map. As follows from the expression (3), if the velocity is V = V min , then the robot can turn at any angle from -π to +π. As the robot's velocity increases, the sector available for its maneuver begins to decrease in accordance with (3). To consider these limits, a virtual obstacle is added to the environment map as shown in Figure 5. Figure 5 shows the evolution of a virtual obstacle in the case when V 3 > V 2 > V 1 . The virtual obstacle is V-shaped. At low speeds, this obstacle is located at a convex angle to the robot. As the velocity increases, this angle decreases to unfold and becomes sharp. The presence of a virtual obstacle makes it possible to increase the safety of the robot's movement without using additional algorithms to avoid collisions. As shown in Figure 6, if the robot moves in a narrow passage between two obstacles and receives a target point located behind it, then a collision may occur if the robot's velocity is restricted by inequalities 0<Vmin≤V≤Vmax.
The advantage of virtual obstacles for eliminating such collisions is that they do not increase the computational complexity of the used planning algorithm. The procedure for introducing a virtual obstacle is based on the expression (3), which does not present additional computational complexity.
At the same time, it should be noted that the parameters ∆ϕ min and α, which determine the turning radius in the expression (3), depend on the type of the robot and the conditions of its movement. In this regard, these parameters must be set, and the robot must evaluate the movement resistance coefficients.    The Init_Tree () function initializes the tree and the matrix of paths' weights Matrix_tr. During the initialization phase, the starting point of the path (x, y) and the target point (x t , y t ) are added to the list of nodes. The first tree node in the list is always the starting position of the robot (x, y). The last node in the tree list is always the target point (x t , y t ). The path matrix Matrix_tr is initialized as a zero 2×2 matrix.
The Save_Vir_Obs function adds a virtual obstacle to the map based on the current velocity and the robot's movement direction. In this case, the environment map is a matrix of dimension N x ×N y , where the number of matrix cells is determined by the expressions:   ) and (x t , y t ) without collisions. Such a check is carried out using the Str_Line () function. It returns the FL_0 flag, which is equal to 1 in the presence of a collision, which is determined when the robot's trajectory approaches any obstacle closer than the distance L s .
The main tree building loop begins with the generation of a random node X_rand for which the Rand_Node () function is used. This function generates a node so that it is located at a distance of at least L s from the obstacles and the other nodes of the tree. Next, the search for parent nodes is carried out using the Parent_Node () function. The presence of two-parent nodes is a key difference between the used algorithm and the traditional version of RRT. The main planned effect of this introduction of two-parent nodes is a decrease in the length of the planned path, which occurs at cost of increasing the tree construction time. This effect is due to the fact that the presence of two-parent nodes increases the number of links in the graph under construction, which makes it possible to optimize the path length more effectively.
If the parent nodes are found successfully, then a new node X_rand is added to the tree and the corresponding links are added to the Matrix_tr matrix using the functions Expand_Tree () and Expand_Matr ().
Next, using the function, Str_Line () is checked for a direct collisionfree path from the new X_rand node to the target point (x t , y t ). If such a path exists, then the FL_1 counter is incremented.
The search cycle continues until it finds N add nodes that are added to the tree and gives a new path to the target point.
The search for a path on the constructed graph is carried out using the ant colony optimization algorithm [40]. The choice of the evolutionary algorithm is due to the need of optimizing the search time. The search for the shortest path (line 21) using the ant colony optimization algorithm is implemented by Aco () function that takes Tree and the matrix of coupling Matrix_tr as an input. The result of the operation of the Aco () function is a list of points of the found path Path_opt. This list defines a piecewise broken path that needs to be smoothed. It should also be noted that the RRT algorithm converges rather slowly, so it is necessary to further optimize its length.

Path optimization and smoothing.
Path_opt is the first approximation path. The task is to reduce the length of the curve Path_opt, assuming that the optimized curve is found in a certain bounded neighborhood, or in a certain corridor relative to the original trajectory. To solve this problem, the following functional can be generated: where the first term is the sum of the squares of the deviations of the points (x i , y i ) of the constructed trajectory from the points of the original trajectory (x′ i , y′ i ); the second term is the trajectory length T r ; the coefficients System (8) is reduced to two independent systems of linear equations with respect to vectors X = [x 2 … x N-1 ] T and Y = [y 2 … y N-1 ] T . The solution of these two systems has the form: = .
Let us show that at the found stationary point (9) the sufficient condition for a local maximum with respect to minors of the Hessian matrix is satisfied for any values of 1 2 , δ δ such that, Indeed, it is easy to observe that the Hessian matrix for functional (6) is the block-diagonal matrix: where 0 N-2×N-2 is a zero matrix of dimension N-2×N-2.
It is known that if each of the blocks of such a matrix is a positive definite matrix, then the resulting block diagonal matrix will also be positive definite [41]. Also, according to the corollary to the Gershgorin   [41], a sufficient condition for any symmetric matrix with positive entries on the main diagonal to be positive definite is the condition of strict dominance of diagonal entries. In this case of symmetric tridiagonal Toeplitz matrix of type (9), this condition takes the form: Since 1 2 0 , 1 ≤ δ δ ≤ , then the expression (14) takes the form: Thus, an additional constraint is a requirement that the coefficient δ 1 does not vanish.
Note that the global minimum of functional (6) is simultaneously attained at point (8) due to the uniqueness of the stationary point for this functional.
For practically important cases of minimization of the resulting curve, two conditions can be used. The first condition is the natural requirement of not colliding with obstacles. The second is not exceeding a certain maximum deviation from the original curve, expressed, for example, in the form of the limiting root-mean-square discrepancy σ kr between the corresponding nodal points. This condition is written as: The following procedure is used to find the offset path of the minimum length, taking into account the first and second constraints. First, the values of the coefficient δ 1 are sorted out in the range [δ 1min , δ 1max ] with a step ∆δ 1 <<δ 1max . The value δ 1 =δ 1min corresponds to the case when the optimized trajectory is the closest in terms of the standard deviation to the straight line connecting the starting and target points. If there are no collisions and the deviation from the initial trajectory does not exceed a given one, then the optimal trajectory for δ 1 =δ 1min is the best. If at least one of these conditions is not met, it is necessary to move to the next larger value of the coefficient δ 1 and re-check the specified conditions. This procedure is repeated iteratively until there is such a value δ 1 from the corresponding finite sampling sequence of the interval [δ 1min , δ 1max ], for which both of the above constraints are satisfied, or until the search is completed. To exclude the last case, it is necessary to choose the value of the step ∆δ 1 in accordance with the value of the upper admissible boundary σ kr of the limiting root-mean-square discrepancy between the corresponding nodal points, and the less is the ∆δ 1 the less should be the step.
After finding the optimized curve, it has to be smoothed. For this, the method of approximation by straight-line segments, smoothly conjugate to arcs of the circles, is applied. Moreover, these arcs are constructed for each of the nodal points of the resulting curve.
The resulting trajectory in the form of a list of points that the robot must pass is fed to the motion control function, which calculates the desired direction of motion and setpoints for the linear and angular velocities of the robot. The motion control system is implemented in the form of an adaptive multi-loop system presented in [38].

Numerical modeling results.
In this section, a case study of the developed algorithms is carried out. The simulation was carried out in MATLAB. During the simulation, the following parameters of the mobile robot were set: a v = 1; a w = 5; V max = 4 m/s; w max = 2 rad/s. The safe distance at which the robot starts braking is r s = 6. The minimum distance to the obstacles is L s = 1 m. The dimensions of the environment are L x = 100 m, L y = 100 m. The maximum number of obstacles is 24. The maximum width and length of each obstacle is 11 m. An urban environment is simulated with a maximum building rate of 30 %.

Investigation of the influence of the number of parent nodes on the trajectory length and the time of the trajectory calculation.
The study of the influence of the number of nodes on the trajectory length and the time for calculating the trajectory was carried out for various values of the parameter N add = 20, 40, 80. For each number of nodes and the parameter N add , 1000 tests were carried out, the results of which are summarized in Table 1 and presented in Figure 8 and Figure 9. The confidence probability is accepted as 95%. The confidence interval for the path length is 0.026, and the confidence interval for the path calculation time is 0.011.  The N add parameter determines the degree of optimality of the first approximation path. The larger this parameter, the higher the probability of the first approximation path approaching the optimal trajectory.  The average relative path length is calculated according to the expression: where Dl i is the length of the l-th path in the i-th experiment; r rti is the distance from the starting point of the robot's position to the target point in the i-th experiment. The calculation time was measured using the tic and toc MATLAB functions. The table shows the average calculation time calculated in accordance with the expression: where t i is the time of calculating the path in the i-th experiment. As can be seen from Figure 8 and Table 1, the path length decreases with an increase in the number of parent nodes in the RRT algorithm. This is explained by the fact that with an increase in the number of parent nodes, the number of links in the graph under construction increases. This entails increasing the different path options from point (x, y) to the target point (x t , y t ).
It follows from the simulation results that the introduction of an additional parent node results in 6.4 to 10.8% reduction of the planned path length, depending on the N add parameter. The introduction of two additional Reducing the length of the planned path increases the computational cost of the algorithm. Thus, Figure 9 shows the dependence of the average time for calculating the trajectory from a different number of nodes and different values of the parameter N add . From the graphs shown in Figure 9 and Table 1, it can be seen that the increase in calculation time is from 28 to 40% for one additional parent node and from 40 to 45% for two additional parent nodes.
At the same time, we note that an increase in calculation time is associated with both the procedure for constructing a random graph and further searching for a path. When using one parent node, the path from a potential new node only to the nearest graph node is checked. When using additional parent nodes, the path to several nearest nodes of the graph is checked, which leads to additional computational costs.
At the same time, it should be noted that using the ant colony algorithm for finding the shortest path allows parallelizing the computational process [42], which reduces computational costs.

Study of the optimization and smoothing algorithm.
The used method of local optimization and trajectory smoothing is based on the optimization of functional (6). In this case, the main issue in the formation of this functional is the rational choice of the admissible average deviation σ kr of the optimal trajectory from the first approximation path. This conclusion follows from the fact that when the condition δ 1 +δ 2 =1 is satisfied, there is only one independent weighting coefficient. The coefficient δ 1 was chosen as such a coefficient, which is sought using the procedure described in Section 3. Thus, the weighting coefficients of functional (6) are chosen automatically; therefore, it is necessary to choose only the parameter σ kr . The influence of this parameter on the trajectory length is shown in Figure 10 and Figure 11. Figure 10 shows the result of modeling the procedure for finding a displaced trajectory of the minimized length, taking into account the limiting conditions for the following input data: σ kr = 10 m, the length of the original curve is 148.5 m. As a result of applying the procedure of local optimization and smoothing described in Section 3, a trajectory with a length of 71.63 m is obtained. The parameter change interval is δ 1 [0.02 0.98], and step is 1 0, 02 ∆δ = . The optimal value of the parameter is δ 1 = 0.1. In this case, the standard deviation at the nodes of the original and displaced curves is 9.85 m, which is less than the maximum allowable value. Similar simulation results for σ kr = 5 m are shown in Figure 11, taking into account the limiting conditions and for the same initial trajectory of the first approximation as in the previous case. The interval for changing the parameter δ 1 and the step are the same. As a result, the length of the optimized curve is 90.8 m with the optimal value δ 1 = 0.6. Thus, in the first case with σ kr = 10 m, the length of the first approximation path is reduced by approximately 51.8%. In the second case, with σ kr = 5 m, the length of the first approximation path is reduced by about 38.8%. Figure 10 and Figure 11 show the first approximation path obtained using the RRT algorithm and the ant colony algorithm, the trajectories of intermediate iterations, and the optimal trajectory of the movement. The nodes of the optimized trajectory according to the considered algorithm are shown separately.
Based on the simulation, it can be concluded that with the tightening of restrictions on deviations from an initial curve, i.e. as the value of σ kr decreases, the length of the optimized curve increases. Thus, solving the problem of optimizing the original trajectory, taking into account the restrictions on avoiding collisions and not exceeding the maximum permissible standard deviation from the original curve, requires prioritization when answering the question of whether it is more important to significantly reduce the length of the curve, or to prevent deviation from the original curve by more than a given value σ kr .

Study of a trajectory planning and motion control system.
The results of the simulation of the mobile robot control system are shown in Figure 12 -14. Figure 12 shows the obstacles, the virtual obstacle at the initial moment of time, the initial position of the robot with the symbol "*", the target position of the robot with the symbol "+", the path obtained using the RRT algorithm with two-parent nodes, and the smoothed path obtained as a result of smoothing the initial trajectories. To plan the path in Figure 12, the RRT algorithm generated a graph that includes 341 nodes. As a result of the search for the shortest path using the ant colony algorithm, a path was found containing the following 8 nodes, including the starting and target nodes:  The path length obtained as a result of the application of the RRT algorithm is approximately 107 m. The length of the optimized trajectory is 99.9 m. Thus, the reduction in the path is about 6.6%. The results of simulation on a sample of 1000 random situations showed that the smoothing and optimization procedure gives an effect of approximately 6 -8% on the path's length. In this case, the greater the number of intermediate points between the starting and target points, the more decrease in the length of the path. For the first approximation paths consisting of two and three points, the use of the local optimization procedure does not make sense. Figure 13 shows the change in the velocity of the mobile robot in time and the reference velocity. In the time interval from 0 to 5 s, the robot accelerates to V max . Then, in the interval from 5 to 13 seconds, the motion control system reduces the velocity four times. The first three velocity maneuvers are due to turns, and the last speed reduction is due to passing close to an obstacle. When approaching the target point, the robot also slows down.  Figure 14 shows the change in the yaw angle of the robot when moving to the target point. It can be seen from this graph that the robot makes turns at the initial moment, and then three turns when moving between the nodes of the planned path. Figure 15 shows the simulation results demonstrating the movement of the robot in an environment with obstacles, including the movement in a narrow corridor formed by the obstacle and the boundary of the area.  Studies have shown that the presence of narrow corridors can significantly increase the planning time of the robot's trajectory. In this regard, the RRT algorithm uses a variant of generating a new potential node in a certain limited area relative to the last node added to the graph. This way of building a tree gives an additional effect in terms of reducing the length of the planned trajectory. So, for the MPN-RRT algorithm with twoparent nodes, an additional 1000 experiments were carried out with the generation of a new potential node within a radius of 20 meters from the last node added to the tree. An additional reduction in the trajectory length is obtained, ranging from 5 to 10%, depending on the value of the parameter N add . However, in the MPN-RRT algorithm, there is a significant increase in the trajectory search time, which is on average 30% compared to the variant of random generation of a new node over the entire area of operation. This increase in the computation time is observed only on average. For situations with narrow corridors, the algorithm with a local area for generating a new potential node does not lead to an increase in the computation time. In this regard, it is possible to recommend the use of a unit for evaluating the functioning environment in the robot control system. Such a block can be built using intelligent technologies. Thus, [43] presents a neural network that estimates the geometric complexity of the environment. This estimate is used to determine the clearest areas for planning a route for a group of robots. Note that generating a new potential node in a certain bounded area relative to the constructed tree leads to trajectories consisting of a larger number of points. However, the application of the above-described iterative local optimization and smoothing procedure allows obtaining a smooth trajectory with a minimized length. In this case, the effect of reducing the path length becomes higher and amounts to approximately 10% for the simulated movement conditions. A more significant decrease in the trajectory length is due to an increase in the optimization efficiency with an increase in the number of nodal points of the first approximation trajectory. Figure 16 shows the results of one of the numerical experiments in which a new potential node is generated within a radius of 20 meters relative to the last added node. Fig. 16. The robot's trajectory when a new potential node is generated within a radius of 20 meters relative to the last added node 7. Discussion and conclusion. Some discussions about the highlights of the approach proposed in this article are summarized as follows.
1) Taking into account the dynamic and design constraints of mobile robots when planning a path usually leads to the complication of planning algorithms. In this article, it is proposed to take into account the restrictions on the angle of rotation of the robot by introducing virtual obstacles. Such obstacles are located in such a way that they cover sectors into which the mobile robot cannot turn at a given time. As a result, the planned trajectory is physically realizable. Thus, compared to the existing methods of considering dynamic constraints, the proposed approach does not affect the planning method if it uses a map to calculate the trajectory.
2) The potential of using probabilistic methods for path planning is limited by a large number of iterations to obtain the optimal solution. This article proposes an improved MPN-RRT algorithm that uses multiple parent nodes. This algorithm allows creating a graph with a large number of connections, resulting in an increase in the optimality of the planned path.
3) To reduce the number of iterations in the path search, a twostage path planning method is proposed. At the first stage, the first approximation path is searched using the proposed MPN-RRT algorithm. At the second stage, the resulting trajectory is optimized and smoothed by an iterative local search algorithm. Compared to the one-stage MPN-RRT algorithm, the two-stage method, which includes MPN-RRT and an iterative optimization and smoothing procedure, reduces the time it takes to find a trajectory of the same length by 7-13%. This effect is due to the slow convergence of algorithms based on the probabilistic method.
4) The numerical experiments performed have shown that for a simulated urban environment, the MPN-RRT algorithm with two-parent nodes finds a trajectory of the same length as the original RRT algorithm approximately 30-40% faster. Adding a third parent node reduces the path search time by roughly 25%. With a fixed trajectory calculation time, the introduction of an additional parent node reduces the trajectory length by 6-11%. The introduction of two additional parent nodes reduces the length of the trajectory by 15-20% compared to the original RRT algorithm.