CN106767776A - 移动设备及求取移动设备姿态的方法 - Google Patents

Abstract

本发明提供一种移动设备及求取移动设备姿态的方法。移动设备包括加速度传感器、磁力计传感器、陀螺仪传感器以及处理器。加速度传感器、磁力计传感器以及陀螺仪传感器在采样时刻分别产生移动设备的加速度数据、磁场强度数据以及角速度数据。处理器根据加速度数据决定对应的俯仰角及滚动角，根据磁场强度数据决定对应的偏航角，决定俯仰角、滚动角以及偏航角所对应的四元数，根据上述采样时刻的四元数及角速度数据决定当前时刻移动设备的预测的姿态信息，以及根据加速度数据及磁场强度数据对预测的姿态信息进行修正。本发明能够更精准地反应移动设备的实际姿态。

Description

移动设备及求取移动设备姿态的方法

技术领域

本发明主要关于移动装置的姿态信息求取方案，特别有关于整合由多种不同的传感器所产生的数据来求取移动装置的姿态信息的技术提案。

背景技术

随着移动设备的普及，体感技术相关的应用在近年来成为热门的发展领域，有越来越多的应用能够通过移动设备内建的传感器来取得移动设备的姿态信息，用以提供更为人性化及智能的人机互动方式，其应用领域除了游戏设计之外，还可运用在室内/室外导航***、医疗看护、室内导览等，适用范围相当多元。

在一传统做法中，移动设备里除了主处理器之外，并未额外配置专属的处理器用于控制传感器，因此，即便在移动设备处于休眠模式时，主处理器仍需对传感器进行控制以维持对姿态信息的即时掌握，若是为了确保姿态信息的精确度，主处理器必须对传感器进行频繁的控制，此将造成主处理器的负载及功耗增加；然而，若是为了降低主处理器的负载及功耗而降低对传感器进行控制的频率，则会牺牲姿态信息的精确度。

发明内容

本发明的一实施例提供了一种移动设备，其包括一加速度传感器、一磁力计传感器、一陀螺仪传感器以及一处理器。上述加速度传感器用以产生一加速度数据。上述磁力计传感器用以产生一磁场强度数据。上述陀螺仪传感器用以产生一角速度数据。上述处理器用以根据上述加速度数据决定一俯仰角及一滚动角，根据上述磁场强度数据决定一偏航角，决定上述俯仰角、上述滚动角以及上述偏航角所对应的一四元数，根据上述四元数及上述角速度数据决定上述移动设备的一姿态信息，以及根据上述加速度数据及上述磁场强度数据对上述姿态信息进行修正。

本发明的另一实施例提供了一种求取移动设备姿态的方法，适用于包括一加速度传感器、一磁力计传感器以及一陀螺仪传感器的一移动设备。上述求取移动设备姿态的方法包括以下步骤：根据由一加速度传感器产生的一加速度数据决定一俯仰角及一滚动角；根据由一磁力计传感器产生的一磁场强度数据决定一偏航角；决定上述俯仰角、上述滚动角以及上述偏航角所对应的一四元数；根据上述四元数以及由一陀螺仪传感器产生的一角速度数据决定上述移动设备的一姿态信息；以及根据上述加速度数据及上述偏航角对上述姿态信息进行修正。

本发明能够更精准地反应移动设备的实际姿态。

关于本发明其他附加的特征与优点，此领域的技术人员，在不脱离本发明的精神和范围内，当可根据本发明实施方法中所揭露的移动设备及求取移动设备姿态的方法做些许的更动与润饰而得到。

附图说明

图1是根据本发明一实施例所述的移动设备的示意图。

图2是根据本发明一实施例所述的传感器中枢20的示意图。

图3是根据本发明一实施例所述的求取移动设备姿态的方法流程图。

图4是根据本发明一实施例所述的参考坐标系及载体坐标系的示意图。

图5是根据本发明一实施例所述的移动设备的运动示意图。

图6是根据本发明一实施例所述针对移动设备侦测磁场强度向量的示意图。

其中，附图中符号的简单说明如下：

100：移动设备；10：主控制器；20：传感器中枢；21：微处理器；22：存储器；23：传输接口；30：加速度传感器；40：磁力计传感器；50：陀螺仪传感器；60：输入输出装置；70：储存装置；S301～S311：步骤。

