Multi-sensor fusion double-algorithm intelligent navigation and obstacle avoidance optimization method

Abstract

The invention relates to the technical field of intelligent navigation and obstacle avoidance of mobile robots, and discloses a multi-sensor fusion double-algorithm intelligent navigation and obstacle avoidance optimization method, which comprises the following steps of: realizing high-precision positioning by adopting an extended Kalman filtering algorithm through multi-source data fusion of a laser radar, a GPS (Global Positioning System), an IMU (Inertial Measurement Unit) and an encoder; a * global planning and telocalplanner local obstacle avoidance dual algorithms are adopted for cooperation, and unification of global optimum and dynamic adaptation is achieved; through a hierarchical obstacle avoidance strategy and proportional-integral path regression control, the problem of disjunction between obstacle avoidance and planning is solved; automatic multi-point navigation is realized through a queue management mechanism; according to the invention, the problems of insufficient positioning precision, disjunction of navigation and obstacle avoidance and the like of a single sensor are solved, so that the mobile robot can stably and efficiently autonomously work in a complex environment.

Description

Technical Field

[0001] This invention relates to the field of intelligent navigation and autonomous obstacle avoidance technology for mobile robots, and more specifically, to a multi-sensor fusion dual-algorithm intelligent navigation and obstacle avoidance optimization method. Background Technology

[0002] With the rapid development of industrial automation and intelligent mobile devices, the demand for autonomous navigation and obstacle avoidance of mobile robots in complex indoor and outdoor environments is becoming increasingly urgent. However, traditional navigation and obstacle avoidance solutions suffer from several technical bottlenecks. First, single sensors are susceptible to environmental interference, resulting in large fluctuations in positioning accuracy and failing to meet the requirements of high-precision operations. Second, global path planning algorithms often employ static distance optimization, lacking sufficient consideration of environmental complexity and robot motion constraints, leading to path redundancy. Third, local obstacle avoidance algorithms often use fixed strategies, resulting in delayed responses to sudden obstacles. Fourth, the disconnect between global planning and local obstacle avoidance algorithms makes it easy for the robot to deviate from the planned path after obstacle avoidance. These problems collectively lead to unstable robot positioning, navigation redundancy, and collision risks, severely restricting the application effectiveness of mobile robots in complex environments. Summary of the Invention

[0003] This invention provides a multi-sensor fusion dual-algorithm intelligent navigation and obstacle avoidance optimization method, which solves the technical problems in related technologies such as insufficient positioning accuracy of a single sensor, disconnect between global path planning and local obstacle avoidance, and delayed obstacle avoidance response.

[0004] This invention discloses a multi-sensor fusion dual-algorithm intelligent navigation and obstacle avoidance optimization method, comprising the following steps: Time synchronization, filtering, and preprocessing of LiDAR point cloud, GPS positioning, IMU attitude, and encoder data to eliminate redundant interference generated during sensor acquisition; Calculating the robot's displacement and rotation angle between adjacent sampling times using a differential motion model based on encoder data to obtain odometer data, and then performing drift correction using a Kalman filter; Matching the synchronized filtered point cloud data with the corrected odometer data, and calculating the robot's relative pose using a scanning matching method; Fusing GPS absolute positioning data, LiDAR-IMU relative positioning data, and corrected odometer data using an extended Kalman filter algorithm to obtain accurate fused positioning results; Based on the fused positioning results, searching the global grid map using the A* algorithm, and considering the actual node cost, heuristic estimation cost, environmental complexity, and robot turning angle and rotation angle... A global planning path is generated by constraining the robot's motion to the radius. While the robot travels along this path, a local cost map is constructed using a rolling window centered on the current position. This map is then fused with real-time LiDAR scan data, and the teb_local_planner algorithm is used to generate a local planning trajectory. Based on the closest distances to obstacles around the robot measured by the real-time LiDAR, a hierarchical obstacle avoidance strategy is adopted to generate obstacle avoidance commands. Different obstacle distance intervals trigger corresponding strategies for emergency obstacle avoidance, normal obstacle avoidance, or no obstacle avoidance. After obstacle avoidance, a proportional-integral control algorithm is used to calculate the deviation between the robot's current position and the global planning path. Corrective control variables are generated based on the current and cumulative deviations to guide the robot back to the global planning path. By monitoring the robot's navigation status, it is determined whether the current target point has been reached. If so, the next target point is retrieved from the multi-target point queue, and the global path planning step is returned. If not, the local obstacle avoidance step is returned, achieving automatic multi-point navigation.

