不等式约束的非线性系统卡尔曼滤波方法技术方案

本发明专利技术公开的不等式约束的非线性系统卡尔曼滤波方法，属于深空探测技术领域。本发明专利技术实现方法为：构建柔性附着导航估计问题；根据无迹卡尔曼滤波器求解不考虑柔性体约束条件的无约束估计值，将柔性附着导航估计问题中约束估计值表示成关于无约束估计值的函数，更新节点状态的估计误差协方差矩阵。基于修正函数构建用于解算约束估计值的约束卡尔曼滤波器；引入缩放因子连接变换后的约束估计误差与无约束估计误差；分析缩放因子对于该误差协方差矩阵中各元素的影响。将修正函数设计成投影函数，求解柔性体约束下节点状态可行域的凸包，将无约束估计值投影到该凸包上，从而解算出柔性附着导航估计问题中的约束估计值，提高柔性附着导航估计精度。附着导航估计精度。附着导航估计精度。

【技术实现步骤摘要】
不等式约束的非线性系统卡尔曼滤波方法


[0001]本专利技术涉及一种滤波估计方法，尤其涉及一种不等式约束的非线性系统卡尔曼滤波方法，属于深空探测


技术介绍

[0002]在小天体柔性附着过程中，由于受到柔性材料限制，柔性着陆器各节点间相对位置矢量时刻满足柔性体约束。这种不等式形式的物理约束为柔性着陆器状态估计提供了丰富的先验信息，限定了状态空间的可行区域。通过在状态估计过程中合理引入约束，可以提升状态估计的精度，进而提升滤波器的性能。
[0003]考虑上述因素，有必要对不等式约束的非线性系统滤波方法进行研究，在考虑状态受到物理约束限制的条件下，实现状态的精确估计。

技术实现思路

[0004]本专利技术公开的不等式约束的非线性系统卡尔曼滤波方法所解决的技术问题是：根据柔性着陆器非线性动力学状态方程、观测方程以及节点间相对位置矢量时刻满足的柔性体约束，设计不等式约束的非线性滤波器，实现对柔性着陆器状态的精确估计。
[0005]本专利技术目的是通过下述技术方案实现的。
[0006]本专利技术公开的不等式约束的非线性系统卡尔曼滤波方法，构建柔性着陆器节点动力学方程与观测方程，并给定柔性体约束条件，进而构建柔性附着导航估计问题。根据无迹卡尔曼滤波器求解不考虑柔性体约束条件的无约束估计值，将柔性附着导航估计问题中约束估计值表示成关于无约束估计值的函数，并对节点状态的估计误差协方差矩阵进行更新。以无约束估计值为自变量，以约束估计值为因变量，定义通过修正无约束估计值解算得到约束估计值的修正函数，基于所述修正函数构建用于解算约束估计值的约束卡尔曼滤波器。对柔性着陆器节点的状态空间进行坐标变换，引入缩放因子连接变换后的约束估计误差与无约束估计误差。利用节点状态估计误差协方差矩阵，分析缩放因子对于该误差协方差矩阵中各元素的影响，并分析缩放因子对误差协方差矩阵迹的影响，从而分析缩放因子对于柔性附着导航估计性能的影响。将修正函数设计成投影函数，求解柔性体约束下节点状态可行域的凸包，将所述凸包作为约束卡尔曼滤波器的约束，通过约束卡尔曼滤波器对无约束估计值进行修正，将无约束估计值投影到该凸包上，从而解算出待解算的柔性附着导航估计问题中的约束估计值，提高柔性附着导航估计精度。
[0007]本专利技术公开的不等式约束的非线性系统卡尔曼滤波方法，包括如下步骤：
[0008]步骤1、在小天体中心固连坐标系下，构建柔性着陆器节点动力学方程与观测方程，并给定柔性体约束条件，进而构建柔性附着导航估计问题；根据无迹卡尔曼滤波器求解不考虑柔性体约束条件的无约束估计值，将柔性附着导航估计问题中约束估计值表示成关于无约束估计值的函数，并对节点状态的估计误差协方差矩阵进行更新。以无约束估计值为自变量，以约束估计值为因变量，定义通过修正无约束估计值解算得到约束估计值的修
正函数，基于所述修正函数构建用于解算约束估计值的约束卡尔曼滤波器，通过约束卡尔曼滤波器对无约束估计值进行修正，得到柔性附着导航估计问题中的约束估计值。所述修正函数基于后续步骤2至4得到。
[0009]柔性附着过程中，节点ψ(ψ＝1,2,3)在小天体中心固连坐标系下的动力学方程为
[0010][0011]其中，ω＝[ω
x
,ω
y
,ω
z
]T
为小天体自转角速度矢量，g(r
ψ
)＝[g
ψx
,g
ψy
,g
ψz
]T
为当地引力加速度矢量，T
ψ
＝[T
ψx
,T
ψy
,T
ψz
]T
为推力矢量，为推力幅值，为柔性内力矢量，m
ψ
为节点ψ的质量。
[0012]柔性内力由保守力项和耗散力项构成，分别使用三维弹簧项和阻尼项对保守力项和耗散力项进行近似。对于节点1，节点1受到的柔性内力表示为和分别表示为
[0013][0014][0015]其中，和分别为节点12和节点13间弹簧项的系数矩阵，和分别为节点12和节点13间阻尼项的系数矩阵；r
12
＝r2‑
r1，r
13
＝r3‑
r1，Δr
12
＝||r
12
||
‑
l0和Δr
13
＝||r
13
||
‑
l0分别为节点12和节点13间的形变，l0为节点间的初始距离，v
12
＝v2‑
v1，v
13
＝v3‑
v1，和分别为节点12和节点13间的相对速度。
[0016]对于节点2，节点2受到的柔性内力表示为表示为
[0017][0018]其中，和分别为节点23和节点23间弹簧项和阻尼项的系数矩阵；r
23
＝r3‑
r2，Δr
23
＝||r
23
||
‑
l0为节点23间的形变，为节点23间的相对速度。
[0019]对于节点3，节点3受到的柔性内力表示为
[0020]在附着过程中，柔性着陆器节点观测量通过安装在节点上的光学相机和激光测距仪得到，因此节点ψ关于导航路标的观测方程表示为
[0021][0022]其中，f是焦距，p为像素大小，为第个导航路标在小天体固连坐标系的位置向量，和分别为带有噪声δu和δv的像素坐标，δd为激光测距仪的测量误差。像素坐标和表示为
[0023][0024]由于柔性材料的形变受到限制，附着过程中节点时刻满足柔性约束，表示为
[0025]l
min
≤||r
ψj
‑
r
ψi
||≤l
max
(7)
[0026]其中，ψj和ψi表示任意两个相邻节点，l
min
和l
max
分别表示最大
‑
最小容许的形变。
[0027]在小天体中心固连坐标系下，根据如公式(1)至公式(4)所示柔性着陆器节点动力学方程与公式(5)(6)所示观测方程，并给定柔性体约束条件(7)，进而联立公式(1)至公式(7)构建柔性附着导航估计问题。在附着过程中，通过所述柔性附着导航估计问题估计柔性着陆器节点的状态。
[0028]定义状态向量和观测向量其中
[0029][0030]由于节点受到的推力用惯性测量元件测出，经过离散化，由公式(1)至公式(4)所示的动力学方程和由公式(5)(6)所示的观测方程表示为
[0031][0032]根据公式(10)至公式(20)求解不考虑柔性体约束条件的无约束估计值，实现方法如下：利用公式(10)选择2n+1个sigma点
[0033][0034]其中，n为状态向量的维数，λ＝α2(n+κ)
‑
n，κ＝3
‑
n，α决定本文档来自技高网...

【技术保护点】

