CN105511266A - Delta robot locus programming method based on gravitation searching particle swarm algorithm - Google Patents

Abstract

The invention provides a Delta robot locus programming method based on a gravitation searching particle swarm algorithm, comprising: employing a grid division mode to segment a Delta robot main conveyor belt operation area into M*N grids and determining a standard point coordinate; constructing a Cartesian space door-shaped grabbing locus, and deriving the kinematic inverse solution of each driving shaft angle and a rear end performer position; employing a gravitation searching particle swarm algorithm, using time optimum as a fitness function, and under the condition of meeting continuous and smooth joint speed, acceleration and pulsation and not exceeding a servo motor rated restriction value, constructing seven non-uniform rational B-spline functions, and obtaining an off line interpolation joint space angle-time node sequence; and after the Delta robot identifying a target point coordinate, calculating a joint space angle discrete sequence, querying data off-line stored in a three-dimensional array, obtaining a time node sequence, programming a motion locus and completing specified motion.

Description

Based on the Delta method for planning track of robot of gravitation search particle cluster algorithm

Technical field

The present invention relates to Delta robot trajectory planning field, specifically refer to a kind of Delta method for planning track of robot based on gravitation search particle cluster algorithm.

Background technology

In industry spot, only need the Origin And Destination of given robot motion, robot just can complete corresponding actions.For ensureing the compliance of action between robot terminal, reduce the wearing and tearing of physical construction, and quick acting under the prerequisite meeting servomotor indices, need to carry out trajectory planning to the course of action in each joint of robot.

At present, trajectory planning scheme, according to the difference of optimization aim, can be divided into the types such as energetic optimum, time optimal, minimal ripple.Wherein, the trajectory planning of time optimal and minimal ripple can meet industry spot to production efficiency and the demand in prolonged mechanical serviceable life, applies comparatively extensive.Time optimal trajectory planning mainly will be asked for timing node and be converted into non-linear optimization problem, current, a lot of solution is proposed both at home and abroad, as particle swarm optimization algorithm, Trust Region Algorithm, self-adaptation harmonic search algorithm and quadratic programming etc. for non-linear optimization problem.Due to above-mentioned optimized algorithm complicacy and present stage hardware computing velocity constraint, real-time becomes this algorithm application in the bottleneck of industrial robot.

Summary of the invention

The present invention in order to reach each joint start and stop friction when Delta robot captures diverse location object on main belt and working time the shortest object, provide a kind of Delta robot time optimal trajectory planning method based on gravitation search particle cluster algorithm of satisfied industrial requirement of real-time.

For reaching this object, the present invention is achieved through the following technical solutions:

A. adopt the mode of stress and strain model Delta robot main belt perform region to be divided into the square that M × N number of length of side is dcm (0 < d≤2), each square is called grid, and choosing grid element center point is standard point;

