一种确定终端姿态的方法及装置与流程

本发明涉及自动化计算机领域，尤其涉及一种确定终端姿态的方法及装置。

背景技术：

随着智能手机的硬件水平的不断提升，很多智能手机已经配备了加速度传感器、陀螺仪、磁力计等传感器，从而具备感知加速度、角速度、磁场强度的能力。然而，由于智能手机中的传感器精度低，以及室内环境的复杂性，智能手机空间姿态角的精度受到以下多种因素影响：

(1)传感器本身的误差，包括系统性误差和随机误差。系统性误差为常值，随机误差在每次传感器启动过程中都不相同。

(2)累积误差。利用陀螺仪的测量数据，通过姿态更新算法可以得到智能手机的姿态角，其短时间内精度较高，但是由于陀螺仪的积分特性，其精度随时间下降。

(3)磁场干扰。由于磁场干扰，导致磁传感器数据异常，给姿态角计算带来误差，严重时会导致系统输出的姿态角偏离正常值；当磁干扰消除后，需要很长时间才能使姿态角恢复到正常值。

现有的智能手机传感器的空间姿态算法为：将智能手机的状态分为快速运动状态与接近静止状态；在快速运动状态下采用陀螺仪对时间积分计算手机的空间姿态，在接近静止的状态下采用加速度传感器与电子罗盘计算手机的空间姿角，并对当前的空间姿态进行修正与误差消除。主要应用于室外高速运动导航，由于高速运动对空间姿态精度要求不高，且可以使用位置参数对结果进行修正，故实际应用效果较好。

然而，基于智能手机传感器的低速运动定位导航，即基于智能手机的行人 导航受到智能手机中的传感器精度低以及室内环境的复杂性等因素影响，应用效果并不理想。

技术实现要素：

本发明提供一种确定终端姿态的方法及装置，用以解决现有技术中智能手机传感器的空间姿态算法不适用于低速运动定位导航的问题。

本发明实施例提供一种确定终端姿态的方法，包括：

在当前采样周期，确定终端的陀螺仪输出的采样角速度值，并补偿确定的所述采样角速度值；

根据补偿的采样角速度值和初始姿态矩阵，确定更新姿态矩阵；

根据所述确定补偿的采样角速度值和所述更新姿态矩阵，通过卡尔曼滤波确定所述终端的姿态最优估计值。

可选的，所述初始姿态矩阵根据以下方法确定：

根据加速度传感器的系统误差，确定终端的俯仰角和翻滚角；

根据所述终端的俯仰角和翻滚角，确定所述终端的初始姿态矩阵。

可选的，所述根据加速度传感器的系统误差，确定终端的俯仰角和翻滚角，包括：

在接收到传感器触发指令时，获取所述加速度传感器输出的初始加速度值；根据所述初始加速度值和所述加速度传感器的系统误差，确定所述俯仰角和所述翻滚角；

所述根据所述终端的俯仰角和翻滚角，确定所述终端的初始姿态矩阵，包括：

在接收到传感器触发指令时，获取磁传感器输出的方位角；

根据所述方位角、所述俯仰角和所述翻滚角确定所述终端的初始姿态矩阵。

可选的，所述根据所述补偿的采样角速度值和所述更新姿态矩阵，通过卡尔曼滤波确定所述终端的姿态最优估计值，包括：

根据所述补偿的采样角速度值确定卡尔曼滤波器的预测值，根据所述更新姿态矩阵确定所述卡尔曼滤波器的量测值；

根据确定的所述量测值修正所述预测值，得到所述终端的最优姿态估计值。

可选的，所述终端的最优姿态估计包括陀螺仪的随机误差；

所述补偿确定的所述采样角速度值，包括：

根据上一个采样周期最优姿态估计中的所述陀螺仪的随机误差，以及陀螺仪的系统误差，补偿当前采样周期陀螺仪输出的采样角速度值。

可选的，所述在当前采样周期，确定终端的陀螺仪输出的采样角速度值，并补偿确定的所述采样角速度值之前，还包括：

