近些年，随着无人驾驶、机器人、物联网等技术的发展，室内导航定位技术成为了研究热点。相比在开阔的室外空间定位，在狭小的室内空间中定位环境复杂，对定位精度要求高，国内外许多学者和研究机构对其进行了大量的研究^[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的正态分布。

移动前后的距离真值分别加上δ，模拟初始状态下UWB测量距离r_ij⁰和移动后的UWB测量距离r_ij¹。每组仿真实验皆做1 000次的重复实验。

表 2给出了物体移动的平移参数和旋转参数，其中的旋转参数包括横滚角、俯仰角和航向角。符世琛等^[18]指出，定位标签与定位基站之间的距离越远则Caffery方法和CT方法的定位误差越大，因此设置的3组实验中定位标签与定位基站在X轴方向的距离依次增大，旋转角度保持一致。

将定位基站的坐标和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方法中不使用距离约束。

根据实验结果可知，使用定位标签之间的距离参量作为约束条件，能够明显提高CGD方法的优化质量，挺高定位精度。

3.4 定位精度实验结果

3种方法计算的系统定位误差柱状图如图 2所示，1 000次重复实验的定位误差均值如表 4所示。

根据图 2和表 4可知，在每组实验中，CGD方法的定位误差最小，Caffery方法最大，说明CGD方法的稳定性最高。

从实验1到3系统的定位误差不断增大，说明增大定位基站与定位标签在X轴方向的距离会增大定位误差。表 3中，CGD方法3组实验的定位误差均值均皆不超过CT方法的70%。

3.5 位姿精度实验结果

3组实验的位姿精度如图 3所示。重复实验的平移误差均值如表 5所示，旋转误差均值如表 6所示。

结果表明，从实验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可知，距离误差越大权值越小，体现了CGD方法对距离的筛选作用。由于实验1到3的定位误差越来越大，因此距离误差也越来越大。

4 真实环境实验 4.1 实验环境及数据采集

UWB定位真实环境如图 5所示，在4个三脚架上分别安装定位基站，保持三脚架的位置及高度不变。图 6中显示4个定位标签分别固定在小车的4根长杆上，车体形状、长杆固定位置和定位标签固定位置保持不变，以此保证移动车体时定位标签的运动是刚性的。

采集数据时，调节好全站仪后将其固定。使用全站仪采集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)分别到某个定位标签之间的距离误差。

3.2节中指出，本文实验的UWB测距误差会大于文[18]中的数据。表 7中的UWB的测距误差均值和标准差明显大于文[18]中的数据，说明多径效应能明显增大UWB测距误差，验证了3.2节的结论。同时表 7中存在个别的测距误差较大，说明多径效应容易导致UWB测距精度不稳定。

4.2 真实数据定位精度结果

计算每组实验两个状态的定位误差，共有3组实验6组定位误差结果如表 8所示，位姿误差结果如表 9所示。

在3组实验中，CGD方法都有明显的优化效果，其定位误差和位姿误差皆最小。而CT方法对实验2定位精度优化失败，对实验1和2的位姿误差优化失败，说明CT方法的鲁棒性较差。

与CT方法相比，CGD方法无论是优化效果还是鲁棒性都有明显的提升。

5 结论

在多径效应等随机误差的干扰较大时，UWB测距精度不稳定，本文提出了提出基于冗余距离筛选的定位优化方法，以多个定位标签的间距为约束条件，根据Caffery方法坐标初值计算的距离与UWB测量的距离之间的误差设置权重，对冗余距离进行筛选。该方法能够明显提高Caffery方法的定位精度和位姿精度，提高系统的稳定性。该方法的优化效果和稳定性明显好于CT方法，仿真实验中其定位误差仅为CT方法的70%。