新聞中心

EEPW首頁(yè) > 嵌入式系統(tǒng) > 設(shè)計(jì)應(yīng)用 > 基于DSP的室內(nèi)慣性導(dǎo)航系統(tǒng)設(shè)計(jì)

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

作者: 時(shí)間:2015-05-23 來(lái)源:電子產(chǎn)品世界 收藏

  HMC5883L三軸磁感應(yīng)傳感器的作用相當(dāng)于羅盤(pán),在水平情況下,無(wú)需借助其他傳感器便可以計(jì)算出航向。其初始化如下:

本文引用地址:http://m.butianyuan.cn/article/274609.htm

  unsigned char Init_HMC5883(void)

  {

  unsigned char Return1;

  unsigned char Data;

  // Bit4 Bit3等于11時(shí),選擇2000度/秒的量程

  Data = 0x00;

  Return1 = IIC_WriteData(0x3C, 0x02, 1);

  if(Return1)

  return 1;

  else

  return 0;

  }

  由于裝置是要在不同環(huán)境下進(jìn)行工作的,所以其并不能保持時(shí)刻水平,就需要加速度傳感器來(lái)糾正由于傾斜引起的誤差。

  3.2卡爾曼濾波算法應(yīng)用

  于是裝置在室內(nèi)區(qū)域進(jìn)行勘測(cè)搜索,小車的運(yùn)行特點(diǎn)與一般的飛機(jī)、船、車不同,它的運(yùn)動(dòng)變化快,軌跡不定,而且要適用于不同的環(huán)境下工作,因此常用的卡爾曼濾波算法需要進(jìn)一步改進(jìn)才能應(yīng)用??柭^(guò)濾是用前一個(gè)估計(jì)值和最近一個(gè)觀察數(shù)據(jù),來(lái)估計(jì)信號(hào)的當(dāng)前值,它是用狀態(tài)方程和遞推的方法進(jìn)行估計(jì)的,它的解是以估計(jì)值形式給出的。其運(yùn)用在加速度器和陀螺儀上的卡爾曼濾波程序如下:

  // float gyro_m:陀螺儀測(cè)得的量(角速度)

  //float incAngle:加速度器測(cè)得的角度值

  #define dt 0.0015//卡爾曼濾波采樣頻率

  #define R_angle 0.71 //測(cè)量噪聲的協(xié)方差(即是測(cè)量偏差)

  #define Q_angle 0.0001//過(guò)程噪聲的協(xié)方差

  #define Q_gyro 0.0003 //過(guò)程噪聲的協(xié)方差過(guò)程噪聲協(xié)方差為一個(gè)一行兩列矩陣

  float kalmanUpdate(const float gyro_m,const float incAngle

  {

  float K0;//含有卡爾曼增益的另外一個(gè)函數(shù),用于計(jì)算最優(yōu)估計(jì)值

  float K1;//含有卡爾曼增益的函數(shù),用于計(jì)算最優(yōu)估計(jì)值的偏差

  float Y0;

  float Y1;

  float Rate;//去除偏差后的角速度

  float Pdot[4];//過(guò)程協(xié)方差矩陣的微分矩陣

  float angle_err;//角度偏量

  float E;//計(jì)算的過(guò)程量

  static float angle = 0; //下時(shí)刻最優(yōu)估計(jì)值角度

  static float q_bias = 0; //陀螺儀的偏差

  static float n[2][2] = {{ 1, 0 }, { 0, 1 }};//過(guò)程協(xié)方差矩陣

  Rate = gyro_m - q_bias;

  //計(jì)算過(guò)程協(xié)方差矩陣的微分矩陣

  Pdot[0] = Q_angle - P[0][1] - P[1][0];

  Pdot[1] = - n[1][1];

  Pdot[2] = - n[1][1];

  Pdot[3] = Q_gyro;

  angle += Rate * dt; //角速度積分得出角度

  n[0][0] += Pdot[0] * dt; //計(jì)算協(xié)方差矩陣

  n[0][1] += Pdot[1] * dt;

  n[1][0] += Pdot[2] * dt;

  n[1][1] += Pdot[3] * dt;

  angle_err = incAngle - angle; //計(jì)算角度偏差

  E = R_angle + P[0][0];

  K0 = n[0][0] / E; //計(jì)算卡爾曼增益

  K1 = n[1][0] / E;

  Y0 = n[0][0];

  Y1 = n[0][1];

  n[0][0] -= K0 * Y0; //跟新協(xié)方差矩陣

  n[0][1] -= K0 * Y1;

  n[1][0] -= K1 * Y0;

  n[1][1] -= K1 * Y1;

  angle += K0 * angle_err; //給出最優(yōu)估計(jì)值

  q_bias += K1 * angle_err;//跟新最優(yōu)估計(jì)值偏差

  return angle;

 ?。?/p>

  通過(guò)濾波時(shí)數(shù)據(jù)平滑將加速度輸出電壓附近產(chǎn)生的波動(dòng)噪聲濾掉。



關(guān)鍵詞: DSP JTAG

評(píng)論


相關(guān)推薦

技術(shù)專區(qū)

關(guān)閉