19年电赛B题设计总结

这几天2019年电赛结束了，感觉心里不是滋味，要问为什么的话，当然是因为我已经不是本科生了，不然的话我就又能拿国奖了。（贱笑中。。。）

因为一些原因，我也参与到了一群学弟的指导中。（当然是无偿的。。。）他们选择了飞行器类的题目，我觉得飞行器类的题目是最难的，也是最简单的，其中的关键就是 “飞行控制器” 。如果你想要从头到脚，从惯性数据采集、姿态检测、姿态控制到速度位置控制再到最后的图像检测、动作决策等全部过一遍的话，那这无疑是地狱级别难度。而你如果并不是大神的话，捷径也是存在的，那就是成品飞控。成品飞控相对于自制飞控明显更加专业，程序中的问题也更少。而且今年的赛题中也没有加入相关的限制，相信以后的电赛设计中，将会有更多人采用成品分控而不是自制。

在电赛准备期间，因为电赛官方给出的信息是会有一道题指定TI公司的芯片，很多人担心这个限制，结果导致了“某名飞控”推出的基于TI芯片的飞控模块直接销量上千，而且售价不菲，整套飞控加传感器下来得有一千多。。。（想想2015年的电赛我当时的条件：风力摆四个空心杯电机、L298N模块、STM32F103核心板和MPU6050全套总共不到100块钱），只能说是真的壕。然而赛题下来一看都傻眼了，根本就没有对飞信器的题目有任何芯片方面的限制。

好了，不说废话，进入正题，先看题目：

基本任务1：巡线机器人从距 A 杆塔 1 米范围内的起飞点起飞，以 1 米定高绕杆巡检，巡检流程为:起飞→杆塔 A→电力线缆→绕杆塔 B→电力线缆→杆塔 A，然后稳定降落;巡检期间，巡线机器人激光笔轨迹应落在地面虚线框内。

基本任务1是整个题目中最基本也是技术含量最高的一项任务，如果第一项都完不成，那之后那些分数就不用想了。把上面的任务分解一下可以分为以下几个子功能

基本功能要求

1.飞行器须有检测和调节飞行高度的能力，且精度至少要精确到2cm以内。

2.飞行器须有调整Yaw（偏航角，即朝向角）的能力，以及调整水平方向运动状态的能力（可以是两轴的速度或加速度）

3.飞行器须有检测杆塔方位和距离的能力

4.飞行器须有检测其与电缆线之间距离和Yaw偏角的能力

5.飞行器须有检测自身在一段时间内旋转过的角度的能力

只要实现了上述五个功能，则这道题就很好做了。

整体流程

1.飞行器从指定地点起飞，到指定高度，这里用到了检测和调节飞行高度的功能。

2.飞行器检测周围的杆塔，确定杆塔A方位和距离，然后到达杆塔A的前方。这里用到了检测杆塔和水平运动状态调整的功能

3.飞行器开始巡线，检测线的倾角和距离，实时调整自身的朝向，使摄像头始终垂直指向电缆线。并调整其与电缆线之间的距离。

4.飞行器在巡线三米后，到达杆塔B，此时检测杆塔B的方位角和距离，然后启动绕杆程序

5.当角度旋转到一定值时，说明飞行器已经绕过了B杆，重新启动巡线程序

6.最后返回A杆，降落。

由上可以看出，飞行器每时每刻都在采集被控对象的状态并进行控制。

接下来将上面的功能一个一个实现（默认采用成品飞控）

功能实现

1.高度检测和调整

首先对于飞行器而言，其高度检测是不能使用超声波测距模块的，那种数据跳变的情况会让你在调试的时候伤透脑筋。最好使用激光或者ToF，当然如果你有条件的话，可以使用激光雷达，有些激光雷达的测距范围可以到12cm~3500cm,你只要读取其中垂直指向地面的那个数据就可以得知高度。不过基本不会有人那么干。一般还是采用激光或者ToF的方法。当然还有一种方法是使用激光笔和摄像头，摄像头与激光笔之间呈一定夹角，然后采集和处理图像，得到红点的位置（角度），通过三角测距也可以得到飞行器的高度。这些方案都是可行的，但是就要你就成本和重量等综合考虑了。最后一定要确定测距的量程，一定要确保飞行器在地面上时传感器也能读取到数值。

关于高度的调整要看底层的飞控提供了何种接口，是加速度类的控制还是速度类的控制，需要根据飞控不同的接口编写不同的控制器。对于加速度类的控制接口，可以编写串级PID控制器以实现比较好的高度控制效果。

2.飞行器的Yaw角和水平方向的运动状态控制

由于飞控基本采集不到比较精准的Yaw数据（地磁传感器并不能靠得住），因此在此控制飞行器的yaw角角速度，通过控制角速度来调整角度，一般飞控都会留有Yaw角速度的控制接口。然后是对水平运动状态的控制，这里最好使用Pitch Roll 倾角的控制，也可以看做是水平加速度的控制，原因是飞控如果不配合可靠的光流信息的话，其无法获知准确的运动速度，自然无法准确地调节。

3.飞行器检测杆塔

飞行器要绕杆塔旋转，就必须要知道其与杆塔之间的相对位置，包括方位角和距离，然而使用单个摄像头基本不可能实现，这里有两种方案：一种是双目摄像头，通过三角测量法，结合两个摄像头检测的杆的位置以及两个摄像头之间的相对位姿，获得杆塔的深度和方位角。但是实际上摄像头和杆塔之间的相对安装位置很难确定，因为要考虑杂乱背景的影响，以及杆塔脚架的影响。第二种方法则更具备可行性，就是采用激光雷达。

激光雷达

3.1激光雷达

如果用激光雷达来检测杆塔则会大大降低难度，但是一般激光雷达都是用于ROS，SLAM等场景，难以找到相应的驱动，且成本较高。但是为了电赛也是拼了。我这里接触过一个激光雷达，是Turtlebot3机器人平台上自带的激光雷达，此激光雷达的价格在一千多，但是在TB上，有二手拆机可用的同样型号的价格在100左右，搜索turtlebot3激光雷达就能找到。激光雷达的使用方法其实很简单，用简单的STM32F103系列就能很轻易的处理。一般激光雷达是带有串口接口的，它在上电后，无需进行配置，就能自动向外发送360度的测距信息。激光雷达相当于一个激光测距的模块在那一直转，每转1°就会采集一次深度值。每转一圈会输出360个距离信息。每秒一般会转5圈。

3.2激光雷达的使用

接下来我将以STM32F103型单片机为例介绍如何通过单片机采集激光雷达的数据。

STM32单片机比较强大，支持DMA功能，为了减轻CPU的负担，我们使用DMA和USART外设配合，当激光雷达的数据以230400的波特率传来时，直接将接收的字节保存在实现定义的环形缓冲区中。然后通过定时中断每隔一段时间读取缓冲区中的内容，根据官方提供了的数据包结构对激光雷达的数据进行解析。（不同厂家设计生产的产品使用的数据包结构不同）解析其实很简单，就是包头包尾和校验之类的。缓冲区的大小要根据你每次读取的时间间隔和雷达发送数据的速率来确定，比如我用的这个就是每秒5圈，每圈60个42字节的数据包，总共12600B/S。如果我要每秒100次解析缓冲区，则缓冲区必须定义为126以上，为了稳妥，最好定义为两倍以上才可。还有就是每次解析时只需要提取出你最想要的东西（对应角度的距离）即可，然后定义一个360长度的short数组专门用来存放距离信息。

具体的代码可以通过贿赂我的方式来获取。

3.3杆塔的检测算法

杆塔的检测算法很简单，因为比赛场地除了杆塔没有其他立着的物体，因此直接寻找距离数组中距离为最小值的那个角度索引即可。注意排除飞行器自身的影响（划定一个角度范围进行扫描）。

4.飞行器检测电缆线

飞行器检测电缆线的方式有很多种，貌似可以通过电磁场等手段检测，不过一般的方式是采用摄像头。相信很多人会因为电缆线后面的杂乱背景而感到头疼，其实大可不必，只需要用支架将摄像头抬高，使摄像头往斜下方拍摄即可。整个飞行器的形状就像是一只扑腾着翅膀的母鸡。由于地面的背景颜色较为单一，因此可以去除几乎所有干扰。如果采用这种安装方法，则线检测的算法可以说是非常简单了，甚至可以用两个线阵CCD来获取线的位置和倾角。

摄像头与线缆的相对位置

此时我检测到线在画面中的倾角和中心高度了，我要如何使用？我要得到的是什么信息？线的倾角和中心高度不是我们想要的，我们想要的是飞行器摄像头相对于电缆线所呈的倾角，以及飞行器与电缆线之间的距离。首先考虑倾角：当摄像头正对电缆线时，会在画面中得到水平的一条黑线。当摄像头的Yaw角发生了变化时，摄像头不再正对电缆线，此时画面中的线就出现倾斜的情况。同时如果飞行器姿态调整时Roll角（就是左右倾斜的角）发生变化的话，也会影响到画面中的线。

4.1画面中线的倾角的利用

飞行器Yaw和Roll对画面中采集的线的倾角影响

由上图可得，画面中的线的倾角主要反应了飞行器的两个角度，一个是Yaw，一个是Roll，考虑当Yaw为偏转90度以后，则画面中线的倾角近似90度。当Roll角偏转90度后，画面中线的倾角也为90度，因此可以认为Yaw和Roll两个角度直接相加得到了画面中线的倾角。可能不太科学，但是近似应该是没有太大问题。所以当我们要求的飞行器在Yaw方向与线缆所呈角度时，只需在画面中线的倾角量中减除Roll角即可。此时会得到一个近似的Yaw偏角。

4.2画面中线的中心高度的利用

飞行器与线缆之间距离的计算

上面的图已经画得很清楚了，对于定高性能较好的飞行器来说 h 是已知常量，摄像头安装的朝向（绿线）角度也是已知的。首先可以通过画面中线中心的纵坐标y和摄像头的纵向可视角度获取一个近似的线缆相对于摄像头的方位角，然后再与摄像头的安装角度相加即可得出 θ 。已知θ和h，则l可以直接通过tan函数求出。当然还需要考虑到飞行器的Pitch角，飞行器在面朝线缆前后仰时，其Pitch角变化，导致摄像头的安装角度变化，为了消除这种影响，需要获取飞行器的Pitch角并对θ进行补偿，更进一步的，还需要对摄像头的 h 进行补偿，最终得到比较精准的距离数据。

经过上面的两个步骤，基本就能获得比较精准的飞行器Yaw偏移和与线缆的水平距离了。

5.获取飞行器Yaw方向角度变化量

其实判断飞行器是否绕满180度有两种策略，一种是根据角速度积分，另一种是记录开始旋转时的角度，然后用之后的实时角度与之前的角度进行比对，两种方法都可以很容易的检测到旋转的角度。但是这里要注意的一点是，尽量不要将旋转完毕的判定角度设为180，因为飞行器在绕圈时存在惯性，很容易因为来不及叉车而撞到线缆上，因此可以设成160或者170，这样可以使得整个过程衔接更平滑，且节省时间。

三个典型的控制策略

1.杆塔定点控制

杆塔定点控制的功能是让飞行器可以在以杆塔为远点的坐标系中精准稳定的定（x，y）点悬停。而这个功能实现的前提是飞行器的Yaw在整个控制过程中内没有明显的偏移。

飞行器与杆塔的相对位置求取

现在我们已经通过激光雷达确定了杆的方位角和距离，很轻易就能求出以杆塔为原点的坐标系中飞行器的坐标值。现在如果指定一个给定值（10，-40），就可通过给定值减去实际值获得偏差，通过设计控制器就能得到很好的控制效果。当然一切的前提是Yaw角需满足一定条件，也就是飞行器全程必须正面对着线缆的方向。如果飞行器摆不正，则无法获取到精准的相对位置。

2.巡线控制

巡线控制的目标是可以使飞行器能够以指定速度、与线缆相隔指定距离向右飞行。

前面已经说明了通过摄像头采集的线缆的倾角和中心坐标的处理方法，最终获得飞行器的Yaw角偏差和飞行器与线缆之间的距离。1、在巡线过程中Yaw偏角的理想状态为0，因此需要设计控制器通过控制飞行器的Yaw角速度使飞行器始终正面面向线缆的方向。2、巡线过程中飞行器需与线缆保持最佳距离，因此需要设计控制器，根据给定距离和实际距离的偏差通过控制飞行器的俯仰角度进而控制飞行器的速度，实现距离的控制。3、巡线过程中需要以一定的速度向右滑行，这里有两个方案，一是在巡线的整个过程中给一个固定的roll倾角，当roll倾角提供的加速度与风阻平衡时，飞行器会匀速运动，此种方法简单粗暴，但是易受气流影响。二是通过光流传感器等手段，实时获知当前的速度值，然后通过PID控制器精确控制侧向飞行速度。

3.绕杆控制

绕杆控制实现的目标是以指定速度，沿指定半径绕杆旋转。

通过雷达可以得知杆相对于飞行器的方位角和距离，如此一来控制起来就很简单了。1、Yaw角控制，根据雷达反馈的杆塔的方位角，设计控制器通过控制Yaw角速度，使飞行器始终正面面向杆（θ始终为0）。2、距离控制，与巡线相同。3、速度控制与巡线相同。

只要搞定了这三种典型的控制过程，基本这题就算是解决了。以上就是我分享的所有经验，至于什么拍照，二维码之类的那全是没啥意义的东西，就不说了。

有什么高见的话，欢迎评论怼我。觉得有用的话，别犹豫，打赏吧，从11点到5点一直没休息的我快饿死了（用写文章的快感来麻痹饥饿已达到不吃饭省钱的目的的崇高境界我想你们是不会理解的）。