CN117073691B - 基于优化的视觉惯性紧耦合航天器姿态测量方法 - Google Patents

Abstract

本发明公开了一种基于优化的视觉惯性紧耦合航天器姿态测量方法，涉及航天器姿态测量技术领域，分为数据预处理，视觉惯性初始化和优化三个部分。测量前端包括视觉特征的跟踪和存储，惯性陀螺仪利用预积分完成状态量的传播，并建立相邻帧约束。初始化通过星敏感器的初始姿态完成视觉惯性对准。后端优化则基于关键帧的滑动窗口法，融合陀螺仪预积分和视觉观测，并边缘化最老帧保留约束信息。最终优化器包含四项约束：陀螺仪预积分、视觉重投影误差、边缘化信息和绝对姿态误差。本发明提出了基于优化的紧耦合融合框架，可以有效降低单一传感器测量缺陷，提高航天器三轴测量精度以及抑制陀螺仪漂移，增强星敏感器空间迷失等情况下姿态输出鲁棒性。

Description

基于优化的视觉惯性紧耦合航天器姿态测量方法

技术领域

本发明涉及航天器姿态测量技术领域，尤其涉及基于优化的视觉惯性紧耦合航天器姿态测量方法。

背景技术

高精度的姿态信息是确保航天器完成各种复杂空间任务的前提。航天器的姿态主要是利用星载姿态敏感器获得的测量信息，估计其在天球坐标系下的三轴姿态，为姿态控制系统提供精确的输入以便实时准确地调整航天器的姿态。

星敏感器是航天器上测量精度最高的单体传感器。但其动态性能差，姿态更新频率较低。此外，当受到杂散光的干扰，可能会出现空间迷失(lost-in-space)无法正常输出。陀螺仪因其体积小、成本低，被广泛用于测量航天器的三轴角速度。陀螺仪具有动态响应快、姿态输出频率高等优点。但是陀螺仪的输出会发生漂移，长期测量会积累误差。因此，常将二者结合进行联合姿态估计。目前，绝大多数融合算法采用滤波类方法，由于提供了马尔可夫假设，其计算速度较快，但也容易陷入局部最优估计，且高度依赖于当前时刻的测量，星敏感器如果无法正常输出则会严重影响整体估计精度。

综上所述，当前所存在的方法在估计精度、姿态输出频率方面存在缺陷，并且忽略了图像信息短期失效的情况。

发明内容

为解决上述技术问题，本发明提供了一种基于优化的视觉惯性紧耦合航天器姿态测量方法，通过批量优化的思想对一段时间的状态量进行联合估计避免陷入局部最优；利用滑动窗口法控制待估计量的规模确保姿态估计的实时性。本发明分为数据预处理，视觉惯性初始化和优化三个部分。测量前端包括视觉特征的跟踪和存储，惯性陀螺仪利用预积分完成状态量的传播，并建立相邻帧约束。初始化通过星敏感器的初始姿态完成视觉惯性对准。后端优化则基于关键帧的滑动窗口法，融合陀螺仪预积分和视觉观测，并边缘化最老帧保留约束信息。最终优化器包含四项约束：陀螺仪预积分、视觉重投影误差、边缘化信息和绝对姿态误差。本发明有效提高航天器三轴测量精度以及抑制陀螺仪漂移，增强星敏感器空间迷失等情况下姿态输出鲁棒性。

为达到上述目的，本发明采用如下技术方案：

本发明的基于优化的视觉惯性紧耦合航天器姿态测量方法包括以下步骤：

步骤一，获取星敏感器图像帧后，将星敏感器观测星点视为特征点进行质心提取，如果获取的是第一帧图像，则进行全天区星图识别，获取恒星星号、赤经赤纬；如果获取的不是第一帧图像，则将上帧识别出的星敏感器观测星点位置与陀螺仪预测星点位置进行匹配，完成恒星识别，并在特征处理器中存储成功识别的星点，将观测到的同一颗恒星由其在星库中的恒星编号作为唯一标识，并记录其赤经赤纬、不同星敏感器图像帧中的位置坐标、被不同图像帧观测到的次数；

步骤二，获取陀螺仪角速度后，利用陀螺仪预积分进行状态量的估计和传播，并建立相邻帧的惯性约束；利用相机/陀螺仪相对位置关系对前一帧图像识别出的星敏感器观测星点在当前帧中进行位置预测；

步骤三，当滑动窗口内图像帧第一次达到预设的数量时，将陀螺仪预积分与星敏感器初始姿态对准，完成陀螺仪漂移初值估计；

步骤四，获取新的星敏感器图像帧后，通过与滑窗内最新的关键帧进行视差判断，判断是否为新的关键帧，当判断为新的关键帧时，边缘化滑窗内的最老关键帧；当判断为非新的关键帧时，边缘化该星敏感器图像帧；

