﻿ 基于UKF的环境自适应UWB/DR室内定位方法
 计算机系统应用  2020, Vol. 29 Issue (5): 175-181 PDF

Adaptive UWB/DR Indoor Co-Localization Approach Based on UKF
ZHOU Lin, LI Xiao-Ming, JIANG Xian-Zhi
Faculty of Mechanical Engineering and Automation, Zhejiang Sci-Tech University, Hangzhou 310018, China
Abstract: In view of the Non-Line-Of-Sight (NLOS) error of Ultra-WideBand (UWB) signal propagation in complex indoor environments, an adaptive UWB/DR co-localization approach based on Unscented Kalman Filter (UKF) is proposed. It combines the positioning information of UWB and Dead Reckoning (DR) by establishing an adaptive UKF filtering model. In this process, the principle of innovation and Gaussian distribution is used to detect whether the UWB positioning result contains NLOS error, and then the environmental adaptation coefficient, which is constructed by real-time estimation covariance and theoretical covariance of the innovation, dynamically correct the observed noise of UWB and make it adaptive to the real environment to reduce the impact of NLOS error on the positioning result to a greater extent. The experimental results show that the proposed approach can effectively reduce the NLOS error of UWB positioning, and because of the innovative introduction of environmental adaptation coefficient, it has higher positioning accuracy and stronger anti-NLOS error performance than UKF positioning and Particle Filtering (PF) positioning.
Key words: UKF     environmental adaptation coefficient     UWB/DR     3σ principle     Non-Line-Of-Sight (NLOS) detection

1 DR定位

