A non-singular hand-eye calibration method for robots based on sequential rotation mechanism

Abstract

The present invention discloses a robot non-singular hand-eye calibration method based on a sequential rotation mechanism, which relates to the field of visual calibration technology and includes the following steps: collecting a measurement data set {A i ,B i}, and extract the angle value θ of the unknown hand-eye matrix rotation part X ; If θ X If θ is not equal to or far from π radians, the unknown hand-eye matrix is ​​solved according to the hand-eye calibration algorithm based on the modified Rodriguez parameter; if θ X Equal to or close to π radians, a non-singular hand-eye calibration method for robots based on a sequential rotation mechanism is used to obtain the rotation and translation values ​​of the unknown hand-eye matrix. This paper proposes a non-singular hand-eye calibration method based on a sequential rotation mechanism, which can be applied to the problem of solving unknown hand-eye matrices with angle values ​​of any value including π radians, improving the scope of application and safety of the analytical solution method for robot hand-eye calibration proposed by RYTsai.

Description

Technical Field

[0001] The present invention relates to the technical field of vision calibration, and in particular to a robot non-singularity hand-eye calibration method based on a sequential rotation mechanism. Background Art

[0002] For the robot hand-eye vision system, the coordinate relationship between the robot tool coordinate system (Hand) and the coordinate system of the camera it holds (Eye) is usually called the "hand-eye matrix". The process of solving this matrix is ​​called "hand-eye calibration". The accuracy of solving the unknown hand-eye matrix will directly affect the precision and safety of the robot's visual operation tasks.

[0003] In the 1980s, RYTsai et al. first proposed the concept of hand-eye calibration. Based on the modified Rodrigues parameter, they presented a classic analytical solution to the hand-eye calibration problem. This method is still widely used as a classic method in the field of robotic hand-eye calibration. However, the modified Rodrigues parameter itself has an insurmountable limitation: it can only be used to parameterize rotation matrices with angle values ​​not equal to π radians.

[0004] Therefore, when the angle value of the unknown hand-eye matrix rotation part is equal to π radians, the hand-eye matrix value estimated by the hand-eye calibration method based on this type of parameters will seriously deviate from its true value (here it is referred to as a singular phenomenon), thereby causing the failure of the robot hand-eye calibration task. In addition, under the influence of external measurement noise, the present invention finds that when the angle value of the unknown hand-eye matrix rotation part is closer to π radians, the hand-eye matrix estimated by the hand-eye calibration analytical solution proposed by RYTsai will also have a large error. That is, in this case, the hand-eye calibration method has poor anti-interference ability to noise. At the same time, the existing technology cannot perfectly solve the singular phenomenon inherent in the robot hand-eye calibration method proposed by RYTsai in the above two situations. Summary of the Invention

[0005] The purpose of the present invention is to address the deficiencies of the above-mentioned prior art and provide a robot non-singular hand-eye calibration method based on a sequential rotation mechanism, so as to solve the problem in the prior art that when the angle value of the rotation part of the unknown hand-eye matrix is ​​equal to π radians, the hand-eye matrix value estimated by the hand-eye calibration method based on this type of parameters will seriously deviate from its true value.

[0006] The present invention specifically provides the following technical solution: a robot non-singular hand-eye calibration method based on a sequential rotation mechanism, comprising the following steps:

[0007] Collect measurement data sets for hand-eye calibration {A i,B i}, and from the measurement data set {A i ,B i} Extract the rotation angle value θ of the unknown hand-eye matrix rotation part X , where A i With B i are the transition matrix and relative pose matrix respectively;

[0008] If θ X If the hand-eye matrix is ​​not equal to or far from π radians, the unknown hand-eye matrix is ​​solved according to the hand-eye calibration algorithm based on the modified Rodriguez parameter. Otherwise, three types of user matrices are defined according to the sequential rotation mechanism. j R user , j = 1, 2, 3;

[0009] Using Vector Groups j R user k Bi Replace the original vector group k Bi , solve for vector group k Ai Rotate to j R user k Bi The rotation matrix required for coincidence and extract the rotation matrix Corresponding angle value

[0010] From the said Choose an angle between 0 and 2π / 3 radians and write it as and record The corresponding rotation matrix and user matrix G R user ;

[0011] Using the and G R user Calculate the unknown hand-eye matrix estimate R X ,Right now

