四轴自适应控制算法的一些尝试开源我的山猫飞控和梯度在线辨识自适应等算法

这篇文章被同时发在这里和 阿莫论坛的四轴飞行器区

http://www.amobbs.com/thread-5539404-1-1.html

本文的最主要目的在于抛砖引玉，阿莫论坛真的是非常好的一个论坛，没有这个论坛，没有那么多这个论坛上的前人无私的奉献和热烈的讨论，我想我是怎么也无法入门四轴的控制的。只是论坛上已经很多年都没有看到过新东西了，大家都把自己的成果悄悄藏了起来，于是这么多年了，在商业四轴已经泛滥的今天，论坛上依然还停留在给PID调参数这种初级阶段，甚至就是这么简单的东西人们尽然会把它抱得那么紧，生怕别人学会了，只是你没有往出走一步真正的看下别人都做到哪一步了而已，我们不要做井底之蛙，大家团结起来共同进步才能带动中国四轴飞控算法的进步，所以我决定把我的代码以及相关的资料全都贡献出来，源于奉献，敢于奉献，乐于奉献。希望我这块砖头能真的为我们的四轴DIY事业添砖加瓦。

首先，我们要声明，我这文章不是成功以后的炫耀，而是失败之后的总结，为以后将要顺着这条道路走下去的“战士”们清理荆棘。我要扼杀掉那些认为自适应控制是“完全万能”的人的美梦，我们这里已经实现了自适应的极点配置控制器，在线的辨识系统参数（从控制输入到得出欧拉角）和控制器参数（使用闭环辨识的方法直接辨识得到），并且在单轴的情况下实现了非常好的控制效果（可以观看视频），然而在对两个轴同时控制的时候，互相之间的相关性导致了我们的自适应始终不能稳定而失败。但是我们并没有因此丧失我们对此算法的乐观态度，原因及相关的内容会在后面提及。

文章主要会分成三个部分，诸看客依旧可以直接挑选感兴趣的内容观看。

①对论坛发发牢骚

②对附件中的山猫飞控和自适应控制算法做介绍

③对所作工作的总结，归纳出主要的问题和注意事项

④致谢

第一部分：牢骚

真的非常庆幸有阿莫这样的论坛，会有好多大牛来这里交流经验，让我等小辈可以从大神们的交流中提取到养分，慢慢成长。只是当时初来乍到，能看到的仅仅是四轴区被顶到前几页的那些帖子，虽说大家现在都是照着那几篇文章在做，但是在咨询了坛子里的一些大神后才发现，原来阿莫上四轴的鼎盛时期已经过去好几年了，许多真正很牛的技术贴早就石沉大海，并没有被顶起来，我没有在阿莫上看到有版主管理版面的概念，更沮丧的是，没有版面的置顶帖只有广告的置顶帖，没有总结帖，导致大神们的月经贴就这样白白的冲进了下水道，再也找不到了，这里强烈建议论坛完善管理体制，请有存档的大神整理总结帖，要想让论坛继续为中国无人机事业发光发热就不能让新手都“无从下手”。

第二个牢骚是发给那些只在论坛里吹嘘自己的能力指责别人的不足却始终不敢把自己的代码和方法贡献出来的人，当然特指如我这般还是学生、用着别人的东西却还自欺欺人的认为自己做了多少工作不舍得拿出来分享的人。既然是学生，不愁吃穿，又不了解市场行情，自己做的东西能拿来赚钱么？既然不能，为什么不分享出来让大家共同帮着你找出问题呢？就算是做失败了又怎样？失败后的总结才是他人和自己最需要的，尤其当大家把你从困难中拉出来的时候，那时候才会明白“开源”这个词背后蕴藏着到底多大的能量。

由于一些特殊的原因，接下来的几个月时间我都会处于颠沛流离的状态，没有办法完善的代码了，但考虑到几个月后如果我没办法再拿起我的四轴的话，未免有点太可惜了，于是就匆忙的把现有的工作挂了出来而不是等到算法彻底成型了再推出。

