论文部分内容阅读
[摘 要]本文设计了一种基于STM32、MPU6050与HMC5883L的行人相对方位运动的追踪系统。系统以STM32F103CBT6为主控制器，通过I2C总线采集MPU6050和HMC5883L的测量数据。之后对采集到的数据进行处理，首先通过融合算法解算行人的运动姿态，得到四元数，通过姿态解算再将四元数转化为欧拉角；
[关键词]传感器，数据融合，航向角
中图分类号：TH864 文献标识码：A 文章编号：1009-914X（2017）32-0000-01
一、引言
随着计算机、数据处理和通信技术的迅速发展，现代导航技术呈现出多样化的发展趋势。无论在军事还是在民用方面，导航技术都已成为一项不可或缺的关键技术[1]。
随着传感器相应技术的发展，基于惯性期间的室内定位技术也取得了长足的进步，按照计算运动距离的方法可以分为2类，分别是基于运动模型的行人航迹推算和基于捷联惯性导航技术的系统[2]。本文利用航迹推算方法，将行人的的运动状态分解为两部分，一部分是航向角，另一部分是行走的距离，之后将两部分数据结合起来，就能获得行人的运动轨迹了。本文的创新点在于，将多传感器信号进行融合，得到較为精确的航向角，为行人运动轨迹再现打下基础。
二、实验系统
本實验搭建的系统由STM32F103、以及MPU6050、HMC5883L传感器组成。由STM32F103完成数据采集以及姿态解算等，最终输出载体的俯仰角、横滚角以及航向角信息，如图1所示。
三、硬件连接
STM32F103与MPU6050通过I2C总线连接，HMC5883L通过与MPU6050连接，在通过I2C总线来读取数据。如图2所示。
四、多传感器数据融合算法
角速度传感器输出的是瞬间的角加速度的数值，而一般不能直接使用，我们需要得到的是角度的变化量，需要使用角加速度和时间进行积分运算，从而得到角度的变化量。但是实际运算中，积分是存在误差的，随着时间的流逝，误差越来越大，使得计算出的角度和实际角度相差甚远，导致测出的姿态与实际姿态差距较大[3]。
加速度传感器是以重力作为参考方向的，在无外力的情况下，测量出来的是物体的绝对姿态。但是加速度传感器无法区分重力加速度和外力加速度，因为运动中的物体受到重力和外力的共同作用，所以单纯依靠加速度计不能表示物体的真实姿态。
而对于磁强计传感器，它的读数是磁强计不需要加速度传感器也能计算出航向角，但是，前提是，传感器必须要保证水平，也就是说，俯仰角和横滚角都是0时计算出的航向角才是准确的。但是我们无法保证俯仰角和横滚角始终为0，于是就需要通过加速度计来进行修正。
综上所述，我们发现这些传感器单独工作的时候都会存在较大的误差。但与此同时它们各自的误差又可以通过另外一种传感器的数据来进行修正，于是有了数据融合算法，来提高估算结果的精确度。
常用的解决方法为采用互补滤波方法，互补滤波法在确定姿态的算法中用于滤除加速度计存在的线加速度，以及磁力计收到干扰等因素引起的测量信息中的高频噪声，同时可以消除陀螺仪零偏及其他误差造成的积分低频噪声漂移，使两种算法得到的姿态角互补，提高精确度。具体算法如下：
4.1 利用起始姿态角计算初始四元数
首先确定一个载体的坐标系，取载体的重心为坐标系原点，分别定义为右前上（XYZ）。将测量单元中的陀螺仪、加速度计和磁强计的测量数据定义到载体坐标内。为了后续的计算，载体坐标系需要转换到地理坐标系，即东北天（XYZ）坐标系。姿态解算常用的算法有欧拉角法、方向余弦法和四元数法。欧拉角法在求解姿态时存在奇点，不能用于全姿态的解算；方向余弦可用于全姿态的解算但计算量大，不能满足实时性的要求，四元数法既没有奇点，又能够满足姿态解算的实时性要求，所以本文利用四元数对坐标系进行转换。
姿态解算的原理：对于一个确定的向量，用不同的坐标系表示时，他们所表示的大小和方向一定是相同的。但是由于这两个坐标系的旋转矩阵存在误差，那么当一个向量经过这么一个有误差存在的旋转矩阵后，在另一个坐标系中肯定和理论值是有偏差的，我们通过这个偏差来修正这个旋转矩阵。这个旋转矩阵的元素是四元数，我们修正的就是四元数，这样姿态就被修正了。
4.2 互补滤波
陀螺仪动态响应特性良好，但计算姿态时会产生累积误差。磁力计和加速度计测量姿态没有累积误差，但动态响应较差。因此他们在频域上特性互补，所以采用互补滤波器融合这三种传感器的数据，提高测量精度和系统的动态性能。
互补滤波的传递函数为：
其中，，具有一阶低通滤波特性，具有一阶高通滤波特性。
用表示载体运动过程中真实的姿态矩阵，
其中是加速度计和磁力计检测到的姿态矩阵，是陀螺仪测量到的数据计算得到的姿态矩阵。为高频噪声，为低频累计误差。因此互补滤波能够消除高频噪声和低频累计误差，能够很好的融合多传感器的数据。
四元数姿态解算的整体步骤如下：
步骤一：初始化四元数
解算姿态的初始，将已知载体的初始姿态角带入下式，求解出初始时刻的四元数。
步骤二：获取相应的数据
STM32通过I2C总线读取加速度计的测量值，陀螺仪的测量值，以及磁力计的测量值。将加速度计、磁力计测量值化为单位向量。
步骤三：从四元数里获得重力向量和磁场向量
通常将惯性测量单元中的加速度计、陀螺仪和磁力计的测量数据定义到载体坐标系b系，即以载体的重心为原点，分别将三轴定义为右前上。为了后续计算，载体坐标系要转换到导航坐标系n系及东北天坐标系。
一个坐标系到另一个坐标系的变换，可以通过绕不同坐标轴的三次连续转动来实现。从参考系到一个新的坐标系的变换可以表示： 绕参考坐标系的Z轴转动角；
绕新坐标系的Y轴转动角；
绕新坐标系的X轴转动角
其中，叫做歐拉转动角。
其中，是重力向量，中间为bCn的转换矩阵，是n系中加速度计的输出，即重力向量。
加速度计的实际测量值记为，和均表示在b系中垂直向下的向量，则误差矩阵：
磁强计与加速度计一样，都是有大小和方向的，但是和加速度计不同的是，方向不再是单纯的竖直向下，而是与X（或Y）轴呈一个角度，与Z轴呈一个角度。记做，这里我们让X轴对准正北，即令，则矩阵变为。
加速度计修正后的bCn为：
其中，为磁强计在b系中的输出值。
磁强计在XOY平面（n系）的向量大小必定相同，即；对于的处理不做变动。
经过这样的处理之后，旋转矩阵为：
误差矩阵：
步骤五：计算误差矩阵
最终的误差矩阵为：
步骤六：利用误差来修正陀螺仪
步骤七：利用修正的陀螺仪数值来更新四元数
步骤八：规范化新的四元数
步骤九：将四元数转换为欧拉角
五、实验结果
本系统将搭建完成的硬件系统绑置于行人的脚面上，如图4所示。
当传感器的X指向行人的正前方，Y轴指向行人的左侧，Z轴垂直向下，则初始示数如图5。
行人运动过程中，初始时航向角数值并不是在0附近，需要站定，等待航向角通过反馈修正使其恢复到一个平稳的数值之后再开始运动，由于在载体上这个平台并不水平，因此数值会比上述数值偏移更大。下图为行进过程中测量的航向角的误差水平。图6为加速度计的累计误差，用来修正陀螺仪零偏的。
根据实际实验，由于是行人行走过程，所以行人航向角以绕Z轴旋转偏离X/Y轴的角度为行人航向角，抛开行人走路时的晃动情况，对行人几种特定角度继進行测量，航向角的偏差如图7。
根据实际实验可以看出，通过互补滤波算法，航向角的偏差控制在4度以内，可以满足行人轨迹推算的需求。
参考文献
[1] 朱金林.基于MEMS惯性传感器的室内3D定位系统研究[D].华侨大学，2015.
[2] 代汝勇.基于MEMS与智能手机电子罗盘的室内定位与导航算法研究[D].北京工业大学，2016.
[3] 米刚，田增山，金悦，等.基于MIMU和磁力计的姿态更新算法研究[J].传感技术学报，2015，28（1）：43-48.
[4] 张承岫，李铁鹰，王耀力.基于MPU6050和互补滤波的四旋翼飞控系统设计[J].传感技术学报，2016，29（7）：1011-1015.
[5] 闫爱天，陈雄，周长省，等.基于地磁传感器与陀螺仪的姿态测量法[J].弹箭与制导学报，2016，36（2）：143-146.
[6] 朱立明，杜双伟，李海，等.基于捷联惯性导航的室内定位系统研究[J].南京师范大学学报（工程技术版），2016（2016年01）：42-47.
[7] 陈孟元，谢义建，陈跃东.基于四元数改进型互补滤波的MEMS姿态解算[J].电子测量与仪器学报，2015，29（9）：1391-1397.
[8] 于飞，白红美，叶攀，等.基于误差四元数的单兵导航系统算法[J].传感器与微系统，2015，34（9）：148-150.
[关键词]传感器，数据融合，航向角
中图分类号：TH864 文献标识码：A 文章编号：1009-914X（2017）32-0000-01
一、引言
随着计算机、数据处理和通信技术的迅速发展，现代导航技术呈现出多样化的发展趋势。无论在军事还是在民用方面，导航技术都已成为一项不可或缺的关键技术[1]。
随着传感器相应技术的发展，基于惯性期间的室内定位技术也取得了长足的进步，按照计算运动距离的方法可以分为2类，分别是基于运动模型的行人航迹推算和基于捷联惯性导航技术的系统[2]。本文利用航迹推算方法，将行人的的运动状态分解为两部分，一部分是航向角，另一部分是行走的距离，之后将两部分数据结合起来，就能获得行人的运动轨迹了。本文的创新点在于，将多传感器信号进行融合，得到較为精确的航向角，为行人运动轨迹再现打下基础。
二、实验系统
本實验搭建的系统由STM32F103、以及MPU6050、HMC5883L传感器组成。由STM32F103完成数据采集以及姿态解算等，最终输出载体的俯仰角、横滚角以及航向角信息，如图1所示。
三、硬件连接
STM32F103与MPU6050通过I2C总线连接，HMC5883L通过与MPU6050连接，在通过I2C总线来读取数据。如图2所示。
四、多传感器数据融合算法
角速度传感器输出的是瞬间的角加速度的数值，而一般不能直接使用，我们需要得到的是角度的变化量，需要使用角加速度和时间进行积分运算，从而得到角度的变化量。但是实际运算中，积分是存在误差的，随着时间的流逝，误差越来越大，使得计算出的角度和实际角度相差甚远，导致测出的姿态与实际姿态差距较大[3]。
加速度传感器是以重力作为参考方向的，在无外力的情况下，测量出来的是物体的绝对姿态。但是加速度传感器无法区分重力加速度和外力加速度，因为运动中的物体受到重力和外力的共同作用，所以单纯依靠加速度计不能表示物体的真实姿态。
而对于磁强计传感器，它的读数是磁强计不需要加速度传感器也能计算出航向角，但是，前提是，传感器必须要保证水平，也就是说，俯仰角和横滚角都是0时计算出的航向角才是准确的。但是我们无法保证俯仰角和横滚角始终为0，于是就需要通过加速度计来进行修正。
综上所述，我们发现这些传感器单独工作的时候都会存在较大的误差。但与此同时它们各自的误差又可以通过另外一种传感器的数据来进行修正，于是有了数据融合算法，来提高估算结果的精确度。
常用的解决方法为采用互补滤波方法，互补滤波法在确定姿态的算法中用于滤除加速度计存在的线加速度，以及磁力计收到干扰等因素引起的测量信息中的高频噪声，同时可以消除陀螺仪零偏及其他误差造成的积分低频噪声漂移，使两种算法得到的姿态角互补，提高精确度。具体算法如下：
4.1 利用起始姿态角计算初始四元数
首先确定一个载体的坐标系，取载体的重心为坐标系原点，分别定义为右前上（XYZ）。将测量单元中的陀螺仪、加速度计和磁强计的测量数据定义到载体坐标内。为了后续的计算，载体坐标系需要转换到地理坐标系，即东北天（XYZ）坐标系。姿态解算常用的算法有欧拉角法、方向余弦法和四元数法。欧拉角法在求解姿态时存在奇点，不能用于全姿态的解算；方向余弦可用于全姿态的解算但计算量大，不能满足实时性的要求，四元数法既没有奇点，又能够满足姿态解算的实时性要求，所以本文利用四元数对坐标系进行转换。
姿态解算的原理：对于一个确定的向量，用不同的坐标系表示时，他们所表示的大小和方向一定是相同的。但是由于这两个坐标系的旋转矩阵存在误差，那么当一个向量经过这么一个有误差存在的旋转矩阵后，在另一个坐标系中肯定和理论值是有偏差的，我们通过这个偏差来修正这个旋转矩阵。这个旋转矩阵的元素是四元数，我们修正的就是四元数，这样姿态就被修正了。
4.2 互补滤波
陀螺仪动态响应特性良好，但计算姿态时会产生累积误差。磁力计和加速度计测量姿态没有累积误差，但动态响应较差。因此他们在频域上特性互补，所以采用互补滤波器融合这三种传感器的数据，提高测量精度和系统的动态性能。
互补滤波的传递函数为：
其中，，具有一阶低通滤波特性，具有一阶高通滤波特性。
用表示载体运动过程中真实的姿态矩阵，
其中是加速度计和磁力计检测到的姿态矩阵，是陀螺仪测量到的数据计算得到的姿态矩阵。为高频噪声，为低频累计误差。因此互补滤波能够消除高频噪声和低频累计误差，能够很好的融合多传感器的数据。
四元数姿态解算的整体步骤如下：
步骤一：初始化四元数
解算姿态的初始，将已知载体的初始姿态角带入下式，求解出初始时刻的四元数。
步骤二：获取相应的数据
STM32通过I2C总线读取加速度计的测量值，陀螺仪的测量值，以及磁力计的测量值。将加速度计、磁力计测量值化为单位向量。
步骤三：从四元数里获得重力向量和磁场向量
通常将惯性测量单元中的加速度计、陀螺仪和磁力计的测量数据定义到载体坐标系b系，即以载体的重心为原点，分别将三轴定义为右前上。为了后续计算，载体坐标系要转换到导航坐标系n系及东北天坐标系。
一个坐标系到另一个坐标系的变换，可以通过绕不同坐标轴的三次连续转动来实现。从参考系到一个新的坐标系的变换可以表示： 绕参考坐标系的Z轴转动角；
绕新坐标系的Y轴转动角；
绕新坐标系的X轴转动角
其中，叫做歐拉转动角。
其中，是重力向量，中间为bCn的转换矩阵，是n系中加速度计的输出，即重力向量。
加速度计的实际测量值记为，和均表示在b系中垂直向下的向量，则误差矩阵：
磁强计与加速度计一样，都是有大小和方向的，但是和加速度计不同的是，方向不再是单纯的竖直向下，而是与X（或Y）轴呈一个角度，与Z轴呈一个角度。记做，这里我们让X轴对准正北，即令，则矩阵变为。
加速度计修正后的bCn为：
其中，为磁强计在b系中的输出值。
磁强计在XOY平面（n系）的向量大小必定相同，即；对于的处理不做变动。
经过这样的处理之后，旋转矩阵为：
误差矩阵：
步骤五：计算误差矩阵
最终的误差矩阵为：
步骤六：利用误差来修正陀螺仪
步骤七：利用修正的陀螺仪数值来更新四元数
步骤八：规范化新的四元数
步骤九：将四元数转换为欧拉角
五、实验结果
本系统将搭建完成的硬件系统绑置于行人的脚面上，如图4所示。
当传感器的X指向行人的正前方，Y轴指向行人的左侧，Z轴垂直向下，则初始示数如图5。
行人运动过程中，初始时航向角数值并不是在0附近，需要站定，等待航向角通过反馈修正使其恢复到一个平稳的数值之后再开始运动，由于在载体上这个平台并不水平，因此数值会比上述数值偏移更大。下图为行进过程中测量的航向角的误差水平。图6为加速度计的累计误差，用来修正陀螺仪零偏的。
根据实际实验，由于是行人行走过程，所以行人航向角以绕Z轴旋转偏离X/Y轴的角度为行人航向角，抛开行人走路时的晃动情况，对行人几种特定角度继進行测量，航向角的偏差如图7。
根据实际实验可以看出，通过互补滤波算法，航向角的偏差控制在4度以内，可以满足行人轨迹推算的需求。
参考文献
[1] 朱金林.基于MEMS惯性传感器的室内3D定位系统研究[D].华侨大学，2015.
[2] 代汝勇.基于MEMS与智能手机电子罗盘的室内定位与导航算法研究[D].北京工业大学，2016.
[3] 米刚，田增山，金悦，等.基于MIMU和磁力计的姿态更新算法研究[J].传感技术学报，2015，28（1）：43-48.
[4] 张承岫，李铁鹰，王耀力.基于MPU6050和互补滤波的四旋翼飞控系统设计[J].传感技术学报，2016，29（7）：1011-1015.
[5] 闫爱天，陈雄，周长省，等.基于地磁传感器与陀螺仪的姿态测量法[J].弹箭与制导学报，2016，36（2）：143-146.
[6] 朱立明，杜双伟，李海，等.基于捷联惯性导航的室内定位系统研究[J].南京师范大学学报（工程技术版），2016（2016年01）：42-47.
[7] 陈孟元，谢义建，陈跃东.基于四元数改进型互补滤波的MEMS姿态解算[J].电子测量与仪器学报，2015，29（9）：1391-1397.
[8] 于飞，白红美，叶攀，等.基于误差四元数的单兵导航系统算法[J].传感器与微系统，2015，34（9）：148-150.