A laser radar algorithm based on anti-degradation scene of a robot dog

Abstract

The application discloses a laser radar algorithm based on anti-degradation scene of a robot dog and belongs to the technical field of robot environment perception and laser radar data processing. Point cloud data of each scanning circle layer of a laser radar is acquired, and a high-confidence area and a low-confidence area are divided according to three-dimensional space distribution density and echo intensity distribution; a nearest neighbor point set in a point cloud space neighborhood in the high-confidence area is extracted, and local plane fitting parameters are generated according to a point cloud height variation quantity and an echo intensity difference quantity; the point cloud of the low-confidence area is projected along a laser radar radial direction to a space plane where the high-confidence area is located, a weighted sum of a space distance between the projected point cloud and fitting points and an echo intensity difference value is calculated as a degradation migration quantity; global pose initial estimation is carried out according to the degradation migration quantity of the point cloud of the low-confidence area of each circle layer at a current motion moment, and laser radar anti-degradation motion estimation quantity is acquired based on a space continuity constraint between adjacent scanning circle layers.

Description

Technical Field

[0001] This invention relates to the field of robot environmental perception and lidar data processing technology, specifically a lidar algorithm based on robot dogs in anti-degradation scenarios. Background Technology

[0002] During autonomous movement, the robot dog uses LiDAR to acquire point clouds of the surrounding environment and estimate its own pose. Existing LiDAR algorithms typically register consecutive point cloud frames or extract geometric features such as curvature, normals, planes, and edges from the point cloud, and combine these with point cloud echo intensity to construct constraint relationships to solve for the robot dog's motion changes. In environments with rich structural features, these algorithms can obtain relatively stable motion estimation results by utilizing multi-directional geometric constraints. In degraded scenarios such as corridors, open platforms, and single facades, the point clouds obtained by LiDAR scanning are sparsely distributed in some directions, with high geometric feature repetition. There are significant differences in the number of point clouds and the distribution of echo intensity in different concentric layers or different orientations. Conventional algorithms use uniform weights for each region within the scanning concentric layer or only filter based on the number of point clouds, without simultaneously considering the three-dimensional spatial distribution density and the distribution of point cloud echo intensity. This results in the constraint contributions between high-confidence and low-confidence points not being distinguished, and weakly constrained point clouds in some degraded directions being assigned excessively high weights. Some methods directly discard low-confidence point clouds or use only spatial distance for degradation judgment, losing effective information in echo intensity differences and failing to reflect the degree of degradation migration of low-confidence point clouds relative to high-confidence regions. Global pose initial estimation often relies on features within a single frame point cloud or the nearest point distance between frames, lacking spatial continuity constraints between adjacent scan layers, making the robot prone to pose drift in the degradation direction.

[0003] In the degradation scenario of the robot dog, the confidence division of the point cloud region does not simultaneously consider the three-dimensional spatial distribution density and echo intensity distribution. The degradation migration of the point cloud in the low confidence region is insufficiently represented. The initial global pose estimation lacks the continuity constraint of adjacent scanning layers, and the motion estimate drifts along the direction of weak constraint. Summary of the Invention

[0004] To address the problem of interference pose estimation in weakly constrained regions of LiDAR point clouds for robot dogs in degraded scenarios, a LiDAR algorithm based on robot dogs for degraded scenarios is proposed. This method utilizes the 3D spatial distribution density and echo intensity distribution of point clouds in each scanning layer of the LiDAR to divide the region into high-confidence and low-confidence regions. Local plane fitting parameters of the high-confidence region are extracted. The point cloud of the low-confidence region is projected radially along the LiDAR to the spatial plane of the high-confidence region, and the degradation migration is calculated. An initial global pose estimate is performed based on the degradation migration, and the LiDAR's anti-degradation motion estimate is obtained through iterative adjustment using spatial continuity constraints between adjacent scanning layers.

[0005] To achieve the above objectives, the present invention provides the following technical solution: The present invention provides a LiDAR algorithm based on a robot dog's anti-degradation scenario, comprising: acquiring point cloud data of each scanning layer of the LiDAR within a preset time period before the current movement moment of the robot dog, and dividing each layer into high-confidence regions and low-confidence regions according to the three-dimensional spatial distribution density and point cloud echo intensity distribution of the point cloud data of each layer; extracting the nearest neighbor set of points in the spatial neighborhood of each point cloud in the high-confidence region, and generating a set of local planes for each high-confidence region according to the point cloud height change and echo intensity difference of the nearest neighbor set. Fitting parameters: Project each point cloud in the low-confidence region radially to the spatial plane of the adjacent high-confidence region, and calculate the weighted sum of the spatial distance and echo intensity difference between the point cloud and each fitted point on the spatial plane after projection, as the degradation migration amount of the point cloud; Based on the degradation migration amount of all low-confidence region point clouds in each layer at the current motion moment, perform global pose initial estimation on the point cloud frame at the current motion moment, and based on the spatial continuity constraint of each point cloud between adjacent scanning layers in the global pose initial estimation, obtain the LiDAR anti-degradation motion estimate at the current motion moment.

[0006] The above method can effectively distinguish between high-confidence regions with stable geometric structures and echo characteristics and low-confidence regions susceptible to degradation in degraded scenarios where the robot dog is located. The degradation migration amount of the point cloud in the low-confidence region is characterized by the local plane fitting parameters of the high-confidence region, and this is used to constrain the initial global pose estimation and the spatial continuity between adjacent scanning layers, thereby improving the motion estimation stability and anti-degradation ability of the lidar in degraded scenarios.

[0007] Preferably, when dividing each concentric circle into high-confidence and low-confidence regions based on the 3D spatial distribution density and echo intensity distribution of the point cloud data, the point cloud within each concentric circle is divided into equally spaced partitions according to the horizontal azimuth angle. The number of point clouds and the mean echo intensity in each partition are statistically analyzed, and the ratio of the number of point clouds to the mean echo intensity in each partition is calculated. The global standard deviation of this ratio for all partitions within the concentric circle is calculated. All point clouds in the corresponding partitions with a ratio greater than the global standard deviation are marked as high-confidence regions, and all point clouds in the corresponding partitions with a ratio less than or equal to the global standard deviation are marked as low-confidence regions. This partitioning method can adaptively identify stable and degenerate regions based on the density of point clouds and the consistency of echo intensity, reducing interference from degenerate regions in subsequent pose estimation.

[0008] Preferably, when extracting the nearest neighbor set within the spatial neighborhood of each point cloud in the high-confidence region, the target point cloud in the high-confidence region is taken as the center. A predetermined number of neighboring point clouds with the smallest spatial Euclidean distance are selected from both the current concentric layer and the adjacent concentric layers above and below the target point cloud. All selected neighboring point clouds are then combined to form the nearest neighbor set of the target point cloud. This nearest neighbor set reflects the geometric continuity and cross-concentric structural characteristics of the target point cloud in local space, which helps improve the accuracy of local plane fitting.

