Synthesis of Trajectory Planning Algorithms Using Evolutionary Optimization Algorithms

Abstract

The article considers the problem of planning the optimal trajectory of a delta robot. The workspace of the robot is limited by the range of permissible values of the angles of the drive revolute joints, interference of links and singularities. Additional constraints related to the presence of obstacles have been introduced. Acceptable values of the robot’s input coordinates are obtained based on the inverse kinematics, taking into account the constraints of the workspace, represented as a partially ordered set of integers. For the given initial and final coordinates, a randomly generated family of trajectories belonging to a valid set is obtained. Optimization of each of the trajectories of the family based on evolutionary algorithms is performed. The optimization criterion is a function proportional to the duration of movement along the trajectory. The results of modeling are presented.

This work was supported by the state assignment of Ministry of Science and Higher Education of the Russian Federation under Grant FZWN-2020-0017.

1 Introduction

Robot trajectory planning is essential for avoiding various obstacles and obtaining the optimal path in terms of various criteria. Currently, there are a number of methods that can be used for trajectory planning. Some of the well-known methods are based on route networks, including the method based on the application of Visibility Graphs [1]. An alternative method for determining routes is based on the use of Generalized Voronoi Diagrams [2]. Thus, the method of uninformed search allows implementing breadth-first search, depth-first search, search by cost criterion [3]. Heuristic pathfinding algorithms are designed to quickly find a route in a graph by propagating towards more promising vertices [4, 5]. In recent years, a number of methods for trajectory planning have been proposed. A two-stage method for planning the trajectory of two mobile manipulators for joint transportation in the presence of static obstacles is considered in [6]. At the first stage of path planning in progress, the shortest possible path between the initial and target configuration is executed in the workspace. The second stage consists in calculating a sequence of time-optimal trajectories for passing between consecutive points of the path, taking into account non-holonomic constraints and maximum permissible joint accelerations. A new method of spatial decomposition is presented in [7]. It was applied to define a space for trajectory planning, called RV-space. In [8], the method of directed reachable volumes (DRVs) is presented, which makes it possible to obtain the area taking into account constraints on the positions of the robot’s links and end-effector. The current work considers the application of evolutionary and bio-inspired algorithms for planning the trajectory of a delta robot, taking into account the limitations of its workspace.

Fig. 1.

Structure of the delta robot

2 Setting an Optimization Problem

The delta robot [9] with 3 degrees of freedom is showed on Fig. 1. The end-effector of delta robot is the center P of the moving platform with $x_P,y_P,z_P$ coordinates.

The input coordinates for a delta robot are the angles $\theta _{i}$ of rotation of the drive revolute joints $A_{i}$. Inverse kinematics allows to transfer the described constraints of the workspace to the space of input coordinates. Inverse kinematics [10]:

$$\begin{aligned} \theta _{i}=2 \tan ^{-1}\left( \frac{-F_{i} \pm \sqrt{E_{i}^{2}+F_{i}^{2}+G_{i}^{2}}}{G_{i}-E_{i}}\right) \end{aligned}$$

(1)

where $E_i$, $F_i$ and $G_i$ defined as $E_{1}=2 d\left( y_P+\frac{a-2 c}{2 \sqrt{3}}\right) , F_{1}=2 z_P d$,

$$\begin{aligned}&G_{1}=v+\left( \frac{a-2 c}{2 \sqrt{3}}\right) ^{2}+2 y_P\left( \frac{a-2 c}{2 \sqrt{3}}\right) -e^{2}, E_{2}=-d\left( \sqrt{3}\left( x_P+\frac{2 c-a}{4}\right) +y_P+\frac{2 c-a}{4 \sqrt{3}}\right) ,\\&F_{2}=2 z_P d, G_{2}=v+\left( \frac{2 c-a}{4}\right) ^{2}+\left( \frac{2 c-a}{4 \sqrt{3}}\right) ^{2}+2\left( x_P\left( \frac{2 c-a}{4}\right) +y_P\left( \frac{2 c-a}{4 \sqrt{3}}\right) \right) -e^{2},\\&F_{3}=2 z_P d, G_{3}=v+\left( \frac{2 c-a}{4}\right) ^{2}+\left( \frac{2 c-a}{4 \sqrt{3}}\right) ^{2}+2\left( y_P\left( \frac{2 c-a}{4 \sqrt{3}}\right) -x_P\left( \frac{2 c-a}{4}\right) \right) -e^{2}, E_{3}\\&=\,d\left( \sqrt{3}\left( x_P-\frac{2 c-a}{4}\right) -y_P-\frac{2 c-a}{4 \sqrt{3}}\right) , v=x_P^{2}+y_P^{2}+z_P^{2}+d^{2} \end{aligned}$$

An arbitrary trajectory can be represented as a set of movements (steps), during which the revolute joint drives operate at a constant angular velocity and the movement in the space of input coordinates is rectilinear. In order to reduce the duration of such small movements, the highest of the angular speeds of the drives at each step should correspond to the maximum possible. The duration of a move is proportional to the sum of individual steps defined in accordance with the Chebyshev metric:

$$\begin{aligned} t=\frac{1}{\omega _{\max }} \sum _{i=1}^{n} \rho _{i} \end{aligned}$$

(2)

where $\rho _i= \max _{j \in \{1,2, \ldots , m\}}\left| \theta _{i, j}-\theta _{i-1, j}\right|$ - according to Chebyshev distance between the points of the beginning of $C_{i-1}(\theta _{i-1,1}, \theta _{i-1,2},..,\theta _{i-1,m})$ and end with $C_{i-1}(\theta _{i,1}, \theta _{i,2},..,\theta _{i,m})$ i-th step; m is the number of the input coordinates (for the Delta robot m = 3); $\omega _{max}$ is the maximum angular velocity of the drive revolute joints. However, the direct application of this indicator as a criterion function for optimizing the trajectory is impractical, since the Chebyshev metric introduces significant ambiguity. On the other hand, using the criterial “usual” length of the trajectory (the sum of the Euclidean lengths of all steps) as a criterion function also not allowed. The duration of movement in this case may be far from optimal as a result. Therefore, it is proposed to supplement the criterion function with a Euclidean metric taken with a small weighting coefficient $\epsilon$:

$$\begin{aligned} F=\sum _{i=1}^{n}\left( \max _{j \in \{1,2, \ldots , m\}}\left| \theta _{i, j}-\theta _{i-1, j}\right| +\varepsilon \sqrt{\sum _{j=1}^{m}\left( \theta _{i, j}-\theta _{i-1, j}\right) ^{2}}\right) \rightarrow \min \end{aligned}$$

(3)

The approach based on the application of a criterion function of this type was successfully tested by the authors to optimize the 3-RPR mechanism’s trajectory [11]. Optimization should be carried out with constraints on the size of the workspace. In the framework of previous works, the authors proposed to use the representation of the workspace in the form of a partially ordered set of integers $A_P$ [12]. Therefore, checking the optimization constraint consists of two steps.

The First Stage. Definition of the Set B of Trajectory Coordinates in the Space of Integers

For this purpose, an algorithm based on a modification of the algorithm is developed Bresenham’s algorithm [13], which assumes that the trajectory is represented as a polyline consisting of many segments. In [14], a modification of the algorithm was proposed for the 3D case, but the coordinates of the beginning and end of the segments belong to the space of integers, which leads to a displacement of the trajectory segment and the set B (Fig. 2). Cells that intersect the orthosis are highlighted in red for coordinates represented as integers, yellow for coordinates represented as real numbers, and orange for both cases. As you can see from the figure, using integer coordinates does not allow you to accurately determine the set B.

Fig. 2.

Offset of the trajectory segment depending on the source data

Modifying the algorithm considering the original data that belongs to the 3D space of real numbers (coordinates of the beginning $x_1, y_1, z_1$ and end $x_2, y_2, z_2$ segments). In this case, the coordinates must correspond to the covering set of the workspace, represented as a partially ordered set of integers, respectively, they must be obtained taking into account the accuracy of the approximation $\varDelta _j$ and the displacement $k_j$ along the j coordinate axes by the formula

$$\begin{aligned} x_{i}=\frac{\theta _{i, 1}+k_{1}}{\varDelta _{1}}, \quad y_{i}=\frac{\theta _{i, 2}+k_{2}}{\varDelta _{2}}, \quad z_{i}=\frac{\theta _{i, 3}+k_{2}}{\varDelta _{2}} \end{aligned}$$

(4)

The algorithm works as follows:

The algorithm for determining the coordinates of the trajectory in the space of integers for a 2D trajectory assumes the exclusion of the z dimension. Otherwise, it is similar to the algorithm given above for the 3D case.

The Second Stage. Checking Whether the Resulting set B Belongs to the Workspace Set $A_\theta$

Thus the optimization constraint condition has the form

