Simulation and Verification Method and System for Navigation, Measurement and Control of Mining Equipment Based on Inertial Devices

Abstract

This invention discloses a method and system for simulating and verifying navigation and control of mining equipment based on inertial devices, relating to the field of inertial navigation. The method includes: constructing a simulated operating environment for the mining equipment and performing three-dimensional terrain analysis; constructing a terrain feature map and simulating motion to obtain a terrain scan point cloud dataset; performing real-time spatial registration of the point cloud dataset and the terrain feature map to generate terrain-constrained pose correction values; traversing the point cloud dataset and performing bidirectional priority search to generate a calculated trajectory; using the terrain-constrained pose correction values ​​for fusion correction to generate an optimal navigation state estimation sequence; simulating the optimal estimation sequence and performing point-by-point comparison to calculate the cumulative error distribution data for navigation and control simulation verification of the mining equipment. This application solves the problem that existing mining equipment navigation and positioning errors diverge over operating time and cannot be effectively corrected externally, achieving the effect of effectively constraining and correcting cumulative inertial navigation errors.

Description

Technical Field

[0001] This application relates to the field of inertial navigation, and in particular to a simulation verification method and system for navigation, measurement and control of mining equipment based on inertial devices. Background Technology

[0002] The accuracy of navigation and control of mining equipment in underground roadways directly affects the safety and production efficiency of mining operations, and is the core guarantee for ensuring that mining equipment can autonomously cut and move along the designed roadway cross-section. Currently, a dead reckoning navigation method is commonly used, which combines a strapdown inertial navigation system with an odometer. In this system, the inertial devices measure the three-axis angular velocity and three-axis acceleration of the mining equipment, and the navigation computer performs integration calculations to obtain the real-time position and attitude information of the equipment. At the same time, the odometer provides distance constraints to suppress long-term cumulative drift of inertial navigation. The combination of these two systems forms the navigation and control technology solution for mining equipment. However, the actual working conditions in the tunnel environment lead to certain limitations of the above navigation methods. Inertial devices are affected by factors such as temperature changes and vibration and shock, and their measurement output contains drift errors that increase continuously over time. Odometers are prone to slippage and wheel speed measurement deviations on soft or slippery tunnel floors. Both of these factors cause the navigation calculation results that rely solely on the integration of internal sensor measurements to gradually deviate from the true pose as the operation time increases. The positioning error continues to accumulate and cannot be effectively corrected during downhole operations.

[0003] At present, the relevant technologies have the technical problem that the navigation and positioning error of mining equipment during long-distance autonomous navigation underground can continue to diverge with the operation time and cannot be effectively corrected externally in the underground environment. Summary of the Invention

[0004] This application provides a simulation verification method and system for navigation and control of mining equipment based on inertial devices. By constructing an environment and map and collecting point clouds, registering the point clouds and map to obtain correction values, solving the trajectory through bidirectional search and fusing the correction values ​​to obtain the optimal estimate, and comparing the optimal estimate with a benchmark to verify the navigation and control performance, this application solves the technical problem that the navigation and positioning error of existing mining equipment during long-distance autonomous navigation underground continues to diverge with the operation time and cannot be effectively corrected externally in the underground environment. It achieves the technical effect of effectively constraining and correcting the cumulative error of inertial navigation by utilizing the terrain features of the roadway itself in an underground environment without external artificial beacons, so that the navigation and positioning error remains convergent during long-distance operations.

[0005] This application provides a simulation verification method for navigation and control of mining equipment based on inertial devices, comprising: constructing a simulated operating environment for the mining equipment and performing three-dimensional terrain analysis; constructing a terrain feature map and simulating motion to obtain a terrain scan point cloud dataset; performing real-time spatial registration of the terrain scan point cloud dataset and the terrain feature map to generate terrain constraint pose correction quantities; traversing the terrain scan point cloud dataset and performing bidirectional priority search calculation to generate a calculated trajectory; using the terrain constraint pose correction quantities as observations to fuse and correct the calculated trajectory to generate an optimal navigation state estimation sequence; simulating the optimal navigation state estimation sequence and performing point-by-point comparison; calculating the cumulative error distribution data to simulate and verify the navigation and control of the mining equipment using inertial devices.

[0006] In a possible implementation, a simulated mining equipment operating environment is constructed for 3D terrain analysis, a terrain feature map is constructed for simulated motion, and a terrain scan point cloud dataset is obtained. The following processes are then performed: 3D geometric analysis is conducted based on the simulated mining operating environment to calculate a 3D tunnel geometric model; the 3D tunnel geometric model is meshed, dividing it into multiple mesh surface parameters; point cloud analysis is performed based on the multiple mesh surface parameters to generate a dense reference point cloud dataset; multi-scale division is performed based on the dense reference point cloud dataset to construct a multi-scale voxel pyramid; the multi-scale voxel pyramid is traversed to calculate the eigenvalue ratio, and extreme value calculation is performed based on the eigenvalue ratio to determine multiple candidate feature points; 3D occupancy grid parameters are constructed based on the multiple candidate feature points according to normal constraints, and the 3D occupancy grid is added to the terrain feature map.

[0007] In a possible implementation, a three-dimensional occupancy grid parameter is constructed based on the multiple candidate feature points according to normal constraints. The three-dimensional occupancy grid is added to the terrain feature map, and the following processing is performed: traversing the multiple candidate feature points to identify neighborhoods, determining multiple neighborhood point sets, calculating covariance based on the multiple neighborhood point sets, and constructing a covariance matrix; extracting the minimum eigenvalue of the matrix based on the covariance matrix and calculating local normal vectors for the multiple candidate feature points to construct normal constraints; using the multiple candidate feature points as centers to expand the space according to the normal constraints, constructing multiple rectangular grid data; identifying the normal vectors based on the multiple rectangular grid data, dividing them into normal vector pointing regions and normal vector deflection regions; marking the normal vector pointing regions as occupied and the normal vector deflection regions as idle; discarding the multiple rectangular grid data according to the idle markings and retaining the multiple rectangular grid data according to the occupied markings, thus constructing the three-dimensional occupancy grid.

[0008] In a possible implementation, the terrain scan point cloud dataset and the terrain feature map are spatially registered in real time to generate a terrain constraint pose correction value. The following processes are then performed: the terrain scan point cloud dataset is mapped to the terrain feature map for spatial calculation to obtain an error covariance matrix; a position error submatrix and an attitude error submatrix are extracted based on the error covariance matrix; the position major and minor axes are calculated based on the position error submatrix to obtain position uncertainty, and the attitude uncertainty is obtained through local calculation based on the attitude error submatrix; a 3D spatial search box is constructed by expanding outward based on the position uncertainty and the attitude uncertainty; a local search range is defined by performing a local intersection search between the 3D spatial search box and the 3D occupied grid; a 3D description label is generated based on the terrain scan point cloud dataset to construct a scan point feature description set; bidirectional nearest neighbor matching is performed on the scan point feature description set according to the local search range to construct a set of matching point pairs; and the terrain feature map is calculated by performing a transformation coordinate accumulation based on the set of matching point pairs to construct the terrain constraint pose correction value.

[0009] In a possible implementation, the position uncertainty is obtained by calculating the major and minor axes of the position based on the position error submatrix, and then the following processing is performed: eigenvalue decomposition is performed on the position error submatrix to obtain N orthogonal eigenvectors, each containing N eigenvalues, where N is an integer greater than 1; N principal axis directions are set based on the N orthogonal eigenvectors, and N semi-axis length values ​​are set based on the N eigenvalues ​​according to the N principal axis directions; position uncertainty ellipsoid parameters are constructed based on the N principal axis directions and the N semi-axis length values; and the position uncertainty is obtained by calculating the major and minor axes of the position based on the position uncertainty ellipsoid parameters.

[0010] In a possible implementation, bidirectional nearest neighbor matching is performed on the scan point feature description set according to the local search range to construct a matching point pair set, and the following processing is performed: traversing the scan point feature description set to extract multiple feature descriptors, performing local search on the multiple feature descriptors according to the local search range, and calculating multiple distance parameters; arranging the multiple distance parameters in ascending order to construct multiple distance parameter sequences, extracting the first-order value to determine the first distance parameter, extracting the second-order value to determine the second distance parameter; calculating the ratio between the first distance parameter and the second distance parameter to obtain distance ratio data; presetting a first threshold and a second threshold, where the first threshold is greater than the second threshold; when the distance ratio data is less than the first threshold, generating a first matching reception signal, performing forward matching through the first matching reception signal to generate a first matching dataset; when the distance ratio data is less than the second threshold, generating a second matching reception signal, performing reverse matching through the second matching reception signal to generate a second matching dataset; and constructing the matching point pair set based on the data intersection of the first matching dataset and the second matching dataset.

[0011] In a possible implementation, the terrain scan point cloud dataset is traversed using a bidirectional priority search to generate a calculated trajectory, and the following processing is performed: Initial pose estimation is performed by traversing the terrain scan point cloud dataset forward along the time axis based on a depth-first search, generating the first pose estimation data; the first pose estimation data is matched with the terrain feature map to generate a first matched pose; the pose error value between the first matched pose and the first pose estimation data is calculated and filtered to generate a forward filtered trajectory sequence; initial pose estimation is performed by traversing the terrain scan point cloud dataset backward along the time axis based on a breadth-first search, generating the second pose estimation data. The first matching pose is used as a single-point observation constraint to perform constraint analysis on the forward-filtered trajectory sequence, and the forward-optimized trajectory parameters are solved. The second matching pose is used as a multi-point observation constraint to perform constraint analysis on the backward-filtered trajectory sequence, and the reverse-optimized trajectory parameters are solved. The forward-optimized trajectory parameters and the reverse-optimized trajectory parameters are bidirectionally correlated to construct the inferred trajectory.

