A robot positioning method

Abstract

The application discloses a robot positioning method, comprising the following steps: step 1, acquiring the speed information, heading information and position information of the robot through two IMU sensors respectively, and acquiring the travel distance information of the robot through a travel information acquisition unit; step 2, inputting the information collected by the two IMU sensors and the travel distance information into different Kalman filters, inputting the speed information and heading information of the non-corresponding IMU sensor into each Kalman filter respectively, and obtaining the speed scale factor, the heading angle deviation and the coordinates of the robot in the coordinate system, thereby constructing the system covariance matrix, the noise covariance matrix, the system state quantity matrix and the observation value matrix in the corresponding Kalman filter parameters; and step 3, each Kalman filter uses a prediction model to iteratively predict the positioning information of the robot. The application can realize real-time positioning of a mobile device.

Description

Technical Field

[0001] This invention relates to the field of positioning methods, specifically a robot positioning method. Background Technology

[0002] Navigation methods in GPS-denied areas have become a key research focus both domestically and internationally. Extensive research has been conducted on navigation technologies applicable to GPS-denied areas, including: ground-based radio navigation, ultra-wideband (UWB) navigation, visual navigation, and inertial navigation.

[0003] Inertial navigation, with its characteristics of not relying on external information and not radiating energy to the outside, has become a key research focus for researchers both domestically and internationally. The basic working principle of inertial navigation is based on Newton's laws of motion. By measuring the acceleration of the carrier in an inertial reference frame, integrating it over time, and transforming it into the navigation coordinate system, information such as velocity, yaw angle, and position in the navigation coordinate system can be obtained. The inertial measurement unit (IMU) is the core measurement device in the inertial navigation system. The main components of the IMU are a gyroscope, an accelerometer, and a magnetometer. The gyroscope obtains the acceleration along each axis, the accelerometer obtains the acceleration in the x, y, and z directions, and the magnetometer obtains information about the surrounding magnetic field. The main function of the IMU is to fuse the data from the three sensors to obtain relatively accurate attitude information.

[0004] However, the position information output by an IMU sensor will develop increasingly larger errors over time due to acceleration drift, leading to significant inaccuracies in the positioning results. Furthermore, in confined spaces, it is inconvenient to deploy nodes (Wi-Fi, Bluetooth, etc.) for auxiliary positioning. Therefore, finding a method to correct the errors generated by the IMU in confined spaces without requiring extensive auxiliary means is a key aspect of IMU-based positioning. Summary of the Invention

[0005] This invention provides a robot localization method to solve the problem that existing IMU sensor localization methods require a large number of auxiliary means to correct the error of the output result.

[0006] To achieve the above objectives, the technical solution adopted by the present invention is as follows:

[0007] A robot localization method includes the following steps:

[0008] Step 1: Obtain the robot's speed, heading, and position information through two IMU sensors, and obtain the robot's travel distance information through the travel information acquisition unit;

[0009] Step 2: Using two identical Kalman filters, input the velocity, heading, and position information obtained from one of the IMU sensors in Step 1 into one Kalman filter, and input the velocity, heading, and position information obtained from the other IMU sensor in Step 1 into the other Kalman filter. Input the travel distance information obtained in Step 1 into both Kalman filters respectively; each Kalman filter also inputs the velocity and heading information obtained from non-corresponding IMU sensors.

[0010] Each Kalman filter obtains a speed scaling factor based on the speed information collected by two IMU sensors. Each Kalman filter corrects the heading information collected by the corresponding IMU sensor with the heading information collected by the non-corresponding IMU sensor to obtain the heading angle deviation. Each Kalman filter also corrects the position information collected by the corresponding IMU sensor with the travel distance information collected by the travel information acquisition unit to obtain the robot's coordinates in the coordinate system. Thus, each Kalman filter constructs the system covariance matrix in the corresponding Kalman filter parameters based on the speed scaling factor, heading angle deviation, coordinates in the coordinate system, and the speed and heading information obtained by the corresponding IMU sensor.

[0011] Each Kalman filter also uses the velocity scaling factor, heading angle deviation, heading information obtained from the corresponding IMU sensor, velocity information obtained from the non-corresponding IMU sensor, and coordinates in the coordinate system to construct the noise covariance matrix in the corresponding Kalman filter parameters.

[0012] Each Kalman filter also uses the coordinates, velocity information, heading information, velocity scaling factor, and heading angle deviation obtained from the corresponding IMU sensor in the coordinate system to construct the system state matrix in the Kalman filter parameters;

[0013] Each Kalman filter also constructs its own parameter matrix using the output values ​​of another Kalman filter;

[0014] Step 3: Load the system covariance matrix, noise covariance matrix, system state matrix, and observation matrix set in Step 2 into each Kalman filter, and use the prediction model to iteratively predict the robot's localization information. Then, use the localization information output by one Kalman filter to correct the error of the localization information output by the other Kalman filter.

[0015] In further step 1, the heading information includes heading angle and heading angular velocity.

[0016] Furthermore, the coordinate system in step 2 is the UTM coordinate system.

[0017] Furthermore, the prediction model in step 3 is the CTRV motion model.

[0018] Furthermore, in step 3, the system state matrix in the Kalman filter parameters is updated during each iteration. After multiple iterations, the output system state matrix is ​​the robot's positioning information output by the Kalman filter.

[0019] In this invention, the position information acquisition unit employs an odometry system and two IMU sensors. A separate Kalman filter is constructed for each IMU. The velocity, heading, and position information acquired by each IMU are input into the corresponding Kalman filter. Each Kalman filter also receives velocity and heading information from non-corresponding IMU units, as well as the travel distance information acquired by the odometry. Each Kalman filter uses the two IMU sensors to mutually correct each other's heading angles and uses the travel distance obtained from the odometry to correct the position information of each IMU sensor. Thus, the corrected position information of each IMU sensor yields the robot's coordinates in the coordinate system, and a velocity scaling factor obtained by comparing the velocity information from the two IMU sensors is introduced.

[0020] Compared with existing technologies, the present invention can achieve error correction of positioning output results in a closed and confined space without GPS signals, network deployment, or excessive auxiliary means, thereby enabling real-time positioning of mobile devices. Attached Figure Description

[0021] Figure 1 This is a schematic diagram of a system according to an embodiment of the present invention.

[0022] Figure 2 This is a flowchart of the method according to an embodiment of the present invention. Detailed Implementation

[0023] The present invention will be further described below with reference to the accompanying drawings and embodiments.

[0024] like Figure 1 As shown, this embodiment discloses a robot localization method, such as... Figure 1As shown, the robot integrates an inertial navigation information acquisition unit, a travel information acquisition unit, and a positioning unit. The inertial navigation information acquisition unit uses two IMU sensors, each independently acquiring the robot's speed, heading, and position information. The travel information acquisition unit uses an odometer to acquire the robot's distance traveled. The positioning unit incorporates two Kalman filters. The first Kalman filter acquires the speed, heading, and position information acquired by the first IMU sensor, the distance traveled by the odometer, and the speed and heading information acquired by the second IMU sensor. The second Kalman filter acquires the speed, heading, and position information acquired by the second IMU sensor, the distance traveled by the odometer, and the speed and heading information acquired by the first IMU sensor.

[0025] like Figure 2 As shown, the robot localization method in this embodiment includes the following steps:

[0026] Step 1: Obtain the robot's speed, heading, and position information through two IMU sensors, and obtain the robot's travel distance information through the travel information acquisition unit;

[0027] Step 2: Input the velocity, heading, and position information collected by the first IMU sensor in Step 1 into the first Kalman filter; input the velocity, heading, and position information collected by the second IMU sensor in Step 1 into the second Kalman filter; input the distance traveled in Step 1 into both Kalman filters respectively; input the velocity and heading information collected by the second IMU sensor in Step 1 into the first Kalman filter; input the velocity and heading information collected by the first IMU sensor in Step 1 into the second Kalman filter. The heading information includes the heading angle and heading angular velocity.

[0028] Each Kalman filter compares the velocity information acquired by the two IMU sensors to obtain a velocity scaling factor.

[0029] Each Kalman filter subtracts the heading angle from the heading information acquired by the non-corresponding IMU sensor from the heading angle acquired by the corresponding IMU sensor to correct the heading angle in the heading information acquired by the corresponding IMU sensor and obtain the heading angle deviation.

[0030] Each Kalman filter corrects the position information collected by the corresponding IMU sensor using the travel distance information collected by the travel information acquisition unit, thus obtaining the robot's coordinates in the UTM coordinate system.

[0031] Then, each Kalman filter constructs the system covariance matrix P in its parameters based on the velocity scaling factor, heading angle deviation, coordinates in the coordinate system, and the velocity and heading information obtained from the corresponding IMU sensor. The system covariance matrix P is shown in the following formula:

[0032] P= (1),

[0033] In formula (1): This represents the variance of the robot's x-coordinate in the UTM coordinate system. This represents the covariance between the robot's x and y coordinates in the UTM coordinate system. This represents the covariance between the robot's x-coordinate and its velocity in the UTM coordinate system. This represents the covariance between the robot's x-coordinate and heading angle in the UTM coordinate system. This represents the covariance between the robot's x-coordinate and heading angular velocity in the UTM coordinate system. This represents the covariance between the robot's x-coordinate and the velocity scaling factor in the UTM coordinate system. This represents the covariance between the robot's x-coordinate and heading angle in the UTM coordinate system.

[0034] This represents the covariance between the robot's ordinate and abscissa in the UTM coordinate system. This represents the variance of the robot's ordinate in the UTM coordinate system. This represents the covariance between the robot's ordinate and its velocity in the UTM coordinate system. This represents the covariance between the robot's ordinate and heading angle in the UTM coordinate system. This represents the covariance between the robot's ordinate and heading angular velocity in the UTM coordinate system. This represents the covariance between the robot's ordinate and the velocity scaling factor in the UTM coordinate system. This represents the covariance between the robot's ordinate and heading angle in the UTM coordinate system. This represents the covariance between the robot's velocity and its x-coordinate in the UTM coordinate system. This represents the covariance between the robot's velocity and its ordinate in the UTM coordinate system. This represents the variance of the robot's speed. This represents the covariance between the robot's speed and heading angle. This represents the covariance between the robot's velocity and its heading angular velocity. This represents the covariance value between the robot's speed and the speed scaling factor. This represents the covariance between the robot's speed and heading angle deviation. This represents the covariance between the robot's heading angle and its x-coordinate in the UTM coordinate system. This represents the covariance between the robot's heading angle and its ordinate in the UTM coordinate system. This represents the covariance between the robot's heading angle and its velocity. This represents the variance of the robot's heading angle. This represents the covariance between the robot's heading angle and its heading angular velocity. This represents the covariance value of the robot's heading angle and velocity scaling factor. The covariance value representing the robot's heading angle and its deviation from the heading angle. This represents the covariance between the robot's angular velocity and its x-coordinate in the UTM coordinate system. This represents the covariance between the robot's angular velocity and its ordinate in the UTM coordinate system. This represents the covariance between the robot's heading angular velocity and its velocity. This represents the covariance between the robot's angular velocity and its heading angle. The variance of the robot's heading angular velocity. This represents the covariance between the robot's heading angular velocity and the velocity scaling factor. The covariance value representing the robot's angular velocity and angular deviation.

[0035] This represents the covariance between the robot's velocity scaling factor and the robot's horizontal coordinate in the UTM coordinate system. This represents the covariance between the robot's velocity scaling factor and its ordinate in the UTM coordinate system. This represents the covariance between the robot's speed scaling factor and its speed. This represents the covariance between the robot's speed scaling factor and its heading angular velocity. This represents the variance of the robot's speed scaling factor. This represents the covariance between the robot's speed scaling factor and the robot's heading angle deviation. This represents the covariance between the robot's heading angle deviation and its x-coordinate in the UTM coordinate system. This represents the covariance between the robot's heading angle deviation and its ordinate in the UTM coordinate system. This represents the covariance between the robot's heading angle deviation and its velocity. This represents the covariance between the robot's heading angle deviation and its heading angle. This represents the covariance between the robot's heading angle deviation and its heading angular velocity. This represents the covariance between the robot's heading angle deviation and the robot's velocity scaling factor. This represents the variance of the robot's heading angle deviation.

[0036] Each Kalman filter also constructs the noise covariance matrix R in the corresponding Kalman filter parameters using the velocity scaling factor, heading angle deviation, heading information obtained from the corresponding IMU sensor, velocity information obtained from the non-corresponding IMU sensor, and coordinates in the coordinate system. The noise covariance matrix R is shown in formula (2):

[0037] R= (2),

