STM32自主定位的小车导航系统设计

王玥，张海

(北京航空航天大学 自动化科学与电气工程学院，北京 100191)



王玥，张海

(北京航空航天大学 自动化科学与电气工程学院，北京 100191)

摘要:设计了基于STM32F103VET6的自主定位四轮差速小车。为单轴陀螺仪ADIS16265、集成惯性测量单元MPU-9150及超声波测距模块设计了合理的数据采集与处理方法，完成小车短时间内高精度的航向角计算及长时间稳定的磁航向角计算，并对程序开发中遇到的堆栈溢出问题提出了解决方案。最后提出了各传感器信息的有效融合方法，采用航位推算的方法完成小车自主定位，实现了小车长时间沿预定轨迹自主稳定行驶的功能。

关键词:STM32；ADIS16265；MPU-9150；磁航向角；堆栈溢出

引言

本文采用惯性/码盘/磁罗盘/超声波测距组合的方式，在测量数据融合的基础上实现小车的自主导航定位，并通过电机调速控制实现了车辆的自主定位。

1方案选择

小车的导航与控制系统由导航/控制解算模块、惯性/距离/测速传感器、电机调速控制电路、WiFi无线通信模块等构成，系统组成及主控芯片接口分配如图1所示。

图1　系统组成及主控芯片接口分配图

车辆采用4轮差速控制，每个减速电机上装有光电码盘测量电机的实际转速。

主控芯片在采集到传感器数据后进行小车导航定位信息的解算。已知初始航向、位置信息，利用陀螺仪测量的航向角速度信息积分得到短时间内精确的小车航向角信息，加上光电码盘测得的车轮转速信息，通过航位推算的方法获得小车定位信息。利用惯性/磁集成传感器测量小车姿态,进而获得磁航向，抑制惯性航向解算的误差累积。在本文实验环境下，还可以利用超声波测距信息对小车位置及航向信息进行修正。

在获得了小车的导航定位信息之后，可通过轨迹跟踪控制规律计算电机的控制参数，实现差速控制，进而完成小车对预定轨迹的跟踪。此外，系统还加入了无线通信模块,实现了与PC机的无线通信，方便关键数据的实时上传与分析。

2硬件设计

2.1微控制器

主控芯片选用STM32F103VET6[1]，内核为ARM 32位的Cortex-M3 CPU，最大工作频率为72 MHz，指令执行速度达到1.25 DMIPS/MHz，片上有512 KB的Flash及64 KB的SRAM，芯片含有80个GPIO接口，提供I2C、SPI和USART接口，满足系统与不同传感器的通信功能及对执行单元的控制功能。

2.2惯性测量模块设计

小车的导航与定位信息主要通过惯性测量模块获得。由于小车的使用环境可基本视为二维平面，即小车行走过程中的俯仰角与横滚角可近似为零，因此为小车安装测量轴垂直运动平面的单轴陀螺仪进行航向角速度的测量。通过单轴陀螺仪的测量值可以在短时间内解算得到精确的小车航向角信息，但由于测量误差的累积，长时间后求得的航向角会发散，因此又为小车安装了由三轴陀螺仪、三轴加速度计及三轴磁罗盘组成的集成惯性测量单元，利用磁航向角测量无误差累积的特点，融合修正小车的航向角信息，使系统可以在长时间内有效地运作。

2.2.1ADIS16265接口及数据处理方法

考虑到成本及测量精度等因素，本文中的单轴陀螺仪选用ADI公司生产的ADIS16265[2]，陀螺仪工作电压为4.75～5.25 V，提供±80/±160/±320 °/s三种量程供用户选择，自带温度补偿功能，校准温度范围为-40~+85 ℃，可选测量带宽为50或330 Hz，可配置陀螺仪内部采样频率，输出数据可通过自带的Bartlett窗口FIR滤波器进行处理，滤波阶数可以设置，陀螺仪通过SPI通信协议输出14位数字量角速度测量数据。

ADIS16265的接口电路略——编者注。

本文设置陀螺仪量程为±80 °/s，测量带宽为50 Hz，陀螺仪自带的FIR滤波器滤波阶数为32阶，可将数字滤波器的带宽降至2.6 Hz。考虑到小车导航信息解算的频率要求，经过测量与比较不同采样频率下陀螺仪零偏的平均值与方差，确定陀螺仪内部采样频率为128 Hz。主控芯片通过SPI (CPOL=1，CPHA=1，MSB first,16位) 向陀螺仪写命令，对其进行初始化，写入的第一个字节为寄存器地址，第二个字节为配置字，寄存器配置操作如表1所列。

陀螺仪初始化过程的具体代码略——编者注。

ADIS16265陀螺仪在静止情况下，128 Hz的1 h测量情况如图2所示。多次实验表明，在上电工作5 min后陀螺仪零偏趋于稳定，且每隔5 min需要重新标定，使用时

表1　ADIS16265寄存器配置

图2　单轴陀螺仪零偏变化曲线

应特别注意。

