基于捷联惯性导航的室内定位系统研究_朱立明

第16卷第1期 2016年3月 Vol 16 No 1 Mar 2016 南京师范大学学报 工程技术版 JOURNAL OF NANJING NORMAL UNIVERSITY ENGINEERING AND TECHNOLOGY EDITION doi 10 3969 j issn 1672 1292 2016 01 007 基于捷联惯性导航的室内定位系统研究 朱立明 杜双伟 李海 徐寅林 南京师范大学物理科学与技术学院 江苏 南京 210023 摘要 基于捷联惯性导航的室内定位系统是综合运用 MPU6050 陀螺加速度计模块 HMC5983 三轴电子指南 针模块和蓝牙模块来实现定位 系统以 STM32 单片机为控制核心 依此算出四元数和旋转矩阵 并利用卡尔曼 滤波算法计算最优姿态角 在此基础上获取载体在导航坐标系中的运动加速度 通过对加速度的两次积分计算 出载体在导航坐标系中的位置信息 为了克服惯性导航固有的不可消除的积累误差 布设关键通信节点 采用 局部蓝牙通信定位 并通过加权平均融合对位置信息做出修正 提高了定位系统的定位精度 关键词 姿态角 卡尔曼滤波算法 四元数 旋转矩阵 导航坐标系 加权平均融合 中图分类号 TP302 文献标志码 A 文章编号 1672 1292 2016 01 0042 06 Research on Indoor Positioning System Based on Strapdown Inertial Navigation Zhu Liming Du Shuangwei Li Hai Xu Yinlin School of Physics and Technology Nanjing Normal University Nanjing 210023 China Abstract The indoor positioning system based on the strapdown inertial navigation comprehensively uses the module of gyroscope and accelerometer named MPU6050 the module of three axis electronic compass named HMC5983 and Blue tooth module to realize positioning The system takes the STM32 MCU as the core of the control It needs calculating the quaternion and rotation matrix and makes use of Kalman filtering algorithm to calculate the optimal attitude angle On this basis it gets acceleration in the coordinate system of the navigation Through the two integrals for acceleration the position ination in the coordinate system of the navigation is calculated Considering the inertial navigation inherently can t eliminate the accumulated error The paper creatively sets the key communication nodes It adopts the local Blue tooth communication localization It improves the positioning accuracy of the positioning system by the algorithm of weighted average fusion revising the position ination Key words attitude angle Kalman filtering algorithm quaternion rotation matrix the coordinate system of the naviga tion weighted average fusion 惯性导航技术最早应用在 V2 火箭上 但由于当时的工业水平和消除噪声算法的限制使其导航误差 很大 随着航空 航天科技的迅猛发展 迫切需要一款精度高 小巧方便 功耗低 抗干扰能力强的导航系 统 这促使更多的工程技术专家和学者们投入到对惯性器件和导航算法的研究与改进中 其中 惯性器 件主要有旋转质量陀螺仪 激光陀螺仪 光纤陀螺仪 FOG 集成光学陀螺仪 IOG 半球谐振子陀螺仪 HRG 微电子机械系统 MEMS 等 导航算法主要有四元数法 卡尔曼滤波等 这些共同构成了现代惯 性导航 1 的基础 姿态角直接反映了载体与导航坐标系各轴的角度关系 姿态角的精确度对减小加速度比力换算误差 起着重要作用 同时也影响着定位的准确性 早期室内定位系统未安装三轴电子指南针模块 只能人为给 定一个初始值 再通过毕卡逼近法的二阶近似迭代算法实时计算现状态下四元数的值 然后用四元数计 算出现状态下的旋转矩阵 最后用旋转矩阵解算出实时姿态角 由于初始姿态角不断通过二阶近似迭代 收稿日期 2015 05 31 通讯联系人 徐寅林 博士 教授 研究方向 精密仪器设计 E mail xuyinlin 42 朱立明 等 基于捷联惯性导航的室内定位系统研究 导致其累积误差很大且无法得到补偿 本设计针对此特定问题增加了三轴电子指南针模块以实现对姿态 角的实时修正 补偿姿态角误差所造成的位置累积误差 考虑到室内定位应用场合 运动载体增加了蓝牙 通信模块 并在室内关键位置节点布设了相应的蓝牙通信节点 通过几个位置修正节点发来的 RSSI 接 收信号强度指示 值计算出载体的位置 并对已有的位置信息进行修正 补偿位置信息的累积误差 大大 提高了室内定位系统的精度 且可与服务端通信实现更多的附加功能 系统可广泛应用于超市 大型商 场 博物馆 机场等 发展空间广阔 市场前景良好 1系统的整体设计方案和实现定位的流程图 1 1系统的整体设计方案 该系统主要由手持部分和蓝牙位置修正节点部分组成 如图 1 图 2 所示 手持部分包含 STM32 单 片机 MPU6050 陀螺加速度计模块 HMC5983 三轴电子指南针模块 蓝牙模块和液晶显示屏 STM32 单 片机通过 IIC 协议和 USART 协议与各模块进行通信 蓝牙位置修正节点由单片机控制蓝牙模块 通过 USART 协议与蓝牙模块进行通信 并向手持部分发送自身 ID 和坐标信息 图1手持部分 Fig 1Hand held device 图2 系统组成 Fig 2System composition 1 2定位实现流程图 为实现对移动载体的定位 首先采集三轴加速 度计 三轴陀螺仪和三轴指南针的数据各 20 组并求 其平均值消除偶然误差 为实现对加速度的比力换 算 采用毕卡逼近法的二阶近似迭代算法更新四元 数值并计算现状态下的旋转矩阵和最优姿态角完成 比力换算 最后对导航坐标系下的加速度两次积分 得出现状态下的位置信息 为进一步优化定位算 法 要对载体的姿态角是否变化和是否检测到蓝牙 修正节点进行判定 在载体姿态角不发生变化的情 况下 判断陀螺仪三个轴输出值的平均值是否为 0 理想情况下 载体姿态保持不变陀螺仪的输出一直 为 0 而实际情况下 由于震动等原因导致载体姿态 略微变化使得陀螺仪的输出绝对值略大于 0 这样 做在一定程度上不仅可以减小累积误差 还可以提 高运算速度 在检测到蓝牙位置修正节点时 还需 通过蓝牙位置修正节点计算出的位置信息 并与惯 性器件所计算出的位置信息进行加权平均融合 以 补偿位置信息的累积误差 定位流程如图 3 所示 图3定位流程图 Fig 3 The flow chart of positioning 43 南京师范大学学报 工程技术版 第16卷第1期 2016年 2定位算法设计 定位算法是室内定位系统的核心 为实现对运动载体的精确定位 需利用实时更新的四元数值来 计算旋转矩阵 并通过旋转矩阵解算出载体在现状态下的姿态角 2 针对姿态角的累计误差问题 需利 用加速度计和指南针测量出的姿态角进行加权平均融合计算得出修正后的最优姿态角 还需利用比 力换算完成加速度由载体坐标系到导航坐标系的换算 并对导航坐标系下的加速度两次积分得到现状 态下的位置信息 针对惯性定位系统位置信息固有的累计误差问题 需通过蓝牙位置修正节点来修正 误差 2 1姿态角的计算 姿态角通过四元数 3 计算旋转矩阵得到 四元数算 法中涉及到两个坐标系 载体坐标系 b 系 和导航坐标 系 n 系 四元数算法能够快速地实现 b 系到 n 系的坐 标转换 如图 4 所示 即先用毕卡逼近法的二阶近似迭 代算法更新四元数的值并对其进行归一化处理 再通过 归一化后四元数计算出旋转矩阵 4 Cb n q2 0 q 2 1 q 2 2 q 2 3 2 q1q2 q0q3 2 q1q3 q0q2 2 q1q2 q0q3 q2 0 q 2 1 q 2 2 q 2 3 2 q2q3 q0q1 2 q1q3 q0q2 2 q2q3 q0q1 q2 0 q 2 1 q 2 2 q 2 3 1 式中 Cb n n 系到 b 系 为旋转矩阵 q0是四元数的标量部分 表示转角的一半余弦值 q1 q3是四元数 矢量部分的系数 表示瞬时转动轴的方向 瞬时转动轴与参考坐标系轴间的方向余弦值 因旋转矩阵 为正交矩阵 满足C n b C b n 1 Cb n T 即可求出旋转矩阵C n b 从旋转矩阵的三角表达式 5 中即可解算出 姿态角 Cb n cos cos sin sin sin cos sin sin cos sin sin cos sin cos cos cos sin sin cos cos sin sin sin sin cos cos sin cos cos 2 由式 2 可知 1 k arcsin Cb n 2 3 1 k arctan2 Cb n 1 3 C b n 3 3 1 k arctan2 Cb n 2 1 C b n 2 2 3 整个计算流程如图 5 所示 2 2姿态角的修正计算 由于相对角速度存在一定误差 随着时间的 推移姿态角 中 1 k 1 k 1 k 的累积误差 会 越 来 越 大 因 此 必 须 通 过 采 集 加 速 度 计 和 指 南 针 模 块 的 数 据 来 推 算 出 姿 态 角 2 k 2 k 2 k 6 并将二者进行加权平均融合以补偿累积误差 融合计算时各权重系数的大小取值 目前还没有很好的解决方法 需凭经验反复试探测试 根据现场情况反应决定 一般来说 在载体静止 或匀速运动且俯仰角和横滚角较小时姿态角 误差较小 可加大其权重 否则应减小其权重以减小 误差 本文初步测试时各取 0 5 基本能得出比较明显的修正结果 在此采用多个器件和计算方法计算 姿态角 可减小单个器件和计算方法带来的误差 在一定程度上可提高精确度 需要注意的是加速度 计在载体静止或匀速运动状态下且指南针模块不受干扰时测量出的姿态角 2 k 2 k 2 k 比较 接近真实值 因此 在实际应用中为了尽可能地排除干扰和保证计算的准确性必须对其读出的数据进 行卡尔曼滤波 7 再计算 如图 6 所示 最后 将得到的两个姿态角进行加权平均融合修正 得出最优姿 态角 k k k 图4刚体定点转动坐标变换图 Fig 4The Coordinate transation diagram of the rig