遮挡情况下刚体位姿估计的自适应无迹卡尔曼分布式融合

冯远静,黄良鹏,张文安

(浙江工业大学信息工程学院,浙江杭州 310023)

1 引言

基于视觉传感器的刚体位姿估计是计算机视觉领域中一个重要的研究方向,广泛应用于机械臂跟踪[1-2]、手术器械位姿估计[3-6]、风洞模型姿态测量[7]、机器人导航[8-10]等领域.

目前,大部分位姿估计的研究都是在相机内参已知的前提下,通过采集图像并加以处理,得到二维特征信息,利用二维、三维坐标系间的对应关系设计位姿估计方法,从而实现对目标位姿的估计.在现有的研究方法中,大部分研究都是基于点特征的,也存在一些相对成熟的点特征位姿估计方法,例如正交迭代(orthogonal iteration,OI)[11]、高效n 点透视(efficient perspective-n-point,EPnP)[12]、自适应扩展卡尔曼(adaptive extended Kalman filter,AEKF)[13]以及迭代自适应扩展卡尔曼(iterative adaptive extended Kalman filter,IAEKF)[14].OI正交迭代和EPnP的估计精度过分依赖观测噪声.当噪声过大时,估计精度便会急剧下降[15],因此人们提出了AEKF[16],通过对每一个采样步长中的过程噪声以及观测噪声进行采样从而克服环境中因噪声过大或者环境异变时产生的影响,使得系统更加鲁棒.与AEKF相比,IAEKF通过将AEKF的结果进行迭代,得到最小的估计方差,提高了精度,但同时也使得计算量大幅增加.在某些状况下,如初始值误差过大、相机标定误差过大,基于单个视觉传感器的位姿估计方法所得到的结果误差将会过大甚至发散[17].从而出现了基于多个视觉传感器的集中式融合方法[18],通过结合多个视觉传感器采集的图像信息,并利用AEKF或IAEKF对目标进行位姿估计,一定程度上弥补了单个视觉传感器方法的缺陷,提高了系统估计精度,但观测矩阵维数会随着观测信息的增加而上升,计算量也会随之成平方增长,造成系统的响应速度大幅下降[19],并且对于多视觉传感器位姿估计系统,若某个局部子系统出现遮挡,集中式卡尔曼方法将会失效.

而分布式融合估计方法先对各个子系统单独进行估计,然后将估计结果融合.对于缺失观测数据的子系统,通常只对其进行状态预测,再将剩余子系统的估计结果与其预测结果融合[20],然而在观测数据连续丢失的情况下,相应子系统的估计结果将过度依赖于模型,往往导致跟踪偏差过大.遮挡过程正是一个观测数据连续缺失的过程,因此这类方法不适用于遮挡情况下多相机位姿估计系统.为了解决遮挡情况下多视觉传感器对刚体的位姿估计问题,本文针对多视觉传感器位姿估计系统的特点,提出了一种基于自适应无迹卡尔曼滤波的分布式融合方法,可有效地解决多相机位姿估计系统中的遮挡问题.首先,建立了多相机系统的状态空间模型,进而设计了AUKF和矩阵加权分布式融合估计方法,然后将遮挡情况划分为部分遮挡情况和严重遮挡情况,并针对不同遮挡情况提出了对应的解决方案.系统未出现遮挡时,利用AUKF求出相机子系统的状态信息,进而筛选出基准估计值,通过引入改进的Sage-Husa噪声估计器[21]更新过程噪声.相机子系统出现严重遮挡时,该时刻不对其进行计算,等待剩余子系统融合结束,利用融合结果对其进行初始化;子系统出现部分遮挡时,利用基准估计值和先验知识估计出缺失的特征点坐标,继而利用无迹卡尔曼滤波(unscented Kalman filter,UKF)获取子系统状态信息.结合正常、部分遮挡子系统的状态信息,利用分布式融合方法得到最终结果,实现对运动刚体的位姿估计.最后,通过仿真获取了区分遮挡情况的阈值,并进一步通过实验验证表明,所提出的设计方法能够有效提升系统在遮挡情况下的估计精度与鲁棒性.

