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

CN121995918ACN 121995918 ACN121995918 ACN 121995918ACN-121995918-A

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 realizes high-precision positioning by multi-source data fusion of a laser radar, a GPS, an IMU and an encoder and an extended Kalman filtering algorithm; the invention realizes the unification of global optimum and dynamic adaptation by adopting the cooperation of double algorithms of A-global planning and teb-local-planner local obstacle avoidance, overcomes the problems of obstacle avoidance and planning disjoint through hierarchical obstacle avoidance strategy and proportional-integral path regression control, realizes automatic multi-point navigation through a queue management mechanism, solves the problems of insufficient positioning precision of a single sensor, navigation obstacle avoidance disjoint and the like, and ensures that the mobile robot can stably and efficiently operate autonomously in a complex environment.

Inventors

JIANG QING
AN XIN
ZHU QIAN
DING XINRUI
ZHOU XIANCUN
LI XIAOYONG
ZHANG JING
YANG YADONG
ZHOU BAO
YIN YING
DU CHANG

Assignees

皖西学院

Dates

Publication Date: 20260508
Application Date: 20260202

Claims (10)

1. The intelligent navigation and obstacle avoidance optimization method for the multi-sensor fusion double algorithm is characterized by comprising the following steps of: Performing time synchronization, filtering and preprocessing on laser radar point cloud, GPS positioning, IMU gesture and encoder data, and eliminating redundant interference generated in the acquisition process of a sensor; calculating displacement and rotation angle between adjacent sampling moments of the robot by utilizing the differential motion model through encoder data, obtaining odometer data, and further utilizing a Kalman filter to carry out drift correction; the method comprises the steps of carrying out point cloud matching on synchronized filtering point cloud data and corrected odometer data, calculating the relative pose of a robot through a scanning matching method, fusing GPS absolute positioning data, laser radar-IMU relative positioning data and corrected odometer data through an extended Kalman filtering algorithm, obtaining an accurate fusion positioning result, searching a global grid map through an A-type algorithm based on the fusion positioning result, generating a global planning path according to the actual cost of nodes, heuristic estimated cost, environment complexity and motion constraint of a steering angle and a steering radius of the robot, constructing a rolling window local cost map with the current position as a center in the running process of the robot along the global path, fusing real-time laser radar scanning data, generating a local planning track through a teb-local-planner algorithm, generating obstacle avoidance instructions through a hierarchical obstacle avoidance strategy according to the nearest distance of obstacles around the robot measured by the real-time laser radar, triggering emergency obstacle avoidance, conventional obstacle avoidance or non-obstacle avoidance corresponding strategies in different intervals, calculating the current position of the robot and the global planning path through a proportional-integral control algorithm after the obstacle avoidance is completed, calculating the current position of the robot and the global planning path according to the current position and the overall control regression of the global planning path, and correcting the current deviation of the robot navigation path through the overall planning control deviation, and monitoring the planned robot, judging whether the current target point is finished, if so, acquiring the next target point from the multi-target point queue and returning to the global path planning step, and if not, returning to the local obstacle avoidance step to realize automatic multi-point navigation; the extended Kalman filtering algorithm comprises a prediction stage and an updating stage, wherein the prediction stage predicts the state of the current moment according to the robot motion model and the state estimation of the previous moment, and the updating stage corrects the prediction state by utilizing the observation data of the current moment so as to solve the optimal fusion state estimation.
2. The method of claim 1, wherein the time synchronization of the multi-sensor data is to time align the filtered laser radar point cloud data with the gesture data collected by the IMU sensor, so as to ensure the time sequence consistency of the two types of data.
3. The method of claim 1, wherein the extended kalman filter algorithm only fuses the lidar-IMU relative positioning data and the corrected odometer data in an indoor GPS signal-free environment, and still maintains stable fusion positioning accuracy.
4. The method of claim 1, wherein the cost function of the a-algorithm takes into account the actual cost of having reached the node, the remaining cost of heuristic estimation, and adds environmental complexity factors in the grid map while incorporating the motion constraints of the steering angle and steering radius of the robot to ensure that the generated path meets both optimality and real motion capability of the robot.
5. The method of claim 1, wherein the hierarchical obstacle avoidance strategy comprises triggering an emergency obstacle avoidance strategy when the obstacle distance is less than a first distance threshold, reducing the robot linear speed and planning a detour trajectory to ensure an obstacle avoidance safety distance, triggering a constant obstacle avoidance strategy when the obstacle distance is between the first distance threshold and a second distance threshold, dynamically adjusting a local path through an elastic band algorithm, and continuing to travel along a globally planned path without obstacle avoidance when the obstacle distance is greater than the second distance threshold.
6. The method of claim 1, wherein the proportional-integral control algorithm generates a correction control amount according to a deviation between a current position of the robot and the global planned path, an accumulated value of the deviation, and a proportional coefficient and an integral coefficient, so that the robot gradually adjusts a movement direction to return to the global planned path.
7. The method of claim 1, wherein the multi-target point information is received and a queue is constructed by monitoring ROS topics, and the system sequentially generates a global path between two adjacent targets by using a segment planning method according to the current position and a target point sequence in the queue.
8. The method of claim 1, wherein the rolling window of the local cost map is constructed centered on the current position of the robot, and the newly acquired laser radar point cloud data is fused into the local cost map to mark the obstacle position.
9. The method of claim 1, wherein the point cloud matching uses a scan matching method to register the current frame of point cloud with the previous frame of point cloud, and calculates the displacement and the rotation angle between adjacent moments of the robot by combining the motion initial value of the odometer data.
10. A multi-sensor fusion dual-algorithm intelligent navigation and obstacle avoidance optimization system for performing the method of any of claims 1-9, comprising: The system comprises a sensing fusion module, a positioning module, a global planning module, a local obstacle avoidance module, a path regression module and a navigation management module, wherein the sensing fusion module is used for carrying out time synchronization, filtering and preprocessing of laser radar, GPS, IMU and encoder multi-sensor data, the positioning module is used for carrying out point cloud matching, odometer drift correction and extended Kalman filtering fusion calculation and outputting fusion positioning results, the global planning module is used for executing an A-algorithm to carry out global path planning according to the fusion positioning results and target points, the local obstacle avoidance module is used for constructing a local cost map, executing a teb_local_planner algorithm and generating obstacle avoidance instructions according to a hierarchical obstacle avoidance strategy, the path regression module is used for executing a proportional-integral control algorithm to calculate a correction control quantity, and the navigation management module is used for managing a multi-target point queue, monitoring navigation state information, judging the completion condition of the target points and triggering the planning flow of the next target point.

