小天体柔性附着状态融合估计方法

本发明涉及一种柔性附着系统状态估计方法，尤其涉及一种小天体柔性附着的多节点状态精确估计方法，属于深空探测。

背景技术：

1、小天体是当前深空探测领域的热点目标之一，由于小天体引力微弱、形貌复杂且先验信息匮乏，探测器的附着过程面临着一系列难题，现有的刚性附着方案其可靠性较低，而接触即走方案无法实现探测器的稳定附着。先前技术提出了柔性附着的概念，柔性着陆器利用面状外形增大与小天体的接触面积，通过柔性材料吸收着陆时的残余动能，极大降低了附着过程中发生翻滚倾覆事件的概率，保证了附着过程的安全性。由于柔性着陆器为无穷维系统动力学特性复杂，先技术提出了简化的三节点柔性着陆器模型，认为敏感器与执行机构都集中在柔性着陆器的三个节点处，针对三节点构型的柔性着陆器，需考虑对柔性着陆器三个节点的状态进行估计进而实现对柔性着陆器整体的状态表征。在现有的敏感器技术下，利用观测信息进行滤波估计节点状态的方法最终会导致单节点存在分米级乃至米级的估计误差，对于柔性着陆器而言节点间相对距离的估计误差达到了与自身尺寸相当的量级，因而估计结果无法直接应用于柔性着陆器的制导与控制。为了实现柔性着陆器附着过程节点状态的精确估计，需要考虑柔性着陆器自身的物理与运动特性，协同多节点的观测信息，研究节点状态融合估计方法。

技术实现思路

1、为了解决现有技术下利用敏感器观测信息进行节点状态滤波估计的方法误差大，无法直接适用于柔性着陆器节点状态估计的问题，本发明的目的是提供一种小天体柔性附着状态融合估计方法，该方法采用无迹采样的方法对离线训练的神经网络进行前向递推，对节点间相对状态进行预报，提取柔性着陆器运动过程中节点间牵连运动信息。利用节点相机重合观测区匹配的角点信息求解基本矩阵，获取部分节点间相对位置与相对姿态。利用多源异构信息分块对角矩阵融合校正方法，将维度不一致的预报相对状态与解算相对状态进行融合。利用无迹卡尔曼滤波器对三节点状态进行估计，随后利用融合的节点间相对状态对滤波结果进行修正，实现柔性着陆器节点状态的精确估计。

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

3、本发明公开的小天体柔性附着状态融合估计方法，针对柔性着陆器节点间运动强耦合，牵连运动信息难提取的问题，利用离线训练的深度神经网络对节点间相对状态进行智能预报，利用相机重合观测信息求解基本矩阵对节点间相对状态进行解算。针对预报的节点间相对状态与解算的节点间相对状态维度不一致问题，对二者的进行重构与扩维，并求取分块对角矩阵形式的加权矩阵对二者进行融合，获得更为精确的节点间相对状态。针对利用敏感器观测信息进行滤波估计获得的节点状态误差过大的问题，利用融合的节点间相对状态和三节点状态滤波估计值对滤波结果进行修正，提高柔性着陆器节点状态的估计精度。

4、本发明公开的小天体柔性附着状态融合估计方法，包括如下步骤：

5、步骤一：随机给定柔性着陆器的初始状态，求取此时的柔性体节点间相对状态；随机在三个节点施加力，利用柔性着陆器离散曲率动力学模型进行动力学递推，求取递推后的柔性体节点间相对状态，将递推前节点间相对状态、施加的力和递推后的节点间相对状态做为一组数据进行记录；重复数据组生成过程，直到生成预定数量的数据组；以节点递推前柔性体节点间相对状态、施加的力为输入，递推后的柔性体节点间相对状态为输出进行有监督训练，得到节点间相对状态预报网络。

6、步骤一的具体实现方法为：

7、柔性着陆器的三节点位置、速度、姿态、角速度分别为xi、vi、θi、ωi(i＝1,2,3)，则柔性着陆器节点i状态si与柔性着陆器整体状态s表述为

8、

9、任意两个节点间的节点间相对状态可表述为

10、lij＝[δxijt,δvijt,δθijt,δωijt]t                          (2)

11、其中，δxij为节点i相对于节点j的位置，δvij为节点i相对于节点j的速度，δθij为节点i相对于节点j的姿态，δωij为节点i相对于节点j的角速度。为了避免重复计算，定义ij取值为21、32、13。

12、柔性着陆器整体的节点间相对状态l表述为

13、l＝[l21t,l32t,l31t]t                              (3)

