首先构建基于UWB与IMU融合的室内定位系统总体分布结构，介绍基于IMU与UWB的SA-ESKF定位算法。本文设计的室内融合定位算法实验的基站布置方式如图 1a所示，UWB传感器总体由1个未知坐标的标签与4个已知坐标的基站组成，4个基站以矩形方式布置，UWB标签与IMU固定在实验小车上，其中，IMU传感器由1个三轴加速度计与1个三轴陀螺仪组成。

算法融合框架如图 1b所示。UWB的原始数据为飞行时间与标签坐标，IMU的原始数据为三轴加速度与角速度，分别对其进行本地预处理和计算：IMU的数据经过处理后得到误差项; UWB的数据在进行非视距处理和伪距解算后得到更新后伪距; 最后将处理后的数据经SA-ESKF算法得到高精度三维定位信息。

无线信号测距技术主要划分为基于距离以及非基于距离2种类型。本文所采用的UWB定位方式隶属于基于飞行时间的测距范畴，该技术的实施过程包含2个阶段：在第1阶段，借助飞行时间来估算标签与基站之间的距离信息以及角度数据; 而在第2阶段，则依据估算信息，确定标签在UWB坐标系中的具体位置坐标。本文采用的LinkTrack UWB测距方式是依据UWB所发出电磁波的飞行时间来求解基站与标签的距离，即双边测距技术。双边测距技术的原理是由4个基站依次交替地向标签TAG发送并接收电磁波，同时记录电磁波往返的飞行时间，以此来测定两点之间的双向距离，其具体流程如图 2所示。

在双边测距^[24]中，T_pro表示电磁波在基站与标签之间的飞行时间，将基站表示为设备A，标签表示为设备B。从设备A开始，T_round1表示从设备A发送查询消息到接收到设备B回复消息的时间，T_round2表示从设备B发送响应消息到接收到设备A最终消息的时间。同样，T_reply1与T_reply2表示设备A与设备B接收和发送消息的时间延迟，其关系为：

因此，根据上式得到电磁波在基站与标签之间的飞行时间T_pro为：

用C表示电磁波在空气中的传播速度，则基站与标签之间的距离d可表示为：

上述为单一基站与标签之间的测距方式，完成距离测量后，使用最小二乘法优化定位标签的坐标。UWB三维定位中三边定位原理如图 3所示，其中包含3个基站与1个标签。显然，在三维测量中，以3个基站坐标为圆心，以3个基站到标签的距离为半径作球形，则标签应在3个球形的交点处，如果3个基站不在一条直线上，则球形解算结果唯一。假设未知定位标签在UWB坐标系下的坐标为P=(x，y，z)^T，3个基站坐标分别为(x_i，y_i，z_i)，则三边定位方程可表示为：

解得定位标签与第i个基站之间的解算距离d_i为：

当遇到多层停车场或基站数量增多时，假设基站增加至n个，则表示为：

将上述公式进行矩阵变换，可得到：

式中：

因此，在UWB坐标系下使用最小二乘法解算的标签坐标 p^u为：

在UWB与IMU融合定位时，由于IMU硬件输出的数据不在导航坐标系下，需要根据二者之间的坐标转换关系，进行不同坐标系之间的相互转换和姿态矩阵转换。进行融合定位时，为减少计算量，只考虑车体坐标系相对于导航坐标系的旋转，将导航坐标系与UWB坐标系重合，选择的坐标系分别为车体坐标系和导航坐标系。根据捷联惯性导航系统(SINS)^[25]中相关知识理论，对不同坐标系之间的变换关系和姿态矩阵计算作简单描述。

现设车体坐标系的原点与车辆质心重合，X_b轴与Y_b轴分别指向车辆的正前方与右侧，Z_b轴与车辆所在水平面垂直，与前两轴形成右手坐标系OX_bY_bZ_b。导航坐标系由UWB硬件布置方式确定。UWB基站编号分别为A₀、A₁、A₂、…、A_n，基站所在平面为UWB坐标系的XY平面，其中，基站A₀、A₃所在直线为UWB坐标系的X_u轴，基站A₀、A₁所在直线为UWB坐标系的Y_u轴，Z_u轴垂直于前二者构成的平面。IMU的加速度计与陀螺仪输出的数据属于车体坐标系，姿态矩阵为2个坐标系之间的坐标变换矩阵。

