1 PID算法
PID算法是一种常用的反馈修正算法，根据误差值来不断调整系统的输入，让测量值不断趋近目标值。详细介绍参考wikipedia: PID控制器。
在我们的循线场景中，误差是小车中部偏离黑线的距离，我们把它叫做CTE(cross track error)。
误差是由于小车的走向和黑线的走向不一致造成的，我们的控制信号就是调整小车的转向角度。
P、I、D分别代表对误差(error)的三种修正成分，分别是：
-
Proportional: 比例控制，只考虑当前的误差，纠正一定比例；容易造成小车在线的两旁反复摇摆。 -
Integral: 积分控制，考虑过去的累积误差，可以控制小车更快的逼近位置。 -
Derivative: 微分控制，考虑了误差的变化趋势，可以减少小车在线两旁震荡的现象。
最终的控制输出是：
其中有三个增益系数是要根据实际情况调整的，在后面的PID调参部分再介绍。
2 视觉处理
接下来介绍图像处理部分，这是我们首次处理小车摄像头的图像，这部分opencv基础操作很重要，特别是校准和变换。以后都会用到。
2.1 摄像头校准(calibration)
摄像头校准也叫标定，是为了消除摄像头把三维世界映射到二维图像产生的几何畸变而做的操作。
针孔摄像头模型(pinhole camera model)产生的畸变包括径向的畸变：离摄像头中心越远的越弯曲，和平面的畸变：图像平面和摄像头平面不平行时，离摄像头所在平面近的地方会比原来显得更近。如下图所示：
2.2 opencv标定摄像头的方法
opencv提供了用若干张棋盘图片来校准摄像头的方法，你只需要打印一幅黑白的棋盘图片，平放在摄像头前面，从不同的角度拍摄一些图片(大于10张)，然后调用下面的代码就获取到摄像头的参数矩阵。
棋盘图片(适合于打印到一张A4纸上)：
opencv会使用棋盘的已知交叉点的位置跟在图像上的对应位置来计算出摄像头参数矩阵。
src/applications/cv_utils.py
def calibrate_camera(chessboard_input_images: list, chessboard_corners=(9, 6), debug=False):
"""
Do camera calibration.
"""
assert type(chessboard_input_images) is list, 'input should be a list of images'
# Note the image origin is (0, 0), the bottom right corner is (8, 5)
obj_points = [] # 3D points in real world space
img_points = [] # 2D points in image plane
# world points always on flat plane, like (0, 0, 0), (1, 0, 0), (2, 0, 0) ..., (8, 5, 0), according to image space
obj_points_for_one_image = np.zeros((chessboard_corners[0] * chessboard_corners[1], 3), np.float32)
obj_points_for_one_image[:, :2] = np.mgrid[0:chessboard_corners[0], 0:chessboard_corners[1]].T.reshape(-1, 2) # x, y coordinates
for chessboard_image in chessboard_input_images:
gray = cv2.cvtColor(chessboard_image, cv2.COLOR_BGR2GRAY)
# find chessboard corners
ret, corners = cv2.findChessboardCorners(gray, chessboard_corners, None) # corners in image space
if debug:
img = cv2.drawChessboardCorners(chessboard_image, chessboard_corners, corners, ret)
cv2.imshow('Corners', img)
cv2.waitKey()
if ret is True:
img_points.append(corners)
obj_points.append(obj_points_for_one_image)
ret, mtx, dist, rvecs, tvecs = cv2.calibrateCamera(obj_points, img_points, gray.shape[::-1], None, None)
return mtx, dist, corners, gray.shape[::-1]
获取到的参数矩阵我们用pickle保存到文件备用。
with open('./config/calibration_result.pkl', 'bw') as f:
pickle.dump((mtx, dist, corners, img_size), f)
得到参数矩阵后，调用undistort方法就可以对输入的图片做去除畸变的处理：
cv2.undistort(image, mtx, dist, None, mtx)
以下是一个消除畸变后的棋盘图片：
opencv也提供了详细的文档：
OpenCV-Python Tutorials
Camera Calibration and 3D Reconstruction
2.3 视角变换(perspective transformation)
去除图片的畸变后，我们需要把图片的视角变成从上到下俯瞰的(top down view / bird's eye view)。
进行视角变换需要找到4个源点和映射后的4个目标点(映射点的数量可以不是4个)，如上图所示。然后调用opencv的两个方法就可以了：
M = cv2.getPerspectiveTransform(src_points, dest_points)
cv2.warpPerspective(image, M, dst_size)
下图是我的小车拍摄的图片经过畸变消除、视角转换后的效果：
2.4 寻线(line detection)
我们地上的黑线是用黑色PVC绝缘胶带粘的，得到包含黑线的图片后，我们要通过一系列步骤确定黑线的位置。
2.4.1 预处理
由于我们的黑线相对于地面是比较突出的，所以对图片预处理比较简单：
- 消畸和变换
- 截取自定义的兴趣区域
- 转成灰度
- 二值化
-
mycar/src/components/pid.py:
def _preprocess_image(self, img):
undist = undistort_and_tansform(img, self.c_mtx, self.c_dist, self.c_corners, self.c_img_size)
roi_top_left, roi_bottom_right = self.roi[0], self.roi[1]
roi = undist[roi_top_left[1]:roi_bottom_right[1], roi_top_left[0]:roi_bottom_right[0]]
gray = cv2.cvtColor(roi, cv2.COLOR_BGR2GRAY)
_, binary = cv2.threshold(gray, self.white_threshold, 255, cv2.THRESH_BINARY_INV)
return binary
下图是转成灰度和预处理后的结果：
2.4.2 图像直方图
现在黑线已经转成了白线，由于目标线在纵坐标方向延伸，在横轴方向找图像的直方图的最高点就可以简单确定线的位置了。
-
mycar/src/components/pid.py:
def _find_line(self, img) -> tuple:
"""
Use pixel histogram to find line.
Returns:
line_position, car_position
"""
binary = self._preprocess_image(img)
# car at the middle
car_position = binary.shape[1] // 2 - self.camera_offset
histogram = np.sum(binary, axis=0)
line_base = np.argmax(histogram)
# Create an output image to draw on and visualize the result
if len(self.publication) == 3:
image_out = np.dstack((binary, binary, binary))
cv2.circle(image_out, (line_base, binary.shape[0]-2), 3, (0, 255, 0), thickness=2)
return line_base, car_position, image_out
上图圆点的位置为线的位置。如果摄像头在车头的中间，那么图片的横轴中间点就是车子的位置；由于摄像头不一定安装得很正中，所以加入一个camera_offset来调整车子偏离图片中心的位置。
3 PID调参
3.1 PID转向
前面得到了线的位置和车的位置，把两者之差CTE代入前面的PID反馈式子，就得出了小车要转向的角度：
mycar/src/components/pid.py
def _pid_steering(self, cte):
"""
Use the equation: new_steering = Kp * cte + Ki * cte_integrational + Kd * cte_differential
to calculate the new steering.
"""
diff_cte = cte - self.prev_cte
self.prev_cte = cte
self.int_cte += cte
# proportional
pid_p = self.pid_coeffs[0] * cte
# differential
pid_d = self.pid_coeffs[1] * diff_cte
# integrational
if abs(cte) <= 10: # anti integerator windup (over shooting)
self.int_cte = 0
pid_i = self.pid_coeffs[2] * self.int_cte
# anti windup via dynamic integrator clamping
int_limit_max = 0.0
int_limit_min = 0.0
if pid_p < 1:
int_limit_max = 1 - pid_p
if pid_p > -1:
int_limit_min = -1 - pid_p
if pid_i > int_limit_max:
pid_i = int_limit_max
elif pid_i < int_limit_min:
pid_i = int_limit_min
# sum up
steer = pid_p + pid_d + pid_i
# apply limits
if steer < -1:
return -1
elif steer > 1:
return 1
else:
return steer
3.2 调参
但有三个系数要确定的。这三个参数的值跟小车本身的特性、运行的速度、运行的环境有关系，要在实际环境中确定。
这里我们采用一种手工调参的方式：不断让小车跑一段同样的线路，同时调整参数的大小来降低总的CTE。
方法如下：
根据经验初始化Kp, Ki, Kd的值
P，如P = [-0.003, -0.003， -0.0003]，和每个参数的增量D，如D = [-0.0003, -0.0003, -0.00003]。让小车跑重复跑一段同样的路线，记录下每次跑完累计的CTE：
CTE_sum。重复以下的操作直到D变得足够小(如<0.0001)：
2.1 轮流调整Kp, Ki, Kd的值，如它们顺序为i，让 P[i] = P[i] + D[i]。
2.2 如果调整后，CTE_sum变小了，则增大调整量：D[i] = D[i] x 1.1。回到2.1调整下一个参数。
2.3 如果调整后，CTE_sum变大了，则P[i] = P[i] - 2 * D[i]，即还原P[i]并让P[i]减去D[i]。
---- 2.3.1 如果调整后CTE_sum变小了，那么增大调整量: D[i] = D[i] x 1.1。
---- 2.3.2 如果调整后CTE_sum变大了，那么还原P[i]，P[i] = P[i] + D[i]，并减少调整量，D[i] = D[i] x 0.9。回到2.1调整下一个参数。
这种方法是比较容易理解的，代码里的_twiddle_pid_params方法集成了这种方式，每次调整完会把参数保存到文件，方便断点续调和以后适用。记得把train_mode参数设为True。
代码里的默认参数值是我在某次调参后得到的，不一定适用于大家。
4 程序运行
本次实现的功能主要在mycar/src/components/pid.py文件，本文只介绍了一些重点的地方，请看源文件来了解完整实现。
首先按上一篇文章连接好手柄，或者修改配置文件使用上上一篇文章介绍的手机控制也可以。
git clone https://github.com/evan-wu/mycar
git checkout -b blog-5
cd mycar
bin/run.sh config/pid_line_follower.yml 120
程序运行起来后，把小车放到黑线前面，点击手柄Y或者web界面的Autonomous Mode，小车就会沿着黑线前进啦！在检查不到黑线的情况下，车子会自动停下。你也可以用手柄开启录像或者调节前进的速度哟！
请看文章开头的演示视频。
欢迎blog/github点赞，评论，讨论！
后续预告：MPC(Model Predictive Control)算法。敬请期待！