A fusion positioning method for underground rubber-wheeled vehicles based on UWB and IMU

Abstract

The present invention relates to the field of fusion positioning technology, and in particular to a fusion positioning method for an underground rubber-wheeled vehicle based on UWB and IMU, comprising: S100, setting a vehicle-mounted positioning processing device; S200, deploying a UWB base station; S300, a UWB tag sending a ranging signal; an on-board computer reading the distance calculation results fed back by each UWB base station and sorting each UWB base station; S400, storing inertial data in a double-ended queue; S500, based on the sorting result, selecting two UWB base stations as reference base stations, performing UWB one-dimensional positioning solution, and outputting the UWB positioning solution result of the UWB tag; S600, based on the inertial data in the double-ended queue, using a mechanical arrangement algorithm to perform IMU one-dimensional positioning solution, and generating an IMU mechanical arrangement solution result; S700, fusing the UWB positioning solution result and the IMU mechanical arrangement solution result, and generating a fusion positioning result. The adoption of this solution improves the real-time positioning performance and realizes the precise positioning of the rubber-wheeled vehicle while driving in a tunnel.

Description

Technical Field

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

[0002] Coal transportation is an integral part of underground coal mining operations. With the development of the times and technological advancements, rubber-tyred trackless vehicles, owing to their autonomous operation and flexibility, have become an emerging mode of underground transportation and have been rapidly adopted and adopted in my country. At the same time, ensuring the safe, stable, efficient, and intelligent operation and dispatch of these vehicles has become a pressing challenge within the industry. Real-time and precise positioning of these vehicles in underground environments is fundamental to their autonomous operation.

[0003] In recent years, UWB technology has been widely used for positioning in various indoor environments due to its advantages, including simple system implementation, strong multipath resolution, and high positioning accuracy. IMU positioning systems integrate sensors such as accelerometers, gyroscopes, and magnetometers, offering fully autonomous navigation, high data update rates, high short-term positioning accuracy, and immunity to external interference. Effectively integrating these two positioning technologies to improve the accuracy and reliability of rubber-tyred vehicles in tunnels remains a critical challenge. Underground tunnels are unique indoor environments with significantly different environmental conditions than those found in ordinary buildings. Conventional indoor positioning typically uses two-dimensional or three-dimensional positioning, targeting objects that are typically moving in a plane or space. However, tunnels are often long and narrow passages with known geometric position information. Using one-dimensional positioning to accurately determine the relative position of a rubber-tyred vehicle along the tunnel's extension can meet the required positioning accuracy. This system requires fewer devices, is simpler and more reliable to maintain, and is less expensive. However, a comprehensive and practical one-dimensional positioning solution for vehicles in tunnels is currently lacking in China.

[0004] Patent publication number CN114323003A discloses a high-precision positioning system and method for unmanned mine vehicles. This system utilizes four components: UWB tags, UWB anchors, underground communication access points (APs), and beacons to locate vehicles underground in mines. However, the system's adaptability to various scenarios is limited, and there is no detailed solution for deploying UWB base stations or switching base stations during vehicle movement.

[0005] Patent publication number CN113286360A discloses a fused positioning method for underground mining based on UWB, IMU, and LiDAR. This invention integrates positioning information from three dimensions: UWB, IMU, and LiDAR to construct an EKF fusion positioning model to achieve positioning for unmanned vehicles underground. However, this method fails to account for potential NLOS (non-local operating system) scenarios during underground positioning, nor does it account for the significant impact of underground dust on LiDAR. This results in low reliability of the positioning results and low vehicle positioning accuracy.

[0006] Patent publication number CN114166221B discloses a method and system for positioning an auxiliary transport robot in a dynamic and complex mine environment. This invention obtains the auxiliary transport robot's acceleration information and UWB distance measurement information, uses an extended Kalman filter to estimate the robot's position, and performs outlier detection on the UWB measurement. If the UWB measurement is abnormal, the extended Kalman filter is modified by correcting the innovation covariance matrix to improve positioning accuracy. However, this invention fails to take into account the specificities of the underground environment, resulting in a short test length and no final presentation of positioning results. Summary of the Invention

[0007] The present invention provides a fusion positioning method for underground rubber-wheeled vehicles based on UWB and IMU, which eliminates the adverse effects of IMU cumulative errors, realizes dynamic switching of UWB reference base stations, improves the real-time positioning, has the ability to resist UWB random measurement errors and NLOS errors, and can effectively adapt to the precise positioning of rubber-wheeled vehicles when driving in tunnels.

[0008] In order to achieve the above objectives, this application provides the following technical solutions:

[0009] A UWB and IMU-based fusion positioning method for underground rubber-wheeled vehicles includes the following steps:

[0010] S100, installing an on-board positioning processing device on a rubber-wheeled vehicle, the on-board positioning processing device comprising a UWB tag, an IMU device, an on-board computer, and a wireless communication module; the antenna of the UWB tag is installed on the top of the rubber-wheeled vehicle;

[0011] S200, deploying UWB base stations, the UWB base stations including a UWB starting base station and a UWB intermediate base station; deploying the UWB starting base station at the tunnel entrance, and deploying several UWB intermediate base stations in the tunnel with the UWB starting base station as a reference; obtaining one-dimensional position data of each UWB intermediate base station relative to the UWB starting base station, and storing the data in an onboard database;

[0012] S300: The UWB tag sends a ranging signal. After receiving the ranging signal, each UWB base station calculates the distance to the UWB tag using a bilateral ranging method and feeds back the distance calculation result to the UWB tag. The on-board computer reads the distance calculation results fed back by each UWB base station and sorts the UWB base stations according to the distance between the UWB tag and each UWB base station, and stores the sorting results in the on-board database.

[0013] S400, the onboard computer reads the inertial data output by the IMU device and stores the inertial data in a double-ended queue;

[0014] S500: Based on the sorting result, two UWB base stations are selected as reference base stations to perform UWB one-dimensional positioning solution, and the generated UWB position solution result is subjected to Kalman filtering to output the UWB positioning solution result of the UWB tag;

