Foot type humanoid robot control method based on point cloud clustering

Abstract

The invention discloses a foot type humanoid robot control method based on point cloud clustering, and relates to the technical field of robots, and the method comprises the steps: carrying out the pedestrian clustering and cross-frame association of non-ground point clouds, merging the non-ground point clouds into a motion group, constructing a motion trend flow field containing an average motion vector and space density distribution, and carrying out the clustering of the non-ground point clouds; a control decision is changed from sensitive prediction of individual trajectories to structured expression of group flow direction and density boundaries, so that dependence on future accurate positions of single pedestrians is reduced, and traffic continuity and decision stability at bottleneck intersections are improved. Performing distinguishing processing on short-time abnormal fluctuation and continuous distortion through a time window of flow field confidence and hysteresis judgment; and when the confidence is reduced or the association fails continuously, the corresponding group is switched to a quasi-static obstacle and the trend compliance weight is reduced, so that the controller automatically converges to a more conservative, explainable and enforceable obstacle avoidance mode under the condition of low credible information, and abrupt interpenetration or repeated correction caused by a wrong trend is reduced.

Description

Technical Field

[0001] This invention relates to the field of robotics, and in particular to a control method for a legged humanoid robot based on point cloud clustering. Background Technology

[0002] Legged humanoid robots are capable of traversing uneven ground, steps, and other natural terrains. When performing guidance, inspection, and material delivery in public spaces such as airports and train stations, they often need to move continuously through dense crowds with two-way flow while ensuring pedestrian comfort and safety. Existing systems typically adopt a layered architecture: the bottom layer is for walking stability and gait generation, and the upper layer is for local obstacle avoidance and path decision-making based on environmental perception. In the gait stability layer, metrics such as the zero moment point (ZMP) are widely used for gait generation and stability constraint control. The typical paradigm is to first generate a reference gait that satisfies the support polygon / stability margin, and then the controller tracks and executes it. Early fine dynamics programming often deviated from the linear implementation due to the computational burden, while online methods often introduce simplified models such as linear inverted pendulums to improve real-time performance, but this brings problems such as model inconsistency and limited stability margin.

[0003] In crowd navigation, common methods include prediction-planning-based optimization control (such as MPC or its variants) and learning-based policy methods (such as deep reinforcement learning). When crowds converge to form dynamic bottlenecks, if simplified predictions such as constant speed are used or the interaction between pedestrians and robots is decoupled and modeled, the planning results are prone to deviate from the subsequent avoidance behavior of pedestrians, leading to sudden stops and frequent replanning. Under dense interaction, even with predictive information, the planner may still experience the "robot freeze problem" due to conservatism, resulting in long periods of waiting in place and decreased task efficiency.

[0004] To alleviate the above problems, some solutions introduce social force models, ORCA / RVO and other interactive hypothesis models or interactive MPC to enhance the description of pedestrian avoidance. However, the assumptions and parameter sensitivity of such models, such as "cooperation with others" and "omniscient perception", may not hold true in real-world scenarios, and there are still uncertainties due to occlusion and real-time computing power limitations. Learning methods can obtain a certain level of crowd passage ability through reward design and simulation training, but they still face engineering implementation problems such as simulation to reality transfer and insufficient policy interpretability.

[0005] Furthermore, the aforementioned planning and control effects are highly dependent on the stable perception of surrounding pedestrians and obstacles. In public spaces, 3D LiDAR point clouds are often used for pedestrian / obstacle detection because they are unaffected by lighting. Point cloud clustering (such as Euclidean clustering, DBSCAN, etc.) is a common method for constructing obstacle candidates. However, in dense crowds, problems such as target fragmentation caused by sparse point clouds and occlusion, adhesion of adjacent pedestrian clusters, and increased computational overhead can easily occur, making the upper-level decision input unstable and inducing undesirable behaviors such as sudden stops, freezing, or abrupt insertion. Therefore, there is still a need for a control method that can stably extract passable structures and couple them with a foot-based control closed loop under crowded dynamic crowd point cloud conditions. Summary of the Invention

[0006] In view of the aforementioned existing problems, the present invention is proposed.

[0007] This invention provides a control method for legged humanoid robots based on point cloud clustering to solve the problems of legged robots easily colliding and stopping abruptly, frequently replanning and freezing, or abruptly weaving in crowded areas.

[0008] To solve the above-mentioned technical problems, the present invention provides the following technical solution: This invention provides a control method for a legged humanoid robot based on point cloud clustering. The robot is equipped with a depth sensor, an inertial measurement unit, and a plantar pressure sensor, and includes: Step S1: Obtain the environmental point cloud of the depth sensor and filter out the ground point cloud according to the ground segmentation rules; Step S2: Determine the clustering and segmentation of the non-ground point cloud and obtain the pedestrian cluster set according to the pedestrian screening rules; Step S3: Calculate the data association of pedestrian clusters in multiple consecutive frames to obtain the motion vector of each cluster. The motion vector is a velocity vector that represents the motion speed and direction of the pedestrian cluster. Step S4: Generate a motion group by clustering pedestrians according to a distance threshold and a motion similarity threshold, and generate a motion trend flow field for each motion group that includes an average motion vector and a spatial density distribution. The motion similarity threshold includes a velocity angle threshold and a velocity difference threshold. Step S5: Determine the flow field confidence level of each motion group based on the motion trend flow field calculation. The flow field confidence level is a confidence index characterizing the motion consistency and spatial clustering of the motion group. When the flow field confidence level is lower than the confidence level threshold or the number of consecutive association failures exceeds the number threshold, switch the corresponding group to quasi-static obstacle processing. The quasi-static obstacle processing involves using the occupied area of ​​the corresponding group as the obstacle avoidance constraint input to the model prediction controller. Reduce the trend compliance weight, which is the weight of the trend compliance cost term in the comprehensive cost. Step S6: Output the control sequence that solves the motion trend flow field, the robot body state acquired by the inertial measurement unit, and the global navigation target input model prediction controller in the prediction time domain, satisfying the dynamic constraints and obstacle avoidance constraints and minimizing the overall cost; the control sequence is a robot motion reference sequence or a joint control input sequence. Step S7: Issue the first control command to execute the control sequence, wherein the control command is a motion reference value command or a joint control input command; Step S8: Update the body state based on the contact feedback from the plantar pressure sensor, and repeat steps S1 to S7.

[0009] As a preferred embodiment of the point cloud clustering-based control method for a legged humanoid robot described in this invention, the clustering segmentation adopts either Euclidean clustering or density clustering, and the pedestrian screening rules include a height interval threshold and a point count interval threshold. Point cloud clusters that satisfy the thresholds are determined as pedestrian clusters.

[0010] As a preferred embodiment of the point cloud clustering-based control method for a legged humanoid robot described in this invention, the data association includes: calculating the centroid displacement of pedestrian clusters in adjacent frames within a time window and converting it into a velocity vector; matching based on a distance threshold and a velocity threshold; and processing clusters that fail to match according to a loss counting rule and using them for the number of times threshold determination.

[0011] As a preferred embodiment of the point cloud clustering-based control method for a legged humanoid robot described in this invention, the merging of motion groups satisfies the following conditions: when the centroid distance between two humanoid clusters is ≤ the distance threshold, the velocity angle is ≤ the velocity angle threshold, and the velocity difference is ≤ the velocity difference threshold, they are merged into the same motion group; if these conditions are not met, they remain as different motion groups.

[0012] As a preferred embodiment of the point cloud clustering-based control method for a legged humanoid robot described in this invention, the average motion vector is calculated according to a point weighting rule, and the spatial density distribution is generated according to a grid size range or a kernel width range. The grid size or kernel width is selected by segmenting the point cloud sparsity using a mapping table; the point cloud sparsity is a statistical index of the point cloud density within a local planning area; the mapping table is a table showing the correspondence between point cloud sparsity segments and parameter intervals.

[0013] As a preferred embodiment of the point cloud clustering-based control method for a legged humanoid robot described in this invention, the flow field confidence is determined by both motion consistency and spatial clustering. The confidence level is increased when the velocity direction change within K consecutive frames is less than or equal to the direction threshold and the velocity change is less than or equal to the velocity change threshold. When the spatial clustering degree is less than or equal to the clustering dispersion threshold, the confidence level is increased. The motion consistency is calculated using a time window and hysteresis rules. The time window is a sliding window consisting of multiple consecutive frames. When K consecutive frames satisfy the condition that the velocity direction change does not exceed a direction threshold and the velocity change does not exceed a velocity change threshold, the flow field confidence level is increased. When a frame fails to meet either the direction threshold or the velocity change threshold, an anomaly count is accumulated without triggering a decrease in the flow field confidence level. When M consecutive frames corresponding to the anomaly count fail to meet the conditions, the flow field confidence level is decreased. Here, K and M are positive integers. The direction threshold, the velocity change threshold, and the lower limit of the trend compliance weight are selected by segmenting the point cloud according to the sparsity of the mapping table; the lower limit of the trend compliance weight is the minimum allowable value used when limiting the trend compliance weight.

