滚球经验总结（PID）_pid滚球控制

 

1、正点原子STM32F407探索者 + openmv3——串口通信

最近在用stm32f407探索者做滚球，一开始使用的摄像头是openmv3（openmv4太贵）——实时检测小球坐标（x，y），然后把x和y发送到407开发板的LCD显示。

openmv的好处是可以使用otsu（大津算法）自动检测阈值，而ov摄像头只能手动调阈值才能找出最佳二值化效果。

# 自动检测阈值
img = sensor.snapshot()
t=img.get_histogram().get_threshold()

 openmv最人性化的就是允许我们在嵌入式上使用Python来编程，比如调用find_blobs()方法，就可以获得一个列表，包含所有色块的信息，遍历所有色块后找出面积最大的就是小球。

img = sensor.snapshot()  #获取图像
    img.binary([th])   # 二值化
    img.draw_rectangle(ROI,color=(220,20,60))   #画出roi区域
    blobs=img.find_blobs([thresholds], roi=ROI,pixels_threshold=100, area_threshold=100, merge=True)   #根据pixels_threshold=100, area_threshold=100在roi区域寻找所有色块
    if blobs:
        data=[]                                                                                     
        cx_max = 0                                         #面积最大的色块x坐标清零
        cy_max = 0                                         #面积最大的色块y坐标清零
        pixel_max = 0                                      #面积最大的色块面积清零
        for b in blobs:
            if b.pixels() >= pixel_max :                   #遍历所有色块，找到面积最大的一块。
                cx_max = b.cx()
                cy_max = b.cy()
                pixel_max = b.pixels()
        img.draw_cross(cx_max,cy_max)
        img.draw_circle(cx_max,cy_max,30)

openmv虽然操作简单准确，但是我们在openmv和407通信这个地方折腾了很久。

STM32F407有6个串口，每个串口对应的I/O可以从芯片原理图中对应找到，对于其串口1，PA9端口对应串口1的发送端，PA10端口对应串口1的接收端。下面是我在407开发板的原理图上截取的串口1的部分： 

 

一开始我们想的很简单，先初始化串口，然后用write函数发送就ok了。

uart = UART(3, 115200)  #串口3，波特率115200
uart.write(data_out +'\n') 

实际上，openmv得先把数据打包好，设置帧头，407再根据帧头按位接收数据。

（一）openmv发送数据
 
uart = pyb.UART(3, 115200)  #串口3，波特率115200
uart.init(115200, bits=8, parity=None, stop=1)  #8位数据位，无校验位，1位停止位
 
def send_data_packet(x, y):
    temp = struct.pack("<bbii",                #格式为俩个字符俩个整型
                   0xAA,                       #帧头1
                   0xAE,                       #帧头2
                   int(x), # up sample by 4    #数据1
                   int(y)) # up sample by 4    #数据2
    uart.write(temp)                           #串口发送
 
（二）stm32接收数据
      usart.c
 
 
extern int ov_frame;
u8 X,Y;    //СÇòµÄ×ø±êÐÅÏ¢
 
void Data_Processing(u8 *data_buf,u8 num)
{
	int theta_org,rho_org;
    theta_org = (int)(*(data_buf+1)<<0) | (int)(*(data_buf+2)<<8) | (int)(*(data_buf+3)<<16) | (int)(*(data_buf+4)<<24) ;
    X = theta_org;
 
    rho_org = (int)(*(data_buf+5)<<0) | (int)(*(data_buf+6)<<8) | (int)(*(data_buf+7)<<16) | (int)(*(data_buf+8)<<24) ;
    Y = rho_org;
}
 
 
void Receive_Prepare(u8 data)
{
    static u8 RxBuffer[10];
    static u8  _data_cnt = 0;
    static u8 state = 0;
    if(state==0 && data==0xAA)
    {
        state=1;
    }
    else if(state==1 && data==0xAE)
    {
        state=2;
        _data_cnt = 0;
    }
    else if(state==2)
    {
        RxBuffer[++_data_cnt]=data;
        if(_data_cnt>=8)
        {
            state = 0;
            Data_Processing(RxBuffer,_data_cnt);
        }
    }
    else
        state = 0;
}
 
void USART1_IRQHandler(void)			 
{
	u8 temp;
	if( USART_GetITStatus(USART1,USART_IT_RXNE)!=RESET )
	{
		USART_ClearITPendingBit(USART1,USART_IT_RXNE);
		temp = USART_ReceiveData(USART1);
		Receive_Prepare(temp);
	}
}

代码中的X,Y就是openmv发送给407的球心坐标了。

2、正点原子STM32F407探索者 + OV2640

相比于支持python的openmv，ov系列摄像头就很原始了，关于ov系列，我们一开始考虑了两个选择——2640和7670。

OV7670，30W像素，带FIFO模块.......

OV2640，200W像素，不带FIFO模块，先读一行像素，再跳到下一行重新开始读取新一行像素（见下面代码第一个if）......

又在淘宝上对比了一系列参数，最终权衡之下选择了贵了三十块钱的2640。

2640用到了407芯片自带的数字摄像头接口DCMI，DCMI支持DMA。初始化时钟 →初始化OV2640（初始化IO口 ，上电，并复位 ，执行初始化序列 ）→设置图像窗口设置和图像输出大小设置，可以调整图像大小或者缩放模式 →初始化DCMI（配置相关引脚的复用功能，使能DCMI时钟，设置DCMI工作模式及PCLK/HSYNC/VSYNC等参数，设置DMA ，启动DCMI传输，即设置DCMI->CR的最低位为1）（当然这部分代码我们直接用的正点原子的）。

对照openmv，2640的代码就通俗易懂多了。

for(i=0;i<B;i++)
    {
      for(j=0;j<=A;j++)
      {
        if(j==A)
        {
          hang++;
          LCD_SetCursor(0,i+1);  
          LCD_WriteRAM_Prepare();		//¿ªÊ¼Ð´ÈëGRAM
         }		
				//LCD->LCD_RAM=rgb_buf[i][j];				
								
         gray=((rgb_buf[i][j]>>11)*19595+((rgb_buf[i][j]>>5)&0x3f)*38469 +(rgb_buf[i][j]&0x1f)*7472)>>16;
         if(gray<=20)                                   //ÕâÀïÊÇСÇòºÚ°×¶þÖµ»¯
         {											  
					 if(i>12&&i<136&&j<150&&j>19)
					 {
						 if(i>X_MAX) X_MAX=i;
						 if(i<X_MIN) X_MIN=i;										 									      
						 if(j>Y_MAX) Y_MAX=j;
						 if(j<Y_MIN) Y_MIN=j;											 
					 }
           LCD->LCD_RAM=WHITE;
         }
         else
         {											 
           LCD->LCD_RAM=BLACK;
         }
				 
       }

3、PID

PID控制器是工业过程控制中广泛采用的一种控制器，其中，P、I、D分别为比例（Proportion）、积分（Integral）、微分（Differential）的简写；将偏差的比例、积分和微分通过线性组合构成控制量，用该控制量对受控对象进行控制，称为PID算法 。

其中KP、KI、KD分别为比例系数、积分系数、微分系数。

P，打个比方，如果现在的输出是1，目标输出是100，那么P的作用是以最快的速度达到100，把P理解为一个系数即可；而I呢？大家学过高数的，0的积分才能是一个常数，I就是使误差为0而起调和作用；D呢？大家都知道微分是求导数，导数代表切线是吧，切线的方向就是最快到至高点的方向。这样理解，最快获得最优解，那么微分就是加快调节过程的作用了。         

P——比例

偏差（目标值减去当前值）与调节装置的“调节力度”，建立一个一次函数的关系，就可以实现最基本的“比例”控制了。

P越大，调节作用越激进，P越小会让调节作用更保守。当比较接近目标值时，P的控制作用就比较小了。

但是，只有P不能让平衡车站起来，水温也控制得晃晃悠悠，好像整个系统不是特别稳定，总是在“抖动”。

D——微分

D的作用就是让物理量的速度趋于0，只要什么时候，这个量具有了速度，D就向相反的方向用力，尽力刹住这个变化。

D越大，向速度相反方向刹车的力道就越强。

如果是平衡小车，加上P和D两种控制作用，如果参数调节合适，它应该可以站起来了。

I——积分

设置一个积分量，只要偏差存在，就不断地对偏差进行积分（累加），并反应在调节力度上。

I 的值越大，积分时乘的系数就越大，积分效果越明显。

所以，I的作用就是，减小静态情况下的误差，让受控物理量尽可能接近目标值。

I在使用时还有个问题：需要设定积分限制，防止在刚开始时，就把积分量积得太大，难以控制。

PID参数调节口诀

参数整定找最佳，从小到大顺序查

先是比例后积分，最后再把微分加      //调整顺序：P→I→D

曲线振荡很频繁，比例度盘要放大    

曲线漂浮绕大湾，比例度盘往小扳

曲线偏离回复慢，积分时间往下降       

曲线波动周期长，积分时间再加长

曲线振荡频率快，先把微分降下来

动差大来波动慢，微分时间应加长

理想曲线两个波，前高后低四比一

一看二调多分析，调节质量不会低

若要反应增快，增大P减小I

若要反应减慢，减小P增大I

如果比例太大，会引起系统震荡

如果积分太大，会引起系统迟钝

PID公式

PID的增量型公式：

离散化公式：

最终公式：

PID控制并不一定要三者都出现，也可以只是PI、PD控制，关键决定于控制的对象。

 

 

 

 

附：openmv滚球完整代码：gunqiu.py

 
import sensor, image, time, math
import pyb,utime
import json
from pyb import UART     #加载串口模块
from pyb import Timer     #加载定时器模块
from pyb import LED      #加载LED模块
import struct
 
ROI=(44,0,230,224)  # 设置检测区域（兴趣范围）
thresholds = (205, 255)  # 寻找色块的阈值
 
#摄像头初始化
sensor.reset()
sensor.set_pixformat(sensor.GRAYSCALE)
sensor.set_framesize(sensor.QVGA)
sensor.skip_frames(time = 2000)
sensor.set_auto_gain(False) 
sensor.set_auto_whitebal(False) 
clock = time.clock()
 
#openmv串口初始化
uart = UART(3, 115200)  #串口3，波特率115200
uart.init(115200, bits=8, parity=None, stop=1)  #8位数据位，无校验位，1位停止位
 
#自动检测阈值
#img = sensor.snapshot()
#img.draw_rectangle(ROI)
#t=img.get_histogram().get_threshold()
 
#th  二值化阈值
th=[]
th.append(0)
th.append(80)
 
#串口数据发送格式
def send_data_packet(x, y):
    temp = struct.pack("<bbii",                #格式为俩个字符俩个整型
                   0xAA,                       #帧头1
                   0xAE,                       #帧头2
                   int(x), # up sample by 4    #数据1
                   int(y)) # up sample by 4    #数据2
    uart.write(temp)                           #串口发送
 
while(True):
    clock.tick()
    img = sensor.snapshot()
    img.binary([th])
    img.draw_rectangle(ROI,color=(220,20,60))
    blobs=img.find_blobs([thresholds], roi=ROI,pixels_threshold=100, area_threshold=100, merge=True)
    if blobs:
        data=[]                                                    #数据清零
        cx_max = 0                                                  #面积最大的色块x坐标清零
        cy_max = 0                                                  #面积最大的色块y坐标清零
        pixel_max = 0                                               #面积最大的色块面积清零
        for b in blobs:
            if b.pixels() >= pixel_max :                         #遍历所有色块，找到面积最大的一块。
                cx_max = b.cx()
                cy_max = b.cy()
                pixel_max = b.pixels()
        img.draw_cross(cx_max,cy_max)
        img.draw_circle(cx_max,cy_max,30)
 
        send_data_packet(cx_max, cy_max)
        print(cx_max, cy_max)