四旋翼飞行器避障系统基础

一、四旋翼飞行器

无人飞行器可以被用于民用事业、满足国防需求，也可以开发和利用太空资源、实现太空操作和太空试验等，因此，国内外对无人飞行器作了大量研究。

主要研究的无人飞行器包括旋翼式飞行器、直升机、软式小型飞船及其它飞行器等。相比于有人驾驶的飞行器，无人飞行器具有很多优势。例如，其有很强的机动性，能瞬时改变姿态；体积小，灵活性高：可以避免驾驶员在危险环境中工作等等，近些年来无人飞行器在军用和民用领域发展非常迅速。

小型无人驾驶飞行器可分为固定翼飞行器和旋翼飞行器两种。固定翼的飞机技术相对成熟一些，因为出现的时间比较早，但在一些需要在较狭窄的空间内执行任务的场合，固定翼的无人飞行器就会存在机动性差的缺点，而旋翼飞行器由于其体积小，轻便灵活则可以弥补这一缺陷，旋翼飞行器能够实现垂直起降、全方向飞行以及目标上空悬浮，即使在很小的空间内也可以完成以上这些动作，因此旋翼式飞行器在执行监视和侦查类任务时更占优势。

四轴飞行器是旋翼飞行器的一种，和其他的旋翼飞行器相比，四轴飞行器使用了四个独立的电机来作为驱动，它拥有四个螺旋桨，可以产生相比于单翼直升机更大的升力，并且它的机架倾角是固定的，在飞行的过程中如果想改变飞行速度和姿态不需要通过调节旋翼的倾角，从而大大的简化了飞行器的结构，也减轻了自身的重量。飞机的四个旋翼产生的推力可以更好的实现飞行器的静态盘旋，还可以在悬空静止的状态下实现瞬时姿态改变，有极大的有效承载力和极高机动性。由于四轴飞行器具有以上种种的优点，它成为了目前学者和机械爱好者们研究的热点。

四旋翼飞行器能够稳定的悬停在空中，所以有利于一些需要稳定性较高的空中作业，例如航拍、遥感探测等。

人们对智能控制技术的要求越来越高，许多事情并不像以前一样要求事先对作业环境进行大量的信息采集。而飞行器的工作环境也从只在已知的环境中飞行转变到了在无法预测和探知的环境中作业。

四旋翼飞行器如果想要在复杂多变的动态环境中实现自动避障和自主飞行。

我们的研究重点就应该放在如何让四轴飞行器可以自动检测到影响其飞行的障碍物，并且可以自行进行数据处理，做出避开障碍物的反应，在面对动态的和不确定的环境的情况下，让四轴飞行器实现这一功能是必不可少的。四旋翼飞行器飞行系统的关键性能指标是能否实现智能化自动避障功能，同时，自动避障也保证了四旋翼飞行器的行驶安全。运动规划中的基本问题之一是避障1，路径规划中的难点一直都是如何实现自动避开障碍物的功能）。四旋翼飞行器避障系统的一个基本要求是主动避开障碍，同时这也是四轴飞行器能否实现自主作业的关键。然而，对于在室内飞行的四旋翼飞行器来说，它需要在有条幅或其它活动物的场馆中飞行，或者经常有人走动和障碍物摆放凌乱并且形状比较复杂的环境中飞行，所以，为了保证飞行器、人以及周边环境的安全，让室内四旋翼飞行器实现快速有效地避障显得分外重要1.因此自动避障13】的研究对于推动和发展四旋翼飞行器的应用事业有着较强的现实意义。通常室内四旋翼飞行器的体积比较小，重量亦不是很大，所以他的自动避障系统也必须做到重量轻、体积小、灵敏度高的要求，尤其重要的是，由于其四个螺旋桨高速旋转，一定要保证其对人眼等脆弱部分没有安全隐患。四旋翼飞行器进行的所有飞行活动的前提条件是要能够自动躲避障碍物，有关自动避障的问题通常可以分为两种：一种是基于传感器的局部避障法，另一种是基于模型的静态避障法，第一种方法是利用各种传感器来探测周围的环境，使得四旋翼飞行器可以直接检测到环境的变化，及时返回相关数据给主控制芯片，从而可以实现实时避开障碍物的功能4，这种方法不需要在避障前对环境进行规划及空间建模，因而其可以节省大量的系统资源，效率也相对较高；第二种方法首先需要知道整个环境中障碍物的信息，然后根据环境信息进行建模，然后实现全局无碰撞的路径规划，这种方法可以很方便地搜索到全局目标点，这是它的最大优点，然而这种方法是假设环境是静态不变的，这种方法只适应于静态环境，对于运动的障碍物或者变化的环境，这种方法就显得无能为力了10.因此，本文拟采用第一种方法来实现飞行器的蔽障。

让四旋翼飞行器进行一定范围内的自动避障的目的是为了在没有人干预的情况下，四旋翼飞行器能够有目的的飞行避开障碍物保证自身及其环境的安全。

自主避障的研究对于推动和发展四翼飞行器的应用有着较强的现实意义。

二、四轴飞行器系统的原理

四轴飞行器是一种旋翼式无人机，从某一方面来说，我们可以认为它是四个单翼直升机的组合，但从另一方面说它与直升机又有很大的不同。直升机是靠尾部的尾桨来抵消主螺旋桨所带来的反桨矩，而四轴飞行器是靠两对旋转方向相反的旋翼去消除每个旋翼产生的反桨矩；此外，直升机的转向是靠调节桨叶间的桨矩角来实现的，而四轴飞行器只需要调节四个电机的转速使得每个螺旋桨产生的升力不同，即可控制和实现飞机的姿态控制。与直升机相比，极大的简化了控制方式和控制部件，减轻了飞机的重量，节能环保。

1.结构框架

在图2.1中，飞行器主要由如下部分组成15：

轴翼分：这部分是为整个飞行器提供动力的所在，固定安装在飞机四个轴的尾端，此部分包括正螺旋桨、反螺旋桨、电机以及一些固定组件。

飞行器控制部分：此部分位于四个轴的中心点，他的作用是控制整个四轴飞行器的运动姿态，其中包括遥控接收部分、主控制器部分、姿态采集部分、自动避障部分等等。

机身：用于固定飞行器控制模块和旋翼模块的部分，此外，在电机和飞行控制部分之间，根据所使用的电机不同，有不同的设计，有些电机为了实现更好更快捷的调速，需要安装专门的电子调速器来调速。

如上所示的这种四旋翼结构中，为了使整个四轴重心稳定，四个螺旋桨需要安放在同一高度平面，飞控部分需要安装在四轴的中心处，这样安放也方便飞控部分通过调整四个螺旋桨的转速来控制整个四轴飞行器的运动姿态，这种经典的四轴结构控制方式简单，控制部件少，工作方式也非常易于被我们所接受。

与传统的直升机相比较，四轴飞行器主要有以下几个特点：

（）四个螺旋桨可以产生更大的升力。可使飞行器的负载能力有较大的提高，因为相比于直升机的单翼来说，四翼产生的升力更大，当然了，更多翼（如六轴）的飞行器负载会更大，但是桨翼增多会造成控制系统变的更加复杂，所以综合了这些优缺点，这里我们选择四旋翼的结构。

（2）对称的四轴结构，各通道之间耦合很小。四轴飞行器具有非常对称的结构，它的上下运动和其他方向的运动之间不存在耦合，在理想情况下，横滚运动与前后运动之间也没有耦合，这就极大的方便了系统的设计和分析。

（③）四轴的结构可以通过控制四个螺旋桨各自的旋转速度来实现姿态控制。

飞行器在空中运动，将包括a.上下飞行；b.前后飞行；c.侧向飞行；d水平转动四种运动。

2.运动原理

（1）旋翼飞行器的特点旋翼类飞行器的姿态控制主要是通过改变桨翼拉力的大小或者改变螺旋桨盘的方向来控制飞行器的运动，四轴飞行器是旋翼飞行器的一种，它与直升机相比，有以下异同：

共同点：

与直升机一样两者都是旋翼类飞行器，因此，四轴飞行器的空气动力学运动原理分析和直升机的运动原理基本相似。

不同点：

（）传统的直升机不仅需承受螺旋桨和机身本身产生的力矩和力，还需要承受尾翼所产生的力和力矩。而四轴飞行器只受到螺旋桨和机体本身所产生的力矩和力。

（b）在姿态控制方面，直升机是通过改变桨矩角来实现转向的，而四轴飞行器由于其拥有四个螺旋桨，所以它可以通过调整各个电机的转速，从而改变每个桨翼所产生的扭矩和升力，从而实现飞行器的飞行姿态的控制，维持飞行器的稳定。

（2）四轴姿态调整方法：

整个四轴飞行器可以抽象成图2.2所示的模型。四轴飞行器的所有控制动作都是通过调整四个螺旋桨电机的转速来实现的，通过调整四个桨产生的力矩和力来控制飞行器的姿态。

控制飞行器四个桨翼产生的力矩和升力即可产生不同的飞行姿态，实现各种飞行任务；在图2.2中，F左前，F右前，F左后，F右后，分别代表着四个螺旋桨产生的升力，M左前，M右前，M左后，M右后，分别代表着四个螺旋桨产生的力矩；通过以上方法调控螺旋桨的力矩和升力，飞行器可以产生的具体基本飞行动作有如下几个：

（a）上下运动

