A real-time obstacle avoidance motion control method and system for a humanoid robot

Abstract

A humanoid robot real-time obstacle avoidance motion control method and system based on collision distance field self-filtering and fixed-length obstacle pool, through real-time joint configuration, a collision distance field is constructed to eliminate false obstacle avoidance constraints by bulk removing point clouds from the body measurement points; a fixed capacity obstacle pool is designed to match a far end sentinel filling strategy, an obstacle tensor with constant size is output to an instant compilation solver, and obstacle dynamic updating does not need to repeat the compilation computation graph; a double-layer collision model is constructed to balance self-collision safety and point cloud query computing power; the end of the double-arm tracking, point cloud obstacle avoidance and self-collision constraint are integrated into the same kinematics optimization target, and a low-frequency planning and high-frequency speed limiting interpolation double-layer time sequence architecture is adopted to output smooth joint instructions. The present application solves the defects of the prior art such as self-body point interference, severe delay fluctuation and excessive computing power consumption, and can be widely applied to humanoid robots, wheeled mobile manipulator dynamic scene real-time obstacle avoidance operation.

Description

Technical Field

[0001] This invention belongs to the field of robot perception fusion and motion control technology, specifically involving a control method and system for real-time obstacle avoidance and synchronous end-effector tracking of a humanoid robot by performing self-entity filtering on a sensed 3D point cloud, combining fixed tensor input of a fixed-length obstacle pool, and performing kinematic optimization solution by just-in-time compilation. Background Technology

[0002] When humanoid and mobile manipulation robots operate in complex and dynamic scenarios, they need to simultaneously perform end-effector pose tracking and obstacle avoidance in the entire environment. Current mainstream technologies unify obstacle avoidance, end-effector tracking, and self-constraint into a nonlinear least-squares kinematics optimization problem, relying on just-in-time (JIT) compilation to accelerate iterative solutions and output joint control commands in real time; environmental perception relies on depth cameras and LiDAR to output 3D point clouds online as obstacle input.

[0003] When existing technologies integrate online point clouds into the JIT optimization solution loop, the onboard point cloud contains self-measured points from the robot's torso, arms, and head. Directly participating in the optimization will generate false exclusion constraints, causing unreasonable movements such as robot shaking, tracking failure, and self-avoidance. The number of effective obstacle points per frame changes dynamically with scene occlusion and sampling strategies, and the input tensor size is not fixed. This continuously triggers the recompilation of the JIT computation graph, resulting in drastic fluctuations in planning latency, which cannot meet the requirements of millisecond-level real-time control. Self-collision detection requires a complete link-pairing geometric model. If batch distance calculations with the same accuracy are used for dense environmental point clouds, the computing power overhead increases dramatically with the product of the number of links and obstacles, crowding out the computing power for single-cycle planning.

[0004] For example, Songyan Power disclosed a layered AI navigation and obstacle avoidance method for humanoid robots based on depth information in Chinese patent CN119356318A. The upper-layer reinforcement learning model takes various information about the humanoid robot as input and outputs speed commands, which are then used as input to the lower-layer reinforcement learning model. The lower-layer model outputs the torques of each joint and controls the robot's movement. It also estimates the current speed and angular velocity of the humanoid robot through an estimation network, fusing this information with the robot's body information during deployment to estimate the robot's real-time position. By setting a layered architecture, the navigation and obstacle avoidance capabilities of the humanoid robot can be almost decoupled from its motion capabilities, and the networks can be trained separately. The lower-layer reinforcement learning model estimates the speed with high accuracy, even without external positioning, and can estimate the robot's speed and position. The upper-layer reinforcement learning model then adjusts the speed through analysis of visual images, ultimately achieving obstacle avoidance. While this solution achieves obstacle avoidance through layered reinforcement learning, the model has significant room for optimization.

