姿态篇：四.非线性最小二乘与飞控传感器校准

[深入浅出多旋翼飞控开发][姿态篇][四][非线性最小二乘与飞控传感器校准]

Github
作者：Sky

前言

搞好了传感器，那意味着飞控已经完成了一半。

不用猜了，这句话正是鄙人说的:)。飞控的软硬件相关工作，绝大部分是围绕传感器展开的，它们的重要性，可能比你想象中的更大。

硬件上，为了寻找性能优秀又符合产品实际需求的传感器如陀螺仪、加速度计、磁力计等，我们经常需要对来自不同厂商的样品进行实际性能测试与评估，即使实际上常用的型号就那么几个。为了提升飞控的性能，许多传感器问题我们还需要从硬件上进行解决，如降低电源噪声、隔离机身震动甚至还要为传感器提供恒定温度的工作环境。

软件上，传感器校准、滤波与融合更是飞控程序中的重点，作为一名渣渣飞控工程师，浑浑噩噩工作数年，发现到头来大部分时间都是花在了传感器数据的处理上面，而且未来需要解决的问题，依然和传感器有关。

本篇文章将结合实际飞控代码，以循序渐进的方式向读者讲述，如何使用最优化算法，解决飞控中最基本的传感器校准问题。

飞控技术交流 562983648

一.传感器校准简述

飞控上会使用各种各样的传感器，但对于多旋翼而言，最核心的是陀螺仪无疑。离开了陀螺仪，你甚至无法让一架多旋翼飞行器正常离地。其次则是加速度计，提供姿态观测，让飞行器可以得到一个较为客观的自身姿态观测信息，使得自稳飞行成为可能（能够自动保持自身姿态水平或一定角度）。除此之外，还有许许多多的的其它传感器，共同为飞控提供稳定飞行所需要的数据。

其中，陀螺仪、加速度计和磁力计在飞行前（出厂前），均需要进行校准，才能达到正常工作所需要的性能。这便引出了一个问题：什么是传感器校准（标定）。

答案很简单，所有事物都不是非黑即白，而是时刻处于一个不可预知的混沌状态。工厂按照特定的技术及工艺标准制造生产出一批传感器，这些传感器中所有个体的实际状态处于一个随机分布且均值为期望误差的序列上。简单来说，我们买来10个传感器，这些传感器均存在着特定误差且这10个传感器的误差各不一致。这就要求我们在实际使用这些传感器时，需要对每一个进行单独的校准，得到特定的校准参数，从而正常发挥传感器的性能。

二.传感器误差类型

接下来我们会面临下面两个问题：

• 传感器有哪些误差
• 如何测量传感器的误差

如果从我们目前所使用的MEMS传感器的自身原理及制造工艺出发，分析传感器的误差来源，那可能需要长篇大论一番了，但那是超出作者能力的事情，也不是本篇文章所讨论的重点。

根据实际经验，我们可以把传感器误差分为两大类：

• 零偏误差（offset）
• 刻度误差（scale）

这两个是我们最容易理解，也是飞控中三大传感器均需要解决的误差类型，其中

零偏误差： 指传感器在零激励下的输出误差。如陀螺仪在静止状态下，其角速度测量值理论为0，而实际上我们将飞控静止放置，陀螺仪依然会输出诸如1°/s之类的数据，这便是该陀螺仪传感器的零偏误差。

刻度误差： 指传感器在一定激励输入下，其输出值与输入值的比值。比如我们将飞控放在一个以500°/s恒定角速度运动的转台上，而陀螺仪实际测量得到的角速度输出为495°/s，500 / 495 ≈ 1.01 便是该陀螺仪的刻度误差。

从上述说明中我们可以看出，要正确测量传感器的误差，必须向传感器施加特定的激励信号，通过比较传感器的实际输出与激励输入量，计算得到传感器的误差。

不同类型的传感器，所需要的激励信号源也不一样。如陀螺仪，要提供精确的激励信号，必须依靠专业的转台设备，这是个人及小公司均不具备的条件。好在对于大多数应用而言，陀螺仪的刻度误差在一个可以忍受的范围内，因此可被我们忽略。但对于精益求精的产品而言，要提升飞控性能到极致，刻度误差校准还是必须的，如某行业龙头，其飞控产品及整机产品，出厂前均需使用转台进行校准。缺少这些条件的个人爱好者，就只能暂时忽略这一项，只校准陀螺仪的零偏误差了，而大多数情况下，陀螺仪的零偏校准方式很简单，使用均值校准即可。因此，陀螺仪的校准不在本篇文章的讨论范围内。