[0015] S600: Using the UWB positioning solution result as the IMU initial position, the mechanical arrangement algorithm is used to perform the IMU one-dimensional positioning solution based on the inertial data in the double-ended queue, and the IMU mechanical arrangement solution result is generated;

[0016] S700 uses a Kalman filter to fuse the UWB positioning solution and the IMU mechanical arrangement solution through loose combination, and generates a fused positioning result;

[0017] S800 displays the fused positioning results and transmits them to the cloud server through the wireless communication module.

[0018] Furthermore, the UWB base stations are all deployed on the top center line of the tunnel;

[0019] S200 includes:

[0020] S201, deploying a UWB starting base station at the tunnel entrance;

[0021] S202, measuring an effective ranging distance between a UWB base station and a UWB tag in a tunnel, and generating a base station spacing according to the effective ranging distance; the base station spacing is less than the effective ranging distance;

[0022] S203, deploying several UWB intermediate base stations in the tunnel based on the base station spacing;

[0023] S204, establishing a two-dimensional reference coordinate system; using the tunnel entrance as the reference origin, obtaining the tunnel extension direction, with the tunnel extension direction as the positive direction of the x-axis and the tunnel entrance as the starting point of the x-axis; obtaining the horizontal height of the UWB tag, with the vertical upward direction as the positive direction of the y-axis and the horizontal height of the UWB tag as the starting point of the y-axis;

[0024] S205 , obtaining one-dimensional position data of each UWB intermediate base station relative to the UWB starting base station according to the two-dimensional reference coordinate system, and storing the data in the vehicle-mounted database.

[0025] Furthermore, in S300 , the UWB tag sends a ranging signal at a preset interval, and the onboard computer generates a corresponding ranking result;

[0026] According to the ranking results, two UWB base stations are selected as reference base stations, including:

[0027] S1, analyzing whether the number of UWB base stations in the sorting result is less than three, if so, executing S2, if not, executing S3;

[0028] S2, if the number of UWB base stations in the sorting result is two, use the two UWB base stations in the sorting result as the first reference base station and the second reference base station respectively;

[0029] S3, taking the UWB base station closest to the UWB tag in the sorting result as the first reference base station, and taking the two UWB base stations closest to the UWB tag among the remaining UWB base stations as candidate base stations;

[0030] S4, determining whether the distances between the two candidate base stations and the UWB tag are equal, if so, executing S5, if not, using the UWB base station that is closer to the UWB tag as the second reference base station;

[0031] S5, analyzing whether the distance between the first reference base station and the UWB tag is less than a distance threshold, if so, executing S6, if not, using a UWB base station that is closer to the UWB tag as a second reference base station;

[0032] S6, obtain the previous sorting result generated by the on-board computer; analyze whether the rankings of the three UWB base stations closest to the UWB tag in the previous sorting result and the current sorting result are the same; if so, use the UWB base station closer to the UWB tag as the second reference base station; if not, use the UWB base station farther from the UWB tag as the second reference base station.

[0033] Furthermore, S400 includes:

[0034] S401, the onboard computer initializes the UART and generates a designated port number and data transmission baud rate;

[0035] S402, read UART data and create a double-ended queue with a length of 200;

[0036] S403: Read the inertial data output by the IMU device and store the inertial data in a double-ended queue.

[0037] Furthermore, S500 includes:

[0038] S501, obtaining the slope of the tunnel, analyzing whether the slope of the tunnel is 0, and generating a slope analysis result;

[0039] S502, drawing two circles with the two reference base stations as the centers and the distances between the two reference base stations and the UWB tag as the radii; analyzing whether the two circles intersect, and generating a position relationship analysis result;

[0040] S503, performing a UWB one-dimensional positioning solution using a midpoint evaluation method or a three-sided evaluation method based on the slope analysis results and the position relationship analysis results, and generating a UWB position solution result;

[0041] S504: The generated UWB position solution result is subjected to Kalman filtering to output the UWB positioning solution result of the UWB tag.

[0042] Furthermore, S600 includes:

[0043] S601, obtaining the pitch angle, roll angle, and yaw angle of the IMU device, and generating the attitude matrix of the rubber-wheeled vehicle from the b-frame to the n-frame;

[0044] The b system to the n system undergoes three rotations, and the transformation matrix corresponding to each rotation is:

[0045]

[0046] The attitude matrix from system n to system b is:

[0047]

[0048] Since the coordinate system always maintains a rectangular coordinate system during the rotation from the n system to the b system, according to the properties of the unit orthogonal matrix:

[0049]

[0050] The attitude matrix from b to n is:

[0051]

[0052] Where, ψ is the pitch angle; θ is the roll angle; γ is the yaw angle; n is the n-axis; b is the b-axis;

[0053] S602, obtain the acceleration of the IMU device and compare it with the attitude matrix After multiplication, perform quadratic integration to calculate the relative position of the rubber-wheeled vehicle at time t relative to time t-1;

[0054] The acceleration of the IMU device after coordinate conversion is:

[0055]

[0056] After time Δt, the speed of the rubber-wheeled vehicle is:

[0057]

[0058] After time Δt, the position of the rubber-wheeled vehicle is:

[0059]

[0060] Where, is the acceleration of frame b at time t, is the attitude conversion matrix from b system to n system, is the acceleration of system n at time t, is the speed of the rubber-wheeled vehicle under the n-axis at time t-1, is the speed of the rubber-wheeled vehicle at time t, is the position of the rubber-wheeled vehicle under n at time t-1, is the position of the rubber-wheeled vehicle at time t.

[0061] Furthermore, S700 further includes: storing the fused positioning result in an onboard database;

[0062] The Kalman filter is used to fuse the UWB positioning solution results and the IMU mechanical arrangement solution results through loose combination, and generate a fused positioning result, including:

[0063] Establish a discrete state space model of the UWB and IMU loosely combined positioning system:

[0064] Equation of state: X k =FX k-1 +W k-1

[0065] Measurement equation: Z k =HX k +V k

[0066] State one-step prediction: X k|k-1 =FX k-1|k-1

[0067] One-step forecast covariance: P k|k-1 =FP k-1|k-1 F T +Q

[0068] Filter gain: K k =P k|k-1 H T (HP k|k-1 H T +R) -1

