A robot obstacle avoidance method and system based on millimeter-wave radar sparse point clouds

Abstract

This invention discloses a robot obstacle avoidance method and system based on millimeter-wave radar sparse point clouds, relating to the field of obstacle avoidance and recognition technology. A robot obstacle avoidance system based on millimeter-wave radar sparse point clouds includes: a point cloud acquisition module, a negative obstacle recognition module, a weak obstacle recognition module, a point cluster recognition module, a risk map module, a trial and verification module, and an obstacle avoidance decision module. This invention extracts suspected obstacle point clusters from the enhanced point cloud based on reflection intensity thresholds and spatial proximity relationships. Under the constraints of the robot's motion trajectory, it constructs a theoretical parallax model of real static obstacles. Simultaneously, it compares the Doppler velocity distribution of each frame with the Doppler physical laws of static obstacles, classifying point clusters that do not satisfy multi-view geometric consistency or Doppler physical laws, distinguishing between multipath false point clusters and dynamic point clusters.

Description

Technical Field

[0001] This invention relates to the field of obstacle avoidance and recognition technology, and in particular to a robot obstacle avoidance method and system based on millimeter-wave radar sparse point clouds. Background Technology

[0002] In mobile robots, millimeter-wave radar is increasingly being applied to environmental perception and obstacle avoidance scenarios due to its all-weather capability, resistance to rain and fog, and certain penetration ability against both metallic and non-metallic targets. The sparse point cloud output by millimeter-wave radar contains information such as distance, azimuth, reflection intensity, and Doppler velocity. By jointly processing the point cloud with the robot's pose and velocity information, the robot can obtain environmental geometry and some motion information in complex indoor and outdoor environments, thereby planning obstacle avoidance trajectories locally or globally. To enhance the advantages of millimeter-wave radar in low-visibility and obstructed environments, the industry has proposed methods based on multi-frame point cloud accumulation and simple grid map construction to enhance and utilize single-frame sparse point clouds.

[0003] Existing obstacle avoidance methods based on millimeter-wave radar still have several shortcomings. On the one hand, some schemes rely solely on single-frame point clouds or fixed time windows for accumulation, failing to fully utilize robot pose and velocity information. This limits the spatial alignment accuracy and temporal consistency of point clouds, making it difficult to stably characterize ground morphology and obstacle contours under sparse observation conditions. Negative obstacle regions (such as steps and potholes) are difficult to distinguish from ordinary ground elevation variations. On the other hand, in complex material and multipath environments, weakly reflective obstacles are mixed with noise and background echoes. Multipath false point clusters intertwine with real static and dynamic obstacles on the point cloud. The lack of a joint discrimination mechanism combining historical echo statistics, multi-view geometric relationships, and Doppler physical laws leads to missed detection of weakly reflective obstacles, false detection of multipath obstacles, and insufficient handling of dynamic obstacles. The trajectory planning stage lacks verification perception and local correction steps for high-risk areas, easily resulting in overly conservative or overly aggressive obstacle avoidance behaviors. Summary of the Invention

[0004] This invention proposes a robot obstacle avoidance method based on millimeter-wave radar sparse point clouds. Under millimeter-wave radar sparse point cloud conditions, it fully utilizes pose and velocity information to uniformly perceive and classify obstacles of various types. It performs hierarchical modeling in a robot-centered risk grid map and reduces false detections, missed detections, and trajectory conservatism during obstacle avoidance through candidate trajectory generation, enhanced orientation perception, and confirmatory micro-motions, thereby improving the safety and passability of robot obstacle avoidance.

[0005] To achieve the above objectives, the technical solution adopted by the present invention is as follows:

[0006] A robot obstacle avoidance method based on millimeter-wave radar sparse point clouds includes:

[0007] Acquire the sparse point cloud, pose information, and velocity information of the robot in the current frame; compensate the sparse point cloud based on the pose information and perform reverse registration to obtain the enhanced point cloud;

[0008] The robot's front area is divided into several sectors according to its azimuth angle. The enhanced point cloud of each sector is fitted with local ground to obtain the ground model of each sector. By comparing the height continuity of the local ground models of adjacent sectors, negative obstacle areas are identified.

[0009] Based on the millimeter-wave radar echo data collected during the current operation and the historical echo statistics of preset locations in the environmental prior map, combined with the current time series accumulation, weak reflection obstacle areas are identified.

[0010] In the augmented point cloud, detect suspected obstacle point clusters and determine whether they are multipath false point clusters or dynamic point clusters; mark multipath false point clusters as low-confidence obstacles, dynamic point clusters as dynamic obstacles, and other suspected obstacle point clusters as real obstacles;

[0011] Negative obstacle areas, weak reflection obstacle areas, low confidence obstacles, dynamic obstacles, and real obstacles are mapped onto a robot-centered risk grid map. Different risk weights and safety expansion radii are set for different types of obstacles, and the risk level of each grid is calculated.

[0012] Candidate trajectories are generated. For high-risk areas of the candidate trajectories, millimeter-wave radar is used to enhance directional perception, and the robot is controlled to perform verification micro-movements.

[0013] After confirmatory micro-movements, update the risk grid map and plan the final obstacle avoidance trajectory on the updated risk grid map.

[0014] As a preferred embodiment of the present invention, the acquisition of the enhanced point cloud includes: acquiring the sparse point cloud through a millimeter-wave radar installed on the robot, including distance, azimuth angle, reflection intensity, and Doppler velocity, and the pose information being the robot's current position and attitude information; performing attitude compensation on the sparse point cloud based on the position and attitude information and transforming it to the robot coordinate system, and determining an adaptive time window based on the velocity information; back-registering several frames of historical sparse point cloud within the adaptive time window to a unified robot coordinate system according to the corresponding pose information, and merging it with the current one to obtain the enhanced point cloud.

[0015] As a preferred embodiment of the present invention, the identification of negative obstacle regions includes: dividing a preset distance range in front of the robot into several distance segments along the distance direction; for each distance segment, obtaining the corresponding ground height information based on the ground model of each sector, and determining whether the ground models of each sector within the target distance segment are continuous; for a certain distance segment, when it is detected that a certain sector cannot be fitted with a ground model that satisfies the height continuity criterion by the augmented point cloud in the corresponding sector within the target distance segment, and the ground height of the target sector in the target distance segment is reduced by more than a preset height threshold relative to the ground height of the adjacent sector in the target distance segment, the spatial region corresponding to the target sector in the target distance segment is identified as a negative obstacle region.

[0016] As a preferred embodiment of the present invention, the identification of weak reflection obstacle regions includes: during one or more runs of the robot targeting the target environment, constructing or updating a priori environmental map based on sparse point cloud and pose information, and recording historical echo statistics of static structures corresponding to preset positions in the priori environmental map; statistically analyzing the millimeter-wave radar echoes of the grids corresponding to the static structures at multiple times, and marking the target grid as a candidate when the proportion of times a certain grid has no echo or an echo intensity lower than a first preset threshold during multiple frame observations or multiple runs is greater than a preset proportion. Selecting a region: During the robot's current operation, extracting points from the enhanced point cloud that fall near the edge of the candidate region and have a reflection intensity lower than a second preset threshold as low-intensity candidate points, and accumulating the spatial positions of the low-intensity candidate points temporally within an adaptive time window. When a certain spatial position is hit by low-intensity candidate points a number of times within the adaptive time window is not less than a preset number and its spatial position change range is less than a preset spatial threshold, the point at the corresponding spatial position is determined as the edge point of the weak reflection obstacle. Expanding the region with the edge point as the center according to a preset safety distance, and marking the expanded region as the weak reflection obstacle region.

[0017] As a preferred embodiment of the present invention, the determination of multipath false point clusters or dynamic point clusters includes: in the enhanced point cloud, selecting points with reflection intensity higher than the noise threshold as candidate obstacle points; clustering based on the spatial distance and point number threshold between candidate obstacle points; and selecting clustering results that meet a preset spatial proximity threshold and have a point number not lower than a preset minimum point number threshold as suspected obstacle point clusters; for each suspected obstacle point cluster, acquiring multi-frame enhanced point clouds covering the suspected obstacle point cluster during robot motion, and calculating the spatial position of the suspected obstacle point cluster in each frame based on the pose information of the corresponding frames; constructing a theoretical disparity model of a real static obstacle under different viewpoints based on the robot's motion trajectory, and comparing the actual changes in the spatial position of the suspected obstacle point cluster in multiple frames with the theoretical disparity model. The model is compared, and when the deviation of the actual change exceeds the preset geometric consistency threshold, the suspected obstacle point cluster is determined to not meet the multi-view geometric consistency condition. At the same time, the Doppler velocity of the suspected obstacle point cluster in each frame is obtained, and the distribution change of the Doppler velocity between multiple frames is analyzed. When the Doppler velocity distribution is inconsistent with the Doppler response that a real static obstacle should have in the robot's current motion state and the deviation exceeds the preset physical constraint threshold, the suspected obstacle point cluster is determined to not meet the Doppler physical law. When the suspected obstacle point cluster does not meet the multi-view geometric consistency condition or the Doppler physical law, the motion trajectory and Doppler velocity distribution of the point cluster are used to further determine whether the point cluster is dynamic. If it is, it is marked as a dynamic point cluster; otherwise, it is determined to be a multipath false point cluster.

