A single-line laser radar-based multi-obstacle prediction navigation obstacle avoidance method
By combining single-line LiDAR and ROS function packages with point cloud data processing, low-cost, real-time navigation and obstacle avoidance in dynamic obstacle environments is achieved, solving the planning and decision-making errors of traditional ROS navigation systems and improving the safety and efficiency of robots.
Patent Information
- Authority / Receiving Office
- CN · China
- Patent Type
- Patents(China)
- Current Assignee / Owner
- NORTHEASTERN UNIV CHINA
- Filing Date
- 2023-04-19
- Publication Date
- 2026-05-19
AI Technical Summary
In dynamic obstacle environments, existing technologies cannot update local cost maps in a timely manner, leading to planning and decision-making errors when the robot moves quickly over obstacles. Furthermore, depth cameras are susceptible to lighting conditions, and 3D LiDAR is expensive, failing to meet the real-time obstacle avoidance requirements of mobile robots in complex environments.
By employing a single-line LiDAR combined with the ROS function package, obstacles are detected through point cloud data filtering, clustering, and rectangle fitting. Kalman filtering is used to track the motion state of obstacles, and constant velocity models and network models are combined to predict obstacle trajectories, thereby achieving end-to-end policy navigation and formulating obstacle avoidance strategies to improve safety and flexibility.
It enables navigation and obstacle avoidance under low-cost and real-time conditions, improves the safety and flexibility of mobile robots, reduces dependence on high-computing-power servers, avoids the influence of lighting, shortens the planning path, and improves obstacle avoidance efficiency.
Smart Images
Figure CN116576857B_ABST
Abstract
Description
Technical Field
[0001] This invention belongs to the field of multi-obstacle tracking and prediction and mobile robot navigation and obstacle avoidance, specifically involving a multi-obstacle prediction navigation and obstacle avoidance method based on single-line lidar. Background Technology
[0002] In recent years, the development of deep learning and robotics has driven progress in fields such as logistics warehousing, autonomous driving, smart homes, and drones. As application scenarios become increasingly complex, the problem of robots being unable to avoid fast-moving obstacles is becoming increasingly prominent. For example, in autonomous driving, vehicles need to detect other vehicles, pedestrians, and obstacles in their surroundings, while also predicting the trajectory of moving obstacles over a future period to achieve safe motion control in the current moment.
[0003] In industrial settings, AGV robots often encounter multiple walking workers while transporting goods, requiring timely avoidance without disrupting the work progress. Intelligent monitoring necessitates detecting the movement of vehicles and pedestrians, analyzing potential safety hazards, and issuing timely warnings. These scenarios all involve obstacle prediction. Complete obstacle prediction should include obstacle detection, obstacle tracking, and obstacle prediction. However, most researchers currently assume that the historical trajectory and movement state of obstacles are known, focusing only on trajectory prediction. This has resulted in a lack of a comprehensive obstacle prediction algorithm framework suitable for mobile robots, despite the burgeoning research in this area. "Obstacle prediction" encompasses obstacle detection, obstacle tracking, and trajectory prediction. A key challenge in robot applications in dynamic obstacle environments is the prediction of dynamic obstacles. Traditional research on dynamic obstacle prediction typically assumes that the movement state and historical trajectory of dynamic obstacles are known, focusing only on trajectory prediction. In mobile robots, complete obstacle prediction requires obstacle detection, obstacle tracking, and trajectory prediction.
[0004] Based on sensor type, obstacle prediction is generally divided into algorithms that use images from depth cameras as raw data and algorithms that use point clouds from LiDAR as raw data. Image data is relevant to target detection and tracking, which are currently hot research topics. However, algorithms that process image data typically only track bounding boxes in obstacle tracking, failing to acquire depth information. While disparity maps can be used to compensate, the errors are significant and unsuitable for tracking and predicting the true spatial location of obstacles. Furthermore, deep learning network models are often used, which are time-consuming to train, slow to deploy, and require high computational power, making them unsuitable for mobile robots. In addition, depth cameras are susceptible to lighting conditions. LiDAR algorithms mostly use 3D LiDAR data and network models, which are only suitable for autonomous driving scenarios and cannot be applied to mobile robots.
[0005] Furthermore, traditional ROS navigation, when encountering fast-moving dynamic obstacles, suffers from inaccurate local cost maps, leading to incorrect planning decisions. Lacking an obstacle prediction module, the robot only initiates obstacle avoidance maneuvers when obstacles are extremely close, requiring constant path replanning and consuming significant processor resources, thus reducing the robot's safety and flexibility. Therefore, when performing tasks in complex environments with dynamic obstacles, the robot often fails to meet mission requirements. Summary of the Invention
[0006] To address the shortcomings of existing technologies, this invention provides a multi-obstacle prediction navigation and obstacle avoidance method based on single-line lidar, which can enable mobile robots to navigate and avoid obstacles in dynamic obstacle environments while meeting the requirements of real-time performance and low cost.
[0007] A multi-obstacle prediction navigation and obstacle avoidance method based on single-line lidar specifically includes the following steps:
[0008] Step 1: Using the ROS package of the LiDAR in the Ubuntu system, collect obstacle point cloud data detected by the single-line LiDAR, then filter the point cloud data, transform the filtered point cloud data to the global coordinate system, and then use a clustering algorithm to classify the point cloud data after transformation to the global coordinate system, thus completing the clustering of the entire frame of point cloud data and obtaining the obstacle point cloud array.
[0009] Step 1.1: Apply a moving average filter with a window size of 3 to the point cloud data in both the temporal and spatial dimensions, specifically as follows:
[0010]
[0011] in express At that moment, the The coordinates of each point on the global map;
[0012] Based on the ROS global map, points are obtained. Coordinates under lidar In addition to the car's displacement relative to the global map, the calculation points are... The coordinates on the global map are as follows:
[0013]
[0014] In the formula and The information is obtained from odometer data, which is obtained by fusing and filtering the IMU and encoder data. It is the distance of point i relative to the lidar. It is the angle of deviation of point i relative to the 0° line of the lidar;
[0015] Step 1.2: Perform coarse global segmentation on the filtered data transformed to global coordinates obtained in Step 1.1, and classify the resulting point cloud clusters using the density-based DBSCAN algorithm. Finally, complete the clustering of the entire frame data to obtain the obstacle array; specifically:
[0016] The filtered obstacle point cloud data is directly clustered based on Euclidean distance. Since clustering algorithms require significant computational resources, the LiDAR data is first sequentially segmented according to the continuity of the point cloud scan before clustering. For each LiDAR point cloud block, the starting point... Index satisfies and ,end satisfy and The steps of the sequential partitioning algorithm are as follows:
[0017] First, calculate the Euclidean distance between two adjacent points in the obstacle point cloud data; if the Euclidean distance is less than a threshold T, they are considered to belong to the same point cloud cluster; the threshold T is set according to the environment; points with an Euclidean distance less than T are added to the current point cloud cluster. In the middle; if the Euclidean distance is greater than a certain threshold T, it is considered that they do not belong to the same cloud cluster; the current cloud cluster is added. New point cloud cluster ,Will Join Repeat the above operations until the obstacle point cloud data of the current frame is processed, and obtain the final processing result for use by the point cloud clustering algorithm;
[0018] Finally, the point cloud cluster array after sequential segmentation is classified by the DBSCAN algorithm of density clustering, and the clustering of the whole frame data is completed to obtain the obstacle point cloud array.
[0019] Step 2: Perform rectangle fitting on the obstacle point cloud array obtained after clustering and classification in Step 1 to detect obstacles, obtain obstacle position, shape and size information, and store it in the obstacle array;
[0020] Step 2.1: In the global coordinate system, perform an extreme value search on the coordinate values of the obstacle point cloud array to obtain the coordinate values of each point in the obstacle point cloud array. Maximum value on the axis and minimum value And in Maximum value on the axis and minimum value Its expression is:
[0021]
[0022] Step 2.2: Utilizing the four extreme points The process involves drawing a rectangular bounding box, fitting the appearance of obstacles, and removing obstacles along the walls. Specifically, removing obstacles along the walls involves:
[0023] Let the aspect ratio threshold for determining whether an obstacle is a wall be . The longer side of the rectangle is The shorter side is The aspect ratio is ,but:
[0024]
[0025] When aspect ratio Greater than the threshold In some cases, obstacles are considered walls, and the point cloud composing those obstacles is removed. In reality, small obstacles may also satisfy this condition, so a constraint is added that the longer side must be greater than 3 meters for this purpose. The expression is:
[0026]
[0027] Step 3: Perform data association on the obstacles detected in the obstacle point cloud data of two adjacent obstacle arrays to obtain the current motion state of each obstacle and record its historical trajectory to achieve obstacle tracking;
[0028] The process of associating obstacle data to obtain the current motion state of each obstacle and recording its historical trajectory specifically involves:
[0029] Step 3.1: Initialize the obstacle array to empty. When the data of the first frame is obtained, define a Kalman object and predict the position of the obstacle in the next frame, and initialize the Kalman object array. Calculate the trajectory of the obstacle using the position information in the obstacle arrays of the two adjacent frames. Initialize the newly generated trajectory with a KF filter and mark it as "uncertain". After matching and confirming again in the next two frames, mark the uncertain trajectory as "certain". This process from uncertain to certain is the lifecycle update.
[0030] Step 3.2: When processing the second frame of data, the observation value of the current frame is associated with the predicted value of the Kalman object in the previous frame; the determined trajectory and the observation value obtained by the obstacle detector are used to construct a bipartite graph for matching. The matching uses KNN, Hungarian algorithm or KM algorithm. The matching results are divided into three types: Kalman object and observation value that are successfully matched, Kalman object that is not successfully matched and observation value that is not successfully matched.
[0031] Step 3.3: For the Kalman object in the previous frame that did not match successfully, update it. If the termination condition is triggered, remove it from the Kalman array and the obstacle array. The termination condition is set by the following method: if the Kalman object does not match successfully for 3 consecutive frames, its lifecycle changes from "determined" to "indetermined". Kalman objects marked as "indetermined" are removed from the obstacle array.
[0032] Step 3.4: For observations in the current frame that do not match successfully, initialize them as Kalman objects and add them to the Kalman array;
[0033] Step 3.5: For successfully matched Kalman objects, perform Kalman updates using the observations of the current frame. If the obstacle condition is triggered, add the object to the obstacle array.
[0034] Step 3.6: Update the obstacle array to obtain the motion state of the obstacles;
[0035] Step 3.7: Repeat steps 3.2 to 3.6 to achieve real-time updates of obstacle information;
[0036] Step 4: Based on Step 3, use the constant velocity model to predict the trajectories of multiple obstacles over a future period of time;
[0037] Step 4.1: Select the length of the predicted trajectory based on the complexity of the scene; the scene complexity is determined based on the number of pedestrians and the motion state of environmental obstacles.
[0038] Step 4.2: For short-sequence trajectory prediction, select a prediction method based on motion models;
[0039] Step 4.3: For long-order trajectory prediction, select the prediction method based on the network model, where long-order trajectory prediction is only enabled when short-order prediction is severely distorted;
[0040] Step 5: Read the results from the ROS global path planner and divide them into multiple local paths;
[0041] Step 6: Implement mapless end-to-end strategy navigation in each local path obtained in Step 5;
[0042] Step 6.1: Read the obstacle information obtained in the above steps;
[0043] Step 6.2: Read global path planner information;
[0044] Step 6.3: Segment the trajectory in the global path planner information and plan the starting point;
[0045] Step 6.4: Determine whether the obstacle has passed the road section ahead. Based on the characteristics of Newton's first law, use the constant velocity model to calculate the meeting point by comparing the current motion state of the obstacle with the motion state of the robot. If there is no meeting point, the mobile robot chooses to go straight. The robot is a mobile robot.
[0046] Step 6.5: Determine if the obstacle is within a safe distance; if so, proceed straight; otherwise, perform strategy planning.
[0047] By calculating the nearest obstacle to the robot. Develop corresponding obstacle avoidance strategies for the area in question; among them, when Located in front of the mobile robot Meter length A rectangular area of meters is among them , The robot adopts an execution strategy of retreating backward at a high speed of fm / s (a / fs). ;when Located at coordinate point With the central major axis as The minor axis of the meter is When the robot is in a semi-elliptical region of meters, it adopts a stopping strategy; when Located outside the aforementioned areas, with the robot itself as the center and a radius of [missing information]. When the robot takes a circular area of meters, Proceeding at a low speed of m / s ;
[0048] When there are no obstacles in the detection area, the robot takes action. The robot moves forward at m / s; when the robot stops in the elliptical area for more than 2 seconds, it will detour. First, it will determine whether it can turn right to detour through the obstacle based on the direction vector of the obstacle. If it cannot detour through the obstacle by turning right, it will turn left. The angular velocity of each turn is 0.017 rad / s and the time is 1 second. The area, velocity and time can be adjusted according to different environments to achieve the best effect.
[0049] Step 6.6: Determine whether turning right can avoid the obstacle. If yes, turn right; otherwise, turn left.
[0050] Step 6.7: Repeat steps 6.1-6.6 to implement end-to-end policy navigation.
[0051] Beneficial technical effects of the present invention:
[0052] This invention solves the problem of traditional ROS navigation systems failing to update local cost maps in a timely manner when encountering fast-moving obstacles. It provides mobile robots with a navigation and obstacle avoidance solution that balances low cost and real-time performance in obstacle tracking and prediction. Furthermore, the algorithm has fewer parameters, requiring no training compared to deep learning network models and not relying on high-performance servers. It overcomes the limitations of depth cameras, which are susceptible to lighting conditions. Additionally, by using a single-line LiDAR as the data acquisition sensor, it reduces the system's hardware cost compared to 3D LiDAR algorithms. Applied to mobile robots such as robotic vacuum cleaners, service robots, and AGVs, it can improve obstacle avoidance safety and offers real-time performance at a low cost. Attached Figure Description
[0053] Figure 1 This is an algorithm framework for a multi-obstacle prediction, navigation, and obstacle avoidance method based on a single-line lidar according to an embodiment of the present invention;
[0054] Figure 2 This is a schematic diagram of the transformation of LiDAR point cloud to the global coordinate system according to an embodiment of the present invention. Figure a is a schematic diagram of the global map coordinate system, and Figure b is a schematic diagram of the LiDAR coordinate system.
[0055] Figure 3 This is a schematic diagram of the sequential segmentation of point cloud clusters according to an embodiment of the present invention;
[0056] Figure 4 This is a schematic diagram of the motion state and historical trajectory based on obstacle prediction according to an embodiment of the present invention;
[0057] Figure 5 This is a flowchart of the end-to-end policy navigation according to an embodiment of the present invention;
[0058] Figure 6 This is a schematic diagram of the strategy planning scenario division according to an embodiment of the present invention;
[0059] Figure 7 This is a comparison diagram of the conventional navigation method and the path planning method of the present invention in the embodiments of the present invention; wherein Figure a is the conventional navigation method and Figure b is the method of the present invention. Detailed Implementation
[0060] The present invention will be further described below with reference to the accompanying drawings and embodiments;
[0061] A multi-obstacle prediction navigation and obstacle avoidance method based on single-line lidar, as shown in the appendix. Figure 1 As shown, the specific steps include:
[0062] Step 1: Using the ROS package for LiDAR in the Ubuntu system, collect point cloud data of obstacles detected by a single-line LiDAR, then filter the point cloud data, and perform coordinate transformation on the filtered point cloud data to convert it to the global coordinate system, as shown in the attached figure. Figure 2 As shown, the point cloud data after being transformed to global coordinates is classified using a clustering algorithm to complete the clustering of the entire frame of point cloud data and obtain the obstacle point cloud array;
[0063] Step 1.1: Apply a moving average filter with a window size of 3 to the point cloud data in both the temporal and spatial dimensions, specifically as follows:
[0064]
[0065] in express At that moment, the The coordinates of each point on the global map;
[0066] Based on the ROS global map, points are obtained. Coordinates under lidar In addition to the car's displacement relative to the global map, the calculation points are... The coordinates on the global map are as follows:
[0067]
[0068] In the formula and The information is obtained from odometer data, which is obtained by fusing and filtering the IMU and encoder data. It is the distance of point i relative to the lidar. It is the angle of deviation of point i relative to the 0° line of the lidar;
[0069] Step 1.2: Perform coarse global segmentation on the filtered data transformed to global coordinates obtained in Step 1.1, and classify the resulting point cloud clusters using the density-based DBSCAN algorithm. Finally, complete the clustering of the entire frame data to obtain the obstacle array; specifically:
[0070] The filtered obstacle point cloud data is directly clustered based on Euclidean distance. Since clustering algorithms require significant computational resources, the LiDAR data is first sequentially segmented according to the continuity of the point cloud scan before clustering. For each LiDAR point cloud block, the starting point... Index satisfies and ,end satisfy and The sequential partitioning algorithm is shown in the appendix. Figure 3 As shown, the steps are as follows:
[0071] First, calculate the Euclidean distance between two adjacent points in the obstacle point cloud data. If the Euclidean distance is less than a threshold T, they are considered to belong to the same point cloud cluster. The threshold T is set according to the environment and is generally between 0.1 meters and 0.3 meters. Points with an Euclidean distance less than T are added to the current point cloud cluster. In the middle; if the Euclidean distance is greater than a certain threshold T, it is considered that they do not belong to the same cloud cluster; the current cloud cluster is added. New point cloud cluster ,Will Join Repeat the above operations until the obstacle point cloud data of the current frame is processed, and obtain the final processing result for use by the point cloud clustering algorithm;
[0072] Finally, the point cloud cluster array after sequential segmentation is classified by the DBSCAN algorithm of density clustering, and the clustering of the whole frame data is completed to obtain the obstacle point cloud array.
[0073] Step 2: Perform rectangle fitting on the obstacle array obtained after clustering and classification in Step 1 to detect obstacles, obtain the location, shape and size information of obstacles, and store them in the obstacle array;
[0074] Step 2.1: In the global coordinate system, perform an extreme value search for the coordinate values of the obstacle array to obtain the coordinates of each point in the obstacle point cloud array. Maximum value on the axis and minimum value And in Maximum value on the axis and minimum value Its expression is:
[0075]
[0076] Step 2.2: Utilizing the four extreme points The process involves drawing a rectangular bounding box, fitting the appearance of obstacles, and removing obstacles along the walls. Specifically, removing obstacles along the walls involves:
[0077] Let the aspect ratio threshold for determining whether an obstacle is a wall be . The longer side of the rectangle is The shorter side is The aspect ratio is ,but:
[0078]
[0079] When aspect ratio Greater than the threshold In some cases, obstacles are considered walls, and the point cloud composing those obstacles is removed. In reality, small obstacles may also satisfy this condition, so a constraint is added that the longer side must be greater than 3 meters for this purpose. The expression is:
[0080]
[0081] Step 3: Associate the detected obstacles in two adjacent frames of obstacle point cloud data in the obstacle array to obtain the current motion state of each obstacle and record its historical trajectory, thereby achieving obstacle tracking; a schematic diagram of the motion state and historical trajectory based on obstacle prediction is attached. Figure 4 As shown;
[0082] The process of associating obstacle data to obtain the current motion state of each obstacle and recording its historical trajectory specifically involves:
[0083] Step 3.1: Initialize the obstacle array to empty. When the data of the first frame is obtained, define a Kalman object and predict the position of the obstacle in the next frame, and initialize the Kalman object array. Calculate the trajectory of the obstacle using the obstacle point cloud data of the two adjacent frames. Initialize the newly generated trajectory with a KF filter and mark it as "uncertain". After matching and confirming again in the next two frames, mark the uncertain trajectory as "certain". This process from uncertain to certain is the lifecycle update.
[0084] Step 3.2: When processing the second frame of data, the observation value of the current frame is associated with the predicted value of the Kalman object in the previous frame; the determined trajectory and the observation value obtained by the obstacle detector are used to construct a bipartite graph for matching. The matching uses KNN, Hungarian algorithm or KM algorithm. The matching results are divided into three types: Kalman object and observation value that are successfully matched, Kalman object that is not successfully matched and observation value that is not successfully matched.
[0085] Step 3.3: For the Kalman object in the previous frame that did not match successfully, update it. If the termination condition is triggered, remove it from the Kalman array and the obstacle array. The termination condition is set by the following method: if the Kalman object does not match successfully for 3 consecutive frames, its lifecycle changes from "determined" to "indetermined". Kalman objects marked as "indetermined" are removed from the obstacle array.
[0086] Step 3.4: For observations in the current frame that do not match successfully, initialize them as Kalman objects and add them to the Kalman array;
[0087] Step 3.5: For successfully matched Kalman objects, perform Kalman updates using the observations of the current frame. If the obstacle condition is triggered, add the object to the obstacle array.
[0088] Step 3.6: Update the obstacle array to obtain the motion state of the obstacles;
[0089] Step 3.7: Repeat steps 3.2 to 3.6 to achieve real-time updates of obstacle information;
[0090] Step 4: Based on Step 3, use the constant velocity model to predict the trajectories of multiple obstacles over a future period of time;
[0091] Step 4.1: Select the length of the predicted trajectory based on the complexity of the scene; the scene complexity is determined based on the number of pedestrians and the motion state of environmental obstacles.
[0092] Step 4.2: For short-sequence trajectory prediction, select a prediction method based on a motion model; the prediction method based on a motion model in Step 4.2 includes, but is not limited to, constant velocity model, acceleration model, kinematic and dynamic model, Monte Carlo method, and Kalman filter method.
[0093] Step 4.3: For long-order trajectory prediction, select a prediction method based on a network model; the prediction method based on a network model includes, but is not limited to, deep network models, attention mechanism-based models, long short-term memory networks, and recurrent neural networks, etc., wherein long-order trajectory prediction is only enabled when short-order prediction is severely distorted;
[0094] Step 5: Read the results from the ROS global path planner and divide them into multiple local paths;
[0095] Step 6: Implement mapless end-to-end strategy navigation in each local path obtained in Step 5; as shown in the appendix. Figure 5 As shown;
[0096] Step 6.1: Read the obstacle information obtained in the above steps;
[0097] Step 6.2: Read global path planner information;
[0098] Step 6.3: Segment the trajectory in the global path planner information and plan the starting point;
[0099] Step 6.4: Determine whether the obstacle has passed the road ahead. Based on the characteristics of Newton's first law, use the constant velocity model to calculate the meeting point by comparing the current motion state of the obstacle with the motion state of the robot. If there is no meeting point, the mobile robot chooses to go straight. The robot is a mobile robot, such as a sweeping robot, service robot, and AGV.
[0100] Step 6.5: Determine if the obstacle is within a safe distance; if so, proceed straight; otherwise, perform strategy planning.
[0101] By calculating the nearest obstacle to the robot. Develop corresponding obstacle avoidance strategies for the area in question; among them, when When positioned within a rectangular area 0.5 meters wide and 4 meters long in front of the mobile robot, the robot adopts a strategy of retreating backward at a high speed of 2 m / s for 0.5 seconds; where... When the robot is located in a semi-elliptical region centered at coordinates (0, 0.5), with a major axis of 4 meters and a minor axis of 1 meter, it adopts a stopping strategy; when When located in a circular area with a radius of 5 meters centered on the robot (excluding the areas mentioned above), the robot moves forward at a low speed of 0.5 m / s. When there are no obstacles in the detected area, the robot moves forward at 1.5 m / s. When the robot stops in an elliptical area for more than 2 seconds, it will detour. First, it determines whether it can turn right to detour through based on the direction vector of the obstacle. If it cannot detour to the right, it will turn left. Each turning angular velocity is 0.017 rad / s, and the time is 1 second. (See attached diagram) Figure 6 As shown;
[0102] Step 6.6: Determine whether turning right can avoid the obstacle. If yes, turn right; otherwise, turn left.
[0103] Step 6.7: Repeat steps 6.1-6.6 to implement end-to-end policy navigation.
[0104] The navigation obstacle avoidance algorithm and path planning method designed in this invention eliminates the local cost map and adds obstacle tracking and prediction functions, thus avoiding decision-making errors due to untimely updates of the local cost map. This improves planning efficiency, and in simulation environments, compared to the traditional ROS planning algorithm, the planned path is shortened by 50% in scenarios with dynamic obstacles. A schematic diagram of the path planning results of this method and the traditional method is attached. Figure 7 As shown;
[0105] Table 1. Comparison of algorithm performance when the start and end points are 15m apart;
[0106] Pass rate / % Time / s path / m Do not avoid fast-moving obstacles RT-Follow 95% 22s 19m yes Traditional navigation 20% >50s >38m no
[0107] The time and path were recorded when the robot moved through a 15m corridor using different algorithms. After multiple experiments, the pass rates of different algorithms were obtained. Comparison revealed that in the simulated 15m corridor environment, traditional navigation, due to the presence of obstacles and the influence of dynamic obstacles, generally planned paths longer than 38m. The method of this invention, however, only planned half the path of traditional navigation, and the movement time was reduced from 50s to 22s. Because traditional navigation cannot address the problem of untimely updates to the local cost map when encountering fast-moving obstacles, it cannot perform correct path planning when facing the last sphere with a speed of 2m / s, resulting in a collision. Using the method of this invention, dynamic obstacles can be avoided, and the efficiency and safety of obstacle avoidance navigation for mobile robots are greatly improved.
Claims
1. A multi-obstacle prediction, navigation, and obstacle avoidance method based on single-line lidar, characterized in that, Specifically, the following steps are included: Step 1: Using the ROS package of the LiDAR in the Ubuntu system, collect obstacle point cloud data detected by the single-line LiDAR, then filter the point cloud data, transform the filtered point cloud data to the global coordinate system, and then use a clustering algorithm to classify the point cloud data after transformation to the global coordinate system, thus completing the clustering of the entire frame of point cloud data and obtaining the obstacle point cloud array. Step 2: Perform rectangle fitting on the obstacle point cloud array obtained after clustering and classification in Step 1 to detect obstacles, obtain obstacle position, shape and size information, and store it in the obstacle array; Step 3: Perform data association on the obstacles detected in the obstacle point cloud data of two adjacent obstacle arrays to obtain the current motion state of each obstacle and record its historical trajectory to achieve obstacle tracking; Step 4: Based on Step 3, use the constant velocity model to predict the trajectories of multiple obstacles over a future period of time; Step 5: Read the results from the ROS global path planner and divide them into multiple local paths; Step 6: Implement mapless end-to-end strategy navigation in each local path obtained in Step 5; Step 6 specifically involves: Step 6.1: Read the obstacle information obtained in the above steps; Step 6.2: Read global path planner information; Step 6.3: Segment the trajectory in the global path planner information and plan the starting point; Step 6.4: Determine whether the obstacle has passed the road section ahead. Based on the characteristics of Newton's first law, use the constant velocity model to calculate the meeting point by comparing the current motion state of the obstacle with the motion state of the robot. If there is no meeting point, the mobile robot chooses to go straight. The robot is a mobile robot. Step 6.5: Determine if the obstacle is within a safe distance; if so, proceed straight; otherwise, perform strategy planning. Step 6.6: Determine whether turning right can avoid the obstacle. If yes, turn right; otherwise, turn left. Step 6.7: Repeat steps 6.1-6.6 to implement end-to-end policy navigation; Step 6.5, strategy planning, specifically involves: By calculating the nearest obstacle to the robot. Develop corresponding obstacle avoidance strategies for the area in question; among them, when Located in front of the mobile robot Meter length A rectangular area of meters is among them , The robot adopts an execution strategy of retreating a / fs at a high speed of fm / s, f [2,3];when Located at coordinate point With the central major axis as The minor axis of the meter is When the robot is in a semi-elliptical region of meters, it adopts a stopping strategy; when Located outside the aforementioned areas, with the robot itself as the center and a radius of [missing information]. When the robot takes a circular area of meters, Proceeding at a low speed of m / s ; When there are no obstacles in the detection area, the robot takes action. The robot moves forward at m / s; when the robot stops in the elliptical area for more than 2 seconds, it will detour. First, it will determine whether it can turn right to detour through the obstacle based on the direction vector of the obstacle. If it cannot detour through the obstacle by turning right, it will turn left. The angular velocity of each turn is 0.017 rad / s and the time is 1 second.
2. The multi-obstacle prediction navigation and obstacle avoidance method based on single-line lidar according to claim 1, characterized in that, Step 1 is as follows: Step 1.1: Apply a moving average filter with a window size of 3 to the point cloud data in both the temporal and spatial dimensions, specifically as follows: in express At that moment, the The coordinates of each point on the global map; Based on the ROS global map, points are obtained. Coordinates under lidar In addition to the car's displacement relative to the global map, the calculation points are... The coordinates on the global map are as follows: In the formula and The information is obtained from odometer data, which is obtained by fusing and filtering the IMU and encoder data. It is the distance of point i relative to the lidar. It is the angle of deviation of point i relative to the 0° line of the lidar; Step 1.2: Perform coarse global segmentation on the filtered data transformed to global coordinates obtained in Step 1.1, and classify the resulting point cloud clusters using the density-based DBSCAN algorithm. Finally, complete the clustering of the entire frame data to obtain the obstacle array; specifically: The filtered obstacle point cloud data is directly clustered based on Euclidean distance features. Before clustering, the LiDAR data is first sequentially segmented according to the continuity of the point cloud scan. For each LiDAR point cloud block, the starting point... Index satisfies and ,end satisfy and The steps of the sequential partitioning algorithm are as follows: First, calculate the Euclidean distance between two adjacent points in the obstacle point cloud data; if the Euclidean distance is less than a threshold T, they are considered to belong to the same point cloud cluster; the threshold T is set according to the environment; points with an Euclidean distance less than T are added to the current point cloud cluster. In the middle; if the Euclidean distance is greater than a certain threshold T, it is considered that they do not belong to the same cloud cluster; the current cloud cluster is added. New point cloud cluster ,Will Join Repeat the above operations until the obstacle point cloud data of the current frame is processed, and obtain the final processing result for use by the point cloud clustering algorithm; Finally, the point cloud cluster array after sequential segmentation is classified using the DBSCAN algorithm of density clustering, and the clustering of the entire frame data is completed to obtain the obstacle point cloud array.
3. The multi-obstacle prediction navigation and obstacle avoidance method based on single-line lidar according to claim 1, characterized in that, Step 2 is as follows: Step 2.1: In the global coordinate system, perform an extreme value search on the coordinate values of the obstacle point cloud array to obtain the coordinate values of each point in the obstacle point cloud array. Maximum value on the axis and minimum value And in Maximum value on the axis and minimum value Its expression is: Step 2.2: Utilizing the four extreme points The process involves drawing a rectangular bounding box, fitting the appearance of obstacles, and removing obstacles along the walls. Specifically, removing obstacles along the walls involves: Let the aspect ratio threshold for determining whether an obstacle is a wall be . The longer side of the rectangle is The shorter side is The aspect ratio is ,but: When aspect ratio Greater than the threshold In some cases, obstacles are considered walls, and the point cloud composing those obstacles is removed. In reality, small obstacles may also satisfy this condition, so a constraint is added that the longer side must be greater than 3 meters for this purpose. The expression is: 。 4. The multi-obstacle prediction navigation and obstacle avoidance method based on single-line lidar according to claim 1, characterized in that, Step 3 specifically involves: Step 3.1: Initialize the obstacle array to empty. When the data of the first frame is obtained, define a Kalman object and predict the position of the obstacle in the next frame, and initialize the Kalman object array. Calculate the trajectory of the obstacle using the position information in the obstacle arrays of the two adjacent frames. Initialize the newly generated trajectory with a KF filter and mark it as "uncertain". After matching and confirming again in the next two frames, mark the uncertain trajectory as "certain". This process from uncertain to certain is the lifecycle update. Step 3.2: When processing the second frame of data, the observation value of the current frame is associated with the predicted value of the Kalman object in the previous frame; the determined trajectory and the observation value obtained by the obstacle detector are used to construct a bipartite graph for matching. The matching uses KNN, Hungarian algorithm or KM algorithm. The matching results are divided into three types: Kalman object and observation value that are successfully matched, Kalman object that is not successfully matched and observation value that is not successfully matched. Step 3.3: For Kalman objects from the previous frame that did not match successfully, update them; if the termination condition is triggered, remove them from the Kalman array and the obstacle array. The termination condition is set by changing the lifecycle of a Kalman object from "determined" to "indetermined" if the object fails to match for three consecutive frames. Kalman objects marked as "indetermined" are then removed from the obstacle array. Step 3.4: For observations in the current frame that do not match successfully, initialize them as Kalman objects and add them to the Kalman array; Step 3.5: For successfully matched Kalman objects, perform Kalman updates using the observations of the current frame. If the obstacle condition is triggered, add the object to the obstacle array. Step 3.6: Update the obstacle array to obtain the motion state of the obstacles; Step 3.7: Repeat steps 3.2-3.6 to achieve real-time updates of obstacle information.
5. The multi-obstacle prediction navigation and obstacle avoidance method based on single-line lidar according to claim 1, characterized in that, Step 4 is as follows: Step 4.1: Select the length of the predicted trajectory based on the complexity of the scene; the scene complexity is determined based on the number of pedestrians and the motion state of environmental obstacles. Step 4.2: For short-sequence trajectory prediction, select a prediction method based on motion models; Step 4.3: For long-order trajectory prediction, select the prediction method based on the network model, where long-order trajectory prediction is only enabled when short-order prediction is severely distorted.