为避免在姿态变换时，由于俯仰角接近±90°产生的“万向锁”问题，SA-ESKF融合算法采用四元数法对车辆的位姿进行更新。四元数是四维空间中的旋转表示，在位姿更新计算中，其具有计算速度快、能减少角度计算引入的误差、减少航向角与翻滚角信息丢失等多项优点。如图 4所示，在导航坐标系中，使用φ表示车辆航向角，θ表示俯仰角，γ表示翻滚角，以上3个参数为导航坐标系与车体坐标系间的欧拉角集合。将其绕[1, 0, 0]、 [0, 1, 0]与[0, 0, 1] 旋转3次，角度分别为φ、θ与γ，即得到相对应的四元数。根据四元数乘法，将四元数与旋转后的四元数相乘，得到新的四元数表示矩阵 Q：

为简化表达，假设旋转矩阵C₂^b、C₁²和C_u¹分别表示欧拉角的3个旋转矩阵，则从车身坐标系到导航坐标系的变换矩阵C_u^b表示为：

C_u^b的详细表达式为：

在该系统中，由于2个坐标系之间的变换矩阵为正交矩阵，根据正交矩阵的性质有：

假设ω_ub^b为车体坐标系下，车体坐标系相对于导航坐标系的三轴旋转角速度，则有：

车辆上搭载的IMU所给出的数据为 ω^b=[ω_x  ω_y  ω_z]^T和a^b=[a_x^b  a_y^b  a_z^b]^T，ω^b和 a^b分别表示在车体坐标系下的角速度和加速度。Q_b^u表示由车体坐标系向UWB坐标系的转换矩阵，Q_u^b表示UWB坐标系向车体坐标系的转换矩阵，二者都为反对称矩阵，且有：

现对上述变量实施以下变换，则有：

将上述相关矩阵代入式(15)，求出详细表达式后并将矩阵右下角三阶矩阵分块，则可以简记为：

假设$ \boldsymbol{\dot{Q}}_b^u$表示四元数的微分结果，k+1时刻与k时刻间隔时间为Δt，则四元数更新方程为：

现将IMU在车体坐标系下输出的三轴角速度 ω^b转换到导航坐标系下，转换公式为：

为得到导航坐标系下三轴加速度，需要将车体坐标系下的加速度值a_z^b去除重力加速度g，对IMU输出的加速度测量值做以下处理：

同理，设a^u=[a_x^u，a_y^u，a_z^u]^T为导航坐标系下的三轴加速度测量值，则有：

由于IMU硬件的采样时间短，可将车辆在三轴方向上的运动均视为匀速直线运动。在导航坐标系中，假设Δv^u表示速度增量，v^u表示车辆的速度，Δx^u表示位移增量，x^u表示车辆的位移，根据加速度、速度与位移之间的关系可得：

在传统的车辆UWB与IMU融合定位模式中，多数情况下仅对平面二维坐标进行优化。然而，在多层室内停车场中，UWB标签通常以多组形式呈现，并且分布于每一层，导致车辆难以准确获取高度信息以及所在楼层信息。对于低成本的UWB定位模块而言，当UWB定位模块的标签所在平面与基站所在平面高度差较小时，Z轴的定位精度欠佳，误差约为0.3 m。不过，车辆在室内停车场行驶时，往复变速运动较多且速度较慢，2个坐标定位点之间的距离不大。同时，惯性测量单元在短时间内定位精度高，数据可靠。基于此，在忽略小车姿态误差的前提下，提出采用IMU短时位移积分与模拟退火(SA)算法^[26]来优化误差状态卡尔曼滤波(ESKF)先验估计值。

在室内停车场中，设UWB基站所在平面与地面垂直高度为z，车载标签到地面的垂直高度为z₁，当车辆进入室内停车场，标签与基站建立联系后，可得到初始状态的精确高度信息，即：z₀=z-z₁。车辆进入室内停车场UWB定位区域时，车速较低，此时由最小二乘法解算的UWB坐标系的X轴与Y轴坐标精度高，可作为起始坐标信息，加入初始高度信息，则有初始三维坐标点为：p₀=[x₀  y₀  z₀]^T。