[0018] As a preferred technical solution of the present invention, the acquisition of candidate trajectories includes: acquiring local target points of the robot, combining pose information, and generating multiple initial trajectories in the workspace corresponding to the risk grid map according to preset kinematic and dynamic constraints; projecting the initial trajectories onto the risk grid map, calculating the risk level of each grid passed by the initial trajectory, and determining the corresponding area as a high-risk area when the risk level of the grid passed by the initial trajectory is higher than a preset risk threshold; and performing feasibility verification on the initial trajectories, and selecting the initial trajectory that passes the feasibility verification and has the smallest cumulative risk level value of the grid passed as the candidate trajectory.

[0019] As a preferred technical solution of the present invention, the directional perception enhancement includes: adjusting the perception control parameters of the millimeter-wave radar based on the target perception sector pointing to the high-risk area by the candidate trajectory, performing directional perception enhancement on the target perception sector, and collecting enhanced point clouds during the verification micro-motion process.

[0020] As a preferred embodiment of the present invention, the verification micro-motion includes: determining the micro-motion target based on the relative position of the target perception sector in the robot coordinate system, including the rotation direction and angle range of a small rotation in place, and the displacement direction and displacement amplitude of a small lateral translation; the range of the micro-motion target is limited to within a preset maximum attitude change amplitude and a maximum translation amplitude, and satisfies the constraint that the displacement along the candidate trajectory direction does not exceed a preset displacement threshold; under the premise of satisfying preset linear velocity, angular velocity, and acceleration constraints, as well as the safe distance constraint between the robot and surrounding obstacles, generating a local motion trajectory from the robot's current pose to the micro-motion target, and solving for control commands based on the local motion trajectory; driving the robot to execute the micro-motion according to the control commands, adjusting the perception control parameters in real time according to the micro-motion target and control commands during the execution process, maintaining enhanced directional perception of the target perception sector, monitoring the safe distance between the robot and high-risk areas in real time, and interrupting the micro-motion when a violation of the safe distance constraint is detected.

[0021] As a preferred technical solution of the present invention, the updating of the risk grid map includes: after the verification micro-movement ends, registering the collected augmented point cloud with the robot's current pose information and mapping it to the grid corresponding to the risk grid map; statistically analyzing the augmented point cloud falling into each grid to obtain the current observation result of each grid; based on a preset temporal fusion strategy, fusing the current observation result with the historical observation result and the historical echo statistics result of the preset position in the environmental prior map to update the risk level of each grid; and updating the risk grid map according to the updated risk level of each grid.

[0022] A robot obstacle avoidance system based on millimeter-wave radar sparse point clouds includes:

[0023] Point cloud acquisition module: Acquires sparse point cloud, pose information and velocity information of the robot in the current frame; compensates the sparse point cloud based on the pose information and performs reverse registration to obtain the enhanced point cloud;

[0024] Negative obstacle recognition module: Divide the area in front of the robot into several sectors according to the azimuth angle, perform local ground fitting on the enhanced point cloud of each sector to obtain the ground model of each sector; identify negative obstacle areas by comparing the height continuity of the local ground models of adjacent sectors.

[0025] Weak obstacle identification module: Based on the millimeter-wave radar echo data collected during the current operation and the historical echo statistics of preset locations in the environmental prior map, combined with the current time series accumulation, it identifies weak reflection obstacle areas;

[0026] Point cluster identification module: Detects suspected obstacle point clusters in the enhanced point cloud and determines whether the suspected obstacle point clusters are multipath false point clusters or dynamic point clusters; marks multipath false point clusters as low-confidence obstacles, dynamic point clusters as dynamic obstacles, and other suspected obstacle point clusters as real obstacles;

[0027] Risk Map Module: Maps negative obstacle areas, weak reflection obstacle areas, low confidence obstacles, dynamic obstacles, and real obstacles onto a robot-centered risk grid map, sets different risk weights and safety expansion radii for different types of obstacles, and calculates the risk level of each grid.

[0028] Exploration and verification module: Generates candidate trajectories, controls millimeter-wave radar to enhance directional perception in high-risk areas of the candidate trajectories, and controls the robot to perform verification micro-movements;

[0029] Obstacle avoidance decision module: After confirmatory micro-movement, update the risk grid map and plan the final obstacle avoidance trajectory on the updated risk grid map.

[0030] The present invention has the following advantages:

[0031] This invention divides the robot's front into sectors according to azimuth angles, performs local ground fitting on the enhanced point cloud of each sector, and divides the distance into distance segments. Combining the height continuity criterion of the ground model of adjacent sectors and a preset height threshold, it determines sectors that cannot fit a continuous ground, thus achieving targeted identification of negative obstacle areas such as recessed steps and potholes.

[0032] This invention constructs an environmental prior map based on sparse point cloud and pose information during multiple runs, performs long-term statistics on the number of echo-free and low-intensity echoes of the grid corresponding to the static structure, and combines the low-intensity candidate points near the edge of the candidate region with the temporal accumulation within the adaptive time window during the current run to identify the edge of the weak reflection obstacle and perform safe distance dilation to form a weak reflection obstacle region.

[0033] This invention extracts suspected obstacle clusters from enhanced point clouds based on reflection intensity thresholds and spatial proximity relationships, constructs a theoretical parallax model of real static obstacles under robot motion trajectory constraints, and compares the Doppler velocity distribution of each frame with the Doppler physical laws of static obstacles. It then classifies point clusters that do not meet the multi-view geometric consistency or Doppler physical laws, distinguishing between multipath false point clusters and dynamic point clusters, thereby suppressing multipath false obstacle interference while preserving real obstacles.

[0034] This invention determines the target perception sector based on the high-risk area traversed by the candidate trajectory, adjusts the perception control parameters of the millimeter-wave radar, performs directional perception enhancement on the target perception sector, and executes verification micro-motion based on the relative position of the target perception sector during this period. Under preset attitude change amplitude, translation amplitude, and linear velocity, angular velocity, and acceleration constraints, a local motion trajectory is generated and control commands are output. At the same time, the safe distance from the high-risk area is monitored in real time, and multi-view enhanced point cloud is collected without violating the safe distance constraints, thereby improving the observation quality and obstacle determination reliability of the high-risk area in a local range. Attached Figure Description

[0035] To more clearly illustrate the technical solutions in the embodiments of the present invention or the prior art, the drawings used in the description of the embodiments or the prior art will be briefly introduced below. Obviously, the drawings in the following description are only schematic diagrams of the present invention. For those skilled in the art, other drawings can be obtained based on the provided drawings without creative effort.

[0036] Figure 1 This is a schematic diagram of a robot obstacle avoidance system based on millimeter-wave radar sparse point clouds used in an embodiment of the present invention. Detailed Implementation

[0037] To make the objectives, technical solutions, and advantages of this invention clearer, the invention will be further described in detail below with reference to the accompanying drawings. Obviously, the described embodiments are only some embodiments of this invention, and not all embodiments. Based on the embodiments of this invention, all other embodiments obtained by those skilled in the art without creative effort are within the scope of protection of this invention.

[0038] Example 1: A robot obstacle avoidance method based on millimeter-wave radar sparse point clouds, comprising the following steps:

[0039] Step S1: Obtain the sparse point cloud, pose information, and velocity information of the robot in the current frame; compensate the sparse point cloud based on the pose information and perform reverse registration to obtain the enhanced point cloud;

[0040] In this embodiment, the acquisition of the enhanced point cloud includes: the sparse point cloud is acquired by a millimeter-wave radar installed on the robot, including range, azimuth, reflection intensity, and Doppler velocity, and the pose information is the robot's current position and attitude information; the sparse point cloud is subjected to attitude compensation based on the position and attitude information and transformed to the robot coordinate system, and an adaptive time window is determined based on the velocity information; several frames of historical sparse point cloud within the adaptive time window are reverse-registered to a unified robot coordinate system according to the corresponding pose information, and merged with the current one to obtain the enhanced point cloud.

[0041] The current frame of sparse point cloud is acquired at the moment triggered by the motion control module on the robot itself, and is aligned with the pose and velocity information in terms of timestamps. A time synchronization mechanism ensures that the three types of data correspond to the same observation time. The robot's current pose information is used to correct the point coordinates in the original point cloud, unifying the measurement results affected by robot pitch, roll, yaw, and other pose changes to a standard pose, thereby reducing the impact of sensor installation errors, ground slope, and robot pose changes on the spatial distribution of the point cloud. Using the pose information from historical moments, the historical frame of sparse point cloud is reverse-transformed to the current robot coordinate system. After reverse registration, multiple frames of point cloud are aligned in the same coordinate system, and the spatial density and stability of the environmental structure are improved through time accumulation.