[0009] Preferably, when generating a set of local plane fitting parameters for each high-confidence region based on the point cloud height change and echo intensity difference of the nearest neighbor point set, the difference between the maximum and minimum height values ​​of all point clouds in the nearest neighbor point set is calculated as the point cloud height change of the nearest neighbor point set, and the difference between the maximum and minimum echo intensity values ​​of all point clouds in the nearest neighbor point set is calculated as the echo intensity difference. The product of the point cloud height change and the echo intensity difference is used as the weight coefficient for the local plane fitting of the high-confidence region. Weighted plane fitting is then performed on the nearest neighbor point set based on this weight coefficient to obtain the local plane fitting parameters. By introducing a joint weight of height change and echo intensity difference, the local plane fitting becomes more sensitive to geometric undulations and echo feature changes in degraded scenes, enhancing the robustness of the spatial plane representation of the high-confidence region.

[0010] Preferably, when projecting each point cloud in a low-confidence region radially onto the spatial plane of an adjacent high-confidence region from the lidar, within the same scanning circle of each point cloud in the low-confidence region, the two high-confidence regions with the smallest difference in horizontal azimuth angle from that point cloud are identified, and the local plane fitting parameters corresponding to these two high-confidence regions are obtained respectively. The point cloud is then projected radially onto the local spatial plane of the two high-confidence regions to obtain the positions of the two projection points and the intensity values ​​of the two projection echoes. By selecting the two high-confidence regions with the closest horizontal azimuth angle to the low-confidence region point cloud for radial projection, the spatial correspondence between the low-confidence region point cloud and the stable structure can be established more accurately, improving the reliability of degradation migration calculation.

[0011] Preferably, when calculating the weighted sum of the spatial distance and echo intensity difference between the projected point cloud and each fitted point on the spatial plane, the two fitted points with the smallest spatial distance to the projected point of the point cloud are found among all fitted points on the local spatial plane of each high-confidence region. The absolute values ​​of the spatial distance and echo intensity difference between the point cloud and these two fitted points are calculated respectively. The sum of the spatial distance between the point cloud and the two fitted points and the sum of the absolute values ​​of the echo intensity difference are added together to obtain the weighted sum of the point cloud in the high-confidence region. The average of the weighted sums of the point cloud in the two high-confidence regions is taken as the degradation migration amount of the point cloud. This calculation method integrates spatial geometric deviation and echo intensity deviation, which can more comprehensively reflect the degree of migration of the point cloud in the degradation direction in the low-confidence region and reduce misjudgments caused by relying on only a single feature.

[0012] Preferably, when performing initial global pose estimation of the point cloud frame at the current motion moment based on the degradation migration amount of all low-confidence region point clouds within each layer at the current motion moment, the degradation migration amounts of all low-confidence region point clouds are sorted from near to far according to the radial distance of the point clouds. All low-confidence region point clouds whose degradation migration amounts are below the median of the sorted sequence are selected as reliable degradation point clouds. The coordinate differences and echo intensity differences between the scan position of the reliable degradation point cloud at the current motion moment and its scan position at the previous motion moment are jointly weighted to obtain the initial global pose estimation for the current motion moment. By selecting reliable point clouds with lower degradation migration amounts to participate in the initial pose estimation, the interference of severely degraded regions on the global pose estimation can be suppressed, improving the accuracy and stability of the initial pose estimation.

[0013] Preferably, when obtaining the LiDAR anti-degradation motion estimate at the current motion moment based on the spatial continuity constraints of each point cloud in the initial global pose estimation between adjacent scanning layers, each point cloud in the initial global pose estimation is mapped to its nearest radially distant paired point cloud in its adjacent scanning layer. The spatial distance difference between the point cloud and its paired point cloud is calculated, and the sum of the squares of the spatial distance differences of all point clouds is used as the spatial continuity constraint. With the goal of minimizing this spatial continuity constraint, the initial global pose estimation is iteratively adjusted, and the pose estimate after iterative convergence is used as the LiDAR anti-degradation motion estimate at the current motion moment. By optimizing the spatial continuity constraints between adjacent scanning layers, structural discontinuities and local distortions in LiDAR pose estimation under degradation scenarios can be further reduced, improving the consistency and anti-degradation performance of the motion estimate.

[0014] As a technical solution of the present invention, after obtaining the anti-degradation motion estimate of the LiDAR at the current motion moment, the anti-degradation motion estimate is projected onto the robot dog's body coordinate system to obtain the motion velocity estimate in the body coordinate system. The difference between this motion velocity estimate and the foot velocity estimate output by the robot dog's foot odometer is used as a motion consistency verification quantity. When the motion consistency verification quantity exceeds a preset threshold, the anti-degradation motion estimate is reverse-compensated according to the magnitude of the difference, and the reverse-compensated motion estimate is used as the final output motion estimate. This solution utilizes motion consistency verification and reverse compensation between the foot odometer and the LiDAR motion estimate to further reduce the error of LiDAR motion estimation in degradation scenarios and improve the motion estimation accuracy of the robot dog in real complex environments.

[0015] Preferably, when performing reverse compensation based on the magnitude of the difference in the anti-degradation motion estimate, the difference is decomposed into a longitudinal component along the robot's forward direction and a lateral component perpendicular to the forward direction. The absolute ratio of the longitudinal component to the lateral component is calculated for each component. When the absolute ratio is greater than 1, half of the longitudinal component is subtracted in the reverse direction along the robot's forward direction as the longitudinal compensation amount, while the lateral component remains unchanged. When the absolute ratio is less than or equal to 1, half of the lateral component is subtracted in the reverse direction perpendicular to the forward direction as the lateral compensation amount, while the longitudinal component remains unchanged. By performing targeted compensation based on the directional distribution of the motion error, over-correction that causes new pose disturbances can be avoided, improving the stability and accuracy of the final output motion estimate.

[0016] The technical effects and advantages provided by the present invention in the above technical solution are as follows:

[0017] The point clouds of each scanning layer of the lidar are divided into high-confidence and low-confidence regions based on their 3D spatial distribution density and echo intensity distribution. The nearest neighbor set of each point cloud within the high-confidence region is extracted. Local plane fitting parameters are generated by weighting the point cloud height variation and echo intensity difference of the nearest neighbor set. This process incorporates both the geometric spatial distribution of the point cloud and the echo intensity distribution into the confidence assessment process. Compared to conventional methods that only select regions based on the number of point clouds or a single reflection intensity, this approach can distinguish high-confidence regions with stable geometric constraints and consistent echo differences in degraded scenarios such as corridors, open areas, and single facades. The point cloud height variation and echo intensity difference are multiplied to form the local plane fitting weights. This allows the local plane fitting parameters to suppress neighborhoods with drastic height changes but unstable echo intensity, or neighborhoods with significant echo intensity changes but flat spatial height, reducing interference from low-discrimination neighborhoods and obtaining a consistent local plane reference. The point cloud in the low-confidence region is projected radially along the lidar to the spatial plane containing the adjacent high-confidence region. The degradation migration is calculated as the weighted sum of the spatial distance and echo intensity difference between the projected point cloud and the fitted point on the spatial plane. This process does not directly use unstable points within the low-confidence region for inter-frame registration. Instead, it maps them to the spatial plane fitted by the high-confidence region and uses the spatial distance and echo intensity difference to jointly quantify the migration degree of the point cloud in the degradation direction. This preserves the observation information of the low-confidence region while suppressing the influence of weakly constrained points and outliers on pose estimation. Based on the degradation migration of the point cloud in the low-confidence region within each layer at the current motion moment, a global initial pose estimate is performed. The initial estimate is iteratively adjusted with the goal of minimizing the sum of squared differences in spatial distance between point clouds in adjacent scanning layers. This provides supplementary constraints on spatial continuity between layers for pose estimation in scenarios where the low-confidence region accounts for a large proportion. Compared to pose estimation methods that rely solely on features within a single frame's point cloud or the distance between the nearest points in the same frame, this spatial continuity constraint can reduce pose drift caused by local point cloud degradation in directions with weak geometric features, thus obtaining a degraded motion estimate. Attached Figure Description

[0018] To more clearly illustrate the technical solutions in the embodiments of this application or the prior art, the drawings used in the embodiments will be briefly introduced below. Obviously, the drawings described below are only some embodiments recorded in this invention. For those skilled in the art, other drawings can be obtained based on these drawings.

[0019] Figure 1 This is a flowchart of a LiDAR algorithm for robot dog-based anti-degradation scenarios;

[0020] Figure 2 This is a flowchart of the horizontal azimuth confidence region division of lidar point cloud;

[0021] Figure 3 This is a schematic diagram illustrating the high and low confidence regions of the horizontal azimuth angle partitioning of the lidar scanning layer;

[0022] Figure 4 These are the distribution curves of point cloud degradation migration and kernel density estimation in low-confidence regions;

[0023] Figure 5 It is a comparison curve of the coordinate difference between reliable degraded point clouds and the echo intensity difference;

[0024] Figure 6 This is a schematic diagram of motion consistency verification and reverse compensation for anti-degradation motion estimation of the robot dog lidar. Detailed Implementation

[0025] To make the objectives, technical solutions, and advantages of the embodiments of the present invention clearer, the technical solutions of the embodiments of the present invention will be clearly and completely described below with reference to the accompanying drawings. Obviously, the described embodiments are only some embodiments of the present invention, not all embodiments. Based on the embodiments of the present invention, all other embodiments obtained by those skilled in the art without creative effort are within the scope of protection of the present invention.

[0026] See Figure 1 This invention provides a LiDAR algorithm for robot dog anti-degradation scenarios, comprising: acquiring point cloud data of each scanning layer of the LiDAR within a preset time period before the current movement moment of the robot dog, and dividing each layer into high-confidence regions and low-confidence regions based on the three-dimensional spatial distribution density and echo intensity distribution of the point cloud data of each layer; extracting the nearest neighbor set of each point cloud in the spatial neighborhood of the high-confidence region, and generating a set of local plane fitting parameters for each high-confidence region based on the point cloud height change and echo intensity difference of the nearest neighbor set; and dividing the low-confidence region into high-confidence regions. Each point cloud in the domain is projected radially along the lidar to the spatial plane of the adjacent high-confidence region. The weighted sum of the spatial distance and echo intensity difference between the point cloud and each fitted point on the spatial plane after projection is calculated as the degradation migration amount of the point cloud. Based on the degradation migration amount of all low-confidence region point clouds in each layer at the current motion moment, the global pose initial estimation of the point cloud frame at the current motion moment is performed. Based on the spatial continuity constraint of each point cloud between adjacent scanning layers in the global pose initial estimation, the lidar anti-degradation motion estimate at the current motion moment is obtained.

[0027] In specific implementation, please refer to Figure 2For each scanning layer of the LiDAR, the horizontal azimuth angle is divided into equally spaced zones from 0 degrees to 360 degrees. The angular interval of the equally spaced horizontal azimuth angle zones is consistent with the horizontal angular resolution of the LiDAR to ensure that each horizontal azimuth angle zone contains at least one scanning point. For all point clouds within the current layer, they are classified and categorized according to their horizontal azimuth angles. Each point cloud is assigned to the corresponding horizontal azimuth angle zone.

[0028] In some embodiments, for each horizontal azimuth partition, the initial number of point clouds is zero, and the initial accumulated echo intensity value is zero. All point clouds within the current layer are traversed. For each point cloud encountered, the horizontal azimuth partition to which the point cloud belongs is determined based on its horizontal azimuth. The number of point clouds in the corresponding horizontal azimuth partition is incremented by one, and the accumulated echo intensity value of the corresponding horizontal azimuth partition is increased by the echo intensity of the point cloud. After traversal, for each horizontal azimuth partition, the accumulated echo intensity value is divided by the number of point clouds to obtain the average echo intensity of the corresponding horizontal azimuth partition.

[0029] In practice, for each horizontal azimuth zone, the ratio of the number of point clouds to the average echo intensity is calculated using the following formula:

[0030]

[0031] in, Indicates the first The ratio of the number of point clouds in each horizontal azimuth zone to the mean echo intensity. Indicates the first The number of point clouds within each horizontal azimuth angle partition Indicates the first The average echo intensity within each horizontal azimuth zone.

[0032] After obtaining the ratio of the number of point clouds to the mean echo intensity for all horizontal azimuth zones within the current concentric circle, calculate the global standard deviation of the ratios for all horizontal azimuth zones. The global standard deviation is calculated as follows: first, calculate the average of the ratios for all horizontal azimuth zones within the current concentric circle; then, calculate the square of the difference between the ratio for each horizontal azimuth zone and the average; finally, take the square root of the average of all squared values ​​to obtain the global standard deviation.

[0033] In some embodiments, the ratio of each horizontal azimuth partition is compared with the global standard deviation. If the ratio is greater than the global standard deviation, all point clouds within the corresponding horizontal azimuth partition are marked as high-confidence regions; if the ratio is less than or equal to the global standard deviation, all point clouds within the corresponding horizontal azimuth partition are marked as low-confidence regions. After comparing and marking all horizontal azimuth partitions within the current layer, the current layer is divided into high-confidence and low-confidence regions.

[0034] See Figure 3 In the figure, the horizontal axis represents the horizontal azimuth angle partition within a certain scanning layer of the lidar, in degrees, ranging from 0 to 360 degrees, consistent with the equally spaced partitioning of the horizontal azimuth angle of the scanning layer in the above embodiment. The vertical axis represents the ratio of the number of point clouds to the average echo intensity, reflecting the relationship between the number of point clouds and the average echo intensity within each horizontal azimuth angle partition, consistent with the ratio calculation formula in the above embodiment. Definition.

