CN105606846B - 一种基于姿态信息的加速度计校准方法 - Google Patents

Abstract

本发明公开的一种基于姿态信息的加速度计校准方法，涉及一种加速度计校准方法，属于深空探测技术领域。本发明为了在缺少位置测量信息的情况下进行加速度计校准，采用无陀螺仪惯导方式，推导加速度计输出模型，利用姿态确定系统输出，结合非线性滤波方法进行加速度计漂移估计，重解算加速度计漂移及相应误差方差阵，将加速度计漂移及相应误差方差阵输入非线性滤波器，估计出加速度计漂移，完成加速度计校准，提高校准精度，进而保证大气进入段自主导航性能。本发明要解决的技术问题是在缺少位置测量信息的情况下提高行星接近段加速度计校准精度，保证大气进入段自主导航性能。

Description

一种基于姿态信息的加速度计校准方法

技术领域

本发明涉及一种加速度计校准方法，尤其涉及一种基于姿态信息的加速度计校准方法，属于深空探测技术领域。

背景技术

基于惯性测量单元的惯性导航方案是近期行星着陆探测任务进入段自主导航的首选方式。因此在行星接近段进行惯导校准是减少漂移随时间累积的必要步骤。行星接近段姿态确定系统可以利用星敏感器、太阳敏感器等姿态敏感元件对陀螺仪进行精确的校准，但对加速度计的校准往往依赖于位置测量信息，而由于行星接近段探测器的位置测量信息十分匮乏，因此需要解决在缺少位置测量信息情况下对加速度计校准的问题，以提高行星大气进入段惯性导航性能。

现阶段对针对在缺少位置测量信息情况下进行加速度计校准的问题尚未有系统的研究。在少数的尝试中均采用最小二乘法对加速度计漂移进行估计，估计精度有限，同时所采用的测量手段对于行星探测任务并不完全适用。因此仍需寻求新的加速度计校准方案以提高行星着陆探测自主导航的性能。

发明内容

针对现有技术中存在的缺少位置测量信息的情况下进行加速度计校准精度差的问题。本发明公开的一种基于姿态信息的加速度计校准方法，要解决的技术问题是在缺少位置测量信息的情况下提高行星接近段加速度计校准精度，保证大气进入段自主导航性能。

本发明的目的是通过下述技术方案实现的：

本发明公开的一种基于姿态信息的加速度计校准方法，为了在缺少位置测量信息的情况下进行加速度计校准，采用无陀螺仪惯导方式，推导加速度计输出模型，利用姿态确定系统输出，结合非线性滤波方法进行加速度计漂移估计，重解算加速度计漂移及相应误差方差阵，将加速度计漂移及相应误差方差阵输入非线性滤波器，估计出加速度计漂移，完成加速度计校准，提高校准精度。

本发明公开的一种基于姿态信息的加速度计校准方法，包括如下步骤：

步骤1：建立采用无陀螺仪惯导方式的加速度计校准系统模型。

无陀螺仪惯导方式能够单独利用加速度计测量角加速度。利用n个安装在探测器不同位置P₁,…,P_n上的三轴加速度计，各加速度计安装位置的加速度表示为：

式中a_c为探测器质心处的加速度，r_i为P_i的位置矢量，ω和分别为探测器角速度与角加速度。对于探测器来说，质心处的加速度分为引力加速度g与控制加速度u_c，同样P_i处的加速度也分为引力项g_i和通过加速度计测量的非引力项f_i。而由于探测器尺寸通常不大，重力梯度可以忽略，因此g_i＝g。不同位置P_i处的加速度与探测器角速度及角加速度的关系为：

式中为加速度计参考系到本体坐标系的姿态转移矩阵，同时对称矩阵Ω²及反对称矩阵满足：

ω_x，ω_y和ω_z分别为角速度ω的分量。根据惯性系O_I-X_IY_IZ_I和本体系o_b-x_by_bz_b建立加速度计测量模型为：