[0014] As a preferred embodiment of the point cloud clustering-based control method for a legged humanoid robot described in this invention, the comprehensive cost includes at least: target tracking cost, density collision avoidance cost, and trend compliance cost; wherein the trend compliance cost is weighted in segments according to the angle between the robot's predicted motion direction and the average motion vector, and the penalty weight is increased when the angle is greater than or equal to the reverse motion threshold.

[0015] As a preferred embodiment of the point cloud clustering-based control method for a legged humanoid robot described in this invention, the density collision avoidance cost is constructed according to the repulsion potential field rule, the repulsion strength is positively correlated with the group density, and when the robot's predicted trajectory enters the region in the opposite direction of the group's movement direction, the repulsion strength increases with the depth of entry. The reverse direction region is the region where the robot's predicted motion direction and the average motion vector satisfy the reverse motion determination rule; the entry depth is the projection distance of the robot's predicted trajectory representative point into the group-occupied area along the reverse direction.

[0016] As a preferred embodiment of the point cloud clustering-based control method for a legged humanoid robot described in this invention, the optimization problem of the model predictive controller includes soft constraints on the foot landing point. The soft constraints encourage the landing point to be selected outside the boundary of a group with high confidence and a speed lower than the speed threshold, and a minimum landing distance threshold is set. The low-density region is extracted from the spatial density distribution. Grids with density values ​​less than the density threshold are regarded as low-density grids, and the connected domains of adjacent low-density grids are regarded as candidate low-density regions. The accessibility screening of the foot landing point is performed based on the reachable step size interval and the safety margin of the supporting polygon. When the relative distance between the candidate landing point and the supporting foot falls within the reachable step size interval and the minimum distance of the centroid projection to the boundary of the supporting polygon after landing is not less than the safety margin threshold, the candidate landing point is retained. When the candidate low-density region is empty, the soft constraint of the foot landing point remains the result of the previous cycle or is not enabled, and the optimization is dominated by obstacle avoidance constraints and density collision avoidance costs. The reachable step size interval is the allowable range of the horizontal distance and azimuth angle of the candidate landing point relative to the supporting foot.

[0017] As a preferred embodiment of the point cloud clustering-based control method for a legged humanoid robot described in this invention, when depth sensor data is missing or the number of associative pedestrian clusters is less than a threshold, the motion trend flow field remains the result of the previous cycle, and the trend compliance weight is limited to the weight range. The missing depth sensor data is due to at least one of the following: missing point cloud frames, insufficient number of valid point cloud points, or abnormal point cloud timestamps.

[0018] Through the above technical solution, the present invention can achieve at least the following beneficial effects: Addressing the planning conflicts and abrupt replanning loops caused by strong pedestrian interaction and high trajectory randomness at dynamic bottlenecks, this paper constructs a motion trend flow field containing average motion vectors and spatial density distribution by clustering pedestrians in non-ground point clouds, associating them across frames, and merging them into motion groups. This shifts control decisions from sensitive prediction of individual trajectories to a structured expression of group flow direction and density boundaries, thereby reducing reliance on the precise future location of individual pedestrians and improving traffic continuity and decision stability at bottleneck intersections.

[0019] To address trend misjudgment and control jitter caused by perception occlusion, cluster adhesion, or fragmentation in crowded scenarios, the system distinguishes between short-term abnormal fluctuations and continuous distortions by using a time window and hysteresis determination of flow field confidence. When confidence decreases or association failure persists, the system switches the corresponding group to a quasi-static obstacle and reduces the trend compliance weight, enabling the controller to automatically converge to a more conservative, interpretable, and implementable obstacle avoidance mode under low-confidence information conditions, thereby reducing abrupt interspersing or repeated corrections caused by erroneous trends.

[0020] To address the issue of freezing due to the need to actively cut into gaps to maintain forward movement, this paper considers target tracking, density collision avoidance, and trend compliance simultaneously in the model predictive control. Furthermore, it introduces a preference for low-density areas at the foot landing point layer. This makes the robot more inclined to choose passable gaps as landing and forward directions while meeting accessibility and stability margin constraints. As a result, the probability of prolonged waiting is reduced when feasible gaps exist, thus enhancing the task advancement capability.

[0021] To address the acceptability of public spaces and potential collision risks when traversing upstream / downstream, a cost-weighted approach and a depth-related repulsion enhancement mechanism are used to ensure that the robot maintains a higher cost and stronger avoidance tendency for high-density groups traversing upstream in the prediction time domain, while retaining accessibility to downstream channels. This guides the robot to develop a more aligned passage strategy with the movement trends of the crowd, reducing disturbance to pedestrians and unnecessary close-range interactions.

[0022] To address the execution deviations caused by changes in contact state and fluctuations in stability margin during foot walking, the system uses contact feedback from plantar pressure sensors to continuously update the body state and trigger the rolling solution of the control sequence. This ensures that the control decisions driven by the upper flow field and the contact stability of the lower layer form a closed loop, improving the execution robustness and control continuity under conditions of crowd disturbance, changes in ground friction, or gait phase switching. Attached Figure Description

[0023] To more clearly illustrate the technical solutions of the embodiments of this application, the accompanying drawings used in the embodiments will be briefly described below. It should be understood that the following drawings only show some embodiments of this application and should not be regarded as a limitation on the scope of this application.

[0024] Figure 1 This is a flowchart of the control method for a legged humanoid robot based on point cloud clustering in the embodiment. Detailed Implementation

[0025] To make the objectives, technical solutions, and advantages of this application clearer, the following detailed description is provided in conjunction with the accompanying drawings and embodiments. It should be understood that the specific embodiments described herein are merely illustrative and not intended to limit the scope of this application.

[0026] All terms used in this application (including technical and scientific terms) have the meanings commonly understood by those skilled in the art, unless otherwise defined. It should be noted that the terms used herein should be interpreted in a manner consistent with the context of this specification, and not in an idealized or overly rigid way.

[0027] Example 1: like Figure 1 As shown, this embodiment proposes a control method for a legged humanoid robot based on point cloud clustering. The robot is equipped with a depth sensor, an inertial measurement unit, and a plantar pressure sensor, including: Step S1: Obtain the environmental point cloud from the depth sensor and filter out the ground point cloud according to the ground segmentation rules; Step S2: Determine the clustering and segmentation of the non-ground point cloud and obtain the pedestrian cluster set according to the pedestrian screening rules; Step S3: Calculate the data association of pedestrian clusters in multiple consecutive frames to obtain the motion vector of each cluster. The motion vector is the velocity vector that represents the speed and direction of movement of the pedestrian cluster. Step S4: Generate a motion group by clustering pedestrians according to a distance threshold and a motion similarity threshold, and generate a motion trend flow field for each motion group that includes the average motion vector and spatial density distribution. The motion similarity threshold includes a velocity angle threshold and a velocity difference threshold. Step S5: Determine the flow field confidence level of each motion group based on the motion trend flow field calculation. The flow field confidence level is a confidence index characterizing the motion consistency and spatial clustering of the motion group. When the flow field confidence level is lower than the confidence level threshold or the number of consecutive association failures exceeds the number threshold, switch the corresponding group to quasi-static obstacle processing. Quasi-static obstacle processing uses the occupied area of ​​the corresponding group as the obstacle avoidance constraint input to the model prediction controller. Reduce the trend compliance weight, which is the weight of the trend compliance cost term in the comprehensive cost. Step S6: Output the model predictive controller by inputting the motion trend flow field, the robot body state obtained by the inertial measurement unit, and the global navigation target into the prediction time domain, and solve the control sequence that satisfies the dynamic constraints and obstacle avoidance constraints and minimizes the overall cost. Step S7: Issue the first control command in the execution control sequence; Step S8: Update the body state based on the contact feedback from the plantar pressure sensor, and repeat steps S1 to S7. The environmental point cloud data output by the depth sensor is expressed in either the robot's body coordinate system or the map coordinate system, and coordinate transformation is completed through extrinsic parameter calibration relationships. Ground point cloud filtering is performed according to ground segmentation rules. These rules determine candidate ground point sets based on the height distribution and local geometric consistency of the point cloud, and output a ground model after performing a planar consistency check on the candidate ground point sets. Point clouds that do not meet the ground consistency check are retained as non-ground point clouds. The pedestrian cluster set is obtained from the non-ground point cloud through clustering segmentation and pedestrian filtering rules. The pedestrian filtering rules include at least height interval rules, point count interval rules, and shape scale rules, and point cloud clusters that do not meet the rules are removed. In one implementation, the ground candidate point set is extracted from the point cloud according to the height distribution rules. The height reference surface is either a continuation of the ground model from the previous period or estimated from the lowest height quantile in the current point cloud. The default value of the height candidate bandwidth is 0.30 meters, and the adjustable range is 0.15~0.60 meters. Local geometric consistency is jointly determined by the consistency of the point cloud neighborhood normal and the consistency of the planar residuals. The residual threshold for the planar consistency test is set to 0.03 meters by default, with an adjustable range of 0.01 to 0.08 meters. The angle threshold for normal consistency is set to 15° by default, with an adjustable range of 8° to 25°. To reduce non-ground missed detections caused by the accidental deletion of steps or low obstacles at the ground boundary, the ground candidate bandwidth is widened to the upper limit according to the mapping table when the slope changes or the point cloud sparsity increases, and all point cloud points that fail the consistency test are retained as non-ground point clouds.

