基于引导优化的无人系统相对定位方法与流程

本发明属于无人系统导航定位，特别是指一种拒止环境下无人系统快速、高精度相对定位方法。该方法对于在卫星拒止环境下构建无人系统的高精度、高效率的空间构型感知控制能力与协同定位能力具有重要意义。

背景技术：

1、无人系统以无人作战平台为基础，通过搭载任务载荷完成多样化功能。无人系统能够融合陆、海、空等多个物理空间的作战单元，具备低成本、低损耗、零伤亡、高机动性、高隐蔽性以及高灵活性等特点。精确的位置服务是无人系统发挥作战效能的重要因素，但在高烈度对抗和复杂电磁环境下，卫星导航系统极容易受到干扰破坏，无法提供稳定的位置服务。卫星拒止环境下的相对定位方法能够利用无人系统节点间的距离信息完成节点相对拓扑构型的解算，并且当获取到少数几个节点的绝对地理位置时，便能够解算全部节点的绝对地理位置。整体而言，相对定位方法的鲁棒性强、灵活性高且定位精度高，具备极大的研究和落地应用价值。

2、无人系统相对定位方法的核心算法是相对-绝对坐标系转换关系的解算，其解算效果直接决定了定位精度。当前解算方法主要包括最小二乘法(“基于mds的无人机群协同定位算法研究”,2021,西安电子科技大学,硕士学位论文)、牛顿法(“an inexactsmoothing newton method for euclidean distance matrix optimization underordinal constrain”，2017,35(4):469-485)、遗传算法(“无人集群组网系统相对定位技术研究”,2023,遥测遥控,44(04):16-22)等，但这些方法只是单方面侧重了精度或者效率因素，其中，最小二乘法具有极高的解算速度，但定位精度差；牛顿法、遗传算法解算精度较高，但需要较长的迭代优化次数，无法满足时效性要求。面向未来作战环境，为适配无人系统位置时变、高机动等特性，需要同时满足解算精度和解算时间容限要求，因此需要研究具备高精度、高效率特性的相对定位解算方法。

技术实现思路

1、有鉴于此，本发明提出基于引导优化的无人系统相对定位方法，该方法通过放松旋转矩阵的自由度能够将坐标系转换参数寻优的初始值一步引导至真值邻域附近，然后利用迭代法进行精细寻优，实现了相对定位的短时间、高精度计算。

2、本发明采用的技术方案为：

3、基于引导优化的无人系统相对定位方法，包括以下步骤：

4、步骤1，根据无人系统节点间的测距数值构造距离矩阵，并计算所有节点的相对坐标；

5、步骤2，使用绝对、相对坐标均已知的节点构建差分绝对坐标矩阵、差分相对坐标矩阵，并构建相对-绝对坐标系的转换方程，其转换方程中的未知量为包含三个欧拉角的旋转矩阵；使用基于自由度松弛的引导优化算法完成的旋转矩阵求解，实现对欧拉角的初步估计；

6、步骤3，精确求解转换方程组的参数，将第2步中旋转矩阵的欧拉角计算结果作为初始值，使用牛顿迭代法再次精确估计旋转矩阵中的欧拉角参数；

7、步骤4，基于相对-绝对坐标系的转换方程计算平移向量，利用旋转矩阵和平移向量的估计值，计算无人系统中所有节点的绝对地理坐标。

8、进一步地，所述步骤1具体包括以下步骤：

9、步骤1.1，对包含n个节点的无人系统，使用数据链测量方式获取节点间的距离，记dij表示第i个节点和第j个节点的距离值，构建距离矩阵d＝[dij]n×n，即

10、

11、距离矩阵d为对称矩阵，即dij＝dji；其对角线元素为0，即dii＝0；

12、步骤1.2，根据距离矩阵d计算无人系统的内积矩阵b＝[bij]n×n，其中bij表示第i个节点和第j个节点的内积值，即

13、

14、步骤1.3，对内积矩阵b进行特征值分解

15、b＝rtλr

16、其中，r是特征向量矩阵，λ为包含n个特征值的特征值矩阵，λi为第i个特征值，即

17、

18、步骤1.4，计算n个节点的增广相对坐标矩阵l为

19、

20、其中，矩阵为

21、

