CN102506869B - 基于角速度的欧拉角多项式类指数近似输出方法 - Google Patents

Abstract

本发明公开了一种基于角速度的欧拉角多项式类指数近似输出方法，用于解决现有的飞行器机动飞行时欧拉角输出精度差的技术问题。技术方案是通过引入多个参数并采用可以描述任意有限的统一多项式对滚转、俯仰、偏航角速度p，q，r进行近似逼近描述，可以根据工程精度的要求，可以事先定义常数矩阵M，减少对滚转、俯仰、偏航角速度p，q，r多项式描述的阶次，按照依次求解俯仰角、滚转角、偏航角，直接对欧拉角的表达式进行高阶逼近积分，使得欧拉角的求解按照超线性逼近，保证了确定欧拉角的时间更新迭代计算精度，从而提高了惯性设备输出飞行姿态的准确性。

Description

基于角速度的欧拉角多项式类指数近似输出方法

技术领域

本发明涉及一种飞行器机动飞行姿态确定方法，特别是涉及一种基于角速度的欧拉角多项式类指数近似输出方法。 

背景技术

惯性设备在运动体导航和控制中具有重要作用；刚体运动的加速度、角速度和姿态等通常都依赖于惯性设备输出，因此提高惯性设备的输出精度具有明确的实际意义；在惯性设备中，加速度采用加速度计、角速度采用角速率陀螺直接测量方式，刚体的姿态精度要求很高时如飞行试验等采用姿态陀螺测量，但在很多应用领域都有角速度等测量直接解算输出；主要原因是由于动态姿态传感器价格昂贵、体积大，导致很多飞行器采用角速率陀螺等解算三个欧拉角，使得姿态时间更新输出成为导航等核心内容，也使其成为影响惯导系统精度的主要因素之一，因此设计和采用合理的姿态时间更新输出方法就成为研究的热点课题；从公开发表的文献中对姿态输出主要基于角速度采用欧拉方程直接近似法或采用近似龙格库塔方法解算(孙丽、秦永元，捷联惯导系统姿态算法比较，中国惯性技术学报，2006，Vol.14(3)：6-10；Pu Li，Wang TianMiao，Liang JianHong，Wang Song，An Attitude Estimate Approach using MEMS Sensors for Small UAVs，2006，IEEE International Conference on Industrial Informatics，1113-1117)；由于欧拉方程中三个欧拉角互相耦合，属于非线性微分方程，在不同初始条件和不同飞行状态下的误差范围不同，难以保证实际工程要求的精度。 

发明内容

为了克服现有的飞行器机动飞行时欧拉角输出精度差的问题，本发明提供一种基于角速度的欧拉角多项式类指数近似输出方法。该方法通过引入多个参数并采用可以描述任意有限的统一多项式对滚转、俯仰、偏航角速度p，q，r进行近似逼近描述，通过按照依次求解俯仰角、滚转角、偏航角，直接对欧拉角的表达式进行高阶逼近积分，使得欧拉角的求解按照超线性逼近，从而可以保证确定欧拉角的时间更新迭代计算精度和惯性单元的输出精度。 

本发明解决其技术问题所采用的技术方案是：一种基于角速度的欧拉角多项式类指数近似输出方法，其特点是包括以下步骤： 

1、(a)根据欧拉方程： 

式中：  ψ分别指滚转、俯仰、偏航角；p，q，r分别为滚转、俯仰、偏航角速度；全文参数定义相同；这三个欧拉角的计算按照依次求解俯仰角、滚转角、偏航角的步骤进行；滚转、俯仰、偏航角速度p，q，r的展开式分别为 

p(t)＝pMξ，q(t)＝qMξ，r(t)＝rMξ 

其中 

p＝[p₀ p₁ L p_n-1 p_n]q＝[q₀ q₁ L q_n-1 q_n] 

r＝[r₀ r₁ L r_n-1 r_n]ξ＝[1 t L t^n-1 tⁿ]^T

M为事先定义的常数矩阵；定义T为采样周期，全文符号定义相同； 

(b)俯仰角的时间更新求解式为： 

式中： 

$a_1={\left(\mathrm{qMH\ξ}{|}_{\mathrm{kT}}^{(k+1)T}\right)}^2+{\left(\mathrm{rMH\ξ}{|}_{\mathrm{kT}}^{(k+1)T}\right)}^2-{\left(\mathrm{pMH\ξ}{|}_{\mathrm{kT}}^{(k+1)T}\right)}^2$

