(19)国家知识产权局 (12)发明 专利申请 (10)申请公布号 (43)申请公布日 (21)申请 号 202210685392.4 (22)申请日 2022.06.17 (71)申请人 中国人民解 放军军事科学院国防科 技创新研究院 地址 100071 北京市丰台区东大街5 3号 (72)发明人 谢年好　陈磊　苏文山　李东泽　 (74)专利代理 机构 北京奥文知识产权代理事务 所(普通合伙) 11534 专利代理师 张文 (51)Int.Cl. G06T 7/73(2017.01) G06T 7/246(2017.01) G06T 7/00(2017.01) (54)发明名称 一种仅基于单目视觉信息的无人机编队相 对位姿估计方法 (57)摘要 本发明公开了一种仅基于单目视觉信息的 无人机编队相对位姿估计方法， 包括： 建立相机 投影模型； 获取第一帧图像进行特征点检测， 确 定检测胞和孤立特征点集； 计算完整检测胞对应 的无人机的三维位置和姿态； 获取下一帧图像进 行特征点检测， 确定检测胞和孤立特征点集， 预 测目标无人机当前时刻的三维位置； 将三维位置 预测值下无人机的预设显著特征点重投影到相 机像素坐标系， 得到重投影完整像素胞， 计算重 投影完整像素胞与检测胞和孤立特征点集的最 小差异度， 根据差异度确定跟踪结果； 确定是否 存在未关联完整检测胞， 若是， 计算未关联完整 检测胞对应的无人机的三维位置和姿态并作为 跟踪结果输出。 本发明能同时解决重建和跟踪问 题， 有效应对 遮挡问题。 权利要求书4页 说明书12页 附图3页 CN 115170656 A 2022.10.11 CN 115170656 A 1.一种仅基于单目视觉信息的无人机编队相对位姿估计方法， 其特征在于， 包括以下 步骤： S1， 确定无人机上的至少四个预设显著特征点在其机体坐标系下的三维坐标， 确定每 个无人机搭载的相机的内外参数， 建立相机投影模型； S2， 从相机中获取第一帧图像， 对获取的图像进行特征点检测， 根据检测得到的特征点 与各个无人机的几何关系， 确定无人机对应的检测胞和孤立特征点集， 其中， 无人机对应的 检测胞表示与当前无人机关联、 且与当前无人机不存在关联模糊性的特征点集合， 孤立特 征点集表示与无 人机存在关联模糊性而未被关联到任意无 人机的特 征点集合； S3， 确定完整检测胞， 根据预设显著特征点的三维坐标， 计算完整检测胞对应的无人机 的三维位置和姿态， 其中， 完整检测胞表示包括四个以上 特征点的检测胞； S4， 从相机中获取下一帧图像， 对获取的图像进行特征点检测， 根据检测得到的特征点 与各个无 人机的几何关系， 确定无 人机对应的检测胞和孤立特 征点集； 针对每个目标无人机， 在保持姿态不变的情况下， 根据目标无人机前一时刻的三维位 置预测目标无人机当前时刻的三维位置， 其中， 目标无人机为已知前一时刻的三维位置和 姿态的无 人机； S5， 针对每个目标无人机， 根据相机投影模型， 将三维位置预测值下的目标无人机的预 设显著特征点重投影到相 机像素坐标系， 得到三维位置预测值对应的重投影完整像素胞， 计算三维位置预测值对应的重投影完整像素胞与步骤S4获取的所有检测胞和孤立特征点 集的最小差异度， 若最小差异度不大于预设阈值， 则根据三维位置预测 值更新目标无人机 的姿态， 输出三维位置预测 值和姿态作为对应的目标无人机的跟踪结果， 并将最小差异度 对应的检测胞或孤立特征点子集标记为已关联， 若最小差异度大于预设阈值， 则将对应的 目标无人机视为跟踪丢失； S6， 对剩余的未关联的检测胞和孤立特征点集进行重新检测筛选， 确定是否存在未关 联的完整检测胞， 若存在未关联的完整检测胞， 则根据预设显著特征点的三 维坐标， 计算所 有未关联的完整检测胞对应的无人机的三维位置和姿态， 并将三 维位置和姿态作为对应的 无人机的跟踪结果输出； S7， 返回步骤S4， 直至无 人机编队任务结束。 2.根据权利要求1所述的仅基于单目视觉信息的无人机编队相对位姿估计方法， 其特 征在于， 所述预设显著特 征点包括： 无 人机的翼尖、 尾翼尖、 和/或人工布设的合作标志。 3.根据权利要求1或2所述的仅基于单目视觉信 息的无人机编队相对位姿估计方法， 其 特征在于， 设定： 相机坐标系O ‑XYZ为基准坐标系， 且相机坐标系 与无人机机体坐标系重合， 相机图像坐标系 为o‑xy， 相机像素坐标系为o ‑uv， 点P在第j个无人机的机体坐标系下的三 维坐标为 所述相机投影模型为：权　利　要　求　书 1/4 页 2 CN 115170656 A 2其中， 为第j个无人机的重心Oj在相机坐标系下的三维坐标， 为点P在相机像素坐标系下的坐标， Rj为第j个无人机的姿态旋转矩阵， 表示3x3矩阵， fx和fy表示相机在x方向和y方向的等效焦距， cx,cy表示相机主点在相机 像素坐标系下的坐标， K表示相机内参数构成的内参矩阵。 4.根据权利要求1 ‑3中任一项所述的仅基于单目视觉信 息的无人机编队相对位姿估计 方法， 其特征在于， 所述根据预设显著特征点的三维坐标， 计算完整检测胞对应的无人机的 三维位置和姿态， 包括： 确定完整检测胞对应的无人机的预设显著特征点在机体坐标系下的三维坐标， 根据 Perspective ‑n‑Point原理， 计算确定预设显著特 征点到相机光心的距离； 根据迭代最近点 算法， 计算预设显著特 征点在相机坐标系下的三维坐标； 根据无人机的预设显著特征点与重心间的相对位置关系， 计算确定无人机的三维位置 和姿态。 5.根据权利要求4所述的仅基于单目视觉信息的无人机编队相对位姿估计方法， 其特 征在于， 所述根据目标无人机前一时刻的三维位置预测目标无人机当前时刻的三维位置， 包括： 根据目标无人机前一 时刻的三维位置， 通过目标无人机的动力学模型计算预测目标无 人机在当前时刻的三维位置初始预测值； 基于当前时刻的三维位置初始预测值， 通过粒子采样生成多个粒子预测值， 根据相机 投影模型， 将每个粒子预测值下的目标无人机的预设显著特征点重投影到相机像素坐标 系， 得到每 个粒子预测值对应的重投影完整像素 胞； 根据粒子预测值对应的重投影完整像素胞与步骤S4获取的所有检测胞和孤立特征点 集的差异度， 更新粒子预测值对应的权重， 根据每个粒子预测值及其对应的权重， 计算目标 无人机当前时刻的三维位置预测值。 6.根据权利要求5所述的仅基于单目视觉信息的无人机编队相对位姿估计方法， 其特 征在于， 设定第j个目标无人机的第i个粒子预测值对 应的重投影完整 像素胞为 通过以下 方式获取： 利用以下公式计算第i个粒子预测值下的第j个目标无人机的预设显著特征点在相机 坐标系下的坐标； 根据预设显著特征点在相机坐标系下的坐标， 利用以下公式重投影出预设显著特征点 在相机像素坐标系下的坐标， 得到 重投影完成像素 胞；权　利　要　求　书 2/4 页 3 CN 115170656 A 3