[0069] State estimation: X k =X k|k-1 +K k (Z k -HX k|k-1 )

[0070] Estimated state covariance: P k =P k|k-1 +K k HP k|k-1

[0071] in,

[0072] Where k is the current sampling time; T is the sampling period; W is the process noise of the system, which is set as a white noise sequence with covariance Q; V is the measurement noise of the system, which is set as a white noise sequence with covariance R. W and V are uncorrelated, and X k is the system state variable at time k, F is the state transfer matrix, Z k is the system measurement variable at time k, H is the observation matrix, P k is the error covariance matrix at time k, K k is the filter gain at time k, s x is the displacement in the x direction, v x is the velocity in the x direction, is the displacement calculated by UWB in the x direction;

[0073] In S600, the UWB positioning solution result or the fused positioning result is used as the IMU initial position, and the mechanical arrangement algorithm is used to perform IMU one-dimensional positioning solution based on the inertial data in the double-ended queue.

[0074] Furthermore, the antenna of the UWB tag is arranged above the center line of the tunnel ground.

[0075] Furthermore, the rubber-wheeled vehicle is provided with an on-board electronic map;

[0076] In S800, the on-board computer sends the fused positioning results to the on-board electronic map for display, and transmits the fused positioning results to the cloud server through the wireless communication module.

[0077] The principles and advantages of the present invention are:

[0078] 1. Taking into account the narrow and long tunnel environment, UWB base stations are deployed in the tunnel and UWB tags are installed on rubber-wheeled vehicles. This allows the UWB one-dimensional positioning of the rubber-wheeled vehicle to be solved by calculating the distance between the UWB tags and each UWB base station. In the initial state, the UWB positioning solution result is used as the IMU initial position. When a fused positioning result is generated, the fused positioning solution result is used as the IMU initial position. Then, based on the inertial data in the double-ended queue, a mechanical arrangement algorithm is used to achieve the IMU one-dimensional positioning solution, effectively eliminating the adverse effects of IMU cumulative error. Finally, a Kalman filter is used to fuse the UWB positioning solution results and the IMU mechanical arrangement solution results through loose combination to generate a fused positioning result, achieving accurate positioning of the rubber-wheeled vehicle while driving in the tunnel.

[0079] 2. Placing the UWB tag antenna on the top of the rubber-wheeled vehicle reduces obstruction between the UWB tag and the UWB base station, thus preventing non-line-of-sight (NLOS) transmission of signals between the UWB base station and the UWB tag, thereby preventing NLOS errors in UWB ranging. Furthermore, by measuring tunnel slope, the influence of slope was incorporated into the UWB position solution. The mapping relationship between actual distance and one-dimensional positioning was analyzed, and calculation methods were differentiated for straight and sloped roads, improving the accuracy of positioning results.

[0080] 3. Generate the base station spacing based on the effective ranging distance, and the base station spacing is smaller than the effective ranging distance, so as to prevent the spacing between UWB base stations from being too large, resulting in the UWB tag being unable to obtain signals from two UWB base stations at the same time in the tunnel, making it impossible to calculate the position; at the same time, prevent the spacing between UWB base stations from being too small, resulting in an excessive number of base stations required, thereby reducing the number of base station settings, reducing the cost of operation and maintenance, and avoiding communication congestion caused by base station redundancy.

[0081] 4. The UWB position solution result is filtered by the Kalman filter and then loosely combined with the IMU mechanical arrangement solution result, which improves the UWB positioning accuracy and fusion positioning accuracy; by introducing the UWB absolute position positioning information to compensate for the cumulative error of IMU positioning, and introducing the IMU's autonomous measurement information to resist the large UWB positioning error that may be caused by NLOS, the UWB and IMU can complement each other and improve the accuracy and reliability of positioning.

[0082] In summary, this solution eliminates the adverse effects of IMU cumulative errors, realizes the dynamic switching of UWB reference base stations, improves the real-time positioning, and has the ability to resist UWB random measurement errors and NLOS errors. It can effectively adapt to the precise positioning of rubber-wheeled vehicles driving in tunnels. BRIEF DESCRIPTION OF THE DRAWINGS

[0083] Figure 1 The figure is a flow chart of a method for fusion positioning of an underground rubber-wheeled vehicle based on UWB and IMU according to an embodiment of the present invention.

[0084] Figure 2 The present invention is a schematic diagram of the processing logic of a vehicle-mounted positioning processing device in a UWB- and IMU-based fusion positioning method for an underground rubber-wheeled vehicle according to an embodiment of the present invention.

[0085] Figure 3 This is a schematic diagram of a UWB base station deployment method in an underground rubber-wheeled vehicle fusion positioning method based on UWB and IMU in an embodiment of the present invention.

[0086] Figure 4 This is a schematic diagram of the communication process between the UWB base station and the UWB tag in a UWB- and IMU-based fusion positioning method for an underground rubber-wheeled vehicle in an embodiment of the present invention.

[0087] Figure 5 This is a schematic diagram of a base station selection and confirmation method in a UWB- and IMU-based underground rubber-wheeled vehicle fusion positioning method according to an embodiment of the present invention.

[0088] Figure 6 This is a schematic diagram of the positions of the UWB tag and the UWB base station in a straight road condition in a UWB- and IMU-based fusion positioning method for an underground rubber-wheeled vehicle in an embodiment of the present invention.

[0089] Figure 7 This is a schematic diagram of the geometric relationship between the UWB tag and the UWB base station in a straight road condition in a UWB- and IMU-based fusion positioning method for an underground rubber-wheeled vehicle in an embodiment of the present invention.

[0090] Figure 8 This is a schematic diagram of the positions of the UWB tag and the UWB base station in a ramp situation in a UWB- and IMU-based fusion positioning method for an underground rubber-wheeled vehicle in an embodiment of the present invention.

[0091] Figure 9 This is a schematic diagram of the geometric relationship between the UWB tag and the UWB base station in a ramp situation in a UWB- and IMU-based fusion positioning method for an underground rubber-wheeled vehicle in an embodiment of the present invention.

[0092] Figure 10 This is a schematic diagram of the three-dimensional structure of the tunnel and the UWB base station in a UWB- and IMU-based fusion positioning method for underground rubber-wheeled vehicles in an embodiment of the present invention.

[0093] Figure 11 This is a schematic diagram of the planar structure of the tunnel and the UWB base station in a UWB- and IMU-based fusion positioning method for underground rubber-wheeled vehicles in an embodiment of the present invention. DETAILED DESCRIPTION

[0094] The following is further described in detail through specific implementation methods:

[0095] Example 1:

[0096] A fusion positioning method for underground rubber-wheeled vehicles based on UWB and IMU, such as Figure 1 As shown, the following steps are included:

[0097] S100, an on-board positioning processing device is set on the rubber-wheeled vehicle, and the on-board positioning processing device includes a UWB tag, an IMU device, an on-board computer and a wireless communication module. In order to avoid serious non-line-of-sight effects in the tunnel, the antenna of the UWB tag is set on the top of the rubber-wheeled vehicle to reduce the obstruction between the UWB tag and the UWB base station, and the UWB tag is located above the center line of the tunnel ground. The IMU device is fixedly installed in the rubber-wheeled vehicle to prevent relative movement between the rubber-wheeled vehicle and the IMU device. In this embodiment, the UWB tag uses a UWB chip of the same model as the base station, the IMU device uses a JY901 nine-axis inertial sensor, and the wireless communication module uses a WIFI communication module. In other embodiments of the present application, a 5G communication module can also be used. The on-board positioning processing device composed of a UWB tag, an IMU device, an on-board computer and a wireless communication module is as follows: Figure 2 As shown, it is powered by the vehicle power supply and can transmit data to the cloud server through the WIFI / 5G communication module, and then transmit it to the vehicle position display terminal through the cloud server for remote monitoring.

[0098] S200, deploying a UWB base station. The UWB base station is installed at the center line of the tunnel top wall so that it and the UWB tag are in the same vertical plane. See the installation diagram for details. Figure 3 As shown, Figure 3 The top diagram shows the base station installation in a straight tunnel, while the bottom diagram shows the base station installation in a sloped tunnel. The acceptable horizontal position difference between the UWB base station and the UWB tag is that the UWB tag can slightly deviate from the tunnel centerline (displacement difference less than 1 meter). The UWB base station is equipped with a UWB chip and an omnidirectional antenna.

[0099] The UWB base stations include a UWB starting base station and a UWB intermediate base station; the UWB starting base station is deployed at the tunnel entrance, and several UWB intermediate base stations are deployed in the tunnel based on the UWB starting base station; the one-dimensional position data of each UWB intermediate base station relative to the UWB starting base station is obtained and stored in the vehicle-mounted database; the UWB base stations are all deployed on the top center line of the tunnel.

[0100] In this embodiment, Figure 10As shown in the figure, the tunnel in this embodiment consists of a straight section and a ramp. The tunnel is 5m high and 5m wide. The straight section is 1000m long and the ramp section is 500m long, with an inclination of 10°. Assume that the measured distance D is 100m and the height of the rubber-wheeled vehicle roof antenna is 2.5m. Figure 10 、 Figure 11 As shown, starting from the tunnel entrance, the UWB base stations are arranged on the center line of the tunnel top, maintaining a uniform horizontal interval of 70m (0.7D).

[0101] S200 includes:

[0102] S201, deploying a UWB starting base station at the tunnel entrance; in this embodiment, the UWB starting base station is deployed at the tunnel entrance, and after all intermediate base stations are deployed, the UWB terminal base station is deployed at the tunnel exit, and the base stations are kept in the same tunnel vertical plane.

[0103] S202 measures the effective ranging distance between the UWB base station and the UWB tag in the tunnel, and generates a base station spacing based on the effective ranging distance; the base station spacing is less than the effective ranging distance. Specifically, field testing in the tunnel determined a maximum communication distance D with an average UWB ranging error of less than 80 cm. In this embodiment, the effective ranging distance is 0.8D, and the base station spacing is 0.7D-0.8D. The base station spacing is proportional to the width of the tunnel, ensuring that at least two UWB base stations can simultaneously communicate and measure distance with the UWB tag without being obstructed by the tunnel walls.

[0104] S203: Deploy several UWB intermediate base stations in the tunnel according to the base station spacing.

[0105] S204, establish a two-dimensional reference coordinate system; use the tunnel entrance as the reference origin, obtain the tunnel extension direction, with the tunnel extension direction as the positive direction of the x-axis and the tunnel entrance as the starting point of the x-axis; obtain the horizontal height of the UWB tag, with the vertical upward direction as the positive direction of the y-axis and the horizontal height of the UWB tag as the starting point of the y-axis.

[0106] S205 , obtaining one-dimensional position data of each UWB intermediate base station relative to the UWB starting base station according to the two-dimensional reference coordinate system, and storing the data in the vehicle-mounted database.

[0107] S300, the UWB tag sends a ranging signal; after receiving the ranging signal, each UWB base station uses the bilateral ranging method to calculate the distance to the UWB tag, and feeds back the distance calculation result to the UWB tag; the on-board computer reads the distance calculation results fed back by each UWB base station, and sorts each UWB base station according to the distance between the UWB tag and each UWB base station, and stores the sorting results in the on-board database. Specifically, the UWB base stations are preferentially sorted based on the order of the distance between the UWB tags from small to large; in this embodiment, the UWB tag sends a ranging signal at a preset time interval, and the on-board computer generates a corresponding sorting result.

[0108] In this embodiment, the ranging method adopts the ADS-TWR ranging algorithm based on TOA. The process includes three communications between the UWB base station and the UWB tag. The communication process is as follows: Figure 4 shown.

[0109] (1) First communication: After the UWB tag is initialized, it broadcasts a Poll data frame to the surrounding base stations. The Poll data frame contains information such as the tag ID. At this time, the UWB tag records the timestamp TSP when the Poll data frame is sent. After initialization, the UWB base station is in receiving mode, used to receive data frames sent by the UWB tag. After receiving the Poll data frame, it records the timestamp TRP when the signal arrives.