$a_2=\mathrm{pM\Ω}\left(t\right){|}_{\mathrm{kT}}^{(k+1)T}{H}^T{M}^T{r}^T-\mathrm{pMH\ξ}{|}_{\mathrm{kT}}^{(k+1)T}\mathrm{rMH\ξ}{|}_{\mathrm{kT}}$

$a_3=\mathrm{pM\Ω}\left(t\right){|}_{\mathrm{kT}}^{(k+1)T}{H}^T{M}^T{q}^T-\mathrm{pMH\ξ}{|}_{\mathrm{kT}}^{(k+1)T}\mathrm{qMH\ξ}{|}_{\mathrm{kT}}$

$\left|\mathrm{\λ}\right|=\{\mathrm{pM\Ω}\left(t\right){|}_{\mathrm{kT}}^{(k+1)T}{H}^T{M}^T{p}^T-\mathrm{pMH\ξ}{|}_{\mathrm{kT}}^{(k+1)T}\mathrm{pMH\ξ}{|}_{\mathrm{kT}}$

$+\mathrm{qM\Ω}\left(t\right){|}_{\mathrm{kT}}^{(k+1)T}{H}^T{M}^T{q}^T-\mathrm{qMH\ξ}{|}_{\mathrm{kT}}^{(k+1)T}\mathrm{qMH\ξ}{|}_{\mathrm{kT}}$

$+\mathrm{rM\Ω}\left(t\right){|}_{\mathrm{kT}}^{(k+1)T}{H}^T{M}^T{r}^T-\mathrm{rMH\ξ}{|}_{\mathrm{kT}}^{(k+1)T}\mathrm{rMH\ξ}{|}_{\mathrm{kT}}{\}}^{\frac{1}{2}}$

$H=\mathrm{diag}\{1,\frac{1}{2},\frac{1}{3},L,\frac{1}{n},\frac{1}{n+1}\};$

$\mathrm{\Ω}\left(t\right)=\left[\begin{array}{ccccc}\frac{1}{2}{t}^2& \frac{1}{3}{t}^3& L& \frac{1}{n+1}{t}^{n+1}& \frac{1}{n+2}{t}^{n+2}\\ \frac{1}{3}{t}^3& \frac{1}{4}{t}^4& L& \frac{1}{n+2}{t}^{n+2}& \frac{1}{n+3}{t}^{n+3}\\ M& M& O& M& M\\ \frac{1}{n+1}{t}^{n+1}& \frac{1}{n+2}{t}^{n+2}& L& \frac{1}{2n}{t}^{2n}& \frac{1}{2n+1}{t}^{2n+1}\\ \frac{1}{n+2}{t}^{n+2}& \frac{1}{n+3}{t}^{n+3}& L& \frac{1}{2n+1}{t}^{2n+1}& \frac{1}{2n+2}{t}^{2n+2}\end{array}\right]$

2、在已知俯仰角的情况下，滚转角的时间更新求解式为： 

其中 

$a_4={\left(\mathrm{pMH\ξ}{|}_{\mathrm{kT}}^{(k+1)T}\right)}^2+{\left(\mathrm{rMH\ξ}{|}_{\mathrm{kT}}^{(k+1)T}\right)}^2-{\left(\mathrm{qMH\ξ}{|}_{\mathrm{kT}}^{(k+1)T}\right)}^2$

$a_5=\mathrm{qM\Ω}\left(t\right){|}_{\mathrm{kT}}^{(k+1)T}{H}^T{M}^T{p}^T-\mathrm{qMH\ξ}{|}_{\mathrm{kT}}^{(k+1)T}\mathrm{pMH\ξ}{|}_{\mathrm{kT}}$

$a_6=\mathrm{qM\Ω}\left(t\right){|}_{\mathrm{kT}}^{(k+1)T}{H}^T{M}^T{r}^T-\mathrm{qMH\ξ}{|}_{\mathrm{kT}}^{(k+1)T}\mathrm{rMH\ξ}{|}_{\mathrm{kT}}$

3、在俯仰角、滚转角已知情况下，偏航角的求解式为： 

$\mathrm{\ψ}\left(t\right)=\mathrm{\ψ}\left(\mathrm{kT}\right)+{\∫}_{\mathrm{kT}}^t[b_1\left(t\right)+b_2\left(t\right)]\mathrm{dt}$

