CN102528802A - Motion driving method for robot with nine degrees of freedom - Google Patents

Abstract

The invention provides a motion driving method for a robot with nine degrees of freedom. The method includes realizing motion path planning and gesture planning of a tail-end hand of the robot by the aid of information, which is provided by a user, of positions and gestures of the tail-end hand at multiple points in a cartesian space, and obtaining positions of the tail-end hand of the robot in an integral driving process and gestures of the tail-end hand of the robot in the integral driving process; solving a joint space from a first degree of freedom to a sixth degree of freedom of the robot at a certain point of time by the aid of the obtained information of the positions of the tail-end hand of the robot, and solving a joint space from a seventh degree of freedom to a ninth degree of freedom of the robot at the point of time by the aid of the obtained information of the gestures of the tail-end hand of the robot; and driving the robot to move by the aid of the obtained joint spaces of the ninth degrees of freedom. The motion driving method for the robot has the advantages that the user only needs to provide the positions and the gestures of the tail-end hand of the robot at a few of points in the space, accordingly, the robot can be driven, and the motion driving method is simple in implementation and high-efficiency in running.

Description

The motion driving method of nine-degree of freedom robot

Technical field

The present invention relates to the robot motion and control, the motion driving method of particularly a kind of nine-degree of freedom robot.

Background technology

Along with science and technology and the constant growth of productivity, robot is more extensive in the application of every field, and as in the workshop of big automobile production manufacturer, the application of robot is seen everywhere.As a rule, people need robot can accomplish specified action, like the welding in the automobile production process, thisly are used to study the subject how robot to move and are called as the robot kinematics.The robot kinematics can divide two types: forward and reverse.Direct kinematics problem to be solved is joint space Q (θ)=[θ of known machine people ₁, θ ₂..., θ _n], ask robot end position and attitude.Inverse kinematics problem to be solved is known machine robot end position and attitude, asks its joint space.Compare with direct kinematics, separating of inverse kinematics is also not exclusive, and its solution procedure is more complicated.

In the prior art, the method for solving of multiple inverse kinematics has been proposed, like sciagraphy, the analytic method of in list of references 1 " sophisticated machine people's control, Tan Min, education publishing house, in May, 2007 ", being mentioned.These methods (also are called as complete degree of freedom robot at existing 6DOF industrial robot; Joint in this robot is similar to waist joint, shoulder joint, elbow joint and the wrist joint of human body) in obtained application, industrial robot also depends on these methods just could accomplish mechanical action quickly and accurately.

Nowadays, except the Traditional industrial production field, the applied robot has also become possibility in photography at present.The robot that has camera is in some special screnes, and the scene very high like danger has a wide range of applications.And accomplish the shooting action by robot and compare advantages such as also there being steadily, being difficult for taking place shake with manual work.But consider photography industry practitioner; Like the director; The relevant knowledge that unlikely has robot control, the robot that therefore is applied to photography need provide simple, an easy-operating interface to the user, after the user imports limited information through this interface; Robot should accomplish specified action according to these information in official hour, realize predetermined attitude.For example; The director imports several the require key point of camera lens arrival and the times that video camera arrives these key points on a touch-screen; Robot should just can drive camera according to these information and realize corresponding track, and further generation will be accomplished the joint space of above-mentioned track.In addition, because the restriction of space, the 6DOF robot of the free degree can not satisfy the space requirement of shooting robot fully.For example: the operations such as unlimited rotation around camera lens can not realize through 6 frees degree; So behind the wrist joint of shooting robot, to have added 2 frees degree around rotation of self and special shooting demand needs in order satisfying in the shooting process, and before all joints, to have added a free sliding degree to increase the space of robot.So the robot that makes a video recording need adopt the robot of 9 frees degree.

Exactly because the These characteristics of the robot in the photography makes inverse kinematics method for solving of the prior art can't be applied directly to the photography machine philtrum of 9 frees degree.

Summary of the invention

The objective of the invention is to overcome the photography machine philtrum that existing inverse kinematics method for solving can't be applied directly to 9 frees degree, thereby a kind of driving method of robot motion fast and efficiently is provided.

To achieve these goals, the invention provides a kind of motion driving method of robot, said robot includes 9 frees degree; Linear joint in said robot has first free degree; Shoulder joint has second free degree, and elbow joint has Three Degree Of Freedom, and wrist joint has four-degree-of-freedom; Swivel of hand has the 5th, the 6th, seven freedom, and the rotation joint has the 8th and nine-degree of freedom; This method comprises:

Step 1), terminal the hand position on a plurality of points and the information of attitude in cartesian space of utilizing the said robot that the user provides; Realize the movement locus planning and attitude planning of said robot end's hand, obtain position and robot end hand the attitude in whole driving process of robot end's hand in whole driving process;

Step 2), the positional information of robot end's hand of being obtained by step 1) finds the solution and puts the joint space of said robot from first free degree to six degree of freedom sometime, the attitude information of the robot end's hand that is obtained by step 1) is found the solution the above robot joint space from seven freedom to nine-degree of freedom of said time point; The joint space of resulting nine frees degree is used for drive machines people's motion.

In the technique scheme, also comprise:

Step 3), utilize step 2) the joint space information of nine frees degree of resulting said robot on several time points does emulation, chooses preferred values as the control point the joint space information according to nine frees degree of simulation result from said several time points.

In the technique scheme, in described step 1), described movement locus planning comprises:

The terminal hand of the said robot that the user provides positional information on a plurality of points in cartesian space is done the curve match, obtains at least one path curves.

In the technique scheme, the B-spline curves method is adopted in described curve match, or the Bezier method, or the method that combines with Bezier of B-spline curves.

In the technique scheme, in described step 1), described attitude planning comprises:

Step a), with the consecutive points of two known attitude informations in the cartesian space respectively as original position and target location, the attitude information of said original position and target location is revised as the form of hypercomplex number by the form of Eulerian angles, use Q respectively ₁And Q ₂Expression;

Step b), ask for the attitude under any time, that is: Q between said original position and target location _n=Q ₁+ (Q ₂-Q ₁) * (t _n-t ₁);

