Real-time path data planning and processing method and system based on SLAM

Abstract

The present application relates to the field of path planning technology, and discloses a real-time path data planning and processing method and system based on SLAM. The method includes: performing semantic-geometric joint feature extraction through a lidar and an RGB camera to obtain joint observation data; performing covariance propagation on the SLAM state vector based on a semantically constrained Kalman filter to obtain positioning covariance and covariance ellipse; performing constraint propagation on a set of semantic objects to generate virtual feature points to construct an enhanced feature map; performing environmental grid safety scoring based on semantic risk weights and obstacle distances; and performing semantic cost and SLAM uncertainty processing through a bidirectional A* search to generate a real-time navigation path. The present application solves the technical problem of insufficient comprehensive consideration of semantic safety constraints and positioning uncertainty in real-time path planning based on SLAM in a feature-sparse environment.

Description

Technical Field

[0001] The present application relates to the field of path planning technology, and in particular to a real-time path data planning and processing method and system based on SLAM. Background Art

[0002] Existing SLAM technologies primarily use geometric features for environmental mapping and robot positioning. Using lidar or cameras to acquire geometric information such as points, lines, and surfaces in the environment, they construct a map and simultaneously estimate the robot's position and posture within the map. Traditional SLAM methods typically use occupancy grid maps to represent the environment for path planning, and employ algorithms such as A*, RRT, or artificial potential field methods for path search. These methods primarily rely on geometric obstacle information for obstacle avoidance and achieve good positioning and path planning results in feature-rich environments.

[0003] However, existing technologies have significant shortcomings in feature-sparse environments (such as long corridors, empty warehouses, and monotonous and repetitive scenes): First, pure geometric feature extraction methods cannot obtain sufficient positioning reference points in feature-sparse environments, resulting in positioning drift and cumulative errors in the SLAM system; second, traditional path planning methods only consider geometric obstacles and ignore the semantic safety information in the environment, and are unable to identify and avoid semantic risk areas such as no-entry signs and fragile objects; third, existing methods lack consideration of SLAM positioning uncertainty, and still plan according to a deterministic path when the positioning accuracy is low, which poses a risk of path execution failure.

[0004] Based on the above analysis, the core technical problem facing existing technologies is: how to enhance the feature density of SLAM systems by fusing semantic information with geometric constraints in feature-sparse environments, while also comprehensively considering semantic safety constraints and SLAM positioning uncertainty in path planning to achieve stable and reliable real-time navigation. Specifically, it is necessary to solve the problems of joint observation data fusion of semantic objects and geometric features, the generation of virtual feature points based on building structure constraints, the comprehensive evaluation of semantic risk assessment and SLAM uncertainty, and the problem of efficient path search considering multiple constraints. Summary of the Invention

[0005] The present application provides a SLAM-based real-time path data planning and processing method and system for solving the technical problem of insufficient comprehensive consideration of semantic safety constraints and positioning uncertainty in SLAM-based real-time path planning in a feature-sparse environment.

[0006] In the first aspect, the present application provides a real-time path data planning and processing method based on SLAM, which includes: performing semantic-geometric joint feature extraction processing on a feature-sparse environment through a lidar and an RGB camera to obtain a geometric feature set, a semantic object set and joint observation data; performing covariance propagation processing on the SLAM state vector through a semantic constrained Kalman filter according to the joint observation data to obtain a positioning covariance and a covariance ellipse; performing constraint propagation processing on the semantic object set to obtain virtual feature points based on parallelism constraints and perpendicularity constraints, and constructing an enhanced feature map with the geometric feature set; performing safety scoring processing on the environmental grid according to the semantic risk weight and obstacle distance to obtain a passability score that integrates the covariance ellipse penalty; performing semantic cost and SLAM uncertainty processing on the path nodes through a bidirectional A* search to obtain a real-time navigation path based on the enhanced feature map.

[0007] In a second aspect, the present application provides a real-time path data planning and processing system based on SLAM, the real-time path data planning and processing system based on SLAM comprising:

[0008] The extraction module is used to perform semantic-geometric joint feature extraction on feature-sparse environments using lidar and RGB cameras to obtain geometric feature sets, semantic object sets, and joint observation data;

[0009] A propagation module is used to perform covariance propagation processing on the SLAM state vector through a semantically constrained Kalman filter according to the joint observation data to obtain a positioning covariance and a covariance ellipse;

[0010] A construction module is used to perform constraint propagation processing on the semantic object set to obtain virtual feature points based on parallelism constraints and perpendicularity constraints, and construct an enhanced feature map with the geometric feature set;

[0011] A scoring module is used to perform safety scoring on the environment grid according to the semantic risk weight and the obstacle distance, and obtain a passability score that integrates the covariance ellipse penalty;

[0012] The navigation module is used to perform semantic cost and SLAM uncertainty processing on path nodes through bidirectional A* search to obtain a real-time navigation path based on the enhanced feature map.

[0013] In the third aspect, a real-time path data planning and processing device based on SLAM is provided, comprising: a memory and at least one processor, wherein the memory stores instructions; the at least one processor calls the instructions in the memory so that the real-time path data planning and processing device based on SLAM executes the above-mentioned real-time path data planning and processing method based on SLAM.

[0014] In a fourth aspect, a computer-readable storage medium is provided, wherein instructions are stored in the computer-readable storage medium, which, when executed on a computer, enables the computer to execute the above-mentioned SLAM-based real-time path data planning and processing method.

[0015] The technical solution provided in this application overcomes the problem of insufficient positioning accuracy caused by traditional SLAM methods relying solely on geometric features in feature-sparse environments through semantic-geometric joint feature extraction processing. The collaborative operation of LiDAR and RGB cameras enables the system to simultaneously obtain the environment's geometric structure information and semantic object information, forming a complementary observation data source and significantly enhancing the robustness of feature extraction. The semantically constrained Kalman filter uses covariance propagation processing on the SLAM state vector to accurately quantify positioning uncertainty by integrating the confidence weights of semantic objects. The generation of covariance ellipses provides clear uncertainty boundary constraints for subsequent path planning. Constraint propagation processing utilizes prior knowledge of building structure to derive virtual feature points from limited observations, effectively solving the problem of insufficient available positioning reference points in feature-sparse environments. The enhanced feature map construction significantly improves the positioning stability of the SLAM system in monotonously repetitive scenes. The semantic safety scoring processing of the environment grid combines semantic risk weights with geometric obstacle distances, achieving a significant extension of traditional path planning methods, enabling robots to identify and avoid semantic risk areas. The introduction of a covariance ellipse penalty mechanism ensures that path planning can adapt to dynamic changes in SLAM positioning accuracy.

