pid摄像头循迹（opencv和openmv）

用摄像头进行循迹的方法参考

去年用openmv做了一个循迹小车，效果还不错，实验室里做了汇报，这里也同步分享一下制作的一些细节。
小车灰常简陋，当时硬件水平有限，轻喷>_<

csdn视频放不出来，只能放已经投稿的视频，这里就不展示了叭。
运行效果其实和openmv官方教程给的视频里小车效果类似，大家可以作为参考。

硬件选型方面

1. 稳压模块x2
2. 9v锂电池x1
3. openmv
4. 减速电机（6v 280rpm）
5. 万向轮
6. L298N直流电机驱动模块
   （插一下，这里的稳压可以稳压到7V和3.3V,L298N本身可以将电压稳压到5V）

软件思路

如上图所示，分为图像预处理、线性拟合以及PID控制转速。

一.图像预处理：

1.按阈值取二值化 （可以加入中值滤波或高斯滤波）
2.腐蚀操作 （去除噪点）
3.膨胀操作（可有可无）
因为openmv的算力有限，若是同时加上了膨胀和腐蚀操作，运行循迹程序时帧率会变得很低（可能低于15fps）。

代码部分

图像预处理在openmv和opencv中均有现成的库函数，直接调用即可。
1.openmv：

img.dilate(2) #膨胀
img.erode(2)# 腐蚀
img = sensor.snapshot().binary([THRESHOLD]) # 按阈值二值化

2.opencv：

mask=cv2.inRange(img,black_min,black_max) #二值化
kernel=np.ones((3,3),np.uint8) #半径大小
erode=cv2.morphologyEx(mask,cv2.MORPH_ERODE,kernel)# 腐蚀

腐蚀膨胀 示意图如下所示：

二.线性拟合

这里的线性拟合采用的是最小二乘法进行拟合，遍历所有像素点，复杂度为O(n²)
openmv有现成的库函数：

line = img.get_regression([(100,100,0,0,0,0)], robust = True)

官网给的解释是快速鲁棒线性回归
可以直接返回得到拟合的直线对象

参考文档：https://docs.singtown.com/micropython/zh/latest/openmvcam/library/omv.image.html?highlight=get_regression#image.get_regression

官方文档介绍：

opencv线性拟合：

由于我没有找到相关的线性拟合函数，于是自己写了一个函数：
传入参数img为二值化图像，result为原图。

# 线性拟合
def liner(img,result):
    # print(img)
    n=[len(img[:,0]),len(img[0,:])]
    # print(n,'n')
    (x_point,y_point)=np.nonzero(img)
    if len(x_point)<2:
        return 0
    f1 = np.polyfit(x_point, y_point,1)
    p1 = np.poly1d(f1)
    point1=(int(p1(0)),0)
    point2=(int(p1(n[0])),n[0])
    print(point2,point1)
    result = cv2.line(result, (int(n[1]/2),0) , (int(n[1]/2),n[0]), (0, 0, 255),2)
    result=cv2.line(result,point1,point2,(0,255,0),2)
    cv2.imshow('inside',result)
    print(f1)
    return [n[1]/2-p1(n[0]),f1[1]]

返回值为拟合的直线与图像下方中点的距离和函数的k值
运行效果如下：

从左到右分别为：
1.腐蚀后的图像 2.拟合效果图像 3.二值化图像

实际在树莓派上运行时，帧率也比较高，拟合效果也比较好。

可以看到有81帧每秒，运行速度较为不错。

三.PID控制

pid算法对于熟悉控制算法来说已经是非常常见的算法了，这里就不过多介绍，直接贴代码：
（这里pid的调整输入为角度theta和距离rho）

import time
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 = time.time()*1000 # 当前时间，单位为ms
        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')

一些问题：

newimg=155*np.ones(shape,dtype = np.uint8)

在转化时注意类型转化

img=cv2.resize(img,(480,640))

cv2.inRange %用于按阈值取二值化的函数：

关于控制直流电机：

openmv中控制电机可以采用定时器输出pwm来控制电机
而在树莓派中可以调用RPi.GPIO库或pigpio库来实现。
以RPi.GPIO库为例：

p = GPIO.PWM(channel, frequency)
p.start(dc)   # dc 代表占空比（范围：0.0 <= dc <= 100.0）
p.ChangeFrequency(freq)   # freq 为设置的新频率，单位为 Hz
p.ChangeDutyCycle(dc)  # 范围：0.0 <= dc <= 100.0

来源：小陈皓