Dimension reduction mapping positioning and obstacle avoidance method

Abstract

The application relates to the technical field of robot positioning and obstacle avoidance, and discloses a dimension-reduced mapping positioning and obstacle avoidance method, which comprises the following steps: receiving high-density point clouds, fusing inertial measurement unit and wheel speed encoder data; projecting the point clouds to a two-dimensional plane to generate a two-dimensional grid map, and associating the height semantic information of corresponding point cloud points with non-empty grids, and retaining point cloud points with a height greater than the sum of a ground height and a preset height threshold; running a lightweight simultaneous localization and mapping algorithm to estimate the robot pose, and performing path planning and obstacle avoidance decision-making. According to the scheme, the dense three-dimensional point clouds are compressed into a lightweight grid map with height semantics through height threshold filtering and adaptive two-dimensional projection, the calculation load is reduced, the passable area semantic labeling and speed adaptive obstacle avoidance mechanism are introduced, the obstacle avoidance accuracy and efficiency are improved, the hardware cost and power consumption are compressed, the endurance is prolonged, the 10 Hz-level environmental perception and response capability is ensured, and the method is suitable for resource-limited consumer-level mobile robots.

Description

Technical Field

[0001] This invention relates to the field of robot localization and obstacle avoidance technology, and in particular to a dimensionality-reduced mapping localization and obstacle avoidance method. Background Technology

[0002] A robot is an intelligent electromechanical system that can perceive its environment, make autonomous decisions, and perform tasks. It typically integrates sensors, controllers, actuators, and software algorithms. Depending on the application scenario, robots can be divided into industrial robots, service robots, and special-purpose robots. In mobile robots such as sweeping robots, warehouse AGVs, and delivery robots, the core capabilities include environmental mapping, self-localization, and dynamic obstacle avoidance. These functions rely on sensors such as LiDAR, cameras, and IMUs, as well as algorithms such as Simultaneous Localization and Mapping (SLAM).

[0003] Existing technologies have significant shortcomings in localization and mapping applications for ground-based mobile robots. While mainstream 3D SLAM solutions can use multi-line LiDAR or depth cameras, their algorithms are highly complex and require high-performance computing platforms for real-time operation, making them difficult to adapt to low-cost, low-power embedded systems. The amount of point cloud data they process is enormous, making real-time operation difficult on resource-constrained MCUs. Furthermore, these robots typically only move on the horizontal plane and have limited requirements for Z-axis height information, yet existing methods still process 3D data completely, resulting in a large amount of redundant computation. In addition, small undulations in the ground are often misjudged as obstacles, causing accidental obstacle avoidance triggers or even path planning failures. Although some systems attempt to project 3D point clouds into 2D maps to simplify processing, they mostly use fixed height thresholds and static projection planes, which cannot adaptively adjust according to robot posture or terrain changes, and lack the semantic recognition ability for "passable low obstacles," leading to inaccurate environmental understanding and low passage efficiency. Summary of the Invention

[0004] The purpose of this invention is to construct a lightweight, adaptive dimensionality reduction mapping and obstacle avoidance method for mobile robots that move on the ground plane. By fusing IMU attitude information to dynamically correct point cloud projection and retaining key height channels in the two-dimensional semantic map, a real-time localization and navigation method with high robustness and low misjudgment rate can be achieved on a low-cost embedded platform.

[0005] To achieve the above objectives, the present invention adopts the following technical solution:

[0006] A dimensionality-reduced mapping, localization, and obstacle avoidance method includes: high-density 3D point cloud acquisition and spatiotemporal synchronous preprocessing: receiving high-density point clouds output from 3D sensors and fusing data from inertial measurement units and wheel speed encoders to achieve precise time alignment and coordinate system unification; intelligent dimensionality reduction and semantic map construction: based on the preprocessed data from the previous stage, projecting the point cloud onto a 2D plane to generate a 2D grid map, where each non-empty grid is associated with the height semantic information of its corresponding point cloud point, wherein only point cloud points whose height is greater than the sum of the ground height and a preset height threshold are retained; lightweight SLAM and semantic-driven obstacle avoidance: based on the 2D grid map obtained in the previous stage, running a lightweight synchronous localization and mapping algorithm to estimate the robot pose, and combining the height semantic information for path planning and obstacle avoidance decisions.

