中国科技核心期刊      中国指挥与控制学会会刊     军事装备类重点期刊
Command Control and Simulation

Command Control and Simulation >

2021 , Vol. 43 >Issue 3: 29 - 33

DOI: https://doi.org/10.3969/j.issn.1673-3819.2021.03.006

Theory & Research

The Effect of Particle Weights on the Accuracy of Multi-information Fusion Attitude Estimation Algorithms

  • PU Hui 1 ,
  • XIN Chang-fan 1 ,
  • WANG Rui-hua 2 ,
  • CHEN Ming 2
Expand
  • 1. North University of China, Taiyuan 030051
  • 2. JinXi Industries Group Co.,Ltd, Taiyuan 030051, China

Received date: 2020-11-24

  Revised date: 2021-02-28

  Online published: 2021-06-10

Fold

Abstract

In this paper, a new multi-information fusion algorithm for vectorial inertial tensor attitude estimation is first introduced. The algorithm places a particle at the top of each reference vector and associated observation vector, assigns weight to the particle and computes the inertial tensor of the particle system; diagonalizes each inertial tensor to obtain the eigenvalue, eigenvector, and transformation matrix between the principal axis coordinate system and each coordinate system; by finding the transformation of the respective principal axis coordinate system, the directional cosine matrix between the vector coordinate system and the inertial coordinate system can be computed. In this paper, we study how to assign prime weights to an attitude estimation algorithm for multi-sensor information fusion. In this paper, we study how to assign prime weights to an attitude estimation algorithm for multi-sensor information fusion. Through the analysis, it is found that when the observation errors of sensors are close to each other, the corresponding prime points should be assigned the same weight; while when the observation errors are different, the observation vector with high accuracy should occupy a larger assigned weight in order to get the best attitude estimation. The results of the study are valuable for the attitude estimation problem.

Key words： multi-information fusion; attitude estimation; weight assign

Cite this article

PU Hui , XIN Chang-fan , WANG Rui-hua , CHEN Ming . The Effect of Particle Weights on the Accuracy of Multi-information Fusion Attitude Estimation Algorithms[J]. Command Control and Simulation, 2021 , 43(3) : 29 -33 . DOI: 10.3969/j.issn.1673-3819.2021.03.006

对于空间飞行器姿态的确定,现在已经开发出了多种根据矢量观测的方法。最早和最简单的方法是TRIAD算法,最初由Black创建[1],Bar-Itzhack和Harman提出了TRIAD算法的最优解[2],后来被Shuster证实[3]。然而,TRIAD算法利用两个非平行矢量确定载体的三轴姿态,且存在对主矢量敏感,不能利用第3根矢量的测量信息等缺点,因此,它的准确性是有限的,不能利用额外的观测矢量来减少总姿态估计误差。黎湧提出一种改进的TRIAD算法,克服了主矢量敏感的问题[4],陈浩为了提高飞行器姿态测量精度和简化姿态确定的复杂性,提出了一种基于加权矢量和的TRIAD飞行器姿态确定算法[5]。
Wahba提出了使用任意数量的观测向量的一般姿态估计问题[6],解决Wahba问题的算法分为两类,第一类是求解表示姿态矩阵的四元数,Davenport为求解飞行器姿态提出了q方法[7],夏家和运用q方法有效解决了捷联惯导的动态粗对准问题[8],高薪验证了四元数估计对准算法姿态角误差的收敛速度优于双矢量定姿对准算法[9],徐晓苏针对直接构建量测模型导致收敛速度慢的问题,提出一种基于最优四元数估计法构造K矩阵原理的改进算法[10],谭彩铭基于几何原理提出了在给定不完整双矢量观测信息时的最简姿态确定算法[11];第二类是直接求解姿态矩阵,Farrell和Stuelpnagel提出了奇异值分解SVD算法[12],何颖应用奇异值分解导数的计算公式提出一种在变量投影框架下的多点透视问题求解算法[13],严恭敏针对姿态确定的Wahba问题,证明了姿态矩阵的奇异值分解SVD算法与三种四元数特征向量求解QUEST算法之间的等价性,给出了一种快速奇异值分解算法[14]。
VITAE(Vector Inertia Tensor Attitude Estimation,矢量惯性张量姿态估计)是一种新的姿态计算方法,由Russell于2018年提出[15]。该方法类似于TRIAD方法,但是与TRIAD不同的是,VITAE可以使用任何数量的参考矢量和观测矢量,并使用各自惯性主轴来定义相关的正交坐标系。
本文介绍了基于VITAE算法的多信息融合姿态估计过程与误差分析,使用Matlab对该算法编程,通过计算相同和不同观测精度下不同的质点权重分配对飞行器姿态估计误差的影响,研究了如何给多传感器信息融合的姿态估计算法进行质点权重分配。

