Robot pose estimation method based on kalman filter

Abstract

The application discloses a robot pose estimation method based on Kalman filtering, which comprises the following steps: A1, based on the running result of a pre-set correlation scan matching algorithm, a robot acquires current pose observation data and a noise covariance matrix thereof; then, data correlation processing is performed on the current pose observation data to obtain historical pose observation data; A2, current pose prediction data is calculated based on a kinematics equation; and a prior covariance matrix of the current pose prediction data is calculated based on a state equation; A3, a noise covariance matrix of the current pose observation data and the prior covariance matrix of the current pose prediction data are used to calculate a Kalman gain matrix based on a delayed out-of-order observation equation; A4, the current pose prediction data is corrected according to the residual error between the current pose observation data and the historical pose observation data, and the corrected current pose prediction data is set as the current estimated pose prediction data; and the accuracy of robot pose prediction is improved.

Description

Technical Field

[0001] This application relates to the field of robot control algorithms, specifically a robot pose prediction method based on Kalman filtering. Background Technology

[0002] In the field of Simultaneous Localization and Mapping (SLAM) for low-cost wheeled robots, the Correlative Scan Matching (CSM) algorithm is a scanning matching algorithm based on LiDAR. Specifically, it combines a search window to perform scanning matching and obtain the pose with the highest probability of being occupied by the LiDAR point cloud in the grid map as the final pose. However, the accuracy of this algorithm is heavily dependent on the confidence of the initial search pose.

[0003] Existing technologies can also select the initial search pose as prior information and use the extended Kalman filter (EKF) framework to fuse the sensing data of the inertial measurement unit (IMU) and the sensing data of the rotary encoder to process the robot's final optimized pose.

[0004] The robot's Correlative Scan Matching (CSM) algorithm suffers from problems such as untimely processing of laser data and information delays in the output pose data. For example, the window search and matching operations performed by the CSM algorithm are delayed, resulting in delayed and out-of-order received measurement data. This causes measurements from earlier moments of the same target to arrive at the processing center later than the current measurements, a situation known as the delayed and disordered measurement problem. When delayed measurement data arrives, directly using the delayed measurement data will cause the sensing error of the robot's inertial navigation sensor to increase rapidly. Summary of the Invention

[0005] This application discloses a robot pose prediction method based on Kalman filtering, and the specific technical solution is as follows:

[0006] The robot pose prediction method includes: Step A1, based on the running results of a pre-set correlation scanning matching algorithm, the robot acquires the current pose observation data and its noise covariance matrix; then, the current pose observation data is processed by data association to obtain historical pose observation data; wherein, the original data collected to obtain historical pose observation data using the correlation scanning matching algorithm is matched in time with the original data collected to obtain current pose observation data using the correlation scanning matching algorithm; Step A2, based on the kinematic equations, the current pose prediction data is calculated from the current pose observation data; and the current pose prediction is calculated based on the state equations. Step A3: Based on the delayed out-of-order observation equation, calculate the Kalman gain matrix using the noise covariance matrix of the current pose observation data and the prior covariance matrix of the current pose prediction data; Step A4: Correct the current pose prediction data according to the residual between the current pose observation data and the historical pose observation data, and then set the corrected current pose prediction data as the currently estimated pose prediction data; and correct the prior covariance matrix of the current pose prediction data by combining the Kalman gain matrix, the prior covariance matrix of the current pose prediction data, and the noise covariance matrix of the current pose observation data.

[0007] In summary, this application uses the calculation results of the delayed output of the pre-set correlation scanning matching algorithm as the current pose observation data (delayed measurement data), and then corrects the robot pose data based on the aforementioned steps A1 to A4, setting the corrected pose prediction data as the currently estimated pose prediction data; thereby, at the current moment, it fully utilizes the pose data and covariance matrix of the delayed measurement data (the aforementioned current pose observation data), constrains the sensor error growth of the robot's inertial navigation measurement unit, overcomes the influence of the delayed output pose data of the correlation scanning matching algorithm, and improves the accuracy of robot pose prediction.

[0008] Furthermore, the current pose prediction data and its prior covariance matrix are updated by repeatedly executing steps A3 and A4 until the number of repetitions reaches a preset target number of iterations. The corrected current pose prediction data is then set as the target pose prediction data. Alternatively, steps A2 to A4 are repeated to update the current pose prediction data and its prior covariance matrix until the number of repetitions reaches a preset target number of iterations. The corrected current pose prediction data is then set as the target pose prediction data. This is to meet the preset pose correction accuracy requirements.

[0009] Further, in step A1, when the robot acquires the current pose observation data and its noise covariance matrix, it is configured that the current pose observation data exists at the current time, and it is determined that the timestamp of the current pose observation data is less than the current time. The timestamp of the current pose observation data is the time when the pre-set correlation scanning matching algorithm calculates the current pose observation data. The initial state of the noise covariance matrix of the current pose observation data is determined by the measurement error carried in the pose data output by the pre-set correlation scanning matching algorithm. The measurement error carried in the pose data output by the pre-set correlation scanning matching algorithm includes the measurement error of the inertial measurement unit installed on the robot. Considering the calculation delay of the correlation scanning matching algorithm, the posterior correlation of the state at different times can be formed, and the effectiveness of Kalman filtering is guaranteed.

[0010] Further, in step A3, the method for calculating the Kalman gain matrix includes: in, It is the prior covariance matrix of the current pose prediction data; V i is the noise covariance matrix of the current pose observation data; H is the Jacobian matrix of the delayed out-of-order observation equation relative to the current pose prediction data, representing the observation matrix in the Kalman filter algorithm; T is the transpose of the matrix; K i is the Kalman gain matrix; i is the current time step.

[0011] Furthermore, the method for correcting the current pose prediction data in step A4 includes: in, It is the current pose observation data, and is represented by a vector; It is the historical pose observation data; j is the timestamp of the historical pose observation data, j is less than i; It is the current pose prediction data; It is the current pose prediction data after correction; It is the residual between the current pose observation data and the historical pose observation data; the method for correcting the prior covariance matrix of the current pose prediction data in step A4 includes: in, It is the prior covariance matrix The corrected matrix forms the covariance matrix of the target state at the current moment; I is the identity matrix. When the robot is stationary and it is determined that steps A3 to A4 will be repeated, It can be updated directly in the next moment. It is the residual between the current pose observation data and the historical pose observation data.

[0012] Further, in step A2, the method for calculating the current pose prediction data based on the kinematic equations and the current pose observation data includes: calculating the current pose angle: Where, ω i It is the angular velocity ω measured by the robot's gyroscope at the current moment. b It is a preset bias angular velocity; q{·} is used to convert Euler angles into quaternions. Represents quaternion multiplication; It is the attitude angle at observation time ID. It is the current attitude angle. and All are set to quaternion form; calculate the current speed: Among them, a i It is the acceleration measured by the robot's accelerometer at the current moment, a. b It is a pre-set bias acceleration; It is the posterior rotation matrix of the observation time ID; It is the velocity at the observation time id. It is the current velocity; calculate the current predicted prior displacement: in, It is the predicted prior displacement of the observation time ID. It is the current predicted prior displacement; then, the current attitude angle is... Current speed and the current predicted prior displacement The current pose prediction data constitutes the Among them, the attitude angle at observation time id The velocity of observation time id Predicting prior displacement using the timestamp of the observation time ID The current pose observation data is composed of the observation time id, which is the timestamp of the current pose observation data, and d is the time interval between the current time i and the observation time id.

[0013] Therefore, step A2, based on the robot's kinematic model, establishes the state space using the robot's displacement, velocity, and acceleration at a certain moment, and describes the evolution of the robot's pose state over time based on Newton's laws of motion, so as to predict the prior displacement, velocity, and attitude angle through numerical integration within the framework of the Kalman filter algorithm.

[0014] Further, in step A2, the method for calculating the prior covariance matrix of the current pose prediction data based on the state equation includes: in, It is the prior covariance matrix of the current pose prediction data; It is the prior covariance matrix of the pose prediction data at observation time ID; F ε It is the pose prediction data of the state equation relative to the observation time id. Jacobian matrix; F i It is the Jacobian matrix of the state equation relative to the measurement noise of the inertial measurement unit; Q i Q is the noise covariance matrix of the current pose prediction data; i The noise is determined by the inertial measurement unit. Therefore, using the noise covariance matrix of the current pose prediction data, the pose prediction data at each time step, and the Jacobian matrix of the state equation relative to the noise covariance matrix and the pose prediction data, the prior covariance matrix of the current pose prediction data is calculated.

[0015] Furthermore, the robot pose prediction method further includes: starting from an initial moment, sequentially acquiring N sets of measurement values ​​within one acquisition cycle, wherein the timing start point of one acquisition cycle is time m, and the timing end point of the same acquisition cycle is time m+N-1; one set of measurement values ​​includes acceleration measurement value, angular velocity measurement value, left wheel rotation measurement value, and right wheel rotation measurement value; calculating the weighted mean square error for the N sets of measurement values ​​acquired within one acquisition cycle, and then determining whether the weighted mean square error is lower than a preset statistical threshold; if so, it is determined that the robot is in a stationary state; otherwise, it is determined that the robot is not in a stationary state. Thus, by detecting the degree to which the weighted mean square error deviates from the threshold, it is determined whether the robot is in a stationary state or has started moving.

[0016] Furthermore, when it is determined that the robot is stationary, the average value of N sets of acceleration measurement vectors is used to update the bias acceleration, and the average value of N sets of angular velocity measurement vectors is used to update the bias angular velocity, so that after updating the bias acceleration and the bias angular velocity, the robot updates the current pose prediction data and its prior covariance matrix by executing steps A2 to A4.

[0017] Furthermore, when it is determined that the robot is not stationary, based on the Kalman filter algorithm, the predicted state data of the left wheel and the right wheel are calculated using the measured values ​​of the left and right wheel rotations, respectively. These predicted state data are then corrected. Thus, within the framework of the Kalman filter algorithm, the constraint values ​​are adaptively adjusted according to the robot's actual motion state. After filtering, the error correction value for the rotation rate or rotation distance is obtained, which corrects the robot's speed error, resulting in the corrected three-dimensional velocity. This is the velocity component in the three coordinate axes after correction of the current velocity calculated in step A2 using the Kalman filter algorithm. Then, the position error is updated using the corrected three-dimensional velocity, resulting in the corrected velocity value and corrected velocity direction, thus solving the problem of data processing errors from the rotary encoders on the robot wheels. In complex dynamic environments, the robot uses the corrected three-dimensional velocity prediction vector to correct its position error, ensuring the accuracy of the navigation and positioning results.

[0018] Furthermore, after determining that the robot is not stationary, within the computational framework required by the Kalman filter algorithm, the predicted state data of the left wheel and its prior covariance matrix are calculated based on the state equation, and the predicted state data of the right wheel and its prior covariance matrix are also calculated based on the state equation. Then, by introducing robot motion speed constraints, Kalman filtering is applied to the predicted state data of the left and right wheels respectively to form nonholonomic constraint correction, resulting in corrected predicted state data of the left and right wheels. The corrected predicted state data of the left wheel is then updated to the measured rotation value of the left wheel, and the corrected predicted state data of the right wheel is updated to the measured rotation value of the right wheel. The robot motion speed constraints include the speed constraints required for the robot to undergo lateral displacement. These constraints include: the magnitude of the robot's current speed component in the direction of the robot's wheel axle is not equal to 0, and the magnitude of the robot's current speed component perpendicular to the robot's walking plane is equal to 0. The velocity component of the robot's current speed in the direction of the robot's wheel axle is the lateral velocity resulting from the robot's lateral displacement. This allows the robot's position error to be corrected using the corrected predicted state data of the left wheel and the right wheel when lateral displacement of the robot is detected.

[0019] Furthermore, the method for calculating the weighted mean square error of the N sets of measurements collected within one observation period includes: Where γ is the weighted mean square error; a k It is the acceleration measurement vector at time k. It is the average acceleration during the time interval from time m to time m+N-1, σ a It is the standard deviation of the accelerometer measurement noise, σ a It is preset; the time interval from time m to time m+N-1 is the current observation period; g is the gravitational acceleration; ω k It is the angular velocity measurement vector at time k, σ ω It is the standard deviation of the gyroscope measurement noise, σ ω It is pre-set; It is the measurement vector of the left wheel rotation at time k. σ is the measured value of the right wheel rotation at time k. r σ is the standard deviation of the noise measured by the left or right rotary encoder. r It is pre-set that the robot's left wheel is equipped with a left rotary encoder and the robot's right wheel with a right rotary encoder. Therefore, in the process of calculating the weighted mean square error, the standard deviation of the measurement error distribution of each measurement value is used as the weight. Attached Figure Description

[0020] Figure 1 This is a flowchart illustrating a robot pose prediction method based on Kalman filtering, as disclosed in an embodiment of this application. Detailed Implementation

[0021] The specific embodiments of the present invention will be further described below with reference to the accompanying drawings.

[0022] Robots using Correlative Scan Matching (CSM) or other localization algorithms often encounter problems such as untimely processing of sensor data and information delays in output pose data. For example, the window search and matching operations performed by the CSM algorithm are delayed, resulting in frequently delayed and out-of-order received measurement data. This causes measurements from earlier moments of the same target state to arrive at the search center later than the current measurements, a situation known as the hysteresis disorder measurement problem. When delayed measurement data arrives, traditional filtering methods utilize this delayed data, causing a rapid increase in the sensing error of the robot's inertial navigation sensors. If only the current data is used and the delayed measurement data is discarded, the information in this data cannot be effectively utilized, and the robot's pose cannot be effectively corrected.

[0023] Based on the aforementioned technical deficiencies, this application discloses a robot pose prediction method based on Kalman filtering. The robot pose prediction method is executed by a robot with an inertial measurement unit (IMU) mounted on its body and rotary encoders mounted on its left and right wheels. Essentially, it is a wheeled robot equipped with two types of odometry, which respectively sense the robot's pose state and the rotational state of the wheel axles on both sides of the robot. Figure 1As shown, the robot pose prediction method includes:

[0024] Step A1: Based on the running results of the pre-set correlation scanning matching algorithm, the robot obtains the current pose observation data and its noise covariance matrix; then, the current pose observation data is processed by data association to obtain historical pose observation data; wherein, the original data collected by the correlation scanning matching algorithm to obtain historical pose observation data is matched with the original data collected by the correlation scanning matching algorithm to obtain current pose observation data in time.

[0025] In step A1, it is configured that current pose observation data exists at the current moment, and this current pose observation data is also recorded as out-of-order position observation data. Simultaneously, it is determined that the timestamp of the current pose observation data is less than the current moment. The timestamp of the current pose observation data is the moment when the correlation scanning matching algorithm calculates the current pose observation data, serving as the solution moment within the correlation scanning matching algorithm. Therefore, the current pose observation data can be understood as delayed output pose data processed by the correlation scanning matching algorithm. The original sensor data includes inertial data collected by the inertial measurement unit and point cloud data collected by the laser sensor. After processing by the correlation scanning matching algorithm, the original sensor data yields the current pose observation data, which has been delayed by a certain period from the sensor data acquisition, but the correlation on the timeline is recorded, including the length of the delay period. Further data association processing is performed on the current pose observation data to obtain historical pose observation data. Then, step A2 is executed.

[0026] In addition, the initial state of the noise covariance matrix of the current pose observation data is determined by the measurement error carried in the pose data output by the pre-set correlation scan matching algorithm. The measurement error carried in the pose data output by the pre-set correlation scan matching algorithm includes the measurement error of the inertial measurement unit installed on the robot, and in some embodiments, it also includes the system measurement error of the rotary encoder, the system measurement error of the laser sensor, etc., to determine the initial state of the noise covariance matrix of the current pose observation data.

[0027] The historical pose observation data is matched temporally with the raw data required to obtain the current pose observation data using the correlation scanning matching algorithm, forming a posterior correlation between states at different times. For example, if the current pose observation data is the pose observation data calculated by the correlation scanning matching algorithm in the current matching iteration, then the historical pose observation data is the pose observation data calculated by the correlation scanning matching algorithm in the previous matching iteration. The raw data required to obtain the historical pose observation data using the correlation scanning matching algorithm and the current pose observation data obtained using the correlation scanning matching algorithm are then compared. The raw data to be collected must be time-matched, with the previous and current matching iterations occurring within adjacent time periods. If the timestamp of the pose observation data obtained in the previous iteration is less than the timestamp of the current pose observation data, then the timestamp of the historical pose observation data is determined to be less than the current time. The timestamp of the historical pose observation data can be approximated to an integer acquisition time. When the timestamp j of the historical pose observation data is equal to the difference between the timestamp of the current pose observation data and the value 1, the timestamp j of the historical pose observation data can be less than the difference between the current time i and the value 1. This approach takes into account the computational delay of the correlation scanning matching algorithm, enabling the formation of posterior correlations between states at different times and ensuring the effectiveness of the Kalman filter-based approach.

[0028] Step A2: Based on the kinematic equations, calculate the current pose prediction data from the current pose observation data; and calculate the prior covariance matrix of the current pose prediction data based on the state equations; then proceed to step A3.

[0029] In some embodiments, before correcting the current pose prediction data and its prior covariance matrix using the Kalman filter algorithm or other related algorithms, the current pose prediction data calculated in step A2 can be regarded as the prediction data of the initial state, and the prior covariance matrix of the current pose prediction data calculated in step A2 can be regarded as the covariance matrix of the initial state.

[0030] Step A3: Based on the delayed out-of-order observation equation, calculate the Kalman gain matrix using the noise covariance matrix of the current pose observation data and the prior covariance matrix of the current pose prediction data; then proceed to step A4.

[0031] Specifically, in step A3, the method for calculating the Kalman gain matrix includes:

[0032] This is the delayed out-of-order observation equation, which calculates the Kalman gain using out-of-order location observation data and its noise covariance matrix. Among them, It is the prior covariance matrix of the current pose prediction data; V iis the noise covariance matrix of the current pose observation data; the noise covariance matrix of the current pose observation data is determined by the measurement error carried in the pose data output by the positioning algorithm, and can be determined by the measurement noise of the inertial measurement unit; H is the Jacobian matrix of the delayed out-of-order observation equation relative to the current pose prediction data, which is equivalent to the observation matrix in the Kalman filter algorithm; T is the transpose of the matrix. This is equivalent to inverting the prediction variance. K i is the Kalman gain matrix; i is the current time step.

[0033] Step A4: Based on the residual between the current pose observation data and the historical pose observation data, correct the current pose prediction data, and then set the corrected current pose prediction data as the currently estimated pose prediction data. This can be understood as configuring the current pose prediction data after at least one correction as the robot's optimized pose. Combine the Kalman gain matrix, the prior covariance matrix of the current pose prediction data, and the noise covariance matrix of the current pose observation data to correct the prior covariance matrix of the current pose prediction data, and then determine the pose prediction at the current time.

[0034] The method for correcting the current pose prediction data in step A4 includes: in, It is the current pose observation data, and is represented by a vector; It is the historical pose observation data; j is the timestamp of the historical pose observation data, j is less than i; It is the current pose prediction data; It is the corrected current pose prediction data. It can be represented as Let represent the backward prediction state vector from time i to time id, when the robot is in a stationary state and it is determined that steps A3 to A4 will be repeated. It can be updated directly in the next moment. It is the residual between the current pose observation data and the historical pose observation data.

[0035] The method for correcting the prior covariance matrix of the current pose prediction data in step A4 includes: in, It is the prior covariance matrix The matrix obtained after correction It can be represented as Let represent the backward prediction covariance matrix from time i to time id. When the robot is stationary, it can be directly updated in the next time step. I is the identity matrix.

[0036] In summary, this application uses the calculation results of the delayed output of the pre-set correlation scanning matching algorithm as the current pose observation data (delayed measurement data), and then corrects the robot pose data based on the aforementioned steps A1 to A4, setting the corrected pose prediction data as the currently estimated pose prediction data; thereby, at the current moment, it fully utilizes the pose data and covariance matrix of the delayed measurement data (the aforementioned current pose observation data), constrains the sensor error growth of the robot's inertial navigation measurement unit, overcomes the influence of the delayed output pose data of the correlation scanning matching algorithm, and improves the accuracy of robot pose prediction.

[0037] It should be noted that in the robot pose prediction method, the execution order of steps A1 to A4 includes: first, when the robot's sensing device collects raw data and the pre-set correlation scanning matching algorithm is called to output the current pose observation data, step A1 is executed; after obtaining the historical pose observation data, the current pose observation data, and their noise covariance matrix in step A1, step A2 is executed; after calculating the current pose prediction data and its prior covariance matrix in step A2, step A3 is executed; after calculating the Kalman gain matrix in step A3, step A4 is executed; and in step A4, the currently estimated pose prediction data is corrected.

[0038] Based on the above embodiments, after the robot completes one correction of the current pose prediction data and its prior covariance matrix at the current moment, it enters the next moment i+1. The following implementation methods exist:

[0039] The current pose prediction data and its prior covariance matrix are updated by repeatedly executing steps A3 and A4 until the number of repetitions reaches a preset target iteration count. The corrected current pose prediction data is then set as the target pose prediction data to meet the preset pose correction accuracy requirements. Specifically, when the robot is stationary, steps A3 and A4 can be repeatedly executed to update the current pose prediction data and its prior covariance matrix, achieving iterative correction. During the repeated execution of steps A3 to A4, the current pose observation data and its noise covariance matrix are not updated by the latest output of the localization algorithm, and historical pose observation data are not updated. The noise covariance matrix of the current pose observation data does not need to be updated by the measurement error carried in the latest pose data output by the localization algorithm as we enter the next time step i+1. This iterative calculation continues until the number of repetitions reaches the preset target iteration count, which is pre-set to meet the preset pose correction accuracy requirements. Then, the velocity vector in the corrected current pose prediction data is updated to the predicted velocity vector at the current moment, and the velocity vector in the current pose observation data is updated to the observed velocity vector, thus obtaining the velocity in the target pose prediction data. The robot then determines that it has completed the pose prediction at a certain position. Specifically, after each execution of step A4, it is first determined whether the number of repetitions has reached the preset target number of iterations. If it is determined that the number of repetitions has not reached the preset target number of iterations, the execution of step A4 returns to step A3, and then steps A3 and A4 are executed sequentially, forming a round of repetition from step A3 to step A4. The current pose prediction data is then corrected once more. This process of repetition of steps A3 and A4 continues until the number of repetitions reaches the preset target number of iterations. At this point, the corrected current pose prediction data from the latest execution of step A4 is set as the target pose prediction data.

