一、加速度计

1.1 加速度计原理

1.2 加速度计校准

1.2.1 误差模型

1.2.2 关于MPU6050校准方式与代码思路

二、陀螺仪

2.1 陀螺仪原理

2.1.1 科里奥利力

2.1.2 陀螺仪原理

2.2 陀螺仪校准

三、加速度计与陀螺仪滤波部分

四、磁力计

4.1 磁力计原理

4.2 磁力计校准

4.2.1 干扰源

4.2.2 数值模型

4.2.3 最小二乘法找最优参数

4.2.4 简单一些的校准方式 - 包含代码思路

4.2.5 磁力计滤波

五、超声波传感器

5.1 超声波简介部分

5.2 数据采集和滤波部分

六、气压传感器

6.1 简单介绍 - 没必要看

七、激光测距传感器

7.1 简单介绍 - 没必要看

八、视觉传感器

8.1 光流传感器

8.1.1 光流传感器简单介绍

8.2 视觉里程计

8.2.1 摄像头部分

8.2.2 基于特征的稀疏法

一、加速度计

1.1 加速度计原理

首先说一下以前没有GNSS全球卫星定位系统时，导弹是用惯性测量元件获取信息；
比如加速度计、角速度计；
我们知道获取到了加速度，可以根据加速度对时间进行积分得到速度，对速度进行积分又可以得到路程；对角速度进行积分可以得到角度；

看上去确实很棒，但是惯性测量元件本身会有误差，在通过时间积分之后误差会累积...大多数情况下，成本越低...误差越大...也累积得越快...
在出现GNSS后，现代导弹是利用卫星和惯导同步获取信息的，等于说卫星定位系统来给惯导系统做补偿，来修正这个累积的误差值；

人体的惯导+GNSS

耳蜗中有液体，液体有惯性，运动时耳中神经可以感受到运动特征，所以把耳蜗比作惯导；
眼睛比作GNSS，但是工作方式不同；

那么我们睁着眼睛走路，耳蜗在感受运动特征时，即使出现误差值，可以利用眼睛作修正；
但是闭着眼睛走路，仅仅只有耳蜗作为定位，没有修正，误差值一直累积，导致人很难走直线；
耳蜗受损时，保持平衡的能力受损也类似；

1.2 加速度计校准

1.2.1 误差模型

这里的误差模型中，am'是原始数据，ba‘是零点偏移补偿，

首先，什么是零点偏移补偿？

因为芯片生产、装配有瑕疵，哪怕静止（理论加速度为 0），它也会输出一个错误的小数值，这时候需要ba’零点偏移补偿，来把这个误差抵消；

Sa是尺度因子（尺度比例补偿）

加速度计测量时，可能存在 “量程缩放不准” 的问题。比如真实加速度是 1g，理论该输出 1，但因为芯片 “放大 / 缩小” 了信号，实际输出 0.9（或 1.1）。Sa 就是给 x 轴 “修正比例”，让它从错误的 0.9→1（或 1.1→1 ）。乘以Sa后，得到 “消除零点偏移 + 修正尺度比例后的理论值” 。

安装 / 扭转误差补偿

加速度计芯片理论上 x 轴要和 “实际测量方向” 完全对齐，但实际安装时，可能歪了一点点（比如绕 y 轴转了个小角度），导致测出来的 x 轴数据 “混进了 y 轴的分量”。旋转矩阵Ra 就像 “把歪掉的坐标系，旋转回正确方向”，把混进的错误分量 “转回去”，得到 “消除所有误差后的真实加速度am” 。

说这些可能难懂，我们把加速度计想成个出厂时不准的尺子，
(1).首先零点飘了，零刻度线位置不对，这时候我们用ba’将其恢复到正确位置；
(2).接着，刻度不均匀，我们利用Sa重新刻刻度；
(3).尺子不够直，是歪的，我们利用Ra把尺子"掰正"；
这样使得加速度计这个“尺子”的误差被修正；

