Method and device for calculating a heading angle of a self-moving device, self-moving device and medium

Abstract

The application provides a heading angle calculation method and device of a self-moving device, the self-moving device and a computer readable storage medium, and belongs to the positioning technical field of the self-moving device. In each detection period, the satellite positioning module is used to acquire the positioning data of the self-moving device, and the angle sensor is used to acquire the angle change amount of the self-moving device, so that the observed heading angle and the predicted heading angle can be calculated according to the positioning data and the angle change amount. According to the positioning accuracy of the angle change amount and the positioning data, the positioning error of the positioning data can be obtained, so that the confidence of the observed heading angle can be further determined according to the positioning error of the positioning data. Compared with the confidence preset in the related art, the method can obtain a more accurate confidence. Through the fusion of the observed heading angle and the predicted heading angle by the more accurate confidence, the accuracy of the fusion can be improved, so that the accurate positioning of the self-moving device can be realized.

Description

Technical Field

[0001] This application relates to the field of self-moving device positioning technology, and in particular to a method, apparatus, self-moving device, and computer-readable storage medium for calculating the heading angle of a self-moving device. 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, considering that both IMU (Inertial Measurement Unit) and RTK (Real-time Kinematic) technologies have biases and insufficient accuracy in heading positioning of self-moving devices, the predicted heading angle and observed heading angle of the self-moving device are fused together by combining the confidence level of the observations, thereby achieving accurate positioning of the self-moving device.

[0004] However, if the confidence level of the observations is inaccurate during the fusion process, the final fusion result will also be inaccurate. Therefore, determining the confidence level of the heading angle observed by the self-moving device during the localization process is an important problem that urgently needs to be solved in this field. Summary of the Invention

[0005] This application proposes a method, apparatus, self-moving device, and computer-readable storage medium for calculating the heading angle of a self-moving device. The confidence level of the observed heading angle can be determined by the positioning error of the positioning data, making the obtained confidence level more accurate, thereby ensuring the fusion result and achieving precise positioning of the self-moving device.

[0006] To achieve the above objectives, a first aspect of this application proposes a method for calculating the heading angle of a self-moving device, the method comprising:

[0007] In each detection cycle, the positioning data of the self-moving device is obtained through the satellite positioning module, and the angle change of the self-moving device is obtained through the angle sensor;

[0008] Calculate the observed heading angle and the predicted heading angle based on the positioning data and the angle change;

[0009] The positioning error of the positioning data is obtained based on the angle change and the positioning accuracy of the positioning data.

[0010] The confidence level of the observed heading angle is determined based on the positioning error of the positioning data;

[0011] The target heading angle of the self-moving device is obtained by fusing and correcting the observed heading angle, the predicted heading angle, and the confidence level of the observed heading angle.

[0012] The method in this embodiment acquires positioning data of the self-moving device through a satellite positioning module and acquires the angle change of the self-moving device through an angle sensor in each detection cycle. Based on the positioning data and the angle change, the observed heading angle and the predicted heading angle can be calculated. According to the angle change and the positioning accuracy of the positioning data, the positioning error of the positioning data can be obtained. This positioning error can then be used to determine the confidence level of the observed heading angle, making the obtained confidence level more accurate. By fusing the observed heading angle and the predicted heading angle with a more accurate confidence level, the accuracy of the fusion can be improved, thereby achieving precise positioning of the self-moving device.

[0013] To achieve the above objectives, a second aspect of this application provides a heading angle calculation device for a self-moving device, characterized in that the device comprises:

[0014] The first acquisition module is used to acquire the positioning data of the self-moving device through the satellite positioning module and the angle change of the self-moving device through the angle sensor in each detection cycle.

[0015] The first calculation module is used to calculate the observed heading angle and the predicted heading angle based on the positioning data and the angle change.

[0016] The second acquisition module is used to obtain the positioning error of the positioning data based on the angle change and the positioning accuracy of the positioning data;

[0017] The determination module is used to determine the confidence level of the observed heading angle based on the positioning error of the positioning data;

[0018] The fusion module is used to perform fusion correction based on the observed heading angle, the predicted heading angle, and the confidence level of the observed heading angle to obtain the target heading angle of the self-moving device.

[0019] To achieve the above objectives, a third aspect of the present application provides an electronic device, the electronic device including a memory and at least one processor, the memory storing a computer program, and the at least one processor executing the computer program to implement the method of the first aspect of the present application.

[0020] To achieve the above objectives, a fourth 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.

[0021] 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

[0022] Figure 1 This is a flowchart of the heading angle calculation method for a self-moving device provided in an embodiment of this application;

[0023] Figure 2 This is a flowchart of the steps to obtain the positioning error of the positioning data based on the angle change and the positioning accuracy of the positioning data, provided in an embodiment of this application.

[0024] Figure 3 This is a flowchart of the steps performed before determining the confidence level of the observed heading angle based on the positioning error of the positioning data, as provided in an embodiment of this application.

[0025] Figure 4 This is a flowchart of the steps for determining the confidence level of the observed heading angle based on positioning error, trajectory error, and displacement error, provided in an embodiment of this application.

[0026] Figure 5 This is a schematic diagram of the heading angle calculation device for the self-moving device provided in the embodiments of this application;

[0027] Figure 6 This is a schematic diagram of the hardware structure of the electronic device provided in the embodiments of this application. Detailed Implementation

