Underground personnel positioning and tracking method based on laser ranging

Abstract

The invention belongs to the technical field of coal mine underground positioning, and discloses an underground personnel positioning and tracking method based on laser ranging, which comprises the following steps: calculating the distance between nodes through ranging signals between UWB mobile nodes, and establishing a relative positioning model of the UWB mobile nodes in combination with preset fixed base station coordinates; performing registration alignment on the first local octree map of the collapse area and the relative positioning model, fusing heterogeneous data of the laser radar and the UWB sensor in time and space dimensions, and forming an initial positioning result of the trapped person; acquiring real-time data of the second local octree map and the UWB relative positioning model under the extreme condition of communication blocking or signal interruption, and predicting the position of the trapped person; and according to a path planning result after local obstacle avoidance adjustment, position coordinates of rescue worker mobile nodes are updated in real time, and positioning data of trapped persons are corrected through a graph optimization algorithm. According to the invention, the positioning problem of a mine collapse disaster site in a complex environment is solved.

Description

Technical Field

[0001] The invention belongs to the technical field of underground coal mine positioning, and in particular relates to an underground personnel positioning and tracking method based on laser ranging. Background Art

[0002] In the post-disaster rescue scenario of mines, the LiDAR and UWB multi-sensor fusion positioning system faces a key technical challenge. Due to the complex and changeable underground environment, especially after a collapse, the original LiDAR map may become invalid. At this time, the system needs to rebuild and optimize the positioning map. However, the collapse may also cause the communication between UWB nodes to be limited or even interrupted, and the relative position constraints between nodes cannot be effectively obtained. In this case, the graph optimization framework of the positioning system urgently needs an adaptive adjustment mechanism, which not only needs to dynamically update the map using the only sensor information, but also needs to maintain the relative positioning accuracy and consistency between nodes as much as possible to support the rapid search and precise positioning of trapped personnel. At the same time, the adjustment mechanism also needs to take into account the relative position estimation between the rescuers and the trapped personnel to ensure the safety and efficiency of the rescue operation. In addition, the rescue path planning needs to take into account the dynamic changes of the collapse environment. How to adjust the path in real time while ensuring safety and accessibility, and combine it with the positioning results of the trapped personnel, is the key to improving the rescue efficiency. Therefore, how to achieve adaptive adjustment of the graph optimization framework under non-ideal conditions with limited communication and ensure the real-time, accuracy and robustness of multi-sensor fusion positioning is a key technical problem that needs to be solved urgently. This is of great significance for improving the efficiency and success rate of post-disaster rescue in mines. Summary of the invention

[0003] In view of the above technical problems, the present invention provides a method for positioning and tracking underground personnel based on laser ranging to solve the positioning and rescue problems in the complex environment of mine collapse disaster sites.

[0004] In order to achieve the above object, the present invention adopts the following technical scheme: A method for positioning and tracking underground personnel based on laser ranging includes the following steps: The three-dimensional point cloud data of the mine collapse disaster site and the mobile node signal received by the UWB sensor are obtained through laser radar scanning, and the three-dimensional point cloud data are plane segmented and feature extracted to construct the first local octree map of the collapse area; The distance between UWB mobile nodes is calculated through the ranging signals between the nodes, and the relative positioning model of the UWB mobile nodes is established by combining the preset fixed base station coordinates; The first local octree map of the collapsed area is aligned with the relative positioning model, and the heterogeneous data of the lidar and UWB sensors in time and space dimensions are integrated to form the initial positioning results of the trapped persons; If a secondary collapse occurs at the disaster site, causing the first local octree map to fail, a second local octree map is reconstructed based on the three-dimensional point cloud data obtained from the current lidar scan, and the positions of the moving nodes in the UWB relative positioning model are obtained. Using the graph optimization algorithm, the constraint information of the lidar and UWB is fused to optimize the poses of the second local octree map and the UWB nodes, and the second local octree map of the collapsed environment is reconstructed within the preset positioning coverage range. In extreme cases where communication is blocked or the signal is interrupted, the real-time data of the second local octree map and the UWB relative positioning model are obtained to estimate the positioning information of the trapped personnel and predict the positions of the trapped personnel. Calculate the spatial distance and relative azimuth angle between the predicted result of the position of the trapped personnel and the real-time positioning data of the UWB moving nodes worn by the rescue personnel, and combine the second local octree map to perform real-time planning of the rescue path. Project and match the planned rescue path to the second local octree map, and evaluate the safety and accessibility of the path. If there are collapsed obstacles, the artificial potential field method is used, with the obstacles as repulsive sources and the trapped personnel as attractive sources. By calculating the resultant force of the repulsive and attractive sources, local obstacle avoidance adjustment is performed on the path. According to the path planning result after local obstacle avoidance adjustment, the position coordinates of the rescue personnel's moving nodes are updated in real time, and the positioning data of the trapped personnel are corrected through the graph optimization algorithm.

[0005] The present invention has the following advantages: As described above, the present invention relates to a method for positioning and tracking underground personnel based on laser ranging. This method constructs an octree map of the collapsed area and a UWB node relative positioning model by fusing lidar and UWB sensor data to achieve the initial positioning of trapped personnel. In the case of secondary collapse, the present invention can reconstruct the map and optimize the node poses to maintain positioning accuracy. When communication is blocked, the present invention can predict the positions of trapped personnel and, in combination with the real-time positioning data of rescue personnel, plan the optimal rescue path. Local obstacle avoidance adjustment is performed through the artificial potential field method to ensure the safety and accessibility of the path. Finally, the present invention uses the graph optimization algorithm to correct the positioning of trapped personnel and improve the rescue efficiency. This method effectively solves the positioning and rescue problems in the complex environment of the mine collapse disaster site and has strong adaptability and reliability. BRIEF DESCRIPTION OF THE DRAWINGS

[0006] Figure 1 is a flowchart of the method for positioning and tracking underground personnel based on laser ranging in an embodiment of the present invention; Figure 2 is a schematic diagram of real-time planning of the rescue path in an embodiment of the present invention; Figure 3 is a schematic diagram of local obstacle avoidance adjustment of the path in an embodiment of the present invention. Detailed implementation manners

[0007] The present invention will be further described in detail below in conjunction with the accompanying drawings and specific implementation manners: As Figure 1 shown, in this embodiment, the underground personnel positioning and tracking method based on lidar includes the following steps: Step 1. Obtain the three-dimensional point cloud data of the mine collapse disaster site through lidar scanning and the mobile node signals received by the UWB sensor. Perform plane segmentation and feature extraction on the three-dimensional point cloud data, and construct the first local octree map of the collapse area.

