IMU (Inertial Measurement Unit) algorithm design method for holder carrying friction wheel launching mechanism

Abstract

The invention discloses an I-MU algorithm design method of a holder carrying a friction wheel launching mechanism. The I-MU algorithm design method comprises the following steps: S1, testing and selecting an attitude algorithm; s2, calibrating an IMU (Inertial Measurement Unit); s3, judging whether the data of the magnetometer are fused or not; s4, designing and selecting a low-pass filter; and S5, verifying the overall performance. The scheme designed by the invention is fused with the movement of the pan-tilt motor and the friction wheel, better attitude performance and control performance can be obtained, the code coupling degree is proper, and the calculation performance is optimized.

Description

Technical Field

[0001] The invention belongs to the technical field of the attitude of a gimbal carrying a friction wheel launching mechanism, and in particular relates to an IMU algorithm design method for a gimbal carrying a friction wheel launching mechanism. Background Art

[0002] IMU provides a reference for the posture of the gimbal carrying the friction wheel launching mechanism. It is also one of the main factors affecting the performance of the gimbal, and has the same status as the performance of the gimbal motor. Currently, there is an EKF extended Kalman filter, but after completion, the code coupling is too high and the required computing power is too large.

[0003] To this end, we propose an IMU algorithm design method for a gimbal equipped with a friction wheel launching mechanism to solve the above problems. Summary of the invention

[0004] The technical problem to be solved by the present invention is to provide an IMU algorithm design method for a gimbal carrying a friction wheel launching mechanism.

[0005] The objective of the present invention is achieved through the following technical solutions:

[0006] A method for designing an IMU algorithm for a gimbal with a friction wheel transmitting mechanism comprises the following steps:

[0007] S1. Testing and selection of posture algorithms;

[0008] S2, IMU calibration;

[0009] S3, whether the magnetometer data is integrated or not;

[0010] S4, low-pass filter design selection;

[0011] S5. Overall performance verification.

[0012] Furthermore, in step S2, the accelerometer and gyroscope error models used are shown in the following formula, where a defaults to 1 if there is no calibration bench to test linearity, otherwise a is the linearity coefficient measured by the calibration bench, and b is the zero bias:

[0013] y=ax+b

[0014] After preheating the IMU module to 45 degrees and keeping it for 3 minutes to avoid temperature interference, calibration begins. First, start with calibrating the gyroscope. Let the gyroscope stand for one second to obtain one thousand data for each axis, remove extreme abnormal values ​​and outliers, and obtain the zero bias of each axis. At this time, ignore the Coriolis deflection of the earth's rotation. After completion, the three-sided calibration of the gyroscope is achieved. The same is true for acceleration, but the Z-axis needs to be calibrated separately by placing the sensor sideways to avoid gravity interference.

[0015] Furthermore, the effects of sensor installation and sensor internal non-orthogonality errors are ignored during calibration.

[0016] Furthermore, when the temperature of the IMU module reaches 45 degrees, the program automatically calibrates the gyroscope.

[0017] Furthermore, in step S4, the filter parameters are determined by measuring the motion noise of the robot, the vibration generated by the driving frequency of the pan / tilt motor, and the vibration caused by the rotation of the friction wheel.

[0018] Furthermore, in step S5, after obtaining the data output by the IMU once, the current time is obtained and the time last obtained is subtracted to obtain a strict update interval. Next, the original data of the gyroscope and accelerometer sensors is subtracted from the zero bias calibration value, and after passing through the FIR low-pass filter, it is processed by the complementary filtering algorithm and configured in the robot system. When the friction wheel is turned on and stationary, the error of the complementary filtering algorithm is about 0.01rad every 3 minutes, and the performance of the overall algorithm is verified.

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

[0020] The scheme designed by the present invention is integrated with the movement of the pan-tilt motor and the friction wheel to obtain better attitude performance and control performance. The code coupling is appropriate and its computing performance is optimized. BRIEF DESCRIPTION OF THE DRAWINGS

[0021] Figure 1 It is the code formula diagram;

[0022] Figure 2 This is the algorithm flow chart. DETAILED DESCRIPTION

[0023] The following will describe the implementation methods of the present application in detail with the help of accompanying drawings and examples, so that the implementation process of how the present application uses technical means to solve technical problems and achieve technical effects can be fully understood and implemented accordingly.

[0024] A method for designing an IMU algorithm for a gimbal with a friction wheel transmitting mechanism comprises the following steps:

[0025] S1. Testing and selection of posture algorithm:

[0026] Through research and testing of the well-known Kalman filter (KF), complementary filter algorithm (based on Mahony), and gradient descent algorithm (Madgwick) in engineering and academia, the Mahony algorithm is more suitable as the attitude algorithm of the IMU of the gimbal carrying the friction wheel launch mechanism due to its strong stability and zero drift performance.

[0027] S2. IMU calibration:

[0028] The accelerometer and gyroscope error models used are shown in the following formula. If there is no calibration bench to test linearity, a defaults to 1. Otherwise, a is the linearity coefficient measured by the calibration bench, and b is the zero bias:

[0029] y=ax+b

[0030] After preheating the IMU module to 45 degrees and keeping it for 3 minutes, calibration begins. Start with calibrating the gyroscope. The stationary gyroscope and accelerometer obtain one thousand data for each axis every second, remove extreme outliers and outliers, and obtain the zero bias of each axis. At this time, the Coriolis bias of the earth's rotation is ignored. After completion, the three-sided calibration of the gyroscope is achieved, and the same applies to the acceleration. However, the Z-axis needs to be calibrated separately by placing the sensor on its side to avoid gravity interference. During calibration, the effects of sensor installation and non-orthogonal errors inside the sensor are ignored.

[0031] S3. Whether to integrate magnetometer data:

[0032] For magnetometers, ellipse calibration is usually required to avoid interference from surrounding magnetic components and map the original values ​​onto a spherical surface. However, in fact, due to various reasons, the data interference of the magnetometer is extremely serious, especially when the friction wheel is turned on, the algorithm will refuse to add the geomagnetometer to the fusion algorithm.

[0033] According to the Biot-Savart theorem, the relationship between magnetic induction intensity B, current element I and distance is defined: Then for point A outside the infinitely long straight wire, the distance from the straight line of the straight current is r, then the magnetic field strength at point A is For a straight conductor 0.1m away with a current of 20A (because the maximum current of 3508 phases is 20A), the approximate magnetic field generated is 40μT, while the typical magnetic field of the earth is 25μT to 50μT. The actual test is also relatively close to this value. When the friction wheel is turned on, the average value of the single-axis output of the magnetometer will increase by about 20μT, which is equivalent to the strength of the earth's magnetic field. However, the data fusion algorithm also failed to adopt the magnetometer data.

[0034] The motion control focus of most gimbals with friction wheels is to ensure short-term yaw axis stability. It is not important for the gimbal to have a magnetometer to determine the yaw axis value related to the earth coordinate system. Yaw axis drift is acceptable in short-term scenarios. When the friction wheel is turned off, the magnetometer fusion will be automatically turned on again.

[0035] S4. Design selection of low-pass filter:

[0036] The improvement of the overall IMU algorithm mainly comes from the reasonable selection of filters. In the data output by the gyroscope, high-frequency noise signals cannot be ignored, but if the pass frequency of the low-pass filter is blindly increased, the phase of the filter output will increase, and the data delay of the attitude output will also increase.

[0037] A relatively reasonable solution is to determine the filter parameters by measuring the robot's motion noise. At the same time, since the controller's gyroscope and acceleration sensor have built-in shock absorption solutions, it is necessary to ensure that the controller is completely fixed to the gimbal to prevent other vibrations. Under normal circumstances, the lower the cutoff frequency of the low-pass filter, the greater the delay. After testing a 30Hz low-pass filter, the delay may be as long as 8ms; a 60Hz low-pass filter is generally 3.8ms, and a 120Hz low-pass filter is 1.9ms. However, the cutoff frequency cannot be increased continuously, otherwise it will not be able to filter out high-frequency interference signals. A reasonable solution is to make the cutoff frequency of the low-pass filter as high as possible while ensuring isolation of high-frequency vibrations. In a typical gimbal system, the 27th-order direct FIR low-pass filter with a -3db cutoff frequency of 120Hz has a typical phase delay of about 4.68 degrees. For example, Figure 1 As shown. This value reduces the system delay while ensuring the filtering effect, thereby ensuring the system response speed. For special types of gimbals, it is necessary to consider the overall vibration performance;

