Complex-environment-oriented navigation and three-dimensional reconstruction method for body-equipped intelligent agent

Abstract

The invention discloses a complex environment-oriented body agent navigation and three-dimensional reconstruction method, and relates to the technical field of body agent navigation, and the method comprises the following steps: collecting environment data around a navigation body, using a fine-grained octree for close-range key areas (narrow slits and dead corners), adopting coarse voxel acceleration for a far area, and adopting coarse voxel acceleration for a near-range key area (narrow slits and dead corners); extracting large planes such as wall surfaces, beam columns and the like by using RANSAC (Random Sample Consensus), and constructing an environment'abstract skeleton 'for navigation reference; establishing a temporary three-dimensional space based on the point cloud of the whole body environment, dividing a plurality of detection planes in the space, performing position correction on an obstacle edge dot matrix in the detection planes, determining a movable range, and generating a safe motion path in a feasible space by applying a local planning algorithm; octree + differential coding is carried out on a local map, and only a current activity area and crisis point cloud are reserved; local full-stack deployment enables navigation to be free of dependence on remote communication, and sensing, mapping and navigation closed loop can still be completed when a link is broken.

Description

TECHNICAL FIELD

[0001] The present application relates to the technical field of environmental navigation and construction, in particular to a somatic agent navigation and three-dimensional reconstruction method for complex environments. BACKGROUND

[0002] In complex, dynamic and semi-structured indoor / outdoor scenes, a somatic agent needs to complete target navigation and environmental mapping reconstruction at the same time, which complement each other: navigation relies on accurate local / global three-dimensional maps, and three-dimensional reconstruction requires agent autonomous motive-driven perspective collection and strategy planning.

[0003] After semantic detection, ANM (neural SLAM module) is used for local map stitching, LSP-UNet is responsible for front edge attribute prediction (depth, normal), UPEN ensemble learning generates an uncertainty map to realize voxel completion of unknown areas, and then traditional or neural rendering is used to generate dense point cloud / grid. Using multi-view RGB images and corresponding camera poses, a scene NeRF model is trained to generate continuous body rendering while maintaining details, which is used for semantic-level and geometric-level fine-grained reconstruction, and the available map is output through voxelization or vertexization. Semantic instance segmentation is projected to 3D point cloud, combined with geometric reconstruction products, to generate a three-dimensional map with semantic labels, providing an interpretable environmental model for navigation planning.

[0004] Publication No. CN118258396A discloses a large model-driven somatic agent zero-shot target navigation method, which includes: first, the somatic agent obtains color and depth images of the scene by rotating and shooting; YOLO v7 and DETR models are used to process the color images to identify object labels, and LLaVA and CLIP models are used to generate subtitles for the images to describe the scene content. These information are used as scene clues for navigation; based on these clues, LLaMA and GPT-4 models are used to infer and determine the navigation transition points of the agent; on the Gazebo simulation platform, gmapping and hector_slam tools are used for path planning to construct a navigation cost map, so that the agent can avoid obstacles. After the agent reaches a new navigation transition point, GLIP and InstructDET models are used for open semantic target positioning; if the target positioning accuracy reaches a preset threshold, a target navigation success signal will be triggered; otherwise, the agent will continue to infer the next navigation transition point and repeat the process until the navigation is successful or fails.

[0005] Earthquakes often cause road interruptions, communication interruptions, power interruptions, and extremely chaotic on-site environments. The navigation and three-dimensional reconstruction of the embodied agent can quickly generate a high-precision three-dimensional scene and a feasible path in the disaster area, supporting decision-making of the command department and scheduling of the rescue team. However, when the rescue robot penetrates into the interior of the ruins, it is affected by the communication interruption, which affects the path planning of the rescue robot and leads to a sharp increase in the complexity of actual deployment. SUMMARY

[0006] One of the purposes of the present application is to provide a navigation and three-dimensional reconstruction method for embodied agents in complex environments, which can quickly identify the environment around the body when navigating and reconstructing in the ruins, and then complete the planning of the path, improve the processing capability and speed of the environment around the body, and complete the scheduling task more quickly.

[0007] To achieve the above purpose, the present application is realized by the following technical scheme: a navigation and three-dimensional reconstruction method for embodied agents in complex environments, comprising the following steps:

[0008] A data acquisition matrix deployed in the navigation main body is used to acquire the environment data around the navigation main body, different sensor data is time-stamped and calibrated online, and visual motion blur and laser speckle interference are eliminated through real-time filtering;

