UWB and IMU based tightly coupled positioning method for downhole vehicle, device, and medium thereof

Abstract

The present invention relates to the technical field of downhole positioning, and in particular to a UWB and inertial measurement unit (IMU) based tightly coupled positioning method for a downhole vehicle. The method comprises: a superordinate computer constructs a downhole map on the basis of position coordinates of all base stations, and establishes a base station database, wherein the base station database comprises base station IDs and coordinate information of each base station; on the basis of the base station IDs, the superordinate computer delivers base station data information to the corresponding base stations for storage; a vehicle moves in a downhole roadway, when the vehicle moves to the vicinity of any base station, a UWB tag requests a signal connection to the corresponding base station, the base station transmits base station data information to a terminal device, and the terminal device acquires distance information between the UWB tag and the base station and inertial data outputted by an IMU; and the terminal device processes and calculates the distance information and the inertial data on the basis of the base station data information to obtain current position information of the vehicle. The UWB and IMU based tightly coupled positioning method for a downhole vehicle in the present invention has a simple structure, facilitates modification, and has low implementation costs and high applicability.

Description

Tightly integrated positioning method, equipment and medium for vehicles in mines based on UWB and IMU Technical Field

[0001] The present invention relates to the technical field of underground positioning, and in particular to a tightly combined positioning method for underground vehicles in mines based on UWB and IMU. Background Art

[0002] UWB (Ultra Wide Band) positioning technology boasts high positioning accuracy, low power consumption, high security, and strong signal penetration, providing effective information for precise underground positioning. The underground UWB precision positioning system primarily utilizes a three-tiered architecture consisting of UWB tags, base stations, and a host computer. The UWB tags, carried by personnel or vehicles, serve as the system's positioning terminals. The base stations, also known as card readers, are responsible for both distance measurement and interaction with the terminals and for uploading data to the host computer via a ring network. The host computer handles the comprehensive processing and display of all underground data.

[0003] Currently, underground vehicle location services are calculated using two methods: surface platform calculation and autonomous calculation by underground terminals. The advantage of surface platform calculation is that it can apply the design model of the personnel positioning system without changing the calculation model, and only requires adding a location distribution process. The main disadvantages are as follows:

[0004] ① The transmission of location information requires at least two steps: from the host computer to the base station, and from the base station to the UWB tag. Furthermore, only the location information of the last moment can be obtained, resulting in high network delay risk and poor timeliness.

[0005] ② The wireless bandwidth occupied by the location will affect the performance and capacity of the UWB precise positioning system;

[0006] ③ Unable to deal with signal blind spot problems and wireless communication packet loss problems.

[0007] With the continuous development of technologies in the direction of intelligent, less-manned or unmanned coal mines, the current surface platform computing model can no longer meet the high-efficiency positioning needs such as vehicle-assisted transportation and unmanned driving underground. In order to solve the shortcomings of surface platform computing, the underground terminal autonomous computing is used to integrate multiple reference quantities such as IMU, lidar, UWB, machine vision, RSSI, total station, etc. to make decisions, realize loose combined positioning at the underground terminal, directly construct a three-dimensional map through multiple sensors, and form a navigation system based on the map. The formed navigation system has rich basic information and is suitable for comprehensive decision-making of a small number of large or high-end products. However, due to the need to set up multiple sensors, the implementation structure is complex, not convenient for modification, and the single cost is high, which is not suitable for the actual application of large-scale products.

[0008] Summary of the Invention

[0009] The technical problem to be solved by the present invention is that the existing technology of using autonomous computing in underground terminals requires multiple sensors, resulting in a complex structure, inconvenient modification, high cost per sensor, and unsuitable for practical application in large quantities of products. The present invention provides a tightly integrated positioning method for underground vehicles based on UWB and IMU, which has a simple structure, is easy to modify, has low implementation cost, and is highly applicable.

[0010] The technical solution adopted by the present invention to solve the technical problem is: a tightly combined positioning method for underground vehicles in mines based on UWB and IMU, the method is based on the underground UWB precise positioning system;

[0011] The underground UWB precise positioning system includes: a host computer, several base stations and a vehicle-mounted positioning device installed on a vehicle, wherein the host computer and the vehicle-mounted positioning device are both connected to the base station signal, and the vehicle-mounted positioning device includes a UWB tag, an IMU inertial unit and a terminal device, wherein the UWB tag is connected to the base station signal, and the UWB tag and the IMU inertial unit are both connected to the terminal device;

[0012] The method comprises the following steps:

[0013] S1, the host computer constructs an underground map according to the position coordinates of all base stations and establishes a base station database, wherein the base station database includes the base station ID and the coordinate information of each base station;

[0014] S2, the host computer sends the base station data information to the corresponding base station for storage based on the base station ID;

[0015] S3, the vehicle is located in an underground tunnel and moves. When the vehicle moves near any of the base stations, the UWB tag requests a signal connection from the corresponding base station. The base station transmits the base station data information to the terminal device, and the terminal device obtains the distance information between the UWB tag and the base station, as well as the inertial data output by the IMU inertial unit;

[0016] The base station data information includes the coordinate information of the base station and the base stations on both sides adjacent to the base station;

[0017] S4, the terminal device processes and calculates the distance information and the inertial data based on the base station data information to obtain the current position information of the vehicle.

[0018] Further, specifically, in step S3, the distance information between the UWB tag and the base station includes the following steps:

[0019] S311, the UWB tag sends a ranging signal;

[0020] S312, after receiving the ranging signal, the base station calculates the distance to the UWB tag using a two-way unilateral ranging method, and feeds back the distance calculation result to the UWB tag;