而本文中讨论的重点：加速度计与磁力计，其信号激励源均来自于自然界，前者为地球的重力场，后者为地球的地磁场。

三.传感器校准的简单方式

1.加速度计

假设有下面的情形，我们将飞控水平放置于水平面，测得加速度计z轴输出值$g_1=0.97g$，然后将飞控翻转并同样水平置于水平面，再测得加速度计z轴输出值$g_2=-0.99g$。

设z轴零偏误差为$scal{e}_z$，刻度误差为$offse{t}_z$，已知实际重力加速度为g，可以得到下列两条等式：

• $(g_1-offse{t}_z)\ast scal{e}_z=g$
• $(g_2-offse{t}_z)\ast scal{e}_z=-g$

从而可以计算得到：

• $scal{e}_z\approx 1.02$
• $offse{t}_z=-0.01g$

将这个方法推广到其余两轴，便能得到其校准参数$scal{e}_x,scal{e}_y,offse{t}_x,offse{t}_y$。

缺陷：

这个校准方法较为简单，但是其可靠性建立于一个理想条件下：加速度计在采集每一个方向的数据时，必须严格贴合水平面。否则只能得到一个不太准确的校准数值。实际应用中，我们很难保证校准时让飞控的每一个面保持水平，因为飞行器本身就是一个不规则物体，更多时候我们采集到的加速度数据为$[0.05g,0.08g,0,95g]$，而非$[0,0,0.98g]$。或者说，这种方式的实现成本过高，难以实际应用。

2.磁力计

磁力计的简单校准方法原理等同于加速度计，即采集得到磁力计的三个轴的测量极值（最大与最小值），然后单独计算出每个轴的零偏误差与刻度误差。实现区别在于由于传感器激励信号源不一样，我们需要将磁力计在三维空间中进行各个方向的360°旋转，才能尽可能准确采集到极值。

缺陷：

实际应用中，磁力计校准对于飞行器使用者来说可能颇为频繁，而飞行器大小又各不一致，在很多情况下，我们是很难将飞行器翻来覆去地在各个方向上进行旋转，其可操作性非常低，且不方便普通用户的使用。如果旋转不全面，便无法采集到磁力计的真实极值，校准效果会大打折扣，甚至校准值严重偏离真实值。

四.建立传感器误差模型

对于程序员而言，解决实际问题的标准姿势如下：

提出实际问题—>建立数学模型—>转换为程序—>求解

而上一节中，我们描述了一个较为简单的用于加速度计与磁力计的校准方法，但是其前提条件较为苛刻，且校准效果不佳。于是，在这里我们提出了一个待解决的问题：找到一种使用便捷并且效果更好的校准算法。使用便捷，意味着约束条件越少越好，即在校准算法对传感器的采样点的数量和位置要求不能过于严格的情况下依然可以取得良好的校准效果。

为了解决这个问题，得到传感器的误差参数，首先我们需要建立传感器的误差模型，并将其转换为数学方程，从而进行求解。

以加速度计为例建立传感器误差模型

设三轴加速度计的零偏误差与刻度误差分别为
$$o_x,o_y,o_z,s_x,s_y,s_z$$
某一时刻该三轴加速度计的测量值分别为
$$x,y,z$$
设当前时刻真实加速度向量为
$$a=(a_x,a_y,a_z)$$
有以下关系：
$$\{\begin{array}{l}{a}_x=(x-o_x)s_x\\ a_y=(y-o_y)s_y\\ a_z=(z-o_z)s_z\end{array}$$

静止状态下，不管加速度计方向如何，其测量得到的加速度为重力加速度在各轴上的投影。而在地球上，重力加速度向量模值总是等于$1g$。假设$a_x,a_y,a_z$单位为$g$，便有以下等式成立：
$$a_x^2+a_y^2+a_z^2=1$$
即
$$(x-o_x{)}^2{s}_x^2+(y-o_y{)}^2{s}_y^2+(z-o_z{)}^2{s}_z^2=1$$

至此，我们将加速度计的误差求解转化成为了一个数学问题： 已知 $x,y,z$ ，由上一条等式，求解出 $o_x,o_y,o_z,s_x,s_y,s_z$

五.求解误差方程

上一节中，我们得到了传感器的误差方程：$(x-o_x{)}^2{s}_x^2+(y-o_y{)}^2{s}_y^2+(z-o_z{)}^2{s}_z^2=1$
求解方程，对于经历过九年义务教育的我们来说是一件非常熟悉的事情，但这里存在着几个棘手的问题：

• 该方程为六元二次方程，存在着6个未知数，如果只存在$x,y,$