CN107167127A - 一种载体姿态获取方法、装置及设备 - Google Patents

Abstract

本发明提供了一种载体姿态获取方法、装置及设备，其中方法包括：实时获得载体转动角速度、载体重力加速度和载体磁场强度在载体三轴坐标系上的分量，根据载体转动角速度、载体重力加速度和载体磁场强度在载体三轴坐标系上的分量的变化情况，构造相应的矩阵进行载体姿态解算。本发明可实时进行动态姿态解算，保证解算精度，而且具有设备重量轻、便于应用的优势。

Description

一种载体姿态获取方法、装置及设备

技术领域

本发明涉及导航和控制技术领域，尤其涉及一种载体姿态获取方法、装置及设备。

背景技术

姿态测量一般指利用传感器的输出获得载体相对于导航坐标系的姿态数据，而姿态测量的方法可从以下几个方面考虑：

1、以三轴速率陀螺为传感器，利用其测量得到的姿态运动角速度进行积分运算获得载体的姿态角。该方法动态性能较好，但缺点是长期来看精度受陀螺零偏的影响严重。特别是微机械陀螺的零偏变化较大，则解算出来的姿态将以时间一次方的速度产生漂移。

2、以三轴加速度计为传感器测量重力加速度在载体坐标系上的分量，根据重力加速度在导航坐标下的分量，利用导航坐标系与载体坐标系之间转换的方向余弦阵列建立方程组，最终获得载体的姿态角。该方法静态性能较好，但缺点是当载体变速运动时，加速度计的测量不但包括重力加速度还包括载体本身的加速度，此时再利用该方法计算姿态角就会产生较大的误差。

3、以三轴磁力计为传感器测量地磁感应强度在载体坐标系上的分量，根据地磁感应强度在导航坐标下的分量，利用导航坐标系与载体坐标系之间转换的方向余弦阵列建立方程组，最终获得载体的姿态角。该方法与载体的运动关系不大，但缺点是容易受外界磁干扰的影响，特别是当载体包含铁磁性物质时，则通过磁力计就无法确定载体的绝对姿态。

发明内容

本发明实施例提出了一种载体姿态获取方法、装置及设备，可以解决现有技术测量环境改变时载体姿态测量不准确的问题。

在一个方面，本发明实施例提供一种载体姿态获取方法，包括：

实时获得载体转动角速度、载体重力加速度和载体磁场强度在载体三轴坐标系上的分量；

根据所述载体转动角速度、载体重力加速度和载体磁场强度在载体三轴坐标系上的分量的变化情况，构造相应的矩阵进行载体姿态解算。

在另一方面，本发明实施例提供一种载体姿态获取装置，包括：

载体参量获取单元，用于实时获得所述载体转动角速度、载体重力加速度和载体磁场强度在载体三轴坐标系上的分量；

参量变化判断单元，用于判断所述载体磁场强度和/或载体重力加速度是否发生变化；

载体姿态解算单元，用于根据所述参量变化判断单元的判断结果构造相应的矩阵进行载体姿态解算。

在又一方面，本发明实施例提供一种载体姿态获取设备，包括：

三个单轴陀螺，分别用于测量载体三轴坐标系上的转动角速度；

三个单轴加速度计，分别用于测量载体三轴坐标系上的重力加速度；

一个三轴磁传感器，用于测量载体三轴坐标系上的载体转动角速度；

处理器，用于执行如上文载体姿态获取方法。

本申请有益效果如下：

本申请实施例提供了一种载体姿态获取方法、装置及设备，采用三轴速率陀螺、三轴加速度计以及三轴磁力计实时测量载体转动角速度、重力加速度及地磁强度在载体三轴上的分量，并实时进行动态姿态解算，不同的状态下采用不同的解算方法，保证解算精度，而且具有设备重量轻、便于应用的优势。

附图说明

下面将参照附图描述本发明的具体实施例，其中：

图1示出了本发明实施例一载体姿态获取方法的流程示意图；

图2示出了本发明实施例二载体姿态获取方法的流程示意图；

图3示出了本发明实施例二的沿载体坐标系分布的传感器的示意图；

图4示出了本发明实施例三的载体姿态获取装置的示意图；

图5示出了本发明实施例四的载体姿态获取设备的示意图。

