Method of command control for interactive path guidance of kinematically-redundant manipulator

Abstract

Beginning with a successive commanded end-effector destination shift, the method of the invention, which includes a calculation corresponding to a special algorithm of inverse kinematics using the Jacobi Matrix in the control of a manipulator, effects an optimization of weighted criteria (energy criteria and reference-position criteria) in a real time cycle. The method of the invention can be used in interactive path guidance of a kinematically-redundant manipulator.

Description

FIELD OF THE INVENTION

The invention relates to a method of command control for an interactive path guidance of a kinematically-redundant manipulator based on end-effector destination shifts commanded by a programmer with the aid of a manual control ball or the like, in connection with a calculation according to an algorithm of inverse kinematics with the use of the Jacobi Matrix of kinematics.

REVIEW OF THE RELATED TECHNOLOGY

Methods of command control for a path guidance of a kinematically-redundant manipulator on the basis of end-effector destination shifts commanded by a programmer, in connection with a calculation according to an algorithm of inverse kinematics using the Jacobi Matrix, are known from Siciliano, B., Sciavicco, L.; "Modelling and Control of Robot Manipulators," McGraw-Hill Companies (1996), pp. 95 to 101, and Vukobratovic, M., Kircanski, N.: "Kinematics and Trajectory Synthesis of Manipulation Robots," Springer-Verlag, Tokyo, 1986, pp. 105 to 122.

In this connection, a known method that includes a calculation of a generalized inverse of the Jacobi Matrix possesses a few advantageous properties, but is also associated with a number of disadvantages. For example, a smoothness of the calculated articulated path and low wear of the robot drive are attained through minimization of the local articulated position offset (local energy criteria). In addition, the spacing between the articulated positions of the physical articulated stops is taken into consideration through the optimization of global criteria in the zero space of the Jacobi Matrix. Disadvantages of this method are that it cannot guarantee that path limitations will be maintained with physical articulated stops, an unstable behavior occurs in singular robot position due to a (generalized) inversion of the Jacobi Matrix, and inefficient robot path courses are possible when conflicting local and global criteria exist.

In another known method, in which a calculation of the transposed Jacobi Matrix is performed, the commanded end-effector destination is attained iteratively corresponding to a representation of reverse kinematics. An advantageous feature of this method is a stable behavior in singular robot positions, because the Jacobi Matrix is not inverted. However, path limitations are not considered with physical articulation stops and maximum articulation speeds. Superordinate heuristics must be constructed to meet these requirements. Inefficient path courses occur in the form of interference movements of the end effectors, because the cartesian linear movement commanded by the manual control ball cannot be transferred exactly to the end effector of the manipulator. Moreover, the robot drive is subjected to high material wear due to abrupt passage through singular robot positions and because of a generally insufficiently-smooth articulation path (no consideration of local energy criteria). A further negative consequence is a low convergence speed, that is, a sacrifice of real-time capability, because no practical, optimum strategy is known for determining the positively-defined cartesian stiffness matrix.

DE 33 44 633 C2 describes a real-time control in which the redundant articulations that are not necessary for the movement of an end effector are held securely for calculating the articulation speed, which simplifies the calculation of the inverse Jacobi Matrix. This type of calculation is performed for at least one of the articulation combinations. The speeds for each articulation are then determined through averaging of the calculated articulation speed. Thus, in the prior art, a weighting of the articulation speeds takes place.

U.S. Pat. No. 5,430,643 also describes a real-time method; in this case, the inverse Jacobi Matrix is calculated. The method known from this U.S. patent document takes into consideration weight values for the articulation speeds, as well as path limitations for at least a graphic simulation of the robot's movements.

SUMMARY OF THE INVENTION

It is the object of the invention to provide a method of command control for an interactive path guidance of a kinematically-redundant manipulator, with which path limitations are reliably maintained through physical articulation stops and maximum allowable articulation speeds, and no superordinate heuristics are required for checking the determined solution for allowability with respect to the path limitations, and making corrections if necessary.

In accordance with the invention, which relates to a method of command control for an interactive path guidance of a kinematically-redundant manipulator of the type mentioned at the outset, this object is accomplished in that

