CN108225376A - 一种姿态检测系统中初始姿态自动标定方法及系统 - Google Patents

Abstract

本发明公开了一种姿态检测系统中的初始姿态自动标定方法及系统，将姿态测量单元固定在被测物体表面，进行测量，连续采样姿态测量单元处于当前姿态时的测量值，计算姿态测量单元处于当前姿态时，连续采样获得的测量值的平均值；计算姿态测量单元处于当前状态时，重力场和磁力场在载体坐标系各坐标轴的标准化分量；计算姿态检测系统在姿态检测单元处于当前姿态与混合磁场下的偏移欧拉角；按照姿态检测单元初始状态下的偏移欧拉角修正姿态检测算法，根据修正后的姿态检测算法进行被测物体的姿态检测。

Description

一种姿态检测系统中初始姿态自动标定方法及系统

技术领域

本发明涉及一种姿态检测系统中初始姿态自动标定方法及系统。

背景技术

随着微机电系统(MEMS，Micro-Electro-Mechanical System)的日益发展，MARG(Magnetic，Angular Rate and Gravity)传感器越来越多的应用在姿态检测领域。对物体姿态进行准确检测，是进行其他深入研究的基础。而在基于MARG传感器的姿态检测算法中，要求姿态检测单元的初始状态为理想放置状态，即要求姿态检测单元的载体坐标系与世界坐标系重合，并且假设姿态检测系统的工作环境中磁场只有地磁场而不存在其他干扰磁场。但在实际过程中，由于被检测物体表面不完全水平，以及手动装配测量单元时的装配误差，都将导致检测单元的放置状态与理想状态不一致，并且由于姿态检测系统工作环境中环境磁场的干扰，实际的真实磁场相对于地磁场发生了一定的偏移。以上两个问题的存在，导致姿态检测单元的测量数据在经过姿态检测算法后得到的被检测物体的姿态发生了不同程度的偏移，造成姿态检测精度不稳定。当检测精度过低时，无法满足姿态检测要求，整个姿态检测系统将失去检测意义，无法进行其他上层应用的研究。

为了减小姿态检测单元的初始姿态偏移对姿态检测系统最终检测结果的影响，通常采取的做法是在初始状态下，将姿态检测单元放置在水平面上，尽量调整姿态检测单元的载体坐标系与世界坐标系重合，然后再将姿态检测单元移动至被检测物体表面。这种方式操作繁琐，而且在调整两坐标系重合过程中需要较高的技巧，耗费大量的时间。

发明内容

本发明为了解决上述问题，提出了一种姿态检测系统中的初始姿态自动标定方法及系统，本发明根据姿态检测单元的初始状态，自动修正姿态检测算法中的姿态输出，可以大大降低姿态检测算法对姿态检测单元的初始状态的要求，提高物体姿态检测的精度与效率。

为了实现上述目的，本发明采用如下技术方案：

一种姿态检测系统中的初始姿态自动标定方法，包括以下步骤：

(1)将姿态测量单元固定在被测物体表面，进行测量；

(2)连续采样姿态测量单元处于当前姿态时的测量值，包括重力场、混合磁场以及其分别在载体坐标系中x轴、y轴和z轴上的分量；

(3)计算姿态测量单元处于当前姿态时，连续采样获得的测量值的平均值；

(4)计算姿态测量单元处于当前状态时，重力场和混合磁场在载体坐标系各坐标轴的标准化分量；

(5)计算姿态检测系统在姿态检测单元处于当前姿态与混合磁场下的偏移欧拉角；

(6)按照姿态检测单元初始状态下的偏移欧拉角修正姿态检测算法，根据修正后的姿态检测算法进行被测物体的姿态检测。

进一步的，所述步骤(2)中，重力场的测量值为^ba_t＝[a_xt,a_yt,a_zt]，其中^ba_t为载体坐标系下t时刻重力场的测量值，a_xt，a_yt，a_zt分别为t时刻重力场在载体坐标系x轴，y轴，z轴上的分量。