[0007] As a preferred technical solution of the present invention, the high-density 3D point cloud acquisition and spatiotemporal synchronization preprocessing stage includes the following steps: 3D sensor selection and point cloud input configuration: supporting mainstream 3D environmental perception devices as front-end sensors, and transmitting raw point cloud data to the main control unit via gigabit Ethernet or high-speed USB interface; Multi-source sensor time synchronization strategy: adopting a hardware timestamp mechanism based on a high-precision counter inside the microcontroller to synchronize the 3D point cloud, inertial measurement unit, and wheel encoder data, using the point cloud arrival time as a reference, obtaining the corresponding inertial data through neighbor sampling interpolation, and accumulating encoder pulses to obtain instantaneous motion information; Point cloud coordinate system I and preliminary filtering: performing coordinate system I and noise suppression on the raw point cloud, transforming it from the sensor coordinate system to the global world coordinate system, and removing outliers through depth range constraints, neighborhood consistency detection, and median filtering to provide input for subsequent processing.

[0008] As a preferred technical solution of the present invention, in the intelligent dimensionality reduction and semantic map construction stage, the step of projecting the point cloud onto a two-dimensional plane includes the following steps: when the absolute value of the robot's pitch angle is greater than a preset angle threshold, an inclined projection plane is constructed based on the pitch angle, and the effective height of each point cloud point to the inclined projection plane is calculated; when the absolute value of the pitch angle is not greater than the preset angle threshold, a horizontal projection plane is used, and the original z coordinate of the point cloud point is used as the effective height; only when the effective height is greater than the preset height threshold, the x and y coordinates of the corresponding point cloud point are mapped to the two-dimensional grid map.

[0009] As a preferred embodiment of the present invention, the normal vector of the tilted projection plane is [0, sin(θ), cos(θ)], where θ is the pitch angle, and the effective height is expressed as:

[0010] heff=sin(θ)*(y-yr)+cos(θ)*(z-zr);

[0011] Where heff is the effective height, (x, y, z) are the coordinates of the point cloud points to be processed, and (xr, yr, zr) is the robot's current pose.

[0012] As a preferred technical solution of the present invention, after generating the two-dimensional grid map, each non-empty grid is labeled with a passable semantic label: if the maximum height of the point cloud points in the grid does not exceed a preset passable height threshold, it is marked as a passable area; otherwise, it is marked as an obstacle area.

[0013] As a preferred embodiment of the present invention, in the lightweight SLAM and semantic-driven obstacle avoidance, the path planning adopts an improved A* algorithm, and its node evaluation function is:

[0014] f(n)=g(n)+λ*||n-ngoal||2+wh*cost(n);

[0015] Where g(n) is the actual path length from the starting point to node n, ||n-ngoal||2 is the Euclidean distance from node n to the target node ngoal, cost(n) is the passage cost of the grid where node n is located, λ is the heuristic weight, and wh is the height semantic weight.

[0016] As a preferred technical solution of the present invention, in the intelligent dimensionality reduction and semantic map construction, before projecting the point cloud, motion distortion compensation is performed on the current frame point cloud using wheel encoder data, including: performing inverse rigid body transformation on the point cloud according to the pose increment between adjacent frames to eliminate the point cloud deformation caused by robot motion during scanning.

[0017] As a preferred technical solution of the present invention, in the lightweight SLAM and semantic-driven obstacle avoidance stage, the lightweight synchronous localization and mapping algorithm adopts a particle filter framework, the number of particles is configured to a finite value to meet the real-time requirements of the embedded platform, and pose estimation and map updating are performed based on the two-dimensional grid map.

[0018] As a preferred technical solution of the present invention, when the robot's running speed is greater than a preset speed threshold, the obstacle avoidance safety radius is dynamically expanded.

[0019] As a preferred embodiment of the present invention, the amount of expansion of the obstacle avoidance safety radius is dynamically determined based on the difference between the running speed and the preset speed threshold.

[0020] The present invention has the following beneficial effects:

[0021] 1. Drastic reduction in computing power requirements: This solution significantly reduces the amount of data involved in the calculation by intelligently filtering high-density 3D point clouds based on height thresholds and projecting them into a 2D raster map with semantic information. At the same time, it adopts a lightweight GMAPping SLAM algorithm with limited particle count, enabling the entire mapping and localization process to run efficiently on resource-constrained embedded microcontrollers, significantly reducing the requirements for computing performance.

[0022] 2. Hardware cost reduction: This solution prioritizes low-cost TOF depth cameras as the main environmental perception sensor, and the navigation algorithm is completed on a single MCU such as STM32H7, without the need to deploy additional high-performance computing units such as GPUs or AI acceleration modules, thereby achieving a significant reduction in the overall hardware cost, making it suitable for mass-produced consumer robot products.

