无人机相关算法汇总_无人机算法

一、航迹规划算法 

https://zhuanlan.zhihu.com/p/222542590 

1.传统经典算法

（1）动态规划算法

动态规划算法 - 知乎

动态规划算法是解决多级决策最优化问题的常见算法。将一个复杂问题拆解成若干子问题：f(n)=f(n-1)+f(n-2)。

（2）导数相关法

采用导数相关法，主要有最速下降法、牛顿法、共轭梯度法、拟牛顿法、信赖域方法和最小二乘法，其中最速下降法目前使用比较多。

>最速下降法:  无约束优化问题 — 最速下降法 - 知乎

最速下降法（Steepest descent）是梯度下降法的一种更具体实现形式，其理念为在每次迭代中选择合适的步长 αk ，使得目标函数值能够得到最大程度的减少。

（3）最优控制方法 

 基于最优控制的轨迹优化方法 - 知乎

1）直接方法

该方法将轨迹优化问题转化为一个非线性规划问题（NLP）或者一个多项式优化问题，这个过程常称为transcription。通过对目标函数和约束条件进行适当的转化，然后使用标准的数值优化算法来求解问题。

直接方法常见的有以下几种：

1. 伪谱方法（Pseudospectral Method）：将状态和控制输入离散化为一组节点，在节点处近似表示动态方程和边界条件，并将最优控制问题转化为非线性规划问题。伪谱方法具有高精度和高效率的优点，被广泛应用于飞行器、机器人等领域。
2. 直接单步法（Direct Single Shooting Method）：将整个时间段划分为若干个小时间段，将控制输入视为自变量，状态变量视为因变量，通过求解微分方程来获得状态轨迹，最终将最优控制问题转化为一个非线性规划问题。直接单步法计算效率高，但是精度较低。
3. 直接多步法（Direct Multiple Shooting Method）：将整个时间段划分为若干个小时间段，在每个时间段内都将状态方程视为一组非线性方程，通过求解微分方程来获得状态轨迹，最终将最优控制问题转化为一个非线性规划问题。直接多步法计算效率和精度均比直接单步法高。

2）间接方法

该方法则将轨迹优化问题转化为一个边值问题（两个端点状态已知），然后通过使用变分法将该问题转化为一个微分方程组，最后使用数值方法求解微分方程组得到最优轨迹。与直接方法相比，间接方法的优点是能够保证全局最优解，同时对于特定的系统动力学模型，该方法能够得到系统的解析解。缺点是该方法求解速度比直接方法慢，同时需要较高的数学功底和计算能力。

常见的间接方法有以下几种：

1. 庞特里亚金极小值原理（Pontryagin's Minimum Principle）：将最优控制问题转化为极小值问题，求解得到极小值控制量和状态轨迹。此定理一开始的名称是庞特里亚金最大化原理（Pontryagin's maximum principle），其证明也是以控制哈密顿量最大化为基础。但是，由于在实际应用中，通常需要最小化性能指标，因此人们也将其称为"Pontryagin's minimum principle"。在庞特里亚金的著作中，他解决了许多最小化性能指标的问题，这些问题涵盖了广泛的应用领域，如火箭制导、经济控制等。但当系统的状态空间很大或者非线性时，使用庞特里亚金极小值原理的效果可能会变得不那么好。此外，该方法的求解速度相对较慢，需要高水平的数学和计算能力，因为它需要解决一组微分方程以及运用最优性条件等。
2. 变分法：它用于求解泛函问题。泛函是一个函数集合，其中每个函数映射一个函数到实数。变分法基于寻找泛函的极值，从而导出微分方程的解。它可以将最优控制问题转化为求解变分问题，从而可以简化问题的求解过程，并且可以处理连续和离散的最优控制问题。变分法还具有比较高的准确性和精度。 然而，变分法求解过程中需要解决一组微分方程或者偏微分方程，这个过程可能比较复杂，需要高水平的数学和计算能力。另外，变分法通常需要采用数值方法来求解，求解时间可能会比较长。
3. 欧拉-拉格朗日微分方程：一种基于变分法的优化方法，它将系统的能量泛函通过欧拉-拉格朗日方程变换成等效的微分方程组，然后通过求解这个方程组来得到系统的控制量和状态轨迹。它可以处理非线性问题，并且可以通过求解一组常微分方程来得到系统的控制量和状态轨迹。此外，欧拉-拉格朗日方法还可以处理多目标优化问题。但是，欧拉-拉格朗日方法的缺点是它需要计算高阶导数，因此在处理高阶微分方程和奇异点时会变得复杂和困难。

