一种单基站下基于波达角度的刚体定位测姿方法技术

本发明专利技术公开了一种单基站下基于波达角度的刚体定位测姿方法，属于刚体定位和姿态估计技术领域。所述方法使用波达方向DOA为观测量，在单基站情况下提出定位测姿方案，通过在单基站处装备尺寸合理的天线阵列，使其可以捕捉到刚体表面绑定的各个无线节点的三维DOA信息。以各个节点到达基站的DOA作为观测量，从而避开各无线节点与基站之间同步问题。采用在无线节点和基站间获得的三维DOA信息，结合已知的小范围WSN拓扑结构，在单基站下低成本地设计具有闭式解的定位测姿方案。本方案采用牛顿迭代法进行求解，相对于目前的基于DOA的PSO算法计算量更小，计算速度更快，适合实际的工程场景。

A method of rigid body orientation and attitude measurement based on DOA under single base station

【技术实现步骤摘要】
一种单基站下基于波达角度的刚体定位测姿方法
本专利技术涉及一种单基站下基于波达角度的刚体定位测姿方法，属于刚体定位和姿态估计

技术介绍
现有的技术中，定位和姿态测量往往被分为两个独立技术进行研究。然而，在很多场景下目标物体的位置和姿态是两个互相依存的信息要素，比如在虚拟现实系统，视觉接口需要根据头盔位置和姿态提供相应的图像；在智能机器人或者智能泊车应用中，系统要根据物体的位置和姿态航向规划前进路线；在航空航天的飞行器对接情境下，对接对象实时的位置和姿态更是其对接控制的必要因素等等。现存的定位测姿方案往往是基于卫星系统或者机器视觉图像处理的。然而基于卫星的定位测姿系统有着成本高、非卫星视距失效的问题；而基于视觉图像的位姿估计系统则受限于光线条件、计算量大、成本高、对光线条件的鲁棒性差等缺点。基于无线传感网络(WirelessSensorNetworks，WSN)的定位测姿方案可以克服以上传统定位测姿系统的诸多缺点。基于WSN的定位测姿方案是通过基站获取来自目标刚体的各种观测量，包括信号到达时间、波达角度等等，然后使用与观测量相对应的几何法则寻求此目标的状态信息。目前主流定位测姿方案是基于信号到达时间(TOA)的刚体定位测姿技术，其具体实现是通过多个基站同时测量来自各个无线节点的信号的到达时间，然后融合这些到达时间量测和无线节点的拓扑结构先验信息，实现旋转矩阵和转移矢量的估计。然而基于TOA的定位与姿态测量模型面临着严格时钟同步的实用限制，并且由于TOA所需基站数量过多，导致系统实施成本偏高，存在定位测姿模型复杂、易产生误差影响精度的缺点。当目标为刚体时，在目标刚体表面固定位置绑定小范围的WSN，把点目标转换成扩展目标(ExtendedTarget)，是基于WSN的刚体定位测姿方案的基本思路。现有方法多是基于TOA且需要通过多个基站才能够估计出目标刚体的位置和姿态，这种方法有严格的时钟同步要求并且在求解过程中存在定位测姿模型复杂、计算量大、容易产生误差影响精度的问题。
技术实现思路
为了解决目前目标刚体的位置和姿态估计存在的需要多个基站以及计算复杂。计算量大的问题，本专利技术提供了一种单基站刚体位置和姿态估计方法，所述方法包括：在目标刚体目标表面固定位置装置K个的无线传感器节点，获取其相对拓扑结构信息，K≥3；在单基站处布置二维天线阵列，用于测量各个无线传感器节点的信号来向；在基站处使用改进型牛顿迭代算法融合所述信号来向和无线传感器节点的相对拓扑结构信息解算各个无线传感器节点的三维坐标；通过奇异值分解确定目标刚体相对于已知参考状态的三维位置和姿态。可选的，所述方法还包括：以单基站所处位置为圆心，建立局部坐标系，所述刚体位置和姿态估计方法在所述局部坐标系下实现。可选的，所述以单基站所处位置为圆心，建立局部坐标系，包括：以单基站所处位置为圆心，以垂直于水平面方向为z轴、正东方向为y轴、正南方向为x轴建立局部坐标系。可选的，所述在单基站处布置二维天线阵列，包括：在局部坐标系的原点基站处安置天线阵列：为x、z方向分别等间隔安装M个天线阵元，总共2M-1个天线阵元；相邻天线阵元间的平均距离设定为所用无线传感器节点所发出窄带信号的载波半波长。可选的，获取目标表面固定位置装置的无线传感器节点相对拓扑结构信息，包括：测量K个无线传感器节点中任意两个节点之间的距离d＝[d1,2,...,di,j,...,dK-1,K,],i＜j，且i、j∈[1,...,K]，获得无线传感器的拓扑结构信息；其中K小于天线阵元数M。可选的，所述测量各个无线传感器节点的信号来向，包括：设定刚体参考状态C＝[c1,…,cK],ck＝[ck,x,ck,y,ck,z]T,k∈[1,K]，用以解释刚体的相对姿态和相对位置，其中，ck表示第k个无线节点的位置矢量，ck,x,ck,y,ck,z分别表示其x轴y轴及z轴坐标值；使用多尺度衡量算法预设刚体参考状态；以预设的刚体参考状态为参考，通过在基站处的天线阵列测量来自各个无线传感器节点的信号波达相位，并使用MUSIC算法对相位测量进行阵列信号处理得到信号的波达角度。可选的，所述使用多尺度衡量算法预设刚体参考状态，包括：S1.根据无线传感器的拓扑信息构建尺寸为K×K的对称平方距离矩阵其第i行第j列为：S2.构造定心矩阵令B＝-EDET/2，然后对B进行特征值分解得到B＝Xdiag(λ1,λ2,λ3)XT，其中λ1,λ2,λ3是B的非零特征解，X为λ1,λ2,λ3所对应的特征向量所组成的矩阵；S3.刚体参考状态下的各个无线传感器节点位置由计算得到。可选的，所述以预设的刚体参考状态为参考，通过在基站处的天线阵列测量来自各个无线传感器节点的信号波达相位，并使用现有的MUSIC算法对相位测量进行阵列信号处理得到信号的波达角度，包括：以方位角αk和仰俯角βk分别表示第k个无线节点信号的波达方向与x轴和z轴的夹角，第k个无线节点信号分别被在x方向和z轴方向布局的天线阵列测量得到，k＝1,...,K。K个节点的方位角和仰俯角分别记为α＝[α1,...,αK]和β＝[β1,...,βK]。可选的，所述在基站处使用改进型牛顿迭代算法融合所述信号来向和无线传感器节点的相对拓扑结构信息解算各个无线传感器节点的三维坐标，包括：将方位角αk、仰俯角βk和第k无线节点三维坐标(xk，yk，zk)的关系表示为：使用无线传感器节点的拓扑信息可得，第i和第j个无线传感器节点之间的距离为：定义一个信息向量：F(S′)＝[ω(S′)Td(S′)T]T，其中ω＝ωo＝[α1,x,...,αK,x,β1,z,...,βK,z]T+v以上三式对xk，yk，zk求偏导，得到α、β和d关于未知数x＝[x1,...,xK]、y＝[y1,...,yK]和z＝[z1,...,zK]的雅克比矩阵G尺寸为其中是ω关于S′的偏导，是一个2K×3K的矩阵；是d关于S′的偏导，是一个的矩阵令S＝[xyz]T为未知节点几何坐标S＝[s1,…,sK]，sk＝[xk,yk,zk]T，牛顿迭代法求解步骤如下：1)设定初始值S′0为在步骤四中的参考状态C中的无线节点坐标。2)用S′0计算即时信息向量F(S′0)3)计算S′0生成的和获得和与实际已知测量之间的差距：δ＝S-S′0＝(GTG)-1GT(Fo-F(S′0))，其中Fo为实际信息向量通过S′0＝S′0+δ修正S′0，若S′0超出预设的坐标范围，则返回1)重新设定S′0，若S′0没有超过预设范围，且如果δ＜0.001，则进行下一步，否则返回第2)步。4)计算产生的S′0的信息向量与实际信息向量之间的匹配度ρ＝‖Fo-F(S′0)‖2，若匹配度ρ不足够小，返回第1)步重新设定S′0，本文档来自技高网...

【技术保护点】
1.一种单基站刚体位置和姿态估计方法，其特征在于，所述方法包括：/n在目标刚体目标表面固定位置装置K个的无线传感器节点，获取其相对拓扑结构信息，K≥3；/n在单基站处布置二维天线阵列，用于测量各个无线传感器节点的信号来向；/n在基站处使用改进型牛顿迭代算法融合所述信号来向和无线传感器节点的相对拓扑结构信息解算各个无线传感器节点的三维坐标；/n通过奇异值分解确定目标刚体相对于已知参考状态的三维位置和姿态。/n

【技术特征摘要】
1.一种单基站刚体位置和姿态估计方法，其特征在于，所述方法包括：
在目标刚体目标表面固定位置装置K个的无线传感器节点，获取其相对拓扑结构信息，K≥3；
在单基站处布置二维天线阵列，用于测量各个无线传感器节点的信号来向；
在基站处使用改进型牛顿迭代算法融合所述信号来向和无线传感器节点的相对拓扑结构信息解算各个无线传感器节点的三维坐标；
通过奇异值分解确定目标刚体相对于已知参考状态的三维位置和姿态。


2.根据权利要求1所述的方法，其特征在于，所述方法还包括：
以单基站所处位置为圆心，建立局部坐标系，所述刚体位置和姿态估计方法在所述局部坐标系下实现。


