A robot localization method and device in a degraded tunnel scenario

Abstract

This invention relates to a robot localization method and apparatus in degraded tunnel scenarios, belonging to the field of robot localization technology. The robot localization method in degraded tunnel scenarios includes: obtaining prior path information and landmark structure location information from tunnel design drawings; constructing a path likelihood model, a relative distance constraint model, and a pose constraint model; fusing the path likelihood model, relative distance constraint model, and pose constraint model based on a particle filter algorithm to obtain the robot's odometry pose estimation result; extracting multi-fine-grained features from tunnel side view images captured by cameras mounted on the robot; performing loop closure detection based on the Viterbi algorithm and multi-fine-grained features to obtain the robot's loop closure pose estimation result; and determining the optimal pose estimate of the robot based on the odometry pose estimation result and the loop closure pose estimation result. This invention enables high-precision robot localization in degraded tunnel scenarios.

Description

Technical Field

[0001] This invention relates to the field of robot positioning technology, and in particular to a robot positioning method and apparatus in a degraded tunnel scenario. Background Technology

[0002] Traditional tunnel construction relies primarily on manual labor. For example, circular shield tunnel sections in subway systems require high-precision work such as drilling holes in tunnel walls and installing supports. This presents challenges due to harsh environments, low efficiency, low accuracy, and high labor intensity. Therefore, automated robots are needed to replace manual labor. High-precision positioning is crucial for robots operating within tunnels, ensuring both work quality and automation. However, the enclosed nature of tunnels, structural degradation, and the high degree of similarity in environments present significant challenges to achieving high-precision positioning for robots.

[0003] Because degraded tunnel scenarios during construction (such as subway tunnels) are completely enclosed indoor environments, Global Navigation Satellite System (GNSS) signals are lost, rendering commonly used GNSS-based outdoor positioning methods ineffective. Furthermore, insufficient lighting, limited structural features, and easily confused environments within construction tunnels present numerous technical challenges to the high-precision positioning of operational robots.

[0004] Therefore, how to achieve high-precision robot positioning in degraded tunnel scenarios has become an urgent technical problem to be solved. Summary of the Invention

[0005] In view of this, it is necessary to provide a robot localization method and apparatus in degraded tunnel scenarios to solve the problem of low robot localization accuracy in current degraded tunnel scenarios.

[0006] To address the aforementioned problems, in a first aspect, the present invention provides a robot localization method in a degraded tunnel scenario, comprising: Prior path information and landmark structure location information are obtained from tunnel design drawings. A path likelihood model is constructed based on the prior path information, a relative distance constraint model is constructed based on the landmark structure location information, and a pose constraint model is constructed based on odometry observation data. The path likelihood model is used to constrain the robot's path in the tunnel, and the relative distance constraint model is used to constrain the relative distance between the robot and the landmark structure. The odometry observation data includes the observation data of the IMU and wheel speed meter set on the robot. The odometry pose estimation results of the robot are obtained by fusing the path likelihood model, relative distance constraint model and pose constraint model based on the particle filter algorithm. Multi-fine-grained features are extracted from tunnel side view images captured by cameras mounted on the robot. Loop closure detection is performed based on the Viterbi algorithm and multi-fine-grained features to obtain the robot's loop closure detection pose estimation results. The optimal pose estimate of the robot is determined based on the odometry pose estimation results and the loop closure detection pose estimation results.

[0007] In one possible implementation, constructing the path likelihood model based on prior path information includes: The prior path information is uniformly sampled with a preset step size to obtain a first number of sampling points, and the Gaussian kernel function of the robot position at each sampling point is determined. A path likelihood model is constructed by linearly superimposing and normalizing the Gaussian kernel function of the robot's position at each sampling point.

[0008] In one possible implementation, the construction of the relative distance constraint model based on the location information of landmark structures includes: Construct the linear equations corresponding to the location information of the landmark structures based on the location information of the landmark structures; A relative distance constraint model is constructed based on the linear equations corresponding to the location information of landmark structures and the robot's position.

[0009] In one possible implementation, the odometry-based observation data is used to construct the pose constraint model, which includes: Determine the pose change between adjacent robot states based on odometry observation data; A pose constraint model is constructed based on the pose changes between adjacent robot states.

[0010] In one possible implementation, the odometry pose estimation result of the robot, obtained by fusing the path likelihood model, relative distance constraint model, and pose constraint model based on the particle filter algorithm, includes: The robot's pose at different times is treated as a set of particles, and a joint likelihood model is constructed based on the path likelihood model, the relative distance constraint model, and the pose constraint model. The weight of each particle in the particle set is updated based on the joint likelihood model, and the particle with the largest weight is determined as the odometry pose estimation result of the robot.

[0011] In one possible implementation, the loop closure detection based on the Viterbi algorithm and multi-fine-grained features to obtain the robot's loop closure pose estimation result includes: The observation sequence is constructed based on multiple fine-grained features, and the Viterbi algorithm is used to perform sequence matching on the observation sequence. The robot's loop closure detection pose estimation result is obtained based on the sequence matching result.

[0012] In one possible implementation, determining the optimal pose estimate of the robot based on odometry pose estimation results and loop closure detection pose estimation results includes: The robot's pose is used as the node state variable, the odometry pose estimation result is used as the odometry factor, and the loop closure detection pose estimation result is used as the loop closure factor. The optimal estimate of the robot's pose is determined based on the graph optimization algorithm.

[0013] On the other hand, the present invention also provides a robot positioning device in a degraded tunnel scenario, comprising: The module is used to obtain prior path information and landmark structure location information from tunnel design drawings, build a path likelihood model based on the prior path information, build a relative distance constraint model based on the landmark structure location information, and build a pose constraint model based on odometry observation data. The path likelihood model is used to constrain the robot's path in the tunnel, and the relative distance constraint model is used to constrain the relative distance between the robot and the landmark structure. The odometry observation data includes the observation data of the IMU and wheel speed meter set on the robot. The first determining module is used to fuse the path likelihood model, the relative distance constraint model, and the pose constraint model based on the particle filter algorithm to obtain the odometry pose estimation result of the robot. The loop closure detection module is used to extract multi-fine-grained features from the tunnel side view images captured by the camera set on the robot, and to perform loop closure detection based on the Viterbi algorithm and multi-fine-grained features to obtain the loop closure detection pose estimation results of the robot. The second determining module is used to determine the optimal pose estimate of the robot based on the odometry pose estimation results and the loop closure detection pose estimation results.

[0014] Secondly, the present invention also provides a positioning device, including a memory and a processor, wherein, The memory is used to store programs; The processor, coupled to the memory, is used to execute the program stored in the memory to implement the steps in the robot localization method in the degraded tunnel scenario described in any of the above implementations.

[0015] Thirdly, the present invention also provides a computer-readable storage medium for storing a computer-readable program or instructions, which, when executed by a processor, can implement the steps in the robot localization method in the degraded tunnel scenario described in any of the above implementations.