[0021] S313, the terminal device obtains distance information fed back by each base station.

[0022] Furthermore, specifically, in step S3, the terminal device obtains the inertial data output by the IMU inertial unit and specifically includes the following steps:

[0023] S321, when the vehicle starts to move, the rolling angle of the vehicle is The pitch angle is γ, the heading angle is θ, and the IMU inertial unit measures the angular velocity and acceleration of the vehicle in three axes in real time. According to the strapdown inertial navigation attitude update algorithm, the vehicle attitude conversion matrix C is calculated. According to the fixed axis rotation principle, the roll angle is calculated as The pitch angle is γ and the heading angle is θ:

[0024] S322, rotating the IMU navigation coordinate system according to the heading angle to construct a spatial coordinate system of the downhole UWB precise positioning system map;

[0025] S323, calculating inertial data of the IMU inertial unit in real time according to the IMU positioning model;

[0026] Assume that the state value of the IMU positioning model is:

[0027] The mathematical expression of the IMU positioning model table is: k =f(X k-1 ,ω k );

[0028] in,

[0029] Where: k is the current moment, p k is the position at time k; v k is the speed at time k; q k is the quaternion at time k; a k is the acceleration at time k; g is the acceleration due to gravity; Ω k The quaternion update matrix at time k; represents the rotation matrix at time k;

[0030] Further, specifically, in step S4, the terminal device processes and calculates the distance information and the inertial data based on the base station data information, including the following steps:

[0031] S41, the UWB tag card periodically outputs the coordinate position, and the terminal device obtains the base station data information of the current road section and the distance information of the tag card at time T;

[0032] S42, the IMU inertial unit outputs the inertial data accumulated starting from time T-1;

[0033] S43, combining the IMU weighting parameters, inputting the current road section base station data information, the distance information and the inertial data into the fusion algorithm model for processing to obtain the vehicle position information at time T.

[0034] Furthermore, specifically, in step S43, the fusion algorithm model includes a straight lane algorithm, a curve algorithm, and a blind spot algorithm. The algorithm call specifically includes:

[0035] If the vehicle is not in the UWB signal coverage area, the blind spot algorithm is called, and the terminal device calculates the vehicle's position information based on the inertial data;

[0036] If the vehicle is in the UWB signal coverage area, assume that the vehicle is between the first base station and the second base station, obtain the distance information d1 between the vehicle and the first base station, and the distance information d2 between the vehicle and the second base station. Based on the base station data information, calculate the straight-line distance D between the first base station and the second base station, and calculate the ratio of the coordinate distance between the two base stations to the distance information. The calculation formula is: β = (d1 + d2) / D;

[0037] If β is between 95% and 105%, a straight line algorithm is called to calculate the position information of the vehicle according to the distance information and the base station data information;

[0038] If β is greater than 105%, the curve algorithm is called, the IMU weighted parameter value is β, and the position information of the vehicle is calculated based on the distance information and the inertial data.

[0039] Further, specifically, the position information P of the vehicle at time T is calculated based on the distance information and the base station data information. T The calculation formula is:

[0040] Where: P T-1 is the vehicle position coordinate at time T-1, The direction vector of the current UWB tag movement.

[0041] Furthermore, specifically, the calculation of the curve algorithm includes the following steps:

[0042] Step A, calculating the first coordinate P of the vehicle based on the first distance information, the second distance information and the heading angle t1 , the calculation formula is:

[0043] in, Represents the vehicle position coordinate P at time T-1 T-1 is the starting reference point, and the vehicle travel vector is obtained based on the heading angle, ⊙ LR The first base station and the second base station measure the distance information to calculate the plane where the vehicle is located, and obtain the first coordinate P by calculating the intersection of the vehicle's driving vector and the plane. t1 ;

[0044] Step B: Based on the IMU quadratic integral mathematical model, the vehicle position coordinate P at time T-1 is calculated. T-1 The target prediction coordinate P at the current moment is calculated based on the vehicle starting point, the vehicle's own speed, and the vehicle acceleration measured by the IMU. t2 , the calculation formula is:

[0045] Where v(τ) represents the vehicle's own speed, ΔT represents the time interval between two measurements by the base station, ω(τ) represents the vehicle's angular velocity measured by the IMU, and R ω(τ) represents the rotation matrix calculated based on the vehicle's angular velocity, and a(τ) represents the vehicle acceleration measured using the IMU;

[0046] Step C, the first coordinate P t1 and the current target prediction coordinate P t2 Fusion calculation, get the vehicle position information P at time T T , the calculation formula is: T =P t2 +K(β)(P t1 -HP t2 );

[0047] Among them, K(β) represents the weight calculated using the curve ratio β, H is the state vector, which is used to represent P t1 With P t2 The mapping relationship between them.

[0048] Furthermore, specifically, the reference point of the IMU inertial unit is calibrated, which specifically includes the following steps:

[0049] Map and convert the distance information to obtain the mapped distance of the vehicle;

[0050] A reference point of an inertial unit is selected according to the UWB measurement time and the mapping distance of the vehicle, and the IMU inertial unit is calibrated.

[0051] A computer device comprising:

[0052] processor;

[0053] a memory for storing executable instructions;

[0054] The processor is configured to read the executable instructions from the memory and execute the executable instructions to implement the above-mentioned tightly combined positioning method for underground vehicles in mines based on UWB and IMU.

[0055] A computer-readable storage medium stores a computer program. When the computer program is executed by a processor, the processor implements the above-mentioned UWB and IMU-based tight combination positioning method for underground mine vehicles.