为了提高计算精度，实际测试表明ADIS16265的输出仍需要进行滤波处理，本文采用3连续点中位值滤波，然后对4连续点中位值进行平均的方式，获得了5 min航向角漂移2°的效果。

2.2.2MPU-9150磁航向测量与校正

本文中的集成惯性测量单元选用InvenSense公司生产的MPU-9150[3]，它整合了含三轴陀螺仪与三轴加速度计的MPU-6050和三轴电子罗盘AK8975。MPU-9150采用温度补偿技术和内部滤波器，通过I2C总线接口连接微处理器，工作电压范围为2.375~3.465 V。MPU-9150的配置及接口电路请借鉴参考文献[4]。

使用MPU-9150的三轴加速度计获得元件倾角，在该倾角的基础上利用三轴磁罗盘的磁通量测量结果计算磁航向[5]，由于传感器受小车自身的铁磁干扰[6]，航向计算值会偏离真值。实验室中在无外磁场干扰、已知航向情况下，以10°为间隔对磁罗盘解算结果进行标定，获得各方向的磁罗盘误差，由图3可知，各方向原始计算误差小于2°，在实际使用中进行补偿即可获得高精度磁航向。

图3　静态情况下磁航向角解算精度

2.3其他硬件的设计

小车的电机选用带334线码盘的减速电机，型号是JGA25-371；超声波测距模块选择了国产的US-100超声波测距模块；此外小车还安装了有人公司生产的USR-WIFI232-T串口转无线模块。系统的电源供应选择12 V锂电池，经过电源电路变压为3.3 V及5 V，分别为电机、主控芯片及不同的传感器供电。

参考文献电源电路、超声波测距模块切换电路、光电码盘接口电路及电机驱动电路设计请借鉴[4]。

3软件设计

为了精确地控制小车的运动，需要准确地解算小车导航参数，因此需要合理分配各任务的处理周期，设计各传感器数据采集与处理的方法。

3.1处理逻辑设计

系统要完成传感器数据采集、导航参数解算、对电机的控制等多种任务，根据任务需求为它们分配处理频率。导航解算相关的传感器数据采集及对电机的控制输出采用较高的处理频率，保证了解算与控制的精确；用于避障并辅助导航解算的超声波测距的采样频率相对较低；航位推算及轨迹跟踪参数解算的频率适中，在确保系统正常运行的情况下降低程序计算量。

根据系统的要求，设置不同的任务周期如表2所列。

表2　系统不同任务的周期分配

为了精确控制各任务的处理时机，并设置处理周期，本文将STM32F103VET6的基本定时器6设置为10 ms的溢出中断来完成。在中断服务函数中使用任务周期计数器判断是否到达各个任务的处理时间点，并进行任务处理标志位的设置。在主程序中循环检测各个任务处理标志位情况，若标志位被置位，则执行相应的任务处理程序。具体流程如图4所示。

图4　系统任务处理流程图

除了使用基本定时器6的溢出中断，还使用了PD6双边沿跳变触发的外部中断，用于测量超声波模块测距的渡越时间，需要对这两个中断设置不同的优先级。由于对超声波模块渡越时间的测量过程不能被其他中断打断，因此将PD6引脚对应的EXTI9_5_IRQn中断向量的优先级设置为抢占优先级最高，响应优先级最高。将TIM6的溢出中断对应的TIM6_IRQn中断向量的优先级设置为抢占优先级次高。

3.2惯性测量模块数据的存储与处理

为了提高导航解算精度，使用TIM6的10 ms定时中断进行ADIS16265及MPU-9150的数据采集，以每条记录的方式循环存储，主程序查询数据更新并进行解算。本文在程序中设置数据存储指针与数据处理指针对此过程进行管理。

具体过程如下：在中断服务函数中置标志位，采集惯性测量模块数据存入缓冲区内，并使用数据存储指针记录存储缓冲区空间的使用情况，每次存储后数据存储指针加1。在主程序中检测标志位，取出存储缓冲区内的数据进行处理，将数据处理指针与存储指针进行比较，每次处理后数据处理指针加1，持续处理数据直至数据处理指针与存储指针相等，从而保证了历史数据能得到及时处理。

3.3超声波模块的数据采集与处理

与陀螺仪及磁罗盘的数据采集原理不同，本文从超声波模块处采集到的是测距过程中从测量声波发射到接收的渡越时间。为了在测量渡越时间的同时不影响主程序的运行，将渡越时间的测量放在中断中进行，具体做法是主控芯片的PD6引脚检测超声波ECHO引脚的电平跳变，在中断中控制定时器的开关以实现计时。获得的计时时间即为超声波模块测距的渡越时间TH，通过以下公式计算超声波模块测得的距障碍物的距离：

代码略——编者注。

3.4程序稳定性设计

因存在MPU-9150、ADIS16265、超声波测距等多个高频率、多时间间隔、不同级别的中断测量，大量的中断嵌套导致了微控制器死机现象[7]。在中断处理机制分析基础上，采用了全局变量存储采集数据的方式，减少了动态存储空间分配，无需调节默认堆栈空间大小即可实现程序的稳定运行。

