Research on Coning Attitude Algorithm of Spinning Bodies

ZHANG Shuangbiao; LI Xingcheng

doi:10.15892/j.cnki.djzdxb.2020.04.002

Journal of Projectiles, Rockets, Missiles and Guidance >

2020 , Vol. 40 >Issue 4: 5 - 9

DOI: https://doi.org/10.15892/j.cnki.djzdxb.2020.04.002

Research on Coning Attitude Algorithm of Spinning Bodies

ZHANG Shuangbiao ¹ ,
LI Xingcheng ²

Expand

¹ Beijing Key Laboratory of High Dynamic Navigation Technology, Beijing Information Science and Technology University, Beijing 100101, China
² School of Aerospace Engineering, Beijing Institute of Technology, Beijing 100081, China

Received date: 2019-09-05

Online published: 2025-05-30

Fold

Abstract

Aiming at the divergence error of the conventional attitude algorithms caused by the biaxial cone motion of spinning bodies, a cone attitude calculation method is proposed, and the attitude calculation process based on the rotation vector is given. To compare with accuracy of algorithms, the attitude error and the half cone angle are chosen as main parameters, and the simulation results show that the accuracy of the cone attitude algorithm is higher than that of the Euler angles algorithm, the accuracy of the cone angle attitude based on the two-step attitude solution is higher than that of the Euler angles based on the two-step attitude solution, and the improvement of conventional optimization method is inconspicuous. The cone attitude algorithm is more suitable for the biaxial conical motion of spinning bodies.

Key words： spinning bodies; attitude algorithm; presentation method; algorithm error

Cite this article

ZHANG Shuangbiao , LI Xingcheng . Research on Coning Attitude Algorithm of Spinning Bodies[J]. Journal of Projectiles, Rockets, Missiles and Guidance, 2020 , 40(4) : 5 -9 . DOI: 10.15892/j.cnki.djzdxb.2020.04.002

0 引言

火箭弹、制导炮弹、制导子弹等旋转载体常采用的姿态解算方法是以欧拉角为基础的,如欧拉角姿态解算方法和基于旋转矢量的双步姿态解算方法,前者常用于控制系统设计,后者常用于定位、定向和导航系统,但两种方法均受锥形运动影响而出现不同程度的姿态解算误差^[1-2],因此姿态解算得到了国内外广大学者的关注。研究发现,旋转载体的锥形运动存在两种旋转方式,一种是通过绕3个正交轴旋转表征的形式,如图1所示;另一种是载体因定轴转动而发生的章动和进动表现的双轴旋转形式,如图2所示^[3-4]。两种锥形运动均会影响姿态解算方法,特别是自转姿态误差发散严重。为提高姿态解算的适应性和解算精度,广大学者主要采用便于参数优化的双步姿态解算方法,通过锥形运动条件优化旋转矢量的计算参数,抑制姿态误差,并利用旋转矢量计算姿态矩阵,得到载体姿态,而优化效果凭借误差漂移率体现^[5-9]。然而,目前该类姿态误差并未得到有效消除。

显示原图|下载原图ZIP|生成PPT

图1 三轴旋转表征的锥形运动

显示原图|下载原图ZIP|生成PPT

图2 两轴旋转表征的锥形运动

为此,提出圆锥姿态解算方法,并给出基于旋转矢量的双步姿态解算过程,通过与传统的解算方法进行仿真对比,证明该方法的优越性。

1 常规的姿态解算方法

1.1 欧拉角姿态解算方法

欧拉角姿态解算方法是传统的姿态解算方法,根据飞行力学知识,载体的角运动模型为^[10]:

(1)

$ω b x ω b y ω b z$ = $0 s i n ϑ 1 s i n γ c o s ϑ c o s γ 0 c o s γ - c o s ϑ s i n γ 0 ϑ · ψ · γ ·$

式中: ϑ、ψ、γ分别为俯仰角、偏航角和滚转角,

ψ ·

、

