一种多无人机相对定位系统及相对定位方法

摘要: 本发明涉及多无人机定位,具体涉及一种多无人机相对定位系统及相对定位方法。背景技术:1、多无人机系统在执行编队控制、目标跟踪、协同建图等任务时,准确的相对定位是基...
一种多无人机相对定位系统及相对定位方法

本发明涉及多无人机定位,具体涉及一种多无人机相对定位系统及相对定位方法。

背景技术:

1、多无人机系统在执行编队控制、目标跟踪、协同建图等任务时,准确的相对定位是基础支撑。现有技术中,多无人机相对定位主要分为两类:一类是基于同步定位与建图(

slam)或视觉/激光里程计的间接定位法,通过外部环境特征估计自身状态后计算相对位姿;另一类是基于传感器互观测的直接定位法,通过视觉标签、

led灯阵、超宽带(

uwb)单元直接测量相对方位角和距离。

2、现有技术存在显著缺陷:其一,依赖外部环境特征或基础设施(如地磁、重力参考系),在无特征区域(地下、太空)或非惯性环境下无法工作;其二,非视距(

nlos)场景下方位角观测丢失,定位失效;其三,缺乏异常值抵抗能力,红外反射、信号干扰易导致优化发散;其四,多机扩展时计算量呈指数增长,难以支持大规模集群;其五,实时性差,无法满足复杂未知环境下的全天候任务需求。这些问题严重限制了多无人机系统的应用场景和可靠性。

3、本发明所涉及的英文缩写、英文全称及中文名称如下:

4、

slam:

simultaneouslocalizationandmapping,同步定位与建图;

5、

uwb:

ultrawideband,超宽带;

6、

imu:

inertialmeasurementunit,惯性测量单元;

7、

pcm-

b:

pairwiseconsistencymeasurement -

b,成对一致性校验算法;

8、

nlos:

non-

lineofsight,非视距;

9、

gps:

globalpositioningsystem,全球定位系统;

10、

led:

lightemittingdiode,发光二极管;

11、

id:

identification,身份识别;

12、

svd:

singularvaluedecomposition,奇异值分解;

13、

wahba:

wahba'

sproblem,瓦赫巴优化问题。

技术实现思路

1、本发明要解决的技术问题至少包含以下之一:解决现有技术依赖外部环境特征导致的无特征区域无法定位的问题;解决非视距场景下方位角观测丢失导致的定位失效问题;解决异常值干扰导致的优化发散问题;解决依赖绝对惯性参考系导致的多机扩展计算量指数增长的问题;解决实时性差无法满足大规模集群协同任务的问题。

2、为解决上述技术问题,本发明提供了以下技术方案。

3、一种多无人机相对定位系统,包括硬件系统,所述硬件系统布设于每架无人机的机载平台上,包括鱼眼摄像头、红外

led单元、红外相机单元、超宽带单元、惯性测量单元、电源单元以及主控单元;

4、所述鱼眼摄像头安装在红外相机单元前端,用于实现对周边多无人机的同时观测;

5、所述红外

led单元设置由多个红外发光二极管构成阵列,每架无人机配置唯一二进制编码图案,用于无人机身份标识与红外特征标记;

6、所述红外相机单元用于采集红外波段图像,过滤可见光环境干扰,通过识别红外

led单元光斑及像素坐标,解算无人机之间的相对方位角;

7、所述超宽带单元布设于无人机机身四角,构成全向测距天线阵列,用于测量本机与其他无人机之间的直线距离,输出全网无人机距离矩阵;

8、所述惯性测量单元安装在无人机机身重心位置,集成三轴加速度计与三轴陀螺仪,实时采集无人机线性加速度和角速度,同时输出本地重力方向观测数据;

9、所述电源单元为无人机上所有硬件单元提供工作电力;

10、所述主控单元用于通过统一硬件触发信号实现红外相机单元、超宽带单元、惯性测量单元的多传感器时间同步,并对采集的原始数据进行预处理、去噪与格式转换。