[0040] Alternatively, the current pose prediction data and its prior covariance matrix can be updated by repeatedly executing steps A2 to A4 until the number of repetitions reaches a preset target number of iterations. The currently corrected pose prediction data is then set as the target pose prediction data to meet the preset pose correction accuracy requirements. Illustratively, within a correction cycle, the current pose prediction data and its prior covariance matrix are updated by repeatedly executing steps A2 to A4 to achieve iterative correction. During the repeated execution of steps A2 to A4, the current pose observation data needs to be updated by the corrected current pose prediction data from the previous execution of step A4 as the next time step i+1 is reached. Consequently, the current pose prediction data and its prior covariance matrix calculated in step A2 are updated, the Kalman gain matrix calculated in step A3 is updated, and then, in step A4… In this process, the updated current pose prediction data is corrected based on the residual between the updated current pose observation data and the updated historical pose observation data. Furthermore, the updated prior covariance matrix of the updated current pose prediction data is corrected by combining the updated Kalman gain matrix, the updated prior covariance matrix of the updated current pose prediction data, and the updated noise covariance matrix of the updated current pose observation data. This iterative calculation is performed until the preset target number of iterations is reached. The preset target number of iterations is pre-set to meet preset pose correction accuracy requirements. Then, the velocity vector in the corrected current pose prediction data is updated to the predicted velocity vector at the current moment, and the velocity vector in the current pose observation data is updated to the observed velocity vector. By obtaining the velocity in the target pose prediction data, the robot determines that it has completed pose prediction at a given position. After each execution of step A4, it is first determined whether the number of repetitions has reached the preset target number of iterations. If it is determined that the number of repetitions has not reached the preset target number of iterations, the execution of step A2 is returned from the latest executed step A4, and then steps A2, A3, and A4 are executed in sequence to form a round of repetition of steps A2 to A4. Then, the current pose prediction data is corrected once. This process of repetition of steps A2 to A4 is repeated until it is determined that the number of repetitions has reached the preset target number of iterations. Then, the corrected current pose prediction data in the latest executed step A4 is set as the target pose prediction data.

[0041] As one embodiment, in step A2, the method for calculating the current pose prediction data based on the current pose observation data according to the kinematic equations includes:

[0042] Calculate the current attitude angle: These are the kinematic equations used to calculate the robot's attitude angles. Where ω... i It is the angular velocity ω measured by the robot's gyroscope at the current moment.b It is a preset bias angular velocity, representing the angular velocity bias value of the gyroscope installed on the robot; (ω i -ω b )·d represents the Euler angles that the robot deflects within the time interval d; q{·} is used to convert Euler angles into quaternions, which can be regarded as converting them into rotation matrices; This represents quaternion multiplication to perform Euler transformation on the angle values ​​of rotation about each coordinate axis, and represents converting Euler angles into quaternions through exponential mapping. It is the attitude angle at observation time ID. It is the current attitude angle. and All are set to the form of quaternions. Use quaternions to represent the robot's current attitude angle.

[0043] Calculate the current speed: These are the kinematic equations used to calculate the motion velocity of a robot. Where, a i It is the acceleration measured by the robot's accelerometer at the current moment, a. b It is a pre-set bias acceleration, representing the acceleration bias value of the accelerometer installed on the robot; It is the posterior rotation matrix of the observation time ID. Used to transfer (a i -a b The direction of acceleration is adjusted to be parallel to the direction of gravitational acceleration g; It is the velocity at the observation time id. This is the current speed.

[0044] Calculate the current predicted prior displacement: These are the kinematic equations used to calculate the robot's displacement within a time interval d. Among them, It is the predicted prior displacement at observation time id to represent the robot's predicted position at observation time id. It is the current predicted prior displacement to represent the robot's predicted position at the current time i.

[0045] Then, the current attitude angle Current speed and the current predicted prior displacement Composition of current pose prediction data That is, the pose data calculated from the current pose observation data based on the kinematic equations at the current moment; where the pose angle at observation time id is... The velocity of observation time id Predicted prior displacement at observation time ID The current pose observation data is composed of the observation time id, which is the timestamp of the current pose observation data, and d, which is the time interval between the current time i and the observation time id. It should be noted that within the time interval d, the gravitational acceleration, bias acceleration, and bias angular velocity are considered to be constant; however, updates are allowed after the robot is detected to be stationary.

[0046] Step A2, based on the robot's kinematic model, establishes a state space using the robot's displacement, velocity, and acceleration at a certain moment. It describes the evolution of the robot's pose state over time based on Newton's laws of motion, so that prior displacement, velocity, and attitude angle can be predicted using numerical integration within the framework of the Kalman filter algorithm.

[0047] On the other hand, in step A2, the method for calculating the prior covariance matrix of the current pose prediction data based on the state equation includes: This is equivalent to the state equation required to predict the prior covariance matrix in the Kalman filter algorithm. Wherein, It is the prior covariance matrix of the pose prediction data at observation time ID; F ε It is the pose prediction data of the state equation relative to the observation time id. The Jacobian matrix is ​​equivalent to the state transition matrix required by the Kalman filter algorithm; the pose prediction data at observation time id. In the state equation, F can be considered as the posterior state of the robot; i It is the Jacobian matrix of the state equation relative to the measurement noise of the inertial measurement unit; Q i Q is the noise covariance matrix of the current pose prediction data; i The noise level is determined by the inertial measurement unit (IMU) measurement, Q. i The initial values ​​are determined by the measurement errors of the inertial measurement unit in range and orientation, forming a diagonal matrix. Therefore, using the noise covariance matrix of the current pose prediction data, the pose prediction data at each time step, and the Jacobian matrix of the state equation relative to the noise covariance matrix and the pose prediction data, the prior covariance matrix of the current pose prediction data is calculated.

[0048] As one embodiment, the robot pose prediction method further includes:

[0049] Starting from the initial moment, N sets of measurement values ​​are collected sequentially within one acquisition cycle. The timing start point of one acquisition cycle is time m, and the timing end point of the same acquisition cycle is time m+N-1. If multiple acquisition cycles are set, N sets of measurement values ​​are collected within each acquisition cycle, forming a time series within a sliding window within one acquisition cycle. A set of measurement values ​​includes acceleration measurement values, angular velocity measurement values, left wheel rotation measurement values, and right wheel rotation measurement values. The initial moment is configured as the moment when the robot starts walking, and it is the timing start point of the first acquisition cycle.

[0050] The weighted mean square error is calculated for the N sets of measurements collected within one acquisition cycle, and then it is determined whether the weighted mean square error is lower than a preset statistical threshold. The preset statistical threshold is a value determined by the inventor after multiple experiments to limit the reasonable variation range of acceleration measurement, angular velocity measurement, left wheel rotation measurement, and right wheel rotation measurement. The reasonable variation range is preferably the variation range reflected by the weighted mean square threshold calculated from the acceleration measurement, angular velocity measurement, left wheel rotation measurement, and right wheel rotation measurement when the robot is stationary. It is used to detect the degree to which the weighted mean square error deviates from the threshold, and thus determine whether the robot is stationary or has started moving.

