基于MCU的室外移动机器人组合导航定位系统

 对于在室外环境工作的移动机器人通常使用惯导/卫星组合导航方式。惯性导航系统[1]具有完全自主、抗干扰强、隐蔽能力好和输出参数全面等优点，但它的鲁棒性极低，误差会不断随时间累积发散。卫星导航系统具有精度高、定位范围广和误差不随时间累积等优点，但其自主性差、易受外界遮挡和干扰、接收机数据更新频率低等缺点。因此工程上常常将两者互补结合使用，组成卫星/惯性组合导航系统。

本文以低功耗MSP430F149为核心，设计了能够同时实现卫星导航(GNSS)接收机、惯性测量单元(IMU)、气压高度等导航信息的高速采集与高速合路传输，并进行初步导航定位信息融合的导航系统，即可为室外移动机器人提供直接的导航服务，也可作为高精度组合导航系统的原始测量信息高速采集系统。

系统设计的关键是利用单片机有限的接口资源实现了多传感器信息并行采集，设计了有效的数据同步方法，解决了气压传感器数据手册疏漏导致的无法接入问题，给出了机器人组合定位的基本方法。系统充分利用了MSP430F149单片机的能力，具有结构简单、低功耗、对传感器具有普适性等优点。

1 总体设计

本系统由电源、气压计接口、IMU接口、GNSS接收机接口、SPI转UART模块及MSP430F149构成。系统组成如图1所示。

组合导航系统的功能实现分为IMU数据接收与解析、GNSS数据接收与解析、气压计数据接收与解析、组合导航解算以及数据输出五个部分。IMU数据接收与解析功能用来获取导航解算中需要的加速度和角速度信息；GNSS数据接收与解析功能用来获取导航解算中需要的位置和速度信息(松耦合组合)或者GNSS伪距和伪距率（紧耦合组合）；气压计数据接收与解析功能用来获取高度信息；组合导航解算功能为系统核心，用来进行组合导航解算；数据的输出包括原始数据包的整合输出和解算结果的输出。

 

 

图1 系统组成结构图

 

 

本文所使用的惯性器件和GNSS接收机都是RS-232电平的UART接口，具有通用性，用户可根据成本考虑不同精度的设备。气压计选用美国MEAS公司生产的MS5803-02BA，已经固化在电路中。

 

 

 

2 硬件电路设计

2.1 微控制器接口

整个组合导航定位系统需要三个UART接口和两个SPI接口。其中两个UART接口由430单片机自带的UART资源提供，另外一个UART接口由GPIO模拟SPI通过MAX3111E芯片转化得到；两个SPI接口由GPIO模拟得到。另外需要一个外部中断引脚捕获秒脉冲信号（PPS）、一个外部中断引脚捕获MAX3111E中断信号。MSP430F149管脚资源分配如表1所示。

 

 

 

2.2电源电路

本系统供电需求为3.3V供电，因此采用AMS1117稳压芯片，接入5V电源即可输出3.3V稳定电压，可提供1A电流，满足系统供电需求。电路设计如图2所示。

 

 

图2 电源电路

 

2.3 IMU器件及GNSS接收机接口电路

IMU器件及GNSS接收机都采用UART接口方式接入，采用RS232协议。因此可使用430单片机上自带的两个UART接口，但是需要进行TTL电平与RS232电平转换。这里采用常见的MAX3232芯片，电路设计如图3所示。

 

 

图3 IMU及GNSS接口电路

 

 

2.4 气压计MS5803-02BA接口电路

MS5803-02BA[3]是由MEAS公司生产的数字压力传感器，分辨率达10cm。芯片内部包含一个高线性的压力传感器和一个内部工厂标定系数的超低功耗24位ΔΣ型ADC。该款芯片有SPI和I2C两种接口方式，通过芯片的PS引脚配置了选择不同的接口方式（PS置低时，采用SPI工作模式；PS置高时，采用I2C工作模式）。本文所阐述的定位系统将气压计配置为SPI工作模式。MS5803-02BA与微控制器间的接口电路设计如图4所示。

 

 

图4 MS5803-02BA接口电路

 

MS5803-02BA的控制命令包括复位命令、温度ADC命令、气压ADC命令、ADC读取命令、PROM读取命令。控制命令如表2所示。

控制命令通过SDI口移位输入，响应结果从SDO移位输出。输入的电平判定在时钟信号的上升沿，输出的电平判定在时钟信号的下降沿。输出的气压值可以进行温度补偿，需要利用芯片内部PROM中的系数来补偿。ADC读取命令输入之后，输出24位ADC结果；PROM读取命令输入之后，输出16位补偿系数。

 

 

 

 

下面是读取ADC的C语言代码：

CSN_OFF_MS();//CS置低

SPI_WRITE_8BIT(CMD);//SDI移入8位CMD

delay_ms(10); //延时10ms

CSN_ON_MS();//CS置高

CSN_OFF_MS();//CS置低

SPI_WRITE_8BIT(0x00);//SDI移入8位0x00

result = SPI_READ_24BIT();//SDO移出24位

CSN_ON_MS();//CS置高

下面是读取PROM的C语言代码：

CSN_OFF_MS();//CS置低

SPI_WRITE_8BIT(CMD); //SDI移入8位CMD

result=SPI_READ_16BIT();//SDO移出24位

CSN_ON_MS();//CS置高

结合器件的使用手册及手册疏漏的地方，使用MS5803-02BA时需要注意：

1、温度和气压ADC命令发送之后，芯片内部需要一定的时间进行采样转换，具体时间与过采样率（OSR）有关，最大需求时间为10ms，因此本文采用的延时时间为10ms；

2、片选信号CS的下降沿到时钟SCLK信号的第一个上升沿至少要有21ns的时间延迟，否则命令无法正确写入芯片；

3、8位的ADC读取命令之后，必须保持CS片选信号持续为低，再产生24位时钟信号输入，将24位的ADC结果读取出来（即一个命令字为8位，但实际需要32个连续的时钟周期才能完整读取ADC结果）；

4、对于PROM读取命令同ADC读取命令，一共需要24个连续的时钟周期完成，其中8位命令字输入，16位数据读出。

5、对于所有从SDO移位输出数据的同时，都需要SDI输入端保持低电平。

 

 

2.5 SPI转UART电路

由于MSP430F149的片上UART资源都被IMU器件和GNSS接收机占用，因此需要扩充一个UART接口才能满足定位系统与PC机间的通信。为此将MSP430F149上的GPIO模拟SPI，再通过MAX3111E芯片转成UART接口。

MAX3111E是MAXIM公司推出的全功能收发器，内部包含UART和RS232电平转换两个独立的部分。UART部分包括兼容于SPI的串行接口、可编程波特率发生器、发送移位寄存器、接收移位寄存器、8字长的FIFO以及4种可屏蔽中断发生器；RS232部分包括电泵电容、硬件关断（SHDN管脚），具有±15kV静电保护作用。它可以选择1.8432MHz或3.6864MHz两种晶振作为外部晶振，芯片能够工作在300bps～230kbps波特率，本文使用的是3.6864MHz晶振。MAX3111E与MSP430F149连接电路如图5。

 

 

图5 SPI转UART电路

 

 

MAX3111E的控制命令分为写配置命令、读配置命令、写数据命令以及读数据命令四种。本文写入芯片的配置命令是0xE809，8bit数据长度、使能FIFO、发送中断使能、波特率38400bps。数据的写入需要在每个字节前面加上0x81，例如若要发送字节0x55，则需要从DIN端移位输入0x8155。

 

下面是连续发送n个字节的C语言代码：

SPI_WRITE_16BIT(0xE809);//写配置命令

delay_ms(10);//延时10ms

SPI_WRITE_16BIT(0x8100+DATA1);//发送第1个数据DATA1

delay_ns(250);//延时250ns

SPI_WRITE_16BIT(0x8100+DATA2);//发送第2个数据DATA2

……

delay_ns(250);//延时250ns

SPI_WRITE_16BIT(0x8100+DATAn);//发送第n个数据DATAn

使用MAX3111E时需要注意：

1、片选信号CS的下降沿到时钟SCLK信号的第一个上升沿至少要有100ns的时间延迟；

2、芯片判定片选信号CS失效时，需要CS管脚保持高电平的时间至少为200ns。因此当需要连续发送数据时，两个写数据命令之间至少需要200ns，建议间隔250ns；

3、写配置命令需要晶振工作稳定，因此可以在写配置命令后再读配置，直到写入和读出的配置数据相应位相同时才保证芯片按需求配置完毕。

 

 

3 软件设计

3.1 GPIO模拟SPI实现方法

使用GPIO模拟SPI，对选定的IO口进行时序上的电平操作，模拟出SPI时序。本文将SPI的电平操作采用宏定义的方式，可参考表3中的源代码。

 

 

程序按照SPI逻辑关系使用上述的宏定义，可以在选定的IO端口产生SPI信号，并且可以灵活的控制每个CS有效期间的时钟数。使用GPIO模拟的SPI具有操作直观、灵活可变等优点。相比较模拟得到的SPI，控制器上自带的SPI接口基本不占用控制器资源，发送和接收的移位、时钟信号的产生都交由内部SPI模块处理。而GPIO模拟的SPI需要控制器不断对IO进行操作，因此会占用控制器处理时间。时钟信号也因对电平操作消耗机器周期，导致时钟信号速度有限。

3.2 数据的采集和处理方法

IMU和GNSS接收机的数据会主动发送到430单片机的UART接口，本文采用中断接收的方式接收数据，将每次到来的一个字节循环存储在指定的存储区；气压计需要430单片机查询式获取高度信息。因此当需求IMU或者GNSS信息的时候，需要在存储区内扫描数据，扫描依据是设备发送一帧数据的协议（帧头、帧尾、CRC等），再根据协议获取相应物理量的数值；当需要高度信息的时候，需要430单片机对气压计发送控制字获取信息。在对IMU和GNSS信息扫描时需要一个变量ptr保存有效数据的首地址在存储区内的偏移地址，为了不重复使用已用数据，需要在利用完信息后，破坏这一帧有效数据的帧头帧尾。

另外需要注意，使用循环存储数据和扫描取用这种方式，需要数据的处理速度比数据的接收速度快，否则旧数据会被新数据覆盖。

 

 

 

3.3 高度计算方法

 

高度信息的获取需要通过MS5803-02BA获取的气压信息转换得到。对这款气压计的操作，需要先在芯片内部的PROM获取C1~C6六个参数，再获取温度ADC结果D1和气压ADC结果D2，最后利用官方提供的计算公式计算得到带补偿的气压值。气压值获取的软件流程图如图6。

 

 

图6 高度计算流程图

 

 

按照气压计的用户手册中提供的计算公式，最后可以计算得到一个经过温度补偿的标准气压值，单位mbar。高度信息的获取还需要将气压值经过函数关系转换。高度h和标准大气P之间有如下函数关系：

 

 

 

Th=288.15K，是g0对应高度下的温度下限值；β=-6.5K/km，是温度的垂直变化率；H=0m，是g0对应的高度；Ph=101325Pa，是g0对应高度下的气压下限值；R=287.05287m²/(K●s²)，是气体常数；g0=9.80665m/s²，是海平面重力加速度。

在气压与高度的关系转换中，本文采用分段线性化的方法拟合它们的非线性关系。在不同气压值区间内，线性化公式得到h=a●P+b，取得不同的线性参数a、b，参考表4。

 

 

 

 

3.4 组合导航的时间对准

