2021 대한전기학회 미니드론 자율주행 경진대회

114886 단어 2021 미니드론 자율주행2021 미니드론 자율주행

나갔던 대회 자료 정리하기

대한전기학회 MINI DRONE 자율주행 경진대회

B 리그 Team Himdrong

목차

I. 대회 진행 전략

  1. Python 설치 및 VSCode 개발 환경 구축
  2. 하드웨어
  3. Raspberry Pi OS 설치
  4. 사용 프로그램
  5. 드론 작동법
  6. 트랙 제작

II. 알고리즘

  1. 트랙 분석
  2. 이미지에 따른 모드 분류
  3. 알고리즘 종합

III. 사용 코드

  1. 이미지 처리
  2. 드론 제어
  3. 최종 코드

I. 대회 진행 전략

I-1. Python 설치 및 VSCode 개발 환경 구축

I-2. 하드웨어

  • 모델명 : Raspberry Pi Zero W
  • 프로세서 : BCM2835 @ 1GHz
  • 메모리 : 512MB RAM
  • 가용 전력 : 0.5-0.7V
  • 모델명 : Frank-S01-V1.0 (SZH-EK104)
  • 칩셋 : OV5647
  • 크기 : 60mm × 11.5mm × 5mm
  • 이미지해상도 : 2592×1944p
  • 동영상해상도 : 1080p30, 720p60, 640×480p60/90
  • 화각 : 72.4°
  • 초점방식 : 조절 가능한 고정식 렌즈
  • 제품명 : 코드론 DIY
  • 크기 : 191mm x 191mm x 55mm
  • 무게 : 127g
  • 통신 방식 : RF
  • 탑재 센서 : 옵티컬 플로우 센서, 3축 자이로 센서, 3축 가속도 센서, 기압 센서, 온도 센서, 고도 센서
  • 드론 조종 모드 : 모드1, 모드2

I-3. Raspberry Pi OS 설치

1. Raspberry Pi OS Lite 다운로드

다운로드

2. Raspberry Pi imager 설치

다운로드

3. SD카드에 Raspberry Pi OS 설치

  • PC에 SD카드 연결 후 Raspberry Pi OS 설치

I-4. 사용 PC 프로그램

I-5. 드론 작동법

  • Python 코드를 이용하여 드론 제어
설명기능명령어
드론 객체 선언생성Drone()
이착륙 제어이륙sendTakeOff()
착륙sendLanding()
방향 및 이동 제어이동sendControlPosition16()
이동sendControlWhile()

방향 및 이동제어에서는 좀 더 정밀한 제어가 가능한 sendControlPosition16() 함수를 중점적으로 사용


def sendControlPosition(self, positionX, positionY, positionZ, velocity, heading, rotationalVelocity):
변수 이름형식범위단위설명
position XInt16-100 ~ 100(-10.0 ~ 10.0)meter x 10앞(+), 뒤(-)
position YInt16-100 ~ 100(-10.0 ~ 10.0)meter x 10좌(+), 우(-)
position ZInt16-100 ~ 100(-10.0 ~ 10.0)meter x 10위(+), 아래(-)
velocityInt165~200(0.5 ~ 2.0)m/s x 10위치 이동 속도
headingInt16-360 ~ 360degree좌회전(+), 우회전(-)
rotationalVelocityInt1610 ~ 360degree/s좌우 회전속도

I-6. 트랙 제작

  • 교내 강의실 대여 후 직접 제작


II. 알고리즘

II-1. 트랙 분석

  1. 링과의 거리에 따라 직진 거리 조절
  2. 특정 지점에서 드론 위치 조정
  3. 링 통과 후 색깔 판단
  4. 판단한 색에 따라 드론 동작

II-2. 이미지에 따른 모드 분류

  • 링과의 거리에 따라 검출된 픽셀 수에 맞춰 직선 이동 모드 변경

II-3. 알고리즘 종합

III. 사용 코드

  • 대회 사용 모듈

e-drone==21.1.6
numpy==1.16.2
Pillow==5.4.1
opencv==3.2.0

  • 코드

