Smooth track generation method

Abstract

The invention provides a method for generating a smooth track, which belongs to the technical field of industrial robots and comprises the following steps: S1, constructing a target function; s2, adopting a quintic B spline curve Bp (t) as a planning curve to be solved; s3, determining a boundary constraint condition and a motion constraint condition; and S4, substituting a quintic spline curve Bq (t), generating a final optimal objective function in combination with the boundary constraint condition and the motion constraint condition, and solving an optimal solution of the optimal objective function, namely a planned smooth trajectory. According to the method, a new optimal objective function based on a quintic B-spline curve is provided, the execution time is adjusted and optimized, and various motion constraints are considered.

Description

Technical Field

[0001] The present invention relates to the technical field of industrial robots, and particularly relates to a method for generating a smooth trajectory. Background Art

[0002] A fundamental technical problem in the field of robots is trajectory planning. How to plan an efficient, smooth and robot motion constraint-satisfying trajectory has always been a core problem in this field. This problem is usually transformed into an optimization problem, such as minimizing the execution time, minimizing the jerk, minimizing the energy consumption, etc.

[0003] Existing optimization problems usually have two defects: the execution time is given and motion constraints are usually not considered. The above two defects are caused by the technical method limitations of the optimization problem. Summary of the Invention

[0004] The purpose of the present invention aims to solve at least one of the above technical defects.

[0005] To this end, the purpose of the present invention is to propose a method for generating a smooth trajectory. The method proposes a new optimization objective function based on a fifth-degree B-spline curve, tunes the execution time, and takes into account various motion constraints.

[0006] To achieve the above purpose, an embodiment of the present invention provides a method for generating a smooth trajectory, including the following steps:

[0007] Step S1, construct an objective function;

[0008] Step S2, use the fifth-degree B-spline curve Bp(t) as the planned curve to be solved;

[0009] Step S3, determine the boundary constraint conditions and motion constraint conditions;

[0010] Among them, the formula of the boundary constraint conditions is as follows:

[0011]

[0012] Among them, CPQ j,i represents the component of the i-th control point on the j-axis, which is an unknown quantity; Bj is the component of the vp passing points on the j-axis, A is the transformation matrix between the control points and the passing points, and both A and Bj are known quantities;

[0013] The formula of the motion constraint conditions is as follows:

[0014] |CPV j，k |≤VC j , k = 1,..., n

[0015] |CPA j，k |≤WCj , k = 1, ..., n - 1

[0016] |CPJ j,k | ≤ JC j , k = 1, ..., n - 2;

[0017] Step S4, substitute the 5 - th order spline curve Bq(t), and combine the boundary constraint conditions and motion constraint conditions to generate the final optimization objective function. Solving the optimal solution of the optimization objective function gives the planned smooth trajectory.

[0018] Furthermore, in the step S1, given Vp passing points in the robot axis space, find a curve q(t) that meets the following conditions, where q is an axis - space vector, such that

[0019]

[0020] where k T , k J is the weight coefficient; N is the number of axes of the robot; h i is the execution time from the i - th passing point to the (i + 1) - th passing point; t f is the total execution time; are the velocity, acceleration, and jerk of the j - axis respectively; VC j , WC j , JC j are the constraints on the velocity, acceleration, and jerk of the j - axis respectively.

[0021] Furthermore, in the step S2, the expression of the B - spline curve Bp(t) is as follows:

[0022]

[0023] with

[0024]

[0025] where k = p + 1 and m = n + p + 1.

[0026] where Q i is the i - th control point, and N i，p is the i - th p - th order basis polynomial.

[0027] Furthermore, in the step S3, the motion constraint conditions directly act on the upper limits of velocity, acceleration, and jerk.

[0028] Furthermore, in the step S4,

[0029] Substitute the 5th - order B - spline curve \(B_q(t)\), consider the boundary constraint conditions and motion constraint conditions, and the final optimization objective function is:

[0030]

[0031] With:

[0032]

[0033] \(\vert CPV\) j，k \(\vert\leq VC\) j , \(k = 1,\cdots,n\)

[0034] \(\vert CPA\) j，k \(\vert\leq WC\) j , \(k = 1,\cdots,n - 1\)

[0035] \(\vert CPJ\) j，k \(\vert\leq JC\) j , \(k = 1,\cdots,n - 2\)

[0036] Among them, the optimization variable is \(CPQ_{j,k}\); \(CPQ\) j，k represents the component of the \(k\) - th control point in the \(j\) - axis of the position; \(CPV\) j，k represents the component of the \(k\) - th control point in the \(j\) - axis of the velocity; \(CPA\) j，k represents the component of the \(k\) - th control point in the \(j\) - axis of the acceleration; \(CPJ\) j，k represents the component of the \(k\) - th control point in the \(j\) - axis of the jerk; \(N\) i，p is the \(i\) - th \(p\) - th order basis polynomial of the B - spline curve;

[0037] Solving the optimal solution of the said optimization objective function is the planned smooth trajectory.

[0038] According to the method for generating a smooth trajectory of the embodiment of the present invention, based on the new optimization objective function of the 5th - order B - spline curve, the execution time is optimized, and various motion constraints are considered. The form of the objective function for trajectory planning designed by the present invention is a combination of the execution time and the square of the jerk. The curve family to be solved uses the 5th - order B - spline curve and optimization. The present invention does not need to specify the execution time and considers the motion constraints of the robot, and the planned curve is smoother and more efficient.

[0039] The additional aspects and advantages of the present invention will be partially given in the following description, partially become obvious from the following description, or be understood through the practice of the present invention. Brief Description of the Drawings

[0040] The above - mentioned and / or additional aspects and advantages of the present invention will become obvious and easy to understand from the description of the embodiments in conjunction with the following drawings, where:

[0041] Figure 1 Flow chart of a method for generating a smooth trajectory according to an embodiment of the present invention. Detailed implementation manners

[0042] Embodiments of the present invention will be described in detail below. Examples of the embodiments are shown in the accompanying drawings, where the same or similar reference numerals denote the same or similar elements or elements having the same or similar functions throughout. The embodiments described below with reference to the accompanying drawings are exemplary and are intended to explain the present invention, and should not be construed as limiting the present invention.

[0043] As Figure 1 shown, the method for generating a smooth trajectory according to an embodiment of the present invention includes the following steps:

[0044] Step S1, construct an objective function.

[0045] Given Vp points (including the first and last points) in the robot axis space, to obtain a curve q(t) that minimizes the sum of the execution times of each axis of the robot and is smooth, where q is a vector in the axis space. Therefore, the following optimization problem is established:

[0046]

[0047] The first term of the objective function of the above optimization problem is the sum of the execution times of each axis, and the second term is the smoothness term of the curve.

[0048] Where k T , k J is a weight coefficient; N is the number of axes of the robot; h i is the execution time from the i-th passing point to the (i + 1)-th passing point; t f is the total execution time; the constraints include the velocity, acceleration, and jerk terms of the j-axis. Among them, are the velocity, acceleration, and jerk of the j-axis respectively; VC j , WC j , JC j are the constraints on the velocity, acceleration, and jerk of the j-axis respectively.

[0049] Step S2, use a fifth-order B-spline curve Bp(t) as the planned curve to be solved.

[0050] The B-spline curve is a piecewise polynomial, uniquely determined by its degree p = k - 1, a set of N + 1 control points, and a knot vector t = {t0, t1,..., tm}. These control points are used to define the trend and boundary range of the spline curve. The B-spline curve is parameterized by time t. Here, a fifth-order B-spline curve Bp(t) is used as the planned curve to be solved, and its expression is as follows:

[0051]

[0052] with

[0053]

[0054] where k = p + 1 and m = n + p + 1. (2)

[0055] Among them, Qi is the i-th control point, and Ni,p is the i-th p-th degree basis polynomial. The B-spline curve has very important properties such as convex hull property and high-order invariance. The convex hull property means that the B-spline curve is restricted within the convex hull composed of control points; the high-order invariance means that the differential of the B-spline curve is also a B-spline curve. Combining these two properties, the restrictions on the curve and its velocity and acceleration can be transformed into restrictions on the control points.

[0056] Step S3, determine the boundary constraint conditions and motion constraint conditions;

[0057] Among them, the formula for the boundary constraint conditions is as follows:

[0058]

[0059] Among them, CPQj,i represents the component of the i-th control point on the j-axis, which is an unknown quantity; Bj is the component of vp points (including the start and end points) on the j-axis, and A is the transformation matrix between the control points and the passing points. Both A and Bj are known quantities.

[0060] The motion constraint conditions directly act on the upper limits of velocity, acceleration, and jerk. Due to the convex hull property and high-order invariance of the B-spline curve, the motion constraints on the curve are transformed into constraints on the control points. The formula is as follows:

[0061] |CPV j，k | ≤ VC j , k = 1,..., n

[0062] |CPA j，k | ≤ WC j , k = 1,..., n - 1

[0063] |CPJ j,k | ≤ JC j , k = 1,..., n - 2 (4)

[0064] Step S4, establish the complete objective function and constraint conditions. Substitute the 5th-degree spline curve Bq(t), and combine the boundary constraint conditions and motion constraint conditions to generate the final optimization objective function. Solving the optimal solution of the optimization objective function is the planned smooth trajectory.

[0065] Substitute the 5th - order B - spline curve \(B_q(t)\), consider the boundary constraint conditions and motion constraint conditions, and the final optimization objective function is:

[0066]

[0067] With:

[0068]

[0069] \(\vert CPV\) j，k \(\vert\leq VC\) j , \(k = 1,\cdots,n\)

