CN106403952A - 一种动中通低成本组合姿态测量方法 - Google Patents

Abstract

本发明公开了一种动中通低成本组合姿态测量方法，包括以下步骤：1)采集天线的跟踪信息，并通过三轴陀螺采集载体运动的角速度信号ω_b，再根据天线的跟踪信息计算天线的航向角误差，然后通过天线的航向角误差对载体运动的角速度信号进行校正，并对校正后的载体运动的角速度信号进行积分，得航向角ψ；2)三轴加速计获取载体的机动状态、横滚角及纵摇角；3)获取观测量；4)根据步骤1)得到的航向角ψ、步骤2)得到的载体的机动状态、横滚角及纵摇角、以及步骤3)得到的观测量通过四元数自适应卡尔曼滤波器估计出天线的状态变量，该方法能够准确的估计出天线的状态变量。

Description

一种动中通低成本组合姿态测量方法

技术领域

本发明属于天线姿态测量技术领域，涉及一种动中通低成本组合姿态测量方法。

背景技术

传统的高精度姿态传感器(陀螺、加速度计等)的体积和质量都过于庞大,成本也很高；而基于MEMS技术的微型传感器与之相比,在体积、质量、功耗上都具有很大优势,因而必然成为微型姿态测量系统的最佳选择。

现有几种低成本传感器可以用来获取倾角(roll和pitch)和航向(yaw)信息。对于倾角测量，包括以下几类：第一个是倾角仪，倾角仪的输出与倾角成正比(根据重力场)，但这种传感器响应较慢且受震动的影响，因此不能在倾斜角估计中单独使用。第二个是速率陀螺传感器，陀螺测量旋转角速率。为了获得角度信息，需要对传感器信号进行积分。因此，即使小的速率误差也能导致估计角的漂移。第三个是加速度计。问题在于加速度计受到机动加速度的影响，只有在无机动加速度的情况下才能进行倾角估计；对于航向测量，我们也可以通过积分陀螺信号，但同样会存在漂移问题。另一个传感器是磁强计，它通过测量地磁场来获取航向。虽然传感器本身非常精确和小巧，但由于受到磁场干扰的影响，仍然会存在较大误差。

针对MEMS惯性姿态系统在功能上和精度上的诸多不足，通常采用融合方法来将多个传感器取长补短。先用加速度计输出的加速度信息和磁强计输出磁场强度信息计算的姿态角，用该姿态角作为量测值来补偿陀螺的漂移，这种算法即可以保证定姿的精度，又可以由陀螺保证系统具有较高的动态性和稳定性。在这种工作方式下，采用陀螺、加速度计和磁强计组合通过卡尔曼滤波确定载体的姿态，依靠重力向量和地磁向量为参考信息修正了陀螺积分后的角度漂移，提高航姿系统的精度。但利用磁强计数据决定航向角存在磁场干扰。当传感器处于无磁场干扰的空旷区域时，地磁场没有受到干扰，此时的航向角是正确的。实际上，磁强计是安装在车体上，其中车体平台、车上元件如发动机、电缆等会产生电磁场干扰。另外，外部物体如经过的车，建筑物、高压电线、大的金属物等也会产生电磁干扰。这些干扰会扰乱、弯曲地磁场，最终导致航向角误差，因此严重的影响了估计出来的天线状态变量的精度。

发明内容

本发明的目的在于克服上述现有技术的缺点，提供了一种动中通低成本组合姿态测量方法，该方法能够准确的估计出天线的状态变量。

为达到上述目的，本发明所述的动中通低成本组合姿态测量方法包括以下步骤：

1)采集天线的跟踪信息，并通过三轴陀螺采集载体运动的角速度信号ω_b，再根据天线的跟踪信息计算天线的航向角误差，然后通过天线的航向角误差对载体运动的角速度信号进行校正，并对校正后的载体运动的角速度信号进行积分，得航向角ψ；

2)三轴加速计获取载体的机动状态、横滚角及纵摇角；

3)由于跟踪信号与三轴加速计及三轴陀螺的更新频率不同，当当前无跟踪信息时，则将载体的横滚角及纵摇角作为观测量，当当前有跟踪信息时，步骤1)得到的航向角ψ作为观测量；

4)根据步骤1)得到的航向角ψ、步骤2)得到的载体的机动状态、横滚角及纵摇角、以及步骤3)得到的观测量通过四元数自适应卡尔曼滤波器估计出天线的状态变量。

步骤1)中根据天线的跟踪信息及载体运动的角速度信号ω_b计算航向角ψ的具体操作为：

