如何让机器人识别面前的绿色小球并且进行自动跟随?
先把问题拆成两个子问题。第一,识别绿色小球;第二,自动跟随。
目标识别
目标识别分为三步:
- 图像预处理(定义结构元素,HSV 转换,腐蚀与膨胀)
- 轮廓检测
- 矩形绘制
图像预处理
kernel = cv2.getStructuringElement(cv2.MORPH_RECT,(5,5))
我们使用cv2.getStructuringElement
函数生成结构化元素。
该函数需要两个参数。第一个参数,控制元素的形状。第二个参数,控制元素的大小。
详细用法指路——> opencv-python结构化元素cv2.getStructuringElement()
sensity = 10
green_lower = np.array([50-sensity,43,46])
green_upper = np.array([60+sensity,255,255])
hsv_frame = cv2.cvtColor(img, cv2.COLOR_BGR2HSV)
mask = cv2.inRange(hsv_frame, green_lower,green_upper)
mask2 = cv2.morphologyEx(mask, cv2.MORPH_OPEN, kernel)
mask3 = cv2.morphologyEx(mask2, cv2.MORPH_CLOSE, kernel)
mask4 = FillHole(mask3)
mask5 = cv_erodedAndDilated(mask4)
cnt,heridency = cv2.findContours(mask5,
cv2.RETR_EXTERNAL,
cv2.CHAIN_APPROX_SIMPLE)
maxArea = 0
maxIndex = 0
for i, j in enumerate(cnt):
area = cv2.contourArea(j)
if area > maxArea:
maxArea = area
maxIndex = i
cv2.drawContours(img,cnt,maxIndex,(255,0,255),10)
x, y, w, h = cv2.boundingRect(np.array(cnt[maxIndex]))
cv2.rectangle(img, (x, y), (x+w, y+h), (0, 255, 0), 2)
使用cv2.boundingRect
函数报错的解决方法——>python报错error: (-215:Assertion failed) npoints >= 0 && (depth == CV_32F || depth == CV_32S) in function boundingRect
运动控制
运动控制分为四步:
- 定义标准位置与大小
- 方位判断
- 速度控制
定义标准位置与大小
把标准位置定义在相机中心,标准大小可以自行设置需要的像素点。
height,width,layer = img.shape
center_x = height/2
center_y = width/2
size = 6000
area = w*h
robot_x = x+w/2
robot_y = y+h/2
方位判断
在无人机的飞行过程中,通过不断比较目标位置与标准位置,来判断无人机的方位。
left_sign = 0
right_sign = 0
forward_sign = 0
up_sign = 0
down_sign = 0
if area < size and area > size-2000:
forward_sign = 1
elif area > size-4000 and area <= size-2000:
forward_sign = 2
elif area <= size-4000 and size > 0:
forward_sign = 3
if area >= size and area < size+100000:
forward_sign = -1
elif area >= size+100000 and area < size+300000:
forward_sign = -2
elif area > size+300000:
forward_sign = -3
if robot_x < center_x and robot_x > center_x-120:
left_sign = 1
elif robot_x <= center_x-120 and robot_x >= center_x-240:
left_sign = 2
elif robot_x < center_x-240:
left_sign =3
if robot_x > center_x and robot_x < center_x+120:
right_sign = 1
elif robot_x >= center_x+120 and robot_x <= center_x+240:
right_sign = 2
elif robot_x > center_x+240:
right_sign =3
速度控制
无人机遥控有四个杆量,横滚、俯仰、油门、偏航。这四个杆量表示飞机运动的四个自由度。
横滚,控制无人机左右移动;
俯仰,控制无人机前后移动;
油门,控制无人机高度;
偏航,控制无人机水平方向的角度;
"""
控制飞机遥控器的四个杆量
参数:
a – float:[-100, 100] 横滚
b – float:[-100, 100] 俯仰
c – float:[-100, 100] 油门
d – float:[-100, 100] 偏航
"""
a_level = [25,35,45]
b_level_forward = [15,25,40]
b_level_backward = [-30,-45,-60]
if forward == 1:
tl_flight.rc(b=b_level_forward[0])
elif forward == 2:
tl_flight.rc(b=b_level_forward[1])
elif forward == 3:
tl_flight.rc(b=b_level_forward[2])
elif forward == 0:
tl_flight.rc(b=0)
elif forward == -1:
tl_flight.rc(b=b_level_backward[0])
elif forward == -2:
tl_flight.rc(b=b_level_backward[1])
elif forward == -3:
tl_flight.rc(b=b_level_backward[2])
if left == 1:
tl_flight.rc(a=-a_level[0])
elif left == 2:
tl_flight.rc(a=-a_level[1])
elif left == 3:
tl_flight.rc(a=-a_level[2])
elif right == 1:
tl_flight.rc(a=a_level[0])
elif right == 2:
tl_flight.rc(a=a_level[1])
elif right == 3:
tl_flight.rc(a=a_level[2])
elif right == 0 and left == 0:
tl_flight.rc(a=0)
代码汇总
import time
from robomaster import robot,battery
from robomaster import camera
import cv2
import numpy as np
import sys
sys.path.append(r'P:\Python\cv2')
from string_recognise_self import cv_show,FillHole,cv_drawContours,cv_center,cv_erodedAndDilated
"""碎片填充"""
def FillHole(img,SavePath='img_fillhole.jpg'):
img_in = img
img_floodfill = img_in.copy()
h, w = img_in.shape[:2]
mask = np.zeros((h+2, w+2), np.uint8)
isbreak = False
for i in range(img_floodfill.shape[0]):
for j in range(img_floodfill.shape[1]):
if(img_floodfill[i][j]==0):
seedPoint=(i,j)
isbreak = True
break
if(isbreak):
break
cv2.floodFill(img_floodfill, mask, seedPoint, 255);
img_floodfill_inv = cv2.bitwise_not(img_floodfill)
img_out = img_in | img_floodfill_inv
cv2.imwrite(SavePath, img_out)
return img_out
"""腐蚀与膨胀"""
def cv_erodedAndDilated(img):
size = 20
kernel = cv2.getStructuringElement(cv2.MORPH_RECT,(size,size))
eroded = cv2.erode(img, kernel)
img_dilated = cv2.dilate(eroded, kernel)
return img_dilated
"""小球位置识别"""
def cv_center(img):
kernel = cv2.getStructuringElement(cv2.MORPH_RECT,(5,5))
height,width,layer = img.shape
center_x = height/2
center_y = width/2
sensity = 10
green_lower = np.array([50-sensity,43,46])
green_upper = np.array([60+sensity,255,255])
hsv_frame = cv2.cvtColor(img, cv2.COLOR_BGR2HSV)
mask = cv2.inRange(hsv_frame, green_lower,green_upper)
mask2 = cv2.morphologyEx(mask, cv2.MORPH_OPEN, kernel)
mask3 = cv2.morphologyEx(mask2, cv2.MORPH_CLOSE, kernel)
mask4 = FillHole(mask3)
mask5 = cv_erodedAndDilated(mask4)
cnt,heridency = cv2.findContours(mask5,
cv2.RETR_EXTERNAL,
cv2.CHAIN_APPROX_SIMPLE)
if cnt == []:
continue
maxArea = 0
maxIndex = 0
for i, j in enumerate(cnt):
area = cv2.contourArea(j)
if area > maxArea:
maxArea = area
maxIndex = i
cv2.drawContours(img,cnt,maxIndex,(255,0,255),10)
x, y, w, h = cv2.boundingRect(np.array(cnt[maxIndex]))
cv2.rectangle(img, (x, y), (x+w, y+h), (0, 255, 0), 2)
size = 6000
area = w*h
robot_x = x+w/2
robot_y = y+h/2
left_sign = 0
right_sign = 0
forward_sign = 0
up_sign = 0
down_sign = 0
if area < size and area > size-2000:
forward_sign = 1
elif area > size-4000 and area <= size-2000:
forward_sign = 2
elif area <= size-4000 and size > 0:
forward_sign = 3
if area >= size and area < size+100000:
forward_sign = -1
elif area >= size+100000 and area < size+300000:
forward_sign = -2
elif area > size+300000:
forward_sign = -3
if robot_x < center_x and robot_x > center_x-120:
left_sign = 1
elif robot_x <= center_x-120 and robot_x >= center_x-240:
left_sign = 2
elif robot_x < center_x-240:
left_sign =3
if robot_x > center_x and robot_x < center_x+120:
right_sign = 1
elif robot_x >= center_x+120 and robot_x <= center_x+240:
right_sign = 2
elif robot_x > center_x+240:
right_sign =3
return forward_sign,left_sign,right_sign
if __name__ == '__main__':
tl_drone = robot.Drone()
tl_drone.initialize()
tl_flight = tl_drone.flight
tl_camera = tl_drone.camera
tl_battery = tl_drone.battery
print('battery=\t'+str(tl_battery.get_battery())+'%')
cnt = []
print(tl_drone.get_temp())
tl_flight.takeoff().wait_for_completed()
tl_camera.set_fps(fps='high')
tl_camera.set_bitrate(bitrate=6)
tl_camera.set_resolution(resolution='high')
tl_camera.start_video_stream(display=False)
try:
while True:
img = tl_camera.read_cv2_image(strategy='newest')
forward,left,right = cv_center(img)
"""
控制飞机遥控器的四个杆量
参数:
a – float:[-100, 100] 横滚
b – float:[-100, 100] 俯仰
c – float:[-100, 100] 油门
d – float:[-100, 100] 偏航
"""
a_level = [25,35,45]
b_level_forward = [15,25,40]
b_level_backward = [-30,-45,-60]
if forward == 1:
tl_flight.rc(b=b_level_forward[0])
elif forward == 2:
tl_flight.rc(b=b_level_forward[1])
elif forward == 3:
tl_flight.rc(b=b_level_forward[2])
elif forward == 0:
tl_flight.rc(b=0)
elif forward == -1:
tl_flight.rc(b=b_level_backward[0])
elif forward == -2:
tl_flight.rc(b=b_level_backward[1])
elif forward == -3:
tl_flight.rc(b=b_level_backward[2])
if left == 1:
tl_flight.rc(a=-a_level[0])
elif left == 2:
tl_flight.rc(a=-a_level[1])
elif left == 3:
tl_flight.rc(a=-a_level[2])
elif right == 1:
tl_flight.rc(a=a_level[0])
elif right == 2:
tl_flight.rc(a=a_level[1])
elif right == 3:
tl_flight.rc(a=a_level[2])
elif right == 0 and left == 0:
tl_flight.rc(a=0)
time.sleep(0.08)
cv2.namedWindow('Drone', 0)
cv2.imshow("Drone", img)
cv2.waitKey(10)
tl_flight.rc(a=0,b=0,c=0,d=0)
except KeyboardInterrupt:
pass
cv2.destroyAllWindows()
tl_camera.stop_video_stream()
tl_flight.land().wait_for_completed()
tl_drone.close()
本文内容由网友自发贡献,版权归原作者所有,本站不承担相应法律责任。如您发现有涉嫌抄袭侵权的内容,请联系:hwhale#tublm.com(使用前将#替换为@)