有了前面颜色识别跟踪的基础之后，我们就可以设定颜色路径，让无人车沿着指定线路做自动驾驶了，视频：PID控制无人车自动驾驶
有了前几章的知识铺垫，就比较简单了，也是属于颜色识别的一种应用，主要是掌握自动驾驶中的一些基础知识，这样就可以进一步去了解在无人驾驶当中遇到的各种问题
1、导入库并初始化
from jetbotmini import Camera
from jetbotmini import bgr8_to_jpeg
from IPython.display import display
from jetbotmini import Robot
import numpy as np
import torch
import torchvision
import cv2
import traitlets
import ipywidgets.widgets as widgets
import numpy as np
#初始化摄像头
camera = Camera.instance(width=300, height=300)
#初始化机器人马达
robot = Robot()
#使用PID控制
import PID
turn_gain = 1.7
turn_gain_pid = PID.PositionalPID(0.15, 0, 0.05)这部分很简单，依然是初始化摄像头用来颜色识别，机器人也叫马达，用来驱动轮子的运动，加一个PID控制，让无人车更加的平稳。
2、显示部件
# 红色数组
color_lower=np.array([156,43,46])
color_upper = np.array([180, 255, 255])
image_widget = widgets.Image(format='jpeg', width=300, height=300)
speed_widget = widgets.FloatSlider(value=0.4, min=0.0, max=1.0, description='speed')
display(widgets.VBox([
widgets.HBox([image_widget]),
speed_widget,
]))
width = int(image_widget.width)
height = int(image_widget.height)
def execute(change):
global turn_gain
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)
# 中心偏移量
center = (150 - color_x)/150
#转向增益PID调节
turn_gain_pid.SystemOutput = center
turn_gain_pid.SetStepSignal(0)
turn_gain_pid.SetInertiaTime(0.2, 0.1)
#将转向增益限制在有效范围内
target_value_turn_gain = 0.15 + 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_speedl = speed_widget.value - target_value_turn_gain * center
target_value_speedr = speed_widget.value + 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.stop()
# 更新图像显示至小部件
image_widget.value = bgr8_to_jpeg(frame)
这里是关键部分，检测目标（这里是红颜色），然后通过其检测的位置来控制左右马达的速度，驱动无人车的行驶与转弯，后台通过图像部件来显示无人车的跟踪情况，方便看到无人车在整个行驶过程中的各种状态。
3、调用并执行
execute({'new': camera.value})
camera.unobserve_all()
camera.observe(execute, names='value')这里就是前面介绍的，通过调用observer方法来更新摄像头的数据，使用的是一个上面定义的execute的一个回调方法。
4、停止无人车
import time
camera.unobserve_all()
time.sleep(1.0)
robot.stop()5、倒车
前面介绍的是向前行驶和转弯，还缺少一个能倒车的功能，恩，很简单，调用backward函数即可
robot.backward(0.8)
time.sleep(0.5)
robot.stop()6、调节颜色数组
我这里是用红色的胶带粘贴在地板上，所以使用的是红色的数组，当然这里我们可以显示mask来测试颜色数组是否设置的比较恰当，代码如下
from matplotlib import pyplot as plt
%matplotlib inline
from IPython import display
for i in range(10):
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)
plt.imshow(mask)
plt.show()
#display.clear_output(wait=True)这里我将display.clear_output(wait=True)注释，将会连续生成10张图片，全部在Jupyter中展示出来。我们也可以去掉注释，这样每次的生成将会清除上一次的图片，这样便于更好地观察。10张连续图片也做成了动图便于大家了解：
如果这里没有出现mask或者比较少的情况，就需要调节颜色数组，让其更好地匹配线路。
7、模拟方向盘
有些时候不想要自动驾驶来控制，而且很多场景更需要人来远程控制，比如在矿山等危险地方，最好的方法就是能够远程去控制工程车去进行作业。
有了上面的向前向后和转弯的了解后，我们就可以制作一个模拟方向盘来控制无人车了。
7.1、按钮部件
# 创建按钮
button_layout = widgets.Layout(width='100px', height='80px', align_self='center')
stop_button = widgets.Button(description='停止', button_style='danger', layout=button_layout)
forward_button = widgets.Button(description='向前', layout=button_layout)
backward_button = widgets.Button(description='向后', layout=button_layout)
left_button = widgets.Button(description='向左', layout=button_layout)
right_button = widgets.Button(description='向右', layout=button_layout)
# 显示按钮
middle_box = widgets.HBox([left_button, stop_button, right_button], layout=widgets.Layout(align_self='center'))
controls_box = widgets.VBox([forward_button, middle_box, backward_button])
display(controls_box)如图：
方向盘的布局，通过widgets.Layout创建层，在这个上面通过widgets.Button创建按钮，然后将按钮通过widgets.HBox和widgets.VBox进行横向和垂直的排版即可。Horizontal：水平的，横向。Vertical：垂直的
7.2、方向控制方法
def stop(change):
robot.stop()
def step_forward(change):
robot.forward(0.8)
time.sleep(0.5)
robot.stop()
def step_backward(change):
robot.backward(0.8)
time.sleep(0.5)
robot.stop()
def step_left(change):
robot.left(0.6)
time.sleep(0.5)
robot.stop()
def step_right(change):
robot.right(0.6)
time.sleep(0.5)
robot.stop()前后左右加停止按钮的方法，方法体很简单，就是控制左右马达的速度。
7.3、按钮动作
定义好了各自方法之后，只需要将方法绑定到各自的按钮就可以了。
stop_button.on_click(stop)
forward_button.on_click(step_forward)
backward_button.on_click(step_backward)
left_button.on_click(step_left)
right_button.on_click(step_right)这样就可以点击按钮，远程控制无人车了。
8、心跳开关
最后就是介绍下心跳开关，检测无人车与浏览器的连接是否还存在的一种简单方法。可以通过下面显示的滑块调整心跳周期（以秒为单位），如果两次心跳之内不能在浏览器之间往返通信的，那么心跳的status（状态）属性值将会设置为dead，一旦连接恢复连接，status属性将设置为alive
from jetbotmini import Heartbeat
heartbeat = Heartbeat()
# 这个函数将在心跳状态改变时被调用
def handle_heartbeat_status(change):
if change['new'] == Heartbeat.Status.dead:
robot.stop()
heartbeat.observe(handle_heartbeat_status, names='status')
period_slider = widgets.FloatSlider(description='period', min=0.001, max=0.5, step=0.01, value=0.5)
traitlets.dlink((period_slider, 'value'), (heartbeat, 'period'))
display(period_slider, heartbeat.pulseout)
自动驾驶的相关知识点介绍完毕，有错误之处，请指正，一起学习与进步！
