[发明专利]一种低成本单陀螺航位推算导航方法及系统

摘要

本发明针对传统的导航系统存在的成本较高、体积重量较大、航向误差积累等问题，提供了一种低成本单陀螺航位推算导航方法及系统，该系统利用航向感应传感器(如磁传感器，亦可为偏振光传感器)提供航向参考，采用1个光学陀螺(敏感轴指天向)与航向传感器组合提供高精度动态航向，两个加速度计负责测量载体姿态，一个里程计提供载体里程增量，利用航向、姿态、里程信息进行航位推算，从而计算载体位置，具有小体积、低功耗、低成本等诸多优点。

主权项

一种低成本单陀螺航位推算导航方法，其特征在于，该方法包括如下步骤：步骤S101，利用磁感应计与光学陀螺计算载体系x轴的航向角ψ；假设初始阶段，磁感应计受磁场环境扰动较小，则通过1分钟的静止状态，对磁感应计的航向角输出作平滑，得到初始航向角ψ0；采用光学陀螺辅助磁感应计进行航向角计算，利用光学陀螺输出进行航向角更新：ψi＝ψi‑1+Ωi‑1‑Ωiesin(L)cos(θ)     (1)其中，(i‑1)～i表示一个采样周期Ts，ψi为i时刻的航向角，Ωi‑1为(i‑1)～i时刻的陀螺输出，单位为弧度，Ωie为一个采样周期内的地球自转角度，L为当地纬度,θ为载体俯仰角；陀螺测量值存在漂移，会导致航向角误差随时间积累，利用磁感应计对陀螺的积累误差进行补偿，令ψG＝ψi表示i时刻陀螺解算的航向角，ψM表示磁感应计解算到的航向角；当满足如下条件时，利用ψM替换ψi以消除陀螺漂移等因素的航向角累计误差：条件1:tgyro＞T,其中tgyro为陀螺推算时间，T为某常值参数；条件2:公式(2)成立；(ψTsmoothG‾-ψTsmoothM‾)+Σi=1kδψi-δψi-1Tsk<J---(2)]]>其中表示一个平滑周期Tsmooth内的ψG均值，表示一个平滑周期内ψM均值；将一个平滑周期分为k个子周期，即Tsmooth＝k*Ts；δψi表示第i个子周期内的ψG与ψM差值，即J为航向角波动阈值；不等式左侧第一项反映磁感应计与陀螺解算航向角的常值偏差，第二项反映磁感应计受磁场影响的波动程度；当不等式成立时，表明磁感应计周围磁场无异常波动，可以用ψM替换ψi以消除陀螺漂移等因素的航向角累计误差；步骤S102，利用加速计A1计算载体俯仰角θ；加速度计A1计算载体俯仰角θ的公式如下，θ=arcsin(-a1-abg)---(3)]]>其中a1表示加速度计A1的测量输出量，g为重力加速度，定义载体车头上扬时θ角为正；ab的计算过程如公式5,6所示：ab＝x2                            (5)x2(k+1)＝x2(k)‑h*r*sat(g(k)，δ)            (6)其中：g(k)=x2(k)-sign(z1(k))r(h-ϵ|z1(k)|2+h2)2,|z1(k)|≥δ1x2(k)+z1(k)h,|z1(k)|≤δ1---(8)]]>z1(k)＝e(k)+hx2(k)                   (9)δ＝hr，δ1＝hδ                    (10)e(k)＝x1(k)‑v(k)     (11)x1(k+1)＝x1(k)+hx2(k)                    (12)x2(k)为加速度输出，h为采样步长，r为输入调节参数，依据实际信号特性人为选取，用来调节微分器性能，δ是中间变量，为平滑周期；v(k)为速度，x1(k)表示预测速度，v(k)为v的离散形式；e(k)为预测速度x1(k)与速度v(k)间的偏差；z1(k)为修正后的预测速度；由公式6‑12联立，进而通过ab＝x2，可以获得ab；最后由公式3获得载体俯仰角θ；步骤S103，利用里程计计算一个采样周期的里程增量Δs；一个采样周期的里程增量为：Δs＝KN                      (13)其中，脉冲数N为里程计一个采样周期内的脉冲数；K为里程系数，需要事先标定；步骤S104，计算载体在行驶过程中的位置；假设载体的初始坐标为(X0，Y0)，载体在行驶过程中，k时刻的坐标(Xk，Yk)可通过公式(14)得到：Xk=X0+Σi=1kcos(ψi)cos(θi)ΔsiYk=Y0+Σi=1ksin(ψi)cos(θi)Δsi---(14)]]>其中ψi、θi、Δsi分别为第i个采样周期的航向角、俯仰角、里程增量。