CN102554938A - Tracking method for mechanical arm tail end trajectory of robot - Google Patents

Abstract

The invention provides a tracking method for a mechanical arm tail end trajectory of a robot, which comprises the following steps: the mechanical arm tail end moving track of a robot, which is defined by a user in advance, is expressed in a mathematic form; the moving track expressed in the mathematic form is positioned in a joint space; according to the kinetic equation of the robot, the application scene of the robot and the mechanical arm tail end moving trajectory of the robot, which is defined by the user in advance, a target function and a constraint equation are built; both the target function and the function in the constraint equation can be converted into convex functions; the target function and the function in the constraint function are both converted into the convex functions to form a convex programming optimization model; the convex programming optimization model is solved after being discretized to obtain the amount of exercise of each joint of the robot; and the robot is driven by the amount of exercise of each joint of the robot. The tracking method for a mechanical arm tail end track of a robot has the advantage of being high in calculation speed and easy to expand.

Description

The arm end trace tracking method of robot

Technical field

The present invention relates to robot control field, particularly a kind of arm end trace tracking method of robot.

Background technology

Robot has obtained extensive use in the commercial production field, as utilizes robot to weld or danger coefficient such as assembling is big, the higher work of physical fitness requirement.Along with the quick popularization of cultural industry, the shooting robot also begins to obtain to popularize and uses.Compare with traditional style of shooting, advantage such as the shooting robot has can repetition, high accuracy, high strength and high speed are controlled, and can replace the people to accomplish the shooting in dangerous scene.No matter robot application is in which kind of field, all require robot in environment high-quality, carry out the work according to the requirement of task efficiently and safely.This just need study with motion planning robot control.The motion planning of robot is studied movement locus how to control robot emphatically, and robot is moved along the path of regulation.Because motion planning will make the actual motion track of robot consistent as far as possible with predefined movement locus, so motion planning also is called as track following.

For industrial robot, the research contents of track following is mainly manipulator (being the robot end) track following.The manipulator track following is on robot kinematics and dynamic (dynamical) basis, robot motion's in joint space and cartesian space track is discussed and how is generated the method for these tracks.So-called track is meant displacement, speed and the acceleration of robot in motion process.The manipulator track following has great importance to operational efficiency, stationarity, operation accuracy and the energy consumption of manipulator.Generally, in track following, the desired trajectory of manipulator is the point in advance given a series of cartesian spaces, and constraints is maximal rate, acceleration and the moment that each joint motions of robot are allowed.The purpose of track following is exactly according to these constraints, calculates the smooth track that makes manipulator pass through these points accurately, reposefully.Wherein, refer to that accurately the robot movement track satisfies path constraint; And being meant that steadily smooth trajectory in the running, speed are continuous everywhere, it is not only the prerequisite that manipulator self keeps good running status, also usually is the requisite requirement of operation task to manipulator.

For make track following can satisfy above-mentioned accurately, requirement stably; People have proposed the majorized function of various optimal trajectory; As the time is the shortest, acceleration is minimum, energy consumes minimum etc.; Carry out trajectory planning through these majorized functions, thereby obtain being used to describe the information such as displacement, speed and acceleration of optimal trajectory.More existing trace tracking methods are as in list of references 1 " the kinematics Minimum Time Path planning of manipulator, Ye Hua, Feng Chunbai; Southeast China University's journal, 1990,20 (3) 74-79 " and list of references 2 " Choi Y.K.; Park J.H., Kim H.S., Kim J.H.; Optimal trajectory planning and sliding mode control for robots using evolution strategy, Robotica (2000), 18 (4): 423-428 "; Only consider the kinematics characteristic of robot, the track following problem has been done excessive simplification, can not embody the dynamic characteristic of problem fully.And the other method, as list of references 3 " Wang Jianbin, Ma Peisun; Xu Jun etc. based on the dynamic (dynamical) time optimal trajectory planning of hyper-redundant mechanical arm. Shanghai Communications University's journal; 2002,36 (9): 1360-1364 " and list of references 4 " Lee Y.D.Lee B.H.Kim H.G.An evolutionary approach for time optimal trajectory planning for a robotic manipulator.Information Sciences, 1999; 113:245-260 " in; Then adopted local search algorithm, choosing of its algorithm performance and primary condition is closely related, and can not guarantee to find the solution to obtain ideal results at every turn.In addition, the general amount of calculation of existing trace tracking method is bigger, and computational speed is slower, can not satisfy the demand of commercial Application well.

