Locating method and system of legged humanoid robot in degenerative scene

Abstract

The application relates to the technical field of robot simultaneous localization and navigation, and particularly relates to a positioning method and system of a foot-type humanoid robot in a degenerative scene, which improves the degenerative treatment from a whole-frame binary switch to continuous modulation on a direction level: translation sub-block feature decomposition of a laser observation information matrix is carried out, and a projection operator of an unobservable translation subspace is explicitly constructed; a translation increment projection remapping is remapped in a filter iteration, unobservable direction error laser correction is stripped, and observable direction laser constraint is reserved; a body velocity is taken as pseudo-observation through leg kinematics inverse solution based on foot contact judgment support phase, continuous bottom constraint of a gait cycle is provided for the unobservable direction, and the application is combined with gravity posture pseudo-observation, subspace limited velocity damping, single-frame correction quantity safeguard and geometric weighted confidence evaluation. The application can realize continuous output of a position during degeneration, eliminate positioning jump, make drift bounded, adapt to the gait characteristics of a foot-type robot, is low in calculation cost, and is suitable for embedded body deployment.

Description

Technical Field

[0001] This invention relates to the field of robot synchronous positioning and navigation technology, and in particular to a positioning method and system for a legged humanoid robot in degenerative scenarios. Background Technology

[0002] When legged humanoid robots perform real-time localization on an existing point cloud map, a tightly coupled LiDAR and inertial measurement unit (IMU) scheme is commonly used. The IMU is relied upon for state prediction, and the observation equation is constructed using the point-to-surface matching residuals between the LiDAR point cloud and the map. The pose is then iteratively solved within an error state filtering framework. This scheme works reliably in environments with rich geometric features, but it is prone to localization failures in geometrically degenerate scenarios. Geometrically degenerate scenarios include situations where the robot is facing a large, flat wall, in a narrow passage, in an open area, or facing a glass curtain wall. In these scenarios, the LiDAR point cloud is abundant and the point-to-surface fitting residuals are small, but all effective points are approximately coplanar. The LiDAR can only constrain translations along the normal to the plane; the two translational directions within the plane lack geometric constraints, resulting in structural observation gaps. Without processing, the pose estimation in the corresponding direction will degenerate into pure inertial integration, producing drift that accumulates over time. When the robot turns and the degeneracy is resolved, the accumulated error is corrected all at once, manifesting as a sudden jump in the localization output, which then propagates downstream to the navigation and control modules.

[0003] Compared to wheeled and tracked mobile platforms, legged humanoid robots face three unique constraints in this regard: First, the body is always in a state of periodic undulation and swaying during the gait cycle, and there are almost no moments when the whole machine is at zero speed; Second, the robot is not equipped with wheel encoders, and there are slippage and airborne phases on the soles of the feet, so the kinematic assumptions on which wheel odometry depends are invalid; Third, the legged platform does not satisfy the non-holonomic constraint, that is, the assumption that the vehicle body does not slip laterally, and lateral translation is its normal movement capability.

[0004] Several published patents address localization in degraded scenarios. Patent CN122360417A discloses a tightly coupled laser vision-inertial SLAM method for degraded scenarios, relying on spectral analysis to obtain scalar health status for degradation judgment. It introduces non-integrity constraints and uniform velocity constraints for virtual observation, but this scheme cannot locate specific, unobservable directions, and the constraints are unsuitable for legged robots. Patent CN119247378A discloses a lidar inertial odometry for location-awareness in challenging scenarios, employing point-by-point adaptive weighting to handle degradation, but it cannot solve the structural rank deficiency problem of the information matrix. Patent CN118129733A discloses a mobile robot localization method for typical lidar degradation scenarios, relying on a wheel encoder to provide a fallback factor and depending on backend nonlinear optimization, making it difficult to port to legged platforms. Patent CN119247375A discloses a legged robot localization method and system, relying on environmentally invariant feature database matching for localization, but it fails in degraded scenarios with missing features. Patent CN122329365A discloses an inertial navigation system self-alignment method device navigation system and medium, and patent CN122258869A discloses a zero-bias suppression method for low-cost inertial devices based on rotation modulation. Both of them require the entire machine to be stationary as a constraint triggering premise, which is difficult to be effective on walking legged humanoid robots. The latter also requires an additional hardware rotation mechanism.

[0005] Based on existing technologies, the following technical shortcomings exist in the problem of localization in degenerate scenarios for legged humanoid robots: First, the granularity of degradation determination is too coarse. Existing solutions use point count, effective match ratio, or normalized health scalar as criteria, which can only give an overall conclusion of whether degradation has occurred, but cannot pinpoint the specific, unobservable direction, resulting in subsequent processing having to be applied frame-by-frame.

[0006] Second, the degradation handling method is binary, with costs at both ends. If the entire frame is discarded and the pose is maintained after degradation is determined, the localization output will repeatedly jump between the maintained pose and the real-time pose; if it is updated as usual, the pose in the unobservable direction will be randomly pulled by observation noise and mismatches. Neither of these methods can maintain a continuous and reliable pose output during the degradation period.

[0007] Third, the fallback prior is incompatible with the legged platform. The non-holonomic constraint relies on the vehicle body's lateral non-slippage, and the wheeled odometer factor relies on the wheeled encoder and the assumption of non-slippage. Neither of these holds true for legged humanoid robots that have lateral movement capabilities and experience foot slippage and airborne phases. Directly transplanting these constraints would introduce erroneous constraints that conflict with real-world motion.

[0008] Fourth, the zero-speed assumption of the entire machine is difficult to trigger on a legged platform. Zero-speed correction based on the premise that the entire machine is stationary cannot provide effective constraints during robot walking.

[0009] Fifth, the self-consistency of the filter covariance is broken. During the pose-maintaining process, the covariance no longer reflects the true uncertainty, and the accumulated error is released all at once when the degradation is resolved, resulting in a positioning jump.

[0010] Therefore, a new technical solution is urgently needed to solve the above-mentioned technical problems. Summary of the Invention

[0011] The purpose of this invention is to overcome the problems of the prior art and provide a positioning method and system for legged humanoid robots in degenerative scenarios. This addresses the following technical problems in the positioning of existing legged humanoid robots in laser geometric degradation scenarios: the degradation determination granularity is too coarse, only outputting a scalar degree of degradation and failing to identify specific, unobservable translational directions; degradation handling uses a full-frame binary switch, leading to frequent abrupt changes in positioning output; existing catch-all constraints rely on wheel encoders, lateral non-slip assumptions, or overall robot stillness, which are incompatible with the motion characteristics of legged humanoid robots; the covariance of the degradation process is inconsistent, with concentrated error release at the moment of degradation resolution; sufficient point count but ineffective positioning quality evaluation under coplanar point cloud degradation conditions; and the computational load of the backend nonlinear optimization scheme is large, making it difficult to run in real-time embedded within the robot body with limited computing power.