beginning with a commanded end-effector destination shift and the current actual value (q.sub.i) of the articulation position of the manipulator, a new articulation position (q.sub.i+1) is calculated with consideration of a quality function (f(q)), which is parameterized by non-negative weighting values (.alpha..sub.j, .beta..sub.j) and consideration of path limitations through physical articulation stops (q.sub.min, q.sub.max), maximum articulation speed (q.sub.max) and the kinematic equation represented by the Jacobi Matrix, which articulation position predetermines the new values for the articulation regulator, with the quality function being the sum of energy criterion and reference-position criterion, the energy criterion being calculated from (q-q.sub.i).sup.t diag(.alpha..sub.j)(q-q.sub.i) and the reference-position criterion being calculated form (q-q.sub.ref).sup.t diag(.beta..sub.j)(q-q.sub.ref) and the value q.sub.ref being a predetermined articulation-position value that is determined such that the sequence of calculated articulation-position values (q.sub.i) runs near this reference-position value; beginning with an articulation-position value q.sub.1 as a starting point, an allowable optimization vector is determined on the basis of the quality function with respect to all active constraints that indicate which path limitations have been attained, and this vector is scaled according to the inactive constraints that indicate which path limitations have not been attained; the scaled optimization vector is added to the articulation-position value calculated in the previous iteration step; and the optimum quality of these articulation-position values is evaluated based on the quality function and the constraints activated in the newly-calculated articulation position.

According to an advantageous refinement to the method of the invention, the Jacobi Matrix of kinematics is used to define the desired cartesian, translatoric (transactional) or rotary end-effector destination shift .DELTA.x.sup.d.sub.t and .DELTA.x.sup.d.sub.t per sampling time .DELTA.T starting with a commanded end-effector destination shift .DELTA.x.sub.c :=(.DELTA.x.sub.l,.DELTA.x.sub.r) at a time T.sub.i, with .DELTA.x.sub.t .epsilon..sup.3 and .DELTA.x.sub.r .epsilon..sup.3 respectively indicating the translatoric or rotary component of the commanded end-effector destination shift, and .DELTA.x.sup.max.sub.t and .DELTA..sup.max.sub.r respectively defining the maximum (scalar-value), translatoric or rotary, cartesian position offset of the end effector per sampling time .DELTA.T: ##EQU1## (the quantity .vertline.x.vertline. of a vector x is determined here and hereinafter by its euclidian norm ##EQU2## the desired end-effector destination shift is explained by .DELTA.x.sub.d :=(.DELTA.x.sup.d.sub.t,.DELTA.x.sup.d.sub.t), and the energy criteria and reference-position criteria can be suitably weighted corresponding to the predominant, problem-specific requirements in that each axis j is allocated two positive numerical values .alpha..sub.j, .beta..sub.j, which are predetermined by the programmer, with the value .alpha..sub.j serving in the weighting of an energy criterion that evaluates the difference between two adjacent, calculated articulation positions of axis j, and the value .beta..sub.j serving in the weighting of a criterion that evaluates the deflection of the articulation position q.sub.j from a reference value q.sub.ref j, which is also predetermined by the programmer, like the physical articulation stops q.sub.min, q.sub.max of the manipulator, which relate to path limitations, and the articulation-speed limitations q'.sub.max, such that an allowable, optimum articulation-position offset .DELTA.q.sup.i is calculated in the attainment of the method's output values to effect a desired end-effector destination shift .DELTA.x.sup.i.sub.d at time T.sub.i-1 during the time interval .DELTA.T, with the algorithm of inverse kinematics that progresses in an initialization phase and a subsequent optimization phase. The current nominal value is provided to the articulation regulators from q.sup.i+1.sub.soll :=q.sup.i.sub.soll +.DELTA.q.sup.i (the articulation regulators correspondingly proceed with q.sup.i.sub.soll); allowable articulation-position offset is understood to mean that the articulation-position value that has been updated with .DELTA.q satisfies the physical articulation stops q.sub.min, q.sub.max corresponding to q.sub.min .ltoreq.q.sub.soll +.DELTA.q.ltoreq.q.sub.max, and .DELTA.q satisfies the articulation speed limitations q'.sub.max corresponding to .vertline..DELTA.q.vertline..ltoreq.q'.sub.max .DELTA.T, and that the type of optimum quality of .DELTA.q can be determined by a programmer through the quality-criterion weighting .alpha.,.beta., and the sampling time .DELTA.T is dimensioned, corresponding to .DELTA.T.gtoreq.max(.DELTA.t.sub.q,.DELTA.t.sub.r), such that the computing time .DELTA.t.sub.q for calculating an optimum, allowable articulation-position offset .DELTA.q, and the adjustment time .DELTA.t.sub.r, which the regulators require for adapting the axial positions to the nominal values q.sub.soll, lie within the time span .DELTA.T.

BRIEF DESCRIPTION OF THE DRAWING

In connection with the above-described features of the invention, FIG. 1 illustrates a representation of the input/output flux of the inverse kinematics in a real-time grid.

The base structure of the special method of the invention, which utilizes an inverse kinematics for command control, is illustrated schematically in FIG. 2.

DETAILED DESCRIPTION OF THE PREFERRED EMBODIMENT

The method of the invention results in a smoothness of the calculated articulation path, and low wear of the robot drive through minimization of the local articulation-position offset (local energy criteria). "Narrow" articulation paths around a reference position (e.g., zero position) are attained. Articulation stops are advantageously avoided extensively, or approached gently or with low wear. This behavior is caused by a delay effect of the robot axes, which acts proportionally to the distance from the reference position. In addition, a stable behavior occurs in singular robot positions, because the Jacobi Matrix is not inverted. Another advantage is the occurrence of efficient path courses through the exact transfer of the cartesian linear movement commanded by the manual control ball to the end effector of the manipulator.

The following is an algorithmic description of the method of the invention.

J.sub.i, i=1, . . . , ndof indicates the columns of the Jacobi Matrix of kinematics, in point q.sub.0 of the current manipulator position, ndof indicates the number of articulations and .epsilon..sub.i :=q'.sub.max .DELTA.T indicates the maximum allowed articulation-position offset per sampling time .DELTA.T. With ##EQU3## and J.sub..epsilon. :=(J.sub.1 .epsilon..sub.1,J.sub.2 .epsilon..sub.2, . . . , J.sub.ndof .epsilon..sub.ndof), the incremental kinematic equation is as follows:

(p.DELTA.x.sup.d.sub.t,q.DELTA.x.sup.d.sub.r)=J.sub..epsilon. y.(1)

Because of the path limitations, the following box limitations result for ##EQU4## Here q.sub.0 indicates the most recently calculated, allowable nominal-position set, to whose values the axes of the robot have already been adjusted, the values representing the actual value.

Further define: ##EQU5## So that all path limitations can be met, according to the kinematic equation (1), the desired end-effector destination shift is subjected to a centric extension described in the form of two scalars: 0.ltoreq.p.ltoreq.1, 0.ltoreq.q.ltoreq.1.

The values p, q are maximized with the method of the invention such that all path limitations are maintained.

Define b:=(0,0,0,0,0,0,y.sub.min,0,0,-y.sub.max,-1,-1).epsilon..sup.2n+6, x:=(y,p,q) and the auxiliary values .DELTA.x.sub.t :=(.DELTA.x.sup.d.sub.r,0,0,0).epsilon..sup.6,.DELTA.x.sub.r :=(0,0,0,.DELTA.x.sup.d.sub.r).epsilon..sup.6 and the matrix A.epsilon..sup.2n+6,n with n:=ndof+2: ##EQU6##

Here I.sub.n .epsilon..sup.n,n indicates the unit matrix. Furthermore, .alpha..sub.i, i=1, . . . , 2n+6 indicate the lines of A.

The weights .alpha., .beta. of the criteria are represented by the diagonal matrix .LAMBDA..epsilon..sup.n,n, as the Cholesky factor of the Hess Matrix, from the scaled sum of the energy criterion (q-q.sub.0) .sup.t diag (.alpha..sub.i)(q-q.sub.0) and the reference-position criterion (q-q.sub.ref) .sup.t diag (.beta..sub.j)(q-q.sub.ref). Their main diagonal elements are: ##EQU7##

The base structure of the special method of the invention, which utilizes an inverse kinematics for command control, is illustrated schematically in FIG. 2.

An algorithm used in the method of the invention for inverse kinematics is described in more detail in conjunction with FIG. 2. In the used algorithm of inverse kinematics, first an initialization phase configured as follows is performed:

counting index: k=0; starting value: x.sub.k =(y.sub.k,p.sub.k,q.sub.k)=0; kmax>7; use m.sub.k :=8 to initialize the matrix of active constraints A.sup.t .epsilon..sup.m.sbsp.k.sup.,n : ##EQU8## initialize the index quantity J.sub.k :=(j.sup.1.sub.k j.sup.2.sub.k, . . . ,j.sup.2n+6.sub.k) for characterizing the active, singular and inactive, constraints: ##EQU9## initialize the orthogonal triangular factorization of A: (r.sub.k,J.sub.k,Y.sub.k,Z.sub.k,L.sub.k,S.sub.k):=.PHI..sup.A.sub.0 (A,m.sub.k,J.sub.k); initialize the orthogonal, regular triangular factorization of Z:=.LAMBDA.Z.sub.k : (Q.sup.Z.sub.k,R.sup.Z.sub.k):=.PHI..sup.Z.sub.0 (Z,r.sub.k); initialize the vector g.sub.0 .epsilon.R.sup.n :g.sub.k :=2(.beta..sup.scal.sub.1 (y.sup.1.sub.k -y.sup.1.sub.ref)+.alpha..sup.scal.sub.1 y.sup.1.sub.k, . . . , .beta..sup.scal.sub.ndof (y.sup.ndof.sub.k -y.sup.ndof.sub.ref)+.alpha..sup.scal.sub.ndof y.sup.ndof.sub.k,p.sub.k -1,q.sub.k -1). The following abbreviated form is used for g.sub.k : 2(.beta..sup.scal.sub.i (y.sup.i.sub.k -y.sup.i.sub.ref)+.alpha..sup.scal.sub.i y.sup.i.sub.k,p.sub.k -1,q.sub.k -1).

In the used algorithm of inverse kinematics, an optimization phase constructed as follows is next:

I. Calculate the direction of optimization d.sub.k : R.sup.Z.sub.k d.sub.aux =-Q.sup.Z.sub.k Z.sub.k g.sub.k.

From this, determine d.sub.aux through reverse substitution.

R.sup.Z.sub.k d.sub.z =Q.sup.Z.sub.k d.sub.aux.

From this, determine d.sub.z through reverse substitution. Define the direction of optimization:

d.sub.k :=Z.sub.k d.sub.z. II. Determine the maximum step width s.sub.k and the index j.sup.i0.sub.k of the limiting secondary condition: ##EQU10##

(In the event of ambiguity, select the j0 having the smallest index i0.)

III. Check optimum quality and update all matrix and index values. (a) If s.sub.k <1 (secondary condition j.sup.i0.sub.k has become active): i. If m.sub.k <n (there are still inactive constraints): x.sub.k+1 =x.sub.k +s.sub.k d.sub.k,x.sub.k+1 =(y.sub.k+1,p.sub.k+1,q.sub.k+1); g.sub.k+1 =2(.beta..sup.scal.sub.i (y.sup.i.sub.k+1 -y.sup.i.sub.ref)+.alpha..sup.scal.sub.i y.sup.i.sub.k+1,p.sub.k+1 -1,q.sub.k+1 -1). Update matrix factorizations and index quantity: (r.sub.k+1,m.sub.k+1,J.sub.k+1,Y.sub.k+1,Z.sub.k+1,L.sub.k+1,S.sub.k+1):=.PHI..sup.A.sub.+ (i0,r.sub.k,m.sub.k,J.sub.k,Y.sub.k,Z.sub.k,L.sub.k,S.sub.k). Check x.sub.k+1 for optimum quality: If Z.sup.q.sub.t+1 g.sub.k+1 =0; and .lambda..sub.k+1 =LLS(r.sub.k+1,m.sub.k+1,g.sub.k+1,Y.sub.k+1,L.sub.k+1).gtoreq.0 then: Go to Step IV. Otherwise: k=k+1 (increase iteration index) If k>kmax: Go to Step IV. Otherwise: Determine the orthogonal, regular triangular factorization of Z:=.LAMBDA.Z.sub.k+1 : (Q.sup.Z.sub.k+1,R.sup.Z.sub.k+1):=.PHI..sup.Z (Z,r.sub.k+1). Go to Step I. ii. If m.sub.k =n (corner of the allowable range is attained; exchange of active constraints is necessary): x.sub.k+1 =x.sub.k +s.sub.k d.sub.k,x.sub.k+1 =(y.sub.k+1,p.sub.k+1,q.sub.k+1); g.sub.k+1 =2(.beta..sup.scal.sub.i (y.sub.k+1 -y.sub.ref)+.alpha..sup.scal.sub.i y.sup.i.sub.k+1,p.sub.k+1 -1,q.sub.k+1 -1); .lambda..sub.k =LLS(r.sub.k+1,m.sub.k+1,g.sub.k,Y.sub.k,L.sub.k). Release a direction having the highest optimization potential: ##EQU11## Update matrix factorizations and index quantity: (r.sub.k,m.sub.k,J.sub.k,Y.sub.k,Z.sub.k,L.sub.k,S.sub.k):=.PHI..sub.-.sup.A (j0,r.sub.k,m.sub.k,J.sub.k,Y.sub.k,Z.sub.k,L.sub.k,S.sub.k). Activate constraint j.sup.i0.sub.k. Update matrix factorizations and index quantity: (r.sub.k+1,m.sub.k+1,J.sub.k+1, Y.sub.k+1,Z.sub.k+1,L.sub.k+1,S.sub.k+1):=.PHI..sup.A.sub.+ (i0,r.sub.k,m.sub.k,J.sub.k, Y.sub.k,Z.sub.k,L.sub.k,S.sub.k). Check x.sub.k+1 for optimum quality: If Z.sup.t.sub.k+1 g.sub.k+1 =0; and .lambda..sub.k+1 =LLS(r.sub.k+1,m.sub.k+1,g.sub.k+1,Y.sub.k+1,L.sub.k+1).gtoreq.0 then: Go to Step IV. Otherwise: k=k+1 (increase iteration index) If k>kmax: Go to Step IV. Otherwise: Determine the orthogonal, regular triangular factorization of Z:=.LAMBDA.Z.sub.k+1 : (Q.sup.Z.sub.k+1,R.sup.Z.sub.k+1):=.PHI..sup.Z (Z,r.sub.k+1). Go to Step I. (b) If s.sub.k =1 (no new active constraint). x.sub.k+1 =x.sub.k +d.sub.k,x.sub.k+1 =(y.sub.k+1,p.sub.k+1,q.sub.k+1); g.sub.k+1 =2(.beta..sup.scal.sub.i (y.sub.k+1 -y.sub.ref)+.alpha..sup.scal.sub.i y.sub.k+1,p.sub.k+1 -1,q.sub.k+1 -1). Update matrix factorizations and index quantity: (r.sub.k+1,m.sub.k+1,J.sub.k+1,Y.sub.k+1,Z.sub.k+1,L.sub.k+1,S.sub.k+1):=(r.sub.k,m.sub.k,J.sub.k,Y.sub.k,Z.sup.-.sub.k,L.sub.k,S.sub.k). Check x.sub.k+1 for optimum quality: If Z.sup.l.sub.k+1 g.sub.k+1 =0; and .lambda..sub.k+1 :=LLS(r.sub.k+1,m.sub.k+1,g.sub.k+1,Y.sub.k+1,L.sub.k+1).gtoreq.0 then: Go to Step IV. Otherwise: Release a direction having the highest optimization potential: ##EQU12## Update matrix factorizations and index quantity: (r.sub.k+1,m.sub.k+1,J.sub.k+1,Y.sub.k+1,Z.sub.k+1,L.sub.k+1,S.sub.k+1 =.PHI..sup.A.sub.- (j0,r.sub.k,m.sub.k,J.sub.k,Y.sub.k,Z.sub.k,L.sub.k,S.sub.k). k=k+1 (increase iteration index) If k>kmax: Go to Step IV. Otherwise: Determine the orthogonal, regular triangular factorization of Z:=.LAMBDA.Z.sub.k+1 : (Q.sup.Z.sub.k+1,R.sup.Z.sub.k+1):=.PHI..sup.Z (Z,r.sub.k+1) Go to Step I. IV. Solution x.sub.k+1 is determined. Stop! End of algorithm of inverse kinematics. Definition of the function LLS: .lambda.=LLS(r,m,g,Y,L). .lambda..sub.i :=0,i=r+1, . . . , m; .lambda..sub.nL :=(.lambda..sub.r+1, . . . , .lambda..sub.m); L.sup.t .lambda..sub.L =Y.sup.t g. From this, determine .lambda..sub.L .epsilon..sup.r through reverse substitution. Define the output value of the function: .lambda.:=(.lambda..sub.L,.lambda..sub.nL). With I.sub.eq :=1+max.sub.1.ltoreq.i.ltoreq.m .vertline..lambda.(i).vertline., set the components of .lambda. that are among the secondary conditions of Equation (1) at the value l.sub.eq. End of the function LLS. Definition of the function .PHI..sub.+ : (r,m,J,Y,Z,L,S)=.PHI..sup.A.sub.+ (i0,r,m,J,Y,Z,L,S). Activate constraint .sub.l j.sup.i0. The j.sup.i0 -th line of A is indicated by .alpha..sub.j0 ' with j0:=j.sup.i0. Define Q.sup.1 :=(Y,Z) and form .alpha.:=Q.alpha..sub.j0. Partition .alpha.:=(.alpha..sub.Y,.alpha..sub.Z) with .alpha..sub.Y .epsilon..sup.r and .alpha..sub.Z .epsilon..sup.n-r. Determine the Householder reflection H.epsilon..sup.n-r,n-r so that: .vertline.60 .sub.Z .vertline.e.sub.l =H.alpha..sub.Z, with e.sub.1 :=(1, 0, . . . , 0).epsilon..sup.n-r. Define the unitary transformer H.epsilon..sup.n,n according to: ##EQU13## Define the unitary transformer: Q:=HQ; Update indices: ##EQU14## Update matrix factorizations: Y is the partial matrix of Q.sup.t that includes columns 1 through r of Q.sup.t. Z is the partial matrix of Q.sup.l that includes columns r+1 through n of Q.sup.l. ##EQU15## End of the function .PHI..sub.+. Definition of the function .PHI..sub.- : (r,m,J,Y,Z,L,S)=.PHI..sup.A.sub.- (i0,r,m,J,Y,Z,L,S). Inactivate constraint j.sup.j0. If j0>r: (Eliminate singular, active secondary condition) Update indices: r:=r; m:=m-1; Exchange constraints: ##EQU16## Update matrix factorizations: Y:=Y; Z:=Z; L:=L; S:=(S.sub.1, . . . , S.sub.j0-r-1, S.sub.j0-r+1, . . . , S.sub.m-r), where S.sub.i indicate the S columns. If j0.ltoreq.r: (Eliminate regular, activate constraint) R results from R:=L.sup.1 through striking of the j0-th column. The elements of R that occur at the locations (j0,j0+1), (j0+1 j0+2), . . . , (r,r+1) are canceled through left multiplication with a sequence of unitary elimination matrices E.sub.j0,j0+1, . . . , E.sub.r-1,r .epsilon..sup.r,r. Define linear transformers: R:=E.sub.r-1,r E.sub.r-2,r-1 . . . E.sub.j0,j0+i R; S:=E.sub.r-1,r E.sub.r-2,r-1 . . . E.sub.j0,j0+1 S; ##EQU17## Q.sup.t :=(Y,Z); Q:=E.sub.r-1,r E.sub.r-2,r-1 . . . E.sub.j0,j0+1 Q; If the last line of S is zero, or if r=m: update indices: r:=r-1; m:=m-1; Exchange constraints: ##EQU18## Update matrix factorizations: Y is the partial matrix of Q.sup.t that contains columns 1 through r of Q.sup.t. Z is the partial matrix of Q.sup.t that contains columns r+1 through n of Q.sup.t. If r>m, stroke the last line of S and R. Set: L:=R.sup.t ; S:=S; Otherwise (convert singular, active constraint into a regular, active constraint): Determine the element of the last line of S with the smallest index l0 such that: S.sub.r,l0 .noteq.0. Exchange column l0 and column 1 of S. Define linear transformer: R:=(R,S.sub.l0), where S.sub.i0 indicates the l0-th column of S. Update indices: r:=r; m:=m-1; Exchange constraints: ##EQU19## Update matrix factorizations: Y is the partial matrix of Q.sup.t that includes columns 1 through r of Q.sup.t. Z is the partial matrix of Q.sup.t that includes columns r+1 through n of Q.sup.t. L:=R.sup.t ; S:=(S.sub.2, . . . , S.sub.l0-1,S.sub.1,S.sub.l0+1, . . . , S.sub.m-r), End of the function .PHI.. Definition of the function .PHI..sup.A.sub.0 : (r,J,Y,Z,L,S):=.PHI..sup.A.sub.0 (A,m,J). 1. Define: A.sup.0 :=A, i:=0. 2. Define the following matrix recursion: A.sub.i+1 :=H.sub.i A.sup.i,i.gtoreq.0. The unitary transformer H.sub.i .epsilon..sup.n,n is explained as follows: ##EQU20## The Householder reflection H.sub.i .epsilon..sup.n-i,n-i is defined such that: .vertline..alpha..sup.i.sub.k0 .vertline.e.sub.l =H.sub.i .alpha..sup.i.sub.k0. with e.sub.1 :=(1,0, . . . , 0).epsilon..sup.n-i. Here the vectors .alpha..sup.i.sub.k .epsilon..sup.n-i,k=1, . . . , n-i indicate the columns of the matrix A.sup.i .epsilon..sup.n-i,m-i : ##EQU21## Moreover, 1.ltoreq.k0.ltoreq.n-i is the smallest index with .alpha..sup.i.sub.k0 .noteq.0. If no such k0 exists, then go to Step III; otherwise, go to Step IV. 3. Define indices: r:=i; j:=(j, . . . , j.sup.2n+6). Define matrix factorizations: Q:=H.sub.r-1 H.sub.r-2 . . . H.sub.0 ; Y is the partial matrix of Q.sup.t that includes columns 1 through r of Q.sup.t. Z is the partial matrix of Q.sup.t that includes columns r+1 through n of Q.sup.t. T is the matrix that results from the stroking of lines r+1 through n from A.sup.i+1. Define L.epsilon..sup.r,r, and S.epsilon..sup.r,m-r, according to: (L.sup.t,S):=T. Stop: Function .PHI..sup.A.sub.0 has been performed completely! 4. Exchange column i+k0 with column i+1 in A.sup.i. ##EQU22## Set j.sup.k :=j.sup.k .A-inverted.k, increase the counting index i=i+1 and go to Step II. End of function .PHI..sup.A.sub.0. Definition of function .PHI..sup.Z.sub.0 : (Q.sup.Z,R.sup.Z):=.PHI..sup.Z.sub.0 (Z,r). Determine the QR triangular factorization of Z: ##EQU23## Here Q.sup.Z .epsilon..sup.n,n indicates a unit matrix, and R.sup.Z .epsilon..sup.r,r indicates an upper triangular matrix. End of function .PHI..sup.Z.sub.0.

The foregoing description of the specific embodiments will so fully reveal the general nature of the invention that others can, by applying current knowledge, readily modify and/or adapt for various applications such specific embodiments without undue experimentation and without departing from the generic concept, and, therefore, such adaptations and modifications should and are intended to be comprehended within the meaning and range of equivalents of the disclosed embodiments. It is to be understood that the phraseology or terminology employed herein is for the purpose of description and not of limitation. The means and materials for carrying out various disclosed functions may take a variety of alternative forms without departing from the invention.

Thus the expressions "means to . . . " and "means for . . . " as may be found in the specification above and/or in the claims below, followed by a functional statement, are intended to define and cover whatever structural, physical, chemical or electrical element or structure may now or in the future exist carries out the recited function, whether or not precisely equivalent to the embodiment or embodiments disclosed in the specification above; and it it intended that such expressions be given their broadest interpretation.

Claims

1. A method of command control, by a programmer, for interactive path guidance of a kinematically-redundant manipulator based on end-effector destination shifts from the programmer, in connection with a calculation of articulated-position values according to an algorithm of inverse kinematics; the method comprising the steps of:

providing a manual control device;

calculating a new articulation position (q.sub.i+1) from an end-effector destination shift command from the control device and a current articulation position actual value (q.sub.i) of the manipulator;