[0009] Local front-end SLAM and redundant perception, tightly coupled VIO+LiDAR SLAM: jointly optimize visual odometry and laser loop in the sliding window to reduce drift, automatically switch to IMU+wheel speed odometry dead push mode when vision or LiDAR fails, and combine the existing local map to suppress drift;

[0010] Fine-grained octree is used for close-range key areas (narrow gaps, dead angles), coarse voxel is used for remote areas, RANSAC is used to extract wall surfaces and beam column planes, and an environment "abstract skeleton" is constructed for navigation reference;

[0011] MobileNetV3-based segmentation is run on EdgeTPU / GPU, high-precision reconstruction is performed on "passage" and "safe exit" categories, "collapse risk area", "unstable structure" and "target search point" are automatically labeled, and key events are reported in subsequent planning and communication;

[0012] Dynamic obstacle avoidance decision model, based on the environment point cloud around the body to establish a temporary three-dimensional space, and divide multiple detection planes in the space, correct the position of the obstacle edge point array in the detection plane, determine the movable range, and apply a local planning algorithm to generate a safe motion path in the feasible space;

[0013] Octree + differential encoding is performed on the local map, only the current active area and crisis point cloud are reserved, and the area boundary, key obstacle and target position data when the network is reachable are uploaded.

[0014] In one or more embodiments of the application, the GPS PPS clock signal is mounted when each sensor in the data acquisition matrix is dotted, ensuring the timestamp accuracy of data generated by different devices. After the master board receives the original environmental data, the arrival time is recorded through the ring buffer, and the time delay jitter is compensated. The sliding window is used to observe the features and the IMU pre-integral residual to optimize the camera-inertial navigation external parameter. In the SLAM startup stage, the online rigid matching is used to automatically estimate and periodically fine-tune.

[0015] In one or more embodiments of the application, RANSAC is used to extract large planes from the incremental point cloud, and these planes are connected into a skeleton graph. The skeleton model is used to filter out irrelevant small object noise and generate a simplified three-dimensional abstract representation. The point cloud slices within the last N frames or a fixed radius are maintained to support online insertion and rejection. The slices are quickly indexed through spatial hashing or KD-Tree, and the update is realized in milliseconds. Each new data only updates the octree and skeleton corresponding to the slice, without the need to reconstruct the global model. The abstract skeleton model is used as the underlying topological skeleton of the navigation graph for fast path search.

[0016] In one or more embodiments of the application, the collapse risk area detection includes:

[0017] In the local point cloud slice, fixed compensation is used to grid division in the XY plane, the height difference ΔZ of adjacent grids is calculated, and if ΔZ>H t , it is marked as a height mutation area;

[0018] For each folk song point, the normal vector n is obtained by using RANSAC to fit a plane;

[0019] The angle θ between the normal vector and the direction of gravity is calculated, and if θ>θ t , it is marked as an inclined risk area;

[0020] The "scattered gravel", "fragment", and "tile" category masks in the semantic segmentation output are used;

[0021] The local point cloud is superimposed with the height mutation area and the inclined risk area, and the intersection area is weighted to improve the risk level;

[0022] The height mutation area, the inclined risk area, and the intersection area are calculated according to different weights to obtain a comprehensive risk score;

[0023] The high-risk adjacent units are clustered into risk blocks, and the polygon boundary and center position are output for subsequent path avoidance and reporting.

[0024] In one or more embodiments of the present application, for the set threshold, the threshold in different environments is adjusted by an adaptive threshold strategy:

[0025] The mean μ and standard deviation σ of the height difference and the included angle are updated in real time in the last N frames of point clouds, and the threshold is set as μ=kσ, k is usually between 1.5-2, and is adaptively adjusted according to the noise level;

[0026] When the scene labels such as "indoor", "narrow channel" and "open outdoor" are detected, the respective preset thresholds are switched to.

[0027] In one or more embodiments of the present application, the structure instability detection:

[0028] In the sliding window or slice range, the main plane is fitted by RANSAC;

[0029] The vertical distance residual of all points to the plane is calculated, and if the residual variance σ 2 >σ t , the region has cracks or fragments;

[0030] Based on the semantic segmentation result, the point cloud of the "beam" and "column" categories is extracted and clustered according to the connected domain;

[0031] The centroid position of each cluster is calculated, and the distance d between adjacent beam / column clusters is measured, and the design spacing d r In comparison, if |d-d r |>δ t , it is marked as a structural anomaly;

[0032] The high residual area and the plane spacing anomaly area are spatially clustered in three-dimensional space to obtain the fracture zone or stress concentration zone, and the three-dimensional bounding box and confidence thereof are output.

[0033] In one or more embodiments of the present application, the steps of establishing a temporary three-dimensional space based on the point cloud of the surrounding environment and dividing multiple detection planes are as follows:

[0034] Select a cube or cylinder region based on speed and environment density dynamic adjustment with the navigation main body as the origin, clip all sensor power supplies into this region, and establish a temporary voxel representation;

[0035] Horizontal slicing, dividing several horizontal planes according to the height z0±Δ h , detecting the ground, half body and high obstacles;

[0036] Radial slicing, taking the orientation of the navigation main body as the center, and dividing several radial planes according to the angle θ1...θ k Establish vertical radial planes to cover the front sector;

[0037] Longitudinal slice to navigate the body towards the vertical plane established by x / y axis, and the navigation body is perpendicular to the longitudinal plane, detect the rollover or the top obstacle.

[0038] Each detection plane can be regarded as a two-dimensional obstacle detection window, which facilitates rapid projection and processing.

[0039] In one or more embodiments of the present application, the determination of the movable range is as follows:

[0040] Project the points in the temporary three-dimensional space near the detection plane into the two-dimensional coordinate system of the plane, perform Euclidean clustering on the projected points, separate different obstacle clusters, calculate the convex hull or alpha-hull for each cluster of points, extract the edge point array, and form a polygon contour;

[0041] Convert the edge contour points of each plane from the local plane coordinate system back to the three-dimensional coordinate system of the navigation body, and apply the current pose transformation matrix;

[0042] Use Kalman filtering to match the previous frame edge points in the sliding window with the current edge to offset noise and track dynamic obstacles;

[0043] Dilate all edge contours by the navigation body radius + safety margin to obtain the impassable area polygon, use plane subtraction operation to quickly obtain the linear or polygon segment of the free travel area, and superimpose the feasible areas of each plane in three dimensions to identify the complete safe path and potential dead angle.

[0044] In one or more embodiments of the present application, the safety path planning is:

[0045] Use DWA, RRT* or three-dimensional A* in three-dimensional feasible space to find short-term motion targets;

[0046] Combine local targets with calling global semantic skeleton or sampling viewpoint results to ensure planning continuity;

[0047] Convert the path point sequence into speed or turning angle instructions, issue and execute them through the MPC or PID controller, and continuously correct them in a closed loop.

[0048] In one or more embodiments of the present application, the local map is compressed to generate an event summary:

[0049] Take the current position of the navigation body as the root, construct an octree in the current active area, and propose a far zone grid. Only the increment of the last map state is recorded each frame: new nodes, modified nodes, and deleted nodes.

[0050] For the point cloud marked as "collapse risk area", "unstable structure", and "target search point", it is retained in high-precision mode.

[0051] Calculate the outer box or polygon boundary of the current octree active region as the covered area metadata;

[0052] Obtain the obstacle edge polygon and the search and rescue point three-dimensional coordinates, organize them into a structured event list, each event containing: category, three-dimensional coordinates, risk level, timestamp and confidence, and package them into JSON;

[0053] Trigger the report when the temporary network detects that the connection is available, and send high-level crisis events, followed by uploading regional boundaries and low-risk obstacles, and use a sliding window confirmation mechanism to avoid packet loss;

[0054] Based on the lightweight ACK / NACK or application layer retransmission strategy, ensure that the key summary information finally reaches the command center.

[0055] Through the above technical scheme, the application has the following beneficial effects:

[0056] 1. Local full-stack deployment makes navigation independent of remote communication, and can still complete perception, mapping and navigation closed loop when the link is broken. The tightly coupled VIO+LiDAR SLAM and dead reckoning degradation strategy ensures that positioning is still robust in various sensor failure conditions. The multi-plane dynamic obstacle avoidance model can respond to sudden situations such as gravel and falling rocks, dynamic obstacles, etc. in real time, improving task continuity.

[0057] 2. Hierarchical voxel / octree and local slicing scheme significantly reduces the size of point cloud and grid processing, ensuring millisecond-level mapping and planning feedback. Lightweight semantic segmentation and event filtering only process key categories with high precision, significantly reducing AI inference and storage overhead. Modular design facilitates parallel execution, efficient collaboration of each link, and realization of end-to-end low-latency closed loop.

[0058] 3. Octree+differential encoding only saves active areas and crisis point clouds, and reports summaries based on event-driven strategies, significantly reducing network load. Relay mesh distribution and breakpoint transmission mechanism enhance the reliable transmission of key information, avoid redundant data congestion, and do not need to deploy high-performance cloud in the field for long-term operation, extending the endurance of field or disaster area tasks.

