CN118057120A - 用于估计设备姿态的方法和装置 - Google Patents

用于估计设备姿态的方法和装置 Download PDF

Info

Publication number
CN118057120A
CN118057120A CN202211447184.7A CN202211447184A CN118057120A CN 118057120 A CN118057120 A CN 118057120A CN 202211447184 A CN202211447184 A CN 202211447184A CN 118057120 A CN118057120 A CN 118057120A
Authority
CN
China
Prior art keywords
system state
information
state quantity
covariance
posterior
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Pending
Application number
CN202211447184.7A
Other languages
English (en)
Inventor
黄灿
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Unikom Beijing Technology Co ltd
Original Assignee
Unikom Beijing Technology Co ltd
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Unikom Beijing Technology Co ltd filed Critical Unikom Beijing Technology Co ltd
Priority to CN202211447184.7A priority Critical patent/CN118057120A/zh
Priority to PCT/CN2023/132153 priority patent/WO2024104446A1/zh
Publication of CN118057120A publication Critical patent/CN118057120A/zh
Pending legal-status Critical Current

Links

Classifications

    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
    • G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
    • G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/20—Instruments for performing navigational calculations

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Automation & Control Theory (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Gyroscopes (AREA)

Abstract

本申请公开了用于估计设备姿态的方法。具体实现方案为：采集陀螺仪传感器和磁力计的测量值，得到当前时刻的检测信息；获取当前时刻的检测信息，执行姿态信息校准步骤：获取设备的系统状态量和系统状态协方差；基于当前时刻的角速度信息，对系统状态量进行传播，得到先验估计系统状态量；基于当前时刻的角速度信息，对系统状态协方差进行传播，得到先验估计系统状态协方差；基于当前时刻的磁场强度信息，对先验估计系统状态量和先验估计系统状态协方差更新，得到后验系统状态量和后验系统状态协方差；从后验系统状态量中获取更新后的姿态信息作为设备在当前时刻的姿态信息；响应于确定出设备处于运行状态，循环执行姿态信息校准步骤。

Description

用于估计设备姿态的方法和装置
技术领域
本申请涉及计算机技术领域，尤其涉及用于估计设备姿态的方法和装置。
背景技术
位姿可以用于描述头戴显示设备、机器人等电子设备在指定坐标系下的位置和姿态。相关技术中，各电子设备在使用过程中可以通过计算机视觉技术进行位姿追踪。并且，位姿追踪的准确程度可以影响用户在使用头戴显示设备的交互沉浸感以及机器人导航的准确性等。因此，在电子设备使用过程中，提高检测到的设备位姿的准确性成为目前亟需解决的技术问题。
发明内容
本公开的实施例提出了用于估计设备姿态的方法和装置。
第一方面，本公开的实施例提供了一种用于估计设备姿态的方法，设备包括陀螺仪传感器和磁力计，该方法包括：采集陀螺仪传感器和磁力计的测量值，得到当前时刻的检测信息，其中，检测信息包括角速度信息和磁场强度信息；获取当前时刻的检测信息，执行姿态信息校准步骤：获取设备的系统状态量和系统状态协方差，其中，系统状态量包括设备的姿态信息、陀螺仪偏置和磁力计内参偏置；基于当前时刻的角速度信息，对系统状态量进行传播，得到先验估计系统状态量；基于当前时刻的角速度信息，对系统状态协方差进行传播，得到先验估计系统状态协方差；基于当前时刻的磁场强度信息，对先验估计系统状态量和先验估计系统状态协方差更新，得到后验系统状态量和后验系统状态协方差；从后验系统状态量中获取更新后的姿态信息作为设备在当前时刻的姿态信息；响应于确定出设备处于运行状态，获取下一时刻的检测信息作为当前时刻的检测信息，以及将后验系统状态量和后验系统状态协方差确定为系统状态量和系统状态协方差，执行姿态信息校准步骤。
第二方面，本公开的实施例提供了一种用于估计设备姿态的装置，设备包括陀螺仪传感器和磁力计，该装置包括：采集单元，被配置成采集陀螺仪传感器和磁力计的测量值，得到当前时刻的检测信息，其中，检测信息包括角速度信息和磁场强度信息；获取单元，被配置成获取当前时刻的检测信息，执行姿态信息校准步骤：获取设备的系统状态量和系统状态协方差，其中，系统状态量包括设备的姿态信息、陀螺仪偏置和磁力计内参偏置；基于当前时刻的角速度信息，对系统状态量进行传播，得到先验估计系统状态量；基于当前时刻的角速度信息，对系统状态协方差进行传播，得到先验估计系统状态协方差；基于当前时刻的磁场强度信息，对先验估计系统状态量和先验估计系统状态协方差更新，得到后验系统状态量和后验系统状态协方差；从后验系统状态量中获取更新后的姿态信息作为设备在当前时刻的姿态信息；确定单元，被配置成响应于确定出设备处于运行状态，获取下一时刻的检测信息作为当前时刻的检测信息，以及将后验系统状态量和后验系统状态协方差确定为系统状态量和系统状态协方差，执行姿态信息校准步骤。
第三方面，本公开的实施例提供了一种电子设备，包括：一个或多个处理器；存储装置，其上存储有一个或多个程序，当一个或多个程序被一个或多个处理器执行，使得一个或多个处理器实现上述用于估计设备姿态的方法。
第四方面，本公开的实施例提供了一种存储有计算机指令的非瞬时计算机可读存储介质，其中，计算机指令用于使计算机执行上述用于估计设备姿态的方法。
第五方面，本公开的实施例提供了一种计算机程序产品，包括计算机程序/指令，其中，该计算机程序/指令被处理器执行时实现上述用于估计设备姿态的方法。
附图说明
通过阅读参照以下附图所作的对非限制性实施例所作的详细描述，本公开的其它特征、目的和优点将会变得更明显。
图1是应用本申请的用于估计设备姿态的方法或用于估计设备姿态的装置的实施例的示例性系统架构。
图2是根据本申请的用于估计设备姿态的方法的一个实施例的流程。
图3是根据本申请的用于估计设备姿态的方法的另一实施例的流程。
图4是根据本申请的用于估计设备姿态的方法的又一实施例的流程。
图5是根据本申请的用于估计设备姿态的装置的一个实施例的框图。
图6是适于用来实现本申请实施例的电子设备的计算机系统的结构示意图。
具体实施方式
为使本申请的目的、技术方案和优点更加清楚，下面将结合本申请具体实施例及相应的附图对本申请技术方案进行清楚、完整地描述。可以理解的是，此处所描述的具体实施例仅仅用于解释相关发明，而非对该发明的限定。另外还需要说明的是，为了便于描述，附图中仅示出了与有关发明相关的部分。
需要说明的是，在不冲突的情况下，本公开中的实施例及实施例中的特征可以相互组合。下面将参考附图并结合实施例来详细说明本公开。
图1示出了可以应用本申请的用于估计设备姿态的方法或用于估计设备姿态的装置的实施例的示例性系统架构100。
如图1所示，系统架构100可以包括终端101、待检测设备102。终端101和待检测设备102可以通过网络连接，网络连接可以包括多种方式连接，例如有线、无线通信链路或者光纤电缆等等。终端101和待检测设备102可以用过网络进行交互，以发送或接收数据信息等。
用户可以使用终端101通过网络与待检测设备102交互，以接收或发送数据消息等。终端101可以具有显示屏、中央处理单元(CPU)、存储器、I/O接口等，并可以对接收的数据信息进行处理。上述待检测设备102可以是具有陀螺仪传感器、磁力计等待检测位姿信息的各种电子设备。例如，待检测设备102可以为AR、VR等头戴显示设备。或者，待检测设备102还可以为机器人等电子设备。
终端101可以是具有计算单元的各种电子设备，包括但不限于智能手机、车机、平板电脑、电子书阅读器、膝上型便携计算机和台式计算机等等。
上述终端101还可以提供各种服务。例如，上述终端可以对待检测设备102中陀螺仪传感器和磁力计采集的数据进行处理，通过扩展的卡尔曼滤波器得到待检测设备102准确的姿态信息。
需要说明的是，本申请实施例所提供的用于估计设备姿态的方法一般由终端101执行，相应地，用于估计设备姿态的装置一般设置于终端101中。
应该理解，图1中的终端101和待检测设备102的数目仅仅是示意性的。根据实现需要，可以具有任意数目的终端101和待检测设备102。例如，图1中可以包括两个终端101，其中一个终端101可以对获取的陀螺仪传感器和磁力计的数据进行分析等处理得到待检测设备102的姿态，另一个终端101可以将姿态发送到待检测设备102以便于可以利用姿态进行显示等。
还需要指出的是，本申请公开的方案虽然可以应用于终端101，但是这也并不排除该方案也可以应用于待检测设备102。在本申请公开的方案应用于待检测设备102的情况下，上述待检测设备102可以具有计算单元，因此待检测设备102采集到陀螺仪传感器和磁力计的测量值后可以执行姿态信息校准步骤，从而可以得到校准后的位姿信息。此种情况下，用于估计设备姿态的方法可以由待检测设备102执行，相应地，用于估计设备姿态的装置也可以设置于待检测设备102中。
相关技术中，可以通过对陀螺仪采集到的角速度进行积分等方式，即可以确定出待检测设备的位姿信息。
继续参考图2，示出了根据本申请的用于估计设备姿态的方法的一个实施例的流程200。该用于估计设备姿态的方法，包括以下步骤：
步骤210，采集陀螺仪传感器和磁力计的测量值，得到当前时刻的检测信息。
在本实施例中，待检测姿态的设备至少可以包括陀螺仪传感器和磁力计。因此在设备运行时，上述陀螺仪传感器可以实时测量设备的角速度，磁力计可以实时测量设备的磁场强度。
用于估计设备姿态的方法的执行主体(例如图1所示的终端)可以采集陀螺仪传感器和磁力计传感器的测量值，从而可以得到设备当前时刻的检测信息。其中，检测信息可以包括角速度信息和磁场强度信息。可以理解的是，设备运行时，上述执行主体可以直接执行本申请公开的检测设备姿态的方法，不需要对设备执行预设操作来预先对磁力计进行标定，即可以提高检测到的设备姿态的准确性。
步骤220，获取当前时刻的检测信息，执行姿态信息校准步骤。其中，位姿信息校准可以通过步骤221-229实现。具体如下：
步骤221，获取设备的系统状态量和系统状态协方差。
在本实施例中，在获取当前时刻的角速度信息和当前时刻的磁场强度信息之后，上述执行主体可以通过各种方式获取系统状态量和系统状态量协方差。其中，在执行位姿信息校准步骤时，上述执行主体所获取的系统状态量和系统状态量协方差为当前时刻系统最新存在的系统状态量和系统状态协方差。作为示例，上述执行主体可以将上次执行姿态信息校准步骤得到后验系统状态量和后验系统状态协方差确定为本次执行姿态信息校准步骤所需要获取的系统状态量和系统状态量协方差。其中，系统状态量至少可以包括设备的姿态信息、陀螺仪偏置和磁力计内参偏置。因此，若上述执行主体所获取的系统状态量为上次执行姿态信息校准步骤得到的后验系统状态量，此时上述执行主体所获取的系统状态量可以包括上次执行姿态信息校准步骤得到的后验系统状态量，该系统状态量可以包括更新后的姿态信息、陀螺仪偏置和磁力计内参偏置。以当前时刻为k+1为例，上述执行主体所获取的系统状态量和系统状态协方差可以为上一时刻k执行姿态信息校准步骤得到的后验系统状态量xk＝[IkqG，bg，bm]和后验系统状态协方差Pk。
在一些可选的实现方式中，上述执行主体在执行本次姿态信息校准步骤所获取的系统状态量和系统状态量协方差还可以为预先构建初始化的系统状态量x＝[IqG，bg，bm]和预先构建的系统状态量协方差P0。此种情况下，上述执行主体可以将预先构建初始化的系统状态量和预先构建的系统状态量协方差分别作为上述执行主体执行姿态信息校准步骤所需获取的系统状态量和系统状态协方差，以便于执行本次姿态信息校准步骤。可以理解的是，若上述执行主体所获取的系统状态量为初始化的系统状态量，此时系统状态量可以包括初始状态下的姿态信息、陀螺仪偏置和磁力计内参偏置。
步骤223，基于当前时刻的角速度信息，对系统状态量进行传播，得到先验估计系统状态量。
在本实施例中，基于所获取的当前时刻的角速度信息，以及步骤221所获取的系统状态量，上述执行主体可以通过各种方式得到先验估计系统状态量。
在本实施例的一些可选的实现方式中，上述执行主体可以通过如下步骤得到先验估计系统状态量：基于当前时刻的角速度信息和上一时刻的角速度信息，确定设备的旋转变化量，其中，旋转变化量表征当前时刻相对于上一时刻的旋转变化程度；获取设备上一时刻的姿态信息；按照所确定的旋转变化量对所获取的上一时刻的姿态信息进行旋转计算，预测当前时刻设备的姿态信息，得到先验估计系统状态量。
在该实现方式中，上述执行主体可以得到上一时刻的角速度和当前时刻的角速度的平均角速度，通过平均角速度可以计算出陀螺仪的旋转变化量。为了进一步提高计算得到的旋转变化量的准确性，上述执行主体可以通过所获取的系统状态量中的陀螺仪偏置对当前时刻的角速度和上一时刻的角速度进行矫正，而后得到矫正后的上一时刻的角速度和当前时刻的角速度的平均角速度。
其中，设备在当前时刻k+1相对于上一时刻k的旋转变化量可以通过如下公式计算得到：
其中，为设备在当前时刻k+1相对于上一时刻k的旋转变化量，ωk用于表征k时刻矫正后的角速度，ωk+1用于表征k+1时刻矫正后的角速度，/>用于表征矫正后的角速度的平均值，Δt用于表征k时刻到k+1时刻之间的时间间隔。
其中，ωx、ωy、ωz分别表征角速度ω在预设坐标系下x、y、z轴的分量。
ωk＝ωm，k-bg，其中，ωm,k用于表征k时刻采集得到的角速度，bg用于表征所获取的状态量中的陀螺仪偏置。
ωk+1＝ωm，k+1-bg，其中，ωm,k+1用于表征k+1时刻采集得到的角速度。上述可以通过如下公式计算得到/>
在确定出之后，可以通过如下公式计算先验估计系统状态量：
其中，/>用于表征设备在当前时刻k+1的姿态，用于表征设备在上一时刻k的姿态。
还需要说明的是，系统状态量和系统状态协方差在传播的过程中(即，在k时刻到k+1时刻的过程中)，陀螺仪传感器等没有产生新的检测信息，因此，在传播的过程中bg、bm和Gm并未修正更新。综上所述，通过该实现方式计算得到的先验估计系统状态量为
可以理解的是，上述执行主体可以通过其他方式得到先验估计系统状态量。例如，上述执行主体可以直接将当前时刻矫正后的角速度确定为当前时刻的平均角速度，从而确定出设备的旋转变化量；而后获取上一时刻矫正后的姿态信息，通过所确定的旋转变化量预测当前时刻设备的先验估计系统状态量。或者，上述执行主体还可以直接根据当前时刻采集得到的角速度和上一时刻采集得到角速度计算得到设备的旋转变化量，进而计算出设备的先验估计状态量。这里对于得到先验估计系统状态量的方式没有唯一的限定。
步骤225，基于当前时刻的角速度信息，对系统状态协方差进行传播，得到先验估计系统状态协方差。
在本实施例中，基于所获取的当前时刻的角速度信息，以及步骤221所获取的系统状态协方差，上述执行主体可以通过各种方式得到先验估计系统状态协方差。
在本实施例的一些可选的实现方式中，上述执行主体可以如下方式得到先验估计系统状态协方差：基于当前时刻的角速度信息，计算预先构建的状态转移矩阵和预先构建的噪声协方差矩阵的估计序列；基于上一时刻的后验系统状态协方差，以及所确定的状态转移矩阵、噪声协方差矩阵，得到先验估计系统状态协方差。在该实现方式中，预先构建的状态转移矩阵与设备的姿态信息、陀螺仪偏置和磁力计内参偏置相关，同样地，预先构建的噪声协方差矩阵也与设备的姿态信息、陀螺仪偏置和磁力计内参偏置相关。而后，基于当前时刻的角速度信息计算预先构建的状态转移矩阵的序列和预先构建的噪声协方差矩阵的序列，具体如下：
其中，Φ为离散时间的状态转移矩阵，Θ用于表征上一时刻的姿态与下一时刻之间的状态转移矩阵，Ψ用于表征上一时刻的陀螺仪偏置与下一时刻的姿态之间的状态转移矩阵。
其中，Qd离散时间的系统噪声协方差，Q11用于表征姿态的离散时间噪声协方差，Q12用于表征姿态-陀螺仪偏置的离散时间互相关噪声协方差，Q22用于表征表示陀螺仪偏置的离散时间噪声协方差，σbm用于表征磁力计内参稳定性的系数。
基于步骤221所获取的上一时刻的系统状态协方差Pk|k，上述执行主体可以通过如下公式计算得到先验估计系统状态协方差Pk|k+1:Pk+1|k＝ΦPk|kΦT+Qd。
步骤227，基于当前时刻的磁场强度信息，对先验估计系统状态量和先验估计系统状态协方差更新，得到后验系统状态量和后验系统状态协方差。
在本实施例中，上述执行主体可以执行卡尔曼滤波的更新步骤。具体地，基于当前时刻的磁场强度信息，上述执行主体在将磁场强度信息确定为观测值之后，可以采用各种方式确定磁力计的观测残差，从而对先验估计系统状态量和先验估计系统状态协方差进行更新，得到后验系统状态量和后验系统状态协方差。作为示例，上述执行主体可以获取磁力计当前时刻的观测值、磁力计内参偏置，从而计算出磁力计的观测残差，并利用磁力计的观测残差更新先验估计系统状态量和先验估计系统状态协方差得到后验系统状态量和后验系统状态协方差。
步骤229，从后验系统状态量中获取更新后的姿态信息作为设备在当前时刻的姿态信息。
在本实施例中，上述执行主体在得到后验系统状态量之后，可以从后验系统状态量中获取其中更新后的姿态信息，并将该姿态信息确定为设备在当前时刻的姿态信息。
可以理解的是，在本次执行姿态信息校准步骤得到的后验系统状态量和后验系统状态协方差，可以作为下一时刻执行姿态信息校准步骤中所需要获取的系统状态量和系统状态协方差。
步骤230，响应于确定出设备处于运行状态，采集下一时刻的检测信息作为当前时刻的检测信息，以及将后验系统状态量和后验系统状态协方差确定为系统状态量和系统状态协方差，执行姿态信息校准步骤。
在本实施例中，上述执行主体可以判断头戴显示设备等待预测姿态信息的设备是否还处于运行状态。如果该设备还处于运行状态，该设备可以继续采集陀螺仪传感器和磁力计的测量值，因此上述执行主体可以继续获取当前时刻的检测信息。而后，上述执行主体可以将步骤227得到的后验系统状态量和后验系统状态协方差分别确定为系统状态量和系统状态协方差，以便于上述执行主体可以获取该后验系统状态量和后验系统状态协方差后继续执行下一轮的姿态信息校准步骤。可以理解的是，如果上述设备停止运行，则可以停止对设备姿态信息的预测。
可以理解的是，本次执行姿态信息校准步骤得到后验系统状态量和后验系统状态协方差后，不仅可以得到校准后的姿态信息，还可以得到修正后的陀螺仪偏置和磁力计内参偏置，而后利用校准后的姿态信息、修正后的陀螺仪偏置和磁力计内参偏置等可以再执行下一次的姿态信息校准步骤，从而可以实现在本次的基础上继续对陀螺仪偏置和磁力计内参偏置等进行修正，基于此，循环执行姿态信息可以持续修正陀螺仪偏置和磁力计内参偏置，使得校准后的姿态信息与理论值之间的差距越来越小，进而提高了得到的姿态信息的准确性。
本申请的上述实施例提供的方法，设备中包括磁力计和陀螺仪传感器，在卡尔曼滤波传播过程中输入设备的系统状态量和系统状态协方差，得到先验估计系统状态量和先验估计系统状态协方差，在卡尔曼滤波更新过程中输入先验估计系统状态量和先验估计系统状态协方差，以及磁力计的观测值，从而可以得到后验系统状态量和后验系统状态协方差，在设备运行的过程中通常可以持续得到磁场强度信息，因此在循环执行姿态信息校准步骤时，可以持续对磁力计内参偏置进行修正，从而使得磁力计内置偏置在设备使用过程中持续修正，进而反馈到下一次执行姿态信息校准步骤中，实现持续提高设备姿态信息的准确性。
接下来请参考图3，其示出了根据本申请的用于估计设备姿态的方法的另一实施例的流程300。在本实施例中，待估计姿态信息的设备还可以包括加速度传感器，因此，上述执行主体可以采集陀螺仪传感器、加速度传感器和磁力计的测量值，从而得到包括角速度信息、加速度信息和磁场强度信息的检测信息。因此，与图2所示的实施例相比，本实施例公开的用于估计设备姿态的方法中，由于存在加速度传感器，在姿态信息校准步骤中，上述执行主体在执行步骤227对应的基于磁力计的卡尔曼滤波的更新步骤之后，还可以继续执行基于加速度计的卡尔曼滤波的更新步骤。具体如下：
步骤310，在得到后验系统状态量和后验系统状态协方差之后，将所得到的后验系统状态量和后验系统状态协方差确定为第二先验估计系统状态量和第二先验估计系统状态协方差。
在本实施例中，基于步骤227得到的后验系统状态量和后验系统状态协方差，上述执行主体可以将所得到的后验系统状态量和后验系统状态协方差确定为第二先验系统状态量和第二先验系统状态协方差。
步骤320，基于当前时刻的加速度信息，对第二先验估计系统状态量和第二先验估计系统状态协方差再次更新，得到再次更新后的后验系统状态量和后验系统状态协方差。
在本实施例中，基于当前时刻的加速度信息，上述执行主体可以将加速度信息确定为观测值之后，采用各种方式确定加速度传感器的观测残差，从而对第二先验估计系统状态量和第二先验估计系统状态协方差进行更新，得到后验系统状态量和后验系统状态协方差。该后验系统状态量和后验系统状态协方差可以为再次更新后的后验系统状态量和后验系统状态协方差。可以理解的是，在本实施例中，上述执行主体可以从再次更新后的后验系统状态量中获取姿态信息，而后将该姿态信息确定为上述设备当前时刻的姿态信息。作为示例，上述执行主体可以获取加速度传感器当前时刻的观测值等，计算出加速度传感器的观测残差，并利用加速度传感器的观测残差更新第二先验估计系统状态量和第二先验估计系统状态协方差从而得到再次更新后的后验系统状态量和后验系统状态协方差。这里，第二先验估计系统状态量和第二先验估计系统状态协方差与图2所示实施例中的先验估计系统状态量和先验估计系统状态协方差不同。
在一些可选的实现方式中，与图2所示的实施例相比，该实现方式公开的用于估计设备姿态的方法中，在姿态信息校准步骤中，上述执行主体还可以在执行步骤227对应的基于磁力计的卡尔曼滤波的更新步骤之前，先执行基于加速度计的卡尔曼滤波的更新步骤。这里对两者的先后顺序没有限定。
与图2所示的实施例相比，本申请的上述实施例提供的方法，通过磁力计的观测值和加速度传感器的观测值对先验估计系统状态量和先验估计系统状态协方差进行更新，得到后验系统状态量和后验系统状态量协方差，进一步提高了陀螺仪偏置和磁力计内参偏置的修正效果，且进一步提高了校准后的姿态信息的准确性。
请继续参考图4，其示出了根据本申请的用于估计设备姿态的方法的又一实施例的流程400。在该实施例中，用于估计设备姿态的方法可以包括如下步骤：
步骤410，采集陀螺仪传感器和磁力计的测量值，得到当前时刻的检测信息。其中，检测信息包括角速度信息和磁场强度信息。
在本实施例中，步骤410与上述实施例中步骤210基本相同，这里不再赘述。
步骤420，获取当前时刻的检测信息，执行姿态信息校准步骤。姿态信息校准步骤可以如4210-4280所示：
步骤4210，获取设备的系统状态量和系统状态协方差。
其中，系统状态量可以包括设备的姿态信息、陀螺仪偏置、磁力计内参偏置和环境磁信息。
在本实施例中，在获取当前时刻的角速度信息和当前时刻的磁场强度信息之后，上述执行主体可以通过各种方式获取系统状态量和系统状态量协方差。其中，在执行位姿信息校准步骤时，上述执行主体所获取的系统状态量和系统状态量协方差为当前时刻系统最新存在的系统状态量和系统状态协方差。作为示例，上述执行主体可以将上次执行姿态信息校准步骤得到后验系统状态量和后验系统状态协方差确定为本次执行姿态信息校准步骤所需要获取的系统状态量和系统状态量协方差。其中，系统状态量可以包括设备的姿态信息、陀螺仪偏置、磁力计内参偏置和环境磁信息。因此，若上述执行主体所获取的系统状态量为上次执行姿态信息校准步骤得到的后验系统状态量，此时上述执行主体所获取的系统状态量可以包括上次执行姿态信息校准步骤得到的后验系统状态量，该系统状态量可以包括更新后的姿态信息、陀螺仪偏置、磁力计内参偏置和环境磁信息。以当前时刻为k+1为例，上述执行主体说获取的系统状态量和系统状态协方差可以为上一时刻k执行姿态信息校准步骤得到的后验系统状态量和后验系统状态协方差Pk。
在一些可选的实现方式中，上述执行主体在执行本次姿态信息校准步骤所获取的系统状态量和系统状态量协方差还可以为预先构建初始化的系统状态量x＝[IqG，bg，bm，Gm]和预先构建的系统状态量协方差P0。此种情况下，上述执行主体可以将预先构建初始化的系统状态量和预先构建的系统状态量协方差分别作为上述执行主体执行姿态信息校准步骤所需获取的系统状态量和系统状态协方差，以便于执行本次姿态信息校准步骤。可以理解的是，若上述执行主体所获取的系统状态量为初始化的系统状态量，此时系统状态量可以包括初始状态下的姿态信息、陀螺仪偏置、磁力计内参偏置和环境磁信息。
可以理解的是，设备系统中的系统状态量可以包含12个自由度的，13维的向量，包括姿态信息IqG(该姿态信息可以用四元数表示，是一个四维向量，但实际只有三个自由度)、陀螺仪偏置bg(陀螺仪偏置可以是一个三维向量)、磁力计内参偏置bm(磁力计内参偏置可以是一个三维向量)、环境磁场信息Gm(环境磁场可以是三维向量)。对应的协方差矩阵P是一个12×12的矩阵，表示系统状态量中每个自由度及其自由度之间的相关性。
其中，陀螺仪偏置bg是一个与陀螺仪传感器绑定的量，与众多因素有关，比如温度、传感器应力等，并且还是时变的。磁力计内参偏置bm可以由硬磁偏置和软磁偏置组成，而大部分的磁力计硬磁偏置与软磁偏置相比占主导作用，因此我们这里bm的建模只包含了硬磁偏置，也是一个与陀螺仪传感器绑定的时变量，通常会受到磁力计磁化、消磁等影响而发生变化。环境磁场Gm为一个与环境密切相关的量，空旷场景下可以近似为地球当地磁场，室内环境下则会受到周围磁场干扰，因此在随着时间变化地理位置发生变化时，环境磁场Gm通常会发生变化。因此，在本申请的实施例中，系统状态量不仅包括姿态信息IqG、陀螺仪偏置bg、磁力计内参偏置bm，还包括环境磁场信息Gm，在姿态信息估计过程中可以同时考虑到待检测设备随着使用地位位置变化对磁力计的影响，从而可以进一步提高估计的姿态信息的准确性。
可以理解的是，上述设备的系统状态量可以通过如下方式进行初始化。其中，设备姿态IqG的初始化：假设初始状态下设备的实际加速度为零(加速度为0时，加速度计测量值的模长基本等于重力)。因此，可根据三轴加速度计的测量值与重力的关系得到重力坐标系下的俯仰角和翻滚角，在该重力坐标系下可以令其航向角为0(即初始航向角为0，后续的航向角可以相对于该初始航向角计算)。对应地，可以设置其对应的状态协方差。
bg、bm,Gm的初始化：这三个系统状态量在有先验信息的情况下可以根据先验信息进行初始化。例如，在有先验信息时，初始化时就直接用这个赋值。如果没有先验信息则可以将其初始化为0。
进一步地，上述系统状态协方差可以通过如下方式进行初始化：在有先验信息的情况下可以设置较小的误差状态协方差；在没有先验的情况下可以设置一个较大的误差状态协方差。
步骤4220，基于当前时刻的角速度信息，对系统状态量进行传播，得到先验估计系统状态协方差。
在本实施例中，上述执行在获取当前时刻采集的角速度ωm,k+1之后，可以对系统状态量进行下一时刻的传播。具体的传播方法可以参考上述实施例中步骤223，这里不再赘述。
步骤4230，基于当前时刻的角速度信息，计算预先构建的状态转移矩阵和预先构建的噪声协方差矩阵的估计序列。
其中，预先构建的状态转移矩阵包括环境磁场信息对应的单位矩阵，预先构建的噪声协方差矩阵包括环境磁场信息对应的单位矩阵。
步骤4240，基于上一时刻的后验系统状态协方差，以及所确定的状态转移矩阵、噪声协方差矩阵，得到先验估计系统状态协方差。
在本实施例中，系统状态协方差通过如下公式进行传播：Pk+1|k＝ΦPk|kΦT+Qd，其中，Pk+1|k为当前时刻k+1的先验估计系统状态协方差，Pk|k为上一时刻k的后验系统状态协方差，Φ为离散时间的状态转移矩阵，Qd离散时间的系统噪声协方差。
其中，离散时间的状态转移矩阵通过如下公式计算：
Φ为离散时间的状态转移矩阵，Θ用于表征上一时刻的姿态与下一时刻之间的状态转移矩阵，Ψ用于表征上一时刻的陀螺仪偏置与下一时刻的姿态之间的状态转移矩阵。
其中，/>用于将/>从四元素转换成3×3的旋转矩阵。
用于表征矫正后的角速度的平均值，Δt用于表征k时刻到k+1时刻之间的时间间隔。
离散时间的系统噪声协方差通过如下公式计算：
Q11用于表征姿态的离散时间噪声协方差，Q12用于表征姿态-陀螺仪偏置的离散时间互相关噪声协方差，Q22用于表征表示陀螺仪偏置的离散时间噪声协方差，σr用于表征陀螺仪传感器的测量噪声系数，σω用于表征陀螺仪随机游走系数，σbm用于表征磁力计内参稳定性的系数，σmg用于表征环境磁场的变化程度。
步骤4250，从先验估计系统状态量中获取磁力计内参偏置和环境磁场信息。
步骤4260，将当前时刻的磁场强度信息确定为磁力计当前时刻的观测值。
步骤4270，基于磁力计当前时刻的观测值，以及所获取的磁力计内参偏置和环境磁场信息，计算磁力计的观测残差。
在本实施例中，磁力计的观测残差可以通过如下公式计算：
其中，mm可以表征磁力计当前时刻的观测值，bm可以为所获取的磁力计内参偏置，Gm可以为所获取的环境磁场信息，/>为设备在当前时刻k+1相对于上一时刻k的旋转变化量。
步骤4280，基于磁力计的观测残差，对先验估计系统状态量和先验估计系统状态协方差更新，计算后验系统状态量和后验系统状态协方差。
在本实施例中，基于步骤4270计算得到的磁力计的观测残差，可以通过如下公式计算磁力的观测残差对误差状态量对雅可比H：
其中，/>Hδθ用于表征误差对姿态的雅可比，IRG为当前系统状态量中的姿态信息对应的旋转矩阵。
用于表征误差对环境磁的雅可比。/>用于表征误差对磁力计内参偏置的雅可比。
而后，可以通过如下公式计算残差协方差S：
S＝HPk+1|kHT+R，其中，R为与磁力计的噪声相关的预设值。
卡尔曼增益：K＝Pk+1|kHTS-1
计算误差状态的修正量：
更新系统状态量：
更新系统状态协方差：Kk+1|k+1＝(I6×6-KH)Pk+1|k(I6×6-KH)T。
步骤4290，从后验系统状态量中获取更新后的姿态信息作为设备在当前时刻的姿态信息。
在本实施例中，基于步骤4270更新得到的系统状态量，上述执行主体可以从中获取姿态信息，该姿态信息可以为上述设备在当前时刻的姿态信息。
步骤430，响应于确定出设备处于运行状态，获取下一时刻的检测信息作为当前时刻的检测信息，以及将后验系统状态量和后验系统状态协方差确定为系统状态量和系统状态协方差，执行姿态信息校准步骤。
在本实施例中，步骤430与上述实施例中步骤230基本相同，这里不再赘述。
与图2对应的实施例相比，本申请的上述实施例提供的方法，在卡尔曼滤波传播过程中输入设备的系统状态量和系统状态协方差，得到先验估计系统状态量和先验估计系统状态协方差，在卡尔曼滤波更新过程中输入先验估计系统状态量和先验估计系统状态协方差，以及磁力计的观测值，从而可以得到后验系统状态量和后验系统状态协方差，在设备运行的过程中通常可以持续得到磁场强度信息，因此在循环执行姿态信息校准步骤时，考虑到随着设备使用环境磁会存在地理位置变化，此种情况下环境磁信息会相应地发生变化，本实施例公开的方法可以持续对磁力计内参偏置和环境磁场信息进行修正，从而使得磁力计内置偏置和环境磁信息在设备使用过程中持续修正，进而反馈到下一次执行姿态信息校准步骤中，实现了进一步提高设备姿态信息准确性的效果。
进一步参考图5，作为对上述各图所示方法的实现，本申请提供了一种用于估计设备姿态装置的一个实施例，该装置实施例与图2所示的方法实施例相对应，该装置具体可以应用于各种电子设备中。
如图5所示，本实施例的用于估计设备姿态的装置500，设备包括陀螺仪传感器和磁力计，装置500包括：采集单元510，被配置成采集陀螺仪传感器和磁力计的测量值，得到当前时刻的检测信息，其中，检测信息包括角速度信息和磁场强度信息；获取单元520，被配置成获取当前时刻的检测信息，执行姿态信息校准步骤：获取设备的系统状态量和系统状态协方差，其中，系统状态量包括设备的姿态信息、陀螺仪偏置和磁力计内参偏置；基于当前时刻的角速度信息，对系统状态量进行传播，得到先验估计系统状态量；基于当前时刻的角速度信息，对系统状态协方差进行传播，得到先验估计系统状态协方差；基于当前时刻的磁场强度信息，对先验估计系统状态量和先验估计系统状态协方差更新，得到后验系统状态量和后验系统状态协方差；从后验系统状态量中获取更新后的姿态信息作为设备在当前时刻的姿态信息；确定单元530，被配置成响应于确定出设备处于运行状态，获取下一时刻的检测信息作为当前时刻的检测信息，以及将后验系统状态量和后验系统状态协方差确定为系统状态量和系统状态协方差，执行姿态信息校准步骤。
在本实施例的一些可选的实现方式中，设备还包括加速度传感器，检测信息还包括加速度信息，装置500还包括：加速度信息采集单元，被配置成采集加速度传感器的测量值，得到当前时刻的加速度信息；获取单元中的姿态信息校准步骤还包括：在得到后验系统状态量和后验系统状态协方差之后，将所得到的后验系统状态量和后验系统状态协方差确定为第二先验估计系统状态量和第二先验估计系统状态协方差；基于当前时刻的加速度信息，对第二先验估计系统状态量和第二先验估计系统状态协方差再次更新，得到再次更新后的后验系统状态量和后验系统状态协方差。
在本实施例的一些可选的实现方式中，获取单元520进一步被配置成：在姿态信息校准步骤中，基于当前时刻的角速度信息，对系统状态量进行传播，得到先验估计系统状态量，包括：基于当前时刻的角速度信息和上一时刻的角速度信息，确定设备的旋转变化量，其中，旋转变化量表征当前时刻相对于上一时刻的旋转变化程度；获取设备上一时刻的姿态信息；按照所确定的旋转变化量对所获取的上一时刻的姿态信息进行旋转计算，预测当前时刻设备的姿态信息，得到先验估计系统状态量。
在本实施例的一些可选的实现方式中，系统状态量还包括环境磁场信息；获取单元520进一步被配置成：在姿态信息校准步骤中，基于当前时刻的角速度信息，对系统状态协方差进行传播，得到先验估计系统状态协方差，包括：基于当前时刻的角速度信息，计算预先构建的状态转移矩阵和预先构建的噪声协方差矩阵的估计序列，其中，预先构建的状态转移矩阵包括环境磁场信息对应的单位矩阵，预先构建的噪声协方差矩阵包括环境磁场信息对应的单位矩阵；基于上一时刻的后验系统状态协方差，以及所确定的状态转移矩阵、噪声协方差矩阵，得到先验估计系统状态协方差；在姿态信息校准步骤中，基于当前时刻的磁场强度信息，对先验估计系统状态量和先验估计系统状态协方差更新，得到后验系统状态量和后验系统状态协方差，包括：从先验估计系统状态量中获取磁力计内参偏置和环境磁场信息；将当前时刻的磁场强度信息确定为磁力计当前时刻的观测值；基于磁力计当前时刻的观测值，以及所获取的磁力计内参偏置和环境磁场信息，计算磁力计的观测残差；基于磁力计的观测残差，对先验估计系统状态量和先验估计系统状态协方差更新，计算后验系统状态量和后验系统状态协方差。
在本实施例的一些可选的实现方式中，装置500还包括初始化单元，被配置成响应于确定出设备启动，将初始化后的系统状态量和初始化后的系统状态协方差确定为设备的当前系统状态量和当前系统状态协方差。
装置500中记载的诸单元与参考图2、图3和图4描述的方法中的各个步骤相对应。由此，上文针对方法描述的操作和特征同样适用于装置500及其中包含的单元，在此不再赘述。
下面参考图6，其示出了适于用来实现本申请实施例的电子设备的计算机系统600的结构示意图。图6示出的电子设备仅仅是一个示例，不应对本申请实施例的功能和使用范围带来任何限制。
如图6所示，计算机系统600包括中央处理单元(CPU)601，其可以根据存储在只读存储器(ROM)602中的程序或者从存储部分608加载到随机访问存储器(RAM)603中的程序而执行各种适当的动作和处理。在RAM 603中，还存储有系统600操作所需的各种程序和数据。CPU 601、ROM 602以及RAM 603通过总线604彼此相连。输入/输出(I/O)接口605也连接至总线604。
以下部件连接至I/O接口605：包括键盘、鼠标等的输入部分606；包括诸如阴极射线管(CRT)、液晶显示器(LCD)等以及扬声器等的输出部分607；包括硬盘等的存储部分608；以及包括诸如LAN卡、调制解调器等的网络接口卡的通信部分609。通信部分609经由诸如因特网的网络执行通信处理。驱动器610也根据需要连接至I/O接口605。可拆卸介质611，诸如磁盘、光盘、磁光盘、半导体存储器等等，根据需要安装在驱动器610上，以便于从其上读出的计算机程序根据需要被安装入存储部分608。
特别地，根据本公开的实施例，上文参考流程图描述的过程可以被实现为计算机软件程序。例如，本公开的实施例包括一种计算机程序产品，其包括承载在计算机可读介质上的计算机程序，该计算机程序包含用于执行流程图所示的方法的程序代码。在这样的实施例中，该计算机程序可以通过通信部分609从网络上被下载和安装，和/或从可拆卸介质611被安装。在该计算机程序被中央处理单元(CPU)601执行时，执行本申请的方法中限定的上述功能。需要说明的是，本申请的计算机可读介质可以是计算机可读信号介质或者计算机可读存储介质或者是上述两者的任意组合。计算机可读存储介质例如可以是——但不限于——电、磁、光、电磁、红外线、或半导体的系统、装置或器件，或者任意以上的组合。计算机可读存储介质的更具体的例子可以包括但不限于：具有一个或多个导线的电连接、便携式计算机磁盘、硬盘、随机访问存储器(RAM)、只读存储器(ROM)、可擦式可编程只读存储器(EPROM或闪存)、光纤、便携式紧凑磁盘只读存储器(CD-ROM)、光存储器件、磁存储器件、或者上述的任意合适的组合。在本申请中，计算机可读存储介质可以是任何包含或存储程序的有形介质，该程序可以被指令执行系统、装置或者器件使用或者与其结合使用。而在本申请中，计算机可读的信号介质可以包括在基带中或者作为载波一部分传播的数据信号，其中承载了计算机可读的程序代码。这种传播的数据信号可以采用多种形式，包括但不限于电磁信号、光信号或上述的任意合适的组合。计算机可读的信号介质还可以是计算机可读存储介质以外的任何计算机可读介质，该计算机可读介质可以发送、传播或者传输用于由指令执行系统、装置或者器件使用或者与其结合使用的程序。计算机可读介质上包含的程序代码可以用任何适当的介质传输，包括但不限于：无线、电线、光缆、RF等等，或者上述的任意合适的组合。
可以以一种或多种程序设计语言或其组合来编写用于执行本申请的操作的计算机程序代码，程序设计语言包括面向对象的程序设计语言—诸如Java、Smalltalk、C++，还包括常规的过程式程序设计语言—诸如“C”语言或类似的程序设计语言。程序代码可以完全地在用户计算机上执行、部分地在用户计算机上执行、作为一个独立的软件包执行、部分在用户计算机上部分在远程计算机上执行、或者完全在远程计算机或服务器上执行。在涉及远程计算机的情形中，远程计算机可以通过任意种类的网络——包括局域网(LAN)或广域网(WAN)—连接到用户计算机，或者，可以连接到外部计算机(例如利用因特网服务提供商来通过因特网连接)。
附图中的流程图和框图，图示了按照本申请各种实施例的系统、方法和计算机程序产品的可能实现的体系架构、功能和操作。在这点上，流程图或框图中的每个方框可以代表一个模块、程序段、或代码的一部分，该模块、程序段、或代码的一部分包含一个或多个用于实现规定的逻辑功能的可执行指令。也应当注意，在有些作为替换的实现中，方框中所标注的功能也可以以不同于附图中所标注的顺序发生。例如，两个接连地表示的方框实际上可以基本并行地执行，它们有时也可以按相反的顺序执行，这依所涉及的功能而定。也要注意的是，框图和/或流程图中的每个方框、以及框图和/或流程图中的方框的组合，可以用执行规定的功能或操作的专用的基于硬件的系统来实现，或者可以用专用硬件与计算机指令的组合来实现。
描述于本申请实施例中所涉及到的单元可以通过软件的方式实现，也可以通过硬件的方式来实现。所描述的单元也可以设置在处理器中，例如，可以描述为：一种处理器包括采集单元、获取单元和确定单元。其中，这些单元的名称在某种情况下并不构成对该单元本身的限定，例如，采集单元还可以被描述为“采集陀螺仪传感器和磁力计的测量值，得到当前时刻的检测信息的单元”。
作为另一方面，本申请还提供了一种计算机可读介质，该计算机可读介质可以是上述实施例中描述的装置中所包含的；也可以是单独存在，而未装配入该装置中。上述计算机可读介质承载有一个或者多个程序，当上述一个或者多个程序被该装置执行时，使得该装置：采集陀螺仪传感器和磁力计的测量值，得到当前时刻的检测信息，其中，检测信息包括角速度信息和磁场强度信息；
获取当前时刻的检测信息，执行姿态信息校准步骤：获取设备的系统状态量和系统状态协方差，其中，系统状态量包括设备的姿态信息、陀螺仪偏置和磁力计内参偏置；基于当前时刻的角速度信息，对系统状态量进行传播，得到先验估计系统状态量；基于当前时刻的角速度信息，对系统状态协方差进行传播，得到先验估计系统状态协方差；基于当前时刻的磁场强度信息，对先验估计系统状态量和先验估计系统状态协方差更新，得到后验系统状态量和后验系统状态协方差；从后验系统状态量中获取更新后的姿态信息作为设备在当前时刻的姿态信息；
响应于确定出设备处于运行状态，获取下一时刻的检测信息作为当前时刻的检测信息，以及将后验系统状态量和后验系统状态协方差确定为系统状态量和系统状态协方差，执行姿态信息校准步骤。
以上描述仅为本申请的较佳实施例以及对所运用技术原理的说明。本领域技术人员应当理解，本申请中所涉及的发明范围，并不限于上述技术特征的特定组合而成的技术方案，同时也应涵盖在不脱离上述发明构思的情况下，由上述技术特征或其等同特征进行任意组合而形成的其它技术方案。例如上述特征与本申请中公开的(但不限于)具有类似功能的技术特征进行互相替换而形成的技术方案。
以上描述仅为本公开的较佳实施例以及对所运用技术原理的说明。本领域技术人员应当理解，本公开的实施例中所涉及的发明范围，并不限于上述技术特征的特定组合而成的技术方案，同时也应涵盖在不脱离上述发明构思的情况下，由上述技术特征或其等同特征进行任意组合而形成的其它技术方案。例如上述特征与本公开的实施例中公开的(但不限于)具有类似功能的技术特征进行互相替换而形成的技术方案。

Claims (10)

1.一种用于估计设备姿态的方法，所述设备包括陀螺仪传感器和磁力计，所述方法包括：
采集所述陀螺仪传感器和磁力计的测量值，得到当前时刻的检测信息，其中，所述检测信息包括角速度信息和磁场强度信息；
获取当前时刻的检测信息，执行姿态信息校准步骤：获取所述设备的系统状态量和系统状态协方差，其中，所述系统状态量包括所述设备的姿态信息、陀螺仪偏置和磁力计内参偏置；基于当前时刻的角速度信息，对所述系统状态量进行传播，得到先验估计系统状态量；基于当前时刻的角速度信息，对所述系统状态协方差进行传播，得到先验估计系统状态协方差；基于当前时刻的磁场强度信息，对所述先验估计系统状态量和先验估计系统状态协方差更新，得到后验系统状态量和后验系统状态协方差；从所述后验系统状态量中获取更新后的姿态信息作为所述设备在当前时刻的姿态信息；
响应于确定出所述设备处于运行状态，获取下一时刻的检测信息作为当前时刻的检测信息，以及将所述后验系统状态量和后验系统状态协方差确定为所述系统状态量和系统状态协方差，执行所述姿态信息校准步骤。
2.根据权利要求1所述的方法，其中，所述设备还包括加速度传感器，所述检测信息还包括加速度信息，所述方法还包括：
采集所述加速度传感器的测量值，得到当前时刻的加速度信息；
所述姿态信息校准步骤还包括：
在得到所述后验系统状态量和后验系统状态协方差之后，将所得到的后验系统状态量和后验系统状态协方差确定为第二先验估计系统状态量和第二先验估计系统状态协方差；
基于当前时刻的加速度信息，对所述第二先验估计系统状态量和第二先验估计系统状态协方差再次更新，得到再次更新后的后验系统状态量和后验系统状态协方差。
3.根据权利要求1所述的方法，其中，所述基于当前时刻的角速度信息，对所述系统状态量进行传播，得到先验估计系统状态量，包括：
基于当前时刻的角速度信息和上一时刻的角速度信息，确定所述设备的旋转变化量，其中，所述旋转变化量表征当前时刻相对于上一时刻的旋转变化程度；
获取所述设备上一时刻的姿态信息；
按照所确定的旋转变化量对所获取的上一时刻的姿态信息进行旋转计算，预测当前时刻所述设备的姿态信息，得到先验估计系统状态量。
4.根据权利要求1-3之一所述的方法，其中，所述系统状态量还包括环境磁场信息；
所述基于当前时刻的角速度信息，对所述系统状态协方差进行传播，得到先验估计系统状态协方差，包括：
基于当前时刻的角速度信息，计算预先构建的状态转移矩阵和预先构建的噪声协方差矩阵的估计序列，其中，所述预先构建的状态转移矩阵包括环境磁场信息对应的单位矩阵，所述预先构建的噪声协方差矩阵包括环境磁场信息对应的单位矩阵；
基于上一时刻的后验系统状态协方差，以及所确定的状态转移矩阵、噪声协方差矩阵，得到先验估计系统状态协方差；
所述基于当前时刻的磁场强度信息，对所述先验估计系统状态量和先验估计系统状态协方差更新，得到后验系统状态量和后验系统状态协方差，包括：
从先验估计系统状态量中获取磁力计内参偏置和环境磁场信息；
将所述当前时刻的磁场强度信息确定为所述磁力计当前时刻的观测值；
基于所述磁力计当前时刻的观测值，以及所获取的磁力计内参偏置和环境磁场信息，计算所述磁力计的观测残差；
基于所述磁力计的观测残差，对所述先验估计系统状态量和先验估计系统状态协方差更新，计算后验系统状态量和后验系统状态协方差。
5.根据权利要求1所述的方法，其中，所述方法还包括：
响应于确定出所述设备启动，将初始化后的系统状态量和初始化后的系统状态协方差确定为所述设备的当前系统状态量和当前系统状态协方差。
6.一种用于估计设备姿态的装置，所述设备包括陀螺仪传感器和磁力计，所述装置包括：
采集单元，被配置成采集所述陀螺仪传感器和磁力计的测量值，得到当前时刻的检测信息，其中，所述检测信息包括角速度信息和磁场强度信息；
获取单元，被配置成获取当前时刻的检测信息，执行姿态信息校准步骤：获取所述设备的系统状态量和系统状态协方差，其中，所述系统状态量包括所述设备的姿态信息、陀螺仪偏置和磁力计内参偏置；基于当前时刻的角速度信息，对所述系统状态量进行传播，得到先验估计系统状态量；基于当前时刻的角速度信息，对所述系统状态协方差进行传播，得到先验估计系统状态协方差；基于当前时刻的磁场强度信息，对所述先验估计系统状态量和先验估计系统状态协方差更新，得到后验系统状态量和后验系统状态协方差；从所述后验系统状态量中获取更新后的姿态信息作为所述设备在当前时刻的姿态信息；
确定单元，被配置成响应于确定出所述设备处于运行状态，获取下一时刻的检测信息作为当前时刻的检测信息，以及将所述后验系统状态量和后验系统状态协方差确定为所述系统状态量和系统状态协方差，执行所述姿态信息校准步骤。
7.根据权利要求6所述的装置，其中，所述系统状态量还包括环境磁场信息；
所述获取单元进一步被配置成：
在所述姿态信息校准步骤中，所述基于当前时刻的角速度信息，对所述系统状态协方差进行传播，得到先验估计系统状态协方差，包括：
基于当前时刻的角速度信息，计算预先构建的状态转移矩阵和预先构建的噪声协方差矩阵的估计序列，其中，所述预先构建的状态转移矩阵包括环境磁场信息对应的单位矩阵，所述预先构建的噪声协方差矩阵包括环境磁场信息对应的单位矩阵；
基于上一时刻的后验系统状态协方差，以及所确定的状态转移矩阵、噪声协方差矩阵，得到先验估计系统状态协方差；
在所述姿态信息校准步骤中，所述基于当前时刻的磁场强度信息，对所述先验估计系统状态量和先验估计系统状态协方差更新，得到后验系统状态量和后验系统状态协方差，包括：
从先验估计系统状态量中获取磁力计内参偏置和环境磁场信息；
将所述当前时刻的磁场强度信息确定为所述磁力计当前时刻的观测值；
基于所述磁力计当前时刻的观测值，以及所获取的磁力计内参偏置和环境磁场信息，计算所述磁力计的观测残差；
基于所述磁力计的观测残差，对所述先验估计系统状态量和先验估计系统状态协方差更新，计算后验系统状态量和后验系统状态协方差。
8.一种电子设备，包括：
一个或多个处理器；
存储装置，其上存储有一个或多个程序，
当所述一个或多个程序被所述一个或多个处理器执行，使得所述一个或多个处理器实现如权利要求1-5中任一所述的方法。
9.一种存储有计算机指令的非瞬时计算机可读存储介质，其中，所述计算机指令用于使所述计算机执行权利要求1-5中任一项所述的方法。
10.一种计算机程序产品，包括计算机程序/指令，其中，该计算机程序/指令被处理器执行时实现权利要求1-5中任一项所述的方法。
CN202211447184.7A 2022-11-18 2022-11-18 用于估计设备姿态的方法和装置 Pending CN118057120A (zh)

Priority Applications (2)

Application Number Priority Date Filing Date Title
CN202211447184.7A CN118057120A (zh) 2022-11-18 2022-11-18 用于估计设备姿态的方法和装置
PCT/CN2023/132153 WO2024104446A1 (zh) 2022-11-18 2023-11-16 用于估计设备姿态的方法和装置

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202211447184.7A CN118057120A (zh) 2022-11-18 2022-11-18 用于估计设备姿态的方法和装置

Publications (1)

Publication Number Publication Date
CN118057120A true CN118057120A (zh) 2024-05-21

Family

ID=91068806

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202211447184.7A Pending CN118057120A (zh) 2022-11-18 2022-11-18 用于估计设备姿态的方法和装置

Country Status (2)

Country Link
CN (1) CN118057120A (zh)
WO (1) WO2024104446A1 (zh)

Family Cites Families (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JP2013061309A (ja) * 2011-09-15 2013-04-04 Yamaha Corp カルマンフィルタ、状態推定装置、カルマンフィルタの制御方法、及びカルマンフィルタの制御プログラム
CN107272718B (zh) * 2017-06-19 2020-07-24 歌尔科技有限公司 基于卡尔曼滤波的姿态控制方法及装置
CN109001787B (zh) * 2018-05-25 2022-10-21 北京大学深圳研究生院 一种姿态角解算与定位的方法及其融合传感器
CN108534772B (zh) * 2018-06-24 2021-07-02 西宁泰里霍利智能科技有限公司 姿态角获取方法及装置
CN113155129B (zh) * 2021-04-02 2022-07-01 北京大学 一种基于扩展卡尔曼滤波的云台姿态估计方法
CN113670314B (zh) * 2021-08-20 2023-05-09 西南科技大学 基于pi自适应两级卡尔曼滤波的无人机姿态估计方法

Also Published As

Publication number Publication date
WO2024104446A1 (zh) 2024-05-23

Similar Documents

Publication Publication Date Title
CN115265523B (zh) 机器人同时定位与建图方法、装置及可读介质
CN110197615B (zh) 用于生成地图的方法及装置
CN111551174A (zh) 基于多传感器惯性导航系统的高动态车辆姿态计算方法及系统
US20140195185A1 (en) Angular Velocity Estimation Using a Magnetometer and Accelerometer
CN110567491B (zh) 惯性导航系统的初始对准方法、装置及电子设备
US20180356227A1 (en) Method and apparatus for calculation of angular velocity using acceleration sensor and geomagnetic sensor
CN109781117B (zh) 一种组合定位方法和系统
CN113188505B (zh) 姿态角度的测量方法、装置、车辆及智能臂架
US10168156B2 (en) Orient a mobile device coordinate system to a vehicular coordinate system
CN117191080A (zh) 相机与imu外参的标定方法、装置、设备及存储介质
CN114964214B (zh) 一种航姿参考系统的扩展卡尔曼滤波姿态解算方法
CN118057120A (zh) 用于估计设备姿态的方法和装置
CN113465632B (zh) 传感器的校准方法、装置、设备和介质
CN114001730B (zh) 融合定位方法、装置、计算机设备和存储介质
CN115727871A (zh) 一种轨迹质量检测方法、装置、电子设备和存储介质
CN115900697A (zh) 对象运动轨迹信息处理方法、电子设备以及自动驾驶车辆
CN114636418A (zh) 一种基于mems测量姿态相对变化的方法、装置及电子设备
CN113946151A (zh) 针对自动驾驶车辆的数据处理方法、装置和自动驾驶车辆
CN113379852A (zh) 用于验证相机标定结果的方法、装置、电子设备和介质
CN110033088B (zh) 估计gps数据的方法和装置
CN112595330A (zh) 车辆定位方法、装置、电子设备和计算机可读介质
CN111767513A (zh) 数据处理方法及装置、电子设备和计算机可读存储介质
CN114563018B (zh) 用于校准头戴显示设备的方法和装置
CN112880675B (zh) 用于视觉定位的位姿平滑方法、装置、终端和移动机器人
CN111536968B (zh) 确定路侧感知设备的动态姿态的方法和装置

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination