基于DSP的室內(nèi)慣性導(dǎo)航系統(tǒng)設(shè)計

　　3.軟件設(shè)計及具體實現(xiàn)

　　在軟件程序設(shè)計上，主要應(yīng)用九自由度慣性導(dǎo)航傳感器(ITG3200+ADXL345+HMC5883L)，結(jié)合DSP和卡爾曼濾波算法，能實現(xiàn)較高精度的輪式小車導(dǎo)航和定位。

　　3.1傳感器器件程序設(shè)計

　　九自由度慣性導(dǎo)航傳感器在許多領(lǐng)域都得到了實際的應(yīng)用，如無人機，救災(zāi)機器人等。它包括ITG3200三軸陀螺儀，HMC5883L三軸磁感應(yīng)傳感器和ADXL345三軸加速度傳感器，所以可以得到加速度，角速度以及角度實時的數(shù)值。

　　ITG3200是MEMS三軸陀螺儀，可以測量小車的旋轉(zhuǎn)角速度，同時也可以通過積分把角速度轉(zhuǎn)換為小車的傾角。程序中，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;

　　}

　　其具體功能實現(xiàn)可以在主程序中通過SCI讀取其值。所讀取的值為角速度，不會受到小車運動的影響，因此該信號噪聲很小，同時可以由它積分得到小車傾斜角度，可以平滑信號使其更加穩(wěn)定。

　　由于裝置是要在不同的室內(nèi)區(qū)域進(jìn)行勘測搜索，再加上未知的環(huán)境，所以角速度信號可能存在一定的偏差，會導(dǎo)致積分后的角度出現(xiàn)大的誤差，無法得到實際的數(shù)值。為了消除這個由于偏差而產(chǎn)生的累積誤差，裝置上加上ADXL345三軸加速度傳感器對于獲得的角度信息進(jìn)行校正。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所得到的角度，和陀螺儀積分后的角度進(jìn)行對比，然后使用它們的偏差改變陀螺儀的輸出，從而積分后的角度慢慢校正到實際的角度，如圖5所示。

　　圖5通過加速度傳感器校正角度