We learned how unmanned driving recognizes colors, and a demonstration of the ability of unmanned vehicles to follow color targets . Returning to reality, we found that the speed control of unmanned vehicles is very important. This involves safety issues, such as when waiting for traffic lights, such common situations as slowing down or accelerating to overtake other vehicles were not reflected in the previous section. Here we will learn how to automatically and smoothly accelerate and decelerate in unmanned driving. Let's take a look at a video first: automatic speed adjustment of unmanned driving
1. Instantiate the camera
Import the relevant library, and then initialize the camera. If there is an instantiation error, you can refer to: Real-time captured pictures of the camera of the unmanned vehicle and related operations of 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 instantiation
Then instantiate the Robot and import the PID control algorithm to control the speed of the unmanned vehicle.
Here is another introduction about PID:
PID (Proportional-Integral-Derivative) is a controller algorithm commonly used to control the behavior of industrial processes and machines. A PID controller minimizes this differenceby measuring the difference between the actual value and the desired value and using, proportional P, integral I, and derivative D
The purpose of the PID controller is to make the actual output value consistent with the expected output value by adjusting the control signal. Thecontrol signalis obtained by calculating the error,whichis the difference between the actual output value and the expected output value.
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)It should be noted that PID does not necessarily have to appear, it can be a combination of PI and PD, or only the ratio of P is acceptable, depending on the actual scene to set.
3. PID algorithm
We know that PID is proportional, integral and differential, and the PID controller balances response speed, stability and accuracy by adjusting the coefficients of these three. The functions of the three are as follows:
Scale Factor: Adjusts the strength of the control signal to make the system respond to errors more quickly.
Integral coefficient: reduce the steady-state error of the system and make the system track the expected value more accurately.
Differential coefficient: used to improve the dynamic performance of the system and make the system respond to changes faster.
The positional PID will be used in this article, the code is as follows:
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.SystemOutputThere is also incremental PID. For a more detailed introduction to PID, those who are interested can refer to: PID drive algorithm control and code implementation in Jetbotmini
4. Color detection and tracking
In addition to the color detection and tracking transplanted earlier, the PID control algorithm is also used to allow the unmanned vehicle to change the speed of the detection target. When the unmanned vehicle gets closer to the detection target, the speed will slow down, and the farther the distance will be, the faster the speed will be. Of course, this speed is also within a speed limit.
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})The realization principle here is that when the detection target is farther away, the marked circle becomes smaller, that is, the radius is smaller. Similarly, when the distance is closer, the circle becomes larger and the circle radius is larger. The speed of the vehicle is controlled according to the size of the radius, as shown in the figure below :
5. Real-time update
Then use camera.observe to call the execute callback function to update the screen in real time, so that the effect of tracking the color target is achieved.
camera.unobserve_all()
camera.observe(execute, names='value')When you need to disconnect the camera and stop the robot, execute the following code:
import time
camera.unobserve_all()
time.sleep(1.0)
robot.stop()From the above, we can see that compared with the previous ones, our optimization is to add a key application and a PID control method. In this article, positional PID (Positional PID) is used, and the rest of the code is the same as the previous section. In this way, the unmanned vehicle will smoothly decelerate when encountering obstacles during driving, and it will not accelerate suddenly when it needs to accelerate, so that the riding experience will be much better.