[0038] S5. Overall performance verification:

[0039] After obtaining the data output by the IMU, the current time is obtained and the last time is subtracted to obtain the strict update interval. Next, the zero bias calibration value is subtracted from the original data of the gyroscope and accelerometer, and after passing through the FIR low-pass filter, it is processed by the complementary filtering algorithm and configured in the robot system. When the friction wheel is turned on and the robot is stationary, the error of the complementary filtering algorithm is about 0.01 rad every 3 minutes, and the performance of the entire algorithm is verified.

[0040] In summary, the above are only preferred embodiments of the present invention and are not intended to limit the present invention in any form. Any simple modifications or equivalent changes made to the above embodiments based on the technical essence of the present invention fall within the protection scope of the present invention.

[0041] The above description is only a preferred embodiment of the present invention and does not limit the present invention in any form. Any simple modification, equivalent changes and modifications made to the above embodiments based on the technical essence of the present invention are within the scope of the technical solution of the present invention.

Claims

1. An IMU algorithm design method for a gimbal with a friction wheel launch mechanism, characterized in that: The steps include: S1. Testing and selection of posture algorithms; S2, IMU calibration; S3, whether the magnetometer data is integrated or not; S4, low-pass filter design selection; S5. Overall performance verification.

2. The IMU algorithm design method of the gimbal carrying the friction wheel launching mechanism according to claim 1 is characterized in that: In step S2, the accelerometer and gyroscope error models used are shown in the following formula. If there is no calibration bench to test the linearity, a defaults to 1. Otherwise, a is the linearity coefficient measured by the calibration bench, and b is the zero bias: y=ax+b After preheating the IMU module to 45 degrees and maintaining it for 3 minutes to avoid temperature interference, calibration begins. First, start with calibrating the gyroscope. Let the gyroscope stand for one second to obtain one thousand data for each axis, remove extreme outliers and outliers, take the average of the one thousand data, and get the zero bias of each axis. At this time, ignore the Coriolis deflection caused by the earth's rotation. After completion, the three-sided calibration of the gyroscope is achieved, and the acceleration is the same. However, the Z-axis needs to be placed on its side and calibrated separately to avoid gravity interference.

3. The IMU algorithm design method of the gimbal carrying the friction wheel launching mechanism according to claim 2 is characterized in that: During calibration, the effects of sensor installation and sensor internal non-orthogonality errors are ignored.

4. The IMU algorithm design method of the Robomaster platform according to claim 2, characterized in that: When the IMU module temperature reaches 45 degrees, the program automatically calibrates the gyroscope.

5. The IMU algorithm design method for the gimbal carrying the friction wheel launching mechanism according to claim 1 is characterized in that: In step S4, the filter parameters are determined by measuring the robot's motion noise, the vibration generated by the driving frequency of the pan / tilt motor, and the vibration caused by the rotation of the friction wheel. If the friction wheel is turned on and rotated, the magnetometer data will not be fused.

6. The IMU algorithm design method for the gimbal with a friction wheel launching mechanism according to claim 1, characterized in that: In step S5, after obtaining the data output by the IMU once, the current time is obtained and the last time obtained is subtracted to obtain a strict update interval. Next, the zero bias calibration value is subtracted from the original data of the gyroscope and accelerometer sensors, and after passing through the FIR low-pass filter, it is processed by the complementary filtering algorithm and configured in the robot system. When the friction wheel is turned on and stationary, the error of the complementary filtering algorithm is about 0.01rad every 3 minutes, and the performance of the entire algorithm is verified.

Images