A robot autonomous exploration navigation method and system fusing visual enhanced positioning and multi-modal terrain analysis

Abstract

This invention provides a method and system for autonomous exploration and navigation of robots that integrates visually enhanced localization and multimodal terrain analysis. The method includes: acquiring multi-source heterogeneous data of the target robot in the current dynamic environment; performing spatiotemporal calibration and standardization processing on laser point cloud data, visual image data, and inertial measurement data to obtain standardized point cloud data and standardized image data; performing fusion localization and dual-map construction based on the standardized point cloud data and standardized image data to obtain a jointly estimated pose, an environmental point cloud map, and a static semantic map; performing terrain analysis and path planning based on the jointly estimated pose, environmental point cloud map, and static semantic map to obtain the target motion path; and driving the target robot to perform exploration and navigation actions along the planned path based on the target motion path. This invention enables stable localization, accurate mapping, and intelligent autonomous exploration and navigation of robots in complex, dynamic, and unstructured environments.

Description

Technical Field

[0001] This invention relates to the field of artificial intelligence technology, and in particular to a robot autonomous exploration and navigation method and system that integrates visual augmented localization and multimodal terrain analysis. Background Technology

[0002] As mobile robots are increasingly used in various complex, dynamic, and unstructured on-site operation scenarios such as construction inspection and emergency search and rescue, the requirements for robots to achieve stable positioning, accurate mapping, and intelligent autonomous exploration and navigation in unknown or frequently changing environments are also constantly increasing.

[0003] Existing autonomous navigation solutions for mobile robots typically rely on single or combined perception methods, such as LiDAR and visual sensors, to achieve environmental perception and localization mapping. However, in complex, dynamic, and unstructured environments, navigation relying solely on LiDAR can only acquire geometric contour information, failing to identify objects with specific physical properties. It also cannot provide semantic environmental references for robot path planning and is prone to localization degradation and drift in scenarios lacking geometric features. Furthermore, interference in dynamic environments can lead to false features in the mapping, affecting localization and mapping accuracy. Conversely, navigation relying solely on visual sensors is highly sensitive to changes in lighting and weakly textured environments (such as white walls or nighttime construction areas), resulting in insufficient geometric accuracy and susceptibility to localization drift and map instability. The problem of low matching with the actual environment, and the high computational cost of visual semantic analysis places a huge burden on embedded platforms with limited computing power, making it difficult to balance real-time performance and mapping quality; in addition, navigation methods that integrate laser point clouds and visual recognition lack the ability to comprehensively analyze geometric features and material semantics in unstructured working environments, resulting in insufficient accuracy in terrain passability assessment, which can easily lead robots to enter dangerous terrain. Furthermore, they cannot achieve collaborative construction of point cloud maps and semantic maps, making it difficult to form accurate environmental cognition. This makes the planned path lack safety and rationality, ultimately making it difficult to achieve intelligent autonomous exploration and navigation of robots. Summary of the Invention

[0004] This invention provides a robot autonomous exploration and navigation method and system that integrates visual augmented positioning and multimodal terrain analysis to solve the problems of degradation and drift in single laser navigation positioning, high computational burden and low positioning accuracy in single visual navigation, and insufficient terrain passability assessment and inaccurate environmental cognition in fused navigation. It enables robots to achieve stable positioning, accurate mapping and intelligent autonomous exploration and navigation in complex, dynamic and unstructured environments.

[0005] In a first aspect, the present invention provides a robot autonomous exploration and navigation method that integrates visually enhanced localization and multimodal terrain analysis, comprising: Acquire multi-source heterogeneous data of the target robot in the current dynamic environment; the multi-source heterogeneous data includes laser point cloud data, visual image data, and inertial measurement data; Spatiotemporal calibration and standardization are performed on the laser point cloud data, the visual image data, and the inertial measurement data to obtain standardized point cloud data and standardized image data. Based on the standardized point cloud data and standardized image data, fusion localization and dual map construction are performed to obtain joint estimated pose, environmental point cloud map and static semantic map; Based on the joint estimated pose, the environmental point cloud map, and the static semantic map, terrain analysis and path planning are performed to obtain the target motion path, and the target robot is driven to perform exploration and navigation actions along the planned path based on the target motion path.

[0006] In a second aspect, the present invention also provides a robot autonomous exploration and navigation system integrating visual augmented localization and multimodal terrain analysis, applied to the robot autonomous exploration and navigation method integrating visual augmented localization and multimodal terrain analysis as described in the first aspect; the robot autonomous exploration and navigation system integrating visual augmented localization and multimodal terrain analysis includes: The data acquisition module is used to acquire multi-source heterogeneous data of the target robot in the current dynamic environment; the multi-source heterogeneous data includes laser point cloud data, visual image data and inertial measurement data; The spatiotemporal calibration and processing module is used to perform spatiotemporal calibration and standardization processing based on the laser point cloud data, the visual image data and the inertial measurement data to obtain standardized point cloud data and standardized image data. The localization and map building module is used to perform fusion localization and dual map construction based on the standardized point cloud data and standardized image data to obtain joint estimated pose, environmental point cloud map and static semantic map. The terrain analysis and exploration decision module is used to perform terrain analysis and path planning based on the joint estimated pose, the environmental point cloud map and the static semantic map, to obtain the target motion path, and to drive the target robot to perform exploration and navigation actions along the planned path based on the target motion path.

[0007] Thirdly, the present invention also provides an electronic device, comprising: a memory for storing computer software programs; and a processor for reading and executing the computer software programs, thereby realizing the robot autonomous exploration and navigation method that integrates visual augmented positioning and multimodal terrain analysis as described above.

[0008] Fourthly, the present invention also provides a non-transitory computer-readable storage medium storing a computer software program, which, when executed by a processor, implements the robot autonomous exploration and navigation method as described above, which integrates visual augmented localization and multimodal terrain analysis.

[0009] Fifthly, the present invention also provides a computer program product, including a computer program that, when executed by a processor, implements the robot autonomous exploration and navigation method as described above, which integrates visual augmented localization and multimodal terrain analysis.

[0010] The robot autonomous exploration and navigation method integrating visual augmented localization and multimodal terrain analysis provided in this invention obtains multi-dimensional environmental perception raw data based on multi-source heterogeneous data, including laser point cloud data, visual image data, and inertial measurement data of the target robot in the current dynamic environment. This multi-source heterogeneous data is then spatiotemporally calibrated and standardized to obtain standardized point cloud data and standardized image data, achieving precise matching between laser and visual data. Based on the standardized point cloud data and standardized image data, fusion localization and dual-map construction are performed to obtain a joint estimated pose, an environmental point cloud map, and a static semantic map. This method utilizes the geometric accuracy of the laser point cloud and the semantic information of the visual image. This fusion approach overcomes the shortcomings of single-laser navigation (lacking semantic perception) and single-vision navigation (insufficient geometric accuracy). Furthermore, the construction of dual maps enables a dual representation of environmental geometric and semantic features, providing accurate environmental cognition for terrain analysis and path planning. Finally, terrain analysis and path planning are performed based on jointly estimated pose, environmental point cloud map, and static semantic map to obtain the target motion path. This path then drives the target robot to perform exploratory navigation actions, achieving a comprehensive assessment of terrain passability by combining geometric and semantic features. This avoids the safety hazards associated with relying solely on geometric information to evaluate terrain, and plans a motion path that is both reasonable and safe. In summary, this invention solves the problems of degradation and drift in single-laser navigation, high computational burden and low positioning accuracy in single-vision navigation, and insufficient terrain passability assessment and inaccurate environmental cognition in fused navigation. It enables robots to achieve stable positioning, accurate mapping, and intelligent autonomous exploration and navigation in complex, dynamic, and unstructured environments. Attached Figure Description

[0011] Figure 1 This is a flowchart illustrating the robot autonomous exploration and navigation method that integrates visual augmented localization and multimodal terrain analysis provided in an embodiment of the present invention. Figure 2 This is a schematic diagram of the structure of the robot autonomous exploration and navigation system that integrates visual augmentation localization and multimodal terrain analysis provided in an embodiment of the present invention; Figure 3An embodiment diagram of the electronic device provided in this invention; Figure 4 An embodiment diagram of a computer-readable storage medium provided in accordance with the present invention. Detailed Implementation

[0012] The technical solutions of the embodiments of the present invention will be clearly and completely described below with reference to the accompanying drawings. Obviously, the described embodiments are only some embodiments of the present invention, and not all embodiments. Based on the embodiments of the present invention, all other embodiments obtained by those skilled in the art without creative effort are within the scope of protection of the present invention.

[0013] In the description of this invention, the terms "first" and "second" are used for descriptive purposes only and should not be construed as indicating or implying relative importance or implicitly specifying the number of indicated technical features. Thus, a feature defined as "first" or "second" may explicitly or implicitly include one or more of the stated features. In the description of this invention, "a plurality of" means two or more, unless otherwise explicitly specified.

[0014] See Figure 1 , Figure 1 This is a flowchart illustrating the robot autonomous exploration and navigation method integrating visual augmented localization and multimodal terrain analysis provided by the present invention. In this embodiment, the executing entity of the robot autonomous exploration and navigation method integrating visual augmented localization and multimodal terrain analysis is an autonomous exploration and navigation system. Therefore, the robot autonomous exploration and navigation method integrating visual augmented localization and multimodal terrain analysis includes: Step 10: Acquire multi-source heterogeneous data of the target robot in the current dynamic environment; the multi-source heterogeneous data includes laser point cloud data, visual image data and inertial measurement data.

[0015] Optionally, during the autonomous exploration process of the target robot in a dynamic, unstructured work environment such as construction inspection or emergency search and rescue, the autonomous exploration navigation system continuously collects multi-source heterogeneous data of the target robot's current environment, including laser point cloud data, visual image data, and inertial measurement data. Specifically, for laser point cloud data, the autonomous exploration and navigation system uses the lidar sensor on the target robot to emit laser beams into the current environment and receive reflected signals. Based on information such as the laser propagation time and angle, it calculates the relative distance and spatial position of each object in the environment to the lidar sensor, thereby generating laser point cloud data containing spatial information of the environment's geometric contours. For visual image data, the autonomous exploration and navigation system uses the visual camera sensor on the target robot to continuously acquire images of the current environment at a preset sampling frame rate, generating visual image data containing environmental visual texture, object appearance, color, and semantic features. This visual image data is in the form of a two-dimensional pixel matrix, used to reflect the visual attributes and semantic information of objects in the environment. For inertial measurement data, the autonomous exploration and navigation system uses the inertial measurement unit (including angular velocity sensor and accelerometer) on the target robot to detect the target robot's own motion state in real time, collecting inertial measurement data containing angular velocity data and acceleration data. The angular velocity data is used to reflect the target robot's rotational motion state in three-dimensional space, and the acceleration data is used to reflect the target robot's translational motion state in three-dimensional space.

[0016] Step 20: Perform spatiotemporal calibration and standardization processing based on laser point cloud data, visual image data, and inertial measurement data to obtain standardized point cloud data and standardized image data.