[0008] According to the point cloud data collected by the lidar, perform noise reduction processing on the point cloud data by using voxel filtering, and obtain the point cloud data set in a unified coordinate system through the iterative closest point algorithm; for the point cloud data set, perform rasterization segmentation on the density distribution by using the eight-connected region labeling method, and calculate the density distribution feature map through the Gaussian attenuation function; according to the feature point markings in the density distribution feature map, construct a quadratic surface equation to perform surface fitting on the density data, use bilateral filtering to obtain the density surface model, and establish the mapping relationship between the point cloud data and the octree nodes through Morton encoding; for the mapping relationship of the octree nodes, establish the mapping relationship between the ultra-wideband signal strength and the spatial distance, use the Kalman filter to smooth the node trajectory, and correct the positioning result error through the random forest algorithm.

[0009] Exemplarily, the original point cloud data collected from the lidar scanner is stored in the database, the point cloud data is subjected to noise reduction processing by using voxel filtering, and multiple frames of point cloud data are registered according to the iterative closest point algorithm to obtain the point cloud data set in a unified coordinate system. The spatial distribution of the point cloud data set is statistically analyzed according to the density threshold, the density distribution is rasterized and segmented by using the eight-connected region labeling method, and the signal strength is calculated through the Gaussian attenuation function to obtain the density distribution feature map.

[0010] According to the corresponding relationship between the feature point positions and the signal strength in the density distribution feature map, use morphological opening operation to extract the edge contour, set the gray similarity threshold, and cluster the boundary points through the region growing algorithm to obtain the region contour map. For the point cloud density data in the collapse area in the region contour map, construct a quadratic surface equation and perform density surface fitting by using the least square method, and apply bilateral filtering to the fitted surface for edge-preserving smoothing processing to obtain the density surface model.

[0011] Divide the octree nodes according to the density surface model, use Morton coding to index the spatial positions, establish the mapping relationship between the point cloud data and the octree nodes, and generate the local octree map. Establish the mapping relationship between the ultra-wideband signal strength and the spatial distance according to the attenuation model, suppress the noise of the signal emitted by the mobile node through Gaussian filtering, and use the Kalman filter to smooth the node trajectory to obtain the motion trajectory data.

[0012] Perform spatial positioning in the local octree map according to the motion trajectory data, evaluate and correct the positioning result error through the random forest algorithm, and generate an accurate motion trajectory map. The original point cloud data collected by lidar scanning usually contains a large number of noise points and overlapping areas. In the scanning of the mine collapse disaster site, due to the influence of dust, gravel, etc., the density of the collected point cloud data is uneven and there are a large number of outlier points. Voxel filtering can be used to denoise and homogenize the point cloud data. When the voxel size is set to 0.05 meters, the scene detail features can be better maintained.

[0013] During the registration process of multi-frame point cloud data, establish the corresponding relationship by extracting feature points. Set the convergence threshold to 0.001 meters and the maximum number of iterations to 50 times in the iterative closest point registration algorithm to achieve a registration accuracy better than 0.01 meters. When performing spatial distribution statistics on the registered point cloud data, select a grid cell with a side length of 0.1 meters to calculate the point cloud density distribution, set the density threshold to 800 points per cubic meter, and use the eight-connected region labeling method to segment the density distribution and label different density level regions. When calculating the signal strength, select the Gaussian attenuation function to simulate the spatial propagation characteristics of the ultra-wideband signal, and the attenuation coefficient is taken as 0.8 to obtain a grayscale image reflecting the point cloud density distribution characteristics.

[0014] When extracting the edge contour on the density distribution feature map, use a 3×3 pixel structural element for morphological opening operation to eliminate small noises and maintain the main contour features. Set the gray similarity threshold to 20 in the region growing algorithm, start growing with the region with a higher density value as the seed point, and obtain the complete region contour.

[0015] When performing surface fitting on the point cloud density data in the collapse area, construct a quadratic surface equation containing 6 unknown parameters, solve the parameter values by the least squares method, and control the fitting error within 0.05.

[0016] When constructing the octree map, select the local spatial range of the collapse area as 20 m × 20 m × 10 m, the minimum resolution is 0.2 m, use the Morton coding method to index the spatial positions, and the coding length is 24 bits.

[0017] When mapping the ultra-wideband signal strength and spatial distance, the signal strength values are collected every 1 meter within the range of 0 to 30 meters to establish an attenuation model. The sampling frequency of the mobile node is 10 Hz, the standard deviation of the Gaussian filter is 1.5, the process noise covariance in the Kalman filter is 0.01, and the observation noise covariance is 0.1, to achieve the smoothing of the movement trajectory.

[0018] The random forest algorithm uses 100 decision trees for ensemble learning of the positioning results. The feature dimension is selected as 8, and the maximum tree depth is 15. The final positioning accuracy reaches 0.2 meters.

[0019] Step 2. Calculate the distance between nodes through the ranging signals between UWB mobile nodes, and combine the preset coordinates of the fixed base station to establish a relative positioning model for the UWB mobile nodes.

[0020] According to the timestamp sequence obtained when the ultra-wideband base station receives the signal transmitted by the mobile node, calculate the signal round-trip delay value through the timestamp sequence. The signal round-trip delay value is obtained by the two-way ranging method; use the threshold detection method to screen out multipath signals for the signal round-trip delay value, and establish a signal strength attenuation curve through the signal round-trip delay value. The signal strength attenuation curve is obtained by the free space propagation model; calculate the node distance data according to the signal strength attenuation curve and the preset base station spatial coordinates, and construct a base station node spatial geometric network through the node distance data. The base station node spatial geometric network is obtained by Delaunay triangulation; construct a state space equation for the base station node spatial geometric network, and optimize the state parameters through the state space equation. The state parameters are obtained by the gradient descent method.

[0021] Exemplarily, obtain the timestamp sequence of the signal transmitted by the mobile node from the ultra-wideband base station, calculate the signal round-trip delay through the two-way ranging method, correct the delay deviation according to the signal clock synchronization compensation algorithm, and obtain the signal transmission delay data. For the signal transmission delay data, use the threshold detection method to screen out multipath signals, establish a signal strength attenuation curve according to the free space propagation model, and convert the transmission delay through the distance compensation function to obtain the node distance data.

[0022] According to the preset fixed base station spatial coordinates and the node distance data, perform Gaussian noise filtering on the ranging results, construct a base station node spatial geometric network through Delaunay triangulation, and obtain the base station coverage constraint conditions. Based on the base station coverage constraint conditions, list the observation equations using the trilateration principle, linearize the observation equations through Taylor series expansion, and solve the coordinates of the mobile node according to the weighted least squares method to obtain the node spatial coordinates.

[0023] For the node spatial coordinates, a state space equation based on distance observables is constructed, and the state parameters are optimized by the gradient descent method to obtain the relative positioning model parameters. According to the relative positioning model parameters and the node distance data, a trajectory predictor is established using support vector regression, and the velocity vector and acceleration are selected as regression features to obtain the node motion prediction results.

[0024] Based on the node motion prediction results, the particle weights and velocities are adaptively adjusted by an improved particle swarm algorithm, and the relative positions of the nodes are iteratively optimized to obtain the optimal positioning results. In the mine environment, the two-way ranging method is used to measure the signal transmission delay in ultra-wideband positioning. After the base station transmits an interrogation signal, the mobile node receives and immediately transmits a response signal, and the signal round-trip timestamps are recorded. In practical applications, there are slight differences in the clock frequencies of the base station and the node. When the frequency deviation reaches 50 ppm, centimeter-level positioning errors will occur. The round-trip delay is corrected by the clock synchronization compensation algorithm, and the delay measurement accuracy after compensation is better than 0.1 nanosecond. In the complex mine environment, the ultra-wideband signal will produce a multipath propagation effect, and the receiving end obtains multiple signal copies with different time delays. The threshold detection method is used to set the signal amplitude threshold to 0.6 times the peak value of the first-path signal, and the multipath signal components are screened out. The signal strength and distance mapping relationship is established by combining the free space propagation model, and the ranging accuracy reaches 0.2 meters.

[0025] Fixed base stations are usually deployed on the roadway roof or sidewall. A positioning unit is composed of 4 base stations, and the distance between the base stations is 15 to 25 meters. The Delaunay triangulation algorithm is used to construct a triangular network for the connection of the base station nodes to ensure that the mobile node is always within the coverage of at least 3 base stations. The base station coordinates are measured by a total station, and the accuracy is better than 0.01 meters.

[0026] When establishing the observation equation based on the trilateration principle, the mobile node coordinates are used as unknown parameters, and the distance from the base station to the node is used as the observable. The Taylor series is expanded to the first-order term at the estimated position to construct an overdetermined system of equations, and the weighted least squares method is used to solve the node coordinates. The iteration convergence threshold is set to 0.01 meters. In the state space equation, the node position and velocity are used as state variables, and the observation equation includes distance observables and Doppler frequency shift observables. The learning rate of the gradient descent method is set to 0.1, and the maximum number of iterations is 100 times. The support vector regression predictor selects a 5-dimensional feature vector, including the node's three-axis velocity and two-axis acceleration, and uses a radial basis kernel function with a kernel parameter value of 0.8. In the improved particle swarm algorithm, the number of particles is set to 200, and the velocity update introduces an adaptive inertia weight that dynamically adjusts within the range of 0.4 to 0.9. The local search and global search capabilities are adaptively balanced. After 50 generations of iterative optimization, the positioning error is less than 0.5 meters, meeting the positioning accuracy requirements for mine mobile targets.

[0027] Step 3. Register and align the first local octree map of the collapsed area with the relative positioning model, and fuse the heterogeneous data of the lidar and UWB sensors in the time and space dimensions to form the initial positioning result of the trapped person.

[0028] Obtain the three-dimensional coordinate data of the feature points from the octree map, obtain the position data of the sensor nodes from the relative positioning model, unify the timestamps of the three-dimensional coordinate data and the position data through the time synchronization server to obtain the synchronized data sequence; resample the feature points at the minimum time interval according to the synchronized data sequence, and use the Euclidean distance criterion to construct the corresponding relationship of the feature points to obtain the registration feature set; for the registration feature set, use the singular value decomposition method to solve the rotation matrix and the translation vector, and perform a spatial transformation on the feature point coordinates through the iterative reweighted closest point algorithm to obtain the spatial transformation matrix; perform a coordinate transformation on the lidar point cloud data according to the spatial transformation matrix, and perform multi-source fusion on the position state and the observation data through the Kalman information filter to obtain the fused positioning data.

[0029] Exemplarily, obtain the three-dimensional coordinate data of the feature points from the octree map and the position data of the sensor nodes from the relative positioning model, unify the timestamps of the two types of data through the time synchronization server to obtain the synchronized data sequence. According to the synchronized data sequence, resample the data at the minimum time interval, reduce the three-dimensional coordinates to a two-dimensional feature space through the principal component analysis method, and use the Euclidean distance criterion to construct the corresponding relationship of the feature points to obtain the registration feature set.

[0030] For the registration feature set, use the singular value decomposition method to solve the rotation matrix and the translation vector, perform a spatial transformation on the feature point coordinates through the iterative reweighted closest point algorithm, evaluate the registration accuracy according to the root mean square error, and obtain the spatial transformation matrix. Perform a coordinate transformation on the lidar point cloud data according to the spatial transformation matrix, use a data format converter to unify the heterogeneous data of the point cloud data and the ultra-wideband signal to obtain a standard data stream. Construct a state space equation according to the standard data stream, perform multi-source fusion on the position state and the observation data through the Kalman information filter, and set the cross-covariance threshold to optimize the fusion result to obtain the fused positioning data. For the fused positioning data, use the random sample consensus algorithm to remove the outlier data points, and construct the target position probability density function through the Gaussian kernel function to obtain the target probability distribution map.

[0031] Based on the target probability distribution map, a confidence threshold is set to extract the high-probability region, and the spatial position coordinates of the trapped person are determined by maximum likelihood estimation to obtain the initial positioning result. The lidar point cloud data and the ultra-wideband positioning data at the mine collapse disaster site have different sampling frequencies and time bases. The lidar scanning frequency is usually 10 Hz, while the ultra-wideband signal sampling frequency reaches 100 Hz. A time synchronization server is used to unify the time base, and the synchronization accuracy is better than 100 microseconds to achieve the temporal alignment of the two types of data. During the data resampling process, 1 second is selected as the minimum time interval to perform downsampling on the ultra-wideband signal. When performing dimensionality reduction on the three-dimensional coordinate data by principal component analysis, the eigenvectors corresponding to the eigenvalues with a cumulative contribution rate reaching 95% are selected, and the nearest neighbor criterion is used to match the feature point pairs in the two-dimensional feature space, and the matching threshold is set to 0.5 meters.

[0032] The solution of the spatial transformation matrix uses the singular value decomposition method. In the iterative reweighted closest point algorithm, the weight coefficient decays exponentially with the distance error. The iteration termination condition is that the root mean square error is less than 0.1 meters or the maximum number of iterations reaches 50 times. The transformation matrix includes a 3×3 rotation matrix and a 3D translation vector. During the process of unifying heterogeneous data, the point cloud data is converted into the form of spatial grid occupancy probability, and the grid size is set to 0.2 meters. The ultra-wideband signal is converted into a ranging probability distribution, and the probability value ranges from 0 to 1. In the state space equation, the state vector includes three-dimensional position coordinates and velocity, and the observation vector includes the point cloud occupancy probability and the ranging probability. In the Kalman information filter, the process noise covariance matrix and the observation noise covariance matrix are determined through fault detection, and the cross-covariance threshold is set to 0.7. The data with a value greater than the threshold is re-observed.

