Multi-sensor fusion robot high-precision positioning navigation system

Abstract

The invention belongs to the technical field of navigation, and particularly relates to a multi-sensor fusion robot high-precision positioning navigation system. Comprising a sensor array module, a data preprocessing module, a multi-source data fusion module, a dynamic environment modeling module and an autonomous positioning navigation control module, according to the multi-sensor fused high-precision robot positioning and navigation system disclosed by the invention, the high-precision positioning and navigation of the robot in a complex environment are realized by fusing data of various sensors and combining advanced data processing, fusion, modeling and control algorithms, and the system has a wide application prospect and remarkable technical advantages. The system is successfully applied to various mobile robot platforms such as an indoor cleaning robot, a storage AGV and a security patrol robot, the effectiveness, reliability and practicability of the system are fully proved, and powerful support is provided for development and application of the robot technology.

Description

Technical Field

[0001] The present invention relates to the field of navigation technology, and in particular to a multi-sensor fusion robot high-precision positioning and navigation system. Background Art

[0002] In the field of robotics applications, precise positioning and navigation are key technologies for achieving autonomous operation. Traditional robotic positioning and navigation systems often rely on a single sensor, such as using only lidar or visual sensors for positioning and navigation. However, single sensors have many limitations in complex environments. For example, lidar is prone to point cloud data loss or increased noise when faced with large-area mirror reflections or dusty environments, resulting in reduced positioning accuracy. Visual sensors, on the other hand, have difficulty extracting effective features in low-light or textureless environments, affecting navigation reliability. In addition, the amount of information obtained by a single sensor is limited, making it difficult to fully and meticulously perceive the environment, and unable to meet the robot's high-precision positioning and navigation needs in dynamic and complex environments.

[0003] Therefore, how to integrate the advantages of multiple sensors, overcome the limitations of a single sensor, and improve the accuracy and reliability of robot positioning and navigation has become an urgent problem to be solved. This paper aims to propose a multi-sensor fusion robot high-precision positioning and navigation system to meet the above challenges. Summary of the Invention

[0004] (1) Technical problems solved In view of the shortcomings of the existing technology, the present invention provides a multi-sensor fusion robot high-precision positioning and navigation system, which solves the problems raised in the above background technology.

[0005] (2) Technical solution In order to achieve the above-mentioned purpose, the present invention specifically adopts the following technical solutions: A multi-sensor fusion robot high-precision positioning and navigation system, including: A sensor array module, which includes an inertial measurement unit, a lidar, a binocular vision sensor, a wheel encoder, and a UWB positioning unit, and is used to obtain information about the robot's surrounding environment; A data preprocessing module, connected to the sensor array module, for filtering, converting data formats, and performing coordinate transformation processing on the data collected by the sensors; A multi-source data fusion module is connected to the data preprocessing module. The multi-source data fusion module uses a hybrid filtering algorithm to fuse multi-sensor data and output a high-precision pose estimate; A dynamic environment modeling module is connected to the multi-source data fusion module, and synchronously constructs an occupancy grid map and a topological map with semantic labels based on the fused robot position and posture information; The autonomous positioning and navigation control module is connected to the dynamic environment modeling module and generates a collision-free motion trajectory based on the map and posture.

[0006] Furthermore, the inertial measurement unit (IMU) collects the robot's angular velocity and linear acceleration in real time; the laser radar (LiDAR) generates three-dimensional point cloud data of the environment; the binocular vision sensor outputs RGB-D images and depth information; the wheel encoder measures wheel speed and travel distance; and the UWB positioning unit provides an absolute position reference through anchor point ranging.

[0007] Furthermore, the data preprocessing module adopts an adaptive filtering algorithm to estimate and suppress the noise in the sensor data in real time, and at the same time normalizes the data according to the measurement characteristics of the sensor to ensure that different sensor data are fused in the same coordinate system and data format. The coordinate system is a unified robot local coordinate system or a global map coordinate system, and the data format conversion includes converting the raw data of different sensors into a point cloud data format, image pixel coordinate format or distance angle information format commonly used within the system.

[0008] Furthermore, the multi-source data fusion module includes: Front-end pre-integration unit: pre-integrates IMU data and outputs pose increments Δp, Δv, Δq; Tightly coupled fusion unit: The pre-integration result, LiDAR point cloud matching residual, visual feature reprojection error, and UWB ranging value are input into the extended Kalman filter (EKF). The state vector is defined as: ; where b a， ,b g Zero bias for IMU accelerometer and gyroscope; Back-end optimization unit: uses a factor graph model to fuse closed-loop detection information and optimize the pose trajectory.

