基于无人机的联合自定位和姿态估计方法、存储介质及设备

1.本发明属于无源定位技术领域，具体地，涉及一种基于无人机的联合自定位和姿态估计方法、存储介质及设备。


背景技术：

2.随着导航卫星技术的发展，全球卫星定位系统和北斗卫星定位系统已成为日常生活和工程技术里不可或缺的一部分，为数以亿计的人民提供了廉价的高精度定位导航。然而，卫星信号可能无法穿透恶劣环境中的障碍物(如城市峡谷、树冠和天桥)，从而提供误差较大乃至错误的导航信息。甚至，在较大电磁干扰环境中，卫星信号由于受到了干扰无法提供定位，导致卫星导航拒止。
3.目前大部分基于方位角的定位技术，并未考虑工程环境下的传感器姿态问题。事实上，无人机的姿态并非保持静止，而是可能随着采样观测点的变化而变化的，在额外载荷下，该姿态角可以通过物理手段较为容易的得到。然而，由于现有高能量密度电池的理论和技术陷入瓶颈，现有载荷无人机受自身电池容量和动力限制，并不具备高载荷能力，过高的载荷会导致无人机飞行时间缩短，并降低了无人机的灵活性。另一方面，现有基于方位角的定位技术，大多位于无源定位的框架中，并未拓展到自定位的框架里。在导航拒止时，利用已知位置的锚节点辐射源进行自身位置和姿态的同时确定仍旧是一个未被工程考虑的问题。


技术实现要素：