[0033] The random sample consensus algorithm randomly extracts 10% of the total sample size of data points each time, the number of iterations is 200 times, and the inlier threshold is 0.3 meters. The bandwidth parameter of the Gaussian kernel function is determined by the cross-validation method, and the optimal value is searched within the range of 0.1 meters to 1 meter. The calculation of the probability density function uses the Monte Carlo integration method, and the number of samples is 1000.

[0034] The confidence threshold in the target probability distribution map is set to 0.8, and the high-probability region is extracted as the possible location of the trapped person. The major axis and minor axis of the confidence ellipse of the final positioning result are 0.5 meters and 0.3 meters respectively.

[0035] Step 4. If a secondary collapse occurs at the disaster site, resulting in the failure of the first local octree map, then a second local octree map is reconstructed based on the three-dimensional point cloud data obtained from the current lidar scan, and the positions of the mobile nodes in the UWB relative positioning model are obtained. Using the graph optimization algorithm, the constraint information of the lidar and UWB is fused to optimize the poses of the second local octree map and the UWB nodes, and a second local octree map of the collapsed environment is reconstructed within the preset positioning coverage range.

[0036] Obtain lidar point cloud data, use the statistical outlier filtering algorithm to remove noise from the point cloud data, and obtain feature point data through principal curvature calculation; according to the feature point data, use the normal vector clustering method to extract the scene plane features, and obtain the pose constraint relationship in the spatial topology map through the minimum spanning tree algorithm; for the pose constraint relationship, construct an error function based on the point-plane distance, and use the Gauss-Newton optimization algorithm to obtain the optimized pose parameters; according to the optimized pose parameters, update and reorganize the octree structure, divide the space occupancy state through the density clustering algorithm, and obtain the updated octree structure; if the overlap degree between the updated octree structure and the first local octree map exceeds the preset threshold, it is determined that the scene has collapsed.

[0037] Exemplarily, obtain real-time point cloud data from the lidar, calculate the overlap degree with the first local octree map through point cloud registration, judge the collapse state according to the overlap degree threshold, and obtain the scene change detection result. For the current point cloud data, use the statistical outlier filtering algorithm to remove noise points, extract feature points through principal curvature calculation, and divide the point cloud data into an octree according to the spatial index to obtain the basic octree structure. According to the basic octree structure, use the normal vector clustering method to extract the scene plane features, and construct the spatial topology map of the ultra-wideband base station nodes through the minimum spanning tree algorithm to obtain the pose constraint relationship. For the pose constraint relationship, construct an error function based on the point-plane distance, use the Gauss-Newton optimization algorithm to solve the node pose, and evaluate the optimization result through the Mahalanobis distance criterion to obtain the optimized pose parameters.

[0038] Based on the optimized pose parameters, update and reorganize the basic octree structure, divide the space occupancy state through the density clustering algorithm, split and merge the octree nodes according to the occupancy probability, and obtain the updated octree structure. According to the updated octree structure, use the region growing algorithm to extract the scene boundary, and accurately adjust the node pose through the iterative closest point registration algorithm to obtain the pose correction result. According to the pose correction result and the preset coverage range, perform local reconstruction on the updated octree structure, and determine the boundary of the reconstruction area through spatial connectivity analysis to obtain the second local octree map. In the detection of secondary collapse in the mine, when registering the point cloud data collected by the lidar with the existing octree map, the projection matching method is used for overlap degree calculation. When the number of points with a distance greater than 0.5 meters after point cloud registration in the overlapping area exceeds 30% of the total number of points, it is determined that a collapse has occurred.

[0039] In the preprocessing stage of point cloud data, the search radius for statistical outlier filtering is set to 0.3 meters, the threshold for the number of neighborhood points is 30, and the principal curvature is calculated using least squares fitting within a neighborhood with a radius of 0.2 meters. The basic octree structure uses adaptive resolution division, with a resolution of 0.1 meters in the area close to the sensor and 0.5 meters in the area far from the sensor. When extracting the scene plane features, the threshold for the normal vector angle is set to 15 degrees, and the threshold for the plane fitting residual is 0.02 meters.

[0040] When constructing the minimum spanning tree for the ultra-wideband base station nodes, the edge weight is jointly determined by the ranging accuracy and the signal strength. The weight coefficient for the ranging accuracy is 0.7, and the weight coefficient for the signal strength is 0.3. In the pose optimization, the error function includes the point-plane distance term and the node-to-node distance constraint term. The point-plane distance uses the perpendicular distance from the point to the plane, with a weight coefficient of 0.6, and the node-to-node distance constraint uses the difference between the measured distance and the calculated distance, with a weight coefficient of 0.4.

[0041] The maximum number of iterations for the Gauss-Newton optimization is set to 50, the convergence threshold is 0.001 meters, and the Mahalanobis distance threshold is set to 2.5. During the octree update process, the search radius for density clustering is 0.2 meters, the minimum number of points threshold is 20, the occupancy probability threshold is 0.6, the splitting condition is that the number of points within the node is greater than 100 and the variance is greater than 0.04 square meters, and the merging condition is that the adjacent nodes have the same occupancy status and the sum of the number of points is less than 50. The boundary extraction uses the region growing algorithm, with a growth threshold of 0.15 meters and a minimum region area of 0.5 square meters.

[0042] The determination of the local reconstruction area is based on spatial connectivity analysis. The connected domain is labeled using a 26-neighborhood, and the boundary of the reconstruction area is extracted through the graph cut algorithm. The boundary smoothing uses cubic spline interpolation, with a control point spacing of 0.3 meters. The preset coverage range is usually 2 meters extended outside the collapsed area, and the Resolution of the reconstruction area is set to 0.2 meters to ensure the reconstruction accuracy is consistent with the original ground. Figure 1 Through the above parameter settings and processing procedures, the dynamic update and reconstruction of the octree map in the collapsed environment are realized.

[0043] Step 5. In the extreme case of communication obstruction or signal interruption, obtain the real-time data of the second local octree map and the UWB relative positioning model, estimate the positioning information of the trapped personnel, and predict the position of the trapped personnel.

[0044] Obtain the ultra-wideband signal strength data and packet loss rate data, judge the communication interruption status according to the signal quality statistical value, and obtain the communication status judgment result; use a circular buffer to store the historical map data and positioning data, and obtain an ordered historical data sequence through data timestamp verification and data integrity check; for the ordered historical data sequence, use a recurrent neural network to construct a target motion state predictor, and smooth the prediction sequence through a sliding time window to obtain the target motion prediction result; according to the target motion prediction result, use the Monte Carlo method to generate state sampling points, screen the sampling points through spatial topological constraints, and construct a state transition probability according to the Markov property to obtain the predicted position result of the trapped person.

