Combined navigation sensor and optimization method thereof

Abstract

The invention relates to a combined navigation sensor and an optimization method thereof, the combined navigation sensor comprises a chassis plate, two roller assemblies and a plane positioning module, the two roller assemblies and the plane positioning module are installed on the chassis plate, the chassis plate is vertically provided with two assembly holes, each roller assembly comprises a U-shaped frame and an omnidirectional wheel rotatably installed in the U-shaped frame, the U-shaped frame is fixedly arranged in the assembly holes, and the omnidirectional wheel is rotatably installed in the U-shaped frame. A photoelectric encoder is arranged on a wheel shaft of the omnidirectional wheel; a main control chip, a gyroscope and an accelerometer are arranged on the plane positioning module; the IMU data of the gyroscope and the accelerometer can be effectively combined with the odometer data of the omnidirectional wheel to obtain more accurate speed estimation, the integration drift of the IMU data can be reduced through the fusion method, meanwhile, the accuracy of the odometer data is utilized, a traditional inertial sensor is combined with the orthogonal omnidirectional wheel, and the speed estimation accuracy is improved. A highly integrated sensor fusion technology is realized, the real-time navigation positioning capability of the mobile robot is improved, and the mobile robot has high real-time performance, strong anti-interference performance and high precision.

Description

Technical Field

[0001] The invention relates to the field of robot positioning sensors, and in particular to a combined navigation sensor and an optimization method thereof. Background Art

[0002] Robot positioning technology has been widely used in various fields. In the manufacturing industry, it can help robots accurately grasp and assemble parts; in the logistics field, it supports automated warehousing and logistics operations; in the medical field, it assists surgery and diagnosis; in the agricultural field, it realizes automated planting and harvesting. However, robot positioning technology still faces some challenges, such as the perception and recognition capabilities of machine vision are not intelligent and accurate enough, and it needs to be deeply integrated with other intelligent technologies to cope with complex work scenarios and needs. Robot positioning technology is one of the key tasks in robot navigation and autonomous movement, which involves determining the position and posture of the robot in the environment.

[0003] There are two main types of robot positioning technology: absolute positioning and relative positioning. Absolute positioning relies on a fixed reference point to determine the position, while relative positioning determines the current position by continuously measuring position changes. Relative positioning is theoretically more suitable for fast-moving robots because it can update position information in real time. It relies on inertial navigation devices to measure the changes of the robot relative to the previous position and determines the current position by accumulating these changes. However, this method has some problems. For example, the slippage of the drive wheel may cause inaccurate odometer data, and inconsistent updates of inertial devices and odometer data may also introduce errors, resulting in limited real-time performance, strong anti-interference and accuracy, which needs to be optimized and improved. Summary of the invention

[0004] In order to solve the above problems, the present invention proposes a combined navigation sensor and an optimization method thereof.

[0005] The technical solution of the present invention is: a combined navigation sensor, including a chassis plate, two roller assemblies installed on the chassis plate and a plane positioning module, the chassis plate is vertically provided with two assembly holes, the roller assembly includes a U-shaped frame and an omnidirectional wheel rotatably installed in the U-shaped frame, the U-shaped frame is fixedly arranged in the assembly hole, and a photoelectric encoder is arranged on the wheel axle of the omnidirectional wheel; the plane positioning module is provided with a main control chip, a gyroscope and an accelerometer.

[0006] Preferably, a shock absorbing mechanism is provided between the roller assembly and the chassis plate.

[0007] Preferably, the shock absorbing mechanism comprises a square bracket arranged between the ends of the two assembly holes, a slide seat fixed to the outer side surface of the square bracket and a guide rail vertically fixed to the outer side surface of the U-shaped frame, and the guide rail is slidably connected in the slide seat.

[0008] Preferably, a cross bar is provided on the upper part of the guide rail, and a tension spring is connected between the end of the cross bar and the lower part of the square bracket.

[0009] An optimization method for a combined navigation sensor includes the following steps:

[0010] IMU complementary filtering and odometer data fusion

[0011] Step 1, the state transition model formula is (prediction step):

[0012] V k = V k-1 + a k-1 (1)

[0013] Wherein, the photoelectric encoder on the omnidirectional wheel obtains the rotational speed V k , V k is the speed state at the k-th moment, the accelerometer obtains the position information a k-1 of the sensor, and the initial speed V k-1 is given, a k-1 is the acceleration measurement value at the (k - 1)-th moment;