经过IMU算法处理后可得到误差状态测量值，则算法解算的车辆从k时刻到k+1时刻的三轴位移增量Δp_k|k+1为：

设SA算法的初始坐标为 p_k|k+1，使用上述位移增量与SA算法的初始坐标可得到k+1时刻更为精确的先验估计理论坐标点p_k|k+1^s：

设置迭代次数为M，初始温度为F₀，冷却速度为α，第M次迭代更新后的坐标点为 p_M。首先定义目标函数f(err)，采用三维欧式距离差值作为指标，则第M次更新点与理论坐标点的三轴位移误差Δp^sT与目标函数值为：

式中：i为三维坐标系中第i维的坐标值。

上述目标函数f(err)在理论上应越小越好，且微小变化极易对定位结果造成较大的波动。为了防止退火时衰减率过快，跳过算法最优解，引入迭代中历史最优解best，将每次符合Metropolis抽样准则结果对应的坐标点与三轴误差值进行储存，具体步骤如图 5所示。

在SA过程中，综合考虑能量消耗、算法计算时间与收敛快慢等问题，温度控制计算式为：

为了保证退火时算法搜索区间尽可能广，α取值接近1，一般为0.9~0.95。得到第M次迭代更新的 p_M后，根据新产生的误差和Metropolis抽样准则考虑是否接受新解，准则如下：

若上式成立，则将此时f(err)对应的三轴坐标p_M与Δp^sT存入best中。反之，则停止迭代，且输出当前SA算法迭代后best中存储的坐标值p_M^best和退火后三轴位移误差值Δp^sT。

ESKF中使用十五维误差进行定位优化，现引入速度误差Δv^T、姿态误差Δθ^T、陀螺仪零偏误差Δω_b^T、加速度计零偏误差Δa_b^T，则SA更新后名义状态坐标估计值 p_k^s与十五维误差状态Δx^s为：

在定位研究中，单传感器的定位精度低，定位误差较大，甚至会产生漂移现象，因此需要引入多传感器融合定位。SA-ESKF^[26]是针对卡尔曼滤波器的一种改进，无需使用线性化的高斯过度方法来适应非线性系统，它能更准确地逼近非线性系统，可以更好地处理高斯分布的状态估计，避免了数值稳定性和计算复杂性等相关问题，相较于单传感器定位与传统卡尔曼滤波器融合多传感器定位具有更好的鲁棒性和精度。

由图 6可知，ESKF算法涉及的3个状态量分别为当前状态、名义状态和误差状态，其中，输入滤波器的误差状态求解方式是最重要的环节。首先使用独立的前置算法预处理回传的UWB与IMU数据，并使用IMU积分法得到预测状态误差与名义状态，预测状态误差融合更新后的伪距得到误差状态; 将误差状态与上一帧先验估计值融合并进行模拟退火过程，再将最优解best对应的退火后误差状态与名义状态一同输入滤波器进行算法更新，得到更新后的误差状态信息。通过对名义状态与所求解的误差状态进行融合，获得当前状态。

为迅速确定车辆起始位置，通过计算车载标签在无噪理想环境中的运动倾向，得到系统的名义状态。基于IMU运动方程，利用IMU积分法得到的预测状态误差与更新后伪距信息进行更新。根据实际使用场景，设定噪声不为零，选取位置 p、速度 v以及单位四元数 Q作为名义状态。

考虑IMU测量噪声影响，为提高定位精度，需要使用SA误差状态Δx^s对名义值进行更新。设加速度名义值与角速度名义值分别为a_j和 ω_j，可得IMU的误差方程为：

式中：ω_ie^u=[0  ω_e cos L  ω_e sin L]^T为地球角速度分量。

根据上述退火后误差状态与误差方程，计算k时刻经过SA后的后验状态矩阵估计值 X_k+1|k^s，滤波器中误差状态协方差方程为：

式中：X_k|k^s为k时刻经过SA后的后验状态矩阵估计值; B_k为输入状态转换矩阵; I_k+1为输入矩阵; F_k为状态转移矩阵; W为环境噪声，符合正态分布; A为描述过程噪声协方差矩阵的参数。

为描述系统状态的不确定性以及动态变化过程中的噪声影响，设计过程噪声协方差矩阵 N为：

