Semantic map construction method, apparatus and system for robot navigation

Abstract

The present disclosure provides a semantic map construction method, device and system for robot navigation. The method of the present disclosure embodiment can include: geometric-topological SLAM incremental updating, sparse imaging evidence generation, parsing availability field calculation, formalized semantic constraint compilation, incremental satisfiability solving and minimum conflict processing, logic entropy driven active exploration and update change arbitration and audit book maintenance; wherein the incremental satisfiability solving and minimum conflict processing includes: performing incremental satisfiability solving on the formalized semantic constraint set, if unsatisfiability occurs, locating the minimum unsatisfiable subset, approximately determining the minimum constraint set according to the rollback set and performing the minimum rollback processing to obtain the constraint solving output result, the constraint solving output result includes: the consistency solution space generated in the current period and the logic entropy value. The present disclosure can effectively avoid the accumulation of errors or logical contradictions of semantic information in long-term operation, and ensure the stable evolution of the semantic map.

Description

TECHNICAL FIELD

[0001] The present disclosure relates to the technical field of robots, and more particularly, to a semantic map construction method, device and system for robot navigation. BACKGROUND

[0002] With the continuous expansion of the application of mobile robots in service, inspection and logistics scenarios, the environment mapping technology relied on by robot autonomous navigation has gradually developed from the geometric level of simultaneous localization and mapping (SLAM) to the semantic map construction stage containing semantic information. The existing semantic map construction method assigns a class label to the area or object in the map through image recognition, semantic segmentation or target detection technology on the basis of the geometric map, so that the robot can understand the environment at a higher level.

[0003] However, the existing technology updates the semantic map in a batch or global reconstruction manner, and when the environment state changes or new observation data appears, it often needs to be recalculated or optimized as a whole, resulting in large system operation load, update lag, and easy to produce semantic inconsistency in long-time running, leading to the problems of discontinuous semantic update, logic conflict cannot be repaired and incomplete map expression in the existing technology. SUMMARY

[0004] To solve the above technical problems, the embodiments of the present disclosure provide a semantic map construction method, device and system for robot navigation.

[0005] According to one aspect of the present disclosure, a semantic map construction method for robot navigation is provided, comprising:

[0006] Geometric-topological SLAM incremental update, the geometric-topological SLAM incremental update comprising: obtaining the mileage information and sensor observation data of the robot, updating the geometric map and topological structure information of the environment where the robot is currently located based on the sensor observation data and the mileage information, to obtain voxel occupancy grid and topological graph structure data;

[0007] Sparse imaging evidence generation, the sparse imaging evidence generation comprising: performing non-negative sparse reconstruction based on the wireless signal of the wireless transceiver unit and / or the acoustic signal of the acoustic transceiver unit to obtain a sparse intensity vector, projecting the sparse intensity vector to the world coordinate system through the extrinsic parameters of the wireless transceiver unit and / or the acoustic transceiver unit to form a sparse imaging evidence heat map, the sparse imaging evidence heat map containing the sparse intensity normalized value and its coordinate index of each voxel;

[0008] analyzing the availability field computation, the analyzing the availability field computation comprising: computing a plurality of availability parameter fields related to robot size, kinematic and dynamic constraints based on robot structure parameters, the sparse imaging evidence heat map, the voxel occupancy grid and the topological graph structure data, the plurality of availability parameter fields including a passable availability field, a parking availability field and a pass-through opening availability value;

[0009] formal semantic constraint compilation, the formal semantic constraint compilation comprising: transforming the voxel occupancy grid, the topological graph structure data, the plurality of availability parameter fields, geometric information, topological information and availability information in the sparse imaging evidence heat map into a set of formal semantic constraints;

[0010] incremental satisfiability solving and minimal conflict handling, the incremental satisfiability solving and minimal conflict handling comprising: performing incremental satisfiability solving on the set of formal semantic constraints, locating a minimal unsatisfiable subset if unsatisfiability occurs, determining a minimal constraint set and performing a minimal rollback handling according to a rollback set approximation to obtain a constraint solving output result, the constraint solving output result including: a consistency solution space generated in a current period and a logic entropy value, each element in the consistency solution space containing a set of logic variable assignments and a corresponding constraint satisfaction state;

[0011] logic entropy driven active exploration, the logic entropy driven active exploration comprising: generating an observation action sequence based on the consistency solution space generated in a current period and the logic entropy value;

[0012] update change arbitration and audit ledger maintenance, the update change arbitration and audit ledger maintenance comprising: recording historical data of constraint changes and solving rollbacks.

[0013] In some embodiments, the sparse imaging evidence generation comprises: constructing a measurement equation wherein, is an observation vector, is a measurement matrix composed of sensor array geometry and propagation path, is a non-negative sparse intensity vector, is a noise term; performing non-negative sparse reconstruction with norm minimization to obtain the sparse intensity vector; mapping the sparse intensity vector to a voxel grid and generating a sparse imaging evidence heat map by extrinsic parameters of the wireless transceiver unit and / or the acoustic transceiver unit; normalizing fusion of the sparse imaging evidence heat map and the voxel occupancy grid at a voxel layer to obtain an opening existence probability.

[0014] In some embodiments, the geometry-topology SLAM incremental update further comprises: performing probabilistic normalized fusion between the voxel occupancy grid and the sparse imaging evidence heat map at the voxel layer to update the opening existence probability; and generating a topology edge between two free space connected components according to the updated opening existence probability.

[0015] In some embodiments, the formalized semantic constraint compilation comprises: generating a door-room connection hard constraint, constructing a connectivity constraint with a topology connection relationship between an opening candidate object and two different room candidate objects as a variable; generating an opening size hard constraint, constructing a threshold constraint with a net width and a minimum net width of the opening candidate object and a net height and a minimum net height of the opening candidate object as variables; generating a room compactness soft constraint, constructing an upper limit constraint with a ratio of eigenvalue of inertia principal axis of the room candidate object as a variable; and generating a corridor elongation soft constraint, constructing a lower limit constraint with a ratio of eigenvalue of inertia principal axis of the corridor candidate object as a variable.

[0016] In some embodiments, the formalized semantic constraint compilation further comprises: generating an evidence consistency soft constraint for the opening candidate object, constructing a proportion threshold constraint with a proportion of high response voxels of the sparse imaging evidence heat map in a coverage area of the opening candidate object as a variable, and constructing an adjacency relationship constraint with an intersection relationship between a neighborhood of the opening candidate object and free space boundaries of two room candidate objects as a variable.

[0017] In some embodiments, the incremental satisfiability solving and minimal conflict processing comprises: constructing an incremental satisfiability problem model for the hard constraint and the soft constraint with a relaxation variable; performing local re-solving for an affected variable block after a new constraint is added; when an unsatisfiability is detected, calculating a minimal unsatisfiable subset, and calculating a rollback set approximation according to the minimal unsatisfiable subset to obtain a minimal constraint set to be removed, performing a minimal rollback processing according to the minimal constraint set, and re-solving.

[0018] In some embodiments, the logic entropy driven active exploration comprises: generating a candidate action set based on a consistency solution space generated in the current period and a logic entropy value, enumerating observations and constructing a satisfiable solution space at a next time according to a forward measurement model; performing a cardinality estimation on the satisfiable solution space and calculating a logic entropy; calculating an expected value of the logic entropy for each candidate action, selecting a candidate action with a minimum expected value of the logic entropy and performing the candidate action, generating the observation action sequence, and triggering a next round of cycle of the geometry-topology SLAM incremental update and the sparse imaging evidence heat map generation.

[0019] In some embodiments, the analytical availability field calculation comprises:

[0020] Free-space erosion-dilation is performed to obtain the net width of the passage, and the ratio of the net width of the passage to the robot width is calculated. The local slope is estimated by the ground normal and the difference is calculated with the maximum allowable slope. The curvature of the passage is obtained by fitting the centerline of the passage and the difference is calculated with the curvature corresponding to the minimum turning radius. The ratio of the net width of the passage to the robot width, the difference between the local slope and the maximum allowable slope, and the difference between the curvature of the passage and the curvature corresponding to the minimum turning radius are normalized and the minimum value is taken as the passability availability field value of the voxel.

[0021] The ratio of vertical clearance to minimum clearance, the difference between surface inclination angle and friction cone angle, and the flatness criterion are calculated for the waiting platform surface. The flatness criterion is applied and binarized. The minimum value of the ratio of vertical clearance to minimum clearance, the difference between surface inclination angle and friction cone angle, and the flatness criterion are taken as the docking availability field value after binarization and normalization.

[0022] For each candidate object for an opening, calculate the ratio of the net width to the minimum net width, the ratio of the net height to the minimum net height, and the ratio of the free voxel density on both sides to the reference density. After normalizing these ratios, take the minimum value as the availability value for opening.

[0023] According to one aspect of this disclosure, a semantic map building apparatus for robot navigation is provided, comprising:

[0024] The incremental update module is used to acquire the robot's mileage information and sensor observation data, and update the geometric map and topology information of the robot's current environment based on the sensor observation data and mileage information to obtain voxel occupancy grid and topology structure data.

[0025] A sparse imaging evidence generation module is used to perform non-negative sparse reconstruction based on the wireless signal of the wireless transceiver unit and / or the acoustic signal of the acoustic transceiver unit to obtain a sparse intensity vector, and project the sparse intensity vector onto the world coordinate system through the extrinsic parameters of the wireless transceiver unit and / or the acoustic transceiver unit to form a sparse imaging evidence heatmap. The sparse imaging evidence heatmap contains the sparse intensity normalized value of each voxel and its coordinate index.

[0026] The availability field calculation module is used to calculate various availability parameter fields related to robot size, kinematics and dynamic constraints based on robot structural parameters, the sparse imaging evidence heatmap, the voxel occupancy grid and the topology graph structure data. The various availability parameter fields include the passability availability field, the docking availability field and the access availability value.

[0027] a formalized semantic constraint compilation module for converting the voxel occupancy grid, the topological graph structure data, the plurality of availability parameter fields, geometric information, topological information and availability information in the sparse imaging evidence heat map into a set of formalized semantic constraints;

[0028] a solving and conflict handling module for performing incremental satisfiability solving on the set of formalized semantic constraints, locating a minimal unsatisfiable subset if unsatisfiability occurs, approximately determining a minimal constraint set according to a rollback set and performing a minimal rollback operation to obtain a constraint solving output result, the constraint solving output result including a consistency solution space generated in a current period and a logic entropy value, each element in the consistency solution space containing a set of logic variable assignments and a corresponding constraint satisfaction state;

[0029] an active exploration module for generating an observation action sequence based on the consistency solution space generated in the current period and the logic entropy value;

[0030] a ledger maintenance module for recording historical data of constraint changes and solving rollback operations.

[0031] According to an aspect of the present disclosure, there is provided a semantic map construction system for robot navigation, the system comprising a sensor group, an antenna transceiver unit, an acoustic transceiver unit and a computing device, the computing device comprising a processor and a memory, the memory having stored therein a computer program which, when executed by the processor, causes the processor to perform the method described above.

[0032] By introducing an incremental satisfiability solving and minimal conflict rollback mechanism, the present embodiment of the present disclosure realizes the maintenance of logical consistency of the semantic map in the multi-period updating process. When new observations or new constraints are introduced to cause the system to be unsatisfiable, the minimal conflict subset can be automatically identified and a rollback operation can be performed, thereby avoiding the accumulation of errors or logical contradictions in the semantic information in long-term operation, and thus ensuring the stable evolution and self-correction ability of the semantic map. BRIEF DESCRIPTION OF DRAWINGS

