Unmanned vehicle autonomous navigation method in complex outdoor environment

Abstract

The invention discloses an autonomous navigation method for an unmanned vehicle in a complex outdoor environment, and the method comprises the steps: firstly, collecting data through a depth camera, a laser radar and other multi-source sensors, and carrying out the precise detection of dynamic obstacles and low obstacles through a multi-detector fusion algorithm and a visual language model; secondly, adopting a sector region multi-index waypoint sampling and optimization strategy to realize local path planning; and finally, designing a check point self-adaptive correction mechanism, and automatically planning a new substitute waypoint when the key check point is occupied by the obstacle for a long time. The method effectively solves the problem that the traditional navigation method is difficult to give consideration to dynamic obstacle sensing, low obstacle sensing and unreachable check points at the same time, and realizes the efficient and safe autonomous navigation function of the unmanned vehicle in a complex outdoor environment.

Description

Technical Field

[0001] The present invention belongs to the technical field of unmanned vehicle navigation, and particularly relates to an autonomous navigation method for unmanned vehicles in complex outdoor environments. Background Art

[0002] With the rapid development of artificial intelligence and robotics technologies, unmanned vehicles have been widely used in various fields such as military, security, agriculture, logistics, and urban services. For example, they can perform all-weather patrol and monitoring in factories or parks, conduct "last-mile" unmanned delivery on urban streets, and complete operations such as spraying or picking in farmlands. The above applications highly rely on the accurate perception of the environment and the efficient and safe autonomous navigation ability of unmanned vehicles. In the existing technology, significant progress has been made in the navigation of unmanned vehicles in large-scale outdoor environments, but several key technical challenges still remain. Firstly, the moving speeds and trajectories of common dynamic obstacles (such as pedestrians, vehicles, etc.) in the environment are uncertain, and traditional methods often require high-cost hardware or a large amount of computing resources to ensure the accuracy and real-time performance of recognition. Secondly, the lidar of unmanned vehicles is usually installed at a certain height, making it difficult to detect low obstacles (such as curbs, speed bumps, low steps, etc.) in a timely manner. Once these obstacles are not detected, they may cause the vehicle to overturn or be damaged. Thirdly, during the actual patrol or task execution process, if the pre-planned checkpoints are blocked by newly emerging temporary obstacles, the vehicle will be unable to complete the task. Some methods avoid this problem by relaxing the target arrival determination distance, but this will sacrifice the accuracy of the vehicle in trajectory following.

[0003] In recent years, some methods have attempted to update the environmental map online, but in scenarios with dense dynamic obstacles or frequent emergence of temporary obstacles, navigation failures are still likely to occur. In addition, existing end-to-end visual navigation methods are difficult to provide sufficient interpretability and safety redundancy, and once the perception fails, it is extremely easy to cause collision risks. Therefore, there is an urgent need for an unmanned vehicle navigation system that integrates efficient dynamic obstacle detection, low obstacle perception, path planning optimization, and target point adaptive deviation correction capabilities to improve the task completion rate and safety in complex outdoor environments. Summary of the Invention

[0004] Aiming at the problems of ground vehicles in complex outdoor environments, such as the perception of dynamic and low obstacles and the inability to reach key checkpoints occupied by obstacles, the present invention proposes a navigation method that integrates multi-sensor perception and adaptive deviation correction. Through the multi-detector fusion algorithm, visual language model segmentation, and the multi-index waypoint sampling and optimization strategy in the fan-shaped area, the present invention can, on the premise of controllable computing overhead, achieve the efficient detection and avoidance of dynamic and low obstacles, and automatically correct the deviation to generate new checkpoints when the target point is blocked due to environmental changes, so as to ensure that the unmanned vehicle can successfully complete patrol, transportation, or other specified tasks.

[0005] The dynamic and low-obstacle perception and adaptive deviation correction navigation method proposed by the present invention includes the following steps:

[0006] Step 1: Mount multi-source sensors such as depth cameras and lidar on the unmanned vehicle and collect data. Using the checkerboard calibration method, externally calibrate the camera and lidar to obtain the internal and external parameters required for mutual conversion between the two, ensuring that image and point cloud data can be projected into a unified coordinate system in subsequent processing;

