Humanoid robot navigation method based on visual semantic segmentation and radar obstacle detection

Abstract

The invention belongs to the technical field of robot navigation, and particularly relates to a humanoid robot navigation method based on visual semantic segmentation and radar obstacle detection, which comprises the following steps: synchronously acquiring an RGB image and a depth image of a current environment of a humanoid robot by using a visual sensor, and acquiring point cloud data by using a laser radar; performing semantic segmentation on the preprocessed RGB image; radar point cloud obstacle detection; fusing the semantic segmentation map and the laser point cloud map, introducing a Bayesian decision to judge whether the laser point cloud map is passable or not, and then calculating a fusion cost value to obtain a fusion cost map; adopting an RRT * / TEB algorithm to output an optimal path; using a nonlinear optimization solver to generate a foothold sequence accurate to each step; according to the method, a semantic-geometric two-dimensional navigation cost model is constructed; bayesian reasoning is deeply bound with a navigation scene, so that the navigation adaptability of an unstructured environment is improved; the navigation method is suitable for the humanoid robot, and is low in energy consumption, low in navigation deviation and high in safety.

Description

Technical Field

[0001] This invention belongs to the field of robot navigation technology, specifically relating to a navigation method and system for visual semantic segmentation and radar obstacle detection for humanoid robots. Background Technology

[0002] In current robot navigation technology, multi-sensor fusion solutions for wheeled robots are relatively mature. However, humanoid robots, due to their complex movement patterns (relying on bipedal gait adjustments) and higher environmental interaction requirements (such as climbing stairs and avoiding dynamic obstacles), still face unique challenges in navigation in complex terrain environments (such as unstructured outdoor surfaces and indoor multi-obstacle scenarios). Existing technologies for humanoid robot navigation still suffer from reliance on single sensors or insufficient targeting of fusion strategies.

[0003] Typical technical approaches include pure vision navigation and pure radar navigation. Pure vision solutions acquire image data via cameras and utilize computer vision algorithms to understand the environment; pure radar solutions acquire environmental point cloud data using lidar or millimeter-wave radar to perform obstacle detection and 3D modeling. Single-sensor navigation has limitations. Pure vision navigation, lacking height information (such as step differences), often misjudges "passable areas," leading to robot collisions or falls. Pure radar navigation, lacking material recognition (such as the friction coefficient of grass), cannot adaptively adjust gait, resulting in wasted navigation energy or the risk of slippage. Existing technologies have attempted combined machine vision and lidar navigation, but these are mostly geared towards map building or localization needs for wheeled robots and do not fully adapt to the gait control characteristics of humanoid robots. For example, Chinese Patent 202311043148.9 discloses a navigation and positioning method and robot based on machine vision and LiDAR. This method combines the advantages of machine vision and LiDAR, uses a depth camera to obtain environmental depth map and RGB map data, and then converts the data into LiDAR-type data so as to fuse it with the data collected by LiDAR, and finally constructs a two-dimensional grid map of the environment. Compared with the use of a single sensor, this method can significantly improve the accuracy and completeness of the global map. Chinese Patent 202211523195.9 discloses a method for constructing a local semantic traversability probability grid map for an autonomous mobile robot. The method includes: S1: constructing an occupied grid map based on the measured values ​​of laser data; S2: performing semantic segmentation using a real-time semantic segmentation model, extracting semantic labels, and performing 2D semantic segmentation; S3: after semantic segmentation, projecting the semantic information using a depth map based on coordinate relationships, i.e., constructing a semantic grid map based on the semantic information; S4: merging the semantic grid map with a multi-layer cost map after probabilistic transformation: performing probabilistic transformation of semantic information through distance mapping to form a semantic layer cost map, and then merging it with the laser obstacle layer cost map to obtain the final local semantic traversability probability grid map.

[0004] However, the aforementioned existing technologies do not directly integrate "semantic information (material)" and "geometric information (obstacle height)" into the navigation decision chain, which makes it impossible to determine "whether it is worth detouring" in semi-accessible areas (such as light water accumulation), or to miss key obstacle points in complex terrain (such as grassy steps) due to insufficient data complementarity, directly affecting navigation safety and path efficiency. Summary of the Invention

[0005] The purpose of this invention is to overcome the shortcomings of existing technologies in humanoid robot navigation, such as insufficient adaptation of multi-sensor fusion strategies to bipedal motion characteristics, and to provide a humanoid robot navigation method based on visual semantic segmentation and radar obstacle detection, so as to achieve high-precision and highly adaptable autonomous navigation in complex environments.

[0006] To achieve the above objectives, this invention provides a humanoid robot navigation method based on visual semantic segmentation and radar obstacle detection, the specific steps of which include:

[0007] Step 1: Environmental Sensing Data Acquisition

[0008] The robot uses a visual sensor to simultaneously acquire RGB and depth maps of its current environment, and uses a lidar to collect point cloud data.

[0009] Step 2: Data Preprocessing and Feature Extraction

[0010] 1) Visual data preprocessing and feature extraction

[0011] Perform distortion correction and noise reduction on RGB images.

[0012] Hole filling and scale transformation are performed on the depth map to convert pixel distances into actual physical distances; the nearest obstacle points in each column of the depth map are extracted, and the height features of the obstacles are preserved.

[0013] 2) Laser point cloud preprocessing and point cloud denoising:

[0014] The collected point cloud data is filtered by voxel filtering to reduce redundant points and retain key geometric features. Then, the key geometric features are segmented into ground and clustered into obstacles to extract the bounding rectangle parameters of the obstacles.

[0015] Step 3: Visual semantic segmentation and radar obstacle detection

[0016] 1) Visual semantic segmentation: The SegFormer-B2 network model based on Transformer and known material properties are used to perform semantic segmentation on the preprocessed RGB image to obtain a semantic segmentation image with material properties.

[0017] 2) Radar point cloud obstacle detection: Based on the circumscribed rectangle parameters of the obstacle and the IMU data obtained in step 2, the laser-inertial navigation tight coupling is achieved through the LIO-SAM algorithm to optimize the point cloud pose accuracy, generate a laser point cloud map, and then perform obstacle judgment, marking "crossable obstacle" or "non-crossable obstacle".

[0018] Step 4: Multi-source information fusion and cost map construction

[0019] First, the RGB image obtained by the visual sensor and the point cloud data collected by the radar are divided into 0.1m×0.1m grid units. Then, the visual semantic segmentation map and the laser point cloud map of the radar are fused, and Bayesian decision is introduced to determine whether each grid is passable and to mark it with a qualitative label of "safe path", "cautious path" or "prohibited path". Then, the fusion cost value of each grid marked with "safe path" and "cautious path" is calculated to obtain the fusion cost map.

[0020] Step 5: Path Planning

[0021] The RRT* algorithm is used for global path planning, and the TEB algorithm is used for local path planning. The optimal path is output by combining the differential obstacle judgment logic (distinguishing between passable and impassable obstacles) with "qualitative labels" and "fusion cost".

[0022] Step 6: Footprint Sequence Generation

[0023] Using a nonlinear optimization solver, based on the local path and incorporating ground material properties and obstacle geometry, a sequence of precise landing points is generated for each step. This enables the robot to maintain stability and move forward efficiently in complex environments. Specifically: First, the local path is divided into several segments at equal intervals according to the step length corresponding to the current ground material, with the endpoint of each segment serving as a candidate landing point. Then, the obstacle information corresponding to each candidate point is checked. If there is an traversable obstacle ahead, the landing point position for that step is adjusted, and the corresponding foot lift height is set. If an insurmountable obstacle is encountered, it is marked as needing to be detoured, and the local path is recalculated by the upper-level path planning. After determining the landing point position, the toe direction is calculated based on the turning curvature of the local path, generally within the range of 0° to 30°, to ensure stability during turns or crossings. The output is a set of landing point information arranged in chronological order, including the three-dimensional coordinates of each step and the toe direction angle. The results are directly transmitted to the gait control module of the humanoid robot, realizing closed-loop control from path planning to actual walking.

[0024] The specific steps of semantic segmentation in step 3 of this invention are as follows: global features are extracted by the Transformer encoder, pixel-level semantic labels are output by the decoder, and the labels are divided into 3 categories according to the ground material attributes: dry cement ground and hard flat ground are passable areas, marked "label 0"; grass, light water accumulation and slopes with a gradient of <15° are semi-passable areas, marked "label 1"; deep pits, stairs and water surfaces are impassable areas, marked "label 2"; and a semantic segmentation map with labels and material attributes is output.

[0025] The criterion for obstacle judgment in step 3 of the present invention is that when the obstacle height is less than the robot's maximum leg lifting height and the width is less than the distance between the feet, it is marked as "crossable obstacle"; otherwise, it is marked as "uncrossable obstacle".

[0026] The Bayesian decision-making criteria in step 4 of this invention are as follows: if the visual semantic segmentation map is labeled "Label 0" and the radar / laser cloud point map is labeled "crossable obstacle", it is determined to be a "safe path"; if the visual semantic segmentation map is labeled "Label 1" or the radar / laser cloud point map is labeled "crossable obstacle", it is determined to be a "cautious path"; if the visual semantic segmentation map is labeled "Label 2" or the radar / laser cloud point map is labeled "insurmountable obstacle", it is determined to be a "prohibited path".

[0027] The method for calculating the fusion cost value in step 4 of this invention is as follows: For the "safe path" and "cautious path" selected by Bayesian decision-making, calculate the semantic cost value and geometric cost value of each grid cell, and then calculate the fusion cost value. Specifically, first, calculate the semantic cost value based on the semantic segmentation map: the semantic cost value of traversable areas is 0, the semantic cost value of semi-traversable areas is 50, and the semantic cost value of impassable areas is 100; then calculate the geometric cost value based on the straight-line distance between the robot's current position and the obstacle on the laser point cloud map. The closer the distance, the higher the cost value. The formula for calculating the geometric cost value is: .

[0028] The formula for calculating the total cost of fusion is as follows: ,in, This represents the semantic cost weighting coefficient.

[0029] The input to the nonlinear optimization solver in step 6 of this invention includes three parts: local path, ground material, and obstacle attributes. The local path is obtained from the local path optimization result of the TEB algorithm in the previous step. The path has been smoothed and meets obstacle avoidance constraints, but it has not yet been refined to specific landing points. The ground material is obtained from the visual semantic segmentation in step 3. The material categories include cement, grass, water, etc. Different materials will affect the robot's stride length, stride frequency, and gait. The obstacle attributes come from the laser point cloud map, which can accurately obtain the height, width, and position of obstacles. If necessary, the depth map can be used to assist in the measurement of nearby obstacles. If the obstacle height is less than the robot's maximum leg lift height and the width does not exceed the foot spacing, it can be crossed directly. Otherwise, it needs to be avoided at the path level.

[0030] This invention directly integrates semantic information (material) and geometric information (obstacle height) into the navigation decision chain, enabling the robot to plan a path in real time based on the map, and then generate a footprint for the robot based on the path, thus realizing closed-loop control of the path and footprint.

[0031] Compared with the prior art, the present invention has the following beneficial effects:

[0032] (1) In terms of information dimension, existing technical maps, mainly for wheeled robots, are only single-dimensional, while the cost map (for humanoid robots) in this application is a deep fusion of two dimensions: it includes both "semantic information (ground material, friction coefficient, access level labels 0 / 1 / 2)" and "geometric information (obstacle distance, height, traversability)", which are bound together by cost formula and Bayesian decision. It upgrades from "single environment description" to "decision basis for adapting bipedal movement", solving the core needs of humanoid robots to consider both "material safety (slipping, energy consumption)" and "obstacle crossing (step length, leg lift height)".

[0033] (2) In terms of functional positioning, existing technologies serve "map construction / positioning" with the goal of improving map accuracy (e.g., patent 202311043148.9 emphasizes "global map integrity"), without directly linking navigation decision-making and motion control; while this invention serves "navigation decision-making + gait control": it directly outputs "safe / cautious / prohibited" path labels through rasterization cost (semantic cost + geometric cost), while providing parameters for subsequent footprint generation and gait adjustment (e.g., material → gait frequency mapping). By skipping the intermediate transformation link of "map → decision-making", it achieves end-to-end connection of "perception-planning-execution", avoiding the problem of existing technologies having "high map accuracy but being disconnected from human movement" (e.g., navigation interruption and energy waste in complex terrain).

[0034] (3) Regarding environmental adaptability, existing technologies adapt to the "planar movement" of wheeled robots, but do not consider the impact of unstructured environments (grass, water, steps) on bipedal gait. They can only determine "whether it is passable" but cannot determine "whether it can be crossed / how to adjust the gait". In contrast, this invention adapts to the "bipedal movement" of humanoid robots: for semi-passable areas (such as light water) and crossable obstacles (such as 10cm curbs), it provides strategies such as "small step exploration" and "lifting the leg to cross" through Bayesian decision-making, and combines cost models to balance safety and efficiency. This solves the shortcomings of existing technologies in "missing key obstacle points" and "not being able to determine whether it is worthwhile to detour" in unstructured environments, and improves the navigation adaptability of humanoid robots in complex scenarios such as outdoor grass and indoor steps.

[0035] In summary, this invention constructs a semantic-geometric dual-dimensional navigation cost model, directly transforming sensor data into navigation decision factors such as "passability" and "crossability." It deeply integrates Bayesian inference with the navigation scenario (e.g., prioritizing a "small-step exploration" strategy in semi-accessible areas), improving navigation adaptability in unstructured environments. End-to-end optimization from path planning to gait control is achieved through a "material-gait" mapping table (e.g., concrete → fast gait frequency), reducing navigation energy consumption and navigation deviation when crossing obstacles. Nonlinear QP optimization directly serves navigation safety, ensuring that footprints are always located in passable areas, avoiding the risk of missteps during navigation. This invention's navigation method is suitable for humanoid robots, exhibiting high environmental adaptability, low energy consumption, low navigation deviation, and high safety. Attached Figure Description

[0036] Figure 1 This is a schematic diagram illustrating the principle of the humanoid robot navigation method based on visual semantic segmentation and radar obstacle detection involved in this invention.

[0037] Figure 2 The diagram shows the effect of the humanoid robot navigation method of the present invention, where A is the route planned by a general navigation method and B is the route planned by the navigation method of the present invention. Detailed Implementation

[0038] The present invention will be further described in detail below with reference to specific embodiments and accompanying drawings.

[0039] Example 1:

[0040] This embodiment relates to a humanoid robot navigation method based on visual semantic segmentation and radar obstacle detection, the specific steps of which include:

[0041] Step 1: Environmental Sensing Data Acquisition

[0042] The humanoid robot's current environment is simultaneously acquired using a visual sensor (RGB image and depth map) and point cloud data is collected using a LiDAR scanner. The specific steps are as follows:

[0043] 1) Visual sensor data acquisition

[0044] Visual sensors (such as the Intel RealSense depth camera) are used to simultaneously acquire RGB and depth maps of the environment. The RGB map is used for subsequent semantic segmentation, with a resolution of 1280×720 and a frame rate of 30fps, focusing on collecting ground material (such as grass, concrete, and puddles) and obstacle appearance features. The depth map is used to extract three-dimensional spatial information, acquiring pixel-level distance data with a range of 0.5-10m through a built-in infrared sensor, and preserving geometric features such as obstacle height and outline.

[0045] 2) LiDAR data acquisition

[0046] Point cloud data is collected using a 16-line lidar (such as Velodyne VLP-16), with a scanning frequency of 10Hz, a horizontal field of view of 360°, a vertical field of view of ±15°, and a point cloud density of ≥200 points / ㎡; the three-dimensional coordinates (X,Y,Z) and reflection intensity of the detected target are obtained for obstacle localization and terrain modeling.

[0047] Step 2: Data Preprocessing and Feature Extraction

[0048] 1) Visual data preprocessing and feature extraction

[0049] Distortion correction (based on camera intrinsic matrix) and denoising (Gaussian filtering, σ=1.0) are performed on the RGB image.

[0050] Hole filling (using neighborhood interpolation) and scale transformation are performed on the depth map to convert pixel distances into actual physical distances (unit: meters).

[0051] Extract the nearest obstacle point in each column of the depth map (take the point in each column that is closest to the imaging plane as the nearest obstacle detected by the visual sensor), and retain the height features of the obstacle (extraction formula: (d_i) = min(r_{i1},r_{i2}, ..., r_{ij}), j=1, 2, 3……N; where (d_i) is the distance of the nearest obstacle in the i-th column from the imaging plane, and r_{ij} is the height information of the j-th pixel in the column).

[0052] 2) Laser point cloud preprocessing and point cloud denoising:

[0053] The collected point cloud data is filtered by voxel filtering (voxel size 0.1m×0.1m×0.1m) to reduce redundant points and retain key geometric features; then the key geometric features are used for ground segmentation and obstacle clustering.

[0054] Ground segmentation: The RANSAC algorithm is used to fit the ground plane (plane equation: Ax + By + Cz + D = 0) to separate ground points from obstacle points.

[0055] Obstacle clustering: The DBSCAN algorithm (neighborhood radius 0.3m, minimum number of points 5) is used to extract the bounding rectangle parameters (length, width, height) of obstacles.

[0056] Step 3: Visual semantic segmentation and radar obstacle detection

[0057] 1) Visual Semantic Segmentation: The preprocessed RGB image is semantically segmented using the Transformer-based SegFormer-B2 (SOTA) network model and known material properties to obtain a semantic segmentation map with material properties. Specifically, the Transformer encoder extracts global features, and the decoder outputs pixel-level semantic labels. Based on the ground material properties, the labels are divided into three categories: dry cement ground and hard flat ground (confidence ≥ 0.9) are passable areas, labeled "Label 0"; grass, light water accumulation, and slopes with a gradient < 15° (confidence 0.5-0.9) are semi-passable areas, labeled "Label 1"; deep pits, stairs, and water surfaces (confidence ≥ 0.8) are impassable areas, labeled "Label 2". The output is a semantic segmentation map with labels and material properties (such as friction coefficient: cement ground 0.8, grass 0.6, water accumulation 0.3) (at the same resolution as the RGB image).

[0058] 2) Radar point cloud obstacle detection: Based on the circumscribed rectangle parameters of the obstacle and IMU data (acceleration, angular velocity) obtained in step 2, the laser-inertial navigation tight coupling is achieved through the LIO-SAM algorithm to optimize the point cloud pose accuracy (error ≤ 2cm), generate a laser point cloud map, and then judge the obstacle, marking it as "crossable obstacle" or "non-crossable obstacle"; the IMU is the inertial measurement unit carried by the robot, used to acquire the robot's motion state in real time.

[0059] Obstacle detection: When the obstacle height is less than the robot's maximum leg lift height (e.g., 15cm) and the width is less than the distance between the legs (e.g., 20cm), it is marked as "crossable obstacle"; otherwise, it is marked as "insurmountable obstacle", and its three-dimensional coordinates and contour information are output.

[0060] Step 4: Multi-source information fusion and cost map construction

[0061] First, the RGB image obtained by the current visual sensor and the point cloud data collected by the radar are divided into 0.1m×0.1m grid units; then, the visual semantic segmentation map and the radar laser point cloud map are fused, and Bayesian decision-making is introduced to determine whether each grid is passable. Finally, the fusion cost value of each grid is calculated to obtain the fusion cost map, specifically:

[0062] 1) Semantic – Point Cloud Map Feature Fusion:

[0063] The visual semantic segmentation map and the laser point cloud map obtained in step 3 are transformed to the same robot coordinate system through the extrinsic parameter matrix (rotation matrix R of camera and radar, translation vector T). After fusion, a "semantic-geometric fusion map" is obtained. Then, Bayesian decision-making is introduced to determine the path. Bayesian inference is used to determine the "feasibility of passage" of each grid / path segment and outputs a qualitative label of "safe / cautious / prohibited".

[0064] The "semantic-geometric fusion map" uses the robot's coordinate system as a reference and aligns the "semantic attribute information" (such as ground material and accessibility labels) output by the visual sensors with the "geometric spatial information" (such as obstacle 3D coordinates and terrain height) output by the LiDAR at the pixel / grid level, forming an integrated environment model where "each spatial location simultaneously contains semantic labels and geometric parameters." For example, a certain grid cell (0.1m × 0.1m) in the fusion map is not only labeled "Label 1 (semi-accessible area, material is grass, friction coefficient 0.6)" but also labeled "nearest obstacle distance 0.8m, obstacle height less than 15cm, obstacle can be crossed," providing complete data support for subsequent Bayesian decision-making and cost calculation.

[0065] Bayesian decision: If the visual semantic segmentation map is labeled "Label 0" and the radar / laser cloud point map is labeled "Crossable obstacle", it is determined as a "safe path"; if the visual semantic segmentation map is labeled "Label 1" or the radar / laser cloud point map is labeled "Crossable obstacle", it is determined as a "cautious path"; if the visual semantic segmentation map is labeled "Label 2" or the radar / laser cloud point map is labeled "Uncrossable obstacle", it is determined as a "prohibited path".

[0066] 2) Construction of the cost-of-fusion map:

[0067] For the "safe path" and "cautious path" selected by Bayesian decision-making, calculate the semantic cost value and geometric cost value of each grid cell, and then calculate the fusion cost value to obtain the fusion cost map.

[0068] The semantic cost is calculated based on the semantic segmentation graph: the semantic cost of a passable region is 0, the semantic cost of a semi-passable region is 50, and the semantic cost of an impassable region is 100.

[0069] Geometric cost is used to quantify obstacle avoidance risk in a given area. It is based on the straight-line distance between the robot's current position and obstacles on the laser point cloud map; the closer the distance, the higher the cost. The formula for calculating geometric cost is: .

[0070] The total cost of integration is used to quantify environmental risk. Risk levels are ranked according to the magnitude of the integration cost; the higher the integration cost, the greater the risk. The calculation formula is: .

[0071] in, The semantic cost weight coefficient is an adjustable parameter that dynamically adapts to the scenario, comprehensively considering semantic labels and obstacle markers, with a value range of 0-1. For example, when the semantic label is 0 (passable area), if there are no obstacles, i.e., the obstacle avoidance requirement is low, 'a' can be 0.3-0.4; if the obstacle avoidance requirement is high, 'a' can be 0.1-0.2. When the semantic label is 1 (semi-passable area), if semantic risk is the primary concern (e.g., all grass), 'a' is 0.6-0.7; if obstacle avoidance is the primary concern, 'a' is 0.4-0.5. When the semantic label is 2 (impassable area), 'a' should be 1, at which point the fusion cost value is 100, the maximum value. A fusion cost value between 0-50 can be defined as low risk, between 50-70 as medium risk, and between 70-100 as high risk.