[0045] Exemplarily, obtain the ultra-wideband signal strength data and packet loss rate data from the communication monitor, calculate the signal quality statistical value through window sliding, judge the communication interruption status according to the continuous packet loss quantity, and obtain the communication status judgment result. According to the communication status judgment result, use a circular buffer to store the historical map data and positioning data, and sort the data according to the increasing time sequence through data timestamp verification and data integrity check to obtain an ordered historical data sequence.

[0046] For the ordered historical data sequence, extract the target position coordinates and timestamp information, use a recurrent neural network to construct a target motion state predictor, and smooth the prediction sequence through a sliding time window to obtain the target motion prediction result. Based on the target motion prediction result, perform path sampling in the second local octree map, verify the sampling path through a collision detection function, and eliminate the invalid paths according to the spatial distance constraint to obtain a candidate path set.

[0047] Based on the candidate path set, use the Monte Carlo method to generate state sampling points, screen the sampling points through spatial topological constraints, and construct a state transition probability according to the Markov property to obtain the target state distribution. According to the target state distribution, construct a cost function based on the path length and passage difficulty, search for the target position through the particle swarm optimization algorithm to obtain multiple position hypotheses. For the multiple position hypotheses, use a Bayesian inference framework for probability fusion, score the hypotheses through an observation likelihood function, and determine the predicted position result of the trapped person according to the maximum a posteriori probability criterion.

[0048] The detection of mine communication interruption uses the sliding window method to calculate the signal quality. The window length is 10 seconds, and the window sliding step is 1 second. When the signal strength is lower than -85 dBm and the number of consecutive packet losses exceeds 20, it is determined as a communication interruption. The storage capacity of the circular buffer is set to 120 seconds. The data integrity check includes the verification of timestamp continuity and the integrity verification of data fields, and data segments with a timestamp jump exceeding 0.5 seconds are excluded. The motion state predictor uses a long short-term memory network structure. The input features include the target three-dimensional coordinates and velocity vectors. The number of hidden layer nodes is 128, the time step is 0.1 second, and the prediction time domain is 5 seconds.

[0049] The prediction sequence smoothing uses a 5-point weighted average, and the weight coefficients are set according to the Gaussian distribution with a standard deviation of 1.5. Path sampling is carried out in the octree map with a sampling interval of 0.5 meters. The collision detection function uses the ray tracing method to detect the number of intersection points between the detection ray and obstacles. The spatial distance constraint limits the distance between adjacent sampling points to be less than 1 meter. The evaluation indicators of the candidate path include the path length and spatial curvature. The curvature is calculated using the three-point method, and the curvature threshold is 0.8. State sampling uses the stratified importance sampling method, with 100 state points sampled in each layer, and the sampling range increases with the number of layers, expanding from 1 meter to 5 meters.

[0050] Spatial topological constraints include reachability constraints and connectivity constraints. Reachability is verified through path planning, and connectivity is determined through spatial occupancy detection. The state transition probability is calculated based on the kinematic model, considering the constraints of velocity continuity and direction change. The number of particles in the particle swarm optimization algorithm is 200, and the velocity update uses a linearly decreasing inertia weight, decreasing from 0.9 to 0.4. The weight coefficients of the individual optimal position and the global optimal position are 2.0 and 2.0 respectively.

[0051] In the cost function, the path length weight is 0.6, and the passage difficulty weight is 0.4. The passage difficulty is jointly determined by the spatial occupancy probability and the terrain slope. Bayesian inference uses the sequential importance sampling method. The observation likelihood function is constructed based on the sensor observation model, including the distance observation term and the direction observation term. The diagonal elements of the observation noise covariance matrix are 0.25 square meters and 0.1 radian respectively. The maximum a posteriori probability estimation uses the expectation maximization algorithm with 50 iterations and a convergence threshold of 0.001. The major axis and minor axis of the confidence ellipse of the prediction result are 0.8 meters and 0.5 meters respectively, and the confidence level is 0.95.

[0052] Step 6. Calculate the spatial distance and relative azimuth angle between the predicted result of the trapped person's position and the real-time positioning data of the UWB mobile node worn by the rescue personnel, and combine with the second local octree map to perform real-time planning of the rescue path.

[0053] As Figure 2As shown, obtain the coordinate sequence of the mobile nodes of the rescue personnel collected by the ultra-wideband locator, perform smoothing filtering on the coordinate sequence by the least squares method to obtain the spatial distance parameter from the position of the trapped person; mark the passage space by the obstacle distance transformation according to the spatial distance parameter, and perform dilation processing on the obstacle boundary by morphological operations to obtain the path planning cost map; adopt a heuristic search algorithm for the path planning cost map to generate a global path point set, optimize the path point set through the dynamic programming state transition equation to obtain a discrete path sequence; fit the path curve by cubic spline interpolation according to the discrete path sequence, and perform smoothing processing on the path curve by adaptive window filtering to obtain a smooth path curve.

[0054] Exemplarily, obtain the coordinate sequence of the mobile nodes of the rescue personnel from the ultra-wideband locator, perform smoothing filtering on the coordinate sequence by the least squares method, calculate the spatial distance and azimuth angle from the predicted position of the trapped person according to vector algebra to obtain the position relationship parameter. Determine the starting point and ending point of the path planning according to the position relationship parameter, verify the reachability of the connection line between the starting and ending points by the ray detection method, mark the passage space by the obstacle distance transformation to obtain the spatial passability map. For the spatial passability map, perform dilation processing on the obstacle boundary by morphological operations, quantify the passage cost by distance field calculation, and store the cost data according to the grid index to obtain the path planning cost map.

[0055] Based on the path planning cost map, a heuristic search algorithm is used to generate a global path point set. The path points are optimized through the dynamic programming state transition equation to obtain a discrete path sequence. According to the discrete path sequence, the path points are resampled according to the equal-spacing criterion, and the path curve is fitted by cubic spline interpolation to obtain a continuous path curve. For the continuous path curve, adaptive window filtering is used to smooth the curvature change, and polynomial approximation is used to locally correct the curve segment to obtain a smooth path curve. According to the smooth path curve, a quadratic programming model based on curvature constraints is constructed, and the convex optimization algorithm is used to fine-tune the positions of the path nodes. The feasibility of the path is verified according to the collision detection rules to obtain the rescue path planning result. There are random fluctuations in the positioning data of the rescue personnel's moving nodes. The sliding window least squares method is used for smoothing. The window length is set to 5 seconds, and 50 coordinate points are collected in each window. The smoothed position coordinates are obtained through weighted solution, and the weight coefficient decays with time, decreasing from 1.0 to 0.6. When calculating the spatial distance based on the smoothed coordinates, the Euclidean distance metric is used, and the azimuth angle is calculated through the dot product and cross product of vectors, with an accuracy better than 1 degree. The reachability verification of the path start and end points uses uniformly sampled ray detection. A sampling point is taken every 0.2 meters along the line connecting the start and end points. The collision detection between the ray and the obstacle uses an improved digital differential method. Considering the complex environment of the mine roadway, the eight-neighborhood propagation model is used for distance transformation, and the propagation step size is 0.1 meters to obtain a continuous distance field distribution. The obstacle boundary dilation uses the morphological dilation algorithm, and the size of the structural element is a disk with a radius of 0.4 meters. Considering the passage width requirement of the rescue personnel, the dilation operation is iterated 2 times.

