A multi-sensor fusion-based efficient navigation positioning method and system with a map

Abstract

The application provides a multi-sensor fusion-based efficient navigation positioning method and system with a map, a high-precision grid map is quickly generated through a pre-drawn vector map, or a known grid map is directly used for navigation, and an improved error Kalman filtering algorithm for dynamically adjusting process noise and observation noise covariance is designed for high-precision fusion positioning of UWB / IMU. In addition, local path planning and dynamic obstacle avoidance are combined with low-cost one-dimensional laser radar, the navigation accuracy, real-time performance and robustness of the system are significantly improved, and the hardware cost and calculation complexity are effectively reduced, and an efficient and reliable solution is provided for the application of multi-sensor fusion navigation positioning in a complex environment.

Description

Technical Field

[0001] The invention relates to the field of indoor wireless positioning and navigation, specifically to a map-based efficient navigation and positioning method and system based on multi-sensor fusion. Background Technology

[0002] In challenging environments such as cliffs and vertical walls, LiDAR and cameras struggle to generate high-precision point cloud maps, while traditional GPS navigation and positioning methods fail to provide effective solutions in the vertical direction. For these scenarios, and in geometrically regular indoor environments where maps are easily drawn, pre-mapped systems can significantly improve efficiency. Vector map-based navigation methods, in particular, can be quickly converted into high-precision raster maps, drastically reducing the time and hardware costs required for self-map creation. Compared to traditional methods relying on point cloud data, map-based navigation and positioning solutions offer greater advantages in these specific application scenarios.

[0003] While existing map-based navigation and positioning solutions offer good practicality, they also have significant drawbacks. For example, LiDAR-based solutions require high-performance hardware to process massive amounts of point cloud data, placing high demands on computing resources and power. Furthermore, large amounts of point cloud data consume considerable hard drive storage space. Real-time response efficiency is also affected by data processing latency, making it difficult to meet the high-frequency positioning requirements in dynamic environments. In contrast, a fusion positioning solution combining ultra-wideband (UWB) and inertial measurement unit (IMU) can overcome these shortcomings. UWB provides high-precision ranging capabilities, while the IMU supplements attitude information; the combination enables real-time, high-precision positioning with small data volumes and high computational efficiency.

[0004] Despite the advantages of high accuracy and real-time performance in UWB / IMU fusion positioning, its application in navigation and positioning is still not widespread, mainly due to the lack of map information and the fact that current UWB / IMU fusion positioning methods have not fully considered the complex factors in dynamic environments in their algorithm design. For example, the dynamic adjustment capability of noise models is limited, making it difficult to adapt to fluctuations in sensor performance.

[0005] Therefore, there is an urgent need to improve existing technologies. Summary of the Invention

[0006] To address the challenges of constructing high-precision point cloud maps for positioning using lidar and cameras in challenging environments such as cliffs and vertical walls, and the limitations of traditional GPS positioning methods in providing effective navigation in the vertical direction, this invention proposes a map-based, efficient navigation and positioning method and system based on multi-sensor fusion. This method rapidly generates a high-precision navigation grid map by pre-drawing a vector map and combines this with an improved error Kalman filter algorithm to dynamically adjust the covariance of process noise and observation noise, thereby improving the accuracy and robustness of UWB / IMU fusion positioning. Furthermore, it utilizes a low-cost one-dimensional lidar for local path planning and dynamic obstacle avoidance, further reducing the cost of the navigation and positioning system and improving accuracy and robustness.

[0007] The technical solution adopted by this invention to solve its technical problem is:

[0008] A map-based efficient navigation and positioning method based on multi-sensor fusion includes the following steps:

[0009] S1: Load the pre-drawn map into the system for parsing, extracting key information such as obstacles, passable areas, and target points. First, convert the imported vector map into a raster map, where each raster represents a small area, and the state of each small area is represented by a binary value.

[0010]

[0011] Where M(i,j) represents the state of position (i,j) on the grid map;

[0012] S2: Combining UWB ranging data and IMU inertial measurement data, an improved error Kalman filter is used for positioning to obtain the target's current coordinates (x, y);

[0013] S3: Combining the raster map and positioning results, the A* algorithm is used to plan the global path. First, a heuristic function is defined:

[0014] f(n)=g(n)+h(n) (2)

