CN105953797B - 利用单轴陀螺仪、倾角仪和里程计的组合导航装置及方法 - Google Patents

Abstract

本发明专利公开了一种基于单轴陀螺仪、倾角仪和里程计的组合导航装置及方法，该装置包含单轴陀螺仪、倾角仪、里程计、空间投影变换模块、航位推算模块和卡尔曼滤波模块；单轴陀螺仪与倾角仪安装在同一载体平面，单轴陀螺仪提供自身转动的角速率，倾角仪提供俯仰角，通过空间投影变换算法，将陀螺仪在斜面上的转动角速率投影为水平面上的角速率。同时，将里程计提供的行进距离投影为平面距离；再利用航位推算算法，根据初始点的位置坐标及方位角，推算出当前坐标及方位角。本发明的优点是：***结构简单，算法简洁，实现了在无GNSS信号时的导航定位；同时，在达到一定精度的情况下降低了装备成本。

Description

利用单轴陀螺仪、倾角仪和里程计的组合导航装置及方法

技术领域

本发明专利涉及组合导航***及方法，特别地，涉及一种综合利用单轴陀螺仪、倾角仪和里程计的组合导航装置及方法。

背景技术

常规的车辆等载体定位通常采用GNSS(Global Navigation Satellite System，全球导航卫星***)卫星导航定位，在地面上这种方式能够取得很好的结果。但是在洞穴、巷道、地下采空区等环境中，无法接收到GNSS卫星信号时，通常会采用惯导导航方式。常规的惯导***需要三轴陀螺仪、加速度计等设备实现定位，但是机械陀螺仪精度较低，三轴光纤陀螺仪价格昂贵，基本上价格是单轴光纤陀螺仪的三倍左右。

中国专利CN201210096869.1，单轴陀螺仪和单轴加速度计的车载组合导航***及方法公开的定位***包括GNSS模块、单轴陀螺仪传感器、单轴加速度计传感器、DR模块、逻辑控制模块和卡尔曼滤波器，在GNSS信号有效时，GNSS模块与DR模块协同定位；当GNSS信号无效时，DR模块单独导航，算法非常复杂。中国专利CN20061011810209，利用GPS与陀螺仪、里程计的组合定位装置在GPS信号良好时，利用GPS信号进行定位，在GPS信号丢失时，切换至MEMS陀螺仪、里程计定位。中国专利CN201520358904.1，基于加速度计和陀螺仪的起重机运行行程和偏斜监测装置在采用三轴加速度传感器采集线性加速度信号，用MEMS三轴陀螺仪采集起重机运行偏斜的角速度信号；再对采集数值进行处理后，转化为相应的位移变化量和角速度值。

因此，为了在达到一定精度要求下尽可能的减少生产成本，市场需要一种在无法接收到GNSS卫星信号的情况下，成本更低且能满足低速运动载体的定位需求的导航设备。

发明专利内容

为了实现无GNSS信号时车辆载体等的导航定位，本发明提出了一种综合利用单轴陀螺仪、倾角仪和里程计的组合导航装置及方法，该方法算法简单，设备成本相对较低，能够满足低速运动载体的定位需求。

按照本发明提供的技术方案，一种基于单轴陀螺仪、倾角仪和里程计的组合导航装置，包括水平面角速度计算模块1、航位推算模块2、里程计3、起始点坐标及方位角模块4、卡尔曼滤波器5；

所述水平面角速度计算模块1包括单轴陀螺仪11、倾角仪12、空间投影变换模块13；空间投影变换模块13获取单轴陀螺仪11在任意平面上测得的单位时间内的旋转角度和倾角仪12测得的俯仰角，获得水平面上单位时间内旋转的角度；

所述空间投影变换模块13、所述起始点坐标及方位角模块4、所述里程计3均与航位推算模块2连接；航位推算模块2利用水平面角速度计算模块1获得的单位时间内水平面内旋转的角度、里程计3提供的里程信息以及起始点坐标及方位角模块4提供的起始点坐标及方位角，推算出当前点的位置坐标；

所述航位推算模块2与所述卡尔曼滤波器5连接。

优选的：所述单轴陀螺仪11为单轴光纤陀螺仪。

优选的：卡尔曼滤波器5对计算出的位置坐标进行平滑。