2.智能规划算法

（1）启发式寻优搜索

启发式搜索是在状态空间中的搜索，对每一个搜索的位置进行评估，得到最好的位置，再从这个位置进行搜索直到目标，省略大量的搜索路径，提高效率。

A*算法是一种经典的启发式搜索算法：六分钟！带你掌握A星算法的原理！！_哔哩哔哩_bilibili

A星算法是一种求解最短路径问题的有效算法，通过计算节点的代价值和启发函数来确定最佳路径。启发函数常用的有曼哈顿距离和欧几里得距离，它们用于估计节点到终点的距离。

F=G+H

其中F是起点到终点的总代价

G是从某一节点到开始节点的移动距离（确定）

H是从某一节点到终点的估计移动距离（不确定）

计算H的代价函数 称为 启发函数

（2）遗传算法

 【数之道14】六分钟时间，带你走近遗传算法_哔哩哔哩_bilibili

（3）神经网络方法 

 五分钟秒懂神经网络原理，机器学习入门教程_哔哩哔哩_bilibili

>蚁群算法 【数之道 04】解决最优路径问题的妙招-蚁群ACO算法_哔哩哔哩_bilibili

（4）数值优化算法

二、姿态控制算法

姿态控制算法用于控制四旋翼飞行器的姿态，包括俯仰、横滚和偏航三个方向。姿态控制算法通常采用PID控制算法，通过调节电机转速来控制姿态。

传统的姿态控制方法主要是基于PID控制器，通过对无人机的姿态误差进行测量和反馈，调节无人机的电机输出，从而实现姿态的稳定。然而，PID控制器在应对复杂环境和外部干扰时存在一定的局限性。

（1）基于模型预测控制（Model Predictive Control，MPC）的方法

该算法通过建立无人机的动力学模型，预测未来一段时间内的无人机状态，并根据期望的姿态和当前状态之间的误差，优化控制输入，使得无人机能够实现稳定的姿态控制。相比于PID控制器，MPC算法具有更好的鲁棒性和适应性，能够更好地应对不确定性和干扰。

（2）基于自适应控制的方法

该算法通过对无人机的动力学特性进行建模和辨识，自适应地调整控制器的参数，以提高控制器的性能和适应性。自适应控制算法能够自动调整控制器的参数，适应不同的工况和环境变化，提高无人机的姿态控制精度和稳定性。

三、位置控制算法

位置控制算法用于控制四旋翼飞行器的位置，通常采用PID控制算法，通过调节电机转速来控制位置。

四、速度控制算法

速度控制算法用于控制四旋翼飞行器的速度，包括起飞速度和巡航速度等。速度控制算法通常采用PID控制算法，通过调节电机转速来控制速度。

五、高度控制算法

高度控制算法用于控制四旋翼飞行器的高度，通常采用PID控制算法，通过调节电机转速来控制高度。

 六、无人机飞控算法

1.捷联式惯性导航系统

什么是惯性导航呢?他就是利用载体上的加速度计、陀螺仪这两种惯性远见，去分别测出飞行器的角运动信息和线运动信息，与初始姿态、初始航向、初始位置一起交给计算模块，由计算模块推算出飞机的姿态、速度、航向、位置等导航参数的自主式导航方法。

2.卡尔曼滤波

放弃（通俗公式理解）_哔哩哔哩_bilibili

