【红色飓风Nano二代测评】PID控制器的FPGA实现

发表于 2014/5/20 20:57:45 阅读（4648）

PID控制器的FPGA实现

在小车的自主避障时发现，给定两个电机的速度参数值是一样的，但是两个轮子却走的不一样快，究其原因我没有对小车进行闭环控制，因此，现在开始做PID控制了。

本文前面的说明，一串的公式截图都来自某硕士论文，人家写的真不错，我这里引用了，verilog是自己来完成的。

理论部分：

PID算法及其FPGA实现

PID控制器结构清晰，参数可调，适用于各种控制对象，PID控制器的核心思想是针对控制对象的控制需求，建立描述对象动态特性的数学模型，通过PID参数整定实现在比例，微分，积分三个方面参数调整的控制策略来达到最佳系统响应和控制效果，式子如下：

在数字控制系统中，PID控制规律的实现必须用数值逼近的方法。当采样周期相当

时，用求和代替积分、用后向差分代替微分，使模拟PID离散化变为差分方程。

式子3.8就是我们的位置式PID算法：

下面就是我们要实现上式PID算法。

PID的FPGA实现：

得到：

Verilog实现：

`timescale 1ns / 1ps
//////////////////////////////////////////////////////////////////////////////////
// Company: 
// Engineer: 
// 
// Create Date:    21:02:51 05/14/2014 
// Design Name: 
// Module Name:    pid 
// Project Name: 
// Target Devices: 
// Tool versions: 
// Description: 
//
// Dependencies: 
//
// Revision: 
// Revision 0.01 - File Created
// Additional Comments: 
//
//////////////////////////////////////////////////////////////////////////////////
module pid(
    input clk,
    input rst_n,
    input [8:0] error,
    output reg [16:0] uk
    );

//reg  [16:0]uk;
wire [16:0]uk_wire;		
reg [8:0]error_1,error_2;
parameter k0=5;
parameter k1=1;
parameter k2=1;
always @(posedge clk)
begin
	if(!rst_n)
		begin
			error_1<=0;
			error_2<=0;
		end
	else
		begin
			error_1<=error;
			error_2<=error_1;
		end
end

//
reg [14:0]uk1;
always @(posedge clk)
begin
	if(!rst_n)
		begin
			uk<=0;
			uk1<=0;
		end
	else
		begin
			if((uk_wire>17'd15000)&&(uk_wire<17'b1000_0000_0000_00000))
				begin
					uk<=17'd15000;
				end
			else
					begin
						uk1<=uk[14:0];
						uk<=uk_wire;
					end	
		end
end

wire [14:0] p0;
mult	        u1 (
						.b ( k0 ),
						.a ( error ),
						.p ( p0 ),
						.clk(clk)
						);
						
wire [14:0] p1;
mult	        u2 (
						.b ( k1 ),
						.a ( error_1 ),
						.p ( p1 ),
						.clk(clk)
						);
wire [14:0] p2;
mult	        u3 (
						.b ( k2 ),
						.a ( error_2 ),
						.p ( p2 ),
						.clk(clk)
						);

wire [15:0]s1;						
add			  u4 (
						.a ( p0 ),
						.b ( p1 ),
						.s ( s1 ),
						.clk ( clk )
						);

wire [15:0]s2;						
add			  u5 (
						.a ( p2 ),
						.b ( uk1 ),
						.s ( s2 ),
						.clk ( clk )
						);
				
add2			 u6 (
						.a ( s1 ),
						.b ( s2 ),
						.s ( uk_wire[16:0] ),
						.clk (clk)
						);


endmodule

Testbench：

`timescale 1ns / 1ps

////////////////////////////////////////////////////////////////////////////////
// Company: 
// Engineer:
//
// Create Date:   21:34:28 05/14/2014
// Design Name:   pid
// Module Name:   J:/xilinx_project/pid/test.v
// Project Name:  pid
// Target Device:  
// Tool versions:  
// Description: 
//
// Verilog Test Fixture created by ISE for module: pid
//
// Dependencies:
// 
// Revision:
// Revision 0.01 - File Created
// Additional Comments:
// 
////////////////////////////////////////////////////////////////////////////////

module test;

	// Inputs
	reg clk;
	reg rst_n;
	reg [8:0] error;

	// Outputs
	wire [16:0] uk;

	// Instantiate the Unit Under Test (UUT)
	pid uut (
		.clk(clk), 
		.rst_n(rst_n), 
		.error(error), 
		.uk(uk)
	);


	initial begin
		// Initialize Inputs
		clk = 0;
		rst_n = 0;
		error = 0;

		// Wait 100 ns for global reset to finish
		#40 rst_n=1;
      #20 error=9'b001111111;
		#200 error=9'b000111111;
		#200 error=9'b000011111;
		#200 error=9'b000001111;
		#200 error=9'b000000111;
		#200 error=9'b000000011;
		#800 error=0;
		#200 error=9'b111000000;
		#200 error=9'b111110000;
		#200 error=9'b111111111;
		#800 error=0;
	//	#200 error=9'b100000001;
			
		// Add stimulus here

	end
  always #10 clk=~clk;      
endmodule

中途中mult的实现可以使用LUT或者DSP资源（上一篇博客也有说）