[0012] The above objectives are achieved through the following technical solutions: This invention provides a method for positioning a legged humanoid robot in a degenerative scenario, comprising the following steps: Step S1: Acquire the inertial measurement data output by the inertial measurement unit, perform inertial forward prediction, and output the nominal state, covariance, and intra-frame continuous pose for point cloud distortion correction. Step S2: Obtain the laser point cloud collected by the lidar, and use the intra-frame continuous pose to perform distortion correction and downsampling processing on the laser point cloud; Step S3: Perform a true lower bound determination of geometry based on the number of downsampled point clouds; if it is determined that there is a true geometric missing, skip the laser observation update for the entire frame; otherwise, construct the laser point-surface residual and observation information matrix, and output the observation information matrix to the next step; Step S4: Receive the observation information matrix, perform feature decomposition on its translation sub-blocks, identify the unobservable translation subspace, and construct the corresponding projection operator for the unobservable translation subspace; Step S5: Inside the iterative solution of the error state filter, the projection operator of the unobservable translation subspace is called to perform projection remapping on the translation increment of the filter output, stripping the translation increment component of the unobservable subspace, retaining the translation increment component of the observable subspace, and updating the internal state of the filter. Step S6: Acquire foot contact state and leg joint sensing data. When a foot in the support phase is detected, obtain the body velocity through inverse kinematics of the leg and inject the body velocity as a pseudo-velocity observation into the updated error state filter. Step S7: Perform single-frame correction guard verification on the filter output, then complete the position confidence assessment, and output the pose and positioning status.

[0013] As an optimization of this method, the step S4 of identifying the unobservable translation subspace and constructing the corresponding projection operator of the unobservable translation subspace includes: extracting the third-order translation sub-blocks corresponding to the three translation degrees of freedom of the observation information matrix, performing symmetryization and feature decomposition on the third-order translation sub-blocks; using a double threshold criterion of taking the larger of the absolute threshold and the relative threshold to filter small eigenvalues; spanning the unobservable translation subspace by the eigenvectors corresponding to the small eigenvalues, and constructing the projection operator of the unobservable translation subspace; the projection operator of the unobservable translation subspace is used as a shared criterion and is synchronously supplied to the translation increment remapping, velocity constraint, and confidence evaluation stages of step S5.

[0014] As an optimization of this method, step S5, which calls the projection operator of the unobservable translation subspace to perform projection remapping on the translation increment of the filtered output, includes: subtracting the projection operator of the unobservable translation subspace from the identity matrix to obtain the observable subspace projection matrix, and orthogonally projecting the translation increment obtained by the iterative solution onto the observable subspace; when there are no unobservable degrees of freedom, the projection remapping degenerates into an identity transformation, and a regular filtering update is performed.

[0015] As an optimization of this method, after steps S5 and S6 are completed, a subspace-constrained velocity damping step is also performed: when an unobservable translational degree of freedom is detected and the robot is in motion, the projection operator of the unobservable translational subspace output in step S4 is called as the measurement matrix, and soft zero-reaching constraints are applied only to the unobservable subspace, while the velocity components of the observable subspace are not constrained.

[0016] As an optimization of this method, after the geometric true lower limit determination is completed in step S3 and the determination is not a true geometric missing, and before step S4 is executed, a gravity attitude pseudo-observation step is also executed: the mean value of the inertial specific force within the frame is calculated, and when the linear acceleration of the aircraft meets the preset low acceleration condition, attitude constraints are applied to the roll angle and pitch angle based on the approximate gravity reversal of the specific force, without interfering with the yaw angle; if the low acceleration condition is not met, the attitude constraint is skipped.

[0017] As an optimization of this method, the determination of the true lower bound of geometry in step S3 includes: only when the number of downsampled point clouds is lower than the preset number of points floor, it is determined to be a true geometric missing, and the whole frame skip update is performed while maintaining the pose; the degradation case where the number of points is sufficient but the point clouds are coplanar does not trigger the whole frame skip update, and is handed over to the unobservable subspace identification and projection remapping process in subsequent steps S4-S7.

[0018] As an optimization of this method, the single-frame correction amount guard check in step S7 includes: comparing the translation correction amount and rotation correction amount of the pose before and after the filter update; if the translation correction amount or rotation correction amount exceeds the preset upper limit, it is determined to be an ill correction, the filter state is rolled back to the inertial prediction value output in step S1, and the update of this frame is marked as skipped.

[0019] As an optimization of this method, the location reliability assessment in step S7 includes: calculating the location reliability by combining the effective matching ratio score, the point-to-surface distance quality score, and the planar diversity score using a geometric weighting method; the planar diversity score characterizes the degree of coplanar degradation of the point cloud; and the location status is switched between normal and lost based on the location reliability.

[0020] Another aspect of the present invention provides a positioning system for a legged humanoid robot in a degenerative scenario, for implementing the positioning method described in any of the above claims, including a hardware unit and a software functional module deployed within the hardware unit; The hardware unit includes: a lidar, an inertial measurement unit, a leg joint position and velocity sensor, a foot contact state detection component, and an embedded computing unit; The lidar is used to collect environmental point clouds; the inertial measurement unit is used to collect triaxial angular velocity and triaxial specific force; the leg joint position and velocity sensor is used to read the angle and angular velocity of each joint of the supporting leg; the foot contact state detection component is used to determine whether each foot is in the support phase; the inertial measurement unit can be installed in the hip or head of the aircraft; when the inertial measurement unit is installed in the head of the aircraft, the inertial measurement data needs to be transformed to the aircraft base coordinate system by external parameter rotation before participating in the calculation; The software functional modules include: The inertial forward prediction module is used to advance the nominal state and covariance of the filter based on inertial measurement data and output the continuous pose within the frame. The point cloud distortion correction and downsampling module is used to perform point-by-point motion compensation and voxel downsampling of the laser point cloud based on the continuous pose within the frame. The point-surface residual and information matrix construction module is used to construct point-surface residuals and obtain the observation information matrix and information vector by weighted summation; The translation observability analysis module is used to perform feature decomposition on the translation sub-blocks of the observation information matrix, identify the unobservable translation subspace, and construct the projection operator of the unobservable translation subspace. The demapping module is used to call the projection operator of the unobservable translation subspace to perform projection remapping on the translation increment and remove the translation increment components of the unobservable subspace. The foot-based odometer calculation module is used to inversely calculate the body speed based on the foot contact state and leg sensor data; A support phase velocity pseudo-observation module is used to inject the body velocity obtained by inverse solution as a velocity pseudo-observation into the error state filter; Gravity attitude pseudo-observation module, used to apply attitude constraints to roll and pitch angles under low linear acceleration conditions; Subspace-constrained velocity damping module for applying soft zero-reaching constraints only in unobservable subspaces; The hierarchical gating and degradation arbitration module is used to distinguish between degenerate scenarios where there is truly no usable geometry and degenerate scenarios where there are sufficient points but no direction, and to perform hierarchical judgment. Error state filter is used to fuse various observations to complete pose state iterative update; The single-frame correction guard module is used to identify ill corrections and roll back the filter state when necessary. The location reliability assessment module is used to calculate location reliability by comprehensively considering multiple dimensions of indicators, thereby driving the location state migration. The pose output and status reporting module is used to output the pose estimation results and the current positioning status.

