单基站基于波达角度的刚体位置姿态最大似然估计

姚晓峰,吴圣兰,周彪,庞敏

(1. 江苏省物联网应用技术重点建设实验室(无锡太湖学院),无锡 214122;2. 江南大学 物联网工程学院,无锡 214122)

0 引 言

刚体目标是指一种在运动或受力时仍然保持其外部拓扑结构的物体,包括航天器、虚拟现实应用中使用的头盔或智能机器人．刚体位置和方向估计方案用来估计刚体相对于参考状态的中心坐标和旋转欧拉角[1-2]．与传统的获取目标的点位置不同,刚体定位(RBL)旨在同时获取刚体的位置和姿态信息．

刚体的高精度位置和姿态信息在航天、军事和智能工业领域有着广泛的应用前景[3-6]．在虚拟现实应用中,虚拟现实系统必须根据用户头部的准确位置和姿态提供相应的虚拟图像．在室内精密导航系统[7]中,车辆的位置和方向是智能停车系统所需要的基本信息．航天体的对接也需要精确的航天器姿态和相对位置信息．

目前,基于导航卫星的定位与姿态测量系统是较为主流的室外大型RBL测姿方案[8-9]．卫星载波相位所涉及的整周模糊度分辨问题影响了此方案的稳定性．另外,其应用在非卫星视距场景下的限制也较为明显．利用视觉图像处理的位置和方向估计方案是RBL测姿的一种常用方案[10]．但是该方案的精度过分依赖于特征点的低误差坐标观测值,并且视觉图像处理的技术特点也导致其对光照条件的鲁棒性差,同时计算量较大．

基于小规模无线传感器网络的RBL与姿态测量方案具有实现成本低、对室内外环境和光照条件适应性强等优点[11-12]．基于小规模传感器网络估计方案的实现可以总结如下:首先,将无线传感节点附着在刚体表面,形成一个拓扑结构已知的小规模传感网络;然后,基站对网络中的每个无线节点的信号到达特征进行观测;最后,通过优化算法融合上述信息得到刚体的三维坐标和方位信息．然而,由于含有姿态信息的三维特殊正交群必须满足酉矩阵的单位正交条件,如何优化该模型成为一个更具挑战性的问题．目前,基于无线传感器网络的定位测姿估计方案的研究主要是基于到达时间(TOA)的观测．然而为了保证系统的精度,基于TOA的定位测姿方案需要3～4个基站,增加了系统实现成本的同时也限制了其应用场景．并且由于基站与传感器节点之间存在严格的时钟同步问题,基于TOA的定位与姿态估计方案的实用性较差．

基于观测到达方向(DOA)的定位测姿估计方案可以完全避免时钟同步问题,提高系统的可行性．除此之外,在基站获得的二维DOA信息比TOA信息具有更高的维数,这使得单基站定位方向成为可能,成熟的DOA估计方法也为基于DOA的定位测姿方案奠定了实施基础[13-15]．

1 单基站基于波达角度的刚体定位

框架

本节将介绍基于DOA测量的单基站RBL框架．为了确定三维空间中刚体的状态(包括位置和姿态),在其表面安装无线节点作为特征点,形成一个小规模的无线传感器网络．此传感网络的拓扑信息事先是完全知道的,并且在移动和旋转时保持不变．

方便起见,将单基站位置设置为原点．单基站上配备了一个二维天线阵列进行测量来自传感网中每个节点的信号到达的方向．为了描述无线传感器网络当前的状态,需要预先定义一个已知的参考帧(帧由各个节点的三维位置构成的)．当前帧,即当前刚体的状态,假设是通过参考帧旋转和平移得到(如图1所示),在数学上表示为

E=QC+t⊗1(1×K),

(1)