[0014] The observation model formula is:

[0015] Z k = V k (2)

[0016] Wherein, Z k is the observation value at the k-th moment, which is equal to the speed state at the same moment;

[0017] Step 2, initialize the Kalman filter: The initial state is obtained through the initial speed of the sensor; the initial error covariance matrix is P0, which represents the uncertainty of the initial state estimation;

[0018] Step 3, prediction step: For each time step k, use the previous state and IMU data (acceleration) to predict the current state:

[0019]

[0020] Prediction error covariance:

[0021] P k|k-1 = P k-1|k-1 + Q (4)

[0022] Where Q is the process noise covariance matrix, which includes the uncertainty of acceleration measurement and the drift in the integration process;

[0023] Step 4, update step: When new odometer data Z kWhen it arrives, calculate the Kalman gain:

[0024]

[0025] where R is the observation noise covariance matrix, which includes the uncertainty of odometer measurements, and update the state estimate:

[0026]

[0027] Update the error covariance matrix

[0028] P k|k =(1 - K k )P k|k-1 (7)

[0029] Fusion coordinate calculation of heading angle and odometer

[0030] Step 5, loop execution: The time interval between every two samplings of the main control chip is 5 ms. The system can calculate the instantaneous heading angle at two adjacent sampling moments accordingly. Since the motion process between every two sampling points cannot be determined, the angular motion process of the planar positioning system between every two sampling points in this system is approximated as a uniform rotational motion. Let the heading angle at the (k - 1)th sampling moment be θ k-1 , and the heading angle at the kth sampling moment be θ k , then the calculation formula for the stepping direction within this sampling period is:

[0031]

[0032] The coordinate calculation formula of the displacement vector from the (k - 1)th moment to the subsequent moment in the navigation coordinate system is:

[0033]

[0034] Finally, this sensor obtains the speed information and attitude information of the chassis, and the position information can be obtained by integrating the speed information.

[0035] Preferably, the steps of the IMU complementary filtering algorithm are as follows:

[0036] Step 1, normalize the values of the accelerometer: The values obtained from the accelerometer in the gyroscope are Ax, ay, az, corresponding to the values of the x-axis, y-axis, and z-axis respectively. Normalize these values so that they have the same magnitude as the gravity vector represented by the quaternion;

[0037]

[0038] Step 2, extract the gravity component in the body coordinate system: The attitude matrix for transforming from the geographic coordinate system (E system) to the body coordinate system (b system) calculated by the quaternion is:

[0039]

[0040] Extract the gravity components Vx, Vy, and Vz in the body coordinate system from this matrix. The gravity vector in the geographical coordinate system is [0, 0, 1]. The gravity components in the body coordinate system are obtained by multiplying this vector by the rotation matrix:

[0041]

[0042] Step 3: Calculate the error and integrate: Calculate the error between the normalized value of the accelerometer and the gravity components in the body coordinate system:

[0043] ex = ay * Vz - az * Vy

[0044] ey = az * Vx - ax * Vz

[0045] ez = ax * Vy - ay * Vx (13)

[0047] Integrate the error to eliminate the error:

[0048] accex = accex + Error x ·ki·dt

[0049] accey = accey + Error y ·ki·dt

[0050] accez = accez + Error z ·ki·dt (14)

[0051] where ki is the integration coefficient and dt is the integration period time;

[0052] Step 4: Complementary filtering: Input the error into the PID controller and add it to the angular velocity measured by the gyroscope in this attitude update to obtain a corrected angular velocity value:

[0053] gx corrected = gx + Kp·accex

[0054] gy corrected = gy + Kp·accey

[0055] gz corrected = gz + Kp·accez (15)

[0056] Where Kp is the complementary filtering coefficient;

[0057] Step 5, update the quaternion: Use the corrected angular velocity value to update the quaternion:

[0058]

[0059] Where q is the current quaternion, and × represents the cross product of quaternions;

[0060] By these steps, the information obtained from the accelerometer is effectively utilized to compensate for the angular velocity information of the gyroscope, thereby improving the accuracy and robustness of attitude estimation. By substituting q new into the quaternion and Euler angle conversion formula, the Euler angle attitude of the sensor can be obtained.