14、利用柔性着陆器整体状态可以计算出递推前节点间相对状态。

15、柔性着陆器离散曲率动力学模型将柔性结构依据柔性体表面的离散曲率特性划分为互不重叠的三角单元，用各离散单元的运动特性描述柔性着陆器的运动，离散单元的内力由式(4)计算

16、

17、其中，fei为离散元节点的弹性力，xi为离散元节点在惯性系下的坐标，中间变量ee为应变能由拉伸能es与弯曲能eb组成，e为柔性体的杨氏模量，h为平面的厚度，ν为柔性材料的泊松比，εmn(m,n∈{x,y})为a面内应变，h与k分别为a面内的平均曲率与高斯曲率，d为弯曲模数；根据应变能求取所有离散元节点的弹性力后，再依次求取各离散单元节点受到的外力f，获取所有离散元结点的受力情况，根据受力关系解算出各离散元的拉压与弯曲形变，得到柔性着陆器在承受外力后的形变与运动情况，并计算出递推后的节点间相对状态。则将递推前节点间相对状态、外力、递推后的节点间相对状态做为一组数据进行记录。重复上述过程直至生成nt组数据。

18、以节点递推前柔性体节点间相对状态、施加的力为输入，递推后的柔性体节点间相对状态为输出进行有监督训练，利用梯度下降法更新网络参数，通过多次迭代更新完成节点间相对状态预报网络的训练。

19、步骤二：利用训练好的节点间相对状态预报网络递推预报节点间相对状态，并利用无迹采样的方式计算递推信息的协方差。

20、步骤二的具体实现方法为：

21、利用k-1时刻柔性着陆器的校正节点间相对状态的协方差pfk-1对k-1时刻柔性着陆器的校正节点间相对状态进行sigma采样，获取采样点其中j表示相对状态的第j个采样点。利用节点间相对状态预报网络对所有采样点进行递推

22、

23、其中，为三节点受到的合外力，f1k-1,f2k-1,f3k-1分别k-1时刻三个节点受到的控制力；λjk为第j个采样点预报的k时刻柔性着陆器节点间相对状态；net()函数表示节点间相对状态预报网络的预报过程。

24、完成全部采样点的递推后，求取k时刻预报的柔性着陆器节点间相对状态及其误差协方差

25、

26、其中，为k时刻预报的柔性着陆器节点间相对状态；w(ml)与w(cl)为权重系数矩阵；为k时刻预报的柔性着陆器节点间相对状态的误差协方差；qnn为节点间相对状态预报网络的拟合残差阵。

27、步骤三：判断每两个节点相机重合观测区匹配的角点是否达到八对，若满足八对匹配的角点则利用角点信息解算本质矩阵，结合测距信息，求解节点间相对状态信息，并求出节点间相对状态信息的协方差，若不满足则判定这两个节点间无法获取解算的相对状态信息。重复匹配角点判断与解算过程直至完成全部节点间的匹配角点判断与解算，得到观测解算节点间相对状态及节点间相对状态的协方差。

28、步骤三的具体实现方法为：

29、提取k时刻任意两个节点相机重合观测区域匹配的角点，当数量大于八对时，利用如下方法计算节点间相对状态信息。

30、基本矩阵与匹配的角点像素坐标满足关系

31、

32、其中，为k时刻节点i观测到的第n个匹配角点的齐次坐标；为k时刻节点j观测到的第n个匹配角点的齐次坐标；fijk为k时刻基本矩阵fk的元素

33、

34、利用ak替代式(7)左侧的齐次坐标组成的矩阵，fk指代基本元素组成的矩阵，式(7)可写为

35、akfk＝0                                 (9)

36、对ak阵进行奇异值分解

37、ak＝udvt                               (10)

38、其中，u为akakt的特征向量构成的矩阵；d为aktak特征值的平方根；v为aktak的特征向量构成的矩阵。v的最后一列即为fk的解，由此也可以获得基本矩阵fk。

39、利用基本矩阵计算k时刻本质矩阵e

40、ek＝ktfkk                              (11)

41、其中，k为相机内参矩阵。

42、对k时刻本质矩阵ek进行奇异值分解

43、ek＝u1d1v1t                              (12)

44、其中，u1为ekekt的特征向量构成的矩阵；d1为ektek特征值的平方根；v1为ektek的特征向量构成的矩阵。

45、u1的最后一列及其相反向量即为k时刻两个可能的位置向量t1k与t2k。

46、k时刻两个可能的姿态旋转矩阵与分别为

47、

48、其中，w为常矩阵

