CN114322970A - 一种双陀螺仪寻北方法、系统及存储介质 - Google Patents

Abstract

本发明涉及自主定向定位技术领域，公开了一种双陀螺仪寻北方法、系统及存储介质，该寻北方法包括建立载体坐标系，并建立地理坐标系；根据载体坐标系的各坐标轴确定第一光纤陀螺仪、第二光纤陀螺仪、第一加速度计以及第二加速度计的指向；获取根据第一光纤陀螺仪和第二光纤陀螺仪的输出信息确定的天向陀螺输出信息；获取根据第一加速度计和第二加速度计的输出信息确定的水平姿态角信息；根据天向陀螺输出信息确定姿态关系，其中，姿态关系为分离出纬度信息的姿态关系；并根据水平姿态角信息和姿态关系确定载体坐标系相对于地理坐标系的目标方位角。可以使寻北系统的使用更加便利，应用广。

Description

一种双陀螺仪寻北方法、系统及存储介质

技术领域

本发明涉及自主定向定位技术领域，尤其涉及一种双陀螺仪寻北方法、系统及存储介质。

背景技术

惯性寻北技术是一种完全不依赖外界信息的定向技术，通过加速度计与陀螺的输出数据解算便可以得到载体指向与地理真北偏差角。但是，目前大多数的陀螺寻北系统结构设计较为复杂，往往需要在硬件系统中设计一套精密转位机构，从而使得陀螺仪与加速度计能够转动90°或180°，这样，不仅造成结构复杂，而且要求寻北系统处于一个理论水平的位置，进而导致寻北设备的成本与体积变大，甚至会使得陀螺寻北的时间加长，使得寻北仪的使用过程繁琐，难以适用特殊场景快速性的应用需求。可见，现有的寻北仪结构复杂，存在使用繁琐的问题。

发明内容

本发明提供了一种双陀螺仪寻北方法、系统及存储介质，以解决现有的寻北仪结构复杂，存在使用繁琐的问题。

为了实现上述目的，本发明通过如下的技术方案来实现：

第一方面，本发明提供一种双陀螺仪寻北方法，应用于双陀螺仪寻北系统，所述双陀螺仪寻北系统包括基座平台，以及设于所述基座平台上的第一探测装置和第二探测装置，所述第一探测装置和所述第二探测装置之间呈90°设置，所述第一探测装置包括第一光纤陀螺仪和第一加速度计，所述第二探测装置包括第二光纤陀螺仪和第二加速度计，所述方法包括：

建立载体坐标系，并建立地理坐标系；

根据所述载体坐标系的各坐标轴确定所述第一光纤陀螺仪、所述第二光纤陀螺仪、所述第一加速度计以及所述第二加速度计的指向；

获取根据所述第一光纤陀螺仪和所述第二光纤陀螺仪的输出信息确定的天向陀螺输出信息；

获取根据所述第一加速度计和所述第二加速度计的输出信息确定的水平姿态角信息；

根据所述天向陀螺输出信息确定姿态关系，所述姿态关系为分离出纬度信息的姿态关系；

并根据所述水平姿态角信息和所述姿态关系确定所述载体坐标系相对于所述地理坐标系的目标方位角。

可选地，所述建立载体坐标系，包括：

以所述双陀螺仪寻北系统的重心为原点建立载体坐标系，所述载体坐标系包括第一载体坐标轴、第二载体坐标轴以及第三载体坐标轴，所述第一载体坐标轴与所述第二载体坐标轴位于同一平面内，且所述第一载体坐标轴与所述第二载体坐标轴垂直，且所述第一载体坐标轴和所述第二载体坐标轴均与所述第三载体坐标轴垂直。

可选地，所述建立地理坐标系，包括：

以所述双陀螺仪寻北系统的重心为原点建立地理坐标系，所述地理坐标系包括第一地理坐标轴、第二地理坐标轴以及第三地理坐标轴，所述第一地理坐标轴指向地理东向，所述第二地理坐标轴指向地理北向，所述第三地理坐标轴垂直于当地旋转椭球面指向天向。

可选地，所述根据所述载体坐标系的各坐标轴确定所述第一光纤陀螺仪、所述第二光纤陀螺仪、所述第一加速度计以及所述第二加速度计的指向，包括：

