融合轨道动力学的小行星探测器自主视觉定位系统及方法

1.本发明涉及小行星探测领域，具体涉及一种融合轨道动力学的小行星探测器自主视觉定位系统及方法。


背景技术：

2.小行星探测以获取其资源成分、形态结构和运动特征等基本信息为目标，对于认识太阳系、研究生命起源和演化具有重大意义。由于小行星距离地球距离远，通讯传输延迟长，需要探测器在接近目标天体时具备自主探测能力，其中自主导航和表面建模是行星自主探测的关键技术，能实现获取航天器相对于小行星表面的位置、姿态及三维模型等基本信息。
3.传统航天器导航方式，一般基于累积误差不受控的惯性元件，或基于功耗较高、测量范围受限的激光雷达等主动传感器，在周期长、能耗有限的深空探测任务中应用效果有限。因此，深空自主导航系统普遍采用能耗低且能获取丰富图像信息的视觉导航系统。但是现有的行星视觉导航方法需要对小行星进行长期的绕飞观测，并且要求大量人工参与重建天体模型和标记路标特征，不适应未来自主探测导航的发展需求。另一方面，提前标记参考特征库的方法，容易受光照、尺度、视角变化影响，在特征提取和识别中引入误差，进而影响导航定位精度。
4.针对以上问题，本发明提出一种融合轨道动力学的深空探测器自主视觉定位方法，能在缺乏表面先验信息、无人工手动标记的场景下，通过融合轨道动力学的轨道改进技术实现探测器的高精度视觉导航，并快速建立小行星表面三维模型。该方法能优化探测器初始定位结果，从而实现高精度导航，能够为未来我国的深空探索提供有效技术支撑。


技术实现要素：