具体实施方式

本部分所叙述的是实施本发明的最佳方式，目的在于说明本申请的精神而非用以限定本申请的保护范围，应理解下列实施例可经由软件、硬件、固件、或上述任意组合来实现。

图1是根据本发明一实施例所述的移动设备的示意图。移动设备100包括主控制器10、传感器中枢(sensor hub)20、加速度传感器30、磁力计传感器40、陀螺仪传感器50、输入输出装置60以及储存装置70，其中加速度传感器30、磁力计传感器40、陀螺仪传感器50操作上耦合至传感器中枢20，而传感器中枢20、输入输出装置60以及储存装置70操作上耦合至主控制器10。

移动设备100可为功能型手机、智能手机、平板计算机、智能交通载具、无人机、智能电子玩具、或任何移动式电子计算设备。

主控制器10可为通用处理器、微处理器(Micro Control Unit，MCU)、应用处理器(Application Processor，AP)、或数字信号处理器(Digital Signal Processor，DSP)等。明确来说，主控制器10用以提供数据处理及运算的功能、控制传感器中枢20的运作并接收传感器中枢20所输出的姿态信息、从输入输出装置60接收外部信号(例如：使用者输入信号)或输出信号以及从储存装置70读取或储存数据。

传感器中枢20主要用以控制加速度传感器30、磁力计传感器40以及陀螺仪传感器50的运作，例如：控制各传感器的采样率。此外，还用以接收传感器所产生的量测数据，并对量测数据进行处理以产生姿态信息。值得注意的是，相较于主控制器10，传感器中枢20仅需低电量即可运作。

加速度传感器30用以侦测移动设备100的加速度并产生加速度数据。

磁力计传感器40用以侦测移动设备100的磁场强度变化并产生磁场强度数据。

陀螺仪传感器50用以侦测移动设备100的角速度并产生角速度数据。

在一实施例，加速度传感器30、磁力计传感器40以及陀螺仪传感器50可分别包括一或多个硬件元件，以机构式、电子式、或电容式的方式实现数据量测的目的。

输入输出装置60可包括一或多个按钮、键盘、鼠标、触摸板、视频镜头、麦克风、显示屏幕(例如：液晶显示器、发光二极管显示器、或电子纸显示器等)以及/或喇叭等，用以作为人机界面与使用者互动。

储存装置70为非暂态(non-transitory)的计算机可读取储存介质，例如：随机存取存储器、快闪存储器、磁性储存装置(如：磁带或硬盘)、光学储存装置(如：只读光盘)、或上述的任意组合，用以储存指令集、应用及/或通信协议的程序码等。

当可理解的是，图1所示的元件仅用以提供一说明的范例，并非用以限制本发明的保护范围。举例来说，移动设备100还可包括一无线通信装置(包括：基频信号处理元件、射频信号处理元件以及天线)，用以提供无线通信的功能，或者，移动设备100还可包括一电源供应器、全球定位***仪等。

图2是根据本发明一实施例所述的传感器中枢20的示意图。传感器中枢20包括微处理器21、存储器22以及传输接口23，其中存储器22及传输接口23操作上耦合至微处理器21，而传感器中枢20可通过传输接口23操作上耦合至主控制器10、加速度传感器30、磁力计传感器40以及陀螺仪传感器50。

微处理器21可为一微型处理器，例如：微控制器(Micro-Control Unit，MCU)，其可包括各式电路逻辑，用以通过传输接口23控制加速度传感器30、磁力计传感器40以及陀螺仪传感器50的运作、处理各个传感器的量测数据以决定移动设备100的姿态信息以及从存储器22中读取或写入数据。特别是，微处理器21用以协调控制加速度传感器30、磁力计传感器40以及陀螺仪传感器50的运作，以执行本发明的求取移动设备姿态的方法。

本领域技术人员当可理解，微处理器21中的电路逻辑通常可包括多个晶体管，用以控制该电路逻辑的运作以提供所需的功能及作业。更进一步的，晶体管的特定结构及其之间的连结关系通常是由编译器所决定，例如：暂存器传输语言(Register TransferLanguage，RTL)编译器可由处理器所运作，将类似组合语言码的脚本(script)编译成适用于设计或制造该电路逻辑所需的形式。