Compare with other industrial robot, the track following of shooting robot has special character.The Robot Path of robot camera is followed the tracks of and is not required manipulator running time optimum (promptly make manipulator in the shortest time along the intended trajectory end of run) in the tracking of camera motion track (promptly to), but require manipulator the time point of appointment arrive on the track certain a bit.These characteristics also make existing application can not be directly applied for the track following of shooting robot in the trace tracking method of industrial robot.

Summary of the invention

The objective of the invention is to overcome the defective that existing trace tracking method amount of calculation is big, computational speed is slow, thereby a kind of robot mechanical arm end orbit tracking of high speed is provided.

To achieve these goals, the invention provides a kind of arm end trace tracking method of robot, comprising:

Step 1), the arm end movement locus of the predefined robot of user is represented with mathematical form; This movement locus of representing with mathematical form is arranged in joint space;

Step 2), set up object function and constraint equation according to the predefined arm end movement locus of user in the application scenarios of the kinetics equation of robot, robot and the step 1); Function in said object function, the constraint equation can both be converted into convex function;

Step 3), will be by step 2) object function, the function in the constraint equation that obtain all convert convex function into, forms the Optimization Model of a convex programming, finds the solution after the Optimization Model of said convex programming is done discretization, obtains the amount of exercise in each joint of robot;

Step 4), each joint motions amount drive machines people of robot who obtains with step 3).

In the technique scheme, said 1) comprising:

Step 1-1), the arm end movement locus of the predefined robot of user is represented with the arc length parameters in the cartesian space:

Step 1-2), the end orbit in the cartesian space of arc length parameters being represented converts the joint motions track that arc length parameters is represented into.

In the technique scheme, said object function is:

$\underset{a\left(s\right),b\left(s\right),\mathrm{\τ}\left(s\right)}{\mathrm{MIN}}\underset{l=1}{\overset{L}{\mathrm{\Σ}}}\left|\right|\underset{s_{P_{l-1}}}{\overset{s_{P_l}}{\∫}}\frac{1}{\sqrt{b\left(s\right)}}\mathrm{ds}-T_l^E\left|\right|$

Wherein, s representes arc length parameters, and L+1 representes the quantity of key point, p _lRepresent l key point,

Be illustrated in p _lOn the value of arc length parameters s,

Expression expectation robot end arrives time of l key point by l-1 key point,

Be respectively first derivative and the second dervative of s;

Said constraint equation is:

τ(s)＝M(s)a(s)+C(s)b(s)+G(s)

$b\left(0\right)={\stackrel{.}{s}}_0^2$

$b\left(1\right)={\stackrel{.}{s}}_{\mathrm{<figure-callout\; id="2"\; label="T"\; filenames="HSA00000414933000032.png"\; state="\{\{state\}\}">T</figure-callout>}}^2$

b′(s)＝2a(s)

b(s)≥0

$b\left(s\right)\&le;\stackrel{\&OverBar;}{b}\left(s\right)$

$\underset{\&OverBar;}{f}\left(s\right)\&le;f\left(s\right)a\left(s\right)+h\left(s\right)b\left(s\right)\&le;\stackrel{\&OverBar;}{f}\left(s\right)$

$\underset{\&OverBar;}{\mathrm{\&tau;}}\left(s\right)\&le;\mathrm{\&tau;}\left(s\right)\&le;\stackrel{\&OverBar;}{\mathrm{\&tau;}}\left(s\right)$

The s span of above-mentioned formula is [0,1]

Wherein, τ is a joint power, τ(s), Represent minimum, maximum that joint power allows respectively; M is the mass matrix of positive definite; C is the matrix relevant with Coriolis force and centrifugal force; G represents gravity;

f(s),

Represent the maximum of angular speed square, the minimum of a value and the maximum of joint angle acceleration respectively; F (s), h (s) are two functions about s.

In the technique scheme, in described step 3), convert described object function into convex function and comprise:

Step 3-1-1), convert said object function into an object function and a constraint equation:

$\underset{a\left(s\right),b\left(s\right),\mathrm{\&tau;}\left(s\right)}{\mathrm{MIN}}\underset{l=1}{\overset{L}{\mathrm{\&Sigma;}}}\left|\right|\underset{s_{P_{l-1}}}{\overset{s_{P_l}}{\&Integral;}}\frac{1}{\sqrt{b\left(s\right)}}\mathrm{ds}-T_l^E\left|\right|$

Step 3-1-2), with step 3-1-1) resulting object function and constraint equation combine with other existing constraint equation, forms the Optimization Model of a convex programming.

