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

技术特征：
1.一种移动设备，其特征在于，包括：加速度传感器，用以产生加速度数据；磁力计传感器，用以产生磁场强度数据；陀螺仪传感器，用以产生角速度数据；以及传感器中枢，用以：对上述加速度数据进行向量标准化处理以得到标准化的加速度向量，并根据上述标准化的加速度向量使用如下的第一公式来决定俯仰角及滚动角，其中，[a
x
,a
y
,a
z
]
t
为标准化的加速度向量，θ为俯仰角，φ为滚动角，ψ为偏航角；根据上述磁场强度数据使用第二公式来决定上述偏航角，其中，与为上述磁场强度数据在水平坐标系的x轴与y轴上的分量；根据上述俯仰角、上述滚动角以及上述偏航角来决定上述俯仰角、上述滚动角以及上述偏航角所对应的四元数；根据上述四元数及上述角速度数据决定扩展卡尔曼滤波器中的状态转移矩阵；使用上述扩展卡尔曼滤波器并根据上述状态转移矩阵及上述四元数决定上述移动设备的预测姿态信息；以及决定导航坐标系与上述移动设备的载体坐标系之间的坐标变换矩阵，并根据上述坐标变换矩阵的第一元素和上述加速度数据对上述预测姿态信息中的与俯仰角及滚动角相对应的信息进行修正，以及根据上述坐标变换矩阵的不同于所述第一元素的第二元素及上述磁场强度数据对上述预测姿态信息中的与偏航角相对应的信息进行修正。2.根据权利要求1所述的移动设备，其特征在于，上述传感器中枢根据上述四元数及上述角速度数据决定上述扩展卡尔曼滤波器的上述状态转移矩阵，包括：上述传感器中枢对上述四元数进行向量标准化处理以得到标准化的四元数；上述传感器中枢使用如下的第三公式对上述标准化的四元数进行求异：其中，w为上述角速度数据，q为上述标准化的四元数；根据上述第三公式得到第四公式通过对上述第四公式进行泰勒展开来得到上述状态转移矩阵。3.根据权利要求2所述的移动设备，其特征在于，上述第一元素为上述坐标变换矩阵的第三列元素，以及上述第二元素为上述坐标变换
矩阵的第一行第二列元素和第一行第一列元素。4.根据权利要求1所述的移动设备，其特征在于，上述姿态信息包括估计的四元数。5.根据权利要求4所述的移动设备，其特征在于，上述传感器中枢还决定上述估计的四元数所对应的欧拉角。6.根据权利要求1所述的移动设备，其特征在于，上述传感器中枢还决定以导航坐标系为决定上述俯仰角、上述滚动角以及上述偏航角的参考坐标系。7.根据权利要求1所述的移动设备，其特征在于，上述传感器中枢还在决定上述姿态信息之前先对上述四元数进行向量标准化处理。8.一种求取移动设备姿态的方法，其特征在于，适用于包括加速度传感器、磁力计传感器以及陀螺仪传感器的移动设备，上述求取移动设备姿态的方法包括：对上述加速度传感器产生的加速度数据进行向量标准化处理以得到标准化的加速度向量，并根据上述标准化的加速度向量使用如下的第一公式来决定俯仰角及滚动角，其中，[a
x
,a
y
,a
z
]
t
为标准化的加速度向量，θ为俯仰角，φ为滚动角，ψ为偏航角；根据由上述磁力计传感器产生的磁场强度数据使用第二公式来决定上述偏航角，其中，与为上述磁场强度数据在水平坐标系的x轴与y轴上的分量；根据上述俯仰角、上述滚动角以及上述偏航角来决定上述俯仰角、上述滚动角以及上述偏航角所对应的四元数；根据上述四元数以及由上述陀螺仪传感器产生的角速度数据决定扩展卡尔曼滤波器中的状态转移矩阵；使用上述扩展卡尔曼滤波器并根据上述状态转移矩阵及上述四元数决定上述移动设备的预测姿态信息；以及决定导航坐标系与上述移动设备的载体坐标系之间的坐标变换矩阵，并根据上述坐标变换矩阵的第一元素和上述加速度数据对上述预测姿态信息中的与俯仰角及滚动角相对应的信息进行修正，以及根据上述坐标变换矩阵的不同于所述第一元素的第二元素及上述磁场强度数据对上述预测姿态信息中的与偏航角相对应的信息进行修正。9.根据权利要求8所述的求取移动设备姿态的方法，其特征在于，上述决定状态转移矩阵的步骤包括：对上述四元数进行向量标准化处理以得到标准化的四元数；使用如下的第三公式对上述标准化的四元数进行求异：其中，w为上述角速度数据，q为上述标准化的四元数；
根据上述第三公式得到第四公式通过对上述第四公式进行泰勒展开来得到上述状态转移矩阵。10.根据权利要求8所述的求取移动设备姿态的方法，其特征在于，上述第一元素为上述坐标变换矩阵的第三列元素，以及上述第二元素为上述坐标变换矩阵的第一行第二列元素和第一行第一列元素。11.根据权利要求8所述的求取移动设备姿态的方法，其特征在于，上述姿态信息包括估计的四元数。12.根据权利要求11所述的求取移动设备姿态的方法，其特征在于，还包括：决定上述估计的四元数所对应的欧拉角。13.根据权利要求8所述的求取移动设备姿态的方法，其特征在于，还包括：决定以导航坐标系为决定上述俯仰角、上述滚动角以及上述偏航角的参考坐标系。14.根据权利要求8所述的求取移动设备姿态的方法，其特征在于，还包括：在决定上述姿态信息之前先对上述四元数进行向量标准化处理。

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


技术研发人员：林霞 伍东兴 顾德明 毛洋
受保护的技术使用者：格兰菲智能科技有限公司
技术研发日：2016.11.17
技术公布日：2021/12/16