Collision-free motion planning for closed kinematics

Abstract

A method is described for collision-free motion planning of a first manipulator with closed kinematics. The method includes defining a dynamic optimization problem, solving the optimization problem using a numerical approach, and determining a first movement path for the first manipulator based on the solution of the optimization problem. The dynamic optimization problem includes a cost function that weights states and control variables of the first manipulator, a dynamic that defines states and control variables of the first manipulator as a function of time, and at least one inequality constraint for a distance to collisions. Furthermore, the optimization problem includes at least one equality constraint for the closed kinematics.

Claims

1. A method for collision-free motion planning for a first manipulator with closed kinematics, the method comprising: defining a dynamic optimization problem; solving the optimization problem via a numerical approach; determining a first movement path for the first manipulator based on the solution of the optimization problem; and executing the first movement path by controlling the first manipulator to travel along the first movement path, wherein the dynamic optimization problem comprises a dynamic that defines states and control variables of the first manipulator as a function of time, a cost function that weights the states and the control variables of the first manipulator, at least one inequality constraint for a distance to collisions, and at least one equality constraint for closed kinematics, and wherein the optimization problem is solved on a successively shortened time horizon T in parallel with the execution of the first motion path.

2. The method according to claim 1, wherein the dynamic optimization problem additionally comprises boundary conditions that define a start condition and an end condition.

3. The method according to claim 2, wherein the boundary conditions are a start position and a target position of the first manipulator.

4. The method according to claim 1, wherein the dynamic optimization problem further comprises one or more control variable constraints defining minimum and maximum values of the control variables.

5. The method according to claim 4, wherein the one or more control variable constraints include at least one of a minimum speed, a minimum acceleration, a maximum speed, and a maximum acceleration of the first manipulator.

6. The method according to claim 1, wherein the dynamic optimization problem further comprises one or more state constraints defining limits of the states.

7. The method according to claim 6, wherein the one or more state constraints include at least one of a minimum joint angle, a minimum joint angular velocity, a maximum joint angle, and a maximum angular velocity of the first manipulator.

8. The method according to claim 1, wherein the dynamic optimization problem further comprises a defined end time T defining a duration of the movement.

9. The method according to claim 8, wherein the end time T is selected to be a fixed value.

10. The method according to claim 8, wherein the end time T is weighted by the cost function.

11. The method according to claim 8, wherein the dynamic optimization problem further comprises one or more end time constraints.

12. The method according to claim 11, wherein the end time constraints includes at least one of a minimum end time and a maximum end time.

13. The method according to claim 1, wherein the at least one inequality constraint for a distance to collisions defines a minimum distance to collisions.

14. The method according to claim 1, wherein the at least one equality constraint for the closed kinematics defines a first dynamic position constraint of the first manipulator.

15. The method according to claim 1, wherein the at least one equality constraint for the closed kinematics defines a first dynamic orientation constraint of the first manipulator.

16. The method according to claim 1, wherein the numerical approach comprises transforming the dynamic optimization problem from a constrained optimization problem into an unconstrained optimization problem.

17. The method according to claim 16, wherein the at least one equality constraint for the closed kinematics and the at least one inequality constraint for a distance to collisions are each multiplied by a multiplier and added to the cost function.

18. The method according to claim 16, wherein the at least one equality constraint for closed kinematics and the at least one inequality constraint for a distance to collisions are multiplied in squared form by a penalty parameter and added to the cost function.

19. The method according to claim 18, wherein, for solving the dynamic optimization problem, the multiplier and the penalty parameter are iteratively adapted.

20. The method according claim 16, wherein the numerical approach comprises a gradient approach for solving the unconstrained optimization problem.

21. The method according to claim 1, wherein: the dynamic optimization problem has an end time T that defines a duration of the movement, the numerical approach generates in a first step a first partial solution for at least one first period, the first period is smaller than the duration of the movement, the numerical approach generates in at least one second step a second partial solution for a second period, and for the second step, an adapted optimization problem is used that corresponds to the dynamic optimization problem and takes into account a state at the end of the first period as an initial condition.

22. The method according to claim 1, further comprising: determining a second movement path for a second manipulator, wherein the cost function additionally weights the states and control variables of the second manipulator, wherein the dynamics defines the states and control variables of the second manipulator as a function of time, and wherein the first manipulator and the second manipulator form the closed kinematics.

23. The method according to claim 22, wherein the at least one equality constraint for the closed kinematics comprises a first dynamic position constraint of the first manipulator and a second dynamic position constraint of the second manipulator, which are in a defined, fixed relationship to one another, so that the closed kinematics is fulfilled.

24. The method according to claim 22, wherein the at least one equality constraint for the closed kinematics comprises a first dynamic orientation constraint of the first manipulator and a second orientation constraint of the second manipulator, which are in a defined, fixed relationship to one another, so that the closed kinematics is fulfilled.

25. An apparatus comprising: a first manipulator; and a control unit for collision-free motion planning for the first manipulator based on a dynamic optimization problem, wherein the control unit is configured to: solve the optimization problem via a numerical approach, determine a first movement path for the first manipulator based on the solution of the optimization problem, and execute the first movement path by controlling the first manipulator to travel along the first movement path, wherein the dynamic optimization problem comprises: a cost function that weights states and control variables of the first manipulator, a dynamic that defines states and control variables of the first manipulator as a function of time, at least one inequality constraint for a distance to collisions, and at least one equality constraint for closed kinematics, and wherein the control unit is configured to solve the optimization problem on a successively shortened time horizon T in parallel with the execution of the first movement path.

Description

BRIEF DESCRIPTION OF THE DRAWINGS

(1) Embodiments of the disclosure are shown in the drawings and are explained in more detail in the following description.

(2) FIG. 1 is a block diagram of a method according to an example embodiment.

(3) FIG. 2 is a formal representation of an optimization problem according to an example embodiment.

(4) FIG. 3 is a schematic representation of parallel planning and execution according to an example embodiment.

(5) FIG. 4 is schematic representation of an apparatus according to an example embodiment.

DETAILED DESCRIPTION

(6) In FIG. 1, an example of the new procedure in its entirety is marked with the reference number 100.

(7) The method starts with the formulation of a suitable optimization problem for motion planning in step 102. The basis of the optimization problem is a system model that describes the dynamics of the moving system with respect to the possible states and control variables as a function of time. Furthermore, the optimization problem includes a cost function that specifies the goal of the optimization. The cost function contains one or more functions that weight the parameters of the system model.

(8) The goal of optimization is to find a function of an independent variable (usually a time function) that minimizes the cost function. In other words, in the case of motion planning, one goal is to find an optimal input trajectory that can be used to control the manipulator. This is also referred to as optimal control problem.

(9) The optimization problem also includes constraints that restrict free movement in space. The constraints must be taken into account when planning the motion. The constraints include at least one inequality constraint that takes into account obstacles in space and at least one equality constraint that takes into account kinematic constraints.

(10) The result of step 102 is a formal representation of the system, its limits and the desired optimization.

(11) Then, in step 104, the optimization problem is solved, i.e. the function that minimizes the cost function is searched. This is done using a numerical approach, for instance, computer-aided. Instead of a concrete solution, the numerical approach allows an approximate calculation of the optimization problem. Thus, a solution can also be specified if no explicit solution presentation is possible or if it cannot be found in a reasonable period.

(12) A large number of different numerical methods and programs are known with which the optimization problem can be solved in principle. Which approach is best suited depends on various factors, including the required accuracy or the available computing time. It shall be noted that the disclosure is not limited to a specific numerical solution approach.

(13) The result of step 104 is an optimal solution function or an approximation of this optimal function with sufficient accuracy.

(14) Based on the optimal solution function or its approximation, a concrete motion path for the first manipulator is determined in step 106. This means that based on the numerical solution the movement of the manipulator is defined. In the simplest case, the solution of the optimization problem is an optimal input trajectory, which can be directly converted into a motion control of a manipulator. The method according to the disclosure may thus provide directly and immediately the desired control, which in principle leads to a smooth and natural movement of the manipulator.

