前言：
本篇博客加入了PID调控，基于黑线对于图像中线位置的偏移量与黑线的角度进行的上位机PID调参，输出的是电机的目标转速。传给下位机左进一步处理。（今晚上先放上代码，明天再继续更新）
PID简述：
广义上的PID可以分为数字式PID和模糊式PID，这里我对数字式PID进行了简单的学习，本篇文章也主要是对数字式PID的一个讲解。
用一句话去概括数字式PID，就是对输入偏差进行比例积分微分运算，运算的叠加结果去控制执行机构。
P 就是比例，就是输入偏差乘以一个系数（提高响应速度）；
I 就是积分，就是对输入偏差进行积分运算（减少误差）；
D 就是微分，对输入偏差进行微分运算（抑制震荡）。
PID 控制作用中，比例作用是基础控制；微分作用是 用于加快系统控制速度；积分作用是用于消除静差。
PID在我们语言描述上可以体现为一个控制器，我们将比例、积分、微分三种调节规律结合在一起， 只要三项作用的强度配合适当，既能快速调节，又能消除余差，可得到满意的控制效果。这里我给大家举一个简单的例子，给大家讲述一下这个过程。
PID经典例子：
PID经典例子：让水保持一定温度，假如说我需要使用加热棒在冬天把水温保存在40℃左右，那我直接把加热棒调到40℃。只要我检测到水温低了，那我就再加热，水温高了，那我就停止加热，似乎根本用不到上面提到的微分和积分对吧？可是大家考虑过这样一个问题没有，当我在加热停止的时候，加热棒是不是还需要一段时间才彻底失去温度；此外，当我在加热的时候，因为天气寒冷的原因，热量散失的也比较快，加热和散热是同时进行的，我加热棒设置的温度是40℃，算上散热的话，水温到底是不是40℃。那么问题就来了，我如果单纯地考虑水温低了就加热，水温高了就不加热的话，这水温的曲线，一定不是平缓的，甚至说，变化幅度还不小，那我们如何让这个水温的曲线近乎平缓一些呢？
那如何解决这个问题呢？我们引入三个量，Kp，Kd和Ki
Kp———-比例常数
Ki= (Kp_T)/Ti———积分常数
Kd=(Kp_Td)/T———微分常数
第一个量叫做Kp。在让水保持一定温度的这个过程中，我们希望水的温度为40℃，这个我们叫做目标值，而水的实际温度，我们叫做实际值。我们都知道，理想的状态是实际水温跟40℃差的比较大的时候，我们要快速加热到40℃左右，当两者相差不大的时候，我们稍微的加热一下就可以。Kp起到的就是这个作用。如果Kp比较大，那么水温上升的就比较快，如果Kp比较小，那么水温上升的就比较慢。推广一下，Kp越大，调节作用越大，Kp越小，调节作用越小。
接着，我们引入Kd，刚才我们提到当水温比较接近40℃了，那么我们这个Kp起到的作用就比较小了，这时候，我们的Kd就会大显身手了，大家都知道，微分的字面意思是把数给分成许多许多个微小的部分。换句话说，这个微分可以把水温的差减小，抑制水温下降或上升的趋势。只要水温有一个降低或者上升的趋势，通过这个Kd，我们就可以让这个趋势变小。Kd越大，对这个趋势的抑制就越大。
其实对于一个比较简单的工程来说，处于Kp和Kd的控制下，就可以运行了。但为了控制的更加精细些，Ki是必不可少的。刚才，咱们提到了个前提条件：因为天气寒冷的原因，热量散失的也比较快，加热和散热是同时进行的。假如这时候温度比较低，散热和加热达到了一个平衡，我们这水不会达到40℃，而是在37℃就不动了。那怎么办，这时候Kp和Kd就显得很无力了，按照咱们之前的说法，Kp这时候对于这个系统的调节作用很小，而温度根本没有上升或者下降的趋势，我们提到的Kd根本派不上用场。这时候，我们这个系统如果仅仅使用Kp和Kd就出现大问题了。此刻，我们引入Ki。
我们设定一个专门的变量（积分量）用来作为目标值和实际值的差。通过这个差的累加，我们会发现，这个值一旦经过时间的累积，将这个积分量和Ki进行相乘，得到值还是很大的。这时候，我们的系统就能反应过来了，原来还没到达指定温度，还需要加热。这时候，Ki的作用就体现出来了。Ki越大，积分效果越明显。推广一下的话，Ki的作用就是减小静态情况下的误差，让受控物理量尽可能接近目标值。
综上所述，PID其实就是一个控制系统，我们的工程在这个系统的控制下可以变得更加的平稳。
简述位置式PID和增量式PID的区别：
位置式PID：
下面是位置式PID的公式：
u(k)=Kp∗e(k)+Ki∗∑i=ki=0e(i)+Kd∗[e(k)−e(k−1)]
为了方便大家对于公式的理解，我这里给大家做了简单的注释。
e(k)：用户设定的值（目标值） — 控制对象的当前的状态值 —->误差。
∑e(i)：误差的累加。
e(k) - e(k-1)：这次误差-上次误差。
位置式PID是根据当前系统的实际位置，与我们想要达到的预期位置的偏差，进行PID控制的。这里有两点需要注意：1、因为有误差积分 ∑e(i)的存在，也就导致了当前的输出u(k)与过去的所有状态都有关系。输出的u(k)对应的是执行者的实际位置，一旦控制输出出错(控制对象的当前的状态值出现问题 )，一定会引起u(k)的大幅变化，也就会导致系统的大幅变化。2、位置式PID在积分项达到饱和时,误差仍然会继续累积，一旦误差开始反向变化，系统需要一定时间从饱和状态退出，所以在u(k)达到最大和最小时，要停止积分作用，并且要有积分限幅和输出限幅（这一点是我在去年电赛的时候遇到的，当时没有对其进行限制，导致小车容易跑偏）
看到了这，大家就能理解我在上面叙述的例子，就是一个经典的位置式PID的例子。
同时，提醒大家一点，舵机和平衡小车的直立和温控系统常常使用位置式PID。
增量式PID：
u(k)=u(k)−u(k−1)
u(k)=Kp∗[e(k)−e(k−1)]+Ki∗e(k)+Kd∗[e(k)−2e(k−1)+e(k−2)]
增量式PID根据公式可以很好地看出，一旦确定了 Kp、Ki 、Kd，只要使用前后三次测量值的偏差， 即可由公式求出控制增量。增量式PID中不会累加。控制增量Δu(k)的确定仅与最近3次的采样值有关，容易通过加权处理获得比较好的控制效果，并且在系统发生问题时，增量式对系统的影响不会像位置式那样严重。
用python来实现这两种PID算法：
这里呢，我借鉴的这篇文章
python实现PID控制_五把手的博客-CSDN博客_pid控制python实现
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);
#endifpid.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()
评论（0）
您还未登录，请登录后发表或查看评论