Heading angle correction method, self-moving equipment and storage medium

Abstract

This application proposes a heading angle correction method, electronic device, and storage medium for use in self-moving devices. The correction method includes: acquiring the observed heading angle at the current moment through a real-time differential positioning module and an inertial measurement unit; acquiring the fused heading angle and fused angular velocity at the previous moment; determining the time interval between the previous and current moments; and calculating the predicted heading angle at the current moment based on the fused heading angle, fused angular velocity, and time interval between the previous and current moments. Furthermore, based on the prediction and observation confidence levels at the current moment, the predicted and observed heading angles are fused to obtain a fused heading angle, which improves the accuracy of the acquired heading angle and enables precise positioning of the self-moving device.

Description

Technical Field

[0001] This application relates to the field of positioning technology, and in particular to a heading angle correction method, a self-moving device, and a computer-readable storage medium. Background Technology

[0002] During the operation of the self-propelled mobile device along the pre-planned path, it is necessary to obtain the accurate heading angle of the self-propelled mobile device in real time in order to control the self-propelled mobile device to better track the pre-planned path.

[0003] Currently, if the angle obtained by integrating the angular velocity of an IMU (Inertial Measurement Unit) is used alone as the heading angle of an automated mobile device, steady-state drift occurs during long-term operation. Furthermore, after the IMU's internal gyroscope experiences temperature drift, zero drift, or vibration, the steady-state error of the measured heading angle of the automated mobile device becomes significant. If RTK (Real-time Kinematic) technology is used alone to calculate the heading angle of the automated mobile device from the positioning coordinates, the angle jump is large within a short period of operation. It is evident that both IMU and RTK technologies have inherent biases and insufficient accuracy in heading positioning of automated mobile devices. Summary of the Invention

[0004] The main objective of this application is to propose a heading angle correction method, a self-moving device, and a computer-readable storage medium. The aim is to combine the prediction confidence and observation confidence at the current moment, fusing the predicted heading angle and observed heading angle of the self-moving device, thereby correcting the heading angle of the self-moving device based on the fused heading angle, and thus achieving accurate positioning of the self-moving device.

[0005] To achieve the above objectives, a first aspect of this application proposes a heading angle correction method applied to a self-moving device, the method comprising:

[0006] During the movement of the self-moving device, the current observation heading angle is obtained through the real-time differential positioning module and the inertial measurement unit;

[0007] Obtain the fused heading angle and the fused angular velocity of the previous moment;

[0008] Determine the time interval between the previous time and the current time;

[0009] The predicted heading angle at the current moment is calculated based on the time interval, the fused heading angle at the previous moment, and the fused angular velocity at the previous moment.

[0010] Determine the prediction confidence and observation confidence at the current moment, wherein the observation confidence is adjusted according to the positioning accuracy of the real-time differential positioning module;

[0011] Based on the prediction confidence and observation confidence at the current moment, the observed heading angle and the predicted heading angle at the current moment are fused to obtain the fused heading angle at the current moment;

[0012] The observed heading angle at the current moment is corrected based on the fused heading angle at the current moment.

[0013] The method in this application embodiment obtains the observed heading angle at the current moment through a real-time differential positioning module and an inertial measurement unit. Since the observed heading angle has a certain deviation, the predicted heading angle at the current moment can be calculated based on the fused heading angle and fused angular velocity at the previous moment, as well as the time interval between the previous moment and the current moment. Then, based on the prediction confidence and observation confidence at the current moment, the predicted heading angle and the observed heading angle at the current moment are fused to obtain the fused heading angle. This application can improve the accuracy of the heading angle. By adjusting the positioning accuracy of the real-time differential positioning module to adjust the prediction confidence and observation confidence at the current moment, the confidence of the predicted heading angle and the observation heading angle at the current moment can be determined. Therefore, by fusing the predicted heading angle and the observation heading angle at the current moment based on the prediction confidence and observation confidence, the obtained fused heading angle is more accurate, thereby achieving precise positioning of the self-moving device.

[0014] To achieve the above objectives, a second aspect of the present application provides a self-moving device, which includes a memory and a processor. The memory stores a computer program, and the processor executes the computer program to implement the method of the first aspect of the present application.

[0015] To achieve the above objectives, a third aspect of the present application provides a computer-readable storage medium storing a computer program that, when executed by a processor, implements the method of the first aspect of the present application.

[0016] It should be understood that the above general description and the following detailed description are exemplary and explanatory only, and do not limit this application. Attached Figure Description

[0017] Figure 1 This is a flowchart of the heading angle correction method provided in the embodiments of this application;

[0018] Figure 2This is a flowchart illustrating the steps of obtaining the observed heading angle at the current moment through a real-time differential positioning module and an inertial measurement unit, as provided in an embodiment of this application.

[0019] Figure 3 This is a flowchart of the steps to obtain the observed heading angle at the current moment based on the first positioning information, the second positioning information, and the angle change, provided in an embodiment of this application.

[0020] Figure 4 This is a flowchart provided in an embodiment of the present application for determining the movement state of a self-moving device based on the amount of angle change;

[0021] Figure 5 This is a flowchart illustrating the steps for obtaining the fused angular velocity at the previous moment, as provided in an embodiment of this application.

[0022] Figure 6 This is a flowchart illustrating the steps for obtaining the fused heading angle of the previous moment, as provided in an embodiment of this application.

[0023] Figure 7 This is a flowchart of the steps performed after obtaining the observed heading angle and the predicted heading angle at the current time, as provided in an embodiment of this application.

[0024] Figure 8 This is another flowchart of the heading angle correction method provided in the embodiments of this application;

[0025] Figure 9 This is a schematic diagram of the structure of the self-moving device provided in the embodiments of this application. Detailed Implementation

[0026] Exemplary embodiments will now be described more fully with reference to the accompanying drawings. However, these exemplary embodiments can be implemented in many forms and should not be construed as limited to the examples set forth herein; rather, they are provided to make the description of this application more comprehensive and complete, and to fully convey the concept of the exemplary embodiments to those skilled in the art. The drawings are merely illustrative of this application and are not necessarily drawn to scale. The same reference numerals in the drawings denote the same or similar parts, and therefore repeated descriptions of them will be omitted.

[0027] For self-moving devices, such as lawnmowers and food delivery robots, they often need to move along pre-planned paths during operation. In this case, it is necessary to obtain the accurate heading angle of the self-moving device in real time in order to control the self-moving device to better track the pre-planned path.

[0028] To obtain the heading angle of a self-moving device, related technologies employ an IMU for measurement, using the angle obtained by integrating the angular velocity as the heading angle of the self-moving device. Alternatively, related technologies also use RTK technology to calculate the heading angle of the self-moving device from the positioning coordinates.

[0029] However, when using an IMU for measurement, steady-state drift occurs during prolonged operation of the self-moving device. Furthermore, after the IMU's internal gyroscope experiences temperature drift, zero drift, or vibration, the steady-state error of the measured heading angle of the self-moving device becomes significant. RTK technology, on the other hand, exhibits large angle jumps within short periods of operation. Therefore, both IMU and RTK technologies result in biases and insufficient accuracy in heading positioning of self-moving devices.

[0030] Based on this, this application proposes a heading angle correction method for use with self-moving devices. This method can fuse the predicted heading angle and the observed heading angle of the self-moving device, thereby correcting the heading angle of the self-moving device based on the fused heading angle. This improves the accuracy of the acquired heading angle and enables precise positioning of the self-moving device.

[0031] Reference Figure 1 , Figure 1 This is a flowchart of the heading angle correction method provided in the embodiments of this application. Figure 1 The method shown is applied to self-moving equipment, and the heading angle correction method includes, but is not limited to, steps S110 to S170.

[0032] Step S110: During the movement of the self-moving device, the observation heading angle at the current moment is obtained through the real-time differential positioning module and the inertial measurement unit.

[0033] In this embodiment, the self-moving device generally moves along a pre-planned path during operation. The real-time position and heading angle of the self-moving device can be obtained through a real-time differential positioning module and an inertial measurement unit installed on it, thereby determining whether the self-moving device is moving along the pre-planned path. The real-time differential positioning module can be used to measure the self-moving device's position, velocity, and attitude information during movement. The inertial measurement unit can be used to measure the self-moving device's acceleration, angular velocity, and angle change information during movement.

[0034] Based on the output results of the real-time differential positioning module and the inertial measurement unit at various times, the observed heading angle of the self-moving device at each time moment can be calculated in this embodiment. Naturally, based on the output results of the real-time differential positioning module and the inertial measurement unit at the current time moment, the observed heading angle of the self-moving device at the current time moment can be calculated.

[0035] In this embodiment, the real-time differential positioning module exhibits significant deviations in its measurement results during short-term movement of the self-moving device, while the inertial measurement unit (IMU) shows substantial deviations during long-term movement. Consequently, the observed heading angles calculated from the outputs of the real-time differential positioning module and the IMU at various moments throughout the self-moving process are inaccurate. In other words, directly using the observed heading angles obtained from the real-time differential positioning module and the IMU is insufficient for precise positioning of the self-moving device. Therefore, it is necessary to correct these observed heading angles by fusing them to achieve accurate positioning of the self-moving device.

[0036] In one embodiment of this application, reference is made to Figure 2 , Figure 2 This is a flowchart illustrating the steps involved in obtaining the current observation heading angle using a real-time differential positioning module and an inertial measurement unit, as provided in an embodiment of this application. Figure 2 As shown, the observation heading angle at the current moment is obtained through the real-time differential positioning module and the inertial measurement unit, including but not limited to steps S210 to S230.

[0037] Step S210: Obtain the first positioning information from the previous moment and the second positioning information from the current moment through the real-time differential positioning module.

[0038] In this embodiment, the real-time differential positioning module can measure the location information of the mobile device. Therefore, based on the location information measured by the real-time differential positioning module, the first location information of the mobile device at the previous moment and the second location information at the current moment can be obtained.

[0039] For example, the position coordinates of the self-moving device at time k-1 can be measured by the real-time differential positioning module. Position coordinates at time k

[0040] Step S220: Obtain the angle change between the previous moment and the current moment through the inertial measurement unit.

[0041] For example, the angular change between time k and time k-1, which can be measured by an inertial measurement unit, can be denoted as Δθ.

[0042] Step S230: Based on the first positioning information, the second positioning information, and the angle change, obtain the observed heading angle at the current moment.

[0043] In this embodiment of the application, after obtaining the first positioning information of the self-moving device at the previous moment, the second positioning information at the current moment, and the angle change between the previous moment and the current moment, the observation heading angle of the self-moving device at the current moment can be calculated based on the first positioning information, the second positioning information, and the angle change.

[0044] In one embodiment of this application, reference is made to Figure 3 , Figure 3 This is a flowchart illustrating the steps involved in obtaining the observed heading angle at the current moment based on first positioning information, second positioning information, and angle change, as provided in an embodiment of this application. Figure 3 As shown, the observed heading angle at the current moment is obtained based on the first positioning information, the second positioning information, and the angle change, including but not limited to steps S310 to S330.

