Adv Syst Sci Appl 2021; 02:71–82 Published online at https://ijassa.ipu.ru. Algorithm for Optimal Two-Link Trajectory Planning in Evasion from Detection Problem of Mobile Vehicle with Non-Uniform Radiation Pattern Andrey Galyaev1, Pavel Lysenko1,2*and Victor Yakhno1 1Institute of Control Sciences of RAS, Moscow, Russia, 117997 2Moscow Institute of Physics and Technology, Dolgoprudny, Russia, 141701 Abstract: The problem of an optimal PP/TP (path/trajectory planning) evasion of a mobile vehicle from the detection is considered. The objective is to minimize the risk of a moving vehicle being detected by a static sensor when moving between two specified points on the plane. Detection is based on the primary acoustic field emitted by a vehicle with an inhomogeneous radiation pattern. An algorithm for finding a two-link optimal trajectory is proposed. The optimal trajectory and the law of speed of a mobile vehicle, as well as the value of the criterion, are found. Keywords: UUV path/trajectory planning; non-detection probability; non-uniform radiation pattern, evasion from detection 1. INTRODUCTION The problems of path/trajectory planning of autonomous and controlled vehicles are currently interesting due to their wide usage. For example, in the evasion problem, the moving object must remain hidden for the detector system [1]. These problems are extremely relevant for modern military applications. Technological advances allow the onboard algorithms of unmanned mobile vehicles to be non-trivial and use solutions from broad scientific fields, such as optimal control and differential games. The problem of evading radar detection, in particular, is considered in [2] as a problem of automated planning of the trajectory of combat unmanned aerial vehicles (UAVs) in the presence of radar-guided surface-to-air missiles. It turns out that the solution of the problem significantly depends on the assumption of the isotropic capabilities of the radar (i.e., the independence of the reflected signal from the spatial orientation of the UAV). In case of isotropic characteristics, an analytical solution for the problem is possible, which facilitates the construction of an onboard control algorithm. Homogeneous radiation capabilities of mobile vehicle are explored in papers [3–8]. The paper considers the problem of the covert infiltration of an unmanned underwater vehicle (UUV) into a given area under the supervision of a stationary passive sonar conducting a search on the primary hydro-acoustic field as in [9]. In contrast to previous works dealing with path planning problems, the acoustic signal emitted by a mobile vehicle has a non-uniform pattern. The task of the UUV optimal route planning from the starting point of space to the end point can be formulated as a problem of calculus of variations. The optimization criterion in this problem is the UUV non-detection probability, e.g. the chance of the sonar to make a decision about UUV presence in the region under study during ∗Corresponding author: PashLys@yandex.ru 72 A. GALYAEV, P. LYSENKO UUV’s passing along the route. The decision rule in threshold statistics is examined to make the detection decision [10]. Let the UUV trajectory be represented by a piecewise linear trajectory with the duration of a movement on each rectilinear section equal to the duration of the observation ”tact”. For each clock cycle, we denote the judgment ”UUV absent” by 0, and the judgment ”UUV present” by 1. Then the results of making decisions on the UUV trajectory can be represented by a sequence of zeros and ones. UUV will not be detected on the path if there are no ones in the corresponding sequence of zeros and ones on the selected path. The optimal trajectory is the one for which the probability of such an event is maximum. Articles [5, 11, 12] are devoted to the problems of optimal planning of the UUV route under threat conditions, in which other probabilistic and energy criteria are implemented. At CoDit 2020 conference results for optimal one-link trajectories of mobile vehicle, evading from one sonar, were presented [13]. The detection of sonar is based on primary hydro-acoustic field signals. But later in [14] it was shown, that sufficient optimal conditions can brake and then multi-link trajectory becomes optimal. In that article problem statements for two-link optimal trajectories were discussed. The current article considers the algorithm for solving of the supplementary problem, needed for two-link optimal trajectories constructing. Firstly, in the article the non-detection probability of UUV on the route is derived. Secondly, a path planning problem is formulated and analytically solved. Later in next chapter the special case of two-link optimal trajectories is explored and the algorithm for finding such trajectories is obtained. Finally, several examples are presented. 2. NON-DETECTION PROBABILITY OF UUV UNDER PASSIVE SURVEIL- LANCE The UUV covertness on the selected trajectory with a given speed law change can be characterized by the probability Pnd that during the passage of the route it will not be detected at any tact. This probability is denoted Pnd and will be called the non-detection probability of UUV on the trajectory. In the case of one sonar this probability is [3] Pnd = ∏ j Fn  χ2 1−α,n 1 + σ2 0(υj/υ0) µr20 σ2 nr 2 j γ  , (2.1) where Fn is χ2 is a probability distribution with n degrees of freedom, α is a false alarm probability, γ is an attenuation coefficient, rj is the distance from UUV to sonar at j tact, starting from the moment of appearance of UUV on the trajectory, and υj is a constant speed of UUV at this tact, σ0, σn, µ, r0, υ0 are some model parameters. The number of factors in the product is equal to the number of tacts when moving UUV along a trajectory. In the case of several sonars, the product of expressions of the form (2.1) over all available sonars is taken [1]. Thus the formalization of the UUV covertness concept is reduced to the formula (2.1). Now the task is to construct the optimal trajectory and the optimal law of the UUV velocity variation, maximizing the probability Pnd. Based on actual algorithms for information processing, using decisive threshold rules, in the article [13] a formula was obtained for calculating the risk of mobile vehicle detection. It is represented as an integral, which we call the threat functional [6], [13] in the problem of UUV route planning for the case of passive sonar R = ∫ T0 0 σ2 s(t) σ2 n(t) dt. (2.2) Copyright © 2021 ASSA. Adv Syst Sci Appl (2021) ALGORITHM FOR OPTIMAL TWO-LINK TRAJECTORY PLANNING IN EVASION 73 This conclusion coincides with the result given in [3] for σ2 s = σ2 0 (υj/υ0) µ r20 r2 γ, and σ2 n(t) = const. (2.3) Moreover, for the path planning task, it is not required to know the exact values of the information processing parameters because they are not included in (2.2). 3. PATH PLANNING PROBLEM 3.1. Risk functional To formulate the path planning problem as the problem of calculus of variations let us rewrite the risk functional (2.2). Due to (2.3), after omitting the constants the expression in integral can take a general form as a power model σ2 s(t) σ2 n(t) ∼ vµ rk . (3.4) As well as in [1] from the physical point of view this function is the instantaneous level of signal S, received by the sensor. It depends on the current distance between sensor and evading object r, for some types of physical fields – on absolute instant velocity of the object v too and can be represented as S = vµ rk . (3.5) The exponents k and µ characterize the physical field used for detection. Depending on values of k and µ this can be magnetic, thermal, acoustic or electromagnetic fields. Moreover, this signal depends on the receiving diagram of the sensor and the radiation pattern of the object S = vµ rk A0(ϕ)g(ψ, ϕ), (3.6) where multiplierA0(ϕ) is responsible for diagram of sensor antenna and g(ψ, ϕ) is a radiation pattern of the moving object. The risk R is the integral value of this signal, so the criterion of optimization (2.2) is a function of phase coordinates of the object and qualities of the sensor and the object itself: R = ∫ T0 0 ( vµ rk A0(ϕ)g(ψ, ϕ) ) dt. (3.7) We consider sensor antenna diagram to be homogeneous, so A0(ϕ) ≡ 1. The geometric meaning of angles ψ and ϕ is as follows. Here ψ is the angle of rotation of object’s velocity vector and ϕ is the angle of rotation of radius vector as shown in Fig. 3.1. We study the case of k = 2 and µ = 2 for acoustic field. Non-uniform radiation pattern of the object can be described as g(ψ, ϕ) = g(β). Thus signal expression (3.6) has a form S = v2 r2 g(β). Copyright © 2021 ASSA. Adv Syst Sci Appl (2021) 74 A. GALYAEV, P. LYSENKO Fig. 3.1. The moving object in the Cartesian coordinate system with sensor S. Fig. 3.2. Rectangular triangle of velocity vectors ~v,~vr and ~vϕ. 3.2. Mathematical statement of the problem Therefore we can formulate the optimal path planning problem. Problem 3.1: It is required to find such trajectory (r(t), ϕ(t)), which minimizes the functional R = ∫ T0 0 v2 r2 g(β)dt, (3.8) where v is a velocity of the object, r is the distance between the sensor and the object. Boundary conditions are r(0) = xA, r(T0) = rB, ϕ(0) = ϕA, ϕ(T0) = ϕB. (3.9) Time T0 of moving on route from point A to B is fixed. The detection system consists of one static sensor placed in the origin of Cartesian coordinate system with X axis passing point A. The polar coordinate system is used for solution simplicity. It is easy to see that ψ − ϕ = β is the angle in triangle built from radial vr and transversal vϕ velocities of the object, e.g. the angle between velocity of the object and its projection on radius vector as shown in Fig. 3.2. Next lemma is valid. Lemma 3.1: Substitution of variable ρ = ln r brings functional (3.8) to the form R(ρ(·), ϕ(·)) = ∫ T0 0 Sdt = ∫ T0 0 (ρ̇2 + ϕ̇2)g ( arctan ϕ̇ ρ̇ ) dt. (3.10) Copyright © 2021 ASSA. Adv Syst Sci Appl (2021) ALGORITHM FOR OPTIMAL TWO-LINK TRAJECTORY PLANNING IN EVASION 75 A proof for Lemma 3.1 can be found in [13]. Instead of solving the original Problem 1 according to Lemma 2 it is necessary to solve a two point boundary value variational problem on the minimum of the functional (3.10). Problem 3.2: It is required to find the trajectory (ρ∗(t), ϕ∗(t)), which minimizes the functional R(ρ(·), ϕ(·)) = ∫ T 0 ( ρ̇2 + ϕ̇2 ) g ( arctan ϕ̇ ρ̇ ) dt→ min ρ(·),ϕ(·) . (3.11) with boundary conditions ρ(0) = ρA, ρ(T ) = ρB, ϕ(0) = ϕA, ϕ(T ) = ϕB. 4. SOLUTION OF THE PATH PLANNING PROBLEM 4.1. The Necessary Optimality Conditions Theorem 4.1: Suppose that 0 < g1 < g(β) < g2 for all β ∈ [0, 2π] is a twice continuously differentiated function of β, where g1, g2 are some constant values, and ρ̈(t), ϕ̈(t) exist and are continuous functions of t. Then the extremal trajectory satisfies the following system of equations{ ρ̇ = const, ϕ̇ = const. (4.12) Thus the extremal trajectory, which can be the solution for Problem 3.2 can be represented in the parametric form of logarithmic spiral r(t) = rA exp ( t T0 ln rB rA ) , ϕ(t) = ϕA + ϕB − ϕA T0 t. (4.13) On the plane (r, ϕ) this line has a form r(ϕ) = rA exp ( ϕ− ϕA ϕB − ϕA ln rB rA ) . (4.14) Next two lemmas define the velocity law of the vehicle on the extremal trajectory and the risk value on it. Lemma 4.1: The velocity law on the extremal trajectory Equation (4.14) is represented as follows v(t) = rA T exp ( t T ln rB rA )√ ln2 rB rA + (ϕB − ϕA)2. (4.15) Lemma 4.2: The explicit dependence risk (3.11) from boundary conditions on the extremal trajectory Equation (4.14) has the form R∗ = (ρB − ρA)2 + (ϕB − ϕA)2 T g(β0) = L2 T g(β0), (4.16) Copyright © 2021 ASSA. Adv Syst Sci Appl (2021) 76 A. GALYAEV, P. LYSENKO where β0 = arctan ϕB − ϕA ρB − ρA , and L = √ (ρB − ρA)2 + (ϕB − ϕA)2 is the length of the straight line segment between points A and B in the space (ρ, ϕ). The proofs of Lemmas 4.1 and 4.2 remain valid as shown in Reference [9]. 4.2. The Sufficient Optimality Conditions The sufficient optimality conditions for path planning problem 3.2 and the Hessian matrix explicit form are presented in [9] as follows Lemma 4.3: Let S(ρ, ρ̇, ϕ, ϕ̇, t) = ( ρ̇2 + ϕ̇2 ) g (β), g(β) – thrice continuously differentiated function of β, then Hessian matrix H equals H = ( H11 H12 H21 H22 ) where H11 = ∂2S ∂ρ̇2 = 2g (β)− 2ϕ̇ρ̇g′ (β)− g′′ (β) ϕ̇2 ϕ̇2 + ρ̇2 , H12 = ∂2S ∂ρ̇∂ϕ̇ = ( ρ̇2 − ϕ̇2 ) g′(β)− g′′(β) ϕ̇2 + ρ̇2 , H21 = ∂2S ∂ϕ̇∂ρ̇ = ( ρ̇2 − ϕ̇2 ) g′(β)− g′′(β) ϕ̇2 + ρ̇2 , H22 = ∂2S ∂ϕ̇2 = 2g (β) + 2ϕ̇ρ̇g′ (β) + g′′ (β) ρ̇2 ϕ̇2 + ρ̇2 , (4.17) and the Hessian itself is the determinant of the matrix detH = 4g2(β) + 2g(β)g′′(β)− g′2(β). (4.18) Theorem 4.2: Assume that the conditions of Theorem 4.1, Lemma 4.3 are satisfied, and the inequality detH > 0 is valid for all values β. Then the optimal trajectory given by Equation (4.12) brings the strong minimum to the risk functional Equation (3.11). 5. ALGORITHM FOR FINDING TWO-LINK OPTIMAL TRAJECTORIES If conditions of theorem 4.2 are not fulfilled, e.g. detH ≤ 0, then optimal trajectory can consist of many segments of logarithmic spirals. The current article explores the case of two segments, however it can be shown that any optimal multi-link trajectory is constructed from segments of two base directions. As explained above, these segments in (ρ, ϕ) space transform into straight lines. This fact is illustrated in Figure 5.3. Copyright © 2021 ASSA. Adv Syst Sci Appl (2021) ALGORITHM FOR OPTIMAL TWO-LINK TRAJECTORY PLANNING IN EVASION 77 0 ρ− ρ0 ϕ− ϕ0 L0 L1 L2 β0 β1 β2 Fig. 5.3. The mobile vehicle in the (ρ, ϕ) coordinate system. Next Lemma allows to choose two optimal angles β1 and β2 from the whole set of such angles, fulfilling the boundary conditions of the Problem. A corresponding to these angles optimal risk is denoted as R∗(β1, β2) and is calculated according to Lemma 4.2. Lemma 5.1: Angles β∗1 , β ∗ 2 for optimal trajectory can be found from system cos(β2 − β1) = cos(ξ(β1)− ξ(β2)), g(β2) g(β1) = cos2 ξ(β2) cos2 ξ(β1) , (5.19) where function ξ(β) = arctan 1 2 g′(β) g(β) . We introduce two new functions χ(β) = β + ξ(β), η(β) = g 1 2 (β) cos ξ(β) (5.20) and rewrite the system (5.19) as { χ(β1) = χ(β2), η(β1) = η(β2). (5.21) Notice that cos ξ(β) > 0 according to definition function ξ(β) in Lemma 5.1. Thereby the search of optimal two-link trajectories is reduced to the solution of the system (5.21). Unfortunately, it can not be solved analytically, so we have developed a specific algorithm for finding solutions of system (5.21). First of all, let us investigate each of the functions χ(β) and η(β) by extreme’s and find their first derivatives dχ(β) dβ = 4g2(β)− (g′(β))2 + 2g′′(β)g(β) 4g2(β) + (g′(β))2 . The numerator of this fraction is a part of expression for detH, so dχ(β) dβ = 0 ⇐⇒ detH = 0. Copyright © 2021 ASSA. Adv Syst Sci Appl (2021) 78 A. GALYAEV, P. LYSENKO As for η(β) function we have dη(β) dβ = g 1 2 (β)g′(β) 4g2(β) √ 4g2(β) + g′2(β) detH From last expression follows dη(β) dβ = 0 ⇐⇒ detH = 0 or g′(β) = 0. The illustration of the Theorem 4.2 implementation is as follows. If detH > 0 for all β, then dχ(β) dβ > 0, i.e. the function χ(β) increases monotonically and therefore there is no solution for the first of the equations (5.21) and one-link trajectory is optimal. Now we propose algorithm for finding system (5.21) solution. We consider β ∈ [0◦, 450◦] and denote all intervals β ∈ (a2i−1, a2i), i = 1, .., N , where detH > 0. Algorithm 1: Algorithm for finding optimal values of β1, β2 Result: β∗1 , β∗2 initialization β1 = a1, β2 = a3, R∗ = R(β1, β2) while i < N , i = i+ 1 do k = i+ 1; while k < N , k = k + 1 do find system (5.21) single solution (β1, β2), β1 ∈ [a2i−1, a2i], β2 ∈ [a2k−1, a2k]; if such solution exists then calculate R(β1, β2); if R(β1, β2) < R∗ then R∗ = R(β1, β2); (β∗1 , β ∗ 2) = (β1, β2); else continue end else continue end end end The proposed algorithm has the following advantages. It looks for solutions on reduced β domains only where detH > 0 and additionally where solution is possible. All possible optimal directions are found, regardless of the initialization values of the algorithm, as will be shown in the example. The optimal value of the criterion depends on the directions β1 and β2 and the length of the corresponding trajectory segments. Moreover, that is an efficient algorithm. The only other way to find optimal angles β1, β2 is a brute force approach of sorting through all possible two-link trajectories, which is a much more time-consuming process, compared to the algorithm above. 6. EXAMPLE This section demonstrates the use of proposed algorithm in one model case. The considered algorithm has been implemented in Matlab. Also Matlab scripts have been developed to validate and illustrate obtained results. Copyright © 2021 ASSA. Adv Syst Sci Appl (2021) ALGORITHM FOR OPTIMAL TWO-LINK TRAJECTORY PLANNING IN EVASION 79 Figures 6.4 and 6.5 show a fairly simple radiation pattern of a mobile vehicle g(β) = 1 + 0.3 cos(4β) 1.3 . (6.22) Fig. 6.4. Radiation pattern of the mobile vehicle on Cartesian plane Fig. 6.5. Radiation pattern of the mobile vehicle as a function of β Figure 6.4 shows a radiation pattern on a Cartesian plane. This pattern is symmetrical in respect to both Cartesian axes. The solid line represents the function g(β) level line. The value g(β) is a radius-vector modulo directed from zero point to point lying on the shown level line. Figure 6.5, on the other hand, presents a radiation pattern as a dependence from β. Figures 6.6 and 6.7 demonstrate functions χ(β) and η(β) respectively. Blue color solid line segments on those graphs are assigned to intervals β ∈ (a2i−1, a2i), i = 1, .., N , where detH > 0. We see that in the example N = 5. Fig. 6.6. Function χ(β) Copyright © 2021 ASSA. Adv Syst Sci Appl (2021) 80 A. GALYAEV, P. LYSENKO Fig. 6.7. Function η(β) Let us introduce notation a′i for further exploration. We declare that for β ∈ [a2i−1, a2i] and fixed k a′2i−1 = max(a2i−1, arg(χ(β) = χ(a2k−1))), a′2i = min(a2i, arg(χ(β) = χ(a2k))). This notation takes place because χ(β) is monotonically increasing function on β ∈ [a2i−1, a2i]. Further we need to narrow down the exploration area on β of functions χ(β), η(β). Figures 6.8 and 6.9 correspond to the first pair of narrowed intervals of hessian positivity [a′1, a ′ 2] and [a′3, a ′ 4]. Black dots show found at this iteration of the algorithm 1 solutions β1, β2. Fig. 6.8. Enlarged segment of function χ(β). Fig. 6.9. Enlarged segment of function η(β). So the domain of values of function χ(β) when β belongs to the first interval [a1, a2] do not cross with the same one when β belongs to the third, forth and fifth interval [a5, a6], [a7, a8], [a9, a10], then the algorithm continues searching the solution comparing intervals two and three on β, then three and four, and so on. Candidates of optimal pairs of (β1, β2) are approximately (59◦, 121◦), (149◦, 211◦), (239◦, 301◦), (329◦, 391◦). These directions and trajectories are presented in Figure 6.10. If the direction β0 lies in the domain where detH > 0 then the optimal trajectory is one-link one. On the contrary if the direction β0 lies in the domain where detH < 0 then two-link trajectory is optimal. Found by algorithm 1 optimal pairs (β1, β2) in every domain where detH < 0 are the same as shown on Figure 6.10. Copyright © 2021 ASSA. Adv Syst Sci Appl (2021) ALGORITHM FOR OPTIMAL TWO-LINK TRAJECTORY PLANNING IN EVASION 81 Fig. 6.10. Optimal trajectories for different β0. 7. CONCLUSIONS The problem of minimizing the risk of UUV detection by a static sensor while moving between two given points on a plane is solved. The detection is based on the primary acoustic field radiated by the vehicle with a non-uniform radiation pattern, which differs this work form the previous researches in the area. In the first part of the article, the non-detection probability is derived. Further it is used as an optimization criterion in the path planning problem. The optimal trajectory and velocity law of the UUV are found, as well as the risk value. A case of the two-link optimal trajectories is studied, when sufficient optimal conditions are not fulfilled. A special algorithm has been developed for finding optimal angles of logarithmic segments. It allows to solve the whole problem and find optimal angles β1, β2. In practical applications this algorithm allows to preprocess these angles for all initial conditions of β0 for a particular radiation pattern g(β) of the mobile vehicle, store it in memory and use later this information for onboard calculations. This approach saves a lot of computational power, which is very important for resource-efficient missions. ACKNOWLEDGEMENTS The work of A.A.G. was partially supported by Programm of Basic Research of RAS; the work of P.V.L. was partially supported by Russian Foundation for Basic Research grant 20- 38-90215; the work of V.P.Y. was partially supported by Programm of Basic Research of RAS. The Authors declare that there is no conflict of interest. REFERENCES 1. Galyaev, A. A. & Maslov, E. P. (2010) Optimization of a mobile object evasion laws from detection. J. Comput. Syst. Sci. Int., 49, 560–569. 2. Kabamba, P. T., Meerkov, S. M., & Zeitz, F. H. (2006) Optimal path planning for unmanned combat aerial vehicles to defeat radar tracking. J Guid Control Dynam, 29, 279–288. 3. Sysoev, L. P. (2011) Detection probability criterion on the path for mobile object control problem in conflict environment. Autom Remote Control, 72, 65–72. Copyright © 2021 ASSA. Adv Syst Sci Appl (2021) 82 A. GALYAEV, P. LYSENKO 4. Dogan, A. & Zengin, U. (2006) Unmanned aerial vehicle dynamic-target pursuit by using probabilistic threat exposure map. J Guid Control Dynam, 29, 944–954. 5. Galyaev, A. A., Lysenko, P. V., & Yakhno, V. P. (2020) Moving object evasion from single detector at given speed. Probl. Upr., 83–91. 6. Zabarankin, M., Uryasev, S., & Pardalos, P. (2002) Optimal risk path algorithms. In Murphey, R. & Pardalos, P. (eds.), Cooperative Control and Optimization, (pp. 273– 298), Dordrecht: Kluwer Acad. 7. Sidhu H, M. G. & M, S. (2006) Optimal trajectories in a threat environment. JBT , 9, 33–39. 8. Pachter, L. S. & Pachter, M. (2001) Optimal paths for avoiding a radiating source. In Proceedings of the 40th IEEE Conference on Decision and Control, vol. 4, 3581–3586. 9. Galyaev, A. A., Dobrovidov, A. V., Lysenko, P. V., Shaikin, M. E., & Yakhno, V. P. (2020) Path planning in threat environment for uuv with non-uniform radiation pattern. Sensors, 20. 10. Lehmann, E. L. & Romano, J. P. (2005) Testing statistical hypotheses. Springer Texts in Statistics, New York: Springer. 11. Rubinovich, E. Y. & Andreev, K. V. (2016) Moving observer trajectory control by angular measurements in tracking problem. Autom Remote Control, 77, 106–129. 12. Mercer, G. N. & Sidhu, H. S. (2007) Two continuous methods for determining a minimal risk path through a minefield. In Read, W. & Roberts, A. J. (eds.), Proceedings of the 13th Biennial Computational Techniques and Applications Conference, CTAC-2006, vol. 48 of ANZIAM J., 293–306. 13. Galyaev, A. A., Lysenko, P. V., & Yakhno, V. P. (2020) Evasion from detection of the moving object with non-uniform radiation pattern. In 2020 7th International Conference on Control, Decision and Information Technologies (CoDIT), vol. 1, 463–468. 14. Galyaev, A. A., Lysenko, P. V., & Yakhno, V. P. (2021) 2d optimal trajectory planning problem in threat environment for uuv with non-uniform radiation pattern. Sensors, 21. Copyright © 2021 ASSA. Adv Syst Sci Appl (2021) Introduction Non-detection probability of UUV under passive surveillance Path planning problem Risk functional Mathematical statement of the problem Solution of the path planning problem The Necessary Optimality Conditions The Sufficient Optimality Conditions Algorithm for Finding Two-Link Optimal Trajectories Example Conclusions