-
开放科学（资源服务）标识码（OSID）：
-
随着设施农业的快速发展，温室、大棚和植物工厂等作业环境对作物生长状态监测、农机作业调度和自动化作业路径规划的依赖不断增强，移动平台、喷杆机、轨道运输小车等装备对高精度位置与姿态信息的需求愈发突出[1-4]。然而，此类作业环境通常为封闭或半封闭结构，全球导航卫星定位系统(Global Navigation Satellite System，GNSS)信号严重衰减甚至完全不可用，即便采用GNSS/INS组合导航，也难以在棚内、棚间提供连续可靠的定位服务[5-10]。
在这类场景中，超宽带(Ultra-Wideband，UWB)依靠纳秒级脉冲和较大带宽，在视距(Line-of-Sight，LOS)条件下能够提供较高精度的测距能力，且系统结构相对简单，已在温室移动平台、设施园艺自动运输系统等领域逐步应用。但在大棚骨架、栽培架、金属管道和作业人员遮挡情况下，测距中会出现显著的非视距(Non-Line-of-Sight，NLOS)正偏差，单纯依赖UWB几何定位容易产生“拖尾”“跳点”等现象，导致农机或作业平台轨迹不稳定[11-18]。低成本微机电系统惯性测量单元(Inertial Measurement Unit，IMU)具有短时间连续性好、对遮挡不敏感的优点，适合在农机作业过程中提供高频运动信息，但零偏和随机噪声会随时间积累，难以独立长期使用。将UWB与IMU进行紧组合，利用UWB对惯导漂移进行约束，并借助运动学模型对异常测距进行一致性检查，已成为设施农业环境中获取连续高精度位姿信息的重要技术路线之一[19-23]。
现有UWB/IMU组合定位研究已取得诸多成果。例如，丁俊杰等[24]面向设施园艺场景遮挡严重导致的定位精度与稳定性问题，构建UWB/IMU组合定位系统，并提出自适应抗差组合定位算法(ARCPA)，在对比多种滤波方法时给出了更低的RMSE，并显著优于单传感器定位。王凯等[25]针对煤矿无人驾驶车辆跨“地面-斜井-井下”全流程定位，设计了GNSS/UWB融合IMU的无缝系统，并提出以“模型切换延时”作为评价指标；井下部分采用UWB/IMU紧组合并以误差状态卡尔曼滤波(ESKF)进行优化，进一步采用模糊自适应改进交互式多模型(FAIMM-ESKF)提升了定位精度，满足无缝切换时延要求。谭兴龙等[26]针对NLOS误差与传感器杆臂双重影响，提出“强局部加权回归”的NLOS识别与抑制，并在抗差奇异值分解无迹滤波算法(SVD-UKF)框架下引入Huber抗差迭代，实现杆臂估计/补偿与融合定位。杨秀建等[27]面向GNSS拒止且UWB与LiDAR均易退化的复杂环境(NLOS与点云特征稀疏环境)，提出利用LiDAR与UWB互补的“时变因子重定位”，并引入3种运动模型与无迹卡尔曼滤波(Unscented Kalman Filter，UKF)，再通过交互式多模型-无迹卡尔曼滤波算法(IMM-UKF)解算出最终位置。黄卫华等[28]针对复杂环境下UWB的NLOS与随机误差，提出基于改进Sage-Husa KF(SHKF)的UWB/IMU组合定位思路，通过滤波器噪声统计量更新提升鲁棒性与适应性。上述方法在一定程度上缓解了NLOS和噪声失配问题，但仍存在以下不足：一是大多数方法仅仅针对观测噪声协方差进行经验式的离线调参，对统计特性显著时变的复杂环境适应性有限；二是滤波器内部未对新息统计特性进行系统建模与鲁棒处理，仍可能出现滤波发散或轨迹剧烈活动的情况。
针对上述问题，本文提出一种基于鲁棒新息统计的UWB/IMU紧组合扩展卡尔曼滤波定位算法(Robust Innovation-Based Adaptive Extended Kalman Filter，RI-AEKF)，核心思路是提出基于归一化新息统计的异常观测检测与鲁棒加权策略，通过引入带阈值的权函数对新息进行重加权，实现对NLOS异常测距的软抑制，避免简单丢弃观测带来的信息浪费。设计新息协方差驱动的双通道自适应机制，一方面利用滑动窗口归一化新息估计观测噪声协方差，修正UWB测距噪声的时变特性，另一方面利用新息长期偏离程度对过程噪声协方差进行渐进调节，提高对IMU建模误差与环境扰动的适应能力。结合履带式试验车的室内轨迹试验，对比传统扩展卡尔曼滤波(Extended Kalman Filter，EKF)、改进自适应EKF(IAEKF)与本文方法在LOS和NLOS场景下的表现，量化分析所提算法在精度、鲁棒性和收敛性方面的优势。
HTML
-
考虑以设施农业中的平面运动场景为主，引入下列坐标系。导航坐标系n：平面直角坐标系，为右手系，记为(x，y)；载体坐标系b：固定在履带小车上，X轴沿着前进方向，Y轴向左，Z轴向上。假设载体主要在水平面内运动，仅考虑偏航角ψ，滚转角和俯仰角均为小角度，可忽略不计。IMU安装在载体上，输出三轴比力加速度am=[amx，amy，amz]和三轴角速度ωm=[ωmx，ωmy，ωmz]。UWB系统由若干基站构成，其在导航坐标系下的坐标已知。
-
在二维平面模型下，定义系统的状态向量为：
式中：px、py为载体在导航坐标系中的位置；vx、vy为导航坐标系中的速度；ψ为偏航角；bax、bay为加速度计在X、Y轴方向的零偏；bgz为陀螺仪Z轴零偏。
IMU量测模型可写为：
式中：axb、ayb、ωzb为载体坐标系下的真实值；nax，nay，naz为零均值白噪声。
然后，对零偏进行补偿，可以得到：
载体坐标系到导航坐标系的旋转矩阵为：
则导航坐标系下的水平加速度为：
在采样周期Δtk内，采用离散时间运动学模型，可得系统状态更新方程为：
式中：wbax，wbay，wbgz为零偏的随机游走过程噪声。
将式(6)记为非线性离散系统，得：
其中，输入向量为：
式中：wk为过程噪声，满足wk~N(0，Qk)。
在实际实现过程中，首先按式(3)-式(6)计算状态转移函数f，得到先验状态预测值；随后对f在当前估计点处进行线性化，构造状态转移雅可比矩阵Fk：
在滤波过程中，可根据式(6)对各状态分量求偏导得到，或者通过数值差分近似得到。
-
设系统一共部署了M个UWB锚点，第i个锚点在导航系中的坐标为(xi，yi)。载体搭载UWB标签，其到第i个锚点的理想几何距离为：
实际测距量测可写为：
式中：nr，i，k为测距噪声，在LOS条件下可近似为零均值高斯噪声；在NLOS环境中则可能出现较大的偏差和野值。
将同一时刻各锚点的量测组成量测向量，得：
对应的非线性量测方程为：
其中，
量测方程的雅可比矩阵Hk为：
其中，第i行的非零项为：
其余对速度、姿态和零偏的偏导均为0。
1.1. 坐标系与运动假设
1.2. 状态向量与系统方程
1.3. UWB测距量测模型
-
在上一节给出的系统状态模型式(7)和UWB量测模型式(13)的基础上，本文首先构建传统的UWB/IMU紧组合扩展卡尔曼滤波器(EKF)，作为后续自适应与鲁棒改进算法的基准。EKF假设系统过程噪声和量测噪声服从零均值高斯分布，通过对非线性状态方程和量测方程在当前估计点处进行一阶线性化，将非线性滤波问题转化为线性高斯滤波问题的近似求解。记
$\hat{\boldsymbol{x}}_{k \mid k}$ 为k时刻的后验状态估计，$\hat{\boldsymbol{x}}_{k \mid k-1}$ 为先验估计，协方差矩阵分别为Pk|k和Pk|k-1。 -
预测步骤由IMU数据驱动，用于在相邻量测时刻之间对状态进行连续外推。根据离散化后的系统状态模型，可得到k时刻的先验状态和协方差预测为：
式中：f(·)表示由比力和角速度积分得到的非线性状态转移函数；IMU量测uk决定了平台在该采样周期内的运动；雅可比矩阵Fk刻画了当前状态对下一状态的线性敏感性，用于近似传播协方差；过程噪声协方差Qk反映了IMU噪声、温漂、模型简化以及未建模动态等对系统不确定性的累计影响，其大小直接影响预测协方差的发散速度。
-
当k时刻获得UWB测距数据时，利用量测模型对先验状态进行校正。首先根据当前先验状态计算UWB量测的预测值
$\hat{\boldsymbol{z}}_{k \mid k-1}$ ，并构造新息向量vk和量测雅可比矩阵Hk：新息反映了实际UWB测距与基于先验状态计算得到的几何距离之间的差异，是衡量当前量测与系统预测是否一致的关键统计量。理想情况下，在噪声统计特性建模准确时，新息应近似服从零均值高斯分布。基于新息构造创新协方差矩阵Sk和卡尔曼增益Kk，可得到传统EKF的量测更新公式：
式中：Rk为UWB量测噪声协方差矩阵，用于表征测距误差水平和不同基站量测之间的相关性。
卡尔曼增益Kk实质上给出了“相信量测”和“相信预测”的权重分配：当Rk较小、UWB量测质量较高时，Sk减小，Kk增大，滤波器更依赖UWB对惯导漂移进行校正；反之，当UWB量测噪声较大或可用基站数量较小时，Kk减小，系统更依赖IMU预测结果。通过反复执行预测和更新2个步骤，即可在全程运动过程中实现对平台位置和速度的递推估计。
在传统EKF中，Rk预先设为常数对角矩阵，当UWB量测受到NLOS和野值影响时，滤波性能将明显恶化。
2.1. 预测步骤(时间更新)
2.2. 更新步骤(量测更新)
-
为提高UWB/IMU紧组合滤波在复杂室内环境下的鲁棒性，本文在传统EKF的量测更新阶段引入鲁棒新息统计与自适应观测噪声调整，核心思想是利用新息及其协方差构造标准化新息统计量，识别异常量测，通过鲁棒权函数给每个量测赋予不同的权重，然后将权重映射到观测噪声协方差Rk，实现对NLOS和野值的抑制，最后对整体噪声水平引入缓慢自适应因子，使Rk能自动匹配实际噪声强度。基于RI-AEKF的UWB/IMU紧组合定位系统如图 1所示。
-
在不改动预测部分的前提下，首先按式(18)中传统EKF的思路构造中间量：
式中：R0=σr2IM为基础测距噪声协方差；σr为在LOS条件下标称的测距标准差。
对第j个量测构造标准化新息：
式中：vj，k2为新息向量vk的第j个分量，表示第j个UWB量测的实际观测值与预测观测值之间的差值，即该量测的残差；Sjj，k为创新协方差矩阵Sk的第j个对角元素，表示第j个量测新息的理论方差，它综合考虑了状态预测误差通过量测方程传播后的影响以及量测噪声本身的贡献。
γj，k反映了第j条量测的“离谱程度”，在模型及噪声匹配且量测为高斯分布的理想条件下，其期望值应该接近1。若γj，k远远大于1，则表明该量测存在较大误差，可能为NLOS或野值。
-
采用Huber型权函数对每个量测构造权重wj，k：
式中：c为经验门限，一般取2~3。
由式(22)可知：当γj，k较小时，即量测正常时，wj，k≈1，量测权重不变；当γj，k较大时，即量测异常时，wj，k≪1，对应的量测权重将显著下降。
此外，可设置策略剔除门限γmax，若γj，k＞γmax，则直接将该量测视为野值，从当前量测向量中剔除，不参与本次更新。
-
为了将鲁棒权重体现到滤波器中，本文将Rk设计为对角矩阵，其中第j个对角元素与权重成反比：
这样，对于某条量测的权重wj，k较小，即不可信时，其对应的观测噪声方差Rjj，k会被放大，从而在卡尔曼增益中自动降低其影响。进一步地，为了反映不同时间段整体噪声水平的变化，引入整体尺度因子sk，并利用所有有效量测的平均NIS自适应进行更新。设有效量测索引集合为Jk，该索引是剔除野值后剩余的，得到平均标准化新息为：
尺度因子按照一阶低通形式进行更新：
式中：α∈(0，1)为自适应步长。
若整体残差偏大，则sk会逐步增大，表示当前噪声水平高于标称值；反之则略有减小。综合式(23)和式(25)，本文最终采用的观测噪声协方差为：
-
在以上设计基础上，对UWB/IMU紧组合鲁棒自适应EKF的量测更新步骤进行如下整理：
步骤1 基于基础观测噪声计算中间新息统计。
利用R0=σr2IM计算下式：
步骤2 构造标准化新息与鲁棒权重。
对每个量测分量j=1，…，M，按式(21)计算γj，k，再由式(22)得到ωj，k。若γj，k＞γmax，则将该量测剔除。
步骤3 计算整体尺度因子并构造自适应观测噪声协方差。
对有效量测集合Jk，按式(24)得到γk，再由式(25)更新sk，最终由式(26)构造
$\widetilde{\boldsymbol{R}}_k$ 。步骤4 基于自适应观测噪声完成EKF更新。
将
$\widetilde{\boldsymbol{R}}_k$ 替代式(19)中的$\widetilde{\boldsymbol{R}}_k$ ，对保留的量测子向量和对应的Hk子矩阵执行更新：综上，量测异常越严重，其对应的权重越小，观测噪声越大，卡尔曼增益越小，从而能够有效抑制NLOS和野值对融合解的影响。同时，整体尺度因子sk又能缓慢跟踪实际噪声强度，使滤波器在不同环境下具有良好的自适应性能。
3.1. 标准化新息统计量
3.2. 鲁棒权函数
3.3. 自适应观测噪声协方差调整
3.4. 鲁棒自适应EKF更新步骤
-
为验证本文所提出算法的有效性，选取3种滤波算法进行对比：传统EKF、改进自适应EKF(IAEKF)以及本文提出的RI-AEKF。其中，IAEKF在EKF基础上依据归一化新息在线调整观测噪声协方差，但不包含异常量测鲁棒权重机制，因此可用于评估“仅自适应”与“自适应+鲁棒”2类方法在温室NLOS环境中的性能差异。同时，基于Matlab搭建仿真试验环境。仿真试验的环境设置参考图 2所示的温室大棚作业小车工况，在温室内构建12 m×12 m的二维工作区域，UWB系统在区域四角布设4个基站，坐标分别为A(0，0)、B(0，12)、C(12，12)、D(12，0)，标签固定在车辆的质心处。车辆真实轨迹为：
仿真中假设车辆沿给定轨迹从A点依次经过B、C、D，再回到起点，基准行驶速度设为v0=0.6 m/s。为刻画实际工况中频繁起停和轻微加减速行为，实际速度的公式为：
式中：Δvsin(t)=Avsin(ωvt)为低频正弦速度扰动；Av为扰动幅值(本文取0.2 m/s)；ωv为扰动角频率(对应周期约为20 s)，用以模拟驾驶员在长直段上的周期性加减速；Δvrand(t)为均值为0、方差较小的白噪声随机扰动，用以刻画局部细小速度波动。
车辆沿轨迹的切向速度由v(t)给定，再结合轨迹几何关系积分得到各时刻的“真实”位置和速度，用于后续IMU和UWB量测的生成。仿真时间步长设为0.1 s，按该步长离散得到各历元的位置、速度和航向，并由此获得IMU的线加速度和角速度观测。
为了模拟消费级IMU的测量特性，仿真中在理想值基础上叠加2类误差：一是零均值高斯白噪声，用于反映随机测量噪声；二是缓慢变化的随机游走项，用于近似表示加速度计和陀螺零偏漂移。具体实现为：在每个仿真采样时刻，先由真实轨迹计算得到“无误差”的加速度和角速度，然后在该时刻叠加噪声和漂移项得到IMU输出，即：
式中：na，k、ng，k为零均值高斯噪声；ba，k、bg，k按一阶随机游走模型在相邻时刻缓慢更新。
对于UWB测距部分，同样在每个采样时刻根据真实位置与各基站坐标位置计算几何距离，在LOS条件下在几何距离上叠加零均值高斯噪声；在NLOS条件下则额外叠加正偏差项和少量随机野值，以模拟遮挡和多径效应。本文预先设定若干时间区间为NLOS段，此时对应基站的测距会出现明显偏大和偶发突变，其表现为距离误差突然增大或位置估计产生孤立跳点。
由图 3可知，EKF、IAEKF与RI-AEKF 3种算法在轨迹跟踪上的表现存在显著差异。EKF算法在整个试验过程中能够较好地跟随真实轨迹，但在NLOS区域出现较大偏差，导致定位精度下降。相比之下，IAEKF算法在NLOS区域表现出更为稳定的轨迹跟踪，减少了误差，这表明其在动态环境中的适应能力较强。而RI-AEKF算法在所有测试阶段均表现优异，尤其是在NLOS环境中，能够有效抑制噪声影响，保持较高的定位精度，体现了鲁棒新息统计方法在抗干扰能力方面的优势。总体而言，RI-AEKF算法在复杂环境中的定位精度和稳定性均优于其他2种算法。
图 4为X轴位置误差对比图，图中的X轴误差展示了EKF、IAEKF和RI-AEKF算法在X轴方向的定位表现。可以看到，RI-AEKF算法在整个测试过程中误差较小，尤其是在误差的波动性上，表现得相对平稳，这与表 1中的均方根误差(RMSE)数据结果一致，表明RI-AEKF在X轴上的RMSE为0.126 m，明显低于EKF的0.296 m和IAEKF的0.254 m。相比之下，EKF和IAEKF在某些时刻的误差波动较大，特别是在系统受到干扰或出现非线性测量时，误差幅度明显增加，这表明RI-AEKF在处理动态变化的过程中表现出更强的鲁棒性。
在Y轴方向上，RI-AEKF的误差表现同样优于其他2种算法。由图 5可知，RI-AEKF呈现出更加平稳的走势，误差波动相对较小，符合其在RMSE数据中显示出的较低值(0.162 m)。IAEKF和EKF的误差波动较大，尤其是在某些较高峰值时，误差明显增大。尽管如此，IAEKF的误差较EKF有所改善，尤其在初始阶段波动较小，但依然无法与RI-AEKF相比。
图 6为欧式距离误差对比图，从欧氏距离误差随时间变化的曲线可以看出，3种算法在整体趋势上均能跟踪轨迹变化。但EKF和IA-EKF在多处出现尖锐峰值，最大误差分别接近2.0 m和1.0 m，与表 1中分别对应的最大位置误差2.009、1.061 m基本一致；RI-AEKF的误差波动明显收敛于0~0.70 m，峰值约为0.65 m，对应的最大位置误差为0.655 m，同时背景误差水平也显著更低。这一点在均方根指标上也得到印证，RI-AEKF的二维位置RMSE为0.205 m，仅为EKF(0.439 m)的一半左右，较IAEKF(0.375 m)也有明显降低。
图 7给出了3种算法二维位置误差的累积分布函数(CDF)，纵坐标表示误差不超过横坐标给定值的累积概率，进一步反映了误差分布特性。RI-AEKF曲线在小误差段上升最快，在同一误差阈值下对应的累积概率始终高于另外2种算法，说明大部分历元的二维误差集中在较小范围内；IAEKF曲线整体优于EKF，但在中、大误差段仍存在一定“长尾”。结合RMSE与最大误差结果，可以认为鲁棒新息统计在抑制大偏差样本、压缩误差尾部方面效果更为突出，使位置误差分布更集中，有利于提升系统的整体可靠性。
图 8和图 9分别为X、Y轴的速度误差图，表 2给出了不同定位算法速度误差的统计情况。在速度误差方面，3种算法在X轴和Y轴均表现出类似规律。EKF的速度误差曲线波动幅度最大，局部存在明显尖峰，X轴和Y轴的最大速度误差分别接近3.544 m/s和3.943 m/s，X轴和Y轴速度分量构成的合成二维速度RMSE达到0.911 m/s。IAEKF通过噪声自适应调整后，整体波动有所减弱，尖峰幅值明显下降，二维速度RMSE降至0.577 m/s，但在测量质量突变位置仍可观察到较大的负、正脉冲。RI-AEKF的速度误差曲线最为收敛，X轴和Y轴误差基本被限制在±0.5 m/s左右，偶发峰值也明显低于前2种算法，二维速度RMSE仅为0.330 m/s。结合图中平稳的误差波形可以看出，鲁棒新息统计一方面抑制了由异常测距引起的瞬时速度冲击，另一方面通过对噪声协方差的在线整定减弱了速度估计的高频抖动，使得速度解算在2个方向上都具有更高的精度和稳定性。
-
本文针对设施农业环境下UWB/IMU紧组合定位易受NLOS测距误差和噪声统计失配影响的问题，提出了一种基于鲁棒新息统计的自适应扩展卡尔曼滤波(RI-AEKF)定位算法。在统一的UWB测距与惯性运动学模型基础上，算法通过归一化新息构造权函数，实现对异常观测的软约束抑制，并结合滑动窗口新息协方差估计，对观测噪声和过程噪声协方差进行联合整定，从而减轻噪声先验不准确带来的滤波增益失调。
在温室作业场景下构建的UWB/IMU仿真平台中，设置骨架和栽培架遮挡引起的NLOS区段，对比EKF、IAEKF与RI-AEKF 3种算法的定位性能。结果表明：在相同模型和初始条件下，RI-AEKF将二维位置RMSE从0.439 m降至0.205 m，将最大位置误差从2.009 m降至0.655 m，二维速度RMSE从0.911 m/s降至0.330 m/s，误差累积分布明显收缩，小误差比例显著提高。尤其在温室骨架密集、遮挡突变的路段，RI-AEKF能有效抑制位置和速度的瞬时尖峰，保持轨迹的连续性与平滑性，体现出在温室大棚干扰环境中具有较强的鲁棒性和工程应用潜力。
DownLoad: