Multi-sensor fusion positioning method

Abstract

This invention provides a multi-sensor fusion positioning method, comprising: processing ultra-wideband measurement data to obtain ultra-wideband positioning data; fusing inertial navigation data and satellite navigation data to obtain fused positioning data; processing laser point cloud data by combining inertial navigation data and prior maps to obtain laser point cloud positioning data; and fusing odometer data, fused positioning data, ultra-wideband positioning data, and laser point cloud positioning data to obtain a motion state estimation result for the target object. This invention solves the problem of low accuracy in vehicle navigation systems in existing technologies, especially the positioning drift problem in complex scenarios. This invention improves the robustness of the positioning system in complex scenarios (underground mining) by evaluating the information quality of different sensors and through efficient multi-source data fusion and collaborative positioning algorithms.

Description

Technical Field

[0001] This invention relates to the field of high-precision positioning technology, and more specifically, to a multi-sensor fusion positioning method. Background Technology

[0002] Currently, most positioning systems on the market rely on IMU (Inertial Measurement Unit) / GNSS (Global Navigation Satellite System) information fusion. They achieve centimeter-level accuracy by integrating RTK (Real-time kinematic) differential signal enhancement technology with specialized inertial navigation components. However, this cannot meet the accuracy requirements of positioning systems in complex road conditions (elevated bridges, multi-intersections, etc.). Especially in culvert and tunnel scenarios, the loss of satellite signals remains the root cause of positioning system failure.

[0003] Therefore, the present invention provides a multi-sensor fusion positioning method. Summary of the Invention

[0004] To address the shortcomings of existing technologies, this invention provides a multi-sensor fusion positioning method, the method comprising:

[0005] Ultra-wideband measurement data is processed to obtain ultra-wideband positioning data;

[0006] Inertial navigation data and satellite navigation data are fused to obtain fused positioning data;

[0007] By combining the inertial navigation data and the prior map, the laser point cloud data is processed to obtain laser point cloud positioning data;

[0008] By fusing odometer data, fused positioning data, ultra-wideband positioning data, and laser point cloud positioning data, the motion state estimation result of the target object is obtained.

[0009] According to an embodiment of the present invention, the ultra-wideband positioning data is obtained through the following steps:

[0010] The ultra-wideband measurement data is calibrated to correct the influence of temperature, air pressure, humidity, and sensor error factors on the ranging accuracy, resulting in ranging data that includes temperature ranging data, air pressure ranging data, humidity ranging data, and sensor ranging data.

[0011] Based on the ranging data, a positioning calculation is performed to obtain the ultra-wideband positioning data containing the three-dimensional position of the target object.

[0012] According to an embodiment of the present invention, the ultra-wideband measurement data is calibrated for ranging through the following steps:

[0013] The ultra-wideband measurement data is statistically analyzed, and the measurement results of the ultra-wideband tag and ultra-wideband base station, as well as the actual measurement results of the laser rangefinder or total station, are recorded in real time.

[0014] Based on the statistical results, principal component analysis and least squares methods were used for modeling, and bilateral two-way ranging was used to estimate the measurement time to obtain the ranging data.

[0015] According to an embodiment of the present invention, the ultra-wideband positioning data containing the three-dimensional position of the target object is obtained through the following steps:

[0016] The ranging data is preprocessed and then imported into a global topology map for pattern determination.

[0017] The ultra-wideband positioning data is obtained through multimodal positioning processing that includes one-dimensional positioning, two-dimensional positioning, and three-dimensional positioning.

[0018] According to an embodiment of the present invention, the fused positioning data is obtained through the following steps: based on the difference between the position and velocity information output by the inertial navigation data and the satellite navigation data, respectively, as a measurement value, an extended Kalman filter is used to estimate the errors of the inertial device and inertial navigation, and then the device error and navigation error are corrected to obtain the fused positioning data.

[0019] According to an embodiment of the present invention, the laser point cloud positioning data is obtained through the following steps:

[0020] Invalid points are removed and the laser point cloud data is filtered to obtain valid point cloud information, which is used as input for real-time scanning and positioning.

[0021] The inertial navigation data is synchronized and pre-integrated to obtain the change information of the inertial measurement sensor in adjacent time moments, which is used as the initial value input for scanning matching.

[0022] Obtain the prior map as the map for scanning and positioning;

[0023] Using the effective point cloud information, the transformation information, and the prior map, scan matching is achieved, pose optimization is performed, and the laser point cloud positioning data is obtained.

[0024] According to an embodiment of the present invention, the motion state estimation result of the target object is obtained through the following steps:

[0025] Based on the fused positioning data, a pose prediction model is established;

[0026] Based on the pose prediction model and the ultra-wideband positioning data, non-line-of-sight errors are identified.

