CN112975992B - Error-controllable robot track synchronous optimization method - Google Patents

Error-controllable robot track synchronous optimization method Download PDF

Info

Publication number
CN112975992B
CN112975992B CN202110557131.XA CN202110557131A CN112975992B CN 112975992 B CN112975992 B CN 112975992B CN 202110557131 A CN202110557131 A CN 202110557131A CN 112975992 B CN112975992 B CN 112975992B
Authority
CN
China
Prior art keywords
dimensional
track
points
linear
transition
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
CN202110557131.XA
Other languages
Chinese (zh)
Other versions
CN112975992A (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 Hanmai Technology Co ltd
Original Assignee
Wuhan Hanmai Technology Co ltd
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 Hanmai Technology Co ltd filed Critical Wuhan Hanmai Technology Co ltd
Priority to CN202110557131.XA priority Critical patent/CN112975992B/en
Publication of CN112975992A publication Critical patent/CN112975992A/en
Application granted granted Critical
Publication of CN112975992B publication Critical patent/CN112975992B/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/1628—Programme controls characterised by the control loop
    • B25J9/163—Programme controls characterised by the control loop learning, adaptive, model based, rule based expert control
    • 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
    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00—Control of position, course or altitude of land, water, air, or space vehicles, e.g. automatic pilot
    • G05D1/10—Simultaneous control of position or course in three dimensions

Abstract

The invention provides a robot track synchronous optimization method with controllable errors, which adopts multidimensional track points to represent various robot tracks, establishes a unified operation rule and a multidimensional curve of the multidimensional track points based on the definition of the multidimensional track points, realizes the high-continuity synchronous optimization of the robot tracks, and realizes the high-precision interpolation of various types of robot tracks based on a geometric iteration method.

Description

Error-controllable robot track synchronous optimization method
Technical Field
The invention belongs to the field of track optimization of industrial robots, and particularly relates to a robot track synchronous optimization method with controllable errors.
Background
The low precision of the trajectory execution and the problem of vibrations during execution are the major problems facing current industrial robots. Compared with a multi-axis machine tool, the industrial robot has the advantages of compact structure and high flexibility, and is suitable for three-dimensional complex application. However, the industrial robot is an open-loop system, the stability is poor, and an online control system is not mature in a multi-axis machine tool. With diversification and complication of the operation modes of the industrial robot, ensuring precision and reducing vibration are one of the main targets of the current industrial robot.
The industrial robot motion command mainly comprises a linear motion command and an arc motion command. The motion trajectories represented by straight line segments and circular arcs have only G0 continuity at the junction. In the robot operation process, the speed must be reduced in order to accurately reach a given track point, so that the operation efficiency is greatly reduced; in addition, speed and acceleration discontinuities may cause vibrations when the robot is in motion, thereby accelerating machine wear and affecting trajectory accuracy. The track smoothing technology can improve the continuity of the track of the robot and has important significance for solving the vibration problem and improving the precision.
However, a track smoothing method which can satisfy both continuity and guarantee accuracy is lacked in the current robot control system. Most of the existing robot controllers integrate local corner transition algorithms, but the smoothing algorithms have the following problems in practical application: (1) the transition error cannot be controlled, or only the transition error of the position track can be controlled; (2) the positions and the postures are not synchronized smoothly; (3) the geometry of the smooth trajectory is determined by the controller and cannot be directly controlled by the user. The invention patent with the application number of CN201911300865.9 provides a pose-synchronous six-axis industrial robot track smoothing method, wherein a circular arc curve is adopted for position track transition, and a quaternion B spline is adopted for posture transition, but the transition method can only meet the requirement of G1 continuous robot track transition, and target track points cannot be interpolated.
In addition, the KUKA robot and the Motoman robot provide local spline interpolation instructions, the generated spline track can interpolate a target track point, but the shape of the interpolation spline is determined by the inside of the controller and cannot be controlled by a user, and therefore the smooth track has an uncontrollable chord height difference between the two track points.
Based on the problems, the invention provides an error-controllable local interpolation optimization algorithm, the method can realize the geometric synchronous interpolation of position points and attitude points, the method has multi-track applicability, the smooth track has good shape, linear tracks can be interpolated, and the chord height error between the linear tracks is controllable.
Disclosure of Invention
Aiming at the problems in the prior art, the invention constructs a multi-dimensional optimization track meeting the interpolation precision, the chord height difference constraint, the shape-preserving constraint and the symmetrical constraint, the optimization track is obtained by performing corner transition on a virtual linear track, and the core of the algorithm is to construct the virtual linear track by adopting a geometric iteration method and calculate a transition parameter meeting the constraints.
A robot track synchronous optimization method with controllable errors is characterized by comprising the following steps:
step 1, representing the positions and the gesture tracks of various robots by defining multi-dimensional track points, wherein the multi-dimensional track points can simultaneously represent three-dimensional position tracks, pose tracks of an SCARA robot and pose tracks of a six-joint robot;
step 2, based on the definition of the multi-dimensional track points, establishing a unified operation rule and a multi-dimensional curve of the multi-dimensional track points, wherein the multi-dimensional operation of the multi-dimensional track points comprises a multi-dimensional distance, a multi-dimensional addition method, a multi-dimensional number multiplication and a multi-dimensional subtraction method, and the multi-dimensional curve comprises a multi-dimensional line segment and a multi-dimensional B-spline curve;
step 3, establishing a high continuous synchronous transition method of the robot track expressed based on the convex combination based on the multi-dimensional track points and the multi-dimensional operation, wherein the method can be expanded and customized according to different conditions, robot track transition of different types of robots and different continuity requirements is realized, the synchronous transition can adopt curves such as circular arcs, parabolas and B splines as transition curves, and the difference of different transition types is only based on the difference of basis functions;
a series of robot linear track points are expressed as
Figure 106374DEST_PATH_IMAGE002
Wherein
Figure 501583DEST_PATH_IMAGE004
For multi-dimensional tracing points, traverse
Figure 784797DEST_PATH_IMAGE006
Based on convex combinations
Figure 200735DEST_PATH_IMAGE004
The transition trajectory at a point is represented as:
Figure DEST_PATH_IMAGE008A
wherein
Figure 936610DEST_PATH_IMAGE010
Is a parameter of the curve that is,
Figure 728985DEST_PATH_IMAGE012
，
Figure 397864DEST_PATH_IMAGE014
the addition, the number multiplication and the subtraction in the formula all represent the multidimensional operation of the multidimensional track points,
Figure 391228DEST_PATH_IMAGE016
and
Figure 614399DEST_PATH_IMAGE018
the method is characterized in that the method is a basic function of a transition curve, and the specific representation of the basic function is obtained by derivation according to the type of the transition curve and continuity conditions;
step 4, establishing a pose synchronization high-precision interpolation optimization method of the multi-dimensional robot linear track, firstly explaining symbols required to be used in the following,
Figure 351411DEST_PATH_IMAGE020
in order to optimize the linear track of the multi-dimensional robot,
Figure 468271DEST_PATH_IMAGE022
multi-dimensional linear tracing points before representation optimization
Figure 898116DEST_PATH_IMAGE024
Is indexed by
Figure 139741DEST_PATH_IMAGE026
The upper bound of (a) is,
Figure 883706DEST_PATH_IMAGE028
for the virtual linear trajectory to be solved,
Figure 527177DEST_PATH_IMAGE030
and
Figure 455819DEST_PATH_IMAGE032
for the transition parameters to be solved, wherein
Figure 184740DEST_PATH_IMAGE034
，
Figure 732396DEST_PATH_IMAGE036
For the optimized curve to be solved,
Figure 230374DEST_PATH_IMAGE038
is a transition track
Figure 2021DEST_PATH_IMAGE036
The parameter mid-points of (c):
Figure 557853DEST_PATH_IMAGE040
wherein
Figure 440359DEST_PATH_IMAGE042
、
Figure 261684DEST_PATH_IMAGE044
、
Figure 204232DEST_PATH_IMAGE046
Respectively represent
Figure 235642DEST_PATH_IMAGE026
A first, a
Figure 656259DEST_PATH_IMAGE048
Is first and second
Figure 597670DEST_PATH_IMAGE050
Virtual linear track points to be solved;
step 4.1: construct the first iteration (number of iterations)
Figure 711120DEST_PATH_IMAGE052
) The initial virtual linear trajectory and the transition parameters, wherein the initial virtual linear trajectory
Figure 636351DEST_PATH_IMAGE054
Linear locus with input
Figure 454134DEST_PATH_IMAGE020
Same, transition parameter
Figure 515631DEST_PATH_IMAGE056
And
Figure 268823DEST_PATH_IMAGE058
according to a threshold value of chord height difference
Figure 681350DEST_PATH_IMAGE060
Conformal constrained upper bound
Figure 302824DEST_PATH_IMAGE062
And calculating the position track symmetry constraint, wherein the calculation method is to solve the following linear optimization problem:
Figure DEST_PATH_IMAGE064AA
wherein
Figure 546724DEST_PATH_IMAGE066
An arithmetic function representing the three-dimensional distance between two location points, i.e.
Figure 736397DEST_PATH_IMAGE068
Refers to two position points
Figure 105061DEST_PATH_IMAGE070
And
Figure 671172DEST_PATH_IMAGE072
the three-dimensional distance between the two electrodes,
Figure 972840DEST_PATH_IMAGE074
refers to two position points
Figure 926890DEST_PATH_IMAGE070
And
Figure 314009DEST_PATH_IMAGE076
the three-dimensional distance therebetween;
step (ii) of4.2: calculating a transition curve
Figure 683810DEST_PATH_IMAGE078
And the target point
Figure 43247DEST_PATH_IMAGE024
A pose error of, the error passing
Figure 840302DEST_PATH_IMAGE080
And the target point
Figure 449138DEST_PATH_IMAGE024
Error assessment of (2), wherein
Figure 216105DEST_PATH_IMAGE082
，
Figure 226787DEST_PATH_IMAGE080
And the target point
Figure 194743DEST_PATH_IMAGE024
The error between them adopts multidimensional distance
Figure 759716DEST_PATH_IMAGE084
Calculating, judging the position error of the current calculation
Figure 471320DEST_PATH_IMAGE086
And attitude error
Figure 336508DEST_PATH_IMAGE088
Whether the input position interpolation error threshold is satisfied
Figure 68841DEST_PATH_IMAGE090
And attitude interpolation error threshold
Figure 652269DEST_PATH_IMAGE092
If the input track is a three-dimensional linear track, the attitude error does not need to be judged
Figure 370826DEST_PATH_IMAGE094
And is
Figure 356100DEST_PATH_IMAGE096
Then give an order
Figure 665858DEST_PATH_IMAGE098
,
Figure 330058DEST_PATH_IMAGE100
,
Figure 383465DEST_PATH_IMAGE102
Go to step 4.5, otherwise go to step 4.3.
Step 4.3: calculating the adjustment vector and step length of the virtual linear track according to the target point error
Figure 223245DEST_PATH_IMAGE104
The new virtual linear trajectory is:
Figure 172746DEST_PATH_IMAGE106
wherein
Figure 465187DEST_PATH_IMAGE108
In order to be the step size,
Figure 915760DEST_PATH_IMAGE110
to adjust the vector, the head and tail target points are not adjusted, i.e.:
Figure 610047DEST_PATH_IMAGE112
the step size of each iteration is related to the selection method of the adjustment vector;
the adjustment vector is determined by two methods: method 1 is a weighted iterative method of variable coefficients: the geometrical meaning of the method is as follows: to the first
Figure 261608DEST_PATH_IMAGE026
An iteration point
Figure 775766DEST_PATH_IMAGE114
When making adjustments, only the current point is consideredAnd the locus point to be interpolated
Figure 170975DEST_PATH_IMAGE024
Error vector between:
Figure 719768DEST_PATH_IMAGE116
(ii) a The method 2 is a least square iterative method with variable coefficients, and the geometric meaning of the method is as follows: to the first
Figure 401285DEST_PATH_IMAGE026
An iteration point
Figure 668318DEST_PATH_IMAGE114
When adjusting, not only the current point and the track point to be interpolated are considered
Figure 867218DEST_PATH_IMAGE024
The error vector between the two points is also considered; the adjustment vector is calculated as follows:
Figure 739360DEST_PATH_IMAGE118
in the above-mentioned formula, the above formula,
Figure 998303DEST_PATH_IMAGE120
in order to adjust the set of vectors,
Figure 346107DEST_PATH_IMAGE122
，
Figure 83119DEST_PATH_IMAGE124
,
Figure 606504DEST_PATH_IMAGE126
,
Figure 505190DEST_PATH_IMAGE128
，
estimating the range of the step size according to the adjustment vector: let a certain matrix be
Figure 100002_DEST_PATH_IMAGE130
For method 1:
Figure 100002_DEST_PATH_IMAGE132
(ii) a For method 2:
Figure 100002_DEST_PATH_IMAGE134
step length of
Figure 74712DEST_PATH_IMAGE108
The value range is as follows:
Figure 100002_DEST_PATH_IMAGE136
wherein
Figure 100002_DEST_PATH_IMAGE138
Is a matrix
Figure 677732DEST_PATH_IMAGE130
Maximum eigenvalue of (2), special, preferable
Figure 100002_DEST_PATH_IMAGE140
Taking the value as the step length, and turning to the step 4.4 after the adjustment vector is calculated;
step 4.4: calculating a virtual linear track and a transition parameter according to the adjustment vector, wherein the calculation formula of the virtual linear track is as follows:
Figure 321202DEST_PATH_IMAGE106
transition parameter
Figure 100002_DEST_PATH_IMAGE142
And
Figure 100002_DEST_PATH_IMAGE144
is calculated similarly to step 4.1, except that the chordal height difference threshold is set
Figure 718686DEST_PATH_IMAGE060
Conformal constrained upper bound
Figure 447607DEST_PATH_IMAGE062
And position track symmetric constraint, and convergence constraint of an iterative algorithm also needs to be considered, and the calculation method is to solve the following linear optimization problem:
Figure 100002_DEST_PATH_IMAGE146
the meaning of the target function is target point position interpolation, the first two constraint conditions are chord height difference constraint and shape-preserving constraint, the third constraint condition is symmetry constraint, the last two constraint conditions are convergence constraint of position and posture respectively, the optimization problem is a secondary optimization problem with constraint, and the optimization problem can be solved easily to meet the conditions
Figure 100002_DEST_PATH_IMAGE148
And
Figure 100002_DEST_PATH_IMAGE150
after the calculation is finished, the step 4.2 is switched;
step 4.5: from virtual linear trajectories
Figure 57580DEST_PATH_IMAGE028
And transition parameters
Figure 100002_DEST_PATH_IMAGE152
,
Figure 100002_DEST_PATH_IMAGE154
Structure of the device
Figure 149033DEST_PATH_IMAGE024
Optimized trajectories at points
Figure 655101DEST_PATH_IMAGE036
:
Figure 100002_DEST_PATH_IMAGE156
Wherein the basis functions
Figure 340160DEST_PATH_IMAGE016
And
Figure 816141DEST_PATH_IMAGE018
and
Figure 168625DEST_PATH_IMAGE152
,
Figure 111173DEST_PATH_IMAGE154
is related to the value of (1), particularly to the type of the selected transition curve, and traverses all target points except the head and the tail
Figure 100002_DEST_PATH_IMAGE158
And optimizing the track, wherein the final optimized track consists of a linear part and a transition curve part.
In the step 1, the multi-dimensional track points are represented by unified parameters, and the multi-dimensional track points comprise a three-dimensional position track, a pose track of a SCARA (selective Compliance Robot arm) Robot and a pose track of a six-joint Robot, and have the following forms:
Figure 100002_DEST_PATH_IMAGE160
wherein
Figure 100002_DEST_PATH_IMAGE162
A three-dimensional location point is represented,
Figure 100002_DEST_PATH_IMAGE164
indicating the position and attitude of the SCARA robot,
Figure 100002_DEST_PATH_IMAGE166
the rotating shaft of the posture of the SCARA robot is fixed, only the rotating angle is a variable,
Figure 100002_DEST_PATH_IMAGE168
representing the position and attitude points of a six-joint robot,
Figure 100002_DEST_PATH_IMAGE170
is a quaternion representation of the pose of the six-joint robot.
Step 2, the multi-dimensional distance refers to an abstract distance between two multi-dimensional track points, and is set
Figure 100002_DEST_PATH_IMAGE172
Defining a multidimensional distance operation
Figure 100002_DEST_PATH_IMAGE174
：
Figure 100002_DEST_PATH_IMAGE176
When in use
Figure 142583DEST_PATH_IMAGE162
When the temperature of the water is higher than the set temperature,
Figure 100002_DEST_PATH_IMAGE178
refers to two position points
Figure 100002_DEST_PATH_IMAGE180
And
Figure 156675DEST_PATH_IMAGE024
the three-dimensional distance therebetween; when in use
Figure 98086DEST_PATH_IMAGE164
Or
Figure 945957DEST_PATH_IMAGE168
The multi-dimensional distance includes a three-dimensional position distance
Figure 464663DEST_PATH_IMAGE178
Angle distance from posture
Figure 100002_DEST_PATH_IMAGE182
Wherein
Figure 100002_DEST_PATH_IMAGE184
Representing the absolute value of the difference of the two rotation angles,
Figure 100002_DEST_PATH_IMAGE186
representing a number of elements from
Figure 100002_DEST_PATH_IMAGE188
Gesture of representation is rotated to
Figure 100002_DEST_PATH_IMAGE190
The angle by which the represented gesture is rotated.
The multidimensional addition in the step 2 is a combination of three-dimensional linear space position addition and three-dimensional rotation space attitude addition, and the form is as follows when
Figure 751287DEST_PATH_IMAGE164
The addition operation in time coincides with the addition in the linear space,
Figure 78364DEST_PATH_IMAGE168
the time-dependent addition operation consists of addition in a linear space and quaternion multiplication in a three-dimensional rotation space:
Figure 100002_DEST_PATH_IMAGE192
wherein
Figure 100002_DEST_PATH_IMAGE194
Representing quaternion
Figure 425031DEST_PATH_IMAGE188
And
Figure 837558DEST_PATH_IMAGE190
is multiplied by
Figure 100002_DEST_PATH_IMAGE196
,
Figure 100002_DEST_PATH_IMAGE198
Multiplication of quaternions is represented as:
Figure 100002_DEST_PATH_IMAGE200
wherein s is a one-dimensional variable,
Figure 100002_DEST_PATH_IMAGE202
is a three-dimensional vector.
In step 2, the multidimensional multiplication is a multiplication operation of a multidimensional vector and a constant, and is a combination of position multiplication and attitude multiplication, and is expressed as the following formula:
Figure 100002_DEST_PATH_IMAGE204
when in use
Figure 100002_DEST_PATH_IMAGE206
The number multiplication operation in time is consistent with the number multiplication in the linear space,
Figure 724612DEST_PATH_IMAGE168
the time-dependent number multiplication operation consists of number multiplication in a linear space and exponential operation in an attitude space,
Figure 100002_DEST_PATH_IMAGE208
representing quaternion
Figure 906194DEST_PATH_IMAGE190
M is a constant for multiplication of numbers.
In the step 2, the multidimensional subtraction adopts multidimensional addition and multidimensional multiplication to represent:
Figure 100002_DEST_PATH_IMAGE210
。
in the step 2, moreThe dimension segment is obtained by combining linear interpolation of linear space and spherical linear interpolation (SLERP) of rotation space (attitude), and is set
Figure 830288DEST_PATH_IMAGE172
Two multi-dimensional tracing points, a multi-dimensional line segment between two points
Figure 100002_DEST_PATH_IMAGE212
Can be represented by the following formula:
Figure 100002_DEST_PATH_IMAGE214
，
the addition, the number multiplication and the subtraction in the formula all represent the multi-dimensional operation of the multi-dimensional track points.
The multidimensional B-spline curve in the step 2 is obtained by combining a B-spline curve of a linear space and a quaternion B-spline curve of a rotation space (attitude), and is specifically defined as follows: given control vertex
Figure DEST_PATH_IMAGE216
Vector of nodes
Figure 100002_DEST_PATH_IMAGE218
Number of times
Figure 100002_DEST_PATH_IMAGE220
Multidimensional B-spline curve
Figure DEST_PATH_IMAGE222
Can be expressed as:
Figure DEST_PATH_IMAGE224
wherein:
Figure DEST_PATH_IMAGE226
for the cumulative B-spline basis function, the cumulative B-spline basis function is transformed from a common B-spline basis function, wherein the common B-spline basis function
Figure DEST_PATH_IMAGE228
Expressed as:
Figure DEST_PATH_IMAGE230
the cumulative B-spline basis function is represented as follows:
Figure DEST_PATH_IMAGE232
。
step 4.5 when using successive pairs of B-splines G2
Figure DEST_PATH_IMAGE234
Transition, the distribution of the control points of the multidimensional B-spline curve and the specific form of the basis function are as follows,
Figure DEST_PATH_IMAGE236
Figure DEST_PATH_IMAGE238
in the formula
Figure DEST_PATH_IMAGE240
Is the 5 control points of the B-spline,
Figure DEST_PATH_IMAGE242
in order to be the starting point of the transition,
Figure DEST_PATH_IMAGE244
in order to be the end point of the transition,
Figure 100002_DEST_PATH_IMAGE246
for the basis function of cubic B-spline with 5 control points, take
Figure 100002_DEST_PATH_IMAGE248
According to
Figure 100002_DEST_PATH_IMAGE250
Calculate out
Figure 100002_DEST_PATH_IMAGE252
According to
Figure 100002_DEST_PATH_IMAGE254
Calculate out
Figure DEST_PATH_IMAGE256
Further, 5 control points are calculated; traverse all target points except head and tail
Figure 635171DEST_PATH_IMAGE158
Optimizing the track, wherein the final optimized track consists of a linear part and a transition curve part, and in the above case, the final optimized track is the linear part
Figure DEST_PATH_IMAGE258
Part of a curve
Figure DEST_PATH_IMAGE260
Linear part
Figure DEST_PATH_IMAGE262
Part of a curve
Figure DEST_PATH_IMAGE264
Up to the curved part
Figure DEST_PATH_IMAGE266
Linear part
Figure DEST_PATH_IMAGE268
The composition has G2 continuity between the linear locus and the optimization curve.
The invention has the following advantages:
1. the error-controllable robot track synchronous optimization method can realize high-precision interpolation of positions and postures, the smooth track has a good shape, linear tracks can be interpolated, and the chord height error between the linear tracks is controllable;
2. the robot track optimization method provided by the invention can realize high-continuity smooth track with synchronous position and attitude geometric parameters; the algorithm has high calculation efficiency and stable numerical calculation;
3. the invention has the advantages that the prominent multidimensional track points and the operation method thereof have multi-track applicability: the method can be simultaneously suitable for the geometric smoothness of the three-dimensional position track, the SCARA robot pose track and the six-joint robot pose track; a unified computing framework is established, any transition curve format and any continuity requirement can be covered, and expansion and customization can be easily realized for different conditions.
Drawings
FIG. 1 is a schematic diagram of a six-joint robot position trajectory transition;
FIG. 2 is a schematic diagram of a six-joint robot pose trajectory transition;
FIG. 3 is a comparison graph before and after the trajectory optimization proposed by the present invention;
fig. 4 is a flowchart of a trajectory optimization algorithm proposed by the present invention.
Detailed Description
The technical scheme of the invention is further specifically described by the following embodiments and the accompanying drawings.
The method can realize geometric synchronous transition or interpolation optimization of a plurality of robot pose (position and attitude) tracks, and the optimized position and attitude simultaneously meet high continuity and controllable errors. The invention mainly comprises the following two parts:
(1) defining a corner transition algorithm of a multi-dimensional track point and a multi-dimensional robot track: the multi-dimensional track points adopt unified parameters to represent three-dimensional position tracks, pose tracks of the SCARA robot and pose tracks of the six-joint robot, and the form of the multi-dimensional track points is as follows:
Figure DEST_PATH_IMAGE160A
wherein
Figure 556658DEST_PATH_IMAGE162
A three-dimensional location point is represented,
Figure 186223DEST_PATH_IMAGE164
indicating the position and attitude of the SCARA robot,
Figure 812376DEST_PATH_IMAGE166
the rotating shaft of the posture of the SCARA robot is fixed, only the rotating angle is a variable,
Figure 668337DEST_PATH_IMAGE168
representing the position and attitude points of a six-joint robot,
Figure 772559DEST_PATH_IMAGE170
is a quaternion representation of the pose of the six-joint robot.
The operation of the multi-dimensional track points mainly comprises multi-dimensional distance, multi-dimensional addition, multi-dimensional number multiplication and multi-dimensional subtraction.
The multidimensional distance refers to the abstract distance between two multidimensional track points and is set
Figure 928734DEST_PATH_IMAGE172
Defining a multidimensional distance operation
Figure 319264DEST_PATH_IMAGE174
：
Figure DEST_PATH_IMAGE176A
When in use
Figure 396941DEST_PATH_IMAGE162
When the temperature of the water is higher than the set temperature,
Figure 570434DEST_PATH_IMAGE178
refers to two position points
Figure 174590DEST_PATH_IMAGE180
And
Figure 142546DEST_PATH_IMAGE024
the three-dimensional distance therebetween; when in use
Figure 238678DEST_PATH_IMAGE164
Or
Figure 153545DEST_PATH_IMAGE168
The multi-dimensional distance includes a three-dimensional position distance
Figure 284312DEST_PATH_IMAGE178
Angle distance from posture
Figure 16644DEST_PATH_IMAGE182
Wherein
Figure 600072DEST_PATH_IMAGE184
Representing the absolute value of the difference of the two rotation angles,
Figure 849788DEST_PATH_IMAGE186
representing a number of elements from
Figure 303903DEST_PATH_IMAGE188
Gesture of representation is rotated to
Figure 613662DEST_PATH_IMAGE190
The angle by which the represented gesture is rotated.
The multidimensional addition method of the multidimensional track point is the combination of the three-dimensional linear space position addition method and the three-dimensional rotation space attitude addition method, and the form is as follows when
Figure 277861DEST_PATH_IMAGE164
The addition operation in time coincides with the addition in the linear space,
Figure 331268DEST_PATH_IMAGE168
time-of-flight addition operation by lineThe addition of the sexual space and the quaternion multiplication of the three-dimensional rotation space comprise:
Figure DEST_PATH_IMAGE192A
wherein
Figure 639890DEST_PATH_IMAGE194
Representing quaternion
Figure 714025DEST_PATH_IMAGE188
And
Figure 6466DEST_PATH_IMAGE190
is multiplied by
Figure 863564DEST_PATH_IMAGE196
,
Figure 26692DEST_PATH_IMAGE198
Multiplication of quaternions is represented as:
Figure 678253DEST_PATH_IMAGE200
wherein s is a one-dimensional variable,
Figure DEST_PATH_IMAGE270
is a three-dimensional vector.
The number multiplication of the multi-dimensional tracing points is expressed as the following formula when
Figure 317045DEST_PATH_IMAGE271
The number multiplication operation in time is consistent with the number multiplication in the linear space,
Figure DEST_PATH_IMAGE272
the time-dependent number multiplication operation consists of number multiplication in a linear space and exponential operation in an attitude space:
Figure DEST_PATH_IMAGE204A
Figure 774571DEST_PATH_IMAGE208
representing quaternion
Figure 323364DEST_PATH_IMAGE190
M is a constant for multiplication of numbers.
Multidimensional subtraction can be represented by multidimensional addition and multidimensional multiplication:
Figure 411405DEST_PATH_IMAGE273
。
based on the multi-dimensional operation, a multi-dimensional line segment and a multi-dimensional B spline curve can be established, wherein the multi-dimensional line segment is obtained by combining linear interpolation of a linear space and spherical linear interpolation (SLERP) of a rotation space (posture), and the multi-dimensional line segment is set
Figure DEST_PATH_IMAGE274
Two multi-dimensional tracing points, a multi-dimensional line segment between two points
Figure 147280DEST_PATH_IMAGE275
Can be represented by the following formula:
Figure DEST_PATH_IMAGE276
。
the multidimensional B-spline curve is obtained by combining a B-spline curve of a linear space and a quaternion B-spline curve of a rotation space (attitude), and is specifically defined as follows: given control vertex
Figure 674077DEST_PATH_IMAGE277
Vector of nodes
Figure 342955DEST_PATH_IMAGE218
Number of times
Figure 70740DEST_PATH_IMAGE220
Multidimensional B-spline curve
Figure DEST_PATH_IMAGE278
Can be expressed as:
Figure DEST_PATH_IMAGE224A
wherein:
Figure 152965DEST_PATH_IMAGE279
for the cumulative B-spline basis function, the cumulative B-spline basis function is transformed from a common B-spline basis function, wherein the common B-spline basis function
Figure DEST_PATH_IMAGE280
Expressed as:
Figure DEST_PATH_IMAGE230A
the cumulative B-spline basis function can be expressed as follows:
Figure 217873DEST_PATH_IMAGE281
。
based on the definition and multi-dimensional operation of multi-dimensional track points, the invention provides a pose synchronization transition method for describing multi-dimensional robot tracks based on convex combination, and a series of target points of linear tracks of the robot are expressed as
Figure DEST_PATH_IMAGE282
Go through
Figure 210100DEST_PATH_IMAGE283
Based on the description of the convex combinations, in
Figure 374365DEST_PATH_IMAGE004
The transition trajectory at a point may be represented as:
Figure DEST_PATH_IMAGE285AAAA
addition, multiplication and subtraction in the formulaThe method represents multi-dimensional operation of multi-dimensional track points, wherein
Figure DEST_PATH_IMAGE286
And
Figure 678308DEST_PATH_IMAGE287
the basis functions, called transition curves, the specific representation of which is derived from the type of transition curve used and the continuity conditions.
Two transition curve and basis function representations are presented next.
The first is G1 continuous corner transition, the transition track adopts cubic Bezier curve of 4 control points, and the node vector is
Figure 219010DEST_PATH_IMAGE289
The control points and basis functions are represented by the following formula, wherein
Figure 190377DEST_PATH_IMAGE242
Located on a multi-dimensional line segment
Figure 791123DEST_PATH_IMAGE291
In the above-mentioned manner,
Figure 254465DEST_PATH_IMAGE293
located on a multi-dimensional line segment
Figure 67701DEST_PATH_IMAGE295
In the above-mentioned manner,
Figure 565678DEST_PATH_IMAGE297
and
Figure 71746DEST_PATH_IMAGE299
and
Figure 881439DEST_PATH_IMAGE004
superposing:
Figure DEST_PATH_IMAGE301AA
Figure DEST_PATH_IMAGE303A
Figure 826261DEST_PATH_IMAGE242
、
Figure 178745DEST_PATH_IMAGE297
、
Figure 324556DEST_PATH_IMAGE305
、
Figure 762490DEST_PATH_IMAGE293
the number of the multi-dimensional control points is 4,
Figure 448686DEST_PATH_IMAGE307
，
Figure 249152DEST_PATH_IMAGE309
for a cubic B-spline basis function with four control points,
Figure 362602DEST_PATH_IMAGE311
two proportional parameters.
The second is G2 continuous corner transition, the transition track adopts a cubic B-spline curve with 5 control points, and the node vector is
Figure 287832DEST_PATH_IMAGE313
The control points and basis functions are represented by the following formula, wherein
Figure 980982DEST_PATH_IMAGE242
,
Figure 308058DEST_PATH_IMAGE297
Located on a multi-dimensional line segment
Figure 185884DEST_PATH_IMAGE291
In the above-mentioned manner,
Figure 332832DEST_PATH_IMAGE315
located on a multi-dimensional line segment
Figure 360831DEST_PATH_IMAGE295
In the above-mentioned manner,
Figure 11255DEST_PATH_IMAGE299
and
Figure 466507DEST_PATH_IMAGE004
and (4) overlapping.
Figure DEST_PATH_IMAGE317AA
Figure DEST_PATH_IMAGE238A
Wherein
Figure 428647DEST_PATH_IMAGE242
、
Figure 588233DEST_PATH_IMAGE297
、
Figure 624322DEST_PATH_IMAGE305
、
Figure 250475DEST_PATH_IMAGE319
The number of the multi-dimensional control points is 5,
Figure DEST_PATH_IMAGE320
is the basis function of a cubic B-spline with 5 control points,
Figure 840856DEST_PATH_IMAGE311
two proportional parameters.
Fig. 1 and 2 are schematic diagrams of corner transitions of position and attitude, respectively, using three B-spline of 5 control points. The dotted line locus in fig. 1 is a line of a three-dimensional linear spaceLinear interpolation line segment (black dot)
Figure DEST_PATH_IMAGE322
Representing a location point), a triangle point
Figure DEST_PATH_IMAGE324
And 5 control points for three-dimensional position optimization are shown, and the solid line track is a smooth curve after the position track is optimized. The dotted trace in fig. 2 is a spherical linear interpolation of the three-dimensional rotation space (black dots)
Figure DEST_PATH_IMAGE326
Representing pose points), triangle points
Figure DEST_PATH_IMAGE328
And 5 control points for three-dimensional attitude optimization are represented, and the solid line track is a smooth curve after the attitude track is optimized.
(2) A pose synchronization high-precision interpolation optimization method for a multi-dimensional robot linear track. The method aims to construct a multi-dimensional optimization track meeting interpolation precision, chord height difference constraint, shape-preserving constraint and symmetrical constraint, the optimization track is obtained by performing corner transition on a virtual linear track, and the core of the algorithm is to construct the virtual linear track by adopting a geometric iteration method and calculate transition parameters meeting the constraints.
First, the symbols to be used hereinafter will be explained. As shown in fig. 3, the solid line has a dot trace
Figure DEST_PATH_IMAGE329
A triangular dotted line track is a linear track of the multi-dimensional robot before optimization
Figure DEST_PATH_IMAGE330
For the virtual linear trajectory to be solved,
Figure DEST_PATH_IMAGE331
and
Figure DEST_PATH_IMAGE332
is to be asked forTransition parameters of the solution, wherein
Figure DEST_PATH_IMAGE333
，
Figure DEST_PATH_IMAGE334
A transition curve constructed for the virtual linear trajectory to be solved and the transition parameters,
Figure DEST_PATH_IMAGE335
is a transition track
Figure 335292DEST_PATH_IMAGE334
The parameter mid-points of (c):
Figure DEST_PATH_IMAGE336
。
as shown in fig. 4, the pose synchronization high-precision interpolation optimization method of the multi-dimensional robot linear track includes the following specific steps:
step 4.1: construct the first iteration (number of iterations)
Figure DEST_PATH_IMAGE337
) Virtual linear trajectory of
Figure DEST_PATH_IMAGE338
And transition parameters
Figure DEST_PATH_IMAGE339
And
Figure DEST_PATH_IMAGE340
wherein the initial virtual linear trajectory
Figure 553783DEST_PATH_IMAGE338
Linear locus with input
Figure 944313DEST_PATH_IMAGE329
Same, transition parameter
Figure 553149DEST_PATH_IMAGE339
And
Figure 461062DEST_PATH_IMAGE340
according to a threshold value of chord height difference
Figure DEST_PATH_IMAGE341
Conformal constrained upper bound
Figure DEST_PATH_IMAGE342
And calculating the position track symmetry constraint, wherein the calculation method is to solve the following linear optimization problem:
Figure DEST_PATH_IMAGE064AAA
wherein in the first two inequalities, the first term describes the chord height difference constraint and the second term describes the shape-preserving constraint, wherein
Figure DEST_PATH_IMAGE344
In relation to a specific basis function, in the first transition type described above,
Figure DEST_PATH_IMAGE346
in the second type
Figure DEST_PATH_IMAGE348
Wherein
Figure DEST_PATH_IMAGE350
The linear part between the two transition curves accounts for the whole line segment.
Step 4.2: calculating a transition curve
Figure DEST_PATH_IMAGE351
And the target point
Figure 65219DEST_PATH_IMAGE024
The position and attitude error of (1) can directly pass through the parameter midpoint according to the symmetry of the transition curve
Figure 33175DEST_PATH_IMAGE080
And the target point
Figure 722782DEST_PATH_IMAGE024
Wherein:
Figure DEST_PATH_IMAGE352
，
Figure 434386DEST_PATH_IMAGE080
and the target point
Figure 33995DEST_PATH_IMAGE024
The error between can adopt multidimensional distance
Figure DEST_PATH_IMAGE353
Calculation of multidimensional distance by position error
Figure DEST_PATH_IMAGE354
And attitude error
Figure DEST_PATH_IMAGE355
Composition, judging whether two errors meet the input position interpolation error threshold value
Figure DEST_PATH_IMAGE356
And attitude interpolation error threshold
Figure DEST_PATH_IMAGE357
To make a judgment on
Figure DEST_PATH_IMAGE358
And is
Figure DEST_PATH_IMAGE359
When the input track is a three-dimensional linear track, the attitude error does not need to be judged, if the two errors meet the condition, the current transition track can interpolate a target track point, the circulation can be stopped, and the three-dimensional linear track is made to have a smooth transition track
Figure DEST_PATH_IMAGE360
，
Figure DEST_PATH_IMAGE361
,
Figure DEST_PATH_IMAGE362
Go to step 4.5, otherwise go to step 4.3.
Step 4.3: calculating the adjustment vector and step length of the virtual linear track according to the target point error
Figure DEST_PATH_IMAGE363
The new virtual linear trajectory is:
Figure DEST_PATH_IMAGE364
wherein
Figure 297486DEST_PATH_IMAGE108
In order to be the step size,
Figure 208810DEST_PATH_IMAGE110
to adjust the vector. The head and tail target points are not adjusted, namely:
Figure DEST_PATH_IMAGE365
. The step size of each iteration is related to the selection method of the adjustment vector.
The invention provides two methods for determining an adjustment vector: the first method is called a weighted iteration method of variable coefficients, and the geometrical meaning of the method is as follows: to the first
Figure 724105DEST_PATH_IMAGE026
An iteration point
Figure DEST_PATH_IMAGE366
When adjusting, only the current point and the track point to be interpolated are considered
Figure 178220DEST_PATH_IMAGE024
Error vector between:
Figure DEST_PATH_IMAGE367
the second method for taking the adjustment vector is named as a least square iterative method of variable coefficients, and the geometric meaning of the method is as follows: to the first
Figure 81454DEST_PATH_IMAGE026
An iteration point
Figure 886599DEST_PATH_IMAGE366
When adjusting, not only the current point and the track point to be interpolated are considered
Figure 940006DEST_PATH_IMAGE024
The error vector between the two points is also considered, and the adjustment vector is calculated as follows:
Figure DEST_PATH_IMAGE368
in the above-mentioned formula, the above formula,
Figure 248628DEST_PATH_IMAGE120
in order to adjust the set of vectors,
Figure DEST_PATH_IMAGE369
，
Figure DEST_PATH_IMAGE370
,
Figure DEST_PATH_IMAGE371
,
Figure DEST_PATH_IMAGE372
。
the range of step sizes is then estimated. Let a certain matrix be
Figure 791604DEST_PATH_IMAGE130
For method 1:
Figure DEST_PATH_IMAGE373
(ii) a For method 2:
Figure DEST_PATH_IMAGE374
. Step size
Figure 954819DEST_PATH_IMAGE108
The value range is as follows:
Figure DEST_PATH_IMAGE375
wherein
Figure 546337DEST_PATH_IMAGE138
Is a matrix
Figure 975044DEST_PATH_IMAGE130
The maximum eigenvalue of (c). Specially, preferably
Figure DEST_PATH_IMAGE376
And (4) taking the value of the step length, and turning to the step 4.4 after the adjustment vector is calculated.
Step 4.4: calculating a virtual linear track and a transition parameter according to the adjustment vector, wherein the calculation method of the virtual linear track comprises the following steps:
Figure 626606DEST_PATH_IMAGE364
transition parameter
Figure DEST_PATH_IMAGE377
And
Figure DEST_PATH_IMAGE378
is calculated similarly to step 1, except that the chordal height difference threshold is set
Figure 468660DEST_PATH_IMAGE341
Conformal constrained upper bound
Figure 863869DEST_PATH_IMAGE342
And position track symmetric constraint, and convergence constraint of an iterative algorithm also needs to be considered, and the calculation method is to solve the following linear optimization problem:
Figure DEST_PATH_IMAGE146A
the meaning of the target function is the interpolation of the position of a target point, the first two constraint conditions are chordal height difference constraint and shape-preserving constraint, the third constraint condition is symmetry constraint, and the last two constraint conditions are convergence constraint of the position and the posture respectively. The optimization problem is a secondary optimization problem with constraint, and can be solved easily to meet the conditions
Figure 334033DEST_PATH_IMAGE148
And
Figure 625337DEST_PATH_IMAGE150
. And 4.2, after the virtual linear track and the transition parameters of the iteration are calculated, turning to the step.
Step 4.5: from virtual linear trajectories
Figure 892371DEST_PATH_IMAGE330
And transition parameters
Figure 91271DEST_PATH_IMAGE152
,
Figure 415942DEST_PATH_IMAGE154
Structure of the device
Figure 409306DEST_PATH_IMAGE024
Optimized trajectories at points
Figure 632477DEST_PATH_IMAGE334
:
Figure DEST_PATH_IMAGE156A
Wherein the basis functions
Figure 228543DEST_PATH_IMAGE286
And
Figure 751928DEST_PATH_IMAGE287
and
Figure 916193DEST_PATH_IMAGE152
,
Figure 626660DEST_PATH_IMAGE154
the values of (a) are related to the selected transition curve type, and reference may be made to the two transition curve types described above. Traverse all target points except head and tail
Figure DEST_PATH_IMAGE379
Optimizing the track, wherein the final optimized track consists of a linear part and a transition curve part, and in the above case, the final optimized track is the linear part
Figure DEST_PATH_IMAGE380
Part of a curve
Figure DEST_PATH_IMAGE381
Linear part
Figure DEST_PATH_IMAGE382
Part of a curve
Figure DEST_PATH_IMAGE383
Up to the curved part
Figure DEST_PATH_IMAGE384
Linear part
Figure DEST_PATH_IMAGE385
The composition has G2 continuity between the linear locus and the optimization curve.
The protective scope of the present invention is not limited to the above-described embodiments, and it is apparent that various modifications and variations can be made to the present invention by those skilled in the art without departing from the scope and spirit of the present invention. It is intended that the present invention cover the modifications and variations of this invention provided they come within the scope of the appended claims and their equivalents.

