(19)国家知识产权局 (12)发明 专利申请 (10)申请公布号 (43)申请公布日 (21)申请 号 202211059827.0 (22)申请日 2022.08.29 (71)申请人 中国地质大 学 （武汉） 地址 430000 湖北省武汉市洪山区鲁磨路 388号 (72)发明人 翟超　雷雄　 (74)专利代理 机构 武汉知产时代知识产权代理 有限公司 42 238 专利代理师 魏波 (51)Int.Cl. G01S 13/86(2006.01) G01S 19/42(2010.01) G06N 3/00(2006.01) G06V 10/44(2022.01) G06V 10/75(2022.01) (54)发明名称 基于图像特征匹配的多智能体协同定位方 法、 系统及设备 (57)摘要 本发明公开了一种基于图像特征匹配的多 智能体协同定位方法、 系统及设备， 本发明针对 传统多智能体协同定位方法对抗外界干扰的鲁 棒性差的问题和特征提取与匹配算法中计算量 复杂的问题进行了改进。 通过对相机视野重叠的 智能体获取的图像进行特征提取与匹配， 获得相 邻智能体 之间的相对位姿， 从而解决上述技术问 题。 权利要求书3页 说明书4页 附图2页 CN 115327533 A 2022.11.11 CN 115327533 A 1.一种基于图像特 征匹配的多智能体协同定位方法， 所述智能体均配置有相机； 其特征在于， 包括以下步骤： 步骤1： 智能体通过配置的相机获取图像； 步骤2： 基站获取网络中各个智能体的图像， 并对其进行分析， 获得相机视野重叠率大 于阈值的两个智能体； 步骤3： 基站对步骤2中获得的两个智能体获取的图像进行特征提取与匹配， 获得相邻 智能体之间的相对位姿； 步骤4： 根据相邻 智能体之间的相对位姿， 进行智能体间协同定位。 2.根据权利要求1所述的基于图像特征匹配的多智能体协同定位方法， 其特征在于： 步 骤3中， 智能体A到智能体B的变换为xb＝Rxa+t； 其中， R为旋转矩阵， t为平移向量， xa、 xb分别 表示智能体A、 智能体B的位置坐标； 引入齐次坐标和变换矩阵， 对智能体A、 智能体B之间的变换进行重写： 式(1)中TAB表示变换矩阵； 不相邻的两个智能体A和C之间的变换通过式(2)推导出： 式(2)中TBC、 TAC表示变换矩阵； 从相邻两个智能体的相机拍摄的图像 中提取特征点， 得到n对3D特征点对， 分别表示为 集合P＝{p1,L,pn}和P′＝{p′1,L,p′n}； 定义权重函数φ(pi)∈[0,1)来表示特征点pi在图像中的重要性的正权重， 将特征点pi 及其重要性写成函数fi(p,φ)， 并分别将两个集 合中的点简写为fi和f′i； 需要找到一个欧氏变换， 其中R和t满足： 构建最小二乘问题， 求 解使得误差平方和最小的欧氏变换： 定义两组点的质心为 对式(4)的误差函数进行变形：权　利　要　求　书 1/3 页 2 CN 115327533 A 2求解欧氏变换以获得使得误差项最小的旋转矩阵R和平移向量t， 分别记为R*和t*； 具体 包括以下步骤： (1)计算两组点的质心位置p和p ′， 计算每个点的去质心坐标： qi＝fi‑p， q′i＝f′i‑p′； (2)计算使误差项最小的旋转矩阵 (3)将旋转矩阵代入， 计算 误差项最小的平 移向量： t*＝p‑Rp′； 因此， 展开关于 旋转矩阵的误差项： 由于旋转矩阵R是正交矩阵， 所以RTR＝I， 误差项只有第三项与旋转矩阵有关， 则目标函 数化简为： 定义一个三维矩阵 并对其进行SDV分解得到W＝UΣVT， 其中Σ为奇异值组 成的对角矩阵， 对角线元素依次减小， 矩阵U和V均为正交矩阵； 则目标函数(5)变成： tr(RW) ＝tr(RUΣVT)＝tr(ΣVTRU)， 其中R、 U、 V均为正交矩阵， 则M＝VTRU也是正交矩阵， 其行、 列向 量均为正交的单位向量； M的列向量mj均有 |mij|≤1， 则有： 不等式(6)中等号成立， 即取得最大值的条件为mii＝1， 由于M为正 交矩阵， 因此M为单位 矩阵， 则当矩阵W满秩时， 求得旋转矩阵为： R＝VUT； 最后根据求解欧式变换的步骤(3)求得 平移向量t。 3.根据权利要求2所述的基于图像特征匹配的多智能体协同定位方法， 其特征在于： 通权　利　要　求　书 2/3 页 3 CN 115327533 A 3