利用单轴陀螺仪、倾角仪和里程计的组合导航装置及方法
【专利摘要】本发明公开了一种基于单轴陀螺仪、倾角仪和里程计的组合导航装置及方法，该装置包含单轴陀螺仪、倾角仪、里程计、空间投影变换模块、航位推算模块和卡尔曼滤波模块；单轴陀螺仪与倾角仪安装在同一载体平面，单轴陀螺仪提供自身转动的角速率，倾角仪提供俯仰角，通过空间投影变换算法，将陀螺仪在斜面上的转动角速率投影为水平面上的角速率。同时，将里程计提供的行进距离投影为平面距离；再利用航位推算算法，根据初始点的位置坐标及方位角，推算出当前坐标及方位角。本发明的优点是：系统结构简单，算法简洁，实现了在无GNSS信号时的导航定位；同时，在达到一定精度的情况下降低了装备成本。
【专利说明】
利用单轴陀螺仪、倾角仪和里程计的组合导航装置及方法
技术领域
[0001] 本发明专利设及组合导航系统及方法，特别地，设及一种综合利用单轴巧螺仪、倾 角仪和里程计的组合导航装置及方法。
【背景技术】
[0002] 常规的车辆等载体定位通常采用GNSS(Global化vigation Satellite System, 全球导航卫星系统)卫星导航定位，在地面上运种方式能够取得很好的结果。但是在桐穴、 巷道、地下采空区等环境中，无法接收到GNSS卫星信号时，通常会采用惯导导航方式。常规 的惯导系统需要=轴巧螺仪、加速度计等设备实现定位，但是机械巧螺仪精度较低，=轴光 纤巧螺仪价格昂贵，基本上价格是单轴光纤巧螺仪的=倍左右。
[0003] 中国专利CN201210096869.1，单轴巧螺仪和单轴加速度计的车载组合导航系统及 方法公开的定位系统包括GNSS模块、单轴巧螺仪传感器、单轴加速度计传感器、DR模块、逻 辑控制模块和卡尔曼滤波器，在GNSS信号有效时，GNSS模块与D財莫块协同定位；当GNSS信号 无效时，DR模块单独导航，算法非常复杂。中国专利CN20061011810209，利用GI^与巧螺仪、 里程计的组合定位装置在GPS信号良好时，利用GPS信号进行定位，在GPS信号丢失时，切换 至MEMS巧螺仪、里程计定位。中国专利CN201520358904.1，基于加速度计和巧螺仪的起重机 运行行程和偏斜监测装置在采用=轴加速度传感器采集线性加速度信号，用MEMS =轴巧螺 仪采集起重机运行偏斜的角速度信号;再对采集数值进行处理后，转化为相应的位移变化 量和角速度值。
[0004] 因此，为了在达到一定精度要求下尽可能的减少生产成本，市场需要一种在无法 接收到GNSS卫星信号的情况下，成本更低且能满足低速运动载体的定位需求的导航设备。
[0005] 发明专利内容
[0006] 为了实现无 GNSS信号时车辆载体等的导航定位，本发明提出了一种综合利用单轴 巧螺仪、倾角仪和里程计的组合导航装置及方法，该方法算法简单，设备成本相对较低，能 够满足低速运动载体的定位需求。
[0007] 按照本发明提供的技术方案，一种基于单轴巧螺仪、倾角仪和里程计的组合导航 装置，包括水平面角速度计算模块1、航位推算模块2、里程计3、起始点坐标及方位角模块4、 卡尔曼滤波器5;
[000引所述水平面角速率计算模块1包括单轴巧螺仪11、倾角仪12、空间投影变换模块 13;空间投影变换模块13获取单轴巧螺仪11在任意平面上测得的单位时间内的旋转角度和 倾角仪12测得的俯仰角，获得水平面上单位时间内旋转的角度；
[0009] 所述空间投影变换模块13、所述起始点坐标及方位角模块4、所述里程计3均与航 位推算模块2连接;航位推算模块2利用水平面角速率计算模块1获得的单位时间内水平面 内旋转的角度、里程计3提供的里程信息W及起始点坐标及方位角模块4提供的起始点坐标 及方位角，推算出当前点的位置坐标；
[0010] 所述航位推算模块2与所述卡尔曼滤波器5连接。
[0011] 优选的:所述单轴巧螺仪11为单轴光纤巧螺仪。
[0012] 优选的：卡尔曼滤波器5对计算出的位置坐标进行平滑。
[0013] 优选的：单轴巧螺仪11、倾角仪12安装在某一载体的平面上，且倾角仪的俯仰角转 动轴垂直于载体纵轴线。
[0014] 本申请还公开了利用上述的组合导航装置的组合导航方法，包括步骤：
[0015] 空间投影变换模块13获取单轴巧螺仪11在任意平面上测得的单位时间内的旋转 角度和倾角仪12测得的俯仰角，获得水平面上单位时间内旋转的角度；
[0016] 航位推算模块2利用水平面角速率计算模块1获得的单位时间内水平面内旋转的 角度、里程计3提供的里程信息W及起始点坐标及方位角模块4提供的起始点坐标及方位 角，推算出当前点的位置坐标。
[0017] 本申请综合利用单轴巧螺仪、倾角仪和里程计的组合导航装置包括单轴巧螺仪、 倾角仪、里程计、空间投影变换、航位推算、卡尔曼滤波器等。单轴巧螺仪、倾角仪安装在车 辆载体的某一水平面上，随载体做刚性运动。且倾角仪俯仰角的旋转轴垂直于载体平面的 纵轴。里程计提供车辆的里程信息，即相对于初始位置行进了多少距离。
[0018] 车辆载体作低速运动，无论是直线运动还是曲线运动，在足够小的时间间隔内行 进路线均可W视为直线。单轴巧螺仪测量载体平面在该时间间隔内转过的角度，倾角仪提 供载体的某一时刻的俯仰角。
[0019] 本方案综合利用单轴巧螺仪、倾角仪和里程计的组合导航方法步骤如下：
[0020] 第一步:获取前一时刻记录的载体俯仰角ai和里程信息&。
[0021] 第二步:获取当前时刻记录的载体俯仰角02和里程信息S2。
[0022] 第=步:通过单轴巧螺仪，获取时间间隔内载体平面转动的角度O。通过空间投影 变换，将载体在任意斜面转动的角度《投影在水平面上，得到角度0。
[0023]
[0024] 第四步:起始点平面坐标为^〇,7〇，11〇)，起始方位角为4〇。可获得当前点坐标^1， yi, hi)及方位角Al为：
[0025] Al = Ao+白
[0026] xi = xo+S*Cosa2Cos Al
[0027] yi = y〇+S*Cosa2Sin Al [002引 hi = ho+S 巧 ina2
[0029] 第五步:重复上述步骤，不断获取载体的位置坐标及方位角。
[0030] 与现有技术相比，本发明的有益效果是：
[0031] 充分利用精度高的光纤巧螺仪，从而可W获得相对准确的设备水平姿态变化值， 同时，利用倾角仪和里程计辅助，在没有GNSS卫星信号的情况下，准确获得当前位置坐标。 整个算法相对简单，相应的设备造价相对较低。
【附图说明】
[0032] 构成本申请的一部分的附图用来提供对本发明专利的进一步理解，本发明专利的 示意性实施例及其说明用于解释本发明专利，并不构成对本发明专利的不当限定。在附图 中：
[0033] 图I是本发明申请的整体结构示意图；
[0034] 图2是本发明申请的倾角仪俯仰角方向示意图；
[0035] 图3是本发明公式推导过程中使用的示意图；
[0036] 图4是图3的右视图；
[0037] 图5是圆形切割成多边形的示意图；
[0038] 图6是运动载体的轨迹示意图；
[0039] 图7是运动载体的方向变化示意图；
[0040] 其中，1、水平面角速度计算模块，11、单轴巧螺仪，12、倾角仪，13、空间投影变换模 块，
[0041] 2、航位推算模块，3、里程计，4、起始点坐标及方位角模块，5、卡尔曼滤波器；
[0042] 6、运动载体，7、车轮。
【具体实施方式】
[0043] W下结合附图对本发明专利的实施例进行详细说明，但是本发明专利可W根据权 利要求限定和覆盖的多种不同方式实施。
[0044] 实施例1:
[0045] -种综合利用单轴巧螺仪、倾角仪和里程计的组合导航装置，包括水平面角速度 计算模块1、航位推算模块2、里程计3、起始点坐标及方位角模块4、卡尔曼滤波器5。水平面 角速率计算模块1包括单轴巧螺仪11、倾角仪12、空间投影变换模块13;空间投影变换模块 13获取单轴巧螺仪11在任意平面上测得的单位时间内的旋转角度和倾角仪12测得的俯仰 角，获得水平面上单位时间内旋转的角度。
[0046] 空间投影变换模块13、所述起始点坐标及方位角模块4、所述里程计3均与航位推 算模块2连接;航位推算模块2利用水平面角速率计算模块1获得的单位时间内水平面内旋 转的角度、里程计3提供的里程信息W及起始点坐标及方位角模块4提供的起始点坐标及方 位角，推算出当前点的位置坐标。航位推算模块2与所述卡尔曼滤波器5连接。
[0047] 单轴巧螺仪、倾角仪安装在车辆载体的某一水平面上，随运动载体6做刚性运动。 且倾角仪12俯仰角的旋转轴垂直于运动载体6所在平面的纵轴，参见图2,倾角仪12的放置 方向与运动载体6的车轮7所前进的方向一致，其俯仰角方向为图上箭头所示，WX轴为旋转 轴，从纸面向纸面后方转动。里程计提供车辆的里程信息，即相对于初始位置行进了多少距 离。
[0048] 在起始点位置获取当前时刻的平面坐标为^〇,7〇，11〇)，载体纵轴起始方位角为八〇。 设置采样间隔为50化，在获取角速率、俯仰角、里程值时保证时间同步。
[0049] 车辆载体作低速运动，无论是直线运动还是曲线运动，在足够小的时间间隔内行 进路线均可W视为直线。
[0050] 单轴巧螺仪可为单轴光纤巧螺仪。单轴光纤巧螺仪实时提供载体的角速率9^，在时 间间隔化内，载体在任意斜面转动的角度仿=界*姑，对于该时间间隔，前一时刻获取的载 体俯仰角为〇1，当前时刻的俯仰角为02,行进的斜面距离为S，对其组成的图形进行几何分 析，通过空间投影变换，可W将载体在斜面上转动的角度变换到平面上转动的角度
[0化1]当前点坐标(xi，yi，hi)及方位角Al可W通过下式计算得到：
[0化2] Al = Ao+白
[0053] xi = xo+S*Cosa2Cos Al
[0054] yi = y〇+S*Cosa2Sin Al [0化5] hi = ho+S 巧 ina2
[0056] 下一时刻的位置坐标可W通过重复上述步骤得到。
[0057] W上公式的推导过程可参见图3和图4, W-个普遍的过程为例，前一时刻运动在 位置在0点，行驶的方向为0A，下一时刻载体沿OB行驶到B点，由于当时间间隔尽可能小的时 候载体的形式轨迹可W视为直线，因此W上假设可行。
[005引 OCD所在平面为水平面，OAB所在平面为斜面，OA长度设为Sl，OB长度设为S2，ZAOC (设为a 1)为载体在0点处的垂直倾角，由倾角仪测量，ZBOD(设为a2)为载体在B点时的垂直 倾角，倾角仪测量。其中，C点是A点向水平面作垂线所得的焦点，D点同理。E点是在ABCD平 面中，从B点向AC作垂线所得的交点。BE丄ACdZAOB(设为W )是载体从0点运动到B点转动的 角度，由巧螺仪测量，ZCOD(设为0)是CO在水平面上的投影，也是计算方位角是需要加上的 角度。
[0化9]由图3中几何关系，
[0060] AC = Sl*sinal BD = S2*sina2 0C = Sl*cosal 〇D = S2*cosal
[0061 ] 由余弦定理
[0062]
[0063] 由图3关系可得
[0064] AE=AC-BD [00化]由勾股定理可得
[0066] CD2 = AB2-AE2
[0067] 将上述关系式代入，即可得到 [006引
[0069] W上即为空间投影变换的公式推导。
[0070] 在实际应用中，如果采用实际设备采集的数据，由于精度问题，无法验证算法的正 确性，因此采用模拟数据。
[0071] 例如:载体在水平面上运行一个圆周。常规的圆周是一个光滑的弧形，在实际运行 中，可W将圆形尽可能多的切割成多边形，如图5。
[0072] 当多边形边数足够多时，可W认为运动载体运行的轨迹是圆形。
[0073] 如图6的轨迹，X轴指向北方向，y轴指向东方向，初始位置为原点，初始方位角为 0%指向北方向。假设圆形被切割成边数为20000的多边形，则多边形内角和为180*(20000- 2) = 3599640°，每一个内角的补角为0.018°，从图7中可W看到，即载体运动中每一多边形 边长转动的角度为0.018°。假设每一边长为1。
[0074] 在起始点0,坐标为北东高(0,0,0)，方位角为0°，经过第一个时刻t运动到A处，由 于在平面上，不需要空间投影变换，0.018°即为上一问题推导的0。由下述公式，
[0075] Al = Ao+白
[0076] xi = xo+S*Cosa2Cos Ai
[0077] yi = y〇+S*Cosa2Sin Al [007引 hi = ho+S 巧 ina2
[0079] A点处坐标为：
[0080] Al=0+0.018 = 0.018。
[0081 ] xl =0+l*cos0*cos0.018 = 0.999999
[0082] yl=0+l*cos0*sin0.018 = 0.00031416
[0083] hi =0+l*sin0 = 0
[0084] 继续行进到B点：
[0085] A2 = 0.018+0.018 = 0.036。
[0086] x2 = 0.999999+l*cos0*cos0.036 = 1.9999999
[0087] y2 = 0.00094248
[0088] h2 = 0+l*sin0 = 0
[0089] 一直计算下去，最终能够回到0点，证明算法正确。
[0090] W上所述仅为本发明专利的优选实施例而已，并不用于限制本发明专利，对于本 领域的技术人员来说，本发明专利可W有各种更改和变化。凡在本发明专利的精神和原则 之内，所作的任何修改、等同替换、改进等，均应包含在本发明专利的保护范围之内。
【主权项】
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)测量的 单位时间前后载体的俯仰角分别为α 1，α2; 根据空间投影变换（13)计算出单位时间内载体运动投影到水平面上转动的角度6. 根据|乂利妥豕1所还的一柙单轴陀螺仪、倾角仪和里程计的组合导航装置，其特 征在于:起始点平面坐标为(xq，yo，ho)，起始方位角为Αο;由里程计(3)测量的单位时间内载 体进行距离为S;由航位推算模块(2)可获得当前点坐标( xl，yi，h〇及方位角^为：7. 利用权利要求1至6任一项所述的组合导航装置的组合导航方法，其特征在于，包括 步骤： 空间投影变换模块（13)获取单轴陀螺仪（11)在任意平面上测得的单位时间内的旋转 角度和倾角仪(12)测得的俯仰角，获得水平面上单位时间内旋转的角度； 航位推算模块(2)利用水平面角速率计算模块（1)获得的单位时间内水平面内旋转的 角度、里程计(3)提供的里程信息以及起始点坐标及方位角模块(4)提供的起始点坐标及方 位角，推算出当前点的位置坐标。8.根据权利要求7所述的组合导航方法，其特征在于，步骤具体为： 第一步:获取前一时刻记录的载体俯仰角^和里程信息Si; 第二步:获取当前时刻记录的载体俯仰角α2和里程信息S2; 第三步：通过单轴陀螺仪，获取时间间隔内载体平面转动的角度ω ;通过空间投影变 换，将载体在任意斜面转动的角度ω投影在水平面上，得到角度Θ ;第四步:起始点平面坐标为(x〇，y〇,ho)，起始方位角为Αο;可获得当前点坐标(XI，yi,hi) 及方位角Αι为：第五步:重复上述步骤，不断获取载体的位置坐标及方位角。
【文档编号】G01C21/16GK105953797SQ201610564309
【公开日】2016年9月21日
【申请日】2016年7月16日
【发明人】谢翔, 杜年春, 金俊
【申请人】中国有色金属长沙勘察设计研究院有限公司