Object attitude measurement method based on dual inertial measurement units

Abstract

The present invention relates to the technical field of object attitude measurement methods, and discloses an object attitude measurement method based on dual inertial measurement units, and the measurement method steps include: S1, fixing the spatial relative positions of two inertial measurement units and a carrier; S2, using the acceleration data of the inertial measurement units to solve the Euler angle of the carrier; S3, using a two-stage complementary filter to perform noise filtering and data fusion. This method uses the output data of two inertial sensors connected in a fixed manner to complement each other, which can solve the problem of unstable x-axis angle of a single inertial sensor; secondly, a nonlinear complementary filter is used to correct the deviation of the gyroscope online, and a linear complementary filter is used to fuse acceleration data and angular velocity data to calculate the attitude angle, which can effectively solve the phenomenon of errors in the existing attitude angle estimation, and at the same time solve the problems of complex gain parameter adjustment, cumbersome operation, and low efficiency of conventional filtering algorithms.

Description

Technical Field

[0001] The present invention relates to the technical field of object attitude measurement methods, and in particular to an object attitude measurement method based on dual inertial measurement units. Background Art

[0002] Euler angles are a mathematical method for describing the posture of a rigid body in three-dimensional space. They use three angles to describe the rotation state of an object in three-dimensional space. These three angles represent the rotation angles of the object around the three main axes (x, y, z axes) of a fixed coordinate system. By performing three rotations in a fixed order, Euler angles can fully describe the posture of an object.

[0003] The nine-axis inertial sensor is a device that combines multiple sensor elements to measure the motion and attitude information of an object. It usually contains three types of sensors, namely a three-axis accelerometer, a three-axis gyroscope, and a three-axis magnetometer, hence the name nine-axis inertial sensor. The sensor collects different motion data in real time, combines these data through a complementary filtering algorithm, calculates the Euler angle of the object, and expresses the object's attitude in real time.

[0004] However, when the existing single inertial sensor uses Euler angles to represent the posture of an object, a gimbal deadlock phenomenon will occur at a specific angle. When the two rotation axes become collinear, it will cause loss of freedom or unstable calculation. Specifically, this deadlock phenomenon usually occurs when the second Euler angle representing the rotation "posture" is close to ±90°. For example, when the fixed rotation order is z, y, x, when the y-axis angle is close to ±90°, the x-axis angle will become unstable.

[0005] At the same time, the existing conventional linear complementary filter cannot estimate the gyroscope deviation online in real time, resulting in errors in attitude angle estimation. In addition, the gain parameter adjustment of the conventional filtering algorithm is complex, the operation is cumbersome and the efficiency is low.

[0006] Therefore, it is necessary to further solve the above problems, and the applicant has proposed an object posture measurement method based on dual inertial measurement units. Summary of the invention

[0007] In view of the deficiencies in the prior art, the present invention provides an object posture measurement method based on dual inertial measurement units, which solves the problems raised by the above background technology.

[0008] To achieve the above object, the present invention provides the following technical solutions:

[0009] The object attitude measurement method based on dual inertial measurement units includes the following steps:

[0010] S1, fix the spatial relative positions of the two inertial measurement units and the carrier;

[0011] S2, using the acceleration data of the inertial measurement unit to solve the carrier Euler angle;

[0012] S3, using a two-stage complementary filter for noise filtering and data fusion;

[0013] The specific steps of step S1 are as follows:

[0014] S11. Prepare two nine-axis inertial measurement units, namely imu1 and imu2. Align the x-axis of imu1 to the right along the horizontal axis of the carrier, the y-axis to the front along the longitudinal axis of the carrier, and the z-axis to be perpendicular to the x- and y-axes and upward along the vertical axis of the carrier. After alignment, perform angle calibration so that the initial angles of the three axes of imu1 are zero. Use bolts to fix imu1 on the carrier to be measured in the above manner.

[0015] S12. Align the x-axis of the imu2 device with the y-axis of the imu1 device, and the y-axis of the imu2 device with the x-axis of the imu1 device in the opposite direction. Align the z-axis of the imu2 device with the z-axis of the imu1 device. After aligning the coordinate axes, calibrate the angle of imu2 so that the initial angle of imu2 is zero. Use bolts to fix imu2 on the carrier whose posture needs to be measured in the above manner.

[0016] As a further solution of the present invention: the specific steps of step S2 are as follows:

[0017] S21. Use the acceleration data of imu1 to calculate the Euler angle. The formula is as follows:

[0018]

[0019]

[0020] in , , is the three-axis acceleration data of imu1, is the roll angle calculated using the imu1 acceleration data, that is, the angle at which imu1 rotates around its own x-axis. is the pitch angle calculated using the imu1 acceleration data, that is, the angle at which imu1 rotates around its own y-axis;

[0021] S22, use the acceleration data of imu2 to calculate the pitch angle, the formula is as follows:

[0022]

[0023] in , , is the three-axis acceleration data of imu2, is the pitch angle calculated using the imu2 acceleration data, that is, the angle at which imu2 rotates around its own y-axis;

[0024] S23. When the y-axis angle of imu1 is close to ±90°, the x-axis and z-axis of imu1 become collinear, resulting in loss of freedom and unstable calculation, and gimbal deadlock. Since the y-axis of imu2 is in the opposite direction to the x-axis of imu1, the y-axis data of imu2 can be used to replace the x-axis data of imu1. Finally, the formula for solving the carrier attitude using acceleration is as follows:

[0025]

[0026]

[0027] in, It represents the pitch angle of the carrier calculated by the acceleration of two inertial sensors, that is, the angle of rotation of the carrier around its own x-axis. It represents the roll angle calculated by the acceleration of two inertial sensors, that is, the angle of rotation of the carrier around its own y-axis.

[0028] As a further solution of the present invention: the specific steps of step S3 are as follows:

[0029] S31, using a nonlinear filtering method to estimate angular velocity data error;

[0030] S32. Use a linear filtering algorithm to fuse the attitude angle data.

[0031] As a further solution of the present invention: the specific steps of step S31 are as follows:

[0032] The angular velocity error output by the inertial measurement unit is estimated using a nonlinear filtering method based on a proportional-integral controller. The error formula is as follows:

[0033]

[0034]

[0035] Where s is the Laplace operator, is the proportional gain, is the integral gain, and They are the angular velocity error data of the carrier's x-axis and y-axis, and They are the final carrier roll angle and pitch angle obtained by fusing the two attitude angles using complementary filters;

[0036] The ideal angular velocity data can be obtained by subtracting the angular velocity error from the average angular velocity data of imu1 and imu2. The formula is as follows:

[0037]

[0038]

[0039] in and They are the ideal angular velocity data of the x-axis and y-axis angular velocity data of the inertial measurement unit after being processed by the filtering algorithm. The x-axis angular velocity data output by imu1. It is the x-axis angular velocity data output by imu2. The y-axis angular velocity data output by imu1, It is the y-axis angular velocity data output by imu2.

[0040] As a further solution of the present invention: the specific steps of step S32 are as follows:

[0041] The attitude angle can be obtained by integrating the angular velocity, and then the linear filtering algorithm is used to fuse the attitude angle based on the angular velocity and acceleration to finally obtain the carrier attitude angle. The formula is as follows:

[0042]

[0043] .

[0044] Compared with the prior art, the invention has the following beneficial effects: the method uses the output data of two inertial sensors connected in a fixed manner to complement each other, and when the y-axis Euler angle of the first inertial sensor is close to 90°, a universal joint deadlock phenomenon occurs, and when the x-axis angle is inaccurate, the second inertial sensor data is used to replace the x-axis angle of the first inertial sensor, which can solve the problem of unstable x-axis angle of a single inertial sensor; secondly, a nonlinear complementary filter is used to correct the deviation of the gyroscope online, and a linear complementary filter is used to fuse acceleration data and angular velocity data to calculate the attitude angle, which can effectively solve the problem that the existing conventional linear complementary filter cannot estimate the deviation of the gyroscope online in real time, resulting in errors in attitude angle estimation, and at the same time solves the problems of complex gain parameter adjustment, cumbersome operation and low efficiency of conventional filtering algorithms. BRIEF DESCRIPTION OF THE DRAWINGS

[0045] Figure 1 It is a schematic diagram of the dual inertial sensors and the carrier installation position structure of the present invention;

[0046] Figure 2 It is the operation flow chart of the present invention;

[0047] Figure 3 This is a comparison diagram of the posture measurement method of the present invention and the conventional measurement method when the y-axis is rotated to 90°;

[0048] Figure 4 This is a diagram of the root mean square error of the attitude angle when the method uses different filter parameters in the simulation experiment of the present invention. DETAILED DESCRIPTION