[0016] The bidirectional A-search algorithm, supported by a semantic cost function and a SLAM uncertainty heuristic function, achieves efficient real-time path generation. By simultaneously searching from both the starting and target points, the algorithm significantly reduces the search space and improves the real-time performance of path planning. Specifically for SLAM-based real-time path data planning and processing applications, the introduction of the semantically weighted bidirectional A-search algorithm enables path search to simultaneously optimize path length, semantic safety, and SLAM positioning reliability. The semantically constrained Kalman filter algorithm is characterized by its ability to dynamically adjust the fusion weights of geometric and semantic features. When semantic object detection confidence is high, its contribution to SLAM state estimation is increased, while when geometric feature matching quality is good, the weight of geometric observations is increased. This adaptive weight adjustment mechanism significantly improves the localization accuracy of SLAM systems in complex environments. The constraint propagation algorithm, by leveraging prior knowledge of building structures, such as parallelism and perpendicularity constraints, can derive virtual feature points even when observations are insufficient, enabling the system to maintain stable localization and path planning capabilities in scenarios where traditional methods fail. BRIEF DESCRIPTION OF THE DRAWINGS

[0017] In order to more clearly illustrate the technical solutions of the embodiments of the present invention, the following briefly introduces the drawings required for use in the description of the embodiments. Obviously, the drawings described below are some embodiments of the present invention. For ordinary technicians in this field, other drawings can be obtained based on these drawings without creative work.

[0018] Figure 1 Schematic diagram of an embodiment of a real-time path data planning and processing method based on SLAM in an embodiment of the present application;

[0019] Figure 2 Schematic diagram of an embodiment of a real-time path data planning and processing system based on SLAM in an embodiment of the present application;

[0020] Figure 3 It is a structural schematic block diagram of a real-time path data planning and processing device based on SLAM in an embodiment of the present invention. DETAILED DESCRIPTION

[0021] The embodiments of the present application provide a real-time path data planning and processing method and system based on SLAM. The terms "first", "second", "third", "fourth", etc. (if any) in the specification and claims of this application and the above-mentioned drawings are used to distinguish similar objects and are not necessarily used to describe a specific order or sequence. It should be understood that the data used in this way can be interchangeable where appropriate so that the embodiments described herein can be implemented in an order other than that illustrated or described herein. In addition, the terms "including" or "having" and any variations thereof are intended to cover non-exclusive inclusions, for example, a process, method, system, product or device that includes a series of steps or units is not necessarily limited to those steps or units clearly listed, but may include other steps or units that are not clearly listed or inherent to these processes, methods, products or devices.

[0022] For ease of understanding, the specific process of the embodiment of the present application is described below. Figure 1 In the embodiment of the present application, an embodiment of the real-time path data planning and processing method based on SLAM includes:

[0023] Step S101: Perform semantic-geometric joint feature extraction on a feature-sparse environment using a laser radar and an RGB camera to obtain a geometric feature set, a semantic object set, and joint observation data;

[0024] Step S102: performing covariance propagation processing on the SLAM state vector through semantically constrained Kalman filtering according to the joint observation data to obtain positioning covariance and covariance ellipse;

[0025] Step S103: performing constraint propagation processing on the semantic object set to obtain virtual feature points based on parallelism constraints and perpendicularity constraints, and constructing an enhanced feature map with the geometric feature set;

[0026] Step S104: Perform safety scoring on the environment grid according to the semantic risk weight and obstacle distance to obtain a passability score fused with covariance ellipse penalty;

[0027] Step S105: Perform semantic cost and SLAM uncertainty processing on the path nodes through bidirectional A* search to obtain a real-time navigation path based on the enhanced feature map.

[0028] It is understandable that the execution subject of this application can be a real-time path data planning and processing system based on SLAM, or a terminal or a server, which is not limited here. The embodiment of this application is described by taking the server as the execution subject as an example.

[0029] Specifically, environmental point cloud data is collected by lidar, and image information is obtained by RGB camera. The lidar data is subjected to the RANSAC algorithm to extract line segment features and corner point features to form a geometric feature set, where line segment features are obtained by fitting collinear points in the point cloud, and corner point features are calculated by calculating the intersection of adjacent line segments. The RGB image data is input into the deep learning semantic segmentation network to identify stable objects such as door frames, signboards, and structural edges to form a semantic object set. Each semantic object is assigned a weight value according to the detection confidence, and the geometric feature set and the semantic object set are merged to form joint observation data.

[0030] The semantically constrained Kalman filter algorithm processes the SLAM state vector, which contains the robot's position coordinates, orientation angle, and environmental features. The prediction phase calculates the predicted state value based on the robot's motion model. The update phase uses joint observation data to correct the prediction result. The uncertainty of the semantic observation is dynamically adjusted based on the confidence weight, while the uncertainty of the geometric observation is determined based on the quality of feature matching. During the Kalman filter process, the covariance matrix reflects the positioning uncertainty. The covariance ellipse is calculated using the eigenvalues ​​and eigenvectors of the covariance matrix. The lengths of the major and minor axes of the ellipse represent the positioning error range in different directions. The semantic object set is input into the constraint propagation algorithm. The parallelism constraint utilizes the parallel relationship between the walls on both sides of the corridor. When a line segment on one wall is detected, the position of the opposite wall is inferred based on the parallelism constraint. The perpendicularity constraint utilizes the perpendicular relationship between the wall and the ground. The side position of the door frame is inferred from the top and bottom edges of the door frame. During the constraint propagation process, the coordinates of virtual feature points are calculated using the geometric constraint equations. These virtual feature points are merged with the original geometric feature set to form an enhanced feature map. The enhanced feature density is significantly higher than the original geometric feature distribution.

[0031] The environment is divided into grids, and each grid cell is assigned a semantic safety score and a geometric passability score. The semantic safety score is calculated based on nearby semantic risk objects. Risk objects such as no-entry signs and fragile items are assigned different weights. The distance from the grid to the risk object is calculated using the Euclidean distance; the closer the distance, the lower the safety score. The geometric passability score is calculated based on the distance from the grid center to the nearest obstacle. The robot radius is used as a safety threshold in the score calculation. A penalty factor is applied to grid cells outside the covariance ellipse to reduce the passability score. The comprehensive score is obtained by the weighted sum of the semantic safety score and the geometric passability score.

[0032] The bidirectional A* search algorithm starts searching from the starting point and the target point at the same time. The forward search maintains an open list extending from the starting point, and the reverse search maintains an open list extending from the target point. The cost function of the node consists of three parts: actual cost, heuristic cost, and semantic cost. The actual cost is calculated by accumulating the path length. The heuristic cost uses the Euclidean distance to estimate the distance to the target point. The semantic cost is calculated based on the semantic risk weight around the node. SLAM uncertainty affects the heuristic cost through the trace value of the covariance ellipse. When the node distance of the forward and reverse searches is less than the grid resolution and the covariance ellipses overlap, the path merging is triggered. The final output navigation path is smoothed to eliminate the jagged effect caused by the discrete grid.

[0033] In a specific embodiment, the process of executing step S101 may specifically include the following steps:

[0034] The point cloud data is obtained by LiDAR and RANSAC feature extraction is performed to obtain a geometric feature set consisting of line segment features and corner point features;

[0035] The image data obtained by the RGB camera is processed through deep learning semantic segmentation to obtain a set of semantic objects consisting of door frames, signboards, and structural edges;