[0028] 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.

[0029] 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.

[0030] 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.

[0031] 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.

[0032] Therefore, in some schemes, the predicted heading angle and the observed heading angle of the self-moving device can be fused by combining the confidence level of the observations, which can improve the accuracy of the acquired heading angle and achieve precise positioning of the self-moving device.

[0033] However, if the confidence level of the observations is inaccurate during the fusion process, the final fusion result will also be inaccurate. Therefore, determining the accurate confidence level of the observed heading angle is an important prerequisite for ensuring the fusion result and achieving precise positioning.

[0034] Based on this, this application proposes a method for calculating the heading angle of a self-moving device. The method determines the confidence level of the observed heading angle by using the positioning error of the positioning data, making the obtained confidence level more accurate. This ensures the accuracy of the fusion results and achieves precise positioning of the self-moving device.

[0035] Reference Figure 1 , Figure 1 This is a flowchart of the heading angle calculation method for a self-moving device provided in the embodiments of this application, including but not limited to steps S110 to S150.

[0036] In step S110, during each detection cycle, the positioning data of the self-moving device is obtained through the satellite positioning module, and the angle change of the self-moving device is obtained through the angle sensor.

[0037] In this embodiment, the self-moving device generally moves along a pre-planned path when performing operations. The real-time position and heading angle of the self-moving device can be obtained through a satellite positioning module and angle sensor installed on it, thereby determining whether the self-moving device is moving along the pre-planned path.

[0038] The satellite positioning module can be used to measure the positioning data of the mobile device during its movement, including location information and positioning accuracy. The specific type of satellite positioning module can be determined according to actual needs. For example, the satellite positioning module may include one or more of the following: Global Positioning System (GPS), Real-time Kinematic (RTK) carrier phase differential module, and BeiDou Navigation Satellite System (BDS).

[0039] Angle sensors can be used to measure one or more of the following information during the movement of a self-moving device: acceleration, angular velocity, and angle change. The specific type of angle sensor can be determined according to actual needs; for example, angle sensors can include one or more of the following: inertial measurement unit (IMU), gyroscope, level, etc.

[0040] Therefore, in each detection cycle, for example, from time k-1 to time k, the satellite positioning module can acquire the positioning data of the mobile device at time k. Additionally, the angle sensor can acquire the angle change of the mobile device between time k-1 and time k.

[0041] Step S120: Calculate the observed heading angle and the predicted heading angle based on the positioning data and the angle change.

[0042] In this embodiment, the observed heading angle corresponding to each detection cycle of the self-moving device can be calculated based on the output results of the satellite positioning module and the angle sensor in each detection cycle. For example, the position coordinates of the self-moving device at time k-1 are measured by the satellite positioning module as follows: Position coordinates at time k

[0043] In some embodiments, since the satellite positioning module outputs its position coordinates in the world coordinate system, when the position of the satellite positioning module installed on the mobile device does not coincide with the motion center point of the mobile device, a coordinate system transformation is required. This involves converting the position coordinates of the satellite positioning module in the world coordinate system to the position coordinates of the motion center point of the mobile device in the world coordinate system, thus treating the position coordinates of the motion center point of the mobile device in the world coordinate system as the position coordinates of the mobile device in the world coordinate system. Here, the world coordinate system is an absolute coordinate system, which can also be understood as a global coordinate system.

[0044] Specifically, if the position coordinates of the self-moving device at time k-1 in the world coordinate system are set as Let the position coordinates at time k be set as Then we have formula (1) as follows:

[0045]

[0046] In equation (1), M is the distance between the location point of the satellite positioning module and the center point of motion of the self-moving device, and θ k The angle between the forward direction of the self-moving device at time k and the X-axis in the world coordinate system is the heading angle of the self-moving device at time k.

[0047] The position coordinates of the satellite positioning module at time k-1 are obtained from the measurement of the satellite 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 world coordinate system can be determined. and the position coordinates at time k

[0048] 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:

[0049]

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

[0051]

[0052] 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:

[0053] θ k-1 =atan2(Δy,Δx) (4).

[0054] atan2() is a function that calculates the azimuth angle.

[0055] At this point, since the angle change Δθ between time k and time k-1 is considered to be 0, the observed heading angle θ of the self-moving device at time k can be calculated. k =θ k-1 .

[0056] 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 angle sensor is Δθ. Then, the first coordinate change Δx, the second coordinate change, and the angle change Δθ satisfy formula (5). Formula (5) is:

[0057]

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

[0059]

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

[0061]

[0062] 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).

[0063] Since the angle change between time k and time k-1 measured by the angle sensor is Δθ, the observed heading angle θ of the self-moving device at time k can be calculated. k =θ k-1 +Δθ.

[0064] In this embodiment, the observed heading angle of the self-moving device at time k-1 can be calculated by using the position coordinates at time k-1 and time k measured and output by the satellite positioning module, and the angle change between time k-1 and time k measured and output by the angle sensor. Then, based on the observed heading angle at time k-1 and the angle change between time k-1 and time k, the observed heading angle of the self-moving device at time k can be calculated.

[0065] Because satellite positioning modules exhibit significant deviations in measurement results during prolonged movement of the mobile device, and angle sensors show substantial deviations during short-term movement, the calculated heading angle of the mobile device, based on the outputs of both modules, is inaccurate throughout its movement. In other words, directly using the heading angle obtained from these two sensors is insufficient for precise positioning of the mobile device. Therefore, a corrective approach can be taken by fusing the heading angles to achieve accurate positioning of the mobile device.