[0042] Sparse point cloud is the set of points output by a millimeter-wave radar within a single scan cycle. The millimeter-wave radar is installed at a predetermined position on the robot body. After transmitting and receiving millimeter-wave signals, it outputs structured point cloud data after preprocessing. For each point in the sparse point cloud, distance characterizes the radial distance between the point and the millimeter-wave radar, azimuth characterizes the horizontal angle of the point relative to the radar's forward direction, reflection intensity characterizes the reflection characteristics of the target corresponding to that point to the millimeter-wave signal, and Doppler velocity characterizes the relative velocity of the target corresponding to that point in the radar's line-of-sight direction. Pose information comes from the robot's onboard localization module, which fuses wheel odometry data, inertial measurement unit data, and environmental perception map alignment results to output the robot's position and attitude in the environmental reference coordinate system. Velocity information comes from the motion control module or odometry data calculation results, characterizing the robot's current linear and angular velocities for determining the adaptive time window.

[0043] The adaptive time window is a dynamically adjusted time accumulation interval based on the robot's current speed information. It constrains the number and time span of historical frames involved in the augmented point cloud construction. The length of the adaptive time window corresponds to the speed information. When the robot speed is low, the adaptive time window covers a relatively long period, thus introducing more historical frames to augment the current environment's sparse point cloud structure. When the robot speed is high, the adaptive time window covers a relatively short period, introducing only a small amount of historical frame data highly consistent with the current environment to avoid accumulating structures that have left the current field of view or have changed into the augmented point cloud.

[0044] By utilizing the robot's pose in the environmental reference coordinate system, the point cloud originally represented in the radar coordinate system is transformed into a robot coordinate system with the robot itself as the reference. During the attitude compensation process, the tilt of the point cloud caused by changes in robot attitude such as pitch and roll is corrected, so that structures such as the ground and walls maintain stable geometric relationships in the robot coordinate system.

[0045] For each frame of point cloud within the time window, the relative pose of the point cloud relative to the current robot coordinate system is calculated using the pose information at the time of acquisition. Based on this, coordinate transformation is performed on the historical frame point clouds to align all historical frame point clouds with the current frame point cloud in the same robot coordinate system. The reverse registration process compresses the robot's motion trajectory within the time window, re-superimposing the environmental structures acquired at different times onto the current reference coordinate system, thereby creating a cumulative enhancement of the point cloud in space.

[0046] After reverse registration, the sparse point clouds of all historical frames within the adaptive time window are merged with the sparse point cloud of the current frame to obtain the enhanced point cloud. The enhanced point cloud introduces information from multiple frames on top of the sparse point cloud of a single frame, resulting in higher spatial point density and a more complete description of the environmental structure.

[0047] The robot coordinate system is a right-handed Cartesian coordinate system established with a predetermined reference point on the robot body as the origin, the forward direction as the x-axis, the leftward direction as the y-axis, and the vertical upward direction as the z-axis. This coordinate system moves synchronously with the robot body and is used to describe the positional relationship of various point clouds and obstacle areas in the environment relative to the robot body.

[0048] Step S2: Divide the area in front of the robot into several sectors according to the azimuth angle, perform local ground fitting on the enhanced point cloud of each sector to obtain the ground model of each sector; identify negative obstacle areas by comparing the height continuity of the local ground models of adjacent sectors.

[0049] In this embodiment, based on the enhanced point cloud obtained in step S1, the space in front of the robot is structurally divided in the robot coordinate system, and the ground morphology in each spatial region is modeled. To provide a detailed description of the ground state in front of the robot, the area is divided into several sectors according to azimuth angles. These sectors are centered on the origin of the robot coordinate system, and in the xy plane, with the robot's forward direction as the reference, the space within a certain angular range in front is divided at preset angular intervals. Each segment is fan-shaped on the plane and is called a sector. Points in the enhanced point cloud that fall into a sector are assigned to that sector based on their azimuth angle and distance relationship. The point set within the corresponding sector is used to fit the local ground model of that sector.

[0050] Local ground fitting is performed on the enhanced point cloud of each sector to obtain the ground model for each sector. Within each sector, points related to the ground structure are selected from the enhanced point cloud, and the height distribution of the ground within that sector is estimated based on spatial relationships. The input data for local ground fitting comes from the coordinates of points in the enhanced point cloud. By selecting points with low height, continuous distribution, and consistent with the height range of the robot's bottom, a point set suitable for ground fitting is constructed. In practical applications, for close-range areas near the robot body, the ground point distribution is relatively dense, and local ground fitting yields a relatively stable height estimate. For more distant areas, the accumulation of the enhanced point cloud over time increases the point density, also providing data support for ground fitting.

[0051] By comparing the height continuity of local ground models in adjacent sectors, negative obstacle areas are identified. After fitting the ground models for each sector, the ground heights of adjacent sectors at the same distance are compared to analyze whether the changes in ground height in the lateral direction conform to a continuous and smooth ground morphology. When there is a significant discontinuity or abrupt change in ground height between adjacent sectors over a short distance, it indicates the presence of ground height drop structures such as depressions, steps, or pits in front of the robot. These structures manifest as missing ground areas in a planar environment, typically posing a significant risk to robot driving safety.

[0052] "Negative obstacle zone" refers to a spatial location where the ground height is significantly reduced relative to the normal ground height, forming a concave area. In a two-dimensional ground projection, this type of area appears as a reduced height or missing ground, posing a risk of the robot sinking into the ground when driving on wheels or tracks.

[0053] The identification of negative obstacle regions includes: dividing the preset distance range in front of the robot into several distance segments along the distance direction; for each distance segment, obtaining the corresponding ground height information based on the ground model of each sector, and determining whether the ground models of each sector within the target distance segment are continuous; for a certain distance segment, when it is detected that a certain sector cannot be fitted with a ground model that meets the height continuity criterion by the augmented point cloud in the corresponding sector within the target distance segment, and the ground height of the target sector in the target distance segment is reduced by more than a preset height threshold relative to the ground height of the adjacent sector in the target distance segment, the spatial region corresponding to the target sector in the target distance segment is identified as a negative obstacle region.

[0054] The preset distance range in front of the robot is jointly determined by the task scenario and the effective detection range of the radar, and is used to define the working area for negative obstacle recognition. For example, in an indoor mobile scenario, the preset distance range can be set to an area of ​​0.5 meters to 5 meters in front, which covers both the area on the robot's short-term planned trajectory and the critical area within the robot's braking distance. This preset distance range is divided into several distance segments along the distance direction, and the continuous forward distance axis is segmented according to a preset distance step size. Each segment corresponds to a fixed forward distance interval, called a distance segment.

[0055] For each distance segment, ground height information is obtained based on the ground model of each sector. For the center distance of the distance segment or the distance interval covered by the segment, the ground height of the corresponding sector at that distance is calculated or estimated using the ground model of that sector. Based on this height information, a "ground height profile" in the lateral direction is represented for the distance segment. When the profile shows a smooth change between adjacent sectors, it indicates that the ground at that distance is relatively flat and continuous; when the profile shows a significant drop in height or no effective fitting result in a certain sector, there is a negative obstacle risk at the corresponding location of that sector.

[0056] The height continuity criterion is a threshold judgment rule for the variation pattern of ground height, used to constrain the variation range of ground height between adjacent sectors and the validity of the ground model fitting results. Specifically, within a target distance segment, for the ground height of adjacent sectors, if the height difference is within a reasonable range and the ground model fitting residual meets the preset requirements, then the ground at that distance is considered to have height continuity in the lateral direction; if there is a significant height drop between the ground height of a sector and its adjacent sectors, or if the sector cannot fit a stable ground model within the target distance segment due to a lack of sufficient ground points, then the ground model of that sector does not meet the height continuity criterion.

[0057] A preset height threshold is used to quantitatively characterize the degree of height drop in negative obstacle regions. When the ground height of a target sector in a target distance segment decreases by more than the preset height threshold relative to the ground height of adjacent sectors in the same target distance segment, it indicates that a significant height depression structure has appeared at the corresponding location of the target sector, exceeding the height difference range caused by ordinary ground undulations. For a certain distance segment, when it is detected that a ground model satisfying the height continuity criterion cannot be fitted from the augmented point cloud within the target distance segment for a sector, and the ground height of the target sector in the target distance segment decreases by more than the preset height threshold relative to the ground height of adjacent sectors in the same target distance segment, the spatial region corresponding to the target sector in the target distance segment is identified as a negative obstacle region.

[0058] Step S3: Based on the millimeter-wave radar echo data collected during the current operation and the historical echo statistics of the preset locations in the environmental prior map, combined with the current time series accumulation, identify the weak reflection obstacle area;

[0059] In this embodiment, obstacles with weak reflectivity in the environment are identified. Weakly reflective obstacle regions refer to areas where the surface material, electromagnetic properties, or geometry result in low millimeter-wave radar echo energy. For example, equipment casings with absorbing coatings, non-metallic structures with high surface roughness, thick curtains, and foam material barriers have significantly lower reflectivity than surrounding walls, columns, and other conventional static structures, but still physically pose a collision risk to the robot's movement. Since the echo intensity of such obstacles is close to background noise in a single-frame point cloud, they appear as sparse, low-intensity points in the enhanced point cloud. If only single-frame or single-moment information is relied upon, weakly reflective obstacle regions are difficult to distinguish from ordinary noise.