进一步的，所述步骤(2)中，混合磁场的测量值为^bm_t＝[m_xt,m_yt,m_zt]，^bm_t为载体坐标系下连续采样获得的混合磁场的平均值，m_x，m_y，m_z分别为t时刻混合磁场在载体坐标系x轴，y轴，z轴上的分量。

进一步的，所述步骤(3)中，各测量值的平均值为其对应的测量值从零时刻到停止时刻的总和与连续采样的时间段中数据总数之比。

进一步的，所述步骤(4)中，计算载体坐标系下连续采样获得的重力场平均值和混合磁场的平均值的2范数，然后计算重力场和混合磁场在载体坐标系各坐标轴的标准化分量。

进一步的，所述步骤(5)中，选择载体坐标系原点与载体的质心重合，X轴平行于载体轴线并指向载体的前方，Y轴垂直于X轴并指向载体右方，Z轴垂直于X轴和Y轴组成的平面并指向载体下方。

进一步的，所述步骤(5)中，选择北东地坐标系作为世界坐标系。

进一步的，所述步骤(6)中，进行修正是将姿态检测算法输出的欧拉角去除姿态检测单元处于初始姿态与混合磁场下的偏移欧拉角的影响。

一种姿态检测系统中的初始姿态自动标定系统，运行于处理器上，包括：姿态测量单元，计算机。

姿态测量单元，固定在被测物体表面，进行测量，包括：信息采集模块、计算模块和修正模块。

信息采集模块，连续采样姿态测量单元处于当前姿态时的测量值，包括重力场、混合磁场以及其分别在载体坐标系中x轴、y轴和z轴上的分量；

计算模块，被配置为计算姿态测量单元处于当前姿态时，连续采样获得的测量值的平均值；计算姿态测量单元处于当前状态时，重力场和混合磁场在载体坐标系各坐标轴的标准化分量；计算姿态检测系统在姿态检测单元处于当前姿态与混合磁场下的偏移欧拉角；

修正模块，被配置为按照姿态检测单元初始状态下的偏移欧拉角修正姿态检测算法，根据修正后的姿态检测算法进行被测物体的姿态检测。

计算机，用于实时显示被测物体在三维空间中的姿态。

本发明的基本原理是：在姿态检测单元固定在被测物体表面后，由姿态检测系统自动测定姿态检测单元处于当前姿态以及混合磁场下的初始姿态角，进而修正姿态测量算法得到的姿态角，最后得到消除了初始偏移量的被检测物体的姿态角。

本发明的有益效果是：在对被测物体进行姿态检测之前通过姿态检测系统自动测定姿态检测单元初始姿态角，对姿态检测单元初始状态没有要求，姿态检测单元可以任意放置；提高了姿态检测单元的抗干扰能力，在工作环境存在干扰磁场的情况下也能够正常工作；缩短姿态检测系统进行姿态检测的准备时间；提高了姿态检测的精度以及稳定性。

附图说明

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

图1为本发明提供的一个示意性例子的框架示意图；

图2为本发明提供的一种初始姿态自动标定方法的原理流程示意图；

图3(a)和图3(b)为本发明提供的一种测量坐标系选取方式示意图；

图4(a)-图4(c)为本发明提供的一个示意性例子在具体应用场景下的曲线示意图；

具体实施方式：

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

应该指出，以下详细说明都是例示性的，旨在对本申请提供进一步的说明。除非另有指明，本文使用的所有技术和科学术语具有与本申请所属技术领域的普通技术人员通常理解的相同含义。

需要注意的是，这里所使用的术语仅是为了描述具体实施方式，而非意图限制根据本申请的示例性实施方式。如在这里所使用的，除非上下文另外明确指出，否则单数形式也意图包括复数形式，此外，还应当理解的是，当在本说明书中使用术语“包含”和/或“包括”时，其指明存在特征、步骤、操作、器件、组件和/或它们的组合。

在本发明中，术语如“上”、“下”、“左”、“右”、“前”、“后”、“竖直”、“水平”、“侧”、“底”等指示的方位或位置关系为基于附图所示的方位或位置关系，只是为了便于叙述本发明各部件或元件结构关系而确定的关系词，并非特指本发明中任一部件或元件，不能理解为对本发明的限制。