[0005] Furthermore, the time synchronization of the multi-sensor data refers to aligning the filtered lidar point cloud data with the attitude data collected by the IMU sensor in time to ensure the temporal consistency of the two types of data.

[0006] Furthermore, the extended Kalman filter algorithm can maintain stable fusion positioning accuracy even in indoor environments without GPS signals, by fusing only the relative positioning data of the lidar-IMU and the corrected odometer data.

[0007] Furthermore, the cost function of the A* algorithm comprehensively considers the actual cost of the reached nodes, the heuristically estimated remaining cost, and incorporates the environmental complexity factors in the grid map. It also incorporates the motion constraints of the robot's turning angle and turning radius to ensure that the generated path satisfies both optimality and conforms to the robot's actual motion capabilities.

[0008] Furthermore, the hierarchical obstacle avoidance strategy includes: when the obstacle distance is less than a first distance threshold, an emergency obstacle avoidance strategy is triggered, reducing the robot's linear velocity and planning a detour trajectory to ensure a safe obstacle avoidance distance; when the obstacle distance is between the first and second distance thresholds, a regular obstacle avoidance strategy is triggered, dynamically adjusting the local path through an elastic band algorithm; when the obstacle distance is greater than the second distance threshold, the robot continues to travel along the globally planned path without obstacle avoidance.

[0009] Furthermore, the proportional-integral control algorithm generates a corrective control quantity based on the deviation between the robot's current position and the global planned path, the cumulative value of the deviation, and the proportional and integral coefficients, so that the robot gradually adjusts its motion direction to return to the global planned path.

[0010] Furthermore, the multi-target point information is received by listening to ROS topics and a queue is constructed. The system uses a segmented planning method based on the current position and the sequence of target points in the queue to generate a global path between two adjacent target points in sequence.

[0011] Furthermore, the scrolling window of the local cost map is constructed with the robot's current position as the center, and the newly acquired LiDAR point cloud data is fused into the local cost map to mark the location of obstacles.

[0012] Furthermore, the point cloud matching adopts a scanning matching method to register the current frame point cloud with the previous frame point cloud, and calculates the displacement and rotation angle of the robot between adjacent moments by combining the initial motion values ​​of the odometry data.

[0013] This invention also discloses a multi-sensor fusion dual-algorithm intelligent navigation and obstacle avoidance optimization system for executing the above-mentioned methods, comprising: a perception fusion module for time synchronization, filtering, and preprocessing of multi-sensor data such as LiDAR, GPS, IMU, and encoder; a positioning module for point cloud matching, odometer drift correction, and extended Kalman filter fusion calculation, outputting fused positioning results; a global planning module for executing the A* algorithm to perform global path planning based on the fused positioning results and target points; a local obstacle avoidance module for constructing a local cost map, executing the teb_local_planner algorithm, and generating obstacle avoidance instructions according to a hierarchical obstacle avoidance strategy; a path regression module for executing a proportional-integral control algorithm to calculate corrected control quantities; and a navigation management module for managing a multi-target point queue, listening to navigation status messages, judging the completion status of target points, and triggering the planning process for the next target point.

