CN117705104A

CN117705104A - Robust autonomous positioning and mapping method, device and medium based on UWB and IMU fusion

Info

Publication number: CN117705104A
Application number: CN202311700420.6A
Authority: CN
Inventors: 何雷; 王聪; 马文峰; 田辉
Original assignee: Army Engineering University of PLA
Current assignee: Army Engineering University of PLA
Priority date: 2023-12-12
Filing date: 2023-12-12
Publication date: 2024-03-15

Abstract

The invention provides a robust autonomous positioning and mapping method, device and medium based on UWB and IMU fusion in a GNSS refusing environment. The four-rotor unmanned aerial vehicle has the characteristic of strong nonlinearity, in order to solve the estimation problem under the strong nonlinearity condition, the sensor is expanded more conveniently simultaneously, and the method designs a set of positioning frame taking unscented Kalman filtering (Unscented Kalman Filter, UKF) as a core, so that the UWB and the inertial sensor can be mutually complemented in function. In the positioning process, the IMU can be matched with UWB to improve positioning accuracy, and good positioning performance can be still shown when the UWB measurement temporarily shows outliers. The positioning algorithm framework has strong universality, other sensors can be expanded, and the positioning algorithm framework is also fused with fixed-height radar measurement, so that the estimation accuracy of the z-axis direction is improved, and a multi-sensor redundancy strategy is designed for the stable flight of the unmanned aerial vehicle. By designing the four-rotor unmanned aerial vehicle platform and running the positioning algorithm, the unmanned aerial vehicle can be positioned quickly and accurately, and the method has great application value.

Description

Robust autonomous positioning and mapping method, device and medium based on UWB and IMU fusion

Technical Field

The invention relates to a robust autonomous positioning and mapping method, device and medium based on UWB and IMU fusion, and belongs to the technical field of autonomous positioning.

Background

Unmanned systems have been increasingly used in various fields in recent years, such as environmental exploration, post-disaster search and rescue, automatic driving, etc., which benefit from the fact that unmanned systems can autonomously act and perform specific functions in unmanned situations. Unmanned aerial vehicle flight typically relies on global navigation satellite systems (Global Navigation Satellite System, GNSS) for positioning, which meets the positioning requirements of a large number of outdoor unmanned aerial vehicles. However, many application scenarios are complex and unknown, and often, GNSS signals are not good or even lost, such as indoor environments, caverns, jungle environments, etc., where GNSS cannot provide effective positioning information, which greatly limits the application of the unmanned aerial vehicle. Therefore, the method for autonomous positioning and navigation through the sensor of the unmanned aerial vehicle is a popular research topic in the field of unmanned aerial vehicles at present.

The technique of autonomous navigation by a device carried by the unmanned aerial vehicle itself, independent of external sensors, is called SLAM (Simultaneous LocalizationAndMapping) technique. The autonomous positioning problem of the unmanned aerial vehicle is a state estimation problem, and the state of the unmanned aerial vehicle can comprise the states of the unmanned aerial vehicle, such as position, speed, gesture and the like. The reliable attitude estimation can ensure the stable flight of the four rotors, and the reliable position estimation is an important guarantee of environment perception, obstacle avoidance, motion path planning and unmanned plane control. The map building problem refers to that the unmanned aerial vehicle carries out perception reconstruction on the surrounding environment, and is the basis of obstacle avoidance and motion planning of the unmanned aerial vehicle. As can be seen, SLAM technology is one of the core technologies for various upper-layer applications such as environmental exploration, rescue and relief work.

Ultra-wideband (UWB) is a novel electric wave ranging technology in recent years that calculates a distance between two modules by measuring an arrival time, an arrival time difference, or an arrival angle of an electric wave. Because the wave band of the emitted electric wave is between 3.1GHz and 4.8GHz, the interference of other electric wave signals can be effectively overcome. In addition, the higher bandwidth can easily overcome the multipath effect and weaken the influence of non-line-of-sight measurement.

The UWB positioning method can complete position estimation in the GPS rejection environment only by deploying a low-cost sensor, is not influenced by illumination and weather, and more importantly has a global reference system, thereby providing convenience for direct application of multiple unmanned aerial vehicles. However, the positioning effect of the UWB sensor is not ideal when the UWB sensor acts alone, firstly, UWB signals are easy to be interfered, the positioning precision is reduced to some extent under the non-line-of-sight propagation scene with dense obstacles, and the stability is general; secondly, the accuracy of the UWB positioning method is not high due to the adoption of a constant speed assumption; thirdly, the update rate of the position estimation is limited by the UWB measurement frequency, about 20Hz, and the stability of the unmanned aerial vehicle is kept, and the update rate of the position is at least more than 100Hz, which obviously does not meet the requirement of the controller on the state estimation.

Disclosure of Invention

The invention aims to overcome the defects in the prior art, and aims to provide a robust autonomous positioning and mapping method, device and medium based on the fusion of UWB and IMU, so as to improve the positioning performance of an unmanned aerial vehicle in a GPS refusing environment.

In order to achieve the above purpose, the invention is realized by adopting the following technical scheme:

in a first aspect, the present invention provides a robust autonomous positioning and mapping method based on UWB and IMU fusion, comprising the steps of:

step 1: acquiring UWB original data and IMU data;

step 2: performing distance calibration on the UWB original data to obtain calibrated UWB data;

step 3: judging whether the anchor point position is known or not, if not, carrying out anchor point calibration according to the calibrated UWB data, and then jumping to the step 4, if yes, jumping to the step 4;

step 4: performing outlier detection on the calibrated UWB data to obtain screened UWB data;

step 5: performing low-pass filtering on the IMU data to obtain filtered IMU data;

step 6: and inputting the screened UWB data and the filtered IMU data into a state estimator to obtain the position, speed and attitude information of the unmanned aerial vehicle. The state estimator approximates probability distribution of states by taking unscented Kalman filtering as a core, so that mean and covariance are estimated.