[0072] Step 5: Path Planning

[0073] The algorithm employs the RRT* algorithm for global path planning and the TEB algorithm for local path planning. It uses differentiated obstacle judgment logic (distinguishing between passable and impassable obstacles) combined with semantic tags and fusion cost values ​​to collaboratively output the optimal path. Specifically, this includes:

[0074] 1. Global Path Planning (based on RRT* algorithm)

[0075] Input: Fusion cost map (obtaining semantic labels and fusion cost information), coordinates of the starting point and the target point.

[0076] Constraints: The distance between the path and the impassable area is ≥0.5m, and the cumulative turning angle is ≤180°; the path is allowed to pass through passable obstacles (such as low curbs ≤5cm, grass depressions), but must be marked as "areas requiring gait adjustment"; the cumulative turning angle is ≤180°; the objective is to "minimize the total cost of integration".

[0077] Output: Global optimal path (consisting of a series of waypoints, spaced 1m apart, including passability attribute labels).

[0078] 2. Local path optimization (based on the TEB algorithm)

[0079] The core of local path planning is to deal with real-time dynamic obstacles (such as sudden pedestrians). Based on the global path, combined with real-time radar scanning data, the fusion cost increment of the grid around the dynamic obstacle is calculated, the local path is finely adjusted, and the corresponding gait is triggered according to the "risk level" of the fusion cost. The gait is a small step trial, which is triggered by medium risk in the global path.

[0080] The input to the TEB algorithm is the global path (including accessibility attributes) and the robot kinematics model (e.g., maximum speed 1 m / s, maximum acceleration 0.5 m / s²); the robot kinematics model refers to the robot's own hardware parameters.