[0012] The hand-eye matrix estimate R X Substitute R Ai t X +t Ai =R X t Bi +t X In the example, get the unknown hand-eye matrix translation column vector t X The linear least squares estimate of Ai With R Bi A i With B iThe rotation matrix, t Ai , t Bi and t X The matrix A is i 、B i and R X The position column vector of any matrix X in .

[0013] Preferably, the measurement data set {A i ,B i}, including the following steps:

[0014] Control the robot's manipulator to perform two different movements and record the transition matrix A of the manipulator tool coordinate system between the two different movements i And the relative pose matrix B of the camera coordinate system held by the robotic arm between the two different movements i ;

[0015] Repeatedly obtain the transition matrix A i and the relative pose matrix B i The process of obtaining k groups of measurement data {A i ,B i}, and extract the corresponding rotation axis unit direction vector {k Ai ,k Bi} and the angle value {θ Ai ,θ Bi}.

[0016] Preferably, the measurement data set {A i ,B i} Extract the rotation angle value θ of the unknown hand-eye matrix rotation part X , including the steps of:

[0017] According to the corresponding relationship between the modified Rodriguez parameters and the rotation matrix, construct A in sequence i With B i The corresponding modified Rodriguez parameter P Ai With P Bi ;

[0018] Constructing overdetermined linear equations[P Ai +P Bi ] × P' X =P Ai -P Bi , use the linear least squares method to solve the vector P' X The value of

[0019] The vector P' corresponding to the unknown hand-eye matrix X Bring in P X =2P' X(1+||P' X || 2 ) -0.5 Inversely solve for P X The value of

[0020] According to the vector P X and θ X =2·atan||P X || 2 , we can get the unknown hand-eye matrix angle value θ X ;

[0021] Among them, the vector P X is the modified Rodriguez parameter corresponding to the matrix X; atan is the inverse tangent operator, and ||·|| is the modulo operator for vectors.

[0022] Preferably, the structure A i With B i The corresponding modified Rodriguez parameter P Ai With P Bi , the specific expression is:

[0023] P Ai =2sin(θ Ai / 2)k Ai

[0024] P Bi =2sin(θ Bi / 2)k Bi .

[0025] Preferably, solving the unknown hand-eye matrix according to the hand-eye calibration algorithm based on modified Rodriguez parameters specifically includes the following steps:

[0026] Given a rotation matrix R X , get the rotation matrix R X The corresponding modified Rodriguez parameter P X ;

[0027] Collect the homogeneous coordinate transformation matrix A between the robot tool coordinate system and the camera coordinate system between two adjacent movements i With B i , and extract A in sequence i With B i The modified Rodriguez parameter P corresponding to the rotation part Ai With P Bi ;

[0028] Obtain an overdetermined linear equation system composed of multiple measurement data [P Ai +P Bi ] × ·P' X =PAi -P Bi , and calculate P' in the overdetermined linear equations X ;

[0029] in,[·] × is the antisymmetric matrix corresponding to the vector, ||·|| is the modulus operator of the vector;

[0030] According to P X =2P' X (1+||P' X || 2 ) -0.5 , construct vector P' X The corresponding Rodriguez parameter P X , obtain P X The corresponding rotation matrix R X ;

[0031] R X Substitute into the equation R Ai t X +t Ai =R X t Bi +t X , get the hand-eye matrix translation column vector t X The linear least squares estimate of .

[0032] Preferably, the modified Rodriguez parameter P X The calculation expression is:

[0033] P X =2sin(θ X / 2)[k x k y k z ] T

[0034] Among them, θ X With the vector [k x k y k z ] T R X The angle value and the unit direction vector of the rotation axis.

[0035] Preferably, the vector P' X The calculation expression is:

[0036] P' X =P X / (2cos(θ X / 2))

[0037] P X =2sin(θX / 2)[k x k y k z ] T Substitute P' X =P X / (2cos(θ X / 2)) and calculate P' X =tan(θ X / 2)[k x k y k z ] T ;

[0038] Among them, θ X Refers to the rotation angle value of the unknown hand-eye matrix.

[0039] Preferably, the three types of user matrices are defined according to the sequential rotation mechanism: j R user , including the steps of:

[0040] According to the sequential rotation mechanism, the first type of user matrix is ​​defined: 1 R user =Rot(k x ,θ);

[0041] According to the sequential rotation mechanism, the second type of user matrix is ​​defined: 2 R user =Rot(k y ,θ);

[0042] According to the sequential rotation mechanism, the third type of user matrix is ​​defined: 3 R user =Rot(k z ,θ);

[0043] Among them, k x is the unit direction vector of the coordinate axis X; k y is the unit direction vector of the coordinate axis Y; k z is the unit direction vector of the coordinate axis Z; θ is any angle value between 0 and π radians; Rot(k x ,θ) is the rotation around k x The rotation matrix obtained by rotating θ radians; Rot(k y ,θ) is the rotation around k y The rotation matrix obtained by rotating θ radians; Rot(k z ,θ) is the rotation around k z The rotation matrix resulting from a rotation by θ radians.

[0044] Preferably, the three types of user matrices j R userIn , there exists a user matrix that satisfies the rotation angle value between 0 and 2π / 3 radians, which specifically includes the following steps:

[0045] Vector group k Ai Rotate to j R user k Bi The rotation matrix required for coincidence is but

[0046] Obtained from the properties of the rotation matrix, user-defined rotation matrix The traces are:

[0047]

[0048]

[0049]

[0050] Among them, r ij Represents R X The element in row i and column j, tr(·) is the trace of the rotation matrix;

[0051] According to R X The constraints between internal elements can be sorted out as follows:

[0052]

[0053] The three types of user matrices are obtained by the properties of the rotation matrix trace. At least one matrix has a non-negative trace, that is, there is at least one user matrix with a rotation angle between 0 and 2π / 3 radians.

[0054] Preferably, the vector group j R user k Bi Rotate to known measurement data a R Ai The axis of rotation a k Ai , which coincides with the required rotation matrix The rotation angle value and R X The corresponding rotation angle values ​​are different.

[0055] Compared with the prior art, the present invention has the following significant advantages:

[0056] The present invention proposes a non-singular hand-eye calibration method based on a sequential rotation mechanism. From a security perspective, three types of user matrices are constructed through the sequential rotation mechanism to obtain rotation matrices and angle values. An angle value and a rotation matrix between 0 and 2π / 3 radians are selected within the angle value, and the unknown hand-eye matrix estimation value is calculated using the rotation matrix and the user matrix. It can be seen that the present invention is suitable for solving the problem of unknown hand-eye matrices with angle values ​​of any value including π radians. It can effectively avoid modifying the singularity of the Rodriguez parameters themselves, and to a certain extent improves the scope of application and anti-interference ability of the robot hand-eye calibration analytical solution method proposed by RYTsai. BRIEF DESCRIPTION OF THE DRAWINGS

[0057] Figure 1 This is a flow chart of the robot non-singular hand-eye calibration method based on modifying Rodriguez parameters of the present invention;

[0058] Figure 2 This is a flow chart of the robot non-singular hand-eye calibration method based on the sequential rotation mechanism of the present invention. DETAILED DESCRIPTION

[0059] The following is a clear and complete description of the technical solutions of the embodiments of the present invention in conjunction with the accompanying drawings. Obviously, the described embodiments are part of the embodiments of the present invention, not all of them. Based on the embodiments of the present invention, all other embodiments obtained by ordinary technicians in this field without making creative efforts should fall within the scope of protection of the present invention.

[0060] For ease of understanding and explanation, see the attached Figure 1-2 As shown, the present invention provides a robot non-singular hand-eye calibration method based on a sequential rotation mechanism, comprising the following steps:

[0061] Step S1: Collecting measurement data set for hand-eye calibration {A i ,B i}, and from the measurement data set {A i ,B i} Extract the rotation angle value θ of the unknown hand-eye matrix rotation part X , where A i With B i are the transition matrix and relative pose matrix respectively.

[0062] Step S10: Within the camera's field of view, the robot's arm is controlled to perform two different movements, and the transition matrix A of the tool coordinate system of the robot arm between the two adjacent movements is recorded. i And the relative pose matrix B of the camera coordinate system held by the robotic arm between two adjacent movements i .

[0063] Step S11: Repeat step S10 k times to obtain k groups of measurement data {A i ,B i}, and extract the corresponding rotation axis unit direction vector {k Ai ,k Bi} and the angle value {θ Ai ,θ Bi}

