CN105021193A - 一种无陀螺仪惯性导航系统的控制算法 - Google Patents

Abstract

本发明涉及一种无陀螺仪的捷联式惯性导航系统的控制算法，采用磁传感器测量并计算出运载体的航向角，结合加速度计测量并计算出运载体的俯仰角和横滚角，对四元数姿态更新算法进行逆向递推，计算出运载体的角速度，以此替代陀螺仪，再经过双级串行PID控制器输出控制量。在同等的测量精度条件下，本系统的成本约为陀螺仪惯性导航系统的1/4，扩展惯性导航系统的应用领域，使其可以应用在消费电子等对成本要求较高的领域上。

Description

一种无陀螺仪惯性导航系统的控制算法

技术领域

本发明涉及惯性导航系统的控制算法，尤其是一种无陀螺仪惯性导航系统的控制算法。

背景技术

惯性导航系统基本工作原理是以牛顿力学定律为基础，通过测量载体在载体坐标系的加速度和角速度，并将它们对时间进行积分，之后将其变换到导航坐标系，得到在导航坐标系中的速度、偏航角和姿态参数等。对于捷联式惯性导航系统，直接安装在载体上的惯性测量装置测得载体坐标系的加速度和角速度，经过转换后便可得到导航坐标系的加速度和角速度，有了已知方位的加速度和角速度之后，导航计算机便可根据相应的力学方程解出要求的导航和姿态参数来。

传统的惯性导航系统均采用陀螺仪来测量载体的角速度，但是目前市面上常用的MEMS陀螺仪几乎被几家外国公司垄断，价位很高。此外，由于陀螺漂移随时间逐渐积累，惯性导航系统长时间运行必将导致客观的积累误差。为了提高测量精度，需要使用其他的惯性器件对系统进行校准，从而导致惯性导航系统成本高居不下，一定程度上限制了其应用领域。

发明内容

本发明要解决的技术问题是提供一种惯性导航系统的控制算法，无需使用陀螺仪对运载体的角速度进行测量，通过创新的算法根据运载体的姿态角计算出运载体的角速度。

为了解决上述技术问题，本发明的技术方案为：

一种无陀螺仪惯性导航系统的控制算法，包括以下步骤：

步骤1：将重力加速度在地理坐标系t上的投影                                               转换为载体坐标系b上的投影，转换式为；

步骤2：将、、进行归一化处理，推导计算俯仰角和横滚角，，；

步骤3：将地磁场在载体坐标系b上的投影转换为地理坐标系t上的投影，根据正交笛卡尔变换式可知，；

步骤4：根据、计算出航向角，再加上当地磁偏角，得到真实航向角；

步骤5：根据运载体的航向角、俯仰角和横滚角，推算出四元数；

步骤6：根据四元数微分方程的变换式推算出角速度，

其中，

    假设时刻的值为，时刻的值为，则，为保证角速度的计算精度，的值须保证在1ms以内，同时运载体的姿态角、、的刷新频率必须保证在1KHz以上。

步骤7：运载体的航向角、俯仰角、横滚角和角速度输入PID控制器，输出控制量。

    优先的，为了计算出高精度的俯仰角和横滚角，对三轴加速度计测量的数据进行零点偏置校准和卡尔曼滤波。

优先的，为了计算出高精度的航向角，对三轴磁传感器测量的数据进行三维椭球拟合校准。

优先的，所述的PID控制器为双极串行PID控制器，提高惯性导航系统的响应速度和稳定性。

本发明采用磁传感器测量并计算出载体的航向角，结合加速度计测量并计算出的俯仰角和横滚角，对四元数姿态更新算法进行逆向递推，计算出载体的角速度，以此替代陀螺仪。在同等的测量精度条件下，本系统的成本约为陀螺仪惯性导航系统的1/4，扩展惯性导航系统的应用领域，使其可以应用在消费电子等对成本要求较高的领域上。

附图说明

图1是测量计算运载体俯仰角和横滚角的流程图。

图2是测量计算运载体航向角的流程图。

图3是推算运载体角速度的流程图。

图4是双级串行PID控制系统流程图。

具体实施方式