[0056] The beneficial effects of the present invention are:

[0057] (1) The present invention's tightly integrated positioning method for underground mine vehicles based on UWB and IMU uses an underground UWB precise positioning system as a framework to fuse UWB measurement data and IMU measurement data, thereby improving the accuracy of vehicle positioning and providing highly reliable, low-latency positioning services for underground vehicle-assisted transportation and unmanned driving.

[0058] (2) The present invention is easy to modify and effectively reduces platform storage space usage and core computing time consumption in scenarios with limited computing resources. It has the characteristics of low system response delay, high robustness, simple structure, low construction cost and strong adaptability, effectively improving system operation accuracy and stability;

[0059] (3) The present invention inputs UWB measurement data and IMU measurement into a fusion algorithm model, and uses a calculation method based on UWB measurement data to distinguish between straight lines, curves, and blind spots, thereby further improving the positioning accuracy of underground vehicles.

[0060] (4) The present invention obtains a one-dimensional positioning solution for the vehicle by mapping and converting the distance information. Combined with the selection of the reference point of the IMU inertial unit based on the UWB measurement time, the adverse effects of the IMU cumulative error can be effectively eliminated. BRIEF DESCRIPTION OF THE DRAWINGS

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

[0062] FIG1 is a schematic diagram of a method flow chart according to a first embodiment of the present invention.

[0063] FIG2 is a schematic structural diagram of an underground UWB precise positioning system according to an embodiment of the present invention.

[0064] FIG3 is a schematic diagram of a processing flow of a terminal device according to an embodiment of the present invention.

[0065] FIG4 is a schematic diagram of vehicle position calculation according to a first embodiment of the present invention.

[0066] FIG5 is a schematic diagram of the positions of a vehicle and a base station according to an embodiment of the present invention.

[0067] FIG6 is a schematic diagram of the movement of a vehicle according to an embodiment of the present invention

[0068] FIG7 is a schematic diagram of the hardware structure of a computing device according to a second embodiment of the present invention.

[0069] In the figure, 1 is the host computer; 2 is the base station; 3 is the vehicle-mounted positioning device; 31 is the UWB tag; 32 is the IMU inertial unit; 33 is the terminal device; and 34 is the display. DETAILED DESCRIPTION

[0070] The present invention will now be described in further detail with reference to the accompanying drawings, which are simplified schematic diagrams that illustrate the basic structure of the present invention in a schematic manner.

[0071] In the description of the present invention, it should be understood that the terms "center", "longitudinal", "lateral", "length", "width", "thickness", "up", "down", "front", "back", "left", "right", "vertical", "horizontal", "top", "bottom", "inside", "outside", "clockwise", "counterclockwise", "axial", "radial", "circumferential" and the like indicate orientations or positional relationships based on the orientations or positional relationships 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. In addition, features defined as "first" or "second" may explicitly or implicitly include one or more of such features. In the description of the present invention, unless otherwise specified, "multiple" means two or more.

[0072] In the description of the present invention, it should be noted that, unless otherwise expressly specified or limited, the terms "mounted," "connected," and "connected" should be understood in a broad sense. For example, they may refer to fixed, detachable, or integral connections; mechanical or electrical connections; direct or indirect connections through an intermediate medium; and internal communication between two components. Those skilled in the art will understand the specific meanings of the above terms in the present invention based on the specific circumstances.

[0073] Example 1

[0074] An embodiment of the present application provides a tightly integrated positioning method for underground vehicles in mines based on UWB and IMU. The method is based on an underground UWB precise positioning system as a framework. As shown in Figure 2, the underground UWB precise positioning system includes: a host computer 1, several base stations 2, and a vehicle-mounted positioning device 3 installed on a vehicle. The host computer 1 and the vehicle-mounted positioning device 3 are both signal-connected to the base station 2. The vehicle-mounted positioning device 3 includes a UWB tag 31, an IMU inertial unit 32, and a terminal device 33. The UWB tag 31 is signal-connected to the base station 2. The UWB tag 31 and the IMU inertial unit 32 are both connected to the terminal device 33.

[0075] In this embodiment, as shown in FIG1 , the method includes the following steps:

[0076] S1, the host computer 1 constructs an underground map according to the position coordinates of all base stations 2 and establishes a base station database, which includes a base station ID and coordinate information of each base station 2.

[0077] Before the vehicle enters the mine, a wireless LAN connection is established between the host computer 1 and the terminal device 33. The host computer 1 transmits the mine map to the terminal device 33, see Figure 3(a), for vehicle location service.

[0078] S2: The host computer 1 sends the base station data information to the corresponding base station 2 for storage based on the base station ID. The base station data information includes the coordinate information of the base station 2 and the base stations 2 on both sides of the base station 2.

[0079] S3, the vehicle is moving in an underground tunnel. When the vehicle moves near any base station 2, the UWB tag 31 requests a signal connection from the corresponding base station 2, and the base station 2 transmits base station data information to the terminal device 33, see Figure 3(b), and the terminal device 33 obtains the distance information between the UWB tag 31 and the base station 2, as well as the inertial data output by the IMU inertial unit 32.

[0080] S4, the terminal device 33 processes and calculates the distance information and inertial data based on the base station data information, see Figure 3(c) to obtain the current position information of the vehicle.

[0081] It should be noted that the terminal device 33 also includes a display 34 for updating the vehicle's current location information on a map in real time (see Figure 3(d)). The terminal device 33 employs, but is not limited to, an onboard computer. For example, the UWB tag 31 and the IMU inertial unit 32 are connected to the onboard computer via a communication module, which may employ, but is not limited to, an RS485 module or a CAN module. The onboard computer also establishes wireless communication connections with the base station and the host computer via a wireless communication module, which may be, but is not limited to, a Wi-Fi module or a 4G module.

