A downhole mobile equipment positioning method based on ultra-wideband and inertial unit fusion

Abstract

The application discloses a kind of based on ultra-wide band and inertial unit fusion underground mobile equipment positioning method, first six-way inertial unit data are fused and solved: by attitude transformation matrix, the three-axis angular velocity and three-axis proportional acceleration of six-way inertial unit are all converted into the parameters under the coordinate system of drilling machine, and data fusion is carried out to obtain fusion proportional acceleration, fusion angular velocity, and the pose information of drilling machine is solved;Secondly, the fusion acceleration obtained by inertial unit is fused with fusion angular velocity and the three-dimensional position of ultra-wide band, and the factor graph model of IMU / UWB is established, and the minimum error function is obtained;Finally, the optimal value is found by Harris eagle optimization algorithm, and then the three-dimensional coordinates of drilling machine are obtained;The application not only avoids the positioning information jump caused by single UWB positioning, effectively converges the positioning error of UWB, and avoids the cumulative error caused by single IMU positioning, but also can flexibly combine different measurement frequency sensors to realize the function of plug and play.

Description

Technical Field

[0001] The present invention relates to unmanned positioning technology, belongs to the field of underground coal mines, and particularly relates to an underground mobile equipment positioning method based on the fusion of ultra-wideband and inertial units. Background Art

[0002] Rock burst refers to the phenomenon in which rock stress is redistributed during coal mining due to tunnel excavation or coal extraction at the working face. When the stress within the coal reaches its limit, the energy in the coal, roof, and floor is released to form stress waves, causing the rock to burst and eject. To prevent rock burst from causing damage to the mine and even casualties, the most effective prevention and control method is to drill holes in the tunnel or working face to relieve the pressure.

[0003] Existing drilling equipment and corresponding systems are becoming increasingly unmanned and intelligent, effectively solving the problems of high risk and low efficiency in tunnel excavation under complex conditions. For example, anti-blow drilling robots, which have a series of actions such as movement and drilling, can drill and relieve pressure in tunnels or working surfaces.

[0004] Unmanned positioning is a necessary prerequisite for the anti-collision drilling robot to perform a series of actions and is also an important factor in the effectiveness of drilling pressure relief. Currently, the unmanned positioning of the anti-collision drilling robot mainly includes ultra-wideband (UWB) positioning and inertial unit (IMU) positioning. UWB positioning is performed through ultra-wideband wireless communication, that is, the distance between the two devices is calculated using the transmission time of sending and receiving, and the position information of the target to be located is obtained by geometric positioning method combined with angle information. However, it has disadvantages such as positioning information jump and lack of process information during the positioning process. IMU positioning mainly uses gyroscopes and accelerometers to measure attitude angles and accelerations for positioning, but starting from the initial alignment, its error will increase over time, that is, it will have the disadvantage of large cumulative error. In addition, some commonly used UWB / IMU fusion methods cannot achieve the expected effect when one sensor fails. Summary of the Invention

[0005] The purpose of the present invention is to provide a positioning method for underground mobile equipment based on the fusion of ultra-wideband and inertial units. It not only avoids the positioning information jump caused by single UWB positioning, effectively converges the positioning error of UWB, and avoids the cumulative error caused by single IMU positioning, but also can flexibly combine sensors with different measurement frequencies to achieve plug-and-play functionality.

[0006] To achieve the above-mentioned purpose, the present invention provides a positioning method for underground mobile equipment based on the fusion of ultra-wideband and inertial unit, which is characterized by comprising the following steps:

[0007] Step 1: Fuse and solve the six-way inertial unit data:

[0008] The three-axis angular velocity and three-axis proportional acceleration of the six inertial units are converted into parameters in the drilling rig coordinate system through the attitude transformation matrix. Then, data fusion is performed to obtain the fused proportional acceleration and fused angular velocity, and the position information of the drilling rig is obtained by solution.

[0009] Step 2: Integrate the fused acceleration and fused angular velocity obtained by the inertial unit in step 1, as well as the three-dimensional position of the ultra-wideband into the factor graph, establish the factor graph model of IMU / UWB, and obtain the minimum error function;

