PID+模糊逻辑控制四轴悬停的思路（老子终于调通了）

camion

原帖在http://www.geek-workshop.com/thread-4556-1-2.html

对问题经过了反复的尝试，先写了一个纯粹模糊逻辑的算法， 不光各种 if elseif 老长，而且对三个指标任然控制不好，直到想出来了这个： （如要在matlab中运行，需要改注释//为%, 将elseif 改为 elsif， 将自定义的PorportionDet函数移到主代码外重新定义）

clear;
//basic para
L=0.5;
m=0.8;
J=2;
g=10;
delt_T=0.01;
N=900;

function result = PorportionDet(limA, limB, outA, outB, x)
    a = (x - limB) / (limA-limB);
    result = a*outA + (1-a)*outB;
endfunction

//consigne
ver_vit_standard = 0;
hori_vit_standard = 0;
angle_standard =  0;
//registres
F_basic = zeros(1,N);
F_rot = zeros(1,N);
command_basic  = zeros(1,N);
command_rot = zeros(1,N);

angle_acc = zeros(1,N);
angle_vit = zeros(1,N);
angle = zeros(1,N);

hori_acc = zeros(1,N);
hori_vit = zeros(1,N);
hori = zeros(1,N);

vert_acc = zeros(1,N);
vert_vit = zeros(1,N);
vert = zeros(1,N);

error_angle = zeros(1,N);
error_vert_vit = zeros(1,N);

sum_error_angle = zeros(1,N);
sum_error_vert_vit = zeros(1,N);

error_hori_vit = zeros(1,N);
sum_error_hori_vit = zeros(1,N);
//PID paras
P_basic=2;
I_basic=3.5;
D_basic=0.2;

P_rot=16;
I_rot=0.02;
D_rot=13;

// time 1 sample
F_rot(1)= 4.9;
F_basic(1) =0.1;

angle(1) =0.1;
angle_vit(1) = 0.2;
angle_acc(1) = F_rot(1)*L/J;
vert_vit(1) =1.2;
vert_acc(1) = ( 2*F_basic(1)*cos(angle(1)) - m*g )/m ;
error_angle(1) = angle_standard - angle(1);
error_vert_vit(1) = ver_vit_standard- vert_vit(1);
sum_error_angle(1) = error_angle(1)*delt_T;
sum_error_vert_vit(1) = error_vert_vit(1)*delt_T;
hori_acc(1) = -( 2*F_basic(1)*sin(angle(1)) )/m ;
hori_vit(1)=0;