2 问题描述

2.1 投影模型

考虑一类有多个特征点位于平面上的投影模型,如图1所示.点O为像素坐标系原点,位于图像左上角;U,V 两轴分别穿过原点,平行于图像的水平边缘与垂直边缘;点OI为图像坐标系原点,位于图像中心;XI,YI两轴分别平行于U,V 两轴;点Oc为相机坐标系原点,位于相机的光心;Zc轴穿过相机光心,垂直于图像平面,与图像交于图像坐标系原点OI;Xc,Yc轴分别平行于XI,YI轴.物体固定于一个六自由度并联平台,将物体下表面中心视为物体坐标系原点,与并联平台坐标系重合.在本模型中,相机位姿保持不变,通过控制六自由度并联平台实现目标刚体的运动,在实验前对相机进行标定,获得世界坐标系和相机坐标间的转换矩阵.

图1 特征点在图像平面的投影示意图Fig.1 Projection sketch of feature points on image plane

刚体上标有8个特征点,假设第j(j=1,···,8)个特征点Pj在物体坐标系和相机坐标系中的坐标分别为,且转换关系如下:

其中:cRb为坐标系Ob−XbYbZb与Oc−XcYcZc之间的旋转矩阵;为平移向量,表示物体坐标系原点相对于相机坐标系原点的位置关系.

像素坐标系与相机坐标系下特征点之间的关系可以通过针孔模型描述:

其中:[ujvj]T表示第j个特征点在像素坐标系下的坐标;f为相机焦距;dx,dy分别为单个像素点在图像坐标系中沿x轴、y 轴方向的物理距离,可通过标定得到相机内参,即fx=f/dx,fy=f/dy,u0,v0,(u0,v0)表示图像中心点在像素坐标系下的坐标.为了方便与六自由度并联平台反馈的位姿作对比,利用物体坐标系原点在世界坐标系下的位姿表示系统状态量.由于Haug[22]提出,对于位姿估计系统,使用二阶加速度模型具有更好的估计结果,因此采用二阶加速度模型来描述刚体的运动.

2.2 位姿估计系统描述

考虑一类基于多相机的运动刚体位姿估计系统,其中运动刚体的运动学模型描述如下:

其中:∆t为系统采样时间,I3×3为3×3的单位矩阵.

对于第i(i=1,···,N)个相机子系统,其观测模型如下:

其中:r为量测噪声,满足均值为0、协方差为R的高斯分布;

通过标定可求得k1,k2,p1,p2四个参数;可通过状态信息以及先验知识获得:

其中Rod1,Rod2,Rod3满足如下关系:

本文将采用多相机融合和自适应UKF分布式滤波方法解决如图1所示的存在遮挡情况的运动刚体位姿估计问题.

3 基于AUKF的分布式融合方法

3.1 基于AUKF的局部滤波器设计

由式(3)和(5)可知,运动刚体位姿估计系统是一个非线性系统,一种简便的处理方法是将系统进行线性化后再做进一步处理,而在忽略高阶项的同时,势必会使系统产生较大误差.UKF[16]使用确定性采样策略逼近非线性函数的概率密度分布,计算精度较高[23],且无需求解雅克比矩阵[24],故本文采用UKF作为系统局部滤波器.

由于刚体运动过程中系统噪声未知,故不能简单地将过程噪声协方差Q 定为一个常数,Min等[21]针对系统噪声时变的系统提出了改进的次优Sage-Husa方法,通过引入噪声估计器,更新系统噪声,一定程度上补偿了系统噪声的影响.然而同时更新Q和观测噪声协方差R可能导致结果发散,可通过实验事先校准获得较准确的R[21].

本文所设计的基于AUKF的局部滤波器设计步骤包括:

步骤1初始化.

步骤2预测.

同时,计算其权值:

式中:n 是状态维数,在系统(3)中,n=18;xi(i=0,1,···,2n)为选定的sigma点;分别为计算均值及其协方差的权值系数;λ=α2(n+κ)−n,α通常设为一个较小的正数,设置为1e−4;κ是一个比例参数,设置为3−n.

将sigma点通过非线性变换对系统状态进行预测,得到各个sigma点的状态预测值:

式中:A是状态转移矩阵;利用sigma点的状态预测和均值的权值系数,得到系统状态预测值:

同时,获取各个sigma点的状态预测误差:

类似地,可以得到各个sigma点的观测预测值、系统观测预测值及各个sigma点观测预测误差:

步骤3更新.

利用各个sigma点状态预测误差、观测预测误差和协方差权值系数求取状态误差方差矩阵、观测误差方差矩阵及状态观测之间的互协方差矩阵:

进而获取系统的卡尔曼增益:

在状态预测值基础上,利用k时刻的量测信息便可获得更新的状态估计值和状态误差方差矩阵:

步骤4调整过程噪声协方差Q.

将改进的Sage-Husa噪声估计器应用于UKF:

其中:da,k−1=(1−b)/(1−bk);b为遗忘因子,满足0

修正系统过程噪声后，再执行步骤2.

3.2 矩阵加权分布式融合估计器设计

在卡尔曼滤波器中,若使用集中式融合,将会增加观测矩阵维数,从而造成计算量大幅增加.而分布式融合通过将各个局部估计结果按照一定的方法进行加权,从而得到全局最优值,同时避免了观测扩维,减少了计算量.在线性情况下,下列所述的矩阵加权分布式融合已被证明等价于集中式融合[25],该方法也适用于非线性系统,以AUKF为局部估计器的矩阵加权分布式融合方法可描述如下:首先,根据先验信息计算k时刻系统的融合状态预测值以及对应的误差协方差,参考UKF 中对状态预测以及误差协方差的计算,求取相应的sigma点:

进而求取融合状态预测值和预测误差:

进而求取融合状态预测协方差:

然后结合每个相机子系统的状态估计值、状态预测值、误差协方差估计值及其预测值,更新融合后的状态估计值以及融合误差协方差矩阵:

式中:i=1,2,···,L,L 表示局部子系统的个数;分别表示k时刻第i个局部子系统状态的估计值和预测值;分别为对应的误差协方差.

4 遮挡情况下的位姿估计方法

在进行位姿估计的过程中,常常会因为眩光、外部噪声干扰、人为干预等问题而出现特征信息被遮挡或无法识别的情况.对于视觉传感器,遮挡的产生会造成目标特征信息不稳定甚至丢失.而对于位姿估计算法而言,其关键在于利用足够的特征信息来完成对目标的估计,因此遮挡的出现会给刚体位姿估计带来很大的困难.本文根据局部相机子系统观测到的特征点个数对遮挡情况进行区分,假定n为刚体上特征点的总个数,ns为局部子系统当前时刻能够观测到的特征点数量,nL为阈值,用于区分严重遮挡和部分遮挡,通过多次仿真获得.若局部子系统满足nL≥ns

4.1 严重遮挡下的改进策略

严重遮挡情况下,有效特征点数量过少,相应相机子系统的估计结果无法对整体系统估计精度作出有效贡献.因此对于当前时刻出现严重遮挡的子系统,本文不对其进行单独估计,等待其他子系统融合完成后,利用融合结果对该子系统进行初始化.

假设在第k时刻,第j个局部相机子系统出现严重遮挡.对部分遮挡和无遮挡的子系统进行局部估计,并利用分布式融合方法求得状态估计值和状态误差协方差,然后对出现严重遮挡的子系统进行初始化:

从而保证该局部相机子系统在初始化时获得较高精度的初始值,使得局部滤波器能够正常运行.

4.2 部分遮挡下的改进策略

