Unmanned platform active path planning method based on viewpoint adaptive push-off

Abstract

The invention provides an unmanned platform active path planning method based on viewpoint adaptive push-away, which comprises the following steps: constructing an occupation map in an unknown environment and carrying out leading edge detection to generate a candidate viewpoint set; obtaining the distance between the candidate viewpoint and the current position of the unmanned platform, and dividing the candidate viewpoint into a short-distance interval, a middle-distance interval and a long-distance interval according to the distance; for candidate viewpoints located in the middle distance interval, viewpoint self-adaptive push-off is executed based on a unit direction vector of a connecting line from the current position of the platform to the leading edge center point, and push-off viewpoints are obtained; performing trafficability detection on the two normal sides based on the push-off direction, and applying small-angle disturbance on the reachable side to obtain a final viewpoint; and performing environment information preprocessing and path sequence planning based on the final viewpoint set, and generating an execution path meeting constraint conditions. According to the method, the exploration coverage rate of the corner area can be enhanced, and high-cost backtracking is reduced, so that the exploration efficiency and the path planning quality in an unknown environment are improved.

Description

Technical Field

[0001] This invention belongs to the field of autonomous navigation and environmental exploration technology for unmanned platforms, specifically involving an active path planning method for unmanned platforms based on viewpoint adaptive push-away. Background Technology

[0002] With the development of sensors, computing platforms, and intelligent algorithms, unmanned platforms are widely used in scenarios such as disaster search and rescue, environmental monitoring, security patrol, and warehousing and logistics. Among them, wheeled mobile robots and multi-rotor drones serve as typical carriers, widely used for autonomous exploration tasks in unknown or semi-unknown environments. During autonomous exploration, unmanned platforms typically need to complete path planning and target point selection while constructing an environmental map, in order to achieve full coverage of unknown areas within a limited time.

[0003] Existing active exploration methods mainly include front detection-based exploration strategies and sampling-based exploration strategies. Front detection-based methods identify the boundary grid between known and unknown spaces as the front and use the nearest front or the front with the greatest information gain as the navigation target. They have the advantages of simple implementation and relatively low computational cost, but they generally lack modeling and utilization of environmental structural features. In scenes with complex structures and many corners and occluded areas, they are prone to problems such as sparse viewpoints in corner areas, incomplete coverage, and high-cost backtracking, resulting in more redundancy in the exploration path and a decrease in overall efficiency.

[0004] Sampling-based exploration methods select targets by randomly or heuristically sampling candidate viewpoints in free space and comprehensively considering factors such as information gain, path cost, and reachability. These methods offer good global search capabilities and flexibility, but typically require information gain evaluation and path searching for a large number of sampled viewpoints, resulting in a heavy computational burden. Furthermore, because structural features such as corners and narrow passages are not explicitly characterized, viewpoint distribution can be uneven, risking the omission of crucial spatial information in corner areas, thus affecting overall coverage. As the task space scale increases, frequent frontier extraction and voxel-based gain statistics further increase computational overhead, limiting the real-time response capability of the path planning module.

[0005] In summary, existing exploration methods generally suffer from insufficient coverage of corner areas, redundant backtracking paths, and high planning computation costs in large-scale and structurally complex environments. They are difficult to balance path planning efficiency and path quality while ensuring the integrity of exploration. There is an urgent need for a path planning method that can actively guide corner areas and balance exploration efficiency and real-time planning. Summary of the Invention

[0006] To address the aforementioned issues, this invention discloses an active path planning method for unmanned platforms based on viewpoint adaptive push-away. By introducing viewpoint adaptive push-away and normal small perturbation, the coverage capability of the viewpoint in the corner area is significantly improved, reducing high-cost backtracking. Combined with lightweight gain estimation and local path cost calculation, the front-end planning overhead is effectively reduced, and the exploration efficiency and real-time performance of path planning are improved.

[0007] To achieve the above objectives, the technical solution of the present invention is as follows:

[0008] An active path planning method for unmanned platforms based on viewpoint adaptive push-away includes: constructing a two-dimensional terrain grid map based on simultaneous localization and mapping (SLT) methods, detecting leading edge units between known and unknown areas on the map, clustering the leading edge units to form leading edge regions, and calculating the initial position of the center of each leading edge region as a candidate viewpoint; obtaining the distance between the current position of the unmanned platform and the center point of the leading edge, and classifying the leading edge into three categories: near leading edge, medium leading edge, and far leading edge according to preset near and far distance thresholds.

[0009] For near-distance and far-distance front edges, a viewpoint selection method based on the nearest passable point is adopted to reduce viewpoint adjustments and path detours. For mid-distance front edges, the present invention introduces an adaptive viewpoint push-away mechanism: based on the direction vector from the unmanned platform to the center of the front edge, and according to the position of the front edge distance within the mid-distance interval, the push-away step size is determined, causing the viewpoint to move along this direction into the effective perception area in front of the platform, thereby expanding the viewpoint's visibility range to corners and occluded areas and reducing omissions. On this basis, using the normal direction of the push-away direction as a reference, small lateral perturbations are applied to both sides of the normal direction based on the passability detection results to fine-tune the viewpoint position or orientation, thereby improving the spatial diversity of viewpoint distribution and enhancing the guidance capability for corner areas.

[0010] Regarding viewpoint evaluation, this invention constructs a lightweight information gain model based on the number of leading edge units, using the number of leading edge units within the viewpoint's visible range as the gain index to replace the traditional voxel statistics method and reduce computational overhead. Regarding path cost acquisition, the path distance estimate between the unmanned platform and the target viewpoint is obtained by dynamically selecting between a local grid map and a sparse route map. Based on this, existing path sequence optimization and path smoothing methods are used to generate an executable path that satisfies the dynamic constraints of the unmanned platform.

[0011] The beneficial effects of this invention are as follows:

[0012] The present invention discloses an active path planning method for unmanned platforms based on viewpoint adaptive push-away. It introduces viewpoint adaptive push-away and normal small perturbation, which significantly improves the coverage of the viewpoint in the corner area and reduces high-cost backtracking. Combined with lightweight gain estimation and local path cost calculation, it effectively reduces the front-end planning overhead and improves exploration efficiency and real-time performance of path planning. Attached Figure Description

[0013] Figure 1 This is a system flowchart of a path planning method according to an embodiment of the present invention;

[0014] Figure 2 This is a flowchart of a viewpoint adaptive push-away and small perturbation optimization method according to an embodiment of the present invention;

[0015] Figure 3 This is a schematic diagram of the viewpoint adaptive push-away and small perturbation optimization process according to an embodiment of the present invention;

[0016] Figure 4 This is an illustration of the effect of path smoothing and end-point push-off correction in an embodiment of the present invention. Detailed Implementation

[0017] The present invention will be further illustrated below with reference to the accompanying drawings and specific embodiments. It should be understood that the following specific embodiments are for illustrative purposes only and are not intended to limit the scope of the invention.

[0018] The unmanned platform of this invention can be a wheeled mobile robot or a multi-rotor drone, equipped with lidar and an onboard computing unit, to perform autonomous exploration tasks in unknown indoor environments. The system as a whole adopts a hierarchical structure of "front-end path generation - back-end path optimization," such as... Figure 1 As shown: The front-end module constructs a 3D grid map with occupancy information based on the simultaneous localization and mapping algorithm, and generates a 2D occupancy grid map for path planning through accessibility analysis; on this basis, it performs front edge detection and candidate viewpoint extraction, and completes the adaptive push-away of viewpoints and small perturbation optimization as well as environmental information preprocessing; the back-end module can optimize and generate feasible exploration paths based on the final viewpoint set, and smooth and correct the paths.

[0019] like Figure 1 and Figure 2 As shown, the active path planning method for unmanned platforms based on viewpoint adaptive push-off according to the present invention includes the following steps:

[0020] Step 1: Construct an occupation map in the unknown environment and perform front detection to generate a set of candidate viewpoints;

[0021] Step 2: Obtain the distance between each frontal center point and the current position of the unmanned platform. The candidate viewpoints are divided into near-distance, medium-distance, and far-distance ranges based on their distance.