1 多信息融合姿态估计算法及其误差分析

1.1 姿态计算

多信息融合姿态估计算法的输出是载体坐标系到惯性坐标系的变换矩阵。首先,在每个参考矢量和相关观测矢量的顶端放置一个质点,并计算出此质点系的惯性张量。这样,观测矢量中包含的信息就转移到了惯性张量中。在观测矢量无误差的情况下,载体坐标系惯性张量的特征值与惯性坐标系惯性张量的特征值相同。对每个惯性张量对角化可得到特征值、特征向量和主轴坐标系与各坐标系之间的变换矩阵。通过找到各自主轴坐标系的变换,就可以计算出载体坐标系与惯性坐标系之间的方向余弦矩阵,从而得到姿态估计值。
分配给每个矢量的质点的权重可以根据其相关的测量误差进行调整,以获得更精确的姿态估计值。由于惯性张量是对称的,有两个或三个相同的特征值的情况下,特征向量是退化的,不能定义一个唯一的正交坐标系。在这些情况下,可以调整质点的权重来消除退化性,恢复姿态估计的准确性。如果在载体坐标系中得到的观测矢量惯性的特征值与惯性坐标系中得到的参考矢量惯性的相关特征值非常接近,则姿态估计可以非常准确。
令 V →为一个参考单位矢量,m是位于 V →顶端的质点,同样, W →是一个相应的观测单位矢量,质点m位于其顶端。定义C为
C= V → V → T
惯性张量I,由 V →与其质点m的关系可知
I=m[tr(C)(Id)-C]
其中,Id是3*3的单位矩阵。对于一组参考矢量,惯性坐标系上的总惯性张量II是所有参考矢量及对应质点的惯性张量之和。
II= ∑ 1 Nmi[tr(Ci)Id-Ci]
用观测矢量 W → i和相应质点也可定义一个类似的惯性张量,得到载体坐标系上的总惯性张量Ib。
由式(3)确定的惯性张量是一个实对称的3*3矩阵,可以很容易地进行对角化,得到其特征值和特征向量。特征方程为
IXi=λiXi
其中Xi和λi分别为第i个特征向量和特征值。将方程(4)转换到主轴坐标系,得
I x 0 0 0 I y 0 0 0 I z X 1 X 2 X 3= λ 1 X 1 λ 2 X 2 λ 3 X 3
其中,Ix、Iy、Iz为沿各自主轴的惯性矩,λ1、λ2、λ3为各自的特征值。令UI为从主轴坐标系到惯性坐标系的变换,令Ub为从主轴坐标系到载体坐标系的变换,即
Xb=UbXb XI=UIXd
由于Ub将主轴坐标系中的特征向量转换为载体坐标系,所以Ub的每一列是载体坐标系中的特征向量。同样,UI的每一列也是惯性坐标系中的特征向量。因此,将载体坐标系中的惯性张量和惯性坐标系中的惯性张量对角化,就可以得到各自的特征向量和变换矩阵Ub和UI。
消除式(6)中的Xd,得到惯性坐标系和载体坐标系之间的变换,即
Xb=(Ub U I - 1)XI=AXI
因此,载体姿态的估计值A为
A=Ub U I - 1
如果参考矢量和观测矢量没有误差,则由式(8)给出的载体姿态估计可以得到精确的解。在实际工作中,参考矢量与观测矢量的误差相比,其误差可以忽略不计。因此,UI可以准确计算,姿态估计误差仅由观测矢量的误差引起。总之,一旦利用向量和权重计算出各自的惯性张量,就通过对各自惯性张量的对角线化找到特征向量。然后用特征向量构造各自的变换矩阵,用来计算从惯性坐标系到载体坐标系的变换。

1.2 误差分析