[0028] When performing data association on pedestrian clusters across multiple consecutive frames, a set of clustered trajectories is maintained, and motion vectors, loss counts, and update timestamps are recorded for each trajectory. When the loss count meets the consecutive failure condition, the trajectory is terminated and removed from the set of clustered trajectories. When a new pedestrian cluster meets the initialization condition, a new trajectory is generated and added to the set of clustered trajectories. Motion groups are obtained by merging pedestrian clusters based on proximity distance thresholds and motion similarity thresholds, and hysteresis rules are used for merging and splitting groups to avoid frequent switching of groups within adjacent periods.

[0029] The motion trend flow field includes the average motion vector and spatial density distribution of the motion group. The spatial density distribution is represented by a grid or voxels and covers the robot's local planning area. The resolution of the grid or voxels is determined by segmenting the point cloud sparsity. The flow field confidence is determined by both motion consistency and spatial clustering, and a confidence level is output. When the flow field confidence is lower than the confidence threshold or the number of consecutive association failures exceeds the threshold, the corresponding motion group is switched to quasi-static obstacle processing. The quasi-static obstacle is obtained from the group's spatial boundary through a safe expansion rule and is used as an obstacle avoidance constraint input to the model predictive controller. At the same time, the trend compliance weight is limited to the lower end of the weight interval. In this embodiment, the local planning area is defined in the environmental reference frame with the robot's current pose as the center. The default coverage range is 6.00 meters forward and ±3.00 meters laterally, and the adjustable range is 3.00~10.00 meters forward and ±2.00~±5.00 meters laterally. When the spatial density distribution is represented using rasterization, the raster size is selected in three segments based on the point cloud sparsity, with default values ​​of 0.20 meters, 0.30 meters, and 0.50 meters, and an adjustable range of 0.10 to 0.80 meters. When represented using voxels, the voxel side length and the raster size are segmented in the same way. The density value is obtained by counting and normalizing the number of points per unit raster, with the normalization benchmark determined by the total number of points and the effective observation area within the local planning region. When the density value is updated online over time, a time window smoothing rule is used, with a default smoothing time constant of 0.50 seconds and an adjustable range of 0.20 to 1.20 seconds, to suppress short-term density fluctuations caused by occlusion and point cloud sparsity. The segment boundaries for point cloud sparsity are determined based on the number of points per unit area. By default, a number of points per unit area greater than 500 is considered low sparsity, 200-500 is considered medium sparsity, and less than 200 is considered high sparsity. The adjustable ranges are 300-800, 150-600, and 80-400, respectively. The direction threshold, velocity change threshold, and trend conformity weight lower limit for each segment are adjusted synchronously according to the mapping table to avoid overly strict judgment of fluctuations under sparse point cloud conditions.

[0030] The input data for the model predictive controller includes the motion trend flow field, the robot's body state, and the global navigation target. The robot's body state includes at least its posture, angular velocity, linear velocity, and support state. The global navigation target includes at least the target orientation or target pose. In the prediction time domain, the model predictive controller generates a predicted trajectory using a robot dynamics prediction model. This dynamics prediction model consists of center-of-mass dynamics and contact constraints, and includes equivalent boundaries for joint and actuator constraints. The comprehensive cost includes target tracking cost, density collision avoidance cost, and trend compliance cost. The density collision avoidance cost is calculated based on the relative relationship between the spatial density distribution and the robot's predicted trajectory. The trend compliance cost is calculated based on the relative relationship between the robot's predicted motion direction and the average motion vector. The weights of each cost item in the comprehensive cost are determined by weight segmentation rules and are limited within the weight range.

[0031] The control sequence output by the model predictive controller is either a robot motion reference sequence or a low-level control input sequence. When the output is a motion reference sequence, the control commands include at least one of the desired body velocity, desired body posture, and desired foot trajectory, and are converted into joint drive torque commands or joint position commands by the low-level whole-body controller. When the output is a low-level control input sequence, the control commands directly include joint drive torque commands or joint position commands. Foot pressure sensors are used to determine the support contact state and contact stability, and based on this, update the support state, limit the swinging foot trajectory, and trigger a rolling re-solution of the control sequence.

[0032] In this embodiment, steps S1 to S8 are executed in a rolling control cycle of 0.05 seconds, with an adjustable range of 0.02 to 0.10 seconds. The sampling cycle of the depth sensor point cloud frame is 0.10 seconds, with an adjustable range of 0.05 to 0.20 seconds. The sampling cycles of the inertial measurement unit and the plantar pressure sensor are 0.005 seconds and 0.002 seconds, respectively, with adjustable ranges of 0.001 to 0.010 seconds and 0.001 to 0.005 seconds, respectively. Each sensor data carries a timestamp generated by the monotonic clock of the same system, with the timestamp in seconds. The timestamp of the depth sensor point cloud frame is used as the reference timestamp for environmental observation in the current cycle. At the beginning of each control cycle, the most recent point cloud frame whose timestamp falls within a 0.03-second tolerance range before or after the reference timestamp is selected as the input for step S1. If no point cloud frame exists within the tolerance range, the missing measurement flag for the depth sensor data in that cycle is set to 1. The data from the inertial measurement unit (IMU) and plantar pressure sensor are aligned according to a reference timestamp. Alignment samples are generated using nearest neighbor hold or linear interpolation. If alignment fails, the missing data flag of the IMU or the missing data flag of the plantar pressure sensor is set to 1 and included in the output record. The output record contains at least the following set of fields: reference timestamp, control cycle number, point cloud frame timestamp, each missing data flag, number of pedestrian clusters, number of valid trajectories, number of motion groups, velocity amplitude and direction of the average motion vector of each motion group, resolution and coverage of the spatial density distribution, flow field confidence level of each motion group, trend compliance weight and its lower limit, current values ​​of confidence threshold and frequency threshold, prediction time domain and number of discrete nodes of the model predictive controller, optimization solution status flag, summary of the first control command issued, and support status determined by the plantar pressure sensor. When any missing data flag is 1 or the flow field confidence level of any motion group drops to a low level, a warning flag is set to 1 and stored with the output record. The warning flag is cleared after the missing data is resolved and the stable cycle condition is met.

[0033] In this embodiment, the clustering segmentation adopts either Euclidean clustering or density clustering. The pedestrian screening rules include height interval threshold and point count interval threshold. Point cloud clusters that meet the thresholds are determined as pedestrian clusters. Euclidean clustering segments non-ground point clouds using neighborhood connectivity rules, while density clustering segments them using neighborhood density connectivity rules. A pedestrian screening rule calculates the height range, number of points, and circumscribed scale for each point cloud cluster. Point cloud clusters whose height range falls within a preset height range, whose number of points falls within a preset number range, and whose circumscribed scale meets a preset scale range are identified as pedestrian clusters. Point cloud clusters whose circumscribed scale significantly deviates from the pedestrian scale are removed according to a rejection rule, and the remaining point cloud clusters are output as the pedestrian cluster set. In one implementation, voxel downsampling and outlier removal are performed on the non-ground point clouds before clustering. The default voxel side length is 0.05 meters, adjustable from 0.03 to 0.08 meters. Outlier removal is determined by a neighborhood point count threshold, with a default value of 8 and an adjustable range of 5 to 15. When using Euclidean clustering, the default neighborhood connectivity radius is 0.25 meters, with an adjustable range of 0.18 to 0.35 meters. The default lower limit for the number of points in a point cloud cluster is 60, and the default upper limit is 2000, with adjustable ranges of 30 to 120 and 800 to 4000, respectively. When using density clustering, the default neighborhood density radius is 0.25 meters, with an adjustable range of 0.15 to 0.35 meters. The default minimum number of neighborhood points is 15, with an adjustable range of 10 to 30. The default value for the height range threshold is 1.20~2.20 meters, and the adjustable range is 0.90~2.40 meters; the default value for the number of points range threshold is 80~1200, and the adjustable range is 40~2000; the shape scale rule is determined according to the circumscribed scale range. The default value of the circumscribed scale range is based on the historical sample quantile and can be adjusted in segments according to the scene density to avoid the clustering of pedestrians caused by proximity in dense scenes being misjudged as a single pedestrian cluster.