[0059] 4. Semantic priority filtering and risk labeling enable rescue teams to quickly locate "passageways", "safe exits", "collapse risk areas" and trapped points, improving search efficiency. Abstract skeleton and multi-level obstacle coverage provide high explainability and safety boundaries for path planning, reducing personnel and equipment risks. Real-time holographic map and event summary provide reliable basis for command center decision-making, shortening disaster analysis and resource scheduling time.

[0060] Additional features and advantages of the present application will be set forth in the description that follows, and in part will be apparent from the description, or can be learned by practice of the application. The objectives and other advantages of the present application will be realized and attained by the structure particularly pointed out in the written description and claims hereof as well as the appended drawings. BRIEF DESCRIPTION OF DRAWINGS

[0061] Figure 1 Flow chart of the method of the present application;

[0062] Figure 2 Navigation main body top view section position map of the present application. DETAILED DESCRIPTION

[0063] Embodiments of the present application will be described below with reference to the accompanying drawings. For the purpose of explanation, numerous specific details will be set forth in the following description in order to provide a thorough understanding of the present application. It will be apparent, however, to one skilled in the art that the present application can be practiced without these specific details. In other instances, well-known structures and functions have not been described in order not to obscure the related description of the present application with unnecessary detail. Also, different embodiments can have different characteristics, and features of different embodiments can be interchanged.

[0064] Unless otherwise defined, all terms (including technical and scientific terms) used herein have the same meaning as commonly understood by one of ordinary skill in the art to which this application belongs. It will be further understood that terms, such as those defined in commonly used dictionaries, should be interpreted as having a meaning that is consistent with their meaning in the context of the relevant art and the present disclosure. Unless expressly defined in the specification, terms of approximation such as "substantially", "approximately" and the like are intended to have a meaning compatible with their normal use in the art, and not a critical meaning.

[0065] Reference Figure 1 As shown, the present application provides a method for embodied agent navigation and 3D reconstruction in complex environments, which can quickly plan and navigate the path and reconstruct 3D data when exploring the interior of the ruins.

[0066] The embodied agent navigation and 3D reconstruction method comprises the following steps:

[0067] Data acquisition matrix, deployed inside the navigation main body for collecting environmental data around the navigation main body, timestamping different sensor data and online calibration, eliminating visual motion blur and laser speckle interference through real-time filtering;

[0068] Local front-end SLAM and redundant perception, tightly coupled VIO+LiDAR SLAM: jointly optimize visual odometry and laser loop in the sliding window, reduce drift, automatically switch to IMU+wheel speed odometry dead reckoning mode when vision or LiDAR fails, and combine existing local map to suppress drift;

[0069] Fine-grained octree for close-range key areas (narrow gaps, dead corners), coarse voxels for distant areas, RANSAC for large planes (walls, beams, columns), abstract skeleton for navigation reference;

[0070] MobileNetV3-based segmentation on EdgeTPU / GPU, high-precision reconstruction of "passage" and "emergency exit", automatic labeling of "collapse risk area", "unstable structure", and "search and rescue point", and reporting of key events in subsequent planning and communication;

[0071] Dynamic obstacle avoidance decision model, temporary three-dimensional space based on surrounding environment point cloud, multiple detection planes in space, position correction of obstacle edge point array in detection plane, determination of movable range, and application of local planning algorithm to generate safe motion path in feasible space;

[0072] Octree + differential encoding for local map, only current active area and crisis point cloud are retained, and regional boundary, key obstacle, and target location data are uploaded when network is reachable.

[0073] In a practical way, in a communication-restricted and real-time-required ruin environment, the local front-end SLAM must have high robustness and adaptive degradation capability to ensure that the agent can maintain controllable positioning even when the key sensor fails.

[0074] Tightly coupled VIO + LiDAR SLAM

[0075] Data preprocessing:

[0076] Image, distortion removal, grayscale, and FAST / ORB feature extraction;

[0077] Point cloud, voxel filtering (VoxelGrid) down-sampling, ground removal (RANSAC ground fitting) to speed up matching;

[0078] IMU, pre-integration (IMU pre-integration) after gravity separation to provide priori for optimization.

[0079] Sliding window optimization:

[0080] State variables, including the poses of several key frames in the window, IMU bias, and partial visual feature depth;

[0081] Residual term:

[0082] Visual reprojection error - distance of image feature points to camera projection plane;