In the technique scheme, in described step 3), the Optimization Model of said convex programming done to find the solution after the discretization comprise:

Step 3-2-1), arc length parameters s is carried out uniform sampling, obtain discrete variable (s ₀, s ₁... S _K);

Step 3-2-2), according to step 3-2-1) a resulting K+1 discrete variable is that function relevant with arc length parameters s in the Optimization Model of said convex programming carries out discretization, forms the Optimization Model of the convex programming after the discretization.

In the technique scheme, in described step 3), adopt a kind of Optimization Model in second order cone planning, Subgradient sciagraphy, the interior point method to find the solution to the convex programming after the discretization.

In the technique scheme, said amount of exercise comprises discrete speed, acceleration and the moment in joint.

The invention has the advantages that:

1, computational speed is fast, efficient is high.

2, the demand to the user is simple, and is user-friendly.

3, has good autgmentability.

Description of drawings

Fig. 1 is that the user is the sketch map of the predefined movement locus of shooting robot before shooting in one embodiment;

Fig. 2 is the flow chart of one embodiment of the present of invention;

Fig. 3 is the sketch map of transition function;

Fig. 4 is the sketch map of piecewise linear function.

The specific embodiment

Below in conjunction with the accompanying drawing and the specific embodiment the present invention is explained.

Because shooting industry practitioners such as director generally do not possess the professional knowledge that the robot motion controls the field; Therefore the user can only provide rough essential information usually; These information comprise the running orbit of the predefined robot mechanical arm of user terminal (for the shooting robot, the end of its manipulator is video camera), comprising: video camera when motion the key point that will arrive; Arrive the time of these key points, and the attitude of video camera on the key point position etc.As in Fig. 1, having provided the user is the sketch map of the predefined movement locus of shooting robot before shooting.Utilize these information can not directly control the shooting robot according to predefined orbiting motion; Method of the present invention what will do is exactly the running orbit according to predefined robot mechanical arm; The acceleration in each joint of robot obtains making a video recording; When satisfying each joint constraint and the key point constraint time of advent, make that the movement locus of actual motion is consistent as far as possible with given in advance movement locus under the control of these joint acceleration.This arm end of robot that how to make moves along given in advance path with specific attitude, and the track following problem that is exactly among the application to be mentioned in the problem that specific time point arrives specified point in the given path in advance.Before the present invention is elaborated, at first basic concepts involved among the present invention is described.

Convex programming: convex programming be one type simpler, and Non-Linear Programming with some good character.The feasible zone of convex programming is a convex set, and its local minimum point is exactly a global minimum point, and convex set of all formations of local minimum point.When the object function of convex programming was strictly convex function, its global minimum point was unique.

The canonical form of convex programming is:

$\underset{x}{\min }f\left(x\right)$

s.t.g _i(x)≤0，i＝1，…m

h _i(x)＝0，i＝1，…p

Wherein, f (x), g _i(x) be convex function, h _i(x) be affine function.

The related content of convex programming can referring to:

http://en.wikipedia.org/wiki/Convex_optimization。

Joint space and cartesian space: at the industrial machine philtrum, be useful on the joint coordinate system of describing joint motions, be useful on and describe the terminal position of robot and the terminal cartesian coordinate system of attitude.Suppose that robot has n the free degree, the joint of robot coordinate system is a n-dimensional space so, is referred to as joint space.Generally speaking, terminal cartesian coordinate system is described with three-dimensional translating and Three dimensional rotation, is referred to as cartesian space.

The related notion among the present invention being done on the basis of above-mentioned explanation,, realize that with regard to being used to the related embodiment of the inventive method describes below in conjunction with Fig. 2.

Step 1), the predefined movement locus of user is represented with mathematical form.

In general, be applicable to that the shooting robot of the inventive method can provide a personal-machine interactive interface to the user, like touch-screen, the user can draw needed movement locus on this Man Machine Interface.The movement locus that this interface receives is illustrated in the cartesian space, and the movement locus with these cartesian spaces in the present embodiment is converted into the mathematical form of representing with arc length parameters, like B-spline curves, and the Bei Saier curve.

How movement locus is expressed as general knowledge known in this field with mathematical way, at list of references 1: in " Xiao Shutie chief editor's " algebraically and how much " (Higher Education Publishing House, 2003 July the 2nd edition) " this also there is detailed description.

Step 2), calculate the movement locus of shooting robot in joint space according to the predefined movement locus of user.