[0009] Furthermore, the dynamic environment modeling module performs: Occupancy grid map update: using logarithmic probability model: ; in, , is the prior probability log odds; Semantic map construction: Use convolutional neural networks (CNNs) to identify object categories in images and map labels to point clouds to generate semantic topology maps.

[0010] Furthermore, the autonomous decision-making control module includes: Global path planner: uses the improved A* algorithm, and the cost function is: ; where uncertainty(n) is the inverse of the confidence level of the grid map position; Local obstacle avoidance controller: Apply model predictive control (MPC) and optimize the following objectives: .

[0011] Furthermore, the improved A* algorithm includes a bidirectional search mechanism: Forward search: expand from the starting point to the target; Reverse search: expand from the target to the starting point; Convergence condition: When the distance between the forward node and the reverse node is d <dt hres Generate connection paths.

[0012] Furthermore, the system is deployed on a mobile robot platform and includes: Indoor cleaning robot: semantic map identifies "carpet" and "furniture" areas and adjusts cleaning strategies; Warehouse AGV: Use topological maps to plan the shortest path between shelves; Security patrol robot: realizes human-machine avoidance through dynamic obstacle detection.

[0013] Furthermore, it also includes an energy consumption optimization unit to reduce power consumption by: Sensor hierarchical wake-up mechanism: only IMU and encoder are enabled during low-speed motion; Computational load balancing: Positioning tasks are assigned to the GPU, while control tasks run on the CPU.

[0014] Furthermore, it also includes a map storage and sharing unit: Adopting incremental storage format, only saving map changes; Multi-robot system synchronizes semantic topology key nodes through wireless network.

[0015] The system includes a human-computer interaction interface, providing: Real-time pose visualization interface; Semantic map editing tools; Navigation mission command input terminal.

[0016] (3) Beneficial effects Compared with the existing technology, the present invention provides a multi-sensor fusion robot high-precision positioning and navigation system, which has the following beneficial effects: This invention, a multi-sensor fusion high-precision robot positioning and navigation system, achieves high-precision positioning and navigation in complex environments by fusing data from multiple sensors and combining advanced data processing, fusion, modeling, and control algorithms. This system has broad application prospects and significant technical advantages. The successful application of this system on various mobile robot platforms, including indoor cleaning robots, warehouse AGVs, and security patrol robots, has fully demonstrated its effectiveness, reliability, and practicality, providing strong support for the development and application of robotics technology. BRIEF DESCRIPTION OF THE DRAWINGS

[0017] Figure 1 Schematic diagram of the overall system of the present invention; Figure 2 is a schematic diagram of a sensor array module of the present invention; Figure 3 It is a schematic diagram of the process of the present invention. DETAILED DESCRIPTION

[0018] The following will clearly and completely describe the technical solutions in the embodiments of the present invention in conjunction with the accompanying drawings. Obviously, the described embodiments are only part of the embodiments of the present invention, not all of the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by ordinary technicians in this field without making creative efforts are within the scope of protection of the present invention.

[0019] Example like Figure 1 、 Figure 2 As shown, an embodiment of the present invention proposes a multi-sensor fusion robot high-precision positioning and navigation system, including: A sensor array module, which includes an inertial measurement unit, a lidar, a binocular vision sensor, a wheel encoder, and a UWB positioning unit, and is used to obtain information about the robot's surrounding environment; A data preprocessing module is connected to the sensor array module and is used to filter, convert data formats, and perform coordinate transformation on the data collected by the sensors to improve data accuracy and consistency; A multi-source data fusion module is connected to the data preprocessing module. The multi-source data fusion module uses a hybrid filtering algorithm to fuse multi-sensor data and output a high-precision pose estimate; A dynamic environment modeling module is connected to the multi-source data fusion module, and synchronously constructs an occupancy grid map and a topological map with semantic labels based on the fused robot position and posture information; The autonomous positioning and navigation control module is connected to the dynamic environment modeling module and generates a collision-free motion trajectory based on the map and posture.

[0020] In some embodiments, the inertial measurement unit (IMU) collects the robot's angular velocity and linear acceleration in real time; the laser radar (LiDAR) generates three-dimensional point cloud data of the environment; the binocular vision sensor outputs RGB-D images and depth information; the wheel encoder measures wheel speed and travel distance; and the UWB positioning unit provides an absolute position reference through anchor point ranging.