[0036] Perform spatial coordinate transformation on line segment features and corner point features to obtain the geometric feature observation Jacobian matrix;

[0037] Extract the position information of door frames, signboards and structure edges to obtain the semantic object observation Jacobian matrix;

[0038] The geometric feature observation Jacobian matrix and the semantic object observation Jacobian matrix are fused to obtain joint observation data including line segment features, corner point features and semantic object sets.

[0039] Specifically, when the lidar obtains point cloud data for RANSAC feature extraction processing, the lidar sensor emits a laser beam to the environment and receives the reflected laser signal, records the three-dimensional coordinate position and reflection intensity value of each laser point, and forms a dense point cloud data set. The RANSAC algorithm, as a random sampling consistency algorithm, fits the geometric model by randomly selecting the minimum sample set in the point cloud, and then counts the number of inliers that conform to the model. The process is repeated until the optimal model parameters are found. Specifically, when extracting line segment features, the RANSAC algorithm randomly selects two point cloud points to form a straight line model, calculates the distance from other points to the straight line, and points with distances less than the set threshold are considered to be inliers. The straight line model with the largest number of inliers is the extracted line segment feature. The corner point feature is obtained by detecting the intersection position of adjacent line segment features. Each line segment feature records the starting point coordinates, end point coordinates and direction vector, and the corner point feature records the two-dimensional coordinate position of the intersection. When the RGB camera obtains image data for deep learning semantic segmentation processing, the camera collects color image data of the environment. The image is stored in the form of a pixel matrix, and each pixel contains the values ​​of the three color channels of red, green and blue. After receiving the input image, the deep learning semantic segmentation network extracts image features through the convolution layer, reduces the feature map resolution through the pooling layer, and restores the image size through the upsampling layer. Finally, the semantic category label of each pixel is output. The door frame semantic object is obtained by identifying the rectangular border structure and specific texture feature detection, including the pixel coordinates of the four corner points of the door frame and the center position of the door frame. The signboard semantic object is identified through text area detection and shape analysis, and the bounding box coordinates and text content type of the signboard are recorded. The structural edge semantic object is obtained through edge detection algorithm and geometric shape analysis, including the starting and ending pixel coordinates of the edge segment.

[0040] When performing spatial coordinate conversion on line segment features and corner point features, the feature point coordinates in the laser radar coordinate system are converted to the robot body coordinate system. The conversion process requires the installation position and posture parameters of the laser radar relative to the robot, including the translation vector and rotation matrix. The geometric feature observation Jacobian matrix describes the partial derivative relationship of the geometric feature observation value relative to the robot state variable. The number of rows of the matrix is ​​equal to the dimension of the geometric feature observation, and the number of columns is equal to the dimension of the robot state vector. The observation value of the line segment feature includes the distance and angle parameters of the line segment, and the observation value of the corner point feature includes the distance and azimuth of the corner point. Each element of the Jacobian matrix is ​​calculated by taking the partial derivative of the observation model function, reflecting the degree of influence of the robot position and posture changes on the geometric feature observation.

[0041] When extracting position information of door frames, signboards, and structural edges, the pixel coordinates are first converted into three-dimensional coordinates in the camera coordinate system. The conversion process requires the camera's intrinsic parameter matrix and depth information. The intrinsic parameter matrix contains the camera focal length and principal point coordinates. The depth information is obtained through stereo vision or RGB-D camera. The semantic object observation Jacobian matrix describes the sensitivity of the semantic object observation value to the robot state. Door frame observations include the distance and azimuth angle of the door frame center, signboard observations include the distance and relative orientation angle of the signboard, and structural edge observations include the endpoint coordinates and normal vector direction of the edge segment. The observation Jacobian matrix of each semantic object is obtained by taking the partial derivative of the corresponding observation model.

[0042] When the geometric feature observation Jacobian matrix and the semantic object observation Jacobian matrix are subjected to data fusion processing, the two matrices are spliced ​​row by row to form a joint observation Jacobian matrix. The splicing order is arranged according to the timestamp of the observation data, with the geometric feature observation data in front and the semantic object observation data in the back. The joint observation data structure contains the coordinate information of the geometric feature, the position information of the semantic object and the corresponding observation uncertainty. The uncertainty of the geometric feature is determined by the laser ranging accuracy and angular resolution, and the uncertainty of the semantic object is calculated based on the confidence of the deep learning network and the pixel positioning accuracy. The timestamp also needs to be synchronized during the data fusion process to ensure the temporal consistency of the geometric feature and semantic object observation data.

[0043] In a specific embodiment, the process of executing step S102 may specifically include the following steps:

[0044] The robot's posture information is combined with the geometric feature set and the semantic object set to construct a state vector to obtain a SLAM state vector containing position, posture and environmental features;

[0045] Perform motion model prediction processing on the SLAM state vector to obtain the state prediction value and the corresponding state prediction covariance matrix;

[0046] The observation model is used to calculate the joint observation data and the state prediction value to obtain the observation residual vector and the innovation covariance matrix;

[0047] The Kalman gain is calculated based on the geometric feature observation Jacobian matrix and the semantic object observation Jacobian matrix to obtain the filter gain matrix that integrates the semantic weights.

[0048] The state prediction covariance matrix is ​​updated with the filter gain matrix to obtain the positioning covariance that characterizes the robot's position uncertainty and the covariance ellipse used for path constraint.

[0049] Specifically, when the robot pose information is processed with the geometric feature set and the semantic object set to construct the state vector, the two-dimensional position coordinates and orientation angle of the robot are used as the first three state variables, and each line segment feature and corner feature in the geometric feature set are added to the state vector with their coordinate parameters respectively. The door frame, signboard and structure edge in the semantic object set are extended with their position information and attribute parameters respectively. The SLAM state vector is represented as a high-dimensional vector containing the robot pose, all geometric feature positions and semantic object positions, where the robot pose part contains the x-coordinate, y-coordinate and orientation angle theta, the geometric feature part contains the start and end point coordinates of each line segment and the xy coordinates of each corner point, and the semantic object part contains the coordinates of the four corner points of the door frame, the center position of the signboard and the end point coordinates of the structure edge. The dimension of the state vector is equal to the robot pose dimension plus the sum of the coordinate dimensions of all features.

[0050] When the SLAM state vector is processed by motion model prediction, the state prediction value at the next moment is calculated based on the robot's motion control input. The motion model describes the state transfer relationship of the robot under a given control input. For wheeled robots, the motion model is established based on kinematic constraints and includes control inputs of linear velocity and angular velocity. The state prediction value is calculated by combining the current state with the motion model. The predicted value of the robot's posture is updated according to the kinematic equation, and the predicted value of the environmental feature remains unchanged. The state prediction covariance matrix describes the uncertainty of the state prediction. The diagonal elements of the covariance matrix represent the variance of each state variable, and the non-diagonal elements represent the covariance between different state variables. The noise during the motion process is added to the state prediction covariance matrix through the process noise covariance matrix. When the observation data and state prediction values ​​are combined for observation model calculation and processing, the observation model describes the functional relationship between the sensor observation value and the robot state. The observation model of geometric features converts the world coordinates of line segment features and corner point features into observation values ​​in the sensor coordinate system. The observation model of semantic objects converts the world coordinates of door frames, signboards and structural edges into pixel coordinates in the camera coordinate system. The observation residual vector is calculated by subtracting the predicted observation value of the observation model from the actual observation value. Each element of the residual vector represents the prediction error of the corresponding observation quantity. The innovative covariance matrix describes the statistical characteristics of the observation residual. The matrix calculation involves matrix operations on the observation Jacobian matrix, the state prediction covariance matrix and the observation noise covariance matrix.