[0005] Chinese patent document CN113934219B discloses an automatic obstacle avoidance method for robots. The method involves: acquiring the robot's initial coordinates and the target coordinates of the target point; initializing a particle filter and determining the initial coordinates of each particle; calculating the coordinates of each obstacle at the current moment; moving the particles a specific distance in a random direction and determining the coordinates of each particle after the movement; calculating the repulsive force field between each particle and each obstacle, and the gravitational field with the target point; determining the weight of each particle; determining the coordinates the robot will reach in the next moment; determining whether the robot has reached the target point; if yes, the process ends; otherwise, the determined coordinates of the robot reaching in the next moment are used as the robot's current coordinates, and the process continues to determine the coordinates of the robot reaching in the next moment. This technical solution combines particle filtering and artificial potential fields to avoid local extrema and target unreachability problems, effectively achieving automatic obstacle avoidance for robots. However, from the perspective of real-time performance and stability requirements, this technical solution lacks any deterministic advantage.

[0006] However, there is an urgent need in existing technologies for a technical solution that can simultaneously resolve the three major contradictions of self-generated false obstacles, JIT compilation steady state, and computing power balance, and can be engineered to implement a closed-loop system for real-time obstacle avoidance in humanoid robots across the entire domain. Summary of the Invention

[0007] To address the shortcomings of existing technologies, such as self-point cloud interference, repeated compilation triggered by obstacle tensor dynamics, and imbalance between self-collision and environmental query computing power, this invention aims to provide a real-time obstacle avoidance motion control method and system for humanoid robots based on collision distance field self-filtering and a fixed-length obstacle pool.

[0008] According to the present invention, it is possible to eliminate false obstacle constraints by batch eliminating self-body measurement points based on the collision distance field; by combining a fixed-capacity obstacle pool with a sentinel filling strategy, the shape of the obstacle geometric tensor input to the solver is kept constant, and the obstacle content update does not require recompiling the computation graph; the double-precision collision model separates self-collision and environmental point cloud query tasks, balancing computing power consumption; and integrates end-point tracking, point cloud obstacle avoidance, and self-collision constraints into the same optimization objective, stably outputting speed-limiting joint control commands to form a complete real-time control closed loop.

[0009] According to the present invention, a real-time obstacle avoidance motion control method for a humanoid robot based on collision distance field self-filtering and a fixed-length obstacle pool is provided, comprising the following steps: Load the robot's dual-collision model, periodically collect the current joint configuration q and the 3D sensor point cloud, and unify the coordinates of the non-world coordinate system point cloud through forward kinematics; Construct a collision distance field based on the robot's current configuration, batch query the minimum distance between the point cloud and the robot links, and remove points with a distance less than the self-filtering margin m. sf By measuring the self-entity points, a clean environmental point cloud is obtained; Pre-allocated capacity N max A fixed-length obstacle pool is used. Environmental point clouds are preprocessed and truncated before being filled into the pool. Empty slots are filled with the poses of remote sentinels. The output is a batch of obstacle spherical geometry with constant size. Construct a kinematic optimization objective that integrates the end-effector pose tracking cost and the point cloud obstacle avoidance soft constraint cost, and reuse a pre-compiled fixed-size computation graph to iteratively solve for the target joint configuration. ; At a control frequency higher than that used in the planning solution, the target joint Perform speed-limited step interpolation; Output the speed-limited joint control commands to the underlying servo driver.

[0010] According to the present invention, a real-time obstacle avoidance motion control system for humanoid robots based on collision distance field self-filtering and a fixed-length obstacle pool is also provided. The system includes a point cloud access and coordinate transformation module, a dual collision model management module, a point cloud self-filtering module, a fixed-length obstacle pool module, a kinematic optimization solution module, an instruction output module, and a communication module. The modules are connected in series to form a complete closed loop of perception preprocessing, steady-state obstacle encoding, JIT optimization, and speed limit instruction issuance, which supports the operation of the above control method.