[0066] In some embodiments, the predicted heading angle at time k in the detection period can be calculated using formula (8), as follows:

[0067]

[0068] In formula (8), This represents the predicted heading angle at time k. This represents the fused heading angle at time k-1. Δt represents the fusion angular velocity at time k-1, and Δt represents the time interval between time k-1 and time k.

[0069] According to formula (8), the predicted heading angle at time k It is the fused heading angle at time k-1. The fusion angular velocity at time k-1 The time interval Δt between time k-1 and time k is calculated, where the time interval Δt between time k-1 and time k can be obtained directly.

[0070] In some embodiments, the fusion angular velocity at time k-1 It can be obtained through the following process:

[0071] The observed angular velocity w at time k-1 is obtained from the angle sensor of the mobile device. k-1 The fused angular velocity corresponding to the time preceding time k-1 (i.e., time k-2) As the predicted angular velocity at time k-1, i.e. Then, the confidence level of the angular velocity prediction at time k-1 is determined as follows: The reliability of the angular velocity observation at time k-1 is determined to be: Therefore, the reliability can be predicted based on the angular velocity at time k-1. Reliability of angular velocity observations at time k-1 The predicted angular velocity at time k-1 and the observed angular velocity w at time k-1 k-1 Perform fusion to obtain the fusion angular velocity at time k-1. Angular velocity fusion can then be performed using formula (9). Formula (9) is:

[0072]

[0073] The above and It can be configured according to actual needs. For example, in some examples, the above... and These can be set to 0.1 and 0.9 respectively; in other examples, the above... and These can be set to 0.2 and 0.8 respectively; in other examples, the above... and Other values ​​may also be set, and this application does not restrict them.

[0074] In some embodiments, the fused heading angle at time k-1 The confidence level δ of the observed heading angle at time k-1 can be used as a basis. k-1 The predicted heading angle at time k-1 and the observed heading angle θ at time k-1 k-1 The fused heading angle at time k-1 can be calculated using the following formula (10):

[0075]

[0076] Among them, the confidence level δ of the observed heading angle at time k-1 k-1 It can be obtained during the previous calculation of the target heading angle. That is to say, the fused heading angle at time k-1 is the target heading angle at time k-1; or, it can also be understood as the target heading angle corresponding to the previous detection cycle.

[0077] In other embodiments, the fused heading angle and fused angular velocity at time k-1 can also be obtained in other ways. Even the predicted heading angle at time k, the specific process of obtaining the predicted heading angle is not limited in the embodiments of this application.

[0078] Step S130: Based on the angle change and the positioning accuracy of the positioning data, the positioning error of the positioning data is obtained.

[0079] In this embodiment, considering that during the process of measuring and outputting the position coordinates at time k-1 using a satellite positioning module, there may be random errors in the output of the satellite positioning module, which may lead to errors in the position coordinates at time k-1 obtained by the satellite positioning module. It is the result of adding the random error at time k-1 to the actual position coordinates at time k-1.

[0080] Similarly, in the process of measuring and outputting the position coordinates at time k using a satellite positioning module, the random errors in the output of the satellite positioning module will lead to inaccurate position coordinates at time k obtained by the satellite positioning module. It is the result of adding the random error at time k to the actual position coordinates at time k.

[0081] For example, the position coordinates at time k-1 obtained using the satellite positioning module are: The random error at time k-1 is In fact, the position coordinates at time k-1 satisfy formula (11), which is:

[0082]

[0083] Similarly, the position coordinates at time k obtained using the satellite positioning module are: The random error at time k is In fact, the position coordinates at time k satisfy formula (12), which is:

[0084]

[0085] In other words, the presence of random errors leads to positioning errors in the positioning data. These random errors are related to the positioning accuracy of the positioning data; therefore, embodiments of this application can calculate the positioning error of the positioning data based on the angle change and the positioning accuracy of the positioning data.

[0086] Step S140: Determine the confidence level of the observed heading angle based on the positioning error of the positioning data.

[0087] In this embodiment, after determining the positioning error of the obtained positioning data, the confidence level of the observed heading angle can be further determined based on the positioning error. Compared with a fixed confidence level, the method provided in this embodiment can make the obtained confidence level more accurate.

[0088] Step S150: Perform fusion correction based on the observed heading angle, the predicted heading angle, and the confidence level of the observed heading angle to obtain the target heading angle of the self-moving device.

[0089] In this embodiment of the application, after obtaining the accurate confidence level, the observed heading angle and the predicted heading angle can be fused based on the confidence level to obtain the fused target heading angle. The fusion process can be performed using formula (13):

[0090]

[0091] In the formula, This represents the target heading angle at time k. θ represents the predicted heading angle at time k. k δ represents the observed heading angle at time k. k This represents the confidence level of the observed heading angle at time k.

[0092] It should be understood that the weighted fusion scheme shown in formula (13) is only an illustrative example of the embodiments of this application. In some other embodiments, other fusion algorithms (such as quaternion method, first-order complementary algorithm, Kalman filter fusion algorithm, etc.) may be used for angle fusion, and the embodiments of this application do not limit this.

