Whole body obstacle avoidance method and system for a legged humanoid robot

Abstract

The present application relates to the technical field of robot environment perception and motion control, and particularly relates to a full-body obstacle avoidance method and system for a legged humanoid robot, which successively carries out full-body joint and connecting rod geometric calibration, retains parameter residual covariance, propagates and calculates each connecting rod expansion allowance through connecting rod pose Jacobian, constructs a three-level structure of connecting rod collision bodies, completes time alignment of joint state and sliding window occupancy map and broadcasts the pose of each level of collision body, selects a risk sub-chain through hierarchical pruning, greedily merges voxels in a region of interest, executes signed distance query for each connecting rod in combination with unobserved voxel sub-site disposal strategy, obtains distance, nearest point pair and unit direction determined by the line connecting the two nearest points, and then distributes the query results according to kinematic sub-chains to generate two types of obstacle avoidance constraints, which are sent to corresponding execution units after arbitration and failure degradation processing. The robot can walk while the upper limbs autonomously avoid obstacles, and the passing capacity and operation safety in narrow channels are improved.

Description

Technical Field

[0001] This invention relates to the field of robot environmental perception and motion control technology, and in particular to a method and system for full-body obstacle avoidance of a legged humanoid robot. Background Technology

[0002] Currently, several humanoid robot obstacle avoidance and motion calibration technologies have been publicly disclosed in the industry. However, all existing technologies suffer from limitations in scene adaptability and lack of technical dimensions. They struggle to meet the core operational requirements of legged humanoid robots, such as high-degree-of-freedom dynamic shapes, real-time dynamic obstacle avoidance, high-precision safety and fault tolerance, and layered differentiated control. They cannot achieve full-dimensional, high-precision, whole-body obstacle avoidance. Publication numbers CN119088030A, CN118915773A, and CN118836877A are all traditional path planning-based obstacle avoidance technologies with significant limitations in adaptability. CN119088030A, in particular, relies on visual segmentation for path planning, simplifying the robot into a planar moving point or a fixed envelope model throughout the process. It completely ignores the multi-link, three-dimensional structural characteristics of humanoid robots and cannot adapt to obstacle avoidance scenarios where the robot's shape dynamically changes during movement. CN118915773A achieves obstacle crossing and avoidance based on model predictive control, but relies solely on obstacle height as the single trigger condition. It can only achieve fixed crossing actions and does not detect or protect against collision risks during the movement of each link in the robot. Furthermore, it only relies on a 2.5-dimensional terrain map for environmental perception, resulting in a lack of spatial awareness. CN118836877A uses generative adversarial networks for path planning, but uses a single scalar distance of the entire robot as the obstacle avoidance trigger. It cannot accurately locate the positions of risky links, and its effectiveness is highly dependent on the training dataset, exhibiting poor generalization and failing to achieve deep integration with the robot's joint-level fine-grained motion control.

[0003] Patents with publication numbers CN122253272A and CN116985184A focus on the field of robot self-collision detection, but can only detect collisions between links on the robot body and lack dynamic obstacle avoidance capabilities in the external environment. CN122253272A relies on an offline simulation environment to complete the detection work, only outputting Boolean collision judgment results, and cannot provide effective control information such as collision distance and avoidance direction. Moreover, it requires the computing power of a high-performance graphics processor, making it unsuitable for embedded or airborne robot operation scenarios. CN116985184A uses a fixed joint envelope model for self-collision detection, but the model accuracy cannot be adapted to the dynamic movement scenarios of humanoid robots. In addition, it does not build a real-time environmental map, cannot cope with external dynamic obstacles, and does not distinguish between two independent execution channels for body walking and upper limb movement, thus failing to achieve differentiated obstacle avoidance control for different parts of the robot.

[0004] Patents CN122346163A and CN116909284A represent coarse obstacle avoidance technologies at the whole-machine level, lacking refined control capabilities. CN122346163A relies on a priori maps from a Building Information Model (BIM) for obstacle avoidance navigation, heavily depending on preset static maps. This makes it unsuitable for unknown dynamic environments and treats the robot as a single unit for obstacle avoidance judgment, failing to differentiate between the obstacle avoidance needs of different moving parts such as the torso, upper and lower limbs, and head. It can only adjust the overall gait parameters and cannot achieve independent obstacle avoidance movement of the upper limbs. CN116909284A achieves obstacle avoidance control by limiting the speed threshold of the robot's fixed coordinate system. It only adjusts the robot's walking speed based on overall obstacle data, without considering the real-time risk status of individual links. It can only control the robot's movement and cannot independently manage the movement of the upper limb joints, making it difficult to achieve simultaneous robot walking and upper limb obstacle avoidance in operational scenarios.

[0005] Patents with publication numbers CN119871388A, CN118305796A, and CN117718958A represent traditional robot kinematic calibration technologies, which have significant limitations in both application scenarios and technical logic. The core objective of these solutions is solely to improve the positioning accuracy of industrial robot end effectors, relying heavily on external calibration fixtures for offline calibration, making them unsuitable for the real-time calibration needs of humanoid robots. Furthermore, these technologies only perform parameter identification and accuracy correction, directly discarding the parameter residual covariance data generated during the calibration process. They fail to convert uncertainties such as parameter deviations and assembly errors into obstacle avoidance safety margins, thus failing to address the industry pain points of accumulated errors in far-end links and redundant safety margins in near-end links.

[0006] In summary, existing technologies are fragmented and isolated, each with its own shortcomings, and cannot systematically solve the core technical challenges of humanoid robot dynamic shape adaptation, parameter deviation tolerance, real-time detection with low computing power, and dual-channel layered obstacle avoidance. Summary of the Invention

[0007] The purpose of this invention is to overcome the problems of the prior art and provide a whole-body obstacle avoidance method and system for legged humanoid robots. This method solves the dilemma of existing fixed-envelope obstacle avoidance schemes, addresses the problem of whole-body collision determination under the dynamic shape of multi-link humanoid robots, and takes into account calibration uncertainty, hardware computing power constraints and the robot's dual-channel execution architecture to achieve differentiated obstacle avoidance technology for different limbs.

[0008] The above objectives are achieved through the following technical solutions: This invention provides a method for full-body obstacle avoidance of a legged humanoid robot, comprising the following steps: Step S1: Perform geometric calibration of all joints and links, retain the parameter residual covariance obtained from the calibration, and calculate the expansion margin corresponding to each link by propagating the parameter residual covariance through the link pose Jacobian. Step S2: Construct a three-level hierarchical structure of link colliders, align the joint states with the map occupied by the sliding window in terms of joint state time, and broadcast the poses of each level of colliders; Step S3: Perform hierarchical pruning and screening to obtain risk subchains. Greedy merge the voxels occupying the region of interest corresponding to the risk subchains. Combine the unobserved voxels with the part-based handling strategy, perform a signed distance query for each link to obtain the signed distance, nearest point pair, and unit direction determined by the line connecting the two nearest points. Step S4: The query results are split into kinematic subchains to generate body velocity constraints for the leg and torso subchain and upper limb joint obstacle avoidance constraints for the upper limb and head subchain. After arbitration and failure degradation processing, the results are sent to the corresponding execution units.

[0009] As an optimization of this method, the full-body joint and link geometry calibration in step S1 includes: observing the body markers fixed on the robot links using onboard sensing components, collecting multiple sets of body marker observation values ​​and joint readings under different configurations, identifying joint zero-position deviations and link geometric deviations, and outputting the corrected kinematic parameters and the parameter residual covariance; propagating the parameter residual covariance through the link pose Jacobian matrix to obtain the link pose covariance in Cartesian space, calculating the anisotropic expansion margin of the link from the pose covariance, with the expansion margin of the distal link being greater than that of the link near the base, and superimposing the expansion margin along the outer normal of the link convex hull to the corresponding link convex hull.

[0010] As an optimization of this method, the three-level hierarchical structure of the link collision body in step 2 includes: a zero-level whole-machine enveloping sphere, a first-level sub-chain envelope capsule, and a second-level link-by-link convex hull; the zero-level whole-machine enveloping sphere is generated from the vertices of all link convex hulls; the first-level sub-chain envelope capsule is generated by aggregating kinematic sub-chains; the second-level link-by-link convex hull is the link convex hull after superimposing the expansion margin of the corresponding link; adjacent link pairs and normally contacting link pairs are pre-removed to generate a whitelist of detection pairs, and subsequent distance queries skip the detection pairs in the whitelist.

