一种基于阴影匹配的协同定位增强方法与流程

本发明涉及一种基于阴影匹配的协同定位增强方法，属于定位与导航。

背景技术：

1、无人集群系统能够在陆海空等各类复杂环境下，执行多种任务。然而在复杂地形环境工作中，无人集群系统的卫星、uwb等导航定位信号经常被附近地形遮挡，导致定位精度降低。此时可以利用集群成员间的相对距离、角度等观测信息，基于滤波、图理论在内的协同导航方法，为处于卫星拒止环境的待辅助成员提供协同辅助定位，对于提升集群整体定位精度具有重要意义。

2、卫星导航阴影匹配定位技术是近年来针对城市复杂环境下定位需求而发展起来的一种创新方法，该方法的工作原理为：卫星信号在城市内部传播过程中会被建筑物遮挡，形成特定的“阴影”区域（即信号缺失区域），这些阴影区域与接收机所在位置密切相关。通过三维城市模型和卫星轨道信息，预先模拟不同位置上各卫星信号的可见性（即哪些卫星信号被遮挡，哪些可接收），生成一张“阴影地图”——记录每个地理位置的卫星信号遮挡状态。卫星导航接收机在实际运行中，测量当前可接收到的卫星信号（即“可见卫星”列表），将其与阴影地图中预存的遮挡模式进行比对，找到最匹配的位置，从而实现阴影匹配定位。

3、尽管阴影匹配技术在城市环境中展现出良好的定位潜力，但仍面临关键挑战。其中，最突出的问题是“遮挡模式模糊性”——在部分区域（如对称街道、规则建筑群），多个不同位置可能产生高度相似甚至完全相同的遮挡模式，导致定位结果出现歧义，难以确定真实位置，严重制约其实际应用效果。

技术实现思路

1、本发明的技术解决问题是：克服现有技术的不足，提供一种基于阴影匹配的协同定位增强方法，提高待定位用户在复杂地形环境下的定位精度。

2、本发明的技术解决方案是：

3、本发明公开了基于阴影匹配的协同定位增强方法，包括：

4、获取待定位用户的初始位置估计信息，以及用户所收到无线通信信号的协同导航成员在地球坐标系中的位置坐标信息；

5、根据初始位置估计信息，确定用户阴影匹配定位的搜索范围，生成采样点集；

6、以所述初始位置估计信息为中心，初始化地形八叉树模型的空间结构；

7、获取用户周围环境的三维点云数据；

8、将所述三维点云数据映射至所述地形八叉树模型，将包含三维点云数据的体素节点标记为占用节点，建立三维地形占用地图；

9、对于所述采样点集中的每一个采样点，以及每一个协同导航成员，构建以该采样点为起点、指向该协同导航成员的射线矢量；

10、利用射线与八叉树相交检测算法，判断所述射线矢量是否经过所述三维地形占用地图中的占用节点；

11、根据所述射线矢量是否经过占用节点，计算各采样点对各协同导航成员的能见性得分，并将所有协同导航成员的得分累加，得到各采样点的总阴影匹配得分；

12、根据各采样点的总阴影匹配得分，得到用户的定位解。

13、进一步地，在上述方法中，所述根据初始位置估计信息，确定用户阴影匹配定位的搜索范围，生成采样点集，具体为：

14、设定阴影匹配搜索半径，得到为以用户初始位置估计为圆心，为半径的圆形区域，得到阴影匹配搜索区域；

15、设定阴影匹配搜索区域内的采样分辨率为，根据所述搜索半径和所述采样分辨率，确定采样点集的规模为，得到采样点集。

16、进一步地，在上述方法中，所述初始化地形八叉树模型，具体为：

17、s31、以初始位置估计信息为中心，建立初始立方体作为八叉树的根节点，并记录所述根节点在物理空间中的起始边界坐标；

18、s32、设置初始体素边长、最大递归深度，以及最小体素边长;

19、s33、根据所述最大递归深度和初始体素边长，计算八叉树模型覆盖的最大物理空间边长，其中；

20、s34、初始化八叉树节点状态：将所述根节点及其后续生成的所有节点占用状态标记为0；

21、进一步地，在上述方法中，所述将所述三维点云数据映射至所述地形八叉树模型，将包含三维点云数据的体素节点标记为占用节点，建立三维地形占用地图，具体为：

22、s41、通过机载或车载激光雷达获取用户周围环境的三维点云数据，所述三维点云数据包含多个点云三维坐标；

23、s42、遍历所述点云数据中的点云三维坐标，对于第个点云，根据当前处理的八叉树节点的体素边长与起始边界坐标，通过向下取整运算计算该点云所在的离散体素网格索引坐标；

24、计算公式为：,,，其中，表示向下取整运算；

25、s43、基于步骤s42得到的体素坐标，将其各分量转换为二进制位序列，通过位交错打包生成对应节点的morton编码m，用于唯一标识八叉树中的节点位置：

26、s44、更新八叉树节点占用状态：根据生成的morton编码 m 定位到八叉树中对应的叶节点，将该叶节点的占用状态标记为1，以构建三维地形占用地图。

27、进一步地，在上述方法中，所述构建以该采样点为起点、指向该协同导航成员的射线矢量，具体为：

