基于Openmv的pid巡线，请大佬们帮我看看可行性高不高。

第一次打电赛，也没什么经验。队友的板子还没弄好，我写完没法调试。

背景为21年送药小车。本文没有经过实际调试，慎用。

直接上代码！

import sensor, image, time, math
from pyb import LED
from machine import UART

sensor.reset()
#sensor.set_hmirror(True)
#sensor.set_vflip(True)
sensor.set_auto_gain(False) # 关闭自动自动增益。默认开启的，在颜色识别中，一定要关闭白平衡。
sensor.set_auto_whitebal(False)
sensor.set_pixformat(sensor.RGB565)
sensor.set_framesize(sensor.QQVGA)
sensor.skip_frames(time = 2000)
clock = time.clock()

#补光
#LED(1).on()  # 红灯
#LED(2).on()  # 绿灯
#LED(3).on()  # 蓝灯  三个一起开就是白灯

# 获取目标色块阈值
roi1 = (130, 70, 100, 100)


# 将图像分割成5块区域，分别计算线的中心坐标再求平均

#test_roi6 = (0, 120, 360, 24)
#test_roi7 = (0, 144, 360, 24)
#test_roi8 = (0, 168, 360, 24)
#test_roi9 = (0, 192, 360, 24)
#test_roi10 = (0, 216, 360, 24)

# —————————————————————————————————————————————————————————————————————————————————————————
# 通过串口打印
uart = UART(3, 115200)
def send_data(data1):
    global uart
    data1 = str(data1)
    data = bytearray("[", data1, "]")
    uart.write(data)

# —————————————————————————————————————————————————————————————————————————————————————————
# 查找最大色块
def find_max_blobs(blobs):
    max_size = 0
    max_blob = None
    for blob in blobs:
        if blob.pixels() > max_size:
            max_blob = blob
            max_size = blob.pixels()
    return max_blob

# 寻找直线
line_threshold = [(32, 81, -7, 80, -5, 66)]
def find_blobs_to_line(myroi, line_threshold, max_width=50):
    blobs = img.find_blobs(line_threshold, roi=myroi, area_threshold=50, pixel_threshold=50)
    if blobs and len(blobs) == 1:
        return blobs[0].cx(), blobs[0].cy()
    else:
        max_blob = find_max_blobs(blobs)
        if max_blob is not None and max_blob.w() < max_width:
            return max_blob.cx(), max_blob.cy()
    return None

# 查找数字
num_roi = (0, 48, 160, 40)
def find_num(num_roi, num_threshold, max_width=80):
    blobs = img.find_blobs(num_threshold, roi=num_roi, area_threshold=50, pixel_threshold=50)
    if blobs:
        max_blob = find_max_blobs(blobs)
        if max_blob.w() > max_width:
            return 1
    return 0

#查找十字路口
linecross_roi = (0, 48, 160, 40)
def find_linecross(linecross_roi, line_threshold, max_width=50):
    blobs = img.find_blobs(line_threshold, roi=linecross_roi, area_threshold=50, pixel_threshold=50)
    if blobs:
        max_blob = find_max_blobs(blobs)
        if 80< max_blob.area() < 120:  # 面积会增大
            return 1
    return 0

# 是否掉头
#turn_180_threshold = [()]  # 黑色块阈值
turn_180_roi = (0, 48, 160, 40)
def find_turn_180(turn_180_roi, turn_180_threshold, max_width=80):
    blobs = img.find_blobs(turn_180_threshold, roi=turn_180_roi, area_threshold=50, pixel_threshold=50)
    if blobs:
        max_blob = find_max_blobs(blobs)
        if 80 < max_blob.area() < 100:
            return 2
    return 0


# 获取目标色块阈值
roi2 = (170, 110, 20, 20)
def find_target_threshold():
    for i in range(100):
        img = sensor.snapshot()
        img.draw_rectangle(roi2, color=(255, 0, 0))
        Statistics = img.get_statistics(roi=roi2)
        Threshold = [Statistics.l_min(), Statistics.l_max(),
                     Statistics.a_min(), Statistics.a_max(),
                     Statistics.b_min(), Statistics.b_max()]
        print(Threshold)
    return Threshold