步骤五，在优化器中建立滑窗内所有关键帧之间的视觉重投影误差、陀螺仪预积分、边缘化信息以及绝对姿态误差。

有益效果：

首先，本发明提出了一种基于视觉惯性紧耦合方法，能有效克服单一传感器测量缺陷，保证航天器全天区全天时姿态测量，并极大提升姿态更新频率。其次，利用批量优化的方法对一段时间内航天器的状态量以及陀螺仪的漂移进行估计，能有效解决了滤波方法陷入局部最优的问题，提升姿态估计精度。最后，利用滑动窗口法和边缘化技术控制待估计状态量的规模，保证了测量的实时性。

附图说明

图1为本发明的基于优化的视觉惯性紧耦合航天器姿态测量方法的整体框架示意图。

图2为仿真星图。

图3为视觉重投影误差示意图。

具体实施方式

为了使本发明的目的、技术方案及优点更加清楚明白，以下结合附图及实施例，对本发明进行进一步详细说明。应当理解，此处所描述的具体实施例仅仅用以解释本发明，并不用于限定本发明。此外，下面所描述的本发明各个实施方式中所涉及到的技术特征只要彼此之间未构成冲突就可以相互组合。

首先分别设置星敏感器和陀螺仪的参数如表1所示：

表1

，

根据上述参数将史密松天文台星表中星等亮度超过6Mv的恒星映射到图像中，施加0.1~0.5像素的星点位置误差。并设置航天器的三轴角速度为，连续生成300幅星图，如图2所示。

如图1所示，本发明实施例的基于优化的视觉惯性紧耦合航天器姿态测量方法主要包括如下步骤：

步骤一，获取星敏感器图像帧后，对星敏感器观测星点进行质心提取，如果是第一帧图像则进行全天区星图识别，获取恒星星号，赤经赤纬等信息。如果不是第一帧图像则将提取的星点位置与陀螺仪预测星点位置进行匹配，完成恒星识别，并在特征处理器中存储成功识别的星点，将观测到的同一颗恒星由其在星库中的恒星编号作为唯一标识，并记录其赤经赤纬、不同星敏感器图像帧中的位置坐标、被不同图像帧观测到的次数；

步骤二，首先利用姿态动力学方程进行状态量的估计和传播，其中待估计的状态量包括绝对姿态和陀螺仪漂移/>。

，

其中，和/>是星敏感器相邻帧k和k+1时刻航天器在天球坐标系下绝对姿态四元数，/> 表示四元数的乘法，t是k和k+1之间的任意时刻，/>表示t时刻，航天器相对于/>坐标系的瞬时姿态四元数。/>表示t 时刻，陀螺仪测量角速度，/>是陀螺仪漂移关于时间的导数，/>是陀螺仪三轴角速度，/>是陀螺仪漂移高斯白噪声；/>表示将一个三维矢量映射为4×4矩阵，则/>可表示为：

，

表示陀螺仪三轴角速度；

然后利用预积分建立相邻帧相对运动姿态与陀螺仪漂移误差的关系：

，

其中，是相邻帧相对姿态四元数，/>表示/>的逆运算，/>是陀螺仪漂移误差，/>是相对姿态/>相对于漂移/>的雅各比矩阵，/>表示/>为0时的相对姿态四元数；

最后，利用相机/陀螺仪相对位置关系得到恒星在相机靶面上的递推公式，用于预测所有观测星点的运动轨迹：

，

其中，和/>表示陀螺仪连续采样i和i+1时刻同一颗恒星的图像坐标；/>表示陀螺仪三轴角速度，/>表示i和i+1之间的时间间隔，/>是星敏感器的焦距。

步骤三，最小化滑窗S内所有相邻图像帧相对姿态误差，提供陀螺仪初始漂移估计，完成视觉惯性对准，其中滑窗内所有绝对姿态由星敏感器初始姿态捕获得到：

，

其中，S表示滑窗，包含所有待优化的关键帧，和/>是星敏感器相邻帧k和k+1航天器在天球坐标系下绝对姿态四元数，/>是相邻帧相对姿态四元数，/>是陀螺仪漂移误差。/>表示二范数的平方，其值等于矢量所有元素求欧式距离的平方。/>表示通过求解/>使括号内的表达式值最小。/>表示k从0到滑窗最大值S-1，将括号内所有元素求和。对上式使用Cholesky分解，对陀螺仪漂移误差/>数值求解：

，

其中， 表示四元数的乘法，/>是相对姿态/>相对于漂移/>的雅各比矩阵，/>表示/>的逆运算，/>表示k从0开始一直到滑窗最大值S-1，将括号内所有元素相乘。/>表示只取四元素的虚部。