[0021] The modules in this system operate according to the following data flow and calling relationships: The inertial forward prediction module outputs continuous intra-frame pose to the point cloud distortion correction and downsampling module, and simultaneously outputs state data to the gravity attitude pseudo-observation module and the error state filter; the point cloud distortion correction and downsampling module outputs point cloud data to the point-surface residual and information matrix construction module; the point-surface residual and information matrix construction module outputs data to the translation observability analysis module and the hierarchical gating and downsampling arbitration module, respectively; the projection operator of the non-observable translation subspace output by the translation observability analysis module is shared and distributed to the de-mapping module and the subspace. The constrained velocity damping module and the fixed position reliability assessment module; the body velocity calculated by the foot odometry calculation module is sent to the support phase velocity pseudo-observation module; the support phase velocity pseudo-observation module, the gravity attitude pseudo-observation module, the subspace constrained velocity damping module, and the deremapping module respectively inject the measured or processed state increment into the error state filter; the output result of the error state filter passes through the single frame correction amount guard module and the fixed position reliability assessment module in sequence, and then the pose output and status report module outputs the pose and positioning status to the outside.

[0022] This invention provides a method and system for localization of a legged humanoid robot in degenerative scenarios. It constructs an unobservable translation subspace projection operator by performing feature decomposition on the translation sub-blocks of the laser observation information matrix, upgrading the degradation determination from scalar to subspace. Through internal filtering iteration and incremental translation projection remapping, it achieves differentiated handling of observable and unobservable directions within the same frame of observation. By determining the support phase through foot contact and combining it with inverse kinematics of the legs to generate pseudo-observations of the body velocity, it achieves continuous bottom-line constraint on unobservable directions within the gait cycle. This enables continuous pose output in degenerative scenarios, eliminates localization jumps, suppresses drift divergence, and simultaneously maintains computational efficiency for embedded robot operation. Compared to existing technologies, it offers the following technical advantages: First, the positioning output is continuous during degradation, without the back-and-forth jumps caused by pose maintenance. Since degradation frames with sufficient points are no longer discarded as a whole, the positioning results will not fluctuate between pose maintenance and real-time pose, and the downstream navigation and control modules will no longer receive step-like pose inputs.

[0023] Second, the drift in the unobservable direction changes from divergent to bounded. After removing the error correction in this direction and imposing constraints at the velocity level, this direction no longer degenerates into a pure inertial quadratic integral. The drift amount changes from a quadratic relationship with the increase of the degradation duration to an approximately linear relationship, and is limited by the damping strength.

[0024] Third, the location jump at the moment of degradation removal is eliminated. Since all priors are injected in the form of standard measurement updates, the covariance remains consistent during degradation, and there is no mechanism for a one-time release after error accumulation.

[0025] Fourth, the fallback constraint covers the entire walking process of the robot. Compared to the zero-speed assumption of the whole machine, which is only applicable when the robot is stationary, the support phase constraint of this invention is continuously valid throughout the gait cycle and is also protected when passing through the degradation region during movement.

[0026] Fifth, the velocity constraints do not conflict with the robot's actual motion. Since the velocity damping measurement matrix is ​​the projection operator of the unobservable translation subspace, its effect is strictly limited to the unobservable direction, and the actual velocity in the observable direction is not suppressed in any way, thus avoiding the systematic deviations introduced by uniform velocity constraint methods under variable speed conditions; at the same time, this invention does not use non-holonomic constraints, and is compatible with the actual motion capability of the legged platform, which has lateral movement capability.

[0027] Sixth, the positioning quality indicators remain sensitive even in degraded scenarios with a sufficient number of points. The confidence index, with planar diversity as the dominant weight, can promptly reflect the decline in positioning quality when facing a large wall area, avoiding the failure of point-based criteria in this situation.

[0028] Seventh, it has low computational overhead and is suitable for embedded deployment on the robot body. The core operation of this invention is the eigenvalue decomposition of a third-order symmetric matrix and several three-dimensional measurement updates. Compared with the scheme using back-end nonlinear optimization, the computational cost is several orders of magnitude lower, and it can run in real time on the robot body with limited computing power.

[0029] Eighth, the treatment intensity adaptively adjusts according to the degree of degradation, eliminating the need for manual mode switching. Furthermore, this invention is compatible with both head and crotch mounting positions of the inertial measurement unit, allowing a single algorithm to adapt to different overall mechanical layouts. Attached Figure Description

[0030] Figure 1 This is a logical block diagram of the positioning system of a legged humanoid robot in a degenerative scenario according to the present invention; Figure 2 This is a flowchart of the preprocessing, geometric true lower bound determination, and two types of motion prior injection in the localization method of a legged humanoid robot in a degenerate scene according to the present invention. Figure 3 This is a flowchart illustrating the observability analysis, deduplication mapping, velocity damping, and correction guarding in the localization method of a legged humanoid robot in a degenerate scenario according to the present invention. Figure 4 This is a data flow diagram of the positioning system in the positioning method of a legged humanoid robot in a degenerate scenario according to the present invention; Figure 5 This is a schematic diagram illustrating the composition of the non-observable translational subspace under three types of deterioration conditions in the positioning method of a legged humanoid robot in a deterioration scenario according to the present invention; Figure 6This is a table of key parameter values ​​in the positioning method of a legged humanoid robot in a degraded scenario according to the present invention. The values ​​are the preferred values ​​in this embodiment, and can be adjusted according to the number of lidar lines, the weight of the whole machine and the characteristics of the working environment. Detailed Implementation

[0031] The present invention will now be described in further detail with reference to the accompanying drawings and embodiments. The described embodiments are merely some, not all, of the embodiments of the present invention. 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.

[0032] like Figures 1-3 As shown, this solution provides a localization method and system for a legged humanoid robot in degenerate scenarios. It improves the degradation handling from a "binary switch at the whole frame level" to "continuous modulation at the direction level," and replaces the general motion assumptions that fail on the platform with the foot contact and leg kinematics information inherent to the legged platform. This invention consists of three mutually coupled essential technical features: First, it performs feature decomposition on the translation sub-blocks of the laser observation information matrix, explicitly constructing a projection operator for the unobservable translation subspace, thus upgrading the degradation determination product from a scalar to a subspace; second, it performs projection remapping on the translation increment within the iterative filtering solution, stripping away the laser correction amount in the unobservable direction, allowing for differentiated handling of observable and unobservable directions within the same frame observation; third, it uses foot contact to determine the support phase, and the body velocity obtained through inverse leg kinematics is used as a pseudo-observation injection filter, providing a continuously usable catch-all constraint for unobservable directions within the gait cycle. These three elements form a closed loop of "measured direction—error correction shielding—prior information supplementation."