[0016] The beneficial effects of this invention are as follows: The robot localization method and apparatus in degraded tunnel scenarios provided by this invention, while constraining the pose using odometry observation data, further corrects the robot's position using prior path information and landmark structure information in the tunnel design drawings, thereby improving the positioning accuracy without relying on GNSS signals. In addition, this invention further ensures the accuracy of robot localization by extracting multi-fine-grained features from the tunnel side view images collected by the robot for loop closure detection. Finally, the optimal pose estimate of the robot is obtained by fusing the odometry pose estimation results and the loop closure detection pose estimation results, thus achieving high-precision robot localization in degraded tunnel scenarios. Attached Figure Description

[0017] Figure 1 This is a schematic flowchart of an embodiment of the robot localization method in a degraded tunnel scenario provided by the present invention; Figure 2 A schematic flowchart of an embodiment of the degraded tunnel positioning and mapping process provided by the present invention; Figure 3 A schematic diagram of the structure of one embodiment of the degraded tunnel positioning and mapping system provided by the present invention, and a schematic diagram of the process of one embodiment; Figure 4 A schematic diagram of an embodiment of the tunnel landmark structure detection process provided by the present invention; Figure 5 This is a schematic flowchart of an embodiment of the particle filtering process based on the HMM model provided by the present invention. Figure 6 A schematic flowchart of an embodiment of the Viterbi sequence matching process provided by the present invention; Figure 7 A schematic diagram of an embodiment of the dynamic causal domain graph model provided by the present invention; Figure 8 A schematic diagram of an embodiment of the robot positioning device in a degraded tunnel scenario provided by the present invention; Figure 9 A schematic diagram of an embodiment of the positioning device provided by the present invention. Detailed Implementation

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

[0019] In the description of the embodiments of the present invention, unless otherwise stated, "multiple" means two or more. "And / or" describes the relationship between related objects, indicating that there can be three relationships. For example, A and / or B can represent three situations: A exists alone, A and B exist simultaneously, and B exists alone.

[0020] The terms "first," "second," etc., used in the embodiments of this invention are for descriptive purposes only and should not be construed as indicating or implying their relative importance or implicitly specifying the number of technical features indicated. Therefore, a technical feature defined with "first" or "second" may explicitly or implicitly include at least one of that feature.

[0021] In this document, the term "embodiment" means that a particular feature, structure, or characteristic described in connection with an embodiment may be included in at least one embodiment of the invention. The appearance of this phrase in various places throughout the specification does not necessarily refer to the same embodiment, nor is it a separate or alternative embodiment mutually exclusive with other embodiments. It will be explicitly and implicitly understood by those skilled in the art that the embodiments described herein can be combined with other embodiments.

[0022] Because degraded tunnel scenarios during construction (such as subway tunnels) are completely enclosed indoor environments, GNSS signals are lost, rendering commonly used GNSS-based outdoor positioning methods ineffective. Furthermore, insufficient lighting, limited structural features, and easily confused environments within construction tunnels present numerous technical challenges to the high-precision positioning of operational robots.

[0023] 1. Since the tunnel is still under construction, most of the infrastructure has not been installed in the early stages of construction. For example, the transponder-based positioning method commonly used in subways cannot be applied to tunnel construction robots, and indoor positioning methods based on wireless signal facilities also cannot be applied.

[0024] (2) Tunnels often have long distances. Odometry positioning methods based on sensors such as inertial measurement units (IMUs) and lidar are mainly based on the continuous recursion of adjacent positions. Therefore, the position measurement error will accumulate with the increase of distance. Many Simultaneous Localization and Mapping (SLAM) methods add loop closure optimization on the basis of odometry, which can reduce the impact of accumulated error. However, existing scene recognition technologies have difficulty achieving accurate loop closure detection in degraded tunnel environments with repetitive scenes.

[0025] (3) Currently, the LiDAR SLAM localization and mapping method is commonly used in indoor scenes. It extracts and registers the point cloud features by scanning the surrounding environment to obtain high-precision mapping results. However, due to its reliance on scene structural features, and the fact that tunnel structures are largely repetitive and the walls are smooth, the accuracy of the general SLAM method for localization and mapping is significantly reduced, resulting in severe drift and localization failure, and obvious ghosting in the mapping. In addition, during construction, the sensors carried by the robot may completely fail due to harsh environments (such as dust, vibration, lack of lighting, etc.) or equipment failure, leading to loss of localization and seriously affecting the construction progress and safety.

[0026] To address the shortcomings of robot localization in degraded tunnel scenarios, this invention provides a robot localization method and apparatus for degraded tunnel scenarios, which will be described below.

[0027] Figure 1 This is a schematic flowchart of an embodiment of the robot localization method in a degraded tunnel scenario provided by the present invention, as shown below. Figure 1 As shown, the robot localization method in the degraded tunnel scenario includes: S101. Obtain prior path information and landmark structure location information from the tunnel design drawings. Construct a path likelihood model based on the prior path information, construct a relative distance constraint model based on the landmark structure location information, and construct a pose constraint model based on the odometry observation data. The path likelihood model is used to constrain the robot's path in the tunnel, and the relative distance constraint model is used to constrain the relative distance between the robot and the landmark structure. The odometry observation data includes the observation data from the IMU and wheel speed meter installed on the robot.

[0028] It should be noted that the robot localization method for degraded tunnel scenarios provided by this invention can be applied to the localization of robots working inside tunnels, especially during tunnel construction. When performing robot localization, the localization device (e.g., a desktop or portable computer located outside the tunnel, or a microcomputer mounted on the robot) can first obtain prior path information and landmark structure location information from the tunnel design drawings. The prior path information may include the two-dimensional coordinates and direction of the path within the tunnel, while the landmark structure location information may include the dimensions and location of connecting passages within the tunnel.

[0029] After obtaining prior path information and landmark location information, a path likelihood model can be constructed based on the prior path information. This model establishes a correlation between the design drawing path and the positioning, using the path as a new constraint to improve positioning accuracy. Then, a relative distance constraint model can be constructed based on the landmark location information. The robot's position is corrected by the relative distance between the robot's position and the landmark location, further improving positioning accuracy. During operation, the landmark location can be detected by the lidar installed on the robot. Finally, a pose constraint model can be constructed using odometry data (i.e., data from the wheel speedometer and IMU installed on the robot) to apply traditional pose constraints to the robot.

[0030] S102. Based on the particle filter algorithm, the path likelihood model, relative distance constraint model and pose constraint model are fused to obtain the odometry pose estimation result of the robot.

[0031] It should be noted that after completing the path likelihood model, relative distance constraint model, and pose constraint model, the robot's assumed posture in the tunnel can be used as a particle. The path likelihood model, relative distance constraint model, and pose constraint model can be used as observations to update the particle state and weights, thereby obtaining the robot's odometry pose estimation results. By fusing the path likelihood model, relative distance constraint model, and pose constraint model, the accuracy of robot localization can be further improved.

[0032] S103. Extract multi-fine-grained features from the tunnel side view images captured by the camera set on the robot, perform loop closure detection based on the Viterbi algorithm and multi-fine-grained features, and obtain the loop closure detection pose estimation result of the robot.