根据天线的跟踪信息计算天线波束的指向偏差角，将天线波束的指向偏差角通过坐标变换得天线的航向角误差，再通过PI控制器对天线的航向角误差进行PI校正，然后再通过PI校正后的天线航向角误差对载体运动的角速度信号ω_b进行校正，再PI校正后的载体运动的角速度信号ω_b进行积分处理，得航向角ψ。

PI控制器的增益K_p及K_i为：

其中，为PI控制器的阻尼值。

通过载体的机动状态实时调整四元数自适应卡尔曼滤波器中的量测协方差矩阵R。

根据载体的机动状态判断载体是否存在外部加速度，当载体存在外部加速度时，则四元数自适应卡尔曼滤波器中的量测协方差矩阵R为：

其中，r_φ及r_θ分别为载体的横滚角及纵摇角对应的方差，r_ψ为航向角对应的方差。

通过载体的横滚角及纵摇角调整四元数自适应卡尔曼滤波器的增益。

本发明具有以下有益效果：

本发明所述的动中通低成本组合姿态测量方法在具体操作时，先计算天线的航向角误差，再通过天线的航向角误差对载体运行的角速度信号进行校正，并对校正后的角速度信号进行积分处理，消除磁场干扰对航向角的影响，同时本发明将航向角作为四元数自适应卡尔曼滤波器中的一个变量进行数据融合，从而精确获取天线的状态变量，操作简单，避免利用磁强计数据确定航向角存在的磁场干扰问题，减小载体运动加速度对姿态估计的影响。

附图说明

图1为本发明的流程图；

图2为本发明中系统图；

图3为本发明中四元数自适应卡尔曼滤波器的循环更新图。

具体实施方式

下面结合附图对本发明做进一步详细描述：

参考图1，本发明所述的动中通低成本组合姿态测量方法包括以下步骤：

1)采集天线的跟踪信息，并通过三轴陀螺采集载体运动的角速度信号ω_b，再根据天线的跟踪信息计算天线的航向角误差，然后通过天线的航向角误差对载体运动的角速度信号进行校正，并对校正后的载体运动的角速度信号进行积分，得航向角ψ；

2)三轴加速计获取载体的机动状态、横滚角及纵摇角；

3)由于跟踪信号与三轴加速计及三轴陀螺的更新频率不同，当当前无跟踪信息时，则将载体的横滚角及纵摇角作为观测量，当当前有跟踪信息时，步骤1)得到的航向角ψ作为观测量；

4)根据步骤1)得到的航向角ψ、步骤2)得到的载体的机动状态、横滚角及纵摇角、以及步骤3)得到的观测量通过四元数自适应卡尔曼滤波器估计出天线的状态变量。

步骤1)中根据天线的跟踪信息及载体运动的角速度信号ω_b计算航向角ψ的具体操作为：

根据天线的跟踪信息计算天线波束的指向偏差角，将天线波束的指向偏差角通过坐标变换得天线的航向角误差，再通过PI控制器对天线的航向角误差进行PI校正，然后再通过PI校正后的天线航向角误差对载体运动的角速度信号ω_b进行校正，再PI校正后的载体运动的角速度信号ω_b进行积分处理，得航向角ψ。

PI控制器的增益K_p及K_i为：

其中，为PI控制器的阻尼值。

通过载体的机动状态实时调整四元数自适应卡尔曼滤波器中的量测协方差矩阵R。

根据载体的机动状态判断载体是否存在外部加速度，当载体存在外部加速度时，则四元数自适应卡尔曼滤波器中的量测协方差矩阵R为：

其中，r_φ及r_θ分别为载体的横滚角及纵摇角对应的方差，r_ψ为航向角对应的方差。

通过载体的横滚角及纵摇角调整四元数自适应卡尔曼滤波器的增益。

根据步骤1)得到的航向角ψ、步骤2)得到的载体的机动状态、横滚角及纵摇角、以及步骤3)得到的观测量通过四元数自适应卡尔曼滤波器估计出天线的状态变量的具体操作为：

a)建立状态方程：

其中，X为天线的状态变量，系统噪声w为随机独立的零均值高斯白噪声序列。

状态变量X为：

其中，q＝[q₀ q₁ q₂ q₃]^T为姿态四元素，ω_bias＝[ω_{r_bias} ω_{p_bias} ω_{y_bias}]^T为陀螺的零偏估计误差。

系统状态转移矩阵为：

其中，

b)建立观测方程

Z＝h(X)+v

其中，Z＝[φ θ ψ]^T为量测变量，量测噪声v为随机独立的零均值高斯白噪声序列，即

系统量测转移矩阵为：

由于跟踪信息的更新频率比加速度计的慢很多，因而，将观测量相应分为两部分：一部分是利用加速度计敏感的重力分量求解载体的横滚角及纵摇角，另一部分为航向角信息，分别对应两个不同计算周期的量测转移矩阵H₁和H₂。

