Graphical efficient navigation positioning method and system based on multi-sensor fusion

Abstract

The invention provides a graphical high-efficiency navigation positioning method and system based on multi-sensor fusion, a high-precision grid map is quickly generated through a pre-drawn vector map, or a known grid map is directly used for navigation, so that the navigation efficiency is improved. Meanwhile, an improved error Kalman filtering algorithm for dynamically adjusting covariance of process noise and observation noise is designed and is used for high-precision fusion positioning of UWB / IMU. Besides, local path planning and dynamic obstacle avoidance are performed in combination with a low-cost one-dimensional laser radar, so that the navigation precision, real-time performance and robustness of the system are remarkably improved, the hardware cost and the calculation complexity are effectively reduced, and an efficient and reliable solution is provided for 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, and specifically to a method and system for efficient navigation and positioning with a map based on multi-sensor fusion. Background Art

[0002] In special scenarios such as cliffs and vertical walls, it is difficult for lidar and cameras to generate high-precision point cloud maps, and traditional GPS navigation and positioning methods cannot provide effective solutions in the vertical direction. For the above scenarios, as well as indoor environments with geometric rules and easy-to-draw maps, the efficiency of the navigation and positioning system can be significantly improved by pre-drawing maps. In particular, navigation methods based on vector maps can be quickly converted to high-precision raster maps, while significantly reducing the time and hardware costs required to build maps. Compared with traditional methods that rely on point cloud data, navigation and positioning solutions with maps have more advantages in these specific application scenarios.

[0003] Although the existing map-based navigation and positioning solutions have good practicality, they also have obvious defects. For example, the lidar-based solution requires high-performance hardware support to process massive point cloud data, which places high demands on computing resources and computing power. In addition, a large amount of point cloud data will take up hard disk storage space. The real-time response efficiency is also affected by the delay in data processing, and it is difficult to meet the high-frequency positioning requirements in dynamic environments. In contrast, the fusion positioning solution that combines ultra-wideband (UWB) and inertial measurement unit (IMU) can overcome these shortcomings. UWB has high-precision ranging capabilities, and IMU can supplement attitude information. The combination of the two can achieve real-time and high-precision positioning under the conditions of small data volume and high computing efficiency.

[0004] Although UWB / IMU fusion positioning has the advantages of high precision and real-time performance, its application in the field of navigation and positioning is still not widespread, mainly because of the lack of map information assistance. The current UWB / IMU fusion positioning method has not fully considered the complex factors in the dynamic environment in algorithm design. For example, the dynamic adjustment ability of the noise model is limited, and it is difficult to adapt to the fluctuation of sensor performance.

[0005] Therefore, it is urgent to improve the existing technology. Summary of the invention

[0006] Aiming at the problem that it is difficult for laser radar and camera to construct high-precision point cloud maps for positioning in special environments such as cliffs and vertical walls, and the limitation that traditional GPS positioning methods cannot provide effective navigation in the vertical direction, and the existing UWB / IMU fusion positioning scheme is susceptible to IMU cumulative errors and UWB non-line-of-sight and multipath effects, and lacks map information support, resulting in insufficient positioning accuracy and robustness challenges, the present invention proposes a method and system for efficient navigation and positioning with a map based on multi-sensor fusion. This method quickly generates a high-precision navigation grid map by pre-drawing a vector map, and combines an improved error Kalman filter algorithm to dynamically adjust the process noise and observation noise covariance, thereby improving the accuracy and robustness of UWB / IMU fusion positioning. At the same time, a low-cost one-dimensional laser radar is used to achieve 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 the present invention to solve its technical problem is:

[0008] A method for efficient navigation and positioning with a map based on multi-sensor fusion, comprising the following steps:

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

[0010]

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

[0012] S2: Combine UWB ranging data and IMU inertial measurement data, use improved error Kalman filter for positioning, and obtain the current coordinates (x, y) of the target;

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

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

[0015] Among them, f(n) is the comprehensive 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 the target point (x goal ,y goal) as a heuristic estimate:

[0017]

[0018] Expand nodes step by step and update the path until the optimal path is found;

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

[0020] S5: When the obstacle completely blocks the path to the target point, the global map is corrected and the A* replanning mechanism is enabled to adjust the global path;

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

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