[0022] Step 3: For candidate viewpoints located in the mid-distance range, based on the platform's current position... To the center point of the front Unit direction vector of the connecting line Perform adaptive push-away viewpoint to obtain the push-away viewpoint. ;

[0023] Step 4: Perform accessibility detection on both sides of the normal direction based on the push-off direction, apply a small-angle perturbation on the accessible side, and obtain the final viewpoint;

[0024] Step 5: Perform environmental information preprocessing based on the final viewpoint set. The environmental information preprocessing includes: viewpoint information gain calculation and path cost estimation; and perform path sequence planning based on the environmental preprocessing results to generate execution paths that meet the constraints.

[0025] In step one, the unmanned platform obtains its own pose estimate based on laser SLAM or other simultaneous localization and mapping methods, and obtains its current pose in the world coordinate system. A 3D grid map with occupancy information is constructed. Through connectivity analysis, the 3D map is projected into a 2D occupancy grid map for planning. On the current 2D occupancy grid map, cells adjacent to unknown cells and which are themselves free cells are marked as frontier cells. Several frontier regions are obtained through connectivity clustering, and the geometric center point of each frontier region is calculated. , Indicates the first There are several frontier regions. The initial positions of the center points of each frontier region constitute a set of candidate viewpoints.

[0026] In step two, in order to incorporate distance information into the viewpoint optimization process, this embodiment calculates the current position of the unmanned platform. With each frontier center point Euclidean distance between The frontier is then divided into different strategy intervals based on the relationship between this distance and a preset threshold. The distance piecewise function is constructed as follows:

[0027] (1);

[0028] in, This indicates the nearest-point strategy. This represents the viewpoint adaptive push-away and small perturbation optimization strategy. This represents the lower limit of the range of medium-distance intervals, and the corresponding upper limit. Difference It can be flexibly adjusted according to the platform's task requirements and environmental scale.

[0029] For satisfying or For the frontier area, i.e., the area that is too close or too far away, in order to ensure the feasibility of the path and minimize the travel cost of exploring the path, the system adopts a viewpoint selection method based on the nearest passable point. Search for the nearest and reachable free grid near the current location in the foreground region as the viewpoint location.

[0030] For falling into the mid-range In the leading-edge region, this embodiment performs adaptive viewpoint shifting based on the initial candidate viewpoints to improve the perception coverage of the corner areas. For example... Figure 3 As shown, first calculate from the current position Pointing to the center of the front unit direction vector This direction vector is used as the reference direction for viewpoint displacement. Then, the viewpoint is adaptively displaced along this direction to obtain the displaced viewpoint position. :

[0031] (2);

[0032] in, It is an adaptive shift factor, and its value varies with... exist It changes with the position within the interval. When When approaching the two ends of the interval, Take smaller values ​​to avoid over-extension; when When taking the median value A larger value is chosen to more fully expose the unknown areas behind the leading edge. Through the above adaptive push-away operation, the viewpoint is moved from the high-density area near the center of the leading edge to a position that is more conducive to observing the corner areas, thereby enhancing the perception of areas with limited visibility, such as corners, from the source.

[0033] Subsequently, to further enhance the visibility of corners or obstructed areas and the spatial balance of the viewpoint, this embodiment pushes the viewpoint away. Based on this, a small perturbation optimization strategy is introduced in the normal direction. The system is based on the direction vector. Construct its unit normal direction, generate candidate perturbation directions on both sides of the normal direction, and perform connectivity detection on the candidate perturbation directions in conjunction with the current raster map. Specifically, first calculate the connectivity with... Perpendicular unit normal vector To push away the viewpoint Based on, in and Candidate viewpoints are generated for the left and right sides respectively, based on the direction. For viewpoints from the current position... The path to each candidate viewpoint is checked on a 2D occupied grid map to see if it crosses obstacle grids. If the path is still reachable in that direction after a small angular offset and does not cross obstacle grids, the perturbation direction is considered feasible. When both the left and right sides meet the passage conditions, the system can randomly select one side to apply a perturbation to expand the perception range and increase the diversity of viewpoint spatial distribution; when only one side is passable, the perturbation is applied to the passable side; when neither side is feasible, the viewpoint is retained and pushed away. As the final viewpoint Through the above small perturbation optimization, the final generated viewpoints exhibit a moderately divergent spatial distribution around the corner areas. Compared to generating viewpoints only near the front center, this is more conducive to covering easily overlooked areas such as corners and narrow passages, and reduces field-of-view overlap and redundant paths.