[0033] Figure 1 is a flowchart of a semantic map construction method for robot navigation provided by the present embodiment of the present disclosure;

[0034] Figure 2 is a structural schematic diagram of a semantic map construction apparatus for robot navigation provided by the present embodiment of the present disclosure;

[0035] Figure 3 is a schematic structural block diagram of a semantic map construction system for robot navigation provided by the present embodiment of the present disclosure. DETAILED DESCRIPTION

[0036] Hereinafter, example embodiments according to the present disclosure will be described in detail with reference to the accompanying drawings. Obviously, the described embodiments are only a part of the embodiments of the present disclosure, and not all of the embodiments of the present disclosure, and it should be understood that the present disclosure is not limited to the example embodiments described herein.

[0037] As described previously, there are problems such as semantic update discontinuity, logical conflict cannot be repaired, and map expression is incomplete in semantic map construction in the prior art. Specifically, the existing semantic map construction method generally has problems such as accumulation of logical conflicts and difficulty in maintaining semantic consistency in a multi-period, long-term running environment. When there is a contradiction between new observation data and existing semantic labels, the system handles it through simple weight update or confidence re-estimation, but this method cannot effectively identify and eliminate logical conflicts at the semantic level, resulting in semantic drift, object attribute duplication or constraint conflict in the semantic map, and further causing problems such as semantic update discontinuity, logical conflict cannot be repaired, and map expression is incomplete, which seriously affects the reliability of subsequent navigation and task planning.

[0038] The embodiments of the present disclosure are applicable to robot platforms with mobility, environmental perception and computing capabilities, including but not limited to wheeled robots, tracked robots, legged robots, hybrid mobile platforms and autonomous vehicles. The foregoing robots are configured with a computing unit, a ranging sensor, a vision sensor, a wireless transceiver unit, an acoustic transceiver unit and a positioning device, for continuously acquiring geometric and topological information in the environment, and performing incremental update of the map in combination with semantic constraints.

[0039] The symbols and their meanings involved in the embodiments of the present disclosure are as follows:

[0040] : pose of the robot at the moment;

[0041] : voxel occupancy grid matrix, also referred to as voxel occupancy grid;

[0042] A: measurement matrix, determined by sensor array geometry and propagation path;

[0043] : non-negative sparse intensity vector, representing the reflection intensity or radiation intensity of the spatial system;

[0044] : observation vector;

[0045] : noise vector;

[0046] : passable availability field:

[0047] : parking availability field;

[0048] : opening availability value;

[0049] : formal semantic constraint set;

[0050] : logical entropy function, representing an uncertainty measure of the solution space;

[0051] : represents a free space set, containing all passable regions;

[0052] : constraint updated solution space change amount.

[0053] Figure 1 A flowchart of a semantic map construction method for robot navigation provided by an embodiment of the present disclosure is shown. The overall running logic of the method of the present embodiment is that: a multi-source data channel is established through a physical sensor, and through analysis, compilation and logical solving of geometric and semantic information, continuous incremental construction and maintenance of a semantic map is realized. The entire process is repeatedly executed in a periodic form during the running of the robot, and the input of each cycle includes current observation data and historical constraint state, and the output includes updated geometric and semantic information.

[0054] Referring to Figure 1 , the semantic map construction method for robot navigation of the present embodiment includes the following steps:

[0055] Step 101, geometric-topological SLAM incremental update, which includes: obtaining the mileage information and sensor observation data of the robot, updating the geometric map and topological structure information of the environment where the robot is currently located based on the sensor observation data and the mileage information, to obtain voxel occupancy grid and topological graph structure data;

[0056] Step 102, sparse imaging evidence generation, which includes: performing non-negative sparse reconstruction based on the wireless signal of the wireless transceiver unit and / or the acoustic signal of the acoustic transceiver unit to obtain a sparse intensity vector, and projecting the sparse intensity vector to the world coordinate system through the extrinsic parameters of the wireless transceiver unit and / or the acoustic transceiver unit to form a sparse imaging evidence heat map, which contains the sparse intensity normalized value and the coordinate index of each voxel.

[0057] At step 103, the availability field calculation is parsed, which includes calculating a plurality of availability parameter fields related to robot size, kinematic and dynamic constraints based on robot structure parameters, sparse imaging evidence heat map, voxel occupancy grid and topological graph structure data, the plurality of availability parameter fields including passable availability field, parking availability field and pass-through opening availability value.

[0058] At step 104, the formalized semantic constraint compilation is performed, which includes converting geometric information, topological information and availability information in the voxel occupancy grid, topological graph structure data, a plurality of availability parameter fields, sparse imaging evidence heat map into a set of formalized semantic constraints.

[0059] At step 105, the incremental satisfiability solving and minimal conflict processing are performed, which includes performing incremental satisfiability solving on the set of formalized semantic constraints, locating a minimal unsatisfiable subset if unsatisfiability occurs, and performing a minimal rollback processing to obtain a constraint solving output result according to a rollback set approximation of the minimal constraint set, the constraint solving output result including a consistency solution space generated in the current period and a logical entropy value, each element in the consistency solution space containing a set of logical variable assignments and a corresponding constraint satisfaction state.

[0060] At step 106, the logic entropy driven active exploration is performed, which includes generating an observation action sequence based on the consistency solution space generated in the current period and the logical entropy value.

[0061] At step 107, the update change arbitration and audit ledger maintenance are performed, which includes recording historical data of constraint changes and solving rollback.

[0062] The embodiment of the present disclosure introduces an incremental satisfiability solving and minimal conflict rollback mechanism to realize the maintenance of logical consistency of the semantic map in the multi-period update process. When new observations or new constraints are introduced to cause system unsatisfiability, the minimal conflict subset can be automatically identified and rollback operation is performed to avoid the accumulation of errors or logical contradictions in the semantic information in long-term operation, thereby ensuring the stable evolution and self-correction ability of the semantic map.

[0063] At step 101, the geometric-topological SLAM incremental update can be performed in a periodic manner during the operation of the robot to generate and update the geometric map and topological structure information of the environment. The pose graph is constructed and pose graph optimization is performed based on the mileage information and sensor observation data to obtain the robot pose. The logit ratio of the voxel occupancy grid is updated at the voxel resolution, and the free space connected components are extracted and the topological graph is generated according to the free space connectivity.

[0064] Step 101 can take the current motion estimation data (e.g., odometry information) of the robot and the environment sensor observation data (i.e., the environment observation data of each sensor) as input, and output the voxel occupancy grid and the topological graph structure data, providing basic data for subsequent steps.

[0065] Specifically, step 101 can include: performing probabilistic normalization fusion of the voxel occupancy grid and the previously generated sparse imaging evidence heat map at the voxel layer, updating the opening existence probability; and generating a topological edge between two free space connected components according to the updated opening existence probability, which can be used for subsequent formal semantic constraint compilation, incremental satisfiability solving, and minimum conflict processing. Here, the opening existence probability can be used to generate the topological edge, and the topological information can be used as a basis for formal semantic constraint compilation and incremental satisfiability solving.

[0066] In some examples, in each execution cycle, the example implementation process of the geometry-topology SLAM incremental update in step 101 can include the following steps a1-a4:

[0067] Step a1, obtaining the odometry information and sensor observation data of the robot;

[0068] The sensor observation data can include inertial measurement unit data, and the sensor observation data can also include at least one of visual sensor data and laser sensor data. In other examples, the sensor observation data can include observation data of each sensor loaded on the robot body.

[0069] Step a2, updating the pose graph construction based on the time-synchronized odometry information and the observation data of each sensor, the pose graph recording the pose of the robot at each time and the relative motion constraint between adjacent frames in the form of nodes and edges;

[0070] The nodes in the pose graph represent the pose (i.e., position and attitude) of the robot at each time, and the edges in the pose graph represent the relative motion constraint between the robot at adjacent times, which can be obtained through the observation data of sensors such as visual sensors, lidar, odometer, etc. The edges in the pose graph connect the nodes to form a graph structure.

[0071] Specifically, let the pose of the robot at the i-th time be The motion constraint between adjacent times can be represented as follows:

[0072] ,

[0073] wherein, is a measurement constraint vector.

[0074] wherein, is a motion model function, representing the motion of the robot from the i-th time to the i+1-th time.​ the relative motion between the two time instances. In robot SLAM or pose estimation, is the motion model that describes the motion from pose to pose . It can be a displacement, a rotation, or a combination of them. The motion of the robot between two observations can be estimated based on the motion model of the robot (e.g., odometry model, visual odometry model, or lidar odometry, etc.). It can be represented as follows:

[0075] ,

[0076] where , are the position and pose of the robot at the first time instance and the second time instance, respectively. where is the Gaussian noise vector that represents the uncertainty introduced during the measurement process. It is considered as a Gaussian white noise that follows a Gaussian distribution with zero mean and covariance matrix

[0077] . The Gaussian noise vector can reflect the measurement error or the inaccuracy of the sensor, and its form is represented as follows:

[0078] ,

[0079] where is the covariance matrix that is used to represent the variance and noise characteristics of the motion measurement between the two time instances (i.e., the first time instance and the second time instance). The specific forms of the aforementioned and

[0080] depend on the sensors and motion models used. For example, in visual odometry, can be estimated based on the motion of the feature points matched by vision, while can be modeled based on the camera noise or the error of the vision matching. The pose graph optimization is performed in the form of minimizing an objective function to obtain the optimal pose estimates of all nodes . The objective function can be defined as follows:

[0081]

[0082] ,

[0083] where is the set of edges in the pose graph, is the measurement noise covariance matrix.​​​​​

[0084] For example, the pose graph optimization can be implemented by solving the above objective function through a nonlinear least squares optimization to obtain optimal pose estimates for all nodes in the pose graph .

[0085] Specifically, each node in the pose graph represents the state of the robot at a certain time, which consists of coordinates and angles. In the pose graph optimization, the robot creates a new node in the graph every time it passes a location, and these nodes are connected by edges between adjacent nodes. Each edge represents the relative motion of the robot between different time points (e.g., motion constraints measured by sensors).

[0086] Step a3, after completing the pose graph optimization, generate a voxel occupancy grid based on the sensor point cloud or depth map data of each frame.

[0087] Specifically, the update of the voxel occupancy probability is expressed in the form of logit, and let the logit of the voxel at time be , the update formula can be as follows:

[0088] ,

[0089] where is the probability of the voxel being occupied under the current observation data , and is the initial logit constant. The updated voxel logit is limited within a set interval . represents the logit of the voxel at time .

[0090] The logit can be transformed by a Sigmoid function to obtain the voxel occupancy probability at time , i.e., the voxel occupancy probability at time , can be calculated as follows:

[0091] ,

[0092] Voxels with voxel occupancy probability greater than a preset voxel occupancy probability threshold are marked as occupied voxels, and voxels with voxel occupancy probability less than the threshold are marked as free voxels. The voxels in the interval between the two thresholds remain unknown, thereby establishing a voxel occupancy grid.