[0027] Based on the odometry data, the fused positioning data, the ultra-wideband positioning data, and the laser point cloud positioning data, a filter observation equation is constructed;

[0028] Based on the filter observation equation, the extended Kalman filter algorithm is used to obtain the motion state estimation result including position, velocity, and attitude.

[0029] According to one embodiment of the present invention, non-line-of-sight errors are identified by the following expression:

[0030] The ranging residual is expressed as:

[0031]

[0032] Based on the ranging residual, determine whether the non-line-of-sight error exists:

[0033]

[0034] Where: Δρ i,k+1 For distance measurement residuals; Estimate the ranging residual for the IMU; For UWB observation ranging residuals; δρ max is the non-line-of-sight empirical threshold; i is the UWB base station number; k is the ranging time.

[0035] According to another aspect of the invention, a storage medium is also provided, which includes a series of instructions for performing the steps of the method described in any of the preceding claims.

[0036] According to another aspect of the present invention, a multi-sensor fusion positioning system is also provided, performing the method as described in any of the preceding claims, the system comprising:

[0037] The ultra-wideband positioning subsystem is used to process ultra-wideband measurement data to obtain ultra-wideband positioning data;

[0038] The inertial satellite navigation and positioning subsystem is used to fuse inertial navigation data and satellite navigation data to obtain fused positioning data.

[0039] The laser point cloud positioning subsystem is used to process the laser point cloud data by combining the inertial navigation data and the prior map to obtain laser point cloud positioning data.

[0040] The fusion subsystem is used to fuse odometer data, fused positioning data, ultra-wideband positioning data, and laser point cloud positioning data to obtain motion state estimation results for the target object.

[0041] The multi-sensor fusion positioning method provided by this invention has the following advantages compared with the prior art:

[0042] 1) This invention solves the problem of low accuracy in vehicle navigation systems in the prior art, especially the positioning drift problem in complex scenarios;

[0043] 2) This invention utilizes the evaluation of information quality from different sensors and improves the robustness of the positioning system in complex scenarios (underground mines) through efficient multi-source data fusion and collaborative positioning algorithms;

[0044] 3) This invention performs data synchronization processing between the ultra-wideband positioning subsystem, the inertial satellite navigation positioning subsystem, and the laser point cloud positioning subsystem to ensure data consistency at the front end of the fusion system;

[0045] 4) The communication and transmission of data in this invention are accomplished using the ROS system (Robot Operating System), which has a flexible system architecture and a short iteration cycle;

[0046] 5) The present invention can tailor the subsystems according to the actual scenario to achieve high-precision positioning in specific scenarios. For example, in scenarios with good satellite signals (open-pit mines), only the inertial satellite navigation and positioning subsystem can be used for high-precision positioning; in the scenario of personnel positioning in underground mines, the ultra-wideband positioning subsystem can be used; in the scenario of vehicle positioning in short-distance underground mines, the laser point cloud positioning subsystem can be used.

[0047] Other features and advantages of the invention will be set forth in the description which follows, and will be apparent in part from the description, or may be learned by practicing the invention. The objects and other advantages of the invention may be realized and obtained by means of the structures particularly pointed out in the description, claims, and drawings. Attached Figure Description

[0048] The accompanying drawings are provided to further illustrate the invention and form part of the specification. They are used in conjunction with the embodiments of the invention to explain the invention and do not constitute a limitation thereof. In the drawings:

[0049] Figure 1 A flowchart of a multi-sensor fusion localization method according to an embodiment of the present invention is shown;

[0050] Figure 2 A flowchart of adaptive calibration of ultra-wideband measurement data according to an embodiment of the present invention is shown;

[0051] Figure 3A schematic diagram of the localization calculation and a schematic diagram of the global topology map are shown according to an embodiment of the present invention;

[0052] Figure 4 A flowchart of a multimodal localization process according to an embodiment of the present invention is shown;

[0053] Figure 5 A schematic diagram of ultra-wideband positioning results according to an embodiment of the present invention is shown;

[0054] Figure 6 A schematic diagram illustrating the principle of inertial navigation data and satellite navigation data fusion positioning according to an embodiment of the present invention is shown.

[0055] Figure 7 A diagram illustrating the laser point cloud localization process architecture according to an embodiment of the present invention is shown.

[0056] Figure 8 A diagram illustrating the architecture of a multi-sensor fusion positioning system according to an embodiment of the present invention is shown.

[0057] In the accompanying drawings, the same parts use the same reference numerals. Also, the drawings are not drawn to scale. Detailed Implementation

[0058] To make the objectives, technical solutions, and advantages of the present invention clearer, the embodiments of the present invention will be described in further detail below with reference to the accompanying drawings.