[0014] The beneficial effects of this invention are as follows: This invention addresses the technical problems of insufficient positioning accuracy, disconnect between navigation and obstacle avoidance, delayed response, and chaotic multi-point navigation in mobile robots under complex environments through multi-sensor fusion, dual-algorithm collaboration, hierarchical obstacle avoidance, and path regression. The invention achieves the following technical effects: Firstly, by fusing multi-source data from lidar, GPS, IMU, and odometer, and using the extended Kalman filter algorithm, the error accumulation and random interference of a single sensor are effectively eliminated, achieving stable centimeter-level positioning in multiple scenarios. Secondly, by fully incorporating environmental complexity and robot motion constraints through the A* algorithm, the generated global path satisfies both optimality and conforms to the robot's actual motion capabilities, thus reducing path redundancy. Third, by combining global planning and local obstacle avoidance algorithms and using proportional-integral path regression control, the disconnect between obstacle avoidance and planning is eliminated, and the robot actively returns to the global path after obstacle avoidance is completed. Fourth, by adopting a graded obstacle avoidance strategy and using different processing methods for different obstacle distances, the system's response time to real-time sensor data is reduced from seconds to less than 300ms. Fifth, through queue management and status monitoring mechanisms, automatic multi-point navigation is achieved without manual intervention, and the target point switching response is rapid. Attached Figure Description

[0015] Figure 1 This is a system architecture diagram of the present invention; Figure 2 This is a diagram illustrating the implementation of the intelligent control algorithm of this invention; Figure 3 This is a diagram of the multi-sensor fusion positioning algorithm of the present invention; Figure 4 This is the core diagram of the dual-algorithm cooperative navigation and obstacle avoidance of the present invention; Figure 5 This is a diagram of the multi-point navigation and dynamic adjustment algorithm of the present invention; Figure 6 This is the task queue management diagram of the present invention. Detailed Implementation

[0016] The subject matter described herein will now be discussed with reference to exemplary embodiments. It should be understood that these embodiments are discussed only to enable those skilled in the art to better understand and implement the subject matter described herein, and changes may be made to the function and arrangement of the elements discussed without departing from the scope of this specification. Various processes or components may be omitted, substituted, or added as needed in the examples. Furthermore, some features described in the examples may be combined in other examples.

[0017] Example 1 A multi-sensor fusion dual-algorithm intelligent navigation and obstacle avoidance optimization method includes the following steps: Step 100, System Architecture Design; The system adopts a modular design and is built on the ROS framework. It consists of five parts: a perception layer, a decision-making and planning layer, a control layer, a data transmission and interaction layer, and an energy supply module. Each module achieves efficient data interaction through standardized interfaces and topic communication. The system architecture diagram is shown below. Figure 1 As shown; Perception layer: Responsible for the collection, fusion and preprocessing of environmental and robot state data. Core components include LiDAR (dual radars front and rear, 360° scanning), GPS / IMU fusion positioning module, visual sensor (optional), and data filtering and synchronization unit to realize obstacle detection, robot pose calculation and environmental feature extraction.

[0018] Decision Planning Layer: The core functional layer, which includes a global path planning module and a local obstacle avoidance module. It achieves the navigation logic of "global optimal + dynamic adaptation" through the collaboration of two algorithms. It also integrates a target point queue management unit to support multi-point navigation and cyclic cruise.

[0019] Control layer: Converts decision commands into action signals that the robot can execute, including PLC control module and motion control module, adapts to various robot chassis such as differential drive and omnidirectional drive, and realizes precise speed (linear velocity, angular velocity) control and motion status feedback.

[0020] Data transmission and interaction layer: Composed of ROS topic communication bus, wireless communication module (WiFi / 4G / 5G), and remote monitoring terminal, it realizes real-time transmission of sensor data, planned path, and control commands, and supports remote target point setting, navigation status monitoring, and emergency intervention.

[0021] Energy supply module: Composed of a large-capacity battery pack and a power management unit, it provides stable power to each module, monitors power, voltage and current parameters in real time, triggers an alarm when the power is low and plans a return charging path.

