CN115922705B - Robot joint speed calculation method, system and computer equipment - Google Patents
Robot joint speed calculation method, system and computer equipment Download PDFInfo
- Publication number
- CN115922705B CN115922705B CN202211499431.8A CN202211499431A CN115922705B CN 115922705 B CN115922705 B CN 115922705B CN 202211499431 A CN202211499431 A CN 202211499431A CN 115922705 B CN115922705 B CN 115922705B
- Authority
- CN
- China
- Prior art keywords
- robot
- joint
- singular
- speed
- damping
- 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
- 238000004364 calculation method Methods 0.000 title claims abstract description 29
- 230000014509 gene expression Effects 0.000 claims abstract description 104
- 239000013598 vector Substances 0.000 claims abstract description 94
- 238000013016 damping Methods 0.000 claims abstract description 83
- 238000000034 method Methods 0.000 claims abstract description 50
- 230000006870 function Effects 0.000 claims description 69
- 239000011159 matrix material Substances 0.000 claims description 61
- 230000009466 transformation Effects 0.000 claims description 11
- 238000013461 design Methods 0.000 claims description 9
- 238000005457 optimization Methods 0.000 claims description 5
- 230000008569 process Effects 0.000 description 7
- 230000001133 acceleration Effects 0.000 description 3
- 230000011218 segmentation Effects 0.000 description 2
- 230000008859 change Effects 0.000 description 1
- 238000010586 diagram Methods 0.000 description 1
- 230000000694 effects Effects 0.000 description 1
- 238000009776 industrial production Methods 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 238000012545 processing Methods 0.000 description 1
- 238000011160 research Methods 0.000 description 1
- 238000006467 substitution reaction Methods 0.000 description 1
- 238000012795 verification Methods 0.000 description 1
Classifications
-
- Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
- Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
- Y02T—CLIMATE CHANGE MITIGATION TECHNOLOGIES RELATED TO TRANSPORTATION
- Y02T10/00—Road transport of goods or passengers
- Y02T10/10—Internal combustion engine [ICE] based vehicles
- Y02T10/40—Engine management systems
Abstract
The invention belongs to the field of robot motion trail control, and particularly relates to a robot joint speed calculation method, a robot joint speed calculation system and computer equipment. The method comprises the following steps: establishing a joint robot position inverse solution model, a speed forward solution model and a speed inverse solution model; obtaining a singular condition expression of robot motion, establishing a relation model of joint speed and the singular condition expression, and rewriting a speed inverse solution model; designing a damping reciprocal function for each joint; solving a joint angle vector solution through a singular condition expression, defining a singular region, and determining a singular boundary of the robot; and solving a joint angle vector by using the position inverse solution model, optimizing parameters of an iterative damping reciprocal function, constructing a new robot speed inverse solution model, and solving joint speeds of all track points. The method can ensure that the robot smoothly passes through the singular configuration region, improves the overall motion precision, and solves the problems of high randomness and lack of theoretical basis in the traditional method of parameter selection.
Description
Technical Field
The invention belongs to the field of robot motion trail control, and particularly relates to a robot joint speed calculation method, a system and computer equipment.
Background
Industrial robots (such as six-degree-of-freedom joint robots) have wide application in the industrial field, and greatly improve the industrial production efficiency; but industrial robots all have kinematic singular configurations. When the robot moves to the vicinity of the singular pose or the movement track needs to pass through the singular pose, the movement speed of the joints of the robot is suddenly changed, so that the track tracking accuracy of the robot is reduced, the system stability is poor, and the control system of the robot is invalid due to the overlarge joint angle, so that the normal work of the robot is influenced. Therefore, it is very important to design a singular avoidance algorithm of the robot and improve the track tracking precision and the system stability of the robot under singular configurations. At present, related scholars research a singular configuration avoidance algorithm of a robot.
In recent years, a method based on damping reciprocal is proposed to avoid the singular configuration of the robot, and track tracking precision higher than that of a conventional damping method is obtained; however, in the process of designing and introducing the damping factor, the damping factor is still discontinuous in sections, so that when the robot enters a singular area, the joint acceleration of the robot is discontinuous, and speed abrupt change can be generated.
In order to solve the problems of discontinuous damping factor and low precision of the conventional Gaussian damping factor introduced in the scheme, an improved Gaussian damping function is proposed in recent years; however, the track tracking precision of the non-singular region is reduced due to the fact that a global damping function is introduced in the joint velocity solving process.
In the prior art, when the joint speed is calculated to avoid singular configurations, the parameters of the damping function are generally directly given, the directly given parameters cannot be guaranteed to be optimal parameters, and the same parameters are given to a plurality of joints, so that the parameters of some joints are obviously unreasonable, and the track tracking error of the tail end is increased.
Disclosure of Invention
In order to further improve the global track tracking precision of the robot and simultaneously consider the constraint that the joint angular velocity has an upper limit, the invention provides a robot joint velocity calculation method, a system and computer equipment, designs a novel damping reciprocal function, can carry out parameter design aiming at each joint angular velocity upper limit, not only maintains the continuity of the joint angular velocity, ensures that the calculated joint angular velocity does not exceed the speed upper limit, improves the track tracking precision of the robot in a singular region, and solves the problems of low joint velocity solving precision caused by large randomness and lack of theoretical basis in parameter selection in the prior method.
The method is realized by the following technical scheme: a robot joint speed calculation method comprises the following steps:
according to the robot tail end pose vector and the robot joint angle vector, a joint robot position inverse solution model, a robot speed forward solution model and a robot speed inverse solution model are established; the robot speed forward solution model and the robot speed inverse solution model are expressed through a Jacobian matrix;
calculating a singular condition expression of robot motion according to the Jacobian matrix, and establishing a relation model between joint speed and the singular condition expression; rewriting a robot speed inverse solution model according to the established relation model;
designing a damping reciprocal function for each joint of the robot according to the singular condition expression;
solving a solution of the joint angle vector by using a singular condition expression, defining a singular region based on the solved solution, and determining a singular boundary of the robot according to the singular region;
setting an initial value of a damping maximum value and an initial value of a first joint bottom parameter, giving a starting point and an end point of a robot motion path, and solving a Cartesian track of the robot motion; solving the maximum combining speed and solving a joint angle vector corresponding to each Cartesian track point by using a robot position inverse solution model;
optimizing parameters of damping reciprocal functions of all joints of the iterative robot according to the calculated joint angle vector and the maximum joint speed;
and constructing a new robot speed inverse solution model according to the iterated inverse function parameters of each joint damping, and solving the joint speed of each track point.
Preferably, according to the robot end pose vector and the robot joint angle vector, an inverse joint robot position solution model, a robot speed forward solution model and a robot speed inverse solution model are established, comprising:
establishing a robot tail end pose vector P= [ x ] P y P z P ψ Px ψ Py ψ Pz ] T A robot position inverse solution model to the robot joint angle vector;
and solving the jacobian matrix according to the robot position inverse solution model, and establishing a robot speed forward solution model and a robot speed inverse solution model.
Preferably, the singular conditional expression of the robot motion is calculated according to the jacobian matrix, and a relation model between the joint velocity and the singular conditional expression is established, including:
dividing the Jacobian matrix to obtain a plurality of submatrices;
solving and simplifying determinant expressions of the submatrices according to the segmented submatrices, and defining singular condition expressions according to the determinant expressions of the simplified submatrices, wherein the singular condition expressions are nonlinear functions of the angle vector theta of the robot joint;
and respectively inverting the submatrices, substituting the inversion results of the submatrices into a robot speed inverse solution model, and simplifying the robot speed inverse solution model.
Preferably, according to the singular condition expression, a damping reciprocal function designed for each joint of the robot is a fractional expression, and the damping incremental function is set in a denominator expression of the damping reciprocal function; the damping reciprocal function and the damping incremental function are both functions of a singular conditional expression.
Preferably, the singular conditional expressions include a first singular conditional expression, a second singular conditional expression, and a third singular conditional expression;
solving a solution of the joint angle vector by a singular condition expression, defining a singular region based on the solved solution, and determining a singular boundary of the robot according to the singular region, comprising:
the first singular condition expression is equal to 0, a solution theta' of a joint angle vector meeting the first singular condition expression equal to 0 is obtained through the singular condition expression, a singular region is defined, and a first singular boundary epsilon is calculated 1 ；
Respectively making the second and third singular conditional expressions equal to 0, respectively solving the solutions of joint angle vectors meeting the second and third singular conditional expressions equal to 0 through the singular conditional expressions, defining singular regions, and calculating a second singular boundary epsilon 2 Third singular boundary ε 3 。
The system of the invention is realized by the following technical scheme: a robotic joint speed calculation system comprising the following modules:
the inverse solution and forward solution model building module is used for building a joint robot position inverse solution model, a robot speed forward solution model and a robot speed inverse solution model according to the robot tail end pose vector and the robot joint angle vector; the robot speed forward solution model and the robot speed inverse solution model are expressed through a Jacobian matrix;
the singular condition calculation module is used for calculating a singular condition expression of the robot motion according to the Jacobi matrix and establishing a relation model between the joint speed and the singular condition expression; rewriting a robot speed inverse solution model according to the established relation model;
the function design module is used for designing a damping reciprocal function for each joint of the robot according to the singular condition expression;
the singular boundary calculation module is used for calculating a solution of the joint angle vector through a singular condition expression, defining a singular region based on the calculated solution, and determining a singular boundary of the robot according to the singular region;
the joint angle vector solving module is used for giving a starting point and an ending point of a robot motion path and solving a Cartesian track of the robot motion; solving the maximum combining speed and solving a joint angle vector corresponding to each Cartesian track point by using a robot position inverse solution model;
the function parameter optimization module is used for setting an initial value of a damping maximum value and an initial value of a first joint bottom parameter, and optimizing parameters of damping reciprocal functions of all joints of the iterative robot according to the calculated joint angle vector and the maximum joint speed;
and the joint speed solving module is used for constructing a new robot speed inverse solution model according to the iterated joint damping reciprocal function parameters and solving the joint speed of each track point.
The computer device comprises a processor and a memory for storing a program executable by the processor, wherein the processor is used for executing the steps of the robot joint speed calculation method when the program stored in the memory is executed.
Compared with the prior art, the invention has the following advantages:
the invention designs a novel damping reciprocal function, provides a damping maximum value and a bottom parameter selecting and optimizing method based on joint speed upper limit and nonsingular region speed error constraint, provides a basis for parameter design, and can acquire better parameters; according to the joint speed and the acceleration calculated by the novel damping reciprocal function, the joint speed and the acceleration are continuous in the global range including the singular potential zone, so that the robot can be ensured to smoothly pass through the singular potential zone; on the premise of meeting the constraint of the upper limit of the joint angular velocity, the calculation precision of each joint angular velocity is improved.
According to the method, the angular velocity of the robot joint is calculated, the singular configuration region of the robot in the motion process can be avoided, and the method has important significance in improving the track tracking precision of the robot under the singular configuration. The method solves the problem of low joint speed solving precision caused by high randomness and lack of theoretical basis in the traditional method, improves the technical problem of low global motion precision of the terminal, and improves the global motion precision of the terminal.
Drawings
Fig. 1 is a schematic diagram of coordinates of a D-H link of a robot in an embodiment of the present invention.
Fig. 2 is a flowchart of a robot joint velocity calculation method according to an embodiment of the present invention.
FIG. 3 is a graph showing the comparison of the characteristics of the present invention and 4 methods of the prior art.
Fig. 4 is a partial enlarged view of the characteristic comparison chart of fig. 3.
FIG. 5 is a graph comparing the error between the original reciprocal 1/K (undamped) for the example of the present invention and the 4 methods of the prior art.
Fig. 6 is a graph of the error between the angular velocity of the joint 6 calculated using the example of the present invention and the 4 methods of the prior art, and the angular velocity of the joint 6 calculated using the original reciprocal 1/K (undamped).
Fig. 7 is a comparison and enlargement of the angular velocity error of the joint 6 of fig. 6.
Detailed Description
The present invention will be further described with reference to the drawings and examples, but the embodiments of the present invention are not limited thereto.
Example 1
The six degree of freedom articulated robot D-H linkage is shown in FIG. 1, wherein O 0 -X 0 Y 0 Z 0 Is a ground coordinate system; o (O) i For the origin of the joint coordinates of the ith joint, O i -X i Z i Joint coordinates for the ith joint; a, a i-1 Is Z i-1 Axis to Z i Along axis X i-1 Distance in the axial direction; alpha i-1 Is Z i-1 Axis to Z i Around axis X i-1 The rotation angle of the shaft; d, d i Is X i-1 Axis to X i Along axis Z i Distance in the axial direction; θ i Is X i-1 Axis to X i Around axis Z i Rotation angle of the shaft, where i=1, 2, …,6. Taking a certain six-degree-of-freedom joint robot as an example, the robot D-H parameters are shown as follows:
TABLE 1 six degrees of freedom robot D-H parameter table
| Connecting rod i | α i-1 (°) | a i-1 (mm) | d i (mm) | θ i (°) |
| 1 | 0 | 0 | 0 | θ 1 |
| 2 | -90 | 190 | 0 | θ 2 |
| 3 | 0 | 560 | 0 | θ 3 |
| 4 | -90 | 155 | 724 | θ 4 |
| 5 | 90 | 0 | 0 | θ 5 |
| 6 | -90 | 0 | 0 | θ 6 |
For convenience of subsequent expression, s1=sinθ is defined herein 1 ，c1＝cosθ 1 ，s2＝sinθ 2 ，c2＝cosθ 2 ，s3＝sinθ 3 ，c3＝cosθ 3 ，s23＝sin(θ 2 +θ 3 )，c23＝cos(θ 2 +θ 3 )，s4＝sinθ 4 ，c4＝cosθ 4 ，s5＝sinθ 5 ，c5＝cosθ 5 ，s6＝sinθ 6 ，c6＝cosθ 6 。
The embodiment provides that a section of the method satisfies K 3 Six-degree-of-freedom joint machine under (theta) =0 Cartesian straight line trackMethod for calculating joint velocity of robot, and joint vector θ of trajectory start point 0 And endpoint joint vector θ e The method comprises the following steps:
θ 0 ＝[0.5332 0.1527 -0.1192 -1.6256 0.5341 1.6345] T
θ e ＝[-0.6299 0.2777 -0.2658 1.5869 0.6301 -1.5907] T
corresponding terminal pose vector P 0 And an end-point pose vector P e The method comprises the following steps of:
P 0 ＝[0.864 0.5099 0.6850 -2.9107 -1.5707 -0.2309] T
P e ＝[0.8641 -0.6299 0.6849 -2.6486 -1.5706 -0.4929] T
and obtaining a planned track by an S track planning method. The trajectory comprises n cartesian trajectory points. The terminal pose vector of the nth track point is recorded as P n The corresponding joint vector is theta n . Referring to the flowchart shown in fig. 2, the specific implementation steps include:
s1, establishing a six-degree-of-freedom joint robot position inverse solution model, a robot speed forward solution model and a robot speed inverse solution model, wherein the robot speed forward solution model and the robot speed inverse solution model are expressed through a Jacobian matrix. The method comprises the following steps:
s11, establishing a robot tail end pose vector P= [ x ] P y P z P ψ Px ψ Py ψ Pz ] T To robot joint angle vector θ= [ θ ] 1 θ 2 θ 3 θ 4 θ 5 θ 6 ] T A robot position inverse solution model θ=Φ (P). Wherein θ is i (i=1, 2, …, 6) is the joint angle, x, of the i-th joint of the robot P 、y P 、z P Is the robot end position, ψ Px 、ψ Py 、ψ Pz For the robot end pose, Φ (P) is the transformation function of the articulated robot end pose vector to the robot joint angle.
The robot terminal pose vector P= [ x ] P y P z P ψ Px ψ Py ψ Pz ] T The pose matrix Θ of (2) is:
wherein [nx n y n z ] T Is a gesture coordinate vector [ phi ] Px ψ Py ψ Pz ] T A component projected in the X-axis direction; [ o ] x o y o z ] T Is a gesture coordinate vector [ phi ] Px ψ Py ψ Pz ] T A component projected in the Y-axis direction; [ a ] x a y a z ] T Is a gesture coordinate vector [ phi ] Px ψ Py ψ Pz ] T The component projected in the Z-axis direction.
The coordinate transformation matrix of two adjacent joints i and i-1 of the robot is set as i-1 T i (i=1, 2,3, …, 6), then a coordinate transformation matrix is determined from the robot D-H parameters i-1 T i The analytical expression for the joint angle vector θ is:
then according to the obtained coordinate transformation matrix i-1 T i Calculating the coordinate transformation matrix of the ith joint and the ground 0 T i Is a analytic expression of (2):
wherein [i q x i q y i q z ] T For the ith joint origin of coordinates O i In the coordinate system O 0 -X 0 Y 0 Z 0 Position coordinate vector of (a); [ i n x i n y i n z ] T For the ith joint origin of coordinates O i Is at X 0 A component of the axial projection; [ i o x i o y i o z ] T For the ith joint origin of coordinates O i Is at Y 0 A component of the axial projection; [ i a x i a y i a z ] T For the ith joint origin of coordinates O i Is at Z 0 The component of the axial projection.
Calculating to obtain an expression of the gesture matrix Θ under the ith joint coordinate 0 T i ) -1Θ, wherein (0 T i ) -1 Is a coordinate transformation matrix 0 T i Is a matrix of inverse of (a).
Based on% 0 T i ) -1Θ and 0 T i And obtaining a robot position inverse solution model theta=phi (P) by using an inversion method.
S12, establishing a robot speed forward solution model according to the robot position reverse solution modelSum-velocity inverse solution model wherein V＝[vx v y v z ω x ω y ω z ] T Is a robot tip speed vector; v x 、v y and vz The robot tip X, Y and the Z-axis directional velocity components, respectively; omega x 、ω y and ωz Angular velocity components of the robot tip rotating about X, Y and Z axes, respectively; />Is the angular velocity vector of the robot joint +.>Angular velocity of the i-th joint of the robot; j is a Jacobian matrix of the robot, J -1 Is a robot JackInverse of the ratio matrix.
In this step, the matrix may be transformed according to the coordinates in step S11 0 T i The analytical expressions of (2) are defined as follows:
z i ＝[ i a x i a y i a z ] T
0 q i ＝[ i q x i q y i q z ] T
the jacobian matrix J of the robot is solved by the following formula:
finally, a robot speed forward model can be obtainedIs described.
S2, calculating a singular condition expression of robot motion according to the Jacobian matrix, and establishing a relation model between joint speed and the singular condition expression; and modeling and simplifying a robot speed inverse solution model according to the established relation.
S21, segmentation Jacobian matrixObtaining 4 submatrices J 11 、J 12 、J 21 and J22 All are 3 x 3 matrices.
In this step, the jacobian matrix can be divided according to the following method to obtain 4 submatrices:
J 12 ＝0 3×3
wherein ,d4 Is X 3 Axis to X 4 Along axis Z 4 Distance in axial direction, a i-1 Is Z i-1 Axis to Z i Along axis X i-1 Distance in the axial direction.
S22, solving and simplifying the submatrix J according to the 4 submatrices after segmentation 11 and J22 Defining a singular conditional expression K based on the reduced determinant expression of the submatrix 1 (θ)、K 2(θ) and K3 (θ) wherein the singular conditional expression K 1 (θ)、K 2(θ) and K3 (θ) is a nonlinear function of the robot joint angle vector θ.
Neutron matrix J in this embodiment 11 and J22 The determinant expression of (2) is:
det(J 11 )＝[a 1 +a 2 cosθ 2 +a 3 cos(θ 2 +θ 3 )-d 4 sin(θ 2 +θ 3 )](a 3 sinθ 3 +d 4 cosθ 3 )
det(J 22 )＝sinθ 5
the present embodiment therefore uses the singular conditional expression K j (θ) (j=1, 2, 3) is defined as:
K 1 (θ)＝a 1 +a 2 cosθ 2 +a 3 cos(θ 2 +θ 3 )-d 4 sin(θ 2 +θ 3 )
K 2 (θ)＝a 3 sinθ 3 +d 4 cosθ 3
K 3 (θ)＝sinθ 5
the step is based on S21, in the submatrix J 11 、J 22 Separating out the corresponding singular expression K j (θ) (j=1, 2, 3), and sub-matrix J 11 、J 22 The rewriting is as follows:
wherein ,J′11 Is J 11 Is a temporary calculation matrix of J' 22 Is J 22 Is used to calculate the matrix temporarily.
wherein ,
then sub-matrix J 11 、J 22 Respectively inverting to obtain J 11 -1 ＝H 1 J′ 11 -1 and J22 -1 ＝H 2 J′ 22 -1, wherein ：
sub-matrix J 11 、J 22 Is substituted into a joint velocity solving formula and a robot velocity inverse solution modelThe following expression is obtained:
wherein ,J′11 -1 For temporarily calculating matrix J' 11 Inverse matrix of J' 22 -1 For temporarily calculating matrix J' 22 An inverse matrix of (a); definition:
the robot velocity inverse model can be further rewritten:
and S3, designing a novel damping reciprocal function for each joint according to the singular condition expression.
In this step, the following novel damping reciprocal functions g are respectively constructed for the ith joint of the robot i (i＝1,2,3,…,6)：
Wherein e is a natural constant; epsilon j (j=1, 2, 3) is a singular boundary; lambda (lambda) i (i=1, 2,3, …, 6) is a damping delta function, λ max Is the damping maximum value; sigma (sigma) i (i=1, 2,3, …, 6) is the bottom parameter. It can be seen that the damping reciprocal function is a fractional expression, and the damping increment function is arranged in a denominator expression of the damping reciprocal function; the damping reciprocal function and the damping incremental function are both functions of a singular conditional expression.
S4, solving a solution of the joint angle vector through a singular condition expression, defining a singular region based on the solved solution, and determining a singular boundary of the robot according to the singular region.
S41, first, let K 1 (θ) =0, and the satisfaction of K is found by the singular conditional expression 1 A set of solutions θ ' to the joint angle vector of (θ) =0, defining [ θ ' - Δθ, θ ' +Δθ]Is a singular region.
To preserve the calculation accuracy, the present embodiment expresses the above solution θ' in the form containing pi as:
taking K 1 (θ' - Δθ) and K 1 The maximum value of (θ' +Δθ) is taken as a first singular boundary ε 1 . Preferably, takeThrough epsilon 1 ＝max{K 1 (θ′-Δθ),K 1 (θ' +Δθ) } to obtain a first singular boundary ε 1 ＝0.174。
S42, then, respectively letting K 2 (θ)＝0、K 3 (θ) =0, and the singular boundary ε is obtained by the method in step S41 2 ＝0.065，
ε 3 ＝0.05。
S5, giving a starting point and an ending point of a robot motion path, and solving a Cartesian track of the robot motion; the maximum speed V is obtained max And solving the joint angle vector corresponding to each Cartesian track point by using a robot position inverse solution model.
S51, calculating each Cartesian track point P by utilizing a robot position inverse solution model theta=phi (P) n Corresponding joint angle vector θ n 。
S52, calculating the sum velocity V of all Cartesian track points 2 The robot tail end speed vector corresponding to the maximum value of the combined speed is V max 。
And S6, optimizing parameters of damping reciprocal functions of all joints of the iterative robot according to the obtained joint angle vector and the maximum joint speed.
S61, setting a damping maximum value lambda max Initial value lambda of 0 And a first joint bottom parameter sigma 1 Initial value sigma of 1,0 The method comprises the steps of carrying out a first treatment on the surface of the Sigma can be made for convenient calculation 1,0 ＝e，λ 0 ＝ε 1 Where e is a natural constant.
S62, according to a given lambda 0 、σ 1,0 When K is 1 (θ)∈[0,ε 1 ]Damping reciprocal function g 1 K corresponding to the maximum value of (2) 1 (θ) is denoted as K 1 ' (θ). Definition of all satisfaction of K 1 (θ)≥K 1 Singular conditional expression K of' (θ) 1 Minimum value of (θ) isAt this time, the joint angle vector is +.>Definition of all satisfaction of K 1 (θ)≥ε 1 Singular conditional expression K of (2) 1 The minimum value in (θ) is +.>At this time, switchThe pitch angle vector is +.>
S63, calculating the sum velocity V of all Cartesian track points 2 The robot tail end speed vector corresponding to the maximum value of the combined speed is V max The method comprises the steps of carrying out a first treatment on the surface of the Combining the above joint angle vectorsV in S52 max Obtaining F 1 (θ, V) global upper bound +.>
S64, ifOutput lambda 0 The method comprises the steps of carrying out a first treatment on the surface of the Otherwise lambda 0 Increase step h 1 Returning to step S62, re-calculating the joint angle vector +/corresponding to each Cartesian locus point> and /> wherein />Maximum angular velocity of the first joint of the robot set for the user.
S65, iterative optimization damping maximum lambda max And determining the bottom parameter sigma of all joints i (i＝1,2,3,…,6)。
S651, giving an upper limit E of the velocity error of the first joint in the nonsingular region 1 Combining the above joint angle vectorsObtaining F 1 (θ, V) lower bound on singular boundary values +.>Preliminary determination of damping maximum lambda max 。
In this step, if:
lambda then max ＝λ 0 ，σ 1 ＝σ 1,0 ；
Otherwise, the bottom parameter sigma 1 Initial value sigma of 1,0 Increase step h 2 The process returns to step S62 to re-calculate the joint angle vector.
S652, damping maximum value lambda obtained in step S651 max Determining lambda 2 Similarly let sigma 2,0 =e; damping maximum lambda for the second joint by the method of steps S62-S64 max And (5) performing calculation.
S653, giving an upper limit E of the velocity error in the nonsingular region of the second joint 2 Obtaining a bottom parameter sigma corresponding to the second joint by adopting the method of the step S651 2 。
S654, obtaining the final lambda according to the method of the steps S652-S653 max And the third to sixth joint bottom parameter sigma 3 、σ 4 、σ 5 Sigma (sigma) 6 。
And S7, constructing a new robot speed inverse solution model according to the iterated inverse function parameters of each joint damping, and solving the joint speed of each track point.
S71, inversely solving the joint velocity in the modelReplaced by corresponding inverse damping function g i (i=1, 2,3, …, 6) creating a new robot velocity inverse model +.> wherein />
S72, utilizing the velocity inverse solution model established in the step S71Calculating joint speeds of all track points in the Cartesian track>
After the processing according to the singular configuration avoidance method, the present embodiment selects 4 conventional damping functions for comparison in the effect verification process, and the comparison situation is shown in fig. 3-7.
Example 2
Based on the same inventive concept as embodiment 1, this embodiment provides a robot speed calculation system including the following modules:
the inverse solution and forward solution model building module is used for building a joint robot position inverse solution model, a robot speed forward solution model and a robot speed inverse solution model according to the robot tail end pose vector and the robot joint angle vector; the robot speed forward solution model and the robot speed inverse solution model are expressed through a Jacobian matrix;
the singular condition calculation module is used for calculating a singular condition expression of the robot motion according to the Jacobi matrix and establishing a relation model between the joint speed and the singular condition expression; rewriting a robot speed inverse solution model according to the established relation model;
the function design module is used for designing a damping reciprocal function for each joint of the robot according to the singular condition expression;
the singular boundary calculation module is used for calculating a solution of the joint angle vector through a singular condition expression, defining a singular region based on the calculated solution, and determining a singular boundary of the robot according to the singular region;
the joint angle vector solving module is used for giving a starting point and an ending point of a robot motion path and solving a Cartesian track of the robot motion; solving the maximum combining speed and solving a joint angle vector corresponding to each Cartesian track point by using a robot position inverse solution model;
the function parameter optimization module is used for optimizing parameters of damping reciprocal functions of all joints of the iterative robot according to the calculated joint angle vector and the maximum joint speed;
the joint speed solving module is used for setting an initial value of a damping maximum value and an initial value of a first joint bottom parameter, constructing a new robot speed inverse solution model according to each joint damping inverse function parameter after iteration, and solving the joint speed of each track point.
The above modules in this embodiment are used to execute steps S1 to S7 in embodiment 1, respectively, and the detailed execution process is referred to embodiment 1 and is not repeated here.
Example 3
The embodiment provides a computer device based on the same inventive concept as that of embodiment 1, including a processor and a memory for storing a program executable by the processor, where the processor is configured to execute steps S1-S7 of embodiment 1 when executing the program stored by the memory, and the detailed execution process is referred to embodiment 1 and is not repeated herein.
The foregoing is only illustrative of the preferred embodiments of the present invention, but the scope of the invention is not limited thereto, and any other changes, modifications, substitutions, combinations, and simplifications that do not depart from the spirit and principles of the present invention should be made therein and are intended to be equivalent substitutes.
Claims (10)
1. The robot joint speed calculation method is characterized by comprising the following steps of:
according to the robot tail end pose vector and the robot joint angle vector, a joint robot position inverse solution model, a robot speed forward solution model and a robot speed inverse solution model are established; the robot speed forward solution model and the robot speed inverse solution model are expressed through a Jacobian matrix;
calculating a singular condition expression of robot motion according to the Jacobian matrix, and establishing a relation model between joint speed and the singular condition expression; rewriting a robot speed inverse solution model according to the established relation model;
designing a damping reciprocal function for each joint of the robot according to the singular condition expression;
solving a solution of the joint angle vector by using a singular condition expression, defining a singular region based on the solved solution, and determining a singular boundary of the robot according to the singular region;
giving a starting point and an ending point of a robot motion path, and solving a Cartesian track of the robot motion; solving the maximum combining speed and solving a joint angle vector corresponding to each Cartesian track point by using a robot position inverse solution model;
setting an initial value of a damping maximum value and an initial value of a first joint bottom parameter according to the calculated joint angle vector and the maximum joint speed, and optimizing parameters of damping reciprocal functions of all joints of the iterative robot;
constructing a new robot speed inverse solution model according to the iterated inverse function parameters of each joint damping, and solving the joint speed of each track point;
the damping reciprocal function designed for each joint of the robot is a fractional expression according to the singular condition expression, and the damping incremental function is arranged in a denominator expression of the damping reciprocal function; the damping reciprocal function and the damping incremental function are both functions of a singular conditional expression.
2. The method according to claim 1, wherein the step of creating an inverse model of the joint robot position, an inverse model of the robot velocity, and an inverse model of the robot velocity based on the robot end pose vector and the robot joint angle vector, comprises:
establishing a robot tail end pose vector P= [ x ] P y P z P ψ Px ψ Py ψ Pz ] T A robot position inverse solution model to the robot joint angle vector;
and solving the jacobian matrix according to the robot position inverse solution model, and establishing a robot speed forward solution model and a robot speed inverse solution model.
3. The method for calculating the joint velocity of the robot according to claim 1, wherein the step of calculating the singular conditional expression of the robot motion from the jacobian matrix and establishing a relational model between the joint velocity and the singular conditional expression comprises:
dividing the Jacobian matrix to obtain a plurality of submatrices;
solving and simplifying determinant expressions of the submatrices according to the segmented submatrices, and defining singular condition expressions according to the determinant expressions of the simplified submatrices, wherein the singular condition expressions are nonlinear functions of the angle vector theta of the robot joint;
and respectively inverting the submatrices, substituting the inversion results of the submatrices into a robot speed inverse solution model, and simplifying the robot speed inverse solution model.
4. The method for calculating a robot joint velocity according to claim 1, wherein,
damping reciprocal function g constructed for the ith joint of a robot i The method comprises the following steps of:
wherein e is a natural constant; epsilon j Is a singular boundary; lambda (lambda) i Lambda is the damping delta function max Is the damping maximum value; sigma (sigma) i The bottom parameter; j=1, 2,3, i=1, 2,3, …,6; θ is the robot joint angle vector, K 1 (θ) is a first singular conditional expression, K 2 (θ) is a second singular conditional expression, K 3 And (θ) is a third singular conditional expression.
5. The robot joint velocity calculation method according to claim 1, wherein the singular condition expressions include a first singular condition expression, a second singular condition expression, and a third singular condition expression;
solving a solution of the joint angle vector by a singular condition expression, defining a singular region based on the solved solution, and determining a singular boundary of the robot according to the singular region, comprising:
the first singular condition expression is equal to 0, a solution theta' of a joint angle vector meeting the first singular condition expression equal to 0 is obtained through the singular condition expression, a singular region is defined, and a first singular boundary epsilon is calculated 1 ；
Respectively making the second and third singular conditional expressions equal to 0, respectively solving the solutions of joint angle vectors meeting the second and third singular conditional expressions equal to 0 through the singular conditional expressions, defining singular regions, and calculating a second singular boundary epsilon 2 Third singular boundary ε 3 。
6. The method according to claim 5, wherein the robot tip pose vector p= [ x P y P z P ψ Px ψ Py ψ Pz ] T The pose matrix Θ of (2) is:
wherein [nx n y n z ] T Is a gesture coordinate vector [ phi ] Px ψ Py ψ Pz ] T A component projected in the X-axis direction; [ o ] x o y o z ] T Is a gesture coordinate vector [ phi ] Px ψ Py ψ Pz ] T A component projected in the Y-axis direction; [ a ] x a y a z ] T Is a gesture coordinate vector [ phi ] Px ψ Py ψ Pz ] T A component projected in the Z-axis direction;
let O be i -X i Z i The joint coordinates of the ith joint are set as coordinate transformation matrixes of two adjacent joints i and i-1 of the robot i-1 T i Then the coordinate transformation matrix is obtained according to the D-H parameters of the robot i-1 T i The analytical expression for the joint angle vector θ is:
wherein ai-1 Is Z i-1 Axis to Z i Along axis X i-1 Distance in the axial direction; alpha i-1 Is Z i-1 Axis to Z i Around axis X i-1 The rotation angle of the shaft; d, d i Is X i-1 Axis to X i Along axis Z i Distance in the axial direction; θ i Is X i-1 Axis to X i Around axis Z i The rotation angle of the shaft; i=1, 2,3, …,6;
then according to the obtained coordinate transformation matrix i-1 T i Calculating the coordinate transformation matrix of the ith joint and the ground 0 T i Is a analytic expression of (2):
wherein [i q x i q y i q z ] T For the ith joint origin of coordinates O i In the coordinate system O 0 -X 0 Y 0 Z 0 Position coordinate vector of (a); [ i n x i n y i n z ] T For the ith joint origin of coordinates O i Is at X 0 A component of the axial projection; [ i o x i o y i o z ] T For the ith joint origin of coordinates O i Is at Y 0 A component of the axial projection; [ i a x i a y i a z ] T For the ith joint origin of coordinates O i Is at Z 0 A component of the axial projection;
calculating to obtain an expression of the gesture matrix Θ under the ith joint coordinate 0 T i ) -1Θ, wherein (0 T i ) -1 Is a coordinate transformation matrix 0 T i An inverse matrix of (a);
based on% 0 T i ) -1Θ and 0 T i Obtaining a robot position inverse solution model theta=phi (P) by using an inversion method;
the robot speed orthographic solution model isThe speed inverse solution model of the robot is +.> wherein />Is the angular velocity vector of the robot joint, V is the terminal velocity vector of the robot, J is the Jacobian matrix of the robot, J -1 Is the inverse of the robot jacobian matrix.
7. The robot joint speed calculation method according to claim 6, wherein a singular conditional expression is defined as:
first singular conditional expression K 1 (θ)＝a 1 +a 2 cosθ 2 +a 3 cos(θ 2 +θ 3 )-d 4 sin(θ 2 +θ 3 )；
Second singular conditional expression K 2 (θ)＝a 3 sinθ 3 +d 4 cosθ 3 ；
Third singular conditional expression K 3 (θ)＝sinθ 5 ；
Dividing the Jacobian matrix into 4 submatrices J 11 、J 12 、J 21 and J22 And sub-matrix J 11 、J 22 The rewriting is as follows:
wherein ,J′11 Is J 11 Is a temporary calculation matrix of J' 22 Is J 22 Is used for temporarily calculating a matrix;
sub-matrix J 11 、J 22 Respectively inverting to obtain and /> wherein ：
sub-matrix J 11 、J 22 Is substituted into the robot velocity inverse solution modelThe following expression is obtained:
wherein ,for temporarily calculating matrix J' 11 Inverse matrix of>For temporarily calculating matrix J' 22 An inverse matrix of (a); definition:
further simplifying to obtain a robot speed inverse solution model:
setting an initial value of a damping maximum value and an initial value of a first joint bottom parameter according to the calculated joint angle vector and the maximum joint speed, optimizing parameters of damping reciprocal functions of all joints of the iterative robot, and comprising the following steps:
setting a damping maximum lambda max Initial value lambda of 0 And a first joint bottom parameter sigma 1 Initial value sigma of 1,0 ；
According to a given lambda 0 、σ 1,0 When K is 1 (θ)∈[0,ε 1 ]K corresponding to the maximum value of the inverse damping function 1 (θ) is denoted as K 1 ' s (θ); definition of all satisfaction of K 1 (θ)≥K′ 1 Singular conditional expression of (θ) K 1 Minimum value of (θ) isAt this time, the joint angle vector is +.>Definition of all satisfaction of K 1 (θ)≥ε 1 Singular conditional expression K of (2) 1 The minimum value in (θ) is +.>At this time, the joint angle vector is +.>
Calculating the sum speed V of all the Cartesian track points 2 The robot tail end speed vector corresponding to the maximum value of the combined speed is V max The method comprises the steps of carrying out a first treatment on the surface of the Combined joint angle vectorObtaining F 1 Upper limit of (θ, V)>
If it isOutput lambda 0 The method comprises the steps of carrying out a first treatment on the surface of the Otherwise lambda 0 Increase step h 1 Re-solving the joint angle vector corresponding to each Cartesian track point; wherein->The maximum angular velocity of the first joint of the robot is set;
iterative optimization of damping maximum lambda max And determining the bottom parameter sigma of all joints i 。
8. The method of claim 7, wherein the iteratively optimizing damping maximum λ max And determining the bottom parameter sigma of all joints i Comprising:
given an upper limit E of velocity error of the first joint in the nonsingular region 1 In combination with the joint angle vectorObtaining F 1 (θ, V) lower bound on singular boundary +.>Preliminary determination of damping maximum lambda max The method comprises the steps of carrying out a first treatment on the surface of the If:
lambda then max ＝λ 0 ，σ 1 ＝σ 1,0 The method comprises the steps of carrying out a first treatment on the surface of the Otherwise, the bottom parameter sigma 1 Initial value sigma of 1,0 Increase step h 2 Re-solving the joint angle vector;
based on the determined damping maximum lambda max Determining lambda 2 Similarly let sigma 2,0 =e; damping maximum lambda for the second joint by adopting a method for calculating parameters corresponding to the first joint max Calculating;
giving an upper limit E of velocity error in the non-singular region of the second joint 2 Obtaining a bottom parameter sigma corresponding to the second joint by adopting a method for obtaining the bottom parameter corresponding to the first joint 2 ；
Calculating the final damping maximum lambda by adopting a calculation method of parameters corresponding to the second joint max And the bottom parameters of the remaining joints.
9. A robotic joint speed calculation system, comprising the following modules:
the inverse solution and forward solution model building module is used for building a joint robot position inverse solution model, a robot speed forward solution model and a robot speed inverse solution model according to the robot tail end pose vector and the robot joint angle vector; the robot speed forward solution model and the robot speed inverse solution model are expressed through a Jacobian matrix;
the singular condition calculation module is used for calculating a singular condition expression of the robot motion according to the Jacobi matrix and establishing a relation model between the joint speed and the singular condition expression; rewriting a robot speed inverse solution model according to the established relation model;
the function design module is used for designing a damping reciprocal function for each joint of the robot according to the singular condition expression;
the singular boundary calculation module is used for calculating a solution of the joint angle vector through a singular condition expression, defining a singular region based on the calculated solution, and determining a singular boundary of the robot according to the singular region;
the joint angle vector solving module is used for giving a starting point and an ending point of a robot motion path and solving a Cartesian track of the robot motion; solving the maximum combining speed and solving a joint angle vector corresponding to each Cartesian track point by using a robot position inverse solution model;
the function parameter optimization module is used for setting an initial value of a damping maximum value and an initial value of a first joint bottom parameter, and optimizing parameters of damping reciprocal functions of all joints of the iterative robot according to the calculated joint angle vector and the maximum joint speed;
the joint speed solving module is used for constructing a new robot speed inverse solution model according to the iterated joint damping reciprocal function parameters and solving the joint speed of each track point;
the function design module is used for designing a damping reciprocal function for each joint of the robot according to the singular condition expression, wherein the damping reciprocal function is a fractional expression, and the damping incremental function is arranged in a denominator expression of the damping reciprocal function; the damping reciprocal function and the damping incremental function are both functions of a singular conditional expression.
10. A computer device comprising a processor and a memory for storing a program executable by the processor, wherein the processor, when executing the program stored in the memory, is adapted to perform the robot joint velocity calculation method of any of claims 1-8.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202211499431.8A CN115922705B (en) | 2022-11-28 | 2022-11-28 | Robot joint speed calculation method, system and computer equipment |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202211499431.8A CN115922705B (en) | 2022-11-28 | 2022-11-28 | Robot joint speed calculation method, system and computer equipment |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN115922705A CN115922705A (en) | 2023-04-07 |
| CN115922705B true CN115922705B (en) | 2023-09-15 |
Family
ID=86556828
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202211499431.8A Active CN115922705B (en) | 2022-11-28 | 2022-11-28 | Robot joint speed calculation method, system and computer equipment |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN115922705B (en) |
Citations (8)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US8532825B1 (en) * | 2010-08-25 | 2013-09-10 | The Boeing Company | Software compensation for kinematically singular machines |
| CN104070525A (en) * | 2014-06-18 | 2014-10-01 | 大连大学 | Space manipulator continuous trajectory tracking method |
| CN105437234A (en) * | 2016-01-25 | 2016-03-30 | 珠海格力电器股份有限公司 | Multi-singular-point processing method and system and industrial robot |
| JP2018015896A (en) * | 2017-10-31 | 2018-02-01 | キヤノン株式会社 | Robot device and robot control method |
| CN112589797A (en) * | 2020-12-11 | 2021-04-02 | 中国科学院合肥物质科学研究院 | Method and system for avoiding singular points of non-spherical wrist mechanical arm |
| CN113211442A (en) * | 2021-05-14 | 2021-08-06 | 山东科技大学 | Singularity processing method of 6-degree-of-freedom robot or 7-degree-of-freedom robot |
| CN114714335A (en) * | 2022-03-09 | 2022-07-08 | 泉州华中科技大学智能制造研究院 | Inverse solution method and device for seven-joint redundant degree of freedom robot |
| CN115179288A (en) * | 2022-07-13 | 2022-10-14 | 安徽省配天机器人集团有限公司 | Inverse kinematics solution method for robot, and computer-readable storage medium |
-
2022
- 2022-11-28 CN CN202211499431.8A patent/CN115922705B/en active Active
Patent Citations (8)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US8532825B1 (en) * | 2010-08-25 | 2013-09-10 | The Boeing Company | Software compensation for kinematically singular machines |
| CN104070525A (en) * | 2014-06-18 | 2014-10-01 | 大连大学 | Space manipulator continuous trajectory tracking method |
| CN105437234A (en) * | 2016-01-25 | 2016-03-30 | 珠海格力电器股份有限公司 | Multi-singular-point processing method and system and industrial robot |
| JP2018015896A (en) * | 2017-10-31 | 2018-02-01 | キヤノン株式会社 | Robot device and robot control method |
| CN112589797A (en) * | 2020-12-11 | 2021-04-02 | 中国科学院合肥物质科学研究院 | Method and system for avoiding singular points of non-spherical wrist mechanical arm |
| CN113211442A (en) * | 2021-05-14 | 2021-08-06 | 山东科技大学 | Singularity processing method of 6-degree-of-freedom robot or 7-degree-of-freedom robot |
| CN114714335A (en) * | 2022-03-09 | 2022-07-08 | 泉州华中科技大学智能制造研究院 | Inverse solution method and device for seven-joint redundant degree of freedom robot |
| CN115179288A (en) * | 2022-07-13 | 2022-10-14 | 安徽省配天机器人集团有限公司 | Inverse kinematics solution method for robot, and computer-readable storage medium |
Non-Patent Citations (2)
| Title |
|---|
| 双臂机器人避关节极限与避奇异位形优化研究;李贺立;杨冬;杨德志;李铁军;马宏;;科学技术与工程(第03期);全文 * |
| 空间机器人动力学奇异回避的笛卡尔轨迹规划;金荣玉;耿云海;;宇航学报(第08期);全文 * |
Also Published As
| Publication number | Publication date |
|---|---|
| CN115922705A (en) | 2023-04-07 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN105773620B (en) | The trajectory planning control method of industrial robot free curve based on Double quaternions | |
| CN111702762B (en) | Industrial robot operation attitude optimization method | |
| JP5011525B2 (en) | Inverse kinematics of 6-degree-of-freedom robot arm by sequential search method, and robot system, control method, and program using the same | |
| CN111761582B (en) | Mobile mechanical arm obstacle avoidance planning method based on random sampling | |
| CN111230880A (en) | Complex curved surface processing track generation method in offline programming | |
| CN107160401B (en) | Method for solving problem of joint angle deviation of redundant manipulator | |
| CN110000773B (en) | Robot control device and inverse transformation processing method for robot | |
| CN114800534B (en) | Mechanical arm control method and device | |
| WO2023024317A1 (en) | Robot obstacle avoidance method and apparatus, and robot | |
| CN111914980A (en) | Redundant mechanical arm kinematics resolving method based on analytical solution and improved genetic algorithm | |
| CN112947298A (en) | Method, system and terminal for optimally generating curved surface machining track of robot | |
| Xu et al. | Models for three new screw-based IK sub-problems using geometric descriptions and their applications | |
| CN116038702A (en) | Seven-axis robot inverse solution method and seven-axis robot | |
| CN115922705B (en) | Robot joint speed calculation method, system and computer equipment | |
| Xu et al. | Inverse kinematics for 6-DOF serial manipulators with offset or reduced wrists via a hierarchical iterative algorithm | |
| CN112975954B (en) | Control method of robot arm, computer device, and storage medium | |
| CN111958602B (en) | Real-time inverse solution method for wrist offset type 6-axis robot | |
| CN112356032B (en) | Posture smooth transition method and system | |
| Ji et al. | E-RRT*: Path Planning for Hyper-Redundant Manipulators | |
| CN114063621B (en) | Wheel type robot formation tracking and obstacle avoidance control method | |
| CN112684793B (en) | Trajectory tracking control method for zero-radius over-bending of robot in discrete system | |
| CN115056213A (en) | Robot track self-adaptive correction method for large complex component | |
| CN117733872B (en) | Series robot inverse kinematics control method based on directional performance | |
| CN113867260B (en) | Robot curved surface machining joint track generation method adopting numerical integration | |
| CN114536337B (en) | Mechanical arm linear motion track smoothing method and system and storage medium |
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 |