[0045] Step S310: Based on the first positioning information and the second positioning information, determine the first coordinate change corresponding to the first coordinate axis and the second coordinate change corresponding to the second coordinate axis, respectively.

[0046] Step S320: Based on the first coordinate change, the second coordinate change, the angle change, and the preset inverse trigonometric function formula, obtain the observed heading angle at the previous moment.

[0047] Step S330: Based on the observed heading angle and angle change at the previous moment, obtain the observed heading angle at the current moment.

[0048] In this system, the first coordinate axis can be the x-axis, and the second coordinate axis can be the y-axis. The first and second positioning information can be represented using x-axis and y-axis coordinates, respectively. Using the first and second positioning information, the change in the first coordinate along the x-axis and the change in the second coordinate along the y-axis can be determined. Then, based on the changes in the first and second coordinates, the angle change, and a preset inverse trigonometric function formula, the observed heading angle of the self-moving device at the previous moment can be calculated. Furthermore, based on the observed heading angle and the angle change at the previous moment, the observed heading angle at the current moment can be obtained.

[0049] For example, the position coordinates of the self-moving device at time k-1 (i.e., the previous time) measured by the real-time differential positioning module are: Position coordinates at time k (i.e., the current time) At this point, considering that the real-time differential positioning module outputs position coordinates in the RTK coordinate system, and that the position of the real-time differential positioning module installed on the self-moving device does not coincide with the motion center point of the self-moving device, a coordinate system transformation is required first, that is, converting the position coordinates in the RTK coordinate system to the position coordinates in the ODOM coordinate system. The RTK coordinate system is a coordinate system with the location of the real-time differential positioning module as its origin. The ODOM coordinate system is a coordinate system with the motion center point of the self-moving device as its origin. Specifically, if the position coordinates of the self-moving device at time k-1 in the ODOM coordinate system are set as... Let the position coordinates at time k be set as Then we have formula (1) as follows:

[0050]

[0051] In equation (1), M is the distance between the location point of the real-time differential positioning module and the center point of the self-moving device, and θ is the angle between the X-axis in the RTK coordinate system and the X-axis in the ODOM coordinate system.

[0052] The position coordinates of the self-moving device at time k-1 are obtained from the real-time differential positioning module. and the position coordinates at time k Then, according to formula (1), the position coordinates of the self-moving device at time k-1 in the ODOM coordinate system can be determined. and the position coordinates at time k After coordinate system transformation, the position coordinates at time k-1 after transformation can be used as a reference. and the position coordinates at time k The first coordinate change Δx and the second coordinate change Δy of the self-moving device at time k are calculated. The first coordinate change Δx and the second coordinate change Δy satisfy formula (2), which is as follows:

[0053]

[0054] At this time, if the movement state of the self-moving device is a straight-line driving state, that is, the angle change Δθ between time k and time k-1 measured by the inertial measurement unit is regarded as 0, then the first coordinate change Δx and the second coordinate change Δy satisfy formula (3), and formula (3) is as follows:

[0055]

[0056] In formula (3), Then, according to formula (3), we can obtain Therefore, the observed heading angle of the self-moving device at time k-1 can be calculated according to formula (4), which is: θ k-1=atan2(Δy, Δx)(4). At this point, since the angle change Δθ between time k and time k-1 is considered zero, the observed heading angle θ of the self-moving device at time k can be calculated. k =θ k-1 .

[0057] If the self-moving device is not in a straight line, such as when turning, the angle change between time k and time k-1 measured by the inertial measurement unit is Δθ. Then, the first coordinate change Δx, the second coordinate change, and the angle change Δθ satisfy formula (5). Formula (5) is:

[0058]

[0059] In formula (5), Then, by transforming formula (5), we can obtain formula (6) as follows:

[0060]

[0061] Based on formula (6), we can obtain formula (7), which is as follows:

[0062]

[0063] Therefore, according to formula (7), the observed heading angle θ of the self-moving device at time k-1 can be calculated. k-1 =atan2(sinΔθ*Δy+(cosΔθ-1)*Δx,sinΔθ*Δx+(1-cosΔθ)*Δy). Since the angle change between time k and time k-1 measured by the inertial measurement unit is Δθ, the observed heading angle θ of the self-moving device at time k can be calculated. k =θ k-1 +Δθ.

[0064] This embodiment of the application uses the position coordinates of the previous moment and the current moment measured and output by the real-time differential positioning module, and the angle change between the previous moment and the current moment measured and output by the inertial measurement unit, to calculate the observed heading angle of the self-moving device at the previous moment. Then, based on the observed heading angle of the previous moment and the angle change between the previous moment and the current moment, the observed heading angle of the self-moving device at the current moment can be calculated. It can be seen that the observed heading angle of the self-moving device at the current moment is calculated based on the observed heading angle of the previous moment.

[0065] In one embodiment of this application, reference is made to Figure 4 , Figure 4 This is a flowchart illustrating how the movement state of a self-moving device is determined based on the change in angle, as provided in an embodiment of this application. Figure 4As shown, determining the movement state of the self-moving device based on the angle change includes steps S410 to S440.

[0066] Step S410: Obtain the angle change between the previous moment and the current moment through the inertial measurement unit.

[0067] Step S420: Determine whether the change in angle is greater than a preset angle threshold.

[0068] Step S430: If the angle change is greater than the preset angle threshold, determine that the movement state of the self-moving device is a turning driving state.

[0069] Step S440: If the angle change is not greater than the preset angle threshold, determine that the movement state of the self-moving device is a straight-line driving state.