[0010] Step three: Use the error function in step two as the objective function to find the optimal value through the Harris Eagle optimization algorithm to obtain the three-dimensional coordinates of the drilling rig.

[0011] Furthermore, the six inertial units in step 1 are arranged in a regular tetrahedron space, and are located at the five connection points and the center of the regular tetrahedron;

[0012] The z-axis of the inertial unit at the center of the regular tetrahedron points to the upper vertex of the regular tetrahedron, and the y-axis points forward. The z-axis of the inertial unit at the upper vertex points to the bottom surface, and the y-axis points forward. The four inertial units at the bottom are distributed in a differential structure at the four vertices of the bottom surface of the regular tetrahedron, and the z-axes of two adjacent inertial units point in opposite directions, and the y-axes all point to the center point of the bottom surface; the x-axes of the six inertial units all obey the right-hand coordinate system principle.

[0013] Furthermore, the angular velocity attitude transformation matrix is ​​expressed as:

[0014]

[0015] Among them, ω ibx 、ω iby 、ω ibz They represent the three-axis angular velocity corresponding to position i in the drilling rig coordinate system, γ i ,θ i 、 They represent the three-axis rotation angles of inertial unit i relative to the drilling rig coordinate system, ω ix 、ω iy 、ω iz Respectively represent the three-axis angular velocity measured by the inertial unit;

[0016] The proportional acceleration attitude transformation matrix is ​​expressed as:

[0017]

[0018] Where: f ibx 、f iby 、f ibzThey represent the three-axis proportional acceleration corresponding to position i in the drilling rig coordinate system, γ i ,θ i 、 They represent the rotation angles of the inertial unit i around the three axes relative to the drilling rig coordinate system, f ix 、f iy 、f iz They represent the three-axis proportional accelerations measured by the inertial unit respectively;

[0019] The fused angular velocity is expressed as:

[0020]

[0021] The fused proportional acceleration is expressed as:

[0022]

[0023] Furthermore, in step 2, the minimum error function is:

[0024] argmin x F(x 1:n )

[0025]

[0026] Among them, f prior is the prior factor node;

[0027] f bias (α k+1 ,α k ) is the bias factor node related to the IMU inertial device;

[0028] f IMU (x k+1 ,x k ,α k ) is the factor node of IMU at time k;

[0029] f UWB (x k ) is the factor node of UWB at time k.

[0030] Furthermore, the error function corresponding to the prior factor node is:

[0031]

[0032] Where μ = {X1, V1, α1} represents the initial state of the vehicle, X1, V1, and α1 represent the initial estimates of the three-dimensional position, velocity, and three-axis angle of the anti-collision drilling robot in the corresponding state, respectively, and Σ Prior The error covariance matrix representing the initial state of the anti-collision drilling robot;

[0033] represents the prior information available at the initial moment, and They represent the initial estimation of the three-dimensional position, velocity and three-axis angle of the anti-collision drilling robot respectively.

[0034] Furthermore, the factor node formula of the IMU at time k is:

[0035]

[0036] Among them, x k is the navigation state at the current time k, x k+1 is the navigation state at the next moment;

[0037] α k is the error variable generated by the IMU inertial device at time k;

[0038] d() represents the error function connecting the upper and lower adjacent states;

[0039] is the IMU measurement information, and its formula is:

[0040]

[0041] f b is the fusion proportional acceleration at the current k moment; w b is the fused angular rate at the current k moment.

[0042] Furthermore, the factor node of the UWB at time k is defined as:

[0043]

[0044] in, is the measurement equation of ultra-wideband;

[0045] h UWB (x k ) is the measurement function of the UWB estimated true value.

[0046] Furthermore, the deviation factor node formula related to the IMU inertial device is:

[0047]

[0048] Among them, α k+1 is the error variable generated by the IMU inertial device at time k+1;

[0049] g(α k ) is the error update equation that calculates the error variable at time k+1 through the error variable at time k.

[0050] Furthermore, in step 3, the Harris Eagle optimization algorithm is used to find the optimal value of the objective function and obtain the three-dimensional coordinates of the drilling rig. The specific steps are as follows:

[0051] Step a: Initialize Harris Eagle algorithm parameters;