[0033] It should be noted that during robot localization, multi-fine-grained features can be extracted from the tunnel side view images captured by cameras mounted on the robot. For example, the image can be divided into 1×1 (global tunnel texture), 2×2 (construction joints, pipeline layout), and 4×4 (local blemishes, stains) scales. This multi-fine-grained segmentation enhances the recognizability of the tunnel scene. Then, loop closure detection is performed using the Viterbi algorithm and multi-fine-grained features to determine the relative pose relationship between the robot's current position and map nodes, obtaining the robot's loop closure pose estimation result, further ensuring the accuracy of robot localization.

[0034] S104. Determine the optimal pose estimate of the robot based on the odometry pose estimation results and the loop closure detection pose estimation results.

[0035] It should be noted that after determining the odometry pose estimation results and the loop closure detection pose estimation results, the optimal pose estimate of the robot can be obtained by fusing the odometry pose estimation results and the loop closure detection pose estimation results, thereby achieving high-precision positioning of the robot in degraded tunnel scenarios.

[0036] In summary, the robot localization method in degraded tunnel scenarios provided by this invention not only constrains the robot's position using odometry observation data, but also further corrects the robot's position using prior path information and landmark structure information from tunnel design drawings. This improves positioning accuracy without relying on GNSS signals. Furthermore, this invention extracts multi-fine-grained features from tunnel side view images collected by the robot for loop closure detection, further ensuring the accuracy of robot localization. Finally, the optimal robot pose estimate is obtained by fusing the odometry pose estimation results and the loop closure detection pose estimation results, achieving high-precision robot localization in degraded tunnel scenarios.

[0037] In some embodiments of the present invention, the construction of a path likelihood model based on prior path information includes: The prior path information is uniformly sampled with a preset step size to obtain a first number of sampling points, and the Gaussian kernel function of the robot position at each sampling point is determined. A path likelihood model is constructed by linearly superimposing and normalizing the Gaussian kernel function of the robot's position at each sampling point.

[0038] It should be noted that when constructing a path likelihood model based on prior path information, the prior path information can first be uniformly sampled at a preset step size to obtain the first number of sampling points. For example, on the path in the design drawing, ... Perform uniform sampling for the step size. ,in This indicates the number of sampling points. Each sampling point includes its two-dimensional coordinates and the path tangent angle of that point.

[0039] Then the Gaussian kernel function for the robot's position at each sampling point can be determined. For example, for each sampling point on the tunnel path. A kernel function can be used to describe the robot's position. At the sampling point The probability density function (PDF) on the surface is used in this invention with a dimension of . The standard multivariate Gaussian kernel function is shown below:

[0040] in, For bandwidth kernel function, It is a diagonal matrix, where the diagonal elements are Gaussian models. Bandwidth across all dimensions. The bandwidth parameter is dynamically adjusted according to the allowable error range of tunnel construction to ensure that path constraints are effectively corrected within the construction accuracy range, avoiding model failure caused by deviations between tunnel design drawings and the actual site. arrive The closer the distance, the higher the probability.

[0041] Finally, the Gaussian kernel function of the robot's position at each sampling point can be linearly superimposed and normalized to construct a path likelihood model.

[0042] For example, based on the kernel density estimation method, The likelihood model based on path assistance is obtained by linearly superimposing and normalizing the Gaussian kernel functions of the path sampling points, as shown in the following formula:

[0043] in, This represents the robot's position in the tunnel's top-view plane, along with its two-dimensional projection coordinates and heading angle.

[0044] In some embodiments of the present invention, the construction of a relative distance constraint model based on landmark structure location information includes: Construct the linear equations corresponding to the location information of the landmark structures based on the location information of the landmark structures; A relative distance constraint model is constructed based on the linear equations corresponding to the location information of landmark structures and the robot's position.

[0045] It should be noted that when constructing a relative distance constraint model based on the location information of landmark structures, the corresponding linear equations can be constructed based on the location information of the landmark structures. For example, the location information of landmark structures in a tunnel can be pre-stored in the form of linear equations using design drawings, as follows:

[0046] in, Indicates the first in the design drawings Information about the straight lines of the building's wall outline for each road sign. This represents the total number of straight lines outlining the landmark walls in the drawing.

[0047] Next, a relative distance constraint model can be constructed using the linear equations corresponding to the location information of landmark structures and the robot's position. For example, using the robot's location coordinates... The relative distance to the wall of the connecting passage can be calculated, thus forming a relative positioning constraint:

[0048] In some embodiments of the present invention, the construction of the pose constraint model based on odometry observation data includes: Determine the pose change between adjacent robot states based on odometry observation data; A pose constraint model is constructed based on the pose changes between adjacent robot states.

[0049] It should be noted that when constructing a pose constraint model based on odometry observation data, the pose change between adjacent robot states can be determined first based on the odometry observation data, and then the pose constraint model can be constructed using the pose change between adjacent robot states.

[0050] In some embodiments of the present invention, the step of fusing the path likelihood model, the relative distance constraint model, and the pose constraint model based on the particle filter algorithm to obtain the robot's odometry pose estimation result includes: The robot's pose at different times is treated as a set of particles, and a joint likelihood model is constructed based on the path likelihood model, the relative distance constraint model, and the pose constraint model. The weight of each particle in the particle set is updated based on the joint likelihood model, and the particle with the largest weight is determined as the odometry pose estimation result of the robot.

[0051] It should be noted that when obtaining the odometry pose estimation result of the robot by fusing the path likelihood model, relative distance constraint model and pose constraint model according to the particle filter algorithm, the robot's pose at different times can be used as a set of particles. A joint likelihood model is constructed through the path likelihood model, relative distance constraint model and pose constraint model. Then, the weight of each particle in the set of particles is updated through the joint likelihood model, and the particle with the largest weight is determined as the odometry pose estimation result of the robot.

[0052] In some embodiments of the present invention, the step of performing loop closure detection based on the Viterbi algorithm and multiple fine-grained features to obtain the robot's loop closure pose estimation result includes: The observation sequence is constructed based on multiple fine-grained features, and the Viterbi algorithm is used to perform sequence matching on the observation sequence. The robot's loop closure detection pose estimation result is obtained based on the sequence matching result.

[0053] It should be noted that when performing loop closure detection based on the Viterbi algorithm and multiple fine-grained features to obtain the robot's loop closure pose estimation results, an observation sequence can be constructed based on the multiple fine-grained features. The Viterbi algorithm is then used to perform sequence matching on the observation sequence, and the robot's loop closure pose estimation results are obtained based on the sequence matching results. In other words, the optimal hidden state sequence is found through the tunnel side-view image observation sequence.

[0054] In some embodiments of the present invention, determining the optimal pose estimate of the robot based on odometry pose estimation results and loop closure detection pose estimation results includes: The robot's pose is used as the node state variable, the odometry pose estimation result is used as the odometry factor, and the loop closure detection pose estimation result is used as the loop closure factor. The optimal estimate of the robot's pose is determined based on the graph optimization algorithm.

