基于DSP的室内惯性导航系统设计

　　3.软件设计及具体实现

　　在软件程序设计上，主要应用九自由度惯性导航传感器(ITG3200+ADXL345+HMC5883L)，结合DSP和卡尔曼滤波算法，能实现较高精度的轮式小车导航和定位。

　　3.1传感器器件程序设计

　　九自由度惯性导航传感器在许多领域都得到了实际的应用，如无人机，救灾机器人等。它包括ITG3200三轴陀螺仪，HMC5883L三轴磁感应传感器和ADXL345三轴加速度传感器，所以可以得到加速度，角速度以及角度实时的数值。

　　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 = IIC_WriteData(0xD0， 0x16， 1);

　　Data = 0x00;

　　Return4 = IIC_WriteData(0xD0， 0x17， 1);

　　if(Return1 Return2 Return3 Return4)

　　return 1;

　　else

　　return 0;

　　}

　　其具体功能实现可以在主程序中通过SCI读取其值。所读取的值为角速度，不会受到小车运动的影响，因此该信号噪声很小，同时可以由它积分得到小车倾斜角度，可以平滑信号使其更加稳定。

　　由于装置是要在不同的室内区域进行勘测搜索，再加上未知的环境，所以角速度信号可能存在一定的偏差，会导致积分后的角度出现大的误差，无法得到实际的数值。为了消除这个由于偏差而产生的累积误差，装置上加上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(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(Return1

　　return 1;

　　else

　　return 0;

　　}

　　通过ADXL345所得到的角度，和陀螺仪积分后的角度进行对比，然后使用它们的偏差改变陀螺仪的输出，从而积分后的角度慢慢校正到实际的角度，如图5所示。

　　图5通过加速度传感器校正角度