11、本发明一个实施方式中,还包括软件系统,所述软件系统由下至上依次为数据标准化层、单帧相对位姿估计层、多帧相对位姿估计层以及输出层;

12、所述数据标准化层用于对多传感器原始数据进行时间同步、噪声滤除以及异构数据格式统一转换,输出标准化测量数据;

13、所述单帧相对位姿估计层仅利用当前时刻观测数据,通过多维标度、重力场方向估计、旋转变换、手性判定、偏航解算及坐标变换的分步闭式求解算法,解算得到无人机初始相对位姿,且内置异常值剔除模块,用于滤除方位角观测异常值;

14、所述多帧相对位姿估计层采用滑动窗口机制,融合多帧历史观测数据与

imu预积分信息,对单帧输出结果进行紧耦合优化修正;

15、所述输出层将解算得到的相对位姿、相对里程计信息输出至无人机飞行控制系统。

16、本发明一个实施方式中,所述红外

led单元与红外相机单元按照主控单元触发信号同步工作;红外

led单元按预设周期以唯一编码模式闪烁,红外相机单元同步抓拍红外图像,经光斑检测、亚像素坐标提取、编码解码及相机反投影处理,同步完成无人机身份识别与相对方位角测量。

17、本发明一个实施方式中,数据标准化层采用分级差异化去噪策略;对红外相机图像采用中值滤波结合高斯滤波联合去噪,对超宽带测距采用滑动窗口平均滤波,对惯性测量单元数据采用卡尔曼滤波去噪。

18、本发明一个实施方式中,所述多帧相对位姿估计层设置普通帧滑动窗口与关键帧滑动窗口双分支判断逻辑;无人机定位系统刚启动或跟踪丢失需要初始化时,启用普通帧滑动窗口;正常稳定运行后,启用关键帧滑动窗口,采用边缘化策略淘汰旧帧、加入新帧,维持窗口固定尺寸。

19、本发明还提供了一种多无人机单帧与多帧融合相对定位方法,包括以下步骤:

20、

s1、对无人机机载红外相机、超宽带单元、惯性测量单元采集的原始图像、测距、惯性数据进行硬件触发时间同步、分级去噪和格式标准化转换,输出方位角、距离、重力向量、惯性数据及邻居节点五类标准测量数据;

21、

s2、采用

pcm-

b成对一致性校验算法,检测并剔除方位角观测异常值,保留几何一致的有效观测数据;

22、

s3、利用全网无人机距离矩阵,采用多维标度法,构建双中心化算子,结合距离矩阵元素平方后的矩阵

d2及奇异值分解,仅依靠距离信息求解各无人机在公共参考系下的拓扑位置;

23、

s4、结合本地重力观测与方位角观测构建线性方程组,求解全局重力场方向,对齐公共参考系

z轴,确定俯仰角与横滚角约束;

24、

s5、构建

wahba优化模型,融合方位角观测与重力观测求解各无人机本地坐标系与公共参考系的旋转矩阵,结合行列式修正矩阵,解决多维标度法带来的镜像手性歧义;

25、

s6、通过空间共面度分析计算各节点手性得分,择优确定最优拓扑手性,再通过水平面投影解析求解相对偏航角,将所有无人机状态变换至参考无人机本地机载坐标系下;

26、

s7、以多维标度法、重力场方向估计、旋转变换求解、空间共面度手性决策、偏航解算及坐标系变换分步得到的单帧闭式初始位姿作为迭代初值,构建包含距离、方位角、重力观测的最大似然代价方程,引入马氏距离平方运算进行残差加权,采用非线性优化完成单帧粗优化,输出原始相对位姿与优化后精确相对位姿;

27、

s8、对相邻时刻

imu高频加速度、角速度数据进行预积分,得到帧间位置、速度、姿态增量,同时结合

