An adaptive fusion correction method for wheeled robot odometer

Abstract

The present invention discloses an adaptive fusion correction method for a wheeled robot's odometer. This method first reads measurement information from the wheeled robot's inertial navigation unit (IMU) and the wheeled robot's odometer. The method then uses the IMU's measurement information as a model prediction value and the odometer's measurement information as an observation value. The odometer data is corrected using an adaptive Kalman fusion algorithm. The corrected odometer data is used to calculate the wheeled robot's travel distance and compared with the laser radar data to calculate the data's accuracy. The present invention fully explores the changing relationship between the predicted and observed values ​​during the fusion process. Using the adaptive Kalman fusion algorithm to correct the odometer data, the method can obtain accurate position, velocity, and acceleration information for the wheeled robot.

Description

Technical Field

[0001] The present invention relates to the field of sensor data processing, and in particular to an adaptive Kalman fusion correction method for a wheeled robot odometer combined with an inertial navigation unit (IMU). Background Art

[0002] Odometers are widely used as measurement sensors in wheeled robots. However, due to factors such as installation and operating environment, odometer measurement data contains certain errors. This error accumulates over time, resulting in large deviations in the calculated distance traveled by the wheeled robot. To address this issue, most current solutions fuse the odometer with absolute measurement sensors such as GPS. However, due to the large errors in civilian GPS data, the fused data may be unusable. Kalman fusion methods based on inertial navigation units (IMUs) are also common, but most of them calculate the Kalman gain based on a fixed covariance matrix. The fusion effect varies with the calibration of the sensor noise covariance. Summary of the Invention

[0003] In order to solve the technical problems existing in the above-mentioned prior art, the present invention provides a wheeled robot odometer data adaptive fusion correction method, which combines the inertial navigation IMU information and uses an adaptive Kalman fusion algorithm to correct the wheeled robot odometer data to obtain the accurate position, velocity and acceleration information of the wheeled robot.

[0004] In order to solve the above technical problems, the technical solution adopted by the present invention is: a wheeled robot odometer adaptive fusion correction method, comprising the following steps:

[0005] S01) Read the measurement information of the wheeled robot inertial navigation IMU;

[0006] S02), reading the measurement information of the wheeled robot odometer;

[0007] S03) Using the IMU measurement information as the predicted value and the odometer measurement information as the observed value, the odometer data is corrected using the adaptive Kalman fusion algorithm;

[0008] Step S03) is specifically: using the measurement data imu.v of the inertial navigation IMU as the predicted value of the adaptive Kalman fusion algorithm, that is:

[0009]

[0010]

[0011] Where kalman.state is the state matrix of the Kalman fusion algorithm, transform.matrix is ​​the state transfer matrix, imu.x and imu.y are the x-axis position information and y-axis position information of the IMU respectively, imu.vx and imu.vy are the x-axis velocity information and Y-axis velocity information of the IMU respectively, and imu.accx and imu.accy are the x-axis acceleration information and y-axis acceleration information of the IMU respectively;

[0012] The state covariance matrix state.uncertainty of the inertial navigation IMU is:

[0013] state.uncertainty

[0014] =transform.matrix·state.uncertainty·transform.matrix′

[0015] +pricdit.noise,

[0016] Where pricdit.noise is the predicted noise, which is determined by the relationship between the inertial navigation IMU and the true value.

[0017] transform.matrix′ is the inverse matrix of the state transfer matrix;

[0018] The covariance matrix odom.uncertainty of the odometer sensor is:

[0019] odom.uncertainty=

[0020] transform.matrix·odom.uncertainty·transform.matrix′

[0021] +odom.noise,

[0022] odom.noise is the odometer noise, which is determined according to the characteristics of the odometer sensor;

[0023] The fusion covariance matrix novel.uncertainty is:

[0024] novel.uncertainty=state.uncertainty+odom.uncertainty

[0025] The kalman gain kalman.gain of the adaptive kalman fusion algorithm is:

[0026] kalman.gain=state.uncertainty·novel.uncertainty′,

[0027] Where novel.uncertainty′ is the inverse matrix of the fusion covariance matrix;

[0028] The observation matrix kalman.observation of the adaptive Kalman fusion algorithm is set to the data of the inertial odometry, that is:

[0029]

[0030] odom.x and odom.y are the x-axis position information and y-axis position information of the odometer respectively, odom.vx and odom.vy are the x-axis speed information and y-axis speed information of the odometer respectively, and odom.accx and odom.accy are the x-axis acceleration information and y-axis acceleration information of the odometer respectively;

[0031] The fusion optimal value kalman.optimstate is:

[0032] kalman.optimstate