[0035] In the figure, blue scatter dots represent the ratio data of horizontal azimuth angle partitions classified as high-confidence regions, red scatter dots represent the ratio data of horizontal azimuth angle partitions classified as low-confidence regions, and the black dashed line represents the global standard deviation threshold line. According to the description of the above embodiment, if the ratio of a certain horizontal azimuth angle partition is greater than the global standard deviation threshold, then all point clouds within that region are marked as high-confidence regions; otherwise, they are marked as low-confidence regions. In the figure, the ratios of high-confidence regions are significantly higher than the global standard deviation threshold line and are relatively dispersed, with values ​​generally covering a range from 1 to above 6, indicating a large difference in the ratio of point cloud number to mean echo intensity between different horizontal azimuth angle partitions. The ratios of low-confidence regions are all closely distributed below the threshold line, with values ​​concentrated between 0 and 1.2, indicating that the number of point clouds in the corresponding regions is lower than the mean echo intensity.

[0036] In practice, the nearest neighbor set within the spatial neighborhood of each point cloud in the high-confidence region is extracted. For the target point cloud in the high-confidence region, the current scanning layer of the target point cloud is determined with its spatial location as the center, and the upper and lower adjacent scanning layers are obtained. From the current scanning layer, the upper adjacent scanning layer, and the lower adjacent scanning layer, a preset number of neighboring point clouds with the smallest spatial Euclidean distance are selected. The preset number is set to 5, based on the principle that 5 neighboring point clouds can contain sufficient surface undulation information within a local area, while avoiding the introduction of point clouds that are too far apart, which could cause local planar fitting deviations.

[0037] In practice, the spatial Euclidean distance is calculated as follows: For the target point cloud and candidate neighboring point clouds, the 3D spatial coordinates of the target point cloud and the candidate neighboring point clouds are obtained respectively, and the Euclidean distance between them is calculated. Within the current scanning layer, all candidate neighboring point clouds are sorted by spatial Euclidean distance from smallest to largest, and the top 5 candidate neighboring point clouds are selected. Within the upper adjacent scanning layer, all candidate neighboring point clouds are sorted by spatial Euclidean distance from smallest to largest, and the top 5 candidate neighboring point clouds are selected. Within the lower adjacent scanning layer, all candidate neighboring point clouds are sorted by spatial Euclidean distance from smallest to largest, and the top 5 candidate neighboring point clouds are selected. The 5 candidate neighboring point clouds of the current scanning layer, the 5 candidate neighboring point clouds of the upper adjacent scanning layer, and the 5 candidate neighboring point clouds of the lower adjacent scanning layer are merged to form the nearest neighbor set of the target point cloud.

[0038] In practice, the point cloud height change and echo intensity difference are calculated based on the nearest neighbor point set. The point cloud height change is calculated as follows: obtain the height coordinates of all point clouds in the nearest neighbor point set; determine the maximum and minimum height values ​​from all height coordinates; and subtract the minimum height value from the maximum height value to obtain the point cloud height change. The echo intensity difference is calculated as follows: obtain the echo intensity values ​​of all point clouds in the nearest neighbor point set; determine the maximum and minimum echo intensity values ​​from all echo intensity values; and subtract the minimum echo intensity value from the maximum echo intensity value to obtain the echo intensity difference.

[0039] In practice, the weighting coefficients for the local plane fitting of the high-confidence region to which the target point cloud belongs are obtained by multiplying the point cloud height change by the echo intensity difference. The weighting coefficients are obtained using the following formula:

[0040]

[0041] in, The weighting coefficients represent the local plane fitting of the high-confidence region. They are dimensionless values ​​and range from 0 to 0. This represents the change in point cloud height of the nearest neighbor set, in meters. This represents the difference in echo intensity among the nearest neighbor set, and the unit is the same as that of the lidar echo intensity. The height normalization feature scale is represented by the standard deviation of the height coordinates in the lidar scan point cloud or a preset height reference range, and the unit is meters. The normalized feature scale for echo intensity is represented by the standard deviation of echo intensity in the LiDAR scanned point cloud or a preset reference range for echo intensity, with units consistent with those of the LiDAR echo intensity. The dimensionless relative value of height change is obtained by dividing the point cloud height change by the height normalized feature scale, and the dimensionless relative value of echo intensity difference is obtained by dividing the echo intensity difference by the echo intensity normalized feature scale. Multiplying these two values ​​yields a dimensionless weighting coefficient. The weighting coefficient is greater than or equal to 0 because both the normalized relative value of point cloud height change and the relative value of echo intensity difference are greater than or equal to 0. The weighting coefficient is obtained by first calculating the point cloud height change of the nearest neighbor set, then calculating the echo intensity difference of the nearest neighbor set, normalizing each value, and then multiplying them together to obtain the weighting coefficient.

[0042] In practice, a weighted plane fitting is performed on the nearest neighbor set of the target point cloud based on weight coefficients. The weighted plane fitting uses the weighted least squares method, taking the 3D spatial coordinates of each point cloud in the nearest neighbor set as the observation data for plane fitting, and using the weight coefficients as the fitting weights for each point cloud in the nearest neighbor set. The objective of the weighted plane fitting is to minimize the sum of squared weighted distances from all nearest neighbor point clouds to the plane to be determined, which consists of a plane normal vector and a plane bias term. Solving the weighted least squares problem yields the plane normal vector and the plane bias term, which are then used as the local plane fitting parameters for the high-confidence region to which the target point cloud belongs.

[0043] In practice, for each point cloud in a low-confidence region, all high-confidence regions are acquired within the same scanning layer where the low-confidence region point cloud is located. The absolute value of the difference between the horizontal azimuth angle of the low-confidence region point cloud and the representative value of the horizontal azimuth angle of each high-confidence region is calculated. The representative value of the horizontal azimuth angle is taken as the center azimuth angle of the corresponding horizontal azimuth angle partition of the high-confidence region. All high-confidence regions are sorted in ascending order of the absolute value of the horizontal azimuth angle difference, and the two high-confidence regions with the smallest absolute value of the horizontal azimuth angle difference are selected as the first high-confidence region and the second high-confidence region.

[0044] In the specific implementation, the local plane fitting parameters corresponding to the first high-confidence region and the second high-confidence region are obtained respectively. The local plane fitting parameters include the plane normal vector and the plane bias term. The point cloud of the low-confidence region is projected along the radial direction of the lidar onto the local spatial plane where the first high-confidence region is located to obtain the position of the first projection point and the first projection echo intensity value. The point cloud of the low-confidence region is then projected along the radial direction of the lidar onto the local spatial plane where the second high-confidence region is located to obtain the position of the second projection point and the second projection echo intensity value.