优选的：单轴陀螺仪11、倾角仪12安装在某一载体的平面上，且倾角仪的俯仰角转动轴垂直于载体纵轴线。

本申请还公开了利用上述的组合导航装置的组合导航方法，包括步骤：

空间投影变换模块13获取单轴陀螺仪11在任意平面上测得的单位时间内的旋转角度和倾角仪12测得的俯仰角，获得水平面上单位时间内旋转的角度；

航位推算模块2利用水平面角速度计算模块1获得的单位时间内水平面内旋转的角度、里程计3提供的里程信息以及起始点坐标及方位角模块4提供的起始点坐标及方位角，推算出当前点的位置坐标。

本申请综合利用单轴陀螺仪、倾角仪和里程计的组合导航装置包括单轴陀螺仪、倾角仪、里程计、空间投影变换、航位推算、卡尔曼滤波器等。单轴陀螺仪、倾角仪安装在车辆载体的某一水平面上，随载体做刚性运动。且倾角仪俯仰角的旋转轴垂直于载体平面的纵轴。里程计提供车辆的里程信息，即相对于初始位置行进了多少距离。

车辆载体作低速运动，无论是直线运动还是曲线运动，在足够小的时间间隔内行进路线均可以视为直线。单轴陀螺仪测量载体平面在该时间间隔内转过的角度，倾角仪提供载体的某一时刻的俯仰角。

本方案综合利用单轴陀螺仪、倾角仪和里程计的组合导航方法步骤如下：

第一步：获取前一时刻记录的载体俯仰角α₁和里程信息S₁。

第二步：获取当前时刻记录的载体俯仰角α₂和里程信息S₂。

第三步：通过单轴陀螺仪，获取时间间隔内载体平面转动的角度ω。通过空间投影变换，将载体在任意斜面转动的角度ω投影在水平面上，得到角度θ。

第四步：起始点平面坐标为(x₀,y₀,h₀)，起始方位角为A₀。可获得当前点坐标(x₁,y₁,h₁)及方位角A₁为：

A₁＝A₀+θ

x₁＝x₀+S*Cosα₂Cos A₁

y₁＝y₀+S*Cosα₂Sin A₁

h₁＝h₀+S*Sinα₂

第五步：重复上述步骤，不断获取载体的位置坐标及方位角。

与现有技术相比，本发明的有益效果是：

充分利用精度高的光纤陀螺仪，从而可以获得相对准确的设备水平姿态变化值，同时，利用倾角仪和里程计辅助，在没有GNSS卫星信号的情况下，准确获得当前位置坐标。整个算法相对简单，相应的设备造价相对较低。

附图说明

构成本申请的一部分的附图用来提供对本发明专利的进一步理解，本发明专利的示意性实施例及其说明用于解释本发明专利，并不构成对本发明专利的不当限定。在附图中：

图1是本发明申请的整体结构示意图；

图2是本发明申请的倾角仪俯仰角方向示意图；

图3是本发明公式推导过程中使用的示意图；

图4是图3的右视图；

图5是圆形切割成多边形的示意图；

图6是运动载体的轨迹示意图；

图7是运动载体的方向变化示意图；

其中，1、水平面角速度计算模块，11、单轴陀螺仪，12、倾角仪，13、空间投影变换模块，

2、航位推算模块，3、里程计，4、起始点坐标及方位角模块，5、卡尔曼滤波器；6、运动载体，7、车轮。

具体实施方式

以下结合附图对本发明专利的实施例进行详细说明，但是本发明专利可以根据权利要求限定和覆盖的多种不同方式实施。

实施例1：

一种综合利用单轴陀螺仪、倾角仪和里程计的组合导航装置，包括水平面角速度计算模块1、航位推算模块2、里程计3、起始点坐标及方位角模块4、卡尔曼滤波器5。水平面角速度计算模块1包括单轴陀螺仪11、倾角仪12、空间投影变换模块13；空间投影变换模块13获取单轴陀螺仪11在任意平面上测得的单位时间内的旋转角度和倾角仪12测得的俯仰角，获得水平面上单位时间内旋转的角度。

空间投影变换模块13、所述起始点坐标及方位角模块4、所述里程计3均与航位推算模块2连接；航位推算模块2利用水平面角速度计算模块1获得的单位时间内水平面内旋转的角度、里程计3提供的里程信息以及起始点坐标及方位角模块4提供的起始点坐标及方位角，推算出当前点的位置坐标。航位推算模块2与所述卡尔曼滤波器5连接。