[0083] LiDAR scan matching error - ICP / NDT distance between current point cloud and local map (or sub-map);

[0084] IMU pre-integrated error - difference between pre-integrated displacement / pose and optimization result;

[0085] Solver: Incremental Gauss-Newton or Ceres Solver, real-time update window state and sliding.

[0086] Loop detection and global correction:

[0087] Feature clustering, extract Voxel-FIR, Scan Context, etc. global descriptors for LiDAR scans;

[0088] Scene matching, fast search for similar historical keyframes, trigger ICP fine registration;

[0089] Graph optimization, add loop constraints to the backend Pose Graph, call g2o / GT-SAM for global pose adjustment, suppress long-term drift.

[0090] Sensor degradation and dead reckoning mode

[0091] Failure detection:

[0092] Vision failure, consecutive multiple frames of feature tracking number below threshold, or re-projection residual increases dramatically;

[0093] LiDAR failure, laser point number decreases sharply, echo intensity is abnormal or ICP matching rate decreases;

[0094] State switching, based on residual monitor in sliding window, trigger degradation timeout threshold.

[0095] Dead reckoning switching:

[0096] IMU + wheel odometer fusion, IMU pre-integrated continuously outputs high-frequency short-time displacement, wheel speed calculates plane speed, both are fused by extended Kalman filter or unscented Kalman filter to generate real-time pose;

[0097] Local map correction, every fixed distance or time, use the recently constructed point cloud subgraph to do coarse ICP on the current laser scan, fine-tune the dead reckoning pose, and suppress the cumulative drift.

[0098] Redundant map matching:

[0099] Subgraph cache, save the local map of the point cloud of the last N keyframes;

[0100] Fast registration, use KD-Tree for initial correspondence, then perform point-to-plane ICP to quickly correct the dead reckoning result;

[0101] Adaptive weight, automatically adjust the fusion ratio of dead reckoning and SLAM output according to ICP residual, ensure the priority of using map constraints when local map is available.

[0102] In an embodiment, GPS PPS clock signal is mounted when each sensor in the data acquisition matrix is internally dotted, ensuring the timestamp accuracy of data generated by different devices. After the host board receives the original environmental data, the arrival time is recorded through the ring buffer, and the time delay jitter is compensated. The extrinsic parameters of camera-inertial navigation are optimized by combining feature observation and IMU pre-integration residual in the sliding window. During the SLAM startup phase, the extrinsic parameters are automatically estimated and periodically fine-tuned through online rigid matching.

[0103] In an implementable way, standardized slots are reserved inside the navigation main body (robot chassis or unmanned aerial vehicle body), and LiDAR, RGB-D / single camera, IMU, wheel speed encoder, etc. modules are installed. All sensors are connected to the host board through the same bus (CAN, EtherCAT, USB3).

[0104] In an embodiment, RANSAC is used to extract large planes from incremental point clouds, and these planes are connected to form a skeleton graph. The skeleton model retains the environment and filters out irrelevant small object noise, generating a simplified three-dimensional abstract representation. The last N frames or fixed radius point cloud slices are maintained, supporting online insertion and removal. The slices are quickly indexed through spatial hashing or KD-Tree, achieving millisecond-level updates. Each new data only updates the octree and skeleton corresponding to the slice, without the need to reconstruct the global model. The abstract skeleton model serves as the underlying topological skeleton of the navigation graph, which is used for fast path search.

[0105] In an implementable way, the local voxel map provides a feasible space interface for dynamic obstacle avoidance. For close-range key areas such as narrow passages and dead angles, fine-grained octrees are used for storage, and coarse voxels are used for fast downsampling for long-distance scenes. The hierarchical strategy ensures local details while significantly reducing global memory and computing overhead.

[0106] On resource-constrained edge devices, high-precision, real-time three-dimensional reconstruction and navigation support are completed at the lowest computing and storage cost. Among them, the perception, SLAM, reconstruction, and planning modules are deployed on the navigation main body, ensuring that it can still run autonomously when disconnected. In the process of deepening the ruins, ground relays are deployed to build a low-bandwidth mesh network, supporting multi-machine active and command point communication.

[0107] In an embodiment, the collapse risk area detection:

[0108] In the local point cloud slice, fixed compensation is used to grid division in the XY plane, and the height difference ΔZ of adjacent grids is calculated. If ΔZ>H t, marked as high mutation region;

[0109] For each folk song, the normal vector n is obtained by fitting a plane using RANSAC;

[0110] The angle θ between the normal vector and the direction of gravity is calculated. If θ > θ t , it is marked as a tilt risk region;