在接收到传感器校正指令时，获取所述加速度传感器输出的校验加速度的值，根据所述校验加速度的值确定所述加速度传感器的系统误差。

可选的，所述在当前采样周期，确定终端的陀螺仪输出的采样角速度值，并补偿确定的所述采样角速度值之前，还包括：

在接收到传感器校正指令时，获取所述陀螺仪输出的校验角速度的值，根据所述校验角速度的值确定所述陀螺仪的系统误差。

本发明实施例还提供一种确定终端姿态的装置，包括：

补偿模块：用于在当前采样周期，确定终端的陀螺仪输出的采样角速度值，并补偿确定的所述采样角速度值；

姿态矩阵模块：用于根据补偿的采样角速度值和初始姿态矩阵，确定更新姿态矩阵；

估计值确定模块：用于根据所述确定补偿的采样角速度值和所述更新姿态矩阵，通过卡尔曼滤波确定所述终端的姿态最优估计值。

可选的，所述初始姿态矩阵根据以下方法确定：

根据加速度传感器的系统误差，确定终端的俯仰角和翻滚角；

根据所述终端的俯仰角和翻滚角，确定所述终端的初始姿态矩阵。

可选的，所述姿态矩阵模块，具体用于：

在接收到传感器触发指令时，获取所述加速度传感器输出的初始加速度值；

根据所述初始加速度值和所述加速度传感器的系统误差，确定所述俯仰角和所述翻滚角；

在接收到传感器触发指令时，获取磁传感器输出的方位角；

根据所述方位角、所述俯仰角和所述翻滚角确定所述终端的初始姿态矩阵。

可选的，所述估计值确定模块，具体用于：

根据所述补偿的采样角速度值确定卡尔曼滤波器的预测值，根据所述更新姿态矩阵确定所述卡尔曼滤波器的量测值；

根据确定的所述量测值修正所述预测值，得到所述终端的最优姿态估计值。

可选的，所述终端的最优姿态估计包括陀螺仪的随机误差；

所述补偿模块，具体用于：

根据上一个采样周期最优姿态估计中的所述陀螺仪的随机误差，以及陀螺仪的系统误差，补偿当前采样周期陀螺仪输出的采样角速度值。

可选的，所述补偿模块，还用于：

在接收到传感器校正指令时，获取所述加速度传感器输出的校验加速度的值，根据所述校验加速度的值确定所述加速度传感器的系统误差。

可选的，所述补偿模块，还用于：

在接收到传感器校正指令时，获取所述陀螺仪输出的校验角速度的值，根据所述校验角速度的值确定所述陀螺仪的系统误差。

本发明提供了一种确定终端姿态的方法和装置，在当前采样周期，确定终端的陀螺仪输出的采样角速度值，并补偿确定的所述采样角速度值；根据补偿的采样角速度值和初始姿态矩阵，确定更新姿态矩阵；根据所述确定补偿的采样角速度值和所述更新姿态矩阵，通过卡尔曼滤波确定所述终端的姿态最优估计值。本发明实施例提供的确定终端姿态的方法和装置，提高低速运动导航的精度、减小了终端姿态估计的误差。

附图说明

为了更清楚地说明本发明实施例中的技术方案，下面将对实施例描述中所需要使用的附图作简要介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域的普通技术人员来讲，在不付出创造性劳动性的前提下，还可以根据这些附图获得其他的附图。

图1为本发明实施例提供的一种确定终端姿态的方法流程示意图；

图2为本发明实施例提供的一种导航坐标系示意图；

图3为本发明实施例提供的一种终端坐标系示意图；

图4为本发明实施例提供的一种终端姿态的方法流程图；

图5为本发明实施例提供的一种确定终端姿态的装置结构示意图。

具体实施方式

为了使本发明的目的、技术方案和优点更加清楚，下面将结合附图对本发明作进一步地详细描述，显然，所描述的实施例仅仅是本发明一部份实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其它实施例，都属于本发明保护的范围。

