[发明专利]姿态矩阵的计算方法及基于姿态矩阵的定位导航方法

摘要

本发明公开了一种姿态矩阵的计算方法及基于姿态矩阵的定位导航方法,其中姿态矩阵的计算方法可基于智能机加速度计、智能机磁力计或者智能机Android系统函数中的getRotationMatrix函数。通过计算得到的姿态矩阵Rg的第1行是智能机x轴在局部坐标系下的单位矢量；第2行是智能机y轴在局部坐标系下的单位矢量；第3行是智能机z轴在局部坐标系下的单位矢量；其中局部坐标系是z轴沿重力指向地球外，x轴朝北y轴朝西的坐标系。运用姿态矩阵可以进行室内外导航、运动测量姿态控制、遥控、指向控制、视频图像纠正处理、基于图像的定位等。

主权项

1.一种智能机姿态矩阵计算方法，其特征在于，包括以下步骤：通过智能机加速度计检测三个值{vax’,vay’,vaz’}计算ax、ay和az：或对{vax’,vay’,vaz’}进行滤波后得到{vax,vay,vaz}，计算ax、ay和az：其中R_a是重力加速度传感器相对于智能机的姿态转换矩阵；通过智能机磁力计获取方位角的三角函数；或者方向传感器获取方位角计算姿态矩阵姿态矩阵Rg的第1行是智能机x轴在局部坐标系下的单位矢量；第2行是智能机y轴在局部坐标系下的单位矢量；第3行是智能机z轴在局部坐标系下的单位矢量；其中局部坐标系是z轴沿重力指向地球外，x轴朝北y轴朝西的坐标系；其中当az<0时，s＝‑1，az≥0时，s＝1；其中mx,my,mz是智能机磁力计检测出来的三个值{vmx,vmy,vmz}，或{vmx,vmy,vmz}经过滤波得到的三个值；若将替换成则算出的R_g是X轴朝东Y朝北坐标系下的姿态矩阵。