式中，C=[c1,…,cK]∈R(3×K)和E=[e1,…,eK]∈R(3×K)分别表示参考帧和当前帧．在C和E中,第k个节点的三维坐标位置分别表示为ck=[cx,k,cy,k,cz,k]T和ek=[ex,k,ey,k,ez,k]T．在式(1)中,t=[x,y,z]T是未知的平移向量;Q是未知的旋转矩阵,属于三维特殊正交矩阵

(2)

对于广义RBL问题,其任务是估计旋转矩阵Q和平移向量t．从而通过求出未知的当前帧和已知的参考帧的关系来描述刚体当前的位置和姿态．

传感器网络的拓扑信息是精确已知的,且在旋转平移中不变,那么可以用节点对的欧氏距离来描述此拓扑信息

图1 单基站刚体定位测姿场景

d=[d1,2,…,di,j,…,dK-1,K]T,i,j=1,…,

K,i

(3)

式中：di,j=ci-cj=ei-ej是第i和第j节点之间的欧式距离；d是一个×1向量．

另一个信息是传感器节点到基站原点的波达角度．即当前帧中第k个节点ek信号传播至原点的方向与x轴和z轴之间的夹角为αk和βk．为了方便起见,将αk和βk映射为投影角θ=[θ1,…,θK]T,即信号传播至原点的方向和平面xoy和zoy的夹角,其映射方法表示如下:

(4)

其中

θ=[θ1,…,θK]T,

(5)

2 基于DOA的RBL框架的最大似

然估计

为了估计表示刚性目标的姿态和位置的旋转矩阵Q和平移向量t,本节介绍了两种最大似然估计算子从不同的角度计算旋转矩阵和平移向量．第一种算法是先求出各个节点的三维坐标E,然后用单位四元数法求出平移向量和旋转矩阵．另一种方法是将平移向量和旋转矩阵作为未知量直接求出．

2.1 拓扑匹配最大似然估计

第一种基于最大似然估计的RBL框架建立分为两个阶段:建立最大似然估计来计算当前帧中节点的三维坐标,然后采用点匹配算法找到Q和t．

首先使用投影角作为量测,是为了简化当前帧中的节点坐标ek和θk之间的关系:

ek=[tanθk,x,1,tanθk,z]T*ey,k.

(6)

如此一来,当前帧的3×K个三维坐标未知数减少为1×K个未知数,即

φ1=ey=[ey,1,…，ey,k,…,ey,K]T．

然后利用无线节点的确切已知拓扑信息d建立最大似然函数．d和待估计值φ1的关系为

d(φ1)=ei-ej,i,j=1,…,K,i

(7)

(8)

(9)

利用高斯-牛顿法求出使得式(8)最小的ey的解,将ey代入(6)即可得到无线节点在当前帧中的所有节点的三维坐标．最后,利用单位四元数法或奇异值分解法,即可得到刚体的平移向量和旋转矩阵．

实质上,拓扑匹配最大似然估计是通过获取K传感器的ey坐标来实现RBL的,其匹配信息是K(K-1)/2个距离．而待估计的参数个数为K个ey,k,故K>3为估计成功的必要条件．

2.2 测量匹配最大似然估计

第二种最大似然估计是直接计算旋转角度和平移向量,要匹配的信息是DOA信息,即称测量匹配最大似然估计．现场实施过程中可以精确测得节点对距离信息,然后利用多维尺度算法[16](MDS)对节点距离信息进行计算处理得到参考帧(在本文的仿真工作中,参考帧是预先确定的,而不是通过MDS算法来融合节点对距离得到)．

旋转矩阵可以由三个欧拉角Υ=[φp,φy,φr]T决定,φp,φy,φr分别为俯仰角、偏航角和翻滚角．三个角度决定了旋转矩阵为:

Q(Υ)= [r1,r2,r3]T=

(10)

式中,s*、c*分别代表sinφ*以及cosφ*．因此,包括平移向量t=(x,y,z)T在内,有6个未知数φ2=[ΥT,tT]T．由于距离已被融合以获得参考帧,唯一可用的条件是DOA测量值．K个节点产生2×K个DOA．根据式(1)和式(6),DOA测量值与6个未知量之间的关系可以表示为θ=θ(φ2),具体为