[0052] Step b: Calculate the escape energy E of the prey according to the energy convergence formula;

[0053] E=E0E1

[0054] E1=2(1-t m / T m )

[0055] Where: E0 is a random number in [-1, 1]; E1 is the convergence factor; t m and T m are the current number of iterations and the maximum number of iterations respectively;

[0056] Step c: According to the escape energy E of the prey in step b, enter different search phases to update the position, and update the current global optimal position and fitness value according to the objective function;

[0057] Step d: If the number of iterations t m Reach the maximum number of iterations T m , the optimization process ends and the global optimal position is output. Otherwise, jump to step b and continue execution until the global optimal position is output.

[0058] Compared with the existing technology, this underground mobile equipment positioning method based on the fusion of ultra-wideband and inertial units obtains the fused proportional acceleration and fused angular velocity by fusing and solving the six-way inertial unit data, and integrates the IMU information and UWB information into a factor graph framework, establishes the IMU / UWB factor graph model, obtains the minimum error function, and finds the optimal value through the Harris Eagle optimization algorithm to obtain the three-dimensional coordinates of the drilling rig. This method not only avoids the positioning information jump caused by single UWB positioning, effectively converges the UWB positioning error, and avoids the cumulative error caused by single IMU positioning, but also can flexibly combine sensors with different measurement frequencies, and can also cope with the situation where the sensor is temporarily unavailable or new sensor information is introduced, realizing plug-and-play functionality, and can still be used when a single sensor fails. BRIEF DESCRIPTION OF THE DRAWINGS

[0059] Figure 1 This is a flow chart of the anti-bumping drilling positioning method of the present invention;

[0060] Figure 2 This is a schematic diagram of the present invention being applied to an anti-bumping drilling robot;

[0061] Figure 3This is a front view of the present invention applied to an anti-bumping drilling robot;

[0062] Figure 4 This is a physical schematic diagram of the redundant inertial unit in the present invention;

[0063] Figure 5 It is the three-axis pointing diagram of each inertial unit in the redundant inertial unit of the present invention;

[0064] Figure 6 It is the IMU factor graph model in the present invention;

[0065] Figure 7 is the overall structure of the factor graph in the present invention;

[0066] In the figure: 1. Ultra-wideband terminal, 2. Redundant inertial mechanism, 3. Crawler mobile chassis, 4. Rotating platform, 5. First hydraulic support, 6. Vise, 7. Motor, 8. Second hydraulic support, 9. Drill rod box, 10. Rod raising platform, 11. Crawler chassis top plate. DETAILED DESCRIPTION

[0067] The present invention will be further described below with reference to the accompanying drawings.

[0068] The following will clearly and completely describe the technical solutions in the embodiments of the present invention in conjunction with the accompanying drawings. 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 making creative efforts are within the scope of protection of the present invention.

[0069] like Figure 2 、 Figure 3 、 Figure 4 As shown, the present invention provides a positioning method for underground mobile equipment based on the fusion of ultra-wideband and inertial unit, which is mainly used in an anti-bumping drilling robot. In some embodiments, the anti-bumping drilling robot includes:

[0070] The walking mechanism includes a crawler-type mobile chassis 3 that is driven to move, and a first hydraulic support 5 that can be raised and lowered is provided around the crawler-type mobile top plate;

[0071] The rotating platform 4 is located on the top plate 11 of the crawler chassis and is provided with an upper rod platform 10. The second hydraulic supports 8 are provided at the surrounding positions.

[0072] The drill rod is located in the drill rod box 9 and is transported to the vise 6 located at the end side of the rotating platform 4 through the rod upper platform 10 for clamping. The rear side of the rotating rod is provided with a motor 7 to drive its rotation;

[0073] The ultra-wideband terminal 1 is fixedly connected to the top of the redundant inertial mechanism 2, and the redundant inertial mechanism 2 is fixedly connected to the vise 6;

[0074] Specifically, the walking mechanism is the bottom support structure of the anti-collision drilling robot, which can adopt a traditional heavy robot; the ultra-wideband terminal 1 is matched with several other base stations to form an ultra-wideband module;

