CN113408623A - 非合作目标柔性附着多节点融合估计方法 - Google Patents

非合作目标柔性附着多节点融合估计方法 Download PDF

Info

Publication number
CN113408623A
CN113408623A CN202110686620.5A CN202110686620A CN113408623A CN 113408623 A CN113408623 A CN 113408623A CN 202110686620 A CN202110686620 A CN 202110686620A CN 113408623 A CN113408623 A CN 113408623A
Authority
CN
China
Prior art keywords
node
estimation
nodes
observation
state
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Granted
Application number
CN202110686620.5A
Other languages
English (en)
Other versions
CN113408623B (zh
Inventor
崔平远
陆晓萱
朱圣英
梁子璇
葛丹桐
徐瑞
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Beijing Institute of Technology BIT
Original Assignee
Beijing Institute of Technology BIT
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Beijing Institute of Technology BIT filed Critical Beijing Institute of Technology BIT
Priority to CN202110686620.5A priority Critical patent/CN113408623B/zh
Publication of CN113408623A publication Critical patent/CN113408623A/zh
Application granted granted Critical
Publication of CN113408623B publication Critical patent/CN113408623B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06F—ELECTRIC DIGITAL DATA PROCESSING
    • G06F18/00—Pattern recognition
    • G06F18/20—Analysing
    • G06F18/25—Fusion techniques
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/005—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 with correlation of navigation data from several sources, e.g. map or contour matching
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/24—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 specially adapted for cosmonautical navigation
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06F—ELECTRIC DIGITAL DATA PROCESSING
    • G06F30/00—Computer-aided design [CAD]
    • G06F30/20—Design optimisation, verification or simulation
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06F—ELECTRIC DIGITAL DATA PROCESSING
    • G06F2111/00—Details relating to CAD techniques
    • G06F2111/04—Constraint-based CAD

Landscapes

  • Engineering & Computer Science (AREA)
  • Physics & Mathematics (AREA)
  • Remote Sensing (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Theoretical Computer Science (AREA)
  • General Physics & Mathematics (AREA)
  • Automation & Control Theory (AREA)
  • General Engineering & Computer Science (AREA)
  • Evolutionary Computation (AREA)
  • Data Mining & Analysis (AREA)
  • Artificial Intelligence (AREA)
  • Bioinformatics & Computational Biology (AREA)
  • Bioinformatics & Cheminformatics (AREA)
  • Evolutionary Biology (AREA)
  • Life Sciences & Earth Sciences (AREA)
  • Computer Vision & Pattern Recognition (AREA)
  • Astronomy & Astrophysics (AREA)
  • Computer Hardware Design (AREA)
  • Geometry (AREA)
  • Image Processing (AREA)

Abstract

本发明公开的非合作目标柔性附着多节点融合估计方法，涉及一种非合作目标附着协同导航方法，属于深空探测技术领域。本发明实现方法为：针对小天体柔性附着过程中节点构型时变的问题，利用节点间重合观测区域内角点特征的像元、像线构建多视点几何光学信息，在柔性连接约束的前提下利用多视点几何光学信息对节点构型进行估计，得到较为准确的节点构型，进而利用构型将各节点自身的观测数据或状态估计转化到相邻节点，并通过动力学递推将不同时刻的观测数据或状态估计进行同步处理，实现多节点融合估计。本发明能够提高柔性附着过程中各节点的自主导航性能，有利于实现小天体表面的安全附着。

Description

非合作目标柔性附着多节点融合估计方法
技术领域
本发明涉及一种非合作目标附着协同导航方法，尤其涉及一种小天体附着协同导航方法，属于深空探测技术领域。
背景技术
小天体动力学环境复杂且先验信息匮乏，在其表面实现安全精确附着十分困难。未来小天体探测任务中，柔性多节点协同附着模式能够降低探测器在小天体表面发生倾覆、反弹的风险，并通过协同导航、制导和控制有效提高任务自主性和安全性。为了实现协同自主导航，文献中提出了数据融合和状态融合两种策略，并分别提出了联邦卡尔曼滤波，协方差交叉融合等估计方法。在此基础上，为了进一步提升协同导航性能，还有文献提出了观测时滞及不完全测量下的协同估计方法，修正观测步长异步不确知对协同估计影响。然而，针对非合作小天体柔性附着的多节点协同导航问题，还需要结合柔性附着的特点，考虑节点间相对状态(节点构型)的时变特性，从而实现准确的协同估计，为安全附着提供支持。
发明内容
本发明公开的非合作目标柔性附着多节点融合估计方法要解决的技术问题是：针对小天体柔性多节点协同附着过程，考虑节点构型时变不确知对节点信息融合的影响，在柔性连接约束的前提下，利用多节点光学观测信息估计节点间相对状态，对各节点观测数据或状态进行融合，提高柔性附着过程中各节点自主导航性能，进而提高非合作目标柔性附着协同导航的性能，有利于非合作目标在小天体表面实现安全附着。
本发明的目的是通过下述技术方案实现的。
本发明公开的非合作目标柔性附着多节点融合估计方法，针对小天体柔性附着过程中节点构型时变的问题，利用节点间重合观测区域内角点特征的像元、像线构建多视点几何光学信息，在柔性连接约束的前提下利用多视点几何光学信息对节点构型进行估计，得到较为准确的节点构型，进而利用构型将各节点自身的观测数据或状态估计转化到相邻节点，并通过动力学递推将不同时刻的观测数据或状态估计进行同步处理，实现多节点融合估计。本发明能够提高柔性附着过程中各节点的自主导航性能，有利于实现小天体表面的安全附着。
本发明公开的非合作目标柔性附着多节点融合估计方法，包括如下步骤：
步骤一、由节点间重合观测区域内角点特征的像元、像线构建多视点几何光学信息，在柔性连接约束下利用多视点几何光学信息构建节点构型估计问题，实现协同附着多节点间构型的准确估计。
为了减小时变不确知节点构型对多节点信息融合的影响，在柔性连接约束下利用多视点几何光学信息构建节点构型估计问题，作为优选，所述估计问题选用估计的最小二乘问题。
节点构型表示为
{Lxij∈LXF||(i,j)∈ε} (1)
其中，LXF为节点构型，Lxij＝(Lpij,Lvij，Lqij)为组成构型的元素，
Figure BDA0003124960330000021
代表节点构型抽象成的图，ν代表节点，ε代表节点间的连接关系，与节点i相邻节点的集合
Figure BDA0003124960330000022
表示为
Figure BDA0003124960330000023
节点搭载相机的观测方程为
Figure BDA0003124960330000024
其中，
Figure BDA0003124960330000025
为归一化后的特征像元、像线坐标，(xi,yi,zi)为节点i在附着点坐标系下的位置，cij为节点i在附着点坐标系下姿态转换矩阵中的元素，
Figure BDA0003124960330000026
为特征在附着点坐标系下的三维坐标。考虑各节点的相机存在重合观测区域，通过同名特征点的像素坐标两两建立图像间的对极约束关系
Figure BDA0003124960330000027
其中，
Figure BDA0003124960330000028
和
Figure BDA0003124960330000029
为同一特征在两视点图像中的像元、像线坐标，为本质矩阵，其表征两相机间的位姿关系
Figure BDA00031249603300000210
其中，
Figure BDA0003124960330000031
为节点间的相对位置，
Figure BDA0003124960330000032
为节点间的相对姿态。联立多组同名特征形成的方程对本质矩阵进行求解，特征分解得到两相机间的相对位姿关系
Figure BDA0003124960330000033
Figure BDA0003124960330000034
Figure BDA0003124960330000035
由式(7)和式(8)，得到节点间相对姿态和相对位置矢量间的约束。值得注意的是，节点间相对姿态的具体取值由特征可见性唯一判定。
Figure BDA0003124960330000036
其中，Ri为本体坐标系，|·|为绝对值的数学符号，
Figure BDA0003124960330000037
为取模长的数学符号。
同时，由节点间的柔性连接关系还得到节点间相对距离的不等式约束
Figure BDA0003124960330000038
其中，
Figure BDA0003124960330000039
为标称状态下节点间的相对距离，
Figure BDA00031249603300000310
为结构允许的节点间相对距离变化范围。
式(9)和式(10)为节点构型状态估计提供有效信息，即节点构型的后验估计需要满足
Figure BDA00031249603300000311
s.t.
Figure BDA00031249603300000312
Figure BDA00031249603300000313
其中，J为节点构型无约束最优估计的损失函数，
Figure BDA00031249603300000314
为动力学得到的构型先验估计，
Figure BDA00031249603300000315
为的构型先验协方差，式(12)为式(9)定义的等式约束，式(13)为式(10)定义的不等式约束，式(13)所定义的不等式约束通过有效集法转化为等式约束
Figure BDA0003124960330000041
上式的具体形式为
Figure BDA0003124960330000042
式(11)-(13)是一个带约束的最优估计问题，通过拉格朗日乘子法构建约束下节点构型最优估计问题为
Figure BDA0003124960330000043
Figure BDA0003124960330000044
其中，*J为约束下节点构型最优估计问题的损失函数，λ1和λ2为拉格朗日乘子，上标(κ)表示矩阵的第κ行，n1和n2为约束个数。对以上最优估计问题进行求解，估计得到的节点构型转入步骤二或步骤三以实现各节点的状态融合估计。
步骤二、将小天体表面形貌特征在各节点相机中的像素坐标作为观测量，当某节点观测信息不足时，利用节点构型将其他节点的观测转化为该节点观测坐标系中，将各节点观测进行融合。
柔性探测器有m个测量节点，节点动力学方程为
Figure BDA0003124960330000045
其中，
Figure BDA0003124960330000046
和
Figure BDA0003124960330000047
为第i个节点的状态，
Figure BDA0003124960330000048
为控制量，
Figure BDA0003124960330000049
为噪声项。节点受到的控制加速度
Figure BDA00031249603300000410
主要包括执行机构推力加速度
Figure BDA00031249603300000411
和柔性结构作用加速度
Figure BDA00031249603300000412
两个部分，其被惯性导航敏感器测量
Figure BDA00031249603300000413
Figure BDA00031249603300000414
上式中，
Figure BDA00031249603300000415
和
Figure BDA00031249603300000416
为加速度计和陀螺仪的测量值，
Figure BDA00031249603300000417
和
Figure BDA00031249603300000418
为惯性导航敏感器的测量噪声。除惯性敏感器外，各节点上还搭载有导航相机，导航相机的观测量为
Figure BDA0003124960330000051
其中，yi为导航相机的测量量，
Figure BDA0003124960330000052
为特征像素坐标，h为节点距离附着平面的高度，wc为测量噪声，测量噪声wc为高斯白噪声。各节点相机测量方程为
Figure BDA0003124960330000053
Figure BDA0003124960330000054
其中，yi为观测量，f为相机焦距，
Figure BDA0003124960330000055
为噪声项，
Figure BDA0003124960330000056
为特征点与节点i相机间的视线矢量，pi为节点i在附着点坐标系下的位置矢量，
Figure BDA0003124960330000057
为特征点在附着点坐标系下的位置矢量，Ri,c为相机坐标系与附着点坐标系间的姿态转换矩阵。
利用步骤一所估计的节点构型将其他节点的观测转化为该节点观测坐标系中，进行各节点观测融合，观测信息转化过程为
Figure BDA0003124960330000058
其中，yj→i为节点j转化到节点i的观测，
Figure BDA0003124960330000059
为节点j的齐次观测量，
Figure BDA00031249603300000510
为节点j本体系到节点i本体系的姿态转换矩阵，
Figure BDA00031249603300000511
为节点i本体系下节点i到节点j的位置矢量方向。
观测融合后，节点i的观测量yi*为
Figure BDA00031249603300000512
其中，yi为节点i自身的观测，yj→i为相邻节点观测转化而来的观测。
步骤三、各节点对自身状态进行估计，然后结合步骤一所估计的节点构型，采用估计协方差最小准则对各节点状态估计结果进行融合，提高柔性附着过程中各节点自主导航性能，进而提高非合作目标柔性附着协同导航的性能，有利于非合作目标在小天体表面实现安全附着。
考虑到节点动力学和观测方程均具有较强的不确定性，采用无损卡尔曼滤波器对节点状态进行估计。由无损变换计算得到k-1时刻的sigma采样点为
Figure BDA0003124960330000061
其中，
Figure BDA0003124960330000062
为k-1时刻节点i状态
Figure BDA0003124960330000063
的sigma采样点，n为
Figure BDA0003124960330000064
的状态维数，λ为尺度参数，ι为过程变量。
无损卡尔曼滤波器的状态预测过程为
Figure BDA0003124960330000065
无损卡尔曼滤波器的状态更新过程为
Figure BDA0003124960330000066
通过坐标变换将节点自身的状态估计转化为对相邻节点的状态估计。考虑到状态估计异步，通过无损变换进行状态递推将不同时刻的估计值进行同步处理，状态估计的转化过程为
Figure BDA0003124960330000067
Figure BDA0003124960330000068
其中，
Figure BDA0003124960330000069
为由τ时刻状态估计结果
Figure BDA00031249603300000610
动力学递推得到的k时刻构型估计，
Figure BDA00031249603300000611
为由ζ时刻状态估计结果
Figure BDA00031249603300000612
动力学递推得到的k时刻估计。同理，协方差估计的转化过程为
Figure BDA0003124960330000071
Figure BDA0003124960330000072
其中，
Figure BDA0003124960330000073
为由ζ时刻状态估计协方差
Figure BDA0003124960330000074
动力学递推得到的k时刻状态估计协方差。
通过协方差交叉融合的方式对各节点的状态进行融合估计
Figure BDA0003124960330000075
Figure BDA0003124960330000076
其中，πij为加权系数，由协方差的迹加权得到
Figure BDA0003124960330000077
利用多节点光学观测信息估计节点间相对状态，进而对各节点观测数据或状态进行融合，提高柔性附着过程中各节点自主导航性能，进而提高非合作目标柔性附着协同导航的性能，有利于非合作目标在小天体表面实现安全附着。
有益效果：
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个，节点在探测器本体坐标系的位置分别为(
Figure BDA0003124960330000081
-0.375，0)，(0,0.75,0)和(
Figure BDA0003124960330000082
-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)的形式，通过优化工具包对以上最优估计问题进行求解，估计得到的节点构型转入步骤二及步骤三以实现各节点的融合估计。
步骤二、将小天体表面形貌特征在各节点相机中的像素坐标作为观测量，当某节点观测信息不足时，利用节点构型将其他节点的观测转化为该节点观测坐标系中，进行各节点观测进行融合。
各节点的在附着点坐标系下的动力学方程为
Figure BDA0003124960330000091
其中，Lpi为附着点坐标系下的第j个节点状态为第j个节点的位置，Lvi为第j个节点的速度，Lqi为第j个节点的姿态四元数，
Figure BDA0003124960330000092
为节点受到的合加速度，Ri为当前姿态对应的姿态转换矩阵，
Figure BDA0003124960330000093
和
Figure BDA0003124960330000094
为惯性导航敏感器的漂移参数，
Figure BDA0003124960330000095
和
Figure BDA0003124960330000096
为马尔可夫过程参数，Lω为小天体自旋角速度，Lgi为第j个节点的重力角速度，
Figure BDA0003124960330000097
为第j个节点的角加速度，Ω(·)为4阶反对称矩阵变换符，
Figure BDA0003124960330000098
为反对称矩阵变换符，wi为噪声项。节点受到的合加速度
Figure BDA0003124960330000099
主要包括执行机构推力加速度
Figure BDA0003124960330000101
和柔性结构作用加速度
Figure BDA0003124960330000102
两个部分，被惯性导航敏感器测量为
Figure BDA0003124960330000103
Figure BDA0003124960330000104
上式中，
Figure BDA0003124960330000105
和
Figure BDA0003124960330000106
为加速度计和陀螺仪的测量值，
Figure BDA0003124960330000107
和
Figure BDA0003124960330000108
为惯性导航敏感器的测量噪声。除惯性敏感器外，各节点上搭载导航相机及激光测距仪等光学导航敏感器。各节点相机的参数一致，机视场角为30°，相机焦距为717mm，成像分辨率为1024×1024。对于六自由度的状态估计问题，至少需要三个附着系中三维位置已知的路标。利用步骤一所估计的节点构型将其他节点的观测转化到该节点进行观测融合，观测信息转化过程如(24)所示。
步骤三、各节点对自身状态进行估计，然后结合步骤一所估计的节点构型，采用估计协方差最小准则对各节点状态估计结果进行融合，提高柔性附着过程中各节点自主导航性能，进而提高非合作目标柔性附着协同导航的性能，有利于非合作目标在小天体表面实现安全附着。
考虑到节点动力学和观测方程均具有较强的不确定性，采用式(26)至(28)所述的无损卡尔曼滤波器对节点状态进行估计。基于所估计的节点构型，通过式(29)至(32)将节点自身的状态和状态协方差估计转化为对相邻节点的状态估计和状态协方差估计，进而按照式(33)至(35)所述的协方差交叉融合的方式对各节点的状态进行融合估计，提高柔性附着过程中各节点自主导航性能，进而提高非合作目标柔性附着协同导航的性能，有利于非合作目标在小天体表面实现安全附着。
考虑附着轨迹如图2所示，节点构型在z方向上与标称构型存在5％的随机误差，动力学积分步长为5s，动力学噪声误差协方差为
Figure BDA0003124960330000109
图3和图4分别给出了附着过程中节点间相对位置和相对姿态的构型估计误差变化曲线，其中蓝色虚线是标称构型与实际构型间的误差，红色实线是步骤一所估计的构型与实际构型的误差。图5和图6分别给出了各节点附着过程中的位置、速度和姿态的估计误差变化曲线，其中蓝色虚线是各节点状态估计融合前的状态估计误差，红色实线是各节点状态估计融合后的状态估计误差，在所考虑的场景中每个节点每个观测时刻均可观测到三个以上的已知地形特征。
以上所述的具体描述，对发明的目的、技术方案和有益效果进行了进一步详细说明，所应理解的是，以上所述仅为本发明的具体实施例而已，并不用于限定本发明的保护范围，凡在本发明的精神和原则之内，所做的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。

Claims (5)

1.非合作目标柔性附着多节点融合估计方法，其特征在于：包括如下步骤，
步骤一、由节点间重合观测区域内角点特征的像元、像线构建多视点几何光学信息，在柔性连接约束下利用多视点几何光学信息构建节点构型估计问题，实现协同附着多节点间构型的准确估计；
步骤二、将小天体表面形貌特征在各节点相机中的像素坐标作为观测量，当某节点观测信息不足时，利用节点构型将其他节点的观测转化为该节点观测坐标系中，将各节点观测进行融合；
步骤三、各节点对自身状态进行估计，然后结合步骤一所估计的节点构型，采用估计协方差最小准则对各节点状态估计结果进行融合，提高柔性附着过程中各节点自主导航性能，进而提高非合作目标柔性附着协同导航的性能，有利于非合作目标在小天体表面实现安全附着。
2.如权利要求1所述的非合作目标柔性附着多节点融合估计方法，其特征在于：步骤一实现方法为，
节点构型表示为
{Lxij∈LXF||(i,j)∈ε} (1)
其中，LXF为节点构型，Lxij＝(Lpij,Lvij，Lqij)为组成构型的元素，
Figure FDA0003124960320000011
代表节点构型抽象成的图，
Figure FDA0003124960320000012
代表节点，ε代表节点间的连接关系，与节点i相邻节点的集合
Figure FDA0003124960320000013
表示为
Figure FDA0003124960320000014
节点搭载相机的观测方程为
Figure FDA0003124960320000015
其中，
Figure FDA0003124960320000016
为归一化后的特征像元、像线坐标，(xi,yi,zi)为节点i在附着点坐标系下的位置，cij为节点i在附着点坐标系下姿态转换矩阵中的元素，
Figure FDA0003124960320000017
为特征在附着点坐标系下的三维坐标；考虑各节点的相机存在重合观测区域，通过同名特征点的像素坐标两两建立图像间的对极约束关系
Figure FDA0003124960320000018
其中，
Figure FDA0003124960320000019
和
Figure FDA00031249603200000110
为同一特征在两视点图像中的像元、像线坐标，为本质矩阵，其表征两相机间的位姿关系
Figure FDA0003124960320000021
其中，
Figure FDA0003124960320000022
为节点间的相对位置，
Figure FDA0003124960320000023
为节点间的相对姿态；联立多组同名特征形成的方程对本质矩阵进行求解，特征分解得到两相机间的相对位姿关系
Figure FDA0003124960320000024
Figure FDA0003124960320000025
Figure FDA0003124960320000026
由式(7)和式(8)，得到节点间相对姿态和相对位置矢量间的约束；值得注意的是，节点间相对姿态的具体取值由特征可见性唯一判定；
Figure FDA0003124960320000027
其中，Ri为本体坐标系，|·|为绝对值的数学符号，
Figure FDA0003124960320000028
为取模长的数学符号；
同时，由节点间的柔性连接关系还得到节点间相对距离的不等式约束
Figure FDA0003124960320000029
其中，
Figure FDA00031249603200000210
为标称状态下节点间的相对距离，
Figure FDA00031249603200000211
为结构允许的节点间相对距离变化范围；
式(9)和式(10)为节点构型状态估计提供有效信息，即节点构型的后验估计需要满足
Figure FDA00031249603200000212
s.t.
Figure FDA00031249603200000213
Figure FDA00031249603200000214
其中，J为节点构型无约束最优估计的损失函数，
Figure FDA00031249603200000215
为动力学得到的构型先验估计，
Figure FDA00031249603200000216
为的构型先验协方差，式(12)为式(9)定义的等式约束，式(13)为式(10)定义的不等式约束，式(13)所定义的不等式约束通过有效集法转化为等式约束
Figure FDA00031249603200000217
上式的具体形式为
Figure FDA0003124960320000031
式(11)-(13)是一个带约束的最优估计问题，通过拉格朗日乘子法构建约束下节点构型最优估计问题为
Figure FDA0003124960320000032
Figure FDA0003124960320000033
其中，*J为约束下节点构型最优估计问题的损失函数，λ1和λ2为拉格朗日乘子，上标(κ)表示矩阵的第κ行，n1和n2为约束个数；对以上最优估计问题进行求解，估计得到的节点构型转入步骤二或步骤三以实现各节点的状态融合估计。
3.如权利要求2所述的非合作目标柔性附着多节点融合估计方法，其特征在于：步骤二实现方法为，
柔性探测器有m个测量节点，节点动力学方程为
Figure FDA0003124960320000034
其中，
Figure FDA0003124960320000035
和
Figure FDA0003124960320000036
为第i个节点的状态，
Figure FDA0003124960320000037
为控制量，
Figure FDA0003124960320000038
为噪声项；节点受到的控制加速度
Figure FDA0003124960320000039
主要包括执行机构推力加速度
Figure FDA00031249603200000310
和柔性结构作用加速度
Figure FDA00031249603200000311
两个部分，其被惯性导航敏感器测量
Figure FDA00031249603200000312
Figure FDA00031249603200000313
上式中，
Figure FDA00031249603200000314
和
Figure FDA00031249603200000315
为加速度计和陀螺仪的测量值，
Figure FDA00031249603200000316
和
Figure FDA00031249603200000317
为惯性导航敏感器的测量噪声；除惯性敏感器外，各节点上还搭载有导航相机，导航相机的观测量为
Figure FDA00031249603200000318
其中，yi为导航相机的测量量，
Figure FDA00031249603200000319
为特征像素坐标，h为节点距离附着平面的高度，wc为测量噪声，测量噪声wc为高斯白噪声；各节点相机测量方程为
Figure FDA0003124960320000041
Figure FDA0003124960320000042
其中，yi为观测量，f为相机焦距，
Figure FDA0003124960320000043
为噪声项，
Figure FDA0003124960320000044
为特征点与节点i相机间的视线矢量，pi为节点i在附着点坐标系下的位置矢量，
Figure FDA0003124960320000045
为特征点在附着点坐标系下的位置矢量，Ri,c为相机坐标系与附着点坐标系间的姿态转换矩阵；
利用步骤一所估计的节点构型将其他节点的观测转化为该节点观测坐标系中，进行各节点观测融合，观测信息转化过程为
Figure FDA0003124960320000046
其中，yj→i为节点j转化到节点i的观测，
Figure FDA0003124960320000047
为节点j的齐次观测量，
Figure FDA0003124960320000048
为节点j本体系到节点i本体系的姿态转换矩阵，
Figure FDA0003124960320000049
为节点i本体系下节点i到节点j的位置矢量方向；
观测融合后，节点i的观测量yi*为
Figure FDA00031249603200000410
其中，yi为节点i自身的观测，yj→i为相邻节点观测转化而来的观测。
4.如权利要求3所述的非合作目标柔性附着多节点融合估计方法，其特征在于：步骤三实现方法为，
考虑到节点动力学和观测方程均具有较强的不确定性，采用无损卡尔曼滤波器对节点状态进行估计；由无损变换计算得到k-1时刻的sigma采样点为
Figure FDA00031249603200000411
其中，
Figure FDA00031249603200000412
为k-1时刻节点i状态
Figure FDA00031249603200000413
的sigma采样点，n为
Figure FDA00031249603200000414
的状态维数，λ为尺度参数，ι为过程变量；
无损卡尔曼滤波器的状态预测过程为
Figure FDA0003124960320000051
无损卡尔曼滤波器的状态更新过程为
Figure FDA0003124960320000052
通过坐标变换将节点自身的状态估计转化为对相邻节点的状态估计；考虑到状态估计异步，通过无损变换进行状态递推将不同时刻的估计值进行同步处理，状态估计的转化过程为
Figure FDA0003124960320000053
Figure FDA0003124960320000054
其中，
Figure FDA0003124960320000055
为由τ时刻状态估计结果
Figure FDA0003124960320000056
动力学递推得到的k时刻构型估计，
Figure FDA0003124960320000057
为由ζ时刻状态估计结果
Figure FDA0003124960320000058
动力学递推得到的k时刻估计；同理，协方差估计的转化过程为
Figure FDA0003124960320000059
Figure FDA00031249603200000510
其中，
Figure FDA00031249603200000511
为由ζ时刻状态估计协方差
Figure FDA00031249603200000512
动力学递推得到的k时刻状态估计协方差；
通过协方差交叉融合的方式对各节点的状态进行融合估计
Figure FDA00031249603200000513
Figure FDA00031249603200000514
其中，πij为加权系数，由协方差的迹加权得到
Figure FDA00031249603200000515
利用多节点光学观测信息估计节点间相对状态，进而对各节点观测数据或状态进行融合，提高柔性附着过程中各节点自主导航性能，进而提高非合作目标柔性附着协同导航的性能，有利于非合作目标在小天体表面实现安全附着。
5.如权利要求4所述的非合作目标柔性附着多节点融合估计方法，其特征在于：为了减小时变不确知节点构型对多节点信息融合的影响，在柔性连接约束下利用多视点几何光学信息构建节点构型估计问题，所述估计问题选用估计的最小二乘问题。
CN202110686620.5A 2021-06-21 2021-06-21 非合作目标柔性附着多节点融合估计方法 Active CN113408623B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202110686620.5A CN113408623B (zh) 2021-06-21 2021-06-21 非合作目标柔性附着多节点融合估计方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202110686620.5A CN113408623B (zh) 2021-06-21 2021-06-21 非合作目标柔性附着多节点融合估计方法

Publications (2)

Publication Number Publication Date
CN113408623A true CN113408623A (zh) 2021-09-17
CN113408623B CN113408623B (zh) 2022-10-04

Family

ID=77681972

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202110686620.5A Active CN113408623B (zh) 2021-06-21 2021-06-21 非合作目标柔性附着多节点融合估计方法

Country Status (1)

Country Link
CN (1) CN113408623B (zh)

Cited By (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN114111782A (zh) * 2021-10-11 2022-03-01 北京理工大学 柔性着陆器状态表征与估计方法
CN114296069A (zh) * 2021-12-23 2022-04-08 青岛科技大学 一种基于视觉雷达的小天体探测器多模型导航方法

Citations (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN103884340A (zh) * 2014-03-31 2014-06-25 北京控制工程研究所 一种深空探测定点软着陆过程的信息融合导航方法
CN107132542A (zh) * 2017-05-02 2017-09-05 北京理工大学 一种基于光学和多普勒雷达的小天体软着陆自主导航方法
WO2020087846A1 (zh) * 2018-10-31 2020-05-07 东南大学 基于迭代扩展卡尔曼滤波融合惯性与单目视觉的导航方法
CN112985420A (zh) * 2021-03-01 2021-06-18 北京理工大学 小天体附着光学导航特征递推优选方法

Patent Citations (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN103884340A (zh) * 2014-03-31 2014-06-25 北京控制工程研究所 一种深空探测定点软着陆过程的信息融合导航方法
CN107132542A (zh) * 2017-05-02 2017-09-05 北京理工大学 一种基于光学和多普勒雷达的小天体软着陆自主导航方法
WO2020087846A1 (zh) * 2018-10-31 2020-05-07 东南大学 基于迭代扩展卡尔曼滤波融合惯性与单目视觉的导航方法
CN112985420A (zh) * 2021-03-01 2021-06-18 北京理工大学 小天体附着光学导航特征递推优选方法

Non-Patent Citations (3)

* Cited by examiner, † Cited by third party
Title
XU YUAN等: "Hazard Avoidance Guidance for Planetary Landing Using a Dynamic Safety Margin Index", 《IEEE》 *
ZHENGSHI YU等: "A Robust Kalman Filter for Mars Entry Navigation", 《PROCEEDINGS OF THE 34TH CHINESE CONTROL CONFERENCE》 *
崔平远等: "小天体自主附着技术研究进展", 《宇航学报》 *

Cited By (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN114111782A (zh) * 2021-10-11 2022-03-01 北京理工大学 柔性着陆器状态表征与估计方法
CN114111782B (zh) * 2021-10-11 2023-11-10 北京理工大学 柔性着陆器状态表征与估计方法
CN114296069A (zh) * 2021-12-23 2022-04-08 青岛科技大学 一种基于视觉雷达的小天体探测器多模型导航方法
CN114296069B (zh) * 2021-12-23 2024-05-28 青岛科技大学 一种基于视觉雷达的小天体探测器多模型导航方法

Also Published As

Publication number Publication date
CN113408623B (zh) 2022-10-04

Similar Documents

Publication Publication Date Title
CN106708066B (zh) 基于视觉/惯导的无人机自主着陆方法
CN104236548B (zh) 一种微型无人机室内自主导航方法
Panahandeh et al. Vision-aided inertial navigation based on ground plane feature detection
CN108731670A (zh) 基于量测模型优化的惯性/视觉里程计组合导航定位方法
CN109269512B (zh) 行星着陆图像与测距融合的相对导航方法
CN111156998A (zh) 一种基于rgb-d相机与imu信息融合的移动机器人定位方法
CN106017463A (zh) 一种基于定位传感装置的飞行器定位方法
CN106989744A (zh) 一种融合机载多传感器的旋翼无人机自主定位方法
CN113408623B (zh) 非合作目标柔性附着多节点融合估计方法
CN110726406A (zh) 一种改进的非线性优化单目惯导slam的方法
Siegwart et al. Autonomous mobile robots
CN107144278B (zh) 一种基于多源特征的着陆器视觉导航方法
CN109631876B (zh) 一种基于单相机导航影像的巡视探测器定位方法
CN107883965A (zh) 基于光学信息交互多模型强跟踪容积卡尔曼滤波导航方法
Li et al. Rock modeling and matching for autonomous long‐range Mars rover localization
CN113074725B (zh) 一种基于多源信息融合的小型水下多机器人协同定位方法及系统
CN115272596A (zh) 一种面向单调无纹理大场景的多传感器融合slam方法
CN110412868A (zh) 一种使用星间光学图像的非合作航天器轨道确定方法
CN111667100A (zh) 敏捷卫星单轨多点目标立体成像任务规划方法及系统
CN114719848A (zh) 基于视觉与惯性导航信息融合神经网络的无人机高度估算方法
CN113022898B (zh) 弱引力环境柔性附着系统状态估计方法
So et al. Visual odometry for a hopping rover on an asteroid surface using multiple monocular cameras
CN114459474B (zh) 一种基于因子图的惯性/偏振/雷达/光流紧组合导航的方法
CN115930948A (zh) 一种果园机器人融合定位方法
Huntsberger et al. Sensory fusion for planetary surface robotic navigation, rendezvous, and manipulation operations

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination
GR01 Patent grant
GR01 Patent grant