[0011] As an optimization of this method, the hierarchical pruning includes a zero-level coarse screening and a first-level risk subchain screening; The zeroth-level coarse screening specifically involves determining whether the enclosing sphere of the whole machine intersects with the set of occupying voxels; If no match is found, all exact queries will be skipped for this period. If they intersect, perform the first-level risk subchain screening: determine whether the subchain envelope capsule body and the occupying voxel set intersect for each subchain, and mark the intersecting subchain as a risk subchain; if they do not intersect, skip all precise queries in this cycle.

[0012] As an optimization of this method, the unobserved voxel segmentation handling strategy in step S3 includes: unobserved voxels belonging to the leg and torso subchain are directly included as obstacles; unobserved voxels belonging to the upper limb and head subchain are only included as obstacles when the linkage's movement trend is towards the obstacle, and are not included as obstacles when the linkage moves towards the body; the greedy merging is performed by performing stroke encoding greedy merging along the three coordinate axes, merging discrete occupied voxels into cuboid blocks and constructing them as collision objects.

[0013] As an optimization of this method, the signed distance query for each link in step S3 involves constructing the second-level link convex hull after superimposing the expansion margin and the cuboid block as a collision object and performing a signed distance calculation on the convex body. When the links separate, the distance is taken as a positive value, and when they penetrate each other, the distance is taken as a negative value of the penetration depth. The output includes the signed distance, the nearest point pair between the link and the obstacle, and the unit direction determined by the line connecting the two nearest points. The query process skips the detection pairs in the whitelist.

[0014] As an optimization of this method, the generation of the body velocity constraint corresponding to the leg-torso subchain in step S4 includes: obtaining the minimum signed distance of the leg-torso subchain and the direction of the corresponding nearest point connection; decomposing the body velocity command into a normal component and a tangential component along the direction of the nearest point connection, performing attenuation scaling only on the normal component, while keeping the tangential component unchanged; after scaling, performing an executable check on each velocity channel, if the channel velocity amplitude is greater than 0 and less than the minimum executable velocity amplitude of the legged platform, then setting the channel to zero and setting the obstacle avoidance and stop sign.

[0015] As an optimization of this method, the step S4 of generating the upper limb head sub-chain corresponding to the upper limb joint obstacle avoidance constraint includes: using the link pose Jacobian, projecting the direction of the nearest point connection line onto the joint velocity space to construct a linear obstacle avoidance inequality of the joint velocity; taking the expected upper limb joint velocity as the main task, solving the quadratic programming under the zero-space projection framework; when the quadratic programming solution is not feasible, setting all joints of the sub-chain to zero, and driving the upper limb to retreat to the preset convergence configuration.

[0016] Another aspect of the present invention provides a full-body obstacle avoidance system for a legged humanoid robot, used to implement the full-body obstacle avoidance method described in any of the above claims, comprising: The whole-body joint zero position and link geometry calibration module is used to traverse multiple sets of calibration configurations, observe the body markers using airborne sensing components, identify joint zero position deviations and link geometry deviations, and output the corrected kinematic parameters and parameter residual covariance. The calibration residual propagation and link-by-link expansion margin calculation module is used to receive the parameter residual covariance and convert it into a link-by-link expansion margin table through pose Jacobian propagation. The three-level offline construction module for the link collision body is used to receive the link-by-link expansion margin table and construct the three-level hierarchical structure of the link collision body. The self-collision pair and normal contact pair elimination module is used to eliminate adjacent link pairs directly connected to the joint and normal contact link pairs, and outputs a whitelist of detection pairs. The joint state time alignment and forward kinematics calculation module is used to align the joint state to the map timestamp occupied by the sliding window and calculate the whole body link pose based on the corrected kinematic parameters. The whole-body collider coordinate transformation broadcast module is used to attach coordinate systems to each level of collider and broadcast the pose of each level of collider on a cycle-by-cycle basis. The whole machine surrounds the sphere and sub-chain envelope capsule grading and coarse screening module is used to perform zero-level and first-level stratified pruning to screen out risky sub-chains; The sliding window-occupied map building module is used to maintain a 3D sliding window-occupied map centered on the robot with a constant memory footprint. The probabilistic fusion and sensor noise margin dilation module is used to perform probabilistic fusion on voxels and apply only the map dilation amount corresponding to the sensor noise. The unknown voxel segmentation module is used to perform segmentation and discarding of unobserved voxels within the region of interest according to the kinematic subchain. The module for greedy merging of voxels within a region of interest and constructing collision objects is used to perform run-length encoding greedy merging of voxels within the region of interest, generate cuboid blocks, and construct collision objects. The signed distance and nearest point query module for each link is used to perform convex distance calculation for each link in combination with the whitelist of the detection pair, and output the signed distance for each link, the nearest point pair, and the unit direction determined by the line connecting the two nearest points; The distance result splitting module is used to split the distance query results into two groups of data according to the kinematic sub-chain: the leg and trunk group and the upper limb and head group. A body velocity directional scaling module is used to generate body velocity constraints based on the leg and torso group data. The upper limb joint obstacle avoidance inequality construction and null space solution module is used to construct obstacle avoidance inequalities based on the upper limb head group data and solve the quadratic programming by null space projection, and output upper limb joint obstacle avoidance constraints. The whole-body obstacle avoidance arbitration and failure degradation module is used to merge the body speed constraints and the upper limb joint obstacle avoidance constraints, verify the system status timeliness, and perform failure degradation. The status and minimum distance reporting module is used to report obstacle avoidance speed limit status, minimum distance of each sub-chain, trigger links, pruning level and solution status to the outside world.

[0017] The modules in this system operate according to the following data flow and calling relationships: Offline Phase: The residual covariance of the output parameters from the whole-body joint zero-position and link geometry calibration module is sent to the calibration residual propagation and link-by-link expansion margin calculation module; the link-by-link expansion margin table output by the calibration residual propagation and link-by-link expansion margin calculation module is sent to the link collision body three-level offline construction module; the link collision body three-level offline construction module and the self-collision pair and normal contact pair elimination module complete offline calculations and output the three-level collision body configuration and detection pair whitelist for online module calls; During the online phase, joint encoder data is sent to the joint state time alignment and forward kinematics calculation module, and the link pose output by the joint state time alignment and forward kinematics calculation module is sent to the whole body collision coordinate transformation broadcast module; environmental perception and pose estimation data are sent to the sliding window occupancy map construction module, and map data are sequentially sent to the probability fusion and sensor noise margin expansion module and the unknown voxel sub-part processing module. The collider pose information output by the whole-body collider coordinate transformation broadcast module and the voxel processing information output by the unknown voxel sub-part processing module are jointly sent to the whole-machine enclosing sphere and sub-chain envelope capsule grading coarse screening module; the risk sub-chain information output by the coarse screening is sent to the region of interest voxel greedy merging and collision object construction module. The region of interest voxel greedy merging and collision object construction module outputs a cuboid block and constructs it as a collision object. This cuboid block, along with the detection pair whitelist obtained offline, is sent to the link-by-link signed distance and nearest point query module. The distance query result is input into the kinematic subchain splitting module. The distance results are distributed to the corresponding groups of data by the kinematic subchain distribution module to the body velocity directional scaling module and the upper limb joint obstacle avoidance inequality construction and null space solution module, respectively; the output constraints of the two modules are jointly input to the whole-body obstacle avoidance arbitration and failure degradation module; the whole-body obstacle avoidance arbitration and failure degradation module sends the final execution command to the external foot and upper limb execution units, and at the same time drives the status and part-by-part minimum distance reporting module to report the system status to the outside.

[0018] This invention provides a method and system for full-body obstacle avoidance in a legged humanoid robot. It overcomes the limitations of a fixed body envelope by constructing a dynamically updated three-level hierarchical structure of link-collision bodies. By propagating calibration residuals and converting them into differentiated expansion margins for each link, it ensures the safety and reliability of distal links while preserving the mobility of proximal links. Through layered pruning and voxel greedy merging, it enables real-time geometric queries on an embedded platform. Kinematic subchain diversion generates dual-channel constraints to achieve simultaneous walking and upper limb obstacle avoidance. By handling unobserved voxels at different locations and verifying speed execution, it ensures reliable operation in scenarios with incomplete maps and avoids invalid gait commands. Overall, it improves the practicality and safety of obstacle avoidance in complex environments for legged humanoid robots. Thanks to the dynamic updating of the three-level hierarchical structure of link-collision bodies with the robot's joint configuration, and the fact that the sliding window occupies the map and only applies sensor noise margin expansion, the entire body geometry is supported by the robot's lateral convex hull model. Compared to existing technologies, it offers the following advantages: First, it eliminates the dilemma of "over-conservatism" and "missed detection" in the fixed body envelope. The geometry on one side of the robot is accurately represented by the link-by-link convex hull and updated periodically with the configuration. When the robot retracts its arms, the width of the passable passage is close to the actual width of the torso, and the arms themselves are protected when the arms are extended forward. The passage capacity is no longer determined by the worst envelope under the full configuration, and the arms, raised legs, and forward-leaning head are no longer out of protection.

