运动物体的姿态估计的方法及装置制造方法及图纸

本发明专利技术提供了运动物体的姿态估计的方法及装置。本发明专利技术被用于估计运动物体的姿态。本发明专利技术基于陀螺仪数据来估计姿态，然后基于加速计/磁力计数据来计算校正增益和校正矢量。校正增益和校正矢量被用于校准所述陀螺仪数据，最后经校准的陀螺仪数据被用于更新所述物体姿态。与现有技术相比，当根据加速计的读数，物体正以高加速度运动时，本发明专利技术能够减弱基于加速计数据的重力估计结果的校准。

【技术实现步骤摘要】
运动物体的姿态估计的方法及装置
本专利技术涉及物体姿态估计的方法及装置，更具体地，涉及用于估计运动物体的姿态的方法及装置。
技术介绍
现在为了估计运动物体的姿态和方向，传统上通过四元数或欧拉角(俯仰(pitch)角、滚转(roll)角和偏航(yaw)角)来表示物体姿态。两种类型的表示可以互相转换。此外，已知四元数表示从一个坐标系统到另一个坐标系统的变换。这种变换也可以表示为转换矩阵。转换矩阵的典型示例用于将矢量从地心坐标系(levelframe)(例如，东、北、上)变换到体坐标系。针对在无失真的地磁场中的静止物体，可以通过加速计数据来计算静止物体的姿态，或者基于加速计数据和磁力计数据来充分且精确地计算静止物体的姿态。然而，针对运动物体，将重力加速度和运动加速度合并为来自加速计的测量。在知道物体姿态之前，无法明确地将它们分开。此外，在物体周围的地磁场常因铁或磁性材料而失真。这种影响给基于加速计数据或基于加速计数据和磁力计数据的姿态估计结果带来偏差。另一方面，如果知道上次的物体姿态，则能够通过陀螺仪数据来估计物体姿态。来自陀螺仪的测量基本上不受上述环境噪声(物体运动、磁场失真等)的影响。然而，估计处理基于积分运算。常常遇到零漂移问题，这会对姿态估计结果引起累积偏差。在相关文献中，公开了不同姿态估计方法。针对该问题的典型解决方案是将来自两种类型的传感器数据的估计结果融合在一起。两种广泛使用的数据融合方法是基于卡尔曼滤波器法和基于增益调度法。其中，增益调度是利用线性控制器族来控制非线性系统的最流行的方法之一。增益调度基于调度变量的当前测量来调整控制器增益。用户能够设计控制器增益如何随测量的变量而改变。在姿态估计系统中，利用预定义调度基于传感器测量来计算增益。该增益用于合并所述两个估计结果或校准陀螺仪数据以改善物体姿态。美国专利第US8688403B2号公报公开了基于惯性传感器数据来估计物体姿态的方法。该方法使用来自加速计和磁力计的数据计算第一姿态矩阵梯度，然后使用来自陀螺仪的数据计算第二姿态矩阵梯度。通过混合系数来使第一姿态矩阵梯度与第二姿态矩阵梯度融合。最后，融合的梯度用于更新精细的姿态矩阵。在此，通过将来自加速计的数据与重力进行比较，并且还将来自磁力计的数据与一般已知当地磁场进行比较，来确定所述混合系数。作为基于增益调度法的非常重要的步骤，增益(混合系数)计算步骤并未被清楚地记载在该专利中，而是在专利技术说明书中仅被粗略地说明并凭经验被定义。当物体突然或不规则地运动时，运动物体的姿态改变很大，如何设计基于测量的、适应的校正增益是针对该方法的关键问题。此外，理想地讲，在地心坐标系中重力矢量是常数(0，0，g)，但是来自不同加速计的重力测量可能略微不同，因此如何精确地从加速计数据中去除重力分量是物体姿态估计的挑战所在。另外，不存在定量地描述物体的运动加速度与基于姿态估计的加速计/磁力计数据的偏差之间的关系的理论数学函数。在该专利中凭经验来估计混合系数。不存在验证计算出的混合系数的有效性和正确性的步骤。
技术实现思路
因此，在本公开中提出了新的姿态估计方法和装置，以解决上述问题中的至少一个，来提高在真实世界中的运动物体的姿态估计的精确度。本专利技术基于陀螺仪数据来估计姿态，然后基于传感器数据来计算校正增益和校正矢量。校正增益和校正矢量用于校准陀螺仪数据，最后校准的陀螺仪数据用于更新物体姿态。根据本专利技术的一个方面，提供了一种姿态估计方法，所述姿态估计方法包括：校正增益确定步骤，其用于基于利用所述陀螺仪数据和加速计数据而测量的物体运动程度，来确定校正增益；校正矢量计算步骤，其用于通过分别基于所述陀螺仪数据和加速计，将所述参考矢量从地心坐标系转换到体坐标系，来计算校正矢量，然后进行两个转换的参考矢量之间的叉积运算，以生成校正矢量；传感器数据校准步骤，其用于使用所述校正增益和所述校正矢量，来校准所述陀螺仪数据；姿态估计步骤，用于利用经校准的陀螺仪数据来估计所述姿态；此外，所述校正增益确定步骤还包括以下步骤：基于所述加速计数据来估计在体坐标系中的重力分量；通过从加速度数据中去除重力分量来计算所述物体的运动加速度；基于所述物体的所述运动加速度的预定义函数来计算所述校正增益。此外，所述姿态估计方法还包括调整步骤，所述调整步骤基于利用当前陀螺仪数据和经校准的陀螺仪数据计算的姿态估计偏差，来调整所述校正增益。本专利技术旨在使用校正矢量和适应的校正增益来校准陀螺仪数据，以精确地估计物体姿态。因此，本专利技术由三个模块构成：校正矢量生成、校正增益确定和物体姿态校准，由此在物体处于静止的情况下，本专利技术能够增强基于加速计数据的物体姿态估计，而在物体以高加速度运动的情况下，减弱基于加速计数据的物体姿态估计。此外，基于姿态估计偏差来调整在上次估计处理中计算出的校正增益。在本文中呈现的本专利技术的优势是，能够使得物体姿态估计更精确，具体而言，当物体突然运动时，由于本专利技术通过使用校正矢量和校正增益以直接校准陀螺仪数据，因此可以将不同的传感器数据融合以精确地估计物体姿态。附图说明图1是示出能够执行本专利技术的实施例的计算机系统的硬件构造的框图。图2是示出物体姿态估计装置的构造的框图。图3是示出根据本专利技术的一个实施例的装置进行物体姿态估计的流程图。图4是根据本专利技术的一个实施例的校正增益的确定方法的流程图。图5A是示出根据本专利技术的一个实施例的校正增益确定的线性函数的示意图。图5B是示出根据本专利技术的一个实施例的校正增益确定的高斯函数的示意图。图6是根据本专利技术的一个实施例的校正矢量的生成方法的流程图。具体实施方式参照上述所列附图，本部分描述了特定实施例及其详细结构和操作。请注意，下文将要描述的实施例仅以说明而不是限制的方式进行阐述，因此它们不限制本专利技术的范围且能够在本专利技术的范围内被改变为各种形式。在此，鉴于该教导，本领域的技术人员能够认识到存在与在此描述的示例实施例等同的范围。图1是示出能够执行本专利技术的实施例的计算机系统的硬件构造的框图。如图1所示，系统至少包括计算机100，该计算机100包括CPU101、RAM102、ROM103、系统总线104、输入设备105、输出设备106和驱动器107。例如，计算机100是物体姿态估计系统或姿态航向参考系统(AHRS)。注意，计算机100可以包括一个或多个计算机，并且多个计算机可以个别地实现计算机100的各功能。CPU101根据RAM102或ROM103中存储的程序执行全部过程。当CPU101进行诸如本专利技术的实施例等的各种处理时，RAM102被用作临时存储区域。输入设备105可以是测量设备(例如，传感器、加速计、陀螺仪或磁力计)或用户输入接口或网络接口，其使得用户能够向计算机100发出各种指令。输出设备106包括输出外围接口、显示设备(即，监视器、CRT、液晶显示器或图形控制器)，其使得用户能够输出本专利技术的校准的姿态结果。驱动器107被构造为驱动诸如硬盘、存储卡或光盘(例如，CD-ROM或DVDROM)等存储介质。例如，测量数据或用于进行物体姿态估计处理的程序被存储在存储介质中，并被驱动器107驱动。系统总线104连接CPU101、RAM102、ROM103、输入设备105、输出设备106和驱动器107。数据在系统本文档来自技高网...