[0034] In this embodiment, data association includes: calculating the centroid displacement of pedestrian clusters in adjacent frames within a time window and converting it into a velocity vector; matching based on a distance threshold and a velocity threshold; and processing clusters that fail to match according to a loss counting rule and using them for the number of times threshold determination. Within the time window, the centroid of the pedestrian cluster is calculated from the geometric center or robust center of the point cloud cluster; the displacement vector between adjacent frames is calculated using a unified coordinate system and converted into a velocity vector as the motion vector. The matching process calculates distance and velocity consistency for candidate matching pairs and determines a unique match from the candidate set that satisfies the distance and velocity thresholds; when multiple candidates satisfy the thresholds, the matching pair is determined according to the rule of minimum matching cost. For unmatched historical trajectories, the loss count is incremented; for trajectories with consecutive unmatched events exceeding the trajectory termination condition, termination processing is performed; for the current unmatched pedestrian cluster, an initialization judgment is performed, and if the initialization condition is met, a new trajectory is generated and assigned an initial motion vector and initial confidence state. In one implementation, the distance and velocity thresholds are set in conjunction with the pedestrian velocity range according to the control cycle. The default value of the distance threshold is 1.00 meters, with an adjustable range of 0.50 to 1.80 meters; the default value of the velocity threshold is 1.80 meters per second, with an adjustable range of 0.80 to 3.00 meters per second. Distance consistency is calculated using the Euclidean distance between the predicted and observed centroid positions. The predicted centroid position is extrapolated from the centroid of the previous frame and the motion vector. Velocity consistency is determined by the velocity difference and direction difference between the predicted and observed velocities. The default value for the direction difference is 45°, with an adjustable range of 20° to 80°. When multiple historical trajectories of the same observation cluster meet the threshold, the matching cost is determined by a weighted sum of the distance and velocity terms. The default weight for the distance term is 0.60, and the default weight for the velocity term is 0.40, with adjustable ranges of 0.40 to 0.80 and 0.20 to 0.60, respectively. The candidate with the lowest matching cost is determined as the unique match, and the remaining candidates are treated as unmatched and enter the loss counting process. The default value for the number of consecutive unmatched frames corresponding to the trajectory termination condition is 5, with an adjustable range of 3 to 10. The default value for the number of consecutive matched frames corresponding to the initialization condition is 2, with an adjustable range of 2 to 4. The body pose change used for self-motion compensation is calculated by the inertial measurement unit. The compensation object is the centroid displacement vector between adjacent frames, so that the motion vector is in the environmental reference frame. When the inertial measurement unit's missing measurement flag is 1, the self-motion compensation retains the result of the previous cycle and sets the corresponding cycle's warning flag to 1.

[0035] In this embodiment, the merging of motion groups satisfies the following conditions: the centroid distance between two pedestrian clusters is ≤ the distance threshold, the velocity angle is ≤ the velocity angle threshold, and the velocity difference is ≤ the velocity difference threshold, when they are merged into the same motion group; if these conditions are not met, they remain as different motion groups. Centroid distance, velocity angle, and velocity difference are all calculated in the same coordinate system. Group merging employs hysteresis rules, which include merging and splitting thresholds, and ensure that the merging and splitting thresholds meet different threshold ranges to reduce jitter. The spatial boundary of the group is obtained by merging the outer boundaries of pedestrian clusters within the group, and a safe expansion rule is applied to the merged boundary to generate the group-occupied area for obstacle avoidance constraints and density calculation.

[0036] In this embodiment, the average motion vector is calculated according to the point weighting rule, and the spatial density distribution is generated according to the grid size range or the kernel width range. The grid size or kernel width is selected by segmenting the point cloud sparsity using the mapping table; the point cloud sparsity is a statistical index of the point cloud density within a local planning area; the mapping table is a table showing the correspondence between the point cloud sparsity segments and the parameter intervals. Point cloud sparsity is determined by the point cloud density index within the local planning area. The point cloud density index is obtained by counting the number of points per unit area or per voxel. The mapping table divides the point cloud sparsity into at least three segments, and assigns a corresponding raster size range or kernel width range to each segment. The spatial density distribution projects or voxels each motion group within the local planning area, and normalizes the projected occupancy intensity or voxel occupancy intensity into a density value. The density value is updated online over time, and short-term abrupt changes are suppressed according to the time window smoothing rule.

[0037] In this embodiment, the flow field confidence level is determined by both motion consistency and spatial clustering, wherein: The confidence level is increased when the velocity direction change within K consecutive frames is less than or equal to the direction threshold and the velocity change is less than or equal to the velocity change threshold. When the spatial clustering degree is less than or equal to the clustering dispersion threshold, the confidence level is increased. Motion consistency is determined by the consistency of direction changes and velocity changes over multiple consecutive frames. Direction change consistency is determined by whether the direction change falls within a direction threshold range, and velocity change consistency is determined by whether the velocity change falls within a velocity change threshold range. Spatial clustering is determined by the spatial dispersion of pedestrian clusters within a group and boundary stability. Boundary stability is determined by whether the change in the group's spatial boundary within the time window falls within a boundary change threshold range. Confidence levels include at least three levels: low, medium, and high, and are output according to level switching rules. When the confidence level drops to low, rules for reducing the weight of trend compliance and switching the group processing mode are triggered.

[0038] Motion consistency is calculated using a time window and hysteresis rules. The time window is a sliding window consisting of multiple consecutive frames. When K consecutive frames meet the conditions that the velocity direction change does not exceed the direction threshold and the velocity change does not exceed the velocity change threshold, the flow field confidence level is increased. When a frame does not meet the direction threshold or velocity change threshold, the anomaly count is accumulated without triggering a decrease in the flow field confidence level. When M consecutive frames corresponding to the anomaly count do not meet the conditions, the flow field confidence level is decreased. Here, K and M are positive integers. Among them, the orientation threshold, velocity change threshold and trend compliance weight lower limit are selected by the mapping table according to the point cloud sparsity. When the point cloud sparsity increases, the orientation threshold and velocity change threshold can be relaxed and the trend compliance weight lower limit can be reduced. In one implementation, during the flow field confidence process in step S6, motion consistency is used to measure the motion stability of the same motion group within a time window, reflecting the fluctuation level of the average motion vector of the group in terms of direction and velocity amplitude. Motion consistency uses the group velocity vector obtained in step S3 as input. Before calculation, the velocity vector undergoes self-motion compensation based on the body pose changes calculated by the inertial measurement unit, ensuring the velocity vector is within the environmental reference frame and avoiding spurious velocity changes introduced by robot walking.

[0039] Motion consistency is determined using a time window combined with hysteresis counting. Within the control cycle, a sliding time window of length K frames is maintained for each motion group. The velocity vector and velocity magnitude of the group in each frame are recorded, and stable and abnormal counts are maintained. Within the time window, the change in the angle between the current frame's velocity vector and the velocity vector of the previous frame, as well as the difference in velocity magnitude, are used as fluctuation parameters. In one implementation, K defaults to 10 and is adjustable from 5 to 20; M defaults to 3 and is adjustable from 2 to 8, ensuring the time window length covers the effective observation range of 0.50 to 2.00 seconds and is consistent with the point cloud frame sampling period. Stable counts and abnormal counts maintain independent states in each motion group. Stable counts are reset to zero when a single frame abnormality occurs, while abnormal counts are accumulated during consecutive abnormal periods and reset to zero when a valid observation frame meeting the threshold appears. Invalid observation frames do not trigger updates to stable or abnormal counts, and the number of invalid observation frames is written to the output record and used for setting warning flags. An exemplary numerical operation example is as follows: the control cycle is 0.05 seconds, the point cloud frame sampling cycle is 0.10 seconds, the time window is K = 10 frames, the prediction time domain is 1.00 seconds and discretized into 20 nodes; the Euclidean clustering neighborhood connectivity radius is 0.25 meters, the point number interval threshold is 80~1200, and the height interval threshold is 1.20~2.20 meters; the data association distance threshold is 1.00 meters, and the speed threshold is 1.80 meters per second; the motion group merging distance threshold is 1.20 meters, the speed angle threshold is 30°, and the speed difference threshold is 0.60 meters per second; the confidence threshold is 0.50, the number threshold is 3, the quantity threshold is 2, and the stable cycle condition is that the number of valid trajectories is not less than the quantity threshold and the warning mark is 0 for 5 consecutive control cycles. At the start of the first control cycle, one frame of point cloud is acquired and ground filtering is completed. After obtaining the non-ground point cloud, 18 point cloud clusters are obtained by clustering. 14 pedestrian clusters are retained according to the pedestrian screening rules. Within the time window, the centroid displacement of the clusters in adjacent frames is matched to form 12 valid trajectories and calculate their respective motion vectors. Three motion groups are obtained according to the merging conditions and a spatial density distribution is generated. The grid size is 0.30 meters and the number of grid nodes in the local planning area is 20 x 30. The flow field confidence is calculated for the three motion groups. Two groups meet the direction threshold and velocity change threshold within 10 consecutive frames and are upgraded to high level. The other group is switched to quasi-static obstacle processing because it triggers the threshold for the number of association failures 3 times in a row. The trend compliance weight is reduced to near the lower limit of the weight interval. At the same time, the warning flag is set to 1 and written to the output record. The model predictive controller solves the control sequence on 20 nodes. After satisfying the obstacle avoidance constraints and dynamic constraints, the first control command is issued. Subsequently, if point cloud frames are missing within a 0.03-second tolerance during the 2nd to 4th control cycles, the depth sensor data missing marker is set to 1, and the motion trend flow field or default flow field update rule of the previous cycle is used. The density collision avoidance cost remains effective, and the trend compliance weight is maintained at the lower limit. When the point cloud is restored and the number of effective trajectories is not less than 2 for 5 consecutive control cycles and the warning marker is 0 for 5 consecutive cycles, the trend compliance weight is restored to the normal range of the corresponding sparsity segment according to the mapping table and continues to be executed.

