A method for measuring the spatial rotation angle

Abstract

The present invention belongs to the field of general measurement methods, and relates to a method for measuring the spatial rotation angle. A method for measuring the spatial rotation angle includes step S1 of fixedly installing a MEMS three-dimensional attitude sensor on the surface of a rigid body; step S2 of constructing a world coordinate system S for the rotation of the rigid body around an axis to obtain an angle calculation formula; step S3 of establishing a sensor coordinate system T, measuring the Euler angles of the MEMS three-dimensional attitude sensor at the starting position and the ending position of rotation in the sensor coordinate system T, and calculating the three-axis unit coordinate vectors of the sensor at the starting position and the ending position of rotation; step S4 of calculating the spatial rotation angle of the rigid body. The present invention can be used to measure the rotation angle of a spatial rotation axis at any position without the need to find the rotation axis, has no requirements for the installation position, avoids installation errors, has strong applicability, a simple measurement method, low cost, is easy to realize automatic measurement and mass production of products based on this method.

Description

Technical Field

[0001] The present invention belongs to the field of general measurement methods, and particularly relates to a method for measuring the spatial rotation angle. Background Art

[0002] In various fields of industry, there are requirements for measuring the rotation angle of a fixed-axis, such as the rotation angle of an aircraft rudder surface, the rotation angle of a robotic arm, and the angle of a turntable, etc. The methods for measuring the rotation angle mainly include mechanical, electromagnetic, optical, etc., and their working principles and application scopes are not the same. Electromagnetic and mechanical methods are generally manual measurements and require finding their rotation axes, which limits the measurement accuracy and is not easy to automate. The optical method also has its application scope. Taking the measurement of the aircraft rudder surface rotation angle as an example, during the current ground installation and adjustment test stage, an inclinometer is generally used to measure the rotation angle of the horizontal rudder surface. This method has measurement errors when the rudder surface rotation axis is not parallel to the horizontal plane or the installation axis of the inclinometer is not parallel. When measuring the vertical rudder surface, a mechanical method or measuring the chord length of the rotation trajectory of a certain point on the rudder surface is generally used, and these measurement methods have installation and measurement errors. When measuring the rotation angles of other non-horizontal and non-vertical rudder surfaces, there is currently a method using a two-axis inclinometer, and its measurement range is also limited, and the measurement error is large when the rotation axis is approximately perpendicular to the ground. Currently, the measurement methods in various situations are not universal and unified.

[0003] With the development of MEMS technology, three-dimensional attitude sensors have developed rapidly. It includes motion sensors such as three-axis gyroscopes, three-axis accelerometers, and three-axis electronic compasses. In some occasions, MEMS attitude sensors have begun to be used to measure the rotation angle, and the drift problem of its measurement result value is gradually being optimized. Due to its advantages of small volume, low cost, wide measurement range, and strong universality, MEMS attitude sensors are applied more and more widely. However, when directly using a MEMS attitude sensor to measure the rotation angle, its rotation axis must also be found, and there are also installation errors, which also limit it in the field of rotation angle measurement. Summary of the Invention

[0004] The object of the present invention: The object of the present invention is to provide a method for measuring the rotation angle to solve the drawbacks of some current measurement methods, such as weak universality, limited application scope, and being affected by installation errors. This method is based on a MEMS three-dimensional attitude sensor, without the need to find its rotation axis, and only needs to fix the sensor at any position on the rigid body, measure the Euler angles before and after rotation, and then the rotation angle can be calculated and measured.

[0005] The technical solution of the present invention:

[0006] A method for measuring the spatial rotation angle includes the following steps:

[0007] Step S1, fixedly install a MEMS three-dimensional attitude sensor on the surface of the rigid body;

[0008] Step S2: Construct the world coordinate system S for the rotation of the rigid body about an axis, and obtain the angle calculation formula.

[0009] Step S3: Establish the sensor coordinate system T, measure the Euler angles of the MEMS three-dimensional attitude sensor at the starting and ending positions of rotation in the sensor coordinate system T, and calculate the three-axis unit coordinate vectors of the sensor at the starting and ending positions of rotation.

[0010] Step S4: Calculate the spatial rotation angle of the rigid body.

[0011] Furthermore, specifically in step S1, a measurement point is selected on the rigid body. The selected point is located at any position on the surface of the rigid body. A MEMS three-dimensional attitude sensor is installed at the selected point, so that the MEMS three-dimensional attitude sensor is in a fixed connection state with the rigid body.

[0012] Furthermore, step S2 specifically includes the following steps:

[0013] Step S21: According to the constructed world coordinate system S, establish the three-axis unit vector coordinate system of the MEMS three-dimensional attitude sensor at the initial position of rotation. Then, construct matrix D based on the three-axis unit vector coordinate system of the MEMS three-dimensional attitude sensor at the initial position of rotation. s1 Similarly, according to the constructed world coordinate system S, establish the three-axis unit vector coordinate system of the MEMS three-dimensional attitude sensor at the ending position of rotation. Then, construct matrix D based on the three-axis unit vector coordinate system of the MEMS three-dimensional attitude sensor at the ending position of rotation. s2 ;

[0014] Step S22: Calculate the rotation matrix R of the MEMS three-dimensional attitude sensor from the initial position of rotation to the ending position of rotation according to the matrices D s1 and D s2 calculated in step S21.

[0015] Step S23: Construct the angle calculation formula according to the R calculated in step S22.

[0016] Furthermore, in step S21, based on the constructed world coordinate system S, the three-axis unit vector coordinate system of the MEMS three-dimensional attitude sensor at the initial position of rotation is p S1 u S1 v S1 w S1 , and matrix D s1 is constructed as follows:

[0017] where is the three-axis unit coordinate vector of the MEMS three-dimensional attitude sensor at the initial position of rotation; x uS1 -x pS1 , y uS1 -y pS1 , zuS1 -z pS1 is the three-axis unit coordinate vector of the rotation starting position sensor Components of the x, y, and z axes in the world coordinate system S; x vS1 -x pS1 , y vS1 -y pS1 , z vS1 -z pS1 is the three-axis unit coordinate vector of the rotation starting position sensor Components of the x, y, and z axes in the world coordinate system S; x wS1 -x pS1 , y wS1 -y pS1 , z wS1 -z pS1 is the three-axis unit coordinate vector of the rotation starting position sensor Components of the x, y, and z axes in the world coordinate system S;

[0018] The three-axis unit vector coordinate system of the rotation ending position sensor is p S2 u S2 v S2 w S2 , construct matrix D s2 :

[0019]

[0020] Among them, is the three-axis unit coordinate vector of the rotation ending position sensor; x uS2 -x pS2 , y uS2 -y pS2 , z uS2 -z pS2 is the three-axis unit coordinate vector of the rotation ending position sensor Components of the x, y, and z axes in the world coordinate system S; x vS2 -x pS2 , y vS2 -y pS2 , z vS2 -z pS2 is the three-axis unit coordinate vector of the rotation ending position sensor Components of the x, y, and z axes in the world coordinate system S; x wS2 -x pS2 , y wS2 -y pS2 , z wS2 -z pS2 is the three-axis unit coordinate vector of the rotation ending position sensor Components of the x, y, and z axes in the world coordinate system S.

[0021] Further, in step S22, the rotation matrix R of the MEMS three-dimensional attitude sensor from the initial rotation position to the final rotation position is calculated according to the following formula;

[0022] , where θ is the angle turned from the initial position to the final position when the rigid body rotates about the axis.

[0023] Further, in step S23, taking the trace of the rotation matrix R of the MEMS three-dimensional attitude sensor from the initial rotation position to the final rotation position, we can obtain:

[0024]

[0025] Then the angle calculation formula is:

[0026]

[0027] Further, in step S3, a sensor coordinate system T is established. The Euler angles of the MEMS three-dimensional attitude sensor at the starting position and the ending position of rotation in the sensor coordinate system T are (γ1, β1, α1) and (γ2, β2, α2), respectively;

[0028] The unit coordinate vectors of the three axes of the sensor at the starting position of rotation are: That is

[0029]