[0064] Step S12: According to the corresponding relationship between the modified Rodriguez parameters and the rotation matrix, construct A in sequence i With B i The corresponding modified Rodriguez parameters: P Ai =2sin(θ Ai / 2)k Ai With P Bi =2sin(θ Bi / 2)k Bi .

[0065] Step S13: Construct an overdetermined linear equation system [P Ai +P Bi ] × P' X =P Ai -P Bi , use the linear least squares method to solve the vector P' X The value of .

[0066] Step S14: The vector P' corresponding to the unknown hand-eye matrix X Bring in P X =2P' X (1+||P' X || 2 ) -0.5 Inversely solve for P X The value of .

[0067] Step S15: According to vector P X and θ X =2·atan||P X || 2 , we can get the unknown hand-eye matrix angle value θ X .

[0068] Among them, atan is the inverse tangent operator, ||·|| is the modulus operator of the vector

[0069] Step S2: If θ X If it is not equal to or far away from π radians, the unknown hand-eye matrix is ​​solved according to the hand-eye calibration algorithm based on the modified Rodriguez parameter.

[0070] Step S20: Given the rotation matrix R X, get the rotation matrix R X The corresponding modified Rodriguez parameter P X .

[0071] P X =2sin(θ X / 2)[k x k y k z ] T

[0072] Among them, θ X With the vector [k x k y k z ] T R X The angle value and the unit direction vector of the rotation axis.

[0073] Step S21: Acquire the homogeneous coordinate transformation matrix A between the robot tool coordinate system and the camera coordinate system between two adjacent movements i With B i , according to the above method, and extract A i With B i The modified Rodriguez parameter P corresponding to the rotation part Ai With P Bi .

[0074] Step S22: Obtain an overdetermined linear equation system consisting of multiple measurement data [P Ai +P Bi ] × ·P' X =P Ai -P Bi , and calculate P' in the overdetermined linear equations X .in,[·] × is the antisymmetric matrix corresponding to the vector, and ‖·|| is the modulus operator of the vector.

[0075] Step S23: According to P X =2P' X (1+||P' X || 2 ) -0.5 , construct vector P' X The corresponding Rodriguez parameter P X , obtain P X The corresponding rotation matrix R X .

[0076] Step S24: R X Substitute into the equation R Ai t X +t Ai=R X t Bi +t X , get the hand-eye matrix translation column vector t X The linear least squares estimate of . Ai , R Bi and R X A i , B i and the rotation matrix of X, t Ai With t Bi and t X The matrix A is i With B i and the position column vector of X.

[0077] Considering P' X =P X / (2cos(θ X / 2)), where θ X Refers to the rotation angle value of the unknown hand-eye matrix, P X =2sin(θ X / 2)[k x k y k z ] T Substitute P' X =P X / (2cos(θ X / 2)) we can get P' X =tan(θ X / 2)[k x k y k z ] T Obviously, when θ X =π radians, tan(θ X / 2) will tend to infinity. And since [k x k y k z ] T is a unit vector, then the vector P' X The value of will also tend to infinity.

[0078] However, in the classic hand-eye calibration method proposed by RYTsai, solving the vector P' X It is one of the key steps. Obviously, when the rotation angle of the unknown hand-eye matrix is ​​equal to π radians, the vector P' X The value of cannot be accurately obtained by the hand-eye calibration algorithm proposed by RYTsai, which will cause the hand-eye matrix estimated by RYTsai to deviate seriously from its true value, that is, the hand-eye calibration singularity phenomenon will occur. The reason for this phenomenon is the singularity of modifying the Rodriguez parameter itself.

[0079] Based on the sequential rotation mechanism, a singularity-free robot hand-eye calibration method is proposed.

[0080] Step S3: If θ X Equal to or close to π radians, three types of user matrices are defined based on the sequential rotation mechanism j R user , j=1,2,3.

[0081] Step S31: According to the sequential rotation mechanism, define the first type of user matrix: 1 R user =Rot(k x ,θ).

[0082] Step S32: According to the sequential rotation mechanism, define the second type of user matrix: 2 R user =Rot(k y ,θ).

[0083] Step S33: Based on the sequential rotation mechanism, define the third type of user matrix: 3 R user =Rot(k z ,θ).

