【无人机 学习笔记 2】无人机导航制导与控制

梳理完控制以及飞行力学的一些基本原理之后，就该正式进入无人机导航、制导与控制的讨论了。导航制导与控制是无人机系统中最复杂的分系统，其功能可以有多种划分方法，本文中，我们就以下面框图所示的划分方法为例，对无人机导航制导与控制系统的基本原理和常用方法做一下介绍和归纳。

由于GPS、室内定位甚至自动驾驶在生活中的广泛应用，“导航”、“制导”、“控制”这几个词也越来越为大众所熟悉和使用，但是对于这些词的定义，我们日常生活中的使用和理解方法可能与无人机语境有所不同，所以有必要对其在本系列文章中的含义做一下解释：

导航：即无人机获得自己当前（在某个参照系下）的位置、速度等信息，必要时还需要获得当前（相对于某个参照系）的姿态、姿态角速度等信息。例如，采用纯惯性导航可以获得无人机在某个惯性系下的位置、速度和加速度，以及相对于该惯性系的姿态角和角速度；GPS导航系统则可以提供无人机在WGS84坐标系下的速度、位置和航向角等信息；而借助如Vicon、UWB等室内定位系统则可以获得无人机相对于室内某个坐标系的速度、位置等信息。因此，简要概括导航的主要工作就是要“知道自己在哪，知道自己的姿态”。

制导：即无人机发现（或外部输入）目标的位置、速度等信息，并根据自己的位置、速度以及内部性能和外部环境的约束条件，获得抵达目标所需的位置或速度指令。例如，按照规划的航路点飞行时，计算无人机径直或者沿某个航线飞抵航路点的指令；采用基于计算机视觉目标跟踪的光学制导时，根据目标在视场中的位置（以及摄像头可能存在的离轴角）计算跟踪目标所需的过载或者姿态角速度指令；而当预装（或SLAM获得的）地图中存在需要规避的障碍物或禁飞区时，根据无人机飞行性能计算可行的规避路线或者速度指令。因此，简要概括制导的主要工作就是要“知道目标在哪，如何抵达目标”。

控制：即无人机根据当前的速度、姿态等信息，通过执行机构作用来改变姿态、速度等参数，进而实现稳定飞行或跟踪制导指令。例如，当固定翼无人机需要爬升高度时，计算需要的俯仰角和俯仰角速度指令，以及为了让空速不至于大幅降低所需的油门指令；当沿着航线飞行，但是存在侧风时，计算所需的偏航角指令以利用侧滑抵消侧风影响；或者当多旋翼无人机的某个旋翼失效时，计算如何为剩余旋翼分配指令以尽可能实现稳定飞行。因此，简要概括控制的主要工作就是“改变飞行姿态，跟踪制导指令”。

虽然理论上，导航、制导和控制这三者各司其职，只是在指令计算和执行上有顺承关系，但是在实际系统中，三者可能会有很多交叉因素。例如，导航系统中所测量或估计出的角速度，既要用于导航系统的速度和位置估计，又要用于姿态控制；而在一些高机动性的飞行器（如直接碰撞杀伤的动能拦截器等）和空天飞行器（如升力体再入返回的制导控制）上也有制导与控制一体化设计的趋势。但在本文中，仍然根据无人机的固有特性，尽量将三者作为具有独立功能的分系统看待。其中，导航系统原理可以大致分为以下几个类型：

基于绝对参考系的导航。如惯性导航、磁罗盘导航等。惯性导航运用牛顿力学原理，通过构建一个与机体固联的惯性平台，从而根据加速度计测量的惯性加速度计算在某惯性参考系下的速度和位置，根据陀螺仪测量所得的角速度计算机体相对于惯性平台的姿态角，从而只需要加速度计和陀螺仪满足一定的精度要求，就可以在不需要外部信息的情况下获得机体相对于惯性参考系的速度、位置和姿态角。之所以将与机体固联的移动参照系成为惯性平台，是因为早期的平台式惯性导航设备中确实存在一个物理上的框架，该框架基于陀螺进动原理始终与惯性系（或当地铅锤坐标系）保持平行。高精度的平台惯导系统可以长期不需要外部信息进行导航，例如有些核潜艇所装备的惯导系统可以保证水下航行数月的导航误差在数海里的量级。

虽然平台惯导的精度很高，但是由于系统复杂且体积巨大，不便于在小型飞行器上装备，随着计算机技术和导航器件技术的发展，捷联惯导越来越多地被使用。与平台惯导所用的物理平台不同，捷联惯导的陀螺仪和加速度计都与机体固连，因此采用虚拟的数学惯性平台，即惯性器件测量所得数据都会经过坐标变换的数学运算转换到惯性坐标系下，由于去掉了物理平台，捷联惯导系统的体积大幅缩减。特别是近二十年来快速发展的MEMS（微机电系统）器件，已经可以将捷联惯导系统的体积缩小到几立方厘米的量级。

当然，惯性导航并非完美，由于导航过程依赖惯性器件的输出数据、坐标变换以及数值积分，所以器件误差和数值计算的截断误差会不断累积，在缺乏额外的相对于绝对坐标系的信息时，该误差无法被修正，因此，惯导系统通常作为飞行器的主要导航系统，但同时还需要其他导航信息对惯导结果进行修正。

几乎其他所有导航方法都可以用于修正惯导系统误差，甚至是惯导系统本身，如AHRS（航姿参考系统），这种系统除了采用陀螺仪积分得出姿态角，还能根据加速度计测量的重力方向以及磁罗盘测量的磁航向对姿态角结果进行修正，从而在陀螺仪精度不高的情况下获得长期稳定的姿态角输出，不过由于低精度器件所得的姿态角结果短期和长期均有不同程度的误差，该系统无法进行精确的航位推算。

基于距离测量的导航。如卫星导航、室内定位等。这类导航方式一般是通过测量飞行器与已知精确位置的参考点之间的距离，从而解算出飞行器位置。例如卫星导航系统就是通过接收多颗卫星发射出来的星历信息，从中得出时间差并根据光速计算出距离，从而解算出飞行器在WGS84坐标系下的位置和经纬高度信息。同样采用类似方式的还有室内定位应用中很火的WIFI定位和UWB定位技术，均是利用信号强度或发送接收的时间差计算飞行器与各参考点之间的距离，从而解算飞行器实时位置。

基于特征匹配的导航。如地形匹配、运动捕捉系统等。这类导航方式通常是通过飞行器实时提取地磁、地貌、图像等特征，并与特征库进行比对或进行相应计算，从而得到飞行器位置、速度等信息实现导航功能，如巡航导弹中所使用的地形匹配方法和现在比较火的SAR（合成孔径雷达）地貌匹配方法，都是通过提取飞行路径上的一维或二维地形地貌信息，并与数字高程地图库进行比对，从而获知当前位置、速度等信息，这在卫星导航信号丢失时的长期导航具有重要意义。运用计算机视觉技术，通过识别已知位置上的标记物特征完成位置、速度估计的方法也归属此类。还有另一类导航方法就是类似于Vicon的运动捕捉系统，这种系统则是通过已知位置的光学等传感器识别飞行器上设置的标记物，从而解算出飞行器实时位置、速度。

而既然说到基于特征，就不得不关注计算机视觉在导航中的应用，例如在消费级无人机上运用多年的稀疏光流算法，就是根据灰度图像中特征点的运动计算出无人机的运动速度，近年来火爆的SLAM则更是将计算机视觉发挥到极致，这种算法通过将运动中实时采集的图像特征性信息与惯导等系统信息进行融合，从而可以在未知环境中一边完成周围场景的三维模型重建，一边进行自身在场景中相对位置和速度的解算。

说回无人机的导航，当前多数无人机采用惯导/卫星导航组合作为基本的导航方式，可以保证绝大多数场景下的稳定导航。大型军用无人机由于对导航系统的轻量化和成本要求不高，为了实现较高的导航精度，其通常仍采用光纤/激光陀螺和石英加速度计组成的高精度惯导系统，而中小型和民用无人机则采用更轻小更廉价，但是精度较低的MEMS器件组成惯导或航姿参考系统，与卫星导航组合后，仍能提供有效的导航信息输出。