单轴陀螺仪、倾角仪安装在车辆载体的某一水平面上，随运动载体6做刚性运动。且倾角仪12俯仰角的旋转轴垂直于运动载体6所在平面的纵轴，参见图2，倾角仪12的放置方向与运动载体6的车轮7所前进的方向一致，其俯仰角方向为图上箭头所示，以X轴为旋转轴，从纸面向纸面后方转动。里程计提供车辆的里程信息，即相对于初始位置行进了多少距离。

在起始点位置获取当前时刻的平面坐标为(x₀,y₀,h₀)，载体纵轴起始方位角为A₀。设置采样间隔为50Hz，在获取角速率、俯仰角、里程值时保证时间同步。

车辆载体作低速运动，无论是直线运动还是曲线运动，在足够小的时间间隔内行进路线均可以视为直线。

单轴陀螺仪可为单轴光纤陀螺仪。单轴光纤陀螺仪实时提供载体的角速率在时间间隔dt内，载体在任意斜面转动的角度对于该时间间隔，前一时刻获取的载体俯仰角为α₁，当前时刻的俯仰角为α₂，行进的斜面距离为S，对其组成的图形进行几何分析，通过空间投影变换，可以将载体在斜面上转动的角度变换到平面上转动的角度

当前点坐标(x₁,y₁,h₁)及方位角A₁可以通过下式计算得到：

A₁＝A₀+θ

x₁＝x₀+S*Cosα₂Cos A₁

y₁＝y₀+S*Cosα₂Sin A₁

h₁＝h₀+S*Sinα₂

下一时刻的位置坐标可以通过重复上述步骤得到。

以上公式的推导过程可参见图3和图4，以一个普遍的过程为例，前一时刻运动在位置在O点，行驶的方向为OA，下一时刻载体沿OB行驶到B点，由于当时间间隔尽可能小的时候载体的形式轨迹可以视为直线，因此以上假设可行。

OCD所在平面为水平面，OAB所在平面为斜面，OA长度设为S1，OB长度设为S2，∠AOC(设为α1)为载体在O点处的垂直倾角，由倾角仪测量，∠BOD(设为α2)为载体在B点时的垂直倾角，倾角仪测量。其中，C点是A点向水平面作垂线所得的焦点，D点同理。E点是在ABCD平面中，从B点向AC作垂线所得的交点。BE⊥AC。∠AOB(设为ω)是载体从O点运动到B点转动的角度，由陀螺仪测量，∠COD(设为θ)是ω在水平面上的投影，也是计算方位角是需要加上的角度。

由图3中几何关系，

AC＝S1*sinα1 BD＝S2*sinα2 OC＝S1*cosα1 OD＝S2*cosα1

由余弦定理

由图3关系可得

AE＝AC-BD

由勾股定理可得

CD²＝AB²-AE²

将上述关系式代入，即可得到

以上即为空间投影变换的公式推导。

在实际应用中，如果采用实际设备采集的数据，由于精度问题，无法验证算法的正确性，因此采用模拟数据。

例如：载体在水平面上运行一个圆周。常规的圆周是一个光滑的弧形，在实际运行中，可以将圆形尽可能多的切割成多边形，如图5。

当多边形边数足够多时，可以认为运动载体运行的轨迹是圆形。

如图6的轨迹，x轴指向北方向，y轴指向东方向，初始位置为原点，初始方位角为0°，指向北方向。假设圆形被切割成边数为20000的多边形，则多边形内角和为180*(20000-2)＝3599640°，每一个内角的补角为0.018°，从图7中可以看到，即载体运动中每一多边形边长转动的角度为0.018°。假设每一边长为1。

在起始点O，坐标为北东高(0,0,0)，方位角为0°，经过第一个时刻t运动到A处，由于在平面上，不需要空间投影变换，0.018°即为上一问题推导的θ。由下述公式，

A₁＝A₀+θ

x₁＝x₀+S*Cosα₂Cos A₁

y₁＝y₀+S*Cosα₂Sin A₁

h₁＝h₀+S*Sinα₂

A点处坐标为：

A1＝0+0.018＝0.018°

x1＝0+1*cos0*cos0.018＝0.999999

y1＝0+1*cos0*sin0.018＝0.00031416