[0007] Step 2: Use the obtained multi-sensor data to design an obstacle perception algorithm based on multi-detector fusion;

[0008] Step 2.1: The improved KDSCAN clustering detection method constructs a KD tree index in the lidar point cloud, then performs neighborhood search based on a distance threshold, groups adjacent points into the same cluster, generates candidate obstacle point cloud clusters, and calculates the three-dimensional bounding box to obtain preliminary pose information;

[0009] Step 2.2: The U-depth depth top-down detection method converts the depth image into a top-down plane, scans to detect the edges of obstacles, generates two-dimensional bounding boxes, and combines depth information to calculate the height and three-dimensional coordinates of the obstacles;

[0010] Step 2.3: The YOLO-IQR image detection method uses a lightweight YOLO network to detect the target area in the RGB image. Through the pixel data in the depth map, the interquartile range is used to filter out noise, and the effective depth data is converted into three-dimensional coordinates to obtain the center position and size of the obstacle;

[0011] Step 2.4: The multi-detector consistency fusion method matches the detection results of the improved KDSCAN, U-depth, and YOLO-IQR, verifies the consistency through the intersection over union and geometric differences, fuses the results using a weighted average strategy, and combines a simple online real-time tracking algorithm to predict the movement trajectory of the obstacles, improving the detection accuracy and stability;

[0012] Step 2.5: Use a vision-language model with open-vocabulary detection capabilities to perform pixel-level segmentation on the color image, and identify various low obstacles such as curbs, speed bumps, steps, and stacked objects through text prompts or class labels; Subsequently, convert the pixel coordinates of the segmentation mask into 3D point clouds through the camera internal parameter matrix, project them into the world coordinate system through the external parameters, and use the inverse perspective projection method to obtain their plane coordinates and height information in the world coordinate system, and jointly integrate them with the dynamic obstacle detection results into a unified cost map, so as to accurately reflect the distribution of all obstacles;

[0013] Step 3: After completing the detection and map integration of dynamic and low obstacles in the environment, the present invention enters the local path planning and waypoint optimization stage;

[0014] Step 3.1: Set the maximum radius R m and the angular range θ r centered at the current position of the unmanned vehicle to form a fan-shaped search area, and obtain a number of candidate waypoints by uniformly discretizing and sampling the radius and the angle;

[0015] Step 3.2: Calculate the minimum safety distance from each candidate waypoint to the dynamic obstacle, the nearest distance to the static and low obstacles, the deviation angle relative to the preset path direction, and the distance suitability to the current task target point in sequence, and synthesize them into a unified cost function through linear weighting or priority strategy, sort all candidate waypoints, select the one with the minimum comprehensive cost and send it to the underlying execution module as the local navigation command;

[0016] Step 4: After completing the dynamic planning of the local waypoints, the present invention proposes an adaptive deviation correction mechanism for the problem that the checkpoint may be occupied by obstacles;

[0017] Step 4.1: In the inspection or operation task, the unmanned vehicle goes to the target position one by one according to the preset checkpoints; once it is detected in real time that a certain checkpoint is occupied by a static or long-term staying dynamic obstacle, the regular planning for this point is interrupted and the deviation correction process is entered;

[0018] Step 4.2: Around the obstacle occupancy area, construct a local fan-shaped area again and sample and optimize multiple indicators for its waypoints; select a nearest and feasible new waypoint from the obtained candidate waypoints and update it as an alternative checkpoint;

[0019] Through the above steps, the present invention can make full use of multi-source sensors to detect dynamic and low obstacles in real time in a complex outdoor environment, combine the detection results with local planning, enable the unmanned vehicle to avoid obstacles autonomously while maintaining follow-up of the preset task route; once a certain checkpoint is affected by a temporary obstacle and is inaccessible, it can also automatically perform deviation correction and update to avoid global task failure. The present invention can also maintain a high detection and planning efficiency under limited hardware resources, greatly improving the safety and success rate of the unmanned vehicle performing inspections, deliveries and other operations in the outdoor environment. BRIEF DESCRIPTION OF THE DRAWINGS