进行组合导航滤波解算时，从IMU和GNSS接收机接收到的数据在时间上应该是同步的，因为如果在一个数据融合点上，进入Kalman滤波器的来自两个子系统中的数据来自不同的时间点，会给滤波计算带来误差，同时也会给校正计算带来影响。GNSS接收机输出的导航数据都带有精确的时间标签，而从 IMU 输出的数据只有一个相对时间标签。以GNSS接收机的时间标签作为时间基准，分析时间差的组成。系统时间示意图如图7所示。

 

 

图7 系统时钟示意图

 

 

3.4.1 时间差分析

当一个GNSS数据（1Hz）到来时，接收机产生一个秒脉冲信号（PPS），用于时间对准。GNSS接收机本身存在数据时延：接收机整个计算过程会产生一个解算时间延迟△t₁、从卫星导航接收机和惯导系统输出的数据分别经过McBSP和RS-232数据接口输入导航解算处理器会产生一定的传输延迟△t₂。因此在PPS信号前的△t₁+△t₂时刻才是当前接收到的GNSS数据帧的真实时刻。IMU数据处理时间相对于递推时间来说很小，可以忽略。另外PPS脉冲信号和惯性测量信号之间的时间差△τ，它描述GNSS绝对时间和IMU相对时间之间的关系，使得GNSS和IMU统一在同一个时间标准下。

3.4.2 处理思想

根据总时间差△T =△t₁+△t₂+△τ，找到IMU数据对应的插值时刻。通过时间△T就可以获得对准时间点与最近的两个惯性采集时刻，其后对时间同步点两侧时间点上，即图中t(k-1)和t(k)时间点的惯性数据进行线性插值运算，就可以获得了惯导数据和GNSS接收机数据在同一时间点上的同步化测量数据。

3.4.3 实施方法

GNSS接收机解算时间△t₁由接收机提供。

传输时间△t₂通常为一个固定时间，使用示波器分别测量GNSS接收机发送时间和导航解算电路的接收时间，再将两个时间作差即可获得。

 

△τ的获取需要开启MSP430F149的计时器和外部中断，通过中断计时的方式获得。具体方法是：通过将PPS信号接入导航处理器的外部中断接口，采用边沿触发方式触发中断事件的发生。中断事件启动计时器工作。当通过串口中断方式接收到IMU数据时，读取计时器的值，即可得到IMU数据相对于PPS的延迟时间△τ。

结论

本文基于MSP430F149单片机设计的室外移动机器人组合导航定位系统，通过接口的扩展使得该款定位系统能够接入IMU、GNSS接收机、气压计三路信息，完成初步导航定位服务功能，同时可作为多路数据采集设备，将多路数据整合到一路高速输出接口，用于进一步的高精度导航解算。该系统根据使用者的需求不同，可接入不同成本和精度的设备，只要满足RS-232协议即可。笔者将其实际运用，整个系统充分利用该款单片机的资源，结构简单、功耗低、适用范围广，不仅可作为初步导航定位服务的设备，还可作为多路数据采集设备。

 

参考文献

[1] 张宗麟. 惯性导航与组合导航[M], 航空工业出版社.2000. 7-8

[2]TEXAS INSTRUMENTS. MSP430x1xx Family User's Guide[EB/OL]. [2006].http://www.ti.com.cn/cn/lit/ug/slau049f/slau049f.pdf

[3]MeasurementSpecialties Inc. MS5803-02BA Miniature Altimeter Module[EB/OL]. [2013-03].

http://www.meas-spec.cn/downloads/MS5803-02BA.pdf

[4]Maxim Integrated Products. MAX3110E/MAX3111EDatasheet[EB/OL]. [2005].http://datasheets.maximintegrated.com/en/ds/MAX3110E-MAX3111E.pdf

[5] 郝振海, 黄圣国. 高精度气压高度表的研制 [J]. 南京航空航天大学学报, 2009, 41(1): 134-138.