The input of the state estimator is UWB ranging data after information calibration and outlier rejection and IMU data after noise processing through low-pass filtering; the output is the position, speed and attitude information of the unmanned aerial vehicle.

Further, step 2: performing distance calibration on the UWB original data, including:

measuring UWB distanceModeling is linear relation with the true value of the distance d, namely:

wherein n is _d Is zero mean white noise.

A linear regression method is adopted, and a plurality of true value-measurement samples are obtained by placing two UWB modules at different positions for distance measurementThe measured value in each sample is a reading of the UWB range, and the range truth value is obtained by a high-precision indoor positioning system. And (3) determining the scale factor a and the static difference b under the current model through fitting the model by a large number of samples.

Where a is the mean of the distance truth values,is the mean of the distance measurements.

Further, in step 3, performing anchor point calibration according to the calibrated UWB data, including:

firstly, a global coordinate system is established, and the coordinate system is an anchor point A in the overlooking view ₁ The position is the origin of the coordinate system, the anchor point A ₂ And A is a ₁ The connecting line of (a) is the direction of the x-axis, A ₁ The height direction of (2) is the z-axis and the y-axis direction can be determined according to the right-hand rule.

Will A ₃ Placed in a direction approximating the y-axis, A ₄ Can be placed at any position.

The calibration stage makes the unmanned aerial vehicle keep static, the label periodically sends a distance measurement request to the anchor points according to the set time slot table, and the distance from the label to each anchor point is analyzed according to the TW-TOF principle.

And starting an echo ranging function of the UWB module, namely enabling the tag to receive the distance information between the anchor points. The mutual ranging is also started between the anchor points, and the distance measurement value is stored in the buffer memory of the anchor points. After the tag sends a ranging request to the anchor point, the echo ranging value is added into the response information when the anchor point replies to the tag, so that the tag receives the distance measurement between the two anchor points.

The to-be-estimated value of the anchor point calibration stage is the position p of the tag under the global system G _T And anchor point position

Constructing a nonlinear least squares problem:

in the formula (7), C is a set of anchor points, and the positions of the tag and each anchor point can be calibrated by solving a cost function. Offset of tag from origin of body systemKnown a priori information, the position p of the drone under the global system at this time can be deduced, which will be the initial iteration value p of the state estimator ₀ 。

Further, step 4: performing outlier detection on the calibrated UWB data, including:

And detecting an outlier by adopting an addition threshold value method, and once the distance measurement is considered to be the outlier, directly eliminating the outlier.

If p _k The position of the unmanned aerial vehicle output by the k moment estimator is the position of the unmanned aerial vehicle when the label on the unmanned aerial vehicle at the k+1 moment receives the position of the anchor point A _i Distance measurement of (2)If it meets

The current distance measurement is determined to be an outlier. Where γ is an adjustable threshold, v _max For the maximum flight speed of the unmanned aerial vehicle, f is the ranging update rate of the UWB module. If the distance measurement is determined to be an outlier, the distance measurement is directly abandoned, and if the distance measurement is normal, the distance measurement is directly abandonedAs distance measurement is sent to the estimator for status update.

Further, step 5: low pass filtering the IMU data, comprising:

and the IMU data is subjected to low-pass filtering through an IIR digital low-pass filter with the cut-off frequency of about 30Hz, and noise pollution can be effectively reduced through the IMU data subjected to low-pass filtering.

The IIR low-pass filtering algorithm frequency domain representation is shown in equation (4):

wherein m and n, a _i I=1, …, m and b _j J=1, …, n are filter parameters. The time domain form of the low pass filter is:

y(t)＝a ₀ u(t)+a ₁ u(t-1)+…+a _m u(t-m)-b ₁ y(t-1)-…-b _n y(t-n) (5)

wherein u represents IMU original measurement data, y represents a low-pass filtering processing result, and the data after low-pass filtering is used as IMU measurement value to perform multi-sensor fusion.

Further, step 6: inputting the filtered UWB data and the filtered IMU data into a state estimator, comprising:

the state estimator takes Unscented Kalman Filtering (UKF) as a core, and is designed based on the unscented Kalman filtering, and a system equation and an observation equation have the following forms:

wherein u is _k Is an input to the system, w _k Zero mean system noise. y is _k For observation of state, η _k Observation noise is zero mean. w (w) _k And eta _k Uncorrelated, variance is respectivelyAnd->Let x be _k ，w _k ，η _k The dimensions of (2) are n, p, q, respectively.

The estimator mainly estimates position, velocity and attitude, so the state vector of the selection system is:

wherein p is the position of the unmanned aerial vehicle under the global system, v is the speed, q is the unit quaternion, and b is the attitude of the unmanned aerial vehicle _a And b _ω Zero offset for IMU.

After adding IMU data, the system equations can be written as follows:

here the IMU reading is taken as an input to the system equationThe system noise vector is

Starting state estimator when k=0, first iterating initial value for system stateInitializing, collecting 400 groups of IMU accelerometer and gyroscope measurement data in a static state, taking an average value, and zero-offset initial value of acceleration in the state +. >Zero offset of angular velocity +.>And (3) estimating:

initial value p of position ₀ According to the deduction of the anchor point calibration step result, other state variables are initialized to zero, and a covariance matrix is initialized according to an empirical value:

when IMU data is acquired at time k after the unmanned aerial vehicle takes off, predicting the system state at time k+1 by using the data. Suppose at this time the system state x _k Mean and covariance of (2) areAnd P _k System noise w _k Mean and covariance of +.>And->The augmented state and corresponding covariance of the l=n+p dimension is:

obtaining an aggregate matrix of Sigma points by applying UT transformation to the system state:

where λ is a scalar parameter, λ=α ² (L+k) -L, constant α characterizes the degree of dispersion of Sigma points around the state, 1e ^-4 Alpha is more than or equal to 1, and k is an auxiliary scaling parameter. (. Cndot. _i The i-th column is shown in the matrix.

The Sigma points were then propagated according to the nonlinear system equation to obtain new Sigma points:

wherein the method comprises the steps ofFor status part in Sigma Point, +.>Is the systematic noise fraction in Sigma points. Sigma Point set χ propagated through nonlinear System equations _ik+1|k Can be used to predict the state mean after propagation:

where W is _i ^m ，W _i ^c Is scalar weight

The above is the prediction step of the estimator, when k+1 is time, the observed quantity y of UWB or fixed-height radar is present _k+1 When arrived, an update step is performed. Firstly, the state average value obtained in the prediction step is calculatedAnd observation noise eta _k Composition l=n+q-dimensional augmentation state:

then generate a new Sigma point set χ according to UT transformation _ik+1|k ，χ _ik+1|k By a nonlinear observation equation h (x _k ,η _k ) Propagation generates a set of points y _ik+1|k ，y _ik+1|k Observations used to generate estimates

Thus, the Kalman gain K can be calculated to update the state to obtain the average value of the posterior stateAnd covariance P _k+1 ：

Further, the method also comprises the following steps:

acquiring a fixed Gao Leida measurement signal, correcting a z-axis component of a position p in a system state by fusing the fixed-height radar measurement signal, and comprising:

when reading radar measurement dataWhen the unmanned plane is used, the gesture of the unmanned plane is utilized to enable +.>Projection equation and observer of fixed-height radar projected onto z-axis of global system for state updateCheng Ru formulas (24) and (25):

equation (24) is the projection equation, R _k In the form of a rotation matrix of the gesture quaternion, the rotation relation from the machine system to the global system at the moment k is expressed, (. Cndot.) _z Representing the z-axis component in the vector. Equation (25) is an observation equation,for global system down position p _k+1 Observation of the z-axis component of +.>Is the measurement noise of the stator Gao Leida.

When a certain Gao Leida measurement is obtainedAt the time, the measured value is projected onto the z-axis of the global system according to the current posture by using the formula (24) to obtain a height measurement result +. >

When (when)When the z-axis height is not updated.

When (when)In this case, the altitude component and altitude measurement in the current position estimate are taken>Calculating relative deviation of difference _err ：

Wherein: (p) _k ) _z Is the calculated height component estimate,is the altitude value measured by the radar altimeter.

When |l _err When I > 0.3, it is considered that there is a topographical mutation, and no use is madeDe-updating state quantity p _k But directly estimates the terrain:

when (when)But |l _err When the I is less than or equal to 0.3, the topography has no mutation, and the +.>The altitude is updated and then the terrain is estimated according to equation (26).

In a second aspect, the present invention provides a robust autonomous positioning and mapping device based on UWB and IMU fusion, the device comprising:

an input module: the method comprises the steps of acquiring UWB original data and IMU data;

and a calibration module: the method comprises the steps of performing distance calibration on UWB original data to obtain calibrated UWB data;

and a judging module: the jump screening module is used for judging whether the anchor point position is known or not, if not, carrying out anchor point calibration according to the calibrated UWB data, and if yes, jumping the screening module;

and a screening module: the method comprises the steps of performing outlier detection on the calibrated UWB data to obtain screened UWB data;

and a filtering module: the IMU data processing module is used for carrying out low-pass filtering on the IMU data to obtain filtered IMU data;

An estimation module: and the position, speed and attitude information of the unmanned aerial vehicle are obtained by inputting the screened UWB data and the filtered IMU data into a state estimator. The state estimator approximates probability distribution of states by taking unscented Kalman filtering as a core, so that mean and covariance are estimated.

In a third aspect, the invention provides a robust autonomous positioning and mapping device based on UWB and IMU fusion, comprising a processor and a storage medium;

the storage medium is used for storing instructions;

the processor is configured to operate in accordance with the instructions to perform the steps of the method according to the first aspect.

In a fourth aspect, the present invention provides a computer readable storage medium having stored thereon a computer program which when executed by a processor performs the steps of the method of the first aspect.

Compared with the prior art, the invention has the beneficial effects that:

the method has very important significance for researching the positioning and navigation algorithm of the four-rotor unmanned aerial vehicle in the GNSS refusing environment. The method is stable and reliable, is not influenced by weather, light and the like, saves calculation resources by an algorithm, has lower requirement on hardware, and has high theoretical and practical values. The invention has the following characteristics and advantages:

(1) The P440 module produced by the Time domain company in the United states is selected to realize the autonomous positioning and mapping technology of the unmanned aerial vehicle in the GNSS rejection environment, and the scheme has high positioning precision, low cost and wide application range. The sensors used in the autonomous navigation scheme under the conventional GNSS rejection environment are expensive and stability cannot be ensured. By means of the anchor point calibration unit of the method, autonomous positioning and mapping of various scenes such as indoor and outdoor scenes can be achieved.

(2) The invention provides a positioning algorithm based on UWB and IMU fusion, which is assisted by redundant constraint of a positioning Gao Leida, and a robust autonomous positioning and mapping technology of an unmanned aerial vehicle is realized by using unscented Kalman filtering. By means of the characteristic that the IMU updates rapidly, the method and the device are fused with UWB to alleviate the phenomenon that the positioning track appears saw-tooth based on UWB positioning, and achieve the effect of track smoothing. The method improves the positioning precision, further constrains the height of the unmanned aerial vehicle, and effectively solves the problem of insufficient estimation precision of the height direction.

(3) The pose solving framework adopted by the invention is unscented Kalman filtering, is easy to expand, can be added with sensor equipment such as a laser radar, a binocular camera and the like according to developers besides sensor equipment such as an IMU, a UWB, a radar altimeter and the like, and can be developed secondarily.

Drawings

FIG. 1 is a flow chart of a robust autonomous positioning and mapping method, device and medium based on UWB and IMU fusion;

FIG. 2 is a diagram of hardware architecture of a robust autonomous positioning and mapping method, device and medium system based on UWB and IMU fusion;

FIG. 3 is a flowchart of a robust autonomous positioning and mapping method, device and medium system algorithm based on UWB and IMU fusion;

FIG. 4 is a schematic illustration of anchor point calibration;

FIG. 5 is a unscented Kalman filter flow chart;

FIG. 6 shows the determination Gao Lei when pitch and roll are not zero;

FIG. 7 illustrates terrain estimation for several situations;

FIG. 8 shows the effect of UWB and IMU fusion indoor track tracking;

FIG. 9 is a graph of the Absolute Position Error (APE) of the UWB and IMU fusion locations;

FIG. 10 is a schematic diagram of a UWB fusion IMU positioning and UWB positioning error box.

Detailed Description

The invention is further described below with reference to the accompanying drawings. The following examples are only for more clearly illustrating the technical aspects of the present invention, and are not intended to limit the scope of the present invention.

Embodiment one:

the invention aims to overcome the defects in the prior art, and aims to provide a robust autonomous positioning and mapping method, device and medium based on UWB and IMU fusion in a GNSS refusing environment. The pose estimation platform of the four-rotor unmanned aerial vehicle in the GNSS refusing environment is shown in fig. 1. Karl Hausman et al have proposed in 2016 a UWB and IMU fusion positioning framework with extended kalman filtering (Extended Kalman Filter, EKF) as the core. The EKF rejects higher order terms above the second order by linearizing the system, resulting in insufficient state estimation accuracy for strongly nonlinear systems. The four-rotor unmanned aerial vehicle has the characteristic of strong nonlinearity, in order to solve the estimation problem under the strong nonlinearity condition, the sensor is expanded more conveniently simultaneously, and the method designs a set of positioning frame taking unscented Kalman filtering (Unscented Kalman Filter, UKF) as a core, so that the UWB and the inertial sensor can be mutually complemented in function. In the positioning process, the IMU can be matched with UWB to improve positioning accuracy, and good positioning performance can be still shown when the UWB measurement temporarily shows outliers. The positioning algorithm framework has strong universality, other sensors can be expanded, and the positioning algorithm framework is fused with a positioning Gao Leida measurement, so that the estimation accuracy of the z-axis direction is improved, and a multi-sensor redundancy strategy is designed for the stable flight of the unmanned aerial vehicle. By designing the four-rotor unmanned aerial vehicle platform and running the positioning algorithm, the unmanned aerial vehicle can be positioned quickly and accurately, and the method has great application value. The technical scheme adopted by the invention comprises two parts of hardware and software, wherein the hardware comprises: the system comprises a UWB ranging module, an IMU inertial measurement sensor, a radar altimeter, an embedded airborne processor and an unmanned aerial vehicle platform. The software comprises: the device comprises an IMU low-pass filtering unit, a distance calibration unit, an anchor point calibration unit, a Kalman filtering unit and an outlier detection unit. The whole algorithm flow is shown in figure 3. The following will specifically describe each part.

Hardware part:

the unmanned aerial vehicle is a four-rotor aircraft with a wheel base of 380mm, and is provided with a DJI E300 motor electric power and a 4S high-voltage battery. The sensor is provided with a 3DM-GX5-25 Inertial Measurement Unit (IMU), a P440 UWB ranging module and a TF mini laser ranging module (fixed Gao Leida), the attitude control is completed by a Pixhawk flight controller, and an airborne computer adopts Intel eighth-generation NUC (a processor is Core i7-8559U 2.7GHz, four cores and eight threads, and the memory size is 32G). The positioning program is developed based on a ROS (Robot Operating System) robot open source framework under a Linux operating system, all algorithms are realized by adopting a C++ language, the positioning program receives data acquired by a sensor in real time by adopting an ROS Message, and the position and the gesture analyzed by the program are sent to a Pixhawk flight controller to further gesture control by using a Mavlink protocol through a serial port.

Considering the reliability of the system, the UWB module needs to have higher ranging precision and more robust multipath interference resistance, and the platform selects and carries the P440 module produced by the American Time domain company. The P440 module is an ultra-wideband wireless transceiver with a wave band between 3.1GHz and 4.8GHz, mainly adopts TW-ToF principle, can simultaneously execute four functions (ranging, data transmission, monostatic radar and multistatic radar), only uses ranging functions, the ranging comprises single-point ranging and networking ranging modes, a ranging network is formed by a tag and an anchor point in an experiment, and networking ranging can adopt ALOHA (random) protocol or TDMA (time division multiple access) protocol. The wave emission power of the UWB module is extremely low (50 uW), the tag uses an on-board battery to supply power after voltage conversion, and the anchor point uses a mobile power supply to directly supply power.

Software part:

(1) Data preprocessing unit

The method comprises three parts of ranging information calibration, outlier removal and IMU noise processing of UWB original data. The processed IMU data can effectively reduce noise pollution. The processed result is fed as input to a kalman filter unit.

The method adds a distance calibration link before the measured value is sent to the Kalman filter, and the measured value can be considered to be a measured value closer to a true value after distance calibration. And (3) when the UWB sensor measurement data are acquired, carrying out linear regression processing through a distance calibration link by default, and calculating a more reasonable distance value. Here, an outlier is detected by adopting an adding threshold method, and once the distance measurement is considered as the outlier, the outlier is directly rejected. For the IMU, according to the fact that the main source of IMU measurement noise is high-frequency vibration of a machine body, the method designs an IIR digital low-pass filter with the cut-off frequency of about 30Hz, and noise pollution can be effectively reduced through IMU data processed through low-pass filtering.

(2) UWB anchor point real-time calibration unit

