Underground vehicle fusion positioning method based on UWB and IMU

Abstract

The invention relates to the technical field of underground positioning, in particular to an underground vehicle fusion positioning method based on UWB and IMU. The method comprises the following steps: arranging a vehicle-mounted positioning device on a vehicle, wherein the vehicle-mounted positioning device comprises a UWB tag, an IMU inertial unit, a vehicle-mounted domain controller and a wireless communication module; an antenna of the UWB tag is arranged at the top of the vehicle; deploying a plurality of UWB base station devices in an underground roadway; the UWB tag sends a ranging signal, after each UWB base station receives the ranging signal, the distance between each UWB base station and the UWB tag is calculated by adopting a TW-ToF method, and a distance calculation result is fed back to the UWB tag, and the UWB tag reads the distance calculation result fed back by each UWB base station, determines the nearest base station, obtains base station data and sends the base station data to the vehicle-mounted domain controller; the vehicle-mounted domain controller obtains inertial data output by the IMU inertial unit; and processing the base station data and the inertial data by adopting an iterative extended Kalman filter based on an error state, and fusing the base station data and the inertial data to generate a fused positioning result. The drifting error of the IMU accumulated along with time is reduced, and the positioning accuracy, robustness and real-time performance are improved.

Description

Technical Field

[0001] The present invention relates to the technical field of underground positioning, and in particular to an underground vehicle fusion positioning method based on UWB and IMU. Background Art

[0002] With the continuous advancement of coal mining technology, the underground environment has become more complex, and traditional manual operations are facing safety risks and efficiency bottlenecks. Therefore, the use of advanced positioning technology to accurately position transport vehicles and mine robots has become the key to improving the safety and efficiency of underground operations. At present, the underground operating environment usually lacks GNSS signals, the environmental characteristics are single, and the tunnels are narrow and complex, resulting in insufficient positioning accuracy. To meet this challenge, researchers have gradually turned to inertial navigation systems, ultra-wideband and other technologies to achieve high-precision positioning services.

[0003] The mainstream positioning method currently used underground is ultra-wideband (UWB) technology, which can achieve rough positioning of underground vehicles through the transmission of short-distance high-frequency signals. UWB signals have good penetration ability and can maintain good positioning effects even in multi-obstacle environments, making them suitable for narrow and complex underground environments. However, when UWB is deployed over a large area, its signal attenuation may affect the positioning accuracy, especially in the case of complex mine structures. The positioning effect of using UWB as the only sensor is not ideal. Therefore, UWB technology is currently mainly used by the dispatch center on the surface to monitor and locate underground vehicles, and cannot achieve precise positioning.

[0004] The inertial navigation system (INS) has a gyroscope and accelerometer inside, which can measure the angular velocity and acceleration of the carrier. By integrating the time, the vehicle speed and attitude information can be obtained, and the vehicle position information can be inferred. The system can operate independently without external signals, is not restricted by environmental conditions, and is suitable for positioning in dynamic and irregular environments. By directly measuring acceleration and angular velocity, the real-time data of INS is updated quickly, which is suitable for fast-moving application scenarios and for positioning vehicles in environments without GNSS signals such as coal mines. However, this system belongs to the dead reckoning navigation method, which requires the known starting point position to calculate the position of the next point of the vehicle, and obtains real-time position information through continuous measurement. A single inertial navigation system cannot accurately position the vehicle for a long time, and usually needs to cooperate with other types of navigation systems such as GPS or Beidou satellite navigation system to form a combined navigation system to achieve accurate positioning.

[0005] Underground tunnels lack GNSS signals, and the features are single and sparse. The use of a single sensor cannot meet the positioning accuracy requirements of underground vehicles. Although existing technologies have improved the positioning accuracy and reliability of underground transport vehicles and robots to a certain extent, they still face challenges in real-time adaptability, system integration, and anti-interference capabilities in dynamic environments. Summary of the invention

[0006] The present invention aims to solve at least one of the technical problems existing in the prior art.

[0007] To this end, the present invention provides an underground vehicle fusion positioning method based on UWB and IMU, which improves the real-time positioning accuracy of vehicles in mine tunnels, reduces the drift error accumulated by IMU over time, and improves the robustness and real-time performance of positioning.