[0061] The beneficial technical effects of the present invention are as follows: The present invention can effectively combine the IMU data (acceleration) of the gyroscope and the accelerometer with the odometer data (speed) of the omnidirectional wheels to obtain a more accurate speed estimate. This fusion method can reduce the integral drift of the IMU data, and at the same time utilize the accuracy of the odometer data. By combining the traditional inertial sensors with the orthogonal omnidirectional wheels, a highly integrated sensor fusion technology is realized, improving the real-time navigation and positioning ability of the mobile robot, making it have high real-time performance, strong anti-interference ability and high precision. Description of the Drawings

[0062] Figure 1 is the three-dimensional structure schematic diagram of the combined navigation sensor;

[0063] Figure 2 is the front view structure schematic diagram of the combined navigation sensor;

[0064] Figure 3 is Figure 2 the sectional view structure schematic diagram in the A-A direction of

[0065] Figure 4 the three-dimensional structure schematic diagram of the roller assembly;

[0066] Figure 5 is the three-dimensional structure schematic diagram of the combined navigation sensor after removing one roller assembly; Figure 6 The physical diagram of the combined navigation sensor.

[0067] In the figure, 1. Chassis plate, 11. Assembly hole, 2. Planar positioning module, 21. Accelerometer, 22. Gyroscope, 31. U-shaped frame, 32. Omnidirectional wheel, 33. Photoelectric encoder, 41. Square bracket, 42. Slide block, 43. Guide rail, 44. Cross bar, 45. Tension spring. Detailed Embodiments

[0068] Example 1, see the appendix Figure 1 - 5 , a combined navigation sensor, including a chassis plate, two roller assemblies and a planar positioning module mounted on the chassis plate. There are two assembly holes vertically arranged on the chassis plate. The roller assembly includes a U-shaped frame and an omnidirectional wheel rotatably installed in the U-shaped frame. The omnidirectional wheel is a Furey wheel, which can achieve movement modes such as forward movement, lateral movement, diagonal movement, rotation and their combinations. Through the independent control of the rotation direction and speed of the four wheels, the omnidirectional movement of the entire platform can be achieved. The U-shaped frame is fixedly arranged in the assembly hole. An optoelectronic encoder is provided on the axle of the omnidirectional wheel. The omnidirectional wheel drives the optoelectronic encoder to rotate to realize the measurement of the odometer; the planar positioning module is provided with a main control chip, a gyroscope and an accelerometer. The accelerometer is a device that measures the three-axis components of the acceleration of the device's movement and the acceleration of gravity in its own coordinate system. When the device has no movement acceleration, the accelerometer uses the acceleration of gravity as a measurement standard. By using the components of the acceleration of gravity on the three axes, the absolute roll angle and pitch angle of the device can be directly calculated; the gyroscope is a device that measures the angular velocity of the device using the Coriolis force. Its design and working principle are various. This gyroscope uses electrostatic drive to make its internal mechanism reach the resonant state to obtain the speed, and uses this speed to generate the Coriolis force to measure the magnitude of the rotational angular velocity of the device. Integrating the angular velocity of the gyroscope can obtain the relative attitude of the device.

[0069] A shock absorption mechanism is provided between the roller assembly and the chassis plate. The shock absorption mechanism includes a square bracket arranged between the ends of the two assembly holes, a sliding seat fixed on the outer side of the square bracket, and a guide rail vertically fixed on the outer side of the U-shaped frame. The guide rail is slidably connected in the sliding seat. A cross bar is provided at the upper part of the guide rail. A tension spring is connected between the end of the cross bar and the lower part of the square bracket. The omnidirectional wheel can slide up and down along the sliding seat on the base plate through the guide rail. The tension spring generates a pulling force between the omnidirectional wheel and the chassis seat to drive them to slide towards each other, so that the omnidirectional wheel forms a suspension structure on the chassis plate. When the movement site of the robot is uneven, this suspension structure effectively avoids the phenomenon that the omnidirectional wheel cannot fully contact the ground, thus ensuring the accuracy of the measurement information as much as possible.

[0070] This combined navigation sensor can effectively combine the IMU data (acceleration) of the gyroscope and the accelerometer with the odometer data (speed) of the omnidirectional wheel to obtain a more accurate speed estimate. This fusion method can reduce the integration drift of the IMU data, and at the same time utilize the accuracy of the odometer data. By combining traditional inertial sensors with orthogonal omnidirectional wheels, a highly integrated sensor fusion technology is realized, improving the real-time navigation and positioning ability of mobile robots, making it have high real-time performance, strong anti-interference ability and high precision.