[0111] Use the "scattered rubble" "fragment" "tile" category mask in the semantic segmentation output;

[0112] Superimpose the local point cloud with the high mutation region and the tilt risk region, and increase the risk level of the intersection area;

[0113] Calculate the comprehensive risk score for the high mutation region, tilt risk region, and intersection area according to different weights;

[0114] Cluster high-risk adjacent units into risk blocks, output the polygon boundary and center position, and use it for subsequent path avoidance and reporting.

[0115] Where H t is the set height threshold, and θ t is the set tilt angle threshold.

[0116] In an embodiment, for the set threshold, the threshold is adjusted in different environments through an adaptive threshold strategy:

[0117] Update the mean μ and standard deviation σ of the height difference and the angle in the last N frames of point cloud in real time, and set the threshold to μ = k·σ, k usually takes between 1.5-2, and adaptively adjust with the noise level.

[0118] When detecting scene labels such as "indoor", "narrow passage", "open outdoor", etc., switch to the respective preset threshold.

[0119] In an embodiment, structure instability detection:

[0120] In a sliding window or slice range, fit the main plane by RANSAC;

[0121] Calculate the vertical distance residual of all points to the plane. If the residual variance σ 2 > σ t , then the region has cracks or fragments;

[0122] Based on the semantic segmentation result, extract the point cloud of "beam" and "column" categories and cluster them according to the connected domain;

[0123] Calculate the centroid position of each cluster, measure the distance d between adjacent beam / column clusters, and compare it with the design spacing d r Compare, if |d-d r |> δt Marked as structural anomaly;

[0124] Spatial clustering is performed on high residual regions and abnormal plane spacing regions in three-dimensional space to obtain fracture zones or stress concentration zones, and their three-dimensional bounding boxes and confidence scores are output.

[0125] σ t Set a threshold for the residual, δ t A threshold is set for the deviation of the spacing between adjacent beams and columns. The threshold setting is related to the height threshold H. t and tilt angle threshold θ t The same applies, and this application will not elaborate further here.

[0126] By detecting collapse risk zones and structural instability, structured risk zones and unstable zones can be marked based on local fine reconstruction, providing a reliable basis for navigation path planning, personnel avoidance, and command center decision-making.

[0127] See Figure 2 As shown, in one embodiment, the steps of establishing a temporary 3D space based on the surrounding environment point cloud and dividing it into multiple detection planes are as follows:

[0128] A cube or cylinder region is selected with the navigation subject as the origin, which is dynamically adjusted based on speed and environmental density. All sensor power supplies are clipped into this region to establish a temporary voxel representation.

[0129] Horizontal slices, according to height z0±Δ h Divide the area into several horizontal planes and detect obstacles on the ground, half-body, and at heights;

[0130] Radial slices, centered on the orientation of the navigation subject, at angles θ1...θ k Establish a vertical radial plane that covers the sector in front;

[0131] Vertical slices are used to establish a vertical plane along the x / y axes with the orientation of the navigation subject, and the orientation of the navigation subject is perpendicular to the vertical plane to detect rollover or obstacle jackknock.

[0132] Each detection plane can be viewed as a two-dimensional obstacle detection window, facilitating rapid projection and processing.

[0133] In one embodiment, the steps for determining the range of motion are as follows:

[0134] Points in the temporary 3D space that fall near the detection plane are projected onto the 2D coordinate system of the plane. Euclidean clustering is performed on the projected points to separate different obstacle clusters. The convex hull or α-hull is calculated for each cluster of points, and the edge point matrix is ​​extracted to form a polygonal outline.

[0135] Convert each planar edge contour point from the local planar coordinate system back to the navigation body three-dimensional coordinate system, apply the current pose transformation matrix;

[0136] Apply Kalman filter matching between the previous frame edge point and the current edge in the sliding window to offset noise and track dynamic obstacles;

[0137] Dilate all edge contours by the navigation body radius + safety margin to obtain the non-passable area polygon, use planar subtraction operation (full field minus dilated area) to quickly obtain the linear or polygonal segment of the free passable area, and superimpose each planar feasible area in three dimensions to identify the complete safe passage and potential dead angle.

[0138] In one embodiment, the safety path planning:

[0139] Use DWA, RRT* or three-dimensional A* in three-dimensional feasible space to find short-term motion targets;

[0140] Combine local targets with calling global semantic skeleton or sampling viewpoint results to ensure planning continuity;

[0141] Convert the path point sequence into speed or turning angle instructions, issue and execute through the MPC or PID controller, and continuously correct in a closed loop.

