CN109895101B - Unique solution method for inverse kinematics numerical value of joint type mechanical arm - Google Patents
Unique solution method for inverse kinematics numerical value of joint type mechanical arm Download PDFInfo
- Publication number
- CN109895101B CN109895101B CN201910278057.0A CN201910278057A CN109895101B CN 109895101 B CN109895101 B CN 109895101B CN 201910278057 A CN201910278057 A CN 201910278057A CN 109895101 B CN109895101 B CN 109895101B
- Authority
- CN
- China
- Prior art keywords
- coordinate system
- joint
- axis
- mechanical arm
- matrix
- 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
Images
Landscapes
- Manipulator (AREA)
- Numerical Control (AREA)
Abstract
The invention discloses a unique inverse kinematics value solution method for a joint type mechanical arm, belongs to the technical field of modern intelligent manufacturing, and relates to a unique inverse kinematics value solution method for a joint type six-degree-of-freedom mechanical arm with a shoulder joint biased forwards in the field of industrial robots. The method comprises the steps of establishing a mechanical arm joint coordinate system according to an improved DH parameter method, determining 4 structural geometric parameters between adjacent joints of a mechanical arm, and calculating homogeneous coordinate transformation matrixes of the two adjacent coordinate systems. For a given end coordinate system O6The pose matrix of the method adopts an improved Newton iteration method, namely a Levenberg-Marquardt iteration algorithm. Calculating the inverse kinematics solution of the joint coordinate system by using the Jacobian matrix J to obtain a group of six joint rotation angle values theta corresponding to the pose matrix and meeting the precision requirementi. The method overcomes the requirement that the traditional Newton iteration method has to be full-rank for the Jacobian matrix J, and the modeling method is simpler and clearer and is effective. The method has the characteristics of high solving precision, high solving speed and simple and easy solving process.
Description
Technical Field
The invention belongs to the technical field of modern intelligent manufacturing, and relates to an inverse kinematics numerical unique solution solving method of a joint type six-degree-of-freedom mechanical arm with a shoulder joint biased forwards in the field of industrial robots.
Background
The problem of inverse kinematics of the joint type mechanical arm is that under the condition that the position and the posture of a coordinate system of an end effector of the mechanical arm relative to a base coordinate system and geometric parameters of joints of the mechanical arm are given, the rotation angle values of the joints of the mechanical arm are obtained, and the process is the inverse process of the forward kinematics. The forward kinematics can obtain a homogeneous coordinate transformation matrix T between the coordinate systems of the front and rear adjacent joints according to the structural geometric parameters of the mechanical arm, namely a pose matrix between the two joints; if the rotation angle theta of each joint is known, the pose matrix of the coordinate system of the mechanical arm end effector can be obtained by sequentially and continuously right-multiplying the homogeneous transformation matrix T of each joint, and the obtained result is unique. The solution of inverse kinematics is relatively complex and may have no solution or multiple solutions, for example, the coordinate system of the end effector has no solution at a singular point, and the periodicity of the inverse trigonometric function causes a theoretical multiple solution problem of the resolution of the rotation angle of the mechanical arm joint.
The wrist of most articulated six-degree-of-freedom manipulators meets the Pieper criterion, namely, the rotation axes of three adjacent joints of the wrist are intersected at a point, and the three adjacent joints of the wrist are decoupled. At present, most of common coordinate system modeling methods in the robot field are a Denavit-Hartenberg parameter method, referred to as DH parameter method for short, namely a matrix method for establishing a coordinate system for each rod piece in a mechanical arm joint chain, coordinate directions and geometric parameters between adjacent joints are described in the method, and the method is intuitive. For example, patent No. CN105573143A, entitled "inverse kinematics solution method for six-degree-of-freedom industrial robot" disclosed by dow-keng et al, needs to dissociate a solution with the smallest sum of the angle difference norms of the respective joint rotation axes corresponding to the positions of the previous joint space from eight sets of results corresponding to the obtained six joint rotation axis rotation angles. However, the norm which is obtained cannot be substituted into calculation again, and is just used as a judgment basis for selecting a better solution from a plurality of inverse solutions, and the calculation accuracy and the calculation speed of the norm cannot meet the requirements of modern production. The patent number CN103942427A discloses a quick and simple method for solving inverse kinematics of a six-degree-of-freedom mechanical arm, namely a method disclosed by Zhuzidan et al, adopts an Euler angle transformation matrix to solve, and does not avoid the problem of universal joint lock of the method. In addition, the "inverse kinematics solution method and device for service robot in intelligent space" disclosed by the field congress et al, and the "inverse kinematics solution method for robot based on particle swarm optimization algorithm" disclosed by luyahui et al, adopt the genetic algorithm and the particle swarm algorithm to carry out inverse kinematics solution, have complicated procedures, need high-performance calculation configuration, and have the problem of insufficient stability.
Disclosure of Invention
The invention aims to overcome the defects and shortcomings of inverse kinematics solution of the existing mechanical arm: the general solving method has the advantages of single coordinate system modeling method, complex and difficult solving process, multiple solutions, long calculating time and low solving speed. The invention discloses a method for solving the unique inverse kinematics value solution of a joint type mechanical arm, which is used for rapidly obtaining the unique inverse kinematics solution of a joint type six-degree-of-freedom mechanical arm with a shoulder joint biased forwards. The method comprises the steps of establishing a mechanical arm joint coordinate system according to an improved DH parameter method, determining 4 structural geometric parameters between adjacent joints of a mechanical arm, calculating homogeneous coordinate transformation matrixes of two adjacent coordinate systems, and multiplying the homogeneous coordinate transformation matrixes right in sequence to obtain a terminal coordinate system O6A pose matrix for the base coordinate system; the position and pose matrix is sorted to obtain the rotation angle theta of each jointiRPY corner rx,ry,rzAnd a translation distance px,py,pzThe relation between them. Then the relational expression is respectively aligned to the joint rotation angles thetaiPartial derivatives are calculated to obtain a Jacobian matrix J of 6 × 6. finally, for a given end coordinate system O6The method adopts an improved Newton iteration method, namely a Levenberg-Marquardt iteration algorithm, utilizes a Jacobian matrix J to calculate the inverse kinematics solution of a joint coordinate system, and obtains a group of six joint rotation angle values theta which correspond to the position matrix and meet the precision requirementiThe requirement that the traditional Newton iteration method has to be full-rank for the Jacobian matrix J is overcome. The method is effective and simple and clear in process.
The invention adopts the technical scheme that the method is a method for solving the unique inverse kinematics numerical value solution of the joint type mechanical arm, the method establishes a mechanical arm joint coordinate system according to an improved DH parameter method, determines 4 structural geometric parameters between adjacent joints of the mechanical arm, and calculates the homogeneous coordinate transformation of two adjacent coordinate systemsMatrix, and right multiplying the homogeneous coordinate transformation matrix to obtain the terminal coordinate system O6A pose matrix for the base coordinate system; the position and pose matrix is sorted to obtain the rotation angle theta of each jointiI is equal to 1,2,3,4,5,6 and RPY corner rx,ry,rzAnd a translation distance px,py,pzThe relation between; then the relational expression is respectively aligned to the joint rotation angles thetaiPartial derivatives are calculated to obtain a Jacobian matrix J of 6 × 6, and finally, for a given end coordinate system O6The pose matrix adopts an improved Newton iteration method-Levenberg-Marquardt iteration algorithm, utilizes a Jacobi matrix J to calculate the inverse kinematics solution of a joint coordinate system, and obtains a group of six joint rotation angle values theta which correspond to the pose matrix and meet the precision requirementi(ii) a The method comprises the following specific steps:
step one, constructing a joint type mechanical arm and a coordinate system.
The joint type mechanical arm consists of a base A, an end effector G, 5 connecting rods B, C, D, E, F and 6 rotary joints 1,2,3,4,5 and 6; establishing a coordinate system of each joint of the mechanical arm based on an improved DH parameter method, wherein the coordinate system comprises: base coordinate system O0Coordinate system O corresponding to six rotary joints of mechanical arm1～O6And a coordinate system O of the end effector of the robot arm6(ii) a The coordinate system of each joint is specifically as follows: z of joint iiThe axis is collinear with the axis of joint i, and xiThe axis is on the common normal line of the axes of the joints i and i +1, and the direction of the axis is from i to i + 1; when the two joint axes intersect, xi-1Cross product z of direction of (d) and two vectorsi-1×ziCoaxial, co-directional or counter, xi-1Always points along a common normal from axis i-1 to axis i; when two axes xi-1And xiParallel and in the same direction theta of the ith rotary jointiIs zero; y isiThe axes are determined by the right-hand rectangular coordinate system rule, wherein i is 1,2,3,4,5 and 6; setting the initial position of the first joint coordinate system on the base of the mechanical arm and the base coordinate system { O }0:x0,y0,z0The coincidence and the base coordinate system are always kept unchanged.
And step two, establishing a homogeneous coordinate transformation matrix of the adjacent coordinate systems of the mechanical arm joints.
According to 4 structural geometric parameters between adjacent joints of the mechanical arm: link rod angle thetaiLink torsion angle αiLength of connecting rod aiDistance d of connecting rodiComputing homogeneous coordinate transformation matrices of adjacent coordinate systemsi-1Ti1,2,. 6; definition of the geometric parameters: link angle theta between adjacent joints i and i +1iIs xiAxis and xi-1Angle between axes, around ziAxis from xi-1Axis to xiAxis, positive when in accordance with the right hand rule, theta for a revolute jointiAs variable, connecting rod torsion angle αiIs ziAxis and zi+1Angle between axes, around xiAxis from ziAxis to zi+1The axis, positive according to the right-hand rule, when the two joint axes are parallel αi0, when the two joint axes are perpendicular, αi-90 ° or 90 °; length of connecting rod aiIs ziAxis and zi+1Length of common perpendicular to axis, along xiAxial direction measurement, when the axes of the two joints are parallel, ai＝li，liFor the length of the connecting rod, when the axes of the two joints are perpendicular, ai0; link distance d between adjacent jointsiIs xiAxis and xi-1Distance between axes at ziOn-axis measurement, for revolute joints, diIs a constant.
Calculating each homogeneous transformation matrix of adjacent coordinate systems according to the homogeneous transformation rule between the joint coordinate systemsi-1Ti(ii) a Homogeneous coordinate transformation matrix between adjacent joint coordinate systems of mechanical armi-1TiComprises the following steps:
wherein, thetaiIs angle of connecting rod αiIs a connecting rod torsion angle aiIs the length of the connecting rod, diIs the link distance.
And step three, establishing a homogeneous coordinate transformation matrix for solving the inverse kinematics of the mechanical arm.
Transforming the homogeneous transformation matrix of adjacent coordinate systems of the mechanical arm jointi-1TiAnd sequentially right multiplying to obtain:
where, the equation matrix on the right:0T1、1T2、2T3、3T4、4T5、5T6respectively, the homogeneous coordinate transformation matrix of the first coordinate system, the second coordinate system, the third coordinate system, the fourth coordinate system, the fifth coordinate system and the terminal coordinate system relative to the former coordinate system.
Left matrix of equation0T6As an end coordinate system O6Relative to a base coordinate system O0A homogeneous coordinate transformation matrix; wherein n isx,ny,nzRespectively, end coordinate system { O6:x6,y6,z6X of6X of axis and base coordinate system0,y0,z0Cosine value of included angle of the shaft; ox,oy,ozY being respectively the terminal coordinate system6X of axis and base coordinate system0,y0,z0Cosine value of included angle of the shaft; a isx,ay,azZ being respectively the terminal coordinate system6X of axis and base coordinate system0,y0,z0Cosine value of included angle of the shaft; p is a radical ofx,py,pzAs the origin O of the terminal coordinate system6Cartesian coordinates in a base coordinate system.
0T6It can also be represented by an RPY combinatorial transform:
wherein the content of the first and second substances,
are respectively
Abbreviation of (n), Trans (p)x,py,pz) Representing a translation p of the end coordinate system along the x, y, z axes of the base coordinate systemx,py,pzA distance;
indicating that the end coordinate system is first rotated about the z-axis of the base coordinate system
Angle, then rotated by an angle theta about a new y-axis and rotated by an angle phi about a new z-axis, i.e. RPY rotation angle rx,ry,rz。
Simultaneous formulas (2) and (3) to obtain the rotation angle theta of each jointi(i-1, 2,3,4,5,6), RPY corner rx,ry,rzAnd a translation distance px,py,pzThe relation between:
and step four, establishing a Jacobian matrix for solving the inverse kinematics of the mechanical arm.
According to r in formula (4)x,ry,rz,px,py,pzAnd thetaiI is a relation between 1,2,3,4,5,6, and f isiI is 1,2,3,4,5,6 to θiAnd i is 1,2,3,4,5,6, and partial derivatives are calculated to obtain a jacobian matrix of the joint coordinate system:
and step five, solving the unique mathematical solution of the inverse kinematics of the joint type mechanical arm.
For a given robot arm tip coordinate system O6When the pose matrix of the joint coordinate system is calculated by using a classical Newton iterative algorithm, the Jacobian matrix J is equal to the joint corner thetaiIn connection withMatrices, in some special cases JTJ is singular, where Newton's iterative method has no solution. Therefore, when the inverse kinematics numerical solution of the joint coordinate system is solved, an improved Newton iteration method-Levenberg-Marquardt iteration algorithm is adopted, and the LM iteration method overcomes the requirement that the Newton iteration method has to be full-rank for the Jacobian matrix J.
When LM iteration is used to calculate the given end coordinate system O of the mechanical arm6Pose matrix T ofendCorresponding inverse kinematics numerical solution θendWhen the current terminal coordinate system O is recorded6Pose matrix T ofcurAnd corresponding joint angles thetacur. From TendAnd TcurCalculating a differential operator Δ:
the differential motion vector D ═ dx dy dz x y z can be obtained by the differential operator delta]T。
For the current iteration point thetacurThe search direction of the LM iteration method is as follows:
dθ＝(JTJ+μI)-1JTD (7)
wherein μ is a positive parameter to prevent when J isTD θ is too large near the odd phase of J. Will thetacurAdding d theta to obtain a new iteration point thetanewAnd calculating a pose matrix T of the end coordinate system of the mechanical arm corresponding to the new iteration point by using the positive kinematics modelnewSubstituting it into formula (5) and formula (6), and updating the Jacobian matrix JnewAnd a differential motion vector Dnew。
To ensure global convergence of the LM iteration method, D is compared after each iterationnewAnd the two-norm of D. If | | | Dnew||2<||D||2Then give an order
Mu tonew,Dnew,JnewBringing in the next iteration. If | | | Dnew||2≥||D||2Let us order munew2 μ, mixingnewD, J, carry over to the next iteration. Up to | | Dnew||2When the iteration precision is smaller than the iteration precision, ending the iteration process; at this time, θnewNamely the inverse kinematics numerical solution of the joint coordinate system.
The method has the advantages that the Levenberg-Marquardt iterative algorithm is adopted to quickly obtain the unique inverse kinematics solution of the joint type six-degree-of-freedom mechanical arm with the shoulder joint biased forwards, the requirement that the traditional Newton iterative method has to be full-rank for the Jacobian matrix J is overcome, the coordinate system modeling method is simple for the mechanical arm with the structure, and the numerical solution precision is high. The inverse kinematics solving process provided by the invention is more effective, and has the characteristics of high solving precision, high solving speed and simpler and easier solving process.
Drawings
FIG. 1 is a flow chart of a method for solving the unique solution of the inverse kinematics numerical value of the joint type mechanical arm.
Fig. 2-structure diagram of joint type six-degree-of-freedom mechanical arm. Wherein, A-base, B-connecting rod 1, C-connecting rod 2, D-connecting rod 3, E-connecting rod 4, F-connecting rod 5, G-end actuator, a2Length of connecting rod 2, a3Length of connecting rod 3, a4Length of the connecting rod 4, d4Distance of link 3 from link 4, d6The distance between the link 5 and the end effector.
FIG. 3 is the structure diagram of the joint type six-freedom-degree mechanical arm and the coordinate system of each joint. Wherein, 1-joint 1, 2-joint 2, 3-joint 3, 4-joint 4, 5-joint 5, 6-joint 6; the determination of each coordinate system takes the form of a modified DH parameter method, each joint being rotated about the z-axis of the joint coordinate system, in particular, ziThe shaft being along the axis of the (i + 1) th joint, xiAlong ziAxis and zi-1Common perpendicular to the axes, directed away from zi-1Direction of axis, yiThe axes are determined by the right-handed rectangular coordinate system rule, OiIs the origin of the ith coordinate system; the embodiment sets the initial position of the first joint coordinate system on the base of the mechanical arm and the base coordinate system O0:x0,y0,z0The coincidence of the base coordinate system is alwaysRemain unchanged.
Detailed Description
The following detailed description of the embodiments of the invention refers to the accompanying drawings and claims.
FIG. 1 is a flow chart of a method for solving a unique inverse kinematics numerical solution of a joint type mechanical arm according to the present invention; fig. 2 is a structural diagram of a joint type six-degree-of-freedom mechanical arm, which is composed of a base a, an end effector G, 5 connecting rods B, C, D, E, F, and 6 rotary joints 1,2,3,4,5, and 6. The coordinate system of each joint of the mechanical arm is shown as the attached figure 3, and the specific steps of the method flow are as follows:
step one, establishing a coordinate system of each joint of the mechanical arm based on an improved DH parameter method, as shown in an attached drawing 3.
Step two, giving 4 structural geometric parameters between adjacent joints of the initial mechanical arm: angle of rotation theta of connecting rodiLink torsion angle αiLength of connecting rod aiDistance d of connecting rodi. In this embodiment, the DH parameters of each link of the mechanical arm are: when i is 1, the link angle is theta1The distance between the connecting rods is 0, the length of the connecting rods is 0, and the torsion angle of the connecting rods is 0; when i is 2, the link angle is theta2+90 °, link distance 0, link length a2The torsion angle of the connecting rod is 90 degrees; when i is 3, the link angle is theta3Link distance is 0 and link length is a3The torsion angle of the connecting rod is 0; when i is 4, the link angle is theta4Distance of connecting rod is d4The length of the connecting rod is a4The torsion angle of the connecting rod is 90 degrees; when i is 5, the link angle is theta5The distance between the connecting rods is 0, the length of the connecting rods is 0, and the torsion angle of the connecting rods is-90 degrees; when i is 6, the link angle is theta6+90 ° and link distance d6The length of the connecting rod is 0, and the torsion angle of the connecting rod is 90 degrees. In this embodiment, d6Including the tool length.
Based on the modified DH parameters method, the parameter values for each structure in the examples were found as follows:
a2＝160,a3＝790,a4＝155,d4＝795,d6＝145
computing homogeneous coordinate transformation of adjacent coordinate systems of mechanical arm jointsMatrix arrayi-1Ti1,2,. 6; using formula (1) homogeneous coordinate transformation matrix between adjacent joint coordinate systems of mechanical armi-1TiIn this embodiment, each transformation matrix is as follows:
step three, combining the formulas (2) and (3) to obtain the rotation angle theta of each jointiI is equal to 1,2,3,4,5,6 and RPY corner rx,ry,rzAnd a translation distance px,py,pzThe relation (4) therebetween.
Step four, according to the actual working range of the mechanical arm, a group of joint rotation angles theta is given according to the embodiment1＝30°,θ2＝-20°,θ3＝40°,θ4＝-40°,θ5＝20°,θ6Obtaining a pose matrix T of the end coordinate system relative to the base coordinate system through positive kinematic calculation at 50 DEGend，
Let the current end coordinate system O6Pose matrix T ofcur，
And corresponding joint angles thetacur＝[0 0 0 0 0 0]T. The iteration point theta at this time is calculated by equation (5)curA corresponding jacobian matrix J is formed,
step five, setting the positive parameter mu to be 0.1, and combining mu, J and Tcur,θcur,TendCarrying out LM iterative algorithm to obtain the inverse kinematics numerical solution theta of the mechanical arm joint coordinate systemend，
θ1＝30.000000000007°,θ2＝-19.999999999997°,θ3＝39.999999999986°,θ4＝-39.999999999747°,θ5＝20.000000000004°,θ6＝49.999999999744°
Thereby obtaining a unique group of mechanical arm joint rotation angle expressions, and the error of the calculation result is less than 1 × 10-7°And performing several sets of positive and negative operations by using the determined expression, wherein the result also satisfies that the error is less than 1 × 10-7°And (4) concluding.
The unique solution solving method for the inverse kinematics numerical value of the joint type six-degree-of-freedom mechanical arm is simple and easy to implement, the unique solution can be rapidly obtained, and the result error is less than 1 × 10-7°。
Claims (1)
1. A method for solving the unique solution of the inverse kinematics numerical value of a joint type mechanical arm is characterized in that a mechanical arm joint coordinate system is established according to an improved DH parameter method, 4 structural geometric parameters between adjacent joints of the mechanical arm are determined, a homogeneous coordinate transformation matrix of two adjacent coordinate systems is calculated, and the homogeneous coordinate transformation matrices are sequentially multiplied right to obtain a terminal coordinate system O6Pose matrix for base coordinate system(ii) a The position and pose matrix is sorted to obtain the rotation angle theta of each jointiI is 1,2,3,4,5,6, RPY corner rx,ry,rzAnd a translation distance px,py,pzThe relation between; then the relational expression is respectively aligned to the joint rotation angles thetaiPartial derivatives are calculated to obtain a Jacobian matrix J of 6 × 6, and finally, for a given end coordinate system O6The pose matrix adopts an improved Newton iteration method-Levenberg-Marquardt iteration algorithm, utilizes a Jacobi matrix J to calculate the inverse kinematics solution of a joint coordinate system, and obtains a group of six joint rotation angle values theta which correspond to the pose matrix and meet the precision requirementi(ii) a The method comprises the following specific steps:
step one, constructing a joint type mechanical arm and a coordinate system:
the joint type mechanical arm consists of a base (A), an end effector (G), 5 connecting rods (B, C, D, E, F) and 6 rotary joints (1, 2,3,4,5 and 6); establishing a coordinate system of each joint of the mechanical arm based on an improved DH parameter method, wherein the coordinate system comprises: base coordinate system O0Coordinate system O corresponding to six rotary joints of mechanical arm1～O6And a coordinate system O of the end effector of the robot arm6(ii) a The coordinate system of each joint is specifically as follows: z of joint iiThe axis is collinear with the axis of joint i, and xiThe axis is on the common normal line of the axes of the joints i and i +1, and the direction of the axis is from i to i + 1; when the two joint axes intersect, xi-1Cross product z of direction of (d) and two vectorsi-1×ziIn the same or opposite direction, xi-1Always points along a common normal from axis i-1 to axis i; when two axes xi-1And xiParallel and in the same direction theta of the ith rotary jointiIs zero; y isiThe axes are determined by the right-hand rectangular coordinate system rule, wherein i is 1,2,3,4,5 and 6; setting the initial position of the first joint coordinate system on the base of the mechanical arm and the base coordinate system { O }0:x0,y0,z0The superposition, the base coordinate system is always kept unchanged;
step two, establishing a homogeneous coordinate transformation matrix of the adjacent coordinate systems of the mechanical arm joints:
according to adjacent closing of mechanical arms4 structural geometric parameters between the nodes: link rod angle thetaiLink torsion angle αiLength of connecting rod aiDistance d of connecting rodiComputing homogeneous coordinate transformation matrices of adjacent coordinate systemsi-1Ti1,2,. 6; definition of the geometric parameters: link angle theta between adjacent joints i and i +1iIs xiAxis and xi-1Angle between axes, around ziAxis from xi-1Axis to xiAxis, positive when in accordance with the right hand rule, theta for a revolute jointiAs variable, connecting rod torsion angle αiIs ziAxis and zi+1Angle between axes, around xiAxis from ziAxis to zi+1The axis, positive according to the right-hand rule, when the two joint axes are parallel αi0, when the two joint axes are perpendicular, αi-90 ° or 90 °; length of connecting rod aiIs ziAxis and zi+1Length of common perpendicular to axis, along xiAxial direction measurement, when the axes of the two joints are parallel, ai＝li，liFor the length of the connecting rod, when the axes of the two joints are perpendicular, ai0; link distance d between adjacent jointsiIs xiAxis and xi-1Distance between axes at ziOn-axis measurement, for revolute joints, diIs a constant;
calculating each homogeneous transformation matrix of adjacent coordinate systems according to the homogeneous transformation rule between the joint coordinate systemsi-1Ti：
Wherein, thetaiIs angle of connecting rod αiIs a connecting rod torsion angle aiIs the length of the connecting rod, diIs the distance of the connecting rod;
step three, establishing a homogeneous coordinate transformation matrix for solving the inverse kinematics of the mechanical arm:
transforming the homogeneous transformation matrix of adjacent coordinate systems of the mechanical arm jointi-1TiAnd sequentially right multiplying to obtain:
where, the equation matrix on the right:0T1、1T2、2T3、3T4、4T5、5T6respectively a homogeneous coordinate transformation matrix of a first coordinate system, a second coordinate system, a third coordinate system, a fourth coordinate system, a fifth coordinate system and a tail end coordinate system relative to a previous coordinate system;
left matrix of equation0T6As an end coordinate system O6Relative to a base coordinate system O0A homogeneous coordinate transformation matrix; wherein n isx,ny,nzRespectively, end coordinate system { O6:x6,y6,z6X of6X of axis and base coordinate system0,y0,z0Cosine value of included angle of the shaft; ox,oy,ozY being respectively the terminal coordinate system6X of axis and base coordinate system0,y0,z0Cosine value of included angle of the shaft; a isx,ay,azZ being respectively the terminal coordinate system6X of axis and base coordinate system0,y0,z0Cosine value of included angle of the shaft; p is a radical ofx,py,pzAs the origin O of the terminal coordinate system6Cartesian coordinates in a base coordinate system;
transforming representations using RPY combinations0T6：
Wherein the content of the first and second substances,
are respectively
Abbreviation of (n), Trans (p)x,py,pz) Representing the coordinate system of the end along the baseX, y, z axis translation p of the systemx,py,pzA distance;
indicating that the end coordinate system is first rotated about the z-axis of the base coordinate system
Angle, then rotated by an angle theta about a new y-axis and rotated by an angle phi about a new z-axis, i.e. RPY rotation angle rx,ry,rz；
Calculating the rotation angle theta of each jointiI is 1,2,3,4,5,6, RPY corner rx,ry,rzAnd a translation distance px,py,pzThe relationship between:
step four, establishing a Jacobian matrix solved by the inverse kinematics of the mechanical arm:
according to rx,ry,rz,px,py,pzAnd thetaiI is a relation between 1,2,3,4,5,6, and f isiI is 1,2,3,4,5,6 to θiAnd i is 1,2,3,4,5,6, and partial derivatives are calculated to obtain a jacobian matrix J of the joint coordinate system:
step five, solving the unique mathematical solution of the inverse kinematics of the joint type mechanical arm:
for a given robot arm tip coordinate system O6When the pose matrix of the joint coordinate system is calculated by using a classical Newton iterative algorithm, the Jacobian matrix J is equal to the joint corner thetaiThe relevant matrix, in some special cases JTJ is singular, and the Newton iteration method has no solution, so that when the inverse kinematics numerical solution of the joint coordinate system is solved, an improved Newton iteration method is adoptedThe Levenberg-Marquardt iterative algorithm, the LM iterative method overcomes the requirement that the Newton iterative method has to be full of ranks for the Jacobian matrix J;
when LM iteration is used to calculate the given end coordinate system O of the mechanical arm6Pose matrix T ofendCorresponding inverse kinematics numerical solution θendWhen the current terminal coordinate system O is recorded6Pose matrix T ofcurAnd corresponding joint angles thetacurFrom TendAnd TcurCalculating a differential operator Δ:
obtaining a differential motion vector D by a differential operator delta:
D＝[dx dy dz x y z]T；
for the current iteration point thetacurSearch direction d θ of LM iterative method:
dθ＝(JTJ+μI)-1JTD (7)
wherein μ is a positive parameter to prevent when J isTWhen J is close to the odd phase, d theta is too large, theta is largercurAdding d theta to obtain a new iteration point thetanewAnd calculating a pose matrix T of the end coordinate system of the mechanical arm corresponding to the new iteration point by using the positive kinematics modelnewAnd updating the Jacobian matrix JnewAnd a differential motion vector Dnew；
To ensure global convergence of the LM iteration method, D is compared after each iterationnewAnd the two norms of D, if Dnew||2＜||D||2Then give an order
Mu tonew,Dnew,JnewCarry over to the next iteration if Dnew||2≥||D||2Let us order munew2 μ, mixingnewD, J carry over to the next iteration until | | Dnew||2When the iteration precision is smaller than the iteration precision, the iteration process is ended; at this time, θnewIs just offThe nodal coordinate system is an inverse kinematics numerical solution.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201910278057.0A CN109895101B (en) | 2019-04-09 | 2019-04-09 | Unique solution method for inverse kinematics numerical value of joint type mechanical arm |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201910278057.0A CN109895101B (en) | 2019-04-09 | 2019-04-09 | Unique solution method for inverse kinematics numerical value of joint type mechanical arm |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN109895101A CN109895101A (en) | 2019-06-18 |
| CN109895101B true CN109895101B (en) | 2020-09-11 |
Family
ID=66955519
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN201910278057.0A Active CN109895101B (en) | 2019-04-09 | 2019-04-09 | Unique solution method for inverse kinematics numerical value of joint type mechanical arm |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN109895101B (en) |
Families Citing this family (38)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN110181480A (en) * | 2019-07-08 | 2019-08-30 | 济南大学 | A kind of multi-joint is helped the elderly invalid wheelchair mechanical arm and its Kinematic Model method based on spinor theory |
| CN110744548B (en) * | 2019-11-08 | 2021-02-19 | 山东大学 | Unified decoupling method for drive line coupling relation of multi-line drive continuum mechanical arm |
| CN111300420B (en) * | 2020-03-16 | 2021-08-10 | 大连理工大学 | Method for solving minimum path of joint space corner of mechanical arm |
| CN111596159B (en) * | 2020-06-11 | 2021-06-15 | 青岛大学 | Electronic system EMI detection and positioning method based on six-axis mechanical arm |
| CN111844005B (en) * | 2020-07-08 | 2022-06-28 | 哈尔滨工业大学 | 2R-P-2R-P-2R mechanical arm motion planning method applied to tunnel wet spraying |
| CN112084576A (en) * | 2020-08-17 | 2020-12-15 | 盐城工学院 | Vehicle-mounted mechanical arm kinematics modeling method |
| CN111958602B (en) * | 2020-08-20 | 2022-05-20 | 华中科技大学 | Real-time inverse solution method for wrist offset type 6-axis robot |
| CN112487569B (en) * | 2020-11-13 | 2022-09-16 | 大连理工大学 | Solving method for working space of mobile robot capable of reaching fixed time length |
| CN112597437B (en) * | 2020-12-29 | 2022-09-16 | 深圳市优必选科技股份有限公司 | Method, device and equipment for analyzing inverse kinematics of mechanical arm |
| CN112828862B (en) * | 2020-12-30 | 2022-09-16 | 诺创智能医疗科技(杭州)有限公司 | Master-slave mapping method for parallel platform, mechanical arm system and storage medium |
| CN112936272B (en) * | 2021-02-04 | 2024-03-15 | 清华大学深圳国际研究生院 | Judgment method and solving method for singular configuration of mechanical arm |
| TW202235231A (en) * | 2021-03-09 | 2022-09-16 | 香港商香港科能有限公司 | Robot cooperation system and application and machining accuracy evaluation method thereof |
| CN113127989B (en) * | 2021-04-22 | 2024-02-06 | 中国科学院沈阳自动化研究所 | Six-degree-of-freedom mechanical arm inverse kinematics analytic solution control method |
| CN113715062B (en) * | 2021-09-14 | 2023-07-07 | 西安交通大学 | Calibration method for connecting rod parameters of mechanical arm |
| CN113878585A (en) * | 2021-11-03 | 2022-01-04 | 上海景吾智能科技有限公司 | Five-degree-of-freedom robot motion control method and system |
| CN114129266B (en) * | 2021-11-11 | 2024-05-14 | 深圳市精锋医疗科技股份有限公司 | Method, mechanical arm, equipment, robot and medium for keeping RC point unchanged |
| CN114012728B (en) * | 2021-11-12 | 2023-04-25 | 哈尔滨工业大学（威海） | Inverse kinematics solving method suitable for two-section 6-degree-of-freedom continuum mechanical arm |
| CN114131601A (en) * | 2021-12-02 | 2022-03-04 | 武汉科技大学 | Cartesian locus tracking inverse kinematics algorithm for offset wrist 6R industrial manipulator |
| CN114147714B (en) * | 2021-12-02 | 2023-06-09 | 浙江机电职业技术学院 | Method and system for calculating control parameters of mechanical arm of autonomous robot |
| CN114367979B (en) * | 2021-12-16 | 2023-07-18 | 西安理工大学 | Automatic connecting method for mining drill rod |
| CN114199706B (en) * | 2021-12-17 | 2023-06-27 | 合肥工业大学 | Load data decoupling method for corrugated pipe of automobile exhaust system |
| CN114102609B (en) * | 2021-12-27 | 2022-07-22 | 仲恺农业工程学院 | Newton iteration method-based inverse kinematics calculation method of banana picking robot |
| CN114002990B (en) * | 2021-12-30 | 2022-04-08 | 之江实验室 | Real-time control method and device for joint of parallel biped robot |
| CN114518709B (en) * | 2022-01-26 | 2024-05-17 | 哈尔滨工业大学 | Method, equipment and medium for resolving frame angle instruction of full-attitude four-axis turntable |
| CN114922179A (en) * | 2022-05-31 | 2022-08-19 | 中国矿业大学 | Side-clamping type hydraulic pile driver pose monitoring system and pose inverse solution method thereof |
| CN114888812B (en) * | 2022-06-20 | 2023-08-29 | 上海大学 | Mobile mechanical arm station planning method based on rigidity performance optimization |
| CN114800534B (en) * | 2022-06-29 | 2022-10-14 | 杭州三坛医疗科技有限公司 | Mechanical arm control method and device |
| CN115741672B (en) * | 2022-10-21 | 2024-04-19 | 杭州邦杰星医疗科技有限公司 | DH deducing method based on rigid body transformation |
| CN115982893B (en) * | 2023-03-20 | 2023-07-18 | 广东工业大学 | Multi-degree-of-freedom mechanism kinematics modeling method, device, equipment and storage medium |
| CN116408800B (en) * | 2023-03-27 | 2024-01-09 | 中铁隧道局集团有限公司 | Automatic positioning method for anchor rod trolley based on hole site coordinates |
| CN116494250B (en) * | 2023-06-26 | 2023-11-03 | 极限人工智能(北京)有限公司 | Mechanical arm control method, controller, medium and system based on speed compensation |
| CN117077384A (en) * | 2023-08-01 | 2023-11-17 | 中铁隧道局集团有限公司 | Inverse solution method for anchor rod pose at tail end of mechanical arm of anchor rod trolley |
| CN116673966B (en) * | 2023-08-02 | 2023-10-03 | 北京迁移科技有限公司 | Joint angle generation method for robot and robot system |
| CN116974257B (en) * | 2023-09-25 | 2023-12-26 | 浪潮云洲工业互联网有限公司 | Automatic filling control method, equipment and medium for oil filling riser based on inverse dynamics |
| CN117207200A (en) * | 2023-11-09 | 2023-12-12 | 湖南视比特机器人有限公司 | Method and device for generating working space of mechanical arm and computer equipment |
| CN117817654A (en) * | 2023-11-23 | 2024-04-05 | 佛山科学技术学院 | Motion planning method for heavy-duty robot |
| CN117290980B (en) * | 2023-11-27 | 2024-02-02 | 江西格如灵科技股份有限公司 | Mechanical arm simulation method and system based on Unity platform |
| CN117817668B (en) * | 2024-01-30 | 2024-06-14 | 哈尔滨工业大学 | Kinematic reconstruction method for single joint fault of SSRMS-configuration mechanical arm |
Citations (5)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US3920972A (en) * | 1974-07-16 | 1975-11-18 | Cincinnati Milacron Inc | Method and apparatus for programming a computer operated robot arm |
| CN102152307A (en) * | 2011-01-24 | 2011-08-17 | 西安交通大学 | Inclination-angle-constraint-based kinematic calibration method for Stewart parallel robot |
| CN104535027A (en) * | 2014-12-18 | 2015-04-22 | 南京航空航天大学 | Robot precision compensation method for variable-parameter error recognition |
| CN107589934A (en) * | 2017-07-24 | 2018-01-16 | 大连理工大学 | A kind of acquiring method of articulated manipulator inverse kinematics parsing solution |
| CN108656116A (en) * | 2018-05-18 | 2018-10-16 | 南京邮电大学 | Serial manipulator kinematic calibration method based on dimensionality reduction MCPC models |
-
2019
- 2019-04-09 CN CN201910278057.0A patent/CN109895101B/en active Active
Patent Citations (5)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US3920972A (en) * | 1974-07-16 | 1975-11-18 | Cincinnati Milacron Inc | Method and apparatus for programming a computer operated robot arm |
| CN102152307A (en) * | 2011-01-24 | 2011-08-17 | 西安交通大学 | Inclination-angle-constraint-based kinematic calibration method for Stewart parallel robot |
| CN104535027A (en) * | 2014-12-18 | 2015-04-22 | 南京航空航天大学 | Robot precision compensation method for variable-parameter error recognition |
| CN107589934A (en) * | 2017-07-24 | 2018-01-16 | 大连理工大学 | A kind of acquiring method of articulated manipulator inverse kinematics parsing solution |
| CN108656116A (en) * | 2018-05-18 | 2018-10-16 | 南京邮电大学 | Serial manipulator kinematic calibration method based on dimensionality reduction MCPC models |
Non-Patent Citations (3)
| Title |
|---|
| 一般6R机器人逆运动学算法的改进;房立金等;《机械科学与技术》;20180930;第37卷(第9期);第1325-1330页 * |
| 基于L_M方法的6R机器人轨迹规划快速逆解算法;邹丽梅等;《制造业自动化》;20161031;第38卷(第10期);第92-94、99页 * |
| 基于改进牛顿迭代法的手腕偏置型六自由度关节机器人逆解算法;韩磊等;《机械传动》;20170131;第41卷(第1期);第127-130、150页 * |
Also Published As
| Publication number | Publication date |
|---|---|
| CN109895101A (en) | 2019-06-18 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN109895101B (en) | Unique solution method for inverse kinematics numerical value of joint type mechanical arm | |
| CN107589934B (en) | Solving method for inverse kinematics analytic solution of joint type mechanical arm | |
| CN110757454B (en) | Path planning method and device for cooperative rotation of double robots | |
| Gong et al. | Analytical inverse kinematics and self-motion application for 7-DOF redundant manipulator | |
| CN111300420B (en) | Method for solving minimum path of joint space corner of mechanical arm | |
| CN108908347B (en) | Fault-tolerant repetitive motion planning method for redundant mobile mechanical arm | |
| CN107160401B (en) | Method for solving problem of joint angle deviation of redundant manipulator | |
| CN113715016B (en) | Robot grabbing method, system, device and medium based on 3D vision | |
| CN107791248B (en) | Control method of six-degree-of-freedom series robot based on criterion of not meeting Pieper | |
| CN113043286B (en) | Multi-degree-of-freedom mechanical arm real-time obstacle avoidance path planning system and method | |
| CN111496783B (en) | Inverse kinematics solving method for 6R industrial robot | |
| CN111958602B (en) | Real-time inverse solution method for wrist offset type 6-axis robot | |
| CN108527368B (en) | Method for determining optimal initial pose of flexible support series industrial robot operation | |
| CN111791234A (en) | Anti-collision control algorithm for working positions of multiple robots in narrow space | |
| CN113878571B (en) | Configuration optimization comprehensive method for seven-degree-of-freedom cooperative robot | |
| CN110722562A (en) | Space Jacobian matrix construction method for machine ginseng number identification | |
| CN109366486B (en) | Flexible robot inverse kinematics solving method, system, equipment and storage medium | |
| Li et al. | Solving inverse kinematics model for 7-DoF robot arms based on space vector | |
| CN113580135B (en) | Real-time inverse solution method for seven-axis robot with offset | |
| CN116330267A (en) | Control method based on industrial robot wrist singular point calculation | |
| Valsamos et al. | Introduction of the high performance area measure for the evaluation of metamorphic manipulator anatomies | |
| CN111482968A (en) | Six-degree-of-freedom offset robot inverse solution method based on BFS algorithm | |
| Zhang et al. | Kinematic singularity analysis and simulation for 7dof anthropomorphic manipulator | |
| Kang et al. | Coordinated workspace analysis and trajectory planning of redundant dual-arm robot | |
| Liu et al. | Inverse kinematics solution of manipulator based on IPSO-BPNN |
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 |