[0093] In this embodiment, a more accurate confidence level can be obtained based on the positioning error of the positioning data, making the target heading angle obtained by fusion more accurate, thereby enabling precise positioning of the self-moving device.

[0094] It should be noted that the higher the confidence level of the obtained observed heading angle, the more reliable the observed heading angle is, and the more trustworthy the observed heading angle can be. Conversely, the lower the confidence level, the less reliable the observed heading angle is, and the more trustworthy the predicted heading angle should be.

[0095] In one embodiment of this application, reference is made to Figure 2 , Figure 2 This is a flowchart of the steps to obtain the positioning error of the positioning data based on the angle change and the positioning accuracy of the positioning data, provided in the embodiments of this application, including but not limited to steps S210 to S230.

[0096] Step S210: Obtain the first fusion accuracy based on the first accuracy of the positioning data obtained in the current detection cycle and the first accuracy of the positioning data obtained in the previous detection cycle;

[0097] Step S220: Obtain the second fusion accuracy based on the second accuracy of the positioning data obtained in the current detection cycle and the second accuracy of the positioning data obtained in the previous detection cycle;

[0098] Step S230: Based on the angle change, the first fusion accuracy, and the second fusion accuracy, the positioning error of the positioning data is obtained.

[0099] In this embodiment, the positioning accuracy includes a first accuracy in a first direction and a second accuracy in a second direction. Since the random error of the satellite positioning module follows a Gaussian distribution of the positioning accuracy of the satellite positioning module, i.e., n... k-1 ~N(0, σ k-1 ), n k ~N(0, σ k ), where σ k-1This represents the positioning accuracy of the positioning data at time k-1 (i.e., the previous detection cycle), which includes the x-coordinate positioning accuracy at time k-1. (i.e., the first precision) and y-coordinate positioning precision (That is, the second precision). σ k This represents the positioning accuracy of the positioning data at time k (i.e., the current detection period), which includes the x-coordinate positioning accuracy at time k. (i.e., the first precision) and y-coordinate positioning precision (That is, the second precision).

