CN109664303B - Error-controllable B-spline transition type smooth trajectory generation method for four-axis industrial robot - Google Patents

Error-controllable B-spline transition type smooth trajectory generation method for four-axis industrial robot Download PDF

Info

Publication number
CN109664303B
CN109664303B CN201910151081.8A CN201910151081A CN109664303B CN 109664303 B CN109664303 B CN 109664303B CN 201910151081 A CN201910151081 A CN 201910151081A CN 109664303 B CN109664303 B CN 109664303B
Authority
CN
China
Prior art keywords
track
spline
point
transition
error
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Active
Application number
CN201910151081.8A
Other languages
Chinese (zh)
Other versions
CN109664303A (en
Inventor
何姗姗
颜昌亚
李振瀚
邓炎超
黄昆涛
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Wuhan Institute of Technology
Original Assignee
Wuhan Institute of Technology
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Wuhan Institute of Technology filed Critical Wuhan Institute of Technology
Priority to CN201910151081.8A priority Critical patent/CN109664303B/en
Publication of CN109664303A publication Critical patent/CN109664303A/en
Application granted granted Critical
Publication of CN109664303B publication Critical patent/CN109664303B/en
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
    • B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
    • B25J9/00—Programme-controlled manipulators
    • B25J9/16—Programme controls
    • B25J9/1656—Programme controls characterised by programming, planning systems for manipulators
    • B25J9/1664—Programme controls characterised by programming, planning systems for manipulators characterised by motion, path, trajectory planning

Abstract

An error-controllable B-spline transition type smooth trajectory generation method for a four-axis industrial robot comprises the following steps: step 1, track preprocessing of a four-axis industrial robot: dividing the track into a track section needing to be smoothed and a track section not needing to be smoothed, and preprocessing the four-axis posture to ensure that a minor arc track is formed between two track points; step 2, smoothing a four-axis trajectory B spline: traversing the track sections to be smoothed, which are generated in the step 1, and generating a B spline transition type smoothing track for each track section by adopting a geometric iteration method according to the track point error threshold, the position point chord height error threshold and the continuity requirement. The B-spline transition type smooth track consists of a linear track and a B-spline track, the whole track has G1 or G2 continuity with synchronous position and posture, track point errors and position chord height errors between track points are met, the track smoothness of complex application of the four-axis industrial robot can be realized by adopting the spline track, and further the working efficiency and the quality of the four-axis industrial robot are improved.

Description

Error-controllable B-spline transition type smooth trajectory generation method for four-axis industrial robot
Technical Field
The invention belongs to the field of track optimization of industrial robots, and particularly relates to an error-controllable B-spline transition type smooth track generation method for a four-axis industrial robot.
Background
Four-axis industrial robot, also known as a plane joint (SCARA) robot, has three rotation axes and a translation axis, is widely used in operations such as transport, assembly and gluing.
The motion instruction of the four-axis industrial robot is mostly consistent with that of the six-axis industrial robot and comprises a linear instruction, an arc instruction and an axis joint motion instruction. While each instruction may specify a point-accurate or smooth transition. The accurate point-reaching instruction can meet the requirement of track accuracy, but the speed of the point-reaching track must be reduced to zero, so that the operation efficiency is reduced; smoothing can smooth the transition without slowing down to zero at the trace points, but results in a reduction in the accuracy of the trace points.
The track of the complex application of the robot is mostly described by a linear instruction or an arc instruction, and a free curve or spline motion instruction is added by a certain robot manufacturer in order to improve the accuracy while improving the continuity. For example, the MOVS command of the antuan robot adopts a free curve interpolation, and the track is a parabola passing through three points. The KUKA robot adopts the concept of spline group to establish continuous straight line, circular arc or spline segment. It is easier to maintain the programmed speed than with conventional sports; however, the track error between two track points cannot be controlled by the user, and the situation that the track deviates from the original track too much may occur, thereby causing the reduction of the operation quality.
The accepted patent application 201710097192.6 proposes a method for generating a smooth motion trajectory of an industrial robot capable of controlling a position point error and a chord height error simultaneously, wherein a transition curve provides a cubic B-spline curve and a quartic B-spline curve, but the postures of the double-track smooth at the connecting point of a straight line segment and a sample bar segment are not necessarily continuous. The accepted patent application document 201811468150.X proposes an error-controllable three-dimensional track point track smoothing method, which is only suitable for continuity and shape preservation of three-dimensional position points and track smoothing meeting precision.
In the track (position and attitude, short for pose) expression of the existing four-axis industrial robot, smooth track expression which simultaneously meets high continuity (pose synchronous continuity) and high precision (meets track point errors and position chord height errors between track points) is not provided.
Disclosure of Invention
The invention aims to solve the technical problems that aiming at the defects existing in the track expression of the existing four-axis industrial robot, an error-controllable B-spline transition type smooth track generation method of the four-axis industrial robot is provided, two smooth methods of a G1 continuous spline and a G2 continuous spline are provided, and the generation and calculation of the smooth splines are simple; g1 continuity and G2 continuity capable of achieving synchronization of the whole track pose; the error of track points (position and attitude) of the four-axis industrial robot and the position point chord height error between the track points can be met; and spline instructions can be added on the basis of not changing the existing track point definition of the robot in the robot controller.
The technical scheme adopted by the invention for solving the technical problems is as follows:
an error-controllable B-spline transition type smooth trajectory generation method for a four-axis industrial robot comprises the following steps:
step 1, track preprocessing of a four-axis industrial robot: dividing the linear track into a track section needing to be smoothed and a track section not needing to be smoothed according to the position distance and the included angle; and preprocessing the posture;
step 2, smoothing the track section needing to be smoothed: traversing the sections of the track needing to be smoothed, which are generated in the step 1, and generating a continuous B spline transition type smoothing track of G1 or G2 for each section of the track needing to be smoothed by adopting a geometric iteration method according to a track point error threshold, a position point chord height error threshold and continuity requirements.
According to the scheme, the step 1 specifically comprises the following steps:
step 1.1, calculating a segmentation index set according to a segmentation threshold, inputting track segments above two continuous track points, and outputting a segmentation index; set the trajectory points of the four-axis industrial robot as input
Figure BDA0001981557190000021
The number N of the track points is more than or equal to 2, wherein each track point Pi(xi,yi,zi,θi) Is a position (x)i,yi,zi) And attitude, i.e. angle of rotation about the Z axisiComposed four-dimensional vector, position segmentation condition by position distance threshold deltadAnd angle of position threshold deltaaAs an index;
and traversing the track point index i as 1,2 and … N-1, and respectively judging whether the index i meets the position segmentation condition:
firstly, calculating a track segment Pi-1PiAnd PiPi+1The distance d between the two sectionsi-1And diIf d isi-1Or diLess than a position distance threshold deltadIf so, the index i is considered to meet the position segmentation condition; otherwise, calculating the track segment Pi-1PiAnd PiPi+1At a position angle ofiIf sin aiLess than a position angle threshold deltaaIf so, the index i is considered to meet the position segmentation condition; otherwise, the index i is considered not to meet the position segmentation condition; adding indexes i meeting the position segmentation condition into a segmentation index set, and adding indexes 0 and N from beginning to end into the beginning and the end of the segmentation index set by default;
step 1.2, segmenting according to the segmented index set, and dividing a whole track segment into a plurality of track segments according to the segmented index set, wherein the track segment with the track point number larger than 2 in the track segments is marked as a track segment needing to be smoothed and is used for smoothing the track in the next step; otherwise, marking as a section without a smooth track, and outputting the section to the smooth track according to the linear track;
step 1.3, posture preprocessing, traversing track point index i to be 1,2, … N according to the principle that minor arcs between two tracks are prior, and if two adjacent track points P arei-1,PiIs included angle distance oi-1Greater than 180 DEG, oi-1＝|θi-θi-1If P is modifiediAttitude angle of the fourth dimension of (1): if thetai>0, then is modified to Pi(xi,yi,zi,θi-360 °); otherwise, modifying to Pi(xi,yi,zi,θi+360°)。
According to the scheme, the method for generating the B-spline transition type smooth track for each section of the section needing the smooth track in the step 2 specifically comprises the following steps:
step 2.1, setting initial iteration parameters, and setting four-dimensional track points of the current linear track segment as
Figure BDA0001981557190000022
Marking as an original track point, and setting a position distance error threshold value which needs to be met by the smooth track as epsilonmaxPosition chord height error threshold is recorded as deltamaxError threshold o of included angle of attitude pointmaxThreshold value k of iteration numbermaxSetting the currentThe iteration times are k equal to 0, and the iteration track points are recorded as
Figure BDA0001981557190000031
Step 2.2, traversing the index i to be 1,2, … N-1, and respectively generating iterative track points according to position point chord height error constraint, G1 or G2 continuity constraint and conformal constraint
Figure BDA0001981557190000032
Transition B-spline trajectory of
Figure BDA0001981557190000033
First separately calculate
Figure BDA0001981557190000034
And
Figure BDA0001981557190000035
is located a distance di-1And diAnd angle of position point betaiAnd calculate di-1And diSmaller value d ofmin＝min(di-1,di)；
And then calculating according to the position point chord height error constraint and the conformal constraint
Figure BDA0001981557190000036
Transition ratio r of front and rear position pointsi-1And riFirstly, calculating the transition length of the front and rear position points, wherein for the G1 transition B spline, the transition length is as follows:
Figure BDA0001981557190000037
for the G2 transition B spline, the transition length is
Figure BDA0001981557190000038
Two transition ratios are then calculated:
Figure BDA0001981557190000039
wherein 0<α<1 is a conformal parameter, representing twoThe linear track section between the section transition B splines accounts for the distance proportion of the whole track section; c. C>1 is a proportionality coefficient for determining a control point; for G1 continuous splines
Figure BDA00019815571900000310
Then, the front three-dimension of the transition spline forms a planar PH spline; for G2 continuous splines, take c ═ 1.5;
finally, calculating control points of the four-dimensional spline according to the G1 or G2 continuous condition and the two transition ratios, wherein the transition spline meeting the G1 continuous condition has four control points; the transition spline satisfying the G2 continuous condition has five control points, and the calculation method of each control point is as follows:
g1 spline:
Figure BDA00019815571900000311
g2 spline:
Figure BDA00019815571900000312
step 2.3, calculating the ith transition B spline by traversing the index i to 1,2, … N-1
Figure BDA00019815571900000313
With the original track point QiFirstly, according to the control points in step 2.2, a four-dimensional B spline is constructed
Figure BDA00019815571900000319
B-spline curve with three uniform knots:
Figure BDA00019815571900000314
wherein t is ∈ [0,1 ]](ii) a For the G1 spline, M ═ 3; for the G2 spline, M is 4. B isj,3(t) is a B spline basis function and is obtained by calculation according to the node vector;
then calculating the parameter midpoint of the four-dimensional B spline track as the maximum error point of the track point:
Figure BDA00019815571900000315
finally, calculating transition B spline
Figure BDA00019815571900000316
With the original track point QiTrack point error, including position distance error and attitude point included angle error, based on
Figure BDA00019815571900000317
Obtaining the three-dimensional position distance and the attitude included angle distance;
step 2.4, calculating the maximum position distance error between all transition B-spline and original track point
Figure BDA00019815571900000318
Error of included angle with maximum attitude point
Figure BDA0001981557190000041
If it is
Figure BDA0001981557190000042
Less than a position distance error threshold epsilonmaxAnd is and
Figure BDA0001981557190000043
less than the error threshold o of the included angle of the attitude pointmaxOr the current iteration number k is larger than the iteration number threshold kmaxTerminating iteration and outputting B-spline smooth track
Figure BDA0001981557190000044
Turning to step 2.6; otherwise, turning to step 2.5;
step 2.5, traversing i to 1, … N-1 according to the original track point QiMaximum point of sum track point error
Figure BDA0001981557190000045
Calculating an offset vector
Figure BDA0001981557190000046
And updating iteration track points:
Figure BDA0001981557190000047
changing k to k +1, and turning to step 2.2;
step 2.6, the smooth tracks output in the step 2.4 are sorted and output, the smooth tracks are formed by combining N four-dimensional linear tracks and (N-1) four-dimensional B spline smooth tracks, and the steps are sequentially as follows: linear trajectory
Figure BDA0001981557190000048
Smooth B-spline trajectory
Figure BDA0001981557190000049
Linear trajectory
Figure BDA00019815571900000410
Smooth B-spline trajectory
Figure BDA00019815571900000411
Smooth B-spline trajectory
Figure BDA00019815571900000412
Linear trajectory
Figure BDA00019815571900000413
Through the technical scheme, compared with the prior art, the invention has the advantages that:
1. according to the arc transition type smooth track generation method of the four-axis industrial robot, a G1 continuous smooth track with synchronous position and attitude can be generated, the thought is popularized to B spline track generation of the four-axis industrial robot, a B spline transition type smooth track of the four-axis industrial robot with high continuity (G1 or G2 continuity), shape preservation and high precision can be generated, the smooth track is composed of a linear track and the B spline track, the track point precision and the position precision between track points can be met, and the G1 or G2 continuous smooth track with synchronous position and attitude is achieved;
2. compared with the existing linear track, smooth function and spline or free curve instruction of the four-axis industrial robot, the B spline transition type smooth track generated by the invention has higher continuity, can keep higher execution precision while ensuring the continuity, can reach the position and the posture of a preset track, and ensures that the position precision between track points is met, thereby improving the operation efficiency of the four-axis industrial robot and improving the operation precision;
3. the pose-synchronous G1 continuous smooth track can be generated, the pose-synchronous G2 continuous track can be generated, the B spline track can generate a smooth track with smaller curvature relative to the arc track, and the B spline track has better flexibility and continuity relative to the arc transition type smooth track.
Drawings
FIG. 1 is a B-spline one-time iteration smoothing flow chart of a four-axis industrial robot in the embodiment of the invention;
FIG. 2 is a schematic diagram of the construction of G1 continuous splines according to an embodiment of the present invention;
FIG. 3 is a schematic diagram of the construction of G2 continuous splines according to an embodiment of the present invention;
FIG. 4 is a schematic diagram illustrating the position and attitude synchronization sequence according to the embodiment of the present invention.
Detailed Description
In order to make the objects, technical solutions and advantages of the present invention more apparent, the present invention will be described in further detail below with reference to the accompanying drawings and embodiments. It should be understood that the specific embodiments described herein are merely illustrative of the invention and are not intended to limit the invention.
The invention discloses an error-controllable B-spline transition type smooth track generation method for a four-axis industrial robot, which comprises the following steps of:
step 1, track preprocessing of a four-axis industrial robot: dividing the linear track into a track section needing to be smoothed and a track section not needing to be smoothed according to the position distance and the included angle; traversing all track points of the whole track, segmenting according to the position distance and the included angle of the track, and dividing the whole track into a plurality of track segment sets; and preprocessing the four-axis attitude:
step 1.1, calculating a segmentation index set according to a segmentation threshold value, inputting track segments above two continuous track points, outputting a segmentation index, and settingThe input four-axis industrial robot has the track point set
Figure BDA0001981557190000051
(the number N of the track points is more than or equal to 2), wherein two track points Pi-1,PiThe position distance between the two is calculated by
Figure BDA0001981557190000052
Setting three adjacent track points Pi-1,Pi,Pi+1The position points of the former three-dimensional composition of (1) are respectively pi-1,pi,pi+1(ii) a If d isi-1diNot equal to 0, the included angle of the three position points is
Figure BDA0001981557190000053
Position segmentation condition by position distance threshold deltadAnd adjacent position angle threshold deltaaAs an index, according to the fact that the position segmentation is consistent with a three-dimensional point segmentation method of an accepted patent application (201811468150.X), traversing a track point index i to be 1,2 and … N-1, and respectively judging whether the index i meets the position segmentation condition; if d isi-1<δdOr di<δdIf the index i meets the position segmentation condition, otherwise, if sina is not meti<δaIf so, the index i is considered to meet the position segmentation condition; adding indexes i meeting the position segmentation condition into a segmentation index set, and adding indexes 0 and N from beginning to end into the beginning and the end of the segmentation index set by default;
step 1.2, segmenting according to the segmented index set, and dividing a whole track segment into a plurality of track segments according to the segmented index set, wherein the track segment with the track point number larger than 2 in the track segments is marked as a track segment needing to be smoothed and is used for smoothing the track in the next step; otherwise, marking as a section without a smooth track, and outputting the section to the smooth track according to the linear track;
step 1.3, preprocessing the posture according to the principle that the minor arc between the two tracks is prior, traversing track point indexes i equal to 1,2 and … N, and enabling two adjacent track points P to be adjacenti-1,PiThe method for calculating the distance between the included angles of the postures is oi-1＝|θi-θi-1If o |, ifi-1Greater than 180 deg., modify PiAttitude angle of the fourth dimension of (1): if thetai>0, then is modified to Pi(xi,yi,zi,θi-360 °); otherwise, modifying to Pi(xi,yi,zi,θi+360°)。
Step 2, traverse the smooth track section of needs that step 1 generated, need smooth track section to each section and adopt the smooth track of geometry iteration method generation B spline transition formula according to track point error threshold, position point chordal height error threshold and continuity requirement, the thinking that generates smooth track is unanimous with the smooth thinking of circular arc, the main difference lies in the B spline curve's that satisfies precision and continuity structure, following need smooth track section to introduce the step that the smooth track of geometry iteration method construction B spline transition formula with one section:
step 2.1, setting initial iteration parameters, and setting four-dimensional track points of the current linear track segment as
Figure BDA0001981557190000054
Marking as an original track point, and setting a position distance error threshold value which needs to be met by the smooth track as epsilonmaxPosition chord height error threshold is recorded as deltamaxError threshold o of included angle of attitude pointmaxThreshold value k of iteration numbermaxSetting the current iteration number as k equal to 0, and recording the iteration track point as
Figure BDA0001981557190000061
When k ismaxWhen the value is 1, a transition spline similar to the existing smooth function is generated, as shown in fig. 1, a dotted line is a position point connecting line of a linear track, a solid line is a transition curve with the maximum iteration number of 1, the transition curve can achieve a continuous effect, but the precision of the position point cannot be guaranteed to meet the requirement of high precision, so that a B spline transition track which meets the requirements of precision and continuity for pose synchronization is generated by adopting the following steps;
step 2.2, traversing the index i to be 1,2, … N-1, and respectively generating iterative track points according to position point chord height error constraint, G1 or G2 continuity constraint and conformal constraint
Figure BDA0001981557190000062
Transition B-spline trajectory of
Figure BDA0001981557190000063
First separately calculate
Figure BDA0001981557190000064
And
Figure BDA0001981557190000065
is located a distance di-1And diAnd angle of position point betaiThe calculation method of the position distance and the position point included angle needs to take out the front three-dimension of the four-dimensional vector, calculate the three-dimensional distance and the three-dimensional position point included angle, and calculate di-1And diSmaller value d ofmin＝min(di-1,di)；
And then calculating according to the position point chord height error constraint and the conformal constraint
Figure BDA0001981557190000066
Transition ratio r of front and rear position pointsi-1And riFirstly, calculating the transition length of front and rear position points, deducing according to the property, continuity, precision requirement and conformal requirement of the B spline, and for the G1 transition B spline, the transition length is as follows:
Figure BDA0001981557190000067
for the G2 transition B spline, the transition length is:
Figure BDA0001981557190000068
the length refers to a transition length of the three-dimensional position point, a first part of the transition length represents the transition length meeting a chord height error threshold, and a second part represents the transition length meeting the shape-preserving requirement;
two transition ratios are then calculated:
Figure BDA0001981557190000069
wherein 0<α<1 is a shape-preserving parameter which represents the distance proportion of a linear track section between two transition B-splines in the whole track section; c. C>1 is a proportionality coefficient for determining a control point; for G1 continuous splines
Figure BDA00019815571900000610
And meanwhile, the front three dimensions of the transition spline form a planar PH spline with good arc length calculation property, and the spline can calculate the arc length of the B spline very simply through an analytical method, so that convenience is brought to speed planning. For G2 continuous splines, c may be 1.5 without loss of generality.
Finally, calculating the control points of the four-dimensional spline according to the continuous condition of G1 or G2 and two transition ratios, wherein the transition spline meeting the continuous condition of G1 has four control points (white dots) and is symmetrical left and right as shown in FIG. 2; as shown in fig. 3, the transition spline satisfying the G2 continuous condition has five control points (white dots), the first two and the last two are bilaterally symmetric, the middle point coincides with the transition point, and the calculation method of each control point is as follows (the operation rule of the four-dimensional vector is the same as the three-dimensional vector):
g1 spline:
Figure BDA00019815571900000611
g2 spline:
Figure BDA00019815571900000612
step 2.3, calculating the ith transition B spline by traversing the index i to 1,2, … N-1
Figure BDA0001981557190000071
With the original track point QiFirstly, according to the control points in step 2.2, a four-dimensional B spline is constructed
Figure BDA0001981557190000072
B-spline curve with three uniform knots:
Figure BDA0001981557190000073
whereint∈[0,1](ii) a For the G1 spline, M ═ 3; for the G2 spline, M ═ 4; b isj,3(t) is a B spline basis function and is obtained by calculation according to the node vector; wherein the node vector of the G1 continuous spline is [0,0,0,0,1,1](ii) a The node vectors of the G2 continuous splines are [0,0,0,0,0.5,1,1]；
Then calculating the parameter midpoint of the four-dimensional B spline track as the maximum error point of the track point:
Figure BDA0001981557190000074
Figure BDA0001981557190000075
as indicated by the asterisks in fig. 2 and 3;
finally, calculating transition B spline
Figure BDA0001981557190000076
With the original track point QiTrack point error, including position distance error and attitude point included angle error, based on
Figure BDA0001981557190000077
Obtaining the three-dimensional position distance and the attitude included angle distance;
step 2.4, calculating the maximum position distance error between all transition B-spline and original track point
Figure BDA0001981557190000078
Error of included angle with maximum attitude point
Figure BDA0001981557190000079
The maximum position distance is the maximum value of the three-dimensional position distances between all the transition B splines and the iteration track points, the maximum attitude point included angle error is the maximum value of the attitude included angles between all the transition B splines and the iteration track points, the maximum position distance and the maximum attitude point included angle are compared with a threshold value, and if the maximum position distance and the maximum attitude point included angle are not equal to the threshold value, the maximum position distance and the maximum attitude point included angle are compared with the threshold value
Figure BDA00019815571900000710
Less than a position distance error threshold epsilonmaxAnd is and
Figure BDA00019815571900000711
less than the error threshold o of the included angle of the attitude pointmaxOr the current iteration number k is larger than the iteration number threshold kmaxStopping iteration and outputting B-spline transition type smooth track
Figure BDA00019815571900000712
Turning to step 2.6; otherwise, turning to step 2.5;
step 2.5, traversing i to 1, … N-1 according to the original track point QiMaximum point of sum track point error
Figure BDA00019815571900000713
Calculating an offset vector
Figure BDA00019815571900000714
And updating iteration track points:
Figure BDA00019815571900000715
changing k to k +1, and turning to step 2.2;
step 2.6, the smooth tracks output in the step 2.4 are sorted and output, the smooth tracks are formed by combining N four-dimensional linear tracks and (N-1) four-dimensional B spline smooth tracks, and the steps are sequentially as follows: linear trajectory
Figure BDA00019815571900000716
Smooth B-spline trajectory
Figure BDA00019815571900000717
Linear trajectory
Figure BDA00019815571900000718
Smooth B-spline trajectory
Figure BDA00019815571900000719
Smooth B-spline trajectory
Figure BDA00019815571900000720
Linear trajectory
Figure BDA00019815571900000721
The obtained smooth track can ensure the synchronous continuity of the position and the posture, namely the position p of any track point on the smooth track is ensuredi(xi,yi,zi) G1 or G2 are continuous and any point q is guaranteedi＝pi+RZ(θi) v G1 or G2 continuity. Wherein v is an arbitrary three-dimensional vector, RZ(θi) Is a matrix of rotations about the z-axis. As shown in FIG. 4, the solid line is the linear position trajectory of the smooth front Tool Center Point (TCP); the dotted line is a smooth locus of the position point (the position point p is shown as the above formula) consisting of a smooth straight line and a splineiThe locus of (e), the bold dashed line in the figure may be regarded as the locus of the position of a point on the X-axis of the tool coordinate system (e.g., the position point q of the above formula)iCan be seen to be in synchronous continuity of position and attitude.

Claims (1)

1. An error-controllable B-spline transition type smooth trajectory generation method for a four-axis industrial robot is characterized by comprising the following steps:
step 1, track preprocessing of a four-axis industrial robot: dividing the linear track into a track section needing to be smoothed and a track section not needing to be smoothed according to the position distance and the included angle; and preprocessing the gesture, specifically comprising the following steps:
step 1.1, calculating a segmentation index set according to a segmentation threshold, inputting track segments above two continuous track points, and outputting a segmentation index; set the trajectory points of the four-axis industrial robot as input
Figure FDA0003155974530000011
The number N of the track points is more than or equal to 2, wherein each track point Pi(xi,yi,zi,θi) Is a position (x)i,yi,zi) And attitude, i.e. angle of rotation about the Z axisiComposed four-dimensional vector, position segmentation condition by position distance threshold deltadAnd the angle of positionThe value deltaaAs an index;
and traversing the track point index i as 1,2 and … N-1, and respectively judging whether the index i meets the position segmentation condition:
firstly, calculating a track segment Pi-1PiAnd PiPi+1The distance d between the two sectionsi-1And diIf d isi-1Or diLess than a position distance threshold deltadIf so, the index i is considered to meet the position segmentation condition; otherwise, calculating the track segment Pi-1PiAnd PiPi+1At a position angle ofiIf sin aiLess than a position angle threshold deltaaIf so, the index i is considered to meet the position segmentation condition; otherwise, the index i is considered not to meet the position segmentation condition; adding indexes i meeting the position segmentation condition into a segmentation index set, and adding indexes 0 and N from beginning to end into the beginning and the end of the segmentation index set by default;
step 1.2, segmenting according to the segmented index set, and dividing a whole track segment into a plurality of track segments according to the segmented index set, wherein the track segment with the track point number larger than 2 in the track segments is marked as a track segment needing to be smoothed and is used for smoothing the track in the next step; otherwise, marking as a section without a smooth track, and outputting the section to the smooth track according to the linear track;
step 1.3, posture preprocessing, traversing track point index i to be 1,2, … N according to the principle that minor arcs between two tracks are prior, and if two adjacent track points P arei-1,PiIs included angle distance oi-1Greater than 180 DEG, oi-1＝|θi-θi-1If P is modifiediAttitude angle of the fourth dimension of (1): if thetai>0, then is modified to Pi(xi,yi,zi,θi-360 °); otherwise, modifying to Pi(xi,yi,zi,θi+360°)；
Step 2, smoothing the track section needing to be smoothed: traversing the sections of the track needing to be smoothed, which are generated in the step 1, and generating a continuous B spline transition type smoothing track of G1 or G2 for each section of the track needing to be smoothed by adopting a geometric iteration method according to a track point error threshold, a position point chord height error threshold and continuity requirements, wherein the method specifically comprises the following steps:
step 2.1, setting initial iteration parameters, and setting four-dimensional track points of the current linear track segment as
Figure FDA0003155974530000012
Marking as an original track point, and setting a position distance error threshold value which needs to be met by the smooth track as epsilonmaxPosition chord height error threshold is recorded as deltamaxError threshold o of included angle of attitude pointmaxThreshold value k of iteration numbermaxSetting the current iteration number as k equal to 0, and recording the iteration track point as
Figure FDA0003155974530000013
Step 2.2, traversing the index i to be 1,2, … N-1, and respectively generating iterative track points according to position point chord height error constraint, G1 or G2 continuity constraint and conformal constraint
Figure FDA0003155974530000014
Transition B-spline trajectory of
Figure FDA0003155974530000015
First separately calculate
Figure FDA0003155974530000016
And
Figure FDA0003155974530000017
is located a distance di-1And diAnd angle of position point betaiAnd calculate di-1And diSmaller value d ofmin＝min(di-1,di)；
And then calculating according to the position point chord height error constraint and the conformal constraint
Figure FDA0003155974530000021
Transition ratio r of front and rear position pointsi-1And riFirst before calculationTransition length of the back position point, for the G1 transition B-spline, the transition length is:
Figure FDA0003155974530000022
for the G2 transition B spline, the transition length is
Figure FDA0003155974530000023
Two transition ratios are then calculated:
Figure FDA0003155974530000024
wherein 0<α<1 is a shape-preserving parameter which represents the distance proportion of a linear track section between two transition B-splines in the whole track section; c. C>1 is a proportionality coefficient for determining a control point; for G1 continuous splines
Figure FDA0003155974530000025
Then, the front three-dimension of the transition spline forms a planar PH spline; for G2 continuous splines, take c ═ 1.5;
finally, calculating control points of the four-dimensional spline according to the G1 or G2 continuous condition and the two transition ratios, wherein the transition spline meeting the G1 continuous condition has four control points; the transition spline satisfying the G2 continuous condition has five control points, and the calculation method of each control point is as follows:
g1 spline:
Figure FDA0003155974530000026
g2 spline:
Figure FDA0003155974530000027
step 2.3, calculating the ith transition B spline by traversing the index i to 1,2, … N-1
Figure FDA0003155974530000028
With the original track point QiFirstly, according to the control points in step 2.2, a four-dimensional B spline is constructed
Figure FDA0003155974530000029
B-spline curve with three uniform knots:
Figure FDA00031559745300000210
wherein t is ∈ [0,1 ]](ii) a For the G1 spline, M ═ 3; for the G2 spline, M ═ 4; b isj,3(t) is a B spline basis function and is obtained by calculation according to the node vector;
then calculating the parameter midpoint of the four-dimensional B spline track as the maximum error point of the track point:
Figure FDA00031559745300000211
finally, calculating transition B spline
Figure FDA00031559745300000212
With the original track point QiTrack point error, including position distance error and attitude point included angle error, based on
Figure FDA00031559745300000213
Obtaining the three-dimensional position distance and the attitude included angle distance;
step 2.4, calculating the maximum position distance error between all transition B-spline and original track point
Figure FDA00031559745300000214
Error of included angle with maximum attitude point
Figure FDA00031559745300000215
If it is
Figure FDA00031559745300000216
Less than a position distance error threshold epsilonmaxAnd is and
Figure FDA00031559745300000217
less than the error threshold o of the included angle of the attitude pointmaxOr current iterationThe number k is greater than the threshold number k of iterationsmaxTerminating iteration and outputting B-spline smooth track
Figure FDA00031559745300000218
Turning to step 2.6; otherwise, turning to step 2.5;
step 2.5, traversing i to 1, … N-1 according to the original track point QiMaximum point of sum track point error
Figure FDA00031559745300000219
Calculating an offset vector
Figure FDA00031559745300000220
And updating iteration track points:
Figure FDA00031559745300000221
changing k to k +1, and turning to step 2.2;
step 2.6, the smooth tracks output in the step 2.4 are sorted and output, the smooth tracks are formed by combining N four-dimensional linear tracks and (N-1) four-dimensional B spline smooth tracks, and the steps are sequentially as follows: linear trajectory
Figure FDA0003155974530000031
Smooth B-spline trajectory
Figure FDA0003155974530000032
Linear trajectory
Figure FDA0003155974530000033
Smooth B-spline trajectory
Figure FDA0003155974530000034
Smooth B-spline trajectory
Figure FDA0003155974530000035
Linear trajectory
Figure FDA0003155974530000036
CN201910151081.8A 2019-02-28 2019-02-28 Error-controllable B-spline transition type smooth trajectory generation method for four-axis industrial robot Active CN109664303B (en)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201910151081.8A CN109664303B (en) 2019-02-28 2019-02-28 Error-controllable B-spline transition type smooth trajectory generation method for four-axis industrial robot

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201910151081.8A CN109664303B (en) 2019-02-28 2019-02-28 Error-controllable B-spline transition type smooth trajectory generation method for four-axis industrial robot

