OpenMV模块视觉测距（避障）+颜色识别+TFT-LCD显示（实现原理与代码分析）

0.准备工作

• OpenMV 硬件版本随意，此功能对算力要求不高
• IDE使用：OpenMV IDE （v2.9）
• 建议拥有一定的Python和初中物理基础

1.工作原理

首先直接上原理图：
根据原理图，OpenMV测距的原理大致可以总结为以下步骤：

1. 根据小孔成像原理，将摄像头的镜头视为小孔，则可以得到真实物体与摄像头中的物体图像二者间存在如图1-1所示的几何关系。
   
2. 上式中两个方程联立得：
   
   且对于β角，由镜头右侧的几何关系得到下式：
   
3. 将步骤2中的第二个式子带入第一个式子中可得：
   
4. 摄像头的视角α为确定硬件参数，Rm在对一个物体进行测距时其值亦是确定的，Apix值得大小在代码中设置图像像素时也已确定。所以可以简化得到下式：
   
   所以：
   所以实际物体与镜头得距离Lm与实际物体在摄像头图像中所占的像素值Bpix成反比，即对于同一个物体，距离越近，像素点越多。所以在程序设计中，将目标色块的宽和高的均值近似代替物体在摄像头图像中所占的像素值。

2.程序流程分析

下图为视觉模块运行伪码，大致思想一致，仅供参考。

3.代码分析

代码还添加了两个比较简单的功能：颜色识别（红色和蓝色）和LCD屏幕显示（需要给OpenMV 外接一个TFT-LCD的扩展），此外还对图像进行了一定的滤波处理。

如果需要对这些功能进一步讲解的可在评论区留言，后续考虑更新。

需要特别注意的是：测距的比例常数K参数和颜色阈值需要根据实际情况进行调节。

#导入函数库
import sensor, image, time, math ,lcd
clock = time.clock()# 跟踪FPS帧率
kernel_size = 1 # 3x3==1, 5x5==2, 7x7==3, etc.
#初始卷积核
kerne1 = [-1, -1,  -1, \
          -1,  9,  -1, \
           -1,  -1,  -1]
#变量初始化
K=327#测距比例常数 s=k*pix
Red_TH      = (46, 79, 28, 65, -24, 63)#红色阈值
Blue_TH     = (24, 63, -12, 33, -69, -25)#蓝色阈值
#TFT-LCD初始化
lcd.init()                #lcd函数初始化
lcd.set_direction(1)      #设置LCD显示方向 0和2是竖屏 1和3是横屏
#函数定义
def Get_MaxIndex(blobs):
    maxb_index=0                          #最大色块索引初始化
    max_pixels=0                          #最大像素值初始化
    for i in range(len(blobs)):           #对N个色块进行N次遍历
        if blobs[i].pixels() > max_pixels:#当某个色块像素大于最大值
            max_pixels = blobs[i].pixels()#更新最大像素
            maxb_index = i                #更新最大索引
            return  maxb_index
#摄像头初始化
sensor.reset()                     #初始化相机传感器
sensor.set_pixformat(sensor.RGB565)#设置相机模块的像素模式 16 bits/像素 GRAY为8
sensor.set_framesize(sensor.QQVGA) #设置相机模块的帧大小 160x120
sensor.skip_frames(30)             #跳过30帧 让相机图像在改变相机设置后稳定下来
sensor.set_auto_gain(False)        #关闭自动增益
sensor.set_auto_whitebal(False)    #关闭默认的白平衡
#主函数
while (1):
    clock.tick()
	OBS_img = sensor.snapshot()#镜头畸变校正 去除镜头的鱼眼效果
	OBS_img.mean(1)#均值滤波
	OBS_img.morph(kernel_size, kerne1)#卷积核滤波
	#在LCD上打印帧率和OpenMV工作模式
	OBS_img.draw_string(0, 20,"OpenMv Mode:\r(2)Obstacle Mode")
	OBS_img.draw_string(0, 0, "FPS:%.2f"%(clock.fps()))
	#寻找色块 (颜色阈值,ROI区域,像素阈值,区域阈值)
	blobs=OBS_img.find_blobs(OBS_TH1,pixels_threshold=100, 				                                            area_threshold=100,merge=True)
    if blobs:#找到了黄色色块
                    maxb_index =Get_MaxIndex(blobs)#找到最大色块并返回索引值
                    #返回最大色块外框元组(x,y,w,h) 绘制线宽为2的矩形框 不填充矩形
                    OBS_img.draw_rectangle(blobs[maxb_index].rect(), thickness = 2, fill = False)
                    #最大色块的中心位置标记十字
                    OBS_img.draw_cross(blobs[maxb_index].cx(),blobs[maxb_index].cy())
                    maxb= blobs[maxb_index]#定义最大色块为maxb
                    CPix = (maxb[2]+maxb[3])/2#色块的23索引可以获得色块宽度和高度 近似代替实物像素
                    Lm =round((K/CPix),2) #实际距离和像素大小成反比 圆整到小数后两位
                    OBS_img.draw_string(0, 0, "FPS:%.2f"%(clock.fps()))#LCD上显示帧率
                    OBS_img.draw_string(80, 0,"Dis:%.2fcm"%Lm)     #LCD上显示距离
                    print("Dis:%.2fcm"%Lm)
                    if Lm > OBS_DisTH:#如果距离大于阈值
                                OBS_img.draw_string(0, 10,"Distance is Ok")#LCD显示距离正常
                    if Lm <= OBS_DisTH:#如果距离小于阈值
                                OBS_img.draw_string(0, 10,"Dis is Not Ok ")#LCD显示距离过小
                else :#如果没找到色块 则距离也是正常 相当于距离大于阈值
                        OBS_img.draw_string(0, 10,"Distance is Ok")#LCD显示距离正常
                color_num=0
                blobs1 = OBS_img.find_blobs([Red_TH],pixels_threshold=100)
                if blobs1:
                    color_num=1
                    print("物体颜色:红色")
                    OBS_img.draw_string(0, 40,"Object:\r Red")
                else :
                    blobs2 = OBS_img.find_blobs([Blue_TH],pixels_threshold=100)
                    if blobs2:
                        color_num=2
                        print("物体颜色:蓝色")
                        OBS_img.draw_string(0, 40,"Object:\r Blue")
                lcd.display(OBS_img)#LCD显示图像

代码运行实现效果：

若没有外接LCD扩展，则将代码段中和LCD相关的函数删除即可，使用OpenMV IDE在线调试即可。

4.总结

上图所示代码实际运行的帧率比较低，主要原因有以下两个：

1.硬件问题，博主使用的版本比较低，可以尝试使用高版本OpenMV

2.图像滤波问题。之前加入图像滤波是想提高色块识别的准确性，但是相应的运算速度也会下降。可以考虑不对图像进行滤波提高帧率，即删除下面两行代码：

Tips：IDE在线调试所显示的帧率一般为脱机上电运行帧率一半左右。