四旋翼飞行器Quadrotor飞控之 PID调节（參考APM程序）

做四轴也有一段时间了。近期一直在做PID方面的工作。

如今四轴基本能够实现室内比較稳定的飞行，操控手感也能够接受。稍后上试飞视频。在此把一些PID方面的经验总结总结和大家分享一下。

首先介绍一下大概的硬件组成：

MCU：Freescale MK60D

传感器（IMU）：GY-86模块（MPU6050 + HMC5883L + MS5611）

433MHz传输数据模块

DC-DC可调式开关稳压模块

遥控设备：JR XG7发射机 + JR 611B接收机

机架：普通红白f450机架

电机：朗宇2212kv880高效能电机（黑色）

电调：天行者40A

桨：仿APC1047

电池：DUPU 2200mah 25c

电路非常easy用洞洞板搭的简易測试电路。

软件方面最基本的是姿态解算，终于用的是MPU6050的DMP（Digital Motion Process）输出，遥控器控制的目标姿态也是四元数表示的。为了有较好的线性度，将两者的姿态差转换为欧拉角后再进行PID的控制。

以下进入正题。

PID算法非常大程度上參考了APM（国外成熟开源飞控项目。官网点击打开链接）的控制算法。它是採用的角度P和角速度PID的双闭环PID算法。角度的误差被作为期望输入到角速度控制器中。

双闭环PID相比传统的单环PID来说性能有了极大的提升，笔者也以前调试过传统的PID控制算法，即便參数经过了精心调整和双环控制算法相比在控制效果上的差距依然非常大。

不管是悬停的稳定性，打舵时的高速尾随性和回正时的高速性上都是后者的效果明显优于前者。

大概介绍一下APM算法的大概流程。这个PID控制流程图能够在APM官网找到。笔者參考的是APM3.1.0的代码和图上2.9版本号的有一点点出入，不影响大局（控制频率100hz）。

横滚（Roll）和俯仰（Pitch）的控制算法是一样的，控制參数也比較接近。首先得到轴姿态的角度差（angle error）。将这个值乘以角度系数p后限幅（限幅必须有。否则剧烈打舵时easy引发震荡）作为角速度控制器期望值（target_rate）。target_rate与陀螺仪得到的当前角速度作差，得到角速度误差（rate_error）乘以kp得到P。在I值小于限幅值（这个值大概在5%油门）或者rate_error与i值异号时将rate_error累加到I中。前后两次rate_error的差作为D项。值得注意的是加须要入20hz（也能够採用其他合适频率）滤波，以避免震荡。

将P,I,D三者相加并限幅（50%油门）得到终于PID输出。

偏航（Yaw）的控制算法和前两者略有不同。是将打舵量和角度误差的和作为角速度内环的期望值，这样能够获得更好的动态响应。

角速度内环和横滚与俯仰的控制方法一致，參数（积分限幅值会非常小，默认仅仅有万分之8）上有不同。

把APM的算法移植过来。限幅值也移过来之后就能够进行PID參数调整了。

一般先进行横滚或俯仰方向的调整。

把四轴固定起来。仅仅留一个待调整方向的旋转自由度（简直就是旋转“烤四轴”有木有）。笔者稍后奉上调试时使用的装置。强烈建议调试时带上厚的防护手套，不然的话我的手早就被被打烂了。调个飞机还负伤就不值得了。

左右两个直线轴承，用几颗自攻螺丝拧在桌子上（可怜的桌子），插上铝合金管（外径12mm。壁厚2mm，注意要调整两个直线轴承的位置使它们同轴），两头用角铁限位也是拧在桌子上。把四轴用胶带+扎带绑在上面就可以開始“烤四轴”。

大概过程（注意修正反向）：

1、预计大概的起飞油门。

2、调整角速度内环參数。

3、将角度外环加上，调整外环參数。

4、横滚俯仰參数一般可取一致，将飞机解绑，抓在手中測试两个轴混合控制的效果（注意安全）。有问题回到“烤四轴”继续调整，直至飞机在手中不会抽搐。

5、大概设置偏航參数（不追求动态响应，起飞后头不偏就可以），起飞后再观察横滚和俯仰轴向打舵的反应，如有问题回到“烤四轴”。