[0033] The hardware for implementing the positioning method of this invention includes: (1) One lidar is installed on the robot body to collect environmental point clouds; (2) At least one inertial measurement unit, which can be installed in the hip or head of the aircraft, is used to collect triaxial angular velocity and triaxial specific force; (3) Leg joint position and velocity sensor, used to read the angle and angular velocity of each joint of the supporting leg; (4) Foot contact state detection component, which may be any one of foot pressure sensor, foot end torque sensor or ground contact switch, used to determine whether each foot is in the support phase; (5) An embedded computing unit for running the positioning program of the present invention.

[0034] The installation extrinsic parameters between the lidar and the inertial measurement unit (IMU), and between the IMU and the fuselage base, need to be pre-calibrated. When the IMU is installed at the head of the fuselage, the inertial measurement data it collects must first be transformed to the fuselage base coordinate system using extrinsic parameters before being used in calculations; when installed at the hip of the fuselage, it can be used directly. The system distinguishes between the two installation methods by configuration items and automatically switches the corresponding extrinsic parameter transformation path, allowing the same method to adapt to different overall structural layouts.

[0035] The positioning system of this invention, such as Figure 1 As shown, the system includes multiple functional modules: inertial forward prediction module, point cloud distortion correction and downsampling module, point-surface residual and information matrix construction module, translation observability analysis module, de-mapping module, foot odometry calculation module, support phase velocity pseudo-observation module, gravity attitude pseudo-observation module, subspace-constrained velocity damping module, hierarchical gating and downgrade arbitration module, error state filter, single-frame correction guard module, position confidence evaluation module, and pose output and status reporting module.

[0036] The functions of each module are as follows: The point cloud distortion correction and downsampling module eliminates point cloud distortion caused by the machine's motion within the laser frame and controls the number of points involved in the calculation. Based on the continuous pose within the frame provided by the inertial forward prediction module, this module performs point-by-point motion compensation on each point within the frame according to its acquisition time, followed by voxel downsampling. Its output is fed into the point-surface residual and information matrix construction module.

[0037] The inertial forward prediction module uses inertial measurement data to advance the nominal state and covariance of the filter, and provides intra-frame continuous pose for point cloud distortion correction. Considering that foot impacts from a footed platform gait can introduce high-frequency vibrations into the inertial measurement data, this module applies a sliding window smoothing technique to the inertial samples before integration to avoid impact spikes contaminating the propagation of the state. Its output is simultaneously fed into the point cloud distortion correction and downsampling module, the gravity attitude pseudo-observation module, and the error state filter.

[0038] The foot-based odometer calculation module utilizes the unique kinematic information of the foot-based platform to calculate the body velocity, providing an independent velocity information source for directions unobservable by laser. This module reads the foot's contact state to determine the foot currently in the support phase, then reads the joint angle and angular velocity of the supporting leg. Based on the condition that the velocity of the supporting foot's contact point relative to the ground is zero, the body velocity is derived through inverse kinematics of the leg. This module is a key component distinguishing this invention from wheeled platform and general-purpose platform solutions; its output is sent to the support phase velocity pseudo-observation module.

[0039] The point-area residual and information matrix construction module constructs the laser observation equations and accumulates them to obtain the observation information matrix for the current frame. This module transforms the downsampled point cloud to a map coordinate system, retrieves nearest neighbor points and fits a local plane, constructs the point-to-area distance residuals and corresponding Jacobian values, and weights and accumulates them to obtain the information matrix and information vector. A robust kernel is introduced in the weighting process to reduce the weight of large residual points, suppressing the contamination of observations by dynamic occlusions and mismatches. Its output is simultaneously fed into the translation observability analysis module and the hierarchical gating and degradation arbitration module.

[0040] The translation observability analysis module is responsible for quantitatively and directionally identifying unobservable translation directions from laser observations themselves, serving as the central criterion for the entire scheme. This module takes a third-order sub-block corresponding to the translational degrees of freedom from the information matrix, performs symmetry transformation, and then performs eigenvalue decomposition. It uses a dual-threshold criterion—selecting the larger of the relative and absolute thresholds—to filter out small eigenvalues. The corresponding eigenvectors span an unobservable translation subspace, and a projection operator for this subspace is constructed. Simultaneously, the module outputs the number of unobservable degrees of freedom and the extreme values ​​of the eigenvalues. Its output is shared by the demapping module, the subspace-constrained velocity damping module, and the positional reliability evaluation module.

[0041] The demapping module's function is to shield error corrections only in unobservable directions without discarding the entire frame of observations. Within each iteration of the filter's solution, this module strips the calculated translation increments along the unobservable subspace components, retaining only the observable subspace components for injection into the nominal state. This ensures that in the same frame of observations, observable directions are still constrained by the laser, while unobservable directions are not affected by observation noise.

[0042] The support phase velocity pseudo-observation module provides continuously usable velocity constraints for unobservable directions within the gait cycle. Upon detecting a support phase, this module injects the body velocity output from the foot odometry module as a velocity measurement into the error state filter. Compared to zero-velocity correction based on the assumption of the entire robot being stationary, the constraints of this module are periodically and continuously valid during the robot's movement.

[0043] The gravity attitude pseudo-observation module prevents attitude errors during degradation from being coupled into the translation channel via gravity projection. During periods of low body linear acceleration, this module applies absolute constraints to roll and pitch angles based on the approximate direction of the specific force in the opposite direction of gravity, without interfering with yaw angles.

[0044] The subspace-constrained velocity damping module restricts velocity divergence in unobservable directions during degenerate robot motion, without interfering with the actual motion in observable directions. This module directly uses the projection operator of the unobservable translational subspace output by the translation observability analysis module as the measurement matrix to apply a soft zero-reaching constraint to the velocity, thus the gain of this constraint on the velocity components within the observable subspace is zero.

[0045] The hierarchical gating and degradation arbitration module distinguishes between two different operating conditions: "no usable geometry" and "sufficient points but lacking orientation," and applies different actions accordingly. This module determines observation availability step-by-step based on the original number of points, the number of downsampled points, the number of effective matched points, and the matching ratio. Only when the number of points falls below the true lower limit of geometry does it skip updating the entire frame and maintain the pose. For degraded frames with sufficient points, no pose-maintaining action is taken; these are handled by the translation observability analysis module and the pseudo-observation modules. This module and the translation observability analysis module form a complementary two-tiered defense. The lower limit of the original number of points can be preset based on the number of lines and the effective scanning field of view of the lidar.

[0046] The single-frame correction guard module intercepts ill-conditioned corrections made during the moment of orientation recovery after a long period of degradation. This module compares the pose corrections before and after the filter update. If the single-frame translation or rotation correction exceeds a preset upper limit, the correction is deemed unreliable, the state is rolled back to the predicted value, and the update for this frame is marked as skipped.

[0047] The location reliability assessment module accurately reports the current location quality and drives the location state machine. This module calculates confidence based on three indicators: effective match ratio, point-to-surface distance quality, and planar diversity. It then combines these with translation observability analysis results to drive the location state's transition between normal and lost states. Planar diversity is defined as one minus the proportion of dominant planar points, specifically characterizing the degenerate feature of "sufficient points but highly coplanar."

