CN102554938B - 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 tracking method for mechanical arm tail end trajectory of robot

Technical field

The present invention relates to robot control field, particularly a kind of tracking method for mechanical arm tail end trajectory of robot.

Background technology

Robot is widely applied at field of industrial production, as utilizes robot to weld or the danger coefficient such as assembling is large, physical fitness requirement is higher work.Along with the Rapid Popularization of cultural industry, shooting robot also starts to obtain universal and application.Compared with traditional style of shooting, shooting robot has can repetition, high accuracy, high strength and the advantage such as high speed is controlled, and can replace people to complete 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 to study robot control and motion planning.How the motion planning of robot studies emphatically the movement locus of control, the path motion that Robot is specified.Because motion planning will make the actual motion track of robot as far as possible consistent with predefined movement locus, therefore motion planning is also referred to as track following.

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

For make track following can meet above-mentioned accurately, requirement stably, people have proposed the majorized function of various optimal trajectory, as shortest time, acceleration minimum, energy consume minimum etc., carry out trajectory planning by these majorized functions, thereby obtain the information such as displacement, speed and acceleration for describing optimal trajectory.More existing trace tracking methods, as at bibliography 1, " the kinematics Minimum Time Path of manipulator is planned, Ye Hua, Feng Chunbai, Southeast China University's journal, 1990, 20 (3) 74-79 " and bibliography 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 " in, only consider the kinematics characteristic of robot, track tracking problem has been done to excessive simplification, can not embody the dynamic characteristic of problem completely.And other method, as at bibliography 3 " Wang Jianbin, Ma Peisun, Xu Jun etc. based on the dynamic (dynamical) time optimal trajectory planning of hyper redundant manipulator. Shanghai Communications University's journal, 2002, 36 (9): 1360-1364 " and bibliography 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, adopt local search algorithm, choosing of its algorithm performance and primary condition is closely related, can not ensure to solve at every turn and can obtain desirable result.In addition, the general amount of calculation of existing trace tracking method is larger, and computational speed is slower, can not meet well the demand of commercial Application.

Compared 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 in (i.e. the tracking to camera motion track), is not required manipulator optimum running time (make manipulator within the shortest time along intended trajectory end of run), but require manipulator the time point of specifying arrive on track certain a bit.This feature also makes the existing trace tracking method that is applied to industrial robot can not be directly applied for the track following of shooting robot.

Summary of the invention

The object of the invention is to overcome the defect that existing trace tracking method amount of calculation is large, 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 tracking method for mechanical arm tail end trajectory of robot, comprising:

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

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

Step 3), by by step 2) function in the object function, the constraint equation that obtain is all converted to convex function, form the Optimized model of a convex programming, after the Optimized model of described convex programming is done to discretization, solve, obtain the amount of exercise in each joint of robot;

Step 4), by step 3) each joint motions amount driven machine people of robot of obtaining.

In technique scheme, described 1) comprising:

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

Step 1-2), end orbit in cartesian space that arc length parameters is represented is converted to the joint motions track that arc length parameters represents.

In technique scheme, described 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 represents arc length parameters, and L+1 represents the quantity of key point, p _lrepresent l key point, be illustrated in p _lon the value of arc length parameters s, represent to expect that robot end is arrived the time of l key point by l-1 key point, be respectively first derivative and the second dervative of s;

Described 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)\≤\stackrel{\‾}{b}\left(s\right)$

$\underset{\‾}{f}\left(s\right)\≤f\left(s\right)a\left(s\right)+h\left(s\right)b\left(s\right)\≤\stackrel{\‾}{f}\left(s\right)$

$\underset{\‾}{\mathrm{\τ}}\left(s\right)\≤\mathrm{\τ}\left(s\right)\≤\stackrel{\‾}{\mathrm{\τ}}\left(s\right)$

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

Wherein, τ is joint power, τ(s), represent respectively minimum, maximum that joint power allows; M is the mass matrix of positive definite; C is the matrix relevant to Coriolis force and centrifugal force; G represents gravity; f(s), represent respectively the maximum of angular speed square, minimum of a value and the maximum of joint angle acceleration; F (s), h (s) are two functions about s.

In technique scheme, in described step 3) in, described object function is converted to convex function and comprises:

Step 3-1-1), described object function is converted to an object function and a constraint equation:

$\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|$