[0060] The "environmental prior map" is an environment-level knowledge representation structure built during one or more robot runs in a target environment. It records the spatial distribution of static structures in the environment and the historical statistical results of millimeter-wave radar echoes at corresponding locations. The environmental prior map uses a gridded spatial organization, dividing the target environment into multiple preset locations, each corresponding to a grid or voxel unit in the environment. For each preset location, as the robot passes through it multiple times, it continuously collects millimeter-wave radar echo information at that location, forming "historical echo statistics," which characterizes the average reflection characteristics of that location over a long time scale.

[0061] The identification of "weakly reflective obstacle areas" relies not only on historical data from the prior environmental map but also on real-time observations during the robot's current operation. Millimeter-wave radar echo data collected during the current operation is synchronized temporally with the robot's current posture, velocity, and augmented point cloud. Using the current operating moment as a baseline, each point in the augmented point cloud carries spatial location and reflection intensity information. This information, after being mapped to a preset location on the prior environmental map, is compared with historical echo statistics, thereby fusing long-term statistical features with current temporal features under the same spatial index.

[0062] "Current temporal accumulation" is a statistical accumulation of low-intensity candidate points in several frames of enhanced point cloud during the current operation under the constraint of adaptive time window. By statistically analyzing the number of times a certain spatial location is repeatedly hit by low-intensity points within the time window and the range of position changes, structural edge points that are stable in spatial location and have low reflection intensity over a long period of time are extracted.

[0063] The identification of weak reflection obstacle regions includes: during one or more robot runs in the target environment, constructing or updating a priori environmental map based on sparse point cloud and pose information, and recording historical echo statistics of the static structure corresponding to a preset position in the priori environmental map; statistically analyzing the millimeter-wave radar echoes of the grid corresponding to the static structure at multiple times, and marking the target grid as a candidate region when the number of times a certain grid has no echo or a reflection intensity lower than a first preset threshold during multiple frame observations or multiple runs is greater than a preset proportion; during the current robot run, extracting points from the enhanced point cloud that fall near the edge of the candidate region and have a reflection intensity lower than a second preset threshold as low-intensity candidate points, and accumulating the spatial positions of the low-intensity candidate points temporally within an adaptive time window, and determining the point at the corresponding spatial position as the edge point of the weak reflection obstacle when the number of times a certain spatial position is hit by a low-intensity candidate point within the adaptive time window is not less than a preset number and the range of its spatial position change is less than a preset spatial threshold; expanding the region with the edge point as the center according to a preset safety distance, and marking the expanded region as the weak reflection obstacle region. The area near the edge is the region whose distance from the candidate region boundary does not exceed a preset edge distance threshold.

[0064] As the robot gradually explores the target environment, sparse point clouds collected at different times and their corresponding pose information are used to uniformly map the scene structure observed multiple times onto the environmental coordinate system. When the robot passes the same physical location multiple times along different paths and in different directions, the point cloud data corresponding to that location is repeatedly projected onto the corresponding grid in the environmental prior map. The environmental prior map is built from scratch on the first run, and in subsequent runs, the echo statistics of existing grids are incrementally updated, thus gradually forming a stable statistical layer covering the main static structures of the target environment. For example, when the robot travels back and forth along the warehouse aisle multiple times, the grids corresponding to static structures such as shelves, walls, and columns form stable echo statistics in the environmental prior map, while the moving pallets and pedestrians that occasionally appear in the aisle will not form stable grid features in long-term statistics.

[0065] For each grid or preset location, the presence and reflection intensity distribution of millimeter-wave radar echoes at that location are statistically analyzed across multiple frames of observation or multiple runs. Historical echo statistics include at least the total number of observations, the number of times no echo occurred, and the number of times echoes occurred with reflection intensity below a first preset threshold. The total number of observations measures the frequency with which the grid is covered by the robot sensor; the number of times no echo occurred reflects the degree to which effective echoes were lacking at that location across multiple observations; and the number of echoes below the first preset threshold describes the degree to which the echo energy at that location is low.

[0066] The millimeter-wave radar echoes of the corresponding grids for static structures are statistically analyzed at multiple times. When the proportion of times a grid exhibits no echo or reflection intensity below a first preset threshold across multiple frames or runs exceeds a preset proportion, the target grid is marked as a candidate region. This process quantitatively constrains weak reflection characteristics through a preset proportion. The first preset threshold distinguishes between normal reflection and significantly low reflection; when the echo intensity is below this threshold, the observation is considered a weak reflection observation. The preset proportion limits the percentage of weak reflection or no echo in the total observations; when this proportion exceeds the preset proportion, it indicates that the grid exhibits significant weak reflection characteristics in a long-term statistical sense. Candidate regions refer to spatial areas selected from the prior environmental map that exhibit weak reflection characteristics over a long time scale. These regions are geometrically associated with static structures but differ from ordinary static structures in echo intensity.

[0067] For the enhanced point cloud acquired within the current time window, the spatial location of each point is first determined to indicate whether it is near the edge of a candidate region. Then, the reflection intensity of each point is used to determine whether it belongs to the low-intensity echo category under the current observation. The area near the edge of the candidate region is defined as a region within a preset edge distance threshold from the candidate region boundary. This region spatially covers a band-shaped area around the outer edge of the candidate region and is used to capture echo changes near the geometric contours of weak reflection obstacles. A second preset threshold is used to distinguish between low-intensity echo points and ordinary reflection points in the current operation. Its value differs from the first preset threshold to adapt to the current noise level and dynamic environmental changes. Points that meet both the spatial location and reflection intensity conditions are marked as low-intensity candidate points and used as input data for subsequent time-series accumulation.

[0068] Within an adaptive time window, the spatial locations of low-intensity candidate points are accumulated temporally. When a spatial location is hit by low-intensity candidate points at a number not less than a preset number and its spatial location variation range is less than a preset spatial threshold, the point at that spatial location is identified as an edge point of a weak reflection barrier. The adaptive time window is consistent with the time window used for enhancing point cloud construction in step S1, and the length of the time window is dynamically adjusted according to the robot's current speed information. For each frame of enhanced point cloud within the time window, points falling near the edge of the candidate region and with a reflection intensity lower than a second preset threshold are spatially assigned to the corresponding counting unit, forming a temporal hit statistics for low-intensity candidate points. The preset number of hits is used to constrain a spatial location to be hit by low-intensity points multiple times within the time window, thereby eliminating the influence of accidental noise points; the preset spatial threshold is used to constrain the spatial drift range of the location within the time window, ensuring that the location hit multiple times remains spatially stable. Spatial locations that meet the hit count threshold and the spatial stability threshold can be regarded as real structural edge locations, and their corresponding points are identified as edge points of weak reflection barriers. The edge points of weak reflective barriers are distributed along the outline of the barrier in space. The reflective intensity is low over a long period of time, but it has high stability in both time and space.

[0069] The region is expanded by using the edge point as the center and following a preset safety distance. The expanded region is then marked as the weak reflection obstacle region. Region expansion refers to extending a spatial region, centered on the edge point and within a preset safety distance, onto a three-dimensional space or a two-dimensional plane, to cover the main body of the weak reflection obstacle and its safety buffer zone. The preset safety distance is determined based on the robot's dimensions, positioning error, and motion control accuracy, and is used to reserve a safety margin beyond the actual outer contour of the obstacle.

[0070] The "near the edge" refers to the region whose distance from the candidate region boundary does not exceed a preset edge distance threshold. This preset edge distance threshold controls the spatial width of the "near the edge," and is related to the angular resolution, ranging accuracy, and typical size of obstacles in the environment of the millimeter-wave radar. When the edge distance threshold is small, low-intensity candidate points are mainly concentrated in a narrow band near the candidate region boundary, and the positioning of edge points is closer to the actual obstacle outline. When the edge distance threshold is slightly larger, the coverage area of ​​the near-edge region increases, further accommodating edge position offsets caused by attitude changes and registration errors.

[0071] Step S4: Detect suspected obstacle point clusters in the enhanced point cloud and determine whether the suspected obstacle point clusters are multipath false point clusters or dynamic point clusters; mark multipath false point clusters as low-confidence obstacles, dynamic point clusters as dynamic obstacles, and other suspected obstacle point clusters as real obstacles;

[0072] In this embodiment, high-intensity echo points in the enhanced point cloud are spatially clustered to obtain suspected obstacle point clusters that are spatially adjacent and appear as a group of connected reflection points from the radar perspective. These suspected obstacle point clusters represent a set of point clouds that, under the current observation conditions, spatially constitute several "object candidates." These clusters include point clouds corresponding to real static obstacles, false point clouds caused by multipath effects, and dynamic point clouds generated by pedestrians, mobile devices, etc.

[0073] "Suspected obstacle clusters" refer to a set of points in an augmented point cloud that meets preset spatial proximity constraints and point count threshold constraints, obtained through spatial clustering algorithms. Each suspected obstacle cluster occupies a certain volume in space and has a definite position, size, and shape in the robot coordinate system.

[0074] "Multipath spurious point clusters" refer to point cloud aggregations formed by millimeter-wave signals undergoing multiple reflections and refractions in the environment, resulting in locations that do not correspond to real spatial positions. Multipath effects typically occur in environments such as metal surfaces, glass curtain walls, and narrow passages. Radar signals, after multiple reflections from walls, floors, or other structures, return to the radar receiver. Geometrically, the corresponding reflection paths no longer coincide with the actual obstacle locations, thus generating spurious point clusters in the augmented point cloud that do not overlap with real obstacles. These point clusters do not conform to the geometric and physical constraints of real static obstacles in terms of spatial position and Doppler response. If directly treated as obstacles, they will interfere with robot path planning.

[0075] "Dynamic point clusters" refer to point cloud aggregations generated by targets moving relative to the robot (such as pedestrians, mobile vehicles, and the end effector of rotating robotic arms). The spatial position of dynamic point clusters changes over time during multi-frame observations, and their Doppler velocity distribution differs significantly from the Doppler pattern of the static environment generated by the robot's own motion. Dynamic point clusters require different treatment during obstacle avoidance, and different safety strategies are typically employed in risk assessment and trajectory planning compared to static obstacles.

[0076] "Low-confidence obstacles" are the labeling results of multipath spurious point clusters, used to separately annotate these spurious obstacles in the risk raster map. Low-confidence obstacles deviate from real obstacles in geometric location, lack stable spatial consistency in multi-view observations, and do not conform to the physical laws of static objects in Doppler response. Labeling multipath spurious point clusters as low-confidence obstacles allows for the assignment of lower risk weights to these clusters in subsequent risk assessments, preventing them from excessively influencing the final obstacle avoidance trajectory, while maintaining a certain degree of conservatism for uncertain areas.

[0077] The determination of multipath false point clusters or dynamic point clusters includes: in the enhanced point cloud, selecting points with reflection intensity higher than the noise threshold as candidate obstacle points; clustering based on the spatial distance and point count threshold between candidate obstacle points; and selecting clustering results that meet a preset spatial proximity threshold and have a point count not lower than a preset minimum point count threshold as suspected obstacle point clusters; for each suspected obstacle point cluster, acquiring multi-frame enhanced point clouds covering the suspected obstacle point cluster during robot movement, and calculating the spatial position of the suspected obstacle point cluster in each frame based on the pose information of the corresponding frames; constructing a theoretical disparity model of a real static obstacle under different viewpoints based on the robot's motion trajectory, and comparing the actual changes in the spatial position of the suspected obstacle point cluster in multiple frames with the theoretical disparity model. When the deviation of the actual change exceeds the preset geometric consistency threshold, the suspected obstacle point cluster is determined to not meet the multi-view geometric consistency condition. At the same time, the Doppler velocity of the suspected obstacle point cluster in each frame is acquired, and the distribution change of the Doppler velocity between multiple frames is analyzed. When the Doppler velocity distribution is inconsistent with the Doppler response that a real static obstacle should have in the robot's current motion state and the deviation exceeds the preset physical constraint threshold, the suspected obstacle point cluster is determined to not meet the Doppler physical law. When the suspected obstacle point cluster does not meet the multi-view geometric consistency condition or the Doppler physical law, the motion trajectory and Doppler velocity distribution of the point cluster are used to further determine whether the point cluster is dynamic. If so, it is marked as a dynamic point cluster; otherwise, it is determined to be a multipath false point cluster.

[0078] Among all points in the enhanced point cloud, points with reflection intensity significantly higher than the background noise level are selected as reliable observations of objects on the surface, based on the reflection intensity data field. The noise threshold is derived from the noise characteristics of the millimeter-wave radar and environmental noise statistics, and is either calibrated offline through a static calibration scenario or dynamically estimated during operation by combining observation data over a period of time.

[0079] A clustering algorithm based on spatial proximity is employed to group candidate obstacle points that are close to each other in the robot coordinate system into the same category. Spatial distance is calculated based on the 3D coordinates of points in the enhanced point cloud. A preset spatial proximity threshold is used to limit the maximum proximity distance between two points within the same cluster. When the distance between two points does not exceed this threshold, the two points belong to the same connected region during the clustering process. A point count threshold is used to constrain the minimum number of points contained in each cluster result. The preset minimum point count threshold is jointly determined by the environmental noise level, radar resolution, and target size, and is used to eliminate pseudo-clusters consisting of only a small number of noise points.

[0080] For each suspected obstacle cluster, multiple frames of enhanced point cloud covering the cluster are acquired during the robot's movement, and the spatial position of the suspected obstacle cluster in each frame is calculated based on the pose information of the corresponding frames. Specifically, when the robot moves along a certain trajectory, the same object is observed by radar from different azimuth angles and multiple distance positions in the enhanced point clouds of different frames. By utilizing the pose information of each frame, the position of the object's point cluster in each frame is uniformly mapped to the robot coordinate system or the environment coordinate system, thereby obtaining the actual spatial trajectory of the point cluster as time changes.

[0081] The "theoretical parallax model" is a geometric model based on the robot's own motion and the fixed position of static obstacles, used to describe the geometric projection relationship of static obstacles in multi-frame observations. For example, when the robot moves in a straight line, the observed position of a static pillar located to the right front of the robot in the multi-frame augmented point cloud should smoothly move along a certain curve in the robot's coordinate system; when the robot circles an obstacle, the azimuth angle of the obstacle changes regularly at different observation times. The theoretical parallax model originates from the geometric relationship between the robot's trajectory and the relative position of the obstacle, and uses this model to predict the expected trajectory of static obstacles in multiple viewpoints.

[0082] The actual spatial changes of suspected obstacle clusters across multiple frames are compared with the theoretical disparity model. When the deviation of the actual changes exceeds a preset geometric consistency threshold, the suspected obstacle cluster is determined to not meet the multi-view geometric consistency condition. Actual changes refer to the time series of the center position or representative point position of the suspected obstacle cluster obtained from pose registration in multi-frame observations. The theoretical disparity model provides a reference trajectory for the expected positional changes of static obstacles. By comparing the deviation between the actual positional changes and the theoretical reference trajectory, and using the preset geometric consistency threshold as the judgment criterion, clusters whose spatial trajectories do not match the geometric characteristics of static obstacles are identified.

[0083] For each cluster of suspected obstacles, physical constraint analysis is performed using Doppler velocity information output by millimeter-wave radar. Doppler velocity reflects the relative radial velocity between the target and the radar along the radar's line of sight. For static obstacles, in scenarios caused solely by the robot's own motion, the Doppler response exhibits a distinct pattern, meaning the Doppler velocity is jointly determined by the robot's velocity and the obstacle's geometric position.

[0084] When the Doppler velocity distribution is inconsistent with the Doppler response that a real static obstacle should have under the robot's current motion state, and the deviation exceeds a preset physical constraint threshold, the suspected obstacle cluster is determined to not satisfy the Doppler physical laws. The preset physical constraint threshold is determined based on the velocity measurement accuracy of the millimeter-wave radar and the robot's motion state, and is used to accommodate measurement errors and environmental disturbances within a certain range. In multipath scenarios, due to the superposition of reflection paths and phases, multipath spurious clusters exhibit Doppler distribution characteristics that are significantly different from those of real static obstacles, and are therefore identified as not satisfying the Doppler physical laws in the physical constraint determination.

[0085] When a suspected obstacle cluster does not meet the multi-view geometric consistency condition or the Doppler physical law, the cluster's motion trajectory and Doppler velocity distribution are used to further determine whether it is dynamic. If so, it is marked as a dynamic cluster; otherwise, it is identified as a multipath false cluster. Specifically, for clusters that simultaneously exhibit large spatial displacement and significant Doppler responses consistent with the robot's motion state in multiple frames of observation, it is inferred that they are caused by targets with independent motion relative to the robot, i.e., dynamic clusters. For clusters that do not meet the multi-view geometric consistency condition and whose Doppler responses do not match any reasonable dynamic motion mode, they are more consistent with the characteristics of multipath false clusters. In this case, the cluster is identified as a multipath false cluster and participates in risk assessment as a low-confidence obstacle in subsequent steps.

[0086] Step S5: Map the negative obstacle area, weak reflection obstacle area, low confidence obstacle, dynamic obstacle and real obstacle to the risk grid map centered on the robot, set different risk weights and safety expansion radii for different types of obstacles, and calculate the risk level of each grid.

[0087] In this embodiment, a unified spatial representation and risk quantification process are performed based on negative obstacle regions, weak reflection obstacle regions, low confidence obstacles, dynamic obstacles, and real obstacles, and a risk grid map is constructed within a local spatial range centered on the robot.

[0088] A "risk grid map" is a two-dimensional or three-dimensional gridded map that discretly divides the robot's workspace around it, using the robot's current position as a reference. This map divides the workspace into several regular grids on a plane with a fixed grid resolution, each grid corresponding to a spatial region. The grid map's coordinate system uses a planar coordinate system that is consistent with the robot's coordinate system or has a definite transformation relationship, allowing augmented point clouds and various obstacle regions to be directly mapped into the grid coordinates in space.

[0089] Based on the spatial location of the aforementioned obstacles in the robot coordinate system or environment coordinate system, their corresponding spatial regions are projected onto the grid index of the risk grid map. For negative obstacle regions and weakly reflective obstacle regions, the mapping process uses their spatial contours or boundaries as the basis, marking all grids within the region as the corresponding category; for low-confidence obstacles, dynamic obstacles, and real obstacles represented by point clusters, the grids falling within the bounding box or expansion region of the point cluster are marked as the corresponding category based on the spatial extent of the point cluster.

