-
对串级
PID
和单级
P
ID
的理解
(
基于匿名微型六轴
)
来源：本站
|
作者：小古
|
发表时间：
2015-04-01 |
点击量：
14145
/********************
(C)
COPYRIGHT
2014
ANO
Tech
***************************
*
文件名
：
ANO_
*
描述
：飞行控制
***********
**************************************************
********
*************/
include
ANO_FlyControl fc;
/*
先整定内环，后整定外环。
参数整定找最佳，从小到大顺序查
先是比例后积分，最后再把微分加
曲线振荡很频繁，比例度盘要放大
曲线漂浮绕大湾，比例度盘往小扳
曲线偏离回复慢，积分时间往下降
曲线波动周期长，积分时间再加长
曲线振荡频率快，先把微分降下来
动差大来波动慢。微分时间应加长
理想曲线两个波，前高后低
4
比
1
*/
/*
ROLL
和
PIT
轴向按照以上公式计算
PID
输出，但
YAW
轴比较特殊，因为偏航角
法线方向刚好和地球重力平
行，
这个方向的角度无法由加速度计直接测得，
需要增加一个电子罗盘来替代加速度
计。如果不使用罗盘的话，
我们可以单纯的通过角速度积分来测得偏航角，
缺
点是由于积分环节中存在积分
漂移，偏航角随着时间的推移
<
/p>
会偏差越来越大。我们不使用罗盘就没有比例项，只仅使用微分环节来控制。
*/
ANO_FlyControl::ANO_FlyControl()
{
yawRate = 120;
//
重置
PID
参数
< br>
PID_Reset();
}
//
重置
P
ID
参数
void
ANO_FlyControl:
{
//
因为
YAW
角度会漂移，所以参数和
ROLL
、
PITCH
不一样
pid[PIDROLL].set_pid(70, 15,
120, 2000000); //ROLL
角度的内环控制系
数
,20000:
积分上限
pid[PIDPITCH].set_pid(70, 30, 120,
2000000);//PITCH
角度的内环控制系数
pid[PIDYAW].set_pid(100,
50, 0, 2000000);
//YAW
角度的内环控制系数
pid[PIDLEVEL].set_pid(280, 0, 0, 0);
//
外环控制系数
pid[PIDMAG].set_pid(15, 0, 0, 0);
//
电子罗盘控制系数
}
/*
【扫盲知识】
< br>串级
PID
：采用的角度
P
和角速度
PID
的双闭环
< br>PID
算法
------>
角度
的误差被
作为期望输入到角速度控制器中
（角度的微分就是角速度）
对于本
系统则采用了将角度控制与角速度控制级联的方式组成整个串级
PID
控
ID_Reset(void)
制器。
串级
PID
算法中，角速度内环占
着极为重要的地位。在对四旋翼飞行的物理模
型进
行分析后，可以知道造成系统不稳定的物理表现之一就是不稳定的角速度。
因此，
若能够直接对系统的角速度进行较好的闭环控制，
必然会改善系统的动态
特性
及其稳定性，
通常也把角速度内环称为增稳环节。
而角
度外环的作用则体现在对
四旋翼飞
行器的姿态角的精确控制。
外环：
输入为角度
,
输出为角速度
内环：输入为角速度，输出为
PWM
增量
使用串级
pid
，
分为：角度环控制
pid
环，和角速度控制环稳定环。主调为角
度
环（外环），副调为角速度环（内环）。
< br>参数整定原则为先内后外，故在整定内环时将外环的
PID
均设为
0
所谓外环就是只是一个
P
在起作用，也就是比例在起作用；
P
也
就是修正力度，
越大越容易使飞机震荡。
震荡的特点是：频率小、幅度大
*/
/*
【横滚（
Roll
）和俯仰（
Pitch
）
的控制算法】
横滚（
Roll
）和俯仰（
Pitch
）的控制算法是一样
的，控制参数也比较接近。
首先得到轴姿态的角度差
（
angle
error
）
，
将这个值乘以角度系数
p
后限幅
（限
幅
必
须
有
，
否
则
剧
烈
打
舵
时
容
易
引
发
< br>震
荡
）
作
为
角
速
度
控
制
器
期
望
值
（
target_rate
）。
target_rate
与陀螺仪得到的当前角
速度作差，得到角速度
误差（
rate_error
）乘以
kp
得到
P
。在
I
值小于限幅值（这个值大概在
5%
油门）
或者
rat
e_error
与
i
值异号时将
rate_error
累加到
I
中。
前后两次
rate_error
的差作为
D
项，值得注意的是加需要入
20hz
（也可以采用其它合适频率）滤波，
以避免震
荡。将
P,I,D
三者相加并限幅（
5
0%
油门）得到最终
PID
输出。
*/
//<
/p>
串环
PID
调节详情参见：
/super_mic ... 36723
//
飞行器姿态外环控制
void
ANO_FlyControl::Attitude_Outter_Loop(void)
{
int32_t errorAngle[2];
Vector3f Gyro_ADC;
//
计算角度误差值
,
角度误差值
=
期望值
-
此刻姿态值
//constrain_
int32
作用：
32
位整型数限幅，
使其控制输入的最大飞行倾角不大
于
2
5
度（如果控制量比
25
度大，飞机早
就坠毁了）
//d[ROLL]
：遥控数据
.x
：此刻姿态
(
角度
)
//1.
得到轴姿态的角度
差（
errorAngle
）
//2.
这个角度差值进行限幅
(const
rain_int32)
（正负
FLYANGLE_MAX
）（限幅必
须有，
否则剧烈打舵时容易引发
震荡）
作为角速度控制器期望值
（
ta
rget_rate
）
errorAngle[ROLL]
=
constrain_int32((d[ROLL]
*
2)
,
-((int)FLYANGLE_MAX), +FLYANGLE_MAX) -
.x * 10;
errorAngle[PITCH]
=
constrain_int32((d[PITCH]
*
2)
,
-((int)FLYANGLE_MAX), +FLYANGLE_MAX) -
.y * 10;
//
获取此时
陀螺仪上的角速度，取角速度的四次平均值
Gyro_ADC = _Gyro() / 4;
/*
得到外环
PID
输出（角速度的差值）
(
实质是相当于内环的
P
比例项
)-------->
_rate
与陀螺仪得到的当前角速度作差，得到角速度误差（
RateErro
r
）
乘以
kp
（外环控制系数
pid[PIDLEVEL]--->(280, 0, 0, 0)
）得到给内环的
P
。
< br>
*/
//
横滚
roll
：外环控制。输入为角度
,
输出为角速度。
RateError[ROLL]
作为内
环的输入。
RateError[ROLL] =
pid[PIDLEVEL].get_p(errorAngle[ROLL]) -
Gyro_ADC.x;
//Gyro_ADC.x:
陀螺仪
X
轴的值
//
俯仰
pitch
< br>：外环控制。输入为角度
,
输出为角速度。
RateError[PITCH]
作为
内环的输入。
RateError[PITCH]
=
pid[PIDLEVEL].get_p(errorAngle[PITCH])
-
-
-
-
-
-
-
-
-
-
上一篇：植物中英文对照
下一篇：河北教育出版社六年级英语下册教案