CN104864874A - 一种低成本单陀螺航位推算导航方法及*** - Google Patents

Abstract

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

Description

一种低成本单陀螺航位推算导航方法及***

技术领域

本发明涉及一种单陀螺导航技术，具体涉及一种低成本单陀螺航位推算导航方法及***。

背景技术

在军、民领域中的各类设备上，惯性导航***作为一种现代化导航设备已被广泛应用。惯性导航***主要分为平台式惯性导航***和捷联式惯性导航***两大类。捷联惯性导航***和平台式惯性导航***一样，能精确提供载体的姿态、地速、经纬度等导航参数。

捷联惯性导航***有以下独特优点：去掉了复杂的平台机械***，***结构极为简单，减小了***的体积和重量，同时降低了成本，简化了维修，提高了可靠性。目前的导航***通常由3个陀螺、3个加速度计组成，虽然具有选配的灵活性，但是由于使用3个陀螺造成了成本、体积、重量等的增加。

因此，有必要研究出一种低成本单陀螺航位推算导航方法及***，从而解决现有技术的上述缺陷。

发明内容

针对捷联导航***存在的成本较高、航向误差积累、体积、重量较大等问题，本发明提供了一种低成本单陀螺航位推算导航方法及***，该***利用航向感应传感器(如磁传感器，亦可为偏振光传感器)提供航向参考，采用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)成立。

$\left(\stackrel{\&OverBar;}{{\mathrm{\&psi;}}_{T_{smooth}}^G}-\stackrel{\&OverBar;}{{\mathrm{\&psi;}}_{T_{smooth}}^M}\right)+\frac{{\&Sigma;}_{i=1}^k\frac{{\mathrm{\&delta;\&psi;}}_i-{\mathrm{\&delta;\&psi;}}_{i-1}}{Ts}}{k}<J---\left(2\right)$

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

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

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

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

$\mathrm{\&theta;}=\arcsin (-\frac{a1-a_b}{g})---\left(3\right)$

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

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

a_b＝x₂   (5)

x₂(k+1)＝x₂(k)-h*r*sat(g(k)，δ)   (6)

其中： $sat\left(x,\mathrm{\&delta;}\right)=\left\{\begin{array}{cc}sign\left(x\right),& \left|x\right|>\mathrm{\&delta;}\\ \frac{x}{g},& \left|x\right|\&le;\mathrm{\&delta;}\end{array}\right.---\left(7\right)$

$g\left(k\right)=\left\{\begin{array}{cc}{x}_2\left(k\right)-sign\left(z_1\left(k\right)\right)\frac{r\left(h-\sqrt{\frac{8\left|z_1\left(k\right)\right|}{2}+h^2}\right)}{2},& \left|z_1\left(k\right)\right|\&GreaterEqual;{\mathrm{\&delta;}}_1\\ x_2\left(k\right)+\frac{z_1\left(k\right)}{h},& \left|z_1\left(k\right)\right|\&le;{\mathrm{\&delta;}}_1\end{array}\right.---\left(8\right)$

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的离散形式；

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

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

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

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

Δs＝KN   (13)

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

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

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

$\left\{\begin{array}{c}{X}_k=X_0+{\&Sigma;}_{i=1}^k\cos \left({\mathrm{\&psi;}}_i\right)\cos \left({\mathrm{\&theta;}}_i\right){\mathrm{\&Delta;s}}_i\\ Y_k=Y_0+{\&Sigma;}_{i=1}^k\sin \left({\mathrm{\&psi;}}_i\right)\cos \left({\mathrm{\&theta;}}_i\right){\mathrm{\&Delta;s}}_i\end{array}\right.---\left(14\right)$

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

本申请还请求保护一种低成本单陀螺航位推算导航***，包括：

姿态测量单元，包括光学陀螺，磁感应计，两个加速度计，其中姿态测量单元坐标系选取前-左-上分别作为x-y-z；光学陀螺敏感轴与z轴重合指向天向；两个加速度计敏感轴分别指向x和y轴方向；磁感应计零位指向x轴方向；姿态测量单元将各个光学陀螺，磁感应计，两个加速度计的信号传输给信号采集电路；

信号采集电路，用于将接收的信号经过A/D转换，通过串行通信接口发送给处理器；

里程计，用于将载体轮胎的转动转换为脉冲，通过信号采集电路送给处理器；

处理器，接收信号采集电路发送的信号，得到载体的位置、速度、姿态，并计算载体的位置。

进一步的，处理器计算载体的位置是通过上述的导航方法获得的。

附图说明

图1为本发明的低成本单陀螺航位推算导航方法的流程示意图；

图2为本发明的导航***的结构示意图；

具体实施方式

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

如图1所示，本发明提出了一种低成本单陀螺航位推算导航方法，在该方法中以本地地理坐标系(东-北-天，E-N-U)作为导航坐标系(n系)，包括如下步骤：

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

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

磁感应计的输出易受磁场波动的影响，因此本发明采用光学陀螺辅助磁感应计进行航向角计算，利用光学陀螺输出进行航向角更新(公式(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)成立。

$\left(\stackrel{\&OverBar;}{{\mathrm{\&psi;}}_{T_{smooth}}^G}-\stackrel{\&OverBar;}{{\mathrm{\&psi;}}_{T_{smooth}}^M}\right)+\frac{{\&Sigma;}_{i=1}^k\frac{{\mathrm{\&delta;\&psi;}}_i-{\mathrm{\&delta;\&psi;}}_{i-1}}{Ts}}{k}<J---\left(2\right)$

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

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

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

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

$\mathrm{\&theta;}=\arcsin (-\frac{a1-a_b}{g})---\left(3\right)$

其中a1表示加速度计A1的测量输出量，g为重力加速度，定义载体车头上扬时θ角为正。载体有前向加速度a_b时，A1加速度输出为a1＝gsin(θ)+a_b,因此需要扣除加速度对俯仰角的影响。

本发明利用里程计信号经过二阶微分得到x轴向的加速度。里程计输出信号为一个采样周期的里程增量Δs，则速度为：

v＝Δs/Ts   (4)

若要得到载体前向加速度a_b，需要对速度v进行微分，而再次利用上式直接微分会进一步放大噪声。V(k)是v的离散形式。

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

a_b＝x₂   (5)

x₂(k+1)＝x₂(k)-h*r*sat(g(k)，δ)   (6)

其中： $sat\left(x,\mathrm{\&delta;}\right)=\left\{\begin{array}{cc}sign\left(x\right),& \left|x\right|>\mathrm{\&delta;}\\ \frac{x}{g},& \left|x\right|\&le;\mathrm{\&delta;}\end{array}\right.---\left(7\right)$

$g\left(k\right)=\left\{\begin{array}{cc}{x}_2\left(k\right)-sign\left(z_1\left(k\right)\right)\frac{r\left(h-\sqrt{\frac{8\left|z_1\left(k\right)\right|}{2}+h^2}\right)}{2},& \left|z_1\left(k\right)\right|\&GreaterEqual;{\mathrm{\&delta;}}_1\\ x_2\left(k\right)+\frac{z_1\left(k\right)}{h},& \left|z_1\left(k\right)\right|\&le;{\mathrm{\&delta;}}_1\end{array}\right.---\left(8\right)$

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的离散形式。

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

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

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

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

Δs＝KN   (13)

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

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

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

$\left\{\begin{array}{c}{X}_k=X_0+{\&Sigma;}_{i=1}^k\cos \left({\mathrm{\&psi;}}_i\right)\cos \left({\mathrm{\&theta;}}_i\right){\mathrm{\&Delta;s}}_i\\ Y_k=Y_0+{\&Sigma;}_{i=1}^k\sin \left({\mathrm{\&psi;}}_i\right)\cos \left({\mathrm{\&theta;}}_i\right){\mathrm{\&Delta;s}}_i\end{array}\right.---\left(14\right)$

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

下面通过图2对本发明的导航***作进一步的介绍。

如图2，低成本单陀螺航位推算导航***100包括：

姿态测量单元101，包括光学陀螺，磁感应计，两个加速度计，其中姿态测量单元坐标系选取前-左-上分别作为x-y-z；光学陀螺敏感轴与z轴重合指向天向；两个加速度计敏感轴分别指向x和y轴方向；磁感应计零位指向x轴方向；姿态测量单元将各个光学陀螺，磁感应计，两个加速度计的信号传输给信号采集电路102；

信号采集电路102，用于将接收的信号经过A/D转换，通过串行通信接口发送给处理器；

里程计103，用于将载体轮胎的转动转换为脉冲，通过信号采集电路送给处理器；

处理器104，接收信号采集电路发送的信号，得到载体的位置、速度、姿态，并计算载体的位置。

处理器计算载体的位置是通过权前述的导航方法获得的。

应当理解的是，本发明的应用不限于上述的举例，对本领域普通技术人员来说，可以根据上述说明加以改进或变换，所有这些改进和变换都应属于本发明所附权利要求的保护范围。

Claims (3)

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

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

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

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

ψ_i＝ψ_i-1+Ω_i-1-Ω_iθsin(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)成立。

$(\stackrel{\&OverBar;}{{\mathrm{\&psi;}}_{Tsmooth}^G}-\stackrel{\&OverBar;}{{\mathrm{\&psi;}}_{Tsmooth}^M})+\frac{{\&Sigma;}_{i=1}^k\frac{{\mathrm{\&delta;\&psi;}}_i-{\mathrm{\&delta;\&psi;}}_{i-1}}{Ts}}{k}<J---\left(2\right)$

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

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

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

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

$\mathrm{\&theta;}=arcsim(-\frac{a1-a_b}{g})---\left(3\right)$

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

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

a_b＝x₂   (5)

x₂(k+1)＝x₂(k)-h*r*sat(g(k)，δ)   (6)

其中： $sat(x,\mathrm{\&delta;})=\left\{\begin{array}{cc}sign\left(x\right),& \left|x\right|>\mathrm{\&delta;}\\ \frac{x}{\mathrm{\&delta;}},& \left|x\right|\&le;\mathrm{\&delta;}\end{array}\right.---\left(7\right)$

$g\left(k\right)=\left\{\begin{array}{cc}{x}_2\left(k\right)-sign\left(z_1\right(k\left)\right)\frac{r(h-\sqrt{\frac{s\left|z_1\left(k\right)\right|}{z}+h^z})}{2},& \left|z_1\left(k\right)\right|\&GreaterEqual;{\mathrm{\&delta;}}_1\\ x_2\left(k\right)+\frac{z_1\left(k\right)}{h},& \left|z_1\left(k\right)\right|\&GreaterEqual;{\mathrm{\&delta;}}_1\end{array}\right.---\left(8\right)$

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的离散形式；

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

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

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

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

Δs＝KN   (13)

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

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

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

$\{\begin{array}{c}{X}_k=X_0+{\&Sigma;}_{i=1}^k\cos \left({\mathrm{\&psi;}}_i\right)\cos \left({\mathrm{\&theta;}}_i\right){\mathrm{\&Delta;s}}_i\\ Y_k=Y_0+{\&Sigma;}_{i=1}^k\sin \left({\mathrm{\&psi;}}_i\right)\cos \left({\mathrm{\&theta;}}_i\right){\mathrm{\&Delta;s}}_i\end{array}---\left(14\right)$

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

2.一种低成本单陀螺航位推算导航***，其特征在于包括：

姿态测量单元，包括光学陀螺，磁感应计，两个加速度计，其中姿态测量单元坐标系选取前-左-上分别作为x-y-z；光学陀螺敏感轴与z轴重合指向天向；两个加速度计敏感轴分别指向x和y轴方向；磁感应计零位指向x轴方向；姿态测量单元将各个光学陀螺，磁感应计，两个加速度计的信号传输给信号采集电路；

信号采集电路，用于将接收的信号经过A/D转换，通过串行通信接口发送给处理器；

里程计，用于将载体轮胎的转动转换为脉冲，通过信号采集电路送给处理器；

处理器，接收信号采集电路发送的信号，得到载体的位置、速度、姿态，并计算载体的位置。

3.如权利要求2所述的***，其特征在，处理器计算载体的位置是通过权利要求1所述的导航方法获得的。