[0100] In this embodiment of the application, due to random error n k-1 ~N(0, σ k-1 ), n k ~N(0, σ k Then, according to Gauss's rules of addition and subtraction, we can obtain... Therefore, formula (14) can be obtained as follows:

[0101]

[0102] In equation (14), This represents the x-coordinate fusion positioning accuracy (i.e., the first fusion accuracy) at time k (i.e., the current detection period). This represents the x-coordinate positioning accuracy (i.e., the first accuracy) at time k (i.e., the current detection cycle). This represents the x-coordinate positioning accuracy (i.e., the first accuracy) at time k-1 (i.e., the previous detection cycle). This represents the y-coordinate fusion positioning accuracy (i.e., the second fusion accuracy) at time k (i.e., the current detection period). This represents the y-coordinate positioning accuracy (i.e., the second accuracy) at time k (i.e., the current detection cycle). This represents the y-coordinate positioning accuracy (i.e., the second accuracy) at time k-1 (i.e., the previous detection cycle). In other words, the positioning accuracy of the current detection cycle includes the x-coordinate positioning accuracy of the current detection cycle. (i.e., the first precision) and y-coordinate positioning precision (This is the second precision). Specifically, the x-coordinate fusion positioning precision for the current detection cycle... The positioning accuracy can be determined based on the x-coordinate of the previous detection cycle. and the x-coordinate positioning accuracy of the current detection cycle The calculated y-coordinate fusion positioning accuracy for the current detection cycle. The positioning accuracy can be determined based on the y-coordinate of the previous detection cycle. And the y-coordinate positioning accuracy of the current detection cycle Calculated.

[0103] In this embodiment of the application, the formula for calculating the observed heading angle θ at time k-1 of the self-moving device is used. k-1 =atan2(sinΔθ*Δy+(cosΔθ-1)*Δx,sinΔθ*Δx+(1-cosΔθ)*Δy) combined with formula (14), we can obtain the molecular expectation as:

[0104] μ1=sinΔθ*Δy+Δx*(cosΔθ-1) (15).

[0105] Therefore, the numerator variance can be obtained as:

[0106]

[0107] At the same time, the expected value of the denominator can be obtained as:

[0108] μ2=-sinΔθ*Δx-Δy*(cosΔθ-1) (17).

[0109] Therefore, the variance of the denominator can be:

[0110]

[0111] Dividing the two Gaussian distributed data sets transforms the system's error model into a Cauchy distribution, where the probability density function of the Cauchy distribution is:

[0112]

[0113] For the Cauchy distribution, there is no mean or variance, and the probability of the Cauchy distribution is at its highest point when x = x0. Where γ is the scale parameter in the Cauchy distribution, and Therefore, the positioning error of the current detection cycle can be represented by the scale parameter γ, and the following formula (20) can be obtained:

[0114]

[0115] In equation (20), δ k The positional error at time k is represented by Δθ, where Δθ represents the angle change between time k-1 and time k. This represents the x-coordinate fusion positioning accuracy at time k (i.e., the first fusion accuracy). This represents the y-coordinate fusion positioning accuracy at time k (i.e., the second fusion accuracy).

[0116] In one embodiment of this application, reference is made to Figure 3 , Figure 3This is a flowchart of the steps performed before determining the confidence level of the observed heading angle based on the positioning error of the positioning data, as provided in the embodiments of this application, including but not limited to steps S310 to S340.

[0117] Step S310: Obtain the angular velocity of the self-moving device through the angle sensor;

[0118] Step S320: Obtain the linear velocity of the self-moving device using an odometer;

[0119] Step S330: Determine the trajectory curvature of the self-moving device based on the angular velocity and linear velocity, and obtain the trajectory error based on the trajectory curvature;

[0120] Step S340: Determine the first displacement distance based on the positioning data, determine the second displacement distance based on the odometer reading, and determine the displacement error based on the first displacement distance and the second displacement distance.

[0121] In this embodiment, the travel trajectory of the self-moving device is also considered during the process of obtaining the observed heading angle. When the travel trajectory of the self-moving device is not a straight line, an arc model is needed to calculate the observed heading angle.

[0122] In the calculation process, it is usually assumed that the self-moving device travels along a specified radius from time k-1 to time k, meaning the trajectory curvature R of the self-moving device is a fixed value. However, the self-moving device may not necessarily travel along the specified radius; the trajectory curvature R may fluctuate, leading to an error between the observed trajectory and the actual trajectory. Therefore, to ensure a more accurate confidence level for the final calculated observed heading angle, the influence of trajectory error can be considered. This trajectory error is related to the trajectory curvature R of the self-moving device; the greater the fluctuation in the trajectory curvature R, the greater the perceived difference between the observed and actual trajectories.

[0123] In this embodiment of the application, the angular velocity w of the self-moving device can be obtained through an angle sensor. m The linear velocity v of a mobile device can be obtained through an odometer. m Therefore, the trajectory curvature R of the self-moving device can be determined based on the angular velocity and linear velocity. m ,in, After obtaining the trajectory curvature R m Then, further calculations can be made based on the trajectory curvature R. m The trajectory error is obtained.

[0124] In this embodiment, when the self-moving device slips or is manually dragged, the arc trajectory calculated by the odometer deviates significantly from the actual movement. Under these conditions, the observed angle value calculated using the above method will be distorted, and there may be discrepancies between the first displacement distance calculated from the positioning data and the second displacement distance calculated by the odometer. Therefore, there is a certain correlation between the displacement error of the first and second displacement distances and the confidence level of the observed angle value; the impact of displacement error on the confidence level of the observed heading angle can be considered.

[0125] According to the embodiments of this application, the first displacement distance can be determined based on the positioning data, and the second displacement distance of the self-moving device in the same detection cycle can be directly determined by the odometer, so that the displacement error can be determined based on the first displacement distance and the second displacement distance.

[0126] Specifically, the displacement error can be calculated using formula (21) in this embodiment. Formula (21) is:

[0127]

[0128] In equation (21), This represents the displacement error at time k. This represents the first displacement distance at time k. This represents the second displacement distance at time k.

[0129] In this embodiment, after obtaining the positioning error, trajectory error, and displacement error respectively, the confidence level of the observed heading angle can be determined based on these errors. That is, the determined confidence level of the observed heading angle comprehensively considers the influence of the positioning error, trajectory error, and displacement error on the confidence level, thereby improving the accuracy of the obtained confidence level.

[0130] In one embodiment of this application, the sampling period of the angle sensor and the sampling period of the odometer are both shorter than the detection period, and the number of trajectory curvatures is greater than 1. The trajectory error is obtained based on the trajectory curvature, including:

[0131] The variance of several trajectory curvatures is calculated to obtain the trajectory error.

[0132] In this embodiment, the sampling period of the angle sensor and the sampling period of the odometer are both shorter than the detection period. For example, assuming the detection period is the interval between time k-1 and time k, the detection period is the same as the sampling period of the satellite positioning module, and the sampling periods of the angle sensor and the odometer are both 1 / 20 of the detection period. In this scenario, within one detection period, the satellite positioning module can acquire one frame of RTK data, while the odometer can acquire 20 frames of odometer data, and the angle sensor can correspondingly acquire 20 frames of angle data. Therefore, 20 linear velocities can be determined from the 20 frames of odometer data, 20 angular velocities can be determined from the 20 frames of angle data, and 20 trajectory curvatures can be determined from the 20 linear velocities and 20 angular velocities.

[0133] Therefore, during this detection cycle, several linear velocities v can be obtained from the odometer output. m and several angular velocities w output by the angle sensor m ,pass Several trajectory curvatures are calculated, and the variance of these trajectories can be calculated to obtain the trajectory error.

[0134] Specifically, the trajectory error at the current moment can be calculated using formula (22) based on the curvature of several sub-trajectories. Formula (22) is as follows:

[0135]

[0136] In equation (22), R represents the trajectory error at time k, N represents the total number of sub-trajectory curvatures, and R represents the total number of sub-trajectory curvatures. m Let E{R} represent the curvature of the m-th sub-trajectory in the trajectory from time k-1 to time k. m} represents the expectation of the curvature of the sub-trajectory, where, v m w represents the linear velocity corresponding to the curvature of the m-th sub-trajectory. m This represents the angular velocity corresponding to the curvature of the m-th sub-trajectory.

[0137] For example, assume the interval between time k-1 and time k is 400ms, i.e., the detection period is 400ms. During this period, assume the sampling period of both the odometer and the angle sensor is 10ms, then the linear velocity v output by the odometer... m and the angular velocity w output by the angle sensor m There are 40, thus the curvature R of each of the 40 sub-trajectories can be calculated separately. m Then, the curvature R of these 40 sub-trajectories can be solved. m The variance (i.e., trajectory error) is used to evaluate the reliability of the trajectory curvature R of the self-moving device throughout the entire detection cycle.