[0071] Example 2, see the appendix of the specification Figure 1 - 5, An optimization method for the combined navigation sensor in Embodiment 1. The mathematical methods used in this optimization method are

[0072] 1. Deriving the direction cosine matrix using the coordinate system rotation matrix

[0073] Determining the rotation axis and angle: Suppose we need to rotate coordinate system A to coordinate system B. We need to know the rotation axis and angle. The rotation can be a single rotation about the x-axis, y-axis, or z-axis, or a composite rotation about an arbitrary axis. Constructing the basic rotation matrix: For rotations about the x-axis, y-axis, and z-axis, the basic rotation matrices are respectively:

[0074] Rotation about the x-axis by angle θx:

[0075]

[0076] Rotation about the y-axis by angle θy:

[0077]

[0078] Rotation about the z-axis by angle θz:

[0079]

[0080] Combined rotation matrix: If the rotation is not about a single axis but a composite rotation about multiple axes, the basic rotation matrices can be combined through matrix multiplication. For example, if a coordinate system needs to be rotated first about the z-axis by θz, then about the y-axis by θy, and finally about the x-axis by θx, the total rotation matrix R can be obtained in the following way:

[0081] R = R x (θ x )R y (θ y )R z (θ z )

[0082] Note that the multiplication order of the rotation matrices is important and is usually in the order of z - y - x.

[0083] 2. Representing the direction cosine matrix using quaternions

[0084] Given a unit quaternion q = w + xi + yj + zk, the corresponding direction cosine matrix R can be calculated through the following formula:

[0085]

[0086] Each row and each column of this matrix corresponds to an axis in the original coordinate system, and each element in the matrix is a combination of quaternion components. Through this matrix, we can transform a vector from the original coordinate system to a new coordinate system rotated by a quaternion.

[0087] 3. Quaternion to Euler Angles

[0088] Given a quaternion q = (q0, q1, q2, q3), the corresponding Euler angles (α, β, γ) can be calculated by the following formulas:

[0089]

[0090] β = arcsin(2(q0q2 - q3q1))

[0091]

[0092] These formulas provide a method for converting between quaternions and Euler angles, which are very useful in 3D space rotation and orientation estimation.

[0093] 4. Quaternion Solving and Updating

[0094] The differential equation of the quaternion can be expressed as:

[0095]

[0096] where q is the quaternion, and ωq is the quaternion formed by the angular velocity vector ω (without a real part, and the imaginary part is the angular velocity vector).

[0097] Updating the quaternion using the first-order Runge-Kutta method

[0098] By measuring the angular velocity ω with a gyroscope, given the current quaternion q and the time step Δt, the steps to update the quaternion using the first-order Runge-Kutta method are as follows:

[0099] Calculate the derivative of the quaternion:

[0100]

[0101] Update the quaternion:

[0102]

[0103] Normalize the quaternion: The updated quaternion needs to be normalized to keep it as a unit quaternion:

[0104]

[0105] This method is simple and easy to implement, but has low precision and is suitable for occasions with low precision requirements. For applications that require higher precision.

[0106] The optimization method steps of the combined navigation sensor are as follows:

[0107] IMU complementary filtering and odometer data fusion. The IMU complementary filtering algorithm is a filtering method that combines accelerometer and gyroscope data, aiming to improve the accuracy of attitude estimation.

[0108] Step 1, the state transition model formula is (prediction step): V k = V k-1 + a k-1 where the photoelectric encoder on the omnidirectional wheel obtains the rotational speed V k , V k is the speed state at the k-th moment, the accelerometer obtains the position information ak-1 of the sensor, and given the initial speed Vk-1, ak-1 is the acceleration measurement value at the (k - 1)-th moment; the observation model formula is: Z k = V k where Z k is the observation value at the k-th moment, which is equal to the speed state at the same moment;

[0109] Step 2, initialize the Kalman filter: the initial state is which is obtained through the initial speed of the sensor; the initial error covariance matrix is P0, which represents the uncertainty of the initial state estimation;

[0110] Step 3, prediction step: for each time step k, use the previous state and IMU data (acceleration) to predict the current state: Prediction error covariance: P k|k-1 = P k-1|k-1 + Q, where Q is the process noise covariance matrix, which includes the uncertainty of acceleration measurement and the drift during the integration process;