$$\begin{aligned} B_i \subset A_\theta ,i \in 1,..,n \end{aligned}$$

(5)

where n is the number of segments that make up the trajectory.

Thus, the optimization problem looks like this.

• parameters: coordinates of intermediate points of the trajectory $x_i, y_i, z_i, i \in 1,..,(n-1)$. For a delta robot, the coordinates are the rotation angles of the drive revolute joints, i.e. $[(x_i y_i z_i )]^T=[(\theta _{i,1} \theta _{i,2} \theta _{i,3} )]^T$.

• parameter change range: overall dimensions of the workspace in the space of input coordinates $\theta _{i,j}\in [\theta _{j,min};\theta _{j,max} ]$.

It should be noted that the optimization parameters change in the space of real numbers. The transition to the space of integers to calculate the cells checked at the second stage is carried out using the formula (4) and the modified Bresenham’s algorithm.

• criterion: the function F calculated by formula (3).

• constraint: condition (5).

To increase the efficiency of optimization in the presence of obstacles, we transfer the optimization constraint to the criterion function

$$\begin{aligned} F^{\prime }=F+\sum _{i=1}^{n}\left( \vartheta _{i}\left( p_{1} \sqrt{\sum _{j=1}^{m}\left( \theta _{i, j}-\theta _{i-1, j}\right) ^{2}}+p_{2}\right) \right) \rightarrow \min \end{aligned}$$

(6)

where $p_{1}$, $p_2$ are the penalty coefficient, and $\vartheta _i$ is the Heaviside function:

$$\begin{aligned} \vartheta _{i}=\left\{ \begin{array}{c} 0, \text {if } B_{i}\subset A\\ 1-\text{ otherwise } \end{array}\right. \end{aligned}$$

(7)

3 Algorithms for a Path Optimization

The choice of algorithms is justified by their efficiency and high level of applicability to a number of different problems. However, the authors do not conclude that these algorithms are better than other evolutionary algorithms for solving this particular problem. The purpose of this investigation is an initial assessment of the applicability of some of the most widely used evolutionary and bio-inspired algorithms for optimizing a trajectory within a workspace represented as a partially ordered set of numbers. This creates the prerequisites for further in-depth research, including a comparative analysis of the application of a larger number of algorithms for this problem and the selection of their parameters.

We apply the following evolutionary and bio-inspired algorithms to solve optimization problem.

3.1 Genetic Algorithm (GA)

The basic principles of GA were first rigorously formulated by Holland [15]. The GA works with a population of “individuals”, each representing a possible solution to a given problem. Genetic algorithms are widely applied, including for the synthesis of a control system for robots [16], for planning the trajectory of collaborative robots [17]. We use a modification of the genetic algorithm described earlier in [18]. To speed up the algorithm, we apply parallel computing (Fig. 3). Dashed lines indicate areas where calculations are performed simultaneously.

Fig. 3.

A genetic algorithm using parallel computing

3.2 Particle Swarm Algorithm (PSO)

Particle Swarm Optimization (PSO) is a widely applied two-component swarm-based evolutionary optimization method [19, 20]. The particle swarm algorithm solves the problem by having a population of potential solutions, here called particles, and moving these particles in the search space according to a simple mathematical formula over the position and velocity of the particle. The movement of each particle depends on its local best-known position, but is also directed to the best-known positions in the search space, which are updated as other particles find better positions. This is expected to move the swarm towards better solutions [18]. New position of the $s_i$ particle At time t is determined by the vector of its coordinates $q_i$, and its velocity by the vector $\vartheta _i$:

$$\begin{aligned} q_{i,t+1}=q_{i, t}+v_{i, t+1}; \end{aligned}$$

(8)

$$\begin{aligned} v_{i,t+1}=\alpha v_{i,t}+P_{n}[0 ; \beta ] \times \left( q_{G,t}-q_{i,t}\right) +P_{n}[0 ; \gamma ] \times \left( q_{P,t}-q_{i,t}\right) \end{aligned}$$

(9)

where $P_n$ [a; b] is an n-dimensional vector of pseudorandom values uniformly distributed over the interval [a,b];$q_{G,t}$ is the coordinate vector of the best particle in the group; $q_{B,t}$ is the coordinate vector of the best in population particle; $\alpha$, $\beta$, $\gamma$ are free parameters of the algorithm with the following recommended values: $\alpha$ = 0.7, $\beta$ = 1.4, and $\gamma$ = 1.4 according to [18]. To speed up the work, parallel calculations are applied in the same way when determining the new position of particles and the value of the criterion function in the new position.

3.3 Grey Wolf Algorithm (GWO)

Algorithm The Grey Wolf Optimization (GWO) algorithm [21] shows its reliability in solving real optimization problems where the objective function is not linear. The study in [21] shows that the PSO and GWO algorithms show better results in comparison with a number of other algorithms. The paper [22] presents a method for optimal trajectory generation (OTG) for creating a simple and error-free continuous motion along a trajectory with fast convergence using the GWO method. The authors of [23] compared the GA, PSO, and GWO algorithms for optimizing efficient hybrid robot control for controlling the foot trajectory of a robot during walking. The results showed that the GWO algorithm performs more efficiently and quickly at similar torques for configuring a hybrid controller based on LQR (Linear quadratic regulator) and PID (proportional–integral–derivative controller) than other traditional algorithms. Based on these works, newer methods for controlling robot navigation were also developed, which uses a hybrid concept of using the GWO algorithm and the artificial potential field (APF) method for planning the trajectory of a mobile robot [24].

We apply a modification of the GWO algorithm described earlier in [25] using parallel calculations to modify the parameters of a possible solution and determine the value of the fitness function.

4 Numerical Results

The problem of determining the workspace $A_P$ for a delta robot in the space of output coordinates is considered by the authors in [26]. The constraints of the workspace $A_P$ are transferred from the space of output coordinates $x_P$, $y_P$, $z_P$ to the space of input coordinates $\theta _i$ using the solution of the inverse kinematics (1). The workspace $A_\theta$ in the input coordinate space is similarly represented as a partially ordered set of integers after the transfer. An object was added as an obstacle with a parallelepiped shape (Fig. 4a). Elements of the covering set of the workspace in the output coordinate space (Fig. 4b) belonging to the obstacle were excluded $A_P^\prime =A_P{\setminus }C$. The transfer of constraints $A_P^\prime \rightarrow A_\theta$ (Fig. 4c) was performed for the updated set $A_P^\prime$.

Fig. 4.

Additional boundaries, related to the overall dimensions of the obstacle: a) obstacle C; b) Workspace set $A_P$; c) $A_P^\prime$; c) $A_\theta$