6、横滚和俯仰ok以后。再调整偏航轴參数以达到好的动态效果。

过程具体解释：

1、要在飞机的起飞油门基础上进行PID參数的调整。否则“烤四轴”的时候调试稳定了，飞起来非常可能又会晃荡。

2、内环的參数最为关键！

理想的内环參数可以非常好地尾随打舵（角速度控制模式下的打舵）控制量。

在平衡位置附近（正负30度左右），舵量突加，飞机高速响应；舵量     回中，飞机立马停止运动（差点儿没有回弹和震荡）。

2.1首先改变程序，将角度外环去掉。将打舵量作为内环的期望（角速度模式，在APM中叫ACRO模式。在大疆中叫手动模式）。

2.2加上P。P太小。不能修正角速度误差表现为非常“软”倾斜后难以修正。打舵响应也差。

P太大，在平衡位置easy震荡，打舵回中或给干扰（用手突加干扰）时会震荡。

合适的P能较好的对打舵进行响应。又不太会震荡。可是舵量回中后会回弹好几下才干停止（没有D）。

2.3加上D，D的效果十分明显，加快打舵响应，最大的作用是能非常好地抑制舵量回中后的震荡。可谓立竿见影。太大的D会在横滚俯仰混控时表现出来（虽然在“烤四轴”时的表现可能非常好），详细表现是四轴抓在手里推油门会抽搐。假设这样。仅仅能回到“烤四轴”减少D。同一时候P也仅仅能跟着减少。D调整完后可以再次加大P值，以可以尾随打舵为推断标准。

2.4加上I，会发现手感变得柔和了些。因为笔者“烤四轴”的装置中四轴的重心高于旋转轴，这决定了在四轴偏离水平位置后会有重力分量使得四轴会继续偏离平衡位置。

I的作用就能够使得在一定角度范围内（30度左右）能够修正重力带来的影响。表现打舵使得飞机偏离平衡位置，舵量回中后飞机立马停止转动，若没有I或太小，飞机会因为重力继续转动。

3、角度外环仅仅有一个參数P。将外环加上（在APM中叫Stabilize模式，在大疆中叫姿态模式）。

打舵会相应到期望的角度。

P的參数比較简单。太小，打舵不灵敏。太大，打舵回中易震荡。以合适的打舵反应速度为准。

4、至此，“烤四轴”效果应该会非常好了，可是两个轴混控的效果怎样还不一定。有可能会抽（两个轴的控制量叠加起来，特别是较大的D，会引起抽搐）。假设抽了。减少PD的值。I基本不用变。

5、加上偏航的修正參数后（直接给双环參数，角度外环P和横滚差点儿相同，内环P比横滚大些。I和横滚差点儿相同，D能够先不加）。拿在手上试过修正和打舵方向正确后能够试飞了（试飞非常危急。！

。！选择在宽敞、无风的室内，1米的高度（高度太低会有地面效应干扰，太高不easy看清姿态且easy摔坏），避开人群的地方比較适合，如有意外情况，立马关闭油门。！。

5.1试飞时主要观察这么几个方面的情况，一般经过调整的參数在平衡位置不会大幅度震荡，须要观察：

5.1.1在平衡位置有没有小幅度震荡（可能是因为机架震动太大导致姿态解算错误造成。也可能是角速度内环D的波动过大，前者能够加强减震措施。传感器下贴上3M胶，必要时在两层3M泡沫胶中夹上“减震板”，注意：铁磁性的减震板会干扰磁力计读数；后者能够尝试减少D项滤波的截止频率）。

5.1.2观察打舵响应的速度和舵量回中后飞机的回复速度。

5.1.3各个方向（记得測试右前。左后等方向）大舵量突加输入并回中时是否会引起震荡。如有。尝试减小内环PD也可能是因为“右前”等混控方向上的舵量太大造成。

6、横滚和俯仰调好后就能够调整偏航的參数了。合适參数的推断标准和之前一样。打舵高速响应。舵量回中飞机立马停止转动（參数D的作用）。

至此。双环PID參数调节完成！祝爽飞！