Rflysim | 传感器标定与测量实验一
本讲是关于多旋翼的传感器，其中也包括测磁场的磁力计，主要讲解三轴加速度计、三轴陀螺仪、三轴磁力计、超声波测距模块、气压计、二维激光测距雷达、GPS模块、摄像机的基本原理、标定方法和测量模型，并对如何进行传感器标定以及传感器测量模型是什么做解答。
传感器标定与测量实验1
一. 飞控惯性测量单元简介
惯性测量单元（Inertial Measurement Unit， IMU）主要用来检测和测量加速度、倾斜、冲击、振动、旋转和多自由度运动。通常是指使用加速度计和陀螺仪来测量物体单轴、双轴或三轴姿态角（或角速率）以及加速度的装置。在飞控中的IMU系统一般包括三轴陀螺仪、三轴加速度计以及三轴磁力计，陀螺仪用来测量三轴的角速度，加速度计用来测量三轴的加速度，磁力计提供朝向信息。
二. 三轴加速度标定
2.1 三轴加速度计基本原理
三轴加速度计是一种惯性传感器，能够测量物体的比力，即去掉重力后的整体加速度或者单位质量上作用的非引力。当加速度计保持静止时，加速度计能够感知重力加速度，而整体加速度为零。在自由落体运动中，整体加速度就是重力加速度，但加速度计内部处于失重状态，而此时三轴加速度计输出为零。
三轴加速度计的原理能够用来测量角度，直观的如图所示。弹簧压缩量由加速度计与地面的角度决定，比力能够通过弹簧压缩长度来测量，因此在没有外力作用的情况下，加速度计能够精确地测量俯仰角和滚转角，且没有累积误差。
2.2 三轴加速度计误差模型
三轴加速度计会因为制造或安装误差，从而造成数据测量的不准确产生误差。因此需要对加速度计进行校正，目前，校正的方式有两种：一般校正：需要外部标定设备，麻烦但精确；自动校正：不需要外部标定设备，简单，精度略微差。本文将以自动校正为例进行阐述，自动校正前，需要提前建立标定前和标定后的三轴加速度的关系。误差模型如下式：
为了校正加速度计的测量值，我们需要估计下列未知参数：
通过化简可将其右边记为如下函数：
比力是在固定角度测量多次得到，因此可以忽略其测量噪声。具体地，加速度计以不同角度进行旋转，在每个固定角度下保持静止一段时间。记第k个时间间隔内测量得到的比力均值为标定前的加速度值，其标定原理是无论加速度计放置的角度如何变化，其比力的模长始终是常值，即当地重力加速度，记为g。根据这个原理，用以下优化方程来估计未知参数：
从而采取Levenberg-Marquardt算法得到最优解。具体推导过程请见文献[1]第7章内容。
2.3 MATLAB代码实现
1 %文件描述:
2 % 根据加速度计误差模型，使用lm优化算法对加速度计误差模型参数进行计算。
3 close all
4 clc
5 clear
6
7 load AccRaw %加载未标定加速度计参数
8 g = 9.8;
9 m = length(AccRaw);
10
11 y_dat = g*ones(m, 1); %期望重力加速度值
12 p0 = [1 1 1 0 0 0]’;
13 p_init = [1.0 1.0 1.0 0.1 0.1 0.1]’; %加速度计误差模型参数初始值
14
15 y_raw = calFunc(AccRaw, p0); %未标定时的重力加速度值
16 y_raw = y_raw(:);
17
18 r_raw = y_dat - y_raw; %未标定时加速度计测量的重力加速度与标准重力加速度的差
19 p_fit = lm(’calFunc’, p_init, AccRaw, y_dat);
20 y_lm = calFunc(AccRaw, p_fit); %标定后的重力加速度值
21 y_lm = y_lm(:);
22 r_lm = y_dat - y_lm;
23
24 kx = p_fit(1);
25 ky = p_fit(2);
26 kz = p_fit(3);
27 bx = p_fit(4);
28 by = p_fit(5);
29 bz = p_fit(6);
30
31 Ka9_8 = [kx 0 0; 0 ky 0; 0 0 kz]
32 ba9_8 = [bx by bz]’
33 save(’calP9_8’, ’Ka9_8’, ’ba9_8’)
在MATLAB中，运行完本文件，MATLAB将会弹出如下图像，其中第1行分别是：X、Y、Z轴的加速度计的采集数据和特征点，第2行分别是：LM算法的迭代次数、标定前后的对比图、标定前后的数据指标图。
MATLAB命令行中也将弹出，最终标定后的参数，如下图所示。
注：本实验对应demo文件对于RflySim v3.0以下版本地址为：*\PX4PSP\RflySimAPIs\Exp02_FlightControl\e3-SensorCalib；
对于RflySim v3.0及以上版本地址为：*\PX4PSP\RflySimAPIs\5.RflySimFlyCtrl\1.BasicExps\e3-SensorCalib。
2.4 三轴加速度计的测量模型
加速度计固联在机体轴上，测量的是三个机体轴方向上的比力，它可以表示为：
漂移量又可建模为如下的高斯随机游走过程：
注：由于大多数MEMS传感器均是以半导体材料作为基础加工的，而半导体对温度很敏感，再加上其他的一些安装，电路设计等影响因素，零点漂移和温度漂移是不可避免。
三. 重力加速度对加速度计标定影响分析
不同的海拔高度，对应就会产生不同的重力加速度值，根据如下公式：
其中Ta=I3。若重力加速度发生改变，例如g变成αg，α>0。在等式两边同乘以α可以得到如下关系式：
这就意味着在重力加速度改变后，根据优化方程可得到K*’’’’’’= αK*和ba*’=ba*。矫正步骤如下：
①：打开“calLM.m”文件，将重力加速度g的值由 9.8 改为 1。
1 %文件描述:
2 % 根据加速度计误差模型，使用lm优化算法对加速度计误差模型参数进行计算。
3 close all
4 clc
5 clear
6
7 load AccRaw %加载未标定加速度计参数
8 g = 1; % 此处改为1
9 m = length(AccRaw);
10
11 y_dat = g*ones(m, 1); %期望重力加速度值
12 p0 = [1 1 1 0 0 0]’;
13 p_init = [1.0 1.0 1.0 0.1 0.1 0.1]’; %加速度计误差模型参数初始值
14
15 y_raw = calFunc(AccRaw, p0); %未标定时的重力加速度值
16 y_raw = y_raw(:);
17
18 r_raw = y_dat - y_raw; %未标定时加速度计测量的重力加速度与标准重力加速度的差
19 p_fit = lm(’calFunc’, p_init, AccRaw, y_dat);
20 y_lm = calFunc(AccRaw, p_fit); %标定后的重力加速度值
21 y_lm = y_lm(:);
22 r_lm = y_dat - y_lm;
23
24 kx = p_fit(1);
25 ky = p_fit(2);
26 kz = p_fit(3);
27 bx = p_fit(4);
28 by = p_fit(5);
29 bz = p_fit(6);
30
31 Ka9_8 = [kx 0 0; 0 ky 0; 0 0 kz]
32 ba9_8 = [bx by bz]’
33 save(’calP9_8’, ’Ka9_8’, ’ba9_8’)
②：将在基础实验中获得的传感器数据复制到“calLM.m”文件所在目录下。
③：运行“calLM.m”文件，即可得到标定结果和曲线。校正参数为：
可以看到 g=1 时得到的标定参数ba′与g=9.8 时得到的标定参数ba相比几乎不变，但是 K*a′的值变得更小。K*’α的值应该缩小为αK*α的1/9.8，这与理论分析的结论一致。三轴加速度计固连在多旋翼机体上，其坐标轴与机体坐标轴一致。可以通过矫正后加速度计的值和公式计算得到g分别为9.8和1时的俯仰角。为了俯仰角在连续过程的变化情况，当f飞控被缓慢旋转时加速度计的数据被再次采集。因此可以发现加速度计计算得到的结果和参考的重力加速度的大小无关。
注：本实验对应demo文件对于RflySim v3.0以下版本地址为：*\PX4PSP\RflySimAPIs\Exp02_FlightControl\e3-SensorCalib；
对于RflySim v3.0及以上版本地址为：*\PX4PSP\RflySimAPIs\5.RflySimFlyCtrl\1.BasicExps\e3-SensorCalib。
参考文献：
[1] 全权,杜光勋,赵峙尧,戴训华,任锦瑞,邓恒译.多旋翼飞行器设计与控制[M],电子工业出版社,2018.
[2] 全权,戴训华,王帅.多旋翼飞行器设计与控制实践[M],电子工业出版社,2020.