步骤四，星敏感器捕获新的图像帧后，当完成星点提取和匹配，与滑窗内最新关键帧中相同的星点进行比较，当星点平均位移超过设定的阈值（如10个像素）或观测星点的个数小于设定的阈值（如10颗），则判定为新的关键帧，并边缘化滑窗内最老关键帧，保留其对当前帧的约束信息。反之当位移小于10个像素，则判定为非关键帧，并边缘化该图像帧。

步骤五，首先根据滑窗内所有关键帧之间的约束建立整体损失函数，如下式所示。其中各项分别对应于边缘化信息、陀螺仪预积分、视觉重投影误差、和绝对姿态误差。

，

其中，表示边缘化信息、/>表示陀螺仪预积分、/>表示视觉重投影误差、/>表示绝对姿态误差，X是滑窗内所有待估计状态量，S表示滑窗，包含所有待优化的关键帧，k、i、j表示S中任意一关键帧，/>表示i、j之间的所有共视星点，/>表示S中任意两幅关键帧之间的共视点重投影误差的和，表示S中所有关键帧的观测点和预测点重投影误差和，/>,/>和/>分别表示陀螺仪预积分、视觉重投影误差、绝对姿态误差的信息矩阵，信息矩阵表征的是误差优化时的权重。/>表示二范数的平方，其值等于矢量所有元素求欧式距离的平方。/>表示通过求解/>使括号内的表达式值最小。

假设滑窗内关键帧数为N，第k帧状态量应包含绝对姿态/>和陀螺仪漂移/>：

，

进一步，所述陀螺仪预积分由步骤二建立，表示滑窗内相邻两个关键帧之间由漂移误差构建的惯性约束。

进一步，所述视觉重投影误差表示滑窗内任意两个关键帧之间由共视点l构建的视觉约束。其原理示意图由图3所示。其中和 />是任意两幅关键帧，/> 和/>是对同一个共视点l所产生的观测点，/>是将/>通过旋转矩阵/>旋转至/>的预测点，e即为重投影误差。

进一步，所述绝对姿态误差表示滑窗内所有关键帧的估计姿态与星敏感器解算姿态的误差，其大小由关键帧的观测点和预测点的重投影误差计算。

然后将损失函数放入Google的Ceres优化器进行最小二乘求解，得到最优的状态估计。

最后将估计姿态输出，陀螺仪漂移值更新。

优化前后三轴姿态误差对比如表2所示：

表2

，

最后说明的是，以上实施例仅用以说明本发明的技术方案而非限制，尽管参照较佳实施例对本发明进行了详细说明，本领域的普通技术人员应当理解，可以对本发明的技术方案进行修改或者等同替换，而不脱离本发明技术方案的宗旨和范围，其均应涵盖在本发明的权利要求范围当中。

Claims (6)

1.基于优化的视觉惯性紧耦合航天器姿态测量方法，其特征在于，包括以下步骤：

步骤一，获取星敏感器图像帧后，将星敏感器观测星点视为特征点进行质心提取，如果获取的是第一帧图像，则进行全天区星图识别，获取恒星星号、赤经赤纬；如果获取的不是第一帧图像，则将上帧识别出的星敏感器观测星点位置与陀螺仪预测星点位置进行匹配，完成恒星识别，并在特征处理器中存储成功识别的星点，将观测到的同一颗恒星由其在星库中的恒星编号作为唯一标识，并记录其赤经赤纬、不同星敏感器图像帧中的位置坐标、被不同图像帧观测到的次数；

步骤二，获取陀螺仪角速度后，利用陀螺仪预积分进行状态量的估计和传播，并建立相邻帧的惯性约束；利用相机/陀螺仪相对位置关系对前一帧图像识别出的星敏感器观测星点在当前帧中进行位置预测；

步骤三，当滑动窗口内图像帧第一次达到预设的数量时，将陀螺仪预积分与星敏感器初始姿态对准，完成陀螺仪漂移初值估计；

步骤四，获取新的星敏感器图像帧后，通过与滑窗内最新的关键帧进行视差判断，判断是否为新的关键帧，当判断为新的关键帧时，边缘化滑窗内的最老关键帧；当判断为非新的关键帧时，边缘化该星敏感器图像帧；

步骤五，在优化器中建立滑窗内所有关键帧之间的视觉重投影误差、陀螺仪预积分、边缘化信息以及绝对姿态误差。

2.根据权利要求1所述的基于优化的视觉惯性紧耦合航天器姿态测量方法，其特征在于，所述步骤一包括：对星敏感器观测星点和陀螺仪预测星点进行匹配完成恒星识别后，在特征处理器中存储滑窗内所有关键帧的信息，其中关键帧定义为：当前后两帧图像中观测星点的平均位移超过设定的阈值或观测星点的个数小于设定的阈值，当前图像帧即为关键帧。

3.根据权利要求2所述的基于优化的视觉惯性紧耦合航天器姿态测量方法，其特征在于，所述步骤二包括：