Aiming at the requirement of universality of multiple scenes such as indoor and outdoor, the method designs a real-time anchor point calibration unit. Since the position of the ground anchor point relative to the drone prior to deployment is unknown, the relative position can be obtained by manual methods to measure but is time consuming and labor intensive. According to the method, an anchor point calibration algorithm is designed by adopting a nonlinear least square theory, the positions of anchor points around the environment are calibrated while a global system is created, and the anchor point calibration result is sent to a state estimator for storage, so that the subsequent algorithm can be smoothly carried out.

(3) Unscented Kalman filter unit

The state estimator designed by the method takes Unscented Kalman Filtering (UKF) as a core, the UKF is different from EKF in that the UKF approximates the system linearly, but approximates the probability distribution of states, the core idea is that the states are sampled before state propagation, a point set (called Sigma points) after sampling is subjected to state propagation according to a nonlinear system equation and an observation equation, and then the distributed function of the Sigma points is estimated according to Gaussian distribution, so that the mean value and the covariance are estimated. Errors due to the EKF linearization process are avoided while the distribution of the propagated states is made closer to the true distribution by increasing the sampling points, although still under the assumption of gaussian distribution. The UKF has almost the same time complexity as the EKF, but the estimation accuracy of the UKF is higher, and the method is simpler in implementation than the EKF.

(4) Multi-sensor redundancy unit

The measurement principle of the fixed-height radar is similar to UWB, a laser emitter of the radar emits a beam of laser to the ground, and a sensor calculates the linear distance to the ground according to the emission time and the receiving time. When the ground is always level, the measurement of the fixed Gao Leida can be directly sent to the estimator for state updating after passing through a projection equation, and when the terrain changes (such as the roughness of a certain place on the ground), the measurement result of the fixed-height radar also contains the terrain information. To this end, the method proposes a multi-sensor redundancy strategy for terrain estimation, whereby the drone can estimate the terrain and appropriately change the fly height when the terrain changes, in order to avoid accidents. The principle of the terrain estimator is as follows:

When the fix Gao Leida measurement is obtained, the measurement is projected onto the z-axis of the global system according to the current pose.

The four-rotor unmanned aerial vehicle positioning algorithm in the GNSS rejection environment is further described below with reference to the accompanying drawings.

Hardware part:

Software part:

(1) Data preprocessing unit

The accuracy of UWB distance measurements can ideally be on the order of centimeters, but in practice the measurements often have static differences and errors for some reason. In order to reduce factors affecting positioning accuracy as much as possible, the method adds a distance calibration link before the measured value is sent to the Kalman filter, and the measured value can be considered to be closer to a true value after distance calibration.

wherein n is _d Is zero mean white noise. The distance calibration link adopts a linear regression method, and a plurality of true value-measurement samples are obtained by placing two UWB modules at different positions for distance measurementThe measured value in each sample is a reading of the UWB range, and the range truth value is obtained by a high-precision indoor positioning system. And (3) determining the scale factor a and the static difference b under the current model through fitting the model by a large number of samples. />

And (3) when the UWB sensor measurement data are acquired, carrying out linear regression processing through a distance calibration link by default, and calculating a more reasonable distance value.

Although the excellent characteristics of UWB can avoid the influence of multipath effect and non-line of sight to the maximum extent, it is still possible to introduce outliers in the scene of dense obstacles, where the outliers indicate measurements that are contrary to the fact, such as negative readings, abrupt occurrence of large jumps, etc., and the introduction of outliers can lead to erroneous state estimation. Here, an outlier is detected by adopting an adding threshold method, and once the distance measurement is considered as the outlier, the outlier is directly rejected.

If p _k The position of the unmanned aerial vehicle output by the k moment estimator is not found at the k+1 momentLabel receiving and anchor point A on man-machine _i Distance measurement of (2)If it meets

For IMU, because it adopts the strapdown mode to fix directly on the organism, unmanned aerial vehicle receives the influence of screw vibrations in the flight in-process, leads to measured data to receive great noise pollution. According to the invention, an IIR digital low-pass filter with the cut-off frequency of about 30Hz is designed according to the fact that the main source of IMU measurement noise is high-frequency vibration of a machine body, and noise pollution can be effectively reduced through IMU data processed through low-pass filtering. The IIR low-pass filtering algorithm frequency domain representation is shown in equation (4):

y(t)＝a ₀ u(t)+a ₁ u(t-1)+…+a _m u(t-m)-b ₁ y(t-1)-…-b _n y(t-n) (5)

(2) UWB anchor point online calibration unit

The section adopts a nonlinear least square theory to design an anchor point calibration algorithm, the positions of anchor points around the environment are calibrated while a global system is created, and the anchor point calibration result is sent to a state estimator for storage, so that the subsequent algorithm can be smoothly carried out. The anchor point calibration link is performed only once in the whole process.

The anchor point calibration schematic diagram is shown in figure 3. Firstly, a global coordinate system is established, and the coordinate system is an anchor point A in the overlooking view ₁ The position is the origin of the coordinate system, the anchor point A ₂ And A is a ₁ The connecting line of (a) is the direction of the x-axis, A ₁ The height direction of (2) is the z-axis, thus establishing two coordinate axes of the right-hand system, and the y-axis direction can be determined according to the right-hand rule. For simplicity, A may be ₃ Placed in a direction approximating the y-axis, A ₄ Can be placed at any position. The calibration stage makes the unmanned aerial vehicle keep static, the label periodically sends a distance measurement request to the anchor points according to the set time slot table, and the distance from the label to each anchor point is analyzed according to the TW-TOF principle. In addition, the echo ranging function of the UWB module is started, namely the tag can receive the distance information between the anchor points. The principle can be explained as follows: the mutual ranging is also started between the anchor points, and the distance measurement value is stored in the buffer memory of the anchor points. After the tag sends a ranging request to the anchor point, the echo ranging value is added into the response information when the anchor point replies to the tag, so that the tag receives the distance measurement between the two anchor points.

From fig. 4, a nonlinear least squares problem can be constructed:

(3) Unscented Kalman filter unit