飞行器最基本的飞行动作，即升降和悬停。下面我们在理想情况下对飞行器的升降及悬浮原理进行简要的说明。如图2.3所示，当飞行器四个螺旋桨以相等的速度旋转时，各个桨翼所产生的升力大小是相同的，当总的升力大小大于飞行器自身重力时，飞行器即可以实现上下垂直起飞：当总的升力大小小于飞行器自身重力时，飞行器即可以实现上下垂直降落；而当总的升力大小等于自身重力时，可以实现水平悬停。

[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-fqhuhM1w-1654962524709)(https://raw.githubusercontent.com/xkyvvv/blogpic3/main/img/202206112019536.png)]

（b）横滚和俯仰

飞行器的基本转向动作，主要包括俯仰运动和左右侧移。如图2.4所示，当右边两个螺旋桨的转速低于左边两个螺旋桨的转速时，右边产生的总升力小于左边两个螺旋桨所产生的总升力，使飞行器产生将向右侧移动的动作；同样道理，当左侧升力小于右侧时飞行向左侧移，当前方两个螺旋桨产生的升力大于或小于后面时，飞行器做相应的仰俯动作。

[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-vyncdtNk-1654962524710)(https://raw.githubusercontent.com/xkyvvv/blogpic3/main/img/202206112020505.png)]

（c）偏航（自旋转）运动

偏航运动即是四轴飞行器的原地自旋转运动。如图2.5所示，当不同对角线上的螺旋桨转速不同，同一对角线上的一组螺旋桨的转速相同时，由于每个电机产生的扭矩不同，飞行器螺旋桨电机产生的反扭矩不能相互抵消，从而导致飞行器的原地转向，即偏航运动。当A+B

[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-jGU5V95r-1654962524710)(https://raw.githubusercontent.com/xkyvvv/blogpic3/main/img/202206112020681.png)]

（3）在调整运动姿态时需要注意的问题：

（a）首先：必须让任意两个相邻的电机的转向相反，这样才能克服电机的反扭矩，防止飞行器自旋，如图2.6中所示，其中甲电机和乙电机，乙电机和丁电机，丁电机和丙电机，丙电机和甲电机转向都是相反的；而在同一对角线的一对电机（甲和丁；乙和丙）转向相同，通过对这两对电机转速的精确调整，使其反扭矩可以相互抵消，为避免在调节过程中造成飞行器俯仰角度发生变化或机体发生侧倾，位置相对的一对电动机的转速也需要同步改变。

[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-RhvoHj73-1654962524710)(https://raw.githubusercontent.com/xkyvvv/blogpic3/main/img/202206112021737.png)]

（b）其次，在调整飞行器的水平的时候，如果需要改变四个电机中某一个或多个电机的转速时，会造成飞行器自旋因为四个电机反扭矩的无法相互抵消，所以如果要改变某一个电机的转速就需要同步调整与之在同对角线上的一个电机的转速，使其产生的总扭矩可以不变。例如飞行器由于某种外界干扰造成右倾，这时我们就需要增加电机乙的转速以增加它的升力，然而这一行为会造成整个飞行器的逆时针力矩增加从而开始自旋，所以在增加电机乙的转速的同时还需要同步的减小电机丙的转速，以使得其维持力矩的平衡。

（©）最后，在以后升级更智能的自动避障系统时，可能需要维持高度的稳定，保持与障碍物的安全距离等功能，这时就需要利用某些传感器来检测飞行器与地面和障碍物的距离，为解决这一问题，我们可以选用的方案是使用红外测距和GPS技术，其中GPS和红外测距的分工是不同的，红外测距主要负责室内没有GPS信号的地方，且只适合低空（030cm）飞行时，而在室外可用GPS定位，且再超出地面30cm时利用卫星进行粗略的高度定位以维持高度不变。

3.自动避障原理

本文以STM32单片机作为控制核心、红外反射式传感器作为检测元件，设计了一个自动避障飞行器。要求飞行器在自动行驶过程中能够及时、灵活地避开障碍物。

具体的避障要求如下：如果飞行器前方无障碍物，飞行器继续直飞（保持原飞行指令不变）；如果仅在飞行器右方有障碍物靠近，飞行器向左拐弯：如果仅在飞行器左方有障碍物靠近，飞行器向右拐弯；如果飞行器直前方有障碍物，飞行器后退并保持安全距离悬停。

该系统主要由单片机、感测模块、电源模块、驱动模块等组成。具体工作原理为：首先，利用感测模块检测是否有障碍物，如果检测到障碍物，将障碍物位置信号送入单片机；然后，由单片机进行程序处理，根据不同的障碍物位置信号输出相应的控制信号，控制电机驱动模块；最后，由电机驱动电路输出不同PWM信号，使得左边两个和右边两个电机产生速度差，从而实现转向，最终达到自动避障的目的。

三、四轴飞行器硬件系统设计

飞行器的整个控制系统包括电源功能模块、遥控接收模块、惯性测量模块

（姿态传感器）、主控制器及接口与扩展等部分1617.系统如图3.1所示。

[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-0lDY9jnK-1654962524711)(https://raw.githubusercontent.com/xkyvvv/blogpic3/main/img/202206112028177.png)]

电源模块主要作用是给其他多个模块进行电力供应，利用电源模块，我们把锂电池供给的3.7V电压转换成3.3V的电压供给给其余各个模块使用。

需要说明的一点是，在遥控器上我们设计了锂电池充电电路，充电模块的电力直接由外界USB供给。

遥控接收模块则主要来实现用户操作指令的接受，一共有四个信号，分别是副翼（roll）、升降（pitch）、油门（thro）、旋转（yaw）的指令；陀螺仪和加速度传感器作为飞行器姿态检测部分，通过不断检测飞行器的角加速度和线加速度，从而判断出飞行器的当前姿态，然后与目标姿态进行比较从而形成闭环控制。

主控制芯片负责处理遥控接收模块接收到的飞行指令和MPU-6050传感器采集到的数据，将这两个数据进行融合，经过PID控制运算，得出电机的控制量信息，再以PWM波的形式输出以驱动螺旋桨电机。

四、避障传感器种类

用来采集运动时环境中障碍物信息的环境信息采集系统是自动避障系统的重要组成部分。全面、精确信息的获取，能够增强智能小车的自动避障性能，同时必须考虑信息处理的及时性和难易程度。视觉传感器、激光雷达测距传感器、红外测距传感器和超声波测距传感器等是主要的几种应用于智能小车自动避障的探测器。

1.超声波传感器

超声波测距传感器的工作机理是发射一定频率的信号，通过检测被物体反射、散射回来的信号，判断前方是否有物体3.超声波拥有比较好的传播方向性，能够沿直线传播，且穿透力比较强，能够得到相对集中的超声波能量。超声波频率的增加，导致其绕过障碍物能力不断减弱以及反射能力逐渐增强38.超声波测距传感器具有测距迅速、处理信息简单、控制实时、计算方便、价格低廉等特点，被广泛应用于智能小车测距、测速、定位和环境建模中。

1.1超声波测距原理

相位探测法、渡越时间探测法和声波幅值探测法是超声波的三种主要的测距方法

相位探测法，通过计算返回波与发射波间隔的相位来判断距离；声波幅值探测法，通过计算二者间相差的幅度，判断距离；渡越时间探测法，通过计算两者间的时间延误，判断距离。相位探测法测量精确度比较高，但是探测范围小。声波幅值探测法的探测精度受反射波的影响比较大。论文采用超声波测距最常用的方法一渡越时间探测法。下面主要介绍超声测距的渡越时间探测法原理。

渡越时间探测法，指的是超声波发生器往某个方向发射超声波，计时开始于发射的时间点，此后超声波沿直线传播，当超声波撞击到物体时就被反射回来，当超声波接收器接收到返回来的回波时计时停止42.超声波测距传感器与物体之间的距离d可以由公式（2.1）得出，其中℃为空气中超声波沿直线传播的速度，t为计时器所记录的时间。

[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-PaZQK88f-1654962524711)(https://raw.githubusercontent.com/xkyvvv/blogpic3/main/img/202206112043599.png)]

在实际环境中温度和湿度会对超声波的传播速度c造成影响，但是由于其变化很小，并不影响智能小车自动避障对测距的要求，所以设计中c取常值，即c=340m/s。图2.2为超声波完成测距过程的原理图

[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-HlurReFy-1654962524711)(https://raw.githubusercontent.com/xkyvvv/blogpic3/main/img/202206112044302.png)]

1.2超声波传感器测距的优缺点

利用超声波测距传感器测距具有如下优点：

（1）环境适应能力强。与视觉传感器相比较，超声波传感器能够在亮度低、有灰尘，煤烟等不理想的环境下，保持良好的测距性能，确保及时无误地采集到距离信息，然后将数据发送到信息处理设备。

（2）对色彩、光照度不敏感。超声波测距传感器可识别玻璃，抛光体等能透过光线和表面粗糙的物体。

（3）与视觉传感器相比超声波传感器可以得到距离信息。视觉传感器只能检测到物体的存在，而不能检测到障碍物和智能小车之间的距离，超声波传感器在检测到有物体的同时也可以得到与物体的距离信息。

（4）探测速度快。虽然在空气中光速比声速快，但视觉传感器较超声波传感器而言并没有速度上的优势。由于视觉传感器采集到的图片信息还需要经过图像识别过程，这个过程需要比较长的时间，制约了智能小车采集信息的速度。

（5）和视觉传感器、激光测距仪相比，超声波传感器还具有构造简易、占空间小、价格便宜，数据处理简单可靠，不容易损坏等优点。

所以超声波测距传感器，易于小型化和集成化，这更适合智能小车微型化的要求。

当然，利用超声波测距传感器测距也不能避免以下缺点：

（1）抗干扰性差：超声波能量较强，因此多个超声波传感器一起使用会相互干扰。

即使只使用一个超声波传感器，仍存在受声波的反复反射而造成干扰误差。

（2）由于超声波的波束角比较大，对物体位置的检测不够准确。

（3）单个超声波传感器的稳定性不理想，近距离测距有盲区45。

（4）超声波测距只能检测障碍物的距离数据，无法得到障碍物的尺寸和形状等信息。

综上所述，超声波测距传感器广泛应用在智能小车自动避障、汽车倒车雷达测距，物位（液位）探测，超声波开关和安防监控等相关领域4.但是超声波测距传感器不适用于需要提供尺寸和形状的应用环境，多个超声波传感器不能同时同方向近距离使用。

2.红外测距传感器

红外测距传感器是利用红外线反射特性制成的测距传感器。红外线属于光波的一种，也有其特殊性，具有光波反射传输的物理性质。红外测距通过信号发射源发射信息并反馈回来得到距离信息。由于红外测距具有测量精确度高、速度快等优势，所以红外测距技术被广泛应用于智能小车自动避障、地形测量、坦克飞机等对打击物的测量、通信卫星测量等领域。

2.1红外测距传感器的原理

红外测距是利用三角测量原理实现测距的。图2.3为红外测距的原理图，首先红外发射器发射红外光束，发射的红外光速是成一定角度的，当红外光束碰到物体时，会被物体反射回来，CCD检测器能够检测到返回的光束，而获得偏移值，利用三角定理，可得到如公式（2.2）的几何关系式。

其中，D为红外传感器到日标物体的距离，f为滤镜的焦距，L为偏移值，X为中心距，a表示发射角，c表示红外线在空气中的传播速度（c=3*108m/s），红外测距的原理如图2.3所示，

[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-ngXLt5On-1654962524712)(https://raw.githubusercontent.com/xkyvvv/blogpic3/main/img/202206112049313.png)]

由图23可知，当目标物体距离红外传感器D足够小时，偏移值L会非常的大，甚至大于CCD检测器的检测最大值。同样当D值很大时，L则很小。也就是说是否能够获得偏移值L的关键在于滤镜的焦距。

2.2红外测距传感器测距的优缺点

利用红外测距传感器测量距离具有的优点如下：

（1）抗干扰能力强。同时同方向使用多个红外传感器时不会相互干扰。

（2）测量精度高。红外测距传感器与超声波测距传感器相比，具有测量精度的优势。

（3）测距速度快。由于光速比声音在空气中传播的速度快，所以利用红外光线的反射原理制成的红外测距传感器比利用声波反射原理制成的超声波测距传感器测距，反应时间短。

（4）适用于长距离测距。由于红外线在空气中传播具有不扩散的性质，并且它在穿透物体时对折射率的影响不大，所以红外测距经常被用在距离较大的测量任务中。

（5）具有更强的发射和接收能力。因为自然界中只要温度高于绝对零度的物体都会反射红外线，所以利用红外反射原理制成的测距传感器发射和接收能力优越。

红外测距传感器测距的主要缺点是强光直射的环境会对测量精度造成影响。

3.激光雷达测距传感器

市面上有多种类型的激光传感器，从测距原理上大致可以分为四类：脉冲法，相位法，干涉法和三角法。其中前两种方法都是根据激光的飞行时间来换算为距离。干涉法是通过的相干光源产生的干涉现象中的干涉条纹数量来计算距离。三角法则是通过三角关系来计算距离。三角测距雷达的结构如下图2-7所示：

[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-STX4TUgj-1654962524712)(https://raw.githubusercontent.com/xkyvvv/blogpic3/main/img/202206112100595.png)]

式中的f和s己知，则可以通过像素点的位置来计算出该点的距离值q。

[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-dbBr69S3-1654962524712)(https://raw.githubusercontent.com/xkyvvv/blogpic3/main/img/202206112100122.png)]

上式对x求导，可以得到：

[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-3RGAnCQA-1654962524713)(https://raw.githubusercontent.com/xkyvvv/blogpic3/main/img/202206112101906.png)]

从上式可以得到，三角测距原理的激光雷达的测量噪声会随着测量距离的增加而增加，噪声水平与距离平方成正比。三角测距雷达的原理简单，成本低廉，但是测量精度会随着测量距离而降低，因此，三角测距原理的激光雷达适用于中短距离测量。市面上的低成本商用激光雷达多采用此方案。将三角测距的模块在一个平面中进行回转，就可以生成周围场景的平面扫描图。激光雷达实物图如图2-8所示。

[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-BmDRhyHI-1654962524713)(https://raw.githubusercontent.com/xkyvvv/blogpic3/main/img/202206112102687.png)]

图2-8（a）基于三角测距原理的激光雷达测距范围多在20米以内，而（b）中的雷达测距范围可达100米甚至更远。前者的数据采样频率大约为4000Hz，后者的采样频率可达300000Hz。当然前者的售价也为大大低于后者。

4.相机

在使用相机之前，先要对相机进行标定。其目的是建立起世界坐标中，相机坐标系和图像坐标系这三者之间的转换关系。首先介绍相机坐标系与图像坐标系之间的关系。

像素坐标系是一个二维坐标系，其一般定义是以图像的左上角为原点O，水平向右为u轴正方向，垂直向下为y轴正方向。

图像坐标系是一个二维坐标系，其一般定义是以光轴和像平面的交点为原点O1，水平向右为x轴正方向，垂直向下为y轴正方向。单位为物理单位，比如：米。

相机坐标系是一个三维坐标系（Xc,Yc,Zc），其一般定义是以相机的光心为原点Oc, Zc轴和相机的光轴重合，Xc轴和Y。轴分别平行于图像坐标系的和v轴。

相机坐标系和像素坐标系如下图2-4（a）所示，像素坐标和图像坐标系的关系如下图2-4（b）所示：

[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-zUPbIiDp-1654962524713)(https://raw.githubusercontent.com/xkyvvv/blogpic3/main/img/202206112107503.png)]

上图2-4中，（o,vo）为相机坐标系的Z轴和图像平面的交点，称为主点，以像素为单位。

在已知像素的物理尺寸后，可以建立起图像坐标系中的点和像素坐标系中的点的对应关系。图像坐标系下的点（x,y）变换到像素坐标系下（u,y）点的公式如下式（2-5）、（2-6）

所示。

[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-6pC8pMaG-1654962524714)(https://raw.githubusercontent.com/xkyvvv/blogpic3/main/img/202206112108150.png)]

上式（2-5）中中，dx表示x轴方向上像素的物理尺寸，dy表示y轴方向上像素的物理尺寸。uo,Vo为主点的像素坐标将上式写作齐次形式可以得式C（2-7）

[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-SeC4Lj9i-1654962524714)(https://raw.githubusercontent.com/xkyvvv/blogpic3/main/img/202206112108154.png)]

相机坐标系下的点坐标和像素坐标系下的点的坐标变化关系称为相机内部参数。这个参数需要通过标定得到。考虑小孔成像模型，忽略畸变，通过相似三角形并进行一定的简化之后可以得到这样的齐次变换式，如式（2-8）所示，变换的示意图如图2-5所示。

[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-mrTWiDwG-1654962524714)(https://raw.githubusercontent.com/xkyvvv/blogpic3/main/img/202206112109099.png)]

上式中，f为镜头的焦距，（Xc,Yc,Zc）为相机坐标系下的一个三维点。

世界坐标系用于表示空间点的物理位置。可以通过一个旋转平移矩阵来表示世界坐标系和相机坐标系之间的位姿关系。这个旋转平移矩阵也称作相机外部参数。这个变换如图2-6所示。

[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-1rvzwRya-1654962524714)(https://raw.githubusercontent.com/xkyvvv/blogpic3/main/img/202206112110346.png)]

在得到旋转和平移矩阵之后即可将世界坐标系下的点坐标转换到像素坐标系中。转换式如式（2-9）所示。

[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-YrHoxh44-1654962524715)(https://raw.githubusercontent.com/xkyvvv/blogpic3/main/img/202206112111018.png)]

上式（2-9）中，K为相机内参矩阵，P为包含了内部参数和外部参数的投影矩阵。R为33的三维旋转矩阵，T为31的平移矩阵。

5.传感器的选择方案

为了给智能小车环境信息采集系统选择具体的测距传感器，对测距传感器提出下列要求：

（1）满足功能：即能够测距，且输入、输出接口符合Arduino要求。

（2）满足性能：即满足环境信息采集系统的性能指标要求。

（3）经济性：在功能和性能都满足的前提下，测距传感器的价格越低越好。

下面列举的是测距传感器的主要性能指标

（1）测量精度：由于布置在车头位置的测距传感器，容易受到智能小车运动的影响，所以要求精度为1.由于车体侧面发生碰撞的可能性比较小，所以布置在车体两侧的测距传感器精度要求为10mm即可。

（2）测量范围：两种传感器的最小测量值要求在0.5倍的安全距离（安全距离见3.2节）以内，即为0~150mm。

（3）工作温度，耐腐蚀性以及光亮度：传感器须满足智能小车工作环境的中温度，腐蚀度以及光亮度的要求。其中温度范围要求为0~50°，耐腐蚀性要求为适用于中性环境即可，亮度要求为适应环境光，智能小车运动中没有强光直射车头。

五、姿态传感器种类

飞行器若想平稳飞行，必须依靠姿态检测模块对其进行实时的姿态检测，将检测到的数据实时反馈给主控芯片，由主控芯片对其进行分析并实时调整飞行器各个旋翼的转速以使飞行器按正确的姿态平稳飞行。

常见的姿态检测器件有加速度计、陀螺仪、磁力计、超声波传感器、气压传感器以及GPS等；在当前的姿态检测研究当中，囊括了以上传感器的所有数据较是较为复杂的融合表示方法，这种方法可以获得飞行器较准确的飞行姿态；可是针对飞行器飞行系统的应用，这种复杂的基于多传感器数据融合算法会受到嵌入式平台运算能力的限制，所以我们需要选择一种轻量而且廉价的惯性测量单元（IMUU8））用来输出飞行器的姿态信息；本设计中采用只使用角速度和加速度两种传感器来做为姿态测量器件，其相关的知识和原理详见如下介绍。

1.加速度计

加速度计是一种用来测量加速力的传感器，加速力是在物体在加速过程中作用在物体上的力，例如地球引力，也就是重力；我们可以通过测量由重力引起的加速度，从而计算出设备相对于水平面的倾斜角度。

在三维立体空间中，利用重力和传感器的关系，可以分析得到如图3.5所示的三个姿态角p（pitch）、p（roll）、日（yaw）：X轴相对于地面的角度是p、是Y轴相对于地面的角度是φ、Z轴相对于重力的夹角是0。

[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-qOLCNU5E-1654962524715)(https://raw.githubusercontent.com/xkyvvv/blogpic3/main/img/202206112125168.png)]

因此可以推导出：

[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-mImscP2p-1654962524715)(https://raw.githubusercontent.com/xkyvvv/blogpic3/main/img/202206112125749.png)]

其中：

ax是重力在X轴方向产生的加速度

ay是重力在Y轴方向产生的加速度

az是重力在Z轴方向产生的加速度

2.陀螺仪

陀螺仪（角速度的传感器）是一种以角动量的守恒为工作原理的传感器，它是一个用于维持方向以及测量角度的设备，通常的陀螺仪分为Gimbal陀螺仪及MEMS陀螺仪两种类型。

[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-OpycytRh-1654962524715)(https://raw.githubusercontent.com/xkyvvv/blogpic3/main/img/202206112126499.png)]

（1）Gimbal2型陀螺仪

如图3.4（A）所示的是传统的Gimba陀螺仪，其中在外面的三个环称作平衡环，而中间的是一个转子，它是在不停旋转的所以由于惯性的作用不会受到外界的影响，然而外面的三个环却会因为设备姿态的改变而跟着改变，这样就可以以此来检测所测物体的当前状态，陀螺仪里面的X、Y、Z三个轴就是外面的这三个环所在的轴；但是Gimbal陀螺仪有一个万向节锁24，即指当三个万向节中的两个轴发生重合的时候，会失去一个自由度的情况。为了消除这一个缺陷，一般的应用中就需要使用更多更昂贵的机械部件来进行协助补偿。

（b）MEMS陀螺仪

MEMS陀螺仪相比于体积较大的Gimbal陀螺仪有着更大的优势；如图3.4-B中的旋转盘，在直线0A上的两个点B和C，当磁盘顺时针旋转时，B位置的点会移动到C的位置，虽然这两个点的角度率是相同的，但是在C点的速度是比B点快，这说明存在一个作用力引起了这个速度的改变，而这个力被称为科里奥利力；MEMS陀螺仪的工作原理就是通过利用科里奥利力来对物体的角速度进行测量的。

（3）传感器选择

目前MEMS陀螺仪与加速度传感器由于其体积和价格上的优势，被广泛应用于电子设备中进行姿态检测，故本设计采用MEMS陀螺仪与加速度传感器，在选择传感器类型时需要综合考虑到器件的量程、灵敏度及供电电压，表3.1和表3.2列出了目前市场上主流的3轴MMS陀螺仪和加速度传感器。

[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-DS53m066-1654962524716)(https://raw.githubusercontent.com/xkyvvv/blogpic3/main/img/202206112129481.png)]

通过上表参数，考虑到重量等各方面因素，本设计选取了加速度和角速度一体的MUP-6050芯片作为飞行器的姿态控制传感器。

---

本文采用的PU6500集成了三轴MEMS数字陀螺仪、数字加速度计，可以测量多旋翼无人飞行器运动状态下的三轴角速率®，0,0：、三轴加速度Q,a,a。陀螺仪测量的角速度数据动态响应速度快，长期使用时，存在较大的积分误差，因此，陀螺仪无法单独使用。加速度计是一种长期稳定的惯性器件。

解算姿态时，通过融合珈速度计与陀螺仪的数据，能够得到稳定的姿态角信息；计算速度和位置信息时，通过加速度计积分获得相应的速度与位置。

3.磁力计

三轴磁力计通过磁敏器件，获取地磁场在机体坐标系下的三维磁场矢量投影。通过磁力计对航向角解算时，无需进行积分运算，因此它是一种长期稳定的器件。

4.气压计

所采用的M$5611气压高度计可以获得高度信息。气压计通过测量大气压的值能够计算当前海拔高度。其中，气压与高度的关系如下所示：

式中，R代表大气温度下限值、g代表重力加速度，P，为当前高度测得的大气静压，T6P,h，代表当前大气压的大气温度下限值、大气压力和重力势高度，B为温度垂直变化率。通过上述公式，能够计算得到当前高度。

5.GPS

GPS能够测量三轴速度、经度、维度以及海拔高度。在开阔的户外环境中，GPS能够达到2.5米的位置定位精度（采用差分GPS甚至可达到厘米级的定位精度），速度测量精度能够达到约0.1m/s。然而，GPS的海拔高度以及竖直方向速度测量精度较低，且附近有遮挡物时，由于信号折射等原因，GPS的位置定位精度会大幅下降，但速度测量精度影响较小。

综上所述，陀螺仪测量得到的三轴角速度数据通过积分能够得到短时精度较高的姿态角。然而，随着时间的推移，由于陀螺仪传感器存在零偏、随机游走以及温漂等误差存在，导致了其长期姿态角解算精度较低。因此，本文将加速度计、磁力计传感器测量得到的数据与陀螺仪测量得到的数据进行融合，进行姿态解算，从而得到长期稳定的姿态角信息。另外，GP$虽然能够测量速度、位置信息，但其测量精度与工作环境相关，存在延时大，输出频率低，信号不稳定的问题。此外，气压计虽然能够检测海拔高度，但由于大气易受干扰，短时间内无法反映真实的海拔高度。综上所述，四旋翼飞行器所配置的每一个传感器都存在一定的局限性，单一的传感器测量数据都无法满足无人机的导航精度要求，因此，需要进行优势互补，以得到长期解算稳定，短期解算精度高的目的。

六、传感器数据融合

多传感器信息融合（Multi-sensor Information Fusion,MSIF），就是利用计算机技术将来自多传感器或多源的信息和数据，在一定的准则下加以自动分析和综合，以完成所需要的决策和估计而进行的信息处理过程。

1.多传感器融合几个概念

硬件同步、硬同步：使用同一种硬件同时发布触发采集命令，实现各传感器采集、测量的时间同步。做到同一时刻采集相同的信息。

软件同步：时间同步、空间同步。

时间同步、时间戳同步、软同步：通过统一的主机给各个传感器提供基准时间，各传感器根据已经校准后的各自时间为各自独立采集的数据加上时间戳信息，可以做到所有传感器时间戳同步，但由于各个传感器各自采集周期相互独立，无法保证同一时刻采集相同的信息。

空间同步： 将不同传感器坐标系的测量值转换到同一个坐标系中，其中激光传感器在高速移动的情况下需要考虑当前速度下的帧内位移校准。

[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-Jt5ZYN3K-1654962524716)(https://raw.githubusercontent.com/xkyvvv/blogpic3/main/img/202206112305811.png)]

2.基本原理

多传感器融合基本原理就像人脑综合处理信息的过程一样，将各种传感器进行多层次、多空间的信息互补和优化组合处理，最终产生对观测环境的一致性解释。在这个过程中要充分利用多源数据进行合理支配与使用，而信息融合的最终目标则是基于各传感器获得的分离观测信息，通过对信息多级别、多方面组合导出更多有用信息。这不仅是利用了多个传感器相互协同操作的优势，而且也综合处理了其它信息源的数据来提高整个传感器系统的智能化。

具体来讲，多传感器数据融合原理如下：

（1）多个不同类型传感器（有源或无源）收集观测目标的数据；

（2）对传感器的输出数据（离散或连续的时间函数数据、输出矢量、成像数据或一个直接的属性说明）进行特征提取的变换，提取代表观测数据的特征矢量Yi；

（3）对特征矢量Yi进行模式识别处理（如聚类算法、自适应神经网络或其他能将特征矢量Yi变换成目标属性判决的统计模式识别法等），完成各传感器关于目标的说明；

（4）将各传感器关于目标的说明数据按同一目标进行分组，即关联；

（5）利用融合算法将目标的各传感器数据进行合成，得到该目标的一致性解释与描述。

3.多传感器的前融合与后融合技术

3.1后融合算法：

每个传感器各自独立处理生成的目标数据。
每个传感器都有自己独立的感知，比如激光雷达有激光雷达的感知，摄像头有摄像头的感知，毫米波雷达也会做出自己的感知。
当所有传感器完成目标数据生成后，再由主处理器进行数据融合。

3.2前融合算法：

只有一个感知的算法。对融合后的多维综合数据进行感知。
在原始层把数据都融合在一起，融合好的数据就好比是一个Super传感器，而且这个传感器不仅有能力可以看到红外线，还有能力可以看到摄像头或者RGB，也有能力看到LiDAR的三维信息，就好比是一双超级眼睛。在这双超级眼睛上面，开发自己的感知算法，最后会输出一个结果层的物体。

[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-D97VlAWg-1654962524717)(https://raw.githubusercontent.com/xkyvvv/blogpic3/main/img/202206112305779.png)]

4.融合算法

对于多传感器系统而言，信息具有多样性和复杂性，因此对信息融合算法的基本要求是具有鲁棒性和并行处理能力。其他要求还有算法的运算速度和精度；与前续预处理系统和后续信息识别系统的接口性能；与不同技术和方法的协调能力；对信息样本的要求等。一般情况下，基于非线性的数学方法，如果具有容错性、自适应性、联想记忆和并行处理能力，则都可以用来作为融合方法。

多传感器数据融合的常用方法基本上可分为两大类：随机类和人工智能类。

4.1随机类

（1）加权平均法

信号级融合方法最简单直观的方法是加权平均法，将一组传感器提供的冗余信息进行加权平均，结果作为融合值。该方法是一种直接对数据源进行操作的方法。

（2）卡尔曼滤波法

主要用于融合低层次实时动态多传感器冗余数据。该方法用测量模型的统计特性递推，决定统计意义下的最优融合和数据估计。如果系统具有线性动力学模型，且系统与传感器的误差符合高斯白噪声模型，则卡尔曼滤波将为融合数据提供唯一统计意义下的最优估计。

卡尔曼滤波的递推特性使系统处理无需大量的数据存储和计算。但是采用单一的卡尔曼滤波器对多传感器组合系统进行数据统计时，存在很多严重问题，例如：① 在组合信息大量冗余情况下，计算量将以滤波器维数的三次方剧增，实时性难以满足。② 传感器子系统的增加使故障概率增加，在某一系统出现故障而没有来得及被检测出时，故障会污染整个系统，使可靠性降低。

（3）多贝叶斯估计法

将每一个传感器作为一个贝叶斯估计，把各单独物体的关联概率分布合成一个联合的后验概率分布函数，通过使联合分布函数的似然函数为最小，提供多传感器信息的最终融合值，融合信息与环境的一个先验模型以提供整个环境的一个特征描述。

（4）D-S证据推理法

该方法是贝叶斯推理的扩充，包含3个基本要点：基本概率赋值函数、信任函数和似然函数。

D-S方法的推理结构是自上而下的，分为三级：第一级为目标合成，其作用是把来自独立传感器的观测结果合成为一个总的输出结果（ID）；第二级为推断，其作用是获得传感器的观测结果并进行推断，将传感器观测结果扩展成目标报告。这种推理的基础是：一定的传感器报告以某种可信度在逻辑上会产生可信的某些目标报告；第三级为更新，各传感器一般都存在随机误差，因此在时间上充分独立地来自同一传感器的一组连续报告比任何单一报告更加可靠。所以在推理和多传感器合成之前，要先组合（更新）传感器的观测数据。

（5）产生式规则

采用符号表示目标特征和相应传感器信息之间的联系，与每一个规则相联系的置信因子表示它的不确定性程度。当在同一个逻辑推理过程中，2个或多个规则形成一个联合规则时，可以产生融合。应用产生式规则进行融合的主要问题是每个规则置信因子的定义与系统中其他规则的置信因子相关，如果系统中引入新的传感器，需要加入相应的附加规则。

4.2.AI类

（1）模糊逻辑推理

模糊逻辑是多值逻辑，通过指定一个0到1之间的实数表示真实度（相当于隐含算子的前提），允许将多个传感器信息融合过程中的不确定性直接表示在推理过程中。如果采用某种系统化的方法对融合过程中的不确定性进行推理建模，则可以产生一致性模糊推理。

与概率统计方法相比，逻辑推理存在许多优点，它在一定程度上克服了概率论所面临的问题，对信息的表示和处理更加接近人类的思维方式，一般比较适合于在高层次上的应用（如决策）。但是逻辑推理本身还不够成熟和系统化。此外由于逻辑推理对信息的描述存在很多的主观因素，所以信息的表示和处理缺乏客观性。

模糊集合理论对于数据融合的实际价值在于它外延到模糊逻辑，模糊逻辑是一种多值逻辑，隶属度可视为一个数据真值的不精确表示。在MSF过程中，存在的不确定性可以直接用模糊逻辑表示，然后使用多值逻辑推理，根据模糊集合理论的各种演算对各种命题进行合并，进而实现数据融合。

（2）人工神经网络法

神经网络具有很强的容错性以及自学习、自组织及自适应能力，能够模拟复杂的非线性映射。神经网络的这些特性和强大的非线性处理能力，恰好满足多传感器数据融合技术处理的要求。在多传感器系统中，各信息源所提供的环境信息都具有一定程度的不确定性，对这些不确定信息的融合过程实际上是一个不确定性推理过程。神经网络根据当前系统所接受的样本相似性确定分类标准，这种确定方法主要表现在网络的权值分布上，同时可以采用学习算法来获取知识，得到不确定性推理机制。利用神经网络的信号处理能力和自动推理功能，即实现了多传感器数据融合。

再推荐几篇比较好的文章：

传感器融合是什么？及其类型和应用

多传感器融合（算法）综述

---

七、姿态解算，控制律运算

1.姿态解算

姿态解算(attitude algorithm),是指把陀螺仪，加速度计, 罗盘等的数据融合在一起，得出飞行器的空中姿态，飞行器从陀螺仪器的三轴角速度通过四元数法得到俯仰，航偏，滚转角，这是快速解算，结合三轴地磁和三轴加速度得到漂移补偿和深度解算。

2.姿态的数学模型坐标系

姿态解算需要解决的是四轴飞行器和地球的相对姿态问题。地理坐标系是固定不变的，正北，正东，正上构成了坐标系的X，Y，Z轴用坐标系R表示，飞行器上固定一个坐标系用r表示，那么我们就可以适用欧拉角，四元数等来描述r和R的角位置关系。

[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-b57Df5dz-1654962524717)(https://ask.qcloudimg.com/http-save/yehe-1637364/27x4t51tdw.jpeg?imageView2/2/w/1620)]

姿态的数学表示

姿态有多种数学表示方式，常见的是四元数，欧拉角，矩阵和轴角。在四轴飞行器中使用到了四元数和欧拉角,姿态解算的核心在于旋转。姿态解算中使用四元数来保存飞行器的姿态，包括旋转和方位。在获得四元数之后，会将其转化为欧拉角，然后输入到姿态控制算法中。姿态控制算法的输入参数必须要是欧拉角。AD值是指MPU6050的陀螺仪和加速度值，3个维度的陀螺仪值和3个维度的加速度值，每个值为16位精度。AD值必须先转化为四元数，然后通过四元数转化为欧拉角。在四轴上控制流程如下图：

下面是用四元数表示飞行姿态的数学公式，从MPU6050中采集的数据经过下面的公式计算就可以转换成欧拉角，传给姿态PID控制器中进行姿态控制.

[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-LUNv1YtA-1654962524717)(https://raw.githubusercontent.com/xkyvvv/blogpic3/main/img/202206112316283.jpeg)]

3.PID控制算法

先简单说明下四轴飞行器是如何飞行的,四轴飞行器的螺旋桨与空气发生相对运动，产生了向上的升力，当升力大于四轴的重力时四轴就可以起飞了。四轴飞行器飞行过程中如何保持水平：我们先假设一种理想状况：四个电机的转速是完全相同的是不是我们控制四轴飞行器的四个电机保持同样的转速，当转速超过一个临界点时（升力刚好抵消重力）四轴就可以平稳的飞起来了呢？答案是否定的，由于四个电机转向相同，四轴会发生旋转。我们控制四轴电机1和电机3同向，电机2电机4反向，刚好抵消反扭矩，巧妙的实现了平衡, 但是实际上由于电机和螺旋浆本身的差异，造成我们无法做到四个电机产生相同的升力，这样飞行器起飞之后就会失去平衡。

PID控制器

PID控制器通过PID三个参数来对被控对象进行控制，是应用最广泛一种的控制器

比例§控制器

比例控制是一种最简单的控制方式。其控制器的输出与输入误差信号成比例关系。当仅有比例控制时系统输出存在稳态误差。

积分(I)控制器

在积分控制中，控制器的输出与输入误差信号的积分成正比关系。对一个自动控制系统，如果在进入稳态后存在稳态误差，则称这个控制系统是有稳态误差的 或简称有差系统（System with Steady-state Error）。为了消除稳态误差，在控制器中必须引入“积分项”。积分项对误差取决于时间的积分，随着时间的增加，积分项会增大。这样，即便误差很小，积 分项也会随着时间的增加而加大，它推动控制器的输出增大使稳态误差进一步减小，直到等于零。因此，比例+积分(PI)控制器，可以使系统在进入稳态后无稳态误差。

微分(D)控制器

微分调节就是偏差值的变化率。使用微分环节能够实现系统的超前控制。如果输入偏差值线性变化，则在调节器输出侧叠加一个恒定的调节量。大部分控制系统不需要调节微分时间。因为只有时间滞后的系统才需要附加这个参数。如果画蛇添足加上这个参数反而会使系统的控制受到影响。

四轴飞行器中的控制器

目前四轴飞行器中所使用的是增量是PD控制器，下面以ROLL方向为例

测得ROLL轴向偏差

偏差 = 目标期望角度 – 传感器实测角度

DIF_ANGLE.X = EXP_ANGLE.X - Q_ANGLE.Roll;

复制

比例项计算

比例项输出 = 比例系数P * 偏差

Proportion = PID_Motor.P * DIF_ANGLE.x;

复制

微分计算

微分输出 = 微分系数 × 角速度

整合结果

ROLL方向总控制量 = 比例项输出+微分量输出

电机输出

Motor[2] = (int16_t)(Thr - Pitch - Roll - Yaw );    //M3
Motor[0] = (int16_t)(Thr + Pitch + Roll - Yaw );    //M1
Motor[3] = (int16_t)(Thr - Pitch + Roll + Yaw );    //M4
Motor[1] = (int16_t)(Thr + Pitch - Roll + Yaw );    //M2

复制

旋转规则

• Roll方向旋转，则电机1电机2同侧出力，电机0电机3反向出力
• Pitch方向旋转，则电机2电机3同侧出力，电机0电机1反向出力
• Yaw方向旋转，则电机1电机3同侧出力，电机0电机2反向出力

---

现在由于各种公众号和百家号的盛行（无贬义），越来越多的军事爱好者能接触到一些更专业的知识，至少是更专业的名词，比如飞控，以个人观点看飞控已经基本脱离一般爱好者的爱好范畴进入到了工作领域。。。不过既然上篇文章说到抽时间可能会写一下飞控，今天是周末，我就随便闲扯两句飞控的控制律。

4.飞控是什么

飞控全名飞行控制系统，顾名思义，是个综合系统，包括软件与硬件部分。硬件部分好理解，最中心的是个飞控计算机（可以是实际的CPU+外围设备比如内存，比如国内某无人机使用的是powerPC系CPU；也可以是模拟电路，这个在几十年前非常常见，因为处理器性能不够，或者处理器压根就没发明出来）；在飞机的各个部位则放置了各种传感器，比如测加速的的加速度计，测空速的空速管等等；最外面是执行机构，比如控制舵面的舵机就是用来调节飞机舵面偏角的。

软件部分则比较复杂，包括飞行员输入指令处理，控制律计算，容错处理等等，无人机的话还要考虑地面站通信等等，由于飞控软件功能很多，一般会利用一个实时操作系统保证运行的可靠（我们当年给固定翼无人机用的是vxworks，小飞机的话用不到这么高端昂贵的玩意，记得当时有很多课题组流行用ucosII做四旋翼飞控开发，现在不了解行情了）；飞控软件中最核心的任务是控制律计算，飞控计算机根据各个传感器回传的数值首先进行解算，比如当前姿态角是多少，然后根据预先编好的控制律进行运算，计算得出飞机各个舵面偏角后发送到执行机构（舵机），然后就能看见飞机的舵面发生了偏转，完成飞行员期望的动作。

5.控制律是什么

控制律实质上是一组公式，

u=f(x,r)

其中x是系统状态，一般来自传感器反馈，比如x可以是当前飞机姿态角、飞机速度、飞机高度等等；r叫做参考信号，可以来自飞行员，也可以来自自动驾驶仪，控制律就是f()，本质上是一组数学公式，根据实时的x和r来计算得出u，这里的u就是控制器输出，比如舵面偏角，然后计算机通过数据总线发送u到各个执行机构完成本拍（数字控制里面通常把一个控制周期叫一拍）的控制，到了下个周期（下一拍），继续这个流程完成下个周期的控制。换句话说，飞行员开的实际上是个“虚拟机”，现代飞控系统下飞行员的杆量并不意味着舵面偏了这么多角，反过来，飞行员保持操纵杆中立也不意味着舵面没有发生动作，这一切都是为了降低飞行员操作的难度，保证飞行安全。

举例：最简单的线性控制是u=Kx，K是常系数，比如这个是飞机的纵向控制律，x是飞机当前迎角，输出的u是升降舵偏角，假设K=0.5，那就意味着如果反馈攻角是10度的话操纵升降舵偏角到5度；一般来说这种简单粗暴控制的结果就是控制发散，机毁人亡。。。所以这就涉及到下一个话题，怎么设计控制律

6.控制律怎么设计

在控制理论中，控制的核心问题是稳定性，稳定性一个通俗但不严谨的表述是系统受到扰动后会自发回到平衡状态，如果系统本身不稳定，那么就需要设计控制律构造一个新系统=老系统+反馈控制律，控制律需要保证这个新系统是稳定的。控制理论自从1940s创立，发展到今天有了非常多种稳定性分析方法，但最最重要的理论是1890s由俄国力学家李雅普诺夫创立的，具体细节就不讲了，有兴趣的话可以参考百度百科或wiki。但不管根据哪种分析方法，都需要知道系统的数学模型，在飞控设计中最常见的是飞行器的六自由度（6DOF）模型，六个自由度通常认为是飞行器的质心位移自由度（3个）和绕质心旋转的自由度（也是3个）。6DOF模型说白了是一堆微分方程，根据牛顿力学和空气力学建立出来的，用来描述飞行器各个舵面偏角和推力到姿态、速度、高度、角度的关系。

教科书上提供的标准6DOF模型

可以看出来，就算是最简单的6DOF模型已经相当复杂了，里面各个变量相互耦合（比如偏航和滚转这两个通道会干扰到对方）、非线性（各种三角函数）、再考虑到大气温度和密度的变化实际上里面的各项系数并非常数。。。。

为了方便飞控开发，工程中一个常见方法是首先把非线性的6DOF模型线性化，再利用线性控制理论进行设计（主要是非线性控制理论本身就很魔幻，相反，线性控制发展到今天已经比较成熟了）。线性化中最常见的做法是把这一大堆非线性方程在一个工作点（比如飞机高度10000ft，速度0.6M）附近做小信号处理，即认为在该工作点附近6DOF模型是线性的，然后得出雅可比（Jacobian）矩阵，那么在设计时就认为这个Jacobian矩阵就代表了飞行器的动力学模型。

[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-UqypSwRj-1654962524719)(https://raw.githubusercontent.com/xkyvvv/blogpic3/main/img/202206112321921.webp)]F-16的Jacobian矩阵，虽然线性化了依然复杂到爆

得出了小信号后，飞机的模型就变成了

dx/dt=Ax(t)+Bu(t)

y(t)=Cx(t)+Du(t)

ABCD就是图里面的四个矩阵，从图上可以看出F-16模型的输入量有四个，分别是\delta_{th}：推力; \delta_{e}：升降舵偏角；\delta_{a}：副翼偏角；\delta_{r}：方向舵偏角。系统状态x则是一堆动力学变量，比如\alpha是飞机攻角；而系统输出y则是飞机能“看到”的变化，比如攻角\alpha同样也是飞机输出。

获得了模型的下一步就是控制律开发，同样是以线性控制律u=Kx为例，带入到原系统后系统方程转写为

dx/dt=Ax(t)+Bu(t)=Ax(t)+BKx(t)=(A+BK)x(t)

y(t)=Cx(t)+Du(t)

那么我们设计K就要保证这个“新系统” dx/dt=(A+BK)x是稳定的，同时有较快的响应速度，尽量不要有超调和震荡；根据线性系统理论，系统稳定的需求是矩阵A+BK所有特征根在复平面的左半平面（或者说A+BK是Hurwitz的），或者说所有特征根的实部一定要小于0；对震荡则是尽量压低特征根的虚部模值，如果特征根没有虚部那么系统的相应也不会出现任何震荡，虽然可能会牺牲一定的响应速度但至少飞机不会进入帕金森状态。。。

7.飞机模型是怎么得到的

简单回答就是吹风洞，把所有可能的工况（各种高度速度的组合）全吹一遍。。。从大量的风洞数据中可以得到各项气动导数，再通过一定的换算便可以得出飞行器6DOF模型（非线性版），接着做线性化和控制律设计

这一项说起来简单，但实际操作成本非常高，需要大量的人力物力和时间才能得到一个飞机比较详尽的模型

8.控制律怎么验证

利用同样的设计流程，一般来说你会得到一组矩阵K，那么下一步就是验证控制律，得益于数值仿真技术的发展，飞控开发人员不需要把程序烧进真飞机里面做测试，不然随便摔几架就坐上去古拉格的专车了。。。上古时代开发人员往往利用Fortran自行编写仿真程序，本身这门语言也够上古的了，有兴趣的可以搜一下，现在则有了各项专用软件可以有效提高开发效率，比如做控制开发基本离不开MATLAB，在Simulink环境下也很容易实现飞行器数值仿真，用Fortran的话甚至连数值算法都要自己写（可以简单理解为实现一个basic版Simulink，比如写个RK4啥的）（顺便说下Fortran至今在某些大学中依然是必修课，而且很多观点认为虽然Fortran不够傻瓜化，但就像手动挡一样，用的好的话比自动挡的MATLAB高端大气）

飞控开发流程

上图说明了飞控的基本迭代开发流程，在得到飞行器风洞数据后能够搭一个该款飞机仿真模型出来，然后开发人员完成设计后丢到仿真里面去跑，看看稳不稳定、性能好不好，再根据仿真结果重新调整控制律，直到满意后丢到真飞机上飞，再根据真实数据去做设计调整。所以一个飞机的飞控设计需要大量的风洞测试和试飞测试，通常时间是以年为单位的。

非常简单的飞行器俯仰增稳系统

9.目前的飞控发展

上面只是讲了一个基于线性化6DOF模型的最简单的控制设计流程，实际上6DOF模型本身就存在过于简单的问题，比如6DOF模型的假设前提是飞机是个刚体；同时线性化+特征根这种手段也显得比较简单粗暴，没有利用好控制理论近3、40年的发展成果，下面两张图就是一篇17年的文章，按照国内标准属于1区文章，虽然也是利用的线性化模型，但分析方法比较“时髦”（虽然在理论界也是落伍技术了），如果不去考虑一帮中国人去开发F-18的飞控是怎样的一种国际精神的话。。

17年论文的控制律，至少看起来不复杂

17年论文的稳定性分析，这才是文章大头

10.一些闲话

本文的主要写作时间是在思考怎么把这么一坨东西尽量通俗易懂的讲出来，算是提供一些冷知识给感兴趣的朋友们。同时文章也算是对我过去多年工作的一个小结，现在我已经脱离了飞控这个行业，不过回忆起当年刚进lab时和师兄一起约风洞，处理数据，跑仿真做实验写paper的日子也是非常开心，感谢他们当年领我入门，祝他们在各自的行业中取得更大辉煌。

a). 本文的图片大部分来自 1992年版，这本书也是我读的第一本专业飞控书，写的比较详尽，照顾初学者，但要求读者至少掌握线性系统理论的基础知识。

b). 关于控制系统的知识可以参考本站up：DR_CAN的视频

---

八、避障常用算法

首先我们要区分避障算法和路径规划算法。在讲避障算法之前，我们假定机器人已经有了一个导航规划算法对自己的运动进行规划，并按照规划的路径行走。避障算法的任务就是在机器人执行正常行走任务的时候，由于传感器的输入感知到了障碍物的存在，实时地更新目标轨迹，绕过障碍物。

1.Bug算法

Bug算法应该是最简单的一种避障算法了，它的基本思想是在发现障碍后，围着检测到的障碍物轮廓行走，从而绕开它。Bug算法目前有很多变种， 比如Bug1算法，机器人首先完全地围绕物体，然后从距目标最短距离的点离开。Bug1算法的效率很低，但可以保证机器人达到目标。

[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-uax3oAwp-1654962524720)(https://raw.githubusercontent.com/xkyvvv/blogpic3/main/img/202206112201110.png)]

Bug1算法示例

改进后的Bug2算法中，机器人开始时会跟踪物体的轮廓，但不会完全围绕物体一圈，当机器人可以直接移动至目标时，就可以直接从障碍分离，这样可以达到比较短的机器人行走总路径。

[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-jvJBcVI7-1654962524720)([外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-API8Is8v-1654962597688)(https://raw.githubusercontent.com/xkyvvv/blogpic3/main/img/202206112201218.png#pic_center)]
[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-MawuOWN7-1654962591798)(https://raw.githubusercontent.com/xkyvvv/blogpic3/main/img/202206112201218.png#pic_center)]
[外链图片转存中…(img-lTNuLk3k-1654962587599)]
[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-4bVKrXpM-1654962582894)(https://raw.githubusercontent.com/xkyvvv/blogpic3/main/img/202206112201218.png#pic_center)]
[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-6OQ9ptVN-1654962577569)(https://raw.githubusercontent.com/xkyvvv/blogpic3/main/img/202206112201218.png#pic_center)]
)]

Bug2算法示例

除此之外，Bug算法还有很多其他的变种， 比如正切Bug算法等等。在许多简单的场景中，Bug算法是实现起来比较容易和方便的，但是它们并没有考虑到机器人的动力学等限制，因此在更复杂的实际环境中就不是那么可靠好用了。

2.势场法（PFM）

人工势场避障控制法，是一种比较简单又新颖的做法，是另一种仿生学，仿照物理学中电势和电场力的概念，建立机器人工作空间中的虚拟势场，按照虚拟势场力方向，实现局部路径规划。通过构造目标位姿引力场和障碍物周围斥力场共同作用的人工势场，来搜索势函数的下降方向，然后寻找无碰撞路径。听起来很玄乎，但是早已经有应用产品了，Khatib曾应用于移动机器人的导航上。但是并没有得到大规模应用。因为即使对于简单环境很有效，但是都是在静态的研究中得出的，而没有考虑障碍物的速度和加速度的影响，所以在动态避障控制中，人工势场法避障控制不是很理想。因为在复杂的多障碍环境中，不合理的势场数学方程容易产生局部极值点，导致机器人未到达目标就停止运动，或者产生振荡、摆动等现象。另外，传统的人工势场法着眼于得到一条能够避障的可行路径，还没有研究出什么最优路径。

实际上，势场法不仅仅可以用来避障，还可以用来进行路径的规划。势场法把机器人处理在势场下的 一个点，随着势场而移动，目标表现为低谷值，即对机器人的吸引力，而障碍物扮演的势场中的一个高峰，即斥力，所有这些力迭加于机器人身上，平滑地引导机器人走向目标，同时避免碰撞已知的障碍物。当机器人移动过程中检测新的障碍物，则需要更新势场并重新规划。

[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-GY9Y68Iu-1654962524721)([外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-F4rtLuEw-1654962615385)(https://raw.githubusercontent.com/xkyvvv/blogpic3/main/img/202206112202782.png#pic_center)]
)]

上面这个图是势场比较典型的示例图，最上的图a左上角是出发点，右下角是目标点，中间三个方块是障碍物。中间的图b就是等势位图，图中的每条连续的线就代表了一个等势位的一条线，然后虚线表示的在整个势场里面所规划出来的一条路径，我们的机器人是沿着势场所指向的那个方向一直行走，可以看见它会绕过这个比较高的障碍物。最下面的图，即我们整个目标的吸引力还有我们所有障碍物产生的斥力最终形成的一个势场效果图，可以看到机器人从左上角的出发点出发，一路沿着势场下降的方向达到最终的目标点，而每个障碍物势场表现出在很高的平台，所以，它规划出来的路径是不会从这个障碍物上面走的。

一种扩展的方法在基本的势场上附加了?另外两个势场：转运势场和任务势场。它们额外考虑了由于机器人本身运动方向、运动速度等状态和障碍物之间的相互影响。

转动势场"：考虑了障碍与机器人的相对方位，当机器人朝着障碍物行走时，增加斥力， 而当平行于物体行走时，因为很明显并不会撞到障碍物，则减小斥力。任务势场：则排除了那些根据当前机器人速度不会对近期势能造成影响的障碍，因此允许规划出 一条更为平滑的轨迹。

另外还有谐波势场法等其他改进方法。势场法在理论上有诸多局限性， 比如局部最小点问题，或者震荡性的问题，但实际应用过程中效果还是不错的，实现起来也比较容易。

3.向量场直方图（VFH）

它执行过程中针对移动机器人当前周边环境创建了一个基于极坐标表示的局部地图，这个局部使用栅格图的表示方法，会被最近的一些传感器数据所更新。VFH算法产生的极坐标直方图如图所示：

[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-x3OOvYTu-1654962524721)(https://raw.githubusercontent.com/xkyvvv/blogpic3/main/img/202206112204261.png)]

图中x轴是以机器人为中心感知到的障碍物的角度，y轴表示在该方向存在障碍物的概率大小p。实际应用的过程中会根据这个直方图首先辨识出允许机器人通过的足够大的所有空隙，然后对所有这些空隙计算其代价函数，最终选择具有最低代价函数的通路通过。

代价函数受三个因素影响： 目标方向、机器人当前方向、之前选择的方向，最终生成的代价是这三个因素的加权值，通过调节不同的权重可以调整机器人的选择偏好。VFH算法也有其他的扩展和改进，比如在VFH+算法中，就考虑了机器人运动学的限制。由于实际底层运动结构的不同，机器的实际运动能力是受限的，比如汽车结构，就不能随心所欲地原地转向等。VFH+算法会考虑障碍物对机器人实际运动能力下轨迹的阻挡效应，屏蔽掉那些虽然没有被障碍物占据但由于其阻挡实际无法达到的运动轨迹。我们的E巡机器人采用的是两轮差动驱动的运动形式，运动非常灵活，实际应用较少受到这些因素的影响。

具体可以看 一下这个图示：

[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-IOoRFDD0-1654962524722)(https://raw.githubusercontent.com/xkyvvv/blogpic3/main/img/202206112204219.png)]

类似这样传统的避障方法还有很多，除此之外，还有许多其他的智能避障技术，比如神经网络、模糊逻辑等。

神经网络方法对机器人从初始位置到目标位置的整个行走路径进行训练建模，应用的时候，神经网络的输 入为之前机器人的位姿和速度以及传感器的输 入，输出期望的下一目标或运动方向。

模糊逻辑方法核心是模糊控制器，需要将专家的知识或操作人员的经验写成多条模糊逻辑语句，以此控制机器人的避障过程。 比如这样的模糊逻辑：第一条，若右前方较远处检测到障碍物，则稍向左转；第 二条，若右前方较近处检测到障碍物，则减速并向左转更多角度；等等。

避障过程中存在哪些问题？

4.模糊逻辑控制

不必创建可分析的环境模型，目前模糊逻辑方法在解决机器人避开障碍物问题上己经有了大量的研究工作。另一个独特优点也让用专家知识调整规则成为可能，因为规则库的每条规则具有明确的物理意义。在模糊逻辑控制避障法中，模糊控制规则是模糊控制的核心。当前研究工作的新趋势之一是它的渐增本质，特别是在模糊控制规则的自动生成方面，即连同自动模糊数据获取，给予算法在线模糊规则学习能力，数据获取和规则生成均自动执行。

[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-sdWck4p8-1654962524722)(https://raw.githubusercontent.com/xkyvvv/blogpic3/main/img/202206112205264.png)]

5.人工神经网络避障控制法

人工神经网络是由许多单元（又称神经元），按照一定的拓扑结构相互连接而成的一种具有并行计算能力的网络系统，它具有较强的非线性拟合能力和多输入多输出同时处理的能力。用在机器人上，就是通过模拟人脑神经网络处理信息的方式，从另一个研究角度来获取具有人脑那样的信息处理能力。对于智能机器人来说，采用人工神经网络进行信息融合有一个最大优势，即可大规模地并行处理和分布式信息存储，具有良好的自适应、自组织性，以及很强的学习功能、联想功能和容错功能，接近人脑的信息处理模式。

6. 栅格法避障控制法

于用启发式算法在单元中搜索安全路径。赋予每个栅格一个通行因子后，路径规划问题就变成在栅格网上寻求两个栅格节点间的最优路径问题。搜索过程多采用四叉树或八叉树表示工作空间。栅格法以基本元素为最小栅格粒度，将地图进行栅格划分，比如基本元素位于自由区取值为0，处在障碍物区或包含障碍物区为1，直到每个区域中所包含的基本单元全为0或全为1，这样在计算机中就较容易建立一幅可用于路径规划的地图。栅格粒度越小，障碍物的表示会越精确，也就更好避障。但往往会占用大量的储存空间，而且算法也将按指数增加。

九、路径规划算法

在规划模块中首先明确经常容易混淆的几个概念：

运动规划Motion Planning

路径规划Path Planning

轨迹规划Trajectory Planning

1.路径规划+轨迹规划=运动规划

2.路径规划：已知几何约束的情况下（障碍物、地图信息），求解一条路径，也即是找到一条无碰撞的路径。

**3.轨迹规划：已知运动学约束（时间约束、速度约束等）**情况下求解轨迹。

当然也有的路径规划算法在搜索路径过程中也将动力学约束（Hybrid A*）考虑进去，本系列的所涉及的规划算法主要是路径规划，也会涉及一部分轨迹规划。

1. 作用

**路径规划（path planning）**目的是为了寻找成本最小的路径, 需要给出的条件包括: 地图, 起始地点, 目标地点, 成本函数.

2. 组成

路径规划组成

路径规划主要由两部分组成，前端是路径搜索，后端是**轨迹规划，**最后生成一条机器人可执行的路径。

**路径搜索——**在地图中，搜索出一条避开障碍物的轨迹；

**轨迹规划（优化）——**对搜索到的轨迹进行优化，从而符合机器人的运动学和动力学约束。

规划示意图

如上图所示，a路径为前端路径搜索得出的，为一条无碰撞的路径，b为a路径经过轨迹规划过的路径，该路径满足运动学和动力学约束，为一条机器人可执行的路径。

3. 分类

路径规划（搜索）的算法主要可分成这三类，后续的章节也是按照以下的顺序进行介绍的。

3.1 基于搜索的算法

DIjkstra、A*、JPS

3.2 基于采样的算法

PRM、RRT、RRT*

3.3 满足动力学约束的搜索算法

Hybrid A*

更多的内容可看下面的介绍：

焉知科技：路径规划五种算法简述及对比

路径规划算法总结

---

十、SLAM

SLAM是 Simultaneous Localization And Mapping的 英文首字母组合，一般翻译为：同时定位与建图、同时定位与地图构建。

「同时定位与地图构建」这几个词，乍一听起来非常拗口，为了不在一开始就吓跑读者，我们先不对其进行专业的解释，用一个日常生活中形象的例子来进行说明。

1.初步认识SLAM

我们知道现在有不少家用的扫地机器人，可以代替人对室内进行自动清扫。早期的扫地机器人并不智能，它只是具有简单的避障功能，在室内随机游走，遇到障碍物就转弯，这样会导致有很多地方会漏掉，扫地效率非常低。

[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-i0i75XAk-1654962524723)(https://raw.githubusercontent.com/xkyvvv/blogpic3/main/img/202206112338845.jpeg)]

而现在随着SLAM技术在扫地机器人中的应用，现在的扫地机器人已经变的非常智能，可以通过自身的传感器对室内进行扫描建图，根据当前的定位进行Z字形规划清扫，还能实现自动回充、断点续扫等高级功能。

根据前面介绍，我们总结一下，要想真正实现智能的清扫，扫地机器人至少需要知道以下几件事情：

1、我在哪里？也就是扫地机器人在工作过程中要知道自己在房间的具体位置。对应的术语叫：定位（Localization)。

2、我周围的环境是什么样子？也就是扫地机器人需要知道整个房间的地面结构信息。对应的术语叫：建图(Mapping)。

3、我怎样到达指定地点（充电器）？当扫地机器人电量不足时，如何以最短的路径到达充电器所在位置进行自动充电。对应的术语叫：路径规划（Route Planning）。

有了以上的几个能力，扫地机器人就变的非常智能了，不再像无头苍蝇一样在室内乱跑，而是可以从任意位置出发，按照建立好的地图进行规划清扫。当然房间里的物品摆放也会发生变化，所以扫地机器人每次清扫也会根据当前的定位及扫描情况，在建好的地图基础上对进行更新。随着使用时间的增长，扫地机器人建立的地图会越来越准确，规划的清扫路线越来越高效，变的越来越智能。

看明白了上面的例子，我们给出SLAM的定义。SLAM是指当某种移动设备（如机器人、无人机、手机等）从一个未知环境里的未知地点出发，在运动过程中通过传感器（如激光雷达、摄像头等）观测定位自身位置、姿态、运动轨迹，再根据自身位置进行增量式的地图构建，从而达到同时定位和地图构建的目的。定位和建图是两个相辅相成的过程，地图可以提供更好的定位，而定位也可以进一步扩建地图。需要说明的是，上述扫地机器人例子中，定位和建图是SLAM的基本要求，而路径规划是在此基础上的高级功能，不属于SLAM的讨论范畴。

2.SLAM的应用

SLAM所使用的传感器主要分为激光雷达和视觉两大类。在SLAM研究史上，早期SLAM研究几乎全使用激光雷达作为传感器，其优点是精度高，解决方案相对成熟。但是缺点也非常明显，比如价格贵、体积大，信息少不够直观等。

视觉SLAM就是用摄像头作为主传感器，用拍摄的视频流作为输入来实现同时定位与建图。视觉SLAM广泛应用于AR、自动驾驶、智能机器人、无人机等前沿领域。我们知道SLAM的两大核心：定位和建图。下面分别进行说明。

1、定位相关应用

SLAM在自动驾驶中最主要的功能就是更加精确的确定汽车自身的位置（当然建图也很重要）。说到这里，很多人会有疑问：现在手机上的百度/高德/腾讯等地图类App就可以定位，为什么还需要SLAM来定位呢？

原因是这样的：目前地图类App在室外定位、导航方面确实做的很不错，而且衍生出很多基于地理位置的游戏、社交、生活类应用。不过现在的定位系统仍然存在以下两个主要问题。

1、地图类App背后使用的是GPS技术，但民用GPS最多也就达到几米的定位精度，驾车的读者应该深有体会：这些地图类App导航时并不知道当前车辆在哪个车道上。

2、GPS只能在室外使用。而在建筑物内、洞穴、海底等很多地方，GPS会失效。而往往这些地方对于定位的需求很强烈，要想解决GPS失效区域的定位，目前最有效的就是SLAM技术。

如下图所示是著名开源算法ORB-SLAM2使用架设在汽车上的摄像头进行定位的结果。从图中不难看出，算法非常清晰的展现了汽车当前所在的车道。

同样的，对于室内使用的自主移动机器人来说，定位也完全依赖SLAM技术。如下图是单目SLAM的过程。此外像京东，阿里这种大型的电商已经在自己的仓库配备了仓储机器人，也称AGV（Automated Guided Vehicle），可以根据建立的地图确定自身的位置，然后根据任务需求进行路径规划来搬运货物。

[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-IY8ZkEjB-1654962524725)(https://raw.githubusercontent.com/xkyvvv/blogpic3/main/img/202206112338304.jpeg)]

2、建图相关应用

比如使用手机上的单目摄像头可以对物体进行扫描，生成对应的三维模型。这称为小尺度下的三维建模。当然，也可以对较大尺度的场景进行三维重建，比如下图下就是Google project tango利用鱼眼摄像头对室外的大场景进行的三维重建。

[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-CBuH7GEG-1654962524725)(https://raw.githubusercontent.com/xkyvvv/blogpic3/main/img/202206112338080.jpeg)]

最近几年，随着消费级RGB-D相机的普及，室内场景的三维重建也是一个非常热门的方向。利用重建的结果可以实现很多 功能，比如室内移动机器人可以根据重建结果判断障碍物距离、识别理解环境，进行导航；比如可以根据重建结果在家里玩增强现实游戏；还可以将二维图片和重建结果进行融合实现三维漫游等。

3.讨论

SFM（structure from motion）是一种和SLAM非常类似的算法，关于SFM和SLAM的异同，下面说法错误的是？

A. SFM和SLAM基本讨论的是同一问题，不过SFM最早来自计算机视觉领域，而SLAM则是来自于机器人领域。SFM的structure对应SLAM的mapping，SFM的camera pose对应SLAM的location。

B. SFM一般是离线处理，而SLAM更强调实时性，一般在线处理。

C. SFM处理的图片一般没有要求，可以是不同时间不同相机拍摄的同一个场景，而SLAM一般要求是同一相机拍摄的序列图像或连续视频。

D. 目前SFM主要是要完成3D reconstuction，而SLAM主要是要完成localization。

E. SFM通常使用摄像机作为传感器，而SLAM在传感器选择方面，除了摄像机，通常还包括惯导、激光雷达等传感器。

F. 如果想要快速对自由女神像进行3D重建，在没有特殊硬件的情况下，最好的方式是选择SLAM。

---

本文整理于网上各类资源，可随意转载！