ϑ ·

、

γ ·

为对应的角速率;ω_b_x、ω_b_y、ω_b_z为固连在弹体坐标系上角速度测量值。对式(1)进行变形,可以得到欧拉角姿态解算方程:

(2)

$ϑ · ψ · γ ·$ = $0 s i n γ 1 0 c o s γ c o s ϑ s i n γ c o s ϑ 1 - t a n ϑ c o s γ t a n ϑ s i n γ ω b x ω b y ω b z$

利用获得的角速度测量值,可以解算载体姿态。由欧拉角表示的载体坐标系Ox_by_bz_b到参考坐标系Oxyz的姿态矩阵为:

(3)

$C b i$ = $C 11 C 12 C 13 C 21 C 22 C 23 C 31 C 32 C 33$

式中:上角标i表示目标坐标系为参考坐标系; C₁₁=cos(ϑ)cos(ψ);C₁₂=-sin(ϑ)cos(ψ)cos(γ)+sin(ψ)sin(γ);C₁₃=sin(ϑ)cos(ψ)sin(γ)+sin(ψ)·cos(γ);C₂₁=sin(ϑ);C₂₂=cos(ϑ)cos(γ);C₂₃=-cos(ϑ)sin(γ);C₃₁=-cos(ϑ)sin(ψ);C₃₂=sin(ϑ)sin(ψ)cos(γ)+cos(ψ)sin(γ);C₃₃=-sin(ϑ)·sin(ψ)sin(γ)+cos(ψ)cos(γ)。

在锥形运动环境下,利用俯仰角ϑ和偏航角ψ可以近似计算半锥角:

(4)

α= $ϑ 2 + ψ 2$

1.2 双步姿态解算方法

双步姿态解算方法包括旋转矢量更新和姿态矩阵更新两个过程,具体更新如下^[3]:

(5)

ϕ_l=α_l+δϕ_l (5)

(6)a_l=

∫ t - 1 t

ω·dt

(7)δϕ_l≈

∫ t - 1 t 12 ϕ l × ω × 112 ϕ l × (ϕ l × ω)

式中l表示第几次循环,为便于数据更新,式(7)常被近似为下式:

(8)δϕ_l=

∑ i = 1 N - 1

∑ j = i + 1 N

k_ijΔϕ_l(i)×Δϕ_l(j)

式中k_ij为旋转矢量优化参数。

姿态矩阵更新如下:

(9)

$C b, l i, l$ = $C b, l - 1 i, l$ · $C b, l b, l - 1$

(10)

C b, l b, l - 1

=I+f₁(ϕ_l)(ϕ_l×)+f₂(ϕ_l)(ϕ_l×)²

(11)

f₁(ϕ_l)= $s i n | ϕ l | | ϕ l |$ = $∑ k = 1$ (-1)^k^-1 $| ϕ l | 2 (k - 1) 2 k - 1)!$

(12)

f₂(ϕ_l)= $1 - c o s | ϕ l | | ϕ l | 2$ = $∑ k = 1$ (-1)^k^-1 $| ϕ l | 2 (k - 1) 2 k)!$

(13) $\boldsymbol{\phi}_{l} \times=\left[\begin{array}{ccc} 0 & -\phi_{z} & \phi_{y} \\ \phi_{z} & 0 & -\phi_{x} \\ -\phi_{y} & \phi_{x} & 0 \end{array}\right]$

利用旋转矢量解算欧拉角时,俯仰角ϑ、偏航角ψ和滚转角γ分别利用下式计算:

(14)

ϑ^

=arcsinC₂₁

(15)

$ψ^$ =-arctan $C 31 C 11$

(16)

$γ^$ =-arctan $C 23 C 22$

偏航角ψ和滚转角γ真值根据表1和表2确定。

表1 ψ取值

C₁₁	C₃₁	ψ真值
+	-	$ψ^$
-	-	$ψ^$ +π
-	+	$ψ^$ +π
+	+	$ψ^$ +2π