[0022] Step 200, Module and Interface Design; Step 201, Module Design: Perception layer module: Multi-sensor data fusion unit: Collects environmental point cloud data via LiDAR (range range 0.15~10m), filters, synchronizes time, and calibrates spatially using the laser_filters package to eliminate redundant noise; GPS / IMU fusion positioning module processes encoder motion data via the autolabor_pro_driver package, calculates odometer data using a differential motion model algorithm, and then matches the radar point cloud with the odometer data using the cartographer_ros algorithm package to correct positioning drift and achieve centimeter-level positioning (positioning error ≤ ±2cm). Environmental feature extraction unit: Based on the fused point cloud data, it identifies obstacle types (static / dynamic), sizes and distances, providing accurate environmental input for the decision-making and planning layer.

[0023] Decision planning layer module: Global path planning module: Employs the A* algorithm and performs path search based on a grid map (0.05m / grid) built using SLAM. The cost function comprehensively considers path distance, environmental complexity (obstacle density), and robot motion constraints (turning angle ≤30°, turning radius ≥0.5m), prioritizing obstacle-free and low-energy paths. It supports multi-point task queue management, receiving target point sequences (including x, y coordinates and yaw angle) by listening to the / move_base_simple / goal_temp topic, and generating global paths between adjacent target points in sequence, with a path repetition rate of <10%. When a new target point is added or the map is updated, path replanning is completed within 1 second, ensuring a smooth transition with the original path. Local obstacle avoidance module: Employs the teb_local_planner algorithm to construct a 5m×5m scrolling window local cost map, integrating real-time radar point cloud data, achieving an obstacle detection response time ≤300ms; Implements a hierarchical obstacle avoidance strategy: In emergency obstacle avoidance (obstacle distance <1m), the linear velocity is reduced to 0.1m / s and a detour trajectory is planned; In normal obstacle avoidance (obstacle distance 1-3m), the local path is adjusted using an elastic band algorithm to maintain a safe distance ≥0.8m from the obstacle; After obstacle avoidance is completed, the robot returns to the global path using a proportional-integral control algorithm, with a deviation correction time <5 seconds; Target point management unit: Supports two methods for adding target points (manual marking with 2DNavGoal, and recording the pose after the robot is in position via keyboard control), with a default maximum of 4 target points, which can be expanded; provides a cyclic navigation mode, supports adding / deleting / modifying target points, and displays navigation progress and status in real time.

[0024] Control layer module: PLC control module: Receives speed commands (linear velocity, angular velocity) output from the decision planning layer, converts them into chassis drive signals, and adapts to the control protocols of different types of robot chassis; ② Motion control module: Provides real-time feedback on the robot's motion status (actual speed, position deviation), dynamically adjusts control parameters, and ensures path tracking accuracy (position deviation ≤ ±5cm).

[0025] Data transmission and interaction layer module: ROS Topic Communication Bus: It adopts standardized topic formats (such as nav_msgs / Path, geometry_msgs / Twist) to realize data interaction between modules, with a data transmission rate of ≥100Mbps and a latency of <100ms; Remote monitoring terminal: Based on the RViz visualization platform, it displays the global map, local cost map, robot pose, planned path (global path in red, local path in blue), radar point cloud and navigation status in real time. The interface response time is ≤1 second, the data refresh rate is up to 10fps, and it supports remote emergency stop, path reset and other operations.

[0026] Energy supply module: The battery pack has a range of more than 8 hours of continuous operation. The power management unit monitors the power level in real time. When the power level is below 20%, it automatically triggers a low power alarm and sends a signal to the decision planning layer to start planning the return charging path.

[0027] Step 202, Interface Design: Mechanical interface: The sensors (LiDAR, GPS antenna) and the robot chassis adopt a standardized installation interface, which can be adapted to mobile platforms of different sizes, and the installation and debugging time is ≤30 minutes; Electrical interfaces: The modules communicate with each other via CAN bus or Ethernet interface, and are waterproof and dustproof (IP65 or above), with strong anti-electromagnetic interference capabilities to ensure signal transmission stability; Communication interface: Supports multiple protocols such as TCP / IP and MQTT, is compatible with mainstream function packages under the ROS ecosystem, can achieve data interaction with ground control centers and cloud servers, and the interface is compatible with mainstream industrial communication standards, adapting to sensors and chassis of different brands.