[0051] Specifically, the method for calculating the weighted mean square error of N sets of measurements collected within one acquisition period includes:

[0052]

[0053] Where γ is the weighted mean square error, which is the weighted mean square error calculated within one acquisition period; a k It is the measured acceleration value at time k; σ is the average of the N sets of acceleration measurements, used to describe the average acceleration over the time interval from time m to time m+N-1, where time k varies from the integer m to the integer m+N-1. It is suitable for calculating the weighted mean square error γ within the framework of the weighted least squares method. When m changes, the original weighted mean square error γ can be recalculated and updated. a It is the standard deviation of the accelerometer measurement noise, σ a It is preset; the time interval from time m to time m+N-1 is one of the acquisition cycles; g is the acceleration due to gravity; ω k It is the measured value of the angular velocity at time k, σ ω It is the standard deviation of the gyroscope measurement noise, σ ω It is pre-set; It is the measured value of the left wheel rotation at time k. σ is the measured value of the right wheel rotation at time k.r σ is the standard deviation of the noise measured by the left or right rotary encoder. r It is pre-set; thus, in the process of calculating the weighted mean square error, the standard deviation of the measurement error distribution of each measurement value is taken as the weight.

[0054] When the weighted mean square error is determined to be below a preset statistical threshold, the robot is determined to be stationary. When the robot is stationary, It is to adjust the direction of gravitational acceleration g to be parallel to... The direction is parallel, and the accelerometer measures a. k Equal to g, ω as measured by the gyroscope k The value is equal to 0, keeping the robot's pose constant; the right rotary encoder measures... The value is equal to 0, as measured by the left rotary encoder. The size is equal to the value 0.

[0055] In some embodiments, when the robot is determined to be stationary, the robot directly uses a Kalman filter algorithm to correct the current pose prediction data and its prior covariance matrix. The Kalman filter algorithm can be an extended Kalman filter (EKF) algorithm. Inertial data collected by the robot's inertial measurement unit or pose data output by the correlation scan matching algorithm is input into the Kalman filter. The initial search pose in the stationary state is used as prior information. During the execution of the Kalman filter algorithm, after determining the prior information, the pose observation data and its covariance matrix are calculated sequentially, followed by the calculation of the pose prediction data and its covariance matrix, the Kalman gain, and the posterior mean, to update the pose prediction data and its covariance matrix. This process is repeated, thereby using the Kalman filter algorithm to correct the current pose prediction data and its prior covariance matrix during the repeated execution of the preset target number of iterations, in order to estimate relatively accurate pose data.

[0056] In another embodiment, when the robot is determined to be stationary, the robot corrects the current pose prediction data and its prior covariance matrix by repeatedly executing steps A3 to A4. This iterative calculation is performed until the updated / corrected current pose prediction data falls within the pose data range of a target state during the time the robot is stationary. Then, steps B to D are executed to estimate relatively accurate pose data.

[0057] In other embodiments, when the robot is determined to be stationary, the robot updates the bias acceleration using the average of N sets of acceleration measurements and the bias angular velocity using the average of N sets of angular velocity measurements. It then begins to correct the current pose prediction data and its prior covariance matrix by executing steps A2 to A4 to estimate relatively accurate pose data. During the repeated execution of step A2, the current pose prediction data calculated based on the aforementioned kinematic equations will be updated, for example, the current pose angles may be updated. Update current speed And update the current predicted prior displacement. The bias acceleration and the bias angular velocity are updated by using the average measurements of the accelerometer and gyroscope when the robot is stationary (the average of N sets of acceleration measurements and the average of N sets of angular velocity measurements, respectively).

[0058] Since the wheeled sweeping robot periodically stops moving during the planned cleaning period, the average measurements of the accelerometer and gyroscope when the robot is stationary can be used to update the bias acceleration and the bias angular velocity. The Kalman filter algorithm can also be used to correct the current pose prediction data and its prior covariance matrix to correct the drift of the inertial measurement unit and predict relatively accurate pose data; thereby realizing zero-velocity detection and zero-velocity correction.

[0059] When the weighted mean square error is determined to be no less than the preset statistical threshold, it is determined that the robot is not stationary. Then, based on the Kalman filter algorithm, the predicted state data of the left wheel and the right wheel are calculated using the measured values ​​of the left wheel rotation and the right wheel rotation, respectively. The predicted state data of the left wheel and the right wheel are then corrected to obtain the corrected state and covariance matrix. Thus, under the framework of the Kalman filter algorithm, the constraint values ​​are adaptively adjusted according to the actual motion state of the robot. After filtering, the error correction value of the rotation rate or rotation distance is obtained, and the robot's speed error is corrected to obtain the corrected three-dimensional velocity, that is, the velocity components in the three coordinate axis directions after the current velocity calculated in step A2 is corrected by the Kalman filter algorithm. Then, the position error is updated using the corrected three-dimensional velocity to obtain the corrected velocity value and corrected velocity direction of the robot, solving the problem of the sensor data processing error of the rotary encoder on the robot wheel. In a complex dynamic environment, the robot uses the corrected three-dimensional velocity prediction vector to correct the robot's position error, ensuring the accuracy of the navigation and positioning results.

[0060] It should be noted that a rotary encoder is installed on each of the robot's left and right wheels to measure the rotation of the left and right wheels, respectively, and recorded as the left wheel rotation measurement value and the right wheel rotation measurement value. Both can be converted into wheel speed observation data to participate in the correction within the framework of the Kalman filter algorithm.