//recurrence
for i=2:N
    F_rot(i)= command_rot(i-1);
    F_basic(i)= command_basic(i-1);
   
    angle_acc(i) = F_rot(i)*L/J;
    angle_vit(i) = angle_vit(i-1) + angle_acc(i-1)*delt_T;
    angle(i) = angle(i-1) + angle_vit(i-1)*delt_T;
   
    vert_acc(i) = ( 2*F_basic(i)*cos(angle(i)) - m*g )/m ;
    vert_vit(i) = vert_vit(i-1) + vert_acc(i-1)* delt_T;
    vert(i) = vert(i-1) + vert_vit(i-1)*delt_T;
    error_vert_vit(i) =   ver_vit_standard - vert_vit(i);
    sum_error_vert_vit(i) = sum_error_vert_vit(i-1) + error_vert_vit(i)*delt_T;
   
    hori_acc(i) =  -( 2*F_basic(i)*sin(angle(i)) )/m ;
    hori_vit(i) = hori_vit(i-1) + hori_acc(i-1)*delt_T;
    hori(i) = hori(i-1) + hori_vit(i-1)*delt_T;
    error_hori_vit(i) = hori_vit_standard - hori_vit(i);
   
    if ( hori_vit(i)> 3 )
        angle_standard = 0.3;
        error_angle(i) = angle_standard - angle(i);
        sum_error_angle(i) = sum_error_angle(i-1) + error_angle(i)* delt_T;
        command_rot(i)= P_rot*error_angle(i) + D_rot*( error_angle(i) - error_angle(i-1) )/delt_T + I_rot*sum_error_angle(i);
        command_basic(i) = I_basic* sum_error_vert_vit(i) +P_basic*error_vert_vit(i) + D_basic*(error_vert_vit(i)-error_vert_vit(i-1))/delt_T ;
    elseif ( hori_vit(i)> 0.1 &  hori_vit(i)< 3 )
        angle_standard = PorportionDet(0.1, 3, 0.1, 0.3, angle(i) );
        error_angle(i) = angle_standard - angle(i);
        sum_error_angle(i) = sum_error_angle(i-1) + error_angle(i)* delt_T;
        command_rot(i)= P_rot*error_angle(i) + D_rot*( error_angle(i) - error_angle(i-1) )/delt_T + I_rot*sum_error_angle(i);
        command_basic(i) = I_basic* sum_error_vert_vit(i) +P_basic*error_vert_vit(i) + D_basic*(error_vert_vit(i)-error_vert_vit(i-1))/delt_T ;
    elseif ( hori_vit(i)> -0.1 &  hori_vit(i)< 0.1 )
        //angle_standard = PorportionDet(-0.1, 0.1, -0.1, 0.1, angle(i) );
        angle_standard = 0;
        error_angle(i) = angle_standard - angle(i);
        sum_error_angle(i) = sum_error_angle(i-1) + error_angle(i)* delt_T;
        command_rot(i)= P_rot*error_angle(i) + D_rot*( error_angle(i) - error_angle(i-1) )/delt_T + I_rot*sum_error_angle(i);
        command_basic(i) = I_basic* sum_error_vert_vit(i) +P_basic*error_vert_vit(i) + D_basic*(error_vert_vit(i)-error_vert_vit(i-1))/delt_T ;
    elseif ( hori_vit(i)> -3 &  hori_vit(i)< -0.1 )
        angle_standard = PorportionDet(-3, -0.1, -0.3, -0.1, angle(i) );
        error_angle(i) = angle_standard - angle(i);
        sum_error_angle(i) = sum_error_angle(i-1) + error_angle(i)* delt_T;
        command_rot(i)= P_rot*error_angle(i) + D_rot*( error_angle(i) - error_angle(i-1) )/delt_T + I_rot*sum_error_angle(i);
        command_basic(i) = I_basic* sum_error_vert_vit(i) +P_basic*error_vert_vit(i) + D_basic*(error_vert_vit(i)-error_vert_vit(i-1))/delt_T ;
    elseif ( hori_vit(i)< -3 )
        angle_standard = -0.3;
        error_angle(i) = angle_standard - angle(i);
        sum_error_angle(i) = sum_error_angle(i-1) + error_angle(i)* delt_T;
        command_rot(i)= P_rot*error_angle(i) + D_rot*( error_angle(i) - error_angle(i-1) )/delt_T + I_rot*sum_error_angle(i);
        command_basic(i) = I_basic* sum_error_vert_vit(i) +P_basic*error_vert_vit(i) + D_basic*(error_vert_vit(i)-error_vert_vit(i-1))/delt_T ;
    end
   
end

subplot(311);
plot (1:N, angle);
subplot(312);
plot (1:N, vert_vit);
subplot(313);
plot (1:N, hori_vit);

终于可以放心装飞机了

hunduncn

沙发。。。。

弘毅

{:soso_e103:}板凳。。求详细介绍。。。

洪亮

看着很复杂。没有详解是不行的。

camion

弘毅 发表于 2013-5-15 16:48
板凳。。求详细介绍。。。

弘毅前辈你好，我去年这个时候刚接触arduino，从弘毅的帖子中汲取了不少东西，在此表示感谢

详解如下：

1 物理模型

一开始为了简化，所有只设了一对浆， 控制变量有两个，F_basic 和 F_rot， 左浆升力为F_basic - F_rot，右浆升力F_basic + F_rot，
这样设的好处在于，F_basic只和倾角angle共同决定垂直方向上的加度素，而旋转只和F_rot有关。
因此初始条件都有：角度angle，角速度angle_vit，角加速度angle_acc，垂直方向加速度vert_acc,垂直方向速度vert_vit，
垂直位置vert，水平加速度hori_acc，水平速度hori_vit，水平位置hori，以及F_basic、F_rot的初始值。

如果变成实际的两对浆（四个马达），只需要将水平方向分解为x、y两个分量，只要四个轴是互相90度间隔便可以认为x，y方向是独立的，
而z轴有四个升力，则需要做修改。



本来各方向上速度，加速度应该由mp6050之类的传感器提供，我代码中是用物理模型做积分替代的，具体如下：
角度： angle_acc = F_rot*L/I ， angle_vit = angle_vit(初始) + angle_acc*delta_T (积分量)，
angle = angle（初始值）+  angle_vit*delta_T(积分量)  delta_T就是采样的基本事件，设定为 0.01s

同理，垂直方向：vert_acc(i) = ( 2*F_basic(i)*cos(angle(i)) - m*g )/m， 然后做积分得到速度和位置（基本的高中物理对吧）
水平方向： hori_acc(i) =  -( 2*F_basic(i)*sin(angle(i)) )/m  做积分或参考代码里的。。。

需要注意的一点是：方向上垂直向上设为正，水平向右设为正，角度由右侧水平逆时针旋转设为正，这样符号上就不会出错了。


2 PID篇

坛子里关于PID还是有很多信息的，这里具体应用为： 利用垂直方向反馈量（即物理模型中的积分值）和标准值（要求控制达到的量，此处
要求悬停故为0）之差err_vert_vit来控制 F_basic ， 用角度反馈量和标准量之差err_angle 来控制 F_rot

标准的数字采样PID控制举例： command(i) = P* err(i) + D* (err(i) -err(i-1)) + I* 累计积分(err(i))
其中 I项 作用在于消除静态误差， P项和D项 用于加速收敛，区别在于D项感知的是二阶或以上影响量

顺便说下，PID不好调，很容易出震荡或者干脆不收敛，需要对着记录曲线反复尝试，可参考坛子里PID调试的帖子
我代码里面在本回合采样算出command_rot和command_basic后赋给下一回合的F_rot 和F_basic
command_rot(i)= P_rot*error_angle(i) + D_rot*( error_angle(i) - error_angle(i-1) )/delt_T + I_rot*sum_error_angle(i);
command_basic(i) = I_basic* sum_error_vert_vit(i) +P_basic*error_vert_vit(i) + D_basic*(error_vert_vit(i)-error_vert_vit(i-1))/delt_T ;

在http://www.geek-workshop.com/thread-4556-1-2.html中可以看到角度和垂直速度的控制是很不错的

模糊逻辑待续

camion

洪亮 发表于 2013-5-15 18:44
看着很复杂。没有详解是不行的。

3 模糊逻辑篇

模糊逻辑是由L. Zadeh提出的，名字看起来是阿拉伯裔，这哥们nb在于90多岁了还在勤奋工作（等他死了我还真想给他和那个火眼金睛卡尔曼
献个花圈神马的）， 其实模糊逻辑是很直观的，或者说我们正常人很多反应都是自动基于模糊逻辑控制的。

举个栗子吧，如果现在有一水盆，热水管提供80度的水，冷水管提供10度的水，想洗一个40度的热水浴，大部分人会怎么做？
如果是我，我会一只手放在水里面，如果水太冷了，那么我就放大热水管，关小或者干脆关闭冷水管，随着水温升高接近40度，我热水比例
逐渐变小，冷水比例逐渐升高。反之亦然，没错，这就是模糊逻辑。

模糊逻辑实际就是一个操作表格：水烫（100）-->放大冷水（20），水较烫（60）-->放小冷水（30），水温刚好（40）-->放温水（40），
水较冷（20）-->放小热水（50），水冷（0）-->放大热水（80）。中间情况作过渡处理，例如，

介于水烫和较烫之间的 70度=25%*100 + 75%*60, 那么放 25%的大冷水+75%的小冷水， 即 25%*20 + 75%*30 =27.5


function result = PorportionDet(limA, limB, outA, outB, x)
    a = (x - limB) / (limA-limB);
    result = a*outA + (1-a)*outB;
endfunction

就是为这个过渡写的 ， 翻译为 处理结果=PorportionDet（条件边界A，条件边界B，边界A处理值，边界B处理值，输入条件）
当然要求 条件边界A < 输入条件 < 条件边界B
在上例中 放水温度 = =PorportionDet(60， 100， 30， 20 ，70)，算出来就是27.5



之前PID算法能够很好的让飞机保持某个角度或某个垂直速度，但是水平速度完全没有控制，因为err_hori_vit 完全没有纳入反馈值中去
现在想了个办法，如果飞机水平速度向左，而飞机向右倾斜，那么水平速度会自然而然的降低，如果飞机水平向左，而飞机向左倾斜，那么
水平向左的速度只会越来越大，一定要想办法让倾角调整为向右，做得到吗？PID做得到，只要设定angle_standard为一个向右倾的就可以了，
如此一来，向左的速度会逐渐调整到0，甚至调整过大的话变成向右。

以下是我的模糊逻辑表格：
如果水平速度>-3m/s，那么让飞机对着倾角为-0.3rad （约为34度）做PID调整；如果 -3m/s<水平速度<-0.1m/s，让飞机对着倾角-0.1rad做
PID调整；如果水平速度>3m/s，那么让飞机对着倾角为0.3rad （约为34度）做PID调整；如果 3m/s>水平速度>0.1m/s，让飞机对着倾角0.1rad做
PID调整；中间用PorportionDet做过渡