Publications (2)

Publication Number Publication Date
CN109664303A CN109664303A (en) 2019-04-23
CN109664303B true CN109664303B (en) 2021-10-12

Family

ID=66151844

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201910151081.8A Active CN109664303B (en) 2019-02-28 2019-02-28 Error-controllable B-spline transition type smooth trajectory generation method for four-axis industrial robot

Country Status (1)

Country Link
CN (1) CN109664303B (en)

Families Citing this family (11)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN110488758B (en) * 2019-08-09 2021-05-25 南京埃斯顿自动化股份有限公司 Trajectory transition method based on PLCopen specification
CN114728411A (en) 2019-11-25 2022-07-08 三菱电机株式会社 Control device and robot system
CN110900612B (en) * 2019-12-17 2020-06-05 东莞市三姆森光电科技有限公司 Pose-synchronous six-axis industrial robot track smoothing method
CN112356032B (en) * 2020-11-05 2022-05-03 哈工大机器人（合肥）国际创新研究院 Posture smooth transition method and system
CN114488941A (en) * 2020-11-11 2022-05-13 沈机（上海）智能系统研发设计有限公司 Trace fairing method and medium for micro line segments and machine tool numerical control equipment
CN112659126B (en) * 2020-12-23 2023-05-16 广东工业大学 Industrial robot track segment transition method based on non-uniform B spline curve
CN112871587B (en) * 2021-01-08 2021-12-10 昂纳工业技术(深圳)有限公司 Gluing path planning method and gluing system based on 3D visual guidance
CN112975992B (en) * 2021-05-21 2021-08-13 武汉瀚迈科技有限公司 Error-controllable robot track synchronous optimization method
CN113276116B (en) * 2021-05-21 2022-01-18 武汉瀚迈科技有限公司 Error-controllable robot track synchronous transition method
CN113478481B (en) * 2021-06-23 2022-08-23 佛山智能装备技术研究院 Industrial robot position and attitude motion synchronization method
CN115958597B (en) * 2022-12-16 2023-09-15 广州数控设备有限公司 Industrial robot continuous attitude path fairing method and system

Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN103064344A (en) * 2012-12-25 2013-04-24 广东省自动化研究所 Non uniform rational B spline (NURBS) interpolation based speed smooth control method
CN105425725A (en) * 2015-12-09 2016-03-23 华中科技大学 Curve fitting method for discrete cutter path
CN105773620A (en) * 2016-04-26 2016-07-20 南京工程学院 Track planning and control method of free curve of industrial robot based on double quaternions
CN106826829A (en) * 2017-02-22 2017-06-13 武汉工程大学 A kind of industrial robot fairing trace generator method of Controllable Error
US9764469B1 (en) * 2013-12-13 2017-09-19 University Of South Florida Generating robotic trajectories with motion harmonics