[0056] The quantization of the passage cost combines the distance field value and the slope information. The distance weight is 0.7, and the slope weight is 0.3. The cost data is stored in a quadtree structure, and the leaf node resolution is 0.1 meters. The heuristic search uses an improved bidirectional search algorithm. The heuristic function combines the Euclidean distance and the cost map value, and the search sector angle is limited within the range of plus or minus 45 degrees.

[0057] The state of dynamic programming includes the position coordinates and the direction angle. Each state node expands 7 successor states. The state transition cost considers the path length and the turning cost, and the turning penalty coefficient is 1.5. The path resampling uses the equal arc length criterion, and the sampling interval is 0.5 meters. The cubic spline interpolation uses natural boundary conditions, and the control points are selected from 3 adjacent path points to construct a local curve segment. The curvature smoothing uses adaptive Gaussian filtering, and the window size is adaptively adjusted according to the curvature change, ranging from 0.5 meters to 2 meters.

[0058] The optimization objective of the quadratic programming model includes a path length term, a curvature term, and a deviation term, with weight coefficients of 0.4, 0.3, and 0.3 respectively. The constraint conditions include a maximum curvature constraint and an obstacle avoidance constraint. Interior point method is used for convex optimization, with a maximum number of iterations of 100 and a convergence threshold of 0.001. Improved GJK algorithm is used for collision detection, with a safety margin set to 0.3 meters and a verification sampling interval of 0.1 meters.

[0059] Step 7. Project the planned rescue path onto the second local octree map, and evaluate the safety and accessibility of the path. If there are collapsed obstacles, the artificial potential field method is used, with the obstacles as repulsive sources and the trapped persons as attractive sources. By calculating the resultant force of the repulsive and attractive sources, local obstacle avoidance adjustment is performed on the path.

[0060] As Figure 3 shown, the rescue path coordinate sequence is obtained from the octree map, and the coordinate sequence is projected through the vertical projection method to obtain the path safety degree map. For the path safety degree map, the safety gap is evaluated using the passage width threshold, and the collapsed area is segmented by the region growing algorithm to obtain the passage evaluation map. According to the passage evaluation map, the centroid extraction algorithm is used to obtain the obstacle center coordinates, and the density-based clustering method is used to perform clustering operations on the obstacle center coordinates. The repulsive field distribution and the attractive field distribution are calculated according to the Gaussian kernel function and the exponential decay function. For the repulsive field distribution and the attractive field distribution, the resultant force field is calculated according to the linear superposition principle, and the path point positions are iteratively optimized by the improved gradient descent method. The path shape is smoothed according to the cubic spline curve to obtain the obstacle avoidance path result.

[0061] Exemplarily, the rescue path coordinate sequence is obtained from the second local octree map, and the path points are projected onto the map plane through the vertical projection method. The projection point gap is calculated according to the spatial obstacle distance transformation to obtain the path safety degree map. According to the path safety degree map, the safety gap is evaluated using the passage width threshold, and the collapsed area is segmented by the region growing algorithm. The safety passage is determined according to the spatial connectivity to obtain the passage evaluation map.

[0062] For the obstacle area in the passage evaluation map, the centroid extraction algorithm is used to obtain the obstacle center coordinates, and the density-based clustering method is used to cluster the obstacles to obtain the obstacle distribution map. Based on the obstacle distribution map, the Gaussian kernel function is used to calculate the obstacle repulsive field, and the force field range is controlled by adaptive adjustment of the kernel parameter to obtain the repulsive field distribution.

[0063] According to the repulsive force field distribution, an exponential decay function is used to calculate the gravitational field of the trapped person. The range of gravitational action is adjusted dynamically through the decay coefficient, and the gravitational field distribution is obtained. According to the repulsive force field distribution and the gravitational field distribution, the resultant force field is calculated according to the principle of linear superposition, and the direction of the resultant force is determined through the gradient vector to obtain the potential field gradient map. For the potential field gradient map, an improved gradient descent method is used to iteratively optimize the position of the path points, the shape of the path is smoothed by a cubic spline curve, and the feasibility of the path is verified according to the collision detection rules to obtain the obstacle avoidance path result.

[0064] During the projection registration process of the mine rescue path, the vertical projection uses the nearest neighbor search method. The projection distance from the path point to the map plane is usually between 0.1 meter and 0.5 meters. The distance transform uses the scan line algorithm to calculate the obstacle distance field, and the scan resolution is 0.05 meters. The safety clearance threshold between the path projection point and the obstacle is set to 0.8 meters. The passage evaluation uses the multi-scale analysis method. The passage width threshold is set to 1.2 meters according to the requirements for the passage of rescue equipment. The region growing algorithm expands around the projection point as the seed point, and the growth step size is 0.1 meter. It stops when encountering an obstacle or an already visited area. The spatial connectivity determination uses the breadth-first search, and the search radius is 1.5 meters. When clustering obstacles, the density-based spatial clustering algorithm is used, the search radius is set to 0.5 meters, and the minimum number of points threshold is 10. The obstacle clusters are divided through density reachability. The centroid extraction uses the weighted average method, and the weight is proportional to the density of the obstacle points. In the construction of the potential field, the standard deviation of the Gaussian kernel function is adaptively adjusted according to the obstacle size, and the range is between 0.3 meters and 1.5 meters. The intensity of the repulsive force field is positively correlated with the obstacle density. The base number of the exponential decay function of the gravitational field is 2.718, and the decay coefficient is proportional to the distance from the target point, and it changes dynamically between 0.1 and 0.5. The resultant force field calculation uses the vector superposition method, the repulsive force weight coefficient is 0.6, the gravitational force weight coefficient is 0.4, and the gradient vector is calculated using the central difference method, and the difference step size is 0.1 meter. The improved gradient descent method introduces a momentum term, the momentum coefficient is 0.8, the learning rate decreases from 0.1 to 0.01, and the maximum number of iterations is 100 times.

[0065] During the path optimization process, the cubic spline curve uses the natural boundary conditions, the control point interval is 0.3 meters, and the path smoothness is controlled through the curvature constraint, and the maximum curvature limit is 0.5. The collision detection uses the continuous collision detection method, the detection interval is 0.1 meter, the safety margin is set to 0.2 meter, and the local path replanning is triggered when a collision is detected. The path smoothness evaluation uses the cumulative curvature change rate index, and the set threshold is 0.3. When the threshold is exceeded, the path is locally adjusted.