[0070] Since the heading angle reflects the orientation of the automated device during movement, when the orientation of the automated device remains unchanged, it indicates that the device is moving in a straight line, meaning its movement state can be straight-line travel. When the orientation of the automated device continuously changes, it indicates that the device is moving in a curved direction, meaning its movement state can be turning. In this embodiment, after obtaining the angle change Δθ between the previous and current moments through the inertial measurement unit, the magnitude of the angle change Δθ can be further determined to be greater than a preset angle threshold. If the angle change Δθ is greater than the preset angle threshold, the device's movement state can be determined to be turning. If the angle change Δθ is not greater than the preset angle threshold, the device's movement state can be determined to be straight-line travel.

[0071] It is understandable that, considering the influence of air resistance, frictional resistance, and other factors, self-moving devices generally cannot travel in a perfectly straight line during a long period of travel. That is, the change in angle Δθ between the previous moment and the current moment is generally difficult to be zero. In this embodiment, a preset angle threshold is set. If the change in angle Δθ is not greater than the preset angle threshold, the movement state of the self-moving device is considered as a straight-line travel state.

[0072] It is understandable that the preset angle threshold can be set and adjusted according to the actual situation, and the preset angle threshold is a small angle value.

[0073] In this embodiment, the movement state of the self-moving device can be determined by comparing the angle change between the previous and current moments obtained by the inertial measurement unit with a preset angle threshold. Based on this movement state, a formula for calculating the observed heading angle of the self-moving device at the current moment can be determined. For example, if the movement state of the self-moving device is determined to be straight-line travel, the observed heading angle at the current moment can be directly calculated based on the first positioning information from the previous moment and the second positioning information from the current moment, as measured and output by the real-time differential positioning module. This reduces the calculation of parameters such as angle change, improves computational efficiency, and ultimately improves positioning efficiency.

[0074] Step S120: Obtain the fused heading angle and fused angular velocity of the previous moment.

[0075] In this embodiment, after obtaining the observed heading angle at the current moment based on the real-time differential positioning module and the inertial measurement unit, considering that the obtained observed heading angle at the current moment is not accurate enough to accurately position the self-moving device, it is necessary to further obtain the predicted heading angle at the current moment. This predicted heading angle is then fused with the observed heading angle obtained in step S110 to correct the observed heading angle at the current moment based on the fusion result. To obtain the predicted heading angle at the current moment, it is necessary to first obtain the fused heading angle and fused angular velocity from the previous moment.

[0076] In one embodiment of this application, reference is made to Figure 5 , Figure 5 This is a flowchart illustrating the steps for obtaining the fused angular velocity at the previous moment, as provided in an embodiment of this application. Figure 5 As shown, obtaining the fusion angular velocity at the previous moment includes, but is not limited to, steps S510 to S540.

[0077] Step S510: Obtain the observed angular velocity at the previous moment through the inertial measurement unit.

[0078] Step S520: Determine the predicted angular velocity of the previous moment. The predicted angular velocity of the previous moment is the fused angular velocity corresponding to the moment before the previous moment.

[0079] Step S530: Determine the reliability of the angular velocity prediction and the reliability of the angular velocity observation at the previous moment.

[0080] Step S540: Based on the angular velocity prediction confidence and angular velocity observation confidence of the previous moment, the predicted angular velocity and the observed angular velocity of the previous moment are fused to obtain the fused angular velocity of the previous moment.

[0081] In this embodiment, the observed angular velocity w of the self-moving device at the previous moment is obtained through an inertial measurement unit. k-1The fused angular velocity corresponding to the previous moment. As the predicted angular velocity of the previous moment, i.e. Then, the confidence level of the angular velocity prediction at the previous moment is determined as follows: The reliability of the angular velocity observation at the previous moment is determined to be: Therefore, the reliability can be predicted based on the angular velocity at the previous moment. Reliability of angular velocity observations from the previous moment The predicted angular velocity at the previous moment and the observed angular velocity w at the previous moment k-1 Perform fusion to obtain the fusion angular velocity from the previous moment. Angular velocity fusion can then be performed using formula (8). Formula (8) is: in, That is, the sum of the confidence level of the predicted angular velocity at the previous moment and the confidence level of the observed angular velocity at the previous moment is 1.

[0082] It should be noted that the reliability of angular velocity prediction and angular velocity observation can be determined by parameter adjustment during the measurement process of the inertial measurement unit (IMU). The reliability of angular velocity prediction at different times is not entirely the same, nor is the reliability of angular velocity observation at different times.

[0083] In one embodiment of this application, Figure 1 The method shown also includes:

[0084] When the inertial measurement unit is not detected to output an angle change, the product of the fused angular velocity at the previous moment and the time interval is used as the angle change.

[0085] In this embodiment of the application, since the inertial measurement unit can also measure the observed angular velocity of the self-moving device, and then according to... Figure 5 The steps shown allow us to obtain the fused angular velocity at the previous moment. Therefore, when the inertial measurement unit does not directly output the angle change, the product of the fused angular velocity at the previous moment and the time interval can be used as the angle change. That is, the angle change between the previous moment and the current moment can be calculated using the following formula (9):

[0086]

[0087] Where Δθ is the change in angle between the previous moment and the current moment. Δt represents the fusion angular velocity at the previous moment, and Δt represents the time interval between the previous moment and the current moment.

[0088] In this embodiment of the application, when the inertial measurement unit does not output the angle change, the product of the fused angular velocity and the time interval at the previous moment is used as the angle change. This ensures that the angle change is obtained, thereby ensuring that the observed heading angle can be calculated based on the angle change, and ultimately ensuring the positioning of the self-moving device.

