张少辉^1,3, 亓玉浩², 翟方文¹, 吕洪波⁴, 宋亦旭¹
1. 清华大学 计算机科学与技术系, 北京 100084;;
2. 兖州煤业股份有限公司 设备管理中心, 济宁 273500;;
3. 中国地质大学(北京) 信息工程学院, 北京 100083;;
4. 北方工业大学 机械与材料工程学院, 北京 100144
收稿日期：2021-07-21
作者简介：张少辉(1998—), 男, 硕士研究生
通讯作者：宋亦旭, 副研究员, E-mail: songyx@tsinghua.edu.cn

摘要：超宽带(ultra wide band, UWB)定位技术根据距离参量建立标签定位的数学模型。由于测距误差的存在, 标签定位的精确求解问题转换为非线性方程组的优化问题。在某项距离误差较大时, 现有的优化方法可能会失效。该文提出基于冗余距离筛选的优化方法, 以多个定位标签的间距为约束条件, 对冗余距离设置权值进行筛选, 使用梯度下降法优化Caffery方法计算的坐标初值。该方法在仿真实验中定位误差仅为Caffery-Taylor (CT)方法的70%, 在真实数据实验中优化效果明显好于CT方法。
关键词：超宽带(UWB)位姿解算距离筛选梯度下降
UWB positioning optimization method based on redundant distance screening
ZHANG Shaohui^1,3, QI Yuhao², ZHAI Fangwen¹, Lü Hongbo⁴, SONG Yixu¹
1. Department of Computer Science and Technology, Tsinghua University, Beijing 100084, China;
2. Equipment Management Center, Yanzhou Coal Mining Company Limited, Jining 273500, China;
3. School of Information Engineering, China University of Geosciences, Beijing 100083, China;
4. School of Mechanical and Material Engineering, North China University of Technology, Beijing 100144, China

Abstract: Ultra wide band (UWB) positioning technology establishes a mathematical model of the target location based on distance parameters. The problem of accurately solving the tag point positioning is transformed into an optimization problem of nonlinear equations due to ranging errors. Existing optimization methods may fail when certain distance errors are large. Therefore, this paper proposes an optimization method based on redundant distance screening by considering the distances between multiple positioning tags as constraints, filtering the redundant distances by weighting, and using a gradient descent method to optimize the initial value calculated by the Caffery method. In simulation experiments, the positioning error of this method is only 70% of the Caffery-Taylor (CT) method. In real data experiments, this method outperforms the CT method in terms of the optimization effect.
Key words: ultra wide band (UWB)pose solverdistance screeninggradient descent
近些年，随着无人驾驶、机器人、物联网等技术的发展，室内导航定位技术成为了研究热点。相比在开阔的室外空间定位，在狭小的室内空间中定位环境复杂，对定位精度要求高，国内外许多****和研究机构对其进行了大量的研究^[1-4]。室内定位的信号源众多，可根据信号来源分为2类：第一类外置信号源，主要包括WiFi、蓝牙、超宽带(UWB)信号、蜂窝数据等；第二类为内置信号源，如惯性导航的加速度计信号、磁力计信号等^[5]。内置信号源一般难以获得全局坐标系下自身的位姿信息；WiFi信号和蓝牙信号的覆盖区域有限，蜂窝数据对信号基站依赖程度太高。而UWB定位具有定位精度高、抗干扰能力强、穿透力强、功耗低及成本低等优势^[6-8]，已成为室内定位最常用的方法之一。
UWB定位一般是通过距离参量建立定位的数学模型，再通过解算该模型实现定位。因此为提高UWB定位精度，除了改善硬件条件外，通常是提高UWB信号的测距精度。提高测距精度一般分为2类。一类是改善现有的测距方法。例如吴邵华等^[9]提出的结合能量检测与匹配滤波的两步TOA估计方法，通过减少非视距(NLOS)误差的影响提高测距精度；Baba等^[10]提出了一种消除了时钟漂移的方法用来提高测距精度。另一类是对UWB信号进行滤波降噪，如Zhang等^[11]提出同时使用扩展Kalman滤波和鲁棒扩展Kalman滤波去除环境噪声的方法；王川阳等^[12]对UWB原始数据和定位结果使用3种降噪方法，提高了UWB信号的测距精度和定位精度；Yin等^[13]分析了UWB信号测距误差的特性，提出了基于抗差无迹Kalman滤波的UWB室内定位算法，有效提高了定位结果的精度。
综上所述，现有方法大多以消除信号杂波和环境噪声来提高测距精度，从而提高定位精度。但是复杂的室内环境中存在难以消除的随机误差如多径效应误差等，导致UWB信号的测距精度不稳定，如何根据不稳定的测距精度提高定位精度仍是一个亟需解决的问题。本文提出一种基于冗余距离筛选的UWB定位优化方法，以多个定位标签两两之间的距离为约束条件，根据UWB信号测量的距离误差设置每个距离在定位解算中的贡献度，以此筛选出定位解算中的距离参量组合。该方法以多个定位标签间距误差及带权的UWB距离误差设置代价函数，使用梯度下降法对定位标签坐标优化。仿真实验和真实数据实验表明，本文方法在标签定位和目标位姿定位方面优于Caffery方法和Caffery-Taylor (CT)方法。
1 UWB定位系统原理室内定位主要是确定人、物体等在空间中的位置和姿态^[14]。通过UWB信号测量的距离参量建立标签定位的数学模型，根据目标物体上多个定位点的位置信息解算目标物体的姿态信息。
1.1 标签定位的数学模型定位标签的定位模型建立在“当无距离误差时，以各个基站为球心，以测量距离为半径的各个球面一定有共同的交点”这一几何基础上。如果只有3个定位基站，3个球面可能会相交于关于基站平面对称两点，因此本文中使用不在同一平面的4个定位基站进行定位。设UWB定位基站的坐标为(x_i, y_i, z_i)(i=1, 2, 3, 4)，定位标签P_tag的坐标为(x_t, y_t, z_t)，定位基站i与P_tag之间的距离为d_i，则定位基站和定位标签应该满足：

