求大神指导这个姿态控制PID有木有问题
以前发过帖子,还是没搞明白,贴上自己的程序,求大家能给小弟一点指导:思路是这样的,大家看对不对:一个一个通道的调,先调俯仰通道,姿态是有商业产品以100HZ的频率发送姿态数据,DSPF2812串口A接四旋翼姿态收数据,串口B接收上位机发送的设定的俯仰偏航角度值(程序里roll_sp,pitch_sp,调试是设的值都为0)P,I,D (下面主程序 set_KP, set_KI, set_KD),中断接收数据,这样一个简单的但PID回路,用姿态角反馈,可调试时怎么也调不好这几个参数,不知道这个思路有没有问题,如果没问题该怎么调,是先把I,D设成0,调P,好了之后再调I,然后再调D吗?这样我也试过,但效果也不好,弄了好久都没进展,求大神们指导,非常感谢!
<1>这个是PID控制程序,是师兄参考论坛上的写的
void PID_control( struct control *axis)
{
axis->result = axis->error*axis->kp;//;set_KP
axis->accerror = axis->accerror + axis->error*0.01;//
if( axis->accerror > axis->accerror_limt)
{ axis->accerror = axis->accerror_limt; }
else if( axis->accerror < -axis->accerror_limt)
{ axis->accerror = -axis->accerror_limt;}
axis->result = axis->result + axis->accerror*axis->ki;
axis->result = axis->result + 100*( axis->error - axis->errorp )*axis->kd;
axis->errorp = axis->error;
}
void init_PID()
{
roll_c.errorp = 0;
roll_c.accerror = 0;
roll_c.accerror_limt = 500;
roll_c.kp = 3.80;
roll_c.ki = 0.00;
roll_c.kd = 0.480;
roll_c.result = 0;
pitch_c.errorp = 0;
pitch_c.accerror = 0;
pitch_c.accerror_limt = 500;
pitch_c.kp = 3.95; //1.58
pitch_c.ki =0.00; //;0.30
pitch_c.kd =0.680; //;0.420.38
pitch_c.result = 0;
head_c.errorp = 0;
head_c.accerror = 0;
head_c.accerror_limt = 1000;
head_c.kp = 2;
head_c.ki = 0.1;
head_c.kd = 10;
head_c.result = 0;
height_c.errorp = 0;
height_c.accerror = 0;
height_c.accerror_limt = 1000;
height_c.kp = 2;
height_c.ki = 0;
height_c.kd = 0;
height_c.result = 0;
}
<主程序>
for(;;)
{
if(zitai_flag == 1 )
{
zitai_flag = 0;
roll_c.error = roll_sp - mti_roll;
pitch_c.error = mti_pitch - pitch_sp;
head_c.error = mti_heading - head_sp;
height_c.error = height - head_sp;
phi = mti_roll;
theta = mti_pitch;
psi = mti_heading;
//roll_c.kp = set_KP;
//roll_c.ki = set_KI;
//roll_c.kd = set_KD;
pitch_c.kp = set_KP;
pitch_c.ki = set_KI;
pitch_c.kd = set_KD;
//PID_control( &roll_c );
PID_control( &pitch_c );
// PID_control( &head_c );
// PID_control( &height_c );
thr2 = -thr;//
DriveMotor2( (int)roll_c.result, (int)head_c.result, (int)pitch_c.result, (int)thr2 );
} 遇到同样问题摇摆非常厉害无法起飞我把P参数从很小到很大都试了个遍了莫非是PID用的不对
页:
[1]