python - 为什么我的 PID 控制器无法通过图像处理跟踪对象？
问题描述
我正在编写一个机器人来使用摄像头在房间内导航以读取标志（例如：Bathroom-Right 等），我正在使用 Alphabot2 套件和 RPI3B+
机器人从第一个标志前开始，每转90度。
由于机器人基本上应该在一条相当直线上移动，我不确定我做错了什么。我无法让机器人到达第一个标志，他只是失去了控制。
我编写了一个 PID 控制器并调整了该任务的参数，但似乎没有帮助。将感谢您的意见
似乎机器人为 pid 控制器快速移动
import RPi.GPIO as GPIO
import cv2 as cv
import numpy as np
import pytesseract
import time
from picamera.array import PiRGBArray
from picamera import PiCamera
#MESURED PARAMETERS#
focalFactor =1650.83
knownWidth =18.4 #cm
iPWM = 20 # initial motor power in duty cycle
MAX_PWM = 100 #
dt = 0.001 # time step
#PID PARAMETERS#
KP = 0.12
KD = 0.01
KI = 0.00005
TARGETS = ['Restroom', 'Restaurant', 'Balcony']
TARGET = 'Restroom'
class PID(object):
def __init__(self,target):
self.target = target
self.kp = KP
self.ki = KI
self.kd = KD
self.setpoint = 320
self.error = 0
self.integral_error = 0
self.error_last = 0
self.derivative_error = 0
self.output = 0
def compute(self, pos):
self.error = self.setpoint - pos
self.derivative_error = (self.error - self.error_last) / dt
self.error_last = self.error
self.output = self.kp*self.error + self.ki*self.integral_error + self.kd*self.derivative_error
if(abs(self.output)>= MAX_PWM-iPWM and (((self.error>=0) and (self.integral_error>=0))or((self.error<0) and (self.integral_error<0)))):
#no integration
self.integral_error = self.integral_error
else:
#rectangular integration
self.integral_error += self.error * dt
if self.output>= MAX_PWM-iPWM:
self.output = iPWM
elif self.output <= -MAX_PWM:
self.output = -iPWM
print("output"+ str(self.output))
return self.output
class MOTORS(object):
def __init__(self,ain1=12,ain2=13,ena=6,bin1=20,bin2=21,enb=26):
self.AIN1 = ain1
self.AIN2 = ain2
self.BIN1 = bin1
self.BIN2 = bin2
self.ENA = ena
self.ENB = enb
self.PA = iPWM
self.PB = iPWM
GPIO.setmode(GPIO.BCM)
GPIO.setwarnings(False)
GPIO.setup(self.AIN1,GPIO.OUT)
GPIO.setup(self.AIN2,GPIO.OUT)
GPIO.setup(self.BIN1,GPIO.OUT)
GPIO.setup(self.BIN2,GPIO.OUT)
GPIO.setup(self.ENA,GPIO.OUT)
GPIO.setup(self.ENB,GPIO.OUT)
self.PWMA = GPIO.PWM(self.ENA,100)
self.PWMB = GPIO.PWM(self.ENB,100)
self.PWMA.start(self.PA)
self.PWMB.start(self.PB)
self.stop()
def forward(self):
self.PWMA.ChangeDutyCycle(self.PA)
self.PWMB.ChangeDutyCycle(self.PB)
GPIO.output(self.AIN1,GPIO.LOW)
GPIO.output(self.AIN2,GPIO.HIGH)
GPIO.output(self.BIN1,GPIO.LOW)
GPIO.output(self.BIN2,GPIO.HIGH)
def updatePWM(self,value):
if value<0:
self.PB = iPWM
if iPWM+abs(value)<MAX_PWM:
self.PA = iPWM+abs(value)
else:
self.PA = iPWM
print('PA =' +str(self.PA))
print('PB =' +str(self.PB))
elif value>0:
self.PA = iPWM
if iPWM+value<MAX_PWM:
self.PB = iPWM+value
else:
self.PB = iPWM
print('PA =' +str(self.PA))
print('PB =' +str(self.PB))
else:
self.PA = iPWM
self.PB = iPWM
self.PWMA.ChangeDutyCycle(self.PA)
self.PWMB.ChangeDutyCycle(self.PB)
GPIO.output(self.AIN1,GPIO.LOW)
GPIO.output(self.AIN2,GPIO.HIGH)
GPIO.output(self.BIN1,GPIO.LOW)
GPIO.output(self.BIN2,GPIO.HIGH)
def stop(self):
self.PWMA.ChangeDutyCycle(0)
self.PWMB.ChangeDutyCycle(0)
GPIO.output(self.AIN1,GPIO.LOW)
GPIO.output(self.AIN2,GPIO.LOW)
GPIO.output(self.BIN1,GPIO.LOW)
GPIO.output(self.BIN2,GPIO.LOW)
def getDistance(knownWidth, focalLength, perWidth):
# compute and return the distance from the maker to the camera
return (knownWidth * focalFactor) / perWidth
def cropImage(Image, XY, WH):
(x, y) = XY
(w, h) = WH
crop = Image[y:y + h, x:x + w]
return crop
camera = PiCamera()
camera.resolution = (640, 480)
camera.framerate = 30
rawCapture = PiRGBArray(camera, size=(640, 480))
mot = MOTORS()
pid = PID(TARGET)
for frame in camera.capture_continuous(rawCapture, format="bgr", use_video_port=True):
img = frame.array
imgHSV = cv.cvtColor(img,cv.COLOR_BGR2HSV)
h_min = 31
h_max = 42
s_min = 50
s_max = 214
v_min = 108
v_max = 255
lower = np.array([h_min,s_min,v_min])
upper = np.array([h_max,s_max,v_max])
mask = cv.inRange(imgHSV,lower,upper)
result = cv.bitwise_and(img,img, mask = mask)
#find contours
contour = img.copy()
# show the frame
imgBlur = cv.GaussianBlur(result,(5,5),1)
imgGray = cv.cvtColor(imgBlur,cv.COLOR_BGR2GRAY)
threshold1 = 50
threshold2 =200
imgCanny = cv.Canny(imgGray,threshold1,threshold2)
kernel = np.ones((5,5))
imgDil = cv.dilate(imgCanny,kernel, iterations=1)
_,contours,hierarchy = cv.findContours(imgDil, cv.RETR_EXTERNAL,cv.CHAIN_APPROX_NONE)
for cnt in contours:
area = cv.contourArea(cnt)
if area>10000:
peri = cv.arcLength(cnt,True)
approx = cv.approxPolyDP(cnt,0.02*peri,True)
if len(approx) == 4:
cv.drawContours(contour,cnt,-1,(255,0,255),3)
print(area)
x,y,w,h = cv.boundingRect(approx)
d = getDistance(knownWidth, focalFactor, w)
cv.rectangle(contour, (x,y),(x+w,y+h),(0,255,0),5)
cv.putText(contour, str(d)+"meters" ,(contour.shape[1] - 300, contour.shape[0] - 20), cv.FONT_HERSHEY_SIMPLEX,2.0, (0, 255, 0), 3)
cx = int(x+w//2)
cy = int(y+h//2)
if d<= 70:
#read the sign
mot.stop()
# imgText= cropImage(contour,(x,y), (w,h))
# text = pytesseract.image_to_string(imgText)
#
# if count ==1:
# sign1 = list(text.split("\n"))
# print(text)
# print(sign1)
# for line in sign1:
# if list(line.split("-"))[0] == pid.target:
# command = list(line.split("-"))[1]
# if command == 'Right':
# print("RIGHT")
#
# if command == 'LEFT':
# print("LEFT")
# if command == 'STRAIGHT':
# print("STRAIGHT")
#
# if count ==2:
# sign2 = list(text.split(" "))
# command = sign2[1]
# if command == 'RIGHT':
# print("RIGHT")
# if command == 'LEFT':
# print("LEFT")
#
dutyCycle = pid.compute(cx)
pwm = mot.updatePWM(dutyCycle)
print('output=' + str(dutyCycle))
print("error:" + str(320-cx))
cv.circle(contour,(cx,cy),5,(0,0,255,cv.FILLED))
cv.circle(contour,(320,240),5,(255,255,0,cv.FILLED))
stack = stackImages(0.7,([mask,result],[imgDil,contour]))
cv.imshow("Stacked Images",stack)
rawCapture.truncate(0)
if cv.waitKey(1) & 0xFF == ord('q'):
break
cv.destroyAllWindows()
camera.close()
pwmStop()
GPIO.cleanup()