(11)

由于波达方向是独立测量的,并且与零均值高斯白噪声混合(见式(5)),θ的最大似然函数可以表示为

(12)

(13)

得到最优解φ2后,把γ代入方程(10)得到Q．这就完成了t和Q估计．

需要注意的是,通过分析使用条件和要估计的参数的数量,上述两个估计需要刚体表面至少绑定三个或以上的无线节点,方可来完成RBL估计任务．

3 最大似然估计高斯-牛顿算法

为了寻找使代价函数(9)和(13)最小化的最优解,可以采用多种优化方法．在这里高斯-牛顿算法被用来实现这个任务．如图2所示.

图2 高斯-牛顿迭代法中的初值重置

高斯-牛顿迭代法的基本思想是用泰勒级数展开法逼近非线性回归模型,然后经过多次迭代,多次修改回归系数,使回归系数不断逼近非线性回归模型的最佳回归系数,最后使原模型的残差平方和最小．上述两种最大似然估计问题可以表示为

(14)

(15)

这是一个非线性最小二乘问题．

为了将此最小二乘问题线性化,在初始值φ0处对式(7)和式(11)进行泰勒级数展开,并忽略阶数大于2的项,即可得到

(16)

在第t次迭代时,当前信息向量φ(t)和最优解φ之间的差距可以从等式(17)中获得

Δ=φ-φ(m)=(G*TG*)-1G*T(Θo-Θ(φ(t)))

(17)

在下一次迭代中,可以通过替换φ(t+1)=φ(t)+Δ来更新迭代值．上述迭代直到优化收敛ΔTΔ降低到小阈值以下或达到最大迭代次数为止．

从经验上讲,传统的牛顿法在初值φ(0)不接近基准值时容易达到局部最优,因为所述RBL模型为非凸模型．因此可以通过增加收敛性判据对经典牛顿法进行了修正．在停止迭代后,将计算最终得到的估计值φ(tmax)与通过‖Θo-Θ(φ(tmax))‖实际测量的信息向量之间的相似度ρ;同时对估计值进行判断是否符合先验信息范围．如果ρ大于预先设定的阈值或者不符合先验信息范围,则丢弃收敛结果,根据先验知识随机设置φ(0)重置φ,重新开始牛顿迭代．

待估计的参数,包括拓扑匹配方案的ey、DOA匹配方案的t和Q．作为空间相关参数的先验知识,它们的范围可以根据监测区域的房间大小粗略确定或者通过接受信号强度进行测量．此外,DOA匹配方案的欧拉角γ的范围明显在0到2π(rad)之间．若ρ一直达不到阈值,则重置初始值500次后退出整个参数估计过程．

4 性能评估

本文对所提出的RBL方案在一个2.4 GHz四核处理器的PC机上用MATLAB进行了仿真实验,考虑了不同的情况下(不同的刚体尺寸D和不同的DOA测量噪声σ2),从收敛成功率和计算成本以及均方根误差(RMSE)三个方面评估了这两种最大似然估计子．仿真参数定为K=4,那么给出的初始帧如下:

(18)

作为刚性目标的先验状态信息,相对于初始帧,当前帧位于x∈[0,9]∩y∈[0,9]∩z∈[0,5] m的三维空间,单BS置于原点．同时,三个旋转角度从[0，2π]rad进行随机设置．

首先,比较了三种估计在6种不同噪声水平下当D=1 m和3 m的估计算法的鲁棒性．本文所提出的最大似然估计子对每一种场景分别进行1 000次独立的蒙特卡罗运行,在每一次运行中随机选择t和Q的基准值,并根据先验知识随机选择改进的高斯-牛顿法的初始解．因此,基准值和初始解的选择都是根据已知的先验信息范围内得到的,然后估计结果与基准值进行比对,以判断收敛的成功与否．

