非合作目标柔性附着多节点融合估计方法
技术领域
本发明涉及一种非合作目标附着协同导航方法，尤其涉及一种小天体附着协同导航方法，属于深空探测
技术领域
。背景技术
小天体动力学环境复杂且先验信息匮乏，在其表面实现安全精确附着十分困难。未来小天体探测任务中，柔性多节点协同附着模式能够降低探测器在小天体表面发生倾覆、反弹的风险，并通过协同导航、制导和控制有效提高任务自主性和安全性。为了实现协同自主导航，文献中提出了数据融合和状态融合两种策略，并分别提出了联邦卡尔曼滤波，协方差交叉融合等估计方法。在此基础上，为了进一步提升协同导航性能，还有文献提出了观测时滞及不完全测量下的协同估计方法，修正观测步长异步不确知对协同估计影响。然而，针对非合作小天体柔性附着的多节点协同导航问题，还需要结合柔性附着的特点，考虑节点间相对状态(节点构型)的时变特性，从而实现准确的协同估计，为安全附着提供支持。
发明内容
本发明公开的非合作目标柔性附着多节点融合估计方法要解决的技术问题是：针对小天体柔性多节点协同附着过程，考虑节点构型时变不确知对节点信息融合的影响，在柔性连接约束的前提下，利用多节点光学观测信息估计节点间相对状态，对各节点观测数据或状态进行融合，提高柔性附着过程中各节点自主导航性能，进而提高非合作目标柔性附着协同导航的性能，有利于非合作目标在小天体表面实现安全附着。
本发明的目的是通过下述技术方案实现的。
本发明公开的非合作目标柔性附着多节点融合估计方法，针对小天体柔性附着过程中节点构型时变的问题，利用节点间重合观测区域内角点特征的像元、像线构建多视点几何光学信息，在柔性连接约束的前提下利用多视点几何光学信息对节点构型进行估计，得到较为准确的节点构型，进而利用构型将各节点自身的观测数据或状态估计转化到相邻节点，并通过动力学递推将不同时刻的观测数据或状态估计进行同步处理，实现多节点融合估计。本发明能够提高柔性附着过程中各节点的自主导航性能，有利于实现小天体表面的安全附着。
本发明公开的非合作目标柔性附着多节点融合估计方法，包括如下步骤：
步骤一、由节点间重合观测区域内角点特征的像元、像线构建多视点几何光学信息，在柔性连接约束下利用多视点几何光学信息构建节点构型估计问题，实现协同附着多节点间构型的准确估计。
为了减小时变不确知节点构型对多节点信息融合的影响，在柔性连接约束下利用多视点几何光学信息构建节点构型估计问题，作为优选，所述估计问题选用估计的最小二乘问题。
节点构型表示为
{Lxij∈LXF||(i,j)∈ε} (1)
其中，LXF为节点构型，Lxij＝(Lpij,Lvij，Lqij)为组成构型的元素，代表节点构型抽象成的图，ν代表节点，ε代表节点间的连接关系，与节点i相邻节点的集合表示为
节点搭载相机的观测方程为
其中，为归一化后的特征像元、像线坐标，(xi,yi,zi)为节点i在附着点坐标系下的位置，cij为节点i在附着点坐标系下姿态转换矩阵中的元素，为特征在附着点坐标系下的三维坐标。考虑各节点的相机存在重合观测区域，通过同名特征点的像素坐标两两建立图像间的对极约束关系
其中，和为同一特征在两视点图像中的像元、像线坐标，为本质矩阵，其表征两相机间的位姿关系
其中，为节点间的相对位置，为节点间的相对姿态。联立多组同名特征形成的方程对本质矩阵进行求解，特征分解得到两相机间的相对位姿关系
由式(7)和式(8)，得到节点间相对姿态和相对位置矢量间的约束。值得注意的是，节点间相对姿态的具体取值由特征可见性唯一判定。
其中，Ri为本体坐标系，|·|为绝对值的数学符号，为取模长的数学符号。
同时，由节点间的柔性连接关系还得到节点间相对距离的不等式约束
其中，为标称状态下节点间的相对距离，为结构允许的节点间相对距离变化范围。
式(9)和式(10)为节点构型状态估计提供有效信息，即节点构型的后验估计需要满足
s.t.
其中，J为节点构型无约束最优估计的损失函数，为动力学得到的构型先验估计，为的构型先验协方差，式(12)为式(9)定义的等式约束，式(13)为式(10)定义的不等式约束，式(13)所定义的不等式约束通过有效集法转化为等式约束
上式的具体形式为
式(11)-(13)是一个带约束的最优估计问题，通过拉格朗日乘子法构建约束下节点构型最优估计问题为
其中，*J为约束下节点构型最优估计问题的损失函数，λ1和λ2为拉格朗日乘子，上标(κ)表示矩阵的第κ行，n1和n2为约束个数。对以上最优估计问题进行求解，估计得到的节点构型转入步骤二或步骤三以实现各节点的状态融合估计。
步骤二、将小天体表面形貌特征在各节点相机中的像素坐标作为观测量，当某节点观测信息不足时，利用节点构型将其他节点的观测转化为该节点观测坐标系中，将各节点观测进行融合。
柔性探测器有m个测量节点，节点动力学方程为
其中，和为第i个节点的状态，为控制量，为噪声项。节点受到的控制加速度主要包括执行机构推力加速度和柔性结构作用加速度两个部分，其被惯性导航敏感器测量
上式中，和为加速度计和陀螺仪的测量值，和为惯性导航敏感器的测量噪声。除惯性敏感器外，各节点上还搭载有导航相机，导航相机的观测量为
其中，yi为导航相机的测量量，为特征像素坐标，h为节点距离附着平面的高度，wc为测量噪声，测量噪声wc为高斯白噪声。各节点相机测量方程为
其中，yi为观测量，f为相机焦距，为噪声项，为特征点与节点i相机间的视线矢量，pi为节点i在附着点坐标系下的位置矢量，为特征点在附着点坐标系下的位置矢量，Ri,c为相机坐标系与附着点坐标系间的姿态转换矩阵。
利用步骤一所估计的节点构型将其他节点的观测转化为该节点观测坐标系中，进行各节点观测融合，观测信息转化过程为
其中，yj→i为节点j转化到节点i的观测，为节点j的齐次观测量，为节点j本体系到节点i本体系的姿态转换矩阵，为节点i本体系下节点i到节点j的位置矢量方向。
观测融合后，节点i的观测量yi*为
其中，yi为节点i自身的观测，yj→i为相邻节点观测转化而来的观测。
步骤三、各节点对自身状态进行估计，然后结合步骤一所估计的节点构型，采用估计协方差最小准则对各节点状态估计结果进行融合，提高柔性附着过程中各节点自主导航性能，进而提高非合作目标柔性附着协同导航的性能，有利于非合作目标在小天体表面实现安全附着。
考虑到节点动力学和观测方程均具有较强的不确定性，采用无损卡尔曼滤波器对节点状态进行估计。由无损变换计算得到k-1时刻的sigma采样点为
其中，为k-1时刻节点i状态的sigma采样点，n为的状态维数，λ为尺度参数，ι为过程变量。
无损卡尔曼滤波器的状态预测过程为
无损卡尔曼滤波器的状态更新过程为
通过坐标变换将节点自身的状态估计转化为对相邻节点的状态估计。考虑到状态估计异步，通过无损变换进行状态递推将不同时刻的估计值进行同步处理，状态估计的转化过程为
其中，为由τ时刻状态估计结果动力学递推得到的k时刻构型估计，为由ζ时刻状态估计结果动力学递推得到的k时刻估计。同理，协方差估计的转化过程为
其中，为由ζ时刻状态估计协方差动力学递推得到的k时刻状态估计协方差。
通过协方差交叉融合的方式对各节点的状态进行融合估计
其中，πij为加权系数，由协方差的迹加权得到
利用多节点光学观测信息估计节点间相对状态，进而对各节点观测数据或状态进行融合，提高柔性附着过程中各节点自主导航性能，进而提高非合作目标柔性附着协同导航的性能，有利于非合作目标在小天体表面实现安全附着。
有益效果：
1、本发明公开的非合作目标柔性附着多节点融合估计方法，针对柔性结构导致节点构型时变不确知的问题，利用节点间重合观测区域内角点特征的像元、像线构建多视点几何光学信息，在柔性连接约束的前提下利用多视点几何光学信息对节点间相对状态进行估计型，得到节点构型，有助于多节点系统构型的保持。
2、本发明公开的非合作目标柔性附着多节点融合估计方法，利用构型将各节点自身的观测数据或状态估计转化到相邻节点，并通过动力学递推将不同时刻的观测数据或状态估计进行同步处理，实现各节点的观测数据及状态估计的异步融合，有助提高各节点的导航精度及抗风险能力，进而提高非合作目标柔性附着协同导航的性能，有利于非合作目标在小天体表面实现安全附着。
附图说明
图1为本发明公开的非合作目标柔性附着多节点融合估计方法流程图；
图2为小天体柔性多节点协同附着实际附着轨迹图；
图3为小天体附着过程中节点间相对位置估计误差变化曲线，其中图3(a)为节点1和节点2间相对位置估计误差，图3(b)为节点1和节点3间相对位置估计误差，图3(c)为节点2和节点3间相对位置估计误差；
图4为小天体附着过程中节点间相对姿态估计误差变化曲线，其中图4(a)为节点1和节点2间相对姿态估计误差，图4(b)为节点1和节点3间相对姿态估计误差，图4(c)为节点2和节点3间相对姿态估计误差；
图5为小天体附着过程中节点平均位置估计误差变化曲线，其中图5(a)为各节点x轴位置平均估计误差，图5(b)为各节点y轴位置平均估计误差，图5(c)为各节点z轴位置平均估计误差；
图6为小天体附着过程中节点平均姿态估计误差变化曲线，其中图6(a)为各节点偏航角姿态平均估计误差，图6(b)为各节点俯仰角姿态平均估计误差，图6(c)为各节点滚转角姿态平均估计误差。
具体实施方式
为了更好的说明本发明的目的和优点，下面结合附图和实例对发明内容做进一步说明。
实施例1：
为了验证方法的可行性，以针对某小天体的附着任务为例，进行表面附着自主导航的仿真计算。柔性附着测量节点个数为3个，节点在探测器本体坐标系的位置分别为(-0.375，0)，(0,0.75,0)和(-0.375，0)，探测器质心在附着点坐标系中的初始位置为(10,10,800)m，初始速度为(-0.02 0.01 -0.5)m/s，初始姿态为(1,0,0,0)，初始位置误差为εp～N(0,[25，25,25]T m2)，初始速度误差为εv～N(0,[0.01,0.01,0.01]T(m/s)2)，初始姿态误差为εq～N(0,[25，25,25]T)，各节点的初始速度与探测器本体均保持一致。测量节点均搭载惯性敏感器和导航相机，敏感器坐标系与节点本体坐标系均重合，即Ri,c＝Ri,imu＝Ri。附着过程中姿态控制律使探测器z轴始终垂直指向附着平面，探测器从初始位置经过1800s后到达附着点正上方30m处，标称末端速度为(0,0,0)m/s，附着标称轨迹如阿波罗制导律生成，标称状态下各节点保持初始构型且速度与探测器质心均保持一致。附着过程中提取视场内形貌特征中心点作为特征点，形貌特征由仿真平台生成，2D-2D的匹配误差为0.1pixel，2D-3D的匹配误差为1pixel。
如图1所示，本实施例公开的小天体协同附着多节点融合估计方法，具体实现步骤如下：
步骤一、由节点间重合观测区域内角点特征的像元、像线构建多视点几何光学信息，在柔性连接约束下利用多视点几何光学信息构建节点构型估计问题，实现协同附着多节点间构型的准确估计。
假设柔性探测器有3个测量节点，节点构型表示为
LXF＝(Lx12,Lx13,Lx23) (36)
其中，LXF为节点构型，Lxij＝(Lpij,Lvij,Lqij)为组成构型的元素。
各节点相机间均存在重合观测区域且重合观测区域纹理充足，通过提取重合区域的S-T角点进行特征匹配并用Ransac算法进行误匹配剔除，利用五点法对本质矩阵进行求解，特征分解得到两相机间的相对位姿关系。同时，假设结构允许的节点间相对距离变化范围5％，节点构型的最优估计模型为式(11)至(13)，该优化问题转化为式(16)和式(17)的形式，通过优化工具包对以上最优估计问题进行求解，估计得到的节点构型转入步骤二及步骤三以实现各节点的融合估计。
步骤二、将小天体表面形貌特征在各节点相机中的像素坐标作为观测量，当某节点观测信息不足时，利用节点构型将其他节点的观测转化为该节点观测坐标系中，进行各节点观测进行融合。
各节点的在附着点坐标系下的动力学方程为
其中，Lpi为附着点坐标系下的第j个节点状态为第j个节点的位置，Lvi为第j个节点的速度，Lqi为第j个节点的姿态四元数，为节点受到的合加速度，Ri为当前姿态对应的姿态转换矩阵，和为惯性导航敏感器的漂移参数，和为马尔可夫过程参数，Lω为小天体自旋角速度，Lgi为第j个节点的重力角速度，为第j个节点的角加速度，Ω(·)为4阶反对称矩阵变换符，为反对称矩阵变换符，wi为噪声项。节点受到的合加速度主要包括执行机构推力加速度和柔性结构作用加速度两个部分，被惯性导航敏感器测量为
上式中，和为加速度计和陀螺仪的测量值，和为惯性导航敏感器的测量噪声。除惯性敏感器外，各节点上搭载导航相机及激光测距仪等光学导航敏感器。各节点相机的参数一致，机视场角为30°，相机焦距为717mm，成像分辨率为1024×1024。对于六自由度的状态估计问题，至少需要三个附着系中三维位置已知的路标。利用步骤一所估计的节点构型将其他节点的观测转化到该节点进行观测融合，观测信息转化过程如(24)所示。
步骤三、各节点对自身状态进行估计，然后结合步骤一所估计的节点构型，采用估计协方差最小准则对各节点状态估计结果进行融合，提高柔性附着过程中各节点自主导航性能，进而提高非合作目标柔性附着协同导航的性能，有利于非合作目标在小天体表面实现安全附着。
考虑到节点动力学和观测方程均具有较强的不确定性，采用式(26)至(28)所述的无损卡尔曼滤波器对节点状态进行估计。基于所估计的节点构型，通过式(29)至(32)将节点自身的状态和状态协方差估计转化为对相邻节点的状态估计和状态协方差估计，进而按照式(33)至(35)所述的协方差交叉融合的方式对各节点的状态进行融合估计，提高柔性附着过程中各节点自主导航性能，进而提高非合作目标柔性附着协同导航的性能，有利于非合作目标在小天体表面实现安全附着。
考虑附着轨迹如图2所示，节点构型在z方向上与标称构型存在5％的随机误差，动力学积分步长为5s，动力学噪声误差协方差为
图3和图4分别给出了附着过程中节点间相对位置和相对姿态的构型估计误差变化曲线，其中蓝色虚线是标称构型与实际构型间的误差，红色实线是步骤一所估计的构型与实际构型的误差。图5和图6分别给出了各节点附着过程中的位置、速度和姿态的估计误差变化曲线，其中蓝色虚线是各节点状态估计融合前的状态估计误差，红色实线是各节点状态估计融合后的状态估计误差，在所考虑的场景中每个节点每个观测时刻均可观测到三个以上的已知地形特征。
以上所述的具体描述，对发明的目的、技术方案和有益效果进行了进一步详细说明，所应理解的是，以上所述仅为本发明的具体实施例而已，并不用于限定本发明的保护范围，凡在本发明的精神和原则之内，所做的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。
- 上一篇：石墨接头机器人自动装卡簧、装栓机
- 下一篇：基于迁移学习的任务导向的图像质量测评方法