[0030] After transforming the above formula, we get:

[0031]

[0032]

[0033]

[0034] The unit coordinate vectors of the three axes of the sensor at the ending position of rotation are: That is

[0035]

[0036] After transforming the above formula, we get:

[0037]

[0038]

[0039]

[0040] Furthermore, in step 4, in the world coordinate system S and the MEMS three-axis attitude sensor coordinate system T, the included angles between the three axes of the initial position and the final position of the sensor rotation are equal, so their cosine values are also equal. Thus, we have:

[0041]

[0042] Converting the formula gives:

[0043]

[0044] The rigid body spatial rotation angle θ can be calculated through the above formula.

[0045] Furthermore, if any one of the coordinate axes of the MEMS three-axis attitude sensor in the sensor coordinate system T coincides with the rigid body rotation axis during installation, taking the Z-axis as an example, then θ = γ2 - γ1.

[0046] Furthermore, when the rotation axis of the rigid body is at an arbitrary angle in space, the method of the present invention can be used for angle measurement.

[0047] Furthermore, the rotation axis of the rigid body can be a fixed axis or an axis undergoing translational motion, and the method of the present invention can be used for angle measurement.

[0048] Advantages of the present invention: Based on the MEMS three-dimensional attitude sensor, the present invention can be used to measure the rotation angle of the spatial rotation axis at any position without finding the rotation axis, has no requirements for the installation position, avoids installation errors, has strong applicability, simple measurement method, low cost, is easy to realize automatic measurement and mass production of products based on this method; specifically, the present invention discloses:

[0049] Step S1, fixedly install the MEMS three-dimensional attitude sensor on the surface of the rigid body. When using the method of the present invention to measure the spatial rotation angle, only need to fixedly connect the sensor to the rigid body to perform the measurement, has no requirements for the installation position, avoids installation errors and human factor errors, and has wide applicability for measurement.

[0050] Step S2, construct the world coordinate system S of the rigid body rotating around the axis, and obtain the angle calculation formula; this step constructs the world coordinate system S, and constructs the initial position and final position matrices with the sensor three-axis unit vector coordinate system. Through the relationship between the constructed matrix and the rotation matrix, the angle measurement formula is determined as: the trace tr(R) of the rotation matrix = 2cosθ + 1 is equal to the sum of the cosine values of the included angles between the three axes of the initial position and the final position of the rotation. Its purpose and function are to find the relationship between the spatial rotation angle and the sensor position, and to prepare for the next step of finding the relationship between the spatial rotation angle and the parameters measured by the sensor.

[0051] Step S3: Establish a sensor coordinate system T, measure the Euler angles of the MEMS three-dimensional attitude sensor at the starting and ending positions of rotation in the sensor coordinate system T, and calculate the unit coordinate vectors of the three axes of the sensor at the starting and ending positions of rotation. This step constructs the sensor coordinate system T. By using the Euler angles measured by the sensor at the initial and final positions, the vectors of the three axes of the sensor at the initial and final positions in the sensor coordinate system T can be obtained. The purpose and function are to obtain the vector values of the three axes of the sensor through the sensor measurement values, preparing for the next calculation of the spatial rotation angle.

[0052] Step S4: Calculate the rigid body spatial rotation angle. In different coordinate systems, the included angle between the same vectors is equal. After obtaining the vector values of the three axes of the sensor at the initial and final positions in the sensor coordinate system T, the sum of the cosine values of the included angles between the three axes at the initial and final positions of rotation can be known, that is, equal to the trace of the rotation matrix tr(R) = 2cosθ + 1. Then the spatial rotation angle can be calculated. Description of the Drawings

[0053] Figure 1 is a flowchart of a method for measuring the spatial rotation angle of the present invention;

[0054] Figure 2 is a schematic diagram of the rigid body spatial rotation of the present invention;

[0055] Figure 3 is a schematic diagram of the spatial coordinate system of the MEMS three-dimensional attitude sensor of the present invention. Detailed Embodiments