[0075] When the present embodiment is working, the ultra-wideband terminal 1 and the redundant inertial mechanism 2 perform unmanned positioning accordingly, that is, they are used to obtain the posture estimation and position estimation of the motion information of the anti-collision drilling robot, and integrate the optimal three-dimensional coordinates (target position) based on the Harris Eagle optimization algorithm. The posture information obtained is sent to the PLC, and the PLC performs identification and analysis, and controls the hydraulic motor 7 to drive the crawler mobile chassis 3 to move. When it moves to the target position, the first hydraulic support 5 and the second hydraulic support 8 are adjusted downward and upward respectively to support the tunnel ground and the top layer of the tunnel, so as to achieve the fixation of the overall anti-collision drilling robot. The rotating platform 4 is then rotated to the target position, and the drill rod in the drill rod box 9 is transported to the vise 6 through the upper rod platform 10 and clamped. The motor 7 on the drill rod end side drives the drill rod to rotate and act on the corresponding drilling surface.

[0076] After the drilling operation is completed, the rotating platform 4 returns to the center, the first hydraulic support 5 and the second hydraulic support 8 retract to their initial positions, and the ultra-wideband terminal 1 and the redundant inertial mechanism 2 continue to perform real-time positioning through the fusion algorithm.

[0077] like Figure 1 As shown, the present invention provides a positioning method for underground mobile equipment based on the fusion of ultra-wideband and inertial unit, which specifically includes the following steps:

[0078] Step 1: Fuse and solve the six-way inertial unit data:

[0079] The three-axis angular velocity and three-axis proportional acceleration of the six inertial units are converted into parameters in the drilling rig coordinate system through the attitude transformation matrix. Then, data fusion is performed to obtain the fused proportional acceleration and fused angular velocity, and the position information of the drilling rig is obtained by solution.

[0080] Specifically, refer to Figure 4 、 Figure 5 The six inertial units are arranged in a regular tetrahedron space, and are located at the five connection points and the center of the regular tetrahedron. That is, the inertial units are arranged in a specific space to form redundant inertial units.

[0081] The z-axis of the inertial unit at the center of the regular tetrahedron points to the upper vertex of the regular tetrahedron, and the y-axis points forward. The z-axis of the inertial unit at the upper vertex points to the bottom surface, and the y-axis points forward. The four inertial units at the bottom are distributed in a differential structure at the four vertices of the bottom surface of the regular tetrahedron, and the z-axes of two adjacent inertial units point in opposite directions, and the y-axes all point to the center point of the bottom surface; the x-axes of the six inertial units all obey the right-hand coordinate system principle.

[0082] In some embodiments, the angular velocity attitude transformation matrix is ​​expressed as:

[0083]

[0084] Among them, ω ibx 、ω iby 、ω ibz They represent the three-axis angular velocity corresponding to position i in the drilling rig coordinate system. i ,θ i 、 They represent the three-axis rotation angles of the inertial unit i relative to the drilling rig coordinate system. ix 、ω iy 、ω iz Respectively represent the three-axis angular velocity measured by the inertial unit;

[0085] The proportional acceleration attitude transformation matrix is ​​expressed as:

[0086]

[0087] Where: f ibx 、f iby 、f ibz They represent the three-axis proportional acceleration corresponding to position i in the drilling rig coordinate system. i ,θ i 、 They represent the rotation angles of the inertial unit i around the three axes relative to the drilling rig coordinate system. ix 、f iy 、f iz They represent the three-axis proportional accelerations measured by the inertial unit respectively;

[0088] Fusion means that the three-axis angular velocity and three-axis proportional acceleration of the six inertial units are converted into parameters in the drilling rig coordinate system according to their positional relationship through the attitude transformation matrix of angular velocity and proportional acceleration, and then the average value is accumulated;

[0089] That is, the fused angular velocity is expressed as:

[0090]

[0091] The fused proportional acceleration is expressed as:

[0092]

[0093] Step 2: Integrate the fused acceleration and fused angular velocity obtained by the inertial unit (IMU) in step 1, as well as the three-dimensional position of the ultra-wideband (UWB) into the factor graph, establish the IMU / UWB factor graph model, and obtain the minimum error function;