Step 3-1-2), by step 3-1-1) object function that obtains combines with constraint equation and other existing constraint equation, the Optimized model of composition one convex programming.

In technique scheme, in described step 3) in, the Optimized model of described convex programming is done after discretization and solves and comprise:

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

Step 3-2-2), according to step 3-2-1) function relevant with arc length parameters s carries out discretization in K+1 discrete variable the obtaining Optimized model that is described convex programming, forms the Optimized model of the convex programming after discretization.

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

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

The invention has the advantages that:

1, computational speed is fast, efficiency is high.

2, simple to user's demand, user-friendly.

3, there is good autgmentability.

Brief description of the drawings

Fig. 1 is that user is the schematic diagram 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 schematic diagram of transition function;

Fig. 4 is the schematic diagram of piecewise linear function.

Detailed description of the invention

Below in conjunction with the drawings and specific embodiments, the present invention is illustrated.

Because the shooting industry practitioners such as director generally do not possess the professional knowledge of robot motion's control field, therefore user can only provide rough essential information conventionally, these information comprise that the predefined robot mechanical arm end of user is (for shooting robot, the end of its manipulator is video camera) running orbit, comprise: the key point that video camera will arrive in the time of motion, arrive the time of these key points, and the attitude of video camera on key point position etc.Before shooting, be the schematic diagram of the predefined movement locus of shooting robot as provided user in Fig. 1.Utilize these information can not directly control shooting robot according to predefined orbiting motion, what method of the present invention will be done is exactly the running orbit according to predefined robot mechanical arm, the acceleration in each joint of robot obtains making a video recording, in meeting the constraint of each joint and the key point constraint time of advent, make the movement locus of actual motion under the control of these joint acceleration as far as possible consistent with movement locus given in advance.This arm end that how to make robot moves along path given in advance with specific attitude, and the problem of specified point is exactly the track following problem of mentioning in the application in specific time point arrives path given in advance.Before the present invention is elaborated, first basic concepts involved in the present invention is described.

Convex programming: convex programming is that a class is fairly simple, and there is the Non-Linear Programming of some good natures.The feasible zone of convex programming is convex set, and its local minimum point is exactly global minimum point, and convex set of all formations of local minimum point.In the time that the object function of convex programming is strictly convex function, its global minimum point is 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 be referring to:

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

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

The related notion in the present invention being done on the basis of above-mentioned explanation, below in conjunction with Fig. 2, just describe for the related embodiment that realizes the inventive method.

Step 1), predefined user movement locus is represented by mathematical form.

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

How movement locus is expressed as to general knowledge known in this field with mathematical way, at bibliography 1: in " Xiao Shutie chief editor " algebraically and how much " (Higher Education Publishing House, 2003 July the 2nd edition) ", this is also described in detail.

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

In robot kinematics, to the mapping robot end's position and 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 attitude endways, and therefore by the predefined moving track calculation shooting of user robot, the movement locus in joint space belongs to inverse kinematics.Predefined user movement locus can be transformed into joint space from cartesian space according to inverse kinematics method well known in the prior art, thus the movement locus that obtains being represented by arc length parameters in joint space.

How utilizing inverse kinematics method to solve joint space movement locus can be referring to bibliography 2: " " 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 the structure of robot.

The free degree of supposing shooting robot is n; The joint angles of shooting robot is q, and joint angle speed is joint angle acceleration is (R ⁿrepresent n dimension real number space); Joint power is τ, τ ∈ R ⁿ; The kinetics equation of robot is expressed as follows:

$\mathrm{\τ}=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}for the mass matrix of positive definite, for the matrix relevant to Coriolis force and centrifugal force, F _s(q) ∈ R ^{n × n}for frictional force, G (q) represents gravity.In this application, frictional force is ignored.

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

Step 4), according to step 2) movement locus of the predefined robot mechanical arm end of user that obtains, and the Optimized model being made up of object function and constraint equation is set up in constraint and the constraint of each mechanics of the constraint of the motion initial point being obtained by the application scenarios of robot, motion distal point.In this step, the constraint of related mechanics comprises step 3) robot dynamics's equation of setting up, each joint velocity max min, acceleration max min, the max min of each joint drive power.

Step 5), by step 4) generate Optimized model be converted into convex programming, then it is solved, to obtain the amount of exercise in each joint of robot.

Because the conventional method for solving to the Optimized model being generated by object function and constraints is slower, and the solution of trying to achieve may not necessarily ensure global optimum, therefore, solves after needing that in this step described Optimized model is converted into convex programming again.

Step 6), using step 5) amount of exercise in each joint of obtaining is as input, actuated camera device people motion, can obtain the manipulator track that user requires.

Below to step 4) in particular content be described further.

Step 41), set up optimized object function.

Because the manipulator running orbit of shooting robot need to arrive given critical path point in the given time.So in optimizing process using the error of each key point time of advent as the object function of optimizing.When optimization, make object function optimization, just should make this object function as much as possible close to zero.Object function, close to zero, illustrates 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, therefore add these contents as the constraints of optimizing in Optimized model, obtain constraint equation.In addition, the motion of shooting robot need to meet kinetics equation, and therefore the kinetics equation of robot also will join in the middle of constraint equation.

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

$\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|---\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)\≤\stackrel{\‾}{b}\left(s\right)---\left(8\right)$

$\underset{\‾}{f}\left(s\right)\≤f\left(s\right)a\left(s\right)+h\left(s\right)b\left(s\right)\≤\stackrel{\‾}{f}\left(s\right)---\left(9\right)$

$\underset{\‾}{\mathrm{\τ}}\left(s\right)\≤\mathrm{\τ}\left(s\right)\≤\stackrel{\‾}{\mathrm{\τ}}\left(s\right)---\left(10\right)$

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

Wherein, s represents arc length parameters, the first derivative and the second dervative that are respectively s, L+1 represents the quantity of key point, p _lrepresent l key point, be illustrated in p _lon the value of arc length parameters s, represent to expect that robot end is arrived the time of l key point by l-1 key point, τ(s), represent respectively minimum, maximum that joint power allows. f(s), represent respectively the maximum of angular speed square, minimum of a value and the maximum of joint angle acceleration.F (s), h (s) are two functions about s.

In each mathematic(al) representation of above-mentioned Optimized model, the object function that mathematic(al) representation (2) is mentioned before representing; Mathematic(al) representation (3) is the robot dynamics's equation as constraints; Mathematic(al) representation (4), (5) are the constraint of velocity of robot initial point, distal point; Mathematic(al) representation (6), (7) can be by derivation obtains: be b (s)>=0.The constraint that mathematic(al) representation (8), (9) they are the shooting each joint velocity of robot and acceleration, and derivation is as follows:

The angular speed constraint in i joint is:

$-{\stackrel{\‾}{\stackrel{.}{q}}}_i\left(s\right)\≤{\stackrel{.}{q}}_i\left(s\right)\≤{\stackrel{\‾}{\stackrel{.}{q}}}_i\left(s\right)$

$\⇔{\left({\stackrel{\‾}{\stackrel{.}{q}}}_i\left(s\right)\right)}^2={\left(q_i^{\′}\left(s\right)\stackrel{.}{s}\right)}^2={\left(q_i^{\′}\left(s\right)\right)}^{2}b\left(s\right)\≤{\left({\stackrel{\‾}{\stackrel{.}{q}}}_i\left(s\right)\right)}^2$

$\⇔b\left(s\right)\≤\frac{{\left({\stackrel{\‾}{\stackrel{.}{q}}}_i\left(s\right)\right)}^2}{{\left(q_i^{\′}\left(s\right)\right)}^2}$

Order by the angular speed constraint writing vector form in each joint, just can obtain Mathematical representation (8).

The angular acceleration in i joint is constrained to:

${\underset{\‾}{\overset{..}{q}}}_i\left(s\right)\≤{\stackrel{..}{q}}_i\left(s\right)\≤{\stackrel{\‾}{\stackrel{..}{q}}}_i\left(s\right)$

$\⇔{\underset{\‾}{\overset{..}{q}}}_i\left(s\right)\≤q_i^{\′}\left(s\right)\stackrel{..}{s}+q_i^{\′\′}\left(s\right){\stackrel{.}{s}}^2\≤{\stackrel{\‾}{\stackrel{..}{q}}}_i\left(s\right)$

$\⇔{\underset{\‾}{\overset{..}{q}}}_i\left(s\right)\≤q_i^{\′}\left(s\right)a\left(s\right)+q_i^{\′\′}\left(s\right)b\left(s\right)\≤{\stackrel{\‾}{\stackrel{..}{q}}}_i\left(s\right)$

Order by the angular speed constraint writing vector form in each joint, just can obtain Mathematical representation (9).

In the above-mentioned derivation of equation, b ' (s), represent that respectively b (s) asks single order partial derivative and t is differentiated s; q _i(s), represent respectively angle, angular speed and the angular acceleration in i joint; represent minimum of a value, the maximum of i joint angle speed, represent respectively minimum of a value, the maximum of i joint angle acceleration; q _{i '}(s), q _{i "}(s) represent q _i(s) s is asked to single order, second-order partial differential coefficient.

Above-mentioned Optimized model is unconverted front Optimized model, is therefore labeled as Optimized model (I).

Because the object function in Optimized model (I) is non-convex function, so Optimized model (I) belongs to nonconvex programming.In view of the advantage of the convex programming mentioned before, in step 5) in need Optimized model to be converted into convex programming, then carry out discretization and obtain discrete Convex Programming Model.Below this step is described further.

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

$\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|$

Thereby obtain new Optimized model

$\underset{a\left(s\right),b\left(s\right),\mathrm{\τ}\left(s\right)}{\mathrm{MIN}}\underset{s_0}{\overset{s_L}{\∫}}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}}{\∫}}\frac{1}{\sqrt{b\left(s\right)}}\mathrm{ds}-T_l^E\≤0---\left(12\right)$

In above-mentioned formula, l gets respectively following numerical value: 1 ... L

$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)\≤\stackrel{\‾}{b}\left(s\right)$

$\underset{\‾}{f}\left(s\right)\≤f\left(s\right)a\left(s\right)+h\left(s\right)b\left(s\right)\≤\stackrel{\‾}{f}\left(s\right)$

$\underset{\‾}{\mathrm{\τ}}\left(s\right)\≤\mathrm{\τ}\left(s\right)\≤\stackrel{\‾}{\mathrm{\τ}}\left(s\right)$

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

Function in object function and constraint equation in Optimized model after above-mentioned conversion is all convex function, so above-mentioned Optimized model is convex programming.This Optimized model is designated as to Optimized model (II).

Step 52), Optimized model is converted into after convex programming, Optimized model (II) is carried out to discretization below.

First, be K section by the running orbit uniform discrete of shooting robot, obtain K+1 discrete variable.Mention, in the present embodiment, the running orbit of shooting robot represents with arc length parameters s, (the s for discrete variable that therefore generated before ₀, s ₁... s _k) represent.

Then, in the Optimized model (II) that is aforementioned convex programming according to the K+1 that obtained a before discrete variable, the function relevant with arc length parameters s carries out discretization, forms the Optimized model of the convex programming after discretization.

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

A), to object function use trapezoid formula to calculate definite integral, obtain this result of calculation is equivalent to

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

B), mention before, it represents the input signal for controlling shooting robot motion, thus suppose that it is jump function as shown in Figure 3, and for piecewise linear function as shown in Figure 4, therefore 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 Optimized model (II) in integration item at [s _k, s _k+1] can analytical Calculation obtain:

$\underset{s^k}{\overset{s^{k+1}}{\∫}}\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 Optimized model (II) discretization, obtain following convex programming:

$\underset{a^k,b^k,{\mathrm{\τ}}^k}{\mathrm{Min}}\underset{k=0}{\overset{K}{\mathrm{\Σ}}}\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\≤\stackrel{\‾}{b}\left(s^k\right)\mathrm{and}{b}^K\≤\stackrel{\‾}{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}\≤\stackrel{\‾}{f}\left(s^{k+1/2}\right)---\left(22\right)$

$\underset{\‾}{\mathrm{\τ}}\left(s^{k+1/2}\right)\≤{\mathrm{\τ}}^k\≤\stackrel{\‾}{\mathrm{\τ}}\left(s^{k+1/2}\right)---\left(23\right)$

In above-mentioned formula, k gets respectively following numerical value: 0 ... K-1

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

In above-mentioned formula, l gets respectively following numerical value: 0 ... L-1

In above-mentioned formula, x ^k(x can be a, b, τ, s), x ^k+1represent respectively the value of x at k, a k+1 discrete point, use x ^k+1/2represent the value of x at k, k+1 discrete point intermediate point, x ⁰, x ^krepresent the value of x at initial point, distal point place; k _lik _lefor , corresponding discrete series number.The above-mentioned Convex Programming Model through discretization is designated as Optimized model (III).

Step 53), the Optimized model of convex programming is solved.

Owing to being Convex Programming Model through the Optimized model of changing, therefore can adopt the general non-linear instrument that solves to solve to this model.In the present embodiment, above-mentioned Optimized model can be converted into second order cone planning, then use the solver realization that solves specially second order cone planning to solve more effectively and rapidly.But in other embodiments, also can adopt methods such as Subgradient sciagraphy, interior point method the Optimized model that is converted into Convex Programming Model is solved.Optimized model is solved to discrete speed b, acceleration a, the moment τ that obtained result is each joint.

In the above-mentioned embodiment about shooting robot, function in object function and constraint equation can be converted to convex function, therefore the Optimized model being formed by object function and constraint equation can be converted to Convex Programming Model, then utilize the characteristic of Convex Programming Model to carry out rapid solving.In other embodiments, method of the present invention also goes for the arm end track following of other types robot, as long as the constraint function in object function and the constraint equation of these robots also can be converted to convex function.

The invention has the advantages that:

1, the inventive method, in Optimization Solution process, is converted into Convex Programming Model by Optimized model, utilizes the feature of Convex Programming Model to realize effectively, solution procedure efficiently.

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

3, the inventive method has good autgmentability, can in Optimized 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 solved effectively efficiently.

Based on above-mentioned advantage, utilize the method disclosed in the present, can solve effectively rapidly and obtain acceleration in each joint space, thereby control each joint motions, obtain accurate tracking director given trace, and meet 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 had been described in detail with reference to embodiment, those of ordinary skill in the art is to be understood that, technical scheme of the present invention is modified or is equal to replacement, do not depart from the spirit and scope of technical solution of the present invention, it all should be encompassed in the middle of claim scope of the present invention.

Claims (6)

1. a tracking method for mechanical arm tail end trajectory for robot, comprising:

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

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

Described object function is:

Wherein, s represents arc length parameters, and L+1 represents the quantity of key point, P _lrepresent l key point, s _plbe illustrated in P _lon the value of arc length parameters s, represent to expect that robot end is arrived the time of l key point by l-1 key point, be respectively first derivative and the second dervative of s;

Described constraint equation is:

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

b'(s)＝2a(s)

b(s)≥0

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

Wherein, τ is joint power, represent respectively minimum, maximum that joint power allows; M is the mass matrix of positive definite; C is the matrix relevant to Coriolis force and centrifugal force; G represents gravity; represent respectively the maximum of angular speed square, minimum of a value and the maximum of joint angle acceleration; F (s), h (s) are two functions about s;

Step 3), by by step 2) function in the object function, the constraint equation that obtain is all converted to convex function, form the Optimized model of a convex programming, after the Optimized model of described convex programming is done to discretization, solve, obtain the amount of exercise in each joint of robot;

Step 4), by step 3) each joint motions amount driven machine people of robot of obtaining.

2. the tracking method for mechanical arm tail end trajectory of robot according to claim 1, is characterized in that, described step 1) comprising:

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

Step 1-2), end orbit in cartesian space that arc length parameters is represented is converted to the joint motions track that arc length parameters represents.

3. the tracking method for mechanical arm tail end trajectory of robot according to claim 1, is characterized in that, in described step 3) in, described object function is converted to convex function and comprises:

Step 3-1-1), described object function is converted to an object function and a constraint equation:

Step 3-1-2), by step 3-1-1) object function that obtains combines with constraint equation and other existing constraint equation, the Optimized model of composition one convex programming.

4. the tracking method for mechanical arm tail end trajectory of robot according to claim 3, is characterized in that, in described step 3) in, the Optimized model of described convex programming is done after discretization and solves and comprise:

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

Step 3-2-2), according to step 3-2-1) function relevant with arc length parameters s carries out discretization in K+1 discrete variable the obtaining Optimized model that is described convex programming, forms the Optimized model of the convex programming after discretization.

5. the tracking method for mechanical arm tail end trajectory of robot according to claim 4, it is characterized in that, in described step 3) in, adopt a kind of Optimized model to the convex programming after discretization in second order cone planning, Subgradient sciagraphy, interior point method to solve.

6. the tracking method for mechanical arm tail end trajectory of robot according to claim 1, is characterized in that, described amount of exercise comprises discrete speed, acceleration and the moment in joint.