Angular velocity correction method and device and robot

Abstract

The embodiment of the invention provides an angular velocity correction method and device and a robot, and relates to the technical field of motion correction, and the method comprises the steps: determining an angular velocity zero offset according to motion data measured by an inertial measurement unit, and taking the angular velocity zero offset as a zero offset observation value; determining the weight of the zero offset observation value and the weight of the zero offset prediction value based on the discrete degree information of the historical zero offset observation value and the uncertainty information of the zero offset prediction value; updating the zero offset predicted value through the zero offset observed value, the zero offset predicted value, the weight of the zero offset observed value and the weight of the zero offset predicted value; and correcting the angular velocity included in the motion data by adopting the updated zero offset predicted value. The angular velocity zero offset adopted for correcting the angular velocity is determined based on the motion data measured by the inertial measurement unit instead of a pre-calibrated fixed angular velocity zero offset, so that the accuracy of the corrected angular velocity can be improved.

Description

TECHNICAL FIELD

[0001] The present application relates to the technical field of motion correction, in particular to a method and device for correcting angular velocity and a robot. BACKGROUND

[0002] An inertial measurement unit (IMU) includes a gyroscope, an accelerometer, and a magnetometer, and can be used to measure angular velocity, linear acceleration, and horizontal magnetic force direction, and is widely used in the fields of flight control and robot navigation. However, the output angular velocity of the IMU has a significant zero offset error due to temperature drift, environmental disturbance, and hardware process. The zero offset error is the non-zero angular velocity value measured by the gyroscope when the IMU is in a balanced state.

[0003] In the related art, the IMU is kept in a balanced state before use to calibrate the angular velocity zero offset. When the IMU is used subsequently, the measured angular velocity is corrected based on the pre-calibrated angular velocity zero offset. However, the angular velocity zero offset exhibits a non-stationary and slowly changing characteristic during long-term operation of the IMU. Therefore, the accuracy of correcting the angular velocity using the pre-calibrated angular velocity zero offset is poor. SUMMARY

[0004] Embodiments of the present application aim to provide a method and device for correcting angular velocity and a robot to improve the accuracy of the corrected angular velocity. The specific technical solutions are as follows:

[0005] In a first aspect, the embodiments of the present application provide a method for correcting angular velocity, the method comprising:

[0006] determining an angular velocity zero offset from motion data measured by an inertial measurement unit as a zero offset observation value;

[0007] determining a weight of the zero offset observation value and a weight of a zero offset prediction value based on discrete degree information of historical zero offset observation values and uncertainty information of the zero offset prediction value, wherein the historical zero offset observation values include zero offset observation values at each time within a first preset time period, and the uncertainty information represents a difference between the zero offset prediction value and a true value of the angular velocity zero offset;

[0008] updating the zero offset prediction value by using the zero offset observation value, the zero offset prediction value, the weight of the zero offset observation value, and the weight of the zero offset prediction value;

[0009] correcting the angular velocity included in the motion data by using the updated zero offset prediction value.

[0010] In a second aspect, the embodiments of the present application provide a device for correcting angular velocity, the device comprising:

[0011] an observation value determination module configured to determine an angular velocity bias as a bias observation value according to motion data measured by the inertial measurement unit;

[0012] a weight determination module configured to determine a weight of the bias observation value and a weight of the bias prediction value based on dispersion information of historical bias observation values and uncertainty information of the bias prediction value, wherein the historical bias observation values include bias observation values at each time within a first preset time period, and the uncertainty information represents a difference between the bias prediction value and a true value of the angular velocity bias;

[0013] an angular velocity bias updating module configured to update the bias prediction value by using the bias observation value, the bias prediction value, the weight of the bias observation value and the weight of the bias prediction value;

[0014] an angular velocity correction module configured to correct the angular velocity included in the motion data by using the updated bias prediction value.

[0015] In a third aspect, an embodiment of the present application provides a robot, comprising a processor, a communication interface, a memory and a communication bus, wherein the processor, the communication interface and the memory complete mutual communication through the communication bus;

[0016] the memory is configured to store a computer program;

[0017] the processor is configured to execute the program stored on the memory, and implement the method steps of any one of the first aspect.

[0018] In a fourth aspect, an embodiment of the present application provides a computer readable storage medium, wherein the computer readable storage medium stores a computer program, and the computer program is executed by a processor to implement the method steps of any one of the first aspect.

[0019] Embodiments of the present application have the following beneficial effects:

[0020] In the solution provided in this application, since neither the zero-bias observation value nor the zero-bias prediction value is completely accurate, the dispersion information corresponding to the zero-bias observation value can reflect the accuracy of the zero-bias observation value, and the uncertainty information of the zero-bias prediction value can reflect the accuracy of the zero-bias prediction value. Therefore, by assigning corresponding weights based on the accuracy of the zero-bias observation value and the zero-bias prediction value, and updating the zero-bias prediction value according to the determined weights, the updated zero-bias prediction value can be made more accurate. Thus, by determining the angular velocity zero bias based on the motion data measured by the inertial measurement unit, and using the angular velocity zero bias to correct the angular velocity, instead of using a pre-calibrated fixed angular velocity zero bias to correct the angular velocity, the accuracy of the corrected angular velocity can be improved. Of course, implementing any product or method of this application does not necessarily require achieving all of the above advantages simultaneously. Attached Figure Description

[0021] To more clearly illustrate the technical solutions in the embodiments of this application or the prior art, the drawings used in the description of the embodiments or the prior art will be briefly introduced below. Obviously, the drawings described below are only some embodiments of this application. For those skilled in the art, other embodiments can be obtained based on these drawings.

[0022] Figure 1 A flowchart illustrating an angular velocity correction method provided in an embodiment of this application;

[0023] Figure 2 for Figure 1 A specific flowchart of step S102 in the illustrated embodiment;

[0024] Figure 3 Based on Figure 1 A flowchart illustrating a method for determining the amount of change in the illustrated embodiment;

[0025] Figure 4 Based on Figure 1 A flowchart illustrating a method for determining uncertainty information of zero-biased prediction values ​​in the illustrated embodiment;

[0026] Figure 5 for Figure 1 A specific flowchart of step S101 in the illustrated embodiment;

[0027] Figure 6 for Figure 5 A specific flowchart of step S501 in the illustrated embodiment;

[0028] Figure 7 This is a schematic diagram of the structure of an angular velocity correction device provided in an embodiment of this application;

[0029] Figure 8A structural schematic diagram of a robot provided in an embodiment of the present application. DETAILED DESCRIPTION

[0030] The technical solutions in the embodiments of the present application will be clearly and completely described below with reference to the drawings in the embodiments of the present application. Obviously, the described embodiments are only part of the embodiments of the present application, rather than all the embodiments of the present application. Based on the embodiments in the present application, all other embodiments obtained by those skilled in the art based on the present application belong to the scope of protection of the present application.

[0031] In order to improve the correction effect of the angular velocity, the present application provides an angular velocity correction method, device, robot, computer readable storage medium and computer program product. First, an angular velocity correction method provided in an embodiment of the present application will be introduced.

[0032] The angular velocity correction method provided in the present application can be applied to any electronic device that needs to correct the angular velocity, for example, the control device of the robot, the processor of the unmanned aerial vehicle, etc., which is not limited here. In order to describe clearly, the electronic device is called hereinafter.

[0033] As shown in the angular velocity correction method, S101-S104 are included. Figure 1

[0034] S101, determining an angular velocity zero offset according to motion data measured by an inertial measurement unit as a zero offset observation value;

[0035] S102, determining a weight of the zero offset observation value and a weight of the zero offset prediction value based on dispersion degree information of historical zero offset observation values and uncertainty information of the zero offset prediction value.

[0036] The historical zero offset observation values include zero offset observation values at each time in a first preset time period, and the uncertainty information represents a difference between the zero offset prediction value and a true value of the angular velocity zero offset.

[0037] S103, updating the zero offset prediction value by the zero offset observation value, the zero offset prediction value, the weight of the zero offset observation value and the weight of the zero offset prediction value.

[0038] S104, correcting the angular velocity included in the motion data by using the updated zero offset prediction value.