使第一光纤陀螺仪和第二光纤陀螺仪的X轴均指向所述第一载体坐标轴的方向，第一光纤陀螺仪和第二光纤陀螺仪的Y轴均指向所述第二载体坐标轴的方向，其中，第一加速度计的指向与所述第一光纤陀螺仪的指向一致，第二加速度计的指向与所述第二光纤陀螺仪的指向一致。

可选地，所述确定的天向陀螺输出信息，满足如下关系式：

式中，ω_z为天向陀螺输出信息值，ω_ie为地球自转角速率，ω_x为地球自转矢量在当地水平方向上的第一输出分量，ω_y为地球自转矢量在当地水平方向上的第二输出分量。

可选地，所述获取根据所述第一加速度计和所述第二加速度计的输出信息确定的水平姿态角信息，包括：

在重力加速g已知的情况下，通过第一加速度计和第二加速度计的测量值计算出俯仰角θ与滚转角γ相关的三角函数，如下：

式中，A_x为第一加速度计的输出值，A_y为第二加速度计的输出值，g为当地重力加速度，θ为寻北系统此时的俯仰角，γ为寻北系统此时的滚转角。

可选地，所述根据所述天向陀螺输出信息确定姿态关系，如下：

式中，

为单位正交矩阵，φ为纬度。

第二方面，本申请还提供一种双陀螺仪寻北系统，包括基座平台，以及设于所述基座平台上的第一探测装置和第二探测装置，所述第一探测装置和所述第二探测装置之间呈90°设置，所述第一探测装置包括第一光纤陀螺仪和第一加速度计，所述第二探测装置包括第二光纤陀螺仪和第二加速度计。

第三方面，本申请还提供一种计算机可读存储介质，其上存储有计算机程序，该程序被处理器执行时实现如第一方面所述的方法步骤。

有益效果：

本发明提供的双陀螺仪寻北方法，首先建立载体坐标系，并建立地理坐标系；根据载体坐标系的各坐标轴确定第一光纤陀螺仪、第二光纤陀螺仪、第一加速度计以及第二加速度计的指向；获取根据第一光纤陀螺仪和第二光纤陀螺仪的输出信息确定的天向陀螺输出信息；获取根据第一加速度计和第二加速度计的输出信息确定的水平姿态角信息；根据天向陀螺输出信息确定姿态关系，其中，姿态关系为分离出纬度信息的姿态关系；并根据水平姿态角信息和姿态关系确定载体坐标系相对于地理坐标系的目标方位角。这样，利用两个水平向的加速度计计算出水平姿态角信息，利用水平陀螺仪推导理论上的天向陀螺输出，将姿态角计算结果带入到分离出纬度信息的姿态关系中便可以计算出载体的方位角，此过程中不需要给出当地经纬度信息以及不要求使用过程中寻北系统处于水平位置，可以使寻北系统的使用更加便利，应用广。

附图说明

图1为本发明优选实施例的一种双陀螺仪寻北方法的流程图；

图2为本发明优选实施例的载体坐标系示意图。

具体实施方式

下面对本发明的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有作出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。

除非另作定义，本发明中使用的技术术语或者科学术语应当为本发明所属领域内具有一般技能的人士所理解的通常意义。本发明中使用的“第一”、“第二”以及类似的词语并不表示任何顺序、数量或者重要性，而只是用来区分不同的组成部分。同样，“一个”或者“一”等类似词语也不表示数量限制，而是表示存在至少一个。“连接”或者“相连”等类似的词语并非限定于物理的或者机械的连接，而是可以包括电性的连接，不管是直接的还是间接的。“上”、“下”、“左”、“右”等仅用于表示相对位置关系，当被描述对象的绝对位置改变后，则该相对位置关系也相应地改变。

请参见图1，本申请实施例提供一种双陀螺仪寻北方法，应用于双陀螺仪寻北系统，双陀螺仪寻北系统包括基座平台，以及设于基座平台上的第一探测装置和第二探测装置，第一探测装置和第二探测装置之间呈90°设置，第一探测装置包括第一光纤陀螺仪和第一加速度计，第二探测装置包括第二光纤陀螺仪和第二加速度计，方法包括：

建立载体坐标系，并建立地理坐标系；

根据载体坐标系的各坐标轴确定第一光纤陀螺仪、第二光纤陀螺仪、第一加速度计以及第二加速度计的指向；

获取根据第一光纤陀螺仪和第二光纤陀螺仪的输出信息确定的天向陀螺输出信息；