式中：δN_a为加速度计三轴测量噪声对角矩阵的平方; δN_ga为陀螺仪三轴测量噪声对角矩阵的平方; δN_b为加速度计偏置对角矩阵的平方; δN_gb为对角矩阵与对角矩阵相乘的陀螺仪偏置矩阵的平方。

为了优化滤波器的性能，使估计结果更加准确可靠，平衡系统预测模型与实际观测之间的关系，设计噪声增益矩阵 G为：

根据上式可得过程噪声协方差矩阵N_A为：

建立系统中UWB的观测方程，减弱原始数据中噪声带来的误差，增加定位精度，设UWB的观测方程为：

式中：Z_k为观测数据; h(x)为观测函数; μ(x)为观测噪声，其符合高斯分布。

为增加解算精度，引入观测函数的雅可比矩阵H，对误差状态 $ \partial x$进行求导，雅可比矩阵 H为：

式中：$\frac{\partial h}{\partial x} $为观测函数h(x)在其观测点附近的线性展开。

根据传统卡尔曼滤波算法，求解卡尔曼增益 K_k+1、后验误差协方差矩阵 P_k+1|k+1和误差状态值 X_k+1|k+1的更新，三者的更新方程为：

式中：K_k+1为系统在k+1时刻的卡尔曼增益; P_k+1|k为k+1时刻的先验估计协方差矩阵; H为状态转移矩阵; P_k+1|k+1为k+1时刻后验协方差矩阵; Z_k为观测矩阵; X_k+1|k+1为k+1时刻的后验估计状态矩阵。

在得到上述误差状态后，将X_k+1|k+1中的Δp_k+1|k+1与上一时刻SA后的名义状态 p_k^s融合，得到标签此时的融合定位位置：

为综合评价本文提出的SA-ESKF融合定位算法的性能，开展静态定位与动态定位实验。实验场地为一地下室内停车场，如图 7所示。采用图 1a所示的设计形式布置实验场地，基站位于矩形4个顶点上，并固定三脚架的高度与位置，测试场地总面积约为120 m²。为模拟真实地下车库环境且保证实验真实可靠，实验场地不封闭，实验过程中有车辆驶入或驶离该车库。

遥控实验小车与硬件布置方式如图 8所示，UWB模块与IMU模块固定于移动小车顶端。当车辆运动时，UWB模块、IMU模块与小车被视为一个整体，模块与车辆之间不会产生滑移现象。由图 8可以看出，将IMU模块与UWB模块安装在车辆上后，由于这2个模块体积较小，不会对小车的正常行驶以及驾驶员的视野造成安全影响。并且，大多数车辆自身携带IMU模块，加入UWB模块后与车辆上IMU融合定位，可提高传感器信息的利用率，一定程度上提升了IMU传感器的经济效益。

数据采集模块为深圳空循环Nooploop科技有限公司的LinkTrack模块，采样频率为4~6.5 GHz，带宽为499.2 Hz，在120 m²内定位精度可靠，在视距良好且空旷的地区其定位精度高达0.1 m。该设备的缺点是在标签距离基站平面较近时，Z轴方向精度较低且波动较大。实验所用IMU为维特智能WTGAHRS3-485模块，该模块带宽为5~256 Hz，加速度计零漂为±80 mg，陀螺仪静置零漂为±0.02%/℃。实验前通过模块自带上位机矫正加速度计与陀螺仪偏置，减少由实验设备自身带来的误差。

进行静态实验时，首先在实验场地的UWB布置区域内拟定真实坐标点，使用激光测距仪测量其在导航坐标系中的精确位置作为真实位置。控制小车并使小车标签、IMU与真实坐标点对齐，在UWB的上位机中，对每一个点位采集2 s数据，最终取2 s内平均值作为UWB单一硬件输出坐标，同时采集这2 s内的IMU输出值作为融合算法中的IMU数据。伪距解算坐标点为2 s内UWB输出的飞行时间进行最小二乘法解算的坐标点平均值，SA-ESKF算法根据二者数据解算出的平均坐标点作为算法定位点。

最终以UWB平均坐标、伪距三维解算坐标、融合算法定位坐标与激光测距仪实际坐标的欧氏距离为标准计算优化距离，以UWB硬件定位点到真实坐标点的距离，减去伪距解算点、融合算法定位点与真实坐标点的距离，得到欧氏距离的误差，再除以UWB定位点到真实坐标点的距离得到优化百分比，结果如图 9所示。