存储器22可为非暂态的计算机可读取储存介质，例如：随机存取存储器、或快闪存储器，用以储存本发明的方法的指令集或程序码。另外，存储器22还可储存来自加速度传感器30、磁力计传感器40以及陀螺仪传感器50的量测数据以及数据运算过程中的暂存数据。

传输接口23用以提供传感器中枢20与传感器中枢20中的其他元件之间的数据及信号传输与接收，包括：接收来自加速度传感器30、磁力计传感器40以及陀螺仪传感器50的量测数据，传送控制信号至加速度传感器30、磁力计传感器40以及陀螺仪传感器50，接收来自主控制器10的控制信号，以及传送移动设备100的姿态信息给主控制器10。

图3是根据本身请一实施例所述的求取移动设备姿态的方法流程图。在此实施例，本发明所述的求取移动设备姿态的方法适用于移动设备100，特别由传感器中枢20的微处理器21所执行。

首先，在步骤S301，微处理器21决定以导航坐标系(North East Down，NED)为参考坐标系O-X_nY_nZ_n，如图4所示，即x轴指向正北方(以N标示)、y轴指向正东方(以E标示)、z轴往下指向地心(以D标示)，同时，将移动设备100平放于例如桌面的水平平面，决定移动设备100的载体坐标系(body frame)O-X_bY_bZ_b，即x轴指向移动设备100的前方(以x标示)、y轴指向移动设备100的右方(以y标示)、z轴指向移动设备100的下方(以z标示)。

接着，在步骤S302，微处理器21决定参考坐标系与载体坐标系之间的坐标变换矩阵。明确来说，当移动设备100的空间姿态因一运动而产生变化时，其姿态(orientation)可视为是在导航坐标系中依次绕z轴、y轴、x轴作3次基本旋转(依次以偏航(Yaw)、俯仰(Pitch)、滚动(Roll)标示)而得到的结果。其中，绕z轴旋转的角度可定义为偏航角ψ，绕y轴旋转的角度可定义为俯仰角θ，绕x轴旋转的角度可定义为滚动角φ。

根据上述定义的欧拉角(由偏航角ψ、俯仰角θ、滚动角φ所构成)，进一步决定空间变换的关系可如下表示：

其中，由坐标系O－X_nY_nZ_n绕Z_n轴旋转ψ角度而得到坐标系O－X₁Y₁Z₁，其变换的方向余弦矩阵为R_z(ψ)；由坐标系O－X₁Y₁Z₁绕Y₁轴旋转θ角度而得到坐标系O－X₂Y₂Z₂，其变换的方向余弦矩阵为R_x(θ)；由坐标系O－X₂Y₂Z₂绕X₂轴旋转φ角度而得到坐标系O－X_bY_bZ_b，其变换的方向余弦矩阵为R_x(φ)。

其中，坐标系O－X_nY_nZ_n为参考坐标系(即导航坐标系)，坐标系O－X_bY_bZ_b为载体坐标系。根据上述空间变换的关系，决定导航坐标系与载体坐标系之间的坐标变换矩阵，如下公式(1)所示：

由于旋转属于正交的转换关系，而正交矩阵的性质可表示为以下公式(2)：

其中，坐标变换矩阵即为姿态矩阵，其可表示为以下公式(3)：

在确定坐标系以及姿态矩阵之后，流程前进到步骤S303，微处理器21首先对加速度传感器30所侦测到的加速度数据进行向量标准化处理，得到标准化的加速度向量。在一实施例，假设{x,y,z}为代表加速度数据的向量值，则向量标准化处理可以是如以下公式(4)所示：

在步骤S304，微处理器21根据标准化的加速度向量决定对应的俯仰角θ及滚动角φ。明确来说，移动设备100的运动如图5所示，其中，坐标系O－X_nY_nZ_n为参考坐标系(即导航坐标系)，坐标系O－X_bY_bZ_b为载体坐标系，并通过以下公式(5)计算出俯仰角θ及滚动角φ。

其中，[A_x,A_y,A_z]^T为标准化的加速度向量。