[0048] The pose output and status reporting module is responsible for outputting the pose estimation results and the current positioning status to the downstream navigation and control module.

[0049] The complete process of the positioning method of this invention is as follows: Figure 2 , Figure 3 As shown, the two are represented by "turn". Figure 3 "and "connect" Figure 2 The connecting frames are linked end to end, together forming a complete process.

[0050] Figure 2 This is a flowchart of the preprocessing, geometric true lower bound determination, and injection of two types of motion priors in the localization method. Steps A to C complete the preprocessing of inertial forward prediction and observation data; Step D is the geometric true lower bound determination, which skips the entire frame update only when there is truly no usable geometry, avoiding misjudging degraded frames with sufficient points as occlusions; Steps F to L inject two types of motion priors, namely support phase velocity and gravity attitude, before the laser update, so that the prior information is included in the covariance first, rather than being remedied after the laser update. The two skip branches correspond to the cases of the airborne phase and the case of large linear acceleration, respectively, and neither interrupts the main process.

[0051] Figure 3This is a flowchart of the observability analysis, de-mapping, velocity damping, and correction guarding in the localization method. Steps O to T are the core processes of this invention, which complete the eigenvalue decomposition of the translation sub-block of the information matrix, the construction of the unobservable subspace, and the de-mapping. When the unobservable degrees of freedom are zero, the branch is directly merged into the guarding decision. At this time, the remapping formula automatically degenerates into an identity transformation, which is completely consistent with the conventional update. Steps U to W apply velocity damping with a limited subspace when the robot is degenerate and moving. Steps X to Y intercept the ill-conditioned single-frame correction at the moment of orientation recovery after a long period of degeneration. Step Z completes the confidence assessment, localization state update, and pose output.

[0052] The data stream of this invention is as follows Figure 4 As shown, three data paths exist in parallel within the system: the laser path generates an observation information matrix and information vector from the laser point cloud through point-by-point motion compensation, nearest-neighbor plane fitting, and robust weighted accumulation; the inertial path generates nominal state and covariance from specific force and angular velocity through sliding window smoothing and integration, and further constructs gravity attitude measurements; the foot path generates pseudo-observations of body velocity from joint angles, angular velocities, and foot contact forces through support phase determination and inverse kinematics of the legs. The three paths ultimately converge at the measurement update point of the error state filter. The nominal state generated by the inertial path feeds back to the point-by-point motion compensation of the laser path, meaning that the continuous pose within a frame comes from inertial prediction rather than the pose of the previous frame; the projection operator of the unobservable translation subspace obtained by the eigenvalue decomposition of the translation subblock of the information matrix is ​​the only shared criterion quantity in the entire system, and is simultaneously distributed to three consumers: translation increment remapping, subspace velocity damping measurement, and plane diversity confidence, ensuring criterion self-consistency.

[0053] As a specific embodiment of this solution, in this embodiment, a legged humanoid robot performs inspection tasks in an indoor environment. The robot body is equipped with one multi-line LiDAR, one inertial measurement unit, position and velocity sensors on each joint of both legs, pressure sensors on the soles of the feet to determine the contact state, and an embedded computing unit to run the positioning program. The robot operates in pure positioning mode within a pre-established point cloud map, without updating the map during operation.

[0054] The work environment includes three types of geometrically degraded areas: Scene X is a large area with a flat wall directly in front, Scene Y is a narrow passage, and Scene Z is a spacious open area.

[0055] There are two options for the installation location of the inertial measurement unit (IMU): Option 1 is to install it at the head of the fuselage, and Option 2 is to install it at the hip of the fuselage. This embodiment adopts Option 1. The inertial measurement data is first transformed to the fuselage base coordinate system by the calibrated extrinsic parameters before participating in subsequent calculations.

[0056] like Figure 1As shown, the system hardware collects data and sends it to the inertial forward prediction module, the point cloud distortion correction and downsampling module, and the foot odometry calculation module, respectively. After each module completes its logical calculations, the data converges to the error state filter, then passes through the single-frame correction guard module and the positional reliability evaluation module, and finally the pose output and status reporting module outputs the results. The projection operator of the non-observable translation subspace output by the translation observability analysis module is shared with the demapping module, the subspace-constrained velocity damping module, and the positional reliability evaluation module.

[0057] like Figure 2-6 As shown, the positioning method in this embodiment is performed according to the following steps: Step 1: Inertial Forward Prediction The system receives the triaxial angular velocity and triaxial specific force output from the inertial measurement unit. To suppress high-frequency vibrations introduced by the impact, the four most recent inertial samples are averaged using a sliding window. The averaged data is then used to advance the aircraft's attitude, position, and velocity, while simultaneously advancing the covariance matrix according to the error state equation. If the time interval between two adjacent inertial data points is negative or greater than 0.2 seconds, or if non-finite values ​​appear in the data, the current prediction is skipped, and the previous state is maintained to prevent amplification of abnormal data through double integration. (The corresponding example is...) Figure 2 Steps A and B.

[0058] Step 2: Point Cloud Distortion Reduction and Downsampling Upon receiving the laser frame, based on the continuous pose obtained in step one, point-by-point motion compensation is performed on each point within the frame according to its acquisition time, unifying all points to the body coordinate system at the end of the frame; subsequently, voxel downsampling is performed. In this embodiment, the voxel side length is preferably 0.25 meters. This value can be adjusted according to the number of lidar lines and is not intended to limit the scope of protection of this invention. (Corresponding to...) Figure 2 Step C.

[0059] Step 3: Geometric True Lower Bound Determination The number of points after downsampling is counted. If the number of points is lower than the true geometric lower bound, it is determined that there is indeed no usable geometry, the entire frame is skipped from observation update, the pose is kept in the most recent reliable state and the velocity is set to zero, and then proceed to step ten. The true geometric lower bound is the larger of the original point count lower bound and the absolute floor of the downsampling point count. In this embodiment, the absolute floor of the downsampling point count is preferably 20 points, and this value can be adjusted according to the number of lidar lines. It should be noted that when facing a large area wall, although the point cloud only forms a plane, the number of points is usually more than several hundred, far exceeding the lower bound, and therefore will not be judged as having no geometry and thus dropped. This is the key difference between this invention and the point-count-based frame dropping scheme. Corresponding to... Figure 2 Steps D and E.