4.针对现有技术的不足，本发明提供了一种基于无人机的联合自定位和姿态估计方法、存储介质及设备，该联合自定位和姿态估计方法解决导航拒止下，利用锚节点实现高精度自定位及姿态估计，仅需一维搜索即可获取最终结果，具有较低的复杂度，且具有重要的工程应用价值。
5.为实现上述目的，本发明采用以下技术方案：一种基于无人机的联合自定位和姿态估计方法，具体包括以下步骤：
6.步骤s1：装载阵列传感器的无人机接收k个位置已知的锚节点辐射源的信号，并对k个锚节点辐射源进行方位角估计；
7.步骤s2：根据方位角估计值以及各锚节点辐射源的位置坐标，利用最小二乘准则构建以无人机位置和姿态为变元的优化函数；
8.步骤s3：利用不等关系消除优化函数中的无人机位置变元，构造仅以姿态为变元的优化函数；
9.步骤s4：利用一维搜索以姿态为变元的优化函数，得到最优无人机姿态和无人机自身位置。
10.进一步地，步骤s1中无人机接收位置已知的锚节点辐射源的信号为：
11.x(t)＝a(θk,ζ)sk(t)+n(t)
12.其中，sk(t)表示第k个锚节点辐射源所辐射的信号经过传播后到达无人机时的信号，n(t)表示零均值复高斯噪声，a(θk,ζ)表示线阵流形，j表示虚数单位，θk表示第k个锚节点辐射源的方位角，[u
kx
,u
ky
]表示第k个锚节点辐射源的位置坐标，p＝[p
x
,py]表示当前无人机的真实位置坐标，ζ表示无人机姿态，λ为信号波长，d为阵元间距，m表示阵元数。
[0013]
进一步地，步骤s1中对k个锚节点辐射源进行方位角估计的过程为：
[0014]
步骤s1.1、求取无人机接收位置已知的锚节点辐射源的信号的协方差矩阵其中，表示均值符号，h表示共轭转置；
[0015]
步骤s1.2、对协方差矩阵进行特征值分解，其中，λ表示特征值矩阵，λ＝diag{λ1,...,λm}，λ的对角线元素λ1,...,λm为协方差矩阵的特征值，λ1≥...≥λm；表示特征矩阵，e1,...,em为与λ1,...,λm一一对应的特征向量；
[0016]
步骤s1.3、取前k个特征值所对应的特征向量，构成信号子空间es＝[e1,
…
,ek]，取信号子空间es前m-1行和后m-1行分别构成矩阵e
x
和ey；
[0017]
步骤s1.4、对矩阵进行特征值分解得到特征矩阵e，并将特征矩阵e分解为k
×
k的子矩阵：
[0018][0019]
步骤s1.5、计算辅助矩阵并求出所述辅助矩阵的特征值得到方位角估计值
[0020]
进一步地，步骤s2包括如下子步骤：
[0021]
步骤s2.1、根据理想情况下方位角并根据正切函数的定义，得到方位角估计值与无人机位置具有下述近似关系：
[0022][0023]
其中，表示方位角估计值，ζ表示无人机姿态，p＝[p
x
,py]表示当前无人机的真实位置坐标，[u
kx
,u
ky
]表示第k个锚节点辐射源的位置坐标；
[0024]
步骤s2.2、根据方位角估计值与无人机位置具有下述近似关系构造k个方程，得到ap≈b，
[0025]
其中，a为k
×
2的矩阵，b为k
×
1的矩阵，由k个方程的所有常数项的堆叠表示，
[0026]
步骤2.3、利用最小二乘准则构建以无人机位置和姿态为变元的优化函数
[0027]
其中，||｜表示欧式范数。
[0028]
进一步地，步骤3的具体过程为：根据不等关系||ap-b||2≥||a(a
t
a)-1at
b-b||2消除优化函数中的无人机位置变元，得到以态为变元的优化函数：
[0029][0030]
进一步地，步骤4中最优无人机姿态最优无人机自身位置
[0031]
进一步地，本发明还提供了一种计算机可读存储介质，存储有计算机程序，所述计算机程序使计算机执行所述的基于无人机的联合自定位和姿态估计方法。
[0032]
进一步地，本发明还提供了一种电子设备，包括：存储器、处理器及存储在存储器上并可在处理器上运行的计算机程序，所述处理器执行计算机程序时，实现所述的基于无人机的联合自定位和姿态估计方法。
[0033]
与现有技术相比，本发明具有如下有益效果：本发明基于无人机的联合自定位和姿态估计方法将无人机位置和姿态变量同时考虑，因此能够同时对自身位置和姿态进行估计，与传统方法相比能够额外获取自身姿态信息，更加贴合实际工程场景；同时，本发明利用单个装载阵列传感器的无人机即可实施位置和姿态的同时估计，降低了无人机负载，无需额外物理外设就可以确立无人机自身姿态；此外，本发明利用一维搜索得到最优姿态和无人机自身位置，大大降低了无人机位置估计和姿态估计的难度。
附图说明
[0034]
图1为本发明基于无人机的联合自定位和姿态估计方法的流程图；
[0035]
图2为本发明所述的定位场景图；
[0036]
图3为本发明基于无人机的联合自定位和姿态估计方法随着方位角估计误差变化的自定位性能图；
[0037]
图4为本发明基于无人机的联合自定位和姿态估计方法随着方位角估计误差变化的姿态估计性能图。
具体实施方式
[0038]
下面结合附图对本发明的技术方案作进一步地解释说明。
[0039]
如图1为本发明基于无人机的联合自定位和姿态估计方法的流程图，该联合自定位和姿态估计方法具体包括如下步骤：
[0040]
步骤s1：装载阵列传感器的无人机接收k个位置已知的锚节点辐射源的信号，并对k个锚节点辐射源进行方位角估计，为后续协方差矩阵提供先验信息，使用较低方位角估计复杂度的方法进行估计，大大降低运算复杂度；
[0041]
如图2，根据定位场景，本发明中无人机接收位置已知的锚节点辐射源的信号为：
[0042]
x(t)＝a(θk,ζ)sk(t)+n(t)
[0043]
其中，sk(t)表示第k个锚节点辐射源所辐射的信号经过传播后到达无人机时的信号，n(t)表示零均值复高斯噪声，a(θk,ζ)表示线阵流形，j表示虚数单位，θk表示第k个锚节点辐射源的方位角，[u
kx
,u
ky
]表示第k个锚节点辐射源的位置坐标，p＝[p
x
,py]表示当前无人机的真实位置坐标，ζ表示无人机姿态，λ为信号波长，d为阵元间距，m表示阵元数。
[0044]
步骤s1中对k个锚节点辐射源进行方位角估计的过程为：
[0045]
步骤s1.1、求取无人机接收位置已知的锚节点辐射源的信号的协方差矩阵其中，表示均值符号，h表示共轭转置；
[0046]
步骤s1.2、对协方差矩阵进行特征值分解，其中，λ表示特征值矩阵，λ＝diag{λ1,...,λm}，λ的对角线元素λ1,...,λm为协方差矩阵的特征值，λ1≥...≥m；表示特征矩阵，e1,...,em为与λ1,...,λm一一对应的特征向量；
[0047]
步骤s1.3、取前k个特征值所对应的特征向量，构成信号子空间es＝[e1,...,ek]，取信号子空间es前m-1行和后m-1行分别构成矩阵e
x
和ey；
[0048]
步骤s1.4、对矩阵进行特征值分解得到特征矩阵e，并将特征矩阵e分解为k
×
k的子矩阵：
[0049][0050]
步骤s1.5、计算辅助矩阵并求出所述辅助矩阵的特征值得到方位角估计值
[0051]
步骤s2：根据方位角估计值以及各锚节点辐射源的位置坐标，利用最小二乘准则构建以无人机位置和姿态为变元的优化函数，最小二乘准则具有拟线性的特征，应用该准
能够降低求解难度；具体包括如下子步骤：
[0052]
步骤s2.1、根据理想情况下方位角并根据正切函数的定义，得到方位角估计值与无人机位置具有下述近似关系：
[0053][0054]
其中，表示方位角估计值，ζ表示无人机姿态，p＝[p
x
,py]表示当前无人机的真实位置坐标，[u
kx
,u
ky
]表示第k个锚节点辐射源的位置坐标；
[0055]
步骤s2.2、根据方位角估计值与无人机位置具有下述近似关系构造k个方程，得到ap≈b，
[0056]
其中，a为k
×
2的矩阵，b为k
×
1的矩阵，由k个方程的所有常数项的堆叠表示，
[0057]
步骤2.3、利用最小二乘准则构建以无人机位置和姿态为变元的优化函数
[0058]
其中，||||表示欧式范数。
[0059]
步骤s3：利用不等关系消除优化函数中的无人机位置变元，构造仅以姿态为变元的优化函数，降低了优化过程的维度；具体地，先优化无人机位置变量p，由于最小二乘的解为因此对任意的无人机姿态ζ，若使得优化函数最小，仅需让p＝(a
t
a)-1at
b，因此，得出以下不等关系根据不等关系||ap-b||2≥||a(a
t
a)-1at
b-b||2消除优化函数中的无人机位置变元，得到以态为变元的优化函数：
[0060]
步骤s4：利用一维搜索以姿态为变元的优化函数，得到最优无人机姿态和无人机自身位置，降低求解复杂度；具体地，根据不等关系将以无人机位置和姿态为变元的优化函数转化成一维优化问题，通过一维搜索得到其最优解，注意到其中，n是正整数，是一个以π为周期的周期函数，仅需要在区间[0,π]中进行搜索，对每个搜索点求优化函数值，找到使得优化函数值最小的搜索点作为最优的姿态最优无人机自身位置
[0061]
将本发明基于无人机的联合自定位和姿态估计方法进行仿真实验，设置仿真参
数：三个锚辐射源的位置为[1050]
t
，[3020]
t
,和[400]
t
，无人机的位置坐标为[00]
t
，单位为m；无人机的姿态角为40
°
。
[0062]
图3为本发明基于无人机的联合自定位和姿态估计方法随着方位角估计误差变化的自定位性能图，可以看出随着方位角估计误差提升，本发明的自定位误差随之上升，但始终具有较低的自定位误差，具有最好的自定位性能；图4为本发明基于无人机的联合自定位和姿态估计方法随着方位角估计误差变化的姿态估计性能图，可以看出，随着方位角估计误差提升，本发明的姿态估计误差随之上升，但始终具有较低的姿态估计误差，具有最好的姿态估计性能。
[0063]
在本发明的一个技术方案中，还提供了一种计算机可读存储介质，存储有计算机程序，所述计算机程序使计算机执行所述的基于无人机的联合自定位和姿态估计方法。
[0064]
在本发明的另一个技术方案中，还提供了一种电子设备，包括：存储器、处理器及存储在存储器上并可在处理器上运行的计算机程序，所述处理器执行计算机程序时，实现所述的基于无人机的联合自定位和姿态估计方法。
[0065]
在本技术所公开的实施例中，计算机存储介质可以是有形的介质，其可以包含或存储以供指令执行系统、装置或设备使用或与指令执行系统、装置或设备结合使用的程序。计算机存储介质可以包括但不限于电子的、磁性的、光学的、电磁的、红外的、或半导体系统、装置或设备，或者上述内容的任何合适组合。计算机存储介质的更具体示例会包括基于一个或多个线的电气连接、便携式计算机盘、硬盘、随机存取存储器(ram)、只读存储器(rom)、可擦除可编程只读存储器(eprom或快闪存储器)、光纤、便捷式紧凑盘只读存储器(cd-rom)、光学储存设备、磁储存设备、或上述内容的任何合适组合。
[0066]
本领域普通技术人员可以意识到，结合本技术所公开的实施例描述的各示例的单元及算法步骤，能够以电子硬件或者计算机软件和电子硬件的结合来实现。这些功能究竟以硬件还是软件方式来执行，取决于技术方案的特定应用和设计约束条件。专业技术人员可以对每个特定的应用来使用不同方法来实现所描述的功能，但是这种实现不应认为超出本技术的范围。
[0067]
以上所述，仅为本发明中的具体实施方式，但本发明的保护范围并不局限于此，任何熟悉该技术的人在本发明所揭露的技术范围内，可理解想到的变换或替换，都应涵盖在本发明的包含范围之内，因此，本发明的保护范围应该以权利要求书的保护范围为准。

技术特征：
1.一种基于无人机的联合自定位和姿态估计方法，其特征在于，具体包括以下步骤：步骤s1：装载阵列传感器的无人机接收k个位置已知的锚节点辐射源的信号，并对k个锚节点辐射源进行方位角估计；步骤s2：根据方位角估计值以及各锚节点辐射源的位置坐标，利用最小二乘准则构建以无人机位置和姿态为变元的优化函数；步骤s3：利用不等关系消除优化函数中的无人机位置变元，构造仅以姿态为变元的优化函数；步骤s4：利用一维搜索以姿态为变元的优化函数，得到最优无人机姿态和无人机自身位置。2.根据权利要求1所述的一种基于无人机的联合自定位和姿态估计方法，其特征在于，步骤s1中无人机接收位置已知的锚节点辐射源的信号为：x(t)＝a(θ
k
,ζ)s
k
(t)+n(t)其中，s
k
(t)表示第k个锚节点辐射源所辐射的信号经过传播后到达无人机时的信号，n(t)表示零均值复高斯噪声，a(θ
k
,ζ)表示线阵流形，j表示虚数单位，θ
k
表示第k个锚节点辐射源的方位角，[u
kx
,u
ky
]表示第k个锚节点辐射源的位置坐标，p＝[p
x
,p
y
]表示当前无人机的真实位置坐标，ζ表示无人机姿态，λ为信号波长，d为阵元间距，m表示阵元数。3.根据权利要求2所述的一种基于无人机的联合自定位和姿态估计方法，其特征在于，步骤s1中对k个锚节点辐射源进行方位角估计的过程为：步骤s1.1、求取无人机接收位置已知的锚节点辐射源的信号的协方差矩阵其中，表示均值符号，h表示共轭转置；步骤s1.2、对协方差矩阵进行特征值分解，其中，λ表示特征值矩阵，λ＝diag{λ1,...,λ
m
}，λ的对角线元素λ1,...,λ
m
为协方差矩阵的特征值，λ1≥...≥λ
m
；表示特征矩阵，e1,...,e
m
为与λ1,...,λ
m
一一对应的特征向量；步骤s1.3、取前k个特征值所对应的特征向量，构成信号子空间e
s
＝[e1,
…
,e
k
]，取信号子空间e
s
前m-1行和后m-1行分别构成矩阵e
x
和e
y
；步骤s1.4、对矩阵进行特征值分解得到特征矩阵e，并将特征矩阵e分解为k
×
k的子矩阵：步骤s1.5、计算辅助矩阵并求出所述辅助矩阵的特征值λ
k
，得到方位角估计
4.根据权利要求1所述的一种基于无人机的联合自定位和姿态估计方法，其特征在于，步骤s2包括如下子步骤：步骤s2.1、根据理想情况下方位角并根据正切函数的定义，得到方位角估计值与无人机位置具有下述近似关系：其中，表示方位角估计值，ζ表示无人机姿态，p＝[p
x
,p
y
]表示当前无人机的真实位置坐标，[u
kx
,u
ky
]表示第k个锚节点辐射源的位置坐标；步骤s2.2、根据方位角估计值与无人机位置具有下述近似关系构造k个方程，得到ap≈b，其中，a为k
×
2的矩阵，b为k
×
1的矩阵，由k个方程的所有常数项的堆叠表示，步骤2.3、利用最小二乘准则构建以无人机位置和姿态为变元的优化函数其中，|| ||表示欧式范数。5.根据权利要求4所述的一种基于无人机的联合自定位和姿态估计方法，其特征在于，步骤3的具体过程为：根据不等关系||ap-b||2≥||a(a
t
a)-1
a
t
b-b||2消除优化函数中的无人机位置变元，得到以态为变元的优化函数：6.根据权利要求5所述的一种基于无人机的联合自定位和姿态估计方法，其特征在于，步骤4中最优无人机姿态最优无人机自身位置7.一种计算机可读存储介质，存储有计算机程序，其特征在于，所述计算机程序使计算机执行如权利要求1-6任一项所述的基于无人机的联合自定位和姿态估计方法。8.一种电子设备，其特征在于，包括：存储器、处理器及存储在存储器上并可在处理器上运行的计算机程序，所述处理器执行计算机程序时，实现如权利要求1-6任一项所述的基于无人机的联合自定位和姿态估计方法。

技术总结
本发明公开了一种基于无人机的联合自定位和姿态估计方法、存储介质及设备，该联合自定位和姿态估计方法包括：装载阵列传感器的无人机接收K个位置已知的锚节点辐射源的信号，并对K个锚节点辐射源进行方位角估计；根据方位角估计值以及各锚节点辐射源的位置坐标，利用最小二乘准则构建以无人机位置和姿态为变元的优化函数；利用不等关系消除优化函数中的无人机位置变元，构造仅以姿态为变元的优化函数；利用一维搜索以姿态为变元的优化函数，得到最优无人机姿态和无人机自身位置。本发明无需额外载荷即可实现高精度姿态角确定，同时实现了自定位与姿态角确定，贴合导航拒止场景，运作成本低廉，工程应用价值十分重大。工程应用价值十分重大。工程应用价值十分重大。


技术研发人员：李潘 李建峰 晋本周 张小飞 吴启晖
受保护的技术使用者：南京航空航天大学
技术研发日：2023.05.18
技术公布日：2023/8/6