B. at cartesian space, to the standard point in described steps A and fixed placement point Q ₆adopt the mode of straight line and circular interpolation, obtain the position discrete series { Q of cartesian space "door" type track ₀, Q ₁, LQ ₆;

C. according to Delta robot architecture model, each main drive shaft angle θ is obtained ₁, θ ₂, θ ₃with end effector position E ₀inverse Kinematics Solution;

D. according to the position discrete series { Q of the Inverse Kinematics Solution in described step C by cartesian space "door" type track in described step B ₀, Q ₁, LQ ₆convert the angle discrete series { P of Delta robotic joint space to ₀, P ₁, LP ₆;

E. the timing node sequence { t of initialization w group joint space ₀, t ₁, Lt ₆; the timing node sequence often organizing joint space is consistent with the timing node sequence in cartesian space with standard point corresponding to the "door" type track of starting point; for simplified illustration, be below referred to as " timing node sequence ", and often organize interval { the Δ t of timing node sequence ₀, Δ t ₁, L Δ t ₅be random number within 1s;

F. by the angle discrete series { P of joint space in described step D ₀, P ₁, LP ₆with described step e in any one group of timing node sequence { t ₀, t ₁, Lt ₆corresponding, the angle-timing node sequence { P of composition joint space _i, t _i, i=0,1, L6, and the angle-timing node sequence constructing 7 these joint spaces of non-uniform rational B-spline function interpolation, control the motion in each joint;

G. construct time optimal fitness function, meet joint velocity, acceleration, pulsation continuously smooth and under being no more than the condition of the specified limits value of servomotor, adopt gravitation search particle cluster algorithm, to w group timing node sequence { t ₀, t ₁, Lt ₆off-line optimization, until meet maximum iteration time, export one group of optimal time sequence node;

H. adopt offline mode to M in described steps A × N number of standard point, repeat the above step B ~ G, obtain the optimal time sequence node { t of M × N group with standard point corresponding to the "door" type track of starting point ₀, t ₁, Lt ₆and be stored in three-dimensional array A and B;

I. industrial intelligent camera identifies the central point of object on main belt, and this point is called impact point, utilizes online query mode, one group of optimal time sequence node { t that to obtain with impact point place mesh standard point be starting point corresponding to "door" type track ₀, t ₁, Lt ₆, this impact point is adopted the method being similar to standard point simultaneously, repeat the quick planning robot's movement locus of the above step B ~ D, complete set motion.

The invention has the beneficial effects as follows: provide the Delta robot time optimal trajectory planning method based on gravitation search particle cluster algorithm.Adopt the mode of stress and strain model that Delta robot main belt perform region is divided into M × N number of grid and the point coordinate that settles the standard; Build cartesian space "door" type and capture track, derive the Inverse Kinematics Solution of each main drive shaft angle and end effector position; Adopt gravitation search particle cluster algorithm, take time optimal as fitness function, meeting joint velocity, acceleration, pulsation continuously smooth and under being no more than the condition of the specified limits value of servomotor, construct 7 non-uniform rational B-spline functions, obtain the angle-timing node sequence of off-line interpolation joint space; After Delta robot ONLINE RECOGNITION goes out coordinate of ground point, calculate the angle discrete series of joint space, the data of inquiry offline storage in three-dimensional array, obtain timing node sequence, programming movement track, completes set motion.

Accompanying drawing explanation

Accompanying drawing is used to provide a further understanding of the present invention, and forms a part for instructions, together with embodiments of the present invention for explaining the present invention, is not construed as limiting the invention.In the accompanying drawings:

Fig. 1 is off-line algorithm flow process;

Fig. 2 is on-line Algorithm flow process;

Fig. 3 is gridding main belt perform region;

Fig. 4 is that cartesian space "door" type captures track;

Fig. 5 is Delta robot kinematics model;

Fig. 6 is standard point in grid and impact point.

Embodiment

For making the object, technical solutions and advantages of the present invention clearly understand, below in conjunction with specific embodiment, and with reference to accompanying drawing, the present invention is described in further detail.

Basic object of the present invention is with the artificial platform of Delta machine, carries out trajectory planning to the action capturing diverse location object on main belt, under the condition meeting the specified limits value of robot servo motors, and hunting time optimal motion track.Whole algorithm flow is mainly divided into target localization, off-line optimization goes out optimal time sequence node, online query optimal time sequence node, as shown in Figure 1 and Figure 2.

Further, specific implementation step is:

A. adopt the mode of stress and strain model that Delta robot main belt perform region is divided into M × N number of square, each square is called grid, and its length of side is that the numerical value of dcm, d is not more than 2 and generally gets 1 or 2, the value of M is generally even number, and choosing grid element center point is standard point.As Fig. 3, M ' × N number of grid is distributed in first quartile, remaining M ' × N number of grid is distributed in the second quadrant, and they are symmetrical about Y-axis, wherein M '=M/2.

B. at cartesian space, to the standard point in steps A and fixed placement point Q ₆by straight line and circular interpolation, to generate in cartesian space 7 discrete position sequences to build "door" type crawl path, as shown in Figure 4.7 discrete position sequence coordinates are:

Q ₀＝(x ₀,y ₀,z ₀),

Q ₁＝(x ₀,y ₀,z ₀+H),

$Q_2=(\frac{R\&CenterDot;(x_6-x_0)}{\sqrt{{(x_6-x_0)}^2+{(y_6-y_0)}^2}}+x_0,\frac{R\&CenterDot;(y_6-y_0)}{\sqrt{{(x_6-x_0)}^2+{(y_6-y_0)}^2}}+y_0,z_0+H+R),$

$Q_3=(\frac{x_6+x_0}{2},\frac{y_6+y_0}{2},z_0+H+R),$

$Q_4=(x_6-\frac{R\&CenterDot;(x_6-x_0)}{\sqrt{{(x_6-x_0)}^2+{(y_6-y_0)}^2}},y_6-\frac{R\&CenterDot;(y_6-y_0)}{\sqrt{{(x_6-x_0)}^2+{(y_6-y_0)}^2}},z_0+H+R),$

Q ₅＝(x ₆,y ₆,z ₆+H),

Q ₆＝(x ₆,y ₆,z ₆)(1)

In formula, Q ₀(x ₀, y ₀, z ₀) be standard point, Q ₆(x ₆, y ₆, z ₆) be fixed placement point, position hoisting depth is Hcm, and numerical value can according to the requirement setting of Delta robot actual crawl object; Circular interpolation radius is Rcm, and numerical value can according to standard point Q ₀to fixed placement point Q ₆distance finely tune, to eliminate because interpolation point in cartesian space chooses the phenomenon that distortion appears in the very few and "door" type arc locus transition portion that causes, the large I of R value is determined according to formula (2):

$R=\left\{\begin{array}{cc}{R}_{\max }& (R_{\max }<0.1g|Q_0{Q}_6\left|\right)\\ 0.1g\left|Q_0{Q}_6\right|& (R_{\min }<0.1g|Q_0{Q}_6|\&le;R_{\max })\\ R_{\min }& \left(0.1g\right|Q_0{Q}_6|\&le;R_{\min })\end{array}\right.---\left(2\right)$

In formula, for cartesian space during current grasping movement is got the bid Q on schedule ₀with fixed placement point Q ₆between line segment length, R _maxwith R _minbe respectively the maxima and minima of arc transition radius R, setting value is:

$\left\{\begin{array}{c}{R}_{max}=0.1g(\frac{3}{4}{L}_{max}+\frac{1}{4}{L}_{\min })\\ R_{\min }=0.1g(\frac{3}{4}{L}_{max}+\frac{1}{4}{L}_{\min })\end{array}\right.---\left(3\right)$

In formula, L _maxwith L _minbe respectively fixed placement point Q ₆the nose segment length formed with M on main belt × N number of mesh standard point and line of shortest length segment length, numerical value can obtain according to Delta robot work region off-line measurement.

C. Fig. 5 is the structural model of Delta robot, sets up basis coordinates system O-XYZ and solve coordinate system O-X in the center of Delta robot stationary platform _iy _iz, considers that Delta robot three side chains are identical, the mode around Z axis rotating coordinate system can be adopted to simplify and solve, obtain each main drive shaft angle θ of Delta robot ₁, θ ₂, θ ₃with end effector position E ₀inverse Kinematics Solution:

${\mathrm{\&theta;}}_i=\arctan \left(\frac{z_{J_i}}{y_{J_i}+\frac{f}{2\sqrt{3}}}\right),i=1,2,3---\left(4\right)$

Wherein,

$a=x_{i0}^2+{(y_{i0}-\frac{e}{2\sqrt{3}})}^2+z_{i0}^2+r_f^2-r_e^2-\frac{f^2}{12},$

$b=\frac{e-f-2\sqrt{3}\&CenterDot;y_{i0}}{2\sqrt{3}\&CenterDot;z_{i0}},$

$c={(ab+\frac{f}{2\sqrt{3}})}^2+(b^2+1)(\frac{f^2}{12}+a^2-r_f^2),$

$y_{J_i}=\frac{-\frac{f}{2\sqrt{3}}-ab-\sqrt{c}}{b^2+1},$

$z_{J_i}=a+b\&CenterDot;y_{J_i},---\left(5\right)$

Setting θ _ibe rotated down as positive dirction, rotate up as negative direction, a, b, c are intermediate variable, e=A ' B '=

B ' C '=C ' A ' is the leg-of-mutton length of side of mobile platform, and f=AB=BC=CA is the leg-of-mutton length of side of stationary platform, r _e=J ₁e ₁for the length of slave arm, r _f=F ₁j ₁for the length of master arm, be the master arm of i-th side chain and the intersection point J of slave arm _iat solving model coordinate system O-X _iy _ithe Y-axis coordinate of Z and Z axis coordinate, E ₀(x ₀, y ₀, z ₀) be the coordinate of end effector of robot in basis coordinates system, E ₁₀(x ₁₀, y ₁₀, z ₁₀), E ₂₀(x ₂₀, y ₂₀, z ₂₀), E ₃₀(x ₃₀, y ₃₀, z ₃₀) be respectively i=1,2,3 time point E ₀(x ₀, y ₀, z ₀) solving coordinate system O-X _iy _icoordinate under Z, is specially:

D. according to the position discrete series { Q of the Inverse Kinematics Solution in step C by cartesian space "door" type track in step B ₀, Q ₁, LQ ₆convert the angle discrete series { P of Delta robotic joint space to ₀, P ₁, LP ₆;

E. the timing node sequence { t of initialization w group joint space ₀, t ₁, Lt ₆; the timing node sequence often organizing joint space is consistent with the timing node sequence in cartesian space with standard point corresponding to the "door" type track of starting point, is simplified illustration, is below referred to as " timing node sequence "; meanwhile, the interval of delta t of timing node sequence is often organized in definition _ifor:

Δt _i＝t _i+1-t _i(i＝0,1,L,5)(7)

In formula, Δ t _ifor servomotor is by angle P _iturn to P _i+1the time interval, and Δ t _ibe the random number within 1s; t _ifor servomotor turns to P _icorresponding moment and t _ifor non-decreasing series.

F. by the angle discrete series { P of the joint space in step D ₀, P ₁, LP ₆with step e in any one group of timing node sequence { t ₀, t ₁, Lt ₆corresponding, the angle-timing node sequence { P of composition joint space _i, t _i, i=0,1, L6; Construct the angle-timing node sequence of 7 these joint spaces of non-uniform rational B-spline function interpolation, control the forms of motion in each joint.Describedly to be specially:

For the angle-timing node sequence { P of joint space _i, t _i, i=0,1, L6, wherein P _i=[P _1i, P _2i, P _3i] ^t, the angle in the joint 1,2,3 of corresponding i-th discrete series respectively.Pulsation continuous path requires each joint trajectories curvilinear function P _m=f _m(t) at least three rank geometry continuum, and track crosses the angle of joint space-timing node sequence, i.e. P _mi=f _m(t _i), m=1,2,3 correspond to 3 joints of Delta robot.

K time non-homogeneous B spline curve Unify legislation is:

$P\left(u\right)=\underset{j=0}{\overset{n}{\&Sigma;}}{d}_j{N}_{j,k}\left(u\right)=\underset{j=i-k}{\overset{i}{\&Sigma;}}{d}_j{N}_{j,k}\left(u\right)---\left(8\right)$

In formula, d _j∈ R ^{3 × 1}for control vertex vector, j=0,1, Ln; for knot vector; P (u) ∈ R ^{3 × 1}for the joint angles vector that the u moment is located; N _j,ku basis function that () is k non-uniform rational B-spline, and:

The r order derivative P of k non-homogeneous B spline curve ^r(u), r=1,2,3, can be calculated by De Buer recursion formula:

$P^r\left(u\right)=\underset{j=i-k+r}{\overset{i}{\&Sigma;}}{d}_j^r{N}_{j,k-r}\left(u\right)---\left(10\right)$

$d_j^l=\left\{\begin{array}{cc}{d}_j,& l=0\\ (k+1-l)\frac{d_j^{l-1}-d_{j-1}^{l-1}}{u_{j+k+1-l}-u_j},& \begin{array}{c}l=1,2,L,r\\ j=i-k+l,L,i\end{array}\end{array}\right.---\left(11\right)$

According to the angle-timing node sequence of joint trajectories through joint space, reverse 7 non-homogeneous B spline curve control vertex sequences.Adopt accumulative Chord Length Parameterization method to timing node sequence { t ₀, t ₁, Lt ₆normalization, determine knot vector u ∈ [u ₀, u ₁, L, u _{6+2 × 7}]:

u ₀＝u ₁＝L＝u ₇,

$u_{i+7}=u_{i+6}+\frac{\left|{\mathrm{\&Delta;t}}_{i-1}\right|}{\underset{j=0}{\overset{5}{\&Sigma;}}\left|{\mathrm{\&Delta;t}}_i\right|},i=1,L,6,$

u ₆₊₇＝u ₆₊₇₊₁＝L＝u _6+2×7.(12)

7 equations that the angle-timing node sequence constraints of joint space is corresponding can be listed:

$P\left(u_{i+7}\right)=\underset{j=i}{\overset{i+7}{\&Sigma;}}{d}_j{N}_{j,7}\left(u_{i+7}\right)=P_i---\left(13\right)$

In formula, i=0,1, L, 6, d _jfor 13 control vertex vectors of B-spline geometric locus, j=0,1, L, 12.Therefore, also need to increase by 6 equations by boundary condition.The numerical value of setting joint start and stop speed, acceleration and pulsation, obtains 13 equations:

$\left\{\begin{array}{c}P\left(u_{i+7}\right)=\underset{j=i}{\overset{i+7}{\&Sigma;}}{d}_j{N}_{j,7}\left(u_{i+7}\right)=P_i,i=0,1,L6\\ P^{\&prime;}\left(u_{i+7}\right)=v_s,P^{\&prime;}\left(u_{n+7}\right)=v_e\\ P^{\&prime;\&prime;}\left(u_{i+7}\right)=a_s,P^{\&prime;\&prime;}\left(u_{n+7}\right)=a_e\\ P^{\&prime;\&prime;\&prime;}\left(u_{i+7}\right)=j_s,P^{\&prime;\&prime;\&prime;}\left(u_{n+7}\right)=j_e\end{array}\right.---\left(14\right)$

V in formula _s, a _s, j _srepresent joint starting velocity, acceleration and pulsation vector respectively, v _e, a _e, j _ebe respectively joint and stop speed, acceleration, pulsation vector, P ' (u), P " (u), P " ' (u) is respectively joint velocity, acceleration, pulsation geometric locus.

Above formula is described as the form of matrix equation:

C _md _m＝P _m(15)

Wherein, C _m∈ R ^{13 × 13}for matrix of coefficients and d _m=[d _m0, d _m1, L, d _m12] ^t, P _m=[P _m0, P _m1, L, P _m6, v _ms, v _me, a _ms, a _me, j _ms, j _me] ^t, the control vertex vector of 7 non-uniform rational B-spline tracks can be obtained thus:

d _m＝C _m ^-1P _m(16)

The C of joint m can be obtained according to control vertex vector timing node vector ⁶continuous print 7 non-uniform rational B-spline joint trajectories.

G. construct time optimal fitness function, meet joint velocity, acceleration, pulsation continuously smooth and under being no more than the condition of the specified limits value of servomotor, adopt gravitation search particle cluster algorithm, to w group timing node sequence { t ₀, t ₁, Lt ₆off-line optimization, until meet maximum iteration time, export one group of optimal time sequence node.Concrete steps are:

Under the prerequisite that Delta robot operates steadily, require that motion T.T. is the shortest, structure objective function is:

$J=min\left(\underset{i=0}{\overset{5}{\&Sigma;}}{\mathrm{\&Delta;t}}_i\right)---\left(17\right)$

$\begin{array}{cc}s.t.& \left\{\begin{array}{c}\left|{\mathrm{\&theta;}}_{mj}^{\&prime;}\left(t\right)\right|\&le;V_{Cj}\\ \left|{\mathrm{\&theta;}}_{mj}^{\&prime;\&prime;}\left(t\right)\right|\&le;A_{Cj}\\ \left|{\mathrm{\&theta;}}_{mj}^{\&prime;\&prime;\&prime;}\left(t\right)\right|\&le;J_{Cj}\end{array}\right.\end{array},t\&Element;\&lsqb;t_i,t_{i+1}\&rsqb;;m=1,2,3;i=0,1,L5;---\left(18\right)$

In formula, V _cj, A _cj, J _cjrepresent the specified limits value of the speed of robot servo motors, acceleration and pulsation respectively.θ ' _mj(t), θ " _mj(t), θ " ' _mjt () is respectively each joint velocity, acceleration, pulsation, can be obtained by formula (10) ~ (11):

$\left\{\begin{array}{c}{\mathrm{\&theta;}}_{mj}^{\&prime;}\left(t\right)=P^{\&prime;}\left(u\right)=\underset{j=i-k+1}{\overset{i}{\&Sigma;}}{d}_j^1{N}_{j,k-1}\left(u\right)\\ {\mathrm{\&theta;}}_{mj}^{\&prime;\&prime;}\left(t\right)=P^{\&prime;\&prime;}\left(u\right)=\underset{j=i-k+2}{\overset{i}{\&Sigma;}}{d}_j^2{N}_{j,k-2}\left(u\right)\\ {\mathrm{\&theta;}}_{mj}^{\&prime;\&prime;\&prime;}\left(t\right)=P^{\&prime;\&prime;\&prime;}\left(u\right)=\underset{j=i-k+3}{\overset{i}{\&Sigma;}}{d}_j^3{N}_{j,k-3}\left(u\right)\end{array}\right.---\left(19\right)$

Adopt gravitation search particle cluster algorithm, under the condition meeting formula (17) ~ (18), to w group timing node sequence { t ₀, t ₁, Lt ₆carry out off-line optimization, optimizing expression formula is:

$\left\{\begin{array}{c}{V}_i^d(t+1)=\mathrm{\&omega;}\&times;V_i^d\left(t\right)+c_1^{\&prime;}\&times;{\mathrm{rand}}_1\&times;a_i^d\left(t\right)+c_2^{\&prime;}\&times;{\mathrm{rand}}_2\&times;\left({\mathrm{gbest}}^d\right(t)-x_i^d(t\left)\right)\\ X_i^d(t+1)=X_i^d\left(t\right)+V_i^d(t+1)\end{array}\right.---\left(20\right)$

In formula, ω is the weight coefficient between 0-1; Rand ₁with rand ₂it is equally distributed random function between 0-1; C ' ₁represent that the acceleration of a upper moment particle self is to the influence degree of subsequent time particle rapidity; C ' ₂for the step-length regulating particle to fly to global history optimal location; Gbest ^dt () represents the optimum solution that the whole population of cut-off t finds; D represents dimension, and value is 0,1, L5; represent that the numerical value tieed up at t i-th particle d corresponds respectively to the interval of delta t of timing node sequence _i6 numerical value.

Optimizing algorithm meets maximum iteration time, then export one group of optimal time sequence node { t ₀, t ₁, Lt ₆.

H. adopt offline mode to M in steps A × N number of standard point, repeat above step B ~ G, obtain the optimal time sequence node { t of M × N group with standard point corresponding to the "door" type track of starting point ₀, t ₁, Lt ₆and be stored in three-dimensional array A and B.Concrete steps are as follows:

The distribution situation of the first quartile mesh standard point formed for X-axis positive dirction and Y-axis positive dirction sets up three-dimensional array A, and the first dimension number of elements is consistent with the standard point quantity distributed along X-axis positive dirction single file, is M '; Second dimension number of elements is consistent with the standard point quantity distributed along Y-axis positive dirction single file, is N; Third dimension number of elements is consistent with the quantity that each organizes timing node sequence, is 7, and therefore call number is respectively 0-(M'-1), 0-(N-1), 0-6; In like manner, the distribution situation of the second quadrant mesh standard point formed for X-axis negative direction and Y-axis positive dirction sets up three-dimensional array B, and dimension is identical with three-dimensional array A with call number.

Under offline mode, to M in steps A × N number of standard point, repeat above step B ~ G, obtain M × N group optimal time sequence node { t ₀, t ₁, Lt ₆.

According to the position of each standard point on main belt, one group of optimal time sequence node { t that to determine with this standard point be starting point corresponding to "door" type track ₀, t ₁, Lt ₆call number in three-dimensional array.When horizontal ordinate x>=0 of standard point, then store this group optimal time sequence node { t ₀, t ₁, Lt ₆relevant position in three-dimensional array A; Otherwise, then the relevant position in three-dimensional array B is stored in, for robot on-line operation is prepared.

I. when industrial intelligent camera identifies object central point on main belt, this point is also referred to as impact point, the coordinate position of robot working space is converted into by coordinate transform, the position of this object place mesh standard point is determined, one group of optimal time sequence node { t that to calculate with this standard point be starting point corresponding to "door" type track by the value of horizontal ordinate and ordinate ₀, t ₁, Lt ₆call number in three-dimensional array, obtain this group optimal time sequence node { t in three-dimensional array A and the B utilizing the mode of online query to set up in step H ₀, t ₁, Lt ₆, this impact point is adopted the method being similar to standard point, repeat the quick planning robot's movement locus of above step B ~ D, complete set motion, as shown in Figure 2, the relation of impact point and standard point is as shown in Figure 6 for online query flow process simultaneously.

The foregoing is only explanation embodiments of the present invention; be not limited to the present invention, for a person skilled in the art, within the spirit and principles in the present invention all; any amendment of doing, equivalent replacement, improvement etc., all should be included within protection scope of the present invention.

Claims (1)

1., based on a Delta method for planning track of robot for gravitation search particle cluster algorithm, it is characterized in that, comprise following steps:

A. adopting the mode of stress and strain model that Delta robot main belt perform region is divided into M × N number of length of side is the square of dcm, 0 < d≤2, and each square is called grid, and choosing grid element center point is standard point;

B. at cartesian space, to the standard point in described steps A and fixed placement point Q ₆adopt the mode of straight line and circular interpolation, obtain the position discrete series { Q of cartesian space "door" type track ₀, Q ₁, LQ ₆;

C. according to Delta robot architecture model, each main drive shaft angle θ is obtained ₁, θ ₂, θ ₃with end effector position E ₀inverse Kinematics Solution;

D. according to the position discrete series { Q of the Inverse Kinematics Solution in described step C by cartesian space "door" type track in described step B ₀, Q ₁, LQ ₆convert the angle discrete series { P of Delta robotic joint space to ₀, P ₁, LP ₆;

E. the timing node sequence { t of initialization w group joint space ₀, t ₁, Lt ₆; the timing node sequence often organizing joint space is consistent with the timing node sequence in cartesian space with standard point corresponding to the "door" type track of starting point; for simplified illustration, be below referred to as " timing node sequence ", and often organize interval { the Δ t of timing node sequence ₀, Δ t ₁, L Δ t ₅be random number within 1s;

F. by the angle discrete series { P of joint space in described step D ₀, P ₁, LP ₆with described step e in any one group of timing node sequence { t ₀, t ₁, Lt ₆corresponding, the angle-timing node sequence { P of composition joint space _i, t _i, i=0,1, L6, and the angle-timing node sequence constructing 7 these joint spaces of non-uniform rational B-spline function interpolation, control the motion in each joint;

G. construct time optimal fitness function, meet joint velocity, acceleration, pulsation continuously smooth and under being no more than the condition of the specified limits value of servomotor, adopt gravitation search particle cluster algorithm, to w group timing node sequence { t ₀, t ₁, Lt ₆off-line optimization, until meet maximum iteration time, export one group of optimal time sequence node;

H. adopt offline mode to M in described steps A × N number of standard point, repeat the above step B ~ G, obtain the optimal time sequence node { t of M × N group with standard point corresponding to the "door" type track of starting point ₀, t ₁, Lt ₆and be stored in three-dimensional array A and B;

I. industrial intelligent camera identifies the central point of object on main belt, and this point is called impact point, utilizes online query mode, one group of optimal time sequence node { t that to obtain with impact point place mesh standard point be starting point corresponding to "door" type track ₀, t ₁, Lt ₆, this impact point is adopted the method being similar to standard point simultaneously, repeat the quick planning robot's movement locus of the above step B ~ D, complete set motion.