III-1. 이미지 처리

1-a) 원본 이미지

  • 각 지점에서 드론 Raspberry Pi에 연결된 카메라로 찍은 원본 이미지

1-b) Threshold 값 설정

  • Toolbar code를 이용하여 적정 Threshold 값 탐색

  • 변수 안에 값 설정

lower_blue = (95, 0, 50)  
upper_blue = (110, 255, 250)

lower_red = (0, 0, 5)  
upper_red = (17, 255, 240)

lower_purple = (110, 0, 5)
upper_purple = (140, 255, 50)

1-c) HSV 변환

  • 각 지점에서 받아온 원본 이미지를 HSV 변환
img = frame.array
img = cv2.flip(img, 0)
img = cv2.flip(img, 1)
imghsv = cv2.cvtColor(img, cv2.COLOR_BGR2HSV)

1-d) Threshold 값 적용

  • HSV 변환된 이미지에 각 Threshold 값을 적용
imgH_B = cv2.inRange(imghsv, lower_blue, upper_blue)
imgH_R = cv2.inRange(imghsv, lower_red, upper_red)
imgH_P = cv2.inRange(imghsv, lower_purple, upper_purple)

1-e) MedianBlur 적용

  • Threshold 값 적용된 이미지에 MedianBlur로 노이즈 제거
tmpB = cv2.medianBlur(imgH_B, 21)
tmpR = cv2.medianBlur(imgH_R, 7)
tmpP = cv2.medianBlur(imgH_P, 7)




III-2. 드론 제어

2-a) Blue 픽셀 개수에 따라 모드 분류

  • Blue 픽셀 개수 합으로 링과의 거리 판단 및 모드 분류 
mode = {1:[range(100, 30000), range(30000, 100000), range(100000, 150000), 150000], 
        2:[range(100, 50000), range(50000, 120000), range(120000, 170000), 170000], 
        3:[range(100, 70000), range(70000, 140000), range(140000, 200000), 200000]}

2-b) 직선 이동

  • 링과의 거리에 따라 이동 속도와 거리 조절
def moveLarge(drone, BlueSum):
    #이동 11
    print(f"move Large / BlueSum : {BlueSum}")            
    drone.sendControlPosition16(11, 0, 0, 6, 0, 0)
    sleep(4)
        

def moveSoso(drone, BlueSum):
    #이동 8
    print(f"move soso / BlueSum : {BlueSum}")
    drone.sendControlPosition16(8, 0, 0, 5, 0, 0)
    sleep(4)

2-c) 링 근접

  • 근접 거리에 따라 드론 위치 조정