[0011] Specifically, according to one aspect of the present invention, a real-time obstacle avoidance motion control method for a humanoid robot based on collision distance field self-filtering and a fixed-length obstacle pool is provided, characterized by comprising the following steps: Obtain the current joint configuration q of the humanoid robot and the 3D sensor point cloud; if the point cloud coordinate system is not the planned world coordinate system, complete the coordinate alignment through the robot's forward kinematics. A real-time collision distance field is constructed based on a pre-built robot collision model. The point cloud is segmented and encapsulated into batch query spheres, and the minimum distance d between each measurement point and the robot link is calculated in batches. i , will satisfy d i ≤m sf m sf Self-body measurement points with preset self-filtering margin are removed, while environmental point clouds are retained; The maximum pre-allocated capacity is N max A fixed-length obstacle pool is used to filter and downsample the environmental point cloud before filling it with the sentinel poses that are far from the robot's working space, without occupying any slots. The output size is constant at N. max Batch spherical obstacle geometry; Using the current joint configuration q as the initial value for iteration, the kinematic optimization problem is solved by reusing the fixed-size computation graph compiled in real time. The optimization objective includes at least the end-effector pose tracking cost and the soft constraint cost of point cloud obstacle avoidance based on batch spherical obstacle geometry. The contribution of sentry pose in the obstacle pool to the optimization cost is negligible. The obstacle content is updated online without recompiling the computation graph. The target joint configuration is output. ; Set the planning and solving frequency to be lower than the control output frequency, and configure the target joints according to the speed limits of each joint within multiple consecutive control cycles. Step interpolation speed limiter generates smooth joint control commands and outputs them to the underlying servo.

[0012] Preferably, two sets of differentiated collision models are pre-built: the first collision model covers all non-adjacent link pairs of the robot and is used for self-collision cost calculation; the second collision model is a lightweight subset obtained by trimming internal redundant links from the first collision model and is used only for batch distance queries of point clouds.

[0013] Preferably, the environmental point cloud is preprocessed at least once before being filled into the fixed-length obstacle pool: maximum distance filtering, minimum height filtering, and voxel downsampling; after processing, the number of effective obstacle points exceeds N. max At that time, truncate and retain the N closest to the robot. max There are 10 measurement points.

[0014] The point cloud obstacle avoidance soft constraint is set with a fixed safety margin, and the constraint cost is set with an amplitude capping threshold to prevent the optimization value from overflowing under extreme obstacles.

[0015] Preferably, the kinematic optimization objective is further superimposed with any one or more of the following costs: self-collision constraint cost, ground static geometric collision cost, and robot nominal pose regularization cost.

[0016] Preferably, the humanoid robot includes a torso and two left and right manipulators. The end-effector pose tracking cost synchronously constrains the desired pose of the end-effectors of the left and right manipulators, thereby achieving synchronous tracking of the two arms and obstacle avoidance across the entire domain.

[0017] Preferably, the sentinel pose coordinates are set at a distance of hundreds of meters outside the robot's working space, and the minimum distance between the sentinel spherical obstacle and the robot's linkage is much greater than the obstacle avoidance safety margin, so that no exclusionary constraints are generated during the optimization iteration.

[0018] Furthermore, according to another aspect of the present invention, a control system for implementing the above-described control method is also provided, comprising: The point cloud access and coordinate transformation module is used to collect sensor point clouds and joint states and complete multi-coordinate system transformation; The dual-collision model management module is used to store and call the first collision model dedicated to self-collision and the second collision model dedicated to point cloud query. The point cloud self-filtering module removes self-body measurement points in the point cloud in batches based on the collision distance field; Fixed-length obstacle pool module, fixed capacity storage of environmental obstacles, filling of sentinel poses in empty slots, outputting batch obstacle geometry with constant size; The kinematics optimization solution module loads a just-in-time compiled fixed-size calculation graph and integrates multi-cost solutions for the target joint configuration. The command output module outputs smooth joint servo commands by interpolating at a control frequency higher than the planned frequency. The communication module is used for the transmission and interaction of joint data, point clouds, optimization parameters, and control commands between various modules.

[0019] Preferably, the system is deployed on a CPU or GPU computing unit, and the 3D point cloud output from the depth camera and LiDAR can be used as the module input data source.

[0020] In addition, according to another aspect of the present invention, a computer-readable storage medium is provided having a computer program stored thereon, which, when executed by a processor, implements all the steps of the above-described method for real-time obstacle avoidance motion control of a humanoid robot based on collision distance field self-filtering and a fixed-length obstacle pool.

