Positioning and navigation method for spreading robot based on multi-sensor fusion

Abstract

The embodiment of the invention discloses a leveling robot positioning and navigation method based on multi-sensor fusion, and the method comprises the steps: obtaining IMU data through error calibration, and the error calibration comprises gyroscope error calibration, accelerometer error calibration and magnetometer error calibration; based on the IMU data, a strapdown inertial navigation system is adopted for calculation, and navigation parameters are obtained; estimating and correcting the error of the strapdown inertial navigation system by using UWB data through a combined filter; the fault of the IMU and the fault data of the IMU can be corrected in time, and the error of the strapdown inertial navigation system is estimated and corrected by using the UWB data through the filter, so that the strapdown inertial navigation system keeps high-precision navigation capability.

Description

Technical Field

[0001] The present invention relates to the technical field of data fusion, and in particular, to a positioning and navigation method for a leveling robot based on multi-sensor fusion. Background Art

[0002] At present, mobile robots are widely used in military, industrial and agricultural production, medical care, education and other fields to replace people to complete many dangerous, repetitive and heavy tasks. However, in the field of grain storage, the advantages of robots have not been reflected. In the grain storage link, to ensure the quality of stored grain, it is necessary to level the stored grain. Moreover, there is a lot of dust and a poor environment in the current granary, and it is difficult for manual leveling. Most of the work in the current grain storage link is completed manually, with low work efficiency of workers and their health not guaranteed. Therefore, aiming at the specific environment of domestic granaries and the need for grain leveling, an autonomous navigation leveling robot is proposed to level the stored grain. Reliable positioning data is the basic requirement for the robot's navigation operation.

[0003] In an outdoor open environment, RTK (Real-Time Kinematic) can provide centimeter-level positioning data. Inside the granary belongs to a GNSS (Global Navigation Satellite System) denied environment, and how to achieve high-precision positioning is still a technical problem. In a GNSS denied environment, relying on inertial navigation for a long time will cause the accumulation of positioning errors. Summary of the Invention

[0004] The main purpose of the present invention is to provide a positioning and navigation method for a leveling robot based on multi-sensor fusion, which can correct errors in a timely manner and improve the accuracy of inertial navigation.

[0005] To achieve the above object, the first aspect of the present application provides a positioning and navigation method for a leveling robot based on multi-sensor fusion, and the method includes:

[0006] Obtain IMU data through error calibration, where the error calibration includes gyroscope error calibration, accelerometer error calibration, and magnetometer error calibration;

[0007] Based on the IMU data, perform calculations using a strapdown inertial navigation system to obtain navigation parameters;

[0008] Estimate and correct the errors of the strapdown inertial navigation system by using UWB data through a combined filter.

[0009] Optionally, the gyroscope error calibration includes:

[0010] Collect the three-axis angular velocities of the gyroscope and solve the single rotation angle through integration;

[0011] Take the mean of the single rotation angles obtained from multiple measurements, and combine with the error model of the gyroscope to calculate the zero bias error and scale factor error of the gyroscope respectively.

[0012] Optionally, the zero bias error of the gyroscope is calculated by the following formula:

[0013]

[0014] where is the mean of the angles integrated by the gyroscope when rotating one circle clockwise along an axis for ten times; is the mean of the angles integrated by the gyroscope when rotating one circle counterclockwise along an axis for ten times; t is the data acquisition time; ω e is the angular velocity of the Earth's rotation; is the local latitude;

[0015] The scale factor error of the gyroscope is calculated by the following formula:

[0016]

[0017] where d is the angle of a single rotation.

[0018] Optionally, the accelerometer error calibration includes:

[0019] Record the accelerometer data using the six-position method and record the mean values of the accelerometers in each direction;

[0020] Substitute the obtained acceleration values and the ideal acceleration data into the error model of the accelerometer to solve for the accelerometer error parameters.

[0021] Optionally, substituting the obtained acceleration values and the ideal acceleration data into the error model of the accelerometer to solve for the accelerometer error parameters includes:

[0022] Substitute the obtained acceleration values and the ideal acceleration data into the error model of the accelerometer, and use the least squares method to calculate the zero bias error, scale factor error, and installation angle error of the accelerometer;

[0023] The ideal acceleration data is determined based on the local gravitational acceleration.

[0024] Optionally, the magnetometer error calibration includes:

[0025] Rotate around the three coordinate axes of the magnetometer, rotate one circle around each single axis to collect magnetometer data;

[0026] Substitute the magnetometer data into the mathematical model of the three-axis magnetometer error to obtain the magnetometer error parameters.

[0027] Optionally, the magnetometer error parameter includes the magnetometer zero bias error.

[0028] The mathematical model of the triaxial magnetometer error is expressed as:

[0029] h = AH + H0

[0030] Where h is the actual output value of the triaxial magnetometer in the IMU, H is the local geomagnetic field intensity, and H0 is the magnetometer zero bias error.

[0031] A second aspect of the present application provides a positioning and navigation device for a bulkhead-flattening robot based on multi-sensor fusion, including:

[0032] An error calibration module for obtaining IMU data through error calibration, where the error calibration includes gyroscope error calibration, accelerometer error calibration, and magnetometer error calibration;

[0033] A strapdown inertial navigation module for performing calculations based on the IMU data using a strapdown inertial navigation system to obtain navigation parameters;

[0034] A combined correction module for estimating and correcting the errors of the strapdown inertial navigation system using UWB data through a combined filter.

[0035] A third aspect of the present application provides an electronic device, including a memory and a processor. When the computer program stored in the memory is executed by the processor, the processor is caused to execute the steps of the first aspect and any possible implementation manner thereof.

[0036] A fourth aspect of the present application provides a computer-readable storage medium storing a computer program. When the computer program is executed by a processor, the processor is caused to execute each step in the method described in the first aspect.

[0037] The present application provides a positioning and navigation method for a bulkhead-flattening robot based on multi-sensor fusion. IMU data is obtained through error calibration, where the error calibration includes gyroscope error calibration, accelerometer error calibration, and magnetometer error calibration; calculations are performed based on the IMU data using a strapdown inertial navigation system to obtain navigation parameters; the errors of the strapdown inertial navigation system are estimated and corrected using UWB data through a combined filter; faults of the IMU can be corrected in a timely manner and its faulty data can be corrected, and the errors of the strapdown inertial navigation system are estimated and corrected using UWB data through a filter, enabling the strapdown inertial navigation system to maintain a high-precision navigation ability. Description of the Drawings

[0038] To more clearly illustrate the technical solutions in the embodiments of the present application or the prior art, the following will briefly introduce the accompanying drawings required for the description of the embodiments or the prior art. Obviously, the accompanying drawings in the following description are only some embodiments of the present application. For those of ordinary skill in the art, without creative efforts, other accompanying drawings can also be obtained based on these drawings.

[0039] Wherein:

[0040] Figure 1 It is a schematic flowchart of a positioning and navigation method for a leveling robot based on multi-sensor fusion provided by an embodiment of the present application;

[0041] Figure 2 It is a schematic flowchart of another positioning and navigation method for a leveling robot based on multi-sensor fusion provided by an embodiment of the present application;

[0042] Figure 3 It is a schematic structural diagram of a positioning and navigation device for a leveling robot based on multi-sensor fusion provided by an embodiment of the present application;

[0043] Figure 4 It is a schematic structural diagram of an electronic device provided by an embodiment of the present application. Detailed implementation manners

[0044] To enable those skilled in the art to better understand the solutions of the present application, the following will clearly and completely describe the technical solutions in the embodiments of the present application in conjunction with the accompanying drawings in the embodiments of the present application. Obviously, the described embodiments are only some embodiments of the present application, rather than all embodiments. Based on the embodiments in the present application, all other embodiments obtained by those of ordinary skill in the art without creative efforts belong to the scope of protection of the present application.