(15) Since, as will be explained in more detail in the following, the optimization problem takes into account obstacles via the at least one inequality constraint and the closed kinematics via the at least one equality constraint, the method generates overall a smooth and natural movement of the manipulator around the obstacles and without violating the requirement for the closed kinematics. Thus, the various problems of motion planning (obstacles, kinematics) can be fully taken into account, whereby planning can be made iterative and especially deterministic through the numerical approach. The planning times are short and determinable, which makes a real-time implementation of motion planning possible. The resulting movement is natural, collision-free and takes into account the closed kinematics.

(16) FIG. 2 shows a formal representation of the dynamic optimization problem, which here is designated in its entirety by reference numeral 12. Starting point is the system model, which describes the dynamics 14 of the moving system, for example the manipulator, with its states 16 and control variables 18. Dynamics 14 can be assumed in a simplified way as an integrator {dot over (q)}=u with the states X=q and the control variables {dot over (q)}, or as a double integrator {umlaut over (q)}=u with the states

(17) ${x\left[q^T,{\stackrel{.}{q}}^T\right]}^T$
and the control variables u={umlaut over (q)}.

(18) If only one path is to be calculated with the optimization without time parameterization, the simple integrator is sufficient as a system model. If, on the other hand, the calculated trajectory is to be executed directly, the double integrator should be used, since here the minimum and maximum acceleration can be taken into account.

(19) The cost function 20 weights the states 16 and control variables 18. The task of optimization is to find the optimum control function from the usually infinite number of control functions, which minimizes the cost function 20.

(20) Via the at least one equality constraint 22, closed kinematics of the system are taken into account while planning. A closed kinematic system is given if there is at least one closed kinematic chain. This is the case, for example, when a manipulator is kinematically connected to its base in two different ways.

(21) Closed kinematics is also given if a robot has several manipulators that are connected to each other, for example, if the manipulators jointly lift and move a workpiece. In this case, a pose of the end effectors of the respective manipulators is always relative to a pose of the end effectors of the respective other manipulators. Thus, there is a constraint in poses that the individual manipulators can have to each other, which is represented in pairs as an equality constraint (P.sub.1(x)P.sub.relativ=P.sub.2(x)) The equality constraints thus define dynamic limits that must be considered when solving the dynamic optimization problem. In other words, the condition of a closed kinematics flows into the optimization problem to be solved as an additional constraint. Thereby, the closed kinematics are directly taken into account in motion planning and are an integral component of motion planning itself.

(22) Furthermore, the dynamic optimization problem 12 includes at least one inequality constraint 24, by which obstacles in space can be considered. Preferably, the at least one inequality constraint establishes a minimum distance from collisions with obstacles in space. The calculation of this collision distance and its partial derivatives according to the states represents the most complex part of the optimization problem.

(23) With the inequality constraints 24, the distance to self-collisions and the distance to collisions with obstacles can be taken into account. To calculate the distance for self-collisions, in an example embodiment, an approximation of the robot can be which approximates the robot using a plurality of balls having center and radius. For the collision with obstacles, an approximation of the environment can be made by means of a raster map, whereby the distance to be taken into account results from the Euclidean distance transformation of the raster map. The smallest distance is taken into account either to the self-collision and/or to the collision with an obstacle.

(24) It goes without saying that, in addition to the constraints mentioned, further boundary conditions 26 and constraints can be covered by the dynamic optimization problem 12. In particular, further inequality constraints 24 may be envisaged, which define upper and lower limits of possible states. For example, limits for minimum and maximum joint angles can be defined and taken into account by the optimization problem. Furthermore, control variables are conceivable which define minimum and maximum values of the control variables, in particular minimum and maximum speeds and/or accelerations of the first manipulator.

(25) In various implementations, the duration of the movement specified by a defined end time T may also be considered. Either the end time T can be fixed depending on the start state and the end state or it can be a component of the optimization as a further variable. In the latter case, the time is weighted by the cost function 20. A minimum value and a maximum value can be defined for the end time. By considering the end time T, a deterministic behavior of the optimization can be achieved.

