Disclosure of Invention
In order to overcome the defects in the prior art, the invention provides the industrial robot path fairing method and system with continuous curvature, which adopt the quintic Bezier curve containing the shape adjusting parameters, realize the continuous curvature when the arc section path and other types of paths are switched, and improve the motion stability of the industrial robot when passing through different paths.
The technical scheme adopted by the method is as follows: a method of fairing an industrial robot path with a continuous curvature, comprising the steps of:
reading a path of a given industrial robot, and calculating curvatures at all position points on the path;
dividing the path into a curvature abrupt change region and a curvature continuous region according to whether the curvature is abrupt;
for the curvature abrupt change region, calculating the curvature, unit tangent vector and unit normal vector at two endpoints of the region;
defining a quintic Bezier curve containing shape adjustment parameters for a fairing curvature mutation zone;
defining an objective function of a shape adjustment parameter in the quintic Bezier curve according to boundary conditions at the end points and according to a curvature change energy and length energy composite minimum principle;
converting the shape adjustment parameter solving problem into a minimized objective function problem, respectively solving partial derivatives of the shape adjustment parameters through the objective function, and solving the objective function by adopting a quasi-Newton method to obtain the numerical value of the shape adjustment parameters;
substituting the numerical value of the shape adjusting parameter into the quintic Bezier curve to obtain the fairing path of the curvature mutation region.
The technical scheme adopted by the system of the invention is as follows: an industrial robot path fairing system with continuous curvature comprising the following modules:
the curvature calculation module is used for reading the path of the given industrial robot and calculating the curvatures at all the position points on the path;
the abrupt change area dividing module is used for dividing the path into a curvature abrupt change area and a curvature continuous area according to whether the curvature is abrupt or not;
the abrupt region processing module is used for calculating the curvature, the unit tangent vector and the unit normal vector at the two end points of the curvature abrupt region;
the Bezier curve definition module is used for defining a five-order Bezier curve fairing curvature mutation zone containing shape adjustment parameters; defining an objective function of a shape adjustment parameter in the quintic Bezier curve according to boundary conditions at the end points and according to a curvature change energy and length energy composite minimum principle;
the shape adjustment parameter solving module is used for converting the shape adjustment parameter solving problem into a minimized objective function problem, respectively solving partial derivatives of the shape adjustment parameters through the objective function, and solving the objective function by adopting a quasi-Newton method to obtain the numerical value of the shape adjustment parameters; substituting the numerical value of the shape adjusting parameter into the quintic Bezier curve to obtain the fairing path of the curvature mutation region.
Compared with the prior art, the invention has the following technical effects:
the invention adopts the quintic Bezier curve containing the shape adjusting parameters, realizes the curvature continuity when the arc section path and other types of paths are switched, realizes the smooth transition of the robot path curvature continuity, and improves the motion stability of the industrial robot when passing through different paths.
Detailed Description
The technical scheme of the present invention will be described in further detail with reference to examples and drawings, but embodiments of the present invention are not limited thereto.
Example 1
The embodiment provides a fairing method with continuous curvature when an industrial robot arc path and an arc path transition, as shown in fig. 1, the method comprises the following main steps:
step 1, reading a path of a given industrial robot, and calculating curvatures at all position points on the path;
the path c (t) of the industrial robot comprises two arc segment paths, namely c1 (t) and c2 (t) the circle centers are respectively marked as C1 And C2 The radii are denoted as r respectively1 And r2 Each circular arc segment path comprises 300 position points Pi (xi ,yi ,zi ) Where i=1, 2,3 …, N, xi 、yi 、zi Coordinates of the ith position point on the x-axis, the y-axis and the z-axis are respectively, N is the number of position points included in the path, and N is 300 in the embodiment; the non-smooth pose path is shown in fig. 2.
Calculating the curvature k (t) at all the position points on the two circular arc path sections:
where c' (t) is the first derivative of path c (t), c "(t) is the second derivative of path c (t), t is the normalized parameter variable, t ε [0,1].
Step 2, dividing the path into a curvature abrupt change area and a curvature continuous area according to whether the curvature is abrupt;
the invention uses the contact point P of two arc segment pathsj The point is defined as the point of abrupt change of curvatureThe embodiment normalizes the parameter variable t E [0.9,1 ]]The included arc segment path is divided into curvature abrupt change areas.
Step 3, calculating the curvature, unit tangent vector and unit normal vector of the curvature mutation region at two end points of the region;
the location points of the two end points of the curvature mutation region are denoted as p0 And p1 Obtaining the curvature k of two endpoints of the curvature abrupt change region according to the curvature calculation formula in thestep 10 And k1 The method comprises the steps of carrying out a first treatment on the surface of the Contact point P of two arc segment pathsj And two end points p of curvature mutation region0 And p1 Respectively calculating two arc segment paths c1 (t) and c2 (T) tangential vectors at both end points, and performing unitization processing on the tangential vectors to obtain a unitized tangential vector T0 And T1 The method comprises the steps of carrying out a first treatment on the surface of the The two unit tangential vectors are respectively rotated by 90 degrees anticlockwise to obtain two normal vectors, and the two normal vectors are respectively subjected to unitization treatment to obtain a unit normal vector N0 And N1 。
That is, the unit tangent vector T in the present embodiment0 And T1 The vector unitization is obtained by two endpoints of the curvature mutation region and the curvature mutation point; the unit normal vector is denoted as N0 And N1 Is obtained by rotating the unit tangent vector by 90 degrees anticlockwise
Step 4, defining a quintic Bezier curve containing shape adjustment parameters for a fairing curvature mutation zone;
defining a quintic Bezier curve b (t) containing shape adjustment parameters as:
b(t)=b0 B0 (t)+b1 B1 (t)+b2 B2 (t)+b3 B3 (t)+b4 B4 (t)+b5 B5 (t)
wherein b0 、b1 、b2 、b3 、b4 And b5 As control points, the expression of each control point is:
wherein alpha is0 、α1 、β0 、β1 The shape adjusting parameter is larger than zero and is used for adjusting the tangential directions of the starting point and the end point of the spline curve and the positions of the control points;
B0 (t)、B1 (t)、B2 (t)、B3 (t)、B4 (t) and B5 (t) is a control point Bernstant polynomial of the formula:
step 5, defining an objective function of a shape adjustment parameter in the quintic Bezier curve according to boundary conditions at the end points and according to a curvature change energy and length energy composite minimum principle;
the objective function is defined as:
where b '"(t) is the third derivative of b (t) and b' (t) is the first derivative of b (t).
Step 6, converting the problem of solving the shape adjusting parameters into a minimized objective function problem, respectively solving partial derivatives of the shape adjusting parameters through the objective function, and solving the objective function by adopting a quasi-Newton method to obtain the numerical value of the shape adjusting parameters;
the minimization of the objective function problem is: minf (alpha)0 ,α1 ,β0 ,β1 );
The partial derivatives of the shape adjustment parameters are respectively calculated through the objective function, and the method comprises the following steps:
and->
Solving an objective function by adopting a quasi-Newton method, comprising the following steps of:
given an initial value
And a stop condition epsilon; wherein the superscript 0 denotes alpha at
iteration 0
0 ,α
1 ,β
0 ,β
1 Is set to an initial value of (1);
setting a hessian matrix Hk Initial value H of0 Is a unit matrix and calculates the gradient g of the objective functionk :
Determining a search direction dk =Hk gk ;
Updating shape adjustment parameters
Wherein the superscript k denotes alpha at the kth iteration
0 ,α
1 ,β
0 ,β
1 Is a value of (2); the superscript k+1 denotes α at the k+1th iteration
0 ,α
1 ,β
0 ,β
1 Is a value of (2); lambda (lambda)
k Representing step size factor, by solving->
Lambda at the time of obtaining the minimum value;
judging
If so, outputting a shape adjustment parameter, otherwise, updating the gradient of the hessian matrix and the objective function:
and performs the next calculation of the shape adjustment parameters.
And 7, substituting the numerical value of the shape adjusting parameter into the quintic Bezier curve to obtain the fairing path of the curvature mutation region.
Fig. 2 shows a path before the two circular arc segment paths are straightened in this embodiment, and fig. 3 shows a curvature change condition after the two circular arc segment paths are straightened in this embodiment. It can be seen that after the path fairing is performed by adopting the method of the embodiment, the curvatures of the two transition path sections are equal to the curvatures of the two arc path sections respectively, as shown in fig. 4, so that the continuous change of the path curvature after the fairing is ensured, and therefore, the movement speed of the robot can not be reduced to zero, thereby improving the movement efficiency and ensuring the movement stability.
Example 2
The present embodiment provides a path fairing method with continuous curvature when the arc path and the straight path of the industrial robot are transited, and the main steps are basically the same as those ofembodiment 1. Fig. 5 shows a path before a circular arc section and a straight path section of the present embodiment, fig. 6 shows a path after a circular arc section and a straight path section of the present embodiment, fig. 7 shows a curvature change condition after a circular arc section and a straight path section of the present embodiment are straightened, a curvature of the path after the straightening continuously changes, a movement speed of the robot may not be reduced to zero, a movement efficiency of the robot is improved, and a movement stability of the robot is also ensured.
Example 3
The present embodiment, based on the same inventive concept asembodiment 1 andembodiment 2, provides an industrial robot path fairing system with continuous curvature, which includes the following modules:
the curvature calculation module is used for reading the path of the given industrial robot and calculating the curvatures at all the position points on the path;
the abrupt change area dividing module is used for dividing the path into a curvature abrupt change area and a curvature continuous area according to whether the curvature is abrupt or not;
the abrupt region processing module is used for calculating the curvature, the unit tangent vector and the unit normal vector at the two end points of the curvature abrupt region;
the Bezier curve definition module is used for defining a five-order Bezier curve fairing curvature mutation zone containing shape adjustment parameters; defining an objective function of a shape adjustment parameter in the quintic Bezier curve according to boundary conditions at the end points and according to a curvature change energy and length energy composite minimum principle;
the shape adjustment parameter solving module is used for converting the shape adjustment parameter solving problem into a minimized objective function problem, respectively solving partial derivatives of the shape adjustment parameters through the objective function, and solving the objective function by adopting a quasi-Newton method to obtain the numerical value of the shape adjustment parameters; substituting the numerical value of the shape adjusting parameter into the quintic Bezier curve to obtain the fairing path of the curvature mutation region.
Wherein, the defined quintic Bezier curve b (t) containing the shape adjusting parameters is:
b(t)=b0 B0 (t)+b1 B1 (t)+b2 B2 (t)+b3 B3 (t)+b4 B4 (t)+b5 B5 (t)
wherein b0 、b1 、b2 、b3 、b4 And b5 As control points, the expression of each control point is:
B0 (t)、B1 (t)、B2 (t)、B3 (t)、B4 (t) and B5 (t) is the control point, the Bernstan polynomial:
wherein p is0 And p1 The two end points of the curvature mutation zone are position points; alpha0 、α1 、β0 And beta1 The shape adjusting parameter is larger than zero and is used for adjusting the tangential directions of the starting point and the end point of the spline curve and the positions of the control points; k (k)0 And k1 Is the curvature of the two endpoints of the curvature abrupt region.
The shape adjustment parameter obtaining module is used for defining an objective function of the shape adjustment parameter in the quintic Bezier curve according to boundary conditions at the end points and the curvature change energy and length energy composite minimum principle, wherein the objective function is as follows:
wherein b '"(t) is the third derivative of b (t) and b' (t) is the first derivative of b (t);
the shape adjustment parameter solving module converts the shape adjustment parameter solving problem into a minimized objective function problem, respectively solves partial derivatives of the shape adjustment parameters through the objective function, solves the objective function by adopting a quasi-Newton method to obtain the numerical value of the shape adjustment parameters, and comprises the following steps:
the minimization of the objective function problem is: minf (alpha)0 ,α1 ,β0 ,β1 )
The partial derivatives of the shape adjustment parameters are respectively calculated through the objective function, and the method comprises the following steps:
and->
Solving an objective function by adopting a quasi-Newton method, comprising the following steps of:
given an initial value
And a stop condition epsilon;
setting the initial value H of the Heisen matrix
0 Calculating the gradient of the objective function as an identity matrix
Determining a search direction dk =Hk gk Wherein H isk Is a hessian matrix;
updating shape adjustment parameters
Wherein the superscript k denotes alpha at the kth iteration
0 ,α
1 ,β
0 ,β
1 The superscript k+1 denotes α at the k+1th iteration
0 ,α
1 ,β
0 ,β
1 Is a value of (2); lambda (lambda)
k Representing step size factor, by solving->
Lambda at the time of obtaining the minimum value;
judging
If so, outputting a shape adjustment parameter, otherwise, updating the gradient of the hessian matrix and the objective function:
and performs the next shape adjustment parameter calculation.
Please refer toembodiment 1 andembodiment 2 for the implementation process of each module in this embodiment, which is not repeated.
Although embodiments of the present invention have been shown and described above, it will be understood that the above embodiments are illustrative and not to be construed as limiting the invention, and that variations, modifications, alternatives, and variations may be made in the above embodiments by those skilled in the art without departing from the spirit and principles of the invention.