[0012] In a possible implementation, the optimal estimation sequence of the navigation state is simulated and compared point by point to calculate the cumulative error distribution data. The following processing is performed: the optimal estimation sequence of the navigation state is time-stamp aligned with the preset reference trajectory sequence to calculate the three-dimensional position error and construct an error vector time series; the error vector time series is traversed to perform multi-time three-dimensional position error calculation, the position error sequence is identified to perform position boundary analysis, and the position error boundary value is defined; the cumulative error distribution data is located by performing point-by-point calculation based on the position error boundary value.

[0013] In a possible implementation, the cumulative error distribution data is used to simulate and verify the navigation and control of the mining equipment using inertial devices. The following processing is performed: based on the cumulative error distribution data, multi-dimensional error geometric feature parameters are extracted for error feature analysis. Multiple error features are identified to trace the source of errors in the inertial devices and locate the error source type data, which has a contribution coefficient. The mining equipment using inertial devices is controlled according to the contribution coefficient and the error source type to construct navigation and control adjustment commands. The navigation and control adjustment commands are simulated and executed iteratively until the multi-dimensional error geometric feature parameters meet a preset verification threshold, thus completing the simulation verification closed loop.

[0014] This application also provides a navigation and control simulation verification system for mining equipment based on inertial devices, comprising: a motion simulation module for constructing a simulated mining equipment operating environment for three-dimensional terrain analysis, constructing a terrain feature map for simulated motion, and obtaining a terrain scan point cloud dataset; a terrain constraint pose correction generation module for real-time spatial registration of the terrain scan point cloud dataset and the terrain feature map to generate a terrain constraint pose correction; a navigation state optimal estimation sequence generation module for traversing the terrain scan point cloud dataset for bidirectional priority search and calculation, generating a calculated trajectory, and using the terrain constraint pose correction as an observation to fuse and correct the calculated trajectory to generate a navigation state optimal estimation sequence; and a navigation and control simulation verification module for simulating the navigation state optimal estimation sequence for point-by-point comparison, calculating the cumulative error distribution data, and performing simulation verification of navigation and control for mining equipment using inertial devices.

[0015] The proposed method and system for simulating and verifying navigation and control of mining equipment based on inertial devices, as described in this application, firstly constructs a simulated operating environment for the mining equipment and performs 3D terrain analysis, constructs a terrain feature map, simulates motion, and obtains a terrain scanning point cloud dataset. Next, the terrain scanning point cloud dataset and the terrain feature map are spatially registered in real time to generate terrain-constrained pose correction quantities. Then, the terrain scanning point cloud dataset is traversed for bidirectional priority search calculation to generate a calculated trajectory. The terrain-constrained pose correction quantities are used as observations to fuse and correct the calculated trajectory, generating an optimal navigation state estimation sequence. Finally, the optimal navigation state estimation sequence is simulated and compared point by point to calculate the cumulative error distribution data, thus simulating and verifying the navigation and control of mining equipment using inertial devices. Through the above process, the method and system proposed in this application achieve the technical effect of effectively constraining and correcting the cumulative error of inertial navigation in an underground environment without external artificial beacons, utilizing the terrain features of the roadway itself, and keeping the navigation and positioning error in a convergent state during long-distance operations. Attached Figure Description

[0016] To more clearly illustrate the technical solutions of the embodiments of the present invention, the accompanying drawings of the embodiments of the present invention will be briefly described below. Flowcharts are used in this application to illustrate the operations performed by the system according to the embodiments of the present application. It should be understood that the preceding or following operations are not necessarily performed precisely in sequence. Instead, various steps can be processed in reverse order or simultaneously as needed. Furthermore, other operations can be added to these processes, or one or more steps can be removed from these processes.

[0017] Figure 1 This is a flowchart illustrating the simulation and verification method for navigation, measurement, and control of mining equipment based on inertial devices, provided in an embodiment of this application.

[0018] Figure 2 This is a schematic diagram of the position uncertainty ellipsoid provided for an embodiment of this application.

[0019] Figure 3 This is a schematic diagram of the structure of the inertial device-based navigation and control simulation verification system for mining equipment provided in an embodiment of this application.

[0020] Figure labeling: Simulation motion module 10, terrain constraint pose correction generation module 20, navigation state optimal estimation sequence generation module 30, navigation measurement and control simulation verification module 40. Detailed Implementation

[0021] To further illustrate the technical means and effects of the present invention in achieving its intended purpose, the following detailed description of the specific implementation methods, structures, features, and effects of the present invention, in conjunction with the accompanying drawings and preferred embodiments, is provided below.

[0022] This application provides a simulation and verification method for navigation, measurement, and control of mining equipment based on inertial devices, such as... Figure 1 As shown, the method includes: Step S100: Construct a simulated mining equipment operating environment for three-dimensional terrain analysis, construct a terrain feature map for simulated motion, and obtain a terrain scan point cloud dataset.

[0023] Specifically, a simulated operating environment for mining equipment is constructed. This environment is a 3D virtual scene of an underground coal mine roadway or metal mine roadway, containing geometric elements such as roadway walls, roof, floor, and roadway intersections. A 3D geometric modeling tool is used to create a geometric model of this scene, which describes the roadway spatial boundaries in the form of a triangular mesh. A 3D terrain analysis is performed on this geometric model, analyzing the roadway cross-sectional shape, roadway axis orientation, roadway slope changes, and the spatial topological relationships at roadway intersections. After completing the 3D terrain analysis, a terrain feature map is constructed to characterize the geometric features and occupancy status of the roadway space, providing spatial reference for the simulated movement. The simulated movement refers to simulating the movement of the mining equipment in the roadway environment, with the movement path encompassing straight sections, turning sections, and slope sections. During the simulated movement, a laser scanner mounted on the mining equipment scans the environment, with scanning parameters including horizontal field of view, vertical field of view, scanning angular resolution, and maximum distance measurement. The laser scanner emits a laser beam and receives the reflected signal, recording the three-dimensional spatial coordinates of each scan point. All scan points constitute a terrain scan point cloud dataset.

[0024] In one possible implementation, a simulated mining equipment operating environment is constructed for 3D terrain analysis, a terrain feature map is constructed for simulated motion, and a terrain scan point cloud dataset is obtained. Step S100 further includes step S110, which involves performing 3D geometric analysis based on the simulated mining operating environment to calculate a 3D tunnel geometric model. Specifically, the simulated mining operating environment refers to a complete underground operating space including tunnels, chambers, mining faces, and transport tunnels. 3D geometric analysis involves analytically representing all wall boundaries in the operating space, using computer-aided design and modeling software to establish tunnel cross-sectional contour lines, and stretching or lofting the cross-sectional contour lines along the tunnel centerline path to generate a continuous tunnel wall solid model. For tunnel intersections and turns, a surface splicing method is used to handle cross-sectional transitions. The 3D tunnel geometric model is stored in boundary representation form, including a vertex coordinate list, a triangular facet index list, and a facet normal vector list. The modeling software output format adopts a standard triangular mesh file format.

[0025] Step S120 involves meshing the three-dimensional tunnel geometry model, dividing it into multiple mesh surface parameters. Specifically, the triangular faces of the three-dimensional tunnel geometry model are used as input for meshing, i.e., continuous triangular faces are discretized into regular spatial mesh units. Specifically, the maximum and minimum coordinate values ​​of the three-dimensional tunnel geometry model along the three spatial coordinate axes are calculated to obtain the model bounding box. The side length of the mesh unit is set to a fixed size, which is determined based on the minimum cross-sectional size of the tunnel. The bounding box is divided at equal intervals along the three spatial coordinate axes according to the side length of the mesh unit, forming a three-dimensional mesh array. Each mesh unit is uniquely identified by its mesh index along the three coordinate axes. All mesh units are traversed, and it is determined whether the center point of the mesh unit is located inside the three-dimensional tunnel geometry model. If it is located inside, the mesh unit is marked as an internal mesh unit. The set of indices of all internal mesh units constitutes multiple mesh surface parameters, each containing the index value of the mesh unit along the three coordinate axes and the coordinates of the center point of the mesh unit.

[0026] Step S130: Based on the multiple grid surface parameters, point cloudification analysis is performed to generate a dense reference point cloud dataset. The dense reference point cloud dataset is then divided into multiple scales to construct a multi-scale voxel pyramid. Specifically, point cloudification analysis refers to converting grid cells into three-dimensional spatial points. For each internal grid cell, its center point coordinates are used as a reference point. Simultaneously, multiple internal points are generated within the grid cell using a uniform random sampling method. The center points and internal points of all internal grid cells together constitute the dense reference point cloud dataset. Each point in the dense reference point cloud dataset contains three-dimensional spatial coordinates.

[0027] The construction process of the multi-scale voxel pyramid is as follows: The number of pyramid layers is set to a fixed number, and the voxel side length of the finest layer is set to the base voxel side length. For the zeroth layer (the finest layer), the voxel side length equals the base voxel side length; for the kth layer, the voxel side length equals the base voxel side length multiplied by 2 to the power of k. For each layer of the pyramid, the bounding box of the dense reference point cloud dataset is divided into voxels based on the voxel side length of that layer, with each voxel being a cube unit. All voxels in that layer are traversed, and the number of reference points falling within each voxel is counted. If the number of reference points exceeds a preset threshold, the voxel is marked as an occupied voxel. The set of occupied voxels across all layers constitutes the multi-scale voxel pyramid, with each voxel storing its layer number, 3D spatial index, and center coordinates.