1.2.2 关于MPU6050校准方式与代码思路

使用MPU6050时，假如没有高精度平台情况下，可以考虑用下列简易的方法进行校准；
(1).把装了加速度计的飞控水平放置，这时候Z轴朝着正下方，XY轴的重力分量是0，Z轴测到加速度量为1g；
(2).这时候，XY轴上读到的数据就是零点飘移，Z轴的数据-1g也可以认为是Z轴的零点飘移；

下列代码中：
sum[7] 前三个是存加速度计ax/ay/az 原始数据累加的；
acc_sum_cnt 是统计加速度计采样次数，判断有没有达到校准所需要的多次采样；
/**********************************************************
* 函数原型:  void MPU6050_calibration()
* 功  能:   校准加速度计和陀螺仪
**********************************************************/
void MPU6050_calibration(void)
{static uint16_t acc_sum_cnt = 0, gyro_sum_cnt = 0;static int32_t sum[7] = {0,0,0,0,0,0};if(mpu6050.Acc_CALIBRATE){acc_sum_cnt++;sum[0] += mpu6050.ax_raw;sum[1] += mpu6050.ay_raw;sum[2] += mpu6050.az_raw - 4096; if(acc_sum_cnt >= MEMS_CALIB_AVG_NUM){mpu6050.Ax_offset = (float)sum[0]/MEMS_CALIB_AVG_NUM;mpu6050.Ay_offset = (float)sum[1]/MEMS_CALIB_AVG_NUM;mpu6050.Az_offset = (float)sum[2]/MEMS_CALIB_AVG_NUM;acc_sum_cnt = 0;if(mpu6050.Acc_CALIBRATE == 1){Param_SaveAccelOffset(mpu6050.Ax_offset, mpu6050.Ay_offset, mpu6050.Az_offset);}mpu6050.Acc_CALIBRATE = 0;sum[0] = sum[1] = sum[2] = 0;}}if(mpu6050.Gyro_CALIBRATE){gyro_sum_cnt++;sum[3] += mpu6050.gx_raw;sum[4] += mpu6050.gy_raw;sum[5] += mpu6050.gz_raw; if(gyro_sum_cnt >= MEMS_CALIB_AVG_NUM){mpu6050.Gx_offset = (float)sum[3]/MEMS_CALIB_AVG_NUM;mpu6050.Gy_offset = (float)sum[4]/MEMS_CALIB_AVG_NUM;mpu6050.Gz_offset = (float)sum[5]/MEMS_CALIB_AVG_NUM;gyro_sum_cnt = 0;if(mpu6050.Gyro_CALIBRATE == 1){Param_SaveGyroOffset(mpu6050.Gx_offset, mpu6050.Gy_offset, mpu6050.Gz_offset);}mpu6050.Gyro_CALIBRATE = 0;sum[3] = sum[4] = sum[5] = 0;}}
}

二、陀螺仪

2.1 陀螺仪原理

2.1.1 科里奥利力

用立创eda画的...先凑合着看...

这里我只是提一嘴，所以不够严谨，以理解个大概就行...需要深入的可以去查一下；

首先，假设有个圆盘，它不动的。那么盘上点A有个物体，受到F作用到点B直线运动，此时它运动路程也是直线的，很好理解吧...

现在，在次基础上，圆盘以ω逆时针运动，那么路径是下面这样的吧

我们可以引入一个虚拟力， $F_{c} =- 2m \omega \times v$ ，来表述另物体m改变运动轨迹的力...

2.1.2 陀螺仪原理

现在，假如有两个小方块，沿着x轴，同时做反向振动 - 你动我也动，方向相反，但是速度大小相同；根据刚刚上面说到的科里奥利力，会发现它们两个受到的科里奥利力相反，这一对相反的力会把两个小块往相反的方向拉，导致它们和固定电极的间距变化；从而导致电容差分变化；

说人话一点就是，陀螺仪里面两小玩意在那振动，你让这陀螺仪旋转了，里面电容变化了，然后这个电容变化被映射到角速度上面去，然后反馈回来给我们知道；