In the robot kinematics, to robot end's position and the mapping the attitude, be called the direct kinematics of robot by the coordinate of joint of robot coordinate system.To the mapping the coordinate of joint of robot coordinate system, be called inverse kinematics by robot end's position and attitude.Obviously, the predefined movement locus of user is exactly shooting robot position and an attitude endways, therefore calculates the movement locus of shooting robot in joint space by the predefined movement locus of user and belongs to inverse kinematics.Can the predefined movement locus of user be transformed into joint space from cartesian space according to inverse kinematics method well known in the prior art, thereby obtain the movement locus in joint space, represented by arc length parameters.

How utilizing the inverse kinematics method to find the solution the joint space movement locus can be referring to list of references 2: " " the sophisticated machine people's control " that Tan Min etc. write (Higher Education Publishing House, 2007 May the 1st edition) ".

Step 3), set up the kinetics equation of this robot according to robot construction.

The free degree of supposing the shooting robot is n; The joint angles of shooting robot is q, and joint angle speed does

The joint angle acceleration does

(R ⁿExpression n dimension real number space); Joint power is τ, τ ∈ R ⁿThen the kinetics equation of robot is represented as follows:

$\mathrm{\&tau;}=M\left(q\right)\stackrel{..}{q}+C(q,\stackrel{.}{q}){\stackrel{..}{q}}^2+F_s\left(q\right)\mathrm{sgn}\left(\stackrel{.}{q}\right)+G\left(q\right)---\left(1\right)$

Wherein, M (q) ∈ R ^{N * n}Be the mass matrix of positive definite,

Be the matrix relevant, F with Coriolis force and centrifugal force _s(q) ∈ R ^{N * n}Be frictional force, G (q) represents gravity.In this application, frictional force is ignored.

The related implementation method of setting up robot dynamics's equation also is known in those skilled in the art in this step; At list of references 3: in " L.Sciavicco and B.Siciliano; Modeling and Control of Robot Manipulators.New York:McGraw-Hill, 1996 " detailed description is arranged.

Step 4), according to step 2) the terminal movement locus of the predefined robot mechanical arm of the user that obtains, and set up the Optimization Model of forming by object function and constraint equation by the constraint of the resulting motion initial point of the application scenarios of robot, the constraint and the constraint of each mechanics of motion distal point.Mechanics constraint related in this step comprises robot dynamics's equation that step 3) is set up, each joint velocity max min, acceleration max min, the max min of each joint driving force.

Step 5), the Optimization Model that step 4) is generated are converted into convex programming, then it are found the solution, to obtain the amount of exercise in each joint of robot.

Because the conventional method for solving to the Optimization Model that generated by object function and constraints is slower, and separating of trying to achieve may not necessarily guarantee global optimum, therefore, finds the solution after in this step, need said Optimization Model being converted into convex programming again.

The amount of exercise in step 6), each joint that step 5) is obtained is as input, and actuated camera device people motion can obtain the manipulator track of customer requirements.

Further specify in the face of the particular content in the step 4) down.

Step 41), set up optimized object function.

Because the manipulator running orbit of shooting robot need arrive given critical path point in the given time.So in optimizing process with the error of each key point time of advent as the object function of optimizing.During optimization, make the object function optimization just should make this object function approach zero as much as possible.Object function approaches zero, explains that running orbit conformed to expection through the time of each path key point.

Step 42), add constraints.

Each joint motions speed, acceleration constraint and each joint moment of considering the manipulator of shooting robot all have certain limit, so in Optimization Model, add these contents as the constraints of optimizing, obtain constraint equation.In addition, the motion of shooting robot need be satisfied kinetics equation, so the kinetics equation of robot also will join in the middle of the constraint equation.

According to step 41) and step 42) object function and the constraints set up, can obtain following Optimization Model, the mathematic(al) representation of described Optimization Model is following:

$\underset{a\left(s\right),b\left(s\right),\mathrm{\&tau;}\left(s\right)}{\mathrm{MIN}}\underset{l=1}{\overset{L}{\mathrm{\&Sigma;}}}\left|\right|\underset{s_{P_{l-1}}}{\overset{s_{P_l}}{\&Integral;}}\frac{1}{\sqrt{b\left(s\right)}}\mathrm{ds}-T_l^E\left|\right|---\left(2\right)$

τ(s)＝M(s)a(s)+C(s)b(s)+G(s) (3)

$b\left(0\right)={\stackrel{.}{s}}_0^2---\left(4\right)$

$b\left(1\right)={\stackrel{.}{s}}_T^2---\left(5\right)$

b′(s)＝2a(s) (6)

b(s)≥0 (7)

