2021电子设计竞赛飞控视觉之openmv寻找方格中心

写在前面

这是我在电赛飞控备赛期间写的一个小函数，功能是寻找目标点所在方格的中心。这样四旋翼在方格地图上移动一定距离之后就可以使用openmv将四旋翼辅助定位至目前所在方格的中心。今年G题刚出来的时候本来以为能用上这个函数进行辅助定位，但是后面补充说明中方格的线太细，无法使用，就没能用上这个方案，把这个分享给大家。

详述

整体思路是先使用openmv中寻找直线的函数，找到所有横线和竖线，并筛选出离目标点最近的两条横线和竖线，从而锁定方格中心。
寻找直线的函数如下

该函数使用霍夫变换返回所有直线对象，我们调用该函数，通过限制寻找直线的角度来找到图像中所有的横线和竖线

min_degree = 80
max_degree = 100
for l in img.find_lines(roi=(50,30,220,180),threshold = 900, theta_margin = 25, rho_margin = 25):
        if (min_degree <= l.theta()) and (l.theta() <= max_degree):
            img.draw_line(l.line(), color = (255, 0, 0))
            heng.append(l)

 for l in img.find_lines(roi=(50,30,220,180),threshold = 1000,theta_margin = 35, rho_margin = 25):
        if (30 >= l.theta()) or (l.theta() >= 150):
            img.draw_line(l.line(), color = (255, 0, 0))
            shu.append(l)

对于找到的每条直线，我们可以获得其角度theta值和视野左上角到直线的垂直距离rho值，示意图如下，对于openmv来说，图中的原点就是openmv视野的左上角。

由于我们寻找的都是横线和竖线，可以把rho值近似认为是横线的纵坐标值和竖线的横坐标值，并以此筛选出离目标点最近的两条横线和两条竖线。

if len(heng)>=2:
        heng1 = heng[0]



        for l in heng:
            if abs(l.rho()-my_place[0])<abs(heng1.rho()-my_place[0]):
                heng1 = l

        heng.pop(heng.index(heng1))
        #print(heng)
        heng2 = heng[0]
        for l in heng:
            if abs(l.rho()-my_place[0])<abs(heng2.rho()-my_place[0]):
                heng2 = l
        count.append(1)

if len(shu)>=2:
        shu1 = shu[0]



        for l in shu:
            if abs(abs(l.rho())-my_place[1])<abs(abs(shu1.rho())-my_place[1]):
                shu1 = l

        shu.pop(shu.index(shu1))
       # print(shu)
        shu2 = shu[0]
        for l in shu:
            if abs(abs(l.rho())-my_place[1])<abs(abs(shu2.rho())-my_place[1]):
                shu2 = l
        count.append(1)

找到离目标最近的两条横线和竖线之后，我首先想到的是将两条横线的rho值取平均作为方格中心的纵坐标，将两条竖线的rho值取平均作为方格中心的横坐标。但经过验证，即使在倾斜的角度很小的情况下，直接使用rho取平均的做法也会让最后的结果产生较大的误差。因此我最后采用了另一种思路，我们得到四根直线（两根离目标最近的横线和两根离目标最近的竖线）的角度theta值和rho值之后就可以写出这四根直线的方程，将横线的方程与竖线的方程两两组合联立，即可解出方格对角两个点的坐标，再用这两个对角的横纵坐标分别取平均就能得到方格中心的坐标。

 if  len(count)==2:
        center_y_1=(math.cos(shu1.theta()/180*3.14159)*heng1.rho()-math.cos(heng1.theta()/180*3.14159)*shu1.rho())/(math.sin(heng1.theta()/180*3.14159)*math.cos(shu1.theta()/180*3.14159)-math.sin(shu1.theta()/180*3.14159)*math.cos(heng1.theta()/180*3.14159))
        center_x_1=(math.sin(shu1.theta()/180*3.14159)*heng1.rho()-math.sin(heng1.theta()/180*3.14159)*shu1.rho())/(math.cos(heng1.theta()/180*3.14159)*math.sin(shu1.theta()/180*3.14159)-math.cos(shu1.theta()/180*3.14159)*math.sin(heng1.theta()/180*3.14159))
        center_y_2=(math.cos(shu2.theta()/180*3.14159)*heng2.rho()-math.cos(heng2.theta()/180*3.14159)*shu2.rho())/(math.sin(heng2.theta()/180*3.14159)*math.cos(shu2.theta()/180*3.14159)-math.sin(shu2.theta()/180*3.14159)*math.cos(heng2.theta()/180*3.14159))
        center_x_2=(math.sin(shu2.theta()/180*3.14159)*heng2.rho()-math.sin(heng2.theta()/180*3.14159)*shu2.rho())/(math.cos(heng2.theta()/180*3.14159)*math.sin(shu2.theta()/180*3.14159)-math.cos(shu2.theta()/180*3.14159)*math.sin(heng2.theta()/180*3.14159))
        center_x=(abs(center_x_1)+abs(center_x_2))/2
        center_y=(abs(center_y_1)+abs(center_y_2))/2