[0051] When the Kalman gain is calculated for the Jacobian matrix of geometric feature observations and the Jacobian matrix of semantic object observations, the Kalman gain matrix describes the degree of dependence of state estimation on observation information. The calculation of the gain matrix involves inverse matrix operations of the state prediction covariance matrix, the observation Jacobian matrix and the innovation covariance matrix. The number of rows of the filter gain matrix is ​​equal to the dimension of the state vector, and the number of columns is equal to the dimension of the observation vector. The numerical size of the matrix elements reflects the sensitivity of each state variable to each observation. Semantic weight fusion is achieved by adjusting the noise covariance of semantic object observations. Observations with high semantic confidence are assigned smaller noise variance, and observations with low semantic confidence are assigned larger noise variance, thereby automatically adjusting the relative weights of geometric features and semantic object observations in the Kalman gain calculation.

[0052] When the filtering gain matrix updates the state prediction covariance matrix, the state posterior covariance matrix is ​​calculated by subtracting the product of the gain matrix and the observation Jacobian matrix from the identity matrix, and then multiplying it with the state prediction covariance matrix. The positioning covariance is obtained by extracting the submatrix corresponding to the robot posture from the state posterior covariance matrix. The covariance ellipse is calculated by eigenvalue decomposition of the positioning covariance matrix. The major and minor axis lengths of the ellipse correspond to the square roots of the maximum and minimum eigenvalues ​​of the covariance matrix, respectively. The tilt angle of the ellipse corresponds to the direction of the eigenvector corresponding to the maximum eigenvalue. The boundary coordinates of the covariance ellipse are calculated using the ellipse parametric equation. The area inside the ellipse represents the high-probability range of the robot position.

[0053] Taking a logistics warehouse environment as an example, the robot's current position is in the center of the warehouse aisle, with its heading angle pointing toward the shelf. The SLAM state vector contains the robot's xy coordinates and heading angle, as well as the line segment feature coordinates of the shelf edges on both sides of the aisle and the corner feature coordinates of the shelf corners. It also contains the coordinates of the four corner points of the shelf entrance door frame and the center position of the shelf identification plate. Based on the robot's forward command, the motion model predicts that the robot will move a specific distance forward along the aisle at the next moment. The predicted position of the environmental features remains unchanged, but the state prediction covariance matrix reflects the accumulated uncertainty during the motion. The joint observation data includes the shelf edge distance angle information detected by the lidar and the door frame pixel coordinates captured by the camera. The observation residual vector shows the difference between the lidar observation and the predicted observation, as well as the deviation between the door frame pixel coordinates and the predicted pixel coordinates. In the calculation of the Kalman gain matrix, the door frame observation is given greater weight due to its higher semantic confidence. The updated localization covariance matrix after filtering shows that the robot's position uncertainty has decreased compared to the prediction stage. The major axis of the covariance ellipse is along the aisle direction, and the minor axis is perpendicular to the aisle direction. The ellipse boundary defines the credible range of the robot's position.

[0054] In a specific embodiment, the process of executing step S103 may specifically include the following steps:

[0055] Perform geometric structure analysis on the door frame in the semantic object set to obtain the door frame size data and the coordinate positions of the door frame corners;

[0056] Parallelism calculation is performed based on line segment features to obtain the wall normal vector and parallelism constraint;

[0057] The coordinates of the door frame corner points are geometrically deduced from the vertical relationship between the door frame corner points and the wall surface to obtain the virtual corner point coordinates that meet the verticality constraint.

[0058] Based on the parallelism constraint, the line segment features are symmetrically mapped to obtain the spatial coordinates of the virtual feature points;

[0059] The virtual corner point coordinates and the spatial coordinates of the virtual feature points are merged with the geometric feature set to obtain an enhanced feature map.

[0060] Specifically, when the door frame in the semantic object set is subjected to geometric structure analysis and processing, the door frame pixel area output by the deep learning semantic segmentation network is used to extract the four boundary lines of the door frame through the edge detection algorithm. The door frame size data is obtained by calculating the pixel length of the horizontal boundary line and the pixel length of the vertical boundary line. The pixel size is converted into the actual physical size in combination with the camera intrinsic parameter matrix and the depth information. The coordinate position of the door frame corner point is obtained by detecting the intersection of the four boundary lines, including the pixel coordinates of the upper left corner point, the upper right corner point, the lower left corner point and the lower right corner point. These pixel coordinates are converted into three-dimensional coordinate positions in the world coordinate system through camera coordinate transformation and depth data. The geometric structure analysis of the door frame also includes calculating the center position coordinates of the door frame and the normal vector direction of the door frame plane. The normal vector is obtained by the cross product operation of the upper edge vector and the side edge vector of the door frame. When calculating the parallelism of line segment features, the direction vectors of all line segment features are extracted from the geometric feature set. The direction vector is calculated by subtracting the end point coordinates from the starting point coordinates of the line segment. The parallelism calculation is achieved by comparing the angles between the direction vectors of different line segments. The angle is calculated by dividing the dot product of the two direction vectors by the product of the vector moduli. When the angle is less than the set parallelism threshold, the two line segments are considered parallel. The wall normal vector is calculated by the perpendicular direction of the parallel line segments. Specifically, the direction vector of the parallel line segment is rotated 90 degrees to obtain the perpendicular vector, which is the wall normal vector. The parallelism constraint is expressed as the distance between the parallel line segments remains constant. The constraint condition is verified by calculating the distance from any point on the parallel line segment to the opposite parallel line segment. The distance calculation is implemented using the point-to-line distance formula.

[0061] When geometrically deducing the relationship between the door frame corner coordinates and the wall's perpendicularity, the geometric constraint relationship is established using the prior knowledge of the door frame and the wall's perpendicularity in the building structure. The perpendicularity constraint requires that the dot product of the door frame's side direction vector and the wall's normal vector be zero. The geometric derivation process calculates the unobserved door frame side corner coordinates using the known door frame's upper and lower corner coordinates and the perpendicularity constraint. In the derivation, it is first determined that the door frame's side direction vector must be perpendicular to the wall's normal vector. Then, the specific coordinate positions of the side corners are calculated based on the door frame's standard geometric size constraints. The virtual corner coordinates are calculated by adding or subtracting the door frame size's projection vector in the side direction from the known corner coordinates. The length of the projection vector is equal to the standard width or height of the door frame.