$b\left(s\right)\&le;\stackrel{\&OverBar;}{b}\left(s\right)---\left(8\right)$

$\underset{\&OverBar;}{f}\left(s\right)\&le;f\left(s\right)a\left(s\right)+h\left(s\right)b\left(s\right)\&le;\stackrel{\&OverBar;}{f}\left(s\right)---\left(9\right)$

$\underset{\&OverBar;}{\mathrm{\&tau;}}\left(s\right)\&le;\mathrm{\&tau;}\left(s\right)\&le;\stackrel{\&OverBar;}{\mathrm{\&tau;}}\left(s\right)---\left(10\right)$

The s span of above-mentioned formula is [0,1]

Wherein, s representes arc length parameters,

Be respectively first derivative and the second dervative of s, L+1 representes the quantity of key point, p _lRepresent l key point,

Be illustrated in p _lOn the value of arc length parameters s,

Expression expectation robot end arrives time of l key point by l-1 key point,

τ(s), Represent minimum, maximum that joint power allows respectively.

f(s),

Represent the maximum of angular speed square, the minimum of a value and the maximum of joint angle acceleration respectively.F (s), h (s) are two functions about s.

In each of the above-mentioned mathematical expression optimization model, the mathematical expression (2) represents the objective function previously mentioned; mathematical expression (3) is a constraint condition of the robot dynamic equation; mathematical expression (4), (5 ) for the robot initial point, the end point velocity constraint; mathematical expression (6), (7) can be deduced:

is that b (s) ≥ 0.Mathematic(al) representation (8), (9) are the constraint of shooting each joint velocity of robot and acceleration, and derivation is following:

The angular speed constraint in i joint is:

$-{\stackrel{\&OverBar;}{\stackrel{.}{q}}}_i\left(s\right)\&le;{\stackrel{.}{q}}_i\left(s\right)\&le;{\stackrel{\&OverBar;}{\stackrel{.}{q}}}_i\left(s\right)$

$\&DoubleLeftRightArrow;{\left({\stackrel{\&OverBar;}{\stackrel{.}{q}}}_i\left(s\right)\right)}^2={\left(q_i^{\&prime;}\left(s\right)\stackrel{.}{s}\right)}^2={\left(q_i^{\&prime;}\left(s\right)\right)}^{2}b\left(s\right)\&le;{\left({\stackrel{\&OverBar;}{\stackrel{.}{q}}}_i\left(s\right)\right)}^2$

$\&DoubleLeftRightArrow;b\left(s\right)\&le;\frac{{\left({\stackrel{\&OverBar;}{\stackrel{.}{q}}}_i\left(s\right)\right)}^2}{{\left(q_i^{\&prime;}\left(s\right)\right)}^2}$

Make

angular speed constraint in each joint is write vector form, just can obtain mathematical notation formula (8).

The angular acceleration in i joint is constrained to:

${\underset{\&OverBar;}{\overset{..}{q}}}_i\left(s\right)\&le;{\stackrel{..}{q}}_i\left(s\right)\&le;{\stackrel{\&OverBar;}{\stackrel{..}{q}}}_i\left(s\right)$

$\&DoubleLeftRightArrow;{\underset{\&OverBar;}{\overset{..}{q}}}_i\left(s\right)\&le;q_i^{\&prime;}\left(s\right)\stackrel{..}{s}+q_i^{\&prime;\&prime;}\left(s\right){\stackrel{.}{s}}^2\&le;{\stackrel{\&OverBar;}{\stackrel{..}{q}}}_i\left(s\right)$

$\&DoubleLeftRightArrow;{\underset{\&OverBar;}{\overset{..}{q}}}_i\left(s\right)\&le;q_i^{\&prime;}\left(s\right)a\left(s\right)+q_i^{\&prime;\&prime;}\left(s\right)b\left(s\right)\&le;{\stackrel{\&OverBar;}{\stackrel{..}{q}}}_i\left(s\right)$

Make angular speed constraint in each joint is write vector form, just can obtain mathematical notation formula (9).

In the above-mentioned derivation of equation, b ' (s),

Represent that respectively b (s) asks the single order partial derivative to s and t is differentiated; q _i(s),

Angle, angular speed and the angular acceleration of representing i joint respectively; Minimum of a value, the maximum of representing i joint angle speed,