[0110] (2) Second communication: After receiving the Poll data frame, the UWB base station sends a Response data frame to the UWB tag after a time interval TSRPa and marks the sending timestamp TSR, where TSRPa includes the time of processing the Poll signal and generating the Response data frame. After receiving the Response data frame, the UWB tag records the receiving timestamp TRR.

[0111] (3) Third communication: After a time interval TSRPT, the UWB tag sends a Final data frame to the base station and records the timestamp TSF of the sending time. The Final data frame contains the tag ID, TSP, TRR, and TSP information. After receiving the Final signal, the UWB base station records the timestamp TRF of the receiving time.

[0112] TSP: The timestamp when the Poll data frame is sent. After the tag is initialized, the tag broadcasts the Poll data frame to the surrounding base stations. The Poll data frame contains information such as the tag ID;

[0113] TRP: The base station records the timestamp TRP when the signal arrives after receiving the Poll data frame;

[0114] TSRPa: A time interval that includes the time to process the Poll signal and generate the Response data frame;

[0115] TSR: Sending timestamp TSR. After receiving the Poll data frame, the base station sends a Response data frame to the tag after a time interval TSRPa.

[0116] TRR: Receive timestamp, recorded after the tag receives the Response data frame;

[0117] TSRPt: a time interval;

[0118] TSF: Transmission timestamp TSF, recorded after the base station sends the Final data frame after a time interval TSRPt. The Final data frame contains tag ID, TSP, TRR, and TSP information;

[0119] TRF: Receive timestamp, recorded after the base station receives the Final signal;

[0120] The UWB base station calculates the flight time of the signal between the UWB base station and the UWB tag based on the timestamp. The calculation formula is as follows:

[0121]

[0122]

[0123] The distance between the UWB tag and the UWB base station can be obtained by multiplying the signal's flight time by the speed of light. The UWB puts this distance in the Response data frame and sends it to the UWB. Assuming that the maximum speed of the underground rubber-wheeled vehicle is 40 km / h and the general speed is 20 km / h, the recommended ranging rate between the UWB tag and the UWB base station is 10 Hz, that is, 10 ranging and positioning operations are performed per second.

[0124] The on-board computer reads the distance data between the UWB tag and the UWB base station calculated by the UWB base station in the previous ranging from the Response data packets received by the UWB tag, and stores the base station ID, its position coordinates and distance data in the computer's cache.

[0125] S400: The onboard computer reads the inertial data output by the IMU device and stores the inertial data in a double-ended queue for subsequent speed and position calculation of the rubber-wheeled vehicle. Specifically, the onboard computer and the IMU device use UART for data communication, distinguishing inertial information such as acceleration, angular velocity, and Euler angles through a data header, and using a double-ended queue with a length of 200 to store the IMU data.

[0126] S400 includes:

[0127] S401, the onboard computer initializes the UART and generates a designated port number and data transmission baud rate.

[0128] S402, read UART data and create a double-ended queue with a length of 200.

[0129] S403: Read the inertial data output by the IMU device and store the inertial data in a double-ended queue.

[0130] S500: Based on the sorting result, two UWB base stations are selected as reference base stations to perform UWB one-dimensional positioning solution, and the generated UWB position solution result is subjected to Kalman filtering to output the UWB positioning solution result of the UWB tag.

[0131] According to the ranking results, two UWB base stations are dynamically selected as reference base stations, including:

[0132] S1, analyzing whether the number of UWB base stations in the sorting result is less than three, if so, executing S2, if not, executing S3.

[0133] S2. If the number of UWB base stations in the ranking result is two, use the two UWB base stations in the ranking result as a first reference base station and a second reference base station, respectively.

[0134] S3, taking the UWB base station closest to the UWB tag in the sorting result as the first reference base station, and taking the two UWB base stations closest to the UWB tag in the remaining UWB base stations as candidate base stations; Figure 5 As shown, based on the order of the distance between the UWB tag and the UWB base station from small to large, the distance sequence r1, r2, r3 and the corresponding base station sequence A, B, C are obtained.

[0135] S4, determine whether the distances between the two candidate base stations and the UWB tag are equal. If so, execute S5. If not, use the UWB base station that is closer to the UWB tag as the second reference base station. That is, if r3-r2>0, select B as the second reference base station.

[0136] S5, analyze whether the distance between the first reference base station and the UWB tag is less than the distance threshold (the base station switching issue is considered only when the UWB tag is close to the UWB base station). If so, execute S6; if not, use the UWB base station that is closer to the UWB tag as the second reference base station.

[0137] S6, obtaining the last sorting result generated by the on-board computer; analyzing whether the rankings of the three UWB base stations closest to the UWB tag in the last sorting result and the current sorting result are the same; if so, the UWB base station closer to the UWB tag is used as the second reference base station; if not, the UWB base station farther from the UWB tag is used as the second reference base station. In this embodiment:

[0138] (1) If r1 is less than or equal to 5m, that is, when the UWB tag is close to base station A, in order to eliminate the possible false switching caused by random measurement errors, the base station sequence at the previous moment is A, B, C, and at the current moment it becomes A, C, B. A and B are continued to be selected. Only when the base station sequence at the next moment is still A, C, B, A and C are selected to achieve base station switching.

[0139] (2) If r1 is greater than 5m, that is, the UWB tag is far away from base station A, the base station sequence at the previous moment is A, B, C, and at the current moment it becomes A, C, B, indicating that the ranging of base station B at the current moment has a large NLOS error. At this time, base station switching and one-dimensional positioning solution cannot be performed. A and B should continue to be selected as positioning base stations, but the positioning of the UWB tag needs to be determined by the UWB tag position at the previous moment and the current driving speed of the rubber-wheeled vehicle.

[0140] S500 includes:

[0141] S501, obtaining the slope of the tunnel, analyzing whether the slope of the tunnel is 0, and generating a slope analysis result.

[0142] S502 , drawing two circles with the two reference base stations as the centers and the distances between the two reference base stations and the UWB tag as the radii; analyzing whether the two circles intersect, and generating a position relationship analysis result.

[0143] S503 , performing UWB one-dimensional positioning solution using a midpoint evaluation method or a three-side evaluation method according to the slope analysis result and the position relationship analysis result, and generating a UWB position solution result.