[0049] The following will be combined with the drawings in the embodiments of the present invention to clearly and completely describe the technical solutions in the embodiments of the present invention. Obviously, the described embodiments are only part of the embodiments of the present invention, not all of the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by ordinary technicians in this field without creative work are within the scope of protection of the present invention.

[0050] Example

[0051] See also Figure 1-4 The present invention provides an object attitude measurement method based on a dual inertial measurement unit, and the measurement method comprises the following steps:

[0052] S1. Fix the relative spatial position of the two inertial measurement units and the carrier:

[0053] (1) Prepare two nine-axis inertial measurement units, namely imu1 and imu2. Align the x-axis of imu1 to the right along the horizontal axis of the carrier, the y-axis to the front along the longitudinal axis of the carrier, and the z-axis to the x- and y-axis, and upward along the vertical axis of the carrier. After alignment, perform angle calibration so that the initial angle of the three axes of imu1 is zero. Use bolts to fix imu1 on the carrier that needs to measure the attitude in the above manner;

[0054] (2) Align the x-axis of the imu2 device with the y-axis of the imu1 device, and the y-axis of the imu2 device with the x-axis of the imu1 device in the opposite direction. Align the z-axis of the imu2 device with the z-axis of the imu1 device. After aligning the coordinate axes, calibrate the angle of imu2 so that the initial angle of imu2 is zero. Use bolts to fix imu2 in the above manner on the carrier that needs to measure the posture. The final fixation of the carrier and the two imu devices is as follows: Figure 1 shown.

[0055] S2. Use the inertial measurement unit acceleration data to solve the carrier Euler angle:

[0056] (1) Use the acceleration data of imu1 to calculate the Euler angle. The formula is as follows:

[0057]

[0058]

[0059] in , , is the three-axis acceleration data of imu1, is the roll angle calculated using the imu1 acceleration data, that is, the angle at which imu1 rotates around its own x-axis. is the pitch angle calculated using the imu1 acceleration data, that is, the angle at which imu1 rotates around its own y-axis;

[0060] (2) Use the acceleration data of imu2 to calculate the pitch angle. The formula is as follows:

[0061]

[0062] in , , is the three-axis acceleration data of imu2, is the pitch angle calculated using the imu2 acceleration data, that is, the angle at which imu2 rotates around its own y-axis;

[0063] (3) When the y-axis angle of imu1 is close to ±90°, the x-axis and z-axis of imu1 become collinear, resulting in loss of freedom and unstable calculation, and gimbal deadlock. Since the y-axis of imu2 is in the opposite direction to the x-axis of imu1, the y-axis data of imu2 can be used to replace the x-axis data of imu1. Finally, the formula for solving the carrier attitude using acceleration is as follows:

[0064]

[0065]

[0066] in, It represents the pitch angle of the carrier calculated by the acceleration of two inertial sensors, that is, the angle of rotation of the carrier around its own x-axis. It represents the roll angle calculated by the acceleration of two inertial sensors, that is, the angle of rotation of the carrier around its own y-axis.

[0067] S3, using two-stage complementary filter for noise filtering and data fusion:

[0068] (1) Use nonlinear filtering method to estimate angular velocity data error:

[0069] The angular velocity error output by the inertial measurement unit is estimated using a nonlinear filtering method based on a proportional-integral controller. The error formula is as follows:

[0070]

[0071]

[0072] Where s is the Laplace operator, is the proportional gain, is the integral gain, and They are the angular velocity error data of the carrier's x-axis and y-axis, and They are the final carrier roll angle and pitch angle obtained by fusing the two attitude angles using complementary filters;

[0073] The ideal angular velocity data can be obtained by subtracting the angular velocity error from the average angular velocity data of imu1 and imu2. The formula is as follows:

[0074]

[0075]

[0076] in and They are the ideal angular velocity data of the x-axis and y-axis angular velocity data of the inertial measurement unit after being processed by the filtering algorithm. The x-axis angular velocity data output by imu1. It is the x-axis angular velocity data output by imu2. The y-axis angular velocity data output by imu1, It is the y-axis angular velocity data output by imu2.

[0077] (2) Use linear filtering algorithm to fuse attitude angle data:

[0078] The attitude angle can be obtained by integrating the angular velocity, and then the linear filtering algorithm is used to fuse the attitude angle based on the angular velocity and acceleration to finally obtain the carrier attitude angle. The formula is as follows:

[0079]

[0080] .