具体实施方式

为了使本发明的技术方案及优点更加清楚明白，以下结合附图对本发明的示例性实施例进行进一步详细的说明，显然，所描述的实施例仅是本发明的一部分实施例，而不是所有实施例的穷举。并且在不冲突的情况下，本说明中的实施例及实施例中的特征可以互相结合。

本发明实施例提供的载体姿态获取方法、装置及设备，采用三轴速率陀螺、三轴加速度计以及三轴磁力计实时测量载体转动角速度、重力加速度及地磁强度在载体三轴上的分量，并实时进行动态姿态解算，保证解算精度，而且具有设备重量轻、便于应用的优势。

如图1所示，本发明实施例一的载体姿态获取方法包括下述步骤：

步骤10，实时获得载体转动角速度、载体重力加速度和载体磁场强度在载体三轴坐标系上的分量；

步骤20，根据所述载体转动角速度、载体重力加速度和载体磁场强度在载体三轴坐标系上的分量的变化情况，构造相应的矩阵进行载体姿态解算。

如图2所示，本发明实施例二提供一种载体姿态获取方法，包括下述步骤：

步骤101，进行初始对准；

在惯性导航系统上电启动时，载体的姿态是不确定的，因此在进入导航解算前，需要将载体的初始姿态对准。

初始对准采取静基座自对准方式，即在系统上电启动时，需要一段时间保持载体处于相对静止状态，此时没有外界对加速度的干扰，所测得的加速度只有重力加速度。通过重力加速度在各个轴向方向的分量求取载体的初始姿态角。

步骤102，选取载体坐标系，获得相应传感器的输出数据；

本实施例中选取“右-前-上”坐标系作为载体坐标系，该坐标系为惯性导航系统常用的一种载体坐标系，如图3所示，利用沿载体坐标系分布的三个速率陀螺测量载体转动角速度在载体坐标系三轴上的分量，Gx、Gy、Gz分别表示绕X、Y、Z三个轴线方向的角速率分量。利用沿载体坐标系分布的三个重力加速度计测量重力加速度在载体坐标系三轴上的分量，Ax、Ay、Az分别表示沿X、Y、Z三个轴线方向的加速度分量。利用一个磁力计测量周围磁场强度在载体坐标系三轴上的分量，Mx、My、Mz分别表示在X、Y、Z三个轴线方向感应的磁场强度分量。这些传感器输出的数据是下文进行方程解算时构造矩阵的基础数据。

在载体处于相对静止状态时，当前通过速率陀螺G所测得的角速率值是陀螺零偏的初始值，这是一个初始的不精确观测量，在下文对姿态进行进一步解算过程中，通过相应的算法可以对这个估值进行精确逼近。

步骤103，根据姿态解算算法获得载体的姿态数据；

根据现有的姿态解算算法，对传感器输出的状态量融合解算，获得载体的姿态信息，即姿态角q_e以及陀螺的零偏ΔB，本实施例采用的算法如下：

状态方程X(t)＝F(t)X(t)+G(t)W(t)；

观测方程Z(t)＝H(t)X(t)+V(t)；

状态量

其中，Z(t)为测量量矩阵，由三轴加速度值、角速度值和磁场强度值构成， H(t)为测量函数矩阵，由三轴加速度、角速度和磁场强度测量关系函数构成， V(t)为测量噪声矩阵，由各传感器的噪声信号构成，X(t)为状态矩阵包含载体的姿态状态量和陀螺零偏，F(t)为状态转移函数，由当前状态转移到下一状态的函数解算关系因子构成，G(t)为控制矩阵，包含控制的目标参量，W(t)为系统噪声，其中t为时间变量。

通过上述解算算法可以得出姿态角q_e以及陀螺的零偏ΔB，获得载体当前姿态。

步骤104，实时监测传感器的输出数据，判断加速度和/或磁场环境参量是否发生变化，未发生变化时执行步骤106，否则执行步骤105；

例如当加速度计所获得的加速度信息发生变化时，可以根据实际应用中测得加速度数据特性，判定载体处于变速运动状态，此时需要重新构造矩阵进行航姿解算，避免因加速度变化而带来的解算误差。