Step c), the attitude that step b) is tried to achieve are changed back the Eulerian angles form.

In the technique scheme, described step 2) comprising:

Step 2-1), choose positional information and the attitude information on several time points in positional information from the resulting whole driving process of step 1) and the attitude information;

Step 2-2), with step 2-1) positional information on resulting said several time points computing formula of substitution gradient projection method respectively, obtain the joint space of said robot separately on these several times from first free degree to six degree of freedom;

Step 2-3), by step 2-2) joint space of said robot that calculate from first free degree to six degree of freedom; And step 2-1) attitude information on resulting said several time points calculates the adjustment amount of attitude, calculates on said several time points the joint space of said robot from seven freedom to nine-degree of freedom separately by the adjustment amount of attitude.

In the technique scheme, at described step 2-2) in, the computing formula of described gradient projection method is:

In the above-mentioned formula, The expression joint velocity, J is a Jacobian matrix, J ⁺Be the pseudo inverse matrix of J, Be said position to the first derivative of time,

Be the set of free vector, α represents amplification coefficient; Wherein,

β _W, β _D, β _L, β _RBe weight coefficient, they are value numbers in [0,1];

But W is the operational readiness function, $W=\sqrt{\mathrm{Det}\left({\mathrm{JJ}}^T\right)};$

D (θ) is for keeping away the barrier function, θ represents a certain attitude in the joint space, and i represents the numbering of the bounding box of capsule shape, and j represents the numbering of barrier, d ₀Represent the threshold value of safe distance, η is a coefficient;

L (θ) is the joint constraint function,

a _i=(θ _Imax+ θ _Imin)/2 are the intermediate value of each joint allowed band, θ _ImaxRepresent the value that i joint angles is maximum, θ _IminRepresent the value that i joint angles is minimum, n is for closing joint number;

is one 9 dimensional vector,

λ represents

means the gradient values.

In the technique scheme, described step 2-3) comprising:

Step 2-3-1), by step 2-2) the said robot that calculates calculates the attitude R in a certain moment to the joint space of six degree of freedom from first free degree ₁, the attitude that the attitude information that is generated by the planning of the attitude of step 1) obtains in the target location is R ₂, computing machine robot end hand is carved into the attitude R of the required adjustment in target location when a certain:

$R=R_1^T{R}_2$

Step 2-3-2), utilize rotation transformation to ask for equivalent rotating shaft;

$R\left(i\right)=\mathrm{Rot}(f,{\mathrm{\θ}}_t)$

$=\left[\begin{array}{cccc}{f}_x{f}_x{\mathrm{vers\θ}}_t+{\cos \mathrm{\θ}}_t& f_y{f}_x{\mathrm{vers\θ}}_t-f_z{\sin \mathrm{\θ}}_t& f_z{f}_x{\mathrm{vers\θ}}_t+f_y{\sin \mathrm{\θ}}_t& 0\\ f_y{f}_x{\mathrm{vers\θ}}_t+f_z{\sin \mathrm{\θ}}_t& f_y{f}_y{\mathrm{vers\θ}}_t+{\cos \mathrm{\θ}}_t& f_z{f}_y{\mathrm{vers\θ}}_t-f_x{\sin \mathrm{\θ}}_t& 0\\ f_z{f}_x{\mathrm{vers\θ}}_t-f_y{\sin \mathrm{\θ}}_t& f_y{f}_z{\mathrm{vers\θ}}_t+f_x{\sin \mathrm{\θ}}_t& f_z{f}_z{\mathrm{vers\θ}}_t+{\cos \mathrm{\θ}}_t& 0\\ 0& 0& 0& 1\end{array}\right]$

f _xf _yf _zBe the equivalent rotating shaft of cartesian space rotation transformation, corresponding robot end's hand the 7th, the 8th, nine-degree of freedom; θ _tBe attitude R under the time t ₁With R ₂Differential seat angle; Vers θ _t=1-cos θ _t

Step 2-3-3), ask for the adjustment amount of robot, the joint space of nine-degree of freedom promptly the 7th, the 8th, with respect to initial attitude by equivalent rotating shaft.

In the technique scheme, described step 3) comprises:

Step 3-1), utilize the speed of axle and the simulation result that acceleration is asked the PTP motion;

Step 3-2), ask the simulation result of PTP motion and the match similarity of aim curve, choose preferred values as the control point the joint space information according to nine frees degree of match similarity from said several time points.

In the technique scheme, described step 3-1) comprising:

Step 3-1-1), i the joint required acceleration maximum duration in the PTP motion that obtains robot according to the speed and the acceleration parameter of axle;

Wherein, t _AiThe acceleration deadline of representing i joint,

The maximal rate of the spindle motor of expression robot, The peak acceleration of the spindle motor of expression robot;

The distance of expression conversion,

(terminal point joint space θ _IEnd)-(starting point joint space θ _IStart)

Step 3-1-2), by whole the longest deadline of acceleration of the resulting object computer device of last step people, at the uniform velocity the longest deadline and the longest deadline of deceleration;

t _di＝t _ei-t _ai

maxT _a＝max(t _a1，t _a2，…，t _ai)

maxT _d＝max(t _d1，t _d2，…，t _di)

maxT _e＝max(t _e1，t _e2，…，t _ei)

Wherein, t _EiThe expression concluding time, t _DiThe time that expression is at the uniform velocity accomplished;

Step 3-1-3),, calculate each joint at unified acceleration, the at the uniform velocity speed of service under the situation and operation acceleration;

$A_i=\frac{V_i}{\max T_a}$

Wherein, V _iBe the speed of service of joint i, A _iIt is the operation acceleration of joint i;

Step 3-1-4), try to achieve PTP joint space in service expression formula θ in time by the aforementioned calculation result _t, obtain simulation result F in the substitution forward kinematics solution _Ptp(θ _{1sta rt}, θ _IEnd, t); Wherein,

${\mathrm{\&theta;}}_t=\left\{\begin{array}{cc}\frac{1}{2}{A}_i\&CenterDot;t^2& 0<t\&le;\max T_a\\ V_i\&CenterDot;t-\frac{1}{2}\frac{{V_i}^2}{A_i}& \max T_a<t\&le;\max T_d\\ V_i\&CenterDot;\max T_e-\frac{A_i}{2}\&CenterDot;{(\max T_e-t)}^2& \max T_d<t\&le;\max T_e\end{array}\right..$

In the technique scheme, described step 3-2) comprising:

Step 3-2-1), utilize autocorrelative principle to ask similarity degree

The value of

that step 3-2-2), will calculate and threshold value

compare; If less than; Think that then the match of this section curve is successful; Otherwise replenish a key point at aim curve intermediate point in time; Repeated execution of steps 3-1 after aim curve is divided into two sections), accomplish until the match of whole piece curve; In fit procedure the key point that will use be exactly said control point.

The invention has the advantages that:

Adopt robot motion's driving method of the present invention, the user only needs given robot end's hand position and attitude on several a spot of points in the space, just can realize the driving to robot, realizes that simply, operation is efficient.

Description of drawings

Fig. 1 is the sketch map of 9 frees degree of 9 degree of freedom robot involved in the present invention;

Fig. 2 is the flow chart of a robot motion's driving method among the embodiment;

Fig. 3 be 9 degree of freedom robot involved in the present invention on the rotation joint the 7th, eight, the sketch map of nine-degree of freedom;

Fig. 4 is a sketch map of keeping away the barrier structure in one embodiment;

Fig. 5 be in one embodiment the joint quicken, at the uniform velocity, the sketch map of moderating process.

The specific embodiment

The present invention will be described below in conjunction with accompanying drawing and the specific embodiment.

Before present invention is described, understand for ease, some related among the present invention notions are done unified explanation.

1,9 of robot frees degree: with reference to figure 1, at the linear joint of robot first free degree is arranged, shoulder joint has second free degree; Elbow joint has Three Degree Of Freedom; Wrist joint has four-degree-of-freedom, and swivel of hand has the 5th, the 6th, seven freedom, and the rotation joint has the 8th and nine-degree of freedom.

2, cartesian space: be meant the linear space of using the Cartesian coordinates definition.Use three-dimensional cartesian space to represent that broad sense attendes the space of robot among the application.

3, joint space: the supposition robot has n the free degree; The joint of robot coordinate system is a n-dimensional space so; The set that joint space is made up of a series of n-dimensional spaces, come to the same thing (being that each characteristic is identical with attitude through the robot end position that direct kinematics solves in the joint space) of the forward kinematics solution of each characteristic in this set.

4, gradient projection method: the fundamental formular of gradient projection method is:

Wherein,

Be the time-derivative of the angle that need try to achieve, J is a Jacobian matrix, J ⁺Be the pseudo inverse matrix of J, Be position and attitude vector to the first derivative of time,

Be the set of free vector (free vector is any vector that satisfies dimension), α represents a scalar factor.

Free vector

can be expressed as a vector about joint space θ, and free vector was differentiated on the time can obtain following formula:

Wherein,

The expression free vector

To the Grad of joint space, [*] ^TRepresent transposed matrix.

By top formula (1) and formula (2), the formula after being expanded (3):

5, attitude: refer to the terminal posture of robot towards.

6, direct kinematics: the joint coordinates in each joint of known machine people, each terminal pose of robot confirm thereupon, thisly is called direct kinematics by joint space to the mapping robot end's cartesian space.

After above-mentioned notion being done unified the description, the motion driving method to shooting of the present invention robot among the embodiment below describes.

In background technology, mention; The user need provide some basic information for the shooting robot; Like the several Key Points in the movement locus of camera lens, camera lens arrives the time of these key points, the time that camera lens pauses etc. on some key point.The information of described key point includes the position coordinates on these x in cartesian space, y, the z axle, and the attitude vector (Roll, Pitch, Yaw) of robot end on these aspects.The information of these key points in fact all is the position and the attitude of camera lens (the robot end of just making a video recording); Known in those skilled in the art; The motion of robot of realizing making a video recording drives, and all joints that need know the shooting robot are the information that changes of angle in time.Just be the basis hereinafter, how present embodiment realized the motion driving of shooting robot is described in conjunction with Fig. 2 with above-mentioned essential information.

Step 1, the movement locus planning that realizes camera lens and attitude planning.

Artificial 9 degree of freedom robot shown in Figure 1 of shooting machine that the application is related; During by the joint velocity of shooting robot end's position and 9 frees degree of Attitude Calculation; Need to calculate (9 * 6) matrix and corresponding with it (6 * 9) pseudo inverse matrix, the therefore computing time of meeting labor.With reference to figure 3; The dead in line of the seven freedom on eight degrees of freedom, nine-degree of freedom and the swivel of hand on the rotation joint of the shooting machine philtrum that the application is related on one point; Shine upon three direction of rotation of three-dimensional cartesian space respectively; Therefore the robot that makes a video recording only just can change attitude through said seven freedom, eight degrees of freedom and nine-degree of freedom, similarly, the position change of shooting robot also only with 9 frees degree in preceding six-freedom degree relevant.Based on shooting robot These characteristics structurally; Can the position of shooting robot be separated consideration with attitude in the present embodiment; The movement locus planning of camera lens of machine philtrum of being about to make a video recording is planned to separate with attitude and is found the solution; Can reduce the complexity that computing is found the solution like this, thereby improve computational efficiency, guarantee to accomplish in real time correlation computations.

Based on above-mentioned consideration, step 1 may further comprise the steps:

Step 1-1, user are actually the several discrete point in the cartesian space through several key points that relevant interface offers the shooting robot; Therefore at first need be transformed into the curve that the camera lens movement locus is represented in continuous being used to by the point that these are discrete, this transition process also is called as movement locus planning.

The planning of described movement locus can be adopted correlation technique of the prior art, and as adopting the method for B-spline curves, this method both can guarantee the continuity of moving to have equation solution advantages of simplicity and high efficiency advantage again; Perhaps adopt the method for Bezier, this method is used in computer graphics extensively, has the not available operation of B-spline curves method advantage intuitively.In the present embodiment,, can above-mentioned B-spline curves method and Bezier method be combined, obtain by the discrete some full curve that forms by the said discrete position coordinates of key point on x, y, z axle as a kind of preferred implementation.Mention, the discrete point that the user provides is in cartesian space before, and the full curve that is therefore generated by discrete point is in cartesian space.Owing to have many by the formed curve of some spots, so the possibility of result of movement locus planning has a plurality of.In sum, through movement locus planning, can generate many full curves that include said key point by the position coordinates of several discrete key points of user's input, these full curve available functions are represented.