[0045] In some embodiments, the radial direction of the lidar is a direction vector pointing from the lidar scanning center to the spatial location of the low-confidence region point cloud. The projection calculation method is as follows: A radial ray is constructed from the lidar scanning center as the starting point and the spatial location of the low-confidence region point cloud as the directional endpoint. The radial ray intersects with the local spatial plane containing the first high-confidence region; the intersection point is the location of the first projection point. The radial ray intersects with the local spatial plane containing the second high-confidence region; the intersection point is the location of the second projection point. The first projected echo intensity value is obtained by interpolating the echo intensity of the fitted point on the local spatial plane containing the first high-confidence region, and the second projected echo intensity value is obtained by interpolating the echo intensity of the fitted point on the local spatial plane containing the second high-confidence region.

[0046] In specific implementation, for the local spatial plane where the first high-confidence region is located, the two fitting points with the smallest spatial distance to the first projection point are found and denoted as the first fitting point and the second fitting point of the first high-confidence region, respectively. The spatial distance between the first projection point and the first fitting point of the first high-confidence region is calculated and denoted as the first spatial distance of the first high-confidence region. The spatial distance between the first projection point and the second fitting point of the first high-confidence region is calculated and denoted as the second spatial distance of the first high-confidence region. The absolute value of the difference between the first projected echo intensity value and the echo intensity value of the first fitting point of the first high-confidence region is calculated and denoted as the absolute value of the first echo intensity difference of the first high-confidence region. The absolute value of the difference between the first projected echo intensity value and the echo intensity value of the second fitting point of the first high-confidence region is calculated and denoted as the absolute value of the second echo intensity difference of the first high-confidence region.

[0047] In practice, the first spatial distance, the second spatial distance, the absolute value of the first echo intensity difference, and the absolute value of the second echo intensity difference in the first high-confidence region are added together to obtain a weighted sum of the point cloud data of the low-confidence region over the first high-confidence region. This weighted sum is obtained using the following formula:

[0048]

[0049] in, This represents the weighted sum of the point cloud in the low-confidence region over the first high-confidence region, and is a dimensionless value. This represents the spatial distance between the first projection point and the first fitted point in the first high-confidence region, expressed in meters. This represents the spatial distance between the first projection point and the second fitting point in the first high-confidence region, expressed in meters. This represents the intensity value of the first projected echo. This represents the echo intensity value at the first fitted point in the first high-confidence region. This represents the echo intensity value of the second fitted point in the first high-confidence region. The unit of the echo intensity value is consistent with the unit of the lidar echo intensity. The scale represents the normalized feature of spatial distance, which is the standard deviation of spatial distance in the LiDAR scan point cloud or a preset distance reference range, and the unit is meters. This represents the normalized characteristic scale of the echo intensity, taken as the standard deviation of the echo intensity in the lidar scanned point cloud or a preset reference range of echo intensity, with units consistent with the lidar echo intensity units. Spatial distance is calculated using Euclidean distance in three-dimensional spatial coordinates, with values ​​greater than or equal to 0, divided by... This is then converted to a dimensionless value. The absolute value of the echo intensity difference is obtained by subtracting the two echo intensity values ​​and taking the absolute value, which is greater than or equal to 0. (The value is then divided by...) The values ​​are then converted to dimensionless values. The first projected echo intensity value is obtained by interpolating the echo intensity of the fitted point on the local spatial plane where the first high-confidence region is located.

[0050] In specific implementation, for the local spatial plane where the second high-confidence region is located, find the two fitting points with the smallest spatial distance to the second projection point, and denot them as the first fitting point and the second fitting point of the second high-confidence region, respectively. Calculate the spatial distance between the second projection point and the first fitting point of the second high-confidence region, and denot it as the first spatial distance of the second high-confidence region. Calculate the spatial distance between the second projection point and the second fitting point of the second high-confidence region, and denot it as the second spatial distance of the second high-confidence region. Calculate the absolute value of the difference between the second projection echo intensity value and the echo intensity value of the first fitting point of the second high-confidence region, and denot it as the absolute value of the first echo intensity difference of the second high-confidence region. Calculate the absolute value of the difference between the second projection echo intensity value and the echo intensity value of the second fitting point of the second high-confidence region, and denot it as the absolute value of the second echo intensity difference of the second high-confidence region.

[0051] In practice, the first spatial distance of the second high-confidence region, the second spatial distance of the second high-confidence region, the absolute value of the first echo intensity difference of the second high-confidence region, and the absolute value of the second echo intensity difference of the second high-confidence region are added together to obtain the weighted sum of the low-confidence region point cloud over the second high-confidence region. Finally, the weighted sum of the low-confidence region point cloud over the first high-confidence region is added together with the weighted sum of the low-confidence region point cloud over the second high-confidence region, and the result is divided by 2 to obtain the degradation migration amount of the low-confidence region point cloud.

[0052] See Figure 4In the figure, the horizontal axis represents degradation migration, and the vertical axis represents frequency. The green bars indicate the distribution of degradation migration in the low-confidence region point cloud in the above embodiment. Overall, the degradation migration distribution exhibits a right-skewed long-tailed shape, with higher frequencies in the low-value range. The peak values ​​are concentrated between approximately 3 and 10, indicating that the degradation migration in most low-confidence region point clouds is within a relatively small range. As the degradation migration increases, the frequency gradually decreases, approaching zero when it reaches 40 or higher, indicating that point clouds with extremely high degradation migration are relatively few.

[0053] The red dashed line represents the kernel density estimation curve, used to smooth the probability density distribution of degradation migration. The kernel density curve peaks at approximately 7 degradation migration points and then slowly declines, consistent with the distribution trend of the histogram, reflecting the central tendency and tail distribution characteristics of degradation migration.

[0054] In practice, the initial global pose estimation of the point cloud frame at the current motion moment is performed based on the degradation migration of all low-confidence region point clouds within each layer at the current motion moment. The radial distance of each low-confidence region point cloud is obtained; the radial distance is the Euclidean distance from the LiDAR scanning center to the 3D spatial coordinates of the low-confidence region point cloud. All low-confidence region point clouds are sorted from closest to furthest in radial distance, resulting in a sorted sequence. The median position of the sorted sequence is determined; the median position is the sorting number corresponding to the total number of low-confidence region point clouds divided by 2 and rounded up. All low-confidence region point clouds in the sorted sequence that are located before the median position are considered reliable degraded point clouds, which are the set of half of the point clouds with the closest radial distance in the sorted sequence.