姿态估计精度取决于观测矢量的精度和惯性张量的各特征值之间的差异。设 W → i为观测矢量,由式(1)中的相关参数Ci计算为
Ci= W → i W → i T
当 W → i有误差时,Ci为
Ci= W → i W → i T+Δ W → i W → i T+ W → iΔ W → i T=C0+Δ W → i W → i T+ W → iΔ W → i T
其中,Δ W → i为 W → i中的误差,C0为无误差时C的值。每个观测矢量的Ci中的误差通过式(2)和式(3)传播到总惯性张量I中。
I=I0+Q
其中
Q=- ∑ imi(Δ W → i W → i T+ W → iΔ W → i T)
在式(11)中,I0为观测矢量无误差时的惯性张量,Q为观测矢量有误差时I的误差。
将一阶扰动理论应用于式(4)中的特征值问题,研究Q对载体坐标系惯性张量相关的特征值、特征向量和变换矩阵的影响。若观测向量的误差很小,则无误差特征值问题的解如下
I0X0i=λ0iX0i
在式(13)中,与惯性张量相关的特征值只有三个,而相关的特征向量构成了一个完整的正交向量集。
当存在误差时,则特征方程为
IXi=λiXi
其中I由式(11)给出,λi由式(15)给出,Xi由式(16)给出:
λi=λ0i+λ1i
Xi=X0i+ ∑ mamiX0m
在式(15)和(16)中只保留了一阶误差项,λ1i为特征值的一阶误差。由于式(13)中的未扰动特征向量形成了一个完整的正交集合,所以Xi的一阶误差是式(16)中的求和项,其中ami为系数。因此,公式(16)为一阶近似解和由此产生的姿态的最小二乘最优估计值。将公式(11)、(15)和(16)代入(14),并去掉由(13)给出的零阶项,可以发现:
QX0i+ ∑ mamiX0mλ0m=λ1iX0i+λ0i ∑ mamiX0m
将式(17)乘以X0i,并利用特征向量的正交性,得到特征值的一阶误差为
λ1i= X T 0 iQX0i
这个结果可用来计算姿态估计的置信度,因为特征值的误差很小,则Q也很小。式(17)中的系数ami,可以通过将式(18)乘以X0m来求得,其结果为
ami= X T 0 m Q X 0 i λ 0 i - λ 0 m
将式(19)代入式(16)中,可得到特征向量的一阶近似值。如果式(19)中的特征值相互接近,则可能违背一阶近似的假设,因为ami与特征值的差值成反比。因此Q必须远小于特征值的差值,才能使特征向量的一阶误差较小。总之,观测向量的误差会影响惯性张量,从而影响特征向量。由于特征向量形成了一个正交集合,所以式(16)中的一阶扰动误差相当于一个微小转动,称为姿态误差向量,即
Xi= 1 a 12 a 13 a 21 1 a 23 a 31 a 32 1 X 01 X 02 X 03= 1 Δ θ Z - Δ θ Y - Δ θ Z 1 Δ θ X Δ θ Y - Δ θ X 1 X 01 X 02 X 03
因此,姿态误差矢量的转动角可用系数aij来定义,如式(21)所示。
Δθi= Δ θ X Δ θ Y Δ θ Z= - a 32 a 31 - a 21= a 23 - a 13 a 12
定义姿态协方差矩阵Pθθ为姿态误差矢量的协方差矩阵,如式(22)所示,其中E表示期望值。
Pθθ=E{ΔθΔθT}= a 23 2 - a 32 a 31 a 32 a 21 - a 32 a 31 a 31 2 - a 31 a 21 a 21 a 32 - a 21 a 31 a 21 2
由于协方差矩阵中的每个项都涉及特征值之差,如式(19)所示,如果特征值在数值上非常接近,估计精度就会下降。对于固定的观测向量,可以调整各质点的权重来增加特征值的差异,从而提高姿态估计精度。当然,调整质量后的姿态估计将不再是最优的,但是,数值结果表明,姿态估计精度的损失非常小。下面将通过数值仿真研究各质点的权重对姿态估计精度的影响。

2 质量分配对估计误差影响的仿真研究