[0021] In the sensor array module: IMU sampling frequency ≥ 200 Hz, zero bias stability ≤ 0.1° / h; The vertical field of view of LiDAR is ≥30° and the angular resolution is ≤0.05°; The baseline distance of the binocular vision sensor is 10cm±0.5cm; The UWB positioning unit deploys at least 4 anchor nodes, and the ranging error is ≤10cm.

[0022] The laser radar is a multi-line laser radar with at least 16 laser emission and receiving channels. It can obtain point cloud data of the robot's surrounding environment with high precision and high density. The scanning range covers 360 degrees horizontally around the robot and a certain angle vertically. The vertical scanning angle range is not less than ±30 degrees.

[0023] The binocular vision sensor also includes at least two industrial-grade cameras with a resolution of no less than 1280×720 pixels and a frame rate of no less than 30fps. It has a wide-angle lens and autofocus function, and can stably obtain image information of the robot's surrounding environment under different lighting conditions. It can also assist the robot in positioning and environmental perception through image feature extraction and matching algorithms.

[0024] In some embodiments, the data preprocessing module uses an adaptive filtering algorithm to estimate and suppress the noise in the sensor data in real time, and at the same time normalizes the data according to the measurement characteristics of the sensor to ensure that different sensor data are fused in the same coordinate system and data format. The coordinate system is a unified robot local coordinate system or a global map coordinate system, and the data format conversion includes converting the raw data of different sensors into a point cloud data format, image pixel coordinate format or distance angle information format commonly used within the system.

[0025] In some embodiments, the multi-source data fusion module performs a joint probability density estimation on the position, speed, posture, and environmental feature information provided by each sensor, and uses Bayes' theorem to continuously update the posterior probability of the robot state to improve positioning accuracy and reliability. The fusion algorithm can dynamically adjust the weight of each sensor data in the fusion process according to the performance and environmental conditions of different sensors to ensure the optimality of the fusion result.

[0026] In some embodiments, the multi-source data fusion module includes: Front-end pre-integration unit: pre-integrates IMU data and outputs pose increments Δp, Δv, Δq; Tightly coupled fusion unit: The pre-integration result, LiDAR point cloud matching residual, visual feature reprojection error, and UWB ranging value are input into the extended Kalman filter (EKF). The state vector is defined as: ; where b a， ,b g Zero bias for IMU accelerometer and gyroscope; Back-end optimization unit: uses a factor graph model to fuse closed-loop detection information and optimize the pose trajectory.

[0027] In the tightly coupled fusion unit, the observation model includes: LiDAR observation item: Calculate the point cloud matching residual using the ICP algorithm: ; Visual observation item: reprojection error based on ORB feature points: ; UWB observation term: ranging equation: .

[0028] In some embodiments, the dynamic environment modeling module performs: Occupancy grid map update: using logarithmic probability model: ; in, , is the prior probability log odds.

[0029] Semantic map construction: Use convolutional neural networks (CNNs) to identify object categories in images and map labels to point clouds to generate semantic topology maps.

[0030] The semantic topology graph includes a traversable area analysis unit, which is divided based on semantic labels: Ground passable area (marked as A freeAfree ); Dynamic obstacle area (marked as A dynamic ); Structured channel area (marked as A doorAdoor ,A elevator ).

[0031] The semantic map construction adopts a multi-stage fusion strategy: Stage 1: The YOLOv7 network detects the bounding boxes of objects in the RGB image; Stage 2: DeepLabv3+ network generates pixel-level semantic segmentation masks; Stage 3: Project the semantic labels onto the point cloud to generate a voxel map with category labels.

[0032] In some embodiments, the autonomous decision control module includes: Global path planner: uses the improved A* algorithm, and the cost function is: ; where uncertainty(n) is the inverse of the confidence level of the grid map position; Local obstacle avoidance controller: Apply model predictive control (MPC) and optimize the following objectives: .

[0033] The local obstacle avoidance controller introduces a dynamic risk field model: Obstacle repulsion field function: ; Path gravitational field function: ; The robot's motion direction is determined by the resultant force field Decide.

[0034] The dynamic risk field model introduces speed adaptive regulation: The repulsive field coefficient η is positively correlated with the robot speed v: ; The safety distance σ is dynamically adjusted with the speed: .

[0035] In some embodiments, the improved A* algorithm includes a bidirectional search mechanism: Forward search: expand from the starting point to the target; Reverse search: expand from the target to the starting point; Convergence condition: When the distance between the forward node and the reverse node is d <dt hres Generate connection paths.

[0036] In some embodiments, the system is deployed on a mobile robotic platform and includes: Indoor cleaning robot: semantic map identifies "carpet" and "furniture" areas and adjusts cleaning strategies; Warehouse AGV: Use topological maps to plan the shortest path between shelves; Security patrol robot: realizes human-machine avoidance through dynamic obstacle detection.

[0037] In some embodiments, an energy consumption optimization unit is further included to reduce power consumption by: Sensor hierarchical wake-up mechanism: only IMU and encoder are enabled during low-speed motion; Computational load balancing: Positioning tasks are assigned to the GPU, while control tasks run on the CPU.

[0038] In some embodiments, a map storage and sharing unit is further included: Adopting incremental storage format, only saving map changes; Multi-robot system synchronizes semantic topology key nodes through wireless network.

[0039] The system includes a human-computer interaction interface, providing: Real-time pose visualization interface; Semantic map editing tools; Navigation mission command input terminal.

[0040] The system also includes a sensor calibration module for regularly calibrating and calibrating each sensor to ensure the accuracy and consistency of sensor data. The calibration module corrects the installation position, angle, and measurement parameters of the sensor through specific calibration equipment and algorithms.

[0041] like Figure 3 As shown, when used, it includes the following steps: synchronous acquisition of raw data by multiple sensors; spatiotemporal alignment and motion compensation processing; tightly coupled filtering fusion pose estimation; construction and updating of semantic-geometric fusion maps; hierarchical path planning and dynamic obstacle avoidance control.

[0042] Example 1: Indoor cleaning robot application example System Deployment: In an indoor environment, the multi-sensor fusion positioning and navigation system of this invention is installed on an indoor cleaning robot. Four UWB anchor nodes are deployed in the room to ensure signal coverage throughout the cleaning area. Simultaneously, the robot's IMU, LiDAR, binocular vision sensor, and wheel encoder are installed and debugged to ensure the proper functioning of each sensor and the accuracy of data collection.

[0043] Data acquisition and preprocessing: When the robot begins operating, each sensor in the sensor array module synchronously collects data. The IMU collects the robot's angular velocity and linear acceleration in real time at a frequency of 200Hz; the LiDAR scans the surrounding environment at a frequency of 10Hz, generating 3D point cloud data; the binocular vision sensor captures RGB-D images and depth information at a frame rate of 30fps; and the wheel encoder measures wheel speed and travel distance in real time. The data preprocessing module filters, converts formats, and performs coordinate transformation on the collected data. For example, the extended Kalman filter algorithm is used to filter the IMU data in real time to remove noise interference; the LiDAR point cloud data is converted to a unified point cloud data format and rasterized; the image data collected by the binocular vision sensor is converted to a standard image pixel coordinate format, and depth information is extracted to generate a depth image; and the data from each sensor is unified into the robot's local coordinate system.

[0044] Multi-source data fusion and pose estimation: The multi-source data fusion module fuses the preprocessed data. The front-end pre-integration unit pre-integrates the IMU data and outputs pose increments Δp, Δv, and Δq. The tightly coupled fusion unit inputs the pre-integration results, along with the lidar point cloud matching residuals, visual feature reprojection errors, and UWB ranging values, into an extended Kalman filter to estimate the robot's pose state in real time, including position coordinates, velocity components, and attitude quaternions. It also estimates and corrects the IMU's zero bias. The back-end optimization unit uses a factor graph model to fuse closed-loop detection information and perform global optimization of the pose trajectory to further improve positioning accuracy.

[0045] Environment Modeling and Map Updates: The dynamic environment modeling module simultaneously constructs a semantically labeled occupancy grid map and topology map based on fused pose information and sensor data. The occupancy grid map updates employ a logarithmic probability model to reflect the distribution of static obstacles, such as walls and furniture, in the robot's surroundings in real time. Semantic map construction uses a convolutional neural network (CNN) to identify object categories in images, such as "carpet" and "furniture," and maps these labels to a point cloud to generate a semantic topology map, which provides a basis for the robot's path planning and cleaning strategy adjustments.

[0046] Path Planning and Obstacle Avoidance Control: The autonomous positioning and navigation control module generates collision-free motion trajectories based on the constructed map and estimated pose. The global path planner uses an improved A* algorithm, combined with a bidirectional search mechanism and a cost function that considers the confidence level of the grid map position, to plan the robot's globally optimal path from the starting position to the target position. The local obstacle avoidance controller uses model predictive control (MPC) to optimize control inputs based on real-time environmental information and the robot's motion state. This allows the robot to adjust its direction and speed in a timely manner when encountering obstacles (such as suddenly appearing objects or pedestrians), achieving dynamic obstacle avoidance and ensuring the continuity and safety of cleaning operations. At the same time, the robot adjusts its cleaning strategy based on the "carpet" and "furniture" areas identified by the semantic map, such as increasing cleaning effort in carpet areas and maintaining a safe distance around furniture to improve cleaning effectiveness and efficiency.