(26) Solving the dynamic optimization problem 12, as presented here, can be achieved in different ways and by different numerical approaches. In various implementations, the numerical solution of the dynamic optimization problem 12 can be achieved by a gradient approach in combination with the extended Lagrange method. In this approach, the dynamic optimization problem with constraints is transferred into an unconstrained optimization problem. In other words, the optimization problem is reduced to the optimization of an auxiliary function that has no constraints. For the transformation, the constraints are multiplied by factors, the so-called Lagrange multipliers, and embedded into the target function. Positive Lagrange multipliers are used to punish a violation of the constraints.

(27) The extended Lagrange method further includes a quadratic tightening function, which is embedded into the target function using an additional multiplier.

(28) The resulting auxiliary function, also called the extended Lagrange function, is
L(x,μ,ρ)=f(x)+μg(x)+½ρg(x).sup.2
with the Lagrange multiplier μ and the penalty parameter ρ. The front portion of the equation f(x)+μg(x) corresponds to the usual Lagrange function and the rear portion ½ρg(x).sup.2 to a square tightening function. Assuming strong duality and the existence of an optimal solution x*, μ*, it can be shown that for each ρ≥0 the saddle point condition
L(x*,μ,ρ)≤L(x*,μ*,ρ)≤L(x,μ*,ρ)
must be fulfilled. From this, it is apparent that the Lagrange function must be minimized with respect to the variable x and maximized with respect to the Lagrange multipliers μ, i.e. a new optimization problem

(29) $\underset{u}{\max }\underset{x}{\min }L\left(x,\mu ,\rho \right)$
must be solved.

(30) For solving this problem, at first, the underlying minimization problem

(31) $x^{\left(k+1\right)}=\underset{x}{\mathrm{argmin}}L\left(x,{\mu }^{\left(k\right)},{\rho }^{\left(k\right)}\right).$
is defined via an iteration k, which is solved by a gradient approach. Afterwards the Lagrange multipliers will be updated by the steepest ascent
μ.sup.(k)=μ.sup.(k)+ρ.sup.(k)g(x.sup.(k))
with the penalty parameter ρ(k) as step size. The convergence of the procedure can be significantly improved by adapting the penalty parameter. The heuristics