49、

50、可能的位置向量与可能的姿态旋转矩阵组成四组可能的位姿解，利用相机观测量对四组解进行判断即还原k时刻节点i与节点j间正确的相对位置方向与相对姿态角t-k与rk。

51、利用两节点激光测距仪k时刻的观测量dik与djk还原相对位置向量

52、

53、其中，[txk-tyk-tzk-]t为tk-的三个分量。相对位置向量与相对姿态角构成k时刻节点i与节点j间的解算相对状态信息

54、k时刻相机观测量不确定性可以通过求取奇异值分解的雅可比矩阵传递至相对位置方向与相对姿态角

55、

56、其中，aij表示ak中元素；与可以通过u、v、d三个矩阵中的元素线性求出。

57、将式(15)右侧与相对姿态角依次对tk-，rk，dik，djk求导即可获取还原相对位置过程的雅可比矩阵jep，获得k时刻节点i与节点j间相对位置、相对姿态误差协方差表示为

58、

59、其中，与分别为两节点激光测距仪的观测协方差。

60、若满足每两个节点间都能够解算相对状态信息，则获取的k时刻解算节点间相对状态为协方差为若存在节点间无法解算相对状态，则与删减对应的分块。

61、每两两节点都重复一遍上述匹配角点判断与解算过程，直至完成全部节点间的匹配角点判断与解算。

62、步骤四：利用多源异构信息分块对角矩阵融合方法融合步骤二获取的预报节点间相对状态与步骤三获取的观测解算节点间相对状态，得到融合后的节点间相对状态。

63、步骤四的具体实现方法为：

64、由于预报的节点间相对状态较解算的节点间相对状态多包含了节点间的相对速度与节点间的相对角速度，且解算的节点间相对状态并不能在每个时刻都稳定获取所有节点间的信息，即与向量对应位置表征的数据并不一定是同一个信息，因此重构k时刻的预报节点间相对状态与解算节点间相对状态信息及对应的协方差阵，使得二者都包含的信息排在向量的前部，内的相对速度、相对姿态及无法解算的相对位置与姿态信息排在向量尾部，同时向解算的相对状态向量尾部补上对应数量的0使得二者具备相同的维度。重构后的预报节点间相对状态及其协方差记为与重构后的解算节点间相对状态及其协方差记为与记二者共有的状态有lm维，预报相对状态独有的状态有ln维。

65、计算校正权值矩阵

66、

67、其中，分块元由下式计算。

68、

69、其中，m＝1时代表取状态m＝2时代表取状态n＝1时代表取状态的前lm维，n＝2时代表取状态的后ln维；||运算为求矩阵行列式；*表示求矩阵的伴随矩阵；δnn为对应维度的联合误差协方差，δnnij为联合误差协方差中的子矩阵同时也是与的子矩阵，其具体关系如下

70、

71、δnnij即为状态与状态(根据i、j的值，参照m进行判断)的前lm维或后ln维(根据前述n的取值判断)的误差协方差或互协方差(此中两组状态不存在互协方差，即δnn12＝δnn12＝0)。

72、利用加权矩阵求得融合的节点间相对状态

73、

74、融合后节点间相对状态的协方差为

75、

76、将融合的节点间相对状态还原为式(3)的相对状态顺序获得及其误差协方差pfk。

77、步骤五：利用相机观测的特征点信息、激光测距仪测量的高度信息进行无迹卡尔曼滤波，得到三节点状态滤波估计值。

78、步骤五的具体实现方法为：

79、当相机坐标系与节点本体坐标系重合，则光学导航相机的观测方程为

80、

81、其中，是k时刻节点i对导航路标mi的观测值；f为相机焦距；cpq,i为着陆点固连坐标系到节点i的节点本体坐标系的转移矩阵cba,i中的元素；是路标mi在着陆点固连坐标系下的位置；为k时刻节点i在着陆点固连坐标系下的位置，wc,i为节点i相机的观测噪声。

82、激光测距仪的观测方程有

83、

84、其中，ni为激光测距仪在节点i本体系的指向；wl,i为节点i的激光测距仪噪声。

85、有节点i的观测方程

86、

87、其中，为k时刻节点i的观测量；mn为观测到的特征点数量；为观测噪声，为高斯白噪声。

88、进行状态估计时利用刚体-弹簧-扭簧等效动力学方程进行状态一步递推，单节点的动力学方程如下

89、