[0038] In formula (2): This represents the variance of the lateral position measured by non-corresponding IMU sensors. This represents the variance of the longitudinal position measured by non-corresponding IMU sensors. This represents the variance of the speed output from the non-corresponding IMU sensors. This represents the variance of the robot's heading angle output by the corresponding IMU sensor. This represents the variance of the robot's heading angular velocity output by the corresponding IMU sensor. The variance representing the robot's speed scaling factor. This represents the variance of the robot's heading angle deviation, where the elements in matrix R are variances set according to the characteristics of the sensors and are related to the accuracy of the sensors. and The value is 0;

[0039] Each Kalman filter also uses the coordinates, velocity information, heading information, velocity scaling factor, and heading angle deviation obtained from the corresponding IMU sensor in the coordinate system to construct the system state matrix X in the Kalman filter parameters. The system state matrix X is shown in formula (3):

[0040] (3),

[0041] In formula (3): x represents the horizontal coordinate of the robot inputting the Kalman filter in the UTM coordinate system, y represents the vertical coordinate of the robot inputting the Kalman filter in the UTM coordinate system, and v represents the velocity of the robot inputting the Kalman filter. This represents the robot's heading angle when input to the Kalman filter. This represents the angular velocity of the robot as input to the Kalman filter. This represents the speed scaling factor of the input Kalman filter. This indicates the heading angle deviation of the input Kalman filter.

[0042] Each Kalman filter also constructs its own observation matrix Z from the output values ​​of the other Kalman filter, as shown in Equation (4):

[0043] , (4),

[0044] In formula (4), , This indicates the position information output by the robot's position information acquisition unit. It is the heading angle output by the robot's heading information acquisition unit. The speed information output by the robot's speed information acquisition unit. This is the angular velocity of the heading angle output by the robot's heading information acquisition unit. The correspondence between the system state variables X and Z is as follows: Corresponding to x, Corresponding to y, Corresponding to , By v and Calculations show that Depend on and The calculation yielded the result.

[0045] Step 3: Load the system covariance matrix, noise covariance matrix, system state matrix, and observation matrix set in Step 2 into each Kalman filter, and use the prediction model to iteratively predict the robot's positioning information.

[0046] In this embodiment, the prediction model is the CTRV motion model, which is shown in formula (5):

[0047]

[0048] (5)

[0049]

[0050]

[0051] In formula (5): v t This represents the robot's velocity at time t. This represents the robot's heading angle at time t. , Let x and y represent the robot's x and y coordinates in the UTM coordinate system at time t, respectively. This represents the robot's velocity at time t-1. This represents the robot's heading angle at time t-1. and These represent the x-coordinate and y-coordinate of the robot in the UTM coordinate system at time t-1, respectively. Indicates time difference, This represents the speed scaling factor of the vehicle at time t-1. This represents the robot's heading angle deviation at time t-1. Let be the angular velocity of the heading angle at time t-1.

[0052] At each iteration, the system state matrix in the Kalman filter parameters is... Updates will be performed, including:

[0053] Update method: based on the prediction model and the location output from another IMU sensor ( , and vehicle speed It can be calculated The value of is used in the prediction model to calculate the robot's velocity. The formula contains the velocity at the previous moment. and scaling factor At the same time, speed Substitute into the formula and In the updated model, the sensor receives position information (x, y) and velocity v from another IMU sensor. The system variable X is then updated based on the predicted values, the sensor's output values, and its noise covariance matrix R and covariance matrix P. This is used to correct the robot's speed.

[0054] Update method: based on the prediction model and the location output from another IMU sensor ( , ) and the heading angle yaw output by the IMU It can be calculated The value of . In the predictive model calculation, the formula for calculating the robot's heading angle is: , which contains ; heading angle Substitute into the formula and In the update module, the sensor receives position information (x, y) from another IMU sensor, along with the IMU's output heading angle and angular velocity. At this point, the system variable X is updated based on the predicted value, the sensor's output value, and its noise covariance matrix R and covariance matrix P. This is used to correct the robot's heading angle.

[0055] System state matrix All parameters need to be updated. The output is directly from the sensor.

[0056] The system state matrix X output after multiple iterations is the robot's localization information output by the Kalman filter.

[0057] In this embodiment, the positioning information output by one Kalman filter is used to correct the positioning information output by another Kalman filter in a continuous time interval. Specifically: at time t, the positioning information of the Kalman filter corresponding to the first IMU sensor is used as the reference, and the positioning information of the Kalman filter corresponding to the second IMU sensor is corrected for error based on the positioning information of the Kalman filter corresponding to the first IMU sensor; at time t+1, the positioning information of the Kalman filter corresponding to the second IMU sensor is used as the reference, and the positioning information of the Kalman filter corresponding to the first IMU sensor is corrected for error based on the positioning information of the Kalman filter corresponding to the second IMU sensor; at time t+2, the positioning information of the Kalman filter corresponding to the first IMU sensor is used as the reference, and the positioning information of the Kalman filter corresponding to the second IMU sensor is corrected for error based on the positioning information of the Kalman filter corresponding to the first IMU sensor; and so on, alternating the error correction of the positioning information of the two Kalman filters in a continuous time interval.

[0058] The positioning method provided by this invention is compared with the existing GPS+IMU combined positioning and UWB (Bluetooth, Wi-Fi, etc.)+IMU network deployment positioning effects, as well as the convenience of the whole system. As shown in Table 1, it can be seen that the method provided by this invention can ensure the positioning accuracy of the system while adapting the hardware to small spaces and being simple and convenient. It can accurately locate the robot information regardless of whether there is obstruction or not.

[0059]

[0060] Table 1 Comparison Table

[0061] The preferred embodiments of the present invention have been described in detail above with reference to the accompanying drawings. These embodiments are merely descriptions of preferred embodiments and are not intended to limit the scope or concept of the invention. The specific technical features described in the above embodiments can be combined in any suitable manner without contradiction. Such combinations, as long as they do not violate the spirit of the present invention, should also be considered as part of this disclosure. To avoid unnecessary repetition, the present invention will not further describe the various possible combinations.

[0062] This invention is not limited to the specific details of the above embodiments. Within the scope of the technical concept of this invention and without departing from the design idea of ​​this invention, all modifications and improvements made by those skilled in the art to the technical solutions of this invention should fall within the protection scope of this invention. The technical content for which protection is sought in this invention has been fully described in the claims.

Claims

1. A robot localization method, characterized in that, Includes the following steps: Step 1: Acquire the robot's speed, heading, and position information using two IMU sensors, and acquire the robot's travel distance using the travel information acquisition unit. Step 2: Using two identical Kalman filters, input the speed, heading, and position information obtained from one IMU sensor in Step 1 into one Kalman filter, and input the speed, heading, and position information obtained from the other IMU sensor in Step 1 into the other Kalman filter. Input the travel distance information obtained in Step 1 into both Kalman filters. Each Kalman filter also inputs the speed and heading information from non-corresponding IMU sensors. Each Kalman filter obtains a speed scaling factor based on the speed information from the two IMU sensors. Each Kalman filter corrects the heading information from the corresponding IMU sensor using the heading information from the non-corresponding IMU sensor to obtain the heading angle deviation. Each Kalman filter also corrects the position information from the corresponding IMU sensor using the travel distance information acquired by the travel information acquisition unit to obtain the robot's coordinates in the coordinate system. Thus, each... Each Kalman filter constructs a system covariance matrix in its parameters based on the velocity scaling factor, heading angle deviation, coordinates in the coordinate system, and velocity and heading information obtained from the corresponding IMU sensor. Each Kalman filter also constructs a noise covariance matrix in its parameters using the velocity scaling factor, heading angle deviation, heading information from the corresponding IMU sensor, velocity information from non-corresponding IMU sensors, and coordinates in the coordinate system. Each Kalman filter also constructs a system state matrix in its parameters using coordinates in the coordinate system obtained from the corresponding IMU sensor, velocity information, heading information, velocity scaling factor, and heading angle deviation. Each Kalman filter also constructs an observation matrix in its parameters using the output value of another Kalman filter. Step 3: Each Kalman filter loads the system covariance matrix, noise covariance matrix, system state matrix, and observation matrix set in Step 2, and uses a prediction model to iteratively predict the robot's positioning information. The positioning information output by one Kalman filter is used to correct the error in the positioning information output by the other Kalman filter.

2. The robot localization method according to claim 1, characterized in that, In step 1, the heading information includes heading angle and heading angular velocity.

3. The robot localization method according to claim 1, characterized in that, The coordinate system in step 2 is the UTM coordinate system.

4. The robot localization method according to claim 1, characterized in that, The prediction model in step 3 is the CTRV motion model.

5. The robot localization method according to claim 1, characterized in that, In step 3, the system state matrix in the Kalman filter parameters is updated during each iteration. After multiple iterations, the output system state matrix is ​​the robot's localization information output by the Kalman filter.

Images