(19)中华 人民共和国 国家知识产权局 (12)发明 专利申请 (10)申请公布号 (43)申请公布日 (21)申请 号 202111662081.8 (22)申请日 2021.12.31 (71)申请人 杭州电子科技大 学 地址 310018 浙江省杭州市杭州经济技 术 开发区白杨街道 2号大街1 158号 (72)发明人 姚英彪　刘月萍　冯维　李沛　 徐欣　杨阿锋　韦博　刘晴　 (74)专利代理 机构 浙江千克知识产权代理有限 公司 33246 代理人 冷红梅 (51)Int.Cl. G01C 21/16(2006.01) G01C 21/08(2006.01) G06K 9/62(2022.01) G06F 17/16(2006.01) (54)发明名称 基于机器学习分类的融合航向角估计方法 及系统 (57)摘要 本发明公开了一种基于机器学习分类的融 合航向角估计方法及系统， 方法按如下步骤： 步 骤1： 通过测量获得的三轴磁感应强度和三轴加 速度信息计算航向角； 步骤2： 通过测量获得的三 轴角速度信息计算航向角； 步骤3： 对步骤1获得 的三轴磁感应强度数据是否受到磁场干扰进行 分类； 步骤4： 根据步骤3的分类结果， 在无磁场干 扰的场景下， 将步骤1和步骤2获得的航向角进行 卡尔曼滤波融合计算最终的航向角； 在有磁场干 扰的场景下， 仅利用步骤2获得的航向角来估计 最终的航向角。 本发明既避免了陀螺仪的累积误 差， 又避免使用了受到强磁场干扰的磁力计的数 据， 从而使得本发明提出的航向角估计结果更加 鲁棒， 精度也更高。 权利要求书4页 说明书10页 附图1页 CN 114323008 A 2022.04.12 CN 114323008 A 1.基于机器学习分类的融合 航向角估计方法， 其特 征是按如下步骤： 步骤1： 通过测量获得的三轴磁感应强度和三轴加速度信息计算 航向角； 步骤2： 通过测量获得的三轴角速度信息计算 航向角； 步骤3： 对步骤1 获得的三轴磁感应强度数据是否受到磁场干扰进行分类； 步骤4： 根据步骤3的分类结果， 在无磁场干扰的场景下， 将步骤1和步骤2获得的航向角 进行卡尔曼滤波融合计算最 终的航向角； 在 有磁场干扰的场景下， 仅利用步骤2获得的航向 角来估计最终的航向角。 2.如权利要求1所述基于机器学习分类的融合航向角估计方法， 其特征是， 所述步骤1， 采用磁力计测 量三轴磁感应强度， 采用加速度计测量三轴加速度信息， 计算航向角的具体 步骤如下： 步骤1.1： 利用智能手机内置的磁力计和加速度计采集载体坐标系下的三轴磁感应强 度和三轴加速度值； 载体坐标系(b系)与载体固连， 原点位于载体质心， 三轴分别表示为X， Y， Z； 行人导航系 统的载体坐标系定义为右前上坐标系， 其X轴沿载体横轴向右， Y轴沿载体横轴向前， Z轴沿 着载体竖直向上； 记磁力计在b系下采集得到的三轴磁感应强度为 记加速度 计在b系下采集到的三轴加速度为(Ax,Ay,Az)； 导航坐标系(n系)采用地理坐标系， 选用东北天坐标系， 即X轴指 向水平东方， Y轴指向 水平北方， Z轴垂直于地 面沿垂线向上； 行人行走过程中的载体坐标系和导航坐标系并不重合， 坐标系需要经过三次旋转得 到， 以旋转顺序Z ‑X‑Y为例， 其中绕X轴旋转为俯仰角 θ， 绕Y轴旋转为横滚角γ， 绕Z轴旋转为 航向角 ψ； 根据加速度传感器信息初始化手机的姿态信息， 计算俯仰角 θ和横滚角γ， 其中g为重 力加速度， 具体 计算公式如下： n系经过Z‑X‑Y轴旋转变化后得到b系， 姿态 矩阵 如下所示： 则由b系到n系的姿态变换矩阵如下 所示： 由于航向角不参与转换， 设航向角 ψ＝0， 利用磁力计在b系下的测量值计算n系下三轴 磁力计分量， 转换公式如下：权　利　要　求　书 1/4 页 2 CN 114323008 A 2步骤1.2： 根据b系下磁力计测量 值计算n系下的航向角 ψm； 根据式(1.5)计算 参考坐标系下的航向角， 其中 ψMD表示当地磁偏角； 3.如权利要求2所述基于机器学习分类的融合航向角估计方法， 其特征是， 所述步骤2， 采用陀螺 仪传感器测量的三轴角速度值计算 航向角， 具体包括下述 步骤： 步骤2.1： 利用陀螺仪传感器测量的三轴角速度 值(Wx,Wy,Wz)和四元数法求解姿态变换 矩阵； 定义四元 数如下： q＝q0+q1·i+q2·j+q3·k    (1.6) 其中四元 数之间满足如下关系： 利用四元 数表示由b系到n系的姿态变换矩阵如下 所示： 结合式(1.3)和(1.8)， 得 出四元数与欧拉角的关系： 进行四元 数更新的微分方程如下： 步骤2.2： 根据上述四元 数更新后的数据计算 航向角 ψg； 4.如权利要求3所述基于机器学习分类的融合航向角估计方法， 其特征是， 所述步骤3，权　利　要　求　书 2/4 页 3 CN 114323008 A 3