Step 1-2, attitude planning is done by the shooting robot.

Mention before, the positional information that the information of the key point of user's input is mentioned, also include attitude information in step 1-1, these attitude informations are with (Roll, Pitch, Yaw) expression.But the number of the key point that the user imported is limited, and the attitude of the some points between two adjacent key points can not directly obtain.The purpose of attitude planning is exactly the attitude information that will calculate these intermediate points.

In the attitude planning process, with the time in the adjacent key point in preceding key point as original position, with the time after key point as the target location, robot uses in the attitude of original position and is R ₁Expression is used R in the attitude of target location ₂Expression.R ₁And R ₂In all include Roll, Pitch, Yaw component.Common practise according to this area; At first can the attitude information of original position and target location be rewritten into hypercomplex number (referring to 190 pages of lists of references 2 " 3D Math Primer for Graphics andGame Development "; Fletcher Dunn; Wordware Publishing, Inc), i.e. R ₁→ Q ₁, R ₂→ Q ₂, ask the Q under any time then _n=Q ₁+ (Q ₂-Q ₁) * (t _n-t ₁); At last with hypercomplex number Q _nChange Roll again into, Pitch, the Eulerian angles form of Yaw.

Through attitude planning, can obtain the attitude that any time is represented with (Roll, Pitch, Yaw) down in the whole driving process.

The result of step 1-3, the planning of preservation movement locus and attitude planning.

The result of resulting movement locus program results of step 1-1 before and step 1-2 and attitude planning is a continuous functions, does not need so many information in the robot driving process, as long as on said continuous function, get several discrete points.Because robot control all has minimum time

; Therefore; Can be divided into the integral multiple of

robot motion's the whole time; The function that is used for representing movement locus and attitude that 1-1 of substitution abovementioned steps then and step 1-2 are generated obtains position and the attitude of robot end on a plurality of discrete points.These information are preserved.In this step, the precision when the concrete value of the integral multiple of said

can be according to practical operation is confirmed.

Step 2, find the solution the shooting robot joint space.

Before mention, in order to calculate the convenience of finding the solution, the position of shooting robot is separated consideration with attitude.The generation of the joint space of preceding 6 frees degree of shooting robot is relevant with the positional information of shooting robot end hand, and the generation of the joint space of 3 frees degree then is relevant with the attitude information of shooting robot end hand.So explain respectively hereinafter.

Step 2-1, the joint space of finding the solution preceding 6 frees degree of shooting robot.

On the basis of the positional information of known machine robot end hand, through inverse robot in the kinematics solution joint space with the pairing array of these positional informations, the joint space of promptly preceding 6 frees degree.

Described inverse kinematics is found the solution must possess four constraints: 1) guarantee the robot inherently safe; 2) avoid collision; 3) avoid joint constraint; 4) avoid the joint unusual unusual with algorithm.

The kinematical equation of robot can be expressed as (can referring to 1, the 79 page of aforementioned reference):

X＝f(θ) (4)

In the above-mentioned formula, X ∈ R ^m, the position of expression robot in cartesian space (because the separating of position and attitude, therefore, mentioned herein to X be positional information, slightly different with the attitude and the position of X representative in the formula (1) before); θ ∈ R ⁿThe joint space of expression robot, n wherein refers to the number in joint, and m is the dimension of object vector, in this application; M refers to the cartesian space dimension, and it specifically comprises 6 dimensions (x, y, z; Roll.Pitch.Yaw), for shooting robot with redundant degree of freedom, m＜n.

To in addition conversion of top formula (4), the following formula of relation between the cartesian space speed

of the robot that can obtain being used to representing making a video recording and joint velocity

:

$\stackrel{\&CenterDot;}{X}=J\stackrel{\&CenterDot;}{\mathrm{\&theta;}}---\left(5\right)$

Jacobian matrix in the known above-mentioned formula

, J ∈ R ^{M * n}, in the speed of given shooting robot hand end in cartesian space

After (can the time first derivation be obtained), formula (5) is found the solution, can obtain the counter as follows of robot kinematics and separate through the positional information in cartesian space that generates by step 1:

The fundamental formular of the ladder sciagraphy that above-mentioned formula is mentioned before being exactly, i.e. formula (1), it is the general solution form of formula (5), J wherein ⁺Be the Moore-Penrose pseudoinverse of Jacobian matrix J, α ∈ R ^IBe a scalar factor,

Be the free vector that to choose arbitrarily.First on the right of this formula is the particular solution of formula (5), and second is homogeneous solution.On physical significance, first motion that defines robot hand, second has then defined the joint space autokinesis that does not influence hand exercise.During the joint space autokinesis was meant after confirming Function Mapping given desired value solution procedure, serial joint did not influence mapping result because redundant cause can change.

Can know that from the description of front the fundamental formular of ladder sciagraphy can be done further expansion, the formula after the expansion is shown in formula (3).

Observe the correlation formula of gradient projection method and can find out J, J ⁺,

I is known or can calculates through data with existing, therefore will calculate joint velocity

Key be how to ask for free vector With the scalar factor alphak that is called as amplification coefficient.

Find the solution free vector

When finding the solution free vector

, need consider four involved when aforementioned inverse kinematics is found the solution constraintss: guarantee the robot inherently safe; Avoid collision; Avoid joint constraint; Avoid the joint unusual unusual with algorithm.Can set up corresponding object function according to these constraintss, explain respectively below.

1, unusual and algorithm is unusual for fear of the joint, but the function of operational readiness introduced among the application.But operational readiness is Yoshikawa to be defined when the ability to work of research redundant degree of freedom robot, but operational readiness can represent with W, but the computing formula of operational readiness is following:

$W=\sqrt{\det \left({\mathrm{JJ}}^T\right)}---\left(6\right)$

The most directly relation of W and J can be expressed as the amassing of m singular value of the J that the singular value decomposition of utilizing J obtains

W＝σ ₁·σ ₂·…·σ _m (7)

σ in the formula ₁>=σ ₂>=...>=σ _m>=0 is the singular value of J matrix.By matrix theory; All m singular value of the J matrix of row full rank is all greater than zero; And must be promising in the singular value of the J matrix of contraction corresponding J matrix contraction during zero value .W=0, also promptly the robot of this moment to be in the joint unusual, this is in the motion planning of robot, least to hope to see.Want to avoid moving to the joint unusual position takes place, but must make operational readiness greater than zero.The W value is big more, and the joint is far away more from the unusual position of generation, and operability is just good more.Therefore introduce this relation in the motion control of redundant degree of freedom robot in a repulsion source can regarding null value as W, and this will help being avoided moving to the joint unusual position takes place.

2,, can set and keep away the barrier function for fear of collision problem.Consider robot be basically with the chain bar as its basic structure, therefore can use the structure of the simplest capsule shape as shown in Figure 4 to surround it in the present embodiment.When barrier to the interstructural distance of capsule shape of robot less than certain value after, can obtain following keep away barrier function D (θ)=d (θ, B):

