四轴飞行器的设计之PID控制电机的参数整定——STC15单片机实战指南连载
时间：10-02
整理：3721RD
点击：
四轴的PID调试，过程真心比较累人，也比较复杂，这里以飞蜓二號的调试为准，为读者介绍其调试的过程。以下部分内容参考了博文“super_mice”的博文，以表感谢。这里不是直接告诉读者PID调试的结果，主要说明一些思路和经验，具体的源码后面会讲述。
软件方面最主要的是姿态解算，最终用的是MPU6050的DMP(Digital Motion Process)输出，遥控器控制的目标姿态也是四元数表示的。为了有较好的线性度，将两者的姿态差转换为欧拉角后再进行PID的控制。
PID算法很大程度上参考了APM(国外成熟开源飞控项目)的控制算法。它是采用的角度P和角速度PID的双闭环PID算法。角度的误差被作为期望输入到角速度控制器中。双闭环PID相比传统的单环PID来说性能有了极大的提升，笔者也曾经调试过传统的PID控制算法，即便参数经过了精心调整和双环控制算法相比在控制效果上的差距依旧很大，无论是悬停的稳定性，打舵时的快速跟随性和回正时的快速性上都是后者的效果明显优于前者。
其中欧拉角、四元数，后续再叙。横滚(Roll)和俯仰(Pitch)的控制算法是一样的，控制参数也比较接近。首先得到轴姿态的角度差(angle error)，将这个值乘以角度系数p后限幅(限幅必须有，否则剧烈打舵时容易引发震荡)作为角速度控制器期望值(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参数调整了。一般先进行横滚或俯仰方向的调整。把四轴固定起来，只留一个待调整方向的旋转自由度，这样就可以减少一个轴的分量，以便调试。不同的人，可能采用的方法和步骤不同，这里笔者总结几点，仅供读者参考。
①估计大概的起飞油门，以便有个参考基准;
②调整角速度内环参数;
③将角度外环加上，调整外环参数。
④横滚俯仰参数一般可取一致，将飞机解绑，抓在手中测试两个轴混合控制的效果(注意安全)，有问题回到“单轴”继续调整，直至飞机在手中不会抽搐。
⑤大概设置偏航参数(不追求动态响应，起飞后头不偏即可)，起飞后再观察横滚和俯仰轴向打舵的反应，如有问题再回到“单轴”调试模式。
⑥横滚和俯仰OK以后，再调整偏航轴参数以达到好的动态效果。
有了以上大概的调试流程，接下来，再来详细讲述调试过程，以及部分参数的定论。
(1)要在飞机的起飞油门基础上进行PID参数的调整，否则“单轴”的时候调试稳定了，飞起来很可能又会晃荡。
(2)内环的参数最为关键。理想的内环参数能够很好地跟随打舵(角速度控制模式下的打舵)控制量。在平衡位置附近(正负30度左右)，舵量突加，四轴快速响应;舵量回中，飞机立刻停止运动(几乎没有回弹和震荡)。
①首先改变程序，将角度外环去掉，将打舵量作为内环的期望(角速度模式，在APM中叫ACRO模式，在大疆中叫手动模式)。
②加上P。P太小，不能修正角速度误差表现为很“软”倾斜后难以修正，打舵响应也差;P太大，在平衡位置容易震荡，打舵回中或给干扰(用手突加干扰)时会震荡。合适的P能较好的对打舵进行响应，又不太会震荡，但是舵量回中后会回弹好几下才能停止(没有D)。
③加上D。D的效果十分明显，加快打舵响应，最大的作用是能很好地抑制舵量回中后的震荡，可谓立竿见影。太大的D会在横滚俯仰混控时表现出来(尽管在“单轴”调试时的表现可能很好)，具体表现是四轴抓在手里推油门会抽搐。如果这样，只能回到“单轴”降低D，同时P也只能跟着降低。D调整完后可以再次加大P值，以能够跟随打舵为判断标准。
④加上I。会发现手感变得柔和了些。由于笔者“单轴”的装置中四轴的重心高于旋转轴，这决定了在四轴偏离水平位置后会有重力分量使得四轴会继续偏离平衡位置。I的作用就可以使得在一定角度范围内(30度左右)可以修正重力带来的影响。表现打舵使得飞机偏离平衡位置，舵量回中后飞机立刻停止转动，若没有I或太小，飞机会由于重力继续转动。
(3)角度外环只有一个参数P。将外环加上(在APM中叫Stabilize模式，在大疆中叫姿态模式)，打舵会对应到期望的角度。P的参数比较简单。太小，打舵不灵敏;太大，打舵回中易震荡，以合适的打舵反应速度为准。
(4)至此，“单轴”调试的效果应该会很好了，但是两个轴混控的效果如何还不一定，有可能会抽(两个轴的控制量叠加起来，特别是较大的D，会引起抽搐)。如果抽了，降低P、D的值，I基本不用变。
(5)加上偏航的修正参数后(直接给双环参数，角度外环P和横滚差不多，内环P比横滚大些，I和横滚差不多，D可以先不加)，拿在手上试过修正和打舵方向正确后可以试飞了(试飞很危险，选择在宽敞、无风的室内，1米的高度(高度太低会有地面效应干扰，太高不容易看清姿态且容易摔坏)，避开人群的地方比较适合，如有意外情况，立刻关闭油门。
试飞时主要观察这么几个方面的情况，一般经过调整的参数在平衡位置不会大幅度震荡，需要观察。
在平衡位置有没有小幅度震荡，可能是由于机架震动太大导致姿态解算错误造成，也可能是角速度内环D的波动过大，前者可以加强减震措施，传感器下贴上3M胶，必要时在两层3M泡沫胶中夹上“减震板”，注意，铁磁性的减震板会干扰磁力计读数，后者可以尝试降低D项滤波的截止频率。
观察打舵响应的速度和舵量回中后飞机的回复速度。
各个方向(记得测试右前、左后等方向)大舵量突加输入并回中时是否会引起震荡，如有，尝试减小内环P、D，也可能是由于“右前”等混控方向上的舵量太大造成。
(6)横滚和俯仰调好后就可以调整偏航的参数了。合适参数的判断标准和之前一样，打舵快速响应，舵量回中飞机立刻停止转动(参数D的作用)。
最后说明下和本文配套的STC15开发板目前正在电子发烧友销售，如果需要请戳这里购买： http://bbs.elecfans.com/product/stc15.html 我将持续更新内容，下一节讲
软件方面最主要的是姿态解算，最终用的是MPU6050的DMP(Digital Motion Process)输出，遥控器控制的目标姿态也是四元数表示的。为了有较好的线性度，将两者的姿态差转换为欧拉角后再进行PID的控制。
PID算法很大程度上参考了APM(国外成熟开源飞控项目)的控制算法。它是采用的角度P和角速度PID的双闭环PID算法。角度的误差被作为期望输入到角速度控制器中。双闭环PID相比传统的单环PID来说性能有了极大的提升，笔者也曾经调试过传统的PID控制算法，即便参数经过了精心调整和双环控制算法相比在控制效果上的差距依旧很大，无论是悬停的稳定性，打舵时的快速跟随性和回正时的快速性上都是后者的效果明显优于前者。
其中欧拉角、四元数，后续再叙。横滚(Roll)和俯仰(Pitch)的控制算法是一样的，控制参数也比较接近。首先得到轴姿态的角度差(angle error)，将这个值乘以角度系数p后限幅(限幅必须有，否则剧烈打舵时容易引发震荡)作为角速度控制器期望值(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参数调整了。一般先进行横滚或俯仰方向的调整。把四轴固定起来，只留一个待调整方向的旋转自由度，这样就可以减少一个轴的分量，以便调试。不同的人，可能采用的方法和步骤不同，这里笔者总结几点，仅供读者参考。
①估计大概的起飞油门，以便有个参考基准;
②调整角速度内环参数;
③将角度外环加上，调整外环参数。
④横滚俯仰参数一般可取一致，将飞机解绑，抓在手中测试两个轴混合控制的效果(注意安全)，有问题回到“单轴”继续调整，直至飞机在手中不会抽搐。
⑤大概设置偏航参数(不追求动态响应，起飞后头不偏即可)，起飞后再观察横滚和俯仰轴向打舵的反应，如有问题再回到“单轴”调试模式。
⑥横滚和俯仰OK以后，再调整偏航轴参数以达到好的动态效果。
有了以上大概的调试流程，接下来，再来详细讲述调试过程，以及部分参数的定论。
(1)要在飞机的起飞油门基础上进行PID参数的调整，否则“单轴”的时候调试稳定了，飞起来很可能又会晃荡。
(2)内环的参数最为关键。理想的内环参数能够很好地跟随打舵(角速度控制模式下的打舵)控制量。在平衡位置附近(正负30度左右)，舵量突加，四轴快速响应;舵量回中，飞机立刻停止运动(几乎没有回弹和震荡)。
①首先改变程序，将角度外环去掉，将打舵量作为内环的期望(角速度模式，在APM中叫ACRO模式，在大疆中叫手动模式)。
②加上P。P太小，不能修正角速度误差表现为很“软”倾斜后难以修正，打舵响应也差;P太大，在平衡位置容易震荡，打舵回中或给干扰(用手突加干扰)时会震荡。合适的P能较好的对打舵进行响应，又不太会震荡，但是舵量回中后会回弹好几下才能停止(没有D)。
③加上D。D的效果十分明显，加快打舵响应，最大的作用是能很好地抑制舵量回中后的震荡，可谓立竿见影。太大的D会在横滚俯仰混控时表现出来(尽管在“单轴”调试时的表现可能很好)，具体表现是四轴抓在手里推油门会抽搐。如果这样，只能回到“单轴”降低D，同时P也只能跟着降低。D调整完后可以再次加大P值，以能够跟随打舵为判断标准。
④加上I。会发现手感变得柔和了些。由于笔者“单轴”的装置中四轴的重心高于旋转轴，这决定了在四轴偏离水平位置后会有重力分量使得四轴会继续偏离平衡位置。I的作用就可以使得在一定角度范围内(30度左右)可以修正重力带来的影响。表现打舵使得飞机偏离平衡位置，舵量回中后飞机立刻停止转动，若没有I或太小，飞机会由于重力继续转动。
(3)角度外环只有一个参数P。将外环加上(在APM中叫Stabilize模式，在大疆中叫姿态模式)，打舵会对应到期望的角度。P的参数比较简单。太小，打舵不灵敏;太大，打舵回中易震荡，以合适的打舵反应速度为准。
(4)至此，“单轴”调试的效果应该会很好了，但是两个轴混控的效果如何还不一定，有可能会抽(两个轴的控制量叠加起来，特别是较大的D，会引起抽搐)。如果抽了，降低P、D的值，I基本不用变。
(5)加上偏航的修正参数后(直接给双环参数，角度外环P和横滚差不多，内环P比横滚大些，I和横滚差不多，D可以先不加)，拿在手上试过修正和打舵方向正确后可以试飞了(试飞很危险，选择在宽敞、无风的室内，1米的高度(高度太低会有地面效应干扰，太高不容易看清姿态且容易摔坏)，避开人群的地方比较适合，如有意外情况，立刻关闭油门。
试飞时主要观察这么几个方面的情况，一般经过调整的参数在平衡位置不会大幅度震荡，需要观察。
在平衡位置有没有小幅度震荡，可能是由于机架震动太大导致姿态解算错误造成，也可能是角速度内环D的波动过大，前者可以加强减震措施，传感器下贴上3M胶，必要时在两层3M泡沫胶中夹上“减震板”，注意，铁磁性的减震板会干扰磁力计读数，后者可以尝试降低D项滤波的截止频率。
观察打舵响应的速度和舵量回中后飞机的回复速度。
各个方向(记得测试右前、左后等方向)大舵量突加输入并回中时是否会引起震荡，如有，尝试减小内环P、D，也可能是由于“右前”等混控方向上的舵量太大造成。
(6)横滚和俯仰调好后就可以调整偏航的参数了。合适参数的判断标准和之前一样，打舵快速响应，舵量回中飞机立刻停止转动(参数D的作用)。
最后说明下和本文配套的STC15开发板目前正在电子发烧友销售，如果需要请戳这里购买： http://bbs.elecfans.com/product/stc15.html 我将持续更新内容，下一节讲
四元数与滤波算法。
看看。正在学习中，支持下！
看看。正在学习中，支持下！
看看。正在学习中，支持下！
看看。正在学习中，支持下！
学习学习
学习学习