我正在尝试组装一个程序化的机器人,该机器人可以通过阅读标志(例如浴室右侧)的说明来导航房间。我正在使用 AlphaBot2 套件和 RPI 3B+。
图像处理部分运行良好,但由于某种原因,运动控制不起作用。我写了一个简单的 PID 控制器来“馈送”电机,但是一旦电机开始转动,机器人就会关闭。
iPWM = 20 # initial motor power in duty cycle
MAX_PWM = 100
dt = 0.001 # time step
#PID PARAMETERS#
KP = 0.2
KD = 0.01
KI = 0.00005
targets = ['BATHROOM', 'RESTAURANT', 'BALCONY']
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.integral_error += self.error * TIME_STEP
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 = MAX_PWM-iPWM
elif self.output <= -MAX_PWM:
self.output = iPWM - MAX_PWM
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,500)
self.PWMB = GPIO.PWM(self.ENB,500)
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.PA = iPWM+abs(value)
self.PB = iPWM
self.PWMA.ChangeDutyCycle(self.PA)
self.PWMB.ChangeDutyCycle(self.PB)
if value>0:
self.PA = iPWM
self.PB = iPWM+value
self.PWMB.ChangeDutyCycle(self.PB)
self.PWMA.ChangeDutyCycle(self.PA)
if value ==0:
self.PA = iPWM
self.PB = iPWM
self.PWMB.ChangeDutyCycle(self.PB)
self.PWMA.ChangeDutyCycle(self.PA)
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)
然后我有相机捕捉的循环,在那里我识别最近的标志并计算他的宽度和它的中心的x坐标:
cx = int(x+w//2)
if d<= 60:
mot.stop()
GPIO.cleanup()
dutyCycle = pid.compute(cx)
pwm = mot.updatePWM(dutyCycle)