5.本发明的目的是解决现有深空自主视觉导航系统需对小行星进行长时间的前期绕飞观测，同时提前标记的特征参考容易受光照、尺度、视角变化影响的问题。
6.为解决上述技术问题，本发明提出一种融合轨道动力学的深空探测器自主视觉定位系统及方法，能在缺乏表面先验信息、无人工手动标记的场景下，通过融合轨道动力学的轨道改进技术实现探测器的高精度视觉导航，并建立小行星表面三维模型。
7.本发明提供了一种融合轨道动力学的小行星探测器自主视觉定位系统，所述系统包括视觉传感器、视觉slam定位模块、引力场建模模块和轨道优化模块；
8.所述视觉传感器，用于拍摄小行星表面的图像信息；
9.所述视觉slam定位模块，用于接收相机拍摄的小行星表面的图像信息，并对小行星表面的图像特征进行提取匹配与跟踪，通过因子图优化算法估计探测器位姿，通过回环检测修正视觉累积误差；
10.所述引力场建模模块，用于接收相机拍摄的小行星表面的图像信息，并根据绕飞段的图像由sfm算法恢复相机运动并重构行星表面三维模型，通过mvs进行稠密重建，随后
对点云进行网格化处理，再基于多面体法对不规则小行星引力场建模；
11.所述轨道优化模块，用于根据所述视觉slam定位模块估计的探测器相对小行星位置姿态和所述引力场建模模块求解的行星不规则引力场来分析反演视觉初始定轨误差在轨道动力学中的传播过程，并对视觉定位累计误差进行修正；
12.所述定位系统设计了一种基于轨道动力学的伪相对运动分析精确轨道优化算法，基于定位模块估计的探测器相对小行星位置姿态和引力场建模模块求解的行星不规则引力场，分析反演视觉初始定轨误差在轨道动力学中的传播过程，修正视觉定位累计误差，改善初始定位结果。
13.本发明提供了一种融合轨道动力学的小行星探测器自主视觉定位方法，其特征在于，所述方法包括以下步骤：
14.首先，通过视觉传感器获取小行星表面图像信息；
15.其次，视觉slam定位模块根据获取的图像信息对小行星表面的特征进行提取匹配，通过因子图优化算法估计探测器位姿，并进行回环检测；
16.同时，引力场建模模块根据获取的图像信息重构行星表面三维模型，实现基于多面体法的行星不规则引力场建模；
17.最后，轨道优化模块基于估计的探测器位姿和引力场建模求解行星不规则引力场，分析反演视觉初始定轨误差在轨道动力学中的传播过程，修正视觉定位累计误差，实现高精度导航定位。
18.作为上述技术方案的一种改进，所述方法在探测过程中进行坐标系定义：定义小行星固连坐标系为并以此作为世界坐标系，其中，a为坐标系原点，通常是为小行星质心；定义探测器坐标系为其中，s为探测器本体坐标系原点，在绕飞时轴指向小行星质心，为迹向方向，为及叉乘结果；定义相机坐标系为在设定中相机位于探测器正前方，三轴与探测器坐标系一致；定义像平面坐标系为
19.作为上述技术方案的又一种改进，所述方法采用视觉slam相对定位算法对小行星表面特征进行提取，并根据提取的路标特征实时计算探测器相对于环境的位置和姿态，并建立环境地图；
20.所述视觉slam相对定位算法包括三个线程：追踪、局部建图及回环检测；
21.所述追踪线程主要负责根据输入图像提取特征进行初始位姿估计，并建立关键帧；所述局部建图线程根据关键帧建立局部地图；所述回环检测线程通过词袋模型检测候选帧是否回环，最后优化全局地图并更新。
22.作为上述技术方案的另一种改进，所述视觉slam相对定位算法对小行星表面进行特征提取时，使用orb(oriented fast and rotated brief)特征提取算法进行特征提取，所述orb特征提取算法使用增强的fast(features from accelerated segment test)提取关键点，并使用方向归一化的brief(binary robust independent elementary features)描述子对关键点进行描述。
23.作为上述技术方案的再一种改进，所述追踪线程将输入的序列图像与地图点进行匹配计算相机位姿，具体步骤为：将第一帧图像的相机坐标系作为系统参考坐标系，通过序列图像的帧间匹配可以传递特征点的世界坐标，在帧间进行3d-3d对应估计可以求解参考
坐标系与每帧图像对应的位姿关系；在前一帧追踪成功时，采用匀速运动模型或关键帧模型获得初始的位姿矩阵，再基于因子图优化模型最小化3d-2d重投影误差函数，迭代求解得到更优的位姿，其中误差函数为：
[0024][0025]
其中，是系统参考坐标系相对于第i帧相机坐标系的旋转和平移向量，(uj,vj)为第j个特征点的像素坐标，n为匹配到的特征点总和，ρ()为huber损失函数，用于外点的剔除，project()表示3d-2d重投影误差函数，xj为第j个匹配点在参考坐标系中的x坐标，∑为和每个特征点的尺度相关的信息矩阵；
[0026]
在对每帧影像进行以上操作的同时，所述局部建图线程负责在当前地图插入关键帧和筛选、产生新的地图点，实时地优化局部若干帧的位姿及相应3d点的位置，对误差进行修正。
[0027]
作为上述技术方案的更一种改进，所述回环监测线程使用词袋模型，将每张图像构建为一个词袋向量，通过相似度度量：
[0028][0029]
其中，vi和vj为对应的两幅图像的词袋向量，来评价两幅图像的相似度得分，判断是否进行回环检测。
[0030]
作为上述技术方案的还一种改进，所述方法通过从运动恢复结构(structure from motion，sfm)结合多视角立体几何(multi-view stereo，mvs)算法依靠绕飞拍摄的图像数据实现三维重建，具体流程为：
[0031]
在输入小行星表面序列图像之后，先通过sfm算法恢复相机运动信息和内外参，建立行星表面稀疏点云，具体步骤为：利用sift算法进行特征提取和匹配，随后对图像对进行几何验证即求解基础矩阵f并去除外点；在初始化时选择重叠度较高的两张图像，并增量式地依次添加新的图像和其特征点云坐标；使用光束平差法(bundle adjustment，ba)优化相机内外参和特征点云，并将此作为mvs算法的输入；其中，光束平差法通过最小化重投影误差优化相机参数，定义n个空间点pi＝[xi,yi,zi]
t
在像素平面的投影坐标为ui＝[ui,vi]
t
，相机的旋转r和平移变换t的李群表示为t，k为相机内参矩阵，构建如下列公式所示的最小二乘问题：
[0032]
其中，si是距离尺度
[0033]
来求解最优的相机位姿；
[0034]
mvs稠密建图部分通过融合深度图实现，具体步骤为：首先由立体匹配patchmatch算法进行视差估计得到深度图，并对深度图进行融合和滤波处理，并进一步生成点云的法向量，并使用泊松重建方法进行表面重建。
[0035]
作为上述技术方案的进一步的改进，所述方法选择多面体法进行引力场重建，所
述多面体法计算得到的引力势能和引力加速度分别是：
[0036][0037][0038]
其中，u(r)表示引力势，g表示万有引力常量，ρ是小行星密度，e是edge，表示棱边，f是face，表示面片，r表示任一点(x,y,z)位置向量，re是该点到多面体上面a和面b棱边上一点(x0,y0,z0)的向量((x
0-x),(y
0-y),(z
0-z))，面a和面b是多面体任意两个面；其中，是面a的单位法向量，是面a的“顶点1到2”边的单位法向量，分别是面b的单位法向量和“顶点2到1”边的单位法向量，其中，|r1|、|r2|表示(x,y,z)到棱边两顶点的距离，e
12
是棱边长度，rf表示(x,y,z)到任意面内的任意一点(x1,y1,z1)的向量((x
1-x),(y
1-y),(z
1-z))，其中，是任意面的单位法向量，其中，β＝r1·
(r2×
r3)，α＝|r1||r2||r3|+|r1|(r2·
r3)+|r2|(r3·
r1)+|r3|(r1·
r2)，r1、r2、r3是用来计算点到1、2、3顶点的向量，u(r)对r求导获得加速度g(r)。
[0039]
作为上述技术方案的更进一步的改进，所述方法通过求解目标函数得到相对运动方程中的相关系数，进而反向解算出相对初始位置和速度，最终得到更为精确的探测器轨道估计结果，具体步骤为：
[0040]
定义探测器在绕飞若干个周期之后，通过视觉slam估计的一系列状态结果为其中时刻i＝{1,...,n}，位置rv＝[x,y,z]，旋转四元数qv＝[q0,q
x
,qy,qz]
t
，速度vv＝[v
x
,vy,vz]
t
，角速度ωv＝[ω
x
,ωy,ωz]
t
；
[0041]
取某一时刻i的作为输入，通过动力学方程可数值积分可递推出经过时间t之后的状态及以为初值的一条“预报轨道”；所述“预报轨道”的状态相比于真实轨道该时刻的状态x(i)存在误差δu(i)，且轨道递推的误差随着时间i逐渐累积，因此δu
(i+t)
＞δu(i)，通过不断迭代搜索合适的初值使得减少i+t时刻附近的误差，达到对的进一步优化；
[0042]
通过在真实轨道附近分布w(i)～n{0,δ}的视觉定位结果进行曲线拟合，获取符合运动学方程的精确轨道；选取作为递推初值，在区间选取m个递推结果：
[0043][0044]
与视觉对应状态求得的误差和作为约束，经过不断的误差反演求解最优的使得：
[0045]
[0046]
则由确定的一条符合真实运动学方程的轨道即为优化后的探测器定位结果。
[0047]
本发明的优点在于，本发明所述融合轨道动力学的行星探测器自主视觉定位系统及方法相比于传统的深空自主视觉导航系统，具有以下优势：
[0048]
1)该系统仅仅通过视觉传感器获取数据，利用视觉slam算法实现对探测器相对于小行星表面的定位，并通过回环检测算法优化定位累积误差；
[0049]
2)通过多视角立体几何方法重建小行星的稠密地图，并实现基于多面体法的不规则小行星引力场建模，在保证效率的同时提高精度；
[0050]
3)采用融合轨道动力学的航天器轨道改进方法，初衷就是最大限度的“挖掘”航天器轨道测量数据中的轨道信息，充分利用这些轨道信息来实现轨道精度的进一步提高。该改进方法的前提是航天器通过前述视觉slam技术的初定位基本完成，通过这些轨道信息并借助相关的数据处理方法，便可以实现定轨精度的改进。
附图说明
[0051]
图1为本发明所述融合轨道动力学的小行星探测器自主定位系统框架图；
[0052]
图2为本发明所述视觉slam定位算法的流程图；
[0053]
图3为本发明所述引力场建模模块中的小行星稠密三维重建流程图；
[0054]
图4为本发明所述融合轨道动力学模块的系统框架；
[0055]
图5为探测器绕飞小行星模型eros433的建模效果示意图；
[0056]
图6为仿真引擎生成的小行星模型eros433图像；
[0057]
图7为视觉slam在eros433定位结果示意图；
[0058]
图8为融合轨道动力学改进前后x、y、z三轴的定位误差和轨迹定位误差对比图；
[0059]
图9为小行星eros433的相关效果图，其中，图9(a)为仿真模型图，图9(b)为重建模型图，图9(c)为多面体网格图；
[0060]
图10为小行星eros433的重建引力场示意图；
[0061]
图11为重建引力场与真实引力场误差值示意图。
具体实施方式
[0062]
以下结合实施例进一步说明本发明所提供的技术方案。
[0063]
1.融合轨道动力学的行星探测器自主定位系统框架
[0064]
如图1所示，为本发明所述融合轨道动力学的小行星探测器自主定位系统框架图，根据功能主要分为四个模块：
[0065]
1)视觉传感器：其传感器硬件为相机，用于拍摄小行星表面的图像信息；
[0066]
2)视觉slam定位模块：根据获取的小行星表面的图像信息对小行星表面的特征进行提取匹配与跟踪，通过因子图优化算法估计探测器位姿，通过回环检测修正视觉累积误差，提高定位精度。
[0067]
3)引力场建模模块：根据绕飞段的图像由sfm算法恢复相机运动并重构行星表面三维模型，通过mvs进行稠密重建，随后对点云进行网格化处理，再基于多面体法对不规则小行星引力场建模。
[0068]
4)轨道优化模块：设计了一种基于轨道动力学的伪相对运动分析精确轨道优化算
法，基于定位模块估计的探测器相对小行星位置姿态和引力场建模模块求解的行星不规则引力场，分析反演视觉初始定轨误差在轨道动力学中的传播过程，修正视觉定位累计误差，改善初始定位结果，从而实现高精度导航。
[0069]
2.融合轨道动力学的行星探测器自主定位技术实现
[0070]
2.1坐标系定义
[0071]
定义小行星固连坐标系为并以此作为世界坐标系，其中a为坐标系原点，通常是为小行星质心。定义探测器坐标系为原点为探测器本体坐标系原点s，为了保证探测过程中小行星始终处于相机视野中，在绕飞时轴指向小行星质心，迹向方向，为及叉乘结果。定义相机坐标系为在设定中相机位于探测器正前方，三轴与探测器坐标系一致。定义像平面坐标系为
[0072]
2.2视觉slam相对定位算法
[0073]
视觉slam能根据通过视觉传感器传来的图像信息提取到的路标特征实时计算自身相对于环境的位置和姿态，并建立环境地图。如图2所示，为本发明所述视觉slam定位算法的流程图，算法包括三个线程：追踪、局部建图及回环检测。追踪线程包括提取orb特征、初始位姿估计/重定位、跟踪局部地图、搜索关键帧四个过程：主要负责根据输入的双目图像提取特征进行初始位姿估计，并建立关键帧；局部建图线程包括插入关键帧、筛选地图点、产生新的地图点、局部集束调整、局部关键帧筛选五个过程：负责根据关键帧建立局部地图；回环检测线程包括选择候选帧、计算相似度、回环融合、优化本质图四个过程：负责通过词袋模型检测候选帧是否回环，最后优化全局地图并更新。
[0074]
2.2.1特征提取
[0075]
在航天器绕飞阶段，在成像时主要呈现表面纹理的灰度信息。一般小行星表面都是凸的，绝大部分小行星表面都被环境光照亮，阴影覆盖较小，提取特征和特征匹配时主要需要解决随轨道高度带来的尺度变化和角度变化。因此使用orb(oriented fast and rotated brief)特征进行特征提取，其是一种具有尺度不变性和旋转不变性的特征点描述方法，利用增强的fast(features from accelerated segment test)提取关键点，并使用方向归一化的brief(binary robust independent elementary features)描述子对关键点进行描述。
[0076]
2.2.2追踪
[0077]
追踪进程将输入的序列图像与地图点进行匹配计算相机位姿。将第一帧图像的相机坐标系作为系统参考坐标系，通过序列图像的帧间匹配可以传递特征点的世界坐标，在帧间进行3d-3d对应估计可以求解参考坐标系与每帧图像对应的位姿关系。为了提高匹配精度降低误匹配，在前一帧追踪成功时，采用匀速运动模型或关键帧模型获得初始的位姿矩阵，再基于因子图优化模型最小化3d-2d重投影误差函数，迭代求解得到更优的位姿，其中误差函数为：
[0078]
[0079]
其中，是系统参考坐标系相对于第i帧相机坐标系的旋转和平移向量，(uj,vj)为第j个特征点的像素坐标；n为匹配到的特征点总和；ρ()为huber损失函数，用于外点的剔除；project()表示3d-2d重投影误差函数；xj为第j个匹配点在参考坐标系中的x坐标；∑为和每个特征点的尺度相关的信息矩阵。
[0080]
2.2.3局部建图
[0081]
在对每帧影像进行以上操作的同时，局部建图线程负责在当前地图插入关键帧和筛选、产生新的地图点，实时地优化局部若干帧的位姿及相应3d点的位置，对误差进行修正。
[0082]
2.2.4回环检测
[0083]
由于视觉slam定位根据输入的序列图像增量式地进行帧间匹配并估计相机位姿，因此必然存在累积误差。探测器一旦再次回到之前抵达过的“关键帧”，就能通过“关键帧”的相互关联性进行有效的闭合回环，优化位姿、纠正累积误差。小行星绕飞轨迹大多为环形轨道，多次绕飞之后可以通过回环检测将视觉定位的误差约束在一定范围内，也为下一步融合轨道动力学提供基础。为实现回环检测本专利使用词袋模型，将每张图像构建为一个词袋向量，通过相似度度量：
[0084][0085]
其中，vi和vj为对应的两幅图像的词袋向量，来评价两幅图像的相似度得分，判断是否进行回环检测。
[0086]
2.3小行星稠密建图及引力场建模算法
[0087]
2.3.1小行星稠密三维重建
[0088]
虽然视觉slam算法也能在实现定位的同时进行建图，但是由orb算法提取的特征较为稀疏，存在较多的空洞和重建不完全区域，而且由于受到视觉的定位误差的影响导致精度较低，会影响后续引力场建模效果。一般小行星任务通过多视角立体几何方法重建小行星的稠密地图，主要包括从运动恢复结构(structure from motion，sfm)结合多视角立体几何(multi-view stereo，mvs)方法。多视角立体几何法相比于现有小行星任务常用的立体光度测量法(spc，stereophotoclinometry)，仅靠绕飞拍摄的图像数据即可实现三维重建，其结果稳定性强且精度较高，是未来小行星探测任务中表面重建的主要技术之一。
[0089]
算法流程如图3所示，在输入小行星表面序列图像之后，先通过sfm算法恢复相机运动信息和内外参，建立行星表面稀疏点云。首先利用sift算法进行特征提取和匹配，随后对图像对进行几何验证即求解基础矩阵f并去除外点。在初始化时选择重叠度较高的两张图像，对其进行图像注册及三角化处理，并增量式地依次添加新的图像和其特征点云坐标；为了减少误差，在每次添加图像之后，对其进行光束调整：使用光束平差法(bundle adjustment，ba)优化相机内外参和特征点云，这里可循环进行使误差不断减小以满足需求，建立行星表面稀疏点云，求解最优的相机位姿，并将此作为mvs算法的输入。其中光束平差法通过最小化重投影误差优化相机参数，定义n个空间点pi＝[xi,yi,zi]
t
在像素平面的投影坐标为ui＝[ui,vi]
t
，相机的旋转r和平移变换t的李群表示为t，k为相机内参矩阵，构建如下式所示的最小二乘问题，求解最优的相机位姿。
[0090][0091]
其中，si是距离尺度。
[0092]
mvs稠密建图部分通过融合深度图实现。首先由立体匹配patchmatch算法进行视差估计得到深度图，并对深度图进行融合和滤波处理。由于多面体法需要用到模型表面的三角面片，因此还需要进一步生成点云的法向量，并使用泊松重建方法进行表面重建。
[0093]
2.3.2引力场建模算法
[0094]
重建完成后，本发明所述方法选择多面体法进行引力场建模。多面体法(polyhedron method)是不规则小行星引力场建模的主要方法之一，目前理论分析中多以该方法所得结果为精确值，其相对于球谐函数法和质点群法，具有更高的精度。考虑到探测器高精度导航的需求，因此本专利主要选择多面体法进行引力场重建。
[0095]
多面体法计算得到的引力势能和引力加速度分别是：
[0096][0097][0098]
其中，u(r)表示引力势，g表示万有引力常量，ρ是小行星密度，e是edge，表示棱边，f是face，表示面片，r表示任一点(x,y,z)位置向量，re是该点到多面体上面a和面b棱边上一点(x0,y0,z0)的向量((x
0-x),(y
0-y),(z
0-z))，面a和面b是多面体任意两个面；其中，是面a的单位法向量，是面a的“顶点1到2”边的单位法向量，分别是面b的单位法向量和“顶点2到1”边的单位法向量；其中，|r1|、|r2|表示(x,y,z)到棱边两顶点的距离，e
12
是棱边长度，rf表示(x,y,z)到任一面内的任意一点(x1,y1,z1)的向量((x
1-x),(y
1-y),(z
1-z))，其中，是任一面的单位法向量，其中，β＝r1·
(r2×
r3)，α＝|r1||r2||r3|+|r1|(r2·
r3)+|r2|(r3·
r1)+|r3|(r1·
r2)，r1、r2、r3是用来计算点到1、2、3顶点的向量，u(r)对r求导获得加速度g(r)。
[0099]
2.4伪轨道动力学修正算法
[0100]
采用融合轨道动力学的航天器轨道改进方法，初衷就是最大限度的“挖掘”航天器轨道测量数据中的轨道信息，充分利用这些轨道信息来实现轨道精度的进一步提高。该改进方法的前提是航天器通过前述视觉slam技术的初定位基本完成，通过这些轨道信息并借助本专利提出的处理方法，便可以实现定轨精度的改进。
[0101]
如图4所示，为融合轨道动力学实现探测器轨道估计优化模块的系统框架，该框架一方面将视觉slam技术对小行星探测器的位姿估计结果作为航天器初定轨结果，得到观测轨道；另一方面将基于视觉信息的小行星高精度三维重建模型通过多面体法完成小行星引力场建模，将航天器初定轨的数据作为初值输入高精度轨道动力学模型，进而得到其高精度轨道动力学模型，再通过动力学积分得到预报轨道，通过观测轨道和预测轨道可以计算得到轨道误差值。然后，将观测轨道和预报轨道分别输入基于“伪相对运动”的探测器轨道
优化算法，通过求解目标函数得到相对运动方程中的相关系数，进而反向解算出相对初始位置和速度，最终得到更为精确的探测器轨道估计结果。
[0102]
定义探测器在绕飞若干个周期之后，通过视觉slam估计的一系列状态结果为其中时刻i＝{1,...,n}，位置rv＝[x,y,z]，旋转四元数qv＝[q0,q
x
,qy,qz]
t
，速度vv＝[v
x
,vy,vz]
t
，角速度ωv＝[ω
x
,ωy,ωz]
t
。假设只考虑小天体本身引力作用，不考虑太阳光压力等其他摄动力。其中引力加速度已经由重建模型利用多面体加速度计算模型给出，取某一时刻i的作为输入，通过动力学方程可数值积分可递推出经过时间t之后的状态及以为初值的一条“预报轨道”，由于其不真实存在，因此称为“伪轨道”。由于相比于真实轨道该时刻的状态x(i)存在误差δu(i)，且轨道递推的误差是随着时间i逐渐累积的，因此δu
(i+t)
＞δu(i)，通过不断迭代搜索合适的初值使得减少i+t时刻附近的误差，就可以达到对的进一步优化。
[0103]
因为真实轨道是未知的，只能通过在真实轨道附近分布w(i)～n{0,δ}的视觉定位结果进行曲线拟合，获取符合运动学方程的精确轨道。选取作为递推初值，在区间选取m个递推结果：
[0104][0105]
与视觉对应状态求得的误差和作为约束，经过不断的误差反演求解最优的使得：
[0106][0107]
则由确定的一条符合真实运动学方程的轨道即为优化后的探测器定位结果。
[0108]
3.仿真验证与分析
[0109]
3.1实验数据构建
[0110]
为了对本发明所述方法进行仿真验证，本实施例选用nasa官方公布的小行星模型eros433，该模型最长轴为13km，最短轴为33km，平均密度为2670kg/m3。该模型包括小行星的三维结构，纹理信息采用unreal engine引擎中提供的小行星表面纹理，可视化效果如图5所示，即为探测器绕飞小行星的建模效果。
[0111]
使用该小行星模型，并参考真实引力场通过轨道动力学模型生成仿真绕飞的探测轨道。此轨道距离小行星质心的最小距离为14.3km，最大距离为26.4km，轨道的时间间隔为50秒。通过程序控制摄像机按指定轨道绕飞小行星，可以实现小行星全部区域的探测而不产生盲区。在绕飞过程中探测器朝向始终保持对准小行星质心，同时视场相机拍摄行星表面图像，系统输出每张图像对应的时间戳、相机位置及旋转四元数，其可作为位姿估计精度评估过程中的真值(ground truth，gt)。最终在轨拍摄时间60000s，共生成1201张图像，如图6所示，即为仿真引擎生成图像。相机分辨率为1024*1024，视场角为15
°
。
[0112]
3.2视觉slam相对定位实验及分析
[0113]
通过观测图像提取orb特征点并进行匹配，能实时获取探测器的相对位姿信息。由于视觉slam得到的是相机坐标系下的相对定位结果，为了对比分析算法精度，将相机坐标系转换到世界坐标系下。由图7看出视觉定位结果与真实轨道基本一致。
[0114]
视觉的定位结果与真值误差对比如图8所示，分别对比了x、y、z三轴的定位误差和轨迹定位误差，其中横坐标表示时间戳，单位是s，纵坐标表示误差，单位为km。
[0115]
在实际工程应用中，经常通过绝对轨迹误差(absolute pose error,ape)来评价算法的估计轨迹与真实轨迹的差异。假设系统真实轨迹为t
gt,i
，估计的轨迹为t
esti,i
，其中i＝1,2,
···
,n，那么绝对轨迹误差则表征了每个时刻位姿李代数的均方根误差(root-mean-squared,rmse)，其数学定义如下式所示：
[0116][0117]
计算求解得到视觉定位的均方根误差为116.97m，误差最大值为203.86m，均值为103.33m。绕飞轨道大致可以认为环绕小行星飞行两圈，从图8的三轴和总体的误差趋势也能看到定位误差所代表的实线曲线呈周期性变化，同时在30000s前后探测器又回到了初始点位置附近。由于回环检测算法在此匹配到之前的关键帧，累积误差被修正，整体误差在一个周期内呈现先变大后变小的趋势，这是因为随着时间的增加，误差会逐渐累积，校正后能将误差降低。该结果验证了自主视觉定位算法的有效性，并进一步肯定了视觉slam技术应用于小行星绕飞轨道估计的可行性，但误差仍有进一步提升空间。
[0118]
3.3小行星稠密建图及引力场建模实验及分析
[0119]
根据输入的序列图像对小行星进行表面三维重建，得到如图9(b)所示的稠密点云模型，总计168743897个点，外形与unreal仿真引擎中的小行星形态，图9(a)所示，基本一致。
[0120]
为了便于重建不规则小行星的引力场模型，考虑到引力场反演和轨道计算所需的时间，本实施例使用meshlab工具将点云模型简化至902个点，并通过泊松重建算法对曲面进行重建，生成由1800个三角面片构成的多面体网格，如图9(c)所示。
[0121]
通过多面体法实现多面体网格的引力场重建，获得其重新建模的加速度精度，得到近地表面的引力加速度分布如图10所示。
[0122]
如图11所示，为重建引力场与真实引力场误差值示意图。与真实引力场模型下的加速度进行对比，图11计算得到的引力场误差均值为2.9232
×
10-8
km/s2，最大值为3.6549
×
10-7
km/s2，最小值为1.2825
×
10-9
km/s2，误差占比均值为0.75％，最大不超过7.31％，引力场建模精度能满足任务需求。
[0123]
3.4伪轨道动力学修正实验及分析
[0124]
选取5000s处的纯视觉定位结果作为初值输入，在整体轨道60000s上间隔1000s均匀采样60个点作为约束，不断迭代优化，获得融合轨道动力学之后的结果，误差如图8虚线曲线所示，可以看出无论是各个分量的误差还是总体误差，皆比未优化前的精度要高。
[0125]
表1视觉slam和融合轨道动力学后的定位误差对比
[0126][0127]
根据表1的统计结果，融合轨道动力学的改进方法将原先均值为103.33米、均方根误差为116.97米的视觉定位误差降低至均值为59.61米，均方根误差为65.602米的水平，精度提升43.92％，可以满足未来小行星自主探测任务中的定位要求。因此融合轨道动力学进行视觉轨道优化后的轨道误差能将视觉slam定位误差进一步降低。
[0128]
从上述对本发明的具体描述可以看出，本发明提出的一种融合轨道动力学的小行星探测器自主视觉定位系统及方法通过融合轨道动力学的轨道改进技术实现探测器的高精度视觉导航，并快速建立小行星表面三维模型，并且能够优化探测器初始定位结果，从而实现高精度导航，能够为未来我国的深空探索提供有效技术支撑。
[0129]
最后应说明的是，以上实施例仅用以说明本发明的技术方案而非限制。尽管参照实施例对本发明进行了详细说明，本领域的普通技术人员应当理解，对本发明的技术方案进行修改或者等同替换，都不脱离本发明技术方案的精神和范围，其均应涵盖在本发明的权利要求范围当中。