(1)量测转移矩阵H₁

三轴加速度计测得的是载体所受的比力f,设惯性测量组件中消噪后的三轴加速度计的测量值为f_x,f_y,f_z,当载体处于静止及准静态时，由于载体的速度较低,可以忽略哥氏加速度及其他干扰加速度的影响,在导航坐标系为“北东地”，载体坐标系为“前右下”情况下，可由三轴加速度计的测量值按照下式得到载体的姿态角：

φ＝-atan(f_y/f_z)

对应的量测转移矩阵：

(2)量测转移矩阵H₂

跟踪信息经过处理后可以得到天线波束的指向偏差角，通过变换后得到航向角误差，航向角误差经过一个PI控制器后校正陀螺值ω_bz，将校正后的陀螺值积分后，得航向角ψ，其中，PI控制器的增益K_p及K_i与截止频率ω和阻尼相关：

将航向角ψ作为卡尔曼的量测变量，得到量测转移矩阵：

载体的机动状态判别

将加速度值转变到导航坐标系，根据下面规则判断：

If(|a_nx|>δ_x)|(|a_ny|>δ_y)|(|a_nz-g|>δ_z)，认定载体存在外部加速度。

其中，a_nx、a_ny及a_nz为导航坐标系下的加速度值。

当载体存在外部加速度时，通过调整量测协方差矩阵R来改变四元数自适应卡尔曼滤波器的增益，此时，测协方差矩阵为：

四元数自适应卡尔曼滤波器的设计为：

a1)计算卡尔曼增益值

a2)更新估测值

a3)更新误差之矩阵

a4)预测参考状态误差

a5)估测误差矩阵

其中，当跟踪信息无效时，量测矩阵为H₁，测量值对应于Z₁，测量估计值为当跟踪信息有效时，量测矩阵为H₂，测量值对应于Z₂，测量估计值为

Claims (6)

1.一种动中通低成本组合姿态测量方法，其特征在于，包括以下步骤：

1)采集天线的跟踪信息，并通过三轴陀螺采集载体运动的角速度信号ω_b，再根据天线的跟踪信息计算天线的航向角误差，然后通过天线的航向角误差对载体运动的角速度信号进行校正，并对校正后的载体运动的角速度信号进行积分，得航向角ψ；

2)三轴加速计获取载体的机动状态、横滚角及纵摇角；

3)由于跟踪信号与三轴加速计及三轴陀螺的更新频率不同，当无跟踪信息时，则将载体的横滚角及纵摇角作为观测量，当有跟踪信息时，步骤1)得到的航向角ψ作为观测量；

4)根据步骤1)得到的航向角ψ、步骤2)得到的载体的机动状态、横滚角及纵摇角、以及步骤3)得到的观测量通过四元数自适应卡尔曼滤波器估计出天线的状态变量。

2.根据权利要求1所述的动中通低成本组合姿态测量方法，其特征在于，步骤1)中根据天线的跟踪信息及载体运动的角速度信号ω_b计算航向角ψ的具体操作为：

根据天线的跟踪信息计算天线波束的指向偏差角，将天线波束的指向偏差角通过坐标变换得天线的航向角误差，再通过PI控制器对天线的航向角误差进行PI校正，然后再通过PI校正后的天线航向角误差对载体运动的角速度信号ω_b进行校正，再将PI校正后的载体运动角速度信号ω_b进行积分处理，得航向角ψ。

3.根据权利要求2所述的动中通低成本组合姿态测量方法，其特征在于，PI控制器的增益K_p及K_i为：

其中，为PI控制器的阻尼值。

4.根据权利要求2所述的动中通低成本组合姿态测量方法，其特征在于，通过载体的机动状态实时调整四元数自适应卡尔曼滤波器中的量测协方差矩阵R。

5.根据权利要求4所述的动中通低成本组合姿态测量方法，其特征在于，根据载体的机动状态判断载体是否存在外部加速度，当载体存在外部加速度时，则四元数自适应卡尔曼滤波器中的量测协方差矩阵R为：

$R=\left[\begin{array}{ccc}100\×r_{\mathrm{\φ}}& 0& 0\\ 0& 1000\×r_{\mathrm{\θ}}& 0\\ 0& 0& r_{\mathrm{\ψ}}\end{array}\right]$

其中，r_φ及r_θ分别为载体的横滚角及纵摇角对应的方差，r_ψ为航向角对应的方差。

6.根据权利要求1所述的动中通低成本组合姿态测量方法，其特征在于，通过载体的横滚角及纵摇角调整四元数自适应卡尔曼滤波器的增益。