[0020] The accompanying drawings forming a part of the present invention are used to provide a further understanding of the present application. The schematic embodiments of the present application and their descriptions are used to explain the present application and do not constitute an improper limitation to the present invention.

[0021] Figure 1 is the overall system flowchart of the present invention;

[0022] Figure 2 is the overall system framework diagram of the present invention;

[0023] Figure 3 It is a schematic diagram of the waypoint optimization algorithm of the present invention;

[0024] Figure 4 It is the implementation effect diagram of the obstacle perception algorithm based on multi-detector fusion of the present invention;

[0025] Figure 5 It is the navigation effect diagram of the present invention applied to the actual unmanned vehicle for inspection tasks; Specific implementation method

[0026] Step 1: Installation and calibration of multi-source sensors; Mount multi-source sensors such as the RealSense D455 binocular depth camera and the Mid-360 lidar on the unmanned vehicle, and ensure the stable attitude of the sensors and collect sensor data through rigid connection. Using the checkerboard calibration method, place the calibration board about 2m in front of the unmanned vehicle, adjust the installation pose of the camera and the lidar, so that the calibration board completely covers the field of view of the depth camera; Calculate the external parameter matrix between the camera and the lidar through the tf2 tool chain of ROS2 to achieve the unified coordinate mapping of the subsequent point cloud and depth data. In terms of data synchronization, the hardware trigger mode is adopted, and the lidar and the depth camera are triggered to sample simultaneously through the GPIO interface, and the timestamp alignment error is less than 1ms, so as to ensure the accurate corresponding relationship between the two types of data in space.

[0027] Step 2: Obstacle perception based on multi-detector fusion; The KDSCAN algorithm first constructs a KD tree index in the lidar point cloud, and sets the neighborhood search radius to 0.3m and the minimum clustering point number to 10; When the distance between adjacent points is less than this threshold, they are classified into the same class, and the isolated points are removed and the three-dimensional bounding box of the obstacle is output. The U-depth algorithm converts the depth map D(u, v) into a top-down projection, extracts the obstacle edge through the Sobel operator and combines the depth threshold to generate a two-dimensional bounding box, and then back-projects it into the three-dimensional coordinate system to obtain the height and position of the obstacle. The YOLO-IQR algorithm detects the target area in the RGB image based on the YOLOv5 network; The pixel depth on the corresponding depth map is filtered by the interquartile range, and its effective range can be expressed as: d ∈ [Q1 - α(Q3 - Q1), Q3 + α(Q3 - Q1)] Among them, Q1 and Q3 are the first and third quartiles respectively, which are the values at the 25% and 75% positions respectively after sorting the pixel depth values in the depth map from smallest to largest. α is an adjustable parameter used to control the width of the effective interval, usually set to 1.5. This is because in statistics, α = 1.5 is an empirical rule based on Tukey's fences, which can effectively eliminate most outliers while retaining about 99.3% of the normal data distribution, and is suitable for dealing with the noise or outliers that may exist in the depth map. Map the pixels that meet this interval to the internal and external parameters of the camera to obtain the center position and three-dimensional size of the obstacle. Denote the detection results of dynamic obstacles of KDSCAN, U-depth, and YOLO-IQR as {B k}, {B u} and {B y} respectively. The intersection over union (IoU) between every two detection results can be expressed as: where B i and B j are the bounding boxes corresponding to any two algorithms. By calculating their IoU, the detection consistency between different methods can be evaluated, so as to fuse effective information and eliminate outliers. In dynamic obstacle detection, if the intersection ratio of two detection results is greater than 0.7, and the position difference does not exceed 0.1 meter, and the size differences are less than 0.025 meters (length, width) and 0.0175 meters (height) respectively, they are regarded as the same obstacle. The IoU threshold is set to 0.7 to ensure significant overlap between detection results and reduce false detections; the position and size tolerances are dynamically adjusted based on sensor accuracy and the actual scenario to avoid misjudgment due to measurement errors. By reasonably setting these thresholds, the system can accurately identify and track obstacles. The fusion process of the results of the three detectors can be expressed as: where N is the number of detected results that match, B n is the bounding box of the dynamic obstacle output by different detectors, representing a single detection result, and B f is the weighted average fusion result of multiple B n , which improves consistency and accuracy by integrating the detection frames of different algorithms. Finally, the prediction equation of the Kalman filter is: where x t+1 is the state vector at the next moment, is the deterministic evolution of the system state, and F is the state transition matrix that describes how the current state x t is linearly mapped to the state at the next moment. Assuming the obstacle moves at a constant speed, the value of F is: where Δt is the time interval, represents the uncertainty disturbance of the system state, G is the noise driving matrix, which determines the influence mode of the process noise w t on the state, and the process noise w t is Gaussian noise with a mean of 0 and a covariance of Q, representing the unmodeled dynamic disturbance. Assuming the obstacle moves at a constant speed, the value of G is: By estimating the speed and short-term trajectory of the obstacle, the motion information of the dynamic obstacle can be obtained.

