轮式驱动单元电机PID控制说明



原文地址：http://blog.exbot.net/archives/66       

       PID控制是一种简单有效且具有较强鲁棒性较强的控制手段，在任何一本关于自动控制的教材中均可找到相应的介绍，在此不过多介绍基本原理，而侧重于程序的使用方法及其在轮式驱动单元中的测试结果。
       目前机器人的电机大多采用脉宽调制（Pulse width modulation）或简称PWM进行控制，而不是使用模拟功率电路。在软件中通过改变脉冲宽度（如图1，上图对下图），我们可以改变等效的模拟电机信号，从而达到控制电机转速的目的。电机系统的行为就像是一个积分器，在一定时间的范围内对数字信号进行积分。而 则称为占空比。

 

图1 PWM

问题1 电机控制的非线性
      比较遗憾的是，通常情况下，电机所产生的速度与PWM信号比并非是线性关系，如图2是我们对轮式驱动单元进行实验的结果，而图3右图则是我们查阅得到的Faulhaber 2230电机的测量曲线，图3左图是电机的阶跃响应。由于实验的电机的减速箱和速度的单位的不同，图2、3所得到的具体数值不尽相同，但它们却共同反映了电机PWM控制的一个特性——非线性！

 

图2 轮式单元车轮转速（周每秒）/PWM

 

图3 Faulhaber 2230电机的阶跃响应与速度/PWM比

 
问题2 开环控制的不准确
       通常电机可以实现正/反转、速度调节。但是，我们却并不知道电机实际的运行速度。应当注意到电机的实际运行速度并不仅仅只与输入的PWM信号有关，还会受到例如负载（例如机器人的重量或是驱动区域的倾斜程度）等外部因素的影响。如果使用开环控制的话，在低速情况下，电机的扭矩通常也非常的小。
 
解决方法 增加PID控制
    电机增加编码盘后，便为闭环控制的实现提供了物质基础。但仅仅是简单的反馈是不够的，简单的比例P反馈控制利用误差进行校正，而误差却是始终存在的（只是能将控制在有限的范围内），若想实现零稳态误差响应，还需要增加一个积分I环节。如果想进一步提高机器人的灵敏度（动态响应性能）还可以增加一个微分环节D，这样便可以组成一个PID控制器。但在这里不建议大家使用微分D环节，因为稍有不慎便有可能造成振荡，而在此应用中D环节对性能的提升也十分有限。下面我们介绍一下我们编写的程序。
 
PID控制程序
    注：我们是在Arduino下进行的开发，但此程序进行简单的修改便可移植到任何类型的单片机上。
1、连接接口针号设置
       A、B、C分别表示三个不同的轮式单元，ctrl_1、ctrl_2、ctrl_3表示三个电机控制线，counter是编码盘输入，analog是电机电流检测（用于观测电机是否堵转，当读数超过532并持续一段时间时，表示堵转，应及时断开电机以避免烧毁）

const int A_ctrl_1=3;          //PWM OUT const int A_ctrl_2=4;          //Motor control 2 const int A_ctrl_3=12;          //Motor control 3 const int A_counter= 2;        //Motor counter input const int A_analog = 0;       //Motor current measure input const int B_ctrl_1=5;          //PWM OUT const int B_ctrl_2=4;          //Motor control 2 const int B_ctrl_3=12;          //Motor control 3 const int B_counter= 19;        //Motor counter input const int B_analog = 4;       //Motor current measure input const int C_ctrl_1=6;          //PWM OUT const int C_ctrl_2=7;          //Motor control 2 const int C_ctrl_3=8;          //Motor control 3 const int C_counter= 20;        //Motor counter input const int C_analog = 5;       //Motor current measure input

2、主要参数设置
    Kp为比例值，我们设为1，通常越大调节速度越快，代价是有可能出现超调，或超调过大甚至是不稳定。Ki是积分项，我们设为4.

/you can chane the following const paramters  const int CURRENT_LIMIT=530; // high current warning, if the value of Motor curren is bigger than CURRENT_LIMIT more than 10 sample, stop. const int NUM_C=200;      //the number of counter of a roll.                            //IF select CHANGE in attachInterrupt(0, rencoder, CHANGE) ,the  NUM_C should be multiple by 2                             //Other selection in attachInterrupt(0, rencoder, FALLING), the  NUM_C is equal to the number of counter of a roll const int LOOPTIME =50;   //the loop time(ms) of PID control float Kp=1;          // PID proportional control gain float Ki=4;          // PID i control gain const float K_modal=1;     // K_modal is the modal number of motor. it is eaual 255/(the full RPS of motor).  RPS is roll per second ///

3、PI控制程序
      我们首先需要将码盘连接至一个中断，每来一次中断便counter++。当中断设为FALLING或RISING时NUM_C设为100，因为码盘100个齿每转一周产生100个FALLING中断。当中断设为CHANGE时，则是NUM_C设为200，因为码盘100个齿每转一周产生200个CHANGE中断。然后我们通过记录一个周期looptime内counter的增加数便可得到实际的转动速度（转/秒），如下。

speed_act= float (count- count_fomer)*1000/ float (looptime*NUM_C);

误差项为期望速度（转/秒）减去实际速速。

error = abs (desire_act) - abs (speed_act);

PID控制量如下：
    

pidTerm =  Kp * error  + Kp * Ki * (error +last_I);

         

更新积分值，注意last_I应使用静态变量，以不断积分。
    

last_I = error + last_I;

PI完整代码如下，我们的代码非常简单，关键的只有几行（便于学习），并且在经过了实际的测试，即使在低速的情况下仍能保持较大的力矩，无论上坡还是下坡均能保持指定的速度巡航。我们发现在Arduino论坛里有一个PID类（附件中），感兴趣的读者可以拿来研究使用一下。

float PID_updata( float command, float targetValue, float currentValue) //computePWM value {         float pidTerm = 0;                             // PID correction      float error=0;                          static float last_I=0;      error = abs (targetValue) - abs (currentValue);      pidTerm =  Kp * error  + Kp * Ki * (error + last_I);                                last_I = error + last_I;      return pidTerm; }   int control_loop( int looptime , float speed_req, int PWM_val) {      long lastMilli;    long count_fomer;    float speed_act;    lastMilli=millis();    count_fomer=count;    interrupts();    while ((millis()-lastMilli) <= looptime)     {  ;  } // enter tmed loop                                                          noInterrupts();    speed_act= float (count- count_fomer)*1000/ float (looptime*NUM_C);    PWM_val= PID_updata(PWM_val, speed_req, speed_act);   // compute PWM    return constrain(PWM_val, 0, 255); } //