Hierarchical real-time optimal planning of collision-free trajectories of collaborative robots

Journal of Intelligent & Robotic Systems (2023) 107:57 https://doi.org/10.1007/s10846-023-01848-9 SHORT PAPER Hierarchical Real-Time Optimal Planning of Collision-Free Trajectories of Collaborative Robots Dalibor Lukáš1 ·Tomáš Kot1 Received: 9 June 2022 / Accepted: 3 March 2023 / Published online: 21 April 2023 © The Author(s) 2023, corrected publication 2023 Abstract In collaborative robotics the manipulator trajectory has to be planned to avoid collisions, yet in real-time. In this paper we pose the problem as minimization of a quadratic functional among piecewise linear trajectories in the angular (joint) space. The minimization is subjected to novel nonlinear inequality constraints that simplify the original non-penetration constraints to become cheap to evaluate in real time while still preserving collision-avoidance. The very first and most critical step of the computation is to find an initial trajectory that is free of collisions. To that goal we minimize a weighted sum of the violated constraints until they become feasible or a maximal number of steps is reached. Sometimes an incremental growing of the obstacle helps. By incremental growing we mean that we sequentially solve auxiliary subproblems with obstacles growing from ground or falling from top and use as the initial trajectory the one optimized in the previous step. The initial trajectory is then optimized while preserving feasibility at each step. We solve a sequence of simple-bound constrained quadratic programming problems formulated in the dual space of Lagrange multipliers, which are related to the original linearized inequality constraints that are active or close-to-active. Finally, we refine the trajectory parameterization and repeat the optimization, which we refer to as an hierarchical approach, until an overall prescribed time limit, being well below a second, is reached. Keywords Real-time ·Collision-free ·Path planning ·Hierarchical optimization method ·Steepest-descent ·Active-set · Inverse kinematics 1 Introduction Collaborative robotics is an essential topic in the robotic research [1]. A human operator is allowed to support the robotic manipulator with some actions in the working environment [2–4]. The sensoric system, typically depth cameras [5], of the robotic manipulator announces a presence of an obstacle. The control system has to be updated with a new collision-free trajectory. This turns out to be a nontrivial task especially as the new trajectory has to be available in much less than a second, which we refer to as real-time. There is a lot of algorithmic approaches, but basically the problem is split into two. First of all we need to find a feasible, i.e., collision-free trajectory. Secondly, the trajectory is optimized with respect to energy, for instance. For BDalibor Lukáš [email protected] 1VŠB–Technical University of Ostrava, 17. listopadu 15, 708 00 Ostrava-Poruba, Czech Republic the former task, which are referred to as planning problems, variants of graph search algorithms are often employed to arrive at a series of way-points [6,7]. On the other hand optimization problems or nonlinear integrators solve the latter task to follow the way-points in an optimal manner [8–10]. When a real-time collision-free planning [11] is required the graph or discrete planning algorithms become tricky to use especially in case of a high-dimensional configuration space. The main novelty of the present paper is to propose for such higher-dimensional problems a new fast planning algorithm implemented in the C programming language. It relies on tailored collision indicators. Additionally our algorithm is adjusted with two novel strategies: an incremental growing ofobstaclesandahierarchicalexpansionoftrajectorycontrol points. Prior to trajectory planning the inverse kinematics [12,13] and dynamics [14,15] of the robotic manipulator is treated. The manipulators may have more degrees of freedom (DOF) than necessarry, which can make the collision-free trajectory planning easier. However, one has to additionally treat singu0123456789().: V,-vol 123 57 Page 2 of 14 Journal of Intelligent & Robotic Systems (2023) 107:57 larsolutions.IncasetheDOFsaredefinedinjointstheinverse kinematic problem can be seen as finding real-valued roots of a matrix polynomial. Unfortunately, the numerical stability is not guaranteed. When studying such a system, namely the inverse kinematics of the UR3 manipulator, in this paper we develop a geometric insight into the structure of the roots. To our best knowledge these particular derivations have not been published in literature yet. The rest of the paper is organized as follows: In Section 2we put our work in the context of existing approaches. In Section 3we give a novel and computationaly stable geometricderivationofinversekinematicsoftheUR3manipulator. In Section 4we set the optimal trajectory problem whilepresentinganovelcollision indicators (constraint functions) that are efficient in the sense that the gradient methods can escape from collisions easily and these collision indicators are fast to evaluate at the same time. In Section 5 we present a collision-free planning algorithm relying on the steepest-descent method, a novel incremental growing of obstacles, and a novel hierarchical optimization strategy. We give numerical validations in Section 6such that for each given starting and goal positions of the effector an optimized collision-free trajectory is found at 273 milliseconds at latest. The paper is concluded in Section 7. 2 Related Work The first part of our paper presents a novel geometric insight into inverse kinematic solutions of a 6-DOF robotic manipulator UR3. We arrive at new analytic formulae that are numerically stable. In literature the systems comprising of joint DOFs are most frequently solved numerically via finding roots of a matrix polynomial. Many methods rely on the Lyapunov stability theory [16]. In [17] the authors employ the Levenberg-Marquart method. Avoiding self-collisions is treated in [18]. Parameterized classes of inverse kinematic solutions for redundant robots are searched for in [19,20]. In [21] the inverse kinematics is tailored to a problem of optimal spraying. The key part of the collision-avoidance system is the optimal trajectory planning algorithm. First of all an initial feasible trajectory has to be found. Among the most frequently used approaches there are variants of graph algorithms such as a D∗-algorithm [22]oranL∗-algorithm [23], which is a linear complexity counterpart to the more traditional A∗-algorithm. Here edges in the graph represent feasible paths between way-points being the graph vertices. Thesemethods are mostlyefficient in the2-dimensional (2D) motions of robotic vehicles. In higher-dimensional configuration spaces such as 6D in our case, the approach is hardly effective in the real-time regime. Another class of methods that is less effective in higher-dimensions rely on rapidlyexploring random trees [24]. On the other hand among the methods that are efficient in higher dimensions there is a class of potential field methods [20,25–27], in which an artificial potential is introduced such that the trajectory fixed at the start is attracted to the goal and repeled from the obstacles at the same time. In terms of optimization methods [28] this is a penalty approach. A drawback is that it does not always guarantee a collision-freetrajectory, in whichcase the repulsive forces have to be strenghtened. Similar approaches rely on modelling the potential field by the finite element method[29]orbythelevel-setmethod[30].Notethatatsome specificsetupsonecandevelopageometricapproach[31,32] to the trajectory planning. The above mentioned methods can be combined with stochastic approaches [33,34] and neural networks [19,35,36]. In case the optimization function can be well approximated by a quadratic function the Newton methods [20,37] become very efficient as they converge with a quadratic rate and they allow for a bigger trajectory changes towards the optimum. The Newton methods are further used successively in the class of sequential convex or quadratic programming algorithms [19,38,39]. The tricky part of the algorithms is the treatment of collisions. In [40,41] the authors develop a class of interior point methods, where the collision constraints are penalized via so-called barrier functions, which suffer from ill-conditioning and also from the fact that the constraints might be slightly violated similarly to the artificial potential methods. In this paper we opt for an active-set approach, where the feasibility of the trajectory is preserved at each iteration. For a survey of the planning algorithms we refer to [42]. To our best knowledge not many papers are devoted to development of efficient collision indicators meaning that their gradients lead to a fast escape from the collision. This is one of the main topics in the present paper. We rely our collision indicators on a fast approximation formulae for the penetrationdistance andpenetrationvolumesof mutualintersectionsofcylindersandintersectionsofcylindersandvoxels representing obstacles. A related approach in literature can be found in [22], where the surfaces are covered with point clouds and interactions of their bounding boxes is treated in a hierarchical approach using binary trees. Similarly in [43] the bodies are approximated by series of spheres. Important task for the trajectory planning is the trajectory parameterization. Having in mind a subsequent optimal path following, smooth trajectories such as Bézier curves [37,44,45] are preferable. Here the principal question is how many control parameters we shall take. The more we take the closer to the optimum we get, however, the more computational time it spends. Therefore, hierarchical approaches are benefitial. In [46] information about obstacles is updated using a multi-resolution wavelet basis. In [47] the large optimality systems to be solved is replaced by a hierarchy of 123 Journal of Intelligent & Robotic Systems (2023) 107:57 Page 3 of 14 57 subsystems, where the obstacles are included successively. Similarly, in [48] the successive optimal subproblems alternateamongincorporationofsingleobstacles.Inthispaperwe relyonahierarchyoftrajectoryparameterizations.Wesearch for deformations of sub-optimal trajectories from a previous step. Such deformations were presented in [49] without a hierarchical approach. Though it is not the subject of this paper for the sake of completeness we shall mention some work devoted to a large and important area of optimal path-following. Here an optimal control problem is solved such that a given feasible trajectory is followed in the most efficient way when considering the robot dynamics. One typically aims at a minimum energy consumption and a minimum torque in joints. We refer to the seminal paper [8], where the unconstrained problem is introduced, and to [9], where collisions are taken into account. We refer to [50] for an actual survey. 3 Inverse Kinematics of Robotic Manipulators 3.1 Planar Two-Arm Manipulator The planar two-arm manipulator is the very fundamental setup, which is worth recalling here. We denote the lengths of arms by a1and a2and the respective rotational degrees of freedombyθ:= (θ1,θ 2)asdepictedinFig. 1(left).Thepositions of arm end-points are denoted by x(1)and x(2):= x. They are computed by the simple means of the following forward kinematics: x:= a1cos θ1 sin θ1   =:x(1) +a2cos(θ1+θ2) sin(θ1+θ2).(1) For a given position xthe inverse kinematics computes the angles θ1and θ2. It relies on the law of cosines, see Fig. 1(left), α=arccos x2+a2 1−a2 2 2a1x, β=arccos a2 1+a2 2−x2 2a1a2.(2) Provided that xis reachable, i.e., x≤a1+a2, we get the so-called lefty (L) and righty (R) solutions θL 1:=γ+α, θL 2:=β−π, θR 1:=γ−α, θR 2:=π−β, (3) where γ:= arg(x1+ıx2). Though the inverse kinematics returns 0, 1 (in case x=a1+a2), or 2 solutions, we simplify its notation to IK2 :R2  x→R2  θ , see Fig. 1(left), {θL,θR}:=IK2(x):= IK2(a1,a2,x). (4) 3.2 Planar Three-Arm Manipulator Consider a planar three-arm manipulator of the arm lengths a1,a2, and a3with the rotational degrees of freedom θ:= (θ1,θ 2,θ 3), respectively, as depicted in Fig. 1(right). The kinematics of the arms is denoted by x(1),x(2), and x, respectively, and it is computed as follows: x:= a1cos θ1 sin θ1   =:x(1) +a2cos(θ1+θ2) sin(θ1+θ2)   =:x(2) +a3cos θ sin θ,(5) where θ:= θ1+θ2+θ3. Now the inverse kinematics has θas a parameter, which is actuallyprescribedas the direction, wherethe effector should Fig. 1 Planar two-arm (left) and three-arm (right) manipulators 123 57 Page 4 of 14 Journal of Intelligent & Robotic Systems (2023) 107:57 point to. Hence the position and angle of the third arm is given and we arrive back at the inverse kinematics of a twoarm manipulator. As a mapping the inverse kinematics may again return 0 (in case x>a1+a2+a3), 1 (in case x=a1+a2+a3), or 2 (in case x<a1+a2+a3) solutions, which we simplify to IK3 :R2  x×R  θ→ R3  θ . The evaluation reads as follows: θL/R 1 θL/R 2:= IK2 a1,a2,x−a3cos θ sin θ, θL/R 3:= θ−θL/R 1+θL/R 2.(6) 3.3 Universal Robot UR3 Finally, we consider a 5-arm manipulator with 6 degrees of freedom as depicted in Fig. 2. We denote the arm lengths by ai>0, i=1,2,...,5 and the degrees of freedom by θ1,θ 2,...,θ 6, which are rotations at joints. Additionally, there are three axial displacements d2,d3,d4>0of the vertical arms and we denote the total displacement by d:= d2−d3+d4, which is assumed to be positive. At each joint ι∈{1,2,...,5}we introduce a local orthogonal coordinate system described by unit vectors iι,jι,kι∈R3.The kinematics is computed by the following sequence of formulae for the arm end-points and transformations of the local Fig. 2 Universal robot UR3 coordinate systems see also Fig. 2(right): i1:= ⎛ ⎝ 0 0 1⎞ ⎠,j1:= ⎛ ⎝ 0 1 0⎞ ⎠,k1:= ⎛ ⎝−1 0 0⎞ ⎠, x1:= a1i1,(i2,j2,k2)=(i1,j1,k1)·⎛ ⎝ 10 0 0 cos θ1−sin θ1 0sinθ1cos θ1⎞ ⎠   =:R23(θ1) , x2:= x1+d2k2   =:x2d +a2(i2,j2)·cos θ2 sin θ2   =:r(θ2) ,(i3,j3,k3) =(i2,j2,k2)·R12(θ2), x3:= x2−d3k3   =:x3d +a3(i3,j3)·r(θ3), (i4,j4,k4) =(i3,j3,k3)·R12(θ3), x4:= x3+d4k4   =:x4d +a4(i4,j4)·r(θ4), (i5,j5,k5) =(i4,j4,k4)·R12(θ4), x:= x4+a5(k5,−j5)·r(θ5). (7) Obviously, the last degree of freedom θ6, the rotation of Joint6, see Fig. 2(right), does not influence the kinematics unlessthe robot manipulates an axially nonsymmetricobject. Nonetheless, throughout this paper we shall not consider θ6 anylonger.Thedegreesoffreedomofourinterestaredenoted by θ:= (θ1,θ 2,...,θ 5)∈R5. As for the inverse kinematics, we proceed in three steps. First of all, we resolve the fifth arm and θ1. Given the effector position xand the unit directional vector v(effector orientation), the fifth arm is determined, x4:= x−a5v.We project x4onto the plane ρperpendicular to the direction k2:= k3:= k4:= k5:= (−cos θ1,−sin θ1,0)of axial displacements d2,d3,d4, see Fig. 3(left), ρ:= span ⎛ ⎝j2:= ⎛ ⎝−sin θ1 cos θ1 0⎞ ⎠,i2:= ⎛ ⎝ 0 0 1⎞ ⎠⎞ ⎠.(8) The projection reads xρ 4:= x4−dk2and xρ 4∈ρimplies the complex-valued equation (rather than the real system) to be solved for η, which is an auxiliary variable and θ1, (x4)1+ı(x4)2   =:|x4|eıα+deıθ1  k2 =ηeı(θ1+π/2)   j2 .(9) 123 Journal of Intelligent & Robotic Systems (2023) 107:57 Page 5 of 14 57 Fig. 3 Determining of the fourth arm of UR3 (left), determining the remaining arms in the plane ρ(right) Provided d≤|x4|, we get the following two branches of solutions: θ± 1:= α±arccos −d |x4|,(10) where |x4|:=(x4)2 1+(x4)2 2and α:= arg((x4)1+ ı(x4)2). Secondly, we resolve the fourth arm by observing that its direction i4=i5is orthogonal to vas well as to the axial displacement direction k± 2. The latter implies that i4∈ρ, which together with v⊥i4gives the two additional branches of solutions, i±,+ 4:= −(v·i1)k± 2+(v·k± 2)i1,i±,− 4:= −i±,+ 4,(11) recalling i1:= (0,0,1). Hence, we get up to four projections of x3onto ρ, xρ,p,q 3:= xρ,p 4−a4ip,q 4,(12) where p,q∈{+,−}. This determines the angle θp,q 5:= arg((v·kp 5)−ı(v·jp,q 5)), (13) where jp,q 5:= kp 5×ip,q 5. Finally, θ2and θ3can be determined from the inverse kinematics of the two-arm planar system in ρ, see Fig. 3(right). We shift the origin to a1i1and express xρ,p,q 3in the coordinate system i2:= i1and jp 2,  xρ,p,q 3:= (xρ,p,q 3−a1i1)·i2 (xρ,p,q 3−a1i1)·jp 2.(14) We have θp,q,r 2 θp,q,r 3:= IK2(a2,a3, xρ,p,q 3), (15) where r∈{L,R}. The remaining angle can be computed in ρas follows: θp,q,r 4:= arg (xρ,p 4−xρ,p,q 3)·ip,q,r 4+ı(xρ,p 4−xρ,p,q 3)·jp,q,r 4, (16) where ip,q,r 4:= cos(θ p,q,r 23 )i2+sin(θ p,q,r 23 )jp 2,jp,q,r 4:= −sin(θ p,q,r 23 )i2+cos(θ p,q,r 23 )jp 2,θp,q,r 23 := θp,q,r 2+θp,q,r 3. To conclude the inverse kinematics of UR3 can be represented as the mapping IKUR3 :R3  x×R3  v→R5  θ , θp,q,r:=IKUR3(x,v):=IKUR3(a1,a2,a3,a4,a5,d,x,v), (17) where v=1, p,q∈{+,−},r∈{L,R}. In general, there could be either of 0, 1, 2, 4, or 8 solutions. 4 Optimal Collision-Free Trajectory Problem Given a starting joint position θstart and ending position and orientation of the effector, xstop and vstop, first of all the related ending degrees of freedom are computed by the inverse kinematics θstop := IKUR3(xstop,vstop)so that we do not switch between the branches unless necesarry. 123 57 Page 6 of 14 Journal of Intelligent & Robotic Systems (2023) 107:57 The optimal unconstrained trajectory is the straight line θline(t):= θstart +t(θstop −θstart),t∈[0,1]. In case of collision we shall search for an as short as possible collisionfree curve. The parameter domain [0,1]is decomposed into n+1 equidistant intervals, over which continuous piecewise linear trajectoriesareconsideredsothatthestraightlineisperturbed in a normal direction as depicted in Fig. 4. The normal directions are spanned by the 4-dimensional space span (n1,...,n4)   =:N∈R5×4=Null θstop −θstart,(18) where ni=1. The trajectories pass nbreak-points and they read θ(t)( p):= θstart +t(θstop −θstart)   =θline(t) + n  i=1 ϕi(t)N·pi,(19) where the vector p:= (p1,..., pn)∈R4nto be optimized describes perturbations of θfrom the unconstrained optimum θline and where ϕi(t)are the continuous basis functions that are piecewise linear along the discretization ti:= i n+1, i=0,1,...,n+1, with the step h:= 1 n+1, i.e., for i=1,2,...,n, ϕi(t):= ⎧ ⎪ ⎨ ⎪ ⎩ t−ti−1 h,t∈[ti−1,ti], −t−ti+1 h,t∈[ti,ti+1], 0,elsehere. (20) We denote the 4n-dimensional vector space of the trajectories by Tn. Note that it is rather straightforward to change the nonsmooth piecewise linear basis functions to, e.g., cubic splines or to globally smooth basis functions such as Lagrange polynomials. They can be beneficial in terms of preserving a continuous acceleration in joints. Fig. 4 Piecewise linear parameterization of the trajectory We solve the following nonlinearly constrained quadratic programming problem: p∗:= (p∗ 1,..., p∗ n):= arg min p∈[−5 6π,5 6π]4n: g(θ(t)( p))≤0 p2,(21) where the bounds ±5 6πare construction limits of joints and where g:Tn→Rmis the constraint function that avoids collisions. 4.1 Constraints Avoiding Collisions The constraint g(θ(t)( p)) ≤0avoids mutual collisions of arms, collisions of arms with a horizontal workspace construction,towhichwefurtherrefertoastheground,collisions of arms and joints with a vertical workspace construction, and collisions of arms and joints with voxels representing the obstacle. Namely, referring to Fig. 2, we consider m:= 20 constraints gias follows: •Constraints i=1,2,3 avoid collisions of Arm1 with Arm4, Arm1 with Arm5, and Arm2 with Arm5. •Constraints i=4,5,6,7 avoid collisions of Arm2, Arm3, Arm4, and Arm5 with the ground G:= {(x1,x2,x3)∈R3:x3≤0}. •Constraints i=8,9,10,11 avoid collisions of Arm3, Joint3, Arm4, and Arm5 with the left quadrant QL:= {(x1,x2,x3)∈R3:x1≤−q1and x2≥q2}, where q1,q2>0 describe positions of two fixed vertical pillars holding the system of cameras. The left pillar is installed at (−q1,q2). •Constraints i=12,13,14,15 avoid collisions of Arm3, Joint3, Arm4, and Arm5 with the right quadrant QR:= {(x1,x2,x3)∈R3:x1≥q1and x2≥q2},seetheprevious item. The right pillar is installed at q:= (q1,q2), symmetrically (with respect to the plane x2=0) to the left pillar. •Constraints i=16,17,18,19,20 avoid collisions of Arm2, Arm3, Arm4, Arm5, and Joint3 with voxels representing the obstacle. Other collisions do not need to be considered due to the dimensions of the universal robot UR3 and the workspace as described in Section 6. Notice that arms as well as joints are represented by their cylindrical envelops. The joints the collisions of which do not need to be indicated by a collision of connected arms are Joint2, Joint3, and Joint4. They are exactly those related to axial displacements d2,d3,d4that have not played a role in the inverse kinematics, but they do so now. To our best knowledge there is no analytic procedure to evaluate collisions of θ(t)for all t∈[0,1]. Therefore we 123 Journal of Intelligent & Robotic Systems (2023) 107:57 Page 7 of 14 57 discretize the trajectory parameter interval into Nequidistant subintervals and consider the constraints at the worst-case discrete parameter as follows: gi(θ(t)( p)) := max j∈{0,1,...,N}gi(θ(j/N)( p)), (22) where gi:R5→R. 4.1.1 Mutual Collisions of Arms We prescribe the constraint as an approximate depth of the penetration of the two arms as follows: gi:= rj+rk−dist(Sj,Sk), (23) where Sj,Skare the axes (segments) and rj,rkare the radii of the cylindrical arms. Notice that this approximation of the penetration depth is a strong indicator meaning that it always indicates the true penetration but it can also indicate a false penetration, see Fig. 5(left). On the other hand it is very cheap to evaluate, which is desired in the real-time optimization. An exact indicator would rely on computing the volume of intersection of the cylinders, which would be computationally demanding. 4.1.2 Collisions of Arms with the Ground The constraint is prescribed as an approximate depth of the penetration of the arm with the plane x3=0 (the ground) as follows: gi:= r−min t∈[0,1]{a3+t(b3−a3)},(24) where a3,b3are the vertical (third) coordinates of the arm end-points and ris the radius. Again, this constraint also givespositivefalseindicationsofcollisions,seeFig.5(right), but it is cheap to evaluate hence proper for the real-time optimization. An exact constraint would evaluate the volume of intersection of the cylinder with the ground, which would be computationally demanding. 4.1.3 Collisions of Arms and Joints with the Quadrants The constraint approximates the depth of the penetration of an arm or joint with a quadrant. Consider the horizontal projection of the arm/joint axis, S:= {(x1,x2):= (a1,a2)+t(b1−a1,b2−a2)∈R2:t∈[0,1]} with the axis end-points a,b∈R3, the radius r, and the first quadrant Q:= {(x1,x2)∈R2:x1,x2≥0}. We define the level-set function (S,Q):= ⎧ ⎨ ⎩ dist(S,Q), S∩Q=∅, −max x∈S∩Qmin{x1,x2},S∩Q=∅.(25) The constraint avoiding penetration with the right quadrant QR:= (q1.q2)  =:q +Qreads gi:= r−(S+q,Q), (26) while the one avoiding penetration with the left quadrant QL:= {(x1,x2)∈R2:x1≤−q1,x2≥q2}reads gi:= r−(S+q,Q), (27) where S:= {(−x1,x2)∈R2:(x1,x2)∈S}. An exact constraint would evaluate the volume of intersection of the cylinderwith the quadrants, which wouldbe computationally demanding. 4.1.4 Collisions of Arms and Joints with the Obstacle Thisconstraintapproximates the depth-times-volumeofpenetration of an arm or joint into the voxels representing the Fig. 5 Mutual collisions of arms (left), collisions of an arm with the ground (right) 123 57 Page 8 of 14 Journal of Intelligent & Robotic Systems (2023) 107:57 obstacle. The product of depth and volume turned out to resolve more collisions than the single depth constraint, like 23 and 24, or the single volume constraint. The voxels are received from cameras and they typically envelop hands ofa human operator.Considera cylindricalarm/joint Sdetermined by radiusrand axial end-points a,b. Further, consider the union of all the voxels. We compute the constraint in the following four steps: 1. Detect voxels boundary := ∂. It collects those square voxel faces that are not shared with another voxel. This step has a quadratic complexity in terms of numbers of voxels, but it is performed only once per evaluation of g. 2. Replace each cylinder with a box envelop determined by four edges x±i(t):= a±√2rni+t(b−a), t∈[0,1],(28) where ni,i=1,2, is an orthonormal basis of Null(n3), where n3:= (n3 1,n3 2,n3 3):= (b−a)/b−a. This can be generally discontinuous with respect to n3, but we make it (including all partial derivatives) continuous almost everywhere by the use of spherical coordinates. Namely, we determine the polar ϑand azimuthal ϕangles of n3i.e. ϑ:= arg(n3 1+ın3 2)and ϕ:= arccos(n3 3). Then we apply the spherical transformation to (0,1,0)and (0,0,1)to arrive at n1:= R13(ϕ) ·R12(ϑ) ·(0,1,0)Tand n2:= R13(ϕ) ·R12(ϑ) · (0,0,1)T, respectively. Indeed, the resulting mapping n3→ (n1,n2)is continuous up to the two directions n3=(0,0,±1). 3. Computeintervalsofpenetrationsofthefouredges x±i(t) into , I±i:= I±i 1∪···∪I±i N±i⊂[0,1].(29) This is done by computing all intersection points of x±i(t)with and sorting them along the parameter t. The consecutive pairs of the sorted parameters determine intervals inside, which are of our interest, and outside . 4. Compute the constraint by summing up penetrations of the four edges gi:= ⎧ ⎪ ⎪ ⎨ ⎪ ⎪ ⎩ r2 k∈{+1,−1,+2,−2} Nk  j=1|Ik j|dist(xk(tk j), ),  k∈{+1,−1,+2,−2} Ik=∅, r−dist(S,), elsewhere, (30) where |Ik j|is the length of Ik jand tk jis the mid-point of Ik j. 5 Hierarchical Active-Set Optimization Method 5.1 Finding an Initial Collision-Free Trajectory The first and most crucial step is to find a feasible trajectory. A natural idea would be to search for a feasible polygonal trajectory in the 5-dimensional space of joint degrees of freedom. However, the global search suffers from an exponential computational complexity. Hence we rely on the following local search algorithm: Given a (zero) non-feasible initial trajectory p0, we proceed the steepest-descent iterations pk+1:= pk+αkdk(31) until pk+1is feasible or k, and hence computational time, reaches a given maximum. The descent direction is the negative weighted gradient of non-feasible constraints, dk:= −  i:gi(pk)≥0 gi(pk)∇gi(pk), (32) and αkis computed by the line-search (bisections) method αk:= arg min α≥0: pk+αdk∈[−5 6π,5 6π] max i:gi(pk)≥0gi(pk+αdk). (33) The bisection starts at the maximal feasible αk 0:= arg max α≥0: pk+αdk∈[−5 6π,5 6π] α. (34) If we do not succeed we adopt an incremental growing of the obstacle, represented by the voxels, from the ground as sketched in Fig. 6. First of all we find the maximal vertical coordinate x3:= arg maxx∈x3. Starting at the unconstrained optimum p:= 0, we solve a sequence of M problems to find a feasible trajectory by algorithm 31,32,33, while incrementaly growing the obstacle as follows: m:= −M−m Mx3⎛ ⎝ 0 0 1⎞ ⎠,m=1,2,...,M.(35) The feasible path of the m-th problem is a starting point to the (m+1)-st problem. If we still do not succeed, we let the obstacle fall from the top as follows: m:= +2M−m Mx3⎛ ⎝ 0 0 1⎞ ⎠, m=M+1,M+2,...,2M.(36) 123 Journal of Intelligent & Robotic Systems (2023) 107:57 Page 9 of 14 57 Fig. 6 Incremental growing of the obstacle from the ground 5.2 Optimization After we find a collision-free trajectory, denoted again by p0, g(p0)≤0, we finally continue to optimize it. We solve the nonlinearly constrained quadratic programming problem 21 approximately (numerically) by means of the presented optimization algorithms. We employ the sequential quadratic programming and the active-set approach, the iterations of which read pk+1:= pk+αkδk,α k:= arg maxα α∈(0,1]: pk+δk∈[−5 6π,5 6π] g(pk+αδk)≤0 ,(37) where the following quadratic programming subproblem is solved: δk:= arg min gA(pk)+(Gk A)T·δ=0pk+δ2.(38) Theconstraintmatrix Gk Acomprisesofcolumns∇gi(pk), i∈A, at active or close-to-active indices. These are chosen at the 10% level of the maximal constraint, i.e., A:= i∈{1,...,m}: −1 10maxj∈{1,...,m}|gj(pk)|≤gi(pk)≤0. (39) Moreover, linearly dependent columns of Gk Aare removed. Hence, 38 is a well-posed linear saddle-point problem that reads as follows: IG k A (Gk A)T0·δk λk=pk −gA(pk).(40) The latter admits the solution δk=−pk−Gk A(Gk A)T·Gk A−1 ·gA(pk)−(Gk A)T·pk.(41) 5.3 Hierarchical Optimization For higher numbers of trajectory break points n, i.e., higher dimensions 4n, finding a feasible design and its optimization become difficult. Therefore we employ the idea of multigrid methods[51]tosolvetheproblemthrough anestedhierarchy. At the initial level l:= 1 we start with n(1):= 1 and the unconstrained optimum p(1) 0:= 0. The resulting, so-called triangular, trajectory p(1) ∗, no matter whether it is feasible or not, is taken as the initial guess p(2) 0at the next level l:= 2. We introduce two additional break points as depicted in Fig. 7, hence n(2):= 3. The process continues at this level and we arrive at the, so-called pentagonal, trajectory p(2) ∗. The latter is interpolated to the next level l:= 3, n(3):= 7, where we stop. The resulting trajectory is called nonagonal. The interpolations satisfy θ(l+1)(t)( p(l+1) 0)=θ(l)(t)( p(l) ∗). Forour choice of basisfunctions20wearriveat thefollowing interpolation matrices P(1,2)and P(2,3): p(2) 0:= ⎡ ⎣⎛ ⎝ 1/2 1 1/2⎞ ⎠⊗I⎤ ⎦   =:P(1,2) ·p(1) ∗, Fig. 7 Hierarchical optimization 123