2.2 陀螺仪校准

这部分直接参考加速度计那里

三、加速度计与陀螺仪滤波部分

因为无人机电机旋转产生的振动容易引起传感器与加速度计的高频噪声；

所以在采集数据之后，可以试着采用平滑滤波，或者FIR、IIR滤波进行处理；
滤波算法 - 滑动平均滤波_滑动平均滤波算法原理-CSDN博客
这篇文章有我之前工作中用过的一种简单的滤波方法，直接复制粘贴就能用了；

四、磁力计

4.1 磁力计原理

利用磁致电阻AMR来检测空间中磁感应强度大小；
磁场的强弱变化会导致AMR阻值发生变化；

AMR磁阻效应：
(1).首先，磁场方向 / 强度变化时，阻值也跟着变化；
(2).制造AMR时，先用强磁预磁化，让材料内部形成一个主磁域，或者说是给它固定一个初始磁场方向；这时，材料有一个“敏感轴”，与主磁域垂直方向，磁场变化最敏感；
(3).根据资料显示，为了使测量结果以线性的方式变化，AMR材料上的金属导线是呈45°角排列；

(4).外界有磁场时，AMR主磁域方向会变化，导致磁场方向和电流夹角也变化，夹角的变化导致AMR阻值变化；
之后利用惠斯通电桥检测AMR阻值变化；
磁场变化的时候，假设R1R2增加a，R3R4减少a，电桥失衡，那么会出现一个微小电压被检测到；

4.2 磁力计校准

4.2.1 干扰源

(1).干扰源在机体外，这种一般来说比较远，能量小，不用怕可以忽略；但是也存在能量大的，比如高压线、金属矿产等异常磁场区，遇到这种就只能避免用磁力计推算航向了；
(2).干扰源在机身上，机载电子设备或者电机；
假如说，没有干扰的情况下，地球磁场矢量是均匀分布在磁力计X、Y、Z轴上，形成三维圆球；
那么，受到上述两种干扰后，这种圆球就会畸变成椭球形；

4.2.2 数值模型

(1).数值模型：
原始数据： $m'=[x_m ,y_m,z_m]^T$

校准后的数据： $m=[x_c,y_c,z_c]^T$

圆心偏移参数： $b'=[b_x,b_y,b_z]^T$

比例尺度参数:

矫正后的值满足下列公式：

这里校准本质是“还原”，原始数据先抵消偏移，再修正尺度，得到校准值；
在上面说过，理想状态（无干扰）下，数据应该是分布在圆球上面，那么 $x^2+y^2+z^2=R^2$

(2).开始引入误差：
但是，实际校准总是域理论会有些差别，这里引入误差u；

$u=x_c^2+y_c^2+z_c^2-R^2$
把 $x_c$ 、 $y_c$ 、 $z_c$ 再分别用上面的式子带入，
会得到：

然后转换成

则误差为：

这里是通过 “特征向量+系数向量”的定义，把二次函数拆解成线性组合，
u里面包含
二次项： $s_{mx}^2x_m^2$ 、 $s_{my}^2y_m^2$ 、 $s_{mz}^2z_m^2$
一次项： $2 x_m b_x s_{mx}^2$ 、 $2 y_m b_y s_{my}^2$ 、 $2 z_m b_z s_{mz}^2$
常数项： $b_x^2s_{mx}^2+b_y^2s_{my}^2+b_z^2s_{mz}^2-R^2$
我们先定义向量V，把所有xm、ym、zm的项按照顺序排进去；

然后利用向量P，把我们误差u里面的东西，都提取出来...

就完事了...

4.2.3 最小二乘法找最优参数

然后要开始对u求平方和，
为什么？因为误差u 表现出来的是“校准效果好不好”，那么我们要让所有采样点的误差平方和最小；
为了找最小值，对 U 中所有参数（a,b,c,d,e,f,g ）求偏导，并令偏导为 0，如下列式子