部分遮挡情况下,目标刚体上的部分特征点的观测信息缺失,此时无法直接利用上文所提的卡尔曼滤波器进行位姿估计,为此,本文提出一种改进的方案,通过利用先验信息对遮挡处的特征点进行修复,间接获得其观测值,提高了数据利用率,同时改善了估计结果.

定义1假设在第k个时刻,未出现遮挡的局部子系统有m个,利用UKF分别求得各个局部子系统的状态估计值及其误差协方差矩阵,分别记为x1,x2,···,xm和P1,P2,···,Pm,计算各个局部子系统误差协方差的迹,并将与最小的协方差迹对应的状态估计值称为基准估计值,用下式表示:

其中:tr(Pi)为第i个局部子系统所求得的误差协方差的迹;s表示局部子系统的序号,即对于第s个局部子系统,若其误差协方差的迹是最小的,那么其状态估计值xs被称为基准估计值.

假设在第k个时刻,第j个出现部分遮挡的子系统中,目标刚体上第l个特征点被遮挡,该特征点在物体坐标系下的坐标为.针对部分遮挡情况,本文处理方法如下:

步骤1对于未出现遮挡的m个局部子系统,利用UKF估计其状态值和误差协方差,进而计算基准估计值xs,并利用第s个系统更新过程噪声协方差Q.

步骤2基准估计值xs包含位置向量及姿态向量,分别表示为(α,β,γ)将姿态向量代入式(11),可计算获得物体坐标系相对于世界坐标系的旋转矩阵,从而可将其坐标转换矩阵表示为.通过标定可获得每个子系统中世界坐标系相对于相机坐标系的旋转矩阵cjRw和平移矩阵cjtw,从而可将其坐标转换矩阵表示为.已知第l个特征点在物体坐标系下的位置,则可通过下式获得该点在世界坐标系下的坐标

该点在相应相机坐标系中坐标Pcjl 可进一步由下式求得:

相应坐标系之间转换关系如图2所示.

图2 相应坐标系之间转换关系Fig.2 Conversion relations between corresponding coordinate systems

步骤3利用相机坐标系与像素坐标系之间的转换关系,修复被遮挡的特征点,通过下式可求得被修复的特征点坐标(ul,vl):

图3 补全特征信息Fig.3 Completion of characteristic information

步骤4补全被遮挡的特征点后,利用UKF估计局部子系统状态信息.

综上所述,遮挡处理流程如图4所示:假设共有M个局部相机子系统,首先根据观测信息判断每个子系统的遮挡情况,若子系统处于严重遮挡状态,则放弃对该子系统进行计算,并等待剩余子系统融合结束,反之,则判断是否处于部分遮挡状态,若特征点均未被遮挡,利用UKF估计局部子系统的状态信息,进而筛选获得基准估计值,并更新过程噪声协方差Q;若处于部分遮挡状态,利用基准估计值以及先验知识修复该子系统中被遮挡的特征点然后再通过UKF计算获得该子系统的状态信息.利用分布式融合方法,结合未出现遮挡的局部子系统的状态信息,计算获得k时刻最终的融合结果,最后利用该融合结果对处于严重遮挡状态的局部子系统进行初始化.

图4 遮挡处理流程图Fig.4 Flow chart of processing scheme under occlusion

5 实验

5.1 平台搭建

本文所用实验平台包括一台由PI公司生产的精度达0.1µm的六自由度并联平台、一台配套的并联平台控制器、一台PC机以及多台普通固焦相机.六自由度并联平台可在一定范围内任意移动.相机型号为今贵S9,能够采集分辨率为640×480的图像,如图5所示.

实验场景如图6所示.用一个带有8个特征点的高精度定制方木块作为被跟踪刚体,固定于高精度定制角件上,并将其安装于六自由度并联平台,从而刚体可随六自由度并联平台的移动而移动,且刚体与并联平台相对位姿保持不变.固焦相机通过云台固定于三脚架上,并保持相机和三脚架位置、姿态不变.

图5 实验平台Fig.5 Experimental platform

图6 实验场景Fig.6 Experimental scene