两种估计子的收敛成功率如图3所示．正如所期望,在DOA测量噪声水平和系统性能成反比的同时,较大的刚体尺寸下,两种估计方案也能获得较高的收敛成功率．这是因为较大的刚体尺寸也就意味着更大的DOA测量差值,也就提高了估计算法的鲁棒性．

横向来看,基于DOA测量匹配最大似然估计方案的鲁棒性优于基于拓扑信息匹配方案,前者的收敛成功率始终保持在95%以上,即使在最恶劣的情况下(当D=1 m时最高噪声水平σ=1 deg)．这是一个较为出色的性能,因为牛顿迭代法对于初值是相当敏感的,在改进的方案中,通过初值重置,基于DOA测量匹配最大似然估计也能具有良好的性能,而拓扑匹配方案的收敛成功率仍然较低．直观地说,从高斯-牛顿法的角度来说,匹配条件越多(在K=4个节点的情况下,测量匹配方案的8个条件,包括8个DOA测量值;拓扑匹配方案的6个条件,包括6个节点对距离),测量匹配最大似然估计方案对测量噪声的鲁棒性越强,因此DOA测量匹配最大似然估计方案性能高于拓扑匹配．图3也给出了RBL方法的计算成本．在一般意义上,待估计参数的维数越高,收敛速度越慢．然而可以看出基于测量匹配最大似然估计方案计算速度却更加有效．这是因为其较高的收敛成功率减少了牛顿算法中初始值重置的次数．相反,拓扑匹配方案显然需要最大的计算资源．还应注意到的是,DOA测量匹配最大似然估计方案在低级测量噪声下是有效的,而随着噪声的增加,其效率会降低．

(a)拓朴信息匹配最大似然估计方案

(b)DOA测量匹配最大似然估计方案图3 两种最大似然RBL估计算法的计算成本和收敛成功率分析

图4和图5展示了所有估计方案在D=1和D=3时的RBL精确度．本文选择拓扑匹配方案中的收敛成功结果计算RMSE,但与克拉美罗界相比仍有明显的差距,特别是对于小型刚体．这与上面的分析保持一致,即使用的匹配条件越多,性能越好．DOA匹配方案利用了更多的匹配条件,在较高的DOA噪声水平下可以达到克拉美罗界,但与克拉美罗界的性能差距越来越大．此现象可能是由于旋转角度与旋转矩阵之间转换过程中的高度非线性造成的．

DOA噪声标准差/deg(a)平移矢量t估计

DOA噪声标准差/deg(b)放置矩阵Q估计图4 当D=1 m时RBL估计精度比较

DOA噪声标准差/deg(a)平移矢量t估计

DOA噪声标准差/deg(b)旋转矩阵Q估计图5 当D=3 m时RBL估计精度比较

5 结论和展望

本文提出了基于DOA的单基站RBL框架的两种最大似然估计方法,一是通过匹配拓扑信息来估计当前帧中节点的坐标,然后估计位置姿态,称为拓扑匹配最大似然估计方案;二是利用DOA测量直接求出最优的平移向量和旋转角度,称为DOA测量匹配最大似然估计方案．采用改进的牛顿-高斯算法对两个MLE函数进行优化,根据先验信息对初值进行有规律的重置,以降低收敛失败率．最后,从收敛成功率、计算代价和精度方面对两种估计方案在不同情况下进行评估．结果表明,在不同情况下,第二种方案可以在保持95%以上的收敛成功率的同时,在高噪声情况下其RMSE近似于克拉美罗界．

这项工作只考虑到拓扑信息是精确已知的,当刚性目标来自第三方时,可能无法满足这一要求．因此,在未来的研究中,只需大致了解RBL的拓扑结构,便可以研究噪声拓扑信息对RBL性能的影响．在完成RBL任务的同时,只利用DOA测量和粗糙拓扑信息对噪声拓扑信息进行校正,将是一个有趣的研究课题．