[0047] Implementation Results: In indoor cleaning robot applications, the system of this invention achieves high-precision positioning and navigation, with a positioning error of ≤5cm, a path planning time of ≤1s, and an obstacle avoidance success rate of ≥99%. Compared with traditional indoor cleaning robots, cleaning efficiency is increased by over 30% and cleaning coverage by over 20%, effectively improving the performance and user experience of indoor cleaning robots.

[0048] Example 2: Warehouse AGV application example System Deployment: Four UWB anchor points were deployed in a 5,000-square-meter warehouse environment to ensure signal coverage throughout the warehouse area. The multi-sensor fusion positioning and navigation system was installed on a warehouse automated guided vehicle (AGV). The IMU, lidar, binocular vision sensor, and wheel encoder on the AGV were installed and debugged to ensure the normal operation of each sensor and the accuracy of data collection.

[0049] Data acquisition and preprocessing: When the AGV is operating, each sensor in the sensor array module synchronously collects data. The IMU collects the AGV's angular velocity and linear acceleration in real time. The LiDAR scans the surrounding environment at a 10Hz frequency, generating 3D point cloud data. The binocular vision sensor collects RGB-D images and depth information of the warehouse environment. The wheel encoder measures wheel speed and travel distance in real time. The data preprocessing module filters, converts formats, and performs coordinate transformation on the collected data to ensure data accuracy and consistency.

[0050] Multi-source data fusion and pose estimation: The multi-source data fusion module fuses preprocessed data and outputs a high-precision pose estimate. The front-end pre-integration unit pre-integrates the IMU data. The tightly coupled fusion unit feeds the pre-integration results, along with the lidar point cloud matching residuals, visual feature reprojection errors, and UWB ranging values, into an extended Kalman filter to estimate the AGV's pose in real time and estimate and correct the IMU's zero bias. The back-end optimization unit uses a factor graph model to fuse closed-loop detection information and optimize the pose trajectory.

[0051] Environmental Modeling and Map Updates: The dynamic environment modeling module simultaneously constructs a semantically labeled occupancy grid map and topological map based on fused pose information and sensor data. The occupancy grid map update utilizes a logarithmic probability model to reflect the real-time distribution of static obstacles such as shelves, goods, and aisles within the warehouse environment. Semantic map construction uses a convolutional neural network (CNN) to identify object categories in images, such as shelves and goods, and maps these labels to a point cloud to generate a semantic topological map, providing richer environmental information for AGV path planning and navigation.

[0052] Path Planning and Obstacle Avoidance Control: The autonomous positioning and navigation control module generates collision-free motion trajectories based on the constructed map and estimated position. The global path planner uses an improved A* algorithm, combined with a bidirectional search mechanism and a cost function that considers the confidence level of grid map positions, to plan the globally optimal path for the AGV from its starting position to the target shelf. The local obstacle avoidance controller uses model predictive control (MPC) to optimize control inputs based on real-time environmental information and the AGV's motion status. This allows the AGV to adjust its direction and speed in a timely manner when encountering dynamic obstacles (such as other AGVs or personnel), achieving dynamic obstacle avoidance and ensuring the safety and reliability of cargo transportation. Furthermore, the AGV uses the topological map to plan the shortest path between shelves, improving logistics efficiency.

[0053] Implementation Results: In warehouse AGV applications, the system of the present invention has achieved significant performance improvements in positioning accuracy, path planning time, and obstacle avoidance success rate. Test results show that compared to traditional warehouse AGV systems, the positioning error of the present invention has been reduced from 15cm to 3.8cm, path planning time has been shortened from 2.1s to 0.9s, and the obstacle avoidance success rate has been increased from 85% to 99.2%. Through precise positioning navigation and efficient path planning, the operating efficiency of AGVs has been greatly improved, the accuracy and safety of cargo transportation have been significantly enhanced, and the automation level and economic benefits of warehouse logistics have been effectively improved.