[0040] When K consecutive frames all satisfy the condition that the direction change does not exceed the direction threshold and the velocity change does not exceed the velocity change threshold, the stability count meets the upgrade condition, and the confidence level corresponding to motion consistency is upgraded.

[0041] When a single frame anomaly occurs, the stability count is reset to zero and the anomaly count is incremented by one; if the anomaly count does not reach the downgrade condition, the confidence level remains unchanged. When there are M consecutive anomalies, the confidence level is lowered, and the trend of this group is constrained by the lower limit of the weighting according to a preset lower limit value to suppress the amplification effect of short-term false detections on the control cost. K and M satisfy M≥2, and K is selected according to the control period and the range of pedestrian movement speed so that the time window covers the observation frames within multiple gait periods.

[0042] In another implementation, under occlusion scenarios, the group may experience association failures or missing point cloud measurements in certain frames. In this case, the velocity vector of that frame is recorded as an invalid observation, and the time window only updates the fluctuation amount for valid observation frames. Invalid observation frames do not trigger confidence downgrading, the anomaly count does not increment, and the stable count is paused. After the group recovers association in subsequent frames, it continues to perform continuous K-frame determinations on valid observation frames. If the number of consecutive association failures exceeds the threshold in step S5, the group is treated as a quasi-static obstacle, and the trend compliance weight is reduced, thereby avoiding traveling along the wrong trend when there is long occlusion or target loss.

[0043] In another implementation, in clustering adhesion scenarios, when two or more pedestrians are clustered into the same group, the centroid position and velocity vector often exhibit abrupt changes, manifested as significantly increased changes in orientation angle and velocity. Motion consistency is sensitive to these abrupt changes in these fluctuations; a single-frame abrupt change triggers a reset of the stability count, and consecutive occurrences trigger a decrease in confidence. The hysteresis mechanism ensures that a single abrupt change caused by occasional adhesion does not directly cause confidence fluctuations. After the adhesion is resolved, the group velocity returns to stability, and the confidence is increased again when the threshold condition is met for K consecutive frames. For the short-term erroneous average motion vector caused by adhesion, the trend compliance weight is constrained by both the confidence level and the lower limit of the weight, causing the controller to prioritize density collision avoidance and target tracking costs at this stage, reducing reliance on the trend term.

[0044] Point cloud sparsity is used to segmentally adjust the orientation threshold, velocity change threshold, and trend conformity weight lower limit. Point cloud sparsity is calculated as the ratio of the number of points in a group to the volume of its 3D bounding box; fewer points or a larger volume indicates higher sparsity.

[0045] Different threshold relaxation coefficients and weight lower limits are used when the sparsity falls into different intervals: When the sparsity is low, the orientation threshold and velocity change threshold use the baseline value, and the lower limit of the trend conformity weight takes a higher value; When the sparsity is moderate, the direction threshold and velocity change threshold are increased by a relaxation factor, while the lower limit of the trend conformity weight is decreased proportionally. When the sparsity is high, the direction threshold and velocity change threshold are further relaxed, and the lower limit of the trend conformity weight is further reduced.

[0046] The interval boundary can be set to three or more segments according to the number of points per unit volume, and matched with the depth sensor range, viewpoint and scene occlusion ratio, so that the sparse point cloud caused by long distance and occlusion is not over-judged as motion inconsistency, while limiting the minimum effect strength of the trend compliance term to a controllable range.

[0047] As can be seen, among the various implementations of flow field confidence in step S6, motion consistency is used to describe the stability of motion vectors of the same motion group within a time window, focusing on the fluctuation level of direction and velocity changes. Before calculation, it incorporates self-motion compensation from the inertial measurement unit, ensuring that fluctuations originate from external pedestrian movement rather than robot walking. In implementation, a sliding time window and hysteresis counting are used. Confidence is increased when K consecutive frames meet the direction and velocity change thresholds. When a single frame anomaly occurs, confidence is not immediately decreased; only M consecutive anomalies trigger a downgrade. Confidence changes occur within a buffer zone, avoiding jitter caused by short-term fluctuations. Occlusion-induced association failures or missing frames are treated as invalid observations, and the anomaly count is not accumulated. After association is restored, continuous judgment continues based on valid frames. When occlusion persists and reaches the number of occurrences threshold, it is treated as a quasi-static obstacle, and the trend compliance weight is reduced. Velocity abrupt changes caused by clustering adhesion interrupt stable counting and reduce confidence when they occur consecutively. The hysteresis mechanism prevents single adhesions from amplifying their impact. The threshold for segmented adjustment of point cloud sparsity is relaxed and the lower limit of weight is increased to keep the judgment and control weights under sparse point clouds under controllable conditions.

[0048] In this embodiment, the comprehensive cost includes at least: target tracking cost, density collision avoidance cost, and trend compliance cost; wherein the trend compliance cost is weighted according to the angle between the robot's predicted motion direction and the average motion vector, and the penalty weight is increased when the angle is greater than or equal to the reverse motion threshold. The target tracking cost is determined based on the deviation between the robot's predicted state and the global navigation target; the density collision avoidance cost is determined based on the relative relationship between the robot's predicted trajectory and the spatial density distribution, and increases according to a penalty weighting rule when the robot approaches a high-density area; the trend compliance cost is determined based on the angle between the robot's predicted motion direction and the average motion vector of the motion group, and the penalty weight is assigned segmented values ​​according to the angle segmentation rule. The weights of each cost item in the comprehensive cost are determined according to the weight segmentation rule, and upper and lower limits are applied within the weight interval; when the confidence level is low, the weight of the trend compliance cost is limited to the lower end of the weight interval, and when the confidence level is high, the weight of the trend compliance cost is limited to the upper end of the weight interval.

[0049] In this embodiment, the density collision avoidance cost is constructed according to the repulsion potential field rule. The repulsion strength is positively correlated with the group density. When the robot's predicted trajectory enters the region in the opposite direction of the group's movement direction, the repulsion strength increases with the depth of entry. The reverse direction region is the region where the robot's predicted motion direction and average motion vector satisfy the reverse motion determination rule; the entry depth is the projection distance of the robot's predicted trajectory representative point into the group's occupied area along the reverse direction. The repulsion potential field rule determines the basic quantity of repulsion strength based on the minimum distance between the robot's predicted trajectory representative point and the area occupied by the motion group, and weights this basic quantity with the group density. The reverse direction region is determined by the relative direction relationship between the robot's predicted motion direction and the average motion vector of the motion group; when the relative direction satisfies the reverse motion determination rule, it is identified as a reverse direction region. The entry depth is determined by the projection distance of the robot's predicted trajectory representative point projected into the group-occupied area along the reverse direction, and the repulsion strength is enhanced according to the entry depth segmentation rule. The repulsion strength is subject to saturation limits and safety boundary constraints during the enhancement process. In one implementation, the robot's predicted trajectory representative point is selected as either the projection point of the robot's centroid or the projection point of the robot's geometric center, and one of these is fixedly identified in the output record. The safety boundary constraint is implemented using a minimum distance threshold, which defaults to 0.60 meters, is adjustable from 0.30 to 1.20 meters, and can increase with increasing gait speed. The default value for the direction threshold of the reverse motion determination rule is 120°, and the adjustable range is 90°~150°. The entry depth segmentation rule adopts a 3-segment enhancement method, with the default segment boundaries being 0.20 meters and 0.50 meters, and the adjustable range being 0.10~0.80 meters. After the entry depth exceeds the upper segment boundary, the repulsion intensity is kept at a saturation limit and does not increase further. The saturation upper limit is tuned according to the weight interval and the solver numerical stability method, and the current value is saved in the output record.

[0050] In this embodiment, the optimization problem of the model prediction controller includes soft constraints on the foot landing point. The soft constraints encourage the landing point to be selected outside the boundary of a low-density area or a group with high confidence and speed below the speed threshold, and a minimum landing distance threshold is set. The foot landing point is parameterized in two-dimensional position in the support plane coordinate system and together with the gait phase consistency constraint, constitutes the landing point constraint set. Low-density regions are extracted from the spatial density distribution according to the density threshold rule, and grid connected regions or voxel connected regions that satisfy the density threshold rule are determined as candidate regions. The soft constraint of the landing point applies a penalty rule to the distance between the landing point and the candidate region, and applies a minimum landing distance threshold constraint to the minimum distance between the landing point and the group-occupied region. When the candidate region is empty or does not meet the accessibility determination rule, the soft constraint of the landing point switches to the default landing rule and keeps the obstacle avoidance constraint effective. In this embodiment, the minimum step length threshold of the reachable step length interval is set to 0.10 meters by default, the maximum step length threshold is set to 0.45 meters by default, and the adjustable ranges are 0.05~0.20 meters and 0.30~0.70 meters, respectively; the allowable turning angle interval is set to ±40° by default, and the adjustable range is ±20°~±60°. The default safety margin threshold for the supporting polygon is 0.04 meters, with an adjustable range of 0.02 to 0.08 meters. When the plantar pressure sensor determines that the contact reliability is low, the safety margin threshold is amplified by a preset amplification factor, which defaults to 1.50 and is adjustable from 1.20 to 2.00. When the candidate low-density region is empty or unreachable, the default duration for carrying over the results from the previous cycle is 10 control cycles, with an adjustable range of 5 to 30 control cycles. If the region remains empty after the duration exceeds 10 control cycles, the soft constraint of the landing point remains disabled until the candidate low-density region meets the reachability screening again. At the same time, the warning flag is set to 1 and written to the output record. The warning flag is cleared after the candidate low-density region becomes available for two consecutive point cloud frame cycles.