Kalman (r.e. Kalman) first proposed Kalman Filter (KF) in 1960, KF is a model-based linear minimum variance estimation, and its standard form has advantages such as recursive computation. KF is only applicable in cases where both the system equation and the observation equation are linear, i.e. the system is linear gaussian. The KF is used in the linear Gaussian system to propagate the state and the noise respectively and then form the linear Gaussian system after the state is propagated, so that recursive estimation is completed, and the conclusion cannot be established in the nonlinear system. Extended Kalman Filtering (EKF) solves the estimation problem when the system and observation are nonlinear by making a linear approximation to the equation, but the linearization process discards higher order terms above second order, resulting in insufficient state estimation accuracy for strongly nonlinear systems.

The state estimator designed in this section uses Unscented Kalman Filter (UKF) as core, and the state estimator is designed based on unscented Kalman filter, and its basic flow is shown in figure 5. The UKF approximates the system linearly unlike EKF, but approximates the probability distribution of the state, the core idea is to sample the state before the state is propagated, the sampled point set (called Sigma point) propagates the state according to the nonlinear system equation and the observation equation, and then the propagated Sigma point estimates the distribution function according to Gaussian distribution, so as to estimate the mean and covariance. Errors due to the EKF linearization process are avoided while the distribution of the propagated states is made closer to the true distribution by increasing the sampling points, although still under the assumption of gaussian distribution. The UKF has almost the same time complexity as the EKF, but the estimation accuracy of the UKF is higher, and the method is simpler in implementation than the EKF.

Unscented Kalman filtering is based on a model state estimation method, and therefore requires determining system equations and observation equations before sensor fusion and state estimation, selecting the estimator's system state vector. Generally the system equations and observation equations have the form:

Wherein u is _k Is an input to the system, w _k Zero mean system noise. y is _k For observation of state, η _k Observation noise is zero mean. w (w) _k And eta _k Uncorrelated, variance is respectivelyAnd->Let x be _k ，w _k ，η _k The dimensions of (2) are n, p, q, respectively. />

The estimator is mainly used for estimating the position, the speed, the gesture and the like of the unmanned aerial vehicle, so that the state vector of the system is selected as follows:

After the IMU sensor is added, the system equation can be written as follows:

Starting state estimator when k=0, first iterating initial value for system stateInitializing, collecting 400 groups of IMU accelerometer and gyroscope measurement data in a static state, taking an average value, and zero-offset initial value of acceleration in the state +.>Zero offset of angular velocity +.>And (3) estimating:

Instead of linearly approximating the system, the Sigma points are then propagated according to a nonlinear system equation to obtain new Sigma points:

where W is _i ^m ，W _i ^c Is scalar weight

/>

The multi-sensor fusion mainly refers to fusion of a UWB ranging module and an IMU inertial measurement sensor, and the height of the radar altimeter in the direction of the auxiliary measurement z axis is added, so that the state estimation of the unmanned aerial vehicle is more accurate, and the fusion process is introduced in step six.

The UWB positioning method can complete position estimation in the GPS rejection environment only by deploying a low-cost sensor, is not influenced by illumination and weather, and more importantly has a global reference system, thereby providing convenience for direct application of multiple unmanned aerial vehicles. However, the positioning effect of the UWB sensor is not ideal when the UWB sensor acts alone, firstly, UWB signals are easy to be interfered, the positioning precision is reduced to some extent under the non-line-of-sight propagation scene with dense obstacles, and the stability is general; secondly, the accuracy of the UWB positioning method is not high due to the adoption of a constant speed assumption; thirdly, the update rate of the position estimation is limited by the UWB measurement frequency, about 20Hz, and the stability of the unmanned aerial vehicle is kept, and the update rate of the position is at least more than 100Hz, which obviously does not meet the requirement of the controller on the state estimation. The IMU can measure the acceleration and the angular velocity of the three axes of the machine body at high frequency, and can just make up the defect of UWB when acting independently. Therefore, UWB and IMU measurement can be fused, and the positioning performance of the unmanned aerial vehicle in the GPS refusing environment is improved.

(4) Multi-sensor redundancy unit

The method is used for improving the z-axis estimation precision through the integration of the stator Gao Leida, the sensor is arranged on the back of the frame, and when the pitch angle and the roll angle of the machine body are not zero, the stator Gao Lei is as shown in figure 6.

To correct the z-axis component of position p in the system state using the fix Gao Leida, the radar measurement data needs to be processed in one step before the fix Gao Leida measurement value is sent to the estimator for updating, when the radar measurement data is readWhen the unmanned plane is used, the gesture of the unmanned plane is utilized to enable +.>And the projection equation and the observation equation of the fixed-height radar are shown as formulas (24) and (25):

The measurement principle of the fixed-height radar is similar to UWB, a laser emitter of the radar emits a beam of laser to the ground, and a sensor calculates the linear distance to the ground according to the emission time and the receiving time. When the ground is always level, the measurement of the fixed Gao Leida can be directly sent to the estimator for state updating after passing through a projection equation, and when the terrain changes (such as the roughness of a certain place on the ground), the measurement result of the fixed-height radar also contains the terrain information. To this end, this section proposes a multi-sensor redundancy strategy for terrain estimation, where the drone can estimate the terrain and change the fly height appropriately when the terrain changes, in order to avoid accidents. The principle of the terrain estimator is as follows:

When a certain Gao Leida measurement is obtainedAt the time, the measured value is projected onto the z-axis of the global system according to the current posture by using the formula (24) to obtain a height measurement result +.>The three cases are discussed separately below with reference to fig. 7. />

When (when)In this case, the z-axis height is not updated, and the optimal use distance of the fixed Gao Leida is 0.2 to 20m according to the instruction of the fixed Gao Lei. And the sensor is mounted on the back of the frame, the sensor reading being approximately 0.2 when the aircraft is resting on the ground, as shown in fig. 7 (a). Outliers can be considered generated when the reading is less than 0.2, and therefore culled directly.