[0023] 3. Improved obstacle avoidance accuracy: A height-based semantic safety passage mechanism is introduced, which distinguishes ground noise from real obstacles by setting a height threshold and marks low-lying passable areas. The path planner dynamically adjusts the passage cost accordingly, effectively avoiding misjudging carpets, ground cracks, etc. as insurmountable obstacles, while preventing the neglect of dangerous obstacles, thus improving the accuracy and rationality of obstacle avoidance decisions.

[0024] 4. Enhanced battery life: Since the system does not require a high-power computing module, it can complete all perception, mapping and decision-making tasks by relying only on a low-power MCU and a TOF sensor. The overall power consumption is significantly reduced. With the same battery capacity, the continuous working time of the robot can be effectively extended to meet the long-term autonomous operation needs in scenarios such as home.

[0025] 5. Real-time optimization: Thanks to point cloud dimensionality reduction and algorithm lightweighting, the system can achieve high-frequency closed-loop updates on embedded platforms; the entire process from point cloud acquisition, synchronization, projection to SLAM pose estimation and path planning has low latency, supports environmental perception and response at the 10 Hz level, and ensures the robot's timely response and stable operation in dynamic scenes. Attached Figure Description

[0026] Figure 1 This is a schematic diagram of the structure of a dimension-reduced mapping, localization, and obstacle avoidance method proposed in this invention;

[0027] Figure 2 A flowchart for the high-density 3D point cloud acquisition and spatiotemporal synchronization preprocessing stage;

[0028] Figure 3 A flowchart for the intelligent dimensionality reduction and semantic map construction phase;

[0029] Figure 4 A flowchart for safe height channel annotation in lightweight SLAM and semantic-driven obstacle avoidance steps. Detailed Implementation

[0030] The technical solutions of the present invention will be clearly and completely described below with reference to the accompanying drawings of the embodiments of the present invention. Obviously, the described embodiments are only some embodiments of the present invention, and not all embodiments.

[0031] Before introducing the technical solutions described in this manual, a brief introduction to existing robot localization and obstacle avoidance methods will be given:

[0032] 1. Current mainstream high-precision positioning and mapping systems generally adopt SLAM algorithms based on 3D point clouds. Typical examples include LOAM, LeGO-LOAM, VINS-Mono / Fusion, and SuMa. These algorithms typically use depth cameras such as Intel RealSense D455 and Orbbec AstraPro as front-end sensors, and can output 10,000 to tens of thousands of 3D points per frame. The algorithm process involves point cloud preprocessing, feature extraction, inter-frame registration, pose graph optimization, and loop closure detection. The entire process is highly dependent on floating-point operations, large memory caches, and parallel computing capabilities. Therefore, actual deployments are almost entirely concentrated on high-performance edge computing platforms with Linux operating systems, such as NVIDIA Jetson series, Rockchip RK3588, or Raspberry Pi 4B+, etc. Their typical computing power can reach tens to hundreds of GFLOPS, and the memory capacity is 4–8GB.

[0033] However, these platforms have high power consumption, typically 5–20W, high cost, slow startup, and complex systems, making them difficult to adapt to a large number of low-cost robotic products targeting the consumer market. For example, household robotic vacuum cleaners, small logistics AGVs, or educational robots often use ARM Cortex-M7 / M4 series microcontrollers, such as the STM32H743, with a main frequency of 480MHz, only 1–2MB of SRAM, no MMU, and no Linux support. On such resource-constrained platforms, even with significant reductions in LOAM or VINS, the processing frame rate is generally still below 5FPS, far from meeting the 10–20Hz update frequency required for real-time navigation. Therefore, there is a serious computing power gap between existing 3D SLAM solutions and mainstream embedded hardware.

[0034] 2. In pursuit of high environmental perception accuracy, current high-end mobile robots generally construct a perception link centered on "3D LiDAR + high-performance main control + high-precision IMU". Typical configurations include: 16-line or higher mechanical rotating LiDAR, solid-state MEMS radar, or high frame rate RGB-D camera; coupled with a high-precision six-axis IMU; plus a main control module such as Jetson NX. The material cost of the entire perception and computing subsystem often exceeds five thousand, accounting for more than 50% of the total cost of the machine.

