无人驾驶中识别颜色并跟踪的优化(加入PID算法控制)

我们了解到无人驾驶是如何去识别颜色的，以及无人车能够跟随颜色目标的演示。回到现实中我们发现，无人车的速度控制是很关键的，这个涉及到安全问题，比如等待红绿灯时，该减速或加速超车等这样很常见的情形，在上节没有体现，这里我们一起来了解下，无人驾驶中是如何自动平稳进行加速和减速的。我们可以先来看一个视频：无人驾驶的速度自动调节

1、实例化摄像头

导入相关的库，然后将摄像头做初始化，如果出现实例化错误的情况，可以查阅：无人车的摄像头的实时捕获图片以及widgets小部件的相关操作

from jetbotmini import Camera
from jetbotmini import bgr8_to_jpeg
from jetbotmini import Robot
import torch
import torchvision
import cv2
import traitlets
import ipywidgets.widgets as widgets
from IPython.display import display
import numpy as np

camera = Camera.instance(width=300, height=300)
global color_lower
global color_upperv
color_lower=np.array([100,43,46])
color_upper = np.array([124, 255, 255])

2、机器人实例化

然后就是将Robot实例化，并导入PID控制算法，用来控制无人车速度快慢的。
关于PID这里再次介绍下：

PID(Proportional-Integral-Derivative)是一种控制器算法，常用于控制工业过程和机器的行为。PID控制器通过测量实际值与期望值之间的差异，并使用比例P、积分I和微分D，这三个系数来计算控制信号，以最小化这个差异。
PID控制器的目的就是通过调整控制信号来使实际输出值与期望输出值保持一致。其中控制信号是通过对误差进行计算得出的，误差是实际输出值与期望输出值之间的差异。

robot = Robot()

import PID

follow_speed = 0.5
turn_gain = 1.7
follow_speed_pid_model = 1
#follow_speed_pid = PID.PositionalPID(3, 0, 0)
follow_speed_pid = PID.PositionalPID(1.5, 0, 0.05)
turn_gain_pid = PID.PositionalPID(0.15, 0, 0.05)

需要注意的是，PID不一定都要出现，可以是PI和PD的组合，也可以只有P这个比例都是可以的，看实际场景来设定。

3、PID算法

我们知道PID是比例，积分与微分，而PID控制器就是通过调整这三者的系数，以平衡响应速度、稳定性和准确性。三者的作用分别如下：

比例系数：调整控制信号的强度，以使系统更快地响应误差。
积分系数：减少系统的稳态误差，使系统更准确地跟踪期望值。
微分系数：用于提高系统的动态性能，使系统更快地响应变化。

在本文将用到的是位置式PID，代码如下：

PID.py

class PositionalPID:
    def __init__(self, P, I, D):
        self.Kp = P
        self.Ki = I
        self.Kd = D
 
        self.SystemOutput = 0.0
        self.ResultValueBack = 0.0
        self.PidOutput = 0.0
        self.PIDErrADD = 0.0
        self.ErrBack = 0.0
    
    #设置PID控制器参数
    def SetStepSignal(self,StepSignal):
        Err = StepSignal - self.SystemOutput
        KpWork = self.Kp * Err
        KiWork = self.Ki * self.PIDErrADD
        KdWork = self.Kd * (Err - self.ErrBack)
        self.PidOutput = KpWork + KiWork + KdWork
        self.PIDErrADD += Err
        self.ErrBack = Err
 
    #设置一阶惯性环节系统  其中InertiaTime为惯性时间常数
    def SetInertiaTime(self, InertiaTime,SampleTime):
        self.SystemOutput = (InertiaTime * self.ResultValueBack + SampleTime * self.PidOutput) / (SampleTime + InertiaTime)
        self.ResultValueBack = self.SystemOutput

还有增量式PID，关于PID的更详细介绍，有兴趣的可以查阅：Jetbotmini中的PID驱动算法控制与代码实现

4、颜色检测与跟踪

这里除了前面移植过来的颜色检测与跟踪之外，还使用了PID控制算法，让无人车对于检测目标的距离来改变车速，当无人车越靠近检测目标速度就会变慢，距离越远速度就会越快，当然这个快也是在一个限速范围内。

image_widget = widgets.Image(format='jpeg', width=300, height=300)

display(widgets.VBox([
    widgets.HBox([image_widget]),
]))

width = int(image_widget.width)
height = int(image_widget.height)
        
def execute(change):
    global follow_speed
    global turn_gain
    global follow_speed_pid
    target_value_speed = 0
    
    #-------------颜色识别处理------------------------------
    frame = camera.value
    frame = cv2.resize(frame, (300, 300))
    frame_=cv2.GaussianBlur(frame,(5,5),0)             
    hsv=cv2.cvtColor(frame,cv2.COLOR_BGR2HSV)
    mask=cv2.inRange(hsv,color_lower,color_upper)
    mask=cv2.erode(mask,None,iterations=2)
    mask=cv2.dilate(mask,None,iterations=2)
    mask=cv2.GaussianBlur(mask,(3,3),0)   
    cnts=cv2.findContours(mask.copy(),cv2.RETR_EXTERNAL,cv2.CHAIN_APPROX_SIMPLE)[-2]
    #-------------------------------------------------------
    # 检测到目标
    if len(cnts)>0:
        cnt = max (cnts,key=cv2.contourArea)
        (color_x,color_y),color_radius=cv2.minEnclosingCircle(cnt)
        if color_radius > 10:
            # 将检测到的颜色标记出来
            cv2.circle(frame,(int(color_x),int(color_y)),int(color_radius),(255,0,255),2)
            #控制将机器人向前移动，并控制成比例的目标与中心的x距离
            center = (150 - color_x)/150

            #跟随速度PID调节
            follow_speed_pid.SystemOutput = 90000 * (color_radius/200)
            follow_speed_pid.SetStepSignal(10000)
            follow_speed_pid.SetInertiaTime(0.2, 0.1)

            #转向增益PID调节
            turn_gain_pid.SystemOutput = center
            turn_gain_pid.SetStepSignal(0)
            turn_gain_pid.SetInertiaTime(0.2, 0.1)

            #将转向增益限制在有效范围内
            target_value_turn_gain = 0.2 + abs(turn_gain_pid.SystemOutput)
            if target_value_turn_gain < 0:
                target_value_turn_gain = 0
            elif target_value_turn_gain > 2:
                target_value_turn_gain = 2

            #将输出电机速度保持在有效行驶范围内
            target_value_speed = 0.8 + follow_speed_pid.SystemOutput / 90000
            target_value_speedl = target_value_speed - target_value_turn_gain * center
            target_value_speedr = target_value_speed + target_value_turn_gain * center
            if target_value_speedl<0.3:
                target_value_speedl=0
            elif target_value_speedl>1:
                target_value_speedl = 1
            if target_value_speedr<0.3:
                target_value_speedr=0
            elif target_value_speedr>1:
                target_value_speedr = 1

            robot.set_motors(target_value_speedl, target_value_speedr)
    # 没有检测到目标
    else:
        robot.forward(float(follow_speed))
        #robot.stop()
        
    # 更新图像显示至小部件
    image_widget.value = bgr8_to_jpeg(frame)

execute({'new': camera.value})

这里的实现原理就是当检测目标距离越远的时候，标注的圆圈就变小了，也就是半径小了，同理，当距离越近的时候，圆圈就变得越大，圆圈半径就越大，就是根据这个半径的大小来控制车速，如下图：

5、实时更新

然后使用camera.observe来调用execute这个回调函数，实时的更新画面，这样就做到了跟踪颜色目标的效果了。

camera.unobserve_all()
camera.observe(execute, names='value')

需要断开摄像头与停止机器人的时候，执行下面代码即可：

import time
camera.unobserve_all()
time.sleep(1.0)
robot.stop()

从上面我们可以看到，和前面的比较，我们的优化是加了一个关键应用，增加了PID控制方法，在本文是使用了位置式PID(Positional PID)，其余代码跟上节是一样的。这样就会让无人车在行驶过程中，遇到障碍等都会平稳减速，另外需要加速也不会突然加速，这样乘坐体验会好很多。