[0094] Among them, the three-dimensional position of ultra-wideband Obtained using conventional methods.

[0095] Specifically, first, an IMU factor graph model is established;

[0096] The current navigation state is:

[0097]

[0098] Where: x is the navigation state at the previous moment, including three-dimensional coordinates, speed information, and attitude information;

[0099] α represents the error variable generated by the IMU inertial device; f b is the fusion proportional acceleration at the current moment; w b is the fusion angular rate at the current moment. h() is the navigation state x at the previous moment and the current moment f b 、w b , α obtains the current navigation state The state update equation of

[0100] The measurement information formula of IMU is:

[0101]

[0102] Assuming the current time is k, the current navigation state can be expressed as x k , then the navigation state at the next moment can be expressed as x k+1 And so on. The navigation state x adjacent to the next moment at time k k and x k+1 In connection with it, after discretizing the time update equation described, the relationship between the two is obtained as follows:

[0103]

[0104] Where: x k is the navigation state at the current time k, x k+1 is the navigation state at the next moment; n IMU is the measurement noise of UWB;

[0105] Through the above equation, the IMU factor node formula is:

[0106]

[0107] Explanation: d() represents the error function connecting the upper and lower adjacent states, also known as factor potential energy;

[0108] Define an IMU factor node, refer to Figure 6 , the two navigation state quantities x k+1 and x k And the error variable α generated by the IMU inertial device k Through this IMU factor node f IMU (x k+1 ,x k ,α k ) is connected, the current navigation state x k Information measured by IMU To predict the navigation state x at the next moment k+1 , the predicted value and the actual navigation state x at the next moment k+1 The error function between them is the IMU factor node formula.

[0109] As time is updated, the IMU's inertial device error will also change. The inertial device error function is defined as follows:

[0110] α k+1 =g(α k )+n α

[0111] where α k represents the error variable generated by the IMU inertial device theory at time k; g(α k ) is the error update equation for calculating the error variable at time k+1 through the error variable at time k, n α is the measurement noise of α;

[0112] Similarly, the error variable α generated by the IMU inertial device can be obtained k The relevant deviation factor nodes are as follows:

[0113]

[0114] In the formula, an IMU bias factor node is defined, and the two error variables α k+1 and α k Through this IMU bias factor node f bias (α k+1 ,α k ) are connected.

[0115] Secondly, establish the UWB factor graph model;

[0116] The three-dimensional position result obtained by ultra-wideband solution using conventional methods is expressed as:

[0117]

[0118] The measurement equation of ultra-wideband can be expressed as:

[0119]

[0120] Among them, h UWB (x k ) is the measurement function of the UWB estimated true value; n UWB is the measurement noise of UWB;

[0121] Therefore, the factor node formula of UWB is:

[0122]

[0123] Therefore, according to the IMU factor node formula and the UWB factor node formula, the error formula is obtained;

[0124] That is, the error function at time 1 is:

[0125]

[0126] Then the error function within time 2 is:

[0127]

[0128] Similarly, refer to Figure 7 , the minimum error function at time n is:

[0129] argmin x F(x 1:n )

[0130]

[0131] Where: f prior is the prior factor node;

[0132] That is, the prior information available at the initial moment in and They represent the initial estimates of the three-dimensional position, velocity, and three-axis angle of the anti-collision drilling robot, respectively. The error function of the prior factor can be expressed as:

[0133]

[0134] Where μ = {X1, V1, α1} represents the initial state of the vehicle, where X1, V1, and α1 represent the initial estimates of the three-dimensional position, velocity, and three-axis angle of the anti-collision drilling robot in the initial state, respectively, and Σ Prior The error covariance matrix representing the initial state of the anti-collision drilling robot;

[0135] like Figure 7 As shown in the figure, the factor graph structure of the anti-collision drilling robot fusion positioning can be derived. As time goes by, new measurement information continues to enter, and the factor graph continues to extend to the right. It can be seen from the figure that the structure of the factor graph makes it highly scalable. It can flexibly combine sensors with different measurement frequencies, and can also cope with situations where sensors are temporarily unavailable or new sensor information is introduced, realizing plug-and-play functionality.