[0059] This invention provides a solution to the positioning problem in environments such as long tunnels, dim lighting, weak / no GPS signals, and humid conditions. By fusing information from multiple sensors such as IMU (Inertial Measurement Unit), odometer, UWB (Ultra-Wide Band), and lidar, multi-sensor fusion positioning is achieved in special scenarios in underground mines.

[0060] This solution, based on existing technologies, integrates more positioning sources, such as vehicle speed (measured by a wheel speed encoder), UWB ranging information, and laser SLAM (Simultaneous Localization and Mapping) positioning information. Through a multi-sensor fusion algorithm, it achieves high-precision positioning in scenarios with weak or no GPS information, improving the system's robustness. This invention is applicable to both above-ground (with GPS) and underground (weak or no GPS) scenarios and their switching.

[0061] Figure 1 A flowchart of a multi-sensor fusion positioning method according to an embodiment of the present invention is shown.

[0062] like Figure 1 As shown, in step S1, the ultra-wideband measurement data is processed to obtain ultra-wideband positioning data.

[0063] In one embodiment, in step S1, ultra-wideband positioning data is obtained through the following steps: ranging calibration is performed on the ultra-wideband measurement data to correct the influence of temperature, air pressure, humidity, and sensor error factors on ranging accuracy, thereby obtaining ranging data that includes temperature ranging data, air pressure ranging data, humidity ranging data, and sensor ranging data; based on the ranging data, positioning calculation is performed to obtain the ultra-wideband positioning data that includes the three-dimensional position of the target object.

[0064] Figure 2 A flowchart of adaptive calibration of ultra-wideband measurement data according to an embodiment of the present invention is shown.

[0065] In one embodiment, step S1 involves the following steps to calibrate the ranging of the ultra-wideband measurement data: statistical analysis of the ultra-wideband measurement data, real-time recording of the measurement results of the ultra-wideband tag and ultra-wideband base station, and the actual measurement results of the laser rangefinder or total station. Based on the statistical results, principal component analysis and least squares methods are used for modeling, and bilateral bidirectional ranging is employed to estimate the measurement time, thereby obtaining the ranging data.

[0066] To apply UWB ranging information to precise positioning, it is first necessary to ensure the accuracy of the measured distance. Then, positioning calculations are performed based on multiple ranging information, and finally, the positioning information or ranging information is fused with other sensor data.

[0067] Because UWB signals are susceptible to interference from factors such as range, time deviation, and atmospheric conditions, significant ranging errors can occur when UWB is used in different industrial environments or in situations with large fluctuations in environmental factors, thus affecting positioning accuracy. Therefore, calibration is required to address these factors. The entire calibration process is as follows: Figure 2 As shown.

[0068] The data statistics process involves real-time recording of the measurement results from UWB tags and base stations, as well as the actual measurement results from laser rangefinders or total stations. Considering all factors, the relationship between the measured values ​​and the actual values ​​can be simplified to a linear function as follows:

[0069] d′=a*d+b(1)

[0070] Where d′ is the UWB tag measurement value; a is the scaling factor; d is the actual value measured by the laser scanner or total station; and b is the zero offset.

[0071] Then, principal component analysis and least squares methods are used to model these data.

[0072] Two-way ranging (TWR) determines the distance between a UWB tag and a UWB base station by measuring the round-trip time (RTT) of the UWB signal. It eliminates the need for time synchronization between the UWB base station and the tag, thus avoiding time synchronization errors in TOA / TDOA positioning algorithms. However, ordinary TWR ranging methods are generally uncommon because the increased error in time-of-flight estimation due to clock skew makes the estimation highly inaccurate. Double-sided two-way ranging (DS-TWR) is a variation of the basic one-sided two-way ranging method, using two RTT measurements and combining them to obtain the time-of-flight result. It uses the response to the first RTT measurement as the initiation for the second RTT measurement, simplifying it to three messages and saving communication time. The use of an "asymmetric side" algorithm to estimate the measurement time further reduces the impact of clock skew on the measurement time.

[0073] In one embodiment, step S1 involves obtaining ultra-wideband positioning data containing the three-dimensional position of the target object through the following steps: After preprocessing the ranging data, it is imported into a global topology map for pattern determination; based on the pattern determination result, ultra-wideband positioning data is obtained through multimodal positioning processing that includes one-dimensional, two-dimensional, and three-dimensional positioning. Specifically, the purpose of pattern determination is to determine whether to use one-dimensional, two-dimensional, or three-dimensional positioning.

[0074] In terms of positioning, conventional positioning methods have certain requirements regarding the number of effective base stations detected by UWB tags and the environmental configuration. Therefore, additional location considerations are needed during base station deployment, which increases the difficulty of base station deployment. This invention proposes a multimodal UWB positioning method based on a topology map. A positioning diagram is shown below. Figure 3 As shown.