[0089] In one embodiment of this application, reference is made to Figure 6 , Figure 6 This is a flowchart illustrating the steps for obtaining the fused heading angle from the previous moment, as provided in an embodiment of this application. Figure 6 As shown, obtaining the fused heading angle of the previous moment includes, but is not limited to, steps S610 to S640.

[0090] Step S610: Obtain the predicted heading angle of the previous moment.

[0091] Step S620: Obtain the observed heading angle of the previous moment through the real-time differential positioning module and the inertial measurement unit.

[0092] Step S630: Determine the confidence level of the angle prediction at the previous moment and the confidence level of the angle observation at the previous moment.

[0093] Step S640: Based on the confidence level of the angle prediction and the confidence level of the angle observation at the previous moment, the predicted heading angle and the observed heading angle at the previous moment are fused and calculated to obtain the fused heading angle at the previous moment.

[0094] In this embodiment, the predicted heading angle of the previous moment is first obtained. Then, the observed heading angle θ at the previous moment is obtained through the real-time differential positioning module and the inertial measurement unit. k-1 The system can obtain the first positioning information from the previous moment and the second positioning information from the current moment based on the real-time differential positioning module. Then, it can obtain the angle change between the previous moment and the current moment based on the inertial measurement unit. Finally, based on the first positioning information, the second positioning information, and the angle change, it can calculate the observed heading angle θ from the previous moment. k-1 Next, the confidence level of the angle prediction at the previous moment is determined to be... The reliability of the angle observation compared to the previous moment is Therefore, the credibility can be predicted based on the angle at the previous moment. Reliability of the angle observation compared to the previous moment The predicted heading angle at the previous moment The observed heading angle θ at the previous moment k-1 Perform fusion calculations to obtain the fused heading angle from the previous time step. The fused heading angle of the previous moment can be calculated using the following formula (10):

[0095]

[0096] in, That is, the sum of the confidence level of the angle prediction at the previous moment and the confidence level of the angle observation at the previous moment is 1.

[0097] It should be noted that the predicted heading angle at the previous moment... It needs to be calculated using formula (11). Formula (11) is: That is, the predicted heading angle at time k-1 It needs to be calculated based on the merged heading angle at time k-2, the merged angular velocity at time k-2, and the time interval between time k-1 and time k-2.

[0098] It should be noted that the reliability of angle predictions is not entirely the same at different times, nor is the reliability of angle observations. When the positioning accuracy of the real-time differential positioning module is poor, the reliability of angle observations decreases, and the predicted heading angle is relied upon more, thus increasing the reliability of angle predictions.

[0099] Step S130: Determine the time interval between the previous time and the current time.

[0100] In this embodiment of the application, in order to obtain the predicted heading angle at the current moment, after obtaining the fused heading angle and the fused angular velocity at the previous moment, it is also necessary to obtain the time interval between the previous moment and the current moment. Only then can the predicted heading angle at the current moment be calculated based on the fused heading angle, the fused angular velocity at the previous moment and the time interval.

[0101] Step S140: Calculate the predicted heading angle for the current moment based on the time interval, the fused heading angle of the previous moment, and the fused angular velocity of the previous moment.

[0102] In this embodiment of the application, the predicted heading angle at the current moment is calculated using formula (12), which is as follows:

[0103]

[0104] In formula (12), This indicates the predicted heading angle at the current moment. This indicates the fused heading angle at the previous moment. Δt represents the fusion angular velocity at the previous moment, and Δt represents the time interval between the previous moment and the current moment.

[0105] Step S150: Determine the prediction confidence and observation confidence at the current moment, wherein the observation confidence is adjusted according to the positioning accuracy of the real-time differential positioning module.

[0106] In this embodiment, after obtaining the observed heading angle and the predicted heading angle at the current moment, it is necessary to further determine the prediction confidence and observation confidence at the current moment. Since the observed heading angle is calculated using the measurement results of the real-time differential positioning module, the corresponding observation confidence can be adjusted according to the positioning accuracy of the real-time differential positioning module. For example, if the observed heading angle calculated by the real-time differential positioning module is detected to have more than two angle jumps greater than a certain value... When the positioning accuracy of the real-time differential positioning module is poor, the observation reliability can be reduced, meaning more trust can be placed in the predicted heading angle. This makes the final fused heading angle more closely resemble the predicted heading angle, thereby improving the accuracy of the fused heading angle.

[0107] For example, when the positioning accuracy of the real-time differential positioning module is low, the observation confidence level is adjusted to 0.1. At this point, since the sum of the prediction confidence level and the observation confidence level is 1, the prediction confidence level is determined to be 0.9. It can be seen that after adjusting the observation confidence level according to the positioning accuracy of the real-time differential positioning module, the weights of the predicted heading angle and the observed heading angle in the fusion process are effectively determined.

[0108] The embodiments of this application adjust the observation reliability based on the positioning accuracy of the real-time differential positioning module, which can improve the accuracy of the observation reliability and thus improve the accuracy of the fused heading angle.

[0109] Step S160: Based on the prediction confidence and observation confidence at the current moment, fuse the observed heading angle and the predicted heading angle at the current moment to obtain the fused heading angle at the current moment.

[0110] In this embodiment, the fused heading angle at the current moment is calculated using formula (13), which is as follows:

[0111]

[0112] In formula (13), This indicates the fused heading angle at the current moment. This indicates the predicted heading angle at the current moment. θ represents the prediction confidence level at the current moment. k This indicates the observed heading angle at the current moment. This indicates the reliability of the observation at the current moment.