在步骤S305，微处理器21根据磁力计传感器40所产生的磁场强度数据(即磁场强度向量)决定对应的偏航角ψ。明确来说，磁力计传感器40对移动设备100的侦测如图6所示，其中，与为磁场强度在水平坐标系的x轴与y轴上的分量，而磁力计传感器40针对3个轴所测得的地磁强度与姿态矩阵之间的关系可表示为以下公式(6)。

故可通过上述公式(6)计算出偏航角

在步骤S306，微处理器21根据步骤S304及S305所得到的欧拉角来计算其相应的四元数q＝[q₀,q₁,q₂,q₃]^T。其中，q₀、q₁、q₂、q₃由以下公式(7)计算得到。

在步骤S307，微处理器21对四元数q进行向量标准化处理，得到标准化的四元数Q。在一实施例，向量标准化处理可以是如以下公式(8)所示。

在步骤S308，微处理器21对标准化的四元数进行求异，其具体计算过程如以下公式(9)。

其中，w为陀螺仪传感器50针对移动设备100所侦测到的角速度，Q为步骤S307所得到的标准化的四元数。根据公式(9)可进一步得到以下公式(10)。

接着对公式(10)进行泰勒展开(具体来说是对公式中以e为底的指数部分进行泰勒展开)则可得到四元数的更新公式，即扩展卡尔曼滤波器中的状态转移矩阵F。

在步骤S309，微处理器21使用扩展卡尔曼滤波器将四元数作为估计量建立如下状态预测公式(11)：

P_t ^-＝FP_t-1F^T+Q (11)

其中，F为步骤S380所得到的状态转移矩阵，为使用四元数表示的预测姿态(q₀,q₁,q₂,q₃)，为使用四元数表示的上一时刻的最佳估计量，P_t-1为上一时刻的斜方差矩阵，P_t ^-为当前时刻推测的斜方差矩阵，Q为预测模型所引入的噪声斜方差矩阵，针对不同的传感器及不同的应用场景，Q的值可作适当调整，例如：一个合理的值可以是10^-4量级。也就是说，通过公式(11)可得到移动设备100的预测姿态信息。

在步骤S310，微处理器21根据扩展卡尔曼滤波器的状态转移矩阵以及加速度传感器30的侦测数据(即加速度数据)对步骤S309所得到的预测姿态信息进行修正。明确来说，量测方程如以下公式(12)所示。

其中(C₀₂,C₁₂,C₂₂)’为姿态矩阵的第三列，接着再根据如以下公式(13)计算出修正后的状态和协方差P_t ⁺。

其中Z_t为当前时刻加速度传感器30的量测，为当前的测量预测值。

在步骤S311，微处理器21根据扩展卡尔曼滤波器的状态转移矩阵以及磁力计传感器40的侦测数据(即磁场强度数据)对步骤S309所得到的预测姿态信息进行修正。明确来说，量测方程如以下公式(14)所示。

其中C₀₁为姿态矩阵的第一行第二列元素，C₀₀为姿态矩阵的第一行第一列元素。

在步骤S311结束之后所得到的四元数即为完成修正的预测姿态信息，预期将更精准的反应移动设备100的实际姿态。在一实施例，微处理器21还可进一步将四元数转换为相应的欧拉角。

根据图3的实施例，当可理解的是，本发明的求取移动设备姿态的方法的特征在于使用扩展卡尔曼滤波器来预测移动设备的姿态，此外，还进一步将加速度传感器及磁力计传感器的侦测数据反馈对预测姿态信息进行修正。由于陀螺仪传感器在静态时容易随着时间而产生较大误差的侦测数据，所以本发明通过加速度传感器及磁力计传感器的再次反馈来修正预测姿态信息，预期将比传统作法能更精准的反应移动设备的实际姿态。

以上所述仅为本发明较佳实施例，然其并非用以限定本发明的范围，任何熟悉本项技术的人员，在不脱离本发明的精神和范围内，可在此基础上做进一步的改进和变化，因此本发明的保护范围当以本申请的权利要求书所界定的范围为准。

Claims (16)

1.一种移动设备，其特征在于，包括：

加速度传感器，用以产生加速度数据；

磁力计传感器，用以产生磁场强度数据；