[0062] When parallelism constraint is used to perform symmetrical mapping on line segment features, the line segment features of one side of the wall detected in the environment are selected as the reference line segment, and the virtual line segment features of the opposite wall are generated at the relative position according to the parallelism constraint. The symmetrical mapping calculation is achieved by translating the feature points on the reference line segment by a fixed distance along the wall normal vector direction. The translation distance is equal to the standard width of the corridor or passage. The starting coordinates of the virtual line segment are calculated by adding the translation vector to the starting coordinates of the reference line segment, and the end coordinates of the virtual line segment are calculated by adding the same translation vector to the end coordinates of the reference line segment. The spatial coordinates of the virtual feature points include the endpoint coordinates of the virtual line segment and the coordinates of the key points on the virtual line segment. The key point coordinates are generated by sampling the virtual line segment at equal intervals, and the sampling interval is determined according to the density of the original line segment features. When the virtual corner point coordinates and the spatial coordinates of the virtual feature points are merged with the geometric feature set, the virtual feature points are evaluated for confidence. The confidence is calculated based on the semantic object confidence and geometric constraint satisfaction degree used when generating the virtual feature points. Virtual feature points with confidence higher than the threshold are added to the enhanced feature map. During the data merging process, the virtual feature points use the same data structure format as the original geometric features, including coordinate information, feature type identification and confidence weight. The data structure of the enhanced feature map is realized by expanding the storage space of the original geometric feature set. The newly added virtual feature points are inserted into the corresponding positions of the feature map in the order of spatial position. The index structure of the feature map is updated at the same time to support fast search and access of virtual feature points.

[0063] In a specific embodiment, the process of executing step S104 may specifically include the following steps:

[0064] Divide the environment space into grids to obtain an array of environment grid cells with a fixed size;

[0065] Risk assessment is performed based on the no-entry signs and fragile items areas in the semantic object set to obtain the semantic risk weight corresponding to each grid unit;

[0066] Perform distance field calculation on the environment grid cell array based on the obstacle position information to obtain the obstacle distance representing the safe passage distance;

[0067] The semantic risk weight and obstacle distance are weighted and fused to obtain a comprehensive safety score;

[0068] The SLAM uncertainty penalty is applied to the comprehensive safety score based on the boundary of the covariance ellipse to obtain the passability score.

[0069] Specifically, when the environment space is gridded, the fixed size of the grid unit is determined according to the robot's motion accuracy requirements and computing resource limitations, and is usually set to a square grid of 0.1 meter by 0.1 meter. The grid division starts from the minimum boundary coordinates of the environment, and grid units are created in the x and y directions in sequence according to a fixed step size. Each grid unit contains the center point coordinates, boundary coordinates and an index number. The environment grid unit array is represented as a two-dimensional array structure. The row index of the array corresponds to the grid number in the y direction, and the column index corresponds to the grid number in the x direction. The total number of grid units is equal to the environment length divided by the grid size multiplied by the environment width divided by the grid size. When each grid unit is initialized, storage space is allocated to record various score values ​​for subsequent calculations.

[0070] When performing risk assessment on no-entry signs and fragile items areas in a semantic object set, the position coordinates and influence range parameters of each semantic object are first extracted. The influence range of the no-entry sign is set as a circular area with the center of the sign as the center, and the influence range of the fragile items area is set as a rectangular area with a certain safety distance extending from the item boundary. Risk assessment is achieved by calculating the distance from the center point of each grid unit to each risk source. The distance calculation uses the Euclidean distance formula. When the center point of a grid is within the influence range of a risk source, the grid is marked as a risk grid. The semantic risk weight is assigned a value based on the type of risk source and the distance. The no-entry sign is assigned the highest risk weight, the fragile items area is assigned a medium risk weight, the closer the grid is to the risk source, the higher the risk weight is assigned, and the risk weight of the grid beyond the influence range is set to zero. When performing distance field calculations on the array of environmental grid cells based on obstacle location information, the distance field algorithm calculates the shortest distance from each grid cell to the nearest obstacle. The obstacle location information comes from the point cloud data detected by the lidar and the obstacle boundaries identified by semantic segmentation. The distance field calculation uses a breadth-first search algorithm. First, the distance values ​​of all grid cells occupied by obstacles are set to zero and added to the search queue. Then, the grid cells in the queue are processed in sequence, and the distance values ​​of their adjacent grid cells are calculated. The distance value of the adjacent grid is equal to the current grid distance value plus the step distance between grids. During the search process, each grid cell records the distance value to the nearest obstacle. The obstacle distance represents the safe passage distance for that grid position. The larger the distance value, the farther away from the obstacle, and the safer it is.

[0071] When the semantic risk weight and obstacle distance are weightedly fused, the comprehensive safety score is calculated by adding the semantic safety score and the geometric safety score according to the preset weight ratio. The semantic safety score is equal to the unit value minus the semantic risk weight. The geometric safety score is calculated by dividing the obstacle distance by the robot's safety radius. When the obstacle distance is greater than the safety radius, the geometric safety score is set to unit value. The weight of the semantic safety score in the weighted fusion is set to 0.6, and the weight of the geometric safety score is set to 0.4. The setting of the weight ratio highlights the importance of semantic safety information in path planning. The numerical range of the comprehensive safety score is limited to between zero and one. The closer the value is to one, the safer the grid position is.

[0072] When the boundary of the covariance ellipse performs SLAM uncertainty penalty processing on the comprehensive safety score, the covariance ellipse represents the uncertainty range of the robot's current position, the area inside the ellipse represents the high probability range of the robot's position, and the area outside the ellipse represents the low probability range of the robot's position. The SLAM uncertainty penalty mechanism imposes a penalty factor on the grid cells outside the ellipse. The penalty factor is calculated based on the distance from the grid center point to the ellipse boundary. The farther the grid is from the ellipse boundary, the greater the penalty. The penalty processing is achieved by multiplying the comprehensive safety score by the penalty factor. The value of the penalty factor is less than one, which makes the passability score of the grid outside the ellipse relatively lower. The passability score is normalized to ensure that the value range is between zero and one.

[0073] In a specific embodiment, the process of executing step S105 may specifically include the following steps:

[0074] The boundary range is calculated based on the ellipse parameters of the covariance ellipse to obtain the ellipse boundary coordinates that represent the SLAM positioning uncertainty area;

[0075] The position relationship between each grid center point and the ellipse boundary coordinates in the environmental grid cell array is judged and processed to obtain the distribution state inside and outside the grid;

[0076] The penalty factor is calculated for the grid cells outside the ellipse boundary coordinates to obtain the SLAM uncertainty penalty coefficient based on distance attenuation;

[0077] The SLAM uncertainty penalty coefficient is multiplied by the comprehensive safety score to obtain the corrected grid safety score;

[0078] The revised grid safety score is normalized to obtain a range-limited accessibility score.