$D_{\mathrm{ij}}\left(\mathrm{\&theta;}\right)=\left\{\begin{array}{c}\frac{1}{2}\mathrm{\&eta;}(\frac{1}{d_{\mathrm{ij}}\left(\mathrm{\&theta;}\right)}-\frac{1}{d_0}),d_{\mathrm{ij}}\left(\mathrm{\&theta;}\right)\&le;d_0\\ 0,d_{\mathrm{ij}}\left(\mathrm{\&theta;}\right)>d_0\end{array}\right.---\left(8\right)$

In the above-mentioned formula, θ represents a certain attitude in the joint space, and B represents barrier, and i represents the numbering of the bounding box of capsule shape, and j represents the numbering of barrier, d ₀Represent the threshold value of safe distance, η is a coefficient, and its value gets 1 usually.

3,, need the centre position that guarantees that the joint is retraining as far as possible for fear of the joint constraint problem.The method of optimizing joint motions is the centre position (θ range of motion _Imax+ θ _Imin)/2 are as benchmark, and structure joint constraint function is following:

$L\left(\mathrm{\&theta;}\right)=\frac{1}{n}{\mathrm{\&Sigma;}}_{i=1}^n{\left(\frac{\mathrm{\&theta;}{-a}_i}{a_i-{\mathrm{\&theta;}}_{i\max }}\right)}^2---\left(9\right)$

A in the formula _i=(θ _Imax+ θ _Imin)/2 are the intermediate value of each joint allowed band, θ _ImaxRepresent the value that i joint angles is maximum, θ _IminRepresent the value that i joint angles is minimum, n is for closing joint number.Optimization to range of motion is exactly to make above-mentioned joint constraint function L (θ _i) value minimum.

Mention before, also include the constraints of " guaranteeing the robot inherently safe " in four constraintss, this constraints also is resolved in fact when solving above-mentioned three constraintss.

Except top formula (6), formula (8), these three object functions of formula (9); Consider first free degree (because the slip that linear joint comes down on guide rail to be done on the linear joint of shooting robot; Therefore first free degree also can be called as the guide rail free degree or free sliding degree); To resolving very big influence; And the unit of free sliding degree and rotational freedom are not in an one magnitude, therefore here through object function

with it separately as a free vector.This free vector is only influential to guide rail.

After generating above-mentioned four object functions, with regard to how setting up a unified optimization index discuss below.

The autokinesis of formula 3 is separated with particular solution and will be guaranteed in an one magnitude; Four object functions physically will keep meaning consistent so; Could guarantee that like this four constraints can both be meaningful in the performance function, so each object function needs respectively a parameter to be formulated on the same physics rank.Because near unusual position took place in the joint, the operability of robot became very bad, so need pay attention to these zones as the forbidden zone.But operational readiness as the effect of keeping away the unusual optimization index of joint and algorithm not clearly, this is not because there is a higher value in W near " zero ".Therefore choose 1/W (θ) as the parameter of weighing the unusual influence to autokinesis in joint.Similarly, realize that with (1/D (θ)) objective optimization of keeping away barrier compares with other far better.Four motion optimizations with the potential energy field function representation with same trend, so just had identical physical significance.

In the potential energy field function, not only to consider the trend of potential energy field, also will consider the order of magnitude.For instance, but selected suitable operational readiness value W ₀(W=W ₀A near boundary value the singular position of corresponding joint), as W＞W ₀The time, robot is unusual away from the joint, there is no need the influence of considering that again the joint is unusual, promptly there is no need to adjust the robotic joint space autokinesis W (θ) is increased.If still the factor of increase is taken into account on the contrary, just might influence the realization that keeps away barrier or other motion optimization to W (θ).Keeping away the scope that the barrier problem also should the martial arts field of force simultaneously takes into account; Different with the former, the optimization of each range of motion then should require in motion planning all the time, because the value excursion of L (θ) is limited; Motion optimization influence to the former is little, therefore can set up the following unified index of optimizing

β wherein _W, β _D, β _L, β _RBe weight coefficient, they can be defined as the number of a value in [0,1] according to demand.

Find the solution amplification coefficient α

After unified free vector

tried to achieve, then consider amplification coefficient α.Amplification coefficient is directly controlled the path trace optimization effect.If little, optimize effect and can not embody, the result of appearance is that constraints is destroyed; If big, make the autokinesis result greater than elementary solution, cause erroneous results, do not satisfy inverse kinematics and separate.

The principle of choosing amplification coefficient is to guarantee that

reduces along with change of time, for selected optimization target function formula

because

Through choosing the trend that amplification coefficient α satisfies or influences

and change, order for this reason

Here λ＜0, then corresponding

in time T variation and reduce.The α that following formula is set up does

After trying to achieve free vector

and amplification coefficient α; Just can the aforesaid formula of they substitution formula (6) can be calculated the joint velocity

of preceding 6 (being preceding 6 frees degree) of shooting robot

After the joint velocity of trying to achieve preceding 6 frees degree; Can try to achieve the angle information in the concrete moment according to

, can be according to direct kinematics in the hope of robot end's position and attitude.

Step 2-2, ask the joint space of back 3 frees degree

Step 2-2-1, according to the attribute of joint of robot, the 7th, the 8th in the adjustment joint, nine-degree of freedom can not change the position, so need go to adjust attitude with these 3 frees degree.Behind the joint space that obtains preceding 6 frees degree, can solve shooting robot end's hand attitude R at a time in conjunction with direct kinematics ₁, and the attitude that the attitude information that is generated by the planning of the attitude of step 1 can obtain in the target location is R ₂, then robot end's hand is carved into the attitude R of the required adjustment in target location when a certain and is:

$R=R_1^T{R}_2---\left(11\right)$

Step 2-2-2, utilize rotation transformation to ask for equivalent rotating shaft.

$R\left(i\right)=\mathrm{Rot}(f,{\mathrm{\&theta;}}_t)=$

$\left[\begin{array}{cccc}{f}_x{f}_x{\mathrm{vers\&theta;}}_t+{\cos \mathrm{\&theta;}}_t& f_y{f}_x{\mathrm{vers\&theta;}}_t-f_z{\sin \mathrm{\&theta;}}_t& f_z{f}_x{\mathrm{vers\&theta;}}_t+f_y{\sin \mathrm{\&theta;}}_t& 0\\ f_y{f}_x{\mathrm{vers\&theta;}}_t+f_z{\sin \mathrm{\&theta;}}_t& f_y{f}_y{\mathrm{vers\&theta;}}_t+{\cos \mathrm{\&theta;}}_t& f_z{f}_y{\mathrm{vers\&theta;}}_t-f_x{\sin \mathrm{\&theta;}}_t& 0\\ f_z{f}_x{\mathrm{vers\&theta;}}_t-f_y{\sin \mathrm{\&theta;}}_t& f_y{f}_z{\mathrm{vers\&theta;}}_t+f_x{\sin \mathrm{\&theta;}}_t& f_z{f}_z{\mathrm{vers\&theta;}}_t+{\cos \mathrm{\&theta;}}_t& 0\\ 0& 0& 0& 1\end{array}\right]---\left(12\right)$

F=[f wherein _xf _yf _z] ^TBe the equivalent rotating shaft of general rotation transformation, corresponding robot end's hand the 7th, the 8th, nine-degree of freedom; θ _tBe attitude R under the time t ₁With R ₂Differential seat angle; Vers θ _t=1-cos θ _t

Because the θ in the formula (12) _tBe known, each trigonometric function is known, and attitude adjustment amount R can by before formula (11) calculate, therefore can calculate robot end's hand the 7th, the 8th, nine-degree of freedom equivalent rotating shaft f separately _x, f _y, f _z

Step 2-2-3, calculating f _x, f _y, f _zAfter, can ask for the adjustment amount of robot, promptly the 7th, the 8th, the joint space of nine-degree of freedom with respect to initial attitude.

Through above step, can obtain time dependent all information of complete joint space.The time dependent value of joint space θ that calculates is added in the data structure that step 1-3 produced.

Step 3, utilize the joint space information of nine frees degree to do emulation, with resulting curve after the emulation and different matched curve relatively, choose optimum value as the control point in the driving process.

Through step before, obtained the joint space of shooting robot, there have been these just can the drive machines people to move.Preamble is mentioned, and there is corresponding relation in joint space with some positions of terminal hand in cartesian space of shooting robot, and the some position of terminal hand in cartesian space of shooting robot is exactly the discrete point of being mentioned among the step 1-3.The number of these discrete points is many more, and is also just many more with the pairing joint space of these discrete points, and it is also just many more that institute will be transferred to the control information of shooting robot.In theory, multi-joint spatial information more, help more improving to the control accuracy of the shooting robot that will drive.But in the driving process of reality; Be used for drive machines people's device (like servomotor) startup, stop or rapid change all will have the process of an operation, be readily appreciated that, in the process of shooting robot joint velocity from static to a certain; Must experience first acceleration, more at the uniform velocity process.If but the control information of outside input is too frequent; The then last joint velocity no show still of robot; Will carry out new acceleration or moderating process; Joint motions are consumed in acceleration that does not stop or moderating process, and this makes the speed of service in each joint of robot can't reach actual value probably.

Just for the above reasons, therefore, need select the resulting many values of preceding step, guarantee the driving process accurately under the prerequisite, selected discrete point should lack as much as possible.

In the present embodiment, adopt emulation PTP motion realization to ask the process of robot motion's track by joint velocity.In this course, select the joint space θ that step 2 obtains, obtain robot and drive required minimum control point through simulation algorithm.

As those skilled in the art's common practise, in robot driving process, the joint interlock has three kinds of patterns: asynchronous same worker, synchronous same worker, complete synchronous same worker.In the present embodiment, emulation PTP process adopts fully synchronously with worker's pattern.

The PTP motion is through three phases, acceleration, constant velocity and deceleration.Wherein, t=0 to t=t _a(t _aRefer to the acceleration time) be boost phase, t=t _aTo t=t _d(t _dRefer to the at the uniform velocity time) be stage at the uniform velocity, at last to t=t _e(t _eRefer to the deadline of slowing down) be the decelerating phase.As shown in Figure 5, fully synchronously with worker's PTP motion, need to guarantee all joints of robot t when motion _A1=t _A2=...=t _Ai, t _D1=t _D2=...=t _Di, t _E1=t _E2=...=t _EiTo be the basis with worker's pattern fully synchronously, adopt the method for inserting key point by half to ask for the necessary control point in the present embodiment.Its concrete performing step is following:

Step 3-1, the speed of utilizing axle and acceleration are asked the simulation result F of PTP motion _Ptp(θ ₁, θ ₂, t)=X.

Step 3-1-1, i the joint required acceleration maximum duration in the PTP motion that obtains robot according to the speed and the acceleration parameter of axle;

Wherein, The maximal rate of

expression spindle motor, the peak acceleration of

expression spindle motor; The distance of

expression conversion,

Step 3-1-2, the longest deadline of acceleration, at the uniform velocity the longest deadline and the longest deadline of deceleration by the resulting object computer device of last step people integral body.

Specifically, at first, through formula (15) and (16) can calculate by robot i joint at the uniform velocity with the deadline of slowing down

t _di＝t _ei-t _ai (16)

Wherein, t _EiExpression is slowed down the deadline, t _DiThe time that expression is at the uniform velocity accomplished;

And then calculate the whole the longest deadline of acceleration of robot through formula, the at the uniform velocity the longest deadline with slow down the longest deadline

maxT _a＝max(t _a1，t _a2，…，t _ai)

maxT _d＝max(t _d1，t _d2，…，t _di) (17)

maxT _e＝max(t _e1，t _e2，…，t _ei)

Step 3-1-3, calculate each joint at unified acceleration, the at the uniform velocity speed of service under the situation and operation acceleration.

$A_i=\frac{V_i}{\max T_a}---\left(19\right)$

Wherein, V _iBe the speed of service of joint i, A _iIt is the operation acceleration of joint i.

Step 3-1-4, try to achieve PTP joint space in service expression formula θ in time by the aforementioned calculation result _t

${\mathrm{\&theta;}}_t=\left\{\begin{array}{cc}\frac{1}{2}{A}_i\&CenterDot;t^2& 0<t\&le;\max T_a\\ V_i\&CenterDot;t-\frac{1}{2}\frac{{V_i}^2}{A_i}& \max T_a<t\&le;\max T_d\\ V_i\&CenterDot;\max T_e-\frac{A_i}{2}\&CenterDot;{(\max T_e-t)}^2& \max T_d<t\&le;\max T_e\end{array}\right.---\left(20\right)$

With joint space θ _tObtain simulation result F in the substitution forward kinematics solution _Ptp(θ _1start, θ _IEnd, t).

Step 3-2, ask the degree of correlation of match

Utilize the corresponding time to go up movement locus in PTP motion and the step 1 and plan that the ratio of cartesian space length of cartesian space range difference and the aim curve of the aim curve (as: Bezier Bezieer) that obtains asks the degree of correlation of match.The computing formula of the match degree of correlation is following:

The value of

that above-mentioned formula calculates thinks that then the match of this section curve is successful under threshold value

.Otherwise replenish a key point at aim curve intermediate point in time.Aim curve is divided into two sections goes to ask the PTP match; Repeating step 3-1 then; Accomplish until the match of whole piece curve, as can not match then adjusting threshold value

It should be noted last that above embodiment is only unrestricted in order to technical scheme of the present invention to be described.Although the present invention is specified with reference to embodiment; Those of ordinary skill in the art is to be understood that; Technical scheme of the present invention is made amendment or is equal to replacement, do not break away from the spirit and the scope of technical scheme of the present invention, it all should be encompassed in the middle of the claim scope of the present invention.

Claims (11)

1. the motion driving method of a robot; Said robot includes 9 frees degree, at the linear joint of said robot first free degree is arranged, and shoulder joint has second free degree; Elbow joint has Three Degree Of Freedom; Wrist joint has four-degree-of-freedom, and swivel of hand has the 5th, the 6th, seven freedom, and the rotation joint has the 8th and nine-degree of freedom; This method comprises:

Step 1), terminal the hand position on a plurality of points and the information of attitude in cartesian space of utilizing the said robot that the user provides; Realize the movement locus planning and attitude planning of said robot end's hand, obtain position and robot end hand the attitude in whole driving process of robot end's hand in whole driving process;

Step 2), the positional information of robot end's hand of being obtained by step 1) finds the solution and puts the joint space of said robot from first free degree to six degree of freedom sometime, the attitude information of the robot end's hand that is obtained by step 1) is found the solution the above robot joint space from seven freedom to nine-degree of freedom of said time point; The joint space of resulting nine frees degree is used for drive machines people's motion.