[0093] The threshold is a probability threshold for a voxel to be in free space. If the occupancy probability of a certain voxel is lower than the threshold , the voxel is considered to be free space. The threshold is a probability threshold for a voxel to be in occupied space. If the occupancy probability of a certain voxel is greater than the threshold , the voxel is considered to be occupied. The thresholds and serve to determine the occupancy status of a voxel. If the occupancy probability of a voxel is greater than the threshold , the voxel is marked as occupied; if the occupancy probability of a voxel is less than the threshold , it is marked as free; if the occupancy probability of a voxel is between the two thresholds, the voxel is considered unknown and its status cannot be determined.

[0094] After the voxel occupancy grid is established, coordinate transformation and fusion are performed in the global coordinate system according to the robot poses indicated in the aforementioned pose graph to obtain the fused voxel occupancy grid in the global coordinate system. For the voxel occupancy grid at time , the coordinate transformation is determined by the rotation matrix and the translation vector , and the coordinate transformation of the grid center point is:

[0095] ,

[0096] wherein is the grid center point coordinate in the local coordinate system, is the corresponding point coordinate in the global coordinate system. Here, the local coordinate system refers to a reference coordinate system related to the current position of the robot. When the robot moves, a local coordinate system is selected to represent the position and direction of the robot. For example, when the robot is at a certain position, the local coordinate system is a coordinate system with the current position of the robot as the origin, and the axes of the coordinate system are aligned with the orientation of the robot.

[0097] Step a4, in the voxel occupancy grid in the global coordinate system, a connectivity analysis method is used to extract free space connected components to generate a topological graph, and topological graph structure data describing the structure of the topological graph is obtained.

[0098] Connectivity is defined as the spatial adjacency relationship of adjacent voxels sharing faces, edges or vertices in three-dimensional space. All sets of mutually connected free voxels are marked by a three-dimensional flooding algorithm , and each connected component represents a topological node. For any two connected components and , if there exists a voxel pair whose Euclidean distance is less than a predetermined Euclidean distance threshold , then add a connection edge in the topology graph. In this way, a topology graph is generated, which represents the connectivity structure of the environment space in the form of nodes and edges.

[0099] In the topology graph, the topology node can be used to store the index and geometric boundary description of the free space set, and the topology edge can be used to store the connection relationship and the corresponding spatial opening position. In specific applications, the topology structure described by the topology graph can be output in the form of structured data, that is, the topology structure described by the topology graph can be output in the form of topology graph structure data, for the coordinate registration and fusion of the sparse imaging evidence generation step in the subsequent step 102.

[0100] In the embodiments of the present disclosure, the geometric-topological SLAM incremental update of step 101 can be repeatedly executed in each cycle of system operation. In each cycle, the input of step 101 includes the latest sensor observation data and the pose graph and voxel occupancy grid (that is, the voxel grid state) of the last cycle, and the output includes the optimized pose sequence, the updated voxel occupancy grid and the topology graph structure data, which provides the geometric and topological basic data for the subsequent semantic and constraint solving.

[0101] Here, the pose sequence refers to the pose record of the robot at different time points or different states, including the position and attitude of the robot. The pose sequence can be used to optimize and update the position estimation of the robot, especially in an incremental SLAM system, the pose sequence is the basis for optimizing the pose graph. In each cycle, the new sensor data is matched and optimized with the historical pose sequence, so as to obtain a more accurate global pose.

[0102] In step 102, based on the propagation and reflection model of wireless signals and acoustic signals, the invisible areas, openings and thin structures in the space are sparsely reconstructed to generate a sparse imaging evidence heat map corresponding to the voxel coordinate system. The coefficient intensity evidence heat map can be used as the basis for subsequent analysis of availability field calculation and formalized semantic constraint compilation.

[0103] Specifically, step 102 can include: constructing a measurement equation , wherein is an observation vector, is a measurement matrix composed of sensor array geometry and propagation path, is a non-negative sparse intensity vector, is a noise term; based on the measurement equation, performing The non-negative sparse reconstruction of the norm minimization obtains a sparse intensity vector; the sparse intensity vector is mapped to a voxel grid through an extrinsic calibration result and a wireless and / or acoustic sparse imaging evidence heat map is generated: the wireless and / or acoustic sparse imaging evidence heat map and the voxel occupancy grid are normalized and fused in a voxel layer to obtain an opening existence probability, which is used for a formal semantic constraint compiling step.

[0104] In step 102, the wireless signal can be, but is not limited to, a carrier signal of Wi-Fi, UWB or other electromagnetic frequency bands, and the acoustic signal can be, but is not limited to, an ultrasonic, audible frequency waveform, etc. In a specific implementation, a wireless transceiving unit or an acoustic transceiving unit is arranged on the robot platform, the wireless transceiving unit includes at least one group of wireless transmitting antennas and wireless receiving antennas, and the acoustic transceiving unit can include at least one group of acoustic transmitting transducers and receiving transducers.

[0105] In the wireless transceiving unit and the acoustic transceiving unit, the transmitting subunit generates a known waveform signal, and the signal is collected by the receiving subunit after being propagated, reflected and scattered by the environment. The spatial position and attitude between the transmitting subunit and the receiving subunit are determined by the pose data obtained by the geometric-topological SLAM incremental update in step 101, and the pose data is the pose data in the aforementioned pose sequence.

[0106] Suppose the spatial positions of the transmitting subunit and the receiving subunit are and respectively, and the signal propagation path length is , wherein is the coordinate of the center point of the voxel to be estimated. The propagation time delay of the voxel can be calculated by the following formula:

[0107] ,

[0108] wherein, is the propagation speed of the signal in the medium.

[0109] The actual observation signal is sampled in time domain and detected in amplitude to form an observation vector In the measurement modeling stage, a linearized propagation model is established, which satisfies the following formula:

[0110] ,

[0111] wherein, in the linearized propagation model is a noise vector.

[0112] wherein, in the linearized propagation model is a sparse intensity vector of , and is a sparse intensity vector of the observation vector, is a measurement matrix with size , where represents the number of measurements, where denotes the dimension of each measurement or the size of the feature vector, and denote the number of sensors and the dimension of the measurement vector, respectively.

[0113] The measurement matrix is defined as: , The element

[0114] ,

[0115] where is the th frequency component, is the propagation time delay corresponding to the th voxel. The sparse reconstruction procedure obtains the sparse intensity vector by solving a sparse optimization problem with non-negativity constraints.

[0116] The objective function of the sparse reconstruction procedure is represented as follows:

[0117] ,

[0118] where is the sparse regularization parameter.

[0119] In specific applications, the sparse optimization problem with non-negativity constraints can be solved using an iterative thresholding algorithm or a coordinate descent algorithm to obtain the converged sparse intensity vector , which refers to the final output of the transmission intensity, representing the target result obtained after the optimization process, and the output of the finally constructed transmission matrix model.

[0120] After obtaining the sparse intensity vector, the spatial coordinates of each voxel are transformed from the local sensor coordinate system to the global coordinate system (i.e., the world coordinate system) according to the extrinsic parameters of the transmitting sub-unit and the receiving sub-unit in the robot coordinate system. The coordinate transformation formula is as follows:

[0121] ,

[0122] where is the extrinsic rotation matrix, is the extrinsic translation vector, is the voxel coordinate in the local coordinate system, is the voxel coordinate in the global coordinate system.

[0123] After coordinate unification, a sparse intensity distribution matrix is generated by interpolating the mapping in the global voxel space . The matrix is organized by spatial voxel index, and the element value of each voxel corresponds to the sparse intensity of the corresponding voxel position. The normalized sparse intensity of each voxel is obtained by standardizing the matrix through the amplitude normalization function , as shown in the following formula:

[0124] ,

[0125] wherein, is the normalized sparse intensity of voxel , and are the minimum and maximum values of the sparse intensity, respectively.

[0126] The of all voxels forms a sparse imaging evidence heat map in space, which can be stored in the form of a three-dimensional array. The sparse imaging evidence heat map contains the normalized intensity value of each voxel and the corresponding global coordinates.

[0127] Here, in order to register the sparse imaging evidence heat map with the voxel occupancy grid generated in step 101, the same voxel resolution and coordinate origin definition are used for both. If there is a resolution difference between the sparse imaging evidence heat map and the voxel occupancy grid generated in step 101, trilinear interpolation can be used to unify them.

[0128] After step 102, a joint data structure can also be established based on the sparse imaging evidence heat map and the voxel occupancy grid generated in step 101 at each voxel position:

[0129] ,

[0130] wherein, is the voxel occupancy probability of voxel , is the normalized sparse intensity of voxel , denotes the joint data structure of voxel .

[0131] In specific applications, the joint data structure can be stored in the sparse evidence data set for subsequent availability field calculation step to directly call. The sparse evidence data set is organized in the form of key-value index, the key is the voxel coordinate index, and the value is the binary tuple containing the voxel occupancy probability and the normalized sparse intensity value. A new sparse evidence data set is generated every execution cycle, and the voxel index is fused and updated with the previous cycle data set.

[0132] wherein the joint data structure contains information related to the opening existence probability. Specifically, the joint data structure contains the volume and intensity information of each voxel, combining the fusion of sparse imaging evidence and geometric voxel data. In the joint data structure , the main concern is to combine the sparse intensity of each voxel with the opening existence probability to form a complete data set for subsequent processing and updating. That is, the joint data structure can also be regarded as a comprehensive body containing the opening existence probability and other related data.

[0133] wherein the update of the sparse evidence data set can adopt a weighted average strategy, that is, the update of the sparse evidence data set is realized by the following formula:

[0134] ,

[0135] wherein, is the normalized sparse intensity value of the voxel reconstructed in the current cycle, is a time weighting coefficient, and the value range is . is the normalized sparse intensity value of the voxel after the update. is the normalized sparse intensity value of the voxel in the previous cycle.

[0136] In the embodiments of the present disclosure, through step 102, not only the sparse imaging evidence heat map can be obtained, but also the present sparse imaging evidence heat map can be completely aligned with the voxel occupancy grid in space, which can directly participate in the subsequent availability calculation and semantic constraint compilation process. The sparse imaging evidence heat map generated in each cycle can be managed by time stamp and pose index to ensure the spatiotemporal consistency of the data and the robot motion trajectory.

[0137] In step 103, according to the voxel occupancy grid obtained in step 101, the topological graph structure data, and the sparse imaging evidence heat map obtained in step 102, a plurality of availability parameter fields related to the robot size, kinematics and dynamics constraints are calculated, including the passable availability field, the parking availability field and the through opening availability value.

[0138] Specifically, in step 103, the passable availability field and the parking availability field are calculated in the voxel layer, and the through opening availability value for the opening candidate object is calculated in the object layer. The calculation of step 103 takes the robot size, the dynamics constraint, the local slope, the channel curvature, the vertical clearance, the friction cone angle, the flatness criterion and the free voxel density on both sides as inputs.

[0139] In some examples, the passable availability field can be calculated by: performing free space erosion-dilation to obtain a passable width, calculating a ratio of the passable width to a robot width; estimating a local slope by a ground normal and calculating a difference with a maximum allowed slope; fitting a passable centerline to obtain a passable curvature and calculating a difference with a minimum turning radius corresponding curvature; and normalizing the ratio of the passable width to the robot width, the difference of the local slope and the maximum allowed slope, and the difference of the passable curvature and the minimum turning radius corresponding curvature, and taking a minimum value as the passable availability field value of the voxel.