[0090] Based on the differences in safety risks among negative obstacles, weakly reflective obstacles, low-confidence obstacles, dynamic obstacles, and real obstacles, risk level parameters are pre-configured for each category. Risk weights reflect the degree of impact of this type of obstacle on robot operation safety; higher weight values ​​indicate a greater risk of collision or trapping. For example, negative obstacle areas pose a risk of robot falls or getting stuck, so their risk weights are configured to be higher. Weakly reflective obstacle areas have uncertainties in perceived echoes but still physically form obstacles, so their risk weights are close to those of real obstacles. Low-confidence obstacles correspond to multipath false reflections, and their geometric locations may not necessarily have physical entities; their risk weights are set lower than those of real obstacles and weakly reflective obstacles to avoid overly conservative influences on trajectory planning. Dynamic obstacles exhibit positional changes over time, and their risk weights need to reflect the need for dynamic avoidance in local trajectory planning.

[0091] The safety expansion radius is used to reserve a safe buffer distance beyond the actual physical contour of the obstacle, extending the obstacle's influence range to a spatial range that matches the robot's size, control accuracy, and positioning error. Different obstacle categories have different safety expansion radius configurations. For example, a larger expansion radius is used for negative obstacle regions on the horizontal plane to ensure sufficient lateral safety margin for the robot when approaching potholes or step edges; for low-confidence obstacles, a relatively smaller expansion radius is used to reduce excessive avoidance caused by multipath spurious point clusters while maintaining a certain degree of conservatism. Through safety expansion processing, the obstacle itself and the surrounding grid within a certain range are marked as high-risk areas, ensuring that trajectory planning maintains a sufficient safe distance from the obstacle in space.

[0092] Calculating the risk level of each grid cell involves, after obstacle mapping and safety expansion, taking into account the obstacle categories, risk weights, and spatial relationships between the grid cell and the obstacle center in the risk grid map, and then calculating a quantitative risk index for trajectory planning. Specifically, for a grid cell falling within the expansion range of multiple obstacle types, the risk weights of each obstacle type are combined according to preset rules. Based on the weighting method and the distance from the grid center to the obstacle edge or center, the risk value is attenuated to obtain the initial risk value for that grid cell.

[0093] Step S6: Generate candidate trajectories. For high-risk areas of the candidate trajectories, control the millimeter-wave radar to enhance directional perception and control the robot to perform verification micro-movements.

[0094] In this embodiment, based on the risk grid map, several initial trajectories satisfying kinematic and dynamic constraints are generated under the robot's current motion state and local target constraints. Candidate trajectories suitable as motion references are selected from these trajectories. Simultaneously, for high-risk areas traversed by the candidate trajectories, directional perception enhancement is performed by controlling the millimeter-wave radar, and the robot performs verification micro-movements within a defined range. This further improves the reliability of obstacle perception within high-risk areas, thereby providing more accurate environmental information when updating the risk grid map and planning the final obstacle avoidance trajectory.

[0095] A "candidate trajectory" refers to a motion path selected from a set of feasible trajectories within the robot's current local workspace, based on the local target point, robot motion constraints, and environmental risk distribution, for further verification and optimization. The candidate trajectory's geometry matches the kinematic structure of the robot's chassis. For example, for a differential drive robot, the trajectory can be represented as a series of smooth curves satisfying the maximum curvature constraint; for an omnidirectional chassis, the trajectory can be represented as a combination of piecewise straight lines or smooth curves.

[0096] "High-risk areas" refer to regions in the risk grid map where the grid risk level exceeds a preset risk threshold due to the presence of negative obstacles, weakly reflective obstacles, real obstacles, dynamic obstacles, or low-confidence obstacles. These areas significantly impact safety when the robot executes candidate trajectories and require additional perception verification and risk confirmation.

[0097] "Directional sensing enhancement" refers to concentrating the sensing resources of millimeter-wave radar in sensing sectors corresponding to high-risk areas that intersect or are adjacent to candidate trajectories. By adjusting the radar's sensing control parameters, the radar can achieve higher observation frequencies, more concentrated beam energy, or higher angular resolution in these directions, thereby improving the reliability of obstacle recognition in high-risk areas.

[0098] "Verification micro-motion" refers to performing limited pose adjustments near the candidate trajectory without significantly altering the robot's global motion plan. This involves changing the observation perspective through minute rotations or small lateral translations, and collecting enhanced point clouds from multiple viewpoints to verify the authenticity, boundary positions, and risk levels of obstacles in high-risk areas.

[0099] The acquisition of candidate trajectories includes: acquiring local target points of the robot, combining pose information, and generating multiple initial trajectories within the workspace corresponding to the risk grid map according to preset kinematic and dynamic constraints; projecting the initial trajectories onto the risk grid map, calculating the risk level of each grid passed by the initial trajectory, and determining the corresponding area as a high-risk area when the risk level of the grid passed by the initial trajectory is higher than a preset risk threshold; and performing feasibility verification on the initial trajectories, selecting the initial trajectory that passes the feasibility verification and has the smallest cumulative risk level value of the grid passed as the candidate trajectory.

[0100] During candidate trajectory acquisition, local target points originate from the decomposition results of the current stage navigation target by the upper-level planning or task management module, indicating the robot's expected arrival position or orientation within the local workspace. Combining the robot's current pose information, within the workspace covered by the risk grid map, multiple initial trajectories are generated based on the robot's own kinematic constraints (maximum steering angle, minimum turning radius, and non-lateral movement constraints) and dynamic constraints (maximum linear acceleration, maximum angular acceleration, and maximum braking deceleration). These initial trajectories spatially cover multiple possible routes leading to the local target points and conform to the robot's structure and control capabilities in terms of curvature changes and velocity distribution.

[0101] Each initial trajectory is discretely sampled with a certain spatial step size, and the spatial location corresponding to the sampled point is mapped to the grid coordinates of the risk grid map, thereby obtaining the grid sequence traversed by the trajectory during its movement. For each trajectory, the risk level of each grid traversed by the trajectory is calculated, and the existence of high-risk areas on the trajectory is determined based on these risk levels. When the risk level of some grids traversed by the trajectory exceeds a preset risk threshold, the area containing the corresponding grid is marked as a high-risk area for that trajectory.

[0102] Based on the risk assessment, the trajectory is examined from the perspectives of kinematics, dynamics, and environmental constraints to determine whether it meets the safety and feasibility requirements. Feasibility verification includes, but is not limited to: whether the curvature and acceleration of the trajectory are within the robot's allowable range, whether the trajectory meets the minimum safe distance constraint between the robot and obstacles, and whether the trajectory endpoint meets the position and posture requirements of the local target point.

[0103] From the initial set of trajectories that meet the feasibility verification criteria, candidate trajectories are selected based on the cumulative risk level of the grids traversed by the trajectory. The cumulative risk level is a quantitative result of the overall risk along the trajectory. One way to achieve this is to weightedly sum the risk levels of each grid traversed by the trajectory to obtain a comprehensive risk index for the entire travel path.

[0104] The directional perception enhancement includes: adjusting the perception control parameters of the millimeter-wave radar based on the target perception sector pointing to the high-risk area by the candidate trajectory, performing directional perception enhancement on the target perception sector, and collecting enhanced point clouds during the verification micro-motion process.

[0105] During the direction-oriented perception enhancement process, based on the relationship between candidate trajectories and risk grid maps, high-risk areas traversed or adjacent to the candidate trajectories are identified, and "target perception sectors" corresponding to these high-risk areas are calculated in the robot coordinate system. A target perception sector refers to a set of azimuth angles or azimuth angle intervals within the field of view of the millimeter-wave radar that correspond to the spatial direction of the high-risk area.

[0106] Based on the spatial distribution of target sensing sectors, the operating mode of the millimeter-wave radar is configured to allocate more observation resources within these sectors. Sensing control parameters include combinations of technical parameters such as beam pointing, beamwidth, scan repetition frequency, transmit power allocation, and sampling bandwidth allocation. For example, when a sector contains negative obstacle areas or weak reflection obstacle areas, the scan frequency and effective observation time within that sector are increased to enhance echo sampling density; when a sector contains low-confidence obstacles, the beam shape is adjusted to improve the ability to distinguish between multipath and direct paths. By adjusting the sensing control parameters, more detailed point cloud information is obtained in the direction of high-risk areas during verification micro-motion processes.

[0107] During the robot's micro-pose adjustment, the attitude compensation and adaptive time window accumulation mechanism from step S1 are continuously used to perform reverse registration and fusion of multi-frame point clouds within the target perception sector. New observation perspectives and radar echoes from new time periods are introduced on top of the existing enhanced point clouds, further increasing the point cloud density in high-risk areas.