六自由度并联平台精度可达0.1µm,因此可将其反馈的位置和姿态信息作为参考真值.实验过程中,通过固焦相机采集目标运动图像,发送至上位机系统处理,进而利用所提算法对目标刚体进行位姿估计,同时,通过六自由度并联平台控制器,获取同一时刻六自由度并联平台的坐标并保存.

六自由度并联平台坐标系的原点位于上圆盘中心位置,且其X,Y,Z轴方向保持固定.实验前,通过控制器对并联平台进行初始化,将其位移与姿态值置零.并联台移动时,控制器向PC机反馈六自由度并联平台坐标系相对于初始时刻改变的位姿.实验中,为方便与参考真值对比,令物体坐标系Ob−XbYbZb与六自由度并联平台坐标系OS−XSYSZS重合,同时令世界坐标系Ow−XwYwZw与初始时刻六自由度并联平台坐标系重合,并将世界坐标系下物体坐标系原点的位姿作为系统估计状态值,从而使计算获得的估计值与真实值直接进行对比,如图7所示.

图7 世界坐标系与六自由度并联平台坐标系之间关系Fig.7 The relation between world coordinate system and 6-DOF parallel platform coordinate system

5.2 仿真实验

采用带有两个局部相机子系统的系统进行仿真,并利用UKF进行局部估计.仿真时,使其中一个局部子系统始终处于未被遮挡状态(简称为正常子系统),对另一个局部子系统进行遮挡处理,使其只能观测到部分特征点(简称为非正常子系统).对缺失0至7个特征点的情况依次进行仿真,记录系统融合结果和正常子系统的估计结果,仿真结束后,计算正常子系统的平均误差以及每次系统融合误差.通过对比分析两者结果,从而选择合适的阈值,以此来区分子系统处于何种遮挡情况.

对于正常子系统,通过下式计算仿真后的平均状态误差:

其中:j为当前缺失特征点的个数,Nf表示实验场景中的特征点总数,Ns为每种缺失特征点的仿真状态下的仿真次数,Nk表示每次仿真的观测采样次数,表示局部子系统的状态估计值,对应于缺失j个特征点的仿真实验中的第i 个采样时刻,表示在该时刻的状态参考真值.系统分布式融合误差通过下式计算获得:

仿真时,相机水平焦距设定为fx1=fx2=500,垂直焦距设为fy1=fy2=500,光心横坐标设置为u01=u02=300,纵坐标为v01=v02=200,采样周期∆t=0.05 s,系统噪声协方差的初值设为Q0=0.1×diag{0,0,0,5,5,5,0.1,0.1,0.1,0,0,0,2,2,2,0.1,0.1,0.1},观测噪声协方差初值设为R0=0.1×I16×16,I16×16为16×16的单位矩阵,设置系统的初始方差为P0=I18×18,I18×18为18×18的单位矩阵,遗忘因子b=0.9.世界坐标系相对于相机1和相机2坐标系的坐标转换矩阵分别设置为

针对本文实验场景,特征点总数Nf=8,每种仿真状态进行1000次仿真,即Ns=1000,每次仿真时的观测采样次数设置为Nk=1000.

图8和图9分别展示了系统融合结果的位置误差和角度误差与缺失特征个数之间的关系.

图8 系统融合结果的位置误差与缺失特征点个数关系Fig.8 The relationship between the position error of fusion results and the number of missing feature points

图9 系统融合结果的角度误差与缺失特征个数关系Fig.9 The relationship between the angle error of fusion results and the number of missing features

从中可以获得以下结论:1)系统融合结果的误差随缺失特征点个数增加而增大;2)当目标特征点总个数Nf=8,且缺失的特征点个数不超过2个时,融合结果平均误差小于正常子系统的平均估计误差.分析可得,当遮挡的特征点个数超过2个时,通过修补特征点获取丢失观测信息的非正常子系统无法对整体系统产生有效贡献.因此针对本文实验场景,用于区分严重遮挡和部分遮挡的阈值设定为6,即nL=6.对于特征点总数为Nf的实验场景,可根据上述仿真方法计算正常子系统的平均误差以及每次系统融合误差,获取误差大小与缺失特征点个数之间的对应关系,从而进行阈值选取.阈值选取的原则为:当局部子系统能够观测到特征点数量不小于阈值时,系统融合误差不大于正常子系统的平均误差,即满足nL≥ns