[0055] In some embodiments, for each reliable degraded point cloud, the scan position coordinates of the reliable degraded point cloud at the current motion time and the scan position coordinates at the previous motion time are obtained. The scan position coordinates include three components: horizontal azimuth, radial distance, and height coordinates. The scan position coordinates are converted into three-dimensional spatial coordinates in a Cartesian coordinate system. The echo intensity values ​​of the reliable degraded point cloud at the current motion time and the previous motion time are obtained. The coordinate difference vector between the scan position of the reliable degraded point cloud at the current motion time and the scan position at the previous motion time is calculated. The coordinate difference vector is the three-dimensional spatial coordinates at the current motion time minus the three-dimensional spatial coordinates at the previous motion time. The absolute value of the echo intensity difference between the echo intensity values ​​of the reliable degraded point cloud at the current motion time and the echo intensity values ​​at the previous motion time is calculated.

[0056] In practice, the coordinate difference vector and the absolute value of the echo intensity difference are jointly weighted to obtain the initial global pose estimate at the current motion moment. The joint weighting employs a weighted least squares error minimization method to construct a joint weighted error for the reliable degraded point cloud. The joint weighted error is obtained using the following formula:

[0057]

[0058] in, This represents the joint weighted error of the initial global pose estimation, expressed in square meters. This represents the total number of reliable degraded point clouds. This represents the coordinate difference weighting coefficient, with a value of 1.0, and is a dimensionless coefficient. This represents the echo intensity difference weighting coefficient, with a value of 0.5, and is a dimensionless coefficient. This represents the mapping coefficient from echo intensity to length, expressed in meters per unit of echo intensity, converting differences in echo intensity into equivalent spatial distance differences. The value is pre-calibrated based on the correspondence between changes in echo intensity and changes in spatial position. Indicates the first The three-dimensional spatial coordinate vector of a reliable degenerate point cloud at the current moment of motion, in meters. Indicates the first The three-dimensional spatial coordinate vector of a reliable degenerate point cloud at the previous moment of motion, in meters. Indicates the first The echo intensity value of a reliable degraded point cloud at the current moment of motion. Indicates the first The echo intensity value of a reliable degraded point cloud at the previous moment of motion, with units consistent with those of lidar echo intensity. The Euclidean norm of a three-dimensional spatial coordinate vector. Coordinate difference weighting coefficient. The value is 1.0, which is the weighting coefficient for echo intensity difference. The value is set to 0.5, based on the following: coordinate differences are expressed in meters, directly retaining the original dimensional weights; and through mapping coefficients... The echo intensity difference is converted into an equivalent spatial distance and then squared to unify the dimensions of both quantities to square meters. A value of 0.5 is used to reduce the contribution of echo intensity difference to half of the contribution of coordinate difference, so as to avoid echo intensity noise having too much impact on the initial global pose estimation.

[0059] By minimizing the joint weighted error of the initial global pose estimation The goal is to solve for the initial global pose estimate at the current moment of motion. The minimization method involves taking the partial derivatives of the motion parameters in the initial global pose estimate and setting them to zero. These motion parameters include the translation vector and rotation quaternions in the robot's body coordinate system. The obtained translation vector and rotation quaternions are then used as the initial global pose estimate at the current moment of motion.

[0060] In practical implementation, the anti-degradation motion estimate of the lidar at the current motion moment is obtained based on the spatial continuity constraints of each point cloud in the initial global pose estimation between adjacent scanning layers. Each point cloud in the initial global pose estimation is mapped to the nearest radially distant paired point cloud in its adjacent scanning layers. For a point cloud in the initial global pose estimation, the scanning layer in which the point cloud is located is determined, and the adjacent scanning layers are obtained. The adjacent scanning layers are the previous and next scanning layers adjacent to the scanning layer in which the point cloud is located. In the adjacent scanning layers, the radial distance of each point cloud is calculated, and the point cloud with the smallest absolute value of the difference between its radial distance and the current point cloud's radial distance is taken as the paired point cloud of the current point cloud. Then, the spatial distance difference between the current point cloud and the paired point cloud is calculated. The spatial distance difference is the Euclidean distance between the three-dimensional spatial coordinates of the current point cloud and the three-dimensional spatial coordinates of the paired point cloud. The sum of the squares of the spatial distance differences of all point clouds is used as the spatial continuity constraint. The calculation of spatial continuity constraints does not include coordinate difference weighting coefficients and echo intensity difference weighting coefficients; the spatial continuity constraints consist only of the sum of squares of spatial distance differences.

[0061] The initial global pose estimate is iteratively adjusted with the goal of minimizing the spatial continuity constraint. The Levenberg-Marquardt method is used for iterative adjustment, with the Jacobian matrix of the spatial continuity constraint with respect to the motion parameters in the initial global pose estimate serving as the iteration direction. Each iteration updates the translation vector and rotation quaternion. The convergence criteria are set as follows: the change in the spatial continuity constraint between two consecutive iterations is less than 0.001, or the number of iterations reaches 20. The pose estimate obtained after iterative convergence is used as the anti-degradation motion estimate for the lidar at the current motion moment.

[0062] See Figure 5 The curves in the figure illustrate the trends of the magnitude of the coordinate difference vector and the absolute value of the echo intensity difference of the point cloud in the low-confidence region as a function of the reliable degraded point cloud index during the initial global pose estimation based on the reliable degraded point cloud in the above embodiment. The horizontal axis represents the reliable degraded point cloud index sorted from near to far radially, ranging from 1 to 52; the vertical axis represents the numerical values ​​of the corresponding point cloud, covering the magnitude of the coordinate difference vector and the absolute value of the echo intensity difference.

[0063] The solid blue curve represents the magnitude of the coordinate difference vector, which remains at a low level overall, fluctuating between approximately 0 and 0.15. This indicates that the 3D spatial coordinates of the reliable degraded point cloud are relatively similar between the current and previous motion moments, and the spatial position changes are stable. The curve shows slight undulations, but no obvious upward trend, suggesting that the spatial motion changes between point clouds are relatively continuous and smooth.

[0064] The dashed orange curve represents the absolute value of the echo intensity difference, which fluctuates considerably, ranging from approximately 0 to 10. The curve shows a clear phased change: the echo intensity difference is relatively low for the first 20 point clouds, fluctuating between 0 and 8; the middle range (approximately index 20 to 35) shows several peaks in echo intensity difference, with the maximum value approaching 10; and the latter half (approximately index 35 to 52) shows an overall higher echo intensity difference with multiple oscillations, indicating significant changes in echo intensity for some reliable degraded point clouds within this range.

[0065] In practice, after obtaining the anti-degradation motion estimate of the LiDAR at the current moment of motion, this estimate is projected onto the robot dog's body coordinate system. The anti-degradation motion estimate at the current moment of motion includes the displacement vector and rotation quaternion in the LiDAR coordinate system. Dividing the displacement vector in the LiDAR coordinate system by the time interval between the current and previous moments yields the velocity vector in the LiDAR coordinate system. Based on the calibration rotation matrix between the LiDAR and robot dog body coordinate systems, the velocity vector in the LiDAR coordinate system is rotated to the robot dog body coordinate system, resulting in the estimated velocity value in the body coordinate system. The calibration rotation matrix between the LiDAR and robot dog body coordinate systems is obtained through a pre-calibration process, which involves transforming the LiDAR point cloud coordinates to the robot dog body coordinate system.