DR定位是一种精度较高的自主式定位方法, 尤其适用于短时短距定位. 其核心思想可概述为根据当前时刻的位置、速度和航向推算下一时刻的位置. 航位推算的实现原理如图1所示.

 $\left\{ {\begin{array}{*{20}{l}} {{x_{k + 1}} = {x_k} + {v_k}.\left( {{t_{k + 1}} - {t_k}} \right).\sin{\theta _k}} \\ {{y_{k + 1}} = {y_k} + {v_k}.\left( {{t_{k + 1}} - {t_k}} \right).\cos{\theta _k}} \\ {{\theta _{k + 1}} = {\theta _k} + {w_k}} \end{array}} \right.$ (1)
 图 1 航位推算原理图

 $\left\{\begin{array}{l} v = \dfrac{{\left( {{w_R} + {w_L}} \right)}}{2}D \\ w = \dfrac{{\left( {{w_R} - {w_L}} \right)D}}{L} \\ \end{array} \right.$ (2)
 图 2 数据采集移动平台图

 $\left\{ {\begin{array}{*{20}{l}} {{x_{k + 1}} = {x_k} + D\dfrac{{\left( {{w_{{L_k}}} + {w_{{R_k}}}} \right)}}{2}\left( {{t_{k + 1}} - {t_k}} \right).\sin{\theta _k}} \\ {{y_{k + 1}} = {y_k} + D\dfrac{{\left( {{w_{{L_k}}} + {w_{{R_k}}}} \right)}}{2}\left( {{t_{k + 1}} - {t_k}} \right).\cos{\theta _k}} \\ {{\theta _{k + 1}} = {\theta _k} + D\dfrac{{\left( {{w_{{L_k}}} - {w_{{R_k}}}} \right)}}{L}} \end{array}} \right.$ (3)

2 基于TOA的UWB定位

 图 3 DS测距原理图

 ${{{d}}_i} = \dfrac{{\left( {{T_{c1}}.{T_{c2}}{\rm{ - }}{T_{{\rm{r}}1}}.{T_{r2}}} \right)}}{{{T_{c1}} + {T_{c2}} + {T_{{\rm{r}}1}} + {T_{r2}}}}c$ (4)

 $\left\{ \begin{array}{l} \sqrt {{{(x - {x_1})}^2} + {{(y - {y_1})}^2}{\rm{ }}} {\rm{ = }}{{{d}}_1} \\ \sqrt {{{(x - {x_2})}^2} + {{(y - {y_2})}^2}{\rm{ }}} {\rm{ = }}{{{d}}_2} \\ \quad\quad\quad\quad\quad\vdots \\ \sqrt {{{(x - {x_i})}^2} + {{(y - {y_i})}^2}{\rm{ }}} {\rm{ = }}{{{d}}_i} \\ \end{array} \right.$ (5)
 图 4 UWB定位系统原理图

 $\left\{ \begin{array}{l} 2({{{x}}_n} - {x_1})x + 2({y_n} - {y_1})y = d_1^2 - d_n^2 - x_1^2+ x_n^2 - y_1^2 + y_n^2 \\ 2({{{x}}_n} - {x_2})x + 2({y_n} - {y_2})y = d_2^2 - d_n^2 - x_2^2+ x_n^2 - y_2^2 + y_n^2 \\ \qquad\qquad\qquad\qquad\qquad\vdots \\ 2({{{x}}_n} - {x_{n - 1}})x + 2({y_n} - {y_{n - 1}})y = d_{n - 1}^2 - d_n^2 - x_{n - 1}^2 + x_n^2\\ \qquad\qquad\qquad\qquad\qquad\qquad - y_{n - 1}^2 + y_n^2 \\ \end{array} \right.$ (6)

 $X = {({A^{\rm T}}A)^{ - 1}}{A^{\rm T}}B$ (7)

 $A = \left[ {\begin{array}{*{20}{c}} {2({{\rm{x}}_n} - {x_1})}&{2({y_n} - {y_1})} \\ {2({{\rm{x}}_n} - {x_2})}&{2({y_n} - {y_2})} \\ \vdots & \vdots \\ {2({{\rm{x}}_n} - {x_{n - 1}})}&{2({y_n} - {y_{n - 1}})} \end{array}} \right]$
 $B = \left[ {\begin{array}{*{20}{c}} {{{\rm{d}}_1}^2 - {d_n}^2 - {x_1}^2 + {x_n}^2 - {y_1}^2 + {y_n}^2} \\ {{d_2}^2 - {d_n}^2 - {x_2}^2 + {x_n}^2 - {y_2}^2 + {y_n}^2} \\ \vdots \\ {{d_{n - 1}}^2 - {d_n}^2 - {x_{n - 1}}^2 + {x_n}^2 - {y_{n - 1}}^2 + {y_n}^2} \end{array}} \right]$
3 基于UKF的环境自适应UWB/DR定位 3.1 选用UKF的合理性分析

3.2 UKF预测方程和观测方程确定

 ${{{X}}_k} = f\left( {{X_{k - 1}}, {\omega _{{R_{k - 1}}}}, {\omega _{{L_{k - 1}}}}} \right) + {Q_k}$ (7)

 $\begin{array}{l} f\left( {{X_{k - 1}}, {\omega _{{R_{k - 1}}}}, {\omega _{{L_{k - 1}}}}} \right) = \\ \left\{ {\begin{array}{*{20}{l}} {{x_{k + 1}} = {x_k} + D\dfrac{{\left( {{\omega _{{L_k}}} + {\omega _{{R_k}}}} \right)}}{2}\left( {{t_{k + 1}} - {t_k}} \right).\sin {\theta _k}} \\ {{y_{k + 1}} = {y_k} + D\dfrac{{\left( {{\omega _{{L_k}}} + {\omega _{{R_k}}}} \right)}}{2}\left( {{t_{k + 1}} - {t_k}} \right).\cos {\theta _k}} \\ {{\theta _{k + 1}}{\rm{ = }}{\theta _k}{\rm{ + }}D\dfrac{{\left( {{\omega _{{L_k}}} - {\omega _{{R_k}}}} \right)}}{L}\left( {{t_{k + 1}} - {t_k}} \right)} \end{array}} \right. \\ \end{array}$ (8)

 ${Z_k} = H{X_k}{\rm{ + }}{{\rm{R}}_k}$ (9)

 $H = \left( {\begin{array}{*{20}{c}} 1&0&0 \\ 0&1&0 \\ 0&0&1 \end{array}} \right)$ (10)
3.3 UKF方法

(1) 初始化

(2) Sigma点和对应权值计算

 $\left\{ \begin{array}{l} {X_0}(k - 1|k - 1) = \overline X (k - 1|k - 1)\\ {X_i}(k - 1|k - 1) = \overline X (k - 1|k - 1) \\ \qquad\qquad\qquad-\sqrt {(n + \lambda )P{{(k - 1|k - 1)}_i}} ,\;\; {i = 1,2 \cdots ,n} \\ {X_i}(k - 1|k - 1) = \overline X (k - 1|k - 1)\\ \qquad\qquad\qquad+\sqrt {(n + \lambda )P{{(k - 1|k - 1)}_i}} ,\\ \qquad\qquad\qquad\qquad{i = n + 1,n + 2 \cdots ,2n} \end{array} \right.$ (11)
 $\left\{ \begin{array}{l} \omega _0^{(m)} = \lambda /(n + \lambda )\\ \omega _0^{(c)} = \lambda /(n + \lambda ) + \left( {1 - {\alpha ^2} + \beta } \right)\\ \omega _i^{(m)} = \omega _i^{(c)} = 1/2(n + \lambda ),\left( {i = 1, \cdots ,2n} \right) \end{array} \right.$ (12)

(3) 状态预测

 $\left\{ \begin{array}{l} {X_i}(k|k - 1) = f\left[ {{X_i}(k - 1|k - 1)} \right] \\ \overline X (k|k - 1) = \displaystyle\sum\limits_{i = 0}^{2n} {\omega _i^{(m)}} {X_i}(k|k - 1) \\ P(k|k - 1) = \displaystyle\sum\limits_{i = 0}^{2n} {\omega _i^{(c)}} \left[ {\overline X (k|k - 1) - {X_i}(k|k - 1)} \right] \\ \qquad\qquad\qquad{\left[ {\overline X (k|k - 1) - {X_i}(k|k - 1)} \right]^ {\rm T} } + {Q_k} \\ \end{array} \right.$ (13)

(4) 量测预测

 $\left\{ \begin{array}{l} {Z_i}(k|k - 1) = H{X_i}(k|k - 1) \\ \overline Z (k|k - 1) = \displaystyle\sum\limits_{i = 0}^{2n} {\omega _i^{(m)}} {Z_i}(k|k{\rm{ - }}1) \\ {P_{{x_k}{{\textit{z}}_k}}} = \displaystyle\sum\limits_{i = 0}^{2n} {\omega _i^{(c)}} \left[ {{X_i}(k|k - 1) - \overline Z (k|k - 1)} \right] \\ \qquad\qquad{\left[ {{Z_i}(k|k - 1) - \overline Z (k|k - 1)} \right]^{\rm{T}}} \\ {P_{{{\textit{z}}_k}{{\textit{z}}_k}}} = \displaystyle\sum\limits_{i = 0}^{2n} {\omega _i^{(c)}} \left[ {{Z_i}(k|k - 1) - \overline Z (k|k - 1)} \right] \\ \qquad\qquad{\left[ {{Z_i}(k|k - 1) - \overline Z (k|k - 1)} \right]^{\rm{T}}} + {R_k} \\ \end{array} \right.$ (14)

(5) 状态更新

 $\left\{ \begin{array}{l} K(k + 1) = {P_{{x_k}{{\textit{z}}_k}}}P_{{{\textit{z}}_k}{{\textit{z}}_k}}^{ - 1}\\ \overline X (k + 1|k + 1) = \overline X (k + 1|k) \\ \qquad+K(k + 1)[Z(k + 1) - \overline Z (k + 1|k)]\\ P(k + 1|k + 1) = P(k + 1|k) \\ \qquad-K(k + 1){P_{{{\textit{z}}_k}{{\textit{z}}_k}}}{K^{\rm T}}(k + 1) \end{array} \right.$ (15)
3.4 环境适应系数构建

 $\varepsilon (k){\rm{ = }}Z(k) - \overline Z (k|k - 1)$ (16)

 ${{D(k) = }}\sum\limits_{i = 0}^{2n} {\omega _i^{(c)}} {\varepsilon _i}(k)\varepsilon _i^{\rm T}(k){\rm{ + }}{R_k}$ (17)

 $\left\{ \begin{array}{l} LOS:\;{\rm{ - }}3\sqrt {{{D(k)}}} \le \varepsilon (k) \le 3\sqrt {{{D(k)}}} \\ NLOS:\;{\rm{ - }}3\sqrt {{{D(k)}}} > \varepsilon (k),\varepsilon (k) > 3\sqrt {{{D(k)}}} \end{array} \right.$ (18)

 $\widehat {{D}}{{(k) = }}\frac{1}{M}\mathop \sum \limits_{m = 0}^{M - 1} \varepsilon (k{{ - m}}){\varepsilon ^{\rm T}}(k{{ - m}})$ (19)

 ${\alpha _k} = \left\{ {\begin{array}{*{20}{l}} {I, {\rm LOS}} \\ {\left( {\widehat {{D}}{{(k) - }}\sum\limits_{i = 0}^{2n} {\omega _i^{(c)}} {\varepsilon _i}(k){\varepsilon _i}{{(k)}^{\rm T}}} \right){\rm{/}}{R_{\rm LOS}}, {\rm NLOS}} \end{array}} \right.$ (20)

 ${R_k} = {\alpha _k}{R_{\rm LOS}}$ (21)
4 实验分析

 图 5 实验环境图

 图 6 运动轨迹复现

 图 7 定位误差

 图 8 定位累积误差

 图 9 定位误差累积概率分布

5 结论

 [1] 吕源, 李军. 室内环境下定位技术的研究. 电子测试, 2008(4): 19-22. DOI:10.3969/j.issn.1000-8519.2008.04.006 [2] 李大川. 室内外协同定位导航关键技术研究[硕士学位论文]. 成都: 电子科技大学, 2016. [3] 周牧, 耿小龙, 谢良波, 等. 室内Wi-Fi/PDR自适应鲁棒卡尔曼滤波融合定位方法. 电子学报, 2019, 47(1): 9-15. DOI:10.3969/j.issn.0372-2112.2019.01.002 [4] Kok M, Hol JD, Schön TB. Indoor positioning using ultrawideband and inertial measurements. IEEE Transactions on Vehicular Technology, 2015, 64(4): 1293-1303. DOI:10.1109/TVT.2015.2396640 [5] Li SX, Li GY, Wang L, et al. A three-dimensional robust ridge estimation positioning method for UWB in a complex environment. Advances in Space Research, 2017, 60(12): 2763-2775. DOI:10.1016/j.asr.2017.10.040 [6] Luo YJ, Law CL. Indoor positioning using UWB-IR signals in the presence of dense multipath with path overlapping. IEEE Transactions on Wireless Communications, 2012, 11(10): 3734-3743. DOI:10.1109/TWC.2012.081612.120045> [7] 何贵斌. 超宽带导航技术及应用构想. 中国新通信, 2012(8): 43-48. [8] 张令文, 杨刚. 超宽带室内定位关键技术. 数据采集与处理, 2013, 28(6): 706-713. DOI:10.3969/j.issn.1004-9037.2013.06.002 [9] Liu BB, Wu XJ, Reddy V, et al. A RTLS/DR based localization system architecture for indoor mobile robots. Proceedings of 2010 IEEE International Symposium on A World of Wireless, Mobile and Multimedia Networks. Montrreal, QB, Canada. 2010. 1–6. [10] 马勤勇. 两轮差速驱动移动机器人运动模型研究[硕士学位论文]. 重庆: 重庆大学, 2013. [11] Julier SJ, Uhlmann JK, Durrant-Whyte HF. A new approach for filtering nonlinear systems. Proceedings of 1995 American Control Conference-ACC’95. Seattle, WA, USA. 1995. 1628–1632. [12] 王芳, 李楠, 刘汝佳, 等. 基于超宽带和航位推算的室内机器人UKF定位算法. 导航定位与授时, 2017, 4(2): 26-30. [13] 郝晨, 李航. 扩展卡尔曼和无迹卡尔曼滤波应用对比研究. 沈阳师范大学学报(自然科学版), 2015, 33(2): 279-283. [14] 战帅, 冯世民. 扩展卡尔曼滤波器和无迹卡尔曼滤波器的性能对比研究. 信息通信, 2018(5): 35-36. DOI:10.3969/j.issn.1673-1131.2018.05.016 [15] Mehra R. Approaches to adaptive filtering. IEEE Transactions on Automatic Control, 1972, 17(5): 693-698. DOI:10.1109/TAC.1972.1100100 [16] 卞鸿巍, 金志华, 王俊璞, 等. 组合导航系统新息自适应卡尔曼滤波算法. 上海交通大学学报, 2006, 40(6): 1000-1003, 1009. DOI:10.3321/j.issn:1006-2467.2006.06.028