式中为加速度计输出，b_i为加速度计漂移，η_vi和η_ui分别为测量噪声与过程噪声，将测量噪声η_vi和过程噪声η_ui描述为不相关的零均值Gauss白噪声，所述不相关的零均值Gauss白噪声应满足的条件如公式(4)所示：

式中δ(t-τ)为δ函数。假设测量噪声η_vi和过程噪声η_ui的标准差相同，即σ_vi＝σ_v，σ_ui＝σ_u。将式(3)带入式(2)，加速度计的输出模型通过如下的矩阵形式描述为：

式中为R^*＝R^T(RR^T)^-1为R的伪逆矩阵。

通过如下定义：

式中的第j列，则探测器的角加速度表示为：

式(7)即为加速度计的输出模型，将加速度计输出及加速度计漂移与探测器的角加速度联系起来，对上式进行积分即能得到探测器姿态及角速度与加速度计漂移的关系。采用姿态四元数描述探测器的姿态运动学模型。四元数定义为其中q_1:3＝[q₁,q₂,q₃]^T是四元数的矢量部分，q为四元数的标量部分。探测器的姿态运动学方程建立为：

其中

同时定义真实四元数与估计四元数之间的姿态误差角矢量δθ。则误差四元数通过误差角矢量描述为δq≈[1/2δθ,1]^T，误差四元数的微分方程为：

式中为角速度估计值。因此误差角矢量的微分方程满足：

加速度计校准系统的状态定义为x≡[δθ^T,ω^T,b^T]^T，其中式(3)，(7)和(11)构成了加速度计校准系统的动力学模型。

在加速度计校准方法中借助姿态确定系统输出的探测器姿态及姿态角速度作为观测量。测量的误差四元数定义为：

式中为姿态四元数估计值，为的逆，满足则姿态误差矢量的测量值为因此探测器的姿态测量信息直接定义为误差角矢量：

式中为误差角的测量误差噪声。而探测器姿态角速度测量模型为：

式中为角速度的测量误差噪声。因此加速度计校准系统的测量模型描述为测量噪声ν_k的方差阵R_k由姿态确定系统直接输出。

基于加速度计校准系统模型，结合非线性滤波方法对加速度计漂移进行最优估计。

所述的非线性滤波方法选用扩展Kalman滤波、集合Kalman滤波、粒子滤波等。

步骤2：重解算加速度计漂移及误差方差矩阵，将加速度计漂移及误差方差矩阵输入非线性滤波器，估计加速度计漂移，完成基于姿态信息的加速度计校准。

首先根据式(7)，角加速度的估计值为：

式中为加速度计漂移的滤波输出，同时与角速度的滤波输出计算矩阵：

根据式(2)与(3)，加速度计漂移的重解算方程为：

同时将式(3)与(16)带入式(17)得到：

由于只对加速度计漂移进行重解算，则敏感性矩阵定义为：

式中r_i＝[r_ix,r_iy,r_iz]^T，Γ＝[Γ₁,…,Γ_n]，且：

定义矩阵M＝S_b+I₃，则计算式(18)中过程噪声方差矩阵为：

综上所述，状态误差方差阵的重解算方程为：

式中为状态误差方差的滤波输出。

式(15)至式(21)为加速度计漂移及相应误差方差阵的重解算过程，随后将与重新输入非线性滤波器进行滤波计算，再次估计加速度计的漂移，完成在缺少位置测量信息情况下基于姿态信息的加速度计校准，保证大气进入段自主导航性能。

有益效果：

1、本发明公开的一种基于姿态信息的加速度计校准方法，利用姿态确定系统输出的姿态及角速度信息，建立采用无陀螺惯导方式的加速度计校准系统模型，通过非线性滤波方法估计出加速度计漂移，在缺少位置测量信息情况下基于姿态信息的加速度计校准，保证大气进入段自主导航性能。

2、本发明公开的一种基于姿态信息的加速度计校准方法，加入加速度计漂移及状态误差方差阵重解算步骤，将重解算后的加速度计漂移及误差方差阵重新输入非线性滤波器进行滤波计算，再次估计加速度计的漂移，提高加速度计校准精度。

附图说明

图1为本发明公开的一种基于姿态信息的加速度计校准方法的流程图；

图2为无陀螺仪惯导方式原理图；

图3a)至3i)为加速度计漂移的估计误差的仿真结果，其中实线为误差，虚线为误差标准差；其中：图3a)b_1x误差曲线、图3b)b_1y误差曲线、图3c)b_1z误差曲线、图3d)b_2x误差曲线、图3e)b_2y误差曲线、图3f)b_2z误差曲线、图3g)b_3x误差曲线、图3h)b_3y误差曲线、图3i)b_3z误差曲线。

具体实施方式

为了更好的说明本发明的目的和优点，下面结合附图和实例对发明内容做进一步说明。

本实例针对火星接近段基于姿态信息的加速度计校准方案，采用探测器姿态确定系统输出的姿态及角速度信息，结合扩展Kalman滤波器进行滤波解算，实现对三颗加速度计的高精度校准。本实例的具体实施方法如下：

步骤1：建立加速度计校准系统模型

无陀螺仪惯导方式可以单独利用加速度计测量角加速度。利用3个安装在探测器不同位置P₁，P₂，P₃上的三轴加速度计，各加速度计安装位置的加速度可以表示为

式中a_c为探测器质心处的加速度，r_i为P_i的位置矢量，满足r₁＝[1,0,0]^T，r₂＝[0,1,0]^T，r₃＝[0,0,1]^T。ω和分别为探测器角速度与角加速度。对于探测器来说，质心处的加速度可以分为引力加速度g与控制加速度u_c，同样P_i处的加速度也可以分为引力项g_i和可以通过加速度计测量的非引力项f_i。而由于探测器尺寸通常不大，重力梯度可以忽略，因此g_i＝g。不同位置P_i处的加速度与探测器角速度及角加速度的关系为

式中为加速度计参考系到本体坐标系的姿态转移矩阵，考虑各加速度计参考坐标系与本体系重合，即同时对称矩阵Ω²及反对称矩阵满足

ω_x，ω_y和ω_z分别为角速度ω的分量。加速度计测量模型为

式中为加速度计输出，b_i为加速度计漂移，η_vi和η_ui分别为测量噪声与过程噪声，可以描述为不相关的零均值Gauss白噪声

式中δ(t-τ)为δ函数。假设测量噪声η_vi和过程噪声η_ui的标准差相同，即σ_vi＝σ_v＝1×10^-5m/s²，σ_ui＝σ_u＝1×10^-6m/s³。将式(24)带入式(23)，加速度计的输出模型可以通过如下的矩阵形式描述为

式中为R^*＝R^T(RR^T)^-1为R的伪逆矩阵，此时也为R的逆矩阵。

通过如下定义

式中的第j列，则探测器的角加速度可表示为

式(28)即为加速度计的输出模型，将加速度计输出及加速度计漂移与探测器的角加速度联系起来，对上式进行积分即可得到探测器姿态及角速度与加速度计漂移的关系。这里采用姿态四元数描述探测器的姿态运动学模型。四元数定义为其中q_1:3＝[q₁,q₂,q₃]^T是四元数的矢量部分，q为四元数的标量部分。探测器的姿态运动学方程建立为

其中

同时定义真实四元数与估计四元数之间的姿态误差角矢量δθ。则误差四元数可以通过误差角矢量描述为δq≈[1/2δθ,1]^T，误差四元数的微分方程为

式中为角速度估计值。因此误差角矢量的微分方程满足