(32) ${\rho }^{\left(k+1\right)}=\{\begin{array}{cc}{\beta }_{\mathrm{inc}}{\rho }^{\left(k\right)}& .Math.g\left(x^{\left(k\right)}\right).Math.\ge {\gamma }_{\mathrm{inc}}.Math.g\left(x^{\left(k-1\right)}\right).Math..Math..Math.g\left(x^{\left(k\right)}\right).Math.>{\epsilon }_g\\ {\beta }_{\mathrm{dec}}{\rho }^{\left(k\right)}& .Math.g\left(x^{\left(k\right)}\right).Math.\le {\gamma }_{\mathrm{dec}}{\epsilon }_g\\ {\rho }^{\left(k\right)}& \mathrm{otherwise}\end{array}$
with the factors β.sub.inc>1, β.sub.dec<1, γ.sub.inc and γ.sub.dec<1, and an absolute tolerance ε.sub.g has been tried and tested.

(33) If the minimization does not result in a sufficient improvement of the constraints, the penalty parameter is further increased. As soon as the constraint is met with the required tolerance, the penalty parameter can be reduced again. The procedure is repeated until a maximum iteration number k=N.sub.mult is reached or the minimization is converged and all constraints are met.

(34) The inequality constraints h(x)≤0 can be transformed into the equality constraints g(x)=h(x)+z=0 by introducing slack variables z≥0. The minimization regarding z can be calculated explicitly and results in

(35) $z^{*}=\max \left\{0,-\frac{\mu }{\rho }-h\left(x\right)\right\}.$
Inserted in g(x), the equivalent formulation is achieved as

(36) $g_{+}\left(x,\mu ,\rho \right)=\max \left\{h\left(x\right),-\frac{\mu }{\rho }\right\}$
without slack variables, which can be used in the Lagrange function. The adaptation rule for the penalty parameter is formed for inequality constraints without the absolute amount.

(37) In the case of several equality constraints and inequality constraints, there is one Lagrange multiplier per constraint. The penalty parameter can be selected either uniformly for all or, as the Lagrange multiplier, separately for each constraint. To improve robustness of the procedure, the update of the multipliers can be skipped if sufficient convergence has not yet been achieved in the minimization. Furthermore, the Lagrange multipliers and the penalty parameters can be limited by minimum and maximum values.

(38) In detail, the gradient approach carries out the following steps in each iteration j, starting from an initial control variable u.sup.(0)(t): Integrating the system starting from the initial condition in forward time to obtain x.sup.(j)(t); Integrating the adjoint system starting from the end condition in backward time to obtain λ.sup.(j)(t); Solving the underlying line search problem

(39) ${\alpha }^{\left(j\right)}=\underset{\alpha >0}{\mathrm{argmin}}J\left(\psi \left(u^{\left(j\right)}-\alpha g^{\left(j\right)}\right)\right)$  with the search direction:

(40) $g=\frac{\partial H\left(x,u,\lambda \right)}{\partial u}={\left(\frac{\partial l\left(x,u\right)}{\partial x}\right)}^T+{\left(\frac{\partial f\left(x,u\right)}{\partial x}\right)}^T\lambda$
The control variable constraints are taken into account by projecting ψ onto the permissible quantity [u−, u+]; Calculating the new control variables u.sup.(j+1)=ψ(u.sup.(j)−α.sup.(j)g.sup.(j) and sct j=j+1; Stopping the gradient procedure at j=N.sub.grad or when a convergence criterion is reached.

(41) The gradient approach shown here can be implemented relatively easily. The individual iterations require only little computing time and required memory for the approach is low. Especially at the beginning of the procedure, there is usually a significant reduction in the cost function, so that a suboptimal solution is quickly found. However, in order to achieve high accuracy, many iterations are required compared to methods that use second-order information (Hesse matrix).

(42) In various implementations, the planning is performed in parallel to the actual execution, i.e. parallel to the actual movement of the manipulator. This principle of parallel planning and execution is illustrated in FIG. 3.

(43) Parallel planning and execution can both reduce the necessary computing time before the start of the movement and avoid the need for frequent re-planning. A trajectory is further optimized during execution. Thus, the trajectory can already be executed, even if it is not yet completely collision-free. In addition, the trajectory can be adapted to changing environmental conditions by further optimization, such that a local evasion behavior is automatically achieved.

(44) In the first time step, the movement from q.sub.S to q.sub.G is planned (A). In the second time step, the beginning of this trajectory up to q.sub.1=q(ΔT) is executed (B) and at the same time the movement from q.sub.1 to q.sub.G is further optimized (C). When the robot has reached position q1, the next portion of the trajectory is immediately executed (D) up to q.sub.2=q(2ΔT) and the optimization continues from this point (E). This approach is repeated until the target point q.sub.G is reached within one time step (F), so that no further optimization is necessary.

(45) The practical implementation thus requires close interaction between the planning and execution. Each planning step (A, C, D, F) must finish its calculation within the sampling time Δt. In the time step t.sub.k=kΔt, the calculated trajectory from q.sub.k to q.sub.G must be permissible at least for the next sampling time, i.e. q.sub.k.fwdarw.q.sub.k+1) must be collision-free and comply with the closed kinematics. However, the execution (B, D) only starts when the last movement q.sub.k−1.fwdarw.q.sub.k is completed, so that the robot is exactly at the position q.sub.k at the transition point. Problems can arise, for example, if there are hops in speed at the transition point. This must be compensated for using a follow-up control.

(46) For predictive planning, the optimization problem presented at the beginning is adjusted in the time step t.sub.k as follows:

(47) $\underset{\stackrel{\_}{u}(.Math.),T}{\min }J\left(\stackrel{\_}{u},T\right)=V\left(T\right)+{\int }_{0}l\left(\stackrel{\_}{x}\left(\tau \right),\stackrel{\_}{u}\left(\tau \right)\right)d\tau u.B.v.\stackrel{\stackrel{}{\_}}{x}\left(\tau \right)=f\left(\stackrel{\_}{x}\left(\tau \right),\stackrel{\_}{u}\left(\tau \right)\right)\stackrel{\_}{x}\left(0\right)=x_{k+1}=x\left(t_k+\Delta t\right),\stackrel{\_}{x}\left(T\right)=x_G\stackrel{\_}{u}\left(\tau \right)\in \left[u^{-},u^{+}\right],\tau \in \left[0,T\right]T\in \left[T^{-},T^{+}\right]g\left(\stackrel{\_}{x}\left(\tau \right)\right)=0,\tau \in \left[0,T\right]h\left(\stackrel{\_}{x}\left(\tau \right)\right)\le 0,\tau \in \left[0,T\right].$

(48) Here, x and ū denote the internal variables of the optimizer with respect to time τ∈[0, T] to distinguish them from the real state x(t). The initial condition in particular has changed with regard to the optimization problem, since in time step t.sub.k it must be planned based on the future state x(t.sub.k+Δt). Furthermore, the end time T can be considered as a further optimization variable with limits and a weighting by the cost function.

(49) To ensure that the target position q.sub.G is not only asymptotically reached for t.fwdarw.∞, a hard final constraint q(T)=q.sub.G must be set in the optimization problem and the time horizon T must be successively shortened until the target point T=Δt is exactly reached in one step. For this purpose, based on an initial estimation of the time required, the end time T is shortened by Δt after each time step, i.e. T.sup.(k+1)=T.sup.(k)−Δt. To realize a time-optimal behavior as well as an adaptation to changed environmental conditions, the end time T must be considered as a free optimization variable.

(50) Especially when planning with closed kinematics, it can happen that the sampling time Δt is not sufficient to calculate a permissible trajectory in the first step. More time must therefore be invested in the optimization before the first step is taken. For this purpose, either the maximum number of iterations N.sub.ocp of this precalculation can be limited or a time T.sub.validate<T can be predefined. In this case, the execution only starts when the corresponding part of the trajectory is allowed. For example, it can be demanded that the movement must already be collision-free for the next second.

(51) FIG. 4 shows an example embodiment of a corresponding apparatus. The apparatus is designated here in its entirety by reference numeral 10 and illustrated as a two-arm robot with a first manipulator 28 and a second manipulator 30 in this example embodiment. The manipulators 28, 30 are connected to each other via a common base 31 and are constructed symmetrically. Each manipulator 28, 30 has three links 32 and three joints 34 and at the ends of each manipulator 28, 30 there is an end effector 36, which is a gripper in present example. Both manipulators 28, 30 can jointly grip and move a workpiece 38. The connection via workpiece 38 creates a closed kinematic chain 40, which is indicated here by the dotted line.

(52) During the movement of the two manipulators 28, 30 and thus also during their movement planning, this closed kinematics must be taken into account, as otherwise the manipulators 28, 30 would drop the workpiece 38.

(53) The movement of the first manipulator 28 and the second manipulator 30 is coordinated by a control unit 42. The control unit 42 carries out the method described above, i.e. the control unit 42 determines a coordinated movement path for both the first manipulator 28 and the second manipulator 30 taking into account the closed kinematics. The control unit 42 is thus configured to solve the underlying optimization problem numerically.

(54) Preferably, the control unit 42 carries out planning and execution in parallel. This means that the manipulators 28, 30 are already moved even before an optimal and complete motion planning has taken place.

(55) In addition to the closed kinematics, the control unit 42 also takes into account obstacles 44, which are located in the movement range of the manipulators 28, 30. Particularly, not only static obstacles, but also to moving obstacles in the movement space are taken into account.

(56) Due to parallel planning and execution including continuous optimization, a changing environment can be taken into account.

(57) The phrase at least one of A, B, and C should be construed to mean a logical (A OR B OR C), using a non-exclusive logical OR, and should not be construed to mean “at least one of A, at least one of B, and at least one of C.”