[0066] In practice, the estimated foot velocity output from the robot dog's foot-end odometer is obtained. The foot-end odometer outputs the estimated foot velocity in the robot dog's body coordinate system. Subtracting the estimated motion velocity in the body coordinate system from the estimated foot velocity output by the foot-end odometer yields a velocity difference vector, which serves as a motion consistency verification quantity. This motion consistency verification quantity is a three-dimensional vector, possessing a longitudinal component along the robot dog's forward direction, a lateral component perpendicular to the forward direction, and a vertical component perpendicular to the robot dog's body plane in the robot dog's body coordinate system.

[0067] In the specific implementation, a method is set for judging when the motion consistency verification value exceeds a preset threshold. The preset threshold is set to 0.3 m / s, based on the upper limit of the allowable deviation between the foot-based odometer speed estimate and the LiDAR motion speed estimate when the robot dog is walking at medium speed. 0.3 m / s can tolerate normal sensor noise and can distinguish whether the LiDAR anti-degradation motion estimate has obvious anomalies. The magnitude of the motion consistency verification value is calculated. When the magnitude of the motion consistency verification value is greater than the preset threshold, it is determined that the anti-degradation motion estimate at the current moment needs to be reverse compensated. When the magnitude of the motion consistency verification value is less than or equal to the preset threshold, no reverse compensation is performed, and the anti-degradation motion estimate at the current moment is directly used as the final output motion estimate.

[0068] In practical implementation, when the magnitude of the motion consistency verification quantity exceeds a preset threshold, the motion consistency verification quantity is decomposed into a longitudinal component along the robot dog's forward direction and a lateral component perpendicular to the forward direction. The absolute values ​​of the longitudinal and lateral components are calculated separately, and then the ratio of the absolute values ​​of the longitudinal and lateral components is calculated. The ratio is obtained using the following formula:

[0069]

[0070] in, This represents the ratio of the absolute values ​​of the vertical component to the horizontal component. This represents the longitudinal component of the motion consistency verification quantity along the robot dog's forward direction. This represents the lateral component of the motion consistency check quantity perpendicular to the forward direction. This indicates the operation of taking the absolute value of a real number. The range of values ​​for is greater than or equal to 0. When When the value is 0, Defined as infinity and considered to be greater than 1. The method of obtaining the motion consistency verification quantity is as follows: obtain the coordinate value of the motion consistency verification quantity along the forward direction axis in the robot dog's body coordinate system, and use the coordinate value as the longitudinal component; The method for obtaining the motion consistency verification quantity is as follows: obtain the coordinate value of the vertical forward direction axis in the robot dog's body coordinate system, and use the coordinate value as the horizontal component.

[0071] In practical implementation, comparison The size of 1. When When the value is greater than 1, half of the longitudinal component is subtracted in the reverse direction of the robot dog's movement to obtain the longitudinal compensation amount, while keeping the lateral component unchanged. The reason for setting the half coefficient is that taking half of the longitudinal component for the reverse compensation amount can reduce compensation overshoot and prevent the estimated motion speed after compensation from exceeding the estimated foot speed output by the robot dog's foot odometer. The longitudinal compensation amount replaces the original longitudinal component, while the lateral and vertical components remain unchanged, resulting in the compensated velocity difference vector. The compensated velocity difference vector is added to the motion speed estimate in the body coordinate system to obtain the reverse-compensated motion speed estimate. The reverse-compensated motion speed estimate is multiplied by the time interval between the current motion moment and the previous motion moment to obtain the reverse-compensated displacement vector. Then, the reverse-compensated displacement vector is combined with the rotation quaternion in the anti-degradation motion estimate at the current motion moment to obtain the reverse-compensated motion estimate, which is used as the final output motion estimate.

[0072] In specific implementation, when When the value is less than or equal to 1, half of the lateral component is subtracted in the opposite direction perpendicular to the forward direction as the lateral compensation amount, while keeping the longitudinal component unchanged. The half-component coefficient is set according to the same principle as for longitudinal compensation. The lateral compensation amount replaces the original lateral component, while the longitudinal and vertical components remain unchanged, resulting in the compensated velocity difference vector. The compensated velocity difference vector is added to the motion velocity estimate in the body coordinate system to obtain the reverse-compensated motion velocity estimate. The reverse-compensated motion velocity estimate is multiplied by the time interval between the current motion moment and the previous motion moment to obtain the reverse-compensated displacement vector. The reverse-compensated displacement vector is then combined with the rotation quaternion in the anti-degradation motion estimate at the current motion moment to obtain the reverse-compensated motion estimate, which is used as the final output motion estimate.

[0073] See Figure 6 In the figure, the horizontal axis represents the sequence number of the motion moment, and the vertical axis represents the magnitude of the motion consistency verification quantity, in meters per second. The light blue curve represents the trend of the change in the magnitude of the motion consistency verification quantity corresponding to each motion moment, the black dashed line represents the preset threshold of 0.3 meters per second, and the red cross marks the moment when the motion consistency verification quantity exceeds the preset threshold, i.e., the reverse compensation point.

[0074] As shown in the figure, the motion consistency verification magnitude is below the preset threshold of 0.3 m / s for most of the motion moments, indicating that the LiDAR anti-degradation motion estimate and the velocity estimate output by the robot dog's foot odometer are in good consistency for the vast majority of the time, and no reverse compensation processing is required. However, at several discontinuous moments (approximately concentrated in the interval between sequence numbers 100 and 350 and close to sequence number 500), the motion consistency verification magnitude suddenly increases, significantly exceeding the preset threshold, triggering the reverse compensation mechanism.

[0075] The red crosses indicate the execution time of the motion estimation anomaly detection and compensation strategy described in the above embodiment. This strategy determines whether to correct the LiDAR anti-degradation motion estimate by judging whether the motion consistency verification magnitude exceeds a threshold, so as to ensure the stability and accuracy of the final output motion estimate.

[0076] The above description is merely a specific embodiment of this application, but the scope of protection of this application is not limited thereto. Any changes or substitutions that can be easily conceived by those skilled in the art within the scope of the technology disclosed in this application should be included within the scope of protection of this application.

Claims