[0033] =kalman.state+kalman.gain·[kalman.observation-kalman.state] Finally, the posterior covariance state.uncertainty_ of the Kalman fusion algorithm is updated as follows:

[0034] state.uncertainty_=(I-kalman.gain)state.uncertainty, where I is the identity matrix.

[0035] Further, it also includes step S04);

[0036] S04) Calculate the travel distance of the wheeled robot based on the corrected odometer data, and compare it with the laser radar data to calculate the accuracy of the data.

[0037] Furthermore, step S04) is specifically as follows:

[0038] Read the wheeled robot's travel distance data lidar.d calculated by the lidar, calculate the optimal value fused from the odometry, and iteratively calculate the wheeled robot's travel distance:

[0039] in

[0040] kalman.optimstate[0] is the x-axis position information of the fused optimal value position, and kalman.optimstate[1] is the y-axis position information of the fused optimal value position. The calculated correction accuracy is:

[0041]

[0042] When accuracy ≥ 0.95, it indicates that the correction is effective.

[0043] Furthermore, the measurement information of the wheeled robot IMU read in step S01 includes the speed imu.v and acceleration imu.a of the wheeled robot, and then a threshold judgment is performed on the inertial navigation IMU data, namely:

[0044] If imu.v≥MaxVelocity, then imu.v=MaxVelocity; if

[0045] imu.v≤MinVelocity, then imu.v=MinVelocity, if

[0046] If imu.a≥MaxAcc, then imu.a=MaxAcc; if imu.a≤MinAcc, then imu.a=MinAcc, where MaxVelocity and MaxAcc are the maximum speed and maximum acceleration that the wheeled robot can travel, respectively.

[0047] Furthermore, the odometer measurement information read in step S01) includes the rotation speed of the left wheel of the wheeled robot odom.LeftSpeed ​​and the rotation speed of the right wheel odom.RightSpeed. The speed of the wheeled robot in the coordinate system is calculated based on the rotation speed measured by the odometer:

[0048] odom.v = (2π·odom.LeftSpeed·wheel.r + 2π·odom.RightSpeed·wheel.r) / 2, where odom.v is the actual speed of the wheeled robot, wheel.r is the radius of the wheeled robot's wheel, and the time interval dt is dt = current.time - last.time, where current.time is the current time and last.time is the last sampling time. Therefore, the acceleration of the wheeled robot obtained from the data measured by the odometer is odom.acc = odom.v / dt.

[0049] The beneficial effects of the present invention are as follows: the present invention combines the inertial navigation IMU measurement information and adopts the adaptive Kalman fusion algorithm to correct the wheeled robot odometer data, which can more accurately integrate the changing relationship between the predicted value and the observed value during the fusion process and obtain the accurate position, speed and acceleration information of the wheeled robot. DETAILED DESCRIPTION

[0050] The present invention will be further described below with reference to specific embodiments.

[0051] It should be noted that the following detailed descriptions are illustrative and are intended to provide further explanation of the present application.

[0052] Unless defined otherwise, all technical and scientific terms used herein have the same meaning as commonly understood by one of ordinary skill in the art to which this application belongs.

[0053] It should be noted that the terms used herein are only for describing specific embodiments and are not intended to limit the exemplary embodiments according to the present application. As used herein, unless the context clearly indicates otherwise, the singular form is also intended to include the plural form. In addition, it should be understood that when the terms "comprise" and / or "include" are used in this specification, they indicate the presence of features, steps, operations, devices, components and / or combinations thereof.

[0054] Example 1

[0055] This embodiment discloses a method for adaptive fusion correction of a wheeled robot odometer, comprising the following steps:

[0056] Step 1: Read the measurement information of the wheeled robot's inertial navigation unit (IMU);

[0057] Step 2: Read the measurement information of the wheeled robot odometer;

[0058] Step 3: Use the IMU measurement information as the predicted value and the odometer measurement information as the observed value, and use the adaptive Kalman fusion algorithm to correct the odometer data;

[0059] Step 4: Calculate the distance traveled by the wheeled robot based on the corrected odometer data, compare it with the lidar data, and calculate the accuracy of the data.

[0060] In this embodiment, the measurement information of the wheeled robot inertial navigation IMU read in step 1 includes the velocity imu.v and acceleration imu.a of the wheeled robot, and then a threshold judgment is performed on the inertial navigation IMU data, that is: if imu.v≥MaxVelocity, then imu.v=MaxVelocity; if imu.v≤MinVelocity, then imu.v=MinVelocity; if imu.a≥MaxAcc, then imu.a=MaxAcc; if imu.a≤MinAcc, then imu.a=MinAcc, where MaxVelocity and MaxAcc are the maximum speed and maximum acceleration that the wheeled robot can travel, respectively.