[0055] It should be noted that when determining the optimal robot pose estimate based on odometry pose estimation and loop closure detection pose estimation results, the robot pose can be used as a node state variable, the odometry pose estimation result as an odometry factor, and the loop closure detection pose estimation result as a loop closure factor. A graph optimization algorithm can then be used to determine the optimal robot pose estimate. Specifically, the optimization problem is transformed into a problem of relationships between edges and nodes using a factor graph. The robot pose is set as a node state variable in the graph, the pose estimation obtained by particle filtering is used as an odometry factor, and constraints are applied to adjacent nodes. The sequence matching result is used as a loop closure factor to correct for the cumulative positioning error in long-distance tunnels.

[0056] To address the positioning failure problem in degraded tunnels caused by incomplete infrastructure, insufficient lighting, and sparse structural features during construction, this invention provides a robot positioning and mapping method and system for degraded tunnel scenarios.

[0057] Combination Figure 2 The present invention constructs a path likelihood model based on tunnel design drawings, uses lidar to detect landmark structures, and combines wheel speedometers and IMUs to calculate pose transformation; it uses multi-fine-grained features of side view images to perform sequence matching to form loop closure correction, and optimizes robot pose estimation in the graph optimization model to achieve robot mapping and localization in degraded tunnels.

[0058] Combination Figure 3 The positioning and mapping system's sensor layout includes a wheel speedometer, an IMU, a lidar, and a camera. The lidar is mounted on the top of the robot, the camera is mounted on the side of the robot, the IMU is mounted directly below the lidar, and the wheeled mobile chassis has an incremental wheel speedometer.

[0059] This invention provides a detailed solution using a typical subway tunnel scenario as an example, and is also applicable to enclosed, long-distance, degraded tunnel environments such as road tunnels, mining tunnels, and water conservancy tunnels. Specifically, it includes the following steps: 1. Path likelihood model based on tunnel design drawings and various observation constraints.

[0060] This invention utilizes design drawings for location assistance. These drawings include the tunnel's design path and specify the dimensions and locations of typical landmark structures (such as connecting passages). Unlike general SLAM methods, this invention constructs a path likelihood model based on prior path information from the design drawings. This model, combined with multi-source data acquired by the robot's onboard sensors, is used to calculate the robot's pose, effectively avoiding data corruption issues that arise from relying solely on sensor-based positioning. Even if all the robot's onboard sensors fail, the path likelihood model can still output positioning results based on the design path, ensuring continuity of positioning during construction and preventing positioning loss.

[0061] 1.1. Construction of the likelihood model assisted by the tunnel path.

[0062] The robot's movement in a tunnel is limited by the narrow terrain, meaning its position is on a predetermined design path, but construction errors are inevitable compared to the design drawings. This invention proposes a path-aided likelihood model (PA-LM), which establishes a correlation between the design path and the positioning, using the path as a new constraint to improve positioning accuracy. The specific steps are as follows: 1.1.1. Tunnel design drawing path sampling.

[0063] Based on prior path information from subway tunnel design drawings, the continuous path is discretized into sparse sampling points. A path likelihood model is constructed using Gaussian kernel density estimation to constrain the spatial consistency between the robot's trajectory and the design path. Specifically, on the path in the design drawings... Perform uniform sampling for the step size. , where M represents the number of sampling points, and each sampling point includes its two-dimensional coordinates and the path tangent angle of that point.

[0064] 1.1.2. Probability density function of sampling points along the tunnel path.

[0065] For each sampling point on the tunnel path A kernel function can be used to describe the robot's position. At the sampling point The probability density function used in this invention is a standard multivariate Gaussian kernel function with dimension d, as shown below:

[0066] in, For bandwidth kernel function, It is a diagonal matrix, where the diagonal elements are Gaussian models. Bandwidth across all dimensions. The bandwidth parameter is dynamically adjusted according to the allowable error range of tunnel construction to ensure that path constraints are effectively corrected within the construction accuracy range, avoiding model failure caused by deviations between tunnel design drawings and the actual site. arrive The closer the distance, the higher the probability.

[0067] 1.1.3. Path-aided likelihood model based on PDF overlay.

[0068] Based on the kernel density estimation method, the Gaussian kernel functions of the M path sampling points are linearly superimposed and normalized to obtain the path-aided likelihood model, as shown in the following formula:

[0069] in, This represents the robot's position in the tunnel's top-view plane, along with its two-dimensional projection coordinates and heading angle.

[0070] 1.2. Pose constraints based on hybrid features of tunnel point clouds.

[0071] Wheel velocities and IMUs can provide accurate motion estimates in the short term, but robots are susceptible to slippage and vibration in tunnels. LiDAR point cloud registration can generate accurate pose estimates in structurally sound subway tunnel platform areas, but its accuracy is poor in structurally degraded circular shield tunnel sections. A dynamic weighted approach based on filtering is employed, using velocity and angle data from wheel velocities and IMUs as input for prediction, and then updating the pose by calculating pose transformation through hybrid point cloud feature matching.

[0072] Specifically, the hybrid feature approach adds tunnel adaptive intensity features to the commonly used line and surface features. Although structural features are sparse in tunnel scenes, different materials exist, and the reflection intensity of LiDAR point clouds varies for different material surfaces. For example, in subway tunnels, there are stable intensity features such as signs and supports. By statistically analyzing the proportion of each intensity interval using histograms and setting intensity threshold intervals, point clouds whose intensity meets the threshold intervals are extracted as intensity features, providing a good constraint for point cloud registration.

[0073] Furthermore, a degradation factor is introduced to dynamically adjust the state estimation covariance, optimizing the constraint of lidar pose estimation on the odometer in degraded tunnels. The odometer pose estimation observation is the pose change between adjacent states, denoted as... .

[0074] 1.3. Relative pose constraints based on tunnel landmark structure detection.

[0075] In long subway tunnels, there are some clearly visible structures, such as connecting passages and platform junctions, whose locations and structures are clearly marked in the construction drawings. This invention uses LiDAR to scan these landmark structures and combines them with prior information from the design drawings to accurately calculate the robot's relative position, effectively solving the problem of positioning failure caused by long distances in degraded tunnel scenarios. Specifically, when the LiDAR scans a clearly visible structure in the tunnel, it calculates the robot's relative distance by fitting its edge contour lines, thus obtaining the position information. Using the location and geometric parameters of connecting passages and other structures marked in the design drawings, the robot's pose is strongly constrained to the tunnel's landmark structures by fitting the contours using LiDAR point clouds, achieving position correction in tunnel construction scenarios.

[0076] 1.3.1. Extraction of point cloud of tunnel landmark structures.

[0077] By performing principal component analysis (PCA) on the point cloud, it was found that when the laser scanned the connecting passage in the tunnel, the orientation of the principal components changed significantly due to the shape of the point cloud. A change threshold was set to quickly detect landmark structures. The point cloud was filtered to remove ground and top points, leaving only the point cloud of the wall outline. This was then projected onto a two-dimensional plane to obtain the edge outline point cloud.

[0078] 1.3.2. Straight line fitting of the tunnel wall profile for landmarks.

[0079] Furthermore, combined Figure 4 Let's look at the straight line that fits the contour of the wall edge, represented as:

[0080] Where K is the number of detected straight lines along the wall edge contour.

[0081] 1.3.3. Linear constraints on landmark structures.

[0082] Based on the design drawings, the location information of landmark structures in the tunnel is pre-stored in the form of a linear equation as follows:

[0083] in, This represents the information of the straight line outline of the i-th landmark building wall in the design drawings, where I is the total number of landmark wall outlines in the drawings. Using prior equation information from the design drawings, and through the robot's location coordinates... The relative distance to the wall of the connecting passage can be calculated, thus forming a relative positioning constraint:

[0084] Taking the lidar itself as the origin, the relative distance between the robot and the edge of the wall is... After obtaining the laser relative positioning, linear constraints can be established by combining the position equations of landmark structures in the tunnel design drawings.

[0085] 2. Tunnel localization filtering that integrates path likelihood model and multi-source observations.

[0086] The method of this invention integrates robot localization and mapping within a tunnel into a maximum a posteriori (MAP) inference problem within a dynamic Bayesian framework, as shown below:

[0087] in, This represents the robot's 6-DOF pose in the tunnel at time t. This represents the multi-source observation at time t.

[0088] Combination Figure 5 In this invention, a particle filtering method is chosen to estimate the posterior probability using a set of weighted particles, which represent the robot's assumed posture within the tunnel. The specific steps for degenerate tunnel localization using particle filtering are as follows: 2.1. Initial state of the particle.

[0089] The set of robot poses in the tunnel over time t can be represented as: The corresponding weight set is The robot's initial position is known by default, and it can start from a fixed starting point in the tunnel each time. Samples are taken in the vicinity of the initial coordinates using a Gaussian distribution, and each particle is initialized with a uniform weight of 1 / N.

[0090] 2.2. Prediction stage based on first-order HMM.

[0091] The robot's state transitions in the tunnel are represented using a first-order Hidden Markov Model (HMM), meaning the current state depends only on the state at the previous time step. Therefore, the prior probability distribution of the transitions is used as an important function to plot the particles, calculated as follows:

[0092] The robot motion model passes through the previous state Predicted state as follows:

[0093] in, It can be determined by assuming that the robot moves at a constant speed and a constant angular velocity for a short period of time. The process noise follows a zero-mean Gaussian distribution with covariance of [missing value]. It can be obtained from historical states through statistical methods.

[0094] 2.3. Asynchronous weight update stage for multi-observation constraints.

[0095] After obtaining the particle set through important function sampling, the weights are updated based on real-time observations in the tunnel:

[0096] Among them, probability The observation value obtained at the i-th particle. The possibility. The observations mainly consist of three parts: special observations obtained from the tunnel PA-LM. The odometry observations obtained by the sensors on the robot Observation of the relative position of the robot as it passes through the tunnel landmark structure The three observations do not apply to the weighting function simultaneously; an asynchronous update strategy is employed. Assume... , , Since they are mutually independent, the joint likelihood can be written as:

[0097] 2.3.1. Observation probability of path likelihood model.

[0098] For each particle , This indicates that the robot's pose at time t is Possibility of tunnel design path:

[0099] 2.3.2. Mileage estimation observation probability.

[0100] This indicates the state of the robot's posture. Mileage measurement observations occurred at that time The probability of odometer observations. Assume the noise in the observed data follows a Gaussian distribution. Therefore, the conditional probability of odometer observations can be expressed by a Gaussian model as follows:

[0101] in, The pose change obtained through pose estimation is... , Let be the covariance matrix of the odometer observation data, and be the observation function. Specifically: ,in This represents the optimal state estimate at the previous moment.

[0102] 2.3.3. Observation probability of tunnel landmark structures.

[0103] Indicates when the robot is in a pose Road sign observation The probability of establishing landmark observations is as follows: Once a landmark structure is detected, the same structure will be searched around the robot's predicted location in the design drawing.

[0104] in, Distance to tunnel landmark structures obtained via lidar , The landmark observation function at time t: .

[0105] 2.3.4. Weight normalization.

[0106] Furthermore, the weights of each state sample of the robot in the tunnel are updated and normalized:

[0107] 2.4. Particle resampling.

[0108] The weights determine the reliability of a particle and can also be used to assess the effective number of particles. Particles with weights greater than a threshold are considered effective particles. If the number of effective particles is too small, this invention employs a low-variance sampling method for resampling, replicating particles with larger weights according to their proportions to perform resampling.

[0109] 2.5. Particle filter localization results.

[0110] By dynamically updating the particles and their corresponding weights, the optimal robot localization result can ultimately be obtained from the particles with the highest weights. In this way, the odometry pose of the robot in the tunnel can be calculated.

[0111] 3. Loop closure detection based on tunnel multi-fine-grained feature sequence matching.

[0112] As the robot moves back and forth in the tunnel, it returns to its previous position. This invention uses a sequence matching method based on the Viterbi algorithm for loop closure detection, thereby correcting the accumulated positioning error. Unlike general loop closure detection methods, this method enhances the spatiotemporal continuity of visual features in degraded tunnel scenes through multi-fine-grained feature sequence matching, effectively solving the problem of loop closure detection failure caused by scene repetition in the tunnel.

[0113] Combination Figure 6As can be seen, the optimal pose estimation of the robot's history in this invention... and corresponding map information Constitute a hidden state The observation corresponding to the robot's pose during the return journey is Specifically, this means solving for the hidden states corresponding to the query image sequence.

[0114] 3.1. Multi-granular visual representation of tunnel side views.

[0115] This invention performs multi-fine-grained feature extraction on tunnel images viewed from the side by a robot. Specifically, it employs a NetVLAD-based convolutional neural network to extract tunnel visual features, resulting in a normalized 512-bit feature descriptor. To address the challenge of visual repetition in tunnel scenes, the image is segmented, and features are extracted at different fine-grained levels. Global features are extracted at three fine-grained levels, dividing the image into 1×1 (global tunnel texture), 2×2 (construction joints, pipeline layout), and 4×4 (local blemishes, stains) scales. Visual descriptors are extracted through multi-fine-grained segmentation to enhance the recognizability of tunnel scenes.

[0116] Specifically, features were extracted from each image patch of tunnel images at three fine-grained scales. For the 1×1 image, a 512-bit descriptor was obtained, primarily representing the global texture of the tunnel wall. For the 2×2 scale, the original image was segmented at its length and width midpoints, and features were extracted from each of the four equal image patches to obtain four 512-bit descriptors, primarily representing the shield tunnel segment joints, pipeline layout, etc., during construction. Similarly, feature extraction was performed on the 4×4 scale image to obtain 16 512-bit descriptors, primarily representing localized spots and stains on the tunnel wall. Image similarity was described using Euclidean distance.

[0117] in, These represent the features of the currently queried tunnel image and the features of the map nodes, respectively.

[0118] Furthermore, semantic labels are extracted using a deep learning-based visual detection neural network to represent the construction progress in the map progress layer. At the same time, semantic information is injected into the visual representation to enhance the uniqueness of scene identification.

[0119] 3.2. Sequence matching model based on Viterbi algorithm.

[0120] To address the issue of highly repetitive visual scenes during tunnel reversal operations, this invention combines the Viterbi algorithm with the spatiotemporal continuity of multiple image sequences, integrating short-term motion constraints with multi-fine-grained visual feature matching to reduce the false recall rate of single-frame matching. This invention transforms the loop closure detection problem into sequence matching based on the Viterbi algorithm, specifically through tunnel side-view image observation sequences. Find the corresponding optimal hidden state sequence :