[0079] Specifically, when the ellipse parameters of the covariance ellipse are used for boundary range calculation, the ellipse parameters include the ellipse center coordinates, major axis length, minor axis length and tilt angle. The ellipse center coordinates correspond to the current estimated position of the robot. The major axis and minor axis lengths are calculated through the eigenvalues ​​of the covariance matrix. The tilt angle is determined by the direction of the eigenvector of the covariance matrix. The calculation of the ellipse boundary coordinates adopts the ellipse parametric equation. The angle parameter in the parametric equation varies from zero to twice pi. Each angle corresponds to the coordinates of an ellipse boundary point. The boundary coordinate calculation involves the ellipse center coordinates plus the coordinate components in the major axis direction and the minor axis direction. The coordinate components are calculated by multiplying the ellipse radius by the unit vector in the corresponding direction. The ellipse boundary coordinates represent the spatial range of the SLAM positioning uncertainty area. The inside of the ellipse represents the high confidence area of ​​the robot position, and the outside of the ellipse represents the low confidence area of ​​the robot position.

[0080] When the positional relationship between each grid center point and the ellipse boundary coordinates in the environmental grid cell array is judged, the positional relationship judgment adopts the ellipse inside-outside judgment algorithm. The judgment algorithm substitutes the grid center point coordinates into the standard equation of the ellipse. The standard equation of the ellipse is calculated by projecting the offset of the coordinates relative to the ellipse center to the main axis direction of the ellipse. The projection result is divided by the radius length of the corresponding axis and squared. If the sum of the squares in the two directions is less than one, it means that the point is inside the ellipse. If the sum of the squares is greater than one, it means that the point is outside the ellipse. If the sum of the squares is equal to one, it means that the point is on the ellipse boundary. The grid inside-outside distribution status is recorded by assigning a Boolean flag to each grid cell. The internal grid flag is set to true and the external grid flag is set to false. The distribution status data is stored in a two-dimensional Boolean array of the same size as the grid cell array.

[0081] When calculating the penalty factor for grid cells located outside the ellipse boundary coordinates, the penalty factor is calculated based on the shortest distance from the grid center point to the ellipse boundary. The distance calculation uses a numerical method to find the point on the ellipse boundary with the shortest distance from the grid center point. The shortest distance is equal to the Euclidean distance of the grid center point coordinates minus the coordinates of the nearest boundary point. The SLAM uncertainty penalty coefficient based on distance attenuation is calculated using an exponential decay function. The larger the distance in the attenuation function, the smaller the penalty coefficient. The attenuation parameter controls the decay rate of the penalty intensity. The numerical range of the penalty coefficient is limited to between 0.2 and 1. The penalty coefficient of the grid cells inside the ellipse is set to 1, indicating that no penalty is imposed. The grid cells outside the ellipse are assigned different penalty coefficients according to the distance. The farther the grid is from the ellipse boundary, the stronger the penalty is.

[0082] When the SLAM uncertainty penalty coefficient is multiplied by the comprehensive safety score, the product operation is performed on each grid cell separately, and the result of the operation is equal to the comprehensive safety score multiplied by the corresponding penalty coefficient. The revised grid safety score reflects the impact of SLAM positioning uncertainty on path planning. The safety score of the grid inside the ellipse remains the original value, and the safety score of the grid outside the ellipse is relatively reduced. The degree of reduction is proportional to the distance from the grid to the ellipse boundary. The data processing of the product operation is realized by traversing the grid cell array. The comprehensive safety score and penalty coefficient of a grid are read in each iteration. After performing the multiplication operation, the result is stored back to the corresponding grid position. The revised safety score data overwrites the original comprehensive safety score data.

[0083] When the revised grid safety score is normalized, the normalization algorithm first traverses all grid cells to find the maximum and minimum safety scores. Then, the minimum safety score of each grid cell is subtracted and divided by the difference between the maximum and minimum values. The normalization formula maps the original score to a standard range of zero to one, where zero represents the least safe grid location and one represents the safest grid location. The range-limited accessibility score is normalized to ensure numerical consistency and comparability. The normalized score data is directly used in the subsequent path search algorithm, which selects path nodes based on the accessibility score. Grids with high scores are given priority as path points, and grids with low scores are avoided as much as possible.

[0084] In a specific embodiment, the process of executing step S106 may specifically include the following steps:

[0085] Perform forward search and reverse search initialization processing in the grid of accessibility scores based on the starting point and the target point to obtain a forward open list and a reverse open list;

[0086] The passability score and the path node distance are processed by semantic cost function calculation to obtain the node cost value that integrates semantic security information;

[0087] Based on the positioning covariance, the SLAM uncertainty heuristic function is calculated for the path nodes to obtain the heuristic cost value considering the positioning accuracy;

[0088] The node cost value and the heuristic cost value are comprehensively evaluated to obtain the node priority ranking of the bidirectional search;

[0089] When nodes in the forward open list and the reverse open list meet, path merging is performed to obtain a real-time navigation path based on the enhanced feature map.

[0090] Specifically, when the starting point and the target point are initialized for forward search and reverse search in the grid of accessibility score, the bidirectional A* search algorithm expands the search tree from the starting point and the target point at the same time. The forward search initialization adds the grid coordinates of the starting point to the forward open list, and the reverse search initialization adds the grid coordinates of the target point to the reverse open list. The open list is a priority queue data structure, which is sorted according to the comprehensive cost value of the nodes, and the nodes with low cost value are processed first. The forward open list stores the candidate nodes extended from the starting point to the target point, and the reverse open list stores the candidate nodes extended from the target point to the starting point. During the initialization process, the initial cost values ​​of the starting point and the target point are set to zero. The two open lists maintain their respective search states, including the set of visited nodes and the parent-child relationship between nodes. The search process alternates between processing the nodes in the two open lists until the search areas of both parties meet. When the semantic cost function is used to calculate the accessibility score and the distance between path nodes, the semantic cost function integrates the accessibility score of the grid and the movement distance between nodes. The node cost calculation includes the cumulative cost from the starting point to the current node and the semantic penalty cost of the current node. The cumulative cost is calculated by multiplying the path length by the movement cost coefficient. The movement cost coefficient is set according to the accessibility score of the adjacent grid. The grid with low accessibility score is assigned a high movement cost, and the grid with high accessibility score is assigned a low movement cost. The semantic penalty cost directly uses the complement of the accessibility score of the current grid, that is, the unit value minus the accessibility score. The node cost value that integrates semantic security information is calculated by adding the cumulative cost and the semantic penalty cost. This cost value reflects the comprehensive safety and movement efficiency of the path node. The lower the cost value, the more suitable the node is as a path point.

[0091] When the positioning covariance performs SLAM uncertainty heuristic function calculation on the path nodes, the heuristic function estimates the remaining cost from the current node to the target point. The traditional heuristic function uses Euclidean distance or Manhattan distance. The SLAM uncertainty heuristic function adds a positioning accuracy correction term based on the distance. The correction term is calculated according to the position of the current node relative to the SLAM covariance ellipse. The correction term of the node inside the ellipse is zero, and the correction term of the node outside the ellipse is determined according to the distance to the ellipse boundary. The farther from the ellipse, the larger the correction term. The heuristic cost value considering positioning accuracy is equal to the basic distance cost multiplied by the positioning uncertainty correction coefficient. A correction coefficient greater than one indicates an increase in the heuristic estimate, guiding the search to avoid areas with high SLAM uncertainty. A correction coefficient equal to one indicates that the original heuristic estimate is maintained.