式中： 

本发明的有益效果是：由于通过引入多个参数并采用可以描述任意有限的统一多项式对滚转、俯仰、偏航角速度p，q，r进行近似逼近描述，通过按照依次求解俯仰角、滚转角、偏航角，直接对欧拉角的表达式进行高阶逼近积分，使得欧拉角的求解按照超线性逼近，从而保证了确定欧拉角的时间更新迭代计算精度和惯性单元的输出精度。 

下面结合具体实施方式对本发明作详细说明。 

具体实施方式

1、(a)根据刚体姿态方程(欧拉方程)： 

其中：  ψ分别指滚转、俯仰、偏航角；p，q，r分别为滚转、俯仰、偏航角速度；全文参数定义相同；这三个欧拉角的计算按照依次求解俯仰角、滚转角、偏航角的步骤进行；滚转、俯仰、偏航角速度p，q，r的展开式分别为 

p(t)＝pMξ，q(t)＝qMξ，r(t)＝rMξ 

其中 

p＝[p₀ p₁ L p_n-1 p_n]q＝[q₀ q₁ L q_n-1 q_n] 

r＝[r₀ r₁ L r_n-1 r_n]ξ＝[1 t L  t^n-1 tⁿ]^T

M为事先定义的常数矩阵，定义T为采样周期； 

对于Chebyshev(切比雪夫)正交多项式： 

$\left.\begin{array}{c}{\mathrm{\ξ}}_0\left(t\right)=1\\ {\mathrm{\ξ}}_1\left(t\right)=1-2t/b\\ {\mathrm{\ξ}}_2\left(t\right)=8{(t/b)}^2-8(t/b)+1\\ M\\ {\mathrm{\ξ}}_{i+1}\left(t\right)=2{\mathrm{\ξ}}_1\left(t\right){\mathrm{\ξ}}_i\left(t\right)-{\mathrm{\ξ}}_{i-1}\left(t\right)\end{array}\right\}$ i＝2，3，L，n-1，0≤t≤NT，b＝NT

则常数矩阵 

$M=\left\{m(i,j)\right\}=\left[\begin{array}{c}{m}_1\\ m_2\\ m_3\\ M\\ m_N\end{array}\right]=\left[\begin{array}{ccccc}1& 0& 0& L& 0\\ 1& -\frac{2}{b}& 0& L& 0\\ 1& -\frac{8}{b}& \frac{8}{b^2}& L& 0\\ M& M& M& O& M\\ & & & & \end{array}\right]$

$m(i,j)=2m(i-1,j)-m(i-2,j)-\frac{4}{b}m(i-1,j-1),$ (i＝3，4，L，N；j＝1，2，L，i)

m(i，j)＝0，(j＞i)

m(i，0)＝0，(j＝1，2，L，N)

(b)俯仰角的时间更新求解式为： 

式中： 

$a_1={\left(\mathrm{qMH\ξ}{|}_{\mathrm{kT}}^{(k+1)T}\right)}^2+{\left(\mathrm{rMH\ξ}{|}_{\mathrm{kT}}^{(k+1)T}\right)}^2-{\left(\mathrm{pMH\ξ}{|}_{\mathrm{kT}}^{(k+1)T}\right)}^2$

$a_2=\mathrm{pM\Ω}\left(t\right){|}_{\mathrm{kT}}^{(k+1)T}{H}^T{M}^T{r}^T-\mathrm{pMH\ξ}{|}_{\mathrm{kT}}^{(k+1)T}\mathrm{rMH\ξ}{|}_{\mathrm{kT}}$

$a_3=\mathrm{pM\Ω}\left(t\right){|}_{\mathrm{kT}}^{(k+1)T}{H}^T{M}^T{q}^T-\mathrm{pMH\ξ}{|}_{\mathrm{kT}}^{(k+1)T}\mathrm{qMH\ξ}{|}_{\mathrm{kT}}$