[0056] The following is a further detailed description with reference to the drawings and through the description of the embodiments of the specific implementation manners of the present invention, such as the shapes, structures of the various components involved, the mutual positions and connection relationships between the various parts, the functions and working principles of the various parts, the manufacturing process, and the operation and usage methods, etc., to help those skilled in the art have a more complete, accurate, and in-depth understanding of the concept and technical solution of the present invention:

[0057] The technical solution of the present invention is: Fix and install a MEMS three-dimensional attitude sensor at any position on the surface of the rigid body, establish a rigid body coordinate system, and obtain the angle calculation formula By using the MEMS three-dimensional attitude sensor to measure the Euler angles of the sensor at the starting and ending positions of rotation, which are (γ1, β1, α1) and (γ2, β2, α2) respectively, the unit coordinate vectors of the three axes of the sensor at the starting and ending positions of rotation can be obtained, which are and Then the rotation angle can be solved and obtained

[0058] Step S1, select a measurement point on the rigid body. The selected point is located at any position on the surface of the rigid body. Install a MEMS three-axis attitude sensor at the selected point so that the sensor is fixedly connected to the rigid body.

[0059] The function of this step is: when using the method of the present invention to measure the spatial rotation angle, only need to fixedly connect the sensor to the rigid body to perform the measurement, there is no requirement for the installation position, avoiding installation errors and human factor errors, and the measurement has wide applicability.

[0060] Step S2, construct the world coordinate system S for the rotation of the rigid body around the axis, represented by oxyz. The rotation axis of the rigid body is oz, the self-coordinate system of the rigid body at the initial rotation position is ox1y1z1, and the self-coordinate system of the rigid body at the termination rotation position is ox2y2z2. For the convenience of calculation, assume that the world coordinate system S coincides with the self-coordinate system A of the rigid body at the initial rotation position.

[0061] Furthermore, it should be noted that the method described in this technical solution is applicable to any angle of the rotation axis oz of the rigid body in space.

[0062] The function of this step is: some rotation angle measurement methods have restrictions on the angle of the rotation axis. For example, the inclination measuring instrument should measure when the rotation axis is relatively horizontal; the biaxial inclination measuring instrument has poor measurement effects when the rotation axis is vertical or nearly vertical, etc. This method has no requirement for the attitude of the rotation axis OZ.

[0063] Furthermore, it should be noted that the rotation axis oz of the rigid body described in this technical solution can be a fixed axis or an axis undergoing translational motion, and this method is applicable to both, and the derivation processes of the two are not different.

[0064] This method uses vector calculation. It is easy to know that if the rigid body rotates around the OZ axis and the rigid body is undergoing translational motion, the operation processes are the same. Therefore, the present invention is also applicable to the situation where the rigid body rotates around its own axis and undergoes translational motion.

[0065] Furthermore, based on the above constructed world coordinate system S, the three-axis unit vector coordinate system of the sensor at the initial rotation position is p S1 u S1 v S1 w S1 , construct the matrix The three-axis unit vector coordinate system of the sensor at the termination rotation position is p S2 u S2 v S2 w S2 , similarly construct the matrix D S2 .

[0066] Furthermore, from the above constructed matrix D S1 、D S2are all orthogonal matrices, and D S2 = R·D S1 , that is

[0067] where is the rotation matrix from the initial rotation position to the final rotation position, θ is the angle rotated by the rigid body around the axis from the initial position to the final position, and taking the trace of the matrix gives

[0068]

[0069] Therefore, the trace of the rotation matrix is equal to the sum of the cosine values of the angles between the three axes at the initial and final rotation positions.

[0070] The function of step 2 is as follows: This step constructs the world coordinate system S and constructs the initial and final position matrices based on the sensor's three-axis unit vector coordinate system. Through the relationship between the constructed matrices and the rotation matrix, the angle measurement formula is determined as: the trace tr(R) of the rotation matrix = 2cosθ + 1, which is equal to the sum of the cosine values of the angles between the three axes at the initial and final rotation positions. Its purpose and function are to find the relationship between the spatial rotation angle and the sensor position, and to prepare for the next step of finding the relationship between the spatial rotation angle and the parameters measured by the sensor.

