-
The communication mode of UWB ranging in this paper is shown in Fig. 1.
The UWB label first broadcasts a UWB signal POLL and records the sending time
${T_{a1}}$ . After time period${t_f}$ , the signal arrives at the UWB base station. The base station records the time upon receiving the POLL signal${T_{b1}}$ and continues with signal processing. After time period${T_{reply1}}$ , the base station sends a response signal ANSWER to the label at time${T_{b2}}\left( {{T_{b2}} = {T_{b1}} + {T_{reply1}}} \right)$ . After the label has successfully receives the base station's ANSWER, it then records the reception time${T_{a2}}$ and sends a FINAL signal after time period${T_{reply2}}$ . After the base station receives the FINAL signal, estimate the transmission time according to the timestamp of each reception time point.$$\left\{ \begin{array}{l} {T_{round}} = {T_{a2}} - {T_{a1}} \\ {T_{reply}} = {T_{b2}} - {T_{b1}} \\ {{\overset{\frown} t}_f} = \left( {{T_{round}} - {T_{reply}}} \right)/2 \\ \end{array} \right.$$ (1) Due to variations in attitudes, indoor magnetic fields and environmental layout will all impact the ranging results in different ways when UWB is applied in various environments. To ensure the precision of ranging, it is necessary to calibrate UWB according to the positioning environment.
In the experimental environment, the UWB label is placed with a spacing of 0.1 m, and 30 sets of data are measured in a range of 0.1 m to 3.0 m from the UWB base station. To ensure the precision of the calibration, 1 000 ranging data points are obtained at each distance. The mean values of ranging data collected at each distance are plotted against the actual distances, as shown in Fig. 2.
As shown in Fig. 2, with the increase of the ranging distance, the ranging error shows a decreasing trend, with the maximum ranging error at 0.1 m of 0.47 m.
Take the mean value of ranging data of each distance as the abscissa data and the actual distance as the ordinate data for curve fitting. Tab. 1 shows the root mean square error corresponding to each fitting curve.
As shown in Tab. 1, compared with the quadratic fitting curve and cubic fitting curve, the root mean square error of the quartic fitting curve remains relatively small, and that of the quintic fitting curve is the smallest. Even though the root mean square error of the quantic fitting curve is smaller than that of the quartic fitting curve, the improvement is quite small. Considering the ranging precision and real-time performance, this paper adopts the quartic fitting polynomial as the fitting function of the UWB ranging calibration. The specific fitting function is shown in Equation (2).
$$y = 0.029\;87{x^4} - 0.010\;73{x^3} - 0.504\;6{x^2} + 0.910\;8x + 1.544$$ (2) Table 1. Residual modulus of UWB ranging fitting curve
Fitting curve Quadratic fitting Cubic fitting Quartic fitting Quintic fitting Residual 0.04083 m 0.03166 m 0.01924 m 0.01897 m Figure 3 shows the quartic fitting curve.
Figure 4 shows the comparison between actual errors and errors after fitting.
As shown in the figure above, the average ranging error in the 30 ranging data groups before calibration is 0.120 m, while that after calibration fitting is merely 0.012 m. The ranging error of UWB is effectively suppressed by ranging calibration.
-
According to the ranging data obtained in the calibration process, the overall precision of the ranging is about 0.1 m. However, there are outliers with significant errors in the set of ranging values. Figure 5 (a) and (b) show the data samples of 1 m and 2.5 m ranging in the calibration process, respectively.
In the case of a small number of ranging samples, outliers greatly impact the ranging errors, which subsequently affects the trajectory tracking precision of the positioning target. Therefore, this paper adopts an outlier detection method based on Mahalanobis distance to eliminate the outliers in the UWB ranging process.
The conventional Mahalanobis distance outlier detection method directly applies sample data mean and covariance matrix for calculating the Mahalanobis distance. In the case of a small sample size or a large number of outliers, this method deviates the calculated mean and covariance matrix towards the outliers, resulting in outliers being detected as normal values, futher resulting in incomplete exclusion of outliers. This paper proposes an improvement based on the conventional Mahalanobis distance outlier detection method. In this paper, all mean values and covariance matrixes applied in the detection process are stable values calculated in the Minimum Covariance Determinant (MCD) estimates, so that there is a significant difference between the Mahalanobis distance of the outliers and the normal values in the calculated sample, thereby making it possible to exclude the outliers.
The conventional Mahalanobis distance is calculated with the following equation.
$$\left\{ \begin{array}{l} d = \sqrt {{B^{\rm T}}{S^{ - 1}}B} \\ B = \left[ {{x_i} - T} \right] \\ \end{array} \right.$$ (3) Where, d is the Mahalanobis distance,
${x_i}$ is the sample size, T is the mean value of the sample data, and S is the covariance matrix of the sample data. The Mahalanobis distance of the collected sample data is approximately in a chi-square distribution with a degree of freedom of p, and a threshold$\sqrt {x_{p,\alpha }^2} $ is set to a certain confidence coefficient. When the Mahalanobis distance of a calculated ranging value is greater than the set threshold, i.e.,$d > \sqrt {x_{p,\alpha }^2} $ , it is regarded as an outlier and therefore immediately excluded from the data set [9].The core idea of the modified Mahalanobis distance outlier detection method is to apply FAST MCD algorithm (FAST-MCD) proposed by Rousseeuw[9], in combination with the conventional Mahalanobis distance calculation process for a stable covariance matrix as well as a stable mean vector, and then calculate the stable Mahalanobis distance according to the conventional Mahalanobis distance equation for eliminating outliers.
First, randomly select a number of
$h$ distance data from the distance samples, where$h = M{\rm{ \times }}\alpha $ (0.5<$\alpha $ <1), calculate the mean value${T_1}$ and covariance${S_1}$ of the$h$ random distance data, and then apply Equation (1) to calculate the Mahalanobis distance$d$ between the$M$ distance samples and${T_1}$ . After that, select$h$ data samples with the minimum Mahalanobis distance from the$M$ distance samples, and calcute the new mean value${T_2}$ and covariance${S_2}$ based on the newly selected sample data.${S_1} = {S_2}$ is only true when${T_1} = {T_2}$ and$\det \left( {{S_1}} \right) = \det \left( {{S_2}} \right)$ . Keep iterating this process until${T_i} = {T_{i - 1}}$ and${S_i} = {S_{i - 1}}$ .${T_i}$ and${S_i}$ obtained at this moment are considered as the stable mean and covariance. Finally, calculate the Mahalanobis distance of each data in the distance sample based on${T_i}$ and${S_i}$ with Equation (3), completing the detection of outliers.The flow chart of the outlier detection method based on the modified Mahalanobis distance is shown in Fig. 6.
Figure 7(a) and (b) show the ranging results at 1 m and 2.5 m upon the exclusion of the outliers by the outlier detection method based on the modified Mahalanobis distance.
As shown in the above two figures, there are no outliers with relatively large deviations in the whole ranging process, and the ranging values demonstrate quite small fluctuations with strong robustness. The above mentioned ranging experiment has demonstrated the effectiveness of the outlier detection method based on the modified Mahalanobis distance adopted in this paper, and that this method is able to ensure real-time ranging while effectively avoiding the occurrence of outliers in the ranging process.
-
The inertial navigation technology is a fully independent navigation and positioning method. As it has no photoelectric connection with the external environment, it features strong anti-interference capability, high real-time navigation data update rate and excellent stability. However, the errors in the positioning process accumulate over time, which is mainly due to inherent precision of the accelerometer sensor and the gyroscope sensor, thereby making it difficult to position for a prolonged period of time. In current applications, the most effective method to solve this problem is fusion positioning and the use of different sensors to complement each other so as to improve navigational positioning precision[10].
Compared with common indoor wireless positioning technologies such as WiFi, Bluetooth and ZigBee, UWB signals are advantageous in that they have excellent anti-interference ability, high resolution, centimeter-level ranging accuracy, etc., making UWB signal standing out in the field of wireless positioning[11]. However, due to the high environmental complexity and large number of obstacles in the case of indoor conditions, the signals may encounter issues such as non-line-of-sight and multi-path during transmission.
In view of the above considerations, this paper designs a UWB and inertial navigation fusion positioning method based on the extended Kalman filter technology, and leverage the strengths and characteristics of both types of sensors, so that they are integrated to complement each other. Therefore, inertial navigation posture is applied to minimize errors caused by the environmental interference in UWB, in addition, the advantage of high precision ranging of UWB is taken to suppress the cumulative error in the inertial navigation solution process.
The fusion positioning method consists of loose coupling and tight coupling depending on the degree of fusion, of which loose coupling is relatively simple. The two systems involved in fusion positioning are independent of each other in the positioning process. Simple fusion is performed on the two sets of positioning results obtained[12]. On the contrary, tight coupling is a relatively advanced fusion method, where information obtained by the two systems is fused in the process of positioning solution to take full advantages of both, making it an integrated positioning method [13-14]. This paper adopts the tight-coupling fusion method, where the UWB ranging data and inertial navigation data are combined to minimize positioning errors.
-
Generally IMU is fixed to the carrier, so the output data of the sensor is measured in the carrier coordinate system. Therefore, a transformation matrix must be applied to convert the IMU output data into the format in the navigation coordinate system to facilitate calculation. The following section introduces each and every coordinate system adopted in this paper.
(1) Carrier coordinate system
The inertial navigation itself defines an inertial reference coordinate system. However, for the convenience of calculation, when the inertial navigation and the carrier are fixed together, the coordinate system defined by the inertial navigation itself is generally superimposed with the carrier coordinate system. Therefore, when the two coordinate systems are superimposed, the center of mass of the carrier can be taken as the origin of the carrier coordinate system. The X-axis is in the longitudinal direction, that is, the forward direction of the carrier motion. The Z-axis is in the direction perpendicular to the plane of the carrier, and the Y-axis is in a right-handed coordinate system, perpendicular to the XZ plane and pointing to the right. The inertial navigation accelerometer and gyroscope then output the linear acceleration and angular velocity of the carrier moving along the three axes of the carrier coordinate system. The carrier coordinate system in use, also known as the b system, is denoted by
$o{x_b}{y_b}{z_b}$ . The schematic diagram is shown in Fig.8.(2) Navigation coordinate system
The navigation coordinate system, also known as the n system, is a global coordinate system for constructing the whole positioning system denoted by coordinates
$o{x_n}{y_n}{z_n}$ . The navigation coordinate system in this paper is constructed with reference to the UWB base station in indoor positioning environments. The schematic diagram is shown in Fig. 9.The ground plane in the indoor environment is the XY plane. The Z-axis is a right-handed coordinate system, perpendicular to the XY plane and pointing upwards. The base stations are placed along various coordinate axes, and the origin is where base station 0 is located.
-
In this paper, EKF is applied to for the tight-coupling fusion positioning of the inertial navigation system and UWB. The functional block diagram of the tight-coupling fusion positioning of the UWB ranging and inertial navigation system is shown in Fig.10.
Pseudorange
${D_S}$ is obtained by applying the position coordinates obtained by UWB base station with known coordinates and SINS calculation.${D_S}$ is compared with the UWB ranging value${D_U}$ and is used as the observed value of the system. The error of the system state is estimated based on EKF, and the system state quantity is corrected accordingly.Given the dynamic characteristics of the positioning target, the system state vectors taken are the 12 dimensions including position, velocity, direction and angular velocity. Before using EKF to estimate the state quantity of the system, estimations of the system state are divided into two parts: estimation of angular velocity and estimation of other system state quantities. Among them, the output of the gyroscope is directly applied as the estimation for the angular velocity, i.e.:
$${\overset{\frown} \omega } = {{\textit{z}}_{gyro}}$$ (4) The gyroscope errors consist of scale errors and dynamic errors. Assuming that the gyroscope applied by the system has been well calibrated, all such errors can be ignored, and therefore the output of the gyroscope can be modeled as:
$${{\textit{z}}_{gyro}} = \omega + {\eta _{gyro}}$$ (5) In the equation above,
${\eta _{gyro}}$ denotes random noise of the gyroscope.Similar to the modeling process of the gyroscope, the accelerometer is modeled as:
$$a = \ddot X - g + {\eta _{acc}}$$ (6) In the equation above,
$a$ denotes the output data directly obtained from the accelerometer, which is the real acceleration of the target,$g$ denotes the gravity acceleration component, and${\eta _{acc}}$ denotes the random noise of the accelerometer.Therefore, it only requires to make use of EKF to estimate other system states, namely, estimating a total of state quantities in 9 dimensions, including position, velocity and direction. As the computational complexity of Kalman filter is approximately
${n^3}$ ($n$ is the dimension of the state vector) [15], the computational complexity of the whole Kalman filter process is reduced by more than 50%.Therefore, in the process of EKF calculation, the state vector
$\xi \left( k \right)$ is taken as:$$\xi \left( k \right) = \left( \begin{array}{l} X \\ \rho \\ \delta \\ \end{array} \right) = \left( {\begin{array}{*{20}{c}} x\;\;y \;\;{\textit{z}}\;\;{{\rho _x}} \;\;{{\rho _y}}\;\;{{\rho _z}} \;\;{{D_0}}\;\;{{D_1}}\;\;{{D_2}} \end{array}} \right)$$ (7) In the equation above, X denotes the position coordinates of the positioning target in the navigation coordinate system,
$\rho $ denotes the velocity of the positioning target in the carrier system, and the three-dimensional vector represents the attitude error of the positioning target.The orientation of the target here is represented by reference attitude and rotation vector
$\delta $ , among which the magnitude of$\delta $ denotes the rotation angle and the unit vector in the direction of$\delta $ denotes the rotation axis, and the corresponding rotation matrix is denoted by$\exp \left( { [\kern-0.15em[ \delta ]\kern-0.15em] } \right)$ .$ [\kern-0.15em[ \delta ]\kern-0.15em] $ is the skew symmetric matrix of the vector$\delta $ :$$\delta = \left( {\begin{array}{*{20}{c}} {{D_0}}&{{D_1}}&{{D_2}} \end{array}} \right)$$ (8) $$ [\kern-0.15em[ \delta ]\kern-0.15em] = \left[ {\begin{array}{*{20}{c}} 0&{{D_2}}&{ - {D_1}} \\ { - {D_2}}&0&{{D_0}} \\ {{D_1}}&{ - {D_0}}&0 \end{array}} \right]$$ (9) Expand the first order of
$\exp \left( { [\kern-0.15em[ \delta ]\kern-0.15em] } \right)$ as follows:$$\exp \left( { [\kern-0.15em[ \delta ]\kern-0.15em] } \right) = \left( {I + [\kern-0.15em[ \delta ]\kern-0.15em] } \right)$$ (10) Where, I is the identity matrix of size 3 × 3.
The attitude estimation of the positioning target
${\overset{\frown} R} $ can be expressed as follows[16]:$${\overset{\frown} R} = {{\overset{\frown} R} _{ref}}\left( {I + [\kern-0.15em[ {\overset{\frown} \delta } ]\kern-0.15em] } \right)$$ (11) In the equation above, the reference attitude
${{\overset{\frown} R} _{ref}}$ will be updated according to the rotation vector${\overset{\frown} \delta } $ after each Kalman filtering estimation, assuming that the reference attitude before the update is denoted by${{\overset{\frown} R} _{ref,pre}}$ , and the reference attitude updated after the Kalman filtering is denoted by${{\overset{\frown} R} _{ref,post}}$ , then$${{\overset{\frown} R} _{ref,post}} = \exp \left( { [\kern-0.15em[ \delta ]\kern-0.15em] } \right){{\overset{\frown} R} _{ref,pre}}$$ (12) ${\overset{\frown} \delta } $ is reset to zero after the update of${{\overset{\frown} R} _{ref}}$ [17].Assuming that the position change of the internal positioning target relative to the carrier coordinate system during
$\Delta t$ (from discrete time k to time (k+1)) is$\Delta X = \left( {\Delta {x^b},\Delta {y^b},\Delta {{\textit{z}}^b}} \right)$ , then:$$\left\{ \begin{array}{l} \Delta {x^b} = {\rho _{x,k}}\Delta t + \dfrac{1}{2}{a_{x,k}}\Delta {t^2} \\ \Delta {y^b} = {\rho _{y,k}}\Delta t + \dfrac{1}{2}{a_{y,k}}\Delta {t^2} \\ \Delta {{\textit{z}}^b} = {\rho _{{\textit{z}},k}}\Delta t + \dfrac{1}{2}{a_{{\textit{z}},k}}\Delta {t^2} \\ \end{array} \right.$$ (13) R is expressed as the following:
$$R = \left[ {\begin{array}{*{20}{c}} {{R_{00}}}&{{R_{01}}}&{{R_{02}}} \\ {{R_{10}}}&{{R_{11}}}&{{R_{12}}} \\ {{R_{20}}}&{{R_{20}}}&{{R_{22}}} \end{array}} \right]$$ (14) The position of the target in the navigation coordinate system at time (k+1) is then:
$$\left\{ \begin{array}{l} {x_{k + 1}} = {x_k} + {R_{00}}\Delta {x^b} + {R_{01}}\Delta {y^b} + {R_{02}}\Delta {{\textit{z}}^b} \\ {y_{k + 1}} = {y_k} + {R_{10}}\Delta {x^b} + {R_{11}}\Delta {y^b} + {R_{12}}\Delta {{\textit{z}}^b} \\ {{\textit{z}}_{k + 1}} = {{\textit{z}}_k} + {R_{20}}\Delta {x^b} + {R_{21}}\Delta {y^b} + {R_{22}}\Delta {{\textit{z}}^b} \\ \end{array} \right.$$ (15) The velocity of the positioning target in the carrier coordinate system at time (k+1) can be obtained from the following equation.
$$\left\{ \begin{array}{l} {\rho _{x,k + 1}} = {\rho _{x,k}} + {a_{x,k}}\Delta t \\ {\rho _{y,k + 1}} = {\rho _{y,k}} + {a_{y,k}}\Delta t \\ {\rho _{{\textit{z}},k + 1}} = {\rho _{{\textit{z}},k}} + {a_{{\textit{z}},k}}\Delta t \\ \end{array} \right.$$ (16) The update of
$\delta $ can be obtained from the following equation.$${\delta _{k + 1}} = \exp \left( { [\kern-0.15em[ d ]\kern-0.15em] } \right){\delta _k}\exp {\left( { [\kern-0.15em[ d ]\kern-0.15em] } \right)^{\rm T}}$$ (17) In the above equation, d denotes the attitude error represented by the Rodriguez parameter, as follows:
$$d = \left( {{d_0},{d_1},{d_2}} \right) = \left( {\frac{{{\omega _x}dt}}{2},\frac{{{\omega _y}dt}}{2},\frac{{{\omega _z}dt}}{2}} \right)$$ (18) ${\omega _x}$ ,${\omega _y}$ ,${\omega _z}$ are the angular velocity of the gyroscope relative to the three axes of the carrier coordinate system in discrete time interval, and are regarded as constants within the sampling time.The state model of the system is expressed as follows:
$${{\overset{\frown} \xi } _{k + 1}} = f\left( {{{{\overset{\frown} \xi } }_k}} \right)$$ (19) f in the equation above represents the linearized function model after first order Taylor expansion of the system.
-
In UWB and inertial navigation fusion positioning, since the UWB ranging has no cumulative errors and the ranging error is merely around 0.1 m, it falls into the wireless ranging technology with relatively high precision. Therefore, UWB ranging values are used as the observed values of extended Kalman filtering.
One assumption is that the coordinates at known position
${p_i}$ of the UWB base station are$\left( {\begin{array}{*{20}{c}} {{p_{i,x}}}&{{p_{i,y}}}&{{p_{i,{\textit{z}}}}} \end{array}} \right)$ , and the ranging value obtained by using UWB label at time k is${d_{i,k}}$ . The other assumption is that the actual position coordinate of the positioning target is${X_k} = \left( {{{\begin{array}{*{20}{c}} {{x_k}}&{{y_k}}&{\textit{z}}_k \end{array}}}} \right)$ . Therefore, the observation equation can be modeled as:$${d_{i,k}} = \left\| {X - {p_i}} \right\| + {\nu _d}$$ (20) Where,
${\nu _d}$ denotes the process noise of the UWB measurements, and$ \Vert \cdot \Vert $ denotes the Euclidean norm, also known as Euclidean distance, therefore:$$ \left\| {X - {p_i}} \right\| = \sqrt {{{\left( {{x_k} - {p_{i,x}}} \right)}^2} + {{\left( {{y_k} - {p_{i,y}}} \right)}^2} + {{\left( {{z_k} - {p_{i,z}}} \right)}^2}} $$ (21) And the observation matrix is:
$${H_i} = \dfrac{{\partial {d_{i,k}}}}{{\partial \xi }} = \left( {\begin{array}{*{20}{c}} {\dfrac{{\partial {d_{i,k}}}}{{\partial X}}}&{\dfrac{{\partial {d_{i,k}}}}{{\partial \rho }}}&{\dfrac{{\partial {d_{i,k}}}}{{\partial \delta }}} \end{array}} \right)$$ (22) Where,
$$\left\{ \begin{array}{l} \dfrac{{\partial {d_{i,k}}}}{{\partial X}} = \dfrac{{X - {p_i}}}{{\left\| {X - {p_i}} \right\|}} \\ \dfrac{{\partial {d_{i,k}}}}{{\partial \rho }} = \dfrac{{\partial {d_{i,k}}}}{{\partial \delta }} = 0 \\ \end{array} \right.$$ (23) In the above equation,
$\dfrac{{X - {p_i}}}{{\left\| {X - {p_i}} \right\|}}$ can be expressed as an identity vector${e_i}$ . And the observation matrix is found as a sparse matrix.Assuming that the position coordinate obtained by the solution of the positioning target is
${\tilde X_k} = \left( {{{\tilde x}_k}}\;\;{{{\tilde y}_k}}\;\;{\tilde {\textit{z}}}_k \right)$ , therefore, the prediction equation of the observation is:$${Z_k} = \sqrt {{{\left( {{{\tilde x}_k} - {p_{i,x}}} \right)}^2} + {{\left( {{{\tilde y}_k} - {p_{i,y}}} \right)}^2} + {{\left( {{{\tilde {\textit{z}}}_k} - {p_{i,{\textit{z}}}}} \right)}^2}} $$ (24) Based on the state equation and observation equation, the target state information can be obtained by the time update and state update of the EKF.
The time update equation is:
$$\left\{ \begin{array}{l} {{{\overset{\frown} \xi } }_{k + 1|k}} = f\left( {{{{\overset{\frown} \xi } }_{k|k}}} \right) \\ {P_{k + 1|k}} = { \varPhi} {P_{k|k}}{{{\varPhi}} ^{\rm T}} + Q \\ \end{array} \right.$$ (25) State update equation is:
$$\left\{ \begin{array}{l} {K_{k + 1}} = {P_{k + 1|k}}{H^{\rm{T}}}{\left( {H{P_{k + 1|k}}{H^{\rm{T}}} + R} \right)^{ - 1}} \\ {{{\overset{\frown} \xi } }_{k + 1}} = {{{\overset{\frown} \xi } }_{k + 1|k}} + {K_{k + 1}}\left( {{Z_{k + 1}} - H{{{\overset{\frown} \xi } }_{k + 1|k}}} \right) \\ {P_{k + 1}} = \left( {I - {K_{k + 1}}H} \right){P_{k + 1|k}} \\ \end{array} \right.$$ (26) -
The experimental test environment in this paper is a laboratory with a size of 5.0 m×8.0 m. Positioning cameras with the Optitrack high-precision positioning motion capturing system are set around a 4.0 m×4.2 m rectangle in the room. The experiment site in this paper is located in a 2.4 m×2.4 m rectangle within the motion capturing system. The motion capturing system is a high-precision indoor positioning system adopting infrared cameras to cover indoor positioning space. Reflective markers are placed on the objects to be positioned. The three-dimensional position information of these objects is calculated by capturing the reflection images of the reflective markers on the positioning cameras. In addition, the precision of the Optitrack positioning cameras is as high as 0.1 mm. In all experiments in this paper, trajectories calculated based on the motion capturing system are taken as actual target trajectories, the positioning precisions of which are compared with the proposed positioning method in this paper.
In the experimental scene of this paper, six UWB base stations are set up and labelled with ID 0 to 5, respectively. The main control chip applied by the UWB base station is STM32F103RC.
The layout of the UWB base stations in the experimental environment and the navigation coordinate system used in this paper are shown in Fig. 11.
In measuring the coordinates of the base stations, the reflective markers identifiable by the motion capturing system are first placed on the base stations, and then the positioning coordinates captured by the motion capturing system are taken as specific position coordinates of each base station, as shown in Tab. 2.
Table 2. UWB base station coordinates
Base station ID X/m Y/m Z/m 0 0.02 0.01 0.14 1 2.41 0.01 1.48 2 2.40 1.21 0.15 3 2.42 2.40 0.15 4 0.00 2.41 1.49 5 0.01 1.22 0.14 -
To verify the precision of the UWB and inertial navigation fusion positioning method proposed in this paper, field experiments are carried out, the estimated trajectory errors, the positioning accuracy and trajectory error of the two positioning methods are calculated.
As shown in Fig.12, in this experiment, the positioning label is fixed to a positioning trolley. The trolley is controlled by the motion capturing system based on the feedback of the input trajectory coordinates. In the experiment, the trolley moves at a speed of 0.05 m/s, and the motion precision of the trolley is up to centimeter level at this speed. The positioning label affixed to the trolley is supplied by a power bank via a USB port. A reflective marker point identifiable by the motion capturing system is laid on the center of the positioning label.
The controlled trolley moves in a rectangular track of 1.2 m×1.2 m at the experiment site. The schematic diagram of the simulated trolley trajectory is shown in Fig.13. The trolley began its trajectory at point A and moved 1.2 m to point B, after which it further moves 1.2 m to point C and 1.2 m to point D until it eventually returns to point A. In the experiment results, the trajectory of the reflective marker in the center of the positioning label obtained by the motion capturing system is taken as the actual motion trajectory of the positioning label.
First, the ranging data obtained in the movement of the trolley is input into MATLAB and the UWB positioning trajectory was generated. The results are shown in Fig.14(a) and (b).
In Fig.14(a) and (b), the line formed by red dots represents the actual motion trajectory, and the solid blue line represents the position trajectory only with UWB for positioning. An artificial disturbance is added during the trolley motion by human movement, creating obstruction between the UWB base stations and the label. According to the experiment data, better motion trajectories of the positioning target are obtained by applying UWB without obstruction. In segments A B, B C and D A before introducing obstruction, the maximum positioning error and the mean absolute error are 0.461 m and 0.378 m, respectively, which has achieved positioning at centimeter level. However, in segment C D with human interference introduced, the maximum positioning error is 1.153 m, and the mean absolute error is 0.412 m. Throughout the rectangular trajectory experiment, the maximum absolute error is 1.153 m, the mean absolute error is 0.386 m, and the root mean square error is 0.397 m. Based on the above data, that obstructions of the UWB signal have no great impact on the precision of the final positioning. In this case, the positioning results by applying UWB and inertial navigation fusion are given below in Fig.15(a) and (b).
In Fig.15(a) and (b), the line formed by red dots represents the actual motion trajectory, the solid blue line is the motion trajectory obtained with UWB alone for the positioning experiment, and the black double dashed line is the motion trajectory with the UWB and inertial navigation fusion method. It can be seen from the figure that, compared to the positioning only with UWB, the positioning using the fusion of UWB and inertial navigation can achieve a higher positioning accuracy without changing the real-time performance.
Figure 16 and Fig.17 show error comparison of the two positioning methods.
As can be seen from Fig.16, even though a surge of errors in positioning with UWB and inertial navigation fusion is observed in case of interference, the errors are significantly reduced compared with the positioning results only with UWB. Robustness of positioning is significantly improved.In the positioning with UWB and inertial navigation fusion, in segments A B, B C and D A before introducing the obstruction, the maximum positioning error is 0.305 m, and the mean absolute error is 0.232 m, which has enabled positioning at centimeter level. However, in segment C D with human interference introduced, the maximum positioning error is 0.692 m, and the mean absolute error is 0.285 m. Throughout the rectangular trajectory, the mean absolute error is 0.246 m and the root mean square error is 0.255 m, demonstrating that such positioning method has reached a high positioning precision. Compared with the positioning results with UWB alone, the mean precision is improved by 36.3% and the root mean square error is reduced by 35.8%.
According to Fig.17, when UWB alone is used for positioning, the error is within 1.0 m, and around 90% of the points have an error of 0.4 m, while the error of the UWB and inertial navigation fusion positioning is within 0.6 m, and over 90% of the points have an error of less than 0.25 m.
-
摘要: 针对室内环境日益复杂,单一的定位系统已经不能满足人们对定位准确度需求的问题,设计了一种利用超宽带(UWB)和惯性导航融合进行的室内定位方法:首先针对UWB测距结果容易受环境影响的问题,根据实验环境对UWB测距进行了标定;然后利用改进马氏距离的异常值检测方法对测距过程中的异常值进行了剔除;最后采用了紧耦合的卡尔曼滤波器,以UWB测距值作为扩展卡尔曼滤波观测量,以惯性导航解算的位姿作为扩展卡尔曼滤波器的预测量,通过UWB测距来不断校正惯性导航的位姿数据。最终为了验证所提方法的可行性和有效性,进行了UWB单独定位和UWB与惯性导航融合定位的小车搭载矩形运动轨迹实验,通过对两种方法实验数据的对比分析,在加入了外界干扰时的矩形轨迹定位实验中,利用UWB和惯性导航融合的定位结果,平均精度比单独利用UWB进行定位时提高了36.3%;误差结果对比表明,利用UWB和惯性导航融合定位的误差波动更小,具有更高的鲁棒性。表明了该融合定位算法与单独利用UWB技术进行定位的算法相比,能够有效地抑制定位过程中的干扰问题,并且可显著地提高在室内环境下定位系统的鲁棒性和定位精度。Abstract: In response to the increasingly complex indoor environments where a single positioning system is no longer able to satisfy positioning precision demands, an indoor positioning method applying Ultra-Wide Band (UWB) ranging and inertial navigation fusion was proposed. First, the problem that UWB ranging results were proved to be affected by the environment was given, UWB ranging calibration was carried out in the experimental environment. Thereafter, the outliers in the ranging process were eliminated by applying the modified Mahalanobis distance outliers detection method. Then, a tightly coupled Kalman filter was adopted, where UWB ranging values were taken as the extended Kalman filter observation quantity, the position and attitude of the inertial navigation were taken as the extended Kalman filter prediction value, and UWB ranging values were used to constantly correct the position and attitude data of the inertial navigation solution. Finally, to verify the feasibility and effectiveness of the proposed method, trolley-mounted rectangular motion experiments were conducted with UWB positioning alone, as well as positioning by UWB and inertial navigation fusion. According to the comparison of experimental data, in interfered rectangular trajectory positioning experiments, when UWB was combined with inertial navigation positioning, the average precision was increased by 36.6% than that with UWB positioning alone; The comparison of the error results show that the error fluctuation of the fusion positioning using UWB and inertial navigation is smaller, but with higher robustness; The results indicate that, compared with algorithms utilizing the UWB technology alone for positioning, such fusion positioning algorithm can effectively suppress interferences in the positioning process. Furthermore, the robustness and positioning precision of the positioning system in indoor environments can be significantly improved.
-
Key words:
- ultra-wide band (UWB) /
- inertial navigation /
- outliers detection /
- fusion positioning
-
Table 1. Residual modulus of UWB ranging fitting curve
Fitting curve Quadratic fitting Cubic fitting Quartic fitting Quintic fitting Residual 0.04083 m 0.03166 m 0.01924 m 0.01897 m Table 2. UWB base station coordinates
Base station ID X/m Y/m Z/m 0 0.02 0.01 0.14 1 2.41 0.01 1.48 2 2.40 1.21 0.15 3 2.42 2.40 0.15 4 0.00 2.41 1.49 5 0.01 1.22 0.14 -
[1] Uradzinski M, Guo H, Liu X, et al. Advanced indoor positioning using Zigbee wireless technology [J]. Wireless Personal Communications, 2017, 97(67): 6509-6518. [2] Oulose A, Eyobu O S, Han D S. An indoor position estimation algorithm using smartphone IMU sensor data [J]. IEEE Access, 2019, 7(8): 11165-11177. [3] Wang Y L, Lv J N, Li J H, et al. High precision indoor positioning system based on UWB [J]. Yangtze River Information Communication l, 2021, 34(3): 111-114, 117. [4] Chen D L, Yang Z. Study on high precision indoor positioning technology based on UWB/MEMS[D]. Xuzhou: China University of Mining and Technology, 2015.(in Chinese) [5] Sun B W, Fan Q G, Wu Y H, et al. Foot-mounted pedestrian navigation technology based on tightly coupled PDR/UWB [J]. Transducer and Microsystem Technologies, 2017, 36(3): 43-48. [6] Zampella F, Angelis A D, Skog I, et al. A constraint approach for UWB and PDR fusion[C]//Proceedings of the 2012 International Conference on Indoor Positioning and Indoor Navigation (IPIN), IEEE, 2012: 2-6. [7] Chen L L, Yang Y, Yuan E, Zhao Y X, et al. Integrated indoor positioning algorithm based on UWB/PDR [J]. Information Technology and Network Security, 2019, 38(5): 53-57. [8] Alhadrami S, Salman A, Khalifa H A. Ultra wideband positioning: an analytical study of emerging technologies[C]//Proceeding of International Conference Sensor Technologies and Applications (SENSORCOMM). IEEE, 2014: 1-9. [9] Yin P Q, Lu D M, Yuan Y, et al. An improved non-local image de-noising algorithm based on Mahalanobis distance [J]. Journal of Computer-Aided Design & Computer Graphics, 2016, 28(3): 404-410. (in Chinese) [10] Xiong Hailiang, Bian Ruochen, Li Yujun, et al. Fault-Tolerant GNSS/SINS/DVL/CNS integrated navigation and positioning mechanism based on adaptive in-formation sharing factors [J]. IEEE Systems Journal, 2020, 14(3): 3744-3754. doi: 10.1109/JSYST.2020.2981366 [11] Wen Kai, Yu Kegen, Li Yingbing. A new quaternion Kalman filter based foot-mounted IMU and UWB tightly-coupled method for indoor pedestrian navigation [J]. IEEE Transactions on Vehicular Technology, 2020, 69(4): 4340-4352. doi: 10.1109/TVT.2020.2974667 [12] Tian Q L, Wang K I K, Salcic Z. A resetting approach for INS and UWB sensor fusion using Particle Filter for pedestrian tracking [J]. IEEE Transactions on Instrumentation and Measurement, 2020, 69(8): 5914-5921. doi: 10.1109/TIM.2019.2958471 [13] Ascher C, Zwirello L, Zwick T, et al. Integrity monitoring for UWB/INS tightly coupled pedestrian indoor scenarios [C]//Indoor Positioning and Indoor Navigation (IPIN), 2011 International Conference on. IEEE, 2011, PP(1-6): 6071948. [14] Xiong Hailiang, Mai Zhenzhen, Tang Juan, et al. Robust GPS/INS/DVL navigation and positioning method using adaptive federated strong tracking filter based on weighted least square principle [J]. IEEE Access, 2019, 7: 26168-26178. doi: 10.1109/ACCESS.2019.2897222 [15] Bar-Shalom Y, Li X R, Kirubarajan T. Estimation with Applications to Tracking and Navigation: Theory Algorithms and Software[M]. Canada: John Wiley & Sons, 2001: 303-306. [16] Zipfel P H. Modeling and Simulation of Aerospace Vehicle Dynamics[M]. 2nd ed. USA: American Institute of Aeronautics and Astronautics, 2007: 98-101. [17] Markley F L. Attitude error representations for Kalman filtering [J]. Journal of Guidance, Control, and Dynamics, 2003, 26(2): 311-317. doi: 10.2514/2.5048