[0082] In this embodiment, the underground construction drawings are used as reference drawings, 3DGIS technology is adopted, and the earth coordinate system is used as the navigation coordinate system to construct an underground map. The underground map is corrected and adjusted based on actual underground measurements to improve the accuracy of the constructed underground map, and the position coordinates of all base stations 2 are accurately calibrated in the navigation coordinate system.

[0083] It should be noted that when using the underground construction drawings as reference drawings, due to the poor underground measurement environment, it is impossible to completely eliminate the absolute error of the coordinate information underground. There will be a cumulative error problem when constructing the map. In the prior art, a base station 2 is set as the starting base station, and other base stations 2 are deployed with the starting base station as the reference base station. If the error between the starting base station and base station 2 No. 2 is 1, and the error between the starting base station and base station 2 No. 100 is 100, it can be seen that the absolute error of using the starting base station as the reference base station is large, and the accuracy of the underground map constructed with the position coordinates of base station 2 is low. In this embodiment, the previous base station 2 will be used as the reference base station to deploy the next base station, and so on, until all base stations 2 are deployed. By converting the absolute error into the relative error of the current position, the accuracy of the position coordinates of base station 2 is improved, thereby improving the accuracy of the constructed underground map.

[0084] In this embodiment, in step S3, the distance information between the UWB tag 31 and the base station 2 includes the following steps:

[0085] S311, UWB tag 31 sends a ranging signal;

[0086] S312, after receiving the ranging signal, base station 2 uses a two-way unilateral ranging method to calculate the distance to the UWB tag 31, and feeds back the distance calculation result to the UWB tag 31. The UWB tag 31 and base station 2 interact to achieve ranging, so that the UWB ranging accuracy is within 30cm, which can achieve high-precision ranging.

[0087] Furthermore, the calculation formula of the two-way unilateral ranging method is: SS-TWR=1 / 2(T-round–T-reply)*C

[0088] Where T-round represents the total time of a TOF ranging measurement, T-reply represents the time from when base station 2 receives the message to when it sends a response, (T-round–T-reply) is the time it takes for the UWB message to fly twice, C represents the speed of light, and SS-TWR is the distance between base station 2 and UWB tag 31.

[0089] S313, the terminal device 33 obtains the distance information fed back by each base station 2.

[0090] In this embodiment, when the UWB tag 31 is stuck in the area of ​​a base station underground, the UWB tag 31 requests base station data information from the surrounding base station 2. The base station 2 transmits the base station data information to the terminal device 33 through the UWB tag 31. The terminal device 33 then implements underground UWB positioning based on the base station data information and distance information. If the underground base station 2 moves according to usage needs, the coordinate information of the base station 2 changes. The host computer 1 will synchronously update the information based on the changed base station 2 coordinate information and transmit the updated base station data information to the corresponding base station 2. Since the UWB tag 31 requests the base station data information when it accesses the base station 2 and obtains the updated base station data information, it avoids the problem of directly saving all base station data information to the terminal device 33 and being unable to update the base station data information in the prior art. In addition, saving all base station data information to the terminal device 33 requires a large amount of storage space for the terminal device 33. In this embodiment, the UWB tag 31 requests the base station data information when it accesses the base station 2, which reduces the storage space requirement.

[0091] In this embodiment, in step S3, the terminal device 33 obtains the inertial data output by the IMU inertial unit 32, specifically including the following steps:

[0092] S321, when the vehicle starts to move, the rolling angle of the vehicle is The pitch angle is γ, the heading angle is θ, and the IMU inertial unit 32 measures the angular velocity and acceleration of the vehicle in three axes in real time. According to the strapdown inertial navigation attitude update algorithm, the vehicle attitude conversion matrix C is calculated. According to the fixed axis rotation principle, it can be obtained:

[0093] The roll angle can be calculated as The pitch angle is γ and the heading angle is θ:

[0094] S322, rotating the IMU navigation coordinate system according to the heading angle to construct a spatial coordinate system of the underground UWB precise positioning system map;

[0095] Furthermore, the navigation coordinate system (N system) adopts the map coordinate system based on 3DGIS, and the coordinate system of the terminal device 33 (B system) uses the left front upper coordinate system. Convert B system to N system, rotation matrix for:

[0096] Among them, θ k is the heading angle at time k; T s is the sampling period; is the angular velocity in the N system at time k-1.

[0097] S323 , calculating inertial data of the IMU inertial unit 32 in real time according to the IMU positioning model, where the inertial data includes IMU integrated displacement and heading change.

[0098] Assume the state value of the IMU positioning model is:

[0099] The mathematical expression of the IMU positioning model table is: X k =f(X k-1 ,ω k );

[0100] in,

[0101] Where: k is the current moment, p k is the position at time k; v k is the speed at time k; q k is the quaternion at time k; a k is the acceleration at time k; g is the acceleration due to gravity; Ω k The quaternion update matrix at time k; represents the rotation matrix at time k;

[0102] In this embodiment, the IMU inertial unit 32 adopts but is not limited to the LSM6DS3TR integrated MEMS (micro-electromechanical system) sensor. The LSM6DS3TR includes a three-axis accelerometer and a three-axis gyroscope for measuring the acceleration and angular velocity of the vehicle.

[0103] In this embodiment, as shown in FIG4 , in step S4 , the terminal device 33 processes and calculates the distance information and inertial data based on the base station data information, including the following steps:

[0104] S41, the UWB tag 31 periodically outputs its coordinate position, and the terminal device 33 obtains the base station data and distance information of the tag card's current road section at time T;

[0105] S42, the IMU inertial unit 32 outputs the accumulated inertial data starting from time T-1;

[0106] S43, combining the IMU weighting parameters, inputting the current road section base station data information, distance information and inertial data into the fusion algorithm model for processing to obtain the vehicle position information at time T.

[0107] In this embodiment, the fusion algorithm model needs to meet the actual application of the underground environment. In the underground one-dimensional environment, the vehicle's route mainly considers two modes: straight line and curve. In step S43, the fusion algorithm model includes a straight lane algorithm, a curve algorithm, and a blind spot algorithm. The algorithm call specifically includes:

[0108] If the vehicle is in the UWB signal coverage area, assume that the vehicle is between the first base station and the second base station, obtain the distance information d1 between the vehicle and the first base station, and the distance information d2 between the vehicle and the second base station. Based on the base station data information, calculate the straight-line distance D between the first base station and the second base station, and calculate the ratio of the coordinate distance between the two base stations to the distance information. The calculation formula is: β = (d1 + d2) / D

[0109] If β is between 95% and 105%, the straight line algorithm is called to calculate the vehicle's position information P based on the distance information and base station data information. T The calculation formula is:

[0110] Where: P T-1 is the vehicle position coordinate at time T-1, The direction vector of the current UWB tag movement.

[0111] If β is greater than 105%, β deviates significantly from 1. The larger β is, the smaller the curve radius and the more severe the UWB multipath effect. The reliability of UWB distance measurement decreases as β increases, and the positioning accuracy of the vehicle's position calculated using the straight-line algorithm is low. When β is greater than 105%, the curve algorithm is invoked. In this case, the IMU weighting parameter in the curve algorithm is β, and the vehicle's position is calculated based on distance information and inertial data, improving positioning accuracy.

[0112] The calculation of the curve algorithm includes the following steps:

[0113] Step A, calculating the first coordinate P of the vehicle based on the first distance information, the second distance information and the heading angle t1 , the calculation formula is:

[0114] in, Represents the vehicle position coordinate P at time T-1 T-1 is the starting point reference point, and the vehicle driving vector is obtained based on the heading angle, ⊙ LR The first base station and the second base station measure the distance information to calculate the plane where the vehicle is located, and the first coordinate P is obtained by calculating the intersection of the vehicle's driving vector and the plane. t1 ;

[0115] Step B: Based on the IMU quadratic integral mathematical model, the vehicle position coordinate P at time T-1 is calculated. T-1 The target prediction coordinate P at the current moment is calculated based on the vehicle starting point, the vehicle's own speed, and the vehicle acceleration measured by the IMU. t2 , the calculation formula is:

[0116] Where v(τ) represents the vehicle's own speed, ΔT represents the time interval between two measurements by the base station, ω(τ) represents the vehicle's angular velocity measured by the IMU, and R ω(τ) represents the rotation matrix calculated based on the vehicle's angular velocity, and a(τ) represents the vehicle acceleration measured using the IMU;

[0117] Step C, for the first coordinate P t1 and the current target prediction coordinate P t2 Fusion calculation, get the vehicle position information P at time T T , the calculation formula is: T =P t2 +K(β)(P t1 -HP t2 );

[0118] Among them, K(β) represents the weight calculated using the curve ratio β, H is the state vector, which is used to represent P t1 With P t2 The mapping relationship between them.

[0119] If the vehicle is not in the UWB signal coverage area, the blind spot algorithm is called and the terminal device 33 calculates the vehicle's position information based on the inertial data. The calculation process is the same as step B and will not be repeated here for the sake of brevity.

[0120] In this embodiment, the MEMS IMU inertial unit 32 is selected. Although the cost is reduced, the current heading angle θ and the output p of the IMU inertial unit 32 are k The error exponent increases with time, resulting in low positioning accuracy of inertial navigation. In order to improve the positioning accuracy of the inertial unit, taking the underground base station interval of generally 300 meters and the average vehicle speed of 20km / h as an example, the calibration interval of the IMU inertial unit 32 is 54s. The error accumulation of the IMU inertial unit 32 is reduced by increasing the calibration frequency. However, in the position calculation process, the lane is regarded as a one-dimensional line. There is a mapping relationship between the actual distance of the vehicle and the UWB measurement distance. Since the terminal device 33 calculates the relative position of the vehicle and the base station 2, the UWB ranging distance needs to be mapped and converted, otherwise a large error will be generated at close distances, and the error at close distances will greatly affect the selection of the IMU reference point.

[0121] The reference point calibration of the IMU inertial unit 32 specifically includes:

[0122] Step 1: Map and convert the distance information to obtain the mapped distance of the vehicle.

[0123] Specifically, the installation method of the UWB tag 31 is fixed, and the vehicle can be considered to be traveling in a straight line in the underground tunnel. Therefore, the height h between the vehicle and the base station 2 is relatively fixed in actual application. As shown in Figure 5, the coordinates of base stations A, B, and C form a spatial straight line. The distance between base stations A and B must be a straight line. Through UWB ranging, the distance between the vehicle and base station A is a, the distance between the vehicle and base station B is b, and the distance between base stations A and B is c. The trigonometric cosine function is obtained:

[0124] The vehicle's mapping distance L = a*cosα, which obtains the vehicle's coordinate information on the one-dimensional map. Based on the mapping distance, the vehicle changes continuously and uniformly, which facilitates the switching of the IMU reference point and avoids the failure to calculate the mapping distance of the vehicle, resulting in distortion of the distance change and error in the reference point search, which in turn affects the reference point calibration of the inertial unit.

[0125] Step 2: Select the reference point of the inertial unit based on the UWB measurement time and the vehicle's mapping distance. See Figure 6. The specific process is as follows:

[0126] 1) When the vehicle travels from base station A to base station B, at time T1, the reference point is the position coordinates of base station A. When the mapping distance L becomes smaller, it indicates that the vehicle is approaching base station B, and the reference point is about to switch from base station A to base station B.

[0127] 2) At time T2, the mapping distance L is the smallest, and then the mapping distance increases. At this time, the vehicle passes base station B and starts to move towards base station C. The current reference point of the vehicle at base station B is within the time range of T2-T3.

[0128] Since the UWB ranging cycle is long (e.g., 100-2000ms) and the IMU measurement cycle is short (e.g., within 2ms), when switching the reference point with the position coordinates of base station 2 as a reference, if the vehicle's position is not at the nearest point of base station 2, as shown in Figure 6, (T is the UWB measurement time 1s, 1 UWB measurement time corresponds to 1000 IMU measurement times), the reference point is between measurement times T2 and T3. According to the mapping L of time T2 and T3 (for example, L of T2 is 2m, L of T3 is 6m, then the 1000 measurement values ​​of the IMU from t1 to t1000 between T2 and T3, t250 is the calibration point 2 / (2+6)*1000), the IMU parameters of the calibration point are found from the IMU historical measurement points and calibrated to the IMU reference point.

[0129] 3) At time T3, the IMU reference point is switched, the IMU reference value is the IMU value at t250, and the reference point is switched to B (IMU starts to calculate the integral displacement coordinates with B as the reference point). When it is greater than time T3, the reference point is ready to switch to base station C.

[0130] By calibrating the reference point of the IMU inertial unit 32, the IMU calculation starting coordinate point is regularly updated to eliminate the IMU integral cumulative error, thereby improving the overall positioning accuracy of the IMU.

[0131] In summary:

[0132] (1) The present invention's tightly integrated positioning method for underground mine vehicles based on UWB and IMU uses an underground UWB precise positioning system as a framework to fuse UWB measurement data and IMU measurement data, thereby improving the accuracy of vehicle positioning and providing highly reliable, low-latency positioning services for underground vehicle-assisted transportation and unmanned driving.

[0133] (2) The present invention is easy to modify and effectively reduces platform storage space usage and core computing time consumption in scenarios with limited computing resources. It has the characteristics of low system response delay, high robustness, simple structure, low construction cost and strong adaptability, effectively improving system operation accuracy and stability;

[0134] (3) The present invention inputs UWB measurement data and IMU measurement into a fusion algorithm model, and uses a calculation method based on UWB measurement data to distinguish between straight lines, curves, and blind spots, thereby further improving the positioning accuracy of underground vehicles.

[0135] (4) The present invention obtains a one-dimensional positioning solution for the vehicle by mapping and converting the distance information. Combined with the UWB measurement time, the reference point of the IMU inertial unit is selected, which can effectively eliminate the adverse effects of the IMU cumulative error.

[0136] Example 2: An embodiment of the present application provides a computer device, which includes a processor and a memory, wherein the memory stores at least one instruction or at least one program, and the at least one instruction or the at least one program is loaded and executed by the processor to implement a tightly combined positioning method for underground vehicles in mines based on UWB and IMU as provided in the above method embodiment.

[0137] FIG7 shows a schematic diagram of the hardware structure of a device for implementing a method for tightly combined positioning of vehicles in mines based on UWB and IMU provided in an embodiment of the present application. The device may participate in or include the device or system provided in an embodiment of the present application. As shown in FIG7 , the computer device 10 may include one or more processors 1002 (the processor may include but is not limited to a processing device such as a microprocessor MCU or a programmable logic device FPGA), a memory 1004 for storing data, and a transmission device 1006 for communication functions. In addition, it may also include: a display, an input / output interface (I / O interface), a universal serial bus (USB) port (which may be included as one of the ports of the I / O interface), a network interface, a power supply and / or a camera. It will be understood by those skilled in the art that the structure shown in FIG7 is merely illustrative and does not limit the structure of the above-mentioned electronic device. For example, the computer device 10 may also include more or fewer components than those shown in FIG7 , or have a configuration different from that shown in FIG7 .

[0138] It should be noted that the one or more processors and / or other data processing circuits described above may generally be referred to herein as "data processing circuitry". The data processing circuitry may be embodied in whole or in part as software, hardware, firmware, or any other combination thereof. In addition, the data processing circuitry may be a single independent processing module, or may be incorporated in whole or in part into any of the other components of the computer device 10 (or mobile device). As described in the embodiments of the present application, the data processing circuitry serves as a processor control (e.g., selection of a variable resistor terminal path connected to an interface).

[0139] The memory 1004 can be used to store software programs and modules of application software, such as the program instructions / data storage device corresponding to the method for tightly combined positioning of underground vehicles in mines based on UWB and IMU in an embodiment of the present application. The processor executes various functional applications and data processing by running the software programs and modules stored in the memory 1004, thereby implementing one of the above methods. The memory 1004 may include high-speed random access memory and may also include non-volatile memory, such as one or more magnetic storage devices, flash memory, or other non-volatile solid-state memory. In some examples, the memory 1004 may further include memory remotely located relative to the processor, and these remote memories may be connected to the computer device 10 via a network. Examples of the above-mentioned network include, but are not limited to, the Internet, an intranet, a local area network, a mobile communication network, and combinations thereof.