[0017] Optionally, the autonomous exploration navigation system first performs time synchronization calibration based on laser point cloud data, visual image data, and inertial measurement data to eliminate the deviation of data acquisition time from different sensors. Then, it uses inertial measurement data to compensate for motion distortion of the laser point cloud data, correcting the point cloud distortion caused by the target robot's motion. Subsequently, it completes the unified transformation of the coordinate systems of each sensor, mapping the laser point cloud data and visual image data to the same global coordinate system to obtain standardized point cloud data and standardized image data, as detailed in steps 201 to 204.

[0018] Step 30: Based on standardized point cloud data and standardized image data, perform fusion localization and dual map construction to obtain joint estimated pose, environmental point cloud map and static semantic map.

[0019] Optionally, the autonomous exploration navigation system performs positioning and pose processing based on standardized point cloud data and standardized image data to determine the laser positioning pose estimate of the target robot at the current moment. At the same time, it completes the construction of environmental point cloud map and static semantic map. Then, based on the laser positioning pose estimate, it performs joint optimization of robot pose by combining the constructed environmental point cloud map and static semantic map, and finally obtains the joint estimated pose, as shown in steps 301 to 305.

[0020] Step 40: Based on the joint estimated pose, environmental point cloud map and static semantic map, perform terrain analysis and path planning to obtain the target motion path, and drive the target robot to perform exploration and navigation actions along the planned path based on the target motion path.

[0021] Optionally, the autonomous exploration navigation system determines the real-time spatial position of the target robot in the environment based on joint pose estimation. Combining the environmental point cloud map and static semantic map, it completes terrain analysis and front-end point detection and extraction. First, it performs global path planning, and then performs targeted path planning based on the results of global path planning and the preset dynamic obstacle avoidance strategy for the target robot to perceive local obstacles in real time. Under the premise of meeting the requirements of terrain passability, obstacle avoidance and task requirements, the optimal target motion path is planned, as shown in steps 401 to 406.

[0022] Optionally, the autonomous exploration and navigation system performs kinematic analysis based on the optimal target motion path, converting the path information into motion control commands for the target robot. These commands include specific motion parameters such as travel direction, speed, and turning angle, and are matched with the target robot's motion structure to adapt to different types of target robots, such as wheeled, legged, and wheel-legged hybrid robots. The motion control commands are then transmitted in real-time to the target robot's motion execution unit via its built-in communication module. Based on the received commands, the target robot's motion execution unit drives its own power mechanism to complete the corresponding motion actions, enabling the target robot to continuously perform autonomous exploration and navigation actions along the planned target motion path. During the target robot's exploration and navigation process, the autonomous exploration and navigation system continuously collects new multi-source heterogeneous data in real-time, repeating steps 10 to 40 to achieve real-time positioning correction, incremental map updates, and dynamic path replanning for the target robot, ensuring the continuity and safety of the target robot's exploration and navigation in dynamic, unstructured environments.

[0023] The embodiments of the present invention solve the problems of degradation and drift in single laser navigation positioning, high computing power and low positioning accuracy in single vision navigation, and insufficient terrain passability assessment and inaccurate environmental cognition in fusion navigation. They enable robots to achieve stable positioning, accurate mapping and intelligent autonomous exploration navigation in complex dynamic unstructured environments.

[0024] Optionally, the processes of steps 201 to 204 include: Step 201: Interpolate and align the laser point cloud data, visual image data, and inertial measurement data based on their timestamp information to obtain the initial laser point cloud data, initial visual image data, and initial inertial measurement data after time synchronization.

[0025] Optionally, the autonomous exploration and navigation system extracts the timestamp information carried by the laser point cloud data, visual image data, and inertial measurement data, using a preset unified time benchmark as a reference. This unified time benchmark is the only time reference standard set by the autonomous exploration and navigation system for the synchronization of multi-source heterogeneous data, with no time deviation and a constant time progression rate. The timestamps of the laser point cloud data, visual image data, and inertial measurement data are matched one by one to identify the time deviation of the data acquisition time of different sensors. For data with time deviation, a linear interpolation algorithm is used to complete and calibrate the time dimension. The linear interpolation algorithm is an algorithm that calculates the corresponding data value at any time between adjacent valid data based on the valid data at the known acquisition time, to ensure that the interpolated data is continuous and without discontinuities in the time dimension. Based on this, for laser point cloud data, the missing point cloud coordinate information of time nodes is supplemented according to the interpolation algorithm; for visual image data, the precise acquisition time of each image frame under a unified time reference is determined according to the interpolation algorithm to ensure the continuity of the time sequence of the image frames; for inertial measurement data, the gaps in the time dimension of the high-frequency acquired angular velocity data and acceleration data are filled in by the interpolation algorithm, so that the acquisition time of the three types of data is aligned to the same time node of the unified time reference, and finally the initial laser point cloud data, initial visual image data and initial inertial measurement data after time synchronization are obtained.

[0026] Step 202: Based on the angular velocity and acceleration data in the initial inertial measurement data, calculate to obtain the instantaneous motion state data of the robot within a single scanning cycle of the lidar. Then, based on the instantaneous motion state data of the robot, perform reverse displacement correction on the acquisition time position of each laser point in the initial laser point cloud data to obtain the laser point cloud compensation data after motion distortion compensation.

[0027] Optionally, the autonomous exploration and navigation system extracts angular velocity and acceleration data from the initial inertial measurement data. Angular velocity data reflects the rotational speed of the target robot around the X, Y, and Z axes in three-dimensional space, while acceleration data reflects the translational acceleration of the target robot along the X, Y, and Z axes in three-dimensional space. A pre-integration algorithm is used to calculate the extracted angular velocity and acceleration data. This pre-integration algorithm performs integration calculations on the high-frequency acquired inertial measurement data and does not rely on the real-time pose of the target robot. The calculation yields the instantaneous motion state data of the target robot at each minute time point within a single LiDAR scan cycle. A single LiDAR scan cycle is the time it takes for the LiDAR to complete one omnidirectional laser emission, receive the reflected signal, and generate one frame of laser point cloud data. The instantaneous motion state data includes the target robot's instantaneous position, instantaneous velocity, and instantaneous rotation angle, accurately reflecting the real-time motion state of the target robot during the LiDAR scan.

[0028] Optionally, since the target robot moves during the lidar scanning process, the position of each laser point in the initial lidar point cloud data will shift due to the robot's movement, forming point cloud trailing. Based on this, the autonomous exploration and navigation system determines the actual displacement and rotation angle of the target robot at the time of acquisition of each laser point based on the obtained instantaneous motion state data of the robot. In the opposite direction to the robot's movement, it performs reverse displacement compensation and angle correction on the original acquisition position of each laser point. During the correction process, it strictly matches the acquisition time of each laser point with the corresponding instantaneous motion state of the robot to ensure that the position of each laser point is restored to the true spatial position during lidar scanning. Finally, it obtains the laser point cloud compensation data after motion distortion compensation, which eliminates the point cloud distortion problem caused by the target robot's movement.

[0029] Step 203: Based on the lidar coordinate system where the laser point cloud compensation data is located and the camera coordinate system where the initial visual image data is located, and combined with the preset sensor extrinsic parameter matrix, construct a coordinate transformation model, and project the laser point cloud compensation data and the initial visual image data onto the preset global unified reference coordinate system based on the coordinate transformation model, to obtain unified laser point cloud data and unified visual image data under the unified coordinate system.

[0030] Optionally, the autonomous exploration and navigation system determines the coordinate system to which the laser point cloud compensation data belongs is the lidar coordinate system. This lidar coordinate system is a three-dimensional spatial coordinate system established according to the right-hand coordinate system rule, with the center of the lidar as the origin and the laser emission direction of the lidar as a preset axis. It serves as the original coordinate reference for the lidar-acquired point cloud data. The initial visual image data is then determined to belong to the camera coordinate system. This camera coordinate system is a three-dimensional spatial coordinate system established according to the right-hand coordinate system rule, with the optical center of the visual camera as the origin and the optical axis of the camera as a preset axis. It serves as the original coordinate reference for the visual camera-acquired image data. Simultaneously, preset sensor extrinsic parameter matrices and camera intrinsic parameter matrices are retrieved. The sensor extrinsic parameter matrix... , The matrix K is obtained through joint calibration of the sensors and is used to characterize the spatial position transformation relationship between the lidar coordinate system and the camera coordinate system; the camera intrinsic parameter matrix K is the inherent parameter matrix obtained by factory calibration or subsequent joint calibration of the vision camera and is used to characterize the transformation relationship between three-dimensional spatial points in the camera coordinate system and two-dimensional pixel points in the image pixel coordinate system.

[0031] Optionally, the autonomous exploration navigation system uses the lidar coordinate system and camera coordinate system as its spatial basis, and the sensor extrinsic matrix and camera intrinsic matrix as its core transformation parameters to construct a coordinate transformation model. Specifically, with the camera intrinsic matrix as K, the rotation matrix for transforming the lidar coordinate system to the camera coordinate system as R, the translation vector as t, and any 3D point in the lidar coordinate system in the lidar point cloud compensation data as P, its Z-axis coordinate value in the camera coordinate system is... The coordinates of the three-dimensional point projected onto the two-dimensional pixel coordinate system of the image are: For any 3D point in the laser point cloud Its projection onto image pixel coordinates The coordinate transformation model is as follows: ;in, For the camera intrinsic parameter matrix, The extrinsic rotation matrix for the lidar to the camera; Let be the translation vector from the lidar to the camera. This is the Z-axis coordinate value in the camera coordinate system, i.e., the camera depth value.

[0032] Optionally, after the autonomous exploration navigation system determines the coordinate transformation model, it inputs the coordinate values ​​of each laser point in the laser point cloud compensation data into the coordinate transformation model, and completes the transformation from the lidar coordinate system to the global unified reference coordinate system according to the model rules, thus obtaining unified laser point cloud data under the global unified reference coordinate system; and inputs the spatial coordinate values ​​of each pixel in the initial visual image data into the coordinate transformation model, and completes the transformation from the camera coordinate system to the global unified reference coordinate system according to the model rules, thus obtaining unified visual image data under the global unified reference coordinate system, and finally obtaining unified laser point cloud data and unified visual image data under the unified coordinate system.

[0033] Step 204: Based on the unified laser point cloud data and the unified visual image data, the corresponding timestamps and spatial metadata are associated and then encapsulated to obtain standardized point cloud data and standardized image data.

[0034] Optionally, the autonomous exploration navigation system extracts the timestamp information corresponding to the unified laser point cloud data and the unified visual image data, and simultaneously extracts the spatial metadata of both types of data. This spatial metadata describes the spatial attributes of the data in a globally unified reference coordinate system. For the unified laser point cloud data, the spatial metadata includes the three-dimensional coordinate range of the point cloud, the point cloud density, and the number of effective point clouds. For the unified visual image data, the spatial metadata includes the spatial coordinate range corresponding to the image pixels, the image's field of view angle, and the spatial mapping relationship between the image and the environment. The unified laser point cloud data is associated and bound with the corresponding timestamp information and spatial metadata, so that each set of laser point cloud data has a clear description of the acquisition time and spatial attributes. At the same time, the unified visual image data is associated and bound with the corresponding timestamp information and spatial metadata, so that each frame of visual image data has a clear description of the acquisition time and spatial attributes. According to the standardized data format preset by the autonomous exploration navigation system, the associated and bound unified laser point cloud data and unified visual image data are encapsulated separately. After encapsulation, standardized point cloud data and standardized image data are obtained. The preset standardized data format is a unified data storage and transmission format set by the autonomous exploration and navigation system. It includes a fixed structure of data header, data body, and data tail. The data header stores timestamps and spatial metadata, the data body stores core point cloud or image data, and the data tail stores data verification information to ensure the compatibility and accuracy of data during transmission and processing within the system.