When (when)In this case, the altitude component and altitude measurement in the current position estimate are taken>Calculating relative deviation of difference _err When |l _err When I > 0.3, it is considered that there is a topographic mutation, use +.>De-updating state quantity p _k But directly estimates the terrain as shown in fig. 7 (b):

when (when)But |l _err When the level is less than or equal to 0.3, the terrain has no mutation but can be changed slowly, as shown in the figure 7 (c), the +.>The altitude is updated and then the terrain is estimated according to equation (26).

The experimental results are given below

The case carries out a plurality of groups of robust autonomous positioning experiments based on UWB and IMU fusion in a GNSS refusing environment based on multi-sensor fusion on the positioning system.

Fig. 8 shows an indoor track tracking effect in a GNSS rejection environment, where the experimental scenario is indoor and the start point and the end point are the same. From the results, it can be seen that the trajectory based on the method is smoother and more approximate to the true value than the trajectory obtained by positioning using only UWB.

Fig. 9 and 10 are respectively an Absolute Position Error (APE) curve and a UWB fused IMU positioning and UWB positioning error box diagram in a GNSS rejection environment, the curve and the box diagram quantitatively analyze the autonomous positioning error of the unmanned aerial vehicle, and according to the analysis, the maximum value and the average value of the absolute error are improved compared with the positioning based on UWB.

Embodiment two:

the embodiment provides a robust autonomous positioning and mapping device based on UWB and IMU fusion, which comprises:

The apparatus of this embodiment may be used to implement the method described in embodiment one.

Embodiment III:

the embodiment provides a robust autonomous positioning and mapping device based on UWB and IMU fusion, which comprises a processor and a storage medium;

the storage medium is used for storing instructions;

the processor is configured to operate in accordance with the instructions to perform the steps of the method according to embodiment one.

Embodiment four:

the present embodiment provides a computer readable storage medium having stored thereon a computer program which when executed by a processor performs the steps of the method of embodiment one.

It will be appreciated by those skilled in the art that embodiments of the present application may be provided as a method, system, or computer program product. Accordingly, the present application may take the form of an entirely hardware embodiment, an entirely software embodiment, or an embodiment combining software and hardware aspects. Furthermore, the present application may take the form of a computer program product embodied on one or more computer-usable storage media (including, but not limited to, disk storage, CD-ROM, optical storage, and the like) having computer-usable program code embodied therein.

The present application is described with reference to flowchart illustrations and/or block diagrams of methods, apparatus (systems) and computer program products according to embodiments of the application. It will be understood that each flow and/or block of the flowchart illustrations and/or block diagrams, and combinations of flows and/or blocks in the flowchart illustrations and/or block diagrams, can be implemented by computer program instructions. These computer program instructions may be provided to a processor of a general purpose computer, special purpose computer, embedded processor, or other programmable data processing apparatus to produce a machine, such that the instructions, which execute via the processor of the computer or other programmable data processing apparatus, create means for implementing the functions specified in the flowchart flow or flows and/or block diagram block or blocks.

These computer program instructions may also be stored in a computer-readable memory that can direct a computer or other programmable data processing apparatus to function in a particular manner, such that the instructions stored in the computer-readable memory produce an article of manufacture including instruction means which implement the function specified in the flowchart flow or flows and/or block diagram block or blocks.

These computer program instructions may also be loaded onto a computer or other programmable data processing apparatus to cause a series of operational steps to be performed on the computer or other programmable apparatus to produce a computer implemented process such that the instructions which execute on the computer or other programmable apparatus provide steps for implementing the functions specified in the flowchart flow or flows and/or block diagram block or blocks.

The foregoing is merely a preferred embodiment of the present invention, and it should be noted that modifications and variations could be made by those skilled in the art without departing from the technical principles of the present invention, and such modifications and variations should also be regarded as being within the scope of the invention.

Claims

1. The robust autonomous positioning and mapping method based on UWB and IMU fusion is characterized by comprising the following steps:

step A: acquiring UWB original data and IMU data;

and (B) step (B): performing distance calibration on the UWB original data to obtain calibrated UWB data;

step C: judging whether the anchor point position is known or not, if not, carrying out anchor point calibration according to the calibrated UWB data, and then jumping to the step D, if yes, jumping to the step D;

Step D: performing outlier detection on the calibrated UWB data to obtain screened UWB data;

step E: performing low-pass filtering on the IMU data to obtain filtered IMU data;

step F: inputting the screened UWB data and the filtered IMU data into a state estimator to obtain the position, speed and attitude information of the unmanned aerial vehicle; the state estimator approximates probability distribution of states by taking unscented Kalman filtering as a core, so that mean and covariance are estimated;

2. The robust autonomous positioning and mapping method based on UWB and IMU fusion of claim 1, wherein step B: performing distance calibration on the UWB original data, including:

wherein n is _d Zero mean white noise;

a linear regression method is adopted, and a plurality of true value-measurement samples are obtained by placing two UWB modules at different positions for distance measurement The measured value in each sample is the reading of UWB distance, and the distance true value is obtained by a high-precision indoor positioning system; a large number of samples are used for fitting the model, and a scale factor a and a static difference b under the current model are determined;

3. The robust autonomous positioning and mapping method based on UWB and IMU fusion of claim 1, wherein in step C, anchor point calibration is performed according to the calibrated UWB data, comprising:

firstly, a global coordinate system is established, and the coordinate system is an anchor point A in the overlooking view ₁ The position is the origin of the coordinate system, the anchor point A ₂ And A is a ₁ The connecting line of (a) is the direction of the x-axis, A ₁ The height direction of the device is the z axis, and the y axis direction can be determined according to the right hand rule;

will A ₃ Placed in a direction approximating the y-axis, A ₄ Can be placed at any position;