本发明实施例中的终端为指向用户提供语音和/或数据连通性的设备(device)，包括无线终端或有线终端。无线终端可以是具有无线连接功能的手持式设备、或连接到无线调制解调器的其他处理设备，经无线接入网与一个或多个核心网进行通信的移动终端。例如，无线终端可以是移动电话(或称为“蜂窝”电话)和具有移动终端的计算机。又如，无线终端也可以是便携式、袖珍式、手持式、计算机内置的或者车载的移动装置。本发明实施例中的确定终端姿态的方法和装置应用于终端导航系统中。

本发明实施例提供一种确定终端姿态的方法，如图1所示，为本发明实施例提供的一种确定终端姿态的方法流程示意图，包括：

步骤101：在当前采样周期，确定终端的陀螺仪输出的采样角速度值，并补 偿确定的采样角速度值；

步骤102：根据补偿的采样角速度值和初始姿态矩阵，确定更新姿态矩阵；

步骤103：根据确定补偿的采样角速度值和更新姿态矩阵，通过卡尔曼滤波确定终端的姿态最优估计值。

具体地，在步骤101之前，终端首次使用会对终端中的所有传感器进行校验，本发明实施例终端中的传感器至少包含加速度传感器、陀螺仪和磁传感器。使终端保持静止一段时间如30秒。在接收到传感器校正指令时，获取加速度传感器输出的校验加速度的值，并根据校验加速度的值确定加速度传感器每个传感轴的系统误差；同时获取所述陀螺仪输出的校验角速度的值，根据校验角速度的值确定陀螺仪每个传感轴的系统误差。

需要说明的是，传感器的校验可在只终端初次使用的时候进行一次，得到加速度传感器的系统误差和陀螺仪的系统误差，也可以在需要的时候手动校验。

步骤102中，初始姿态矩阵根据以下方法确定：根据加速度传感器的系统误差，确定终端的俯仰角和翻滚角；根据终端的俯仰角和翻滚角，确定终端的初始姿态矩阵。

在每次导航定位时，使终端保持静止一段时间如5秒，根据加速度传感器、磁传感器的输出数据进行姿态的初始化，计算出初始姿态矩阵。具体地，在接收到传感器触发指令时，获取加速度传感器输出的初始加速度值；根据初始加速度值和加速度传感器的系统误差，确定所述俯仰角和所述翻滚角；此外，在接收到传感器触发指令时，获取磁传感器输出的方位角；根据方位角、俯仰角和翻滚角确定所述终端的初始姿态矩阵。

步骤103中，首先根据补偿的采样角速度值确定卡尔曼滤波器的预测值，然后根据更新姿态矩阵确定所述卡尔曼滤波器的量测值；最后根据确定的量测值修正预测值，得到终端的最优姿态估计值。其中，最优姿态估计包括加速度传感器的随机误差、陀螺仪的随机误差以及姿态角误差。此外，根据上一个采样周期最优姿态估计中的陀螺仪的随机误差，以及陀螺仪的系统误差，补偿当 前采样周期陀螺仪输出的采样角速度值。

本发明提供了一种确定终端姿态的方法，在当前采样周期，确定终端的陀螺仪输出的采样角速度值，并补偿确定的所述采样角速度值；根据补偿的采样角速度值和初始姿态矩阵，确定更新姿态矩阵；根据所述确定补偿的采样角速度值和所述更新姿态矩阵，通过卡尔曼滤波确定所述终端的姿态最优估计值。本发明实施例提供的确定终端姿态的方法，提高低速运动导航的精度、减小了终端姿态估计的误差。

下面通过具体的计算分析过程对本发明实施例提供的确定终端姿态的方法进行详细说明。

本发明实施例终端中的传感器至少包含加速度传感器、陀螺仪和磁传感器。终端首次使用会对终端中的所有传感器进行校验使终端保持静止一段时间如30秒，获取陀螺仪和加速度传感器的输出数据，并计算得出30秒内陀螺仪的xyz三轴输出的角速度均值为30秒内加速度传感器的xyz三轴输出的加速度均值为则根据公式(1)得到陀螺仪的系统误差：

式中，bsx,bsy,bsz表示陀螺仪的三轴系统误差，即陀螺仪的三轴常值零偏值。根据公式(2)得到加速度传感器的系统误差：

式中，dsx,dsy,dsz表示加速度传感器的三轴系统误差，即加速度传感器的三轴常值零偏值；g表示重力加速度的值。

需要说明的是，传感器的校验可在只终端初次使用的时候进行一次，得到，也可以在需要的时候手动校验。

在计算终端的初始姿态矩阵之前，首先需要确定坐标系的方向，坐标系包 括导航坐标系和终端坐标系，其中导航坐标系定义为l坐标系，终端坐标系定义为d坐标系。

图2为本发明实施例提供的一种导航坐标系示意图，如图2所示，导航坐标系原点位于地球表面，三轴指向如下：x轴沿卯酉圈指向东，y轴沿子午圈指向北，z轴沿法线指向天顶。图3为本发明实施例提供的一种终端坐标系示意图，如图3所示，终端坐标系的原点位于设备中心，x轴指向设备右侧，y轴指向设备前方，z轴向上。

导航坐标系和终端坐标系之间的旋转关系可以用一个矩阵表示，称为姿态矩阵，如公式(3)：

式中，代表姿态矩阵，y,p,r表示姿态角，其中，y表示方向角(yaw)，p表示俯仰角(pitch)，r表示翻滚角(roll)。

在每次导航定位时，使终端保持静止一段时间如5秒，根据初始加速度值和加速度传感器的系统误差，确定俯仰角p和翻滚角r，如公式(4)：

式中，表示静止时间5秒内加速度计x轴和y轴输出加速度的均值。结合磁传感器输出的方向角y，将公式(4)代入公式(3)，即得到终端的初始姿态矩阵。

在导航定位过程中，根据陀螺仪的输出数据进行姿态更新，获得每个采样周期的姿态，如公式(5)：

式中，表示每个采样周期的姿态矩阵，表示反对称阵，其元素由陀螺 仪的输出数据构成，表示终端坐标系相对于导航坐标系的旋转角速度。根据公式(6)确定：

式中，ωx,ωy,ωz为陀螺仪的输出的角速度经过陀螺仪的系统误差和陀螺仪的随机补偿之后的值。

陀螺仪的输出数据一般为离散的，按照一定的采样周期输出。采样周期可以配置，如0.02s，0.05s，0.1s等。因此将公式(6)代入公式(5)，得到更新姿态矩阵如式(7)所示：

式中，k,k+1分别表示离散化前后两个采样周期的时刻，δt表示采样周期。

下面通过采用卡尔曼滤波进行姿态的最优估计。卡尔曼滤波器的状态向量为9阶，包括3个姿态角误差、3轴陀螺仪随机误差、3轴加速度传感器随机误差。以陀螺仪姿态角的误差方程作为滤波器的状态方程，以姿态角误差作为滤波器的量测值。

具体地，根据公式(8)确定状态向量：

x＝(δpδrδybxbybzdxdydz)^t(8)

式中，x表示状态向量，δp、δr、δy分别表示俯仰角、翻滚角、方向角的姿态误差，bx，by，bz表示陀螺仪的三轴随机误差，dxdydz表示加速度传感器的三轴随机误差。

具体地，根据公式(9)确定状态方程：

xk+1＝fk·xk+gk·wk(9)

式中，xk，xk+1分别表示前后采样周期的状态值；fk表示状态转移矩阵；gk 表示噪声驱动矩阵，其值取为单位阵；wk表示系统的动态噪声。fk根据公式(10)确定：

式中，表示3×3阶的单位阵，表示3×3阶的零矩阵。

wk根据公式(11)确定：

式中，e,d分别表示数学期望和方差。qk+1表示动态噪声的方差。一般在传感器的说明文档中会提供每个轴的动态噪声。

具体地，根据公式(12)确定卡尔曼滤波器的量测方程：

zk+1＝hk+1·xk+1+vk+1(12)

式中，zk+1为量测值，hk+1为量测矩阵，vk+1为量测噪声。

具体地，公式(12)中的量测值zk+1根据公式(13)确定：

式中，pg,rg,yg分别表示经过姿态更新后得到的三个姿态角，pam,ram分别表示加速度传感器输出数据计算出来的更新俯仰角和更新翻滚角，ym表示当前采样周期磁传感器输出的更新方向角。pg,rg,yg根据公式(14)确定：

式中，rij(i＝1,2,3；j＝1,2,3)对应公式(3)中的元素。

pam,ram根据公式(15)确定：

具体地，根据公式(16)确定公式(12)中的量测值噪声vk+1：

式中，rk+1表示观测噪声的方差，根据公式(17)确定观测噪声的方差rk+1:

上式中，表示观测噪声方差的基准值，ythre为方向角误差的阈值。具体地，在卡尔曼滤波过程中，根据卡尔曼滤波量测值与预测值之间的相容性，动态调整观测噪声的方差rk+1。

具体地，根据公式(18)确定公式(12)中的量测矩阵hk+1：

至此，卡尔曼滤波的模型已建立，卡尔曼滤波过程分为两步：时间更新和量测更新。其中时间更新的方程根据公式(19)确定：

量测更新的方程为根据公式(20)确定：

式(20)中的为经过卡尔曼滤波之后的姿态最优估值，包括姿态角误差、陀螺仪随机误差、加速度传感器随机误差。当前采样周期的姿态最优估值可以反馈到下一个周期的姿态更新和滤波过程，即根据当前采样周期最优姿态估计中的所述陀螺仪的随机误差，以及陀螺仪的系统误差，补偿下一个采样周期陀螺仪输出的采样角速度值。

本发明实施例中，在导航定位过程中不断的重复计算姿态更新矩阵和卡尔曼滤波的步骤，即可为每个采样周期提供最优的姿态角信息。

下面通过具体的方法流程，对本发明实施例提供的一种确定终端姿态的方法进行详细说明，图4为本发明实施例提供的一种确定终端姿态的方法流程图，包括：

步骤401：校验加速度传感器和陀螺仪，获取加速度传感器的系统误差和陀螺仪的系统误差。

具体地，终端首次使用会对终端中的所有传感器进行校验，本发明实施例终端中的传感器至少包含加速度传感器、陀螺仪和磁传感器。使终端保持静止一段时间如30秒。在接收到传感器校正指令时，获取加速度传感器输出的校验加速度的值，并根据校验加速度的值确定加速度传感器每个传感轴的系统误差；同时获取所述陀螺仪输出的校验角速度的值，根据校验角速度的值确定陀螺仪每个传感轴的系统误差。

需要说明的是，传感器的校验可在只终端初次使用的时候进行一次，得到加速度传感器的系统误差和陀螺仪的系统误差，也可以在需要的时候手动校验。

步骤402：确定终端的初始姿态矩阵。

具体地，根据加速度传感器的系统误差，确定终端的俯仰角和翻滚角；根据终端的俯仰角和翻滚角，确定终端的初始姿态矩阵。

在每次导航定位时，使终端保持静止一段时间如5秒，根据加速度传感器、 磁传感器的输出数据进行姿态的初始化，计算出初始姿态矩阵。具体地，在接收到传感器触发指令时，获取加速度传感器输出的初始加速度值；根据初始加速度值和加速度传感器的系统误差，确定所述俯仰角和所述翻滚角；此外，在接收到传感器触发指令时，获取磁传感器输出的方位角；根据方位角、俯仰角和翻滚角确定所述终端的初始姿态矩阵。

步骤403：在当前采样周期确定终端的陀螺仪输出的采样角速度值，并补偿确定的采样角速度值。

具体地，根据上一个采样周期最优姿态估计中的陀螺仪的随机误差，以及陀螺仪的系统误差，补偿当前采样周期陀螺仪输出的采样角速度值。

步骤404：根据补偿的采样角速度值和初始姿态矩阵，确定更新姿态矩阵。

步骤405：根据确定补偿的采样角速度值和更新姿态矩阵，通过卡尔曼滤波确定终端的姿态最优估计值。

具体地，根据补偿的采样角速度值确定卡尔曼滤波器的预测值，根据更新姿态矩阵确定所述卡尔曼滤波器的量测值；根据确定的量测值修正预测值，得到终端的最优姿态估计值。其中，最优姿态估计包括加速度传感器的随机误差、陀螺仪的随机误差以及姿态角误差。此外，根据当前采样周期最优姿态估计中的陀螺仪的随机误差，以及陀螺仪的系统误差，补偿下一个采样周期陀螺仪输出的采样角速度值。

基于同样的发明构思，本发明实施例还提供一种确定终端姿态的装置，如图5所示，为本发明实施例提供的一种确定终端姿态的装置，包括：

补偿模块501：用于在当前采样周期，确定终端的陀螺仪输出的采样角速度值，并补偿确定的所述采样角速度值；

姿态矩阵模块502：用于根据补偿的采样角速度值和初始姿态矩阵，确定更新姿态矩阵；

估计值确定模块503：用于根据所述确定补偿的采样角速度值和所述更新姿态矩阵，通过卡尔曼滤波确定所述终端的姿态最优估计值。

可选的，所述初始姿态矩阵根据以下方法确定：

根据加速度传感器的系统误差，确定终端的俯仰角和翻滚角；

根据所述终端的俯仰角和翻滚角，确定所述终端的初始姿态矩阵。

可选的，所述姿态矩阵模块502，具体用于：

在接收到传感器触发指令时，获取所述加速度传感器输出的初始加速度值；

根据所述初始加速度值和所述加速度传感器的系统误差，确定所述俯仰角和所述翻滚角；

在接收到传感器触发指令时，获取磁传感器输出的方位角；

根据所述方位角、所述俯仰角和所述翻滚角确定所述终端的初始姿态矩阵。

可选的，所述估计值确定模块503，具体用于：

根据所述补偿的采样角速度值确定卡尔曼滤波器的预测值，根据所述更新姿态矩阵确定所述卡尔曼滤波器的量测值；

根据确定的所述量测值修正所述预测值，得到所述终端的最优姿态估计值。

可选的，所述终端的最优姿态估计包括陀螺仪的随机误差；

所述补偿模块501，具体用于：

根据上一个采样周期最优姿态估计中的所述陀螺仪的随机误差，以及陀螺仪的系统误差，补偿当前采样周期陀螺仪输出的采样角速度值。

可选的，所述补偿模块501，还用于：

在接收到传感器校正指令时，获取所述加速度传感器输出的校验加速度的值，根据所述校验加速度的值确定所述加速度传感器的系统误差。

可选的，所述补偿模块501，还用于：

在接收到传感器校正指令时，获取所述陀螺仪输出的校验角速度的值，根据所述校验角速度的值确定所述陀螺仪的系统误差。

本发明提供了一种确定终端姿态的装置，在当前采样周期，确定终端的陀螺仪输出的采样角速度值，并补偿确定的所述采样角速度值；根据补偿的采样角速度值和初始姿态矩阵，确定更新姿态矩阵；根据所述确定补偿的采样角速 度值和所述更新姿态矩阵，通过卡尔曼滤波确定所述终端的姿态最优估计值。本发明实施例提供的确定终端姿态的装置，提高低速运动导航的精度、减小了终端姿态估计的误差。

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

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

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

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

显然，本领域的技术人员可以对本发明进行各种改动和变型而不脱离本发明的精神和范围。这样，倘若本发明的这些修改和变型属于本发明权利要求及其等同技术的范围之内，则本发明也意图包含这些改动和变型在内。