FPGA實(shí)現(xiàn)PID控制算法

一. 簡(jiǎn)介

相信大家對(duì)于PID控制算法，都不感到陌生了，平衡車就是靠它平衡起來的，還有飛控的平衡算法也是它，以及FOC中的閉環(huán)控制中也是用的它，它不僅簡(jiǎn)單，而且易于理解。那么本篇文章將簡(jiǎn)要介紹一下算法的原理，然后帶大家使用FPGA來實(shí)現(xiàn)(C語言實(shí)現(xiàn)過程特別簡(jiǎn)單)。

二. PID算法

PID取自比例、積分、微分三個(gè)英文字母的首字母。意味著算法由這三部分組成。

1. P 比例

運(yùn)算過程為 期望值 減去 當(dāng)前值 ，然后再乘上一個(gè)p系數(shù)，就得到了一個(gè)反饋值。比例的作用主要是為了讓 期望值 與 當(dāng)前值相等

2. I 積分

將誤差值不斷累加，然后再乘上一個(gè)I系數(shù),就得到了一個(gè)反饋值。 積分的作用主要是為了消去靜態(tài)誤差，但當(dāng)前值接近 期望值的時(shí)候，這個(gè)時(shí)候，比例的作用就非常小了，可能會(huì)接近于0，而且相鄰兩次的誤差值也近似為0，D微分也起不了多大作用，假如這時(shí)候系統(tǒng)外部的阻力和PD反饋值抵消了，這個(gè)時(shí)候就需要不斷的累加這個(gè)誤差值來使當(dāng)前值等于期望值

3. D 微分

當(dāng)前的誤差值 減去 上一次運(yùn)算的誤差值，然后再乘上一個(gè)d系數(shù),就得到了一個(gè)反饋值。微分的作用主要為了減少系統(tǒng)的震蕩，在系統(tǒng)變化的方向上，施加一個(gè)反方向的反饋，使系統(tǒng)朝這個(gè)方向的變化得到抑制

可以到，PID算法主要涉及到三種運(yùn)算: 加法,減法和乘法。這三種運(yùn)行在FPGA上也是很容易實(shí)現(xiàn)的。

三. FPGA實(shí)現(xiàn)

首先需要注意的是，PID的三個(gè)系數(shù)均為浮點(diǎn)數(shù)，為了便于實(shí)現(xiàn)，這里將浮點(diǎn)數(shù)擴(kuò)大100倍，然后取整就可以了。然后將反饋的結(jié)果縮小100倍就可以了。

1. P 比例實(shí)現(xiàn)

實(shí)現(xiàn)代碼如下，只需要兩個(gè)時(shí)鐘周期即可完成。這里通過左移來實(shí)現(xiàn)縮小100操作，實(shí)際上是縮小了102.倍，不太會(huì)影響結(jié)果。為了和 I 積分 和 D 微分 運(yùn)算周期數(shù)相同，這里打了一拍操作。