2. the motion driving method of robot according to claim 1 is characterized in that, also comprises:

Step 3), utilize step 2) the joint space information of nine frees degree of resulting said robot on several time points does emulation, chooses preferred values as the control point the joint space information according to nine frees degree of simulation result from said several time points.

3. the motion driving method of robot according to claim 1 and 2 is characterized in that, in described step 1), described movement locus planning comprises:

The terminal hand of the said robot that the user provides positional information on a plurality of points in cartesian space is done the curve match, obtains at least one path curves.

4. the motion driving method of robot according to claim 3 is characterized in that, the B-spline curves method is adopted in described curve match, or the Bezier method, or the method that combines with Bezier of B-spline curves.

5. the motion driving method of robot according to claim 1 and 2 is characterized in that, in described step 1), described attitude planning comprises:

Step a), with the consecutive points of two known attitude informations in the cartesian space respectively as original position and target location, the attitude information of said original position and target location is revised as the form of hypercomplex number by the form of Eulerian angles, use Q respectively ₁And Q ₂Expression;

Step b), ask for the attitude under any time, that is: Q between said original position and target location _n=Q ₁+ (Q ₂-Q ₁) * (t _n-t ₁);

Step c), the attitude that step b) is tried to achieve are changed back the Eulerian angles form.

6. the motion driving method of robot according to claim 1 and 2 is characterized in that, described step 2) comprising:

Step 2-1), choose positional information and the attitude information on several time points in positional information from the resulting whole driving process of step 1) and the attitude information;

Step 2-2), with step 2-1) positional information on resulting said several time points computing formula of substitution gradient projection method respectively, obtain the joint space of said robot separately on these several times from first free degree to six degree of freedom;

Step 2-3), by step 2-2) joint space of said robot that calculate from first free degree to six degree of freedom; And step 2-1) attitude information on resulting said several time points calculates the adjustment amount of attitude, calculates on said several time points the joint space of said robot from seven freedom to nine-degree of freedom separately by the adjustment amount of attitude.

7. the motion driving method of robot according to claim 6 is characterized in that, at described step 2-2) in, the computing formula of described gradient projection method is:

In the above-mentioned formula,

