A navigation and positioning method for underground mine boring machine based on graph optimization algorithm

Abstract

The present invention discloses a navigation and positioning method for an underground mine boring machine based on a graph optimization algorithm. The positioning method comprises the following steps: S1, calibrating the zero bias of the inertial measurement unit, calculating the relative position between the inertial measurement unit and the ultra-wideband ranging sensor, recording the calibration parameters and compensating. The present invention is based on navigation modeling and solving under the framework of a factor graph model, which can handle the dynamic addition and deletion of observation information. Different observation constraints are encoded as factors and added to the factor graph to be connected with state variables. The system is more flexible and reliable. The UWB anchor point is automatically dropped during travel to form an automatic network, which reduces the danger of mine operations. The tight coupling of dual UWB ranging and dead reckoning is adopted to constrain and calibrate the heading angle. In the mine, the coal dust concentration is extremely high and the characteristic texture is not obvious. The ultra-wideband has a strong penetration ability, is not affected by the environment, and the error does not diverge over time, which solves the problem of low precision of traditional methods in underground environments.

Description

Technical Field

[0001] The present invention relates to the technical field of mine excavation, and in particular to a navigation and positioning method for an underground mine boring machine based on a graph optimization algorithm. Background Art

[0002] In the future, underground coal mining will gradually develop towards unmanned operation, and the basis for realizing unmanned underground operation is that the excavation equipment has high-precision autonomous navigation and positioning capabilities. At present, my country still needs a lot of manual work in the field of coal mine tunneling, and coal mine accidents occur frequently. In order to gradually reduce manual operation and reduce the danger of operation, it is urgent to improve the level of unmanned and intelligent mine operation.

[0003] Deficiencies of existing technology:

[0004] The existing ground autonomous navigation solutions using laser radar, vision, etc. will no longer be applicable in underground mine environments. The main reason is that there is a lot of dust in the underground environment during coal mining, which affects the work of the laser radar, and the GPS in the underground environment will also fail. Among the existing positioning methods for underground mine equipment, the method using pure inertial navigation is difficult to meet the needs of high-precision operation for a long time. The error of inertial devices will diverge over time and cannot maintain long-term high-precision operation. The method using radio navigation requires manual laying of base stations and reference points, and the addition of an artificial working environment in the mine increases the risk. In terms of positioning algorithms, the Kalman filter method used has large linearization errors, ignores historical constraint information, and has limitations in the flexible addition and deletion of nodes, and there is still room for improvement in positioning accuracy. Based on the above background, there is an urgent need for an autonomous and high-precision underground mine boring machine navigation and positioning method for underground mine operations. The present invention provides a navigation and positioning method for an underground mine boring machine based on a graph optimization algorithm. Summary of the invention

[0005] The object of the present invention is to provide a navigation and positioning method for an underground mine boring machine based on a graph optimization algorithm to solve the problems raised in the above-mentioned background technology.

[0006] A navigation and positioning method for an underground mine boring machine based on a graph optimization algorithm, the positioning method comprising the following steps:

[0007] S1. Calibrate the zero bias of the inertial measurement unit (IMU), calculate the relative position between the IMU and the ultra-wideband ranging sensor (Ultra Wide Band), record the calibration parameters and compensate;

[0008] S2. When the roadheader is moving, the inertial measurement unit and wheel speed sensor carried by the vehicle body are used to perform preliminary navigation and positioning of the roadheader with a dead reckoning algorithm. The vehicle speed observation is obtained through the wheel speed sensor, and the angular rate information is obtained through the gyroscope in the inertial measurement unit. The position information of the roadheader can be calculated in real time based on the speed and angular rate information;

[0009] S3. During the travel of the roadheader, a pair of ultra-wideband ranging anchor points are placed at a certain distance on both sides of the road. The position coordinates of the anchor points are recorded as (X1t, Y1t) and (X2t, Y2t) respectively. Each ultra-wideband ranging anchor point can measure the distance with other ultra-wideband ranging anchor points and the ultra-wideband receiver on the roadheader to obtain the relative distance information between the two points.

[0010] S4. Construct a factor graph model, which includes stateful nodes and factors. Optimize and solve the established factor graph model, use the dead reckoning result as the initial optimization value, and use the LM algorithm for nonlinear iterative optimization until the global error function no longer decreases, thereby obtaining the optimized navigation parameters (X, Y, A);