表2 γ取值

C₂₂	C₂₃	γ真值
+	-	$γ^$
-	-	$γ^$ +π
-	+	$γ^$ +π
+	+	$γ^$ +2π

2 圆锥姿态解算方法

圆锥姿态解算方法借鉴了章动原理,通过定义圆锥姿态角表述锥形运动,分别为进动角δ₁,章动角δ₂,自转角δ₃,其中δ₁取值范围为[0° 360°],δ₂取值范围为[0° 90°],δ₃取值范围为[0° 360°]^[8]。进动角表征锥形运动的旋转过程,章动角表征锥形运动的摇摆幅度,自转角则表征载体在进行锥形运动时自转过程,见图2。需要说明的是,当Ox_by_b面位于铅垂面内时为进动角δ₁起始位置。与章动原理不同的是,文中针对坐标系旋转过程进行了改进,具体为:

$\boldsymbol{a}_{l}=\int_{t-1}^{t} \boldsymbol{\omega} \cdot \mathrm{~d} t$

根据旋转顺序可以得到通过圆锥姿态角表示的参考坐标系Oxyz与载体坐标系Ox_by_bz_b的旋转关系,如图3所示。

显示原图|下载原图ZIP|生成PPT

图3 锥形运动姿态角的两轴旋转过程

根据图3建立旋转载体的角运动方程为:

(17)

$ω b x ω b y ω b z$ = $δ · 1 (c o s δ 2 - 1) + δ · 3 - δ · 1 c o s (δ 1 - δ 3) s i n δ 2 - δ · 2 s i n (δ 1 - δ 3) - δ · 1 s i n (δ 1 - δ 3) s i n δ 2 + δ · 2 c o s (δ 1 - δ 3)$

式中:

δ · 1

、

δ · 2

、

δ · 3

分别对应各自的角速率。对式(17)变形整理可得:

(18)

$δ · 1 δ · 2 δ · 3$ = $0 c o s (δ 1 - δ 3) s i n δ 2 s i n (δ 1 - δ 3) s i n δ 2 0 - s i n (δ 1 - δ 3) c o s (δ 1 - δ 3) 1 - c o s (δ 1 - δ 3) t a n δ 2 2 - s i n (δ 1 - δ 3) t a n δ 2 2 ω b x ω b y ω b z$

当δ₂不为零时,根据式(18)可以利用角速度测量值实时解算锥形运动的姿态角。根据欧拉姿态角与圆锥姿态角的几何关系建立如下表达式:

(19)

ϑ=±arcsin|sinδ₁sinδ₂|

(20)

ψ=±arcsin $s i n 2 δ 2 - s i n 2 ϑ c o s 2 ϑ$

二者的符号可以根据表3和表4选取。

表3 ϑ符号选取

δ₁	ϑ
[0° 180°]	+
(180° 360°]	-

表4 ψ符号选取

δ₁	ψ
[0° 90°)∪(270° 360°]	+
[90° 270°]	-

由式(18)可知,当δ₂为零时,存在奇异值的问题。为提高圆锥姿态适应性,同样可以利用双步姿态解算结构更新载体的圆锥姿态,见式(5)~式(10),载体坐标系Ox_by_bz_b到参考坐标系Oxyz的圆锥姿态矩阵可根据图3旋转关系确定,具体为:

(21)

$D i b$ = $D 11 D 12 D 13 D 21 D 22 D 23 D 31 D 32 D 33$

式中: D₁₁=cosδ₂;D₁₂=-cos(δ₁-δ₃)sinδ₂; D₁₃=-sin(δ₁-δ₃)sinδ₂;D₂₁=cosδ₁sinδ₂;D₂₂=sin(δ₁-δ₃)sinδ₁+cos(δ₁-δ₃)cosδ₁cosδ₂;D₂₃=-cos(δ₁-δ₃)sinδ₁+sin(δ₁-δ₃)cosδ₁cosδ₂;D₃₁=sinδ₁sinδ₂;D₃₂=-sin(δ₁-δ₃)cosδ₁+cos(δ₁-δ₃)sinδ₁cosδ₂;D₃₃=cos(δ₁-δ₃)cosδ₁+sin(δ₁-δ₃)sinδ₁cosδ₂。