$\left\{\begin{array}{l}\left(x_{1}-x_{\mathrm{t}}\right)^{2}+\left(y_{1}-y_{\mathrm{t}}\right)^{2}+\left(z_{1}-z_{\mathrm{t}}\right)^{2}=d_{1}^{2}, \\\left(x_{2}-x_{\mathrm{t}}\right)^{2}+\left(y_{2}-y_{\mathrm{t}}\right)^{2}+\left(z_{2}-z_{\mathrm{t}}\right)^{2}=d_{2}^{2}, \\\left(x_{3}-x_{\mathrm{t}}\right)^{2}+\left(y_{3}-y_{\mathrm{t}}\right)^{2}+\left(z_{3}-z_{\mathrm{t}}\right)^{2}=d_{3}^{2}, \\\left(x_{4}-x_{\mathrm{t}}\right)^{2}+\left(y_{4}-y_{\mathrm{t}}\right)^{2}+\left(z_{4}-z_{\mathrm{t}}\right)^{2}=d_{4}^{2} .\end{array}\right.$

(1)

式(1)是关于x_t、y_t和z_t的非线性方程组，直接求解并不容易，同时由于距离误差的存在导致各个球面不相交于一点，因此标签定位问题就由原来的非线性方程精确求解问题转换为非线性优化的最优估计问题。
1.2 位姿定位模型及解算方法基于目标物体上多个定位标签的位置信息，可以获得目标物体的位姿信息。具体来说，位姿定位通常由不同时刻目标上多组对应点之间的位置关系来解算，这类问题的常见方法主要包括三点法、四点法、N点法等^[15]。在解算位姿定位模型方面，Eggert等^[16]验证了奇异值分解(SVD)方法的优越性，因此本文采用了该方法。
刚体运动的位姿变换可由刚体上任意一点的平移运动和旋转运动表示。本文使用多个定位标签的中心点(x_center, y_center, z_center)的运动表示目标物体的位姿变换：

$\left\{\begin{array}{l}x_{\text {center }}=\frac{\sum\limits_{i}^{n} x_{\mathrm{t}, i}}{n}, \\y_{\text {center }}=\frac{\sum\limits_{i}^{n} y_{\mathrm{t}, i}}{n}, \\z_{\text {center }}=\frac{\sum\limits_{i}^{n} z_{\mathrm{t}, i}}{n} .\end{array}\right.$

(2)

$\left\{\begin{array}{l}{ }_{\mathrm{A}}^{\mathrm{B}} \boldsymbol{T}=\left[\begin{array}{cc}{ }_{\mathrm{A}}^{\mathrm{B}} \boldsymbol{R} & { }^{\mathrm{B}} \boldsymbol{P}_{\mathrm{A}, \text { org }} \\0 & 1\end{array}\right], \\{ }^{\mathrm{B}} \boldsymbol{P}={ }_{\mathrm{A}}^{\mathrm{B}} \boldsymbol{T} \cdot{ }^{\mathrm{A}} \boldsymbol{P} .\end{array}\right.$

(3)

其中：(x_{t, i}, y_{t, i}, z_{t, i})表示定位标签i的坐标，^AP为移动前定位标签坐标表示的3×1维向量，^BP表示移动后该点的向量，^BP_{A, org}是一个3×1维矩阵，表示移动前定位标签的中心点相对于移动后的坐标变化即平移变换；^B_AR为3×3维矩阵，表示移动前定位标签相对于移动后的旋转矩阵即姿态变换。
如果已知物体移动前后不少于3组的对应定位标签坐标，可使用SVD方法对式(3)求解，得出物体的姿态变换矩阵T，采用姿态变换参数的表示式如下：

$\boldsymbol{T}=\left[\begin{array}{cccc}\cos \gamma_{\cos \beta} & -\sin \gamma \cos \alpha+\cos \gamma \sin \beta \sin \alpha & \sin \gamma \sin \alpha+\cos \gamma \sin \beta \cos \alpha & t_{x} \\\sin \gamma_{\cos \beta} & \cos \gamma \cos \alpha+\sin \gamma \sin \beta \sin \alpha & -\cos \gamma \sin \alpha+\sin \gamma \sin \beta \cos \alpha & t_{y} \\-\sin \beta & \cos \beta \sin \alpha & \cos \beta \cos \alpha & t_{z} \\0 & 0 & 0 & 1\end{array}\right].$

(4)

其中：旋转变换参数α、β和γ分别表示横滚角、俯仰角和航向角，t_x、t_y和t_z表示平移变化参数。同样，根据已知的刚体变换参数可使用式(4)解得位姿变换矩阵。
物体上的某个定位标签在移动前和移动后2个状态下的位置坐标构成一组点对。根据3组点对即可使用SVD方法求出位姿变换矩阵，而且点对越多计算的结果越准确。为了提高位姿计算精度及增加冗余信息，本文中设置了4个定位标签。
2 标签定位解算方法1.1节已指出，由于UWB测距误差的存在，UWB对定位标签的定位问题转换成了非线性优化最优估计问题。在无线定位中，常用Caffery方法^[17]求解非线性方程和非线性优化最优估计问题。虽然Caffery方法计算过程简单，但是定位精度一般，面对此问题，符世琛^[18]提出将Caffery算法与Taylor级数展开法融合的CT定位法，通过循环迭代消除定位误差。而本文提出了基于冗余距离筛选的UWB定位优化方法，对Caffery方法计算的坐标进行优化。
2.1 Caffery方法Caffery方法将原来的非线性方程组转化为线性方程组，再利用最小二乘法来估计定位标签的位置。将式(1)中第n个方程减去第(n+1)个方程，得到关于定位标签P的坐标(x_t, y_t, z_t)的线性方程组：

$\left\{\begin{array}{l}\boldsymbol{A} \boldsymbol{X}=\boldsymbol{b}, \\\boldsymbol{A}=\left[\begin{array}{lll}x_{2}-x_{1} & y_{2}-y_{1} & z_{2}-z_{1} \\x_{3}-x_{2} & y_{3}-y_{2} & z_{3}-z_{2} \\x_{4}-x_{3} & y_{4}-y_{3} & z_{4}-z_{3}\end{array}\right], \\\boldsymbol{X}=\left[\begin{array}{l}x_{\mathrm{t}} \\y_{\mathrm{t}} \\z_{\mathrm{t}}\end{array}\right], \\\boldsymbol{b}=\frac{1}{2}\left[\begin{array}{l}x_{2}^{2}+y_{2}^{2}+z_{2}^{2}-x_{1}^{2}-y_{1}^{2}-z_{1}^{2}+d_{1}^{2}-d_{2}^{2} \\x_{3}^{2}+y_{3}^{2}+z_{3}^{2}-x_{2}^{2}-y_{2}^{2}-z_{2}^{2}+d_{2}^{2}-d_{3}^{2} \\x_{4}^{2}+y_{4}^{2}+z_{4}^{2}-x_{3}^{2}-y_{3}^{2}-z_{3}^{2}+d_{3}^{2}-d_{4}^{2}\end{array}\right].\end{array}\right.$

(5)

式(5)的最小二乘解为

$\boldsymbol{X}=\left(\boldsymbol{A}^{\mathrm{T}} \boldsymbol{A}\right)^{-1} \boldsymbol{A}^{\mathrm{T}} \boldsymbol{b}.$

(6)

2.2 Caffery-Taylor定位法Caffery方法可以直接计算得到P_tag的坐标初值(x_t, y_t, z_t)，但是其定位误差较大，因此在P_tag处使用Taylor级数将式(1)展开，通过循环迭代消除误差，其具体步骤如下：
1) 式(6)中Caffery得到的P_tag的坐标初值与真值(x_t0, y_t0, z_t0)之间的误差为(δ_x, δ_y, δ_z), 则有

$\left(x_{\mathrm{t} 0}, y_{\mathrm{t} 0}, z_{\mathrm{t} 0}\right)=\left(x_{\mathrm{t}}, y_{\mathrm{t}}, z_{\mathrm{t}}\right)+\left(\delta_{x}, \delta_{y}, \delta_{z}\right) .$

(7)

2) 将式(1)在P_tag处用Taylor级数展开，忽略二次及以上的项，则有

$\begin{aligned}\left\{\begin{array}{l}\boldsymbol{M}_{1} \boldsymbol{\delta}=\boldsymbol{M}_{2}, \\\boldsymbol{M}_{1}=\left[\begin{array}{lll}x_{\mathrm{t}}-x_{1} & y_{\mathrm{t}}-y_{1} & z_{\mathrm{t}}-z_{1} \\x_{\mathrm{t}}-x_{2} & y_{\mathrm{t}}-y_{2} & z_{\mathrm{t}}-z_{2} \\x_{\mathrm{t}}-x_{3} & y_{\mathrm{t}}-y_{3} & z_{\mathrm{t}}-z_{3} \\x_{\mathrm{t}}-x_{4} & y_{\mathrm{t}}-y_{4} & z_{\mathrm{t}}-z_{3}\end{array}\right], \\\boldsymbol{M}_{2}=\frac{1}{2}\left[\begin{array}{l}d_{1}^{2}-\left(x_{\mathrm{t}}-x_{1}\right)^{2}-\left(y_{\mathrm{t}}-y_{1}\right)^{2}+\left(z_{\mathrm{t}}-z_{1}\right)^{2} \\d_{2}^{2}-\left(x_{\mathrm{t}}-x_{2}\right)^{2}-\left(y_{\mathrm{t}}-y_{2}\right)^{2}+\left(z_{\mathrm{t}}-z_{2}\right)^{2} \\d_{3}^{2}-\left(x_{\mathrm{t}}-x_{3}\right)^{2}-\left(y_{\mathrm{t}}-y_{3}\right)^{2}+\left(z_{\mathrm{t}}-z_{3}\right)^{2} \\d_{4}^{2}-\left(x_{\mathrm{t}}-x_{4}\right)^{2}-\left(y_{\mathrm{t}}-y_{4}\right)^{2}+\left(z_{\mathrm{t}}-z_{4}\right)^{2}\end{array}\right], \\\boldsymbol{\delta}=\left[\begin{array}{ll}\delta_{x} & \delta_{y} & \delta_{z}\end{array}\right]^{\mathrm{T}} .\end{array}\right.\end{aligned}$

(8)

式(8)的最小二乘解为

$\boldsymbol{\delta}=\left(\boldsymbol{M}_{1}^{\mathrm{T}} \boldsymbol{M}_{1}\right)^{-1} \boldsymbol{M}_{1}^{\mathrm{T}} \boldsymbol{M}_{2}.$

(9)

3) 判断条件|δ_x |+|δ_y |+|δ_z |<ε是否成立，若不成立则用(x_t, y_t, z_t)+(δ_x, δ_y, δ_z)代替P_tag坐标的上一次估计值，转步骤2；若成立则停止迭代，得到最终的估计值(x_t, y_t, z_t)。
2.3 基于冗余距离筛选的UWB定位优化方法当测距准确时，定位标签坐标的3个参数只需要3个基站和定位标签距离即可求出，而实际上UWB测距存在误差，3个距离无法准确计算出定位标签坐标，因此添加另外的基站测量距离，同时使用多个定位标签两两之间的距离构成冗余测量系统，通过对误差较大的冗余距离筛选提高系统的定位精度。
2.2节提到的CT优化方法对所有测量数据等同看待，不能进行距离筛选，因此当某些距离误差较大时会失效。本文提出基于冗余距离筛选的UWB定位优化方法，在Caffery方法的基础上进行梯度下降(gradient descent)，简称为CGD。CGD方法以Caffery方法的坐标初值为迭代起点，以多个定位标签两两之间的距离d_ij(i, j=1, 2, 3, 4)为约束条件，通过坐标初值计算的距离与UWB距离r_ij之间的误差设置权重进行距离筛选，然后使用梯度下降法优化坐标初值。具体步骤如下：
1) 设置梯度下降法的代价函数：

$\begin{gathered}J=\sum\limits_{i,j} f\left(\boldsymbol{P}_{\text {tag, } i}, \boldsymbol{P}_{\text {tag, } j}, d_{i j}\right)+ \\\sum\limits_{i,j} l_{i j} f\left(\boldsymbol{P}_{\text {tag, } i}, \boldsymbol{P}_{\text {anchor, } j}, r_{i j}\right)+\mu \sum\limits_{i, j} \varphi\left(l_{i j}\right).\end{gathered}$

(10)

其中：P_{tag, i}和P_{tag, j}分别表示定位标签i和j坐标表示的空间向量；P_{anchor, j}表示定位基站j坐标表示的空间向量；f(P_x, P_y, d)=(‖P_x-P_y ‖²-d²)²表示距离误差；而代价函数中 $\sum\limits_{i, j} f\left(\boldsymbol{P}_{\mathrm{tag}, i}, \boldsymbol{P}_{\mathrm{tag}, j}, d_{i j}\right)$部分的作用是使用定位标签之间的几何距离参量对其进行约束，提高定位标签坐标的优化质量。l_ij表示距离的权重，初值为1；$\mu \sum\limits_{i, j} \varphi\left(l_{i j}\right)$ 用来平衡权重的变化，使权重尽可能接近1。

$\left\{\begin{array}{l}\varphi\left(l_{i j}\right)=\left(\sqrt{l_{i j}}-1\right)^{2}, \\\mu=\max \left|\left\|\boldsymbol{P}_{\text {tag. } i}-\boldsymbol{P}_{\text {anchor, } j}\right\|^{2}-r_{i j}^{2}\right| .\end{array}\right.$

(11)

2) 计算J对P_{tag, i}和l_ij的一阶偏导:

$\left\{\begin{array}{l}\frac{\partial J}{\partial \boldsymbol{P}_{\mathrm{tag}, i}}=\sum\limits_{i, j} f^{\prime}\left(\boldsymbol{P}_{\text {tag }, i}, \boldsymbol{P}_{\text {tag. }, j}, d_{i j}\right)+ \\\;\;\;\;\;\;\;\;\;\;\;\;\;\sum\limits_{i, j} l_{i j} f^{\prime}\left(\boldsymbol{P}_{\text {tag, } i}, \boldsymbol{P}_{\text {anchor, } j}, r_{i j}\right), \\\frac{\partial J}{\partial l_{i j}}=f\left(\boldsymbol{P}_{\text {tag. } i}, \boldsymbol{P}_{\text {anchor, } j}, r_{i j}\right)-\mu \frac{1-\sqrt{l_{i j}}}{\sqrt{l_{i j}}} .\end{array}\right.$

(12)

其中：f′(P_x, P_y, d)表示函数f(P_x, P_y, d)对P_x求一阶偏导，其结果为

$f^{\prime}\left(\boldsymbol{P}_{x}, \boldsymbol{P}_{y}, d\right)=4\left(\left\|\boldsymbol{P}_{x}-\boldsymbol{P}_{y}\right\|^{2}-d^{2}\right)\left(\boldsymbol{P}_{x}-\boldsymbol{P}_{y}\right)^{\mathrm{T}}.$

3) 通过条件 $\left|J_{k+1}-J_{k}\right|<\varepsilon=10^{-3}$ ( $k$ 表示迭代 的次数)判断 $J$ 是否收敛。若不收敛, 则用 $\left(\boldsymbol{P}_{\text {tag, } i}-\right.$ $\left.\delta \cdot \partial J / \partial \boldsymbol{P}_{\text {tag. } i}\right)$ 和 $\left(l_{i j}-\delta \cdot \partial J / \partial l\right)$ 代替上一次的定 位标签坐标值 $\boldsymbol{P}_{\text {tag. }} i$ 和权值 $l_{i j}$, 转步骤 2 , 其中的 $\delta=10^{-3}$ 表示每次迭代的步长。若收敛, 则停止迭 代, 得到优化后的定位标签坐标。
由于权值 $l_{i j}$ 用于距离筕选, 因此其取值范围为 $[0,1]$ 。为了保证步骤 3 中权值 $l_{i j}$ 在合理的范围 内, 首先判断 $\left(l_{i j}-\delta \cdot \partial J / \partial l\right)$ 是否为负, 如果为 负, 将 $\delta$ 折半缩小, 直到 $l_{i j} \in[0,1]$ 。
CGD方法在Caffery方法的基础上进行优化，因此CGD方法的优化结果与Caffery方法的坐标初值有关。
3 仿真实验系统系精度实验本章中设计仿真实验步骤，分别从定位精度和位姿精度两方面分析了3种方法的实验结果。
3.1 误差计算方法系统的定位精度用定位标签坐标计算值和真值之间的误差来衡量，本文中有多个定位标签，采用均方根误差(RMSE)来表示定位精度：

$\mathrm{RMSE}=\sqrt{\sum\limits_{i}^{n} \frac{\left\|\boldsymbol{P}_{\mathrm{tag}, i}^{\prime}-\boldsymbol{P}_{\mathrm{tag}, i}\right\|^{2}}{n}}.$

(13)

其中：P_{tag, i}和P′_{tag, i}分别表示定位标签i坐标向量的真值和计算值。
位姿精度由平移误差和旋转误差表示。平移误差为

$\Delta S=\left\|\boldsymbol{S}-\boldsymbol{S}^{\prime}\right\|.$

(14)

其中S和S′分别表示平移变换矩阵真值和计算值。
旋转误差为

$\Delta \boldsymbol{R}=\boldsymbol{R}^{-1} \boldsymbol{R}^{\prime}.$

(15)

其中R和R′分别表示旋转矩阵真值和计算值。取ΔR对应的旋转角度用于衡量旋转误差。
3.2 仿真实验方法及数据采集首先指定4个定位基站的坐标为P_i^anchor(i=1, 2, 3, 4)和初始状态下4个定位标签坐标真值为P_i^tag如表 1所示。根据表 2用式(2)、(3)、(4)计算出移动后定位标签坐标真值集合{Q_i^tag}。UWB测量的距离真值由集合{P_i^tag}和{Q_i^tag}使用距离公式计算得出。本文用基于飞行时间的双向TW-TOF(two way-time of flight)^[19]测距方法，符世琛等^[18]指出该方法的测距误差满足均值为0、标准差为0.02 m的正态分布。但是此结论的实验中，只有天线相位中心处于同一高度的一对定位基站和定位标签，多径效应的干扰较小。而本实验使用多个定位基站和定位标签，无法保证同样的实验条件，同时由于室内空间中存在遮挡物，多径效应的干扰较大，因此仿真实验设置UWB的测距误差δ满足均值为0、标准差为0.1 m的正态分布。
表 1 初始状态下定位标签和定位基站的坐标

编号	定位基站坐标/m	定位标签坐标/m
1	(0, 0, 0)	(1, 0, 0)
2	(3, 0, 0)	(1, 3, 0)
3	(0, 3, 0)	(5, 0, 0)
4	(0, 0, 3)	(5, 3, 2)

表选项






移动前后的距离真值分别加上δ，模拟初始状态下UWB测量距离r_ij⁰和移动后的UWB测量距离r_ij¹。每组仿真实验皆做1 000次的重复实验。
表 2给出了物体移动的平移参数和旋转参数，其中的旋转参数包括横滚角、俯仰角和航向角。符世琛等^[18]指出，定位标签与定位基站之间的距离越远则Caffery方法和CT方法的定位误差越大，因此设置的3组实验中定位标签与定位基站在X轴方向的距离依次增大，旋转角度保持一致。
表 2 三组实验的移动参数

实验	平移参数/m	旋转参数/(°)
1	(5, 2, 1)	(30, 45, 30)
2	(10, 2, 1)	(30, 45, 30)
3	(20, 2, 1)	(30, 45, 30)

表选项






将定位基站的坐标和UWB测量的距离输入Caffery方法，计算出2个状态下定位标签坐标初值集合{P_i^init}和{Q_i^init}。然后将初始状态下的r_ij⁰和{P_i^init}及移动后状态下的r_ij¹和{Q_i^init}分别输入到CT方法中，计算出优化后的定位标签坐标。类似计算得到CGD方法优化后的定位标签坐标。最后根据定位标签坐标代入计算定位误差和位姿误差。
3.3 定位标签距离参量约束实验2.3节介绍的CGD方法使用了多个定位标签之间的距离为约束，提高定位标签坐标的优化质量。本节以定位误差描述实验结果，验证了定位标签之间的距离约束对CGD方法的影响。定位误差结果如图 1所示，1 000次重复实验的定位误差均值如表 3所示，其中CGD-NoReg表示在CGD方法中不使用距离约束。

图 1 定位标签距离参量约束实验结果

图选项

表 3 重复实验的误差均值?

m
实验	CGD-NoReg	CGD
1	0.261	0.179
2	0.396	0.256
3	0.689	0.441

表选项






根据实验结果可知，使用定位标签之间的距离参量作为约束条件，能够明显提高CGD方法的优化质量，挺高定位精度。
3.4 定位精度实验结果3种方法计算的系统定位误差柱状图如图 2所示，1 000次重复实验的定位误差均值如表 4所示。

图 2 仿真实验的定位精度结果

图选项

表 4 1 000次重复实验定位误差均值?

m
实验	Caffery	CT	CGD
1	0.377	0.255	0.174
2	0.569	0.386	0.255
3	0.986	0.680	0.456

表选项






根据图 2和表 4可知，在每组实验中，CGD方法的定位误差最小，Caffery方法最大，说明CGD方法的稳定性最高。
从实验1到3系统的定位误差不断增大，说明增大定位基站与定位标签在X轴方向的距离会增大定位误差。表 3中，CGD方法3组实验的定位误差均值均皆不超过CT方法的70%。
3.5 位姿精度实验结果3组实验的位姿精度如图 3所示。重复实验的平移误差均值如表 5所示，旋转误差均值如表 6所示。

图 3 仿真实验位姿误差

图选项

表 5 平移误差均值?

m
实验	Caffery	CT	CGD
1	0.436	0.296	0.241
2	0.684	0.478	0.393
3	1.187	0.851	0.717

表选项






表 6 旋转误差均值?

(°)
实验	Caffery	CT	CGD
1	9.073	7.368	6.434
2	13.587	10.501	8.467
3	21.920	16.939	12.601

表选项






结果表明，从实验1到3位姿误差的均值与标准差不断变大，说明增大定位基站与定位标签在X轴方向的距离会增大系统的位姿误差，降低系统的稳定性。
在3组实验中，CGD方法位姿误差的均值与标准差皆是最小，说明使用CGD方法能够明显提高解算位姿精度和稳定性。
上述的仿真实验结果表明，CGD方法的冗余距离筛选比较成功，在定位解算与位姿解算精度方面皆优与CT方法。CGD方法的冗余距离筛选是通过距离权重实现的，由式(12)可知，权值l_ij与距离误差f(P_{tag, i}, P_{anchor, j}, r_ij)有关，误差越大权值越小。图 4为CGD方法迭代结束后，权值l_ij与距离误差f(P_{tag, i}, P_{anchor, j}, r_ij)关系的散点图。

图 4 距离误差和权值大小的关系散点图

图选项

由图 4可知，距离误差越大权值越小，体现了CGD方法对距离的筛选作用。由于实验1到3的定位误差越来越大，因此距离误差也越来越大。
4 真实环境实验4.1 实验环境及数据采集UWB定位真实环境如图 5所示，在4个三脚架上分别安装定位基站，保持三脚架的位置及高度不变。图 6中显示4个定位标签分别固定在小车的4根长杆上，车体形状、长杆固定位置和定位标签固定位置保持不变，以此保证移动车体时定位标签的运动是刚性的。

图 5 UWB工作的走廊环境

图选项

图 6 定位标签的固定方式

图选项

采集数据时，调节好全站仪后将其固定。使用全站仪采集4个定位基站和4个定位标签的三维坐标作为坐标真值；根据坐标真值计算出定位基站到定位标签的距离d_{i, j}⁰作为距离真值；记录此时UWB测量的距离数据r_ij⁰。然后移动车体，同样的方法记录移动后的距离真值d_{i, j}¹和UWB测量距离r_ij¹。
4个定位基站的位置固定，每组实验只需采集一次数据即可。本实验为静态实验，因此在移动前后两个状态下各采集500组以上的距离数据取均值作为UWB信号测量的距离数据。
表 7为UWB信号测量的距离相对于全站仪测量距离的误差。表中每行数据表示4个定位基站P_i^anchor(i=1, 2, 3, 4)分别到某个定位标签之间的距离误差。
表 7 UWB测距误差?

m
实验	定位标签	移动前测距误差	移动后测距误差
1	1	－0.038, 0.005, 0.158, －0.198	－0.087, －0.002, 0.075, 0.038
	2	0.035, 0.053, 0.096, 0.114	0.099, －0.003, 0.152, 0.114
	3	0.157, 0.051, 0.097, 0.181	－0.054, 0.121, －0.303, －0.100
	4	0.060, －0.068, 0.181, －0.025	0.298, －0.017, 0.216, 0.046
2	1	－0.053, －0.017, 0.123, 0.016	－0.085, 0.010, 0.103, 0.088
	2	0.003, 0.040, 0.070, －0.048	0.117, 0.016, 0.154, 0.080
	3	0.120, 0.052, 0.078, 0.046	－0.083, 0.135, 0.158, 0.364
	4	－0.009, 0.075, 0.087, 0.100	－0.039, －0.096, 0.094, 0.079
3	1	－0.035, －0.023, 0.122, －0.048	－0.091, 0.027, 0.052, －0.038
	2	0.026, 0.039, 0.074, －0.042	0.091, －0.005, 0.144, －0.044
	3	0.119, 0.019, 0.074, 0.035	－0.076, 0.080, 0.170, 0.012
	4	－0.009, 0.075, 0.087, 0.141	－0.044, －0.119, 0.105, －0.043

表选项






3.2节中指出，本文实验的UWB测距误差会大于文[18]中的数据。表 7中的UWB的测距误差均值和标准差明显大于文[18]中的数据，说明多径效应能明显增大UWB测距误差，验证了3.2节的结论。同时表 7中存在个别的测距误差较大，说明多径效应容易导致UWB测距精度不稳定。
4.2 真实数据定位精度结果计算每组实验两个状态的定位误差，共有3组实验6组定位误差结果如表 8所示，位姿误差结果如表 9所示。
表 8 真实数据定位精度RMSE ?

m
实验	状态	Caffery	CT	CGD
1	移动前	1.400	1.114	0.152
	移动后	1.569	0.484	0.178
2	移动前	0.428	0.466	0.186
	移动后	0.591	0.546	0.458
3	移动前	0.276	0.195	0.130
	移动后	0.571	0.416	0.359

表选项






表 9 真实数据位姿精度

实验	误差	Caffery	CT	CGD
1	平移误差/m	0.511	2.462	0.225
	旋转误差/(°)	14.938	21.989	5.384
2	平移误差/m	2.475	2.719	1.221
	旋转误差/(°)	38.165	33.071	13.503
3	平移误差/m	2.425	1.489	0.938
	旋转误差/(°)	29.431	16.121	11.026

表选项






在3组实验中，CGD方法都有明显的优化效果，其定位误差和位姿误差皆最小。而CT方法对实验2定位精度优化失败，对实验1和2的位姿误差优化失败，说明CT方法的鲁棒性较差。
与CT方法相比，CGD方法无论是优化效果还是鲁棒性都有明显的提升。
5 结论在多径效应等随机误差的干扰较大时，UWB测距精度不稳定，本文提出了提出基于冗余距离筛选的定位优化方法，以多个定位标签的间距为约束条件，根据Caffery方法坐标初值计算的距离与UWB测量的距离之间的误差设置权重，对冗余距离进行筛选。该方法能够明显提高Caffery方法的定位精度和位姿精度，提高系统的稳定性。该方法的优化效果和稳定性明显好于CT方法，仿真实验中其定位误差仅为CT方法的70%。

参考文献

[1]	HASHEM O, HARRAS K A, YOUSSEF M. Accurate indoor positioning using IEEE 802.11mc round trip time[J]. Pervasive and Mobile Computing, 2021, 75: 101416. DOI:10.1016/j.pmcj.2021.101416
[2]	SADHUKHAN P, GAIN S, DAHAL K, et al. An efficient clustering with robust outlier mitigation for Wi-Fi fingerprint based indoor positioning[J]. Applied Soft Computing, 2021, 109: 107549. DOI:10.1016/j.asoc.2021.107549
[3]	陈国良, 张言哲, 汪云甲, 等. WiFi-PDR室内组合定位的无迹卡尔曼滤波算法[J]. 测绘学报, 2015, 44(12): 1314-1321. CHEN G L, ZHANG Y Z, WANG Y J, et al. Unscented Kalman filter algorithm for WiFi-PDR integrated indoor positioning[J]. Acta Geodaetica et Cartographica Sinica, 2015, 44(12): 1314-321. DOI:10.11947/j.AGCS.2015.20140691 (in Chinese)
[4]	席瑞, 李玉军, 侯孟书. 室内定位方法综述[J]. 计算机科学, 2016, 43(4): 1-6, 32. XI R, LI Y J, HOU M S. Survey on indoor localization[J]. Computer Science, 2016, 43(4): 1-6, 32. (in Chinese)
[5]	闫大禹, 宋伟, 王旭丹, 等. 国内室内定位技术发展现状综述[J]. 导航定位学报, 2019, 7(4): 5-12. YAN D Y, SONG W, WANG X D, et al. Review of development status of indoor location technology in China[J]. Journal of Navigation and Positioning, 2019, 7(4): 5-12. DOI:10.3969/j.issn.2095-4999.2019.04.002 (in Chinese)
[6]	卢靖宇, 余文涛, 赵新, 等. 基于超宽带的移动机器人室内定位系统设计[J]. 电子技术应用, 2017, 43(5): 25-28. LU J Y, YU W T, ZHAO X, et al. Design of indoor positioning system for mobile robot based on ultra-wideband[J]. Application of Electronic Technique, 2017, 43(5): 25-28. (in Chinese)
[7]	黄梦雨, 秦建军, 高磊, 等. 基于UWB的零转径跟随机器人控制系统设计[J]. 重庆理工大学学报(自然科学), 2019, 33(9): 142-150. HUANG M Y, QIN J J, GAO L, et al. Design of zero turning radius following robot control system based on UWB[J]. Journal of Chongqing University of Technology(Natural Science), 2019, 33(9): 142-150. (in Chinese)
[8]	李浩博, 王坚, 王川阳. 超宽带室内动态定位精度影响探究[J]. 导航定位学报, 2018, 6(1): 45-48. LI H B, WANG J, WANG C Y. Discussion on affection factors of UWB indoor kinematic positioning[J]. Journal of Navigation and Positioning, 2018, 6(1): 45-48. (in Chinese)
[9]	吴绍华, 张乃通. 基于UWB的无线传感器网络中的两步TOA估计法[J]. 软件学报, 2007, 18(5): 1164-1172. WU S H, ZHANG N T. A two-step TOA estimation method for UWB based wireless sensor networks[J]. Journal of Software, 2007, 18(5): 1164-1172. (in Chinese)
[10]	BABA A I, WU F, AHMED T. A naive time of flight ranging scheme for wireless sensor networks[J]. International Journal of Communication Networks and Distributed Systems, 2020, 25(4): 347-365.
[11]	ZHANG R, H?FLINGER F, REINDL L. TDOA-Based localization using interacting multiple model estimator and ultrasonic transmitter/receiver[J]. IEEE Transactions on Instrumentation and Measurement, 2013, 62(8): 2205-2214.
[12]	王川阳, 王坚, 宁一鹏, 等. 超宽带定位的降噪方法研究[J]. 测绘科学, 2019, 44(4): 175-181. WANG C Y, WANG J, NING Y P, et al. Study of noise reduction method for ultra wideband positioning[J]. Science of Surveying and Mapping, 2019, 44(4): 175-181. (in Chinese)
[13]	YIN H H, XIA W W, ZHANG Y Y, et al. UWB-based Indoor high precision localization system with robust unscented Kalman filter [C]// 2016 IEEE International Conference on Communication Systems (iccs). Shenzhen, China: IEEE Press, 2016: 1-6.
[14]	刘公绪, 史凌峰. 室内导航与定位技术发展综述[J]. 导航定位学报, 2018, 6(2): 7-14. LIU G X, SHI L F. An overview about development of indoor navigation and positioning technology[J]. Journal of Navigation and Positioning, 2018, 6(2): 7-14. (in Chinese)
[15]	赵连军. 基于目标特征的单目视觉位置姿态测量技术研究[D]. 成都: 中国科学院大学(中国科学院光电技术研究所), 2014. ZHAO L J. Research on mono-vision pose measurement based on features of target [D]. Chengdu: University of Chinese Academy of Sciences (Institute of Optics and Electronics, Chinese Academy of Sciences), 2014. (in Chinese)
[16]	EGGERT D W, LORUSSO A, FISHER R B. Estimating 3-D rigid body transformations: A comparison of four major algorithms[J]. Machine Vision and Applications, 1997, 9(5-6): 272-290.
[17]	CAFFERY J J. A new approach to the geometry of TOA location [C]// IEEE Vehicular Technology Conference Fall 2000. Boston, MA, USA: IEEE Press, 2000: 1943-1949.
[18]	符世琛. 基于UWB测距的悬臂式掘进机位姿检测方法研究[D]. 北京: 中国矿业大学(北京), 2018. FU S S. Research on pose detection method of boom-type roadheader based on UWB distance measurement [D]. Beijing: China University of Mining and Technology (Beijing), 2018. (in Chinese)
[19]	符世琛, 李一鸣, 杨健健, 等. 基于超宽带技术的掘进机自主定位定向方法研究[J]. 煤炭学报, 2015, 40(11): 2603-2610. FU S S, LI Y M, YANG J J, et al. Research on autonomous positioning and orientation method of roadheader based on Ultra Wide-Band technology[J]. Journal of China Coal Society, 2015, 40(11): 2603-2610. (in Chinese)