Claims (10)

1. An error-controllable robot track synchronous optimization method is characterized by comprising the following steps:
step 1, representing the positions and the gesture tracks of various robots by defining multi-dimensional track points, wherein the multi-dimensional track points can simultaneously represent three-dimensional position tracks, pose tracks of an SCARA robot and pose tracks of a six-joint robot;
step 2, based on the definition of the multi-dimensional track points, establishing a unified operation rule and a multi-dimensional curve of the multi-dimensional track points, wherein the multi-dimensional operation of the multi-dimensional track points comprises a multi-dimensional distance, a multi-dimensional addition method, a multi-dimensional number multiplication and a multi-dimensional subtraction method, and the multi-dimensional curve comprises a multi-dimensional line segment and a multi-dimensional B-spline curve;
step 3, establishing a high continuous synchronous transition method of the robot track based on convex combination representation based on multi-dimensional track points and multi-dimensional operation, wherein the synchronous transition adopts an arc, a parabola or a B spline curve as a transition curve, and the difference of different transition types is only based on the difference of a basis function;
a series of robot linear track points are expressed as
Figure DEST_PATH_IMAGE002
Wherein
Figure DEST_PATH_IMAGE004
For multi-dimensional tracing points, traverse
Figure DEST_PATH_IMAGE006
Based on convex combinations
Figure 229763DEST_PATH_IMAGE004
The transition trajectory at a point is represented as:
Figure DEST_PATH_IMAGE008
wherein
Figure DEST_PATH_IMAGE010
Is a parameter of the curve that is,
Figure DEST_PATH_IMAGE012
，
Figure DEST_PATH_IMAGE014
the addition, the number multiplication and the subtraction in the formula all represent the multidimensional operation of the multidimensional track points,
Figure DEST_PATH_IMAGE016
and
Figure DEST_PATH_IMAGE018
the method is characterized in that the method is a basic function of a transition curve, and the specific representation of the basic function is obtained by derivation according to the type of the transition curve and continuity conditions;
step 4, establishing a pose synchronization high-precision interpolation optimization method of the multi-dimensional robot linear track:
is provided with
Figure DEST_PATH_IMAGE020
In order to optimize the linear track of the multi-dimensional robot,
Figure DEST_PATH_IMAGE022
multi-dimensional linear tracing points before representation optimization
Figure DEST_PATH_IMAGE024
Is indexed by
Figure DEST_PATH_IMAGE026
The upper bound of (a) is,
Figure DEST_PATH_IMAGE028
for the virtual linear trajectory to be solved,
Figure DEST_PATH_IMAGE030
and
Figure DEST_PATH_IMAGE032
for the transition parameters to be solved, wherein
Figure DEST_PATH_IMAGE034
，
Figure DEST_PATH_IMAGE036
For the optimized curve to be solved,
Figure DEST_PATH_IMAGE038
is a transition track
Figure 396171DEST_PATH_IMAGE036
The parameter mid-points of (c):
Figure DEST_PATH_IMAGE040
wherein
Figure DEST_PATH_IMAGE042
、
Figure DEST_PATH_IMAGE044
、
Figure DEST_PATH_IMAGE046
Respectively represent
Figure 249944DEST_PATH_IMAGE026
A first, a
Figure DEST_PATH_IMAGE048
Is first and second
Figure DEST_PATH_IMAGE050
Virtual linear track points to be solved;
step 4.1: number of construction iterations
Figure DEST_PATH_IMAGE052
Virtual linear trajectory and transition parameters of a first iteration corresponding in timeWherein the initial virtual linear trajectory
Figure DEST_PATH_IMAGE054
Linear locus with input
Figure 707470DEST_PATH_IMAGE020
Same, transition parameter
Figure DEST_PATH_IMAGE056
And
Figure DEST_PATH_IMAGE058
according to a threshold value of chord height difference
Figure DEST_PATH_IMAGE060
Conformal constrained upper bound
Figure DEST_PATH_IMAGE062
And calculating the position track symmetry constraint, wherein the calculation method is to solve the following linear optimization problem:
Figure DEST_PATH_IMAGE064
wherein
Figure DEST_PATH_IMAGE066
An arithmetic function representing the three-dimensional distance between two location points, i.e.
Figure DEST_PATH_IMAGE068
Refers to two position points
Figure DEST_PATH_IMAGE070
And
Figure DEST_PATH_IMAGE072
the three-dimensional distance between the two electrodes,
Figure DEST_PATH_IMAGE074
refers to two position points
Figure 708793DEST_PATH_IMAGE070
And
Figure DEST_PATH_IMAGE076
the three-dimensional distance therebetween;
step 4.2: calculating a transition curve
Figure DEST_PATH_IMAGE078
And the target point
Figure 97DEST_PATH_IMAGE024
A pose error of, the error passing
Figure DEST_PATH_IMAGE080
And the target point
Figure 267130DEST_PATH_IMAGE024
Error assessment of (2), wherein
Figure DEST_PATH_IMAGE082
，
Figure 59506DEST_PATH_IMAGE080
And the target point
Figure 197226DEST_PATH_IMAGE024
The error between them adopts multidimensional distance
Figure DEST_PATH_IMAGE084
Calculating, judging the position error of the current calculation
Figure DEST_PATH_IMAGE086
And attitude error
Figure DEST_PATH_IMAGE088
Whether the input position interpolation error threshold is satisfied
Figure DEST_PATH_IMAGE090
And attitude interpolation error threshold
Figure DEST_PATH_IMAGE092
If the input track is a three-dimensional linear track, the attitude error does not need to be judged
Figure DEST_PATH_IMAGE094
And is
Figure DEST_PATH_IMAGE096
Then give an order
Figure DEST_PATH_IMAGE098
,
Figure DEST_PATH_IMAGE100
,
Figure DEST_PATH_IMAGE102
Turning to step 4.5, otherwise, turning to step 4.3;
step 4.3: calculating the adjustment vector and step length of the virtual linear track according to the target point error
Figure DEST_PATH_IMAGE104
The new virtual linear trajectory is:
Figure DEST_PATH_IMAGE106
wherein
Figure DEST_PATH_IMAGE108
In order to be the step size,
Figure DEST_PATH_IMAGE110
to adjust the vector, the head and tail target points are not adjusted, i.e.:
Figure DEST_PATH_IMAGE112
；
the adjustment vector is calculated as follows:
Figure DEST_PATH_IMAGE114
，
Figure DEST_PATH_IMAGE116
in order to adjust the set of vectors,
Figure DEST_PATH_IMAGE118
，
Figure DEST_PATH_IMAGE120
,
Figure DEST_PATH_IMAGE122
，
Figure DEST_PATH_IMAGE124
，
estimating the range of the step size according to the adjustment vector: let a certain matrix be
Figure DEST_PATH_IMAGE126
Step length of
Figure 439858DEST_PATH_IMAGE108
The value range is as follows:
Figure DEST_PATH_IMAGE128
wherein
Figure DEST_PATH_IMAGE130
Is a matrix
Figure 787662DEST_PATH_IMAGE126
After the adjustment vector is calculated, the step 4.4 is carried out;
step 4.4: computing a virtual from the adjustment vectorLinear track and transition parameter, the calculation formula of virtual linear track is:
Figure 524674DEST_PATH_IMAGE106
transition parameter
Figure DEST_PATH_IMAGE132
And
Figure DEST_PATH_IMAGE134
calculation of, except for a chordal height difference threshold
Figure 516901DEST_PATH_IMAGE060
Conformal constrained upper bound
Figure 946745DEST_PATH_IMAGE062
And position trajectory symmetric constraint, and convergence constraint of an iterative algorithm also need to be considered, specifically solving the following linear optimization problem:
Figure DEST_PATH_IMAGE136
the meaning of the target function is target point position interpolation, the first two constraint conditions are chord height difference constraint and shape-preserving constraint, the third constraint condition is symmetry constraint, the last two constraint conditions are convergence constraint of position and posture respectively, the optimization problem is a secondary optimization problem with constraint, and the optimization problem can be solved easily to meet the conditions
Figure DEST_PATH_IMAGE138
And
Figure DEST_PATH_IMAGE140
after the calculation is finished, the step 4.2 is switched;
step 4.5: from virtual linear trajectories
Figure 844163DEST_PATH_IMAGE028
And transition parameters
Figure DEST_PATH_IMAGE142
,
Figure DEST_PATH_IMAGE144
Structure of the device
Figure 588128DEST_PATH_IMAGE024
Optimized trajectories at points
Figure 231599DEST_PATH_IMAGE036
:
Figure DEST_PATH_IMAGE146
Wherein the basis functions
Figure DEST_PATH_IMAGE148
And
Figure DEST_PATH_IMAGE150
and
Figure DEST_PATH_IMAGE152
,
Figure DEST_PATH_IMAGE154
is related to the value of (1), particularly to the type of the selected transition curve, and traverses all target points except the head and the tail
Figure DEST_PATH_IMAGE156
And optimizing the track, wherein the final optimized track consists of a linear part and a transition curve part.
2. The method for the synchronous optimization of the trajectory of the robot with controllable errors as claimed in claim 1, wherein: in the step 1, the multi-dimensional track points are represented by unified parameters, and the multi-dimensional track points are in the following forms:
Figure DEST_PATH_IMAGE158
wherein
Figure DEST_PATH_IMAGE160
A three-dimensional location point is represented,
Figure DEST_PATH_IMAGE162
indicating the position and attitude of the SCARA robot,
Figure DEST_PATH_IMAGE164
the rotating shaft of the posture of the SCARA robot is fixed, only the rotating angle is a variable,
Figure DEST_PATH_IMAGE166
representing the position and attitude points of a six-joint robot,
Figure DEST_PATH_IMAGE168
is a quaternion representation of the pose of the six-joint robot.
3. The method for the synchronous optimization of the trajectory of the robot with controllable errors as claimed in claim 1, wherein: step 2, the multi-dimensional distance refers to an abstract distance between two multi-dimensional track points, and is set
Figure DEST_PATH_IMAGE170
Defining a multidimensional distance operation
Figure DEST_PATH_IMAGE172
：
Figure DEST_PATH_IMAGE174
When in use
Figure 347191DEST_PATH_IMAGE160
When the temperature of the water is higher than the set temperature,
Figure DEST_PATH_IMAGE176
refers to two position points
Figure DEST_PATH_IMAGE178
And
Figure DEST_PATH_IMAGE180
the three-dimensional distance therebetween; when in use
Figure 138430DEST_PATH_IMAGE162
Or
Figure 217244DEST_PATH_IMAGE166
The multi-dimensional distance includes a three-dimensional position distance
Figure 715222DEST_PATH_IMAGE176
Angle distance from posture
Figure DEST_PATH_IMAGE182
Wherein
Figure DEST_PATH_IMAGE184
Representing the absolute value of the difference of the two rotation angles,
Figure DEST_PATH_IMAGE186
representing a number of elements from
Figure DEST_PATH_IMAGE188
Gesture of representation is rotated to
Figure DEST_PATH_IMAGE190
Posture of the representationThe angle of rotation.
4. The method for the synchronous optimization of the trajectory of the robot with controllable errors as claimed in claim 1, wherein: the multidimensional addition in the step 2 is a combination of three-dimensional linear space position addition and three-dimensional rotation space attitude addition, and the form is as follows when
Figure DEST_PATH_IMAGE192
The addition operation in time coincides with the addition in the linear space,
Figure DEST_PATH_IMAGE194
the time-dependent addition operation consists of addition in a linear space and quaternion multiplication in a three-dimensional rotation space:
Figure DEST_PATH_IMAGE196
wherein
Figure DEST_PATH_IMAGE198
Representing quaternion
Figure DEST_PATH_IMAGE200
And
Figure DEST_PATH_IMAGE202
is multiplied by
Figure DEST_PATH_IMAGE204
,
Figure DEST_PATH_IMAGE206
Multiplication of quaternions is represented as:
Figure DEST_PATH_IMAGE208
wherein s is a one-dimensional variable,
Figure DEST_PATH_IMAGE210
is a three-dimensional vector.
5. The method for the synchronous optimization of the trajectory of the robot with controllable errors as claimed in claim 1, wherein: in step 2, the multidimensional multiplication is a multiplication operation of a multidimensional vector and a constant, and is a combination of position multiplication and attitude multiplication, and is expressed as the following formula:
Figure DEST_PATH_IMAGE212
when in use
Figure DEST_PATH_IMAGE214
The number multiplication operation in time is consistent with the number multiplication in the linear space,
Figure DEST_PATH_IMAGE215
the time-dependent number multiplication operation consists of number multiplication in a linear space and exponential operation in an attitude space,
Figure DEST_PATH_IMAGE217
representing quaternion
Figure DEST_PATH_IMAGE218
M is a constant for multiplication of numbers.
6. The method for the synchronous optimization of the trajectory of the robot with controllable errors as claimed in claim 1, wherein: in the step 2, the multidimensional subtraction adopts multidimensional addition and multidimensional multiplication to represent:
Figure DEST_PATH_IMAGE220
。
7. the method for the synchronous optimization of the trajectory of the robot with controllable errors as claimed in claim 1, wherein: the multi-dimensional line segment in the step 2Is obtained by combining linear interpolation of linear space and spherical linear interpolation of rotation space
Figure DEST_PATH_IMAGE221
Two multi-dimensional tracing points, a multi-dimensional line segment between two points
Figure DEST_PATH_IMAGE223
Represented by the formula:
Figure DEST_PATH_IMAGE225
，
the addition, the number multiplication and the subtraction in the formula all represent the multi-dimensional operation of the multi-dimensional track points.
8. The method for the synchronous optimization of the trajectory of the robot with controllable errors as claimed in claim 1, wherein: the multidimensional B-spline curve in the step 2 is obtained by combining a B-spline curve in a linear space and a quaternion B-spline curve in a rotation space, and is specifically defined as follows: given control vertex
Figure DEST_PATH_IMAGE227
Vector of nodes
Figure DEST_PATH_IMAGE229
Number of times
Figure DEST_PATH_IMAGE231
Multidimensional B-spline curve
Figure DEST_PATH_IMAGE233
Expressed as:
Figure DEST_PATH_IMAGE235
wherein:
Figure DEST_PATH_IMAGE237
for the cumulative B-spline basis function, the cumulative B-spline basis function is transformed from a common B-spline basis function, wherein the common B-spline basis function
Figure DEST_PATH_IMAGE239
Expressed as:
Figure DEST_PATH_IMAGE241
the cumulative B-spline basis function is represented as follows:
Figure DEST_PATH_IMAGE243
。
9. the method for the synchronous optimization of the trajectory of the robot with controllable errors as claimed in claim 1, wherein: the determination method of the adjustment vector in step 4.3 includes two methods: method 1 is a weighted iterative method of variable coefficients: the geometrical meaning of the method is as follows: to the first
Figure 923087DEST_PATH_IMAGE026
An iteration point
Figure DEST_PATH_IMAGE245
When adjusting, only the current point and the track point to be interpolated are considered
Figure DEST_PATH_IMAGE246
Error vector between:
Figure DEST_PATH_IMAGE248
(ii) a The method 2 is a least square iterative method with variable coefficients, and the geometric meaning of the method is as follows: to the first
Figure 936042DEST_PATH_IMAGE026
An iteration point
Figure 818548DEST_PATH_IMAGE245
When adjusting, not only the current point and the track point to be interpolated are considered
Figure 639873DEST_PATH_IMAGE246
The error vector between the two points is also considered; estimating the range of the step size according to the adjustment vector: let a certain matrix be
Figure 582421DEST_PATH_IMAGE126
For method 1:
Figure DEST_PATH_IMAGE250
(ii) a For method 2:
Figure DEST_PATH_IMAGE252
(ii) a Step size
Figure DEST_PATH_IMAGE253
The value range is as follows:
Figure DEST_PATH_IMAGE254
wherein
Figure DEST_PATH_IMAGE255
Is a matrix
Figure 676148DEST_PATH_IMAGE126
The maximum eigenvalue of (c).
10. The method for the synchronous optimization of the trajectory of the robot with controllable errors as claimed in claim 1, wherein: step 4.5 when using successive pairs of B-splines G2
Figure DEST_PATH_IMAGE257
Transition, the distribution of the control points of the multidimensional B-spline curve and the specific form of the basis function are as follows,
Figure DEST_PATH_IMAGE259
Figure DEST_PATH_IMAGE261
in the formula
Figure DEST_PATH_IMAGE263
Is the 5 control points of the B-spline,
Figure DEST_PATH_IMAGE265
in order to be the starting point of the transition,
Figure DEST_PATH_IMAGE267
in order to be the end point of the transition,
Figure DEST_PATH_IMAGE269
for the basis function of cubic B-spline with 5 control points, take
Figure DEST_PATH_IMAGE271
According to
Figure DEST_PATH_IMAGE273
Calculate out
Figure DEST_PATH_IMAGE275
According to
Figure DEST_PATH_IMAGE277
Calculate out
Figure DEST_PATH_IMAGE279
Further, 5 control points are calculated; traverse all target points except head and tail
Figure DEST_PATH_IMAGE281
Performing track optimization, and finallyThe optimized track consists of a linear part and a transitional curve part, and the final optimized track is the linear part
Figure DEST_PATH_IMAGE283
Part of a curve
Figure DEST_PATH_IMAGE285
Linear part
Figure DEST_PATH_IMAGE287
Part of a curve
Figure DEST_PATH_IMAGE289
Up to the curved part
Figure DEST_PATH_IMAGE291
Linear part
Figure DEST_PATH_IMAGE293
The composition has G2 continuity between the linear locus and the optimization curve.
CN202110557131.XA 2021-05-21 2021-05-21 Error-controllable robot track synchronous optimization method Active CN112975992B (en)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202110557131.XA CN112975992B (en) 2021-05-21 2021-05-21 Error-controllable robot track synchronous optimization method

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202110557131.XA CN112975992B (en) 2021-05-21 2021-05-21 Error-controllable robot track synchronous optimization method