[0035] The embodiments of the present invention solve the problem of difficulty in fusing multi-source heterogeneous sensor data due to differences in acquisition mechanisms, coordinate systems, and data formats, and realize deep fusion and standardized processing of data from three types of sensors: lidar, vision camera, and inertial measurement unit.

[0036] Optionally, the processes of steps 301 to 305 include: Step 301: Based on standardized image data and a pre-trained lightweight semantic segmentation neural network, perform dynamic and static recognition to obtain recognition results, and determine the pixel-level semantic mask corresponding to the dynamic object and the image static label corresponding to the static object based on the recognition results.

[0037] Optionally, the autonomous navigation system retrieves a pre-trained lightweight semantic segmentation neural network and inputs standardized image data into it. This network performs pixel-by-pixel feature extraction and classification on the standardized image data. Feature extraction refers to the process of identifying and extracting features such as texture, color, contour, and spatial relationships for each pixel in the image. Classification involves matching the extracted pixel features with the dynamic and static target features trained in the neural network to determine the target type of each pixel. The final output is the dynamic and static recognition result of the image, which includes pixel-level classification data containing the dynamic and static attributes and target category information of each pixel in the image. This pre-trained lightweight semantic segmentation neural network is a neural network model trained on massive amounts of dynamic and static environmental image samples, with simplified model structure and optimized parameters for embedded computing-limited platforms. It features high computational efficiency, low resource consumption, and high recognition accuracy, thus enabling rapid dynamic and static classification and pixel-level segmentation of targets in images.

[0038] Optionally, the autonomous exploration and navigation system selects a set of pixels representing dynamic objects from the recognition results, outlines and marks the regions of this set of pixels according to their spatial positions in the image, and generates a pixel-level semantic mask corresponding to the dynamic object. Here, a dynamic object refers to an object in motion in the working environment, including pedestrians, construction vehicles, and moving construction machinery, which are targets that can cause dynamic interference to positioning and mapping. The pixel-level semantic mask is a binary mask image that perfectly matches the pixel size of the standardized image data and can accurately mark the pixel position of the dynamic object in the image. The masked area is the pixel of the dynamic object, and the non-masked area is the pixel of the non-dynamic object. Meanwhile, the autonomous exploration and navigation system filters out the pixel set representing static objects from the recognition results. Static objects refer to objects that are stationary in the working environment and have significant identification attributes, including static landmarks such as safety exit signs, door signs, construction area warning signs, and equipment signs that can be used as positioning references. The system then performs target detection and feature extraction on these static objects to determine their pixel positions, contour features, and semantic category information in the image, forming image static identifiers corresponding to the static objects. These image static identifiers are structured data containing the pixel positions, visual features, and semantic attributes of static landmarks.

[0039] Step 302: Based on standardized point cloud data and pixel-level semantic masking, dynamic target removal is performed to obtain pure static laser point cloud data without dynamic interference.

[0040] Optionally, the autonomous exploration navigation system uses standardized point cloud data combined with pixel-level semantic masks to accurately identify and remove point cloud data corresponding to the pixel regions of dynamic objects in the standardized point cloud data, thereby obtaining pure static laser point cloud data that eliminates all dynamic interference and can truly reflect the static geometric structure of the environment, as described in steps 3021 to 3024.

[0041] Step 303: Perform inter-frame registration and pose calculation based on the clean static laser point cloud data to obtain the laser positioning pose estimate of the target robot at the current moment. Then, based on the laser positioning pose estimate, convert the clean static laser point cloud data to the global coordinate system for cumulative stitching to obtain an environmental point cloud map that reflects the geometric structure of the environment.

[0042] Optionally, the autonomous exploration and navigation system retrieves continuous frames of clean, static laser point cloud data. This refers to multiple frames of laser point cloud data collected during the continuous movement of the target robot and after dynamic target removal, with each frame corresponding to a moment in the robot's motion. Inter-frame registration is then performed on the continuous frames of clean, static laser point cloud data. This involves using an iterative nearest-point algorithm to match features and spatially align adjacent frames of point cloud data. This algorithm iteratively optimizes the process by repeatedly finding the nearest point pairs between two frames, calculating the spatial transformation relationship between the point clouds, and continuously optimizing the algorithm to achieve optimal overlap between the two frames. Inter-frame registration eliminates spatial positional deviations in adjacent frames caused by robot motion, achieving precise matching of continuous frame point clouds. Subsequently, based on the inter-frame registration, the pose is calculated based on the spatial transformation relationship of the point cloud obtained by registration. That is, according to the spatial translation and rotation parameters of the point cloud, the spatial pose of the target robot at the corresponding moment relative to the initial position or the position of the previous moment is solved, and the laser positioning pose estimate of the target robot at the current moment is obtained. This laser positioning pose estimate is a six-degree-of-freedom pose data containing the three-dimensional spatial position coordinates and three-dimensional rotation angles of the target robot, which is used to accurately characterize the spatial attitude and position of the target robot in the lidar coordinate system at the current moment.

[0043] Optionally, the autonomous exploration and navigation system retrieves preset global coordinate system parameters. This global coordinate system is a fixed three-dimensional spatial coordinate system set by the autonomous exploration and navigation system for environmental perception and positioning mapping. Each frame of clean static laser point cloud data undergoes spatial coordinate transformation based on the corresponding laser positioning pose estimation value, that is, the point cloud data in the lidar coordinate system is transformed to the global coordinate system, completing the coordinate normalization of a single frame of point cloud. For multiple frames of clean static laser point cloud data after transformation to the global coordinate system, the point cloud data in the global coordinate system are superimposed and fused according to their spatial positions. The point clouds in overlapping areas are deduplicated and optimized, while the point clouds in non-overlapping areas are supplemented and expanded. This achieves the gradual construction and improvement of the geometric structure of the working environment, ultimately obtaining an environmental point cloud map that reflects the complete geometric structure of the environment. This environmental point cloud map is a three-dimensional point cloud collection in the global coordinate system, capable of accurately restoring the geometric features of the working environment, such as terrain undulations, object outlines, and spatial dimensions, without dynamic interference or redundant point clouds.

[0044] Step 304: Based on the static image labeling combined with the clean static laser point cloud data, construct a static semantic map containing semantic attributes.

[0045] Optionally, the autonomous exploration navigation system constructs a static semantic map containing semantic attributes based on image static identifiers and clean static laser point cloud data, so as to realize real-time updates of the static semantic map under the current key frame conditions, as in steps 3041 to 3046.

[0046] Step 305: The laser geometric constraint term constructed based on the laser positioning pose estimation value is combined with the absolute position constraint term constructed based on the static semantic map to jointly optimize the robot pose and obtain the joint estimated pose.

[0047] Optionally, the autonomous exploration and navigation system constructs a laser geometric constraint term based on the laser positioning pose estimation value. This involves using the laser positioning pose estimation values ​​from consecutive frames as a basis, combined with the point cloud spatial transformation relationship obtained from inter-frame registration, to construct pose constraints that reflect the range of pose estimation deviations obtained by the lidar based on environmental geometric features. Simultaneously, an absolute position constraint term is constructed based on a static semantic map. This absolute position constraint term uses 3D semantic landmarks in the static semantic map as absolute position references and is solved using a perspective n-point algorithm. The perspective n-point algorithm solves for the target robot's current pose by finding the correspondence between 2D static markers in the image and 3D semantic landmarks in the global coordinate system. Specifically, first, 2D pixel feature points of the static markers in the image are extracted, then the corresponding 3D semantic landmark spatial feature points in the static semantic map are matched. The perspective n-point algorithm is used to obtain the absolute pose estimation value of the target robot with reference to the 3D semantic landmarks. Based on this absolute pose estimation value, an absolute position constraint term is constructed, providing a globally unified absolute position reference for the target robot's pose, thereby effectively suppressing the cumulative drift of the laser positioning pose.

[0048] Optionally, the autonomous exploration and navigation system constructs a joint cost function based on laser geometric constraints and absolute position constraints to perform global joint optimization of the target robot's pose. This joint cost function is: ;in, The pose transformation matrix of the target robot in the global coordinate system is the core solution objective of the joint optimization. The total error of the laser geometric constraint term represents the degree of deviation between the laser positioning pose and the geometric constraint. , where is the weighting coefficient, a constant preset according to the characteristics of the working environment, used to balance the weights of laser geometric constraints and absolute position constraints; The number of static semantic landmark feature points participating in the matching; The coordinates of the two-dimensional pixel feature points statically labeled in the i-th image; This is a camera perspective projection function used to project points in three-dimensional space onto the pixel plane of a two-dimensional image; is the camera intrinsic parameter matrix, and is the intrinsic parameter matrix of the visual camera, representing the transformation relationship between the camera coordinate system and the image pixel coordinate system; This is the inverse pose transformation matrix from the camera coordinate system to the robot body coordinate system; This is the inverse pose transformation matrix from the global coordinate system to the robot body coordinate system; Let be the spatial coordinates of the i-th 3D semantic landmark in the global coordinate system. Then, the Gauss-Newton algorithm is used to iteratively solve the joint cost function. This Gauss-Newton algorithm is an optimization algorithm that continuously corrects the pose transformation matrix to be optimized by repeatedly calculating the Jacobian matrix and Hessian matrix of the cost function, so that the value of the joint cost function reaches its minimum. When the joint cost function reaches its minimum value, the corresponding pose transformation matrix is... The optimized target robot pose is then converted into six-degree-of-freedom pose data containing three-dimensional spatial position and three-dimensional rotation angle, resulting in a joint estimated pose. This joint estimated pose eliminates the cumulative drift and local positioning error of laser positioning.

[0049] This invention addresses the problems of semantic gaps and easy drift in single laser navigation, low geometric accuracy and high computational burden in single visual navigation, and insufficient positioning accuracy and incomplete environmental cognition in traditional fusion navigation. By jointly estimating pose, it provides stable global high-precision positioning for the robot and achieves dual accurate representation of environmental geometric and semantic features through dual maps, providing a comprehensive, accurate and reliable environmental cognition basis for subsequent terrain analysis and path planning.

[0050] Optionally, the process of steps 3021 to 3024 includes: Step 3021: Based on the three-dimensional coordinates of each laser point in the standardized point cloud data, project them onto the pixel coordinate system of the standardized image data to obtain the projected pixel coordinates of each laser point.