[0039] ​It can be seen that, in the embodiment of the present application, since neither the zero offset observation value nor the zero offset prediction value is completely accurate, the dispersion degree information corresponding to the zero offset observation value can reflect the accuracy of the zero offset observation value, and the uncertainty information of the zero offset prediction value can reflect the accuracy of the zero offset prediction value. Therefore, based on the accuracy of the zero offset observation value and the zero offset prediction value, corresponding weights are allocated, and the zero offset prediction value is updated according to the determined weights, which can make the updated zero offset prediction value more accurate. In this way, the angular velocity zero offset is determined according to the motion data measured by the inertial measurement unit, and the angular velocity zero offset is used to correct the angular velocity, instead of using the fixed and unchanged angular velocity zero offset obtained by pre-calibration to correct the angular velocity, which can improve the accuracy of the corrected angular velocity.

[0040] For a movable device, such as a robot, a drone, and a self-driving vehicle, which needs to determine its own angular velocity, an inertial measurement unit is usually carried. During the operation of the movable device, in order to determine the angular velocity zero offset of the inertial measurement unit, the electronic device can obtain motion data measured by the inertial measurement unit. The inertial measurement unit can include multiple sensors, such as a gyroscope, an accelerometer, and a magnetometer. Correspondingly, the motion data can include angular velocity measured by the gyroscope, linear acceleration direction measured by the accelerometer, and horizontal magnetic force direction measured by the magnetometer.

[0041] In step S101, the electronic device can determine the angular velocity zero offset according to the motion data measured by the inertial measurement unit, as a zero offset observation value. Specifically, the electronic device can take the gravity direction as a vertical reference direction and take the geomagnetic direction as a horizontal reference direction, determine the attitude offset of the inertial measurement unit based on the difference between the linear acceleration direction and the gravity direction and the difference between the horizontal magnetic force direction and the geomagnetic direction, and then determine the angular velocity zero offset according to the angle offset represented by the determined attitude offset and the sampling period of the inertial measurement unit.

[0042] In an embodiment, the electronic device can determine the angular velocity zero offset according to the motion data measured by the inertial measurement unit at the current time, as a zero offset observation value at the current time. Since the angular velocity zero offset is usually continuously changing during the use of the inertial measurement unit, the value obtained by updating the angular velocity zero offset at the previous time can be taken as the zero offset prediction value at the current time. The zero offset prediction value at the initial time is the angular velocity zero offset determined according to the motion data measured by the inertial measurement unit in the static state.

[0043] Since neither the zero bias prediction value nor the zero bias observation value is absolutely reliable, the dispersion degree information of the historical zero bias observation value can reflect the reliability of the zero bias observation value, and the uncertainty information of the zero bias prediction value can reflect the reliability of the zero bias prediction value. Therefore, when determining the angular velocity zero bias, the zero bias observation value and the zero bias prediction value can be assigned corresponding weights for comprehensive consideration.

[0044] To determine the reliability of the zero bias prediction value and the zero bias observation value, the electronic device can determine the weight of the zero bias observation value based on the dispersion degree information of the historical zero bias observation value, and determine the weight of the zero bias prediction value based on the uncertainty information of the zero bias prediction value, that is, step S102 is performed. The historical zero bias observation value can include zero bias observation values at each time within a first preset time period. The first preset time period can be a preset time period from the current time, and the length of the first preset time period can be set according to actual needs, such as 1 second, 2 seconds, 3 seconds, etc. The uncertainty information of the zero bias prediction value represents the difference between the zero bias prediction value and the true value of the angular velocity zero bias.

[0045] If the dispersion degree information of the historical zero bias observation value reflects that the dispersion degree of the historical zero bias observation value is relatively high, it means that the reliability of the historical zero bias observation value is relatively low, and the weight of the zero bias observation value will be relatively small, that is, the dispersion degree information of the historical zero bias observation value is negatively correlated with the weight of the zero bias observation value. If the uncertainty information of the zero bias prediction value reflects that the difference between the zero bias prediction value and the true value of the angular velocity zero bias is large, it means that the reliability of the zero bias prediction value is relatively low, and the weight of the zero bias prediction value will be relatively small, that is, the uncertainty information of the zero bias prediction value is negatively correlated with the weight of the zero bias prediction value.

[0046] In an embodiment, the electronic device can determine the weight of the zero bias observation value at the current time based on the dispersion degree information of the historical zero bias observation value, and determine the weight of the zero bias prediction value at the current time based on the uncertainty information of the zero bias prediction value at the current time. For each time, the sum of the weight of the zero bias prediction value and the weight of the zero bias observation value is 1.

[0047] Next, the electronic device can update the zero bias prediction value based on the zero bias observation value and its weight, and the zero bias prediction value and its weight, that is, step S103 is performed. Specifically, the observation residual can be determined based on the difference between the zero bias observation value and the zero bias prediction value at the current time. Since the weights of the zero bias observation value and the zero bias prediction value can reflect the respective reliabilities, the electronic device can update the zero bias prediction value at the current time based on the observation residual at the current time and the weights of the zero bias observation value and the zero bias prediction value, to obtain the updated angular velocity zero bias at the current time.