#  —————————————————————————————————————————————————————————————————————————————————————————
# 均值滤波器类
class MeanFilter:
        # 定义n次平均值， 缓冲区
    def __init__(self, window_size):
        self.window_size = window_size
        self.buffer = []
        # 更新函数
    def update(self, value):
        self.buffer.append(value)
        if len(self.buffer) > self.window_size:
            self.buffer.pop(0)
        # 获取数据
    def get_value(self):
        if len(self.buffer) == 0:
            return 0
        return sum(self.buffer) / len(self.buffer)

# —————————————————————————————————————————————————————————————————————————————————————————

# 线性最小二乘法计算斜率和截距（相对于y轴）
def liner_least_squares(list_):
    # 分别从不同的roi中获取的数据, list_为n组数据， list__为某一组数据
    n = len(list_)
    sum_x = sum((list__[0] for list__ in list_))
    sum_y = sum((list__[1] for list__ in list_))
    sum_xy = sum((list__[0]*list__[1] for list__ in list_))
    sum_x_squared = sum((list__[0]**2 for list__ in list_))
    # 计算斜率
    try:
        slope = (n * sum_xy - sum_x * sum_y) / (n * sum_x_squared - sum_x * sum_x)
    except ZeroDivisionError:  # 防止除数为0
        slope = 0
    # 计算截距
    try:
        intercept = (sum_y - slope * sum_x) / n
    except ZeroDivisionError:
        intercept = 0

    return slope, intercept

# 计算直线与y轴的斜率并转化为角度
def slope_to_angle(slope):
    angle = math.degrees(math.atan(1 / slope)) if slope!=0 else 0
    return angle

# 根据截距适当调节速度：截距过大则降速， 小则加速
# def intercept_for_speed(intercept, current_speed):
    # temp_speed = 50  # 速度偏移量
    # distance_threshold = 20  # 位置相对偏移量阈值
    # position_offset = abs(intercept)  # 位置相对原点偏移量

    # if position_offset > distance_threshold:
        # current_speed -= temp_speed
    # if position_offset < distance_threshold:
        # current_speed += temp_speed
    # else:
        # return None

# 计算PID—————————————————————————————————————————————————————————————————————————————————————————
class PID:
    # 积分项限幅
    imax = 10

    def __init__(self, kp, ki, kd, target):
        # 初始化
        self.kp = kp
        self.ki = ki
        self.kd = kd
        self.error = 0
        self.last_error = 0
        self.integral = 0
        self.target = target

    # 获取计算结果
    def get_output_for_pid(self, current_angle):
        output = 0
        self.error = self.target - current_angle
        # kp输出——————————————————————
        output += self.error * self.kp

        # ki输出——————————————————————
        self.integral += self.error
        # ki限幅
        if self.integral > self.imax:
            self.integral = self.imax
        elif self.integral < -self.imax:
            self.integral = -self.imax
        output += self.ki * self.integral

        # kd输出———————————————————————
        derivative = self.error - self.last_error
        output += self.kd * derivative

        # 重置误差
        self.last_error = self.error
        return output

    # 获取当前角度
    def get_current_angle(self, current_angle):
        current_angle += self.get_output_for_pid(current_angle)
        return current_angle

# —————————————————————————————————————————————————————————————————————————————————————————
test_roi1 = (40, 0, 80, 24)
test_roi2 = (0, 24, 160, 24)
test_roi3 = (0, 48, 160, 24)
test_roi4 = (0, 72, 160, 24)
test_roi5 = (40, 96, 80, 24)