[0070] \(\vert CPA\) j，k \(\vert\leq WC\) j , \(k = 1,\cdots,n - 1\)

[0071] \(\vert CPJ\) j，k \(\vert\leq JC\) j , \(k = 1,\cdots,n - 2(6)\)

[0072] Among them, the optimization variables are \(CPQ_{j,k}\); \(CPQ\) j，k represents the \(j\) - axis component of the \(k\) - th control point of the position; \(CPV\) j，k represents the \(j\) - axis component of the \(k\) - th control point of the velocity; \(CPA\) j，k represents the \(j\) - axis component of the \(k\) - th control point of the acceleration; \(CPJ\) j，k represents the \(j\) - axis component of the \(k\) - th control point of the jerk; \(N\) i，p is the \(i\) - th \(p\) - th order basis polynomial of the B - spline curve;

[0073] Solving the optimal solution of the optimization objective function is the planned smooth trajectory.

[0074] According to the method for generating a smooth trajectory of the embodiment of the present invention, based on the new optimization objective function of the 5th - order B - spline curve, the execution time is optimized, and various motion constraints are considered. The form of the objective function for trajectory planning designed by the present invention is a combination of the execution time and the square of the jerk. The curve family to be solved is the 5th - order B - spline curve and optimization. The present invention does not need to specify the execution time, and considering the motion constraints of the robot, the planned curve is smoother and more efficient.

[0075] In the description of this specification, the descriptions referring to terms such as "one embodiment", "some embodiments", "examples", "specific examples", or "some examples", etc., mean that the specific features, structures, materials, or characteristics described in connection with the embodiment or example are included in at least one embodiment or example of the present invention. In this specification, the schematic representations of the above terms do not necessarily refer to the same embodiment or example. Moreover, the specific features, structures, materials, or characteristics described may be combined in a suitable manner in any one or more embodiments or examples.

[0076] Although the embodiments of the present invention have been shown and described above, it can be understood that the above embodiments are exemplary and should not be construed as limiting the present invention. Those of ordinary skill in the art can make changes, modifications, substitutions, and variations to the above embodiments within the scope of the present invention without departing from the principles and spirit of the present invention. The scope of the present invention is defined by the appended claims and their equivalents.

Claims

1. A method for generating a smooth trajectory, characterized in that: The steps include: Step S1, constructing an objective function; Step S2, using a 5th-order B-spline curve Bp(t) as the planning curve to be solved; Step S3, determining boundary constraints and motion constraints; The formula of the boundary constraint condition is as follows: Among them, CPQ j,i represents the component of the i-th control point on the j-axis, which is an unknown quantity; Bj is the component of the vp passing points on the j-axis, A is the transformation matrix between the control point and the passing point, and both A and Bj are known quantities; The formula of the motion constraint condition is as follows: Step S4, substitute the 5th-order spline curve Bq(t), and combine the boundary constraints and motion constraints to generate the final optimization objective function, and the optimal solution of the optimization objective function is the planned smooth trajectory.

2. The method for generating a smooth trajectory according to claim 1, characterized in that: In step S1, given Vp passing points in the robot axis space, a curve q(t) that meets the following conditions is searched, where q is an axis space vector such that Among them, k T ,k J is the weight coefficient; N is the number of axes of the robot; h i is the execution time from the i-th passing point to the i+1-th passing point; t f is the total execution time; are the velocity, acceleration and jerk of the j-axis respectively; VC j , W.C. j ,JC j They are the velocity constraint, acceleration constraint and jerk constraint of the j-axis respectively.

3. The method for generating a smooth trajectory according to claim 1, characterized in that: In step S2, the expression of the B-spline curve Bp(t) is as follows: with where k=p+1 and m=n+p+1. Among them, Q i is the i-th control point, N i,p is the i-th p-degree basis polynomial.

4. The method for generating a smooth trajectory according to claim 1, wherein: In step S3, the motion constraints act directly on the upper limits of velocity, acceleration and jerk.

5. The method for generating a smooth trajectory according to claim 1, characterized in that: In step S4, Substituting the 5-degree spline curve Bq(t), considering the boundary constraints and motion constraints, the final optimization objective function is: With: |CPV j，k |≤VC j ,k=1,...,n |CPA j，k |≤WC j ,k=1,...,n-1 |CPJ j，k |≤JC j ,k=1,...,n-2 Among them, the optimization variables are CPQj,k; CPQ j,k Represents the component of the k-th control point on the j-axis; CPV j,k Represents the velocity component of the kth control point on the j axis; CPA j,k Represents the component of acceleration of the kth control point on the j axis; CPJ j,k represents the component of the acceleration of the kth control point on the j axis; N i,p is the i-th p-th base polynomial of the B-spline curve; The optimal solution of the optimization objective function is the planned smooth trajectory.

Images