[0054] Warehouse AGV application: Deploy 4 UWB anchor points in a 5,000 m2 warehouse; During AGV operation: The tightly coupled fusion unit calculates the position and posture in real time (frequency 100Hz); the semantic map marks the shelf area (label A_shelf) and the improved A* algorithm plans the shortest path between shelves: Measured results: Example 3: Security patrol robot application example System Deployment: In security patrol scenarios in industrial parks or commercial areas, deploy an appropriate number of UWB anchor nodes to ensure signal coverage of the patrol area. The multi-sensor fusion positioning and navigation system of this invention is installed on a security patrol robot. The robot's IMU, lidar, binocular vision sensor, and wheel encoder are installed and debugged to ensure the normal operation of each sensor and the accuracy of data collection.

[0055] Data Acquisition and Preprocessing: When the robot begins patrolling, each sensor in the sensor array module synchronously collects data. The IMU collects the robot's angular velocity and linear acceleration in real time. The LiDAR scans the surrounding environment at a 10Hz frequency, generating 3D point cloud data. The binocular vision sensor collects RGB-D images and depth information of the patrol environment. The wheel encoder measures wheel speed and travel distance in real time. The data preprocessing module filters, converts formats, and performs coordinate transformation on the collected data.

[0056] Multi-source data fusion and pose estimation: The multi-source data fusion module fuses preprocessed data and outputs a high-precision pose estimate. The front-end pre-integration unit pre-integrates the IMU data. The tightly coupled fusion unit feeds the pre-integration results, along with the lidar point cloud matching residuals, visual feature reprojection errors, and UWB ranging values, into an extended Kalman filter to estimate the robot's pose in real time and estimate and correct the IMU's zero bias. The back-end optimization unit uses a factor graph model to fuse closed-loop detection information and optimize the pose trajectory.

[0057] Environment Modeling and Map Updates: The dynamic environment modeling module simultaneously constructs semantically labeled occupancy grid maps and topological maps based on fused pose information and sensor data. The occupancy grid map updates employ a logarithmic probability model to reflect the real-time distribution of static obstacles, such as buildings, obstacles, and roads, within the patrol environment. Semantic map construction uses a convolutional neural network (CNN) to identify object categories in images, such as pedestrians, vehicles, and access control systems. These labels are then mapped to a point cloud to generate a semantic topological map, providing richer environmental information for the robot's path planning and navigation.

[0058] Path Planning and Obstacle Avoidance Control: The autonomous positioning and navigation control module generates collision-free motion trajectories based on the constructed map and estimated position. The global path planner uses an improved A* algorithm, combined with a bidirectional search mechanism and a cost function that considers the confidence level of grid map positions, to plan patrol routes for the robot. The local obstacle avoidance controller uses model predictive control (MPC) to optimize control inputs based on real-time environmental information and the robot's motion state. This allows the robot to adjust its direction and speed in a timely manner when encountering dynamic obstacles (such as pedestrians and vehicles), achieving human-machine avoidance and ensuring the safety and continuity of patrol operations. Furthermore, the robot uses a dynamic obstacle detection algorithm to monitor dynamic obstacles in the patrol environment in real time and mark them on the map, providing a basis for subsequent path planning and obstacle avoidance.

[0059] Implementation Results: In security patrol robot applications, the system of this invention achieves high-precision positioning and navigation, with a positioning error of ≤5cm, a path planning time of ≤1s, and an obstacle avoidance success rate of ≥99%. The robot can operate stably in complex patrol environments, accurately identifying and avoiding dynamic obstacles, effectively improving the efficiency and safety of security patrols. Compared to traditional security patrol robots, the system of this invention can better adapt to dynamic environmental changes, reduce manual intervention, and lower operating costs, providing an efficient and reliable automated patrol solution for the security sector.

[0060] Example 4: Energy consumption optimization example Sensor hierarchical wake-up mechanism: When the robot is moving at low speed or stationary, only the IMU and wheel encoders are enabled for basic pose estimation, and the operating frequency of other sensors (such as lidar, binocular vision sensors, and UWB positioning units) is turned off or reduced to reduce sensor energy consumption. When the robot needs to perform high-precision positioning and navigation or encounters a complex environment, other sensors are gradually awakened according to actual needs to ensure system performance while reducing energy consumption. For example, when an indoor cleaning robot finishes cleaning an area and stops waiting, only the IMU and wheel encoders are kept in low-power operation, and other sensors enter a dormant state. When the robot detects that it needs to move to the next cleaning area, it wakes up the lidar, binocular vision sensors, and UWB positioning unit and resumes normal operation.