[0113] This application embodiment fuses the observed heading angle and the predicted heading angle at the current moment based on the prediction confidence and observation confidence at the current moment. In fact, it combines the measurement results of the real-time differential positioning module and the measurement results of the inertial measurement unit to overcome the problem that the real-time differential positioning module will have deviations when the self-moving device is running for a short time, and to overcome the problem that the inertial measurement unit has large angle jumps when the self-moving device is running for a long time.

[0114] In one embodiment of this application, reference is made to Figure 7 , Figure 7 This is a flowchart illustrating the steps performed after obtaining the observed heading angle and the predicted heading angle at the current moment, as provided in an embodiment of this application. Figure 7 As shown, after obtaining the observed heading angle and the predicted heading angle at the current time, the heading angle correction method also includes, but is not limited to, steps S710 to S730.

[0115] Step S710: Determine the angle difference between the observed heading angle and the predicted heading angle at the current moment;

[0116] Step S720: If the angle difference is greater than the preset angle threshold, the preset angle value and the observed heading angle at the current moment are added together to obtain the target heading angle.

[0117] Step S730: Take the target heading angle as the new observed heading angle at the current moment, and perform the step of fusing the observed heading angle and the predicted heading angle at the current moment according to the prediction confidence and observation confidence at the current moment to obtain the fused heading angle at the current moment.

[0118] In this embodiment, after obtaining the observed heading angle at the current moment through step S110 and the predicted heading angle at the current moment through steps S120 to S140, the angle difference between the observed heading angle and the predicted heading angle at the current moment can be further determined. If the angle difference is greater than a preset angle threshold, the preset angle value is added to the observed heading angle at the current moment to obtain the target heading angle. Then, after using the target heading angle as the new observed heading angle at the current moment, step S160 is executed. That is, based on the prediction confidence and observation confidence at the current moment, the new observed heading angle at the current moment and the predicted heading angle at the current moment are fused to obtain the fused heading angle at the current moment.

[0119] For example, when the angle difference between the observed heading angle and the predicted heading angle at the current moment is greater than π, 2π or -2π is added to the observed heading angle at the current moment to obtain the target heading angle. The target heading angle is then used as the new observed heading angle at the current moment to perform subsequent fusion calculations.

[0120] This embodiment of the application takes into account that when the angle difference between the observed heading angle and the predicted heading angle at the current moment is greater than a preset angle threshold, such as greater than π, an angle jump problem may occur. In this case, by adding the preset angle value to the observed heading angle at the current moment, such as adding 2π or -2π to the observed heading angle at the current moment, it can be ensured that the directions of the observed heading angle and the predicted heading angle are consistent.

[0121] Step S170: Correct the observed heading angle at the current moment based on the fused heading angle at the current moment.

[0122] In this embodiment, after obtaining the fused heading angle at the current moment, the observed heading angle at the current moment can be corrected based on the fused heading angle at the current moment. Since the fused heading angle is obtained by fusing the observed heading angle and the predicted heading angle based on the observation confidence and the prediction confidence, the fused heading angle is more accurate and can achieve precise positioning of the self-moving device.

[0123] In one embodiment of this application, reference is made to Figure 8 , Figure 8 This is another flowchart of the heading angle correction method provided in the embodiments of this application, applied to self-moving equipment. For example... Figure 8 As shown, the heading angle correction method includes, but is not limited to, steps S810 to S890.

[0124] Step S810: During the movement of the self-moving device, the current observation heading angle is obtained through the real-time differential positioning module and the inertial measurement unit;

[0125] Step S820: Obtain the observed angular velocity at the current moment through the inertial measurement unit;

[0126] Step S830: Obtain the fused heading angle and fused angular velocity of the previous moment;

[0127] Step S840: Determine the time interval between the previous time and the current time;

[0128] Step S850: Calculate the predicted heading angle for the current moment based on the time interval, the fused heading angle of the previous moment, and the fused angular velocity of the previous moment.

[0129] Step S860: Use the fused angular velocity from the previous moment as the predicted angular velocity for the current moment;

[0130] Step S870: Determine the prediction confidence and observation confidence at the current moment; wherein, the observation confidence is adjusted according to the positioning accuracy of the real-time differential positioning module.

[0131] Step S880: Based on the observed heading angle, observed angular velocity, predicted heading angle, predicted angular velocity, prediction confidence, and observation confidence at the current moment, obtain the fused heading angle at the current moment.

[0132] Step S890: Correct the observed heading angle at the current moment based on the fused heading angle at the current moment.

[0133] In this embodiment of the application, during the movement of the self-moving device, the observed heading angle θ at the current moment can be obtained through a real-time differential positioning module and an inertial measurement unit. k The current observation angular velocity w can be obtained through the inertial measurement unit. k The current observed heading angle θ k and the observed angular velocity w at the current moment k Construct the observation matrix Y at the current time. k ,

[0134] This application embodiment obtains the fused heading angle from the previous moment. Blending angular velocity with the previous moment And determine the time interval Δt between the previous moment and the current moment. Therefore, based on the time interval Δt and the fused heading angle of the previous moment... fusion angular velocity at the previous moment The predicted heading angle at the current moment is calculated. That is, through the formula The predicted heading angle at the current moment can be calculated. Then the fusion angular velocity of the previous moment As the predicted angular velocity at the current moment Right now The predicted heading angle at the current moment and the predicted angular velocity at the current moment Construct the prediction matrix for the current time step

[0135] Therefore, the observation matrix Y at the current moment is obtained by constructing it. k and predictor matrix Then, substitute the values ​​into formula (14) for fusion. Formula (14) is as follows:

[0136]