def basic():
    # 获取直线中心坐标
    centers_list = [find_blobs_to_line(test_roi1, line_threshold),
                   find_blobs_to_line(test_roi2, line_threshold),
                   find_blobs_to_line(test_roi3, line_threshold),
                   find_blobs_to_line(test_roi4, line_threshold),
                   find_blobs_to_line(test_roi5, line_threshold),
                   (80, 120)]  # 添加一个坐标原点，使得线性回归得以拟合，像素为160x120

    centers_list = [center for center in centers_list if center is not None]
    for center in centers_list:
        # img.draw_rectangle(c[0]-5, c[1]-5, 10, 10, color=(255, 0, 0))
        img.draw_cross(center[0], center[1], color=(255, 0, 0))

    # 获取每一段斜率、截距
    slope_list, intercept_list = liner_least_squares(centers_list)
    # 斜率转化为角度
    angle_list = slope_to_angle(slope_list)
    # 更新角度、截距
    angle_filter.update(angle_list)
    intercept_filter.update(intercept_list)
    # 获取平均角度（滤波）
    current_angle = -angle_filter.get_value()
    current_intercept = intercept_filter.get_value() - 120

    return current_angle, current_intercept

#  —————————————————————————————————————————————————————————————————————————————————————————
# 走直线
def turn_0():
    # 期望角度
    pid.target = 0
    current_angle, current_intercept = basic()
    if abs(current_angle) > 2 :
        # 放入pid中
        current_angle = pid.get_current_angle(current_angle)

    # 传输数据
    send_data(int(pid.get_current_angle(current_angle)))

    # print(pid.get_current_angle(current_angle))
    print(f"当前角度：{current_angle}, 当前截距：{current_intercept},\
    应偏移角度：{pid.get_output_for_pid(current_angle)}， 直行")

## 实现90°转弯
def turn_90():
    pid.target = 90 # 期望角度
    current_angle, current_intercept = basic()

    # 放入pid中
#     current_angle = pid.get_current_angle(current_angle)

    # 传输数据
    send_data(int(pid.get_current_angle(current_angle)))
    # print(pid.get_current_angle(current_angle))

    print(f"当前角度：{current_angle}, 当前截距：{current_intercept},\
    应偏移角度：{pid.get_output_for_pid(current_angle)}, 遇到十字路口")

def turn_180():
    pid.target = 180
    current_angle, current_intercept = basic()

    # 放入pid中
    # current_angle = pid.get_current_angle(current_angle)

    # 传输数据
    send_data(int(pid.get_current_angle(current_angle)))
    # print(pid.get_current_angle(current_angle))

    print(f"当前角度：{current_angle}, 当前截距：{current_intercept},\
    应偏移角度：{pid.get_output_for_pid(current_angle)}， 掉头")


# 主函数—————————————————————————————————————————————————————————————————————————————————————————
# 计算n次平均值
angle_filter = MeanFilter(6)
intercept_filter = MeanFilter(6)
#target_threshold = find_target_threshold()
#target_threshold = [(15, 53, -10, 80, 1, 66)]
#LED(1).on()

# pid初始化
pid = PID(kp=1, ki=0.1, kd=0.01, target=0)

while(True):
    clock.tick()
#    img = sensor.snapshot().lens_corr(strength=1.7, zoom=1.0)
    img = sensor.snapshot()
    img.draw_string(10, 10, f"FPS:{int(clock.fps())}")
    img.draw_rectangle((40, 0, 80, 24), color=(0, 0, 0))
    img.draw_rectangle((0, 24, 160, 24), color=(0, 0, 0))
    img.draw_rectangle((0, 48, 160, 24), color=(0, 0, 0))
    img.draw_rectangle((0, 72, 160, 24), color=(0, 0, 0))
    img.draw_rectangle((40, 96, 80, 24), color=(0, 0, 0))

    if find_linecross(linecross_roi, line_threshold) == 1:
        # 转弯
        turn_90()
    elif find_linecross(linecross_roi, line_threshold) == 0:
        # 巡线
        turn_0()
    elif find_turn_180(turn_180_roi, turn_180_threshold) == 2:
        # 掉头
        turn_180()

    LED(1).on()  # 红灯

其中pid还没调，线的阈值都需要自己测，里面有查找目标阈值的方法。望各位佬批评改正