获取根据第一加速度计和第二加速度计的输出信息确定的水平姿态角信息；

根据天向陀螺输出信息确定姿态关系，姿态关系为分离出纬度信息的姿态关系；

并根据水平姿态角信息和姿态关系确定载体坐标系相对于地理坐标系的目标方位角。

上述的双陀螺仪寻北方法，利用两个水平向的加速度计计算出水平姿态角信息，利用水平陀螺仪推导理论上的天向陀螺输出，将姿态角计算结果带入到分离出纬度信息的姿态关系中便可以计算出载体的方位角，此过程中不需要给出当地经纬度信息以及不要求使用过程中寻北系统处于水平位置，可以使寻北系统的使用更加便利，应用广。

可选地，建立载体坐标系，包括：

以双陀螺仪寻北系统的重心为原点建立载体坐标系，载体坐标系包括第一载体坐标轴、第二载体坐标轴以及第三载体坐标轴，第一载体坐标轴与第二载体坐标轴位于同一平面内，且第一载体坐标轴与第二载体坐标轴垂直，且第一载体坐标轴和第二载体坐标轴均与第三载体坐标轴垂直。

本可选的实施方式中，载体坐标系b系如图2所示，其原点O定义为载体的重心处，Ox_b轴指向载体横轴的右侧，Oy_b轴沿载体纵轴指向前方，Oz_b轴垂直与Ox_by_b平面向上，b系与载体固连。

可选地，建立地理坐标系，包括：

以双陀螺仪寻北系统的重心为原点建立地理坐标系，地理坐标系包括第一地理坐标轴、第二地理坐标轴以及第三地理坐标轴，第一地理坐标轴指向地理东向，第二地理坐标轴指向地理北向，第三地理坐标轴垂直于当地旋转椭球面指向天向。

在本可选的实施方式中，地理坐标系g₁系：原点O定义在载体的重心处，Ox_g轴指向地理东向，Oy_g轴指向地理北向，Oz_g轴垂直于当地旋转椭球面指向天向，即“东-北-天”坐标系。

可选地，根据载体坐标系的各坐标轴确定第一光纤陀螺仪、第二光纤陀螺仪、第一加速度计以及第二加速度计的指向，包括：

使第一光纤陀螺仪和第二光纤陀螺仪的X轴均指向第一载体坐标轴的方向，第一光纤陀螺仪和第二光纤陀螺仪的Y轴均指向第二载体坐标轴的方向，其中，第一加速度计的指向与第一光纤陀螺仪的指向一致，第二加速度计的指向与第二光纤陀螺仪的指向一致。

在本可选的实施方式中，将光纤陀螺的x轴指向载体系的Ox_b轴，光纤陀螺的y轴指向载体系的Oy_b轴，形成“右-前”水平测量机构，两个加速度计指向与光纤陀螺指向相同，构成寻北系统基本机构。

可选地，确定的天向陀螺输出信息，满足如下关系式：

式中，ω_z为天向陀螺输出信息值，ω_ie为地球自转角速率，ω_x为地球自转矢量在当地水平方向上的第一输出分量，ω_y为地球自转矢量在当地水平方向上的第二输出分量。

在本可选的实施方式中，寻北仪开始采集光纤陀螺感应到的地球自转矢量在当地水平方向上两个输出分量ω_x、ω_y，而地球自转角速率是一个恒定值且精确已知的，可以通过矢量合成原理来理论计算出此时天向陀螺的输出值，这个天向陀螺为一个虚拟的陀螺，实际中是不存在，即：

其中ω_z为虚拟的天向陀螺输出值，ω_ie为精确已知的地球自转角速率。

可选地，获取根据第一加速度计和第二加速度计的输出信息确定的水平姿态角信息，包括：

在重力加速g已知的情况下，通过第一加速度计和第二加速度计的测量值计算出俯仰角θ与滚转角γ相关的三角函数，如下：

式中，A_x为第一加速度计的输出值，A_y为第二加速度计的输出值，g为当地重力加速度，θ为寻北系统此时的俯仰角，γ为寻北系统此时的滚转角。

在本可选的实施方式中，将寻北仪放置待测位置静置保持不动，两个加速度计的输出分别为：A_x、A_y，采用光纤陀螺与加速度计属于高精度的惯导产品，经过了严格的零偏标定补偿，且其采样计算时间很短，因此零偏误差与随机漂移视作很小忽略不计。

在当前位置下A_x、A_y分别为：

A_x＝gsinθ； (2)

A_y＝-gsinγcosθ； (3)

其中g为当地重力加速度，θ为寻北仪此时的俯仰角，γ为寻北仪此时的滚转角。

在重力加速g已知的情况下，通过加速度计测量值可以计算出俯仰角θ与滚转角γ相关的三角函数：

即通过水平放置的加速度计计算出寻北仪当前的俯仰角θ与滚转角γ信息，此外由于俯仰角θ与滚转角γ的取值范围为-90°～+90°之间，其余弦函数cosθ与cosγ能够单值确定。

可选地，根据天向陀螺输出信息确定姿态关系，如下：

式中，

为单位正交矩阵，φ为纬度。

在本可选的实施方式中，需要说明的是，地球自转角速度在地理坐标系内的表示形式如下所示：

而载体系下陀螺仪敏感到的地球自转角速度为

与地理坐标系下的地球自转角速度

之间的关系可以由分别绕轴三次基本旋转得到，转动角度分别为航向角Ψ、俯仰角θ、滚转角γ，其中航向角北偏东为正，俯仰角抬头正，滚转角右倾为正，本实施例中的姿态关系的数学关系式如下：

即：

由于直角坐标系间的转换矩阵是单位正交矩阵，存在以下关系：

可以得到由载体系到地理坐标系的变换关系：

进一步地，将公式(12)展开可以得到：

ω_iesinφ＝-sinγcosθ*ω_x+sinθ*ω_y+cosγcosθ*ω_z (15)

首先对式让式(13)左右两边乘以cosΨ，式(14)乘以-sinΨ然后将两式相加可以得到：

-ω_iecosφsinΨ＝cosγ*ω_x+sinγ*ω_z (16)

再将式(13)乘以sinΨ，式(14)乘以cosΨ，依旧将两式相加可得：

ω_iecosφcosΨ＝-sinγcosθ*ω_x+cosθ*ω_y-sinθcosγ*ω_z (17)

最后用式(16)除以式(17)可以得到分离开来的航向角Ψ的相关表达式：

此外，还可以由式(15)解出当地纬度信息。

这样，将测量得到的陀螺仪、加速度计信息与式(4)～(7)的结果代入式(18)、式(19)可以计算出载体的方位角，在计算的过程中不需要知道当地的经纬度信息，且可自身计算而出。北向角已经被定义为北偏东为正，取值在0°～360°，其真值由下表1确定：

表1北向角真值表

最后根据式(16)与式(17)的正负值来判断方位角的真值，将其输出为当前的方位角结果。

综上，本申请提供的双陀螺仪寻北方法，利用水平安装的双轴高精度陀螺与双轴加速度计测量当地的地球自转角速度的水平分量与重力加速度的水平分量，无需内部再增加转位机构，利用姿态关系对应的关系求出寻北仪此时的水平角信息，同时根据地球自转角速率恒定不变这一特征，构造出垂直水平轴的第三个虚拟陀螺输出，以地理系与载体的关系矩阵分解出不需要当地纬度信息的关系式，便可以求出载体的方位角以及当地纬度信息。

本申请实施方式还提供一种双陀螺仪寻北系统，包括基座平台，以及设于所述基座平台上的第一探测装置和第二探测装置，所述第一探测装置和所述第二探测装置之间呈90°设置，所述第一探测装置包括第一光纤陀螺仪和第一加速度计，所述第二探测装置包括第二光纤陀螺仪和第二加速度计。

该双陀螺仪寻北系统可以实现上述的双陀螺仪寻北方法的各个实施例，且能达到相同的有益效果，此处，不做赘述。

本申请实施方式还提供一种计算机可读存储介质，其上存储有计算机程序，该程序被处理器执行时实现如上所述的方法步骤。该计算机可读存储介质可以实现上述的双陀螺仪寻北方法的各个实施例，且能达到相同的有益效果，此处，不做赘述。

以上详细描述了本发明的较佳具体实施例。应当理解，本领域的普通技术人员无需创造性劳动就可以根据本发明的构思作出诸多修改和变化。因此，凡本技术领域中技术人员依本发明的构思在现有技术的基础上通过逻辑分析、推理或者有限的实验可以得到的技术方案，皆应在由权利要求书所确定的保护范围内。

Claims (9)