[0092] When the node cost value and the heuristic cost value are comprehensively evaluated, the comprehensive cost function of the bidirectional search includes the actual cost, the heuristic cost and the bidirectional search balance term. The actual cost uses the node cost value calculated above, and the heuristic cost uses the heuristic cost value corrected by SLAM uncertainty. The bidirectional search balance term ensures the expansion balance of the forward search and the reverse search. The balance term is calculated by comparing the progress of the two search directions. The search direction with slower progress obtains a lower balance term value, and the search direction with faster progress obtains a higher balance term value. The node priority ranking is determined according to the size of the comprehensive cost value. Nodes with low comprehensive cost value obtain high priority and are taken out of the open list first for processing. The sorting mechanism ensures that the search algorithm prioritizes exploring low-cost and safe path directions.

[0093] When nodes in the forward open list and the reverse open list meet and path merging is performed, the node encounter is determined by checking whether the node expanded by the forward search is located in the same grid or adjacent grids as the node expanded by the reverse search. The encounter condition also includes that the overlap degree of the SLAM covariance ellipse of the two nodes meets the threshold requirement. The path merging process starts from the encounter node, backtracks to the starting point along the parent-child relationship chain of the forward search, and backtracks to the target point along the parent-child relationship chain of the reverse search. The two path segments are connected at the encounter node to form a complete path. The real-time navigation path based on the enhanced feature map contains all grid coordinate sequences from the starting point to the target point. Each coordinate point in the path corresponds to a valid navigation point in the enhanced feature map. After the path is generated, it is smoothed to eliminate the jagged effect caused by grid discretization. The smoothing algorithm reduces the number of path turning points while maintaining path safety.

[0094] The above describes the real-time path data planning and processing method based on SLAM in the embodiment of the present application. The following describes the real-time path data planning and processing system based on SLAM in the embodiment of the present application. Figure 2 In the embodiment of the present application, an embodiment of a real-time path data planning and processing system based on SLAM includes:

[0095] The extraction module is used to perform semantic-geometric joint feature extraction on feature-sparse environments using lidar and RGB cameras to obtain geometric feature sets, semantic object sets, and joint observation data;

[0096] A propagation module is used to perform covariance propagation processing on the SLAM state vector through a semantically constrained Kalman filter according to the joint observation data to obtain a positioning covariance and a covariance ellipse;

[0097] A construction module is used to perform constraint propagation processing on the semantic object set to obtain virtual feature points based on parallelism constraints and perpendicularity constraints, and construct an enhanced feature map with the geometric feature set;

[0098] A scoring module is used to perform safety scoring on the environment grid according to the semantic risk weight and the obstacle distance, and obtain a passability score that integrates the covariance ellipse penalty;

[0099] The navigation module is used to perform semantic cost and SLAM uncertainty processing on path nodes through bidirectional A* search to obtain a real-time navigation path based on the enhanced feature map.

[0100] above Figure 2 The real-time path data planning and processing system based on SLAM in the embodiment of the present invention is described in detail from the perspective of modular functional entities. The real-time path data planning and processing device based on SLAM in the embodiment of the present invention is described in detail from the perspective of hardware processing.

[0101] Reference Figure 3 In the embodiment of the present invention, a real-time path data planning and processing device based on SLAM is also provided. The real-time path data planning and processing device based on SLAM can be a server, and its internal structure can be as follows: Figure 3 As shown. The real-time path data planning and processing device based on SLAM includes a processor, a memory, a display screen, an input device, a network interface and a database connected via a system bus. Among them, the computer-designed processor is used to provide computing and control capabilities. The memory of the real-time path data planning and processing device based on SLAM includes a non-volatile storage medium and an internal memory. The non-volatile storage medium stores an operating system, a computer program and a database. The internal memory provides an environment for the operation of the operating system and the computer program in the non-volatile storage medium. The database of the real-time path data planning and processing device based on SLAM is used to store the corresponding data in this embodiment. The network interface of the real-time path data planning and processing device based on SLAM is used to communicate with an external terminal via a network connection. When the computer program is executed by the processor, the above method is implemented.

[0102] Those skilled in the art will understand that Figure 3 The structure shown in the figure is merely a block diagram of a portion of the structure related to the solution of the present invention, and does not constitute a limitation on the SLAM-based real-time path data planning and processing device to which the solution of the present invention is applied.

[0103] The present invention also provides a computer-readable storage medium, which can be a non-volatile computer-readable storage medium or a volatile computer-readable storage medium. The computer-readable storage medium stores instructions, and when the instructions are run on a computer, the computer executes the steps of the SLAM-based real-time path data planning and processing method.

[0104] Those skilled in the art will clearly understand that, for the convenience and brevity of description, the specific working processes of the above-described systems, systems and units can refer to the corresponding processes in the aforementioned method embodiments and will not be repeated here.

[0105] If the integrated unit is implemented in the form of a software functional unit and sold or used as an independent product, it can be stored in a computer-readable storage medium. Based on this understanding, the technical solution of the present invention, or the part that contributes to the prior art, or all or part of the technical solution can be embodied in the form of a software product. The computer software product is stored in a storage medium and includes a number of instructions for enabling a SLAM-based real-time path data planning and processing device (which can be a personal computer, server, or network device, etc.) to execute all or part of the steps of the method described in each embodiment of the present invention. The aforementioned storage medium includes: U disk, mobile hard disk, read-only memory (ROM), random access memory (RAM), disk or optical disk, and other media that can store program code.

[0106] The above embodiments are only used to illustrate the technical solutions of the present invention, rather than to limit the same. Although the present invention has been described in detail with reference to the aforementioned embodiments, those skilled in the art should understand that they can still modify the technical solutions described in the aforementioned embodiments, or make equivalent replacements for some of the technical features therein. However, these modifications or replacements do not deviate the essence of the corresponding technical solutions from the spirit and scope of the technical solutions of the various embodiments of the present invention.

Claims

1. A real-time path data planning and processing method based on SLAM, characterized in that: The method comprises: Using LiDAR and RGB cameras to perform semantic-geometric joint feature extraction on feature-sparse environments, we obtain geometric feature sets, semantic object sets, and joint observation data. Performing covariance propagation processing on the SLAM state vector through semantically constrained Kalman filtering according to the joint observation data to obtain positioning covariance and covariance ellipse; Performing constraint propagation processing on the semantic object set to obtain virtual feature points based on parallelism constraints and perpendicularity constraints, and constructing an enhanced feature map with the geometric feature set; Performing safety scoring on the environment grid according to the semantic risk weight and the obstacle distance to obtain a passability score that incorporates the covariance ellipse penalty; The semantic cost and SLAM uncertainty of the path nodes are processed by bidirectional A* search to obtain a real-time navigation path based on the enhanced feature map.