[0028] Step S140: Traverse the multi-scale voxel pyramid to calculate the eigenvalue ratio, and perform extreme value calculation based on the eigenvalue ratio to determine multiple candidate feature points. Specifically, traverse all occupied voxels in the multi-scale voxel pyramid. For each occupied voxel, search for neighboring points within the radius of the voxel's center and a fixed multiple of its side length in the point cloud data of that layer, forming a local point set for that voxel. Calculate the covariance matrix of the local point set, which is a 3x3 symmetric matrix. Perform eigenvalue decomposition on the covariance matrix to obtain three eigenvalues, which are arranged in descending order and denoted as the first eigenvalue, second eigenvalue, and third eigenvalue. The eigenvalue ratio is the ratio of the third eigenvalue to the first eigenvalue. After calculating the eigenvalue ratios of all voxels occupying a layer, extreme value calculations are performed on the eigenvalue ratios. That is, a local minimum is searched among all eigenvalue ratios. The criterion for a local minimum is that the eigenvalue ratio of the voxel is less than the eigenvalue ratios of all its neighboring voxels. Neighboring voxels are defined as six voxels that share a face with the voxel in space. The center point of the voxel whose eigenvalue ratio reaches a local minimum is selected as the candidate feature point for that layer. This process is repeated for all layers, merging the candidate feature points from all layers and removing duplicate points whose spatial distance is less than a preset merging distance threshold. Finally, multiple candidate feature points are obtained, each containing three-dimensional spatial coordinates and its layer number.

[0029] Step S150: Based on the multiple candidate feature points, construct three-dimensional occupancy grid parameters according to normal constraints, and add the three-dimensional occupancy grid to the terrain feature map. Specifically, the normal constraint refers to the local surface normal vector direction of each candidate feature point, which is used to constrain the spatial expansion direction of the grid. The three-dimensional occupancy grid parameters refer to the position, size, and occupancy status of the grid cell in space. Specifically, with each candidate feature point as the center, spatial expansion is performed along the local surface normal vector direction of that point to generate a rectangular grid cell surrounding that point. All rectangular grid cells constitute the three-dimensional occupancy grid. The constructed three-dimensional occupancy grid is added to the terrain feature map, which contains the spatial index and occupancy status identifier of all three-dimensional occupancy grids.

[0030] In one possible implementation, a three-dimensional occupancy grid parameter is constructed based on the multiple candidate feature points according to normal constraints. The three-dimensional occupancy grid is added to the terrain feature map. Step S150 further includes step S151, which involves traversing the multiple candidate feature points to perform neighborhood identification, determining multiple neighborhood point sets, and calculating the covariance based on the multiple neighborhood point sets to construct a covariance matrix. Specifically, for each candidate feature point, a neighborhood search radius is set with the three-dimensional spatial coordinates of the point as the center. All points in the dense reference point cloud dataset whose Euclidean distance from the point is less than the neighborhood search radius are searched, and these points constitute the neighborhood point set of the candidate feature point. Neighborhood identification refers to recording the spatial positional relationship of each point in the neighborhood point set relative to the central candidate feature point. The above neighborhood search and identification operations are performed on all candidate feature points to obtain multiple neighborhood point sets, each neighborhood point set corresponding one-to-one with a candidate feature point.

[0031] For each neighborhood point set, covariance is calculated. Let the neighborhood point set contain L points, and the three-dimensional spatial coordinates of each point be P. m Let m be the index of a point in the neighborhood set. First, calculate the centroid coordinates of the neighborhood set. The centroid coordinates are calculated as the average of the coordinates of all points, i.e., summing the coordinate values ​​along the three coordinate axes and dividing by L. Then, construct a 3x3 covariance matrix. The element in the i-th row and j-th column of the covariance matrix is ​​calculated as follows: for all neighborhood points, multiply the difference between the coordinate value in the i-th direction and the coordinate value of the centroid in the i-th direction by the difference between the coordinate value in the j-th direction and the coordinate value of the centroid in the j-th direction, sum the products, and divide by L. Here, i and j can take the values ​​of the first, second, or third direction, respectively. The covariance matrix is ​​obtained through the above calculations.

[0032] Step S152: Based on the covariance matrix, extract the minimum eigenvalue of the matrix and calculate the local normal vectors for multiple candidate feature points to construct normal constraints. Specifically, perform eigenvalue decomposition on the covariance matrix to obtain three eigenvalues, arrange them in ascending order of size, and take the eigenvector corresponding to the minimum eigenvalue as the local normal vector of the candidate feature point. This local normal vector is a three-dimensional unit vector, representing the local surface orientation at the candidate feature point. The local normal vectors of all candidate feature points constitute normal constraints, used to limit the direction of spatial expansion.

[0033] Step S153: Using the multiple candidate feature points as centers, spatial expansion is performed according to the normal constraint to construct multiple rectangular grid data. Specifically, each candidate feature point is used as the center point of the rectangular grid, and the local normal vector direction of that point is used as the principal axis direction of the rectangular grid. The spatial expansion method is as follows: extending forward along the local normal vector direction by a first expansion length and extending backward by a second expansion length, and extending along two orthogonal directions in the plane perpendicular to the local normal vector by a third expansion length and a fourth expansion length, respectively. The first expansion length, second expansion length, third expansion length, and fourth expansion length are preset fixed values, which are set according to the local flatness of the tunnel wall. After the above spatial expansion, each candidate feature point generates a rectangular grid, which is defined by eight vertex coordinates and six patch indices. The rectangular grids generated by all candidate feature points constitute multiple rectangular grid data.

[0034] Step S154: Based on the multiple rectangular grid data, normal vectors are identified and divided into normal vector pointing regions and normal vector deviating regions. Specifically, a normal vector pointing region refers to a region on the surface of the rectangular grid where the angle between the direction of the external normal vector of each point and the direction of the local normal vector of the candidate feature point is less than 90°, meaning the local normal vector points into this region. A normal vector deviating region refers to a region on the surface of the rectangular grid where the angle between the direction of the external normal vector of each point and the direction of the local normal vector of the candidate feature point is greater than or equal to 90°, meaning the local normal vector deviates from this region. By calculating the dot product of the external normal vector and the local normal vector at the center point of each patch of the rectangular grid, a positive dot product indicates a region pointing to the normal vector, while a negative or zero dot product indicates a region deviating from the normal vector.

[0035] Step S155: The areas pointed to by the normal vectors are marked as occupied, and the areas deviating from the normal vectors are marked as idle. Based on the idle markings, the multiple rectangular grid data are discarded, and based on the occupied markings, the multiple rectangular grid data are retained, thus constructing the three-dimensional occupied grid. Specifically, an occupied marking indicates that the area pointed to by the normal vector is in an occupied state, signifying that the area belongs to the tunnel wall entity. An idle marking indicates that the area deviating from the normal vector is in an idle state, signifying that the area belongs to the passage space inside the tunnel. Based on the idle markings, the portion of the rectangular grid belonging to the area deviating from the normal vector is discarded from the rectangular grid data; that is, this portion is not stored as an occupied grid. Based on the occupied markings, the portion of the rectangular grid belonging to the area pointed to by the normal vector is retained; that is, this portion is retained as an occupied grid. All the retained areas pointed to by the normal vector form the three-dimensional occupied grid, which records the occupied position and boundary information of the tunnel wall in space. The three-dimensional occupied grid is added to the terrain feature map to complete the construction of the terrain feature map.

[0036] For example, suppose a candidate feature point is selected in a straight tunnel with three-dimensional spatial coordinates (10.0, 5.0, 2.0) in meters. The neighborhood of this point contains 30 points. After covariance calculation and eigenvalue decomposition, the local normal vector is (0.0, 0.0, 1.0), pointing vertically upwards towards the roof. The first extension length is set to 0.3 meters, the second extension length to 0.1 meters, the third extension length to 0.2 meters, and the fourth extension length to 0.2 meters. Centered on this candidate feature point, extend upwards by 0.3 meters and downwards by 0.1 meters along the local normal vector direction, and extend horizontally by 0.2 meters along both the tunnel width and length directions, forming a rectangular grid. The six faces of this rectangular grid are labeled with normal vectors. The outer normal vector of the top face is (0.0, 0.0, 1.0), and its dot product with the local normal vector is positive. The top face is then divided into the area pointed to by the normal vector, marked, and stored. The outer normal vector of the bottom surface is (0.0, 0.0, -1.0), and its dot product with the local normal vector is negative. The bottom surface is classified as a region with a negative normal vector, and after being marked as empty, it is removed. The outer normal vectors of the four sides are all perpendicular to the local normal vectors, and their dot product is zero. These are also classified as regions with a negative normal vector and removed. The final 3D occupied grid is a rectangular thin area near the top plate. The horizontal coordinates of its four vertices are (9.8, 4.8), (9.8, 5.2), (10.2, 4.8), and (10.2, 5.2), respectively, and the vertical coordinate is 2.3 meters, which is the position of the lower surface of the top plate. In addition to the candidate feature points at the top slab mentioned above, two additional sets of candidate feature points are set up in the simulation scene for grid construction demonstration of the floor slab and tunnel sidewalls: the second set of candidate feature points has three-dimensional coordinates of (12.0, 5.0, 2.0), and its local normal vector points vertically downwards towards the floor slab; the third set of candidate feature points has three-dimensional coordinates of (10.0, 5.0, 2.0), and its local normal vector points along the positive X-axis towards the tunnel sidewall. The simulation parameters for the three sets of feature points are shown in Table 1.

[0037] Table 1

[0038] Step S200: Perform real-time spatial registration between the terrain scan point cloud dataset and the terrain feature map to generate terrain constraint pose correction.

[0039] Specifically, real-time spatial registration refers to establishing a correspondence between scanned points in a terrain scanned point cloud dataset and 3D occupied grids in a terrain feature map. Each scanned point in the terrain scanned point cloud dataset carries 3D spatial coordinates, while each 3D occupied grid in the terrain feature map carries spatial position and occupancy status. The registration process is executed using an iterative nearest-point algorithm, whose inputs are the source point cloud (the terrain scanned point cloud dataset) and the target point cloud (the set of center points of the occupied grids in the terrain feature map). The iterative nearest-point algorithm executes the following steps: for each point in the source point cloud, search for the nearest point in the target point cloud to form a point pair; calculate the rigid body transformation matrix based on all point pairs to minimize the registration error between the transformed source point cloud and the target point cloud; update the source point cloud using the transformation matrix; repeat the above steps until the registration error converges or the maximum number of iterations is reached. The transformation matrix obtained when the registration error converges contains position offset and attitude offset, which constitute the terrain constraint pose correction amount, used to compensate for the accumulated error in the inertial device navigation calculation.