由图 10可知，多组静态定位实验中SA-ESKF算法解算的定位点坐标更加接近真实点坐标，且算法的优化程度多数大于最小二乘法解算的伪距定位，其平均误差、最大误差与优化程度如表 1所示。

由表 1可知，在三维定位中，通过最小二乘法解算飞行时间的伪距定位点在一定程度上优于UWB单一硬件直接定位，SA-ESKF算法的最大误差与平均误差均小于最小二乘法伪距解算定位点。伪距解算定位与SA-ESKF融合算法定位的精度相较于UWB硬件直接定位分别提高了25.78%与56.33%。上述结果表明，在车辆静态定位实验中，设计的融合算法定位能力更强，定位精度更高。

进行动态实验并采集数据时，UWB基站布置方式与静态实验时一致，人为将运动小车运动轨迹约束在实验场地内，小车运动轨迹封闭。同样使用4个基站与1个定位标签，采集数据时，小车采用遥控控制，按照预先设定轨迹行驶，尽可能模拟车辆在车库中的真实情况，控制小车在运动时作变速运动，不断模拟车辆在车库运动时的各种微小转向以及频繁加速、刹车等情况。

小车在运动时间内，三轴坐标在导航坐标系下的变化与误差变化如图 11所示。在橙色方框内的时间段，地下车库有噪声较大的中小型货车驶入，且为UWB进入定位模式初期，UWB信号会产生较大波动，使得融合算法定位精度下降。当噪声减少且定位模块趋于稳定后，融合算法使UWB模块产生的波动在较短时间内趋于稳定，稳定后定位轨迹更加平滑，不再呈现锯齿状，这也表明该算法具备较强的鲁棒性。在后期定位中，普通车辆与行人正常活动，融合算法也可持续输出稳定的精确位置信息。

动态实验中，每一帧3种定位方式坐标点与真实坐标点之间的欧式距离误差如图 12所示，新提出的融合定位算法解算的定位点到真实坐标点的欧氏距离多数小于UWB单一硬件直接定位点与最小二乘法伪距解算点到真实坐标点的距离。通过对融合算法结果的定量分析，得到其三轴定位的平均误差、最大误差与优化程度，如表 2所示，三轴误差评价结果如表 3所示。

由表 2和表 3可知，本文设计的融合算法在地下车库的动态实验中表现优异，在X轴、Y轴和Z轴方向的最大误差与平均误差均小于UWB单一硬件定位与最小二乘法伪距解算定位2种方式，对低成本UWB在三轴上的波动抑制十分明显。此外，在大多情况下，本文提出的融合算法定位平均误差在各个方向上均小于其他2种方式。融合算法在动态实验中将定位点的平均误差降低至0.02 m，最大误差降低至0.24 m，总体定位精度提高了46.47%，相较于最小二乘法解算的定位精度提高了12.44个百分点。由此得出，本文在ESKF算法的基础上加入SA算法后，能实现低成本UWB的高精度定位，可在地下停车场等封闭环境中提供可靠稳定的定位信息，实现了对UWB信号波动的抑制。

1) 在传统基于ESKF算法的基础上，引入基于IMU短时刻位移增量的SA算法，对滤波器的先验输入值进行改进，从而提升低成本UWB与IMU在室内停车场的融合定位精度。在实际部署和使用过程中，该方法能够满足车辆在室内停车场环境下对定位准确性的要求。

2) UWB与IMU的SA-ESKF融合算法增强了系统的鲁棒性，降低了定位输出结果的波动幅度。在受到噪声干扰后，能够迅速提高定位精度，减小定位误差。在室内停车场动态实验中，车辆在X轴、Y轴和Z轴方向的平均定位误差分别为0.036 3 m、0.035 0 m和0.044 3 m，SA-ESKF融合算法的定位精度提高了46.47%，该算法可应用于室内停车场的定位检测。

3) 改进后的UWB与IMU融合定位算法能够持续输出高精度且波动较小的位置信息。然而，在室内停车场中，该算法受噪声与非视距信号的影响较大。在未来的研究中，将继续优化UWB与IMU的融合算法，提高对非视距信号的识别准确率，并尝试引入更多传感器，从而进一步提升室内停车场的定位精度和可靠性。