加速度计校准系统的状态定义为x≡[δθ^T,ω^T,b^T]^T，其中式(24)，(28)和(32)构成了加速度计校准系统的动力学模型。考虑状态初值δθ＝[0,0,0]^T，ω₀＝[0.1,0,0.01]^T°/s，b₁₀＝[-3×10^-4,2.5×10^-4,1×10^-4]^T m/s²，b₂₀＝[2×10^-4,-1.5×10^-4,-2×10^-4]^T m/s²，b₃₀＝[-1×10^-4,3×10^-4,-2.5×10^-4]^T m/s²。

在加速度计校准方法中借助姿态确定系统输出的探测器姿态及姿态角速度作为观测量。测量的误差四元数定义为

式中为姿态四元数估计值，为的逆，满足则姿态误差矢量的测量值为因此探测器的姿态测量信息可以直接定义为误差角矢量自身

式中为误差角的测量噪声矢量。而探测器姿态角速度测量模型为

式中为角速度的测量噪声矢量。因此加速度计校准系统的测量模型可以描述为测量噪声ν_k的方差阵R_k也可以由姿态确定系统直接输出。

基于加速度计校准系统模型，结合非线性滤波方法可以对加速度计漂移进行最优估计，非线性滤波方法可以选用扩展Kalman滤波、集合Kalman滤波、粒子滤波等。这里采用集合Kalman滤波进行最优估计解算，滤波步长为1s。

步骤2：重解算加速度计漂移及误差方差矩阵，将加速度计漂移及误差方差矩阵输入非线性滤波器，估计加速度计漂移，完成基于姿态信息的加速度计校准。

首先根据式(28)，角加速度的估计值为

式中为加速度计漂移的滤波输出，同时与角速度的滤波输出计算矩阵

根据式(23)与(24)，加速度计漂移的重解算方程为

同时将式(24)与(37)带入式(38)可以得到

由于只对加速度计漂移进行重解算，则敏感性矩阵定义为

式中r_i＝[r_ix,r_iy,r_iz]^T，Γ＝[Γ₁,…,Γ₃]，且

定义矩阵M＝S_b+I₃，则可以计算式(39)中过程噪声方差矩阵为

综上所述，状态误差方差阵的重解算过程为

式中为状态误差方差的滤波输出。

上述为加速度计漂移及相应误差方差阵的重解算过程，随后将与重新输入非线性滤波器进行滤波计算。

仿真运行200s，探测器加速度计漂移的估计误差如图3所示。仿真结果表明所提出的加速度计校准方案可以在不利用位置测量信息的情况下最优估计加速度计的漂移值，收敛快、精度高，适用于未来火星探测接近段的加速度计校准问题。

尽管已经示出和描述了本发明的实施例，本领域的普通技术人员可以理解：在不脱离本发明的原理和宗旨的情况下，可对这些实施例进行多种变化、修改、替换和变形，本发明的范围由权利要求及其等同物限定。

Claims (3)

1.一种基于姿态信息的加速度计校准方法，其特征在于：具体实现方法包括如下步骤，

步骤1：建立采用无陀螺仪惯导方式的加速度计校准系统模型；

无陀螺仪惯导方式能够单独利用加速度计测量角加速度；利用n个安装在探测器不同位置P₁,…,P_n上的三轴加速度计，各加速度计安装位置的加速度表示为：

式中a_c为探测器质心处的加速度，r_i为P_i的位置矢量，ω和分别为探测器角速度与角加速度；对于探测器来说，质心处的加速度分为引力加速度g与控制加速度u_c，同样P_i处的加速度也分为引力项g_i和通过加速度计测量的非引力项f_i；而由于探测器尺寸通常不大，重力梯度可以忽略，因此g_i＝g；不同位置P_i处的加速度与探测器角速度及角加速度的关系为：

式中为加速度计参考系到本体坐标系的姿态转移矩阵，同时对称矩阵Ω²及反对称矩阵满足：

ω_x，ω_y和ω_z分别为角速度ω的分量；根据惯性系O_I-X_IY_IZ_I和本体系o_b-x_by_bz_b建立加速度计测量模型为：