[0040] In one possible implementation, the terrain scan point cloud dataset and the terrain feature map are spatially registered in real time to generate terrain constraint pose correction. Step S200 further includes step S210, which maps the terrain scan point cloud dataset to the terrain feature map for spatial calculation to obtain the error covariance matrix. Specifically, spatial calculation refers to projecting all scan points in the terrain scan point cloud dataset onto the coordinate system of the terrain feature map using the currently estimated pose transformation parameters. A residual vector is formed between the projected scan points and the nearest 3D occupied grid center point in the terrain feature map. Let the number of scan points be O, and the coordinates of the g-th scan point after projection be s. g The coordinates of the nearest occupied grid center point are t. g The residual vector is e g =s g -t g Arrange all residual vectors column-wise to construct the residual matrix. The error covariance matrix is ​​calculated as follows: first, subtract the mean of each column from the residual matrix to obtain the centered residual matrix; then, multiply the transpose of the centered residual matrix by the centered residual matrix and divide by 0 to obtain a 3x3 error covariance matrix. This error covariance matrix characterizes the correlation and variance of the registration error in the three spatial directions.

[0041] Step S220: Extract the position error submatrix and attitude error submatrix based on the error covariance matrix. Specifically, the diagonal elements of the error covariance matrix correspond to the variances of the three spatial coordinate axes, and the off-diagonal elements correspond to the covariances between different directions. The position error submatrix is ​​the error covariance matrix itself, as the distribution of position error in three-dimensional space is fully described by this matrix. The attitude error submatrix is ​​obtained as follows: perform local plane fitting on the scan points in the terrain scan point cloud dataset to obtain the normal vector at each scan point, and construct a normal vector matrix from all normal vectors. Multiply the transpose of the normal vector matrix by the error covariance matrix and then by the normal vector matrix to obtain a three-row, three-column attitude error submatrix. This attitude error submatrix characterizes the uncertainty of the attitude estimation error in the three rotation axis directions.

[0042] Step S230: Calculate the major and minor axes of the position based on the position error submatrix to obtain the position uncertainty; perform local calculations based on the attitude error submatrix to obtain the attitude uncertainty. Specifically, the calculation of the major and minor axes refers to performing geometric analysis on the position error submatrix to obtain the distribution of the position uncertainty in various spatial directions. The position uncertainty is a set of values, including the lengths of the major, median, and minor axes, representing the semi-axial lengths of the position error ellipsoid along the three principal axes. The attitude uncertainty is the trace of the attitude error submatrix, i.e., the sum of the diagonal elements of the attitude error submatrix; this value characterizes the overall magnitude of the attitude error.

[0043] Step S240: Based on the position uncertainty and the attitude uncertainty, an outward expansion is performed to construct a three-dimensional spatial search box. Specifically, outward expansion refers to expanding along the normal vector direction and the tangent plane direction of the local surface where each scan point in the terrain scan point cloud dataset is located, using each scan point as a reference. The expansion amount is jointly determined by the position uncertainty and the attitude uncertainty. Specifically, the expansion amount along the normal vector direction is set as the sum of the position minor axis length and the attitude uncertainty multiplied by a preset scaling factor; the expansion amount along the tangent plane direction is set as the sum of the position major axis length and the attitude uncertainty multiplied by the same preset scaling factor. The preset scaling factor is a fixed constant. After outward expansion, each scan point forms a cuboid bounding box. The bounding boxes of all scan points together constitute the three-dimensional spatial search box, used to limit the search range during feature matching.

[0044] Step S250: Perform a local intersection search between the 3D spatial search box and the 3D occupancy grid to define the local search range. Specifically, the local intersection search refers to retrieving, for each 3D spatial search box, 3D occupancy grids that spatially intersect with that search box in the terrain feature map. The spatial intersection criterion is that the projection intervals of the two cuboids overlap in all three coordinate axes. The set of all 3D occupancy grids that intersect with the current search box is taken as the local search range corresponding to that search box. Each scan point corresponds to a local search range, which limits the candidate matching objects of that scan point in the terrain feature map.

[0045] Step S260: Based on the terrain scan point cloud dataset, perform 3D description identification to construct a scan point feature description set. Then, perform bidirectional nearest neighbor matching on the scan point feature description set according to the local search range to construct a set of matching point pairs. Specifically, 3D description identification refers to extracting local geometric feature descriptors for each scan point in the terrain scan point cloud dataset, representing the geometric shape of the local area surrounding the scan point. Specifically, for each scan point, a descriptor search radius is set with that point as the center. Neighboring points are searched within the descriptor search radius, and a spatial distribution histogram of neighboring points is calculated. This histogram contains the distance and direction distribution information of neighboring points relative to the center point. The descriptors of all scan points constitute the scan point feature description set.

[0046] Bidirectional nearest neighbor matching refers to searching for the most similar 3D occupied raster descriptor within a local search range for each descriptor in the scan point feature description set. The similarity measure is the Euclidean distance between descriptors. Forward matching is a search from scan point descriptors to occupied raster descriptors, and backward matching is a search from occupied raster descriptors to scan point descriptors. When a pair of descriptors are both nearest neighbors in both forward and backward matching, the pair of descriptors constitutes a matching point pair. All matching point pairs constitute the matching point pair set.

[0047] Step S270: Based on the set of matching point pairs, perform transformation coordinate accumulation calculation on the terrain feature map to construct the terrain constraint pose correction. Specifically, each pair in the set of matching point pairs contains the coordinates of a scan point in the terrain scan point cloud dataset and the coordinates of the center point of the occupied grid in the terrain feature map. Transformation coordinate accumulation calculation refers to using all matching point pairs to solve for the rigid body transformation matrix that minimizes the weighted sum of squares between the transformed scan point coordinates and the corresponding center point coordinates of the occupied grid. This rigid body transformation matrix includes a rotation matrix and a translation vector. Compare the rotation matrix in the current pose estimation with the solved rotation matrix to obtain the rotation correction, i.e., the pose correction; compare the translation vector in the current pose estimation with the solved translation vector to obtain the position correction. The rotation correction and the position correction together constitute the terrain constraint pose correction. The calculation of this correction uses the least squares optimization method, and the optimization objective function is the sum of squared residuals of all matching point pairs.

[0048] In one possible implementation, the position uncertainty is obtained by calculating the major and minor axes of the position based on the position error submatrix. Step S230 further includes step S231, which involves eigenvalue decomposition of the position error submatrix to obtain N orthogonal eigenvectors. These N orthogonal eigenvectors contain N eigenvalues, where N is an integer greater than 1. Specifically, eigenvalue decomposition of the position error submatrix yields three orthogonal eigenvectors and three corresponding eigenvalues. The three orthogonal eigenvectors point to the three principal axes of the error ellipsoid, and the three eigenvalues ​​correspond to the variances in these three directions. The eigenvalue decomposition employs the Jacobi iteration method, transforming the matrix into a diagonal matrix through a series of orthogonal similarity transformations. The diagonal elements of the diagonal matrix are the eigenvalues, and the column vectors of the orthogonal transformation matrix are the eigenvectors. In this application, N is set to 3.

[0049] Step S232: Based on the N orthogonal eigenvectors, N principal axis directions are set, and based on the N eigenvalues, N semi-axis length values ​​are set according to the N principal axis directions. Specifically, the directions of the three orthogonal eigenvectors are set as the three principal axis directions of the error ellipsoid. For each principal axis direction, its corresponding semi-axis length value is set as the square root of the eigenvalue in that direction multiplied by a preset confidence factor. The preset confidence factor is set according to the required confidence level; for example, when the confidence level is 95%, the preset confidence factor is 2.447. This yields three semi-axis length values, each corresponding to one of the three principal axis directions.

[0050] Step S233: Construct position uncertainty ellipsoid parameters based on the N principal axis directions and the N semi-axis length values. Specifically, the position uncertainty ellipsoid parameters include three principal axis direction vectors and three corresponding semi-axis length values. The geometric meaning of this ellipsoid in space is: the distance from the center of the ellipsoid (i.e., the estimated position) along any direction to the surface of the ellipsoid is equal to the position uncertainty in that direction. The ellipsoid parameters are stored in matrix form, i.e., the principal axis direction vectors form a rotation matrix, and the semi-axis length values ​​form a scaling diagonal matrix.

[0051] Step S234: Calculate the major and minor axes of the position based on the position uncertainty ellipsoid parameters to obtain the position uncertainty. Specifically, the calculation of the major and minor axes involves extracting three semi-axis length values ​​from the position uncertainty ellipsoid parameters, taking the maximum value as the major axis length, the median value as the median axis length, and the minimum value as the minor axis length. These three length values ​​together constitute the position uncertainty, used to characterize the magnitude of the position estimation error in different directions.

[0052] For example, assume the error covariance matrix calculated in step S220 is a 3x3 matrix with diagonal elements of 0.0025, 0.0016, and 0.0009, and off-diagonal elements of 0. Eigenvalue decomposition is performed on this matrix. Since it is a diagonal matrix, the three eigenvalues ​​are the diagonal elements, corresponding to three orthogonal eigenvectors: (1.0, 0.0, 0.0), (0.0, 1.0, 0.0), and (0.0, 0.0, 1.0). The preset confidence factor is set to 2.447, and the lengths of the three semi-axis are 2.447 × 0.05 = 0.122 meters, 2.447 × 0.04 = 0.098 meters, and 2.447 × 0.03 = 0.073 meters. The major axis length is 0.122 meters, the median axis length is 0.098 meters, and the minor axis length is 0.073 meters. The simulation data is shown in Table 2 and... Figure 2 As shown.

[0053] Table 2

[0054] Taking the data in the first row of the table as an example, the feature value is 0.0025, its square root is 0.05, and after multiplying by the preset confidence factor of 2.447, we get 0.122 meters. This value is the length of the major axis of the position, which corresponds to the range of the error ellipsoid in the direction of the first principal axis.