[0021] According to the present invention, at least the following beneficial effects are achieved: By calculating the minimum distance between measurement points and robot links based on real-time joint configuration, the system accurately distinguishes between self-reflection points and environmental obstacles, completely eliminating false obstacle avoidance behaviors such as self-avoidance and motion jitter, thus significantly improving motion stability. Regardless of the dynamic changes in the number of environmental obstacles in each frame, the output obstacle tensor size remains constant. The JIT computation graph is compiled only once and reused throughout the entire process, eliminating the drastic latency fluctuations caused by repeated compilation. The single-cycle planning latency is deterministic, meeting the requirements for millisecond-level real-time robot control. A complete model is used for self-collision safety, and a lightweight subset is used for batch distance queries of massive point clouds, significantly reducing the computational cost of distance calculations. This system improves the upper limit of planning and solving frequency under the same hardware conditions; it integrates end-effector tracking, point cloud obstacle avoidance, and self-collision constraints into a single least-squares objective, avoiding trajectory conflicts caused by hierarchical control, and simultaneously ensuring end-effector tracking accuracy and global obstacle avoidance safety; low-frequency planning and solving, and high-frequency smooth interpolation significantly reduce servo impact, resulting in continuous and smooth motion trajectories, adapting to the multi-degree-of-freedom flexible motion requirements of humanoid robots; it is compatible with various 3D sensors such as depth cameras and LiDAR, and is compatible with single-arm, dual-arm, full-body humanoid, and wheeled mobile humanoid robot hardware, and can be deployed on CPU / GPU just-in-time compilation devices, with low engineering porting costs. It can simultaneously solve the problems of self-generated false obstacles, JIT compilation steady state, and computing power balance, and can be engineered to implement a closed-loop deployment of global real-time obstacle avoidance for humanoid robots. Attached Figure Description

[0022] Figure 1 This is to illustrate the overall flowchart of a real-time obstacle avoidance motion control method for humanoid robots based on collision distance field self-filtering and a fixed-length obstacle pool according to an embodiment of the present invention. Figure 2This is a schematic diagram of the fixed-length obstacle pool structure of the real-time obstacle avoidance motion control method for humanoid robots based on collision distance field self-filtering and fixed-length obstacle pool according to an embodiment of the present invention. Figure 3 This is a schematic diagram illustrating the point cloud self-body filtering principle based on the collision distance field, which is part of the real-time obstacle avoidance motion control method for humanoid robots based on collision distance field self-filtering and fixed-length obstacle pool according to an embodiment of the present invention. Figure 4 This is a schematic diagram illustrating the linkage filtering relationship between the first and second sets of collision models in a real-time obstacle avoidance motion control method for humanoid robots based on collision distance field self-filtering and a fixed-length obstacle pool according to an embodiment of the present invention. Figure 5 This is a block diagram illustrating the overall module composition of a real-time obstacle avoidance motion control system for a humanoid robot based on collision distance field self-filtering and a fixed-length obstacle pool, according to an embodiment of the present invention. Figure 6 This diagram illustrates the timing relationship between the planning and solving frequency and the underlying command output cycle of a real-time obstacle avoidance motion control system for a humanoid robot based on collision distance field self-filtering and a fixed-length obstacle pool, according to an embodiment of the present invention. Detailed Implementation

[0023] The present invention will now be described in detail with reference to the accompanying drawings and specific embodiments. Those skilled in the art will understand that this description is exemplary and that the present invention is not limited to these specific embodiments.

[0024] Figure 1 This is a flowchart illustrating the overall process of a specific embodiment of the present invention, as follows: Figure 1 The diagram shown is a linear sequential operation flowchart, including: Step 1: Obtain the joint configuration q and the sensor point cloud; Step 2: Coordinate system alignment. Non-world coordinate system point clouds undergo rigid body coordinate transformation through robot forward kinematics. Step 3: Perform point cloud self-body filtering based on the collision distance field to remove the robot's own measurement points; Step 4: Update the fixed-length obstacle pool, fill the empty positions with sentry poses, and ensure that the shape of the output obstacle geometry tensor remains constant. Step 5: Just-in-time compilation of kinematics optimization, external input of end-effector desired pose, optimization of target fusion end-effector tracking cost and point cloud collision cost, and direct reuse of computation graph when obstacle content is updated online, without recompiling; Step 6: Generate and output joint control commands.

[0025] Obstacle content can be dynamically updated online, and a fixed tensor size enables computation graph reuse, eliminating latency jitter from repeated compilation. The overall process fully covers the entire chain from perception preprocessing and optimization to instruction output in a single planning cycle.

