⽆⼈机飞控三⼤算法
⽆⼈机飞控三⼤算法：捷联式惯性导航系统、卡尔曼滤波算法、飞⾏控制PID算法。 ⼀、捷联式惯性导航系统咖啡玉米
说到导航，不得不说GPS，他是接受卫星发送的信号计算出⾃⾝位置的，但是当GPS设备上⽅被遮挡后，GPS设备⽆法定位了。⽐如在室内、隧道内、地下等场所，基本收不到GPS信号。 语录：任何⼀款有缺点的产品，必然成就了另⼀款能克服其缺点的产品。
另⼀种导航⽅式是不依赖外界信息的，这种导航叫做惯性导航。 那什么是惯性导航呢？他就是利⽤载体上的加速度计、陀螺仪这两种惯性远见，去分别测出飞⾏器的⾓
运动信息和线运动信息，与初始姿态、初始航向、初始位置⼀起交给计算模块，由计算模块推算出飞机的姿态、速度、航向、位置等导航参数的⾃主式导航⽅法。
（精益求精：陀螺仪是怎么通过⾓运动信息再经过计算模块计算得出姿态的呢？同理加速度计⼜是怎样通过线运动信息再经过计算模块计算出速度的呢？）
另外惯性导航系统分为平台式惯性导航和捷联式惯性导航。
大蒜剥皮机惯性导航系统分为平台式惯性导航和捷联式惯性导航。
乳化液废水处理早期的惯性导航系统都是平台式的，平台式惯导有实体的物理平台，陀螺仪和加速度计置于由陀螺稳定的平台上，该平台跟踪导航坐标系，以实现速度和位置解算，姿态数据直接取⾃于平台的环架。
优点：直接模拟导航坐标系，计算⽐较简单；能隔离载体的⾓运动，系统精度⾼。
缺点：结构复杂，体积⼤，制作成本⾼。
还有另⼀种捷联式惯性导航，捷联的英⽂原义是“捆绑”的意思。因此捷联式惯性导航也就是将惯性测量元件，包括陀螺仪和加速度计，直接装在需要姿态、速度、航向等导航信息的主体上，⽤计算机的测量信号变换为导航参数。
优点是没有平台，架构简单，体积⼩，维护⽅便。
缺点：惯性元件直接装在载体上，环境恶劣，对元件要求较⾼；坐标变换中计算量⼤。
总体来看，捷联惯导⽐平台式惯导优势明显。
在1969年，捷联惯导系统作为""-13号登⽉飞船的应急备份装置，在其服务舱发⽣爆炸时将飞船成功地引导到返回地球的轨道上时起到了决定性作⽤，成为捷联式惯导系统发展中的⼀个⾥程碑
⼆、卡尔曼滤波算法
卡尔曼滤波算法采⽤信号与噪声的状态空间模型，利⽤前⼀时刻地估计值和现时刻地观测值来更新对状态变量的估计，求出现刻的估计值。
卡尔曼滤波算法是卡尔曼等 ⼈在20世纪60年代提出的⼀种递推滤波算法。它的实质是以最⼩均⽅误差为估计的最佳准则，来寻求⼀套递推估计的算法。这套算法采⽤信号与噪声的状态空间模型，利⽤前⼀时刻地估计值和现时刻的观测值来更新对状态变量的估计，求出现时刻的估计值，在惯性导航系统中有⾮常⼴泛的应⽤。刚才说的噪声指的是计算得出的值与实际值的误差。
那么为什么Kalman滤波会应⽤到惯性导航系统中呢？这主要是因为惯性导航系统的“纯惯性”传感器不⾜以达到所需的导航精度，为了补偿导航系统的不⾜，常常使⽤其他导航设备来提⾼导航精度，以减⼩导航误差。所以利⽤Kalman滤波算法，可以将来⾃惯性导航系统与其他导航装置的数据（如惯性导航系统计算的位置对照GPS接收机给出的位置信息）加以混合利⽤，估计和校正未知的惯性导航系统误差。
卡尔曼滤波算法⼴泛应⽤已经超过30年，包括机器⼈导航，控制， 传感器数据融合甚⾄军事⽅⾯的雷达系统以及导弹追踪等等。
⽐如，在雷达中，⼈们感兴趣的是跟踪⽬标，但⽬标的位置、速度、加速度的测量值往往在任何时候都有噪声。卡尔曼滤波利⽤⽬标的动态信息，设法去掉噪声的影响，得到⼀个关于⽬标位置最优的估计。这个估计可以是对当前⽬标位置的估计(滤波)，也可以是对于将来位置的估计(预测)，也可以是对过去位置的估计(插值或平滑)。
卡尔曼滤波算法是⼀个⾮常复杂的计算，我们结合飞⾏器来简单的讲⼀下它的计算过程，⽐如飞⾏器想知道⾃⼰的⼀个状态，这个状态可以是姿态、速度或位置等信息，我们知道飞⾏器的传感器是可以得到这些信息的，通过惯性导航的数学模型也可以计算出这些信息，但这两个信息的值与实际值还是有⼀定的差距的，把这两个值放在若⼲数学公式⾥可以得到⼀个最优值，通过这个最优值与传感器和数学模型的值进⾏对⽐，我们可以知道哪个值与最优值⽐较接近，下次的计算我们应该较多的参考接近最优值的那个值，⽐如传感器的值最接近最优值，那我们就把传感器的值使⽤较⼤的权重，数学模型得到的值加使⽤较⼩的权重，所得到的这个权重不是随便给的，也是通过数学公式得也来的。那我下⼀个时间段再计算的时候这个权重就要起作⽤了，传感器的值和数学模型的值会带着权重放在数学公式⾥得到最优值，然后我们再把这个最优值与传感器和数学模型的值进⾏对⽐，再看⼀下哪个值与最优值接近，如果还是传感器的值⽐较接近，我们还是会通过公式⽣成⼀个权重，交给下个时间段的计算使⽤。通过不断重复这样的计算，我们就可以得到⼀个相对较优的值，这就是卡尔曼滤波算法的
⼤概过程。
三、飞⾏控制PID算法
PID控制器是⼀种线性控制器，它主要根据给定值和实际输出值构成控制偏差，然后利⽤偏差给出合理的控制量。
⽬前，⼈们通过科学研究获得了诸多具有优异控制效果的算法和理论，但在⼯程应⽤领域，基于经典PID的控制算法仍然是最简单、最有效的控制⽅案。
芯片生产PID控制器是⼀种线性控制器，它主要根据给定值和实际输出值构成控制偏差，然后利⽤偏差给出合理的控制量。
⽬前主流的⼏款开源飞控中，⽆⼀例外地都是采⽤PID控制算法来实现⽆⼈机的姿态和轨迹控制。
PID⾥的P是Proportion的⾸字线，是⽐例的意思，I是Integral的⾸字线，是积分的意思，D是Differential的⾸字母，是微分的意思。
那么PID控制器算法能解决什么问题呢？以多旋翼为例，在没有控制系统的情况下，直接⽤信号驱动电机带动螺旋桨旋转产⽣控制⼒，会出现动态响应太快，或者太慢，或者控制过冲或者不⾜的现象，多旋翼根本⽆法顺利完成起飞和悬停动作。为了解决这些问题，就需要在控制系统回路中加⼊PID控制器算法。在姿态信息和螺旋桨转速之间建⽴⽐例、积分和微分的关系，通过调节各个环节的参数⼤⼩，使多旋翼系统控制达到动态响应迅速、既不过冲、也不⽋缺的现象。
下⾯我们简单的举个例⼦让⼤家了解⼀下PID的作⽤，我们先以⼀个⾃动驾驶的⼩车来举例⼦，为什
么⽤⼩车呢⽽不⽤多旋翼来举例⼦呢？因为⽤⼩车举例，可以看到⼩车的⾏进轨迹，根据轨迹我们可以很直观的看到PID对控制的影响，便于理解PID的作⽤。
全自动发酵烘干房我们先讲P，⽐例控制。现在我们想让这个⼩车沿着绿线向前⾛，我们给P设置⼀个固定的值，这个值可以让离开绿线的⼩车向绿线的⽅向⾏驶，离的越远，⽅向盘打的⾓度越⼤，离的越近，⽅向盘打的⾓度越⼩。橙⾊的箭头表⽰⼩车⾏进的⽅向 ⽐如这个⼩车在这个位置，我们设置了⼀个中等⼤⼩的P值，想要沿着绿线⾛，在⽐例控制下路径是这样的，因为⼩车有⼀定的速度，到达绿线时因为惯性的原因⼜向前运动了，然后再根据中等P值向回打⽅向盘，当到达绿线时，同样因为惯性的原因冲过了头，因为⼩车越接近绿线，⽅向盘打的越⼩，所以⼩车每经过⼀次绿线，它的偏差就越来越⼩，所以随着⼩车多次的往复运动，就能离绿线越来越近，理论上最终能够⾏驶在绿线上⾯。
无动力排风帽如果我们把P值设置的⽐较⼤，它的路径是这样的，因为⽅向盘打的⾓度⽐较⼤，所以⼩车⽐中等P值的时候较早的到达绿线，但同样因为惯性的原因会多次往返绿线的两侧，也是⼀次⽐⼀次接近绿线，因为它⽅向盘⾓度打的⽐较⼤，所以会⽐中等P值往返的次数要多，最终经过多次往返，理论上它会离绿线越来越近，最终到达绿线上⽅。
如果我们把P值设置的⽐较⼩，也就是⽅向盘打的⾓度⽐较⼩，它的路径是这样的，⼩车会较晚到达
绿线，因为惯性会往返绿线两侧，但是因为⽅向盘⾓度⼩，⼩车可以在较少的往复次数下接近绿线。
所以我们对⽐⼀下设置这三种P值的结果，在⼩车与绿线相同距离的情况下，P值越⼤，⼩车的反应越快，P值越⼩，⼩车的反应越慢，所以这三个⼩车第⼀次到达绿线的时间是不⼀样的。虽然P值⼤能够较快的到达绿线，但是反应⽐较剧烈，总是因为过快冲过了头。相反P值⼩的反应⽐较平缓，但是它反应太慢，我们有时候接受不了。
什么样的P值是合理的呢，就是设置后，⼩车的反应不是很剧烈，反应时间你也能够接受，那这就是⼀个相对合理的P值。
那有没有⼀种办法让它反应再快⼀点，反应⼜不那么剧烈呢，那就要⽤到接下来我们要讲的微分控制了。
我们为了不让这个⼩车冲过头，我们再给它加⼀个⼒，这个⼒就是D，让这个⼒来起⼀个作⽤，就是让⼩车越接近绿线的时候，接近绿线的速度越慢，⼩车越远离绿线的时候，接近绿线的速度相对较快，这个接近绿线的速度不是⼩车前进的速度，是⼩车与绿线平⾏线之间的相对速度。这个D⼤家可以理解为⼩车靠近绿线的⼀个阻⼒。
豫公网安备41160202000603
站长QQ:729038198
关于我们
投诉建议