[0028] Step 3: Detection of low obstacles and update of the static obstacle map; According to the installation height and scanning range of the lidar, objects below 0.2 meters are regarded as low obstacles. The low obstacle detection module calls the Grounded-SAM2 model to perform pixel-level segmentation on possible low obstacles after inputting the RGB image and text prompt, generating a mask. The mask is projected onto the bird's-eye view coordinate system through inverse perspective mapping, and then combined with the corresponding depth map to calculate the height of the obstacle, thus obtaining the point cloud of the low obstacle. For the detected dynamic obstacles, if there is an obvious overlap between their spatial bounding boxes and this point cloud, that is, the intersection over union is greater than 0.5 meters, the corresponding point cloud in the dynamic obstacles is removed during the fusion stage to avoid interference; the remaining retained point cloud is downsampled by voxel filtering and then imported into the cost map, and the static obstacle area is marked in the cost map with an inflation radius of 0.5m for subsequent obstacle avoidance planning to call.

[0029] Step 4: Sector area path planning and local waypoint optimization; The path planner sets a fan-shaped search area with a maximum radius of R r , y r ) directly in front of the current position of the unmanned vehicle, with a maximum radius of R m = 4m, an angular range of θ r = π, a radial sampling number of N r = 20, and an angular sampling number of N a = 30 for uniform sampling. The radial position r i is evenly distributed at equal intervals, with a step size of Therefore, each radial position is: r i = i * Δr, i ∈ {1, 2,..., N r} The angular position θ j is uniformly distributed within the range [-θ r , θ r ], with a step size of: So, each angular position is: θ j =-θ r +j*Δθ, j ∈ {1, 2, ..., N a -1} Finally, the position of the candidate waypoint is converted to Cartesian coordinates based on the unmanned vehicle as: To determine the optimal waypoint, the algorithm comprehensively evaluates each candidate waypoint, and the evaluation metrics include: The comprehensive cost C of each waypoint can be denoted as: C(x ij , y ij ) = w o C o + w s C s + w d C d + w do C do where C do represents the cost of dynamic obstacles, and this cost is used to avoid collisions between the unmanned vehicle and dynamic obstacles during path planning. It consists of two parts: distance penalty C dd and direction penalty C dr . The distance penalty measures the minimum Euclidean distance between the waypoint and the predicted trajectory of the dynamic obstacle: where d ij (t) is the instantaneous shortest distance from the waypoint to the dynamic obstacle, x b (t) and y b (t) are the predicted trajectories of the dynamic obstacle, and T is the time of the predicted trajectory of the dynamic obstacle, set to 2 seconds. This formula ensures that the closer the waypoint is to the dynamic obstacle, the greater its cost value, so as to avoid these waypoints as much as possible during the optimization process. To avoid obstacles in advance, the relative position vector of the waypoint relative to the obstacle is compared with the motion direction vector of the obstacle. The calculation formula of the direction vector is as follows: C dir is the cost of path direction consistency, obtained by calculating the deviation of the waypoint from the preset path orientation. By calculating the dot product of vectors, it is determined whether the waypoint is in the motion direction of the obstacle: If the waypoint is in the moving direction of the obstacle, the penalty value for this item is 1, increasing its cost value; otherwise, the penalty value for this item is 0. The maximum direction penalty within the prediction time is: The overall dynamic obstacle cost C that combines distance and direction penalties do is: C do = w dd C dd + w dr C dr where w dd , w dr are weight parameters, both taking the value of 0.5. C o is the static obstacle cost, which is used to penalize waypoints close to static obstacles. The calculation formula is as follows: where d o is the distance from the waypoint to the nearest static obstacle. This formula ensures that the closer the waypoint is to the static obstacle, the greater its cost, thus preferentially selecting waypoints far from static obstacles. C s is the path smoothness cost. This cost item is used to encourage as smooth an angular change as possible in waypoint selection, thus avoiding sharp turns. Its calculation formula is as follows: θ p is the turning angle between adjacent waypoints, θ t is the ideal path angle, θ r is the maximum angular range of the entire sector area. This formula ensures that if the direction change of the waypoint is large, the cost value is high, avoiding the selection of this waypoint and promoting the path to remain as smooth as possible. C d is the target distance cost. The target distance cost is used to ensure that the waypoint is not too far or too close to the target point, but on the ideal path. Its calculation formula is as follows: where d t is the Euclidean distance from the waypoint to the target point. The maximum value of this formula appears at , that is, the optimal position of the target point. If the waypoint is too close to or too far from the target point, this cost will increase. w o , w s , w d , w doThey are the weight parameters of static obstacle, path smoothness, target distance, and dynamic obstacle respectively, all with a value of 0.25. After weighted summation and sorting all waypoints in ascending order of C value, the waypoint with the optimal comprehensive cost is selected and sent to the lower-level controller, and the unmanned vehicle executes navigation. This method optimizes waypoints by integrating multiple cost terms, effectively improving the rationality of path selection, making the path as far away from obstacles as possible, while remaining smooth and having a clear target.