[0137] In formula (14), This represents the fusion matrix at time k, including the fusion heading angle and fusion angular velocity at time k. kThe measurement matrix at time k includes the observed heading angle and observed angular velocity at time k. This represents the prediction matrix at time k, including the predicted heading angle and predicted angular velocity at time k. k Represents the prediction matrix at time k and the observation matrix Y k The weighted coefficient matrix between them.

[0138] Wherein, the weighting coefficient matrix K k It can be calculated using formula (15):

[0139] K k =p k-1 (R k +p k-1 ) -1 (15).

[0140] In formula (15), R k p represents the confidence matrix of the observed heading angle and observed angular velocity at time k. k-1 Let p represent the confidence covariance matrix of the heading angle and angular velocity at time k-1. k-1 It can be calculated using formula (16):

[0141]

[0142] In formula (16), I represents the identity matrix, K k-1 p represents the weighting coefficient matrix at time k-1. k-2 R represents the confidence covariance matrix of the heading angle and angular velocity at time k-2. k-1 This represents the confidence matrix of the observed heading angle and observed angular velocity at time k-1. Furthermore, the confidence covariance matrix of the heading angle and angular velocity at time k is obtained.

[0143] Finally obtained in, This indicates the confidence level of the heading angle at time k. The confidence level of the angular velocity at time k. This indicates the correlation between the heading angle and angular velocity at time k.

[0144] Therefore, through formulas (15) and (16), it can be seen that the prediction matrix at time k is... and the observation matrix Y k The weighted coefficient matrix K between k The confidence covariance matrix p of the heading angle and angular velocity at time k-1 k-1 The confidence matrix R of the observed heading angle and observed angular velocity at time kk The confidence covariance matrix of the heading angle and angular velocity at time k-1 needs to be determined using the confidence covariance matrix p of the heading angle and angular velocity at time k-2. k-2 Prediction matrix at time k-1 and the observation matrix Y k The weighted coefficient matrix K between k-1 The confidence matrix R of the observed heading angle and observed angular velocity at time k-1 k-1 Confirmed. That is, in order to obtain the prediction matrix at time k... and the observation matrix Y k The weighted coefficient matrix K between k First, we need to obtain the confidence covariance matrix p of the heading angle and angular velocity at time k-2. k-2 Prediction matrix at time k-1 and the observation matrix Y k The weighted coefficient matrix K between k-1 The confidence matrix R of the observed heading angle and observed angular velocity at time k-1 k-1 The confidence matrix R of the observed heading angle and observed angular velocity at time k k Only then can we rely on p k-2 K k-1 R k-1 and R k These four quantities are used to calculate K. k .

[0145] In this embodiment, since the predicted heading angle at the current moment needs to be calculated using the fused heading angle and fused angular velocity from the previous moment, and the predicted angular velocity at the current moment needs to be obtained from the fused angular velocity from the previous moment, it is obtained through the formula. For the observation matrix Y k (including observed heading angle and observed angular velocity) and prediction matrix The fusion process (including predicted heading angle and predicted angular velocity) is actually an iterative process. It uses the previous fusion result to calculate the predicted values ​​for the current fusion, then uses these predicted values ​​to calculate the current fusion result. Next, it uses the current fusion result to calculate the predicted values ​​needed for the next fusion, and so on, iterating to output the fusion result at each moment, i.e., the fusion matrix (including fused heading angle and fused angular velocity). The output fused heading angle can be used to correct the observed heading angle at the corresponding moment for accurate positioning of the mobile device.

[0146] It should be noted that since the current fusion value needs to be calculated using the current prediction value, and the current prediction value needs to be calculated using the previous fusion value, it is clear that an initial value must be given to the fusion value or prediction value in order to carry out the subsequent iterative process. Therefore, in this embodiment, at the beginning of fusion, an initial matrix is ​​assigned to the fusion value matrix based on the calculated observation matrix, so that the prediction value matrix can be calculated based on the initial matrix, and then the iterative fusion steps can be executed.

[0147] The observed heading angle and observed angular velocity obtained by the real-time differential positioning module and inertial measurement unit in this embodiment are not accurate enough. Therefore, it is necessary to further obtain the predicted heading angle and predicted angular velocity at the current moment. Then, the obtained observed heading angle, observed angular velocity, predicted heading angle, and predicted angular velocity are fused according to the prediction confidence and observation confidence to obtain a highly accurate fused heading angle. Thus, by correcting the observed heading angle through the fused heading angle, precise positioning of the self-moving device can be achieved.

[0148] Please see Figure 9 , Figure 9 This is a schematic diagram of the structure of the self-moving device provided in an embodiment of this application. The self-moving device includes:

[0149] The processor 901 can be implemented using a general-purpose CPU (Central Processing Unit), microprocessor, ASIC (Application Specific Integrated Circuit), or one or more integrated circuits, and is used to execute relevant programs to implement the technical solutions provided in the embodiments of this application.

[0150] The memory 902 can be implemented in the form of ROM (Read Only Memory), static storage device, dynamic storage device, or RAM (Random Access Memory). The memory 902 can store the operating system and other application programs. When the technical solutions provided in the embodiments of this specification are implemented through software or firmware, the relevant program code is stored in the memory 902 and called and executed by the processor 901 using the heading angle correction method of the embodiments of this application.

[0151] The input / output interface 903 is used to implement information input and output;

[0152] The communication interface 904 is used to enable communication and interaction between this device and other devices. Communication can be achieved through wired means (such as USB, Ethernet cable, etc.) or wireless means (such as mobile network, WIFI, Bluetooth, etc.).

[0153] Bus 905 transmits information between various components of the device (e.g., processor 901, memory 902, input / output interface 903, and communication interface 904);