[0045] The terms "first", "second", etc. in the specification and claims of the present application and the above accompanying drawings are used to distinguish different objects, rather than to describe a specific order. In addition, the terms "include" and "have" and any variations thereof are intended to cover non-exclusive inclusion. For example, a process, method, system, product, or device that includes a series of steps or units is not limited to the listed steps or units, but optionally further includes steps or units not listed, or optionally further includes other steps or units inherent to these processes, methods, products, or devices.

[0046] References to "embodiments" in this document mean that the specific features, structures, or characteristics described in connection with the embodiments can be included in at least one embodiment of this application. The phrase appears in various places in the specification and does not necessarily refer to the same embodiment each time, nor is it an independent or alternative embodiment mutually exclusive with other embodiments. Those skilled in the art will explicitly and implicitly understand that the embodiments described herein can be combined with other embodiments.

[0047] The RTK (Real-Time Kinematic) involved in the embodiments of this application is a high-precision differential GNSS (Global Navigation Satellite System) positioning technology based on carrier phase observations. Its main feature is the ability to provide centimeter-level positioning accuracy under real-time conditions and is widely used in multiple fields such as surveying and mapping, agriculture, construction, and transportation.

[0048] The GNSS (Global Navigation Satellite System) involved in the embodiments of this application is the abbreviation of the Global Navigation Satellite System, which includes multiple satellite navigation systems. GNSS provides all-weather, high-precision position, speed, and time information for various civil and military carriers on land, sea, air, and space globally.

[0049] The embodiments of this application will be described below in conjunction with the accompanying drawings in the embodiments of this application.

[0050] Please refer to Figure 1 , which is a schematic flow chart of a positioning and navigation method for a warehouse leveling robot based on multi-sensor fusion provided by the embodiments of this application. As Figure 1 shown, the method includes:

[0051] 101. Obtain IMU data through error calibration, and the above error calibration includes gyroscope error calibration, accelerometer error calibration, and magnetometer error calibration.

[0052] The method in the embodiments of this application can be executed on a positioning and navigation device for a warehouse leveling robot based on multi-sensor fusion, which can be an electronic device in practical applications, including terminal devices, and specifically can also be a robot. The method in the embodiments of this application can be used in application scenarios such as autonomous operation of warehouse leveling robots in granaries.

[0053] The embodiments of this application mainly aim to provide a UWB / IMU fusion positioning method, which mainly includes steps such as gyroscope error calibration, accelerometer error calibration, magnetometer error calibration, strapdown inertial navigation update, and UWB / IMU Kalman filter fusion positioning.

[0054] In the embodiments of the present application, the UWB (Ultra Wide Band) technology involved calculates the distance by measuring the transmission time of the signal, and has advantages such as high precision and strong anti-interference ability. The IMU (Inertial Measurement Unit) calculates the attitude and position information by measuring the acceleration and angular velocity, and has the characteristics of strong real-time performance and dynamics.

[0055] In an alternative embodiment, the above gyroscope error calibration includes:

[0056] Collect the angular velocities of the three axes of the gyroscope and solve the single rotation angle through integration;

[0057] Take the average value of the above single rotation angles obtained from multiple measurements, and combine with the error model of the above gyroscope to calculate the zero bias error and scale factor error of the above gyroscope respectively.

[0058] Specifically, for a low-cost IMU, the error will greatly affect the estimation of the speed, position, and attitude of the position-closing robot. The gyroscope mainly has zero bias error and scale factor error. In the embodiments of the present application, in order to calculate the zero bias and scale factor errors of the gyroscope, the angular velocities of the X, Y, and Z axes of the gyroscope can be collected, and the single rotation angle can be solved through integration. The measured rotation angles are averaged for multiple measurements, and combined with the error model of the gyroscope, the zero bias and scale factor errors of the gyroscope are calculated respectively.

[0059] Further alternatively, the zero bias error of the above gyroscope is calculated by the following formula:

[0060]

[0061] where, is the average value of the angles after the gyroscope is integrated for one full rotation in the clockwise direction for ten times for one axis; is the average value of the angles after the gyroscope is integrated for one full rotation in the counterclockwise direction for ten times for one axis; t is the data acquisition time; ω e is the angular velocity of the Earth's rotation; is the local latitude;

[0062] The scale factor error of the above gyroscope is calculated by the following formula:

[0063]

[0064] where d is the angle of a single rotation.

[0065] In an alternative embodiment, the above accelerometer error calibration includes:

[0066] Use the six-position method to record the accelerometer data and record the average value of the accelerometer in each direction;

[0067] Substitute the obtained acceleration values and the ideal acceleration data into the error model of the accelerometer to solve for the accelerometer error parameters.

[0068] In the embodiments of the present application, an IMU is a sensor used to measure and report a specific force, angular velocity of an object, and in some cases, the orientation of an object around a magnetic field. An accelerometer is a component of the IMU used to measure linear acceleration.

[0069] Specifically, the calibration process may include:

[0070] Six-position method: Place the X, Y, and Z axes of the IMU upward and downward for a period of time respectively, and record the accelerometer data. This can cover all possible error sources, including scale factor error, cross-axis sensitivity, and bias.

[0071] Record the mean value: For each direction, record the mean value of the accelerometer.

[0072] Substitute data into the formula: Substitute the actually measured acceleration values and the ideal acceleration values into the error model of the accelerometer, and solve for the parameters in the error model through the least squares method or other optimization methods.

[0073] Error compensation: In practical applications, these parameters can be used to compensate the output of the accelerometer to improve the measurement accuracy.

[0074] Optionally, the error model of the above accelerometer may include the following formula:

[0075]

[0076] Where:

[0077] A x 、A y 、A z are the acceleration values measured by the accelerometer.

[0078] a x 、a y 、a z are the acceleration values in the ideal case (for example, when the IMU is stationary, the ideal acceleration is the acceleration due to gravity, approximately 9.81 m / s 2 downward).

[0079] K1, K2, and K3 are scale factors used to describe the magnification multiples of the accelerometer on each axis.

[0080] W xy 、W xz 、W yx 、W yz 、W zx 、W zyis the cross-axis sensitivity coefficient, which is used to describe the influence of the acceleration on one axis on the measurement results of other axes.

[0081] B1 and B3 are bias terms, which are used to describe the output offset of the accelerometer when there is no external force.

[0082] Further optionally, substituting the obtained acceleration value and the ideal acceleration data into the error model of the accelerometer to solve the error parameters of the accelerometer, including:

[0083] Substitute the obtained acceleration value and the above ideal acceleration data into the above error model of the accelerometer, and use the least squares method to calculate the zero-bias error, scale factor error, and installation angle error of the above accelerometer;

[0084] The above ideal acceleration data is determined based on the local gravitational acceleration.

[0085] Specifically, combined with the local gravitational acceleration, the ideal acceleration at each position can be expressed as follows:

[0086]

[0087] The least squares method can be used to calculate the zero bias, scale factor error, and installation angle error of the accelerometer.

[0088]

[0089] Among them, x = [FI 3×3

[0090]

[0091] In an alternative embodiment, the above magnetometer error calibration includes:

[0092] Rotate around the three coordinate axes of the above magnetometer, rotate one circle around each single axis to collect magnetometer data;

[0093] Substitute the above magnetometer data into the three-axis magnetometer error mathematical model to obtain the magnetometer error parameters.

[0094] Further optionally, the above magnetometer error parameters include the magnetometer zero-bias error;

[0095] The above three-axis magnetometer error mathematical model is expressed as:

[0096] h = AH + H0

[0097] Among them, h is the actual output value of the three-axis magnetometer in the IMU, H is the local geomagnetic field intensity, and H0 is the above magnetometer zero-bias error.

[0098] ​Specifically, there are errors in the triaxial magnetometers of the IMU, including inconsistent triaxial measurement sensitivities, non-orthogonal triaxial errors, and zero-offset errors. Therefore, the mathematical model of the triaxial magnetometer errors is constructed as follows.

[0099] h = AH + H0

[0100] Wherein, is the actual output value of the triaxial magnetometer in the IMU;

[0101] is the local true geomagnetic field intensity; H0 is the zero-offset error of the magnetometer.

[0102] In the embodiments of the present application, a magnetometer error correction method based on single-axis rotation is adopted. The magnetometer is rotated around the three coordinate axes of the magnetic sensor, and one full rotation is made around each single axis. The magnetometer data is collected and substituted into the above formula for solution.

[0103] Specifically, for an ideal triaxial magnetometer sensor without an interfering magnetic field, when the triaxial magnetometer is rotated arbitrarily in space, the spatial position points formed by the projection components of the geomagnetic field on the three axes of the triaxial magnetometer are located on the spherical surface with the origin as the center of the sphere and the radius being the local geomagnetic field intensity. At this time, the measured value of the triaxial magnetometer is equal to the local magnetic field intensity and is:

[0104]

[0105] By transposing and transforming, we can obtain:

[0106] H = A -1 (h - H0) (2)

[0107] Then, combining with Equation (1), we get:

[0108]

[0109] It can be seen from Equation (0) that when there are measurement errors and an interfering magnetic field in the triaxial magnetometer, the spatial points formed by the magnetometer output values are located on the ellipsoidal surface. If the values of N and H0 in Equation (0) are obtained, the errors of the triaxial magnetometer can be corrected. In this design, the coefficients of the ellipsoidal equation are obtained by ellipsoidal fitting using the least squares method, and then the values of N and H0 are obtained.

[0110] The general equation of the ellipsoid is expressed as:

[0111] a1x 2 + a2y 2 + a3z 2 + 2a4xy + 2a5xz + 2a6yz + 2a7x + 2a8y + 2a9z + a 10 = 0 (4)

[0112] At the same time, it is defined that:

[0113] F = (a1 a2 a3 a4 a5 a6 a7 a8 a9 a 10 ) T (5)

[0114]

[0115] where F is a 10×1 matrix, X is an n×10 matrix, and n is the number of measurement data. Substituting (5) and (6) into (4) gives:

[0116] XF = 0 (7)

[0117] Rotate one full circle around each of the three axes of the triaxial magnetometer in space to obtain magnetometer measurement data, substitute it into equation (7), and obtain the ellipsoid parameters a1, a2, a3, a4, a5, a6, a7, a8, a9, a10 by the least squares method. Then transform (4) into the matrix representation form:

[0118] h T Bh+(2C)h + D = 0 (8)

[0119] where the matrix matrix C = (a7a8a9), and the constant D = a 10 ;

[0120] Further transforming the above equation gives:

[0121]

[0122] Combining (0) and (9) simultaneously gives:

[0123] H0 = -B -1 C (10)

[0124]

[0125] Calculating H0 and N from the above equation, and since

[0126] N = (A -1 ) T A -1 (2)

[0127] Obtaining A from the above equation -1 , so substituting A -1Substituting the values of and H0 into Equation (2) enables the calibration of the errors of the three-axis magnetometer. Before using the magnetometer each time, the three-axis magnetometer is rotated uniaxially in space, and the magnetometer data collected is substituted into (7). The ellipsoid parameters are obtained by the least squares method, and the obtained ellipsoid parameters are substituted into Equations (10), (1), and (2) to obtain A -1 and the values of H0 are substituted into (2) to calibrate the subsequently collected data.

[0128] 102. Based on the above IMU data, a strapdown inertial navigation system is used for solution to obtain navigation parameters.

[0129] The strapdown inertial navigation system uses a "mathematical platform" calculated by a computer to replace the physical inertial platform. The gyroscopes of the strapdown inertial navigation system measure the angular velocity of the carrier relative to the inertial space, obtain the angular velocity of the carrier coordinate system relative to the navigation coordinate system through coordinate transformation, obtain the attitude matrix through computer solution, and obtain the attitude angles of the carrier from the attitude matrix. Next, the specific force measured by the accelerometer is transformed into the navigation coordinate system through the attitude matrix, and the computer further performs navigation solution to obtain navigation parameters such as velocity and position.

[0130] Specifically, the above strapdown inertial navigation system is introduced as follows:

[0131] 1. Input

[0132] Measurement data of gyroscopes and accelerometers: The gyroscopes measure the angular velocity of the carrier and output angular increments; the accelerometers measure the linear acceleration of the carrier and output velocity increments. These data are the basis for the system to perform navigation solution.

[0133] Initial alignment information: Includes initial attitude, position, velocity, etc. information, which is used for the system to perform initial alignment and calibration at startup.

[0134] Earth parameters: Such as the angular velocity of the Earth's rotation, gravitational acceleration, etc. These parameters are used to calculate Earth-related physical quantities, such as the radius of curvature of the Earth, Coriolis force, etc.

[0135] 2. Output

[0136] Position information: Includes longitude, latitude, altitude, etc., which is used to determine the specific position of the carrier on the Earth.

[0137] Velocity information: Outputs the velocity components of the carrier in three directions in the navigation coordinate system, helping to understand the motion state of the carrier.

[0138] Attitude information: Includes pitch angle, roll angle, heading angle, etc., which reflects the attitude and direction of the carrier and is crucial for the motion control and attitude adjustment of the carrier.

[0139] In a strapdown inertial navigation system, gyroscopes and accelerometers have device errors such as scale factor errors and zero biases, which will affect the navigation accuracy. Through error calibration, these error parameters can be accurately estimated, thereby improving the navigation accuracy. In addition, in a strapdown inertial navigation system, errors accumulate over time, leading to a decrease in navigation accuracy. Through error calibration and compensation, error accumulation can be effectively reduced, and the long-term stability of the system can be improved.

[0140] During the operation of the strapdown inertial navigation system, the calculated error parameters are used in real time to compensate the outputs of the gyroscopes and accelerometers. This can be achieved through software algorithms to ensure that each measured value is corrected, improving the measurement accuracy.

[0141] 103. Estimate and correct the errors of the above strapdown inertial navigation system using UWB data through a combination filter.

[0142] In the embodiments of this application, a Kalman filter can be used to fuse the outputs of the gyroscopes and accelerometers, which can further improve the navigation accuracy. The Kalman filter can provide an optimal state estimate considering measurement noise and system errors.

[0143] Figure 2 It is a schematic flow diagram of another positioning and navigation method for a bulkhead-flattening robot based on multi-sensor fusion provided by the embodiments of this application.

[0144] Specifically, in this integrated navigation method, the measurement data of the position and velocity of both are input into the combination filter through an interface. The role of the combination is reflected in estimating and correcting the errors of the strapdown inertial navigation system using UWB data through the filter, enabling the strapdown inertial navigation system to maintain a high-precision navigation ability. In the embodiments of this application, a linear Kalman filter can be used to implement the combination of UWB and IMU navigation systems.

[0145] In one implementation, the Kalman filtering algorithm is used to fuse UWB and IMU data to improve the positioning accuracy. The specific steps are as follows:

[0146] Initialize the state and covariance matrix of the Kalman filter.

[0147] At each time step, update the state of the Kalman filter using UWB and IMU data.

[0148] Through the output of the Kalman filter, more accurate position, velocity, and attitude information can be obtained.

[0149] The errors of common low-cost IMUs greatly affect the estimation accuracy of speed, position, and attitude. The systematic errors of the IMU account for more than 90% of the total errors, mainly including bias errors, scale factor errors, installation angle errors, etc. In the laboratory, IMU calibration usually relies on equipment such as high-precision turntables to provide azimuth and horizontal references, combined with the angular velocity of the Earth's rotation and the local gravity value. These devices are not only expensive but also require professional personnel for operation and maintenance.

[0150] The embodiments of the present application mainly propose an error calibration method for triaxial accelerometers, triaxial angular velocities, and triaxial magnetometers, which can timely correct the faults of the IMU and correct its fault data, and is of great significance for improving system robustness, measurement accuracy, etc. This method is data-driven, reducing the dependence on errors and measurement principles. This method does not rely on redundant sensors and devices and can operate in the case of a single IMU, being applicable to intelligent devices with resource advantages.

[0151] Based on the description of the foregoing method embodiments, in an embodiment of the present application, a positioning and navigation device for a leveling robot based on multi-sensor fusion is also proposed. Please refer to Figure 3 , Figure 3 which is a schematic structural diagram of a positioning and navigation device for a leveling robot based on multi-sensor fusion provided by the embodiments of the present application. As Figure 3 shown, the positioning and navigation device 300 for a leveling robot based on multi-sensor fusion includes:

[0152] An error calibration module 310, configured to obtain IMU data through error calibration, where the error calibration includes gyroscope error calibration, accelerometer error calibration, and magnetometer error calibration;

[0153] A strap-down inertial navigation module 320, configured to perform calculations using a strap-down inertial navigation system based on the IMU data to obtain navigation parameters;

[0154] A combined correction module 330, configured to estimate and correct the errors of the strap-down inertial navigation system using UWB data through a combined filter.

[0155] Among them, Figure 1 or Figure 2 The method steps in the shown embodiments can be executed in the above-mentioned positioning and navigation device 300 for a leveling robot based on multi-sensor fusion, which will not be elaborated here.

[0156] Based on the description of the foregoing method embodiments, in an embodiment of the present application, an electronic device is also proposed. Please refer to Figure 4 , Figure 4 which is a schematic structural diagram of an electronic device provided by the embodiments of the present application. As Figure 4As shown, the electronic device 400 includes a processor 401 and a memory 402. The memory 402 stores a computer program. When the computer program is executed by the processor 401, it will execute any of the steps in the method embodiments as shown in Figure 1 or Figure 2 . The electronic device 400 may further include an input / output device and the like. In a specific implementation, the electronic device may be a terminal device or the like.

[0157] In one embodiment, a computer-readable storage medium is also proposed. The computer-readable storage medium stores a computer program. When the computer program is executed by the processor 401, it causes the processor 401 to execute any of the steps in the above method embodiments.

[0158] Those of ordinary skill in the art can understand that all or part of the processes of implementing the above method embodiments can be completed by instructing relevant hardware through a computer program. The program can be stored in a non-volatile computer-readable storage medium. When the program is executed, it may include the processes of the above method embodiments. Among them, any reference to a memory, storage, database, or other medium used in the various embodiments provided in the present application may include non-volatile and / or volatile memories. Non-volatile memories may include read-only memory (ROM), programmable ROM (PROM), electrically programmable ROM (EPROM), electrically erasable programmable ROM (EEPROM), or flash memory. Volatile memories may include random access memory (RAM) or external cache memory. By way of illustration and not limitation, RAM is available in various forms, such as static RAM (SRAM), dynamic RAM (DRAM), synchronous DRAM (SDRAM), double data rate SDRAM (DDR SDRAM), enhanced SDRAM (ESDRAM), synchronous link DRAM (SLDRAM), Rambus direct RAM (RDRAM), direct memory bus dynamic RAM (DRDRAM), and Rambus dynamic RAM (RDRAM), etc.

[0159] The technical features of the above embodiments can be combined arbitrarily. For the sake of brevity of description, not all possible combinations of the technical features in the above embodiments are described. However, as long as there is no contradiction in the combination of these technical features, it should be considered to be within the scope described in this specification.

[0160] The above-described embodiments merely represent several implementation manners of the present application. The description thereof is relatively specific and detailed, but it should not be construed as a limitation to the scope of the patent of the present application. It should be noted that for those of ordinary skill in the art, without departing from the concept of the present application, several modifications and improvements can still be made, and these all fall within the protection scope of the present application. Therefore, the protection scope of the patent of the present application shall be subject to the appended claims.

Claims

1. A positioning and navigation method for a closing robot based on multi-sensor fusion, characterized in that, The method includes: Obtaining IMU data through error calibration, where the error calibration includes gyroscope error calibration, accelerometer error calibration, and magnetometer error calibration; Based on the IMU data, using a strap-down inertial navigation system for calculation to obtain navigation parameters; Using UWB data to estimate and correct the errors of the strap-down inertial navigation system through a combined filter.

2. The positioning and navigation method of the leveling robot based on multi-sensor fusion according to claim 1, characterized in that The gyroscope error calibration includes: Collecting the three-axis angular velocity of the gyroscope and solving for the single rotation angle through integration; Taking the mean of the single rotation angles obtained from multiple measurements and combining with the error model of the gyroscope to calculate and obtain the zero-bias error and scale factor error of the gyroscope respectively.

3. The positioning and navigation method of the leveling robot based on multi-sensor fusion according to claim 2, wherein, The zero-bias error of the gyroscope is calculated and obtained through the following formula: Among them, is the average value of the angles obtained by integrating the gyroscope for ten times when it rotates clockwise one full circle around an axis; is the average value of the angles obtained by integrating the gyroscope for ten times when it rotates counterclockwise one full circle around an axis; t is the data acquisition time; ω e is the angular velocity of the Earth's rotation; is the local latitude; The scale factor error of the gyroscope is calculated and obtained through the following formula: where d is the angle of a single rotation.

4. The positioning and navigation method of the warehouse-trimming robot based on multi-sensor fusion according to claim 1, wherein, The accelerometer error calibration includes: Using the six-position method to record accelerometer data and record the mean value of the accelerometer in each direction; Substituting the obtained acceleration values and ideal acceleration data into the error model of the accelerometer to solve for the accelerometer error parameters.

5. The positioning and navigation method of the flatting robot based on multi-sensor fusion according to claim 4, characterized in that, The substituting the obtained acceleration values and ideal acceleration data into the error model of the accelerometer to solve for the accelerometer error parameters includes: Substituting the obtained acceleration values and the ideal acceleration data into the error model of the accelerometer and using the least squares method to calculate and obtain the zero-bias error, scale factor error, and installation angle error of the accelerometer; The ideal acceleration data is determined based on the local gravitational acceleration.

6. The positioning and navigation method of the leveling robot based on multi-sensor fusion according to claim 1, characterized in that, The magnetometer error calibration includes: Rotating around the three coordinate axes of the magnetometer, rotating one circle around each single axis to collect magnetometer data; Substituting the magnetometer data into the three-axis magnetometer error mathematical model to obtain the magnetometer error parameters.

7. The positioning and navigation method of the leveling robot based on multi-sensor fusion according to claim 6, characterized in that, The magnetometer error parameters include the magnetometer zero-bias error; The three-axis magnetometer error mathematical model is expressed as: h = AH + H0 where h is the actual output value of the three-axis magnetometer in the IMU, H is the local geomagnetic field intensity, and H0 is the magnetometer zero-bias error.

8. A positioning and navigation device for a closing robot based on multi-sensor fusion, characterized in that, It includes: An error calibration module for obtaining IMU data through error calibration, where the error calibration includes gyroscope error calibration, accelerometer error calibration, and magnetometer error calibration; A strap-down inertial navigation module for calculating based on the IMU data using a strap-down inertial navigation system to obtain navigation parameters; A combined correction module for estimating and correcting the errors of the strap-down inertial navigation system using UWB data through a combined filter.

9. An electronic device, characterized in that, It includes a memory and a processor. When the computer program stored in the memory is executed by the processor, the processor executes the steps of the method according to any one of claims 1-7.

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

Images