[0066] Step 8. According to the path planning result adjusted by local obstacle avoidance, the position coordinates of the rescue personnel's moving nodes are updated in real time, and the positioning data of the trapped personnel is corrected through the graph optimization algorithm.

[0067] Obtain the original coordinate data sent by the ultra-wideband locator. The original coordinate data generates timestamp information by the rescue personnel's moving nodes; use the Kalman filter to perform state estimation on the node positions according to the original coordinate data. The state estimation obtains the motion speed sequence through Doppler frequency shift measurement; construct the node constraint relationship based on the pose graph for the motion speed sequence. The node constraint relationship establishes the error equation through odometer constraint and observation constraint; use the Gauss-Newton method to solve the error equation, and judge the optimization convergence through the relative entropy criterion to obtain the optimized position parameters.

[0068] Exemplarily, obtain the original coordinate data of the rescue personnel's moving nodes from the ultra-wideband locator, perform synchronous sampling on the data through timestamp calibration, and unify the coordinate system of the original data according to the coordinate transformation to obtain the unified reference data. According to the unified reference data, use the Kalman filter to perform state estimation on the node positions and speeds, measure the motion speed through Doppler frequency shift, and constrain the state prediction according to the path planning result to obtain the motion state sequence.

[0069] For the motion state sequence, construct the node constraint relationship based on the pose graph, establish the error equation through odometer constraint and observation constraint, initialize the pose parameters according to the maximum likelihood criterion to obtain the pose constraint graph. Based on the pose constraint graph, analyze the observation residuals using the Mahalanobis distance criterion, suppress the outliers through the Cauchy kernel function, calculate the observation weights according to the information matrix to obtain the weighted observation data. Based on the weighted observation data, construct the optimization objective function based on the least squares method, calculate the optimization direction through the Jacobian matrix, determine the step size according to the line search criterion to obtain the iterative optimization sequence. For the iterative optimization sequence, use the Gauss-Newton method to solve the position parameters, judge the optimization convergence through the relative entropy criterion, and evaluate the parameter uncertainty according to the covariance propagation to obtain the optimized position result.

[0070] According to the optimized position results, the sliding window method is used to batch process the positioning data of trapped persons, and the graph optimization algorithm is used to jointly adjust the positions to obtain the corrected positioning results. In the mine rescue scenario, the sampling frequency of ultra-wideband positioning data is 100 Hz, and the data synchronization accuracy is controlled within 10 microseconds through timestamp calibration. The coordinate system uniformly adopts the global coordinate system, and the coordinate transformation error is less than 0.01 m. The state vector of the Kalman filter includes three-dimensional position coordinates and three-dimensional velocity components, the observation vector includes position observations and Doppler frequency shift observations, and the diagonal elements of the state noise covariance matrix are 0.01 square meters and 0.1 square meters per second. In the construction of the pose graph, the odometry constraint is constructed based on the pose increment at adjacent times, the observation constraint includes distance observations and angle observations, the maximum likelihood initialization adopts the linear least squares method, and the initial position error is controlled within 0.1 m. The Mahalanobis distance threshold is set to 2.5, and observations greater than the threshold are marked as outliers. The scale parameter of the Cauchy kernel function is set to 0.1, and the diagonal elements of the information matrix are inversely proportional to the observation accuracy.

[0071] The optimization objective function includes pose constraint terms and observation constraint terms. The pose constraint weight is 0.6, and the observation constraint weight is 0.4. The Jacobian matrix is calculated using the numerical differentiation method. The step size search adopts the Armijo criterion, the initial step size is 1.0, and the attenuation coefficient is 0.5. The maximum number of iterations of the Gauss-Newton iteration is 50, and convergence is determined when the relative entropy is less than 0.001 or the position increment is less than 0.01 m. The sliding window length is set to 10 s, the window sliding step size is 1 s, and batch processing uses the marginalization method to process historical data outside the window. The graph optimization uses a sparse solver, and reweighting is performed when the Mahalanobis distance of the observation residuals is greater than 3.0. The covariance matrix of the position parameters is calculated by the linearization method.

[0072] In practical applications, when the moving speed of the rescue personnel is 1 m / s, the positioning accuracy is better than 0.2 m, and the position update delay is less than 0.1 s. The Doppler frequency shift measurement uses the phase difference method, the carrier frequency is 6.5 GHz, the phase resolution is 0.1 radian, and the speed measurement accuracy is better than 0.1 m / s. The uncertainty of the odometry constraint increases with the increase of the distance, and the growth rate is 0.05 per meter. The angular uncertainty is 0.02 radians. In the observation constraint, the standard deviation of the distance measurement is 0.1 m, the standard deviation of the angle measurement is 0.05 radians, and the connectivity of the constraint graph decays exponentially with the distance between nodes, and the decay coefficient is 0.5 per meter.

[0073] Inspired by the above embodiments of the present invention, through the above description, relevant staff can completely make various changes and modifications without departing from the technical idea of this invention. The technical scope of this invention is not limited to the content in the specification, and its technical scope must be determined according to the scope of the claims.

Claims

1. A method for positioning and tracking underground personnel based on laser ranging, characterized in that: The following steps are involved: Step 1. The three-dimensional point cloud data of the mine collapse disaster site obtained by laser radar scanning and the mobile node signal received by the UWB sensor are used to construct the first local octree map of the collapse area; Step 2. Calculate the distance between nodes through the ranging signals between UWB mobile nodes and establish a relative positioning model of UWB mobile nodes; Step 3. Align the first local octree map with the positioning model, fuse the heterogeneous data, and form an initial positioning result; Step 4. If the first local octree map fails, rebuild the second local octree map and obtain the mobile node position in the UWB relative positioning model; Step 5. Estimate the location information of the trapped person and predict the position of the trapped person; Step 6. Calculate the spatial distance and relative azimuth between the position of the trapped person and the rescuer, and plan the rescue path in real time; Step 7. Evaluate the safety and accessibility of the planned rescue path, and make local obstacle avoidance adjustments to the path by calculating the combined force of the repulsive force source and the gravitational force source; Step 8. According to the path planning results, the positioning data of the trapped persons is corrected through the graph optimization algorithm.