如果-0.1m/s<水平速度<-0.1m/s, 等等，这是不是意味着我们飞机水平速度接近零了？对啊，那么让飞机对倾角为0做PID调整

最后的结果是倾角和垂直速度为零，水平速度大于负0.1m/s小于0.1m/s， 当然你可以根据自己喜好改成0.05 0.01 神马的，不过我怀疑改太小有
可能出现震荡

模糊逻辑代码就是下面的了

    if ( hori_vit(i)>= 3 )
        angle_standard = 0.3;
        error_angle(i) = angle_standard - angle(i);
        sum_error_angle(i) = sum_error_angle(i-1) + error_angle(i)* delt_T;
        command_rot(i)= P_rot*error_angle(i) + D_rot*( error_angle(i) - error_angle(i-1) )/delt_T + I_rot*sum_error_angle(i);
        command_basic(i) = I_basic* sum_error_vert_vit(i) +P_basic*error_vert_vit(i) + D_basic*(error_vert_vit(i)-error_vert_vit(i-1))/delt_T ;
    elseif ( hori_vit(i)> 0.1 &  hori_vit(i)< 3 )
        angle_standard = PorportionDet(0.1, 3, 0.1, 0.3, angle(i) );
        error_angle(i) = angle_standard - angle(i);
        sum_error_angle(i) = sum_error_angle(i-1) + error_angle(i)* delt_T;
        command_rot(i)= P_rot*error_angle(i) + D_rot*( error_angle(i) - error_angle(i-1) )/delt_T + I_rot*sum_error_angle(i);
        command_basic(i) = I_basic* sum_error_vert_vit(i) +P_basic*error_vert_vit(i) + D_basic*(error_vert_vit(i)-error_vert_vit(i-1))/delt_T ;
    elseif ( hori_vit(i)>= -0.1 &  hori_vit(i)<= 0.1 )
        //angle_standard = PorportionDet(-0.1, 0.1, -0.1, 0.1, angle(i) );
        angle_standard = 0;
        error_angle(i) = angle_standard - angle(i);
        sum_error_angle(i) = sum_error_angle(i-1) + error_angle(i)* delt_T;
        command_rot(i)= P_rot*error_angle(i) + D_rot*( error_angle(i) - error_angle(i-1) )/delt_T + I_rot*sum_error_angle(i);
        command_basic(i) = I_basic* sum_error_vert_vit(i) +P_basic*error_vert_vit(i) + D_basic*(error_vert_vit(i)-error_vert_vit(i-1))/delt_T ;
    elseif ( hori_vit(i)> -3 &  hori_vit(i)< -0.1 )
        angle_standard = PorportionDet(-3, -0.1, -0.3, -0.1, angle(i) );
        error_angle(i) = angle_standard - angle(i);
        sum_error_angle(i) = sum_error_angle(i-1) + error_angle(i)* delt_T;
        command_rot(i)= P_rot*error_angle(i) + D_rot*( error_angle(i) - error_angle(i-1) )/delt_T + I_rot*sum_error_angle(i);
        command_basic(i) = I_basic* sum_error_vert_vit(i) +P_basic*error_vert_vit(i) + D_basic*(error_vert_vit(i)-error_vert_vit(i-1))/delt_T ;
    elseif ( hori_vit(i)<= -3 )
        angle_standard = -0.3;
        error_angle(i) = angle_standard - angle(i);
        sum_error_angle(i) = sum_error_angle(i-1) + error_angle(i)* delt_T;
        command_rot(i)= P_rot*error_angle(i) + D_rot*( error_angle(i) - error_angle(i-1) )/delt_T + I_rot*sum_error_angle(i);
        command_basic(i) = I_basic* sum_error_vert_vit(i) +P_basic*error_vert_vit(i) + D_basic*(error_vert_vit(i)-error_vert_vit(i-1))/delt_T ;
    end

camion

开个玩笑哈哈，如果我80%是个坏人，20%是个英雄的话，你们应该把我 “打个80%的半死 + 给我开20%的模范奖金”

隨風大俠

前輩您好：
我最近也再做四軸，想請問前輩，您的程式碼有要帶什麼函式庫嗎??

我參照網路上找到的互補濾波和PID演算，都無法平衡!!

互補濾波：