利用姿态动力学方程进行状态量的估计和传播，定义星敏感器与陀螺仪所在的坐标系为本体坐标系b，天球坐标系为世界坐标系w；待估计的状态量包括航天器的姿态和陀螺仪三轴漂移/>：

，

其中，和/>是星敏感器相邻帧k和k+1时刻航天器在天球坐标系下绝对姿态四元数，/> 表示四元数的乘法，t是k和k+1之间的任意时刻，/>表示t时刻，航天器相对于/>坐标系的瞬时姿态四元数；/>表示t 时刻，陀螺仪测量角速度，/>是陀螺仪漂移关于时间的导数，/>是陀螺仪三轴角速度，/>是陀螺仪漂移高斯白噪声；/>表示将一个三维矢量映射为4×4矩阵，则/>表示为：

，

利用预积分建立相邻帧相对运动姿态与陀螺仪漂移误差的关系：

，

其中，是相邻帧相对姿态四元数，/>表示/>的逆运算，/>是陀螺仪漂移误差，/>是相对姿态/>相对于漂移/>的雅各比矩阵，/>表示/>为0时的相对姿态四元数；

利用相机/陀螺仪相对位置关系得到恒星在相机靶面上的递推公式，用于预测所有观测星点的运动轨迹：

，

其中，和/>表示陀螺仪连续采样i和i+1时刻同一颗恒星的图像坐标；/>表示陀螺仪三轴角速度，/>表示i和i+1之间的时间间隔，/>是星敏感器的焦距。

4.根据权利要求3所述的基于优化的视觉惯性紧耦合航天器姿态测量方法，其特征在于，所述步骤三包括：

最小化滑窗内所有相邻图像帧相对姿态误差，提供陀螺仪初始漂移误差估计，完成视觉惯性对准，其中滑窗内所有航天器的姿态由星敏感器初始姿态捕获得到：

，

其中，S表示滑窗，包含所有待优化的关键帧，和/>是星敏感器相邻帧k和k+1时刻航天器在天球坐标系下绝对姿态四元数，/>是相邻帧相对姿态四元数，/>是陀螺仪漂移误差；/>表示二范数的平方，其值等于矢量所有元素求欧式距离的平方；/>表示通过求解/>使括号内的表达式值最小；/>表示k从0开始一直到滑窗最大值S-1，将括号内所有元素求和；

根据所述步骤二中的利用预积分建立相邻帧相对运动姿态与陀螺仪漂移误差的关系，并利用Cholesky分解对漂移误差数值求解：

，

其中， 表示四元数的乘法，/>是相对姿态/>相对于漂移/>的雅各比矩阵，表示/>的逆运算，/>表示k从0到滑窗最大值S-1，将括号内所有元素相乘，表示只取四元素的虚部。

5.根据权利要求4所述的基于优化的视觉惯性紧耦合航天器姿态测量方法，其特征在于，所述步骤四包括：星敏感器捕获新的图像帧后，当完成星敏感器观测星点的提取和与陀螺仪预测星点的匹配后，与滑窗内最新关键帧中相同的星点进行比较，将像素位移作为依据判断当前帧是否为新的关键帧，当前后两帧图像中观测星点的平均位移超过设定的阈值或观测星点的个数小于设定的阈值，则判定为新的关键帧，并边缘化滑窗内最老关键帧，保留其对当前帧的约束信息；当位移小于设定的阈值，则判定为非新的关键帧，并边缘化该星敏感器捕获新的图像帧。

6.根据权利要求5所述的基于优化的视觉惯性紧耦合航天器姿态测量方法，其特征在于，所述步骤五包括：

首先根据滑窗内所有关键帧之间的约束建立整体损失函数，如下式所示；其中各项分别对应于边缘化信息、陀螺仪预积分、视觉重投影误差、和绝对姿态误差：

，

其中，表示边缘化信息、/>表示陀螺仪预积分、/>表示视觉重投影误差、/>表示绝对姿态误差，X是滑窗内所有待估计状态量，S表示滑窗，包含所有待优化的关键帧，k、i、j表示S中任意一关键帧，/>表示i、j之间的所有共视星点，/>表示S中任意两幅关键帧之间的共视点重投影误差的和，表示S中所有关键帧的观测点和预测点重投影误差和，/>,/>和/>分别表示陀螺仪预积分、视觉重投影误差、绝对姿态误差的信息矩阵，信息矩阵表征的是误差优化时的权重；/>表示通过求解/>使括号内的表达式值最小；

假设滑窗内关键帧数为N，第k帧状态量应包含绝对姿态/>和陀螺仪漂移/>：

，

然后将整体损失函数进行最小二乘求解，得到最优的状态估计；

最后将姿态输出，陀螺仪漂移值更新。