多旋翼飞行器设计与控制（七）—— 传感器标定和测量模型

一、三轴加速度计

三轴加速度计是一种惯性传感器，能够测量物体的比力，即去掉重力后的整体加速度或者单位质量上作用的非引力。当加速度计保持静止时，加速度计能够感知重力加速度，而整体加速度为零。在自由落体运动中，整体加速度就是重力加速度，但加速度计内部处于失重状态，而此时三轴加速度计输出为零。

三轴加速度计的原理能够用来测量角度。直观地，如图所示，弹簧压缩量由加速度计与地面的角度决定。比力能够通过弹簧压缩长度来测量。因此在没有外力作用的情况下，加速度计能够精确地测量俯仰角和滚转角，且没有累积误差。

MEMS三轴加速度计是采用压阻式、压电式和电容式工作原理，产生的比力（压力或者位移）分别正比于电阻、电压和电容的变化。这些变化可以通过相应的放大和滤波电路进行采集。该传感器的缺点是受振动影响较大。

三轴加速度计在实际生产过程中和安装过程中，总会出现一些偏差。因此，需要建立标定前和标定后的三轴加速度的关系。误差模型如下：

bam=TaKa(bam′+ba′){^b}a_m = T_a K_a({^b}a_m \prime + b_a \prime) bam=TaKa(bam′+ba′)

Ta=[1ΔΨa−Δθa−ΔΨa1ΔϕaΔθa−Δϕa1]T_a = \begin{bmatrix} 1 & \Delta \Psi _a & -\Delta \theta_a\\ - \Delta \Psi_a & 1 & \Delta \phi_a \\ \Delta \theta_a & - \Delta \phi_a&1 \end{bmatrix} Ta=⎣⎡1−ΔΨaΔθaΔΨa1−Δϕa−ΔθaΔϕa1⎦⎤

Ka=[sax000say000saz]K_a = \begin{bmatrix} s_{ax} & 0 & 0\\ 0 & s_{ay} & 0 \\ 0 & 0 & s_{az} \end{bmatrix} Ka=⎣⎡sax000say000saz⎦⎤

ba′=[bax′bay′baz′]b_a \prime = \begin{bmatrix} b_{ax} \prime\\ b_{ay} \prime \\ b_{az} \prime \end{bmatrix} ba′=⎣⎡bax′bay′baz′⎦⎤

其中bam′为标定前三轴加速度ba′为偏移其中{^b}a_m \prime 为标定前三轴加速度 b_a \prime 为偏移其中bam′为标定前三轴加速度ba′为偏移

为了校正加速度计的测量值，我们需要估计下列未知参数

Θa=[ΔΨaΔθaΔϕasaxsaysazbax′bay′baz′]T\Theta_a = [\Delta \Psi_a ~~\Delta \theta_a ~~ \Delta \phi_a ~~s_{ax} ~~ s_{ay} ~~ s_{az} ~~ b_{ax} \prime ~~ b_{ay} \prime ~~ b_{az} \prime]^T Θa=[ΔΨa Δθa Δϕa sax say saz bax′ bay′ baz′]T

根据 “无论加速度计放置的角度如何变化，其比力的模长始终是常值，即当地重力加速度 我们有：

Θa∗=argmin∑k=1M(∣∣ha(Θa,bam,k′)∣∣−g)2\Theta ^*_a = argmin \sum_{k=1}^M (||h_a(\Theta_a , {^b} a_{m,k} \prime )||-g)^2 Θa∗=argmink=1∑M(∣∣ha(Θa,bam,k′)∣∣−g)2

根据这条公式我们便可以对加速度计进行自动校正。

由于大多数MEMS传感器均是以半导体材料作为基础加工的，而半导体对温度很敏感，再加上其他的一些安装，电路设计等影响因素，零点漂移和温度漂移是不可避免。

三轴加速度计的测量模型如下：

bam=ba+ba+na{^b}a_m = {^b}a +b_a +n_a bam=ba+ba+na

其中bam为校正后测量的比力，ba为真实加速度其中{^b}a_m 为校正后测量的比力， {^b}a为真实加速度其中bam为校正后测量的比力，ba为真实加速度

ba为漂移，na为高斯白噪声b_a为漂移，n_a为高斯白噪声 ba为漂移，na为高斯白噪声

漂移量ba又可以建模为高斯随机游走过程：ba′=nba漂移量b_a又可以建模为高斯随机游走过程：b_a \prime = n_{b_a} 漂移量ba又可以建模为高斯随机游走过程：ba′=nba

二、三轴陀螺仪

当一个质点相对于惯性系做直线运动时，因为质点自身惯性，它相对于旋转体系，其轨迹是一条曲线。立足于旋转体系，我们认为有一个力驱使质点运动轨迹形成曲线。科氏力就是对这种偏移的一种描述。

在陀螺仪中，两个质量块运动速度方向相反，而大小相同。它们产生的科氏力相反，从而压迫两块对应的电容板移动，产生电容差分变化。电容的变化正比于旋转角速度。这就是基本原理。纯加速度并不能使陀螺仪产生读数。这是因为纯加速度变化只能导致两个质量块朝相同方向移动，不会带来电容的差分变化。