[0121] If the robot's initial state probability is The state transition probability is Launch probability Then when the observation sequence is At time t Probability of the best matching sequence at the end of the state It can be deduced that:

[0122] 3.2.1. Initial state probability.

[0123] Assuming the robot starts running from any point in the tunnel, the nearest map node to that point is... Then the state space The initial probability distribution is: And there are:

[0124] in, This indicates that the robot is moving from a map node in the tunnel. The probability of departure.

[0125] 3.2.2. State transition probability.

[0126] This invention utilizes a kinematic model The state transition probability is expressed as This indicates the state of the feature map from the first two time points. , Transition to the current state The probability is then determined by... This indicates that the robot starts from a map node. go through Transferred to The probability of state transitions between different map nodes in a tunnel needs to be considered in terms of both the robot's kinematic model and the tunnel's topological structure. The former reflects the state transition relationships between different map node positions, while the latter reflects the connectivity of the map nodes. A Gaussian model is used to model the state transition probabilities:

[0127] in, Represents map nodes The location can be determined according to and Current state prediction covariance This is used to adjust the state transition probabilities between different map nodes, and finally, all nodes that have reached the map node are considered. The state transition probabilities are normalized.

[0128] 3.2.3. Launch probability.

[0129] Emission probability in this invention Indicates a map node Obtaining tunnel side view images The probability of emission. As seen in the previous steps, by extracting multi-fine-grained feature descriptors to match observations and map nodes, the more similar the query image and the map image, the higher the emission probability. The emission probability is modeled as follows:

[0130] in, Indicates the current state and observation The Euclidean distance between the matched map node feature descriptors and the target map node feature descriptors. covariance Used to adjust the relationship between emission probabilities, in this invention it is set as follows: Finally, the emission probability at time t is... Normalize.

[0131] 3.2.4. Sequence matching solution.

[0132] Based on the above probability modeling, the Viterbi algorithm is used to recursively calculate and solve the problem. The optimal map matching sequence when the maximum probability is reached, with a threshold set to ensure sequence length. Map nodes with the highest joint probability. This refers to the map node that is closest to the tunnel image to be matched.

[0133] 3.3. Pose calculation based on visual feature point matching of tunnel wall.

[0134] Furthermore, by using the local feature matching results of the tunnel wall based on the SuperPoint algorithm, the EPnP algorithm is used to construct the 2D-3D feature point correspondence, thereby solving the relative pose relationship between the current position and the map node, and then calculating the robot's absolute pose in the global tunnel map. .

[0135] 4. Robot pose estimation and mapping based on dynamic causal relationship optimization domain graph model.

[0136] This invention uses a graphical model to fuse the odometry observation constraints in step 2 and the loop closure detection constraints in step 3, solving for the optimal robot pose estimation, and then transforming the point cloud to construct a 3D map of the tunnel scene. Combined with... Figure 7 This invention employs a factor graph model within a dynamic causal correlation optimization domain. The tunnel factor graph consists of multiple nodes and connected edges. Nodes represent the robot's pose within the tunnel, and edges represent error terms for each observation. By adjusting the node positions to satisfy the constraints formed by the edges, the total error is minimized, achieving a globally optimal solution.

[0137] 4.1. Construction of the pose factor graph model of the tunnel robot.

[0138] This invention addresses degraded tunnel scenarios by using factor graphs to transform the optimization problem into a problem of relationships between edges and nodes. The robot pose is set as the node state variable in the graph, and the pose estimation obtained through particle filtering is used as an odometry factor to constrain adjacent nodes. Sequence matching results are used as a closure factor to correct for accumulated positioning errors in long-distance tunnels.

[0139] 4.1.1. Odometer factor error function.

[0140] The error function corresponding to the odometer factor is:

[0141] in, and This refers to the state quantity at the corresponding time. This represents the pose change of the filtered localization results between two adjacent particles. Let be the covariance matrix of the observations.

[0142] 4.1.2. Loop Factor Error Function.

[0143] The error function corresponding to the cyclic factor is:

[0144] in, The pose estimate is obtained after sequence matching. This is the observation covariance of the loop.

[0145] 4.2. Solving the dynamic causal domain graph model for long tunnels.

[0146] Due to the long tunnel distance, the graph model accumulates observation data from different times over time, leading to a continuous increase in model size, which in turn increases computational complexity and memory consumption, affecting optimization efficiency. To address this issue, this invention employs a dynamic causal association domain mechanism to dynamically maintain data within a fixed range, ensuring the graph model size is controllable while significantly reducing optimization time. The association domain moves with the current frame, and the optimization objective function within the resulting optimization window is:

[0147] in, For odometer factor, It is a cyclic factor. This represents all nodes to be optimized within the sliding window. The robot's globally optimal pose is obtained through continuous iteration. .

[0148] 4.3. Construction of tunnel point cloud map based on graph optimization results.

[0149] Tunnel point cloud acquired by lidar The robot's localization results are transformed and converted to a point cloud in the global map coordinate system. And merge the point cloud into the global point cloud map. : Post-processing techniques such as voxel filtering are used to remove duplicate points and noise, reducing the volume of the point cloud map and making the 3D point cloud map of the tunnel smoother.

[0150] Furthermore, by recursively partitioning the tunnel point cloud map space using an octree, the storage requirements of the point cloud data are effectively reduced. The hierarchical structure of the octree facilitates fast querying, making neighborhood search more efficient during localization. Moreover, the tunnel point cloud map data can be dynamically updated, and the tree structure can be flexibly adjusted and reorganized.

[0151] By storing the information in an octree format, when the robot re-enters the tunnel scene, it can obtain prior information by querying the point cloud map, which can then serve as a reference benchmark for the robot's operation.

[0152] Furthermore, a dynamically updated tunnel construction map is constructed using a structure layer, a feature layer, a trajectory layer, and a progress layer. Specifically, the structure layer, i.e., 3D point cloud data, is used to represent the geometric structure of the tunnel environment; the feature layer stores multi-fine-grained visual features, which can be used for global relocalization and loop closure detection in spatiotemporal sequence matching; the trajectory layer records the robot's historical localization trajectory, providing motion constraints and mapping references; the progress layer contains key semantic information such as support structures and equipment installation nodes marked on the point cloud structure and visual images. It can not only mark and align installed equipment detected by LiDAR and cameras, but also mark the locations of equipment that needs to be installed in advance on the map progress layer. The construction progress is visualized by color coding: green (completed), yellow (in progress), and red (incomplete), which is used for construction progress management and can also be used to provide semantic localization constraints.

[0153] During tunnel construction, the robot will enter the tunnel multiple times to perform different tasks, and the construction progress will be updated accordingly. After each operation, the robot uses LiDAR and visual data to detect semantic changes and update the construction progress status on the map; combined with the construction plan database, it will mark incomplete and missed areas with warnings and adjust the robot's autonomously planned operation path.

