CN106871859A - A kind of rigid space fixed-axis rotation angle detecting method - Google Patents

Abstract

The invention discloses a kind of rigid space fixed-axis rotation angle detecting method, propose in total station survey positioning principle, i.e. by using total powerstation two positions to the horizontal angle and vertical angular measurement of same index point, on the premise of obtaining coordinate value of the corresponding index point under total station instrument coordinate system, one group of vector is constructed by the coordinate value of index point, determine the pose that appearance principle determines Rigid Body In Space using double vectors, on this basis, the derivation and checking of rotational angle calculation method are carried out, for the rigid body pose parameter measurement under big range of movement provides new solution.

Description

A kind of rigid space fixed-axis rotation angle detecting method

Technical field

The invention belongs to rigid body outer corner measurement technical field, and in particular to a kind of rigid space fixed-axis rotation angle detection side The design of method.

Background technology

Rigid body refers to that under the effect of any external force, the completely constant object of its shapes and sizes, is a kind of preferable particle System.The most basic forms of motion of rigid body is translation and rotation, and any complicated rigid motion can be broken into translation and turn Dynamic superposition, both simple motions are most common motions in engineering, are also the basis for studying compound movement.In engineering some The motion of object, for example：Motion, motion of lathe upper tool post of piston etc. belong to translation of rigid body；And most common tooth Wheel, the main shaft of lathe, the rotor of motor etc. belong to rigid body round fixed axis rotation.

The outer corner measurement of rigid motion has been widely used for many fields such as industry, military affairs, space flight, navigation, and to surveying The requirement of accuracy of measurement also more and more higher.In Industry Control and measurement, angle is measured as an important parameter to it It is very necessary.Such as in some land-based station or sea base station antenna servo-drive system, in the industrial production for needing turntable, exist In the real-time control of the slow-speed of revolution, all can't do without the measurement of angle.

Industrial existing main angle-measuring method has：Mechanical angle-measuring method, electromagnetism angle-measuring method, optics angle-measuring method with And photoelectricity angle-measuring method etc..Mechanical angle measurement technique is one of earliest angle measurement technique of research, and it is mainly with end tooth indexing Represent.Mechanical angle-measuring method early stage application is more, and its advantage is simple, low cost.Electro-magnetic Graduation formula angle measurement technique is using electricity The principle of magnetic induction measures angle, mainly uses the various parameters to carry out indexing angle measurement.Electro-magnetic Graduation technology is widely used in Further subdivision, expands the indexing of instrument and measurement range, and resolution ratio is improved, using in extensive range.Mainly there is round magnetic at present Two kinds of grid angle measurement and inductosyn angle measurement.Optics angle-measuring method is always subject to the weight of people with its high accuracy of measurement Depending on the application of optics angle-measuring method is also more and more extensive.Current optics angle-measuring method except well-known optical dividing head method and Outside multiaspect shuttle body method, conventional also has Moire fringe technique, parallel interferogram technique, Circular gratings method, optical internal-reflection method, laser interference Method etc..In photoelectricity angle-measuring method, there are optical code disc type sensor, loop laser using commonplace photoelectric sensor at present Device and imageing sensor, it has the advantages that high accuracy, high reliability, realizes that simple, small volume, angular sensitivity are high.

Wherein mechanical angle-measuring method, electromagnetism angle-measuring method belong to traditional angle measurement method, and most Bian is contacted by hand The mode of measurement, the precision and stability of measurement is influenceed larger by external environment.Optics angle-measuring method, photoelectricity angle-measuring method by Do not contacted with testee in measurement process, not only realize the high precision non-contact measurement of corner, and its is simple to operate, System reliability is high, is easy to implement measurement at the scene, and can substantially reduce manual labor.

For fixed-axis rotation, angle detection method can be divided into contact measurement and non-contact detection.For example passed in gear In dynamic system, now axis is fixed and is easily installed the equipment such as encoder, and corner can be easily detected using contact measurement； It is conventional and for example in the roll angle inspections such as aircraft rudder surface, axis fixes indefinite or outer corner measurement element and is not easy to install The detection method such as installation encoder be no longer applicable, common detection method is non-contact detection method, such as vision measurement, Major part vision measuring method is all based on the e measurement technology of binocular stereo vision at present.