[0008] According to an embodiment of the present invention, a method for underground vehicle fusion positioning based on UWB and IMU includes the following steps:

[0009] S1, installing a vehicle-mounted positioning device on a vehicle, wherein the vehicle-mounted positioning device includes a UWB tag, an IMU inertial unit, a vehicle-mounted domain controller and a wireless communication module; the antenna of the UWB tag is arranged on the top of the vehicle;

[0010] S2, deploy several UWB base station devices in the underground tunnel. When deploying, deploy a UWB starting base station at the tunnel entrance, and deploy several UWB intermediate base stations in the tunnel based on the UWB starting base station. Measure the distance from each UWB intermediate base station to the UWB starting base station, input the base station ID and coordinate information into the host computer and send it to each UWB base station, and establish the global two-dimensional coordinate system G in the mine;

[0011] S3, the UWB tag sends a ranging signal. After receiving the ranging signal, each UWB base station uses the TW-ToF method to calculate the distance to the UWB tag and feeds back the distance calculation result to the UWB tag. The UWB tag determines the nearest base station based on the distance to each UWB base station, obtains the base station data and sends it to the vehicle domain controller;

[0012] S4, the vehicle-mounted domain controller obtains inertial data output by the IMU inertial unit;

[0013] S5, using an iterative extended Kalman filter based on error state to process the base station data and inertial data, and fuse them to generate a fused positioning result;

[0014] Wherein, step S5 comprises:

[0015] S51, using the inertial data and the posture data at the previous moment as input data of the iterative extended Kalman filter;

[0016] S52, constructing an error state equation based on the error state variable to predict the state at the next moment;

[0017] S53, using the calibrated and filtered base station data as observation data, establishing an observation equation, updating and correcting the error state variable, iteratively calculating the Kalman gain matrix, and updating the error state equation until convergence, calculating the posterior covariance matrix, and outputting the fused positioning result.

[0018] The beneficial effect of the present invention is that the underground vehicle fusion positioning method based on UWB and IMU of the present invention obtains base station data through UWB tags and UWB base stations, and calibrates and screens them to obtain more accurate real-time base station data as vehicle position observation information, then obtains the error state of the IMU inertial unit, predicts and updates the error state of the inertial navigation data, and uses an iterative extended Kalman filter based on error state variables to complete the fusion of base station data and inertial data, thereby improving the accuracy and robustness of underground vehicle positioning. In addition, the error state equation of the iterative extended Kalman filter is constructed through error state variables, which simplifies the calculation process, and while ensuring the positioning accuracy, it also improves the system operation efficiency, thereby enabling the underground vehicle to be quickly positioned.

[0019] According to one embodiment of the present invention, in step S52, the error state variable is expressed as:

[0020]

[0021] Where P t , v t ,R t Represent the position, velocity and attitude transformation matrix of the vehicle in the global coordinate system, Represents the IMU's gyro bias and acceleration bias, g t is the gravity of the IMU;

[0022] The error state equation is expressed as:

[0023] x t =f(x t-1 ,u t ,w t )

[0024] Among them, x t-1 is the optimal estimate at the previous moment, u t Represents the system input vector at the current moment, w t is the system noise.

[0025] The error state linearization equation is expressed as:

[0026] x t =F t-1 x t-1 +B t-1 w t-1

[0027]

[0028] Among them, P t - is the system prior state covariance matrix, Q w Represents the covariance matrix corresponding to the system noise, F t-1 ,B t-1 is the Jacobian matrix of the linearized state equation at the previous moment, w t-1 is the system noise at the previous moment, P t-1 is the covariance matrix of the system at the previous moment.

[0029] According to one embodiment of the present invention, in step S53, the calibration of the base station data performs external parameter calibration on the UWB and IMU based on the global coordinate system, and the UWB calibration model is:

[0030]

[0031] Among them, P UWB is the result measured by the UWB vehicle identification card, d is the true value distance, k is the proportional coefficient, is the offset; ψ is the standard distribution of random noise;

[0032] After the calibration is completed, an outlier filtering mechanism is set up to filter the calibrated base station data.