[0011] S5, real-time solution, repeating step S4, and constantly updating the navigation parameters at the current moment, thereby realizing the navigation and positioning function of the tunnel boring machine in the mine.

[0012] As a further improvement of the present invention, in step S1 of the method, the initial heading angle A0 is obtained by using the inertial measurement unit at the starting point, and the starting coordinates in the two-dimensional coordinate system are recorded, the starting point is taken as the origin, the X-axis is taken as the initial direction of travel, and the starting point coordinates are recorded as (X0, Y0, A0);

[0013] Among them, X0, Y0 are the horizontal and vertical coordinate values, in meters, and A0 is the heading angle, in degrees.

[0014] As a further improvement of the present invention, the tunnel boring machine described in step S2 of the method needs to simultaneously measure the distance between two ultra-wideband anchor points placed on both sides of the road. The distance between the two anchor points can be obtained through wireless ranging between the anchor points. The distance observation information provided by a single ultra-wideband is used to effectively constrain the heading angle error of the tunnel boring machine and calibrate the heading error.

[0015] As a further improvement of the present invention, the state quantity of the state node is the variable to be optimized and solved, the position coordinates X, Y of the tunnel boring machine, the heading angle A, and the mathematical form of each factor is a constraint residual equation f(X1, X1, ... X2) containing the state quantity. n ).

[0016] As a further improvement of the present invention, the state quantity of the dead reckoning model is (X, Y, A), and the model is:

[0017] Xt =X t-1 +V t TxD

[0018] Y t =Y t-1 +V t TsinA

[0019] Among them, X t is the horizontal coordinate value at time t, Y t is the ordinate value at time t, A is the heading angle at time t, T is the wheel speed meter sampling interval, and V is the speed observation.

[0020] As a further improvement of the present invention, the observation equation of the ultra-wideband ranging sensor ranging information is:

[0021]

[0022]

[0023] Among them, D1 describes the distance equation between the position of the tunnel boring machine at time t and the first anchor point in a set of anchor points, and D2 describes the distance equation between the position of the tunnel boring machine at time t and the second anchor point in a set of anchor points. It is used to construct the error between the theoretical distance calculated from the current position of the tunnel boring machine and the position of the anchor point and the actual distance measured by the ultra-wideband. T is the wheel speed meter sampling interval, and V is the speed observation.

[0024] As a further improvement of the present invention, the mathematical form corresponding to the factor graph model is a plurality of residual equations, and a nonlinear optimization algorithm is used to perform global optimization on the navigation parameters. The specific model is as follows:

[0025]

[0026]

[0027] Wherein, d1 is the distance measurement value between the first anchor point and the ultra-wideband ranging sensor on the tunnel boring machine, and d2 is the distance measurement value between the second anchor point and the ultra-wideband ranging sensor on the tunnel boring machine. is the constraint residual equation between the roadheader and the first anchor point of a pair of placed anchor points, It is the constraint residual equation between the tunnel boring machine and the second anchor point in a pair of placed anchor points, representing the error between the theoretical distance calculated from the current position of the tunnel boring machine and the position between the anchor points at time t and the actual distance measured by the ultra-wideband. This error is used to correct the current position of the tunnel boring machine.

[0028] As a further improvement of the present invention, the residual equation is derived to guide the direction of graph optimization, and the Jacobian matrix obtained by the derivation is:

[0029]

[0030] Based on the above formula, nonlinear optimization is performed, with the dead reckoning coordinates (X, Y, A) as the initial optimization value, and the Jacobian matrix as the gradient. The residual terms generated at all times are summed to obtain the system residual equation of the entire factor graph model. The specific model is as follows:

[0031]

[0032] Among them, Σ i covariance of the relative distances measured for the UWB base stations;

[0033] Based on the residual function e(x) calculated above, the sum of the residuals is reduced to a minimum value through iterative optimization, completing the nonlinear optimization process, and calculating the state update amount Δx according to the formula:

[0034] e(x+Δx)≈e(x)+J(x) T Δx

[0035]

[0036] Among them, J(x) T is the transpose of the Jacobian matrix calculated in the above steps, Δx is the update amount of the state variables at each optimization iteration, which is used to calibrate the state of the roadheader with errors;

[0037] The state variables (X, Y, A) are continuously updated: X = X + Δx, based on the current position variable X obtained from the optimization results t , Y t The position variable X at the previous moment t-1 , Y t-1 Update the heading angle:

[0038]

[0039] As a further improvement of the present invention, each state update in step S4 of the method will cause the residual function e(x) to continuously decrease. After multiple iterations, when the residual function no longer decreases or the amplitude of each decrease is less than a threshold, the algorithm stops and determines that it has converged to the optimal value.

[0040] As a further improvement of the present invention, in the method, the tunnel boring machine is equipped with multiple ultra-wideband ranging anchor points, ultra-wideband receivers, inertial measurement units and wheel speed meters that can self-network. The ultra-wideband ranging anchor points are designed as anchor balls that can be dropped at any time as transmitters. Every two of the ultra-wideband ranging anchor points form a pair and are dropped on both sides of the path at the same time to form a local network. The ultra-wideband receiver, inertial measurement unit and wheel speed meter are installed on the tunnel boring machine.

[0041] Compared with the prior art, the present invention has the following beneficial effects:

[0042] 1. Compared with the filtering method, the present invention includes the observation information generated at the historical moment, and uses more information than the filtering algorithm. Since the graph optimization algorithm performs multiple iterations at a linearization point based on the principle of gradient descent, the linearization error is smaller than that of a single iteration of the filtering algorithm, and thus the accuracy is higher;

[0043] 2. The navigation modeling and solving based on the factor graph model framework of the present invention can handle the dynamic addition and deletion of observation information. Different observation constraints are encoded as factors and added to the factor graph to be connected with the state variables, making the system more flexible and reliable.

[0044] 3. The present invention automatically drops the anchor point of the ultra-wideband ranging sensor during the process to form an automatic network, thus avoiding the manual laying of base stations and reducing the danger of mine operations;

[0045] 4. The coal dust concentration in the mine is extremely high and the characteristic texture is not obvious. The traditional navigation and positioning solutions such as lidar and vision are seriously affected by the present invention. However, ultra-wideband has extremely strong penetration ability, is not affected by the environment, and the error does not diverge over time, which solves the problem of low accuracy of traditional methods in underground environments. BRIEF DESCRIPTION OF THE DRAWINGS

[0046] Figure 1 It is a flow chart of a navigation and positioning method for an underground mine boring machine based on a graph optimization algorithm of the present invention;

[0047] Figure 2 It is a system overall schematic diagram of an underground mine boring machine navigation and positioning method based on a graph optimization algorithm of the present invention;

[0048] Figure 3 A flowchart of a graph optimization algorithm for a navigation and positioning method for an underground mine boring machine based on a graph optimization algorithm according to the present invention;

[0049] Figure 4 It is a schematic diagram of factor graph modeling of a navigation and positioning method for an underground mine boring machine based on a graph optimization algorithm of the present invention. DETAILED DESCRIPTION

[0050] The following will be combined with the drawings in the embodiments of the present invention to clearly and completely describe the technical solutions in the embodiments of the present invention. Obviously, the described embodiments are only part of the embodiments of the present invention, not all of the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by ordinary technicians in this field without creative work are within the scope of protection of the present invention.

[0051] Example