28、对于所述采样点集中，第个采样点以及第个协同导航成员，构建射线参数方程；其中，为采样点为起点、指向第个协同导航成员的位置坐标的方向矢量，为射线参数。

29、进一步地，在上述方法中，所述利用射线与八叉树相交检测算法，判断所述射线矢量是否经过所述三维地形占用地图中的占用节点，具体为：

30、s61、将八叉树节点对应的物理空间区域定义为轴对齐包围盒：

31、；

32、计算射线到达各轴向平面的参数值：

33、,，同理计算y、z轴对应的、、及；，；，；

34、令，；

35、若满足且，则判定射线与当前节点相交，执行步骤s62；否则，判定射线未经过八叉树地形中的占用节点；

36、s62、若当前节点为非叶节点且判定相交，则根据方向矢量的分量正负号确定射线在各轴向上的遍历顺序；计算射线穿越当前节点中位平面时的参数值，确定射线进入的子立方体空间索引，其中；

37、将空间索引按x-y-z顺序组合为3位二进制子节点编码；将对应的二进制位拼接至当前节点morton编码的低位，生成子节点morton编码；利用索引八叉树结构获取子节点信息，将该子节点作为新的当前节点；

38、s63、判断新的当前节点是否为叶节点，若否，则执行步骤s61；否则，进入步骤s64；

39、s64、读取叶节点的占用状态；若占用状态为1，则判定该射线矢量经过八叉树地形中的占用节点；若占用状态为0，则判定射线未经过八叉树地形中的占用节点。

40、进一步地，在上述方法中，根据所述射线矢量是否经过占用节点，计算各采样点对各协同导航成员的能见性得分，并将所有协同导航成员的得分累加，得到各采样点的总阴影匹配得分，具体为：

41、对于采样点集中的第个采样点以及第个协同导航成员，若判定从第个采样点指向第个协同导航成员的射线矢量经过八叉树模型中的占用节点，则判定该方向被地形遮挡，令该方向对第个采样点的能见性贡献分；若该射线矢量未经过八叉树模型中的占用节点，则判定该方向未被遮挡，令；

42、将第个采样点对所有个协同导航成员的能见性贡献分累加，得到该采样点的总阴影匹配得分，计算公式为；

43、

44、其中，为协同导航成员的总数，为第个采样点被地形遮挡的协同导航成员数量（即射线经过占用节点的成员数量）；即为第个采样点的阴影匹配得分，表示该位置下能够清晰接收信号的协同导航成员总数。

45、进一步地，在上述方法中，所述根据各采样点的总阴影匹配得分，得到用户的定位解，具体为：

46、获取采样点集中各采样点的总阴影匹配得分中的最高得分；

47、筛选出阴影匹配得分等于的所有采样点，构成最高得分采样点集合；设最高得分采样点集合中包含个采样点，第个采样点的三维空间位置坐标为，其中；

48、对最高得分采样点集合中所有采样点的坐标分量分别求算术平均值，得到用户的定位解，其中:

49、

50、

51、

52、其中，，,分别表示用户定位解在三维空间坐标系中x轴、y轴、z轴方向的直角坐标分量。

53、本发明与现有技术的有益效果在于：

54、1、本发明采用基于协同导航成员与用户周边三维地形信息融合的协同定位增强机制 ，实现了复杂地形环境下对用户位置的高精度定位信息输出，显著提升了导航系统的定位鲁棒性与可用性，解决了传统导航系统在复杂地形中因信号遮挡导致定位精度下降、稳定性差的技术问题；

55、2、本发明提出将无人集群协同机制引入阴影匹配定位框架。通过构建动态的集群协同网络，集群成员之间可共享各自的观测信息（如可见卫星列表、初步位置估计等），实现多视角、多源信息的融合。该方法不仅能有效弥补单个节点因局部遮挡导致的观测信息不足，提升遮挡模式的丰富度与区分度，还可通过加权融合与异常检测机制，抑制误判或异常观测的影响，显著增强定位的准确性与鲁棒性。

56、3、本发明在传统阴影匹配卫星定位技术中引入集群协同，通过多设备信息互补与融合，能够有效克服单点观测局限与匹配模糊性，具备高度技术可行性与广阔应用前景。

57、4、本发明采用基于三维数字地图的gnss卫星与集群协同能见性联合评分与最优匹配算法，实现了在待定位用户可能位置空间中对阴影匹配模式的高效评估与最优位置优选，通过融合多源协同观测信息提升匹配精度，解决了单一节点阴影匹配在复杂地形中因遮挡模式相似性导致定位模糊、精度不足的技术瓶颈。

58、5、本发明采用于一致性的融合定位算法，实现了集群内各成员定位结果的协同优化与异常观测抑制，显著提高了定位结果的鲁棒性与可靠性，解决了传统阴影匹配在相似遮挡区域易产生定位歧义的技术问题。

59、6、本发明采用动态阴影地图在线更新机制，通过集群成员实时上报实际观测的信号遮挡状态，实现对预存阴影地图的自适应修正，提升了系统对临时障碍物与环境变化的适应能力，解决了静态地图难以应对动态城市环境的技术局限。