def moveSmall(drone, BlueSum, dist_x, dist_y):
    #이동 6
    print(f"move small / BlueSum : {BlueSum}")
    if (dist_x == 0 and dist_y == 0):
        drone.sendControlPosition16(6, 0, 0, 4, 0, 0)
        sleep(3)
                
    elif (dist_x != 0 and dist_y == 0):
        drone.sendControlPosition16(0, dist_x//abs(dist_x), 0, 1, 0, 0)
        sleep(3)
        drone.sendControlPosition16(6, 0, 0, 4, 0, 0)
        sleep(3)
    
    elif (dist_x == 0 and dist_y != 0):
        drone.sendControlPosition16(0, 0, 2*(dist_y//abs(dist_y)), 2, 0, 0)
        sleep(3)
        drone.sendControlPosition16(6, 0, 0, 4, 0, 0)
        sleep(3)
                    
    elif (dist_x != 0 and dist_y != 0):
        drone.sendControlPosition16(0, dist_x//abs(dist_x), 0, 1, 0, 0)
        sleep(3)
        drone.sendControlPosition16(0, 0, 2*(dist_y//abs(dist_y)), 2, 0, 0)
        sleep(3)
        drone.sendControlPosition16(6, 0, 0, 4, 0, 0)
        sleep(3)


def move2Small(drone, BlueSum, dist_x, dist_y):
    #속도 4
    print(f"move 2 Small / BlueSum : {BlueSum}")
    if (dist_x == 0 and dist_y == 0):
        drone.sendControlPosition16(4, 0, 0, 3, 0, 0)
        sleep(3)
                
    elif (dist_x != 0 and dist_y == 0):
        drone.sendControlPosition16(0, dist_x//abs(dist_x), 0, 1, 0, 0)
        sleep(3)
        drone.sendControlPosition16(4, 0, 0, 3, 0, 0)
        sleep(3)
    
    elif (dist_x == 0 and dist_y != 0):
        drone.sendControlPosition16(0, 0, 2*(dist_y//abs(dist_y)), 2, 0, 0)
        sleep(3)
        drone.sendControlPosition16(4, 0, 0, 3, 0, 0)
        sleep(3)
                    
    elif (dist_x != 0 and dist_y != 0):
        drone.sendControlPosition16(0, dist_x//abs(dist_x), 0, 1, 0, 0)
        sleep(3)
        drone.sendControlPosition16(0, 0, 2*(dist_y//abs(dist_y)), 2, 0, 0)
        sleep(3)
        drone.sendControlPosition16(4, 0, 0, 3, 0, 0)
        sleep(3)



2-d) 링 대비 드론 위치 판단

  • 이미지 상,하,좌,우 4분할 후 Blue 픽셀 개수 차이 검출
def detectDist(tmpB):
    tmpB_div_1 = tmpB[:240, :320]
    tmpB_div_2 = tmpB[240:, :320]
    tmpB_div_3 = tmpB[:240, 320:]
    tmpB_div_4 = tmpB[240:, 320:]

    tmpB_div_1 = np.sum(tmpB_div_1 == 255, axis = None)
    tmpB_div_2 = np.sum(tmpB_div_2 == 255, axis = None)
    tmpB_div_3 = np.sum(tmpB_div_3 == 255, axis = None)
    tmpB_div_4 = np.sum(tmpB_div_4 == 255, axis = None)
    
    dist_x = (tmpB_div_3 + tmpB_div_4) - (tmpB_div_1 +tmpB_div_2)
    dist_y = (tmpB_div_4 + tmpB_div_2) - (tmpB_div_3 +tmpB_div_1)
    
    return (dist_x, dist_y)

2-e) 링 내부 판단 후 드론 동작

  • 링 내부일 경우 색 검출 후 앞으로 이동 후 검출된 색에 따라 드론 동작 (Rotate 90 / Landing)
if (BlueSum < Dmode[0][0]):
    #링 내부로 들어왔다는 뜻
    print(f"inside ring / BlueSum : {BlueSum}")
    
    imgH_R = cv2.inRange(imghsv, lower_red, upper_red)
    imgH_P = cv2.inRange(imghsv, lower_purple, upper_purple)
    tmpR = cv2.medianBlur(imgH_R, 7) # 좌회전을 판단할 점 요소
    tmpP = cv2.medianBlur(imgH_P, 7)
    RedSum = np.sum(tmpR == 255, axis = None)
    PurpleSum = np.sum(tmpP == 255, axis = None)

    if (RedSum != 0 and level_cnt != 3):
        if (RedSum < 200):
            print(f"detect red / BlueSum : {BlueSum}")
            drone.sendControlPosition16(4, 0, 0, 4, 0, 0)
            sleep(4)
            drone.sendControlPosition16(0, 0, 0, 0, 90, 20)
            sleep(6)
            level_cnt += 1
            print("red, rotate complete, now level_cnt:", level_cnt)
            Dmode = mode[level_cnt]
            no_trap = True
            drone.sendControlPosition16(10,0,0,5,0,0)
            sleep(5)
            continue

        else:
            print(f"detect red / BlueSum : {BlueSum}")
            drone.sendControlPosition16(3, 0, 0, 3, 0, 0)
            sleep(4)
            drone.sendControlPosition16(0, 0, 0, 0, 90, 20)
            sleep(6)
            level_cnt += 1
            print("red, rotate complete, now level_cnt:", level_cnt)
            Dmode = mode[level_cnt]
            no_trap = True
            drone.sendControlPosition16(10,0,0,5,0,0)
            sleep(5)
            continue

        
    elif (RedSum == 0 and PurpleSum != 0 or level_cnt == 3):
        print("purple")
        drone.sendLanding()
        drone.close()
        break



    else:
        #둘다 노이즈로 판명난 경우엔 다시 검출을 합니다
        continue

2-f) 링 외부일 경우

  • 링 외부일 경우 링과의 거리에 따라 직선 이동 함수 실행
if (BlueSum < Dmode[0][0]):...
else:
    # 링 외부에 있다는 뜻
    print(f"out of ring / BlueSum : {BlueSum}")

    (dist_x, dist_y) = detectDist(tmpB)
    
    if (BlueSum in Dmode[0]):               
	#링 경계   
        if (BlueSum - lastBluesum >= 0 and no_trap):
            #현재 - 과거 픽셀이며, 양수면 매우 멀리있는 경우이므로 moveLarge 모드로 더 다가가야 함
            moveLarge(drone, BlueSum)

        else:
            #음수면 링과 매우 근접한 경우이므로, 조금만 움직여야 함.
            move2Small(drone, BlueSum, dist_x, dist_y)
        lastBluesum = BlueSum
        continue
    
    elif (BlueSum in Dmode[1]):
        if (BlueSum - lastBluesum >= 0):
            moveSoso(drone, BlueSum)
            no_trap = False
        else:
            moveSoso(drone, BlueSum)
            no_trap = False

        lastBluesum = BlueSum
        continue

    
    elif (BlueSum in Dmode[2]):
        if (BlueSum - lastBluesum >= 0):
            moveSmall(drone, BlueSum, dist_x, dist_y)
            no_trap = False
        else:
            move2Small(drone, BlueSum, dist_x, dist_y)
            no_trap = False
        
        lastBluesum = BlueSum
        continue


    elif (BlueSum >= Dmode[3]):
        moveSmall(drone, BlueSum, dist_x, dist_y)
        lastBluesum = BlueSum
        continue

    else:
        continue

III-3. 최종 코드

  • 최종 코드 main.py를 Raspberry Pi에서 실행
from e_drone.drone import *
from picamera.array import PiRGBArray
from picamera import PiCamera
from time import sleep
import numpy as np
import cv2



lower_blue = (95, 0, 50)  
upper_blue = (110, 255, 250)

lower_red = (0, 0, 5)  
upper_red = (17, 255, 240)

lower_purple = (110, 0, 5)
upper_purple = (140, 255, 50)


level_cnt = 1
no_trap = True

mode = {1:[range(100, 30000), range(30000, 100000), range(100000, 150000), 150000], 
        2:[range(100, 50000), range(50000, 120000), range(120000, 170000), 170000], 
        3:[range(100, 70000), range(70000, 140000), range(140000, 200000), 200000]}  # [min,max]

lastBluesum = 0

def moveLarge(drone, BlueSum):
    #이동 11
    print(f"move Large / BlueSum : {BlueSum}")            
    drone.sendControlPosition16(11, 0, 0, 6, 0, 0)
    sleep(4)
        

def moveSoso(drone, BlueSum):
    #이동 8
    print(f"move soso / BlueSum : {BlueSum}")
    drone.sendControlPosition16(8, 0, 0, 5, 0, 0)
    sleep(4)
   
                    
def moveSmall(drone, BlueSum, dist_x, dist_y):
    #이동 6
    print(f"move small / BlueSum : {BlueSum}")
    if (dist_x == 0 and dist_y == 0):
        drone.sendControlPosition16(6, 0, 0, 4, 0, 0)
        sleep(3)
                
    elif (dist_x != 0 and dist_y == 0):
        drone.sendControlPosition16(0, dist_x//abs(dist_x), 0, 1, 0, 0)
        sleep(3)
        drone.sendControlPosition16(6, 0, 0, 4, 0, 0)
        sleep(3)
    
    elif (dist_x == 0 and dist_y != 0):
        drone.sendControlPosition16(0, 0, 2*(dist_y//abs(dist_y)), 2, 0, 0)
        sleep(3)
        drone.sendControlPosition16(6, 0, 0, 4, 0, 0)
        sleep(3)
                    
    elif (dist_x != 0 and dist_y != 0):
        drone.sendControlPosition16(0, dist_x//abs(dist_x), 0, 1, 0, 0)
        sleep(3)
        drone.sendControlPosition16(0, 0, 2*(dist_y//abs(dist_y)), 2, 0, 0)
        sleep(3)
        drone.sendControlPosition16(6, 0, 0, 4, 0, 0)
        sleep(3)


def move2Small(drone, BlueSum, dist_x, dist_y):
    #속도 4
    print(f"move 2 Small / BlueSum : {BlueSum}")
    if (dist_x == 0 and dist_y == 0):
        drone.sendControlPosition16(4, 0, 0, 3, 0, 0)
        sleep(3)
                
    elif (dist_x != 0 and dist_y == 0):
        drone.sendControlPosition16(0, dist_x//abs(dist_x), 0, 1, 0, 0)
        sleep(3)
        drone.sendControlPosition16(4, 0, 0, 3, 0, 0)
        sleep(3)
    
    elif (dist_x == 0 and dist_y != 0):
        drone.sendControlPosition16(0, 0, 2*(dist_y//abs(dist_y)), 2, 0, 0)
        sleep(3)
        drone.sendControlPosition16(4, 0, 0, 3, 0, 0)
        sleep(3)
                    
    elif (dist_x != 0 and dist_y != 0):
        drone.sendControlPosition16(0, dist_x//abs(dist_x), 0, 1, 0, 0)
        sleep(3)
        drone.sendControlPosition16(0, 0, 2*(dist_y//abs(dist_y)), 2, 0, 0)
        sleep(3)
        drone.sendControlPosition16(4, 0, 0, 3, 0, 0)
        sleep(3)

def detectDist(tmpB):
    tmpB_div_1 = tmpB[:240, :320]
    tmpB_div_2 = tmpB[240:, :320]
    tmpB_div_3 = tmpB[:240, 320:]
    tmpB_div_4 = tmpB[240:, 320:]

    tmpB_div_1 = np.sum(tmpB_div_1 == 255, axis = None)
    tmpB_div_2 = np.sum(tmpB_div_2 == 255, axis = None)
    tmpB_div_3 = np.sum(tmpB_div_3 == 255, axis = None)
    tmpB_div_4 = np.sum(tmpB_div_4 == 255, axis = None)
    
    dist_x = (tmpB_div_3 + tmpB_div_4) - (tmpB_div_1 +tmpB_div_2)
    dist_y = (tmpB_div_4 + tmpB_div_2) - (tmpB_div_3 +tmpB_div_1)
    
    return (dist_x, dist_y)


drone = Drone()
drone.open()

try:
    drone.sendTakeOff()
    sleep(5)
    drone.sendControlPosition16(11,0,0,5,0,0)
    sleep(5) 

    camera = PiCamera() 
    camera.resolution = (640, 480) 
    camera.framerate = 32 
    rawCapture = PiRGBArray(camera, size=(640, 480))
    Dmode = mode[level_cnt] 

    for frame in camera.capture_continuous(rawCapture, format='bgr', use_video_port=True):
        img = frame.array
        img = cv2.flip(img, 0)
        img = cv2.flip(img, 1)
        imghsv = cv2.cvtColor(img, cv2.COLOR_BGR2HSV)

        imgH_B = cv2.inRange(imghsv, lower_blue, upper_blue)

        rawCapture.truncate(0)

        tmpB = cv2.medianBlur(imgH_B, 21) # 파란색 링에 medianBlur 적용한 이미지
        
        BlueSum = np.sum(tmpB == 255, axis = None) # 파란색링의 이미지
        print(f"first BlueSum : {BlueSum}")
        
        if (BlueSum < Dmode[0][0]):
            #링 내부로 들어왔다는 뜻
            print(f"inside ring / BlueSum : {BlueSum}")
            
            imgH_R = cv2.inRange(imghsv, lower_red, upper_red)
            imgH_P = cv2.inRange(imghsv, lower_purple, upper_purple)
            tmpR = cv2.medianBlur(imgH_R, 7) # 좌회전을 판단할 점 요소
            tmpP = cv2.medianBlur(imgH_P, 7)
            RedSum = np.sum(tmpR == 255, axis = None)
            PurpleSum = np.sum(tmpP == 255, axis = None)


            if (RedSum != 0 and level_cnt != 3):
                #일단 노이즈 없이 검출 되는 경우
                if (RedSum < 200):
                    print(f"detect red / BlueSum : {BlueSum}")
                    drone.sendControlPosition16(4, 0, 0, 4, 0, 0)
                    sleep(4)
                    drone.sendControlPosition16(0, 0, 0, 0, 90, 20)
                    sleep(6)
                    level_cnt += 1
                    print("red, rotate complete, now level_cnt:", level_cnt)
                    Dmode = mode[level_cnt]
                    no_trap = True
                    drone.sendControlPosition16(10,0,0,5,0,0)
                    sleep(5)
                    continue

                else:
                    print(f"detect red / BlueSum : {BlueSum}")
                    drone.sendControlPosition16(3, 0, 0, 3, 0, 0)
                    sleep(4)
                    drone.sendControlPosition16(0, 0, 0, 0, 90, 20)
                    sleep(6)
                    level_cnt += 1
                    print("red, rotate complete, now level_cnt:", level_cnt)
                    Dmode = mode[level_cnt]
                    no_trap = True
                    drone.sendControlPosition16(10,0,0,5,0,0)
                    sleep(5)
                    continue

                
            elif (RedSum == 0 and PurpleSum != 0 or level_cnt == 3):
                print("purple")
                drone.sendLanding()
                drone.close()
                break



            else:
                #둘다 노이즈로 판명난 경우엔 다시 검출을 합니다
                continue  

        else:
            # 링 외부에 있다는 뜻
            print(f"out of ring / BlueSum : {BlueSum}")

            (dist_x, dist_y) = detectDist(tmpB)
            
            if (BlueSum in Dmode[0]):               
                #링 경계   
                if (BlueSum - lastBluesum >= 0 and no_trap):
                    #현재 - 과거 픽셀이며, 양수면 매우 멀리있는 경우이므로 moveLarge 모드로 더 다가가야 함
                    moveLarge(drone, BlueSum)

                else:
                    #음수면 링과 매우 근접한 경우이므로, 조금만 움직여야 함.
                    move2Small(drone, BlueSum, dist_x, dist_y)
                lastBluesum = BlueSum
                continue
            
            elif (BlueSum in Dmode[1]):
                if (BlueSum - lastBluesum >= 0):
                    moveSoso(drone, BlueSum)
                    no_trap = False
                else:
                    moveSoso(drone, BlueSum)
                    no_trap = False

                lastBluesum = BlueSum
                continue

            
            elif (BlueSum in Dmode[2]):
                if (BlueSum - lastBluesum >= 0):
                    moveSmall(drone, BlueSum, dist_x, dist_y)
                    no_trap = False
                else:
                    move2Small(drone, BlueSum, dist_x, dist_y)
                    no_trap = False
                
                lastBluesum = BlueSum
                continue


            elif (BlueSum >= Dmode[3]):
                moveSmall(drone, BlueSum, dist_x, dist_y)
                lastBluesum = BlueSum
                continue

            else:
                continue

except Exception as e:
    print("exception")
    drone.sendLanding()
    drone.close()
TIL-44
Generics : 제너릭

좋은 웹페이지 즐겨찾기

개발자 우수 사이트 수집

개발자가 알아야 할 필수 사이트 100선 추천 우리는 당신을 위해 100개의 자주 사용하는 개발자 학습 사이트를 정리했습니다
best100-homepage