[0061] Specifically, after determining that the robot is not stationary, within the computational framework required by the Kalman filter algorithm, the measured value of the left wheel rotation can be used based on the pre-set wheel speed observation equation. and the measured value of the right wheel rotation The observed state data of the left wheel and the observed state data of the right wheel are calculated separately. Then, based on the state equation, the predicted state data of the left wheel and its prior covariance matrix are calculated from the observed state data of the left wheel and its prior covariance matrix. Similarly, the predicted state data of the right wheel and its prior covariance matrix are calculated from the observed state data of the right wheel and its prior covariance matrix based on the state equation. This achieves the calculation of the predicted state data of the left wheel and its prior covariance matrix based on the state equation, and the predicted state data of the right wheel and its prior covariance matrix based on the state equation. Then, by introducing robot motion speed constraints, Kalman filtering correction is performed on the predicted state data of the left wheel and the right wheel respectively to obtain the corrected predicted state data of the left wheel and the right wheel, thus forming a nonholonomic constraint correction. Then, the corrected left wheel predicted state data is updated to the left wheel rotation measurement value, and the corrected right wheel predicted state data is updated to the right wheel rotation measurement value to estimate relatively accurate pose data; so that: when the robot is detected to have lateral displacement, the robot position error, including the robot's lateral velocity error, is corrected by using the corrected left wheel predicted state data and the corrected right wheel predicted state data.

[0062] The robot motion speed constraints include the speed constraints required for the robot to undergo lateral displacement. The speed constraints required for the robot to undergo lateral displacement include: the magnitude of the robot's current speed in the direction of the robot's wheel axle (i.e., the magnitude of the lateral speed) is not equal to 0, and the magnitude of the robot's current speed in the direction perpendicular to the robot's walking plane is equal to 0. The velocity component of the robot's current speed in the direction of the robot's wheel axle is the lateral speed caused by the robot's lateral displacement.

[0063] In this embodiment, the process from calculating the observed state data of the left wheel and the observed state data of the right wheel to updating the corrected predicted state data of the left wheel and the corrected predicted state data of the right wheel is equivalent to: using the speed observation values ​​of the rotary encoder and the inertial measurement unit / nonholonomic constraint as auxiliary sources with errors to input the Kalman filter, configuring the wheel rotation rate measured by the rotary encoder as the observation vector, and configuring the current pose prediction data as the state vector, and performing a measurement update process of Kalman filtering.

[0064] After introducing the Kalman filter algorithm into the robot motion speed constraint conditions, the current pose prediction data can be calculated from the current pose observation data according to the kinematic equations in step A2. The time interval between the current moment and the observation moment can be limited according to the robot motion speed constraint conditions and the actual motion state of the robot, for example, the observation moment can be set to the previous moment. According to the kinematic equations in step A2, the robot motion speed constraint conditions not only generate constraints between continuous velocity, attitude angle, and displacement, but also generate constraints on the state increment between the velocity of the accelerometer and gyroscope and the preset inertial error estimate, thereby improving the pose prediction accuracy of the Kalman filter algorithm in dynamic environments.

[0065] It should be noted that Kalman filtering is insensitive to the parameters of the robot motion model (e.g., the kinematic equations in step A2) and the initial values ​​of the system within a certain range, provided that the parameter settings ensure the robot system can effectively "capture the target". Kalman filtering is a system that uses temporal information for information fusion. It can weight observation information through Kalman gain, thereby correcting prediction uncertainties. This correction of uncertainty accumulates over time; as the correction moves towards the true value, the pose prediction data and its prior covariance matrix gradually converge. In the state transition model (target prior) and the observation model (sensor prior), the degree of matching between the model and the actual process determines the final corrected pose estimation accuracy.

[0066] The above description is merely a preferred embodiment of the present invention and is not intended to limit the invention in any other way. Any person skilled in the art may make changes or modifications to the above-disclosed technical content to create equivalent embodiments. However, any simple modifications, equivalent changes, and modifications made to the above embodiments based on the technical essence of the present invention without departing from the scope of the present invention shall still fall within the protection scope of the present invention.

Claims

1. A robot pose prediction method based on Kalman filtering, characterized in that, Robot pose prediction methods include: Step A1: Based on the running results of the pre-set correlation scanning matching algorithm, the robot obtains the current pose observation data and its noise covariance matrix; then, the current pose observation data is processed by data association to obtain historical pose observation data; wherein, the original data required to obtain historical pose observation data using the correlation scanning matching algorithm is matched with the original data required to obtain current pose observation data using the correlation scanning matching algorithm in time. Step A2: Based on the kinematic equations, calculate the current pose prediction data from the current pose observation data; and calculate the prior covariance matrix of the current pose prediction data based on the state equations. Step A3: Based on the delayed out-of-order observation equation, calculate the Kalman gain matrix using the noise covariance matrix of the current pose observation data and the prior covariance matrix of the current pose prediction data; Step A4: Based on the residual between the current pose observation data and the historical pose observation data, correct the current pose prediction data, and then set the corrected current pose prediction data as the currently estimated pose prediction data; and combine the Kalman gain matrix, the prior covariance matrix of the current pose prediction data, and the noise covariance matrix of the current pose observation data to correct the prior covariance matrix of the current pose prediction data. The robot pose prediction method also includes: Starting from the initial moment, N sets of measurement values ​​are collected sequentially within one acquisition cycle. The timing start point of one acquisition cycle is time m, and the timing end point of the same acquisition cycle is time m+N-1. A set of measurement values ​​includes acceleration measurement value, angular velocity measurement value, left wheel rotation measurement value, and right wheel rotation measurement value. The weighted mean square error is calculated for the N sets of measurement values ​​collected within one collection cycle. Then it is determined whether the weighted mean square error is lower than a preset statistical threshold. If it is, the robot is determined to be stationary; otherwise, the robot is determined not to be stationary. After determining that the robot is not stationary, within the computational framework required by the Kalman filter algorithm, the predicted state data of the left wheel and its prior covariance matrix are calculated based on the state equation, and the predicted state data of the right wheel and its prior covariance matrix are also calculated based on the state equation. Then, by introducing robot motion speed constraints, Kalman filtering is applied to the predicted state data of the left and right wheels respectively to form nonholonomic constraint correction, resulting in corrected predicted state data of the left and right wheels. Finally, the corrected predicted state data of the left wheel is updated with the measured rotation value of the left wheel, and the corrected predicted state data of the right wheel is updated with the measured rotation value of the right wheel. Among them, the robot motion speed constraints include the speed constraints required for the robot to undergo lateral displacement; the speed constraints required for the robot to undergo lateral displacement include: the magnitude of the robot's current speed in the direction of the robot's wheel axle is not equal to 0, and the magnitude of the robot's current speed in the direction perpendicular to the robot's walking plane is equal to 0. The component of the robot's current speed in the direction of the robot's wheel axle is the lateral velocity resulting from the robot's lateral displacement.