22、步骤1.5，取增广相对坐标矩阵l的前3行，舍弃第3行至第n行的全部元素，得到无人系统n个节点的相对坐标矩阵p，即

23、p＝[p1 … pn]

24、其中，pi表示第i个节点的相对坐标。

25、进一步地，所述步骤2具体包括以下步骤：

26、步骤2.1，设无人系统中存在w个绝对地理坐标已知的节点，即已知节点，且w<n，已知节点相应的绝对坐标矩阵为

27、q(k)＝[q1 … qw]

28、其中，qi表示第i个节点的绝对地理坐标；相对坐标矩阵为

29、p(k)＝[p1 … pw]

30、剩余的w-n个节点只能获知相对坐标，绝对地理坐标为未知量，将这些节点称为未知节点；

31、步骤2.2，根据步骤2.1的计算结果，在w个已知节点中随机选择一个节点o，构建差分绝对坐标矩阵q(d)为

32、q(d)＝[q1-qo q2-qo … qo-1-qo qo+1-qo … qw-qo]

33、相应的差分相对坐标矩阵p(d)为

34、p(d)＝[p1-po p2-po … po-1-po po+1-po …pw-po]

35、步骤2.3，构建相对-绝对坐标系的转换方程，有

36、q(k)＝mp(k)+s

37、其中，m为旋转矩阵，s为平移向量；在差分情况下，有

38、q(d)＝mp(d)

39、步骤2.4，根据步骤2.2和步骤2.3的结果，在自由度缩放的情况下进行旋转矩阵m的估计，估计值为：

40、

41、步骤2.5，根据步骤3.1估计的旋转矩阵计算欧拉角α0,β0,γ0：

42、

43、

44、

45、其中，表示矩阵中的第m行、第n列个元素。

46、进一步地，所述步骤3具体包括以下步骤：

47、步骤3.1，对于步骤2.2中的w-1对差分相对、绝对坐标，利用步骤2.3中的差分转换方程，构建包含3×(w-1)个方程的方程组f(α,β,γ)＝0，欧拉角α,β,γ为方程的优化参数；对于第t对差分相对-绝对坐标关系，其构建的3个方程为

48、

49、

50、

51、其中，表示差分相对坐标矩阵pd中的第m行、第n列个元素，表示差分绝对坐标矩阵qd中的第m行、第n列个元素；

52、方程组f(α,β,γ)＝0中的各表达式f为：

53、f＝{fr|r＝1,2,…,3×(w-1)}

54、步骤3.2，设置最大迭代次数tmax，迭代收敛阈值∈，将优化参数α,β,γ的初始值设置为α0,β0,γ0；

55、步骤3.3，迭代计算参数α,β,γ，记第k次迭代的参数为θk＝[αk,βk,γk]，计算方程的雅可比矩阵jk为

56、

57、步骤3.4，计算雅可比矩阵jk的逆矩阵为

58、

59、步骤3.5，更新参数

60、

61、若迭代次数大于tmax或者|αk+1-αk|、|βk+1-βk|和|γk+1-γk|均小于迭代收敛阈值∈，则输出参数估计结果并停止迭代，否则返回步骤3.3继续迭代。

62、进一步地，所述步骤4具体包括以下步骤：

63、步骤4.1，根据步骤3输出的参数α,β,γ的估计值，得到旋转矩阵估计值为

64、

65、步骤4.2，根据旋转矩阵估计值得到相对-绝对坐标系平移向量s的估计值为

66、

67、步骤4.3，根据旋转矩阵和平移向量的估计值，通过相对坐标实现对任意节点pi的绝对地理坐标推算，即

68、

69、本发明的有益效果在于：

70、1、本发明利用了基于自由度松弛的引导优化算法，其通过放松旋转矩阵的自由度能够将参数寻优的初始值一步引导至真值邻域附近，然后利用迭代法进行精细寻优，实现了坐标系转换参数的短时间、高精度计算。

71、2、在卫星拒止环境下，无人系统能够依靠数据链的高精度测距能力实现相对定位导航。由于无人系统位置时变、高机动等特性，在进行相对定位解算时需要同时满足精度和效率需求。本发明通过优化旋转矩阵的参数估计方式，在保障相对定位计算精度的基础上大幅度缩短了解算时间，能够直接赋能无人系统协同作战，具有极大的应用价值。