Minimum of a value, the maximum of representing i joint angle acceleration respectively; q _{I '}(s), q _{I "}(s) expression q _i(s) s is asked single order, second-order partial differential coefficient.

Above-mentioned Optimization Model is unconverted preceding Optimization Model, therefore is labeled as Optimization Model (I).

Because the object function in the Optimization Model (I) is a non-convex function, so Optimization Model (I) belongs to nonconvex programming.Advantage in view of the convex programming of being mentioned before need be converted into convex programming with Optimization Model in step 5), carry out the Convex Programming Model that discretization obtains dispersing then.Further specify in the face of this step down.

Step 51), at first, the object function of above-mentioned optimization is replaced as follows

$\underset{a\left(s\right),b\left(s\right),\mathrm{\&tau;}\left(s\right)}{\mathrm{MIN}}\underset{l=1}{\overset{L}{\mathrm{\&Sigma;}}}\left|\right|\underset{s_{P_{l-1}}}{\overset{s_{P_l}}{\&Integral;}}\frac{1}{\sqrt{b\left(s\right)}}\mathrm{ds}-T_l^E\left|\right|$

Thereby obtain new Optimization Model

$\underset{a\left(s\right),b\left(s\right),\mathrm{\&tau;}\left(s\right)}{\mathrm{MIN}}\underset{s_0}{\overset{s_L}{\&Integral;}}b\left(s\right)\mathrm{ds}---\left(11\right)$

τ(s)＝M(s)a(s)+C(s)b(s)+G(s)

$\underset{s_{P_{l-1}}}{\overset{s_{P_l}}{\&Integral;}}\frac{1}{\sqrt{b\left(s\right)}}\mathrm{ds}-T_l^E\&le;0---\left(12\right)$

L gets following numerical value respectively in the above-mentioned formula: 1 ... L

$b\left(0\right)={\stackrel{.}{s}}_0^2$

$b\left(1\right)={\stackrel{.}{s}}_{\mathrm{<figure-callout\; id="2"\; label="T"\; filenames="HSA00000414933000032.png"\; state="\{\{state\}\}">T</figure-callout>}}^2$

b′(s)＝2a(s)

b(s)≥0

$b\left(s\right)\&le;\stackrel{\&OverBar;}{b}\left(s\right)$

$\underset{\&OverBar;}{f}\left(s\right)\&le;f\left(s\right)a\left(s\right)+h\left(s\right)b\left(s\right)\&le;\stackrel{\&OverBar;}{f}\left(s\right)$

$\underset{\&OverBar;}{\mathrm{\&tau;}}\left(s\right)\&le;\mathrm{\&tau;}\left(s\right)\&le;\stackrel{\&OverBar;}{\mathrm{\&tau;}}\left(s\right)$

The s span is [0,1] in the above-mentioned formula

Object function and the function in the constraint equation through in the Optimization Model after the above-mentioned conversion all are convex functions, so above-mentioned Optimization Model is a convex programming.This Optimization Model is designated as Optimization Model (II).

Step 52), Optimization Model is converted into convex programming after, carry out discretization in the face of Optimization Model (II) down.

At first, with the shooting robot the running orbit uniform discrete be the K section, obtain K+1 discrete variable.Mention, in the present embodiment, the running orbit of shooting robot representes that with arc length parameters s the discrete variable that is therefore generated is with (s before ₀, s ₁... S _K) expression.

Then, according to before a resulting K+1 discrete variable be that the function relevant with arc length parameters s carries out discretization in the Optimization Model (II) of aforementioned convex programming, the Optimization Model of the convex programming after the formation discretization.

Specifically, the function relevant with arc length parameters s being carried out discretization comprises:

A), use trapezoid formula to calculate definite integral, obtain

this result of calculation and be equivalent to

object function

Trapezoid formula calculates definite integral can be referring to document 4: " Guan Ye Lu Jin just compiles " foundation of numerical analysis " (Higher Education Publishing House, 1998 May the 1st edition) ".

B), mention before;

its expression is used to control shooting robot motion's input signal; So it is jump function as shown in Figure 3 for a hypothesis; And

is piecewise linear function as shown in Figure 4, so b (s) sectional is expressed as following linear function:

$b\left(s\right)=b^k+\left(\frac{b^{k+1}-b^k}{s^{k+1}-s^k}\right)(s-s^k)$

Further, constraint in the Optimization Model (II)

In integral at [s _k, s _K+1] but analytical Calculation gets:

$\underset{s^k}{\overset{s^{k+1}}{\&Integral;}}\frac{1}{\sqrt{b\left(s\right)}}\mathrm{ds}=\frac{2(s^{k+1}-s^k)}{\sqrt{b^{k+1}}+\sqrt{b^k}}$