//P -------------------------------------------------
always@(posedge clk or negedge rst_n)
begin
   if( rst_n == 1'b0)
       Kp_fb <= 1'b0;
   else if( pid_en == 1'b1)
       Kp_fb <= ( desired_value - current_value ) * Kp;
   else
       Kp_fb <= Kp_fb;
end


always@(posedge clk or negedge rst_n)
begin
   if( rst_n == 1'b0)
       Kp_fb_reduce <= 'd0;
   else if( cal_delay_0 == 1'b1)
       Kp_fb_reduce <= (Kp_fb >>> 7) + (Kp_fb >>> 9); // /102.4
   else
       Kp_fb_reduce <= Kp_fb_reduce;
end


always@(posedge clk or negedge rst_n)
begin
   if( rst_n == 1'b0)
       Kp_fb_reduce_d0 <= 'd0;
   else if( cal_delay_1 == 1'b1)
       Kp_fb_reduce_d0 <= Kp_fb_reduce;
   else
       Kp_fb_reduce_d0 <= Kp_fb_reduce;


end
//--------------------------------------------------------------------

代碼片段：可切換語言，無法單獨(dú)設(shè)置文字格式

2. I 積分實(shí)現(xiàn)

實(shí)現(xiàn)代碼如下，比P比例稍微輔助一點(diǎn)。這里考慮到了一個(gè)積分限幅的問題，如果積分值一直累加的話得，可能會(huì)導(dǎo)致系統(tǒng)穩(wěn)定不下來，所以這里設(shè)置為3000。

always@(posedge clk or negedge rst_n)
begin
   if( rst_n == 1'b0)
       Ki_integral <= 'd0;
   else if( pid_en == 1'b1)
       if( Ki_integral > $signed('d3000) && ( desired_value - current_value ) > $signed('d0) )
           Ki_integral <= Ki_integral;
       else if( Ki_integral < $signed(-'d3000) && ( desired_value - current_value ) < $signed('d0) )
           Ki_integral <= Ki_integral;
       else
           Ki_integral <= Ki_integral + ( desired_value - current_value );
   else
       Ki_integral <= Ki_integral;
end


always@(posedge clk or negedge rst_n)
begin
   if( rst_n == 1'b0 )
       Ki_fb <= 'd0;
   else if( cal_delay_1 == 1'b1 )
       Ki_fb <= Ki_integral * Ki;
   else
       Ki_fb <= Ki_fb;
end


always@(posedge clk or negedge rst_n)
begin
   if( rst_n == 1'b0 )
       Ki_fb_reduce <= 'd0;
   else if( cal_delay_2 == 1'b1)
       Ki_fb_reduce <= (Ki_fb >>> 7) + (Ki_fb >>> 9); // /102.4
   else
       Ki_fb_reduce <= Ki_fb_reduce;
end


//-------------------------------------------------------------------

代碼片段：可切換語言，無法單獨(dú)設(shè)置文字格式

3. D微分 實(shí)現(xiàn)

D 微分操作實(shí)現(xiàn)如下，按照公式來即可

//D    ---------------------------
always@(posedge clk or negedge rst_n)
begin
   if( rst_n == 1'b0)
       Kd_error <= 'd0;
   else if( pid_en == 1'b1)
       Kd_error <= ( desired_value - current_value );
   else
       Kd_error <= Kd_error;
end


always@(posedge clk or negedge rst_n)
begin
   if( rst_n == 1'b0)
       Kd_fb <= 'd0;
   else if( cal_delay_0 == 1'b1)
       Kd_fb <= (Kd_error - Kd_last_error) * Kd;
   else
       Kd_fb <= Kd_fb;
end


always@(posedge clk or negedge rst_n)
begin
   if( rst_n == 1'b0)
       Kd_last_error <= 'd0;
   else if( cal_delay_0 == 1'b1)
       Kd_last_error <= Kd_error;
   else
       Kd_last_error <= Kd_last_error;
end


always@(posedge clk or negedge rst_n)
begin
   if( rst_n == 1'b0)
       Kd_fb_reduce <= 'd0;
   else if( cal_delay_1 == 1'b1)
       Kd_fb_reduce <= (Kd_fb >>> 7) + (Kd_fb >>> 9); // /102.4
   else
       Kd_fb_reduce <= Kd_fb_reduce;
end
//--------------------------------

代碼片段：可切換語言，無法單獨(dú)設(shè)置文字格式

四. 仿真驗(yàn)證

測(cè)試代碼如下，初始化當(dāng)前值為500，然后根據(jù)期望值和PID輸出的反饋值，來調(diào)節(jié)當(dāng)前值。

always@(posedge clk or negedge rst_n) begin
       if( rst_n == 1'b0)
           current_value <= 'd500;
       else if( pid_ack == 1'b1)
           current_value <= current_value + out;
       else
           current_value <= current_value;
   end


PID_Control PID_Control_i(
   .clk            (   clk),
   .rst_n          (   rst_n),


  .pid_en          (   1'b1),
  .pid_ack         (   pid_ack),


   .desired_value  (   desired_value),
   .current_value  (   current_value),


   .Kp             (   'd10),
   .Ki             (   'd1),
   .Kd             (   'd10),
   
   .out            (   out)
);

代碼片段：可切換語言，無法單獨(dú)設(shè)置文字格式

仿真波形如下

這個(gè)是設(shè)置了D為0的情況，可以看到系統(tǒng)的震蕩