[0026] Figure 2 This is a schematic diagram of a fixed-length obstacle pool structure, as shown below. Figure 2 As shown, the total fixed capacity of the fixed-length obstacle pool storage structure is denoted as N. max The slots are divided into two categories: the first k slots are filled with the effective environmental obstacle centers (marked by solid black dots) after self-filtering and downsampling; the remaining k+1 to N slots are filled with the effective environmental obstacle centers (marked by solid black dots). max Unoccupied slots are uniformly filled with sentry poses (hollow circles, cross markers) located away from the robot's working space. Regardless of the number of effective obstacles k, the uniform output length remains constant at N. max The batch spherical obstacle geometry tensor.

[0027] Unoccupied slots are filled with remote sentinels. The sentinels are extremely far from the robot and their contribution to the optimization cost function is close to 0. This ensures that the input tensor size is completely fixed and does not interfere with the obstacle avoidance solution.

[0028] Figure 3 This is a schematic diagram of the point cloud self-entity filtering principle based on the collision distance field. The humanoid link on the left is the robot's collision geometry model. The solid black circles represent valid measurement points in the environment, and the hollow circles with crosses represent self-entity measurement points to be removed. Each dashed line marks the minimum distance d from the measurement point to the robot link. i m sf This is a preset self-filtering margin threshold. Judgment rule: If the minimum distance d between the measurement points... i ≤m sf If the point is identified as a reflection point of the robot body, it will be directly eliminated; d i >m sf Retained as environmental obstacle points. The small box in the lower right corner is a schematic of the batch query unit, which unifies the segmented point cloud into batch query spheres, calculates distances in batches in parallel, and improves the efficiency of filtering computing power; the whole relies on the current joint configuration to generate a global collision distance field, distinguishing the body / environment measurement points at one time, and eliminating false obstacle avoidance constraints from the source.

[0029] Figure 4This diagram illustrates the relationship between the first and second collision models. M1 on the left represents the first collision model (dedicated to self-collision), covering all non-adjacent link pairs on the robot. All links in the dashed lines participate in self-collision distance detection, ensuring high collision geometry completeness and guaranteeing safety during collisions between the robot body. M2 on the right represents the second collision model (dedicated to environmental point cloud queries). It is a subset obtained by removing redundant links from M1 according to link selection rules. Links in the gray dashed lines do not participate in batch point cloud distance queries; only the robot's outer operating links are retained for obstacle distance calculation. The arrows represent the model generation logic. Based on the complete self-collision model, redundant internal links are trimmed to obtain a lightweight query subset, achieving the dual goals of high-precision self-collision safety verification and low-computing-power batch environmental point cloud queries, balancing safety and real-time computing power.

[0030] Figure 5 The system module block diagram shows the main data flow: sensor point / joint status input, point cloud access and coordinate transformation module, point cloud self-filtering module, fixed-length obstacle pool module, kinematics optimization solution module, command output module, and joint control command output; Auxiliary support link 1: the dual collision model management module (dashed line) connects the point cloud self-filtering module and the kinematics optimization solution module, providing two sets of geometric models for filtering distance calculation and optimizing self-collision cost; Auxiliary support link 2: the communication module (dashed line) connects all business modules, responsible for cross-unit transmission of joint status, point cloud data, optimization parameters, and control commands; External input: the desired pose of the end effector is separately connected to the kinematics optimization solution module as the benchmark for end effector tracking cost.

[0031] All modules work together to form a complete hardware and software closed-loop system encompassing perception, preprocessing, optimization, and instruction output.

[0032] Figure 6 This diagram illustrates the timing relationship between the planning and solving frequency and the control command output cycle. The upper timing axis represents the planning and solving frequency T. plan (10~50Hz): Periodically initiate optimization solution, complete obstacle pool input and JIT iteration calculation within a single cycle, and output the target joint configuration. The lower timing axis is the underlying control output cycle T. ctrl (50~200Hz), meets T ctrl <T plan The control frequency is significantly higher than the planning frequency; temporal logic: a single planning operation yields a set of objectives. Subsequently, within multiple consecutive control cycles, the target pose is approximated by interpolating and limiting the speed according to the upper limit of the speed of each joint, avoiding the impact of single-step large-angle commands on the servo. The figure intuitively shows the dual-layer timing control architecture of low-frequency planning and high-frequency smoothing, which improves motion smoothness.