例如，当磁力计所获得的磁场信息发生变化时，根据实际应用中测得磁场数据特性，判定载体受到磁干扰，此时需要重新构造对应的矩阵进行航姿解算，避免因磁干扰而带来的解算误差。

步骤105，重新构造测量量矩阵Z(t)和状态矩阵X(t)，然后执行步骤103，利用重构的矩阵重新计算姿态角q_e以及陀螺的零偏ΔB，进行载体航姿修正；

在本步骤中，当检测到载体的重力加速度发生变化时，针对测量量矩阵 Z(t)和状态矩阵X(t)进行重构，重构后的矩阵中重力加速度在三轴上的三个分量不再继续参与计算，仅仅保留磁场强度和角速度在三轴上分量参与计算。

当检测到磁场强度发生变化时，针对测量量矩阵Z(t)和状态矩阵X(t)进行重构，重构后的矩阵中磁场强度在三轴上的三个分量不再参与计算，仅仅保留加速度和角速度在三轴上分量参与计算。

当检测到加速度和磁场强度均发生变化时，针对测量量矩阵Z(t)和状态矩阵X(t)进行重构，重构后的矩阵中加速度和磁场强度在三轴上的三个分量不再参与后续计算，仅仅保留角速度在三轴上的分量参与计算。

步骤106，输出解算出的载体当前的姿态，流程结束。

在本发明实施例二的步骤103中，将处理下述四种可能状态下的数据，这四种状态分别为：1、重力加速度和磁场强度均未受到影响，2、重力加速度受到影响，3、磁场强度受到影响，4、重力加速度和磁场强度均受到影响。针对第2、3、4种状态，步骤105中进行了矩阵重构，后续在步骤103中根据重构矩阵继续进行姿态解算。

本领域技术人员可以根据本发明实施例计算获得的姿态角qe以及陀螺的零偏ΔB进行载体坐标系到导航坐标系的转换。

如图4所示，本发明实施例三提供一种载体姿态获取装置，具体包括下述单元：

载体参量获取单元401，用于实时获得所述载体转动角速度、载体重力加速度和载体磁场强度在载体三轴坐标系上的分量；

参量变化判断单元402，用于判断所述载体磁场强度和/或载体重力加速度是否发生变化；

载体姿态解算单元403，用于根据所述参量变化判断单元402的判断结果构造相应的矩阵进行载体姿态解算。

其中，所述载体姿态解算单元403未接收到所述参量变化判断单元402的判断结果时，利用所述载体参量获取单元401的所述角速度、所述重力加速度和所述磁场强度通过下述方程进行所述载体的姿态解算，获得姿态角q_e以及陀螺的零偏ΔB：

状态方程：X(t)＝F(t)X(t)+G(t)W(t)；

观测方程：Z(t)＝H(t)X(t)+V(t)；

状态量：

其中，所述Z(t)为测量量矩阵，由所述三轴坐标系上的三个所述重力加速度值、三个所述角速度值和三个所述磁场强度值构成，所述H(t)为测量函数矩阵，由所述三轴坐标系上的三个所述重力加速度值、三个所述角速度值和三个所述磁场强度的测量关系函数构成，所述V(t)为测量噪声矩阵，所述X(t)为由所述载体的所述姿态角和所述陀螺零偏构成的状态矩阵，所述F(t)为由当前状态转移到下一状态的函数解算关系因子构成的状态转移函数，所述G(t)为包含控制的目标参量的控制矩阵，所述W(t)为系统噪声，其中所述t为时间变量。

其中当所述载体姿态解算单元403接收到所述参量变化判断单元402的所述载体磁场强度和/或所述载体重力加速度发生变化的判断结果时，针对所述测量量矩阵Z(t)和所述状态矩阵X(t)进行矩阵重构，重构后的矩阵中的所述载体磁场强度和/或所述载体重力加速度的三轴分量不再参与计算，利用重构后的矩阵带入所述方程中计算姿态角q_e以及陀螺的零偏ΔB。

如图5所示，本发明实施例四提供一种载体姿态获取设备，包括：

三个单轴陀螺501，分别用于测量载体三轴坐标系上的转动角速度；

三个单轴加速度计502，分别用于测量载体三轴坐标系上的重力角速度；

一个三轴磁传感器503，用于测量载体三轴坐标系上的载体转动角速度；

处理器504，用于执行上文所述的载体姿态获取方法。

其中，单轴陀螺501为单轴微机械陀螺、单轴加速度计502为单轴微机械加速度计，三轴磁传感器503的输出均为模拟量输出，信号通过数模转换A/D 505化为数字信号，数字信号进入数字信号处理器506进行后期信号处理，包括为了去除高频噪声，对信号进行滤波处理，并进行姿态解算。

其中，数字信号处理器506在滤波处理过程中，可以选择四种卡尔曼滤波模型分别对应：1、重力加速度和磁场强度均未受到影响，2、重力加速度受到影响，3、磁场强度受到影响，4、重力加速度和磁场强度均受到影响这四种情况，针对每个滤波模型对相关参数分别进行设置，从而能够与不同环境下的传感器特性相对应。并且还可将滤波的截至频率设置的较高，保证传感器测量数据的延时小，利于后期应用时利用角速度进行稳定控制。

本发明的设备还可以设置温度传感器等，数字信号处理器506在数据处理过程中通过软件标定和补偿，剔除交叉耦合、加速度、温度以及周围磁场对传感器测量输出的影响，提高传感器的测量精度。

此外，本实施例的设备还包括电源508用于为设备上电。设备接口采用双路RS-232接口507。

为了描述的方便，以上所述装置的各部分以功能分为各种模块或单元分别描述。当然，在实施本发明时可以把各模块或单元的功能在同一个或多个软件或硬件中实现。

本领域内的技术人员应明白，本发明的实施例可提供为方法、系统、或计算机程序产品。因此，本发明可采用完全硬件实施例、完全软件实施例、或结合软件和硬件方面的实施例的形式。而且，本发明可采用在一个或多个其中包含有计算机可用程序代码的计算机可用存储介质(包括但不限于磁盘存储器、 CD-ROM、光学存储器等)上实施的计算机程序产品的形式。

本发明是参照根据本发明实施例的方法、设备(系统)、和计算机程序产品的流程图和/或方框图来描述的。应理解可由计算机程序指令实现流程图和 /或方框图中的每一流程和/或方框、以及流程图和/或方框图中的流程和/ 或方框的结合。可提供这些计算机程序指令到通用计算机、专用计算机、嵌入式处理机或其他可编程数据处理设备的处理器以产生一个机器，使得通过计算机或其他可编程数据处理设备的处理器执行的指令产生用于实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能的装置。

这些计算机程序指令也可存储在能引导计算机或其他可编程数据处理设备以特定方式工作的计算机可读存储器中，使得存储在该计算机可读存储器中的指令产生包括指令装置的制造品，该指令装置实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能。

这些计算机程序指令也可装载到计算机或其他可编程数据处理设备上，使得在计算机或其他可编程设备上执行一系列操作步骤以产生计算机实现的处理，从而在计算机或其他可编程设备上执行的指令提供用于实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能的步骤。

尽管已描述了本发明的优选实施例，但本领域内的技术人员一旦得知了基本创造性概念，则可对这些实施例作出另外的变更和修改。所以，所附权利要求意欲解释为包括优选实施例以及落入本发明范围的所有变更和修改。

Claims (10)

1.一种载体姿态获取方法，其特征在于包括下述步骤：

实时获得载体转动角速度、载体重力加速度和载体磁场强度在载体三轴坐标系上的分量；

根据所述载体转动角速度、载体重力加速度和载体磁场强度在载体三轴坐标系上的分量的变化情况，构造相应的矩阵进行载体姿态解算。

2.如权利要求1所述的方法，其特征在于，所述根据所述载体转动角速度、载体重力加速度和载体磁场强度在载体三轴坐标系上的分量的变化情况，构造相应的矩阵进行载体姿态解算的步骤中：

当载体转动角速度、载体重力加速度和载体磁场强度在载体三轴坐标系上的分量均未变化时，通过下述方程进行载体姿态测量获得姿态角q_e以及陀螺的零偏ΔB：

状态方程：X(t)＝F(t)X(t)+G(t)W(t)；

观测方程：Z(t)＝H(t)X(t)+V(t)；

状态量：

