多传感器系统的偏差补偿和状态估计.pdf

第 61 卷 第 11 期Vol.61 No.112023 年 11 月November 2023农业装备与车辆工程AGRICULTURAL EQUIPMENT&VEHICLE ENGINEERINGdoi:10.3969/j.issn.1673-3142.2023.11.032多传感器系统的偏差补偿和状态估计杨权（650500 云南省 昆明市 昆明理工大学 信息工程与自动化学院）摘要 在多传感器目标跟踪系统中，偏差补偿对于正确的数据融合至关重要，消除异步多传感器的偏差是多传感器精确数据融合的基础。针对量测存在时间偏差和系统偏差的多传感器系统，提出一种新的多传感器误差偏移估计方法。首先，推导并计算了时间偏移伪测量方程，以获得相对时间偏移估计；其次将传感器系统偏差与目标状态相结合以获得增强状态向量，并建立了增强状态模型；最后，设计了一种基于扩展卡尔曼滤波的目标状态与传感器系统偏差扩维联合估计算法。实验结果表明，在量测存在时间偏差和系统偏差的情况下，所提算法可以获得相对时间偏移的无偏估计，同时能够有效地解决带有系统误差的状态估计问题。关键词 多传感器；扩展卡尔曼滤波；运动状态估计；时延估计；目标跟踪 中图分类号 TP212.9 文献标志码 A 文章编号 1673-3142(2023)11-0159-04引用格式：杨权.多传感器系统的偏差补偿和状态估计 J.农业装备与车辆工程,2023,61(11):159-162.Bias compensation and state estimation for multi-sensor systemsYANG Quan(School of Information Engineering and Automation,Kunming University of Science and Technology,Kunming 650500,Yunnan,China)Abstract In multisensor target tracking system,deviation compensation was very important for correct data fusion.Eliminating the deviation of asynchronous multisensor was the basis of accurate multisensor data fusion.A new method of multisensor error offset estimation was proposed for multisensor systems with time bias and system bias.Firstly,the pseudo measurement equation of time migration was derived and calculated to obtain the estimation of relative time migration;Secondly,the sensor system bias was combined with the target state to obtain the enhanced state vector,and the enhanced state model was established.Finally,a joint estimation algorithm of target state and sensor system deviation based on extended Kalman filter was designed.Experimental results showed that the proposed algorithm could obtain unbiased estimation of relative time offset and effectively solve the problem of state estimation with systematic error in the presence of time bias and system bias in the measurement.Key words multi sensor;extended Kalman filter;motion state estimation;time delay estimation;target tracking0 引言多传感器数据融合是目标跟踪的重要组成部分，通过融合多个传感器的测量值，系统可获得比单传感器测量值更好的跟踪性能1-2。传统方法通常假设测量的时间戳表示相对于单个时刻的实际测量瞬间，而在实际应用中，由于信号处理、数据传输、滤波器延迟等原因，导致测量瞬间和时间戳之间存在时间延迟3，如图 1 所示。此外，由于传感器内部结构原因，测量存在恒定的空间偏差，包括二维坐标的距离偏差和方位偏差。如果不适当补偿时空偏差，多传感器数据融合的性能可能会大大降低，甚至比单个传感器的性能更差。因此，时空偏差估计和补偿是多传感器数据融合前的一个重要部分。在时间偏差估计问题上，He 等4在广义最小二乘估计的理论框架下，提出一种离线联合系统误差估计算法估计雷达空间偏差和自动相关监视广播（ADS-B）设备时间偏差。该算法的缺点是雷达和ADS-B 需要具有相等的采样周期，此外，未考虑收稿日期：2022-11-25图 1 观测时刻、时间戳和时间延迟的关系图Fig.1 Relationship between observation moment,timestamp and time delay传感器 1 观测时刻传感器 2 观测时刻传感器 1 和传感器 2 的时间戳t1t2T2T1tk1tk2tk1，tk2160农业装备与车辆工程 2023 年雷达时间戳有时延的情况；Huck 等5通过过滤测量周期持续时间并估计最小时间偏移，提出了时间偏移最小化的算法，该方法的缺点是仅使用测量的时间戳，且无法从理论上评估其性能。若充分利用距离和方位测量，它可以获得更好的性能。目标状态与系统偏差联合估计法将系统误差作为状态向量中的扩维部分，进行系统误差和目标状态的联合滤波，主要有 Nabaa 等6提出的基于 EKF 的扩维方法 ASEKF（Augmented State Extended Kalman Filter），宋强等7-8、刘瑜等9提出的基于 ASUKF（Augmented State Unscented Kalman Filter）的实时误差配准算法以及 Li 等10提出的基于 EM-KF 的联合估计算法。以上算法均未考虑时间偏差，只研究了传感器本身带来的空间偏差。然而在实际应用中，时间偏差所带来的跟踪误差往往是不可忽视的。考虑到现有偏移误差估计算法的不足，本文在现有研究的基础上通过使用轨迹估计来构造伪测量对多传感器系统的时间偏移进行估计，且针对带有固定传感器偏差的目标状态估计问题，研究并提出一种基于扩展卡尔曼滤波的目标状态与传感器偏差扩维联合估计算法。将目标的运动模型和传感器偏差配准模型有机地结合起来，形成扩维的状态空间模型，即将传感器偏差作为目标状态向量中的分量，然后基于扩展卡尔曼滤波，实现状态与偏差联合实时估计。1 问题描述1.1 时间偏移伪测量一个多传感器目标跟踪系统由具有时间偏移的测量模型和目标动态模型组成。假设有 N 个雷达在极坐标中提供二维（2D）测量，即距离和方位测量。设 rim(tik)和 im(tik)分别表示传感器 i 在时间 tki的距离和方位测量，其中 k 表示第 k 个时间步长。假设传感器在不同采样时间提供的测量值的时间偏移为常数，则传感器 i 的时间偏移可以表示为tttikikiD=-r (1)式中：tkir传感器 i 的第 k 个时间戳；tki相应的精确采样时刻且未知。在时间 tkir到达处理中心的测量实际上是传感器 i 在采样瞬间 tki的测量，可以表示为Z trttr ttvtvtikiimkiimkiikiikiirkiikiii=+irgggggggHHH (2)式中：ri(tki)、i(tki)在时间 tki的真实测量距离和方位角；vir(tki)、vi(tki)距离和方位角的相应测量噪声。噪声为零均值白高斯噪声，相应的协方差分别为 r2、2，且统计独立。采用无偏测量坐标转换算法将极坐标转换为笛卡尔坐标，即cossinxyerttrtt/imkiimkiimkiimki22ii=diggggHH (3)转换测量方程为Z tt x tV tHikikikiiki=+rgggg (4)目标状态方程为x tttx tw ttFkikikikikiki111=+rrrrrrghgh (5)式中：H(tki)线性测量转移矩阵，以 CV 模型为例，H1 0 0 00 0 1 0=H，因为同一个运动模型下 H(tk1)和 H(tk2)的测量模型相同，所以统一简写为 H；ttFkiki1+rrh目标运动状态转移矩阵；w ttkiki1+rrh零均值高斯白噪声，其协方差为 Q ttkiki1+rrh；,x v y vxxyT=6目标在笛卡尔坐标系中的状态向量，其中x和y分别表示沿x轴和y轴的位置坐标，vx和 vy分别是相应的速度。由传感器 1 和传感器 2 的观测值可得：ZkZ tZ tF ttttx tx tw ttw ttV tV tF ttttx tx tw ttV tV tHI FIHHI FIHpkkkkkkkkkkkkkkkkkkkkkkkk112211121211121122111212211122=-=-+-=-+-rrrrrrrrghhhhgghgghhgggggg777AAAHH (6)式中：diagttBIkf ttf ttFkkkkkk111111=rrrhhh7A；diagttBIkf ttf ttFkkkkkk121212=rrrhhh7A；tttf101kkk111D=rh=G，tttTf101,kkkk1211 2DD=+rrhH。此时时间偏移的伪测量方程为Zkv tv tv tv tttTv tv tkkt kbkkVHV,cvxkykxkykkkkxkykpcvcvp1122121 222DDDD=-+=+rgggggggggggg=GGH (7)式中：Ttt,kkk1 212D=-rrr；Hcv(k)伪测量观测矩阵；bcv(k)一个关于时间偏差的偏置量；Vp(k)协方差为 R(tki)的零均值高斯白噪声向量。1.2 增强状态方程假设有传感器A和传感器B对目标进行观测，其量测值分别为(RA，A)和(RB，B)。传感器 A 和传感器 B 的距离系统误差分别为 RA和 RB，方位161第 61 卷第 11 期杨权：多传感器系统的偏差补偿和状态估计角系统误差分别为 A和 B。本文考虑了目标的匀速运动模型。k 时刻的目标状态描述为,X kx kx ky ky k=ooggggg6 (8)式中：(),()x ky k 目标的位置；,x ky koogg目标的速度。目标的运动模型描述为X kk X kk v kF1C+=+hgggg (9)式中：(k)过程噪声增益矩阵；F(k)状态转移矩阵。目标扩维状态向量 Xaug(k)为,kx k x k y k y kR kkRkkXaugAABBiiDDDD=ooggggggggg6(10)则目标的扩维运动模型描述为()kkkk vkXFX1augaugauaugauggC+=+hggg (11)式中：aug(k)扩维过程噪声增益矩阵；Faug(k)扩维状态转移矩阵。,diagdiagkkkkvkvkFFII0augaugaugTTCC=gggggghh6 (12)其中I为单位矩阵。系统的量测方程可以定义为,Z kX kW kRkk RkkhAABBii=+=gghggggg6(13)式中：h观测矩阵；v(k)、vaug、W(k)白色高斯随机噪声，其方差矩阵为 Q1(k)、Q(k)、L(k)。2 本文算法首先针对传感器观测信息的时间偏移为常数的情况，使用轨迹估计构造伪测量对多传感器系统的时间偏移进行估计，其次将目标的运动模型和传感器偏差配准模型组合在一起，形成扩维的状态空间模型，即将传感器偏差作为状态向量中的分量，然后利用扩展卡尔曼滤波技术实现目标状态和传感器偏差的联合估计。在不失通用性的前提下，为了简化，本文考虑使用 2 个传感器的集中式系统。2.1 时间偏移估计如果传感器 A 和传感器 B 的时间偏移被建模为未知常数，则递归最小二乘（RLS）估计器11可用于基于 CV 模型的伪测量方程来估计相对时间偏移。时间步长 k 的时间偏移估计步骤为：（1）由式（7）获取伪测量值 Zcv(k)，测量噪声协方差 R(k)的表达式为R kHQ ttHR tR tkBkATkAkB=+ghgg (14)（2）分别计算时间偏移更新增益和残差G kHkHkHkR k,cvkA BTcvcvkA BT111=+-ggggg8B/(15)r kZkHktbk,cvcvkA Bcv1D=-tgggg (16)式中：t,kA B1D-t假设的相对时间偏移估计；,kA B1-/相应的估计误差协方差。（3）分别更新时间偏移估计及其误差协方差ttG k r k,kA BkA B1DD=+-tthh (17)IG k HkIG k HkG k R k G k,cvkA BcvkA BTT1=-+-ggggggg66/(18)2.2 增强状态估计将目标的运动模型和传感器偏差配准模型组合在一起，形成扩维的状态空间模型，即将传感器偏差作为状态向量中的分量，然后利用扩展卡尔曼滤波技术实现目标状态和传感器偏差的联合估计。将由此进行的基于 EKF 的目标状态和系统偏差的联合估计定义为 ASEKF 算法。（1）初始化：设置状态初值为 x0，协方差矩阵初值为 P0；（2）时间更新：在 k 时刻，由 2 个传感器的观测可得观测值 Z(k)，通过时间偏移估计以及偏差补偿后可得到观测值 Z*(k)。此时预测状态和误差协方差为kkkkXFX1augaugaug+=_igg (19)kkkkkkPFPFQ1augaugT+=+_igggg (20)（3）测量更新：计算卡尔曼增益（其中：H是 h 的雅克比矩阵）kkkkkkKPHHPHL11TT1=+-_giig7A(21)更新状态和误差协方差为kkkk Z khkkXXKX111augaugaug+=+-+)_higgii7A(22)kkkkPIKH P11+=-+_hgi6 (23)3 仿真结果及分析3.1 仿真设置为验证本文算法的有效性，采用蒙特卡洛仿真，将本文算法与 ASEKF 进行比较。设定蒙特卡洛仿真次数为 100 次，步数设置为 100 步。仿真环境：采用 2 部同步传感器 A 和 B 进行组网融合，采样间隔均为 T=1 s。设定传感器 A 和 B 的笛卡尔坐标分别为（0，0）m、（1 000，0）m；各传感器的距离、方位角量测精度均分别设置为 50 m、0.3；而测距系统误差分别为 150 m、200 m，测方位角系统误差分别为 0.2、0.3。假设传感器公共探测区中目标做匀速直线运动，目标初始状态分别设置为：X（0）=3 000，10，5 000，12。162农业装备与车辆工程 2023 年选取目标位置估计的均方根误差（Root Mean Square Error，RMSE）作为算法性能评价指标；仿真软件为 MATLAB R2019a。3.2 仿真结果及分析传感器存在时间偏差和系统偏差的情况下，图 2 为 2 种算法在 X、Y 轴方向上的目标状态估计的RMSE曲线，图3为目标位置估计的RMSE曲线。由图 2 和图 3 可知，由于 ASEKF 没有考虑测量存在时间偏差的情况，其滤波误差相比本文算法误差较大，且随着仿真步数的增大而增大，本文所提出的算法则随仿真步数的增加趋于稳定。4 结论本文针对传感器量测存在时间偏差和系统偏差的系统，提出了一种基于扩展卡尔曼滤波的多传感偏差补偿和状态估计算法，首先通过使用轨迹估计构造出伪测量方程对多传感器系统的时间偏移进行估计；然后将目标的运动模型和传感器偏差模型有机地结合起来，形成扩维的状态空间模型，再基于扩展卡尔曼滤波，实现状态与偏差联合实时估计。所提的算法在估计精度、算法实时性、算法鲁棒性等方面远超 ASEKF 算法，为带有系统误差和时间误差的非线性状态估计问题提供了一种解决方案。参考文献1 BU Shizhe,ZHOU Chang,ZHOU Gongjian.Simultaneous spatiotemporal bias and state estimation for asynchronous multi-sensor systemJ.The Journal of Engineering,2019,2019(19):5824-5828.2 何友,王国宏,关欣.信息融合理论及应用 M.北京:电子工业出版社,2010.3 LI Song,CHENG Yongmei,BROWN D,et al.Comprehensive time-offset estimation for multisensor target trackingJ.IEEE Transactions on Aerospace and Electronic Systems,2020,56(3):2351-2373.4 HE You,ZHU Hongwei,TANG Xiaoming.Joint systematic error estimation algorithm for radar and automatic dependent surveillance broadcastingJ.IET Radar Sonar and Navigation,2013,7(4):361-370.5 HUCK T,WESTENBERGER A,FRITZSCHE M,et al.Precise timestamping and temporal synchronization in multi-sensor fusionC.2011 IEEE Intelligent Vehicles Symposium(IV).Baden-Baden,Germany:IEEE,June 2011:242-247.6 NABAA N,BISHOP R H.Solution to a multisensor tracking problem with sensor registration errorsJ.IEEE Transactions on Aerospace and Electronic Systems,1999,35(1):354-363.7 宋强,何友,董云龙.一种目标状态与系统偏差的联合估计算法 J.弹箭与制导学报,2007(4):312-315,322.8 宋强,何友,杨俭.基于 UKF 的目标状态与系统偏差的联合估计算法 J.弹箭与制导学报,2007(3):311-313,316.9 刘瑜,何友,王海鹏,等.基于平方根容积卡尔曼滤波的目标状态与传感器偏差扩维联合估计算法 J.吉林大学学报(工学版),2015,45(1):314-321.10 LI Zhenhua,CHEN Siyue,LEUNG H,et al.Joint data association,registration,and fusion using EM-KFJ.IEEE Transactions on Aerospace and Electronic Systems,2010,46(2):496-507.11 LIN Xiangdong,BAR-SHALOM Y,KIRUBARAJAN T.Exact multisensor dynamic bias estimation with local tracksJ.IEEE Transactions on Aerospace and Electronic Systems,2004,40(2):576-590.作者简介 杨权（1997-），男，硕士研究生，通信工程专业，研究方向：信息融合。E-mail：（b）图 2 不同坐标方向各算法的 RMSE 随时间变化曲线Fig.2 RMSE curves with time for each algorithm in different coordinate directions（a）X 轴方向（b）Y 轴方向0 10 20 30 40 50 60 70 80 90 100t/s1086420RMSE/m本文算法ASEKF（a）10864200 10 20 30 40 50 60 70 80 90 100t/sRMSE/mASEKF本文算法图 3 目标位置各算法的 RMSE 随时间变化曲线Fig.3 RMSE curves with time for each algorithm at the target locationRMSE/m0 10 20 30 40 50 60 70 80 90 100t/sASEKF本文算法1086420