CN113787525B - Mechanical arm movement time optimization method based on joint performance limitation - Google Patents
Mechanical arm movement time optimization method based on joint performance limitation Download PDFInfo
- Publication number
- CN113787525B CN113787525B CN202111366701.3A CN202111366701A CN113787525B CN 113787525 B CN113787525 B CN 113787525B CN 202111366701 A CN202111366701 A CN 202111366701A CN 113787525 B CN113787525 B CN 113787525B
- Authority
- CN
- China
- Prior art keywords
- value
- track
- point
- mechanical arm
- joint
- 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
Links
- 230000033001 locomotion Effects 0.000 title claims abstract description 51
- 238000000034 method Methods 0.000 title claims abstract description 29
- 238000005457 optimization Methods 0.000 title claims abstract description 15
- 230000001133 acceleration Effects 0.000 claims abstract description 87
- 239000012636 effector Substances 0.000 claims abstract description 51
- 230000010354 integration Effects 0.000 claims abstract description 29
- 238000013507 mapping Methods 0.000 claims abstract description 14
- 238000012545 processing Methods 0.000 claims abstract description 10
- 239000011159 matrix material Substances 0.000 claims description 45
- 238000009499 grossing Methods 0.000 claims description 32
- 238000003491 array Methods 0.000 claims description 10
- 230000008878 coupling Effects 0.000 claims description 9
- 238000010168 coupling process Methods 0.000 claims description 9
- 238000005859 coupling reaction Methods 0.000 claims description 9
- 230000007935 neutral effect Effects 0.000 claims description 9
- 230000008569 process Effects 0.000 claims description 8
- 230000008859 change Effects 0.000 claims description 6
- 230000003247 decreasing effect Effects 0.000 claims description 4
- 101100149325 Escherichia coli (strain K12) setC gene Proteins 0.000 claims description 3
- 238000009795 derivation Methods 0.000 claims description 3
- 230000005484 gravity Effects 0.000 claims description 3
- 238000010606 normalization Methods 0.000 claims description 3
- 238000012163 sequencing technique Methods 0.000 claims description 3
- 239000013598 vector Substances 0.000 claims description 3
- 238000012795 verification Methods 0.000 claims description 3
- 238000004422 calculation algorithm Methods 0.000 abstract description 3
- 238000004364 calculation method Methods 0.000 description 8
- 238000010586 diagram Methods 0.000 description 3
- 230000009286 beneficial effect Effects 0.000 description 2
- 238000004519 manufacturing process Methods 0.000 description 2
- 238000012549 training Methods 0.000 description 2
- 238000013528 artificial neural network Methods 0.000 description 1
- 238000006243 chemical reaction Methods 0.000 description 1
- 230000007547 defect Effects 0.000 description 1
- 230000000694 effects Effects 0.000 description 1
- 230000006872 improvement Effects 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 238000004088 simulation Methods 0.000 description 1
- 238000006467 substitution reaction Methods 0.000 description 1
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/1612—Programme controls characterised by the hand, wrist, grip 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/1628—Programme controls characterised by the control loop
- B25J9/1633—Programme controls characterised by the control loop compliant, force, torque control, e.g. combined with position 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/1628—Programme controls characterised by the control loop
- B25J9/1651—Programme controls characterised by the control loop acceleration, rate 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
Landscapes
- Engineering & Computer Science (AREA)
- Robotics (AREA)
- Mechanical Engineering (AREA)
- Health & Medical Sciences (AREA)
- General Health & Medical Sciences (AREA)
- Orthopedic Medicine & Surgery (AREA)
- Numerical Control (AREA)
- Manipulator (AREA)
Abstract
The invention belongs to the technical field of mechanical arm control, and particularly discloses a joint performance limitation-based methodThe method for optimizing the motion time of the mechanical arm comprises the following steps: (1) constructing a mapping; (2) carrying out torque limitation on a mechanical arm joint motor; (3) carrying out acceleration solution on the locus points to obtain an acceleration limit value; (4) mapping the speed limit of the mechanical arm joint motor to the speed limit of the end effector moving along the track, and acquiring the speed limit of the end effector corresponding to the speed limit of the joint motor (A)) A plane curve; (5) carrying out front and back numerical integration processing on acceleration limit values of various characteristic track point positions to obtain corresponding torque limit based on joint motor) A plane curve; (6) combining the curves to obtain a track length-speed limit curve; (7) and finishing the optimization of the motion time of the mechanical arm. The method has simple algorithm, effectively improves the movement speed of the mechanical arm based on the performance of the joint motor of the mechanical arm, and realizes the optimization of the movement time.
Description
Technical Field
The invention relates to the technical field of mechanical arm control, in particular to a mechanical arm movement time optimization method based on joint performance limitation.
Background
The application of the mechanical arm greatly improves the production efficiency, and along with the implementation of an intelligent manufacturing policy, the application range of the robot is more and more extensive. When the mechanical arm is designed, the performance of the joint motor usually depends on the working experience of an engineer, and the fixed motion trail is subjected to simulation calculation so as to determine the type selection of the joint motor. For a user of the mechanical arm, the planned motion track is complex and variable, and when the mechanical arm runs along the track set by the user, the end effector of the mechanical arm often moves along the track at a low speed, which ensures that the mechanical arm can complete the planned track, but does not exert the maximum efficiency of the performance of the joint motor. In the prior art, in order to improve the speed of the motion track of the mechanical arm and optimize the motion time, intelligent algorithms such as a neural network are mostly adopted, but a large amount of data sets are needed for training, so that not only is the time consumed, but also engineering experience is needed for parameter adjustment.
Disclosure of Invention
The invention aims to overcome the defects of the prior art and provides a mechanical arm movement time optimization method based on joint performance limitation.
The purpose of the invention is realized by the following technical scheme:
a mechanical arm movement time optimization method based on joint performance limitation comprises the following steps:
(1) coupling relations exist among all joint motors of the mechanical arm, and the coupling relations can be described through Lagrange equations, wherein the Lagrange equations are as follows:
in the formula,M(θ)is a 6 x 6 joint space inertia matrix,a 6 x 6 coupling matrix of coriolis forces and centripetal forces,G(θ)is a 6 x 1 matrix of gravity vectors,θis a rotation angle matrix of a manipulator joint space of 6 multiplied by 1,τa 6 x 1 joint moment matrix of the mechanical arm;is a pair ofθDerivation, namely a velocity matrix of the mechanical arm joint;is a pair ofθSolving two leads, namely an acceleration matrix of the mechanical arm joint;
the above equation can be rewritten with the christofel notation as the following kinetic equation of motion:
in the formula,is a transposed matrix of the velocity matrix of the mechanical arm joint,a three-dimensional tensor of 6 x 6;
in the formula,sfor the length of the track that moves along the track,the speed at which the end effector of the robotic arm moves along the trajectory,is the acceleration of the end effector of the robotic arm along the trajectory,θ s is a differential matrix of joint angle versus track length,θ ss a second order differential matrix of joint angle versus track length;
(2) the moment to arm joint motor restricts, specifically does:
in the formula,iis the first of a robot armiThe joint is formed by a plurality of joints,τ i is the first of a robot armiThe moment of each joint motor at a certain moment,τ i is composed ofτOne of the values in the matrix;τ i minis as followsiThe minimum torque of the motor of each joint,τ i maxis as followsiMaximum torque of each joint motor;
according to the above equation formulas (1), (2), (3) and (4), the following results are obtainedThe plane is based on the constraint inequality of mechanical arm joint motor moment restriction:
in the formula,the angle value of the mechanical arm joint is mapped through the track length, and is specifically a mapping relation function;
(3) carrying out acceleration solving on the locus points; acquiring the acceleration range of the end effector which can be provided by each mechanical arm joint on the track point, taking the maximum value from the intersection of the acceleration ranges of the end effector which can be provided by each mechanical arm joint on the track point as the acceleration limit value of the track point, and recording the maximum value as the acceleration limit valuesdd(ii) a Wherein the symbols in the constraint inequality (5) are transformed:
thereby converting the constraint inequality into:
wherein, according to the operation rulea(s)、b(s)Andc(s)all of which are 12 × 1 matrices;
according to the constraint inequality (6), the upper limit value of the acceleration of the end effector and the lower limit value of the acceleration of the end effector, which can be provided by each joint of the mechanical arm at the locus point, are obtained as follows:
when in usea i (s)When the acceleration is more than 0, the mechanical arm is closed, the energy can be provided, and the acceleration upper limit value of the end effector is as follows:
when in usea i (s)When the acceleration is less than 0, the lower limit value of the acceleration of the end effector provided by the mechanical arm closing energy is as follows:
when in usea i (s)When the speed is not less than 0, selecting a smaller end effector acceleration limiting value in two adjacent track point positions before and after the track point position as an end effector acceleration limiting value which can be provided by the track point position;
in the formula (7) and the formula (8),iis the first on the mechanical armiAn individual joint;a i (s) Is a 12 x 1 matrixa(s)Is of neutral degreeiThe numerical values corresponding to the individual joints are,b i (s) Is a 12 x 1 matrixb(s)Is of neutral degreeiThe numerical values corresponding to the individual joints are,c i (s) Is a 12 x 1 matrixc(s)Is of neutral degreeiThe numerical value corresponding to each joint;
(4) mapping a speed limit of a robotic arm joint motor to an end effector speed limit moving along a trajectory, specifically:
in the formula,tas a matter of time, the time is,is the amount of change in the angle of the arm joint, dtFor integration time interval, dsIn order to be the amount of change in the track length,in order to limit the speed of the end effector of the robotic arm,the rear part of the formula (9) is specifically that the speed limit of the mechanical arm joint motor is converted into the speed limit of the mechanical arm end effector;
obtaining the corresponding speed limit of the joint motor on all discrete points along the appointed motion track through the mapping of the formula (9)A plane curve;
(5) carrying out front and back numerical integration processing on acceleration limit values of various characteristic track point positions to obtain the acceleration limit value corresponding to the moment limit based on the joint motorA plane curve; acceleration limit values of various characteristic track point locations are obtained through the step (3), and the various characteristic track point locations comprise a starting point, an ending point and a discontinuous point; specifically, the integration formula is as follows:
in the formula,is the track length value of the track point forward,is the forward speed value of the locus point,is the length value of the track at the current track point,the velocity value of the mechanical arm end effector at the current track point position,is the value of the length of the track backward of the point location of the track,the backward velocity value of the locus point is taken as the velocity value,dtin order to be an integration time interval,sddthe acceleration limit value of the track point position obtained in the step (3) is obtained; the forward direction of the track point location is the direction from 1 to 0 of the track length, the backward direction of the track point location is the direction from 0 to 1 of the track length, and the motion track of the mechanical arm is determined, so the track length can be obtained by adopting a normalization methodsIn the range of 0 to 1;
(6) corresponding the speed limit based on the joint motor obtained in the step (4)The plane curve corresponds to the torque limit obtained in step (5) based on the joint motorCombining the plane curves; corresponding to torque limits based on joint motorsHigher in plane curve than what corresponds based on joint motor speed limitThe part of the plane curve is omitted, and the smaller speed is taken as the curve speed value at the same point, so that a continuous track meeting the speed limit and the torque limit of the joint motor at the same time is obtainedA length-speed limit curve;
(7) and (4) taking the track length-speed limit curve obtained in the step (6) as a basis condition, and enabling the mechanical arm end effector to operate on the appointed motion track to complete optimization of the motion time of the mechanical arm.
According to a preferable scheme of the invention, in the step (5), when the front and back numerical integration is carried out on the discontinuity point, the selection of the initial value of the velocity integration at the discontinuity point is gradually increased so as to obtain the highest acceleration value of the discontinuity point under the condition of meeting the track length for integration; when the selection of the speed integral initial value at the intermittent point is gradually improved, the minimum value at the intermittent point is continuously approached according to a preset fixed growth proportion, and the selection of the speed integral initial value is stopped by setting a threshold value.
According to a preferred scheme of the invention, the speed plan of the track length-speed limit curve is smoothed by adopting a plurality of times of B-spline curves, so that a smoothed track length-speed limit curve is obtained.
Preferably, willuRecording the ratio of the distance from the discrete point on the track length-speed limit curve to the track starting point to the track length, and makingFor a sequence of arrays that is monotonically non-decreasing,nis the array sequenceUThe number of the elements in the Chinese character can be selected by self-definition; thereinu 0,u 1,…u n Representing different discrete points on the path length-speed limit curveuValue, i.e. in the following formula (14)uThe specific numerical values of (a); meanwhile, a cubic B-spline curve is adopted to carry out smoothing treatment on the track length-speed limiting curve, so that a cubic B-spline curve equation can be expressed:
in the formula,p(u)the speed value is mapped on a track length-speed limit curve;is composed ofkA B-spline basis function of power, whereink=3;jSequence of arrays for the basis functionUIn the above-mentioned position(s),C j is a control point;
further, the air conditioner is provided with a fan,p(u)the value can be passed throughuValue derived, so control points in the above equation (14)C j Is a unique variable; at this point, all of the arrays in the sequence are sorteduSubstituting the values into equation (14) and standard DeBoolean-Corx recursion equation to obtain the control pointsC j And sequencing to obtain a smooth track length-speed limit curve.
Preferably, the speed of the smoothed track length-speed limit curve is verified; selecting a plurality of discrete points to be verified on the track length-speed limit curve after the smoothing treatment, and smoothing speed value of each discrete point to be verifiedV′The speed value corresponding to the track length-speed limit curve of the discrete point to be verified before the smoothing processVComparing, and adjusting a curve according to a comparison result;
when in useV′≤VWhen the current is needed, no treatment is carried out;
when in useV′>VThen all control points will be setC j By a specified ratio, and again comparing the discrete pointsV′Value andVvalue until all selected discrete points to be verified are satisfiedV′≤V。
Preferably, after the smooth speed verification of the track length-speed limit curve after the smoothing processing is completed, the smooth acceleration of the track length-speed limit curve after the smoothing processing is verified; selecting a plurality of discrete points to be verified on the track length-speed limit curve after the smoothing treatment, and respectively calculating smooth acceleration values for the same discrete points to be verifiedaAnd smoothing the acceleration limit valuesdd′And for the smooth acceleration value of the same discrete point to be verifiedaAnd smoothing the acceleration limit valuesdd' a comparison and adjustment process is performed,in which the acceleration limit is smoothedsdd' by step (3), the smoothed acceleration value can be obtainedaThe following formula is used to obtain:
in the formula,for the smooth velocity values of the discrete points to be verified,is the value of the smoothed velocity of the neighboring point,dmthe distance between the discrete point to be verified and the adjacent point is defined;
if it isa ≤ sdd', do not process;
if it isa > sdd′Then all control points will beC j By a specified ratio, recalculating and comparing the discrete pointsaValue andsdd′value until all selected discrete points to be verified are satisfieda ≤ sdd′。
Compared with the prior art, the invention has the following beneficial effects:
1. on the premise that the motion trail of the mechanical arm is determined, the motion speed of the mechanical arm is limited, guidance on the motion trail of the mechanical arm is realized through a trail length-speed limiting curve, the performance of a joint motor is fully exerted, the mechanical arm can complete trail motion quickly and efficiently, the motion time is shortened, and the aim of optimizing the motion time of the mechanical arm is fulfilled.
2. The time optimization plan of the preset track of the mechanical arm is solved from the condition factors of the moment limit and the speed limit of the joint motor of the mechanical arm, so that the optimization accuracy of the motion time of the mechanical arm is high, the performance of the joint motor can be fully exerted, and the mechanical arm can move along the preset track at the speed as high as possible.
3. The invention realizes the optimization of the motion time of the mechanical arm by adopting a numerical algorithm mode, has the real-time calculation effect, small calculation amount, no need of massive data training or complicated calculation process, real-time property and good reliability, and can adapt to any preset motion track.
Drawings
FIG. 1 is a block flow diagram of a method for optimizing robot arm movement time based on joint performance constraints according to the present invention.
FIG. 2 is a graph corresponding to joint motor speed limitSchematic of the planar curve (continuous).
FIG. 3 is a graph corresponding to joint motor speed limitSchematic of the planar curve (interrupted).
FIG. 4 is a graph corresponding to joint motor speed limitPlane curve (broken thin solid line L2) and corresponding based on joint motor torque limitationThe plane curve (dotted line L3) is schematically shown.
Fig. 5 is a diagram of a trajectory length-speed limit curve (thick solid line L4).
Fig. 6 is a schematic diagram of a trajectory length-speed limit curve (two-dot chain line L5) after the smoothing process.
Detailed Description
The present invention will be further described with reference to the following examples and drawings, but the embodiments of the present invention are not limited thereto.
Referring to fig. 1 to 6, the present embodiment discloses a method for optimizing a robot arm movement time based on joint performance limitation, including the following steps:
(1) coupling relations exist among all joint motors of the mechanical arm, and the coupling relations can be described through Lagrange equations, wherein the Lagrange equations are as follows:
in the formula,M(θ)is a 6 x 6 joint space inertia matrix,a 6 x 6 coupling matrix of coriolis forces and centripetal forces,G(θ)is a 6 x 1 matrix of gravity vectors,θis a rotation angle matrix of a manipulator joint space of 6 multiplied by 1,τa 6 x 1 joint moment matrix of the mechanical arm;is a pair ofθDerivation, namely a velocity matrix of the mechanical arm joint;is a pair ofθSolving two leads, namely an acceleration matrix of the mechanical arm joint;
the above equation can be rewritten with the christofel notation as the following kinetic equation of motion:
in the formula,is a transposed matrix of the velocity matrix of the mechanical arm joint,is a three-dimensional tensor of 6 x 6.
The time optimal problem of the motion track of the mechanical arm is actually to solve the maximum value problem of the speed of the mechanical arm under the condition of meeting a kinetic equation and convert the problem into the maximum value problemThe solved target can be more intuitively felt on the plane. The Lagrange equation describes the motion state of the mechanical arm in a joint space, so that the Lagrange equation is mapped to the mechanical arm in a structural mapping modePlane, thus giving:
in the formula,sfor the length of the track that moves along the track,the speed at which the end effector of the robotic arm moves along the trajectory,is the acceleration of the end effector of the robotic arm along the trajectory,θ s is a differential matrix of joint angle versus track length,θ ss is a second order differential matrix of joint angle versus track length.
(2) The moment to arm joint motor restricts, specifically does:
in the formula,iis the first of a robot armiThe joint is formed by a plurality of joints,τ i is the first of a robot armiThe moment of each joint motor at a certain moment,τ i is composed ofτOne of the values in the matrix;τ i minis as followsiThe minimum torque of the motor of each joint,τ i maxis as followsiMaximum torque of each joint motor;
according to the above equationFormulas (1), (2), (3) and (4) are obtained inThe plane is based on the constraint inequality of mechanical arm joint motor moment restriction:
in the formula,the angle value of the mechanical arm joint is mapped through the track length, and the mapping relation function is specific.
(3) Carrying out acceleration solving on the locus points; acquiring the acceleration range of the end effector which can be provided by each mechanical arm joint on the track point, taking the maximum value from the intersection of the acceleration ranges of the end effector which can be provided by each mechanical arm joint on the track point as the acceleration limit value of the track point, and recording the maximum value as the acceleration limit valuesdd(ii) a Wherein the symbols in the constraint inequality (5) are transformed:
thereby converting the constraint inequality into:
wherein, according to the operation rulea(s)、b(s)Andc(s)all of which are 12 × 1 matrices;
according to the constraint inequality (6), the upper limit value of the acceleration of the end effector and the lower limit value of the acceleration of the end effector, which can be provided by each joint of the mechanical arm on the locus point, are obtained through the following equation. Since the robot arm used in this embodiment is a six-axis robot arm, and after the foregoing calculation, there are two results, that is, less than the inequality (formula (7)) and greater than the inequality (formula (8)), for each robot arm joint, each trajectory point will include 12 inequalities in general.
When in usea i (s)When the acceleration is more than 0, the mechanical arm is closed, the energy can be provided, and the acceleration upper limit value of the end effector is as follows:
when in usea i (s)When the acceleration is less than 0, the lower limit value of the acceleration of the end effector provided by the mechanical arm closing energy is as follows:
when in usea i (s)And when the speed is not less than 0, selecting a smaller end effector acceleration limit value of two adjacent track point positions before and after the track point position as an end effector acceleration limit value which can be provided by the track point position. In particular, for example, when the robot arm jointiCorresponding tos0.1, take the multiple integral dotted linessThe minimum value of the acceleration of the end effector which can be provided in the vicinity of =0.1 is taken as the upper limit value of the acceleration of the end effector which can be provided by the robot arm switch (it can be understood here ifai(s)=0, then the calculation of the locus point is left alone, after all other non-zero discrete points are calculated, a plurality of dotted lines are formed by integration, and then the adjacent point of the locus point is obtained from the plurality of dotted linessddThe minimum being the point of the trajectorysddThereby completing the dotted line of the locus point).
In the above-mentioned formula (7) and formula (8),iis the first on the mechanical armiAn individual joint;a i (s) Is a 12 x 1 matrixa(s)Is of neutral degreeiThe numerical values corresponding to the individual joints are,b i (s) Is a 12 x 1 matrixb(s)Is of neutral degreeiThe numerical values corresponding to the individual joints are,c i (s) Is a 12 x 1 matrixc(s)Is of neutral degreeiThe numerical value corresponding to each joint.
(4) Mapping the speed limit of the joint motor of the mechanical arm to the speed limit of the end effector moving along the track, and converting the speed limit of the joint motor of the mechanical arm into the speed limit of the end effector moving along the track through conversionA limit on a plane; specifically, the method comprises the following steps:
in the formula,tas a matter of time, the time is,is the amount of change in the angle of the arm joint, dtFor integration time interval, dsIn order to be the amount of change in the track length,in order to limit the speed of the end effector of the robotic arm,the rear part of the formula (9) is specifically that the speed limit of the mechanical arm joint motor is converted into the speed limit of the mechanical arm end effector;
obtaining the corresponding speed limit of the joint motor on all discrete points along the appointed motion track through the mapping of the formula (9)A plane curve. As shown by the thin solid line L1 in FIG. 2, the plane curve is a continuous curve, butMore generally, the plane curve has discontinuities P1, P2, or even more discontinuities, as shown by the thin solid line L2 in fig. 3.
(5) Carrying out front and back numerical integration processing on acceleration limit values of various characteristic track point positions to obtain the acceleration limit value corresponding to the moment limit based on the joint motorThe flat curve, which also includes a plurality of discrete curves, is shown by the dashed line L3 in FIG. 4, while the thin solid line L2 in FIG. 4 corresponds to the joint motor speed limitA plane curve. Acceleration limit values of various characteristic track point locations are obtained through the step (3), and the various characteristic track point locations comprise a starting point, an ending point and a discontinuous point; specifically, the integration formula is as follows:
in the formula,is the track length value of the track point forward,is the forward speed value of the locus point,is the length value of the track at the current track point,the velocity value of the mechanical arm end effector at the current track point position,is the value of the length of the track backward of the point location of the track,the backward velocity value of the locus point is taken as the velocity value,dtin order to be an integration time interval,sddthe acceleration limit value of the track point position obtained in the step (3) is obtained; the forward direction of the track point location is the direction from 1 to 0 of the track length, the backward direction of the track point location is the direction from 0 to 1 of the track length, and the motion track of the mechanical arm is determined, so the track length can be obtained by adopting a normalization methodsIn the range of 0 to 1.
In addition, when the front and back numerical integration is carried out on the discontinuity point, the selection of the initial value of the velocity integration at the discontinuity point is gradually improved so as to obtain the highest acceleration value of the discontinuity point under the condition of meeting the track length for integration; when the selection of the speed integral initial value at the intermittent point is gradually improved, the minimum value at the intermittent point is continuously approached according to a preset fixed growth proportion, and the selection of the speed integral initial value is stopped by setting a threshold value. Specifically, when the front and rear numerical integration is performed on the discontinuity point, one third of the smaller value at the discontinuity point is selected as the initial value of the speed integration, after the numerical integration, if the range of the track length is 0-1, the selection of the initial value of the speed integration is continuously improved, the initial value of the speed integration is selected as two thirds of the smaller value at the discontinuity point, and the numerical integration is repeated until the smaller value at the discontinuity point is gradually approached on the premise of meeting the range of the track length 0-1; the selection of the starting value of the velocity integral may be terminated by setting an error threshold with a smaller value at the discontinuity. In this way, it is beneficial to make the discontinuity as high as possible on the (s, s ̇) plane corresponding to the torque limit of the joint motor, so that the higher the speed limit value of the discontinuity in the subsequent processing, the higher the speed of the mechanical arm during movement, and the further optimization of the movement time.
(6) Corresponding the speed limit based on the joint motor obtained in the step (4)The plane curve (shown by thin solid line L2 in FIGS. 3 and 4) and the curve obtained in step (5)Based on torque-limiting correspondence of joint motorThe planar curves (shown as dashed line L3 in fig. 4) are combined. When combined, will correspond based on joint motor torque limitationsHigher in plane curve than what corresponds based on joint motor speed limitThe portion of the plane curve is discarded, and a smaller speed is taken at the same point as the curve speed value, so as to obtain a continuous track length-speed limit curve which simultaneously satisfies the joint motor speed limit and the joint motor torque limit, as shown by a thick solid line L4 in fig. 5, and a thin solid line L2 in fig. 5 is a curve corresponding to the joint motor speed limitPlane curve, dotted line corresponding based on torque limitation of joint motorThe truncated portion of the plane curve.
(7) The velocity plan of the trajectory length-velocity limit curve is smoothed using a multiple B-spline curve to obtain a smoothed trajectory length-velocity limit curve, as shown by the two-dot chain line L5 in fig. 6.
Firstly, the following components are mixeduRecording the ratio of the distance from the discrete point on the track length-speed limit curve to the track starting point to the track length, and makingFor a sequence of arrays that is monotonically non-decreasing,nis the array sequenceUThe number of the elements in the Chinese character can be selected by self-definition; thereinu 0,u 1,…u n Expressing track length-speed limit curveOf different discrete points on the lineuValue, i.e. in the following formula (14)uThe specific numerical values of (a); meanwhile, a cubic B-spline curve is adopted to carry out smoothing treatment on the track length-speed limiting curve, so that a cubic B-spline curve equation can be expressed:
in the formula,p(u)the speed value is mapped on a track length-speed limit curve;is composed ofkA B-spline basis function of power, whereink=3;jSequence of arrays for the basis functionUIn the above-mentioned position(s),C j is a control point;
in the course of the above-mentioned treatment,p(u)the value can be derived from the value of u, so the control point in equation (14) aboveC j Is a unique variable; at this point, all of the arrays in the sequence are sorteduSubstituting the values into equation (14) and standard DeBoolean-Corx recursion equation to obtain the control pointsC j And sequencing to obtain a smooth track length-speed limit curve.
The standard de boolean-cox recurrence formula in this example is as follows:
in the formula,jsequence of arrays for the basis functionUIn the above-mentioned position(s),k=3;
meanwhile, due to the characteristics of the cubic B-spline curve, two additional control points need to be added, and the equation sets of the two additional control points in this embodiment are two equations in which the velocities at the starting point and the ending point of the trajectory are 0, respectively.
For example, with a track length of 100cm as a reference, a ratio of 50 discrete points can be selected to determine the control pointC j Because of the characteristics of the cubic B-spline curve, two additional control points are added, so that there are 52 control points. The 50 discrete points are uniformly selected over the track length of 100cm, namelyu1/50, 2/50, 3/50, … and 50/50 respectively, and according to the aboveuThe value can be obtained accordinglyp(u)A value; thus, 50 equations for obtaining the control points, i.e., 50 equations of the above equation (14), are obtained, and then 50 control points are obtained by solving the equations. After the control points are obtained, according to the characteristics of the B-spline curve, a track length-speed limit curve subjected to smoothing processing can be obtained.
(8) And verifying the speed of the track length-speed limit curve after the smoothing treatment. Selecting a plurality of discrete points to be verified on the track length-speed limit curve after the smoothing treatment, and smoothing speed value of each discrete point to be verifiedV′The speed value corresponding to the track length-speed limit curve of the discrete point to be verified before the smoothing processVComparing, and adjusting a curve according to a comparison result;
when in useV′≤VWhen the current is needed, no treatment is carried out;
when in useV′>VThen all control points will be setC j By a specified ratio, and again comparing the discrete pointsV′Value andVvalue until all selected discrete points to be verified are satisfiedV′≤V. For example, for a reference value with a track length of 100cm, 100 discrete points may be equidistantly selected, and a smooth velocity value of the 100 discrete points on the smoothed curve is sequentially calculated, and compared with the velocity value on the smoothed curve, and the smoothed curve is adjusted according to the comparison result. When adjusting, all control points can be controlled firstC j The vertical coordinate of the control point is reduced by 10 percent, and after the comparison and calculation are carried out again, if the condition is not met, all the control points are continuously controlledC j The vertical coordinate of (a) is decreased by 10% again until all the discrete points satisfy the condition position.
(9) The smooth acceleration of the track length-speed limit curve after the smoothing treatment is verified, so that the smooth acceleration is obtainedThe adjusted trajectory length-speed limit curve is smoothed and verified. Selecting a plurality of discrete points to be verified on the track length-speed limit curve after the smoothing treatment, and respectively calculating smooth acceleration values for the same discrete points to be verifiedaAnd smoothing the acceleration limit valuesdd′And for the smooth acceleration value of the same discrete point to be verifiedaAnd smoothing the acceleration limit valuesdd′Performing comparison and adjustment processes in which the acceleration limit value is smoothedsdd′The smooth acceleration value can be obtained through the step (3)aThe following formula is used to obtain:
in the formula,for the smooth velocity values of the discrete points to be verified,is the value of the smoothed velocity of the neighboring point,dmthe distance between the discrete point to be verified and the adjacent point is defined; wherein the adjacent points can be selected to the left or right of the discrete point to be verified, where the purpose is to smooth the acceleration values of the discrete point to be verifiedaThe calculation is carried out, and the selection of the adjacent points is not limited;
if it isa ≤ sdd', do not process;
if it isa > sdd′Then all control points will beC j By a specified ratio, recalculating and comparing the discrete pointsaValue andsdd′value until all selected discrete points to be verified are satisfieda ≤ sdd′。
(10) And (4) taking the track length-speed limit curve subjected to smoothing, verification and adjustment in the step (9) as a basis condition, and enabling the end effector of the mechanical arm to operate on the appointed motion track to complete optimization of the motion time of the mechanical arm.
In this embodiment, after the track length-speed limit curve is obtained, since the curve is not smooth, a B-spline curve is further adopted for smoothing, so that the speed of the mechanical arm when the mechanical arm performs the specified motion track is smoother, thereby facilitating improvement of the stability of the mechanical arm when the mechanical arm moves. Meanwhile, the smoothed curve is verified to ensure that the smoothed curve is still within the range limited by the speed and the moment of the joint motor of the mechanical arm, so that the reliability of the optimized curve is improved. Of course, for the trajectory length-speed limit curve of the present embodiment, the specific B-spline curve smoothing process may refer to the prior art, and may also be performed in other manners.
The present invention is not limited to the above embodiments, and any other changes, modifications, substitutions, combinations, and simplifications which do not depart from the spirit and principle of the present invention should be construed as equivalents and are included in the scope of the present invention.
Claims (6)
1. A mechanical arm movement time optimization method based on joint performance limitation is characterized by comprising the following steps:
(1) coupling relations exist among all joint motors of the mechanical arm, and the coupling relations can be described through Lagrange equations, wherein the Lagrange equations are as follows:
in the formula,M(θ)is a 6 x 6 joint space inertia matrix,a 6 x 6 coupling matrix of coriolis forces and centripetal forces,G (θ)is a 6 x 1 matrix of gravity vectors,θis a rotation angle matrix of a manipulator joint space of 6 multiplied by 1,τa 6 x 1 joint moment matrix of the mechanical arm;the derivation is theta, namely a speed matrix of the mechanical arm joint;is a pair ofθSolving two leads, namely an acceleration matrix of the mechanical arm joint;
the above equation can be rewritten with the christofel notation as the following kinetic equation of motion:
in the formula,is a transposed matrix of the velocity matrix of the mechanical arm joint,a three-dimensional tensor of 6 x 6;
in the formula,sfor the length of the track that moves along the track,the speed at which the end effector of the robotic arm moves along the trajectory,for adding the end-effector of a robot arm to the movement along a trajectoryThe speed of the motor is controlled by the speed of the motor,θ s is a differential matrix of joint angle versus track length,θ ss a second order differential matrix of joint angle versus track length;
(2) the moment to arm joint motor restricts, specifically does:
in the formula,iis the first of a robot armiThe joint is formed by a plurality of joints,τ i is the first of a robot armiThe moment of each joint motor at a certain moment,τ i is composed ofτOne of the values in the matrix;τ i min is as followsiThe minimum torque of the motor of each joint,τ i max is as followsiMaximum torque of each joint motor;
according to the above equation formulas (1), (2), (3) and (4), the following results are obtainedThe plane is based on the constraint inequality of mechanical arm joint motor moment restriction:
in the formula,the angle value of the mechanical arm joint is mapped through the track length, and is specifically a mapping relation function;
(3) carrying out acceleration solving on the locus points; first acquiring each machine at the locus pointThe acceleration range of the end effector which can be provided by the arm joint, and the maximum value in the intersection of the acceleration ranges of the end effector which can be provided by each mechanical arm joint on the track point is taken as the acceleration limit value of the track point and recorded as the acceleration limit valuesdd(ii) a Wherein the symbols in the constraint inequality (5) are transformed:
thereby converting the constraint inequality into:
wherein, according to the operation rulea(s)、b(s)Andc(s)all of which are 12 × 1 matrices;
according to the constraint inequality (6), the upper limit value of the acceleration of the end effector and the lower limit value of the acceleration of the end effector, which can be provided by each joint of the mechanical arm at the locus point, are obtained as follows:
when in usea i (s)When the acceleration is more than 0, the mechanical arm is closed, the energy can be provided, and the acceleration upper limit value of the end effector is as follows:
when in usea i (s)When the acceleration is less than 0, the lower limit value of the acceleration of the end effector provided by the mechanical arm closing energy is as follows:
when in usea i (s)When the speed is not less than 0, the smaller end effector acceleration limiting value in the two adjacent track point positions before and after the track point position is selected as the track point potential energyA provided end effector acceleration limit;
in the formula (7) and the formula (8),iis the first on the mechanical armiAn individual joint;a i (s) Is a 12 x 1 matrixa(s)Is of neutral degreeiThe numerical values corresponding to the individual joints are,b i (s) Is a 12 x 1 matrixb(s)Is of neutral degreeiThe numerical values corresponding to the individual joints are,c i (s) Is a 12 x 1 matrixc(s)Is of neutral degreeiThe numerical value corresponding to each joint;
(4) mapping a speed limit of a robotic arm joint motor to an end effector speed limit moving along a trajectory, specifically:
in the formula,tas a matter of time, the time is,is the amount of change in the angle of the arm joint, dtFor integration time interval, dsIn order to be the amount of change in the track length,in order to limit the speed of the end effector of the robotic arm,the rear part of the formula (9) is specifically that the speed limit of the mechanical arm joint motor is converted into the speed limit of the mechanical arm end effector;
obtaining the corresponding speed limit of the joint motor on all discrete points along the appointed motion track through the mapping of the formula (9)A plane curve;
(5) for each classCarrying out front and back numerical integration processing on the acceleration limit value of the symbolic track point position to obtain the acceleration limit value corresponding to the torque limit based on the joint motorA plane curve; acceleration limit values of various characteristic track point locations are obtained through the step (3), and the various characteristic track point locations comprise a starting point, an ending point and a discontinuous point; specifically, the integration formula is as follows:
in the formula,is the track length value of the track point forward,is the forward speed value of the locus point,is the length value of the track at the current track point,the velocity value of the end effector of the mechanical arm at the current locus point,is the value of the length of the track backward of the point location of the track,the backward velocity value of the locus point is taken as the velocity value,dtin order to be an integration time interval,sddthe acceleration limit value of the track point position obtained in the step (3) is obtained; the forward direction of the locus point is the direction from 1 to 0 of the locus length, and the backward direction of the locus point is the locusThe length is from 0 to 1, and the motion track of the mechanical arm is determined, so the track length can be obtained by adopting a normalization methodsIn the range of 0 to 1;
(6) corresponding the speed limit based on the joint motor obtained in the step (4)The plane curve corresponds to the torque limit obtained in step (5) based on the joint motorCombining the plane curves; corresponding to torque limits based on joint motorsHigher in plane curve than what corresponds based on joint motor speed limitThe part of the plane curve is omitted, and meanwhile, a smaller speed is taken at the same point position as a curve speed value, so that a continuous track length-speed limiting curve which meets the speed limit and the torque limit of the joint motor at the same time is obtained;
(7) and (4) taking the track length-speed limit curve obtained in the step (6) as a basis condition, and enabling the mechanical arm end effector to operate on the appointed motion track to complete optimization of the motion time of the mechanical arm.
2. The method for optimizing the motion time of the mechanical arm based on the joint performance limitation as claimed in claim 1, wherein in the step (5), when the front and rear numerical integration is performed on the discontinuity point, the selection of the initial value of the velocity integration at the discontinuity point is gradually increased so as to obtain the highest acceleration value of the discontinuity point when the track length condition is met for integration; when the selection of the speed integral initial value at the intermittent point is gradually improved, the minimum value at the intermittent point is continuously approached according to a preset fixed growth proportion, and the selection of the speed integral initial value is stopped by setting a threshold value.
3. The method for optimizing mechanical arm motion time based on joint performance limitation as claimed in claim 1 or 2, wherein a plurality of times of B-spline curve is adopted to carry out smoothing processing on the speed plan of the track length-speed limitation curve, so as to obtain a smooth track length-speed limitation curve.
4. The method for optimizing mechanical arm movement time based on joint performance limit of claim 3, wherein the method is characterized in thatuRecording the ratio of the distance from the discrete point on the track length-speed limit curve to the track starting point to the track length, and makingFor a sequence of arrays that is monotonically non-decreasing,nis the array sequenceUThe number of the elements in the Chinese character can be selected by self-definition; thereinu 0,u 1,… u n Representing different discrete points on the path length-speed limit curveuValue, i.e. in the following formula (14)uThe specific numerical values of (a); meanwhile, a cubic B-spline curve is adopted to carry out smoothing treatment on the track length-speed limiting curve, so that a cubic B-spline curve equation can be expressed:
in the formula,p(u)the speed value is mapped on a track length-speed limit curve;is composed ofkA B-spline basis function of power, whereink=3;jSequence of arrays for the basis functionUIn the above-mentioned position(s),C j is a control point;
further, the air conditioner is provided with a fan,p(u)the value can be passed throughuValue derived, thereforeThe control point in the above equation (14)C j Is a unique variable; at this point, all of the arrays in the sequence are sorteduSubstituting the values into equation (14) and standard DeBoolean-Corx recursion equation to obtain the control pointsC j And sequencing to obtain a smooth track length-speed limit curve.
5. The method for optimizing mechanical arm movement time based on joint performance limitation according to claim 4, wherein the speed of the track length-speed limitation curve after the smoothing processing is verified; selecting a plurality of discrete points to be verified on the track length-speed limit curve after the smoothing treatment, and smoothing speed value of each discrete point to be verifiedV′The speed value corresponding to the track length-speed limit curve of the discrete point to be verified before the smoothing processVComparing, and adjusting a curve according to a comparison result;
when in useV′≤VWhen the current is needed, no treatment is carried out;
when in useV′>VThen all control points will be setC j By a specified ratio, and again comparing the discrete pointsV′Value andVvalue until all selected discrete points to be verified are satisfiedV′≤V。
6. The method for optimizing the motion time of the mechanical arm based on the joint performance limitation as claimed in claim 5, wherein after the verification of the smoothed velocity value of the track length-velocity limitation curve is completed, the smoothed acceleration value of the smoothed track length-velocity limitation curve is verified; selecting a plurality of discrete points to be verified on the track length-speed limit curve after the smoothing treatment, and respectively calculating smooth acceleration values for the same discrete points to be verifiedaAnd smoothing the acceleration limit valuesdd′And for the smooth acceleration value of the same discrete point to be verifiedaAnd smoothing the acceleration limit valuesdd′Performing comparison and adjustment processes in which the acceleration limit value is smoothedsdd′Through the step (3), the method can be carried outFinding a smoothed acceleration value thereinaThe following formula is used to obtain:
in the formula,for the smooth velocity values of the discrete points to be verified,is the value of the smoothed velocity of the neighboring point,dmthe distance between the discrete point to be verified and the adjacent point is defined;
if it isa ≤ sdd′No treatment is carried out;
if it isa > sdd′Then all control points will beC j By a specified ratio, recalculating and comparing the discrete pointsaValue andsdd' value, until all selected discrete points to be verified are satisfieda ≤ sdd′。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202111366701.3A CN113787525B (en) | 2021-11-18 | 2021-11-18 | Mechanical arm movement time optimization method based on joint performance limitation |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202111366701.3A CN113787525B (en) | 2021-11-18 | 2021-11-18 | Mechanical arm movement time optimization method based on joint performance limitation |
Publications (2)
Publication Number | Publication Date |
---|---|
CN113787525A CN113787525A (en) | 2021-12-14 |
CN113787525B true CN113787525B (en) | 2022-02-01 |
Family
ID=78955372
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN202111366701.3A Active CN113787525B (en) | 2021-11-18 | 2021-11-18 | Mechanical arm movement time optimization method based on joint performance limitation |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN113787525B (en) |
Families Citing this family (1)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN117564430B (en) * | 2024-01-15 | 2024-04-02 | 中国核动力研究设计院 | Pressurizing assembly, equipment and welding method for curved surface workpiece diffusion welding |
Citations (6)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
EP2384863A2 (en) * | 2010-01-21 | 2011-11-09 | Institutul de Mecanica Solidelor al Academiei Romane | Method and device for dynamic control of a walking robot |
CN109623810A (en) * | 2018-11-26 | 2019-04-16 | 南京航空航天大学 | A kind of method for the time optimal trajectory planning that robot is smooth |
CN110209048A (en) * | 2019-05-20 | 2019-09-06 | 华南理工大学 | Robot time optimal trajectory planning method, equipment based on kinetic model |
CN110421569A (en) * | 2019-08-29 | 2019-11-08 | 南京邮电大学 | A kind of inverting sliding formwork machinery arm controller design method based on finite time disturbance observer |
CN113119111A (en) * | 2021-03-18 | 2021-07-16 | 深圳市优必选科技股份有限公司 | Mechanical arm and track planning method and device thereof |
CN113290555A (en) * | 2021-05-08 | 2021-08-24 | 浙江大学 | Optimization method for time optimal control trajectory of industrial robot |
-
2021
- 2021-11-18 CN CN202111366701.3A patent/CN113787525B/en active Active
Patent Citations (6)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
EP2384863A2 (en) * | 2010-01-21 | 2011-11-09 | Institutul de Mecanica Solidelor al Academiei Romane | Method and device for dynamic control of a walking robot |
CN109623810A (en) * | 2018-11-26 | 2019-04-16 | 南京航空航天大学 | A kind of method for the time optimal trajectory planning that robot is smooth |
CN110209048A (en) * | 2019-05-20 | 2019-09-06 | 华南理工大学 | Robot time optimal trajectory planning method, equipment based on kinetic model |
CN110421569A (en) * | 2019-08-29 | 2019-11-08 | 南京邮电大学 | A kind of inverting sliding formwork machinery arm controller design method based on finite time disturbance observer |
CN113119111A (en) * | 2021-03-18 | 2021-07-16 | 深圳市优必选科技股份有限公司 | Mechanical arm and track planning method and device thereof |
CN113290555A (en) * | 2021-05-08 | 2021-08-24 | 浙江大学 | Optimization method for time optimal control trajectory of industrial robot |
Also Published As
Publication number | Publication date |
---|---|
CN113787525A (en) | 2021-12-14 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN106647282B (en) | Six-degree-of-freedom robot trajectory planning method considering tail end motion error | |
CN109343345B (en) | Mechanical arm polynomial interpolation track planning method based on QPSO algorithm | |
CN112975992B (en) | Error-controllable robot track synchronous optimization method | |
CN109240091B (en) | Underwater robot control method based on reinforcement learning and tracking control method thereof | |
Leottau et al. | Decentralized reinforcement learning of robot behaviors | |
CN109765887B (en) | Automatic driving control method | |
CN110928290B (en) | Quintic curve path planning method and lane center line maintenance method | |
CN105676636B (en) | A kind of redundancy space manipulator Multipurpose Optimal Method based on NSGA-II algorithm | |
Zhang et al. | Curve fitting and optimal interpolation on CNC machines based on quadratic B-splines | |
CN105773620A (en) | Track planning and control method of free curve of industrial robot based on double quaternions | |
CN114952868B (en) | 7-degree-of-freedom SRS (sounding reference Signal) type mechanical arm control method and device and piano playing robot | |
CN112757306A (en) | Inverse solution multi-solution selection and time optimal trajectory planning algorithm for mechanical arm | |
CN113787525B (en) | Mechanical arm movement time optimization method based on joint performance limitation | |
CN107168309A (en) | A kind of underwater multi-robot paths planning method of Behavior-based control | |
CN109822550B (en) | High-efficiency high-precision teaching method for complex curved surface robot | |
CN111283683B (en) | Servo tracking accelerated convergence method for robot visual feature planning track | |
CN108427282A (en) | A kind of solution of Inverse Kinematics method based on learning from instruction | |
CN114779785A (en) | Mobile robot smooth track planning method based on PSO parameter setting | |
CN110948488A (en) | Robot self-adaptive trajectory planning algorithm based on time optimization | |
Toan et al. | Mapless navigation with deep reinforcement learning based on the convolutional proximal policy optimization network | |
CN115685988A (en) | Method and device for automatically generating free-form surface spraying path | |
CN111515954B (en) | Method for generating high-quality motion path of mechanical arm | |
CN113848725A (en) | Mechanical arm time optimal trajectory planning and optimizing method based on adaptive genetic algorithm | |
CN116117796B (en) | Industrial robot gesture track transition and speed planning method and system | |
Peng et al. | Deep reinforcement learning with a stage incentive mechanism of dense reward for robotic trajectory planning |
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 |