其中，所述Z(t)为测量量矩阵，由所述三轴坐标系上的三个所述重力加速度值、三个所述角速度值和三个所述磁场强度值构成，所述H(t)为测量函数矩阵，由所述三轴坐标系上的三个所述重力加速度值、三个所述角速度值和三个所述磁场强度的测量关系函数构成，所述V(t)为测量噪声矩阵，所述X(t)为由所述载体的所述姿态角和所述陀螺零偏构成的状态矩阵，所述F(t)为由当前状态转移到下一状态的函数解算关系因子构成的状态转移函数，所述G(t)为包含控制的目标参量的控制矩阵，所述W(t)为系统噪声，其中所述t为时间变量。

3.如权利要求2所述的方法，其特征在于，所述根据所述载体转动角速度、载体重力加速度和载体磁场强度在载体三轴坐标系上的分量的变化情况，构造相应的矩阵进行载体姿态解算的步骤中：

当所述载体磁场强度和/或所述载体重力加速度发生变化时，针对所述测量量矩阵Z(t)和所述状态矩阵X(t)进行矩阵重构，重构后的矩阵中的所述载体磁场强度和/或所述载体重力加速度的三轴分量不再参与计算，利用重构后的矩阵带入所述方程中计算姿态角q_e以及陀螺的零偏ΔB。

4.如权利要求2所述的方法，其特征在于，所述三轴坐标系为载体的空间三维坐标系。

5.一种载体姿态获取装置，其特征在于，包括：

载体参量获取单元，用于实时获得所述载体转动角速度、载体重力加速度和载体磁场强度在载体三轴坐标系上的分量；

参量变化判断单元，用于判断所述载体磁场强度和/或载体重力加速度是否发生变化；

载体姿态解算单元，用于根据所述参量变化判断单元的判断结果构造相应的矩阵进行载体姿态解算。

6.如权利要求5所述的装置，其特征在于，所述载体姿态解算单元未接收到所述参量变化判断单元的判断结果时，利用下述方程进行所述载体的姿态解算，获得姿态角q_e以及陀螺的零偏ΔB：

状态方程：X(t)＝F(t)X(t)+G(t)W(t)；

观测方程：Z(t)＝H(t)X(t)+V(t)；

状态量：

其中，所述Z(t)为测量量矩阵，由所述三轴坐标系上的三个所述重力加速度值、三个所述角速度值和三个所述磁场强度值构成，所述H(t)为测量函数矩阵，由所述三轴坐标系上的三个所述重力加速度值、三个所述角速度值和三个所述磁场强度的测量关系函数构成，所述V(t)为测量噪声矩阵，所述X(t)为由所述载体的所述姿态角和所述陀螺零偏构成的状态矩阵，所述F(t)为由当前状态转移到下一状态的函数解算关系因子构成的状态转移函数，所述G(t)为包含控制的目标参量的控制矩阵，所述W(t)为系统噪声，其中所述t为时间变量。

7.如权利要求6所述的装置，其特征在于，当所述载体姿态解算单元接收到所述参量变化判断单元的所述载体磁场强度和/或所述载体重力加速度发生变化的判断结果时，针对所述测量量矩阵Z(t)和所述状态矩阵X(t)进行矩阵重构，重构后的矩阵中的所述载体磁场强度和/或所述载体重力加速度的三轴分量不再参与计算，利用重构后的矩阵带入所述方程中计算姿态角q_e以及陀螺的零偏ΔB。

8.如权利要求5所述的装置，其特征在于，所述三轴坐标系为载体的空间三维坐标系。

9.一种载体姿态获取设备，其特征在于，包括：

三个单轴陀螺，分别用于测量载体三轴坐标系上的转动角速度；

三个单轴加速度计，分别用于测量载体三轴坐标系上的重力加速度；

一个三轴磁传感器，用于测量载体三轴坐标系上的载体转动角速度；

处理器，用于执行如权利要求1至5所述的任意一种载体姿态获取方法。

10.如权利要求9所述的设备，其特征在于，所述设备进一步包括：

数模转换器，用于将所述单轴陀螺、单轴加速度计和三轴磁传感器输出的模拟信号转换为数字信号后发送给所述处理器；

双路RS-232接口；

其中所述处理器还包括对所述数字信号进行滤波，其中滤波参数根据所述单轴陀螺、单轴加速度计和三轴磁传感器的状态和环境对应设置。