[0051] Optionally, the autonomous navigation system extracts the three-dimensional coordinates of each laser point in the standardized point cloud data, i.e., the spatial coordinates under a globally unified reference coordinate system. Simultaneously, it retrieves the coordinate transformation model constructed in step 203, sequentially substituting the three-dimensional coordinates of each laser point into the model. Following the projection operation logic of "LiDAR coordinate system → camera coordinate system → image pixel coordinate system," it completes the projection transformation from three-dimensional spatial coordinates to two-dimensional pixel coordinates, obtaining homogeneous projected pixel coordinates. Based on these homogeneous projected pixel coordinates, its horizontal and vertical pixel coordinate values ​​are extracted as the corresponding projected pixel coordinates of the laser point. These projected pixel coordinates perfectly match the pixel coordinate system of the standardized image data, accurately representing the pixel position of the laser point in the standardized image data. Ultimately, each laser point in the standardized point cloud data is matched with a unique corresponding projected pixel coordinate, achieving precise spatial mapping between the laser point cloud data and the visual image data in the pixel coordinate system.

[0052] Step 3022: Based on the projected pixel coordinates and pixel-level semantic mask, laser points located within the dynamic object region are selected from the standardized point cloud data to obtain a candidate dynamic laser point group.

[0053] Optionally, the autonomous exploration navigation system retrieves pixel-level semantic masks and traverses each laser point in the standardized point cloud data, extracting its corresponding projected pixel coordinates. Based on these projected pixel coordinates, the pixel position of the laser point in the pixel-level semantic mask is determined. Then, the mask pixel value corresponding to the pixel position is read. If the mask pixel value corresponding to the laser point's projected pixel coordinates is a dynamic identifier value, the laser point is determined to be located within a dynamic object region and is included in the candidate dynamic laser point group. If the mask pixel value corresponding to the laser point's projected pixel coordinates is a static identifier value, the laser point is determined to be located within a non-dynamic object region and is retained in the original standardized point cloud data. Finally, the candidate dynamic laser point group is obtained, which is the set of all laser points in the standardized point cloud data projected onto the pixel region of the dynamic object.

[0054] Step 3023: Based on the depth value of each laser point in the candidate dynamic laser point group and the depth distribution characteristics of its neighborhood, perform a depth consistency check on the candidate dynamic laser point group to obtain the depth check results of each laser point.

[0055] Optionally, since the candidate dynamic laser point group includes laser points that are actually dynamic objects, it may also include laser points of static background structures behind the dynamic objects. Based on this, the autonomous exploration and navigation system extracts the depth value of each laser point in the candidate dynamic laser point group, that is, the Z-axis coordinate value of the laser point after transformation to the camera coordinate system. At the same time, it defines a neighborhood range for each laser point in the candidate dynamic laser point group. This neighborhood range is a point cloud region composed of a preset fixed number of adjacent laser points centered on the laser point. The preset fixed number can be adjusted according to the point cloud density of the operating environment to ensure that the neighborhood range can reflect the depth distribution characteristics around the laser point. The neighborhood depth distribution characteristics are the distribution pattern, dispersion, and mean characteristics of the depth values ​​of all laser points within the neighborhood range.

[0056] Optionally, the autonomous exploration and navigation system constructs a depth distribution model for the candidate dynamic laser point group, that is, first calculates the depth value of the mask center point corresponding to the mask region of each dynamic object. The depth value at the center point of the mask is the arithmetic mean of the depth values ​​of all laser points within the mask area, used to characterize the overall depth features of the corresponding dynamic object. Then, a depth window constraint formula is introduced. ,in, This is the depth deviation threshold; For the coefficients of the deep quadratic term, These are depth constants, all fixed parameters calibrated based on measured data from the operational environment, used to adapt to depth deviation judgment standards at different depths. Based on this, the autonomous exploration navigation system performs a depth consistency check on each laser point in the candidate dynamic laser point group. The check process involves first calculating the depth value of the laser point. Depth value of the corresponding mask center point absolute depth deviation The absolute depth deviation and the depth deviation threshold are compared. A comparison is performed, and the depth verification result is obtained based on the comparison results. If If the depth of the laser point is consistent with the center depth of the dynamic object, the depth verification result is a dynamic occlusion point, meaning the laser point is the point cloud data of the dynamic object itself; if If the depth of the laser point is inconsistent with the center depth of the dynamic object, the depth verification result is determined to be a background structure point, meaning the laser point is point cloud data of a static background structure behind the dynamic object. A unique depth verification result is matched to each laser point in the candidate dynamic laser point group, ultimately achieving accurate classification of the candidate dynamic laser points.

[0057] Step 3024: Based on the depth verification results, laser points identified as dynamic occlusion points in the candidate dynamic laser point group are removed, and laser points identified as background structures are retained, to obtain pure static laser point cloud data without dynamic interference.

[0058] Optionally, the autonomous exploration navigation system retrieves the original dataset of all laser point depth verification results combined with standardized point cloud data, and performs laser point removal and retention according to the following rules: For laser points in the candidate dynamic laser point group whose depth verification results indicate dynamic occlusion, they are directly removed from the standardized point cloud data to eliminate dynamic interference from the point cloud of the dynamic object itself on positioning and mapping. For laser points in the candidate dynamic laser point group whose depth verification results indicate background structure points, they are retained in the standardized point cloud data to avoid accidentally deleting static background structure point clouds behind dynamic objects, thus ensuring the integrity of the environmental geometry. For laser points in the standardized point cloud data not included in the candidate dynamic laser point group, they are directly identified as laser points in non-dynamic object regions and are retained without any removal process. After completing the removal and retention of all laser points, the remaining laser point data is re-integrated and structured, retaining the core information such as the global unified reference coordinate system 3D coordinates and spatial metadata of each laser point, ultimately obtaining pure static laser point cloud data without dynamic interference. This pure static laser point cloud data contains only the static geometric structure point cloud of the working environment, without interference from dynamic object point clouds, and completely preserves the point cloud information of the static background structure, thus being able to truly and accurately reflect the static geometric features of the working environment.

[0059] The embodiments of the present invention solve the problems of difficulty in accurately removing dynamic interference and easy deletion of static structures in traditional fusion navigation. The pure static laser point cloud data obtained can truly, accurately and completely reflect the static geometric features of the working environment, providing a high-purity and high-precision point cloud data foundation for subsequent laser positioning mapping and static semantic map construction.

[0060] Optionally, the processes of steps 3041 to 3046 include: Step 3041: Based on the static image identifier and the preset static semantic landmark category whitelist, static pixel regions are selected, and ray projection calculation is performed based on the two-dimensional feature points of the static pixel regions and the pure static laser point cloud data to obtain the three-dimensional observation coordinates of the initial static semantic landmark.

[0061] Optionally, the autonomous navigation system retrieves a pre-defined whitelist of static semantic landmark categories. This whitelist is a set of static marker categories that can be used as positioning references, pre-set according to the robot's operational scenario requirements. It includes marker categories with fixed positions, significant visual features, and absolute positional references, such as safety exit signs, door signs, equipment signs, and construction area warning signs. The obtained image static markers are matched one by one with the whitelist of static semantic landmark categories. Image static markers belonging to the whitelist categories are selected. Based on the pixel position of this type of marker in the standardized image data, its corresponding pixel range is defined to obtain a static pixel region. This static pixel region is the complete pixel range of static semantic landmarks belonging to the whitelist categories in the standardized image data. Two-dimensional feature points are then extracted from the static pixel region. These two-dimensional feature points are pixels within the static pixel region that possess significant visual features such as corners, edges, and texture abrupt changes. The ray projection method is adopted, with the optical center of the vision camera as the ray origin and each extracted two-dimensional feature point as the ray direction. The projected ray is emitted into the three-dimensional space where the pure static laser point cloud data is located, and the coordinates of the intersection point of the projected ray and the three-dimensional point cloud in the pure static laser point cloud data are solved. The coordinates of the intersection point are the actual position of the corresponding two-dimensional feature point in the three-dimensional space.

[0062] Optionally, the autonomous exploration navigation system performs fusion calculation on the coordinates of the three-dimensional intersection points corresponding to all two-dimensional feature points of the same static semantic landmark, takes the coordinates of its geometric center as the three-dimensional spatial position of the static semantic landmark, and obtains the initial three-dimensional observation coordinates of the static semantic landmark. These three-dimensional observation coordinates are the three-dimensional spatial coordinates under the lidar coordinate system, which is the initial observation position of the static semantic landmark in three-dimensional space.

[0063] Step 3042: Based on the three-dimensional observation coordinates and the preset Bayesian probability model, determine the corresponding existence confidence level, and filter based on the existence confidence level and the preset confidence threshold to obtain a high-confidence candidate landmark group.

[0064] Optionally, the autonomous exploration navigation system retrieves a preset Bayesian probability model. This Bayesian probability model is a probability update model used to calculate the probability of the existence of static semantic landmarks, and its core formula is: ;in, To observe the current data Post-static semantic landmarks The posterior existence confidence, i.e. the existence confidence corresponding to the three-dimensional observation coordinates; For static semantic landmarks Under the condition of existence, the current three-dimensional observation coordinate data is observed. The likelihood probability is determined by the measurement error of the three-dimensional observation coordinates and the point cloud matching accuracy; Static semantic landmarks The prior confidence level is set to 0.5 and is dynamically updated with subsequent observations. To observe the current three-dimensional observation coordinate data The total probability is a normalization constant used to ensure that the posterior confidence level ranges from 0 to 1.

[0065] Optionally, the autonomous exploration navigation system uses the three-dimensional observation coordinates of the initial static semantic landmarks as observation data. The existence confidence score of each initial static semantic landmark is calculated by substituting the values ​​into a Bayesian probability model. This confidence score ranges from 0 to 1. The closer the value is to 1, the more accurate the 3D observation coordinates of the static semantic landmark are; the closer the value is to 0, the more accurate the 3D observation coordinates are. A preset confidence threshold is then retrieved. This threshold is a critical value set in advance based on the positioning accuracy requirements of the operational scenario to select high-confidence semantic landmarks. The value ranges from 0 to 1; in this embodiment, the preset confidence threshold is 0.7. The existence confidence score of each initial static semantic landmark is compared with the confidence threshold. Initial static semantic landmarks with an existence confidence score greater than or equal to the confidence threshold are selected and integrated into a high-confidence candidate landmark group. Initial static semantic landmarks with an existence confidence score lower than the confidence threshold are judged as false detections or low-precision landmarks and are discarded.

[0066] Step 3043: Based on the pose change and information entropy of the target robot in the current frame, determine whether the current frame meets the key frame insertion conditions. If it does, project the high-confidence candidate landmark group to the global coordinate system to obtain the global coordinate estimate, and associate the global coordinate estimate with the existing landmarks in the initial semantic map to obtain the association result. The initial semantic map is the static semantic map that the target robot has completed in the previous moment.

[0067] Optionally, the autonomous exploration and navigation system extracts the pose change of the target robot in the current frame relative to the previous frame. This pose change is the six-degree-of-freedom pose difference between the target robot at the time of acquisition of the current frame and the time of acquisition of the previous frame, including the change in three-dimensional spatial position and the change in three-dimensional rotation angle, used to characterize the robot's motion amplitude. Simultaneously, it calculates the information entropy of the clean static laser point cloud data of the current frame. This information entropy is a value representing the content of unknown environmental information contained in the point cloud data of the current frame. The larger the value, the more new environmental information the current frame contains, and the more valuable it is as a keyframe. It then retrieves preset keyframe insertion conditions. These conditions are dual criteria for determining whether the current frame can be used as a keyframe for incremental registration of the semantic map, including a pose change threshold and an information entropy threshold: if the pose change of the robot in the current frame is greater than or equal to the pose change threshold, or the information entropy of the current frame point cloud data is greater than or equal to the information entropy threshold, the current frame is deemed to meet the keyframe insertion conditions; if both indicators are lower than the corresponding thresholds, the conditions are not met, and subsequent landmark projection and association operations are not performed.