[0033] The invention will be further described in detail below using specific embodiments such as a full-body wheeled humanoid robot.

[0034] Example 1: Full-body wheeled humanoid robot The controlled object is a wheeled humanoid robot with a mobile chassis, torso, and left and right arms, which performs tasks such as end-effector tracking and dynamic obstacle avoidance in material handling. The control steps are as follows: Figure 1 As shown.

[0035] Model loading: Generate the M1 complete self-collision model and the M2 lightweight point cloud query model using the URDF structure file; Data acquisition: The depth camera point cloud and joint encoder angles are read at a 10Hz cycle. The point cloud is in the local coordinate system of the camera and is transformed to the world planning coordinate system through the robot's forward kinematics. Collision distance field self-filtering: point cloud block size 2048, query sphere radius 0.001m, self-filtering margin m sf =0.03m, batch calculate the minimum link distance of the measuring points, and remove the arms and torsos from the body points with a distance of less than 0.03m; Fixed-length obstacle pool coding: Maximum pool capacity N max =512, the environmental point cloud performs a minimum height filter of 0.1m and voxel downsampling of 0.02m, the effective points are truncated to within 512, the empty slots are filled with the far sentinel at coordinates (100,100,100), and the output is a 512-dimensional fixed spherical obstacle tensor. JIT kinematics optimization: Input the desired poses of the left and right arm end-effectors. The optimization objectives include end-effector tracking least-squares cost, 0.05m safety margin point cloud obstacle avoidance soft constraint, and self-collision cost. Reuse a pre-compiled fixed-size computation graph and solve the target joints using Gauss-Newton iteration. ; Dual-layer timing command issuance: planning cycle 20Hz, bottom layer control output frequency 100Hz, each joint speed limited by interpolation step of 0.8rad / s maximum angular velocity, and group output of torso, left arm and right arm servo commands.

[0036] Example 2: Fixed base single operating arm The chassis movement degree of freedom is cancelled, and only the pose of a single end effector is tracked; the entire process of dual collision model, point cloud self-filtering, fixed-length obstacle pool, JIT reuse, and speed limit command is consistent with Example 1, only the target is optimized to retain the cost of single-arm end effector tracking, which is suitable for fixed robotic arm scenarios.

[0037] Example 3: ESDF-based range field Based on the spherical tensor output by the fixed-length obstacle pool, the Euclidean symbolic distance field is updated simultaneously, and the collision cost of the distance field is superimposed on the optimization target. The self-filtering and sentinel fixed-length preprocessing processes remain unchanged, taking into account the dual advantages of fast calculation for sparse spherical fields and high-precision obstacle avoidance for dense distance fields.

[0038] The key implementation parameters are summarized in Table 1: Table 1

[0039] In summary, the present invention has been described in detail through specific embodiments. However, those skilled in the art will understand that various modifications and changes can be made. As long as they do not depart from the spirit and purpose of the present invention, all such modifications and changes should fall within the protection scope of the present invention, which is defined by the appended claims.

Claims

1. A real-time obstacle avoidance motion control method for a humanoid robot based on collision distance field self-filtering and a fixed-length obstacle pool, characterized in that, include: Acquire the robot's current joint configuration and sensor point cloud data; Based on the current joint configuration and robot collision model, perform self-entity filtering on the point cloud: take the points in the point cloud as query geometry, calculate their distance relative to the robot collision geometry, and remove points whose distance is less than the preset self-filter margin to obtain the environment point cloud. Update the environmental point cloud to a pre-allocated fixed-length obstacle pool; The fixed-length obstacle pool has a fixed maximum number of obstacles; unoccupied positions in the pool are filled with preset sentry poses so that the shape of the output obstacle geometry remains fixed. Using the current joint configuration as the initial value, the kinematic optimization problem is solved to obtain the target joint configuration; The kinematic optimization problem includes at least the end-effector pose tracking cost and the point cloud collision cost based on the geometry of the fixed-shape obstacle; The solution is performed on a just-in-time compiled computational graph, and the computational graph is reused when the obstacle content is updated online. Joint control commands are generated and output based on the target joint configuration.