设惯性系下的两个参考矢量分别为:
r1=(1,0,0)T;r2=(0,1,0)T
对应的观测矢量的均值为0,方差分别为:
σ1=1e-2rad;σ2=1e-2rad
观测向量 W → i与参考向量 V → i和测量误差ηi的关系见式(25):
W → i=A V → i+ηi
其中,ηi为零均值高斯噪声,A为由式(26)给出的惯性坐标系到载体坐标系的变换矩阵。
A= 0.117 0.985 - 0.133 0.633 0.174 0.754 0.766 0 0.642
由数据可知,两个观测向量具有相同的测量误差σ1、σ2,为了得出质量的权重m1、m2对姿态误差的影响,首先研究m1=0~1,m2=1-m1时特征值的变化,如图1所示,特征值随着 W 1 →和 W 2 →之间各自的权重m1和m2的变化而变化,当m1=m2时,λ1=λ2,这时最大特征值轴的方向是不确定的,见图1。
图1 质点权重m1对特征值的影响
进一步研究质点权重对姿态估计误差的影响,改变m1及m2,计算姿态估计值及姿态估计误差,同时进行10 000次蒙特卡洛模拟仿真计算,通过计算出姿态协方差矩阵Pθθ,计算流程图如图2所示,得到姿态估计误差的变化如图3所示。
图3 σ1=σ2时,质点权重m1对估计误差的影响
在图3中,给两个观测向量分配权重以获得最优解,当m1接近0时,估计误差约为1.13×10-2 rad;m1=m2时,估计误差约为7.89×10-3 rad;m1接近1时,估计误差约为8.89×10-3 rad。由此可知,在测量精度相同时,两个观测矢量质点分配相同的权重可以得到最准确的测量误差,得到最优解。
若σ1<σ2,令σ1=1×10-4 rad,σ2=1×10-2 rad,再进行仿真计算,得到在σ1<σ2时,质点权重m1对估计误差的影响,如图4所示,当m1接近0时,估计误差约为2.37×10-3 rad;m1=m2时,估计误差约为3.7×10-3 rad;m1接近1时,估计误差约为8.1×10-5 rad。
图4 σ1=1×10-4 rad,σ2=1×10-2 rad时, m1对估计误差的影响
令σ1=1×10-4 rad,σ2=1×10-3 rad,再进行仿真计算,得到质点权重m1对估计误差的影响,如图5所示,估计误差较图4中更小,当m1接近0时,估计误差约为1.7×10-3 rad;m1=m2时,估计误差约为7.66×10-4 rad;m1接近1时,估计误差约为3.79×10-5 rad。
图5 σ1=1×10-4 rad,σ2=1×10-3 rad时, m1对估计误差的影响
由此可知,在测量精度不同时,给测量精度高的观测矢量分配更高的权重能够显著提高估计误差精度,得到更优的姿态估计。

3 结束语

本文通过对多信息融合姿态估计算法中的质点权重分配进行研究,得出如下结论:在多信息融合姿态估计算法中,两个观测矢量的精度相近时,质点应分配相同的权重,而两个观测矢量的精度不同时,精度高的观测矢量应分配更高的权重,这样才能得到最优的姿态估计。此结果对姿态估计的精度分析具有一定的价值。

References

Publishing order | Descend order by publishing year | Descend order by cited within
[1]
Black H.D. A Passive System for Determining the Attitude of a Satellite[J]. AIAA Journal, 1964, 2(7):1350-1351.

DOI

[2]
Bar-Itzhack I.Y, Harman R.R, . Optimized TRIAD Algorithm for Attitude Determination[J]. Journal of Guidance Control and Dynamics, 1997, 20(1):208-211.

DOI

[3]
Shuster M D. The Optimization of TRIAD[J]. Journal of the Astronautical Sciences, 2007, 55(2):245-257.

DOI

[4]
黎湧, 吴宏鑫, 刘良栋. 融合TRIAD算法用于GPS姿态确定[J]. 中国空间科学技术, 2000, 20(2):33-39.

[5]
陈浩, 谭久彬. 一种改进的TRIAD飞行器姿态确定方法[J]. 系统仿真学报, 2008, 20(8):2101-2104.

[6]
Wahba G. A Least-squares Estimate of Satellite Attitude[J]. SIAM Rev, 1965, 7(3):409.

[7]
Wertz J R. Three-Axis Attitude Determination Methods[M]. Dordrecht: Reidel Publishing Company, 1978.

[8]
夏家和, 秦永元, 游金川. 捷联惯导动态粗对准算法研究与仿真[J]. 系统仿真学报, 2010, 22(4):967-970.

[9]
高薪, 卞鸿巍, 傅中泽, 等. 捷联惯导晃动基座四元数估计对准算法[J]. 中国惯性技术学报, 2014, 22(6):724-727,740.

[10]
徐晓苏, 周峰, 张涛, 等. 基于四元数自适应卡尔曼滤波的快速对准算法[J]. 中国惯性技术学报, 2016, 24(4):454-459.

[11]
谭彩铭, 高翔, 徐国政, 等. 基于不完整双矢量观测的三轴姿态确定算法[J]. 仪器仪表学报, 2018, 39(11):140-146.

[12]
Farrell J L, Stuelpnagel J C, Wessner R H, et al. A Least Squares Estimate of Satellite Attitude (Grace Wahba)[J]. Siam Review, 1966, 8(3):384-386.

DOI

[13]
何颖, 马戎, 李岁劳, 等. 变量投影框架下基于Wahba问题的多点透视问题求解算法[J]. 光学学报, 2018, 38(11):250-256.

[14]
严恭敏, 陈若彤, 郭鹍. 多矢量定姿的SVD和QUEST算法等价性分析[J]. 中国惯性技术学报, 2019, 27(5):568-572.

[15]
Russell P. Patera. Attitude Estimation Based on Observation Vector Inertia[J]. Advances in Space Research: The Official Journal of the Committee on Space Research (COSPAR), 2018, 62(2):383-397.

DOI

Options
Outlines

/

〈 〉