$\left|\mathrm{\λ}\right|=\{\mathrm{pM\Ω}\left(t\right){|}_{\mathrm{kT}}^{(k+1)T}{H}^T{M}^T{p}^T-\mathrm{pMH\ξ}{|}_{\mathrm{kT}}^{(k+1)T}\mathrm{pMH\ξ}{|}_{\mathrm{kT}}$

$+\mathrm{qM\Ω}\left(t\right){|}_{\mathrm{kT}}^{(k+1)T}{H}^T{M}^T{q}^T-\mathrm{qMH\ξ}{|}_{\mathrm{kT}}^{(k+1)T}\mathrm{qMH\ξ}{|}_{\mathrm{kT}}$

$+\mathrm{rM\Ω}\left(t\right){|}_{\mathrm{kT}}^{(k+1)T}{H}^T{M}^T{r}^T-\mathrm{rMH\ξ}{|}_{\mathrm{kT}}^{(k+1)T}\mathrm{rMH\ξ}{|}_{\mathrm{kT}}{\}}^{\frac{1}{2}}$

$H=\mathrm{diag}\{1,\frac{1}{2},\frac{1}{3},L,\frac{1}{n},\frac{1}{n+1}\};$

$\mathrm{\Ω}\left(t\right)=\left[\begin{array}{ccccc}\frac{1}{2}{t}^2& \frac{1}{3}{t}^3& L& \frac{1}{n+1}{t}^{n+1}& \frac{1}{n+2}{t}^{n+2}\\ \frac{1}{3}{t}^3& \frac{1}{4}{t}^4& L& \frac{1}{n+2}{t}^{n+2}& \frac{1}{n+3}{t}^{n+3}\\ M& M& O& M& M\\ \frac{1}{n+1}{t}^{n+1}& \frac{1}{n+2}{t}^{n+2}& L& \frac{1}{2n}{t}^{2n}& \frac{1}{2n+1}{t}^{2n+1}\\ \frac{1}{n+2}{t}^{n+2}& \frac{1}{n+3}{t}^{n+3}& L& \frac{1}{2n+1}{t}^{2n+1}& \frac{1}{2n+2}{t}^{2n+2}\end{array}\right]$

2、在已知俯仰角的情况下，滚转角的时间更新求解式为： 

其中 

$a_4={\left(\mathrm{pMH\ξ}{|}_{\mathrm{kT}}^{(k+1)T}\right)}^2+{\left(\mathrm{rMH\ξ}{|}_{\mathrm{kT}}^{(k+1)T}\right)}^2-{\left(\mathrm{qMH\ξ}{|}_{\mathrm{kT}}^{(k+1)T}\right)}^2$

$a_5=\mathrm{qM\Ω}\left(t\right){|}_{\mathrm{kT}}^{(k+1)T}{H}^T{M}^T{p}^T-\mathrm{qMH\ξ}{|}_{\mathrm{kT}}^{(k+1)T}\mathrm{pMH\ξ}{|}_{\mathrm{kT}}$

$a_6=\mathrm{qM\Ω}\left(t\right){|}_{\mathrm{kT}}^{(k+1)T}{H}^T{M}^T{r}^T-\mathrm{qMH\ξ}{|}_{\mathrm{kT}}^{(k+1)T}\mathrm{rMH\ξ}{|}_{\mathrm{kT}}$

3、在俯仰角、滚转角已知情况下，偏航角的求解式为： 

$\mathrm{\ψ}\left(t\right)=\mathrm{\ψ}\left(\mathrm{kT}\right)+{\∫}_{\mathrm{kT}}^t[b_1\left(t\right)+b_2\left(t\right)]\mathrm{dt}$

式中： 

当对惯性设备直接输出滚转、俯仰、偏航角速度p，q，r采用三阶逼近描述时，所得结果也接近O(T³)，相比欧拉方程直接近似法或采用近似龙格库塔方法解算等方法的O(T²)精度要高。 

Claims (1)

1.一种基于角速度的欧拉角多项式类指数近似输出方法，其特征在于包括以下步骤：

步骤1、(a)根据欧拉方程:

式中：θ，ψ分别指滚转、俯仰、偏航角；p(t),q(t),r(t)分别为滚转、俯仰、偏航角速度；这三个欧拉角的计算按照依次求解俯仰角、滚转角、偏航角的步骤进行；滚转、俯仰、偏航角速度p(t),q(t),r(t)的展开式分别为

p(t)＝pMξ,q(t)＝qMξ,r(t)＝rMξ

其中

p＝[p₀ p₁ … p_n-1 p_n]  q＝[q₀ q₁ … q_n-1 q_n]

r＝[r₀ r₁ … r_n-1 r_n]  ξ＝[1 t … t^n-1 tⁿ]^T

