Single-axis rotation angle estimation method and system based on space conversion

Abstract

The invention provides a single-axis rotation angle estimation method and system based on spatial conversion, and the method comprises the steps: S1, setting a sensor coordinate system {# imgabs0 #}, a geodetic coordinate system {# imgabs1 #} and a rigid reference coordinate system {# imgabs2 #}, and setting a plurality of specific rotation matrixes according to the conversion relation between the coordinate systems; s2, setting a rotating shaft, and estimating a vector # imgabs4 # of the rotating shaft under a sensor coordinate system {# imgabs3 #} at an initial moment based on the rotating matrix; s3, on the basis of the rotation matrix, the rotation axis # imgabs5 # is subjected to space conversion, and # imgabs7 # under the geodetic coordinate system {# imgabs6 #} is obtained; s4, constructing a quaternion single-axis estimation rotation angle model, and analyzing the relationship between the quaternion and the rotation axis # imgabs8 # and the rotation angle to obtain an estimated rotation angle; and S5, correcting the estimated rotation angle to obtain a final estimated single-axis rotation angle. The problems that an existing single-axis angle measuring method is not high in precision and is only suitable for a visible rotating shaft are solved.

Description

Technical Field

[0001] The present invention relates to the field of attitude solution and angle estimation. Specifically, it relates to a method for calculating the rotation angle of a single-axis estimation based on space transformation, which is applicable to precision measurement fields such as aerospace, robotics, and automotive engineering. Background Art

[0002] In precision measurement fields such as aerospace, robotics, and automotive engineering, accurately measuring the angle of rotation around a fixed rotation axis is an important task. Currently, existing solutions based on inertial measurement units (IMUs) usually require the X-axis or Y-axis of the angle device coordinate system to be strictly aligned with the measured plane rotation axis. However, in practical engineering applications, due to mechanical installation errors, there is a problem that the actual rotation axis is inconsistent with the assumed rotation axis, ultimately resulting in a large deviation between the actual measured angle and the true angle, which poses a severe challenge to the stability of high-precision control systems.

[0003] Publication No. CN109990697A discloses a magnetic angle sensor device and a method for estimating the rotation angle. The magnetic angle sensor device includes a multi-pole magnet rotatable around a rotation axis. The geometric arrangement of the multi-pole magnet is rotationally asymmetric with respect to the rotation axis. Each magnetic field sensor circuit includes a first magnetic field sensor element sensitive to a first magnetic field component and a second magnetic field sensor element sensitive to a second magnetic field component perpendicular to the first magnetic field component. The processing circuit system is configured to calculate first intermediate angle information based on the combination of signals from multiple first magnetic field sensor elements, calculate second intermediate angle information based on the combination of signals from multiple second magnetic field sensor elements, and calculate an estimate of the rotation angle of the fixture and / or the multi-pole magnet based on the first and second intermediate angle information.

[0004] The prior art has problems such as magnetic field interference, being only applicable to visible axes, and difficulty in measuring angles for different coordinate systems. Therefore, a method and system for estimating the rotation angle of a single axis based on space transformation are needed. Summary of the Invention

[0005] Aiming at the deficiencies in the prior art, the purpose of the present invention is to provide a method and system for estimating the rotation angle of a single axis based on space transformation.

[0006] According to a method for estimating the rotation angle of a single axis based on space transformation provided by the present invention, it includes: Step S1: Set a sensor coordinate system { }, a geodetic coordinate system { }, and a rigid body reference coordinate system { }, and set multiple specific rotation matrices according to the transformation relationships between the coordinate systems; Step S2: Set the rotation axis and estimate the vector of the rotation axis in the sensor coordinate system { } at the initial moment by combining the acceleration data ; Step S3: Based on a specific rotation matrix, perform a spatial transformation on the rotation axis to obtain the rotation axis in the geodetic coordinate system { }; Step S4: Analyze the correspondence between the rotation axis and the rotation angle, and construct a quaternion single-axis estimated rotation angle model according to the correspondence to obtain the estimated rotation angle; Step S5: Correct the estimated rotation angle to obtain the final estimated single-axis rotation angle.

[0007] Preferably, the step S2 includes: Step S2.1: Let the accelerometer collect the acceleration data in the sensor coordinate system { }, and calculate the vector corresponding to the acceleration data collected by the accelerometer in the rigid body reference coordinate system { } according to the conversion of a specific rotation matrix, which is: where

[0008] represents the rotation matrix from the rigid body reference coordinate system { } to the sensor coordinate system { }; Step S2.1: Let the accelerometer collect the acceleration data in the sensor coordinate system { }, and calculate the vector corresponding to the acceleration data collected by the accelerometer in the rigid body reference coordinate system { } according to the conversion of a specific rotation matrix, which is: where represents the rotation matrix from the rigid body reference coordinate system {

[0009] } to the sensor coordinate system { }; Step S2.2: Set the rotation axis in the rigid body reference coordinate system, and the corresponding vector . Analyze the vector relationship corresponding to the acceleration data measured by the accelerometer in the reference system { } at the moment , which is:

[0010] ​​Indicates rotation around in the rigid body reference coordinate system { } by an angle, and the rotation matrix is composed of. When the rigid body is at the initial moment, the acceleration data in the rigid body reference coordinate system { } is Step S2.3: Calculate the difference vector of the accelerometer measurement data based on the acceleration data measured by the accelerometer at multiple different moments. Specifically, when the sensor coordinate system { } rotates about the rotation axis by a single degree of freedom, the trajectory of is a circle perpendicular to <. < For any time }, the vectors corresponding to the accelerometer measurement data in the rigid body reference coordinate system { , and are respectively, where , are both perpendicular to the straight line where the rotation axis is located. It can be obtained that:

[0011] where is a preset constant. Step S2.4: Construct an accelerometer estimated rotation axis model based on the acceleration difference vector, and calculate the vector corresponding to the rotation axis in the sensor coordinate system { } at the initial moment. Specifically, convert the acceleration difference vector through the rotation matrix to get:

[0012] Construct an accelerometer estimated rotation axis model to obtain the vector corresponding to the rotation axis in the sensor coordinate system { } at the initial moment, which is: .

[0013] Preferably, the step S3 includes: Step S3.1: When rotating from the initial position to the rotation position, set the quaternion output at the initial position to , and the quaternion output when moving to the rotation position to .

[0014] Each represents a component of the initial position quaternion. Each represents a component of the rotated position quaternion; Step S3.2: According to the quaternion at the initial moment , calculate the rotation matrix from the geodetic coordinate system { } to the sensor coordinate system { }: :

[0015] Step S3.3: According to the rotation matrix and , calculate the spatial direction of the rotation axis vector in the geodetic coordinate system { }, which is:

[0016] Wherein, represents the component of the rotation axis on the X-axis in the geodetic coordinate system { }, represents the component of the rotation axis on the Y-axis in the geodetic coordinate system { }, represents the component of the rotation axis on the Z-axis in the geodetic coordinate system { }.

[0017] Preferably, the step S4 includes: Step S4.1: Analyze the correspondence between the differential quaternion and the rotation axis and rotation angle. According to the correspondence, construct a quaternion single-axis estimated rotation angle model, which is:

[0018] represents the actual rotation angle around the fixed rotation axis ; , , are the components of the rotation axis on the three axes, satisfying ; Step S4.2: Input the rotation axis and the parameters of the rotated position quaternion into the quaternion single-axis estimated rotation angle model to obtain multiple estimated rotation angles; specifically,

[0019] Wherein, represents the first estimated rotation angle, represents the second estimated rotation angle, represents the third estimated rotation angle; Step S4.3: Integrate multiple estimated rotation angles to obtain the final estimated rotation angle : .

[0020] Preferably, step S5 comprises: Step S5.1: According to the rotation axis component The amplitude of , design adaptive dynamic weights , used to correct deviations, specifically,

[0021] in is the adaptive threshold, which is:

[0022] in is the preset smoothing coefficient, Indicates taking the middle value; is the adaptive threshold at time t, for Adaptive threshold at the moment; Step S5.2: Based on the adaptive dynamic weight Correct the estimated rotation angle to get the final estimated single-axis rotation angle:

[0023] in is the regularization parameter.

[0024] A system for estimating a single-axis rotation angle based on space conversion provided by the present invention includes: Module M1: Setting the sensor coordinate system { }、Geod coordinate system{ } and the rigid body reference coordinate system { }, and set a specific rotation matrix according to the transformation relationship between each coordinate system; Module M2: Set the rotation axis, combine the acceleration data, and estimate the rotation axis in the sensor coordinate system at the initial moment { } ; Module M3: Based on a specific rotation matrix, the rotation axis Perform space transformation to obtain the geodetic coordinate system { } ; Module M4: Construct a quaternion single-axis rotation angle estimation model, and convert the rotation axis The parameters of the rotation position are input into the model to obtain the estimated rotation angle; Module M5: Correct the estimated rotation angle to obtain the final estimated single-axis rotation angle.

[0025] Preferably, the module M2 comprises: Module M2.1: Let the accelerometer collect the sensor coordinate system { Acceleration data under , and according to the specific rotation matrix transformation, calculate the acceleration data measured by the accelerometer in the rigid body reference coordinate system { The corresponding vector is:

[0026] in Represents the rigid body reference coordinate system { } to sensor coordinate system { }'s rotation matrix; Module M2.2: Setting the Rotation Axis in a Rigid Body Reference Frame , the corresponding vector , analysis At the moment, the acceleration data measured by the accelerometer is in the reference frame { The corresponding vector relationship is:

[0027] Indicates winding In the rigid body reference coordinate system { Rotation The rotation matrix composed of angles, When the rigid body is at the initial moment, the rigid body reference coordinate system { }acceleration data in; Module M2.3: Calculate the difference vector of the accelerometer measurement data according to the acceleration data measured by the accelerometer multiple times at different times; Specifically, when the sensor coordinate system { }Around the axis of rotation When performing a single degree of freedom rotation, The trajectory is vertical For any time < < , the accelerometer measurement data is in the rigid body reference coordinate system { The corresponding vectors are , and ,in, , Both with the rotation axis The straight lines are perpendicular, so:

[0028] wherein is a preset constant; Module M2.4: Based on the acceleration difference vector, construct an accelerometer estimated rotation axis model, and calculate the corresponding vector of the rotation axis in the sensor coordinate system { } at the initial moment; Specifically, transform the acceleration difference vector through the rotation matrix to obtain:

[0029] Construct an accelerometer estimated rotation axis model to obtain the corresponding vector of the rotation axis in the sensor coordinate system { } at the initial moment, which is: .

[0030] Preferably, the module M3 includes: Module M3.1: When rotating from the initial position to the rotation position, set the quaternion output at the initial position as , and the quaternion output at the rotation position as ;

[0031] respectively represent each component of the initial position quaternion, respectively represent each component of the rotation position quaternion; Module M3.2: According to the quaternion at the initial moment, calculate the rotation matrix from the geodetic coordinate system { } to the sensor coordinate system { }:

[0032] Module M3.3: According to the rotation matrix and , calculate the spatial direction of the rotation axis vector in the geodetic coordinate system { , which is:

[0033] wherein, represents the component of the rotation axis on the X-axis in the geodetic coordinate system { }, represents the component of the rotation axis on the Y-axis in the geodetic coordinate system { }, represents the component of the rotation axis on the Z-axis in the geodetic coordinate system { } The component on the Z-axis below.

[0034] Preferably, the module M4 includes: Module M4.1: Analyze the corresponding relationship between the difference quaternion and the rotation angle, and construct a quaternion single-axis estimated rotation angle model, which is:

[0035] Indicates the actual rotation angle around the fixed rotation axis ; , , Are the components of the rotation axis on the three axes, satisfying ; Module M4.2: Input the parameters of the rotation axis and the rotation position quaternion into the quaternion single-axis estimated rotation angle model to form multiple estimated rotation angles; specifically,

[0036] Among them, Indicates the first estimated rotation angle, Indicates the second estimated rotation angle, Indicates the third estimated rotation angle; Module M4.3: Integrate multiple estimated rotation angles to obtain the final estimated rotation angle : .

[0037] Preferably, the module M5 includes: Module M5.1: According to the amplitude of the rotation axis component , , design an adaptive dynamic weight for correcting the deviation. Specifically,

[0038] Among them Is the adaptive threshold, which is:

[0039] Among them Is the preset smoothing coefficient, Indicates taking the median value; Is the adaptive threshold at time t, Is the adaptive threshold at time t-1; Module M5.2: Correct the estimated rotation angle according to the adaptive dynamic weight to obtain the final estimated single-axis rotation angle, which is:

[0040] wherein is a regularization parameter.

[0041] Compared with the prior art, the present invention has the following beneficial effects: 1. Different from the traditional technology that uses a visible model of the rotating axis, the present invention uses a triaxial accelerometer to estimate the position of the rotating axis, which allows the system to accurately estimate the rotation angle even under complex dynamic conditions, solving the problem of the invisible rotating axis. At the same time, the present invention can adapt to different application scenarios through multi-coordinate system collaboration and establishing spatial transformation, avoiding the influence of the sensor installation position or the initial attitude, and improving the system robustness.

[0042] 2. By deeply analyzing the conversion relationship between quaternion and axis-angle pair, the present invention effectively reduces the noise interference and improves the accuracy of data processing. This method not only enhances the system robustness but also improves the reliability in a changing environment.

[0043] 3. The present invention further corrects the error of the estimated angle, adjusts the compensation coefficients of each axis to correct the estimation deviation caused by the reciprocal operation, and improves the long-term stability. BRIEF DESCRIPTION OF THE DRAWINGS

[0044] By reading the following detailed description of the non-limiting embodiments with reference to the accompanying drawings, other features, objects, and advantages of the present invention will become more apparent: Figure 1 is a flowchart of a method for estimating the rotation angle of a single axis based on spatial transformation according to the present invention; Figure 2 is a flowchart of the method for estimating the rotation angle of a single axis based on spatial transformation in Embodiment 1 of the present invention; Figure 3 is a schematic diagram of an accelerometer for estimating the rotating axis model in Embodiment 2 of the present invention; Figure 4 is a schematic diagram of a quaternion single-axis estimated rotation angle model in Embodiment 2 of the present invention. DETAILED DESCRIPTION OF THE INVENTION

[0045] The present invention will be described in detail below with reference to specific embodiments. The following embodiments will help those skilled in the art to further understand the present invention, but do not limit the present invention in any form. It should be noted that those of ordinary skill in the art can make several changes and improvements without departing from the concept of the present invention. These all belong to the protection scope of the present invention.

[0046] The object of the present invention is to solve the problems of low accuracy and applicability only to visible rotating axes existing in the existing single-axis angle measurement method. To solve the above problems, the present invention proposes a method for estimating the rotation angle of a single axis. First, this method uses a three-axis accelerometer to estimate the position of the rotating axis. Secondly, by transforming the rotating axis in the sensor coordinate system to the earth coordinate system, the position of the rotating axis in the earth coordinate system is determined. Finally, the relationship between the quaternion and the axis-angle pair is deeply analyzed, and the rotation angle is estimated in combination with the determined rotating axis.

[0047] Combined with the attached Figure 1 A method for estimating the rotation angle of a single axis based on space transformation provided by the present invention includes: Step S1: Set the sensor coordinate system { }, the earth coordinate system { } and the rigid body reference coordinate system { }, and set a plurality of specific rotation matrices according to the transformation relationship between the coordinate systems; Step S2: Set the rotating axis, and combine the acceleration data to estimate the vector of the rotating axis in the sensor coordinate system { } at the initial moment; Step S3: Based on the specific rotation matrix, perform a space transformation on the rotating axis to obtain in the earth coordinate system { }; Step S4: Analyze the corresponding relationship between the rotating axis and the rotation angle, and construct a quaternion single-axis estimated rotation angle model according to the corresponding relationship to obtain the estimated rotation angle; Step S5: Correct the estimated rotation angle to obtain the final estimated single-axis rotation angle.

[0048] Specifically, step S2 includes: Step S2.1: Let the accelerometer collect the acceleration data in the sensor coordinate system { }, and calculate the vector corresponding to the acceleration data collected by the accelerometer in the rigid body reference coordinate system { } according to the transformation of the specific rotation matrix, which is:

[0049] where represents the rotation matrix from the rigid body reference coordinate system { } to the sensor coordinate system { }; Step S2.1: Let the accelerometer collect the acceleration data in the sensor coordinate system { , and calculate the vector corresponding to the acceleration data collected by the accelerometer in the rigid body reference coordinate system { } according to a specific rotation matrix transformation, which is:

[0050] where represents the rotation matrix from the rigid body reference coordinate system { } to the sensor coordinate system { }; Step S2.2: Set the rotation axis in the rigid body reference coordinate system, and the corresponding vector . Analyze the vector relationship corresponding to the acceleration data measured by the accelerometer at the moment in the reference system { }, which is:

[0051] represents the rotation matrix composed of rotating by angle in the rigid body reference coordinate system { }, is the acceleration data in the rigid body reference coordinate system { } when the rigid body is at the initial moment; Step S2.3: Calculate the difference vector of the accelerometer measurement data based on the acceleration data measured by the accelerometer at different times; Specifically, when the sensor coordinate system { } rotates with a single degree of freedom around the rotation axis , the trajectory of is a circle perpendicular to . For any time < < , the vectors corresponding to the accelerometer measurement data in the rigid body reference coordinate system { } are respectively , and , where , are both perpendicular to the straight line where the rotation axis is located, and it can be obtained that:

[0052] where is a preset constant; Step S2.4: Construct an accelerometer estimated rotation axis model based on the acceleration difference vector, and calculate the vector corresponding to the rotation axis in the initial moment sensor coordinate system { }; Specifically, the acceleration difference vector is transformed by a rotation matrix to obtain:

[0053] An accelerometer estimated rotation axis model is constructed to obtain the vector corresponding to the rotation axis in the sensor coordinate system { } at the initial moment, which is: .

[0054] Specifically, step S3 includes: Step S3.1: When rotating from the initial position to the rotation position, the quaternion output at the initial position is set as , and the quaternion output when moving to the rotation position is ;

[0055] respectively represent each component of the quaternion at the initial position, respectively represent each component of the quaternion at the rotation position; Step S3.2: According to the quaternion at the initial moment, calculate the rotation matrix } from the geodetic coordinate system { } to the sensor coordinate system { }:

[0056] Step S3.3: According to the rotation matrix and , calculate the spatial direction } of the rotation axis vector in the geodetic coordinate system { , which is:

[0057] where represents the component of the rotation axis on the X-axis in the geodetic coordinate system { }, represents the component of the rotation axis on the Y-axis in the geodetic coordinate system { }, represents the component of the rotation axis on the Z-axis in the geodetic coordinate system { }.

[0058] Specifically, step S4 includes: Step S4.1: Analyze the correspondence between the difference quaternion and the rotation axis and rotation angle. According to the correspondence, construct a quaternion single-axis estimated rotation angle model, which is:

[0059] Represents rotation around a fixed axis The actual rotation angle of , , The rotation axis The components on the three axes satisfy ; Step S4.2: Rotate the axis The parameters of the rotation position quaternion are input into the quaternion single-axis rotation angle estimation model to obtain multiple estimated rotation angles; specifically,

[0060] in, represents the first estimated rotation angle, represents the second estimated rotation angle, represents the third estimated rotation angle; Step S4.3: Integrate multiple estimated rotation angles to obtain the final estimated rotation angle : .

[0061] Specifically, step S5 includes: Step S5.1: According to the rotation axis component The amplitude of , design adaptive dynamic weights , used to correct deviations, specifically,

[0062] in is the adaptive threshold, which is:

[0063] in is the preset smoothing coefficient, Indicates taking the middle value; is the adaptive threshold at time t, for Adaptive threshold at the moment; Step S5.2: Based on the adaptive dynamic weight Correct the estimated rotation angle to get the final estimated single-axis rotation angle:

[0064] in is the regularization parameter.

[0065] Example 1 According to the attached Figure 2The object of the present invention is to solve the problems of low accuracy and applicability only to visible rotating axes existing in the existing single-axis angle measurement method. To solve the above problems, the present invention proposes a method for estimating the rotation angle of a single axis based on space transformation. First, this method uses a three-axis accelerometer combined with a zero-velocity detection technique to estimate the position of the rotating axis. Secondly, the rotating axis in the sensor coordinate system is transformed into the geodetic coordinate system, and thus the position of the rotating axis in the geodetic coordinate system is determined. Finally, the relationship between the quaternion and the axis-angle pair is analyzed in depth, and the rotation angle is estimated in combination with the determined rotating axis.

[0066] The core concept of the present invention is to accurately calculate the angle of rotation around a single axis through a method for estimating the rotation angle of a single axis based on space transformation. Especially in the case where it is difficult to achieve high precision by traditional methods due to the invisibility of the fixed rotating axis, the solution includes the following contents: 1 Estimation of the rotating axis: The acceleration signal in the sensor coordinate system is collected in real time by a three-axis accelerometer, and the zero-velocity detection technique is combined to identify the moment when the object is in a static state. The direction of the rotating axis in the sensor coordinate system at the initial moment is determined according to the accelerometer data at the static moment.

[0067] 2 Coordinate system transformation and rotating axis positioning: The rotating axis vector in the sensor coordinate system is transformed into the geodetic coordinate system through the initial quaternion. In the geodetic coordinate system, the limitation that the traditional method only depends on the visible rotating axis is thus solved, and it is applicable to the estimation of the rotating axis in an invisible or complex environment.

[0068] 3 Correlation analysis of quaternion and rotation angle: Precise angle estimation is achieved by analyzing the relationship between the quaternion and the axis-angle pair.

[0069] According to the above inventive concept, the present invention adopts the following technical solution: A method for estimating the rotation angle of a single axis based on space transformation, comprising the following steps: Step S1, in the measurement scenario where the sensor is fixedly connected to the rigid body, the sensor angle measurement device rotates together with the rigid body. Define the sensor coordinate system { }, the geodetic coordinate system { } and the rigid body reference coordinate system { }, which provides a unified mathematical framework for the subsequent solution of the rotating axis and the angle. In the present invention, the rotation matrix represents the transformation from the coordinate system { } to the coordinate system { }, and the rotation matrix indicates the transformation rule between the coordinate systems. Set the rotation matrix ,, represents the transformation from the rigid body reference coordinate system { } to the sensor coordinate system { Rotation matrix of {

[0070] Step S2: Construct an accelerometer estimated rotation axis model, and conduct a theoretical analysis on the spatial geometric relationship between the sensor coordinate system { } and the rigid body reference coordinate system { }, and convert the rotation axis in the rigid body reference coordinate system { } to in the initial sensor coordinate system { }, realizing the initialization of the rotation axis direction and laying a stable data foundation for subsequent rotation angle estimation.

[0071] Step S3: Use the initial quaternion to construct the rotation matrix , and through perform a spatial transformation on the rotation axis , and convert the rotation axis from the initial sensor coordinate system { } to in the geodetic coordinate system { }, ensuring the consistency of the rotation axis in the global coordinate system and enhancing the physical interpretability of the angle estimation.

[0072] Step S4: Construct a quaternion single-axis estimated rotation angle model, deeply analyze the relationship between the rotation axis and the rotation angle and the quaternion, and combine the analytical relationship between the rotation axis direction and the angle component to realize the linearized expression of the rotation angle around the fixed axis. Finally, by weighted fusion of the multi-component prediction results, while reducing the computational complexity, the estimation efficiency and accuracy are taken into account, which is suitable for embedded systems with high real-time requirements. Predict the rotation angle multiple times, predict the rotation angle according to different parameters, and make the prediction more accurate.

[0073] Furthermore, step S2 specifically includes the following steps: Step S21: Define the straight line where the rotation axis is located as the rigid body reference coordinate system. Assume that represents the rotation matrix from the rigid body reference coordinate system { } coordinate system to the initial position sensor { } coordinate system. Therefore, it can be deduced that:

[0074] According to the above formula, it can be known that the vector corresponding to the accelerometer measurement value in the rigid body reference coordinate system { } is:

[0075] Step S22. When the rigid body is in a static state (i.e., the initial moment when no deflection occurs), the spatial relationship between the sensor coordinate system { } and the rigid body reference coordinate system { } remains constant. Therefore, at this time is a fixed value. Assume that the acceleration data in the rigid body reference coordinate system { } at this time is .

[0076] Step S23. The above change process can be described by the rigid body rotation transformation theory. At this time, the vector corresponding to the accelerometer measurement value in the reference system { } is:

[0077] Step S24. When the sensor coordinate system { } rotates around the rotation axis with a single degree of freedom, obviously, as the rotation angle changes, 's trajectory is perpendicular to 's circle. Therefore, for any time , and , and , there is , and . Therefore, it can be concluded that , are both perpendicular to the straight line where the rotation axis is located. Further, it can be obtained that:

[0078] where is a constant. If the above equation is multiplied by the rotation matrix simultaneously, it can be obtained that:

[0079] Furthermore, it can be obtained that:

[0080] Step S25. From this, it can be known that in the accelerometer estimated rotation axis model, only the accelerometer measurement values of the gravitational acceleration at three different rotation direction positions are required to obtain the vector corresponding to the rotation axis in the sensor coordinate system { } at the initial moment:

[0081] Furthermore, Step S3 specifically includes the following steps: Step S31. According to the quaternion at the initial moment, the corresponding rotation matrix is obtained :

[0082] Specifically, the conversion from quaternion to rotation matrix is based on the algebraic expansion of the quaternion rotation vector. By converting quaternion multiplication into matrix multiplication, an equivalent rotation matrix is obtained.

[0083] Therefore, according to the rotation matrix it can be obtained that:

[0084] Furthermore, the specific steps of step S4 include the following steps: Step S41, construct a quaternion uniaxial estimated rotation angle model. Assume that the set axis rotates from the initial position to position 1. At the initial position, the quaternion output is , and when moving to position 1, the quaternion output is . It is defined as follows:

[0085] Step S42, in order to describe the rotational motion of the sensor coordinate system { } in the geodetic coordinate system { } around the fixed rotation axis , a difference quaternion is introduced to describe the relative relationship between two postures. The difference quaternion quantifies the rotational motion relationship of the sensor coordinate system rotating from the initial position to position 1. It is defined as:

[0086] represents the actual rotation angle around the fixed rotation axis ; , , are the components of the rotation axis on the three axes, satisfying ; Step S43, substitute the rotation axis obtained above into the above formula for simplification, and the expression of the rotation angle can be obtained:

[0087] Among them, represents the first estimated rotation angle, represents the second estimated rotation angle, represents the third estimated rotation angle; Integrate multiple estimated rotation angles to ensure that the estimated rotation angle will not deviate too much due to position deviation, and obtain the final estimated rotation angle. :

[0088] However, there will be problems with the above integration to find the average value, because the operation involving the reciprocal of the component may cause estimation deviation problems.

[0089] Step S44, according to the amplitude of the rotation axis component , , design an adaptive dynamic weight to correct the deviation. When the amplitude of the rotation axis component is less than or equal to the preset smoothing threshold , then set the adaptive dynamic weight to 0. When the amplitude of the rotation axis component is greater than the preset smoothing threshold , then calculate the adaptive dynamic weight . Dynamically adjust the contribution degree of each direction according to the amplitude of the rotation axis component, avoiding the numerical instability problem caused by the axis component approaching zero. At the same time, suppress abnormal fluctuations through the smoothing threshold , significantly improving the robustness and accuracy of the rotation angle estimation. Specifically,

[0090] where is the adaptive threshold, which is:

[0091] where is the preset smoothing coefficient, means taking the middle value; is the adaptive threshold at time t, is the adaptive threshold at time; Correct the estimated rotation angle according to the adaptive dynamic weight to obtain the final estimated single-axis rotation angle, which is:

[0092] where is the regularization parameter.

[0093] Embodiment 2 The preferred embodiment of the present invention is described in detail below with reference to the accompanying drawings: 1. Accelerometer estimates the rotation axis Step 1, as Figure 3 A in defines the straight line where the rotation axis is located The axis is a rigid body reference coordinate system. Assume that represents the rotation matrix from the coordinate system { } to the coordinate system { }. Therefore, it can be deduced that:

[0094] According to the above formula, it can be known that: Let the accelerometer collect the acceleration data in the sensor coordinate system { }, and calculate the vector corresponding to the acceleration data measured by the accelerometer in the rigid body reference coordinate system { } according to the rotation matrix. It is:

[0095] represents the rotation matrix from the rigid body reference coordinate system { } to the sensor coordinate system { }; Step 2. When the rigid body is in a static state (i.e., the initial moment when there is no deflection), the spatial relationship between the sensor coordinate system { } and the rigid body reference coordinate system { } remains constant. So at this time is a fixed value. Assume that the acceleration data in the rigid body reference coordinate system { } at this time is .

[0096] Step 3. The above change process can be described by the rigid body rotation transformation theory. At this time, the vector corresponding to the accelerometer measurement value in the reference system { } is:

[0097] Step 4. When the sensor coordinate system { } rotates around the rotation axis with a single degree of freedom, obviously as in Figure 3 B, as the rotation angle changes, the trajectory of is a circle perpendicular to . So for any time , and , and , < < , there are , and . Therefore, it can be concluded that , are both perpendicular to the straight line where the rotation axis is located. Further, it can be obtained that:

[0098] where is a constant. If the above equation is multiplied by the rotation matrix we get:

[0099] Furthermore, we get:

[0100] Step 5. Thus, it can be seen that in the accelerometer estimated rotation axis model, only the measured values of the gravitational accelerometer at three different rotation direction positions are required to obtain the corresponding vector of the rotation axis in the sensor coordinate system { } at the initial moment:

[0101] II. Quaternion single-axis estimated rotation angle In the measurement scenario where the sensor is fixedly connected to the rigid body, the sensor angle measuring device will rotate with the rigid body. Therefore, in the example, it is necessary to study the relationship between the sensor coordinate system { } and the earth coordinate system { }, as shown in Figure 4 This schematic diagram describes the spatial rotation motion process of the sensor coordinate system { } relative to the earth coordinate system { }. It can be seen from the schematic diagram that the sensor coordinate system is in three different positions in the earth coordinate system. Among them, the unit vector represents the spatial direction of the rotation axis vector in the earth coordinate system { }, represents the rotation angle from the initial position to position 1 Step 1. Construct a quaternion single-axis estimated rotation angle model. Assume that the rotation is from the initial position to position 1, and the quaternion output at the initial position is and the quaternion output when moving to position 1 is . Define as follows:

[0102] Step 2. To describe the rotation motion of the sensor coordinate system { } in the earth coordinate system { } around the fixed rotation axis , a difference quaternion is introduced to describe the relative relationship between two postures. The difference quaternion quantifies the rotation motion relationship of the sensor coordinate system from position 1 to position 2, is defined as:

[0103] Step 3. Substitute the rotation axis obtained above into the above formula for simplification, and the expression of the rotation angle can be obtained:

[0104] where represents the first estimated rotation angle, represents the second estimated rotation angle, represents the third estimated rotation angle; Integrate multiple estimated rotation angles to obtain the final estimated rotation angle :

[0105] However, there will be problems with the above integration to find the average value because the operation involving the reciprocal of the component may cause estimation deviation problems.

[0106] Step 4. According to the amplitude of the rotation axis component , design the adaptive dynamic weight . According to the amplitude of the rotation axis component , design the adaptive dynamic weight , design the adaptive dynamic weight , design the adaptive dynamic weight , which is used to correct the deviation, dynamically adjust the contribution degree of each direction according to the amplitude of the rotation axis component, avoid the numerical instability problem caused by the axis component approaching zero, and at the same time suppress abnormal fluctuations through the smoothing threshold , significantly improving the robustness and accuracy of the rotation angle estimation. Specifically,

[0107] where is the adaptive threshold, which is:

[0108] where is the preset smoothing coefficient, means taking the middle value; is the adaptive threshold at time t, is the adaptive threshold at time; Correct the estimated rotation angle according to the adaptive dynamic weight to obtain the final estimated single-axis rotation angle, which is:

[0109] where is the regularization parameter.

[0110] The present invention also provides a single-axis estimated rotation angle system based on spatial transformation. The single-axis estimated rotation angle system based on spatial transformation can be implemented by executing the process steps of the single-axis estimated rotation angle method based on spatial transformation. That is, those skilled in the art can understand the single-axis estimated rotation angle method based on spatial transformation as a preferred embodiment of the single-axis estimated rotation angle system based on spatial transformation.

[0111] A single-axis estimated rotation angle system based on spatial transformation according to the present invention includes: Module M1: Set a sensor coordinate system { }, a geodetic coordinate system { }, and a rigid body reference coordinate system { }, and set a specific rotation matrix according to the transformation relationship between the coordinate systems; Module M2: Set a rotation axis, and combine the acceleration data to estimate the vector of the rotation axis in the sensor coordinate system { } at the initial moment; Module M3: Based on the specific rotation matrix, perform a spatial transformation on the rotation axis to obtain in the geodetic coordinate system { }; Module M4: Construct a quaternion single-axis estimated rotation angle model, input the rotation axis and the parameters of the rotation position into the model to obtain a predicted rotation angle; Module M5: Correct the predicted rotation angle to obtain the final estimated single-axis rotation angle.

[0112] Specifically, Module M2 includes: Module M2.1: Let the accelerometer collect the acceleration data in the sensor coordinate system { }, and calculate the vector corresponding to the acceleration data measured by the accelerometer in the rigid body reference coordinate system { } according to the transformation of the specific rotation matrix, which is:

[0113] where represents the rotation matrix from the rigid body reference coordinate system { } to the sensor coordinate system { }; Module M2.2: Set a rotation axis in the rigid body reference coordinate system, and the corresponding vector . Analyze the vector relationship corresponding to the acceleration data measured by the accelerometer in the reference system { } at the moment as:

[0114] represents rotation around In the rigid body reference coordinate system { } rotates by an angle to form a rotation matrix. is the acceleration data in the rigid body reference coordinate system { } when the rigid body is at the initial moment; Module M2.3: Calculate the difference vector of the accelerometer measurement data based on the acceleration data measured by the accelerometer at different times; Specifically, when the sensor coordinate system { } rotates about the rotation axis by one degree of freedom, 's trajectory is a circle perpendicular to For any time < < The vectors corresponding to the accelerometer measurement data in the rigid body reference coordinate system { } are respectively , and , where , are both perpendicular to the straight line where the rotation axis is located, and it can be obtained that:

[0115] where is a preset constant; Module M2.4: Construct an accelerometer estimated rotation axis model based on the acceleration difference vector, and calculate the vector corresponding to the rotation axis in the sensor coordinate system { } at the initial moment; Specifically, convert the acceleration difference vector through the rotation matrix to get:

[0116] Construct an accelerometer estimated rotation axis model to obtain the vector corresponding to the rotation axis in the sensor coordinate system { } at the initial moment, which is: .

[0117] Specifically, Module M3 includes: Module M3.1: When rotating from the initial position to the rotation position, set the quaternion output at the initial position to , and the quaternion output when moving to the rotation position to ;

[0118] respectively represent each component of the initial position quaternion.​Each represents each component of the rotation position quaternion; Module M3.2: According to the quaternion at the initial moment , calculate the rotation matrix } from the geodetic coordinate system { } to the sensor coordinate system { :

[0119] Module M3.3: According to the rotation matrix and , calculate the spatial direction of the rotation axis vector in the geodetic coordinate system { , which is:

[0120] Among them, represents the component of the rotation axis on the X-axis in the geodetic coordinate system { }, represents the component of the rotation axis on the Y-axis in the geodetic coordinate system { }, represents the component of the rotation axis on the Z-axis in the geodetic coordinate system { }.

[0121] Specifically, Module M4 includes: Module M4.1: Analyze the correspondence between the differential quaternion and the rotation angle, and construct a quaternion single-axis estimated rotation angle model, which is:

[0122] represents the actual rotation angle around the fixed rotation axis ; , , are the components of the rotation axis on the three axes, satisfying ; Module M4.2: Input the rotation axis and the parameters of the rotation position quaternion into the quaternion single-axis estimated rotation angle model to form multiple estimated rotation angles; Specifically,

[0123] Among them, represents the first estimated rotation angle, represents the second estimated rotation angle, represents the third estimated rotation angle; Module M4.3: Integrate multiple estimated rotation angles to obtain the final estimated rotation angle : .

[0124] Specifically, module M5 includes: Module M5.1: According to the amplitude of the rotation axis component , design an adaptive dynamic weight for correcting the deviation. Specifically ,

[0125] wherein is the adaptive threshold, which is:

[0126] wherein is the preset smoothing coefficient means taking the median value; is the adaptive threshold at time t is the adaptive threshold at time t - 1; Module M5.2: According to the adaptive dynamic weight correct the estimated rotation angle to obtain the final estimated single-axis rotation angle, which is:

[0127] wherein is the regularization parameter.

[0128] Those skilled in the art know that in addition to implementing the system and its various devices, modules, and units provided by the present invention in the form of pure computer-readable program code, the method steps can be logically programmed to enable the system and its various devices, modules, and units provided by the present invention to be implemented in the form of logic gates, switches, application-specific integrated circuits, programmable logic controllers, and embedded microcontrollers, etc., to achieve the same functions. Therefore, the system and its various devices, modules, and units provided by the present invention can be regarded as a hardware component, and the devices, modules, and units included therein for implementing various functions can also be regarded as the structure within the hardware component; the devices, modules, and units for implementing various functions can also be regarded as either software modules for implementing the method or the structure within the hardware component.

[0129] The specific embodiments of the present invention have been described above. It should be understood that the present invention is not limited to the above specific embodiments, and those skilled in the art can make various changes or modifications within the scope of the claims, which does not affect the essence of the present invention. Without conflict, the embodiments of the present application and the features in the embodiments can be combined with each other arbitrarily.

Claims

1. A method for estimating the rotation angle of a single axis based on spatial transformation, characterized in that, Including: Step S1: Set up a sensor coordinate system { }, a geodetic coordinate system { }, and a rigid body reference coordinate system { }, and set multiple specific rotation matrices according to the transformation relationships between the coordinate systems; Step S2: Set the rotation axis, and estimate the vector of the rotation axis in the sensor coordinate system { } at the initial moment by combining the acceleration data ; Step S3: Based on a specific rotation matrix, let the rotation axis perform a spatial transformation to obtain under the geodetic coordinate system { }; Step S4: Parse the rotation axis The corresponding relationship with the rotation angle, and construct a quaternion single-axis estimated rotation angle model according to the corresponding relationship to obtain the estimated rotation angle; Step S5: Correct the estimated rotation angle to obtain the final estimated single-axis rotation angle.

2. The method for estimating the rotation angle of a single axis based on spatial transformation according to claim 1, wherein The said step S2 includes: Step S2.1: Let the accelerometer collect the acceleration data in the sensor coordinate system { }, and calculate the vector corresponding to the acceleration data collected by the accelerometer in the rigid body reference coordinate system { } according to a specific rotation matrix conversion, which is: } wherein represents the rotation matrix from the rigid body reference coordinate system { } to the sensor coordinate system { }; Step S2.2: Set the rotation axis in the rigid body reference coordinate system , the corresponding vector , analyze At time, the vector relationship corresponding to the acceleration data measured by the accelerometer in the reference frame { } is: Indicates rotation about in the rigid body reference coordinate system { } by an angle, the rotation matrix composed of angles, when the rigid body is at the initial moment, the acceleration data in the rigid body reference coordinate system { }; Step S2.3: Calculate the difference vector of the acceleration data measured by the accelerometer according to the acceleration data measured by the accelerometer at different times. Specifically, when the sensor coordinate system { } rotates about the rotation axis with one degree of freedom, 's trajectory is a circle perpendicular to . For any time < < , the vectors corresponding to the accelerometer measurement data in the rigid body reference coordinate system { } are respectively , and . Among them, , are both perpendicular to the straight line where the rotation axis is located, and it can be obtained that: wherein is a preset constant; Step S2.4: According to the acceleration difference vector, construct an accelerometer estimated rotation axis model and calculate the corresponding vector of the rotation axis in the sensor coordinate system { } at the initial moment; Specifically, the acceleration difference vector is transformed by the rotation matrix to obtain: Construct an accelerometer-based estimated rotation axis model to obtain the vector corresponding to the rotation axis in the sensor coordinate system { } at the initial moment, which is: 。 3. The method for estimating the rotation angle of a single axis based on spatial transformation according to claim 1, wherein The said step S3 includes: Step S3.1: When rotating from the initial position to the rotation position, set the quaternion output at the initial position as , and the quaternion output when moving to the rotation position as ; Each represents a component of the initial position quaternion, Each represents a component of the rotation position quaternion; Step S3.2: According to the quaternion at the initial moment , calculate the rotation matrix from the geodetic coordinate system { } to the sensor coordinate system { }: Step S3.3: According to the rotation matrix and , calculate the spatial direction of the rotation axis vector in the geodetic coordinate system { }, which is: as: Among them, represents the component of the rotation axis on the X-axis in the geodetic coordinate system { }, represents the component of the rotation axis on the Y-axis in the geodetic coordinate system { }, represents the component of the rotation axis on the Z-axis in the geodetic coordinate system { }.

4. The method for estimating the rotation angle of a single axis based on spatial transformation according to claim 3, characterized in that The said step S4 includes: Step S4.1: Parse the difference quaternion , the corresponding relationship between the rotation axis and the rotation angle. According to the corresponding relationship, construct a quaternion single-axis estimated rotation angle model, which is: Indicates the actual rotation angle around a fixed rotation axis ; , , are the components of the rotation axis on the three axes, satisfying ; Step S4.2: The rotation axis and the parameters of the rotation position quaternion are input into the quaternion single-axis estimated rotation angle model to obtain multiple estimated rotation angles; specifically, Among them, represents the first estimated rotation angle, represents the second estimated rotation angle, represents the third estimated rotation angle; Step S4.3: Integrate multiple estimated rotation angles to obtain the final estimated rotation angle : 。 5. The method for estimating the rotation angle of a single axis based on spatial transformation according to claim 1, wherein The said step S5 includes: Step S5.1: According to the amplitude of the rotation axis component , , design an adaptive dynamic weight for correcting the deviation. Specifically, wherein is the adaptive threshold value, which is: Among them is the preset smoothing coefficient, denotes taking the median value; is the adaptive threshold at time t, is the adaptive threshold at time Step S5.2: According to the adaptive dynamic weight correct the estimated rotation angle to obtain the final estimated single-axis rotation angle, which is: wherein is a regularization parameter.

6. A system for estimating the rotation angle of a single axis based on spatial transformation, characterized in that, Including: Module M1: Set the sensor coordinate system { }, the earth coordinate system { }, and the rigid body reference coordinate system { }, and set a specific rotation matrix according to the conversion relationship between the coordinate systems; Module M2: Set the rotation axis, combine the acceleration data, and estimate the vector of the rotation axis in the sensor coordinate system { } at the initial moment ; Module M3: Based on a specific rotation matrix, the rotation axis is subjected to a spatial transformation to obtain the in the geodetic coordinate system { }; Module M4: Build a quaternion single-axis estimated rotation angle model and input the parameters of the rotation axis and the rotation position into the model to obtain the estimated rotation angle; Module M5: Correct the estimated rotation angle to obtain the final estimated single-axis rotation angle.

7. The system for estimating the rotation angle of a single axis based on spatial transformation according to claim 6, characterized in that, The said module M2 includes: Module M2.1: Let the accelerometer collect the acceleration data in the sensor coordinate system { }, and calculate the vector corresponding to the acceleration data collected by the accelerometer in the rigid body reference coordinate system { } according to a specific rotation matrix transformation, which is: } Among them represents the rotation matrix from the rigid body reference coordinate system { } to the sensor coordinate system { }; Module M2.2: Set the axis of rotation in the rigid body reference coordinate system , the corresponding vector , analyze At time, the vector relationship corresponding to the acceleration data measured by the accelerometer in the reference frame { } is: Indicates rotation about in the rigid body reference coordinate system { } by the rotation matrix composed of the angle , and is the acceleration data in the rigid body reference coordinate system { } when the rigid body is at the initial moment; ​ Module M2.3: Calculate the difference vector of the acceleration data measured by the accelerometer according to the acceleration data measured by the accelerometer at different times. Specifically, when the sensor coordinate system { } rotates about the rotation axis with one degree of freedom, 's trajectory is a circle perpendicular to . For any time < < , the vectors corresponding to the accelerometer measurement data in the rigid body reference coordinate system { } are respectively , and . Among them, , are both perpendicular to the straight line where the rotation axis is located, and it can be obtained that: wherein is a preset constant; Module M2.4: Construct an accelerometer estimated rotation axis model based on the acceleration difference vector and calculate the corresponding vector of the rotation axis in the sensor coordinate system { } at the initial moment; Specifically, the acceleration difference vector is transformed by the rotation matrix to obtain: Construct an accelerometer-based estimated rotation axis model to obtain the vector corresponding to the rotation axis in the sensor coordinate system { } at the initial moment, which is: 。 8. The system for estimating the rotation angle of a single axis based on spatial transformation according to claim 6, wherein The said module M3 includes: Module M3.1: When rotating from the initial position to the rotational position, the quaternion output at the initial position is set to , and the quaternion output when moving to the rotational position is ; Each represents a component of the initial position quaternion, Each represents a component of the rotated position quaternion; Module M3.2: Calculate the rotation matrix from the geodetic coordinate system { } to the sensor coordinate system { } based on the quaternion at the initial moment }: : Module M3.3: According to the rotation matrix and , calculate the spatial direction of the rotation axis vector in the geodetic coordinate system { }, which is : Among them, represents the component of the rotation axis on the X-axis in the geodetic coordinate system { }, represents the component of the rotation axis on the Y-axis in the geodetic coordinate system { }, represents the component of the rotation axis on the Z-axis in the geodetic coordinate system { }.

9. The system for estimating the rotation angle of a single axis based on spatial transformation according to claim 8, wherein The said module M4 includes: Module M4.1: Parse the differential quaternion The corresponding relationship with the rotation angle, and construct a quaternion single-axis estimated rotation angle model, which is: Indicates the actual rotation angle about a fixed rotation axis ; , , are the components of the rotation axis on the three axes, satisfying ; Module M4.2: The rotation axis is input into the parameter of the rotation position quaternion to the single-axis estimation rotation angle model of the quaternion to form multiple estimated rotation angles; specifically, Among them, represents the first estimated rotation angle, represents the second estimated rotation angle, represents the third estimated rotation angle; Module M4.3: Integrate multiple estimated rotation angles to obtain the final estimated rotation angle : 。 10. The system for estimating the rotation angle of a single axis based on spatial transformation according to claim 6, wherein The said module M5 includes: Module M5.1: Based on the magnitude of the rotation axis component , , design an adaptive dynamic weight to correct the deviation. Specifically, wherein is an adaptive threshold value, which is: wherein is a preset smoothing coefficient, denotes taking the median value; is the adaptive threshold at time t, is the adaptive threshold at time t-1; Module M5.2: According to the adaptive dynamic weight Correct the estimated rotation angle to obtain the final estimated single-axis rotation angle, which is: wherein is a regularization parameter.

Images