生长算法和巡中线算法python实现代码示例（自用）

生长算法和巡中线算法python实现代码示例（自用）

import cv2
import time
import numpy as np
from math import pi, isnan

# PID算法类
class PID():
    _kp = _ki = _kd = _integrator = _imax = 0
    _last_error = _last_derivative = _last_t = 0
    _RC = 1 / (2 * pi * 20)

    def __init__(self, p=0, i=0, d=0, imax=0):
        self._kp = float(p)
        self._ki = float(i)
        self._kd = float(d)
        self._imax = abs(imax)
        self._last_derivative = float('nan')

    def get_pid(self, error, scaler):
        tnow = int(round(time.time() * 1000))
        dt = tnow - self._last_t
        output = 0
        if self._last_t == 0 or dt > 1000:
            dt = 0
            self.reset_I()
        self._last_t = tnow
        delta_time = float(dt) / float(1000)
        output += error * self._kp
        if abs(self._kd) > 0 and dt > 0:
            if isnan(self._last_derivative):
                derivative = 0
                self._last_derivative = 0
            else:
                derivative = (error - self._last_error) / delta_time
            derivative = self._last_derivative + \
                         ((delta_time / (self._RC + delta_time)) *
                          (derivative - self._last_derivative))
            self._last_error = error
            self._last_derivative = derivative
            output += self._kd * derivative
        output *= scaler
        if abs(self._ki) > 0 and dt > 0:
            self._integrator += (error * self._ki) * scaler * delta_time
            if self._integrator < -self._imax:
                self._integrator = -self._imax
            elif self._integrator > self._imax:
                self._integrator = self._imax
            output += self._integrator
        return output

    def reset_I(self):
        self._integrator = 0
        self._last_derivative = float('nan')

