(19)国家知识产权局 (12)发明 专利 (10)授权公告 号 (45)授权公告日 (21)申请 号 202210883012.8 (22)申请日 2022.07.26 (65)同一申请的已公布的文献号 申请公布号 CN 114964266 A (43)申请公布日 2022.08.30 (73)专利权人 中国人民解 放军国防科技大 学 地址 410073 湖南省长 沙市开福区德雅路 109号 (72)发明人 刘一麟　向文楷　曹聚亮　潘献飞　 涂哲铭　褚超群　周文舟　 (74)专利代理 机构 长沙国科天河知识产权代理 有限公司 432 25 专利代理师 赵小龙 (51)Int.Cl. G01C 21/20(2006.01)G06T 7/62(2017.01) G06T 7/55(2017.01) G06T 7/73(2017.01) G06V 10/44(2022.01) G06V 10/764(2022.01) 审查员 章慧敏 (54)发明名称 基于多视觉矢量的运动状态协同群组相对 姿态确定方法 (57)摘要 本发明公开了一种基于多视觉矢量的运动 状态协同群组相对姿态确定方法， 首先， 寻找双 目镜头内的多个特征目标， 分别得到其相对观测 者的位置信息， 并利用其坐标构建多个不共线的 矢量； 面对同类目标检测算法不可识别的情况， 利用JPDA算法将同类目标标签化处理加以分辨， 确保集群内所有参与观测的节 点向量观测一致； 最后根据视觉矢量的构建情况选择合适的姿态 快速解算算法解算姿态转换矩阵。 本发明应用于 协同导航领域， 充分利用了摄像头捕捉到特征目 标构建视觉信息矢量， 在协同群组具有共视目标 的情况下， 利用姿态解算算法解算协同群组相对 姿态转换矩阵， 提高了协同导航算法相对定姿精 度， 具有计算简单， 鲁棒性强， 定姿精度高的优 点。 权利要求书3页 说明书6页 附图2页 CN 114964266 B 2022.10.21 CN 114964266 B 1.一种基于多视觉矢量的运动状态协同群组相对姿态确定方法， 其特征在于， 包括以 下步骤： 步骤1， 获取协同群组内两载具平台共视区域的多个特征目标， 并分别解算各特征目标 在观测载 具镜头坐标系与目标 载具镜头坐标系下的探测坐标； 步骤2， 基于步骤1解算的探测坐标， 得到同一时刻帧下， 观测载具镜头坐标系中所有的 观测平台视觉矢量， 以及目标载具镜头坐标系中所有的目标平台视觉矢量； 其中， 观测平台 视觉矢量由观测载具镜头坐标系中任意两个不同目标的探测坐标相连得到， 目标平台视觉 矢量由目标载具镜头坐标系中任意两个不同目标的探测坐标相连得到， 即观测平台视觉矢 量与目标平台视觉矢量一一对应， 且一个观测平台视觉矢量与对应的目标平台视觉矢量组 成一组矢量对； 步骤3， 基于至少两组矢量对， 利用姿态解算算法得到观测载具与目标载具之间的相对 姿态转换矩阵， 即完成相对姿态确定； 步骤3中， 所述基于至少两组矢量对， 利用姿态解算算法得到观测载具与目标载具之间 的相对姿态转换矩阵， 具体为： 根据目标观测的精确性以及观测平台视觉矢量 、 目标平台视觉矢量 的共线性选 择对应的姿态解算算法， 计算最小化损失函数 的矩阵， 即可得到观测载具与目标载具之间 的最优姿态转换矩阵； 其中， 为j时刻帧第 i个特征目标 与第k个特征目标 形成的观测平台视觉矢 量， 为j时刻帧第 i个特征目标 与第k个特征目标 形成的目标平台视觉矢量， i=1~ M、k=1~M且i≠k； 所述选择对应的姿态解 算算法， 具体为： 步骤3.1， 计算目标侦测的置信系数； 步骤3.2， 判断置信系数 是否大于第一阈值， 若是则进入步骤3.3， 否则进入步骤3.4； 步骤3.3， 计算任意两个观测平台视觉矢量之间的第 一共线度， 以及任意两个目标平台 视觉矢量之间的第二共线度： 若存在第一共线度低于第 二阈值的两个观测平台视觉矢量， 则任意舍弃其中一个观测 平台视觉矢量， 以及舍弃与之对应的目标平台视觉矢量， 再重新计算第一共线度与第二共 线度， 直至不存在第一共线度低于第二阈值的情况； 若存在第二共线度低于第 二阈值的两个目标平台视觉矢量， 则任意舍弃其中一个目标 平台视觉矢量， 以及舍弃与之对应的观测平台视觉矢量， 再重新计算第一共线度与第二共 线度， 直至不存在第二共线度低于第二阈值的情况； 若存在一个第一共线度或第二共线度大于第二阈值且低于第三阈值， 则选择QUEST算 法作为姿态 解算算法， 否则选择ESOQ2 算法作为姿态 解算算法； 并以剩余的观测平台视觉矢 量、 目标平台视 觉矢量作为姿态解 算算法的输入； 步骤3.4， 计算任意两个观测平台视觉矢量之间的第 一共线度， 以及任意两个目标平台 视觉矢量之间的第二共线度： 若存在第一共线度低于第 二阈值的两个观测平台视觉矢量， 则任意舍弃其中一个观测权　利　要　求　书 1/3 页 2 CN 114964266 B 2平台视觉矢量， 以及舍弃与之对应的目标平台视觉矢量， 再重新计算第一共线度与第二共 线度， 直至不存在第一共线度低于第二阈值的情况； 若存在第二共线度低于第 二阈值的两个目标平台视觉矢量， 则任意舍弃其中一个目标 平台视觉矢量， 以及舍弃与之对应的观测平台视觉矢量， 再重新计算第一共线度与第二共 线度， 直至不存在第二共线度低于第二阈值的情况； 若存在一个第一共线度或第二共线度大于第二阈值且低于第三阈值， 则选择SVD算法 作为姿态 解算算法， 否则选择FLAE算法作为姿态解算算法； 并以剩余的观测平台视觉矢量、 目标平台视 觉矢量作为姿态解 算算法的输入； 步骤3.3与步骤3.4中， 所述第一共线度与第二共线度的计算过程 为： 对于任意两个第一视觉矢量 与 ， 或任意两个第二视 觉矢量 与 ， 其对应的第一共线度或第二共线度为： 式中， 为第一共线度或第二共线度。 2.根据权利要求1所述的基于多视觉矢量的运动状态协同群组相对姿态确定方法， 其 特征在于， 步骤1中， 所述特 征目标包括但不限于汽车、 树木与建筑。 3.根据权利要求2所述的基于多视觉矢量的运动状态协同群组相对姿态确定方法， 其 特征在于， 步骤1中， 所述分别解算各特征目标在观测载具镜头坐标系与目标载具镜头坐标 系下的探测坐标， 具体为： 若两载具平台共视区域中共有 M个特征目标， 首先获取目标 i在观测载具、 目标载具像 素坐标中的图像坐标； 再根据对极几何原理能够使用像素坐标系中的坐标， 分别计算出特征目标 i在摄像头 坐标系中的坐标， 分别为 、 ； 其中， 为j时刻帧第 i个特征目标 在观测载具镜头坐标系下的探测坐标， 为j时 刻帧第i个特征目标 在目标载具镜头坐标系下的探测坐标， i=1~M。 4.根据权利要求1或2或3所述的基于多视觉矢量的运动状态协同群组相对姿态确定方 法， 其特征在于， 在步骤1 中， 对于观测到的同类而 无法分辨的目标， 采用JPDA 算法将目标标 签化以在集群中进行分辨， 进 而将每一个探测坐标归属到对应的特 征目标。 5.根据权利要求1或2或3所述的基于多视觉矢量的运动状态协同群组相对姿态确定方 法， 其特征在于， 步骤3.1中， 所述计算目标侦测的置信系数， 具体为： 计算观测载 具目标侦测的第一置信系数， 为：权　利　要　求　书 2/3 页 3 CN 114964266 B 3