[0019] Second, the collision criterion has been upgraded from a single bit to complete geometric information that supports differentiated actions. Signed distance indicates "how much is left," the direction of the line connecting the nearest points indicates "which way to retreat," and the link identifier indicates "who is facing the collision." Based on this, two types of actions—arm retraction when facing a collision and restriction of body movement pointing towards the obstacle when facing a collision—can be implemented separately, and the robot is no longer limited to stopping as a whole.

[0020] Third, the per-link expansion margin ensures the reliability of the criteria for distal links without affecting proximal links. The expansion margin is obtained from the calibration residual of the link itself through pose Jacobian propagation. Distal links such as the hand receive a larger margin, while links near the base such as the torso receive a smaller margin. Compared to a uniform safety distance based on the overall value of the most uncertain links throughout the body, the loss of passage capacity of proximal links is significantly reduced, while the criteria for distal links still cover their true uncertainty.

[0021] Fourth, the overhead of precise geometric queries is adapted to the degree of openness of the environment, allowing it to run on embedded platforms according to control cycles. Level 0 and Level 1 pruning reduce the cost per cycle in open environments to constant levels; risk subchain filtering limits the number of links entering the precise query to a single limb range in common scenarios such as walking close to walls; and greedy merging of voxels within the region of interest reduces the number of collision objects from thousands to tens. The combination of these three factors eliminates the need for parallel acceleration by the graphics processor to query the precise distance between the link mesh and the occupying voxels.

[0022] Fifth, the robot can "retract its arm to pass through" instead of "stopping and waiting." The upper limb obstacle avoidance constraint is constructed in the joint velocity space through pose Jacobian modeling and coexists with the task-expected velocity as a zero-space projection. Therefore, the arm's yielding movement does not occupy the body's velocity channel or change the walking command. When the robot passes through the gap in the shelf, it walks normally while retracting its arm to the side of its torso, rather than stopping in front of the gap.

[0023] Sixth, the tangential motion when touching obstacles is preserved. The robot's speed is reduced only in the normal component pointing to the line connecting the nearest points, while the tangential component is not reduced. Therefore, the robot can still translate or move around the surface of walls, shelves, or table edges when it is close to them, and will not be locked up due to the small normal distance.

[0024] Seventh, the obstacle avoidance speed limit is compatible with the minimum executable speed amplitude of the legged platform. After directional scaling, the amplitude is checked channel by channel to see if it falls into the non-executable range. If it does, it is explicitly set to zero and a stop sign is set. The system only exists in two states: "clearly stationary" and "moving at an executable speed." It will not issue a command that has a positive amplitude but the gait layer cannot generate a step, thus avoiding the robot's "command issued but no step taken" stagnation and subsequent stop-and-go movement near obstacles.

[0025] Eighth, global freeze does not occur when mapping is incomplete. Unobserved voxels are handled according to the subchains they face: those in the torso and foot directions are still treated as obstacles to ensure a safety margin, while the movement of the upper limbs in the direction of retraction towards the body can be moderately optimistic. The robot can still walk and retract its arms when it first enters a new area and when a large area of ​​the map is unobserved, instead of standing still and waiting for mapping to be completed.

[0026] Ninth, the two types of geometric representations of the robot and the environment are aligned at the same time. Before solving the link pose, the joint position is interpolated or extrapolated to occupy the map timestamp in the sliding window to avoid the robot geometry and the environment geometry being different by a data link delay during the arm swing; the end position deviation corresponding to this delay is on the same order of magnitude as the expansion margin. If they are not aligned, the margin obtained by calibration will be canceled out by the delay.

[0027] Tenth, calibration can be completed on-site using onboard components. The observation source for calibration is the onboard visual sensing component, and the observed object is a body marker fixed to the robot's own linkage, eliminating the need for position-sensitive detectors, structured light devices, or external calibration plates. The robot can be recalibrated on-site after collisions or maintenance cause changes in joint zero positions, with the expansion margin updated accordingly. Attached Figure Description

[0028] Figure 1 This is a logic block diagram of a full-body obstacle avoidance system for a legged humanoid robot according to the present invention; Figure 2 This is a flowchart illustrating the collision body hierarchy construction, whole-body calibration, and expansion margin calculation in the whole-body obstacle avoidance method for a legged humanoid robot described in this invention. Figure 3 This is a flowchart illustrating the layered pruning, voxel merging, and linked-by-link signed distance query process in the whole-body obstacle avoidance method for a legged humanoid robot described in this invention. Figure 4 This is a flowchart illustrating the process of distributing distance results by sub-chain and dual-channel constraints for a full-body obstacle avoidance method for a legged humanoid robot as described in this invention. Figure 5 This is a schematic diagram of the three-level hierarchical structure of the linkage collision body and the step-by-step pruning in the whole-body obstacle avoidance method of a legged humanoid robot described in this invention. Detailed Implementation

[0029] 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.

[0030] This solution provides a full-body obstacle avoidance method for legged humanoid robots. The geometric criterion for obstacle avoidance is improved from "using the body envelope with a single expansion radius to perform Boolean point queries on the occupied map" to "using a calibrated residual-corrected full-body link mesh model to perform hierarchical signed distance queries on the real-time sliding window occupied map". Furthermore, the distance of each link and the direction of the line connecting the nearest point are divided into two types of executable constraints according to the kinematic subchain to which the link belongs: the legged platform body velocity command and the upper limb joint command.

[0031] This invention consists of four intercoupled essential technical features: Firstly, the propagation of whole-body joint calibration and its residuals to link-by-link expansion margins. Onboard sensing components observe body markers fixed to the robot's links, collecting paired observations under multiple configurations with simultaneous joint readings to identify joint zero-position deviations and link geometric deviations. Calibration not only outputs corrected kinematic parameters but also retains parameter residual covariance, which is propagated via the Jacobian of each link's pose to the link's pose covariance in Cartesian space. This covariance is then converted into the link's unique anisotropic expansion margin, which is superimposed along the outer normal of its convex hull. The margin is larger for distal links and smaller for links near the base, thus replacing a uniform safety distance across the entire robot.

[0032] Secondly, the three-level hierarchical collider construction of the link mesh and the pose broadcast synchronized with the coordinate transformation tree. The collision mesh of each link is taken from the robot body description file, and offline convex decomposition is performed to generate a three-level hierarchy: the zeroth level is the whole-machine bounding sphere; the first level is the envelope capsule aggregated according to kinematic subchains (torso head, left arm, right arm, left leg, right leg); and the second level is the convex hull of each link, superimposed with the expansion margin of that link according to feature one. A coordinate system is attached to each collider on the coordinate transformation tree, and its pose is calculated and broadcast by the calibrated and corrected forward kinematics. Before broadcasting, the joint positions are interpolated or extrapolated according to the joint velocities to the same timestamp as the sliding window occupying the map, ensuring that the robot geometry and the environment geometry are aligned at the same moment.

[0033] Thirdly, on-demand voxel merging and signed distance querying under layered pruning. Each control cycle prunes from level zero to level two: if the bounding sphere of the entire machine does not intersect with the occupied voxel set, the entire frame is skipped; otherwise, risky subchains are screened out using the first-level envelope capsules of each subchain, and non-intersecting subchains are skipped; only within the axis-aligned region of interest bounded by the first-level envelope of the risky subchain in the sliding window occupied map, the occupied voxels are greedily merged into a small number of cuboid blocks along the three axes according to the travel encoding and constructed as collision objects, and then signed distance queries are performed on each pair of links in the second-level convex hull of the subchain, outputting the signed distance of each link, the nearest point pair, and the direction of the nearest point connection. Here, the occupied map no longer expands according to the machine radius, and the expansion amount only takes the sensor noise margin; the machine geometry is completely expressed by the second-level convex hull.

[0034] Fourth, obstacle avoidance constraints are issued based on the kinematic subchain branching. The minimum signed distance between the leg and torso subchains and the direction of the line connecting their nearest points are converted into a directional scaling of the body velocity command: only the velocity component along this direction is reduced, while the tangential component is not reduced, allowing the robot to pass sideways along the obstacle instead of stopping immediately; if the amplitude of a certain channel falls into the range that the legged platform cannot execute after scaling, then that channel is set to zero and an obstacle avoidance stop sign is set, without issuing a command that will not move even if issued. The minimum signed distance between the upper limb and head subchains is projected by the Jacobian projection of the link's pose into a linear inequality on the joint velocity, which, together with the upper limb joint velocity expected by the task, is solved by a quadratic programming with zero-space projection, allowing the arm to actively retract and make way without changing the body's walking command. Both share the result of the same distance query; adjacent link pairs directly connected by joints, as well as normally contact link pairs recorded during calibration, are removed from the detection pair whitelist.

[0035] The four features form a closed loop of "accurate measurement, feasible construction, fast querying, and usability": Feature 1 makes the collision criterion of the far link reliable and does not affect the near link; Feature 2 makes the geometry on one side of the robot change with the configuration as it is and aligned with the geometry and time of the environment; Feature 3 makes the precise geometric query between the link mesh and the occupied voxel can be completed on the embedded platform according to the control cycle; Feature 4 makes the query results truly fall on the two execution channels of the legged humanoid robot, so that the robot can "pass through with its arm retracted" and "pass through sideways", instead of only being able to stop.

[0036] Hardware requirements for implementing this invention: (1) A set of whole-body joint position sensing components, which are position encoders for each joint of the robot, used to output the position of the whole-body joints, joint velocity and their sampling timestamps; (2) A set of environmental perception components, which can be lidar or depth camera, to output point cloud or depth map with timestamps for constructing a local occupancy map around the robot; (3) One body pose estimation component, used to output the pose of the robot base in the world fixed coordinate system; (4) A set of airborne visual perception components is installed on the head of the robot body and is used to observe the body markers fixed on the robot's own linkage during the calibration phase; this component can be shared with the depth camera in (2); (5) A set of foot motion control unit, which receives the three-degree-of-freedom body velocity command consisting of forward linear velocity, lateral linear velocity and yaw angular velocity, and calculates it into the landing point and joint trajectory of the two legs; the unit has a minimum executable speed amplitude for each of the above three channels, and commands below the amplitude cannot generate effective steps; (6) One set of upper limb motion control unit, which receives position or speed commands from each joint of the upper limb; (7) An embedded computing unit for running the whole-body obstacle avoidance program of the present invention; (8) A robot body description file containing the collision mesh and nominal kinematic parameters of each link, as input for the offline preparation stage.

[0037] The output of the environmental perception component needs to be pre-transformed to the machine base coordinate system through extrinsic parameter calibration, or it can be directly output in the machine base coordinate system by the component. A coordinate transformation relationship that can be queried in real time must exist between the local occupancy map and the machine base coordinate system. The system uses configuration items to declare the set of links contained in each kinematic subchain, and whether each subchain belongs to the machine velocity channel or the upper limb joint channel, so that the same method can be adapted to different overall machine joint arrangements and different upper and lower limb division methods.

[0038] Figure 1 The diagram shown is a logical block diagram of the system composition of the present invention; the robot body description file and the link collision mesh are input into the three-level offline construction module of the link collision body; the output of the whole body joint position encoder is sent to the whole body joint zero position and link geometry calibration module; the environmental perception component and the body pose estimation component are connected to the sliding window occupying map construction module; the signals of each module are transferred, and finally the instructions are output to the foot motion control unit and the upper limb motion control unit respectively, while the execution status reporting is performed.

[0039] like Figure 1 As shown, the system of this invention consists of the following functional modules: a whole-body joint zero-position and link geometry calibration module; a calibration residual propagation and link-by-link expansion margin calculation module; a three-level offline construction module for link collision bodies; a self-collision pair and normal contact pair elimination module; a joint state time alignment and forward kinematics calculation module; a whole-body collision body coordinate transformation broadcast module; a whole-machine enclosing sphere and sub-chain envelope capsule grading coarse screening module; a sliding window occupied map construction module; a probability fusion and sensor noise margin expansion module; an unknown voxel part-based processing module; a region of interest voxel greedy merging and collision object construction module; a link-by-link signed distance and nearest point query module; a distance result distribution module based on kinematic sub-chains; a body velocity directional scaling module; an upper limb joint obstacle avoidance inequality construction and null space solution module; a whole-body obstacle avoidance arbitration and failure degradation module; and a state and part-by-part minimum distance reporting module; specifically as follows: The whole-body joint zero-position and link geometry calibration module ensures that the calculated pose of the whole-body links matches the actual position. This module drives the robot to traverse multiple calibration configurations, using onboard vision sensing components to observe body markers fixed to its own links, collecting the pairing of observed measurements with simultaneous joint readings, and using nonlinear least squares to identify the zero-position deviation of each joint and the geometric deviation of the links. Unlike calibration methods that rely on external measuring devices, this module's observation source is onboard components, and the observed object is the robot's own components, thus allowing direct execution on-site. Its output consists of corrected kinematic parameters and parameter residual covariance, which are respectively fed into the joint state time alignment and forward kinematics calculation module, and the calibration residual propagation and link-by-link expansion margin calculation module.

[0040] The calibration residual propagation and link-by-link expansion margin calculation module converts the calibration uncertainty into geometric margins in the obstacle avoidance criterion. This module uses the pose Jacobian of each link relative to the calibration parameters to propagate the parameter residual covariance into the pose covariance of that link in Cartesian space, and then calculates it into anisotropic expansion margins of the link along the three principal axes according to a preset confidence level. This module is a key component that distinguishes this invention from calibration schemes aimed at improving absolute positioning accuracy: the calibration residuals are not discarded here, but become the source of link-by-link safety margins. Its output is a link-by-link expansion margin table, which is fed into the three-level offline construction module of the link collision body to be superimposed on the second-level convex hull.

[0041] The three-level offline construction module for link colliders transforms the collision meshes in the robot body description file, which are designed for rendering and simulation, into a three-level collider hierarchy that supports progressive pruning. This module performs convex decomposition on the collision meshes of each link to obtain the second-level link-by-link convex hull. It then aggregates the convex hull vertices of the links contained in each sub-chain according to the configured kinematic sub-chains to obtain the first-level envelope capsule. Finally, it calculates the minimum bounding sphere over all vertices to obtain the zeroth-level whole-machine bounding sphere. The geometric parameters of the first and zeroth levels are functions of the configuration, updated with the link poses during the online phase, while their radii are determined once during the offline phase. Its output is sent to the whole-body collider coordinate transformation broadcast module.

[0042] The self-collision pair and normal contact pair elimination module preemptively excludes detection pairs that are consistently reported as contact, thus inevitably generating false alarms. This module enumerates all link pairs, eliminating adjacent link pairs directly connected by joints; and during the offline phase, it drives the robot to traverse several normal configurations, recording link pairs whose distance is consistently less than the contact determination threshold as normal contact pairs and eliminating them as well. The output of this module is a whitelist of detection pairs, which is sent to the link-by-link signed distance and nearest point query module.

[0043] The joint state time alignment and forward kinematics calculation module aligns the robot's geometry with the environment's geometry at the same moment and calculates the pose of the whole-body links according to the corrected parameters. This module reads the timestamp of the current frame's sliding window occupying the map, interpolates or extrapolates the joint positions to that timestamp based on joint velocities, and then performs forward kinematics calculation using the corrected kinematic parameters. Without joint state time alignment, the arm's pose during swing differs from the map by an angle corresponding to a data link delay. The positional deviation of the end effector links can be on the same order of magnitude as its expansion margin, causing the meaning of the margin to be negated by the delay. If the extrapolation duration exceeds the maximum allowable duration, a joint state overdue flag is set. Its output is sent to the whole-body collision coordinate transformation broadcast module.

[0044] The whole-body collider coordinate transformation broadcast module's function is to make the poses of each level of colliders a uniformly queried quantity within the system. This module attaches a coordinate system to each collider on the coordinate transformation tree and broadcasts the calculated pose cycle by cycle; the center and axis of the first-level enveloping capsule and the zeroth-level whole-machine surrounding sphere are also recalculated and broadcast according to the link poses of this cycle. Its output is sent to the whole-machine surrounding sphere and sub-chain enveloping capsule grading coarse screening module.

[0045] The sliding window occupies the map building module, which maintains a 3D environment representation centered on the robot with a constant memory footprint. This module organizes the voxel buffer using circular addressing; when the robot moves beyond a preset threshold, only a few layers of voxels at the boundary are cleared without reconstructing the entire map, thus making the mapping overhead during long-distance robot movement independent of the walking distance. Its output is fed into the probability fusion and sensor noise margin expansion module.

