CN102298391A - Motion trail planning method for heavy-duty industrial robot in operating space - Google Patents

Abstract

The invention discloses a motion trail planning method for a heavy-duty industrial robot in an operating space, which relates to a planning method for robot motion trail. The method provided for the invention is used for solving the following problems that: in the existing heavy-duty robot motion planning method, acceleration is discontinuous at a start point and a stop point of a space multi-point spline, and smoothness of motion instructions is not implemented; and the existing heavy-duty robot motion planning is failed in implementing best balance between optimal time and optimal energy consumption. Technical points of the method provided by the invention are as follows: the method modifies a traditional cubic spline by additional items, so that the spline keeps continuous speed and acceleration in whole process; and simultaneously, time optimization and energy consumption optimization are considered synthetically, system energy consumption and execution time are selected as optimization targets based on a dynamical model, speed, torque and jerk are selected as constraint conditions so as to establish a multi-target optimization model, and the optimization model is solved by a NSGA (Non-dominated Sorting Genetic Algorithm)2. The method provided by the invention is suitable for offline computation processes, such as trail planning, motion planning and the like, of the heavy-duty industrial robot.

Description

Movement locus planing method in a kind of heavily loaded industrial robot operating space

Technical field

The present invention relates to a kind of planing method of robot motion's track, belong to robot motion's control technology field.

Background technology

The high-speed overload industrial robot is widely used in automobile making, aviation, petrochemical industry metallurgy, port logistics industry.The high-speed overload robot increases production line flexible manufacturing ability for enhancing productivity, and improving the industrial automation level has significance.The movement locus planning of heavy duty robot be guarantee that heavily loaded industrial robot is efficient, reliable, one of the key core technology of even running.There are following 2 basic demands in present heavily loaded robot motion planning:

1, will generate movement locus must be enough smooth.The motion planning algorithm need be selected a smooth enough basis function thus, and it is continuous that it need possess position command C2, and speed command C1 is continuous, and the acceleration instruction is continuous, acceleration instruction bounded.If motion planning is the (rate signal of non-differentiability for example inadequately rationally, discontinuous acceleration signal, the acceleration signal of unbounded), can cause the reduction of robot system bearing accuracy and track following precision, the generation of residual oscillation, thereby the robot performance is seriously reduced, even damage the precision drive parts, reduce the serviceable life of robot.

2, to satisfy the movement locus that is generated between the each point of mission requirements and guarantee that working time is the shortest, the operation energy consumption minimum.Working time, minimum helped the quickening of production efficiency raising, productive temp, reduced production costs.The operation energy consumption minimum then is from the angle that cuts down the consumption of energy motion planning to be carried out requirement, and this point is especially at the harbour, and is even more important on the metallurgy industry heavy duty stacker handling robot application.

For above requirement, there is following problem in the similar techniques that at present domestic and international robot and relevant kinetic control system are adopted.The kinetic control system of at first present heavily loaded robot and main flow can't guarantee at the acceleration of starting point and terminating point continuous when the multiple spot spline interpolation of space, can't guarantee that promptly acceleartion boundary is 0.This point need improve.Secondly all there is deficiency in domestic and international known public algorithm.As at fixed route, Shin KG has proposed the motion planning algorithm of time and energy consumption optimum [referring to scientific paper " K.G.Shin; N.D.McKay. " Minimum-time control of robotic manipulators with geometric path constraints " IEEE Trans Automat Control; vol.30; pp.531-541; Jun.1985 " and " K.G.Shin; N.D.McKay. " A Dynamic programming approach to trajectory planning of robotic manipulators " IEEE Trans Automat Control.vol.31, pp.491-500, Jun.1986. "]; but the acceleration of its generation and torque command are discontinuous; can not satisfy top requirement at first, can't practical application.By to the torque command amplitude limit, Constantinescu is when guaranteeing slickness, proposed a kind of at time optimal algorithm, but it lacks the consideration [referring to scientific paper " B.J.Martin; J.E.Bobrow.Minimum effort motions for open chain manipulators with task-dependent end-effector constraints.Int J Robot Res; vol.18, pp.213-224, Feb.1999. "] to energy consumption.Saramago and Steffen has proposed the corresponding sports planning algorithm at the energy consumption optimum, its with execution time and energy consumption as optimization aim, but its gained is optimized the result and is depended on two targets weight separately, choose comparatively responsive [referring to scientific paper " S.F.P.Saramago; J.V.Steffen; Optimization of the trajectory planning of robot manipulators taking into account the dynamics of the system.Mech and Mach Theory; vol.33; pp.883-894, Oct.1998. "] to weight.

Summary of the invention

The present invention does not realize the movement instruction slickness in order to solve in the existing heavily loaded robot motion planning method in space multiple spot batten starting point and the discontinuous problem of terminating point acceleration; And have the problem that heavily loaded robot motion planning is not realized time optimal, both optimal compromise of energy consumption optimum now, and then provide movement locus planing method in a kind of heavily loaded industrial robot operating space.

The present invention solves the problems of the technologies described above the technical scheme of taking to be:

The detailed process of movement locus planing method is in a kind of heavily loaded industrial robot of the present invention operating space:

Steps A, the splines of adopt revising at multiple spot in the space are carried out interpolation:

Steps A 1, employing cubic spline curve are set up basic interpolation function, adopt cubic polynomial to carry out interpolation between any two presets, and the expression formula of basic interpolation function is as follows:

S _j(t)＝a _j+b _j(t-t _j)+c _j(t-t _j) ²+d _j(t-t _j) ³

t∈[t _j，t _j+1]，j＝0，1，...，n-2 (1)

The time interval between two adjacent presets is h _j=t _J+1-t _ja _j, b _j, c _j, d _jBe coefficient to be asked; N represents preset quantity;

Steps A 2, while must letter of guarantee numerical value S for the assurance continuity in each preset _j(t) first derivative values S ' _j(t) and functional value S _j(t) second derivative value S " _j(t) equate, be:

S _j(t _j)＝α _j，j＝0，1，..，n-2

S _n-2(t _n-1)＝α _n-1 (2)

α _jRepresent each preset coordinate figure;