[0028] Step 300: Implementation of intelligent control algorithm; Step 301, Multi-sensor fusion localization algorithm: The core process of employing a multi-source data fusion strategy of "LiDAR + GPS / IMU + Odometer" is as follows: Data preprocessing: Filtering (removing noise points and outliers) and time synchronization (aligning with IMU data) are performed on the lidar point cloud data, and drift correction is performed on the odometer data; Pose calculation: Point cloud matching is achieved through the cartographer_ros algorithm package, and the robot's relative pose is calculated by combining the attitude data (roll angle, pitch angle, yaw angle) of the IMU; Fusion optimization: The extended Kalman filter (EKF) algorithm is adopted to fuse the absolute positioning data of GPS and the relative positioning data of LiDAR-IMU, eliminating the error accumulation of a single sensor and achieving centimeter-level positioning (static positioning error ≤ ±2cm, dynamic positioning error ≤ ±5cm). In indoor scenarios without GPS, stable positioning can still be maintained by relying solely on LiDAR and IMU (positioning error ≤ ±3cm).

[0029] Step 302, Core of Dual-Algorithm Cooperative Navigation and Obstacle Avoidance: This invention innovatively adopts a dual-algorithm fusion scheme of "global path planning (Algorithm A) + local obstacle avoidance (teb_local_planner algorithm)" to solve the pain points of "global rigidity and local blindness" of traditional single algorithms, forming a closed-loop collaborative logic: Initial planning: After SLAM mapping is completed, Algorithm A generates the globally optimal path based on the global map and the target point queue, clarifying the overall direction and key nodes of the robot's movement; Dynamic adaptation: During the robot's movement, the teb_local_planner algorithm monitors the local environment in real time through a scrolling window. If an obstacle is detected, a hierarchical obstacle avoidance strategy is immediately activated to adjust the local trajectory and ensure obstacle avoidance safety. Path regression: After obstacle avoidance is completed, the local algorithm guides the robot back to the global path through path tracking logic to avoid deviating from the target; Iterative optimization: The local algorithm feeds back environmental features (such as obstacle type and avoidance path efficiency) during the obstacle avoidance process to the global algorithm, providing a reference for subsequent path replanning, optimizing the cost function weights, and improving the overall navigation efficiency.

[0030] Step 303, Multi-point navigation and dynamic adjustment algorithm: Task queue management: By adding the topic / move_base_simple / goal_temp, information on multiple target points is received and a queue is built. The system listens to the topic / move_base / status to get the completion status of the target point and automatically triggers the navigation process of the next target point, realizing "single point completion - automatic switching - continuous navigation". Cyclic cruise control: After enabling the cyclic mode through the RViz plugin MultiNaviGoalsPanel, the robot will automatically return to the first target point after completing the last target point, forming a closed-loop cruise, which supports infinite loop or specified number of loops. Dynamic target adjustment: Target points can be added, deleted, or modified in real time during navigation. The system responds quickly and replans the route to ensure a smooth transition between the new target and the existing route, without any lag or trajectory conflict.

[0031] Example 2 This embodiment provides a multi-sensor fusion dual-algorithm intelligent navigation and obstacle avoidance optimization method, including the following steps: Step 1: Filter and time-synchronize the lidar point cloud data; The mobile robot's LiDAR acquires raw point cloud data. The `laser_filters` package is used to filter out noisy and outlier points in the point cloud data, eliminating redundant interference generated during sensor acquisition. Simultaneously, the filtered point cloud data is time-aligned with the attitude data acquired by the IMU sensor to ensure temporal consistency between the two types of data.

[0032] Step 2: Acquire odometer data and perform drift correction; The coded pulse data of the robot wheels is acquired by an encoder and input into the differential motion model algorithm to calculate the displacement and rotation angle between adjacent sampling times, thus obtaining odometry data. Since drift can occur over long periods, a Kalman filter is used to correct the odometry data, eliminating accumulated errors. The input to the Kalman filter is the original odometry data, and the output is the drift-corrected odometry data.