[0138] In one embodiment of this application, determining the first displacement distance based on positioning data includes:

[0139] The first displacement distance is determined based on the positioning data obtained in the current detection cycle and the positioning data obtained in the previous detection cycle.

[0140] In this embodiment, the first displacement distance can be calculated based on the positioning data obtained in the current detection cycle and the positioning data obtained in the previous detection cycle. For example, the position coordinates of the self-moving device at time k-1, measured by the satellite positioning module, are obtained. and the position coordinates at time k Then, the first coordinate change Δx and the second coordinate change Δy of the self-moving device at time k can be calculated according to formula (2). Therefore, based on... The first displacement distance is calculated.

[0141] In one embodiment of this application, when the self-moving device is traveling in a straight line, the first displacement distance can also be calculated by the observed heading angle of the previous detection cycle and the linear velocity distance of the wheels of the self-moving device in the current detection cycle.

[0142] Specifically, in the odometer coordinate system, when the self-moving device is traveling in a straight line, the corresponding straight-line model formula (23) is:

[0143]

[0144] In formula (23), ΔS k The linear velocity distance of the self-moving device's wheels at time k (i.e., the current detection cycle) can be calculated from the linear velocity of the self-moving device's wheels at time k-1 (i.e., the previous detection cycle) and the time interval between time k-1 and time k. This can be expressed as ΔS. k =v k-1 *Δt is calculated. θ k-1 The observed heading angle at time k-1 (i.e., the previous detection cycle).

[0145] In one embodiment of this application, when the self-moving device is not traveling in a straight line, the first displacement distance can be calculated by the observed heading angle of the previous detection cycle, the linear velocity distance of the self-moving device's wheels in the current detection cycle, and the angular change between the previous and current detection cycles.

[0146] Specifically, in the odometer coordinate system, when the self-moving device is not traveling in a straight line, the corresponding arc model formula (24) is:

[0147]

[0148] When the X-axis and Y-axis in the odometer coordinate system and the world coordinate system are interchanged, the formula (24) corresponding to the arc model can be corrected to obtain the formula (25) corresponding to the arc model after correction:

[0149]

[0150] In formula (25), ΔS k Let be the linear velocity of the wheel of the self-moving device at time k. This can be calculated from the linear velocity of the wheel at time k-1 and the time interval between time k-1 and time k, which can be expressed as ΔS. k =v k-1 The heading angle of the self-moving device at time k-1 can be calculated using the positioning data at time k, the positioning data at time k-1, and the angle change between time k-1 and time k. The angle change Δθ can be calculated using the angular velocity at time k-1 measured by the angle sensor and the time interval between time k-1 and time k, i.e., Δθ = w k-1 *Δt is calculated.

[0151] When the X-axis and Y-axis of the odometer coordinate system are the same as those of the world coordinate system, the odometer coordinate system can be regarded as the world coordinate system.

[0152] In this embodiment, the first coordinate change Δx of the self-moving device at time k can be calculated using formula (25). k Second coordinate change Δy k Therefore, it is possible to base on The first displacement distance is calculated.

[0153] In one embodiment of this application, the odometer includes an optical encoder; determining the second displacement distance based on the odometer includes:

[0154] The second displacement distance is determined based on the number of pulses collected by the photoelectric encoder in the current detection cycle.

[0155] In this embodiment, the odometer includes an optical encoder, allowing the second displacement distance to be directly calculated based on the number of pulses collected by the optical encoder during the current detection cycle. The second displacement distance obtained through the optical encoder is unaffected by the travel angle of the self-moving device. Under normal driving conditions, the distance of the self-moving device's actual trajectory can be obtained, which is the second displacement distance ΔO. xyk .

[0156] In one embodiment of this application, reference is made to Figure 4 , Figure 4This is a flowchart of the steps for determining the confidence level of the observed heading angle based on the positioning error, trajectory error and displacement error provided in the embodiments of this application, including but not limited to steps S410 to S420.

[0157] Step S410: Obtain the weights corresponding to the positioning error, trajectory error, and displacement error, respectively;

[0158] Step S420: Determine the confidence level of the observed heading angle based on the positioning error, trajectory error, and displacement error and their corresponding weights.

[0159] In this embodiment of the application, after obtaining the positioning error, trajectory error and displacement error, the first weight corresponding to the positioning error, the second weight corresponding to the trajectory error and the third weight corresponding to the displacement error can be obtained respectively. Then, based on the positioning error, trajectory error, displacement error and their corresponding first weight, second weight and third weight, the confidence level of the observed heading angle is calculated.

[0160] Specifically, the confidence level of the observed heading angle can be calculated using formula (26):

[0161]

[0162] In formula (26), δ Rθ δ represents the confidence level of the observed heading angle at time k (i.e., the current detection period). k This represents the positioning error at time k (i.e., the current detection period). This represents the trajectory error at time k (i.e., the current detection period). Let k represent the displacement error at time k (i.e., the current detection period), where k1 represents the first weight, k2 represents the second weight, and k3 represents the third weight.