陀螺仪传感器，用以产生角速度数据；以及

处理器，用以根据上述加速度数据决定俯仰角及滚动角，根据上述磁场强度数据决定偏航角，决定上述俯仰角、上述滚动角以及上述偏航角所对应的四元数，根据上述四元数及上述角速度数据决定上述移动设备的姿态信息，以及根据上述加速度数据及上述磁场强度数据对上述姿态信息进行修正。

2.根据权利要求1所述的移动设备，其特征在于，上述处理器根据上述四元数及上述角速度数据决定扩展卡尔曼滤波器中的状态转移矩阵，以及使用上述扩展卡尔曼滤波器并根据上述状态转移矩阵及上述四元数决定上述姿态信息。

3.根据权利要求2所述的移动设备，其特征在于，上述处理器使用上述扩展卡尔曼滤波器并根据上述状态转移矩阵及上述加速度数据对上述姿态信息进行修正，以及使用上述扩展卡尔曼滤波器并根据上述状态转移矩阵及上述磁场强度数据对上述姿态信息进行修正。

4.根据权利要求1所述的移动设备，其特征在于，上述姿态信息包括估计的四元数。

5.根据权利要求4所述的移动设备，其特征在于，上述处理器还决定上述估计的四元数所对应的欧拉角。

6.根据权利要求1所述的移动设备，其特征在于，上述处理器还决定以导航坐标系为决定上述俯仰角、上述滚动角以及上述偏航角的参考坐标系，以及决定上述导航坐标系与上述移动设备的载体坐标系之间的坐标变换矩阵。

7.根据权利要求1所述的移动设备，其特征在于，上述处理器还在决定上述俯仰角及上述滚动角之前先对上述加速度数据进行向量标准化处理。

8.根据权利要求1所述的移动设备，其特征在于，上述处理器还在决定上述姿态信息之前先对上述四元数进行向量标准化处理。

9.一种求取移动设备姿态的方法，其特征在于，适用于包括加速度传感器、磁力计传感器以及陀螺仪传感器的移动设备，上述求取移动设备姿态的方法包括：

根据由上述加速度传感器产生的加速度数据决定俯仰角及滚动角；

根据由上述磁力计传感器产生的磁场强度数据决定偏航角；

决定上述俯仰角、上述滚动角以及上述偏航角所对应的四元数；

根据上述四元数以及由上述陀螺仪传感器产生的角速度数据决定上述移动设备的姿态信息；以及

根据上述加速度数据及上述磁场强度数据对上述姿态信息进行修正。

10.根据权利要求9所述的求取移动设备姿态的方法，其特征在于，上述决定姿态信息的步骤包括：

根据上述四元数及上述角速度数据决定扩展卡尔曼滤波器中的状态转移矩阵；以及

使用上述扩展卡尔曼滤波器并根据上述状态转移矩阵及上述四元数决定上述姿态信息。

11.根据权利要求10所述的求取移动设备姿态的方法，其特征在于，上述对姿态信息进行修正的步骤包括：

使用上述扩展卡尔曼滤波器并根据上述状态转移矩阵及上述加速度数据对上述姿态信息进行修正；以及

使用上述扩展卡尔曼滤波器并根据上述状态转移矩阵及上述磁场强度数据对上述姿态信息进行修正。

12.根据权利要求9所述的求取移动设备姿态的方法，其特征在于，上述姿态信息包括估计的四元数。

13.根据权利要求12所述的求取移动设备姿态的方法，其特征在于，还包括：决定上述估计的四元数所对应的欧拉角。

14.根据权利要求9所述的求取移动设备姿态的方法，其特征在于，还包括：

决定以导航坐标系为决定上述俯仰角、上述滚动角以及上述偏航角的参考坐标系；以及

决定上述导航坐标系与上述移动设备的载体坐标系之间的坐标变换矩阵。

15.根据权利要求9所述的求取移动设备姿态的方法，其特征在于，还包括：在决定上述俯仰角及上述滚动角之前先对上述加速度数据进行向量标准化处理。

16.根据权利要求9所述的求取移动设备姿态的方法，其特征在于，还包括：在决定上述姿态信息之前先对上述四元数进行向量标准化处理。