[0154] Compared with the prior art, the present invention has the following technical advantages and beneficial effects: 1. Subway tunnel scenarios are characterized by long distances and simple paths. The pose calculations performed by the robot's own sensors will drift, and the measurement errors will accumulate over time. This invention constructs a path-assisted likelihood model, fully utilizing the design drawings' paths as constraints for the robot's localization during tunnel construction. It also uses unique landmark structures in the design drawings, such as connecting passages, as localization references to correct accumulated errors during localization and mapping, thus solving the drift problem in long-distance localization and effectively improving the accuracy of mapping and localization. In long-distance shield tunnels (distance > 5km), the accumulated localization error can be limited to the decimeter level. Even if the robot's onboard sensors completely fail, the path likelihood model can still output localization results based on the design path, ensuring the continuity of localization during construction and avoiding localization loss.

[0155] 2. In degraded tunnel scenarios, geometric and textural features are not prominent, and the scene repetition is extremely high, posing a significant challenge to scene recognition. The localization and mapping system of this invention employs a side-view camera arrangement to acquire images of the tunnel walls, thereby extracting more fine-grained visual features. Then, a visual sequence matching is constructed based on the Viterbi algorithm. By combining information from multiple frames, the spatiotemporal continuity of the matching is enhanced, avoiding loss of scene recognition accuracy due to local environmental changes. This helps recover location information and reduce uncertainty, better judging loop closure detection, and achieving more stable re-localization capabilities even in the event of localization loss.

[0156] 3. Existing feature-based LiDAR SLAM relies on feature tracking and often fails in degraded subway tunnel environments. This failure is primarily manifested in its rapid malfunction upon entering circular shield tunnel sections, where the odometer becomes stationary or even regresses. This invention proposes a robot localization and mapping method and system for subway tunnel scenarios. Based on a dynamic causal relational domain graph optimization framework, it integrates multi-sensor observation constraints and design path constraints from wheel speedometers, IMUs, LiDAR, and cameras through the combined effects of odometer factors and loop closure factors. This solves the localization and mapping challenges in degraded tunnel scenarios, reducing localization costs without relying on auxiliary localization facilities. It achieves accurate, fast, and robust localization and mapping based on multi-source information fusion, ensuring the stability of the robot localization system in degraded tunnel scenarios and providing a guarantee for subsequent operations. Dynamic management of multi-level maps, with real-time annotation of construction progress at the progress layer, allows the robot to adjust its operations based on the map, improving construction efficiency while maintaining operational accuracy.

[0157] To better implement the robot localization method in the degraded tunnel scenario in this invention embodiment, based on the robot localization method in the degraded tunnel scenario, correspondingly, as follows: Figure 8 As shown, this embodiment of the invention also provides a robot positioning device for a degraded tunnel scenario. The robot positioning device 800 for a degraded tunnel scenario includes: Module 801 is used to obtain prior path information and landmark structure location information from tunnel design drawings, construct a path likelihood model based on the prior path information, construct a relative distance constraint model based on the landmark structure location information, and construct a pose constraint model based on odometry observation data. The path likelihood model is used to constrain the robot's path in the tunnel, and the relative distance constraint model is used to constrain the relative distance between the robot and the landmark structure. The odometry observation data includes the observation data of the IMU and wheel speed meter set on the robot. The first determining module 802 is used to fuse the path likelihood model, the relative distance constraint model, and the pose constraint model based on the particle filter algorithm to obtain the odometry pose estimation result of the robot. The loop closure detection module 803 is used to extract multi-fine-grained features from the tunnel side view image captured by the camera set on the robot, perform loop closure detection based on the Viterbi algorithm and multi-fine-grained features, and obtain the loop closure detection pose estimation result of the robot. The second determining module 804 is used to determine the optimal pose estimate of the robot based on the odometry pose estimation results and the loop closure detection pose estimation results.

[0158] The robot positioning device 800 in the degraded tunnel scenario provided in the above embodiments can realize the technical solutions described in the robot positioning method embodiments in the degraded tunnel scenario. The specific implementation principles of each module or unit can be found in the corresponding content in the robot positioning method embodiments in the degraded tunnel scenario, and will not be repeated here.

[0159] like Figure 9 As shown, the present invention also provides a positioning device 900. The positioning device 900 includes a processor 901, a memory 902, and a display 903. Figure 9 Only some components of the positioning device 900 are shown, but it should be understood that it is not required to implement all of the components shown, and more or fewer components may be implemented instead.

[0160] In some embodiments, processor 901 may be a central processing unit (CPU), microprocessor, or other data processing chip, used to run program code stored in memory 902 or process data, such as the robot localization method in the degraded tunnel scenario of the present invention.

[0161] In some embodiments, processor 901 may be a single server or a group of servers. The server group may be centralized or distributed. In some embodiments, processor 901 may be local or remote. In some embodiments, processor 901 may be implemented on a cloud platform. In one embodiment, the cloud platform may include a private cloud, public cloud, hybrid cloud, community cloud, distributed cloud, internal cloud, multi-cloud, etc., or any combination thereof.

[0162] In some embodiments, memory 902 may be an internal storage unit of positioning device 900, such as a hard disk or memory of positioning device 900. In other embodiments, memory 902 may also be an external storage device of positioning device 900, such as a pluggable hard disk, smart media card (SMC), secure digital (SD) card, flash card, etc., provided on positioning device 900.

[0163] Furthermore, the memory 902 may include both internal storage units of the positioning device 900 and external storage devices. The memory 902 is used to store the application software and various types of data installed on the positioning device 900.

[0164] In some embodiments, display 903 may be an LED display, a liquid crystal display, a touch-sensitive liquid crystal display, or an organic light-emitting diode (OLED) touchscreen. Display 903 is used to display information from positioning device 900 and to display a visual user interface. Components 901-903 of positioning device 900 communicate with each other via a system bus.

[0165] In one embodiment, when processor 901 executes the robot localization program for the degenerate tunnel scenario stored in memory 902, the following steps can be implemented: Prior path information and landmark structure location information are obtained from tunnel design drawings. A path likelihood model is constructed based on the prior path information, a relative distance constraint model is constructed based on the landmark structure location information, and a pose constraint model is constructed based on odometry observation data. The path likelihood model is used to constrain the robot's path in the tunnel, and the relative distance constraint model is used to constrain the relative distance between the robot and the landmark structure. The odometry observation data includes the observation data of the IMU and wheel speed meter set on the robot. The odometry pose estimation results of the robot are obtained by fusing the path likelihood model, relative distance constraint model and pose constraint model based on the particle filter algorithm. Multi-fine-grained features are extracted from tunnel side view images captured by cameras mounted on the robot. Loop closure detection is performed based on the Viterbi algorithm and multi-fine-grained features to obtain the robot's loop closure detection pose estimation results. The optimal pose estimate of the robot is determined based on the odometry pose estimation results and the loop closure detection pose estimation results.

[0166] It should be understood that when the processor 901 executes the robot localization program in the degenerate tunnel scenario in the memory 902, in addition to the functions mentioned above, it can also perform other functions, as can be found in the description of the corresponding method embodiments above.

