基于python-opencv实时识别黑线赛道（三）之上位机PID调控前篇

前言：

        本篇文章是前两篇文章的进阶版本（基于python-opencv实时识别黑线赛道（一）与基于python-opencv实时识别黑线赛道（二）），在实时识别黑线的基础上标注了黑线的角度，同时，本篇文章也是作为使用树莓派作为上位机调节PID的第一步。后续内容将与我的博客（从零开始制作STM32F103RCT6小车系列）有联系。

        本篇博客是基于如图所示的小车的体系所写：使用树莓派4B作为上位机，使用STM32F103RCT6芯片作为下位机。这里我遇到的一些问题和坑，大家可以借鉴一下。

 

识别黑线的最终用途：

        最开始我也是晕晕乎乎的，觉得识别完黑线只需要回传给下位机的四个轮子的转速即可，但是，事情没有这么简单。在请教了智然学长之后，我的思路才渐渐捋顺。我们这个采用上下位机的小车依靠两个环来对PID进行调控，上位机（也就是树莓派4B）利用摄像头识别黑线，并根据黑线偏移的位置和黑线的角度进行PID调节，输出的是一个转速的目标值，为的是让小车有一个合理的目标速度来进行循迹。再将这个目标值传输给下位机（也就是STM32F103RCT6），下位机通过控制电机和编码器再进行一轮PID调节，让小车行驶的更加丝滑。这时候就能达到一个比较好的巡线效果了。

        也就是下图所示的一个流程：

 如何让摄像头进行一个PID调节呢？

        我在电赛期间就已经发现了如果单纯的依靠最底层黑色像素点的中心位置来对小车速度进行调节是完全不够的，我当时就在想着如果再对图像中间部分的像素点进行一个处理，以获取一个中点的位置，让摄像头在这两个点的作用下，进行一个调节，一定会是更好的，只不过当时时间有限，而且下位机的问题更加严重，这个想法也就被搁置了下来。在暑假里，我又重新拾起了这个想法，在智然学长的指导下，我也是懂得了这个调节的原理。最底层黑色像素点的中心位置其实可以看做黑线偏移图像中心的距离（简称距离），通过在图像适当位置标注两点，大家都知道，两点确定一条直线，通过这条直线，我们就可以计算出这个直线的斜率，同时也就能算出直线的角度（以下简称角度）。

        这里用一张图片来给大家展示一下，如何根据两点去计算一下角度。这里的虚线就是我们场地上的黑线

        下图就是常见的两种调控。

 opencv在实时视频中画线：

        为了方便我们观看黑线的角度，同时也为了避免错误，我查阅了大量的资料，都没有发现基于python的opencv在实时视频画出一个线来，唉，干脆我自己写一个吧。

        在开始之前，我们要先知道，如何使用python-opencv来画一个线呢，我们需要调用cv2库里的函数cv2.line。

cv2.line(image, start_point, end_point, color, thickness)

#比如下面
cv2.line(img, (0, 0), (511, 511), (0, 0, 255), 5)

image:它是要在其上绘制线条的图像。

start_point：它是线的起始坐标。坐标表示为两个值的元组，即(X坐标值，Y坐标值)。

end_point：它是直线的终点坐标。坐标表示为两个值的元组，即(X坐标值Y坐标值)。

color:它是要绘制的线条的颜色采用BGR方式，例如：(255，0，0)为蓝色。

thickness:它是线的粗细像素。

对于cv2.line()这里需要注意的一点，无论是start_point还是end_point，这个坐标采用的都是先是列，再是行，我在这里没少吃亏

        我这里采用的是摄像头实时捕捉画面。返回值frame是object类型的,这个frame返回的是每一帧的画面。

ret, frame = cap.read()

我们可以直接将这个frame替换为cv2.line()函数里面的img,不过要注意一点的是，如果是显示灰度图的画，我们所画的线就会被二值化，看到的就不再是我们要的线条了，这里我们可以额外开辟一个窗口，就像我在本篇博客最后展示的那样，这样就可以看到线条了

使用python计算直线的角度：

        然后，我们要学会使用python来计算出这个黑线偏移的角度来，如果盲目的使用numpy.arctan()函数，计算出来的是弧度制，我们需要把这个值给转换成角度值。这里我们再粘贴一下这个计算角度的图片，方便大家理解

angle = '%.2f'%(math.degrees(numpy.arctan(100/(direction2-direction1))))
#其中，direction2，和direction1分别对应的上面图片的x2和x1，100就是这个K值

代码部分：

        这里的代码没有涉及到PID的调控部分，这个我打算过段时间有空了再写有关上位机PID的博客。

        注意：我这里没有把代码移植到树莓派上，我用小车的摄像头连接在电脑上，用A4纸贴上黑胶带来模拟赛道。

import math

import cv2
import numpy
import numpy as np
import serial
import time