[0046] The probabilistic fusion and sensor noise margin expansion module provides the voxel occupancy state after multi-frame observation fusion and applies only the expansion amount corresponding to the sensor noise. This module maintains the logarithmic probability of occupancy for each voxel, accumulating the probability based on the hit increment when a voxel is hit and decreasing it based on the penetration increment when a ray passes through it, with upper and lower limits truncated. When the voxel occupancy state flips, the expansion count of its neighborhood is updated incrementally to avoid recalculating the entire image expansion every frame. The expansion amount in this module only takes the margin corresponding to the sensor ranging noise and voxel resolution, without any component representing the machine size—this is the dividing point between this invention and the common paradigm of "expanding the map according to the machine radius + degenerating the machine into points." Its output is sent to the voxel greedy merging and collision object construction module of the region of interest.

[0047] The unknown voxel segmentation module provides a segmentation strategy for handling incomplete maps, rather than applying a uniformly conservative or optimistic approach. This module treats unobserved voxels within the region of interest according to the sub-chains they face: those facing leg and torso sub-chains are considered obstacles; those facing upper limb and head sub-chains are excluded only if the linkage's movement trend is away from the body's convergence direction, otherwise they are included. Its output is fed into the region of interest voxel greedy merging and collision object construction module.

[0048] The overall bounding sphere and sub-chain envelope capsule grading coarse screening module's function is to eliminate most unnecessary detection scenarios at a very low cost before entering the precise geometric query. This module first determines whether the zeroth-level overall bounding sphere intersects with the occupied voxel set. If they do not intersect, there are no obstacle avoidance constraints in the entire channel for this cycle, and all subsequent precise queries are skipped. If they intersect, it checks whether the first-level envelope capsule body intersects with the occupied voxel set, obtaining a risk sub-chain set. If the risk sub-chain set is empty, subsequent queries are also skipped. This module reduces the robot's cycle overhead to constant levels in open environments. Its output is fed into the region of interest voxel greedy merging and collision object construction module.

[0049] The module for greedy merging and collision object construction of voxels in the region of interest (ROI) transforms a discrete set of voxels into a small set of convex bodies that can be directly geometrically queried with triangular meshes. For each risky subchain, this module defines an axis-aligned ROI by adding the subchain's maximum expansion margin and the upper limit of the query distance to its first-level envelope capsule. Within this region, a greedy merging algorithm is performed along the three coordinate axes, merging adjacent occupying voxels into the largest possible cuboid block and constructing collision objects. Creating a collision object for each voxel would result in thousands of objects, making real-time geometric queries impossible; this module's merging typically reduces the number of objects to tens. Its output is fed into the linked-by-link signed distance and nearest-point query module.