本发明中，术语如“固接”、“相连”、“连接”等应做广义理解，表示可以是固定连接，也可以是一体地连接或可拆卸连接；可以是直接相连，也可以通过中间媒介间接相连。对于本领域的相关科研或技术人员，可以根据具体情况确定上述术语在本发明中的具体含义，不能理解为对本发明的限制。

根据背景技术中叙述的，在使用姿态检测单元来进行物体姿态检测的姿态检测算法中，要求姿态检测单元的初始状态时的载体坐标系与世界坐标系一致，并且没有考虑混合磁场的影响，无法满足实际应用的需求。

鉴于此，本发明在对被测物体进行姿态检测之前通过姿态检测系统自动测定姿态检测单元初始姿态角，消除初始偏移姿态角以及干扰磁场的影响。下面结合附图与实施实例对本发明做进一步说明。

图1为本发明提供的一个示意性例子的框架示意图。

如图1所示，将姿态测量单元固定在转台上，载体坐标系与世界坐标系不完全重合，通过初始状态下的姿态检测单元测量的加速度以及混合磁场信息，求得初始状态下的偏移姿态角，修正姿态检测算法中的欧拉角计算方法，从而获得转台实际转动的角度。其中计算机分别与姿态检测单元和转台与相连，用于处理姿态检测单元和转台的测量信息，实时显示转台在三维空间中的姿态。图2为本发明提供的一种初始姿态自动标定方法的原理流程示意图。

具体的，本发明的初始姿态自动标定方法为：

第一步：将姿态测量单元固定在被测物体表面；

第二步：开启姿态测量单元电源；

第三步：连续采样姿态测量单元在当前姿态时的测量值^ba_t＝[a_xt,a_yt,a_zt]，^bm_t＝[m_xt,m_yt,m_zt]。其中^ba_t为载体坐标系下t时刻重力场的测量值，a_xt，a_yt，a_zt分别为t时刻重力场在载体坐标系x轴，y轴，z轴上的分量；^bm_t为载体坐标系下连续采样获得的混合磁场的平均值，m_x，m_y，m_z分别为t时刻混合磁场在载体坐标系x轴，y轴，z轴上的分量。

第四步：计算姿态测量单元在当前姿态时，连续采样获得的测量值的平均值^ba＝[a_x,a_y,a_z]，^bm＝[m_x,m_y,m_z]。其中^ba为载体坐标系下连续采样获得的重力场平均值，a_x，a_y，a_z分别为重力场在载体坐标系x轴，y轴，z轴上的分量平均值；^bm为载体坐标系下连续采样获得的混合磁场的平均值，m_x，m_y，m_z分别为混合磁场在载体坐标系x轴，y轴，z轴上的分量平均值。

第五步：计算姿态测量单元处于当前状态时，重力场和混合磁场在载体坐标系各坐标轴的标准化分量^ba_bias，^bm_bias。

第六步：计算姿态检测系统在姿态检测单元处于当前姿态与混合磁场下的偏移欧拉角θ_bias与ψ_bias。

第七步：按照姿态检测单元初始状态下的偏移欧拉角θ_bias，ψ_bias修正姿态检测算法。

第八步：根据修正后的姿态检测算法进行被测物体的姿态检测。

所述第四步中，计算连续采样获得的测量值的平均值^ba＝[a_x,a_y,a_z]，^bm＝[m_x,m_y,m_z]的公式为：

其中，n为连续采样的数据总数，^ba为载体坐标系下连续采样获得的重力场平均值，a_x，a_y，a_z分别为重力场在载体坐标系x轴，y轴，z轴上的分量平均值；^bm为载体坐标系下连续采样获得的混合磁场的平均值，m_x，m_y，m_z分别为混合磁场在载体坐标系x轴，y轴，z轴上的分量平均值；a_xt，a_yt，a_zt分别为t时刻重力场在载体坐标系x轴，y轴，z轴上的分量；^bm_t为载体坐标系下连续采样获得的混合磁场的平均值，mx，m_y，mz分别为t时刻混合磁场在载体坐标系x轴，y轴，z轴上的分量。

所述第五步中，计算姿态测量单元处于当前状态时，重力场和混合磁场在载体坐标系各坐标轴的标准化分量ba_bias，bm_bias。

首先，计算载体坐标系下连续采样获得的重力场平均值^bm和混合磁场的平均值^bm的2范数||^ba||₂，||^bm||₂，计算公式为：

其次，计算重力场和混合磁场在载体坐标系各坐标轴的标准化分量ba_bias，bm_bias，其中^ba_bias＝[cos(a_x)cos(a_y)cos(a_z)]，^bm_bias＝[cos(m_x)cos(m_y)cos(m_z)]，计算公式为：

cos(a_x)，cos(a_y)，cos(a_z)分别为在姿态测量单元处于初始状态时重力场进行标准化后在载体坐标系三个坐标轴上的分量cos(m_x)，cos(m_y)，cos(m_z)分别为在姿态测量单元处于初始状态时混合磁场进行标准化后在载体坐标系三个坐标轴上的分量。

所述第六步中，计算姿态检测系统在姿态检测单元处于当前姿态与混合磁场下的偏移欧拉角θ_bias与ψ_bias。

需要注意的是，世界坐标系与载体坐标系的选择方式多种多样，此处坐标系的选择参见图3(a)的世界坐标系和图3(b)所示的载体坐标系。世界坐标系选择北东地(NED)坐标系。载体坐标系遵循右手法则，载体坐标系原点与载体的质心重合，X轴平行于载体轴线并指向载体的前方，Y轴垂直于X轴并指向载体右方，Z轴垂直于X轴和Y轴组成的平面并指向载体下方。

在这种坐标系选择方式下，此时偏移欧拉角θ_bias与ψ_bias的计算公式如下：

所述第七步中，按照姿态检测单元初始状态下的偏移欧拉角θ_bias，ψ_bias进行姿态检测算法的修正：

θ_act＝θ_orig-θ_bias

ψ_act＝ψ_orig-ψ_bias

其中，θ_act，ψ_act表示修正后的姿态检测算法输出的欧拉角，θ_bias，ψ_bias为姿态检测单元处于初始姿态与混合磁场下的偏移欧拉角，θ_orig，ψ_orig为姿态检测算法在未进行初始欧拉角偏移时输出的原始欧拉角。

为了使本领域技术人员更清楚的明白本发明的技术方案，还提供了具体的实例，具体的可为：

将姿态测量单元固定在转台上，开启姿态检测单元的电源，连续采样初始状态下姿态测量单元的测量值。转台按照设定的凸轮表转动，计算机通过编码器反馈的信息实时记录转台实际转动的角度值，同时姿态检测算法根据姿态测量单元的测量值自动标定初始姿态，输出转台转动的欧拉角。图4(a)-图4(c)三幅图分别为转台在运动过程的偏航角，横滚角和俯仰角的值。每幅图中的虚线为通过编码器反馈获得的转台的实际转动的欧拉角，实线为未经过初始姿态标定的姿态检测算法的欧拉角输出值，点线为经过本发明提出的初始姿态自动标定后的姿态检测算法的欧拉角输出值。可以明显看出，经过初始姿态自动标定后，姿态检测算法的欧拉角输出值明显优于未经初始姿态自动标定的结果，更加接近转台的实际欧拉角值，突显出本发明的实际应用价值。

本发明的技术方案不局限于所给出的参考坐标系以及应用场景，在实际过程中，由于姿态检测单元的应用场景、磁场干扰以及参考坐标系的选择是多种多样的，但根据实际的坐标测量单元的放置状态以及环境磁场的影响，在相应的参考坐标系下求解初始姿态偏移量，修正姿态检测算法中的实际姿态角的输出，这个进行偏差修正的思路完全相同，因此本领域技术人员在本发明思路下针对具体应用场景、姿态检测单元放置状态、环境磁场干扰情况以及参考坐标系的选取所做的修正仍属于本发明的技术方案。

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

上述虽然结合附图对本发明的具体实施方式进行了描述，但并非对本发明保护范围的限制，所属领域技术人员应该明白，在本发明的技术方案的基础上，本领域技术人员不需要付出创造性劳动即可做出的各种修改或变形仍在本发明的保护范围以内。