ERROR = -999
run_flag = 0 #通过该标志位用来在识别到黑色十字之后停止
# center定义
center = 320
#ser = serial.Serial('/dev/ttyUSB0',2400)
ser = serial.Serial('com4',2400)

# 打开摄像头，图像尺寸640*480（长*高），opencv存储值为480*640（行*列）
cap = cv2.VideoCapture(0)
while (1):
    ret, frame = cap.read()

    # 转化为灰度图
    if ret == False:  # 如果是最后一帧这个值为False
       break
    gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)
    # 大津法二值化
    retval, dst = cv2.threshold(gray, 0, 255, cv2.THRESH_OTSU)

    # 膨胀，白区域变大
    dst = cv2.dilate(dst, None, iterations=2)
    # # 腐蚀，白区域变小
    #dst = cv2.erode(dst, None, iterations=6)
    cv2.imshow("dst",dst)
    # 看第400行的像素值，第400行像素就约等与图片的底部
    color = dst[400]
    # 再看第200行的像素值与第300行的像素值
    color1 = dst[200]
    color2 = dst[300]
    # 找到黑色的像素点个数
    black_count = np.sum(color == 0)
    print("黑色像素点为：",black_count)
    if black_count >= 300:  #假如识别到了黑色十字就给串口发r:0000l:0000让小车停下来
        time.sleep(0.2)
        ser.write("r:0000l:0000\r\n".encode())
        run_flag = 1
    else:
        run_flag = 0    #未识别到黑色十字
    # 找到黑色的像素点索引
    black_count_judge = np.sum(color == 255)#利用这个变量来查找摄像头是否观察到黑色
    if black_count_judge == 640:
        print("黑色像素点为:0")
        time.sleep(0.2)
        ser.write("r:0000l:0000\r\n".encode())#在这里我加上了串口
        pass
    else:
        if run_flag == 0:
            black_index = np.where(color == 0)
            # 防止black_count=0的报错
            if black_count == 0:
                black_count = 1
            #在这里，我们要计算偏移的角度。
            black_count1_judge = np.sum(color1 == 255)#第200行如果全是白色的话就不计算角度了
            black_count2_judge = np.sum(color2 == 255)
            black_index1 = np.where(color1 == 0)
            black_index2 = np.where(color2 == 0)
            black_count1 = np.sum(color1 == 0)
            black_count2 = np.sum(color2 == 0)
            if black_count1_judge < 630 and black_count2_judge < 630:
                center1 = (black_index1[0][black_count1 - 1] + black_index1[0][0]) / 2#对应的是第200行
                direction1 = center1 - 302
                center2 = (black_index2[0][black_count2 - 1] + black_index2[0][0]) / 2#对应的是第300行
                direction2 = center2 - 302
                print("center1:",center1,"center2:",center2)
                angle = '%.2f'%(math.degrees(numpy.arctan(100/(direction2-direction1))))
                print("偏转角为：", angle)
                cv2.line(frame,(int(center2),300), (int(center1),200),  color = (255,0,0), thickness = 3)  # 蓝色的线
                cv2.line(frame, (0, 300), (640, 300), color=(0, 0, 255), thickness=3)                      # 红色的线
                cv2.line(frame, (0, 200), (640, 200), color=(0, 0, 255), thickness=3)
                cv2.imshow("frame", frame)
                pass
            if black_count1_judge >= 630 or black_count2_judge>= 630:  #如果没有发现第150行喝第300行的黑线
                angle = ERROR
                print("偏转角为：", angle)
                pass
            # 找到黑色像素的中心点位置
            center = (black_index[0][black_count - 1] + black_index[0][0]) / 2
            direction = center - 302 #在实际操作中，我发现当黑线处于小车车体正中央的时候应该减去302
            direction = int('%4d'%direction)
            print("方向为：",direction)
            # 计算出center与标准中心点的偏移量
            '''当黑线处于小车车体右侧的时候，偏移量为正值，黑线处于小车车体左侧的时候，偏移量为负值（处于小车视角）'''
            if direction > 0:
                right_param = 1999 + (direction*4) #这个参数可以后期更改
                light_param = 1999
                final_param = 'r:' + str(light_param) + 'l:' + str(right_param) + '\r\n'
                print(final_param)
                time.sleep(0.2)
                ser.write(final_param.encode())
            else:
                media = -direction
                light_param = 1999 + (media * 4)
                right_param = 1999
                final_param = 'r:' + str(light_param) + 'l:' + str(right_param) + '\r\n'
                print(final_param)
                time.sleep(0.2)
                ser.write(final_param.encode())
        else:
            print("小车已经停止\n")
    if cv2.waitKey(1) & 0xFF == ord('q'):
        break

# 释放清理
cap.release()
cv2.destroyAllWindows()

最终效果展示：

        这里的蓝线呢是我们真正的黑线角度部分。红线是为了方便标注这条蓝线所做的辅助线。