Kalman Filter 算法，是一种递推预测滤波算法，算法中涉及到滤波，也涉及到对下一时刻数据的预测。Kalman Filter 由一系列递归数学公式描述。它提供了一种高效可计算的方法来估计过程的状态，并使估计均方误差最小。卡尔曼滤波器应用广泛且功能强大：它可以估计信号的过去和当前状态，甚至能估计将来的状态，即使并不知道模型的确切性质。

（1）适用系统：线性高斯系统（噪声满足正态分布）

（2）宏观意义：滤波就是加权（理想状态：信号*1+噪声*0）。对于卡尔曼滤波来讲，就是估计值和观测值之间权重问题，由权重实现修正

（3）卡尔曼直观图解

xk-1^是上一时刻的最优估计值，xk是当前估计值，,xk- ^是当前最优估计值（他的方差比xk-1^小是因为修正后方差要小的缘故，而且他是由xk-^和yk最终决定的），yk是观测值。

（4）卡尔曼公式理解

实现过程：使用上一次的最优结果预测当前的值（先验估计），同时使用观测值修正当前值，得到最优结果（最优估计值）

 xt-^为先验估计，pt-为先验估计协方差

测量方程：zt=

 3.飞行控制PID算法

无人机的飞行姿态控制，我们的控制对象是无人机的三个角度。每个角度都考虑用一个PID环去控制，最终作用到执行机构上的还是四路电机的PWM信号。

四轴单级PID姿态控制
同理，三个角度。（三个角度的控制，最终作用到四路电机的PWM控制效果是线性叠加）

串级PID控制

主要是在角度控制的基础上，增加了角速度控制。期望姿态角与姿态结算出来的角度进行角度PID控制，输出的角度值，是离散型数据。又因为w（角速度）=a（角度）/t（时间）。所以可以认为输入的也是角速度。于是就实现了外环PID值作为内环PID的期望值目的。再结合陀螺仪比较精准的采集角速度的原理。内环的角速度控制可以保证飞行器测量值迅速的达到期望值。

这样一来就大大缓解了单角度环控制，由于飞机的电机转速与升力不成正比而出现的失衡（比如说在目标位置前后，左右偏移），跟随滞后等等问题。

四旋翼无人机相关算法的其他分类

 四旋翼算法可以分为飞行控制算法、导航算法、路径规划算法和图像处理/视觉算法。下面是这些分类中的一些具体算法：

1.飞行控制算法：

PID控制器：使用比例、积分和微分控制器来调整无人机的姿态、高度和位置。
LQR控制器：线性二次调节控制器，可用于设计稳定的飞行控制系统。
MPC控制器：模型预测控制器，通过对未来状态的预测来计算最优控制指令。

2.导航算法：

GPS导航：使用全球定位系统（GPS）来确定无人机的位置和速度。
惯性导航：使用加速度计、陀螺仪和磁力计等传感器来测量无人机的加速度、角速度和方向。
扩展卡尔曼滤波（EKF）：用于融合不同传感器的数据，实现更准确的位置和姿态估计。

3.路径规划算法：

A*算法：基于图搜索的算法，用于在离散的地图/环境中找到最短路径。
Dijkstra算法：用于计算无权图中的最短路径。
RRT（Rapidly-exploring Random Tree）算法：用于快速生成无人机的可行路径。

4.图像处理/视觉算法：

特征提取和匹配：例如使用SIFT（尺度不变特征变换）或ORB（Oriented FAST and Rotated BRIEF）算法来提取和匹配图像特征，用于目标检测和跟踪。
视觉SLAM（Simultaneous Localization and Mapping）：使用相机图像和运动信息进行实时定位和地图构建。
光流算法：通过分析连续图像帧之间的光流模式，估计无人机的运动速度和方向。

5.目标检测与跟踪算法：

（1） Haar特征级联分类器：用于检测人脸等对象。

（2）HOG（方向梯度直方图）特征与SVM（支持向量机）：用于行人检测。卷积神经网络

（3）（CNN）：用于实现高精度的目标检测和跟踪。

这些算法只是四旋翼无人机领域中的一部分，具体的算法选择取决于无人机的应用需求和硬件配置。