h1＝0+1*sin0＝0

继续行进到B点：

A2＝0.018+0.018＝0.036°

x2＝0.999999+1*cos0*cos0.036＝1.9999999

y2＝0.00094248

h2＝0+1*sin0＝0

一直计算下去，最终能够回到O点，证明算法正确。

以上所述仅为本发明专利的优选实施例而已，并不用于限制本发明专利，对于本领域的技术人员来说，本发明专利可以有各种更改和变化。凡在本发明专利的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明专利的保护范围之内。

Claims (8)

1.一种基于单轴陀螺仪、倾角仪和里程计的组合导航装置，其特征在于：包括水平面角速度计算模块(1)、航位推算模块(2)、里程计(3)、起始点坐标及方位角模块(4)、卡尔曼滤波器(5)；

所述水平面角速度计算模块(1)包括单轴陀螺仪(11)、倾角仪(12)、空间投影变换模块(13)；空间投影变换模块(13)获取单轴陀螺仪(11)在任意平面上测得的单位时间内的旋转角度和倾角仪(12)测得的俯仰角，获得水平面上单位时间内旋转的角度；

所述空间投影变换模块(13)、所述起始点坐标及方位角模块(4)、所述里程计(3)均与航位推算模块(2)连接；航位推算模块(2)利用水平面角速度计算模块(1)获得的单位时间内水平面内旋转的角度、里程计(3)提供的里程信息以及起始点坐标及方位角模块(4)提供的起始点坐标及方位角，推算出当前点的位置坐标；

所述航位推算模块(2)与所述卡尔曼滤波器(5)连接。

2.根据权利要求1所述的一种基于单轴陀螺仪、倾角仪和里程计的组合导航装置，其特征在于：所述单轴陀螺仪(11)为单轴光纤陀螺仪。

3.根据权利要求1所述的一种基于单轴陀螺仪、倾角仪和里程计的组合导航装置，其特征在于：卡尔曼滤波器(5)对计算出的位置坐标进行平滑。

4.根据权利要求1所述的一种基于单轴陀螺仪、倾角仪和里程计的组合导航装置，其特征在于：单轴陀螺仪(11)、倾角仪(12)安装在某一载体的平面上，且倾角仪的俯仰角转动轴垂直于载体纵轴线。

5.根据权利要求1所述的一种基于单轴陀螺仪、倾角仪和里程计的组合导航装置，其特征在于：

载体运动过程中，单轴陀螺仪(11)在单位时间之间内转动的角度为ω；倾角仪(12)测量的单位时间前后载体的俯仰角分别为α₁，α₂；

根据空间投影变换模块(13)计算出单位时间内载体运动投影到水平面上转动的角度

6.根据权利要求1所述的一种基于单轴陀螺仪、倾角仪和里程计的组合导航装置，其特征在于：起始点平面坐标为(x₀,y₀,h₀)，起始方位角为A₀；由里程计(3)测量的单位时间内载体进行距离为S；由航位推算模块(2)可获得当前点坐标(x₁,y₁,h₁)及方位角A₁为：

7.利用权利要求1至6任一项所述的组合导航装置的组合导航方法，其特征在于，包括步骤：

空间投影变换模块(13)获取单轴陀螺仪(11)在任意平面上测得的单位时间内的旋转角度和倾角仪(12)测得的俯仰角，获得水平面上单位时间内旋转的角度；

航位推算模块(2)利用水平面角速度计算模块(1)获得的单位时间内水平面内旋转的角度、里程计(3)提供的里程信息以及起始点坐标及方位角模块(4)提供的起始点坐标及方位角，推算出当前点的位置坐标。

8.根据权利要求7所述的组合导航方法，其特征在于，步骤具体为：

第一步：获取前一时刻记录的载体俯仰角α₁和里程信息S₁；

第二步：获取当前时刻记录的载体俯仰角α₂和里程信息S₂；

第三步：通过单轴陀螺仪，获取时间间隔内载体平面转动的角度ω；通过空间投影变换，将载体在任意斜面转动的角度ω投影在水平面上，得到角度θ；

第四步：起始点平面坐标为(x₀,y₀,h₀)，起始方位角为A₀；可获得当前点坐标(x₁,y₁,h₁)及方位角A₁为：

第五步：重复上述步骤，不断获取载体的位置坐标及方位角。