[0055] In one possible implementation, bidirectional nearest neighbor matching is performed on the scan point feature description set according to the local search range to construct a set of matching point pairs. Step S260 further includes step S261, which involves traversing the scan point feature description set to extract multiple feature descriptors, performing a local search on the multiple feature descriptors according to the local search range, and calculating multiple distance parameters. Specifically, for each feature descriptor in the scan point feature description set, for the current feature descriptor, all 3D occupied grid descriptors are retrieved within the local search range corresponding to the scan point. For each candidate occupied grid descriptor, the Euclidean distance between it and the current feature descriptor is calculated, and this Euclidean distance is the distance parameter. The distance parameter is calculated by treating both descriptors as vectors, calculating the sum of the squares of the differences between corresponding elements of the two vectors, and then taking the square root. Each scan point descriptor corresponds to multiple distance parameters, and each distance parameter corresponds to an occupied grid descriptor within the local search range.

[0056] Step S262: Based on the multiple distance parameters, arrange them in ascending order to construct multiple distance parameter sequences. Extract the first-order value to determine the first distance parameter, extract the second-order value to determine the second distance parameter. Specifically, for all distance parameters corresponding to each scan point descriptor, sort them in ascending order of value to obtain a distance parameter sequence. Extract the first-order distance parameter from this sequence as the first distance parameter, which corresponds to the occupied raster descriptor most similar to the scan point descriptor within the local search range. Extract the second-order distance parameter from this sequence as the second distance parameter, which corresponds to the occupied raster descriptor second most similar to the scan point descriptor within the local search range.

[0057] Step S263: Calculate the distance ratio based on the ratio of the first distance parameter to the second distance parameter to obtain distance ratio data. Specifically, the distance ratio data = first distance parameter ÷ second distance parameter. This ratio is less than or equal to 1. The smaller the ratio, the higher the distinguishability of the nearest neighbor descriptor relative to the second nearest neighbor descriptor, and the higher the reliability of the match.

[0058] Step S264: A first threshold and a second threshold are preset, with the first threshold being greater than the second threshold. When the distance ratio data is less than the first threshold, a first matching reception signal is generated. Forward matching is performed using the first matching reception signal to generate a first matching dataset. Specifically, the first and second thresholds are preset constants, with the first threshold being greater than the second threshold. The distance ratio data is compared with the first threshold. If the distance ratio data is less than the first threshold, a first matching reception signal is generated. This signal indicates that the match between the current scan point descriptor and its nearest neighbor occupied grid descriptor is initially reliable. Among all scan points that have generated the first matching reception signal, the scan point descriptor and its nearest neighbor occupied grid descriptor form a matching pair. All matching pairs constitute the first matching dataset. Forward matching refers to the matching direction from the scan point descriptor to the occupied grid descriptor.

[0059] Step S265: When the distance ratio data is less than the second threshold, a second matching reception signal is generated. Reverse matching is then performed using the second matching reception signal to generate a second matching dataset. Specifically, the distance ratio data is compared with the second threshold. If the distance ratio data is less than the second threshold, a second matching reception signal is generated. This signal indicates that the current match still has high reliability under reverse verification. Reverse matching refers to verifying, for each matching pair in the first matching dataset, whether searching for the nearest neighbor in the scan point feature description set, starting from the occupied raster descriptor in the matching pair, still points to the scan point descriptor in the matching pair. If so, the matching pair passes the reverse verification. All matching pairs that pass the reverse verification constitute the second matching dataset.

[0060] Step S266: Based on the intersection of the first matching dataset and the second matching dataset, construct the matching point pair set. Specifically, data intersection refers to taking the intersection of the first matching dataset and the second matching dataset. Since the second threshold value is smaller, matching pairs that satisfy the reverse matching condition must also satisfy the forward matching condition. The second matching dataset is a subset of the first matching dataset, and their intersection is the matching pair that simultaneously satisfies the forward coarse screening and the reverse high-precision verification. The set of these matching pairs constitutes the matching point pair set. Each pair in the matching point pair set contains a scan point and a three-dimensional occupancy grid, and the two have a high degree of correspondence in space and features.

[0061] For example, assume there is a scan point descriptor A in the scan point feature description set. Within the local search range, three occupying grid descriptors B1, B2, and B3 are obtained, with calculated distance parameters of 0.15, 0.45, and 0.60, respectively. These distance parameters are arranged in ascending order to obtain the sequence [0.15, 0.45, 0.60], where the first distance parameter is 0.15, the second distance parameter is 0.45, and the distance ratio is 0.15 ÷ 0.45 = 0.333. A first threshold is set to 0.6, and a second threshold is set to 0.4. Since 0.333 is less than 0.6, a first matching reception signal is generated, and scan point descriptor A and occupying grid descriptor B1 form a matching pair and are added to the first matching dataset. Simultaneously, since 0.333 is less than 0.4, a second matching reception signal is generated, and the matching pair (A, B1) is reverse-verified. If the verification passes, it is added to the second matching dataset. The final set of matching point pairs includes (A, B1). This simulated data is shown in Table 3.

[0062] Table 3

[0063] Step S300: Traverse the terrain scan point cloud dataset to perform bidirectional priority search and solve, generate the inferred trajectory, use the terrain constraint pose correction amount as the observation to fuse and correct the inferred trajectory, and generate the optimal estimation sequence of navigation state.

[0064] Specifically, the bidirectional priority search solution refers to simultaneously processing the terrain scan point cloud dataset along both the forward and reverse time axes to obtain trajectory estimates in two directions, and then fusing the results from the two directions. Forward processing performs a depth-first search along the time-increasing direction starting from the initial time, while reverse processing performs a breadth-first search along the time-decreasing direction starting from the termination time. The inferred trajectory refers to the pose sequence obtained solely from inertial device measurements and integration, containing the position and attitude at each time step. The terrain-constrained pose correction obtained in step S200 is used as an external observation and fused with the inferred trajectory for correction. The fusion method employs the extended Kalman filter algorithm, which uses the inferred trajectory as the predicted value and the terrain-constrained pose correction as the observed value. After calculating the Kalman gain, the state estimate is updated to obtain the optimal navigation state estimate at each time step. The optimal navigation state estimates at all time steps constitute the optimal navigation state estimate sequence.

[0065] In one possible implementation, a bidirectional priority search is performed on the terrain scan point cloud dataset to generate a calculated trajectory. Step S300 further includes step S310, which involves performing an initial pose estimation by traversing the terrain scan point cloud dataset along the time axis based on a depth-first search to generate the first pose estimation data. Specifically, the depth-first search refers to processing the point cloud data frame by frame in the time-incrementing direction, starting from the initial frame of the terrain scan point cloud dataset. For the current frame point cloud, the angular velocity and acceleration measured by the inertial device are integrated to obtain the relative pose transformation of the current frame relative to the previous frame. This relative pose transformation is accumulated onto the pose estimation of the previous frame to obtain the initial pose estimation of the current frame. The above operation is performed sequentially on all frames to obtain a sequence of initial pose estimations for each position, which is the first pose estimation data. Each pose estimation data includes a timestamp, a 3D position, and a 3D attitude angle.

[0066] Step S320: Match the first pose estimation data with the terrain feature map to generate a first matched pose. Specifically, for each pose in the first pose estimation data, use this pose as an initial transformation to transform the terrain scan point cloud data frame at the corresponding time into the terrain feature map coordinate system. Then, use the iterative nearest point algorithm for fine matching to obtain the finely matched pose, which is the first matched pose. The first matched pose sequence corresponds to all frames on the positive time axis.

[0067] Step S330: Calculate the pose error value between the first matched pose and the first pose estimation data, and perform filtering processing to generate a forward-filtered trajectory sequence. Specifically, the pose error value includes position error and attitude error. The position error is the position coordinates of the first matched pose minus the position coordinates of the first pose estimation data, and the attitude error is the attitude angle of the first matched pose minus the attitude angle of the first pose estimation data. The filtering processing uses a low-pass filter to smooth the pose error value and remove high-frequency noise. The filtered pose error value is used to correct the first pose estimation data to obtain a corrected pose sequence, which is the forward-filtered trajectory sequence.

[0068] Step S340: Based on breadth-first search, the terrain scan point cloud dataset is traversed backward along the time axis to perform initial pose estimation, generating second pose estimation data. Specifically, breadth-first search refers to processing the point cloud data frame by frame in the decreasing time direction, starting from the terminating frame of the terrain scan point cloud dataset. For the current frame point cloud, the angular velocity and acceleration measured by inertial devices are integrated backward to obtain the relative pose transformation of the current frame relative to the next frame. This relative pose transformation is accumulated to the pose estimation of the next frame to obtain the initial pose estimation of the current frame. The above operation is performed sequentially for all frames to obtain a sequence of initial pose estimations for each frame, which is the second pose estimation data.

[0069] Step S350: Register the second pose estimation data with the terrain feature map to obtain the second matching pose. Specifically, for each pose in the second pose estimation data, use this pose as the initial transformation to transform the terrain scan point cloud data frame at the corresponding time to the terrain feature map coordinate system. Then, use the iterative nearest point algorithm for fine registration to obtain the finely registered pose, which is the second matching pose. The second matching pose sequence corresponds to all frames on the reverse time axis.

[0070] Step S360: Calculate the pose error value between the second matched pose and the second pose estimation data, perform filtering processing, and generate a backward-filtered trajectory sequence. Specifically, the pose error value includes position error and attitude error. The position error is the position coordinates of the second matched pose minus the position coordinates of the second pose estimation data, and the attitude error is the attitude angle of the second matched pose minus the attitude angle of the second pose estimation data. The filtering processing uses a low-pass filter to smooth the pose error value and remove high-frequency noise. The filtered pose error value is used to correct the second pose estimation data to obtain a corrected pose sequence, which is the backward-filtered trajectory sequence.