[0034] After completing the aforementioned viewpoint adaptive shift and small-perturbation optimization, the system obtains a final set of viewpoints that combines edge and corner coverage with feasibility. To improve planning efficiency while ensuring exploration quality, this implementation performs environmental information preprocessing and path sequence planning based on this viewpoint set to generate the execution path of the unmanned platform. The environmental information preprocessing includes information gain calculation and path cost estimation. In an optional implementation, a lightweight information gain evaluation method based on the number of leading edge units can be used to quickly screen candidate viewpoints. Specifically, for candidate viewpoints... Determine the set of frontier units within its visible range. The information gain of this viewpoint is defined as:

[0035] (3);

[0036] in, From the perspective Information gain value; To indicate the viewpoint The set of frontier units within the visible range; For the frontier unit The information contribution is calculated by statistically analyzing the number and weight of visible frontier units to approximate the observational value of the viewpoint, without requiring detailed volumetric statistics for all unknown voxels. Using this information gain model, the system can... Candidate viewpoints are ranked, discarding those with low information gain and overly dense spatial distribution, retaining a representative set of target viewpoints for subsequent path planning. As another important part of environmental information preprocessing, the system further performs path cost estimation to obtain path cost information between the current position of the unmanned platform and each target viewpoint, as well as between each pair of target viewpoints, providing input for path sequence planning.

[0037] In the path cost estimation stage, a sparse route graph is constructed to reduce the computational overhead caused by repeated path searches in large-scale environments. The system uniformly samples the free grid within a local window centered on the current location to generate candidate graph nodes, and removes sampled points whose distance from existing nodes is less than a threshold to ensure the representativeness and sparsity of the node distribution. Subsequently, a neighborhood search method based on KD-tree is used to select several neighboring nodes for each node within a set radius, and undirected edges are established to form a sparse route graph structure, provided that the connection does not cross obstacles. This structure is updated incrementally with the environment map.

[0038] Within a local area, an A* search can be performed on a 2D grid map to obtain the precise path between the unmanned platform and each viewpoint. When the target viewpoints are sparsely distributed or span a large area, a search can be performed on a sparse route map to obtain the approximate path length. Combining the above-mentioned precise local paths and coarse global paths, a path distance matrix is ​​constructed between "current location - viewpoint" and "viewpoint - viewpoint", providing a foundation for backend path sequence planning.

[0039] In one implementation, the viewpoint access order can be determined based on the path distance matrix using a traveling salesman problem heuristic algorithm or other sequence optimization methods. Combined with a comprehensive cost evaluation mechanism, the path sequence with the lower comprehensive cost is selected from multiple candidate paths. The comprehensive cost evaluation mechanism can use a multi-factor comprehensive cost function to evaluate the path from the current position to each candidate viewpoint, and the comprehensive cost can be expressed as:

[0040] (4);

[0041] in, The path length is obtained from the path distance matrix; This is a boundary cost used to encourage priority coverage of viewpoints closer to the local sliding window boundary; This is a costly measure to encourage priority coverage of viewpoints located near narrow areas such as corners; As a motion consistency cost, it is used to penalize paths that have a large angle with the current motion direction; , , The weighting coefficients for the corresponding cost terms can be set according to task requirements and experience. Through the aforementioned multi-factor comprehensive cost function, a balance can be achieved between path length, boundary region coverage, small-area priority, and motion consistency, further improving the overall path planning quality. To ensure the generated path meets the requirements of path continuity and obstacle avoidance safety, B-spline curves can be used to smooth the discrete path nodes, and local corrections can be made at the path ends when necessary, generating a continuous executable path that meets safety distance and curvature constraints. Figure 4As shown, the smoothed and corrected path can maintain a sufficient safe distance from obstacles in complex environments, resulting in smoother and more stable movement.

[0042] Through the above implementation methods, the present invention introduces a viewpoint adaptive push-away mechanism based on distance segmentation and a normal small perturbation optimization mechanism at the front end, making the spatial distribution of candidate viewpoints around corners and occluded areas more reasonable, significantly improving the coverage of corner areas and reducing high-cost backtracking caused by corner omissions; at the same time, combined with lightweight viewpoint evaluation and efficient path information preprocessing, it provides high-quality target input for back-end path planning, improving the overall exploration efficiency and path planning quality of unmanned platforms in unknown environments while ensuring path feasibility and safety.

[0043] It should be noted that the above content merely illustrates the technical concept of the present invention and should not be construed as limiting the scope of protection of the present invention. For those skilled in the art, various improvements and modifications can be made without departing from the principle of the present invention, and all such improvements and modifications fall within the scope of protection of the claims of the present invention.

Claims

1. A method for active path planning of an unmanned platform based on viewpoint adaptive push-off, characterized in that, include: Step 1: Construct an occupation map in the unknown environment and perform front detection to generate a set of candidate viewpoints; Step 2: Obtain the distance between each frontal center point and the current position of the unmanned platform. The candidate viewpoints are divided into near-distance, medium-distance, and far-distance ranges based on their distance. Step 3: For candidate viewpoints located in the mid-distance range, based on the platform's current position... To the center point of the front Unit direction vector of the connecting line Perform adaptive push-away viewpoint to obtain the push-away viewpoint. ; Step 4: Perform accessibility detection on both sides of the normal direction based on the push-off direction, apply a small-angle perturbation on the accessible side, and obtain the final viewpoint; Step 5: Perform environmental information preprocessing based on the final viewpoint set. The environmental information preprocessing includes: viewpoint information gain calculation and path cost estimation. Based on the environmental preprocessing results, path sequence planning is performed to generate execution paths that meet the constraints.

2. The active path planning method for unmanned platforms based on viewpoint adaptive push-off as described in claim 1, characterized in that, Step one is as follows: The unmanned platform obtains its own pose estimation based on laser synchronous localization and mapping, and obtains its current pose in the world coordinate system. A 3D grid map with occupancy information is constructed. Through connectivity analysis, the 3D map is projected into a 2D occupancy grid map for planning. On the current 2D occupancy grid map, cells adjacent to unknown cells and which are themselves free cells are marked as frontier cells. Several frontier regions are obtained through connectivity clustering, and the geometric center point of each frontier region is calculated. , Indicates the first The initial positions of the center points of each frontier region constitute a set of candidate viewpoints.

3. The active path planning method for unmanned platforms based on viewpoint adaptive push-off as described in claim 2, characterized in that, Calculate the current pose of the unmanned platform With each frontier center point Euclidean distance between The frontier is divided into different strategy intervals based on the relationship between this distance and a preset threshold; the distance piecewise function is constructed as follows: （1）； in This represents the upper limit of the range for a medium-distance interval. This represents the lower limit of the range for a medium-distance interval. , The interval is adjustable, and can be adjusted according to the platform's task requirements and environmental scale. This is within a short distance range. The time falls within the medium distance range. This time belongs to the long distance range. When the distance between the candidate viewpoint and the unmanned platform or At that time, a viewpoint selection strategy based on the nearest passable point is adopted. ,when At that time, an adaptive viewpoint shift and small perturbation optimization strategy is adopted. .

4. The active path planning method for unmanned platforms based on viewpoint adaptive push-off as described in claim 3, characterized in that, The viewpoint adaptive shift in step three includes: Calculate the current position of the unmanned platform The center point of the leading edge region within the mid-range interval unit direction vector The candidate viewpoint is then pushed away in the direction according to the following formula to obtain the position of the pushed-away viewpoint. : （2）； in, This is an adaptive push-away factor, the value of which is determined based on the distance between each leading edge center point and the unmanned platform. In the mid-range The internal position is adaptively adjusted so that when The displacement decreases as it approaches the two ends of the interval. The magnitude of the push increases as it approaches the middle of the interval.