三轴陀螺仪在实际生产过程中和安装过程中，总会出现一些偏差。因此，需要建立标定前和标定后的三轴角速度的关系。误差模型如下：

bωm=TgKg(bωm′+bg′){^b}\omega_m = T_g K_g({^b}\omega_m \prime + b_g \prime) bωm=TgKg(bωm′+bg′)

Tg=[1ΔΨg−Δθg−ΔΨg1ΔϕgΔθg−Δϕg1]T_g = \begin{bmatrix} 1 & \Delta \Psi _g & -\Delta \theta_g\\ - \Delta \Psi_g & 1 & \Delta \phi_g \\ \Delta \theta_g & - \Delta \phi_g&1 \end{bmatrix} Tg=⎣⎡1−ΔΨgΔθgΔΨg1−Δϕg−ΔθgΔϕg1⎦⎤

Kg=[sag000sag000sag]K_g = \begin{bmatrix} s_{ag} & 0 & 0\\ 0 & s_{ag} & 0 \\ 0 & 0 & s_{ag} \end{bmatrix} Kg=⎣⎡sag000sag000sag⎦⎤

bg′=[bgx′bgy′bgz′]b_g \prime = \begin{bmatrix} b_{gx} \prime\\ b_{gy} \prime \\ b_{gz} \prime \end{bmatrix} bg′=⎣⎡bgx′bgy′bgz′⎦⎤

根据 “角速度积分得到的角度与标定过的加速度计估计得到的角度进行比较，来标定未知参数” ，定义如下操作：

这个实现可以利用四元数变化率与机体角速度的关系模型

qeb′(t)=12[0−bωTbω−[bω]x]qeb(t)q^b_e \prime (t) = \frac{1}{2} \begin{bmatrix} 0&-{^b} \omega^T\\{^b} \omega & -[{^b} \omega]_x \end{bmatrix} q^b_e (t) qeb′(t)=21[0bω−bωT−[bω]x]qeb(t)

并用龙格库塔法积分实现，得到递推后得到的加速度：

校正时我们希望通过陀螺仪积分得到角度与加速度测量的角度尽量相同：

其中函数hg的作用是根据上一次的比力和角速度信息来估计当前比力。其标定原理进一步表示为：期望估计值尽可能地与标定后的加速度计测量值接近。根据这个原理，给出以下优化问题:

三轴陀螺仪的测量模型如下：

bωm=bω+bg+ng{^b}\omega_m = {^b}\omega +b_g +n_g bωm=bω+bg+ng

其中bωm为校正后测量的角速度，bω为真实角速度其中{^b}\omega_m 为校正后测量的角速度， {^b}\omega为真实角速度其中bωm为校正后测量的角速度，bω为真实角速度

bg为漂移，ng为高斯白噪声b_g为漂移，n_g为高斯白噪声 bg为漂移，ng为高斯白噪声

漂移量bg又可以建模为高斯随机游走过程：bg′=nbg漂移量b_g又可以建模为高斯随机游走过程：b_g \prime = n_{b_g} 漂移量bg又可以建模为高斯随机游走过程：bg′=nbg

三、三轴磁力计

该磁力仪是采用三个互相垂直的磁阻传感器，每个轴向上的传感器检测在该方向上的地磁场强度。一种方式是采用具有晶体结构的合金材料。它们对外界的磁场很敏感，磁场的强弱变化会导致磁阻传感器电阻值发生变化。

误差模型如下：

校正方法如下：

测量模型如下：

超声波测距仪

超声波是指振动频率大于20kHz的声波，其每秒的振动次数很高，超出了人耳听觉的上限，被称为超声波。它方向性好，穿透能力强，广泛应用于测距和测速等方面。超声波测距仪也存在一些不足。首先，它的测量范围小。其次，如图所示，柔软的物体或者与传感器处于特定角度的物体可能反射的声波较少，甚至没有反射波。

超声波传感器一般用于测量相对高度，位于多旋翼正下方。如果传感器测得距离为d，那么飞行器离地高度为：

dSONAR=−1cosθcosϕ+ndSONARd_{SONAR} = - \frac{1}{cos\theta cos\phi} + n_{d_{SONAR}} dSONAR=−cosθcosϕ1+ndSONAR

其中θ为俯仰角，ϕ为滚转角，ndSONAR为高斯白噪声其中\theta 为俯仰角，\phi为滚转角，n_{d_{SONAR}}为高斯白噪声其中θ为俯仰角，ϕ为滚转角，ndSONAR为高斯白噪声

气压计

多旋翼多采用压电式气压计，气压计也是一种压力传感器，新一代的气压计既可以测量气压和高度，又可以测量温度。大气压随高度的增加而减小，气压计正是通过测量大气压来估计高度。因为大气压分布不是均匀的而且气压计对气流的影响很敏感（有风情况测量不准），因此气压计只能得到飞行高度的近似值。

测量模型如下：