[0071] Step S370: The first matched pose is used as a single-point observation constraint to perform constraint analysis on the forward-filtered trajectory sequence, solving for the forward optimized trajectory parameters. The second matched pose is used as a multi-point observation constraint to perform constraint analysis on the backward-filtered trajectory sequence, solving for the backward optimized trajectory parameters. Specifically, the single-point observation constraint refers to treating each first matched pose as an independent observation point and applying position and attitude constraints to the forward-filtered trajectory at that observation point. The constraint analysis uses a graph optimization method, treating each pose in the forward-filtered trajectory sequence as an optimization variable, using the inertial integral between adjacent poses as edge constraints, and using the first matched pose as an absolute pose constraint. The optimization objective is to minimize the weighted sum of squares of all constraint residuals. Solving this optimization problem yields the forward optimized trajectory parameters, which represent the position and attitude of each optimized pose.

[0072] Multi-point observation constraints refer to treating multiple consecutive second-matched poses as a group of observation points and applying a global constraint to the back-filtered trajectory segment corresponding to this group of observation points. The constraint analysis also employs a graph optimization method, treating each pose in the back-filtered trajectory sequence as an optimization variable, the inertial integral between adjacent poses as edge constraints, and the second-matched pose sequence as absolute pose constraints. The optimization objective is to minimize the weighted sum of squares of all constraint residuals. Solving this optimization problem yields the inverse optimized trajectory parameters, which represent the position and attitude of each optimized pose.

[0073] Step S380: The forward optimized trajectory parameters and the reverse optimized trajectory parameters are bidirectionally correlated to construct the inferred trajectory. Specifically, bidirectional correlation refers to a weighted fusion of the forward optimized trajectory parameters and the reverse optimized trajectory parameters. For each time step, the forward optimized trajectory parameters provide a pose estimate for that time step, and the reverse optimized trajectory parameters provide another pose estimate for that time step. The fusion method involves weighting the two pose estimates according to preset weight coefficients. The preset weight coefficients are set based on the covariance of the forward and reverse filters, with the smaller covariance having a larger weight coefficient. The fused pose sequence constitutes the inferred trajectory.

[0074] Step S400: Simulate the optimal estimation sequence of the navigation state and compare it point by point. Calculate the cumulative error distribution data to simulate and verify the navigation and control of the mining equipment using inertial devices.

[0075] Specifically, point-by-point comparison refers to comparing each pose in the optimal navigation state estimation sequence with the pose at the corresponding timestamp in the preset reference trajectory sequence, calculating the position deviation and attitude deviation at each moment. The deviation values ​​at all moments constitute the cumulative error distribution data, which is used to evaluate the performance of the inertial device navigation and control system, including position accuracy, attitude accuracy, and error drift characteristics. Based on this data, simulation verification of navigation and control for inertial device mining equipment is conducted. The verification includes whether the positioning accuracy meets the requirements of tunnel excavation and whether the attitude estimation meets the requirements of cutting head control.

[0076] In one possible implementation, the optimal navigation state estimation sequence is simulated and compared point by point to calculate the cumulative error distribution data. Step S400 further includes step S410, which involves aligning the optimal navigation state estimation sequence with a preset reference trajectory sequence using timestamps to calculate the three-dimensional position error and constructing an error vector time series. Specifically, the preset reference trajectory sequence is the actual motion trajectory of the mining equipment in the simulated environment, which is directly output by the simulator and serves as a standard reference for evaluating navigation accuracy. Timestamp alignment refers to finding poses with the same timestamp in the preset reference trajectory sequence for each moment in the optimal navigation state estimation sequence. If the timestamps are completely consistent, they are directly paired; otherwise, a linear interpolation method is used to interpolate the preset reference trajectory sequence to obtain the reference pose at that moment. After pairing, the three-dimensional position error is calculated. The three-dimensional position error is the optimal navigation state estimation position coordinates minus the reference position coordinates, resulting in a three-dimensional error vector. The three-dimensional error vectors at all moments are arranged in chronological order to form an error vector time series.

[0077] Step S420: Traverse the time series of the error vector to calculate the three-dimensional position error at multiple moments, identify the view position error sequence for position boundary analysis, and delineate the position error boundary value. Specifically, traverse each three-dimensional error vector in the error vector time series, calculate its magnitude as the scalar instantaneous position error at that moment, and the scalar instantaneous position errors at all moments constitute the view position error sequence. Position boundary analysis refers to performing statistical analysis on the view position error sequence, calculating the mean, standard deviation, and extreme values ​​of the sequence; the mean plus three times the standard deviation is used as the upper limit of the error per unit time. The total running time includes h moments, then the cumulative error judgment threshold = upper limit of error per unit time × total number of moments, and this threshold is the position error boundary value, representing the statistical upper limit of the allowable cumulative position error throughout the entire process.

[0078] Step S430: Perform point-by-point cumulative calculation based on the position error boundary value to locate the cumulative error distribution data. Specifically, point-by-point cumulative calculation means starting from the beginning of the error vector time series, sequentially accumulating the instantaneous position error magnitude at each moment to obtain a cumulative error sequence. Compare the cumulative error sequence with the position error boundary value, which represents the upper limit of the allowable cumulative error, to locate the moment when the cumulative error first exceeds the position error boundary value; this moment is the navigation and positioning failure moment. Simultaneously, record the values ​​of the cumulative error sequence at each moment; these values ​​constitute the cumulative error distribution data.

[0079] In one possible implementation, the calculation of cumulative error distribution data is used to simulate and verify the navigation and control of inertial device mining equipment. Step S400 further includes step S440, which involves extracting multi-dimensional error geometric feature parameters based on the cumulative error distribution data for error feature analysis, identifying multiple error features to trace the source of errors in the inertial device, and locating error source type data, wherein the error source type data has a contribution coefficient. Specifically, the multi-dimensional error geometric feature parameters include the mean position error, standard deviation of position error, maximum position error, mean attitude error, standard deviation of attitude error, maximum attitude error, and error accumulation rate. Error feature analysis refers to comparing the above parameters with a preset error feature library, which stores feature patterns corresponding to various error sources. Error source types include gyroscope zero bias error, accelerometer zero bias error, gyroscope scale factor error, accelerometer scale factor error, gyroscope random walk error, and accelerometer random walk error. The identification process involves calculating the correlation coefficient between the current error feature parameters and the feature patterns of various error sources in the feature library, and taking the error source type with the largest correlation coefficient as the identification result. The contribution coefficient is the normalized value of the correlation coefficient, representing the proportion of the error source's contribution to the overall error. The location error source type data consists of the identified error source types and their contribution coefficients.

[0080] Step S450: Control the mining equipment of the inertial device according to the contribution coefficient and the error source type to construct a navigation and control adjustment command. Specifically, the navigation and control adjustment command includes parameter adjustment values ​​and adjustment directions. For the located error source type, the adjustment range is determined according to its contribution coefficient; the larger the contribution coefficient, the larger the adjustment range. The adjustment method is to superimpose compensation values ​​on the original parameters of the inertial device. For example, for gyroscope zero bias error, the compensation value is the negative of the currently estimated zero bias value multiplied by the contribution coefficient. The compensation method is the same for accelerometer zero bias error. All compensation values ​​constitute the navigation and control adjustment command.

[0081] Step S460 involves simulating the execution of the navigation and control adjustment commands for iterative verification until the multi-dimensional error geometric feature parameters meet the preset verification thresholds, thus completing the simulation verification closed loop. Specifically, simulating the execution of navigation and control adjustment commands means applying the adjustment commands to the simulation model of the inertial device in a simulation environment and updating the output data of the inertial device. Iterative verification involves repeating steps S100 to S450, recalculating the multi-dimensional error geometric feature parameters after each iteration. The preset verification thresholds include position error thresholds and attitude error thresholds. When the mean value of the position error and the mean value of the attitude error in the multi-dimensional error geometric feature parameters are simultaneously less than the corresponding preset verification thresholds, the iteration stops, and the simulation verification closed loop is completed. If the preset verification thresholds are not met, new navigation and control adjustment commands are generated based on the new error feature analysis results, and the iteration continues.

[0082] This application's embodiments solve the technical problem that existing mining equipment's navigation and positioning errors continuously diverge over time during long-distance autonomous navigation underground, and cannot obtain effective external correction in the underground environment, by constructing an environment and map and collecting point clouds, registering the point clouds and map to obtain correction amounts, solving the trajectory through bidirectional search and fusing the correction amounts to obtain the optimal estimate, and comparing the optimal estimate with a benchmark to verify navigation and control performance. It achieves the technical effect of effectively constraining and correcting the cumulative error of inertial navigation by utilizing the terrain features of the roadway itself in an underground environment without external artificial beacons, so that the navigation and positioning error remains in a convergent state during long-distance operations.

[0083] In the above text, refer to Figures 1-2 This paper describes in detail a simulation and verification method for navigation, measurement, and control of mining equipment based on inertial devices, according to embodiments of the present invention. Next, reference will be made to... Figure 3 This invention describes a simulation and verification system for navigation, measurement, and control of mining equipment based on inertial devices, according to an embodiment of the present invention.

[0084] The inertial device-based navigation and control simulation verification system for mining equipment according to embodiments of the present invention addresses the technical problem that existing mining equipment experiences continuously diverging navigation and positioning errors over long-distance autonomous navigation underground, and cannot obtain effective external correction in the underground environment. It achieves the technical effect of effectively constraining and correcting accumulated inertial navigation errors in underground environments without external artificial beacons, utilizing the terrain features of the roadway itself, thus keeping the navigation and positioning errors convergent during long-distance operations. The inertial device-based navigation and control simulation verification system for mining equipment includes: a motion simulation module 10, a terrain-constrained pose correction generation module 20, a navigation state optimal estimation sequence generation module 30, and a navigation and control simulation verification module 40.

