地外天体表面探测光学导航位姿加权解耦估计方法

本发明涉及一种地外天体表面探测光学导航位姿加权解耦估计方法，特别适用于利用光学导航陆标作为观测信息的地外天体探测器导航系统，属于自主导航领域。

背景技术：

1、获取探测器的准确位姿信息是开展地外天体表面探测的重要前提。由于地外天体通常距离地球较远，探测器与地面的通讯存在时间延迟，这就对探测器的自主导航技术提出了新的挑战，如惯性导航等探测器实时导航方式被相继提出。由于惯性测量单元存在常值偏差和漂移，导航精度随时间降低，单一的惯性导航系统很难满足精度要求。同时随着导航相机等光学敏感器发展，以光学信息为主的探测器自主导航方法成为未来的热门研究方向。任务开始前获取地外天体表面岩石，陨石坑等天然地形地貌特征和人工陆标的绝对位置信息，利用光学相机获取上述特征作为导航陆标的自主导航方法，具有较高的导航性能。

2、基于光学导航陆标的探测器自主光学导航方法已成为目前研究的热点，在难以获取探测器与陆标间距离信息的情况下，利用两个陆标视线向量形成的观测角进行位姿估计，能实现探测器的自主导航。由于相机图像处理存在噪声以及陆标位置确定结果的误差，探测器利用陆标像素信息进行导航必然存在偏差。合理的陆标分布形成的观测角构型，能够提高系统整体的可观度，让位姿估计结果对误差不敏感。如何在陆标观测质量不同的情况下提高系统的可观度，以此提高探测器位姿估计的精度，是基于光学导航陆标导航的关键技术。

技术实现思路

1、本发明的目的是提供一种地外天体表面探测光学导航位姿加权解耦估计方法，该方法利用陆标观测角对观测方程进行解耦，以此估计探测器的位置和姿态信息；基于可观度对观测矩阵进行分析，评价观测角对地外天体表面的可观度；根据对地外天体表面的可观度给出每个观测角的权重，构造观测角加权矩阵并融入到位姿解耦估计中，实现光学导航位姿加权解耦估计。

2、本发明的目的是通过下述技术方案实现的。

3、本发明公开的地外天体表面探测光学导航位姿加权解耦估计方法，包括如下步骤：

4、步骤一：建立地外天体表面探测坐标系，基于陆标观测方程建立光学导航位姿估计模型，在小偏差线性化条件下，得到观测角偏差与位置偏差的线性化方程，根据惯性导航给出的探测器估计位置和姿态，分别得到估计观测角和估计观测向量，根据观测向量构建线性观测矩阵，利用观测角偏差补偿位置估计误差，得到探测器位置解耦估计方程和姿态解耦估计方程，通过所述两个解耦估计方程实现对观测方程的解耦估计。所述地外天体表面探测坐标系包括导航坐标系σ、探测器本体坐标系σb和相机坐标系σc。

5、定义以下坐标系：导航坐标系σ、探测器本体坐标系σb和相机坐标系σc。为简化运算，将导航坐标系定义为初始时刻的探测器本体坐标系，且相对于地外天体静止，为三维空间中的笛卡尔坐标系。探测器的位置坐标表示在导航坐标系下。探测器本体坐标系原点b取在探测器的质心上，xb轴平行于底盘平面并指向前，yb轴平行于底盘平面并指向左，zb轴垂直于底盘平面并指向上。相机坐标系σc的原点c位于相机的光学中心。zb轴与相机光轴平行，指向目标体的中心。xc和yc方向标识图像平面。在导航坐标系中，探测器的位置记为r，第i个地标位置记为ρi，导航坐标系到探测器本体坐标系的变换矩阵记为cbw，探测器本体坐标系到相机坐标系的变换矩阵记为ccb。则在相机坐标系下，导航陆标的位置为：

6、

7、相机坐标系的原点与探测器本体坐标系的原点重合，则陆标的像元、像线坐标将表示为：

8、

9、式中，pi为陆标的像元坐标，li为陆标的像线坐标，x、y、z为探测器三轴位置坐标，xi、yi、zi为陆标三轴位置坐标，cij为传递矩阵ccw的等价系数，f为导航相机焦距。

10、将第i个和第j个陆标视线矢量的观测角度表示为aij：

11、

12、式中，ri、rj表示导航坐标系中陆标相对于探测器的位置，ri、rj表示陆标与探测器之间的距离。

13、利用光学图像中的像素和图像线坐标，观察角度表示为：

14、

15、对于连接两个陆标pi，pj的线段pipj，必须存在一个环形表面，该环形表面是通过绕pipj轴旋转圆弧生成的，满足连接环形表面上任意点的线段之间形成的任意角度，并且两个陆标满足相同的aij。圆弧o的中心位于线段pipj的垂直平分线上。用向量方程将几何关系表示为：

16、(r-ρi)·(r-ρj)＝|r-ρi||r-ρj|cosaij  (5)

17、式(5)即为地外天体表面光学导航位姿估计模型，由式(5)知，观测角aij仅取决于探测器的位置r，与探测器的姿态状态无关。通过求解式(5)独立获得探测器的位置。

18、为了解决式(5)非线性直接求解的困难，基于小偏差线性化方法对式(5)进行线性化，得到围绕标称轨迹的线性测量方程。在小偏差线性化条件下，式(5)得：

19、

20、定义单位视线向量和辅助向量为：

21、

22、式(6)用辅助向量表示为：

23、

24、定义观测向量为：

25、

26、得出：

27、δaij＝hijδr  (10)