[0015] Where f(n) is the total cost of the current node, g(n) is the actual cost from the starting point to the current node, and h(n) is the heuristic estimate from the current node to the target node;

[0016] h(n) uses the current point (x) n ,y n ) to target point (x goal ,y goalThe Euclidean distance of ) is used as a heuristic estimate:

[0017]

[0018] Gradually expand the nodes and update the path until the optimal path is found;

[0019] S4: Obtain obstacle information in real time through LiDAR, and perform local path planning and obstacle avoidance;

[0020] S5: When obstacles completely block the path to the target point, correct the global map and enable the A* replanning mechanism to adjust the global path;

[0021] S6: Adjust the robot's direction of travel using a path tracking algorithm;

[0022] Calculate the angle Δθ formed between the target point and the current position, and make adjustments accordingly:

[0023]

[0024] Where, θ current This is the current heading angle of the robot;

[0025] Controlling velocity v and angular velocity ω:

[0026] v = v max , ω=k·Δθ (5)

[0027] Where k is the proportionality coefficient, v max Maximum linear velocity;

[0028] The specific steps for local path planning and obstacle avoidance are as follows:

[0029] S41: Determine the current target point (x) based on the global path. goal ,y goal The scanning angle range of the lidar is set to [-θ]. max ,θ max The distance is divided into n directions, and the distance measurement value for each direction is d. i :

[0030] d = {d1, d2, ..., d} n}, d i ≥0 (6)

[0031] Where, d i is the distance measured in the i-th direction, and n is the total number of points measured;

[0032] S42: Set obstacle detection conditions, i.e., the location of the obstacle is:

[0033] (x obs ,yobs )=(x robot +d i cosθ i ,y robot +d i sinθ i ), d i <d threshold (7) Where, d threshold This is the obstacle threshold distance of the lidar, (x robot ,y robot θ is the robot's current position. i It is the scanning angle of the lidar;

[0034] S43: Local path planning using the dynamic window method:

[0035] First, establish the motion equations of the robot on the plane:

[0036]

[0037] Use a dynamic window to obtain the range of speeds that the robot can achieve per unit time:

[0038] v min ≤v≤v max , ω min ≤ω≤ω max (9)

[0039] Among them, v min It is the minimum linear velocity, ω min and ω max These are the minimum and maximum angular velocities of the robot's motion, respectively.

[0040] The optimal velocity pair (v,ω) is selected using the cost function G(v,ω):

[0041] G(v,ω)=α·G goal (v,ω)+β·G obstacle (v,ω)+γ·G velocity (v,ω) (10)

[0042] Secondly, set the costs for target deviation and obstacle avoidance:

[0043] ① Cost of deviation from target:

[0044]

[0045] Among them, the prediction point (x) pred ,y pred This was derived using the current speed;

[0046] ② Cost of obstacle avoidance:

[0047]

[0048] Where, d min It is the minimum obstacle distance in the direction of velocity;

[0049] ③Speed ​​cost:

[0050] G velocity (v,ω)=v (13)

[0051] Finally, calculate the velocity pair that minimizes G(v,ω):

[0052]

[0053] S44: Control the robot's movement and update its position;

[0054] The process of localization using the improved error Kalman filter includes the following steps:

[0055] S21: Assume the system state is:

[0056]

[0057] Where p = [x, y] T It is a position, v = [v x ,v y ] T It is velocity, q = [q ω ,q x ,q y ] T It is a posture quaternion, b a and b g It is the deviation between the IMU acceleration and the gyroscope;

[0058] The error state vector is δx:

[0059]

[0060] Where δθ is the small angle error approximation;

[0061] S22: Prediction using IMU data, the equation of motion is:

[0062]

[0063] Where R(q) is the rotation matrix calculated using quaternions, Ω(ω) is the attitude change matrix, g is the gravitational acceleration vector, and a imu and ω imu n represents the IMU acceleration and heading angle. a and n gThis is IMU noise;

[0064] The linearized dynamic model of the error state is obtained:

[0065]

[0066] Where F is the state transition matrix, G is the noise coupling matrix, and w is the process noise;

[0067] S23: Utilize a deep learning prediction model to predict the true UWB ranging value between each base station and the positioning tag, and use the predicted true ranging value to perform least-squares positioning to obtain the UWB measurement position z. UWB The observation model is:

[0068] z UWB =p+n UWB (19)

[0069] Where, n UWB It is UWB measurement noise;

[0070] The error observation equation is obtained as follows:

[0071] δz UWB =H UWB δx+n UWB H UWB =[I 3×3 0 3×12 (20)

[0072] S24: Dynamically adjust the process noise covariance Q and the observation noise covariance R, and construct a function. and Dynamically map signal characteristics to a noise model:

[0073]

[0074] Among them, Q nominal R is the nominal process noise covariance matrix of the IMU. nominal It is the nominal observation noise covariance matrix of UWB, I UWB It is a UWB signal quality indicator, S IMU These are IMU stability metrics, where β1 and β2 are adjustment coefficients. th and S th These are the UWB measurement threshold and the stability threshold for IMU data variance, respectively.

[0075] S25: Data fusion, which fuses UWB and IMU data by extending the state vector:

[0076] State prediction based on IMU:

[0077]

[0078] State updates are based on UWB:

[0079]

[0080] S26: Nonlinear observation processing, processing nonlinear observations through iterative optimization:

[0081]

[0082] Furthermore, the dynamic adjustment process noise covariance Q and observation noise covariance R functions and The specific implementation steps are as follows:

[0083] S241: Using I UWB Signal strength is evaluated using Received Signal Strength Indicator (RSSI) and Signal-to-Noise Ratio (SNR).

[0084]

[0085] Wherein, signal-to-noise ratio (SNR) = 10log 10 (P signal / P noise ), P signal and P noise These represent signal power and noise power, respectively, RSSI max The maximum RSSI value used as a reference for normalization;

[0086] S242: When I UWB Below the threshold I th This indicates that UWB measurements are unreliable and the observation noise covariance needs to be increased.

[0087]

[0088] S243: Using S IMU The stability evaluation metric for IMU data is obtained by calculating the short-time variance of acceleration and angular velocity:

[0089]

[0090] in, It is the variance of acceleration. It is the variance of angular velocity, a i and These are the current acceleration and the process average acceleration, ω. i and These are the current angular velocity and the process average angular velocity, respectively.

[0091] S244: According to SIMU Adjustment process noise covariance:

[0092]

[0093] Where α is the adjustment coefficient, S max It is the maximum permissible variance value;

[0094] Furthermore, the map-based efficient navigation and positioning method can directly import grid maps drawn by LiDAR and cameras, and skip the map parsing step;

[0095] In the improved error Kalman filter positioning process, at least N UWB base stations and 1 UWB tag are required to obtain distance information to participate in the least squares positioning solution. In the two-dimensional positioning, N≥3 and in the three-dimensional positioning, N≥4. In the two-dimensional space, the positions of every three UWB base stations participating in the positioning solution cannot be collinear, and in the three-dimensional space, the positions of every four UWB base stations participating in the positioning solution cannot be coplanar.

[0096] The IMU is mounted on top of the UWB tag to be located and is securely connected to the UWB tag;

[0097] In addition, the present invention also provides a map-based high-efficiency navigation and positioning system based on multi-sensor fusion, including a sensor data acquisition module, a data preprocessing module, a feature extraction module, a multi-sensor fusion module, a path planning and obstacle avoidance module, a position estimation and navigation module, a system control and feedback module, and a communication and data transmission module.

[0098] The sensor data acquisition module acquires raw data from UWB, IMU, and lidar sensors in real time; the UWB module provides distance information, the IMU module provides acceleration and angular velocity data, and the lidar module provides obstacle point cloud information.

[0099] The data preprocessing module filters and removes noise from the collected sensor data to ensure the stability and accuracy of subsequent data processing.

[0100] The multi-sensor fusion module fuses data from UWB and IMU sensors by improving error Kalman filtering to obtain more accurate location information. The module also includes noise covariance adjustment based on the error model.

[0101] The path planning and obstacle avoidance module performs global path planning and local obstacle avoidance based on the positioning information provided by the system. Global path planning calculates the shortest path from the starting point to the target point on the known map, while local obstacle avoidance uses real-time point cloud data from the lidar to dynamically avoid obstacles during movement.

[0102] The position estimation and navigation module performs real-time positioning estimation and navigation control based on the position and velocity information provided by the multi-sensor fusion module. By calculating the current precise position and attitude, the control system guides the moving body to perform precise navigation. In addition, the module performs attitude compensation and adjustment based on the real-time positioning results.

[0103] The system control and feedback module coordinates and controls the operation of each module and provides navigation information and status feedback to the user or other systems.

[0104] The communication and data transmission module is responsible for communication between various modules within the system and for connecting the system with external devices.

[0105] The beneficial effects of this invention are mainly reflected in:

[0106] (1) Navigation and positioning using existing maps is particularly suitable for complex scenarios where it is difficult for lidar and cameras to obtain accurate point cloud maps, as well as when scene maps are easy to draw. By parsing self-built maps or importing existing raster maps, data conversion is more efficient, avoiding tedious map building steps. In addition, relying on high-precision known maps can significantly improve navigation and positioning accuracy, while simplifying the implementation process and reducing the system's hardware dependence and computing costs;

[0107] (2) Global path planning is achieved by combining the A* algorithm and local path planning is performed by low-cost one-dimensional LiDAR. The system dynamically adjusts to deal with obstacles. In addition, a path replanning mechanism based on real-time point cloud environmental data is used to effectively solve the problem of obstacles completely blocking the path, ensuring the flexibility and reliability of navigation and adapting to the diverse needs of complex dynamic environments.

[0108] (3) An improved error Kalman filtering method is proposed, which deeply integrates UWB ranging and IMU inertial measurement data, and improves positioning accuracy and robustness by dynamically adjusting the function model of process noise and observation noise covariance. This method can adapt to changes in signal strength and sensor data stability, effectively reduce positioning error fluctuations, and has lower hardware computing power requirements and faster real-time response compared to positioning schemes involving lidar. Attached Figure Description

[0109] Figure 1 This is a flowchart of the graph-based efficient navigation and positioning method based on multi-sensor fusion of the present invention;

[0110] Figure 2 This is a flowchart of the invention using lidar for local path planning and obstacle avoidance;

[0111] Figure 3 This is a flowchart of the UWB / IMU fusion localization using improved error Kalman filtering in this invention;

[0112] Figure 4 This invention uses functions and Flowcharts for dynamically adjusting process noise covariance Q and observation noise covariance R;

[0113] Figure 5 This is a schematic diagram of the module composition of the high-efficiency navigation and positioning system based on multi-sensor fusion according to the present invention. Detailed Implementation

[0114] To facilitate understanding of the present invention, the present invention will be described more fully and in detail below with reference to the accompanying drawings and preferred embodiments, but the scope of protection of the present invention is not limited to the following specific embodiments.

[0115] like Figure 1 As shown in the figure, this embodiment of a map-based efficient navigation and positioning method based on multi-sensor fusion includes the following steps:

[0116] S1: Load the pre-drawn map into the system for parsing, extracting key information such as obstacles, passable areas, and target points. First, convert the imported vector map into a raster map, where each raster represents a small area, and the state of each small area is represented by a binary value.

[0117]

[0118] Where M(i,j) represents the state of position (i,j) on the grid map;

[0119] S2: Combining UWB ranging data and IMU inertial measurement data, an improved error Kalman filter is used for positioning to obtain the target's current coordinates (x, y);

[0120] S3: Combining the raster map and positioning results, the A* algorithm is used to plan the global path. First, a heuristic function is defined:

[0121] f(n)=g(n)+h(n) (2)

[0122] Where f(n) is the total cost of the current node, g(n) is the actual cost from the starting point to the current node, and h(n) is the heuristic estimate from the current node to the target node;

[0123] h(n) uses the current point (x) n ,y n ) to target point (x goal ,y goal The Euclidean distance of ) is used as a heuristic estimate:

[0124]

[0125] Gradually expand the nodes and update the path until the optimal path is found;

[0126] S4: Obtain obstacle information in real time through LiDAR, and perform local path planning and obstacle avoidance;

[0127] S5: When obstacles completely block the path to the target point, correct the global map and enable the A* replanning mechanism to adjust the global path;

[0128] S6: Adjust the robot's direction of travel using a path tracking algorithm;

[0129] Calculate the angle Δθ formed between the target point and the current position, and make adjustments accordingly:

[0130]

[0131] Where, θ current This is the current heading angle of the robot;

[0132] Controlling velocity v and angular velocity ω:

[0133] v = v max , ω=k·Δθ (5)

[0134] Where k is the proportionality coefficient, v max Maximum linear velocity;

[0135] like Figure 2 As shown, the specific steps of the local path planning and obstacle avoidance are as follows:

[0136] S41: Determine the current target point (x) based on the global path. goal ,y goal The scanning angle range of the lidar is set to [-θ]. max ,θ max The distance is divided into n directions, and the distance measurement value for each direction is d. i :

[0137] d = {d1, d2, ..., d} n}, d i ≥0 (6)

