写在前面
这是我在电赛飞控备赛期间写的一个小函数,功能是寻找目标点所在方格的中心。这样四旋翼在方格地图上移动一定距离之后就可以使用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)
本文内容由网友自发贡献,版权归原作者所有,本站不承担相应法律责任。如您发现有涉嫌抄袭侵权的内容,请联系:hwhale#tublm.com(使用前将#替换为@)