[0085] The simulation motion module 10 is used to construct a simulated mining equipment operating environment for three-dimensional terrain analysis, construct a terrain feature map for simulated motion, and obtain a terrain scan point cloud dataset. The terrain constraint pose correction generation module 20 is used to perform real-time spatial registration between the terrain scan point cloud dataset and the terrain feature map to generate terrain constraint pose corrections. The navigation state optimal estimation sequence generation module 30 is used to traverse the terrain scan point cloud dataset for bidirectional priority search and calculation, generate a calculated trajectory, and use the terrain constraint pose corrections as observations to fuse and correct the calculated trajectory to generate a navigation state optimal estimation sequence. The navigation measurement and control simulation verification module 40 is used to simulate the navigation state optimal estimation sequence for point-by-point comparison, calculate the cumulative error distribution data, and perform navigation measurement and control simulation verification for mining equipment with inertial devices.

[0086] The detailed description of the specific configuration of the simulation motion module 10 is explained as follows: As mentioned above, a simulated mining equipment operating environment is constructed for three-dimensional terrain analysis, a terrain feature map is constructed for simulated motion, and a terrain scan point cloud dataset is obtained. The simulation motion module 10 may further include: a three-dimensional geometry analysis unit for performing three-dimensional geometry analysis based on the simulated mining operating environment and calculating a three-dimensional tunnel geometry model; a meshing processing unit for meshing the three-dimensional tunnel geometry model and dividing it into multiple mesh surface parameters; a multi-scale partitioning unit for performing point cloud analysis based on the multiple mesh surface parameters, generating a dense reference point cloud dataset, performing multi-scale partitioning based on the dense reference point cloud dataset, and constructing a multi-scale voxel pyramid; an extreme value calculation unit for traversing the multi-scale voxel pyramid to calculate the eigenvalue ratio, performing extreme value calculation based on the eigenvalue ratio, and determining multiple candidate feature points; and a three-dimensional occupancy grid parameter construction unit for constructing three-dimensional occupancy grid parameters based on the multiple candidate feature points according to normal constraints, and adding the three-dimensional occupancy grid to the terrain feature map.

[0087] The 3D occupancy grid parameter construction unit, which constructs 3D occupancy grid parameters based on the multiple candidate feature points according to normal constraints, and adds the 3D occupancy grid to the terrain feature map, may further include: a covariance calculation subunit for traversing the multiple candidate feature points to identify neighborhoods, determining multiple neighborhood point sets, calculating covariance based on the multiple neighborhood point sets, and constructing a covariance matrix; a local normal vector calculation subunit for extracting the minimum eigenvalue of the matrix based on the covariance matrix and calculating local normal vectors for the multiple candidate feature points to construct normal constraints; and a spatial expansion subunit. The unit is used to expand the space according to the normal constraint using the multiple candidate feature points as the center to construct multiple rectangular grid data; the normal vector identification subunit is used to identify the normal vectors based on the multiple rectangular grid data, dividing them into normal vector pointing areas and normal vector deviating areas; the three-dimensional occupied grid construction subunit is used to mark the normal vector pointing areas as occupied, mark the normal vector deviating areas as idle, remove the multiple rectangular grid data according to the idle markings, and retain the multiple rectangular grid data according to the occupied markings to construct the three-dimensional occupied grid.

[0088] The detailed configuration of the terrain constraint pose correction generation module 20 is explained below: As mentioned above, the terrain scan point cloud dataset and the terrain feature map are spatially registered in real time to generate terrain constraint pose corrections. The terrain constraint pose correction generation module 20 may further include: a spatial calculation unit for mapping the terrain scan point cloud dataset to the terrain feature map for spatial calculation to obtain an error covariance matrix; an error submatrix extraction unit for extracting position error submatrices and attitude error submatrices based on the error covariance matrix; and an uncertainty calculation unit for calculating the position major and minor axes based on the position error submatrix to obtain position uncertainty, and calculating the position uncertainty based on the attitude error submatrix. The system performs several calculations to obtain the attitude uncertainty. A 3D spatial search box construction unit expands outward based on the position and attitude uncertainties to construct a 3D spatial search box. A local intersection search unit performs a local intersection search between the 3D spatial search box and the 3D occupied grid to define the local search range. A bidirectional nearest neighbor matching unit performs 3D description identification based on the terrain scan point cloud dataset, constructs a scan point feature description set, and performs bidirectional nearest neighbor matching on the scan point feature description set according to the local search range to construct a set of matching point pairs. A transformation coordinate accumulation calculation unit performs transformation coordinate accumulation calculation on the terrain feature map based on the set of matching point pairs to construct the terrain constraint pose correction amount.

[0089] The method involves calculating the position major and minor axes based on the position error submatrix to obtain the position uncertainty. The uncertainty calculation unit may further include: an eigenvalue decomposition subunit for performing eigenvalue decomposition on the position error submatrix to obtain N orthogonal eigenvectors, where each of the N orthogonal eigenvectors contains N eigenvalues, where N is an integer greater than 1; an axis setting subunit for setting N principal axis directions based on the N orthogonal eigenvectors and N semi-axis length values ​​based on the N eigenvalues ​​according to the N principal axis directions; a position uncertainty ellipsoid parameter construction subunit for constructing position uncertainty ellipsoid parameters based on the N principal axis directions and the N semi-axis length values; and a position major and minor axis calculation subunit for calculating the position major and minor axes based on the position uncertainty ellipsoid parameters to obtain the position uncertainty.

[0090] The bidirectional nearest neighbor matching unit further includes: a local search subunit for traversing the scan point feature description set to extract multiple feature descriptors, performing a local search on the multiple feature descriptors according to the local search range, and calculating multiple distance parameters; an ascending order sorting subunit for sorting the multiple distance parameters in ascending order, constructing a sequence of multiple distance parameters, extracting the first rank value, determining the first distance parameter, extracting the second rank value, and determining the second distance parameter; and a ratio calculation subunit for comparing the first distance parameter with the second distance parameter. The system calculates distance ratio data; a forward matching subunit is used to preset a first threshold and a second threshold, where the first threshold is greater than the second threshold. When the distance ratio data is less than the first threshold, a first matching reception signal is generated, and forward matching is performed using the first matching reception signal to generate a first matching dataset; a reverse matching subunit is used to generate a second matching reception signal when the distance ratio data is less than the second threshold, and reverse matching is performed using the second matching reception signal to generate a second matching dataset; a data intersection subunit is used to perform data intersection based on the first matching dataset and the second matching dataset to construct the matching point pair set.

[0091] The detailed configuration of the navigation state optimal estimation sequence generation module 30 is explained below: As mentioned above, the navigation state optimal estimation sequence generation module 30 further includes: a first pose estimation data generation unit for performing a bidirectional priority search solution by traversing the terrain scan point cloud dataset to generate a calculated trajectory; a first matching pose generation unit for matching the first pose estimation data with the terrain feature map to generate a first matching pose; a first filtering processing unit for calculating the pose error value between the first matching pose and the first pose estimation data and performing filtering processing to generate a forward filtered trajectory sequence; and a second pose estimation data generation unit for performing a breadth-first search solution by traversing the terrain scan point cloud dataset along the time axis to generate a forward filtered trajectory sequence. An initial pose estimation is performed on the terrain scan point cloud dataset to generate second pose estimation data. A second matching pose generation unit is used to register the second pose estimation data with the terrain feature map to obtain a second matching pose. A second filtering processing unit is used to calculate the pose error value between the second matching pose and the second pose estimation data, perform filtering processing, and generate a backward filtered trajectory sequence. An optimized trajectory parameter solving unit is used to perform constraint analysis on the forward filtered trajectory sequence using the first matching pose as a single-point observation constraint to solve the forward optimized trajectory parameters, and to perform constraint analysis on the backward filtered trajectory sequence using the second matching pose as a multi-point observation constraint to solve the backward optimized trajectory parameters. A bidirectional association unit is used to perform bidirectional association between the forward optimized trajectory parameters and the backward optimized trajectory parameters to construct the estimated trajectory.

[0092] The detailed description of the specific configuration of the navigation measurement and control simulation verification module 40 is as follows: As mentioned above, the navigation measurement and control simulation verification module 40 simulates the optimal estimation sequence of the navigation state and performs point-by-point comparison to calculate the cumulative error distribution data. The navigation measurement and control simulation verification module 40 may further include: a three-dimensional position error calculation unit for performing time stamp alignment of the optimal estimation sequence of the navigation state with the preset reference trajectory sequence to calculate the three-dimensional position error and construct an error vector time series; a position boundary analysis unit for traversing the error vector time series to perform multi-time three-dimensional position error calculation, identifying the position error sequence to perform position boundary analysis, and defining the position error boundary value; and a point-by-point cumulative calculation unit for performing point-by-point cumulative calculation based on the position error boundary value to locate the cumulative error distribution data.

[0093] The simulation verification of navigation and control of mining equipment using inertial devices, which calculates cumulative error distribution data, can be further divided into: an error tracing unit for extracting multi-dimensional error geometric feature parameters based on the cumulative error distribution data, performing error feature analysis, identifying multiple error features to trace the source of errors in the inertial device, and locating error source type data, wherein the error source type data has a contribution coefficient; a navigation and control adjustment command construction unit for controlling the mining equipment using the inertial device according to the contribution coefficient and the error source type, and constructing navigation and control adjustment commands; and a simulation verification unit for simulating the execution of the navigation and control adjustment commands for iterative verification until the multi-dimensional error geometric feature parameters meet a preset verification threshold, thus completing the simulation verification closed loop.

[0094] The inertial device-based navigation and control simulation verification system for mining equipment provided in this embodiment of the invention can execute the inertial device-based navigation and control simulation verification method for mining equipment provided in any embodiment of the invention, and has the corresponding functional modules and beneficial effects of the execution method.

[0095] Although this application makes various references to certain modules in the system according to the embodiments of this application, any number of different modules can be used and run on user terminals and / or servers. The various units and modules included are only divided according to functional logic, but are not limited to the above division, as long as the corresponding functions can be achieved; in addition, the specific names of each functional unit are only for easy distinction between each other and are not used to limit the scope of protection of this invention.

[0096] The above description is merely a preferred embodiment of the present invention and is not intended to limit the present invention in any way. Although the present invention has been disclosed above with reference to preferred embodiments, it is not intended to limit the present invention. Any person skilled in the art can make some modifications or alterations to the above-disclosed technical content to create equivalent embodiments without departing from the scope of the present invention. Any modifications, equivalent changes, and alterations made to the above embodiments based on the technical essence of the present invention without departing from the scope of the present invention shall still fall within the scope of the present invention.

Claims

1. A simulation and verification method for navigation, measurement, and control of mining equipment based on inertial devices, characterized in that, The method includes: A simulated mining equipment operating environment was constructed for 3D terrain analysis, a terrain feature map was constructed for simulated motion, and a terrain scan point cloud dataset was obtained. The terrain scan point cloud dataset is spatially registered with the terrain feature map in real time to generate terrain constraint pose correction. The terrain scan point cloud dataset is traversed to perform bidirectional priority search and calculation to generate a calculated trajectory. The terrain constraint pose correction is used as an observation to fuse and correct the calculated trajectory, and the optimal estimation sequence of navigation state is generated. The optimal estimation sequence of the simulated navigation state is compared point by point, and the cumulative error distribution data is calculated to simulate and verify the navigation and control of the mining equipment with inertial devices.

2. The simulation and verification method for navigation, measurement, and control of mining equipment based on inertial devices as described in claim 1, characterized in that, The method involves constructing a simulated mining equipment operating environment for 3D terrain analysis, building a terrain feature map for simulated motion, and obtaining a terrain scan point cloud dataset. Three-dimensional geometric analysis is performed based on the simulated mining operation environment to calculate the three-dimensional roadway geometric model; The three-dimensional tunnel geometry model is meshed, and multiple mesh surface parameters are divided. Point cloud analysis is performed based on the multiple grid surface parameters to generate a dense reference point cloud dataset. Multi-scale division is performed based on the dense reference point cloud dataset to construct a multi-scale voxel pyramid. The eigenvalue ratio is calculated by traversing the multi-scale voxel pyramid, and the extreme values ​​are calculated based on the eigenvalue ratio to determine multiple candidate feature points. Based on the multiple candidate feature points, a three-dimensional occupancy grid parameter is constructed according to the normal constraint, and the three-dimensional occupancy grid is added to the terrain feature map.

3. The simulation and verification method for navigation, measurement, and control of mining equipment based on inertial devices as described in claim 2, characterized in that, Based on the multiple candidate feature points, a three-dimensional occupancy grid parameter is constructed according to normal constraints. The three-dimensional occupancy grid is then added to the terrain feature map. The method includes: The multiple candidate feature points are traversed to identify neighborhoods, and multiple neighborhood point sets are determined. The covariance is calculated based on the multiple neighborhood point sets to construct a covariance matrix. Based on the covariance matrix, the minimum eigenvalue of the matrix is ​​extracted, and local normal vectors are calculated for multiple candidate feature points to construct normal constraints. Using the multiple candidate feature points as centers, spatial expansion is performed according to the normal constraints to construct multiple rectangular raster data; Based on the multiple rectangular grid data, normal vectors are identified and divided into regions where normal vectors point and regions where normal vectors deviate from the grid. The area pointed to by the normal vector is marked as occupied, and the area away from the normal vector is marked as free. The multiple rectangular grid data are removed according to the free markers, and the multiple rectangular grid data are retained according to the occupied markers to construct the three-dimensional occupied grid.

4. The simulation and verification method for navigation, measurement, and control of mining equipment based on inertial devices as described in claim 3, characterized in that, The method involves real-time spatial registration of the terrain scan point cloud dataset with the terrain feature map to generate terrain constraint pose correction values. The terrain scan point cloud dataset is mapped to the terrain feature map for spatial calculation to obtain the error covariance matrix; Based on the error covariance matrix, extract the position error submatrix and the attitude error submatrix; The position uncertainty is obtained by calculating the major and minor axes of the position based on the position error submatrix, and the attitude uncertainty is obtained by performing local calculations based on the attitude error submatrix. Based on the position uncertainty and the attitude uncertainty, an outward expansion is performed to construct a three-dimensional spatial search box; A local intersection search is performed between the three-dimensional spatial search box and the three-dimensional occupied grid to define the local search range; Based on the terrain scan point cloud dataset, a 3D description and labeling are performed to construct a scan point feature description set. The scan point feature description set is then subjected to bidirectional nearest neighbor matching according to the local search range to construct a set of matching point pairs. Based on the set of matching points, the terrain feature map is transformed and the coordinates are accumulated to construct the terrain constraint pose correction amount.

5. The simulation and verification method for navigation, measurement, and control of mining equipment based on inertial devices as described in claim 4, characterized in that, The position uncertainty is obtained by calculating the major and minor axes of the position based on the position error submatrix, and the method includes: The position error submatrix is ​​decomposed into eigenvalues ​​to obtain N orthogonal eigenvectors, each of which contains N eigenvalues, where N is an integer greater than 1. Based on the N orthogonal eigenvectors, N principal axis directions are set, and based on the N eigenvalues, N semi-axis length values ​​are set according to the N principal axis directions; Based on the N principal axis directions and the N semi-axis length values, construct the position uncertainty ellipsoid parameters; The position uncertainty is obtained by calculating the major and minor axes based on the position uncertainty ellipsoid parameters.

6. The simulation and verification method for navigation, measurement, and control of mining equipment based on inertial devices as described in claim 4, characterized in that, The method includes performing bidirectional nearest neighbor matching on the scan point feature description set according to the local search range to construct a set of matching point pairs, comprising: The scan point feature description set is traversed to extract multiple feature descriptors. The multiple feature descriptors are then locally searched according to the local search range, and multiple distance parameters are calculated. Based on the multiple distance parameters, arrange them in ascending order to construct multiple distance parameter sequences. Extract the first-order value to determine the first distance parameter, extract the second-order value to determine the second distance parameter; The distance ratio data is obtained by calculating the ratio between the first distance parameter and the second distance parameter. A first threshold and a second threshold are preset, wherein the first threshold is greater than the second threshold. When the distance ratio data is less than the first threshold, a first matching reception signal is generated. Forward matching is performed through the first matching reception signal to generate a first matching dataset. When the distance ratio data is less than the second threshold, a second matching reception signal is generated, and reverse matching is performed using the second matching reception signal to generate a second matching dataset; The matching point pair set is constructed by intersecting the first matching dataset and the second matching dataset.

7. The simulation and verification method for navigation, measurement, and control of mining equipment based on inertial devices as described in claim 1, characterized in that, The method involves traversing the terrain scan point cloud dataset to perform a bidirectional priority search solution and generate the inferred trajectory. Initial pose estimation is performed by traversing the terrain scan point cloud dataset along the time axis using a depth-first search to generate the first pose estimation data. The first pose estimation data is matched with the terrain feature map to generate a first matched pose; The pose error value between the first matched pose and the first pose estimation data is calculated and filtered to generate a forward filtered trajectory sequence. The initial pose estimation is performed by traversing the terrain scan point cloud dataset in reverse along the time axis using breadth-first search, and the second pose estimation data is generated. The second pose estimation data is registered with the terrain feature map to obtain the second matching pose; The pose error value between the second matched pose and the second pose inferred data is calculated and filtered to generate a backward filtered trajectory sequence. The first matched pose is used as a single-point observation constraint to perform constraint analysis on the forward filtered trajectory sequence, and the forward optimized trajectory parameters are solved. The second matched pose is used as a multi-point observation constraint to perform constraint analysis on the backward filtered trajectory sequence, and the reverse optimized trajectory parameters are solved. The forward optimized trajectory parameters and the reverse optimized trajectory parameters are bidirectionally correlated to construct the inferred trajectory.

8. The simulation and verification method for navigation, measurement, and control of mining equipment based on inertial devices as described in claim 1, characterized in that, The method includes simulating the optimal estimation sequence of the navigation state and performing point-by-point comparison to calculate the cumulative error distribution data. The optimal navigation state estimation sequence is time-stamp aligned with the preset baseline trajectory sequence to calculate the three-dimensional position error, and an error vector time series is constructed. The error vector time series is traversed to perform multi-time three-dimensional position error calculation, the view position error series is identified to perform position boundary analysis, and the position error boundary value is defined. The cumulative error distribution data is located by performing point-by-point cumulative calculations based on the location error boundary values.

9. The simulation and verification method for navigation, measurement, and control of mining equipment based on inertial devices as described in claim 1, characterized in that, The method for simulating and verifying the navigation and control of mining equipment using inertial devices by calculating the cumulative error distribution data includes: Based on the cumulative error distribution data, multi-dimensional error geometric feature parameters are extracted for error feature analysis. Multiple error features are identified to trace the source of errors in inertial devices and locate error source type data. The error source type data has a contribution coefficient. The inertial device mining equipment is controlled according to the contribution coefficient and the error source type to construct navigation and control adjustment commands; The simulation execution of the navigation and control adjustment commands is performed iteratively until the multi-dimensional error geometric feature parameters meet the preset verification threshold, thus completing the simulation verification closed loop.

10. A simulation and verification system for navigation, measurement, and control of mining equipment based on inertial devices, characterized in that, The system is used to implement the simulation verification method for navigation, measurement, and control of mining equipment based on inertial devices as described in any one of claims 1-9, and the system comprises: The simulation motion module is used to construct a simulated mining equipment operating environment for 3D terrain analysis, construct a terrain feature map for simulated motion, and obtain a terrain scan point cloud dataset. The terrain constraint pose correction generation module is used to perform real-time spatial registration between the terrain scan point cloud dataset and the terrain feature map to generate terrain constraint pose correction. The navigation state optimal estimation sequence generation module is used to traverse the terrain scan point cloud dataset to perform bidirectional priority search and calculation, generate the estimated trajectory, and use the terrain constraint pose correction amount as an observation to fuse and correct the estimated trajectory to generate the navigation state optimal estimation sequence. The navigation measurement and control simulation verification module is used to simulate the optimal estimation sequence of the navigation state for point-by-point comparison and calculate the cumulative error distribution data to perform navigation measurement and control simulation verification on the mining equipment with inertial devices.

Images