根据式(21),可以建立圆锥姿态计算公式:

(22)

$δ^1$ =arctan $D 21 D 31$

(23)

δ₂=arccosD₁₁

(24)

δ₁- $δ^3$ =-arctan $D 12 D 13$

需要说明的是,自转角δ₃是需要确定进动角δ₁后再计算。进动角δ₁和自转角δ₃的真值可根据表5和表6所示确定。

表5 δ₁取值

D₂₁	D₃₁	δ₁真值
+	+	$δ^$
-	-	$δ^1$ +π
-	+	$δ^1$ +π
+	-	$δ^1$ +2π

表6 δ₃取值

D₁₂	D₁₃	δ₃真值
-	+	δ₁+ $δ^3$
+	+	δ₁+( $δ^3$ +π)
+	-	δ₁+( $δ^3$ +π)
-	-	δ₁+( $δ^3$ +2π)

3 仿真与分析

以两轴旋转的锥形运动作为仿真条件,α(0)=0.087 rad,Ω=π rad/s,

α ·

=0.000 2 rad/s,ω=4π rad/s,仿真时间为60 s,利用式(17)~式(20)生成角速度、圆锥姿态和欧拉角姿态数据。图4为角速度测量值。旋转矢量计算采用常规的抛物线拟合三子样优化,优化前系数为k₁= 33/80、k₂= 57/80;优化后系数为k₁=0.45、k₂=0.675。

显示原图|下载原图ZIP|生成PPT

图4 角速度测量值

首先,将欧拉角姿态解算方法与双步姿态解算方法进行对比。图5为旋转矢量方法优化后欧拉角姿态误差,姿态角存在振荡式发散误差,误差量级为10^-5~10^-4rad。图6为旋转矢量方法优化前后的欧拉角姿态误差对比,误差表现为复杂的发散现象,量级为10^-14~10^-13rad。结合图5和图6,说明双轴旋转锥形运动引起的姿态发散振荡误差,在旋转矢量优化后仍然存在,优化效果并不明显,同时说明利用双步姿态解算方法得到欧拉角存在误差。

显示原图|下载原图ZIP|生成PPT

图5 旋转矢量方法优化后欧拉角姿态误差

显示原图|下载原图ZIP|生成PPT

图6 旋转矢量方法优化前后的欧拉角姿态误差对比

然后,将圆锥姿态解算方法与双步姿态解算方法进行对比。图7为旋转矢量方法优化后圆锥姿态误差,可见仍存在振荡式误差,但并未发散,章动角δ₂误差量级为10^-5rad,自转角δ₃的误差量级为10^-6rad,精度均高于欧拉角姿态。图8为旋转矢量方法优化前后圆锥姿态误差对比,优化前后误差并未发散,误差量级为10^-4~10^-3rad,且自转角δ₃精度未得到提高。分析图7和图8可知,旋转矢量方法的优化与否对圆锥姿态自转角的影响并不明显,同时也证明双步姿态解算方法存在误差。

显示原图|下载原图ZIP|生成PPT

图7 圆锥姿态解算方法与旋转矢量对比

显示原图|下载原图ZIP|生成PPT

图8 旋转矢量方法优化前后的圆锥姿态误差对比

最后,分别利用基于欧拉角姿态的解算方法与基于圆锥姿态的解算方法计算半锥角α,并进行对比。如图9所示,欧拉角姿态计算存在发散的误差,圆锥姿态解算方法无误差,但其双步姿态解算方法存在不发散的振荡误差,具体误差关系为:

$\Delta \alpha_{\text {圆锥案态解算 }}<\Delta \alpha_{\text {除拉角姿态解算 }}<\Delta \alpha_{\text {双步 (圆锥客态) }}<\Delta \alpha_{\text {双步 (欧拉角) }}$

显示原图|下载原图ZIP|生成PPT

图9 半锥角误差

4 结论

考虑了锥形运动存在的不同形式,提出了一种圆锥姿态解算方法,同时为提高该方法的适应性,给出了基于旋转矢量的解算过程,通过与传统姿态解算方法对比发现:

1)相比传统方法,圆锥姿态解算方法的精度高于欧拉角姿态解算方法。

2)基于双步姿态解算的圆锥姿态解算精度高于基于双步姿态解算的欧拉角姿态解算,且常规优化的旋转矢量方法效果并不明显。

文中提出的圆锥姿态解算方法更适合旋转载体的双轴锥形运动条件,能够为旋转载体的复杂角运动解耦和姿态诱导误差抑制的研究工作提供理论参考。未来工作将进一步研究双步姿态解算方法引起姿态出现发散的振荡误差的机理,以及相应的误差抑制方法。

References

Publishing order | Descend order by publishing year | Descend order by cited within

[1]	高丽珍, 张晓明, 李杰. 旋转制导弹药姿态测试技术研究现状分析[J]. 兵器装备工程学报, 2018, 39(12):73-77.

[2]	尚剑宇, 邓志红, 付梦印, 等. 制导炮弹转速测量技术研究进展与展望[J]. 自动化学报, 2016, 42(11):1620-1629.

[3]	ZHANG S B, LI X C, SU Z. Cone algorithm of spinning vehicles under dynamic coning environment[J]. International Journal of Aerospace Engineering, 2015:1-11.

[4]	ZHANG S B, LI X C, SU Z. Measuring and solving real coning motion of spinning carriers[J]. Proceedings of the Institution of Mechanical Engineers, Part G:Journal of Aerospace Engineering, 2016: 230(13):2369-2378.

[5]	祝燕华, 刘建业, 曾庆化, 等. 双频圆锥运动及旋转矢量的改进算法[J]. 南京航空航天大学学报, 2008, 40(5):660-664.

[6]	严恭敏, 杨小康, 翁浚, 等. 捷联惯导中求解圆锥误差系数的通用算法[J]. 导航定位学报, 2017, 5(3): 1-4.

[7]	王真, 高凤岐, 高敏, 等. 旋转矢量多迭代捷联姿态解算误差补偿算法[J]. 中国测试, 2016, 42(8):113-117.

[8]	WANG M S, WU W Q, HE X F, et al. Higher-order rotation vector attitude updating algorithm[J]. The Journal of Navigation, 2019: 72(3):721-740.

[9]	TANG C Y, CHEN L, CHEN J F. Efficient coning algorithm design from a bilateral structure[J]. Aerospace Science and Technology, 2018, 79:48-57.

[10]	钱杏芳, 林瑞雄, 赵亚男. 导弹飞行力学[M]. 北京: 北京理工大学出版社, 2011.

Options

Outlines

模态框（Modal）标题

Abstract

Cite this article

0 引言

图1 三轴旋转表征的锥形运动

图2 两轴旋转表征的锥形运动

1 常规的姿态解算方法

1.1 欧拉角姿态解算方法

1.2 双步姿态解算方法

表1 ψ取值

表2 γ取值

2 圆锥姿态解算方法

图3 锥形运动姿态角的两轴旋转过程

表3 ϑ符号选取

表4 ψ符号选取

表5 δ1取值

表6 δ3取值

3 仿真与分析

图4 角速度测量值

图5 旋转矢量方法优化后欧拉角姿态误差

图6 旋转矢量方法优化前后的欧拉角姿态误差对比

图7 圆锥姿态解算方法与旋转矢量对比

图8 旋转矢量方法优化前后的圆锥姿态误差对比

图9 半锥角误差

4 结论

References

表5 δ₁取值

表6 δ₃取值