the calculating step including as calculating factors a quality function (f(q)), which is parameterized by non-negative weighting values (.alpha..sub.j, .beta..sub.j) and path limitations through physical articulation stops (q.sub.min, q.sub.max), maximum articulation speed (q.sub.max), and a kinematic equation represented by the Jacobi Matrix;

calculating an energy criterion from

(q-q.sub.i).sup.t diag(.alpha..sub.j)(q-q.sub.i)

calculating a reference-position criterion from

(q-q.sub.ref).sup.t diag(.beta..sub.j)(q-q.sub.ref)

predetermining by the new articulation position new values for an articulation regulator, with the quality function being a sum of the energy criterion and the reference-position criterion;

determining a value q.sub.ref as a predetermined articulation-position value such that a sequence of calculated articulation-position values (q.sub.i) runs near a value of the reference-position criterion;

determining an allowable optimization vector, beginning with one of the articulation-position values q.sub.i as a starting point, on the basis of the quality function with respect to all active constraints that indicate which path limitations have been attained;

scaling the allowable optimization vector according to inactive constraints that indicate which path limitations have not been attained;

adding the scaled optimization vector to the articulation-position value calculated in the previous step of the iteration;

evaluating an optimum quality of the articulation-position values based on the quality function and the constraints activated in the newly-calculated articulation position.

2. The method according to claim 1, in which the control device comprises a control ball.

3. The method according to claim 1, further comprising steps of:

defining by use of the Jacobi Matrix of kinematics predetermined cartesian translational or rotary end-effector destination shift .DELTA.x.sup.d.sub.t and .DELTA.x.sup.d.sub.r per sampling time .DELTA.T starting with a commanded end-effector destination shift .DELTA.x.sub.c :=(.DELTA.x.sub.l,.DELTA.x.sub.r) at a time T.sub.i, with .DELTA.x.sub.t .epsilon..sup.3 and .DELTA.x.sub.r .epsilon..sup.3 respectively indicating a translational or rotary component of the commanded end-effector destination shift, and .DELTA.x.sup.max.sub.t and .DELTA.x.sup.max.sub.r respectively defining a maximum scalar-valued, translational or rotary, cartesian position offset of the end effector per sampling time .DELTA.T: ##EQU24## determining a quantity .vertline.x.vertline. of a vector x by its euclidian norm ##EQU25## wherein the desired end-effector destination shift is given by .DELTA.x.sub.d :=(.DELTA.x.sup.d.sub.t,.DELTA.x.sup.d.sub.r), and the energy criteria and reference-position criteria are suitably weighted corresponding to predominant, problem-specific requirements in that each axis j is allocated two positive numerical values .alpha..sub.j, .beta..sub.j, which are predetermined by the programmer, with the value .alpha..sub.j serving as the weighting of an energy criterion that evaluates a difference between two adjacent, calculated articulation positions of axis j, and the value .beta..sub.j serving as the weighting of a criterion that evaluates deflection of the articulation position q.sub.j from a reference value q.sub.ref j, which is also predetermined by the programmer, similar to the physical articulation stops q.sub.min, q.sub.max of the manipulator, which relate to path limitations, and

calculating the articulation-speed limitations q'.sub.max, such that an allowable, optimum articulation-position offset .DELTA.q.sup.i is calculated in attaining output values to effect a desired end-effector destination shift .DELTA.x.sup.i.sub.d at time T.sub.i-1 during the time interval .DELTA.T, with an algorithm of inverse kinematics that progresses in an initialization phase and a subsequent optimization phase, the current nominal value being provided to the articulation regulators from q.sup.i+1.sub.soll :=q.sup.i.sub.soll +.DELTA.q.sup.i (the articulation regulators correspondingly proceed with q.sup.i.sub.soll), with allowable articulation-position offset implying that the articulation-position value that has been updated with .DELTA.q satisfies the physical articulation stops q.sub.min, q.sub.max corresponding to q.sub.min .ltoreq.q.sub.soll +.DELTA.q.ltoreq.q.sub.max, and .DELTA.q satisfies the articulation speed limitations q'.sub.max corresponding to .vertline..DELTA.q.vertline..ltoreq.q'.sub.max .DELTA.T, and that the type of optimum quality of .DELTA.q is determined by the programmer through the quality-criterion weighting .alpha., .beta., and the sampling time .DELTA.T is dimensioned, corresponding to .DELTA.T.gtoreq.max(.DELTA.t.sub.q,.DELTA.t.sub.r);

whereby computing time .DELTA.t.sub.q for calculating an optimum, allowable articulation-position offset .DELTA.q, and an adjustment time .DELTA.t.sub.r, which the regulators require for adapting axial positions to the nominal values q.sub.soll, lie within the time span .DELTA.T.

4. The method according to claim 3, characterized in that the cartesian end-effector destination shift .DELTA.x.sub.c commanded at a time T.sub.i by the programmer is commanded in the form of a 6-dimensional increment vector.

5. The method according to claim 3, characterized in that the increment vector of the cartesian end-effector destination shift .DELTA.x.sub.c commanded at a time T.sub.i is commanded by the programmer through a difference formation between absolute end-effector coordinates.