28、探测器实际探测过程中，根据惯性导航算法给出的估计位置r*，得到观测角和观测向量hij的估计值。通过式(10)用观测角偏差δaij补偿估计位置误差δr，提高当前位置的精度。对于n个陆标的情况，相应的观测角偏差为：

29、δa＝[δa12 δa13 … δaij … δa(n-1)n]t i,j∈{1,2…n},i≠j  (11)

30、线性观测矩阵表示为：

31、h＝[h12 h13 … hij … h(n-1)n]t  (12)

32、有：

33、δa＝hδr  (13)

34、探测器的当前时刻位置向量更新为：

35、r＝r*+(hth)-1htδa  (14)

36、式(14)即为探测器位置解耦估计方程。

37、根据式(1)，在相机坐标系下，陆标的视线向量表示为：

38、

39、式(15)使用陆标的像元像线坐标表示为：

40、

41、在已知探测器位置的情况下，基于多矢量姿态确定原理，得到从导航坐标系到探测器本体坐标系的姿态转移矩阵的最优解，表示为：

42、

43、式中，

44、

45、式(17)即为探测器姿态解耦估计方程。

46、在已知陆标位置的情况下，通过迭代式(14)得到探测器相对于导航坐标系的位置，探测器相对于导航坐标系的姿态由式(17)确定。

47、步骤二：分析步骤一中的观测矩阵的可观度，得到可观度大小与观测角观测矢量的关系，该关系用四面体的表面积与体积公式表示，利用四面体的面体比表征观测角可观度贡献值的大小，利用可观度贡献值对观测角进行评价，得到单个观测角的可观度权重。

48、步骤二即为构建的观测角评价方法，用于评价单个观测角的可观度。

49、对于最小二乘问题，存在如下关系：

50、x-x*＝(hth)-1ht(y-y*)  (18)

51、式中，其中x表示待估计值，x*表示x的先验估计，表示观测值，y*表示y的先验估计。

52、矩阵(hth)-1的特征值用来评价导航系统的可观性。特征值越小，与其对应的特征向量同方向的估计精度越高。如果观测噪声彼此独立且误差方差等于则估计协方差矩阵p的迹将与(hth)-1的特征值之和成正比。

53、分析利用三个观测角来估计相对位置的情况，矩阵hht表示为：

54、

55、式中，θ12，θ13，θ23表示观测向量h1，h2，h3之间的夹角。通过计算矩阵hht的逆，得到(hht)-1的特征值之和为：

56、

57、四面体棱与三个观测向量h1，h2，h3重合的四面体的体积v计算为：

58、

59、四面体边三角形的面积s1，s2，s3表示为：

60、

61、因此，用四面体的体积和侧面积的函数重新表示为：

62、

63、根据式(23)得，位置确定误差随着面积与体积比的增大而增大。除了陆标到探测器的距离外，定位精度主要取决于观测角的大小。观测向量1参与构成四面体的两个侧面，侧面积为和由于面体比越小，位置估计精度越高，通过为面体比贡献越小的观测向量分配更高的权重，以此提高位置估计的精度。由于观测向量1表示的是观测角1，于是观测角1在这一组观测角的可观度贡献值为：

64、

65、分析利用4个观测角来估计相对位置的情况，矩阵hht表示为：

66、

67、(hht)-1的特征值之和为：

68、

69、上式分子用四面体体积表示，进一步转换为用四面体侧面积表示为：

70、

71、观测向量的权重用观测向量参与构成的侧面积表示。为了简便运算，忽略掉同类项和系数，观测角1的可观度贡献值为：

72、

73、扩展到利用n个观测角进行位置估计的情况，观测角1的可观度贡献值为：

74、

75、利用可观度贡献值直观的对观测角进行评价。

76、步骤三：根据步骤二构建的观测角评价方法给出每个观测角的权重，根据权重构造加权矩，将加权矩阵融入到步骤一中探测器位置和姿态解耦估计方程中，形成探测器加权位姿解耦估计方程，根据加权位姿解耦估计方程得到融入可观度权重后的探测器的位置和姿态，实现光学导航位姿解耦估计。

77、令观测角的可观度贡献值为其权重。将所有观测角权重进行归一化：

78、

79、构造加权矩阵为：

80、

81、对式(13)进行加权最小二乘，融入角度观测权重后，探测器当前时刻的位置矢量为：

82、r＝r*+(htwah)-1htwaδa  (32)

83、利用单独解耦估计的位置结果作为已知量,单独解耦估计探测器姿态：

84、

85、通过式(32)和式(33)实现基于观测角可观度加权的探测器位姿解耦估计，得到探测器的位置和姿态。

86、步骤四：根据步骤三得到的探测器位置和姿态进行导航，实现高精度的地外天体表面探测任务。

87、有益效果：

88、1、本发明公开的地外天体表面探测光学导航位姿加权解耦估计方法，在难以获取地外天体表面陆标视线距离信息的条件下，利用陆标观测角作为观测量，对观测方程中探测器的位置和姿态进行解耦求解，充分利用导航资源，并提高地外天体表面探测光学导航位姿加权解耦估计的解算时间。

89、2、本发明公开的地外天体表面探测光学导航位姿加权解耦估计方法，通过分析观测矩阵的可观度，得到可观度大小与观测角观测矢量的关系，该关系用四面体的表面积与体积公式表示，利用四面体的面体比表征观测角可观度贡献值的大小，利用可观度贡献值对观测角进行评价，得到单个观测角的可观度权重；根据权重构造加权矩，将加权矩阵融入到探测器位置和姿态解耦估计方程中，形成探测器加权位姿解耦估计方程，根据加权位姿解耦估计方程得到融入可观度权重后的探测器的位置和姿态，为导航精度贡献高的观测角分配更高权重，提高探测器位姿估计的精度。