[0142] Construct a temporary three-dimensional space and multiple detection planes in a small range around the navigation body, limit the data volume, accelerate point cloud projection, edge extraction and feasible region calculation, and ensure that obstacle avoidance and path planning closed loop are completed in a very short time.

[0143] Combine horizontal, radial and longitudinal plane slices to simultaneously detect ground protrusions, lateral obstacles, high suspensions and other threats, and form a full-coverage safe passage after fusion of multiple plane results.

[0144] In one embodiment, the local map is compressed to generate an event summary:

[0145] Take the current position of the navigation body as the root, build an octree in the current active area, and propose a far area grid, only record the increment with the last map state: new nodes, modified nodes and deleted nodes;

[0146] For point clouds labeled as "collapse risk area", "unstable structure" and "target search and rescue point", keep them in high-precision mode;

[0147] Calculate the outer box or polygon boundary of the current octree active area as the covered area metadata;

[0148] Obtain the three-dimensional coordinates of the obstacle edge polygon and the search and rescue point, organize them into a structured event list, each event contains: category, three-dimensional coordinates, risk level, timestamp and confidence, and package them into JSON;

[0149] Triggering the report when the temporary network detects the connection available, limited sending high-level crisis events, followed by uploading regional boundaries and low-risk obstacles, using a sliding window confirmation mechanism to avoid packet loss;

[0150] Based on lightweight ACK / NACK or application layer retransmission strategy, ensure that the key summary information finally reaches the command center.

[0151] Although the present application is disclosed in connection with the above embodiments, it is not intended to limit the present application, and any skilled person can make various modifications and decorations without departing from the spirit and scope of the present application, therefore the protection scope of the present application should be defined by the appended claims.

Claims

1. A method for embodied intelligent agent navigation and 3D reconstruction in complex environments, comprising the following steps: Collecting environment data around the navigation agent using a data collection matrix deployed inside the navigation agent, timestamping and online calibrating different sensor data, and eliminating visual motion blur and laser speckle interference through real-time filtering; Local front-end SLAM and redundant perception, tightly coupled VIO+LiDAR SLAM: jointly optimizing visual odometry and laser loop closure in a sliding window to reduce drift, automatically switching to IMU+wheel speed odometry in dead reckoning mode when vision or LiDAR fails, and combining existing local maps to suppress drift; Using fine-grained octree for close-range key areas and coarse voxel for distant areas to accelerate, using RANSAC to extract walls and beam-column planes to construct an "abstract skeleton" of the environment for navigation reference; Running MobileNetV3-based segmentation on EdgeTPU / GPU to reconstruct "passageway" and "safe exit" categories with high precision, and automatically labeling "collapse risk area", "unstable structure", and "target search point"; Dynamic obstacle avoidance decision model, based on the surrounding environment point cloud to establish a temporary three-dimensional space, and divide multiple detection planes in the space, correct the position of obstacle edge points in the detection plane, determine the movable range, and apply local planning algorithm to generate a safe motion path in the feasible space; Octree + differential encoding of local maps, retaining only the current active area and crisis point cloud, uploading regional boundaries, key obstacles, and target location data when the network is reachable.

2. The method of claim 1, wherein, Mounting GPS PPS clock signal inside each sensor in the data collection matrix to ensure the timestamp accuracy of data generated by different devices, recording the arrival time through a ring buffer after the host board receives the original environment data, compensating for time delay jitter, and jointly optimizing the camera-inertial external parameters using feature observation and IMU pre-integral residual in the sliding window, automatically estimating and periodically fine-tuning the parameters during the SLAM startup phase through online rigid matching.

3. The method of claim 2, wherein, Using RANSAC to extract large planes from incremental point clouds, connecting these planes into a skeleton graph, retaining the environment skeleton model, filtering out irrelevant small object noise, generating a simplified three-dimensional abstract representation, maintaining the latest N frames or fixed radius point cloud slices, supporting online insertion and removal, and quickly indexing between slices through spatial hashing or KD-Tree to achieve millisecond-level updates, updating only the octree and skeleton corresponding to the slice with each new data, without reconstructing the global model, and using the abstract skeleton model as the underlying topological skeleton of the navigation graph.