class RobotControl():
    def __init__(self):
        # PID调参的地方
        self.rho_pid = PID(p=0.4, i=0)
        self.theta_pid = PID(p=0.001, i=0)
        # 图像大小处理
        self.h = 0  # 480
        self.w = 0  # 640
        self.middle_line_x = 0
        # 存中线坐标
        self.middle_line_list = []
        # 生长算法初始点
        self.first_dot = (0, 0)

    def skip_image(self, cap, num):
        for i in range(num):
            _, frame = cap.read()

    def img_dispose(self, cap):
        _, frame = cap.read()
        frame = cv2.GaussianBlur(frame, (3, 3), 1)
        # 灰度
        frame = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)
        # 二值化
        frame = cv2.adaptiveThreshold(frame, 255, cv2.ADAPTIVE_THRESH_GAUSSIAN_C, cv2.THRESH_BINARY, 25, 5)
        # retval, frame = cv2.threshold(frame, 127, 255, cv2.THRESH_BINARY_INV)
        # kernel = np.ones((3, 3), np.uint8)
        # img = cv2.morphologyEx(img, cv2.MORPH_OPEN, kernel)
        # 边界提取
        # img1 = cv2.Canny(img, 80, 150, (5, 5))
        # img = cv2.Canny(img, 80, 150, (3, 3))
        # img = np.concatenate((frame, img), axis=1)
        # img = np.hstack((img1, img2))
        return frame

    def find_middle_line(self, frame):
        # print(len(frame[0]))
        point_size = 1
        point_color = (127, 127, 127)  # BGR
        thickness = 4  # 可以为 0 、4、8
        left_line = []
        right_line = []
        countall = 0
        count = 0
        # middle_dot = []
        for i in range(self.h):
            # 找左边
            for j in range(int(self.middle_line_x), 0, -1):
                if frame[i][j - 1] == 0:
                    left_line.append((i, j - 1))
                    break
            else:
                left_line.append((i, -1))
            # 找右边
            for j in range(int(self.middle_line_x), self.w):
                if frame[i][j] == 0:
                    right_line.append((i, j))
                    break
            else:
                right_line.append((i, -1))
        for i in range(self.h):
            if left_line[i][1] != -1 and right_line[i][1] != -1:
                # middle_dot.append((i, (left_line[i][1]+right_line[i][1])//2))
                count += 1
                a = (left_line[i][1] + right_line[i][1]) // 2
                countall += a
                cv2.circle(frame, (a, i), point_size, point_color, thickness)
        if count != 0:
            self.middle_line_x = countall // count  # 更新赛道中线的位置
        return frame

    # 生长算法
    def grow_line(self, img, mode, begin_dot=(-1, -1)):
        """
        :param img: 传入的图片
        :param left_dot: 左边开始找的初始点
        :param right_dot: 右边开始找的初始点
        :param mode: 0 1 o代表找左边， 1代表找右边
        :return: 返回找的线的元组列表
        """
        dot_dic = {}

        def road_true(dot, mode, num, fx, fy):
            """
            :param dot: 开始的点
            :param mode: 当前情况下查点的模式
            :param num:  希望查的点的个数
            :param fx: 查横的方向
            :param fy: 查竖的方向
            :return: 返回是否合法
            """
            if fx == 0 and fy == 0:
                return -1
            elif fx == 1 and fy == 1:
                if mode == 0:  # 向左
                    if dot[0] - num >= 0 and dot[1] - num >= 0:
                        return 1
                    else:
                        return 0
                else:  # 向右
                    if dot[0] + num < self.w - 1 and dot[1] - num >= 0:
                        return 1
                    else:
                        return 0
            elif fx == 1:
                if mode == 0:
                    if dot[0] - num >= 0:
                        return 1
                    else:
                        return 0
                else:
                    if dot[0] + num < self.w - 1:
                        return 1
                    else:
                        return 0
            elif fy == 1:
                if mode == 0:
                    if dot[1] - num >= 0:
                        return 1
                    else:
                        return 0
                else:
                    if dot[1] - num >= 0:
                        return 1
                    else:
                        return 0

        # 先假设路是白色255， 边界是黑色0
        road = 255
        middle_road = 127
        borad = 0

        if begin_dot[0] != -1:
            current_dot = begin_dot  # current_dot[1]为y， current_dot[0]为x
            if mode == 0:  # 向左找
                while 1:
                    if road_true(current_dot, mode, 1, fx=1, fy=0) and road_true(current_dot, mode, 1, fx=0, fy=1):
                        if (img[current_dot[1]][current_dot[0]] == road or img[current_dot[1]][
                            current_dot[0]] == middle_road) and img[current_dot[1]][current_dot[0] - 1] == borad:
                            dot_dic[current_dot[1]] = current_dot[0]
                            if current_dot[1] <= 0:
                                break
                            else:
                                current_dot = (current_dot[0], current_dot[1] - 1)
                        elif img[current_dot[1]][current_dot[0]] == borad:
                            current_dot = (current_dot[0] + 1, current_dot[1])
                        elif img[current_dot[1]][current_dot[0]] == road or img[current_dot[1]][
                            current_dot[0]] == middle_road:
                            current_dot = (current_dot[0] - 1, current_dot[1])
                    elif road_true(current_dot, mode, 1, fx=0, fy=1):
                        current_dot = (begin_dot[0], current_dot[1] - 1)
                    else:
                        break

            else:  # 向右找
                while 1:
                    if road_true(current_dot, mode, 1, fx=1, fy=0) and road_true(current_dot, mode, 1, fx=0, fy=1):
                        if (img[current_dot[1]][current_dot[0]] == road or img[current_dot[1]][
                            current_dot[0]] == middle_road) and img[current_dot[1]][current_dot[0] + 1] == borad:
                            dot_dic[current_dot[1]] = current_dot[0]
                            if current_dot[1] <= 0:
                                break
                            else:
                                current_dot = (current_dot[0], current_dot[1] - 1)
                        elif img[current_dot[1]][current_dot[0]] == borad:
                            current_dot = (current_dot[0] - 1, current_dot[1])
                        elif img[current_dot[1]][current_dot[0]] == road or img[current_dot[1]][
                            current_dot[0]] == middle_road:
                            current_dot = (current_dot[0] + 1, current_dot[1])
                    elif road_true(current_dot, mode, 1, fx=0, fy=1):
                        current_dot = (begin_dot[0], current_dot[1] - 1)
                    else:
                        break

        return img, dot_dic

    def run(self):
        cap = cv2.VideoCapture(0)
        cap.set(cv2.CAP_PROP_FRAME_WIDTH, 160)
        cap.set(cv2.CAP_PROP_FRAME_HEIGHT, 120)
        self.skip_image(cap, 30)
        _, frame = cap.read()
        self.w, self.h = frame.shape[1], frame.shape[0]
        # 输出当前的图片大小
        print(self.w, self.h)
        self.middle_line_x = self.w // 2
        left_first_dot = (self.middle_line_x, self.h - 1)
        right_first_dot = (self.middle_line_x, self.h - 1)
        self.first_dot = (self.middle_line_x, self.h - 1)
        while 1:
            try:
                flag = 1
                # 返回处理过的黑白图像
                frame = self.img_dispose(cap)
                # 找中线并更新数据
                frame, dot_dic1 = self.grow_line(frame, 0, left_first_dot)
                frame, dot_dic2 = self.grow_line(frame, 1, right_first_dot)
                # print(dot_dic1, dot_dic2)
                for i in range(self.h + 1):
                    if dot_dic1.get(i, -1) != -1 and dot_dic2.get(i, -1) != -1:
                        frame[i][(dot_dic1.get(i, -1) + dot_dic2.get(i, -1)) // 2] = 127
                # 更新起始点
                for i in range(self.h - 1, self.h - 12, -5):
                    if dot_dic1.get(i, -1) != -1 and dot_dic2.get(i, -1) != -1:
                        left_first_dot = (dot_dic1.get(i, -1) + 5, self.h - 1)
                        right_first_dot = (dot_dic2.get(i, -1) - 5, self.h - 1)
                        break
                else:
                    left_first_dot = self.first_dot
                    right_first_dot = self.first_dot
                cv2.imshow("1", frame)
                command = cv2.waitKey(1) & 0xFF
                if command == ord('q'):
                    break
            except Exception as e:
                print(e)
                cv2.imshow("1", frame)

if __name__ == '__main__':
    robot = RobotControl()
    robot.run()