式中为加速度计输出，b_i为加速度计漂移，η_vi和η_ui分别为测量噪声与过程噪声，将测量噪声η_vi和过程噪声η_ui描述为不相关的零均值Gauss白噪声，所述不相关的零均值Gauss白噪声应满足的条件如公式(4)所示：

式中δ(t-τ)为δ函数；假设测量噪声η_vi和过程噪声η_ui的标准差相同，即σ_vi＝σ_v，σ_ui＝σ_u；将式(3)带入式(2)，加速度计的输出模型通过如下的矩阵形式描述为：

若利用三个以上三轴加速度计，矩阵Ω通过式(6)计算为：

式中为R^*＝R^T(RR^T)^-1为R的伪逆矩阵；

通过如下定义：

式中为的第j列，则探测器的角加速度表示为：

式(7)即为加速度计的输出模型，将加速度计输出及加速度计漂移与探测器的角加速度联系起来，对式(7)进行积分即能得到探测器姿态及角速度与加速度计漂移的关系；采用姿态四元数描述探测器的姿态运动学模型；四元数定义为其中q_1:3＝[q₁,q₂,q₃]^T是四元数的矢量部分，q为四元数的标量部分；探测器的姿态运动学方程建立为：

其中

同时定义真实四元数与估计四元数之间的姿态误差角矢量δθ；则误差四元数通过误差角矢量描述为δq≈[1/2δθ,1]^T，误差四元数的微分方程为：

式中为角速度估计值；因此误差角矢量的微分方程满足：

加速度计校准系统的状态定义为x≡[δθ^T,ω^T,b^T]^T，其中式(3)、(7)和(11)构成了加速度计校准系统的动力学模型；

在加速度计校准方法中借助姿态确定系统输出的探测器姿态及姿态角速度作为观测量；测量的误差四元数定义为：

式中为姿态四元数估计值，为的逆，满足则姿态误差矢量的测量值为因此探测器的姿态测量信息直接定义为误差角矢量：

式中为误差角的测量误差噪声；而探测器姿态角速度测量模型为：

式中为角速度的测量误差噪声；因此加速度计校准系统的测量模型描述为测量噪声ν_k的方差阵R_k由姿态确定系统直接输出；

基于加速度计校准系统模型，结合非线性滤波方法对加速度计漂移进行最优估计；

步骤2：重解算加速度计漂移及误差方差矩阵，将加速度计漂移及误差方差矩阵输入非线性滤波器，估计加速度计漂移，完成基于姿态信息的加速度计校准。

2.如权利要求1所述的一种基于姿态信息的加速度计校准方法，其特征在于：所述的步骤2的具体实现方法为，

首先根据式(7)，角加速度的估计值为：

式中为加速度计漂移的滤波输出，同时与角速度的滤波输出计算矩阵：

根据式(2)与(3)，加速度计漂移的重解算方程为：

同时将式(3)与(16)带入式(17)得到：

由于只对加速度计漂移进行重解算，则敏感性矩阵定义为：

式中r_i＝[r_ix,r_iy,r_iz]^T，Γ＝[Γ₁,…,Γ_n]，且：

定义矩阵M＝S_b+I₃，则计算式(18)中过程噪声方差矩阵为：

综上所述，状态误差方差阵的重解算方程为：

式中为状态误差方差的滤波输出；

式(15)至式(21)为加速度计漂移及相应误差方差阵的重解算过程，随后将与重新输入非线性滤波器进行滤波计算，再次估计加速度计的漂移，完成在缺少位置测量信息情况下基于姿态信息的加速度计校准，保证大气进入段自主导航性能。

3.如权利要求1或2所述的一种基于姿态信息的加速度计校准方法，其特征在于：步骤1所述的非线性滤波方法选用扩展Kalman滤波、集合Kalman滤波、粒子滤波。