这里为了简化计算，定义
于是把那个很长的式子转成
但是因为是齐次线性方程组，解不唯一，所以

4.2.4 简单一些的校准方式 - 包含代码思路

(1).水平匀速旋转，收集X、Y数据；
(2).Z轴匀速旋转设备90°，收集Z数据；
(3).代码部分

MagMAX和MagMIN 是用于存储磁力计收集到的最大最小值；
MagSum是数据范围；

首先采样规定小于400，防止一些电磁干扰产生极端异常值；
然后更新最大最小值...
然后计算偏移量，这里用*0.5f，我印象里乘法比除法计算量小一些；

void HMC58831_CalOffset_Mag(void)
{static Vector3f_str MagMAX = { -100, -100, -100 };static Vector3f_str MagMIN = { 100, 100, 100 }, MagSum;static uint16_t cnt_m = 0;if (Mag_CALIBRATED){//采样规定小于400if (ABS(HMC58831.Mag_Adc.x) < 400 && ABS(HMC58831.Mag_Adc.y) < 400 && ABS(HMC58831.Mag_Adc.z) < 400){//求采样中的最大值MagMAX.x = MAX(HMC58831.Mag_Adc.x, MagMAX.x);MagMAX.y = MAX(HMC58831.Mag_Adc.y, MagMAX.y);MagMAX.z = MAX(HMC58831.Mag_Adc.z, MagMAX.z);//求采样中的最小值MagMIN.x = MIN(HMC58831.Mag_Adc.x, MagMIN.x);MagMIN.y = MIN(HMC58831.Mag_Adc.y, MagMIN.y);MagMIN.z = MIN(HMC58831.Mag_Adc.z, MagMIN.z);}if (cnt_m == CALIBRATING_MAG_CYCLES){    //利用最大最小值求偏移HMC58831.Mag_Offset.x = (int16_t)((MagMAX.x + MagMIN.x) * 0.5f);HMC58831.Mag_Offset.y = (int16_t)((MagMAX.y + MagMIN.y) * 0.5f);HMC58831.Mag_Offset.z = (int16_t)((MagMAX.z + MagMIN.z) * 0.5f);MagSum.x = MagMAX.x - MagMIN.x;MagSum.y = MagMAX.y - MagMIN.y;MagSum.z = MagMAX.z - MagMIN.z;//以x为基准进行比例缩放HMC58831.Mag_Gain.y = MagSum.x / MagSum.y;HMC58831.Mag_Gain.z = MagSum.x / MagSum.z;if (Mag_CALIBRATED){    //保存数据Param_SaveMagOffset(&HMC58831.Mag_Offset);}Mag_CALIBRATED = 0;cnt_m = 0;}cnt_m++;}
}

4.2.5 磁力计滤波

平滑滤波，参考加速度计滤波那里，我放了篇博客，里面有代码可以直接复制粘贴拿去用；

五、超声波传感器

5.1 超声波简介部分

首先为什么要用超声波？
我们虽然可以靠GPS获取位置数据，但是有障碍物的时候，卫星给到的信号有噪声。在室内甚至用不了GPS，所以需要利用超声波传感器进行高度检测；

超声波传感器原理？
首先超过20kHz的声波称为超声波，比如40kHz，这种声波波长短，方向性好，对固液体穿透力很大；超声波传感器利用发射器往一个方向发射超声波，这时候开始计时，超声波在空气中以声速(340m/s)传播，跑到杂质或者分界面返回，接收器接收到反射回来的超声波停止计时。那么
测量距离 = (计时器记录的时间*声速)/2 ，可以估算出障碍物距离；

盲区：
(1).检测盲区：超声波传感器在发射超声波时，发射头会有一段短暂的 “恢复期”，在这个时间段内，它无法准确识别接收到的反射波是来自近处物体还是发射头自身的干扰信号，所以无法检测位于超声波发射 / 接收器前段的部分物体，从而形成检测盲区。
例子：假设你在一个房间里使用超声波传感器来检测前方是否有障碍物。把超声波传感器放在桌子上，当一个很小的物体，比如一颗弹珠，放在距离传感器非常近（处于检测盲区）的地方时，传感器可能不会检测到弹珠的存在，就好像弹珠在传感器 “眼皮底下” 隐身了一样，仍然显示前方没有障碍物。

(2).角度引发的三角误差：超声波传感器通过发射超声波并接收反射波来计算距离，其原理基于直线传播。当被测物体与传感器成一定角度时，超声波实际传播的路径是一个斜边，而我们希望得到的是物体到传感器的垂直距离（直角边）。根据三角函数原理，斜边长度大于直角边，这就导致所测距离与实际距离存在偏差，即三角误差。
例子：想象在一个空旷的停车场，你用超声波传感器测量斜停在旁边的汽车的距离。传感器发射超声波，由于汽车是斜着的，超声波沿着斜线路径传播到汽车并反射回来。传感器计算出的距离是按照传播路径（斜边）来算的，而实际上我们想知道的是汽车与传感器的垂直距离，两者之间就会有误差。比如实际垂直距离是 3 米，但由于汽车斜停，传感器测出的距离可能是 3.5 米。

5.2 数据采集和滤波部分

这里其实我没写什么东西，

因为目前基本都是用超声波模块，不同的模块手册、说明书不同，实际上还是要根据厂家给到的说明书来应用采集；

滤波方面还是得做的，首先原始数据容易受到供电纹波等方面影响产生高频噪声，其次超声波受到温度、传输介质(这里一般指空气)变化也会产生影响；
这里简单点的也用平滑滤波得了...温度的画，挺多模块自带温度补偿功能；

六、气压传感器

6.1 简单介绍 - 没必要看

原理：气压传感器利用的是地球表面大气密度不相等（地表密度大，高层密度小）进行测量的；里面有个薄膜，接了个柔性电阻，气压升降的时候，薄膜带动顶针，电阻器阻止变化；

为什么有？ 有时候无人机飞行高度超过超声波传感器探测范围；

七、激光测距传感器

7.1 简单介绍 - 没必要看

D=ct
D表示与目标间的距离，
c表示光在空气中的传播速度(3*10^8m/s)，
t光往返一次的时间

八、视觉传感器

8.1 光流传感器

8.1.1 光流传感器简单介绍

原理：利用一个或者多个视觉传感器系统，在时间上采集图像序列，然后利用图像序列中的像素强度数据的时域变化和相关性来确定像素位置的“运动”，从而通过算法识别出图像中的物体相对于视觉传感器几何中心的运动信息。

我理解的是在时域上对图像进行采样，然后利用分辨图像上像素点不同时刻位置，与相关性来判断像素的“速度”；

光流计算方法：
(1).基于匹配的方法；
(2).频域方法；
(3).梯度方法；

8.2 视觉里程计

8.2.1 摄像头部分

从下图中可以知道，

这里 z 称为 p 点的深度值，除此之外，相机焦距光圈中心有不同参数，用矩阵C来表示；

这里 fx 和 fy 是x、y轴方向的焦距，cx 和 cy 是x、y轴方向的偏移；
然后，还得算上三维旋转(旋转矩阵R) 和三维位移(平移向量D)

8.2.2 基于特征的稀疏法

这种方法主要思想是把两张图片中具有代表性的点(特征点)提取出来，针对这些点进行空间位置估计、追踪运动情况；图像中非特征点都被丢弃；
相关的算法有：
(1).尺度不变特征变换 SIFT
(2).角点检测 Harris
(3).快速角点特征提取 FAST
(4).快速鲁棒特征提取 SURF

对于单目视觉系统来说，获取到的匹配点信息是二维图像(没包含深度)；
对于双目视觉系统来说，获取到的匹配点信息是三维图像；

SLAM系统中，视觉SLAM采用视觉里程计对周边环境建立点云的集合描述，通过大量空间点来估计周边地图的样貌；