pid摄像头循迹（opencv和openmv）
pid摄像头循迹（opencv和openmv）
- 用摄像头进行循迹的方法参考
-
- 硬件选型方面
- 软件思路
- 一.图像预处理：
- 代码部分
- 二.线性拟合
- opencv线性拟合：
- 实际在树莓派上运行时，帧率也比较高，拟合效果也比较好。
- 三.PID控制
- 关于控制直流电机：
用摄像头进行循迹的方法参考
去年用openmv做了一个循迹小车，效果还不错，实验室里做了汇报，这里也同步分享一下制作的一些细节。
小车灰常简陋，当时硬件水平有限，轻喷>_<
csdn视频放不出来，只能放已经投稿的视频，这里就不展示了叭。
运行效果其实和openmv官方教程给的视频里小车效果类似，大家可以作为参考。
硬件选型方面
- 稳压模块x2
- 9v锂电池x1
- openmv
- 减速电机（6v 280rpm）
- 万向轮
- 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