CN103942427A - 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 class six degree of freedom manipulator motion is learned the contrary fast and convenient method of asking of separating

Technical field

The invention belongs to mechanical arm inverse kinematics field, be specifically related to an a kind of class six degree of freedom manipulator motion and learn the contrary angle estimating method of separating.

Background technology

Mechanical arm inverse kinematics is that position and the attitude of known mechanical arm end calculated the angle value that each joint turns over, it is the anti-process of direct kinematics problem, direct kinematics problem is relatively simple and separate unique, inverse kinematics solve relative complex, may there is separate or without the situation of separating more.Conventionally, the non-zero parameter of mechanical arm connecting rod is more, the mode that reaches a certain specific objective is also more, these link parameters depend on the geometry of mechanical arm, therefore the structure of mechanical arm is more complicated, solving of inverse kinematics is more complicated, for one be all the six degree of freedom mechanical arm of rotary joint, may there is at most the situation of 16 kinds of solutions.For the situation of many solutions, the method for solving of mechanical arm inverse kinematics is divided into two large classes: sealing is separated and numerical solution.Numerical solution iterative nature makes to solve speed and greatly slows down, this is unfavorable for the real-time control of modern industry mechanical arm, sealing solution is that we wish to obtain, but be not that any six degree of freedom mechanical arm all has sealing solution, only have the six degree of freedom mechanical arm that meets certain condition just to there is sealing solution.

Pieper has proposed a class and has had the intersect at a point six degree of freedom mechanical arm of feature of 3 adjacent axles, this class mechanical arm meets the condition with sealing solution, and the method that the present invention proposes is just for this type of mechanical arm, 6 joints are rotary joint and 3 mechanical arms that axle intersects at a point below.Industrial machinery arm major part belongs to this type of mechanical arm, and the research that therefore contrary motion is separated for this class mechanical arm has a very big significance.The solution procedure that the present invention proposes has reference to solving of this type of mechanical arm inverse kinematics.The speed of asking for of the contrary solution of moving and accuracy meeting directly affect the real-time control of mechanical arm, and for the mechanical arm that will carry out complex task, the speed of asking for that contrary motion is separated and accuracy will directly determine the ability of mechanical arm execution complex task.

Summary of the invention

The object of the invention is to propose a kind of class six degree of freedom manipulator motion that is applied to geometry difference but belong to the speed that solves that on a class six degree of freedom mechanical arm, the contrary motion of raising is separated against the fast and convenient method of asking of separating.

The object of the invention is to realize by following scheme:

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

The distance of learning rear three joint shaft joining O and mechanical arm tail end by measurement is L, and the attitude matrix of end known,

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

Attitude matrix middle first row element o, r, u represents the X-axis of mechanical arm tail end tool coordinates system and the X-axis of mechanical arm basis coordinates system successively, Y-axis, Z axis folder cosine of an angle, secondary series element p, s, v represents that Y-axis and the mechanical arm basis coordinates of mechanical arm tail end tool coordinates system are X-axis successively, Y-axis, Z axis folder cosine of an angle, the 3rd column element q, t, w represents that Y-axis and the mechanical arm basis coordinates of mechanical arm tail end tool coordinates system are X-axis successively, Y-axis, Z axis folder 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 arm wrist point O;

(2) utilize geometric method to solve the angle θ that first three joint shaft turns over _i, i=1,2,3:

On six joint shafts, set up coordinate system, be followed successively by coordinate system 1～No. 6, solve by the geometrical construction of class six degree of freedom mechanical arm 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 the distance between No. 3 coordinate origins and No. 4 coordinate origins, l ₃represent the distance between No. 4 coordinate systems and mechanical arm wrist point O, d ₁represent that No. 1 coordinate system z axle and basis coordinates are the distance between z axle, d ₂represent that No. 1 coordinate origin and basis coordinates are that initial point is the distance on z direction of principal axis in basis coordinates; The angle turning over according to first three joint shaft of having tried to achieve and the attitude matrix of mechanical arm tail end obtain the X-Y-Z Eulerian angle transformation matrix of mechanical arm;

(3) try to achieve rear three angle θ that joint shaft turns over by Eulerian angle transformation matrix _i(i=4,5,6):

Transformation relation by rotation matrix obtains:

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

Wherein, be No. 6 coordinate systems rotation matrix with respect to basis coordinates system, determined by first three joint angles of mechanical arm, be the conversion that No. 4 coordinate is tied to No. 6 coordinate systems, rear three joint rotation angles are all 0, transformation matrix for X-Y-Z Eulerian angle:

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

Wherein be a constant matrix, the angle θ being turned over by first three joint shaft of having tried to achieve _i(i=1,2,3) and $R_6^4{|}_{{\mathrm{\θ}}_4={\mathrm{\θ}}_5={\mathrm{\θ}}_6=0}^{-1}$ Can determine $R_6^4{|}_{{\mathrm{\θ}}_4={\mathrm{\θ}}_5={\mathrm{\θ}}_6=0}^{-1}\·R{|}_{{\mathrm{\θ}}_4=0}^{-1}_4^0,$

According to the attitude matrix of end with $R_6^4{|}_{{\mathrm{\θ}}_4={\mathrm{\θ}}_5={\mathrm{\θ}}_6=0}^{-1}\·R{|}_{{\mathrm{\θ}}_4=0}^{-1}_4^0$ Calculate

X-Y-Z Eulerian angle transformation matrix is:

$R_{\mathrm{XYZ}}({\mathrm{\θ}}_4,{\mathrm{\θ}}_5,{\mathrm{\θ}}_6)=\left[\begin{array}{ccc}c{\mathrm{\θ}}_{5}c{\mathrm{\θ}}_6& -c{\mathrm{\θ}}_{5}s{\mathrm{\θ}}_6& s{\mathrm{\θ}}_5\\ s{\mathrm{\θ}}_{4}s{\mathrm{\θ}}_{5}c{\mathrm{\θ}}_6+c{\mathrm{\θ}}_{4}s{\mathrm{\θ}}_6& -s{\mathrm{\θ}}_{4}s{\mathrm{\θ}}_{5}s{\mathrm{\θ}}_6+c{\mathrm{\θ}}_{4}c{\mathrm{\θ}}_6& -s{\mathrm{\θ}}_{4}c{\mathrm{\θ}}_5\\ -c{\mathrm{\θ}}_{4}s{\mathrm{\θ}}_{5}c{\mathrm{\θ}}_6+s{\mathrm{\θ}}_{4}s{\mathrm{\θ}}_6& c{\mathrm{\θ}}_{4}s{\mathrm{\θ}}_{5}s{\mathrm{\θ}}_6+s{\mathrm{\θ}}_{4}c{\mathrm{\θ}}_6& c{\mathrm{\θ}}_{4}c{\mathrm{\θ}}_5\end{array}\right],$

As $R_{\mathrm{XYZ}}({\mathrm{\θ}}_4,{\mathrm{\θ}}_5,{\mathrm{\θ}}_6)=\left[\begin{array}{ccc}{r}_{11}& r_{12}& r_{13}\\ r_{21}& r_{22}& r_{23}\\ r_{31}& r_{32}& r_{33}\end{array}\right],$

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θ ₅))，

Finally obtain three angle θ that joint shaft turns over after mechanical arm _i(i=4,5,6).

Beneficial effect of the present invention is: the thinking that the method proposes can be applied to geometry difference but belong on this type of six degree of freedom mechanical arm.The method has been simplified the solution procedure that the contrary motion of mechanical arm is separated greatly, has improved the speed that solves of contrary motion solution, can meet the requirement for rapidity and accuracy in the control in real time of industrial machinery arm.

Brief description of the drawings

Fig. 1 is the schematic diagram of the coordinate system set up on this type of each joint shaft of six degree of freedom mechanical arm;

Fig. 2 is the process flow diagram that solves about the Inverse Kinematics Solution of a class six degree of freedom mechanical arm of mentioning in the present invention.

Embodiment

For example the present invention is described in more detail below in conjunction with accompanying drawing:

Seek out three joint shaft joinings after mechanical arm according to the position of mechanical arm tail end and attitude matrix, it is the position of mechanical arm wrist, obtain according to the geometry of the position coordinates of mechanical arm wrist and mechanical arm the angle that first three joint shaft of mechanical arm turns over, obtain again the X-Y-Z Eulerian angle transformation matrix of mechanical arm according to the attitude matrix of first three joint angle of trying to achieve and mechanical arm tail end, obtain rear three joint angles by this matrix.