[0154] The processor 901, memory 902, input / output interface 903, and communication interface 904 are connected to each other within the device via bus 905.

[0155] The self-moving device in this application embodiment can move effectively and accurately by automatically correcting the heading angle, ensuring that it can move along a pre-planned path.

[0156] This application embodiment also provides a storage medium, which is a computer-readable storage medium, storing a computer program that, when executed by a processor, implements the above-described heading angle correction method.

[0157] Memory, as a non-transitory computer-readable storage medium, can be used to store non-transitory software programs and non-transitory computer-executable programs. Furthermore, memory may include high-speed random access memory, and may also include non-transitory memory, such as at least one disk storage device, flash memory device, or other non-transitory solid-state storage device. In some embodiments, memory may optionally include memory remotely located relative to the processor, and these remote memories can be connected to the processor via a network. Examples of such networks include, but are not limited to, the Internet, intranets, local area networks, mobile communication networks, and combinations thereof.

[0158] The embodiments described in this application are for the purpose of more clearly illustrating the technical solutions of the embodiments of this application, and do not constitute a limitation on the technical solutions provided by the embodiments of this application. As those skilled in the art will know, with the evolution of technology and the emergence of new application scenarios, the technical solutions provided by the embodiments of this application are also applicable to similar technical problems.

Claims

1. A heading angle correction method, characterized in that, Applied to self-moving devices, the method includes: During the movement of the self-moving device, the current observation heading angle is obtained through the real-time differential positioning module and the inertial measurement unit; Obtain the fused heading angle and the fused angular velocity of the previous moment; Determine the time interval between the previous time and the current time; The predicted heading angle at the current moment is calculated based on the time interval, the fused heading angle at the previous moment, and the fused angular velocity at the previous moment. Determine the prediction confidence and observation confidence at the current moment, wherein the observation confidence is adjusted according to the positioning accuracy of the real-time differential positioning module; Based on the prediction confidence and observation confidence at the current moment, the observed heading angle and the predicted heading angle at the current moment are fused to obtain the fused heading angle at the current moment; Based on the fused heading angle at the current moment, the observed heading angle at the current moment is corrected; The process of obtaining the current observation heading angle through a real-time differential positioning module and an inertial measurement unit includes: The real-time differential positioning module obtains the first positioning information from the previous moment and the second positioning information from the current moment. The angle change between the previous moment and the current moment is obtained through the inertial measurement unit; The observed heading angle at the current moment is obtained based on the first positioning information, the second positioning information, and the angle change.

2. The method according to claim 1, characterized in that, Obtaining the fused heading angle of the previous moment includes: Obtain the predicted heading angle of the previous moment; The observed heading angle at the previous moment is obtained through the real-time differential positioning module and the inertial measurement unit; Determine the confidence level of the angle prediction at the previous moment and the confidence level of the angle observation at the previous moment; Based on the confidence level of the angle prediction at the previous moment and the confidence level of the angle observation at the previous moment, the predicted heading angle at the previous moment and the observed heading angle at the previous moment are fused and calculated to obtain the fused heading angle at the previous moment.

3. The method according to claim 1, characterized in that, Obtaining the fused angular velocity at the previous moment includes: The observed angular velocity at the previous moment is obtained through the inertial measurement unit; Determine the predicted angular velocity of the previous moment, wherein the predicted angular velocity of the previous moment is the fused angular velocity corresponding to the moment before the previous moment; Determine the reliability of the angular velocity prediction and the reliability of the angular velocity observation at the previous moment; Based on the reliability of the predicted angular velocity and the reliability of the observed angular velocity at the previous moment, the predicted angular velocity at the previous moment and the observed angular velocity at the previous moment are fused to obtain the fused angular velocity at the previous moment.

4. The method according to claim 1, characterized in that, The step of obtaining the observed heading angle at the current moment based on the first positioning information, the second positioning information, and the angle change includes: Based on the first positioning information and the second positioning information, the first coordinate change corresponding to the first coordinate axis and the second coordinate change corresponding to the second coordinate axis are determined respectively; The observed heading angle at the previous moment is obtained based on the first coordinate change, the second coordinate change, the angle change, and the preset inverse trigonometric function formula. The observed heading angle at the current moment is obtained based on the observed heading angle at the previous moment and the change in angle.

5. The method according to claim 4, characterized in that, The method further includes: When the change in angle is less than or equal to a preset angle threshold, the movement state of the self-moving device is determined to be a straight-line driving state. When the change in angle is greater than a preset angle threshold, the movement state of the self-moving device is determined to be a turning driving state.

6. The method according to any one of claims 1-5, characterized in that, The method further includes: When the inertial measurement unit is not detected to output an angle change, the product of the fused angular velocity at the previous moment and the time interval is taken as the angle change.

7. The method according to any one of claims 1-5, characterized in that, The method further includes: Determine the angle difference between the observed heading angle at the current moment and the predicted heading angle at the current moment; If the angle difference is greater than a preset angle threshold, the preset angle value and the observed heading angle at the current moment are added together to obtain the target heading angle. The target heading angle is used as the new observed heading angle at the current moment, and the step of fusing the observed heading angle and the predicted heading angle at the current moment based on the prediction confidence and observation confidence at the current moment is performed to obtain the fused heading angle at the current moment.

8. A self-moving device, characterized in that, The self-moving device includes a memory and a processor, the memory storing a computer program, and the processor executing the computer program to implement the method according to any one of claims 1 to 7.

9. A computer-readable storage medium storing a computer program, characterized in that, When the computer program is executed by a processor, it implements the method of any one of claims 1 to 7.

Images