CN103942427B - Quick and simple method for solving inverse kinematics of six-degree-of-freedom mechanical arm - Google Patents

Abstract

The invention relates to the field of inverse kinematics of mechanical arms, in particular to a quick and simple method for solving inverse kinematics of a six-degree-of-freedom mechanical arm. A coordinate of an intersection of three rear joint shafts is determined according to the position and a posture matrix of the tail end of the mechanical arm; angles which three front joint shafts rotate by are solved in a geometrical method; the angles which the three rear joint shafts rotate by are solved according to an Euler angle transformation matrix. The concept put forward in the method can be applied to six-degree-of-freedom mechanical arms which have different geometrical structures but belong to the same type. According to the method, the solving process of the inverse kinematics of the mechanical arm is greatly simplified, the solving speed of the inverse kinematics is improved, and requirements for quickness and accuracy in the real-time control of the industrial mechanism arm can be met.

Description

One the fast and convenient of class sixdegree-of-freedom simulation Inverse Kinematics Solution seeks method

Technical field

The invention belongs to mechanical arm inverse kinematics field is and in particular to an a kind of class sixdegree-of-freedom simulation Inverse Kinematics Solution Angle estimating method.

Background technology

Mechanical arm inverse kinematics is the position of known mechanical arm end and attitude to calculate the angle value that each joint turns over, It is the inverse process of direct kinematics problem, direct kinematics problem is relatively easy and solves unique, and the solution of inverse kinematics is then Relative complex in fact it could happen that the situation of many solutions or no solution.Generally, the non-zero parameter of robot linkage is more, reaches a certain spy The mode setting the goal is also more, and these link parameters depend on the geometry of mechanical arm, and the structure of therefore mechanical arm is more complicated, The solution of inverse kinematics is then more complicated, for the sixdegree-of-freedom simulation of all rotary joint, at most may The situation of 16 kinds of solutions occurs.For the situation of many solutions, the method for solving of mechanical arm inverse kinematics is divided into two big class：Closing solution and Numerical solution.Numerical solution iterative nature makes solving speed significantly slow down, and this is unfavorable for the real-time control of modern industry mechanical arm System, and close Xie Zeshi it is desirable that obtaining, but it is not that any sixdegree-of-freedom simulation all has closing solution, only full The sixdegree-of-freedom simulation of sufficient certain condition just has closing solution.

Pieper proposes a class and has 3 adjacent axles and intersects at a point the sixdegree-of-freedom simulation of feature, this kind of machine Tool arm meets the condition with closing solution, and method proposed by the present invention is exactly directed to such mechanical arm, and that is, 6 joints are rotation Turn joint and 3 axles intersect at a point below mechanical arm.Industrial machinery arm major part belongs to such mechanical arm, therefore for The research of this kind of mechanical arm inverse motion solution has a very big significance.Solution procedure proposed by the present invention inverse motion to such mechanical arm The solution learned has reference.Inverse motion solution ask for speed and the degree of accuracy can directly affect the real-time control of mechanical arm, right For the mechanical arm of complex task to be executed, inverse motion solution ask for speed and the degree of accuracy will directly determine that mechanical arm execution is multiple The ability of miscellaneous task.

Content of the invention

The purpose of the present invention is to propose to one kind is applied to geometry difference but belongs on a class sixdegree-of-freedom simulation The fast and convenient of class sixdegree-of-freedom simulation Inverse Kinematics Solution improving the solving speed of inverse motion solution seeks method.

The purpose of the present invention is realized by below scheme：

（1）Position (X, Y, Z) according to mechanical arm tail end and attitude matrixTo determine rear three joint shaft joinings O Coordinate (x, y, z), i.e. the position of mechanical wrist point：

Learn that rear three joint shaft joinings O are L with the distance of mechanical arm tail end by measurement, and the attitude matrix of endIt is known,

Attitude matrixIn the first column element o, r, u represent X-axis and the mechanical arm of mechanical arm tail end tool coordinates system successively The X-axis of basis coordinates system, Y-axis, Z axis press from both sides cosine of an angle, the second column element p, and s, v represent mechanical arm tail end tool coordinates system successively Y-axis and mechanical arm basis coordinates system X-axis, Y-axis, Z axis press from both sides cosine of an angle, the 3rd column element q, and t, w represent mechanical arm tail end work successively The Y-axis of tool coordinate system and mechanical arm basis coordinates system X-axis, Y-axis, Z axis press from both sides cosine of an angle, wherein,

X=X+L q

Y=Y+L t,

Z=Z+L w

Determine the coordinate (x, y, z) of mechanical wrist point O；

