Mobile robot positioning and mapping method based on UWB radar

Abstract

The invention discloses a mobile robot positioning and mapping method based on UWB radar, and belongs to the technical field of mobile robot SLAM. According to the method, two UWB radars carried by an ROS system mobile robot platform are used for collecting environment original time domain data, and the target distance is solved through timestamp alignment, noise reduction, normalization, Savitzky-Golay filtering and self-adaptive peak detection in combination with odometer information; then visual range verification and polar coordinate conversion are completed through a triangulation positioning method, and polar coordinate parameters of the target relative to the robot are obtained; and finally, an RBPF-SLAM algorithm optimized by a firefly algorithm is adopted, pose estimation, scanning matching, particle weight updating and partition optimization, resampling and map updating are sequentially executed, and self-localization and environment map construction are achieved. A base station does not need to be arranged in advance, the limitation of a traditional odometer method is overcome, and the method is high in positioning precision, good in mapping effect, high in robustness and suitable for the autonomous navigation scene of the mobile robot in the unknown environment in the indoor environment and the like.

Description

TECHNICAL FIELD

[0001] The present application relates to the method of positioning and mapping of mobile robots, in particular to the sensor data of UWB radar and odometry, environment data, simultaneous localization and mapping (SLAM) method based on improved Rao-Blackwellised particle filtering, realizing the autonomous positioning of mobile robots and constructing the indoor environment map. BACKGROUND

[0002] In order to realize the autonomous movement of mobile robots in an unfamiliar environment, the first problem to be solved is "where am I?" and "how is the surrounding environment?". The problem of "where am I?" can be attributed to the positioning problem, and the mobile robot estimates the position and attitude information according to the surrounding environment information and its own motion. The problem of "how is the surrounding environment?" is the mapping problem, and the mobile robot collects information through the sensors (such as laser radar or camera, etc.) carried by it to construct the environment map. With the development of robot technology, especially the increasing demand for autonomous navigation in unknown or dynamic environments, SLAM technology has become a key technology. However, the traditional SLAM method based on vision or laser radar faces challenges in some special scenarios, such as poor lighting conditions, sparse texture, or environments with a large amount of reflective materials. UWB radar has become a potential alternative solution due to its high-precision ranging, strong penetration ability, and anti-multipath interference ability.

[0003] UWB technology usually adopts short pulse modulation technology, and the pulse duration is extremely short, in the order of nanoseconds to microseconds, with the characteristic of short pulse duration; its bandwidth usually covers several GHz or even wider frequency range, with the characteristic of extremely wide spectrum bandwidth. In the case of transmission distance less than 10 meters, the data amount transmitted per second by UWB can reach or even exceed the level of gigabit.

[0004] Most UWB-based robot positioning methods use fixed base station and tag configuration. The UWB positioning tag is installed on the mobile robot, which sends nanosecond-level pulse signals to the surrounding environment, and the UWB base station fixedly installed in the surrounding environment receives and measures the pulse signals, and calculates the arrival time of the pulse signals and other positioning measurement information through filtering, sliding correlation and other operations. This method relies on existing infrastructure and requires the base station to be laid in advance, which cannot meet the exploration under unknown environmental conditions. The existing UWB radar-based mobile robot mapping and positioning has two kinds. One is a single UWB radar that uses synthetic aperture radar technology to image targets in the spatial frequency domain and obtains depth information of the target through wave vector decomposition, and then uses a multi-view contour extraction and stitching method to recover the complete planar map using multiple ultra-wideband radar sensors. This method only realizes the environment map construction, and does not realize the autonomous positioning of the robot; the other method is to use the ultra-wideband radar installed on both sides of the robot to obtain environmental information, filter the outliers after data preprocessing, and finally use the extended Kalman filter for positioning and landmark recognition, proving that the UWB radar can be used in the field of robot SLAM and has high positioning accuracy and environmental landmark recognition capability. This method linearizes the nonlinear system using the first-order Taylor, which to some extent solves the nonlinear problem, but the feature map established cannot be used for subsequent autonomous navigation. SUMMARY

[0005] To solve the above problems, the application provides a mobile robot positioning and mapping method based on UWB radar, which obtains environmental information through two UWB radars installed on the mobile robot, and uses an improved Rao-Blackwellised particle filter SLAM (RBPF-SLAM) method for mobile robot autonomous positioning and environmental map construction after data preprocessing. BRIEF DESCRIPTION OF DRAWINGS

[0006] Figure 1 is a main flowchart of a mobile robot positioning and mapping method based on UWB radar provided by an embodiment of the application;

[0007] Figure 2 is a schematic diagram of the working mode of the UWB radar carried by the mobile robot of the application;

[0008] Figure 3 is a schematic diagram of the principle of the triangular positioning method of the data preprocessing module in the embodiment of the application;

[0009] Figure 4 is a schematic diagram of the working process of the RBPF-SLAM based on the firefly algorithm optimization in the embodiment of the application;