[0081] It should be noted that the above embodiments are only used to illustrate the technical solutions of the present invention rather than to limit it. Although the present invention has been described in detail with reference to the preferred embodiments, those skilled in the art should understand that the technical solutions of the present invention may be modified or replaced by equivalents without departing from the spirit and scope of the technical solutions of the present invention, which should all be included in the scope of the claims of the present invention.

Claims

1. An object attitude measurement method based on dual inertial measurement units, characterized in that: The measurement method steps include: S1, fix the spatial relative positions of the two inertial measurement units and the carrier; S2, using the acceleration data of the inertial measurement unit to solve the carrier Euler angle; S3, using a two-stage complementary filter for noise filtering and data fusion; The specific steps of step S1 are as follows: S11. Prepare two nine-axis inertial measurement units, namely imu1 and imu2. Align the x-axis of imu1 to the right along the horizontal axis of the carrier, the y-axis to the front along the longitudinal axis of the carrier, and the z-axis to be perpendicular to the x- and y-axes and upward along the vertical axis of the carrier. After alignment, perform angle calibration so that the initial angles of the three axes of imu1 are zero. Use bolts to fix imu1 on the carrier to be measured in the above manner. S12, make the x-axis of the imu2 device coincide with the y-axis of the imu1 device, the y-axis of the imu2 device coincide with the x-axis of the imu1 device in the opposite direction, and the z-axis of the imu2 device coincide with the z-axis of the imu1 device. After aligning the coordinate axes, calibrate the angle of imu2 so that the initial angle of imu2 is zero. Use bolts to fix imu2 on the carrier that needs to measure the posture in the above manner. The specific steps of step S3 are as follows: S31, using a nonlinear filtering method to estimate angular velocity data error; S32. Use a linear filtering algorithm to fuse the attitude angle data.

2. The object attitude measurement method based on dual inertial measurement units according to claim 1, characterized in that: The specific steps of step S2 are as follows: S21. Use the acceleration data of imu1 to calculate the Euler angle. The formula is as follows: in , , is the three-axis acceleration data of imu1, is the roll angle calculated using the imu1 acceleration data, that is, the angle at which imu1 rotates around its own x-axis. is the pitch angle calculated using the imu1 acceleration data, that is, the angle at which imu1 rotates around its own y-axis; S22, use the acceleration data of imu2 to calculate the pitch angle, the formula is as follows: in , , is the three-axis acceleration data of imu2, is the pitch angle calculated using the imu2 acceleration data, that is, the angle at which imu2 rotates around its own y-axis; S23. When the y-axis angle of imu1 is close to ±90°, the x-axis and z-axis of imu1 become collinear, resulting in loss of freedom and unstable calculation, and gimbal deadlock. Since the y-axis of imu2 is in the opposite direction to the x-axis of imu1, the y-axis data of imu2 can be used to replace the x-axis data of imu1. Finally, the formula for solving the carrier attitude using acceleration is as follows: in, It represents the pitch angle of the carrier calculated by the acceleration of two inertial sensors, that is, the angle of rotation of the carrier around its own x-axis. It represents the roll angle calculated by the acceleration of two inertial sensors, that is, the angle of rotation of the carrier around its own y-axis.

3. The object attitude measurement method based on dual inertial measurement units according to claim 1, characterized in that: The specific steps of step S31 are as follows: The angular velocity error output by the inertial measurement unit is estimated using a nonlinear filtering method based on a proportional-integral controller. The error formula is as follows: Where s is the Laplace operator, is the proportional gain, is the integral gain, and They are the angular velocity error data of the carrier's x-axis and y-axis, and They are the final carrier roll angle and pitch angle obtained by fusing the two attitude angles using complementary filters; The ideal angular velocity data can be obtained by subtracting the angular velocity error from the average angular velocity data of imu1 and imu2. The formula is as follows: in and They are the ideal angular velocity data of the x-axis and y-axis angular velocity data of the inertial measurement unit after being processed by the filtering algorithm. The x-axis angular velocity data output by imu1. It is the x-axis angular velocity data output by imu2. The y-axis angular velocity data output by imu1, It is the y-axis angular velocity data output by imu2.

4. The object attitude measurement method based on dual inertial measurement units according to claim 1, characterized in that: The specific steps of step S32 are as follows: The attitude angle can be obtained by integrating the angular velocity, and then the linear filtering algorithm is used to fuse the attitude angle based on the angular velocity and acceleration to finally obtain the carrier attitude angle. The formula is as follows: 。

Images