（2）Solve the angle, θ that first three joint shaft turns over using geometric method_i, i=1,2,3：

Set up coordinate system on six joint shafts, be followed successively by 1～No. 6 coordinate system, several by class sixdegree-of-freedom simulation What construction solves the angle, θ that first three joint shaft turns over_i(i=1,2,3)：

l₃cos(θ₂+θ₃)-l₂sin(θ₂+θ₃)-l₁sinθ₂=x/cos θ₁-d₁,

l₃sin(θ₂+θ₃)+l₂cos(θ₂+θ₃)+l₁cosθ₂=z-d₂,

tanθ₁=y/x,

Wherein l₁Represent the distance between No. 2 coordinate origins and No. 3 coordinate origins, l₂Represent No. 3 coordinate origins with The distance between No. 4 coordinate origins, l₃Represent the distance between No. 4 coordinate systems and mechanical wrist point O, d₁Represent No. 1 coordinate It is the distance between z-axis and basis coordinates system z-axis, d₂Represent No. 1 coordinate origin with basis coordinates system initial point in basis coordinates system z-axis side Distance upwards；Angle and the attitude matrix of mechanical arm tail end that first three joint shaft according to having tried to achieve turns overObtain The X-Y-Z Eulerian angles transformation matrix of mechanical arm；

（3）The angle, θ that rear three joint shafts turn over is tried to achieve by Eulerian angles transformation matrix_i(i=4,5,6)：

Obtained by the transformation relation of spin matrix：

Wherein,For No. 6 coordinate systems with respect to basis coordinates system spin matrix,By first three joint of mechanical arm Angle is determined,It is tied to the conversion of No. 6 coordinate systems for No. 4 coordinates, rear three joint rotation angles are all 0, Transformation matrix for X-Y-Z Eulerian angles：

WhereinFor a constant matrix, the angle, θ being turned over by first three joint shaft tried to achieve_i(i =1,2,3) andCan determine that

Attitude matrix according to endWithCalculate

X-Y-Z Eulerian angles transformation matrix is：

As

If cos is θ₅≠ 0,

θ₄=tan^-1((-r₂₃/cosθ₅)/(r₃₃/cosθ₅)),

θ₆=tan^-1((-r₁₂/cosθ₅)/(r₁₁/cosθ₅)),

The angle, θ that after finally obtaining mechanical arm, three joint shafts turn over_i(i=4,5,6).

The beneficial effects of the present invention is：The thinking that the method proposes may apply to geometry difference but belongs to this On the sixdegree-of-freedom simulation of class.The method enormously simplify the solution procedure of mechanical arm inverse motion solution, improves inverse motion solution Solving speed, disclosure satisfy that the requirement for rapidity and accuracy in industrial machinery arm real-time control.

Brief description

Fig. 1 is the schematic diagram of the coordinate system set up on each joint shaft of such sixdegree-of-freedom simulation；

Fig. 2 is the solution flow chart of the Inverse Kinematics Solution with regard to the class sixdegree-of-freedom simulation mentioned in the present invention.

Specific embodiment

Below in conjunction with accompanying drawing citing, the present invention is described in more detail：

Position according to mechanical arm tail end and attitude matrix seek out three joint shaft joinings after mechanical arm, i.e. mechanical arm The position of wrist, the position coordinates according to mechanical wrist and the geometry of mechanical arm are obtained first three joint shaft of mechanical arm and are turned The angle crossed, the attitude matrix further according to first three joint angle tried to achieve and mechanical arm tail end obtains the X-Y-Z Euler of mechanical arm Angular transformation matrix, obtains rear three joint angles by this matrix.

The solution throughway that the method proposes may apply to geometry difference but belongs to such six degree of freedom machinery On arm.The method enormously simplify the solution procedure of mechanical arm inverse motion solution, improves the solving speed of inverse motion solution, Neng Gouman For the requirement of rapidity and accuracy in sufficient industrial machinery arm real-time control.

Other invention great majority for this problem are all implemented separately using geometric method or algebraic approach, and the present invention Geometric method and Eulerian angles converter technique are combined in the solution being applied to mechanical arm inverse motion solution by the method proposing.In theory this The method of bright proposition does not have error, and this ensure that the solving precision of such sixdegree-of-freedom simulation Inverse Kinematics Solution, and And the solution procedure of this invention proposition is simply many compared to simple application geometric method or algebraic approach, this ensure that such The solving speed of sixdegree-of-freedom simulation Inverse Kinematics Solution.Small part invention is had to propose geometric method and algebraic approach use in conjunction Solve mechanical arm inverse motion solution, but with the present invention in propose solution procedure and differ, in the present invention proposition solution Journey is easier than the solution procedure proposing in other similar inventions, and the scope of application is more wide.