[0144] like Figure 6 、 Figure 7 As shown, if the slope analysis result indicates that the tunnel is a horizontal straight road, two circles are drawn with the two reference base stations as the center and the distance between the two reference base stations and the UWB tag as the radius. Based on the difference in ranging error (the sum of the two ranging values ​​is greater than, equal to, or less than the distance between the two base stations), the two circles are first analyzed to see if they intersect;

[0145] If (the two circles intersect), the one-dimensional position calculation formula of the UWB tag relative to the reference base station is as follows: (Use the relationship between the three sides and the angle of the triangle to solve the one-dimensional position of tag C relative to base station A)

[0146]

[0147]

[0148] x D =AD

[0149] If not (the two circles are separated or tangent), first find the coordinates of the intersection of the two circles and AB, and then use the midpoint of the X-axis coordinates of the two intersections as the X-axis position of the mobile label, that is, the horizontal coordinate of point D:

[0150]

[0151] In this embodiment:

[0152] D: vertical projection of point C on AB;

[0153] AB: the length of the line connecting base station A and base station B;

[0154] α: the angle between tag C and base station A;

[0155] r1: the distance between tag C and base station A;

[0156] r2: the distance between tag C and base station B;

[0157] x D : The horizontal distance between tag C and base station A, calculated using the formula;

[0158] like Figure 8 、 Figure 9 As shown, if the slope analysis result shows that the tunnel is a ramp, two circles are drawn with the two reference base stations as the center and the distance between the two reference base stations and the UWB tag as the radius. First, it is determined whether the relationship between the two circles is tangent or separated.

[0159] If the two circles are separated (tangent), you can first find the coordinates of the intersection of the two circles with AB, and then use the midpoint of the X-axis coordinates of the two intersections as the X-axis position of the mobile label, that is, the horizontal coordinate of point D:

[0160] x D =D·cosβ

[0161] When the two circles intersect, the one-dimensional position of tag C relative to base station A is solved based on the relationship between the three sides of the triangle:

[0162]

[0163]

[0164] x D =D·cosβ

[0165] In this embodiment:

[0166] D: vertical projection of point C on AB;

[0167] AB: the length of the line connecting base station A and base station B;

[0168] DC: The length of the line between points C and D

[0169] α: the angle between tag C and base station A;

[0170] β: the inclination angle of the ramp;

[0171] r1: the distance between tag C and base station A;

[0172] r2: the distance between tag C and base station B;

[0173] x D : The horizontal distance between tag C and base station A, calculated using the formula;

[0174] S504: The generated UWB position solution result is subjected to Kalman filtering to reduce the adverse effects of random noise, and the UWB positioning solution result of the UWB tag is output. Specifically:

[0175] State one-step prediction: X k|k-1 =FX k-1|k-1

[0176] One-step forecast covariance: P k|k-1 =FP k-1|k-1 F T +Q

[0177] Filter gain: K k =P k|k-1 H T (HP k|k-1 H T +R) -1

[0178] State estimation: X k =X k|k-1 +K k (Z k -HX k|k-1 )

[0179] Estimated state covariance: P k =P k|k-1 +K k HP k|k-1

[0180] Among them, the observation State quantity The observation matrix is ​​H = [1 0], and the process noise covariance matrix is The measurement noise covariance matrix R = [0.15 2 ].

[0181] Where k is the current sampling time; T is the sampling period, X k is the system state variable at time k, F is the state transfer matrix, Z k is the system measurement variable at time k, H is the observation matrix, P k is the error covariance matrix at time k, K k is the filter gain at time k, s x is the displacement in the x direction, v x is the velocity in the x direction, is the displacement calculated by UWB in the x direction.

[0182] S600: Using the UWB positioning solution result or the fused positioning result as the IMU initial position, and using the mechanical arrangement algorithm to perform IMU one-dimensional positioning solution based on the inertial data in the double-ended queue, and generate an IMU mechanical arrangement solution result;

[0183] S600 includes:

[0184] S601, obtaining the pitch angle, roll angle, and yaw angle of the IMU device, and generating the attitude matrix of the rubber-wheeled vehicle from the b-frame to the n-frame;

[0185] The b system to the n system undergoes three rotations, and the transformation matrix corresponding to each rotation is:

[0186]

[0187] The attitude matrix from system n to system b is:

[0188]

[0189] Since the coordinate system always maintains a rectangular coordinate system during the rotation from the n system to the b system, according to the properties of the unit orthogonal matrix:

[0190]

[0191] The attitude matrix from b to n is:

[0192]

[0193] Where, ψ is the pitch angle; θ is the roll angle; γ is the yaw angle; n is the n-axis; b is the b-axis;

[0194] S602, obtain the acceleration of the IMU device and compare it with the attitude matrix After multiplication, perform quadratic integration to calculate the relative position of the rubber-wheeled vehicle at time t relative to time t-1;

[0195] The acceleration of the IMU device after coordinate conversion is:

[0196]

[0197] After time Δt, the speed of the rubber-wheeled vehicle is:

[0198]

[0199] After time Δt, the position of the rubber-wheeled vehicle is:

[0200]

[0201] Where, is the acceleration of frame b at time t, is the attitude conversion matrix from b system to n system, is the acceleration of system n at time t, is the speed of the rubber-wheeled vehicle under the n-axis at time t-1, is the speed of the rubber-wheeled vehicle at time t, is the position of the rubber-wheeled vehicle under n at time t-1, is the position of the rubber-wheeled vehicle at time t.

[0202] After the rubber-wheeled vehicle enters the tunnel entrance, UWB positioning is first turned on, and the UWB positioning position information is continuously recorded for 5 seconds. The position information in the 5th second is used as the initial position for the IMU mechanical arrangement solution.

[0203] S700 uses a Kalman filter to fuse the UWB positioning solution and the IMU mechanical arrangement solution through loose combination, and generates a fused positioning result; the fused positioning result is stored in the vehicle database;

[0204] The Kalman filter is used to fuse the UWB positioning solution results and the IMU mechanical arrangement solution results through loose combination, and generate a fused positioning result, including:

[0205] Using the forward position and forward velocity of the IMU as state variables and the UWB positioning solution results as measurement variables, a discrete state space model of the UWB and IMU loosely combined positioning system is established:

[0206] Equation of state: X k =FX k-1 +W k-1

[0207] Measurement equation: Z k =HX k +V k

[0208] State one-step prediction: X k|k-1=FX k-1|k-1

[0209] One-step forecast covariance: P k|k-1 =FP k-1|k-1 F T +Q

[0210] Filter gain: K k =P k|k-1 H T (HP k|k-1 H T +R) -1

[0211] State estimation: X k =X k|k-1 +K k (Z k -HX k|k-1 )

[0212] Estimated state covariance: P k =P k|k-1 +K k HP k|k-1

[0213] in, H=[1 0],process noise covariance The measurement noise covariance matrix R = [0.1 2 ];

[0214] Where k is the current sampling time; T is the sampling period; W is the process noise of the system, which is set as a white noise sequence with covariance Q; V is the measurement noise of the system, which is set as a white noise sequence with covariance R. W and V are uncorrelated, and X k is the system state variable at time k, F is the state transfer matrix, Z k is the system measurement variable at time k, H is the observation matrix, P k is the error covariance matrix at time k, K k is the filter gain at time k, s x is the displacement in the x direction, v x is the velocity in the x direction, is the displacement calculated by UWB in the x direction.

[0215] After completing the fusion positioning calculation, the fusion positioning result is used as the initial position of the next moment IMU dead reckoning positioning, and the position information is stored in the on-board computer memory so that the positioning information can be sent to the monitoring center later. The positioning processing flow of the fusion positioning system is as follows: Figure 1 shown.

[0216] S800: Display the fused positioning results and transmit them to the cloud server via the wireless communication module. The rubber-wheeled vehicle is equipped with an onboard electronic map. The onboard computer sends the fused positioning results to the onboard electronic map for display and transmits the fused positioning results to the cloud server via the wireless communication module. The ground monitoring center can then obtain the location information of the rubber-wheeled vehicle through the cloud server and display it.

[0217] This solution eliminates the adverse effects of IMU cumulative errors, realizes dynamic switching of UWB reference base stations, improves real-time positioning, and has the ability to resist UWB random measurement errors and NLOS errors. It can effectively adapt to the precise positioning of rubber-wheeled vehicles driving in tunnels.

[0218] The above are only embodiments of the present invention. Common knowledge such as the known specific structures and characteristics in the scheme are not described in detail here. Ordinary technicians in the field are aware of all common technical knowledge in the technical field of the invention before the application date or priority date, can obtain all existing technologies in the field, and have the ability to apply conventional experimental means before that date. Ordinary technicians in the field can improve and implement this scheme in combination with their own abilities under the inspiration given by this application. Some typical known structures or known methods should not become obstacles for ordinary technicians in the field to implement this application. It should be pointed out that for those skilled in the art, without departing from the structure of the present invention, several variations and improvements can be made, which should also be regarded as the scope of protection of the present invention. These will not affect the effect of the implementation of the present invention and the practicality of the patent. The scope of protection required by this application shall be based on the content of its claims, and the specific implementation methods and other records in the specification can be used to interpret the content of the claims.

Claims

1. A UWB and IMU-based fusion positioning method for underground rubber-wheeled vehicles, characterized by: The following steps are involved: S100, installing an on-board positioning processing device on a rubber-wheeled vehicle, the on-board positioning processing device comprising a UWB tag, an IMU device, an on-board computer, and a wireless communication module; the antenna of the UWB tag is installed on the top of the rubber-wheeled vehicle; S200, deploying UWB base stations, the UWB base stations including a UWB starting base station and a UWB intermediate base station; deploying the UWB starting base station at the tunnel entrance, and deploying several UWB intermediate base stations in the tunnel with the UWB starting base station as a reference; obtaining one-dimensional position data of each UWB intermediate base station relative to the UWB starting base station, and storing the data in an onboard database; S300: The UWB tag sends a ranging signal. After receiving the ranging signal, each UWB base station calculates the distance to the UWB tag using a bilateral ranging method and feeds back the distance calculation result to the UWB tag. The on-board computer reads the distance calculation results fed back by each UWB base station and sorts the UWB base stations according to the distance between the UWB tag and each UWB base station, and stores the sorting results in the on-board database. S400, the onboard computer reads the inertial data output by the IMU device and stores the inertial data in a double-ended queue; S500: Based on the sorting result, two UWB base stations are selected as reference base stations to perform UWB one-dimensional positioning solution, and the generated UWB position solution result is subjected to Kalman filtering to output the UWB positioning solution result of the UWB tag; S600: Using the UWB positioning solution result as the IMU initial position, the mechanical arrangement algorithm is used to perform the IMU one-dimensional positioning solution based on the inertial data in the double-ended queue, and the IMU mechanical arrangement solution result is generated; S700 uses a Kalman filter to fuse the UWB positioning solution and the IMU mechanical arrangement solution through loose combination, and generates a fused positioning result; S800 displays the fused positioning results and transmits them to the cloud server through the wireless communication module.

2. The underground rubber-wheeled vehicle fusion positioning method based on UWB and IMU according to claim 1 is characterized by: The UWB base stations are all deployed on the top center line of the tunnel; S200 includes: S201, deploying a UWB starting base station at the tunnel entrance; S202, measuring an effective ranging distance between a UWB base station and a UWB tag in a tunnel, and generating a base station spacing according to the effective ranging distance; the base station spacing is less than the effective ranging distance; S203, deploying several UWB intermediate base stations in the tunnel based on the base station spacing; S204, establishing a two-dimensional reference coordinate system; using the tunnel entrance as the reference origin, obtaining the tunnel extension direction, with the tunnel extension direction as the positive direction of the x-axis and the tunnel entrance as the starting point of the x-axis; obtaining the horizontal height of the UWB tag, with the vertical upward direction as the positive direction of the y-axis and the horizontal height of the UWB tag as the starting point of the y-axis; S205 , obtaining one-dimensional position data of each UWB intermediate base station relative to the UWB starting base station according to the two-dimensional reference coordinate system, and storing the data in the vehicle-mounted database.

