Rflysim | 传感器标定与测量实验二

本讲是关于多旋翼的传感器，其中也包括测磁场的磁力计，主要讲解三轴加速度计、三轴陀螺仪、三轴磁力计、超声波测距模块、气压计、二维激光测距雷达、GPS模块、摄像机的基本原理、标定方法和测量模型，并对如何进行传感器标定以及传感器测量模型是什么做解答。

Rflysim平台| 传感器标定与测量实验2

1. 三轴磁力计标定

1.1. 三轴磁力计原理

磁力计是提供导航及基于位置服务的重要组成，一般利用各向异性磁致电阻或者霍尔效应来检测空间中的磁感应强度。此外，基于洛伦兹力的磁力计在不断研究和发展。基于洛伦兹力原理，电磁场会激发电磁力，进而己电路中的电容大小。

如上图所示，该磁力仪是采用三个互相垂直的磁阻传感器，每个轴向上的传感器检测在该方向上的地磁场强度。一种方式是采用具有晶体结构的合金材料。它们对外界的磁场很敏感，磁场的强弱变化会导致磁阻传感器电阻值发生变化。另外三轴磁力计还可以采用洛伦兹力原理，电流流过磁场产生力，从而驱动电容等变化。

图 洛伦兹力原理：磁场方向B表示由屏幕内指向外，q表示带电粒子

1.2. 三轴磁力计误差模型

三轴磁力计在实际生产过程中和安装过程中，总会出现一些偏差，不仅如此周边的原件（飞控芯片，供电电路，电机）也会对其产生影响。误差模型如下

为了标定磁力计，需要估计以下未知参数：

模型的右边可以记为如下函数：

这里忽略了测量噪声，因为磁感应强度是在同一位置测量多次得到。具体地，磁力计以不同角度旋转M次，在每个固定角度下保持静止一段时间。通常，无论磁力计位置放置的角度如何变化，磁感应强度都是常量。根据这个原理，可推导出：

从而采取Levenberg-Marquardt算法得到最优解。具体推导过程请见文献[1]第7章内容。

1.3. MATLAB代码实现

1 close all
2 clc
3 clear
4 load MagRaw.mat
5 CAL_MAG_SCALE = [1, 1, 1]’; %自驾仪中的标定值
6 CAL_MAG_OFF = [0.064, 0.014, -0.053]’;
7 MagRaw = (mag + CAL_MAG_OFF)./CAL_MAG_SCALE; %原始的磁力计数据
8
9 m = length(MagRaw);
10 MagSum = 0;
11 for k = 1 : m
12 MagSum = MagSum + norm(MagRaw(:, k));
13 end
14 MagAver = MagSum/m; %估计的磁场强度大小
15 Vdata = MagRaw/MagAver; %归一化
16
17 y_dat = ones(m, 1);
18 p0 = [1 1 1 0 0 0]’;
19 p_init = [1 1 1 0.01 0.01 0.01]’; %参数估计初始值
20
21 y_raw = calFunc(Vdata, p0); %未标定时的磁力计值
22 y_raw = y_raw(:);
23 r_raw = y_dat - y_raw;
24 p_fit = lm(’calFunc’, p_init, Vdata, y_dat, 0.001);
25 y_lm = calFunc(Vdata, p_fit); %标定后的磁力计值
26 y_lm = y_lm(:);
27 r_lm = y_dat - y_lm;
28 y_px4 = calFunc(mag/MagAver, p0); %px4标定过的磁力计值
29 y_px4 = y_px4(:);
30 r_px4 = y_dat - y_px4;
31
32 kx = p_fit(1);
33 ky = p_fit(2);
34 kz = p_fit(3);
35 bx = p_fit(4);
36 by = p_fit(5);
37 bz = p_fit(6);
38
39 Km = [kx 0 0;0 ky 0;0 0 kz]
40 bm = [bx by bz]’

运行calLM.m文件，即可弹出标定的结果如下图所示。其中图1表示利用LM法，迭代过程中适应度函数的变化；图 2 表示未标记数据、LM法标记数据、PX4标记数据的指标函数对比；图3 表示未标定和标定后的数据对比。

MATLAB命令行中也将弹出，最终标定后的参数，如下图所示。

注：本实验对应demo文件对于RflySim v3.0以下版本地址为：*\PX4PSP\RflySimAPIs\Exp02_FlightControl\e3-SensorCalib；对于RflySim v3.0及以上版本地址为：*\PX4PSP\RflySimAPIs\5.RflySimFlyCtrl\1.BasicExps\e3-SensorCalib。

1.4. 三轴磁力计的测量模型

三维磁力计固联在机体轴上，测量的是机体坐标系的三个方向上的磁场矢量。它们可以表示为：

进一步，漂移量bm又可建模为如下的高斯随机游走过程：

其中nbm示为高斯白噪声。

参考文献：
[1] 全权,杜光勋,赵峙尧,戴训华,任锦瑞,邓恒译.多旋翼飞行器设计与控制[M],电子工业出版社,2018.
[2] 全权,戴训华,王帅.多旋翼飞行器设计与控制实践[M],电子工业出版社,2020.

飞思实验室是北京卓翼智能科技有限公司旗下的无人智能教育及科研板块的品牌，主要致力于：无人智能体的集群和本体的教学科研平台的研发。经过多年的技术迭代，在无人智能体控制、集群、视觉等科研应用方向的研发、自动控制类课程教学及创新及实训类实验室的建设等领域处于行业领导地位，用户遍及全球各地。