2. The robot pose prediction method according to claim 1, characterized in that, The current pose prediction data and its prior covariance matrix are updated by repeatedly executing steps A3 and A4 until the number of repetitions reaches the preset target number of iterations, and the current corrected current pose prediction data is set as the target pose prediction data. Alternatively, the current pose prediction data and its prior covariance matrix can be updated by repeatedly executing steps A2 to A4 until the number of repetitions reaches the preset target number of iterations, and then the current corrected current pose prediction data can be set as the target pose prediction data.

3. The robot pose prediction method according to claim 1, characterized in that, In step A1, when the robot acquires the current pose observation data and its noise covariance matrix, it is configured that the current pose observation data exists at the current time and it is determined that the timestamp of the current pose observation data is less than the current time. The timestamp of the current pose observation data is the time when the current pose observation data is calculated by the pre-set correlation scanning matching algorithm. The initial state of the noise covariance matrix of the current pose observation data is determined by the measurement error carried in the pose data output by the pre-set correlation scan matching algorithm; the measurement error carried in the pose data output by the pre-set correlation scan matching algorithm includes the measurement error of the inertial measurement unit installed on the robot.

4. The robot pose prediction method according to claim 1, characterized in that, In step A3, the method for calculating the Kalman gain matrix includes: ； in, It is the prior covariance matrix of the current pose prediction data; It is the noise covariance matrix of the current pose observation data; is the Jacobian matrix of the delayed out-of-order observation equation relative to the current pose prediction data, representing the observation matrix in the Kalman filter algorithm; T is the transpose of the matrix; is the Kalman gain matrix; i is the current time step.

5. The robot pose prediction method according to claim 4, characterized in that, The method for correcting the current pose prediction data in step A4 includes: ;in, It is the current pose observation data, and is represented as a vector; It is the historical pose observation data; j is the timestamp of the historical pose observation data, j is less than i; It is the current pose prediction data; It is the current pose prediction data after correction; It is the residual between the current pose observation data and the historical pose observation data; The method for correcting the prior covariance matrix of the current pose prediction data in step A4 includes: ;in, It is the prior covariance matrix of the current pose prediction data. The corrected matrix is ​​used to form the covariance matrix of the target state at the current moment. It is an identity matrix.

6. The robot pose prediction method according to claim 1, characterized in that, In step A2, the method for calculating the current pose prediction data based on the kinematic equations from the current pose observation data includes: Calculate the current attitude angle: ;in, It is the angular velocity at the current moment, measured by the robot's gyroscope. It is a preset bias angular velocity; q{ } is used to convert Euler angles to quaternions. Represents quaternion multiplication; It is the attitude angle at observation time ID. It is the current attitude angle. All are set to quaternion form; Calculate the current speed: ;in, It is the acceleration measured by the robot's accelerometer at the current moment. It is a pre-set bias acceleration; It is the posterior rotation matrix of the observation time ID; It is the velocity at the observation time id. ; Calculate the current predicted prior displacement: ;in, It is the predicted prior displacement of the observation time ID. It is the current predicted prior displacement; Then, the current attitude angle Current speed and the current predicted prior displacement The current pose prediction data constitutes the ; Among them, the attitude angle at observation time id The speed of observation time ID Predicting prior displacement using the timestamp of the observation time ID The current pose observation data constitutes the data. Wherein, the observation time id is the timestamp of the current pose observation data. It is the time interval between the current time i and the observation time id.

7. The robot pose prediction method according to claim 6, characterized in that, In step A2, the method for calculating the prior covariance matrix of the current pose prediction data based on the state equation includes: ； in, It is the prior covariance matrix of the current pose prediction data; It is the prior covariance matrix of the pose prediction data at observation time ID; It is the pose prediction data of the state equation relative to the observation time id. The Jacobian matrix; It is the Jacobian matrix of the state equation relative to the measurement noise of the inertial measurement unit; It is the noise covariance matrix of the current pose prediction data.

8. The robot pose prediction method according to claim 6, characterized in that, When the robot is determined to be stationary, the average value of N sets of acceleration measurement vectors is used to update the bias acceleration, and the average value of N sets of angular velocity measurement vectors is used to update the bias angular velocity, so that after updating the bias acceleration and the bias angular velocity, the robot updates the current pose prediction data and its prior covariance matrix by executing steps A2 to A4.

9. The robot pose prediction method according to claim 6, characterized in that, When it is determined that the robot is not stationary, based on the Kalman filter algorithm, the predicted state data of the left wheel and the predicted state data of the right wheel are calculated using the measured values ​​of the left wheel rotation and the right wheel rotation, respectively, and then the predicted state data of the left wheel and the right wheel are corrected.

10. The robot pose prediction method according to claim 6, characterized in that, The method for calculating the weighted mean square error of N sets of measurements collected within one acquisition period includes: ； in, It is the weighted mean square error; It is the acceleration measurement vector at time k. It is the average acceleration during the time interval from time m to time m+N-1. It is the standard deviation of the noise measured by the accelerometer. It is preset; the time interval from time m to time m+N-1 is the current observation period; g is the gravitational acceleration; It is the angular velocity measurement vector at time k. It is the standard deviation of the noise measured by the gyroscope. It is pre-set; It is the measurement vector of the left wheel rotation at time k. It is the measured value of the right wheel rotation at time k. It is the standard deviation of the noise measured by the left or right rotary encoder. It is pre-set; The robot's left wheel is equipped with a left rotary encoder, and the robot's right wheel is equipped with a right rotary encoder.

Images