[0060] Step 4: Support Phase Determination and Foot Odometer Calculation The normal contact force of each foot pressure sensor is read. The support phase determination force threshold is set proportionally based on the robot's overall weight and the noise level of the foot sensors, used to distinguish between foot contact with the ground and hovering. When the normal contact force of a foot is greater than the support phase determination force threshold, the foot is determined to be in the support phase. For a foot in the support phase, the angles and angular velocities of each joint of the leg are read, and the body velocity is inversely solved according to the following formula (Formula 5, inverse kinematic solution of the support camera body velocity): , In the formula, R represents the body velocity obtained by inverse kinematics from the foot odometer. It is a vector in the world coordinate system and the unit is meters per second. R represents the body attitude rotation matrix, which is the rotation transformation from the body coordinate system to the world coordinate system. This represents the angular velocity vector in the body coordinate system, with units of radians per second. This represents the position vector of the contact point of the supporting foot in the body coordinate system, in meters, and is calculated from the positive kinematics of the supporting leg based on the joint angles. This represents the Jacobian matrix of the support foot contact point for each joint of the leg, in meters per radian. This represents the angular velocity vector of each joint of the supporting leg, in radians per second; the symbol × indicates the vector cross product. This formula is based on the assumption that the macroscopic velocity of the contact point of the supporting foot is zero in the world coordinate system; the traditional zero-speed correction when the entire machine is stationary is a special case of this formula.

[0061] In this embodiment, the preferred standard deviation of the pseudo-observation noise for the support phase velocity is 0.015 m / s, which can be adjusted within the range of 0.010-0.050 m / s according to actual operating conditions. The body velocity obtained from the inverse kinematics is used as this noise standard deviation as the velocity measurement injection error state filter. If there are currently no feet in the support phase (levitation phase), this step is skipped. (Corresponding to...) Figure 2 Steps F, G, H, I.

[0062] Step 5: Gravity Attitude Constraints Calculate the mean value of the force measurement within this frame. If the absolute value of the difference between the magnitude of this mean and the local gravitational acceleration does not exceed a preset threshold, the aircraft is determined to be in a low linear acceleration state. At this time, the force direction approximately points in the opposite direction of gravity. Based on this, absolute constraints are applied to the roll and pitch angles, while the yaw angle is unaffected. In this embodiment, the standard deviation of the gravity direction residual noise is preferably 0.10 radians, with an adjustable range of 0.05-0.20 radians. If the threshold is exceeded, this step is skipped. (Corresponding to...) Figure 2 Steps J, K, L, then turn Figure 3 .

[0063] Step Six: Construction of Point-to-Surface Residuals and Information Matrix The downsampled point cloud is transformed to the map coordinate system according to the current pose estimation. For each point, its nearest neighbors are searched and a local plane is fitted to construct the point-to-plane distance residual. To suppress the influence of dynamic occlusions and mismatches, points whose absolute residual value exceeds the robust kernel threshold are inversely weighted. This embodiment can use the Huber robust kernel, or other types of robust kernels such as Cauchy can also be selected. The Jacobian of each point is accumulated according to the weights to obtain the observation information matrix and information vector of this frame. Figure 3 Step O.

[0064] Step 7: Translation Observability Analysis Take the third-order sub-block corresponding to the three translational degrees of freedom in the observation information matrix, denoted as... After symmetrization, the eigenvalues ​​are decomposed according to the following formula (Equation 1, eigenvalue decomposition of the translation sub-block of the laser information matrix): , In the formula, This represents a third-order symmetric positive semi-stator block in the laser observation information matrix corresponding to three translational degrees of freedom; k represents the index of the translational characteristic direction, with values ​​of 1, 2, and 3; The kth eigenvalue is arranged in ascending order. Its physical meaning is the amount of information provided by laser observation in the corresponding direction. The smaller the value, the less geometric constraint there is in that direction. express The corresponding unit eigenvector is the direction vector in the world coordinate system; the superscript ⊤ indicates matrix transpose.

[0065] Next, calculate the unobservability decision threshold according to the following formula (Formula 2, the double threshold for unobservability decision). : , In the formula, This represents the threshold for determining non-observability. This represents the preset absolute threshold floor, which serves to provide an absolute reference for judgment when the information content of the entire frame is generally low, and is adjusted synchronously with the residual weighting level. This represents the relative threshold coefficient, which is dimensionless. In this embodiment, it is preferably set to 0.05, with an adjustable range of 0.02 to 0.10, and the value is between 0 and 1. This represents the largest of the three feature values. The larger of the relative and absolute thresholds is used instead of the smaller one because: when using only the relative threshold, if the overall information content of the frame is generally low, the largest feature value itself is very small, and the relative criterion will fail; when using only the absolute threshold, it will miss detections in dense scenes where information content is generally abundant. Using the larger of the two can cover both sparse and dense scenes simultaneously.

[0066] Then, according to the following formula (Equation 3, projection operator of the unobservable translation subspace), the eigenvectors corresponding to all eigenvalues ​​less than the threshold are used to span the unobservable translation subspace, constructing the projection operator U of the unobservable translation subspace, and recording the number of unobservable degrees of freedom d: , In the formula, Let be a projection operator for an unobservable translation subspace, and be a third-order idempotent symmetric matrix; the summation domain is all subspaces satisfying . The feature direction index k. The number of indices that satisfy this condition is denoted as k. This refers to the number of unobservable translational degrees of freedom, which is an integer ranging from 0 to 3. When the robot is facing a single large-area plane... Typically, a value of 2 is used when the passage is narrow. Typically, the value is 1; when geometric features are abundant... Set U to 0 and U is a zero matrix. Corresponding to Figure 3 Steps P, Q, S.

[0067] Step 8: Iterative Update and Remapping In each iteration of the error state filter, the state increment for this round is first calculated using the conventional method. If... If it is greater than zero, then according to the following formula (Formula 4, Projection Remapping of Translation Increment), the translation increment is stripped along the components of the unobservable subspace, and only the observable subspace components are retained. , In the formula, U represents the translation increment obtained from this iteration, in meters; I represents the third-order identity matrix; the arrow indicates that the result on the right replaces the variable on the left. Since U is idempotent and symmetric... Also known as the projection operator, the geometric meaning of this formula is to orthogonally project the translation increment onto the observable subspace, so that the correction amount generated by observation noise and mismatch in the unobservable direction is completely stripped away, while the correction amount in the observable direction is retained as is.

[0068] like If U equals zero, then U is a zero matrix, and equation (4) automatically degenerates into an identity transformation, i.e., it is executed according to the normal update. The processed increment is injected into the nominal state, and the covariance matrix is ​​updated. The upper limit of the number of iterations is 5, and the process exits early when the infinite norm of the increment is less than the convergence threshold. Figure 3 Steps R and T.

[0069] Step Nine: Subspace-Confined Velocity Damping and Single-Frame Correction Guarding like If the value is greater than zero and the robot is currently in motion, then a soft zero-reaching constraint is applied to the velocity in the unobservable subspace according to the following formula (Equation 6, Velocity Damping in the Unobservable Subspace): , In the formula, The measurement residual of velocity damping is expressed in meters per second; v represents the current estimated body velocity of the filter, which is a vector in the world coordinate system and is expressed in meters per second; the meaning of U is the same as in equation (3). The measurement matrix of this measurement is directly taken as U, so its gain on the velocity component in the observable subspace is zero, that is, the constraint only works in the direction that the laser cannot observe, and will not conflict with the robot's actual motion in the observable direction; in this embodiment, the subspace velocity damping noise standard deviation is preferably 0.15 meters per second, with an adjustable range of 0.10-0.30 meters per second, and a larger noise standard deviation is taken to reflect its soft constraint nature.