1. A lidar algorithm for anti-degradation scenarios based on robot dogs, characterized in that, include: The system acquires point cloud data of each scanning layer of the LiDAR within a preset time period before the current movement of the robot dog, and divides each layer into high-confidence and low-confidence regions based on the three-dimensional spatial distribution density and point cloud echo intensity distribution of the point cloud data of each layer. Extract the nearest neighbor set of each point cloud in the spatial neighborhood of the high confidence region, and generate a set of local plane fitting parameters for each high confidence region based on the point cloud height change and echo intensity difference of the nearest neighbor set. Each point cloud in the low-confidence region is projected radially from the lidar to the spatial plane of the adjacent high-confidence region. The weighted sum of the spatial distance between the point cloud and each fitted point on the spatial plane after projection and the difference in echo intensity is calculated as the degradation migration amount of the point cloud. Based on the degradation migration of all low-confidence region point clouds in each layer at the current motion moment, a global initial pose estimation is performed on the point cloud frame at the current motion moment. Based on the spatial continuity constraints of each point cloud between adjacent scanning layers in the global initial pose estimation, the anti-degradation motion estimation of the lidar at the current motion moment is obtained.

2. The lidar algorithm based on robot dog anti-degradation scenarios according to claim 1, characterized in that, The step of dividing each concentric circle into high-confidence and low-confidence regions based on the three-dimensional spatial distribution density and point cloud echo intensity distribution of the point cloud data of each concentric circle further includes: The point cloud within the circle is divided into equally spaced zones according to the horizontal azimuth angle. The number of point clouds and the average echo intensity in each zone are counted, and the ratio of the number of point clouds to the average echo intensity in each zone is calculated. Calculate the global standard deviation of this ratio for all partitions within the concentric circle. Mark all point clouds in the corresponding partitions with ratios greater than the global standard deviation as high-confidence regions, and mark all point clouds in the corresponding partitions with ratios less than or equal to the global standard deviation as low-confidence regions.

3. The lidar algorithm based on robot dog anti-degradation scenarios according to claim 2, characterized in that, The step of extracting the set of nearest neighbors in the spatial neighborhood of each point cloud in the high-confidence region further includes: Centered on the target point cloud in the high confidence region, a preset number of neighboring point clouds with the smallest spatial Euclidean distance are selected in the current layer and the adjacent layers above and below the target point cloud. All the selected neighboring point clouds are combined into the nearest neighbor set of the target point cloud.

4. The lidar algorithm based on robot dog anti-degradation scenarios according to claim 3, characterized in that, The step of generating a set of local plane fitting parameters for each high-confidence region based on the point cloud height change and echo intensity difference of the nearest neighbor point set further includes: The difference between the maximum and minimum height values ​​of all point clouds in the nearest neighbor set is calculated as the height change of the point clouds in the nearest neighbor set. The difference between the maximum and minimum echo intensity values ​​of all point clouds in the nearest neighbor set is calculated as the echo intensity difference. The product of the point cloud height change and the echo intensity difference is used as the weight coefficient for the local plane fitting of the high confidence region. The local plane fitting parameters are obtained by performing weighted plane fitting on the nearest neighbor set according to the weight coefficient.

5. A lidar algorithm based on robot dog anti-degradation scenarios according to claim 1, characterized in that, The step of projecting each point cloud in the low-confidence region radially along the lidar to the spatial plane of the adjacent high-confidence region further includes: Within the same scanning layer of each point cloud in the low-confidence region, find the two high-confidence regions with the smallest difference in horizontal azimuth angle with the point cloud, and obtain the local plane fitting parameters corresponding to the two high-confidence regions respectively. The point cloud is projected along the radial direction of the lidar onto the local spatial plane where the two high-confidence regions are located, respectively, to obtain the positions of the two projection points and the intensity values ​​of the two projection echoes.

6. A lidar algorithm based on robot dog anti-degradation scenarios according to claim 5, characterized in that, The step of calculating the weighted sum of the spatial distance between the projected point cloud and each fitted point on the spatial plane and the echo intensity difference further includes: Among all fitted points on the local spatial plane of each high confidence region, find the two fitted points with the smallest spatial distance to the point cloud projection point, and calculate the absolute value of the spatial distance between the point cloud and the two fitted points and the echo intensity difference, respectively. The sum of the spatial distances between the point cloud and the two fitted points is added to the sum of the absolute values ​​of the echo intensity differences to obtain the weighted sum of the point cloud in the high confidence region. The average value of the weighted sums of the point cloud in the two high confidence regions is taken as the degradation migration amount of the point cloud.

7. A lidar algorithm based on robot dog anti-degradation scenarios according to claim 1, characterized in that, The step of performing initial global pose estimation of the point cloud frame at the current motion moment based on the degradation migration amount of all low-confidence region point clouds within each layer at the current motion moment further includes: The degradation migration of all low-confidence region point clouds is sorted from near to far according to the radial distance of the point clouds. All low-confidence region point clouds whose degradation migration is located before the median of the sorting sequence are selected as reliable degradation point clouds. The coordinate difference and echo intensity difference between the scan position of the reliable degraded point cloud at the current motion moment and its scan position at the previous motion moment are jointly weighted to obtain the initial global pose estimate at the current motion moment.

8. A lidar algorithm based on robot dog anti-degradation scenarios according to claim 7, characterized in that, The step of obtaining the anti-degradation motion estimate of the lidar at the current motion moment based on the spatial continuity constraints of each point cloud in the initial global pose estimation between adjacent scanning layers further includes: Each point cloud in the initial global pose estimation is mapped to the nearest radially distant paired point cloud in its adjacent scan layer. The spatial distance difference between the point cloud and the paired point cloud is calculated, and the sum of the squares of the spatial distance differences of all point clouds is used as the spatial continuity constraint. With the goal of minimizing the spatial continuity constraint, the initial global pose estimate is iteratively adjusted, and the pose estimate after iterative convergence is used as the anti-degradation motion estimate of the lidar at the current motion moment.

9. A lidar algorithm based on robot dog anti-degradation scenarios according to claim 1, characterized in that, Following the step of obtaining the lidar anti-degradation motion estimate at the current motion moment, the following steps are further defined: The anti-degradation motion estimate at the current moment of motion is projected onto the robot dog's body coordinate system to obtain the motion velocity estimate in the body coordinate system. The difference between this motion velocity estimate and the foot velocity estimate output by the robot dog's foot odometer is used as the motion consistency verification quantity. When the motion consistency verification value exceeds the preset threshold, the anti-degradation motion estimate is reverse-compensated according to the magnitude of the difference, and the reverse-compensated motion estimate is used as the final output motion estimate.

10. A lidar algorithm based on robot dog anti-degradation scenarios according to claim 9, characterized in that, The step of performing reverse compensation on the anti-degradation motion estimate according to the magnitude of the difference further includes: The difference is decomposed into a longitudinal component along the direction of the robot dog's movement and a lateral component perpendicular to the direction of movement, and the absolute ratio of the longitudinal component to the lateral component is calculated respectively. When the absolute value ratio is greater than 1, half of the longitudinal component is subtracted in the opposite direction of the robot dog's forward movement as the longitudinal compensation amount, while keeping the lateral component unchanged. When the absolute value ratio is less than or equal to 1, half of the lateral component is subtracted in the opposite direction perpendicular to the forward direction as the lateral compensation amount, while keeping the longitudinal component unchanged.

Images