传感器建模及校准含matlab代码

无人机或机器人中常用的传感器主要有：加速度计、陀螺仪、磁力计、气压计、测距仪（超声波或者激光）、GPS。

1、加速度计

加速度计测量的是扣除重力以外的其他力的和。故地面上加速度敏感到的力即为支持力，其方向向上，大小为g（9.80665）。

常用标定手段：
1、利用静止状态的加速度计向量的模值始终等于g，可以采取处于静止状态下各个方向的加速度计矢量，利用最小二乘获取上述图片中的9个变量。

2、将加速度各个轴分别向上、向下采集其数据，然后求解上述的模型参数。

• Reference vectors
• accel_corr_ref[6][3] = [ g 0 0 ] // nose up

•                    | -g  0  0 |     // nose down
  

•                    |  0  g  0 |     // left side down
  

•                    |  0 -g  0 |     // right side down
  

•                    |  0  0  g |     // on back
  

•                    [  0  0 -g ]     // level
  

• accel_raw_ref[6][3]
• accel_corr_ref[i] = accel_T * (accel_raw_ref[i] - accel_offs), i = 0…5
• 6 reference vectors * 3 axes = 18 equations
• 9 (accel_T) + 3 (accel_offs) = 12 unknown constants
• Find accel_offs
• accel_offs[i] = (accel_raw_ref[i2][i] + accel_raw_ref[i2+1][i]) / 2
• Find accel_T
• 9 unknown constants
• need 9 equations -> use 3 of 6 measurements -> 3 * 3 = 9 equations
• accel_corr_ref[i2] = accel_T * (accel_raw_ref[i2] - accel_offs), i = 0…2
• Solve separate system for each row of accel_T:
• accel_corr_ref[j2][i] = accel_T[i] * (accel_raw_ref[j2] - accel_offs), j = 0…2
• A * x = b
• x = [ accel_T[0][i] ]

• | accel_T[1][i] |
  

• [ accel_T[2][i] ]
  

• b = [ accel_corr_ref[0][i] ] // One measurement per side is enough

• | accel_corr_ref[2][i] |
  

• [ accel_corr_ref[4][i] ]
  

• a[i][j] = accel_raw_ref[i][j] - accel_offs[j], i = 0;2;4, j = 0…2
• Matrix A is common for all three systems:
• A = [ a[0][0] a[0][1] a[0][2] ]

• | a[2][0]  a[2][1]  a[2][2] |
  

• [ a[4][0]  a[4][1]  a[4][2] ]
  

• x = A^-1 * b
• accel_T = A^-1 * g

2、陀螺仪

陀螺仪主要用来敏感各个轴的角速度运动，其模型如下：

其精密校准方法同加速度计，首先利用转台获取其各个时刻的角度信息，然后利用单子样或双子样算法获取其解算的表达式，然后利用最小二乘得到其参数解。

但真实场景下我们常采用的是低精度MEMS，也很难有转台设备。通常下普通校准方法如下：
将陀螺仪处于静止状态时，其测量信息的均值及为零偏；将其旋转10圈，其解算得到的角度* 比例因子 = 10*360，即获取其估计的比例因子参数。安装角度信息则不考虑。

3、磁力计

根据采集的充足磁力计数据，将其拟合成球形或者椭球形。然后带入上述公式获取其对应系数。

4、气压计

气压计是根据采集到的气压，然后利用下述公式可以转换为高度信息。利用气压计进行高度估计时可以代入下述公式。

最低绝对零度$T_0=-{273.15}^{\circ }$，海平面温度$T_s={15}^{\circ }$，温度高度梯度$a={\frac{-6.5}{1000}}^{\circ }/m$，海平面气压为$p_1$ kPa，当前气压计测量的气压为$p$kPa，重力加速度为$g$ m/s2，气体常数$R=287.1(J/kg.K)$。$T_s$换至开氏温度$T_1$：$T_1=T_s-T_0$

$$h=\left(T_1{\left(\frac{p}{p_1}\right)}^{-aR/g}-T_1\right)/a$$

$T_s$摄氏温度下的大气密度计算公式如下：

$$\rho =\frac{p}{R(T_s-T_0)}$$

5、空速管

空速管主要是获取当前的动压，利用动压数据可以获取当前的指示空速和真实空速。还可以利用该数据结合气动数据在线估计当前的风速。

空速计温度为摄氏度$T$，压差为$dP $kPa，15摄氏度的海平面参考大气密度${{\rho }_{r}}=1.225Kg/{{m}^{3}}$。其转换至指示空速公式如下：

$$IAS=sign(dp)\sqrt{2\ast dp/{\rho }_r}$$

真实空速计算如下：

$$\rho =\frac{p}{R(T-T_0)}$$

$$TAS=IAS*\sqrt{\frac{{{\rho }_{r}}}{\rho }}$$

6、GPS

做紧耦合定位时可以利用上述公式作为测量方程。

7、matlab 编程

上述各个传感器模型在matlab中均可以找到，其加速度计、磁力计和陀螺仪封装在imuSensor，GPS传感器模型则集成在gpsSensor。

其imu解算主要有单子样（毕卡）、双子样和四阶龙格库塔法。

关注公众号，获取惯导解算代码、传感器模型代码以及imu的allan方差分析代码

❤️ 扫一扫，添加我的公众号或者搜索【无人机开发】