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

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

背景技术：

1、随着导航卫星技术的发展，全球卫星定位系统和北斗卫星定位系统已成为日常生活和工程技术里不可或缺的一部分，为数以亿计的人民提供了廉价的高精度定位导航。然而，卫星信号可能无法穿透恶劣环境中的障碍物(如城市峡谷、树冠和天桥)，从而提供误差较大乃至错误的导航信息。甚至，在较大电磁干扰环境中，卫星信号由于受到了干扰无法提供定位，导致卫星导航拒止。

2、目前大部分基于方位角的定位技术，并未考虑工程环境下的传感器姿态问题。事实上，无人机的姿态并非保持静止，而是可能随着采样观测点的变化而变化的，在额外载荷下，该姿态角可以通过物理手段较为容易的得到。然而，由于现有高能量密度电池的理论和技术陷入瓶颈，现有载荷无人机受自身电池容量和动力限制，并不具备高载荷能力，过高的载荷会导致无人机飞行时间缩短，并降低了无人机的灵活性。另一方面，现有基于方位角的定位技术，大多位于无源定位的框架中，并未拓展到自定位的框架里。在导航拒止时，利用已知位置的锚节点辐射源进行自身位置和姿态的同时确定仍旧是一个未被工程考虑的问题。

技术实现思路

1、针对现有技术的不足，本发明提供了一种基于无人机的联合自定位和姿态估计方法、存储介质及设备，该联合自定位和姿态估计方法解决导航拒止下，利用锚节点实现高精度自定位及姿态估计，仅需一维搜索即可获取最终结果，具有较低的复杂度，且具有重要的工程应用价值。

2、为实现上述目的，本发明采用以下技术方案：一种基于无人机的联合自定位和姿态估计方法，具体包括以下步骤：

3、步骤s1：装载阵列传感器的无人机接收k个位置已知的锚节点辐射源的信号，并对k个锚节点辐射源进行方位角估计；

4、步骤s2：根据方位角估计值以及各锚节点辐射源的位置坐标，利用最小二乘准则构建以无人机位置和姿态为变元的优化函数；

5、步骤s3：利用不等关系消除优化函数中的无人机位置变元，构造仅以姿态为变元的优化函数；

6、步骤s4：利用一维搜索以姿态为变元的优化函数，得到最优无人机姿态和无人机自身位置。

7、进一步地，步骤s1中无人机接收位置已知的锚节点辐射源的信号为：

8、x(t)＝a(θk,ζ)sk(t)+n(t)

9、其中，sk(t)表示第k个锚节点辐射源所辐射的信号经过传播后到达无人机时的信号，n(t)表示零均值复高斯噪声，a(θk,ζ)表示线阵流形，j表示虚数单位，θk表示第k个锚节点辐射源的方位角，[ukx,uky]表示第k个锚节点辐射源的位置坐标，p＝[px,py]表示当前无人机的真实位置坐标，ζ表示无人机姿态，λ为信号波长，d为阵元间距，m表示阵元数。

10、进一步地，步骤s1中对k个锚节点辐射源进行方位角估计的过程为：

11、步骤s1.1、求取无人机接收位置已知的锚节点辐射源的信号的协方差矩阵其中，表示均值符号，h表示共轭转置；

12、步骤s1.2、对协方差矩阵进行特征值分解，其中，λ表示特征值矩阵，λ＝diag{λ1,...,λm}，λ的对角线元素λ1,...,λm为协方差矩阵的特征值，λ1≥...≥λm；表示特征矩阵，e1,...,em为与λ1,...,λm一一对应的特征向量；

13、步骤s1.3、取前k个特征值所对应的特征向量，构成信号子空间es＝[e1,…,ek]，取信号子空间es前m-1行和后m-1行分别构成矩阵ex和ey；

14、步骤s1.4、对矩阵进行特征值分解得到特征矩阵e，并将特征矩阵e分解为k×k的子矩阵：

15、

16、步骤s1.5、计算辅助矩阵并求出所述辅助矩阵的特征值得到方位角估计值

17、进一步地，步骤s2包括如下子步骤：

18、步骤s2.1、根据理想情况下方位角并根据正切函数的定义，得到方位角估计值与无人机位置具有下述近似关系：

19、

20、其中，表示方位角估计值，ζ表示无人机姿态，p＝[px,py]表示当前无人机的真实位置坐标，[ukx,uky]表示第k个锚节点辐射源的位置坐标；

21、步骤s2.2、根据方位角估计值与无人机位置具有下述近似关系构造k个方程，得到ap≈b，

22、其中，a为k×2的矩阵，b为k×1的矩阵，由k个方程的所有常数项的堆叠表示，

23、步骤2.3、利用最小二乘准则构建以无人机位置和姿态为变元的优化函数

24、其中，||｜表示欧式范数。

25、进一步地，步骤3的具体过程为：根据不等关系||ap-b||2≥||a(ata)-1atb-b||2消除优化函数中的无人机位置变元，得到以态为变元的优化函数：

26、

27、进一步地，步骤4中最优无人机姿态最优无人机自身位置

28、进一步地，本发明还提供了一种计算机可读存储介质，存储有计算机程序，所述计算机程序使计算机执行所述的基于无人机的联合自定位和姿态估计方法。

29、进一步地，本发明还提供了一种电子设备，包括：存储器、处理器及存储在存储器上并可在处理器上运行的计算机程序，所述处理器执行计算机程序时，实现所述的基于无人机的联合自定位和姿态估计方法。

30、与现有技术相比，本发明具有如下有益效果：本发明基于无人机的联合自定位和姿态估计方法将无人机位置和姿态变量同时考虑，因此能够同时对自身位置和姿态进行估计，与传统方法相比能够额外获取自身姿态信息，更加贴合实际工程场景；同时，本发明利用单个装载阵列传感器的无人机即可实施位置和姿态的同时估计，降低了无人机负载，无需额外物理外设就可以确立无人机自身姿态；此外，本发明利用一维搜索得到最优姿态和无人机自身位置，大大降低了无人机位置估计和姿态估计的难度。

技术特征：

1.一种基于无人机的联合自定位和姿态估计方法，其特征在于，具体包括以下步骤：

2.根据权利要求1所述的一种基于无人机的联合自定位和姿态估计方法，其特征在于，步骤s1中无人机接收位置已知的锚节点辐射源的信号为：

3.根据权利要求2所述的一种基于无人机的联合自定位和姿态估计方法，其特征在于，步骤s1中对k个锚节点辐射源进行方位角估计的过程为：

4.根据权利要求1所述的一种基于无人机的联合自定位和姿态估计方法，其特征在于，步骤s2包括如下子步骤：

5.根据权利要求4所述的一种基于无人机的联合自定位和姿态估计方法，其特征在于，步骤3的具体过程为：根据不等关系||ap-b||2≥||a(ata)-1atb-b||2消除优化函数中的无人机位置变元，得到以态为变元的优化函数：

6.根据权利要求5所述的一种基于无人机的联合自定位和姿态估计方法，其特征在于，步骤4中最优无人机姿态最优无人机自身位置

7.一种计算机可读存储介质，存储有计算机程序，其特征在于，所述计算机程序使计算机执行如权利要求1-6任一项所述的基于无人机的联合自定位和姿态估计方法。

8.一种电子设备，其特征在于，包括：存储器、处理器及存储在存储器上并可在处理器上运行的计算机程序，所述处理器执行计算机程序时，实现如权利要求1-6任一项所述的基于无人机的联合自定位和姿态估计方法。

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

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