基于九轴姿态传感器的智能汽车平面定位模块设计与实践

王艺辉，刘延飞，赵鹏涛，王 忠

（火箭军工程大学，陕西 西安 710025）

在智能汽车竞赛中，调试过程是提升汽车行驶速度最重要的途径。通常情况下，在调试过程中，是通过人眼来确定车辆速度、位置等信息，而大量研究者在设计和分析智能汽车控制系统调试方法[1-4]，以实现人眼识别难以定量分析的车辆行驶状态，评判控制参数的优劣。因此，位置信息在智能车的调试过程中扮演着十分重要的角色。惯性导航是导航的一种重要方式，它有硬件实现简单的优点。本系统通过研究分析了MPU6050应用于惯性导航的方法，并通过实验验证了通过惯性导航解算位置信息的可行性，以实现对智能车坐标的精确定位和调试过程中的定量分析。

1 系统整体结构

1.1 系统总体设计

惯性导航技术在导弹导航制导领域具有重要的地位，而电子设备成本的降低推动了民用惯性导航技术的发展。如图1所示，平面定位系统主要由角度采集模块、速度采集模块、微处理器、坐标显示模块和电源模块等组成。MPU6050作为角度采集传感器，是通过IIC总线直接采集角速度的，避免了对采集信息进行多次处理和其他信号的干扰。微处理模块对角度和速度信息进行处理后得到平面内坐标，可通过液晶显示屏进行显示，也可以通过蓝牙将坐标传送给电脑进行轨迹复现、储存等操作。

图1 系统总体框图

1.2 MPU6050模块

通过平面姿态角测量模块测量平面姿态角，与速度进行计算，从而实时解算出车辆相对于初始位置（MPU6050初始化处）的位置。MPU6050的角速度全格感测范围为±250，±500，±1 000，±2 000/sec（dps），可准确追踪快速与慢速动作，并且用户可程式控制的加速度计全格感测范围为±2 g、±4 g、±8 g和±16 g。产品传输可透过最高至400 kHz的IIC总线或最高达20 MH称的SPI总线[1]。本文使用的是基于MPU6050制作的GY-521模块，此模块体积小、装卸方便，方便固连在智能车上。GY-521电路原理如图2所示。

图2 GY-521电路原理图

1.3 微处理模块

主控制器选用恩智浦公司Kinetis系列的MCU K60[5-6]。该型号芯片以ARM Crotex-M4为内核，功耗低，有DSP指令和单精度浮点运算单元，提供50 MHz、72 MHz、100 MHz几种CPU频率，并且集成了高精度的16位ADC、12位DAC、可编程增益放大器、高速比较器和内部参考电压。

1.4 速度采集模块

速度采集模块使用的是龙邱公司生产的512线的增量式光电编码器。供电电压从3.3 V到5 V兼容，兼容了正交解码模式。该速度采集模块采用了光电信号的模式测速，与霍尔型传感器相比，在靠近电机测速时极大地避免了电磁信号的干扰。

2 程序设计

系统通过上位机与下位机的配合将智能车轨迹绘制出来。单片机初始化传感器和蓝牙，上位机初始化并配置蓝牙参数。当单片机定时器触发中断时，采集角速度和速度信息，解算角度和走过路程。由于到电脑处理数据的能力比较强，将角度信息和路程信息通过蓝牙通讯发送至上位机处理可以减少解算误差。

2.1 下位机流程设计

下位机的主要功能是角度、速度信息的提取，坐标的解算和显示。考虑到下位机的运算能力比较强，将坐标的解算放在下位机中进行。下位机数据采集与处理流程如图3所示，具体为：①系统先进入各模块初始化函数，其中，IIC模块波特率设为400 kbps，并使用IIC模块初始化MPU6050；速度采集使用DMA触发中断计数，以减少测速对CPU资源的占用；初始化PIT定时器，定时时间为1 ms。②定时器触发中断进入中断处理程序PIT0_IRQHandler()，通过IIC采集角速度，并计算得出角度值。读取DMA计数值，通过计算公式解算坐标，并将坐标显示在液晶显示屏。③当检测到发送按键按下时，将数据经蓝牙发送至上位机。

图3 下位机数据采集与处理流程图

2.2 陀螺仪转角信息获取程序

由于MPU6050与车体捷联，令MPU6050坐标系与图车体坐标系平行。此时，需要采集的角度为Z轴方向转角。

测量转角时，重力方向通过Z轴，智能车在水平面行驶时，Z轴与重力方向夹角恒等于0，此时利用反三角函数求解得加速度输出角度恒等于90°。如果加速度输出角度无法与陀螺仪输出角度进行卡尔曼滤波运算，考虑使用陀螺仪测量角速度积分输出角度。使用这种方法存在两类较大的误差。如图4所示，通过除法运算将采集数据缩放，过滤部分噪声。在智能车行驶过程中，难以消除震动。当MPU6050Z轴方向受到震动时，其他2个方向也受到震动。在整场行驶时，X轴方向不发生转动，因此，X轴在受到震动时，输出近似为震动噪声。MPU6050芯片体积比较小，此时将X轴、Z轴受到的震动视为一致的，将Z轴数据减去X轴数据可以抑制震动噪声。