dBARO=−pze+bdBARO+ndBAROd_{BARO} = -p_{z_e} + b_{d_{BARO}} + n_{d_{BARO}} dBARO=−pze+bdBARO+ndBARO

dBARO为测量高度，pze为真实高度d_{BARO}为测量高度，p_{z_e}为真实高度 dBARO为测量高度，pze为真实高度
bdBARO为漂移，ndBARO为白噪声b_{d_{BARO}}为漂移，n_{d_{BARO}}为白噪声 bdBARO为漂移，ndBARO为白噪声

漂移量又可建模为如下的高斯随机游走过程：bdBARO′=ndBARO漂移量又可建模为如下的高斯随机游走过程： b_{d_{BARO}} \prime = n_{d_{BARO}}漂移量又可建模为如下的高斯随机游走过程：bdBARO′=ndBARO

二维激光测距雷达

二维激光测距仪是利用时间飞行原理来测量距离的一种仪器，激光扫描测量系统基于激光测距原理。通过旋转的光学部件发射形成二维的扫描面，以实现区域扫描及轮廓测量功能。二维激光测距仪可以实现360度一定范围内的激光测距扫描，产生所在空间的平面点云地图信息用于地图测绘、机器人定位导航、物体/环境建模等应用。因为激光扫描测距仪一般用于测高或者避障，它们微小偏差不会对飞行器造成很大的性能下降。因此，一般可以认为出产的传感器已经足够精确。

集LiDAR、GPS和IMU为一体的设备可以生成点云图。点云图数据中含有空间三维信息和激光强度信息。应用分类技术在这些原始数字表面模型中移除建筑物、人造物、覆盖植物等测点，即可获得数字高程模型，并同时得到地面覆盖物的高度。

GPS模块

全球定位系统由许多卫星组成，位置已知。基本原理是测GPS接收机到卫星的距离，然后通过解方程确定GPS接收机的位置。对C/A码测得的伪距称为C/A码伪距，精度约为20米左右，对P码测得的伪距称为P码伪距，精度约为2米左右。考虑电离层、对流层和钟差影响，伪距定位基本观测方程为：

ρ=ρ′+c(δt+δT)+δI\rho = \rho \prime + c(\delta_t + \delta_T) + \delta_I ρ=ρ′+c(δt+δT)+δI

其中ρ为真实距离，ρ′为伪距，c为光速δI为延迟改正数其中\rho为真实距离，\rho \prime为伪距，c为光速 \delta_I为延迟改正数其中ρ为真实距离，ρ′为伪距，c为光速δI为延迟改正数

δt为卫星钟误差改正数，δT为接收机时钟相对于GPS时间的误差改正数\delta_t为卫星钟误差改正数，\delta_T为接收机时钟相对于GPS时间的误差改正数 δt为卫星钟误差改正数，δT为接收机时钟相对于GPS时间的误差改正数

接收机钟差是未知数，其产生是因为GPS 接收机一般采用高精度的石英钟,接收机的钟面时与GPS 标准时之间的差异称为接收机钟差。

定位原理上，接收机接收到它到卫星的距离，通过解方程得知自身位置。

令卫星坐标为ps,k,k=1,...，ns,ns为卫星个数令卫星坐标为p_{s,k},k=1,...，n_s,n_s为卫星个数令卫星坐标为ps,k,k=1,...，ns,ns为卫星个数

用户接收机的位置坐标为pr,可以得到用户接收机的位置坐标为p_r ,可以得到用户接收机的位置坐标为pr,可以得到

∣∣ps,k−pr∣∣−δI=ρ′+c(δt,k+δT),k=1,...,ns||p_{s,k}-p_r|| - \delta_I = \rho \prime + c(\delta_{t,k}+\delta_T),k=1,...,n_s ∣∣ps,k−pr∣∣−δI=ρ′+c(δt,k+δT),k=1,...,ns

该式子中接收机钟误差和接收机位置坐标均为未知数，因为未知数有4个，因此理论上至少需要4颗卫星才能够定位。

介绍一种更高性能的定位方式。差分GPS(DGPS)主要是通过消除误差公共项来改善定位性能。DGPS系统由基准站、数据链和用户三部分构成，它要求有高质量的GPS基准接收机放在已知坐标的基准站上，基准站估算每个卫星的测距误差分量，并对每颗卫星可视范围内的卫星形成校正值，将该校正值或者原始观测值，通过数据链发送给所有的DGPS用户。

GPS定位系统测量模型如下：

摄像机

在机器视觉中，摄像机通过成像透镜将三维场景投影到
二维像平面的投影关系即为成像模型，摄像机成像模型将三维空间中的点与图像平面中的点对应起来。为了研究的方便，空间中任一点P在图像中的成像位置可以用针孔成像模型近似表示，这种关系也称为中心摄影或透视投影模型。

该部分涉及四个坐标系，地球固联坐标系、摄像机坐标系、图像坐标系、像素坐标系，我们需要将地球固联坐标系中的向量变换到像素坐标系中。

地球固联坐标系到摄像机坐标系的变换如下：

摄像机坐标系到像素坐标系的变换如下：

最终摄像机的测量模型如图：