[0061] In this embodiment, the odometer measurement information read in step 2 includes the rotation speed of the left wheel of the wheeled robot, odom.LeftSpeed, and the rotation speed of the right wheel, odom.RightSpeed. The speed of the wheeled robot in the coordinate system is calculated based on the rotation speed measured by the odometer:

[0062]

[0063] Where odom.v is the actual speed of the wheeled robot, and wheel.r is the radius of the wheeled robot's wheel.

[0064] The time interval dt is dt = current.time - last.time, where current.time is the current time and last.time is the last sampling time. Therefore, the acceleration of the wheeled robot measured by the odometer data is odom.acc = odom.v / dt.

[0065] In this embodiment, step 3 is specifically: using the measurement data imu.v of the inertial navigation IMU as the predicted value of the adaptive Kalman fusion algorithm, that is:

[0066]

[0067]

[0068] Among them, kalman.state is the state matrix of the Kalman fusion algorithm, transform.matrix is ​​the state transfer matrix, imu.x and imu.y are the x-axis position information and y-axis position information of the IMU respectively, imu.vx and imu.vy are the x-axis velocity information and Y-axis velocity information of the IMU respectively, and imu.accx and imu.accy are the x-axis acceleration information and y-axis acceleration information of the IMU respectively.

[0069] The state covariance matrix state.uncertainty of the inertial navigation IMU is:

[0070] state.uncertainty

[0071] =transform.matrix·state.uncertainty·transform.matrix′

[0072] +pricdit.noise,

[0073] Where pricdit.noise is the predicted noise, which is determined by the relationship between the inertial navigation IMU and the true value, and transform.matrix′ is the inverse matrix of the state transfer matrix.

[0074] The covariance matrix odom.uncertainty of the odometer sensor is:

[0075] odom.uncertainty=

[0076] transform.matrix·odom.uncertainty·transform.matrix′

[0077] +odom.noise,

[0078] odom.noise is the odometer noise, which is determined according to the characteristics of the odometer sensor.

[0079] This embodiment proposes an innovative method for calculating the covariance matrix novel.uncertainty, that is, setting a fusion covariance matrix, and the fusion covariance matrix novel.uncertainty is: novel.uncertainty = state.uncertainty + odom.uncertainty

[0080] The Kalman gain kalman.gain of the adaptive Kalman fusion algorithm is: kalman.gain = state.uncertainty·novel.uncertainty′

[0081] Where novel.uncertainty′ is the inverse matrix of the fusion covariance matrix.

[0082] The observation matrix kalman.observation of the adaptive Kalman fusion algorithm is set to the data of the inertial odometry, that is:

[0083]

[0084] Among them, odom.x and odom.y are the x-axis position information and y-axis position information of the odometer respectively, odom.vx and odom.vy are the x-axis speed information and y-axis speed information of the odometer respectively, and odom.accx and odom.accy are the x-axis acceleration information and y-axis acceleration information of the odometer respectively.

[0085] The fusion optimal value kalman.optimstate is:

[0086] kalman.optimstate

[0087] =kalman.state+kalman.gain·[kalman.observation-kalman.state], the fused optimal value is the corrected wheeled robot odometer data.

[0088] Finally, the posterior covariance state.uncertainty_ of the Kalman fusion algorithm is updated as follows:

[0089] state.uncertainty_=(I-kalman.gain)state.uncertainty, where I is the identity matrix, which facilitates the correction of the wheeled robot's odometer data for the next shot.

[0090] In this embodiment, step 4 specifically includes: reading the wheeled robot's travel distance data lidar.d calculated by the lidar, calculating the optimal value fused from the odometer, and iteratively calculating the wheeled robot's travel distance:

[0091]

[0092] Among them, kalman.optimstate[0] is the x-axis position information of the fused optimal value position, and kalman.optimstate[1] is the y-axis position information of the fused optimal value position. The calculated correction accuracy is:

[0093]

[0094] When accuracy ≥ 0.95, it indicates the effectiveness of the correction.

[0095] This embodiment more accurately integrates the changing relationship between the predicted value and the observed value during the fusion process, and combines the inertial navigation IMU adaptive Kalman fusion correction wheeled robot odometer to obtain the accurate position, speed, and acceleration information of the wheeled robot.

[0096] Although the above describes the specific implementation methods of the present invention, it does not limit the scope of protection of the invention. Those skilled in the art should understand that various modifications or variations that can be made by those skilled in the art on the basis of the technical solution of the present invention without creative work are still within the scope of protection of the present invention.

Claims

1. A wheeled robot odometer adaptive fusion correction method, characterized by: The following steps are involved: S01) Read the measurement information of the wheeled robot inertial navigation IMU; S02), reading the measurement information of the wheeled robot odometer; S03) Using the IMU measurement information as the predicted value and the odometer measurement information as the observed value, the odometer data is corrected using the adaptive Kalman fusion algorithm; Step S03) is specifically: using the measurement data of the inertial navigation IMU as the predicted value of the adaptive Kalman fusion algorithm, that is: Where kalman.state is the state matrix of the Kalman fusion algorithm, transform.matrix is ​​the state transfer matrix, imu.x and imu.y are the x-axis position information and y-axis position information of the IMU respectively, imu.vx and imu.vy are the x-axis velocity information and Y-axis velocity information of the IMU respectively, and imu.accx and imu.accy are the x-axis acceleration information and y-axis acceleration information of the IMU respectively; Calculate the state covariance matrix state.uncertainty of the inertial navigation IMU as: Where pricdit.noise is the predicted noise, which can be determined based on the relationship between the inertial navigation IMU and the actual measurement value, and transform.matrix′ is the inverse matrix of the state transfer matrix; Calculate the covariance matrix odom.uncertainty of the odometer sensor as: odom.noise is the odometer noise covariance, which is determined according to the characteristics of the odometer sensor; We propose a new method to calculate the fusion covariance matrix novel.uncertainty: novel.uncertainty=state.uncertainty+odom.uncertainty, According to novel.uncertainty, the Kalman gain kalman.gain of the adaptive Kalman fusion algorithm is calculated as: kalman.gain=state.uncertainty·novel.uncertainty′, Where novel.uncertainty′ is the inverse matrix of the fusion covariance matrix; The observation matrix kalman.observation of the adaptive Kalman fusion algorithm is set to the data of the inertial odometry, that is: odom.x and odom.y are the x-axis position information and y-axis position information of the odometer respectively, odom.vx and odom.vy are the x-axis speed information and y-axis speed information of the odometer respectively, and odom.accx and odom.accy are the x-axis acceleration information and y-axis acceleration information of the odometer respectively; The fusion optimal value kalman.optimstate is: kalman.optimstate =kalman.state+kalman.gain·[kalman.observation-kalman.state] Finally, the posterior covariance state.uncertainty_ of the Kalman fusion algorithm is updated as follows: state.uncertainty_=(I-kalman.gain)state.uncertainty, where I is the identity matrix.

2. The wheeled robot odometer adaptive fusion correction method according to claim 1, characterized in that: Also includes step S04); S04) Calculate the travel distance of the wheeled robot based on the corrected odometer data, and compare it with the laser radar data to calculate the accuracy of the data.

3. The wheeled robot odometer adaptive fusion correction method according to claim 2, characterized in that: Step S04) is specifically as follows: Read the wheeled robot's travel distance data lidar.d calculated by the lidar, calculate the optimal value fused from the odometry, and iteratively calculate the wheeled robot's travel distance: Among them, kalman.optimstate[0] is the x-axis position information of the fused optimal value position, and kalman.optimstate[1] is the y-axis position information of the fused optimal value position. The calculated correction accuracy is: When accuracy ≥ 0.95, it indicates that the correction is effective.

4. The wheeled robot odometer adaptive fusion correction method according to claim 1, characterized in that: Step S01) The measurement information of the wheeled robot guide IMU read includes the speed imu.v and acceleration imu.a of the wheeled robot, and then the threshold judgment is performed on the inertial navigation IMU data, that is: if imu.v≥MaxVelocity, then imu.v=MaxVelocity; if imu.v≤MinVelocity, then imu.v=MinVelocity, if imu.a≥MaxAcc, then imu.a=MaxAcc; if imu.a≤MinAcc, then imu.a=MinAcc, where MaxVelocity and MaxAcc are the maximum speed and maximum acceleration that the wheeled robot can travel, respectively.

5. The wheeled robot odometer adaptive fusion correction method according to claim 1, characterized in that: Step S01) reads the odometer measurement information, including the rotation speed of the left wheel of the wheeled robot, odom.LeftSpeed, The speed of the right wheel is odom.RightSpeed. The speed of the wheeled robot in the coordinate system is calculated based on the speed measured by the odometer. odom.v=(2π·odom.LeftSpeed·wheel.r+2π·odom.RightSpeed·wheel.r) / 2, where odom.v is the actual speed of the wheeled robot, wheel.r is the radius of the wheeled robot wheel, The time interval dt is dt = current.time - last.time, where current.time is the current time. last.time is the last sampling time. Therefore, the acceleration of the wheeled robot obtained from the data measured by the odometer is odom.acc = odom.v / dt.

Images