[0068] If the current frame meets the keyframe insertion conditions, the autonomous exploration and navigation system uses the laser positioning pose estimation value of the target robot in the current frame as the basis for transformation. It projects the three-dimensional observation coordinates of the lidar coordinate system of the high-confidence candidate landmark group to the global coordinate system, completes the coordinate transformation and scale normalization, and obtains the global coordinate estimation value. This global coordinate estimation value is the three-dimensional spatial coordinate in the global coordinate system, which is used to characterize the position of the high-confidence candidate landmark in the global environment.

[0069] Optionally, the autonomous exploration navigation system retrieves an initial semantic map, which is a static semantic map that the target robot has already constructed at the previous moment, containing the global coordinates and semantic attributes of existing static semantic landmarks. A nearest neighbor matching algorithm is used to calculate the spatial distance and perform feature matching between the estimated global coordinates of the high-confidence candidate landmark group and the global coordinates of existing landmarks in the initial semantic map, completing the data association and obtaining the association result. This association result includes two types: first, a successful match, meaning that the spatial distance between the current high-confidence candidate landmark and a certain existing landmark in the initial semantic map is less than a preset matching threshold and their visual features are consistent, thus they are determined to be the same landmark; second, a failed match, meaning that the spatial distance between the current high-confidence candidate landmark and all existing landmarks in the initial semantic map is greater than the preset matching threshold or their visual features are inconsistent, thus they are determined to be new static semantic landmarks.

[0070] Step 3044: Incremental registration is performed based on the association results to obtain the current semantic map containing temporal information. Based on the multiple observation records of each landmark in the current semantic map under different keyframes, a multi-view co-view relationship graph is constructed.

[0071] Optionally, the autonomous exploration navigation system performs incremental registration of high-confidence candidate landmark groups based on the association results. That is, based on the initial semantic map, only new landmarks are added, and the location and confidence of existing landmarks that have been successfully matched are updated, without rebuilding the entire semantic map, thereby improving map update efficiency. The specific registration rules are as follows: if the association result is a failed match (new landmark), the global coordinate estimate, semantic category, existence confidence, and current keyframe temporal information of the high-confidence candidate landmark are added to the initial semantic map; if the association result is a successful match (same landmark), the global coordinate estimate of the high-confidence candidate landmark is fused and optimized with the coordinates of existing landmarks in the initial semantic map, the 3D coordinates of the existing landmarks are updated, and their existence confidence is updated based on a Bayesian probability model, while the temporal observation information of the current keyframe is added. After completing incremental registration, a current semantic map containing temporal information is obtained. This current semantic map refers to the semantic map updated based on the initial semantic map. It includes the global three-dimensional coordinates, semantic categories, existence confidence, and temporal observation records of all static semantic landmarks under each keyframe. The temporal information can characterize the observation time and observation position of each landmark under different robot poses.

[0072] Optionally, the autonomous exploration navigation system extracts multiple observation records of all static semantic landmarks in the current semantic map under different keyframes, counts the keyframe combinations in which each landmark is jointly observed, and constructs a multi-view co-observation relationship graph. This multi-view co-observation relationship graph is a topological graph with static semantic landmarks as nodes and keyframe co-observation relationships as edges: if two static semantic landmarks are observed simultaneously under the same keyframe, a co-observation edge is established between the two nodes, and the weight of the edge is the number of keyframes in which the two landmarks are jointly observed; the attributes of the nodes are the global coordinates and existence confidence of the landmark, and the attributes of the edges are the temporal information of the co-observation keyframes.

[0073] Step 3045: Based on the multi-view co-view relationship graph, perform spatial consistency verification on the three-dimensional world coordinates of the landmarks in the current semantic map at the current moment, obtain the verification result, and correct the landmarks in the current semantic map based on the verification result to obtain a geometrically accurate static semantic landmark set.

[0074] Optionally, the autonomous exploration and navigation system, based on a multi-view co-view relationship graph, performs spatial consistency verification on the 3D world coordinates of all static semantic landmarks in the current semantic map at the current moment. This involves utilizing the spatial constraints imposed by the multi-view co-view relationship to verify the rationality of the spatial position of each landmark's 3D coordinates in the global coordinate system and eliminate observation errors. The core verification logic is as follows: in the multi-view co-view relationship graph, static semantic landmarks with shared viewing edges should satisfy a fixed relative spatial position relationship in 3D space. This relative position relationship should remain consistent across multi-view observations in different keyframes. If the 3D coordinates of a landmark cause a significant deviation in its relative position relationship with shared landmarks, then the coordinates are considered to have an observation error. Specifically, the autonomous exploration and navigation system first calculates the relative spatial position relationship of each pair of shared landmarks in the multi-view co-view relationship graph under different keyframes, including relative distance and relative orientation, to obtain the relative position reference value for each landmark pair. Then, it calculates the relative position between the 3D world coordinates of each landmark and the shared landmark at the current moment, compares it with the relative position reference value, and calculates the position deviation value. The system retrieves a preset spatial deviation threshold, which is jointly calibrated by the hardware accuracy of the robot's lidar and vision camera. The specific value was determined through multiple experiments and is a general threshold that does not consider the working scenario. It is used to reflect the upper limit of the sensor's measurement error. If the relative position deviation of a landmark is less than or equal to the spatial deviation threshold, the three-dimensional coordinate space of the landmark is considered consistent, and the verification result is qualified. If the relative position deviation is greater than the spatial deviation threshold, the three-dimensional coordinate space of the landmark is considered inconsistent, and the verification result is unqualified. The coordinates have observation errors and need to be corrected.

[0075] Optionally, the autonomous exploration navigation system corrects the coordinates of static semantic landmarks in the current semantic map based on the spatial consistency verification results. Specifically, for landmarks that pass the verification, their current 3D world coordinates are retained without correction; for landmarks that fail the verification, the system uses the precise 3D coordinates and relative position reference values ​​of their shared landmarks, employing spatial interpolation and fusion optimization algorithms to back-calculate and update their coordinate information in the current semantic map. After correcting the coordinates of all unqualified landmarks, the precise 3D coordinates, semantic categories, and existence confidence of all static semantic landmarks in the current semantic map are integrated to obtain a geometrically accurate set of static semantic landmarks.

[0076] Step 3046: Perform global joint optimization based on the static semantic landmark set and the laser positioning pose estimation value to obtain the static semantic map.

[0077] Optionally, the autonomous exploration navigation system performs global joint optimization based on the static semantic landmark set and the laser positioning pose estimation value to obtain a static semantic map, as described in steps 30461 to 30463.

[0078] The embodiments of the present invention solve the problems of low accuracy in semantic map construction, poor update efficiency, difficulty in adapting to dynamic environmental changes, and lack of spatial consistency constraints in traditional fusion navigation. The constructed static semantic map has both high-precision three-dimensional geometric position and complete semantic attribute information, realizing the three-dimensional and structured representation of environmental semantic features. Together with the environmental point cloud map, it forms a dual environmental cognition of geometry and semantics, providing a reliable absolute position reference for subsequent joint optimization of robot pose.

[0079] Optionally, the processes of steps 30461 to 30463 include: Step 30461: Based on the static semantic landmark set and the laser positioning pose estimation value, the static semantic landmark set is used as an absolute position constraint factor and the laser positioning pose estimation value is used as a relative pose constraint factor. These factors are then introduced into a preset factor graph optimization framework to construct a global factor graph model that includes robot pose nodes, laser odometry edges, and semantic landmark observation edges.

[0080] Optionally, the autonomous exploration and navigation system first retrieves a preset factor graph optimization framework. This factor graph optimization framework is a graph-based nonlinear optimization framework, consisting of two core elements: nodes and edges. Nodes are the unknown variables to be optimized, and edges are the constraint relationships between nodes. The static semantic landmark set is introduced into the factor graph optimization framework as an absolute position constraint factor, and the laser positioning pose estimation value is introduced into the factor graph optimization framework as a relative pose constraint factor. The absolute position constraint factor provides a globally fixed spatial position, which can provide a globally unified position reference for the framework and effectively suppress accumulated errors. The relative pose constraint factor reflects the relationship between the robot's pose changes at adjacent times, which can characterize the continuity and geometric consistency of the robot's motion. A global factor graph model is constructed based on the two types of constraint factors. This model includes three core components: robot pose nodes, laser odometry edges, and semantic landmark observation edges.

[0081] Among them, the robot pose node is the core unknown variable node of the factor graph. The autonomous exploration and navigation system constructs the robot pose node by taking the laser positioning pose estimation value of the target robot at each key frame acquisition time as the node initial value. Each node uniquely corresponds to a key frame time. The node attribute is the robot's six-degree-of-freedom pose data in the global coordinate system, including three-dimensional spatial position coordinates and three-dimensional rotation angle. The laser odometry edge is a relative pose constraint edge connecting adjacent robot pose nodes. The autonomous exploration and navigation system calculates the relative pose transformation relationship between adjacent keyframe robot pose nodes based on the laser positioning pose estimation value and constructs the laser odometry edge. This edge is a soft constraint edge. The constraint error of the edge is the deviation between the actual relative pose of the adjacent pose node and the relative pose measured by the laser odometry, which characterizes the geometric observation constraint of the lidar on the robot motion. The semantic landmark observation edge is an absolute position constraint edge connecting the robot pose node and the static semantic landmark virtual node. The autonomous exploration and navigation system first constructs a unique static semantic landmark virtual node for each landmark in the static semantic landmark set. The initial value of the node is the three-dimensional world coordinates of the landmark after spatial consistency verification. Then, according to the spatial position relationship between the robot pose node and the corresponding landmark virtual node, the semantic landmark observation edge is constructed. This edge is a hard constraint edge. The constraint error of the edge is the reprojection error between the landmark coordinates observed by the robot pose node and the initial coordinates of the landmark virtual node, which represents the global absolute position constraint of the static semantic landmark on the robot pose.

[0082] Optionally, after the autonomous exploration navigation system completes the construction of the three core components, it assigns preset constraint weights to the laser odometry edge and the semantic landmark observation edge, respectively. The constraint weights are constants set according to the sensor measurement accuracy, and the weight of the semantic landmark observation edge is higher than that of the laser odometry edge, thereby strengthening the optimization priority of the absolute position constraint, and finally obtaining a global factor graph model containing complete nodes, edges and constraint weights.

[0083] Step 30462: Perform global solution optimization based on the global factor graph model to obtain the keyframe pose sequence and static semantic landmark coordinate set.