the calibration stage makes the unmanned aerial vehicle keep still, the label periodically sends out a distance measurement request to the anchor points according to a set time slot table, and the distance from the label to each anchor point is analyzed according to the TW-TOF principle;

starting an echo ranging function of the UWB module, namely enabling the tag to receive the distance information between the anchor points; the mutual distance measurement is started between the anchor points, and the distance measurement value is stored in the cache of the anchor points; after the tag sends a ranging request to the anchor point, the anchor point adds an echo ranging value into response information when replying to the tag, so that the tag receives the distance measurement between the two anchor points;

Constructing a nonlinear least squares problem:

c in the formula (7) is a set of anchor points, and the positions of the labels and each anchor point can be calibrated by solving a cost function; offset of tag from origin of body systemKnown a priori information, the position p of the drone under the global system at this time can be deduced, which will be the initial iteration value p of the state estimator ₀ 。

4. The robust autonomous positioning and mapping method based on UWB and IMU fusion of claim 1, wherein step D: performing outlier detection on the calibrated UWB data, including:

detecting an outlier by adopting an adding threshold value method, and once the distance measurement is considered to be the outlier, directly eliminating the outlier;

The current distance measurement is determined to be an outlier; where γ is an adjustable threshold, v _max F is the ranging update rate of the UWB module for the maximum flight speed of the unmanned aerial vehicle; if the distance measurement is determined to be an outlier, the distance measurement is directly abandoned, and if the distance measurement is normal, the distance measurement is directly abandoned As distance measurement is sent to the estimator for status update.

5. The robust autonomous positioning and mapping method based on UWB and IMU fusion of claim 1, wherein step E: low pass filtering the IMU data, comprising:

the IMU data is subjected to low-pass filtering through an IIR digital low-pass filter with the cut-off frequency of about 30Hz, and noise pollution can be effectively reduced through IMU data subjected to low-pass filtering treatment;

wherein m and n, a _i I=1, …, m and b _j J=1, …, n are filter parameters; the time domain form of the low pass filter is:

y(t)＝a ₀ u(t)+a ₁ u(t-1)+…+a _m u(t-m)-b ₁ y(t-1)-…-b _n y(t-n) (5)

6. The robust autonomous positioning and mapping method based on UWB and IMU fusion of claim 1, wherein step F: inputting the filtered UWB data and the filtered IMU data into a state estimator, comprising:

Wherein u is _k Is an input to the system, w _k Zero mean system noise; y is _k For observation of state, η _k Observation noise that is zero mean; w (w) _k And eta _k Uncorrelated, variance is respectivelyAnd->Let x be _k ，w _k ，η _k The dimension of (2) is n, p and q respectively;

wherein p is the position of the unmanned aerial vehicle under the global system, v is the speed, q is the unit quaternion, and b is the attitude of the unmanned aerial vehicle _a And b _ω Zero offset for IMU;

after adding IMU data, the system equations can be written as follows:

when IMU data is acquired at time k after the unmanned aerial vehicle takes off, predicting the system state at time k+1 by using the data; suppose at this time the system state x _k Mean and covariance of (2) areAnd P _k System noise w _k Mean and covariance of +.>And->The augmented state and corresponding covariance of the l=n+p dimension is:

where λ is a scalar parameter, λ=α ² (L+k) -L, constant α characterizes the degree of dispersion of Sigma points around the state, 1e ^-4 Alpha is more than or equal to 1, k is an auxiliary scaling parameter; (. Cndot. _i The i-th column is taken from the matrix;

wherein the method comprises the steps ofFor status part in Sigma Point, +.>Is the systematic noise fraction in Sigma points; sigma Point set χ propagated through nonlinear System equations _ik+1|k Can be used to predict the state mean after propagation:

where W is _i ^m ，W _i ^c Is scalar weight

The above is the prediction step of the estimator, when k+1 is time, the observed quantity y of UWB or fixed-height radar is present _k+1 When the user arrives, an updating step is executed; firstly, the state average value obtained in the prediction step is calculatedAnd observation noiseη _k Composition l=n+q-dimensional augmentation state:

Thus, the Kalman gain K can be calculated to update the state to obtain the average value of the posterior state And covariance P _k+1 ：

7. The robust autonomous positioning and mapping method based on UWB and IMU fusion of claim 6, further comprising:

when reading radar measurement dataWhen the unmanned plane is used, the gesture of the unmanned plane is utilized to enable +.>And the projection equation and the observation equation of the fixed-height radar are shown as formulas (24) and (25):

equation (24) is the projection equation, R _k In the form of a rotation matrix of the gesture quaternion, the rotation relation from the machine system to the global system at the moment k is expressed, (. Cndot.) _z Representing the z-axis component in the vector; equation (25) is an observation equation,for global system down position p _k+1 Observation of the z-axis component of +.>Is the measurement noise of the stator Gao Leida;

when a certain Gao Leida measurement is obtainedAt the time, the measured value is projected onto the z-axis of the global system according to the current posture by using the formula (24) to obtain a height measurement result +.>

When (when)When the z-axis height is not updated;

Wherein: (p) _k ) _z Is the calculated height component estimate, Is the altitude value measured by the radar altimeter;

8. A robust autonomous positioning and mapping device based on UWB and IMU fusion, the device comprising:

an estimation module: the position, speed and attitude information of the unmanned aerial vehicle are obtained by inputting the screened UWB data and the filtered IMU data into a state estimator; the state estimator approximates probability distribution of states by taking unscented Kalman filtering as a core, so that mean and covariance are estimated.

9. The robust autonomous positioning and mapping device based on the fusion of UWB and IMU is characterized by comprising a processor and a storage medium;

the storage medium is used for storing instructions;

the processor being operative according to the instructions to perform the steps of the method according to any one of claims 1 to 7.

10. Computer readable storage medium, on which a computer program is stored, characterized in that the program, when being executed by a processor, implements the steps of the method according to any one of claims 1 to 7.