[0075] After the base station locations are mapped and imported into the positioning system, a global topology map is automatically generated based on Euclidean distance. Combining the topology map with the ranging information, the host computer first determines the positioning mode and then executes the corresponding positioning process. This positioning mode process is as follows: Figure 4 As shown.

[0076] In the 3D positioning mode, assuming there are four base stations with coordinates A0(x0,y0,z0), A1(x1,y1,z1), A2(x2,y2,z2), and A3(x3,y3,z3), three distance information d0, d1, d2, and d3 are obtained, along with:

[0077]

[0078] Similarly, using the least squares method, we can obtain:

[0079]

[0080] In the formula, K i d is the square of the Euclidean distance between the base station and the origin. i,0 d represents the distance difference from different tags to base station i; i z is the distance from the tag to base station i; a G represents the distance covariance of observed values. a Let be the transition matrix; h be the observation matrix; and Q represent the measurement covariance based on TDOA (Time Difference of Arrival). Using this result as the initial value, the Taylor algorithm (series expansion algorithm) is used for optimization, obtaining the optimal value through multiple iterations. First, the deviations (Δx, Δy, Δz) of the node positions are calculated. Then, (x0, y0, z0) calculated in each iteration are substituted into the next iteration until (Δx, Δy, Δz) satisfies the following condition, at which point the estimated value of the tag's position can be obtained. A localization result for a given instance is shown below. Figure 5 As shown.

[0081]

[0082] Where: Δx is the deviation of the tag position coordinates in the X-axis direction of the navigation coordinate system; Δy is the deviation of the tag position coordinates in the Y-axis direction of the navigation coordinate system; Δz is the deviation of the tag position coordinates in the Z-axis direction of the navigation coordinate system; This is the deviation threshold of the label position coordinates in the navigation coordinate system.

[0083] like Figure 1 As shown, in step S2, the inertial navigation data and satellite navigation data are fused to obtain fused positioning data.

[0084] In one embodiment, in step S2, fused positioning data is obtained through the following steps: based on the difference between the position and velocity information output by the inertial navigation data and the satellite navigation data, the extended Kalman filter is used to estimate the errors of the inertial device and inertial navigation, and then the device error and navigation error are corrected to obtain fused positioning data.

[0085] Figure 6 A schematic diagram illustrating the principle of inertial navigation data and satellite navigation data fusion positioning according to an embodiment of the present invention is shown.

[0086] like Figure 6As shown, based on the difference between the position and velocity information output by inertial navigation data and satellite navigation data, respectively, extended Kalman filtering is used to estimate the errors of the inertial devices and inertial navigation. Then, the device errors and navigation errors are corrected to improve the accuracy of inertial navigation. Furthermore, in the integrated navigation process of the Inertial Navigation System (INS) and GPS, the INS and GPS operate independently, and their combined effect is mainly manifested in GPS assisting inertial navigation; the output of the integrated system is the fused navigation and positioning result of GPS and the INS.

[0087] like Figure 1 As shown, in step S3, the laser point cloud data is processed by combining inertial navigation data and prior maps to obtain laser point cloud positioning data.

[0088] In one embodiment, step S3 involves obtaining laser point cloud positioning data through the following steps: invalid point removal and point cloud filtering are performed on the laser point cloud data to obtain valid point cloud information, which serves as the input for real-time scanning positioning; data synchronization and pre-integration processing are performed on the inertial navigation data to obtain the transformation information of the inertial measurement sensor within adjacent time intervals, which serves as the input for initial values ​​of scanning matching; a priori map is acquired as the map for scanning positioning; and scanning matching is achieved through the valid point cloud information, transformation information, and priori map, and pose optimization is performed to obtain laser point cloud positioning data.

[0089] Figure 7 A diagram illustrating the laser point cloud localization process architecture according to an embodiment of the present invention is shown.

[0090] like Figure 7 As shown, in the laser point cloud data processing module, data from the lidar sensor is acquired. First, invalid points are removed and the point cloud is filtered to obtain valid point cloud information, which is then transmitted to the laser point cloud positioning module as input for real-time scanning and positioning. The IMU data processing module performs data synchronization and pre-integration processing to obtain the IMU sensor's change information within adjacent timeframes, which serves as the initial input for scanning matching in the laser point cloud positioning module. The map loading module loads the map provided by the client and inputs it into the laser point cloud positioning module as the map for scanning and positioning. Finally, the laser point cloud positioning module, using the acquired IMU pre-integration information, the laser point cloud, and the prior map, performs scanning matching, pose optimization, and outputs the final result.

[0091] like Figure 1 As shown, in step S4, the odometer data, fused positioning data, ultra-wideband positioning data, and laser point cloud positioning data are fused to obtain the motion state estimation result of the target object.

