(19)国家知识产权局 (12)发明 专利申请 (10)申请公布号 (43)申请公布日 (21)申请 号 202210847002.9 (22)申请日 2022.07.06 (71)申请人 同济大学 地址 200092 上海市杨 浦区四平路1239号 (72)发明人 童小华　叶真　谢欢　刘世杰　 陈鹏　冯毅　陈晨　 (74)专利代理 机构 上海科盛知识产权代理有限 公司 312 25 专利代理师 应小波 (51)Int.Cl. G06T 7/73(2017.01) G06F 17/12(2006.01) (54)发明名称 一种月球探测器着陆过程的递增平差连续 位姿估计方法 (57)摘要 本发明涉及一种月球探测器着陆过程的递 增平差连续位姿估计方法， 该方法包括以下步 骤： 步骤1)选取快速调整段至悬停段的降落相机 序列影像， 通过特征匹配获取多重连接点匹配信 息， 采用“双模型相对定向 ‑三影像模型连接 ”进 行初始位姿估计； 步骤2)利用滑动窗口式共面或 共线方程约束， 结合舒尔补边缘化和加权伪观测 约束， 对初始位姿信息进行递增式平差的位姿参 数精化； 步骤3)构建附加外部控制点的递增共线 平差模型对步骤2)的结果进行优化， 从而降低位 姿估计和定位误差。 与现有技术相比， 本发明具 有有效实现了月球探测器着陆降落过程连续位 置姿态参数的高精度快速计算， 附加外部控制点 约束的递增共线平差模型可进一步提高位姿估 计和定位精度等优点。 权利要求书3页 说明书9页 附图2页 CN 115272464 A 2022.11.01 CN 115272464 A 1.一种月球探测器着陆过程的递增平差连续位姿估计方法， 其特征在于， 该方法包括 以下步骤： 步骤1)选取快速调整段至悬停段的降落相机序列影像， 通过特征匹配获取多重连接点 匹配信息， 采用 “双模型相对定向 ‑三影像模型连接”进行初始位姿估计； 步骤2)利用滑动窗口式共面或共线方程约束， 结合舒尔补边缘化和加权伪观测约束， 对初始位姿信息进行递增式平差的位姿参数精化； 步骤3)构建附加外部控制点的递增共线平差模型对步骤2)的结果进行优化， 从而降低 位姿估计和定位 误差。 2.根据权利要求1所述的一种月球探测器着陆过程的递增平差连续位姿估计方法， 其 特征在于， 所述的步骤1)中的通过特征匹配获取多重连接点匹配信息， 采用 “双模型相对定 向‑三影像模型连接”进行初始位姿估计具体为： 首先对选取降落相机序列影 像进行预处理， 将影像上桅杆区域进行掩膜处 理； 然后基于SIFT ‑RANSAC的影像特征匹配策略进行多重连接点匹配以建立多度重叠影像 网络； 再通过相对定向计算出相邻两张降落相机影 像的相对位置关系； 最后依据三重特 征匹配连接点的物方一 致性约束， 对相邻模型进行模型 连接。 3.根据权利要求2所述的一种月球探测器着陆过程的递增平差连续位姿估计方法， 其 特征在于， 所述的模型连接的过程中会产生模型传递间的误差累积， 后续通过修正初始位 姿参数以消除累计误差 。 4.根据权利要求1所述的一种月球探测器着陆过程的递增平差连续位姿估计方法， 其 特征在于， 所述的步骤2)利用滑动窗口式共面或共线方程约束， 结合舒尔补边缘化和加权 伪观测约束， 对初始位姿信息进行递增式平差的位姿参数精化具体为： 201)共面方程约束只涉及对位置姿态参数的精化， 基于共面方程可构建连接点的基础 观测方程， 对观测方程线性 化后得到误差方程： 其中B为系数矩阵， V、 l为残差矩阵、 求 解参数矩阵、 常数项向量； 202)将式(1)所示的误差方程 转化为如下形式： 将所述系数矩阵B分成四个块， M为固定平差滑动窗口， N为边缘化窗口， 共面方程只考 虑对位姿参数的精化， 左上角ΛN存储边缘化的相机外参， 右下角ΛM‑N存储未被边缘化的相 机外参， 每个对角块的维度与相机位姿参数的维度相同， 且均是对角块矩阵； Λ矩阵存储相 机外参与三维点之间的交叉部分； 分别为求解的边缘化相机外参、 未被边缘化相 机外参； 、 lN、 lM‑N分别为与边缘化参数相关的常数项 向量以及未被边缘化参数相关的常数 项向量； 通过高斯消元， 上式可转 化为： 权　利　要　求　书 1/3 页 2 CN 115272464 A 2通过上式可求得 未边缘化窗口内影 像外参 和边缘化窗口内影 像外参 分别为： 其中， ΛM‑N＝BM‑NTPM‑NBM‑N， BM‑N、 PM‑N分别为与未被边 缘化参数相关的系数矩阵及权阵； 203)边缘化窗口中即将要去掉的与相机有关的参数变量， 即相机位姿与仅由它观测到 的物方点 坐标， 误差方程构建如式(6)所示： 其中 为边缘化的相机位姿及由其观测到的物方坐标相关的系数矩阵、 为未边缘化的相机位姿及仅由其观测到的物方坐标相关的系数矩阵、 为 求解的边缘化相机位姿及由其观测到的物方坐标、 为求解的未边缘化相机位姿 及由其观测到的物方坐 标； 分别为边缘化及未边缘化参数相关的常数向 量； 对线性方程组进行高斯消元， 目标 是消去左下角的非对角部分ΛT， 得 消元之后， 方程组第二行变成和 无关的项； 单独拿出来， 得到关于未边缘化部分 的增量方程： 求解完这个方程之后， 把 解得的 代入原方程， 求 解 5.根据权利要求4所述的一种月球探测器着陆过程的递增平差连续位姿估计方法， 其 特征在于， 与共面方程只优化相机 外参不同， 共线方程需同时优化相机 外参与物方点 坐标； 同样基于误差方程(1)， 固定平差滑动窗口为M， 边缘化窗口为N， 系数矩阵B的构造形式 与式(2)有所不同； 假设窗口内有m个相机外参， n个物方点， 由于通常物方点的数量远多于相机外参， 于是 有n＞＞m； 将左上角存储为全部的物方点坐标， 依 次按照逐个相机位置观测到的物方点排 列， 右下角存 储相机外参， 同样按影 像次序顺序排列。权　利　要　求　书 2/3 页 3 CN 115272464 A 3