而在某些特殊应用场景下，卫星导航信号会丢失，如微型无人机在室内和城市楼群之间飞行，这时就需要其他的导航方式进行辅助。常用的比如气压计的使用就可以以较低的综合成本获得低精度的海拔高度（误差100米量级）和较高精度的相对高度信息（误差0.1米量级）。无人机在室内飞行时，可以架设前文提到的WIFI、UWB或Vicon等需要复杂外部设备的室内定位系统，或者外部设置已知位置的标记物，通过无人机的视觉系统完成识别和自身定位。而在极为特殊的场景下，如各种高危未知环境的勘测，使得常用辅助导航系统都难以使用时，就不得不祭出SLAM这一杀手锏了，SLAM技术正处于高速发展中，且已经有多种实用的方案出现了，完美的SLAM系统可以完成科幻电影里那种放出去几驾微型无人机自由飞行，配合一个便携地面站，便可以实时地重建周围环境的3D模型，这种性能在未来五年之内肯定可以实现。当然绝大多数辅助的导航方式都难以输出用于制导控制的高频率（200Hz以上）导航信息，因此通常情况下仍是将辅助导航系统与惯性导航相结合。

下面来讨论无人机的制导，现阶段大多数军用还是民用无人机在自动飞行过程中仅需完成航路点或航线的跟踪，因此制导策略相对简单。多旋翼无人机，跟踪航路点时只需要将飞行速度方向对准下一个航路点，跟踪航线也仅需首先飞到航线上距离当前位置最近的点即可；而这项任务对于固定翼无人机相对复杂。因为固定翼无人机的速度方向需要通过航向来改变，而航向则需要通过滚转来改变，这就使得滚转角与速度方向之间形成了近似二阶环节的过程，这通常可以运用导弹的比例导引法来实现航路点跟踪。比例导引法的基本原理就是让飞行器速度矢量在空间中的转动角速度正比于飞行器与目标间的视线角变化率，对于航路点这一静止目标，只需要无人机与航路点之间的距离足够，就可以保证准确抵达下一个航路点，而对于航线跟踪，则需要选择一个虚拟的目标点使得无人机首先向航线靠近，然后再逐步将方向对准航线方向。例如现在被广泛使用的L1制导算法，就是在航线上选择与无人机距离为L1的参考点，然后根据速度方向与到参考点连线方向之间的夹角计算横向机动的需用过载，进而实现航线跟踪。

而随着无人机在多种场景下应用的不断深入，除了航路点和航线的跟踪以外，无人机抵达目标的最优路径选择，障碍物或禁飞区规避以及多机协同工作所需要的制导策略越来越复杂。我们知道最优控制方法在航天器轨道转移、火箭入轨制导等问题中起到了良好的效果，但是对于大气中飞行的无人机路径规划，基于间接法的最优控制问题很难求解，因此无人机路径规划往往采用基于网格地图的搜索算法，或者蚁群算法、遗传算法等特殊的路径优化方法。例如在基于概率地图的搜索算法中，首先运用随机概率方法在自由空间（任务空间中，除去障碍物后的空间）中选取采样点，并选取距离当前点最近的k个点构成当前点的临近点集，然后利用局部规划器将当前点与其临近点集中的所有点用直线段连接起来，同时进行相交检验，将不与障碍物相交的直线段保留下来构成一个图，作为初始路径， 完成路径规划的学习阶段；在查询阶段，运用优化方法对上述图进行搜索，从而得到由图的边构成的从出发点到目的点并满足优化目标的路径。

另一类常用的算法并不是基于网格地图进行搜索，例如人工势场法，其基本思想是将无人机的运动，设计成一种在抽象的人造引力场中的运动，如下图所示，目标物对无人机产生“引力”，而障碍物对无人机产生“斥力”，通过求解目标和所有障碍物对无人机产生的合力，就可以得到无人机运动速度或加速度指令。相对于大多数搜索算法，人工势场法运算量更小，且得到的轨迹更平滑。

以上这两类制导算法通常适用于一架无人机的航路跟踪或路径规划，而当设计无人机编队甚至集群时，问题复杂程度则骤增。对于集群中的某个无人机来说，其他无人机既是可以协作和互通信息的伙伴，同时又是快速移动的障碍物，而整个集群的路径规划有需要考虑集群以及其中每一架无人机特性所形成的约束条件，或者当集群处于协同作战模式时，又需要对目标自发形成各角度的全向饱和攻击，当然，这其中需要解决的问题正是当前研究的热点。

最后再讨论一下无人机的控制，导航系统获得了无人机当前位置速度和姿态信息，制导系统完成路径规划和制导指令生成，而控制的任务就是精确、快速稳定地跟踪收到的制导指令，因此控制也是最关键的环节。最常用的控制算法还是历久弥新的PID，通过将被控参数参考值与当前值误差的比例、积分和微分进行适当组合，便能够完成大部分近似线性系统的有效控制。

