多旋翼飞行器设计与控制（七）—— 传感器标定和测量模型
一、三轴加速度计
三轴加速度计是一种惯性传感器，能够测量物体的比力，即去掉重力后的整体加速度或者单位质量上作用的非引力。当加速度计保持静止时，加速度计能够感知重力加速度，而整体加速度为零。在自由落体运动中，整体加速度就是重力加速度，但加速度计内部处于失重状态，而此时三轴加速度计输出为零。
三轴加速度计的原理能够用来测量角度。直观地，如图所示，弹簧压缩量由加速度计与地面的角度决定。比力能够通过弹簧压缩长度来测量。因此在没有外力作用的情况下，加速度计能够精确地测量俯仰角和滚转角，且没有累积误差。
MEMS三轴加速度计是采用压阻式、压电式和电容式工作原理，产生的比力（压力或者位移）分别正比于电阻、电压和电容的变化。这些变化可以通过相应的放大和滤波电路进行采集。该传感器的缺点是受振动影响较大。
三轴加速度计在实际生产过程中和安装过程中，总会出现一些偏差。因此，需要建立标定前和标定后的三轴加速度的关系。误差模型如下：
为了校正加速度计的测量值，我们需要估计下列未知参数
根据 “无论加速度计放置的角度如何变化，其比力的模长始终是常值，即当地重力加速度 我们有：
根据这条公式我们便可以对加速度计进行自动校正。
由于大多数MEMS传感器均是以半导体材料作为基础加工的，而半导体对温度很敏感，再加上其他的一些安装，电路设计等影响因素，零点漂移和温度漂移是不可避免。
三轴加速度计的测量模型如下：
二、三轴陀螺仪
当一个质点相对于惯性系做直线运动时，因为质点自身惯性，它相对于旋转体系，其轨迹是一条曲线。立足于旋转体系，我们认为有一个力驱使质点运动轨迹形成曲线。科氏力就是对这种偏移的一种描述。
在陀螺仪中，两个质量块运动速度方向相反，而大小相同。它们产生的科氏力相反，从而压迫两块对应的电容板移动，产生电容差分变化。电容的变化正比于旋转角速度。这就是基本原理。 纯加速度并不能使陀螺仪产生读数。这是因为纯加速度变化只能导致两个质量块朝相同方向移动，不会带来电容的差分变化。
三轴陀螺仪在实际生产过程中和安装过程中，总会出现一些偏差。因此，需要建立标定前和标定后的三轴角速度的关系。误差模型如下：
根据 “角速度积分得到的角度与标定过的加速度计估计得到的角度进行比较，来标定未知参数” ，定义如下操作：
这个实现可以利用四元数变化率与机体角速度的关系模型
并用龙格库塔法积分实现，得到递推后得到的加速度：
校正时我们希望通过陀螺仪积分得到角度与加速度测量的角度尽量相同：
其中函数hg的作用是根据上一次的比力和角速度信息来估计当前比力。其标定原理进一步表示为：期望估计值 尽可能地与标定后的加速度计测量值 接近。根据这个原理，给出以下优化问题:
三轴陀螺仪的测量模型如下：
三、三轴磁力计
该磁力仪是采用三个互相垂直的磁阻传感器，每个轴向上的传感器检测在该方向上的地磁场强度。一种方式是采用具有晶体结构的合金材料。它们对外界的磁场很敏感，磁场的强弱变化会导致磁阻传感器电阻值发生变化。
误差模型如下：
校正方法如下：
测量模型如下：
超声波测距仪
超声波是指振动频率大于20kHz的声波，其每秒的振动次数很高，超出了人耳听觉的上限，被称为超声波。它方向性好，穿透能力强，广泛应用于测距和测速等方面。超声波测距仪也存在一些不足。首先，它的测量范围小。其次，如图所示，柔软的物体或者与传感器处于特定角度的物体可能反射的声波较少，甚至没有反射波。
超声波传感器一般用于测量相对高度，位于多旋翼正下方。如果传感器测得距离为d，那么飞行器离地高度为：
气压计
多旋翼多采用压电式气压计，气压计也是一种压力传感器，新一代的气压计既可以测量气压和高度，又可以测量温度。大气压随高度的增加而减小，气压计正是通过测量大气压来估计高度。因为大气压分布不是均匀的而且气压计对气流的影响很敏感（有风情况测量不准），因此气压计只能得到飞行高度的近似值。
测量模型如下：
二维激光测距雷达
二维激光测距仪是利用时间飞行原理来测量距离的一种仪器，激光扫描测量系统基于激光测距原理。通过旋转的光学部件发射形成二维的扫描面，以实现区域扫描及轮廓测量功能。二维激光测距仪可以实现360度一定范围内的激光测距扫描，产生所在空间的平面点云地图信息用于地图测绘、机器人定位导航、物体/环境建模等应用。因为激光扫描测距仪一般用于测高或者避障，它们微小偏差不会对飞行器造成很大的性能下降。因此，一般可以认为出产的传感器已经足够精确。
集LiDAR、GPS和IMU为一体的设备可以生成点云图。点云图数据中含有空间三维信息和激光强度信息。应用分类技术在这些原始数字表面模型中移除建筑物、人造物、覆盖植物等测点，即可获得数字高程模型，并同时得到地面覆盖物的高度。
GPS模块
全球定位系统由许多卫星组成，位置已知。基本原理是测GPS接收机到卫星的距离，然后通过解方程确定GPS接收机的位置。对C/A码测得的伪距称为C/A码伪距，精度约为20米左右，对P码测得的伪距称为P码伪距，精度约为2米左右。考虑电离层、对流层和钟差影响，伪距定位基本观测方程为：
接收机钟差是未知数，其产生是因为GPS 接收机一般采用高精度的石英钟,接收机的钟面时与GPS 标准时之间的差异称为接收机钟差。
定位原理上，接收机接收到它到卫星的距离，通过解方程得知自身位置。
该式子中接收机钟误差和接收机位置坐标均为未知数，因为未知数有4个，因此理论上至少需要4颗卫星才能够定位。
介绍一种更高性能的定位方式。差分GPS(DGPS)主要是通过消除误差公共项来改善定位性能。DGPS系统由基准站、数据链和用户三部分构成，它要求有高质量的GPS基准接收机放在已知坐标的基准站上，基准站估算每个卫星的测距误差分量，并对每颗卫星可视范围内的卫星形成校正值，将该校正值或者原始观测值，通过数据链发送给所有的DGPS用户。
GPS定位系统测量模型如下：
摄像机
在机器视觉中，摄像机通过成像透镜将三维场景投影到
二维像平面的投影关系即为成像模型，摄像机成像模型将三维空间中的点与图像平面中的点对应起来。为了研究的方便，空间中任一点P在图像中的成像位置可以用针孔成像模型近似表示，这种关系也称为中心摄影或透视投影模型。
该部分涉及四个坐标系，地球固联坐标系、摄像机坐标系、图像坐标系、像素坐标系，我们需要将地球固联坐标系中的向量变换到像素坐标系中。
地球固联坐标系到摄像机坐标系的变换如下：
摄像机坐标系到像素坐标系的变换如下：
最终摄像机的测量模型如图：