[0030] Step 5: Adaptive correction of inspection points; To cope with new obstacles emerging at inspection points, the system designs a dynamic correction strategy applicable to both static and dynamic obstacles. When a static obstacle appears at the inspection point, the inspection point correction should be immediately executed. For dynamic obstacles, if they stay in the inspection point area for more than 60 seconds, it is determined as a long-term stay, and the inspection point also needs to be adjusted. The specific correction method is: select the optimal waypoint closest to the original inspection point from the planned path to replace the blocked inspection point. This method ensures that the unmanned vehicle can still continue to move along the planned path with the smallest path deviation when encountering obstacles. By dynamically adjusting the inspection points, the system effectively reduces the impact of unexpectedly emerging static obstacles on path planning and significantly improves the adaptability and robustness of the navigation system.

[0031] To make the explanation of this method more intuitive, the above text and figures describe a specific process step by step, but it is not limited by the operation sequence. Those skilled in the art can adjust the timing or parameters of each step according to actual needs, as long as these changes are still within the technical idea and scope of the claims of the present invention, they belong to the protection scope of the present invention.

[0032] In summary, through the design of key links such as multi-source perception fusion, fan-shaped area planning, and inspection point correction, the present invention can efficiently identify dynamic and low obstacles in an environment with limited hardware resources, and timely generate feasible alternative points for the situation where the inspection point is occupied by obstacles for a long time, significantly improving the autonomous obstacle avoidance ability and inspection success rate of the unmanned vehicle in complex outdoor scenarios. Those skilled in the art can replace or improve each module on this basis, including replacing the detection algorithm network, adjusting the weight of the cost function, or using different correction strategies, etc., which should all be regarded as falling within the protection scope of the present invention.

Claims

1. An autonomous navigation method for unmanned vehicles in complex outdoor environments, characterized in that, It includes the following steps: Step 1: Obtain lidar, RGB image, and depth image data and achieve data alignment; Step 2: Use the KDSCAN algorithm to construct a KD-tree index for point cloud obstacle clustering; U-depth extracts obstacle coordinates through depth map projection and edge detection; YOLO-IQR performs object detection on the image, filters out depth noise using the interquartile range method, and calculates the three-dimensional coordinates of the obstacles; perform intersection over union (IoU) consistency verification and geometric consistency check on the above three detection results to obtain the positions of dynamic obstacles, and combine Kalman filtering to predict the obstacle trajectories; Use Grounded-SAM2 to segment low obstacles in the image, and integrate the obstacle position information into the cost map through inverse perspective transformation; Step 3: Generate a fan-shaped search area centered on the unmanned vehicle and perform uniform sampling; Adopt a weighted cost function to comprehensively consider the distances to dynamic obstacles, static obstacles, direction deviation, and the distance to the target point, calculate the comprehensive cost of each waypoint, and select the waypoint with the minimum cost as the local navigation waypoint; Step 4: When there is a static obstacle or a dynamic obstacle that has stayed for more than 60 seconds at a waypoint, determine that the point is unreachable, trigger a new round of sampling and optimization, and update the waypoint.