We perform trajectory optimization using the above-mentioned algorithms for the 2D and 3D case of forming a trajectory inside the workspace of a delta robot, as well as for a randomly generated 2D contour with a large number of obstacles. A C++ software package has been developed for this purpose. Parallel computing is implemented using the OpenMP library. Visualization of 2D results is performed using developed Python scripts Python (Matplotlib and JSON libraries). Visualization of 3D results is performed by exporting an ordered set of integers describing the workspace in STL format and arrays of co-ordinates of trajectory points in JSON format, and then importing data in the Blender software package using developed Python scripts.

4.1 2D Case

Let’s make a slice of the workspace $A_\theta$ of the delta robot in the space of input coordinates $\theta _1$ $\theta _2$ $\theta _3$, taking the angle $\theta _1=0$. In this case, the set $A_\theta$ will be 2D (Fig. 5). We assume that the starting point of the trajectory is $\theta _{1,2}=-70^{\circ }$, $\theta _{1,3}=20^{\circ }$, the end point is $\theta _{n,2}=50^{\circ }$, $\theta _{n,3}=0^{\circ }$, and the number of vertices of the trajectory is n = 3. Accordingly, the number of optimization parameters $p=2n=6$. The weight coefficient $\epsilon =0,1$, the penalty coefficients: $p_1=5$, $p_2=500$. Parameters of the GA algorithm: the number of individuals in the initial population H = 1000, the number of generations W = 250, the number of crossovers at each iteration $S_{GA}=500$, the number of possible values of each of the parameters $g=2^{25}$, the probability of mutation $p_m=70\%$. Parameters of the GWO algorithm: H = 1000, W = 250, number of new individuals at each iteration $S_{GWO}$ = 1000. Parameters of the PSO algorithm: H = 1000, W = 250, number of groups G = 2, values of free parameters $\alpha$ = 0.7, $\beta$ = 1.4, $\gamma$ = 1.4. Optimization for each of the tests was performed in four stages. At the first stage, the range of parameters was changed to the ranges corresponding to the overall dimensions of the workspace for each of the coordinates. The parameter ranges at each subsequent stage were reduced by a factor of $10^2$. At the same time, the center of the ranges corresponded to the best result obtained at the previous stage. The optimization results are shown in Table 1. The PSO algorithm provides the best average value of the criterion function.