4功能实现

4.1小车的惯性定位解算

已知小车初始位置及航向信息，使用单轴陀螺仪与光电码盘测量值，通过航位推算可求出车辆行驶的轨迹。在直线行驶测试中，惯性定位解算得到的法向偏差与实际测量值相差较大。经过分析，误差最主要的来源是积分求解的航向角随时间发散。换言之，仅使用小车系统内部的测量信息进行惯性定位解算得到的导航参数在长时间使用中精度无法保证，因此应对外部环境的测量数据进行信息融合修正。

4.2多传感器信息融合

本文中小车实验的场地在楼内的回字形走廊中(尺寸为28 m×18 m)，走廊的墙壁基本可以视为光滑连续且平行。因此考虑使用超声波模块测量的小车距左右墙的距离信息与MPU-9150解算的磁航向角进行信息融合，提高小车长时间导航解算的精度。

在进行信息融合前需要对传感器数据的可用性进行判断，对于超声波模块测距信息，需要判断墙壁是否连续且平行。墙壁的连续性可通过相邻两次超声波测量得到的小车距左右墙距离变化是否有突变进行判别；而墙壁的平行性可通过当航向角变化较小时，超声波测距结果的变化情况进行判别。对于磁航向角，其可用性可通过一段较短时间内，惯性求解的航向角变化量与磁航向角变化量二者之间的偏差是否超过设定阈值进行判断。

当判断墙壁连续且平行时，认为超声波测距信息可用。连续存储多组左右超声波模块测距结果，结合小车沿设定轨迹切线方向的前进距离信息，求解出在这段时间内小车的航向角近似值，及小车的法向偏移值，以修正惯性定位解算结果。当超声波测距信息无效时，可使用磁航向角信息融合修正小车航向角信息。

最后，为了保证通过陀螺仪采集的数据在系统运行长时间后仍真实可信，可在系统运行一段时间后，使小车停车重新采集零偏。本文中设定当小车每跑完一圈路径，即系统运行3 min左右后，重新采集10 s内单轴陀螺仪的数据，取平均值更新零偏。

实际跑车结果略——编者注。

可以看出，只使用码盘及陀螺仪测量的内部信息惯性解算得到的轨迹随着时间推移有发散的趋势，而加入了超声波测距模块及磁罗盘测量的外部信息辅助修正导航参数之后，小车的行驶能够很好地跟踪设定的轨迹。最终小车可以沿预定轨迹自主稳定行驶40 min以上，即沿设定轨迹行驶10圈以上。

结语

本文设计了基于STM32F103VET6微控制器的小车导航与控制系统，详细介绍了ADIS16265、MPU-9150、超声波测距传感器数据的有效采集与处理方法，对程序稳定性设计提出了建议，最后介绍了利用多传感器信息融合方法选择有效的传感器数据实现小车的轨迹跟踪控制的方法，达到了理想的效果。

[1] ST Microelectronics.STM32F103VE Product Specifications [EB/OL].[2015-05].http://www.st.com/st-web-ui/static/active/en/resource/technical/document/datasheet/CD00191185.pdf.

[2] Analog Devices.ADIS16260/ADIS16265 Data Sheet [EB/OL].[2015-05].http://www.analog.com/media/en/technical-documentation/data-sheets/ADIS16260_16265.pdf.

[3] InvenSense Inc.MPU-9150 Product Specification Revision 4.3 [EB/OL].[2015-05].http://www.invensense.com/mems/gyro/documents/PS-MPU-9150A.pdf.

[4] 郑润芳,张海.STM32的小车自主定位与控制系统设计[J].单片机与嵌入式系统应用,2013(9):46-49.

[5] Caruso M J.Applications of magnetic sensors for low cost compass systems[C]//Position Location and Navigation Symposium,IEEE,2000:177-184.

[6] 蒋贤志.数字电子罗盘误差分析及校正技术研究[J].现代雷达,2005(6):39-41,44.

[7] Joseph Yiu.ARM Cortex-M3权威指南[M].宋岩,译.北京:北京航空航天大学出版社,2009.

Wang Yue,Zhang Hai

(School of Automation Science and Electrical Engineering,Beihang University,Beijing 100191,China)

Abstract：A self-positioning four-wheel differential speed drive vehicle based on STM32F103VET6 is designed.In the paper,the reasonable method of data acquisition and processing for single-axis gyroscope ADIS16265,integrated inertial measurement unit MPU-9150 and ultrasonic distance measuring module is designed,which is used to complete the calculation of the heading angle with high precision in short-time and the calculation of the magnetic heading which is stable in long-time.The solution for the stack overflow problem encountered in program development is proposed.Finally,the fusion method of multi-sensor information is given,and the dead-reckoning method is used to accomplish self-positioning for the vehicle.The vehicle implements the function of driving autonomously along predetermined trajectory with high stability in long-time.

Key words:STM32;ADIS16265;MPU-9150;magnetic heading;stack overflow

收稿日期：(责任编辑：薛士然2015-06-15)

中图分类号:TP242

文献标识码:A