[0051] Low-density regions are extracted from spatial density distribution. Grids with density values ​​less than the density threshold are identified as low-density grids, and the connected regions of adjacent low-density grids are identified as candidate low-density regions. The accessibility screening of foot landing points is performed based on the reachable step range and the safety margin of the supporting polygon. If the relative distance between the candidate landing point and the supporting foot falls within the reachable step range and the minimum distance from the centroid projection to the boundary of the supporting polygon after landing is not less than the safety margin threshold, the candidate landing point is retained. When the candidate low-density region is empty, the soft constraint of the foot landing point remains the result of the previous cycle or is not enabled, and obstacle avoidance constraints and density collision avoidance costs dominate the optimization. The reachable step range is the allowable range of the horizontal distance and azimuth angle of the candidate landing point relative to the supporting foot. The safety margin of the supporting polygon is the minimum distance threshold from the centroid projection to the boundary of the supporting polygon after landing. In one implementation, the low-density region is extracted from the spatial density distribution obtained in step S4 and used in the soft constraint construction of the foot landing point in the model prediction controller in step S6. When the spatial density distribution is represented by a rasterization, a two-dimensional ground grid is established within the robot's local planning range. The point cloud of each motion group is counted according to the number of points falling into the grid and normalized to obtain the density value. When kernel density is used, the point cloud is projected onto the ground plane and the density value is accumulated at the center of the grid using a kernel function. To avoid density distortion caused by the sparseness of the point cloud at long distances, the density value can be scaled segmentally according to the sparsity of the point cloud, so that the same number of people can obtain similar density amplitudes at different observation distances.

[0052] The extraction rules for low-density regions are as follows: A threshold determination is applied to each grid cell in the density distribution. Grids with density values ​​less than the density threshold are marked as low-density grids. Adjacent low-density grids are merged according to eight-neighbor or four-neighbor connectivity to obtain one or more low-density connected domains. Each connected domain constitutes a candidate low-density region. To improve the stability of the landing surface, morphological constraints are further applied to the candidate low-density regions, including that the area of ​​the connected domain is not less than a minimum area threshold, the mean density within the connected domain is not higher than a mean threshold, and the minimum distance from the boundary of the connected domain to the high-density grid is not less than a safety distance threshold. The safety distance threshold is used to reserve a buffer zone between the landing point and the boundary of the dense crowd to reduce the risk of collision caused by prediction errors. For scenarios with relative motion to the moving group, the extraction of low-density regions can be achieved by superimposing anisotropic expansion in the direction of the group's velocity on the density distribution. That is, the equivalent density is increased or the high-density boundary is expanded in the area in front of the group's movement direction, so that the low-density region is more likely to be located behind or in the gap channel of the group.

[0053] After obtaining candidate low-density regions: The accessibility screening of landing points is performed based on the reachable step length interval / support polygon safety margin. The reachable step length interval is determined based on the robot's current support foot position, gait type, and joint range of motion. It limits the horizontal distance of the next landing point relative to the support foot to between the minimum and maximum step length thresholds, and limits the relative azimuth angle to within the allowable turning angle range; candidate regions that do not meet the step length interval are eliminated. The support polygon safety margin is used to evaluate the stability margin of the body's centroid projection and the support polygon after landing: for each candidate landing point, the centroid projection position is estimated by combining the body state in the prediction time domain, and the minimum distance to the boundary of the support polygon formed by the support foot and the candidate landing point is calculated. When this minimum distance is less than the safety margin threshold, it is judged as insufficient stability and eliminated. The safety margin threshold is related to the foot size, ground friction estimation, and contact reliability feedback from the foot pressure sensor. When the contact reliability is low, a larger safety margin threshold is used to improve landing stability.

[0054] The reason why the landing point is feasible and stable is that: The low-density region originates from the low-value connected domain of the spatial density distribution. It is spatially far from the high-density boundary and has a reserved buffer zone, which makes the probability of dynamic obstacles near the landing point low. The reachable step length range ensures that the landing point is within the gait range that the robot can achieve, avoiding joint saturation or interference between the swinging leg and obstacles caused by excessive strides. The safety margin of the supporting polygon ensures that after landing, the centroid projection remains inside the supporting polygon during the prediction stage and has sufficient margin, so that dynamic stability can still be maintained when there are external disturbances or state estimation errors.

[0055] The selected landing points are used as preferred targets under soft constraints for optimization. If the obstacle avoidance hard constraints are also met, the landing points are preferentially placed in low-density areas, thereby forming a passable and stable crossing strategy in the pedestrian flow field.

[0056] When candidate low-density regions are empty, a degradation strategy is employed to maintain control enforceability. The degradation strategy includes two paths: First, the landing points or candidate landing points that have passed the accessibility screening in the previous period are used, and only their safe distance determination is updated in this period, so that the landing preference has temporal continuity. Secondly, if the landing point of the previous cycle is unavailable or continuously empty, the low-density soft constraint is suspended, and only the obstacle avoidance hard constraint and density collision avoidance cost are retained, so that the controller focuses on safe obstacle avoidance and target tracking, and the trend conformation weight is limited to the lower limit of the weight range to reduce the dependence on flow field inference.

[0057] This can be understood as follows: low-density regions are obtained by thresholding the spatial density distribution. Grids with densities below the density threshold are marked and merged according to connectivity to form connected regions, which are candidate low-density regions. To ensure that candidate regions meet landing conditions, further screening is performed using a minimum area threshold, a connected region mean threshold, and a safe distance threshold to the high-density boundary. The safe distance provides a buffer for the crowd boundary, reducing the risk of collisions caused by prediction errors. The accessibility screening of landing points restricts the landing distance and azimuth angle by the reachable step size interval, excluding areas outside the gait range. The minimum distance from the centroid projection to the support boundary after landing is evaluated using the support polygon safety margin. If the minimum distance is insufficient, the area is discarded, thus balancing feasibility and stability.

[0058] The feasibility of low-density regions stems from their proximity to gap channels and distance from high-density boundaries. Stability arises from step size constraints preventing joint saturation and swing leg interference, while the support polygon margin provides leeway for state errors and external disturbances. When no candidates are available, a degenerate strategy is employed, prioritizing the use of feasible landing preferences from the previous cycle to maintain continuity. If still unavailable, only hard obstacle avoidance and collision avoidance costs are retained, and the trend-following weight is reduced to near the lower limit to ensure that control can still be solved and executed.

[0059] Example 2: Based on Example 1, when depth sensor data is missing or the number of pedestrian clusters that can be associated is less than the number threshold, the motion trend flow field remains the result of the previous cycle or is updated according to the default value, and the trend conformation weight is limited to the weight range. The missing depth sensor data is due to at least one of the following: missing point cloud frames, insufficient number of valid point cloud points, or abnormal point cloud timestamps. The absence of depth sensor data is determined by absence criteria, which include missing point cloud frames, insufficient number of valid point clouds, and abnormal point cloud timestamps. The number of associatable pedestrian clusters is determined by the number of valid trajectories output from the data association. When there is a lack of data or insufficient samples, the motion trend flow field remains the result of the previous cycle, with the duration limited by the upper limit of the duration. After exceeding the upper limit, a default flow field is generated according to the default value update rule. The default flow field contains at least an empty density distribution and a zero-trend vector, or contains an occupancy distribution based solely on quasi-static obstacles. When there is a lack of data or insufficient samples, the density collision avoidance cost remains valid and increases according to the conservative weighting rule, while the weight of the trend compliance cost remains at the lower end of the weighting interval. When the lack of data is resolved and the number of valid trajectories recovers to above the number threshold and continuously meets the stable cycle condition, the motion trend flow field is regenerated according to the online update rule, and the segmented weighting rule for the trend compliance weight is restored.