我个人非常期待我的飞控中有用的部分能用到大家的四轴中，只是在这里有一点自私的请求，虽然我自知自己的代码特色并不明显，但是如果真的有用并能用在您的四轴中的话，希望您也能开源您得代码，打心底里希望大家能共同努力，把中国的四轴做大做强！

第二部分：飞控、直接法自校正极点配置控制器特点介绍



①硬件和一些不重要的东西：

450的糙机架，朗宇2212 980kv的电机 1045的桨 电调是好赢的四合一电调，STM32F103VET6主控传感器为L3G4200+HMC5883L+ADXL345+BMP085杜邦线连接软件I2C，有个小蜂鸣器，用电阻做了个简单的电池电压测量，富斯的9通遥控和接收机，一个网上买的性能很差的433MHz串口透传模块。

②操作系统：

为了能把控制算法写到足够大并能保证各软件模块间时序的准确性，我们使用了uCOSIII操作系统，这个相比uCOSII有了很大的改进，应该说在我们的STM32上使用非常合适，但是API和前代相比有了很大变化，很多东西需要重新写。另外就是是我们的飞控可以同时使用库函数和寄存器来设置（因为我是使用寄存器的，但是工程里带了库，怕人们困惑才特意加这句话）

③通信与手柄控制的特色：

我们使用MATLAB做的上位机与飞控通信，因为MATLAB对多线程的支持非常不好，串口通信也存在很多问题，所以我的飞控通信效率非常低，使用的是字符的传输，倒是很便于用串口助手观查，最大的好处就是可以直接把飞控上截获的数据拿到MATLAB上来分析，对我等学生来说也许很有用吧，嘿嘿。

另外一个特色是可以同时支持用游戏手柄的遥控和8通航模遥控器的遥控。飞控上可以读取8通接收机的信号，使用的是外部中断的方式，直接把接收机的线接到相应引脚上就行，不需要额外寻找PPM信号（比如我用的富斯接收机就没有PPM信号），与其他控制（这里对比的是游戏摇杆和MATLAB给的控制信号）相比有最高的优先级，既航模遥控器锁电机开关（自己设定的）关闭时会在飞控内屏蔽其他外部控制输入。游戏摇杆当然就是普通的带两个摇杆的游戏手柄应该都是可以的，还是的使用我的MATLAB的上位机，也就是说如果您愿意的话完全可以在MATLAB上位机上直接控制四轴（如果你不怕坠机的话），当然，因为我只有一个性能很差的433串口透传模块，双向传输会牺牲一半以上的通信速度，要是想用后面提到的系统辨识一类对时序非常敏感的程序的话指定是不能用游戏摇杆了。

④可以进行在线的系统辨识，飞控中写入了渐消记忆最小二乘系统辨识算法，可以辨识带噪声模型的ARMA模型，之前看到坛子里有人在讨论用这种方法给四轴建模的，有了这个就方便了吧，嘿嘿~

⑤飞控中可以随意改变控制和姿态解算的频率，只需要改动头文件中的定义即可，当然得考虑算法的复杂度和I2C的读取速度。

⑥姿态解算：

使用的是坛子中 手二哥（俊）的梯度下降的姿态解算的方法，虽然公式略有不同（因为用的旋转矩阵和性能函数不一样）但效果是完全一样的，做了一点小花招，减小了磁阻融合的比例，这样在不要求偏航精度的情况下不经过矫正的磁阻也可以融合姿态了，并且基本不会对Pitch和Roll造成很大影响，只是偏航从起始到稳定需要一小段的时间。如此一来，算上系统启动时自动算均值校正陀螺仪的零漂，需要手工校准的就只有加速度计了。有一个完全派不上用场的“抗震”融合，诚然，效果和坛子里无雪大神的抗震AHRS完全没有可比性。算出了地理坐标系上飞机的空间加速度、速度，但是同样如无雪大神说的，没有经过调校的MEMS传感器求出的IMU确实飘得没法用，所以这里我并没有在控制中使用这些数据，不过有兴趣的可以尝尝鲜。

