一种飞行器自组织网络协同定位方法

1.本发明涉及飞行器协同定位领域，具体涉及一种飞行器自组织网络协同定位方法。


背景技术：

2.协同定位可以使飞行器在gnss拒止环境中依靠器间数据链获得与其它飞行器的相对位置信息进行自身定位，是飞行器定位技术发展的一个重要方向。然而在锚节点较少的场景中，待定位飞行器难以只依靠测距信息进行定位解算。联合利用测距测角信息的协同定位是指飞行器通过数据链同时获得与其它飞行器的相对距离和相对角度信息进行定位解算的方法，可以应用于锚节点较少的定位场景，具有重要的研究意义。
3.常规的协同定位方法需要汇总网络中全部的测量信息到指定的中心计算设备，同时完成所有节点的定位解算，其通信开销大，定位时延高，不适合没有中心节点的自组织飞行器网络。一些联合测距测角协同定位通过构建二维环境下多跳链路线性最小二乘方程，可实现二维网络中节点的分布式定位，但这类方法不适用在三维空间中机动的飞行器定位。
4.与线性最小二乘定位解算方法相比，加权最小二乘定位解算方法具有更高的定位精度。一些加权最小二乘定位解算方法只考虑了测距误差对定位误差带来的影响，没有对测角误差加以考虑。


技术实现要素：