[0060] In this embodiment, the default threshold for determining insufficient effective points in the point cloud is 2000, with an adjustable range of 500~6000; the default tolerance for determining abnormal point cloud timestamps is 0.20 seconds, with an adjustable range of 0.05~0.50 seconds. In the default flow field update rule, the density values ​​corresponding to the empty density distribution are all set to 0, the velocity amplitude corresponding to the zero trend vector is set to 0, and the direction is set to either the direction of the previous cycle or a fixed direction, with the value method fixed in the output record; when the default flow field is enabled, the density collision avoidance cost weight is increased according to the conservative weight rule and the trend conformity weight is kept in the lower end of the weight range until the stable cycle condition is met. The default upper limit of the duration period is 20 control cycles, with an adjustable range of 10~60 control cycles; the default value of the quantity threshold is 2, with an adjustable range of 1~5; the default stable cycle condition is that the number of effective trajectories is not less than the quantity threshold and all missing measurement markers are 0 for 5 consecutive control cycles, with an adjustable range of 3~15 control cycles. During periods of missing data or insufficient samples, the missing data markers, warning markers, number of valid trajectories, number of motion groups, trend conformity weight lower limit, and current weight in the output records are updated and continuously written to provide a traceable basis for switching confidence levels and weight segmentation rules in subsequent periods.

[0061] Example 3: This embodiment provides an application in a bidirectional pedestrian flow area at the arrival level of an airport: a robot walks from the terminal exit along the main passage to the arrival area. The global navigation target is the target pose in the map coordinate system. The depth sensor continuously outputs environmental point clouds, the inertial measurement unit outputs the robot's attitude, angular velocity, and linear velocity, and the plantar pressure sensor outputs the contact status and contact reliability of both feet. The control cycle is 0.05 seconds, and the point cloud frame sampling cycle is 0.10 seconds. Each control cycle uses the timestamp of the most recent point cloud frame as the reference timestamp and is aligned with a tolerance of 0.03 seconds. The model prediction controller predicts in the time domain of 1.00 seconds and discretizes it into 20 nodes, outputs the control sequence, and issues the first control command.

[0062] At a certain moment, the depth sensor acquires one frame of point cloud at the reference timestamp. The number of valid points in the point cloud is 52,000, and no missing data detection is triggered. In step S1, the ground point cloud is filtered out according to the ground segmentation rules. The ground candidate bandwidth is set to 0.30 meters, the plane residual threshold is set to 0.03 meters, and the normal consistency angle threshold is set to 15°. The ground model is output and the non-ground point cloud is retained. In step S2, voxel downsampling and outlier removal are performed on the non-ground point cloud. The voxel side length is set to 0.05 meters and the neighborhood point count threshold is set to 8. Then, Euclidean clustering is used for segmentation, and the neighborhood connectivity radius is set to 0.25 meters, resulting in a total of 22 point cloud clusters. The pedestrian screening rules use a height range threshold of 1.20~2.20 meters and a point count range threshold of 80~1200, combined with the circumscribed scale range for removal. A total of 16 pedestrian clusters are retained, and the number of pedestrian clusters is written into the output record.

[0063] Step S3 performs data association on the centroid displacement of pedestrian clusters in adjacent frames within the time window: the distance threshold is set to 1.00 meters, the velocity threshold is set to 1.80 meters per second, and the direction difference is determined using 45°; the matching cost weights are 0.60 for the distance term and 0.40 for the velocity term. The inertial measurement unit (IMU) has no missing measurements in this cycle and performs self-motion compensation on the centroid displacement vector according to its calculated body pose change, ensuring the motion vector is within the environmental reference frame; after matching, 13 valid trajectories are formed, of which 1 trajectory is not matched and its loss count increments. The trajectory termination condition is 5 consecutive frames of non-matching, and the initialization condition is 2 consecutive frames of matching. The velocity amplitude and direction of the velocity vector are calculated for each of the 13 valid trajectories, and the number of valid trajectories is written to the output record.

[0064] Step S4 merges the 16 pedestrian clusters according to motion group merging rules: a distance threshold of 1.20 meters, a velocity angle threshold of 30°, and a velocity difference threshold of 0.60 meters per second. Pedestrians meeting these criteria are grouped into the same motion group. A hysteresis rule with different merging and splitting thresholds is used to avoid jitter. After merging, three motion groups are obtained: Group 1 is located on the right side of the main passage, with an average motion vector velocity amplitude of approximately 1.10 meters per second and a direction consistent with the forward direction of the passage; Group 2 is located on the left side of the main passage, with an average motion vector velocity amplitude of approximately 0.90 meters per second and a direction opposite to the forward direction of the passage; Group 3 is located in the middle-right part of the passage, with an average motion vector velocity amplitude of approximately 0.20 meters per second and a high point cloud spatial clustering. The local planning area is defined as 6.00 meters forward and ±3.00 meters laterally; the point cloud sparsity is calculated based on the number of points per unit area, falling within the range of 200 to 500; the grid size is selected as 0.30 meters from the mapping table; the spatial density distribution is generated at this resolution and updated according to the time window smoothing rule, with a smoothing time constant of 0.50 seconds; the occupied area of ​​each motion group is obtained by merging the group's outer boundary and safely expanding it, and is used for subsequent obstacle avoidance constraints and density calculations.

[0065] Step S5 calculates the flow field confidence level for each motion group and outputs the confidence level. The time window length is set to K = 10 frames, and the abnormal hysteresis is set to M = 3. The direction threshold and velocity change threshold are selected by segmenting the mapping table according to medium sparsity, and the lower limit of the trend compliance weight is also given by the same mapping table. Group 1 meets the threshold conditions for both velocity direction change and velocity change within 10 consecutive frames, so the confidence level is increased to high. Group 2 maintains a high confidence level because of bidirectional flow but stable motion consistency. Group 3 has experienced continuous association failures in the last 3 frames, and the number of consecutive association failures reaches the threshold of 3, triggering a switch to quasi-static obstacle processing. The area occupied by the obstacle is used as the obstacle avoidance constraint input model prediction controller, and the trend compliance weight is reduced to the lower end of the weight interval. At the same time, the warning flag is set to 1 and written to the output record.

[0066] In step S6, the model prediction controller input includes the motion trend flow field, the robot's body state, and the global navigation target. The body state includes the robot's attitude, angular velocity, linear velocity, and support state; the global navigation target includes the target direction and target pose. The comprehensive cost includes target tracking cost, density collision avoidance cost, and trend compliance cost. The trend compliance cost is weighted according to the angle between the robot's predicted motion direction and the average motion vector, and the penalty weight is increased when the angle reaches the reverse movement threshold. The density collision avoidance cost is constructed according to the repulsion potential field rule. The repulsion strength is positively correlated with the group density. When the robot's predicted trajectory enters the reverse direction region, the repulsion strength increases with the entry depth and is subject to saturation limits and safety boundary constraints. The representative point of the robot's predicted trajectory is selected as the projection point of the robot's centroid, with a minimum distance threshold of 0.60 meters; the reverse movement determination direction threshold is 120°; and the entry depth segment boundaries are 0.20 meters and 0.50 meters. At this time, MPC solves the control sequence at 20 nodes in the 1.00 second prediction time domain: In order to reduce the risk of reverse passage with the second group, the robot's predicted trajectory is shifted from the high-density reverse flow area on the left side of the channel to the downstream flow area on the right side. At the same time, hard obstacle avoidance constraints are applied to the area occupied by the third group to ensure that the predicted trajectory and its outer boundary are kept above the minimum distance threshold.

[0067] In the same cycle when the soft constraint on the foot landing point is activated, low-density regions are extracted from the spatial density distribution: grids with density values ​​less than the density threshold are marked as low-density grids, and the connected domains of adjacent low-density grids are used as candidate low-density regions. Accessibility screening is performed on the candidate regions, using an reachable step size interval and a safety margin constraint on the supporting polygon. The minimum step size threshold is 0.10 meters, the maximum step size threshold is 0.45 meters, and the allowable turning angle interval is ±40°; the safety margin threshold for the supporting polygon is 0.04 meters. The plantar pressure sensor determines that the current supporting foot contact reliability is normal, and the safety margin threshold is not amplified. After screening, two candidate landing points are retained. One landing point is located outside the boundary of the right downstream group and is within the low-density connected domain. The other landing point, although within the low-density connected domain, is eliminated because the minimum distance from the centroid projection to the supporting polygon boundary after landing is less than the safety margin threshold. During optimization, the soft constraint on the landing point applies a preference to the retained candidate landing points, causing the control sequence to prioritize the gait propulsion direction corresponding to the landing point while satisfying obstacle avoidance constraints.

[0068] Step S7 issues the first control command of the execution control sequence: the control command is output in the form of motion reference quantities, including at least one of the desired body velocity, desired body posture, and desired foot trajectory, and is converted by the lower-level whole-body controller into joint drive torque command or joint position command. The first control command issued in this cycle is manifested as maintaining the forward velocity of the body in the medium speed range, deflecting the lateral velocity to the right to approach the downstream channel, and landing the foot point in the right low-density area in the next swing foot trajectory; at the same time, maintaining effective obstacle avoidance constraints on the area occupied by the quasi-static obstacles of the third group. In step S8, the foot pressure sensor feedback shows that the support state has not changed abnormally, the contact stability meets the judgment conditions, and the body state is updated in the next cycle according to the aligned inertial measurement unit and foot pressure data, and the execution of steps S1 to S7 continues.