[0140] Transmission device 1006 is configured to receive or transmit data via a network. A specific example of the aforementioned network may include a wireless network provided by the communications provider of computer device 10. In one embodiment, transmission device 1006 includes a network interface controller (NIC), which can be connected to other network devices via a base station to enable communication with the Internet. In another embodiment, transmission device 1006 may be a radio frequency (RF) module configured to communicate with the Internet wirelessly.

[0141] The display may be, for example, a touch screen liquid crystal display (LCD) that enables a user to interact with a user interface of the computer device 10 (or mobile device).

[0142] Example 3: The embodiment of the present application also provides a computer-readable storage medium, which can be set in a server to store at least one instruction or at least one program related to a tightly combined positioning method for underground vehicles in mines based on UWB and IMU in a method embodiment. The at least one instruction or the at least one program is loaded and executed by the processor to implement a tightly combined positioning method for underground vehicles in mines based on UWB and IMU provided in the above method embodiment.

[0143] Optionally, in this embodiment, the storage medium may be located in at least one of a plurality of network servers in a computer network. Optionally, in this embodiment, the storage medium may include, but is not limited to, various media capable of storing program code, such as a USB flash drive, a read-only memory (ROM), a random access memory (RAM), a mobile hard disk, a magnetic disk, or an optical disk.

[0144] Embodiment 4: This embodiment of the present invention further provides a computer program product or computer program, which includes computer instructions stored in a computer-readable storage medium. A processor of a computer device reads the computer instructions from the computer-readable storage medium and executes the computer instructions, causing the computer device to perform the UWB and IMU-based tight-combination positioning method for underground mine vehicles provided in the various optional embodiments described above.

[0145] It should be noted that the order of the embodiments of the present application described above is for descriptive purposes only and does not represent the superiority or inferiority of the embodiments. The above description is of specific embodiments of the present application. Other embodiments are within the scope of the appended claims. In some cases, the actions or steps described in the claims can be performed in an order different from that in the embodiments and still achieve the desired results. In addition, the processes depicted in the accompanying drawings do not necessarily require the specific order or continuous order shown to achieve the desired results. In some embodiments, multitasking and parallel processing are also possible or may be advantageous.

[0146] The various embodiments in this application are described in a progressive manner. Similar portions between the various embodiments can be referred to in conjunction with each other. Each embodiment focuses on the differences between the other embodiments. In particular, the device, equipment, and storage medium embodiments are generally similar to the method embodiments, so their descriptions are relatively simple. For relevant portions, refer to the descriptions of the method embodiments.

[0147] Those skilled in the art will understand that all or part of the steps to implement the above embodiments may be accomplished by hardware, or by a program to instruct the relevant hardware, and the program may be stored in a computer-readable storage medium, which may be a read-only memory, a disk, or an optical disk, etc.

[0148] With the above-described preferred embodiments of the present invention as inspiration, and with reference to the above description, relevant personnel are fully capable of making various changes and modifications without departing from the technical scope of this invention. The technical scope of this invention is not limited to the contents of the specification and must be determined according to the scope of the claims.

Claims

1. A tightly combined positioning method for underground vehicles in mines based on UWB and IMU, characterized in that: The method is based on the downhole UWB precise positioning system as the framework; The underground UWB precise positioning system comprises: a host computer (1), a plurality of base stations (2) and a vehicle-mounted positioning device (3) installed on a vehicle, wherein the host computer (1) and the vehicle-mounted positioning device (3) are both connected to the base station (2) by signal, and the vehicle-mounted positioning device (3) comprises a UWB tag (31), an IMU inertial unit (32) and a terminal device (33), wherein the UWB tag (31) is connected to the base station (2) by signal, and the UWB tag (31) and the IMU inertial unit (32) are both connected to the terminal device (33); The method comprises the following steps: S1, the host computer (1) constructs a downhole map according to the position coordinates of all base stations (2) and establishes a base station database, wherein the base station database includes the base station ID and the coordinate information of each base station (2); S2, the host computer (1) sends the base station data information to the corresponding base station (2) for storage based on the base station ID; S3, the vehicle is located in an underground tunnel and moves. When the vehicle moves near any of the base stations (2), the UWB tag (31) requests a signal connection from the corresponding base station (2), the base station (2) transmits the base station data information to the terminal device (33), and the terminal device (33) obtains the distance information between the UWB tag (31) and the base station (2), as well as the inertial data output by the IMU inertial unit (32); The base station data information includes coordinate information of the base station (2) and base stations (2) adjacent to the base station (2) on both sides; S4, the terminal device (33) processes and calculates the distance information and the inertial data based on the base station data information to obtain the current position information of the vehicle.

2. The method for tightly combined positioning of vehicles in mines based on UWB and IMU as claimed in claim 1, characterized in that: In step S3, the distance information between the UWB tag (31) and the base station (2) includes the following steps: S311, the UWB tag (31) sends a ranging signal; S312, after receiving the ranging signal, the base station (2) calculates the distance to the UWB tag (31) using a two-way unilateral ranging method, and feeds back the distance calculation result to the UWB tag (31); S313, the terminal device (33) obtains the distance information fed back by each base station (2).