M为事先定义的常数矩阵；定义T为采样周期；

(b)俯仰角的时间更新求解式为：

式中：

$a_1={\left(\mathrm{qMH}{\mathrm{\ξ}|}_{\mathrm{kT}}^{(k+1)T}\right)}^2+{\left(\mathrm{rMH}{\mathrm{\ξ}|}_{\mathrm{kT}}^{(k+1)T}\right)}^2-{\left(\mathrm{pMH}{\mathrm{\ξ}|}_{\mathrm{kT}}^{(k+1)T}\right)}^2$

$a_2=\mathrm{pM}{\mathrm{\Ω}\left(t\right)|}_{\mathrm{kT}}^{(k+1)T}{H}^T{M}^T{r}^T-\mathrm{pMH}{\mathrm{\ξ}|}_{\mathrm{kT}}^{(k+1)T}\mathrm{rMH}{\mathrm{\ξ}|}_{\mathrm{kT}}$

$a_3=\mathrm{pM}{\mathrm{\Ω}\left(t\right)|}_{\mathrm{kT}}^{(k+1)T}{H}^T{M}^T{q}^T-\mathrm{pMH}{\mathrm{\ξ}|}_{\mathrm{kT}}^{(k+1)T}\mathrm{qMH}{\mathrm{\ξ}|}_{\mathrm{kT}}$

$\begin{array}{c}\left|\mathrm{\λ}\right|=\{\mathrm{pM}{\mathrm{\Ω}\left(t\right)|}_{\mathrm{kT}}^{(k+1)T}{H}^T{M}^T{p}^T-\mathrm{pMH}{\mathrm{\ξ}|}_{\mathrm{kT}}^{(k+1)T}\mathrm{pMH}{\mathrm{\ξ}|}_{\mathrm{kT}}\\ +\mathrm{qM\Ω}{\left(t\right)|}_{\mathrm{kT}}^{(k+1)T}{H}^T{M}^T{q}^T-\mathrm{qMH}{\mathrm{\ξ}|}_{\mathrm{kT}}^{(k+1)T}\mathrm{qMH}{\mathrm{\ξ}|}_{\mathrm{kT}}\\ +\mathrm{rM\Ω}{\left(t\right)|}_{\mathrm{kT}}^{(k+1)T}{H}^T{M}^T{r}^T-\mathrm{rMH}{\mathrm{\ξ}|}_{\mathrm{kT}}^{(k+1)T}\mathrm{rMH}{\mathrm{\ξ}|}_{\mathrm{kT}}{\}}^{\frac{1}{2}}\end{array}$

$H=\mathrm{diag}\{1,\frac{1}{2},\frac{1}{3},...,\frac{1}{n},\frac{1}{n+1}\};$

步骤2、在已知俯仰角的情况下，滚转角的时间更新求解式为：

其中

$a_4={\left(\mathrm{pMH}{\mathrm{\ξ}|}_{\mathrm{kT}}^{(k+1)T}\right)}^2+{\left(\mathrm{rMH}{\mathrm{\ξ}|}_{\mathrm{kT}}^{(k+1)T}\right)}^2-{\left(\mathrm{qMH}{\mathrm{\ξ}|}_{\mathrm{kT}}^{(k+1)T}\right)}^2$

$a_5=\mathrm{qM}{\mathrm{\Ω}\left(t\right)|}_{\mathrm{kT}}^{(k+1)T}{H}^T{M}^T{p}^T-\mathrm{qMH}{\mathrm{\ξ}|}_{\mathrm{kT}}^{(k+1)T}\mathrm{pMH}{\mathrm{\ξ}|}_{\mathrm{kT}}$

$a_6=\mathrm{qM}{\mathrm{\Ω}\left(t\right)|}_{\mathrm{kT}}^{(k+1)T}{H}^T{M}^T{r}^T-\mathrm{qMH}{\mathrm{\ξ}|}_{\mathrm{kT}}^{(k+1)T}\mathrm{rMH}{\mathrm{\ξ}|}_{\mathrm{kT}}$

步骤3、在俯仰角、滚转角已知情况下，偏航角的求解式为：

$\mathrm{\ψ}\left(t\right)=\mathrm{\ψ}\left(\mathrm{kT}\right)+{\∫}_{\mathrm{kT}}^t[b_1\left(t\right)+b_2\left(t\right)]\mathrm{dt}$

式中：