2. The method according to claim 1, wherein In Step 2, the effective depth interval for using the interquartile range method is: d ∈ [Q1 - α(Q3 - Q1), Q3 + α(Q3 - Q1)] where the value of parameter α is 1.5, and Q1 and Q3 are the values located at the 25% and 75% positions respectively after arranging the pixel depth values in the depth map in ascending order.

3. The method according to claim 1, characterized in that, In step 3, the cost calculation method for the waypoint (x ij , y ij ) is: C(x ij , y ij ) = 0.25(C o + C s + C d + C do ).

4. The method according to claim 1, wherein Step 1: Obtain lidar, RGB image, and depth image data and achieve data alignment; Step 2: Obstacle perception based on multi-detector fusion; The KDSCAN algorithm first constructs a KD-tree index in the lidar point cloud and sets the neighborhood search radius to 0.3m and the minimum number of clustering points to 10; when the distance between adjacent points is less than this threshold, they are classified into the same class, and after removing isolated points, the three-dimensional bounding box of the obstacle is output; The U-depth algorithm converts the depth map D(u, v) into a top-down projection, extracts the obstacle edges through the Sobel operator and combines with the depth threshold to generate a two-dimensional bounding box, and then back-projects it into the three-dimensional coordinate system to obtain the height and position of the obstacle; The YOLO-IQR algorithm detects the target area in the RGB image based on the YOLOv5 network; filter the pixel depths on the corresponding depth map using the interquartile range interval, and its effective range is expressed as: d ∈ [Q1 - α(Q3 - Q1), Q3 + α(Q3 - Q1)] where Q1 and Q3 are the first and third quartiles respectively, which are the values located at the 25% and 75% positions after arranging the pixel depth values in the depth map in ascending order; α is set to 1.5; Map the pixels that meet this interval to the internal and external camera parameters to obtain the center position and three-dimensional size of the obstacle; record the detection results of the dynamic obstacles of KDSCAN, U-depth, and YOLO-IQR as {B k},{B u} and {B y}, and the intersection over union between every two detection results is expressed as: Among them, B i and B j are the bounding boxes corresponding to any two algorithms. In dynamic obstacle detection, if the intersection ratio of the two detection results is greater than 0.7, and the position difference does not exceed 0.1 meter, and the size differences are less than 0.025 meter in length or width and less than 0.0175 meter in height respectively, they are regarded as the same obstacle; the fusion process of the results of the three detectors is expressed as: where N is the number of detected results matched, and B n is the dynamic obstacle bounding box output by different detectors, representing a single detection result, and B f is multiple B n 's weighted average fusion result; The final prediction equation in the Kalman filter is: where x t+1 is the state vector at the next moment, the deterministic evolution of the system state, F is the state transition matrix describing the current state x t how to linearly map to the state at the next moment. Assuming the obstacle moves at a constant speed, the value of F is: where Δt is the time interval, represents the uncertainty disturbance of the system state, G is the noise driving matrix, which determines the influence mode of the process noise w t on the state, and the process noise w t is Gaussian noise with a mean of 0 and a covariance of Q, representing the unmodeled dynamic disturbance. Assuming that the obstacle moves at a constant speed, the value of C is: Estimate the speed and short-term trajectory of the obstacle to obtain the motion information of the dynamic obstacle; Step 3: Detection of low obstacles and update of the static obstacle map; According to the installation height and scanning range of the lidar, objects below 0.2 meters are regarded as low obstacles; The low obstacle detection module calls the Grounded-SAM2 model to perform pixel-level segmentation on possible low obstacles after inputting the RGB image and text prompt to generate a mask; The mask is projected onto the bird's-eye view coordinate system through inverse perspective mapping, and then combined with the corresponding depth map to calculate the obstacle height, thereby obtaining the point cloud of low obstacles; For the detected dynamic obstacles, if there is an obvious overlap between their spatial bounding boxes and the point cloud, that is, the intersection over union is greater than 0.5 meters, the corresponding point cloud in the dynamic obstacles is removed during the fusion stage to avoid interference; The remaining retained point cloud is downsampled by voxel filtering and then imported into the cost map, and a dilation radius of 0.5m is set to identify the static obstacle area in the cost map; Step 4: Sector area path planning and local waypoint optimization; The path planner sets a fan-shaped search area with a maximum radius of R r , y r ) directly in front of the current position (x n = 4m, an angular range of θ r = π, a radial sampling number N r = 20, an angular sampling number N a = 30 for uniform sampling; The radial position r i is evenly distributed with a step size of Therefore, each radial position is: r = i * Δr, i ∈ {1, 2,..., N r} Angular position θ j is uniformly distributed within the range [-θ r , θ r with a step size of: So, each angular position is: θ j = -θ r + j*Δθ, j ∈ {1, 2,..., N a - 1} Finally, the position of the candidate waypoint is converted to Cartesian coordinates based on the unmanned vehicle as: To determine the optimal waypoint, the algorithm comprehensively evaluates each candidate waypoint, and the evaluation indicators include: The comprehensive cost C of each waypoint is denoted as: C(x ij ,y ij ) = w o C o + w s C s + w d C d + w do C do Among them, C do represents the cost of dynamic obstacles, which consists of a distance penalty C dd and a direction penalty C dr and is composed of two parts; the distance penalty measures the minimum Euclidean distance between the waypoint and the predicted trajectory of the dynamic obstacle: where d ij (t) is the instantaneous shortest distance from the waypoint to the dynamic obstacle, x b (t) and y b (t) is the predicted trajectory of the dynamic obstacle, T is the time of the predicted trajectory of the dynamic obstacle, set to 2 seconds; In order to avoid obstacles in advance, the relative position vector of the waypoint with respect to the obstacle is compared with the movement direction vector of the obstacle The calculation formula of the direction vector is as follows: C dir It is the path direction consistency cost, which is obtained by calculating the deviation of the waypoint from the preset path orientation. The dot product of vectors is calculated to determine whether the waypoint is in the moving direction of the obstacle: If the waypoint is in the moving direction of the obstacle, the penalty value for this item is 1, increasing its cost value; otherwise, the penalty value for this item is 0; The maximum direction penalty within the prediction time is: The overall dynamic obstacle cost C that combines distance and direction penalties do is as follows: C do = w dd C dd + w dr C dr where ω dd , ω dr are weight parameters, both taking the value of 0.

5. C o is the cost of static obstacles, and the calculation formula is as follows: where d o is the distance from the waypoint to the nearest static obstacle; C s is the path smoothness cost, and the calculation formula is as follows: θ p is the steering angle between adjacent waypoints, θ t is the ideal path angle, θ r is the maximum angular range of the entire fan-shaped area; C d is the target distance cost, and the calculation formula is as follows: where d t is the Euclidean distance from the waypoint to the target point, and the maximum value of this formula appears at i.e., the optimal position of the target point. If the waypoint is too close to or too far from the target point, this cost will increase; ω o ,ω s ,ω d ,ω do are the weight parameters of static obstacle, path smoothness, target distance, and dynamic obstacle respectively, all taking the value of 0.

25. After weighted summation, all waypoints are sorted in ascending order of C value, and the waypoint with the optimal comprehensive cost is selected and sent to the underlying controller, and the unmanned vehicle executes navigation; Step 5: Adaptive correction of inspection points; When a static obstacle appears at the inspection point, the inspection point should be immediately corrected; For dynamic obstacles, if they stay continuously in the inspection point area for more than 60 seconds, it is determined as a long-term stay, and the inspection point also needs to be adjusted; The adjustment method is: Select the optimal waypoint closest to the original inspection point from the planned path to replace the blocked inspection point.

Images