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

无人飞机可用于民用事业，满足国防需要，开发利用空间资源，实现空间操作和空间试验。因此，国内外对无人飞机进行了大量的研究。

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

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

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

四旋翼飞机可以稳定地悬挂在空中，因此有利于一些需要高度稳定的空中作业，如航拍、遥感探测等。

人们对智能控制技术的要求越来越高，许多事情不需要像以前那样提前收集大量的工作环境。飞机的工作环境已经从只在已知的环境中飞行转变为在不可预测和探测的环境中工作。

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

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

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

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

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

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

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

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

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

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

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

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

（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<C+D时，如左图。反之，如右图。

[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(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时利用卫星进行粗略的高度定位以维持高度不变。

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

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

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

（姿态传感器）、主控制器及接口与扩展等部分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波的形式输出以驱动螺旋桨电机。

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

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

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

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

渡越时间探测法，指的是超声波发生器往某个方向发射超声波，计时开始于发射的时间点，此后超声波沿直线传播，当超声波撞击到物体时就被反射回来，当超声波接收器接收到返回来的回波时计时停止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）对色彩、光照度不敏感。超声波测距传感器可识别玻璃，抛光体等能透过光线和表面粗糙的物体。

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

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

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

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

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

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

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

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

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

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

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

红外测距是利用三角测量原理实现测距的。图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的关键在于滤镜的焦距。

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

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

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

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

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

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

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

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

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

像素坐标系是一个二维坐标系，其一般定义是以图像的左上角为原点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的平移矩阵。

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

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

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

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

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

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

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

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

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

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

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

在三维立体空间中，利用重力和传感器的关系，可以分析得到如图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轴方向产生的加速度

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

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

（1）Gimbal2型陀螺仪

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

（1）加权平均法

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

（2）卡尔曼滤波法

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

（1）模糊逻辑推理

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

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

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

（2）人工神经网络法

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

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

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

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

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

姿态解算需要解决的是四轴飞行器和地球的相对姿态问题。地理坐标系是固定不变的，正北，正东，正上构成了坐标系的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)]

先简单说明下四轴飞行器是如何飞行的,四轴飞行器的螺旋桨与空气发生相对运动，产生了向上的升力，当升力大于四轴的重力时四轴就可以起飞了。四轴飞行器飞行过程中如何保持水平：我们先假设一种理想状况：四个电机的转速是完全相同的是不是我们控制四轴飞行器的四个电机保持同样的转速，当转速超过一个临界点时（升力刚好抵消重力）四轴就可以平稳的飞起来了呢？答案是否定的，由于四个电机转向相同，四轴会发生旋转。我们控制四轴电机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反向出力

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

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

软件部分则比较复杂，包括飞行员输入指令处理，控制律计算，容错处理等等，无人机的话还要考虑地面站通信等等，由于飞控软件功能很多，一般会利用一个实时操作系统保证运行的可靠（我们当年给固定翼无人机用的是vxwor