[发明专利]弱引力环境柔性附着系统状态估计方法

权利要求书

1.弱引力环境柔性附着系统状态估计方法，其特征在于：包括如下步骤：

步骤一、将小天体表面形貌特征在各节点相机中的像素坐标作为观测量，通过与形貌数据库进行匹配建立多节点状态估计的协同观测方程；

在地面站的支持下，小天体着陆器在绕飞过程中可以建立详尽的目标天体表面形貌数据库，从而确定导航陆标在小天体固连坐标系下的三维位置；选取形貌特征点作为小天体附着过程中的导航陆标；

着陆器的着陆器本体坐标系o_B-x_By_Bz_B为基准观测坐标系；着陆器本体坐标系o_B-x_By_Bz_B的原点o_B为着陆器质心，三轴方向分别指向着陆器三个惯性主轴的方向；定义各节点相机在导航基准坐标系的位置和指向为

([R¹|t¹],…,[R^m|t^m]) (1)

其中，m为带相机的节点的个数，t¹…t^m为各节点相机光心相对于着陆器质心的平移矩阵，R¹…R^m为各节点相机指向相对于导航基准坐标系的旋转矩阵；各节点相机固定安装，即在整个附着过程中，各节点相机相对于导航基准坐标系的指向不变，则各节点相机在导航基准坐标系的指向一致，即满足：

R¹＝...＝R^m＝R (2)

其中，R为各节点相机在导航基准坐标系的指向；

在柔性附着过程中，着陆器在附着点坐标系中的状态被表征为节点在附着点坐标系中的位置^Lr¹，...，^Lr^m和节点构成的平面与附着平面间的旋转矩阵^LR^o，即

x＝[(^Lr¹)^T,…,^Lrⁱ…,(^Lr^m)^T,rotm2eul(^LR^o)]^T (3)

^Lrⁱ＝^Lr+R·tⁱ,i＝1,…,m (4)

其中，rotm2eul(·)为姿态旋转矩阵转换为欧拉角的运算符，^Lr为着陆器质心在附着点坐标系的位置矢量，^LR^o由任意三个节点在附着点坐标系中的位置^Lrⁱ，^Lr^j和^Lr^k所给定，e_x,e_y,e_z为中间变量；

节点i所带相机能够观测到的形貌特征点有n_i个，采用小孔成像的相机模型，任一形貌特征点p_j在节点i任一帧导航图像的像素位置与特征点p_j在附着点坐标系中三维位置间的关系为

其中，^LbR为附着点坐标系到着陆器本体坐标系的转换矩阵，sⁱ为尺度因子，为特征点p_j在节点i导航图像中的齐次像素坐标向量，特征点p_j在节点i的附着点坐标系下的齐次坐标位置Kⁱ为节点i相机的内参矩阵，满足

其中，fⁱ为节点i的相机焦距，和为节点i相机的像素尺度变换系数，为节点i相机的光心坐标；

由此，得到m个节点状态估计的协同观测方程为

其中，^LM¹，…，^LM^m为m个节点特征观测量的集合，为m个节点观测到的特征点的集合；

步骤二、利用节点间的相对测量信息对节点间相对距离的大小和方向进行校正；

节点间相对距离的大小和方向称为几何模型；

节点间相对距离的大小和方向作为多节点状态估计的约束条件，对于m个节点的系统，节点状态的约束表示为

其中，^Lρ¹,…,^Lρ^en为标称的节点间相对位置，en为约束数目；

在附着过程中，受环境扰动、控制误差和柔性体物理特性因素的影响，各节点相机在导航基准坐标系的位置会发生变化，导致节点间的实际几何构型与标称量间往往存在偏差，即

其中，为带误差的节点间相对位置，需要结合节点间测量信息进行校正；

结合节点间测量信息对式(11)进行校正的方法为：

方法1：通过相机视觉测量建立节点间相对位置的指向约束：

其中，为校正后节点间相对位置的单位方向；

方法2：通过无线电测量建立节点间相对位置的长度约束：

其中，为校正后节点间相对位置的模长；

方法3：通过相机视觉测量和无线电测量组合测量建立节点间相对位置的矢量约束：

步骤三、将步骤二误差校正后的几何构型，即式(12)、(13)或(14)作为多节点状态估计的约束条件，将柔性附着系统位姿估计问题转化为带约束条件的多节点状态估计问题，对节点的状态进行精确估计；

各节点相机在基准观测坐标系中的指向R已知，由式(8)构造求解各节点状态求解的目标函数

X＝argminf(X) (15)

其中，f(X)为损失函数，X为需要估计的变量，

X＝[rotm2eul(^LbR·R),(^Lr¹)^T,…,(^Lr^m)^T]^T (17)

把校正后的节点几何构型，即式(12)、(13)或(14)作为各节点在导航基准坐标系中状态求解的约束条件，将柔性附着系统位姿估计问题转化为带约束条件的多节点状态估计问题

通过罚函数法，将带约束条件的状态估计问题转化为以下最小化问题

minL(X,λ) (19)

其中，λ为惩罚系数；由于f(X)和均具有凸的形式，故采用莱文贝格－马夸特方法等对式(20)进行求解；

步骤四、通过式(19)(20)快速估计弱引力环境下柔性附着系统的状态，结合制导控制算法，实现在小天体表面精确安全附着的任务目标。