[0136] Step 3: Using the error function in step 2 as the objective function, the Harris Eagle optimization algorithm is used to find the optimal value and obtain the three-dimensional coordinates of the drilling rig.

[0137] Specifically, the minimum error function of the factor graph is adjusted to the objective function, that is, the objective function is expressed as:

[0138] argmin x F(x 1:n )

[0139]

[0140] The Harris Eagle optimization algorithm is used to find the optimal value of the objective function and obtain the three-dimensional coordinates of the drilling rig. That is, the objective function can be used as the fitness function of the Harris Eagle. The steps are as follows:

[0141] Step a: Initialize the Harris Hawk algorithm parameters, such as population size N and maximum number of iterations T;

[0142] Step b: Calculate the escape energy E of the prey according to the energy convergence formula;

[0143] E=E0E1

[0144] E1=2(1-t m / T m )

[0145] Where: E0 is a random number in [-1, 1]; E1 is the convergence factor; t m and T m are the current number of iterations and the maximum number of iterations respectively;

[0146] Step c: According to the escape energy E of the prey in step b, enter different search phases to update the position, and update the current global optimal position and fitness value according to the minimum error function (objective function);

[0147] Step d: If the number of iterations t m Reach the maximum number of iterations T m , the optimization process ends and the global optimal position is output. Otherwise, jump to step b and continue execution until the global optimal position is output.

[0148] The present invention fuses the data of redundant inertial units to obtain fused proportional acceleration and fused angular velocity, solves and obtains the posture information of the drilling rig, and integrates the IMU information and UWB information into a factor graph framework, establishes the factor graph model of IMU / UWB, obtains the minimum error function, and uses the Harris Eagle optimization algorithm to find the optimal value of the minimum error function to obtain the three-dimensional coordinates of the drilling rig. It not only avoids the positioning information jump caused by single UWB positioning, effectively converges the positioning error of UWB, and avoids the cumulative error caused by single IMU positioning, but also can flexibly combine sensors with different measurement frequencies, and can also cope with the situation where the sensor is temporarily unavailable or new sensor information is introduced, realizing plug-and-play function, and can still be used when a single sensor fails (such as ultra-wideband terminal 1 under non-line-of-sight conditions).

[0149] In the description of the present invention, it should be understood that the terms "center", "up", "down", "front", "back", "left", "right", "vertical", "horizontal", "top", "bottom", "inside", "outside", etc., indicating the orientation or position relationship, are based on the orientation or position relationship shown in the accompanying drawings, and are only for the convenience of describing the present invention and simplifying the description, rather than indicating or implying that the device or element referred to must have a specific orientation, be constructed and operated in a specific orientation, and therefore cannot be understood as limiting the present invention.

Claims

1. A method for positioning underground mobile equipment based on the fusion of ultra-wideband and inertial unit, characterized in that: The specific steps include: Step 1: Fuse and solve the six-way inertial unit data: The three-axis angular velocity and three-axis proportional acceleration of the six inertial units are converted into parameters in the drilling rig coordinate system through the attitude transformation matrix. Then, data fusion is performed to obtain the fused proportional acceleration and fused angular velocity, and the position information of the drilling rig is obtained by solution. Step 2: Integrate the fused acceleration and fused angular velocity obtained by the inertial unit in step 1, as well as the three-dimensional position of the ultra-wideband into the factor graph, establish the factor graph model of IMU / UWB, and obtain the minimum error function; Step 3: Using the error function in step 2 as the objective function, the Harris Eagle optimization algorithm is used to find the optimal value and obtain the three-dimensional coordinates of the drilling rig. In step 1, the six inertial units are arranged in a regular tetrahedron space, and are located at the five connection points and the center of the regular tetrahedron; The z-axis of the inertial unit at the center of the regular tetrahedron points to the upper vertex of the regular tetrahedron, and the y-axis points forward. The z-axis of the inertial unit at the upper vertex points to the bottom surface, and the y-axis points forward. The four inertial units at the bottom are distributed in a differential structure at the four vertices of the bottom surface of the regular tetrahedron, and the z-axes of two adjacent inertial units point in opposite directions, while the y-axes all point to the center of the bottom surface. The x-axes of the six inertial units all follow the right-hand coordinate system principle. The angular velocity attitude transformation matrix is ​​expressed as: Among them, ω ibx 、ω iby 、ω ibz They represent the three-axis angular velocity corresponding to position i in the drilling rig coordinate system, γ i ,θ i 、 They represent the three-axis rotation angles of inertial unit i relative to the drilling rig coordinate system, ω ix 、ω iy 、ω iz They represent the three-axis angular velocities measured by inertial unit i; The proportional acceleration attitude transformation matrix is ​​expressed as: Where: f ibx 、f iby 、f ibz They represent the three-axis proportional acceleration corresponding to position i in the drilling rig coordinate system, γi, θi, They represent the rotation angles of the inertial unit i around the three axes relative to the drilling rig coordinate system, f ix 、f iy 、f iz They represent the three-axis proportional accelerations measured by inertial unit i; The fused angular velocity is expressed as: The fused proportional acceleration is expressed as:

2. The method for positioning underground mobile equipment based on ultra-wideband and inertial unit fusion according to claim 1, characterized in that: In step 2, the minimum error function is: argmin x F(x 1:n ) Among them, f prior is the prior factor node; f bias (α k+1 ,α k ) is the bias factor node related to the IMU inertial device; f IMU (x k+1 ,x k ,α k ) is the factor node of IMU at time k; f UWB (x k ) is the factor node of UWB at time k.

3. The method for positioning underground mobile equipment based on ultra-wideband and inertial unit fusion according to claim 2, characterized in that: The error function corresponding to the prior factor node is: Where μ = {X1, V1, ε1} represents the initial state of the vehicle, X1, V1, and ε1 represent the initial estimates of the three-dimensional position, velocity, and three-axis angle of the anti-collision drilling robot in the corresponding state, respectively, and Σ Prior The error covariance matrix representing the initial state of the anti-collision drilling robot; represents the prior information available at the initial moment, and They represent the initial estimation of the three-dimensional position, velocity and three-axis angle of the anti-collision drilling robot respectively.

4. The method for positioning underground mobile equipment based on ultra-wideband and inertial unit fusion according to claim 3, characterized in that: The factor node formula of the IMU at time k is: Among them, x k is the navigation state at the current time k, x k+1 is the navigation state at the next moment; α k is the error variable generated by the IMU inertial device at time k; d() represents the error function connecting the upper and lower adjacent states; is the IMU measurement information, and its formula is: f b is the fusion proportional acceleration at the current k moment; w b is the fused angular rate at the current k moment.

5. The method for positioning underground mobile equipment based on ultra-wideband and inertial unit fusion according to claim 4, characterized in that: The factor node of UWB at time k is defined as: in, is the measurement equation of ultra-wideband; h UWB (x k ) is the measurement function of the UWB estimated true value.

6. The method for positioning underground mobile equipment based on ultra-wideband and inertial unit fusion according to claim 5, characterized in that: The deviation factor node formula related to the IMU inertial device is: Among them, α k+1 is the error variable generated by the IMU inertial device at time k+1; g(α k ) is the error update equation that calculates the error variable at time k+1 through the error variable at time k.

7. The method for positioning underground mobile equipment based on ultra-wideband and inertial unit fusion according to claim 6, characterized in that: In step 3, the Harris Eagle optimization algorithm is used to find the optimal value of the objective function and obtain the three-dimensional coordinates of the drilling rig. The specific steps are as follows: Step a: Initialize Harris Eagle algorithm parameters; Step b: Calculate the escape energy E of the prey according to the energy convergence formula; E=E0E1 E1=2(1-t m / T m ) Where: E0 is a random number in [-1, 1]; E1 is the convergence factor; t m and T m are the current number of iterations and the maximum number of iterations respectively; Step c: According to the escape energy E of the prey in step b, enter different search phases to update the position, and update the current global optimal position and fitness value according to the objective function; Step d: If the number of iterations t m Reach the maximum number of iterations T m , the optimization process ends and the global optimal position is output; otherwise, the process jumps to step b and continues until the global optimal position is output.

Images