CN115922705A - Robot joint speed calculation method and system and computer equipment - Google Patents
Robot joint speed calculation method and system and computer equipment Download PDFInfo
- Publication number
- CN115922705A CN115922705A CN202211499431.8A CN202211499431A CN115922705A CN 115922705 A CN115922705 A CN 115922705A CN 202211499431 A CN202211499431 A CN 202211499431A CN 115922705 A CN115922705 A CN 115922705A
- Authority
- CN
- China
- Prior art keywords
- robot
- joint
- singular
- speed
- solution model
- 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.)
- Granted
Links
- 238000004364 calculation method Methods 0.000 title claims abstract description 34
- 230000014509 gene expression Effects 0.000 claims abstract description 98
- 239000013598 vector Substances 0.000 claims abstract description 94
- 238000013016 damping Methods 0.000 claims abstract description 76
- 238000000034 method Methods 0.000 claims abstract description 40
- 239000011159 matrix material Substances 0.000 claims description 60
- 230000006870 function Effects 0.000 claims description 58
- 230000009466 transformation Effects 0.000 claims description 10
- 238000005457 optimization Methods 0.000 claims description 5
- 238000004804 winding Methods 0.000 claims description 2
- 238000005192 partition Methods 0.000 claims 1
- 230000008569 process Effects 0.000 description 7
- 230000001133 acceleration Effects 0.000 description 3
- 238000010586 diagram Methods 0.000 description 2
- 230000008859 change Effects 0.000 description 1
- 230000000694 effects Effects 0.000 description 1
- 238000009776 industrial production Methods 0.000 description 1
- 238000012886 linear function Methods 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 238000006467 substitution reaction Methods 0.000 description 1
- 230000001131 transforming effect Effects 0.000 description 1
- 238000012795 verification Methods 0.000 description 1
Images
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
Landscapes
- Numerical Control (AREA)
Abstract
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. The method comprises the following steps: establishing a position inverse solution model, a speed forward solution model and a speed inverse solution model of the joint robot; obtaining a singular conditional expression of robot motion, establishing a relational model of joint speed and the singular conditional expression, and rewriting a speed inverse solution model; designing a damping reciprocal function for each joint; solving the solution of the joint angle vector through a singular conditional expression, defining a singular area and determining a singular boundary of the robot; and (3) solving joint angle vectors 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 invention can ensure that the robot smoothly passes through a singular configuration area, improves the overall motion precision and solves the problems of high randomness and lack of theoretical basis in parameter selection of the existing method.
Description
Technical Field
The invention belongs to the field of robot motion trajectory 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 the industrial production efficiency is greatly improved; but the industrial robots all have kinematic singular configurations. When the robot moves to the position near the singular pose or the motion track needs to pass through the singular pose, the motion speed of the joint of the robot changes suddenly, 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 singularity avoidance algorithm of the robot and improve the trajectory tracking precision of the robot under the singularity configuration and the stability of the system. At present, relevant scholars study the singular configuration evasion algorithm of the robot.
In recent years, a damping reciprocal-based method is proposed to avoid the singular configuration of the robot, and the track tracking precision is higher than that of the conventional damping method; however, in the design and introduction processes of 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 a sudden speed change is 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, due to the fact that a global damping function is introduced in the joint velocity solving process, the track tracking accuracy of the non-singular area is reduced.
In the prior art, when joint velocity is calculated to avoid singular configurations, parameters of a damping function are generally directly given, the directly given parameters cannot be guaranteed to be optimal parameters, the same parameters are given to a plurality of joints, parameters of some joints are obviously unreasonable, and track tracking errors of tail ends are increased.
Disclosure of Invention
In order to further improve the global trajectory tracking precision of the robot and simultaneously consider the constraint of upper limits of joint angular velocities, 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 keeps the continuity of the joint angular velocities and ensures that the solved joint angular velocities do not exceed the velocity upper limit, improves the trajectory tracking precision of the robot in a singular area, and solves the problems of low joint velocity solving precision caused by large randomness of parameter selection and lack of theoretical basis in the existing method.
The method is realized by the following technical scheme: a robot joint velocity calculation method includes the following steps:
according to the robot terminal pose vector and the robot joint angle vector, establishing a joint robot position inverse solution model, a robot speed forward solution model and a robot speed inverse solution model; wherein the robot velocity forward solution model and the robot velocity inverse solution model are expressed by a Jacobian matrix;
calculating to obtain a singular condition expression of robot motion according to a Jacobian matrix, and establishing a relational 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 conditional expression;
solving the solution of the joint angle vector through a singular conditional expression, defining a singular area based on the solved solution, and determining a singular boundary of the robot according to the singular area;
setting an initial value of a maximum damping value and an initial value of a first joint bottom parameter, setting a starting point and an end point of a motion path of the robot, and solving a Cartesian track of the motion of the robot; solving the maximum resultant 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 resultant speed;
and constructing a new robot speed inverse solution model according to the iterated damping reciprocal function parameters of each joint, and solving the joint speed of each track point.
Preferably, the establishing of the joint robot position inverse solution model, the robot speed forward solution model and the robot speed inverse solution model according to the robot terminal pose vector and the robot joint angle vector comprises:
establishing a robot end pose vector P = [ x ] P y P z P ψ Px ψ Py ψ Pz ] T Machine for obtaining angle vector of robot jointA robot position inverse solution model;
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 method includes the steps of calculating to obtain a singular conditional expression of robot motion according to a Jacobian matrix, and establishing a relational model between joint velocity and the singular conditional expression, wherein the relational model includes:
dividing the Jacobian matrix to obtain a plurality of sub-matrices;
solving and simplifying a determinant expression of the submatrix according to the plurality of partitioned submatrices, and defining a singular condition expression according to the determinant expression of the simplified submatrix, wherein the singular condition expression is a nonlinear function of a robot joint angle vector theta;
and respectively inverting the sub-matrixes, and substituting the inversion results of the sub-matrixes into the robot speed inverse solution model to simplify the robot speed inverse solution model.
Preferably, according to the singular conditional expression, a damping reciprocal function designed for each joint of the robot is a fractional expression, and a damping increment 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 the solution of the joint angle vector through a singular conditional expression, defining a singular area based on the solved solution, and determining the singular boundary of the robot according to the singular area, wherein the singular boundary comprises the following steps:
setting the first singular conditional expression equal to 0, solving the solution theta' of the joint angle vector which satisfies the first singular conditional expression equal to 0 through the singular conditional expression, then defining a singular area, and calculating a first singular boundary epsilon 1 ；
Respectively making the second and third singular conditional expressions equal to 0, respectively obtaining the solutions of the joint angle vectors meeting the second and third singular conditional expressions equal to 0 through the singular conditional expressions, and then defining singularRegion, calculating the second singular boundary epsilon 2 Third singular boundary ε 3 。
The system of the invention is realized by the following technical scheme: a robot joint velocity calculation system comprising the following modules:
the inverse solution and forward solution model building module is used for building an inverse solution model of the position of the joint robot, a forward solution model of the speed of the robot and an inverse solution model of the speed of the robot according to the terminal pose vector of the robot and the angle vector of the joint of the robot; wherein the robot velocity forward solution model and the robot velocity inverse solution model are expressed by a Jacobian matrix;
the singular condition calculation module is used for calculating a singular condition expression of the robot motion according to the Jacobian matrix and establishing a relational 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 conditional expression;
the singular boundary calculation module is used for solving the solution of the joint angle vector through a singular conditional expression, defining a singular region based on the solved solution and determining the 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 end point of a motion path of the robot and solving a Cartesian track of the motion of the robot; solving the maximum resultant 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 maximum damping value and an initial value of a first joint bottom parameter, and optimizing parameters of a damping reciprocal function of each joint of the iterative robot according to the calculated joint angle vector and the maximum resultant speed;
and the joint velocity solving module is used for constructing a new robot velocity inverse solution model according to the iterated damping reciprocal function parameters of each joint and solving the joint velocity of each track point.
The computer equipment comprises a processor and a memory for storing a program executable by the processor, wherein the processor is used for executing each step of the robot joint velocity calculating method when the processor executes the program stored in the memory.
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 bottom parameter selection and optimization method according to the upper limit of the joint speed and the speed error constraint of a nonsingular region, provides a basis for parameter design, and can obtain more optimal 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 configuration area, so that the robot can be ensured to smoothly pass through the singular configuration area; on the premise of meeting the upper limit constraint of the angular velocity of each joint, the calculation precision of the angular velocity of each joint is improved.
According to the method, the robot joint angular velocity is calculated, the singular configuration area of the robot in the motion process can be avoided, and the method has important significance for improving the track tracking precision of the robot under the singular configuration. The problem that the existing method is high in parameter selection randomness and low in joint speed solving precision due to the lack of theoretical basis is solved on the whole, the technical problem that the global motion precision of the terminal is low is solved, and the global motion precision of the terminal is improved.
Drawings
FIG. 1 is a schematic diagram of D-H link coordinates of a robot according to 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 comparing the characteristics of 4 methods of the prior art and the example of the present invention.
Fig. 4 is a partially enlarged view of the characteristic comparison diagram of fig. 3.
FIG. 5 is a graph comparing the error between 4 methods of the prior art and the original reciprocal 1/K (no damping) in the example of the present invention.
Fig. 6 is a graph showing the comparison of the angular velocity of the joint 6 calculated by the 4 methods according to the embodiment of the present invention and the prior art with the error of the angular velocity of the joint 6 calculated by the original reciprocal 1/K (no damping).
Fig. 7 is an enlarged view of the angular velocity error of the joint 6 in 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 D-H connecting rod of the six-degree-of-freedom joint robot is shown in figure 1, wherein O 0 -X 0 Y 0 Z 0 Is a ground coordinate system; o is i Is the joint coordinate origin, O, of the ith joint i -X i Z i Joint coordinates for the ith joint; a is i-1 Is Z i-1 Axis to Z i Axial edge X i-1 Distance in the axial direction; alpha (alpha) ("alpha") i-1 Is Z i-1 Axis to Z i Axial X i-1 The angle of rotation of the shaft; d i Is X i-1 Axis to X i Axial edge Z i Distance in the axial direction; theta.theta. i Is X i-1 Axis to X i Axial winding Z i The rotational angle of the shaft, where i =1,2, …,6. Taking a six-degree-of-freedom articulated robot as an example, the D-H parameter table of the robot is as follows:
TABLE 1D-H PARAMETER TABLE FOR SIX DOF ROBOT
| Connecting rod i | α i-1 (°) | a i-1 (mm) | d i (mm) | θ i (°) |
| 1 | 0 | 0 | 0 | θ 1 |
| 2 | -90 | 190 | 0 | |
| 3 | 0 | 560 | 0 | θ 3 |
| 4 | -90 | 155 | 724 | θ 4 |
| 5 | 90 | 0 | 0 | θ 5 |
| 6 | -90 | 0 | 0 | θ 6 |
For convenienceThe expression of the following, 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 。
This example provides that a segment satisfies K 3 Method for calculating joint velocity of six-degree-of-freedom articulated robot on (theta) =0 Cartesian straight-line trajectory having joint vector theta at starting point of trajectory 0 And endpoint joint vector θ e 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 end pose vector P 0 And end point pose vector P e Respectively as follows:
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 planning track by an S track planning method. The trajectory contains n cartesian trajectory points. Recording the terminal pose vector of the nth track point 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 by a Jacobian matrix. The method comprises the following specific steps:
s11, establishing a robot 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 The inverse solution model of robot position of (θ = Φ (P)). Wherein theta is i (i =1,2, …, 6) is the joint angle of the i-th joint of the robot, x P 、y P 、z P For the end position of the robot, psi Px 、ψ Py 、ψ Pz And phi (P) is a transformation function from the terminal pose vector of the joint robot to the joint angle of the robot.
The robot end pose vector P = [ x ] P y P z P ψ Px ψ Py ψ Pz ] T The attitude matrix Θ of (a) is:
wherein [nx n y n z ] T As attitude coordinate vector [ psi Px ψ Py ψ Pz ] T A component projected in the X-axis direction; [ o ] x o y o z ] T As attitude coordinate vector [ psi Px ψ Py ψ Pz ] T A component projected in the Y-axis direction; [ a ] A x a y a z ] T As attitude coordinate vector [ psi Px ψ Py ψ Pz ] T Component projected in the Z-axis direction.
Let the coordinate transformation matrix of two adjacent joints i and joint i-1 of the robot be i-1 T i (i =1,2,3, …, 6), a coordinate transformation matrix is obtained 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 The analytical expression of (1):
wherein [i q x i q y i q z ] T As the ith joint coordinate origin O i In a coordinate system O 0 -X 0 Y 0 Z 0 A position coordinate vector of (1); [ i n x i n y i n z ] T As the ith joint coordinate origin O i In X 0 Component of axial direction projection; [ i o x i o y i o z ] T Is the ith joint coordinate origin O i In the attitude coordinate of 0 Component of axial direction projection; [ i a x i a y i a z ] T As the ith joint coordinate origin O i In Z 0 Component of the axial projection.
Calculating to obtain an expression of the attitude matrix theta under the ith joint coordinate ( 0 T i ) -1Θ, wherein (0 T i ) -1 For transforming matrices of coordinates 0 T i The inverse matrix of (c).
Based on (A) 0 T i ) -1Θ and 0 T i and obtaining the 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 inverse solution model
Inverse solution model of sum velocity
wherein V＝[vx v y v z ω x ω y ω z ] T Is the velocity vector of the tail end of the robot; v. of x 、v y and vz Respectively is the tail end X of the robot,Y and Z axis direction velocity components; omega x 、ω y and ωz The angular velocity components of the robot tip rotating around the X, Y and the Z axis, respectively; />
Is the angular velocity vector of the robot joint, is->
The angular velocity of the ith joint of the robot; j is robot Jacobian matrix, J -1 Is the inverse of the Jacobian matrix of the robot.
In this step, the matrix may be first transformed according to the coordinates in step S11 0 T i The analytical expression (c) is 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 obtained by the following formula:
finally, a robot speed positive solution model can be obtained
The specific expression of (c).
S2, calculating to obtain a singular conditional expression of robot motion according to the Jacobian matrix, and establishing a relational model between joint speed and the singular conditional expression; and simplifying the robot speed inverse solution model according to the established relation model.
S21, divideSecant Jacobi matrix
Obtain 4 sub-matrices J 11 、J 12 、J 21 and J22 All are 3 × 3 matrices.
In this step, the jacobian matrix may be segmented according to the following method to obtain 4 sub-matrices:
J 12 ＝0 3×3
wherein ,d4 Is X 3 Axis to X 4 Axial edge Z 4 Axial distance, a i-1 Is Z i-1 Axis to Z i Axial edge X i-1 Distance in the axial direction.
S22, solving and simplifying the submatrix J according to the 4 partitioned submatrixes 11 and J22 Defining a singular conditional expression K according to 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 non-linear function of the robot joint angle vector θ.
Neutron matrix J of the present embodiment 11 and J22 The determinant expression of (A) is as follows:
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
therefore, the embodiment 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, and the sub-matrix J 11 、J 22 Separates out a corresponding singular expression K j (θ) (J =1,2,3), and sub-matrix J 11 、J 22 The rewrite is:
wherein ,J′11 Is J 11 Temporary calculation matrix of J' 22 Is J 22 The provisional calculation matrix of (2).
wherein ,
and then pair the sub-matrix J 11 、J 22 Respectively inverse 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 Substituting the inversion result into a joint speed solving formula, namely a robot speed inverse solution model
The following expression is obtained:
wherein ,J′11 -1 Is a temporary calculation matrix J' 11 Inverse matrix of, J' 22 -1 Is a temporary calculation matrix J' 22 The inverse matrix of (d); defining:
then the inverse solution model of robot velocity can be further rewritten:
and S3, designing a novel damping reciprocal function for each joint according to the singular conditional expression.
In the step, a novel damping reciprocal function g is constructed for the ith joint of the robot respectively i (i＝1,2,3,…,6)：
Wherein e is a natural constant; epsilon j (j =1,2,3) is a singular boundary; lambda [ alpha ] i (i =1,2,3, …, 6) is a damping increment function, λ max Is the maximum value of damping; sigma i (i =1,2,3, …, 6) is the base parameter. As can be seen, the damping reciprocal function is a fractional expression, and the damping increment 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.
And S4, solving the solution of the joint angle vector through a singular conditional expression, defining a singular area based on the solved solution, and determining the singular boundary of the robot according to the singular area.
S41, firstly, let K 1 (theta) =0, and the satisfaction of K is found by a singular conditional expression 1 (θ) = 0's set of solutions θ' for joint angle vectors defining [ θ '- Δ θ, θ' + Δ θ]Is a singular region.
To preserve computational accuracy, the present embodiment expresses the above solution θ' in a form containing π as:
get K 1 (theta' -delta theta) and K 1 (theta' + delta theta) as the first singular boundary epsilon 1 . Preferably, take
By epsilon 1 ＝max{K 1 (θ′-Δθ),K 1 (theta' + delta theta) } to obtain the first singular boundary epsilon 1 ＝0.174。
S42, then respectively enabling 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, setting a starting point and an end point of a motion path of the robot, and calculating a Cartesian track of the motion of the robot; determining the maximum resultant velocity V max And calculating the joint angle vector corresponding to each Cartesian track point by using the robot position inverse solution model.
S51, solving each Cartesian track point P by using the robot position inverse solution model theta = phi (P) n Corresponding joint angle vector theta n 。
S52, calculating the resultant velocity V of all the Cartesian track points 2 The velocity vector of the tail end of the robot corresponding to the maximum resultant velocity is V max 。
And S6, optimizing parameters of damping reciprocal functions of all joints of the iterative robot according to the solved joint angle vector and the maximum resultant velocity.
S61, setting a maximum damping value lambda max Initial value λ of 0 And a first articular base parameter σ 1 Initial value σ of 1,0 (ii) a Can make sigma convenient to calculate 1,0 ＝e，λ 0 ＝ε 1 Wherein e is a natural constant.
S62, according to given lambda 0 、σ 1,0 When K is 1 (θ)∈[0,ε 1 ]Damping reciprocal function g 1 K corresponding to the maximum value of 1 (theta) is denoted by K 1 ' (theta). Define all satisfy K 1 (θ)≥K 1 ' (theta) singular conditional expression K 1 Minimum of (theta)Has a value of
When the joint angle vector is->
Define all satisfy K 1 (θ)≥ε 1 Singular conditional expression of 1 (θ) has a minimum value of { (θ) }>
The angle vector of the joint is->
S63, calculating the resultant velocity V of all Cartesian track points 2 The velocity vector of the tail end of the robot corresponding to the maximum resultant velocity is V max (ii) a In combination with the above-mentioned joint angle vector
And V in S52 max To obtain F 1 (θ, V) Global Upper Limit @>
S64, if
Output lambda 0 (ii) a Otherwise λ 0 Step length h is increased 1 Returning to step S62, the joint angle vector corresponding to each Cartesian locus point is newly solved>
and />
wherein />
Robot first joint set for userThe maximum angular velocity of.
S65, iteratively optimizing the maximum damping value lambda max And determining the base parameters σ of all joints i (i＝1,2,3,…,6)。
S651, setting the upper limit E of the speed error of the first joint in the nonsingular region 1 In combination with the above joint angle vector
To obtain F 1 Lower bound on (theta, V) singular boundary value [ ] [ (- ])>
Preliminary determination of maximum value of damping lambda max 。
In this step, if:
Or else the bottom parameter sigma 1 Initial value σ of 1,0 Step length h is increased 2 The process returns to step S62 to obtain the joint angle vector again.
S652, maximum damping value λ obtained in step S651 max Determining lambda 2 Likewise let σ 2,0 = e; damping maximum lambda of the second joint by the method of steps S62-S64 max And (6) performing calculation.
S653, an upper limit E of the speed error in the nonsingular region of the second joint is given 2 Obtaining a bottom parameter sigma corresponding to the second joint by the method of step S651 2 。
S654, calculating the final lambda according to the method of the steps S652 to S653 max And base parameter σ of the third to sixth joints 3 、σ 4 、σ 5 And sigma 6 。
And S7, constructing a new robot speed inverse solution model according to the iterated damping reciprocal function parameters of each joint, and solving the joint speed of each track point.
S71, inverse solving the joint velocity in the model
Replaced by corresponding damping reciprocal function g i (i =1,2,3, …, 6), establishing a new robot speed inverse solution model>
wherein />
S72, utilizing the speed inverse solution model established in the step S71
Calculating the joint speed of all track points in the Cartesian track>
After the singular bit shape avoiding method is processed according to the above method, 4 conventional damping functions are selected for comparison in the effect verification process, and the comparison situations are shown in fig. 3 to 7.
Example 2
Based on the same inventive concept as embodiment 1, the present embodiment provides a robot speed calculation system, including the following modules:
the inverse solution and forward solution model building module is used for building an inverse solution model of the position of the joint robot, a forward solution model of the speed of the robot and an inverse solution model of the speed of the robot according to the terminal pose vector of the robot and the angle vector of the joint of the robot; wherein the robot velocity forward solution model and the robot velocity inverse solution model are expressed by a Jacobian matrix;
the singular condition calculation module is used for calculating a singular condition expression of the robot motion according to the Jacobian matrix and establishing a relational 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 conditional expression;
the singular boundary calculation module is used for solving the solution of the joint angle vector through a singular conditional expression, defining a singular region based on the solved solution and determining the 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 end point of a motion path of the robot and solving a Cartesian track of the motion of the robot; solving the maximum resultant 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 the parameters of the damping reciprocal functions of all joints of the iterative robot according to the calculated joint angle vectors and the maximum resultant velocity;
and the joint velocity solving module is used for setting an initial value of the maximum damping value and an initial value of the first joint bottom parameter, constructing a new robot velocity inverse solution model according to the iterated damping reciprocal function parameters of each joint, and solving the joint velocity of each track point.
The above modules of this embodiment are respectively configured to execute steps S1 to S7 of embodiment 1, and the detailed execution process of the above modules is referred to in embodiment 1 and is not described herein again.
Example 3
Based on the same inventive concept as that of embodiment 1, this embodiment provides a computer device, which includes a processor and a memory for storing an executable program of the processor, and the processor is configured to execute steps S1 to S7 of embodiment 1 when executing the program stored in the memory, and the detailed execution process is as shown in embodiment 1 and is not described herein again.
The above description is only a preferred embodiment of the present invention, but the scope of the present invention is not limited thereto, and any other changes, modifications, substitutions, combinations, and simplifications which do not depart from the spirit and principle of the present invention should be regarded as being equivalent to the scope of the present invention.
Claims (10)
1. A robot joint velocity calculation method is characterized by comprising the following steps:
according to the robot 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; wherein the robot velocity forward solution model and the robot velocity inverse solution model are expressed by a Jacobian matrix;
calculating to obtain a singular condition expression of robot motion according to a Jacobian matrix, and establishing a relational 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 conditional expression;
solving the solution of the joint angle vector through a singular conditional expression, defining a singular area based on the solved solution, and determining a singular boundary of the robot according to the singular area;
setting a starting point and an end point of a robot motion path, and solving a Cartesian track of the robot motion; solving the maximum resultant 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 maximum damping value and an initial value of a first joint bottom parameter according to the calculated joint angle vector and the maximum resultant speed, and optimizing parameters of a damping reciprocal function of each joint of the iterative robot;
and constructing a new robot speed inverse solution model according to the iterated damping reciprocal function parameters of each joint, and solving the joint speed of each track point.
2. The robot joint velocity calculation method according to claim 1, wherein the establishing of the joint robot position inverse solution model, the robot velocity forward solution model, and the robot velocity inverse solution model based on the robot end pose vector and the robot joint angle vector comprises:
establishing a robot end pose vector P = [ x ] P y P z P ψ Px ψ Py ψ Pz ] T A robot position inverse solution model of the robot joint angle vector is obtained;
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 a singular conditional expression of the robot motion is calculated according to a Jacobian matrix, and a relational model between the joint velocity and the singular conditional expression is established, including:
dividing the Jacobian matrix to obtain a plurality of sub-matrices;
solving and simplifying a determinant expression of the submatrix according to the plurality of partitioned submatrices, and defining a singular condition expression according to the determinant expression of the simplified submatrix, wherein the singular condition expression is a nonlinear function of a robot joint angle vector theta;
and respectively inverting the sub-matrixes, and substituting the inversion results of the sub-matrixes into the robot speed inverse solution model to simplify the robot speed inverse solution model.
4. The robot joint velocity calculation method according to claim 1, wherein according to the singular condition expression, a damping reciprocal function designed for each joint of the robot is a fractional expression, and the damping increment 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.
5. The robot joint velocity calculation method according to claim 1, wherein the singular conditional expressions include a first singular conditional expression, a second singular conditional expression, and a third singular conditional expression;
solving the solution of the joint angle vector through a singular conditional expression, defining a singular area based on the solved solution, and determining the singular boundary of the robot according to the singular area, wherein the singular boundary comprises the following steps:
setting the first singular conditional expression equal to 0, solving the solution theta' of the joint angle vector which satisfies the first singular conditional expression equal to 0 through the singular conditional expression, then defining a singular area, and calculating a first singular boundary epsilon 1 ；
Respectively making the second and third singular conditional expressions equal to 0, respectively obtaining the solutions of the joint angle vectors which satisfy the second and third singular conditional expressions equal to 0 through the singular conditional expressions, then defining singular areas, and calculating a second singular boundary epsilon 2 Third singular boundary ε 3 。
6. The robot joint velocity calculation method according to claim 5, wherein the robot end pose vector P = [ x ], where P y P z P ψ Px ψ Py ψ Pz ] T The attitude matrix Θ of (a) is:
wherein [nx n y n z ] T As attitude coordinate vector [ psi Px ψ Py ψ Pz ] T A component projected in the X-axis direction; [ o ] x o y o z ] T As attitude coordinate vector [ psi Px ψ Py ψ Pz ] T A component projected in the Y-axis direction; [ a ] A x a y a z ] T As attitude coordinate vector [ psi Px ψ Py ψ Pz ] T A component projected in the Z-axis direction;
let O i -X i Z i The coordinate of the ith joint is set as the coordinate transformation matrix of the adjacent two joints i of the robot and the joint i-1 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 Axial edge X i-1 Distance in the axial direction; alpha (alpha) ("alpha") i-1 Is Z i-1 Axis to X i Axial X i-1 The angle of rotation of the shaft; d i Is X i-1 Axis to X i Axial edge Z i Distance in the axial direction; theta i Is X i-1 Axis to X i Axial winding Z i The angle of rotation of the shaft;
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 The analytical expression of (1):
wherein [i q x i q y i q z ] T Is the ith joint coordinate origin O i In a coordinate system O 0 -X 0 Y 0 Z 0 A position coordinate vector of (1); [ i n x i n y i n z ] T Is the ith joint coordinate origin O i In the X posture coordinate vector 0 A component of the axial projection; [ i o x i o y i o z ] T Is the ith joint coordinate origin O i In the attitude coordinate of 0 A component of the axial projection; [ i a x i a y i a z ] T Is the ith joint coordinate origin O i In Z 0 A component of the axial projection;
calculating to obtain an expression (in the ith joint coordinate) of the attitude matrix theta 0 T i ) -1Θ, wherein (0 T i ) -1 For coordinate transformation matrix 0 T i The inverse matrix of (d);
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 positive solution model of the robot speed is
The inverse model of the speed of the robot is->
wherein />
Is angular velocity vector of robot joint, V is terminal velocity vector of robot, J is Jacobian matrix of robot, J -1 Is the inverse of the Jacobian matrix of the robot.
7. The robot joint velocity calculation method according to claim 6, wherein the 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 ；
Partition the Jacobian matrix into 4 sub-matrices J 11 、J 12 、J 21 and J22 And the submatrix J 11 、J 22 The rewriting is as follows:
wherein ,J′11 Is J 11 Temporary calculation matrix of J' 22 Is J 22 The temporary calculation matrix of (2);
submatrix J 11 、J 22 Respectively inverse 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 Substituting the inversion result into the robot speed inverse solution model
The following expression is obtained:
wherein ,J′11 -1 Is a temporary calculation of matrix J' 11 Inverse matrix, J' 22 -1 Is a temporary calculation matrix J' 22 The inverse matrix of (d); defining:
then further simplifying to obtain a robot speed inverse solution model:
the method for optimizing the parameters of the damping reciprocal functions of the joints of the iterative robot by setting the initial value of the maximum damping value and the initial value of the first joint bottom parameter according to the solved joint angle vector and the maximum resultant velocity comprises the following steps:
setting a maximum value λ of damping max Initial value λ of 0 And a first articular base parameter σ 1 Initial value σ of (2) 1,0 ；
According to a given lambda 0 、σ 1,0 When K is 1 (θ)∈[0,ε 1 ]K corresponding to the maximum value of the reciprocal damping function 1 (theta) is denoted by K 1 ' (θ); define all satisfy K 1 (θ)≥K′ 1 Singular conditional expression K of (theta) 1 The minimum value of (theta) is
When the joint angle vector is->
Define all satisfy K 1 (θ)≥ε 1 Singular conditional expression of 1 (θ) has a minimum value of ≦ ≦>
The angle vector of the joint is->
Calculating the combined speed of all Cartesian track points (V |) 2 The velocity vector of the tail end of the robot corresponding to the maximum resultant velocity is V max (ii) a Joint angle vector
To obtain F 1 Upper limit of (theta, V)>
If it is not
Output lambda 0 (ii) a Otherwise λ 0 Step length h is increased 1 Calculating the joint angle vector corresponding to each Cartesian track point again; wherein->
The set maximum angular velocity of the first joint of the robot;
iterative optimization of the maximum damping λ max And determining the base parameters σ of all joints i 。
8. A robot joint velocity calculation method according to claim 7, characterized in that the iteratively optimized damping maximum λ max And determining the base parameter σ of all joints i The method comprises the following steps:
giving the upper limit E of the speed error of the first joint in the non-singular region 1 Combined with joint angle vector
To obtain F 1 Lower bound on (theta, V) singular boundaries>
Preliminary determination of maximum damping value lambda max (ii) a If:
then λ max ＝λ 0 ，σ 1 ＝σ 1,0 (ii) a Or else the bottom parameter sigma 1 Initial value σ of 1,0 Step length h is increased 2 Then the joint angle vector is solved again;
according to the maximum value lambda of damping max Determining lambda 2 Likewise let σ 2,0 = e; adopting a method for calculating the corresponding parameter of the first joint to carry out damping maximum lambda on the second joint max Calculating;
given an upper limit E of the velocity error in the non-singular region of the second joint 2 Obtaining a bottom parameter sigma corresponding to a second joint by adopting a method for solving the bottom parameter sigma corresponding to the first joint 2 ；
Calculating the final maximum damping value lambda by adopting a calculation method of parameters corresponding to the second joint max And the base parameters of the remaining joints.
9. A robot joint velocity calculation system, comprising the following modules:
the inverse solution and forward solution model building module is used for building an inverse solution model of the position of the joint robot, a forward solution model of the speed of the robot and an inverse solution model of the speed of the robot according to the terminal pose vector of the robot and the angle vector of the joint of the robot; wherein the positive solution model of the robot speed and the inverse solution model of the robot speed are expressed by a Jacobian matrix;
the singular condition calculation module is used for calculating a singular condition expression of the robot motion according to the Jacobian matrix and establishing a relational 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 conditional expression;
the singular boundary calculation module is used for solving the solution of the joint angle vector through a singular conditional expression, defining a singular region based on the solved solution and determining the 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 end point of a motion path of the robot and solving a Cartesian track of the motion of the robot; solving the maximum resultant 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 maximum damping value and an initial value of a first joint bottom parameter, and optimizing parameters of a damping reciprocal function of each joint of the iterative robot according to the calculated joint angle vector and the maximum resultant speed;
and the joint speed solving module is used for constructing a new robot speed inverse solution model according to the damping reciprocal function parameters of each joint after iteration and solving the joint speed of each track point.
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 configured to perform the method of calculating the velocity of a robot joint according to 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 true CN115922705A (en) | 2023-04-07 |
| CN115922705B 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 |
|---|
| 李贺立;杨冬;杨德志;李铁军;马宏;: "双臂机器人避关节极限与避奇异位形优化研究", 科学技术与工程, no. 03 * |
| 金荣玉;耿云海;: "空间机器人动力学奇异回避的笛卡尔轨迹规划", 宇航学报, no. 08 * |
Also Published As
| Publication number | Publication date |
|---|---|
| CN115922705B (en) | 2023-09-15 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN105773620B (en) | The trajectory planning control method of industrial robot free curve based on Double quaternions | |
| Ceccarelli et al. | Fundamentals of the mechanics of robots | |
| 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 | |
| CN107160401B (en) | Method for solving problem of joint angle deviation of redundant manipulator | |
| CN111761582B (en) | Mobile mechanical arm obstacle avoidance planning method based on random sampling | |
| CN111452041B (en) | Inverse kinematics solving method for non-spherical wrist 6R robot | |
| WO2023024317A1 (en) | Robot obstacle avoidance method and apparatus, and robot | |
| CN114800534B (en) | Mechanical arm control method and device | |
| CN111958602B (en) | Real-time inverse solution method for wrist offset type 6-axis robot | |
| CN112975954B (en) | Control method of robot arm, computer device, and storage medium | |
| CN116038702B (en) | Seven-axis robot inverse solution method and seven-axis robot | |
| CN115648228A (en) | Industrial robot multi-source error compensation method, device, equipment and storage medium | |
| Xu et al. | Models for three new screw-based IK sub-problems using geometric descriptions and their applications | |
| CN109366486B (en) | Flexible robot inverse kinematics solving method, system, equipment and storage medium | |
| CN115922705A (en) | Robot joint speed calculation method and system and computer equipment | |
| Wild et al. | Efficient and scalable inverse kinematics for continuum robots | |
| CN111890364A (en) | Robot motion control method, device, electronic device and storage medium | |
| CN113787502B (en) | Three-wheel omnidirectional mobile robot state adjusting method based on neurodynamics | |
| CN112684793B (en) | Trajectory tracking control method for zero-radius over-bending of robot in discrete system | |
| CN112847441B (en) | Six-axis robot coordinate offset detection method and device based on gradient descent method | |
| Ledesma et al. | A non‐recursive Lagrangian solution of the non‐causal inverse dynamics of flexible multibody systems: The planar case | |
| CN113778082A (en) | Unmanned vehicle track tracking control method and system based on self-triggering mechanism | |
| CN117733872B (en) | Series robot inverse kinematics control method based on directional performance | |
| JP2021122894A (en) | Inverse kinematics calculation device and inverse kinematics calculation method | |
| Ren et al. | A heuristic iterative method for solving the inverse solution of kinematics of manipulators |
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 |