3.根据权利要求2所述的方法，其特征在于，所述以单基站所处位置为圆心，建立局部坐标系，包括：
以单基站所处位置为圆心，以垂直于水平面方向为z轴、正东方向为y轴、正南方向为x轴建立局部坐标系。


4.根据权利要求3所述的方法，其特征在于，所述在单基站处布置二维天线阵列，包括：
在局部坐标系的原点基站处安置天线阵列：为x、z方向分别等间隔安装M个天线阵元，总共2M-1个天线阵元；相邻天线阵元间的平均距离设定为所用无线传感器节点所发出窄带信号的载波半波长。


5.根据权利要求4所述的方法，其特征在于，获取目标表面固定位置装置的无线传感器节点相对拓扑结构信息，包括：
测量K个无线传感器节点中任意两个节点之间的距离d＝[d1,2,…,di,j,…,dK-1,K,],i<j，且i、j∈[1,...,K]，获得无线传感器的拓扑结构信息；其中K小于天线阵元数M。


6.根据权利要求5所述的方法，其特征在于，所述测量各个无线传感器节点的信号来向，包括：
设定刚体参考状态C＝[c1,…,cK],ck＝[ck,x,ck,y,ck,z]T,k∈[1,K]，用以解释刚体的相对姿态和相对位置，其中，ck表示第k个无线节点的位置矢量，ck,x,ck,y,ck,z分别表示其x轴y轴及z轴坐标值；
使用多尺度衡量算法预设刚体参考状态；
以预设的刚体参考状态为参考，通过在基站处的天线阵列测量来自各个无线传感器节点的信号波达相位，并使用MUSIC算法对相位测量进行阵列信号处理得到信号的波达角度。


7.根据权利要求6所述的方法，其特征在于，所述使用多尺度衡量算法预设刚体参考状态，包括：
S1.根据无线传感器的拓扑信息构建尺寸为K×K的对称平方距离矩阵其第i行第j列为：
S2.构造定心矩阵令B＝-EDET/2，然后对B进行特征值分解得到B＝Xdiag(λ1,λ2,λ3)XT，其中λ1,λ2,λ3是B的非零特征解，X为λ1,λ2,λ3所对应的特征向量所组成的矩阵；
S3.刚体参考状态下的各个无线传感器节点位置由计算得到。


8.根据权利要求7所述的方法，其特征在于，所述以预设的刚体参考状态为参考，通过在基站处的天线阵列测量来自各个无线传感器节点的信号波达相位，并使用现有的MUSIC算法对相位测量进行阵列信号处理得到信号的波达角度，包括：
以方位角αk和仰俯角βk分别表示第k个无线节点信号的波达方向与x轴和z轴的...

【专利技术属性】
技术研发人员：周彪，吴圣兰，张潇文，姚晓峰，杨乐，
申请(专利权)人：无锡太湖学院，江南大学，
类型：发明
国别省市：江苏;32