Understand the co-ordinate system location of each joint shaft on sixdegree-of-freedom simulation according to Fig. 1, this algorithm is first according to machinery The position (X, Y, Z) of arm end and attitude matrixObtain the coordinate of the wrist point O of mechanical arm, that is, in Fig. 1, No. 5 coordinate systems are former The coordinate (x, y, z) of point, as shown in figure 1, the coordinate of point O has following relation with the position (X, Y, Z) of mechanical arm tail end：

X=X+L q

Y=Y+L t

Z=Z+L w

L is point O and the distance of No. 6 coordinate origins, and q, t, w correspond to attitude matrixTertial element.

Coordinate and the machinery of point O after the coordinate (x, y, z) of point O determines, can be obtained according to the geometry of mechanical arm Arm first three joint angle θ_iThe relation of (i=1,2,3), the relation between them can use equation below to represent：

l₃cos(θ₂+θ₃)-l₂sin(θ₂+θ₃)-l₁sinθ₂=x/cos θ₁-d₁

l₃sin(θ₂+θ₃)+l₂cos(θ₂+θ₃)+l₁cosθ₂=z-d₂

tanθ₁=y/x

Wherein d₁Represent the distance between No. 1 coordinate system z-axis and basis coordinates system z-axis, d₂Represent No. 1 coordinate origin and base Distance on basis coordinates system z-axis direction for the coordinate origin, these distances can be learnt by measurement, l₁, l₂, l₃Equal in FIG There is expression, thus the angle, θ that first three joint shaft of mechanical arm turns over can be tried to achieve by above equation group_i(i=1,2,3).

Can be obtained by the transformation relation of spin matrix：

Wherein,For No. 6 coordinate systems with respect to basis coordinates system spin matrix,By first three joint of mechanical arm Angle is determined,It is tied to the conversion of No. 6 coordinate systems for No. 4 coordinates, the premise of this conversion is rear three joints Corner is all 0 °,For X-Y-Z Eulerian angles transformation matrix.Can be obtained by above-mentioned transformation relation：

WhereinFor a constant matrix, relative with No. 6 coordinate systems depending on No. 4 coordinate systems in Fig. 1 Rotation relationship.Can be obtained by Fig. 1：

The angle, θ being turned over by first three joint shaft tried to achieve_i(i=1,2,3) andCan determine that

According to known terminal angle matrixWithCan calculateAndBe by Three joint angles θ afterwards_i(i=4,5,6) determine.This matrix is X-Y-Z Eulerian angles transformation matrix, and this matrix is No. 6 coordinates System gets through three rotations, correspond to the rotation of 4,5, No. 6 corresponding joint shafts of coordinate system respectively, the rotation of these three axles can To regard the rotation around the x-axis, y-axis and z-axis of No. 6 coordinate systems successively as, X-Y-Z Eulerian angles transformation matrix is：

As

If cos is θ₅≠ 0, can get

θ₄=tan^-1((-r₂₃/cosθ₅)/(r₃₃/cosθ₅))

θ₆=tan^-1((-r₁₂/cosθ₅)/(r₁₁/cosθ₅))

The angle, θ that three joint shafts turn over after said process can obtain mechanical arm_i(i=4,5,6).

The present invention finally gives the solution flow process of such sixdegree-of-freedom simulation Inverse Kinematics Solution, as shown in Figure 2.

Claims (1)

1. a kind of sixdegree-of-freedom simulation Inverse Kinematics Solution angle estimating method it is characterised in that：

(1) position according to mechanical arm tail end (X, Y, Z) and attitude matrixTo determine the coordinate of rear three joint shaft joinings O (x, y, z), i.e. the position of mechanical wrist point：

Learn that rear three joint shaft joinings O are L with the distance of mechanical arm tail end by measurement, and the attitude matrix of endIt is It is known,

$R_E^0=\left[\begin{array}{ccc}o& p& q\\ r& s& t\\ u& v& w\end{array}\right]$

Attitude matrixIn the first column element o, r, u represent the X-axis of mechanical arm tail end tool coordinates system and mechanical arm base successively The X-axis of mark system, Y-axis, Z axis press from both sides cosine of an angle, the second column element p, and s, v represent the Y-axis of mechanical arm tail end tool coordinates system successively With mechanical arm basis coordinates system X-axis, Y-axis, Z axis press from both sides cosine of an angle, the 3rd column element q, and t, w represent mechanical arm tail end instrument successively and sit The Y-axis of mark system and mechanical arm basis coordinates system X-axis, Y-axis, Z axis press from both sides cosine of an angle, wherein,

