《基于dsp的室内惯性导航系统设计.》由会员分享，可在线阅读，更多相关《基于dsp的室内惯性导航系统设计.（11页珍藏版）》请在金锄头文库上搜索。
1、基于DSP的室内惯性导航系统设计2015-05-18 11:05:42 NI电网自动化系统(NI Grid Automation Sy NI智能嵌入式系统加速工业物联网（IIOT） NI趋势展望2015之自动化测试 LabVIEW 2014评估版软件导读轮式遥控机器人已经应用在地震、火灾等一些危险的室内区域进行救援和探测，或执行反恐任务。由于在这些特殊的环境下存在诸多的未知因素，且室内无GPS信号，人们不得不依靠先进的科学技术和仪器来获取遥控机器人小车的导航信息。轮式遥控机器人已经应用在地震、火灾等一些危险的室内区域进行救援和探测，或执行反恐任务。由于在这些特殊的环境下存在诸多的未知因素，且室
2、内无GPS信号，人们不得不依靠先进的科学技术和仪器来获取遥控机器人小车的导航信息。但是目前轮式运动小车主要采用的导航传感方式有视觉、光电、超声、里程计等，比较容易被外界环境干扰，不能满足广大市场的需求。但是惯性导航装备安置在运载体内，工作时不依赖外界信息，也不向外界辐射能量，不容易遭到滋扰，是一种自立式导航系统，优于上述的导航传感例子。并且近年来MEMS(微机电系统)结构的惯性传感器随半导体技术的进步得到了迅速发展，使其低成本而高精度的期望得到了实现。MEMS惯性传感器组成的惯性导航装置结合轮式小车的里程计，能够产生导航和定位信息，减少对外部环境的倚赖，实现在外部环境条件(例如光照、墙壁材质)
3、未知情况下的导航。由于是在室内区域进行勘测搜索，小车的运行特点与一般的飞机、船、车不同，它的运动轨迹变化较快，且在运动时存在一定的振动，因此常用的卡尔曼滤波算法需要进一步改进才能应用。惯性传感器采集数据量大，且进行惯性导航时需要大量的浮点运算，因此本项目采用了具有强大数字信号处理功能的DSP 28335芯片和PC控制终端，实现惯性传感器的数据采集、时序逻辑控制、与驱动系统通信和地图显示功能，具有体积小、成本低、功耗低等优点。综上所述，本文将选用低成本的MEMS器件，结合DSP和卡尔曼滤波算法，能实现较高精度的轮式小车导航和定位。1系统体系架构本文的目标是研制一个轮式小车惯性导航系统，能够通过w
4、ifi实现PC终端和手持终端控制轮式小车行动以及小车所采集数据的传输。搭建如下图1所示的系统，TI公司的浮点DSP TMS320F28335芯片作为主数字信号处理器，采集各MEMS惯性传感器的信号并处理，处理结果通过WIFI将数据输送到PC终端;PC终端负责显示定位结果和地图显示，并向小车驱动系统发送控制命令，同时接收驱动系统反馈的里程计信息。图1总体架构2.硬件计划及详细实现硬件设计上，主要分为核心板和驱动板。核心板包括DSP最小系统，JTAG下载口设计，系统电源供给电路和MEMS传感器，WIFI模块等。而驱动板主要设计的内容是直流大电机的驱动模块。2.1核心板设计2.1.1电源电路设计TM
5、S320F28335工作时需要的电压不同：内核电压(1.9 V)与I/O供电电压(3.3 V)，对于电源比较敏感，所以电源部分利用两路输出电源器件TPS767D318来实现，如图2所示。同时根据仿真实验和实际焊接电路的测试，电源模块输出端最好使用一些容值不小于10uf的保护电容，且不能使用贴片电容，否则工作不稳定。图2 DSP电源设计在电源设计中，考虑到TPS767D318芯片可以产生复位信号，所以在核心板上并没有再另外为DSP设计复位电路。2.1.2 JTAG下载口电路设计图3为JTAG电路，按照仿真器的通信引脚选择14脚的仿真接口，同时要注意EMU0和EMUl信号必须通过上拉电阻连接至电源
6、，其中上拉电阻为10k。图3 JTAG电路设计2.2小车驱动板设计在本装置中，我们采用BTS7960作为直流电机驱动芯片。BTS7960是集成的大电流半桥驱动，其内部包含了一片NMOS、一片PMOS和一片半桥门集驱动，在IOUT = 9 A，VS= 13.5V，Tj = 25C时，其内阻抗为17m。装置采用了两个直流大电机，如图4所示为驱动单个电机前后转向的电路图。图4驱动模块电路设计在电机驱动这里，需要注意一个细节，就是电机在转动过程中有可能会产生反向的电动势，使电流一瞬间过大，导致单片机复位甚至有可能烧坏芯片。因此在设计过程中，可以考虑在单片机PWM输入到电机驱动接口的地方添加光耦器件隔离
7、或者二极管。如图5所示，驱动板选择了tlp521-4这种光耦器件设计隔离电路，减小电压的干扰，减化电路的设计，同时也把四路PWM的I/O电平从3.3上拉到5V.图5 TLP521隔离电路3.软件设计及具体实现在软件程序设计上，主要应用九自由度惯性导航传感器(ITG3200+ADXL345+HMC5883L)，结合DSP和卡尔曼滤波算法，能实现较高精度的轮式小车导航和定位。3.1传感器器件程序设计九自由度惯性导航传感器在许多领域都得到了实际的应用，如无人机，救灾机器人等。它包括ITG3200三轴陀螺仪，HMC5883L三轴磁感应传感器和ADXL345三轴加速度传感器，所以可以得到加速度，角速度以
8、及角度实时的数值。ITG3200是MEMS三轴陀螺仪，可以测量小车的旋转角速度，同时也可以通过积分把角速度转换为小车的倾角。程序中，ITG3200的初始化如下：unsigned char INIt_ITG3200(void)unsigned char Return1，Return2，Return3，Return4;unsigned char Data;Data = 0x00;Return1 = IIC_WriteData(0xD0， 0x3E， 1);Data = 0x07;Return2 = IIC_WriteData(0xD0， 0x15， 1);Data = 0x1E;Return3 =
9、 IIC_WriteData(0xD0， 0x16， 1);Data = 0x00;Return4 = IIC_WriteData(0xD0， 0x17， 1);if(Return1 Return2 Return3 Return4)return 1;elsereturn 0;其具体功能实现可以在主程序中通过SCI读取其值。所读取的值为角速度，不会受到小车运动的影响，因此该信号噪声很小，同时可以由它积分得到小车倾斜角度，可以平滑信号使其更加稳定。由于装置是要在不同的室内区域进行勘测搜索，再加上未知的环境，所以角速度信号可能存在一定的偏差，会导致积分后的角度出现大的误差，无法得到实际的数值。为了消
10、除这个由于偏差而产生的累积误差，装置上加上ADXL345三轴加速度传感器对于获得的角度信息进行校正。ADXL345初始化如下：unsigned char Init_ADXL345(void)unsigned char Return1，Return2，Return3，Return4;unsigned char Data;Data = 0x0b;byReturn1 = IIC_WriteData(0xA6， 0x31， 1);Data = 0x08;Return2 = IIC_WriteData(0xA6， 0x2c， 1);Data = 0x08;Return3 = IIC_WriteData(
11、0xA6， 0x2d， 1);Data = 0x80;Return4 = IIC_WriteData(0xA6， 0x2e， 1);Data = 0x00;Return4 = IIC_WriteData(0xA6， 0x1e， 1);Data = 0x00;Return4 = IIC_WriteData(0xA6， 0x1f， 1);Data = 0x05;Return4 = IIC_WriteData(0xA6， 0x20， 1);if(Return1return 1;elsereturn 0;通过ADXL345所得到的角度，和陀螺仪积分后的角度进行对比，然后使用它们的偏差改变陀螺仪的输出，
12、从而积分后的角度慢慢校正到实际的角度，如图5所示。图5通过加速度传感器校正角度HMC5883L三轴磁感应传感器的作用相当于罗盘，在水平情况下，无需借助其他传感器便可以计算出航向。其初始化如下：unsigned char Init_HMC5883(void)unsigned char Return1;unsigned char Data;/ Bit4 Bit3等于11时，选择2000度/秒的量程Data = 0x00;Return1 = IIC_WriteData(0x3C， 0x02， 1);if(Return1)return 1;elsereturn 0;由于装置是要在不同环境下进行工作的，
13、所以其并不能保持时刻水平，就需要加速度传感器来纠正由于倾斜引起的误差。3.2卡尔曼滤波算法应用于是装置在室内区域进行勘测搜索，小车的运行特点与一般的飞机、船、车不同，它的运动变化快，轨迹不定，而且要适用于不同的环境下工作，因此常用的卡尔曼滤波算法需要进一步改进才能应用。卡尔曼过滤是用前一个估计值和最近一个观察数据，来估计信号的当前值，它是用状态方程和递推的方法进行估计的，它的解是以估计值形式给出的。其运用在加速度器和陀螺仪上的卡尔曼滤波程序如下：/ float gyro_m：陀螺仪测得的量(角速度)/float incAngle：加速度器测得的角度值#define dt 0.0015/卡尔曼滤
14、波采样频率#define R_angle 0.71 /测量噪声的协方差(即是测量偏差)#define Q_angle 0.0001/过程噪声的协方差#define Q_gyro 0.0003 /过程噪声的协方差过程噪声协方差为一个一行两列矩阵float kalmanUpdate(const float gyro_m，const float incAnglefloat K0;/含有卡尔曼增益的另外一个函数，用于计算最优估计值float K1;/含有卡尔曼增益的函数，用于计算最优估计值的偏差float Y0;float Y1;float Rate;/去除偏差后的角速度float Pdot4;/过程
15、协方差矩阵的微分矩阵float angle_err;/角度偏量float E;/计算的过程量static float angle = 0; /下时刻最优估计值角度static float q_bias = 0; /陀螺仪的偏差static float n22 = 1， 0 ， 0， 1 ;/过程协方差矩阵Rate = gyro_m - q_bias;/计算过程协方差矩阵的微分矩阵Pdot0 = Q_angle - P01 - P10;Pdot1 = - n11;Pdot2 = - n11;Pdot3 = Q_gyro;angle += Rate * dt; /角速度积分得出角度n00 += Pdot0 * dt; /计算协方差矩阵n01 += Pdot1 * dt;n10 += Pdot2 * dt;n11 += Pdot3 * dt;angle_err = incAngle