[0081] Optimization objectives: path smoothness (curvature ≤ 0.5 rad / m), obstacle avoidance safety (distance from obstacles ≥ 0.3 m); gait adaptability (prioritize the passage area that matches the robot's stride length).

[0082] The output of the TEB algorithm is: Local Refinement Path (Timestamp + Position Coordinates + Gait Adjustment Instructions), where the timestamp is the time when the robot is expected to arrive at the path point, and the position coordinates are the spatial location of the refinement path.

[0083] For example, in a grassy area (semantic label 1, semantic cost 50) and 0.5m from an obstacle (geometric cost = 100 / (0.5+1)≈66.7), if α=0.6, the fusion cost = 0.6×50 + 0.4×66.7≈56.7, it will be judged as a "medium-risk segment in a cautious path". During global path planning, it will prioritize bypassing this area and selecting safer paths in the surrounding area (areas with a fusion cost less than 50). If there are other paths with higher fusion costs after bypassing this area, then this area will be included in the candidate list. During local path planning, assuming the global planning selects a "safe path", but the LiDAR suddenly detects a dynamic obstacle (such as a pedestrian walking into the original safe path), then the geometric cost increment around the dynamic obstacle must be recalculated, and the new fusion cost is recalculated in combination with the original semantic cost to generate a locally adjusted path. If the global planning selects the aforementioned grassy area, then a "small step probing" gait instruction is generated, such as adjusting the stride to reduce the risk of slipping with each step.

[0084] Step 6: Footprint sequence generation (based on nonlinear QP optimization)

[0085] Using a nonlinear optimization solver, based on the local path and combining ground material properties and obstacle geometry information, a sequence of landing points accurate to each step is generated, enabling the robot to maintain stability and move forward efficiently in complex environments. First, the local path is divided into several segments at equal intervals according to the step length corresponding to the current ground material, and the endpoint of each segment is used as a candidate landing point. Then, the obstacle information corresponding to each candidate point is checked. If there is an obstacle that can be crossed ahead, the landing point position for that step is adjusted and the corresponding foot lift height is set. If an obstacle that cannot be crossed is encountered, it is marked as needing to be detoured, and the local path is recalculated by the upper-level path planning. After determining the landing point position, the toe direction is calculated based on the turning curvature of the local path, generally within the range of 0° to 30°, to ensure stability when turning or crossing.

[0086] The input to the nonlinear optimization solver consists of three parts: local path, ground material, and obstacle properties. The local path is obtained from the local path optimization result of the TEB algorithm in the previous step. The path has been smoothed and satisfies obstacle avoidance constraints, but it has not yet been refined to specific landing points. The ground material is obtained from the visual semantic segmentation in step 3. Material categories include concrete, grass, and puddles. Different materials affect the robot's stride length, stride frequency, and gait. For example, concrete is flat and has high friction, so a stride length of 0.5m and a higher stride frequency can be used; grass has low friction and is soft, so the stride length can be shortened to 0.3m and the stride frequency can be appropriately reduced; puddles have a low coefficient of friction. The stride length can be further shortened to 0.25m, and the center of gravity lowered to prevent slippage. Obstacle attributes are derived from a laser point cloud map, accurately acquiring the height, width, and position of obstacles. If necessary, a depth map assists in measuring nearby obstacles. Obstacles can be directly crossed if their height is less than the robot's maximum leg lift height and their width does not exceed the foot spacing; otherwise, they must be avoided at the path level. The input to the nonlinear optimization solver also includes robot leg kinematic parameters, including inherent parameters of the robot hardware (such as leg link length, joint range of motion, maximum stride length, and maximum leg lift height) and real-time sensor parameters (including joint angles). These sensors include joint encoders, six-dimensional force sensors, and IMU (Inertial Measurement Unit).

[0087] The optimization goal is The constructed minimum objective function for:

[0088]

[0089]

[0090] in: Footprint sequence, each The three-dimensional coordinates of the landing point ; : The coordinates of the reference path point corresponding to the k-th landing point on the local path; : The expected step length corresponding to the current ground material (determined by the built-in material-gait mapping table, for example, 0.5m for cement and 0.3m for grass). The height of obstacles corresponding to the landing point (from laser point cloud or depth map); The robot's maximum traverse height; The angle at which the toes point in step k; Weighting coefficients are used to balance the importance of different objectives (e.g., path following is more important than energy consumption, so weighting coefficients can be used to balance the importance of different objectives). ).

[0091] The optimization solution uses a nonlinear QP solver. Under the constraints of gait stability, obstacle avoidance safety, and energy minimization, the final footprint sequence is output. The output result is a set of foot placement information arranged in chronological order, including the three-dimensional coordinates (X,Y,Z) of each step and the toe pointing angle (0° to 30°). This result will be directly transmitted to the gait control module of the humanoid robot to realize closed-loop control from path planning to actual walking.

[0092] Example 2:

[0093] This embodiment is an experiment demonstrating the effectiveness of the method described in Embodiment 1. Figure 2 As shown in Figure A, the result of conventional path planning is presented by a blue dashed line. Starting from the marked "starting point", the path needs to take a long detour to avoid the marked "obstacles" and "puddles". The overall route is relatively tortuous, and in complex areas, it is necessary to take a roundabout approach to reach the "target point".

[0094] Figure B shows the path planned by the navigation method of this invention, marked with footsteps. The humanoid robot can use its ability to cross obstacles to plan a path more directly by stepping when encountering gravel roads, "obstacles", and "puddles". Compared with the conventional path on the left, the route is more efficient, simpler and more in line with the movement characteristics of the humanoid robot.

Claims

1. A humanoid robot navigation method based on visual semantic segmentation and radar obstacle detection, characterized in that, The specific steps include: Step 1: Environmental Sensing Data Acquisition The robot uses a visual sensor to simultaneously acquire RGB and depth maps of its current environment, and uses a lidar to collect point cloud data. Step 2: Data Preprocessing and Feature Extraction 1) Visual data preprocessing and feature extraction Perform distortion correction and noise reduction on RGB images; Hole filling and scale transformation are performed on the depth map to convert pixel distances into actual physical distances; the nearest obstacle points in each column of the depth map are extracted, and the height features of the obstacles are preserved; 2) Laser point cloud preprocessing and point cloud denoising: The collected point cloud data is filtered by voxel to reduce redundant points and retain key geometric features; then the key geometric features are segmented into ground and clustered into obstacles to extract the bounding rectangle parameters of the obstacles. Step 3: Visual semantic segmentation and radar obstacle detection 1) Visual semantic segmentation: The preprocessed RGB image is semantically segmented using the SegFormer-B2 network model based on the transformer and the known material properties to obtain a semantic segmentation image with material properties; 2) Radar point cloud obstacle detection: Based on the circumscribed rectangle parameters of the obstacle and the IMU data obtained in step 2, the laser-inertial navigation tight coupling is realized through the LIO-SAM algorithm to optimize the point cloud pose accuracy, generate a laser point cloud map, and then the obstacle is judged and marked as "crossable obstacle" or "non-crossable obstacle". Step 4: Multi-source information fusion and cost map construction First, the RGB image obtained by the visual sensor and the point cloud data collected by the radar are divided into 0.1m×0.1m grid units. Then, the visual semantic segmentation map and the laser point cloud map of the radar are fused, and Bayesian decision is introduced to determine whether each grid is passable and to mark it with a qualitative label of "safe path", "cautious path" or "prohibited path". Then, the fusion cost value of each grid marked "safe path" and "cautious path" is calculated to obtain the fusion cost map. Step 5: Path Planning The RRT* algorithm is used for global path planning, and the TEB algorithm is used for local path planning. The optimal path is output by combining differentiated obstacle judgment logic with "semantic labels" and "fusion cost". Step 6: Footprint Sequence Generation Using a nonlinear optimization solver, based on the local path and incorporating ground material properties and obstacle geometry, a sequence of precise landing points is generated for each step. This enables the robot to maintain stability and move forward efficiently in complex environments. Specifically: First, the local path is divided into several segments at equal intervals according to the step length corresponding to the current ground material, with the endpoint of each segment serving as a candidate landing point. Then, the obstacle information corresponding to each candidate point is checked. If there is an traversable obstacle ahead, the landing point position for that step is adjusted, and the corresponding foot lift height is set. If an insurmountable obstacle is encountered, it is marked as needing to be detoured, and the local path is recalculated by the upper-level path planning. After determining the landing point position, the toe direction is calculated based on the turning curvature of the local path, generally within the range of 0° to 30°, to ensure stability during turns or crossings. The output is a set of landing point information arranged in chronological order, including the three-dimensional coordinates of each step and the toe direction angle. The results are directly transmitted to the gait control module of the humanoid robot, realizing closed-loop control from path planning to actual walking.