$\begin{array}{c}x=X+L\·q\\ y=Y+L\·t\\ z=Z+L\·w\end{array},$

Determine the coordinate (x, y, z) of mechanical wrist point O；

(2) solve, using geometric method, the angle, θ that first three joint shaft turns over_i, i=1,2,3：

Set up coordinate system on six joint shafts, be followed successively by 1～No. 6 coordinate system, by the geometry structure of class sixdegree-of-freedom simulation Make and solve the angle, θ that first three joint shaft turns over_i, i=1,2,3：

l₃cos(θ₂+θ₃)-l₂sin(θ₂+θ₃)-l₁sinθ₂=x/cos θ₁-d₁,

l₃sin(θ₂+θ₃)+l₂cos(θ₂+θ₃)+l₁cosθ₂=z-d₂,

tanθ₁=y/x,

Wherein l₁Represent the distance between No. 2 coordinate origins and No. 3 coordinate origins, l₂Represent No. 3 coordinate origins and No. 4 The distance between coordinate origin, l₃Represent the distance between No. 4 coordinate systems and mechanical wrist point O, d₁Represent No. 1 coordinate system z The distance between axle and basis coordinates system z-axis, d₂Represent No. 1 coordinate origin with basis coordinates system initial point in basis coordinates system z-axis direction On distance；Angle and the attitude matrix of mechanical arm tail end that first three joint shaft according to having tried to achieve turns overObtain machine The X-Y-Z Eulerian angles transformation matrix of tool arm；

(3) angle, θ that rear three joint shafts turn over is tried to achieve by Eulerian angles transformation matrix_i, i=4,5,6：

Obtained by the transformation relation of spin matrix：

$R_E^0=R_4^0{|}_{{\mathrm{\θ}}_4=0}\·R_6^4{|}_{{\mathrm{\θ}}_4={\mathrm{\θ}}_5={\mathrm{\θ}}_6=0}\·{R_E^6}_{xyz},$

Wherein,Determined by first three joint angles of mechanical arm；It is tied to No. 6 coordinates for No. 4 coordinates The conversion of system, rear three joint rotation angles are all 0,Transformation matrix for X-Y-Z Eulerian angles：

$R_{\mathrm{xyz}}^E{}_6=R_6^4{|}_{{\mathrm{\θ}}_4={\mathrm{\θ}}_5={\mathrm{\θ}}_6=0}^{-1}\·R_4^0{|}_{{\mathrm{\θ}}_4=0}^{-1}\·R_E^0$

WhereinFor a constant matrix, the angle, θ being turned over by first three joint shaft tried to achieve_i, i=1, 2,3 HesCan determine that

According toWithCalculate

X-Y-Z Eulerian angles transformation matrix is：

$R_{XYZ}\left({\mathrm{\θ}}_4,{\mathrm{\θ}}_5,{\mathrm{\θ}}_6\right)=\left[\begin{array}{ccc}{\mathrm{cos\θ}}_5{\mathrm{cos\θ}}_6& -{\mathrm{cos\θ}}_5{\mathrm{sin\θ}}_6& {\mathrm{sin\θ}}_5\\ {\mathrm{sin\θ}}_4{\mathrm{sin\θ}}_5{\mathrm{cos\θ}}_6+{\mathrm{cos\θ}}_4{\mathrm{sin\θ}}_6& -{\mathrm{sin\θ}}_4{\mathrm{sin\θ}}_5{\mathrm{sin\θ}}_6+{\mathrm{cos\θ}}_4{\mathrm{cos\θ}}_6& -{\mathrm{sin\θ}}_4{\mathrm{cos\θ}}_5\\ -{\mathrm{cos\θ}}_4{\mathrm{sin\θ}}_5{\mathrm{cos\θ}}_6+{\mathrm{sin\θ}}_4{\mathrm{sin\θ}}_6& {\mathrm{cos\θ}}_4{\mathrm{sin\θ}}_5{\mathrm{sin\θ}}_6+{\mathrm{sin\θ}}_4{\mathrm{cos\θ}}_6& {\mathrm{cos\θ}}_4{\mathrm{cos\θ}}_5\end{array}\right],$

As

If cos is θ₅≠ 0,

${\mathrm{\θ}}_5={\tan }^{-1}(r_{13}/\sqrt{r_{11}^2+r_{12}^2}),$

θ₄=tan^-1((-r₂₃/cosθ₅)/(r₃₃/cosθ₅)),

θ₆=tan^-1((-r₁₂/cosθ₅)/(r₁₁/cosθ₅)),

The angle, θ that after finally obtaining mechanical arm, three joint shafts turn over_i, i=4,5,6.