5. The active path planning method for unmanned platforms based on viewpoint adaptive push-off as described in claim 4, characterized in that, The small-angle disturbance in step four includes: From the middle viewpoint Based on, calculate and Vertical unit normal ,exist Accessibility is checked on both sides. The path from the current position of the unmanned platform to each candidate disturbance viewpoint is checked on the two-dimensional occupied grid map. If the path is still reachable and does not cross obstacle grids after a small angular shift in the corresponding disturbance direction, the disturbance direction is determined to be accessible. When both left and right disturbance directions are accessible, a disturbance is randomly selected on one side to slightly shift the viewpoint orientation, thereby expanding the perception range and improving the diversity of viewpoint spatial distribution. When only one disturbance direction is accessible, the disturbance is applied to the accessible side. When both left and right disturbance directions are impassable, the intermediate viewpoint is retained. As the final viewpoint.

6. The active path planning method for unmanned platforms based on viewpoint adaptive push-off as described in claim 1, characterized in that, The environmental information preprocessing in step five includes: For candidate viewpoints Determine the set of frontier units within its visible range. The information gain of this viewpoint is defined as: （3）； in, From the perspective Information gain value; To indicate the viewpoint The set of frontier units within the visible range; For the frontier unit The information contribution is statistically analyzed to obtain the number and weight of leading-edge units, which approximates the observational value of a viewpoint without requiring detailed volumetric statistics for all unknown voxels. Candidate viewpoints are ranked according to the information gain, discarding those with low information gain and overly dense spatial distribution, and retaining... A representative set of target viewpoints is used for subsequent path planning.

7. The active path planning method for unmanned platforms based on viewpoint adaptive push-off as described in claim 1, characterized in that, The path cost estimation in step five, which involves environmental information preprocessing, includes: Candidate graph nodes are generated by sampling a free grid within a local window, centered on the current location of the unmanned platform. Sampling points with a distance less than a preset threshold from existing nodes are removed to obtain a sparse node set. Based on the KD-tree neighborhood search method, neighboring nodes are selected for each node within a set radius. Undirected edges are established under the condition that the connection does not cross the obstacle grid, and a sparse route map is constructed that is updated incrementally with the environment map. The precise path between the unmanned platform and each viewpoint is obtained by using A* search in a local range. When the target viewpoints are sparsely distributed or span a large area, a search is performed on the sparse route map to obtain an approximate path length. Based on this, a path distance matrix is ​​constructed between the current location of the unmanned platform and multiple viewpoints, as well as between each pair of viewpoints.

8. The active path planning method for unmanned platforms based on viewpoint adaptive push-off as described in claim 1, characterized in that, The path sequence planning in step five includes: Based on the path distance matrix, a multi-factor comprehensive cost function is used to evaluate the path from the current position to each candidate viewpoint. Represented as: （4）； in, The path length is obtained from the path distance matrix; This is a boundary cost used to encourage priority coverage of viewpoints closer to the local sliding window boundary; This is a small-area cost, used to encourage priority coverage of viewpoints located near narrow corner areas; As a motion consistency cost, it is used to penalize paths that have a large angle with the current motion direction; , , These are the weighting coefficients for the corresponding cost terms. The multi-factor comprehensive cost function includes path length cost, boundary cost, small area cost, and motion consistency cost. Each cost term is weighted and summed using weight coefficients to obtain the total path cost. Based on the comprehensive path cost and the path distance matrix, a path optimization problem with viewpoint access order as the decision variable is constructed. The traveling salesman problem heuristic is used to search for the viewpoint access sequence with the smaller total comprehensive cost among all candidate viewpoint access sequences, which is taken as the target viewpoint access order for the unmanned platform.

9. The active path planning method for unmanned platforms based on viewpoint adaptive push-off as described in claim 1, characterized in that, Step five also includes: Based on the determined viewpoint access order, a discrete path node sequence is obtained. The discrete path nodes are smoothed using B-spline curves. When a collision or excessive distance between the smoothed path and an obstacle is detected, a push-away local correction is performed on the end of the path along the path normal direction. The push-away intensity is adaptively adjusted according to the shortest distance between the path and the obstacle to generate a continuous executable trajectory that meets the safety distance and curvature constraints.

Images