4. The method of claim 3, wherein, Collapse risk area detection: In the local point cloud slice, the fixed compensation is used to make grid division in the XY plane, the height difference ΔZ of adjacent grids is calculated, and if ΔZ>H t , it is marked as a height mutation area; Fitting a plane using RANSAC for each point in the song to obtain the normal vector n; The angle θ between the calculated vector and the direction of gravity is determined, and if θ > θ t , the area is marked as a tilt risk zone; Using the "scattered rubble", "debris", and "tile" category masks in the semantic segmentation output; Overlaying local point clouds with height mutation areas and tilt risk areas, and weighting the intersection area to increase the risk level; Calculating the comprehensive risk score for height mutation areas, tilt risk areas, and intersection areas with different weights; Clustering high-risk adjacent units into risk blocks, outputting the polygon boundary and center position for subsequent path avoidance and reporting.

5. The method of claim 4, wherein, For the set threshold, the threshold in different environments is adjusted by an adaptive threshold strategy: Real-time update the mean μ and standard deviation σ of the height difference and the included angle in the last N frames of point cloud, and set the threshold as μ = kσ; When the scene labels of "indoor", "narrow passage", and "open outdoor" are detected, switch to the respective preset threshold.

6. The method of claim 5, wherein, Instability detection: In the sliding window or slice range, fit the main plane by RANSAC; Compute the perpendicular distance residual of all points to the plane, if the residual variance σ 2 >σ t then the region has a crack or a chip; Based on the semantic segmentation result, extract the point cloud of the "beam" and "column" categories and cluster them by connected domain; Calculate the centroid position for each cluster, measure the distance d between the centroids of adjacent beam / column clusters, compare to the design spacing d r In contrast, if |d-d r | > δ t Mark as structural anomaly; Do spatial clustering of high residual area and plane spacing anomaly area in three-dimensional space to get the fracture zone or stress concentration zone, and output its three-dimensional bounding box and confidence.

7. The method of claim 6, wherein, The steps of establishing a temporary three-dimensional space based on the surrounding environment point cloud and dividing multiple detection planes are as follows: Select a cube or cylinder region based on speed and environment density dynamic adjustment with the navigation body as the origin, clip all sensor power to this region, and establish a temporary voxel representation; Horizontal slice, by height z0±Δ h Divide several horizontal planes, detect ground, half-body and high obstacles; Radial slices, centered on the navigation body heading, at angles θ1...θ k Establish a perpendicular radial plane covering the forward sector; Longitudinal slicing, establish a vertical plane with the navigation body orientation as the x / y axis, and the navigation body orientation is perpendicular to the longitudinal plane, detect rollover or overhead obstacles.

8. The method of claim 7, wherein, The steps to determine the movable range are as follows: Project the points in the temporary three-dimensional space near the detection plane to the two-dimensional coordinate system of the plane, perform Euclidean clustering on the projected points, separate different obstacle clusters, calculate the convex hull or alpha hull for each cluster of points, extract the edge point array, and form a polygon contour; Convert the edge contour points of each plane from the local plane coordinate system to the navigation body three-dimensional coordinate system and apply the current pose transformation matrix; Use Kalman filtering to match the edge points of the previous frame in the sliding window with the current edge to cancel noise and track dynamic obstacles; Inflate all edge contours by the navigation body radius + safety margin to get the impassable area polygon, use plane subtraction operation to quickly get the linear or polygon segment of the free passage area, and superimpose the three-dimensional feasible areas to identify the complete safe passage and potential dead angle.

9. The method of claim 8, wherein, Safety path planning: Use DWA, RRT*, or three-dimensional A* in the three-dimensional feasible space to find short-term motion targets; Combine local targets with the results of calling global semantic skeleton or sampling view points to ensure planning continuity; Convert the path point sequence to speed or turning angle instructions, issue them for execution through the MPC or PID controller, and continuously correct them in a closed loop.

10. The method of claim 9, wherein, Local map compression generates event summary: Take the current position of the navigation body as the root to build an octree in the current active area, propose a far zone grid, and only record the increment of the last map state: new nodes, modified nodes, and deleted nodes; For point clouds labeled as "collapse risk area", "structural instability", and "target search and rescue points", keep them in high-precision mode; Calculate the bounding box or polygon boundary of the current octree active area as the covered area metadata; Get the three-dimensional coordinates of the obstacle edge polygon and the search and rescue point, and organize them into a structured event list, each event containing: category, three-dimensional coordinates, risk level, timestamp, and confidence, and package them as JSON; Triggered by the temporary network to report when the connection is available, limited to send high-level crisis events, followed by the upload area boundaries and low-risk obstacles, using a sliding window confirmation mechanism to avoid packet loss; Based on lightweight ACK / NACK or application layer retransmission strategy to ensure that the key summary information eventually reaches the command center.

Images