2. The method as described in claim 1, characterized in that, In the steps described: Obtain the current joint configuration q of the humanoid robot and the 3D sensor point cloud; if the point cloud coordinate system is not the planned world coordinate system, complete the coordinate alignment through the robot's forward kinematics. A real-time collision distance field is constructed based on a pre-built robot collision model. The point cloud is segmented and encapsulated into batch query spheres, and the minimum distance d between each measurement point and the robot link is calculated in batches. i , will satisfy d i ≤m sf Self-body measurement point removal, retaining environmental point cloud, where m sf This is a preset self-filtering margin; The maximum pre-allocated capacity is N max A fixed-length obstacle pool is used to filter and downsample the environmental point cloud before filling it. Unoccupied slots in the pool are filled with preset sentinel poses that are far from the robot's working space. The output size is constant at N. max Batch spherical obstacle geometry; Using the current joint configuration q as the initial value for iteration, the kinematic optimization problem is solved by reusing the fixed-size computation graph compiled in just-in-time. The point cloud collision cost is the soft constraint cost for point cloud obstacle avoidance based on batch spherical obstacle geometry. The target joint configuration is then output. ; Set the planning and solving frequency to be lower than the control output frequency, and configure the target joints according to the speed limits of each joint within multiple consecutive control cycles. Step interpolation speed limiter generates smooth joint control commands and outputs them to the underlying servo.

3. The method as described in claim 2, characterized in that, Two differentiated collision models are pre-built: the first collision model covers all non-adjacent link pairs of the robot and is used for self-collision cost calculation; the second collision model is a lightweight subset obtained by trimming internal redundant links from the first collision model and is used only for batch distance queries of point clouds.

4. The method as described in claim 2, characterized in that, Before filling the fixed-length obstacle pool, perform at least one preprocessing step on the environmental point cloud: maximum distance filtering, minimum height filtering, and voxel downsampling; after processing, the number of effective obstacle points exceeds N. max At that time, truncate and retain the N closest to the robot. max For each measurement point, the soft constraint cost for obstacle avoidance in the point cloud is set with a fixed safety margin, and the constraint cost is set with an amplitude capping threshold.

5. The method as described in claim 2, characterized in that, The kinematic optimization objective is further superimposed with any one or more of the following costs: self-collision constraint cost, ground static geometric collision cost, and robot nominal pose regularization cost.

6. The method according to any one of claims 1 to 5, characterized in that, The humanoid robot includes a torso and two left and right manipulators. The end-effector pose tracking cost synchronously constrains the desired pose of the left and right manipulators, enabling synchronous tracking of both arms and obstacle avoidance across the entire domain.

7. The method as described in claim 6, characterized in that, The sentinel's pose coordinates are set at a distance of hundreds of meters outside the robot's working space, and the minimum distance between the sentinel's spherical obstacle and the robot's linkage is much greater than the obstacle avoidance safety margin.

8. A control system for implementing the control method of any one of claims 1-7, comprising: The point cloud access and coordinate transformation module is used to collect sensor point clouds and joint states and complete multi-coordinate system transformation; The dual-collision model management module is used to store and call the first collision model dedicated to self-collision and the second collision model dedicated to point cloud query. The point cloud self-filtering module removes self-body measurement points in the point cloud in batches based on the collision distance field; Fixed-length obstacle pool module, fixed capacity storage of environmental obstacles, filling of sentinel poses in empty slots, outputting batch obstacle geometry with constant size; The kinematics optimization solution module loads a just-in-time compiled fixed-size calculation graph and integrates multi-cost solutions for the target joint configuration. The command output module outputs smooth joint servo commands by interpolating at a control frequency higher than the planned frequency. The communication module is used for the transmission and interaction of joint data, point clouds, optimization parameters, and control commands between various modules.

9. A real-time obstacle avoidance motion control system for a humanoid robot based on collision distance field self-filtering and a fixed-length obstacle pool as described in claim 8, characterized in that, The system is deployed on a CPU or GPU computing unit, and the 3D point cloud output from depth cameras and LiDAR can be used as the module's input data source.

10. A computer-readable storage medium having a computer program stored thereon, the program being executed by a processor to implement all the steps of the above-described method for real-time obstacle avoidance motion control of a humanoid robot based on collision distance field self-filtering and a fixed-length obstacle pool.

Images