Method and System for Estimating Rotation Angle of a Single Axis Based on Spatial Transformation

Abstract

The present invention provides a method and system for estimating a single-axis rotation angle based on spatial transformation, comprising: step S1: setting a sensor coordinate system {#imgabs0#}, a geodetic coordinate system {#imgabs1#}, and a rigid body reference coordinate system {#imgabs2#}, and setting multiple specific rotation matrices based on the transformation relationships between the coordinate systems; step S2: setting a rotation axis, and estimating the vector #imgabs4# of the rotation axis in the sensor coordinate system {#imgabs3#} at the initial moment based on the rotation matrix; step S3: performing a spatial transformation on the rotation axis #imgabs5# based on the rotation matrix to obtain the vector #imgabs7# in the geodetic coordinate system {#imgabs6#}; step S4: constructing a quaternion single-axis rotation angle estimation model, analyzing the relationship between the quaternion, the rotation axis #imgabs8#, and the rotation angle to obtain an estimated rotation angle; and step S5: correcting the estimated rotation angle to obtain a final estimated single-axis rotation angle. The present invention solves the problems of existing single-axis angle measurement methods, such as low accuracy and applicability only to visible rotation axes.

Description

Technical Field

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

[0002] Accurately measuring angles around a fixed axis of rotation is a crucial task in precision measurement fields such as aerospace, robotics, and automotive engineering. Current solutions based on inertial measurement units (IMUs) typically require that the X or Y axis of the angular device coordinate system be strictly aligned with the rotation axis of the measured plane. However, in actual engineering applications, mechanical installation errors can lead to inconsistencies between the actual rotation axis and the assumed rotation axis. This ultimately results in a significant deviation between the actual measured angle and the true angle, posing a significant challenge to the stability of high-precision control systems.

[0003] Publication number CN109990697A discloses a magnetic angle sensor device and a method for estimating a rotation angle. The magnetic angle sensor device includes a multi-pole magnet rotatable about a rotation axis. The geometric arrangement of the multi-pole magnet is rotationally asymmetric relative 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 a combination of signals from the plurality of first magnetic field sensor elements, calculate second intermediate angle information based on a combination of signals from the plurality of 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 existing technology has problems such as magnetic field interference, being applicable only to visible axes, and difficulty in measuring angles in different coordinate systems. Therefore, a method and system for estimating single-axis rotation angles based on spatial transformation is needed. Summary of the Invention

[0005] In view of the defects in the prior art, the object of the present invention is to provide a method and system for estimating a single-axis rotation angle based on spatial transformation.

[0006] According to the present invention, a method for estimating a single-axis rotation angle based on spatial transformation is provided, comprising:

[0007] Step S1: Set the sensor coordinate system { }、Geodis coordinate system{ } and the rigid body reference coordinate system { }, and set multiple specific rotation matrices according to the transformation relationship between each coordinate system;

[0008] Step S2: Set the rotation axis and estimate the sensor coordinate system of the rotation axis at the initial moment by combining the acceleration data. } under the vector ;

[0009] Step S3: Based on a specific rotation matrix, the rotation axis Perform space transformation to obtain the geodetic coordinate system { } ;

[0010] Step S4: Analyze the rotation axis The corresponding relationship with the rotation angle is used to construct a quaternion single-axis rotation angle estimation model to obtain the estimated rotation angle;

[0011] Step S5: Correct the estimated rotation angle to obtain a final estimated single-axis rotation angle.

[0012] Preferably, the step S2 includes:

[0013] Step S2.1: Let the accelerometer collect the sensor coordinate system { Acceleration data under , and according to the specific rotation matrix transformation, calculate the acceleration data collected by the accelerometer in the rigid body reference coordinate system { The corresponding vector is:

[0014]

[0015] in Represents the rigid body reference coordinate system { } to sensor coordinate system { }'s rotation matrix;

[0016] Step S2.1: Let the accelerometer collect the sensor coordinate system { Acceleration data under , and according to the specific rotation matrix transformation, calculate the acceleration data collected by the accelerometer in the rigid body reference coordinate system { The corresponding vector is:

[0017]

[0018] in Represents the rigid body reference coordinate system { } to sensor coordinate system { }'s rotation matrix;

[0019] Step S2.2: Set the rotation axis in the rigid body reference coordinate system , the corresponding vector , analysis At the moment, the acceleration data measured by the accelerometer is in the reference frame { The corresponding vector relationship is:

[0020]

[0021] 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;

[0022] Step S2.3: Calculating a difference vector of the accelerometer measurement data based on the acceleration data measured multiple times by the accelerometer at different times;

[0023] 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:

[0024]

[0025] in is a preset constant;

[0026] Step S2.4: Based on the acceleration difference vector, construct the accelerometer estimated rotation axis model and calculate the rotation axis in the sensor coordinate system at the initial moment { }'s corresponding vector;

[0027] Specifically, the acceleration difference vector is passed through the rotation matrix Convert to:

[0028]

[0029] Construct an accelerometer to estimate the rotation axis model and obtain the rotation axis in the sensor coordinate system at the initial moment { The corresponding vector of} is:

[0030] .

[0031] Preferably, step S3 includes:

[0032] Step S3.1: When the initial position is rotated to the rotation position, the quaternion output of the initial position is set to , the quaternion output from the movement to the rotation position is ;

[0033]

[0034] Represents each component of the initial position quaternion, Represents each component of the rotation position quaternion;

[0035] Step S3.2: Based on the initial quaternion , calculate the geodetic coordinate system { } to sensor coordinate system { }'s rotation matrix :

[0036]

[0037] Step S3.3: According to the rotation matrix and , the rotation axis vector is calculated in the geodetic coordinate system { } under the space pointing ,for:

[0038]

[0039] in, Represents the rotation axis in the geodetic coordinate system { }The component on the X axis, Represents the rotation axis in the geodetic coordinate system { }The component on the Y axis, Represents the rotation axis in the geodetic coordinate system { }The component on the Z axis.

[0040] Preferably, the step S4 includes:

[0041] Step S4.1: Analyze the difference quaternion , the correspondence between the rotation axis and the rotation angle, based on the correspondence, constructs the quaternion single-axis rotation angle estimation model, which is:

[0042]

[0043] Represents rotation around a fixed axis The actual rotation angle; 、 、 The rotation axis The components on the three axes satisfy ;

[0044] 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,

[0045]

[0046] in, represents the first estimated rotation angle, represents the second estimated rotation angle, represents the third estimated rotation angle;

[0047] Step S4.3: Integrate multiple estimated rotation angles to obtain the final estimated rotation angle :

[0048] .

[0049] Preferably, step S5 includes:

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

[0051]

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

[0053]

[0054] in is the preset smoothing coefficient, Indicates taking the middle value; is the adaptive threshold at time t, for Adaptive threshold at the moment;

[0055] Step S5.2: Based on the adaptive dynamic weight Correct the estimated rotation angle to get the final estimated single-axis rotation angle:

[0056]

[0057] in is the regularization parameter.

[0058] According to the present invention, a system for estimating a single-axis rotation angle based on spatial transformation is provided, comprising:

[0059] Module M1: Setting the sensor coordinate system { }、Geodis coordinate system{ } and the rigid body reference coordinate system { }, and set a specific rotation matrix according to the transformation relationship between each coordinate system;

[0060] Module M2: Set the rotation axis, combine the acceleration data, and estimate the rotation axis in the sensor coordinate system at the initial moment { } under the vector ;

[0061] Module M3: Based on a specific rotation matrix, the rotation axis Perform space transformation to obtain the geodetic coordinate system { } ;

[0062] Module M4: Construct a quaternion single-axis rotation angle estimation model, and use the rotation axis The parameters of the rotation position are input into the model to obtain the estimated rotation angle;

[0063] Module M5: Correct the estimated rotation angle to obtain the final estimated single-axis rotation angle.

[0064] Preferably, the module M2 includes:

[0065] Module M2.1: Let the accelerometer collect 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:

[0066]

[0067] in Represents the rigid body reference coordinate system { } to sensor coordinate system { }'s rotation matrix;

[0068] 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:

[0069]

[0070] 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;

[0071] Module M2.3: Calculate the difference vector of the accelerometer measurement data based on the acceleration data measured multiple times by the accelerometer at different times;

[0072] 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:

[0073]

[0074] in is a preset constant;

[0075] Module M2.4: Based on the acceleration difference vector, construct the accelerometer estimation rotation axis model and calculate the rotation axis in the sensor coordinate system at the initial moment { }'s corresponding vector;

[0076] Specifically, the acceleration difference vector is passed through the rotation matrix Convert to:

[0077]

[0078] Construct an accelerometer to estimate the rotation axis model and obtain the rotation axis in the sensor coordinate system at the initial moment { The corresponding vector of} is:

[0079] .

[0080] Preferably, the module M3 includes:

[0081] Module M3.1: When the initial position is rotated to the rotation position, the quaternion output of the initial position is set to , the quaternion output from the movement to the rotation position is ;

[0082]

[0083] Represents each component of the initial position quaternion, Represents each component of the rotation position quaternion;

[0084] Module M3.2: Based on the initial quaternion , calculate the geodetic coordinate system { } to sensor coordinate system { }'s rotation matrix :

[0085]

[0086] Module M3.3: Based on the rotation matrix and , the rotation axis vector is calculated in the geodetic coordinate system { } under the space pointing ,for:

[0087]

[0088] in, Represents the rotation axis in the geodetic coordinate system { }The component on the X axis, Represents the rotation axis in the geodetic coordinate system { }The component on the Y axis, Represents the rotation axis in the geodetic coordinate system { }The component on the Z axis.

[0089] Preferably, the module M4 includes:

[0090] Module M4.1: Analytical Difference Quaternions The corresponding relationship with the rotation angle, and the quaternion single-axis estimation rotation angle model is constructed as follows:

[0091]

[0092] Represents rotation around a fixed axis The actual rotation angle; 、 、 The rotation axis The components on the three axes satisfy ;

[0093] Module M4.2: Rotating axis The parameters of the rotation position quaternion are input into the quaternion single-axis rotation angle estimation model to form multiple estimated rotation angles; specifically,

[0094]

[0095] in, represents the first estimated rotation angle, represents the second estimated rotation angle, represents the third estimated rotation angle;

[0096] Module M4.3: Integrate multiple estimated rotation angles to obtain the final estimated rotation angle :

[0097] .

[0098] Preferably, the module M5 includes:

[0099] Module M5.1: According to the components of the rotation axis The amplitude of , design adaptive dynamic weights , used to correct deviations, specifically,

[0100]

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

[0102]

[0103] in is the preset smoothing coefficient, Indicates taking the middle value; is the adaptive threshold at time t, is the adaptive threshold at time t-1;

[0104] Module M5.2: Adaptive dynamic weighting Correct the estimated rotation angle to get the final estimated single-axis rotation angle:

[0105]

[0106] in is the regularization parameter.

[0107] Compared with the prior art, the present invention has the following beneficial effects:

[0108] 1. Unlike conventional technologies that rely on a visible rotation axis model, this invention uses a triaxial accelerometer to estimate the position of the rotation axis. This allows the system to accurately estimate the rotation angle even under complex dynamic conditions, resolving the issue of invisible rotation axes. Furthermore, by collaborating across multiple coordinate systems and establishing spatial transformations, this invention adapts to diverse application scenarios, avoids the influence of sensor installation position or initial posture, and improves system robustness.

[0109] 2. By deeply analyzing the conversion relationship between quaternions and axis-angle pairs, the present invention effectively reduces noise interference and improves the accuracy of data processing. This method not only enhances the robustness of the system, but also improves its reliability in changing environments.

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

[0111] Other features, objects and advantages of the present invention will become more apparent upon reading the detailed description of non-limiting embodiments with reference to the following drawings:

[0112] Figure 1 This is a flow chart of a method for estimating a single-axis rotation angle based on spatial transformation according to the present invention;

[0113] Figure 2 This is a flowchart of a method for estimating a single-axis rotation angle based on spatial transformation according to embodiment 1 of the present invention;

[0114] Figure 3 Schematic diagram of a model for estimating the rotation axis of an accelerometer according to embodiment 2 of the present invention;

[0115] Figure 4 Schematic diagram of a quaternion single-axis rotation angle estimation model according to embodiment 2 of the present invention. DETAILED DESCRIPTION

[0116] The present invention will be described in detail below with reference to specific embodiments. The following examples will help those skilled in the art to further understand the present invention, but are not intended to limit the present invention in any form. It should be noted that, for those skilled in the art, several changes and improvements can be made without departing from the scope of the present invention. These all fall within the scope of protection of the present invention.

[0117] The present invention aims to address the problems of existing single-axis angle measurement methods, such as low accuracy and applicability only to visible rotation axes. To address these issues, the present invention proposes a single-axis rotation angle estimation method. First, the method uses a three-axis accelerometer to estimate the position of the rotation axis. Second, the rotation axis in the sensor coordinate system is transformed into the earth coordinate system to determine the position of the rotation axis in the earth coordinate system. Finally, the relationship between quaternions and axis-angle pairs is thoroughly analyzed, and the rotation angle is estimated based on the determined rotation axis.

[0118] Combined with attachment Figure 1 The present invention provides a method for estimating a single-axis rotation angle based on spatial transformation, comprising:

[0119] Step S1: Set the sensor coordinate system { }、Geodis coordinate system{ } and the rigid body reference coordinate system { }, and set multiple specific rotation matrices according to the transformation relationship between each coordinate system;

[0120] Step S2: Set the rotation axis and estimate the sensor coordinate system of the rotation axis at the initial moment by combining the acceleration data. } under the vector ;

[0121] Step S3: Based on a specific rotation matrix, the rotation axis Perform space transformation to obtain the geodetic coordinate system { } ;

[0122] Step S4: Analyze the rotation axis The corresponding relationship with the rotation angle is used to construct a quaternion single-axis rotation angle estimation model to obtain the estimated rotation angle;

[0123] Step S5: Correct the estimated rotation angle to obtain a final estimated single-axis rotation angle.

[0124] Specifically, step S2 includes:

[0125] Step S2.1: Let the accelerometer collect the sensor coordinate system { Acceleration data under , and according to the specific rotation matrix transformation, calculate the acceleration data collected by the accelerometer in the rigid body reference coordinate system { The corresponding vector is:

[0126]

[0127] in Represents the rigid body reference coordinate system { } to sensor coordinate system { }'s rotation matrix;

[0128] Step S2.1: Let the accelerometer collect the sensor coordinate system { Acceleration data under , and according to the specific rotation matrix transformation, calculate the acceleration data collected by the accelerometer in the rigid body reference coordinate system { The corresponding vector is:

[0129]

[0130] in Represents the rigid body reference coordinate system { } to sensor coordinate system { }'s rotation matrix;

[0131] Step S2.2: Set the rotation axis in the rigid body reference coordinate system , the corresponding vector , analysis At the moment, the acceleration data measured by the accelerometer is in the reference frame { The corresponding vector relationship is:

[0132]

[0133] 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;

[0134] Step S2.3: Calculating a difference vector of the accelerometer measurement data based on the acceleration data measured multiple times by the accelerometer at different times;

[0135] 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:

[0136]

[0137] in is a preset constant;

[0138] Step S2.4: Based on the acceleration difference vector, construct the accelerometer estimated rotation axis model and calculate the rotation axis in the sensor coordinate system at the initial moment { }'s corresponding vector;

[0139] Specifically, the acceleration difference vector is passed through the rotation matrix Convert to:

[0140]

[0141] Construct an accelerometer to estimate the rotation axis model and obtain the rotation axis in the sensor coordinate system at the initial moment { The corresponding vector of} is:

[0142] .

[0143] Specifically, step S3 includes:

[0144] Step S3.1: When the initial position is rotated to the rotation position, the quaternion output of the initial position is set to , the quaternion output from the movement to the rotation position is ;

[0145]

[0146] Represents each component of the initial position quaternion, Represents each component of the rotation position quaternion;

[0147] Step S3.2: Based on the initial quaternion , calculate the geodetic coordinate system { } to sensor coordinate system { }'s rotation matrix :

[0148]

[0149] Step S3.3: According to the rotation matrix and , the rotation axis vector is calculated in the geodetic coordinate system { } under the space pointing ,for:

[0150]

[0151] in, Represents the rotation axis in the geodetic coordinate system { }The component on the X axis, Represents the rotation axis in the geodetic coordinate system { }The component on the Y axis, Represents the rotation axis in the geodetic coordinate system { }The component on the Z axis.

[0152] Specifically, step S4 includes:

[0153] Step S4.1: Analyze the difference quaternion , the correspondence between the rotation axis and the rotation angle, based on the correspondence, constructs the quaternion single-axis rotation angle estimation model, which is:

[0154]

[0155] Represents rotation around a fixed axis The actual rotation angle; 、 、 The rotation axis The components on the three axes satisfy ;

[0156] 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,

[0157]

[0158] in, represents the first estimated rotation angle, represents the second estimated rotation angle, represents the third estimated rotation angle;

[0159] Step S4.3: Integrate multiple estimated rotation angles to obtain the final estimated rotation angle :

[0160] .

[0161] Specifically, step S5 includes:

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

[0163]

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

[0165]

[0166] in is the preset smoothing coefficient, Indicates taking the middle value; is the adaptive threshold at time t, for Adaptive threshold at the moment;

[0167] Step S5.2: Based on the adaptive dynamic weight Correct the estimated rotation angle to get the final estimated single-axis rotation angle:

[0168]

[0169] in is the regularization parameter.

[0170] Example 1

[0171] According to the attached Figure 2 The present invention aims to address the problems of existing single-axis angle measurement methods, such as low accuracy and applicability only to visible rotation axes. To address these issues, the present invention proposes a single-axis rotation angle estimation method based on spatial transformation. First, the method uses a three-axis accelerometer combined with zero-velocity detection technology to estimate the position of the rotation axis. Second, the rotation axis in the sensor coordinate system is transformed into the geodetic coordinate system, thereby determining the position of the rotation axis in the geodetic coordinate system. Finally, the relationship between quaternions and axis-angle pairs is thoroughly analyzed, and the rotation angle is estimated based on the determined rotation axis.

[0172] The core concept of this invention is to accurately calculate the angle of rotation around a single axis through a single-axis rotation angle estimation method based on spatial transformation. This is especially true when traditional methods have difficulty achieving high accuracy due to the lack of visibility of the fixed rotation axis. This solution includes the following aspects:

[0173] 1. Estimation of the rotation axis: A three-axis accelerometer is used to collect acceleration signals in the sensor coordinate system in real time. Zero-velocity detection technology is used to identify when the object is stationary. The accelerometer data at the stationary moment is used to determine the direction of the rotation axis in the sensor coordinate system at the initial moment.

[0174] 2 Coordinate system conversion and rotation axis positioning: The rotation axis vector in the sensor coordinate system is converted to the earth coordinate system through the initial quaternion. In the earth coordinate system, this solves the limitation of traditional methods that only rely on visible rotation axes and is suitable for rotation axis estimation in invisible or complex environments.

[0175] 3 Correlation analysis between quaternions and rotation angles: Accurate angle estimation is achieved by analyzing the relationship between quaternions and axis-angle pairs.

[0176] According to the above-mentioned inventive concept, the present invention adopts the following technical solutions:

[0177] A method for estimating a single-axis rotation angle based on spatial transformation includes the following steps:

[0178] Step S1: In the measurement scenario where the sensor is fixed to a rigid body, the sensor angle measurement device rotates with the rigid body, and the sensor coordinate system is defined as { }、Geodis coordinate system{ } and the rigid body reference coordinate system { }, which provides a unified mathematical framework for the subsequent calculation of rotation axis and angle. In this invention, the rotation matrix is defined as Represents the coordinate system { }Convert to coordinate system{ }, the rotation matrix indicates the transformation rules between coordinate systems. Set the rotation matrix , Represents the rigid body reference coordinate system { } to sensor coordinate system { }'s rotation matrix.

[0179] Step S2: Construct an accelerometer estimation rotation axis model and calculate the sensor coordinate system { } and the rigid body reference coordinate system { } to carry out theoretical analysis of the spatial geometric relationship, and the rigid body reference coordinate system { } under the axis of rotation Transform to the initial sensor coordinate system { } , which realizes the initialization of the rotation axis direction and lays a stable data foundation for the subsequent rotation angle estimation.

[0180] Step S3, using the initial quaternion Constructing the rotation matrix ,pass Rotation axis Perform space transformation and rotate the axis From the initial sensor coordinate system { }Convert to 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.

[0181] Step S4: Construct a quaternion single-axis rotation angle estimation model and conduct in-depth analysis of the rotation axis. and rotation angle The relationship between quaternions and the analytical relationship between the rotation axis direction and angle components enables a linear expression of the rotation angle around a fixed axis. Ultimately, by weighted fusion of multi-component estimation results, this method reduces computational complexity while balancing estimation efficiency and accuracy, making it suitable for embedded systems with high real-time requirements. Multiple rotation angle estimates, based on different parameters, enhance accuracy.

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

[0183] Step S21, define the rotation axis The straight line Axis is the rigid body reference coordinate system, assuming Represents the rigid body reference coordinate system { }Coordinate system to initial position sensor{ }The rotation matrix of the coordinate system can be derived:

[0184]

[0185] According to the above formula, the accelerometer measurement value in the rigid body reference coordinate system { The corresponding vector is:

[0186]

[0187] Step S22: When the rigid body is in a stationary state (i.e., at the initial moment when no deflection occurs), the sensor coordinate system { }Relative to the rigid body reference coordinate system{ }The spatial relationship remains constant, so at this time is a constant value, assuming that at this time in the rigid body reference coordinate system { The acceleration data in} is .

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

[0189]

[0190] Step S24, when the sensor coordinate system { When performing single-degree-of-freedom rotation around the axis of rotation, it is obvious that as the rotation angle changes, The trajectory is vertical So for any time 、 and ,and ,have 、 and , so it can be concluded that 、 Both with the rotation axis The straight lines are perpendicular, and we can further conclude that:

[0191]

[0192] in is a constant. If the above equation is multiplied by the rotation matrix We can get:

[0193]

[0194] Then we can get:

[0195]

[0196] Step S25: It can be seen that in the accelerometer estimation rotation axis model, only three gravity accelerometer measurement values at different rotation directions are needed to obtain the rotation axis in the sensor coordinate system at the initial moment { }'s corresponding vector:

[0197]

[0198] Furthermore, step S3 specifically includes the following steps:

[0199] Step S31, according to the initial moment quaternion Get the corresponding rotation matrix :

[0200]

[0201] 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.

[0202] So according to the rotation matrix It can be concluded that:

[0203]

[0204] Furthermore, step S4 specifically includes the following steps:

[0205] Step S41, construct a quaternion single-axis rotation angle estimation model. Assume that the axis is set to rotate from the initial position to position 1. The quaternion output at the initial position is , when it moves to position 1, the output quaternion is . It is defined as follows:

[0206]

[0207] Step S42, in order to describe the sensor coordinate system { }In the geodetic coordinate system{ } around a fixed axis of rotation The rotational motion of the two postures is described by the difference quaternion. The rotational motion relationship of the sensor coordinate system from the initial position to position 1 is quantified. Defined as:

[0208]

[0209] Represents rotation around a fixed axis The actual rotation angle; 、 、 The rotation axis The components on the three axes satisfy ;

[0210] Step S43: the rotation axis obtained above is Substituting the above formula for simplification, we can get the expression of the rotation angle:

[0211]

[0212] in, represents the first estimated rotation angle, represents the second estimated rotation angle, represents the third estimated rotation angle;

[0213] Integrate multiple estimated rotation angles so that the estimated rotation angle will not deviate too much due to position deviation, and obtain the final estimated rotation angle :

[0214]

[0215] However, there will be problems with the above integration and averaging, because the operation involving the reciprocal of the components may cause estimation bias.

[0216] Step S44, according to the rotation axis component The amplitude of , design adaptive dynamic weights , used to correct deviation, when the rotation axis component The amplitude is less than or equal to the preset smoothing threshold When , let the adaptive dynamic weight is 0, when the rotation axis component The amplitude is greater than the preset smoothing threshold When , the adaptive dynamic weight is calculated The contribution of each direction is dynamically adjusted according to the amplitude of the rotation axis component, avoiding the numerical instability problem caused by the axis component approaching zero, and at the same time, the smoothing threshold is used to adjust the contribution of each direction according to the amplitude of the rotation axis component. Abnormal fluctuations are suppressed, significantly improving the robustness and accuracy of rotation angle estimation. Specifically,

[0217]

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

[0219]

[0220] in is the preset smoothing coefficient, Indicates taking the middle value; is the adaptive threshold at time t, for Adaptive threshold at the moment;

[0221] According to the adaptive dynamic weight Correct the estimated rotation angle to get the final estimated single-axis rotation angle:

[0222]

[0223] in is the regularization parameter.

[0224] Example 2

[0225] The preferred embodiments of the present invention are described in detail below with reference to the accompanying drawings:

[0226] An accelerometer estimates the rotation axis

[0227] Step 1, such as Figure 3 A defines the rotation axis The straight line Axis is the rigid body reference coordinate system, assuming express{ } coordinate system to { }The rotation matrix of the coordinate system can be derived:

[0228]

[0229] According to the above formula, we can know that: let the accelerometer acquisition sensor coordinate system { }, and according to the rotation matrix transformation, calculate the acceleration data measured by the accelerometer in the rigid body reference coordinate system { The corresponding vector is:

[0230]

[0231] express{ }Rigid body reference coordinate system to { }Rotation matrix of sensor coordinate system;

[0232] Step 2: When the rigid body is at rest (i.e., at the initial moment when no deflection occurs), the sensor coordinate system { }Relative to the rigid body reference coordinate system{ }The spatial relationship remains constant, so at this time is a constant value, assuming that at this time in the rigid body reference coordinate system { The acceleration data in} is .

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

[0234]

[0235] Step 4, when the sensor coordinate system { When performing single degree of freedom rotation around the axis of rotation, it is obvious that Figure 3 In the middle B, as the rotation angle changes, The trajectory is vertical So for any time 、 and ,and , < < ,have 、 and , so it can be concluded that 、 Both with the rotation axis The straight lines are perpendicular, and we can further conclude that:

[0236]

[0237] in is a constant. If the above equation is multiplied by the rotation matrix We can get:

[0238]

[0239] Then we can get:

[0240]

[0241] Step 5: From this, we can see that in the accelerometer estimation rotation axis model, only three gravity accelerometer measurements at different rotation directions are needed to obtain the rotation axis in the sensor coordinate system at the initial moment { }'s corresponding vector:

[0242]

[0243] Two-quaternion single-axis rotation angle estimation

[0244] In the measurement scenario where the sensor is fixed to a rigid body, the sensor angle measurement device will rotate along with the rigid body, so in the example, the sensor coordinate system { } and the geodetic coordinate system { } to study the relationship between Figure 4 The schematic diagram shows the sensor coordinate system { }relative to the geodetic coordinate system{ The spatial rotation process of} can be seen from the schematic diagram that the sensor coordinate system is located in three different positions in the geodetic coordinate system. It represents the rotation axis vector in the geodetic coordinate system { } under the space pointing, It represents the degree of rotation from the initial position to position 1

[0245] Step 1: Construct a quaternion single-axis rotation angle estimation model. Assume that the quaternion is rotated from the initial position to position 1. The output quaternion at the initial position is , when it moves to position 1, the output quaternion is . It is defined as follows:

[0246]

[0247] Step 2, in order to describe the sensor coordinate system { }In the geodetic coordinate system{ } around a fixed axis of rotation The rotational motion of the two postures is described by the difference quaternion. The rotational motion relationship of the sensor coordinate system from position 1 to position 2 is quantified. Defined as:

[0248]

[0249] Step 3: The rotation axis obtained above Substituting the above formula for simplification, we can get the expression of the rotation angle:

[0250]

[0251] in, represents the first estimated rotation angle, represents the second estimated rotation angle, represents the third estimated rotation angle;

[0252] Integrate multiple estimated rotation angles to get the final estimated rotation angle :

[0253]

[0254] However, there will be problems with the above integration and averaging, because the operation involving the reciprocal of the components may cause estimation bias.

[0255] Step 4: According to the rotation axis component The amplitude of , design adaptive dynamic weights , according to the rotation axis component The amplitude of , design adaptive dynamic weights , used to correct the deviation, dynamically adjust the contribution 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 through the smoothing threshold Abnormal fluctuations are suppressed, significantly improving the robustness and accuracy of rotation angle estimation. Specifically,

[0256]

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

[0258]

[0259] in is the preset smoothing coefficient, Indicates taking the middle value; is the adaptive threshold at time t, for Adaptive threshold at the moment;

[0260] According to the adaptive dynamic weight Correct the estimated rotation angle to get the final estimated single-axis rotation angle:

[0261]

[0262] in is the regularization parameter.

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

[0264] According to the present invention, a system for estimating a single-axis rotation angle based on space conversion is provided, comprising: module M1: setting a sensor coordinate system { }、Geodis coordinate system{ } and the rigid body reference coordinate system { }, and set a specific rotation matrix according to the conversion 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 { } under the vector ; 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 use the rotation axis The parameters of the rotation position are input into the model to obtain the estimated rotation angle; Module M5: corrects the estimated rotation angle to obtain the final estimated single-axis rotation angle.

[0265] Specifically, module M2 includes: 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:

[0266]

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

[0268]

[0269] 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;

[0270] Module M2.3: Calculate the difference vector of the accelerometer measurement data based on the acceleration data measured multiple times by the accelerometer at different times;

[0271] 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:

[0272]

[0273] in is a preset constant;

[0274] Module M2.4: Based on the acceleration difference vector, construct the accelerometer estimation rotation axis model and calculate the rotation axis in the sensor coordinate system at the initial moment { }'s corresponding vector;

[0275] Specifically, the acceleration difference vector is passed through the rotation matrix Convert to:

[0276]

[0277] Construct an accelerometer to estimate the rotation axis model and obtain the rotation axis in the sensor coordinate system at the initial moment { The corresponding vector of} is:

[0278] .

[0279] Specifically, module M3 includes: Module M3.1: When the initial position is rotated to the rotation position, the quaternion output of the initial position is set to , the quaternion output from the movement to the rotation position is ;

[0280]

[0281] Represents each component of the initial position quaternion, Represents each component of the rotation position quaternion; Module M3.2: According to the initial moment quaternion , calculate the geodetic coordinate system { } to sensor coordinate system { }'s rotation matrix :

[0282]

[0283] Module M3.3: Based on the rotation matrix and , the rotation axis vector is calculated in the geodetic coordinate system { } under the space pointing ,for:

[0284]

[0285] in, Represents the rotation axis in the geodetic coordinate system { }The component on the X axis, Represents the rotation axis in the geodetic coordinate system { }The component on the Y axis, Represents the rotation axis in the geodetic coordinate system { }The component on the Z axis.

[0286] Specifically, module M4 includes: Module M4.1: Analytical difference quaternion The corresponding relationship with the rotation angle, and the quaternion single-axis estimation rotation angle model is constructed as follows:

[0287]

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

[0289]

[0290] in, 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 :

[0291] .

[0292] Specifically, module M5 includes: Module M5.1: According to the rotation axis component The amplitude of , design adaptive dynamic weights , used to correct deviations, specifically,

[0293]

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

[0295]

[0296] in is the preset smoothing coefficient, Indicates taking the middle value; is the adaptive threshold at time t, is the adaptive threshold at time t-1; Module M5.2: Based on the adaptive dynamic weight Correct the estimated rotation angle to get the final estimated single-axis rotation angle:

[0297]

[0298] in is the regularization parameter.

[0299] Those skilled in the art will appreciate that, in addition to implementing the system and its various devices, modules, and units provided by the present invention in purely computer-readable program code, it is entirely possible to implement the same functions of the system and its various devices, modules, and units provided by the present invention in the form of logic gates, switches, application-specific integrated circuits, programmable logic controllers, and embedded microcontrollers by logically programming the method steps. Therefore, the system and its various devices, modules, and units provided by the present invention can be considered a hardware component, and the devices, modules, and units included therein for implementing various functions can also be considered as structures within the hardware component; the devices, modules, and units for implementing various functions can also be considered as both software modules implementing the method and structures within the hardware component.

[0300] The above describes specific embodiments of the present invention. It should be understood that the present invention is not limited to the specific embodiments described above, and those skilled in the art may make various changes or modifications within the scope of the claims, which do not affect the essence of the present invention. The embodiments of this application and the features in the embodiments may be combined with each other in any manner unless there is a conflict.

Claims

1. A method for estimating a single-axis rotation angle based on spatial transformation, characterized in that: include: Step S1: Set the sensor coordinate system { }、Geodis coordinate system{ } and the rigid body reference coordinate system { }, and set multiple specific rotation matrices according to the transformation relationship between each coordinate system; Step S2: Set the rotation axis and estimate the sensor coordinate system of the rotation axis at the initial moment by combining the acceleration data. } under the vector ; Step S3: Based on a specific rotation matrix, the rotation axis Perform space transformation to obtain the geodetic coordinate system { } ; Step S4: Analyze the rotation axis The corresponding relationship with the rotation angle is used to construct a quaternion single-axis rotation angle estimation model to obtain the estimated rotation angle; Step S5: Correct the estimated rotation angle to obtain the final estimated single-axis rotation angle; The step S5 comprises: Step S5.1: According to the rotation axis component The amplitude of , design adaptive dynamic weights , used to correct deviations, specifically, in is the adaptive threshold, which is: 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: in is the regularization parameter, Represents each component of the difference quaternion.

2. The method for estimating a single-axis rotation angle based on spatial transformation according to claim 1, wherein: The step S2 comprises: Step S2.1: Let the accelerometer collect the sensor coordinate system { Acceleration data under , and according to the specific rotation matrix transformation, calculate the acceleration data collected by the accelerometer in the rigid body reference coordinate system { The corresponding vector is: in Represents the rigid body reference coordinate system { } to sensor coordinate system { }'s rotation matrix; Step S2.2: Set the rotation axis in the rigid body reference coordinate system , the corresponding vector , analysis At this moment, the acceleration data measured by the accelerometer is in the rigid body reference coordinate system { The corresponding vector relationship is: 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; Step S2.3: Calculating a difference vector of the accelerometer measurement data based on the acceleration data measured multiple times by the accelerometer 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: in is a preset constant; Step S2.4: Based on the acceleration difference vector, construct the accelerometer estimated rotation axis model and calculate the rotation axis in the sensor coordinate system at the initial moment { }'s corresponding vector; Specifically, the acceleration difference vector is passed through the rotation matrix Convert to: Construct an accelerometer to estimate the rotation axis model and obtain the rotation axis in the sensor coordinate system at the initial moment { The corresponding vector of} is: 。 3. The method for estimating a single-axis rotation angle based on spatial transformation according to claim 1, wherein: The step S3 comprises: Step S3.1: When the initial position is rotated to the rotation position, the quaternion output of the initial position is set to , the quaternion output from the movement to the rotation position is ; Represents each component of the initial position quaternion, Represents each component of the rotation position quaternion; Step S3.2: Based on the initial quaternion , calculate the geodetic coordinate system { } to sensor coordinate system { }'s rotation matrix : Step S3.3: According to the rotation matrix and , the rotation axis vector is calculated in the geodetic coordinate system { } under the space pointing ,for: in, Represents the rotation axis in the geodetic coordinate system { }The component on the X axis, Represents the rotation axis in the geodetic coordinate system { }The component on the Y axis, Represents the rotation axis in the geodetic coordinate system { }The component on the Z axis.

4. The method for estimating a single-axis rotation angle based on spatial transformation according to claim 3, wherein: The step S4 comprises: Step S4.1: Analyze the difference quaternion , the correspondence between the rotation axis and the rotation angle, based on the correspondence, constructs the quaternion single-axis rotation angle estimation model, which is: Represents rotation around a fixed axis The actual rotation angle; 、 、 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, 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 : 。 5. A system for estimating a single-axis rotation angle based on spatial transformation, characterized in that: include: Module M1: Setting the sensor coordinate system { }、Geodis 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 { } under the vector ; 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 use 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; The module M5 includes: Module M5.1: According to the components of the rotation axis The amplitude of , design adaptive dynamic weights , used to correct deviations, specifically, Among them is the adaptive threshold, which is: in is the preset smoothing coefficient, Indicates taking the middle value; is the adaptive threshold at time t, is the adaptive threshold at time t-1; Module M5.2: Adaptive dynamic weighting Correct the estimated rotation angle to get the final estimated single-axis rotation angle: in is the regularization parameter, Represents each component of the difference quaternion.

6. The system for estimating a single-axis rotation angle based on spatial transformation according to claim 5, characterized in that: The module M2 includes: Module M2.1: Let the accelerometer collect sensor coordinate system { Acceleration data under , and according to the specific rotation matrix transformation, calculate the acceleration data collected by the accelerometer in the rigid body reference coordinate system { The corresponding vector is: 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 this moment, the acceleration data measured by the accelerometer is in the rigid body reference coordinate system { The corresponding vector relationship is: 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 based on the acceleration data measured multiple times by the accelerometer 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: in is a preset constant; Module M2.4: Based on the acceleration difference vector, construct the accelerometer estimation rotation axis model and calculate the rotation axis in the sensor coordinate system at the initial moment { }'s corresponding vector; Specifically, the acceleration difference vector is passed through the rotation matrix Convert to: Construct an accelerometer to estimate the rotation axis model and obtain the rotation axis in the sensor coordinate system at the initial moment { The corresponding vector of} is: 。 7. The system for estimating a single-axis rotation angle based on spatial transformation according to claim 5, characterized in that: The module M3 includes: Module M3.1: When the initial position is rotated to the rotation position, the quaternion output of the initial position is set to , the quaternion output from the movement to the rotation position is ; Represents each component of the initial position quaternion, Represents each component of the rotation position quaternion; Module M3.2: Based on the initial quaternion , calculate the geodetic coordinate system { } to sensor coordinate system { }'s rotation matrix : Module M3.3: Based on the rotation matrix and , the rotation axis vector is calculated in the geodetic coordinate system { } under the space pointing ,for: in, Represents the rotation axis in the geodetic coordinate system { }The component on the X axis, Represents the rotation axis in the geodetic coordinate system { }The component on the Y axis, Represents the rotation axis in the geodetic coordinate system { }The component on the Z axis.

8. The system for estimating a single-axis rotation angle based on spatial transformation according to claim 7, characterized in that: The module M4 includes: Module M4.1: Analytical Difference Quaternions The corresponding relationship with the rotation angle, and the quaternion single-axis estimation rotation angle model is constructed as follows: Represents rotation around a fixed axis The actual rotation angle; 、 、 The rotation axis The components on the three axes satisfy ; Module M4.2: Rotating axis The parameters of the rotation position quaternion are input into the quaternion single-axis rotation angle estimation model to form multiple estimated rotation angles; specifically, in, 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 : 。

Images