【技术特征摘要】
1.不等式约束的非线性系统卡尔曼滤波方法，其特征在于：包括如下步骤，步骤1、在小天体中心固连坐标系下，构建柔性着陆器节点动力学方程与观测方程，并给定柔性体约束条件，进而构建柔性附着导航估计问题；根据无迹卡尔曼滤波器求解不考虑柔性体约束条件的无约束估计值，将柔性附着导航估计问题中约束估计值表示成关于无约束估计值的函数，并对节点状态的估计误差协方差矩阵进行更新；以无约束估计值为自变量，以约束估计值为因变量，定义通过修正无约束估计值解算得到约束估计值的修正函数，基于所述修正函数构建用于解算约束估计值的约束卡尔曼滤波器，通过约束卡尔曼滤波器对无约束估计值进行修正，得到柔性附着导航估计问题中的约束估计值；所述修正函数基于后续步骤2至4得到；步骤2、对柔性着陆器节点的状态空间进行坐标变换，引入缩放因子连接变换后的约束估计误差与无约束估计误差，便于后续步骤3通过分析缩放因子的大小对约束前后估计误差的关系进行分析；步骤3、利用节点状态估计误差协方差矩阵，分析缩放因子对于该误差协方差矩阵中各元素的影响，并分析缩放因子对误差协方差矩阵迹的影响，分析缩放因子对于柔性附着导航估计性能的影响；步骤4、根据凸约束区域下投影函数使得的特性，将步骤1定义的不考虑柔性体约束条件的无约束估计值的修正函数设计成投影函数，求解柔性体约束下节点状态可行域的凸包，将所述凸包作为约束卡尔曼滤波器的约束，通过约束卡尔曼滤波器对步骤1中解算出的无约束估计值进行修正，将无约束估计值投影到该凸包上，从而解算出步骤1中待解算的柔性附着导航估计问题中的约束估计值，提高柔性附着导航估计精度。2.如权利要求1所述的不等式约束的非线性系统卡尔曼滤波方法，其特征在于：步骤1实现方法为，柔性附着过程中，节点ψ(ψ＝1,2,3)在小天体中心固连坐标系下的动力学方程为其中，ω＝[ω
x
,ω
y
,ω
z
]
T
为小天体自转角速度矢量，g(r
ψ
)＝[g
ψx
,g
ψy
,g
ψz
]
T
为当地引力加速度矢量，T
ψ
＝[T
ψx
,T
ψy
,T
ψz
]
T
为推力矢量，为推力幅值，为柔性内力矢量，m
ψ
为节点ψ的质量；柔性内力由保守力项和耗散力项构成，分别使用三维弹簧项和阻尼项对保守力项和耗散力项进行近似；对于节点1，节点1受到的柔性内力表示为节点1受到的柔性内力表示为和分别表示为示为其中，和分别为节点12和节点13间弹簧项的
系数矩阵，和分别为节点12和节点13间阻尼项的系数矩阵；r
12
＝r2‑
r1，r
13
＝r3‑
r1，Δr
12
＝||r
12
||
‑
l0和Δr
13
＝||r
13
||
‑
l0分别为节点12和节点13间的形变，l0为节点间的初始距离，v
12
＝v2‑
v1，v
13
＝v3‑
v1，和分别为节点12和节点13间的相对速度；对于节点2，节点2受到的柔性内力表示为表示为其中，和分别为节点23和节点23间弹簧项和阻尼项的系数矩阵；r
23
＝r3‑
r2，Δr
23
＝||r
23
||
‑
l0为节点23间的形变，v
23
＝v3‑
v2，为节点23间的相对速度；对于节点3，节点3受到的柔性内力表示为在附着过程中，柔性着陆器节点观测量通过安装在节点上的光学相机和激光测距仪得到，因此节点ψ关于导航路标的观测方程表示为其中，f是焦距，p为像素大小，为第个导航路标在小天体固连坐标系的位置向量，和分别为带有噪声δu和δv的像素坐标，δd为激光测距仪的测量误差；像素坐标和表示为由于柔性材料的形变受到限制，附着过程中节点时刻满足柔性约束，表示为l
min
≤||r
ψj
‑
r
ψi
||≤l
max
(7)其中，ψj和ψi表示任意两个相邻节点，l
min
和l
max
分别表示最大
‑
最小容许的形变；在小天体中心固连坐标系下，根据如公式...

【专利技术属性】
技术研发人员：崔平远，陈泽龙，葛丹桐，朱圣英，崔世晟，
申请(专利权)人：北京理工大学，
类型：发明
国别省市：