CN103900559A - 一种基于干扰估计的高精度姿态解算系统 - Google Patents

Abstract

一种基于干扰估计的高精度姿态解算系统，包括姿态敏感器、姿态信息处理板、实时仿真目标机、姿态控制模块和姿态信息显示模块；其中姿态敏感器用于敏感载体当前的姿态信息，姿态信息处理板接收姿态敏感器的量测数据，采用干扰估计器和鲁棒混合多目标滤波器的抗干扰滤波算法，对接收到的原始量测数据进行滤波，得到满足载体精度要求的姿态解算结果，并将姿态解算信息发送给姿态控制模块，实时仿真目标机接收姿态控制模块解算的姿态控制指令，调用执行机构和姿态动力学模块模拟载体的运动状态，同时通过姿态信息显示模块实现数据显示功能。

Description

一种基于干扰估计的高精度姿态解算系统

技术领域

本发明涉及一种基于干扰估计的高精度姿态解算系统，能直接应用于航空、航天等领域的姿态控制回路，又可作为姿态控制半物理仿真平台中的姿态确定单元来验证各种抗干扰姿态的确定。

背景技术

卫星、航天飞机、导弹等飞行器为了完成各自所承担的任务，需要对自身的姿态进行精确地控制，而准确地确定当前姿态信息是上述飞行器实现高性能姿态控制的前提，因此，姿态解算系统是飞行器的重要组成部分。

通常的姿态解算系统主要由姿态敏感器和相应的姿态信息处理算法即滤波算法组成，因此，姿态解算的精度取决于姿态敏感器本身的测量精度和滤波算法的性能。常用的姿态敏感器件主要包括陀螺仪、恒星敏感器、太阳敏感器、地球敏感器和磁强计。每一类敏感器都有自身的长处与不足，在实际的姿态确定系统中，通常采用滤波算法对不同类型敏感器所测得的姿态信息进行融合，从而能以较高的精度对当前姿态进行估计。在对姿态敏感器的量测数据进行滤波处理时，不可避免地会受到包括建模误差、模型参数摄动、敏感器随机噪声在内的多源干扰的影响，这些干扰不能被简单地归入高斯噪声，因此在实际任务中，仅能处理高斯噪声的卡尔曼滤波算法往往无法满足姿态确定系统的精度要求，滤波算法的抗干扰性能越来越成为人们关注的焦点。同时为验证某种滤波算法的性能，需要进行半物理仿真试验，传统的半物理仿真需要将陀螺仪精确安装在三轴转台上才能测量三个轴的姿态角速率，而转台往往是一套半物理仿真平台中最昂贵的装置，对转台的依赖不但给半物理仿真的操作带来不便，而且大大地增加了成本。

发明内容

本发明的技术解决问题是：克服现有技术的不足，提供一种硬件结构简单，能够有效抑制多源干扰影响，可以方便地用于桌面在回路仿真验证的高精度姿态解算系统。

本发明的技术解决方案是：一种基于干扰估计的高精度姿态解算系统，其特征在于包括姿态敏感器、姿态信息处理板、实时仿真目标机、姿态控制模块和姿态信息显示模块；其中姿态敏感器包括陀螺仪、恒星敏感器、太阳敏感器和磁强计，用于敏感载体当前的姿态信息，姿态信息处理板实时接收姿态敏感器的量测数据，采用结合了干扰估计器和鲁棒混合多目标滤波器的抗干扰鲁棒滤波算法，对接收到的原始量测数据进行滤波处理，得到满足载体精度要求的姿态解算结果，并将姿态解算信息发送给姿态控制模块，实时仿真目标机接收姿态控制模块解算的姿态控制指令，调用实时仿真目标机中的执行机构和姿态动力学模块模拟真实载体的运动状态，同时通过姿态信息显示模块实现数据显示的功能；

所述的结合了干扰估计器和鲁棒混合多目标滤波器的抗干扰鲁棒滤波算法的实现如下：

（1）首先把姿态敏感器中陀螺仪的漂移作为可建模干扰，建立如下表达形式的干扰模型：