90、其中，m为节点质量；ffe与mfe分别为根据离散曲率动力学模型计算的节点受到的柔性连接内力与内力矩；fd与md为未建模内力与未建模内力矩；g为节点受到的引力加速度；qi为节点i的姿态四元数，姿态四元数qi能够与欧拉角θi相互转换；符号表示四元数直乘；i为节点转动惯量。

91、由式(26)得节点i的状态方程

92、

93、其中，为过程噪声，为高斯白噪声，为k时刻节点i的过程噪声协方差阵。

94、基于上述状态方程与观测方程，利用无迹卡尔曼滤波对节点状态进行估计

95、

96、其中，为k时刻状态的一步预测值；为k-1时刻节点i状态第j的sigma采样点；为k时刻节点i状态第j个的sigma采样点的一步预测值；w(m)与w(c)为权重系数矩阵；为k时刻节点i状态协方差的一步预测；为k时刻节点i观测量的一步预测；为k时刻节点i状态第j个的sigma采样点观测量的一步预测；为k时刻节点i观测量一步预测值的自协方差；为k时刻节点i观测量一步预测值与状态一步预测值的互协方差；kk为滤波增益；为k时刻节点i的状态估计值；pik为k时刻节点i的状态协方差值。

97、重复利用特征点信息、高度信息进行无迹卡尔曼滤波的过程，依次对三节点状态进行估计，得到三节点状态滤波估计值。

98、步骤六：结合步骤四获得融合的节点间相对状态与步骤五获取的三节点状态滤波估计值，进行节点状态修正，直至完成着陆器附着任务。

99、步骤六的具体实现方法为：

100、若要对节点i的状态进行修正，则利用中对应的分量将其余两节点状态传递至节点i。利用下式将节点j状态传递至节点i

101、

102、其中，ja为节点状态的传递矩阵；为k时刻节点i相对于节点j的柔性体节点间相对状态，为的子向量。

103、

104、节点状态的传递协方差为

105、

106、其中，为pfk的子矩阵。

107、利用式(29)与式(31)将节点h的状态传递至节点i，获得与

108、利用下式对节点i的状态进行第一次修正

109、

110、其中，为k时刻节点i状态第一次修正结果，pik,1为其协方差。

111、利用式(29)至(32)，结合与对节点j进行第一次修正获得再利用与对节点h进行第一次修正获得至此完成全部节点的第一次修正。

112、重复上述修正过程，进行多轮修正，直至节点状态的协方差阵满足预定要求或进行的修正满足预定轮次。

113、完成当前时刻状态估计后若柔性着陆器已实现附着则停止状态估计，若仍未完成附着，则进入步骤二进行下一时刻状态估计，直至根据柔性着陆器节点状态估计结果完成着陆器附着任务。

114、有益效果：

115、1、本发明公开的小天体柔性附着状态融合估计方法，采用无迹采样的方法对离线训练的节点间相对状态预报网络进行前向递推，对节点间相对状态进行预报，预报节点间相对状态及其误差协方差，提取柔性着陆器运动过程中节点间牵连运动信息。利用八点法求解重合观测区的匹配角点信息，判断每两个节点相机重合观测区匹配的角点是否达到八对，若满足八对匹配的角点则利用角点信息解算本质矩阵，结合测距信息，求解节点间相对状态信息，并求出节点间相对状态信息的协方差，若不满足则判定这两个节点间无法获取解算的相对状态信息。重复匹配角点判断与解算过程直至完成全部节点间的匹配角点判断与解算，得到观测解算节点间相对状态及节点间相对状态的协方差。根据相对状态及节点间相对状态的协方差提高后续步骤小天体柔性附着状态融合估计的精度和效率。

116、2、本发明公开的小天体柔性附着状态融合估计方法，考虑预报结果与观测信息维度不一致问题，构建多源异构信息分块对角矩阵融合方法，对获取的两部分相对状态进行重构，对齐向量中包含的信息，并对维度低的相对状态进行扩维，随后构建分块对角矩阵形式的加权矩阵，求取最优融合结果及其误差协方差，最后还原向量中的元素排列顺序，获得融合的节点间相对状态，实现节点间相对状态的精确获取。

117、3、本发明公开的小天体柔性附着状态融合估计方法，利用相机观测的特征点信息、激光测距仪测量的高度信息进行无迹卡尔曼滤波，得到三节点状态滤波估计值；利用相机观测的特征点信息、激光测距仪测量的高度信息进行无迹卡尔曼滤波，得到三节点状态滤波估计值；利用融合的节点间相对状态和三节点状态滤波估计值，对滤波估计的结果进行修正，大幅提高各节点状态的估计精度，估计精度高、导航自主性强。