Patent Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN103064344A (en) * 2012-12-25 2013-04-24 广东省自动化研究所 Non uniform rational B spline (NURBS) interpolation based speed smooth control method
US9764469B1 (en) * 2013-12-13 2017-09-19 University Of South Florida Generating robotic trajectories with motion harmonics
CN105425725A (en) * 2015-12-09 2016-03-23 华中科技大学 Curve fitting method for discrete cutter path
CN105773620A (en) * 2016-04-26 2016-07-20 南京工程学院 Track planning and control method of free curve of industrial robot based on double quaternions
CN106826829A (en) * 2017-02-22 2017-06-13 武汉工程大学 A kind of industrial robot fairing trace generator method of Controllable Error

Non-Patent Citations (1)

* Cited by examiner, † Cited by third party
Title
工业机器人机械设计方法与机器人算法研究;杨林;《中国优秀硕士学位论文全文数据库 信息科技辑》;20140515;全文 *

Also Published As

Publication number Publication date
CN109664303A (en) 2019-04-23

Similar Documents

Publication Publication Date Title
CN109664303B (en) Error-controllable B-spline transition type smooth trajectory generation method for four-axis industrial robot
CN109571473B (en) Error-controllable small line segment trajectory fairing method
CN109676613B (en) Error-controllable arc transition type smooth track generation method for four-axis industrial robot
CN110900612B (en) Pose-synchronous six-axis industrial robot track smoothing method
CN106826829B (en) A kind of industrial robot fairing motion profile generation method of Controllable Error
CN112757306B (en) Inverse solution multi-solution selection and time optimal trajectory planning algorithm for mechanical arm
CN108568817B (en) Delta robot track connection control method based on Bezier curve
CN112975992B (en) Error-controllable robot track synchronous optimization method
CN105500354A (en) Transitional track planning method applied by industrial robot
CN111791236A (en) Industrial robot Cartesian space trajectory transition method
CN109976262B (en) Global curvature continuous fairing method for micro-line segment processing
CN113985817B (en) Robot small line segment track local fairing method and system capable of performing online interpolation
CN111633668B (en) Motion control method for robot to process three-dimensional free-form surface
CN111283683B (en) Servo tracking accelerated convergence method for robot visual feature planning track
CN115202291A (en) NURBS curve interpolation method based on elliptic arc fitting
CN108803480B (en) Method, apparatus and computer readable storage medium for G2 continuous optical switching between tracks
CN110531700B (en) Space corner fairing method based on three-dimensional generalized Euler spiral
CN111515954B (en) Method for generating high-quality motion path of mechanical arm
CN115122329B (en) Convenient industrial robot joint space trajectory planning method
CN116117796B (en) Industrial robot gesture track transition and speed planning method and system
CN113276116B (en) Error-controllable robot track synchronous transition method
CN113608496B (en) Spatial path G 2 Transfer fairing method, apparatus and computer readable storage medium
CN114019911B (en) Curve fitting method based on speed planning
CN113467376B (en) Multi-axis track compression method for multiple processing scenes
CN111708327B (en) PH spline transition linear path processing method with G2 continuity

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination
GR01 Patent grant
GR01 Patent grant