CN116582926A - Underground rubber-tyred vehicle fusion positioning method based on UWB and IMU - Google Patents

Underground rubber-tyred vehicle fusion positioning method based on UWB and IMU Download PDF

Info

Publication number
CN116582926A
CN116582926A CN202310603191.XA CN202310603191A CN116582926A CN 116582926 A CN116582926 A CN 116582926A CN 202310603191 A CN202310603191 A CN 202310603191A CN 116582926 A CN116582926 A CN 116582926A
Authority
CN
China
Prior art keywords
uwb
base station
result
imu
vehicle
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Pending
Application number
CN202310603191.XA
Other languages
Chinese (zh)
Inventor
郭子文
邓平
华锋
刘香渝
周继华
赵涛
陈柯
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Aerospace Xintong Technology Co ltd
Southwest Jiaotong University
Original Assignee
Aerospace Xintong Technology Co ltd
Southwest Jiaotong University
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Aerospace Xintong Technology Co ltd, Southwest Jiaotong University filed Critical Aerospace Xintong Technology Co ltd
Priority to CN202310603191.XA priority Critical patent/CN116582926A/en
Publication of CN116582926A publication Critical patent/CN116582926A/en
Pending legal-status Critical Current

Links

Classifications

    • H—ELECTRICITY
    • H04—ELECTRIC COMMUNICATION TECHNIQUE
    • H04W—WIRELESS COMMUNICATION NETWORKS
    • H04W64/00—Locating users or terminals or network equipment for network management purposes, e.g. mobility management
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
    • G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
    • G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
    • G01C21/165—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments
    • G01C21/1652—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments with ranging devices, e.g. LIDAR or RADAR
    • H—ELECTRICITY
    • H04—ELECTRIC COMMUNICATION TECHNIQUE
    • H04W—WIRELESS COMMUNICATION NETWORKS
    • H04W16/00—Network planning, e.g. coverage or traffic planning tools; Network deployment, e.g. resource partitioning or cells structures
    • H04W16/18—Network planning tools
    • H04W16/20—Network planning tools for indoor coverage or short range network deployment
    • H—ELECTRICITY
    • H04—ELECTRIC COMMUNICATION TECHNIQUE
    • H04W—WIRELESS COMMUNICATION NETWORKS
    • H04W4/00—Services specially adapted for wireless communication networks; Facilities therefor
    • H04W4/02—Services making use of location information
    • H04W4/023—Services making use of location information using mutual or relative location information between multiple location based services [LBS] targets or of distance thresholds
    • H—ELECTRICITY
    • H04—ELECTRIC COMMUNICATION TECHNIQUE
    • H04W—WIRELESS COMMUNICATION NETWORKS
    • H04W4/00—Services specially adapted for wireless communication networks; Facilities therefor
    • H04W4/02—Services making use of location information
    • H04W4/025—Services making use of location information using location based information parameters
    • H04W4/027—Services making use of location information using location based information parameters using movement velocity, acceleration information
    • H—ELECTRICITY
    • H04—ELECTRIC COMMUNICATION TECHNIQUE
    • H04W—WIRELESS COMMUNICATION NETWORKS
    • H04W4/00—Services specially adapted for wireless communication networks; Facilities therefor
    • H04W4/30—Services specially adapted for particular environments, situations or purposes
    • H04W4/40—Services specially adapted for particular environments, situations or purposes for vehicles, e.g. vehicle-to-pedestrians [V2P]
    • H04W4/44—Services specially adapted for particular environments, situations or purposes for vehicles, e.g. vehicle-to-pedestrians [V2P] for communication between vehicles and infrastructures, e.g. vehicle-to-cloud [V2C] or vehicle-to-home [V2H]
    • H—ELECTRICITY
    • H04—ELECTRIC COMMUNICATION TECHNIQUE
    • H04W—WIRELESS COMMUNICATION NETWORKS
    • H04W4/00—Services specially adapted for wireless communication networks; Facilities therefor
    • H04W4/30—Services specially adapted for particular environments, situations or purposes
    • H04W4/40—Services specially adapted for particular environments, situations or purposes for vehicles, e.g. vehicle-to-pedestrians [V2P]
    • H04W4/48—Services specially adapted for particular environments, situations or purposes for vehicles, e.g. vehicle-to-pedestrians [V2P] for in-vehicle communication
    • Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
    • Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
    • Y02D—CLIMATE CHANGE MITIGATION TECHNOLOGIES IN INFORMATION AND COMMUNICATION TECHNOLOGIES [ICT], I.E. INFORMATION AND COMMUNICATION TECHNOLOGIES AIMING AT THE REDUCTION OF THEIR OWN ENERGY USE
    • Y02D30/00—Reducing energy consumption in communication networks
    • Y02D30/70—Reducing energy consumption in communication networks in wireless communication networks