[0111] Step 4, update step: when the new odometer data Zk arrives, calculate the Kalman gain: where R is the observation noise covariance matrix, which includes the uncertainty of odometer measurement, and update the state estimation: Update the error covariance matrix: P k|k = (1 - K k )P k|k-1 ;

[0112] Fusion coordinate calculation of the heading angle and the odometer

[0113] Step 5, loop execution: The time interval between every two samplings of the main control chip is 5 ms. The system can calculate the instantaneous heading angle at two adjacent sampling moments based on this. Since the motion process between every two sampling points cannot be determined, the angular motion process of the planar positioning system between every two sampling points in this system is approximated as a uniform rotational motion. Let the heading angle at the (k - 1)-th sampling moment be θ k-1 , and the heading angle at the k-th sampling moment be θ k . Then the calculation formula for the stepping direction within this sampling period is:

[0114]

[0115] The coordinate calculation formula for the displacement vector from the (k - 1)-th moment to the subsequent moment in the navigation coordinate system is:

[0116]

[0117] Finally, this sensor obtains the speed information and attitude information of the chassis. By integrating the speed information, the position information can be obtained.

[0118] The steps of the IMU complementary filtering algorithm are as follows:

[0119] Step 1, normalize the values of the accelerometer: The values obtained from the accelerometer in the gyroscope are Ax, ay, az, corresponding to the values of the x-axis, y-axis, and z-axis respectively. Normalize these values so that they have the same magnitude as the gravity vector represented by the quaternion;

[0120]

[0121] Step 2, extract the gravity components in the body coordinate system: The attitude matrix for transforming from the geographical coordinate system (E-frame) to the body coordinate system (b-frame) calculated by the quaternion is:

[0122]

[0123] Extract the gravity components Vx, Vy, Vz in the body coordinate system from this matrix. The gravity vector in the geographical coordinate system is [0, 0, 1]. Obtain the gravity components in the body coordinate system by multiplying this vector by the rotation matrix:

[0124]

[0125] Step 3, calculate the error and integrate: Calculate the error between the normalized value of the accelerometer and the gravity components in the body coordinate system:

[0126] ex = ay * Vz - az * Vy

[0127] ey =az * Vxax * Vz

[0128] ez ax* Vy - ay * Vx

[0129] Integrate the error to eliminate the error:

[0130] accex = accex + Error x ·ki·dt

[0131] accey = accey + Error y ·ki·dt

[0132] accez = accez + Error z ·ki·dt

[0133] where ki is the integration coefficient and dt is the integration period time;

[0134] Step 3, complementary filtering: Input the error into the PID controller and add it to the angular velocity measured by the gyroscope in this attitude update to obtain a corrected angular velocity value:

[0135] gx corrected = gx + Kp·accex

[0136] gy corrected = gy + Kp·accey

[0137] gz corrected = gz + Kp·accez

[0138] where Kp is the complementary filtering coefficient;

[0139] Step 5: Update the quaternion: Use the corrected angular velocity value to update the quaternion:

[0140]

[0141] where q is the current quaternion and × represents the cross product of quaternions;

[0142] By these steps, effectively utilize the information obtained by the accelerometer to compensate the angular velocity information of the gyroscope, thereby improving the accuracy and robustness of attitude estimation. Through q new Substitute into the quaternion and Euler angle conversion formula to obtain the Euler angle attitude of the sensor.

Claims

1. A combined navigation sensor, characterized in that: It includes a chassis plate, two roller assemblies mounted on the chassis plate, and a planar positioning module. There are two assembly holes vertically arranged on the chassis plate. The roller assembly includes a U-shaped frame and an omnidirectional wheel rotatably mounted within the U-shaped frame. The U-shaped frame is fixedly arranged within the assembly holes, and an optoelectronic encoder is provided on the axle of the omnidirectional wheel. The planar positioning module is provided with a main control chip, a gyroscope, and an accelerometer.

2. The combined navigation sensor according to claim 1, characterized in that: A shock absorption mechanism is provided between the roller assembly and the chassis plate.

3. The combined navigation sensor according to claim 2, characterized in that: The shock absorption mechanism includes a square bracket arranged between the ends of the two assembly holes, a sliding seat fixed on the outer side of the square bracket, and a guide rail vertically fixed on the outer side of the U-shaped frame. The guide rail is slidably connected within the sliding seat.

