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

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;

　　｝

　　通过滤波时数据平滑将加速度输出电压附近产生的波动噪声滤掉。

　　4 实验测试与结果分析

　　在装置硬件和软件搭建完成后，通过原先设计好的PC端软件和平板安卓软件，搜索到Wi-Fi，自动连入局域网。在实验室测试环境下，小车可以自由行驶并反馈回实时的数据并进行测试比较。

　　4.1 航向角数据测试

　　HMC5883L采集到的航向角存在随机波动性，因此可以对于进行多次采集平均值滤波，以提高系统输出的数据稳定性。如图6为一定时间内滤波前和滤波后航向角的数据。

　　图6 航行角数据采集和对比图

　　上图的数据对比可以得知，航向所采集到的数据进行平均值采样滤波优化后误差大大减小，比原有数据稳定许多，说明滤波效果明显。

　　4.2 角度数据测试

　　在角度获得的数据，可以使用ADXL345和ITG3200所得的数据，转换为横滚角和俯仰角。再结合卡尔曼滤波对数据进行滤波以达到更准确的实验结果。如下表1为滤波1前角度的数据及误差，表2为滤波后角度的数据及误差。

　　表1 滤波前角度的数据及误差

　　表2 滤波后角度的数据及误差

　　由表1和表2的测试数据可知，经过滤波后，最大的误差从3.8降到-0.4，而且大体上都在1°内，大大提高了角度的输出精度。

　　5 结束语

　　本文所设计的轮式小车室内惯性导航装置，分析了该软件设计的各个模块的具体实现方法。经实验结果表明，该设计能够实时监测到移动机器人的位置信息，并对其能实现有效控制。同时其低成本、高精度、易操作的特点将进一步应用于例如巡逻机器人、救援机器人等专业领域，必将吸引国内外众多的投资商对其投资并进行更进一步的研发与应用，使其有着十分广大的创新创业前景、应用前景和市场前景。