angleA= atan2(compass.a.y , compass.a.z) * 180 / 3.14159;
  omega= 0.068 * (gyro.g.x );
  
  angleA2= atan2(compass.a.x , compass.a.z) * 180 / 3.14159;
  omega= 0.068 * (gyro.g.y );
  
  unsigned long now = millis();                           // 当前时间(ms)
  float dt = (now - preTime)/* / 1000.0*/;                    // 微分时间(ms)
  preTime = now;  
  float K = 0.8;                    
  float A = K / (K + dt);                    
  f_angle = A * (f_angle + omega * dt) + (1-A) * angleA;
  f_angle2=A * (f_angle2 + omega2 * dt) + (1-A) * angleA2;

PID:

    now = millis(); // 當前時間(ms)
    float TimeCh = (now - lastTime) / 1000.0; // 採樣時間(s) 這裡怪怪的，採樣是S，因為已經是
                                           millis()，應該是要乘1000吧??
     // 比例係數、積分係數、微分係數
   //float SampleTime = 0.1; // 採樣時間(s)
    float Setpoint = 0; // 設定目標值(degree)
    float outMin = -42.5, outMax = 42.5; // 輸出上限、輸出下限
   
   
   // if(TimeCh >= SampleTime) // 到達預定採樣時間時
    //{
        float Input = f_angle, Input2 = f_angle2; // 輸入賦值
        float error = Setpoint - Input, error2 = Setpoint - Input2; // 偏差值
        ITerm+= (error * TimeCh); // 計算積分項
        ITerm = constrain(ITerm, outMin, outMax); // 限定值域
        float DTerm = (error - lastError) / TimeCh; // 計算微分項
        Output = Kp * error + Ki * ITerm + Kd *  DTerm; // 計算輸出值
        Output = constrain(Output, outMin, outMax); // 限定值域
        
     
         // 控制左驅
      //if(lastError>Input)  
        B = 117.5 - Output;   
        B = B+Bin;     
        B = constrain(B, 95, 180);
        myservo2.write(B);  
              
        // 控制右驅
     D = 119.5 + Output;
        D = D+Din;
        D = constrain(D, 95, 180);
        myservo4.write(D);

馬達控制是使用Servo.h函式，能夠反應的範圍是95~180，但是我看很多人好像不用Servo.h函式控制電調，而是用analogWrite，請問前輩，這兩者有差別嗎??

謝謝

camion

TTTTTTT33 发表于 2013-5-16 13:24
前輩您好：
我最近也再做四軸，想請問前輩，您的程式碼有要帶什麼函式庫嗎??

你好，首先不要叫我前辈啦，我还不到30

是这样的，我的飞机还没有正常飞起来呢，写这个帖子意义在于在快递没么有送到（当然现在无刷加25C电池已经到了）的情况下现在软件中模拟测试控制算法，因为在网上看到不少人因为平衡没调好一头栽在地上摔坏无刷之类的，如果那天我成功飞起来的话会开源的



我前天调通了无刷的驱动，用的是servo库，但是每次打开串口时马达转速好像都会出错

可以看看 http://www.diy-robots.com/?page_id=1062 他说analogwrite范围是20到40，还没有测试

隨風大俠

那您是在模擬上已經沒問題了??

camion

TTTTTTT33 发表于 2013-5-16 13:48
那您是在模擬上已經沒問題了??

你可以在scilab上（或者matlab）跑我的程序，给出任意的初始速度，加速度，角加速度应该都可以自动调整到悬停状态

camion

TTTTTTT33 发表于 2013-5-16 13:48
那您是在模擬上已經沒問題了??

看头像很好奇台湾人还会在某些传统节日穿汉服吗？

隨風大俠

這頭像不是我啦，是新三國裡的許攸~~，看到這集時，覺得他笑得很奸，就截下來用了。

請問您有用arduino控制過電調嗎??

camion

TTTTTTT33 发表于 2013-5-16 13:58
這頭像不是我啦，是新三國裡的許攸~~，看到這集時，覺得他笑得很奸，就截下來用了。

請問您有用ar ...

#include <Servo.h>
Servo myservo;
int val;   
void setup()
        {
          myservo.attach(9,1000,2000);  
          delay(2500);
          myservo.writeMicroseconds(1000);
          delay(2000);
        }
void loop()
        {
          val =100； // 0~180
          myservo.write(val);
          delay(15);
       }

无刷pwm接在 9号口， 还有无刷电机真心烫手

隨風大俠

請問myservo.attach(9,1000,2000);  1000和2000分別是什麼啊??

電機轉久了要小心被燙到!!

謝謝