[0140] In some examples, the stop availability field can be calculated by: calculating a ratio of a vertical clearance to a minimum clearance, calculating a difference of a surface inclination angle and a friction cone angle, performing a flatness criterion and binarizing, normalizing the ratio of the vertical clearance to the minimum clearance, the difference of the surface inclination angle and the friction cone angle, and the binarization of the flatness criterion, and taking a minimum value as the stop availability field value.

[0141] In some examples, the pass-through opening availability value can be calculated by: calculating a ratio of a net width to a minimum net width, calculating a ratio of a net height to a minimum net height, calculating a ratio of a free voxel density on both sides to a reference density, normalizing the ratio of the net width to the minimum net width, the ratio of the net height to the minimum net height, and the ratio of the free voxel density on both sides to the reference density, and taking a minimum value as the pass-through opening availability value.

[0142] The input data used in the step 103 of analyzing the availability field calculation can include: (1) robot structure parameters, including a body width , a body length , a body height , a maximum climbing angle , a minimum turning radius , and a friction coefficient ; (2) an occupancy probability of each voxel in a voxel occupancy grid and a free space set; (3) a sparse imaging evidence heat map ; and (4) a topological graph data structure, i.e., a topological node set and a topological edge set, used to calculate the spatial connectivity of passable channels, openings, and rooms.

[0143] In some embodiments, the example implementation process of analyzing the availability field calculation in the step 103 can include the following steps b1-b4:

[0144] In the step b1, local passable regions in the free space set are identified, a passable availability coefficient of each voxel is calculated, and a passable availability field is formed.

[0145] The centerline of the passage can be extracted by three-dimensional erosion and dilation operations, and the net width of the passage is defined as the minimum diameter of the free voxels in the local cross-section. Let the point set on the passage cross-section be , and the net width of the passage is defined as follows:

[0146] ,

[0147] wherein and represent a point on the path or passage, respectively, and are used to describe the position of the robot or sensor in the path or space.

[0148] The local passage slope can be calculated by the angle between the passage centerline direction vector and the gravity direction unit vector , that is, it can be calculated by the following formula, wherein the point represents the dot product:

[0149] ,

[0150] The passage curvature is estimated by the three-point chord method, and let the coordinates of the three consecutive points on the passage centerline be , , , then the curvature can be obtained by the following formula:

[0151] ,

[0152] wherein , represent the previous point and the next point on the path, respectively, which together with the current point are used to calculate the curvature and other geometric properties of the path. Through these points, the rate of change of the path can be calculated to help optimize and update the path.

[0153] The passability coefficient is defined as the normalized minimum value of the net width of the passage, the slope, and the curvature, that is, it can be calculated by the following formula:

[0154] ,

[0155] wherein represents the body width of the robot, represents the maximum climbing angle of the robot, represents the minimum turning radius of the robot, represents the curvature, represents the local passage slope, represents the net width of the passage.

[0156] In all voxels, the is mapped to interval, forming the passable availability field .

[0157] Step b2, for the stoppable area, extract the ground candidate patches with larger planar support area, calculate the stoppable availability coefficient of the candidate patches , forming the stoppable availability field .

[0158] Each candidate patch is represented by a plane equation as follows:

[0159] ,

[0160] wherein, is the unit normal vector, is the plane constant term.

[0161] The vertical clearance is calculated by the local voxel height distribution , the vertical clearance is the minimum free space height in the direction of the plane normal vector , that is, the vertical clearance is calculated by the following formula:

[0162] ,

[0163] wherein, is the free voxel set within a certain range above the plane. represents a position or node in the path or space, represents a free voxel set.

[0164] The inclination angle is the angle between the plane normal vector and the direction of gravity:

[0165] ,

[0166] wherein, represents the plane normal vector, represents the direction of gravity vector, is the inverse cosine function, is the angle between the plane normal vector and the direction of gravity.

[0167] The flatness coefficient is calculated by the variance of the plane fitting residual, represented as follows:

[0168] ,

[0169] wherein, represents the flatness coefficient, ​、 , denotes the components of the plane normal vector, denotes the constant in the plane equation, 、 and denotes the coordinates of the i-th point.

[0170] Docking availability coefficient is defined as the normalized minimum value of the clearance ratio, the slope ratio and the flatness difference, that is, it can be calculated by the following formula:

[0171] ,

[0172] wherein, is a preset flatness tolerance. After mapping to the interval , the docking availability field is formed.

[0173] Step b3, for the opening object, based on the sparse imaging evidence heat map and the opening region identified by the topological edge set to determine the candidate opening region, calculate the through opening availability value of each candidate opening region, and the through opening availability value of all candidate opening regions is summarized to form the through opening availability value field .

[0174] For each candidate opening region, the net width of the candidate opening region, the of the candidate opening region and the free voxel density on both sides of the candidate opening region are calculated. The free voxel density on both sides reflects the spaciousness of the region and is used to evaluate the passability of the region, wherein the net width of the candidate opening region and the net height of the candidate opening region are calculated by the minimum circumscribed rectangle of the candidate region, which is expressed as the following formula:

[0175] , ,

[0176] wherein, is the size of the candidate opening region in the X-axis, which represents the span of the candidate opening region in the X direction, and represent the maximum and minimum values of the candidate opening region in the X direction, respectively. is the size of the candidate opening region in the Z-axis, which represents the span of the candidate opening region in the Z direction, and represent the maximum and minimum values of the candidate opening region in the Z direction, respectively. ​This represents the density of free voxels on both sides, indicating how much free space exists within the candidate opening region. The density of free voxels reflects the openness of the candidate opening region and is used to assess its accessibility.

[0177] bilateral free voxel density The following formula is used to calculate:

[0178] ,

[0179] in, The number of free radicals in the region. The volume of the candidate region.

[0180] Define the availability value through the opening The minimum value of the three normalized indices is expressed as follows:

[0181] ,

[0182] in, Using the reference free volume density, the availability values ​​through the openings for all candidate regions are calculated. After summarization, an availability value field through the opening is formed. Here, the candidate region refers to all areas used for passage, including candidate opening regions and areas that are not necessarily openings. The candidate opening region is a subset of the candidate regions, specifically referring to those areas that can serve as passageways for robots; they are explicit openings or doorways.

[0183] Step b4, the calculated , , Stored at the same voxel resolution as the voxel-occupied raster, each voxel contains three types of availability values ​​(i.e., passable availability field, dockable availability field, and access availability value) and a coordinate index.

[0184] Within each execution cycle, the three availability fields are updated according to their time index. The updates can be performed using a weighted fusion approach, i.e., by updating the three availability fields using the following formula:

[0185] ,

[0186] in, The availability value calculated for the current period. This is the time weighting coefficient, and its value range is... . Indicates the current moment The calculated accessibility value, i.e., the position the robot has currently passed through. The accessibility at that time. Indicates the previous moment The computed navigability value reflects the navigability of the robot's previous path or area.

[0187] The availability field can be stored as a unified structured data block, each structured data block records three parameters, namely the voxel coordinate index, the navigability availability value, the stopping availability value, and the passing opening availability value, to provide quantitative input for subsequent semantic constraint logic relationship establishment.

[0188] In step 104, the geometric, topological and availability information is converted into a constraint set that can be processed by the incremental satisfiability solving algorithm to establish formalized relationships between semantic objects, and the output result is a formalized semantic constraint set , which serves as the input of the incremental satisfiability solving and minimum conflict processing step. In other words, the opening candidate object, the room candidate object, the corridor candidate object, the navigability availability field, the stopping availability field, the passing opening availability value, the topological connection relationship, and the sparse imaging evidence heat map are converted into a formalized semantic constraint set containing hard constraints and soft constraints.

[0189] Specifically, the formalized semantic constraint compilation of step 104 can include: generating door-room connection hard constraints to construct connectivity constraints with the topological connection relationship between the opening candidate object and two different room candidate objects as variables; generating opening size hard constraints to construct threshold constraints with the net width and minimum net width, the net height and minimum net height of the opening candidate object as variables; generating room compactness soft constraints to construct upper limit constraints with the ratio of the eigenvalue of the inertia principal axis of the room candidate object as a variable; generating corridor elongation soft constraints to construct lower limit constraints with the ratio of the eigenvalue of the inertia principal axis of the corridor candidate object as a variable.

[0190] Further, the formalized semantic constraint compilation of step 104 can also include: generating evidence consistency soft constraints for the opening candidate object to construct a proportion threshold constraint with the proportion of high response voxels in the sparse imaging evidence heat map within the coverage area of the opening candidate object as a variable, and to construct an adjacency relationship constraint with the intersection relationship between the neighborhood of the opening candidate object and the free space boundary of the two room candidate objects as a variable.

[0191] Hard constraints: constraints that must be strictly satisfied, such as the topological relationship of door and room connection, opening size, etc.

[0192] Soft constraints: such as room compactness, corridor elongation, and evidence consistency soft constraints, etc. These constraints can have certain flexibility, allowing for a certain degree of violation, but need to be optimized through a penalty mechanism.

[0193] For the evidence consistency soft constraint, it can be embodied as two main soft constraints: the proportion threshold constraint of high response voxel proportion and the topological constraint of adjacency relationship. The proportion threshold constraint of high response voxel proportion determines whether the constraint is violated by comparing the voxel response intensity in the sparse imaging evidence heat map with the threshold. The topological constraint of adjacency relationship judges the rationality of the opening candidate object by calculating the intersection degree of the opening candidate object and the room free space. If the two constraints are not satisfied, they will be reflected in the soft constraint part through the corresponding penalty function (such as introducing a penalty term), and these soft constraints realize the influence of the constraints through the logic model and the cost function.

[0194] Specifically, the input data of the formal semantic constraint compilation of step 104 can include: 1) voxel occupancy grid and topological graph structure data; 2) passable availability field Parking availability field And the availability value through the opening ; 3) sparse imaging evidence heat map ; 4) a set of topological nodes in the topological graph structure data And a set of topological edges

[0195] In some embodiments, the exemplary implementation process of the formal semantic constraint compilation of step 104 can include the following steps c1-c4:

[0196] Step c1, identify candidate objects at the object level.

[0197] Specifically, through connectivity analysis, extract nodes that satisfy the free voxel number and spatial connectivity threshold conditions from the set of topological nodes as room candidate objects; extract edges that simultaneously adjoin two different room candidate objects from the set of topological edges as opening candidate objects; identify the free voxel set with a high aspect ratio (for example, the ratio of the eigenvalue of the principal axis feature is greater than a preset threshold) as a corridor candidate object. Wherein each candidate object records its spatial range, voxel set index and adjacent object list with a unique identifier .

[0198] Step c2, perform standardization operation on the geometric parameters in the candidate object set.

[0199] Here, the candidate object set refers to the object set selected by algorithm or analysis method in a specific step for processing or verification. These objects are selected according to spatial features, geometric shapes, topological relationships, etc. Specifically, the "candidate object set" is various objects such as room, opening and corridor candidate objects identified from the set of topological nodes through connectivity analysis or other methods.

[0200] Define the compactness parameter of the room candidate object:

[0201] ,

[0202] where, and are the maximum and minimum eigenvalues of the inertia matrix of the room candidate object, respectively.

[0203] Define the elongation parameter of the corridor candidate object:

[0204] ,

[0205] where, denotes the elongation parameter of the corridor candidate object, i.e. denotes the aspect ratio or the "degree of elongation" of the shape of the corridor. denotes the maximum eigenvalue of the principal axis of inertia of the corridor candidate object, i.e. describes the extension of the corridor in the maximum direction. denotes the minimum eigenvalue of the principal axis of inertia of the corridor candidate object, i.e. describes the extension of the corridor in the minimum direction.

[0206] Define the size parameter of the opening candidate object:

[0207] , ,

[0208] Step c3, in the formalization stage, establish constraint expressions for each type of semantic relation.

[0209] Here, the semantic relation refers to the logical or topological relation between spatial elements, which includes the connection mode, relative position, geometric size, etc. between spatial objects.

[0210] Hard constraints include:

[0211] (1) Door-room connection constraint:

[0212] If the opening candidate object is labeled as a door, there must exist two different room candidate objects , such that there exists a topological edge connection between their voxel sets. The formalization of this constraint is as follows:

[0213] ,

[0214] where, denotes that a certain opening candidate object is labeled as a door. denotes that two room candidate objects and are connected by the door . This constraint requires that if is labeled as a door, then these two rooms and The connection in topology must exist.

[0215] (2) Opening size constraints:

[0216] When an opening candidate object is labeled as a door, its width and height should satisfy the following conditions:

[0217] ,

[0218] wherein, and are preset minimum net width and minimum net height, respectively.

[0219] Soft constraints include:

[0220] (1) Room compactness constraint, denoted as follows:

[0221] ,

[0222] wherein, is a room compactness threshold. denotes a room candidate object, denotes an elongation parameter of the room candidate object, i.e., the aspect ratio of the room.

[0223] (2) Corridor elongation constraint, denoted as follows:

[0224] ,

[0225] wherein, is a corridor elongation threshold. denotes a corridor candidate object, denotes an elongation parameter of the corridor candidate object.

[0226] (3) Evidence consistency constraint:

[0227] When an opening candidate object is labeled as a door, it is required that the proportion of high-response voxels in the corresponding region in the sparse imaging evidence heat map is not lower than a set high-response voxel proportion threshold , i.e.,

[0228] ,

[0229] wherein, is the number of voxels in the opening region with sparse intensity greater than a threshold, is the total number of voxels in the opening region. The high-response voxel proportion threshold can be used to ensure the saliency of the opening candidate object in the heat map, preventing low-heat openings from being incorrectly labeled as valid openings. In specific applications, the high-response voxel proportion threshold The determination can be flexible based on experimental data, physical characteristics, and task requirements, etc.

[0230] Step c4, to ensure the consistency of the passability constraint and the geometric constraint, a cross-layer constraint relationship is introduced:

[0231] If the room is connected with the room through the opening , the passability value of the opening should satisfy the following formula:

[0232] ,

[0233] wherein, is the passability threshold. represents a metric value or a state.

[0234] Step c5, all constraints are organized in the form of logical propositions as a formal semantic constraint set Each constraint in the formal semantic constraint set includes a unique constraint identifier, a constraint type (i.e., hard constraint, soft constraint, etc.), a variable set, and a numerical threshold.

[0235] In some examples, the constraint set structure is defined as follows:

[0236] ,

[0237] wherein, is the constraint identifier, is the constraint type, is the constraint variable set, is the constraint expression, i = 1, 2, …, h, h represents a count index, and represents the number of elements in the set or the sequence of elements.

[0238] Specifically, the formal semantic constraint set can be stored in the form of a key-value index inside the system. The key of each constraint is the constraint identifier, and the value is the constraint expression and related metadata. The set supports an incremental update mechanism. When new observation data is introduced or existing constraints are modified, only the affected subset is updated. The update process includes three steps of constraint positioning, replacement, and re-indexing.

[0239] Step 104 the output result of the formal semantic constraint compilation is the updated formal semantic constraint set ​The formal semantic constraint set contains all logical relations and numerical conditions in the current execution cycle. The formal semantic constraint set is passed to the incremental satisfiability solving and minimal conflict processing steps for logical solving and consistency judgment. After each cycle is executed, the formal semantic constraint set is recorded together with the timestamp and pose index to ensure that subsequent version tracking and constraint rollback is feasible. The pose index is used to point to the identifier of a pose in the pose sequence, which is an integer value.

[0240] The incremental satisfiability solving and minimal conflict processing of step 105 takes the formal semantic constraint set as input, performs satisfiability solving and consistency judgment on the constraint system, and performs minimal conflict subset identification and constraint rollback processing when an unsatisfiable condition is detected, thereby maintaining the logical consistency of the semantic map under multi-cycle incremental updating.

[0241] Specifically, the incremental satisfiability solving and minimal conflict processing in step 105 can include: constructing an incremental satisfiability problem model for hard constraints and soft constraints with relaxation variables; performing local re-solving on the affected variable block after adding new constraints; calculating the minimal unsatisfiable subset when an unsatisfiable condition is detected and calculating the rollback set approximation accordingly to obtain the minimal constraint set that needs to be removed, performing minimal rollback processing according to the minimal constraint set and solving again.

[0242] Among them, the soft constraint with relaxation variable is used for the size, topological relationship, etc. of the space element, to ensure that when facing unsatisfiable constraints, the system can obtain an approximate solution by adjusting the relaxation variable, thereby ensuring the robustness and feasibility of the system.

[0243] The input data of the incremental satisfiability solving and minimal conflict processing in step 105 can include: 1) the formal semantic constraint set of the current cycle ; 2) the solving state of the last cycle ; 3) the stored constraint audit book index; 4) the index of new or updated observation evidence.

[0244] In step 105, the formal semantic constraint set is first parsed and variable extraction is performed. Specifically, let the set of logical variables involved in the formal semantic constraint set be , each logical variable represents the true or false state of a semantic assertion. All hard constraints and soft constraints are marked to form the hard constraint subset and the soft constraint subset .

[0245] The semantic assertions are represented by logical variables and used in the constraint system to control and describe the legality of objects, spatial elements, or relationships. The hard constraint subset contains all the semantic assertions that must be satisfied. The soft constraint subset contains those semantic assertions that can tolerate a certain degree of violation. These assertions, together with the constraints, form a complete logical constraint system that ensures the correctness and feasibility of the generated semantic map or robot navigation task.

[0246] Subsequently, the internal state of the incremental solver is constructed. The incremental solver maintains a state set Each element in the state set corresponds to a set of logical variable assignments and their satisfiability flags. At initialization, the part of the state set from the previous period that is not affected by new constraints is preserved, and only the constraint subset involved in the update is locally re-solved. The constraint subset is the set of constraints affected by new constraints, updated constraints, or other state changes during the incremental solving process. By locally re-solving these constraint subsets, the incremental solver can more efficiently update the state without having to globally solve the entire constraint system, thereby improving computational efficiency.

[0247] The constraint set updated incrementally is represented as follows:

[0248]

[0249] The objective function of the incremental solution is defined as follows:

[0250]

[0251] where is a Boolean function that takes the value 1 when the constraint is satisfied, otherwise 0; is the soft constraint weight.

[0252] For hard constraints the system requires all to be satisfied, i.e.:

[0253]

[0254] In step 105, when an unsatisfiable condition is detected during the solving process, the system enters the minimal unsatisfiable subset detection phase. The minimal unsatisfiable subset (MUS) is defined as the smallest subset of any unsatisfiable subset of the formal semantic constraint set.

[0255] ​​​​The solving process of the increment satisfiability is a process of local update by increment on the basis of the existing solving result. Specifically, in step 105, the problem is solved by analyzing and updating the constraint subset affected by the new constraint, instead of solving the entire problem from the beginning. In this way, the calculation efficiency is improved, and only the local change part is recalculated.

[0256] In some examples, the minimum unsatisfiable subset detection phase can be based on a constraint elimination and verification process. Specifically, for each constraint in the formal semantic constraint set , a satisfiability test is performed: if the system turns from unsatisfiable to satisfiable after is removed, record as a key conflict constraint. In this way, all key conflict constraints in the formal semantic constraint set are identified by this method, forming a key conflict constraint set . For the detected , a rollback set approximation calculation is performed to determine the minimum rollback set. After calculating the rollback set, the constraints in the rollback set are marked as temporarily invalid, and a rollback operation is performed.

[0257] wherein the rollback set is defined as the minimum set containing at least one constraint of each conflict subset, satisfying the following formula:

[0258] ,

[0259] wherein represents the rollback set is a subset of the current confirmed constraint set .

[0260] represents that for each minimum unsatisfiable subset .

[0261] represents that the rollback set has an intersection with the minimum unsatisfiable subset , meaning that the rollback set must contain at least one constraint in the minimum unsatisfiable subset.

[0262] Specifically, the rollback operation can include two items: 1) removing the corresponding variable assignment from the state set solved in the current period; 2) generating a rollback record in the constraint ledger, which records content including but not limited to constraint identifier, conflict detection timestamp, conflict context, and rollback reason. Here, the constraint ledger is used to record the state of the constraint and the rollback record, mainly for constraint management.

[0263] In step 105, after the rollback process is completed, the satisfiability solving is performed again on the remaining constraint set to generate a consistency solution space Each element in the consistency solution space contains a set of logical variable assignments and the corresponding constraint satisfaction status. The system selects the assignment that satisfies all hard constraints and minimizes the soft constraint violation cost as the final result.

[0264] where the remaining constraint set refers to the constraint set that has not been affected after the rollback operation, and these constraints will continue to be used for subsequent satisfiability solving. The consistency solution space is a set of solving states that describe all states that satisfy hard constraints and minimize soft constraint violation costs under the remaining constraint set.

[0265] In step 105, in order to evaluate the uncertainty of the system in incremental solving, a logical entropy index is introduced Logical entropy index defined as follows:

[0266] ,

[0267] where, is the probability of the variable taking the true value in the current solving state.

[0268] This logical entropy value is used in the subsequent logical entropy-driven active exploration step to guide the robot to select a new observation direction and position.

[0269] The constraint solving output result of step 105 incremental satisfiability solving and minimum conflict processing is output in a structured form, which can include the following information: 1) the value state of each logical variable; 2) the satisfaction flag of each constraint; 3) the detected minimum conflict set and rollback set index; 4) the logical entropy value .

[0270] Here, the constraint solving output result can be used to generate a semantic map increment, which is a practical embodiment and physical representation of the constraint solving result, specifically showing how to update the map by satisfying constraints in the map construction process.

[0271] The constraint solving output result of step 105 is passed to the subsequent logical entropy-driven active exploration step, which can be used for action decision and data collection planning, and the constraint solving output result of step 105 is recorded in the change arbitration and audit book to ensure the completeness and traceability of the solving history. After each solving is completed, the system retains the solving state set of the current period as the initial state for the next period solving, realizing the continuity and incremental consistency of the whole process.

[0272] Here, the change arbitration and audit ledger is used to record all change operations in the system, covering a wider range of content, including but not limited to constraints.

[0273] The logic entropy driven active exploration of step 106 takes the logic entropy value of the current cycle , semantic map state and robot pose as input, generates candidate observation actions by evaluating the uncertainty of the constraint system, and selects the optimal action to guide the robot's next step of perception and movement behavior, thereby realizing the continuous incremental improvement of the semantic map.

[0274] Wherein, the semantic map state refers to the semantic map and its state of the current cycle, which is related to the solution state set in the foregoing, but it contains more spatial information and relationships. The robot pose refers to a specific position and orientation in the optimized pose sequence.