[0033] Step 3: Perform point cloud matching between the point cloud data and the calibrated odometry data to calculate the robot's relative pose; The time-synchronized filtered point cloud data and the corrected odometry data are input into the point cloud matching algorithm. The input to the scanning matching method is the initial motion values ​​of the current frame point cloud, the previous frame point cloud, and the odometry data. The output is the displacement and rotation angle of the robot between adjacent time points, i.e., the robot's relative pose.

[0034] Step 4: Use the extended Kalman filter algorithm to fuse GPS positioning data, lidar-IMU relative positioning data, and corrected odometer data to obtain the fused positioning result; The Extended Kalman Filter (EKF) algorithm uses absolute positioning data from GPS as the observation, and relative positioning data from LiDAR-IMU and calibrated odometer data as state predictions. By fusing these three types of data, it eliminates the accumulation of errors and random interference from a single sensor, outputting a more accurate fused positioning result. In indoor environments without GPS signals, the EKF algorithm can still maintain stable positioning accuracy by fusing only LiDAR-IMU and odometer data.

[0035] It should be noted that the fusion process of the Extended Kalman Filter algorithm includes a prediction phase and an update phase. In the prediction phase, the robot's motion model and the state estimate from the previous moment are used to predict the current state. In the update phase, the predicted state is corrected using the observation data from the current moment (such as GPS positioning or point cloud matching results) to obtain the optimal state estimate. Let the robot at time... The state is ,in For planar coordinates, Here is the orientation angle. State prediction during the fusion process uses a kinematic model, i.e. ,in The input control quantity (linear velocity, angular velocity). This represents process noise. Observations are derived from GPS, lidar matching results, etc., and are represented as... ,in For observation noise, EKF solves for the optimal state estimate by minimizing the weighted sum of observation and prediction errors. .

[0036] Step 5: Based on the fused positioning results, use the A* algorithm to search the global grid map and generate a globally planned path from the starting point to the target point; The inputs to Algorithm A are the robot's current position in the global coordinate system, the target point coordinates, and the global grid map generated by SLAM. The cost function of Algorithm A is... The actual cost of reaching the nodes Residual cost of heuristic estimation The cost function also incorporates environmental complexity (the density of obstacles in the grid map). It further incorporates the robot's motion constraints, ensuring the search process considers the robot's turning angle and radius limitations. The algorithm outputs the optimal global planning path that satisfies the motion constraints.

[0037] It's important to note that in multi-point navigation scenarios, the robot needs to visit multiple target points sequentially. This method receives multi-target point information by listening to the ` / move_base_simple / goal_temp` topic and constructs a queue. Based on the current position and the sequence of target points in the queue, the system uses a segmented planning approach to generate global paths between adjacent target points. Once a target point is completed, the system automatically triggers path planning for the next target point, achieving automatic connection of multi-point navigation.

[0038] Step 6: Construct a local cost map, fuse real-time LiDAR scan data, and use the teb_local_planner algorithm to generate a local planning trajectory; A scrolling window local cost map is constructed centered on the robot's current position. Newly acquired LiDAR point cloud data is fused into the local cost map to mark obstacle locations. The teb_local_planner algorithm takes the local cost map and the global planned path as input. This algorithm generates a locally planned trajectory that tracks the global path as output, using a time-optimal trajectory planning method, while ensuring robot movement safety.

[0039] Step 7: Generate obstacle avoidance commands using a tiered obstacle avoidance strategy based on the distance between the obstacle and the robot; The robot calculates the nearest distance to obstacles around it based on real-time LiDAR data. When the obstacle distance is less than a first threshold, an emergency obstacle avoidance strategy is triggered, the system reduces the robot's linear velocity and plans a detour trajectory; when the obstacle distance is between the first and second thresholds, a regular obstacle avoidance strategy is triggered, adjusting the local path using an elastic band algorithm; when the obstacle distance is greater than the second threshold, the robot continues to travel along the globally planned path without obstacle avoidance. The results of the obstacle avoidance strategy are converted into speed commands and output to the control layer for execution.