All the time, in the fields such as Multi-axis aircraft, kinematic robot, mechanical arm and space flight and aviation, rigid space fortune Dynamic attitude and location parameter measurement is the important content of rigid motion analysis and control.Traditional Rigid Body In Space kinematic parameter is surveyed Amount method is that gyroscope is installed on rigid body, measures rigid space attitude angle using gyroscope, or combine gyroscope and acceleration The data fusion method for spending meter solves rigid space attitude information, and this mode needs to install the measuring instruments such as gyroscope on rigid body Device, corresponding measurement cost and plant maintenance cost is higher and precision limited.And in some cases, such as rigid body surface is easy When impaired or pivot center cannot determine, contact type measurement mode has significant limitation, therefore is surveyed in Estimating Motion Parameters In amount method, non-contact measurement due to measurement apparatus not with measured surface directly contact, measured surface will not both be damaged, Measurement apparatus will not be caused damage, while avoiding the measurement error introduced due to contact, and widely be paid close attention to.

In contactless measurement, the application study of vision measurement is the most extensive, and vision measurement is to complex environment, multiple Miscellaneous motion has good adaptability and certainty of measurement higher.Wherein, the precision such as heavy construction and construction of underground tunnel on the ground In engineering survey or deformation monitoring field, whole station type electronic distance meter is widely used.Whole station type electronic distance meter is a kind of Integrate light, mechanical, electrical high-tech measuring instrument, be collection horizontal angle, vertical angle, distance (oblique distance, put down away from), vertical survey work( Can be in the instrument of surveying and mapping system of one.Compare with micrometer instrument, optical circle is changed to optoelectronic scanning scale by electronic theodolite, Artificial optics micrometer reading is instead recorded automatically and reading is shown, makes angle measurement simple operation, and error in reading can be avoided Generation, it once disposes the instrument can to complete all to measure work on the survey station.

The content of the invention

It is difficult to determine and the complicated situation in rigid body surface the invention aims to solve rigid space rotary shaft pose Under, there are a variety of drawbacks in Rigid Body in Rotation With angle conventional contact measuring method, it is proposed that a kind of rigid space dead axle turns Dynamic angle detecting method.

The technical scheme is that：A kind of rigid space fixed-axis rotation angle detecting method, comprises the following steps：

S1, rigid body surface marker point location；

S2, definition world coordinate system and Rigid Body in Rotation With start-stop position coordinate system；

S3, construction and the vector for characterizing rigid motion；

S4, calculating Rigid Body in Rotation With angle.

Further, step S1 is specially：

Three index points are set on rigid body surface, each is marked respectively using two total powerstations for being arranged at diverse location Will point is measured, and obtains horizontal angle, the vertical angle of each index point, and according to the measurement and positioning principle of total powerstation, by each mark The horizontal angle of point, vertical angle obtain the D coordinates value of the index point.

Further, step S2 is specially：

Using the coordinate system OXwYwZw of total powerstation characteristic point measurement data as world coordinate system；If Rigid Body in Rotation With is initial Position and final position are respectively m, n position, set up the rigid body coordinate system of n positions respectively using Rigid Body in Rotation With axle as Z axis The rigid body coordinate system O1XmYmZm of O1XnYnZn and m positions；The rigid body coordinate system O1XnYnZn of n positions is relative to m positions There is the amount of spin that angle is θ in rigid body coordinate system O1XmYmZm.

Further, step S3 is specially：

In rigid body coordinate system O1XmYmZm, using three index points on rigid body surface as characteristic point p_mi, i=1,2,3, it is right Answer the characteristic point p in world coordinate system OXwYwZw_wi, i=1,2,3；By point p_m1Tie point p_m2、p_m3, obtain vector L_m1、L_m2, phase It is corresponding by point p_w1Tie point p_w2、p_w3, obtain vector L_w1、L_w2；

If the transforming relationship of world coordinate system OXwYwZw and rigid body coordinate system O1XmYmZm is：

In formulaSpin matrixs of the rigid body coordinate system O1XmYmZm relative to world coordinate system OXwYwZw is represented, T is firm D coordinates value of the origin of body coordinate system O1XmYmZm in world coordinate system OXwYwZw；

Corresponding characteristic point p_miAnd p_wiTransforming relationship be：

Had by the vectorial structure mode in world coordinate system OXwYwZw：

L_w1=p_w2-p_w1,L_w2=p_w3-p_w1 (3)

Similarly have in rigid body coordinate system O1XmYmZm：

L_m1=p_m2-p_m1,L_m2=p_m3-p_m1 (4)

Obtained by formula (2) (3) (4)：

The 3rd vector needed for construction, from two vector L_w1With L_w2Multiplication cross is carried out, and meets following relation：

Define vector J_wAnd J_m：

J_w=L_w1*L_w2,J_m=L_m1*L_m2 (7)

Obtained by formula (6) (7)：

Define matrix D_wAnd D_m：

D_w=[L_w1 L_w2 J_w],D_m=[L_m1 L_m2 J_m] (9)

Obtained by formula (5) (8) (9)：

By matrix D_wAnd D_mConstruction process understand, two matrixes are non-singular matrix, therefore there is invertible matrix, then by Formula (10)：

Similarly understand, rigid body at n positions, rotations of the rigid body coordinate system O1XnYnZn relative to world coordinate system OXwYwZw Torque battle arrayFor：

Wherein：

D_w′=[L_{w′ 1}L_w′2 J_w′],D_n=[L_n1 L_n2 J_n] (13)

During actual Rigid Body in Rotation With, final position n relative initial positions m only exists fixed-axis rotation, and angle of rotation It is θ to spend, thus n positions spin matrixs of the rigid body coordinate system O1XnYnZn relative to the rigid body coordinate system O1XmYmZm of m positionsFor：

Obtained by formula (11) (12) (14)：

Vector Mode has consistency in coordinate transform, therefore there is following relation：

|L_wi|=| L_mi|=| L_ni|, i=1,2 (16).

Further, step S4 is specially：

From the mode of setting up of coordinate system, characteristic point is fixed relative to rigid body coordinate system, therefore characteristic point is at n It is identical to put in the coordinate values with m positions, i.e.,：

p_ni=p_mi, i=1,2,3 (17)

Because the coordinate of each characteristic point is identical, then have：

D_m=D_n (18)

Following relational expression is derived with reference to formula (11) (12) (15) (18)：

From matrix construction process, D_nIt is nonsingular matrix, then from formula (19)：WithIt is similar Matrix；From the property of similar matrix：Similar matrix has identical mark；AndObtained by total station survey Point coordinates is constructed, and is known matrix, order：

Then have：

WhereinObtained by similar matrix property：

2cos θ+1=a₁₁+a₂₂+a₃₃ (22)

From formula (22), θ byMark try to achieve, make a=a₁₁+a₂₂+a₃₃, obtain the expression of rigid body rotational angle theta Formula is：

The beneficial effects of the invention are as follows：The present invention determines appearance principle using non-contact measurement mode and based on double vectors, can According to two groups of coordinate values of index point on the rigid body that total powerstation is measured, the angle of rigid body fixed-axis rotation is easily solved, be big Rigid body pose parameter measurement under range of movement provides new solution.

Brief description of the drawings

A kind of rigid space fixed-axis rotation angle detecting method flow chart that Fig. 1 is provided for the present invention.

Fig. 2 is the total station survey positioning principle schematic diagram of the embodiment of the present invention.

Fig. 3 is the definition coordinate system schematic diagram of the embodiment of the present invention.

Fig. 4 is the characteristic point and vectorial structure schematic diagram of the embodiment of the present invention.

Specific embodiment

Embodiments of the invention are further described below in conjunction with the accompanying drawings.

The invention provides a kind of rigid space fixed-axis rotation angle detecting method, as shown in figure 1, comprising the following steps：

S1, rigid body surface marker point location.

As shown in Fig. 2 setting three index point p on rigid body surface₁、p₂And p₃, diverse location is arranged at using two Total powerstation A, B are measured to each index point respectively, obtain horizontal angle, the vertical angle of each index point, and according to total powerstation Measurement and positioning principle, the D coordinates value of the index point is obtained by the horizontal angle of each index point, vertical angle.

S2, definition world coordinate system and Rigid Body in Rotation With start-stop position coordinate system.

As shown in figure 3, using the coordinate system OXwYwZw of total powerstation characteristic point measurement data as world coordinate system；If rigid body The initial position of rotation and final position are respectively m, n position, set up the rigid body of n positions respectively using Rigid Body in Rotation With axle as Z axis The rigid body coordinate system O1XmYmZm of coordinate system O1XnYnZn and m positions；The rigid body coordinate system O1XnYnZn of n positions is relative to m There is the amount of spin that angle is θ in the rigid body coordinate system O1XmYmZm of position.Because rigid body is in the arbitrary orientation in space, can always lead to Cross and reached around three restricted rotational movements of reference axis, if the initial position co-ordinates system O1XmYmZm of rigid body is relative to world coordinate system There is angle of revolution, the angle of pitch, the rotation amount (characterizing attitude with Eulerian angles) of deflection angle in OXwYwZw.

S3, construction and the vector for characterizing rigid motion.

As shown in figure 4, in rigid body coordinate system O1XmYmZm, using three index points on rigid body surface as characteristic point p_mi,i =1,2,3, the characteristic point p in correspondence world coordinate system OXwYwZw_wi, i=1,2,3；By point p_m1Tie point p_m2、p_m3, sweared Amount L_m1、L_m2, it is corresponding by point p_w1Tie point p_w2、p_w3, obtain vector L_w1、L_w2。

If the transforming relationship of world coordinate system OXwYwZw and rigid body coordinate system O1XmYmZm is：

In formulaSpin matrixs of the rigid body coordinate system O1XmYmZm relative to world coordinate system OXwYwZw is represented, T is firm D coordinates value of the origin of body coordinate system O1XmYmZm in world coordinate system OXwYwZw.

Corresponding characteristic point p_miAnd p_wiTransforming relationship be：

Had by the vectorial structure mode in world coordinate system OXwYwZw：

L_w1=p_w2-p_w1,L_w2=p_w3-p_w1 (3)

Similarly have in rigid body coordinate system O1XmYmZm：

L_m1=p_m2-p_m1,L_m2=p_m3-p_m1 (4)

Obtained by formula (2) (3) (4)：

Obviously, under the same coordinate system, two vectors of construction are not parallel by same point, in order to needed for constructing The 3rd vector, from two vector L_w1With L_w2Multiplication cross is carried out, and meets following relation：

State for convenience, define vector J_wAnd J_m：

J_w=L_w1*L_w2,J_m=L_m1*L_m2 (7)

Obtained by formula (6) (7)：

Define matrix D_wAnd D_m：

D_w=[L_w1 L_w2 J_w],D_m=[L_m1 L_m2 J_m] (9)

Obtained by formula (5) (8) (9)：

By matrix D_wAnd D_mConstruction process understand, two matrixes are non-singular matrix, therefore there is invertible matrix, then by Formula (10)：

Similarly understand, rigid body at n positions, rotations of the rigid body coordinate system O1XnYnZn relative to world coordinate system OXwYwZw Torque battle arrayFor：

Wherein：

D_w′=[L_w′1 L_w′2 J_w′],D_n=[L_n1 L_n2 J_n] (13)

During actual Rigid Body in Rotation With, final position n relative initial positions m only exists fixed-axis rotation, and angle of rotation It is θ to spend, thus n positions spin matrixs of the rigid body coordinate system O1XnYnZn relative to the rigid body coordinate system O1XmYmZm of m positionsFor：

Obtained by formula (11) (12) (14)：

Vector Mode has consistency in coordinate transform, therefore there is following relation：

|L_wi|=| L_mi|=| L_ni|, i=1,2 (16)

From formula (15) (16), there is 6 coordinate unknown quantitys and 1 rotational angle unknown quantity, totally 7 unknown quantitys； By each element corresponding relation of matrix and the equal relation of Vector Mode, 13 relational expressions can be listed altogether, direct solution is complex, Therefore angle of rotation θ is solved using matrixing in the embodiment of the present invention.

S4, calculating Rigid Body in Rotation With angle.

From the mode of setting up of coordinate system, characteristic point is fixed relative to rigid body coordinate system, therefore characteristic point is at n It is identical to put in the coordinate values with m positions, i.e.,：

p_ni=p_mi, i=1,2,3 (17)

Because the coordinate of each characteristic point is identical, then have：

D_m=D_n (18)

Following relational expression is derived with reference to formula (11) (12) (15) (18)：

From matrix construction process, D_nIt is nonsingular matrix, then from formula (19)：WithIt is similar Matrix；From the property of similar matrix：Similar matrix has identical mark；AndObtained by total station survey Point coordinates is constructed, and is known matrix, order：

Then have：

WhereinObtained by similar matrix property：

2cos θ+1=a₁₁+a₂₂+a₃₃ (22)

From formula (22), θ byMark try to achieve, make a=a₁₁+a₂₂+a₃₃, obtain the expression of rigid body rotational angle theta Formula is：

Below according to applicable cases in practice, the accuracy to formula (23) is verified：

(1) position orientation relation, three coordinates of point under rigid body coordinate system under rigid body coordinate system and world coordinate system are given And the rotational angle theta of rigid body₀。

(2) according to coordinate conversion relation, three coordinates of point under two groups of world coordinate systems are calculated.

(3) according to theory deduction process, correlation matrix is constructed, angle of rotation θ is obtained by formula (23).

(4) θ and given rotational angle theta are differentiated₀It is whether consistent.

The result is as shown in the table, as a result proves that the rigid space fixed-axis rotation angle detecting method that the present invention is provided can OK.

One of ordinary skill in the art will be appreciated that embodiment described here is to aid in reader and understands this hair Bright principle, it should be understood that protection scope of the present invention is not limited to such especially statement and embodiment.This area Those of ordinary skill can according to these technical inspirations disclosed by the invention make it is various do not depart from essence of the invention other are each Plant specific deformation and combine, these deformations and combination are still within the scope of the present invention.

Claims (5)

1. a kind of rigid space fixed-axis rotation angle detecting method, it is characterised in that comprise the following steps：

S1, rigid body surface marker point location；

S2, definition world coordinate system and Rigid Body in Rotation With start-stop position coordinate system；

S3, construction and the vector for characterizing rigid motion；

S4, calculating Rigid Body in Rotation With angle.

2. rigid space fixed-axis rotation angle detecting method according to claim 1, it is characterised in that the step S1 tools Body is：

Three index points are set on rigid body surface, using two total powerstations for being arranged at diverse location respectively to each index point Measure, obtain horizontal angle, the vertical angle of each index point, and according to the measurement and positioning principle of total powerstation, by each index point Horizontal angle, vertical angle obtain the D coordinates value of the index point.

3. rigid space fixed-axis rotation angle detecting method according to claim 2, it is characterised in that the step S2 tools Body is：

Using the coordinate system OXwYwZw of total powerstation characteristic point measurement data as world coordinate system；If the initial position of Rigid Body in Rotation With M, n position are respectively with final position, the rigid body coordinate system O1XnYnZn of n positions is set up respectively using Rigid Body in Rotation With axle as Z axis And the rigid body coordinate system O1XmYmZm of m positions；Rigid body coordinates of the rigid body coordinate system O1XnYnZn of n positions relative to m positions Be O1XmYmZm exist angle be θ amount of spin.

4. rigid space fixed-axis rotation angle detecting method according to claim 3, it is characterised in that the step S3 tools Body is：

In rigid body coordinate system O1XmYmZm, using three index points on rigid body surface as characteristic point p_mi, i=1,2,3, correspondence generation Characteristic point p in boundary coordinate system OXwYwZw_wi, i=1,2,3；By point p_m1Tie point p_m2、p_m3, obtain vector L_m1、L_m2, it is corresponding By point p_w1Tie point p_w2、p_w3, obtain vector L_w1、L_w2；

If the transforming relationship of world coordinate system OXwYwZw and rigid body coordinate system O1XmYmZm is：

$OXwYwZw=R_m^w\·O1XmYmZm+T---\left(1\right)$

In formulaSpin matrixs of the rigid body coordinate system O1XmYmZm relative to world coordinate system OXwYwZw is represented, T is rigid body coordinate It is D coordinates value of the origin of O1XmYmZm in world coordinate system OXwYwZw；

Corresponding characteristic point p_miAnd p_wiTransforming relationship be：

$p_{wi}=R_m^w\·p_{mi}+T---\left(2\right)$

Had by the vectorial structure mode in world coordinate system OXwYwZw：

L_w1=p_w2-p_w1,L_w2=p_w3-p_w1 (3)

Similarly have in rigid body coordinate system O1XmYmZm：

L_m1=p_m2-p_m1,L_m2=p_m3-p_m1 (4)

Obtained by formula (2) (3) (4)：

$L_{w1}=R_m^w\·L_{m1},L_{w2}=R_m^w\·L_{m2}---\left(5\right)$

The 3rd vector needed for construction, from two vector L_w1With L_w2Multiplication cross is carried out, and meets following relation：

$L_{w1}*L_{w2}=\left(R_m^w\·L_{m1}\right)*\left(R_m^w\·L_{m2}\right)=R_m^w\·(L_{m1}*L_{m2})---\left(6\right)$

Define vector J_wAnd J_m：

J_w=L_w1*L_w2,J_m=L_m1*L_m2 (7)

Obtained by formula (6) (7)：

$J_w=R_m^w\·J_m---\left(8\right)$

Define matrix D_wAnd D_m：

D_w=[L_w1 L_w2 J_w],D_m=[L_m1 L_m2 J_m] (9)

Obtained by formula (5) (8) (9)：

$D_w=R_m^w\·D_m---\left(10\right)$

By matrix D_wAnd D_mConstruction process understand, two matrixes are non-singular matrix, therefore there is invertible matrix, then by formula (10)：

$R_m^w=D_w\·D_m^{-1}---\left(11\right)$

Similarly understand, rigid body at n positions, spin moments of the rigid body coordinate system O1XnYnZn relative to world coordinate system OXwYwZw Battle arrayFor：

$R_n^w=D_{w^{\′}}\·D_n^{-1}---\left(12\right)$

Wherein：

D_w′=[L_w′1 L_w′2 J_w′],D_n=[L_n1 L_n2 J_n] (13)

During actual Rigid Body in Rotation With, final position n relative initial positions m only exists fixed-axis rotation, and rotational angle is θ, thus n positions spin matrixs of the rigid body coordinate system O1XnYnZn relative to the rigid body coordinate system O1XmYmZm of m positions For：

$R_n^w=\left[\begin{array}{ccc}cos\mathrm{\θ}& -sin\mathrm{\θ}& 0\\ sin\mathrm{\θ}& \cos \mathrm{\θ}& 0\\ 0& 0& 1\end{array}\right]---\left(14\right)$

Obtained by formula (11) (12) (14)：

$R_n^w=R_m^w\·R_n^m---\left(15\right)$

Vector Mode has consistency in coordinate transform, therefore there is following relation：

|L_wi|=| L_mi|=| L_ni|, i=1,2 (16).

5. rigid space fixed-axis rotation angle detecting method according to claim 4, it is characterised in that the step S4 tools Body is：

From the mode of setting up of coordinate system, characteristic point be relative to rigid body coordinate system it is fixed, therefore characteristic point in n positions and It is identical in the coordinate values of m positions, i.e.,：

p_ni=p_mi, i=1,2,3 (17)

Because the coordinate of each characteristic point is identical, then have：

D_m=D_n (18)

Following relational expression is derived with reference to formula (11) (12) (15) (18)：

$R_n^m=D_n\·D_w^{-1}\·D_{w^{\′}}\·D_n^{-1}---\left(19\right)$

From matrix construction process, D_nIt is nonsingular matrix, then from formula (19)：WithIt is similar matrix； From the property of similar matrix：Similar matrix has identical mark；AndIt is the point coordinates obtained by total station survey Construct, be known matrix, make：

$D_w^{-1}={\left[\begin{array}{ccc}{x}_{11}& x_{12}& x_{13}\\ x_{21}& x_{22}& x_{23}\\ x_{31}& x_{32}& x_{33}\end{array}\right]}^{-1},D_{w^{\′}}=\left[\begin{array}{ccc}{y}_{11}& y_{12}& y_{13}\\ y_{21}& y_{22}& y_{23}\\ y_{31}& y_{32}& y_{33}\end{array}\right]---\left(20\right)$

Then have：

$D_w^{-1}\·D_{w^{\′}}={\left[\begin{array}{ccc}{x}_{11}& x_{12}& x_{13}\\ x_{21}& x_{22}& x_{23}\\ x_{31}& x_{32}& x_{33}\end{array}\right]}^{-1}\·\left[\begin{array}{ccc}{y}_{11}& y_{12}& y_{13}\\ y_{21}& y_{22}& y_{23}\\ y_{31}& y_{32}& y_{33}\end{array}\right]=\left[\begin{array}{ccc}{a}_{11}& a_{12}& a_{13}\\ a_{21}& a_{22}& a_{23}\\ a_{31}& a_{32}& a_{33}\end{array}\right]---\left(21\right)$

WhereinI=1,2,3；J=1,2,3, obtained by similar matrix property：

2cos θ+1=a₁₁+a₂₂+a₃₃ (22)

From formula (22), θ byMark try to achieve, make a=a₁₁+a₂₂+a₃₃, the expression formula for obtaining rigid body rotational angle theta is：

$\mathrm{\θ}=\arccos \frac{a-1}{2}---\left(23\right).$