[0084] Optionally, the autonomous exploration and navigation system uses the Gauss-Newton iterative optimization algorithm to perform global solution optimization based on the completed global factor graph model. That is, by repeatedly calculating the Jacobian matrix and Hessian matrix of the constraint error function, the initial values ​​of the nodes in the factor graph are continuously corrected until the total constraint error of all edges reaches the minimum value or the number of iterations reaches the preset threshold, thus completing the global optimal solution for the unknown variables of the nodes.

[0085] Specifically, the autonomous exploration and navigation system constructs a global total error function based on the constraint errors of all edges in the global factor graph model. This global total error function is the weighted sum of the relative pose deviation errors of all laser odometry edges and the reprojection errors of all semantic landmark observation edges. The error weights of the laser odometry edges and semantic landmark observation edges are consistent with the constraint weights assigned in the global factor graph model. Next, the Jacobian matrix of the total error function with respect to all robot pose nodes and static semantic landmark virtual nodes is calculated. This matrix represents the rate of change of the total error function with respect to node values. Then, an approximate matrix of the Hessian matrix is ​​constructed using the Jacobian matrix, and the correction amount for the node values ​​is obtained by solving a system of linear equations. Finally, the initial node values ​​and the correction amounts are superimposed to obtain new node values, which are then substituted into the total error function for recalculation. The total error function value after each iteration is compared with the value of the previous iteration. If the difference between the two values ​​is less than the preset error convergence threshold, or the number of iterations reaches the preset maximum number of iterations, the iteration is determined to be converged and the optimization solution ends. If the convergence condition is not met, the iteration solution and node correction steps are repeated until the convergence condition is met.

[0086] Optionally, after iterative convergence, the autonomous exploration and navigation system extracts the optimized final values ​​of all robot pose nodes in the factor graph, sorts them according to the chronological order of the keyframes, and obtains the keyframe pose sequence. This keyframe pose sequence is the optimized six-degree-of-freedom pose data of the target robot at all keyframe moments in the global coordinate system. At the same time, the optimized final values ​​of all static semantic landmark virtual nodes in the factor graph are extracted and integrated to obtain the static semantic landmark coordinate set. This static semantic landmark coordinate set is the optimized three-dimensional world coordinates of all static semantic landmarks in the global coordinate system.

[0087] Step 30463: Reconstruct the map based on the keyframe pose sequence and the static semantic landmark coordinate set to obtain the static semantic map at the current moment.

[0088] Optionally, the autonomous exploration navigation system performs data verification on the optimized keyframe pose sequence and the static semantic landmark coordinate set to verify the completeness, rationality, and spatial consistency of the two types of data. Specific verification includes: verifying whether the keyframe pose sequence contains pose data for all keyframe moments without missing or redundant data; verifying whether the static semantic landmark coordinate set contains the optimized 3D coordinates of all static semantic landmarks with no obvious anomalies; and verifying whether the spatial observation relationship between the landmark coordinates and the corresponding keyframe poses is reasonable and logically consistent. Data that fails verification will be removed and re-extracted from the optimization results to ensure the validity of the basic data. After successful verification, a static semantic map is reconstructed based on the two types of optimized data. The specific reconstruction process includes: 1. The optimized 3D world coordinates of each landmark in the static semantic landmark coordinate set are associated and bound with the semantic category, existence confidence, multi-view observation records and other attribute information of the landmark to form a structured landmark information unit. Each unit uniquely corresponds to a static semantic landmark and contains all the geometric and semantic attribute information of the landmark. 2. Associate each keyframe pose data in the keyframe pose sequence with the landmark information unit observed at that keyframe moment, and label the keyframe moment when each landmark is observed by the robot, the robot's observation pose and other temporal observation information, so as to realize the bidirectional association between landmark information and robot motion information. 3. Construct a two-layer structure for the static semantic map. The first layer is the geometric location layer, which projects the optimized 3D coordinates of all landmark information units onto the global coordinate system to generate the spatial distribution of landmarks in the global environment, intuitively representing the 3D geometric location of the landmarks. The second layer is the semantic attribute layer, which binds the semantic category, existence confidence, time-series observation information, and other attribute data of each landmark to the corresponding landmark in the geometric location layer, realizing the accurate mapping between the geometric location and semantic attributes of the landmark. Clicking on any landmark in the geometric location layer can directly retrieve all its semantic attribute information. 4. The reconstructed static semantic map is merged with the static semantic map of the target robot at the previous moment. Newly added static semantic landmarks are directly added to the map. Existing landmarks are updated with optimized coordinates and attribute information. Landmarks that disappear due to environmental changes (with a confidence level below a preset threshold) are marked and hidden, thereby realizing incremental updates and improvements to the static semantic map.

[0089] After completing map reconstruction and incremental updates, the map data is standardized and encapsulated, retaining core content such as global coordinate system parameters, landmark information units, keyframe pose association information, and layer structure data, ultimately obtaining a static semantic map at the current moment.

[0090] The embodiments of the present invention solve the problems of superposition of landmark coordinates and robot pose errors, low matching degree between map and actual environment space, and difficulty in achieving accurate global representation in the construction of static semantic maps. The final static semantic map is geometrically accurate, semantically complete, and globally unified, forming a dual environmental cognition of geometry and semantics with the environmental point cloud map.

[0091] Optionally, the processes of steps 401 to 406 include: Step 401: Determine the current position of the target robot based on the joint estimated pose, and extract the front edge points by combining the distribution differences between the scanned occupied space voxels and the unscanned free space voxels in the environmental point cloud map, so as to obtain the environmental front edge point group representing the boundary between the known free area and the unknown area.

[0092] Optionally, the autonomous exploration and navigation system analyzes the six-degree-of-freedom data of the jointly estimated pose, extracts the three-dimensional spatial coordinates, and determines the current position of the target robot. Simultaneously, it retrieves the environmental point cloud map and performs spatial voxelization processing, dividing the environmental space in the global coordinate system into equal-volume cubic spatial units. Each cubic spatial unit is a spatial voxel, and the size of the spatial voxel is preset according to the environmental point cloud density and the localization mapping accuracy to accurately represent the occupancy status of the environmental space. The occupancy status of each spatial voxel is marked, categorized as scanned occupied spatial voxels, unscanned free spatial voxels, and unknown spatial voxels. Scanned occupied spatial voxels contain environmental point cloud data and represent the presence of static obstacles; unscanned free spatial voxels lack environmental point cloud data but are verified by the robot as passable; and unknown spatial voxels have not been scanned by the robot and their occupancy status is unknown. Next, the front edge points are extracted. These front edge points are the boundary points between unscanned free space voxels and unknown space voxels. The extraction rules are as follows: traverse all unscanned free space voxels, select free space voxels that have neighborhood contact relationships with unknown space voxels, use the geometric center coordinates of these voxels as the spatial coordinates of the front edge points, and integrate all the extracted front edge points to obtain the environmental front edge point group. Each front edge point in the environmental front edge point group is a candidate exploration position that the robot can pass through and faces the unknown exploration area.

[0093] Step 402: Based on the rate of change of the normal vector of each environmental front point in the environmental point cloud map and the plane fitting tilt angle of its neighboring points, determine the surface roughness cost and slope cost corresponding to each environmental front point.

[0094] Optionally, the autonomous exploration navigation system defines a neighborhood range for each environmental front point in the environmental front point group. This neighborhood range is a point cloud region centered on the front point and composed of a preset number of adjacent laser points selected in the environmental point cloud map. The preset number is adjusted according to the density of the environmental point cloud to ensure that the neighborhood points can completely represent the terrain geometry around the front point. Geometric feature calculations are then performed on the neighborhood points of each front point to determine the surface roughness cost and slope cost, and a unique surface roughness cost and slope cost are assigned to each front point in the environmental front point group, thus quantifying the terrain geometry cost of all front points.

[0095] The calculation process for the surface roughness cost value includes: first, calculating the three-dimensional normal vector of each laser point in the neighborhood, which is a three-dimensional vector representing the orientation of the terrain surface where the laser point is located; then, calculating the rate of change of the normal vectors of all neighborhood points, which is the average difference in the angle between the normal vectors in the neighborhood points, reflecting the unevenness and smoothness of the terrain surface. The larger the rate of change of the normal vector, the rougher the terrain surface and the worse the passability; finally, normalizing the rate of change of the normal vector and mapping it to a preset cost value range (such as 0 to 10) to obtain the surface roughness cost value corresponding to the front edge point. The larger the value, the rougher the terrain surface and the higher the passability cost. The calculation process for the slope cost value includes: first, performing plane fitting on the neighboring points, and using the least squares method to construct the optimal fitting plane for the neighboring points. The inclination angle of this plane fitting is the angle between the optimal fitting plane and the horizontal plane of the global coordinate system, reflecting the degree of terrain inclination. The larger the plane fitting inclination angle, the steeper the terrain slope and the worse the traversability. Then, the plane fitting inclination angle is normalized and mapped to a preset cost value range (such as 0 to 10) to obtain the slope cost value corresponding to the front edge point. The larger the value, the steeper the terrain slope and the higher the traversal cost.

[0096] Step 403: Based on the surface roughness value and slope value, geometric threshold filtering is performed by combining the preset maximum allowable roughness threshold and maximum allowable slope threshold to obtain a group of candidate frontier points that meet the geometric passability standard.

[0097] Optionally, the autonomous exploration and navigation system retrieves preset maximum permissible roughness thresholds and maximum permissible slope thresholds. These thresholds are geometric passability thresholds pre-set based on the target robot's motion structure characteristics (wheeled, legged, or wheel-leg hybrid) and mobility. The threshold values ​​are within a preset range (0 to 10) for surface roughness cost and slope cost, reflecting the geometric limits of terrain that the robot can safely traverse. Terrain exceeding these thresholds is considered impassable and dangerous terrain. Geometric threshold filtering is then performed based on the surface roughness cost and slope cost. The filtering process involves iterating through each leading point in the environmental leading point group. If its surface roughness cost is less than or equal to the maximum permissible roughness threshold, and its slope cost is less than or equal to the maximum permissible slope threshold, the terrain geometric passability of that leading point is deemed acceptable, and it is included in the candidate leading point group. If either cost exceeds the corresponding threshold, the terrain corresponding to that leading point is deemed impassable and dangerous, and it is removed. The final candidate frontier point set consists of only frontier points that meet the geometric passability criteria, ensuring that the exploration points selected later are all located in terrain areas that the robot can safely pass through, thus avoiding the risk of the robot accidentally entering dangerous terrain from a geometric perspective.

[0098] Step 404: Based on the spatial coordinates of each candidate front point in the candidate front point group, retrieve the landmark semantic category label and surface material attribute label stored in the corresponding location and its neighborhood in the static semantic map, and quantify the value of the landmark semantic category label and surface material attribute label of each candidate front point based on the preset semantic exploration reward rules to obtain the semantic gain value that represents the exploration priority.

[0099] Optionally, the autonomous exploration navigation system defines a semantic retrieval neighborhood for each candidate frontier point in the candidate frontier point group. This semantic retrieval neighborhood refers to a spherical spatial region with a preset radius centered on the spatial coordinates of the candidate frontier point in the global coordinate system. The preset radius is adjusted according to the landmark density of the static semantic map to ensure that the semantic attribute information around the frontier point can be completely retrieved. Based on the spatial coordinates of each candidate frontier point, semantic tag retrieval is performed on its semantic retrieval neighborhood in the static semantic map to extract the landmark semantic category tags and surface material attribute tags stored within the range. The landmark semantic category tags are tags that represent the static semantic landmark categories around the frontier point, such as safety exits, equipment signs, construction warning signs, dead ends, etc.; the surface material attribute tags are tags that represent the surface material categories at the location of the frontier point, such as cement ground, waterlogged areas, loose soil piles, gravel ground, etc.