Claims (9)

1.一种姿态检测系统中的初始姿态自动标定方法，其特征是：包括以下步骤：

(1)将姿态测量单元固定在被测物体表面，进行测量；

(2)连续采样姿态测量单元处于当前姿态时的测量值，包括重力场、混合磁场以及其分别在载体坐标系中x轴、y轴和z轴上的分量；

(3)计算姿态测量单元处于当前姿态时，连续采样获得的测量值的平均值；

(4)计算姿态测量单元处于当前状态时，重力场和磁力场在载体坐标系各坐标轴的标准化分量；

(5)计算姿态检测系统在姿态检测单元处于当前姿态与混合磁场下的偏移欧拉角；

(6)按照姿态检测单元初始状态下的偏移欧拉角修正姿态检测算法，根据修正后的姿态检测算法进行被测物体的姿态检测。

2.如权利要求1所述的一种姿态检测系统中的初始姿态自动标定方法，其特征是：所述步骤(2)中，重力场的测量值为^ba_t＝[a_xt,a_yt,a_zt]，其中^ba_t为载体坐标系下t时刻重力场的测量值，a_xt，a_yt，a_zt分别为t时刻重力场在载体坐标系x轴，y轴，z轴上的分量。

3.如权利要求1所述的一种姿态检测系统中的初始姿态自动标定方法，其特征是：所述步骤(2)中，磁力场的测量值为^bm_t＝[m_xt,m_yt,m_zt]，^bm_t为载体坐标系下连续采样获得的混合磁场的平均值，m_x，m_y，m_z分别为t时刻混合磁场在载体坐标系x轴，y轴，z轴上的分量。

4.如权利要求1所述的一种姿态检测系统中的初始姿态自动标定方法，其特征是：所述步骤(3)中，各测量值的平均值为其对应的测量值从零时刻到停止时刻的总和与连续采样的时间段中数据总数之比。

5.如权利要求1所述的一种姿态检测系统中的初始姿态自动标定方法，其特征是：所述步骤(4)中，计算载体坐标系下连续采样获得的重力场平均值和混合磁场的平均值的2范数，然后计算重力场和磁力场在载体坐标系各坐标轴的标准化分量。

6.如权利要求1所述的一种姿态检测系统中的初始姿态自动标定方法，其特征是：所述步骤(5)中，选择北东地坐标系作为世界坐标系。

7.如权利要求1所述的一种姿态检测系统中的初始姿态自动标定方法，其特征是：所述步骤(5)中，选择载体坐标系原点与载体的质心重合，X轴平行于载体轴线并指向载体的前方，Y轴垂直于X轴并指向载体右方，Z轴垂直于X轴和Y轴组成的平面并指向载体下方。

8.如权利要求1所述的一种姿态检测系统中的初始姿态自动标定方法，其特征是：所述步骤(6)中，进行修正是将姿态检测算法输出的欧拉角去除姿态检测单元处于初始姿态与混合磁场下的偏移欧拉角的影响。

9.一种姿态检测系统中的初始姿态自动标定系统，运行于处理器上，其特征是：包括：姿态测量单元，计算机。

姿态测量单元，固定在被测物体表面，进行测量，包括：信息采集模块、计算模块和修正模块。

信息采集模块，连续采样姿态测量单元处于当前姿态时的测量值，包括重力场、混合磁场以及其分别在载体坐标系中x轴、y轴和z轴上的分量；

计算模块，被配置为计算姿态测量单元处于当前姿态时，连续采样获得的测量值的平均值；计算姿态测量单元处于当前状态时，重力场和混合磁场在载体坐标系各坐标轴的标准化分量；计算姿态检测系统在姿态检测单元处于当前姿态与混合磁场下的偏移欧拉角；

修正模块，被配置为按照姿态检测单元初始状态下的偏移欧拉角修正姿态检测算法，根据修正后的姿态检测算法进行被测物体的姿态检测。

计算机，用于实时显示被测物体在三维空间中的姿态。