[0071] Step S3, the initial coordinate system of the MEMS three-axis attitude sensor is generally the northeast celestial coordinate system, that is, when the angles of the X, Y, and Z axes are zero, the X axis points east, the Y axis points north, and the Z axis points to the sky. Based on the initial coordinate system of the MEMS three-axis attitude sensor, a coordinate system T is established, denoted by o′x′y′z′. The three-axis unit vector coordinate system of the sensor at the initial rotation position is p T1 u T1 v T1 w T1 , and the three-axis unit vector coordinate system of the sensor at the final rotation position is p T2 u T2 v T2 w T2 , and similarly, the matrix

[0072] Furthermore, using the MEMS three-axis attitude sensor to measure the Euler angles of the rigid body at the initial position as (γ1, β1, α1), generally, there are multiple rotation sequences for the Euler angles of the sensor. In this article, taking the Z - Y - X rotation sequence as an example, the three-axis unit vectors of the sensor at the initial rotation position can be obtained as follows: Similarly, measuring the Euler angles of the rigid body at the final position as (γ2, β2, α2), the D T2 can be obtained.

[0073] Further, from the above Similarly, (γ1, β1, α1) can obtain

[0074] The function of step 3 is: this step constructs the sensor coordinate system T. Through the Euler angles measured by the sensor at the initial position and the end position, the vectors of the three axes of the sensor at the initial position and the end position in the sensor coordinate system T can be obtained. Its purpose and function are to obtain the vector values of the three axes of the sensor through the sensor measurement values, so as to prepare for the next calculation of the spatial rotation angle.

[0075] Step S4, in the world coordinate system S and the MEMS three-axis attitude sensor coordinate system T, the included angles between the three axes at the initial rotation position and the end rotation position of the sensor are equal, so their cosine values are also equal. Thus, we have:

[0076]

[0077] Further, after measuring the Euler angles (γ1, β1, α1) at the initial position and the Euler angles (γ2, β2, α2) at the end position, we can obtain:

[0078] The function of step 4 is: in different coordinate systems, the included angles of the same vector are equal. After obtaining the vector values of the three axes of the sensor at the initial position and the end position in the sensor coordinate system T, we know that the sum of the cosine values of the included angles between the three axes at the initial rotation position and the end rotation position is equal to the trace of the rotation matrix tr(R) = 2cosθ + 1. Thus, the spatial rotation angle can be calculated.

[0079] Further, if a certain axis of the MEMS three-axis attitude sensor, such as the Z axis, coincides with the rigid body rotation axis, then θ = γ2 - γ1.

[0080] The above has described the present invention by way of example in conjunction with the accompanying drawings. Obviously, the specific implementation of the present invention is not limited by the above methods. As long as various non-substantive improvements are made by adopting the method concept and technical solution of the present invention, or the concept and technical solution of the present invention are directly applied to other occasions without improvement, they are all within the protection scope of the present invention.

Claims

1. A method for measuring the spatial rotation angle, characterized in that, It includes the following steps: Step S1, fixedly install a MEMS three-dimensional attitude sensor on the surface of the rigid body; Step S2, construct the world coordinate system S for the rotation of the rigid body about an axis, and obtain the angle calculation formula; Step S3, establish the sensor coordinate system T, measure the Euler angles of the MEMS three-dimensional attitude sensor at the starting position and the ending position of rotation in the sensor coordinate system T, and calculate the unit coordinate vectors of the three axes of the sensor at the starting position and the ending position of rotation; the Euler angles of the MEMS three-dimensional attitude sensor at the starting position and the ending position of rotation in the sensor coordinate system T are (γ1, β1, α1) and (γ2, β2, α2) respectively; The three-axis unit coordinate vectors of the rotation starting position sensor are as follows: That is After the above formula is deformed, we get: The three-axis unit coordinate vectors of the rotation stop position sensor are: That is After the above formula is deformed, we get: Step S4, calculate the spatial rotation angle of the rigid body.

2. The method for measuring the spatial rotation angle according to claim 1, characterized in that, Specifically, in step S1, a measurement point is selected on the rigid body. The selected point is located at any position on the surface of the rigid body. A MEMS three-dimensional attitude sensor is installed at the selected point, so that the MEMS three-dimensional attitude sensor is in a fixed connection state with the rigid body.