[0092] In one embodiment, step S4 involves obtaining the motion state estimation result of the target object through the following steps: establishing a pose prediction model based on fused positioning data; identifying non-line-of-sight errors based on the pose prediction model and combined with ultra-wideband positioning data; constructing a filter observation equation based on odometry data, fused positioning data, ultra-wideband positioning data, and laser point cloud positioning data; and obtaining the motion state estimation result including position, velocity, and attitude by using the extended Kalman filter algorithm based on the filter observation equation.

[0093] Based on fused positioning data, a pose prediction model for the navigation and positioning system is established. The nonlinear differential equation consisting of state variables, system input, and noise is as follows:

[0094]

[0095] To compensate for IMU installation errors and address the arm effect of the GNSS antenna and UWB tag: Let the installation position of the GNSS antenna / UWB tag in the vehicle body coordinate system with the IMU as the origin be l. b Let the installation error angles of the IMU be the error elevation angle δα, the error roll angle δγ, and the error azimuth angle δβ. Then, the arm l of the GNSS antenna / UWB tag in the IMU coordinate system... i for:

[0096] l i =(I-μ×)l b (6)

[0097] in:

[0098]

[0099] Position P of GNSS antenna / UWB tag in navigation coordinate system t for:

[0100]

[0101] in: R is the position derivative; v is the velocity in the navigation system; wb Here is the attitude rotation matrix; a is the compensated acceleration value; a m To measure the specific force using a gauge; n a To add Gaussian white noise values; b a To add zero bias; g is the acceleration due to gravity; Ω is the quaternion differential; Ω is the quaternion at the previous moment; ω is the gyroscope measurement after compensation; ω m To measure the angular velocity of a gyroscope; n ω b is Gaussian white noise for the gyroscope; ω The gyroscope has zero bias; w ωw is the gyroscope noise time constant. a is the noise time constant during the table addition process; is ; I is the identity matrix; μ is the symmetric matrix composed of IMU installation error angles; P is the position measured by the GNSS antenna / UWB tag; This is the attitude transformation matrix.

[0102] Based on the IMU's predicted status, determine the handling of UWB non-line-of-sight (the most direct explanation of non-line-of-sight is that the two points of communication have their line of sight blocked, and they cannot see each other; more than 50% of the Fresnel zone is blocked) situation:

[0103] Because obstacles may obstruct UWB measurements, non-line-of-sight (NLOS) errors will exist in the ranging information. These errors need to be assessed before filter fusion. To fully utilize UWB ranging information, and considering the independence between each UWB base station, a measurement update method with dynamically changing measurement equation dimension is adopted. The ranging residual can be expressed as:

[0104]

[0105] Based on the ranging residual, determine whether there is a non-line-of-sight error:

[0106]

[0107] Where: Δρ i,k+1 For distance measurement residuals; Estimate the ranging residual for the IMU; For UWB observation ranging residuals; δρ max is the non-line-of-sight empirical threshold; i is the UWB base station number; k is the ranging time. When the ranging residual is greater than the non-line-of-sight empirical threshold, the ranging information is discarded.

[0108] Let the position, velocity, and heading angle in the satellite navigation data be p, ... g v g ψ g The number of ultra-wideband base stations within the measurement range is n, and the wheel speed odometer speed measurement value is v. o The lidar odometer measurement value is Δp l , Observation is defined as:

[0109]

[0110] The filter observation equation is:

[0111] Z k+1 =Hx k+1 +υ k+1 (12)

[0112] Based on the above filter observation equation, using the extended Kalman filter algorithm, the filter measurement update first calculates the filter update gain matrix:

[0113] K = P k+1 H T HP k+1 H T +R) -1 (13)

[0114] Update the filter state variables:

[0115] x k+1 =x k+1|k +K(Z k+1 -y k+1 (14)

[0116] Update the filter covariance:

[0117] P k+1 =P k+1|k -KHP k+1|k (15)

[0118] Where: ρ 1,k+1 …ρ n,k+1 Let n be the observable values ​​of the UWB base station and the distance measured from the tag at time k; H is the transformation matrix from state variables to measurements (observations), representing the relationship connecting the state and the observations; x k+1 The state of the system at time k+1; υ k+1 The noise observed follows a Gaussian distribution; P k+1 R is the state variance matrix; R is the observation noise matrix; y k+1 For observations; Hx k+1 This represents the state transition matrix multiplied by the state variables; υ k+1 This indicates the noise level in the observation.

[0119] This enables the fusion of GNSS, UWB, IMU, and lidar data to obtain state estimates such as system position, velocity, and attitude.

[0120] Compared with the prior art, this invention adds sensors such as UWB and LiDAR; this invention adds a ranging and positioning algorithm for UWB; this invention adds an inertial / laser SLAM positioning algorithm for LiDAR / IMU; this invention adds time alignment and spatial synchronization compensation algorithms for each system, and implements them through Ubuntu (a Linux operating system mainly for desktop applications) + ROS system.

[0121] The multi-sensor fusion localization method provided by this invention can also be used in conjunction with a computer-readable storage medium. The storage medium stores a computer program, which is executed to run the multi-sensor fusion localization method. The computer program is capable of executing computer instructions, which include computer program code. The computer program code can be in the form of source code, object code, executable file, or some intermediate form.

[0122] Computer-readable storage media can include: any entity or device capable of carrying computer program code, recording media, USB flash drives, portable hard drives, magnetic disks, optical disks, computer memory, read-only memory (ROM), random access memory (RAM), electrical carrier signals, telecommunication signals, and software distribution media, etc.

[0123] It should be noted that the contents of computer-readable storage media may be appropriately added to or subtracted from the contents according to the requirements of legislation and patent practice in a jurisdiction. For example, in some jurisdictions, according to legislation and patent practice, computer-readable storage media may not include electrical carrier signals and telecommunication signals.

[0124] Figure 8 A diagram illustrating the architecture of a multi-sensor fusion positioning system according to an embodiment of the present invention is shown.

[0125] According to another aspect of the present invention, a multi-sensor fusion positioning system is also provided, which performs a multi-sensor fusion positioning method, the system comprising: an ultra-wideband positioning subsystem, an inertial satellite navigation positioning subsystem, a laser point cloud positioning subsystem, and a fusion subsystem.

[0126] The ultra-wideband (UWB) positioning subsystem processes UWB measurement data to obtain UWB positioning data. Specifically, the UWB positioning subsystem hardware includes an UWB base station and UWB tags. The base station is installed at a designated location within the tunnel (coordinates known), and the tags are installed at specific locations on the outside of vehicles, connected to the system via a serial port. Positioning includes two functions: UWB ranging calibration and UWB positioning calculation. UWB ranging calibration corrects for the effects of factors such as temperature, air pressure, humidity, and sensor errors on ranging accuracy, outputting ranging data that meets accuracy requirements under different environments. UWB positioning calculation, based on four or more ranging data points, allows the tag to calculate its own three-dimensional position, which can be used for the state initialization of the precise navigation and positioning system.

[0127] The inertial satellite navigation and positioning subsystem is used to fuse inertial navigation data and satellite navigation data to obtain fused positioning data. Specifically, the inertial satellite navigation and positioning subsystem is installed in a vehicle-specific equipment box. The hardware includes an inertial measurement unit, a GPS receiver, a GPS receiving antenna, a differential radio, and an antenna. The positioning method employs a position-velocity fusion approach.

[0128] The laser point cloud positioning subsystem is used to process laser point cloud data by combining inertial navigation data and prior maps to obtain laser point cloud positioning data. Specifically, the laser point cloud subsystem uses a LiDAR installed at a specific location on the vehicle and employs a scanning matching positioning method. Based on a pre-collected high-precision 3D point cloud map and onboard sensors such as the LiDAR and inertial measurement unit (IMU), the system achieves vehicle positioning. The system as a whole consists of four parts: laser point cloud data processing, IMU data processing, map loading, and laser point cloud positioning. The pre-constructed 3D point cloud map is provided by the client.

[0129] The fusion subsystem is used to fuse odometer data, fusion positioning data, ultra-wideband positioning data, and laser point cloud positioning data to obtain the motion state estimation result of the target object. Specifically, the fusion subsystem uses an extended Kalman filter to fuse odometer, Euler angle, UWB positioning data, and laser reflection positioning data in a loosely coupled manner to achieve the optimal estimation of the vehicle's motion state.

[0130] The aforementioned fusion positioning and attitude determination system, composed of sensors such as GNSS, dual-label UWB, IMU, and lidar, provides the IMU with continuous high-frequency system prediction status and the ability to determine UWB non-line-of-sight situations. The dual-label UWB can calculate the vehicle's heading, giving the system complete attitude measurement information.

[0131] The multi-sensor fusion positioning system integrates information from sensors such as IMU, GPS, wheel speed odometer, UWB, and LiDAR. Through efficient multi-source data fusion and collaborative positioning algorithms, it achieves high-precision positioning in underground mining environments. Due to the redundant design concept in sensor configuration and the development of a fault-tolerant state estimation algorithm, when a sensor experiences a short-term anomaly, the system can determine the anomaly based on the fused data state and automatically filter out the abnormal data, thus not affecting the navigation and positioning state estimation data, and the system can still operate normally.

[0132] like Figure 8 As shown, under the control of the central control unit, each subsystem, with the help of positioning software, can carry out inertial navigation positioning experiments, satellite positioning experiments, UWB positioning experiments, and laser point cloud positioning experiments. This provides a research platform and experimental conditions for conducting research on autonomous and controllable underground rapid positioning technology and for realizing long-distance, all-weather, and fully automatic rapid positioning of vehicles.

[0133] Inertial navigation system (INS) data, satellite positioning receiver data, UWB data, and lidar data can be acquired in real time through corresponding drivers. The data is aligned via a time server and sent to the computing device. The computing device performs appropriate data processing to achieve various positioning estimation methods, including INS positioning, GNSS point positioning, GNSS / INS integrated navigation positioning (wheel speed), and lidar SLAM / INS / wheel speed / UWB / positioning. Data communication and transmission are accomplished using the ROS system, a near real-time system capable of meeting the requirements for real-time data acquisition and positioning.

[0134] In summary, the multi-sensor fusion positioning method provided by this invention has the following advantages compared with the prior art:

[0135] 1) This invention solves the problem of low accuracy in vehicle navigation systems in the prior art, especially the positioning drift problem in complex scenarios;

[0136] 2) This invention utilizes the evaluation of information quality from different sensors and improves the robustness of the positioning system in complex scenarios (underground mines) through efficient multi-source data fusion and collaborative positioning algorithms;

[0137] 3) This invention performs data synchronization processing between the ultra-wideband positioning subsystem, the inertial satellite navigation positioning subsystem, and the laser point cloud positioning subsystem to ensure data consistency at the front end of the fusion system;

[0138] 4) The communication and transmission of data in this invention are accomplished using the ROS system (Robot Operating System), which has a flexible system architecture and a short iteration cycle;

[0139] 5) The present invention can tailor the subsystems according to the actual scenario to achieve high-precision positioning in specific scenarios. For example, in scenarios with good satellite signals (open-pit mines), only the inertial satellite navigation and positioning subsystem can be used for high-precision positioning; in the scenario of personnel positioning in underground mines, the ultra-wideband positioning subsystem can be used; in the scenario of vehicle positioning in short-distance underground mines, the laser point cloud positioning subsystem can be used.

[0140] It should be understood that the embodiments disclosed herein are not limited to the specific structures, processing steps, or materials disclosed herein, but should be extended to equivalent substitutions of these features as understood by those skilled in the art. It should also be understood that the terminology used herein is for the purpose of describing particular embodiments only and is not intended to be limiting.

[0141] In the description of this invention, unless otherwise stated, "a plurality of" means two or more; the terms "upper," "lower," "left," "right," "inner," "outer," "front end," "rear end," "head," "tail," etc., indicate the orientation or positional relationship based on the orientation or positional relationship shown in the accompanying drawings, and are only for the convenience of describing the invention and simplifying the description, and do not indicate or imply that the device or element referred to must have a specific orientation, or be constructed and operated in a specific orientation, and therefore should not be construed as a limitation of the invention. Furthermore, the terms "first," "second," "third," etc., are used for descriptive purposes only and should not be construed as indicating or implying relative importance.

[0142] In the description of this invention, it should be noted that, unless otherwise explicitly specified and limited, the terms "connected" and "linked" should be interpreted broadly. For example, they can refer to a fixed connection, a detachable connection, or an integral connection; they can refer to a mechanical connection or an electrical connection; they can refer to a direct connection or an indirect connection through an intermediate medium. Those skilled in the art can understand the specific meaning of the above terms in this invention based on the specific circumstances.

[0143] Certain terms are used throughout this application to refer to specific system components. As those skilled in the art will recognize, the same components may often be referred to by different names, and therefore this application is not intended to distinguish those components that differ only in name and not in function. In this application, the terms “comprise,” “include,” and “have” are used in an open-ended manner and should therefore be interpreted as meaning “including, but not limited to…”. Furthermore, the terms “substantially,” “materially,” or “approximately” as used herein refer to industry-accepted tolerances for the corresponding terms. The term “coupling,” as may be used herein, includes direct coupling and indirect coupling via additional components, elements, circuits, or modules, wherein, for indirect coupling, the intermediate component, element, circuit, or module does not alter the information of the signal but may adjust its current level, voltage level, and / or power level. Inferred coupling (e.g., one element is inferredly coupled to another element) includes direct and indirect coupling between two elements in the same manner as “coupling.”

[0144] The phrase "an embodiment" or "an embodiment" used in this specification means that a particular feature, structure, or characteristic described in connection with the embodiment is included in at least one embodiment of the invention. Therefore, the phrase "an embodiment" or "an embodiment" appearing in various places throughout the specification does not necessarily refer to the same embodiment.

[0145] The embodiments of the present invention are given for illustrative and descriptive purposes only, and are not intended to be exhaustive or to limit the invention to the forms disclosed. Many modifications and variations will be apparent to those skilled in the art. The embodiments were chosen and described in order to better illustrate the principles and practical application of the invention, and to enable those skilled in the art to understand the invention and to design various embodiments with various modifications suitable for a particular purpose.

[0146] While the embodiments disclosed in this invention are as described above, the content is merely for the purpose of facilitating understanding of the invention and is not intended to limit the invention. Any person skilled in the art to which this invention pertains may make any modifications and variations in form and detail of the implementation without departing from the spirit and scope disclosed herein; however, the scope of patent protection for this invention shall still be determined by the scope defined in the appended claims.

Claims

1. A multi-sensor fusion positioning method, characterized in that, The method includes: Ultra-wideband measurement data is processed to obtain ultra-wideband positioning data; Inertial navigation data and satellite navigation data are fused to obtain fused positioning data; By combining the inertial navigation data and the prior map, the laser point cloud data is processed to obtain laser point cloud positioning data; By fusing odometer data, the fused positioning data, the ultra-wideband positioning data, and the laser point cloud positioning data, the motion state estimation result of the target object is obtained; The ultra-wideband positioning data is obtained through the following steps: the ultra-wideband measurement data is calibrated for ranging to correct the influence of temperature, air pressure, humidity, and sensor error factors on ranging accuracy, resulting in ranging data that includes temperature ranging data, air pressure ranging data, humidity ranging data, and sensor ranging data; based on the ranging data, positioning calculation is performed to obtain the ultra-wideband positioning data that includes the three-dimensional position of the target object. The ultra-wideband measurement data is calibrated by means of the following steps: statistical analysis of the ultra-wideband measurement data is performed, and the measurement results of the ultra-wideband tag and the ultra-wideband base station, as well as the actual measurement results of the laser rangefinder or total station are recorded in real time; based on the statistical results, principal component analysis and least squares method are used to model the data, and bilateral bidirectional ranging is used to estimate the measurement time to obtain the ranging data.

2. The multi-sensor fusion positioning method as described in claim 1, characterized in that, The ultra-wideband positioning data containing the three-dimensional position of the target object is obtained through the following steps: The ranging data is preprocessed and then imported into a global topology map for pattern determination. Based on the pattern determination results, the ultra-wideband positioning data is obtained through multimodal positioning processing that includes one-dimensional positioning, two-dimensional positioning, and three-dimensional positioning.

3. The multi-sensor fusion positioning method as described in claim 1, characterized in that, The fused positioning data is obtained through the following steps: based on the difference between the position and velocity information output by the inertial navigation data and the satellite navigation data, the extended Kalman filter is used to estimate the errors of the inertial device and inertial navigation, and then the device error and navigation error are corrected to obtain the fused positioning data.

4. The multi-sensor fusion positioning method as described in claim 1, characterized in that, The laser point cloud positioning data is obtained through the following steps: Invalid points are removed and the laser point cloud data is filtered to obtain valid point cloud information, which is used as input for real-time scanning and positioning. The inertial navigation data is synchronized and pre-integrated to obtain the change information of the inertial measurement sensor in adjacent time moments, which is used as the initial value input for scanning matching. Obtain the prior map as the map for scanning and positioning; Using the effective point cloud information, the transformation information, and the prior map, scan matching is achieved, pose optimization is performed, and the laser point cloud positioning data is obtained.

5. A multi-sensor fusion positioning method as described in any one of claims 1-4, characterized in that, The motion state estimation result of the target object is obtained through the following steps: Based on the fused positioning data, a pose prediction model is established; Based on the pose prediction model and the ultra-wideband positioning data, non-line-of-sight errors are identified. Based on the odometry data, the fused positioning data, the ultra-wideband positioning data, and the laser point cloud positioning data, a filter observation equation is constructed; Based on the filter observation equation, the extended Kalman filter algorithm is used to obtain the motion state estimation result including position, velocity, and attitude.

6. The multi-sensor fusion positioning method as described in claim 5, characterized in that, Non-line-of-sight errors are identified using the following expression: The ranging residual is expressed as: Based on the ranging residual, determine whether the non-line-of-sight error exists: Where: Δρ i,k+1 For distance measurement residuals; Estimate the ranging residual for the IMU; For UWB observation ranging residuals; δρ max is the non-line-of-sight empirical threshold; i is the UWB base station number; k is the ranging time.

7. A storage medium, characterized in that, It includes a series of instructions for performing the method steps as described in any one of claims 1-6.

8. A multi-sensor fusion positioning system, characterized in that, The system, which performs the method as described in any one of claims 1-6, comprises: The ultra-wideband positioning subsystem is used to process ultra-wideband measurement data to obtain ultra-wideband positioning data; The inertial satellite navigation and positioning subsystem is used to fuse inertial navigation data and satellite navigation data to obtain fused positioning data. The laser point cloud positioning subsystem is used to process the laser point cloud data by combining the inertial navigation data and the prior map to obtain laser point cloud positioning data. The fusion subsystem is used to fuse odometer data, fused positioning data, ultra-wideband positioning data, and laser point cloud positioning data to obtain motion state estimation results for the target object.

Images