⑦封装了常见的PID PD PI控制器，关键是这里面有两个测试用的控制器，一个是开头提到的直接法自校正极点配置控制器，另一个是我自己推导的直接法自校正积分型极点配置控制器，这两个控制器主要是为了我等学生测试用的，目前还不能稳定的飞行，其中直接法自校正极点配置控制器可以实现单轴的稳定控制。

⑧一些必须放在这里说的问题：

首先是我做四轴并不是为了做一个能稳定飞行的玩具，也没有项目要求要做这个东西，所以我并没有以“把PID和姿态解算调到极致”为中心，而是以“方便测试尽可能多的控制算法”为中心。因此我的飞控存在如下问题：

控制频率和姿态解算速率都很低，PID控制时用的是25Hz，在使用极点配置自校正控制时仅为10Hz，姿态解算一律用的50Hz，没有让陀螺更快的积分。这么做必然是不对的，只是当初设计时有三点考虑让我不得不这么做：一是我的通信模块很烂，在做离线系统辨识时承载不了高控制频率带来的庞大数据量。二是当初为了测试自己的控制算法对噪声的敏感度故意在姿态结算上放了水，于是算法都做出来了但是参数基本都没怎么调过。三是当初听说103的计算能力不足，有考虑到自己是带着uCOSIII在跑，怕运行复杂的自适应算法时运算性能不足，于是竟可能的给控制留了足够的运算时间，但是目前加上自适应以后系统资源峰值占用率也没到16%，所以可以放心的提高姿态解算的频率并增加相应的滤波，毕竟对于PID来说（对于PID控制频率为什么要够高我后面会给出一些证明的，但也仅限于PID控制器了）高控制频率时非常必要的。

接下来是自适应算法的一些问题，首先自适应算法使用确实需要一定的基础（因为需要确定系统模型的范围）但目前我写的算法在MATLAB的仿真中具有非常理想的效果，但在实际应用中有4个问题导致目前的使用失败：一是设计的控制器在实际系统中经常会出现不稳定极点，这点导致我设计的带积分项的自适应控制在还没有进入到自适应稳定的状态就已经发散了。二是设计的控制器阶次过高，这是我们选用不靠谱的10Hz的主要原因，因为在10Hz下我们可以做到能稳定的最低阶次：3阶，而在25Hz下就已经达到了6~7阶，此时对系统参数已经过度敏感，在我们四轴中时变效果太强，控制器参数变动太大，难以收敛。三是最严重的问题，就是飞机存在起飞阶段，该阶段本身系统参数变化就很大，加上刚好是自适应开始的时候，控制器参数及由此导致的控制信号大幅超出量程，飞机瞬间就坠毁了，都来不及等到自适应收敛。四是多轴联动时，一个通道的控制量会同时对另外两个通道的输出产生影响（角速度→四元数→旋转矩阵→欧拉角），对高阶控制系统影响尤为明显，导致我们的控制算法无法在短时间内收敛造成坠机。

第三部分：

算法设计当中出现的一些关键问题的总结：

①控制频率与系统模型间的关系：

说一些实验现象，具体原因没有深究：降低控制频率，会增大辨识得到的离散系统的增益。在做PID控制时尤为明显，我在不同控制频率下用我的PID整定方法设计控制器，得到的参数在存在很大差别，但在将控制模型转化为g0+g1Z^-1+g2Z^-2的形式后发现仅仅相差了一个增益K，控制器的零极点并没有改变，仿真上差别不大，但会增大噪声的增益，在使用高频率控制时单纯的PID方法能表现出比在低频时更好的控制效果。



25Hz时的PID



10Hz时的PID

在控制频率低于系统实际的纯延迟时，离散系统会被采样成逆不稳定系统（非最小相位系统）这个时候无论哪种方法都很难体现出控制效果了，尤其PID，即便如此，我所辨识的被控系统始终存在一个在负半轴但在单位圆内的零点，感觉它严重的影响了我各种控制算法的控制效果。



②关于确定极点配置控制器控制频率为10Hz的一些考虑：

如前所述，由于我测量到系统从控制输入到角度反馈输出的延迟大约有0.2s（不知道大家的大四轴是不也是这样的，还是单纯的是我四轴的自身问题）使得控制器在高频时阶次过高，但在低频时会变成逆不稳定系统，这是极点配置控制器不能控制的系统。所以最后折中到了10Hz，同时由于这时候理论延迟d=2，但是依然是逆不稳定的，我们额外为辨识系统多加了一阶延迟，使d=3，这才保证我们设计的控制器在3阶并保证逆稳定的条件。



③为减小自适应初期的抖动所作的一些尝试：

在自适应初期会出现很恐怖的抖动，为了尽可能的减小这种抖动，我们使用了两种方法，第一种是前面说的尽可能的降低阶次，可以有效减小抖动，第二是用仿真得到的稳态的辨识参数作为初值，这两条并用，在我们单轴的测试上达成了良好的控制效果，但是多轴时由于无法完成离线辨识所以我们一直都没有得到飞机实际飞行中的系统模型，这条我们会在下一条中探讨原因，先上一张使用自适应的高阶控制器（后）与低阶控制器（前）在有噪声模型环境下控制效果和控制量输出的仿真对比：



④为什么会使用直接法辨识控制器参数而不是在线辨识被控系统然后解丢番方程求解控制器：

这个是我们在设计自适应控制器之初最头疼的问题，因为在有控制器的情况下辨识被控系统属于闭环辨识的内容，而我们在实际使用中发现，纵使是给控制器叠加白噪声进行闭环系统辨识，也没能收敛到系统的实际参数上，这是导致我们没有得到飞行中的四轴的系统参数的直接原因。在目前我浏览过的自适应控制的书中都提到闭环辨识的问题，但在设计算法的时候却都不约而同的完全没有考虑闭环辨识的问题……我们仅在北航的这本自适应控制的书中找到了这么一个能够解决闭环辨识问题的自适应控制器（后面会附带电子版及书上代码），相关的内容可以在这本书中找到。

⑤为什么没有使用最小方差控制器这类基于最优化的自适应控制器：

我们在试验中发现对于我们的被控系统，使用最小方差控制器会让控制输入超出油门上限N倍，明显不具备可行性因此而放弃。其他控制器如LQ控制器是我的目标，但时间关系我目前只能测试到这里。

⑥为什么我会说乐观？ :-]

因为我们是直接对角度进行的输入输出控制器设计，如果从角速度来进行设计的话，前面的这一大堆问题理论上都会迎刃而解，所以我们对该算法思路非常乐观，也希望想像我一样走角度直接做自适应控制的三思，多考虑从角速度模型进行辨识并控制。

以上是我整理的关于我在做四轴的自适应控制中的一些尝试方案和记录，接下来是致谢：

第四部分：致谢

文章中使用了很多坛子里大神们总结出来的算法和方案，姿态解算部分主要来自手二哥（俊）的帮助，还有无雪大神热心的回答各种AHRS方面的问题，让我深刻的体会到一个足够优秀的AHRS算法对控制的重要性（当然我指的是无雪大神UKF卡尔曼而不是这里的梯度法）。感谢群里的红桃K（zht一堆数字）大哥不停的贴论文拓展我们的视野，啊嘉大哥教我们航模的知识，黑条大哥殷切的鼓励，飞拓大哥东林大群主等诸多大神的帮助，这里就不一一致谢了。

哦 对了 推销一下东林大哥的AHRS&IMU算法讨论QQ群199376386，以上大神均可在此群中瞻仰。

最后本人特别希望能与兴趣在四轴控制算法上开拓创新的“战士们”多交流，欢迎大家加东林大哥的群或加我QQ 八四六九三思六舅（河谐防广告）

附件下载：

http://download.csdn.net/detail/lynx2/5624861