Landscapes

  • Engineering & Computer Science (AREA)
  • Computer Networks & Wireless Communication (AREA)
  • Signal Processing (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Automation & Control Theory (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Position Fixing By Use Of Radio Waves (AREA)

Abstract

The application relates to the technical field of fusion positioning, in particular to an underground rubber-tyred vehicle fusion positioning method based on UWB and IMU, which comprises the following steps: s100, setting a vehicle-mounted positioning processing device; s200, deploying UWB base stations; s300, the UWB tag sends out a ranging signal; the vehicle-mounted computer reads the distance calculation result fed back by each UWB base station and sorts each UWB base station; s400, storing inertial data in a double-end queue; s500, screening two UWB base stations as reference base stations according to the ordering result, performing UWB one-dimensional positioning calculation, and outputting a UWB positioning calculation result of a UWB tag; s600, carrying out IMU one-dimensional positioning calculation by adopting a mechanical arrangement algorithm according to the inertial data in the double-end queue, and generating an IMU mechanical arrangement calculation result; s700, fusing the UWB positioning resolving result and the IMU mechanical compiling resolving result, and generating a fused positioning result. By adopting the scheme, the positioning instantaneity is improved, and the accurate positioning of the rubber-tyred vehicle in the running state in the tunnel is realized.

Description

Underground rubber-tyred vehicle fusion positioning method based on UWB and IMU
Technical Field
The application relates to the technical field of fusion positioning, in particular to an underground rubber-tyred vehicle fusion positioning method based on UWB and IMU.
Background
Coal mine transportation is an indispensable part of underground coal mine production. Along with the development of the times and the technological progress, the trackless rubber-tyred vehicle becomes an emerging underground transportation mode due to the advantages of automatic driving, freedom, flexibility and the like, and is rapidly popularized and applied in China. Meanwhile, the safety, stability, high efficiency, intelligent operation, scheduling and the like of the trackless rubber-tyred vehicle are guaranteed to be the problems to be solved urgently in the industry, and the real-time accurate positioning of the trackless rubber-tyred vehicle in the underground environment is the basis for realizing automatic driving.
In recent years, UWB technology is widely applied to various indoor environment positioning because of the advantages of simple system implementation, strong multipath resolution capability, high positioning accuracy and the like. The IMU positioning system integrates the sensors such as the accelerometer, the gyroscope and the magnetometer, and has the advantages of complete autonomous navigation, high data updating rate, high short-term positioning accuracy and no interference from external environment. How to effectively combine two positioning technologies and improve the positioning accuracy and reliability of a tunnel rubber-tyred vehicle is an important subject to be solved urgently. The underground tunnel is taken as a special indoor environment, and the environment condition is greatly different from the indoor environment of a common building. The common indoor positioning generally adopts two-dimensional or three-dimensional positioning, and the object to be positioned is a moving body in a plane or space; however, the tunnel environment is often a long and narrow channel with known geometric position information, and the relative position of the rubber-tyred vehicle along the extending direction of the tunnel can be accurately determined by adopting one-dimensional positioning, so that the positioning precision requirement can be met. The system has the advantages of less equipment, simpler and more reliable maintenance and lower cost, but a complete and practical one-dimensional positioning scheme for the tunnel environment vehicles is lacking in China at present.
The application patent of publication number CN114323003A discloses a mine unmanned aerial vehicle based high-precision positioning system and method. The application adopts a plurality of UWB Tag nodes Tag, UWB Anchor nodes Anchor, underground communication AP equipment and a beacon to complete the position location of the locomotive under the mine, but the location scene has poor adaptation degree, and no detailed UWB base station deployment scheme and base station switching scheme in the locomotive movement process are provided.
The application patent of publication number CN113286360A discloses a well and mining fusion positioning method based on UMB, IMU and laser radar. According to the method, the positioning of the underground unmanned vehicle is realized by fusing positioning information of three dimensions of UWB, IMU and laser radar to construct an EKF fusion positioning model, but NLOS possibly existing in the underground positioning process is not considered, the problem that the laser radar is greatly influenced by underground dust is not considered, the reliability of the output positioning result is low, and the positioning accuracy of the vehicle is low.
The application patent of publication No. CN114166221B discloses a method and a system for positioning an auxiliary transportation robot in a dynamic complex mine environment. According to the method, the acceleration information and UWB distance measurement information of the auxiliary transportation robot are obtained, the position estimation of the robot is obtained through extended Kalman filtering, abnormal value detection is carried out on UWB measurement, and if the UWB measurement is abnormal, the extended Kalman filtering is improved through correcting an innovation covariance matrix, so that the positioning accuracy is improved. However, the application does not consider the specificity of the downhole environment, the test length is short, and no final positioning result presentation scheme is available.
Disclosure of Invention
The application provides a fusion positioning method of an underground rubber-tyred vehicle based on UWB and IMU, which eliminates the adverse effect of accumulated error of the IMU, realizes dynamic switching of UWB reference base stations, improves positioning instantaneity, has the capability of resisting UWB random measurement error and NLOS error, and can effectively adapt to accurate positioning of the rubber-tyred vehicle in a running state in a tunnel.
In order to achieve the above purpose, the present application provides the following technical solutions:
an underground rubber-tyred vehicle fusion positioning method based on UWB and IMU comprises the following steps:
s100, arranging a vehicle-mounted positioning processing device on a rubber-tyred vehicle, wherein the vehicle-mounted positioning processing device comprises a UWB tag, an IMU device, a vehicle-mounted computer and a wireless communication module; the antenna of the UWB tag is arranged at the top of the rubber-tyred car;
s200, deploying UWB base stations, wherein the UWB base stations comprise UWB starting point base stations and UWB intermediate base stations; deploying UWB starting point base stations at tunnel openings, and deploying a plurality of UWB intermediate base stations in the tunnel by taking the UWB starting point base stations as references; acquiring one-dimensional position data of each UWB intermediate base station relative to a UWB starting point base station and storing the data in a vehicle-mounted database;
s300, the UWB tag sends out a ranging signal; after each UWB base station receives the ranging signals, calculating the distance between the UWB base station and the UWB tag by adopting a bilateral ranging method, and feeding back the distance calculation result to the UWB tag; the vehicle-mounted computer reads the distance calculation result fed back by each UWB base station, sorts each UWB base station according to the distance between the UWB tag and each UWB base station, and stores the sorting result into the vehicle-mounted database;
s400, the vehicle-mounted computer reads inertial data output by the IMU device and stores the inertial data in a double-end queue;
s500, screening two UWB base stations as reference base stations according to the ordering result, performing UWB one-dimensional positioning calculation, performing Kalman filtering on the generated UWB position calculation result, and outputting a UWB positioning calculation result of a UWB tag;
s600, taking the UWB positioning resolving result as an IMU initial position, adopting a mechanical arrangement algorithm to carry out IMU one-dimensional positioning resolving according to the inertial data in the double-end queue, and generating an IMU mechanical arrangement resolving result;
s700, a Kalman filter is adopted to fuse the UWB positioning calculation result and the IMU mechanical coding calculation result in a loose combination mode, and a fusion positioning result is generated;
s800, displaying the fusion positioning result and transmitting the fusion positioning result to the cloud server through the wireless communication module.
Further, the UWB base stations are all deployed on the top middle line of the tunnel;
s200 includes:
s201, deploying UWB starting point base stations at tunnel openings;
s202, measuring the effective distance measurement distance between the UWB base station and the UWB tag in the tunnel, and generating the base station distance according to the effective distance measurement distance; the distance between the base stations is smaller than the effective distance measurement distance;
s203, according to the distance between base stations, arranging a plurality of UWB intermediate base stations in a tunnel;
s204, establishing a two-dimensional reference coordinate system; taking a tunnel portal as a reference origin, acquiring a tunnel extending direction, and taking the tunnel extending direction as an x-axis positive direction, wherein the tunnel portal is an x-axis starting point; acquiring the horizontal height of the UWB tag, wherein the vertical upward direction is taken as the positive direction of the y axis, and the horizontal height of the UWB tag is taken as the starting point of the y axis;
s205, acquiring 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 one-dimensional position data in the vehicle-mounted database.
Further, in S300, the UWB tag transmits a ranging signal every a preset time, and the vehicle-mounted computer generates a corresponding sequencing result;
according to the sequencing result, two UWB base stations are screened as reference base stations, including:
s1, analyzing whether the number of UWB base stations in the sequencing result is less than three, if so, executing S2, and if not, executing S3;
s2, if the number of UWB base stations in the sequencing result is two, respectively taking the two UWB base stations in the sequencing result as a first reference base station and a second reference base station;
s3, taking one UWB base station closest to the UWB tag in the ordering result as a first reference base station, and taking two UWB base stations closest to the UWB tag in the rest UWB base stations as alternative base stations;
s4, judging whether the distances between the two alternative base stations and the UWB tag are equal, if so, executing S5, and if not, taking the UWB base station which is closer to the UWB tag as a second reference base station;
s5, analyzing whether the distance between the first reference base station and the UWB tag is smaller than a distance threshold, if yes, executing S6, and if not, taking the UWB base station which is closer to the UWB tag as a second reference base station;
s6, acquiring a last sequencing result generated by the vehicle-mounted computer; analyzing whether the last ordering result is the same as the ordering among the three UWB base stations closest to the UWB tag in the current ordering result, if so, taking the UWB base station closest to the UWB tag as a second reference base station, and if not, taking the UWB base station farther from the UWB tag as the second reference base station.
Further, S400 includes:
s401, initializing a UART by a vehicle-mounted computer, and generating a designated port number and a data transmission baud rate;
s402, reading UART data and establishing a double-end queue with the length of 200;
s403, reading inertial data output by the IMU device, and storing the inertial data in a double-end queue.
Further, S500 includes:
s501, acquiring the gradient of the tunnel, analyzing whether the gradient of the tunnel is 0, and generating a gradient analysis result;
s502, respectively drawing two circles by taking two reference base stations as circle centers and the distance between the two reference base stations and the UWB tag as radius; analyzing whether the two circles intersect or not, and generating a position relation analysis result;
s503, according to the gradient analysis result and the position relation analysis result, adopting a midpoint evaluation method or a trilateral evaluation method to perform UWB one-dimensional positioning calculation and generate a UWB position calculation result;
s504, the generated UWB position resolving result is subjected to Kalman filtering, and the UWB positioning resolving result of the UWB tag is output.
Further, S600 includes:
s601, acquiring a pitch angle, a roll angle and a yaw angle of an IMU device, and generating a posture matrix of the rubber-tyred vehicle from a b system to an n system;
the b series to the n series are subjected to three rotations, and the transformation matrix corresponding to each rotation is as follows:
the pose matrix from n to b is:
since the coordinate system always maintains the rectangular coordinate system in the process of rotation from the n system to the b system, the following characteristics are given according to the identity orthogonal matrix:
the pose matrix from b to n is:
wherein, psi is pitch angle; θ is the roll angle; gamma is the yaw angle; n is n; b is b;
s602, acquiring acceleration of the IMU device, and combining the acceleration with the gesture matrixMultiplying and integrating twice to calculate the relative position of the rubber-tyred vehicle at the moment t relative to the moment t-1;
the acceleration of the IMU device is converted into the following coordinates:
after the time deltat, the speed of the rubber-tyred vehicle is as follows:
after the time deltat, the positions of the rubber-tyred vehicle are as follows:
in the formula ,acceleration of line b at time t->Posture conversion matrix for b-series to n-series, < >>For the acceleration of the n series at time t,>for t-1 time n is the lower rubber-tyred vehicle speed,/->For t time n is the speed of the lower rubber-tyred vehicle, < + >>For t-1 time n is the lower rubber-tyred position,/->The position of the lower rubber tire vehicle is set at the time n of t.
Further, S700 further includes: storing the fusion positioning result to a vehicle-mounted database;
the UWB positioning resolving result and the IMU mechanical compiling resolving result are fused by adopting a Kalman filter in a loose combination mode, and a fusion positioning result is generated, and the method comprises the following steps:
establishing a discrete state space model of the UWB and IMU loose combination positioning system:
the equation of state: x is X k ＝FX k-1 +W k-1
The measurement equation: z is Z k ＝HX k +V k
State one-step prediction: x is X k|k-1 ＝FX k-1|k-1
One-step prediction covariance: p (P) k|k-1 ＝FP k-1|k-1 F T +Q
Filtering gain: k (K) k ＝P k|k-1 H T (HP k|k-1 H T +R) -1
State estimation: x is X k ＝X k|k-1 +K k (Z k -HX k|k-1 )
Estimating state covariance: p (P) k ＝P k|k-1 +K k HP k|k-1
wherein ,
wherein k is the current sampling time; t is a sampling period; w is the process noise of the system, and is set as a white noise sequence with covariance Q; v is the measurement noise of the system, and is set as a white noise sequence with covariance of R, W and V are uncorrelated, X k The system state variable at the moment k, F is a state transition matrix and Z k For the system measurement variable at the moment k, H is an observation matrix and P k For the K moment error covariance matrix, K k For the filtering gain at time k, s x For displacement in the x direction, v x At the speed of the x-direction,the displacement obtained by UWB calculation in the x direction;
in S600, the UWB positioning resolving result or the fusion positioning result is taken as an IMU initial position, and according to inertial data in the double-end queue, the IMU one-dimensional positioning resolving is carried out by adopting a mechanical arrangement algorithm.
Further, the antenna of the UWB tag is disposed above the centerline of the tunnel floor.
Further, a vehicle-mounted electronic map is arranged on the rubber-tyred vehicle;
in S800, the vehicle-mounted computer sends the fusion positioning result to the vehicle-mounted electronic map for display, and the fusion positioning result is transmitted to the cloud server through the wireless communication module.
The principle and the advantages of the application are as follows:
1. by combining the long and narrow environmental characteristics of the tunnel, UWB base stations are deployed in the tunnel, and UWB labels are arranged on the rubber-tyred vehicle, so that the UWB one-dimensional positioning calculation of the rubber-tyred vehicle can be realized by calculating the distance between the UWB labels and each UWB base station. In an initial state, a UWB positioning resolving result is used as an IMU initial position, when a generated fusion positioning result exists, the fusion positioning resolving result is used as the IMU initial position, and then according to inertial data in a double-end queue, a mechanical arrangement algorithm is adopted to realize one-dimensional IMU positioning resolving, and adverse effects of accumulated errors of the IMU are effectively eliminated. And finally, a Kalman filter is adopted to fuse the UWB positioning resolving result and the IMU mechanical coding resolving result in a loose combination mode, and a fusion positioning result is generated, so that the accurate positioning of the rubber-tyred vehicle in the running state in the tunnel is realized.
2. The antenna of the UWB tag is arranged at the top of the rubber-tyred vehicle, shielding between the UWB tag and the UWB base station is reduced, and therefore NLOS non-line-of-sight transmission of signals between the UWB base station and the UWB tag is avoided, and NLOS errors of UWB ranging are prevented. And by measuring the gradient of the tunnel and introducing gradient influence factors in the UWB position resolving process, the mapping relation of the actual distance in one-dimensional positioning resolving is analyzed, the calculation methods under the conditions of straight roads and slopes are distinguished, and the accuracy of the positioning result is improved.
3. Generating a base station distance according to the effective distance measurement distance, wherein the base station distance is smaller than the effective distance measurement distance, so that the situation that the distance between UWB base stations is too large, and the UWB tag cannot acquire signals of two UWB base stations in a tunnel at the same time, so that position calculation cannot be performed can be prevented; meanwhile, the Ultra Wide Band (UWB) base stations are prevented from being too small in interval, and the number of base stations required to be set is too large, so that the number of base stations can be reduced, the cost of operation and maintenance is reduced, and communication congestion caused by base station redundancy is avoided.
4. After the UWB position resolving result is filtered by a Kalman filter, the UWB position resolving result is combined with the IMU mechanical compiling resolving result in a loose mode, and UWB positioning accuracy and fusion positioning accuracy are improved; the accumulated error of the IMU positioning is compensated by introducing UWB absolute position positioning information, and the autonomous measurement information of the IMU is introduced to resist the larger UWB positioning error possibly caused by NLOS, so that the complementary short-circuit between UWB and the IMU is realized, and the positioning accuracy and reliability are improved.
In sum, by adopting the scheme, the adverse effect of the accumulated error of the IMU is eliminated, the dynamic switching of the UWB reference base station is realized, the positioning instantaneity is improved, the capability of resisting UWB random measurement errors and NLOS errors is realized, and the accurate positioning of the rubber-tyred vehicle in the running state in the tunnel can be effectively adapted.
Drawings
Fig. 1 is a schematic flow chart of a fusion positioning method of an underground rubber-tyred vehicle based on UWB and IMU according to the embodiment of the application.
Fig. 2 is a schematic diagram of processing logic of a vehicle-mounted positioning processing device in a fusion positioning method of an underground rubber-tyred vehicle based on UWB and IMU according to an embodiment of the application.
Fig. 3 is a schematic diagram of a deployment method of UWB base stations in an underground rubber-tyred vehicle fusion positioning method based on UWB and IMU according to an embodiment of the present application.
Fig. 4 is a schematic diagram of a communication process between a UWB base station and a UWB tag in an underground rubber-tyred vehicle fusion positioning method based on UWB and IMU according to an embodiment of the present application.
Fig. 5 is a schematic diagram of a base station selecting and confirming method in an underground rubber-tyred vehicle fusion positioning method based on UWB and IMU according to an embodiment of the present application.
Fig. 6 is a schematic diagram of positions of a UWB tag and a UWB base station in a straight road situation in a method for positioning a fusion of an underground rubber-tyred vehicle based on UWB and IMU according to an embodiment of the present application.
Fig. 7 is a schematic diagram of a geometric relationship between a UWB tag and a UWB base station in a straight road situation in a method for positioning a fusion of an underground rubber-tyred vehicle based on UWB and IMU according to an embodiment of the present application.
Fig. 8 is a schematic diagram of the positions of UWB tags and UWB base stations in the case of a ramp in a method for positioning a fusion of an underground rubber-tyred vehicle based on UWB and IMU according to an embodiment of the present application.
Fig. 9 is a schematic diagram of a geometric relationship between a UWB tag and a UWB base station in a ramp situation in a method for positioning a fusion of an underground rubber-tyred vehicle based on UWB and IMU according to an embodiment of the present application.
Fig. 10 is a schematic diagram of a three-dimensional structure of a tunnel and a UWB base station in a method for positioning a fusion of an underground rubber-tyred vehicle based on UWB and IMU according to an embodiment of the present application.
Fig. 11 is a schematic plan view of a tunnel and a UWB base station in a method for positioning fusion of an underground rubber-tyred vehicle based on UWB and IMU according to an embodiment of the present application.
Detailed Description
The following is a further detailed description of the embodiments:
example 1:
an underground rubber-tyred vehicle fusion positioning method based on UWB and IMU, as shown in figure 1, comprises the following steps:
s100, a vehicle-mounted positioning processing device is arranged on the rubber-tyred vehicle and comprises a UWB tag, an IMU device, a vehicle-mounted computer and a wireless communication module. In order to avoid serious non-line-of-sight influence in the tunnel, the antenna of the UWB tag is arranged at the top of the rubber-tyred vehicle so as to reduce shielding between the UWB tag and the UWB base station, and the UWB tag is positioned above the central line of the ground of the tunnel. The IMU device is fixedly arranged in the rubber-tyred vehicle, so that the rubber-tyred vehicle and the IMU device are prevented from relative movement. In this embodiment, the UWB tag adopts a UWB chip of the same type as the base station, the IMU device adopts a JY901 nine-axis inertial sensor, and the wireless communication module adopts a WIFI communication module. The vehicle-mounted positioning processing device composed of the UWB tag, the IMU device, the vehicle-mounted computer and the wireless communication module is shown in fig. 2, is powered by a vehicle-mounted power supply, can transmit data to the cloud server through the WIFI/5G communication module, and can transmit the data to the vehicle position display terminal through the cloud server, so that remote monitoring is facilitated.
And S200, deploying UWB base stations, wherein the UWB base stations are arranged at the middle line of the wall surface at the top of the tunnel so as to keep the UWB base stations and the UWB labels in the same vertical plane, the installation schematic diagram is shown in FIG. 3, in the third diagram, the upper diagram is the installation schematic diagram of the base stations in the straight channel, and the lower diagram is the installation schematic diagram of the base stations in the ramp. The acceptable horizontal position difference between the UWB base station and the UWB tag is that the UWB tag can deviate from the central line of the tunnel slightly (displacement difference is smaller than 1 m), and the UWB base station is provided with the UWB chip and the omnidirectional antenna.
The UWB base station comprises a UWB starting point base station and a UWB intermediate base station; deploying UWB starting point base stations at tunnel openings, and deploying a plurality of UWB intermediate base stations in the tunnel by taking the UWB starting point base stations as references; acquiring one-dimensional position data of each UWB intermediate base station relative to a UWB starting point base station and storing the data in a vehicle-mounted database; the UWB base stations are all deployed on the top mid-line of the tunnel.
In this embodiment, as shown in fig. 10, the tunnel in this embodiment is composed of two parts, namely a straight channel and a ramp, the height of the tunnel is 5m, the width of the tunnel is 5m, the length of the straight channel is 1000m, the length of the ramp is 500m, and the inclination angle is 10 °. Assume that the measured distance D is 100m and the height of the cart top antenna is 2.5m. As shown in fig. 10 and 11, UWB base stations are arranged on the tunnel top center line starting from the tunnel entrance, maintaining a uniform horizontal spacing of 70m (0.7D).
S200 includes:
s201, deploying UWB starting point base stations at tunnel openings; in this embodiment, a UWB starting base station is deployed at the tunnel entrance, and after all intermediate base stations are deployed, a UWB ending base station is deployed at the tunnel exit, and the base stations remain in the same tunnel vertical plane.
S202, measuring the effective distance measurement distance between the UWB base station and the UWB tag in the tunnel, and generating the base station distance according to the effective distance measurement distance; the base station spacing is less than the effective ranging distance. Specifically, in the tunnel, the field test determines that the maximum communication distance D of UWB average ranging error is smaller than 80cm, and 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 in direct proportion to the width of the tunnel, so that at least two UWB base stations are prevented from being blocked by the wall of the tunnel when in communication ranging with the UWB tag.
S203, arranging a plurality of UWB intermediate base stations in the tunnel according to the base station spacing.
S204, establishing a two-dimensional reference coordinate system; taking a tunnel portal as a reference origin, acquiring a tunnel extending direction, and taking the tunnel extending direction as an x-axis positive direction, wherein the tunnel portal is an x-axis starting point; the method comprises the steps of obtaining the horizontal height of the UWB tag, wherein the vertical upward direction is taken as the positive y-axis direction, and the horizontal height of the UWB tag is taken as the starting point of the y-axis.
S205, acquiring 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 one-dimensional position data in the vehicle-mounted database.
S300, the UWB tag sends out a ranging signal; after each UWB base station receives the ranging signals, calculating the distance between the UWB base station and the UWB tag by adopting a bilateral ranging method, and feeding back the distance calculation result to the UWB tag; the vehicle-mounted computer reads the distance calculation result fed back by each UWB base station, sorts each UWB base station according to the distance between the UWB tag and each UWB base station, stores the sorting result in the vehicle-mounted database, and particularly, performs optimal sorting on the UWB base stations based on the sequence that the distance between the UWB tag and the UWB tag is from small to large; in this embodiment, the UWB tag transmits a ranging signal every a predetermined time, and the vehicle-mounted computer generates a corresponding sequencing result.
In this embodiment, the ranging method adopts the TOA-based ADS-TWR ranging algorithm, and the process includes three times of communication between the UWB base station and the UWB tag, and the communication process is shown in fig. 4.
(1) First communication: after the UWB tag is initialized, a Poll data frame is broadcast to surrounding base stations, and the Poll data frame contains information such as tag ID, and at this time, the UWB tag records a timestamp TSP when the Poll data frame is transmitted. The UWB base station is initialized to be in a receiving mode and is used for receiving a data frame sent by a UWB tag, and a time stamp TRP when a signal arrives is recorded after the data frame Poll is received;
(2) Second communication: after receiving the Poll data frame, the UWB base station sends a Response data frame to the UWB tag through a time interval TSRPa and marks a sending time stamp TSR, wherein the TSRPa comprises the time for processing the Poll signal and generating the Response data frame, and the UWB tag records a receiving time stamp TRR after receiving the Response data frame;
(3) Third communication: after a time interval TSRPt, the UWB tag sends a Final data frame to the base station and records a time stamp TSF when the Final data frame is sent, the Final data frame contains tag ID, TSP, TRR, TSP information, and the UWB base station records a time stamp TRF when receiving the Final signal;
TSP: after the initialization of the tag is completed, the tag broadcasts and transmits the Poll data frame to surrounding base stations, wherein the Poll data frame contains tag ID and other information;
TRP: the base station records a time stamp TRP when a signal arrives after receiving a Poll data frame;
TSRPa: a time interval including the time of processing the Poll signal and generating a Response data frame;
TSR: transmitting a time stamp TSR, and transmitting a Response data frame to the tag by a time interval TSRPa after the base station receives the Poll data frame;
TRR: receiving a time stamp, and recording after the label receives the Response data frame;
TSRPt: a time interval;
TSF: sending a time stamp TSF, and after the base station sends a Final data frame through a time interval TSRPt, recording, wherein the Final data frame contains tag ID, TSP, TRR, TSP information;
TRF: receiving the time stamp, and recording after receiving the Final signal by the base station;
the UWB base station calculates the flight time of the signal between the UWB base station and the UWB tag according to the time stamp, and the calculation formula is as follows:
the distance value between the UWB tag and the UWB base station can be obtained by multiplying the flight time of the signal by the light speed base station, the UWB is placed in a Response data frame, the maximum speed of time of the UWB to be transmitted to the UWB to assume that the underground rubber-tyred vehicle is driven is 40km/h, the general speed of time is 20km/h, and the distance measurement rate of the UWB tag and the UWB base station is 10Hz, namely 10 times per second of distance measurement positioning is suggested.
The vehicle-mounted computer reads distance data between the UWB tag and the UWB base station, which are calculated by the UWB base station in the previous ranging, from each UWB base station Response data packet received by the UWB tag, and stores the base station ID, the position coordinates and the distance data in a cache of the computer.
S400, the vehicle-mounted computer reads inertial data output by the IMU device and stores the inertial data in a double-end queue so as to calculate the speed and the position of a subsequent rubber-tyred vehicle; specifically, the vehicle-mounted computer and the IMU device use UART to carry out data communication, inertial information such as acceleration, angular velocity, euler angle and the like is distinguished through a data head, and double-end queues with the length of 200 are used for storing IMU data.
S400 includes:
s401, initializing UART by the vehicle-mounted computer, and generating a designated port number and a data transmission baud rate.
S402, reading UART data and establishing a double-end queue with the length of 200.
S403, reading inertial data output by the IMU device, and storing the inertial data in a double-end queue.
S500, screening two UWB base stations as reference base stations according to the ordering result, performing UWB one-dimensional positioning calculation, performing Kalman filtering on the generated UWB position calculation result, and outputting the UWB positioning calculation result of the UWB tag.
According to the sequencing result, dynamically screening two UWB base stations as reference base stations, including:
s1, analyzing whether the number of UWB base stations in the sequencing result is less than three, if so, executing S2, and if not, executing S3.
S2, if the number of UWB base stations in the sequencing result is two, the two UWB base stations in the sequencing result are respectively used as a first reference base station and a second reference base station.
S3, taking one UWB base station closest to the UWB tag in the ordering result as a first reference base station, and taking two UWB base stations closest to the UWB tag in the rest UWB base stations as alternative base stations; as shown in fig. 5, the distance sequences r1, r2, r3 and the corresponding base station sequences a, B, C are obtained based on the order of the distances between the UWB tag and the UWB base station from small to large.
S4, judging whether the distances between the two alternative base stations and the UWB tag are equal, if so, executing S5, otherwise, taking the UWB base station which is closer to the UWB tag as a second reference base station, namely, if r3-r2>0, and selecting B as the second reference base station.
S5, analyzing whether the distance between the first reference base station and the UWB tag is smaller than a distance threshold (the problem of base station switching is considered only when the UWB tag is close to the UWB base station), if yes, executing S6, and if not, taking the UWB base station which is close to the UWB tag as a second reference base station.
S6, acquiring a last sequencing result generated by the vehicle-mounted computer; analyzing whether the last ordering result is the same as the ordering among the three UWB base stations closest to the UWB tag in the current ordering result, if so, taking the UWB base station closest to the UWB tag as a second reference base station, and if not, taking the UWB base station farther from the UWB tag as the second reference base station, wherein in the embodiment:
(1) If r1 is less than or equal to 5m, namely when the UWB label approaches to the base station A, in order to eliminate the error switching possibly caused by random measurement errors, the base station sequence at the previous moment is A, B and C, the current moment is changed into A, C and B, the A and B are continuously selected, and the A and C are selected to realize the base station switching only when the base station sequence at the next moment is still A, C and B.
(2) If r1 is larger than 5m, that is, when the UWB tag is far away from the base station A, the base station sequence at the previous moment is A, B and C, the current moment is changed into A, C and B, which indicates that the distance measurement of the base station B at the current moment has a larger NLOS error, the base station switching and one-dimensional positioning calculation cannot be executed at the moment, the A and the B are selected continuously as positioning base stations, and the positioning of the UWB tag is determined by the UWB tag position at the previous moment and the current running speed estimation of the rubber-tyred vehicle.
S500 includes:
s501, acquiring the gradient of the tunnel, analyzing whether the gradient of the tunnel is 0, and generating a gradient analysis result.
S502, respectively drawing two circles by taking two reference base stations as circle centers and the distance between the two reference base stations and the UWB tag as radius; and analyzing whether the two circles intersect or not, and generating a position relation analysis result.
S503, according to the gradient analysis result and the position relation analysis result, adopting a midpoint evaluation method or a trilateral evaluation method to perform UWB one-dimensional positioning calculation, and generating a UWB position calculation result.
As shown in fig. 6 and fig. 7, if the gradient analysis result is that the tunnel is a horizontal straight channel, two circles are drawn respectively by taking two reference base stations as circle centers and the distance between the two reference base stations and the UWB tag as radii. According to the difference of the ranging errors (the sum of the two ranging values is larger than, equal to or smaller than the distance between the two base stations), firstly analyzing whether the two circles are intersected;
if so (in the case of intersection of two circles), the one-dimensional position calculation formula of the UWB tag with respect to the reference base station is as follows: (solving the one-dimensional position of the tag C relative to the base station A by utilizing the relationship between three sides of the triangle and the included angle)
x D ＝AD
If not (the condition that two circles are separated or tangent), the coordinate value of the intersection point of the two circles and the AB is obtained, and then the midpoint of the X-axis coordinate of the two intersection points is used as the X-axis position of the mobile tag, namely the abscissa of the point D:
in this embodiment:
d: vertical projection of point C on AB;
AB: the length of the connection line between the base station A and the base station B;
alpha: the angle between the tag C and the base station a;
r1: a distance value between the tag C and the base station A;
r2: a distance value between the tag C and the base station B;
x D : the horizontal distance between the tag C and the base station A is calculated by using a formula;
as shown in fig. 8 and 9, if the gradient analysis result is that the tunnel is a ramp, two circles are drawn with two reference base stations as circle centers and the distance between the two reference base stations and the UWB tag as radii, respectively, and first, it is determined whether the relationship between the two circles is tangential or separated.
The condition that two circles are separated (tangent) can be considered that coordinate values of intersection points of the two circles and the AB are calculated firstly, and then the midpoint of X-axis coordinates of the two intersection points is used as the X-axis position of the mobile tag, namely, the abscissa of the point D:
x D ＝D·cosβ
under the condition that two circles intersect, solving the one-dimensional position of the tag C relative to the base station A according to the triangular three-side relationship:
x D ＝D·cosβ
in this embodiment:
d: vertical projection of point C on AB;
AB: the length of the connection line between the base station A and the base station B;
DC: length of line between points C and D
Alpha: the angle between the tag C and the base station a;
beta: the inclination angle of the ramp;
r1: a distance value between the tag C and the base station A;
r2: a distance value between the tag C and the base station B;
x D : the horizontal distance between the tag C and the base station A is calculated by using a formula;
s504, carrying out Kalman filtering on the generated UWB position resolving result to reduce adverse effects of random noise, and outputting a UWB positioning resolving result of a UWB tag, wherein the method comprises the following steps of:
state one-step prediction: x is X k|k-1 ＝FX k-1|k-1
One-step prediction covariance: p (P) k|k-1 ＝FP k-1|k-1 F T +Q
Filtering gain: k (K) k ＝P k|k-1 H T (HP k|k-1 H T +R) -1
State estimation: x is X k ＝X k|k-1 +K k (Z k -HX k|k-1 )
Estimating state covariance: p (P) k ＝P k|k-1 +K k HP k|k-1
Wherein, observed quantityState quantity->The observation matrix is H= [ 10 ]]Process noise covariance matrix->Measurement noise covariance matrix r= [0.15 2 ]。
Wherein k is the current sampling time; t is sampling period, X k The system state variable at the moment k, F is a state transition matrix and Z k For the system measurement variable at the moment k, H is an observation matrix and P k For the K moment error covariance matrix, K k For the filtering gain at time k, s x For displacement in the x direction, v x At the speed of the x-direction,the derived displacement is UWB-derived in the x-direction.
S600, taking a UWB positioning resolving result or a fusion positioning result as an IMU initial position, carrying out IMU one-dimensional positioning resolving by adopting a mechanical arrangement algorithm according to inertial data in a double-end queue, and generating an IMU mechanical arrangement resolving result;
s600 includes:
s601, acquiring a pitch angle, a roll angle and a yaw angle of an IMU device, and generating a posture matrix of the rubber-tyred vehicle from a b system to an n system;
the b series to the n series are subjected to three rotations, and the transformation matrix corresponding to each rotation is as follows:
the pose matrix from n to b is:
since the coordinate system always maintains the rectangular coordinate system in the process of rotation from the n system to the b system, the following characteristics are given according to the identity orthogonal matrix:
the pose matrix from b to n is:
wherein, psi is pitch angle; θ is the roll angle; gamma is the yaw angle; n is n; b is b;
s602, acquiring acceleration of the IMU device, and combining the acceleration with the gesture matrixMultiplying and integrating twice to calculate the relative position of the rubber-tyred vehicle at the moment t relative to the moment t-1;
the acceleration of the IMU device is converted into the following coordinates:
after the time deltat, the speed of the rubber-tyred vehicle is as follows:
after the time deltat, the positions of the rubber-tyred vehicle are as follows:
in the formula ,acceleration of line b at time t->Posture conversion matrix for b-series to n-series, < >>For the acceleration of the n series at time t,>for t-1 time n is the lower rubber-tyred vehicle speed,/->For t time n is the speed of the lower rubber-tyred vehicle, < + >>For t-1 time n is the lower rubber-tyred position,/->The position of the lower rubber tire vehicle is set at the time n of t.
After entering the tunnel portal, the rubber-tyred vehicle starts UWB positioning at first, continuously records 5s UWB positioning position information, and takes the 5s position information as an initial position for mechanical coding and decoding of the IMU.
S700, a Kalman filter is adopted to fuse the UWB positioning calculation result and the IMU mechanical coding calculation result in a loose combination mode, and a fusion positioning result is generated; storing the fusion positioning result to a vehicle-mounted database;
the UWB positioning resolving result and the IMU mechanical compiling resolving result are fused by adopting a Kalman filter in a loose combination mode, and a fusion positioning result is generated, and the method comprises the following steps:
the forward position and the forward speed of the IMU are used as state variables, the UWB positioning resolving result is used as a measuring variable, and a discrete state space model of the UWB and IMU loose combination positioning system is established:
the equation of state: x is X k ＝FX k-1 +W k-1
The measurement equation: z is Z k ＝HX k +V k
State one-step prediction: x is X k|k-1 ＝FX k-1|k-1
One-step prediction covariance: p (P) k|k-1 ＝FP k-1|k-1 F T +Q
Filtering gain: k (K) k ＝P k|k-1 H T (HP k|k-1 H T +R) -1
State estimation: x is X k ＝X k|k-1 +K k (Z k -HX k|k-1 )
Estimating state covariance: p (P) k ＝P k|k-1 +K k HP k|k-1
wherein , H＝[1 0]process noise partner->Measurement noise covariance matrix r= [0.1 2 ]；
Wherein k is the current sampling time; t is a sampling period; w is the process noise of the system, and is set as a white noise sequence with covariance Q; v is the measurement noise of the system, and is set as a white noise sequence with covariance of R, W and V are uncorrelated, X k The system state variable at the moment k, F is a state transition matrix and Z k For the system measurement variable at the moment k, H is an observation matrix and P k For the K moment error covariance matrix, K k For the filtering gain at time k, s x For displacement in the x direction, v x At the speed of the x-direction,the derived displacement is UWB-derived in the x-direction.
After the fusion positioning calculation is completed, the fusion positioning result is used as an initial position of the IMU for calculating and positioning at the next moment, and the position information is stored in a memory of the vehicle-mounted computer so as to send positioning information to the monitoring center later. The positioning process flow of the fusion positioning system is shown in fig. 1.
S800, displaying the fusion positioning result and transmitting the fusion positioning result to the cloud server through the wireless communication module. And the rubber-tyred vehicle is provided with a vehicle-mounted electronic map. The vehicle-mounted computer sends the fusion positioning result to the vehicle-mounted electronic map for display, and the fusion positioning result is transmitted to the cloud server through the wireless communication module, so that the ground monitoring center can acquire and display the position information of the rubber-tyred vehicle through the cloud server.
By adopting the scheme, the adverse effect of the accumulated error of the IMU is eliminated, the dynamic switching of the UWB reference base station is realized, the positioning instantaneity is improved, the capability of resisting UWB random measurement errors and NLOS errors is realized, and the accurate positioning of the rubber-tyred vehicle in the running state in the tunnel can be effectively adapted.
The foregoing is merely exemplary of the present application, and specific structures and features well known in the art will not be described in detail herein, so that those skilled in the art will be aware of all the prior art to which the present application pertains, and will be able to ascertain the general knowledge of the technical field in the application or prior art, and will not be able to ascertain the general knowledge of the technical field in the prior art, without using the prior art, to practice the present application, with the aid of the present application, to ascertain the general knowledge of the same general knowledge of the technical field in general purpose. It should be noted that modifications and improvements can be made by those skilled in the art without departing from the structure of the present application, and these should also be considered as the scope of the present application, which does not affect the effect of the implementation of the present application and the utility of the patent. The protection scope of the present application is subject to the content of the claims, and the description of the specific embodiments and the like in the specification can be used for explaining the content of the claims.

Claims (9)

1. The underground rubber-tyred vehicle fusion positioning method based on UWB and IMU is characterized in that: the method comprises the following steps:
s100, arranging a vehicle-mounted positioning processing device on a rubber-tyred vehicle, wherein the vehicle-mounted positioning processing device comprises a UWB tag, an IMU device, a vehicle-mounted computer and a wireless communication module; the antenna of the UWB tag is arranged at the top of the rubber-tyred car;
s200, deploying UWB base stations, wherein the UWB base stations comprise UWB starting point base stations and UWB intermediate base stations; deploying UWB starting point base stations at tunnel openings, and deploying a plurality of UWB intermediate base stations in the tunnel by taking the UWB starting point base stations as references; acquiring one-dimensional position data of each UWB intermediate base station relative to a UWB starting point base station and storing the data in a vehicle-mounted database;
s300, the UWB tag sends out a ranging signal; after each UWB base station receives the ranging signals, calculating the distance between the UWB base station and the UWB tag by adopting a bilateral ranging method, and feeding back the distance calculation result to the UWB tag; the vehicle-mounted computer reads the distance calculation result fed back by each UWB base station, sorts each UWB base station according to the distance between the UWB tag and each UWB base station, and stores the sorting result into the vehicle-mounted database;
s400, the vehicle-mounted computer reads inertial data output by the IMU device and stores the inertial data in a double-end queue;
s500, screening two UWB base stations as reference base stations according to the ordering result, performing UWB one-dimensional positioning calculation, performing Kalman filtering on the generated UWB position calculation result, and outputting a UWB positioning calculation result of a UWB tag;
s600, taking the UWB positioning resolving result as an IMU initial position, adopting a mechanical arrangement algorithm to carry out IMU one-dimensional positioning resolving according to the inertial data in the double-end queue, and generating an IMU mechanical arrangement resolving result;
s700, a Kalman filter is adopted to fuse the UWB positioning calculation result and the IMU mechanical coding calculation result in a loose combination mode, and a fusion positioning result is generated;
s800, displaying the fusion positioning result and transmitting the fusion positioning result to the cloud server through the wireless communication module.
2. The underground rubber-tyred vehicle fusion positioning method based on UWB and IMU according to claim 1, wherein the method comprises the following steps: the UWB base stations are all deployed on the top middle line of the tunnel;
s200 includes:
s201, deploying UWB starting point base stations at tunnel openings;
s202, measuring the effective distance measurement distance between the UWB base station and the UWB tag in the tunnel, and generating the base station distance according to the effective distance measurement distance; the distance between the base stations is smaller than the effective distance measurement distance;
s203, according to the distance between base stations, arranging a plurality of UWB intermediate base stations in a tunnel;
s204, establishing a two-dimensional reference coordinate system; taking a tunnel portal as a reference origin, acquiring a tunnel extending direction, and taking the tunnel extending direction as an x-axis positive direction, wherein the tunnel portal is an x-axis starting point; acquiring the horizontal height of the UWB tag, wherein the vertical upward direction is taken as the positive direction of the y axis, and the horizontal height of the UWB tag is taken as the starting point of the y axis;
s205, acquiring 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 one-dimensional position data in the vehicle-mounted database.
3. The underground rubber-tyred vehicle fusion positioning method based on UWB and IMU according to claim 2, wherein the method comprises the following steps: in S300, the UWB tag sends out ranging signals at intervals of a preset time, and the vehicle-mounted computer generates a corresponding sequencing result;
according to the sequencing result, two UWB base stations are screened as reference base stations, including:
s1, analyzing whether the number of UWB base stations in the sequencing result is less than three, if so, executing S2, and if not, executing S3;
s2, if the number of UWB base stations in the sequencing result is two, respectively taking the two UWB base stations in the sequencing result as a first reference base station and a second reference base station;
s3, taking one UWB base station closest to the UWB tag in the ordering result as a first reference base station, and taking two UWB base stations closest to the UWB tag in the rest UWB base stations as alternative base stations;
s4, judging whether the distances between the two alternative base stations and the UWB tag are equal, if so, executing S5, and if not, taking the UWB base station which is closer to the UWB tag as a second reference base station;
s5, analyzing whether the distance between the first reference base station and the UWB tag is smaller than a distance threshold, if yes, executing S6, and if not, taking the UWB base station which is closer to the UWB tag as a second reference base station;
s6, acquiring a last sequencing result generated by the vehicle-mounted computer; analyzing whether the last ordering result is the same as the ordering among the three UWB base stations closest to the UWB tag in the current ordering result, if so, taking the UWB base station closest to the UWB tag as a second reference base station, and if not, taking the UWB base station farther from the UWB tag as the second reference base station.
4. The underground rubber-tyred vehicle fusion positioning method based on UWB and IMU according to claim 1, wherein the method comprises the following steps: s400 includes:
s401, initializing a UART by a vehicle-mounted computer, and generating a designated port number and a data transmission baud rate;
s402, reading UART data and establishing a double-end queue with the length of 200;
s403, reading inertial data output by the IMU device, and storing the inertial data in a double-end queue.
5. The underground rubber-tyred vehicle fusion positioning method based on UWB and IMU according to claim 3, wherein the method comprises the following steps: s500 includes:
s501, acquiring the gradient of the tunnel, analyzing whether the gradient of the tunnel is 0, and generating a gradient analysis result;
s502, respectively drawing two circles by taking two reference base stations as circle centers and the distance between the two reference base stations and the UWB tag as radius; analyzing whether the two circles intersect or not, and generating a position relation analysis result;
s503, according to the gradient analysis result and the position relation analysis result, adopting a midpoint evaluation method or a trilateral evaluation method to perform UWB one-dimensional positioning calculation and generate a UWB position calculation result;
s504, the generated UWB position resolving result is subjected to Kalman filtering, and the UWB positioning resolving result of the UWB tag is output.
6. The underground rubber-tyred vehicle fusion positioning method based on UWB and IMU according to claim 1, wherein the method comprises the following steps: s600 includes:
s601, acquiring a pitch angle, a roll angle and a yaw angle of an IMU device, and generating a posture matrix of the rubber-tyred vehicle from a b system to an n system;
the b series to the n series are subjected to three rotations, and the transformation matrix corresponding to each rotation is as follows:
the pose matrix from n to b is:
since the coordinate system always maintains the rectangular coordinate system in the process of rotation from the n system to the b system, the following characteristics are given according to the identity orthogonal matrix:
the pose matrix from b to n is:
wherein, psi is pitch angle; θ is the roll angle; gamma is the yaw angle; n is n; b is b;
s602, acquiring acceleration of the IMU device, and combining the acceleration with the gesture matrixMultiplying and integrating twice to calculate the relative position of the rubber-tyred vehicle at the moment t relative to the moment t-1;
the acceleration of the IMU device is converted into the following coordinates:
after the time deltat, the speed of the rubber-tyred vehicle is as follows:
after the time deltat, the positions of the rubber-tyred vehicle are as follows:
in the formula ,acceleration of line b at time t->Posture conversion matrix for b-series to n-series, < >>For the acceleration of the n series at time t,>for t-1 time n is the lower rubber-tyred vehicle speed,/->For t time n is the speed of the lower rubber-tyred vehicle, < + >>For t-1 time n is the lower rubber-tyred position,/->The position of the lower rubber tire vehicle is set at the time n of t.
7. The underground rubber-tyred vehicle fusion positioning method based on UWB and IMU according to claim 1, wherein the method comprises the following steps: s700 further includes: storing the fusion positioning result to a vehicle-mounted database;
the UWB positioning resolving result and the IMU mechanical compiling resolving result are fused by adopting a Kalman filter in a loose combination mode, and a fusion positioning result is generated, and the method comprises the following steps:
establishing a discrete state space model of the UWB and IMU loose combination positioning system:
the equation of state: x is X k ＝FX k-1 +W k-1
The measurement equation: z is Z k ＝HX k +V k
State one-step prediction: x is X k|k-1 ＝FX k-1|k-1
One-step prediction covariance: p (P) k|k-1 ＝FP k-1|k-1 F T +Q
Filtering gain: k (K) k ＝P k|k-1 H T (HP k|k-1 H T +R) -1
State estimation: x is X k ＝X k|k-1 +K k (Z k -HX k|k-1 )
Estimating state covariance: p (P) k ＝P k|k-1 +K k HP k|k-1
wherein ,H＝[1 0]；
wherein k is the current sampling time; t is a sampling period; w is the process noise of the system, and is set as a white noise sequence with covariance Q; v is the measurement noise of the system, and is set as a white noise sequence with covariance of R, W and V are uncorrelated, X k The system state variable at the moment k, F is a state transition matrix and Z k For the system measurement variable at the moment k, H is an observation matrix and P k For the K moment error covariance matrix, K k For the filtering gain at time k, s x For displacement in the x direction, v x At the speed of the x-direction,the displacement obtained by UWB calculation in the x direction;
in S600, the UWB positioning resolving result or the fusion positioning result is taken as an IMU initial position, and according to inertial data in the double-end queue, the IMU one-dimensional positioning resolving is carried out by adopting a mechanical arrangement algorithm.
8. The underground rubber-tyred vehicle fusion positioning method based on UWB and IMU according to claim 1, wherein the method comprises the following steps: the antenna of the UWB tag is arranged above the central line of the tunnel ground.
9. The underground rubber-tyred vehicle fusion positioning method based on UWB and IMU according to claim 1, wherein the method comprises the following steps: a vehicle-mounted electronic map is arranged on the rubber-tyred vehicle;
in S800, the vehicle-mounted computer sends the fusion positioning result to the vehicle-mounted electronic map for display, and the fusion positioning result is transmitted to the cloud server through the wireless communication module.
CN202310603191.XA 2023-05-25 2023-05-25 Underground rubber-tyred vehicle fusion positioning method based on UWB and IMU Pending CN116582926A (en)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202310603191.XA CN116582926A (en) 2023-05-25 2023-05-25 Underground rubber-tyred vehicle fusion positioning method based on UWB and IMU

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202310603191.XA CN116582926A (en) 2023-05-25 2023-05-25 Underground rubber-tyred vehicle fusion positioning method based on UWB and IMU

Publications (1)

Publication Number Publication Date
CN116582926A true CN116582926A (en) 2023-08-11

Family

ID=87537569

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202310603191.XA Pending CN116582926A (en) 2023-05-25 2023-05-25 Underground rubber-tyred vehicle fusion positioning method based on UWB and IMU

Country Status (1)

Country Link
CN (1) CN116582926A (en)

Cited By (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN116761253A (en) * 2023-08-17 2023-09-15 湘江实验室 UWB weighted positioning method based on triangular area

Cited By (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN116761253A (en) * 2023-08-17 2023-09-15 湘江实验室 UWB weighted positioning method based on triangular area
CN116761253B (en) * 2023-08-17 2023-10-20 湘江实验室 UWB weighted positioning method based on triangular area

Similar Documents

Publication Publication Date Title
CN106556854B (en) A kind of indoor and outdoor navigation system and method
US20200033463A1 (en) Slam method and apparatus robust to wireless environment change
US11227168B2 (en) Robust lane association by projecting 2-D image into 3-D world using map information
CN110208842A (en) Vehicle high-precision locating method under a kind of car networking environment
CN113155123B (en) Multi-intelligent-vehicle cooperative localization tracking method and device based on data sharing
CN109443350B (en) Bluetooth/photoelectric/INS integrated navigation device and method based on neural network
CN111426320B (en) Vehicle autonomous navigation method based on image matching/inertial navigation/milemeter
CN107014375B (en) Indoor positioning system and method with ultra-low deployment
US20140316708A1 (en) Oriented Wireless Structural Health and Seismic Monitoring
CN103499350A (en) High-precision vehicle positioning method for fusing multi-source information under GPS (global positioning system) blind area and device
CN101476891A (en) Accurate navigation system and method for movable object
CN106840154B (en) Underground space inertial measurement and wireless sensor combination positioning system and method
US20230358541A1 (en) Inertial navigation system capable of dead reckoning in vehicles
US20170074678A1 (en) Positioning and orientation data analysis system and method thereof
CN110779496B (en) Three-dimensional map construction system, method, device and storage medium
CN111637888B (en) Tunneling machine positioning method and system based on inertial navigation and laser radar single-point distance measurement
US10132915B2 (en) System and method for integrated navigation with wireless dynamic online models
CN116582926A (en) Underground rubber-tyred vehicle fusion positioning method based on UWB and IMU
KR100525517B1 (en) Car navigation system and control method thereof
KR102396675B1 (en) Position estimation and 3d tunnel mapping system of underground mine autonomous robot using lidar sensor, and its method
JP5742794B2 (en) Inertial navigation device and program
CN104634345A (en) Indoor trajectory tracking method with self-adaptive step length
CN110617795A (en) Method for realizing outdoor elevation measurement by using sensor of intelligent terminal
JP2006119144A (en) Road linearity automatic survey device
CN113608556A (en) Multi-robot relative positioning method based on multi-sensor fusion

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination