[发明专利]一种无陀螺仪惯性导航系统的控制算法

说明书

技术领域

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

背景技术

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

传统的惯性导航系统均采用陀螺仪来测量载体的角速度，但是目前市面上常用的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控制器，输出控制量。