这个地方要注意直线对象返回的角度值theta和进行sin，cos运算的函数输入的角度一个是弧度制一个是角度值，需要进行一次转换。经验证，该方案效果较好。

最后

该函数其实不只可以在全方格的地图可以使用，在一些简单的方格迷宫中同样可以进行定位。

例如这样的一个方格迷宫，我们只需要调整好寻找直线函数的参数，就可以在这样只有三条边甚至两条边的“伪方格”中寻找方格中心，从而进行辅助定位。

整个函数如下：

def find_center(img):
    my_place=[120,160]
    heng=[]
    shu=[]
    center = []
    min_degree = 80
    max_degree = 100
    count=[]
###################找最靠近飞机坐标点的两条横线从而得出方框中心点纵坐标
    for l in img.find_lines(roi=(50,30,220,180),threshold = 900, theta_margin = 25, rho_margin = 25):
        if (min_degree <= l.theta()) and (l.theta() <= max_degree):
            img.draw_line(l.line(), color = (255, 0, 0))
            heng.append(l)
   # print("横线p值"heng.rho())
    if len(heng)>=2:
        heng1 = heng[0]
        for l in heng:
            if abs(l.rho()-my_place[0])<abs(heng1.rho()-my_place[0]):
                heng1 = l
        heng.pop(heng.index(heng1))
        #print(heng)
        heng2 = heng[0]
        for l in heng:
            if abs(l.rho()-my_place[0])<abs(heng2.rho()-my_place[0]):
                heng2 = l
        count.append(1)
        #center_y = (heng1.rho()+heng2.rho())/2
        #center.append(center_y)
        #print(heng1,heng2,center_y)
##########################找中心横坐标
    for l in img.find_lines(roi=(50,30,220,180),threshold = 1000,theta_margin = 35, rho_margin = 25):
        if (30 >= l.theta()) or (l.theta() >= 150):
            img.draw_line(l.line(), color = (255, 0, 0))
            shu.append(l)
    #print("竖线坐标"shu)
    if len(shu)>=2:
        shu1 = shu[0]
        for l in shu:
            if abs(abs(l.rho())-my_place[1])<abs(abs(shu1.rho())-my_place[1]):
                shu1 = l
        shu.pop(shu.index(shu1))
       # print(shu)
        shu2 = shu[0]
        for l in shu:
            if abs(abs(l.rho())-my_place[1])<abs(abs(shu2.rho())-my_place[1]):
                shu2 = l
        count.append(1)
    if  len(count)==2:
 
        center_y_1=(math.cos(shu1.theta()/180*3.14159)*heng1.rho()-math.cos(heng1.theta()/180*3.14159)*shu1.rho())/(math.sin(heng1.theta()/180*3.14159)*math.cos(shu1.theta()/180*3.14159)-math.sin(shu1.theta()/180*3.14159)*math.cos(heng1.theta()/180*3.14159))
        center_x_1=(math.sin(shu1.theta()/180*3.14159)*heng1.rho()-math.sin(heng1.theta()/180*3.14159)*shu1.rho())/(math.cos(heng1.theta()/180*3.14159)*math.sin(shu1.theta()/180*3.14159)-math.cos(shu1.theta()/180*3.14159)*math.sin(heng1.theta()/180*3.14159))
        center_y_2=(math.cos(shu2.theta()/180*3.14159)*heng2.rho()-math.cos(heng2.theta()/180*3.14159)*shu2.rho())/(math.sin(heng2.theta()/180*3.14159)*math.cos(shu2.theta()/180*3.14159)-math.sin(shu2.theta()/180*3.14159)*math.cos(heng2.theta()/180*3.14159))
        center_x_2=(math.sin(shu2.theta()/180*3.14159)*heng2.rho()-math.sin(heng2.theta()/180*3.14159)*shu2.rho())/(math.cos(heng2.theta()/180*3.14159)*math.sin(shu2.theta()/180*3.14159)-math.cos(shu2.theta()/180*3.14159)*math.sin(heng2.theta()/180*3.14159))
        center_x=(abs(center_x_1)+abs(center_x_2))/2
        center_y=(abs(center_y_1)+abs(center_y_2))/2
        print("中心x",center_x_1,center_x_2)
        print("中心y",center_y_1,center_y_2)
		return int(center_x), int(center_y)
        img.draw_cross(my_place[1], my_place[0], size=3,color=(0))
        img.draw_cross(int(center_x), int(center_y), size=5,color=(0))
        img.draw_arrow(my_place[1],  my_place[0], int(center_x), int(center_y), color = (0), size = 10, thickness = 2)