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讀取其值。所讀取的值為角速度，不會受到小車運動的影響，因此該信號噪聲很小，同時可以由它積分得到小車傾斜角度，可以平滑信號使其更加穩(wěn)定。

　　由于裝置是要在不同的室內區(qū)域進行勘測搜索，再加上未知的環(huán)境，所以角速度信號可能存在一定的偏差，會導致積分后的角度出現大的誤差，無法得到實際的數值。為了消除這個由于偏差而產生的累積誤差，裝置上加上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通過加速度傳感器校正角度