[0069] If entering a more crowded location results in a missing point cloud frame within a 0.03-second tolerance, the depth sensor data missing marker is set to 1. The motion trend flow field remains the result of the previous cycle or a default flow field is generated according to the default value update rule. The default flow field corresponds to the empty density distribution and the zero trend vector. The density collision avoidance cost remains effective and increases according to the conservative weight rule, while the trend compliance weight remains in the lower end of the weight range. Once the point cloud is restored and the number of effective trajectories is not less than the number threshold 2 for 5 consecutive control cycles, all missing markers are 0, and the warning markers are 0 for 5 consecutive cycles, the trend compliance weight is restored to the normal range of medium sparsity segments according to the mapping table, and the robot continues to move forward by reusing the downstream trend and low-density landing preference.

[0070] This embodiment can be understood as follows: when a dynamic bottleneck is formed by the convergence of dense two-way crowds, the sensitivity to individual predictions is reduced by expressing the flow field at the group level. The quasi-static switching triggered by confidence and the reduction of weights suppress the amplification of error trends caused by occlusion and adhesion. This allows the control action to continue to advance continuously in the downstream channel and automatically turn to conservative obstacle avoidance in a low confidence state, showing the effect of reducing emergency stop replanning and reducing freezing tendency.

[0071] Finally, it should be noted that the above embodiments are only used to illustrate the technical solutions of this application, and are not intended to limit them. Although this application has been described in detail with reference to the foregoing embodiments, those skilled in the art should understand that modifications can still be made to the technical solutions described in the foregoing embodiments, or equivalent substitutions can be made to some or all of the technical features therein. Such modifications or substitutions do not cause the essence of the corresponding technical solutions to deviate from the scope of the technical solutions of the embodiments of this application.

[0072] Furthermore, those skilled in the art will understand that although some embodiments herein include certain features included in other embodiments but not others, combinations of features from different embodiments are meant to be within the scope of this application and form different embodiments. For example, all the embodiments above can be used in any combination. The information disclosed in this background section is intended only to enhance the understanding of the general background of this application and should not be construed as an admission or in any way implying that such information constitutes prior art known to those skilled in the art.

Claims

1. A control method for a legged humanoid robot based on point cloud clustering, wherein the robot is equipped with a depth sensor, an inertial measurement unit, and a plantar pressure sensor, characterized in that, include: Step S1: Obtain the environmental point cloud of the depth sensor and filter out the ground point cloud according to the ground segmentation rules; Step S2: Determine the non-ground point cloud to be clustered and segmented, and obtain the pedestrian cluster set according to the pedestrian screening rules; Step S3: Calculate the data association of pedestrian clusters in multiple consecutive frames to obtain the motion vector of each cluster. The motion vector is a velocity vector that represents the motion speed and direction of the pedestrian cluster. Step S4: Generate a motion group by clustering pedestrians according to a distance threshold and a motion similarity threshold, and generate a motion trend flow field for each motion group that includes an average motion vector and a spatial density distribution. The motion similarity threshold includes a velocity angle threshold and a velocity difference threshold. Step S5: Determine the flow field confidence level of each motion group based on the motion trend flow field calculation. The flow field confidence level is a confidence index characterizing the motion consistency and spatial clustering of the motion group. When the flow field confidence level is lower than the confidence level threshold or the number of consecutive association failures exceeds the number threshold, switch the corresponding group to quasi-static obstacle processing. The quasi-static obstacle processing involves using the occupied area of ​​the corresponding group as the obstacle avoidance constraint input to the model prediction controller. Reduce the trend compliance weight, which is the weight of the trend compliance cost term in the comprehensive cost. Step S6: Output the control sequence that solves the motion trend flow field, the robot body state acquired by the inertial measurement unit, and the global navigation target input model prediction controller in the prediction time domain, satisfying the dynamic constraints and obstacle avoidance constraints and minimizing the overall cost; the control sequence is a robot motion reference sequence or a joint control input sequence. Step S7: Issue the first control command to execute the control sequence, wherein the control command is a motion reference value command or a joint control input command; Step S8: Update the body state based on the contact feedback from the plantar pressure sensor, and repeat steps S1 to S7.

2. The control method for a legged humanoid robot based on point cloud clustering according to claim 1, characterized in that, The clustering segmentation adopts either Euclidean clustering or density clustering. The pedestrian screening rules include a height interval threshold and a point count interval threshold. Point cloud clusters that meet the thresholds are determined as pedestrian clusters.

3. The control method for a legged humanoid robot based on point cloud clustering according to claim 1, characterized in that, The data association includes: calculating the centroid displacement of pedestrian clusters in adjacent frames within a time window and converting it into a velocity vector; matching based on a distance threshold and a velocity threshold; and processing clusters that fail to match according to a loss count rule and using them for the number threshold determination.

4. The control method for a legged humanoid robot based on point cloud clustering according to claim 1, characterized in that, The merging of the motion groups satisfies the following conditions: when the centroid distance between two pedestrian clusters is ≤ the distance threshold, the velocity angle is ≤ the velocity angle threshold, and the velocity difference is ≤ the velocity difference threshold, they are merged into the same motion group; if these conditions are not met, they remain as different motion groups.

5. The control method for a legged humanoid robot based on point cloud clustering according to claim 1, characterized in that, The average motion vector is calculated according to a point weighting rule, and the spatial density distribution is generated according to the grid size range or the kernel width range. The grid size or kernel width is selected by segmenting the point cloud sparsity using a mapping table; the point cloud sparsity is a statistical index of the point cloud density within a local planning area; the mapping table is a table showing the correspondence between point cloud sparsity segments and parameter intervals.

6. The control method for a legged humanoid robot based on point cloud clustering according to claim 1, characterized in that, The confidence level of the flow field is determined by both motion consistency and spatial clustering, wherein: The confidence level is increased when the velocity direction change within K consecutive frames is less than or equal to the direction threshold and the velocity change is less than or equal to the velocity change threshold. When the spatial clustering degree is less than or equal to the clustering dispersion threshold, the confidence level is increased. The motion consistency is calculated using a time window and hysteresis rules. The time window is a sliding window consisting of multiple consecutive frames. When K consecutive frames satisfy the condition that the velocity direction change does not exceed a direction threshold and the velocity change does not exceed a velocity change threshold, the flow field confidence level is increased. When a frame fails to meet either the direction threshold or the velocity change threshold, an anomaly count is accumulated without triggering a decrease in the flow field confidence level. When M consecutive frames corresponding to the anomaly count fail to meet the conditions, the flow field confidence level is decreased. Here, K and M are positive integers. The direction threshold, the velocity change threshold, and the lower limit of the trend compliance weight are selected by segmenting the point cloud according to the sparsity of the mapping table; the lower limit of the trend compliance weight is the minimum allowable value used when limiting the trend compliance weight.

7. The control method for a legged humanoid robot based on point cloud clustering according to claim 1, characterized in that, The comprehensive cost includes at least: target tracking cost, density collision avoidance cost, and trend compliance cost; wherein the trend compliance cost is weighted in segments according to the angle between the robot's predicted motion direction and the average motion vector, and the penalty weight is increased when the angle is greater than or equal to the reverse motion threshold.

8. A control method for a legged humanoid robot based on point cloud clustering according to claim 7, characterized in that, The density collision avoidance cost is constructed according to the repulsion potential field rule. The repulsion strength is positively correlated with the group density. When the robot's predicted trajectory enters the region in the opposite direction of the group's movement direction, the repulsion strength increases with the depth of entry. The reverse direction region is the region where the robot's predicted motion direction and the average motion vector satisfy the reverse motion determination rule; the entry depth is the projection distance of the robot's predicted trajectory representative point into the group-occupied area along the reverse direction.

9. The control method for a legged humanoid robot based on point cloud clustering according to claim 1, characterized in that, The optimization problem of the model predictive controller includes soft constraints on the foot landing point, which encourages the landing point to be selected outside the boundary of a low-density area or a group with high confidence and velocity below a velocity threshold, and sets a minimum landing distance threshold. The low-density region is extracted from the spatial density distribution. Grids with density values ​​less than the density threshold are regarded as low-density grids, and the connected domains of adjacent low-density grids are regarded as candidate low-density regions. The accessibility screening of the foot landing point is performed based on the reachable step size interval and the safety margin of the supporting polygon. When the relative distance between the candidate landing point and the supporting foot falls within the reachable step size interval and the minimum distance of the centroid projection to the boundary of the supporting polygon after landing is not less than the safety margin threshold, the candidate landing point is retained. When the candidate low-density region is empty, the soft constraint of the foot landing point remains the result of the previous cycle or is not enabled, and the optimization is dominated by obstacle avoidance constraints and density collision avoidance costs. The reachable step size interval is the allowable range of the horizontal distance and azimuth angle of the candidate landing point relative to the supporting foot.

10. A control method for a legged humanoid robot based on point cloud clustering according to claim 1, characterized in that, When depth sensor data is missing or the number of associative pedestrian clusters is less than the threshold, the motion trend flow field remains the result of the previous cycle, and the trend conformation weight is limited to the weight range. The missing depth sensor data is due to at least one of the following: missing point cloud frames, insufficient number of valid point cloud points, or abnormal point cloud timestamps.

Images