[0163] It should be noted that the first, second, and third weights can be adjusted according to the magnitude of the corresponding positioning error, trajectory error, and displacement error values.

[0164] The embodiments of this application calculate the confidence level of the observed heading angle by taking into account the positioning error, trajectory error, displacement error and their corresponding weights, so that the calculated confidence level is more accurate, thereby ensuring the fusion result and achieving precise positioning of the self-moving device.

[0165] Please see Figure 5 This application embodiment also provides a heading angle calculation device 500 for a self-moving device, which can implement the above-described heading angle calculation method for a self-moving device. The device includes:

[0166] The first acquisition module 501 is used to acquire the positioning data of the mobile device through the satellite positioning module and the angle change of the mobile device through the angle sensor in each detection cycle.

[0167] The first calculation module 502 is used to calculate the observed heading angle and the predicted heading angle based on the positioning data and the angle change.

[0168] The second acquisition module 503 is used to obtain the positioning error of the positioning data based on the angle change and the positioning accuracy of the positioning data;

[0169] The determination module 504 is used to determine the confidence level of the observed heading angle based on the positioning error of the positioning data;

[0170] The fusion module 505 is used to perform fusion correction based on the observed heading angle, the predicted heading angle, and the confidence level of the observed heading angle to obtain the target heading angle of the self-moving device.

[0171] The specific implementation of the heading angle calculation device of this self-moving device is basically the same as the specific embodiment of the heading angle calculation method of the self-moving device described above, and will not be repeated here.

[0172] Please see Figure 6 , Figure 6 This is a schematic diagram of the structure of an electronic device provided in an embodiment of this application. The electronic device includes:

[0173] The processor 601 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.

[0174] The memory 602 can be implemented in the form of ROM (Read Only Memory), static storage device, dynamic storage device, or RAM (Random Access Memory). The memory 602 can store the operating system and other applications. 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 602 and called by the processor 601 to execute the heading angle calculation method of the self-moving device according to the embodiments of this application.

[0175] The input / output interface 603 is used to implement information input and output;

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

[0177] Bus 606 transmits information between various components of the device (e.g., processor 601, memory 602, input / output interface 603, and communication interface 604);

[0178] The processor 601, memory 602, input / output interface 603, and communication interface 604 are connected to each other within the device via bus 605.

[0179] 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 method for calculating the heading angle of the self-moving device.

[0180] 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.

[0181] 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.

[0182] Those skilled in the art will understand that the technical solutions shown in the figures do not constitute a limitation on the embodiments of this application, and may include more or fewer steps than shown, or combine certain steps, or different steps.

[0183] The device embodiments described above are merely illustrative. The units described as separate components may or may not be physically separate; that is, they may be located in one place or distributed across multiple network units. Some or all of the modules can be selected to achieve the purpose of this embodiment according to actual needs.

[0184] Those skilled in the art will understand that all or some of the steps in the methods disclosed above, as well as the functional modules / units in the systems and devices, can be implemented as software, firmware, hardware, or suitable combinations thereof.

[0185] The terms “first,” “second,” “third,” “fourth,” etc. (if present) in the specification and accompanying drawings of this application are used to distinguish similar objects and are not necessarily used to describe a specific order or sequence. It should be understood that such data can be interchanged where appropriate so that the embodiments of this application described herein can be implemented in orders other than those illustrated or described herein. Furthermore, the terms “comprising” and “having,” and any variations thereof, are intended to cover non-exclusive inclusion; for example, a process, method, system, product, or apparatus that comprises a series of steps or units is not necessarily limited to those steps or units explicitly listed, but may include other steps or units not explicitly listed or inherent to such processes, methods, products, or apparatus.

[0186] It should be understood that in this application, "at least one (item)" means one or more, and "more than" means two or more. "And / or" is used to describe the relationship between related objects, indicating that three relationships can exist. For example, "A and / or B" can represent three cases: only A exists, only B exists, and both A and B exist simultaneously, where A and B can be singular or plural. The character " / " generally indicates that the preceding and following related objects are in an "or" relationship. "At least one (item) of the following" or similar expressions refer to any combination of these items, including any combination of single or plural items. For example, at least one (item) of a, b, or c can represent: a, b, c, "a and b", "a and c", "b and c", or "a and b and c", where a, b, and c can be single or multiple.

[0187] In the several embodiments provided in this application, it should be understood that the disclosed apparatus and methods can be implemented in other ways. For example, the apparatus embodiments described above are merely illustrative; for instance, the division of the units described above is only a logical functional division, and in actual implementation, there may be other division methods. For example, multiple units or components may be combined or integrated into another system, or some features may be ignored or not executed. Furthermore, the coupling or direct coupling or communication connection shown or discussed may be through some interfaces; the indirect coupling or communication connection between apparatuses or units may be electrical, mechanical, or other forms.

[0188] The units described above as separate components may or may not be physically separate. The components shown as units may or may not be physical units; that is, they may be located in one place or distributed across multiple network units. Some or all of the units can be selected to achieve the purpose of this embodiment according to actual needs.

[0189] Furthermore, the functional units in the various embodiments of this application can be integrated into one processing unit, or each unit can exist physically separately, or two or more units can be integrated into one unit. The integrated unit can be implemented in hardware or as a software functional unit.