[0033] According to one embodiment of the present invention, the observation equation is expressed as:

[0034]

[0035] Among them, y t To calibrate the filtered UWB data, is the UWB observation noise, and the posterior mean of the observation equation in the previous iteration is After linearization, it can be expressed as:

[0036]

[0037] Among them, G t ,C t is the Jacobian matrix of the linearized observation equation.

[0038] According to one embodiment of the present invention, the error state equation at the next moment is updated based on the error state equation and the observation equation, which is expressed as:

[0039]

[0040] P t + =(IKt G t ) t -

[0041] Among them, K t is the Kalman gain matrix, R t is the covariance matrix corresponding to the system observation noise, I is the unit matrix, is the prior mean at the current moment, is the posterior mean of the previous iteration, for Corresponding observations.

[0042] According to an embodiment of the present invention, setting an outlier filtering mechanism to filter the calibrated base station data specifically includes:

[0043] By setting the threshold Th, the calibrated base station data is screened. If it is an abnormal value, it will be filtered out. Th is obtained by the following formula:

[0044] Th=αv max / f

[0045] Where α is the filtering factor, v max is the maximum vehicle speed, f is the UWB tag measurement frequency, and is the calibrated base station data at time t, is the base station data at time t+1, if If the calibrated base station data is abnormal, it is considered to be outliers and is filtered out; otherwise, the data is considered normal and is used as observation data.

[0046] According to one embodiment of the present invention, the vehicle-mounted domain controller transmits the generated fusion positioning result to the ground positioning platform for display via the wireless communication module.

[0047] A computer device comprising:

[0048] processor;

[0049] A memory for storing executable instructions;

[0050] Wherein, the processor is used to read the executable instructions from the memory and execute the executable instructions to implement the underground vehicle fusion positioning method based on UWB and IMU as described above.

[0051] A computer-readable storage medium stores a computer program. When the computer program is executed by a processor, the processor implements the underground vehicle fusion positioning method based on UWB and IMU as described above.

[0052] Other features and advantages of the present invention will be described in the following description, and partly become apparent from the description, or understood by practicing the present invention. The purpose and other advantages of the present invention are realized and obtained by the structures particularly pointed out in the description, claims and drawings.

[0053] In order to make the above-mentioned objects, features and advantages of the present invention more obvious and easy to understand, preferred embodiments are given below and described in detail with reference to the accompanying drawings. BRIEF DESCRIPTION OF THE DRAWINGS

[0054] The present invention is further described below in conjunction with the accompanying drawings and embodiments.

[0055] Figure 1 It is a schematic diagram of the method flow of embodiment 1 of the present invention.

[0056] Figure 2 It is a schematic diagram of the system structure of the first embodiment of the present invention.

[0057] Figure 3 It is a schematic diagram of the underground global two-dimensional coordinate system G of the first embodiment of the present invention.

[0058] Figure 4 It is a schematic diagram of the computer device structure of the second embodiment of the present invention.

[0059] In the figure, 10 is a computer device; 1002 is a processor; 1004 is a memory; and 1006 is a transmission device. DETAILED DESCRIPTION

[0060] The present invention will now be described in further detail with reference to the accompanying drawings. These drawings are simplified schematic diagrams, which only illustrate the basic structure of the present invention in a schematic manner, and therefore only show the components related to the present invention.

[0061] 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 positions or positional relationships based on the positions 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 a limitation on the present invention. In addition, features defined as "first" and "second" may explicitly or implicitly include one or more of the features. In the description of the present invention, unless otherwise specified, "multiple" means two or more.

[0062] In the description of the present invention, it should be noted that, unless otherwise clearly specified and limited, the terms "installed", "connected", and "connected" should be understood in a broad sense, for example, it can be a fixed connection, a detachable connection, or an integral connection; it can be a mechanical connection or an electrical connection; it can be a direct connection, or it can be indirectly connected through an intermediate medium, or it can be the internal communication of two components. For ordinary technicians in this field, the specific meanings of the above terms in the present invention can be understood according to specific circumstances.

[0063] Example 1

[0064] The present application embodiment provides a method for underground vehicle fusion positioning based on UWB and IMU, such as Figure 1-3 As shown, the method comprises the following steps:

[0065] S1, installing a vehicle-mounted positioning device on the vehicle, the vehicle-mounted positioning device includes a UWB tag, an IMU inertial unit, a vehicle-mounted domain controller and a wireless communication module, and the antenna of the UWB tag is arranged on the top of the vehicle;

[0066] S2, deploy several UWB base station devices in the underground tunnel. When deploying, deploy a UWB starting base station at the entrance of the tunnel. With the UWB starting base station as the reference, deploy several UWB intermediate base stations in the tunnel. Measure the distance from each UWB intermediate base station to the UWB starting base station, input the base station ID and coordinate information into the host computer and send it to each UWB base station, and establish a global two-dimensional coordinate system G underground. Establish the underground global two-dimensional coordinate system G with the UWB starting base station as the y-axis starting point, the direction from the starting base station to the next base station as the y-axis direction, obtain the height of the vehicle identification card, take the horizontal height of the vehicle identification card as the starting point, and the vertical upward direction as the x-axis direction. Figure 3 shown.

[0067] S3, the UWB tag sends a ranging signal. After each UWB base station receives the ranging signal, it uses the TW-ToF method to calculate the distance to the UWB tag and feeds back the distance calculation result to the UWB tag. The UWB tag determines the nearest base station based on the distance to each UWB base station, obtains the base station data and sends it to the vehicle-mounted domain controller. The UWB tag obtains the base station data in real time, which reduces the calculation time of the system and improves the real-time positioning of the underground vehicle.

[0068] S4, the vehicle domain controller obtains the inertial data output by the IMU inertial unit;

[0069] S5, using an iterative extended Kalman filter based on error state to process the base station data and inertial data, and fuse them to generate a fused positioning result;

[0070] Wherein, step S5 comprises:

[0071] S51, using the inertial data and the posture data at the previous moment as input data of the iterative extended Kalman filter;

[0072] S52, construct an error state equation based on the error state variable to predict the state at the next moment; the error state variable is expressed as:

[0073]

[0074] Where P t , v t ,R t Represent the position, velocity and attitude transformation matrix of the vehicle in the global coordinate system, Represents the IMU's gyro bias and acceleration bias, g t is the gravity of the IMU;

[0075] The error state variable can be defined as:

[0076] P t =P+δP

[0077] v t =v+δv

[0078] R t =RδR

[0079]

[0080] g t =g+δg

[0081] The variables without subscript t are nominal state variables, that is, the original state variables without considering the error variables, δ represents the error, and its discrete kinematic model is:

[0082] δP t =δP+δvdt

[0083] δv t =δv+(-R(ab a ) ∧ δθ-Rδb a +δg)dt+n a

[0084] δθ t =Exp(-(ω-b ω )dt)δθ-δb ω dt-n ω

[0085]

[0086] δg t =δg

[0087] Where, ω,a is the gyroscope and acceleration output of the IMU, Exp(δθ) is the Lie algebra of δR, n ω ,n a is the gyroscope and acceleration Gaussian white noise, n bω ,n ba is the random walk of the gyroscope and the acceleration. Therefore, the error state equation can be expressed as:

[0088] x t =f(x t-1 ,u t ,w t )

[0089] Among them, x t-1 is the optimal estimate at the previous moment, u t Represents the system input vector at the current moment, w t is the system noise.

[0090] The error state linearization equation is expressed as:

[0091] x t =F t-1 x t-1 +B t-1 w t-1

[0092]

[0093] Among them, P t - is the system prior state covariance matrix, Q w Represents the covariance matrix corresponding to the system noise, w t-1 is the system noise at the previous moment, P t-1 is the covariance matrix of the system at the previous moment, F t-1 ,B t-1 is the Jacobian matrix of the linearized state equation at the previous moment, which can be expressed as:

[0094]

[0095] S53, using the calibrated and filtered base station data as observation data, establishing the observation equation, updating and correcting the error state variables, iteratively calculating the Kalman gain matrix, and updating the error state equation until convergence, calculating the posterior covariance matrix, and outputting the fusion positioning result. Using UWB base station data as observation data can correct IMU data in real time, solve its long-term positioning drift problem, and improve the positioning accuracy of underground vehicles;

[0096] Furthermore, by calibrating and screening the base station data, the base station data obtained is more accurate. The calibration of the base station data is based on the global coordinate system to calibrate the external parameters of UWB and IMU. The UWB calibration model is:

[0097]

[0098] Among them, P UWB is the result measured by the UWB vehicle identification card, d is the true value distance, k is the proportional coefficient, is the offset; ψ is the standard distribution of random noise;

[0099] It should be noted that the true distance d is a distance measured by, but not limited to, a laser rangefinder or a total station, and the true distance will be stored after measurement;

[0100] The calculation formula of the proportionality coefficient k is:

[0101]

[0102] Among them, d i Refers to the true distance measured by the i-th group of total stations, is the average value of m sets of true value results, P i UWB represents the i-th group of base station data, is the average value of m groups of base station data.

[0103] Offset The calculation formula is:

[0104]

[0105] The standard distribution ψ of random noise can be calculated as follows:

[0106]

[0107] Jointly calibrate the external parameters of UWB and IMU to obtain the geometric position coordinates of the IMU center in the global coordinate system The pose coordinates generated by IMU in its own coordinate system According to the equation The translation transformation matrix R can be obtained IU , which is convenient for converting the IMU pose to the global coordinate system.

[0108] In order to further improve the positioning accuracy, after the calibration is completed, an outlier filtering mechanism is set up to filter the calibrated base station data. Specifically, it includes:

[0109] By setting the threshold Th, the calibrated base station data is screened. If it is an abnormal value, it will be filtered out. Th is obtained by the following formula:

[0110] Th=αv max / f

[0111] Where α is the filtering factor, v max is the maximum vehicle speed, f is the UWB tag measurement frequency, and is the calibrated base station data at time t, is the base station data at time t+1, if If the calibrated base station data is abnormal, it is considered to be outliers and is filtered out; otherwise, the data is considered normal and is used as observation data.

[0112] The observation equation can be expressed as:

[0113]

[0114] Among them, y t To calibrate the filtered UWB data, is the UWB observation noise, and the posterior mean of the observation equation in the previous iteration is After linearization, it can be expressed as:

[0115]

[0116] Among them, G t ,C t is the Jacobian matrix of the linearized observation equation, which can be obtained by the chain rule:

[0117]

[0118] In this example, the error state equation at the next moment is updated based on the error state equation and the observation equation, which is expressed as:

[0119]

[0120] P t + =(IK t G t ) t -

[0121] Among them, K t is the Kalman gain matrix, R t is the covariance matrix corresponding to the system observation noise, I is the unit matrix, is the prior mean at the current moment, is the posterior mean of the previous iteration, for Corresponding observations.

[0122] In this example, since valid IMU data cannot be collected when the vehicle is not moving, the base station data is used as the initial data input to the iterative extended Kalman filter.

[0123] In an embodiment, the vehicle-mounted domain controller transmits the generated fusion positioning result to the ground positioning platform for display via the wireless communication module.

[0124] In summary, the underground vehicle fusion positioning method based on UWB and IMU of the present invention obtains base station data through UWB tags and UWB base stations, and calibrates and screens them to obtain more accurate real-time base station data as the vehicle's position observation information, and then obtains the error state of the IMU inertial unit, predicts and updates the error state of the inertial navigation data, and uses an iterative extended Kalman filter based on error state variables to complete the fusion of base station data and inertial data, thereby improving the accuracy and robustness of underground vehicle positioning. In addition, the error state equation of the iterative extended Kalman filter is constructed through error state variables, which simplifies the calculation process, and while ensuring the positioning accuracy, it also improves the system operation efficiency, thereby enabling the underground vehicle to be quickly positioned.

[0125] Example 2

[0126] 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 an underground vehicle fusion positioning method based on UWB and IMU as provided in the above method embodiment.

[0127] Figure 4 The hardware structure diagram of a device for implementing a method for fusion positioning of underground vehicles based on UWB and IMU provided in an embodiment of the present application is shown. The device may participate in constituting or include the device or system provided in an embodiment of the present application. Figure 4 As shown, 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 can be understood by those skilled in the art that Figure 4 The structure shown is only for illustration and does not limit the structure of the above electronic device. Figure 4 More or fewer components as shown, or with Figure 4Different configurations are shown.