4. The combined navigation sensor according to claim 3, characterized in that: A cross bar is provided at the upper part of the guide rail, and a tension spring is connected between the end of the cross bar and the lower part of the square bracket.

5. An optimization method for a combined navigation sensor according to any one of claims 1-3, Its features include the following steps: IMU complementary filtering and odometer data fusion Step 1, the state transition model formula is (prediction step): V k = V k-1 + a k-1 wherein, the photoelectric encoder on the omnidirectional wheel acquires the rotational speed V k , V k is the velocity state at the k-th moment, and the accelerometer acquires the position information a k-1 of the sensor. Given the initial velocity V k-1 , a k-1 is the acceleration measurement value at the (k - 1)-th moment; the observation model formula is: Z k = V k wherein, Z k is the observation value at the k-th moment, and it is equal to the velocity state at the same moment; Step 2, initialize the Kalman filter: The initial state is obtained from the initial velocity of the sensor; the initial error covariance matrix is P0, which represents the uncertainty of the initial state estimate; Step 3, Prediction step: At each time step \(k\), predict the current state using the previous state and IMU data (acceleration): Prediction error covariance: \(P\) k|k-1 \(=\) \(P\) k-1|k-1 \(+\) \(Q\), where \(Q\) is the process noise covariance matrix, which includes the uncertainty of acceleration measurement and the drift during the integration process; Step 4, Update Step: When new odometer data Z k arrives, calculate the Kalman gain: where R is the observation noise covariance matrix, which includes the uncertainty of odometer measurements, and update the state estimate: Update the error covariance matrix: P k|k =(1 - K k )P k|k-1 ; Heading angle and odometer fusion coordinate calculation Step 5, loop execution: The time interval between every two samplings of the main control chip is 5 ms. The system can calculate the instantaneous heading angle at two adjacent sampling moments based on this. Since the motion process between every two sampling points cannot be determined, the angular motion process of the planar positioning system between every two sampling points in this system is approximated as a uniform rotational motion. Let the heading angle at the (k - 1)-th sampling moment be θ k-1 , and the heading angle at the k-th sampling moment is θ k . Then the calculation formula for the stepping direction within this sampling period is: The coordinate calculation formula of the displacement vector from the k-1 moment to the subsequent moment in the navigation coordinate system is: Finally, the sensor obtains the speed information and attitude information of the chassis, and the position information can be obtained by integrating the speed information.

6. The optimization method of a combined navigation sensor according to claim 5, characterized in that: The steps of the IMU complementary filtering algorithm are as follows: Step 1, normalize the accelerometer values: The values obtained from the accelerometer in the gyroscope are Ax, ay, az, corresponding to the values of the x-axis, y-axis, and z-axis respectively. Normalize these values so that they have the same magnitude as the gravity vector represented by the quaternion. Step 2, extract the gravity components in the body coordinate system: The attitude matrix for transforming from the geographical coordinate system (E system) to the body coordinate system (b system) is calculated through quaternions: Extract the gravity components Vx, Vy, Vz in the body coordinate system from this matrix. The gravity vector in the geographical coordinate system is [0, 0, 1]. Obtain the gravity components in the body coordinate system by multiplying this vector by the rotation matrix: Step 3, calculate the error and integrate: Calculate the error between the normalized value of the accelerometer and the gravity components in the body coordinate system: ex = ay*Vz - az * Vy ey ＝ az * Vx - ax * Vz ez = ax* Vy - ay * Vx Integrate the error to eliminate the error: accex = accex + Error x ·ki·dt accey = accey + Error y ·ki·dt accez = acccez + Error z ·ki·dt where ki is the integration coefficient and dt is the integration period time; Step 4, complementary filtering: Input the error into the PID controller and add it to the angular velocity measured by the gyroscope in the current attitude update to obtain a corrected angular velocity value: gx corrected = gx + Kp·accex gy corrected = gy + Kp · accey gz corrected = gz + Kp·accez where Kp is the complementary filtering coefficient; Step 5, update the quaternion: Update the quaternion using the corrected angular velocity value: where q is the current quaternion, and × represents the cross product of quaternions; Effectively utilize the information obtained by the accelerometer through these steps to compensate for the angular velocity information of the gyroscope, thereby improving the accuracy and robustness of attitude estimation. Through q new Substitute into the quaternion and Euler angle conversion formula, and the Euler angle attitude of the sensor can be obtained.

Images