下面结合附图和具体实施例对本发明作进一步详细的说明。

一种无陀螺仪惯性导航系统的控制算法，包括以下步骤：

步骤1：将重力加速度在地理坐标系t上的投影转换为载体坐标系b上的投影，转换式为；

步骤2：将、、进行归一化处理，推导计算俯仰角和横滚角，，；

步骤3：将地磁场在载体坐标系b上的投影转换为地理坐标系t上的投影，根据正交笛卡尔变换式可知，；

步骤4：根据、计算出航向角，再加上当地磁偏角，得到真实航向角；

步骤5：根据运载体的航向角、俯仰角和横滚角，推算出四元数；

步骤6：根据四元数微分方程的变换式推算出角速度，

其中，

    假设时刻的值为，时刻的值为，则，为保证角速度的计算精度，的值须保证在1ms以内，同时运载体的姿态角、、的刷新频率必须保证在1KHz以上。

步骤7：运载体的航向角、俯仰角、横滚角和角速度输入PID控制器，输出控制量。

图1为为测量计算运载体俯仰角和横滚角的流程图，本发明采用加速度计来测量运载体的重力加速度，由于一般惯性传感器都存在零点偏置误差，因此需要通过零点偏置校准消除加速度计的零点偏置误差，提高测量精度。所谓零点偏置校准是指加速度计在水平静止的情况下测大量值并求平均，该值即为零点偏置误差。

运载体在静止或匀速直线运动的情况下，没有运动加速度。假设重力加速度在地理坐标系t上的投影为，在载体坐标系b上的投影为，则可得转换式，根据上述转换式可以推算出，。

然而在惯性导航系统中，载体是运动的，运载体会产生除了重力加速度以外的运动加速度。该运动加速度对俯仰角和横滚角的计算精度会产生致命影响，因此必须想办法来消除运载体的运动加速度。本专利采用一种改进型的卡尔曼滤波来消除移动物体的运动加速度，其算法原型如下：

    X_Estimate(k) = X_Estimate(k-1);

    P_EstimateCovariance(k) = P_Covariance(k-1)+Q;

    Kg(k) = P_ EstimateCovariance (k) / [P_ EstimateCovariance (k)+R];

    X_Filter(k) = X_Estimate(k)+Kg(k)*[X(k) - X_Estimate(k)];

P_Covariance(k) = (1-Kg(k))*P_ EstimateCovariance (k);

X_Estimate(k) = X_Filter(k);

    式中X_Estimate(k)是k时刻利用状态预测的估计值,X_Estimate(k-1)是k-1时刻最优的估计值；P_Covariance(k-1)是X_Estimate(k-1)对应的协方差，P_EstimateCovariance(k) 是k时刻的估计协方差，Q值是系统过程噪声的协方差；Kg(k)为卡尔曼增益，R为测量噪声的协方差；X(k)为k时刻的测量值， X_Filter(k)为k时刻的滤波值；P_Covariance(k)是X_Estimate(k)对应的协方差；其中系统过程噪声Q值和测量噪声R值对消除运载体的运动加速度影响很大，需要通过大量实验数据来确定合适的Q值和R值，否则计算出的俯仰角和横滚角的精度会大幅降低。

根据上述公式计算出的俯仰角范围为-180°～+180°，但横滚角的范围只在-90°～+90°，而且横滚角在-90°或+90°附近时会发生奇异。为解决上述问题，本发明对、、进行归一化处理，与此同时俯仰角和横滚角的计算公式变换为，。

图2为测量计算运载体航向角的流程图，本发明采用三轴磁传感器测量载体坐标系三个轴向的地磁场强度，进而计算出运载体的航向角。

在二维水平面上，通过磁传感器测量计算物体的航向角比较简单；但是在三维立体环境下，想要精确测量物体的航向角，计算复杂度成倍数上升。在理想情况下，假设物体周围除了地磁场以外没有任何其他磁场干扰，则只需要对三轴磁传感器进行倾斜补偿即可，倾斜补偿的精度在于俯仰角和横滚角的计算精度。然而现实情况是非理想的，物体周围除了地磁场外，还有固定的硬磁干扰和可变的软磁干扰，因此必须想办法来消除物体周围的各种磁场干扰。本发明主要采用三维椭球拟合算法来消除磁场干扰。