[0108] The verification micro-motion includes: determining the micro-motion target based on the relative position of the target perception sector in the robot coordinate system, including the rotation direction and angle range of a small rotation in place, and the displacement direction and amplitude of a small lateral translation; the range of the micro-motion target is limited to within the preset maximum attitude change amplitude and maximum translation amplitude, and satisfies the constraint that the displacement along the candidate trajectory direction does not exceed a preset displacement threshold; under the premise of satisfying preset linear velocity, angular velocity and acceleration constraints and the safe distance constraint between the robot and surrounding obstacles, generating a local motion trajectory from the robot's current pose to the micro-motion target, and solving the control command based on the local motion trajectory; driving the robot to execute the micro-motion according to the control command, adjusting the perception control parameters in real time according to the micro-motion target and control command during the execution, maintaining enhanced directional perception of the target perception sector, monitoring the safe distance between the robot and high-risk areas in real time, and interrupting the micro-motion when a violation of the safe distance constraint is detected.

[0109] During the aforementioned verification micro-motion process, the form and range of the micro-motion target are determined based on the relative position of the target perception sector in the robot coordinate system. When the target perception sector is mainly distributed on one side in front of the robot, the micro-motion target includes a small in-situ rotation around the vertical axis to change the observation angle of the high-risk area; when the target perception sector is mainly located on the side of the robot and is related to the ground slope, the micro-motion target includes a small lateral translation to change the lateral baseline of the robot relative to the high-risk area. The micro-motion target also includes the rotation direction and angle range, and the translation direction and displacement amplitude, used to describe the pose adjustment space allowed for the robot during the verification phase.

[0110] The range of the micro-motion target is limited to the preset maximum attitude change amplitude and maximum translation amplitude, and satisfies the constraint that the displacement along the candidate trajectory direction does not exceed a preset displacement threshold. The maximum attitude change amplitude and maximum translation amplitude are preset based on the robot chassis size, sensor mounting height, and environmental constraints, and are used to limit the upper limit of the robot's position change during micro-motion. The displacement constraint along the candidate trajectory direction is used to ensure that the micro-motion process does not cause the robot to advance or lag too much in the trajectory direction, avoiding the impact of micro-motion on global time planning.

[0111] A local trajectory planning method is employed to generate a short-time trajectory that is both dynamically feasible and safe for the micro-motion process, while satisfying preset constraints on linear velocity, angular velocity, and acceleration, as well as safe distance constraints between the robot and surrounding obstacles. This local trajectory covers the entire process of the verification micro-motion in time and enables the robot to smoothly transition from its current pose to the target pose in space. Based on the local trajectory, the corresponding control command sequences, such as velocity and steering commands, can be further solved, providing direct input for robot motion control.

[0112] During micro-movements, the robot is driven to perform these movements according to control commands. Throughout the process, the sensing and control parameters are adjusted in real time based on the micro-movement target and control commands, while maintaining enhanced directional perception of the target sensing sector. This ensures that the millimeter-wave radar continuously focuses on high-risk areas throughout the entire micro-movement range. Simultaneously, the safe distance between the robot and high-risk areas is monitored in real time. Based on the obstacle expansion range in the grid map and the robot's trajectory position, the minimum distance between the robot and obstacles is assessed online. If continued micro-movement is detected as violating the safe distance constraint, the micro-movement is immediately interrupted to ensure that the verification action does not introduce new collision risks.

[0113] Step S7: Update the risk grid map after the verification micro-movement, and plan the final obstacle avoidance trajectory on the updated risk grid map.

[0114] In this embodiment, based on the enhanced point cloud obtained from directional perception enhancement and confirmatory micro-motion acquisition, the risk level of each grid in the risk grid map is updated, thereby correcting the existence state, boundary position, and risk weight of obstacles in high-risk areas. Finally, the obstacle avoidance trajectory is planned on the updated risk grid map. The updated risk grid map comprehensively reflects the initial risk assessment results and confirmatory observation results, achieving a balance between safety and passability in the final obstacle avoidance trajectory.

[0115] The process of updating the risk grid map includes: after the verification micro-movement ends, registering the collected augmented point cloud with the robot's current pose information and mapping it to the corresponding grid in the risk grid map; statistically analyzing the augmented point cloud falling into each grid to obtain the current observation result of each grid; based on a preset temporal fusion strategy, fusing the current observation result with historical observation results and historical echo statistics of preset locations in the prior environmental map to update the risk level of each grid; and updating the risk grid map according to the updated risk level of each grid.

[0116] Upon completion of the micro-motion, the robot's current pose information is used to uniformly transform the multi-frame augmented point cloud collected during the micro-motion process into the coordinate system of the risk grid map. Each point in the augmented point cloud, after registration, has a coordinate system corresponding to the risk grid map. Figure 1 This allows for consistent coordinate representation, thus placing it within a unified reference framework with existing obstacle information in the raster map and static structural information in the prior environmental map.

[0117] The grid index to which each point falls is determined based on its spatial coordinates. All points within the same grid are aggregated to form the statistical input for the current observation results of that grid. The current observation results of the grid include the number of point clouds within that grid, the height distribution of the point clouds, the reflection intensity distribution, and the temporal concentration of the point clouds.

[0118] The results of multiple observations are considered comprehensively on the timeline to balance the impact between instantaneous observations and long-term statistics. Historical observation results are derived from the cumulative records of the risk grid map during the current operation, while historical echo statistics in the environmental prior map reflect the long-term reflection characteristics of static structures in the environment. The time-series fusion strategy uses weighted averaging, confidence updates, or recursive updates based on the number of observations to correct the risk level of each grid.

[0119] The new risk levels are written into the raster data structure of the risk raster map, thus forming a risk field distribution that reflects the latest validation results. At this point, the risk raster map simultaneously contains the spatial distribution and risk intensity of negative obstacle areas, weakly reflective obstacle areas, low-confidence obstacles, dynamic obstacles, and real obstacles after validation, providing a decision-making basis for the final obstacle avoidance trajectory planning.

[0120] The updated grid risk level is used as a cost field or constraint to find a safe and low-cost path from the robot's current pose to a local target point within the local workspace. During the planning process, grids with risk levels exceeding the preset upper limit are avoided, and high-risk grids are treated as impassable or high-cost areas; within areas with lower risk levels, paths with lower costs are selected.

[0121] Example 2: A robot obstacle avoidance system based on millimeter-wave radar sparse point clouds, see [link / reference]. Figure 1 As shown, it includes the following modules:

[0122] Point cloud acquisition module: Acquires sparse point cloud, pose information and velocity information of the robot in the current frame; compensates the sparse point cloud based on the pose information and performs reverse registration to obtain the enhanced point cloud;

[0123] Negative obstacle recognition module: Divide the area in front of the robot into several sectors according to the azimuth angle, perform local ground fitting on the enhanced point cloud of each sector to obtain the ground model of each sector; identify negative obstacle areas by comparing the height continuity of the local ground models of adjacent sectors.

[0124] Weak obstacle identification module: Based on the millimeter-wave radar echo data collected during the current operation and the historical echo statistics of preset locations in the environmental prior map, combined with the current time series accumulation, it identifies weak reflection obstacle areas;

[0125] Point cluster identification module: Detects suspected obstacle point clusters in the enhanced point cloud and determines whether the suspected obstacle point clusters are multipath false point clusters or dynamic point clusters; marks multipath false point clusters as low-confidence obstacles, dynamic point clusters as dynamic obstacles, and other suspected obstacle point clusters as real obstacles;

[0126] Risk Map Module: Maps negative obstacle areas, weak reflection obstacle areas, low confidence obstacles, dynamic obstacles, and real obstacles onto a robot-centered risk grid map, sets different risk weights and safety expansion radii for different types of obstacles, and calculates the risk level of each grid.

[0127] Exploration and verification module: Generates candidate trajectories, controls millimeter-wave radar to enhance directional perception in high-risk areas of the candidate trajectories, and controls the robot to perform verification micro-movements;

[0128] Obstacle avoidance decision module: After confirmatory micro-movement, update the risk grid map and plan the final obstacle avoidance trajectory on the updated risk grid map.

[0129] The specific embodiments described above further illustrate the purpose, technical solution, and beneficial effects of the present invention. It should be understood that the above description is only a specific embodiment of the present invention and is not intended to limit the scope of protection of the present invention. Any modifications, equivalent substitutions, improvements, etc., made within the spirit and principles of the present invention should be included within the scope of protection of the present invention.

Claims