[0050] The link-by-link signed distance and nearest-point query module provides the complete geometric information required for obstacle avoidance motion generation. For each link in each risk subchain, this module takes its second-level convex hull (with the link's expansion margin superimposed) and constructs it as a collision object with cuboid blocks within the region of interest of that subchain. It performs a signed distance query on each pair, skipping pairs already removed from the whitelist. The query outputs the signed distance between the pairs, the two nearest points, and the unit direction determined by the line connecting the two nearest points. The distance is positive when the links separate and negative when they penetrate, ensuring continuous availability after penetration and preventing loss of gradient information at the moment of contact. Its output is fed into the kinematic subchain-based distance result distribution module.

[0051] The distance result distribution module, based on kinematic subchains, distributes the results of the same query to two execution channels with different properties, according to the location of the link. This module, based on the configured subchain affiliation, divides the distance results of each link into a leg / torso group and an upper limb / head group, and within each group, it takes the minimum signed distance and the direction of the line connecting its corresponding nearest point. Its output is then sent to the body velocity directionality scaling module and the upper limb joint obstacle avoidance inequality construction and null space solution module, respectively.

[0052] The body velocity directional scaling module restricts body movement when the legs or torso face a collision, limiting only the movement of the portion actually pointing towards the obstacle. This module decomposes the body velocity command along the line connecting the closest points of the leg-torso group with the minimum distance into normal and tangential components. Only the normal component is reduced using a distance attenuation coefficient; the tangential component is not reduced, allowing the robot to still translate along the wall when close to it. After scaling, the amplitude is checked channel by channel to see if it falls within the minimum executable velocity amplitude range of the legged platform. If it does, the amplitude is reset to zero and an obstacle avoidance stop sign is set, preventing the issuance of commands that will not result in any movement. Its output is sent to the whole-body obstacle avoidance arbitration and failure degradation module.

[0053] The upper limb joint obstacle avoidance inequality construction and zero-space solution module enables the arm to autonomously retract and make way while the body continues to move normally. This module constructs a linear inequality for each link in the upper limb head assembly, projecting the direction of the nearest point connection onto the joint velocity space using its positional Jacobian. This inequality states that "the approach velocity of this link along the nearest point connection does not exceed the upper limit determined by the difference between the current distance and the safe distance." Then, using the desired upper limb joint velocity as the primary task, and satisfying all obstacle avoidance inequalities while projecting the primary task onto the zero space, a quadratic programming problem is solved to obtain the actual issued upper limb joint velocity. If the quadratic programming is infeasible, the entire sub-chain reaches zero velocity and retracts the upper limb along the retraction configuration recorded during the calibration phase. Its output is sent to the whole-body obstacle avoidance arbitration and failure degradation module.

[0054] The whole-body obstacle avoidance arbitration and failure degradation module merges the outputs of two channels and unconditionally degrades the system when the system state is unreliable. This module verifies whether the most recent update times of the occupied map, coordinate transformation, and joint states have exceeded their respective timeout thresholds; if any one of these exceeds the threshold, the entire machine outputs zero speed; it also responds to external emergency stop signals. Its output is the final issued body speed command and upper limb joint command.

[0055] The status and minimum distance reporting module for each part provides observable measurements of the entire obstacle avoidance system. This module reports whether the system is currently in an obstacle avoidance speed-limited state, the minimum signed distance of each sub-chain, the trigger limit link identifier and the direction of the line connecting the nearest point, and at which pruning stage the system was skipped in this cycle, facilitating on-site coordination and fault location.

[0056] It should be noted that the signed distance operation of convex bodies, the greedy merging algorithm of stroke encoding, the robot Jacobian null space projection, and the constrained quadratic programming solution are all well-known algorithms in the field of robot motion planning. The innovation of this application does not lie in these underlying algorithms themselves, but in their combination and adaptation to the data flow and constraint generation logic of the whole-body obstacle avoidance of legged humanoid robots. The body markers are visual reference marks fixed on the links, and well-known markers in the field can be selected.

[0057] As an embodiment of this solution, in this embodiment, a legged humanoid robot performs inspection and retrieval tasks in an indoor environment. The robot has 29 movable joints, divided into 5 kinematic subchains: torso head, left arm, right arm, left leg, and right leg. The torso head and two leg subchains belong to the body velocity channel, while the two arm subchains belong to the upper limb joint channel. The body head is equipped with a set of visual perception components, which also serve as the body marker observation source during the calibration phase; the body is equipped with a lidar unit, which outputs point clouds with timestamps; the body pose estimation component outputs the base pose in a fixed world coordinate system; the legged motion control unit receives three-degree-of-freedom body velocity commands consisting of forward linear velocity, lateral linear velocity, and yaw angular velocity; the upper limb motion control unit receives joint velocity commands from 7 joints in each arm; and the embedded computing unit runs a full-body obstacle avoidance program at a cycle of 50 Hz.

[0058] The voxel resolution for locally occupied maps is set to 0.05 meters, the sliding window size is set to 8 meters × 8 meters × 2.5 meters, and the sliding threshold is set to 20 voxels. The sensor noise margin expansion is set to 0.03 meters. The upper limit for signed distance queries is set to 0.5 meters, meaning that link-cubic-block pairs exceeding this distance are not included in the query. The safe distance for the leg-torso group is set to 0.10 meters, and the influence distance is set to 0.35 meters; the safe distance for the upper limb-head group is set to 0.06 meters, and the influence distance is set to 0.25 meters; the convergence time constant for obstacle avoidance constraints is set to 0.3 seconds.

[0059] The operating environment includes three typical working conditions: Condition X is when the robot carries an object through a 0.85-meter-wide gap in the shelving, with the maximum body width being 0.62 meters when the arms are naturally hanging down and 1.05 meters when the arms are extended forward to hold the object; Condition Y is when the robot walks close to the wall, with the side of the torso about 0.15 meters away from the wall; Condition Z is when the robot has just entered a new room that has not been mapped, and a large area of ​​voxels outside the field of view is not observed.

[0060] The method of this invention is divided into an offline preparation stage and an online control cycle stage. Offline preparation stage, such as Figure 2 As shown Figure 2This is a flowchart illustrating the collision body hierarchy construction, whole-body calibration, and expansion margin calculation in the whole-body obstacle avoidance method of the present invention. The offline preparation stage outputs a three-level collision body hierarchy, a whitelist of detection pairs, corrected kinematic parameters, and a link-by-link expansion margin table; including the following steps: Step 1: Construction of the three-level collider hierarchy Read the robot body description file and retrieve the collision mesh and nominal kinematic parameters of each link. Perform convex decomposition on the collision mesh of each link to obtain the second-order convex hull of that link; the maximum number of convex blocks in the convex decomposition is set to 4, and those exceeding this number are replaced by their convex hulls to control the number of convex block pairs during online queries.

[0061] Based on the five kinematic subchains declared in the configuration, aggregate the convex hull vertices of the links contained in each subchain under the calibration configuration, and calculate the radius of the minimum enclosing capsule body to obtain the radius of the first-level enclosing capsule body; the two endpoints of the capsule body are taken as the origin of the coordinate system of the first and last links of the subchain during the online phase. Calculate the radius of the minimum enclosing sphere for the convex hull vertices of all links in the entire machine to obtain the radius of the zeroth-level overall enclosing sphere; the center of the sphere is taken as the origin of the base coordinate system during the online phase.

[0062] Step 2: Detect the construction of the removal set All link pairs are enumerated, and adjacent link pairs directly connected by joints are removed. The robot is then driven to traverse 200 normal configurations covering the full range of motion of each joint. For each configuration, the minimum distance between the remaining link pairs is calculated. Link pairs with a distance less than 0.01 meters in all configurations are recorded as normal contact pairs and removed. The remaining link pairs, along with cross-class detection pairs such as "link-cubic block," constitute a whitelist of detection pairs.

[0063] Step 3: Zero-position calibration of all joints and geometric calibration of connecting rods The robot is driven to traverse 120 sets of calibration configurations. In each configuration, the onboard visual perception component observes the body markers fixed on the forearms and palms of both arms and the lower legs of both legs. The observation position of the markers in the coordinate system of the visual component and the readings of the whole body joints at the same moment are collected to form 120 sets of observation-reading pairs.

[0064] Using the zero-position deviation of each joint and the link length, link offset, and torsion angle deviation of each link as parameters to be identified, a nonlinear least squares problem is constructed according to Equation (1) and solved by the Levenberg-Marquardt method to obtain the corrected kinematic parameters; at the same time, the parameter residual covariance is obtained from the Jacobian and residual at the convergence point according to the posterior covariance expression in Equation (1). , In the formula, This represents the kinematic parameters obtained from the identification; Indicates the first The observation positions of the body markers under the calibration configuration are in meters; This indicates the readings from this group of joints. With parameters Predicted location of landmarks; This represents the observation residuals at the point of convergence; This represents the Jacobian of the observation function with respect to the parameters at the point of convergence; This represents the residual variance estimate for a single observation, with the denominator being the total dimension of all observations minus the number of parameters to be identified. The dimension is the stacked number of all the observation locations of the ontological markers under a single configuration. This represents the residual covariance of the parameters.

[0065] Step 4: Residual Propagation and Link-by-Link Expansion Margin Calculation For each link, the parameter residual covariance is propagated to the position covariance of the link in Cartesian space by the pose Jacobian of the link relative to the calibration parameters according to Equation (2); , In the formula, Let represent the pose covariance of the i-th link in Cartesian space, in square meters; This indicates the pose Jacobian of the link position with respect to the kinematic parameters; This represents an ordered set of joint numbers traversed from the base to the link. Let represent the link transformation matrix of the j-th joint.

[0066] According to formula (3), the confidence half-axis length of the covariance in the three principal axis directions is taken as the anisotropic expansion margin of the link along the three principal axes, and the confidence level is 3 times the standard deviation. The obtained margins are superimposed along the outward normal of each face of the second-level convex hull to obtain the second-level convex hull after superimposing the margins.

[0067] (3), In the formula, and They represent The Each eigenvalue and eigenvector; This represents the confidence level coefficient, with a value of 3 corresponding to approximately a 99.7% confidence level. This indicates that the link is along the first... Expansion allowance for each main shaft, in meters; This indicates the original convex hull of the link without any added allowance. Indicate its boundary, This indicates the convex body obtained by taking the convex hull after expanding outwards.

[0068] In this embodiment, the expansion margin of the torso link is approximately 0.008 meters, the upper arm is approximately 0.015 meters, and the palm is approximately 0.026 meters. If a uniform safety distance is applied to the entire body, then all links must be set to 0.026 meters, resulting in an additional loss of approximately 0.018 meters of unilateral passage margin for the torso and legs.

[0069] Offline preparation complete, transitioning to online control cycle. Online control cycle, such as Figure 3 —5 The zeroth level is the whole machine surrounding the sphere; if there is no intersection, the entire frame is skipped. The first level is five kinematic sub-chains enclosing the capsule body; if there is no intersection, the entire chain is skipped. The second level is the link-by-link convex hull, which is superimposed with the expansion margin and combined with the cuboid block to construct a collision object for distance query.

[0070] Step 5: Align the image reading with the joint status time.

[0071] Read the map occupied by the sliding window in this frame and its timestamp. According to formula (4), interpolate or extrapolate the joint position from its sampling timestamp to the map timestamp; if the extrapolation time exceeds 0.05 seconds, do not extrapolate, but set the joint status overdue flag and hand it over to step fourteen for processing.

[0072] , In the formula, This indicates the joint position aligned to the map timestamp, in radians. and These represent the timestamps in seconds when the sliding window occupies the map and the joint state, respectively. Indicates the maximum allowed interpolation or extrapolation duration, in seconds.

[0073] Step Six: Full-body linkage pose calculation and coordinate transformation broadcast.

[0074] Using the calibrated and corrected kinematic parameters, perform forward kinematic calculations according to formula (5) to obtain the pose of each link relative to the base coordinate system. Then, cascade the pose of the base relative to the world fixed coordinate system to obtain the world pose of each link.

[0075] , In the formula, The first one identified by equation (1) Zero-position deviation of each joint, in radians; This represents the identified geometric deviation of the link; both are... Components; Indicates the first The pose of each link relative to the base coordinate system; This indicates the pose of the base relative to the world fixed coordinate system; This indicates the world pose of the link.

[0076] A coordinate system is broadcast on the coordinate transformation tree for each second-level convex hull, the first-level envelope capsule of each subchain, and the zeroth-level overall bounding sphere. The endpoints of the first-level capsule and the center of the zeroth-level bounding sphere are recalculated according to the pose of the current cycle.

[0077] Step 7: Zero-stage coarse screening.

[0078] According to the zeroth-level criterion of formula (6): , Calculate the lower bound of the distance between the zeroth-level whole-machine bounding sphere and the set of occupied voxels: Using the voxel where the center of the sphere is located as the center, draw an axis-aligned bounding box in the map occupied by the sliding window, according to the sphere radius plus the maximum expansion margin of the whole body and the upper limit of the query distance, and count whether there is an occupied voxel within the bounding box. If there is no occupied voxel, it is determined that there is no obstacle avoidance constraint in the entire channel in this cycle, and directly go to step twelve; if there is, continue.

[0079] Step 8: Screening of first-level risk subchains.

[0080] Equation (6) First-level criterion: , For each subchain, its first-level envelope capsule is used, along with the maximum expansion margin within that subchain and the upper limit of the query distance. An axis-aligned bounding box is then defined, and the presence of occupying voxels is counted. Subchains containing these voxels are classified as risky. If the set of risky subchains is empty, it is determined that there are no obstacle avoidance constraints in the entire channel for this cycle, and the process proceeds to step twelve. Under condition Y (walking close to the wall), typically only the torso head subchain and the arm subchain on the side closest to the wall are classified as risky subchains. The arm subchain on the other side and the two leg subchains are skipped entirely in this step.

[0081] Step 9: Greedy merge of voxels in regions of interest.

[0082] For each risk subchain, calculate its axis-aligned region of interest according to equation (7).

[0083] , In the formula, and This indicates taking the smallest and largest components sequentially; This represents a three-dimensional vector in which all components are 1.

[0084] Traverse voxels within this region: occupied voxels are directly included in the set to be merged; unobserved voxels are handled according to formula (13), that is, when the subchain belongs to the body velocity channel, it is included; when it belongs to the upper limb joint channel, it is included only when the expected joint velocity of the link in this cycle is close to the nearest point in the direction of the line connecting the nearest points, and not included when it is far away. , In the formula, Indicates the set of unobserved voxels; and These represent the sets of links belonging to the body's velocity channel and the upper limb joint channel, respectively. Indicates the first The link in the chain closest to the voxel; This represents the velocity component of the link in the direction away from the obstacle at the desired speed for the mission; a negative value indicates that it is approaching.

[0085] The set to be merged is greedily merged along the three coordinate axes in sequence according to formula (8): first merge the continuous occupying voxels into line segments along the first axis, then merge the adjacent line segments with the same start and end on the second axis into rectangles, and finally merge the adjacent rectangles with the same rectangles on the third axis into cuboid blocks. , In this embodiment, under the shelf gap condition, a single subchain has approximately 1800 occupying voxels within its region of interest, which, after merging, result in approximately 40 cuboids. Each cuboid is then constructed as a collision object.

[0086] Step 10: Signed distance query for each link.

[0087] For each link under each risk subchain, take its second-level convex hull after superimposed expansion margin, and construct it as a collision object with the cuboid block of the subchain. Perform a signed distance query on each pair, skipping pairs that have been removed from the whitelist. Output the signed distance between each pair, the two nearest points and the unit direction determined by the line connecting the two nearest points according to equation (9); the distance is positive when they separate, and the distance is negative when they penetrate each other.

[0088] , In the formula, The operation representing the signed distance between two convex bodies; Indicates the first The second-stage convex hull after the expansion margin of each link is superimposed; This represents the set of detection pairs that have been removed. This represents the cuboid block with the smallest signed distance from the link; and These respectively represent the link and The nearest point on; It represents the unit direction from the nearest point of the cuboid block to the nearest point of the link, determined by the line connecting the two nearest points.

[0089] Step 11: Distance results are split by sub-chain Based on the sub-chain affiliation declared in the configuration, the query results for each link are divided into the leg / torso group and the upper limb / head group. Within each group, the link with the smallest signed distance is selected, and its value, corresponding link identifier, and the direction of the line connecting the nearest point are recorded.

[0090] Step Twelve: Directional scaling of body velocity Take the direction of the line connecting the minimum signed distance between the leg and torso groups and the nearest point. If step seven or eight has determined that there are no obstacle avoidance constraints in the entire channel, then this step will not scale, and the machine speed command will pass as is.

[0091] Otherwise, according to formula (10), the translation part of the body speed command is decomposed into normal and tangential components along the direction of the line connecting the nearest points. Only the normal component is multiplied by the distance attenuation coefficient; the tangential component is not reduced.

[0092] , In the formula, and These represent the translation portion of the machine's speed command before and after scaling; and These represent the minimum signed distance between the leg and trunk groups and the direction of the line connecting their nearest points; Indicates the distance attenuation coefficient; This indicates a truncation operation. Piecewise condition. This indicates that the speed is reduced only when the component of the velocity in that direction is close to the obstacle.

[0093] Perform an executability check channel by channel after scaling: , In this embodiment, the minimum executable speed amplitude for forward and lateral linear velocities is 0.08 meters per second, and the yaw rate is 0.15 radians per second.

[0094] Step Thirteen: Solving the obstacle avoidance inequality of the upper limb joints and the null space.

[0095] For each link in the upper limb head group, a linear inequality is constructed by projecting the direction of the nearest point connection line onto the joint velocity space of the sub-chain according to Equation (11) with its position Jacobi.

[0096] , In the formula, Indicates the first The position of each link relative to the sub-link joint at the nearest point is Jacobian; Indicates upper limb joint velocity command; This represents the convergence time constant of the obstacle avoidance constraint.

[0097] Taking the desired upper limb joint velocity as the main task, a quadratic programming problem is constructed and solved according to equation (12): , The desired velocity for the main task is expressed using null-space projection: , In the formula, A weighted matrix representing the deviation of the main task; Indicates the expected upper limb joint velocity for the task; This indicates the main task is Jacobi's pseudo-reverse; Indicates the expected velocity in the task space; The zero-space projection matrix; Secondary expected joint velocity.

[0098] The solution adopts the null space projection form of the main task, so that obstacle avoidance movement is prioritized to be realized in the null space without affecting the main task. The main task is sacrificed only when it conflicts with the obstacle avoidance constraints. When the quadratic programming is not feasible within the preset number of iterations, the velocities of all joints in the subchain are set to zero, and the joint velocities with limited amplitudes are output with the preset retraction configuration as the target, so that the upper limbs are retracted to the side of the torso.

[0099] Step Fourteen: Arbitration, Failure Degradation, and Instruction Issuance The system verifies whether the most recent update times of the occupied map, coordinate transformation, and joint status exceed their respective timeout thresholds (0.20 seconds, 0.10 seconds, and 0.05 seconds respectively in this embodiment), and whether the external emergency stop signal is set. If any of these conditions are met, the system outputs zero speed: the three channels of body speed are set to zero, the speeds of each joint in the upper limbs are set to zero, and the cause of failure is reported externally.

[0100] Otherwise, the outputs of steps twelve and thirteen are sent to the foot motion control unit and the upper limb motion control unit, respectively.

[0101] Step 15: Status Reporting Report externally whether the current obstacle avoidance speed limit state is in effect, the minimum distance of each sub-chain, the direction of the link marker that triggered the limit and the line connecting the nearest point, whether the current cycle was skipped in the zeroth or first level pruning, and whether secondary planning is feasible.

[0102] The execution effects of the three working conditions in this embodiment are as follows: Condition X (crossing a 0.85-meter shelf gap, with a body width of 1.05 meters when holding the object with both arms): When the robot approaches the gap, the signed distance between the forearms and the hand links of both arms decreases to within the safe distance before the torso links. The minimum distance of the upper limb head group triggers the inequality of equation (11). The secondary programming solves the joint speed that causes the arms to retract towards the torso in the zero space of the main task (maintaining the holding posture). The distance of the torso links is always greater than the safe distance, so the body speed is not scaled, and the robot maintains its walking speed to pass through. If the fixed body envelope is used, the envelope width must be taken as 1.05 meters when the arms are extended forward. The 0.85-meter gap is judged to be impassable.

[0103] Condition Y (walking close to the wall, the side of the torso is 0.15 meters away from the wall): The torso head sub-chain is listed as the risk sub-chain, and its minimum distance direction points to the wall normal. Equation (10) only reduces the component of the body speed along the wall normal, and the forward speed along the wall tangential is not affected. The robot walks along the wall at normal speed. If the operator issues a lateral speed command towards the wall, the normal component of the command is attenuated by distance until the distance reaches the safe distance and the component returns to zero. At this time, the lateral channel amplitude falls below the minimum executable speed amplitude and is set to zero and an obstacle avoidance stop is reported.

[0104] Condition Z (new room, large unobserved area): Unobserved voxels facing the leg and torso groups are included as obstacles according to formula (13). The robot's forward speed is limited by the observed free space boundary, and its behavior is characterized by deceleration and probing. Unobserved voxels facing the arm subchains are not included in the retraction direction, so the upper limbs can still perform the retraction action normally without being frozen. As mapping progresses, unobserved voxels become idle, and the forward speed limit is lifted accordingly.

[0105] 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 full-body obstacle avoidance for a legged humanoid robot, characterized in that, Includes the following steps: Step S1: Perform geometric calibration of all joints and links, retain the parameter residual covariance obtained from the calibration, and calculate the expansion margin corresponding to each link by propagating the parameter residual covariance through the link pose Jacobian. Step S2: Construct a three-level hierarchical structure of link colliders, align the joint states with the map occupied by the sliding window in terms of joint state time, and broadcast the poses of each level of colliders; Step S3: Perform hierarchical pruning and screening to obtain risk subchains. Greedy merge the voxels occupying the region of interest corresponding to the risk subchains. Combine the unobserved voxels with the part-based handling strategy, perform a signed distance query for each link to obtain the signed distance, nearest point pair, and unit direction determined by the line connecting the two nearest points. Step S4: The query results are split into kinematic subchains to generate body velocity constraints for the leg and torso subchain and upper limb joint obstacle avoidance constraints for the upper limb and head subchain. After arbitration and failure degradation processing, the results are sent to the corresponding execution units.

2. The method for full-body obstacle avoidance of a legged humanoid robot according to claim 1, characterized in that, The step S1 described in the process of performing full-body joint and link geometry calibration includes: observing the body markers fixed on the robot links by relying on the airborne sensing components, collecting multiple sets of body marker observation values ​​and joint readings under different configurations, identifying joint zero-position deviations and link geometry deviations, and outputting the corrected kinematic parameters and the parameter residual covariance. The parameter residual covariance is propagated through the link pose Jacobian matrix to obtain the link pose covariance in Cartesian space. The anisotropic expansion margin of the link is calculated from the pose covariance. The expansion margin of the far-end link is greater than that of the link near the base. The expansion margin is then superimposed onto the corresponding link convex hull along the outer normal of the link convex hull.

3. The whole-body obstacle avoidance method for a legged humanoid robot according to claim 1, characterized in that, The three-level hierarchical structure of the connecting rod collision body described in step 2 includes: the zero-level whole-machine enveloping sphere, the first-level sub-chain enveloping capsule body, and the second-level connecting rod convex hull; The zeroth-level whole-machine encircling sphere is generated by all the vertices of the connecting rod convex hulls; The first-level sub-chain envelope capsule body is generated by kinematic sub-chain aggregation; The second-level link-by-link convex hull is the link convex hull after superimposing the expansion margin of the corresponding link; Adjacent link pairs and normally contact link pairs are pre-removed to generate a whitelist of detection pairs. Subsequent distance queries will skip detection pairs in the whitelist.

4. The method for full-body obstacle avoidance of a legged humanoid robot according to claim 1, characterized in that, The stratified pruning includes a zero-level coarse screening and a first-level risk subchain screening; The zeroth-level coarse screening specifically involves determining whether the enclosing sphere of the whole machine intersects with the set of occupying voxels; If no match is found, all exact queries will be skipped for this period. If they intersect, perform the first-level risk subchain screening: determine whether the subchain envelope capsule body and the occupying voxel set intersect for each subchain, and mark the intersecting subchain as a risk subchain; if they do not intersect, skip all precise queries in this cycle.

5. The method for full-body obstacle avoidance of a legged humanoid robot according to claim 1, characterized in that, The unobserved voxel segmentation strategy described in step S3 includes: unobserved voxels belonging to the leg and torso subchain are directly counted as obstacles; unobserved voxels belonging to the upper limb and head subchain are counted as obstacles only when the linkage's movement trend is toward the obstacle, and are not counted as obstacles when the linkage moves toward the body. The greedy merging algorithm is a stroke-coded greedy merging algorithm that performs stroke encoding along the three coordinate axes to merge discrete occupied voxels into cuboid blocks and construct them as collision objects.

6. The method for full-body obstacle avoidance of a legged humanoid robot according to claim 1, characterized in that, The signed distance query for each link in step S3 involves constructing the second-level link convex hull after superimposing the expansion margin and the cuboid block as a collision object, and then performing a signed distance calculation on the convex body. When the links separate, the distance is taken as a positive value; when they penetrate each other, the distance is taken as a negative value representing the penetration depth. The output includes the signed distance, the nearest point pair between the link and the obstacle, and the unit direction determined by the line connecting the two nearest points. The query process skips detection pairs in the whitelist of detection pairs.

7. The method for full-body obstacle avoidance of a legged humanoid robot according to claim 1, characterized in that, The step S4 of generating the body velocity constraint corresponding to the leg-torso subchain includes: obtaining the minimum signed distance of the leg-torso subchain and the direction of the corresponding nearest point connection; decomposing the body velocity command into a normal component and a tangential component along the direction of the nearest point connection, and only attenuating and scaling the normal component while keeping the tangential component unchanged. After scaling, an executable check is performed on each speed channel. If the channel speed amplitude is greater than 0 and less than the minimum executable speed amplitude of the foot platform, then the channel is set to zero and the obstacle avoidance and parking sign is set.

8. A method for full-body obstacle avoidance of a legged humanoid robot according to claim 1, characterized in that, The step S4 of generating the upper limb head subchain corresponding to the upper limb joint obstacle avoidance constraint includes: using the link pose Jacobian, projecting the direction of the nearest point connection line to the joint velocity space to construct a linear obstacle avoidance inequality for the joint velocity; taking the expected upper limb joint velocity as the main task, solving the quadratic programming under the zero-space projection framework. When the quadratic programming solution is not feasible, all joints of the subchain are set to zero, and the upper limb is driven to retract to the preset convergence configuration.

9. A full-body obstacle avoidance system for a legged humanoid robot, used to implement the full-body obstacle avoidance method according to any one of claims 1-8, characterized in that, include: The whole-body joint zero position and link geometry calibration module is used to traverse multiple sets of calibration configurations, observe the body markers using airborne sensing components, identify joint zero position deviations and link geometry deviations, and output the corrected kinematic parameters and parameter residual covariance. The calibration residual propagation and link-by-link expansion margin calculation module is used to receive the parameter residual covariance and convert it into a link-by-link expansion margin table through pose Jacobian propagation. The three-level offline construction module for the link collision body is used to receive the link-by-link expansion margin table and construct the three-level hierarchical structure of the link collision body. The self-collision pair and normal contact pair elimination module is used to eliminate adjacent link pairs directly connected to the joint and normal contact link pairs, and outputs a whitelist of detection pairs. The joint state time alignment and forward kinematics calculation module is used to align the joint state to the map timestamp occupied by the sliding window and calculate the whole body link pose based on the corrected kinematic parameters. The whole-body collider coordinate transformation broadcast module is used to attach coordinate systems to each level of collider and broadcast the pose of each level of collider on a cycle-by-cycle basis. The whole machine surrounds the sphere and sub-chain envelope capsule grading and coarse screening module is used to perform zero-level and first-level stratified pruning to screen out risky sub-chains; The sliding window-occupied map building module is used to maintain a 3D sliding window-occupied map centered on the robot with a constant memory footprint. The probabilistic fusion and sensor noise margin dilation module is used to perform probabilistic fusion on voxels and apply only the map dilation amount corresponding to the sensor noise. The unknown voxel segmentation module is used to perform segmentation and discarding of unobserved voxels within the region of interest according to the kinematic subchain. The module for greedy merging of voxels within a region of interest and constructing collision objects is used to perform run-length encoding greedy merging of voxels within the region of interest, generate cuboid blocks, and construct collision objects. The signed distance and nearest point query module for each link is used to perform convex distance calculation for each link in combination with the whitelist of the detection pair, and output the signed distance for each link, the nearest point pair, and the unit direction determined by the line connecting the two nearest points; The distance result splitting module is used to split the distance query results into two groups of data according to the kinematic sub-chain: the leg and trunk group and the upper limb and head group. A body velocity directional scaling module is used to generate body velocity constraints based on the leg and torso group data. The upper limb joint obstacle avoidance inequality construction and null space solution module is used to construct obstacle avoidance inequalities based on the upper limb head group data and solve the quadratic programming by null space projection, and output upper limb joint obstacle avoidance constraints. The whole-body obstacle avoidance arbitration and failure degradation module is used to merge the body speed constraints and the upper limb joint obstacle avoidance constraints, verify the system status timeliness, and perform failure degradation. The status and minimum distance reporting module is used to report obstacle avoidance speed limit status, minimum distance of each sub-chain, trigger links, pruning level and solution status to the outside world.

10. A full-body obstacle avoidance system for a legged humanoid robot according to claim 9, characterized in that, The module calling logic and data flow of the system are as follows: Offline Phase: The residual covariance of the output parameters from the whole-body joint zero-position and link geometry calibration module is sent to the calibration residual propagation and link-by-link expansion margin calculation module; the link-by-link expansion margin table output by the calibration residual propagation and link-by-link expansion margin calculation module is sent to the link collision body three-level offline construction module; the link collision body three-level offline construction module and the self-collision pair and normal contact pair elimination module complete offline calculations and output the three-level collision body configuration and detection pair whitelist for online module calls; During the online phase, joint encoder data is sent to the joint state time alignment and forward kinematics calculation module, and the link pose output by the joint state time alignment and forward kinematics calculation module is sent to the whole body collision coordinate transformation broadcast module; environmental perception and pose estimation data are sent to the sliding window occupancy map construction module, and map data are sequentially sent to the probability fusion and sensor noise margin expansion module and the unknown voxel sub-part processing module. The collider pose information output by the whole-body collider coordinate transformation broadcast module and the voxel processing information output by the unknown voxel sub-part processing module are jointly sent to the whole-machine enclosing sphere and sub-chain envelope capsule grading coarse screening module; the risk sub-chain information output by the coarse screening is sent to the region of interest voxel greedy merging and collision object construction module. The greedy merging and collision object construction module of the region of interest voxels outputs a cuboid block and constructs it as a collision object. This cuboid block, along with the detection pair whitelist obtained offline, is sent to the signed distance and nearest point query module for each link. The distance query result is input into the kinematic sub-chain branching module; The distance results are distributed to the corresponding groups of data by the kinematic subchain distribution module to the body velocity directional scaling module and the upper limb joint obstacle avoidance inequality construction and null space solution module, respectively; the output constraints of the two modules are jointly input to the whole-body obstacle avoidance arbitration and failure degradation module; the whole-body obstacle avoidance arbitration and failure degradation module sends the final execution command to the external foot and upper limb execution units, and at the same time drives the status and part-by-part minimum distance reporting module to report the system status to the outside.

Images