Adaptive navigation method for legged robot for ship three-dimensional space inspection operation
An adaptive navigation method based on multi-line LiDAR and prior geometric constraints of staircase facades solves the problem of unstable navigation for legged robots in complex scenarios such as narrow staircases on ships. It generates a safe and reliable global path, reduces the risk of collision, and improves the stability and continuity of navigation.
Patent Information
- Authority / Receiving Office
- CN · China
- Patent Type
- Applications(China)
- Current Assignee / Owner
- WUHAN UNIV OF TECH
- Filing Date
- 2026-04-09
- Publication Date
- 2026-06-23
Smart Images
Figure CN122258922A_ABST
Abstract
Description
Technical Field
[0001] This invention relates to the field of autonomous navigation technology for mobile robots, and in particular to an adaptive navigation method for legged robots for three-dimensional spatial inspection operations on ships. Background Technology
[0002] With the increasing application of legged robots in ship inspection operations, their autonomous cross-layer navigation capability has become a key bottleneck restricting large-scale deployment. The internal environment of a ship contains complex three-dimensional structures such as narrow staircases and steep ramps. Traditional navigation methods based on two-dimensional LiDAR or planar maps are insufficient to fully represent such three-dimensional scenes with significant height differences. While multi-line LiDAR can provide high-density three-dimensional point clouds, in spatially confined areas such as narrow staircases on ships, point cloud maps often exhibit discontinuities, distortions, or missing tread information due to viewpoint obstruction and echo loss. How to accurately extract passable areas from incomplete three-dimensional point clouds and generate safe and reliable navigation paths has become a pressing technical challenge in this field.
[0003] However, existing technical solutions often use the starting point, ending point, and current position of the mobile robot as decision parameters to select the corresponding single-layer map to drive the navigation task. The planning process is essentially still a series of independent single-layer navigation stitching operations, making it difficult to generate a geometrically continuous and safe global path in continuous 3D space. More importantly, in complex scenarios such as narrow staircases inside ships, existing methods rely on 3D point clouds with low mapping quality for path planning. Due to noise, discontinuities, or distortions in the point cloud map, the generated path is prone to deviation, directly leading to scraping or even collisions with the robot, posing significant safety hazards. It is difficult to achieve stable and reliable cross-layer navigation for legged robots in complex 3D scenarios such as narrow staircases on ships while ensuring path safety. Summary of the Invention
[0004] The purpose of this invention is to provide an adaptive navigation method for legged robots for three-dimensional space inspection of ships, in order to solve the problem mentioned in the background art that existing legged robots for ship inspection operations are unable to achieve stable and reliable cross-layer navigation in complex three-dimensional scenes such as narrow staircases on ships while ensuring path safety.
[0005] To achieve the above objectives, the present invention provides the following technical solution: an adaptive navigation method for a legged robot for three-dimensional spatial inspection of ships, comprising the following steps: acquiring three-dimensional point cloud data of the ship's internal environment through multi-line lidar and preprocessing it to obtain a homogenized global point cloud; performing ground segmentation on the global point cloud and obtaining a ground point cloud through plane fitting; extracting the facade structure point cloud from the ground point cloud using prior geometric constraints of the staircase facade, and performing load-bearing plane inversion and point cloud completion to generate an enhanced point cloud with confidence weights to characterize the load-bearing area that allows the legged robot to stably land and pass through; evaluating the terrain geometric features of the enhanced point cloud and constructing a drivability evaluation function suitable for the legged robot's body passage and foot stability constraints based on the evaluation results to extract a three-dimensional drivable area; and performing region growth on the three-dimensional drivable area based on voxel multi-feature homogeneity constraints. Clustering is used to construct multiple traversable region clusters, which are then used as a high-level search space. Heuristic search is performed using path length, terrain safety penalty value adapted to the traversal stability of the legged robot, and obstacle distance as transition costs to generate an initial 3D global path. Based on the existing prior mapping path, the guidance weight of the prior mapping path is adaptively adjusted according to the scale metric parameters of each traversable region cluster. The adjusted guidance weight is then integrated with the transition cost to correct and optimize the initial 3D global path, resulting in an optimized global path. Based on the robot's current position, local trajectory planning is performed under the kinematic constraints and dynamic window constraints of the legged robot to obtain multiple candidate trajectories. The fit between the candidate trajectories and the optimized global path, as well as their safety with real-time local obstacles, are comprehensively evaluated. The trajectory with the best evaluation result is selected and sent to the legged robot's underlying controller for execution.
[0006] Optionally, the step of extracting the facade structure point cloud from the ground point cloud using the prior geometric constraints of the staircase facade specifically includes: calculating the local normal vector of each point in the ground point cloud; filtering points whose local normal vectors have an angle less than a preset threshold with the vertical direction to form a candidate point set for the facade structure; fitting the candidate point set for the facade structure using a random sampling consensus algorithm to obtain a set of vertical planes; and filtering the set of vertical planes for planes with consistent normals, arranged in an orderly manner according to the direction of staircase travel and spatially adjacent to each other to obtain a set of staircase facades as the facade structure point cloud.
[0007] Optionally, the steps of performing load-bearing plane inversion and point cloud completion to generate an enhanced point cloud with confidence weights specifically include: estimating the stair travel direction based on the normal principal direction of the stair facade set; robustly estimating the step depth and step height of the stair based on the stair facade set, and filtering out abnormal facades to obtain effective steps; inverting and generating a horizontal load-bearing support plane domain based on the top height, step depth, and lateral width constraints of the effective steps; sampling and generating a load-bearing surface completion point set within the load-bearing support plane domain, and assigning confidence weights to the completion point set of each step based on the plane fitting residuals of the corresponding stair facade and the consistency variance of the step height and step depth parameters; merging the load-bearing surface completion point set with the confidence weights with the global point cloud to obtain the enhanced point cloud.
[0008] Optionally, the step of evaluating the terrain geometry features of the enhanced point cloud and constructing a drivability evaluation function suitable for the legged robot's body passage and foot stability constraints based on the evaluation results to extract a three-dimensional drivable region specifically includes: constructing a neighborhood for each point in the enhanced point cloud, calculating the slope, roughness, and distance to the nearest obstacle; constructing a completion uncertainty modulation term according to the confidence weight; performing a weighted summation of the slope, roughness, the reciprocal of the distance to the nearest obstacle, and the completion uncertainty modulation term to construct a drivability evaluation function suitable for the legged robot's body passage and foot stability; determining points with evaluation function values less than a preset threshold as drivable points to form a drivable point set; and systematizing the drivable point set to obtain a three-dimensional drivable voxel set.
[0009] Optionally, the step of constructing multiple walkable region clusters by performing region growing clustering based on the multi-feature homogeneity constraint of voxels specifically includes: constructing a voxel adjacency graph on the three-dimensional walkable voxel set; constructing a feature vector for each walkable voxel, the feature vector including voxel center height, voxel slope, voxel roughness, obstacle distance features, and load-bearing stability features; defining a weighted feature distance between adjacent voxels, and when the weighted feature distance is less than the clustering threshold, classifying adjacent voxels into the same connected region, obtaining multiple walkable region clusters through region growing, and recording the number of voxels in each region cluster as a scale metric parameter.
[0010] Optionally, the step of using the traversable region cluster as a high-level search space and performing a heuristic search with path length, terrain safety penalty value adapted to the traversal stability of the legged robot, and obstacle distance as transfer costs to generate an initial three-dimensional global path specifically includes: mapping the start point and end point to corresponding voxels in the voxel adjacency graph; for each edge between voxels, calculating the path length cost, terrain safety penalty value, and obstacle distance cost based on the Euclidean signed distance field, and weighting the sum of the three as the transfer cost; and using a heuristic search algorithm to search in the voxel adjacency graph for the path with the minimum transfer cost from the start voxel to the end voxel, which is then used as the initial three-dimensional global path.
[0011] Optionally, the step of adaptively adjusting the guidance weight of the prior mapping path based on the scale metric parameter of each of the accessible region clusters specifically includes: obtaining a pre-stored prior mapping path, which contains multiple path points; defining a minimum distance function from any point in space to the prior mapping path; for each accessible region cluster, comparing its scale metric parameter with a preset scale threshold, and assigning a larger guidance weight if the scale metric parameter is less than the threshold, otherwise assigning a smaller guidance weight.
[0012] Optionally, the step of integrating the adjusted guiding weights with the transition cost to correct and optimize the initial 3D global path specifically includes: calculating the minimum distance from the midpoint of each edge in the voxel adjacency graph to the prior graphing path; constructing a cost term deviating from the graphing path based on the minimum distance; multiplying the cost term deviating from the graphing path by the guiding weight of the corresponding edge's region cluster to obtain the prior cost term; adding the prior cost term to the transition cost to update the edge's comprehensive cost; and performing a heuristic search again based on the updated comprehensive cost to obtain the optimized global path.
[0013] Optionally, the step of performing local trajectory planning under the kinematic constraints and dynamic window constraints of the legged robot specifically includes: extracting a local path of a preset length from the optimized global path based on the current position of the legged robot; extracting real-time local point clouds within a preset range around the legged robot; constructing a velocity dynamic window under the kinematic constraints of the legged robot body, sampling the linear velocity and angular velocity, and generating a candidate trajectory cluster.
[0014] Optionally, the step of comprehensively evaluating the fit between the candidate trajectory and the optimized global path, as well as the safety with real-time local obstacles, specifically includes: for each candidate trajectory, calculating the sum of the shortest distances from each trajectory point to the local path as the fit cost; calculating the sum of the distances from the center of the legged robot's body envelope corresponding to each trajectory point to each point in the real-time local point cloud as the safety cost; weighted summing the fit cost and the safety cost to obtain the trajectory score; selecting the candidate trajectory with the lowest score and sending its corresponding speed command to the legged robot's underlying controller for execution.
[0015] Compared with the prior art, the beneficial effects of the present invention are: This application introduces prior geometric constraints on staircase facades to invert and complete missing stair treads in the point cloud and assigns credibility weights. This effectively solves the problem of broken and misjudged traversable areas caused by incomplete 3D mapping in complex scenarios such as narrow staircases, improving the completeness and planarability of traversable domain representation. Furthermore, by clustering traversable spaces into multi-scale region clusters and adaptively adjusting the guidance weights of prior mapping paths according to the scale of the region clusters, the legged robot can closely follow the verified safe trajectory in narrow areas such as staircases and corridors, while maintaining the freedom of path optimization in open areas. This ensures both global path safety and path optimization. Finally, by combining dynamic window local planning and trajectory comprehensive evaluation, the application achieves the unification of global path tracking and local dynamic obstacle avoidance, significantly reducing the collision risk and instability risk of legged robots in cross-floor inspection operations on ships, and improving the stability, continuity, and robustness of navigation. Attached Figure Description
[0016] Figure 1 This is a schematic diagram of the method steps of the present invention.
[0017] Figure 2 This is a schematic diagram of the simulation map of the present invention.
[0018] Figure 3 This is a schematic diagram illustrating the effect of extracting passable areas according to the present invention.
[0019] Figure 4 This is a schematic diagram of the passable region cluster of the present invention.
[0020] Figure 5 This is a simulation diagram of the prior path cost field of the present invention.
[0021] Figure 6 This is a schematic diagram of the global path guided by the prior path of the present invention.
[0022] Figure 7 This is a schematic diagram of the simulation results of the present invention. Detailed Implementation
[0023] The present invention will now be clearly and completely described in conjunction with the accompanying drawings of the embodiments of the present invention. Obviously, the described embodiments are only some embodiments of the present invention, and not all embodiments.
[0024] It should be noted that the terms "first," "second," etc., in the specification, claims, and accompanying drawings of this application are used to distinguish similar objects and are not necessarily used to describe a specific order or sequence. It should be understood that such data can be used interchangeably where appropriate for the embodiments of this application described herein. Furthermore, the terms "comprising" and "having," and any variations thereof, are intended to cover non-exclusive inclusion; for example, a process, method, system, product, or apparatus that comprises a series of steps or units is not necessarily limited to those steps or units explicitly listed, but may include other steps or units not explicitly listed or inherent to such processes, methods, products, or apparatus.
[0025] Those skilled in the art will understand that, unless explicitly stated otherwise, the singular forms “a,” “an,” “the,” and “the” used herein may also include the plural forms. It should be further understood that the term “comprising” as used in the specification of this application means the presence of features, integers, steps, operations, elements, and / or components, but does not exclude the presence or addition of one or more other features, integers, steps, operations, elements, components, and / or groups thereof. It should be understood that when we say an element is “connected” or “coupled” to another element, it can be directly connected or coupled to the other element, or there may be intermediate elements. Furthermore, “connected” or “coupled” as used herein can include wireless connections or wireless coupling. The term “and / or” as used herein includes all or any units and all combinations of one or more associated listed items.
[0026] It will be understood by those skilled in the art that, unless otherwise defined, all terms used herein (including technical and scientific terms) have the same meaning as commonly understood by one of ordinary skill in the art to which this application pertains. It should also be understood that terms such as those defined in general dictionaries should be understood to have the same meaning as in the context of the prior art, and should not be interpreted in an idealized or overly formal sense unless specifically defined as herein.
[0027] It should be understood that the sequence number and size of each step in this embodiment do not imply the order of execution. The execution order of each process is determined by its function and internal logic, and should not constitute any limitation on the implementation process of this application embodiment.
[0028] It should be noted that, unless otherwise specified, the embodiments and features described in this application can be combined with each other. This application will now be described in detail with reference to the accompanying drawings and embodiments.
[0029] Please refer to Figures 1-7 This invention discloses an adaptive navigation method for a legged robot for three-dimensional spatial inspection of ships. The method includes the following steps: acquiring three-dimensional point cloud data of the ship's internal environment using a multi-line lidar and preprocessing it to obtain a homogenized global point cloud; segmenting the global point cloud into ground and obtaining a ground point cloud through plane fitting; extracting the facade structure point cloud from the ground point cloud using prior geometric constraints of staircase facades; performing load-bearing plane inversion and point cloud completion to generate an enhanced point cloud with confidence weights to characterize the load-bearing area suitable for stable footing and body passage of the legged robot; evaluating the terrain geometric features of the enhanced point cloud and constructing a drivability evaluation function based on the evaluation results, applicable to the constraints of body passage and stable footing of the legged robot, to extract a three-dimensional drivable region; and performing region growth clustering based on voxel multi-feature homogeneity constraints on the three-dimensional drivable region to construct multiple drivable areas. A traversable region cluster is identified, and this cluster serves as a high-level search space. A heuristic search is performed using path length, terrain safety penalty values to adapt to the legged robot's traversal stability, and obstacle distance as transition costs to generate an initial 3D global path. Based on existing prior mapping paths, the guidance weights of these paths are adaptively adjusted according to the scale metric parameters of each traversable region cluster. These adjusted guidance weights are then integrated with the transition costs to correct and optimize the initial 3D global path, resulting in an optimized global path. Based on the legged robot's current position, local trajectory planning is performed under kinematic constraints and dynamic window constraints, yielding multiple candidate trajectories. The fit between the candidate trajectories and the optimized global path, as well as their safety against real-time local obstacles, are comprehensively evaluated. The trajectory with the best evaluation result is then sent to the legged robot's underlying controller for execution.
[0030] Specifically, multi-line lidar is used to acquire real-time 3D point cloud data of the ship's internal environment. Temporal distortion correction is performed on the original point cloud to map all point clouds to the same time coordinate system, and statistical filtering is used to eliminate isolated noise points in the point cloud. .
[0031] In the formula: This indicates that after time distortion correction, the point... exist New three-dimensional coordinates in the time coordinate system Indicates from point to reference time pose transformation. This is the filtered point cloud set. The average distance of the neighborhood. These are statistical parameters.
[0032] In this invention, a downsampling method based on three-dimensional voxel grids is used to spatially homogenize the point cloud, resulting in a uniform global point cloud: .
[0033] In the formula: For the global point cloud, For the filtered point cloud set, since this invention involves cross-floor navigation via stairs, the voxel size should be selected to be less than half the width of the stair tread, and the voxel height should not be too large to prevent loss of the height change of the ground steps.
[0034] Ground point cloud is obtained by ground segmentation and plane fitting of global point cloud. Using the geometric prior constraints of stair facade, facade structure point cloud is extracted from ground point cloud. Enhanced point cloud with confidence weight is generated by bearing plane inversion and point cloud completion.
[0035] The enhanced point cloud is evaluated for terrain geometry features, and a drivability evaluation function suitable for the legged robot's body movement and stable foot placement is constructed to extract 3D drivable regions. Within these drivable regions, region growing and clustering are performed based on voxel multi-feature homogeneity constraints to construct multiple drivable region clusters. These clusters are used as a high-level search space, and a heuristic search is performed using path length, terrain safety penalty values adapted to the legged robot's drivability, and obstacle distance as transition costs to generate an initial 3D global path.
[0036] Based on the existing prior mapping path, the guiding weight of the prior mapping path is adaptively adjusted according to the scale metric parameter of each traversable region cluster. The adjusted guiding weight is then integrated with the transfer cost to correct and optimize the initial 3D global path, resulting in an optimized global path.
[0037] Based on the robot's current position, local trajectory planning is performed under the kinematic constraints and dynamic window constraints of the legged robot, generating multiple candidate trajectories. The fit between the candidate trajectories and the optimized global path, as well as their safety with real-time local obstacles, are comprehensively evaluated. The optimal trajectory is then selected and sent to the legged robot's underlying controller for execution.
[0038] This application introduces prior geometric constraints on staircase facades to invert and complete missing stair treads in the point cloud and assigns credibility weights. This effectively solves the problem of broken and misjudged traversable areas caused by incomplete 3D mapping in complex scenarios such as narrow staircases, improving the completeness and planarability of traversable domain representation. Furthermore, by clustering traversable spaces into multi-scale region clusters and adaptively adjusting the guidance weights of prior mapping paths according to the scale of the region clusters, the legged robot can closely follow the verified safe trajectory in narrow areas such as staircases and corridors, while maintaining the freedom of path optimization in open areas. This ensures both global path safety and path optimization. Finally, by combining dynamic window local planning and trajectory comprehensive evaluation, the application achieves the unification of global path tracking and local dynamic obstacle avoidance, significantly reducing the collision risk of legged robots in cross-floor inspection operations on ships and improving the stability, continuity, and robustness of navigation.
[0039] In some embodiments, the step of performing ground segmentation and planar fitting on the global point cloud to obtain the ground point cloud specifically includes: exist Based on this, in order to accurately select the point cloud set of the staircase facade, the depth sequence of each line bundle is first obtained. That is, the first The first in the wire harness Calculate the height difference and pitch angle change at each point: .
[0040] Then, by utilizing the difference that ground areas exhibit gentle changes in height and small changes in pitch angle with distance, resulting in a relatively continuous and smooth overall pattern, while non-ground areas (steps, walls, handrails, etc.) show more drastic changes in height and abrupt changes in local normals, points that might be on the ground are selected from the current point cloud. Among these, points satisfying the following conditions are considered: If the point is located on the ground, it can be considered to be distributed on the ground and added to the ground candidate point cloud set. .
[0041] Then, in the initial screening of ground point clouds To avoid misidentifying low-lying obstacles as ground, this invention employs a ground plane fitting method based on the Random Sample Consensus Algorithm (RANSAC) to fit planar models to candidate points, resulting in a more reliable set of ground point clouds. .
[0042] In some embodiments, the step of extracting the facade structure point cloud from the ground point cloud using the prior geometric constraints of the staircase facade specifically includes: calculating the local normal vector of each point in the ground point cloud; filtering points whose local normal vectors have an angle less than a preset threshold with the vertical direction to form a candidate point set for the facade structure; fitting the candidate point set for the facade structure using a random sampling consensus algorithm to obtain a set of vertical planes; and filtering the set of vertical planes for planes with consistent normals, arranged in an orderly manner according to the direction of staircase travel and spatially adjacent to each other to obtain a set of staircase facades as the facade structure point cloud.
[0043] Specifically, since the staircase facade sequence is observable, the load-bearing plane is derived through facade geometry inversion, and a point cloud set of the load-bearing plane is generated. ,right various points in the middle Calculate local normal By utilizing the geometric property that the facade is a vertical structure, we select those that meet the requirements. The points constitute the candidate point set for the facade structure. .in , The threshold for judging the stepped facade structure is used to limit the angle between the normal and the gravity direction to be close to 90°, that is, the normal is approximately horizontal.
[0044] Randomly adopt the consensus algorithm RANSAC to pair By fitting the data, we obtain the set of vertical planes in the entire map: In the formula: Represents a planar set. Indicates the first A planar model, This indicates the number of planes obtained from the fitting. Representing a plane The normal vector, Represents an arbitrary three-dimensional position vector; This indicates a plane offset. Therefore, the constraint... This indicates that only a vertical plane with an approximately horizontal normal is retained. Then, for... The set of staircase facades was selected based on characteristics such as normal consistency, facade sequence, and spatial locality. .
[0045] Combination of staircase facades The normal set of principal directions estimates the unit vector of the staircase travel direction. For each Define its along Ordered position scalars: .
[0046] Define the height of the top edge of a single staircase: .
[0047] right Sort to get Construct adjacent differences: .
[0048] Using robust statistics to obtain step depth With BBK : .
[0049] And apply consistency constraints: .
[0050] Therefore, by filtering out abnormal staircase facades, a valid set of step indices is obtained. .
[0051] For each Define the load-bearing horizontal plane: .
[0052] Define the load-bearing support plane domain: .
[0053] in The lateral width constraint domain is determined by the lateral envelope quantiles of the staircase facade point cloud. Inner grid step size Sample to generate a complete set of bearing surface points: .
[0054] This application filters candidate points of the facade by calculating local normal vectors, fits the set of vertical planes using a random sampling consensus algorithm, and further filters the set of stair facades based on normal consistency, sequence, and spatial locality. It can accurately identify the stair facade structure from the ground point cloud, providing a reliable geometric prior for subsequent load-bearing plane inversion and point cloud completion. It avoids misjudging non-stair facades such as walls and handrails as stair structures, and improves the accuracy of stair area identification.
[0055] In some embodiments, the step of performing load-bearing plane inversion and point cloud completion to generate an enhanced point cloud with confidence weights specifically includes: estimating the stair travel direction based on the normal principal direction of the stair facade set; robustly estimating the step depth and step height of the stair based on the stair facade set, and filtering out abnormal facades to obtain effective steps; inverting and generating a horizontal load-bearing support plane domain based on the top height, step depth, and lateral width constraints of the effective steps; sampling and generating a load-bearing surface completion point set within the load-bearing support plane domain, and assigning confidence weights to the completion point set of each step based on the plane fitting residuals of the corresponding stair facade and the consistency variance of the step height and step depth parameters; merging the load-bearing surface completion point set with the confidence weights with the global point cloud to obtain the enhanced point cloud.
[0056] Specifically, to explicitly incorporate the uncertainty of tread completion into subsequent risk assessment, a completion credibility level is defined for each step: In the formula: For the planar fitting residuals of the corresponding staircase facade, They are respectively Consistency variance. Incorporating the completed points into the base point cloud yields the enhanced point cloud: .
[0057] This application robustly estimates the step depth and step height of stairs and filters out abnormal facades. Based on the effective step inversion, it generates a horizontal load-bearing support plane domain. While sampling and generating a complete point set, it assigns a confidence weight, which can restore the original stair point cloud containing only the facade to a continuous load-bearing plane sequence. This solves the problem of incomplete representation of traversable areas caused by missing stair tread point clouds. At the same time, the confidence weight quantifies the uncertainty of the completion, providing risk perception capability for subsequent traversability assessment.
[0058] In some embodiments, the step of evaluating the terrain geometry features of the enhanced point cloud and constructing a drivability evaluation function applicable to the constraints of legged robot body passage and stable foot landing based on the evaluation results to extract a three-dimensional drivable region specifically includes: constructing a neighborhood for each point in the enhanced point cloud, calculating the slope, roughness, and distance to the nearest obstacle; constructing a completion uncertainty modulation term according to the confidence weight; performing a weighted summation of the slope, roughness, the reciprocal of the distance to the nearest obstacle, and the completion uncertainty modulation term to construct a drivability evaluation function applicable to the legged robot body passage and stable foot landing; determining points with evaluation function values less than a preset threshold as drivable points to form a drivable point set; and systematizing the drivable point set to obtain a three-dimensional drivable voxel set.
[0059] Specifically, for To conduct terrain structure analysis, firstly, for the preprocessed point cloud... Each point in Constructing a neighborhood Based on this, three basic characteristics are calculated: Firstly, the normal vector and slope are determined by eigenvalue decomposition using the neighborhood covariance matrix, with the eigenvector corresponding to the smallest eigenvalue serving as the local plane normal vector. And the slope is defined as the angle between the normal vector and the vertical direction. The included angle: The larger the angle, that is, the steeper the slope, the less conducive it is to the walking of legged robots.
[0060] Secondly, roughness, using the smallest eigenvalue of the covariance matrix. As an indicator of roughness, the greater the roughness, the more uneven the ground is.
[0061] Third, obstacle distance, based on non-ground points. For obstacles, calculate the distance from the point to the nearest obstacle: The smaller the distance, the higher the risk of collision. This is used for subsequent global path search.
[0062] Based on the above analysis of terrain geometry features, this invention defines a comprehensive evaluation function for traversable areas: .
[0063] In the formula: For the weighting parameters, in addition, there is a supplementary uncertainty modulation term. for .
[0064] The settings can be configured based on the legged robot's sensitivity to slope, flatness, and safety distance. Specifically, this is achieved by setting a threshold for the passable area. ,if This means that the point is considered passable, and the final set of passable points is obtained: .
[0065] For the set of passable points Voxelization, constructing a resolution of Three-dimensional accessible voxel set The remaining voxels and the unknown space are denoted as obstacle voxels. And construct a Euclidean signed distance field on a voxel grid. This serves as a unified input for subsequent clustering features and planning costs.
[0066] This application constructs a drivability evaluation function suitable for legged robots to traverse the terrain and stabilize their feet by combining enhanced point cloud computing slope, roughness, and obstacle distance with completion uncertainty modulation terms. This function can comprehensively assess the drivability of the terrain itself and the confidence level of the completed area, allowing areas with high completion uncertainty to be conservatively handled in the planning process. This effectively reduces the collision risk caused by false drivable surfaces and improves the reliability and safety of drivable area extraction.
[0067] In some embodiments, the step of constructing multiple walkable region clusters by performing region growing clustering based on voxel multi-feature homogeneity constraints specifically includes: constructing a voxel adjacency graph on the three-dimensional walkable voxel set; constructing a feature vector for each walkable voxel, the feature vector including voxel center height, voxel slope, voxel roughness, and obstacle distance features based on Euclidean signed distance field; defining a weighted feature distance between adjacent voxels, and when the weighted feature distance is less than the clustering threshold, classifying adjacent voxels into the same connected region, obtaining multiple walkable region clusters through region growing, and recording the number of voxels in each region cluster as a scale metric parameter.
[0068] Specifically, in a three-dimensional accessible voxel set Construct an adjacency graph ,in For a set of nodes, This is a set of edges constructed based on adjacency relationships, so that subsequent searches can explicitly utilize the discrete connectivity of the walkable space.
[0069] For each passable voxel By using a region growing algorithm to extract traversable region clusters under the common constraints of spatial connectivity and feature similarity, feature vectors are constructed based on the local geometry and security attributes of the point cloud contained therein. In the formula: The height of the voxel center. The slope of the voxel is determined based on the angle between the normal vector and the vertical direction. The local roughness is evaluated based on the minimum eigenvalue of the covariance matrix. The obstacle distance feature is obtained by ESDF distance normalization to reflect the degree of distance from the obstacle.
[0070] And define the weighted feature distance between adjacent voxels as: In the formula: This is the feature weighting matrix, typically a diagonal matrix. When the weighted feature distance is less than the clustering threshold, i.e. Adjacent voxels Regions are grouped into the same connected region and then grown and merged to obtain a set of region clusters: .
[0071] This application constructs a feature vector containing height, slope, roughness, and obstacle distance for each traversable voxel. Based on weighted feature distance and spatial connectivity, it performs region growing clustering, automatically aggregating fine-grained traversable voxels into several high-level terrain units that are relatively homogeneous in local geometry and safety attributes. This creates a structured high-level search space for global path planning, significantly reducing search complexity and improving planning efficiency.
[0072] In some embodiments, the step of using the traversable region cluster as a high-level search space and performing a heuristic search with path length, terrain safety penalty value adapted to the traversal stability of the legged robot, and obstacle distance as transfer costs to generate an initial three-dimensional global path specifically includes: mapping the start point and end point to corresponding voxels in the voxel adjacency graph; for each edge between voxels, calculating the path length cost, terrain safety penalty value, and obstacle distance cost, and weighting the sum of the three as the transfer cost; and using a heuristic search algorithm to search in the voxel adjacency graph for the path with the minimum transfer cost from the start voxel to the end voxel, which is then used as the initial three-dimensional global path.
[0073] Specifically, let the positions of the starting point and the ending point in space be respectively... and The corresponding voxel indices mapped to the voxel map are respectively and For each voxel edge Define the basic transfer cost: In the formula: The three-dimensional coordinates of the voxel center. Let be the midpoint of the edge connecting the two voxels. In order to be in Calculate the normalized terrain safety penalty value. This is a distance cost function based on ESDF, used to penalize paths that are closer to obstacles. These are the weight parameters for the corresponding cost terms.
[0074] Therefore, a heuristic search algorithm, such as the A* algorithm, can be used to search for a path from the 3D accessible voxel map. arrive The least costly voxel sequence path: In the formula: By backtracking the parent node to obtain the path and further extracting the corresponding voxel center coordinates, a globally discrete path point sequence that is geometrically continuous in three dimensions and satisfies the traversable region constraint can be obtained. The initial three-dimensional global path is obtained by backtracking the parent node to obtain the voxel sequence path and then extracting the corresponding voxel center coordinates.
[0075] This application uses a weighted sum of path length, terrain safety penalty value, and obstacle distance as the transfer cost to perform a heuristic search in a voxel map. This approach can simultaneously consider the geometric length of the path, terrain suitability, and obstacle safety distance. The generated 3D global path satisfies the traversable domain constraint and has good safety and smoothness, providing a high-quality reference trajectory for subsequent local tracking.
[0076] In some embodiments, the step of adaptively adjusting the guidance weight of the prior mapping path based on the scale metric parameter of each traversable region cluster, based on the existing prior mapping path, specifically includes: obtaining a pre-stored prior mapping path, the prior mapping path containing multiple path points; defining a minimum distance function from any point in space to the prior mapping path; for each traversable region cluster, comparing its scale metric parameter with a preset scale threshold, and assigning a larger guidance weight if the scale metric parameter is less than the threshold, otherwise assigning a smaller guidance weight.
[0077] Specifically, during map loading in the planning phase, the saved mapping path file is read and stored. The sequence of path points is as follows: .
[0078] For any point in space The minimum distance from it to the mapping path is defined as: In the formula: Point to line segment The Euclidean distance is used to characterize The degree of deviation from the prior path is defined, and it is specified that if the mapping path is empty, then... This is equivalent to not enabling the prior path.
[0079] For each region cluster Define the prior weights for the graph construction path: ;in < ,like If the value is less than the threshold, a larger guiding weight is assigned. Otherwise, assign a smaller guiding weight. This reflects a stronger preference for mapping path alignment within small-scale regional clusters.
[0080] This application obtains prior mapping paths and defines a minimum distance function from a point to the path. It adaptively adjusts the guidance weights based on the scale metric parameters of the traversable area clusters, assigning higher prior guidance weights to narrow areas such as stairs and corridors, and lower guidance weights to open areas such as cabins. This achieves selective fitting of verified safe trajectories and effectively reduces the risk of global path deviation in distorted areas.
[0081] In some embodiments, the step of integrating the adjusted guiding weights with the transition costs to correct and optimize the initial 3D global path specifically includes: calculating the minimum distance from the midpoint of each edge in the voxel adjacency graph to the prior graphing path; constructing a cost term deviating from the graphing path based on the minimum distance; multiplying the cost term deviating from the graphing path by the guiding weight of the corresponding edge's region cluster to obtain the prior cost term; adding the prior cost term to the transition costs to update the edge's comprehensive cost; and performing a heuristic search again based on the updated comprehensive cost to obtain the optimized global path.
[0082] Specifically, for edges in the A* algorithm Design the cost term for deviation from the graph construction path. The edge midpoint is... Based on this minimum distance, construct the cost term for deviation from the mapping path: ;in, Select about An increasing function of the square, i.e., if the midpoint of the side... The voxel belongs to the region cluster. The final prior cost of the graph construction path is then updated as follows: ; therefore, When the size is small, the cost of deviating from the mapping path is relatively low, allowing for the free search for shorter, smoother paths while ensuring safety. However, when the size is small... When the value is large, the cost of any candidate edge that is far from the graph construction path will increase significantly, and the global path will traverse narrow regions along the proven safe graph construction path.
[0083] To accommodate the varying degrees of reliance on prior knowledge of mapping paths across different application scenarios, this application introduces adaptation setting parameters. This is used to control the scope of the graph-building paths during pathfinding. When, it indicates that the scene is relatively wide and there is no need to use a priori mapping path; when When there are both wide and narrow spaces in the current scene, the prior mapping path helps to better guide the generation of the global path. In this case, the weight of the prior mapping path is... The scope of application is: In the formula: The preset association radius is used to represent the area within a certain spatial range of the same mapping path. When When this occurs, it indicates that the current scenario is very complex, and prior graph construction paths can significantly improve the quality of global paths. In this case, the weights of the prior graph construction paths... Its scope covers the entire currently accessible cluster of regions. .
[0084] Thus, based on the adjustment of global path prior constraints, combined with the heuristic search algorithm, the generated... Then the global path was obtained. By tracing back to the parent node to obtain the path, a globally discrete sequence of path points that is geometrically continuous in three dimensions and satisfies the traversable region constraint can be obtained. .
[0085] This application constructs a deviation cost term by calculating the minimum distance from the midpoint of a voxel edge to the prior mapping path, multiplies it by the guiding weight of the region cluster, and adds it to the original transfer cost. Based on the updated comprehensive cost, a heuristic search is performed again. This allows for flexible adjustment of the strength of prior constraints according to the region scale while ensuring path safety. This enables the global path to closely follow the safe trajectory in narrow regions and maintain optimization freedom in open regions, thus achieving an organic integration of prior knowledge and real-time planning.
[0086] In some embodiments, the step of performing local trajectory planning under the kinematic constraints and dynamic window constraints of the legged robot specifically includes: extracting a local path of a preset length from the optimized global path based on the current position of the legged robot; extracting a real-time local point cloud of a preset range around the legged robot; constructing a velocity dynamic window under the kinematic constraints of the legged robot body, sampling the linear velocity and angular velocity, and generating a candidate trajectory cluster.
[0087] Specifically, based on the current position of the legged robot Find the global path point closest to the robot and extract a segment of length [length missing]. local path Using the point cloud acquired by the legged robot's current LiDAR, real-time local point cloud of a certain range around the robot is extracted. .
[0088] In the body coordinate system, let the planning compensation be... Therefore, the dynamic window constructed at the current speed is The system performs uniform sampling within the window at a preset resolution, and sequentially calculates all candidate trajectories for the current window based on a set step size, thereby generating a candidate trajectory cluster.
[0089] This application extracts a local path from the optimized global path, extracts a real-time local point cloud, and constructs a dynamic velocity window under the kinematic constraints of the legged robot to generate a candidate trajectory cluster. This approach combines the global planning results with the robot's real-time perception, ensuring the accuracy of global path tracking while fully considering the kinematic limitations of the legged robot itself, thus providing a diverse set of candidate trajectories for subsequent trajectory evaluation.
[0090] In some embodiments, the step of comprehensively evaluating the fit between the candidate trajectory and the optimized global path, as well as the safety with real-time local obstacles, specifically includes: for each candidate trajectory, calculating the sum of the shortest distances from each trajectory point to the local path as a fit cost; calculating the sum of the distances from the center of the legged robot's body envelope corresponding to each trajectory point to each point in the real-time local point cloud as a safety cost; weighted summing the fit cost and the safety cost to obtain a trajectory score; selecting the candidate trajectory with the lowest score and sending its corresponding speed command to the legged robot's underlying controller for execution.
[0091] Specifically, when selecting the optimal trajectory, an evaluation function was constructed that considers both trajectory fit and distance to local obstacle point clouds in the world coordinate system: .
[0092] in, These are the weighting factors for each item. For the current trajectory point to distance, For a local point cloud within a certain range of the current legged robot The sum of distances to the center of the envelope of the legged robot body corresponding to the trajectory point; therefore, in the world coordinate system, for a trajectory... trajectory points The local trajectory formed: .
[0093] In the formula: For each trajectory point, the center of the legged robot is... The conversion relationship between them is determined by the size of the legged robot's structure itself. Therefore, the final result for each trajectory is calculated sequentially. Select The lowest trajectory is the optimal trajectory within this window. The corresponding linear velocity and angular velocity are used as motion control commands within the current control cycle and sent to the lower-level controller of the legged robot for execution, thereby achieving stable cross-layer navigation of the legged robot.
[0094] This application calculates the fitting cost between the candidate trajectory and the local path, as well as the safety cost with the real-time local point cloud. After weighted summation of the two, the trajectory with the lowest score is selected for execution. This achieves a dynamic balance between global path tracking and local dynamic obstacle avoidance, ensuring the overall directionality of the legged robot along the planned path while responding to sudden obstacles in the environment in real time, effectively improving the smoothness and safety of local motion.
[0095] If the integrated unit is implemented as a software functional unit and sold or used as an independent product, it can be stored in a computer-readable storage medium. Based on this understanding, the technical solution of this invention, in essence, or the part that contributes to the prior art, or all or part of the technical solution, can be embodied in the form of a software product. This computer software product is stored in a storage medium and includes several instructions to cause an electronic device (which may be a personal computer, server, or network device, etc.) to execute all or part of the steps of the methods of the various embodiments of this invention. The aforementioned storage medium includes various media capable of storing program code, such as USB flash drives, portable hard drives, read-only memory (ROM), random access memory (RAM), magnetic disks, or optical disks.
[0096] Those skilled in the art will understand that all or part of the processes in the methods of the above embodiments can be implemented by a computer program instructing related hardware. The computer program can be stored in a non-volatile computer-readable storage medium. When the computer program is executed, it can include the processes of the embodiments of the above methods. Any references to memory, storage, database, or other media used in the embodiments provided by this invention can include non-volatile and / or volatile memory. Non-volatile memory can include read-only memory (ROM), programmable ROM (PROM), electrically programmable ROM (EPROM), electrically erasable programmable ROM (EEPROM), or flash memory. Volatile memory can include random access memory (RAM) or external cache memory. By way of illustration and not limitation, RAM is available in various forms, such as static RAM (SRAM), dynamic RAM (DRAM), synchronous DRAM (SDRAM), dual data rate SDRAM (DDRSDRAM), enhanced SDRAM (ESDRAM), synchronous link DRAM (SLDRAM), RAMbus direct RAM (RDRAM), direct memory bus dynamic RAM (DRDRAM), and RAMbus dynamic RAM (RDRAM), etc.
[0097] The above are merely embodiments of the present invention and do not limit the patent scope of the present invention. Any equivalent modifications made based on the content of the present invention's specification and drawings, or direct or indirect applications in related technical fields, are similarly included within the patent protection scope of the present invention.
Claims
1. An adaptive navigation method for a legged robot for three-dimensional spatial inspection of ships, characterized by the following steps: include: Three-dimensional point cloud data of the ship's internal environment is acquired by multi-line lidar and preprocessed to obtain a uniform global point cloud. The global point cloud is segmented into ground, and ground point cloud is obtained through plane fitting. Using the prior geometric constraints of the staircase facade, the facade structure point cloud is extracted from the ground point cloud. The bearing plane is inverted and the point cloud is completed to generate an enhanced point cloud with confidence weights to characterize the bearing area that can be stably placed and passed by the legged robot. The terrain geometric features of the enhanced point cloud are evaluated, and based on the evaluation results, a drivability evaluation function suitable for the legged robot's body passage and foot stability constraints is constructed to extract the three-dimensional drivable area. On the three-dimensional traversable region, region growing clustering is performed based on the multi-feature homogeneity constraint of voxels to construct multiple traversable region clusters. The traversable region clusters are used as a high-level search space. Heuristic search is performed with path length, terrain safety penalty value adapted to the traversal stability of the legged robot, and obstacle distance as transfer costs to generate an initial three-dimensional global path. Based on the existing prior mapping path, the guiding weight of the prior mapping path is adaptively adjusted according to the scale metric parameter of each traversable region cluster. The adjusted guiding weight is then integrated with the transfer cost to correct and optimize the initial 3D global path, resulting in an optimized global path. Based on the robot's current position, local trajectory planning is performed under the kinematic constraints and dynamic window constraints of the legged robot to obtain multiple candidate trajectories. The fit between the candidate trajectories and the optimized global path, as well as their safety with real-time local obstacles, are comprehensively evaluated. The trajectory with the best evaluation result is then sent to the legged robot's underlying controller for execution.
2. The adaptive navigation method for legged robots for three-dimensional space inspection of ships according to claim 1, characterized in that, The step of extracting the facade structure point cloud from the ground point cloud using prior geometric constraints of the staircase facade specifically includes: Calculate the local normal vector of each point in the ground point cloud; Points whose local normal vectors make an angle with the vertical direction less than a preset threshold are selected to form a candidate point set for the facade structure; The set of candidate points for the facade structure is fitted using a random sampling consensus algorithm to obtain a set of vertical planes; For the set of vertical planes, planes with consistent normals, arranged in an orderly manner according to the direction of stair travel and spatially adjacent are selected to obtain the set of stair facades as the point cloud of the facade structure.
3. The adaptive navigation method for legged robots for three-dimensional space inspection of ships according to claim 2, characterized in that, The steps of performing bearing plane inversion and point cloud completion to generate an enhanced point cloud with confidence weights specifically include: Estimate the direction of travel of the stairs based on the normal principal direction of the set of stair facades; Based on the set of staircase facades, the step depth and step height of the staircase are robustly estimated, and abnormal facades are filtered out to obtain effective steps; Based on the constraints of the top height, step depth, and lateral width of the effective steps, a horizontal load-bearing support plane domain is generated by inversion. Within the bearing support plane domain, a set of bearing surface completion points is generated by sampling. Based on the plane fitting residuals of the corresponding staircase facade and the consistency variance of the step height and step depth parameters, a confidence weight is assigned to the completion point set of each step. The augmented point cloud is obtained by merging the complete point set of the bearing surface with the aforementioned credibility weight with the global point cloud.
4. The adaptive navigation method for legged robots for three-dimensional space inspection of ships according to claim 3, characterized in that, The steps of evaluating the terrain geometry features of the enhanced point cloud and constructing a drivability evaluation function suitable for the legged robot's body passage and foot stability landing constraints based on the evaluation results to extract the three-dimensional drivable region specifically include: For each point in the enhanced point cloud, a neighborhood is constructed, and the slope, roughness, and distance to the nearest obstacle are calculated. Construct a completion uncertainty modulation term based on the aforementioned confidence weight; The slope, roughness, the reciprocal of the distance to the nearest obstacle, and the completion uncertainty modulation term are weighted and summed to construct a drivability evaluation function suitable for the passage of the legged robot body and the stable landing of the foot. Points whose evaluation function values are less than a preset threshold are identified as passable points, forming a set of passable points; The systematization of the passable point set yields a three-dimensional passable voxel set.
5. The adaptive navigation method for legged robots for three-dimensional space inspection of ships according to claim 1, characterized in that, The steps for constructing multiple walkable region clusters through region growing clustering based on voxel-based multi-feature homogeneity constraints specifically include: Construct a voxel adjacency graph on the three-dimensional accessible voxel set; A feature vector is constructed for each passable voxel, the feature vector including voxel center height, voxel slope, voxel roughness, obstacle distance features, and load-bearing stability features; Define a weighted feature distance between adjacent voxels. When the weighted feature distance is less than the clustering threshold, adjacent voxels are grouped into the same connected region. Multiple passable region clusters are obtained through region growth, and the number of voxels in each region cluster is recorded as a scale metric parameter.
6. The adaptive navigation method for a legged robot for three-dimensional space inspection of ships according to claim 5, characterized in that, The step of using the cluster of passable regions as a high-level search space and performing a heuristic search with path length, terrain safety penalty value adapted to the stability of legged robot passage, and obstacle distance as transfer costs to generate an initial three-dimensional global path specifically includes: Map the start and end points to the corresponding voxels in the voxel adjacency graph; For each edge between voxels, calculate the path length cost, terrain safety penalty, and obstacle distance cost based on the Euclidean signed distance field, and then sum the three values as the transition cost. A heuristic search algorithm is used to search for the path with the minimum transfer cost from the starting voxel to the ending voxel in the voxel adjacency graph, which is then used as the initial 3D global path.
7. The adaptive navigation method for legged robots for three-dimensional space inspection of ships according to claim 6, characterized in that, The step of adaptively adjusting the guiding weight of the prior mapping path based on the scale metric parameter of each traversable region cluster, according to the existing prior mapping path, specifically includes: Obtain a pre-stored prior mapping path, wherein the prior mapping path contains multiple path points; Define the minimum distance function from any point in space to the prior mapping path; For each passable region cluster, its scale metric parameter is compared with a preset scale threshold. If the scale metric parameter is less than the threshold, a larger guidance weight is assigned; otherwise, a smaller guidance weight is assigned.
8. The adaptive navigation method for a legged robot for three-dimensional space inspection of ships according to claim 7, characterized in that, The step of integrating the adjusted guidance weights with the transfer cost to correct and optimize the initial 3D global path specifically includes: Calculate the minimum distance from the midpoint of each edge in the voxel adjacency graph to the prior graph path; Construct a cost term for deviation from the mapping path based on the minimum distance; Multiply the cost term for deviation from the graph construction path by the guiding weight of the corresponding edge's region cluster to obtain the prior cost term; Add the prior cost to the transition cost to update the comprehensive cost of the edge; Based on the updated comprehensive cost, a new heuristic search is performed to obtain the optimized global path.
9. The adaptive navigation method for a legged robot for three-dimensional space inspection of ships according to claim 1, characterized in that, The steps for local trajectory planning under kinematic constraints and dynamic window constraints of a legged robot specifically include: Based on the current position of the legged robot, a local path of a preset length is extracted from the optimized global path; Extract real-time local point cloud data within a preset range around the legged robot; A dynamic velocity window is constructed under the kinematic constraints of the legged robot body, and linear velocity and angular velocity are sampled to generate candidate trajectory clusters.
10. The adaptive navigation method for a legged robot for three-dimensional spatial inspection of ships according to claim 9, characterized in that, The steps for comprehensively evaluating the fit between the candidate trajectory and the optimized global path, as well as the safety against real-time local obstacles, specifically include: For each candidate trajectory, the sum of the nearest distances from each trajectory point to the local path is calculated as the fitting cost; The sum of the distances from the center of the legged robot's body envelope corresponding to each trajectory point to each point in the real-time local point cloud is calculated as a safety cost. The trajectory score is obtained by weighted summation of the fit cost and the security cost. The candidate trajectory with the lowest score is selected, and its corresponding speed command is sent to the legged robot's underlying controller for execution.