[0052] See also Figure 1-4 The present invention provides the following technical solutions: a navigation and positioning method for an underground mine boring machine based on a graph optimization algorithm, wherein the boring machine is equipped with a plurality of ultra-wideband ranging anchor points, an ultra-wideband receiver, an inertial measurement unit and a wheel speed meter that can form a self-network, and the ultra-wideband ranging anchor point is designed as an anchor ball that can be thrown down at any time as a transmitter, and every two of the ultra-wideband ranging anchor points form a pair, which are thrown on both sides of the path at the same time to form a local network (where A1 and A2 are a group of UWB transmitters that are closest to each other and are thrown on both sides of the road respectively in order not to affect the vehicle's travel), and the ultra-wideband receiver, the inertial measurement unit and the wheel speed meter are installed on the boring machine;

[0053] S1. Calibrate the zero bias of the inertial measurement unit, calculate the relative position between the inertial measurement unit and the UWB receiver, record the calibration parameters and compensate them, use the magnetometer at the starting point to obtain the initial heading angle A0, and record the starting coordinates in the two-dimensional coordinate system, take the starting point as the origin, take the X-axis as the initial direction of travel, and record the starting point coordinates as (X0, Y0, A0), where X0, Y0 are the horizontal and vertical coordinate values, and A0 is the heading angle;

[0054] S2. When the roadheader is moving, the inertial measurement unit and wheel speed sensor carried by the vehicle body are used to perform preliminary navigation and positioning of the roadheader with a dead reckoning algorithm. The vehicle speed observation is obtained through the wheel speed sensor, and the angular rate information is obtained through the gyroscope in the inertial measurement unit. The coordinates at the initial moment are set to 0, and the position information of the vehicle can be calculated in real time based on the speed and angular rate information;

[0055] The tail of the tunnel boring machine is equipped with ten sets of anchor balls that can work independently of the Ultra Wide Band sensor. During the process of traveling, whenever the tunnel boring machine turns or the running distance exceeds the effective communication distance of the Ultra Wide Band sensor, the anchor balls are dropped on both sides of the road to form a new local network, and no longer receive longer anchor point ranging information, so as to ensure that the tunnel boring machine is always within the communication range of the Ultra Wide Band sensor. When dropping, the navigation positioning coordinates of the tunnel boring machine at this moment are recorded as the position coordinates of the anchor points, which are recorded as (X1t, Y1t) and (X2t, Y2t) respectively.

[0056] S3. Start the TBM. During the forward movement, the TBM is initially navigated and positioned using the dead reckoning algorithm. The position parameters of the TBM at the current moment are calculated in real time according to the formula. The angle offsets in the X, Y, and Z directions can be obtained by integrating the angular velocity information provided by the gyroscope over time. The pitch angle and the roll angle can be aligned by gravity. The heading angle is obtained by iterative calculation based on the initial value. The equation is as follows:

[0057] A t =A t-1 +w t T

[0058] X t =X t-1 +V t TxD t

[0059] Y t =Y t-1 +V t TsinA t

[0060] Where T is the wheel speed meter sampling interval, V is the speed observation, and w t is the gyroscope angular velocity;

[0061] The observation equation of the ultra-wideband ranging sensor ranging information is:

[0062]

[0063]

[0064] Among them, D1 describes the distance equation between the position of the tunnel boring machine at time t and the first anchor point in a set of anchor points, and D2 describes the distance equation between the position of the tunnel boring machine at time t and the second anchor point in a set of anchor points. It is used to construct the error between the theoretical distance calculated from the current position of the tunnel boring machine and the position of the anchor point and the actual distance measured by the ultra-wideband. T is the wheel speed meter sampling interval, and V is the speed observation.

[0065] S4. Receive the distance observation information of the two anchor points relative to the tunnel boring machine, add the generated observation constraints to the optimization algorithm, optimize the navigation parameters, use the dead reckoning results as the initial optimization value, use the LM algorithm for nonlinear iterative optimization, and use the UWB ranging information to provide the optimization gradient until the global error function no longer decreases. Add a robust kernel function to the UWB observation factor to deal with possible communication failures. When an erroneous distance observation such as communication interruption occurs, the corresponding weight will automatically decrease during the optimization process;

[0066] For the graph optimization algorithm, the observation data generated by the navigation and positioning system is constructed into a factor graph model, such as Figure 3 , the navigation parameters are globally optimized using a nonlinear optimization algorithm, and the residual equation is constructed as:

[0067]

[0068]

[0069] Wherein, d1 is the distance measurement value between the first anchor point and the ultra-wideband ranging sensor on the tunnel boring machine, and d2 is the distance measurement value between the second anchor point and the ultra-wideband ranging sensor on the tunnel boring machine. is the constraint residual equation between the roadheader and the first anchor point of a pair of placed anchor points, is the constraint residual equation between the tunnel boring machine and the second anchor point in a pair of placed anchor points, representing the error between the theoretical distance calculated from the current position of the tunnel boring machine and the position between the anchor points at time t and the actual distance measured by the ultra-wideband. This error is used to correct the current position of the tunnel boring machine. T is the wheel speed meter sampling interval, and V is the speed observation obtained by the wheel speed meter.

[0070] The Jacobian matrix is:

[0071]

[0072] according to Figure 2 Process, perform nonlinear optimization, use the dead reckoning coordinates (X, Y, A) as the initial optimization value, use the Jacobian matrix as the gradient, and calculate the system residual function e(x) according to the constraint relationship:

[0073]

[0074] Among them, t is the current time, i is any time from the start of the algorithm to the current time t, Σ i The covariance of the relative distance measured by the ultra-wideband base station, the specific value of which depends on the measurement error value of the actual device parameters.

[0075] Through iterative optimization, the sum of the residuals is reduced to a minimum value, the nonlinear optimization process is completed, and the state update amount Δx is calculated according to the formula:

[0076] e(x+Δx)≈e(x)+J(x) T Δx

[0077]

[0078] Among them, J(x) Tis the transpose of the Jacobian matrix calculated in the above steps, and Δx is the update amount of the state variables in each optimization iteration, which is used to calibrate the state of the tunnel boring machine with errors.

[0079] During this period, the state quantity (X, Y, A) is continuously updated: X = X + Δx, and the heading angle is updated:

[0080] In the present invention, navigation modeling and solving based on the factor graph model framework can handle the dynamic addition and deletion of observation information. Different observation constraints are encoded as factors and added to the factor graph to be connected with state variables. The system is more flexible and reliable. Ultra-wideband ranging sensors (Ultra Wide Band) anchor points are automatically dropped during travel to form automatic networking, avoiding the manual laying of base stations and reducing the danger of mine operations. The tight coupling of dual ultra-wideband ranging sensors (Ultra Wide Band) ranging and dead reckoning is adopted. Compared with the traditional dead reckoning method, the heading angle can be constrained and calibrated. The coal dust concentration in the mine is extremely high and the characteristic texture is not obvious. Traditional navigation and positioning schemes such as laser radar and vision are seriously affected. Ultra-wideband has extremely strong penetration ability, is not affected by the environment, and the error does not diverge over time, which solves the problem of low precision of traditional methods in underground environments. Compared with the filtering method, the graph optimization algorithm includes the observation information generated at historical moments, and uses more information than the filtering algorithm. Because the graph optimization algorithm performs multiple iterations at a linearization point based on the principle of gradient descent, the linearization error is smaller than that of a single iteration of the filtering algorithm, so the precision is higher.

[0081] It should be noted that, in this article, relational terms such as first and second, etc. are only used to distinguish one entity or operation from another entity or operation, and do not necessarily require or imply any such actual relationship or order between these entities or operations. Moreover, the terms "include", "comprise" or any other variants thereof are intended to cover non-exclusive inclusion, so that a process, method, article or device including a series of elements includes not only those elements, but also other elements not explicitly listed, or also includes elements inherent to such process, method, article or device.

[0082] Although embodiments of the present invention have been shown and described, it will be appreciated by those skilled in the art that various changes, modifications, substitutions and variations may be made to the embodiments without departing from the principles and spirit of the present invention, and that the scope of the present invention is defined by the appended claims and their equivalents.

Claims

1. A navigation and positioning method for underground mine boring machine based on graph optimization algorithm, characterized in that: This positioning method includes the following steps: S1. Calibrate the zero bias of the inertial measurement unit, calculate the relative position between the inertial measurement unit and the ultra-wideband ranging sensor, record the calibration parameters and compensate; S2. When the roadheader is moving, the inertial measurement unit and wheel speed sensor carried by the vehicle body are used to perform preliminary navigation and positioning of the roadheader with a dead reckoning algorithm. The vehicle speed observation is obtained through the wheel speed sensor, and the angular rate information is obtained through the gyroscope in the inertial measurement unit. The position information of the roadheader can be calculated in real time based on the speed and angular rate information; S3. During the roadheader's travel, a pair of ultra-wideband ranging anchor points are placed at a certain distance on both sides of the road. The position coordinates of the anchor points are recorded as (X 1t , Y 1t )、(X 2t , Y 2t ), each UWB ranging anchor point can measure distance with other UWB ranging anchor points and the UWB receiver on the tunnel boring machine to obtain the relative distance information between the two points; S4. Construct a factor graph model, which includes stateful nodes and factors. Optimize and solve the established factor graph model, use the dead reckoning result as the initial optimization value, and use the LM algorithm for nonlinear iterative optimization until the global error function no longer decreases, thereby obtaining the optimized navigation parameters (X, Y, A); S5, real-time solution, repeating the step S4, and constantly updating the navigation parameters at the current moment, thereby realizing the navigation and positioning function of the tunnel boring machine in the mine; In step S1 of the method, the initial heading angle A0 is obtained by using the inertial measurement unit at the starting point, and the starting coordinates in the two-dimensional coordinate system are recorded, the starting point is taken as the origin, the X axis is taken as the initial direction of travel, and the starting point coordinates are recorded as (X0, Y0, A0); Among them, X0, Y0 are the horizontal and vertical coordinate values, in meters, and A0 is the heading angle, in degrees; The state of the dead reckoning model is (X, Y, A), and the model is: X t =X t-1 +V t TcosA Y t =Y t-1 +V t TsinA Among them, X t is the horizontal coordinate value at time t, Y t is the ordinate value at time t, A is the heading angle at time t, T is the wheel speed meter sampling interval, V t is the observed velocity value at time t; The observation equation of the ultra-wideband ranging sensor ranging information is: Wherein, D1 describes the distance equation between the position of the tunnel boring machine at time t and the first anchor point in a set of anchor points, and D2 describes the distance equation between the position of the tunnel boring machine at time t and the second anchor point in a set of anchor points, which are used to construct the error between the theoretical distance calculated between the current position of the tunnel boring machine and the position of the anchor point and the actual distance measured by the ultra-wideband. The mathematical form corresponding to the factor graph model is a plurality of residual equations, and a nonlinear optimization algorithm is used to perform global optimization on the navigation parameters. The specific model is as follows: Wherein, d1 is the distance measurement value between the first anchor point and the ultra-wideband ranging sensor on the tunnel boring machine, and d2 is the distance measurement value between the second anchor point and the ultra-wideband ranging sensor on the tunnel boring machine. is the constraint residual equation between the tunnel boring machine and the first anchor point of a pair of anchor points placed at time t, is the constraint residual equation between the tunnel boring machine and the second anchor point of a pair of anchor points at time t, representing the error between the theoretical distance calculated between the current position of the tunnel boring machine and the position between the anchor points at time t and the actual distance measured by the ultra-wideband. This error is used to correct the current position of the tunnel boring machine; The residual equation is derived to guide the direction of graph optimization, and the Jacobian matrix obtained by the derivation is: Based on the above formula, nonlinear optimization is performed, with the dead reckoning coordinates (X, Y, A) as the initial optimization value, and the Jacobian matrix as the gradient. The residual terms generated at all times are summed to obtain the system residual equation of the entire factor graph model. The specific model is as follows: Among them, Σ i covariance of the relative distances measured for the UWB base stations; is the constraint residual equation between the tunnel boring machine and the first anchor point of a pair of anchor points placed at time i, is the constraint residual equation between the tunnel boring machine and the second anchor point in a pair of anchor points placed at time i; Based on the residual function e(x) calculated above, the sum of the residuals is reduced to a minimum value through iterative optimization, completing the nonlinear optimization process, and calculating the state update amount Δx according to the formula: e(x+Δx)≈e(x)+J(x) T Δx Among them, J(x) T is the transpose of the Jacobian matrix calculated in the above steps, Δx is the update amount of the state variables at each optimization iteration, which is used to calibrate the state of the roadheader with errors; The state variables (X, Y, A) are continuously updated: X = X + Δx, based on the current position variable X obtained from the optimization results t , Y t The position variable X at the previous moment t-1 , Y t-1 Update the heading angle:

2. The underground mine boring machine navigation and positioning method based on graph optimization algorithm according to claim 1, characterized in that: The tunnel boring machine described in step S2 needs to simultaneously measure the distance between two ultra-wideband anchor points placed on both sides of the road. The distance between the two anchor points can be obtained through wireless ranging between the anchor points. The distance observation information provided by a single ultra-wideband is used to effectively constrain the heading angle error of the tunnel boring machine and calibrate the heading error.

3. The underground mine boring machine navigation and positioning method based on graph optimization algorithm according to claim 1, characterized in that: Each state update in step S4 will cause the residual function e(x) to decrease continuously. After multiple iterations, when the residual function no longer decreases or the amplitude of each decrease is less than the threshold, the algorithm stops and determines that it has converged to the optimal value.

4. The underground mine boring machine navigation and positioning method based on graph optimization algorithm according to claim 1, characterized in that: The tunnel boring machine is equipped with multiple ultra-wideband ranging anchor points, ultra-wideband receivers, inertial measurement units and wheel speed meters that can self-network. The ultra-wideband ranging anchor points are designed as anchor balls that can be dropped at any time as transmitters. Every two ultra-wideband ranging anchor points form a pair and are dropped on both sides of the path at the same time to form a local network. The ultra-wideband receiver, inertial measurement unit and wheel speed meter are installed on the tunnel boring machine.

Images