【技术保护点】
一种依据陀螺仪数据和加速计数据的、针对运动物体的姿态估计方法，所述姿态估计方法包括：校正增益确定步骤，其用于基于利用所述陀螺仪数据和加速计数据而测量的物体运动程度，来确定校正增益；校正矢量计算步骤，其用于通过基于所述陀螺仪数据和加速计数据，将所述参考矢量从地心坐标系转换到体坐标系，来计算校正矢量；传感器数据校准步骤，其用于使用所述校正增益和所述校正矢量，来校准所述陀螺仪数据；姿态估计步骤，其用于利用经校准的陀螺仪数据来估计所述姿态，其中，所述校正增益确定步骤还包括以下步骤：基于所述加速计数据来估计在体坐标系中的重力分量；通过从所述加速度数据中去除重力分量来计算所述物体的运动加速度；基于所述物体的所述运动加速度的预定义函数来计算所述校正增益。

【技术特征摘要】
1.一种依据陀螺仪数据和加速计数据的、针对运动物体的姿态估计方法，所述姿态估计方法包括：校正增益确定步骤，其用于基于利用所述陀螺仪数据和加速计数据而测量的物体运动程度，来确定校正增益；校正矢量计算步骤，其用于通过基于所述陀螺仪数据和加速计数据，将所述参考矢量从地心坐标系转换到体坐标系，来计算校正矢量；传感器数据校准步骤，其用于使用所述校正增益和所述校正矢量，来校准所述陀螺仪数据；姿态估计步骤，其用于利用经校准的陀螺仪数据来估计所述姿态，其中，所述校正增益确定步骤还包括以下步骤：基于所述加速计数据来估计在体坐标系中的重力分量；通过从所述加速度数据中去除重力分量来计算所述物体的运动加速度；基于所述物体的所述运动加速度的预定义函数来计算所述校正增益。2.根据权利要求1所述的姿态估计方法，所述姿态估计方法还包括调整步骤，所述调整步骤基于利用当前陀螺仪数据和经校准的陀螺仪数据而计算的姿态估计偏差，来调整所述校正增益。3.根据权利要求1或2所述的姿态估计方法，其中，所述估计在体坐标系中的重力分量还包括：基于所述加速计数据、陀螺仪数据和磁力计数据中的至少一个，来确定所述物体处于静止的状态；在所述物体处于静止状态，计算所述加速度的范数；在所述物体处于静止状态，计算所述加速度的范数的平均值；利用计算出的范数的平均值，构建在所述地心坐标系中的重力矢量；基于使用陀螺仪数据而计算出的转换矩阵，将所述重力矢量从地心坐标系转换到体坐标系。4.根据权利要求1或2所述的姿态估计方法，其中，所述运动加速度的预定义函数是分段函数，并且如果所述运动加速度的范数超过预定义阈值，则将所述校正增益设置为预定义常量；否则，使用所述运动加速度的范数的连续函数来计算所述校正增益。5.根据权利要求1或2所述的姿态估计方法，其中，所述校正矢量计算步骤还包括以下步骤：基于使用陀螺仪数据计算出的转换矩阵，将所述地心坐标系中的参考矢量转换到体坐标系；基于使用加速计数据计算出的转换矩阵，或基于加速计数据和磁力计数据，将地心坐标系中的参考矢量转换到体坐标系；执行两个转换的参考矢量之间的叉积运算，以生成校正矢量。6.根据权利要求1所述的姿态估计方法，其中，所述传感器数据校准步骤还包括以下步骤：通过将校正矢量乘以校正增益，来计算矢量；将所述矢量与在体坐标系的不同轴上的当前陀螺仪数据相加。7.根据权利要求2所述的姿态估计方法，其中，所述姿态估计偏差的计算还包括以下步骤：依据陀螺仪数据分别得到当前姿态估计和经校准的姿态估计；计算所述当前姿态估计与所述经校准的姿态估计之差，以获得所述姿态估计偏差。8.根据权利要求2所述的姿态估计方法，其中，如果新姿态估计偏差与所述姿态估计偏差的平均值之间的差大于预定义阈值，则将所述校正增益强制调整为预定义值。9.根据权利要求2所述的姿态估计方法，其中，如果新姿态估计偏差与所述姿态估计偏差的平均值之间的距离大于所述姿态估计偏差的标准方差的预定义阈值倍数，则将所述校正增益强制调整为预定义值。10.根据权利要求4所述的姿态估计方法，其中，所述运动加速度的范数的所述连续函数是线性函数或高斯函数。11.根据权利要求7所述的姿态估计方法，其中，所述差是以下项目中的一个：1)所述当前陀螺仪数据的范数减去所述经校准的陀...

【专利技术属性】
技术研发人员：刘贺飞，陈玉阳，
申请(专利权)人：佳能株式会社，
类型：发明
国别省市：日本,JP