[0275] Specifically, the logic entropy driven active exploration of step 106 can include: generating a candidate action set, enumerating observations according to the forward measurement model and constructing a satisfiable solution space at the next time; performing cardinality estimation on the satisfiable solution space and calculating the logic entropy; calculating the expected value of the logic entropy for each candidate action, selecting the candidate action with the smallest expected value of the logic entropy and executing it, triggering the next round of loop of the sparse imaging evidence generation step and the geometric-topological SLAM incremental update step.

[0276] Wherein, the forward measurement model and the satisfiable solution space mentioned in step 106 are used to generate candidate actions and calculate logic entropy.

[0277] In step 10-6, the input data of the logic entropy driven active exploration can include: 1) constraint solving output results, including the consistency solution space generated in the current cycle , logic entropy value ; 2) voxel occupancy grid and topological graph structure data; 3) passability field ; 4) current robot pose and motion constraint parameters: 5) sparse imaging evidence heat map . Wherein, the motion constraint parameters are obtained by the physical ability of the robot itself, environmental conditions and task requirements.

[0278] The active exploration process of step 106 can include three stages, namely candidate action generation, action value evaluation and action selection.

[0279] First, in the candidate action generation stage of step 106, according to the topological graph described by the topological graph structure data, starting from the node where the current pose is located , retrieve all adjacent topological node sets directly connected to it corresponding to a potential navigation target position.

[0280] For each candidate topological node, the computer robot moves from the current position to the node of the passable path. The path planning solves the path set by the sequence of topological edges and local geometric constraints, which is represented as follows:

[0281] ,

[0282] where k represents the number of candidate paths generated in the path planning or the sequence length of the path.

[0283] where the candidate topological node refers to the topological node generated by the forward measurement model and available as the next target of the robot in the path planning.

[0284] where each path represents a series of continuous passable voxel coordinate sequences, and satisfies the following formula:

[0285] ,

[0286] where is the passable availability value of the corresponding voxel, is the passable threshold.

[0287] In the action value evaluation stage of step 106, the information gain index of each candidate path is calculated, including the constraint entropy reduction value and the spatial observation coverage.

[0288] The constraint entropy reduction value is defined as follows:

[0289] ,

[0290] where represents the expected value of the constraint entropy after assuming that the observation is performed along the path , which is estimated by the sparse imaging uncertainty of the unobserved area on the candidate path.

[0291] For the voxel set covered by the path , the local observation uncertainty is defined as follows:

[0292] ,

[0293] where represents the observation uncertainty related to the first path (or channel).

[0294] represents the set of all voxels covered by the path or channel , is a set containing paths involved all voxels.

[0295] denotes a set of elements (voxels), i.e. paths covered voxels.

[0296] denotes a path over a certain voxel. A voxel is the smallest unit defined in space, here representing a path covered a certain voxel.

[0297] denotes a sparse intensity normalized value associated with voxel . It is a value between 0 and 1, used to represent the observation uncertainty of the voxel, the larger the value, the higher the sparse intensity of the voxel, the more meaningful it is.

[0298] where, is the sparse intensity normalized value of the voxel.

[0299] The relationship between the constrained entropy reduction value and the observation uncertainty can be expressed as follows:

[0300] ,

[0301] where, is a proportionality coefficient. denotes the logical entropy change value of path , measuring the increase or decrease of the entropy value of path in the information collection process. The proportionality coefficient is used to adjust the proportional relationship between the observation uncertainty and the change amount. The determination of this coefficient depends on the specific environment, task requirements or system design. denotes the observation uncertainty associated with path , which has been previously defined as the average value of the uncertainty of the voxels covered by the path.

[0302] Spatial observation coverage is defined as:

[0303] ,

[0304] where, is the current set of observed voxels, is the global set of free voxels. denotes the set of voxels passed by the current path .

[0305] The two indicators are combined to define the action value function as follows:

[0306] ,

[0307] wherein, is a balance coefficient, controlling the weight of entropy change and coverage, and its value range is . is the value function of path , which is used to represent the priority of the path.

[0308] The greater the value of the function , the higher the value of the corresponding path in reducing system uncertainty and expanding environmental coverage.

[0309] In the action selection stage, the maximum value path that meets the constraint condition is selected from the candidate path set.

[0310] The selection rule is as follows:

[0311] ,

[0312] wherein, is the optimal path, and the path with the maximum path value is selected. is the candidate path set, and argmax represents the path that maximizes the value function.

[0313] And it needs to meet the robot dynamics constraint condition, that is,

[0314] , ,

[0315] wherein, is the path length, is the maximum travel limit, is the maximum slope in the path, is the upper limit of the feasible slope.

[0316] After determining the optimal path , the system generates an action instruction sequence, which is a set of control commands for the robot when executing the path, guiding the robot to perform the task according to the specific path.

[0317] The action instruction is composed of discrete pose points:

[0318] ,

[0319] wherein, represents the action instruction sequence, represents the number of elements in the action instruction sequence, that is, the number of actions the robot needs to perform.

[0320] wherein, is a pose target to be executed by the robot, .

[0321] The action instruction sequence is stored in time sequence and sent to the robot motion control module for execution.

[0322] After the execution of step 106 is completed, the robot collects new sensor observation data, and feeds the resulting data to the geometric-topological SLAM incremental update step to form a new observation period. The system repeats the process of steps 101-105 in the new period.

[0323] At the end of each execution period in step 106, the current action sequence, path number, constraint entropy change value, and timestamp are recorded for subsequent behavior result tracking and consistency recording in the change arbitration and audit book maintenance step.

[0324] wherein, the action sequence refers to the action instruction sequence when the robot executes the path, that is, the control instruction set of the target position and attitude that the robot needs to execute at each time. It is the same concept as the action instruction sequence mentioned earlier, both of which are used to describe the action that the robot should execute at each time in the path.

[0325] The path number is a unique identifier used to distinguish different paths. When executing the path, the path number can be used to know which path the robot is executing, and the action instruction sequence corresponding to the path is associated. That is, the "path number" corresponds to a specific execution path, which is used to track the relationship between the path and the instruction.

[0326] In the embodiments of the present disclosure, the logic entropy driven active exploration in step 106 can realize observation planning in mathematical form, which does not rely on external manual intervention, and can realize continuous incremental information collection and semantic map expansion under the premise of ensuring the logical consistency of the constraint system.

[0327] In step 107, the change arbitration and audit book maintenance are used to record the historical changes of constraints, semantic assertions and their associated evidence, to ensure the traceability, verifiability and consistency maintenance of semantic information in the multi-period incremental map construction process. Specifically, a triple is formed by the evidence identifier, the constraint identifier and the decision identifier, the source and version corresponding to the semantic assertion are recorded, the audit book is generated and the corresponding record is updated when the minimum rollback processing occurs. Among them, the evidence identifier is used to uniquely identify and track the original data or inference result for generating semantic assertions or constraints, to ensure the traceability and consistency of semantic information in multiple incremental update periods. This plays a key role in subsequent audit book maintenance, rollback processing, version update and other processes.

[0328] In step 107, the change arbitration and audit ledger maintenance record each semantic assertion, evidence source and change operation, ensuring the traceability and version management of the incremental semantic map.

[0329] The input data of step 107 can include: 1) the formalized semantic constraint set of the current period and the solution state set ; 2) the previous period ledger state ; 3) the observation action sequence and execution result record output by step 106; 4) the rollback set detected by step 105 .

[0330] Among them, the execution result record refers to the actual result data generated when the robot executes the path, action or performs environmental observation. These records not only include path execution, observation data collection, but also include state changes, feedback and error conditions during execution. They provide key data for change arbitration, audit ledger maintenance and subsequent map updating, ensuring the traceability, consistency and effectiveness of the semantic map in multiple incremental periods.

[0331] The change arbitration and audit ledger maintenance of step 107 can include three stages, namely record structure definition, change event arbitration and ledger update.

[0332] In the record structure definition stage, the system initializes the audit ledger , which records each change event in a triple structure. The ledger is defined as:

[0333] ,

[0334] Among them, is the unique event identifier, is the change content description structure, is the metadata set, represents the total number of records in the audit ledger, i.e. the number of all change records in the current period.

[0335] The change content description structure may include: a) constraint identifier ; b) change type (add, modify, delete, rollback); c) pre-change and post-change state values; d) change trigger source (constraint update, observation addition, action execution or solution conflict).

[0336] The metadata set may include timestamp , execution node index , relevant observation number and operation number. Among them, the execution node index is automatically assigned by the system when executing path planning or operation, which is used to identify the specific node or task executed by the robot. The relevant observation number is a unique number assigned to each collection of environmental data or perception data, which facilitates the association of data with semantic assertions, constraints, etc. The operation number is a unique number assigned by the system to each operation (path execution, perception task, etc.), which is used to distinguish and track the operation.

[0337] In the change event arbitration phase, the legality of all newly generated or revised constraints and semantic assertions is determined and conflict arbitration is performed.

[0338] Among them, the arbitration process can include the following three determinations:

[0339] 1) Source consistency determination: For each change event, check the consistency of its source identifier with the constraint version recorded in the account book last time. If there is a cross-period modification and no explicit source index is included, it is marked as an abnormal record;

[0340] 2) Version dependency determination: When multiple change events refer to the same constraint identifier , generate a dependency sequence according to the timestamp order and logical dependency relationship , and only keep the latest valid version;

[0341] Among them, represents the dependency sequence related to the change event . This dependency sequence contains a set of all constraint identifiers related to the change event. That is, all constraint identifiers that depend on the current event will be recorded in the set in sequence.

[0342] represents a specific constraint identifier (constraint number) related to the change event . Each represents the th constraint identifier that the change event depends on. Each element in the set corresponds to a specific constraint, which needs to be updated or affected in the subsequent change or operation.

[0343] 3) Conflict association determination: If the constraint identifier is included in the conflict rollback set , add a conflict flag field to the corresponding record in the account book and point to the conflict set number. Among them, the conflict set number is a unique identifier that identifies and tracks a specific conflict record in the conflict rollback set. This number is generated when the system detects a conflict and is part of the conflict rollback set, helping the system to record the details of the conflict and its processing process in the account book.

[0344] After the arbitration is completed, an update is performed on the ledger. The update process is performed in chronological order and can specifically include the following steps e1-e4:

[0345] Step e1: Insert a new record item in the ledger index table and assign a unique event identifier ;

[0346] Step e2: Write the change content structure and the metadata set to the ledger main table:

[0347] Step e3: Perform a version sealing operation on the constraint involved in the conflict, move the invalid version into the archive area, retain the index reference but stop participating in subsequent solving;

[0348] Step e4: Update the ledger summary index table to record the ledger size, version number, and last synchronization time.

[0349] In specific applications, the ledger data structure adopts a hierarchical organization method. The upper layer is the index area, which stores the event number and time sequence index; the middle layer is the main record area, which stores the event main body information; and the lower layer is the archive area, which stores the invalid or conflicting versions.

[0350] At the end of each ledger period, the system can generate a ledger summary hash value:

[0351] ,

[0352] wherein, is the hash value of the ledger. It represents the unique identifier of the current ledger state (composed of multiple records). The hash value is obtained by hashing the ledger content (including records, timestamps, change content, metadata, etc.), which is used to ensure the integrity and consistency of the ledger.

[0353] is the unique identifier of each record. It is the mark of each record, used to distinguish different change records or semantic assertions.

[0354] is the timestamp, indicating the generation time of the change record. It is a time value accurate to seconds (or higher precision), used to record the insertion time of each record in the ledger. The timestamp helps to track and sort records, ensuring the chronological order of the ledger.