Table 1. Results table for the 2D case

The obtained trajectories for all tests are almost identical. Examples of trajectories for the first and second tests are shown in Fig. 5. In the second test, the GWO algorithm obtained the trajectory with the largest value of the criterion function. This is clearly seen in Fig. 5b.

Fig. 5.

Results of optimization of the test path: a) 1, b) 2.

The convergence graphs for each of the algorithms are shown in Fig. 6. The minimum value of the criterion function obtained as a result of optimization is applied as the reference value of the function. The minimum value of the criterion function in one of the tests was obtained using the GA algorithm, but in other tests, the PSO algorithm has better convergence rates.

Fig. 6.

Convergence of algorithms for planning a 2D trajectory: a) GA, b) GWO, c) PSO

4.2 3D Case

A computational experiment is performed, which consists in planning the trajectory inside the 3D workspace $B_{\theta }$ of a delta robot in the space of input coordinates, taking into account the obstacle, shown in Fig. 4c. Set the starting and ending points of the trajectory in the output coordinate space: $x_{p1}=250$ mm, $y_{p1}=250$ mm, $z_{p1}=-500$ mm, $x_{p2}=-270$ mm, $y_{p2}=-270$ mm, $z_{p2}=-450$ mm, and the number of vertices of the trajectory $n=3$. Accordingly, the number of optimization parameters p = 3n = 9. Let’s take the parameters of the algorithms $H=250, W=200, S_{GA}=125$, and $S_{GWO}=250$. The remaining parameters of the computational experiment coincide with the 2D case. Optimization for each of the tests is performed in four stages, similar to the 2D case. The optimization results are shown in Table 2. In this case, The GA algorithm showed the best average value of the criterion function.

Table 2. Results table for the 3D case

The convergence graphs are shown in Fig. 7. The minimum value of the criterion function is obtained using the PSO algorithm.

Fig. 7.

Convergence of algorithms for planning a 3D trajectory: a) GA, b) GWO, c) PSO.

Figure 8 shows the trajectories for Test 3 inside the workspace. As can be seen from the figure, the PSO algorithm found only a local minimum of the criterion function for avoiding the obstacle.

Fig. 8.

Results of trajectory optimization

4.3 Trajectory Planning When There are a Large Number of Obstacles

In the first two cases, the trajectory was planned inside the workspace of the delta robot in the presence of a single obstacle. To test the algorithms on the problem of planning a trajectory with a large number of obstacles, a 2D domain was generated, similarly represented as an ordered set of integers. During the experiment, 10 tests were performed similarly for the following initial data: $x_1=-95$ mm,$y_1=-95$ mm, $x_2=85$ mm, $y_2=85$ mm, the number of vertices of the trajectory $n=7$. Accordingly, the number of optimization parameters $p=2n=14$. We assume the parameters of the algorithms $H=2000, W=1000, S_{GA}=1000, S_{GWO}=2000, p_m=90\%$. The other parameters were not changed. Optimization for each of the tests was performed in two stages, rather than four. Figure 9 shows examples of the trajectories obtained as a result of optimization. In Fig. 9a, the path that allows you to avoid all obstacles is obtained only for the GA algorithm, in Fig. 9b and c - by the GA and GWO algorithms, in Fig. 9d - by all algorithms. Figure 10 shows an example of the convergence graph of the algorithms. As a result of performing 10 tests for each of the algorithms, the GA algorithm showed the best results GA (Fig. 10a), each time reaching a trajectory that allows to avoid all obstacles. The GWO algorithm (Fig. 10b) it allowed to exclude the interference with an obstacle in 4 cases, and the PSO algorithm-only in one case.

Fig. 9.

The result of planning a trajectory in the presence of a large number of obstacles

Fig. 10.

Convergence of algorithms when planning a trajectory with a large number of obstacles: a) GA, b) GWO, c) PSO.

5 Conclusion

The application of heuristic algorithms made it possible to solve the problem of trajectory planning for both 2D and 3D domains represented as a partially ordered set of integers. The PSO algorithm showed better convergence rates for planning a trajectory within a 2D workspace of a robot with a single obstacle. In all other cases, the GA algorithm showed the better results. As part of future research in-depth research will be carried out, including a comparative analysis of the application of a larger number of algorithms for this problem and the selection of their parameters. Also, more experiments will be performed for accurate comparative evaluation of algorithms.