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

权利要求书

1.一种低成本单陀螺航位推算导航方法，其特征在于，该方法包括如下步骤：

步骤S101，利用磁感应计与光学陀螺计算载体系x轴的航向角ψ；

假设初始阶段，磁感应计受磁场环境扰动较小，则通过1分钟的静止状态，对磁感应计的航向角输出作平滑，得到初始航向角ψ₀；

采用光学陀螺辅助磁感应计进行航向角计算，利用光学陀螺输出进行航向角更新：

ψ_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:t_gyro＞T,其中t_gyro为陀螺推算时间，T为某常值参数；

条件2:公式(2)成立；

(ψTsmoothG‾-ψTsmoothM‾)+Σi=1kδψi-δψi-1Tsk<J---(2)]]>

其中表示一个平滑周期T_smooth内的ψ^G均值，表示一个平滑周期内ψ^M均值；将一个平滑周期分为k个子周期，即T_smooth＝k*Ts；δψ_i表示第i个子周期内的ψ^G与ψ^M差值，即J为航向角波动阈值；

不等式左侧第一项反映磁感应计与陀螺解算航向角的常值偏差，第二项反映磁感应计受磁场影响的波动程度；当不等式成立时，表明磁感应计周围磁场无异常波动，可以用ψ^M替换ψ_i以消除陀螺漂移等因素的航向角累计误差；

步骤S102，利用加速计A1计算载体俯仰角θ；

加速度计A1计算载体俯仰角θ的公式如下，

θ=arcsin(-a1-abg)---(3)]]>

其中a1表示加速度计A1的测量输出量，g为重力加速度，定义载体车头上扬时θ角为正；

a_b的计算过程如公式5,6所示：

a_b＝x₂(5)

x₂(k+1)＝x₂(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)]]>

z₁(k)＝e(k)+hx₂(k) (9)

δ＝hr，δ₁＝hδ(10)

e(k)＝x₁(k)-v(k) (11)

x₁(k+1)＝x₁(k)+hx₂(k)(12)

x₂(k)为加速度输出，h为采样步长，r为输入调节参数，依据实际信号特性人为选取，用来调节微分器性能，δ是中间变量，为平滑周期；v(k)为速度，x₁(k)表示预测速度，v(k)为v的离散形式；e(k)为预测速度x₁(k)与速度v(k)间的偏差；z₁(k)为修正后的预测速度；

由公式6-12联立，进而通过a_b＝x₂，可以获得a_b；

最后由公式3获得载体俯仰角θ；

步骤S103，利用里程计计算一个采样周期的里程增量Δs；

一个采样周期的里程增量为：

Δs＝KN(13)

其中，脉冲数N为里程计一个采样周期内的脉冲数；K为里程系数，需要事先标定；

步骤S104，计算载体在行驶过程中的位置；

假设载体的初始坐标为(X₀，Y₀)，载体在行驶过程中，k时刻的坐标(X_k，Y_k)可通过公式(14)得到：

Xk=X0+Σi=1kcos(ψi)cos(θi)ΔsiYk=Y0+Σi=1ksin(ψi)cos(θi)Δsi---(14)]]>

其中ψ_i、θ_i、Δs_i分别为第i个采样周期的航向角、俯仰角、里程增量。