The thinking that solves of the method proposition can be applied to geometry difference but belong on this type of six degree of freedom mechanical arm.The method has been simplified the solution procedure that the contrary motion of mechanical arm is separated greatly, has improved the speed that solves of contrary motion solution, can meet the requirement for rapidity and accuracy in the control in real time of industrial machinery arm.

All utilize geometric method or algebraic approachs to realize separately for other invention great majority of this problem, and the method that the present invention proposes combine geometric method and Eulerian angle converter technique to be applied in solving that the contrary motion of mechanical arm separates.The method that the present invention proposes does not in theory have error, this can ensure the contrary solving precision of separating of such six degree of freedom manipulator motion, and the solution procedure that this invention proposes is simply many than simple applicating geometric method or algebraic approach, this can ensure the contrary speed that solves of separating of such six degree of freedom manipulator motion.There is small part invention to propose geometric method and algebraic approach use in conjunction to solve separating against motion of mechanical arm, but it is not identical with the solution procedure proposing in the present invention, the solution procedure proposing in the present invention is easier than the solution procedure proposing in other similar inventions, and the scope of application is more wide.

According to the coordinate system position of each joint shaft on the known six degree of freedom mechanical arm of Fig. 1, this algorithm is first according to the position of mechanical arm tail end (X, Y, Z) and attitude matrix obtain the coordinate of the wrist point O of mechanical arm, i.e. the coordinate of No. 5 coordinate origins (x, y, z) in Fig. 1, as shown in Figure 1, the some coordinate of O and the position of mechanical arm tail end (X, Y, Z) have following relation:

x＝X+L·q

y＝Y+L·t

z＝Z+L·w

L is the distance of an O and No. 6 coordinate origins, q, and t, w correspondence attitude matrix tertial element.

After the coordinate (x, y, z) of some O is determined, can obtain according to the geometry of mechanical arm coordinate and first three joint angle θ of mechanical arm of an O _ithe relation of (i=1,2,3), the available following the Representation Equation of relation between them:

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

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

tanθ ₁＝y/x

Wherein d ₁represent that No. 1 coordinate system z axle and basis coordinates are the distance between z axle, d ₂represent that No. 1 coordinate origin and basis coordinates are that initial point is the distance on z direction of principal axis in basis coordinates, these distances can be learnt by measurement, l ₁, l ₂, l ₃in Fig. 1, all there is expression, can try to achieve the angle θ that first three joint shaft of mechanical arm turns over by above system of equations thus _i(i=1,2,3).

Transformation relation by rotation matrix can obtain:

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

Wherein, be No. 6 coordinate systems rotation matrix with respect to basis coordinates system, determined by first three joint angles of mechanical arm, be the conversion that No. 4 coordinate is tied to No. 6 coordinate systems, the prerequisite of this conversion is that rear three joint rotation angles are all 0 °, for X-Y-Z Eulerian angle transformation matrix.Can be obtained by above-mentioned transformation relation:

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

Wherein be a constant matrix, depend in Fig. 1 the relative rotation relationship of No. 4 coordinate systems and No. 6 coordinate systems.Can be obtained by Fig. 1:

$R_6^4{|}_{{\mathrm{\θ}}_4={\mathrm{\θ}}_5={\mathrm{\θ}}_6=0}^{-1}=\left[\begin{array}{ccc}0& 0& 1\\ 0& -1& 0\\ 1& 0& 0\end{array}\right]$

The angle θ being turned over by first three joint shaft of having tried to achieve _i(i=1,2,3) and can determine $R_6^4{|}_{{\mathrm{\θ}}_4={\mathrm{\θ}}_5={\mathrm{\θ}}_6=0}^{-1}\·R{|}_{{\mathrm{\θ}}_4=0}^{-1}_4^0.$

