本发明专利技术涉及一种无陀螺仪的捷联式惯性导航系统的控制算法，采用磁传感器测量并计算出运载体的航向角 ，结合加速度计测量并计算出运载体的俯仰角和横滚角，对四元数姿态更新算法进行逆向递推，计算出运载体的角速度，以此替代陀螺仪，再经过双级串行PID控制器输出控制量。在同等的测量精度条件下，本系统的成本约为陀螺仪惯性导航系统的1/4，扩展惯性导航系统的应用领域，使其可以应用在消费电子等对成本要求较高的领域上。
【技术实现步骤摘要】
本专利技术涉及惯性导航系统的控制算法，尤其是一种无陀螺仪惯性导航系统的控制 算法。
技术介绍
惯性导航系统基本工作原理是以牛顿力学定律为基础，通过测量载体在载体坐标 系的加速度和角速度，并将它们对时间进行积分，之后将其变换到导航坐标系，得到在导航 坐标系中的速度、偏航角和姿态参数等。对于捷联式惯性导航系统，直接安装在载体上的惯 性测量装置测得载体坐标系的加速度和角速度，经过转换后便可得到导航坐标系的加速度 和角速度，有了已知方位的加速度和角速度之后，导航计算机便可根据相应的力学方程解 出要求的导航和姿态参数来。 传统的惯性导航系统均采用陀螺仪来测量载体的角速度，但是目前市面上常用的MEMS陀螺仪几乎被几家外国公司垄断，价位很高。此外，由于陀螺漂移随时间逐渐积累，惯 性导航系统长时间运行必将导致客观的积累误差。为了提高测量精度，需要使用其他的惯 性器件对系统进行校准，从而导致惯性导航系统成本高居不下，一定程度上限制了其应用 领域。
技术实现思路
本专利技术要解决的技术问题是提供一种惯性导航系统的控制算法，无需使用陀螺仪 对运载体的角速度进行测量，通过创新的算法根据运载体的姿态角计算出运载体的角速 度。 为了解决上述技术问题，本专利技术的技术方案为： ，包括以下步骤： 步骤1 :将重力加速度在地理坐标系t上的投影f1: 转换为载体坐标系b上的投 影_ :祕趙' 转换式^步骤2 :将.f1、jf、# :进行归一化处理，推导计算俯仰角和横滚角f，步骤3 :将地磁场在载体坐标系b上的投影^/纟转换为地理坐标系t上的投影，根据正交笛卡尔变换式可知，步骤4 :根据:_;、$變计算出航向角，再加上当地磁偏角，得到真实航向 角； 步骤5 :根据运载体的航向角痒、俯仰角#和横滚角7，推算出四元数步骤6 :根据四元数微分方程的变换式推算出角速度， 其4假设时刻的f:值为福，餐肘刻的值为斯，则，为保证角速度?的计算 精度，4的值须保证在lms以内，同时运载体的姿态角这、為、r的刷新频率必须保证在lKHz以上。 步骤7 :运载体的航向角％、俯仰角碑、横滚角f和角速度#输入PID控制器，输出 控制量。 优先的，为了计算出高精度的俯仰角#和横滚角r，对三轴加速度计测量的数据 进行零点偏置校准和卡尔曼滤波。 优先的，为了计算出高精度的航向角疼，对三轴磁传感器测量的数据进行三维椭 球拟合校准。 优先的，所述的PID控制器为双极串行PID控制器，提高惯性导航系统的响应速度 和稳定性。本专利技术采用磁传感器测量并计算出载体的航向角a，结合加速度计测量并计算出 的俯仰角#和横滚角:f:_:，对四元数姿态更新算法进行逆向递推，计算出载体的角速度#:，以 此替代陀螺仪。在同等的测量精度条件下，本系统的成本约为陀螺仪惯性导航系统的1/4， 扩展惯性导航系统的应用领域，使其可以应用在消费电子等对成本要求较高的领域上。【附图说明】 toon]图i是测量计算运载体俯仰角#和横滚角r的流程图。 图2是测量计算运载体航向角痒的流程图。 图3是推算运载体角速度?的流程图。 图4是双级串行PID控制系统流程图。【具体实施方式】 下面结合附图和具体实施例对本专利技术作进一步详细的说明。 ，包括以下步骤： 步骤1:将重力加速度在地理坐标系t上的投影丨|fgp转换为载体坐标系b上的投 影.gf，转换式戈步骤2 :将#、犮、#进行归一化处理，推导计算俯仰角和横滚角P，步骤3:将地磁场在载体坐标系b上的投影万/ 转换为地理坐标系 t上的投影:: ，根据正交笛卡尔变换式可知 步骤4:根据：_、_计算出航向角，再加上当地磁偏角，得到真实航向 角； 步骤5:根据运载体的航向角碑、俯仰角#和横滚角:r，推算出四元数步骤6 :根据四元数微分方程的变换式推算出角速度斯， 2 其中假设I对刻的脅値为:知:，I:时刻的皮:值为;_，则，为保证角速度爽的计算 精度，4的值须保证在lms以内，同时运载体的姿态角ir、濟、;T的刷新频率必须保证在lKHz以上。 步骤7:运载体的航向角体:、俯仰角,、横滚角r和角速度输入PID控制器，输出 控制量。 图1为为测量计算运载体俯仰角#和横滚角P的流程图，本专利技术采用加速度计来 测量运载体的重力加速度，由于一般惯性传感器都存在零点偏置误差，因此需要通过零点 偏置校准消除加速度计的零点偏置误差，提高测量精度。所谓零点偏置校准是指加速度计 在水平静止的情况下测大量值并求平均，该值即为零点偏置误差。 运载体在静止或匀速直线运动的情况下，没有运动加速度。假设重力加速度在 地理坐标系t上的投影为g, :gfT，在载体坐标系b上的投影为_:fp，则 可得转换;：，根据上述转换式可以推算出 然而在惯性导航系统中，载体是运动的，运载体会产生除了重力加速度以外的运 动加速度。该运动加速度对俯仰角和横滚角:r的计算精度会产生致命影响，因此必须想 办法来消除运载体的运动加速度。本专利采用一种改进型的卡尔曼滤波来消除移动物体的 运动加速度，其算法原型如下： X-Estimate(k) = X-Estimate(k_l); P-EstimateCovariance(k) = P-Covariance(k_l)+Q; Kg (k) = P_ EstimateCovariance (k) / ; X-Filter(k) = X-Estimate(k)+Kg(k)*; P-Covariance(k) = (l_Kg(k))木P- EstimateCovariance (k); X-Estimate(k) = X-Filter(k); 式中X_Estimate (k)是k时刻利用状态预测的估计值，X_Estimate (k-1)是k-1时刻最优的估计值；P_Cov当前第1页1 2 本文档来自技高网...
【技术保护点】
一种无陀螺仪惯性导航系统的控制算法，采用加速度计测量并计算出运载体的俯仰角和横滚角，其特征在于：采用磁传感器测量并计算出运载体的航向角，结合运载体的俯仰角和横滚角，对四元数姿态更新算法进行逆向递推，计算出运载体的角速度，以此替代陀螺仪，其中所述的加速度计为三轴加速度计，所述的磁传感器为三轴磁传感器。
【技术特征摘要】
【专利技术属性】
技术研发人员：周诚，廖红伟，皮强强，
申请(专利权)人：武汉光华芯科技有限公司，
类型：发明
国别省市：湖北;42
还没有人留言评论。发表了对其他浏览者有用的留言会获得科技券。