[0138] Where, d i is the distance measured in the i-th direction, and n is the total number of points measured;

[0139] S42: Set obstacle detection conditions, i.e., the location of the obstacle is:

[0140] (x obs ,y obs )=(x robot +d icosθ i ,y robot +d i sinθ i ), d i <d threshold (7) Where, d threshold This is the obstacle threshold distance of the lidar, (x robot ,y robot θ is the robot's current position. i It is the scanning angle of the lidar;

[0141] S43: Local path planning using the dynamic window method:

[0142] First, establish the motion equations of the robot on the plane:

[0143]

[0144] Use a dynamic window to obtain the range of speeds that the robot can achieve per unit time:

[0145] v min ≤v≤v max , ω min ≤ω≤ω max (9)

[0146] Among them, v min It is the minimum linear velocity, ω min and ω max These are the minimum and maximum angular velocities of the robot's motion, respectively.

[0147] The optimal velocity pair (v,ω) is selected using the cost function G(v,ω):

[0148] G(v,ω)=α·G goal (v,ω)+β·G obstacle (v,ω)+γ·G velocity (v,ω) (10)

[0149] Secondly, set the costs for target deviation and obstacle avoidance:

[0150] ① Cost of deviation from target:

[0151]

[0152] Among them, the prediction point (x) pred ,y pred This was derived using the current speed;

[0153] ② Cost of obstacle avoidance:

[0154]

[0155] Where, d min It is the minimum obstacle distance in the direction of velocity;

[0156] ③Speed ​​cost:

[0157] G velocity (v,ω)=v (13)

[0158] Finally, calculate the velocity pair that minimizes G(v,ω):

[0159]

[0160] S44: Control the robot's movement and update its position;

[0161] like Figure 3 As shown, the localization process using the improved error Kalman filter includes the following steps:

[0162] S21: Assume the system state is:

[0163]

[0164] Where p = [x, y] T It is a position, v = [v x ,v y ] T It is velocity, q = [q ω ,q x ,q y ] T It is a posture quaternion, b a and b g It is the deviation between the IMU acceleration and the gyroscope;

[0165] The error state vector is δx:

[0166]

[0167] Where δθ is the small angle error approximation;

[0168] S22: Prediction using IMU data, the equation of motion is:

[0169]

[0170] Where R(q) is the rotation matrix calculated using quaternions, Ω(ω) is the attitude change matrix, g is the gravitational acceleration vector, and a imu and ω imu n represents the IMU acceleration and heading angle. a and n g This is IMU noise;

[0171] The linearized dynamic model of the error state is obtained:

[0172]

[0173] Where F is the state transition matrix, G is the noise coupling matrix, and w is the process noise;

[0174] S23: Utilize a deep learning prediction model to predict the true UWB ranging value between each base station and the positioning tag, and use the predicted true ranging value to perform least-squares positioning to obtain the UWB measurement position z. UWB The observation model is:

[0175] z UWB =p+n UWB (19)

[0176] Where, n UWB It is UWB measurement noise;

[0177] The error observation equation is obtained as follows:

[0178] δz UWB =H UWB δx+n UWB H UWB =[I 3×3 0 3×12 (20)

[0179] S24: Dynamically adjust the process noise covariance Q and the observation noise covariance R, and construct a function. and Dynamically map signal characteristics to a noise model:

[0180]

[0181] Among them, Q nominal R is the nominal process noise covariance matrix of the IMU. nominal It is the nominal observation noise covariance matrix of UWB, I UWB It is a UWB signal quality indicator, S IMU These are IMU stability metrics, where β1 and β2 are adjustment coefficients. th and S th These are the UWB measurement threshold and the stability threshold for IMU data variance, respectively.

[0182] S25: Data fusion, which fuses UWB and IMU data by extending the state vector:

[0183] State prediction based on IMU:

[0184]

[0185] State updates are based on UWB:

[0186]

[0187] S26: Nonlinear observation processing, processing nonlinear observations through iterative optimization:

[0188]

[0189] like Figure 4 As shown, the dynamic adjustment process noise covariance Q and observation noise covariance R functions are... and The specific implementation steps are as follows:

[0190] S241: Using I UWB Signal strength is evaluated using Received Signal Strength Indicator (RSSI) and Signal-to-Noise Ratio (SNR).

[0191]

[0192] Wherein, signal-to-noise ratio (SNR) = 10log 10 (P signal / P noise ), P signal and P noise These represent signal power and noise power, respectively, RSSI max The maximum RSSI value used as a reference for normalization;

[0193] S242: When I UWB Below the threshold I th This indicates that UWB measurements are unreliable and the observation noise covariance needs to be increased.

[0194]

[0195] S343: Using S IMU The stability evaluation metric for IMU data is obtained by calculating the short-time variance of acceleration and angular velocity:

[0196]

[0197] in, It is the variance of acceleration. It is the variance of angular velocity, a i and These are the current acceleration and the process average acceleration, ω. i and These are the current angular velocity and the process average angular velocity, respectively.

[0198] S244: According to S IMUAdjustment process noise covariance:

[0199]

[0200] Where α is the adjustment coefficient, S max It is the maximum permissible variance value;

[0201] The map-based efficient navigation and positioning method can directly import grid maps drawn by LiDAR and cameras, and skip the map parsing step;

[0202] In the improved error Kalman filter positioning process, at least N UWB base stations and 1 UWB tag are required to obtain distance information to participate in the least squares positioning solution. In the two-dimensional positioning, N≥3 and in the three-dimensional positioning, N≥4. In the two-dimensional space, the positions of every three UWB base stations participating in the positioning solution cannot be collinear, and in the three-dimensional space, the positions of every four UWB base stations participating in the positioning solution cannot be coplanar.

[0203] The IMU is mounted on top of the UWB tag to be located and is securely connected to the UWB tag;

[0204] In addition, such as Figure 5 As shown, another embodiment of the present invention also provides a map-based high-efficiency navigation and positioning system based on multi-sensor fusion, preferably using the method described above. The system includes a sensor data acquisition module, a data preprocessing module, a feature extraction module, a multi-sensor fusion module, a path planning and obstacle avoidance module, a position estimation and navigation module, a system control and feedback module, and a communication and data transmission module.

[0205] The sensor data acquisition module acquires raw data from UWB, IMU, and lidar sensors in real time; the UWB module provides distance information, the IMU module provides acceleration and angular velocity data, and the lidar module provides obstacle point cloud information.

[0206] The data preprocessing module filters and removes noise from the collected sensor data to ensure the stability and accuracy of subsequent data processing.

[0207] The multi-sensor fusion module fuses data from UWB and IMU sensors by improving error Kalman filtering to obtain more accurate location information. The module also includes noise covariance adjustment based on the error model.

[0208] The path planning and obstacle avoidance module performs global path planning and local obstacle avoidance based on the positioning information provided by the system. Global path planning calculates the shortest path from the starting point to the target point on the known map, while local obstacle avoidance uses real-time point cloud data from the lidar to dynamically avoid obstacles during movement.

[0209] The position estimation and navigation module performs real-time positioning estimation and navigation control based on the position and velocity information provided by the multi-sensor fusion module. By calculating the current precise position and attitude, the control system guides the moving body to perform precise navigation. In addition, the module performs attitude compensation and adjustment based on the real-time positioning results.

[0210] The system control and feedback module coordinates and controls the operation of each module and provides navigation information and status feedback to the user or other systems.

[0211] The communication and data transmission module is responsible for communication between various modules within the system and for connecting the system with external devices.

[0212] With the aid of the teachings present in the foregoing description and related drawings, those skilled in the art will conceive of many modifications and other embodiments of the invention. Therefore, it is to be understood that the invention is not limited to the specific embodiments disclosed, and modifications and other embodiments are considered to be included within the scope of the appended claims. Although specific terms are used herein, they are used in a general and descriptive sense only and are not intended to be limiting.

Claims

1. A map-based efficient navigation and positioning method based on multi-sensor fusion, characterized in that: Includes the following steps: S1: Load the pre-drawn map into the system for parsing, extracting obstacles, passable areas, and target points. First, convert the imported vector map into a raster map, where each raster represents a small area, and the state of each small area is represented by a binary value. Where M(i,j) represents the state of position (i,j) on the grid map; S2: Combining UWB ranging data and IMU inertial measurement data, an improved error Kalman filter is used for positioning to obtain the target's current coordinates (x, y); The improved error Kalman filter constructs a function during the localization process. and Dynamically adjusting the process noise covariance Q and the observation noise covariance R includes the following steps: S241: Using I UWB Signal strength is evaluated using Received Signal Strength Indicator (RSSI) and Signal-to-Noise Ratio (SNR). Wherein, signal-to-noise ratio (SNR) = 10log 10 (P signal / P noise ), P signal and P noise These represent signal power and noise power, respectively, RSSI max The maximum RSSI value used as a reference for normalization; S242: When I UWB Below the threshold I th This indicates that UWB measurements are unreliable and the observation noise covariance needs to be increased. S243: Using S IMU The stability evaluation metric for IMU data is obtained by calculating the short-time variance of acceleration and angular velocity: in, It is the variance of acceleration. It is the variance of angular velocity, a i and These are the current acceleration and the process average acceleration, ω. i and These are the current angular velocity and the process average angular velocity, respectively. S244: According to S IMU Adjustment process noise covariance: Where α is the adjustment coefficient, S max It is the maximum permissible variance value; S3: Combining the raster map and positioning results, the A* algorithm is used to plan the global path. First, a heuristic function is defined: f(n)=g(n)+h(n) (6) Where f(n) is the total cost of the current node, g(n) is the actual cost from the starting point to the current node, and h(n) is the heuristic estimate from the current node to the target node; h(n) uses the current point (x) n ,y n ) to target point (x goal ,y goal The Euclidean distance of ) is used as a heuristic estimate: Gradually expand the nodes and update the path until the optimal path is found; S4: Obtain obstacle information in real time through LiDAR, and perform local path planning and obstacle avoidance; S5: When obstacles completely block the path to the target point, correct the global map and enable the A* replanning mechanism to adjust the global path; S6: Adjust the robot's direction of travel using a path tracking algorithm; Calculate the angle Δθ formed between the target point and the current position, and make adjustments accordingly: Where, θ current This is the current heading angle of the robot; Controlling velocity v and angular velocity ω: v=v max , ω=k·Δθ (9) Where k is the proportionality coefficient, v max This represents the maximum linear velocity.

2. The map-based efficient navigation and positioning method based on multi-sensor fusion as described in claim 1, characterized in that: The local path planning and obstacle avoidance includes the following steps: S41: Determine the current target point (x) based on the global path. goal ,y goal The scanning angle range of the lidar is set to [-θ]. max ,θ max The distance is divided into n directions, and the distance measurement value for each direction is d. i : d={d1,d2,...,d n }, d i ≥0 (10) Where, d i is the distance measured in the i-th direction, and n is the total number of points measured; S42: Set obstacle detection conditions, i.e., the location of the obstacle is: (x obs ,y obs )=(x robot +d i cosθ i ,y robot +d i sinθ i ), d i <d threshold (11) Where, d threshold This is the obstacle threshold distance of the lidar, (x robot ,y robot θ is the robot's current position. i It is the scanning angle of the lidar; S43: Local path planning using the dynamic window method: First, establish the motion equations of the robot on the plane: Use a dynamic window to obtain the range of speeds that the robot can achieve per unit time: v min ≤v≤v max , oh min ≤ω≤ω max (13) Among them, v min It is the minimum linear velocity, ω min and ω max These are the minimum and maximum angular velocities of the robot's motion, respectively. The optimal velocity pair (v,ω) is selected using the cost function G(v,ω): G(v,ω)=α·G goal (v,ω)+β·G obstacle (v,ω)+γ·G velocity (v,ω) (14) Secondly, set the costs for target deviation and obstacle avoidance: ① Cost of deviation from target: Among them, the prediction point (x) pred ,y pred This was derived using the current speed; ② Cost of obstacle avoidance: Where, d min It is the minimum obstacle distance in the direction of velocity; ③Speed ​​cost: G velocity (v,ω)=v (17) Finally, calculate the velocity pair that minimizes G(v,ω): S44: Control the robot's movement and update its position.

3. The map-based efficient navigation and positioning method based on multi-sensor fusion according to claim 1, characterized in that: The process of localization using the improved error Kalman filter includes the following steps: S21: Assume the system state is: Where p = [x, y] T It is a position, v = [v x ,v y ] T It is velocity, q = [q ω ,q x ,q y ] T It is a posture quaternion, b a and b g It is the deviation between the IMU acceleration and the gyroscope; The error state vector is δx: Where δθ is the small angle error approximation; S22: Prediction using IMU data, the equation of motion is: Where R(q) is the rotation matrix calculated using quaternions, Ω(ω) is the attitude change matrix, g is the gravitational acceleration vector, and a imu and ω imu n represents the IMU acceleration and heading angle. a and n g This is IMU noise; The linearized dynamic model of the error state is obtained: Where F is the state transition matrix, G is the noise coupling matrix, and w is the process noise; S23: Utilize a deep learning prediction model to predict the true UWB ranging value between each base station and the positioning tag, and use the predicted true ranging value to perform least-squares positioning to obtain the UWB measurement position z. UWB The observation model is: z UWB =p+n UWB (23) Where, n UWB It is UWB measurement noise; The error observation equation is obtained as follows: δz UWB =H UWB δx+n UWB , H UWB =[I 3×3 0 3×12 ] (24) S24: Dynamically adjust the process noise covariance Q and the observation noise covariance R, and construct a function. and Dynamically map signal characteristics to a noise model: Among them, Q nominal R is the nominal process noise covariance matrix of the IMU. nominal It is the nominal observation noise covariance matrix of UWB, I UWB It is a UWB signal quality indicator, S IMU These are IMU stability metrics, where β1 and β2 are adjustment coefficients. th and S th These are the UWB measurement threshold and the stability threshold for IMU data variance, respectively. S25: Data fusion, which fuses UWB and IMU data by extending the state vector: State prediction based on IMU: State updates are based on UWB: S26: Nonlinear observation processing, processing nonlinear observations through iterative optimization:

4. The map-based efficient navigation and positioning method based on multi-sensor fusion according to any one of claims 1 to 3, characterized in that: When the grid map drawn by the LiDAR and camera is directly imported into the map-based efficient navigation and positioning method, the map parsing step S1 is skipped; In the improved error Kalman filter positioning process, at least N UWB base stations and 1 UWB tag are required to obtain distance information to participate in the least squares positioning solution. In the two-dimensional positioning, N≥3 and in the three-dimensional positioning, N≥4. In the two-dimensional space, the positions of every three UWB base stations participating in the positioning solution cannot be collinear, and in the three-dimensional space, the positions of every four UWB base stations participating in the positioning solution cannot be coplanar. The IMU is mounted on top of the UWB tag to be located and is securely connected to the UWB tag.

5. A map-based high-efficiency navigation and positioning system based on multi-sensor fusion, employing the map-based high-efficiency navigation and positioning method as described in any one of claims 1 to 4, characterized in that: It includes a sensor data acquisition module, a data preprocessing module, a feature extraction module, a multi-sensor fusion module, a path planning and obstacle avoidance module, a position estimation and navigation module, a system control and feedback module, and a communication and data transmission module; The sensor data acquisition module acquires raw data from UWB, IMU, and lidar sensors in real time; the UWB module provides distance information, the IMU module provides acceleration and angular velocity data, and the lidar module provides obstacle point cloud information. The data preprocessing module filters and removes noise from the collected sensor data to ensure the stability and accuracy of subsequent data processing. The multi-sensor fusion module fuses data from UWB and IMU sensors by improving error Kalman filtering to obtain more accurate location information. The module also includes noise covariance adjustment based on the error model. The path planning and obstacle avoidance module performs global path planning and local obstacle avoidance based on the positioning information provided by the system. Global path planning calculates the shortest path from the starting point to the target point on the known map, while local obstacle avoidance uses real-time point cloud data from the lidar to dynamically avoid obstacles during movement. The position estimation and navigation module performs real-time positioning estimation and navigation control based on the position and velocity information provided by the multi-sensor fusion module. By calculating the current precise position and attitude, the control system guides the moving body to perform precise navigation. In addition, the module performs attitude compensation and adjustment based on the real-time positioning results. The system control and feedback module coordinates and controls the operation of each module and provides navigation information and status feedback to the user or other systems. The communication and data transmission module is responsible for communication between modules within the system and for connecting the system with external devices.

Images