3. The UWB- and IMU-based fusion positioning method for underground rubber-wheeled vehicles according to claim 2 is characterized in that: In S300 , the UWB tag sends a ranging signal at a preset interval, and the onboard computer generates a corresponding sorting result; According to the ranking results, two UWB base stations are selected as reference base stations, including: S1, analyzing whether the number of UWB base stations in the sorting result is less than three, if so, executing S2, if not, executing S3; S2, if the number of UWB base stations in the sorting result is two, use the two UWB base stations in the sorting result as the first reference base station and the second reference base station respectively; S3, taking the UWB base station closest to the UWB tag in the sorting result as the first reference base station, and taking the two UWB base stations closest to the UWB tag among the remaining UWB base stations as candidate base stations; S4, determining whether the distances between the two candidate base stations and the UWB tag are equal, if so, executing S5, if not, using the UWB base station that is closer to the UWB tag as the second reference base station; S5, analyzing whether the distance between the first reference base station and the UWB tag is less than a distance threshold, if so, executing S6, if not, using a UWB base station that is closer to the UWB tag as a second reference base station; S6, obtain the previous sorting result generated by the on-board computer; analyze whether the rankings of the three UWB base stations closest to the UWB tag in the previous sorting result and the current sorting result are the same; if so, use the UWB base station closer to the UWB tag as the second reference base station; if not, use the UWB base station farther from the UWB tag as the second reference base station.

4. The underground rubber-wheeled vehicle fusion positioning method based on UWB and IMU according to claim 1 is characterized by: S400 includes: S401, the onboard computer initializes the UART and generates a designated port number and data transmission baud rate; S402, read UART data and create a double-ended queue with a length of 200; S403: Read the inertial data output by the IMU device and store the inertial data in a double-ended queue.

5. The underground rubber-wheeled vehicle fusion positioning method based on UWB and IMU according to claim 3 is characterized by: S500 includes: S501, obtaining the slope of the tunnel, analyzing whether the slope of the tunnel is 0, and generating a slope analysis result; S502, drawing two circles with the two reference base stations as the centers and the distances between the two reference base stations and the UWB tag as the radii; analyzing whether the two circles intersect, and generating a position relationship analysis result; S503, performing a UWB one-dimensional positioning solution using a midpoint evaluation method or a three-sided evaluation method based on the slope analysis results and the position relationship analysis results, and generating a UWB position solution result; S504: The generated UWB position solution result is subjected to Kalman filtering to output the UWB positioning solution result of the UWB tag.

6. The underground rubber-wheeled vehicle fusion positioning method based on UWB and IMU according to claim 1 is characterized by: S600 includes: S601, obtaining the pitch angle, roll angle, and yaw angle of the IMU device, and generating the attitude matrix of the rubber-wheeled vehicle from the b-frame to the n-frame; The b system to the n system undergoes three rotations, and the transformation matrix corresponding to each rotation is: The attitude matrix from system n to system b is: Since the coordinate system always maintains a rectangular coordinate system during the rotation from the n system to the b system, according to the properties of the unit orthogonal matrix: The attitude matrix from b to n is: Where, ψ is the pitch angle; θ is the roll angle; γ is the yaw angle; n is the n-axis; b is the b-axis; S602, obtain the acceleration of the IMU device and compare it with the attitude matrix After multiplication, perform quadratic integration to calculate the relative position of the rubber-wheeled vehicle at time t relative to time t-1; The acceleration of the IMU device after coordinate conversion is: After time Δt, the speed of the rubber-wheeled vehicle is: After time Δt, the position of the rubber-wheeled vehicle is: Where, is the acceleration of frame b at time t, is the attitude conversion matrix from b system to n system, is the acceleration of system n at time t, is the speed of the rubber-wheeled vehicle under the n-axis at time t-1, is the speed of the rubber-wheeled vehicle at time t, is the position of the rubber-wheeled vehicle under n at time t-1, is the position of the rubber-wheeled vehicle at time t.

7. The underground rubber-wheeled vehicle fusion positioning method based on UWB and IMU according to claim 1 is characterized by: S700 further includes: storing the fused positioning result in an onboard database; The Kalman filter is used to fuse the UWB positioning solution results and the IMU mechanical arrangement solution results through loose combination, and generate a fused positioning result, including: Establish a discrete state space model of the UWB and IMU loosely combined positioning system: Equation of state: X k =FX k-1 +W k-1 Measurement equation: Z k =HX k +V k State one-step prediction: X k|k-1 =FX k-1|k-1 One-step forecast covariance: P k|k-1 =FP k-1|k-1 F T +Q Filter gain: K k =P k|k-1 H T (HP k|k-1 H T +R) -1 State estimation: X k =X k|k-1 +K k (Z k -HX k|k-1 ) Estimated state covariance: P k =P k|k-1 +K k HP k|k-1 in, H = [1 0]; Where k is the current sampling time; T is the sampling period; W is the process noise of the system, which is set as a white noise sequence with covariance Q; V is the measurement noise of the system, which is set as a white noise sequence with covariance R. W and V are uncorrelated, and X k is the system state variable at time k, F is the state transfer matrix, Z k is the system measurement variable at time k, H is the observation matrix, P k is the error covariance matrix at time k, K k is the filter gain at time k, s x is the displacement in the x direction, v x is the velocity in the x direction, is the displacement calculated by UWB in the x direction; In S600, the UWB positioning solution result or the fused positioning result is used as the IMU initial position, and the mechanical arrangement algorithm is used to perform IMU one-dimensional positioning solution based on the inertial data in the double-ended queue.

8. The underground rubber-wheeled vehicle fusion positioning method based on UWB and IMU according to claim 1 is characterized by: The antenna of the UWB tag is arranged above the center line of the tunnel ground.

9. The underground rubber-wheeled vehicle fusion positioning method based on UWB and IMU according to claim 1 is characterized by: The rubber-wheeled vehicle is provided with an on-board electronic map; In S800, the on-board computer sends the fused positioning results to the on-board electronic map for display, and transmits the fused positioning results to the cloud server through the wireless communication module.

Images