[0070] Then, the pose correction amounts before and after the filter update for this frame are compared. In this embodiment, the upper limit for single-frame translation correction is preferably 0.5 meters, and the upper limit for single-frame rotation correction is preferably 0.42 radians. These can be adjusted based on the robot's maximum speed and angular velocity. If the single-frame translation correction exceeds the corresponding upper limit or the single-frame rotation correction exceeds the corresponding upper limit, the correction is determined to be ill-conditioned, the state is rolled back to the predicted value, and the update for this frame is marked as skipped. Figure 3 Steps U, V, W, X, Y.

[0071] Step 10: Confidence Assessment and Status Reporting Calculate the location reliability of this frame according to the following formula (Formula 7, Location Reliability): , In the formula, This represents location reliability, is dimensionless, and ranges from 0 to 1. The effective match ratio score is defined as the ratio of the number of points that successfully participated in residual construction to the total number of downsampled points. The quality score represents the distance between a point and a surface, calculated by combining the mean and maximum distances from the point to the surface; the smaller the distance, the higher the score. The plane diversity score is defined as 1 minus the proportion of points contained in the dominant plane to the total number of points. The lower the value, the more concentrated the point cloud is on a single plane. , , The values ​​represent the weights corresponding to the three scores mentioned above; ln represents the natural logarithm, and exp represents the natural exponent. In this embodiment, the preferred weight values ​​are: effective match ratio weight 0.30, point-to-surface distance weight 0.25, and planar diversity weight 0.45; the weights can be adjusted within their respective adjustable ranges. Geometric weighting is used instead of arithmetic weighting because a significant deterioration in any of the three indicators should lead to a significant decrease in the overall confidence score. Geometric weighting possesses this characteristic of being dominated by the weakest link, while arithmetic weighting would allow the deteriorating items to be masked by the other high-scoring items. Among these, the planar diversity score directly characterizes the degenerate feature of "sufficient points but highly coplanar," therefore it has the highest weight among the three.

[0072] If the confidence level is below the loss threshold for 10 consecutive frames (in this embodiment, the preferred location confidence loss threshold is 0.25, with an adjustable range of 0.20-0.30), the location status is set to lost and reported externally. Once the confidence level rises above the loss threshold, the location is restored to normal. Finally, the pose estimation result and the current location status are output. Figure 3 Step Z.

[0073] like Figure 4 The diagram shown is the data flow diagram of this invention. Parallelograms represent data items, and rectangles represent data transformations. The data from the laser path, inertial path, and foot path are ultimately merged into the error state filter measurement update. The projection operator of the non-observable translation subspace is distributed as shared data to multiple modules to ensure self-consistency of the criteria.

[0074] like Figure 5 The diagram shows the composition of the non-observable translational subspace under three types of degradation conditions. The treatment intensity of this invention is automatically adjusted according to the degree of degradation without the need for manual mode switching.

[0075] Condition 1: Scene Z - Open Area. The robot moves along the inspection path. The laser point cloud covers multiple planes with different orientations. All three eigenvalues ​​of the translation sub-block of the information matrix are higher than the threshold. ,determination .at this time Since it is a zero matrix, equation (4) degenerates into an identity transformation, and the system runs according to the regular iterative update; the support phase velocity pseudo-observation participates in the update as an auxiliary constraint, further suppressing the slow accumulation of inertial zero bias.

[0076] Scenario 2: Scene X faces a large wall. When the robot is facing the wall, the effective points are coplanar in height. The feature decomposition results show that two smaller feature values ​​are below the threshold, so a decision is made... The system constructs a projection operator for a rank-2 unobservable translation subspace. According to Equation (4), the laser correction amounts in the two translational directions within the wall are stripped away, leaving only the correction along the wall normal; at the same time, the foot odometer continuously provides pseudo-observations of the body velocity according to Equation (5), and the velocity damping takes effect in this two-dimensional subspace according to Equation (6). Since there are enough points, the geometric true lower limit determination will not be triggered, the system will not enter the pose holding state, and the positioning output remains continuous.

[0077] Scenario 3: Scenario Y, a narrow passage. Observable in two directions perpendicular to the passage's direction, but not observable along the passage's direction. Typical judgment is as follows: The processing procedure is the same as in case 2, except that the rank of the projection operator changes from 2 to 1.

[0078] Condition 4: Degradation Removal. After the robot turns and the wall disappears from view, the previously small eigenvalues ​​increase with the addition of geometric constraints. Upon zeroing, demapping and velocity damping automatically fail, and the system resumes regular updates. Because the pose is always anchored by the pseudo-observations of the support phase velocity during degradation, and all priors are injected in the form of standard measurement updates with consistent covariance, there will be no one-time release of accumulated errors during orientation recovery, and the single-frame correction guard will not be triggered.

[0079] The above description is merely illustrative of the embodiments of the present invention and is not intended to limit the present invention. For those skilled in the art, any modifications, equivalent substitutions, improvements, etc., made within the spirit and principles of the present invention should be included within the protection scope of the present invention.

Claims

1. A method for positioning a legged humanoid robot in a degenerate environment, characterized in that, Includes the following steps: Step S1: Acquire the inertial measurement data output by the inertial measurement unit, perform inertial forward prediction, and output the nominal state, covariance, and intra-frame continuous pose for point cloud distortion correction. Step S2: Obtain the laser point cloud collected by the lidar, and use the intra-frame continuous pose to perform distortion correction and downsampling processing on the laser point cloud; Step S3: Perform a true lower bound determination of geometry based on the number of downsampled point clouds; if it is determined to be a true geometric missing value, skip the laser observation update for the entire frame; Otherwise, construct the laser point-surface residual and observation information matrix, and output the observation information matrix to the next step; Step S4: Receive the observation information matrix, perform feature decomposition on its translation sub-blocks, identify the unobservable translation subspace, and construct the corresponding projection operator for the unobservable translation subspace; Step S5: Inside the iterative solution of the error state filter, the projection operator of the unobservable translation subspace is called to perform projection remapping on the translation increment of the filter output, stripping the translation increment component of the unobservable subspace, retaining the translation increment component of the observable subspace, and updating the internal state of the filter. Step S6: Acquire foot contact state and leg joint sensing data. When a foot in the support phase is detected, obtain the body velocity through inverse kinematics of the leg and inject the body velocity as a pseudo-velocity observation into the updated error state filter. Step S7: Perform single-frame correction guard verification on the filter output, then complete the position confidence assessment, and output the pose and positioning status.