[0355] is the change content, indicating the change information related to the record. It describes the specific content of a certain change event or constraint, such as semantic constraints, reasoning results, or modification content. The change content recorded in the ledger is the actual data related to the current record.

[0356] Metadata, containing additional information about the record. For example, it can include the source of the change, version information, performer, related documents, or other descriptive supplementary information.

[0357] The hash value serves as a unique identifier for the ledger version, used for subsequent version verification and integrity checking.

[0358] When the system detects a constraint rollback event, the following operations are performed: 1) read the last valid version record corresponding to the constraint identifier from the ledger; 2) create a new event record with the change type identified as "rollback" and write the previous version hash value to the field ; 3) mark the previous version state as "frozen" in the ledger archive area, prohibiting it from being activated in subsequent solving processes.

[0359] where the field is the hash value of the previous period ledger, helping the system to perform rollback operations to ensure data consistency and integrity, avoiding the spread of conflicts and errors. The rollback activation process is to restore the previous ledger state according to the field and perform new data processing as needed.

[0360] Each action instruction corresponds to a record entry, including the execution timestamp, path number, observation result summary, and the number of constraint changes triggered by the action. This record is used to calculate the operation-constraint association mapping table of the system to support subsequent behavior analysis and consistency verification.

[0361] At the end of the ledger maintenance period, a ledger state snapshot is generated and stored in a separate version archive area. The snapshot contains the ledger digest hash value, constraint set hash value, and current logical entropy value . When subsequent period execution is performed, the system can perform consistency verification according to the snapshot, and the verification rule can be represented as follows:

[0362] ,

[0363] represents the ledger digest hash value of the previous period ( period). This value is calculated from all change records in the previous period ledger (including event identifier, change content, and metadata), used to uniquely identify the content and version of the ledger.