[0035] This high-cost architecture is acceptable in scientific research verification or high-end commercial scenarios, such as hospital delivery and high-end hotel service robots, but it is completely unsustainable in large-scale mass production scenarios. Taking a household robot vacuum cleaner with an annual shipment of more than 10 million units as an example, the cost of the whole machine needs to be controlled in the range of 500-1000. If the sensing module alone accounts for most of the cost, it will greatly compress the profit margin and weaken the market competitiveness.

[0036] 3. Although the movement of most service robots, such as sweeping robots, food delivery vehicles, and warehouse handling AGVs, is strictly limited to a two-dimensional ground plane, with only x and y translation and rotation around the z axis, their core tasks only need to determine "whether it is passable ahead" and "their own position in the plane", and do not need to reconstruct a complete three-dimensional spatial structure, existing SLAM systems still follow the general three-dimensional mapping paradigm, including all point clouds collected by sensors, including areas completely unrelated to navigation such as ceilings, tops of high furniture, high areas of walls, and lamps, in the processing flow. Whether it is point cloud registration, map storage such as OctoMap octree, or obstacle detection, it is all done in full 3D mode, resulting in a large amount of computing resources being wasted on non-task-related areas;

[0037] Specifically, in LOAM-type algorithms, edge and planar feature extraction traverses all points, even if 90% of the points are located above 1.5 meters; in VINS, visual feature points may originate from distant walls or ceilings, contributing minimally to ground localization; and in the subsequent path planning stage, A* or DWA algorithms still need to search through a massive 3D cost map, resulting in inefficiency. This "all-encompassing" approach not only increases CPU load and memory consumption but also prolongs processing latency and reduces system response speed. Therefore, existing methods lack targeted design for specific application scenarios and are essentially an over-modeling approach, which is particularly undesirable in resource-constrained embedded systems.

[0038] 4. To alleviate the burden of 3D processing, some practical systems attempt to "reduce the dimensionality" of 3D point clouds into 2D grid maps. A common approach is to set a fixed Z-axis height threshold, orthogonally project the points within that interval onto the XY plane, and generate pseudo-laser scan data for use by 2DSLAM (such as GMAPping and Cartographer). Alternatively, algorithms such as RANSAC can be used to fit the global ground plane, and after removing ground points, the remaining points are treated as obstacles. These methods perform reasonably well under ideal conditions where the laboratory floor is flat and the robot remains level.

[0039] However, in real-world home or office environments, robots often face a variety of challenges: thresholds 2–5 cm high on the floor, localized bulges in carpets, uneven micro-terrain variations in tile joints; at the same time, robots will experience pitch or roll changes when climbing, turning, or overcoming obstacles. In these situations, a fixed height threshold may misjudge low structures that could be easily crossed as impassable obstacles; and when the static horizontal projection surface is tilted, it will cause geometric distortion of the point cloud. For example, a wall that was originally vertical will shift to one side after projection, causing map misalignment and positioning drift. More importantly, existing methods rarely distinguish between "dangerous obstacles" and "safe low obstacles," treating them all as blocked areas, causing robots to frequently stop at obstacles, fail to detour, or spin in place, thus easily leading to obstacle avoidance misjudgments.

[0040] Regarding the above phenomenon, please refer to the appendix. Figure 1 This invention proposes a dimensionality-reduced mapping, localization, and obstacle avoidance method. By intelligently compressing high-density 3D point clouds into a 2D grid map with high semantic depth, high-precision mapping, localization, and robust obstacle avoidance are achieved on resource-constrained embedded microcontrollers. The method is divided into three stages: high-density 3D point cloud acquisition and spatiotemporal synchronous preprocessing, intelligent dimensionality reduction and semantic map construction based on attitude adaptation, and lightweight SLAM and semantic-driven obstacle avoidance, forming a complete closed loop from perception to decision-making.

[0041] Please refer to the appendix. Figure 2 High-density 3D point cloud acquisition and spatiotemporal synchronization preprocessing: This involves receiving high-density point clouds from 3D sensors and fusing data from the inertial measurement unit (IMU) and wheel speed encoder to achieve precise time alignment and coordinate system unification. This provides consistent and reliable raw input for subsequent dimensionality reduction. Specifically, this includes the following steps:

[0042] Step 1.1, 3D Sensor Selection and Point Cloud Input Configuration: The system supports mainstream 3D environment perception devices as front-end sensors to balance performance and cost flexibility, including 16-line mechanical rotating LiDARs such as Velodyne VLP-16 and Hesai Pandar16. At a standard 10Hz rotation frequency, each frame scan can generate approximately 20,000 3D points, covering a 360° horizontal field of view, a vertical field of view of ±15°, and a ranging accuracy of ±2cm. The preferred sensor in this solution is a low-cost, high-resolution area array TOF depth camera such as PicoMonstar or OVC3870, which outputs approximately 30,000 depth points per frame at QVGA resolution, with a field of view of 60°×60° and an update frequency of 10Hz. It has advantages such as small size, low power consumption, resistance to strong light interference, and low cost. In cost-sensitive applications such as home robot vacuums and food delivery vehicles, using TOF cameras to replace traditional multi-line LiDAR can reduce the hardware cost of the sensing module from tens of thousands of yuan to less than a thousand yuan, while still providing sufficient point cloud density to support the dimensionality reduction mapping function of this solution. All the aforementioned sensors transmit raw point cloud data to the main control unit via gigabit Ethernet or high-speed USB interfaces. This solution does not rely on reducing sensor resolution or frame rate to adapt to embedded platforms; instead, it achieves computational compression through a subsequent intelligent dimensionality reduction mechanism, ensuring a significant reduction in the burden on downstream algorithms while maintaining sensing capabilities.

[0043] Step 1.2, Multi-source sensor time synchronization strategy: To ensure the consistency of point cloud, IMU, and wheel speed data fusion under dynamic motion, a hardware interrupt-based timestamp alignment mechanism is adopted. All sensor data is enqueued with a microsecond-level timestamp generated by the MCU's internal DWT counter. When a frame of 3D point cloud arrives, its timestamp tk is used as the reference time. The two closest samples before and after tk are extracted from the IMU's circular buffer, and the angular velocity and acceleration at time tk are obtained through first-order linear interpolation. Similarly, the increment accumulated from the encoder pulse buffer to tk is converted into instantaneous linear velocity and angular velocity. This synchronization strategy achieves a time error of less than 5ms in typical scenarios where the robot's motion speed is below 1m / s, meeting the subsequent fusion accuracy requirements. Furthermore, it does not require complex hardware triggering or the PTP protocol, making it suitable for low-cost embedded deployments.

[0044] Step 1.3, Point Cloud Coordinate System and Preliminary Filtering: The original point cloud is located in the sensor coordinate system and requires two transformations: First, it is transformed to the robot body coordinate system using factory calibration parameters (rotation matrix and translation vector); then, it is projected to the global world coordinate system based on the currently estimated robot pose, which is initially the origin. Outlier suppression is then performed: if the depth value of a point exceeds the effective range, or the depth difference between it and its spatial neighbors exceeds a threshold, it is considered noise and removed. In one specific implementation, the effective depth range is set to 0.1m to 4.0m, and the neighborhood depth difference threshold is set to 0.5m. Operators can adaptively adjust these thresholds according to the actual sensor model, application scenario lighting conditions, and robot movement speed. Furthermore, the system performs median filtering on the depth value of each point within its 3×3 neighborhood to further suppress isolated outliers caused by strong light, glass reflection, or low-reflectivity surfaces. This preprocessing significantly improves the point cloud quality, providing high-fidelity input for subsequent dimensionality reduction.

[0045] Please refer to the appendix. Figure 3 Intelligent dimensionality reduction and semantic map construction based on attitude adaptation: Through height threshold filtering, 2D projection, and dynamic plane calibration, dense point clouds of high points / frames are compressed into a 2D occupancy grid map, and safe height channel semantics are introduced to provide lightweight, high-fidelity input for SLAM and obstacle avoidance. The entire process can be efficiently executed on resource-constrained embedded microcontrollers. This includes:

[0046] Step 2.1, Ground noise filtering based on height threshold: The system sets a height threshold Hthres, preferably Hthres=0.02m, to filter ground noise caused by minor undulations, such as low points generated by carpet fibers, pebbles, and tile seams. Let the currently estimated ground height be hground (usually initialized to the Z value corresponding to the robot's installation height, e.g., 0.05m). Then, for the input 3D point cloud P={(x1, y1, z1), ..., (xn, yn, zn)}, only points satisfying zi>hground+Hthres are retained; the rest are filtered out. This operation effectively removes low-lying objects that have no actual obstructive significance, preventing them from being misjudged as obstacles, significantly improving map purity, avoiding frequent obstacle stops caused by ground textures being misjudged as obstacles, and significantly improving navigation smoothness.

[0047] Step 2.2, Adaptive Projection Plane Generation: By default, the system directly projects the X / Y coordinates of the retained points onto the global 2D plane, ignoring the Z-axis information, to generate a 2D occupancy grid map with a preferred resolution of 5cm. Its core logic is as follows:

[0048] def project_to_2D(P, H_thres=0.02):

[0049] # Altitude filtering (to eliminate interference from ground undulations)

[0050] P_filtered = [p for p in P if pz > ground_height + H_thres]

[0051] # Generate a 2D occupancy grid

[0052] grid_map = np.zeros((GRID_SIZE, GRID_SIZE))

[0053] for p in P_filtered:

[0054] # Ignore the Z-axis and only retain the X / Y plane projection

[0055] x_idx = int((px - map_origin_x) / grid_res)

[0056] y_idx = int((py - map_origin_y) / grid_res)

[0057] grid_map[x_idx][y_idx] = 1 # Mark as obstacles

[0058] return grid_map

[0059] Furthermore, when the robot's posture tilts significantly, such as when climbing or overcoming obstacles, a fixed horizontal projection will cause the obstacle's position to shift, affecting the mapping accuracy. To address this, the system sets up a dynamic plane calibration mechanism: when the IMU detects that |θ| exceeds a threshold (preferably 5°, approximately 0.087 rad), the system uses the robot's current position as a reference point to construct a local projection plane with a normal vector of n=[0, sinθ, cosθ], where θ is the pitch angle.

[0060] This vector corresponds to a plane rotated θ around the Y-axis, which more accurately reflects the local terrain. On this new plane, the distance from any point (x, y, z) to the plane, i.e., the effective height, is:

[0061] heff=sin(θ)*(y-yr)+cos(θ)*(z-zr);

[0062] Where (x, y, z) are the coordinates of the point cloud to be processed, and (xr, yr, zr) is the robot's current position. A point is considered an obstacle only when hefl > Hthres. The final grid coordinates are still determined by (x, y), ensuring... Figure 1Consistency is achieved when the pose changes drastically; otherwise, the horizontal projection is maintained, balancing efficiency and accuracy. The overall implementation code is as follows:

[0063] #include<math.h>

[0064] #define H_THRES 0.02f / / 2cm height threshold

[0065] #define PITCH_THRESH_RAD 0.087f / / 5 degrees in radians

[0066] / / Input: Point cloud array points, number of points num_points; robot pose robot_x / y / z; IMU pitch angle pitch_rad

[0067] / / Output: 2D grid map (simplified here as a list of marked obstacles)

[0068] void adaptive_projection(

[0069] const Point3D* points, int num_points,

[0070] float robot_x, float robot_y, float robot_z,

[0071] float pitch_rad,

[0072] GridMap* grid_map)

[0073] {

[0074] float ground_height = robot_z; / / Assuming ground height ≈ robot Z coordinate

[0075] Bool use_tilted_plane = fabsf(pitch_rad) > PITCH_THRESH_RAD;

[0076] for (int i = 0; i < num_points; i++) {

[0077] float x = points[i].x;

[0078] float y = points[i].y;

[0079] float z = points[i].z;

[0080] float effective_height;

[0081] if (!use_tilted_plane) { / / Horizontal projection: use Z-height directly

[0082] effective_height = z - ground_height;

[0083] } else { / / Tilt projection: use the calibrated plane

[0084] float sin_p = sinf(pitch_rad);

[0085] float cos_p = cosf(pitch_rad);

[0086] effective_height = sin_p * (y - robot_y) + cos_p * (z - robot_z);}

[0087] / / Height filtering: Only keep points above the threshold

[0088] if (effective_height > H_THRES) {

[0089] / / Project onto the X / Y plane (regardless of tilt, the final map will still be a horizontal grid)

[0090] int gx = (int)((x - MAP_ORIGIN_X) / GRID_RES);

[0091] int gy = (int)((y - MAP_ORIGIN_Y) / GRID_RES);

[0092] if (gx >= 0 && gx < GRID_SIZE && gy >=0 && gy < GRID_SIZE) {

[0093] grid_map->data[gx][gy]=1

[0094] ; / / Mark as obstacle}

[0095] }

[0096] }}

[0097] Step 2.3: Integrating Wheel Odometer Data to Compensate for Motion Distortion: Due to non-rigid deformation, slippage, or sensor delays during robot movement, the point cloud may experience relative displacement between different frames, leading to map stitching distortion. Therefore, this solution integrates wheel odometer data to perform motion compensation on the point cloud for each frame. Specifically, the robot's translational and rotational increments Δx, Δy, and Δθ are obtained by integrating the wheel speed between two consecutive frames. The point cloud of the current frame is first rotated inversely by -Δθ, then translated (-Δx, -Δy) to align with the previous frame. This operation is performed before projection, eliminating point cloud stretching caused by robot movement during scanning, effectively reducing motion blur, and improving map continuity.

[0098] Lightweight SLAM and Semantic-Driven Obstacle Avoidance: Based on a dimensionality-reduced 2D grid map, an optimized SLAM engine is run for pose estimation, and combined with safe height channel semantics to achieve high-precision localization and intelligent obstacle avoidance decision-making, specifically including:

[0099] Step 3.1, Lightweight GMAPpingSLAM Implementation: The SLAM engine adopts an improved Rao-Blackwellized particle filter framework, deeply optimized for the MCU platform. Key optimizations include: reducing the number of particles from the traditional 1000 to 50, fully utilizing the consistency and sparsity of the projected point cloud; removing the height dimension calculation from the matching cost function, and evaluating similarity only based on the two-dimensional occupancy probability; increasing the localization update frequency to 10Hz, twice that of traditional 3D-SLAM. Loop closure detection is triggered only when the displacement exceeds 5 meters and the CPU is idle, using fast ICP (≤5 iterations) to estimate pose correction. The entire SLAM system runs on an STM32H7 with a frame rate of 10–12Hz, a localization error (ATERMSE) better than 5cm, CPU utilization <35%, and memory usage <600KB.

[0100] Step 3.2, Safe Height Passage Marking: The system marks the permitted low-obstacle areas on the 2D grid map. Please refer to the attached document. Figure 4 The specific rules are as follows: For each non-empty grid cell, calculate the maximum Z-coordinate zmax of all points within it. If zmax ≤ a threshold (which can be 3cm), it is marked as a "passable area" with a path planning cost weight of 0.1, while traditional obstacles have a weight of 1; otherwise, it is marked as a "prohibited area". This mechanism enables the robot to identify and traverse low obstacles such as thresholds and slippers, avoiding unnecessary detours.

[0101] Step 3.3: Improved A* Path Planning and Dynamic Obstacle Avoidance: Based on the 2D grid map with height semantics generated in the aforementioned dimensionality reduction stage, the path planner adopts an improved A* algorithm, where the passage cost of each grid is dynamically assigned according to its maximum observation height: if the height is ≤3cm, the cost is set to 0.1; otherwise, it is set to 1. The node cost function is expressed as:

[0102] f(n)=g(n)+λ*||n-ngoal||2+wh*cost(n);

[0103] Where g(n) represents the actual cost from the starting point to the current node, usually the path length; ||n-ngoal||2 is the Euclidean distance heuristic, λ is the heuristic weight, preferably 0.8; cost(n) represents the passage cost of the node itself, for example: empty ground = 0.1, soft obstacles = 1.0, hard obstacles = ∞; wh is the height semantic weight, which adjusts the importance of environmental cost relative to path length. In a preferred embodiment, the height semantic weight wh is set to 2.0, so that path planning achieves a good balance between obstacle avoidance safety and path efficiency; this parameter can be adjusted according to the robot chassis performance, application scenario or user preference.

[0104] This design allows the planner to balance path length with traffic safety: when the detour distance is too long, it allows passage through "soft obstacle" areas. Simultaneously, the system introduces a dynamic safety boundary adjustment mechanism: when the robot's speed exceeds 1 m / s, it automatically expands the obstacle avoidance radius by 20% to prevent collisions at high speeds. This mechanism significantly improves the system's robustness in complex home environments.

[0105] In summary, this invention constructs a complete, efficient, and low-cost dimensionality reduction navigation system through a three-stage collaborative design. It uses an altitude threshold Hthres to filter out ground noise, an adaptive projection plane to correct attitude distortion, and a safe altitude channel to endow the two-dimensional map with semantic intelligence. The entire solution can process dense point clouds of 30,000 points / frame in real time on a single MCU, achieving a dramatic reduction in computing power requirements, lower hardware costs, improved obstacle avoidance accuracy, extended battery life, and 10Hz real-time performance. It combines engineering practicality with technological advancement.

[0106] The above description is only a preferred embodiment of the present invention, but the scope of protection of the present invention is not limited thereto. Any equivalent substitutions or modifications made by those skilled in the art within the scope of the technology disclosed in the present invention, based on the technical solution and inventive concept of the present invention, should be covered within the scope of protection of the present invention.

Claims

1. A method for dimension-reduced mapping, localization, and obstacle avoidance, characterized in that, Includes the following stages: High-density 3D point cloud acquisition and spatiotemporal synchronization preprocessing: Receive high-density point cloud output from 3D sensors and fuse data from inertial measurement unit and wheel speed encoder to achieve time alignment and coordinate system unification; Intelligent dimensionality reduction and semantic map construction: Based on the preprocessed data from the previous stage, the point cloud is projected onto a two-dimensional plane to generate a two-dimensional grid map. Each non-empty grid is associated with the height semantic information of its corresponding point cloud point. Only point cloud points whose height is greater than the sum of the ground height and a preset height threshold are retained. The process of projecting the point cloud onto a two-dimensional plane includes the following steps: when the absolute value of the robot's pitch angle is greater than a preset angle threshold, an inclined projection plane is constructed based on the pitch angle, and the effective height of each point cloud point to the inclined projection plane is calculated; when the absolute value of the pitch angle is not greater than the preset angle threshold, a horizontal projection plane is used, and the original z-coordinate of the point cloud point is used as the effective height; only when the effective height is greater than a preset height threshold, the x and y coordinates of the corresponding point cloud point are mapped to the two-dimensional grid map. The normal vector of the tilted projection plane is [0, sin(θ), cos(θ)], where θ is the pitch angle, and the effective height is expressed as: heff = sin(θ)*(y-yr) + cos(θ)*(z-zr); where heff is the effective height, (x, y, z) are the coordinates of the point cloud points to be processed, and (xr, yr, zr) is the current pose of the robot; After generating the two-dimensional raster map, a common semantic label is marked for each non-empty raster. If the maximum effective height of point cloud points within a non-empty grid does not exceed the preset passable height threshold, it is marked as a passable area; otherwise, it is marked as an obstacle area. Before projecting the point cloud, motion distortion compensation is performed on the current frame point cloud using wheel encoder data, including: performing inverse rigid body transformation on the point cloud based on the pose increment between adjacent frames to eliminate point cloud deformation caused by robot motion during scanning. Lightweight SLAM and semantic-driven obstacle avoidance: Based on the two-dimensional grid map obtained in the previous stage, a lightweight simultaneous localization and mapping algorithm is run to estimate the robot pose, and the height semantic information is combined to perform path planning and obstacle avoidance decisions. The lightweight synchronous localization and mapping algorithm adopts a particle filter framework, and the number of particles is configured to a finite value to meet the real-time requirements of the embedded platform. Pose estimation and map updating are performed based on the two-dimensional grid map. In the lightweight SLAM and semantic-driven obstacle avoidance, the path planning adopts the improved A* algorithm, whose node evaluation function is: f(n)=g(n)+λ*||n-ngoal||2+wh*cost(n); where g(n) is the actual path length from the starting point to node n, ||n-ngoal||2 is the Euclidean distance from node n to the target node ngoal, cost(n) is the passage cost of the grid where node n is located, λ is the heuristic weight, and wh is the height semantic weight; When the robot's operating speed exceeds a preset speed threshold, the obstacle avoidance safety radius is dynamically expanded.

2. The method for dimension reduction mapping, localization, and obstacle avoidance according to claim 1, characterized in that, The high-density 3D point cloud acquisition and spatiotemporal synchronization preprocessing stage includes the following steps: 3D sensor selection and point cloud input configuration: Supports mainstream 3D environment perception devices as front-end sensors, and transmits raw point cloud data from the sensors to the main control unit via gigabit Ethernet or high-speed USB interface; Multi-source sensor time synchronization strategy: A hardware timestamp mechanism based on a high-precision counter inside the microcontroller is used to synchronize the data of the 3D point cloud, inertial measurement unit and wheel encoder. The arrival time of the point cloud is used as a reference, and the inertial data at the corresponding time is obtained by neighbor sampling interpolation. The encoder pulses are accumulated to obtain instantaneous motion information. Point cloud coordinate system 1 and preliminary filtering: The original point cloud is subjected to coordinate system 1 and noise suppression, and is transformed from the sensor coordinate system to the global world coordinate system. Outliers are removed through depth range constraints, neighborhood consistency detection and median filtering to provide input for subsequent processing.

3. The method for dimension reduction mapping, localization, and obstacle avoidance according to claim 1, characterized in that, The amount by which the obstacle avoidance safety radius is expanded is dynamically determined based on the difference between the operating speed and the preset speed threshold.

Images