2. The humanoid robot navigation method based on visual semantic segmentation and radar obstacle detection according to claim 1, characterized in that, The specific steps of semantic segmentation in step 3 are as follows: global features are extracted by the encoder, and pixel-level semantic labels are output by the decoder. The labels are divided into 3 categories according to the ground material attributes: dry cement ground and hard flat ground are passable areas and are marked "label 0"; grass, light water accumulation and slopes with a gradient of <15° are semi-passable areas and are marked "label 1"; deep pits, stairs and water surfaces are impassable areas and are marked "label 2"; and a semantic segmentation map with labels and material attributes is output.

3. The humanoid robot navigation method based on visual semantic segmentation and radar obstacle detection according to claim 1, characterized in that, The criterion for obstacle judgment in step 3 is that if the obstacle height is less than the robot's maximum leg lifting height and the width is less than the distance between the legs, it is marked as "crossable obstacle"; otherwise, it is marked as "uncrossable obstacle".

4. The humanoid robot navigation method based on visual semantic segmentation and radar obstacle detection according to claim 1, characterized in that, The judgment criteria for Bayesian decision-making in step 4 are: If the visual semantic segmentation map is labeled "Label 0" and the radar laser cloud point map is labeled "Crossable obstacle", then it is determined to be a "safe path"; If the visual semantic segmentation map is marked "Label 1" or the radar / laser cloud point map is marked "Crossable obstacle", it is determined as a "cautious path"; If the visual semantic segmentation map is marked with "Label 2" or the radar / laser cloud point map is marked with "Insurmountable obstacle", it is determined as a "prohibited path".

5. The humanoid robot navigation method based on visual semantic segmentation and radar obstacle detection according to claim 1, characterized in that, The calculation method for the fusion cost value in step 4 is as follows: For the "safe path" and "cautious path" selected by Bayesian decision-making, calculate the semantic cost value and geometric cost value of each grid cell, and then calculate the fusion cost value. Specifically, first, calculate the semantic cost value based on the semantic segmentation map: the semantic cost value of traversable areas is 0, the semantic cost value of semi-traversable areas is 50, and the semantic cost value of impassable areas is 100; then calculate the geometric cost value based on the straight-line distance between the robot's current position and the obstacle on the laser point cloud map. The closer the distance, the higher the cost value. The formula for calculating the geometric cost value is: ； The formula for calculating the total cost of fusion is as follows: ； in, This represents the semantic cost weighting coefficient.

6. The humanoid robot navigation method based on visual semantic segmentation and radar obstacle detection according to claim 1, characterized in that, The input to the nonlinear optimization solver in step 6 includes three parts: local path, ground material, and obstacle attributes. The local path is obtained from the local path optimization result of the TEB algorithm in the previous step. The path has been smoothed and meets obstacle avoidance constraints, but it has not yet been refined to the specific landing point. The ground material is obtained from the visual semantic segmentation in step 3. The material categories include cement, grass, and puddles. Different materials will affect the robot's stride length, stride frequency, and gait. The obstacle attributes come from the laser point cloud map, which can accurately obtain the height, width, and position of obstacles. The depth map assists in the measurement of near obstacles. If the obstacle height is less than the robot's maximum leg lift height and the width does not exceed the foot spacing, it can be crossed directly. Otherwise, it needs to be avoided at the path level.

Images