[0364] represents the current period ( a digest hash value of the ledger of the current period. The value is calculated from all change records in the current period ledger (including event identifier, change content and metadata) and is used to identify the content and version of the current ledger.

[0365] function for verifying the integrity of the ledger. If the ledger hash value of the current period matches the ledger hash value of the previous period and the ledger chain is correct, true is returned, indicating that the content and version of the ledger are consistent; otherwise, false is returned, indicating that the content of the ledger is inconsistent, which may be data corruption or rollback error.

[0366] The verification result of the consistency verification according to the snapshot can be recorded in the ledger control table as the basis for maintaining the consistency of the system.

[0367] Through the above steps, the system realizes the full-process traceable recording of semantic constraints, solution states and observation behaviors in each incremental period. Through change arbitration and audit ledger maintenance, the consistency, version order and operation verifiability of various information in the process of continuous updating of the semantic map can be ensured, providing data support for automatic semantic map maintenance in long-term operation.

[0368] In the implementation process of the embodiments of the present disclosure, the geometric-topological SLAM incremental updating step constructs a pose graph and updates a voxel occupancy grid based on sensor observation data and motion estimation results to obtain local and global geometric representations and topological structure information. The sparse imaging evidence generation step reconstructs the sparse response distribution of openings and occlusions in space based on the propagation model of wireless or acoustic waves. The analytical accessibility field calculation step calculates the feasibility parameters of operations such as passing through, stopping and passing through openings according to the geometric features of the environment and the parameters of the robot body. The formalized semantic constraint compilation step converts the above information into a set of logical constraints and mathematical constraints for formalizing the relationship between semantic objects. The formalized semantic constraint set generates a provably consistent semantic map increment after being solved by the incremental satisfiability solving and minimal conflict processing step, and the increment information is fed back to the geometric-topological SLAM incremental updating step for fusion and updating. The active exploration step driven by logical entropy selects actions according to the constraint solution space state of the current semantic map and triggers the next round of data collection. The change arbitration and audit ledger maintenance step performs data recording after each round of solving and updating.

[0369] The embodiments of the present disclosure have the following beneficial effects:

[0370] 1. By introducing incremental satisfiability solving and minimal conflict rollback mechanism, the logical consistency maintenance of semantic map in multi-period update process is realized. When new observations or new constraints are introduced, the system cannot be satisfied, the minimal conflict subset can be automatically identified and rollback operation is performed, which avoids the accumulation of errors or logical contradictions in semantic information in long-term operation, thereby ensuring the stable evolution and self-correction ability of semantic map.

[0371] 2. The proposed logic entropy driven active exploration mechanism can guide the robot to autonomously generate observation paths with information gain value by evaluating the uncertainty of the constraint system, realizing the deep integration of exploration decision and semantic reasoning. This mechanism can enable the robot to preferentially select the area with the greatest impact on semantic completeness for perception, thereby improving the information collection efficiency and environmental understanding ability in the incremental mapping process.

[0372] 3. The formal semantic constraint compilation mechanism can convert multi-source data from geometry, topology and availability information into a unified logical constraint set, and build a formal semantic expression framework from the perception layer to the reasoning layer. This mechanism realizes the structured and unified heterogeneous information, so that the semantic map has the characteristics of computability, verifiability and reasoning, providing a basis for semantic consistency solving.

[0373] 4. The sparse imaging evidence fusion mechanism is introduced to generate sparse intensity information that can be registered with the geometric map in the environment where visual or optical perception is limited, using the spatial propagation characteristics of wireless signals or acoustic signals. By fusing with voxel occupancy information, a multi-modal semantic perception base layer is formed to provide supplementary evidence for subsequent semantic recognition and constraint generation.

[0374] Figure 2 The structure of the semantic map construction device for robot navigation provided by the embodiment of the present disclosure is shown. Referring to Figure 2 The semantic map construction device for robot navigation provided by the embodiment of the present disclosure can include:

[0375] The incremental update module 201 is configured to obtain the mileage information and sensor observation data of the robot, update the geometric map and topological structure information of the environment where the robot is currently located based on the sensor observation data and the mileage information, and obtain voxel occupancy grid and topological graph structure data.

[0376] The sparse imaging evidence generation module 202 is configured to perform non-negative sparse reconstruction based on the wireless signal of the wireless transceiver unit and / or the acoustic signal of the acoustic transceiver unit to obtain a sparse intensity vector, project the sparse intensity vector to a world coordinate system through the external parameters of the wireless transceiver unit and / or the acoustic transceiver unit to form a sparse imaging evidence heat map, and the sparse imaging evidence heat map contains the sparse intensity normalized value and the coordinate index of each voxel.

[0377] The availability field calculation module 203 is configured to calculate a plurality of availability parameter fields related to robot size, kinematic and dynamic constraints based on the robot structure parameters, the sparse imaging evidence heat map, the voxel occupancy grid and the topological graph structure data, the plurality of availability parameter fields including a passable availability field, a parking availability field and a pass-through opening availability value;

[0378] The formal semantic constraint compilation module 204 is configured to convert geometric information, topological information and availability information in the voxel occupancy grid, the topological graph structure data, the plurality of availability parameter fields and the sparse imaging evidence heat map into a set of formal semantic constraints;

[0379] The solving and conflict processing module 205 is configured to perform incremental satisfiability solving on the set of formal semantic constraints, locate a minimum unsatisfiable subset if unsatisfiability occurs, approximately determine a minimum constraint set according to a rollback set and perform a minimum rollback processing to obtain a constraint solving output result, the constraint solving output result including a consistency solution space generated in a current period and a logic entropy value, each element in the consistency solution space containing a set of logic variable assignments and a corresponding constraint satisfaction state.

[0380] The active exploration module 206 is configured to generate an observation action sequence based on the consistency solution space generated in the current period and the logic entropy value.

[0381] The ledger maintenance module 207 is configured to record historical data of constraint changes and solving rollbacks.

[0382] In the semantic map construction device for robot navigation provided in the embodiments of the present disclosure, the construction and update of the semantic map are realized through the cooperative work of the modules. The modules exchange data through a unified interface format. The data interface between the modules adopts a unified structured format to realize incremental transmission and update. At the data interaction level, the modules exchange information through a shared data interface. The constraint solving module and the ledger maintenance module have a direct communication channel for recording change information as soon as the solving is completed. The active exploration module and the geometry-topology SLAM module exchange path planning data through shared memory.

[0383] The semantic map construction device for robot navigation provided in the embodiments of the present disclosure is periodically run and supports a multi-thread parallel execution mechanism, and the modules can be executed asynchronously according to the dependency relationship.

[0384] The semantic map construction apparatus for robot navigation in the embodiments of the present disclosure can be implemented as software, hardware or a combination of both. Illustratively, the semantic map construction apparatus for robot navigation in the embodiments of the present disclosure can be entirely run on a processor of a computing unit in the following system, and the operation results of all modules are stored in the form of a structured file or a database record with a unique index number to support subsequent data backtracking and verification.

[0385] Figure 3 A schematic structural diagram of the robot environment perception system based on multi-sensor fusion provided by the embodiments of the present disclosure is shown. Referring to Figure 3 The semantic map construction system for robot navigation provided by the embodiments of the present disclosure can include a sensor group 301, a computing unit 302, a wireless transceiver unit 303 and an acoustic transceiver unit 304, the sensor group 301, the wireless transceiver unit 303 and the acoustic transceiver unit 304 are respectively in communication connection with the computing unit 302, the computing unit 302 includes a processor and a memory, the memory stores a computer program, and the computer program causes the processor to execute the foregoing semantic map construction method for robot navigation when the computer program is run by the processor.

[0386] The wireless transceiver unit 303 can be used for electromagnetic wave propagation measurement, and the acoustic transceiver unit 304 can be used for acoustic wave propagation measurement.

[0387] In some examples, the sensor group 301 can include but is not limited to a ranging type sensor and an inertial measurement unit, the ranging type sensor can be but is not limited to a laser radar, a vision sensor, etc. The inertial measurement unit can be used to detect attitude changes and acceleration information.

[0388] The semantic map construction system for robot navigation in the embodiments of the present disclosure can further include a robot body, and the foregoing sensor group 301 and the computing unit 302 can be loaded on the robot body. The robot body has a movable structure and a driving execution mechanism for realizing path tracking and attitude control.

[0389] The computing unit 302 can include one or more processors and a memory.

[0390] The processor can be a central processing unit (CPU), a graphics processing unit (GPU), or other forms of processing units having data processing capabilities and / or instruction execution capabilities, and can control other components in the computing unit to perform desired functions.

[0391] The memory can include one or more computer program products, which can include various forms of computer-readable storage media, such as volatile memory and / or non-volatile memory. The volatile memory, for example, can include random access memory (RAM), and / or a cache, and / or the like. The non-volatile memory, for example, can include read-only memory (ROM), hard disk, flash memory, and / or the like. One or more computer program instructions can be stored on the computer-readable storage media, and the processor can run the program instructions to implement the methods of various embodiments of the present disclosure described above and / or other desired functions.

[0392] The computing unit 302 can also include any other appropriate components according to specific application cases.

[0393] Further, the memory can also be used to cache the data structures output by each module in the aforementioned semantic map construction device for robot navigation, and provide a timestamp indexing mechanism to ensure data synchronization. Specifically, the memory can also be used to cache the voxel occupancy grid, the topological graph structure data, the constraint set, the sparse intensity distribution, and the ledger structure.

[0394] In some examples, the computing unit 302 can also include a high-speed bus, and the communication between the sensor group 301, the wireless transceiver unit 303, and the acoustic transceiver unit 304 and the computing unit 302 can be realized through the high-speed bus interface.

[0395] In specific applications, in the system initialization stage, an initial ledger L0 is established, and an initial constraint identifier space is allocated. As the running period progresses, the system automatically performs incremental updating and version archiving operations. After the end of each running period, the system generates a semantic map version number and stores it in a global index table, which is used for consistency comparison with subsequent periods.

[0396] The system of the present embodiment can be continuously run on a single computing unit, and long-term incremental semantic mapping of the environment is realized through a modular data processing architecture. The module division and communication structure of the system correspond to the aforementioned method steps, and the independent running and result reuse of each processing stage can be realized, thereby ensuring the continuous consistency of the semantic map in the geometric, topological, and logical layers.

[0397] In addition to the methods and devices described above, embodiments of the present disclosure can also be computer program products, which include computer program instructions that, when run by a processor, cause the processor to perform the steps of the methods according to various embodiments of the present disclosure described in the method part of the present specification.

[0398] The computer program product can be written in any combination of one or more programming languages, including an object oriented programming language such as Java, C++ or the like, and conventional procedural programming languages, such as the "C" programming language or similar programming languages. The program code can execute entirely on the user's computing device, partly on the user's computing device, as a stand-alone software package, partly on the user's computing device and partly on a remote computing device or entirely on the remote computing device or server. The embodiments of methods described herein can be implemented in software, firmware, hardware, or any combination thereof.

[0399] Furthermore, embodiments of the present disclosure can also be a computer readable storage medium, having stored thereon computer program instructions which, when executed by a processor, cause the processor to perform steps of the methods described in the above “Exemplary Methods” section according to various embodiments of the present disclosure.

[0400] The computer readable storage medium can be any combination of one or more computer readable media. The computer readable medium can be a computer readable signal medium or a computer readable storage medium. A computer readable storage medium can be, for example, but not limited to, an electronic, magnetic, optical, electromagnetic, infrared, or semiconductor system, apparatus, or device, or any suitable combination of the foregoing. More specific examples (a non-exhaustive list) of the computer readable storage medium include an electrical connection having one or more wires, a portable disc, a hard disk, a random access memory (RAM), a read-only memory (ROM), an erasable programmable read-only memory (EPROM or Flash memory), an optical fiber, a portable compact disc read-only memory (CD-ROM), an optical storage device, a magnetic storage device, or any suitable combination of the foregoing.

[0401] The above description is given for illustrative and descriptive purposes. Furthermore, this description is not intended to limit the embodiments of the present disclosure to the forms disclosed herein. Although several example aspects and embodiments have been discussed above, those skilled in the art will recognize certain variations, modifications, changes, additions and sub-combinations thereof.

Claims

1. A semantic map construction method for robot navigation, characterized in that, include: The geometric-topology SLAM incremental update includes: acquiring the robot's odometer information and sensor observation data, and updating the geometric map and topological structure information of the robot's current environment based on the sensor observation data and odometer information to obtain voxel occupancy grid and topological graph structure data. Sparse imaging evidence generation includes: performing non-negative sparse reconstruction based on the wireless signal of the wireless transceiver unit and / or the acoustic signal of the acoustic transceiver unit to obtain a sparse intensity vector; projecting the sparse intensity vector onto the world coordinate system through the extrinsic parameters of the wireless transceiver unit and / or the acoustic transceiver unit to form a sparse imaging evidence heatmap; the sparse imaging evidence heatmap includes the sparse intensity normalized value of each voxel and its coordinate index. The analytical availability field calculation includes: calculating multiple availability parameter fields related to robot size, kinematics, and dynamic constraints based on robot structural parameters, the sparse imaging evidence heatmap, the voxel occupancy grid, and the topology graph structure data. The multiple availability parameter fields include a passable availability field, a docking availability field, and a pass-through availability value. Formal semantic constraint compilation, wherein formal semantic constraint compilation includes: converting the voxel-occupied grid, the topological graph structure data, the multiple availability parameter fields, the geometric information, topological information and availability information in the sparse imaging evidence heatmap into a set of formal semantic constraints; Incremental satisfiability solving and minimum conflict handling, the incremental satisfiability solving and minimum conflict handling includes: performing incremental satisfiability solving on the formal semantic constraint set; if unsatisfiable constraints occur, locating the minimum unsatisfiable subset; approximating the minimum constraint set based on the rollback set and performing minimum rollback processing to obtain the constraint solution output result; the constraint solution output result includes: the consistent solution space generated in the current period and the logical entropy value; each element in the consistent solution space contains a set of logical variable assignments and the corresponding constraint satisfaction state; Logical entropy-driven active exploration includes: generating an observation action sequence based on the consistent solution space and logical entropy value generated in the current period; The maintenance of the updated arbitration and audit ledger includes recording historical data on constraint changes and solution rollbacks.

2. The method according to claim 1, characterized in that, The sparse imaging evidence generation includes: constructing a measurement equation. ,in, For the observation vector, The measurement matrix is ​​composed of the sensor array geometry and the propagation path. It is a non-negative sparse intensity vector. For noise terms; perform based on the measurement equation. The nonnegative sparse reconstruction with norm minimization yields a sparse intensity vector; the sparse intensity vector is mapped to a voxel grid using the extrinsic parameters of the wireless transceiver unit and / or the acoustic transceiver unit to generate a sparse imaging evidence heatmap; the sparse imaging evidence heatmap and the voxel-occupied grid are normalized and fused at the voxel layer to obtain the probability of the opening.

3. The method according to claim 1, characterized in that, The geometric-topological SLAM incremental update further includes: performing probability normalization fusion on the voxel-occupied grid and the sparse imaging evidence heatmap at the voxel layer to update the opening existence probability; and generating topological edges between two free-space connected components based on the updated opening existence probability.

4. The method according to claim 1, characterized in that, The formal semantic constraint compilation includes: generating hard constraints for door-room connections, constructing connectivity constraints using the topological connection relationship between the opening candidate object and two different room candidate objects as variables; generating hard constraints for opening dimensions, constructing threshold constraints using the net width and minimum net width, and net height and minimum net height of the opening candidate object as variables; generating soft constraints for room compactness, constructing upper limit constraints using the ratio of the principal axis eigenvalues ​​of the room candidate objects as variables; and generating soft constraints for corridor slenderness, constructing lower limit constraints using the ratio of the principal axis eigenvalues ​​of the corridor candidate objects as variables.

5. The method according to claim 4, characterized in that, The formal semantic constraint compilation also includes: generating evidence consistency soft constraints for opening candidate objects, constructing a proportion threshold constraint with the proportion of high-response voxels in the sparse imaging evidence heatmap within the coverage area of ​​the opening candidate object as a variable, and constructing an adjacency constraint with the intersection relationship between the neighborhood of the opening candidate object and the free space boundaries of the two room candidate objects as a variable.

6. The method according to claim 5, characterized in that, The incremental satisfiability solution and minimum conflict handling include: constructing an incremental satisfiability problem model for the hard constraints and the soft constraints with relaxed variables; performing local re-solution on the affected variable block after adding new constraints; when unsatisfiability is detected, calculating the minimum unsatisfiable subset, and calculating a rollback set approximation based on the minimum unsatisfiable subset to obtain the minimum constraint set to be removed, performing minimum rollback processing according to the minimum constraint set, and solving again.

7. The method according to claim 1, characterized in that, The active exploration driven by logical entropy includes: generating a set of candidate actions based on the consistent solution space and logical entropy value generated in the current cycle; enumerating observations and constructing a solution space that can be satisfied at the next time step according to the forward measurement model; performing cardinality estimation on the solution space that can be satisfied and calculating logical entropy; calculating the expected value of logical entropy for each candidate action; selecting the candidate action with the smallest expected value of logical entropy and executing it; generating the observation action sequence and triggering the next cycle of the geometric-topology SLAM incremental update and the generation of the sparse imaging evidence heatmap.

8. The method according to claim 1, characterized in that, The analytical availability field calculation includes: Free-space erosion-dilation is performed to obtain the net width of the passage, and the ratio of the net width of the passage to the robot width is calculated. The local slope is estimated by the ground normal and the difference is calculated with the maximum allowable slope. The curvature of the passage is obtained by fitting the centerline of the passage and the difference is calculated with the curvature corresponding to the minimum turning radius. The ratio of the net width of the passage to the robot width, the difference between the local slope and the maximum allowable slope, and the difference between the curvature of the passage and the curvature corresponding to the minimum turning radius are normalized and the minimum value is taken as the passability availability field value of the voxel. The ratio of vertical clearance to minimum clearance, the difference between surface inclination angle and friction cone angle, and the flatness criterion are calculated for the waiting platform surface. The flatness criterion is applied and binarized. The minimum value of the ratio of vertical clearance to minimum clearance, the difference between surface inclination angle and friction cone angle, and the flatness criterion are taken as the docking availability field value after binarization and normalization. For each candidate object for an opening, calculate the ratio of the net width to the minimum net width, the ratio of the net height to the minimum net height, and the ratio of the free voxel density on both sides to the reference density. After normalizing these ratios, take the minimum value as the availability value for opening.

9. A semantic map building device for robot navigation, characterized in that, The semantic map building apparatus for robot navigation includes: The incremental update module is used to acquire the robot's mileage information and sensor observation data, and update the geometric map and topology information of the robot's current environment based on the sensor observation data and mileage information to obtain voxel occupancy grid and topology structure data. A sparse imaging evidence generation module is used to perform non-negative sparse reconstruction based on the wireless signal of the wireless transceiver unit and / or the acoustic signal of the acoustic transceiver unit to obtain a sparse intensity vector, and project the sparse intensity vector onto the world coordinate system through the extrinsic parameters of the wireless transceiver unit and / or the acoustic transceiver unit to form a sparse imaging evidence heatmap. The sparse imaging evidence heatmap contains the sparse intensity normalized value of each voxel and its coordinate index. The availability field calculation module is used to calculate various availability parameter fields related to robot size, kinematics and dynamic constraints based on robot structural parameters, the sparse imaging evidence heatmap, the voxel occupancy grid and the topology graph structure data. The various availability parameter fields include the passability availability field, the docking availability field and the access availability value. A formal semantic constraint compilation module is used to transform the voxel-occupied grid, the topological graph structure data, the various availability parameter fields, and the geometric, topological, and availability information in the sparse imaging evidence heatmap into a set of formal semantic constraints. The solution and conflict handling module is used to perform incremental satisfiability solving on the formal semantic constraint set. If unsatisfiable constraints are found, the minimum unsatisfiable subset is located, the minimum constraint set is approximately determined based on the rollback set, and minimum rollback processing is performed to obtain the constraint solution output result. The constraint solution output result includes: the consistent solution space generated in the current period and the logical entropy value. Each element in the consistent solution space contains a set of logical variable assignments and the corresponding constraint satisfaction state. The active exploration module is used to generate a sequence of observation actions based on the consistent solution space and logical entropy value generated in the current cycle. The ledger maintenance module is used to record historical data on constraint changes and solution rollbacks.

10. A semantic map construction system for robot navigation, characterized in that, The system includes: a sensor group, an antenna transceiver unit, an acoustic transceiver unit, and a computing device. The computing device includes a processor and a memory. The memory stores a computer program, which, when run by the processor, causes the processor to perform the method as described in any one of claims 1-8.

Images