椭球拟合校准算法的核心是求解椭球曲面方程的参数，椭球曲面的一般方程为：

,式中，。

在求解椭球曲面方程参数时，采用最小二乘拟合算法，等价于测量地磁场点到对应的椭球面求最短距离的问题，其中。

为了简化计算量，利用拉格郎日乘数法化简求解方程，

其中

为了进一步减少计算量，将矩阵进行分割，令，，，；

其中；；

最后将椭球曲面一般方程的参数求解问题化简为一个普通的特征方程组，求解出最小正特征根对应的特征向量，得到确定的椭球方程。

大多数情况下磁传感器并不总是在水平面上，需要通过倾斜补偿算法对航向角进行计算。三轴磁传感器测量地磁场在载体坐标系b上的投影，假设地磁场在地理坐标系t上的投影为，根据正交笛卡尔变换式可知， ，进而计算出航向角，再加上当地磁偏角，得到真实航向角。

图3为推算运载体角速度的流程图，根据前面测量计算得到的运载体的姿态角推算出四元数，

   根据四元数微分方程的变换式推算出角速度，

其中，假设时刻的值为，时刻的值为，则，为保证角速度的计算精度，的值须保证在1ms以内，同时运载体的姿态角、、的刷新频率必须保证在1KHz以上。

    为了提高惯性导航系统的响应速度和稳定性，本发明采用一种双级串行PID控制器，如图4所示。在第一级控制中，对控制角和运载体的实时姿态角之差Angle_Error经过PI控制器得到PID_Angle，其目的在于提高系统的响应速度；在第二级控制中，对第一级控制的输出PID_Angle和运载体的实时角速度之差Rate_Error经过PID控制器，得到PID_Rate，其目的在于提高控制系统的稳定性。对于不同的惯性导航系统，将PID_Rate进行相应的处理，即可快速响应和高稳定性的惯性导航控制。

如上所述，本发明虽然主要阐述无陀螺仪时物体如何实现惯性导航，但本发明的保护范围并不局限于此，对只采用加速度计和磁传感器来实现物体的姿态角计算，以及由姿态角得到角速度的方法都应涵盖在本发明的保护范围之内。

Claims (5)

1.一种无陀螺仪惯性导航系统的控制算法，采用加速度计测量并计算出运载体的俯仰角                                               和横滚角，其特征在于：采用磁传感器测量并计算出运载体的航向角，结合运载体的俯仰角和横滚角，对四元数姿态更新算法进行逆向递推，计算出运载体的角速度，以此替代陀螺仪，其中所述的加速度计为三轴加速度计，所述的磁传感器为三轴磁传感器。

2.根据权利要求1所述无陀螺仪惯性导航系统的控制算法，其特征在于，包括以下步骤：

步骤1：将重力加速度在地理坐标系t上的投影转换为载体坐标系b上的投影；

步骤2：将、、进行归一化处理，推导计算俯仰角和横滚角；

步骤3：将地磁场在载体坐标系b上的投影转换为地理坐标系t上的投影；

步骤4：根据、、计算出航向角，再加上当地磁偏角，得到真实航向角；

步骤5：根据运载体的航向角、俯仰角和横滚角，推算出四元数；

步骤6：根据四元数微分方程的变换式推算出角速度；

步骤7：运载体的航向角、俯仰角、横滚角和角速度输入PID控制器，输出控制量。

3.根据权利要求2所述无陀螺仪惯性导航系统的控制算法，其特征在于：为了计算出高精度的俯仰角和横滚角，对三轴加速度计测量的数据进行零点偏置校准和卡尔曼滤波。

4.根据权利要求2所述无陀螺仪惯性导航系统的控制算法，其特征在于：为了计算出高精度的航向角，对三轴磁传感器测量的数据进行三维椭球拟合校准。

5.根据权利要求2所述无陀螺仪惯性导航系统的控制算法，其特征在于：所述的PID控制器为双极串行PID控制器，提高惯性导航系统的响应速度和稳定性。