[0100] Optionally, the autonomous exploration navigation system retrieves preset semantic exploration reward rules. These rules are pre-set based on the robot's task requirements and quantify the value of different semantic tags. They include reward and penalty items. Semantic tags relevant to the task and possessing high exploration value are assigned positive reward values, such as safety exits and target equipment markers. Semantic tags representing dangerous areas and lacking exploration value are assigned negative penalty values, such as waterlogged areas, loose soil mounds, and dead ends. Semantic tags representing ordinary passable areas and lacking special exploration value are assigned zero values, such as ordinary concrete surfaces and smooth dirt roads. Based on the semantic exploration reward rules, the semantic category tags of landmarks and the surface material attribute tags retrieved for each candidate frontier point are quantified and summed to obtain the semantic gain value corresponding to that candidate frontier point. This semantic gain value represents the exploration priority of the frontier point; a larger value indicates higher exploration value and priority, while a smaller value indicates lower exploration value and priority. A negative value indicates that the frontier point is a dangerous or valueless exploration area.

[0101] Step 405: A comprehensive cost evaluation is performed based on the cost of surface roughness, the cost of slope, and the semantic gain value to obtain the comprehensive exploration cost of each candidate frontier point. The frontier points are then sorted in ascending order based on their comprehensive exploration cost values, and the frontier point with the smallest comprehensive exploration cost value is determined as the target exploration point of the target robot.

[0102] Optionally, the autonomous exploration navigation system retrieves preset slope weights, surface roughness weights, and semantic risk weights. These weights are non-negative constants pre-defined based on the target robot's operating scenario, motion structure characteristics, and task requirements. They respectively characterize the importance of slope, surface roughness, and surface material semantics in terrain passability assessment, balancing the proportion of geometric and semantic features in the overall cost assessment. Simultaneously, the slope cost and surface roughness cost are extracted for each candidate front point. Based on the surface material attribute label corresponding to that candidate front point, a preset material risk factor is retrieved. This material risk factor is a physical attribute weight assigned to different surface materials based on visual semantic classification results, characterizing the passability risk level of different material terrains; the more dangerous the material, the higher the corresponding material risk factor value. The slope cost, surface roughness cost, and material risk factor are multiplied by their respective slope weights, surface roughness weights, and semantic risk weights, and then summed to obtain the overall terrain passability cost for that candidate front point. The formula is as follows: ;in, This represents the overall cost of terrain accessibility; the larger the cost value, the greater the geometric and semantic difficulty of terrain accessibility. For slope weight, As the surface roughness weight, These are semantic risk weights, all of which are preset non-negative constants; The slope of the candidate frontier point is used as a substitute for its value; The surface roughness of the candidate front point is given a substitution value; The material risk factor for the surface material corresponding to the candidate front points, and .

[0103] Optionally, after determining the comprehensive terrain accessibility cost, the autonomous exploration navigation system extracts the semantic gain value for each candidate frontier point. This semantic gain value is a quantified result of the semantic gain term obtained based on the semantic category label of the landmark. When the candidate frontier point is close to high-value exploration markers such as safety exits and target equipment, the semantic gain term takes a positive value; when it is close to valueless / dangerous markers such as dead ends and danger warning lines, the semantic gain term takes a negative value. An exponential function is introduced to weight and regulate the semantic gain term. The positive or negative sign of the semantic gain term is used to reward or penalize the comprehensive terrain accessibility cost, ultimately obtaining the comprehensive exploration cost of the candidate frontier point. The formula is as follows: ;in, The smaller the value, the higher the exploration priority for candidate frontier points; The semantic gain term corresponding to the candidate frontier points is calculated by summing the values ​​when high-value exploration markers are identified and negative values ​​when dangerous / worthless markers are identified. After calculating the comprehensive exploration value for all candidate frontier points, all comprehensive exploration values ​​are sorted in ascending order (from minimum to maximum). The frontier point with the minimum comprehensive exploration value is determined as the target exploration point for the target robot. This target exploration point is the optimal exploration location that balances terrain geometric passability, semantic accessibility, and exploration value; it serves as the target endpoint for subsequent global path planning.

[0104] Step 406: Determine the current position of the target robot and the target position of the target exploration point based on the joint estimated pose. Perform a global path search by combining the obstacle object distribution information in the environmental point cloud map to obtain a global collision-free path. Based on the global collision-free path and the preset dynamic obstacle avoidance strategy for the target robot to perceive local obstacles in real time, generate the target motion path for the target robot to move.

[0105] Optionally, the autonomous exploration and navigation system retrieves the current position of the target robot determined by the joint estimated pose, and the target position determined by the 3D spatial coordinates of the target exploration point, respectively serving as the start and end points of the global path search. Then, it retrieves the environmental point cloud map and extracts the obstacle object distribution information, i.e., the global coordinate distribution of all scanned and occupied spatial voxels. This information accurately represents the spatial position, outline, and geometric features of all static obstacles in the environment. Simultaneously, it retrieves the terrain passability comprehensive cost map, which is a cost grid map in a global coordinate system. Each grid cell contains the corresponding slope cost, surface roughness cost, material risk factor, and terrain passability comprehensive cost, used to characterize the traversal difficulty of that grid area. Using the terrain passability comprehensive cost and path length as core heuristic function parameters, a global path search is performed. Starting from the target robot's current position and ending at the target exploration point, the search finds the global path with the lowest terrain passability comprehensive cost and the optimal path length among the traversable spatial voxels in the environmental point cloud map. This path avoids static obstacle objects throughout and prioritizes traversable areas with flat terrain and safe materials, ultimately yielding a globally collision-free path.

[0106] Optionally, the autonomous exploration navigation system has a preset dynamic obstacle avoidance strategy. This dynamic obstacle avoidance strategy is a local path adjustment strategy formulated by the target robot for dynamic obstacles (such as moving personnel, construction machinery, and temporarily fallen debris) that appear in the working environment in real time. The strategy includes four core links: dynamic obstacle detection, local cost map update, local path replanning, and original path return. It enables the robot to perceive the dynamic changes of the local environment in real time while moving along the global path, quickly update the local cost map, and replan a short-distance collision-free path based on the updated local cost map, so as to return to the original global collision-free path after avoiding dynamic obstacles. Subsequently, using a globally collision-free path as the basic path framework, the local path adjustment rules of the dynamic obstacle avoidance strategy are embedded into each path node of the basic path. This clarifies the dynamic obstacle detection range, local cost map update frequency, and constraints for local path replanning at each node. Simultaneously, considering the target robot's motion characteristics (wheeled, legged, or hybrid wheel-legged), the path nodes of the globally collision-free path are smoothed to eliminate sharp corners and ensure the robot's smooth and continuous motion. Furthermore, the comprehensive terrain passability cost is used as the basis for adjusting the robot's speed. Normal speeds are set for areas with low comprehensive terrain passability costs, while lower speeds are set for areas with slightly higher costs but still passable, further improving the safety of robot movement. The final target motion path is a continuous path that combines global optimality, terrain safety, and local dynamic adaptability.

[0107] The formula for the global path search heuristic function is as follows: ;in, A heuristic score is assigned to path node f. The higher the score, the better the exploration value and path optimality of the node. Path nodes with high scores are selected first during the global path search process. The information gain value corresponding to path node f represents the richness of information in the unknown region that the node points to; Let f be the Euclidean distance from path node f to the current position of the target robot, representing the path length from the current position to that node. This is the semantic gain term corresponding to path node f. It takes a positive value when a high-value exploration marker is identified, and a negative value when a dangerous / worthless marker is identified.

[0108] The embodiments of the present invention solve the problems of insufficient terrain passability assessment, lack of semantic exploration logic, and inability to cope with dynamic environments in traditional path planning. It realizes path planning with deep integration of environmental geometry and semantic features. The planned target motion path can guide the robot to complete intelligent autonomous exploration and navigation safely and efficiently in complex dynamic unstructured environments, thereby improving the robustness and intelligence level of robot exploration and navigation.

[0109] Furthermore, the robot autonomous exploration and navigation system integrating visual augmented localization and multimodal terrain analysis provided by the present invention will be described below. The robot autonomous exploration and navigation system integrating visual augmented localization and multimodal terrain analysis described below can be referred to in correspondence with the robot autonomous exploration and navigation method integrating visual augmented localization and multimodal terrain analysis described above.

[0110] Optionally, refer to Figure 2 , Figure 2 This is a schematic diagram of the structure of the robot autonomous exploration and navigation system integrating visual augmented localization and multimodal terrain analysis provided by the present invention. The robot autonomous exploration and navigation system integrating visual augmented localization and multimodal terrain analysis includes: The data acquisition module 210 is used to acquire multi-source heterogeneous data of the target robot in the current dynamic environment; the multi-source heterogeneous data includes laser point cloud data, visual image data and inertial measurement data; The spatiotemporal calibration and processing module 220 is used to perform spatiotemporal calibration and standardization processing based on laser point cloud data, visual image data and inertial measurement data to obtain standardized point cloud data and standardized image data. The localization and map building module 230 is used to perform fusion localization and dual map construction based on standardized point cloud data and standardized image data to obtain joint estimated pose, environmental point cloud map and static semantic map. The terrain analysis and exploration decision module 240 is used to perform terrain analysis and path planning based on joint estimated pose, environmental point cloud map and static semantic map, obtain the target motion path, and drive the target robot to perform exploration and navigation actions along the planned path based on the target motion path.

[0111] The embodiments of the present invention solve the problems of degradation and drift in single laser navigation positioning, high computing power and low positioning accuracy in single vision navigation, and insufficient terrain passability assessment and inaccurate environmental cognition in fusion navigation. They enable robots to achieve stable positioning, accurate mapping and intelligent autonomous exploration navigation in complex dynamic unstructured environments.

[0112] Please see Figure 3 , Figure 3 An embodiment diagram of an electronic device provided in accordance with the present invention. For example... Figure 3 As shown, an embodiment of the present invention provides an electronic device 300, including a memory 310, a processor 320, and a computer program 311 stored in the memory 310 and executable on the processor 320. When the processor 320 executes the computer program 311, it implements the contents of steps 10 to 40.

[0113] Please see Figure 4 , Figure 4An embodiment diagram of a computer-readable storage medium provided in accordance with an embodiment of the present invention is shown. Figure 4 As shown, this embodiment provides a computer-readable storage medium 400, on which a computer program 311 is stored. When the computer program 311 is executed by a processor, it implements the contents of steps 10 to 40.

[0114] On the other hand, the present invention also provides a computer program product, which includes a computer program that can be stored on a non-transitory computer-readable storage medium. When the computer program is executed by a processor, the computer is able to execute the robot autonomous exploration and navigation method that integrates visual augmented localization and multimodal terrain analysis provided by the above methods. The method includes steps 10 to 40.