2. The positioning of a legged humanoid robot in a degenerate scenario according to claim 1, characterized in that, The identification of the unobservable translation subspace and the construction of the corresponding projection operator for the unobservable translation subspace in step S4 includes: extracting the third-order translation sub-blocks corresponding to the three translation degrees of freedom of the observation information matrix, performing symmetryization and feature decomposition on the third-order translation sub-blocks; using a double threshold criterion of taking the larger of the absolute threshold and the relative threshold to filter small eigenvalues; spanning the unobservable translation subspace by the eigenvectors corresponding to the small eigenvalues, and constructing the projection operator for the unobservable translation subspace; the projection operator for the unobservable translation subspace is used as a shared criterion and is synchronously supplied to the translation increment remapping, velocity constraint, and confidence evaluation stages in step S5.

3. The positioning method for a legged humanoid robot in a degenerate environment according to claim 1, characterized in that, Step S5, which involves calling the projection operator of the unobservable translation subspace to perform projection remapping on the translation increment of the filtered output, includes: subtracting the projection operator of the unobservable translation subspace from the identity matrix to obtain the observable subspace projection matrix, and orthogonally projecting the translation increment obtained by the iterative solution onto the observable subspace; when there are no unobservable degrees of freedom, the projection remapping degenerates into an identity transformation, and a regular filtering update is performed.

4. The positioning method for a legged humanoid robot in a degenerate environment according to claim 1, characterized in that, After steps S5 and S6 are completed, a subspace-constrained velocity damping step is also performed: when an unobservable translational degree of freedom is detected and the robot is in motion, the projection operator of the unobservable translational subspace output in step S4 is called as the measurement matrix, and soft zero-reaching constraints are applied only to the unobservable subspace, while the velocity components of the observable subspace are not constrained.

5. The positioning method for a legged humanoid robot in a degenerate environment according to claim 1, characterized in that, After the geometric true lower limit determination is completed in step S3 and the determination is not a true geometric missing, and before step S4 is executed, a gravity attitude pseudo-observation step is also executed: the mean value of the inertial specific force within the frame is calculated, and when the linear acceleration of the aircraft meets the preset low acceleration condition, attitude constraints are applied to the roll angle and pitch angle based on the approximate gravity reversal of the specific force, without interfering with the yaw angle; if the low acceleration condition is not met, the attitude constraint is skipped.

6. The positioning method for a legged humanoid robot in a degenerate scenario according to claim 1, characterized in that, The geometric true lower bound determination in step S3 includes: if the number of downsampled point clouds is lower than the preset point floor, it is determined to be a true geometric missing condition, and the whole frame skip update is performed while maintaining the pose; the degradation condition where the number of points is sufficient but the point clouds are coplanar does not trigger the whole frame skip update, and is handed over to the unobservable subspace identification and projection remapping process in subsequent steps S4-S7.

7. The positioning method for a legged humanoid robot in a degenerate scenario according to claim 1, characterized in that, The single-frame correction amount guard verification in step S7 includes: comparing the translation correction amount and rotation correction amount of the pose before and after the filter update; if the translation correction amount or rotation correction amount exceeds the preset upper limit, it is determined to be an ill correction, the filter state is rolled back to the inertial prediction value output in step S1, and the update of this frame is marked as skipped.

8. The positioning method for a legged humanoid robot in a degenerate scenario according to claim 1, characterized in that, The location reliability assessment in step S7 includes: calculating location reliability by combining the effective matching ratio score, the point-to-surface distance quality score, and the planar diversity score using a geometric weighting method; the planar diversity score characterizes the degree of coplanar degradation of the point cloud; and the location status is switched between normal and lost based on the location reliability.

9. A positioning system for a legged humanoid robot in a degenerate environment, used to implement the positioning method according to any one of claims 1-8, characterized in that, This includes hardware units and software functional modules deployed within the hardware units; The hardware unit includes: a lidar, an inertial measurement unit, a leg joint position and velocity sensor, a foot contact state detection component, and an embedded computing unit; The lidar is used to collect environmental point clouds; the inertial measurement unit is used to collect triaxial angular velocity and triaxial specific force; the leg joint position and velocity sensor is used to read the angle and angular velocity of each joint of the supporting leg; the foot contact state detection component is used to determine whether each foot is in the support phase; the inertial measurement unit can be installed in the hip or head of the aircraft; when the inertial measurement unit is installed in the head of the aircraft, the inertial measurement data needs to be transformed to the aircraft base coordinate system by external parameter rotation before participating in the calculation; The software functional modules include: The inertial forward prediction module is used to advance the nominal state and covariance of the filter based on inertial measurement data and output the continuous pose within the frame. The point cloud distortion correction and downsampling module is used to perform point-by-point motion compensation and voxel downsampling of the laser point cloud based on the continuous pose within the frame. The point-surface residual and information matrix construction module is used to construct point-surface residuals and obtain the observation information matrix and information vector by weighted summation; The translation observability analysis module is used to perform feature decomposition on the translation sub-blocks of the observation information matrix, identify the unobservable translation subspace, and construct the projection operator of the unobservable translation subspace. The demapping module is used to call the projection operator of the unobservable translation subspace to perform projection remapping on the translation increment and remove the translation increment components of the unobservable subspace. The foot-based odometer calculation module is used to inversely calculate the body speed based on the foot contact state and leg sensor data; A support phase velocity pseudo-observation module is used to inject the body velocity obtained by inverse solution as a velocity pseudo-observation into the error state filter; Gravity attitude pseudo-observation module, used to apply attitude constraints to roll and pitch angles under low linear acceleration conditions; Subspace-constrained velocity damping module for applying soft zero-reaching constraints only in unobservable subspaces; The hierarchical gating and degradation arbitration module is used to distinguish between degenerate scenarios where there is truly no usable geometry and degenerate scenarios where there are sufficient points but no direction, and to perform hierarchical judgment. Error state filter is used to fuse various observations to complete pose state iterative update; The single-frame correction guard module is used to identify ill corrections and roll back the filter state when necessary. The location reliability assessment module is used to calculate location reliability by comprehensively considering multiple dimensions of indicators, thereby driving the location status migration. The pose output and status reporting module is used to output the pose estimation results and the current positioning status.

10. A positioning system for a legged humanoid robot in a degenerate environment according to claim 9, characterized in that, The inertial forward prediction module outputs continuous intra-frame pose to the point cloud distortion correction and downsampling module, and simultaneously outputs state data to the gravity attitude pseudo-observation module and the error state filter; the point cloud distortion correction and downsampling module outputs point cloud data to the point-surface residual and information matrix construction module; the point-surface residual and information matrix construction module outputs data to the translation observability analysis module and the hierarchical gating and downsampling arbitration module, respectively; the projection operator of the non-observable translation subspace output by the translation observability analysis module is shared and distributed to the de-mapping module and the subspace. The constrained velocity damping module and the fixed position reliability assessment module; the body velocity calculated by the foot odometry calculation module is sent to the support phase velocity pseudo-observation module; the support phase velocity pseudo-observation module, the gravity attitude pseudo-observation module, the subspace constrained velocity damping module, and the deremapping module respectively inject the measured or processed state increment into the error state filter; the output result of the error state filter passes through the single frame correction amount guard module and the fixed position reliability assessment module in sequence, and then the pose output and status report module outputs the pose and positioning status to the outside.

Images