[0128] 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 circuits". The data processing circuits may be embodied in whole or in part as software, hardware, firmware, or any other combination thereof. In addition, the data processing circuit may be a single independent processing module, or may be incorporated in whole or in part into any of the other components in the computer device 10 (or mobile device). As described in the embodiments of the present application, the data processing circuit acts as a processor control (e.g., selection of a variable resistor terminal path connected to an interface).

[0129] The memory 1004 can be used to store software programs and modules of application software, such as a program instruction / data storage device corresponding to a method for underground vehicle fusion positioning 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, that is, implementing the above-mentioned method. The memory 1004 may include a high-speed random access memory, and may also include a non-volatile memory, such as one or more magnetic storage devices, flash memory, or other non-volatile solid-state memory. In some instances, the memory 1004 may further include a memory remotely arranged 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.

[0130] The transmission device 1006 is used to receive or send data via a network. The specific example of the above network may include a wireless network provided by a communication provider of the computer device 10. In one example, the transmission device 1006 includes a network adapter (Network Interface Controller, NIC), which can be connected to other network devices through a base station so as to communicate with the Internet. In one example, the transmission device 1006 can be a radio frequency (RF) module, which is used to communicate with the Internet wirelessly.

[0131] 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).

[0132] Example 3

[0133] An 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 an underground vehicle fusion positioning method 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 an underground vehicle fusion positioning method based on UWB and IMU provided in the above method embodiment.

[0134] Optionally, in this embodiment, the storage medium may be located in at least one of the multiple network servers of the computer network. Optionally, in this embodiment, the storage medium may include, but is not limited to, various media that can store program codes, 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.

[0135] Example 4

[0136] The embodiment of the present invention also provides a computer program product or a computer program, which includes a computer instruction stored in a computer-readable storage medium. The processor of the computer device reads the computer instruction from the computer-readable storage medium, and the processor executes the computer instruction, so that the computer device executes a method for underground vehicle fusion positioning based on UWB and IMU provided in the above various optional embodiments.

[0137] It should be noted that the above-mentioned sequence of the embodiments of the present application is for description only and does not represent the advantages and disadvantages of the embodiments. The above-mentioned specific embodiments of the present application are described. Other embodiments are within the scope of the appended claims. In some cases, the actions or steps recorded 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 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.

[0138] Each embodiment in this application is described in a progressive manner, and the same or similar parts between the embodiments can be referred to each other, and each embodiment focuses on the differences from other embodiments. In particular, for the device, equipment and storage medium embodiments, since they are basically similar to the method embodiments, the description is relatively simple, and the relevant parts can be referred to the partial description of the method embodiment.

[0139] A person 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 instructing related hardware through a program, and the program may be stored in a computer-readable storage medium, and the above-mentioned storage medium may be a read-only memory, a disk or an optical disk, etc.

[0140] Based on the above ideal embodiments of the present invention, the relevant staff can make various changes and modifications without departing from the technical concept of the present invention through the above description. The technical scope of the present invention is not limited to the contents of the specification, and its technical scope must be determined according to the scope of the claims.

Claims

1. A method for underground vehicle fusion positioning based on UWB and IMU, characterized in that: The method comprises the following steps: S1, installing a vehicle-mounted positioning device on a vehicle, wherein the vehicle-mounted positioning device includes a UWB tag, an IMU inertial unit, a vehicle-mounted domain controller and a wireless communication module; the antenna of the UWB tag is arranged on the top of the vehicle; S2, deploy several UWB base station devices in the underground tunnel. When deploying, deploy a UWB starting base station at the tunnel entrance, and deploy several UWB intermediate base stations in the tunnel based on the UWB starting base station. Measure the distance from each UWB intermediate base station to the UWB starting base station, input the base station ID and coordinate information into the host computer and send it to each UWB base station, and establish the global two-dimensional coordinate system G in the mine; S3, the UWB tag sends a ranging signal. After receiving the ranging signal, each UWB base station uses the TW-ToF method to calculate the distance to the UWB tag and feeds back the distance calculation result to the UWB tag. The UWB tag determines the nearest base station based on the distance to each UWB base station, obtains the base station data and sends it to the vehicle domain controller; S4, the vehicle-mounted domain controller obtains inertial data output by the IMU inertial unit; S5, using an iterative extended Kalman filter based on error state to process the base station data and inertial data, and fuse them to generate a fused positioning result; Wherein, step S5 comprises: S51, using the inertial data and the posture data at the previous moment as input data of the iterative extended Kalman filter; S52, constructing an error state equation based on the error state variable to predict the state at the next moment; S53, using the calibrated and filtered base station data as observation data, establishing an observation equation, updating and correcting the error state variable, iteratively calculating the Kalman gain matrix, and updating the error state equation until convergence, calculating the posterior covariance matrix, and outputting the fused positioning result.