$\stackrel{\·}{w}\left(t\right)=W\left(t\right)w\left(t\right)+G_3\left(t\right)\mathrm{\δ}\left(t\right)$

其中w(t)为可建模干扰模型的状态变量，W(t)表示可建模干扰模型的系统阵，δ(t)为能量有界的不可建模随机干扰，G₃(t)为不可建模随机干扰的增益阵；

（2）采用上述干扰表达形式后，设计干扰估计器对陀螺仪漂移进行估计，具体形式如下：

$\hat{w}(k+1)=M\left(k\right)\hat{w}\left(k\right)+K_a[y\left(k\right)-\hat{y}\left(k\right)]$

其中，

为鲁棒混合多目标滤波器的状态变量，M(k)为W(t)离散化后所对应的矩阵，K_a为待定的干扰估计器增益矩阵，为鲁棒混合多目标滤波器的输出；

（3）然后针对模型不确定性和范数有界干扰下的鲁棒性能指标以及保成本性能指标，设计了鲁棒混合多目标滤波器对其进行抑制，具体形式如下：

$\left\{\begin{array}{c}\hat{x}(k+1)=A\left(k\right)\hat{x}\left(k\right)+u\left(k\right)+u_{c1}\left(k\right)+L[y\left(k\right)-\hat{y}\left(k\right)]\\ \hat{y}\left(k\right)=C\left(k\right)\hat{x}\left(k\right)+u\left(k\right)+u_{c2}\left(k\right)\end{array}\right.$

其中，A(k)、C(k)为姿态确定系统状态和量测方程中的系数矩阵；

为状态x(k)的估计值，u(k)为载体的控制输入，u_c1(k)以及u_c2(k)两项用于补偿可建模干扰w(k)，

为鲁棒混合多目标滤波器的输出，矩阵L为待定的鲁棒多目标滤波器增益阵；

（4）最后联立干扰估计器和鲁棒混合多目标滤波器构造闭环系统，使用matlab的线性矩阵不等式工具箱（LMI Toolbox）求解出符合载体输出精度要求的干扰估计器和鲁棒混合多目标滤波器。

所述的惯性器件直接放置在桌面上即可将其所敏感到的姿态信息传到姿态信息处理板中进行处理；或安装在载体上敏感载体当前的姿态信息。

所述姿态信息处理板硬件由嵌入式微处理器、电源模块、时钟模块、复位单元、I/O单元、随机存储器、外扩存储器、数据接收模块和数据发送模块组成。其中电源模块通过稳压芯片产生嵌入式微处理器运行时所需的电压信号；时钟模块产生嵌入式微处理器工作所需的时钟信号；复位单元选用专用复位芯片连接在嵌入式微处理器的硬件复位引脚，实现系统的复位功能；I/O单元通过将嵌入式微处理器的部分引脚引出，用以扩展更多外设，保证处理板的可扩展性；随机存储器用于嵌入式微处理器运行数据的随机存储；外扩存储器弥补了嵌入式微处理器芯片片内SRAM和ROM空间较小的不足；数据接收模块和数据发送模块使用串口芯片对嵌入式微处理器上原有的串口资源进行了扩充，保证本发明的姿态信息处理板能同时与多个姿态敏感器以及实时仿真目标机进行数据交互。在嵌入式微处理器运行所述结合了干扰估计器和鲁棒混合多目标滤波器的抗干扰鲁棒滤波算法时，首先根据载体当前的运行模式读取量测数据，而载体当前的运行模式决定于前一个解算周期所解算出的载体姿态角速率，当前运行模式下进行滤波解算所需的姿态敏感器确定后，微处理器通过数据接收单元以一定的频率读取姿态敏感器的量测数据，当所读取到的姿态敏感器数据有效时，微处理器先将量测数据解包并进行单位转换，然后调用抗干扰鲁棒滤波算法对载体当前姿态进行估计，姿态解算结果将通过数据发送单元实时发给实时仿真目标机以便实现显示及姿态控制功能。

本发明的原理是：当本发明姿态解算系统所在的运行环境存在高斯及非高斯噪声、模型不确定性、参数摄动及其它各种形式的干扰时，我们首先把惯性器件漂移建模为一阶高斯-马尔科夫过程，设计了干扰估计器对其进行估计，根据估计的结果抵消惯性器件漂移。下一步，把包括模型不确定性、参数摄动以及非高斯噪声在内的其它形式的干扰融合为一个范数有界变量，设计了鲁棒混合多目标滤波器对其进行抑制，其中H_∞性能抑制范数有界干扰，保成本性能优化估计误差的方差。最后，把干扰估计器和鲁棒混合多目标滤波器联立构成闭环系统，使用matlab的线性矩阵不等式工具箱（LMI Toolbox）对干扰估计器和鲁棒混合多目标滤波器进行求解，使闭环系统满足给定性能指标的要求，从而得到高精度的姿态解算结果。

本发明与现有技术相比的优点在于：本发明利用高性能的微处理器芯片构建了一种基于干扰估计的高精度姿态解算系统，并将其作为抗干扰姿态确定算法的执行核心，它与现有的姿态解算模块相比具有以下特点：

（1）本发明采用了融合多个敏感器量测数据的抗干扰姿态确定算法，与传统的卡尔曼滤波方法只能处理高斯噪声且计算量较大相比，本发明运用了抗干扰鲁棒滤波的方法来抑制多源干扰，它不需要确切地知道系统模型和噪声的统计特性，可以处理范数有界的不确定性，并利用干扰估计器抵消可建模干扰。在姿态确定系统中存在模型不确定性、噪声非高斯等多种干扰时，能够提升系统姿态解算的精度。

（2）相对与传统的姿态解算模块用来验证各种滤波算法时需要精确地安装各个姿态敏感器件，本发明的姿态解算模块直接将陀螺仪放置在桌面上即可进行桌面在回路半物理仿真验证，不但操作方便，而且节省了成本。

（3）通过姿态敏感器敏感载体姿态信息，运用结合了干扰估计器和鲁棒混合多目标滤波器的滤波算法对载体当前姿态进行实时高精度的滤波解算。本发明既能直接应用于航空、航天等领域的姿态控制回路，又可作为姿态控制半物理仿真平台中的姿态确定单元来验证各种抗干扰姿态确定算法的效果。

附图说明

图1为本发明的高精度姿态解算模块结构组成框图；

图2为本发明所采用的抗干扰姿态确定算法的流程图。

具体实施方式

如图1所示，本发明所涉及的一种基于干扰估计的高精度姿态解算系统，主要由姿态敏感器、姿态信息处理板、实时仿真目标机、姿态信息显示模块以及姿态控制模块构成。其中姿态敏感器件包括陀螺仪、恒星敏感器、太阳敏感器以及磁强计；实时仿真目标机连接姿态信息显示模块以及姿态控制模块；在解算载体当前姿态时，姿态敏感器安装在飞行器等载体上，姿态信息处理板中运行抗干扰鲁棒滤波算法，解算结果作为姿态控制模块的输入量；在用来进行半物理仿真验证时，姿态敏感器可以直接放置在桌面上，姿态信息处理板中运行所要验证的滤波算法，解算结果实时发给仿真目标机，仿真目标机通过执行机构模块和姿态动力学模块模拟真实载体的运动，通过姿态控制模块和显示模块实现姿态控制和实时数据显示的功能。

姿态信息处理板主要由嵌入式微处理器、电源模块、时钟模块、复位单元、I/O单元、随机存储器、外扩存储器、数据接收模块和数据发送模块组成。嵌入式微处理器为基于ARM9内核的AT91RM9200主处理器芯片；电源模块通过稳压芯片产生嵌入式微处理器运行时所需的5V、3.3V、1.7V电压信号；时钟模块使用多个不同频率的晶振构造了多个时钟单元，能够产生几种频率的时钟信号；复位单元选用专用复位芯片连接在嵌入式微处理器的硬件复位引脚，便于研究者调试程序；I/O单元通过将嵌入式微处理器的部分引脚引出，预留部分功能未用，可以扩展更多外设，保证了处理板的可扩展性；随机存储器选用SDRAM芯片用于嵌入式微处理器运行数据的随机存储；外扩存储器选用FLASH芯片，弥补了嵌入式微处理器芯片片内SRAM和ROM空间较小的不足；数据接收模块和数据发送模块使用串口芯片对嵌入式微处理器上原有的串口资源进行了扩充，保证本发明的姿态信息处理板能同时与多个姿态敏感器以及实时仿真目标机进行数据交互。

如图2所示，以实验模式为例，给出了本发明抗干扰鲁棒滤波算法的流程图。首先建立了含有多源干扰的姿态确定系统的状态方程和量测方程并对之离散化；第二步建立姿态确定系统状态方程的多胞模型描述；第三步设计干扰估计器对陀螺漂移进行估计；第四步设计鲁棒混合多目标滤波器；第五步联立混合H₂/H_∞滤波器和干扰估计器的方程构成闭环系统；第六步求解干扰观测器和鲁棒混合多目标滤波器。具体的实施步骤如下：

1.建立含有多源干扰的姿态确定系统状态和输出方程

根据陀螺输出的姿态角速率信息，利用四元数更新方程对姿态确定系统的姿态角误差进行建模，将卫星的姿态四元数作为状态方程的状态变量，把星敏感器的量测数据作为输出方程的输出变量，建立含有多源干扰的姿态确定系统的状态方程和输出方程如下：

$\left\{\begin{array}{c}\stackrel{\·}{x}\left(t\right)=F\left(t\right)x\left(t\right)+G\left(t\right)w\left(t\right)+G_1\left(t\right)w_1\left(t\right)+G_2\left(t\right)w_2\left(t\right)\\ y\left(t\right)=C\left(t\right)x\left(t\right)+D\left(t\right)w\left(t\right)+D_1\left(t\right)w_1\left(t\right)+D_2\left(t\right)w_2\left(t\right)\end{array}\right.$

其中，F(t)、G(t)、G₁(t)、G₂(t)、C(t)、D(t)、D₁(t)和D₂(t)均为已知的系数阵；x(t)表示姿态确定系统的三个姿态角，w(t)为可建模干扰，w₁(t)为高斯噪声，wx(t)为范数有界干扰，y(t)为系统输出。将此系统离散化，有：

$\left\{\begin{array}{c}x(k+1)=A\left(k\right)x\left(k\right)+B\left(k\right)w\left(k\right)+B_1\left(k\right)w_1\left(k\right)+B_2\left(k\right)w_2\left(k\right)\\ y\left(t\right)=C\left(k\right)x\left(k\right)+D\left(k\right)w\left(k\right)+D_1\left(k\right)w_1\left(k\right)+D_2\left(k\right)w_2\left(k\right)\end{array}\right.$

其中A(k)、B(k)、B₁(k)、B₂(k)分别为将F(t)、G(t)、G₁(t)、G₂(t)离散化后的系数阵；x(k)、w(k)、w₁(k)、w₂(k)和y(k)分别为对应于x(t)、w(t)、w₁(t)、w₂(t)和y(t)的离散状态变量。

2.建立姿态确定系统状态方程的多胞模型描述

将卫星本体坐标系下姿态确定系统状态方程中参数的变化作为系统的不确定性，并使用不确定系统的多胞模型来描述。矩阵A中变化项包括陀螺的常值漂移和随机噪声，将误差状态方程表示成多胞模型主要就是将陀螺的常值漂移和随机噪声用一个凸多面体表示，该凸多面体的顶点即为姿态确定系统状态方程的多胞模型描述。

3.对可建模干扰设计干扰估计器

陀螺漂移是姿态确定系统中主要的干扰源之一，通常认为其满足一阶马尔科夫过程，可表示为：

$\stackrel{\·}{w}\left(t\right)=W\left(t\right)w\left(t\right)+G_3\left(t\right)\mathrm{\δ}\left(t\right)$

其中w(t)为可建模干扰模型的状态变量，W(t)表示可建模干扰模型的系统阵，δ(t)为能量有界的不可建模随机干扰，G₃(t)为不可建模随机干扰的增益阵；

设计干扰观测器如下：

$\hat{w}(k+1)=M\left(k\right)\hat{w}\left(k\right)+K_a[y\left(k\right)-\hat{y}\left(k\right)]$

其中

为鲁棒混合多目标滤波器的状态变量，M(k)为W(t)离散化后所对应的矩阵，K_a为待定的干扰估计器增益矩阵，

为鲁棒混合多目标滤波器的输出；

4.设计鲁棒混合多目标滤波器

构造鲁棒混合多目标滤波器如下：

$\left\{\begin{array}{c}\hat{x}(k+1)=A\left(k\right)\hat{x}\left(k\right)+u\left(k\right)+u_{c1}\left(k\right)+L[y\left(k\right)-\hat{y}\left(k\right)]\\ \hat{y}\left(k\right)=C\left(k\right)\hat{x}\left(k\right)+u\left(k\right)+u_{c2}\left(k\right)\end{array}\right.$

其中，

为状态x(k)的估计值，u(k)为载体的控制输入，u_c1(k)以及u_c2(k)两项用于补偿可建模干扰w(k)，

为鲁棒混合多目标滤波器的输出，矩阵L为待定的鲁棒多目标滤波器增益阵；

5.联立干扰估计器和鲁棒混合多目标滤波器构造闭环系统

定义误差状态变量： $\tilde{x}\left(k\right)=x\left(k\right)-\hat{x}\left(k\right),\tilde{w}\left(k\right)=w\left(k\right)-\hat{w}\left(k\right),$ 则估计误差系统为：

$\left\{\begin{array}{c}\tilde{x}(k+1)=(A-\mathrm{LC})\tilde{x}\left(k\right)+(B-\mathrm{LD})\tilde{w}\left(k\right)+(B_1-{\mathrm{LD}}_1)w_1\left(k\right)+(B_2-{\mathrm{LD}}_2)w_2\left(k\right)\\ \tilde{w}(k+1)=(w-K_{a}D)\tilde{w}\left(k\right)-K_{a}C\tilde{x}\left(k\right)+(B_3-K_a{D}_1)w_1\left(k\right)-K_a{D}_2{w}_2\left(k\right)\end{array}\right.$

其中各系数矩阵均略去了时间变量k；K_a和L为待定的鲁棒多目标滤波器增益阵。

H_∞的参考输出定义为：

$Z_{\∞}\left(k\right)=C_{\∞1}\tilde{x}\left(k\right)+C_{\∞2}\tilde{w}\left(k\right)$

H₂的参考输出定义为：

$Z_2\left(k\right)=C_{21}\tilde{x}\left(k\right)+C_{22}\tilde{w}\left(k\right)$

其中，C_∞1，C_∞2，C₂₁和C₂₂为选定的权矩阵。

6.求解干扰估计器和鲁棒混合多目标滤波器

使用matlab线性矩阵不等式工具箱（LMI Toolbox），利用凸优化算法，根据系统输出的精度要求求解干扰估计器和鲁棒混合多目标滤波器的待定增益阵K_a和L。

本发明说明书中未作详细描述的内容属于本领域专业技术人员公知的现有技术。

Claims (3)

1.一种基于干扰估计的高精度姿态解算系统，其特征在于：包括姿态敏感器、姿态信息处理板、实时仿真目标机、姿态控制模块和姿态信息显示模块；其中姿态敏感器包括陀螺仪、恒星敏感器、太阳敏感器和磁强计，用于敏感载体当前的姿态信息，姿态信息处理板实时接收姿态敏感器的量测数据，采用结合了干扰估计器和鲁棒混合多目标滤波器的抗干扰鲁棒滤波算法，对接收到的原始量测数据进行滤波处理，得到满足载体精度要求的姿态解算结果，并将姿态解算信息发送给姿态控制模块，实时仿真目标机接收姿态控制模块解算的姿态控制指令，调用实时仿真目标机中的执行机构和姿态动力学模块模拟真实载体的运动状态，同时通过姿态信息显示模块实现数据显示的功能；所述的结合了干扰估计器和鲁棒混合多目标滤波器的抗干扰鲁棒滤波算法的实现如下：

（1）首先把姿态敏感器中陀螺仪的漂移作为可建模干扰，建立如下表达形式的干扰模型：

$\stackrel{\·}{w}\left(t\right)=W\left(t\right)w\left(t\right)+G_3\left(t\right)\mathrm{\δ}\left(t\right)$

其中w(t)为可建模干扰模型的状态变量，W(t)表示可建模干扰模型的系统阵，δ(t)为能量有界的不可建模随机干扰，G₃(t)为不可建模随机干扰的增益阵；

（2）采用上述干扰表达形式后，设计干扰估计器对陀螺仪漂移进行估计，具体形式如下：

$\hat{w}(k+1)=M\left(k\right)\hat{w}\left(k\right)+K_a[y\left(k\right)-\hat{y}\left(k\right)]$

其中

为鲁棒混合多目标滤波器的状态变量，M(k)为W(t)离散化后所对应的矩阵，K_a为待定的干扰估计器增益矩阵，

为鲁棒混合多目标滤波器的输出；

（3）然后针对模型不确定性和范数有界干扰下的鲁棒性能指标以及保成本性能指标，设计了鲁棒混合多目标滤波器对其进行抑制，具体形式如下：

$\left\{\begin{array}{c}\hat{x}(k+1)=A\left(k\right)\hat{x}\left(k\right)+u\left(k\right)+u_{c1}\left(k\right)+L[y\left(k\right)-\hat{y}\left(k\right)]\\ \hat{y}\left(k\right)=C\left(k\right)\hat{x}\left(k\right)+u\left(k\right)+u_{c2}\left(k\right)\end{array}\right.$

其中，A(k)、C(k)为姿态确定系统状态及量测方程中的系数矩阵；

为状态x(k)的估计值，u(k)为载体的控制输入，u_c1(k)以及u_c2(k)两项用于补偿可建模干扰w(k)，

为鲁棒混合多目标滤波器的输出，矩阵L为待定的鲁棒多目标滤波器增益阵；

（4）最后联立干扰估计器和鲁棒混合多目标滤波器构造闭环系统，使用matlab的线性矩阵不等式工具箱（LMI Toolbox）求解出符合载体输出精度要求的干扰估计器和鲁棒混合多目标滤波器。

2.根据权利要求1所述的一种基于干扰估计的高精度姿态解算系统，其特征在于：所述的陀螺仪直接放置在桌面上即可将其所敏感到的姿态信息传到姿态信息处理板中进行处理；或安装在载体上敏感载体当前的姿态信息。

3.根据权利要求1所述的一种基于干扰估计的高精度姿态解算系统，其特征在于姿态信息处理板主要由嵌入式微处理器、电源模块、时钟模块、复位单元、I/O单元、随机存储器、外扩存储器、数据接收模块和数据发送模块组成；其中电源模块通过稳压芯片产生嵌入式微处理器运行时所需的电压信号；时钟模块产生嵌入式微处理器工作所需的时钟信号；复位单元选用专用复位芯片连接在嵌入式微处理器的硬件复位引脚，实现系统的复位功能；I/O单元通过将嵌入式微处理器的部分引脚引出，用以扩展更多外设，保证处理板的可扩展性；随机存储器用于嵌入式微处理器运行数据的随机存储；外扩存储器弥补了嵌入式微处理器芯片片内SRAM和ROM空间较小的不足；数据接收模块和数据发送模块使用串口芯片对嵌入式微处理器上原有的串口资源进行了扩充，保证本发明的姿态信息处理板能同时与多个姿态敏感器以及实时仿真目标机进行数据交互，在嵌入式微处理器运行所述结合了干扰估计器和鲁棒混合多目标滤波器的抗干扰鲁棒滤波算法时，首先根据载体当前的运行模式读取量测数据，而载体当前的运行模式决定于前一个解算周期所解算出的载体姿态角速率，当前运行模式下进行滤波解算所需的姿态敏感器确定后，微处理器通过数据接收单元以一定的频率读取姿态敏感器的量测数据，当所读取到的姿态敏感器数据有效时，微处理器先将量测数据解包并进行单位转换，然后调用抗干扰鲁棒滤波算法对载体当前姿态进行估计，姿态解算结果将通过数据发送单元实时发给实时仿真目标机以便实现显示及姿态控制功能。

Images