[0084] Among them, k x is the unit direction vector of the coordinate axis X; k y is the unit direction vector of the coordinate axis Y; k z is the unit direction vector of the coordinate axis Z; θ is any angle value between 0 and π radians; Rot(k x ,θ) is the rotation around k x The rotation matrix obtained by rotating θ radians; Rot(k y ,θ) is the rotation around k y The rotation matrix obtained by rotating θ radians; Rot(k z ,θ) is the rotation around k z The rotation matrix resulting from a rotation by θ radians.

[0085] A theorem is given: "In the above three types of user-defined matrices There exists at least one user matrix that satisfies the rotation angle value between 0 and 2π / 3 radians. The proof process is as follows:

[0086] If the vector group k Ai Rotate to j R user k Bi The rotation matrix required for coincidence is but

[0087] According to the properties of the rotation matrix, the user-defined rotation matrix The traces are:

[0088]

[0089]

[0090]

[0091] Among them, r ij Represents R X The element at row i and column j.

[0092] According to R X The constraints between the internal elements can be sorted out as follows:

[0093]

[0094] According to the properties of the rotation matrix trace, the trace of at least one of the three types of user matrices is non-negative, that is, there is at least one user matrix with a rotation angle value between 0 and 2π / 3 radians.

[0095] Step S4: According to the hand-eye calibration analytical solution method based on modifying the Rodriguez parameter proposed by RYTsai, the vector group j R user k Bi Replace the original vector group k Bi , solve for vector group k Ai Rotate to j R user k Bi The rotation matrix required for coincidence and extract the rotation matrix Corresponding angle value Among them, j=1,2,3.

[0096] Step S5: From Choose an angle between 0 and 2π / 3 radians and write it as and record The corresponding rotation matrix and user matrix G R user .

[0097] Step S6: Due to and G R user All known, using and G R user Calculate the unknown hand-eye matrix estimate R X ,Right now

[0098] Step S7: Estimation of the hand-eye matrix R X Substitute R Ai t X +t Ai =R X t Bi +t X In the example, get the unknown hand-eye matrix translation column vector t X The linear least squares estimate of .

[0099] It should be noted that the user can adjust the actual measured R Bi Axis of rotation a k Bi Perform a virtual rotation motion, and the corresponding rotation matrix is ​​recorded as R user , at this moment the vector group a k Bi will become R user k Bi .

[0100] Then the vector group R obtained by virtual rotation user k Bi Rotate to match known measurement data a R Ai The axis of rotation a k Ai The rotation matrix required for corresponding coincidence The rotation angle value may no longer be equal to R X The corresponding rotation angle value.

[0101] As long as the user matrix R user Choose appropriately, the measurement vector group {k Ai ,R user k Bi The rotation matrix angle value determined by} is no longer equal to π radians.

[0102] The above content is a further detailed description of the present invention in combination with specific preferred embodiments. For those skilled in the art to which the present invention belongs, several simple deductions or replacements can be made without departing from the concept of the present invention, which should be regarded as falling within the scope of protection of the present invention.

Claims

1. A robot non-singular hand-eye calibration method based on sequential rotation mechanism, characterized by: The following steps are involved: Collect measurement data sets for hand-eye calibration { , }, and from the measurement data set { , } Extract the angle value of the unknown hand-eye matrix rotation part ,in, and are the transition matrix and relative pose matrix respectively; like Not equal to or far from Radians, solve the unknown hand-eye matrix according to the hand-eye calibration algorithm based on the modified Rodriguez parameter, otherwise define three types of user matrices according to the sequential rotation mechanism , j =1,2,3; Using Vector Groups Replace the original vector group , solve for the vector group Rotate to The rotation matrix required for coincidence , and extract the rotation matrix Corresponding angle value ; From the said Select a value between 0 and The angle between radians is recorded as , and record The corresponding rotation matrix and user matrix ; Using the and Compute an estimate of the unknown hand-eye matrix ,Right now ; The hand-eye matrix estimate Substitution Get the unknown hand-eye matrix translation column vector The linear least squares estimate of ; where for The rotation matrix of 、 as well as The matrix 、 as well as Any matrix in The column vector of positions.

2. A robot non-singular hand-eye calibration method based on a sequential rotation mechanism as claimed in claim 1, characterized in that: The measurement data set collected for hand-eye calibration { , }, including the following steps: Control the robot's manipulator to perform two different movements and record the transition matrix of the manipulator's tool coordinate system between the two different movements And the relative pose matrix of the camera coordinate system held by the robotic arm between the two different movements ; Repeatedly obtain the transition matrix and the relative pose matrix The process of obtaining k groups of measurement data { , }, and extract the corresponding rotation axis unit direction vector { } and corner value { , }.