[0061] Computational load balancing: Positioning tasks are assigned to the GPU, leveraging the GPU's parallel computing capabilities to accelerate the processing of large amounts of sensor data (such as LiDAR point cloud data and image data). Control tasks are run on the CPU to ensure the real-time and accuracy of control algorithms. By rationally allocating computing tasks, the performance advantages of both the GPU and CPU are fully utilized, improving system efficiency and reducing energy consumption. For example, during data preprocessing and multi-source data fusion, the GPU is used to rapidly filter, extract features, and perform coordinate transformations on LiDAR point cloud data, while simultaneously performing parallel computations on image data collected by the binocular vision sensor. During autonomous positioning, navigation, and control, the CPU uses the fused pose information and map data to execute path planning and obstacle avoidance control algorithms in real time, generating motion commands for the robot. This computational load balancing strategy reduces system energy consumption by over 30% compared to traditional centralized computing methods, while ensuring real-time and high performance.

[0062] Implementation Results: Through a hierarchical sensor wake-up mechanism and computational load balancing strategy, the system of this invention effectively reduces energy consumption while ensuring positioning and navigation performance. In applications such as indoor cleaning robots, warehouse AGVs, and security patrol robots, the system's battery life has been extended by over 20%, reducing the number of robot recharges and improving operational efficiency and practicality. Furthermore, the reduced energy consumption helps reduce system heat generation, improves system stability and reliability, and extends the lifespan of hardware devices.

[0063] Example 5: Map storage and sharing example Incremental Storage Format: The map storage and sharing unit uses an incremental storage format, saving only map changes, rather than the entire map data. During the map construction process, whenever the environment changes (such as adding obstacles, removing objects, or adjusting the environment layout), the system only records the changed map data and stores it locally. For example, in a warehouse AGV application, when goods are added or removed from a shelf, the system only updates the map data for the corresponding area, significantly reducing map storage space usage. Furthermore, the incremental storage format facilitates version management of map data, making it easy to revisit historical map data when needed.

[0064] Multi-robot system map sharing: In a multi-robot system, semantic topology key nodes are synchronized via wireless networks (such as WiFi or 4G / 5G networks). Each robot uploads its own map changes to a cloud server, downloads other robots' map changes from the cloud server, and updates its local map based on the semantic topology key node information. For example, in a security patrol scenario within an industrial park, multiple security patrol robots work together, sharing map change information via wireless networks and updating their respective maps in real time. This ensures that each robot has access to the latest environmental information, improving patrol efficiency and safety. Through map sharing, the multi-robot system can achieve efficient coverage and collaborative patrols over large areas, reducing duplication of work and wasted resources.

[0065] Human-Machine Interaction Interface: The system includes a human-machine interaction interface, providing a real-time position visualization interface, a semantic map editing tool, and a navigation task command input terminal. The real-time position visualization interface allows operators to intuitively view the robot's current position, posture, and motion trajectory, understanding the robot's operating status. The semantic map editing tool allows operators to add, modify, or delete semantic tags within the map, such as updating furniture layout information within the semantic map of an indoor cleaning robot. The navigation task command input terminal allows operators to send navigation task commands to the robot, such as setting cleaning areas, planning patrol routes, or specifying cargo delivery destinations. The human-machine interaction interface improves the system's usability and flexibility, facilitating operator management and control of the robot.

[0066] Implementation effect: Through the incremental storage format and multi-robot system map sharing strategy, the system of the present invention effectively reduces the map storage space occupied and the amount of data transmission. In the multi-robot collaborative application scenario, the map sharing function enables robots to obtain the latest environmental information in real time, improving collaborative work efficiency and navigation accuracy. The human-computer interaction interface provides operators with a convenient operation method, lowers the threshold for system use, and improves user experience and system operability. For example, in a multi-robot warehousing and logistics system, through map sharing, the path conflicts between AGVs are reduced by more than 40%, and the cargo transportation efficiency is increased by more than 25%. At the same time, operators can easily manage the operation tasks of multiple AGVs through the human-computer interaction interface, realizing efficient and orderly warehousing and logistics management. Finally, it should be noted that the above descriptions are merely preferred embodiments of the present invention and are not intended to limit the present invention. Although the present invention has been described in detail with reference to the aforementioned embodiments, those skilled in the art will be able to modify the technical solutions described in the aforementioned embodiments or substitute equivalents for some of the technical features. Any modifications, equivalent substitutions, and improvements made within the spirit and principles of the present invention shall be included within the scope of protection of the present invention.

Claims