[0167] Furthermore, this embodiment of the invention does not specifically limit the type of positioning device 900 mentioned. Positioning device 900 can be a mobile phone, tablet computer, personal digital assistant (PDA), wearable device, laptop computer, or other portable electronic device. Exemplary embodiments of portable electronic devices include, but are not limited to, portable electronic devices running iOS, Android, Microsoft, or other operating systems. The aforementioned portable electronic device can also be other portable electronic devices, such as a laptop computer with a touch-sensitive surface (e.g., a touch panel). It should also be understood that in some other embodiments of the invention, positioning device 900 may not be a portable electronic device, but rather a desktop computer with a touch-sensitive surface (e.g., a touch panel).

[0168] Accordingly, this application also provides a computer-readable storage medium for storing a computer-readable program or instruction. When the program or instruction is executed by a processor, it can implement the steps or functions of the robot localization method in the degraded tunnel scenario provided in the above-described method embodiments.

[0169] Those skilled in the art will understand that all or part of the processes of the methods described in the above embodiments can be implemented by a computer program instructing related hardware (such as a processor, controller, etc.), and the computer program can be stored in a computer-readable storage medium. The computer-readable storage medium may be a disk, optical disk, read-only memory, or random access memory, etc.

[0170] The robot localization method and apparatus in degraded tunnel scenarios provided by the present invention have been described in detail above. Specific examples have been used to illustrate the principles and implementation methods of the present invention. The description of the above embodiments is only for the purpose of helping to understand the method and core ideas of the present invention. At the same time, for those skilled in the art, there will be changes in the specific implementation methods and application scope based on the ideas of the present invention. Therefore, the content of this specification should not be construed as a limitation of the present invention.

Claims

1. A robot localization method in a degraded tunnel scenario, characterized in that, include: Prior path information and landmark structure location information are obtained from tunnel design drawings. A path likelihood model is constructed based on the prior path information, a relative distance constraint model is constructed based on the landmark structure location information, and a pose constraint model is constructed based on odometry observation data. The path likelihood model is used to constrain the robot's path in the tunnel, and the relative distance constraint model is used to constrain the relative distance between the robot and the landmark structure. The odometry observation data includes the observation data of the IMU and wheel speed meter set on the robot. The odometry pose estimation results of the robot are obtained by fusing the path likelihood model, relative distance constraint model and pose constraint model based on the particle filter algorithm. Multi-fine-grained features are extracted from tunnel side view images captured by cameras mounted on the robot. Loop closure detection is performed based on the Viterbi algorithm and multi-fine-grained features to obtain the robot's loop closure detection pose estimation results. The optimal pose estimate of the robot is determined based on the odometry pose estimation results and the loop closure detection pose estimation results. The construction of the path likelihood model based on prior path information includes: The prior path information is uniformly sampled with a preset step size to obtain a first number of sampling points, and the Gaussian kernel function of the robot position at each sampling point is determined. A path likelihood model is constructed by linearly superimposing and normalizing the Gaussian kernel function of the robot's position at each sampling point; The construction of the relative distance constraint model based on the location information of landmark structures includes: Construct the linear equations corresponding to the location information of the landmark structures based on the location information of the landmark structures; A relative distance constraint model is constructed based on the linear equations corresponding to the location information of landmark structures and the robot's position. The loop closure detection based on the Viterbi algorithm and multi-fine-grained features yields the robot's loop closure pose estimation results, including: The observation sequence is constructed based on multiple fine-grained features, and the Viterbi algorithm is used to perform sequence matching on the observation sequence. The robot's loop closure detection pose estimation result is obtained based on the sequence matching result.

2. The robot localization method in a degraded tunnel scenario according to claim 1, characterized in that, The pose constraint model constructed based on odometry observation data includes: Determine the pose change between adjacent robot states based on odometry observation data; A pose constraint model is constructed based on the pose changes between adjacent robot states.

3. The robot localization method in a degraded tunnel scenario according to claim 1, characterized in that, The method based on particle filtering algorithm, which fuses path likelihood model, relative distance constraint model, and pose constraint model, yields the robot's odometry pose estimation results, including: The robot's pose at different times is treated as a set of particles, and a joint likelihood model is constructed based on the path likelihood model, the relative distance constraint model, and the pose constraint model. The weight of each particle in the particle set is updated based on the joint likelihood model, and the particle with the largest weight is determined as the odometry pose estimation result of the robot.

4. The robot localization method in a degraded tunnel scenario according to claim 1, characterized in that, The determination of the robot's optimal pose estimate based on odometry pose estimation results and loop closure detection pose estimation results includes: The robot's pose is used as the node state variable, the odometry pose estimation result is used as the odometry factor, and the loop closure detection pose estimation result is used as the loop closure factor. The optimal estimate of the robot's pose is determined based on the graph optimization algorithm.

5. A robot positioning device for degraded tunnel scenarios, characterized in that, include: The module is used to obtain prior path information and landmark structure location information from tunnel design drawings, build a path likelihood model based on the prior path information, build a relative distance constraint model based on the landmark structure location information, and build a pose constraint model based on odometry observation data. The path likelihood model is used to constrain the robot's path in the tunnel, and the relative distance constraint model is used to constrain the relative distance between the robot and the landmark structure. The odometry observation data includes the observation data of the IMU and wheel speed meter set on the robot. The first determining module is used to fuse the path likelihood model, the relative distance constraint model, and the pose constraint model based on the particle filter algorithm to obtain the odometry pose estimation result of the robot. The loop closure detection module is used to extract multi-fine-grained features from the tunnel side view images captured by the camera set on the robot, and to perform loop closure detection based on the Viterbi algorithm and multi-fine-grained features to obtain the loop closure detection pose estimation results of the robot. The second determining module is used to determine the optimal pose estimate of the robot based on the odometry pose estimation results and the loop closure detection pose estimation results. The construction of the path likelihood model based on prior path information includes: The prior path information is uniformly sampled with a preset step size to obtain a first number of sampling points, and the Gaussian kernel function of the robot position at each sampling point is determined. A path likelihood model is constructed by linearly superimposing and normalizing the Gaussian kernel function of the robot's position at each sampling point; The construction of the relative distance constraint model based on the location information of landmark structures includes: Construct the linear equations corresponding to the location information of the landmark structures based on the location information of the landmark structures; A relative distance constraint model is constructed based on the linear equations corresponding to the location information of landmark structures and the robot's position. The loop closure detection based on the Viterbi algorithm and multi-fine-grained features yields the robot's loop closure pose estimation results, including: The observation sequence is constructed based on multiple fine-grained features, and the Viterbi algorithm is used to perform sequence matching on the observation sequence. The robot's loop closure detection pose estimation result is obtained based on the sequence matching result.

6. A positioning device, characterized in that, Including memory and processor, among which, The memory is used to store programs; The processor, coupled to the memory, is used to execute the program stored in the memory to implement the steps in the robot localization method in a degraded tunnel scenario as described in any one of claims 1 to 4.

7. A computer-readable storage medium, characterized in that, Used to store computer-readable programs or instructions, which, when executed by a processor, can implement the steps in the robot localization method in a degraded tunnel scenario as described in any one of claims 1 to 4.

Images