[0010] Figure 5 is a comparison chart of the predicted trajectory and the real trajectory in the real scene in the embodiment of the application;

[0011] Figure 6 is a mapping effect diagram in the real scene in the embodiment of the application; DETAILED DESCRIPTION

[0012] The application will be further described below with reference to the drawings. The following embodiments are only used to more clearly illustrate the technical solutions of the application, and cannot be used to limit the protection scope of the application.

[0013] As shown in the figure, a mobile robot positioning and mapping method based on UWB radar comprises the following steps: Figure 1

[0014] First step, sampling. Start the mobile robot platform based on the ROS system (robot operating system), as shown in the figure, the chassis drive controller of the platform is responsible for receiving the odometer information; two ultra-wideband radars carried by the platform can collect the environmental information around the robot, and realize the positioning of the robot itself and the surrounding objects. Figure 2

[0015] Second step, process the original time domain data collected by the ultra-wideband radar, and then solve the distance and angle corresponding to each frame of data. The collected ultra-wideband original radar signal is a channel impulse response, and a two-dimensional data matrix composed of fast time dimension and slow time dimension is formed; first, perform timestamp alignment processing based on linear interpolation on the original radar signal and the odometer data of the robot platform, and then complete the following signal preprocessing and peak detection operations in turn:

[0016] 1. Subtract the background noise from the time-aligned radar signal, and then perform signal amplitude normalization processing;

[0017] 2. Input the normalized data into the basic statistical filtering module to remove all signal components below the average amplitude (i.e. set the amplitude of this part to 0); 3. Introduce the Savitzky-Golay filter to perform local polynomial fitting on the data, which suppresses high-frequency noise while retaining key peak characteristics; 4. Implement an amplitude-based adaptive peak detection algorithm on the filtered signal, the specific steps are:

[0018] (1) Set the maximum window size of the algorithm (where N is the length of the one-dimensional signal sequence);

[0019] (2) For each scale k, check the sampling point x i of position i (satisfying i>k and i+k≤N) in the signal sequence point by point, to determine whether it satisfies x i >x​​i -k and x i ≥ x i local maximum condition of +k;

[0020] (3) Count the number of local peaks that meet the conditions at each scale, and select the scale with the most peaks as the optimal scale k*;

[0021] (4) Traverse all scales from 1 to k, and accumulate the peak scores of each sampling point. If the score of a certain sampling point is equal to k (i.e., the point is identified as a local maximum at all scales), it is determined as a final peak;

[0022] (5) Output the position index corresponding to all final peaks, and calculate the distance corresponding to each frame of radar data based on the index.

[0023] The third step, as shown in Figure 3 , uses two ultra-wideband radars with known spatial positions, combined with the relative distance of the target calculated above, to complete the calculation of the target spatial coordinates using a triangular positioning method, as follows:

[0024] (1) Install two radar modules A and B on the sides of the robot, with a distance d between the two radars, and a field of view angle θ (e.g., 60°) for a single radar. The upper limit of the system's effective range is L, and the distances λ1 and λ2 between the target and the two radars must satisfy λ1, λ2 < L.

[0025] (2) First, calculate the angle constraint threshold:

[0026]

[0027] (3) To ensure that the target is within the common viewing range of the two radars, it is necessary to verify whether the direction angles a and b of the target relative to the two radars satisfy:

[0028] If the above conditions are not met, it is determined that the target is located in an unobservable area, and the system will discard this observation to improve robustness.

[0029] (4) To integrate the distance observation into the SLAM system, it needs to be converted to polar coordinate form: Let s be the translation bias of the radar relative to the robot center, then the polar coordinate distance h and azimuth angle of the target relative to the robot are calculated by the following formula:

[0030]

[0031] The fourth step is to run the RBPF-SLAM optimized based on the firefly algorithm with the collected environmental information as input. The specific process of the algorithm is shown in Figure 4 . Specifically as follows:

[0032] (1) Initial pose estimation: estimate the initial pose of the robot at the current time based on the robot's pose at the previous time with the odometry control information u t-1 , estimate the initial pose of the robot at the current time.

[0033] (2) Scan matching: execute a scan matching algorithm to complete the matching of the current observation data and the map.

[0034] (3) Particle weight update: sample particles in the optimization proposal distribution and update the weights of the current particles.

[0035] (4) Particle set partitioning: divide the particle set into high-likelihood and low-likelihood regions based on the weights of the current particles:

[0036] The high-likelihood region satisfies

[0037] The low-likelihood region satisfies

[0038] (5) High-likelihood region particle number judgment: judge whether the number of particles n in the high-likelihood region is greater than the threshold N threshold :

[0039] If n > N threshold , execute (6);

[0040] If n ≤ N threshold , jump to (9).

[0041] (6) Attraction force calculation: calculate the global optimal value pbest k , and based on the distance r i between particle i and pbest k , calculate the attraction degree between particle i and pbest k using the formula

[0042] .

[0043] (7) Particle state update: update the state information of the particle based on the improved pose update formula.