Description

Multi-sensor fusion double-algorithm intelligent navigation and obstacle avoidance optimization method Technical Field The invention relates to the technical field of intelligent navigation and autonomous obstacle avoidance of mobile robots, in particular to a multi-sensor fusion double-algorithm intelligent navigation and obstacle avoidance optimization method. Background With the rapid development of industrial automation and intelligent mobile equipment, the autonomous navigation and obstacle avoidance demands of mobile robots in indoor and outdoor complex environments are increasingly urgent. However, the conventional navigation obstacle avoidance scheme has a plurality of technical bottlenecks. Firstly, a single sensor is easy to be interfered by environment, the fluctuation of positioning precision is large, and the requirement of high-precision operation cannot be met. Secondly, the global path planning algorithm mostly adopts static distance optimization, and lacks sufficient consideration on environment complexity and robot motion constraint, so that path redundancy is caused. Thirdly, the local obstacle avoidance algorithm mostly adopts a fixed strategy, and response to sudden obstacle is lagged. Fourth, the global planning and the local obstacle avoidance algorithm are disjointed, and the robot is easy to deviate from the planning path after obstacle avoidance. The problems commonly cause unstable positioning, redundant navigation and collision risk of the robot, and severely restrict the application effect of the mobile robot in a complex environment. Disclosure of Invention The invention provides a multi-sensor fusion double-algorithm intelligent navigation and obstacle avoidance optimization method, which solves the technical problems of insufficient positioning precision of a single sensor, global path planning, local obstacle avoidance disconnection, obstacle avoidance response lag and the like in the related technology. The invention discloses a multi-sensor fusion double-algorithm intelligent navigation and obstacle avoidance optimization method, which comprises the following steps of performing time synchronization, filtering and preprocessing on laser radar point cloud, GPS positioning, IMU gesture and encoder data, and eliminating redundant interference generated in the acquisition process of sensors; calculating displacement and rotation angle between adjacent sampling moments of a robot through encoder data by utilizing a differential motion model, obtaining odometer data, further carrying out drift correction by utilizing a Kalman filter, carrying out point cloud matching on synchronized filtering point cloud data and corrected odometer data, calculating relative pose of the robot through a scanning matching method, fusing GPS absolute positioning data, laser radar-IMU relative positioning data and corrected odometer data by utilizing an extended Kalman filtering algorithm, obtaining accurate fusion positioning results, searching a global grid map by utilizing an A algorithm based on the fusion positioning results, generating a global planning path according to actual cost of nodes, heuristic estimation cost, environment complexity and motion constraint of a steering angle and a steering radius of the robot, constructing a rolling window local cost map by taking a current position as a center in the running process of the robot along the global path, fusing real-time laser radar scanning data, generating a local planning track by utilizing a teb-local-planner algorithm, generating obstacle avoidance instructions according to the nearest distance of obstacles around the robot measured by the laser radar, triggering obstacle avoidance strategies in different emergency intervals, or completing the current position and overall situation control deviation and the current position control algorithm, and judging whether the current target point is finished by monitoring the navigation state topics of the robot, acquiring the next target point from the multi-target point queue and returning to the global path planning step if the current target point is finished, and returning to the local obstacle avoidance step if the current target point is not finished, so as to realize automatic multi-point navigation. Further, the time synchronization of the multi-sensor data means that the filtered laser radar point cloud data and the attitude data acquired by the IMU sensor are aligned in time, so that the time sequence consistency of the two types of data is ensured. Furthermore, the extended Kalman filtering algorithm only fuses the laser radar-IMU relative positioning data and the corrected odometer data in an indoor environment without GPS signals, and still can keep stable fusion positioning accuracy. Further, the cost function of the algorithm A comprehensively considers the actual cost of the reached node and the residual cost of heuristic estimation, adds the environmental complexity factor i