2. The real-time path data planning and processing method based on SLAM according to claim 1, wherein The method of performing semantic-geometric joint feature extraction on a feature-sparse environment using a lidar and an RGB camera to obtain a geometric feature set, a semantic object set, and joint observation data includes: Obtaining point cloud data through a laser radar and performing RANSAC feature extraction processing to obtain the geometric feature set consisting of line segment features and corner point features; Acquire image data through an RGB camera and perform deep learning semantic segmentation processing to obtain the semantic object set consisting of door frames, signboards, and structure edges; Performing spatial coordinate transformation on the line segment features and the corner point features to obtain a geometric feature observation Jacobian matrix; Performing position information extraction processing on the door frame, the signboard, and the structure edge to obtain a semantic object observation Jacobian matrix; The geometric feature observation Jacobian matrix and the semantic object observation Jacobian matrix are subjected to data fusion processing to obtain the joint observation data including the line segment features, the corner point features and the semantic object set.

3. The real-time path data planning processing method based on SLAM according to claim 2, characterized in that, The method of performing covariance propagation processing on the SLAM state vector by using semantically constrained Kalman filtering according to the joint observation data to obtain positioning covariance and covariance ellipse includes: Performing state vector construction processing on the robot posture information, the geometric feature set, and the semantic object set to obtain the SLAM state vector including position, posture, and environmental features; Performing motion model prediction processing on the SLAM state vector to obtain a state prediction value and a corresponding state prediction covariance matrix; Perform observation model calculation processing on the joint observation data and the state prediction value to obtain an observation residual vector and an innovation covariance matrix; Performing Kalman gain calculation based on the geometric feature observation Jacobian matrix and the semantic object observation Jacobian matrix to obtain a filter gain matrix fused with semantic weights; The state prediction covariance matrix is ​​updated using the filter gain matrix to obtain the positioning covariance that characterizes the robot position uncertainty and the covariance ellipse used for path constraint.

4. The real-time path data planning and processing method based on SLAM according to claim 3 is characterized in that, The step of performing constraint propagation processing on the semantic object set to obtain virtual feature points based on parallelism constraints and perpendicularity constraints, and constructing an enhanced feature map with the geometric feature set includes: Performing geometric structure analysis on the door frame in the semantic object set to obtain door frame size data and door frame corner point coordinates; Performing parallelism calculation based on the line segment features to obtain a wall normal vector and the parallelism constraint; Performing geometric deduction on the vertical relationship between the door frame corner coordinate position and the wall surface to obtain virtual corner coordinates that meet the verticality constraint; Performing symmetric mapping processing on the line segment features based on the parallelism constraint to obtain the spatial coordinates of the virtual feature points; The virtual corner point coordinates and the spatial coordinates of the virtual feature points are combined with the geometric feature set to obtain the enhanced feature map.

5. The real-time path data planning and processing method based on SLAM according to claim 1 is characterized in that, The safety scoring process of the environment grid according to the semantic risk weight and the obstacle distance is performed to obtain a passability score integrated with the covariance ellipse penalty, including: Divide the environment space into grids to obtain an array of environment grid cells with a fixed size; Perform risk assessment based on the no-entry signs and fragile items areas in the semantic object set to obtain the semantic risk weight corresponding to each grid unit; Performing distance field calculation processing on the environment grid cell array according to the obstacle position information to obtain the obstacle distance representing the safe passage distance; Performing weighted fusion processing on the semantic risk weight and the obstacle distance to obtain a comprehensive safety score; The comprehensive safety score is subjected to SLAM uncertainty penalty processing based on the boundary of the covariance ellipse to obtain the passability score.

6. The real-time path data planning and processing method based on SLAM according to claim 5, characterized in that: The performing SLAM uncertainty penalty processing on the comprehensive safety score based on the boundary of the covariance ellipse to obtain the passability score includes: Perform boundary range calculation based on the ellipse parameters of the covariance ellipse to obtain ellipse boundary coordinates representing the SLAM positioning uncertainty area; Performing positional relationship determination processing on each grid center point in the environmental grid unit array and the ellipse boundary coordinates to obtain a distribution state inside and outside the grid; Performing penalty factor calculation on the grid cells outside the ellipse boundary coordinates to obtain a SLAM uncertainty penalty coefficient based on distance attenuation; Performing a product operation on the SLAM uncertainty penalty coefficient and the comprehensive safety score to obtain a revised grid safety score; The modified grid security score is normalized to obtain the range-limited accessibility score.

7. The real-time path data planning and processing method based on SLAM according to claim 1, characterized in that: The method of performing semantic cost and SLAM uncertainty processing on path nodes through bidirectional A* search to obtain a real-time navigation path based on the enhanced feature map includes: Performing forward search and reverse search initialization processing in the grid of the accessibility score based on the starting point and the target point to obtain a forward open list and a reverse open list; The passability score and the path node distance are processed by semantic cost function calculation to obtain the node cost value of the integrated semantic security information; Performing SLAM uncertainty heuristic function calculation processing on the path nodes based on the positioning covariance to obtain a heuristic cost value considering positioning accuracy; Performing a comprehensive cost evaluation on the node cost value and the heuristic cost value to obtain a node priority ranking for a bidirectional search; When nodes in the forward open list and the reverse open list meet, a path merging process is performed to obtain the real-time navigation path based on the enhanced feature map.

8. A real-time path data planning and processing system based on SLAM, characterized in that: For implementing the SLAM-based real-time path data planning and processing method according to any one of claims 1 to 7, the SLAM-based real-time path data planning and processing system comprises: The extraction module is used to perform semantic-geometric joint feature extraction on feature-sparse environments using lidar and RGB cameras to obtain geometric feature sets, semantic object sets, and joint observation data; A propagation module is used to perform covariance propagation processing on the SLAM state vector through a semantically constrained Kalman filter according to the joint observation data to obtain a positioning covariance and a covariance ellipse; A construction module is used to perform constraint propagation processing on the semantic object set to obtain virtual feature points based on parallelism constraints and perpendicularity constraints, and construct an enhanced feature map with the geometric feature set; A scoring module is used to perform safety scoring on the environment grid according to the semantic risk weight and the obstacle distance, and obtain a passability score that integrates the covariance ellipse penalty; The navigation module is used to perform semantic cost and SLAM uncertainty processing on path nodes through bidirectional A* search to obtain a real-time navigation path based on the enhanced feature map.

9. A real-time path data planning and processing device based on SLAM, characterized in that: The method comprises a memory and a processor, wherein the memory stores a computer program that can be run on the processor, and when the processor executes the computer program, the method for real-time path data planning and processing based on SLAM according to any one of claims 1 to 7 is implemented.

10. A computer-readable storage medium having a computer program stored thereon, characterized in that: When the computer program is executed by a processor, the processor executes the SLAM-based real-time path data planning processing method according to any one of claims 1 to 7.

Images