[0115] Finally, it should be noted that the above embodiments are only used to illustrate the technical solutions of the present invention, and not to limit them; although the present invention has been described in detail with reference to the foregoing embodiments, those skilled in the art should understand that modifications can still be made to the technical solutions described in the foregoing embodiments, or equivalent substitutions can be made to some of the technical features; and these modifications or substitutions do not cause the essence of the corresponding technical solutions to deviate from the spirit and scope of the technical solutions of the embodiments of the present invention.

Claims

1. A robot autonomous exploration and navigation method integrating visual augmented localization and multimodal terrain analysis, characterized in that, include: Acquire multi-source heterogeneous data of the target robot in the current dynamic environment; the multi-source heterogeneous data includes laser point cloud data, visual image data, and inertial measurement data; Spatiotemporal calibration and standardization are performed on the laser point cloud data, the visual image data, and the inertial measurement data to obtain standardized point cloud data and standardized image data. Based on the standardized point cloud data and standardized image data, fusion localization and dual map construction are performed to obtain joint estimated pose, environmental point cloud map and static semantic map; Based on the joint estimated pose, the environmental point cloud map, and the static semantic map, terrain analysis and path planning are performed to obtain the target motion path, and the target robot is driven to perform exploration and navigation actions along the planned path based on the target motion path.

2. The robot autonomous exploration and navigation method integrating visual augmented localization and multimodal terrain analysis according to claim 1, characterized in that, The process of fusing localization and constructing dual maps based on the standardized point cloud data and standardized image data to obtain a joint estimated pose, an environmental point cloud map, and a static semantic map includes: Based on the standardized image data, dynamic and static recognition is performed using a pre-trained lightweight semantic segmentation neural network to obtain recognition results. Then, based on the recognition results, pixel-level semantic masks corresponding to dynamic objects and image static identifiers corresponding to static objects are determined. Based on the standardized point cloud data and the pixel-level semantic mask, dynamic target removal is performed to obtain pure static laser point cloud data without dynamic interference. Based on the pure static laser point cloud data, inter-frame registration and pose calculation are performed to obtain the laser positioning pose estimate of the target robot at the current moment. Based on the laser positioning pose estimate, the pure static laser point cloud data is converted to the global coordinate system for cumulative stitching to obtain an environmental point cloud map that reflects the geometric structure of the environment. Based on the image static identifiers and the clean static laser point cloud data, a static semantic map containing semantic attributes is constructed. The laser geometric constraint term, constructed based on the laser positioning pose estimation value, is combined with the absolute position constraint term, constructed based on the static semantic map, to jointly optimize the robot pose and obtain the joint estimated pose.

3. The robot autonomous exploration and navigation method integrating visual augmentation localization and multimodal terrain analysis according to claim 2, characterized in that, The process of dynamically and accurately removing targets based on the standardized point cloud data and the pixel-level semantic mask to obtain clean, static laser point cloud data free from dynamic interference includes: Based on the three-dimensional coordinates of each laser point in the standardized point cloud data, they are projected onto the pixel coordinate system of the standardized image data to obtain the projected pixel coordinates of each laser point. Based on the projected pixel coordinates and the pixel-level semantic mask, laser points located within the dynamic object region are selected from the standardized point cloud data to obtain a candidate dynamic laser point group. Based on the depth value of each laser point in the candidate dynamic laser point group and the depth distribution characteristics of its neighborhood, the depth consistency of the candidate dynamic laser point group is checked to obtain the depth check results of each laser point. Based on the depth verification results, laser points identified as dynamic occlusion points in the candidate dynamic laser point group are removed, while laser points identified as background structures are retained, resulting in pure static laser point cloud data without dynamic interference.

4. The robot autonomous exploration and navigation method integrating visual augmentation localization and multimodal terrain analysis according to claim 2, characterized in that, The construction of a static semantic map containing semantic attributes based on the image static identifiers and the clean static laser point cloud data includes: Based on the image static identifier combined with the preset static semantic landmark category whitelist, static pixel regions are obtained. Then, based on the two-dimensional feature points of the static pixel regions and the pure static laser point cloud data, ray projection calculation is performed to obtain the three-dimensional observation coordinates of the initial static semantic landmark. Based on the three-dimensional observation coordinates and a preset Bayesian probability model, the corresponding existence confidence level is determined, and the high-confidence candidate landmark group is obtained by filtering based on the existence confidence level and a preset confidence threshold. Based on the pose change and information entropy of the target robot in the current frame, it is determined whether the current frame meets the keyframe insertion condition. If it does, the high-confidence candidate landmark group is projected onto the global coordinate system to obtain the global coordinate estimate. The global coordinate estimate is then associated with the existing landmarks in the initial semantic map to obtain the association result. The initial semantic map is the static semantic map that the target robot has completed in the previous moment. Incremental registration is performed based on the association results to obtain the current semantic map containing temporal information. Based on the multiple observation records of each landmark in the current semantic map under different keyframes, a multi-view co-view relationship graph is constructed. Based on the multi-view co-view relationship graph, the spatial consistency of the three-dimensional world coordinates of the landmarks in the current semantic map at the current moment is checked to obtain the check result. Based on the check result, the landmarks in the current semantic map are corrected to obtain a geometrically accurate static semantic landmark set. The static semantic map is obtained by performing global joint optimization based on the static semantic landmark set and the laser positioning pose estimation value.

5. The robot autonomous exploration and navigation method integrating visual augmented localization and multimodal terrain analysis according to claim 4, characterized in that, The process of performing global joint optimization based on the static semantic landmark set and the laser positioning pose estimation value to obtain the static semantic map includes: Based on the static semantic landmark set and the laser positioning pose estimation value, the static semantic landmark set as an absolute position constraint factor and the laser positioning pose estimation value as a relative pose constraint factor are introduced into a preset factor graph optimization framework to construct a global factor graph model containing robot pose nodes, laser odometry edges, and semantic landmark observation edges. Based on the global factor graph model, a global solution optimization is performed to obtain the keyframe pose sequence and the static semantic landmark coordinate set. Map reconstruction is performed based on the keyframe pose sequence and the static semantic landmark coordinate set to obtain the static semantic map at the current moment.

6. The robot autonomous exploration and navigation method integrating visual augmentation localization and multimodal terrain analysis according to claim 1, characterized in that, The step of performing terrain analysis and path planning based on the jointly estimated pose, the environmental point cloud map, and the static semantic map to obtain the target motion path includes: Based on the joint estimated pose, the current position of the target robot is determined. The front edge points are extracted by combining the distribution differences between the scanned and occupied space voxels and the unscanned free space voxels in the environmental point cloud map, so as to obtain a set of environmental front edge points that represent the boundaries of known free areas and unknown areas. Based on the rate of change of the normal vector of each environmental front point in the environmental point cloud map and the plane fitting tilt angle of its neighboring points, the surface roughness cost and slope cost corresponding to each environmental front point are determined. Based on the surface roughness cost and slope cost, geometric threshold filtering is performed by combining the preset maximum allowable roughness threshold and maximum allowable slope threshold to obtain a group of candidate frontier points that meet the geometric passability standard. Based on the spatial coordinates of each candidate front point in the candidate front point group, the landmark semantic category label and surface material attribute label stored in the corresponding location and its neighborhood range are retrieved in the static semantic map. The landmark semantic category label and surface material attribute label of each candidate front point are valued based on the preset semantic exploration reward rules to obtain the semantic gain value that represents the exploration priority. A comprehensive cost evaluation is performed based on the surface roughness cost, the slope cost, and the semantic gain value to obtain the comprehensive exploration cost of each candidate frontier point. The frontier points are then sorted in ascending order based on the comprehensive exploration cost, and the frontier point with the smallest comprehensive exploration cost is determined as the target exploration point of the target robot. Based on the joint estimated pose, the current position of the target robot and the target position of the target exploration point are determined. Global path search is performed by combining the obstacle pixel distribution information in the environmental point cloud map to obtain a global collision-free path. Based on the global collision-free path and the preset dynamic obstacle avoidance strategy for the target robot to perceive local obstacles in real time, the target motion path for the target robot to move is generated.

7. The robot autonomous exploration and navigation method integrating visual augmented localization and multimodal terrain analysis according to claim 1, characterized in that, The process of performing spatiotemporal calibration and standardization on the laser point cloud data, the visual image data, and the inertial measurement data to obtain standardized point cloud data and standardized image data includes: Interpolation and alignment are performed based on the timestamp information of the laser point cloud data, the visual image data, and the inertial measurement data to obtain the initial laser point cloud data, initial visual image data, and initial inertial measurement data after time synchronization. Based on the angular velocity and acceleration data in the initial inertial measurement data, the instantaneous motion state data of the robot within a single scanning cycle of the lidar is obtained. Based on the instantaneous motion state data of the robot, the acquisition time position of each laser point in the initial laser point cloud data is reversed and the laser point cloud compensation data after motion distortion compensation is obtained. Based on the lidar coordinate system where the laser point cloud compensation data is located and the camera coordinate system where the initial visual image data is located, a coordinate transformation model is constructed in combination with a preset sensor extrinsic parameter matrix. Based on the coordinate transformation model, the laser point cloud compensation data and the initial visual image data are projected onto a preset global unified reference coordinate system to obtain unified laser point cloud data and unified visual image data under the unified coordinate system. Based on the unified laser point cloud data and the unified visual image data, they are encapsulated after being associated with the corresponding timestamps and spatial metadata to obtain the standardized point cloud data and the standardized image data.

8. A robot autonomous exploration and navigation system integrating visual augmented localization and multimodal terrain analysis, characterized in that, The robot autonomous exploration and navigation method that integrates visual augmented localization and multimodal terrain analysis as described in any one of claims 1 to 7; The robot autonomous exploration and navigation system that integrates visual augmentation localization and multimodal terrain analysis includes: The data acquisition module is used to acquire multi-source heterogeneous data of the target robot in the current dynamic environment; the multi-source heterogeneous data includes laser point cloud data, visual image data and inertial measurement data; The spatiotemporal calibration and processing module is used to perform spatiotemporal calibration and standardization processing based on the laser point cloud data, the visual image data and the inertial measurement data to obtain standardized point cloud data and standardized image data. The localization and map building module is used to perform fusion localization and dual map construction based on the standardized point cloud data and standardized image data to obtain joint estimated pose, environmental point cloud map and static semantic map. The terrain analysis and exploration decision module is used to perform terrain analysis and path planning based on the joint estimated pose, the environmental point cloud map and the static semantic map, to obtain the target motion path, and to drive the target robot to perform exploration and navigation actions along the planned path based on the target motion path.

9. An electronic device, characterized in that, include: Memory, used to store computer software programs; A processor is configured to read and execute the computer software program, wherein when the processor executes the computer software program, it implements the robot autonomous exploration and navigation method that integrates visual augmented localization and multimodal terrain analysis as described in any one of claims 1 to 7.

10. A non-transitory computer-readable storage medium, characterized in that, The storage medium stores a computer software program, which, when executed by a processor, implements the robot autonomous exploration and navigation method that integrates visual augmented localization and multimodal terrain analysis as described in any one of claims 1 to 7.

Images