2. The underground personnel positioning and tracking method based on laser ranging according to claim 1 is characterized in that: In the step 1, the point cloud data is collected according to the laser radar, the point cloud data is subjected to noise reduction processing by using voxel filtering, and a point cloud data set in a unified coordinate system is obtained by iterative closest point algorithm; For the point cloud data set, the eight-connected region labeling method is used to perform raster segmentation on the density distribution, and the density distribution feature map is obtained by calculating the Gaussian attenuation function; According to the feature point marks in the density distribution feature map, a quadratic surface equation is constructed to fit the density data, a density surface model is obtained by bilateral filtering, and a mapping relationship between point cloud data and octree nodes is established through Morton coding. According to the mapping relationship of octree nodes, the mapping relationship between ultra-wideband signal strength and spatial distance is established, the Kalman filter is used to smooth the node trajectory, and the random forest algorithm is used to correct the positioning result error.

3. The underground personnel positioning and tracking method based on laser ranging according to claim 1 is characterized in that: In described step 2, the foundation of relative positioning model comprises the steps: (1) obtaining a timestamp sequence when the ultra-wideband base station receives a signal transmitted by a mobile node, and calculating a round-trip delay value of the signal through the timestamp sequence, wherein the round-trip delay value of the signal is obtained by a two-way ranging method; (2) using a threshold detection method to screen out multipath signals from the signal round-trip delay value, and establishing a signal strength attenuation curve based on the signal round-trip delay value, wherein the signal strength attenuation curve is obtained from a free space propagation model; (3) calculating node distance data according to the signal strength attenuation curve and the preset base station spatial coordinates, and constructing a base station node spatial geometric network through the node distance data, wherein the base station node spatial geometric network is obtained by Delaunay triangulation; (4) constructing a state space equation for the base station node spatial geometric network, and optimizing a state parameter through the state space equation, wherein the state parameter is obtained by a gradient descent method.

4. The underground personnel positioning and tracking method based on laser ranging according to claim 1 is characterized in that: In step 3, the initial positioning result of the trapped person is formed, comprising the following steps: (1) Acquire three-dimensional coordinate data of feature points from the octree map, acquire sensor node position data from the relative positioning model, and unify the timestamps of the three-dimensional coordinate data and the position data through a time synchronization server to obtain a synchronized data sequence; (2) resampling the feature points at a minimum time interval according to the synchronization data sequence, constructing a corresponding relationship between the feature points using the Euclidean distance criterion, and obtaining a registration feature set; (3) For the registration feature set, the singular value decomposition method is used to solve the rotation matrix and the translation vector, and the feature point coordinates are spatially transformed by the iterative reweighted closest point algorithm to obtain the spatial transformation matrix; (4) The coordinates of the laser radar point cloud data are transformed according to the spatial transformation matrix, and the position state and the observation data are multi-source fused through the Kalman information filter to obtain fused positioning data.

5. The underground personnel positioning and tracking method based on laser ranging according to claim 1 is characterized in that: In step 4, reconstructing a second local octree map of the collapsed environment within a preset positioning coverage range includes the following steps: (1) Obtaining laser radar point cloud data, removing noise from the point cloud data using a statistical outlier filtering algorithm, and obtaining feature point data through principal curvature calculation; (2) Based on the feature point data, the scene plane features are extracted using a normal vector clustering method, and the pose constraint relationship in the spatial topology graph is obtained using a minimum spanning tree algorithm; (3) According to the pose constraint relationship, an error function based on point-to-surface distance is constructed, and the Gauss-Newton optimization algorithm is used to obtain the optimized pose parameters; (4) updating and reorganizing the octree structure according to the optimized posture parameters, dividing the space occupancy state by a density clustering algorithm, and obtaining an updated octree structure; (5) If the overlap between the updated octree structure and the first local octree map exceeds a preset threshold, it is determined that the scene has collapsed.

6. The underground personnel positioning and tracking method based on laser ranging according to claim 1 is characterized in that: The step 5 comprises the following steps: (1) Obtaining ultra-wideband signal strength data and packet loss rate data, judging the communication interruption status according to the signal quality statistics, and obtaining the communication status judgment result; (2) Use a circular buffer to store historical map data and positioning data, and obtain an ordered historical data sequence through data timestamp verification and data integrity check; (3) For the ordered historical data sequence, a recurrent neural network is used to construct a target motion state predictor, and the prediction sequence is smoothed by a sliding time window to obtain a target motion prediction result; (4) Based on the target motion prediction results, the Monte Carlo method is used to generate state sampling points, the sampling points are screened by spatial topological constraints, and the state transition probability is constructed according to the Markov property to obtain the prediction result of the trapped person's position.

7. The underground personnel positioning and tracking method based on laser ranging according to claim 1 is characterized in that: The step 6 comprises the following steps: (1) Obtaining the coordinate sequence of the mobile node of the rescuer collected by the ultra-wideband locator, smoothing and filtering the coordinate sequence by the least squares method, and obtaining the spatial distance parameter to the position of the trapped person; (2) Marking the passage space by using obstacle distance transformation according to the spatial distance parameter, dilating the obstacle boundary by morphological operation, and obtaining a path planning cost map; (3) A heuristic search algorithm is used to generate a global path point set for the path planning cost graph, and the path point set is optimized through a dynamic programming state transfer equation to obtain a discrete path sequence; (4) Fitting the path curve by cubic spline interpolation according to the discrete path sequence, and smoothing the path curve by adaptive window filtering to obtain a smooth path curve.

8. The method for underground personnel positioning and tracking based on laser ranging according to claim 1, characterized in that: Described step 7 comprises the following steps: (1) Obtaining a rescue path coordinate sequence according to the octree map, and projecting the coordinate sequence by a vertical projection method to obtain a path safety degree graph; (2) Using the pass width threshold to evaluate the safety gap in the path safety graph, segmenting the collapsed area through the region growing algorithm, and obtaining a pass evaluation graph; (3) Obtaining the center coordinates of obstacles using a centroid extraction algorithm based on the traffic assessment map, clustering the center coordinates of obstacles using a density-based clustering method, and calculating the repulsive field distribution and the gravitational field distribution using a Gaussian kernel function and an exponential decay function; (4) The resultant force field is calculated according to the linear superposition principle for the repulsive field distribution and the gravitational field distribution, the path point positions are iteratively optimized by an improved gradient descent method, and the path shape is smoothed according to a cubic spline curve to obtain an obstacle avoidance path result.

9. The underground personnel positioning and tracking method based on laser ranging according to claim 1, characterized in that: The step 8 comprises the following steps: (1) obtaining the original coordinate data sent by the ultra-wideband locator, wherein the original coordinate data is generated by the rescuer's mobile node with time stamp information; (2) using a Kalman filter to perform state estimation on the node position according to the original coordinate data, wherein the state estimation obtains a motion velocity sequence by measuring the Doppler frequency shift; (3) constructing a node constraint relationship based on a pose graph for the motion speed sequence, wherein the node constraint relationship establishes an error equation through an odometer constraint and an observation constraint; (4) The Gauss-Newton method is used to solve the error equation. The solution result is judged by the relative entropy criterion for optimization convergence to obtain the optimized position parameters.

Images