imu运动信息与图像观测质量自适应筛选关键帧;

28、

s9、以无人机为中心开展重力抵消相对运动学推导,消去绝对重力加速度项,建立不依赖绝对惯性参考系的纯相对机载运动递推公式;

29、

s10、依据系统运行状态进行初始化分支判断,初始化阶段采用普通帧滑动窗口,执行位置-旋转-惯性优化;正常运行阶段采用关键帧滑动窗口,执行方位-距离-惯性优化;

30、

s11、在多帧优化图中引入辅助状态节点,将相对惯性约束解耦为参考残差项与目标残差项,引入

imu预积分协方差矩阵;

31、

s12、融合单帧位姿观测、

uwb距离、红外方位角、

imu惯性约束与先验信息,构建大规模非线性图优化方程,采用列文伯格-马夸尔特法迭代求解,分别输出初始化阶段与正常运行阶段的相对里程计。

32、本发明一个实施方式中,定义

n阶单位矩阵

i、全1矩阵

j,构建双中心化算子

h=1-

(1/n)jj

t,完成多维标度法拓扑位置求解。

n为无人机数量,

j为

n×n全1矩阵。

33、本发明一个实施方式中,单帧相对位姿估计设置是否使用重力参考的分支逻辑;启用重力参考时依次进行重力估计、偏航角估计;不启用重力参考时直接进入位置手性判定。

34、本发明一个实施方式中,多帧优化采用固定大小滑动窗口边缘化机制,新增关键帧入窗、最旧关键帧边缘化移出,在保证定位精度的同时控制计算量,满足机载平台实时性要求。

35、本发明一个实施方式中,

pcm-

b成对一致性校验算法通过计算方位角实测几何夹角与理论几何夹角的偏差,以3倍联合线性化标准差作为一致性判定阈值,构建无向一致性图并求取最大完备子图,实现方位角异常值剔除。

36、本发明具有以下技术效果:

37、第一,具有极强的抗极端异常值的鲁棒性。本发明创新性地在单帧闭式解阶段引入-成对一致性校验算法,能够稳健地处理高达90%的方位角观测异常值。这使得系统在存在大量红外反射、信号干扰和复杂光照条件的环境中,仍然能够保持稳定的定位性能,避免了优化过程的发散。

38、第二,摆脱了对全局设施与环境的依赖。本发明的算法推导完全基于相对运动学,不依赖任何绝对惯性假设,如地磁场或固定重力方向。这使得系统不仅适用于常规的地面环境,还可在无特征区域、地下、水下甚至太空等非惯性环境中实现无漂移定位,大大扩展了系统的应用范围。

39、第三,突破了视距限制。本发明通过单帧闭式解对全网无人机距离矩阵的充分利用,结合多帧预积分的深度融合,使得系统在多无人机系统中即便存在严重的遮挡导致视线丢失,仍能保持连续且高精度的定位输出。这使得系统能够在有障碍物的复杂环境中正常工作,提高了系统的适应性。

40、第四,具有很高的定位精度和实时性。本发明采用单帧闭式解初始化加多帧紧耦合优化的两级架构,有效解决了多传感器融合优化的局部极小值问题。单帧闭式解提供了非常好的初值,使得多帧优化能够快速收敛,在便携式机载计算平台上即可达到实时解算要求,实现厘米级和度级的定位精度。

41、第五,具有良好的扩展性。本发明的算法结构能够很容易地向大规模多无人机集群扩展。随着无人机数量的增加,系统的计算量只会线性增加,而不会呈指数增加。这使得系统能够支持几十甚至上百个无人机的大规模集群协同任务。

42、综上所述,本发明的基于多机相对定位的无人机定位系统解决了现有技术中存在的诸多问题,具有鲁棒性强、适应性广、精度高、实时性好、扩展性好等优点,能够满足复杂、未知环境下的多无人机全天候协同任务的需求,具有重要的理论意义和实用价值。