[0040] The aforementioned emergency obstacle avoidance trajectory is generated by searching for an unobstructed path in the local cost map using Dijkstra's algorithm. The algorithm takes the local cost map as input and outputs the trajectory from the robot's current position to a safe position after bypassing the obstacle. The aforementioned elastic band algorithm takes the current local path and obstacle positions as input. By dynamically adjusting the path vertices based on the obstacle positions, it minimizes the increase in path length while maintaining a safe distance from the obstacle, outputting the adjusted local path.

[0041] Step 8: After obstacle avoidance is completed, the proportional-integral control algorithm is used to calculate the correction amount so that the robot returns to the globally planned path; After obstacle avoidance is completed, the system calculates the deviation (including lateral and longitudinal deviations) between the robot's current position and the globally planned path. Using a proportional-integral (PI) control algorithm, a corrective control variable is generated based on the current deviation and the cumulative deviation. Let the current path deviation be... The corrected speed command is then... ,in and These are the proportional and integral coefficients, respectively. Through this control law, the robot gradually adjusts its direction of motion, eventually returning to the globally planned path. The control process is completed within 5 seconds, and the position and velocity commands after the return are output to the control layer.

[0042] Step 9: Listen to the robot's navigation status and determine whether the current target point has been completed. If completed, retrieve the next target point from the multi-target point queue and return to Step 5 to continue planning. If not completed, return to Step 6 to continue the local obstacle avoidance process. The system continuously monitors the ` / move_base / status` topic to obtain the navigation status of the current target point. When the navigation status changes to "Target Achieved," the system checks if there are any remaining target points in the queue. If so, it retrieves the next target point from the queue and returns to step 5 to execute global path planning; otherwise, the navigation task is complete. During target point navigation, the system continuously returns to step 6 to perform local obstacle avoidance and path tracking until the target point is reached.

[0043] It is understood that data preprocessing methods known to those skilled in the art include data cleaning, data transformation, and data reduction. Data transformation includes type conversion and normalization and standardization. Although the dimensions and types of data were omitted in the description of the preceding embodiments, data preprocessing is a technical knowledge known to those skilled in the art and a prerequisite step in data processing. Therefore, the previously described well-known data preprocessing steps were not described independently.

[0044] The embodiments of the present invention have been described above. However, the embodiments are not limited to the specific implementation methods described above. The specific implementation methods described above are merely illustrative and not restrictive. Those skilled in the art can make more equivalent embodiments under the guidance of the present embodiments, and all of them are within the protection scope of the present embodiments.

Claims

1. A multi-sensor fusion dual-algorithm intelligent navigation and obstacle avoidance optimization method, characterized in that, Includes the following steps: The system performs time synchronization, filtering, and preprocessing on LiDAR point cloud, GPS positioning, IMU attitude, and encoder data to eliminate redundant interference generated during sensor acquisition. Using encoder data, a differential motion model is employed to calculate the robot's displacement and rotation between adjacent sampling times, obtaining odometer data, which is then corrected for drift using a Kalman filter. The synchronized filtered point cloud data is matched with the corrected odometer data, and the robot's relative pose is calculated using a scanning matching method. An extended Kalman filter algorithm is used to fuse GPS absolute positioning data, LiDAR-IMU relative positioning data, and the corrected odometer data to obtain accurate fused positioning results. Based on the fused positioning results, the A* algorithm is used to search the global grid map, and the system considers the actual cost of nodes and other relevant factors. The algorithm estimates the cost, environmental complexity, and robot turning angle and radius motion constraints to generate a global planning path. While the robot travels along the global path, a local cost map is constructed centered on the current position, and real-time LiDAR scan data is fused to generate a local planning trajectory using the teb_local_planner algorithm. Based on the nearest distance to obstacles around the robot measured by real-time LiDAR, a hierarchical obstacle avoidance strategy is adopted to generate obstacle avoidance commands. Different obstacle distance intervals trigger corresponding strategies for emergency obstacle avoidance, normal obstacle avoidance, or no obstacle avoidance. After obstacle avoidance is completed, a proportional-integral control algorithm is used to calculate the deviation between the robot's current position and the global planning path. Based on the current deviation and cumulative deviation, a correction control quantity is generated to guide the robot back to the global planning path. By listening to the robot's navigation status, it can determine whether the current target point has been completed. If it has been completed, the next target point is retrieved from the multi-target point queue and the global path planning step is returned. If it has not been completed, the local obstacle avoidance step is returned, thus achieving automatic multi-point navigation. The extended Kalman filter algorithm includes a prediction phase and an update phase. The prediction phase predicts the current state based on the robot's motion model and the state estimate from the previous moment. The update phase uses the observation data from the current moment to correct the predicted state, thereby solving for the optimal fused state estimate.

