(19)国家知识产权局 (12)发明 专利 (10)授权公告 号 (45)授权公告日 (21)申请 号 202211081054.6 (22)申请日 2022.09.06 (65)同一申请的已公布的文献号 申请公布号 CN 115166686 A (43)申请公布日 2022.10.11 (73)专利权人 天津大学 地址 300072 天津市南 开区卫津路9 2号 (72)发明人 田栢苓　卢俊杰　谌宏鸣　左宗玉　 (74)专利代理 机构 北京圣州专利代理事务所 (普通合伙) 11818 专利代理师 黄青青 (51)Int.Cl. G01S 7/48(2006.01) G06T 7/73(2017.01)(56)对比文件 CN 106595659 A,2017.04.26 CN 112000130 A,2020.1 1.27 审查员 张耀天 (54)发明名称 卫星拒止环境下多无人机分布式协同定位 与建图方法 (57)摘要 本发明公开了卫星拒止环境下多无人机分 布式协同定位与建图方法， 包括： 无人机自主定 位， 每架无人机利用激光雷达提供的原始点云数 据提取自身的点云特征信息， 然后估计自身的自 主定位信息； 多无人机协 同定位， 每架无人机利 用其他无人机发布的点云特征信息在自身的特 征地图上做匹配， 从而计算其他无人机在自身参 考坐标系中的相对位姿信息； 多无人机协同建 图， 每架无人机在自身参考坐标系下， 利用自主 定位信息、 相对位姿信息、 点云特征信息维护一 张全局特征点云地图。 本发明采用上述定位与建 图方法， 拒止环境下， 无需外部传感器或高性能 地面处理器， 即可使多无人机系统中的每架无人 机均维护一张全局一 致的特征地图。 权利要求书4页 说明书11页 附图3页 CN 115166686 B 2022.11.11 CN 115166686 B 1.卫星拒止环境下多无 人机分布式协同定位与建图方法， 其特 征在于， 包括： 无人机自主定位， 每架无人机利用激光雷达提供的原始点云数据提取自身的点云特征 信息， 然后估计自身的自主定位信息； 多无人机协同定位， 每架无人机利用其他无人机发布的点云特征信 息在自身的特征地 图上做匹配， 从而计算 其他无人机在自身参 考坐标系中的相对位姿信息； 多无人机协同建图， 每架无人机在自身参考坐标系下， 利用自主定位信息、 相对位姿信 息、 点云特 征信息维护一张全局特 征点云地图； 其中， 无人机自主定位的具体步骤 包括： S11、 从激光雷达点云中提取特征点集 ， 包括位于平坦区域的面特征点集 和位于边 缘或尖锐区域的线特 征点集 ； S12、 通过 特征点集 与特征地图 的匹配关系构建雷达约束残差； 特征点在无 人机自身参 考坐标系下的位置： 线特征点集的匹配残差： 面特征点集的匹配残差： 式中， 为特征点云 中的特征点， 为无人机在自身参考坐标系下的位姿初值； 为线特征点集中的特征点， 与 为无人机自身特征地图 中距离 最近 且不在同一条扫描线上的任意两个点； 为面特征点集中的特征点， 、 、 为无人机自身特 征地图 中距离 最近的任意 三个点； S13、 根据特征点集的匹配残差， 将基于雷达特征 ‑地图匹配的无人机自主定位问题转 化为非线性最小二乘问题； 通过高斯 ‑牛顿法对非线性最小二乘问题的迭代求解， 即可实现 多无人机系统中每架无 人机对自身位姿 的估计； 式中， 为无人机在自身参考坐 标系中的位姿， 为通过无人机在自身参考坐 标系中权　利　要　求　书 1/4 页 2 CN 115166686 B 2位姿的估计值， 为线特征点集 中特征点的数量， 为面特征点集 中特征点的数量。 2.根据权利要求1所述的卫星拒止环境下多无人机分布式协同定位与建图方法， 其特 征在于， S1 1步骤中依据平 滑度确定特 征点集 ， 具体地， 平滑度： 式中， 为激光雷达点云中的点， 为 的邻域点集， 为 的邻域点集中点的数 量， 为向量的范 数， 为 的邻域点集中包含的点； 依据平滑度值对点云排序， 最大的数十个点即为线特征点集 ， 最小的数十个点即为 面特征点集 ， 且规定一个特 征点周围10个邻近点 不能作为特 征点。 3.根据权利要求2所述的卫星拒止环境下多无人机分布式协同定位与建图方法， 其特 征在于， 多无 人机协同定位的具体步骤 包括： S21、 构建无 人机的自主定位因子； 无人机自主定位观测： 位姿增量： 位姿观测 增量： 自主定位因子： 式中， 表示 时刻的无人机自主定位观测矩阵， 表示 时刻的无人机自主 定位观测矩阵， 表示 至 间无人机的位姿增量， 表示无人机自主定位观测 协方差； 表示 时刻 在 参考坐标系下的位姿的逆， 表示 时刻 在 参考坐标系下的位姿； 表示 时刻 自主定位观测矩 阵的逆矩阵； 表示 时刻 相对于 参考坐标系位姿矩阵的估计矩阵 对应的逆矩阵， 表示 时刻 相对于 参考坐标系位姿矩阵的估计 矩阵； S22、 构建无 人机的相对定位因子；权　利　要　求　书 2/4 页 3 CN 115166686 B 3