[0023]

[0024] Among them, θ current is the current robot heading angle;

[0025] Control speed v and angular velocity ω:

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

[0027] Where k is the proportionality coefficient, v max is the maximum line speed;

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

[0029] S41: Determine the current target point (x) according to the global path goal ,y goal ), set the scanning angle range of the laser radar to [-θ max ,θ max ], divided into n directions, the distance measurement value in each direction is d i :

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

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

[0032] S42: Setting obstacle detection conditions, that is, the position of the obstacle is:

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

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

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

[0036]

[0037] Use the dynamic window to get the speed range that the robot can achieve per unit time:

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

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

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

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

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

[0043] ① Cost of target deviation:

[0044]

[0045] Among them, the prediction point (x pred ,y pred ) is derived using the current velocity;

[0046] ②Obstacle avoidance cost:

[0047]

[0048] Among them, d min is the minimum obstacle distance in the velocity direction;

[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 movement and update its position;

[0054] The process of improving the error Kalman filter for positioning includes the following steps:

[0055] S21: Set the system status to:

[0056]

[0057] Where p = [x, y] T is the position, v=[v x ,v y ] T is the speed, q=[q ω ,q x ,q y ] T is the attitude quaternion, b a and b g is the deviation of IMU acceleration and gyroscope;

[0058] The error state vector is δx:

[0059]

[0060] Among them, δθ is the small angle error approximation;

[0061] S22: Use IMU data for prediction, the motion equation is:

[0062]

[0063] Among them, R(q) is the rotation matrix calculated by quaternion, Ω(ω) is the attitude change matrix, g is the gravity acceleration vector, and a imu and ω imu is the IMU acceleration and heading angle, n a and ng is the IMU noise;

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

[0065]

[0066] Among them, F is the state transfer matrix, G is the noise coupling matrix, and w is the process noise;

[0067] S23: Use the deep learning prediction model to predict the UWB real distance measurement value between each base station and the positioning tag, use the predicted real distance measurement value to perform least squares positioning, and obtain the UWB measurement position z UWB , the observation model is:

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

[0069] Among them, n UWB is the UWB measurement noise;

[0070] The error observation equation is obtained as:

[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 noise models:

[0073]

[0074] Among them, Q nominal is the nominal process noise covariance matrix of the IMU, R nominal is the nominal observation noise covariance matrix of UWB, I UWB is the UWB signal quality indicator, S IMU is the IMU stability indicator, β 1 and β 2 is the adjustment factor, I th and S th They are the UWB measurement threshold and the stability threshold of IMU data variance respectively;

[0075] S25: Data fusion, by expanding the state vector, the UWB and IMU data are fused:

[0076] State prediction based on IMU:

[0077]

[0078] Status updates 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 function and The specific implementation steps are as follows:

[0083] S241: Use I UWB It is a signal strength evaluation indicator, which is quantified by the received signal strength indication RSSI and the signal-to-noise ratio SNR:

[0084]

[0085] Among them, signal-to-noise ratio SNR = 10log 10 (P signal / P noise ), P signal and P noise are signal power and noise power, RSSI max The maximum RSSI value for reference is used for normalization;

[0086] S242: When I UWB Below threshold I th , indicating that UWB measurement is unreliable and the observation noise covariance needs to be increased:

[0087]

[0088] S243: Use S IMU It is the stability evaluation index of IMU data, which is obtained by calculating the short-term variance of acceleration and angular velocity:

[0089]

[0090] in, is the acceleration variance, is the angular velocity variance, a i and are the current acceleration and the average acceleration of the process, ω i and They are the current angular velocity and the average angular velocity of the process;

[0091] S244: According to S IMU Adjust process noise covariance:

[0092]

[0093] Among them, α is the adjustment coefficient, S max is the maximum allowable variance value;

[0094] Furthermore, the said efficient navigation and positioning method with map can directly import the raster map drawn by the laser radar and camera, 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, wherein N≥3 in two-dimensional positioning and N≥4 in three-dimensional positioning. In two-dimensional space, the positions of every three UWB base stations participating in the positioning solution cannot be collinear, and in three-dimensional space, the positions of every four UWB base stations participating in the positioning solution cannot be coplanar.

[0096] The IMU is installed on the UWB tag to be located and is firmly connected to the UWB tag;

[0097] In addition, the present invention also provides a high-efficiency navigation and positioning system with a map 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 collects 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 the data from the UWB and IMU sensors by improving the error Kalman filter 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. The global path planning plans the shortest path from the starting point to the target point on a known map, and the local obstacle avoidance uses the real-time point cloud data of the laser radar 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 information and speed information provided by the multi-sensor fusion module. By solving the current precise position and attitude, the control system guides the mobile body to perform precise navigation. In addition, the module performs attitude compensation and adjustment according to the real-time positioning results.

[0103] The system control and feedback module coordinates and controls the work of each module and provides navigation information and status feedback to users or other systems;

[0104] The communication and data transmission module is responsible for the communication between the modules inside the system and the connection between the system and external devices;

[0105] The beneficial effects of the present invention are mainly manifested in:

[0106] (1) Use existing maps for navigation and positioning, especially for complex scenes where it is difficult for lidar and cameras to obtain accurate point cloud maps, and for scenes where scene maps are easy to draw. By parsing self-built maps or importing existing raster maps, data conversion is more efficient and tedious map building steps are avoided. 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) The A* algorithm is used to implement global path planning, and low-cost one-dimensional laser radar is used to perform local path planning, which can be dynamically adjusted 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 to deeply integrate UWB ranging and IMU inertial measurement data, and improve 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 and effectively reduce positioning error fluctuations. Compared with the positioning solution involving lidar, it has lower requirements on hardware computing power and faster real-time response. BRIEF DESCRIPTION OF THE DRAWINGS

[0109] Figure 1 It is a flow chart of the method for efficient navigation and positioning with a map based on multi-sensor fusion of the present invention;

[0110] Figure 2 is a flow chart of the present invention using laser radar for local path planning and obstacle avoidance;

[0111] Figure 3It is a flow chart of the present invention using improved error Kalman filtering to perform UWB / IMU fusion positioning;

[0112] Figure 4 The present invention uses the function and Flowchart for dynamically adjusting process noise covariance Q and observation noise covariance R;

[0113] Figure 5 It is a schematic diagram of the module composition of the high-efficiency navigation and positioning system with a map based on multi-sensor fusion of the present invention. DETAILED DESCRIPTION

[0114] In order to facilitate the understanding of the present invention, the present invention will be described more comprehensively and meticulously below in conjunction with the accompanying drawings and preferred embodiments of the present invention, but the protection scope of the present invention is not limited to the following specific embodiments.

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

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

[0117]

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

[0119] S2: Combine UWB ranging data and IMU inertial measurement data, use improved error Kalman filter for positioning, and obtain the current coordinates (x, y) of the target;

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

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

[0122] Among them, f(n) is the comprehensive 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 the target point (x goal ,y goal ) as a heuristic estimate:

[0124]

[0125] Expand nodes step by step and update the path until the optimal path is found;

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

[0127] S5: When the obstacle completely blocks the path to the target point, the global map is corrected and the A* replanning mechanism is enabled to adjust the global path;

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

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

[0130]

[0131] Among them, θ current is the current robot heading angle;

[0132] Control speed v and angular velocity ω:

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

[0134] Where k is the proportionality coefficient, v max is the maximum line speed;

[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) according to the global path goal ,y goal ), set the scanning angle range of the laser radar to [-θ max ,θ max ], divided into n directions, the distance measurement value in each direction is d i :

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

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