5.有鉴于此，本发明提供了一种飞行器自组织网络协同定位方法，该方法基于两跳范围内锚节点的位置信息与数据链测距测角信息，构建三维环境下加权线性最小二乘观测方程，在几何模型上考虑了测距误差和测角误差对定位误差的影响，设计了权重计算方法，最终实现飞行器在自组织网络中的定位。本方法相比于将所有节点的测量信息汇总到中心计算节点的常规协同定位方法，具有更小的通信开销和更低的定位时延。
6.为达到上述目的，本发明的技术方案为：飞行器协同定位过程中，待定位飞行器s两跳范围内的可见锚节点集合为，其中ai表示编号为i的锚节点；待定位飞行器s一跳邻居节点集合为,其中uj表示编号为j的一跳邻居节点；待定位飞行器s利用锚节点a的位置pa＝[xa,ya,za]
t
、一跳邻居节点u与锚节点a的测距结果d
au
、水平角测角结果θ
au
、高程角测角结果φ
au
、自身与一跳邻居节点的测距结果d
us
、水平角测角结果θ
us
、高程角测角结果φ
us
以及自身与锚节点的测距结果d
as
、水平角测角结果θ
as
、高程角测角结果φ
as
确定自身位置ps＝[xs,ys,zs]
t
。
[0007]
采用相对距离测量模块、相对角度测量模块、通信数据解调模块、多跳链路联合观测量构建模块、权重计算模块以及位置解算模块执行如下步骤；
[0008]
(1)相对距离测量模块通过接收到的通信信号测量待定位节点s与直接可见的锚节点a的相对距离d
as
、待定位节点s与一跳邻居节点u的相对距离d
us
，获得测距结果后，将测
距结果传递给多跳链路联合观测量构建模块和权重计算模块。
[0009]
(2)相对角度测量模块通过接收到的通信信号测量待定位节点s与直接可见的锚节点a的相对水平角θ
as
、待定位节点s与直接可见的锚节点a相对高程角φ
as
、待定位节点s与一跳邻居节点u的的相对水平角θ
us
、待定位节点s与一跳邻居节点u的相对高程角φ
us
，任意节点i和节点j间的角度关系如图2所示。获得测角结果后，将测角结果传递给多跳链路联合观测量构建模块和权重计算模块。
[0010]
(3)通信数据解调模块解调出通信信号中调制的锚节点a的位置pa＝[xa,ya,za]
t
、一跳邻居节点u与锚节点a的测距结果d
au
、水平角测角结果θ
au
、高程角测角结果φ
au
，并将解调出的信息传递给多跳链路联合观测量构建模块和权重计算模块。
[0011]
(4)多跳链路联合观测量构建模块根据输入的锚节点位置和各链路测距测角结果，构建锚节点到待定位节点s的两跳内链路集合其中li表示编号为i的链路，i取值为[1,l]范围内的整数；链路集合包含锚节点a到待定位节点s的单跳链路和锚节点a经一跳邻居节点u中继到待定位节点s的两跳链路。
[0012]
在完成链路集合构建的基础上，对集合中的第i条链路li，分别构建其x轴、y轴、z轴的联合观测量b
x,i
、b
y,i
、b
z,i
；
[0013]
在获得各链路联合观测量后，组合成联合观测向量b＝[b
x,1
,b
x,2
,...,b
x,l
,b
y,1
,b
y,2
,...,b
y,l
,b
z,1
,b
z,2
,...,b
z,l
]
t
，将联合观测向量送入位置解算模块，将链路集合送入权重计算模块；
[0014]
(5)权重计算模块根据输入的锚节点位置和各链路测距测角结果，计算链路li由测距误差带来的定位误差和测角误差带来的定位误差
[0015]
计算链路li上的权重wi，计算方法为
[0016][0017]
计算得到全部链路的权重后，计算权重矩阵w，计算方法为
[0018]
w＝diag([w1,...,w
l
,w1,...,w
l
,w1,...,w
l
])
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(12)
[0019]
得到权重矩阵w后，将权重矩阵传递给位置解算单元；
[0020]
(6)位置解算单元首先计算系数矩阵q，其计算方法为
[0021][0022]
其中1
l
×1为l个1组成的列矩阵，0
l
×1为l个0组成的列矩阵。
[0023]
得到系数矩阵后，根据输入的联合观测向量和权重矩阵计算待定位节点s的位置ps＝[xs,ys,zs]
t
，计算方法为
[0024]
ps＝(q
twt
wq)-1qtwt
wb
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(14)。
[0025]
进一步地，对集合中的第i条链路li，分别构建其x轴、y轴、z轴的联合观测量b
x,i
、b
y,i
、b
z,i
，具体为：
[0026]
若第i条链路li为锚节点a到待定位节点s的单跳链路，则x轴、y轴、z轴的联合观测
量b
x,i
、b
y,i
、b
z,i
的计算方法分别为：
[0027]bx,i
＝xa+d
as
cosθ
as
cosφ
as
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(1)
[0028]by,i
＝ya+d
as
sinθ
as
cosφ
as
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(2)
[0029]bz,i
＝za+d
as
sinφ
as
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(3)
[0030]
若链路li为锚节点a经一跳邻居节点u中继到待定位节点s的两跳链路，则x轴、y轴、z轴的联合观测量b
x,i
、b
y,i
、b
z,i
的计算方法分别为
[0031]bx,i
＝xa+d
au
cosθ
au
cosφ
au
+d
us
cosθ
us
cosφ
us
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(4)
[0032]by,i
＝ya+d
au
sinθ
au
cosφ
au
+d
us
sinθ
us
cosφ
us
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(5)
[0033]bz,i
＝za+d
au
sinφ
au
+d
us
sinφ
us
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(6)。
[0034]
进一步地，计算链路li由测距误差带来的定位误差和测角误差带来的定位误差具体为：
[0035]
若链路li为锚节点a到待定位节点s的单跳链路，则测距误差带来的定位误差和测角误差带来的定位误差分别为
[0036][0037][0038]
其中，σ
distance
和σ
angle
分别为测距标准差和测角标准差；
[0039]
若链路li为锚节点a经一跳邻居节点u中继到待定位节点s的两跳链路，则测距误差带来的定位误差和测角误差带来的定位误差分别为
[0040][0041][0042]
其中e
au
＝[cosθ
au
cosφ
au
,sinθ
au
cosφ
au
,sinφ
au
]
t
为节点a指向节点u的方向向量，e
us
＝[cosθ
us
cosφ
us
,sinθ
us
cosφ
us
,sinφ
us
]
t
为节点u指向节点s的方向向量。
[0043]
有益效果：
[0044]
本发明利用两跳链路内飞行器间相对距离和相对角度测量结果和两跳内可观测锚节点位置，构建多跳链路联合观测量，结合测距误差和测角误差对定位误差的影响构建权重矩阵，基于加权线性最小二乘算法完成飞行器在自组织网络中的协同定位。相对于常规协同定位方法，本方法无需将所有链路测量结果汇总至中心计算节点进行位置计算，只需收集两跳链路内的测量结果即可自主完成自身节点的位置计算，飞行器间通信开销小、定位时延低。
附图说明
[0045]
图1为本发明提供的飞行器自组织网络协同定位方法框图；
[0046]
图2任意节点间角度关系示意图。
具体实施方式
[0047]
下面结合附图并举实施例，对本发明进行详细描述。
[0048]
本发明结构框图如图1所示，包括相对距离测量模块、相对角度测量模块、通信数据解调模块、多跳链路联合观测量构建模块、权重计算模块、位置解算模块。
[0049]
本发明的具体步骤如下：
[0050]
不失一般性，对飞行器协同定位进行如下描述：假设某待定位飞行器s两跳范围内的可见锚节点集合为其中ai表示编号为i的锚节点。待定位飞行器s一跳邻居节点集合为其中uj表示编号为j的一跳邻居节点。待定位飞行器s利用锚节点a的位置pa＝[xa,ya,za]
t
、一跳邻居节点u与锚节点a的测距结果d
au
、水平角测角结果θ
au
、高程角测角结果φ
au
、自身与一跳邻居节点的测距结果d
us
、水平角测角结果θ
us
、高程角测角结果φ
us
以及自身与锚节点的测距结果d
as
、水平角测角结果θ
as
、高程角测角结果φ
as
确定自身位置ps＝[xs,ys,zs]
t
。
[0051]
(1)相对距离测量模块通过接收到的通信信号测量待定位节点s与直接可见的锚节点a的相对距离d
as
、待定位节点s与一跳邻居节点u的相对距离d
us
，获得测距结果后，将测距结果传递给多跳链路联合观测量构建模块和权重计算模块。
[0052]
(2)相对角度测量模块通过接收到的通信信号测量待定位节点s与直接可见的锚节点a的相对水平角θ
as
、待定位节点s与直接可见的锚节点a相对高程角φ
as
、待定位节点s与一跳邻居节点u的的相对水平角θ
us
、待定位节点s与一跳邻居节点u的相对高程角φ
us
，任意节点i和节点j间的角度关系如图2所示。获得测角结果后，将测角结果传递给多跳链路联合观测量构建模块和权重计算模块。
[0053]
(3)通信数据解调模块解调出通信信号中调制的锚节点a的位置pa＝[xa,ya,za]
t
、一跳邻居节点u与锚节点a的测距结果d
au
、水平角测角结果θ
au
、高程角测角结果φ
au
，并将解调出的信息传递给多跳链路联合观测量构建模块和权重计算模块。
[0054]
(4)多跳链路联合观测量构建模块根据输入的锚节点位置和各链路测距测角结果，构建锚节点到待定位节点s的两跳内链路集合其中li表示编号为i的链路。链路集合包含锚节点a到待定位节点s的单跳链路和锚节点a经一跳邻居节点u中继到待定位节点s的两跳链路。
[0055]
在完成链路集合构建的基础上，对集合中的每一条链路li，分别构建其x轴、y轴、z轴的联合观测量b
x,i
、b
y,i
、b
z,i
。
[0056]
若链路li为锚节点a到待定位节点s的单跳链路，则x轴、y轴、z轴的联合观测量b
x,i
、b
y,i
、b
z,i
的计算方法分别为
[0057]bx,i
＝xa+d
as
cosθ
as
cosφ
as
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(1)
[0058]by,i
＝ya+d
as
sinθ
as
cosφ
as
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(2)
[0059]bz,i
＝za+d
as
sinφ
as
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(3)
[0060]
若链路li为锚节点a经一跳邻居节点u中继到待定位节点s的两跳链路，则x轴、y轴、z轴的联合观测量b
x,i
、b
y,i
、b
z,i
的计算方法分别为
[0061]bx,i
＝xa+d
au
cosθ
au
cosφ
au
+d
us
cosθ
us
cosφ
us
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(4)
[0062]by,i
＝ya+d
au
sinθ
au
cosφ
au
+d
us
sinθ
us
cosφ
us
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(5)
[0063]bz,i
＝za+d
au
sinφ
au
+d
us
sinφ
us
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(6)
[0064]
在获得各链路联合观测量后，组合成联合观测向量b＝[b
x,1
,b
x,2
,...,b
x,l
,b
y,1
,by,2
,...,b
y,l
,b
z,1
,b
z,2
,...,b
z,l
]
t
，将联合观测向量送入位置解算模块，将链路集合送入权重计算模块。
[0065]
(5)权重计算模块根据输入的锚节点位置和各链路测距测角结果，计算链路li由测距误差带来的定位误差和测角误差带来的定位误差
[0066]
若链路li为锚节点a到待定位节点s的单跳链路，则测距误差带来的定位误差和测角误差带来的定位误差分别为
[0067][0068][0069]
其中，σ
distance
和σ
angle
分别为测距标准差和测角标准差。
[0070]
若链路li为锚节点a经一跳邻居节点u中继到待定位节点s的两跳链路，则测距误差带来的定位误差和测角误差带来的定位误差分别为
[0071][0072][0073]
其中e
au
＝[cosθ
au
cosφ
au
,sinθ
au
cosφ
au
,sinφ
au
]
t
为节点a指向节点u的方向向量，e
us
＝[cosθ
us
cosφ
us
,sinθ
us
cosφ
us
,sinφ
us
]
t
为节点u指向节点s的方向向量。
[0074]
计算链路li上的权重wi，计算方法为
[0075][0076]
计算得到全部链路的权重后，计算权重矩阵w，计算方法为
[0077]
w＝diag([w1,...,w
l
,w1,...,w
l
,w1,...,w
l
])
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(12)
[0078]
得到权重矩阵w后，将权重矩阵传递给位置解算单元。
[0079]
(6)位置解算单元首先计算系数矩阵q，其计算方法为
[0080][0081]
其中1
l
×1为l个1组成的列矩阵，0
l
×1为l个0组成的列矩阵。
[0082]
得到系数矩阵后，根据输入的联合观测向量和权重矩阵计算待定位节点s的位置ps＝[xs,ys,zs]
t
，计算方法为
[0083]
ps＝(q
twt
wq)-1qtwt
wb
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(14)
[0084]
综上所述，以上仅为本发明的较佳实施例而已，并非用于限定本发明的保护范围。凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。