1. A robot obstacle avoidance method based on millimeter-wave radar sparse point clouds, characterized in that, include: Obtain the sparse point cloud, pose information, and velocity information of the robot in the current frame; The sparse point cloud is compensated based on pose information and then reverse registered to obtain an enhanced point cloud. The robot's front area is divided into several sectors according to its azimuth angle. Local ground fitting is performed on the enhanced point cloud of each sector to obtain the ground model of each sector. By comparing the height continuity of the local ground models of adjacent sectors, negative obstacle areas are identified. Based on the millimeter-wave radar echo data collected during the current operation and the historical echo statistics of preset locations in the environmental prior map, combined with the current time series accumulation, weak reflection obstacle areas are identified. In the augmented point cloud, detect suspected obstacle point clusters and determine whether they are multipath false point clusters or dynamic point clusters; mark multipath false point clusters as low-confidence obstacles, dynamic point clusters as dynamic obstacles, and other suspected obstacle point clusters as real obstacles; The determination of multipath false point clusters or dynamic point clusters includes: in the enhanced point cloud, selecting points with reflection intensity higher than the noise threshold as candidate obstacle points; clustering based on the spatial distance and point count threshold between candidate obstacle points; and selecting clustering results that meet a preset spatial proximity threshold and have a point count not lower than a preset minimum point count threshold as suspected obstacle point clusters; for each suspected obstacle point cluster, acquiring multi-frame enhanced point clouds covering the suspected obstacle point cluster during robot movement, and calculating the spatial position of the suspected obstacle point cluster in each frame based on the pose information of the corresponding frames; constructing a theoretical disparity model of a real static obstacle under different viewpoints based on the robot's motion trajectory, and comparing the actual changes in the spatial position of the suspected obstacle point cluster in multiple frames with the theoretical disparity model. When the deviation of the actual change exceeds the preset geometric consistency threshold, the suspected obstacle point cluster is determined to not meet the multi-view geometric consistency condition. At the same time, the Doppler velocity of the suspected obstacle point cluster in each frame is acquired, and the distribution change of the Doppler velocity between multiple frames is analyzed. When the Doppler velocity distribution is inconsistent with the Doppler response that a real static obstacle should have in the robot's current motion state and the deviation exceeds the preset physical constraint threshold, the suspected obstacle point cluster is determined to not meet the Doppler physical law. When the suspected obstacle point cluster does not meet the multi-view geometric consistency condition or the Doppler physical law, the motion trajectory and Doppler velocity distribution of the point cluster are used to further determine whether the point cluster is dynamic. If it is, it is marked as a dynamic point cluster; otherwise, it is determined to be a multipath false point cluster. Negative obstacle areas, weak reflection obstacle areas, low confidence obstacles, dynamic obstacles, and real obstacles are mapped onto a robot-centered risk grid map. Different risk weights and safety expansion radii are set for different types of obstacles, and the risk level of each grid is calculated. Candidate trajectories are generated. For high-risk areas of the candidate trajectories, millimeter-wave radar is used to enhance directional perception, and the robot is controlled to perform verification micro-movements. After confirmatory micro-movements, update the risk grid map and plan the final obstacle avoidance trajectory on the updated risk grid map.

2. The robot obstacle avoidance method based on millimeter-wave radar sparse point clouds according to claim 1, characterized in that, The acquisition of the enhanced point cloud includes: acquiring the sparse point cloud through a millimeter-wave radar installed on the robot, including range, azimuth, reflection intensity, and Doppler velocity, with pose information being the robot's current position and attitude information; performing attitude compensation on the sparse point cloud based on the position and attitude information and transforming it to the robot coordinate system, and determining an adaptive time window based on the velocity information; back-registering several frames of historical sparse point cloud within the adaptive time window to a unified robot coordinate system based on the corresponding pose information, and merging it with the current one to obtain the enhanced point cloud.

3. The robot obstacle avoidance method based on millimeter-wave radar sparse point clouds according to claim 1, characterized in that, The identification of negative obstacle regions includes: dividing the preset distance range in front of the robot into several distance segments along the distance direction; for each distance segment, obtaining the corresponding ground height information based on the ground model of each sector, and determining whether the ground models of each sector within the target distance segment are continuous; for a certain distance segment, when it is detected that a certain sector cannot be fitted with a ground model that meets the height continuity criterion by the augmented point cloud in the corresponding sector within the target distance segment, and the ground height of the target sector in the target distance segment is reduced by more than a preset height threshold relative to the ground height of the adjacent sector in the target distance segment, the spatial region corresponding to the target sector in the target distance segment is identified as a negative obstacle region.

4. The robot obstacle avoidance method based on millimeter-wave radar sparse point clouds according to claim 2, characterized in that, The identification of weak reflection obstacle regions includes: during one or more robot runs in the target environment, constructing or updating a priori environmental map based on sparse point cloud and pose information, and recording historical echo statistics of the static structure corresponding to a preset position in the priori environmental map; statistically analyzing the millimeter-wave radar echoes of the grid corresponding to the static structure at multiple times, and marking the target grid as a candidate region when the number of times a certain grid has no echo or a reflection intensity lower than a first preset threshold during multiple frame observations or multiple runs is greater than a preset proportion; during the current robot run, extracting points from the enhanced point cloud that fall near the edge of the candidate region and have a reflection intensity lower than a second preset threshold as low-intensity candidate points, and accumulating the spatial positions of the low-intensity candidate points temporally within an adaptive time window, and determining the point at the corresponding spatial position as the edge point of the weak reflection obstacle when the number of times a certain spatial position is hit by a low-intensity candidate point within the adaptive time window is not less than a preset number and the range of its spatial position change is less than a preset spatial threshold; expanding the region with the edge point as the center according to a preset safety distance, and marking the expanded region as the weak reflection obstacle region.

5. The robot obstacle avoidance method based on millimeter-wave radar sparse point clouds according to claim 1, characterized in that, The acquisition of candidate trajectories includes: acquiring local target points of the robot, combining pose information, and generating multiple initial trajectories within the workspace corresponding to the risk grid map according to preset kinematic and dynamic constraints; projecting the initial trajectories onto the risk grid map, calculating the risk level of each grid passed by the initial trajectory, and determining the corresponding area as a high-risk area when the risk level of the grid passed by the initial trajectory is higher than a preset risk threshold; and performing feasibility verification on the initial trajectories, selecting the initial trajectory that passes the feasibility verification and has the smallest cumulative risk level value of the grid passed as the candidate trajectory.

6. The robot obstacle avoidance method based on millimeter-wave radar sparse point clouds according to claim 1, characterized in that, The directional perception enhancement includes: adjusting the perception control parameters of the millimeter-wave radar based on the target perception sector pointing to the high-risk area by the candidate trajectory, performing directional perception enhancement on the target perception sector, and collecting enhanced point clouds during the verification micro-motion process.

7. The robot obstacle avoidance method based on millimeter-wave radar sparse point clouds according to claim 6, characterized in that, The verification micro-motion includes: determining the micro-motion target based on the relative position of the target perception sector in the robot coordinate system, including the rotation direction and angle range of a small rotation in place, and the displacement direction and amplitude of a small lateral translation; the range of the micro-motion target is limited to within the preset maximum attitude change amplitude and maximum translation amplitude, and satisfies the constraint that the displacement along the candidate trajectory direction does not exceed a preset displacement threshold; under the premise of satisfying preset linear velocity, angular velocity and acceleration constraints and the safe distance constraint between the robot and surrounding obstacles, generating a local motion trajectory from the robot's current pose to the micro-motion target, and solving the control command based on the local motion trajectory; driving the robot to execute the micro-motion according to the control command, adjusting the perception control parameters in real time according to the micro-motion target and control command during the execution, maintaining enhanced directional perception of the target perception sector, monitoring the safe distance between the robot and high-risk areas in real time, and interrupting the micro-motion when a violation of the safe distance constraint is detected.

8. The robot obstacle avoidance method based on millimeter-wave radar sparse point clouds according to claim 1, characterized in that, The process of updating the risk grid map includes: after the verification micro-movement ends, registering the collected augmented point cloud with the robot's current pose information and mapping it to the corresponding grid in the risk grid map; statistically analyzing the augmented point cloud falling into each grid to obtain the current observation result of each grid; based on a preset temporal fusion strategy, fusing the current observation result with historical observation results and historical echo statistics of preset locations in the prior environmental map to update the risk level of each grid; and updating the risk grid map according to the updated risk level of each grid.

9. A robot obstacle avoidance system based on millimeter-wave radar sparse point clouds, characterized in that, The system employs a robot obstacle avoidance method based on millimeter-wave radar sparse point clouds as described in any one of claims 1 to 8, comprising: Point cloud acquisition module: Acquires sparse point cloud, pose information and velocity information of the robot in the current frame; compensates the sparse point cloud based on the pose information and performs reverse registration to obtain the enhanced point cloud; Negative obstacle recognition module: Divide the area in front of the robot into several sectors according to the azimuth angle, perform local ground fitting on the enhanced point cloud of each sector to obtain the ground model of each sector; identify negative obstacle areas by comparing the height continuity of the local ground models of adjacent sectors. Weak obstacle identification module: Based on the millimeter-wave radar echo data collected during the current operation and the historical echo statistics of preset locations in the environmental prior map, combined with the current time series accumulation, it identifies weak reflection obstacle areas; Point cluster identification module: Detects suspected obstacle point clusters in the enhanced point cloud and determines whether the suspected obstacle point clusters are multipath false point clusters or dynamic point clusters; marks multipath false point clusters as low-confidence obstacles, dynamic point clusters as dynamic obstacles, and other suspected obstacle point clusters as real obstacles; Risk Map Module: Maps negative obstacle areas, weak reflection obstacle areas, low confidence obstacles, dynamic obstacles, and real obstacles onto a robot-centered risk grid map, sets different risk weights and safety expansion radii for different types of obstacles, and calculates the risk level of each grid. Trial and verification module: Generates candidate trajectories, controls millimeter-wave radar to enhance directional perception in high-risk areas of the candidate trajectories, and controls the robot to perform verification micro-movements; Obstacle avoidance decision module: After confirmatory micro-movement, update the risk grid map and plan the final obstacle avoidance trajectory on the updated risk grid map.

Images