The expression joint velocity, J is a Jacobian matrix, J ⁺Be the pseudo inverse matrix of J,

Be said position to the first derivative of time,

Be the set of free vector, α represents amplification coefficient; Wherein,

β _W, β _D, β _L, β _RBe weight coefficient, they are value numbers in [0,1];

But W is the operational readiness function, $W=\sqrt{\mathrm{Det}\left({\mathrm{JJ}}^T\right)};$

D (θ) is for keeping away the barrier function,

θ represents a certain attitude in the joint space, and i represents the numbering of the bounding box of capsule shape, and j represents the numbering of barrier, d ₀Represent the threshold value of safe distance, η is a coefficient;

L (θ) is the joint constraint function, a _i=(θ _Imax+ θ _Imin)/2 are the intermediate value of each joint allowed band, θ _ImaxRepresent the value that i joint angles is maximum, θ _IminRepresent the value that i joint angles is minimum, n is for closing joint number;

is one 9 dimensional vector,

λ represents

represents the gradient values.

8. the motion driving method of robot according to claim 6 is characterized in that, described step 2-3) comprising:

Step 2-3-1), by step 2-2) the said robot that calculates calculates the attitude R in a certain moment to the joint space of six degree of freedom from first free degree ₁, the attitude that the attitude information that is generated by the planning of the attitude of step 1) obtains in the target location is R ₂, computing machine robot end hand is carved into the attitude R of the required adjustment in target location when a certain:

$R=R_1^T{R}_2$

Step 2-3-2), utilize rotation transformation to ask for equivalent rotating shaft;

$R\left(i\right)=\mathrm{Rot}(f,{\mathrm{\&theta;}}_t)$

$=\left[\begin{array}{cccc}{f}_x{f}_x{\mathrm{vers\&theta;}}_t+{\cos \mathrm{\&theta;}}_t& f_y{f}_x{\mathrm{vers\&theta;}}_t-f_z{\sin \mathrm{\&theta;}}_t& f_z{f}_x{\mathrm{vers\&theta;}}_t+f_y{\sin \mathrm{\&theta;}}_t& 0\\ f_y{f}_x{\mathrm{vers\&theta;}}_t+f_z{\sin \mathrm{\&theta;}}_t& f_y{f}_y{\mathrm{vers\&theta;}}_t+{\cos \mathrm{\&theta;}}_t& f_z{f}_y{\mathrm{vers\&theta;}}_t-f_x{\sin \mathrm{\&theta;}}_t& 0\\ f_z{f}_x{\mathrm{vers\&theta;}}_t-f_y{\sin \mathrm{\&theta;}}_t& f_y{f}_z{\mathrm{vers\&theta;}}_t+f_x{\sin \mathrm{\&theta;}}_t& f_z{f}_z{\mathrm{vers\&theta;}}_t+{\cos \mathrm{\&theta;}}_t& 0\\ 0& 0& 0& 1\end{array}\right]$

f _xf _yf _zBe the equivalent rotating shaft of cartesian space rotation transformation, corresponding robot end's hand the 7th, the 8th, nine-degree of freedom; θ _tBe attitude R under the time t ₁With R ₂Differential seat angle; Vers θ _t=1-cos θ _t

Step 2-3-3), ask for the adjustment amount of robot, the joint space of nine-degree of freedom promptly the 7th, the 8th, with respect to initial attitude by equivalent rotating shaft.

9. the motion driving method of robot according to claim 2 is characterized in that, described step 3) comprises:

Step 3-1), utilize the speed of axle and the simulation result that acceleration is asked the PTP motion;

Step 3-2), ask the simulation result of PTP motion and the match similarity of aim curve, choose preferred values as the control point the joint space information according to nine frees degree of match similarity from said several time points.

10. the motion driving method of robot according to claim 9 is characterized in that, described step 3-1) comprising:

Step 3-1-1), i the joint required acceleration maximum duration in the PTP motion that obtains robot according to the speed and the acceleration parameter of axle;

Wherein, The maximal rate of

expression spindle motor, the peak acceleration of

expression spindle motor; The distance of

expression conversion,

Step 3-1-2), by whole the longest deadline of acceleration of the resulting object computer device of last step people, at the uniform velocity the longest deadline and the longest deadline of deceleration;

t _di＝t _ei-t _ai

maxT _a＝max(t _a1，t _a2，…，t _ai)

maxT _d＝max(t _d1，t _d2，…，t _di)

maxT _e＝max(t _e1，t _e2，…，t _ei)

Wherein, t _EiThe expression concluding time, t _DiThe time that expression is at the uniform velocity accomplished;

Step 3-1-3),, calculate each joint at unified acceleration, the at the uniform velocity speed of service under the situation and operation acceleration;

$A_i=\frac{V_i}{\max T_a}$

Wherein, V _iBe the speed of service of joint i, A _iIt is the operation acceleration of joint i;

Step 3-1-4), try to achieve PTP joint space in service expression formula θ in time by the aforementioned calculation result _t, obtain simulation result F in the substitution forward kinematics solution _Ptp(θ _{1sta rt}, θ _{IEnd, t}); Wherein,

${\mathrm{\&theta;}}_t=\left\{\begin{array}{cc}\frac{1}{2}{A}_i\&CenterDot;t^2& 0<t\&le;\max T_a\\ V_i\&CenterDot;t-\frac{1}{2}\frac{{V_i}^2}{A_i}& \max T_a<t\&le;\max T_d\\ V_i\&CenterDot;\max T_e-\frac{A_i}{2}\&CenterDot;{(\max T_e-t)}^2& \max T_d<t\&le;\max T_e\end{array}\right..$

11. the motion driving method of robot according to claim 10 is characterized in that, described step 3-2) comprising:

Step 3-2-1), utilize autocorrelative principle to ask similarity degree

The value of

that step 3-2-2), will calculate and threshold value

compare; If less than; Think that then the match of this section curve is successful; Otherwise replenish a key point at aim curve intermediate point in time; Repeated execution of steps 3-1 after aim curve is divided into two sections), accomplish until the match of whole piece curve; In fit procedure the key point that will use be exactly said control point.

Images