3. A robot non-singular hand-eye calibration method based on a sequential rotation mechanism as claimed in claim 2, characterized in that: The measured data set { , } Extract the angle value of the unknown hand-eye matrix rotation part , including the steps of: According to the corresponding relationship between the modified Rodriguez parameters and the rotation matrix, construct and The corresponding modified Rodriguez parameters and ; Constructing overdetermined linear equations , use the linear least squares method to solve the vector The value of The vector corresponding to the unknown hand-eye matrix Bring in Inverse solution The value of According to the vector and , we can get the rotation angle value of the unknown hand-eye matrix ; Among them, the vector is a matrix X Corresponding modification of the Rodriguez parameter; a tan is the inverse tangent operator, The modulo operator for vectors.

4. A robot non-singular hand-eye calibration method based on a sequential rotation mechanism as claimed in claim 3, characterized in that: The construction and The corresponding modified Rodriguez parameters and , the specific expression is: 。 5. The robot non-singular hand-eye calibration method based on sequential rotation mechanism as claimed in claim 1, characterized in that: Solving the unknown hand-eye matrix according to the hand-eye calibration algorithm based on modified Rodriguez parameters specifically includes the following steps: Given the hand-eye matrix estimate , get the hand-eye matrix estimate Modify Rodriguez parameters accordingly ; Collect the homogeneous coordinate transformation matrix between the robot tool coordinate system and the camera coordinate system between two adjacent movements and , and extract them in sequence and Modify the Rodriguez parameters corresponding to the rotation part and ; Obtain an overdetermined linear system of equations consisting of multiple measurement data , and calculate the overdetermined linear equations ; in, is the antisymmetric matrix corresponding to the vector, is the modulus operator for vectors; according to , construct vector The corresponding Rodriguez parameter ,get The corresponding hand-eye matrix estimate ; Will Substitute into the equation , get the hand-eye matrix translation column vector The linear least squares estimate of .

6. A robot non-singular hand-eye calibration method based on sequential rotation mechanism as claimed in claim 5, characterized in that: The modified Rodriguez parameters The calculation expression is: in, With vector They are The angle value and the unit direction vector of the rotation axis.

7. A robot non-singular hand-eye calibration method based on sequential rotation mechanism as claimed in claim 6, characterized in that: The vector The calculation expression is: Will Substitution In the calculation, we get ; in, Refers to the rotation angle value of the unknown hand-eye matrix.

8. The robot non-singular hand-eye calibration method based on sequential rotation mechanism as claimed in claim 1, characterized in that: The three types of user matrices are defined according to the sequential rotation mechanism , including the steps of: According to the sequential rotation mechanism, the first type of user matrix is ​​defined: ; According to the sequential rotation mechanism, the second type of user matrix is ​​defined: ; According to the sequential rotation mechanism, the third type of user matrix is ​​defined: ; in, For the coordinate axis X The unit direction vector of ; For the coordinate axis Y The unit direction vector of ; For the coordinate axis Z The unit direction vector of ; Between 0 and Any angle value between radians; For around Rotation The resulting rotation matrix in radians; For around Rotation The resulting rotation matrix in radians; For around Rotation The resulting rotation matrix in radians.

9. A robot non-singular hand-eye calibration method based on sequential rotation mechanism as claimed in claim 8, characterized in that: The three types of user matrices There exists a corner value between 0 and The user matrix between radians includes the following steps: Vector Group Rotate to The rotation matrix required for coincidence is ,but ; Obtained from the properties of the rotation matrix, user-defined rotation matrix , j =1,2,3's traces are: in, represent No. i Rank j Elements of the column, tr (·) is the trace of the rotation matrix; according to The constraints between the internal elements can be sorted out as follows: The three types of user matrices are obtained by the properties of the rotation matrix trace. At least one matrix has a non-negative trace, that is, there is at least one corner value between 0 and User matrix between radians .

10. The robot non-singular hand-eye calibration method based on sequential rotation mechanism as claimed in claim 8, characterized in that: Group the vectors Rotate to known measurement data The axis of rotation , which coincides with the required rotation matrix The rotation angle value and The corresponding rotation angle values ​​are different.

Images