5.3 实际实验

实验采用两个相机对目标刚体进行位姿估计.两个相机的内部参数通过摄像机标定后获取,其各项数值分别如下:=813.3459,=803.1460,=813.0550,=803.0452,=358.9625,=229.8308,=380.9250,=233.6312;畸变参数为=0.2124,=−0.0145,=0.235,=−0.0084,====0.设置过程噪声协方差初始值均为

每个局部子系统的观测噪声协方差初始值均为R0=0.05·I16×16,其中:I16×16为16×16的单位矩阵,遗忘因子b=0.9,六自由度并联平台的初始位置和姿态都为0,因此系统的初始状态量为零向量,设置系统的初始方差为P0=0.01·I18×18,为18×18的单位矩阵.世界坐标系相对于相机1与相机2坐标系的坐标转换矩阵分别为

目标刚体上标记的8个特征点在物体坐标系下的坐标为(35,−80,50),(35,−10,50),(−35,−10,50),(−35,−80,50),(20,−65,50),(20,−10,50),(−20,−65,50),(−20,−25,50),单位为mm.在实验过程中,利用不透明的物体对两个相机镜头轮流进行遮挡,遮挡过程效果如图10所示.

图10 遮挡过程Fig.10 Occlusion process

在实验效果图中,为了方便观察位姿估计结果,将出现遮挡的局部子系统对应状态量设置为零.图11展示了遮挡情况下目标的重投影效果图,其中实线部分表示利用本文方法获得位姿,虚线部分表示出现遮挡的子系统所对应的位姿.

图11 遮挡情况下的重投影效果图Fig.11 The re-projection rendering under occlusion

图12展示了带有遮挡的情况下目标位姿的估计结果,分析图像后可得到以下结论:

1)本文算法在遮挡环境下依然能够有效地跟踪并且估计目标刚体的位姿;

2)当某一局部子系统出现遮挡时,系统整体融合结果并不会与另一有效的局部估计结果完全重合,原因在于融合过程中综合了另一局部估计信息和之前的先验知识,使其得到自己的结果,但是随着时间的推移,系统整体融合结果会趋向有效的局部估计结果.

图12 各个轴跟踪轨迹Fig.12 Tracking trajectory of each axis

为了客观验证算法的有效性,对系统的累积误差进行了统计.为计算方便,令某一时刻出现严重遮挡的子系统的误差与该时刻系统整体融合结果的误差相等.通过图13可以看到,本文算法在遮挡情况下的累计误差小于每个局部估计的累计误差.从图14可以发现,本文算法在整个跟踪过程中的累积误差小于局部子系统使用AUKF的累积误差,证明了所提出算法的有效性.综上所述,过程噪声自适应分布式融合方法在系统出现遮挡的情况下仍然能有效地估计目标刚体的位姿,且其估计结果优于各个局部估计.

图13 遮挡时刻各个轴累计误差Fig.13 Accumulative error of each axis during occlusion time

图14 各个轴累计误差Fig.14 Accumulative error of each axis

6 小结

针对在刚体位姿估计过程中出现的遮挡情况,本文提出了AUKF 分布式融合方法,通过引入改进的Sage-Husa噪声估计器更新过程噪声,并针对不同的遮挡情况采用对应的方案,详细描述了所提出的两种遮挡问题的处理方法,最后,通过仿真确定了区分部分遮挡和严重遮挡的阈值,并进行实验验证了该方法具有很强的鲁棒性和有效性.本文所提方法适用于遮挡情况下的刚体位姿估计,后续工作将针对遮挡情况下的手术器械位姿跟踪进行深入研究.