图4 下位机数据采集与处理流程图

2.3 坐标计算程序

如图5所示，Pn与Pn+1为相邻的2个点，Length为速度采集值，θ为MPU6050 Z轴相对于初始点转过的角度。弧PnPn+1表示实际走过的轨迹。当采集周期趋于0时，弧线与直线围成的图形面积趋于0，即弧PnPn+1可用直线PnPn+1替代[7]。此时，得坐标计算公式为：

计算公式C语言的实现为：

图5 平面坐标系与坐标关系

2.4 上位机流程设计

上位机主要完成接收数据、绘制轨迹、储存和历史记录等工作。上位机使用Visual Studio软件制作，具体工作流程是：①系统初始化，创建储存路径，读取上次使用记录等。②判断串口蓝牙是否打来。当serialPort1.IsOpen==true时，说明蓝牙连接成功，能与蓝牙从机通讯。③当接收数据通过“头文件”和“负载校验和”检验时，说明接收数据完整有效。此时，取出数据，缓存至链表OrbitLength和OrbitAngle中，经过计算绘制出轨迹图案，并将数据储存到指定目录。

3 实验结果与分析

本文进行了陀螺仪原始积分角度与滤波后积分角度对比、相对误差统计、轨迹实现3次实验。结果表明，系统计算出的坐标与实际测量坐标误差满足智能车的调试需求。

3.1 陀螺仪原始积分角度与滤波后积分角度对比

在一次测量中，利用单片机同时解算出陀螺仪原始积分角度与滤波后积分角度，此时2次测量的震动环境相同。由于输出的角度限幅在0°～360°，所以，转动固定圈数后测量可以简单读出两解算值。对比2个数据与标准角度的误差，可以判定滤波算法的实际使用效果。图6为滤波前后数据对比图，横坐标为转过圈数，纵坐标为误差角度。统计分析得，滤波后积分角度函数表达式为y＝0.420 9x－0.304 5，直接积分角度函数表达式为y＝0.470 9x－0.127 3.其中，x为转过圈数，y为角度误差。从图6中可以看出，经滤波后的角度输出误差比较小，并且滤波后函数表达式的k比较小。此时，随着工作时间的延长，抑制误差的效果越明显。结果表明，通过滤波减小了陀螺仪的误差。

3.2 相对误差统计

定义比例系数kx和ky，kx＝（解算横坐标）/（测量横坐标），ky＝（解算纵坐标）/（测量纵坐标）。当kx和ky的方差比较小时，可以将解算坐标视为测量坐标乘以一定系数，此时绘制的轨迹与实际轨迹近似于等比缩放。本文进行了8组实验，计算各点kx、ky以及kx、ky的方差。比例系数测量情况如表1所示。由实验数据可得，舍去误差较大的点计算得kx的平均值为2.759 869，方差为0.005 899；ky的平均数为2.772 692，方差为0.003 745.kx和ky都比较小，且kx≈ky。此时，解算坐标与测量坐标可以看作等比缩放。

图6 滤波前后数据对比

3.3 轨迹实现

在调试智能车的过程中，使用了大量的上位机。当上位机将智能车不同参数下2次行车路线图绘制在同一界面时，播放智能车行驶轨迹点，一方面，可以对比智能车路径的优劣；另一方面，可以通过对比轨迹点评判2次行车在某一路段的行车速度。如图7所示，这些线条分别为2条不同历史轨迹的对比情况。

表1 比例系数测量表

图7 历史轨迹对比

4 结论

本文设计了一种平面定位系统，分别从下位机、上位机入手阐述了角度信息和速度信息的采集、坐标的解算、数据传输、轨迹实现等功能流程。最后通过角度输出统计、相对误差统计和轨迹实现3组实验验证了系统的有效性和实用性。该系统主要用于智能车比赛的调试，通过多次轨迹对比，能有效评判调试参数的优劣，有助于提高智能车的行驶速度。

［1］李臣龙，强俊.基于STM32和MPU6050姿态解算的研究和实现［J］.佳木斯大学学报（自然科学版），2017（2）：295-296.

［2］张桐铭，刘延飞，赵鹏涛，等.基于PID神经网络的智能车追逐控制系统研究［J］.科技与创新，2016（15）：13-14.

［3］刘延飞，郭锁利，侯荣昌.卡尔曼滤波在GPS定位误差处理中的应用［J］.电子科技，2011，24（09）：140-142，148.

［4］许可行，刘延飞，羊帆.基于自适应神经网络的风力摆设计实现［J］.科技与创新，2016（12）：6-7.

［5］杨熙，苏勇，刘屹霄.嵌入式软件设计入门与进阶：基于Kinetis K60/K64［M］.北京：清华大学出版社，2017.

［6］王日阳，廖锦松，申柏华.轻松玩转ARM CORTEX-M4微控制器：基于Kinetis K60［M］.北京：北京航空航天大学出版社，2014.

［7］明日科技.C#从入门到精通［M］.附光盘第3版.北京：清华大学出版社，2012.