$\left\{\begin{array}{c}{S}_j\left(t_{j+1}\right)=S_{j+1}\left(t_{j+1}\right)\\ S_j^{\′}\left(t_{j+1}\right)=S_{j+1}^{\′}\left(t_{j+1}\right)\\ S_j^{\′\′}\left(t_{j+1}\right)=S_{j+1}^{\′\′}\left(t_{j+1}\right)\end{array}\right.,j=\mathrm{0,1},...,n-3---\left(3\right)$

Steps A 3, employing fixed boundary condition, satisfying in starting point and terminating point speed is 0:

S′ ₀(t ₀)＝0，S′ _n-2(t _n-1)＝0 (4)

Steps A 3, to set up cubic spline Total tune equation as follows:

H·C＝S (5)

In the formula:

$C=\left[\begin{array}{c}{\mathrm{<figure-callout\; id="0"\; label="S"\; filenames="HDA0000057879150000041.png,HDA0000057879150000052.png"\; state="\{\{state\}\}">S</figure-callout>}}_0^{\&prime;\&prime;}\left(t_0\right)\\ {\mathrm{<figure-callout\; id="1"\; label="S"\; filenames="HDA0000057879150000011.png,HDA0000057879150000012.png"\; state="\{\{state\}\}">S</figure-callout>}}_1^{\&prime;\&prime;}\left(t_1\right)\\ .\\ .\\ .\\ S_{n-3}^{\&prime;\&prime;}\left(t_{n-3}\right)\\ S_{n-2}^{\&prime;\&prime;}\left(t_{n-2}\right)\\ S_{n-2}^{\&prime;\&prime;}\left(t_{n-1}\right)\end{array}\right],$ $S=\left[\begin{array}{c}\frac{S_0\left(t_1\right)-S_1\left(t_0\right)}{h_1{h}_0}(h_1+h_0)\\ \frac{S_2-S_1}{h_1}-\frac{S_1-S_0}{{\mathrm{<figure-callout\; id="0"\; label="h"\; filenames="HDA0000057879150000041.png,HDA0000057879150000052.png"\; state="\{\{state\}\}">h</figure-callout>}}_0}\\ .\\ .\\ .\\ \frac{S_{n-2}-S_{n-3}}{h_{n-2}}-\frac{S_{n-3}-S_{n-4}}{h_{n-3}}\\ \frac{S_0\left(t_{n-1}\right)-S_1\left(t_{n-2}\right)}{h_n{h}_0}(h_n+h_0)\end{array}\right]$

Steps A 4, formula (1) is revised, in The initial segment with stop section and increase auxiliary, revised batten expression formula:

S ₀(t)＝a ₀+b ₀(t-t ₀)+c ₀(t-t ₀) ²+d ₀(t-t ₀) ³+A ₀(t)

S _n-2(t)＝a _n-2+b _n-2(t-t _n-2)+c _n-2(t-t _n-2) ²+d _n-2(t-t _n-2) ³+A _n-2(t) (6)

N preset quantity has n-1 spacer, and n-1 expression formula promptly arranged; The expression formula of The initial segment is S ₀(t), the expression formula of termination section is S _N-2(t); A ₀(t), A _N-2(t) be the auxiliary item of polynomial form;

Steps A 5, auxiliary the A that finds the solution polynomial form ₀(t) and A _N-2(t) auxiliary expression formula:

A ₀(t) and A _N-2(t) must satisfy following equation of constraint:

$\left\{\begin{array}{c}{A}_0\left(t_0\right)=A_0\left(t_1\right)=0\\ {\mathrm{<figure-callout\; id="0"\; label="A"\; filenames="HDA0000057879150000041.png,HDA0000057879150000052.png"\; state="\{\{state\}\}">A</figure-callout>}}_0^{\&prime;}\left(t_0\right)=A_0^{\&prime;}\left(t_1\right)=0\\ {\mathrm{<figure-callout\; id="0"\; label="A"\; filenames="HDA0000057879150000041.png,HDA0000057879150000052.png"\; state="\{\{state\}\}">A</figure-callout>}}_0^{\&prime;\&prime;}\left(t_0\right)=-{\mathrm{<figure-callout\; id="0"\; label="S"\; filenames="HDA0000057879150000041.png,HDA0000057879150000052.png"\; state="\{\{state\}\}">S</figure-callout>}}_0^{\&prime;\&prime;}\left(t_0\right)\\ A_0^{\&prime;\&prime;}\left(t_1\right)=0\\ A_{n-2}\left(t_{n-2}\right)=A_{n-2}\left(t_{n-1}\right)=0\\ A_{n-2}^{\&prime;}\left(t_{n-2}\right)=A_{n-2}^{\&prime;}\left(t_{n-1}\right)=0\\ A_{n-2}^{\&prime;\&prime;}\left(t_{n-2}\right)=-S_{n-2}^{\&prime;\&prime;}\left(t_{n-2}\right)\\ A_{n-2}^{\&prime;\&prime;}\left(t_{n-1}\right)=0\end{array}\right.---\left(7\right)$

Finding the solution above-mentioned equation, can to get the auxiliary expression formula as follows:

$A_0\left(t\right)={\mathrm{<figure-callout\; id="0"\; label="S"\; filenames="HDA0000057879150000041.png,HDA0000057879150000052.png"\; state="\{\{state\}\}">S</figure-callout>}}_0^{\&prime;\&prime;}\left(t_0\right){\left({t-t}_0\right)}^2{\left({t-t}_1\right)}^3/\left({2\mathrm{<figure-callout\; id="0"\; label="h"\; filenames="HDA0000057879150000041.png,HDA0000057879150000052.png"\; state="\{\{state\}\}">h</figure-callout>}}_0^3\right)$

$A_{n-2}\left(t\right)=S_{n-2}^{\&prime;\&prime;}\left(t_{n-2}\right){(t-t_{n-2})}^2{\left({t-t}_{n-1}\right)}^3/\left({2h}_{n-2}^3\right)---\left(8\right)$

Step B, by to the each point working time of adjustment at interval, by setting up corresponding Optimization Model, make heavily loaded industrial robot execute whole segment of curve motion with the performance of optimum:

Step B1, select the execution time h between each set point ₀, h ₁..., h _N-1As parameters optimization (optimization variable), be total to n optimization variable;

Step B2, with (motor total energy consumption) P of the execution energy consumption in the whole track process and execution time T as optimization aim;

Step B3, with motor speed (i axle speed), moment and acceleration as constraint condition;

Step B4, formula (1), formula (6) and formula (8) formation base interpolating function after basic interpolating function is determined, are converted into following Optimization Model (target) with whole motion planning:

$\min \left\{\begin{array}{c}T=\underset{j=0}{\overset{n-1}{\mathrm{\&Sigma;}}}{h}_i\\ P=\underset{j=0}{\overset{n-1}{\mathrm{\&Sigma;}}}\underset{i=1}{\overset{m}{\mathrm{\&Sigma;}}}({\&Integral;}_{t_j}^{t_{j+1}}{\mathrm{\&tau;}}_i\left(\mathrm{\&theta;}\right)\&CenterDot;d{\mathrm{\&theta;}}_i)\end{array}\right.---\left(9\right)$

M represents joint of robot quantity, and T represents the execution time, and P represents to carry out energy consumption; θ represents the corner vector that each joint rotation angle of robot is formed, θ _iExpression i axle joint rotation angle;

It is as follows to set up constraint condition:

$s.t.\left\{\begin{array}{c}\max \left(\right|{\mathrm{\&tau;}}_i\left|\right)<{\mathrm{\&tau;}}_{i\max }\\ \max \left(\right|v_i\left|\right)<v_{i\max }\\ \max \left(\right|J_i\left|\right)<J_{i\max }\end{array}\right.---\left(10\right)$

τ _iExpression i axle moment, v _iExpression i axle speed, J _iExpression i axle acceleration, τ _ImaxThe expression i axle moment upper limit, v _ImaxExpression i axle speed limit, J _ImaxThe expression i axle acceleration upper limit;

Step C, finish above each step after, adopt the non-domination genetic algorithm of NSGAII type to parameters optimization h ₀, h ₁..., h _N-1Find the solution;

Step D, the parameters optimization h that step C is obtained ₀, h ₁..., h _N-1Substitution formula (5), and then try to achieve and wait to ask coefficient a _j, b _j, c _j, d _jDisaggregation, the final sectional type basis interpolation function determined by formula (1) and formula (2) of obtaining, the basic interpolation function of described sectional type is in order to express the movement locus in the heavily loaded industrial robot operating space.

The invention has the beneficial effects as follows:

The inventive method has been taken all factors into consideration instruction slickness, time and energy consumption, make time and energy consumption all reach optimum simultaneously, be at heavily loaded transfer robot movement instruction generate proposed a kind of have enough smooth, and energy consumption and time optimal movement locus planing method.This method is applicable to and comprises the drag articulation type, cartesian co-ordinate type, and the parallel machine configuration is at interior various heavily loaded conveying robot.This method is revised traditional cubic spline by additive term.Make on its speed that in whole process, (comprises starting point and terminating point), the acceleration and all keep continuously.Take all factors into consideration time and energy consumption optimal problem simultaneously, based on kinetic model selecting system energy consumption and execution time as optimization aim, speed, torque, acceleration are set up Model for Multi-Objective Optimization as constraint condition.Select sensitive issue at the multiple goal solving result for weight, adopt the non-domination genetic algorithm of NSGA2 to be optimized model solution.The present invention relates generally to basic splines, objective function, and the selection of constraint condition and derivation algorithm and improvement are applicable to the calculated off-line process such as trajectory planning, motion planning of heavily loaded industrial robot.

Adopt the splines of revising to carry out interpolation at multiple spot in the space, guaranteed that the movement instruction that is generated is enough smooth, reduce machinery and electrical system are impacted.By the adjustment to each point interval working time, by setting up corresponding Optimization Model, feasible performance with optimum executes whole segment of curve motion.In finding the solution the optimized parameter process, adopt specific optimized Algorithm, can avoid a plurality of optimization aim to find the solution down the dependence of selecting for weights.

Description of drawings

Fig. 1 is the NSGA-II algorithm evolution process flow diagram that adopts in the inventive method; Fig. 2 is that (horizontal ordinate among the figure is the execution time to two target function value curve maps of whole Pareto disaggregation in the NSGA-II algorithmic procedure that adopts, and unit is second; Ordinate is for carrying out energy consumption, and unit is a joule; Elliptic region is represented the Pareto disaggregation);

Fig. 3 a is that initial time movement space distribution diagram at interval (under three-dimensional system of coordinate, has 5 presets among the figure; The unit of three coordinate axis X, Y, Z is a rice), Fig. 3 b is that the movement space distribution diagram of optimizing the gained time interval (under three-dimensional system of coordinate, has 5 presets among the figure; The unit of three coordinate axis X, Y, Z is a rice); Fig. 4 a is that initial time descends each shaft power figure of robot at interval (horizontal ordinate among the figure is working time, and unit is second; Ordinate is an energy consumption, and unit is a joule; " Joint " expression " axle "; " sum " expression " summation "), Fig. 4 b optimizes under time interval of gained each shaft power figure of robot (horizontal ordinate among the figure is working time, and unit be second; Ordinate is an energy consumption, and unit is a joule; " Joint " expression " axle "; " sum " expression " summation "); Fig. 5 a is that (horizontal ordinate among the figure is working time to initial time interval hypozygal speed (axle rotating speed) curve map, and unit is second; Ordinate is a joint velocity, and unit is degree/second), (horizontal ordinate among the figure is working time to Fig. 5 b, and unit is second in order to optimize gained time interval hypozygal speed (axle rotating speed) curve map; Ordinate is a joint velocity, and unit is degree/second); Fig. 6 a is that (horizontal ordinate among the figure is working time to initial time interval hypozygal acceleration (axle acceleration) curve map, and unit is second; Ordinate is a joint velocity, and unit is degree/square second), (horizontal ordinate among the figure is working time to Fig. 6 b, and unit is second in order to optimize gained time interval hypozygal acceleration (axle acceleration) curve map; Ordinate is a joint velocity, and unit is degree/square second); Fig. 7 a is that (horizontal ordinate among the figure is working time to initial time interval hypozygal moment (axle moment) curve map, and unit is second; Ordinate is a joint moment, and unit is a Newton meter), (horizontal ordinate among the figure is working time to Fig. 7 b, and unit is second in order to optimize gained time interval hypozygal moment (axle moment) curve map; Ordinate is a joint moment, and unit is a Newton meter).

Embodiment

Embodiment one: the detailed process of movement locus planing method is in the described heavily loaded industrial robot of the present embodiment operating space:

Steps A, the splines of adopt revising at multiple spot in the space are carried out interpolation:

Steps A 1, employing cubic spline curve are set up basic interpolation function, adopt cubic polynomial to carry out interpolation between any two presets, and the expression formula of basic interpolation function is as follows:

S _j(t)＝a _j+b _j(t-t _j)+c _j(t-t _j) ²+d _j(t-t _j) ³

t∈[t _j，t _j+1]，j＝0，1，...，n-2 (1)

The time interval between two adjacent presets is h _j=t _J+1-t _ja _j, b _j, c _j, d _jBe coefficient to be asked; N represents preset quantity;

Steps A 2, while must letter of guarantee numerical value S for the assurance continuity in each preset _j(t) first derivative values S ' _j(t) and functional value S _j(t) second derivative value S " _j(t) equate, be:

S _j(t _j)＝α _j，j＝0，1，...，n-2

S _n-2(t _n-1)＝α _n-1 (2)

α _jRepresent each preset coordinate figure;

$\left\{\begin{array}{c}{S}_j\left(t_{j+1}\right)=S_{j+1}\left(t_{j+1}\right)\\ S_j^{\&prime;}\left(t_{j+1}\right)=S_{j+1}^{\&prime;}\left(t_{j+1}\right)\\ S_j^{\&prime;\&prime;}\left(t_{j+1}\right)=S_{j+1}^{\&prime;\&prime;}\left(t_{j+1}\right)\end{array}\right.,j=\mathrm{0,1},...,n-3---\left(3\right)$

Steps A 3, employing fixed boundary condition, satisfying in starting point and terminating point speed is 0:

S′ ₀(t ₀)＝0，S′ _n-2(t _n-1)＝0 (4)

Steps A 3, to set up cubic spline Total tune equation as follows:

H·C＝S (5)

In the formula:

$C=\left[\begin{array}{c}{\mathrm{<figure-callout\; id="0"\; label="S"\; filenames="HDA0000057879150000041.png,HDA0000057879150000052.png"\; state="\{\{state\}\}">S</figure-callout>}}_0^{\&prime;\&prime;}\left(t_0\right)\\ {\mathrm{<figure-callout\; id="1"\; label="S"\; filenames="HDA0000057879150000011.png,HDA0000057879150000012.png"\; state="\{\{state\}\}">S</figure-callout>}}_1^{\&prime;\&prime;}\left(t_1\right)\\ .\\ .\\ .\\ S_{n-3}^{\&prime;\&prime;}\left(t_{n-3}\right)\\ S_{n-2}^{\&prime;\&prime;}\left(t_{n-2}\right)\\ S_{n-2}^{\&prime;\&prime;}\left(t_{n-1}\right)\end{array}\right],$ $S=\left[\begin{array}{c}\frac{S_0\left(t_1\right)-S_1\left(t_0\right)}{h_1{h}_0}({\mathrm{<figure-callout\; id="1"\; label="h"\; filenames="HDA0000057879150000011.png,HDA0000057879150000012.png"\; state="\{\{state\}\}">h</figure-callout>}}_1+h_0)\\ \frac{S_2-S_1}{h_1}-\frac{S_1-S_0}{{\mathrm{<figure-callout\; id="0"\; label="h"\; filenames="HDA0000057879150000041.png,HDA0000057879150000052.png"\; state="\{\{state\}\}">h</figure-callout>}}_0}\\ .\\ .\\ .\\ \frac{S_{n-2}-S_{n-3}}{h_{n-2}}-\frac{S_{n-3}-S_{n-4}}{h_{n-3}}\\ \frac{S_0\left(t_{n-1}\right)-S_1\left(t_{n-2}\right)}{h_n{h}_0}(h_n+h_0)\end{array}\right]$

Steps A 4, formula (1) is revised, in The initial segment with stop section and increase auxiliary, revised batten expression formula:

S ₀(t)＝a ₀+b ₀(t-t ₀)+c ₀(t-t ₀) ²+d ₀(t-t ₀) ³+A ₀(t)

S _n-2(t)＝a _n-2+b _n-2(t-t _n-2)+c _n-2(t-t _n-2) ²+d _n-2(t-t _n-2) ³+A _n-2(t) (6)

N preset quantity has n-1 spacer, and n-1 expression formula promptly arranged; The expression formula of The initial segment is S ₀(t), the expression formula of termination section is S _N-2(t); A ₀(t), A _N-2(t) be the auxiliary item of polynomial form;

Steps A 5, auxiliary the A that finds the solution polynomial form ₀(t) and A _N-2(t) auxiliary expression formula:

A ₀(t) and A _N-2(t) must satisfy following equation of constraint:

$\left\{\begin{array}{c}{A}_0\left(t_0\right)=A_0\left(t_1\right)=0\\ A_0^{\&prime;}\left(t_0\right)=A_0^{\&prime;}\left(t_1\right)=0\\ A_0^{\&prime;\&prime;}\left(t_0\right)=-{\mathrm{<figure-callout\; id="0"\; label="S"\; filenames="HDA0000057879150000041.png,HDA0000057879150000052.png"\; state="\{\{state\}\}">S</figure-callout>}}_0^{\&prime;\&prime;}\left(t_0\right)\\ A_0^{\&prime;\&prime;}\left(t_1\right)=0\\ A_{n-2}\left(t_{n-2}\right)=A_{n-2}\left(t_{n-1}\right)=0\\ A_{n-2}^{\&prime;}\left(t_{n-2}\right)=A_{n-2}^{\&prime;}\left(t_{n-1}\right)=0\\ A_{n-2}^{\&prime;\&prime;}\left(t_{n-2}\right)=-S_{n-2}^{\&prime;\&prime;}\left(t_{n-2}\right)\\ A_{n-2}^{\&prime;\&prime;}\left(t_{n-1}\right)=0\end{array}\right.---\left(7\right)$

Finding the solution above-mentioned equation, can to get the auxiliary expression formula as follows:

$A_0\left(t\right)={\mathrm{<figure-callout\; id="0"\; label="S"\; filenames="HDA0000057879150000041.png,HDA0000057879150000052.png"\; state="\{\{state\}\}">S</figure-callout>}}_0^{\&prime;\&prime;}\left(t_0\right){\left({t-t}_0\right)}^2{\left({t-t}_1\right)}^3/\left({2\mathrm{<figure-callout\; id="0"\; label="h"\; filenames="HDA0000057879150000041.png,HDA0000057879150000052.png"\; state="\{\{state\}\}">h</figure-callout>}}_0^3\right)$

$A_{n-2}\left(t\right)=S_{n-2}^{\&prime;\&prime;}\left(t_{n-2}\right){(t-t_{n-2})}^2{\left({t-t}_{n-1}\right)}^3/\left({2h}_{n-2}^3\right)---\left(8\right)$

Step B, by to the each point working time of adjustment at interval, by setting up corresponding Optimization Model, make heavily loaded industrial robot execute whole segment of curve motion with the performance of optimum:

Step B1, select the execution time h between each set point ₀, h ₁..., h _N-1As parameters optimization (optimization variable), be total to n optimization variable;

Step B2, with (motor total energy consumption) P of the execution energy consumption in the whole track process and execution time T as optimization aim;

Step B3, with motor speed (i axle speed), moment and acceleration as constraint condition;

Step B4, formula (1), formula (6) and formula (8) formation base interpolating function after basic interpolating function is determined, are converted into following Optimization Model (target) with whole motion planning:

$\min \left\{\begin{array}{c}T=\underset{j=0}{\overset{n-1}{\mathrm{\&Sigma;}}}{h}_i\\ P=\underset{j=0}{\overset{n-1}{\mathrm{\&Sigma;}}}\underset{i=1}{\overset{m}{\mathrm{\&Sigma;}}}({\&Integral;}_{t_j}^{t_{j+1}}{\mathrm{\&tau;}}_i\left(\mathrm{\&theta;}\right)\&CenterDot;d{\mathrm{\&theta;}}_i)\end{array}\right.---\left(9\right)$

M represents joint of robot quantity, and T represents the execution time, and P represents to carry out energy consumption; θ represents the corner vector that each joint rotation angle of robot is formed, θ _iExpression i axle joint rotation angle;

It is as follows to set up constraint condition:

$s.t.\left\{\begin{array}{c}\max \left(\right|{\mathrm{\&tau;}}_i\left|\right)<{\mathrm{\&tau;}}_{i\max }\\ \max \left(\right|v_i\left|\right)<v_{i\max }\\ \max \left(\right|J_i\left|\right)<J_{i\max }\end{array}\right.---\left(10\right)$

τ _iExpression i axle moment, v _iExpression i axle speed, J _iExpression i axle acceleration, τ _ImaxThe expression i axle moment upper limit, v _ImaxExpression i axle speed limit, J _ImaxThe expression i axle acceleration upper limit;

Step C, finish above each step after, adopt the non-domination genetic algorithm of NSGAII type to parameters optimization h ₀, h ₁..., h _N-1Find the solution; The non-domination genetic algorithm of NSGAII type is the prior art category;

Step D, the parameters optimization h that step C is obtained ₀, h ₁..., h _N-1Substitution formula (5), and then try to achieve and wait to ask coefficient a _j, b _j, c _j, d _jDisaggregation, the final sectional type basis interpolation function determined by formula (1) and formula (2) of obtaining, the basic interpolation function of described sectional type is in order to express the movement locus in the heavily loaded industrial robot operating space.

Each variable detail is referring to table 1:

Table 1 variable detail

m	Joint of robot quantity
		n	Preset quantity
α j	Each preset coordinate figure
		h j	The time interval between the adjacent preset
T	Execution time
		P	Carry out energy consumption
τ i	I axle moment
		v i	I axle speed
J i	I axle acceleration
		τ imax	The i axle moment upper limit
v imax	I axle speed limit
		J imax	The i axle moment upper limit

Further specialize again at above-mentioned embodiment:

One, this patent carries out corresponding improvement on the basis of conventional cubic spline curve.

Specific as follows: adopt cubic spline curve can guarantee that at each default transition point speed is continuous, acceleration is continuous.It is as follows to adopt cubic polynomial to carry out interpolation between any two presets

S _j(t)＝a _j+b _j(t-t _j)+c _j(t-t _j) ²+d _j(t-t _j) ³

t∈[t _j，t _j+1]，j＝0，1，...，n-2 (1)

The time interval between two presets is h _j=t _J+1-t _j, be the assurance continuity simultaneously in each preset, must letter of guarantee numerical value S _j(t), first derivative values S ' _j(t), second derivative value S " _j(t) equate, be

S _j(t _j)＝α _j，j＝0，1，..，n-2

S _n-2(t _n-1)＝αn-1 (2)

$\left\{\begin{array}{c}{S}_j\left(t_{j+1}\right)=S_{j+1}\left(t_{j+1}\right)\\ S_j^{\&prime;}\left(t_{j+1}\right)=S_{j+1}^{\&prime;}\left(t_{j+1}\right)\\ S_j^{\&prime;\&prime;}\left(t_{j+1}\right)=S_{j+1}^{\&prime;\&prime;}\left(t_{j+1}\right)\end{array}\right.,j=\mathrm{0,1},...,n-3---\left(3\right)$

Simultaneously must satisfy in starting point and terminating point speed is 0:

S′ ₀(t ₀)＝0，S′ _n-2(t _n-1)＝0 (4)

Cubic spline Total tune equation is as follows

H·C＝S (5)

Herein

$C=\left[\begin{array}{c}{\mathrm{<figure-callout\; id="0"\; label="S"\; filenames="HDA0000057879150000041.png,HDA0000057879150000052.png"\; state="\{\{state\}\}">S</figure-callout>}}_0^{\&prime;\&prime;}\left(t_0\right)\\ {\mathrm{<figure-callout\; id="1"\; label="S"\; filenames="HDA0000057879150000011.png,HDA0000057879150000012.png"\; state="\{\{state\}\}">S</figure-callout>}}_1^{\&prime;\&prime;}\left(t_1\right)\\ .\\ .\\ .\\ S_{n-3}^{\&prime;\&prime;}\left(t_{n-3}\right)\\ S_{n-2}^{\&prime;\&prime;}\left(t_{n-2}\right)\\ S_{n-2}^{\&prime;\&prime;}\left(t_{n-1}\right)\end{array}\right],$ $S=\left[\begin{array}{c}\frac{S_0\left(t_1\right)-S_1\left(t_0\right)}{h_1{h}_0}({\mathrm{<figure-callout\; id="1"\; label="h"\; filenames="HDA0000057879150000011.png,HDA0000057879150000012.png"\; state="\{\{state\}\}">h</figure-callout>}}_1+h_0)\\ \frac{S_2-S_1}{h_1}-\frac{S_1-S_0}{{\mathrm{<figure-callout\; id="0"\; label="h"\; filenames="HDA0000057879150000041.png,HDA0000057879150000052.png"\; state="\{\{state\}\}">h</figure-callout>}}_0}\\ .\\ .\\ .\\ \frac{S_{n-2}-S_{n-3}}{h_{n-2}}-\frac{S_{n-3}-S_{n-4}}{h_{n-3}}\\ \frac{S_0\left(t_{n-1}\right)-S_1\left(t_{n-2}\right)}{h_n{h}_0}(h_n+h_0)\end{array}\right]$

By finding the solution above-mentioned equation, can obtain the batten expression formula, though but can guarantee to be 0 in starting point and terminating point speed edges, or other setting values, but its acceleartion boundary is not 0, is step signal thereby cause in starting point and terminating point acceleration signal, and this can cause the exciting to system's low order mode of oscillation, therefore need revise common cubic spline interpolation, increase auxiliary in The initial segment and termination section

S ₀(t)＝a ₀+b ₀(t-t ₀)+c ₀(t-t ₀) ²+d ₀(t-t ₀) ³+A ₀(t)

S _n-2(t)＝a _n-2+b _n-2(t-t _n-2)+c _n-2(t-t _n-2) ²+d _n-2(t-t _n-2) ³+A _n-2(t) (6)

A ₀(t), A _N-2(t) be the auxiliary item of polynomial form, it must satisfy following equation of constraint.

$\left\{\begin{array}{c}{A}_0\left(t_0\right)=A_0\left(t_1\right)=0\\ A_0^{\&prime;}\left(t_0\right)=A_0^{\&prime;}\left(t_1\right)=0\\ A_0^{\&prime;\&prime;}\left(t_0\right)=-{\mathrm{<figure-callout\; id="0"\; label="S"\; filenames="HDA0000057879150000041.png,HDA0000057879150000052.png"\; state="\{\{state\}\}">S</figure-callout>}}_0^{\&prime;\&prime;}\left(t_0\right)\\ A_0^{\&prime;\&prime;}\left(t_1\right)=0\\ A_{n-2}\left(t_{n-2}\right)=A_{n-2}\left(t_{n-1}\right)=0\\ A_{n-2}^{\&prime;}\left(t_{n-2}\right)=A_{n-2}^{\&prime;}\left(t_{n-1}\right)=0\\ A_{n-2}^{\&prime;\&prime;}\left(t_{n-2}\right)=-S_{n-2}^{\&prime;\&prime;}\left(t_{n-2}\right)\\ A_{n-2}^{\&prime;\&prime;}\left(t_{n-1}\right)=0\end{array}\right.---\left(7\right)$

Finding the solution above-mentioned equation, can to get the auxiliary expression formula as follows:

$A_0\left(t\right)={\mathrm{<figure-callout\; id="0"\; label="S"\; filenames="HDA0000057879150000041.png,HDA0000057879150000052.png"\; state="\{\{state\}\}">S</figure-callout>}}_0^{\&prime;\&prime;}\left(t_0\right){\left({t-t}_0\right)}^2{\left({t-t}_1\right)}^3/\left({2\mathrm{<figure-callout\; id="0"\; label="h"\; filenames="HDA0000057879150000041.png,HDA0000057879150000052.png"\; state="\{\{state\}\}">h</figure-callout>}}_0^3\right)$

$A_{n-2}\left(t\right)=S_{n-2}^{\&prime;\&prime;}\left(t_{n-2}\right){(t-t_{n-2})}^2{\left({t-t}_{n-1}\right)}^3/\left({2h}_{n-2}^3\right)---\left(8\right)$

Two, after basic interpolating function was determined, whole motion planning problem can be converted into following optimization problem.

Optimization aim is as follows:

$\min \left\{\begin{array}{c}T=\underset{j=0}{\overset{n-1}{\mathrm{\&Sigma;}}}{h}_i\\ P=\underset{j=0}{\overset{n-1}{\mathrm{\&Sigma;}}}\underset{i=1}{\overset{m}{\mathrm{\&Sigma;}}}({\&Integral;}_{t_j}^{t_{j+1}}{\mathrm{\&tau;}}_i\left(\mathrm{\&theta;}\right)\&CenterDot;d{\mathrm{\&theta;}}_i)\end{array}\right.---\left(9\right)$

Constraint condition is as follows:

$s.t.\left\{\begin{array}{c}\max \left(\right|{\mathrm{\&tau;}}_i\left|\right)<{\mathrm{\&tau;}}_{i\max }\\ \max \left(\right|v_i\left|\right)<v_{i\max }\\ \max \left(\right|J_i\left|\right)<J_{i\max }\end{array}\right.---\left(10\right)$

Optimization variable is h ₀, h ₁..., h _N-1, execution time and execution energy consumption are as optimization aim.It is to should be that it is directly related with production efficiency that execution time is used as optimization aim.In heavily loaded robot manipulating task process, effectively energy consumption changes into the variation of robot manipulation's object potential energy simultaneously, and other energy (as the kinetic energy of system) all is converted into thermal dissipation.The general power of in this model motor being done is equally as optimization aim.In Optimization Model, contain three constraint conditions, it is respectively torque limit, the velocity limit and the acceleration limit, wherein torque and velocity limit are determined by driver part, because the resonance that excessive acceleration can evoke mechanical system, increase the wearing and tearing of mechanical system, so acceleration is equally also as one of constraint condition.

Three, at whole Optimization Model, find the solution down the dependence of selecting for weights for avoiding a plurality of optimization aim, adopt the non-domination genetic algorithm of multiple goal NSGA-II to find the solution (referring to Fig. 1 and Fig. 2).For multi-objective optimization question, there is a disaggregation usually.With regard to objective function, can't compare good and badly between these are separated, be characterized in: can't not weaken at least one other objective function when improving any objective function, these are separated and are called the Pareto optimum solution or non-domination is separated.The main task of finding the solution multi-objective optimization question is: find representative satisfactory Pareto optimum solution as much as possible with having no preference, after calculating equally distributed Pareto optimum solution, according to the concrete actual the most satisfied optimization result that requires therefrom to select objectively.NSGA-II algorithm evolution flow process, as illustrated in fig. 1 and 2, detailed process is:

(1) produces the initial parent population P that population scale is N at random ₀, and by genetic operator (intersect, make a variation) generation progeny population Q ₀, its population size also is N;

(2) with parent population P _nWith progeny population Q _nMerging the composition scale is the synthetic population R of 2N _nCarry out quick non-domination ordering, with R _nIn whole 2N individual reclassify by non-domination sequence number (grade), obtain grade F ₁, F ₂, F ₃Calculate the individual local congestion distance and the ordering of each non-domination layer.

(3) choose N individuality as new parent population P according to ranking results _N+1

(4) produce new progeny population Q by genetic operator (select, intersect, make a variation) _N+1

(5) repeat (2) to (4) step up to reaching the maximum iteration time that algorithm is provided with.

Parental generation population that is combined in this algorithm and progeny population carry out non-domination ordering and fill the process of new population, as shown in Figure 1.

When population quantity is made as 1000, when evolutionary generation was made as for 50 generations, whole evolution gained the results are shown in Figure 2, and whole optimization was found the solution 2.4 seconds consuming time.The disaggregation of the Pareto that obtains is as shown in table 2, wherein separates to be chosen as finally and separates for first group.

Table 2NSGA2 obtains optimal motion planning parameter PARETO disaggregation

No.	h 1(S)	h 2(S)	h 3(S)	h 4(S)	T(S)	P(J)(J)
							1	0.1674	0.2394	0.2992	0.5788	1.2848	2601.261
2	0.1668	0.2394	0.2992	0.5789	1.2843	2602.001
							3	0.1667	0.2393	0.2987	0.5789	1.2836	2602.559
4	0.1672	0.2383	0.2994	0.5781	1.2831	2603.299
							5	0.1666	0.2381	0.2997	0.5783	1.2828	2604.093
6	0.1696	0.2363	0.3012	0.5745	1.2816	2605.092
							7	0.1660	0.2379	0.2998	0.5779	1.2816	2605.590
8	0.1658	0.2376	0.2998	0.5779	1.2810	2606.258
							9	0.1682	0.2361	0.3012	0.5746	1.2800	2606.725
10	0.1679	0.2357	0.3008	0.5746	1.2789	2607.665

Whole optimization motion algorithm sample calculation is as follows, has 5 presets (see Fig. 3 a, stain marks) now.Do not add optimization, be 0.5,0.5,0.25,1.25 second the working time between the each point.Optimize the back as can be known, adopt and optimize gained to execute whole orbit segment required time after the time interval be 1.2848 seconds, be initial time half of following T.T. at interval from Fig. 3 b.From Fig. 4 b as can be known total consumed power be 2606 joules, equally little than the performance number of initial time under at interval.Although it is fast more a lot of than initial motion to optimize the gained motion, the speed in each joint and moment values all do not rise a lot, all in allowed band, see Fig. 5 a, 5b, Fig. 7 a and 7b.As can be known under initial parameter, the acceleration of motion starting point and terminating point all is not 0 by Fig. 6 a, is infinitely great thereby cause acceleration.To produce impact to system.And after adopting the parameters optimization of improving cubic spline calculating gained, starting point and terminating point accekeration are 0, and whole motion process is enough level and smooth.

Claims (1)

1. movement locus planing method in the heavily loaded industrial robot operating space, it is characterized in that: the detailed process of described planing method is:

Steps A, the splines of adopt revising at multiple spot in the space are carried out interpolation:

Steps A 1, employing cubic spline curve are set up basic interpolation function, adopt cubic polynomial to carry out interpolation between any two presets, and the expression formula of basic interpolation function is as follows:

S _j(t)＝a _j+b _j(t-t _j)+c _j(t-t _j) ²+d _j(t-t _j) ³

t∈[t _j，t _j+1]，j＝0，1，...，n-2 (1)

The time interval between two adjacent presets is h _j=t _J+1-t _ja _j, b _j, c _j, d _jBe coefficient to be asked; N represents preset quantity;

Steps A 2, while are assurance continuity letter of guarantee numerical value S in each preset _j(t) first derivative values S ' _j(t) and functional value S _j(t) second derivative value S " _j(t) equate, be:

S _j(t _j)＝α _j，j＝0，1，...，n-2

S _n-2(t _n-1)＝α _n-1 (2)

α _jRepresent each preset coordinate figure;

$\left\{\begin{array}{c}{S}_j\left(t_{j+1}\right)=S_{j+1}\left(t_{j+1}\right)\\ S_j^{\&prime;}\left(t_{j+1}\right)=S_{j+1}^{\&prime;}\left(t_{j+1}\right)\\ S_j^{\&prime;\&prime;}\left(t_{j+1}\right)=S_{j+1}^{\&prime;\&prime;}\left(t_{j+1}\right)\end{array}\right.,j=\mathrm{0,1},...,n-3---\left(3\right)$

Steps A 3, employing fixed boundary condition, satisfying in starting point and terminating point speed is 0:

S′ ₀(t ₀)＝0，S′ _n-2(t _n-1)＝0 (4)

Steps A 3, to set up cubic spline Total tune equation as follows:

H·C＝S (5)

In the formula:

$C=\left[\begin{array}{c}{S}_0^{\&prime;\&prime;}\left(t_0\right)\\ S_1^{\&prime;\&prime;}\left(t_1\right)\\ .\\ .\\ .\\ S_{n-3}^{\&prime;\&prime;}\left(t_{n-3}\right)\\ S_{n-2}^{\&prime;\&prime;}\left(t_{n-2}\right)\\ S_{n-2}^{\&prime;\&prime;}\left(t_{n-1}\right)\end{array}\right],$ $S=\left[\begin{array}{c}\frac{S_0\left(t_1\right)-S_1\left(t_0\right)}{h_1{h}_0}(h_1+h_0)\\ \frac{S_2-S_1}{h_1}-\frac{S_1-S_0}{h_0}\\ .\\ .\\ .\\ \frac{S_{n-2}-S_{n-3}}{h_{n-2}}-\frac{S_{n-3}-S_{n-4}}{h_{n-3}}\\ \frac{S_0\left(t_{n-1}\right)-S_1\left(t_{n-2}\right)}{h_n{h}_0}(h_n+h_0)\end{array}\right]$

Steps A 4, formula (1) is revised, in The initial segment with stop section and increase auxiliary, revised batten expression formula:

S ₀(t)＝a ₀+b ₀(t-t ₀)+c ₀(t-t ₀) ²+d ₀(t-t ₀) ³+A ₀(t)

S _n-2(t)＝a _n-2+b _n-2(t-t _n-2)+c _n-2(t-t _n-2) ²+d _n-2(t-t _n-2) ³+A _n-2(t) (6)

N preset quantity has n-1 spacer, and n-1 expression formula promptly arranged; The expression formula of The initial segment is S ₀(t), the expression formula of termination section is S _N-2(t); A ₀(t), A _N-2(t) be the auxiliary item of polynomial form;

Steps A 5, auxiliary the A that finds the solution polynomial form ₀(t) and A _N-2(t) auxiliary expression formula:

A ₀(t) and A _N-2(t) must satisfy following equation of constraint:

$\left\{\begin{array}{c}{A}_0\left(t_0\right)=A_0\left(t_1\right)=0\\ A_0^{\&prime;}\left(t_0\right)=A_0^{\&prime;}\left(t_1\right)=0\\ A_0^{\&prime;\&prime;}\left(t_0\right)=-S_0^{\&prime;\&prime;}\left(t_0\right)\\ A_0^{\&prime;\&prime;}\left(t_1\right)=0\\ A_{n-2}\left(t_{n-2}\right)=A_{n-2}\left(t_{n-1}\right)=0\\ A_{n-2}^{\&prime;}\left(t_{n-2}\right)=A_{n-2}^{\&prime;}\left(t_{n-1}\right)=0\\ A_{n-2}^{\&prime;\&prime;}\left(t_{n-2}\right)=-S_{n-2}^{\&prime;\&prime;}\left(t_{n-2}\right)\\ A_{n-2}^{\&prime;\&prime;}\left(t_{n-1}\right)=0\end{array}\right.---\left(7\right)$

Finding the solution above-mentioned equation, can to get the auxiliary expression formula as follows:

$A_0\left(t\right)=S_0^{\&prime;\&prime;}\left(t_0\right){\left({t-t}_0\right)}^2{\left({t-t}_1\right)}^3/\left({2h}_0^3\right)$

(8)

$A_{n-2}\left(t\right)=S_{n-2}^{\&prime;\&prime;}\left(t_{n-2}\right){(t-t_{n-2})}^2{\left({t-t}_{n-1}\right)}^3/\left({2h}_{n-2}^3\right)---\left(8\right)$

Step B, by to the each point working time of adjustment at interval, by setting up corresponding Optimization Model, make heavily loaded industrial robot execute whole segment of curve motion with the performance of optimum:

Step B1, select the execution time h between each set point ₀, h ₁..., h _N-1As parameters optimization, be total to n optimization variable;

Step B2, with the execution energy consumption P in the whole track process and execution time T as optimization aim;

Step B3, with motor speed, moment and acceleration as constraint condition;

Step B4, formula (1), formula (6) and formula (8) formation base interpolating function after basic interpolating function is determined, are converted into following Optimization Model with whole motion planning:

$\min \left\{\begin{array}{c}T=\underset{j=0}{\overset{n-1}{\mathrm{\&Sigma;}}}{h}_i\\ P=\underset{j=0}{\overset{n-1}{\mathrm{\&Sigma;}}}\underset{i=1}{\overset{m}{\mathrm{\&Sigma;}}}({\&Integral;}_{t_j}^{t_{j+1}}{\mathrm{\&tau;}}_i\left(\mathrm{\&theta;}\right)\&CenterDot;d{\mathrm{\&theta;}}_i)\end{array}\right.---\left(9\right)$

M represents joint of robot quantity, and T represents the execution time, and P represents to carry out energy consumption; θ represents the corner vector that each joint rotation angle of robot is formed, θ _iExpression i axle joint rotation angle;

It is as follows to set up constraint condition:

$s.t.\left\{\begin{array}{c}\max \left(\right|{\mathrm{\&tau;}}_i\left|\right)<{\mathrm{\&tau;}}_{i\max }\\ \max \left(\right|v_i\left|\right)<v_{i\max }\\ \max \left(\right|J_i\left|\right)<J_{i\max }\end{array}\right.---\left(10\right)$

τ _iExpression i axle moment, v _iExpression i axle speed, J _iExpression i axle acceleration, τ _ImaxThe expression i axle moment upper limit, v _ImaxExpression i axle speed limit, J _ImaxThe expression i axle acceleration upper limit;

Step C, finish above each step after, adopt the non-domination genetic algorithm of NSGAII type to parameters optimization h ₀, h ₁..., h _N-1Find the solution;

Step D, the parameters optimization h that step C is obtained ₀, h ₁..., h _N-1Substitution formula (5), and then try to achieve and wait to ask coefficient a _j, b _j, c _j, d _jDisaggregation, the final sectional type basis interpolation function determined by formula (1) and formula (2) of obtaining, the basic interpolation function of described sectional type is in order to express the movement locus in the heavily loaded industrial robot operating space.

Images