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

션·2022년 2월 5일

2021 미니드론 자율주행

2021 미니드론 자율주행 경진대회

목록 보기

1/1

나갔던 대회 자료 정리하기

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

B 리그 Team Himdrong

I. 대회 진행 전략

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

Python 설치
다운로드
VSCode 설치
다운로드
Python VSCode Extension 설치
다운로드

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 프로그램

Python 편집

VSCode
Raspberry Pi 파일 전송

Win SCP
Raspberry Pi 통신

VNC viewer

I-5. 드론 작동법

Python 코드를 이용하여 드론 제어

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

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


def sendControlPosition(self, positionX, positionY, positionZ, velocity, heading, rotationalVelocity):

변수 이름	형식	범위	단위	설명
position X	Int16	-100 ~ 100(-10.0 ~ 10.0)	meter x 10	앞(+), 뒤(-)
position Y	Int16	-100 ~ 100(-10.0 ~ 10.0)	meter x 10	좌(+), 우(-)
position Z	Int16	-100 ~ 100(-10.0 ~ 10.0)	meter x 10	위(+), 아래(-)
velocity	Int16	5~200(0.5 ~ 2.0)	m/s x 10	위치 이동 속도
heading	Int16	-360 ~ 360	degree	좌회전(+), 우회전(-)
rotationalVelocity	Int16	10 ~ 360	degree/s	좌우 회전속도

I-6. 트랙 제작

대회 트랙 규격
사용 재료
크로마키 천 x 3
크로마키 거치대 x 6
거치대 크로스바 x 3
트랙 제작

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

II. 알고리즘

II-1. 트랙 분석

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

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()

션

._.

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

2021 미니드론 자율주행 경진대회

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

B 리그 Team Himdrong

목차

I. 대회 진행 전략

II. 알고리즘

III. 사용 코드

I. 대회 진행 전략

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

I-2. 하드웨어

I-3. Raspberry Pi OS 설치

1. Raspberry Pi OS Lite 다운로드

2. Raspberry Pi imager 설치

3. SD카드에 Raspberry Pi OS 설치

I-4. 사용 PC 프로그램

Python 편집

Raspberry Pi 파일 전송

Raspberry Pi 통신

I-5. 드론 작동법

I-6. 트랙 제작

II. 알고리즘

II-1. 트랙 분석

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

II-3. 알고리즘 종합

III. 사용 코드

대회 사용 모듈

코드

III-1. 이미지 처리

1-a) 원본 이미지

1-b) Threshold 값 설정

1-c) HSV 변환

1-d) Threshold 값 적용

1-e) MedianBlur 적용

III-2. 드론 제어

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

2-b) 직선 이동

2-c) 링 근접

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

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

2-f) 링 외부일 경우

III-3. 최종 코드

0개의 댓글

관련 채용 정보