3. The method for measuring the spatial rotation angle according to claim 1, characterized in that, Step S2 specifically includes the following steps: Step S21: According to the constructed world coordinate system S, establish a three-axis unit vector coordinate system for the initial rotation position sensor of the MEMS three-dimensional attitude sensor, and then construct matrix D based on the three-axis unit vector coordinate system of the initial rotation position sensor s1 , Similarly, according to the constructed world coordinate system S, establish a three-axis unit vector coordinate system for the final rotation position sensor of the MEMS three-dimensional attitude sensor, and then construct matrix D based on the three-axis unit vector coordinate system of the final rotation position sensor s2 ; Step S22, according to the matrix D constructed in step S21 s1 and D s2 calculate the rotation matrix R of the MEMS three-dimensional attitude sensor from the initial rotation position to the final rotation position; Step S23, construct the angle calculation formula according to the R calculated in step S22.

4. The method for measuring the spatial rotation angle according to claim 3, characterized in that, In step S21, based on the constructed world coordinate system S, rotate the three-axis unit vector coordinate system of the initial position sensor to p S1 u S1 v S1 w S1 , and construct matrix D s1 : wherein, is the three-axis unit coordinate vector of the rotation starting position sensor; x uS1 -x pS1 , y uS1 -y pS1 , z uS1 -z pS1 is the three-axis unit coordinate vector of the rotation starting position sensor components of the x, y, and z axes in the world coordinate system S; x vS1 -x pS1 , y vS1 -y pS1 , z vS1 -z pS1 is the three-axis unit coordinate vector of the rotation starting position sensor components of the x, y, and z axes in the world coordinate system S; x wS1 -x pS1 , y wS1 -y pS1 , z wS1 -z pS1 is the three-axis unit coordinate vector of the rotation starting position sensor components of the x, y, and z axes in the world coordinate system S; The three-axis unit vector coordinate system of the rotation stop position sensor is p S2 u S2 v S2 w S2 , construct matrix D s2 : Among them, is the three-axis unit coordinate vector of the rotation termination position sensor; x uS2 -x pS2 ,y uS2 -y pS2 ,z uS2 -z pS2 are the three-axis unit coordinate vectors of the rotation termination position sensor Components of the x, y, and z axes in the world coordinate system S; x vS2 -x pS2 ,y vS2 -y pS2 ,z vS2 -z pS2 are the three-axis unit coordinate vectors of the rotation termination position sensor Components of the x, y, and z axes in the world coordinate system S; x wS2 -x pS2 ,y wS2 -y pS2 ,z wS2 -z pS2 are the three-axis unit coordinate vectors of the rotation termination position sensor Components of the x, y, and z axes in the world coordinate system S.

5. The method for measuring the spatial rotation angle according to claim 4, characterized in that, In step S22, calculate the rotation matrix R of the MEMS three-dimensional attitude sensor from the initial position of rotation to the ending position of rotation according to the following formula; ， where θ is the angle turned by the rigid body about the axis from the initial position to the ending position.

6. The method for measuring the spatial rotation angle according to claim 5, characterized in that, In step S23, taking the trace of the rotation matrix R of the MEMS three-dimensional attitude sensor from the initial position of rotation to the ending position of rotation, we can get: Then the angle calculation formula is:

7. A method for measuring the spatial rotation angle according to claim 6, characterized in that, In step 4, in the world coordinate system S and the coordinate system T of the MEMS three-dimensional attitude sensor, the included angles between the three axes at the initial position of rotation and the ending position of rotation of the sensor are equal, so their cosine values are also equal. Thus, we have: After converting the formula, we get: The spatial rotation angle θ of the rigid body can be calculated through the above formula.

8. A method for measuring the spatial rotation angle according to claim 7, characterized in that, If any one of the coordinate axes of the MEMS three-dimensional attitude sensor coincides with the rotation axis of the rigid body during installation in the sensor coordinate system T, then θ = γ2 - γ1.

Images