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通过加速度传感器校正角度
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；
else
return 0；
}
由于装置是要在不同环境下进行工作的，所以其并不能保持时刻水平，就需要加速度传感器来纠正由于倾斜引起的误差。
3.2卡尔曼滤波算法应用
于是装置在室内区域进行勘测搜索，小车的运行特点与一般的飞机、船、车不同，它的运动变化快，轨迹不定，而且要适用于不同的环境下工作，因此常用的卡尔曼滤波算法需要进一步改进才能应用。卡尔曼过滤是用前一个估计值和最近一个观察数据，来估计信号的当前值，它是用状态方程和递推的方法进行估计的，它的解是以估计值形式给出的。其运用在加速度器和陀螺仪上的卡尔曼滤波程序如下：
// float gyro_m：陀螺仪测得的量（角速度）
//float inCANgle：加速度器测得的角度值
#define dt 0.0015//卡尔曼滤波采样频率
#define R_angle 0.71 //测量噪声的协方差（即是测量偏差）
#define Q_angle 0.0001//过程噪声的协方差
#define Q_gyro 0.0003 //过程噪声的协方差过程噪声协方差为一个一行两列矩阵
float kalmanUpdate（const float gyro_m，const float incAngle
{
float K0；//含有卡尔曼增益的另外一个函数，用于计算最优估计值
float K1；//含有卡尔曼增益的函数，用于计算最优估计值的偏差
float Y0；
float Y1；
float Rate；//去除偏差后的角速度
float Pdot[4]；//过程协方差矩阵的微分矩阵
float angle_err；//角度偏量
float E；//计算的过程量
static float angle = 0； //下时刻最优估计值角度
static float q_bias = 0； //陀螺仪的偏差
static float n[2][2] = {{ 1， 0 }， { 0， 1 }}；//过程协方差矩阵
Rate = gyro_m - q_bias；
//计算过程协方差矩阵的微分矩阵
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； //计算协方差矩阵
n[0][1] += Pdot[1] * dt；
n[1][0] += Pdot[2] * dt；
n[1][1] += Pdot[3] * dt；
angle_err = incAngle - angle； //计算角度偏差
E = R_angle + P[0][0]；
K0 = n[0][0] / E； //计算卡尔曼增益
K1 = n[1][0] / E；
Y0 = n[0][0]；
Y1 = n[0][1]；
n[0][0] -= K0 * Y0； //跟新协方差矩阵
n[0][1] -= K0 * Y1；
n[1][0] -= K1 * Y0；
n[1][1] -= K1 * Y1；
angle += K0 * angle_err； //给出最优估计值
q_bias += K1 * angle_err；//跟新最优估计值偏差
return angle；
｝
通过滤波时数据平滑将加速度输出电压附近产生的波动噪声滤掉。
本文关键字：暂无联系方式DSP/FPGA技术，单片机-工控设备 - DSP/FPGA技术
上一篇：数字信号控制器中断系统与设置方法