[0139] S42: Setting obstacle detection conditions, that is, the position of the obstacle is:

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

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

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

[0143]

[0144] Use the dynamic window to get the speed range that the robot can achieve per unit time:

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

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

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

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

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

[0150] ① Cost of target deviation:

[0151]

[0152] Among them, the prediction point (x pred ,y pred ) is derived using the current velocity;

[0153] ②Obstacle avoidance cost:

[0154]

[0155] Among them, d min is the minimum obstacle distance in the velocity direction;

[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 movement and update its position;

[0161] like Figure 3 As shown, the process of improving the error Kalman filter for positioning includes the following steps:

[0162] S21: Set the system status to:

[0163]

[0164] Where p = [x, y] T is the position, v=[v x ,v y ] T is the speed, q=[q ω ,q x ,q y ] T is the attitude quaternion, b a and b g is the deviation of IMU acceleration and gyroscope;

[0165] The error state vector is δx:

[0166]

[0167] Among them, δθ is the small angle error approximation;

[0168] S22: Use IMU data for prediction, the motion equation is:

[0169]

[0170] Among them, R(q) is the rotation matrix calculated by quaternion, Ω(ω) is the attitude change matrix, g is the gravity acceleration vector, and a imu and ω imuis the IMU acceleration and heading angle, n a and n g is the IMU noise;

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

[0172]

[0173] Among them, F is the state transfer matrix, G is the noise coupling matrix, and w is the process noise;

[0174] S23: Use the deep learning prediction model to predict the UWB real distance measurement value between each base station and the positioning tag, use the predicted real distance measurement value to perform least squares positioning, and obtain the UWB measurement position z UWB , the observation model is:

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

[0176] Among them, n UWB is the UWB measurement noise;

[0177] The error observation equation is obtained as:

[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 noise models:

[0180]

[0181] Among them, Q nominal is the nominal process noise covariance matrix of the IMU, R nominal is the nominal observation noise covariance matrix of UWB, I UWB is the UWB signal quality indicator, S IMU is the IMU stability indicator, β 1 and β 2 is the adjustment factor, I th and S th They are the UWB measurement threshold and the stability threshold of IMU data variance respectively;

[0182] S25: Data fusion, by expanding the state vector, the UWB and IMU data are fused:

[0183] State prediction based on IMU:

[0184]

[0185] Status updates 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 function and The specific implementation steps are as follows:

[0190] S241: Use I UWB It is a signal strength evaluation indicator, which is quantified by the received signal strength indication RSSI and the signal-to-noise ratio SNR:

[0191]

[0192] Among them, signal-to-noise ratio SNR = 10log 10 (P signal / P noise ), P signal and P noise are signal power and noise power, RSSI max The maximum RSSI value for reference is used for normalization;

[0193] S242: When I UWB Below threshold I th , indicating that UWB measurement is unreliable and the observation noise covariance needs to be increased:

[0194]

[0195] S343: Use S IMU It is the stability evaluation index of IMU data, which is obtained by calculating the short-term variance of acceleration and angular velocity:

[0196]

[0197] in, is the acceleration variance, is the angular velocity variance, a i and are the current acceleration and the average acceleration of the process, ωi and They are the current angular velocity and the average angular velocity of the process;

[0198] S244: According to S IMU Adjust process noise covariance:

[0199]

[0200] Among them, α is the adjustment coefficient, S max is the maximum allowable variance value;

[0201] The efficient navigation and positioning method with maps can directly import the raster map drawn by the laser radar and camera, 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, wherein N≥3 in two-dimensional positioning and N≥4 in three-dimensional positioning. In two-dimensional space, the positions of every three UWB base stations participating in the positioning solution cannot be collinear, and in three-dimensional space, the positions of every four UWB base stations participating in the positioning solution cannot be coplanar.

[0203] The IMU is installed on the UWB tag to be located and is firmly connected to the UWB tag;

[0204] In addition, if Figure 5 As shown, another embodiment of the present invention further provides a map-based efficient 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 collects 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 the data from the UWB and IMU sensors by improving the error Kalman filter 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. The global path planning plans the shortest path from the starting point to the target point on a known map, and the local obstacle avoidance uses the real-time point cloud data of the laser radar 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 information and speed information provided by the multi-sensor fusion module. By solving the current precise position and attitude, the control system guides the mobile body to perform precise navigation. In addition, the module performs attitude compensation and adjustment according to the real-time positioning results.

[0210] The system control and feedback module coordinates and controls the work of each module and provides navigation information and status feedback to users or other systems;

[0211] The communication and data transmission module is responsible for the communication between the modules inside the system and the connection between the system and external devices;

[0212] With the aid of the teachings presented in the foregoing description and the associated drawings, many modifications and other embodiments of the present invention will occur to those skilled in the art to which the present invention pertains. Therefore, it is to be understood that the present 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 generic and descriptive sense only and not for limitation.

Claims

1. An efficient navigation and positioning method with images based on multi-sensor fusion, characterized by: The following steps are involved: S1: Load the pre-drawn map into the system for parsing, extract key information such as obstacles, passable areas and target points, and first convert the imported vector map into a raster map. Each raster represents a small area, and the state of each small area is represented by a binary value: Among them, M(i,j) represents the state of position (i,j) on the grid map; S2: Combine UWB ranging data and IMU inertial measurement data, use improved error Kalman filter for positioning, and obtain the current coordinates (x, y) of the target; S3: Combining the rasterized map and positioning results, the A* algorithm is used to plan the global path. First, the heuristic function is defined: f(n)=g(n)+h(n) (2) Among them, f(n) is the comprehensive 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 the target point (x goal ,y goal ) as a heuristic estimate: Expand nodes step by step and update the path until the optimal path is found; S4: Obtain obstacle information in real time through LiDAR, perform local path planning and obstacle avoidance; S5: When the obstacle completely blocks the path to the target point, the global map is corrected and the A* replanning mechanism is enabled to adjust the global path; S6: Adjust the robot's direction of travel through the path tracking algorithm; Calculate the angle Δθ between the target point and the current position and make adjustments: Among them, θ current is the current robot heading angle; Control speed v and angular velocity ω: v=v max , ω=k·Δθ (5) Where k is the proportionality coefficient, v max is the maximum line speed.

2. The method for efficient navigation and positioning based on multi-sensor fusion according to claim 1, characterized in that: The local path planning and obstacle avoidance comprises the following steps: S41: Determine the current target point (x) according to the global path goal ,y goal ), set the scanning angle range of the laser radar to [-θ max ,θ max ], divided into n directions, the distance measurement value in each direction is d i : d={d1,d2,...,d n }, d i ≥0 (6) Among them, d i is the distance measurement value in the i-th direction, and n is the total number of points measured; S42: Setting obstacle detection conditions, that is, the position of the obstacle is: (x obs ,y obs )=(x robot +d i cosθ i ,y robot +d i sinθ i ), d i <d threshold (7) Where, d threshold is the obstacle threshold distance of the lidar, (x robot ,y robot ) is the current position of the robot, θ i is the lidar scanning angle; S43: Local path planning using dynamic window method: First, establish the motion equation of the robot on the plane: x t+1 =x t +vcosθ i Δt, y t+1 =y t +vsinθ i Δt, (8) i t+1 =θ t +ωΔt. Use the dynamic window to get the speed range that the robot can achieve per unit time: v min ≤v≤v max , oh min ≤ω≤ω max (9) Among them, v min is the minimum linear velocity, ω min and ω max are the minimum and maximum angular velocities of the robot’s motion, respectively; The optimal speed pair (v, ω) is selected by the cost function G(v, ω): G(v,ω)=α·G goal (v,ω)+β·G obstacle (v,ω)+γ·G velocity (v,ω) (10) Second, set the target deviation and obstacle avoidance costs: ① Cost of target deviation: Among them, the prediction point (x pred ,y pred ) is derived using the current velocity; ②Obstacle avoidance cost: Among them, d min is the minimum obstacle distance in the velocity direction; ③Speed ​​cost: G velocity (v,ω)=v (13) Finally, calculate the velocity pair that minimizes G(v,ω): S44: Control the robot movement and update its position.

3. The method for efficient navigation and positioning based on multi-sensor fusion according to claim 1 is characterized in that: The process of improving the error Kalman filter for positioning includes the following steps: S21: Set the system status to: Where p = [x, y] T is the position, v=[v x ,v y ] T is the speed, q=[q ω ,q x ,q y ] T is the attitude quaternion, b a and b g is the deviation of IMU acceleration and gyroscope; The error state vector is δx: Among them, δθ is the small angle error approximation; S22: Use IMU data for prediction, the motion equation is: Among them, R(q) is the rotation matrix calculated by quaternion, Ω(ω) is the attitude change matrix, g is the gravity acceleration vector, and a imu and ω imu is the IMU acceleration and heading angle, n a and n g is the IMU noise; The linearized dynamic model of the error state is obtained: Among them, F is the state transfer matrix, G is the noise coupling matrix, and w is the process noise; S23: Use the deep learning prediction model to predict the UWB real distance measurement value between each base station and the positioning tag, use the predicted real distance measurement value to perform least squares positioning, and obtain the UWB measurement position z UWB , the observation model is: z UWB =p+n UWB (19) Among them, n UWB is the UWB measurement noise; The error observation equation is obtained as: δz UWB =H UWB δx+n UWB , H UWB =[I 3×3 0 3×12 ] (20) S24: Dynamically adjust the process noise covariance Q and the observation noise covariance R, and construct a function and Dynamically map signal characteristics to noise models: Among them, Q nominal is the nominal process noise covariance matrix of the IMU, R nominal is the nominal observation noise covariance matrix of UWB, I UWB is the UWB signal quality indicator, S IMU is the IMU stability index, β1 and β2 are adjustment coefficients, I th and S th They are the UWB measurement threshold and the stability threshold of IMU data variance respectively; S25: Data fusion, by expanding the state vector, the UWB and IMU data are fused: State prediction based on IMU: Status updates based on UWB: S26: Nonlinear observation processing, processing nonlinear observations through iterative optimization:

4. The method for efficient navigation and positioning based on multi-sensor fusion according to claim 3 is characterized by: In the step S34 of positioning by improving the error Kalman filter, the process noise covariance Q and the observation noise covariance R function are dynamically adjusted. and The specific implementation includes the following steps: S241: Use I UWB It is a signal strength evaluation indicator, which is quantified by the received signal strength indication RSSI and the signal-to-noise ratio SNR: Among them, signal-to-noise ratio SNR = 10log 10 (P signal / P noise ), P signal and P noise are signal power and noise power, RSSI max The maximum RSSI value for reference is used for normalization; S242: When I UWB Below threshold I th , indicating that UWB measurement is unreliable and the observation noise covariance needs to be increased: S243: Use S IMU It is the stability evaluation index of IMU data, which is obtained by calculating the short-term variance of acceleration and angular velocity: in, is the acceleration variance, is the angular velocity variance, a i and a are the current acceleration and the average acceleration of the process, ω i and They are the current angular velocity and the average angular velocity of the process; S244: According to S IMU Adjust process noise covariance: Among them, α is the adjustment coefficient, S max is the maximum allowable variance value.

5. The method for efficient navigation and positioning with images based on multi-sensor fusion according to claims 1 to 4 is characterized in that: The efficient navigation and positioning method with maps can directly import the raster map drawn by the laser radar and camera, and skip the map parsing step; 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, wherein N≥3 in two-dimensional positioning and N≥4 in three-dimensional positioning. In two-dimensional space, the positions of every three UWB base stations participating in the positioning solution cannot be collinear, and in three-dimensional space, the positions of every four UWB base stations participating in the positioning solution cannot be coplanar. The IMU is installed on the UWB tag to be located and is firmly connected to the UWB tag.

6. A highly efficient navigation and positioning system with a map based on multi-sensor fusion, using the highly efficient navigation and positioning method with a map as claimed in any one of claims 1 to 5, characterized in that: It includes sensor data acquisition module, data preprocessing module, feature extraction module, multi-sensor fusion module, path planning and obstacle avoidance module, position estimation and navigation module, system control and feedback module and communication and data transmission module; The sensor data acquisition module collects 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 the data from the UWB and IMU sensors by improving the error Kalman filter 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. The global path planning plans the shortest path from the starting point to the target point on a known map, and the local obstacle avoidance uses the real-time point cloud data of the laser radar to dynamically avoid obstacles during movement. The position estimation and navigation module performs real-time positioning estimation and navigation control based on the position information and speed information provided by the multi-sensor fusion module. By solving the current precise position and attitude, the control system guides the mobile body to perform precise navigation. In addition, the module performs attitude compensation and adjustment according to the real-time positioning results. The system control and feedback module coordinates and controls the work of each module and provides navigation information and status feedback to users or other systems; The communication and data transmission module is responsible for the communication between the modules inside the system and the connection between the system and external devices.

Images