1. A multi-sensor fusion robot high-precision positioning and navigation system, characterized in that: include: A sensor array module, which includes an inertial measurement unit, a lidar, a binocular vision sensor, a wheel encoder, and a UWB positioning unit, and is used to obtain information about the robot's surrounding environment; A data preprocessing module, connected to the sensor array module, for filtering, converting data formats, and performing coordinate transformation processing on the data collected by the sensors; A multi-source data fusion module is connected to the data preprocessing module. The multi-source data fusion module uses a hybrid filtering algorithm to fuse multi-sensor data and output a high-precision pose estimate; A dynamic environment modeling module is connected to the multi-source data fusion module, and synchronously constructs an occupancy grid map and a topological map with semantic labels based on the fused robot position and posture information; The autonomous positioning and navigation control module is connected to the dynamic environment modeling module and generates a collision-free motion trajectory based on the map and posture.

2. The multi-sensor fusion robot high-precision positioning and navigation system according to claim 1, characterized in that: The inertial measurement unit collects the robot's angular velocity and linear acceleration in real time. The lidar generates three-dimensional point cloud data of the environment. The binocular vision sensor outputs RGB-D images and depth information. The wheel encoder measures wheel speed and travel distance. The UWB positioning unit provides an absolute position reference through anchor point ranging.

3. The multi-sensor fusion robot high-precision positioning and navigation system according to claim 1, characterized in that: The data preprocessing module uses an adaptive filtering algorithm to estimate and suppress noise in sensor data in real time. At the same time, it normalizes the data based on the measurement characteristics of the sensor to ensure that different sensor data are fused in the same coordinate system and data format. The coordinate system is a unified robot local coordinate system or a global map coordinate system, and the data format conversion includes converting the raw data of different sensors into a common point cloud data format, image pixel coordinate format or distance angle information format within the system.

4. The multi-sensor fusion robot high-precision positioning and navigation system according to claim 1, characterized in that: The multi-source data fusion module includes: Front-end pre-integration unit: pre-integrates IMU data and outputs pose increments Δp, Δv, Δq; Tightly coupled fusion unit: The pre-integration result, LiDAR point cloud matching residual, visual feature reprojection error, and UWB ranging value are input into the extended Kalman filter (EKF). The state vector is defined as: ； where b a， ,b g Zero bias for IMU accelerometer and gyroscope; Back-end optimization unit: uses a factor graph model to fuse closed-loop detection information and optimize the pose trajectory.

5. The multi-sensor fusion robot high-precision positioning and navigation system according to claim 1, characterized in that: The dynamic environment modeling module performs: Occupancy grid map update: using logarithmic probability model: ； in, , is the prior probability log odds; Semantic map construction: Use convolutional neural networks (CNNs) to identify object categories in images and map labels to point clouds to generate semantic topology maps.

6. The multi-sensor fusion robot high-precision positioning and navigation system according to claim 1, characterized in that: The autonomous decision-making control module includes: Global path planner: uses the improved A* algorithm, and the cost function is: ; where uncertainty(n) is the inverse of the confidence level of the grid map position; Local obstacle avoidance controller: Apply model predictive control (MPC) and optimize the following objectives: 。 7. The multi-sensor fusion robot high-precision positioning and navigation system according to claim 1, characterized in that: The improved A* algorithm includes a bidirectional search mechanism: Forward search: expand from the starting point to the target; Reverse search: expand from the target to the starting point; Convergence condition: When the distance between the forward node and the reverse node is d <dt hres Generate connection paths.

8. The multi-sensor fusion robot high-precision positioning and navigation system according to claim 1, characterized in that: The system is deployed on a mobile robot platform and includes: Indoor cleaning robots: semantic maps identify "carpet" and "furniture" areas and adjust cleaning strategies; Warehouse AGV: Use topological maps to plan the shortest path between shelves; Security patrol robot: realizes human-machine avoidance through dynamic obstacle detection.

9. The multi-sensor fusion robot high-precision positioning and navigation system according to claim 1, characterized in that: It also includes an energy optimization unit that reduces power consumption by: Sensor hierarchical wake-up mechanism: only IMU and encoder are enabled during low-speed motion; Computational load balancing: Positioning tasks are assigned to the GPU, while control tasks run on the CPU.

10. The multi-sensor fusion robot high-precision positioning and navigation system according to claim 1, characterized in that: Also includes map storage and sharing unit: Adopting incremental storage format, only saving map changes; Multi-robot system synchronizes semantic topology key nodes through wireless network. The system includes a human-computer interaction interface, providing: Real-time pose visualization interface; Semantic map editing tools; Navigation mission command input terminal.

Images