2. The underground vehicle fusion positioning method based on UWB and IMU as claimed in claim 1, characterized in that: In step S52, the error state variable is expressed as: Where P t , v t ,R t Represent the position, velocity and attitude transformation matrix of the vehicle in the global coordinate system, Represents the IMU's gyro bias and acceleration bias, g t is the gravity of the IMU; The error state equation is expressed as: x t =f(x t-1 ,u t ,w t ) Among them, x t-1 is the optimal estimate at the previous moment, u t Represents the system input vector at the current moment, w t is the system noise; The error state linearization equation is expressed as: x t =F t-1 x t-1 +B t-1 w t-1 Among them, P t - is the system prior state covariance matrix, Q w Represents the covariance matrix corresponding to the system noise, F t-1 ,B t-1 is the Jacobian matrix of the linearized state equation at the previous moment, w t-1 is the system noise at the previous moment, P t-1 is the covariance matrix of the system at the previous moment.

3. The underground vehicle fusion positioning method based on UWB and IMU as claimed in claim 1, characterized in that: In step S53, the calibration of the base station data is based on the global coordinate system to perform external parameter calibration on UWB and IMU, and the UWB calibration model is: Among them, P UWB is the result measured by the UWB vehicle identification card, d is the true value distance, k is the proportional coefficient, is the offset; ψ is the standard distribution of random noise; After the calibration is completed, an outlier filtering mechanism is set up to filter the calibrated base station data.

4. The underground vehicle fusion positioning method based on UWB and IMU as claimed in claim 3 is characterized in that: The observation equation is expressed as: Among them, y t To calibrate the filtered UWB data, is the UWB observation noise, and the posterior mean of the observation equation in the previous iteration is After linearization, it can be expressed as: Among them, G t ,C t is the Jacobian matrix of the linearized observation equation.

5. The underground vehicle fusion positioning method based on UWB and IMU as claimed in claim 4 is characterized in that: Based on the error state equation and the observation equation, the error state equation at the next moment is updated, which is expressed as: P t + =(I-K t G t )P t - Among them, K t is the Kalman gain matrix, R t is the covariance matrix corresponding to the system observation noise, I is the unit matrix, is the prior mean at the current moment, is the posterior mean of the previous iteration, for Corresponding observations.

6. The underground vehicle fusion positioning method based on UWB and IMU as claimed in claim 3, characterized in that: Setting an outlier filtering mechanism to filter the calibrated base station data specifically includes: By setting the threshold Th, the calibrated base station data is screened. If it is an abnormal value, it will be filtered out. Th is obtained by the following formula: Th=αv max / f Where α is the filtering factor, v max is the maximum vehicle speed, f is the UWB tag measurement frequency, and is the calibrated base station data at time t, is the base station data at time t+1, if If the calibrated base station data is abnormal, it is considered to be outliers and is filtered out; otherwise, the data is considered normal and is used as observation data.

7. The underground vehicle fusion positioning method based on UWB and IMU as claimed in claim 1, characterized in that: The vehicle-mounted domain controller transmits the generated fusion positioning result to the ground positioning platform for display through the wireless communication module.

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

9. A computer-readable storage medium, characterized in that: The computer-readable storage medium stores a computer program. When the computer program is executed by a processor, the processor implements the underground vehicle fusion positioning method based on UWB and IMU as described in any one of claims 1 to 7.

Images