[0044] (8) Glowworm algorithm iteration judgment: update the global optimal value and judge whether the iteration number or termination threshold of the glowworm algorithm is reached: if reached, stop optimization; if not reached, return to (6) to continue iteration.

[0045] (9) Particle weight normalization: calculate the optimized particle weight and normalize it.

[0046] (10) Particle resampling: to improve the particle degradation phenomenon, when the effective particle number N effWhen the number of particles is less than 1 / 2 of the total number N of particles, a resampling operation is performed.

[0047] To more clearly verify the technical effects of the present application, the trajectory generated by the laser SLAM open source scheme Cartographer algorithm is selected as the reference true value for evaluating the positioning accuracy, the estimated pose output by the present application is aligned with the true trajectory by the iterative closest point (ICP) algorithm, and finally the root mean square (RMS) absolute trajectory error (ATE) is taken as the accuracy evaluation index.

[0048] The experimental environment is selected as a floor elevator corridor, and the number of particles is set to 20 and the map resolution is set to 5 cm during the experiment. The trajectory prediction Figure 5 and mapping effect of the present application Figure 6 can be intuitively presented by the drawings: the root mean square error (RMSE) of the motion trajectory of the robot is 3.15 cm, by fusing the UWB technology and the optimized particle filter, the present application not only overcomes the inherent limitations of the traditional odometry method, but also realizes high positioning accuracy and reliability in the experimental environment, and good mapping effect is obtained.

[0049] For ordinary skilled persons in the art, the specific embodiments are only exemplary descriptions of the present application, and obviously the specific implementation of the present application is not limited by the above method. As long as various non-essential improvements are made by using the method concept and technical solution of the present application, or the concept and technical solution of the present application is directly applied to other occasions without improvement, they are all within the protection scope of the present application.

Claims

1. A method for localization and mapping of a mobile robot based on UWB radar, characterized in that, Includes the following steps: (1) Sampling: Start the mobile robot platform based on the ROS system, collect the raw time domain data of the surrounding environment through the two ultra-wideband radars on the platform, and receive the odometer information through the chassis drive controller; (2) Radar signal processing: The original time domain data and odometer data are aligned by linear interpolation timestamp, and then subjected to background noise reduction, amplitude normalization, Savitzky-Golay filtering and adaptive peak detection. The distance corresponding to each frame of radar data is calculated. The adaptive peak detection determines the final peak value by setting the maximum window size, multi-scale local maximum test, optimal scale selection and peak score accumulation. (3) Target coordinate calculation: Based on the known spatial positions of the two radars and the calculated relative distance between the target, the triangulation method is used. After verification of the visible range and polar coordinate transformation, the polar coordinate distance and azimuth of the target relative to the robot are obtained. (4) Optimize RBPF-SLAM execution: sequentially execute initial pose estimation, scan matching, particle weight update, particle set partitioning, high likelihood region particle optimization (calculate attraction based on firefly algorithm, update particle state and iterate), particle weight normalization, resampling and map update to complete autonomous localization and environmental map construction.

2. The method according to claim 1, characterized in that, The maximum window size for adaptive peak detection in step (2) N is the length of the one-dimensional signal sequence; the optimal scale is the scale with the most local peaks among all scales, and the final peak is the sampling point with a score equal to k.

3. The method according to claim 1, characterized in that, In step (3), two radars are installed on both sides of the robot with a distance of d and a field of view of θ. The distances λ1 and λ2 between the target and the radar satisfy λ1, λ2 < the upper limit of the effective ranging of the system L. The visible range is verified by the minimum constraint conditions of the angle constraint threshold π-θ and the direction angles a and b. If the conditions are not met, the observation is discarded.

4. The method according to claim 1, characterized in that, Polar coordinate distance in step (3): Azimuth: This represents the translational deviation of the radar relative to the center of the robot.

5. The method according to claim 1, characterized in that, In step (4), the particle set is divided into high likelihood regions (T) according to weight. (i) >Δ) and low likelihood region (T) (i) <Δ), the number of particles in the high likelihood region n > the threshold N threshold At that time, the attraction level is calculated using the firefly algorithm: And optimize particle states.

6. The method according to claim 1 or 5, characterized in that, Δ takes values ​​between 0.5 and 0.7, and the number of particles N = 20. threshold The values ​​are 12-18, β0 is 0.8-1.0, and the number of iterations of the firefly algorithm is 15-25.

7. The method according to claim 1, characterized in that, The radar field of view θ = 60°, the map resolution is 5cm, and the positioning accuracy is RMSE = 3.15cm after ICP alignment, with the Cartographer algorithm trajectory as the reference true value.

8. A mobile robot SLAM system, characterized in that, It includes two UWB radars, an odometer, a chassis drive controller, a processor, and a storage medium, wherein the processor executes a computer program to implement the method described in any one of claims 1-7.

9. A computer-readable storage medium, characterized in that, The device contains a computer program that, when executed by a processor, implements the method described in any one of claims 1-7.

Images