A Heading Calibration Method for a Mower to Walk Straight Based on a Gyroscope
By conducting periodic analysis and state judgment on the Z-axis zero drift error of the lawn mower gyroscope, the zero drift error is accurately removed, and the problem of accumulation of heading angle errors in lawn mower in unstable environments is solved, achieving higher accuracy linear walking and path planning.
Patent Information
- Application Number
- CN202211296820.0
- Authority / Receiving Office
- CN · China
- Patent Type
- Patents(China)
- Current Assignee / Owner
- Filing Date
- 2022-10-21
- Publication Date
- 2025-05-30
- Estimated Expiration
- 2042-10-21
AI Technical Summary
In the prior art, due to the zero drift problem of the gyroscope in outdoor lawn mowers, heading angle error accumulates over time, causing the lawn mowers to gradually deviate when they walk in a straight line, making it difficult to effectively calibrate in an unstable outdoor environment.
By periodically analyzing the zero drift error of the gyroscope Z-axis, the cumulative error of the heading angle is reduced, and the Z-axis of the gyroscope is used as the solution data source, and the dynamic or static state is judged based on the state threshold, so as to accurately remove the zero drift error.
It effectively improves the accuracy of the lawn mower during straight walking, enhances the accuracy of path planning and the coverage of mowed area, reduces cost and resource waste, and improves the running speed of the lawn mower.
Smart Images
Figure CN115585806B_ABST
Abstract
Description
Technical Field
[0001] The present invention belongs to the field of attitude calculation, and particularly relates to a heading calibration method for a lawn mower to walk straight based on a gyroscope. Technical Background
[0002] The attitude calculation of outdoor lawn mower equipment generally relies on a six-axis sensor to calculate attitude angles (heading angle, pitch angle, and roll angle) using quaternions. The six-axis sensor includes a gyroscope and an accelerometer. However, due to the zero-drift problem of the gyroscope, the error of the heading angle calculated by the six-axis will accumulate and become larger over time, and the lawn mower will gradually deviate when walking straight. Therefore, it is necessary to solve the zero-drift problem during the operation of the lawn mower.
[0003] For the above problems, there are mainly two solutions in the prior art. One is to analyze the error of the gyroscope to establish a model, and then use the quaternion and error compensation algorithm to finally calculate the attitude angle. However, this method is suitable for use in a relatively stable environment. For example, it can achieve good results in a stationary state, but it is not obvious for a lawn mower running on uneven grass. The other is to add a magnetometer fusion algorithm on the basis of the six-axis to finally achieve the calibration of the heading angle. However, due to the magnetometer being easily interfered, the outdoor environment is unstable and unpredictable, making it still difficult to solve the above problems. Summary of the Invention
[0004] In view of the above problems, the present invention proposes a heading calibration method for a lawn mower to walk straight based on a gyroscope. By periodically calibrating the zero-drift error of the Z-axis of the gyroscope, the cumulative error of the heading angle is reduced. At the same time, the Z-axis of the gyroscope is used as the data source for calculation, which has strong anti-environmental interference ability, and can make the lawn mower achieve a better effect of removing zero-drift error by dividing states during operation.
[0005] The present invention adopts the following technical solutions:
[0006] Utilize the static zero-drift of the gyroscope to solve the drift problem during the operation of the lawn mower. Set a state threshold for judging whether the lawn mower is dynamic or static according to the difference between the angular velocity values of the gyroscope in dynamic and static states. The heading angle can be calculated in two modes, static and dynamic, according to the state threshold, so as to solve the problem of the cumulative error of the heading angle caused by the zero-drift of the gyroscope over time with higher precision. Specifically, it includes the following steps:
[0007] A. Obtain the static data of the Z-axis of the gyroscope
[0008] A-1. When the lawn mower is initialized and powered on, at this time the lawn mower is in a stationary state, and obtain N consecutive angular velocity values of the Z-axis of the gyroscope with a sampling interval of period T in a stationary state.
[0009] A-2. Cumulatively sum the Z-axis angular velocity values of the static gyroscope obtained in A-1 over the period T to obtain the cumulative angle of the heading drift in the stationary state within the N sampling periods.
[0010] B. Calculate the heading angle
[0011] After the lawn mower is turned on, the lawn mower obtains gyroscope data at a period of T. Each time data is obtained, the product of the Z-axis angular velocity value and T is accumulated and added to the previously calculated heading angle, and this is continuously superimposed, which is recorded as the total heading angle from the start of the machine to the current moment.
[0012] C. Determine the motion state
[0013] C-1. According to the different characteristics of the Z-axis angular velocity value of the gyroscope in the moving and stationary states, set a state threshold to distinguish the state of the lawn mower (this state threshold can be determined comprehensively by combining the range, sensitivity of the gyroscope in use, and the scenario where the gyroscope is applied).
[0014] C-2. To prevent misjudgment of the state caused by individual values greater than the threshold due to sudden changes in the gyroscope data, a sliding window is used. That is, each time the latest M consecutive Z-axis angular velocity values are stored in the sliding window, and their average value is calculated. If it is greater than the threshold, it is determined to be in the moving state; if it is less than the state threshold, it is in the stationary state.
[0015] D. Remove the heading angle drift error
[0016] D-1. According to the state threshold for judging the static and dynamic states of the lawn mower in C, when it is judged to be in the stationary state, the Z-axis angular velocity value is directly assigned 0, so the static drift error can be not introduced.
[0017] D-2. When it is judged to be in the moving state, for each heading angle obtained by accumulating the product of N Ts and the Z-axis angular velocity, the cumulative angle of the heading drift in the stationary state within the N sampling periods calculated in A-2 should be subtracted to remove the static drift error during the dynamic process and obtain the calibrated heading angle.
[0018] E. Heading correction
[0019] Theoretically, the heading angle (the heading angle obtained by removing the static drift error in step D) of the lawn mower remains unchanged during straight-line walking. Therefore, when the absolute value of the deviation of the change in the heading angle is greater than the angle threshold set in the algorithm (this angle threshold refers to the maximum amplitude value allowed for the change in the heading angle during straight-line walking), when it is greater than this angle threshold, the motor will adjust the speed of the two wheels to perform heading adjustment and continuously update the angle to make a straight-line motion.
[0020] Advantages of the present invention: In the present invention, first, N Z-axis angular velocity values of the gyroscope at rest with a sampling period of T are collected, and the cumulative angle of heading drift within these N sampling periods is calculated. At the same time, after power-on, the heading angle is calculated by multiplying the Z-axis angular velocity of the gyroscope by the time interval T for obtaining data and accumulating. Then, the heading error is removed accordingly by judging the state through the state threshold, and a more accurate heading angle is calculated.
[0021] By adopting the present invention, the accuracy of the lawn mower moving in a straight line can be effectively improved, the accuracy of path planning is also improved, and at the same time, the coverage rate of the mowing area is increased. For the static state, it can be directly assigned a value of 0 to accurately remove the zero drift. For the moving state, the static drift error can be removed at intervals of N sampling periods. This not only reduces the cost, reduces resource waste, and eliminates the operating cost brought by complex algorithms, but also improves the operating speed of the lawn mower to a certain extent. Description of the Drawings
[0022] Figure 1 is the overall method flow chart;
[0023] Figure 2 is the flow chart for initializing and calculating the fixed static drift;
[0024] Figure 3 is the flow chart for calculating the heading angle;
[0025] Figure 4 is the method effect comparison chart. Detailed Implementation Manner
[0026] In order to make the objectives, technical solutions, and advantages of the present invention clearer and more understandable, further detailed descriptions will be given below in combination with the drawings and examples.
[0027] The example of the present invention is based on the embedded system of the lawn mower. The embedded system of the lawn mower in the present invention is based on the GD32F305 single-chip microcomputer system, and the QMI8658A gyroscope sensor is embedded on this single-chip microcomputer. The gyroscope sensor provides data for the embedded system to perform operations, and finally calculates the heading angle for the embedded system to combine with the motor for heading correction. The method is as Figure 1 shown, and the specific process is as follows:
[0028] A. Obtain the Z-axis static data of the gyroscope
[0029] A-1. When the lawn mower is initialized and powered on, at this time the lawn mower is in a static state, and N consecutive, static Z-axis angular velocity values of the gyroscope with a sampling interval of period T are obtained.
[0030] A-2. Cumulatively sum the Z-axis angular velocity values of the gyroscope in the static state obtained in A-1 over the period T to obtain the cumulative angle of the heading drift in the stationary state within the N periods.
[0031] The process of step A-1 is as Figure 2 shown. The lawn mower samples the data of the gyroscope sensor at a period of 10 ms. During the initialization process of the lawn mower, 200 consecutive Z-axis angular velocity values gyro 1 , ggyo 2 , …, gyro 200 of the gyroscope in the static state are collected.
[0032] In step A-2, calculate the cumulative sum of the product of the 200 consecutive static Z-axis angular velocity values and the period time, that is, the static heading drift error angle within a 2-second time interval That is
[0033] B. Calculate the heading angle
[0034] B-1. The product of the Z-axis angular velocity value of the gyroscope and the sampling time interval is the angle by which the machine rotates around the Z-axis within the sampling interval.
[0035] B-2. After the lawn mower is powered on, the lawn mower obtains the data of the gyroscope at a period of T. Each time the data is obtained, the product of the Z-axis angular velocity value and T is added to the previously calculated heading angle, and this is continuously superimposed, which is recorded as the total heading angle from the power-on to the current moment.
[0036] Step B-1 mainly calculates the heading angle using the formula Yaw = Yaw + gyro t *T after the lawn mower is powered on. The specific process is as Figure 3 shown, where gyro t *T is the product of the sampling interval T and the Z-axis angular velocity gyro t of the gyroscope, representing the heading offset error angle within the time interval T. In order to make the calculated heading angle more accurate, T can be selected as small as possible. Due to the range of the gyroscope and the operation requirements of the single-chip microcomputer, T is selected as 10 ms.
[0037] Step B-2 means that each time new data is obtained at an interval of T, it is added to the previously calculated heading angle, and the real-time heading angle is continuously calculated in turn. However, the heading angle at this time is the heading angle with drift error.
[0038] C. Judge the motion state
[0039] C-1. Set a state threshold to distinguish the mower state according to the different characteristics of the angular velocity value of the gyroscope's Z-axis in motion and stationary states (this state threshold can be comprehensively determined by combining the range and sensitivity of the MEMS gyroscope in use and the scenario where the gyroscope is applied).
[0040] C-2. To prevent misjudgment of the state caused by individual values greater than the threshold due to data mutation of the gyroscope, a sliding window is adopted. That is, each time the latest M consecutive Z-axis angular velocities are stored in the sliding window, and their average value is calculated. If it is greater than the state threshold, it is determined to be in the motion state; if it is less than the state threshold, it is stationary.
[0041] In step C-1, the gyroscope sensor can measure the angular velocity values of each axis. However, due to the zero-drift problem of the gyroscope, the angular velocity of the gyroscope is not zero in the stationary state. But when the gyroscope is in the motion state, the angular velocity value will increase significantly. Therefore, according to the characteristics of the gyroscope, a state threshold can be specified. If the angular velocity value exceeds this state threshold, it is determined to be in the motion state; otherwise, it is stationary.
[0042] In step C-2, in order to more accurately judge the motion state, relying solely on a single angular velocity value is not enough. Therefore, a sliding window needs to be established. Each time the latest gyroscope Z-axis angular velocity is read, the window is updated, deleting the earliest value in the window and adding the latest value, arranged in chronological order, always maintaining the window as the latest angular velocity values of length M. Calculate the average value of the updated window. If the average value is greater than the state threshold, it is determined to be in the motion state; otherwise, it is determined to be in the stationary state.
[0043] D. Remove the heading angle drift error
[0044] D-1. According to the threshold for judging the static and dynamic states of the mower in C, when it is judged to be in the stationary state, directly assign the angular velocity of the Z-axis to 0, so that static drift error can be not introduced.
[0045] D-2. When it is judged to be dynamic, subtract the cumulative heading drift angle calculated in A-2 from the product of every N Ts and the Z-axis angular velocity value to remove the static drift error during the dynamic process.
[0046] The judgment of the mower state in step D-1 is performed each time a new angular velocity is obtained, and then the heading angle is calculated through step B, and then different treatments are made for the motion states distinguished in step C. When it is determined to be stationary, no drift error will be introduced when calculating the heading angle.
[0047] Step D-2 is for the heading angle solved in the motion state. Because drift error is introduced during the calculation process, so subtract the value obtained in step A through the motion state and time interval The final calculation removes the zero drift of the heading angle to obtain the calibrated heading angle.
[0048] E. Heading correction
[0049] Theoretically, during the straight-line movement of the lawn mower, the heading angle (the heading angle obtained by removing the drift error in step D) remains unchanged. Therefore, when the absolute value of the deviation of the heading angle change is greater than the angle threshold set in the algorithm (this angle threshold refers to the maximum amplitude value allowed for the heading angle change during straight-line movement), when it is greater than this angle threshold, the motor will adjust the speed of the two wheels to perform heading adjustment, continuously update the angle, and make a straight-line movement.
[0050] In this step, the heading angle at the start of the straight-line movement is recorded. Therefore, during the movement, when the heading angle deviation is greater than the angle threshold, the motor adjusts the direction by adjusting the speed of the two wheels of the lawn mower. Therefore, on this basis, to ensure the accuracy of the heading angle, the heading angle used in this process is the heading angle obtained by the calculation in step D.
[0051] Figure 4 This is a comparison chart of the effects of using and not using the algorithm for a 20m lawn mower. During the experiment, the distance of the lawn mower from the center line was measured every meter. Due to the unevenness of the grassland, the drift distance fluctuates, but the straight-line walking accuracy of the lawn mower has been improved to a great extent.
[0052] So far, a method for calculating the heading angle applied to the straight-line movement of a lawn mower has been realized. This method conducts straight-line tests on outdoor grasslands and, combined with motor calibration, can calculate the heading angle according to the judged motion state. Moreover, during the straight-line test, when encountering uneven grasslands or obstacles that cause deviation angles, it can also be calibrated in a timely manner, meeting the accuracy requirements for the straight-line movement of the lawn mower.
Claims
1. A method for heading calibration of a lawn mower moving in a straight line based on a gyroscope, characterized in that it specifically includes the following steps: A. Obtain the static data of the Z-axis of the gyroscope A-1. When the lawn mower is initialized and powered on, at this time the lawn mower is in a stationary state, obtain N consecutive, stationary Z-axis angular velocity values of the gyroscope with a sampling interval of period T; A-2. Cumulatively sum the Z-axis angular velocity values with respect to the period T to obtain the cumulative angle of heading drift in the stationary state within the time of these N sampling periods; Solve the heading angle After the lawn mower is powered on, the lawn mower obtains the data of the gyroscope with a period of T. Each time the data is obtained, the product of the Z-axis angular velocity value and T is calculated and added to the previously calculated heading angle, and this is continuously superimposed, denoted as the total heading angle from the start of power-on to the current moment; Judge the motion state C-1. According to the different characteristics of the Z-axis angular velocity value of the gyroscope in the moving and stationary states, set a state threshold to distinguish the state of the lawn mower; C-2. Adopt a sliding window, that is, each time the latest M consecutive Z-axis angular velocity values are stored in the sliding window, calculate their average value. If it is greater than the state threshold, it is determined to be in the moving state. If it is less than the state threshold, it is determined to be in the stationary state; Remove the heading angle drift error D-1. According to the state threshold, when it is judged that the lawn mower is in the stationary state, directly assign the Z-axis angular velocity value to 0 without introducing static drift error; D-2. When it is judged that the lawn mower is in the moving state, for each heading angle obtained by accumulating the product of N Ts and the Z-axis angular velocity value, subtract a cumulative angle of heading drift in the stationary state within the time of N sampling periods calculated in A-2 to remove the static drift error in the dynamic process and obtain the calibrated heading angle; Heading correction When the absolute value of the deviation of the calibrated heading angle change is greater than the set angle threshold, the motor adjusts the speed of the two wheels for heading adjustment, continuously updates the angle, and moves in a straight line.
2. The method for heading calibration of a lawn mower moving in a straight line based on a gyroscope according to claim 1, characterized in that: The state threshold described in step C-1 is comprehensively determined by combining the range, sensitivity of the gyroscope and the scenario where the gyroscope is applied.
3. The method for heading calibration of a lawn mower moving in a straight line based on a gyroscope according to claim 1, characterized in that: The angle threshold described in step E refers to the maximum amplitude value of the allowable change in the heading angle during the straight-line movement.