[0048] In one implementation, the electronic device can determine the updated angular velocity zero bias at the current moment according to this formula. : .in, Based on the previous time ( The angular velocity at time (0) is zero bias relative to the current time (0). The value obtained by predicting the angular velocity zero bias at time (i.e., the zero bias prediction value of the angular velocity zero bias at the current time). To observe the residuals, This represents the zero-bias observation value of the angular velocity at the current moment. This indicates the weight of the zero-biased observation. In the embodiments of this application, the subscript is... The physical quantity is used to characterize the physical quantity corresponding to the current moment, and is indicated by the subscript. The physical quantity is used to characterize the physical quantity corresponding to the previous moment.

[0049] A sliding window is a strategy for extracting zero-biased observations within a recent fixed time range and performing mean or variance calculations. In one implementation, a sliding window can be constructed to improve the stationarity and robustness of the zero-biased observations. Assuming the length of the sliding window is N, the corresponding calculations are performed on the zero-biased observations over the most recent N time intervals. After constructing the sliding window, when determining the zero-biased observation of the angular velocity at the current time, the mean of the zero-biased observations within the sliding window can be used as the zero-biased observation of the angular velocity at the current time. .in, for The zero-bias observation of the angular velocity at time zero bias.

[0050] After determining the updated angular velocity zero bias, the electronic device can use the updated zero bias prediction value to correct the angular velocity measured by the inertial measurement unit (IMU), i.e., execute step S104. The angular velocity zero bias is the non-zero angular velocity value measured by the IMU in an equilibrium state. During the use of the IMU, the angular velocity zero bias needs to be subtracted from the angular velocity measured by the IMU. Specifically, the difference between the angular velocity measured by the IMU at the current moment and the updated zero bias prediction value at the current moment can be calculated to obtain the corrected angular velocity at the current moment.

[0051] The above-mentioned solution has multiple application scenarios. For example, it can be used to perform angular velocity zero-bias compensation on the inertial measurement unit at the waist of a robot, thereby improving the accuracy of digital twin simulation. Another example is for drones or unmanned vehicles, where angular velocity zero-bias can cause severe cumulative errors during long-endurance missions; the solution described in this application can perform long-term online correction of angular velocity using a low-computational-load filter.

[0052] As an implementation form of the embodiment of the present application, the step of determining the weight of the zero bias observation value and the weight of the zero bias prediction value based on the dispersion degree information of the historical zero bias observation value and the uncertainty information of the zero bias prediction value can comprise:

[0053] The gain amount of the uncertainty information of the zero bias prediction value with respect to the dispersion degree information of the historical zero bias observation value is determined as the weight of the zero bias observation value; and the weight of the zero bias prediction value is determined based on the weight of the zero bias observation value.

[0054] As mentioned above, the sum of the weight of the zero bias prediction value and the weight of the zero bias observation value is 1, and the weights of the two are complementary, therefore, the uncertainty information of the zero bias prediction value is positively correlated with the weight of the zero bias observation value, that is, the higher the untrustworthiness of the zero bias prediction value, the higher the trustworthiness of the zero bias observation value. And the weight of the zero bias observation value is negatively correlated with the dispersion degree information of the historical zero bias observation value, therefore, the gain amount of the uncertainty information of the zero bias prediction value with respect to the dispersion degree information of the historical zero bias observation value can be determined as the weight of the zero bias observation value.

[0055] In an implementation form, if the Kalman Filter algorithm is adopted, the uncertainty information of the zero bias prediction value can comprise a prior prediction covariance, the dispersion degree information of the historical zero bias observation value can comprise an observation covariance, and the gain amount can comprise a Kalman gain. In this case, the electronic device can determine the Kalman gain at the current time as the weight of the zero bias observation value at the current time based on the observation covariance corresponding to the zero bias observation value and the prior prediction covariance corresponding to the zero bias prediction value at the current time. The observation covariance can represent the fluctuation degree of the zero bias observation value in a period of time, and the prior prediction covariance can represent the untrustworthiness of predicting the angular velocity zero bias at the current time only by the zero bias prediction value updated at the previous time.

[0056] In the Kalman filter, the Kalman gain can represent the weight of the zero bias observation value. The higher the fluctuation degree of the zero bias observation value, the more untrustworthy the zero bias observation value, and the higher the untrustworthiness of the zero bias prediction value, the more trustworthy the zero bias observation value, therefore, the Kalman gain can be negatively correlated with the observation covariance and positively correlated with the prior prediction covariance.

[0057] In an implementation form, the Kalman gain can be determined according to the following formula: . Wherein, K is the Kalman gain at the current time, P is the prior prediction covariance corresponding to the zero bias prediction value at the current time, Q is the observation covariance corresponding to the zero bias observation value at the current time.

[0058] The observation covariance can be determined according to the fluctuation degree of the bias-free observation values of the preset number of time points. Specifically, the variance of the bias-free observation values in the sliding window can be determined as the observation covariance corresponding to the bias-free observation value at the current time point. In the form of a formula, the above process is described as follows: .

[0059] wherein, is the variance of the bias-free observation values in the sliding window corresponding to the current time point, as the observation covariance corresponding to the bias-free observation value at the current time point, is the bias-free observation value at the current time point.

[0060] In some cases, the motion data measured by the inertial measurement unit can not be transmitted to the electronic device in a short time, resulting in data accumulation. The above-mentioned situation can be determined by the fluctuation amplitude of the sampling period of the inertial measurement unit. If the fluctuation amplitude is greater than the preset time threshold, it is considered that the above-mentioned situation occurs. At this time, the bias-free observation values in a longer period of time can be adaptively selected for variance calculation according to the current sampling period, to ensure the consistency of the results under different sampling frequencies. The preset time threshold can be set according to experience, such as 0.001s, 0.002s, etc.

[0061] For each time point, after determining the weight of the bias-free observation value, the weight of the bias-free prediction value can be determined based on the weight of the bias-free observation value. In an embodiment, the difference between 1 and the weight of the bias-free observation value at the time point can be calculated to obtain the weight of the bias-free prediction value at the time point.

[0062] As can be seen, in the embodiments of the present application, the weight of the bias-free observation value is negatively correlated with the dispersion degree information of the historical bias-free observation value, and is positively correlated with the uncertainty information of the bias-free prediction value, so the gain amount of the uncertainty information of the bias-free prediction value with respect to the dispersion degree information of the historical bias-free observation value can be determined as the weight of the bias-free observation value. In this way, the weight of the bias-free observation value can be quickly and accurately determined, and then the weight of the bias-free prediction value can be determined based on the sum of the weights of the bias-free observation value and the bias-free prediction value being 1. In addition, the formulas used in the scheme of the present application are less, and the code is relatively simple to write, so the calculation resources occupied are less, and the calculation speed is faster.

[0063] As an embodiment of the embodiments of the present application, after the step of determining the gain amount of the uncertainty information of the bias-free prediction value with respect to the dispersion degree information of the historical bias-free observation value, the method can further include:

[0064] updating the uncertainty information of the bias-free prediction value based on the gain amount, to obtain the updated uncertainty information of the bias-free prediction value. ​

[0065] The uncertainty information of the zero bias prediction value and the uncertainty information of the updated zero bias prediction value are paired, and the uncertainty information of the zero bias prediction value is determined based on the uncertainty information of the updated zero bias prediction value. The gain amount is positively correlated with the uncertainty information of the zero bias prediction value, and after the electronic device determines the gain amount, the electronic device can update the uncertainty information of the zero bias prediction value based on the gain amount to obtain the uncertainty information of the updated zero bias prediction value.

[0066] The gain amount can represent the credibility of the zero bias observation value. If the gain amount is larger, it means that the credibility of the zero bias observation value is higher. Since the updated zero bias prediction value comprehensively considers the zero bias observation value and the zero bias prediction value, the higher the credibility of the zero bias observation value, the smaller the uncertainty information of the updated zero bias prediction value, that is, the gain amount is negatively correlated with the uncertainty information of the updated zero bias prediction value.

[0067] In the case of using the Kalman filtering algorithm, the uncertainty information of the updated zero bias prediction value can include a posteriori prediction covariance, and then the priori prediction covariance corresponding to the zero bias prediction value at the current moment can be updated based on the Kalman gain at the current moment to obtain the posteriori prediction covariance corresponding to the zero bias prediction value at the current moment. The posteriori prediction covariance and the priori prediction covariance are paired and are both used to represent the untrustworthiness of the zero bias prediction value. The priori prediction covariance represents the untrustworthiness of predicting the angular velocity zero bias at the current moment only by the updated zero bias prediction value at the previous moment, and the posteriori prediction covariance represents the untrustworthiness of predicting the angular velocity zero bias at the current moment by the updated zero bias prediction value at the previous moment and the zero bias observation value at the current moment. Since the angular velocity zero bias is predicted at the current moment by combining the zero bias observation value at the current moment, the predicted angular velocity zero bias is usually more accurate, so the posteriori prediction covariance at the same moment is usually smaller than the priori prediction covariance.

[0068] The determination of the posteriori prediction covariance and the priori prediction covariance is a cyclic process, that is, the priori prediction covariance at the current moment is determined based on the posteriori prediction covariance at the previous moment, the posteriori prediction covariance at the current moment is determined based on the priori prediction covariance at the current moment, and the priori prediction covariance at the next moment is determined based on the posteriori prediction covariance at the current moment, and so on. The posteriori prediction covariance at the initial moment can be 0.

[0069] That is, for each moment, the priori prediction covariance and the posteriori prediction covariance at the moment need to be determined. When the posteriori prediction covariance is determined, the electronic device can update the priori prediction covariance corresponding to the zero bias prediction value at the current moment based on the Kalman gain at the current moment to obtain the posteriori prediction covariance corresponding to the zero bias prediction value at the current moment.

[0070] In one implementation, the formula can be used to predict the covariance based on the prior information at the current time. and Kalman gain Determine the posterior prediction covariance at the current moment. : As can be seen from the formula, the Kalman gain... Covariance with posterior prediction They are negatively correlated. When updating the posterior predicted covariance, if the inertial measurement unit is in a state of violent motion or is subject to strong magnetic field interference at the current moment, making it impossible to determine the zero-bias observation value at the current moment, then the step of updating the posterior predicted covariance can be skipped, and only the prior predicted covariance at the current moment can be retained. In order to ensure that the subsequent iteration process can proceed normally, the prior predicted covariance at the current moment can be used as the posterior predicted covariance at the current moment.

[0071] As can be seen, in this embodiment, determining the uncertainty information of the zero-bias prediction value and determining the uncertainty information of the updated zero-bias prediction value are cyclical processes. That is, when determining the uncertainty information of the zero-bias prediction value, it is necessary to base it on the uncertainty information of the updated zero-bias prediction value, and vice versa. This ensures that the real-time update of the angular velocity zero bias can proceed normally.

[0072] As one implementation method of this application, such as Figure 2 As shown, prior to the step of determining the gain of information on the degree of dispersion of the uncertainty information of the zero-biased prediction with respect to the historical zero-biased observations, the method may also include S201 and S202.

[0073] S201, based on the degree of difference between the zero-biased observation and the zero-biased prediction, determine the amount of change in the uncertainty information of the updated zero-biased prediction.

[0074] If the difference between the zero-biased observations and the zero-biased predictions is significant, it indicates that the updated zero-biased predictions contain substantial uncertainty. Therefore, the change in uncertainty of the updated zero-biased predictions can be determined based on the degree of difference between the zero-biased observations and the zero-biased predictions.

[0075] When using the Kalman filter algorithm, the aforementioned changes can include the covariance increment. When determining the prior prediction covariance at the current time step using the posterior prediction covariance from the previous time step, the posterior prediction covariance from the previous time step can be added to the covariance increment at the current time step to obtain the prior prediction covariance at the current time step. The covariance increment at the current time step can be determined based on the degree of difference between the zero-biased observation and the zero-biased prediction at the current time step.

[0076] If the difference is large, it indicates that the current time zero bias prediction value can lag behind the zero bias observation value, i.e., the credibility of the zero bias prediction value is low. Since the prior prediction covariance reflects the uncredibility of the zero bias prediction value, it is necessary to make the prior prediction covariance larger. Since the prior prediction covariance is determined based on the covariance increment and the posterior prediction covariance of the previous time, the covariance increment of the current time can be made larger, i.e., the covariance increment of the current time is positively correlated with the difference between the zero bias observation value and the zero bias prediction value of the current time.

[0077] S202, adding the uncertainty information of the updated zero bias prediction value and the change amount to obtain the uncertainty information of the zero bias prediction value.

[0078] After determining the change amount of the uncertainty information of the updated zero bias prediction value, the uncertainty information of the updated zero bias prediction value and the change amount can be added to obtain the uncertainty information of the zero bias prediction value.

[0079] In the case of using the Kalman filtering algorithm, after determining the covariance increment of the current time, the posterior prediction covariance of the previous time and the covariance increment of the current time can be added to obtain the prior prediction covariance of the current time. In the form of formula, the above process is represented as: . Wherein, is the prior prediction covariance of the current time, is the posterior prediction covariance of the previous time, is the covariance increment of the current time.

[0080] It can be seen that in the embodiment of the present application, the change amount of the uncertainty information of the updated zero bias prediction value is not a fixed value, but is determined by the difference between the zero bias observation value and the zero bias prediction value. In the case of high difference, the covariance increment can be adaptively enlarged, so that the uncertainty information of the zero bias prediction value is enlarged, and the elasticity and sensitivity of the uncertainty information of the zero bias prediction value can be improved.

[0081] As an implementation manner of the embodiment of the present application, as shown in Figure 3 The step of determining the change amount of the uncertainty information of the updated zero bias prediction value based on the difference between the zero bias observation value and the zero bias prediction value can include S301 to S303.

[0082] S301, determining whether bias drift occurs. In response to the occurrence of bias drift, step S302 is performed; in response to the non-occurrence of bias drift, step S303 is performed.

[0083] The bias drift represents a difference between the zero bias observation value and the zero bias prediction value to a preset degree. The determination manner of the bias drift will be described in subsequent embodiments. In the case of using the Kalman filtering algorithm, in order to determine whether the covariance increment at the current moment needs to be amplified, the difference between the zero bias observation value at the current moment and the zero bias prediction value can be determined, and it is determined whether the bias drift occurs. If the bias drift occurs, it indicates that the zero bias prediction value at the current moment can lag behind the zero bias observation value, and the credibility of the zero bias prediction value is low, and the covariance increment needs to be amplified. If the bias drift does not occur, the covariance increment does not need to be amplified.

[0084] S302, multiplying the noise power spectral density of the inertial measurement unit, the length of the sampling period of the inertial measurement unit and the preset amplification coefficient to obtain the covariance increment at the current moment.

[0085] In the case of the bias drift, the electronic device can multiply the noise power spectral density of the inertial measurement unit, the length of the sampling period of the inertial measurement unit at the current moment and the preset amplification coefficient to obtain the change amount of the uncertainty information of the updated zero bias prediction value at the current moment. In the case of using the Kalman filtering algorithm, the obtained change amount can include the covariance increment at the current moment. The noise power spectral density is used to describe the intensity of the system process noise, that is, the change uncertainty of the prior prediction covariance per unit time. The sampling period at the current moment is the time interval between the current moment and the previous moment. Therefore, the multiplication result of the noise power spectral density and the length of the sampling period can represent the change uncertainty of the prior prediction covariance between the current moment and the previous moment. The preset amplification coefficient is greater than 1, and the specific value can be set according to experience, for example, the value between 5 and 20 can be taken.

[0086] S303, multiplying the noise power spectral density of the inertial measurement unit and the length of the sampling period of the inertial measurement unit to obtain the covariance increment at the current moment.

[0087] In the case of the bias drift, the electronic device can multiply the noise power spectral density of the inertial measurement unit and the length of the sampling period at the current moment to obtain the change amount of the uncertainty information of the updated zero bias prediction value at the current moment. In the case of using the Kalman filtering algorithm, the obtained change amount can include the covariance increment at the current moment. The following is the calculation formula of the covariance increment in the case of the bias drift and the case of no bias drift: In the case of the bias drift, In the case of no bias drift, . Wherein, ​a noise power spectral density of the inertial measurement unit, a sampling period of a current time.

[0088] It can be seen that, in the embodiments of the present application, whether to amplify the change amount is flexibly selected according to whether the difference between the zero bias observation value and the zero bias prediction value reaches the preset degree, thereby improving the accuracy of determining the angular velocity zero bias.

[0089] As an embodiment of the present application, before the step of determining the change amount of the uncertainty information of the updated zero bias prediction value based on the difference between the zero bias observation value and the zero bias prediction value, the above method further includes at least one of the following.

[0090] In the case that the zero bias observation value of each time is greater than the zero bias prediction value of the time or the zero bias observation value of each time is less than the zero bias prediction value of the time within the second preset period, it is determined that the difference between the zero bias observation value and the zero bias prediction value reaches the preset degree;

[0091] In the case that there is at least one time when the zero bias observation value is not equal to the zero bias prediction value within the second preset period, in response to the case that the ratio between the number difference value and the number sum value is greater than the preset ratio threshold value, it is determined that the difference between the zero bias observation value and the zero bias prediction value reaches the preset degree.

[0092] In the first embodiment, the electronic device can determine whether the zero bias observation value of each time is greater than the zero bias prediction value of the time or the zero bias observation value of each time is less than the zero bias prediction value of the time within the second preset period. If so, it can be determined that the difference between the zero bias observation value and the zero bias prediction value reaches the preset degree, that is, the bias drift occurs. The second preset period is a preset period from the current time, which can be set according to experience, such as the last 5 times, 8 times, 10 times, etc.

[0093] As a specific implementation, the sign of the difference between the zero bias observation value and the zero bias prediction value of each time within the second preset period can be determined, that is, the difference is positive or negative. If the signs of the differences within the second preset period are the same, it indicates that the zero bias observation value is continuously less than the zero bias prediction value or the zero bias observation value is continuously greater than the zero bias prediction value, and then it can be determined that the difference between the zero bias observation value and the zero bias prediction value reaches the preset degree. If the signs of the differences within the recent preset time period are not completely the same, it indicates that the difference between the zero bias observation value and the zero bias prediction value does not reach the preset degree. As another specific implementation, the length of time when the sign of the observation residual is continuously positive , or the length of time when the sign of the observation residual is continuously negative Suppose Or The time length is greater than the preset time period , then the bias drift occurs. Suppose And Both are not greater than the time length of the preset time period , then the bias drift does not occur.

[0094] In the second implementation, if there is at least one time point in the second preset time period, the zero bias observation value is not equal to the zero bias prediction value, the electronic device can calculate the quantity difference, that is, the difference between the number of time points in the second preset time period, the zero bias observation value is greater than the zero bias prediction value, and the number of time points, the zero bias observation value is less than the zero bias prediction value. Next, the electronic device can calculate whether the ratio between the quantity difference and the quantity sum is greater than the preset ratio threshold. The quantity sum can reflect the total amount between the number of time points in the second preset time period, the zero bias observation value is greater than the zero bias prediction value, and the number of time points, the zero bias observation value is less than the zero bias prediction value, and the preset ratio threshold can be set according to experience, such as: 0.5, 0.6, 0.7, etc.

[0095] If the ratio between the quantity difference and the quantity sum is greater than the preset ratio threshold, the difference between the zero bias observation value and the zero bias prediction value reaches the preset degree. If the ratio between the quantity difference and the quantity sum is not greater than the preset ratio threshold, the difference between the zero bias observation value and the zero bias prediction value does not reach the preset degree.

[0096] The quantity difference and the quantity sum can be calculated by the following method: determining the first time point, the zero bias observation value is greater than the zero bias prediction value, and the second time point, the zero bias prediction value is greater than the zero bias observation value, calculating the difference between the number of the first time point and the number of the second time point, obtaining the quantity difference, and calculating the sum between the number of the first time point and the number of the second time point, obtaining the quantity sum.

[0097] As a specific implementation, the electronic device can perform exponential low-pass filtering on the sign of the observation residual of each time point to obtain the average change of the observation residual sign. Suppose the average change is greater than the preset ratio threshold , then the difference between the zero bias observation value and the zero bias prediction value reaches the preset degree. Suppose the average change is not greater than the preset ratio threshold , then the difference between the zero bias observation value and the zero bias prediction value does not reach the preset degree.

[0098] It can be seen that in the embodiment of the application, the electronic device can quickly judge whether the difference between the zero bias observation value and the zero bias prediction value is too large, and amplify the change in a timely manner when the difference is too large, thereby improving the accuracy of determining the angular velocity zero bias.

[0099] As an implementation of the embodiment of the present application, as shown in Figure 4 The method can further include S401-S403 after the step of adding the uncertainty information of the updated zero-offset prediction value and the variation to obtain the uncertainty information of the zero-offset prediction value.

[0100] S401, determine the relationship between the preset covariance range and the prior prediction covariance. In response to the upper limit of the preset covariance range being less than the prior prediction covariance, perform step S402; in response to the lower limit of the preset covariance range being greater than the prior prediction covariance, perform step S403.

[0101] To prevent the uncertainty information of the zero-offset prediction value from suddenly changing, the electronic device can determine the relationship between the preset uncertainty information range and the uncertainty information of the zero-offset prediction value. The upper limit and the lower limit of the preset uncertainty information range can be set according to experience, for example, the upper limit can be , and the lower limit can be .

[0102] If the upper limit of the preset uncertainty information range is less than the uncertainty information of the zero-offset prediction value, the upper limit is taken as the uncertainty information of the zero-offset prediction value; if the lower limit of the preset uncertainty information range is greater than the uncertainty information of the zero-offset prediction value, the lower limit is taken as the uncertainty information of the zero-offset prediction value. If the preset uncertainty information range includes the uncertainty information of the zero-offset prediction value, the uncertainty information of the zero-offset prediction value does not need to be adjusted.

[0103] In the case of using the Kalman filtering algorithm, the preset uncertainty information range can include the covariance range. To prevent the prior prediction covariance from being abnormal, for example, diverging or degenerating to 0, the value of the prior prediction covariance can be constrained by the preset covariance range after the prior prediction covariance is determined each time. In the case where the covariance range includes the prior prediction covariance, it indicates that the value of the prior prediction covariance is in a suitable range, and the value of the prior prediction covariance does not need to be adjusted.

[0104] S402, take the upper limit as the prior prediction covariance.

[0105] In the case where the upper limit of the covariance range is less than the prior prediction covariance, it indicates that the prior prediction covariance diverges. To suppress the divergence, the upper limit of the covariance range can be taken as the prior prediction covariance.

[0106] S403, take the lower limit as the prior prediction covariance.

[0107] In a case where the lower limit of the covariance range is greater than the prior predictive covariance, it indicates that the prior predictive covariance is too small. In order to avoid the prior predictive covariance degenerating to 0, the upper and lower limits of the covariance range can be used as the prior predictive covariance.

[0108] The process of limiting the value of the prior predictive covariance by the covariance range can be expressed as:

[0109] wherein, is the prior predictive covariance after limiting the value, is a clipping function, is the lower limit of the covariance range, is the upper limit of the covariance range.

[0110] It can be seen that, in the embodiment of the application, the electronic device can limit the value of the uncertainty information of the zero bias prediction value by the preset uncertainty information range, thereby inhibiting the divergence or degeneration of the uncertainty information to 0 and improving the stability of the angular velocity correction.

[0111] As an implementation manner of the embodiment of the application, the motion data can further include linear acceleration measured by an accelerometer and horizontal magnetic force direction measured by a magnetometer. In this case, as shown in FIG. 5, the step of determining the angular velocity zero bias according to the motion data measured by the inertial measurement unit can include S501 to S503. Figure 5

[0112] S501, determining an attitude offset according to a difference between the linear acceleration direction and the gravity direction and a difference between the horizontal magnetic force direction and the geomagnetic direction.

[0113] The gravity direction is the direction of the earth's gravitational field, and the gravity direction can be used as a vertical reference direction. When the inertial measurement unit is in a force balance state, the linear acceleration is provided by the gravity, that is, the linear acceleration direction is approximately the gravity direction. Therefore, the attitude offset of the inertial measurement unit in the vertical direction can be determined according to the difference between the linear acceleration direction and the gravity direction. In an implementation manner, the attitude offset in the vertical direction can be represented as a roll angle (Roll) and a pitch angle (Pitch).

[0114] The geomagnetic direction is the horizontal component of the earth's magnetic field, and the geomagnetic direction can be used as a horizontal reference direction. Since the horizontal magnetic force direction is the projection direction of the geomagnetic direction in the device coordinate system of the movable device carrying the inertial measurement unit, the attitude offset of the inertial measurement unit in the horizontal direction can be determined according to the difference between the horizontal magnetic force direction and the geomagnetic direction. In an implementation manner, the attitude offset in the horizontal direction can be represented as a yaw angle (Yaw).

[0115] ​In summary, the electronic device can determine the attitude offset according to the difference between the linear acceleration direction and the gravity direction and the difference between the horizontal magnetic force direction and the geomagnetic direction.

[0116] S502, filter the attitude offset to obtain the attitude offset when the inertial measurement unit is in a balanced state.

[0117] Since the angular velocity bias is the angular velocity measured by the gyroscope when the inertial measurement unit is in a balanced state (static or uniform linear motion), the inertial measurement unit is usually not in a balanced state during the operation of the movable device carrying the inertial measurement unit. Therefore, in this case, the real-time determined angular velocity bias is not accurate enough. In order to make the determined angular velocity bias more accurate, the attitude offset can be corrected to a balanced state. Specifically, the attitude offset can be filtered to obtain the attitude offset when the inertial measurement unit is in a balanced state.

[0118] S503, according to the angle offset represented by the attitude offset in the balanced state and the length of the sampling period of the inertial measurement unit, determine the angular velocity bias.

[0119] After obtaining the attitude offset in the balanced state, the electronic device can determine the angular velocity bias according to the angle offset represented by the attitude offset in the balanced state and the length of the sampling period of the inertial measurement unit.

[0120] The above process is described in the form of formula as follows: assuming that the attitude offset in the balanced state is , By representing by a quaternion, the normalized processing can be performed on to determine a legal attitude expression. Next, the angle offset can be calculated according to the following formula : . . Wherein, is the real part of , is the real part of is the angular velocity bias, is the length of the sampling period of the inertial measurement unit. It can also be expressed as , is a one-dimensional state variable of the angular velocity bias at the current moment, is the observation noise, represents obeys zero-mean Gaussian distribution. It can be expressed as , is a one-dimensional state variable of the angular velocity bias at the previous moment, for process noise, denotes subject to a zero-mean Gaussian distribution, for process covariance.

[0121] It can be seen that, in the embodiments of the present application, the attitude offset of the inertial measurement unit is converted to the balanced state by filtering, which can improve the robustness and numerical stability of the attitude offset. In addition, the angular velocity bias is the angular velocity measured by the gyroscope when the inertial measurement unit is in the balanced state, so such processing can improve the accuracy of determining the angular velocity bias.

[0122] As an implementation of the embodiments of the present application, the step of filtering the attitude offset to obtain the attitude offset of the inertial measurement unit in the balanced state can include at least one of the following:

[0123] In response to the offset angle of the attitude offset being greater than a preset angle, a linear filtering method is used to filter the attitude offset to obtain a filtered attitude offset as the attitude offset.

[0124] In response to the offset angle of the attitude offset being not greater than the preset angle, a spherical linear interpolation method is used to filter the attitude offset to obtain a filtered attitude offset as the attitude offset.

[0125] In the first implementation, the offset angle of the attitude offset is greater than the preset angle, in which case it is indicated that the offset angle of the attitude offset is small, and the change of the attitude can be approximated as linear, so the linear filtering method can be used to filter the attitude offset. For example, the linear filtering method can be exponential weighted filtering, mean filtering, Gaussian filtering, etc. In the case of using exponential weighted filtering, the linear weighting formula can be as follows:

[0126] wherein, is the filtered attitude offset, is the quaternion representation of the attitude offset, is a proportional coefficient, and the degree of the proportional coefficient close to 1 is positively correlated with the offset angle of the attitude offset.

[0127] In the second implementation, the offset angle of the attitude offset is not greater than the preset angle, in which case it is indicated that the offset angle of the attitude offset is relatively large, and the change of the attitude cannot be approximated as linear. If the exponential weighting manner is still used for filtering, the filtering result will be inaccurate. At this time, a nonlinear filtering manner can be used to filter the attitude offset. For example, the nonlinear filtering manner can be a spherical linear interpolation (Slerp) filtering, a Kalman filtering, a particle filtering, and the like. In the case of using the spherical linear interpolation filtering, the following spherical linear interpolation formula can be used:

[0128] . . Wherein, is a unit quaternion, is a quaternion included angle, is the attitude offset.

[0129] It can be seen that in the embodiments of the present application, the electronic device can use a corresponding filtering manner to filter the attitude offset based on the size relationship between the offset angle and the preset angle, so as to avoid the jump or divergence of the attitude offset, thereby improving the accuracy of the angular velocity zero offset.

[0130] As an implementation of the embodiments of the present application, as shown in Figure 6 , the above step of determining the attitude offset according to the difference between the linear acceleration direction and the gravity direction and the difference between the horizontal magnetic force direction and the geomagnetic direction can include S601 to S602.

[0131] S601, determining a first attitude offset according to the difference between the linear acceleration direction and the gravity direction, and determining a second attitude offset according to the difference between the horizontal magnetic force direction and the geomagnetic direction.

[0132] The electronic device can determine the first attitude offset according to the rotation difference between the acceleration direction and the gravity direction. The first attitude offset can be represented by Euler angles, rotation matrices, quaternions, etc. In the case of being represented by quaternions, assuming that the linear acceleration direction measured by the accelerometer is , and the gravity direction is , the first attitude offset can be obtained: .

[0133] The electronic device can determine the second attitude offset according to the rotation difference between the horizontal magnetic force direction and the geomagnetic direction. The second attitude offset can be represented by Euler angles, rotation matrices, quaternions, etc. In the case of being represented by quaternions, assuming that the horizontal magnetic force direction measured by the accelerometer is , the geomagnetic direction is , and then the second attitude offset : .

[0134] S602, the first attitude offset and the second attitude offset are fused to obtain an attitude offset.

[0135] In the case where the attitude offset is represented by a rotation matrix, the first attitude offset and the second attitude offset can be multiplied to obtain the fused attitude offset. In the case where the attitude is represented by Euler angles, the first attitude offset and the second attitude offset can be weightedly fused to obtain the fused attitude offset.

[0136] In the case where the attitude offset is represented by a quaternion, the first attitude offset and the second attitude offset can be multiplied to obtain the attitude offset of the inertial measurement unit, i.e., the attitude offset . Wherein, represents quaternion multiplication.

[0137] It can be seen that in the embodiments of the present application, during the use of the inertial measurement unit, the electronic device can quickly and accurately determine the attitude offset of the inertial measurement unit.

[0138] As an implementation manner of the embodiments of the present application, the above inertial measurement unit can be mounted on the joint of the robot, and in this case, the step of determining the angular velocity bias according to the motion data measured by the inertial measurement unit can include:

[0139] During the motion of the robot, for the joint that occurs motion, the angular velocity bias is determined according to the motion data measured by the inertial measurement unit mounted on the joint.

[0140] In order to accurately determine the angular velocity of the joint of the robot, the inertial measurement unit can be mounted on the joint of the robot. In this way, during the motion of the robot, the angular velocity correction method proposed in the present application can be performed for the joint that occurs motion, thereby improving the accuracy of the control of the joint of the robot.

[0141] Corresponding to the above angular velocity correction method, the embodiments of the present application also provide an angular velocity correction device, and the angular velocity correction device provided by the embodiments of the present application will be introduced below.

[0142] As shown in Figure 7 , an angular velocity correction device, the device includes:

[0143] An observation value determination module 701 is configured to determine an angular velocity bias as a bias observation value according to motion data measured by an inertial measurement unit.

[0144] The weight determination module 702 is configured to determine a weight of the zero bias observation value and a weight of the zero bias prediction value based on the dispersion degree information of the historical zero bias observation value and the uncertainty information of the zero bias prediction value, wherein the historical zero bias observation value includes zero bias observation values at each time within a first preset time period, and the uncertainty information represents a difference between the zero bias prediction value and a true value of the angular velocity zero bias.

[0145] The angular velocity zero bias updating module 703 is configured to update the zero bias prediction value by using the zero bias observation value, the zero bias prediction value, the weight of the zero bias observation value, and the weight of the zero bias prediction value.

[0146] The angular velocity correction module 704 is configured to correct the angular velocity included in the motion data by using the updated zero bias prediction value.

[0147] It can be seen that, in the embodiment of the present application, since neither the zero bias observation value nor the zero bias prediction value is completely accurate, the dispersion degree information corresponding to the zero bias observation value can reflect the accuracy of the zero bias observation value, and the uncertainty information of the zero bias prediction value can reflect the accuracy of the zero bias prediction value. Therefore, the corresponding weights are allocated based on the accuracy of the zero bias observation value and the zero bias prediction value, and the zero bias prediction value is updated according to the determined weights, so that the updated zero bias prediction value is more accurate. In this way, the angular velocity zero bias is determined according to the motion data measured by the inertial measurement unit, and the angular velocity is corrected by using the angular velocity zero bias, instead of using a fixed and unchanged angular velocity zero bias obtained by pre-calibration, so that the accuracy of the corrected angular velocity can be improved.

[0148] As an implementation manner of the embodiment of the present application, the weight determination module 702 can include: a first weight determination sub-module configured to determine a gain amount of the uncertainty information of the zero bias prediction value with respect to the dispersion degree information of the historical zero bias observation value as the weight of the zero bias observation value, wherein the gain amount is negatively correlated with the dispersion degree information of the historical zero bias observation value and positively correlated with the uncertainty information of the zero bias prediction value, and the sum of the weight of the zero bias observation value and the weight of the zero bias prediction value is 1; and a second weight determination sub-module configured to determine the weight of the zero bias prediction value based on the weight of the zero bias observation value.

[0149] As an implementation manner of the embodiment of the present application, the apparatus can further include: a first uncertainty information determination module configured to update the uncertainty information of the zero bias prediction value based on the gain amount to obtain the uncertainty information of the updated zero bias prediction value, wherein the gain amount is negatively correlated with the uncertainty information of the updated zero bias prediction value.

[0150] As an implementation form of the embodiment of the present application, the device can further include: a change amount determination module, configured to determine a change amount of the uncertainty information of the updated zero bias prediction value based on a difference degree between the zero bias observation value and the zero bias prediction value; and a second uncertainty information determination module, configured to add the uncertainty information of the updated zero bias prediction value and the change amount to obtain the uncertainty information of the zero bias prediction value.

[0151] As an implementation form of the embodiment of the present application, the change amount determination module can include: a first change amount determination submodule, configured to, in response to the difference degree between the zero bias observation value and the zero bias prediction value reaching a preset degree, multiply the noise power spectral density of the inertial measurement unit, a time length of the sampling period of the inertial measurement unit and a preset amplification coefficient to obtain the change amount of the uncertainty information of the updated zero bias prediction value, wherein the preset amplification coefficient is greater than 1; and a second change amount determination submodule, configured to, in response to the difference degree between the zero bias observation value and the zero bias prediction value not reaching the preset degree, multiply the noise power spectral density of the inertial measurement unit and the time length of the sampling period of the inertial measurement unit to obtain the change amount of the uncertainty information of the updated zero bias prediction value.

[0152] As an implementation form of the embodiment of the present application, the device can further include: a first difference determination module, configured to, in a case where the zero bias observation value at each time is greater than the zero bias prediction value at the time or the zero bias observation value at each time is less than the zero bias prediction value at the time within a second preset time period, determine that the difference degree between the zero bias observation value and the zero bias prediction value reaches a preset degree, wherein the second preset time period is a preset time period from a current time; and a second difference determination module, configured to, in a case where there is at least one time at which the zero bias observation value is not equal to the zero bias prediction value within the second preset time period, determine that the difference degree between the zero bias observation value and the zero bias prediction value reaches the preset degree in response to a case where a ratio between a quantity difference value and a quantity sum value is greater than a preset ratio threshold value, wherein the quantity difference value reflects a difference between a quantity of times at which the zero bias observation value is greater than the zero bias prediction value and a quantity of times at which the zero bias observation value is less than the zero bias prediction value within the second preset time period, and the quantity sum value reflects a total amount between the quantity of times at which the zero bias observation value is greater than the zero bias prediction value and the quantity of times at which the zero bias observation value is less than the zero bias prediction value within the second preset time period.

[0153] As an implementation of the embodiment of the present application, the device can further comprise: a first value limiting module, configured to, in response to an upper limit of the preset uncertainty information range being less than the uncertainty information of the zero bias prediction value, take the upper limit as the uncertainty information of the zero bias prediction value; and a second value limiting module, configured to, in response to a lower limit of the preset uncertainty information range being greater than the uncertainty information of the zero bias prediction value, take the lower limit as the uncertainty information of the zero bias prediction value.

[0154] As an implementation of the embodiment of the present application, the gain quantity comprises a Kalman gain, the degree of dispersion information comprises an observation covariance, the uncertainty information of the zero bias prediction value comprises a prior prediction covariance, and the updated uncertainty information of the zero bias prediction value comprises a posterior prediction covariance.

[0155] As an implementation of the embodiment of the present application, the motion data further comprises linear acceleration and a horizontal magnetic force direction, and in this case, the observation value determining module 701 can comprise: a posture offset determining sub-module, configured to determine a posture offset quantity according to a difference between the linear acceleration direction and the gravity direction and a difference between the horizontal magnetic force direction and the geomagnetic direction; a filtering sub-module, configured to filter the posture offset quantity to obtain a posture offset quantity of the inertial measurement unit in a balanced state; and a first zero bias determining sub-module, configured to determine an angular velocity zero bias according to an angle offset quantity represented by the posture offset quantity in the balanced state and a time length of a sampling period of the inertial measurement unit.

[0156] As an implementation of the embodiment of the present application, the filtering sub-module can comprise: a first filtering unit, configured to, in response to an offset angle of the posture offset quantity being greater than a preset angle, filter the posture offset quantity in a linear filtering manner to obtain a filtered posture offset quantity as the posture offset quantity; and a second filtering unit, configured to, in response to the offset angle of the posture offset quantity not being greater than the preset angle, filter the posture offset quantity in a nonlinear filtering manner to obtain a filtered posture offset quantity as the posture offset quantity.

[0157] As an implementation of the embodiment of the present application, the posture offset determining sub-module can comprise: a posture offset determining unit, configured to determine a first posture offset quantity according to a difference between the linear acceleration direction and the gravity direction, and determine a second posture offset quantity according to a difference between the horizontal magnetic force direction and the geomagnetic direction; and a posture fusion unit, configured to fuse the first posture offset quantity and the second posture offset quantity to obtain a posture offset quantity.

[0158] As one embodiment of this application, the inertial measurement unit is mounted on the joint of the robot. In this case, the observation value determination module 701 may include a second zero-bias determination submodule, which is used to determine the angular velocity zero bias for the joint that is moving during the movement of the robot, based on the motion data measured by the inertial measurement unit mounted on the joint.

[0159] like Figure 8 The diagram shows a schematic of a robot, including: a memory 801 for storing computer programs; and a processor 802 for executing the program stored in the memory 801 to implement the angular velocity correction method steps described in any of the above embodiments. The robot may also include a communication bus and / or a communication interface, through which the processor 802, the communication interface, and the memory 801 communicate with each other.

[0160] As can be seen, in this embodiment, since neither the zero-bias observation value nor the zero-bias prediction value is completely accurate, the dispersion information corresponding to the zero-bias observation value can reflect the accuracy of the zero-bias observation value, and the uncertainty information of the zero-bias prediction value can reflect the accuracy of the zero-bias prediction value. Therefore, by assigning corresponding weights based on the accuracy of the zero-bias observation value and the zero-bias prediction value, and updating the zero-bias prediction value according to the determined weights, the updated zero-bias prediction value can be made more accurate. In this way, by determining the angular velocity zero bias based on the motion data measured by the inertial measurement unit, and using the angular velocity zero bias to correct the angular velocity, instead of using a pre-calibrated fixed angular velocity zero bias to correct the angular velocity, the accuracy of the corrected angular velocity can be improved.

[0161] The communication bus mentioned above for the robot can be a Peripheral Component Interconnect (PCI) bus or an Extended Industry Standard Architecture (EISA) bus, etc. This communication bus can be divided into address bus, data bus, control bus, etc. For ease of illustration, only one thick line is used to represent it in the diagram, but this does not mean that there is only one bus or one type of bus.

[0162] The communication interface is used for communication between the robot and other devices.

[0163] The memory may include random access memory (RAM) or non-volatile memory (NVM), such as at least one disk storage device. Optionally, the memory may also be at least one storage device located remotely from the aforementioned processor.

[0164] The processor described above can be a general processor, including a central processing unit (CPU), a network processor (NP), etc.; can also be a digital signal processor (DSP), an application specific integrated circuit (ASIC), a field-programmable gate array (FPGA) or other programmable logic device, a discrete gate or transistor logic device, a discrete hardware component.

[0165] In another embodiment provided by the present application, a computer readable storage medium is provided, and the computer readable storage medium stores a computer program. The computer program is executed by a processor to implement the steps of any of the angular velocity correction methods described above.

[0166] In another embodiment provided by the present application, a computer program product containing instructions is provided, and the computer program product, when executed on a computer, causes the computer to execute any of the angular velocity correction methods in the above embodiments.

[0167] In the above embodiments, all or part of the embodiments can be implemented by software, hardware, firmware or any combination thereof. When implemented by software, all or part of the embodiments can be implemented in the form of a computer program product. The computer program product includes one or more computer instructions. When the computer program instructions are loaded and executed on a computer, all or part of the processes or functions described in the embodiments of the present application are generated. The computer can be a general-purpose computer, a special-purpose computer, a computer network or other programmable device. The computer instructions can be stored in a computer readable storage medium or transferred from one computer readable storage medium to another computer readable storage medium, for example, the computer instructions can be transferred from one website, computer, server or data center to another website, computer, server or data center through wired (such as coaxial cable, optical fiber, digital subscriber line (DSL)) or wireless (such as infrared, wireless, microwave, etc.) manner. The computer readable storage medium can be any available medium that can be accessed by a computer or a data storage device such as a server, data center, etc. integrated with one or more available media. The available medium can be a magnetic medium (for example, floppy disk, hard disk, magnetic tape), an optical medium (for example, DVD), or a semiconductor medium (for example, solid state disk (SSD)) and the like.

[0168] It is to be noted that, as used in this document, the terminology "first", "second", etc. is merely used to differentiate one entity or action from another, and does not necessarily imply any actual physical or logical relationship or order between such entities or actions. Moreover, the terms "comprises", "comprising", or any other variations thereof, are intended to cover a non-exclusive inclusion, such that a process, method, article, or apparatus that comprises a list of elements does not include only those elements but can include other elements not expressly listed or inherent to such process, method, article, or apparatus. An element proceeded by "comprises... a" does not, without more constraints, exclude the existence of additional identical elements in the process, method, article, or apparatus that comprises the element.

[0169] Each of the embodiments in the present specification is described in a related manner, and the same or similar parts between the embodiments can be referred to each other. Each of the embodiments focuses on the difference from other embodiments. In particular, for the device, robot, computer-readable storage medium, and computer program product embodiments, since they are basically similar to the method embodiments, the description is relatively simple, and the relevant parts can be referred to the part of the method embodiment.

[0170] The above only describes the preferred embodiments of the present application, and is not intended to limit the protection scope of the present application. Any modification, equivalent replacement, improvement, etc. made within the spirit and principle of the present application shall be included in the protection scope of the present application.

Claims

1. A method of angular velocity correction, characterized by, The method comprises: determining an angular velocity bias as a bias observation value according to motion data measured by an inertial measurement unit; determining a weight of the bias observation value and a weight of a bias prediction value based on dispersion information of historical bias observation values and uncertainty information of the bias prediction value, wherein the historical bias observation values comprise bias observation values at each time within a first preset time period, and the uncertainty information represents a difference between the bias prediction value and a true value of the angular velocity bias; updating the bias prediction value by using the bias observation value, the bias prediction value, the weight of the bias observation value, and the weight of the bias prediction value; correcting an angular velocity included in the motion data by using the updated bias prediction value.

2. The method of claim 1, wherein, The step of determining the weight of the bias observation value and the weight of the bias prediction value based on the dispersion information of the historical bias observation values and the uncertainty information of the bias prediction value comprises: determining a gain amount of the uncertainty information of the bias prediction value with respect to the dispersion information of the historical bias observation values as the weight of the bias observation value, wherein the gain amount is negatively correlated with the dispersion information of the historical bias observation values and positively correlated with the uncertainty information of the bias prediction value, and a sum of the weight of the bias observation value and the weight of the bias prediction value is 1; determining the weight of the bias prediction value based on the weight of the bias observation value.

3. The method of claim 2, wherein, After the step of determining the gain amount of the uncertainty information of the bias prediction value with respect to the dispersion information of the historical bias observation values, the method further comprises: updating the uncertainty information of the bias prediction value based on the gain amount to obtain updated uncertainty information of the bias prediction value, wherein the gain amount is negatively correlated with the updated uncertainty information of the bias prediction value. Before the step of determining the gain amount of the uncertainty information of the bias prediction value with respect to the dispersion information of the historical bias observation values, the method further comprises: determining a change amount of the updated uncertainty information of the bias prediction value based on a difference degree between the bias observation value and the bias prediction value; adding the updated uncertainty information of the bias prediction value and the change amount to obtain the uncertainty information of the bias prediction value.

4. The method of claim 3, wherein, The step of determining the change amount of the updated uncertainty information of the bias prediction value based on the difference degree between the bias observation value and the bias prediction value comprises at least one of the following: in response to the difference degree between the bias observation value and the bias prediction value reaching a preset degree, multiplying a noise power spectral density of the inertial measurement unit, a length of a sampling period of the inertial measurement unit, and a preset amplification coefficient to obtain the change amount of the updated uncertainty information of the bias prediction value, wherein the preset amplification coefficient is greater than 1; in response to the difference between the zero bias observation value and the zero bias prediction value not reaching the preset degree, multiplying noise power spectral density of the inertial measurement unit by a time length of a sampling period of the inertial measurement unit to obtain a change amount of the updated uncertainty information of the zero bias prediction value.

5. The method of claim 4, wherein, Before the step of determining the change amount of the updated uncertainty information of the zero bias prediction value based on the difference between the zero bias observation value and the zero bias prediction value, the method further comprises at least one of the following: in a second preset time period, if the zero bias observation value at each time is greater than the zero bias prediction value at the time or the zero bias observation value at each time is less than the zero bias prediction value at the time, it is determined that the difference between the zero bias observation value and the zero bias prediction value reaches the preset degree, wherein the second preset time period is a preset time period from the current time forward; in the second preset time period, in response to a ratio between a quantity difference value and a quantity sum value being greater than a preset ratio threshold, if there is at least one time at which the zero bias observation value is not equal to the zero bias prediction value, it is determined that the difference between the zero bias observation value and the zero bias prediction value reaches the preset degree; wherein the quantity difference value reflects a difference between a quantity of times at which the zero bias observation value is greater than the zero bias prediction value and a quantity of times at which the zero bias observation value is less than the zero bias prediction value in the second preset time period, and the quantity sum value reflects a total amount between the quantity of times at which the zero bias observation value is greater than the zero bias prediction value and the quantity of times at which the zero bias observation value is less than the zero bias prediction value in the second preset time period.

6. The method according to any one of claims 3 to 5, characterized in that, After the step of adding the updated uncertainty information of the zero bias prediction value and the change amount to obtain the uncertainty information of the zero bias prediction value, the method further comprises at least one of the following: in response to an upper limit of a preset uncertainty information range being less than the uncertainty information of the zero bias prediction value, taking the upper limit as the uncertainty information of the zero bias prediction value; in response to a lower limit of the preset uncertainty information range being greater than the uncertainty information of the zero bias prediction value, taking the lower limit as the uncertainty information of the zero bias prediction value; and / or, the gain amount comprises Kalman gain, the dispersion degree information comprises observation covariance, the uncertainty information of the zero bias prediction value comprises prior prediction covariance, and the updated uncertainty information of the zero bias prediction value comprises posterior prediction covariance.

7. The method according to any one of claims 1 to 6, characterized in that, The motion data further comprises linear acceleration and horizontal magnetic force direction; and the step of determining the angular velocity zero bias according to the motion data measured by the inertial measurement unit comprises: determining an attitude offset amount according to a difference between the linear acceleration direction and the gravity direction and a difference between the horizontal magnetic force direction and the geomagnetic direction; filtering the attitude offset amount to obtain an attitude offset amount of the inertial measurement unit in a balanced state; and determining the angular velocity zero bias according to an angle offset amount represented by the attitude offset amount in the balanced state and a time length of a sampling period of the inertial measurement unit; and / or, The inertial measurement unit is mounted on a joint of the robot; the step of determining the angular velocity bias according to the motion data measured by the inertial measurement unit comprises: during the motion of the robot, for the joint that occurs motion, determining the angular velocity bias according to the motion data measured by the inertial measurement unit mounted on the joint.

8. The method of claim 7, wherein, The step of filtering the attitude offset to obtain the attitude offset of the inertial measurement unit in the balanced state comprises at least one of the following: in response to the offset angle of the attitude offset being greater than a preset angle, filtering the attitude offset by using a linear filtering manner to obtain a filtered attitude offset as the attitude offset; in response to the offset angle of the attitude offset being not greater than the preset angle, filtering the attitude offset by using a nonlinear filtering manner to obtain a filtered attitude offset as the attitude offset; And / or, The step of determining the attitude offset according to the difference between the linear acceleration direction and the gravity direction and the difference between the horizontal magnetic force direction and the geomagnetic direction comprises: determining a first attitude offset according to the difference between the linear acceleration direction and the gravity direction, and determining a second attitude offset according to the difference between the horizontal magnetic force direction and the geomagnetic direction; and fusing the first attitude offset and the second attitude offset to obtain the attitude offset.

9. An angular velocity correction device, characterized by The device comprises: An observation value determination module configured to determine an angular velocity bias as a bias observation value according to motion data measured by an inertial measurement unit; A weight determination module configured to determine a weight of the bias observation value and a weight of a bias prediction value based on dispersion degree information of historical bias observation values and uncertainty information of the bias prediction value, wherein the historical bias observation values comprise bias observation values at each time within a first preset time period, and the uncertainty information represents a difference between the bias prediction value and a true value of the angular velocity bias; An angular velocity bias updating module configured to update the bias prediction value by using the bias observation value, the bias prediction value, the weight of the bias observation value, and the weight of the bias prediction value; An angular velocity correction module configured to correct an angular velocity included in the motion data by using the updated bias prediction value.

10. A robot, characterized in that The device comprises a processor, a communication interface, a memory, and a communication bus, wherein the processor, the communication interface, and the memory complete mutual communication through the communication bus; The memory is configured to store a computer program; The processor is configured to execute the program stored on the memory to implement the method steps of any one of claims 1 to 8. The device comprises a processor, a communication interface, a memory, and a communication bus, wherein the processor, the communication interface, and the memory complete mutual communication through the communication bus; The memory is configured to store a computer program; The processor is configured to execute the program stored on the memory to implement the method steps of any one of claims 1 to 8.

Images