Publications (2)

Publication Number Publication Date
CN112975992A CN112975992A (en) 2021-06-18
CN112975992B true CN112975992B (en) 2021-08-13

Family

ID=76337115

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202110557131.XA Active CN112975992B (en) 2021-05-21 2021-05-21 Error-controllable robot track synchronous optimization method

Country Status (1)

Country Link
CN (1) CN112975992B (en)

Families Citing this family (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN114714351B (en) * 2022-04-06 2023-06-23 上海工程技术大学 Anti-saturation target tracking control method and control system for mobile mechanical arm
CN115202293B (en) * 2022-07-15 2023-04-28 武汉瀚迈科技有限公司 Two-section type speed planning method for industrial robot
CN116909154B (en) * 2023-09-13 2023-12-08 武汉瀚迈科技有限公司 Robot track optimization method for feedforward compensation through feature table lookup
CN117315198B (en) * 2023-10-09 2024-04-16 中微智创(北京)软件技术有限公司 Smooth optimization method and system for smooth fine adjustment of moving target track corner

Citations (8)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
EP0795804A4 (en) * 1995-08-31 2001-03-28 Fanuc Ltd Curve interpolation method for carrying out speed control operation when robot makes connecting action
JP2014208400A (en) * 2014-06-10 2014-11-06 株式会社デンソーウェーブ Robot controller and robot attitude interpolation method
CN106826829A (en) * 2017-02-22 2017-06-13 武汉工程大学 A kind of industrial robot fairing trace generator method of Controllable Error
CN109571473A (en) * 2018-12-03 2019-04-05 武汉工程大学 A kind of small line segment track method for fairing that error is controllable
CN109664303A (en) * 2019-02-28 2019-04-23 武汉工程大学 A kind of four smooth orbit generation methods of shaft industrial robot B-spline transition type that error is controllable
CN109676613A (en) * 2019-02-28 2019-04-26 武汉工程大学 A kind of four smooth orbit generation methods of shaft industrial robot circular arc transition type that error is controllable
CN110308699A (en) * 2019-04-28 2019-10-08 威海印刷机械有限公司 A kind of method for planning track
CN110900612A (en) * 2019-12-17 2020-03-24 东莞市三姆森光电科技有限公司 Pose-synchronous six-axis industrial robot track smoothing method

Patent Citations (8)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
EP0795804A4 (en) * 1995-08-31 2001-03-28 Fanuc Ltd Curve interpolation method for carrying out speed control operation when robot makes connecting action
JP2014208400A (en) * 2014-06-10 2014-11-06 株式会社デンソーウェーブ Robot controller and robot attitude interpolation method
CN106826829A (en) * 2017-02-22 2017-06-13 武汉工程大学 A kind of industrial robot fairing trace generator method of Controllable Error
CN109571473A (en) * 2018-12-03 2019-04-05 武汉工程大学 A kind of small line segment track method for fairing that error is controllable
CN109664303A (en) * 2019-02-28 2019-04-23 武汉工程大学 A kind of four smooth orbit generation methods of shaft industrial robot B-spline transition type that error is controllable
CN109676613A (en) * 2019-02-28 2019-04-26 武汉工程大学 A kind of four smooth orbit generation methods of shaft industrial robot circular arc transition type that error is controllable
CN110308699A (en) * 2019-04-28 2019-10-08 威海印刷机械有限公司 A kind of method for planning track
CN110900612A (en) * 2019-12-17 2020-03-24 东莞市三姆森光电科技有限公司 Pose-synchronous six-axis industrial robot track smoothing method

Also Published As

Publication number Publication date
CN112975992A (en) 2021-06-18

Similar Documents

Publication Publication Date Title
CN112975992B (en) Error-controllable robot track synchronous optimization method
CN109571473B (en) Error-controllable small line segment trajectory fairing method
CN109664303B (en) Error-controllable B-spline transition type smooth trajectory generation method for four-axis industrial robot
CN107263484B (en) Robot joint space point-to-point motion trajectory planning method
CN111791236B (en) Industrial robot Cartesian space trajectory transition method
CN106647282B (en) Six-degree-of-freedom robot trajectory planning method considering tail end motion error
CN110900612B (en) Pose-synchronous six-axis industrial robot track smoothing method
CN109343345B (en) Mechanical arm polynomial interpolation track planning method based on QPSO algorithm
Liu et al. Development and implementation of a NURBS interpolator with smooth feedrate scheduling for CNC machine tools
CN108568817B (en) Delta robot track connection control method based on Bezier curve
Zhang et al. Curve fitting and optimal interpolation on CNC machines based on quadratic B-splines
CN105500354A (en) Transitional track planning method applied by industrial robot
Du et al. An error-bounded B-spline curve approximation scheme using dominant points for CNC interpolation of micro-line toolpath
He et al. A tolerance constrained G2 continuous path smoothing and interpolation method for industrial SCARA robots
CN109676613B (en) Error-controllable arc transition type smooth track generation method for four-axis industrial robot
CN111283683B (en) Servo tracking accelerated convergence method for robot visual feature planning track
CN111633668B (en) Motion control method for robot to process three-dimensional free-form surface
CN113103240B (en) Method, device and system for realizing C2 continuous robot trajectory planning
CN111515954B (en) Method for generating high-quality motion path of mechanical arm
Li et al. A novel cartesian trajectory planning method by using triple nurbs curves for industrial robots
CN113276116B (en) Error-controllable robot track synchronous transition method
CN116117796B (en) Industrial robot gesture track transition and speed planning method and system
Yang et al. Optimal configuration for mobile robotic grinding of large complex components based on redundant parameters
CN115202293B (en) Two-section type speed planning method for industrial robot
CN113787525B (en) Mechanical arm movement time optimization method based on joint performance limitation

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