1.一种双陀螺仪寻北方法，其特征在于，应用于双陀螺仪寻北系统，所述双陀螺仪寻北系统包括基座平台，以及设于所述基座平台上的第一探测装置和第二探测装置，所述第一探测装置和所述第二探测装置之间呈90°设置，所述第一探测装置包括第一光纤陀螺仪和第一加速度计，所述第二探测装置包括第二光纤陀螺仪和第二加速度计，所述方法包括：

建立载体坐标系，并建立地理坐标系；

根据所述载体坐标系的各坐标轴确定所述第一光纤陀螺仪、所述第二光纤陀螺仪、所述第一加速度计以及所述第二加速度计的指向；

获取根据所述第一光纤陀螺仪和所述第二光纤陀螺仪的输出信息确定的天向陀螺输出信息；

获取根据所述第一加速度计和所述第二加速度计的输出信息确定的水平姿态角信息；

根据所述天向陀螺输出信息确定姿态关系，所述姿态关系为分离出纬度信息的姿态关系；

并根据所述水平姿态角信息和所述姿态关系确定所述载体坐标系相对于所述地理坐标系的目标方位角。

2.根据权利要求1所述的双陀螺仪寻北方法，其特征在于，所述建立载体坐标系，包括：

以所述双陀螺仪寻北系统的重心为原点建立载体坐标系，所述载体坐标系包括第一载体坐标轴、第二载体坐标轴以及第三载体坐标轴，所述第一载体坐标轴与所述第二载体坐标轴位于同一平面内，且所述第一载体坐标轴与所述第二载体坐标轴垂直，且所述第一载体坐标轴和所述第二载体坐标轴均与所述第三载体坐标轴垂直。

3.根据权利要求1所述的双陀螺仪寻北方法，其特征在于，所述建立地理坐标系，包括：

以所述双陀螺仪寻北系统的重心为原点建立地理坐标系，所述地理坐标系包括第一地理坐标轴、第二地理坐标轴以及第三地理坐标轴，所述第一地理坐标轴指向地理东向，所述第二地理坐标轴指向地理北向，所述第三地理坐标轴垂直于当地旋转椭球面指向天向。

4.根据权利要求1所述的双陀螺仪寻北方法，其特征在于，所述根据所述载体坐标系的各坐标轴确定所述第一光纤陀螺仪、所述第二光纤陀螺仪、所述第一加速度计以及所述第二加速度计的指向，包括：

使第一光纤陀螺仪和第二光纤陀螺仪的X轴均指向所述第一载体坐标轴的方向，第一光纤陀螺仪和第二光纤陀螺仪的Y轴均指向所述第二载体坐标轴的方向，其中，第一加速度计的指向与所述第一光纤陀螺仪的指向一致，第二加速度计的指向与所述第二光纤陀螺仪的指向一致。

5.根据权利要求1所述的双陀螺仪寻北方法，其特征在于，所述确定的天向陀螺输出信息，满足如下关系式：

式中，ω_z为天向陀螺输出信息值，ω_ie为地球自转角速率，ω_x为地球自转矢量在当地水平方向上的第一输出分量，ω_y为地球自转矢量在当地水平方向上的第二输出分量。

6.根据权利要求1所述的双陀螺仪寻北方法，其特征在于，所述获取根据所述第一加速度计和所述第二加速度计的输出信息确定的水平姿态角信息，包括：

在重力加速g已知的情况下，通过第一加速度计和第二加速度计的测量值计算出俯仰角θ与滚转角γ相关的三角函数，如下：

式中，A_x为第一加速度计的输出值，A_y为第二加速度计的输出值，g为当地重力加速度，θ为寻北系统此时的俯仰角，γ为寻北系统此时的滚转角。

7.根据权利要求1所述的双陀螺仪寻北方法，其特征在于，所述根据所述天向陀螺输出信息确定姿态关系，如下：

式中，

为单位正交矩阵，φ为纬度。

8.一种双陀螺仪寻北系统，其特征在于，包括基座平台，以及设于所述基座平台上的第一探测装置和第二探测装置，所述第一探测装置和所述第二探测装置之间呈90°设置，所述第一探测装置包括第一光纤陀螺仪和第一加速度计，所述第二探测装置包括第二光纤陀螺仪和第二加速度计。

9.一种计算机可读存储介质，其上存储有计算机程序，其特征在于，该程序被处理器执行时实现如权利要求1-7中任一所述的方法步骤。

Images