3. The method for tightly combined positioning of vehicles in mines based on UWB and IMU as claimed in claim 1, characterized in that: In step S3, the terminal device (33) acquires the inertial data output by the IMU inertial unit (32) and specifically comprises the following steps: S321, when the vehicle starts to move, the rolling angle of the vehicle is The pitch angle is γ, the heading angle is θ, and the IMU inertial unit (32) measures the angular velocity and acceleration of the vehicle in three axes in real time. According to the strapdown inertial navigation attitude update algorithm, the attitude conversion matrix C of the vehicle is calculated. According to the fixed axis rotation principle, the roll angle is calculated as The pitch angle is γ and the heading angle is θ: S322, rotating the navigation coordinate system of the IMU according to the heading angle to construct a spatial coordinate system of the underground UWB precise positioning system map; S323, calculating inertial data of the IMU inertial unit (32) in real time according to the IMU positioning model; Assume that the state value of the IMU positioning model is: The mathematical expression of the IMU positioning model table is: k =f(X k-1 ,ω k ); in, Where: k is the current time, p k is the position at time k; v k is the speed at time k; q k is the quaternion at time k; a k is the acceleration at time k; g is the acceleration due to gravity; Ω k is the four Element update matrix; Represents the rotation matrix at time k.

4. The method for tightly combined positioning of vehicles in mines based on UWB and IMU as claimed in claim 3, characterized in that: In step S4, the terminal device (33) processes and calculates the distance information and the inertial data based on the base station data information, including the following steps: S41, the UWB tag (31) card periodically outputs the coordinate position, and the terminal device (33) obtains the base station data information of the current road section of the tag card and the distance information at time T; S42, the IMU inertial unit (32) outputs the inertial data accumulated starting from time T-1; S43, combining the IMU weighted parameters, inputting the base station data information of the current road section, the distance information and the inertial data into the fusion algorithm model for processing, and obtaining the position information of the vehicle at time T.

5. The method for tightly combined positioning of vehicles in mines based on UWB and IMU as claimed in claim 4, characterized in that: In step S43, the fusion algorithm model includes a straight lane algorithm, a curve algorithm and a blind spot algorithm, and the algorithm call specifically includes: If the vehicle is not in the UWB signal coverage area, a blind spot algorithm is called, and the terminal device (33) calculates the position information of the vehicle based on the inertial data; If the vehicle is in the UWB signal coverage area, assume that the vehicle is between the first base station and the second base station, obtain the distance information d1 between the vehicle and the first base station, and the distance information d2 between the vehicle and the second base station, calculate the straight-line distance D between the first base station and the second base station based on the base station data information, and calculate the ratio of the coordinate distance and distance information between the two base stations. The calculation formula is: β=(d1+d2) / D; If β is between 95% and 105%, a straight line algorithm is called to calculate the position information of the vehicle according to the distance information and the base station data information; If β is greater than 105%, the curve algorithm is called, the IMU weighted parameter value is β, and the position information of the vehicle is calculated based on the distance information and the inertial data.

6. The method for tightly combined positioning of underground vehicles in mines based on UWB and IMU as claimed in claim 5, characterized in that: Calculate the vehicle's position information P at time T based on the distance information and the base station data information T The calculation formula is: Where: P T-1 is the vehicle position coordinate at time T-1, It is the direction vector of the current UWB tag movement.

7. The method for tightly combined positioning of vehicles in mines based on UWB and IMU as claimed in claim 5, characterized in that: The calculation of the curve algorithm includes the following steps: Step A, calculating the first coordinate P of the vehicle based on the first distance information, the second distance information and the heading angle t1 , the calculation formula is: in, Represents the vehicle position coordinate P at time T-1 T-1 is the starting point reference point, the vehicle driving vector obtained based on the heading angle, ⊙ LR The first base station and the second base station measure the distance information to calculate the plane where the vehicle is located, and obtain the first coordinate P by calculating the intersection of the vehicle driving vector and the plane. t1 ; Step B: Based on the IMU quadratic integral mathematical model, according to the vehicle position coordinates P at time T-1 T-1 The target prediction coordinates P at the current moment are calculated based on the vehicle starting point, the vehicle's own speed, and the vehicle acceleration measured by the IMU. t2 , the calculation formula is: Where v(τ) represents the vehicle’s own speed, ΔT represents the time interval between two measurements by the base station, ω(τ) represents the vehicle’s angular velocity measured by the IMU, and R ω(τ) represents the rotation matrix calculated based on the vehicle angular velocity, and a(τ) represents the vehicle acceleration measured by the IMU; Step C, the first coordinate P t1 and the current target prediction coordinates P t2 Fusion calculation, get the vehicle position information P at time T T , the calculation formula is: P T =P t2 +K(β)(P t1 -HP t2 ); Among them, K(β) represents the weight calculated using the curve ratio β, and H is the state vector used to represent P t1 With P t2 The mapping relationship between them.

8. The method for tightly combined positioning of underground vehicles in mines based on UWB and IMU as claimed in claim 1, characterized in that: The reference point of the IMU inertial unit (32) is also calibrated, specifically comprising the following steps: Map and convert the distance information to obtain the mapped distance of the vehicle; The reference point of the inertial unit is selected according to the UWB measurement time and the mapping distance of the vehicle, and the IMU inertial unit (32) is calibrated.

9. A computer device, characterized in that: include: processor; A memory for storing executable instructions; The processor is used to read the executable instructions from the memory and execute the executable instructions to implement the tightly combined positioning method for underground vehicles in mines based on UWB and IMU as described in any one of claims 1 to 8.

10. A computer-readable storage medium, characterized in that: The computer-readable storage medium stores a computer program, and when the computer program is executed by a processor, the processor implements the tightly combined positioning method for underground mine vehicles based on UWB and IMU as described in any one of claims 1 to 8.

Images