而事实上，现在工程中所使用的很多PID算法，早已经不是基本的构型了，常用的改进方式主要有以下几种：

增益调度：既然PID控制器设计过程一般是在某个平衡点处做系统的小扰动线性化方程（平心而论，工程中还真不都是这么按流程来，各种野路子都有），进而完成设计的，那么只要在正常工作范围（对于无人机来说可以是飞行包线）内选取足够的平衡点，并根据每个平衡点的模型选择合适的PID控制参数，这样就可以在控制器工作中通过插值等方式选择相应平衡点附近的控制参数，这种变参数的方法就是一种增益调度方法，而基于增益调度的PID控制器就可以针对具有一定非线性特性的系统进行控制。这种方法在飞行控制中已应用多年。

参数自适应：比如以系统积分误差性能指标为准则，搜索使得误差性能指标为最小的参数作为控制器参数，又或者基于神经网络和遗传算法的参数自适应等，不过这些方法在工程中使用的比较少。

串级：通过将被控系统分为内外环，只需要内外环的固有频率有一定的差别（比如说内环频率是外环的五倍以上，无人机的姿态响应和位置响应一般可以满足），即可用实现快变量和慢变量的分别控制，通过简单的调参就可以实现快速的内环响应和精确的外环控制，并具有比单个控制器更好的抗干扰性能。

积分抗饱和：PID控制中的积分作用虽然可用消除稳态误差，但是积分退饱和过程带来的超调往往较大，因此可用在被控参数的误差较大时，停止误差的积分过程，或者对误差的积分值进行限幅，这样就可以显著地降低超调量，缩短过程的稳定时间。

不完全微分：虽然被控参数一般不会出现突变，但是参考值却经常会出现突变，这使得误差的微分也会突变，为了降低这种突变造成的控制量幅值，可以采用不完全微分策略，即微分只作用于被控参数（如飞行控制中的角速度阻尼）。

PID算法的改进方式还有很多，难以细数，不过这种改进终归难以解决所有问题，例如被控对象的高度非线性、强耦合性、时变性等特性，因此新的控制方法层出不穷。下面列举几种较为实用的其他控制方法。

反馈线性化：利用数学变换的方法和微分几何学的知识，将状态和控制变量转变为线性形式，然后，利用常规的线性设计的方法进行设计，将设计的结果通过反变换，转换为原始的状态和控制形式。反馈线性化可以将存在通道间耦合的非线性系统变换为解耦的线性系统，方便外环的线性控制器设计。不过该方法应用中或多或少会存在建模误差，因此设计时要重点考虑鲁棒性的因素。

滑模变结构：这种方法不需要对被控对象进行精确建模，而是在动态过程中，根据系统当前的状态(如偏差及其各阶导数等)有目的地不断变化，迫使系统按照预定“滑动模态”的状态轨迹运动。由于滑动模态可以进行设计且与对象参数及扰动无关,这就使得滑模控制具有快速响应、对应参数变化及扰动不灵敏、无需系统在线辨识、物理实现简单等优点。但是基本的滑模变结构算法存在控制参数抖振的问题，需要再趋近率设计时进行适当的优化策略。

反步控制：其基本思路是将复杂的系统分解成不超过系统阶数的多个子系统，然后通过反向递推为每个子系统设计部分李雅普诺夫函数和中间虚拟控制量，直至设计完成整个控制器。反步方法运用于飞控系统控制器的设计可以处理一类非线性、不确定性因素的影响，而且已经被证明具有比较好稳定性及误差的收敛性。

自适应逆：与动态逆的思想类似，这种方法运用各种自适应逆滤波网络（如LMS滤波器网络、神经网络等）去拟合出被控对象的逆系统，从而将控制器与被控对象构成的前向通道变换成一一映射的线性化解耦系统，而之所以称为“自适应”，则是这个拟合出逆系统的网络可以在线学习被控对象的特性。这种方法在仿真中可以取得比传统控制方法优越很多的效果，但是由于滤波器网络可能存在无法检出的内部缺陷，所以在某些状态组合下，可能会出现故障（包括深度神经网络在内的所有神经网络都潜在此风险）。

本文简要梳理了可用于无人机的导航、制导和控制的方法、策略或算法，其中部分算法将在后续的仿真系统相应的文章详细介绍并在代码中体现。

源自：知乎