[0190] If the integrated unit is implemented as a software functional unit and sold or used as an independent product, it can be stored in a computer-readable storage medium. Based on this understanding, the technical solution of this application, in essence, or the part that contributes to the prior art, or all or part of the technical solution, can be embodied in the form of a software product. This computer software product is stored in a storage medium and includes multiple instructions to cause a computer device (which may be a personal computer, server, or network device, etc.) to execute all or part of the steps of the methods of the various embodiments of this application. The aforementioned storage medium includes various media capable of storing programs, such as USB flash drives, portable hard drives, read-only memory (ROM), random access memory (RAM), magnetic disks, or optical disks.

[0191] The preferred embodiments of the present application have been described above with reference to the accompanying drawings, but this does not limit the scope of the claims of the present application. Any modifications, equivalent substitutions, and improvements made by those skilled in the art without departing from the scope and substance of the embodiments of the present application shall be within the scope of the claims of the present application.

Claims

1. A method for calculating a heading angle of a self-moving device, characterized by, The method comprises: In each detection period, acquiring positioning data of the self-moving device through a satellite positioning module, and acquiring an angle change amount of the self-moving device through an angle sensor; calculating an observed heading angle and a predicted heading angle according to the positioning data and the angle change amount; obtaining a positioning error of the positioning data according to the angle change amount and a positioning accuracy of the positioning data; determining a confidence degree of the observed heading angle according to the positioning error of the positioning data; fusing and correcting the observed heading angle, the predicted heading angle and the confidence degree of the observed heading angle to obtain a target heading angle of the self-moving device; wherein a formula for calculating the predicted heading angle at time k in the detection period is as follows: ， a predicted heading angle at time k, a fused heading angle at time k-1, a fused angular velocity at time k-1, a time interval between time k-1 and time k.

2. The method of claim 1, wherein, The positioning accuracy comprises a first accuracy in a first direction and a second accuracy in a second direction; The positioning error of the positioning data is obtained according to the angle change amount and the positioning accuracy of the positioning data, comprising: obtaining a first fusion accuracy according to the first accuracy of the positioning data acquired in the current detection period and the first accuracy of the positioning data acquired in the last detection period; obtaining a second fusion accuracy according to the second accuracy of the positioning data acquired in the current detection period and the second accuracy of the positioning data acquired in the last detection period; obtaining the positioning error of the positioning data according to the angle change amount, the first fusion accuracy and the second fusion accuracy.

3. The method of claim 1, wherein, Before the confidence degree of the observed heading angle is determined according to the positioning error of the positioning data, the method further comprises: acquiring an angular velocity of the self-moving device through an angle sensor; acquiring a linear velocity of the self-moving device through an odometer; determining a trajectory curvature of the self-moving device according to the angular velocity and the linear velocity, and obtaining a trajectory error according to the trajectory curvature; determining a first displacement distance according to the positioning data, determining a second displacement distance according to the odometer, and determining a displacement error according to the first displacement distance and the second displacement distance; correspondingly, the confidence degree of the observed heading angle is determined according to the positioning error, the trajectory error and the displacement error. The sampling period of the angle sensor and the sampling period of the odometer are both less than the detection period, and the number of the trajectory curvatures is greater than 1; 4. The method of claim 3, wherein, The trajectory error is obtained according to the trajectory curvatures, comprising: performing variance calculation on a plurality of the trajectory curvatures to obtain the trajectory error. The first displacement distance is determined according to the positioning data, comprising:

5. The method of claim 3, wherein, determining the first displacement distance according to the positioning data acquired in the current detection period and the positioning data acquired in the last detection period. The odometer comprises an optical encoder; the second displacement distance is determined according to the odometer, comprising:

6. The method of claim 3, wherein, determining the second displacement distance according to the number of pulses collected by the optical encoder in the current detection period. The confidence degree of the observed heading angle is determined according to the positioning error, the trajectory error and the displacement error, comprising:

7. The method of claim 3, wherein, ​ acquire weights corresponding to the positioning error, the trajectory error and the displacement error respectively; determine the confidence of the observed heading angle according to the positioning error, the trajectory error and the displacement error and the weights corresponding thereto.

8. A heading angle calculation device of a self-moving device, characterized by comprising: The device comprises: a first acquisition module, configured to acquire positioning data of the self-moving device by a satellite positioning module and to acquire an angle change amount of the self-moving device by an angle sensor in each detection period; a first calculation module, configured to calculate an observed heading angle and a predicted heading angle according to the positioning data and the angle change amount; wherein a formula for calculating the predicted heading angle at time k in the detection period is as follows: ， This represents the predicted heading angle at time k. This represents the fused heading angle at time k-1. Represents the fused angular velocity at time k-1. This represents the time interval between time k-1 and time k; a second acquisition module, configured to acquire a positioning error of the positioning data according to the angle change amount and a positioning accuracy of the positioning data; a determination module, configured to determine the confidence of the observed heading angle according to the positioning error of the positioning data; a fusion module, configured to perform fusion correction according to the observed heading angle, the predicted heading angle and the confidence of the observed heading angle to obtain a target heading angle of the self-moving device.

9. An electronic device, comprising: The electronic device comprises a memory and at least one processor, the memory stores a computer program, and the at least one processor implements the method in any one of claims 1 to 7 when executing the computer program.

10. A computer readable storage medium, the storage medium having stored thereon a computer program, characterized in that, The computer program is executed by the processor to implement the method in any one of claims 1 to 7.

Images