Based on foregoing description, after Optimization Model (II) discretization, obtain following convex programming:

$\underset{a^k,b^k,{\mathrm{\&tau;}}^k}{\mathrm{Min}}\underset{k=0}{\overset{K}{\mathrm{\&Sigma;}}}\left(b^k\right)---\left(14\right)$

s.t.τ ^k＝m(s ^k)a ^k+c(s ^k)s ^k+g(s ^k) (15)

$b^0={\stackrel{.}{s}}_0^2---\left(16\right)$

$b^K={\stackrel{.}{s}}_k^2---\left(17\right)$

(b ^k+1-b ^k)＝2a ^k(s ^k+1-s ^k) (18)

b ^k≥0andb ^K≥0 (19)

$b^k\&le;\stackrel{\&OverBar;}{b}\left(s^k\right)\mathrm{and}{b}^K\&le;\stackrel{\&OverBar;}{b}\left(s^K\right)---\left(20\right)$

f(s ^k+1/2)≤f(s ^k+1/2)a ^k+h(s ^k+1/2)b ^k+1/2 (21)

$f\left(s^{k+1/2}\right)a^k+h\left(s^{k+1/2}\right)b^{k+1/2}\&le;\stackrel{\&OverBar;}{f}\left(s^{k+1/2}\right)---\left(22\right)$

$\underset{\&OverBar;}{\mathrm{\&tau;}}\left(s^{k+1/2}\right)\&le;{\mathrm{\&tau;}}^k\&le;\stackrel{\&OverBar;}{\mathrm{\&tau;}}\left(s^{k+1/2}\right)---\left(23\right)$

K gets following numerical value respectively in the above-mentioned formula: 0 ... K-1

$\underset{k=k_{\mathrm{li}}}{\overset{k_{\mathrm{le}}}{\mathrm{\&Sigma;}}}\frac{2(s^{k+1}-s^k)}{\sqrt{b^{k+1}}+\sqrt{b^k}}\&le;T_l^E---\left(24\right)$

L gets following numerical value respectively in the above-mentioned formula: 0 ... L-1

In the above-mentioned formula, x ^k(x can be a, b, and τ, s), x ^K+1Represent the value of x respectively, use x at k, a k+1 discrete point ^K+1/2Expression x is at the value of k, a k+1 discrete point intermediate point, x ⁰, x ^KExpression x is in the value at initial point, distal point place; k _LiK _LeFor

, Corresponding discrete series number.Above-mentioned Convex Programming Model through discretization is designated as Optimization Model (III).

Step 53), the Optimization Model of convex programming is found the solution.

Because the Optimization Model through conversion is a Convex Programming Model, therefore can adopt general nonlinear solution instrument to find the solution to this model.In the present embodiment, can above-mentioned Optimization Model be converted into second order cone planning, use the solver of finding the solution second order cone planning specially to realize finding the solution more effectively and rapidly then.But in other embodiments, also can adopt the Optimization Model that is converted into Convex Programming Model is found the solution such as methods such as Subgradient sciagraphy, interior point methods.Optimization Model is found the solution discrete speed b, acceleration a, the moment τ of resulting result for each joint.

In above-mentioned embodiment about the shooting robot; Function in object function and the constraint equation can convert convex function into; Therefore can utilize the characteristic of Convex Programming Model to carry out rapid solving then with converting Convex Programming Model into by object function and the formed Optimization Model of constraint equation.In other embodiments, method of the present invention also goes for the arm end track following of other types robot, as long as the object function and the constraint function in the constraint equation of these robots also can convert convex function into.

The invention has the advantages that:

1, the inventive method is converted into Convex Programming Model with Optimization Model in optimizing solution procedure, utilizes the characteristics of Convex Programming Model to realize effectively, solution procedure efficiently.

2, the inventive method is used simply, as long as the user provides a manipulator track can realize track following.

3, the inventive method has good autgmentability, can in Optimization Model, add other constraint function and object function.As long as constraint function and object function are convex functions, so new problem also can be found the solution effectively efficiently.

Based on above-mentioned advantage, utilize the disclosed method of the present invention, can find the solution effectively apace and obtain acceleration in each joint space, thereby control each joint motions, obtain accurate tracking director given trace, and satisfy each joint mechanics constraint.

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 (7)

1. the arm end trace tracking method of a robot comprises:

Step 1), the arm end movement locus of the predefined robot of user is represented with mathematical form; This movement locus of representing with mathematical form is arranged in joint space;

Step 2), set up object function and constraint equation according to the predefined arm end movement locus of user in the application scenarios of the kinetics equation of robot, robot and the step 1); Function in said object function, the constraint equation can both be converted into convex function;

Step 3), will be by step 2) object function, the function in the constraint equation that obtain all convert convex function into, forms the Optimization Model of a convex programming, finds the solution after the Optimization Model of said convex programming is done discretization, obtains the amount of exercise in each joint of robot;

Step 4), each joint motions amount drive machines people of robot who obtains with step 3).

2. the arm end trace tracking method of robot according to claim 1 is characterized in that, said 1) comprising:

Step 1-1), the arm end movement locus of the predefined robot of user is represented with the arc length parameters in the cartesian space:

Step 1-2), the end orbit in the cartesian space of arc length parameters being represented converts the joint motions track that arc length parameters is represented into.

3. the arm end trace tracking method of robot according to claim 1 is characterized in that, said object function is:

$\underset{a\left(s\right),b\left(s\right),\mathrm{\&tau;}\left(s\right)}{\mathrm{MIN}}\underset{l=1}{\overset{L}{\mathrm{\&Sigma;}}}\left|\right|\underset{s_{P_{l-1}}}{\overset{s_{P_l}}{\&Integral;}}\frac{1}{\sqrt{b\left(s\right)}}\mathrm{ds}-T_l^E\left|\right|$

Wherein, s representes arc length parameters, and L+1 representes the quantity of key point, p _lRepresent l key point, Be illustrated in p _lOn the value of arc length parameters s,

Expression expectation robot end arrives time of l key point by l-1 key point,

Be respectively first derivative and the second dervative of s;

Said constraint equation is:

τ(s)＝M(s)a(s)+C(s)b(s)+G(s)

$b\left(0\right)={\stackrel{.}{s}}_0^2$

$b\left(1\right)={\stackrel{.}{s}}_T^2$

b′(s)＝2a(s)

b(s)≥0

$b\left(s\right)\&le;\stackrel{\&OverBar;}{b}\left(s\right)$

$\underset{\&OverBar;}{f}\left(s\right)\&le;f\left(s\right)a\left(s\right)+h\left(s\right)b\left(s\right)\&le;\stackrel{\&OverBar;}{f}\left(s\right)$

$\underset{\&OverBar;}{\mathrm{\&tau;}}\left(s\right)\&le;\mathrm{\&tau;}\left(s\right)\&le;\stackrel{\&OverBar;}{\mathrm{\&tau;}}\left(s\right)$

The s span of above-mentioned formula is [0,1]

Wherein, τ is a joint power, τ(s),

Represent minimum, maximum that joint power allows respectively; M is the mass matrix of positive definite; C is the matrix relevant with Coriolis force and centrifugal force; G represents gravity;

f(s),

Represent the maximum of angular speed square, the minimum of a value and the maximum of joint angle acceleration respectively; F (s), h (s) are two functions about s.

4. the arm end trace tracking method of robot according to claim 3 is characterized in that, in described step 3), converts described object function into convex function and comprises:

Step 3-1-1), convert said object function into an object function and a constraint equation:

$\underset{a\left(s\right),b\left(s\right),\mathrm{\&tau;}\left(s\right)}{\mathrm{MIN}}\underset{l=1}{\overset{L}{\mathrm{\&Sigma;}}}\left|\right|\underset{s_{P_{l-1}}}{\overset{s_{P_l}}{\&Integral;}}\frac{1}{\sqrt{b\left(s\right)}}\mathrm{ds}-T_l^E\left|\right|$

Step 3-1-2), with step 3-1-1) resulting object function and constraint equation combine with other existing constraint equation, forms the Optimization Model of a convex programming.

5. the arm end trace tracking method of robot according to claim 4 is characterized in that, in described step 3), the Optimization Model of said convex programming done to find the solution after the discretization comprises:

Step 3-2-1), arc length parameters s is carried out uniform sampling, obtain discrete variable (s ₀, s ₁... S _K);

Step 3-2-2), according to step 3-2-1) a resulting K+1 discrete variable is that function relevant with arc length parameters s in the Optimization Model of said convex programming carries out discretization, forms the Optimization Model of the convex programming after the discretization.

6. the arm end trace tracking method of robot according to claim 5; It is characterized in that; In described step 3), adopt a kind of Optimization Model in second order cone planning, Subgradient sciagraphy, the interior point method to find the solution to the convex programming after the discretization.

7. the arm end trace tracking method of robot according to claim 1 is characterized in that, said amount of exercise comprises discrete speed, acceleration and the moment in joint.

Images