$R_6^4{|}_{{\mathrm{\θ}}_4={\mathrm{\θ}}_5={\mathrm{\θ}}_6=0}^{-1}\·R_4^0{|}_{{\mathrm{\θ}}_4=0}^1=\left[\begin{array}{ccc}\cos \left({\mathrm{\θ}}_1\right)\·\cos ({\mathrm{\θ}}_2+{\mathrm{\θ}}_3)& \sin \left({\mathrm{\θ}}_1\right)\·\cos ({\mathrm{\θ}}_2+{\mathrm{\θ}}_3)& \sin ({\mathrm{\θ}}_2+{\mathrm{\θ}}_3)\\ -\sin {\mathrm{\θ}}_1& \cos {\mathrm{\θ}}_1& 0\\ -\cos \left({\mathrm{\θ}}_1\right)\·\sin ({\mathrm{\θ}}_2+{\mathrm{\θ}}_3)& -\sin \left({\mathrm{\θ}}_1\right)\·\sin ({\mathrm{\θ}}_2+{\mathrm{\θ}}_3)& \cos ({\mathrm{\θ}}_2+{\mathrm{\θ}}_3)\end{array}\right]$

According to known terminal angle matrix with can calculate and by rear three joint angles θ _i(i=4,5,6) determine.This matrix is X-Y-Z Eulerian angle transformation matrixs, this matrix is that No. 6 coordinate systems get through three rotations, the rotation of the joint shaft that respectively corresponding 4,5, No. 6 coordinate systems are corresponding, the rotation of these three axles can be regarded successively the rotation around the x-axis, y-axis and z-axis of No. 6 coordinate systems as, and X-Y-Z Eulerian angle transformation matrix is:

$R_{\mathrm{XYZ}}({\mathrm{\θ}}_4,{\mathrm{\θ}}_5,{\mathrm{\θ}}_6)=\left[\begin{array}{ccc}c{\mathrm{\θ}}_{5}c{\mathrm{\θ}}_6& -c{\mathrm{\θ}}_{5}s{\mathrm{\θ}}_6& s{\mathrm{\θ}}_5\\ s{\mathrm{\θ}}_{4}s{\mathrm{\θ}}_{5}c{\mathrm{\θ}}_6+c{\mathrm{\θ}}_{4}s{\mathrm{\θ}}_6& -s{\mathrm{\θ}}_{4}s{\mathrm{\θ}}_{5}s{\mathrm{\θ}}_6+c{\mathrm{\θ}}_{4}c{\mathrm{\θ}}_6& -s{\mathrm{\θ}}_{4}c{\mathrm{\θ}}_5\\ -c{\mathrm{\θ}}_{4}s{\mathrm{\θ}}_{5}c{\mathrm{\θ}}_6+s{\mathrm{\θ}}_{4}s{\mathrm{\θ}}_6& c{\mathrm{\θ}}_{4}s{\mathrm{\θ}}_{5}s{\mathrm{\θ}}_6+s{\mathrm{\θ}}_{4}c{\mathrm{\θ}}_6& c{\mathrm{\θ}}_{4}c{\mathrm{\θ}}_5\end{array}\right]$

As $R_{\mathrm{XYZ}}({\mathrm{\θ}}_4,{\mathrm{\θ}}_5,{\mathrm{\θ}}_6)=\left[\begin{array}{ccc}{r}_{11}& r_{12}& r_{13}\\ r_{21}& r_{22}& r_{23}\\ r_{31}& r_{32}& r_{33}\end{array}\right]$

If cos is θ ₅≠ 0, can obtain

${\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θ ₅))

Can obtain three angle θ that joint shaft turns over after mechanical arm by said process _i(i=4,5,6).

The present invention has finally provided this type of six degree of freedom manipulator motion and has learned against the flow process that solves of separating, as shown in Figure 2.

Claims (1)

1. a class six degree of freedom manipulator motion is learned the contrary angle estimating method of separating, and it is characterized in that:

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

The distance of learning rear three joint shaft joining O and mechanical arm tail end by measurement is L, and the attitude matrix of end known,

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

Attitude matrix middle first row element o, r, u represents the X-axis of mechanical arm tail end tool coordinates system and the X-axis of mechanical arm basis coordinates system successively, Y-axis, Z axis folder cosine of an angle, secondary series element p, s, v represents that Y-axis and the mechanical arm basis coordinates of mechanical arm tail end tool coordinates system are X-axis successively, Y-axis, Z axis folder cosine of an angle, the 3rd column element q, t, w represents that Y-axis and the mechanical arm basis coordinates of mechanical arm tail end tool coordinates system are X-axis successively, Y-axis, Z axis folder 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 arm wrist point O;

