前言:
本篇博客加入了PID调控,基于黑线对于图像中线位置的偏移量与黑线的角度进行的上位机PID调参,输出的是电机的目标转速。传给下位机左进一步处理。(今晚上先放上代码,明天再继续更新)
PID简述:
广义上的PID可以分为数字式PID和模糊式PID,这里我对数字式PID进行了简单的学习,本篇文章也主要是对数字式PID的一个讲解。
PID,就是对输入偏差进行比例积分微分运算,运算的叠加结果去控制执行机构。
P就是比例,就是输入偏差乘以一个系数(提高响应速度);
I就是积分,就是对输入偏差进行积分运算(减少误差);
D就是微分,对输入偏差进行微分运算(抑制震荡)。PID 控制作用中,比例作用是基础控制;微分作用是 用于加快系统控制速度;积分作用是用于消除静差。
PID在我们语言描述上可以体现为一个控制器,我们将比例、积分、微分三种调节规律结合在一起, 只要三项作用的强度配合适当,既能快速调节,又能消除余差,可得到满意的控制效果。这里我给大家举一个简单的例子,给大家讲述一下这个过程。
PID经典例子:
让水保持一定温度,假如说我需要使用加热棒在冬天把水温保存在40℃左右,那我直接把加热棒调到40℃。只要我检测到水温低了,那我就再加热,水温高了,那我就停止加热,似乎根本用不到上面提到的微分和积分对吧?可是大家考虑过这样一个问题没有,当我在加热停止的时候,加热棒是不是还需要一段时间才彻底失去温度;此外,当我在加热的时候,因为天气寒冷的原因,热量散失的也比较快,加热和散热是同时进行的,我加热棒设置的温度是40℃,算上散热的话,水温到底是不是40℃。那么问题就来了,我如果单纯地考虑水温低了就加热,水温高了就不加热的话,这水温的曲线,一定不是平缓的,甚至说,变化幅度还不小,那我们如何让这个水温的曲线近乎平缓一些呢?
那如何解决这个问题呢?我们引入三个量,Kp,Kd和Ki
Kp-------比例常数
Ki= (Kp*T)/Ti------积分常数
Kd=(Kp*Td)/T------微分常数
目标值,而水的实际温度,我们叫做实际值。我们都知道,理想的状态是实际水温跟40℃差的比较大的时候,我们要快速加热到40℃左右,当两者相差不大的时候,我们稍微的加热一下就可以。Kp起到的就是这个作用。如果Kp比较大,那么水温上升的就比较快,如果Kp比较小,那么水温上升的就比较慢。推广一下,Kp越大,调节作用越大,Kp越小,调节作用越小。
换句话说,这个微分可以把水温的差减小,抑制水温下降或上升的趋势。只要水温有一个降低或者上升的趋势,通过这个Kd,我们就可以让这个趋势变小。Kd越大,对这个趋势的抑制就越大。
其实对于一个比较简单的工程来说,处于Kp和Kd的控制下,就可以运行了。但为了控制的更加精细些,Ki是必不可少的。刚才,咱们提到了个前提条件:因为天气寒冷的原因,热量散失的也比较快,加热和散热是同时进行的。假如这时候温度比较低,散热和加热达到了一个平衡,我们这水不会达到40℃,而是在37℃就不动了。那怎么办,这时候Kp和Kd就显得很无力了,按照咱们之前的说法,Kp这时候对于这个系统的调节作用很小,而温度根本没有上升或者下降的趋势,我们提到的Kd根本派不上用场。这时候,我们这个系统如果仅仅使用Kp和Kd就出现大问题了。此刻,我们引入Ki。
积分量)用来作为目标值和实际值的差。通过这个差的累加,我们会发现,这个值一旦经过时间的累积,将这个积分量和Ki进行相乘,得到值还是很大的。这时候,我们的系统就能反应过来了,原来还没到达指定温度,还需要加热。这时候,Ki的作用就体现出来了。Ki越大,积分效果越明显。推广一下的话,Ki的作用就是减小静态情况下的误差,让受控物理量尽可能接近目标值。
综上所述,PID其实就是一个控制系统,我们的工程在这个系统的控制下可以变得更加的平稳。
简述位置式PID和增量式PID的区别:
位置式PID:
下面是位置式PID的公式:
为了方便大家对于公式的理解,我这里给大家做了简单的注释。
e(k):用户设定的值(目标值) — 控制对象的当前的状态值 --->误差。
∑e(i):误差的累加。
e(k) - e(k-1):这次误差-上次误差。
并且要有积分限幅和输出限幅(这一点是我在去年电赛的时候遇到的,当时没有对其进行限制,导致小车容易跑偏)
看到了这,大家就能理解我在上面叙述的例子,就是一个经典的位置式PID的例子。
同时,提醒大家一点,舵机和平衡小车的直立和温控系统常常使用位置式PID。
增量式PID:
增量式PID根据公式可以很好地看出,一旦确定了 Kp、Ki 、Kd,只要使用前后三次测量值的偏差, 即可由公式求出控制增量。增量式PID中不会累加。控制增量Δu(k)的确定仅与最近3次的采样值有关,容易通过加权处理获得比较好的控制效果,并且在系统发生问题时,增量式对系统的影响不会像位置式那样严重。
用python来实现这两种PID算法:
class pid(object):
def __init__(self,exp_val,p,i,d):
self.exp_val=exp_val
self.kp=p
self.ki=i
self.kd=d
self.now_err=0#现在误差
self.last_err=0#上一次误差
self.now_val=0#现在值
self.sum_err=0#累计误差
self.last_last_err=0
def cmd_pid(self):
"""位置式PID控制"""
self.last_err=self.now_err
self.now_err=self.exp_val-self.now_val
self.sum_err+=self.now_err
self.now_val=self.kp*self.now_err+self.ki*self.sum_err+self.kd*(self.now_err-self.last_err)
return self.now_val
def pid_cmd(self):
"""增量式PID控制"""
self.last_last_err=self.last_err
self.last_err=self.now_err
self.now_err=self.exp_val-self.now_val
self.change_val=self.kp*(self.now_err-self.last_err)+self.ki*self.now_err+self.kd*(self.now_err-2*self.last_err+self.last_last_err)
self.now_val+=self.change_val
return self.now_val
串级PID:
说完了PID的基本应用,我们再来认识一种更加稳定的PID系统,串级PID。串级PID其实就是两个单级PID“串”在一起组成的。我们在上一篇博客中提到的流程其实就是一个串级PID的控制
根据这张图片大家可以看出摄像头采集到的信息,经过处理产生目标转速并讲这个值传递给了电机,电机再根据PID调速达到平缓前进的目的。
我这里写好了一个下位机的PID,仅供大家借鉴
PID.h
#ifndef __PID_H
#define __PID_H
//结构体声明
typedef struct PID_Speed//PID参数
{
float SetSpeed; //目标值
float ActualSpeed; //实际值
float Err; //误差值
float Err_last; //上一次误差值
float Kp1, Ki1, Kd1;//比例 积分 微分系数
float Out; //定义输出值
float Integral; //积分值
}PID_Speed;
void PID_Speed_Init(struct PID_Speed *pPID);
void PID_Speed_Cal(void);
#endif
pid.c
#include "stm32f10x.h" // Device header
#include "pid.h"
#include "stdio.h"
#include "encoder.h"
float encoder_media_1;//用来传递圈数的值
float encoder_media_2;
float encoder_media_3;
float encoder_media_4;
extern unsigned int lift_param; //设定为全局变量
extern unsigned int right_param;
PID_Speed pid1,pid2,pid3,pid4;
void PID_Speed_Init(struct PID_Speed* pPID)
{
pPID->SetSpeed = 0;
pPID->ActualSpeed = 0;
pPID->Err = 0;
pPID->Err_last = 0;
pPID->Kp1 = 0.35;
pPID->Ki1 = 0.001;
pPID->Kd1 = 0.015;
pPID->Out = 0;
pPID->Integral = 0;
}
void PID_Speed_Cal()
{
float bias1,bias2,bias3,bias4;//用来计算累计的偏差值
float bias1_last,bias2_last,bias3_last,bias4_last;
encoder_media_1 = Read_Encoder1();
encoder_media_2 = Read_Encoder2();
encoder_media_3 = Read_Encoder3();
encoder_media_4 = Read_Encoder4();
pid1.ActualSpeed = encoder_media_1/4;//计算速度,单位是脉冲
pid2.ActualSpeed = encoder_media_2/4;
pid3.ActualSpeed = encoder_media_3/4;
pid4.ActualSpeed = encoder_media_4/4;
pid1.SetSpeed = lift_param;
pid2.SetSpeed = right_param;
pid3.SetSpeed = lift_param;
pid4.SetSpeed = right_param;
bias1_last = bias1;
bias2_last = bias2;
bias3_last = bias3;
bias4_last = bias4;
bias1 = pid1.SetSpeed - pid1.ActualSpeed;
bias2 = pid2.SetSpeed - pid2.ActualSpeed;
bias3 = pid3.SetSpeed - pid3.ActualSpeed;
bias4 = pid4.SetSpeed - pid4.ActualSpeed;
pid1.Integral += bias1;
pid2.Integral += bias2;
pid3.Integral += bias3;
pid4.Integral += bias4;
pid1.Out = pid1.Kp1*bias1 + pid1.Ki1*pid1.Integral + pid1.Kd1*(bias1-bias1_last);
pid2.Out = pid2.Kp1*bias1 + pid2.Ki1*pid1.Integral + pid2.Kd1*(bias2-bias2_last);
pid3.Out = pid3.Kp1*bias1 + pid3.Ki1*pid1.Integral + pid3.Kd1*(bias3-bias3_last);
pid4.Out = pid4.Kp1*bias1 + pid4.Ki1*pid1.Integral + pid4.Kd1*(bias4-bias4_last);
printf("调整的PID值为:%f\r\n",pid1.Out);
}
代码部分:
这段代码是放在电脑上运行的
import math
import cv2
import numpy
import numpy as np
import serial
import time
target_L = 0#左侧电机的目标值
target_R = 0#右侧电机的目标值
#PID控制
class pid(object):
def __init__(self,exp_val,act_val,p,i,d):
self.exp_val=exp_val
self.kp=p
self.ki=i
self.kd=d
self.now_err=0#现在误差
self.last_err=0#上一次误差
self.now_val=act_val#现在值
self.sum_err=0#累计误差
self.last_last_err=0
def cmd_pid(self):
"""位置式PID控制"""
self.last_err=self.now_err
self.now_err=self.exp_val-self.now_val
self.sum_err+=self.now_err
self.now_val=self.kp*self.now_err+self.ki*self.sum_err+self.kd*(self.now_err-self.last_err)
return self.now_val
#PID控制
ERROR = -999
run_flag = 0 #通过该标志位用来在识别到黑色十字之后停止
# center定义
center = 320
ser = serial.Serial('com4',2400)
# 打开摄像头,图像尺寸640*480(长*高),opencv存储值为480*640(行*列)
cap = cv2.VideoCapture(0)
while (1):
ret, frame = cap.read()
# 转化为灰度图
if ret == False: # 如果是最后一帧这个值为False
break
gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)
# 大津法二值化
retval, dst = cv2.threshold(gray, 0, 255, cv2.THRESH_OTSU)
# 膨胀,白区域变大
dst = cv2.dilate(dst, None, iterations=2)
cv2.imshow("dst",dst)
# 看第400行的像素值,第400行像素就约等与图片的底部
color = dst[400]
# 再看第200行的像素值与第300行的像素值
color1 = dst[200]
color2 = dst[300]
# 找到黑色的像素点个数
black_count = np.sum(color == 0)
print("黑色像素点为:",black_count)
if black_count >= 300: #假如识别到了黑色十字就给串口发r:0000l:0000让小车停下来
time.sleep(0.2)
ser.write("stop\r\n".encode())
run_flag = 1
else:
run_flag = 0 #未识别到黑色十字
# 找到黑色的像素点索引
black_count_judge = np.sum(color == 255)#利用这个变量来查找摄像头是否观察到黑色
if black_count_judge == 640:
print("黑色像素点为:0")
time.sleep(0.2)
ser.write("stop\r\n".encode())#在这里我加上了串口
pass
else:
if run_flag == 0:
black_index = np.where(color == 0)
# 防止black_count=0的报错
if black_count == 0:
black_count = 1
#在这里,我们要计算偏移的角度。
black_count1_judge = np.sum(color1 == 255)#第200行如果全是白色的话就不计算角度了
black_count2_judge = np.sum(color2 == 255)
black_index1 = np.where(color1 == 0)
black_index2 = np.where(color2 == 0)
black_count1 = np.sum(color1 == 0)
black_count2 = np.sum(color2 == 0)
if black_count1_judge < 630 and black_count2_judge < 630:
center1 = (black_index1[0][black_count1 - 1] + black_index1[0][0]) / 2#对应的是第200行
direction1 = center1 - 302
center2 = (black_index2[0][black_count2 - 1] + black_index2[0][0]) / 2#对应的是第300行
direction2 = center2 - 302
angle = '%.2f'%(math.degrees(numpy.arctan(100/(direction2-direction1))))
angle = int(float(angle))
print("偏转角为:", angle)
pid_val = pid(90,angle, 0.9, 0.1, 0.0015)
angle_pid = "%.2f"%pid_val.cmd_pid()
cv2.line(frame,(int(center2),300), (int(center1),200), color = (255,0,0), thickness = 3) # 蓝色的线
cv2.line(frame, (0, 300), (640, 300), color=(0, 0, 255), thickness=3) # 红色的线
cv2.line(frame, (0, 200), (640, 200), color=(0, 0, 255), thickness=3)
cv2.imshow("frame", frame)
pass
if black_count1_judge >= 630 or black_count2_judge>= 630: #如果没有发现第150行喝第300行的黑线
angle = ERROR
print("偏转角为:", angle)
pass
# 找到黑色像素的中心点位置
center = (black_index[0][black_count - 1] + black_index[0][0]) / 2
direction = center - 302 #在实际操作中,我发现当黑线处于小车车体正中央的时候应该减去302
direction = int(direction)
print("中心点位置为:",direction)
pid_dir = pid(0,direction,0.9,0.1,0.0015)
direction_pid = "%.2f"%pid_dir.cmd_pid()
print("需要调整的位移为:",direction_pid)
#黑线在左边,中心点坐标为负值,线偏左角度为正值
print("需要调整的角度为",angle_pid,"\r\n")
angle_pid = int(float(angle_pid))
direction_pid = int(float(direction_pid))
if angle_pid > 90:
if direction_pid < 0 :
target_L = math.ceil(1999 + (abs(direction_pid) + angle_pid)*2.5)
target_R = math.ceil(1999 - (abs(direction_pid) + angle_pid)*2.5)
else:
target_L = math.ceil(1999 - (abs(direction_pid) + angle_pid))
target_R = math.ceil(1999 + (abs(direction_pid) + angle_pid))
elif angle_pid < 90:
if direction_pid < 0:
target_L = math.ceil(1999 + (abs(direction_pid) + angle_pid))
target_R = math.ceil(1999 - (abs(direction_pid) + angle_pid))
pass
else:
target_L = math.ceil(1999 - (abs(direction_pid) + angle_pid) * 2.5)
target_R = math.ceil(1999 + (abs(direction_pid) + angle_pid) * 2.5)
pass
else:
target_L = 1999
target_R = 1999
print("target_L = ",target_L,"target_R = ",target_R)
else:
print("小车已经停止\n")
if cv2.waitKey(1) & 0xFF == ord('q'):
break
# 释放清理
cap.release()
cv2.destroyAllWindows()
这段代码是放在树莓派4B上运行的,并且使用了python定时器的思路,让串口每隔0.2秒发送一次电机脉冲的目标值,防止STM32串口接收大量数据而卡死。
python定时器思路:
def TIM_interrupt():
light_param = target_L
right_param = target_R
final_param = 'r:' + str(light_param) + 'l:' + str(right_param) + '\r\n'
ser.write(final_param.encode())
t = threading.Timer(0.2,TIM_interrupt)
t.start()
t = threading.Timer(0.2, TIM_interrupt)
t.start()
下面是综合起来的,可以放在树莓派4B上运行的代码。
import math
import cv2
import numpy
import numpy as np
import serial
import time
import threading
target_L = 0#左侧电机的目标值
target_R = 0#右侧电机的目标值
#PID控制
class pid(object):
def __init__(self,exp_val,act_val,p,i,d):
self.exp_val=exp_val
self.kp=p
self.ki=i
self.kd=d
self.now_err=0#现在误差
self.last_err=0#上一次误差
self.now_val=act_val#现在值
self.sum_err=0#累计误差
self.last_last_err=0
def cmd_pid(self):
"""位置式PID控制"""
self.last_err=self.now_err
self.now_err=self.exp_val-self.now_val
self.sum_err+=self.now_err
self.now_val=self.kp*self.now_err+self.ki*self.sum_err+self.kd*(self.now_err-self.last_err)
return self.now_val
#PID控制
def TIM_interrupt():
light_param = target_L
right_param = target_R
final_param = 'r:' + str(light_param) + 'l:' + str(right_param) + '\r\n'
ser.write(final_param.encode())
t = threading.Timer(0.2,TIM_interrupt)
t.start()
t = threading.Timer(0.2, TIM_interrupt)
t.start()
ERROR = -999
run_flag = 0 #通过该标志位用来在识别到黑色十字之后停止
# center定义
center = 320
ser = serial.Serial('/dev/ttyUSB0',115200)
# 打开摄像头,图像尺寸640*480(长*高),opencv存储值为480*640(行*列)
cap = cv2.VideoCapture(0)
while (1):
ret, frame = cap.read()
# 转化为灰度图
if ret == False: # 如果是最后一帧这个值为False
break
gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)
# 大津法二值化
retval, dst = cv2.threshold(gray, 0, 255, cv2.THRESH_OTSU)
# 膨胀,白区域变大
dst = cv2.dilate(dst, None, iterations=2)
cv2.imshow("dst",dst)
# 看第400行的像素值,第400行像素就约等与图片的底部
color = dst[400]
# 再看第200行的像素值与第300行的像素值
color1 = dst[200]
color2 = dst[300]
# 找到黑色的像素点个数
black_count = np.sum(color == 0)
print("黑色像素点为:",black_count)
if black_count >= 300: #假如识别到了黑色十字就给串口发r:0000l:0000让小车停下来
time.sleep(0.2)
ser.write("stop\r\n".encode())
run_flag = 1
else:
run_flag = 0 #未识别到黑色十字
# 找到黑色的像素点索引
black_count_judge = np.sum(color == 255)#利用这个变量来查找摄像头是否观察到黑色
if black_count_judge == 640:
print("黑色像素点为:0")
time.sleep(0.2)
ser.write("stop\r\n".encode())#在这里我加上了串口
pass
else:
if run_flag == 0:
black_index = np.where(color == 0)
# 防止black_count=0的报错
if black_count == 0:
black_count = 1
#在这里,我们要计算偏移的角度。
black_count1_judge = np.sum(color1 == 255)#第200行如果全是白色的话就不计算角度了
black_count2_judge = np.sum(color2 == 255)
black_index1 = np.where(color1 == 0)
black_index2 = np.where(color2 == 0)
black_count1 = np.sum(color1 == 0)
black_count2 = np.sum(color2 == 0)
if black_count1_judge < 630 and black_count2_judge < 630:
center1 = (black_index1[0][black_count1 - 1] + black_index1[0][0]) / 2#对应的是第200行
direction1 = center1 - 302
center2 = (black_index2[0][black_count2 - 1] + black_index2[0][0]) / 2#对应的是第300行
direction2 = center2 - 302
angle = '%.2f'%(math.degrees(numpy.arctan(100/(direction2-direction1))))
angle = int(float(angle))
print("偏转角为:", angle)
pid_val = pid(90,angle, 0.9, 0.1, 0.0015)
angle_pid = "%.2f"%pid_val.cmd_pid()
cv2.line(frame,(int(center2),300), (int(center1),200), color = (255,0,0), thickness = 3) # 蓝色的线
cv2.line(frame, (0, 300), (640, 300), color=(0, 0, 255), thickness=3) # 红色的线
cv2.line(frame, (0, 200), (640, 200), color=(0, 0, 255), thickness=3)
cv2.imshow("frame", frame)
pass
if black_count1_judge >= 630 or black_count2_judge>= 630: #如果没有发现第150行喝第300行的黑线
angle = ERROR
print("偏转角为:", angle)
pass
# 找到黑色像素的中心点位置
center = (black_index[0][black_count - 1] + black_index[0][0]) / 2
direction = center - 302 #在实际操作中,我发现当黑线处于小车车体正中央的时候应该减去302
direction = int(direction)
print("中心点位置为:",direction)
pid_dir = pid(0,direction,0.9,0.1,0.0015)
direction_pid = "%.2f"%pid_dir.cmd_pid()
print("需要调整的位移为:",direction_pid)
#黑线在左边,中心点坐标为负值,线偏左角度为正值
print("需要调整的角度为",angle_pid,"\r\n")
angle_pid = int(float(angle_pid))
direction_pid = int(float(direction_pid))
if angle_pid > 90:
if direction_pid < 0 :
target_L = math.ceil(1999 + (abs(direction_pid) + angle_pid)*2.5)
target_R = math.ceil(1999 - (abs(direction_pid) + angle_pid)*2.5)
else:
target_L = math.ceil(1999 - (abs(direction_pid) + angle_pid))
target_R = math.ceil(1999 + (abs(direction_pid) + angle_pid))
elif angle_pid < 90:
if direction_pid < 0:
target_L = math.ceil(1999 + (abs(direction_pid) + angle_pid))
target_R = math.ceil(1999 - (abs(direction_pid) + angle_pid))
pass
else:
target_L = math.ceil(1999 - (abs(direction_pid) + angle_pid) * 2.5)
target_R = math.ceil(1999 + (abs(direction_pid) + angle_pid) * 2.5)
pass
else:
target_L = 1999
target_R = 1999
print("target_L = ",target_L,"target_R = ",target_R)
else:
print("小车已经停止\n")
if cv2.waitKey(1) & 0xFF == ord('q'):
break
# 释放清理
cap.release()
cv2.destroyAllWindows()