2. The method according to claim 1, characterized in that, The time synchronization of multi-sensor data refers to aligning the filtered lidar point cloud data with the attitude data collected by the IMU sensor in time to ensure the temporal consistency of the two types of data.

3. The method according to claim 1, characterized in that, The extended Kalman filter algorithm can maintain stable fusion positioning accuracy even in indoor environments without GPS signals, by fusing only the relative positioning data of the lidar-IMU and the corrected odometer data.

4. The method according to claim 1, characterized in that, The cost function of the A* algorithm comprehensively considers the actual cost of the reached nodes, the heuristically estimated remaining cost, and incorporates environmental complexity factors from the grid map. It also includes motion constraints on the robot's turning angle and turning radius to ensure that the generated path satisfies both optimality and conforms to the robot's actual motion capabilities.

5. The method according to claim 1, characterized in that, The hierarchical obstacle avoidance strategy includes: when the obstacle distance is less than a first distance threshold, an emergency obstacle avoidance strategy is triggered, reducing the robot's linear velocity and planning a detour trajectory to ensure a safe obstacle avoidance distance; when the obstacle distance is between the first and second distance thresholds, a regular obstacle avoidance strategy is triggered, dynamically adjusting the local path through an elastic band algorithm; when the obstacle distance is greater than the second distance threshold, the robot continues to travel along the globally planned path without obstacle avoidance.

6. The method according to claim 1, characterized in that, The proportional-integral control algorithm generates a corrective control quantity based on the deviation between the robot's current position and the global planned path, the cumulative value of the deviation, and the proportional and integral coefficients, so that the robot gradually adjusts its motion direction to return to the global planned path.

7. The method according to claim 1, characterized in that, The multi-target point information is received by listening to ROS topics and a queue is built. The system uses a segmented planning method based on the current position and the sequence of target points in the queue to generate a global path between two adjacent target points in sequence.

8. The method according to claim 1, characterized in that, The scrolling window of the local cost map is constructed with the robot's current position as the center, and newly acquired LiDAR point cloud data is fused into the local cost map to mark the location of obstacles.

9. The method according to claim 1, characterized in that, The point cloud matching adopts a scanning matching method, which registers the current frame point cloud with the previous frame point cloud, and calculates the displacement and rotation angle of the robot between adjacent moments by combining the initial motion values ​​of the odometry data.

10. A multi-sensor fusion dual-algorithm intelligent navigation and obstacle avoidance optimization system, used to execute the method described in any one of claims 1-9, characterized in that, include: The perception fusion module is used for time synchronization, filtering, and preprocessing of multi-sensor data from LiDAR, GPS, IMU, and encoder. The localization module is used for point cloud matching, odometry drift correction, and extended Kalman filter fusion calculation, and outputs the fused localization result; the global planning module is used to execute the A* algorithm to perform global path planning based on the fused localization result and the target point; the local obstacle avoidance module is used to construct a local cost map, execute the teb_local_planner algorithm, and generate obstacle avoidance instructions based on the hierarchical obstacle avoidance strategy. The path regression module is used to execute the proportional-integral control algorithm to calculate the corrected control quantity. The navigation management module is used to manage the multi-target point queue, listen to navigation status messages, determine the completion status of target points, and trigger the planning process for the next target point.

Images