(2) utilize geometric method to solve the angle θ that first three joint shaft turns over _i, i=1,2,3:

On six joint shafts, set up coordinate system, be followed successively by coordinate system 1～No. 6, solve by the geometrical construction of class six degree of freedom mechanical arm 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 the distance between No. 3 coordinate origins and No. 4 coordinate origins, l ₃represent the distance between No. 4 coordinate systems and mechanical arm wrist point O, d ₁represent that No. 1 coordinate system z axle and basis coordinates are the distance between z axle, d ₂represent that No. 1 coordinate origin and basis coordinates are that initial point is the distance on z direction of principal axis in basis coordinates; The angle turning over according to first three joint shaft of having tried to achieve and the attitude matrix of mechanical arm tail end , obtain the X-Y-Z Eulerian angle transformation matrix of mechanical arm;

(3) try to achieve rear three angle θ that joint shaft turns over by Eulerian angle transformation matrix _i(i=4,5,6):

Transformation relation by rotation matrix obtains:

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

Wherein, be No. 6 coordinate systems rotation matrix with respect to basis coordinates system, determined by first three joint angles of mechanical arm, be the conversion that No. 4 coordinate is tied to No. 6 coordinate systems, rear three joint rotation angles are all 0, transformation matrix for X-Y-Z Eulerian angle:

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

Wherein be a constant matrix, the angle θ being turned over by first three joint shaft of having tried to achieve _i(i=1,2,3) and $R_6^4{|}_{{\mathrm{\θ}}_4={\mathrm{\θ}}_5={\mathrm{\θ}}_6=0}^{-1}$ Can determine $R_6^4{|}_{{\mathrm{\θ}}_4={\mathrm{\θ}}_5={\mathrm{\θ}}_6=0}^{-1}\·R{|}_{{\mathrm{\θ}}_4=0}^{-1}_4^0,$

According to the attitude matrix of end with $R_6^4{|}_{{\mathrm{\θ}}_4={\mathrm{\θ}}_5={\mathrm{\θ}}_6=0}^{-1}\·R{|}_{{\mathrm{\θ}}_4=0}^{-1}_4^0$ Calculate

X-Y-Z Eulerian angle transformation matrix is:

$R_{\mathrm{XYZ}}({\mathrm{\θ}}_4,{\mathrm{\θ}}_5,{\mathrm{\θ}}_6)=\left[\begin{array}{ccc}c{\mathrm{\θ}}_{5}c{\mathrm{\θ}}_6& -c{\mathrm{\θ}}_{5}s{\mathrm{\θ}}_6& s{\mathrm{\θ}}_5\\ s{\mathrm{\θ}}_{4}s{\mathrm{\θ}}_{5}c{\mathrm{\θ}}_6+c{\mathrm{\θ}}_{4}s{\mathrm{\θ}}_6& -s{\mathrm{\θ}}_{4}s{\mathrm{\θ}}_{5}s{\mathrm{\θ}}_6+c{\mathrm{\θ}}_{4}c{\mathrm{\θ}}_6& -s{\mathrm{\θ}}_{4}c{\mathrm{\θ}}_5\\ -c{\mathrm{\θ}}_{4}s{\mathrm{\θ}}_{5}c{\mathrm{\θ}}_6+s{\mathrm{\θ}}_{4}s{\mathrm{\θ}}_6& c{\mathrm{\θ}}_{4}s{\mathrm{\θ}}_{5}s{\mathrm{\θ}}_6+s{\mathrm{\θ}}_{4}c{\mathrm{\θ}}_6& c{\mathrm{\θ}}_{4}c{\mathrm{\θ}}_5\end{array}\right],$

As $R_{\mathrm{XYZ}}({\mathrm{\θ}}_4,{\mathrm{\θ}}_5,{\mathrm{\θ}}_6)=\left[\begin{array}{ccc}{r}_{11}& r_{12}& r_{13}\\ r_{21}& r_{22}& r_{23}\\ r_{31}& r_{32}& r_{33}\end{array}\right],$

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θ ₅))，

Finally obtain three angle θ that joint shaft turns over after mechanical arm _i(i=4,5,6).