아두이노로 만드는 4족보행로봇 - amazon s3 · 4. 로봇시스템 구현 ----- 44 5....

아두이노로 만드는 4족보행로봇

목 차

Ⅰ. 아두이노 및 C프로그래밍기초 ------------------------------------------------------- 1

1. 아두이노 소개 ------------------------------------------------------------------------- 1

2. 아두이노 개발환경 구축 -------------------------------------------------------------- 3

3. C 프로그래밍 기초 -------------------------------------------------------------------- 8

4. 실습 #1 – 디지털 출력(LED제어) --------------------------------------------------- 12

5. 실습 #2 - 디지털 입력(스위치 입력) ----------------------------------------------- 14

6. 실습 #3 – 디스플레이(7세그먼트) --------------------------------------------------- 16

7. 실습 #4 – 아날로그 입력(조이스틱 모듈) ------------------------------------------ 19

8. 실습 #5 – 센서 ---------------------------------------------------------------------- 21

9. 실습 #6 – 통신/ 블루투스 ---------------------------------------------------------- 23

10. 실습 #7 – 모터 --------------------------------------------------------------------- 30

11. 실습 #8 – LCD 모듈 ------------------------------------------------------------- 31

Ⅱ. 보행로봇 시스템 구현 ------------------------------------------------------- 33

1. 보행로봇의 개요 ---------------------------------------------------------------------- 33

2. 로봇 기구해석 ------------------------------------------------------------------------ 36

3. 보행궤적 생성 ------------------------------------------------------------------------ 38

4. 로봇시스템 구현 ---------------------------------------------------------------------- 44

5. Trot 보행 구현 프로그래밍----------------------------------------------------------- 47

6. Trot 보행 구현 프로그래밍(블루투스 원격조종)------------------------------------- 55

Ⅰ. 아두이노 및 C프로그래밍 기초

- 1 -

1. 아두이노(Arduino) 소개

▪ 이탈리아에서 디자인 수업의 목적으로 손쉽게 사용할 수 있도록 개발된 오픈소스형 소형컴퓨터 ▪ Atmel 社의 마이크로컨트롤러를 기반으로 구성

▪ 아두이노(Arduino) = 마이크로컨트롤러 보드 + 프로그램 개발환경 +　인터넷 공개포럼

▪ 공식 포럼 : http://www.arduino.cc


- 2 -

▪ 아두이노 Mega 2560 Overview

- Atmel社의 ATmega2560 마이크로컨트롤러 기반의 아두이노 보드

- 주요 사양

마이크로 컨트롤러 ATmega2560

동작전압 5V

입력전압(허용) 7 - 12V

입력전압(한계) 6 - 20V

디지털 I/O 핀 54개（15개는 PWM 출력기능)

아날로그 입력 핀 16개

핀당 DC 전류 20 mA

3.3V 핀 전류 50 mA

Flash Memory 256 KB (8KB는 부트로더)

SRAM 8 KB

EEPROM 4 KB

클럭속도 16 MHZ

크기 101.52(L) * 53.3(W)

무게 37g


- 3 -

2. 아두이노 개발환경 구축

▪ 통합개발환경(Integration Development Environment) 이란?마이크로컨트롤러 시스템 개발에 필요한 하드웨어 연결 및 설정, 프로그래밍 작성, 다운로드 및 디버깅등의 작업을 손쉽게 할 수 있도록, 관련 기능이 통합되어 있는 소프트웨어 개발 도구 또는 환경.

▪ 통합개발환경 소프트웨어 설치?- 공식포럼(http://www.arduino.cc) 에서 IDE 소프트웨어를 다운로드.


- 4 -

- 다운로드 받은 파일을 실행하고, 프로그램 설치를 진행.


- 5 -

▪ 보드 연결 및 설정

- 아두이노 메가보드를 PC에 연결

- 아두이노 IDE 실행


- 6 -

- 보드 종류 및 포트 설정

※ 아두이노 보드를 다른 PC에서 연결해제 한 뒤 다시 연결할 때 포트 번호나 보드정보가 바뀔 수 있으므로, 프로그램 컴파일이나 보드 업로딩이 안 뒬 경우, 반드시 확인해야함.


- 7 -

▪ 기본 프로그램 작성 및 컴파일, 업로드- 아두이노 IDE 실행 후, 아래 예제를 입력함.

// 아두이노 처음 시작하기 : 시리얼통신으로 메시지 받기

void setup()

{

Serial.begin(9600); // 아두이노와 통신시작

}

void loop()

{

Serial.println("Hello Everyone!!"); // 메시지 출력

delay(1000); // 1초 기다리기

}

- “컴파일” 버튼 클릭 ⇨ “업로드” 버튼 클릭

- “시리얼 모니터” 버튼 클릭 ⇨ 출력 결과 확인


- 8 -

3. C프로그래밍 기초

▪ C언어는?

1972년, 벨 연구소(Bell Lab.) 데니스 리치가 개발한 프로그래밍 언어로서, UNIX OS를 운용하기 위해 사용되었다가 점차적으로 널리 사용되어 현재까지 사용됨

▪ C언어의 특징

- 문법이 간결하고, 명백함 - 구조화가 잘 되어있어, 광범위한 확장성을 지님 - 하드웨어 밀착 제어가 가능하면서도, 사용자 편리성이 우수함

▪ C언어 학습의 필요성

현재 ICT 기술의 발전에 힘입어, 컴퓨터 프로그래밍 기술이 하드웨어에 성능에 대한 제약을 거의 받지 않게 되면서, 사용자 편리성이 우수한 고급형 프로그래밍 언어(Java, C#, Python, VB 등)의 사용이 증대되고 있다.

프로그래밍 언어는 적용되는 시스템 환경과 상황에 맞도록 선택되어야 하며, “특정 프로그래밍 언어가 가장 좋다.” 라는 기준은 없다.

현재 기계공학 분야에서 주로 다루는 하드웨어 및 장비시스템은 아직까지 주로 C프로그래밍을 기반으로 개발되어 진다.

C언어는 하드웨어 성능이 뛰어나지 않은 시스템으로부터 고성능 시스템에 이르기까지 적용될 수 있도록, 컴파일러 환경이 다양하게 구축되어 있으며, 시스템의 성능을 최대한 활용 할 수 있도록 직관적인 문법 구조로 이루어져 있다.

C프로그래밍은 타 프로그래밍언어가 갖는 기본 특성을 모두 담고있어, 프로그래밍 습득에 필요한 선수학습으로서 매우 가치가 있는 프로그래밍 언어이다.

▪ 프로그래밍 문법이란?

사람의 언어와 동일하게, 기본적인 표현방법과 규칙을 명시한 것. 컴퓨터는 문법이 약간이라도 틀리면, 동작이 아예 수행되지 않으므로, 기본적인 프로그래밍 문법을 이해하는 것이 가장 중요하다.


- 9 -

▪ 데이터 :　상수, 변수- 상수 : 프로그램이 실행되기 전에 미리 선언되어, 프로그램 실행중 값이 바뀌지 않는 데이터- 변수 : 프로그램이 실행되기전 데이터형을 미리 선언한후, 프로그램 실행중 필요에 따라 변할 수 있는 데이터(또는 메모리 공간)- 데이터형 : 변수 저장되는 데이터의 종류(정수, 실수, 문자 등)

유 형 크기 (바이트) 범 위

int 2 -32768 ~ 32767

unsigned int 2 0 ~ 65535

long 4 -2147483648 ~ -2147483647

unsigned long 4 4294967295

float 4 3.4028235E+38 ~ -3.4028235E+38

double 4 3.4028235E+38 ~ -3.4028235E+38

boolean 1 true(1) 또는 False(0)

char 1 -128 ~ 127

byte 1 0 ~ 255

※ 데이터형별 크기는 아두이노 기준임

▪ 연산자 : 데이터의 처리를 위해 사용되는 산술, 논리연산을 수행하는 기능을 가진 기호유 형 기 호

산술 연산자 + , - , * , / , %, ++, --

대입 연산자 =, +=, -=, *=, /= , %=

관계 연산자 > , >= , < , <= , ==, !=

논리 연산자 && , || , !

비트연산자 & , | , ^ , ~ , << , >>


- 10 -

▪ 제어문 : 프로그램의 실행흐름을 조건에 따라 여러 가지로 나누거나 바꾸는 기능유 형 형 식 사용예

if 문

if(조건식1)

{

수행문1;

}

else if(조건식2)

{

수행문2;

}

else

{

수행문3;

}

int x = 5;

if(x >10)

{

printf(“x는 10보다 크다”);

}

else if(x >= 5)

{

printf(“x는 5보다 크거나 같다”);

}

else

{

printf(“x는 5보다 작다”);

}

for 문

for(초기값 ; 조건식 ; 증감치)

{

반복수행문;

}

for(int n=1 ; n<=10 ; n++)

{

// 1부터 10숫자 출력

printf(“%d, n);

}

while 문

while(반복조건식)

{

수행문;

}

int n=1;

while(n <= 10)

{

// 1부터 10숫자 출력

printf(“%d, n);

n++;

}

기타

break, continue, return,

do ~ while

switch　~ case

goto

-

▪ 함수(Function) : 프로그램에서 반복 수행되는 부분(또는 기능)을 별도의 기능으로 묶어둔 것▪ 포인터(Pointer) : 데이터 또는 함수가 실행되는 메모리 주소값(Memory Address)▪ 배열(Array) : 동일한 형태의 변수 또는 데이터를 한꺼번에 지정한 것▪ 구조체(Structure) : 서로 다른 데이터형을 하나의 개념체로 묶어서 다루는 것


- 11 -

▪ C프로그래밍 기본예제- 홀/짝수 판별

// 변수에 저장된 숫자가 짝수인지 홀수인지 판단 (변수와 상수, if문)

int number = 1;

int result = 0;

void setup()

{

Serial.begin(9600); // 아두이노와 통신시작

}

void loop()

{

result = number % 2; // number를 2로 나눈 나머지를 result에 저장(0 또는 1)

if(result == 0)

{

Serial.println("EVEN"); // 나머지가 0이면 짝수

}

else

{

Serial.println("ODD"); // 나머지가 1이면 홀수

}

while(1);

}

- 숫자카운트

// 숫자카운트하기 (for문)

void setup()

{

Serial.begin(9600);

}

void loop()

{

for(int n=0 ; n<=10 ; n++)

{

Serial.print("Number : "); // 메시지 출력

Serial.println(n); // 숫자출력

delay(1000);

}

while(1);

}


- 12 -

4. 실습 #1 – 디지털 출력(LED제어)

▪ LED 1개 On/Off 제어

-LED 회로연결

-아두이노 프로그램 작성// LED Blink 기본 예제

void setup()

{

// 13번핀 출력 초기화

pinMode(13, OUTPUT);

}

void loop()

{

digitalWrite(13, HIGH); // LED 켜기


digitalWrite(13, LOW); // LED 끄기


}


- 13 -

▪ LED 8개 On/Off 제어

-LED 회로연결

-아두이노 프로그램 작성// LED 순차적으로 켜고 끄기

void setup()

{

// 출력핀 초기화

for(int pin=6 ; pin <= 13 ; pin++)

{

pinMode(pin, OUTPUT);

}

}

void loop()

{

for(int pin=6 ; pin <= 13 ; pin++)

{

digitalWrite(pin, HIGH);

delay(500);

digitalWrite(pin, LOW);

}

}


- 14 -

5. 실습 #2 – 디지털 입력(스위치 입력)

▪ 스위치 1개 연결

-스위치 회로연결(실습#1에 추가 연결)

-아두이노 프로그램 작성// 버튼 입력할때 LED 켜지기

int buttonPin = 5; // 입력핀 번호

int ledPin = 13; // 출력핀번호

int buttonState = 0; // 버튼상태 저장

void setup()

{

pinMode(ledPin, OUTPUT); // 출력핀

pinMode(buttonPin, INPUT); // 입력핀모드

}

void loop()

{

buttonState = digitalRead(buttonPin); // 버튼 입력

// 버튼상태 확인

if(buttonState == LOW)

{

digitalWrite(ledPin, HIGH); // LED ON

}

else

{

digitalWrite(ledPin, LOW); // LED OFF

}

}


- 15 -

※ QUEST

1. 실습 #2의 회로를 이용해서, LED 동작을 여러 가지로 바꿔보시오.

2. 실습 #2의 회로를 이용해서, 스위치를 누를때마다 LED 점등위치가 바뀌도록

프로그램을 작성해보시오.


- 16 -

6. 실습 #3 – 디스플레이(7세그먼트)

▪ 7세그먼트(FND – Flexible Numerical Display) 란?

- LED를 점등을 이용해서 숫자(0 ~ F) 및 간단한 기호를 표현할 수 있는 디스플레이 장치- 공통단자의 극성에 따라서 캐소드(K)형 또는 에노드(A)형으로 구분됨.


- 17 -

▪ 7세그먼트 1개 연결

-7세그먼트 회로연결

※ 7세그먼트의 a~h 단자를 디지털핀 6~13에 순서대로 연결

-아두이노 프로그램 작성// for문을 이용한 FND 출력예제

int ledPin[8] = {6, 7, 8, 9, 10, 11, 12, 13};

byte numTable[10] = { B00111111, // 0

B00000110, // 1

B01011011, // 2

B01001111, // 3

B01100110, // 4

B01101101, // 5

B01111101, // 6

B00000111, // 7

B01111111, // 8

B01100111 // 9

};

boolean segment = false;

☞ 다음장에 계속...


- 18 -

void setup()

{

for(int i=0 ; i < 8 ; i++)

{

pinMode(ledPin[i], OUTPUT);

}

}

void loop()

{

// 숫자증가 : 0 ~ 9

for(int Num=0 ; Num<=9 ; Num++)

{

// 해당숫자를 FND에 표시

for(int i=0 ; i<8 ; i++)

{

// 해당비트를 추출

segment = ( ( B00000001 << i ) & numTable[Num] ) >> i;

digitalWrite(ledPin[i], segment);

}

delay(1000);

}

}

※ QUEST

1. 실습 #3의 회로를 이용해서, 7세그먼트의 동작을 바꿔보시오.

2. 실습 #3을 기초로, 여러 개의 7세그먼트를 연결해서, 다양한 숫자값을 표현하시

오.


- 19 -

7. 실습 #4 – 아날로그 입력(조이스틱 모듈)

▪ 아두이노 보드 아날로그 입력단자 및 사양

▪ 아두이노 보드 아날로그 출력단자 및 사양

▪ PWM(Pulse Width Modulation, 펄스폭 변조) 이란? - 빠르게 ON/OFF 되는 신호의 ON시간의 비율(Duty Ratio)를 조정하여, 아날로그 출력과 유사한 출력효과를 만드는 것.


- 20 -

▪ 아날로그 조이스틱 입력

- 조이스틱 모듈 연결

- 아두이노 프로그램 작성

// 아날로그 입력값을 전압으로 표현하기

void setup()

{

Serial.begin(9600); // 시리얼모니터 초기화

}

void loop()

{

int sensorValue = analogRead(A0); // 아날로그 A0 입력받기

long voltage = map(sensorValue, 0, 1023, 0, 500);

Serial.print("Digital :");

Serial.print(sensorValue); // 디지털값 출력

Serial.print(" Voltage :");

Serial.println((float)voltage/100); // 전압값 출력

delay(500); // 시간딜레이 0.5초

}

- 프로그램 업로드 ⇨ 시리얼 모니터 실행 ⇨ 조이스틱 모듈 조작, 값 변화 확인


- 21 -

8. 실습 #5 – 센서

▪ 조도 센서

- 센서 결선


// CDS 센서와 LED

int ledPin = 13;

int sensorPin = 0;

void setup()

{

pinMode(ledPin, OUTPUT);

}

void loop()

{

int sensor = analogRead(sensorPin);

digitalWrite(ledPin, HIGH);

delay(sensor);

digitalWrite(ledPin, LOW);

delay(sensor);

}


- 22 -

▪ 온습도센서 모듈 - 센서 결선


#include <DHT.h>

DHT dht(2, DHT11); // 온습도 센서모듈 설정

void setup(){ Serial.begin(9600); Serial.println("DHT11 measure start"); dht.begin(); // 온습도 센서모듈 시작}

void loop(){ float humiValue = dht.readHumidity(); float tempValue = dht.readTemperature();

if( isnan(humiValue) || isnan(tempValue) ) { Serial.println("Failed"); return; }

Serial.print("Humidity:"); Serial.print(humiValue); Serial.println("%RH"); Serial.print("Temperature:"); Serial.print(tempValue); Serial.println("C"); Serial.println();

delay(2000);}


- 23 -

9. 실습 #6 – 통신 / 블루투스

▪ 블루투스 모듈(HC-06) 결선

▪ 블루투스 모듈(HC-06) 설정

- 아두이노 프로그램 작성#include <SoftwareSerial.h>

SoftwareSerial BT(12, 13);

void setup()

{

Serial.begin(9600);

BT.begin(9600);

}

void loop()

{

if (BT.available()) Serial.write(BT.read());

if (Serial.available()) BT.write(Serial.read());

}


- 24 -

- 블루투스 이름, PIN 설정

⇩

⇩


- 25 -

⇩

⇩


- 26 -

⇩

⇩


- 27 -

⇩


- 28 -

▪ 블루투스로 원격 LED 오/오프 제어- 아두이노 프로그램 작성

#include <SoftwareSerial.h>

int ledPin = 13;

SoftwareSerial BT(2, 3);

void setup()

{

BT.begin(9600);

pinMode(ledPin, OUTPUT);

}

void loop()

{

if(BT.available())

{

char a = (char)BT.read();

if(a == '1') digitalWrite(ledPin, HIGH);

else if(a == '0') digitalWrite(ledPin, LOW);

}

}

- 원격 조정테스트(채팅앱 이용)


- 29 -

- 원격 조정테스트(컨트롤러앱 이용)


- 30 -

10. 실습 #7 – 모터

▪ 서보 모터란? 모터와 모터의 위치측정센서, 주변제어부를 포함하여 모터의 위치, 속도를 제어할 수 있도록 구성된 제어시스템을 서보제어시스템 이라고 하며, 이때 사용되는 모터를 통상, 서보모터라고 칭한다.

▪ 서보 모터 구동

- 모터 결선(실습#4에 추가 연결)

- 아두이노 프로그램 작성#include <Servo.h>

Servo myservo;

void setup()

{

myservo.attach(12);

myservo.write(0);

delay(1000);

}

void loop()

{

int sensorValue = analogRead(A0);

int angle = map(sensorValue, 0, 1023, 0, 180);

myservo.write(angle);

delay(50);

}


- 31 -

11. 실습 #8 – LCD모듈

▪ LCD ( Liquid Crystal Display) - 디지털 기기의 기본적인 디스플레이 장치로 널리 사용됨 - 지정된 문자(숫자)만 표시할 수 있는 텍스트 LCD와 문자 및 그림 출력이 가능한 그래픽 LCD로 구분됨.

▪ LCD 구동

- LCD 결선 (I2C 통신 방식)


#include <Wire.h> // I2C 통신용

#include <LiquidCrystal_I2C.h> // LCD제어용

LiquidCrystal_I2C lcd(0x3F, 16, 2); // LCD 초기세팅 (0x3F)

void setup()

{

lcd.init(); // LCD 초기화

lcd.backlight(); // LCD 백라이트 ON

lcd.setCursor(0,1); // LCD 커서위치

Serial.begin(9600);

}

☞ 다음장에 계속...


- 32 -

void loop()

{

// 시리얼 통신으로 전송된 데이터가 있으면 데이터 표시함.

if (Serial.available())

{

delay(100);

lcd.clear();

while (Serial.available() > 0)

{

lcd.write(Serial.read());

}

}

}

- 시리얼 모니터 입력창에 메시지 입력후 LCD에 표시되는지 확인

Hello World!

- LCD 글자 명암 조정(글자가 안보이거나, 화면이 검게 나오는 경우)

Ⅱ. 보행로봇 시스템 구현

- 33 -

1. 보행로봇의 개요

▪ 보행로봇 이란? - 인간이나 동물의 다리(관절)구조를 지니면서 이와 유사한 보행방법을 모방한 이동방식을 사용하는 로봇시스템

▪ 보행로봇 연구의 배경 및 필요성 - 바퀴 또는 궤도형태의 운송수단이 접근할 수 없는 지형에서의 운송수단 - 다양한 환경(험지, 우주, 해저)에서의 활용


- 34 -

▪ 4족 보행로봇 - 4개의 다리구조를 지닌 보행로봇 - 다양한 형태의 보행로봇중, 적절한 동작 안정성 및 시스템 복잡도, 기술난이도임 - 실제 동물의 경우 역시, 빠른 속도, 안정성, 적응성을 가진 동물은 4족 동물임

▪ 견마형 4족 보행로봇 - 4족 보행로봇은 관절의 형태에 따라 Insect Type(Reptile Type : 파충류형)과 Mammal Type(포유류)으로 분류됨. - Insect Type은 무릎관절이 Hip 관절의 높이와 같거나 위에 있음. 다리는 몸을 기준으로 옆으로 벌어지는 형태를 띄고 실제 곤충과 같은 방법으로 보행을 함. - Mammal Type은 무릎관절이 Hip 관절의 아래에 위치하므로 다리가 앞뒤로 움직임. - Insect Type은 느리지만 안정적인 보행, Mammal Type은 빠르고 동적인 보행을 보임.

Insect Type Mammal Type


- 35 -

▪ 견마형 4족의 보행패턴

- Work : 네발이 각각 1개씩 앞으로 내딛어져서 이동하는 보행패턴을 말한다. Walk는 4족 동물의 이동방법중 가장 안정적인 보행방법이다. 한 개의 다리가 공중에서 앞으로 내딛어질 때, 나머지 3다리는 몸체를 지지하며 지면을 앞으로 밀어내어 몸체를 이동시킨다. 이 때 로봇(동물)이 넘어지지 않기 위해서는 지지하는 세발의 지지점을 이은 삼각형 내에 로봇의 무게중심이 위치해야 한다.

Walk

- Trot : 대각선에 위치한 발이 한쌍이 되어 움직이는 보행방법이다. 대각의 한쌍의 발이 몸체를 지지하며 앞으로 이동시키는 동안 다른 발들을 앞으로 내딛게 된다. 따라서 Trot은 Walk에 비해 이동속도는 빠르나, 안정성은 낮다.

Trot

- Gallop : 앞발과 뒷발이 각각 쌍을 이루어 보행하는 방법으로, 보행중 4발이 전부 공중에 있는 Flight Phase가 존재하게 된다. 발이 땅에 닿을 때에는 쌍을 이루는 두 발중 왼쪽발과 오른발이 약간의 시간차이를 두고 땅에 닿게 되며, 시간차이가 없는 것을 Bound라고 한다. 또한 발의 지면접촉순서에 따라 Transverse Gallop과 Rotary Gallop으로 구분 지을 수 있으며, 한 보행주기내에 Flight Phase가 2회일 경우인 Fast Gallop과 1회인 경우인 Slow Gallop으로도 구분 지을 수 있다.

Gallop Bound


- 36 -

2. 로봇 기구해석

▪ 기구부 개요

▪ 좌표계 설정


- 37 -

▪ DH 파라미터(Craig)joint(i) αi-1 ai-1 di θi θ0

1 0 H/2 0 90 02 90 W/2 0 -θ1 -903 0 0 -Bw 0 04 90 Bh -L1 θ2 05 0 L2 0 θ3 0

joint(i) αi-1 ai-1 di θi θ0

1 0 H/2 0 90 02 90 -W/2 0 -θ1 -903 0 0 -Bw 0 04 90 Bh -L1 θ2 05 0 L2 0 θ3 0

Left Front Leg Right Front Leg


1 0 -H/2 0 90 02 90 W/2 0 -θ1 -903 0 0 -Bw 0 04 90 Bh -L1 θ2 05 0 L2 0 θ3 0


1 0 -H/2 0 90 02 90 -W/2 0 -θ1 -903 0 0 -Bw 0 04 90 Bh -L1 θ2 05 0 L2 0 θ3 0

Left Rear Leg Right Rear Leg


- 38 -

3. 보행궤적 생성

▪ 동역학 모델링 -LIPM (Linear Inverted Pendulum Model) : 거동하는 로봇을 질점형태의 역진자로 단순화한 모델

▪ LIPM에 의한 궤적 생성 - LIPM 의 뉴턴방정식은 아래와 같다.

- Z축 방향의 거동은 일정하다고 가정하고, 식을 정리 (Z = Hz(보행높이))

- 미분방정식을 계산하고, 걸음시간(Ts, 한걸음을 내딛는 주기) 동안의 초기위치, 속도값이 주어졌을 때, 미분방정식의 특수해를 구할 수 있음.

-최종 위치값 및 속도값이 아래의 조건을 만족한다고 가정하면, 초기 속도값은 아래와 같음


- 39 -

▪ 순기구학 / 역기구학 -LIPM 에 의한 이동궤적은 실제 지면과 접촉된 발끝의 이동에 의한 상대적인 움직임을 나타내며, 발의 움직임은 로봇을 구성하는 각 관절의 운동으로 이루이진다. 따라서 위에서 생성된 보행궤적을 발의 상대좌표로 변환하고, 상대좌표에 대한 각 관절의 각도를 계산하는 역기구학식이 필요하다.

- 순기구학 : 왼쪽, 오른쪽 다리를 간단한 Link 구조 형태로 간주하여 순기구학식 수립


- 40 -

- 역기구학 - θ1 계산 : 어깨 관절의 오프셋 L1을 고려하여 보행궤적에 따른 θ1 을 계산함.

- θ2 , θ3계산 : DH 변환행렬을 이용해, 목표위치를 다리관절 좌표로 변환 후, 다리관절을 2Link로 간구하여 역기구학식을 계산함.

DH 변환행렬은 발끝좌표를 몸통좌표로 변환하므로, 몸통좌표에서 발끝좌표로 변환하기 위해 DH 변환행렬의 역행렬을 구한뒤, 몸통좌표(목표위치)를 곱하여 발끝좌표를 계산한다.


- 41 -

- MATLAB을 이용한 DH 변환행렬 계산(발끝좌표계 -> 몸통좌표계)


- 42 -

- 좌표변환식(몸통좌표 -> 발끝좌표)


- 43 -

- θ2 , θ3계산(최종) : 2Link 관절의 역기구학 해석과 동일

앞발 관절각도

뒷발 관절각도


- 44 -

1. 서보모터를 0도 또는 180도 방향으로 완전히 돌린 후,

반대방향으로 90도 회전시켜 중앙각도를

맞춘다.

2. 서보모터의 중앙위치가 로봇 관절Link의 일직선 상태와 맞도록 조립한다.

4. 로봇시스템 구현

▪ 기구부 조립

-구성 요소

Link1 ( 2개 / 우2개) Link2 4개) Link3 (4개)

Frame1 (2개) Frame2 (1개) Foot (4개)

-서보모터 조립


- 45 -

▪ 전기/제어시스템 구성

-구성 요소

MEGA2560 HS-485HB 서보모터(12개) 18650 (4개), 충전기(2개)

HC-06 블루투스 모듈 브레드보드,점퍼선 등

- 블루투스 연결


- 46 -

- 서보모터/ 배터리 파워 연결

- 서보모터 위치별 연결 핀번호다리(링크) Left Front Right Front Left Rear Right Rear

Link1 14 7 10 4

Link2 12 6 9 3

Link3 11 5 8 2


- 47 -

5. Trot 보행 구현 프로그래밍

#include <math.h>#include <Servo.h>

#define SAMPLING_TIME_MSEC 10#define _1MSEC_TO_1SEC 0.001#define _1SEC_TO_1MSEC 1000.

enum _COORD{ COORD_X, COORD_Y, COORD_Z, MAX_COORD_NUM};

enum _LEG{ LF, RF, LR, RR, MAX_LEG_NUM};

enum _JOINT{ JOINT_1, JOINT_2, JOINT_3, MAX_JOINT_NUM};

Servo servo[MAX_LEG_NUM][MAX_JOINT_NUM];

// 로봇파라미터double h = 188;double w = 110;double l1 = 0;double l2 = 65;


- 48 -

double l3 = 70;double bh = 23.5;double bw = 20;

double pf[MAX_LEG_NUM][3] = { {h / 2, w / 2, -(bh + l2 + l3)}, {h / 2, -w / 2, -(bh + l2 + l3)}, { -h / 2 - bw, w / 2, -(bh + l2 + l3)}, { -h / 2 - bw , -w / 2, -(bh + l2 + l3)} }; // 몸통중심으로부터 각 발끝의 위치값

// 보행파라미터double s = 20;double stride = 40; // 보폭double stride_h = 20; // 보폭(수직)double velocity = 40; // 보행속도 40 mm/sunsigned long ulPeriod_Step = (unsigned long)(stride / velocity * _1SEC_TO_1MSEC);double g = 9.81;double hz = 160;double w1 = sqrt(g / hz);double ts = ulPeriod_Step * _1MSEC_TO_1SEC;

double xb[MAX_LEG_NUM] = {0, };double yb[MAX_LEG_NUM] = {0, };double zb[MAX_LEG_NUM] = {0, };

// 아두이노 핀설정int PInServo[MAX_LEG_NUM][MAX_JOINT_NUM] = {{14, 12, 11}, {7, 6, 5}, {10, 9, 8}, {4, 3, 2}};

// 모터제어 설정int iTargetAngle[MAX_LEG_NUM][MAX_JOINT_NUM] = {{0,}, };int iOffsetAngle[MAX_LEG_NUM][MAX_JOINT_NUM] = {{90 - 10, 90 - 5, 90 - 5}, {90 + 8, 90, 90 + 5}, {90 + 4, 90, 90 + 5}, {90 + 5, 90, 90 - 5}};int dir[MAX_LEG_NUM][MAX_JOINT_NUM] = {{1, 1, 1}, { -1, -1, -1}, {1, 1, 1}, { -1, -1, -1}};

// 제어관련 변수unsigned long ulPreTime_Step = 0;int iSeq = 0;

void setup(){ // 시리얼 통신 초기화 Serial.begin(9600);

// 서보모터 초기화


- 49 -

for (int iLeg = 0 ; iLeg < MAX_LEG_NUM ; iLeg++) { for (int iAxis = 0 ; iAxis < MAX_JOINT_NUM ; iAxis++) { int angle = 0 + iOffsetAngle[iLeg][iAxis];

servo[iLeg][iAxis].attach(PInServo[iLeg][iAxis]); servo[iLeg][iAxis].write(angle); } }

// 2초 Delay delay(2000);

// 현재시간 저장 ulPreTime_Step = millis();}

void loop(){ unsigned long ulCurTime = millis(); int iLeg;

// 동작스텝변경 if (ulCurTime - ulPreTime_Step >= ulPeriod_Step) { ulPreTime_Step = ulCurTime;

iSeq++;

if (iSeq > 3) { iSeq = 2; } }

// 보행시간 ( t = 0 ~ Step Period ) double t = (double)(((ulCurTime - ulPreTime_Step) % ulPeriod_Step) / SAMPLING_TIME_MSEC ) * (SAMPLING_TIME_MSEC * _1MSEC_TO_1SEC);

if (iSeq == 0) // 보행준비 {


- 50 -

for (int iLeg = 0 ; iLeg < MAX_LEG_NUM ; iLeg++) { // 보행궤적생성 xb[iLeg] = pf[iLeg][COORD_X]; yb[iLeg] = pf[iLeg][COORD_Y]; zb[iLeg] = pf[iLeg][COORD_Z] + s * sin( (PI / 2.) * ( (t * _1SEC_TO_1MSEC) / (double)ulPeriod_Step) );

// 보행주기 설정 // hz = zb[iLeg]; // w1 = sqrt(g / hz); } } else if (iSeq == 1) // 보행 반걸음 { ts = (double)ulPeriod_Step * _1MSEC_TO_1SEC;

double xo[MAX_LEG_NUM] = { stride / 2, -stride / 2, -stride / 2, stride / 2};

for (int iLeg = 0 ; iLeg < MAX_LEG_NUM ; iLeg++) { double xo_dot = ( (1 + exp(w1 * ts)) / (1 - exp(w1 * ts)) ) * w1 * xo[iLeg]; double c1 = 0.5 * (xo[iLeg] + (1 / w1) * xo_dot); double c2 = 0.5 * (xo[iLeg] - (1 / w1) * xo_dot);

// 보행궤적생성 if ((t * _1SEC_TO_1MSEC) >= ulPeriod_Step / 2.) { xb[iLeg] = pf[iLeg][COORD_X] + ( c1 * exp(w1 * t) + c2 * exp(-w1 * t) );

if ( iLeg == RF || iLeg == LR ) { zb[iLeg] = pf[iLeg][COORD_Z] + s + stride_h * sin( (PI) * ( ((t * _1SEC_TO_1MSEC) - (double)(ulPeriod_Step / 2.)) / (double)(ulPeriod_Step / 2.)) ); } }

yb[iLeg] = pf[iLeg][COORD_Y];

} }


- 51 -

else if (iSeq == 2) // 보행 완걸음1 { ts = (double)ulPeriod_Step * _1MSEC_TO_1SEC;

double xo[MAX_LEG_NUM] = { -stride / 2, stride / 2, stride / 2, -stride / 2};


// 보행궤적생성 xb[iLeg] = pf[iLeg][COORD_X] + ( c1 * exp(w1 * t) + c2 * exp(-w1 * t) ); yb[iLeg] = pf[iLeg][COORD_Y];

if ( iLeg == LF || iLeg == RR ) { zb[iLeg] = pf[iLeg][COORD_Z] + s + stride_h * sin( (PI) * ( (t * _1SEC_TO_1MSEC) / (double)(ulPeriod_Step)) ); } } } else if (iSeq == 3) // 보행 완걸음2 { ts = (double)ulPeriod_Step * _1MSEC_TO_1SEC;

double xo[MAX_LEG_NUM] = {stride / 2, -stride / 2, -stride / 2, stride / 2};


// 보행궤적생성 xb[iLeg] = pf[iLeg][COORD_X] + ( c1 * exp(w1 * t) + c2 * exp(-w1 * t) ); yb[iLeg] = pf[iLeg][COORD_Y];

if ( iLeg == RF || iLeg == LR ) { zb[iLeg] = pf[iLeg][COORD_Z] + s + stride_h * sin( (PI) * ( (t * _1SEC_TO_1MSEC) /


- 52 -

(double)(ulPeriod_Step)) ); } } }

// PLOT 확인 Serial.println(xb[LF]);

// 역기구학 계산 : iLeg = LF; // Left Front Leg double th1 = -atan((yb[iLeg] - (w / 2)) / (zb[iLeg])) - asin(l1 / (sqrt((yb[iLeg] - (w / 2)) * (yb[iLeg] - (w / 2)) + zb[iLeg] * zb[iLeg]))); double xf3 = (yb[iLeg] * cos(th1 - PI / 2)) / (cos(th1 - PI / 2) * cos(th1 - PI / 2) + sin(th1 - PI / 2) * sin(th1 - PI / 2) ) - (2 * bh * (cos(th1 - PI / 2) * cos(th1 - PI / 2)) + 2 * bh * (sin(th1 - PI / 2) * sin(th1 - PI / 2)) + w * cos(th1 - PI / 2)) / (2 * (cos(th1 - PI / 2) * cos(th1 - PI / 2) + sin(th1 - PI / 2) * sin(th1 - PI / 2))) + (zb[iLeg] * sin(th1 - PI / 2)) / (cos(th1 - PI / 2) * cos(th1 - PI / 2) + sin(th1 - PI / 2) * sin(th1 - PI / 2)); double yf3 = bw - h / 2 + xb[iLeg]; double zf3 = (2 * l1 * cos(th1 - PI / 2) * cos(th1 - PI / 2) + 2 * l1 * sin(th1 - PI / 2) * sin(th1 - PI / 2) - w * sin(th1 - PI / 2)) / (2 * (cos(th1 - PI / 2) * cos(th1 - PI / 2) + sin(th1 - PI / 2) * sin(th1 - PI / 2))) - (zb[iLeg] * cos(th1 - PI / 2)) / (cos(th1 - PI / 2) * cos(th1 - PI / 2) + sin(th1 - PI / 2) * sin(th1 - PI / 2)) + (yb[iLeg] * sin(th1 - PI / 2)) / (cos(th1 - PI / 2) * cos(th1 - PI / 2) + sin(th1 - PI / 2) * sin(th1 - PI / 2)); double th3 = acos((xf3 * xf3 + yf3 * yf3 - l2 * l2 - l3 * l3) / (2 * l2 * l3)); double th2 = atan(yf3 / xf3) - atan((l3 * sin(th3)) / (l2 + l3 * cos(th3)));

iTargetAngle[iLeg][JOINT_1] = (int)(th1 * (180. / PI)); iTargetAngle[iLeg][JOINT_2] = (int)(th2 * (180. / PI)); iTargetAngle[iLeg][JOINT_3] = (int)(th3 * (180. / PI));

iLeg = RF; // Right Front Leg th1 = -atan((yb[iLeg] + (w / 2)) / (zb[iLeg])) + asin(l1 / (sqrt((yb[iLeg] + (w / 2)) * (yb[iLeg] + (w / 2)) + zb[iLeg] * zb[iLeg]))); xf3 = (yb[iLeg] * cos(th1 - PI / 2)) / (cos(th1 - PI / 2) * cos(th1 - PI / 2) + sin(th1 - PI / 2) * sin(th1 - PI / 2)) - (2 * bh * cos(th1 - PI / 2) * cos(th1 - PI / 2) + 2 * bh * sin(th1 - PI / 2) * sin(th1 - PI / 2) - w * cos(th1 - PI / 2)) / (2 * (cos(th1 - PI / 2) * cos(th1 - PI / 2) + sin(th1 - PI / 2) * sin(th1 - PI / 2))) + (zb[iLeg] * sin(th1 - PI / 2)) / (cos(th1 - PI / 2) * cos(th1 - PI / 2) + sin(th1 - PI / 2) * sin(th1 - PI / 2)); yf3 = bw - h / 2 + xb[iLeg]; zf3 = (yb[iLeg] * sin(th1 - PI / 2)) / (cos(th1 - PI / 2) * cos(th1 - PI / 2) + sin(th1 - PI / 2) * sin(th1 - PI / 2)) - (zb[iLeg] * cos(th1 - PI / 2)) / (cos(th1 - PI / 2) * cos(th1 - PI / 2) + sin(th1 - PI / 2) * sin(th1 - PI / 2)) - (2 * l1 * cos(th1 - PI / 2) * cos(th1 - PI / 2) + 2 * l1 * sin(th1 - PI / 2) *


- 53 -

sin(th1 - PI / 2) - w * sin(th1 - PI / 2)) / (2 * (cos(th1 - PI / 2) * cos(th1 - PI / 2) + sin(th1 - PI / 2) * sin(th1 - PI / 2))); th3 = acos((xf3 * xf3 + yf3 * yf3 - l2 * l2 - l3 * l3) / (2 * l2 * l3)); th2 = atan(yf3 / xf3) - atan((l3 * sin(th3)) / (l2 + l3 * cos(th3)));


iLeg = LR; // Left Rear Leg th1 = -atan((yb[iLeg] - (w / 2)) / (zb[iLeg])) - asin(l1 / (sqrt((yb[iLeg] - (w / 2)) * (yb[iLeg] - (w / 2)) + zb[iLeg] * zb[iLeg]))); xf3 = (yb[iLeg] * cos(th1 - PI / 2)) / (cos(th1 - PI / 2) * cos(th1 - PI / 2) + sin(th1 - PI / 2) * sin(th1 - PI / 2)) - (2 * bh * cos(th1 - PI / 2) * cos(th1 - PI / 2) + 2 * bh * sin(th1 - PI / 2) * sin(th1 - PI / 2) + w * cos(th1 - PI / 2)) / (2 * (cos(th1 - PI / 2) * cos(th1 - PI / 2) + sin(th1 - PI / 2) * sin(th1 - PI / 2))) + (zb[iLeg] * sin(th1 - PI / 2)) / (cos(th1 - PI / 2) * cos(th1 - PI / 2) + sin(th1 - PI / 2) * sin(th1 - PI / 2)); yf3 = bw + h / 2 + xb[iLeg]; zf3 = (2 * l1 * cos(th1 - PI / 2) * cos(th1 - PI / 2) + 2 * l1 * sin(th1 - PI / 2) * sin(th1 - PI / 2) - w * sin(th1 - PI / 2)) / (2 * (cos(th1 - PI / 2) * cos(th1 - PI / 2) + sin(th1 - PI / 2) * sin(th1 - PI / 2))) - (zb[iLeg] * cos(th1 - PI / 2)) / (cos(th1 - PI / 2) * cos(th1 - PI / 2) + sin(th1 - PI / 2) * sin(th1 - PI / 2)) + (yb[iLeg] * sin(th1 - PI / 2)) / (cos(th1 - PI / 2) * cos(th1 - PI / 2) + sin(th1 - PI / 2) * sin(th1 - PI / 2)); th3 = -acos((xf3 * xf3 + yf3 * yf3 - l2 * l2 - l3 * l3) / (2 * l2 * l3)); th2 = atan(yf3 / xf3) - atan((l3 * sin(th3)) / (l2 + l3 * cos(th3)));


iLeg = RR; // Right Front Leg th1 = -atan((yb[iLeg] + (w / 2)) / (zb[iLeg])) + asin(l1 / (sqrt((yb[iLeg] + (w / 2)) * (yb[iLeg] + (w / 2)) + zb[iLeg] * zb[iLeg]))); xf3 = (yb[iLeg] * cos(th1 - PI / 2)) / (cos(th1 - PI / 2) * cos(th1 - PI / 2) + sin(th1 - PI / 2) * sin(th1 - PI / 2)) - (2 * bh * cos(th1 - PI / 2) * cos(th1 - PI / 2) + 2 * bh * sin(th1 - PI / 2) * sin(th1 - PI / 2) - w * cos(th1 - PI / 2)) / (2 * (cos(th1 - PI / 2) * cos(th1 - PI / 2) + sin(th1 - PI / 2) * sin(th1 - PI / 2))) + (zb[iLeg] * sin(th1 - PI / 2)) / (cos(th1 - PI / 2) * cos(th1 - PI / 2) + sin(th1 - PI / 2) * sin(th1 - PI / 2)); yf3 = bw + h / 2 + xb[iLeg]; zf3 = (yb[iLeg] * sin(th1 - PI / 2)) / (cos(th1 - PI / 2) * cos(th1 - PI / 2) + sin(th1 - PI / 2) *


- 54 -

sin(th1 - PI / 2)) - (zb[iLeg] * cos(th1 - PI / 2)) / (cos(th1 - PI / 2) * cos(th1 - PI / 2) + sin(th1 - PI / 2) * sin(th1 - PI / 2)) - (2 * l1 * cos(th1 - PI / 2) * cos(th1 - PI / 2) + 2 * l1 * sin(th1 - PI / 2) * sin(th1 - PI / 2) - w * sin(th1 - PI / 2)) / (2 * (cos(th1 - PI / 2) * cos(th1 - PI / 2) + sin(th1 - PI / 2) * sin(th1 - PI / 2))); th3 = -acos((xf3 * xf3 + yf3 * yf3 - l2 * l2 - l3 * l3) / (2 * l2 * l3)); th2 = atan(yf3 / xf3) - atan((l3 * sin(th3)) / (l2 + l3 * cos(th3)));


// 모터 구동 ConvAngle((int**)iTargetAngle);}

void ConvAngle(int** pTarAngle){ for (int iLeg = 0 ; iLeg < MAX_LEG_NUM ; iLeg++) { for (int iAxis = 0 ; iAxis < MAX_JOINT_NUM ; iAxis++) { int iAngle = *( pTarAngle + (iLeg * MAX_JOINT_NUM) + iAxis ); int iMotorAngle = iOffsetAngle[iLeg][iAxis] + (dir[iLeg][iAxis] * iAngle); // 목표각도를 모터 구동방향에 맞도록 변환

// 모터 구동 servo[iLeg][iAxis].write(iMotorAngle); } }}

void ConvAngle(int iLeg, int iAxis, double dAngle){ int iMotorAngle = iOffsetAngle[iLeg][iAxis] + (dir[iLeg][iAxis] * (int)(dAngle * (180. / PI)));

servo[iLeg][iAxis].write(iMotorAngle);}


- 55 -

6 Trot 보행 구현 프로그래밍(블루투스 원격조종)

▪ 아두이노 프로그래밍 ※ 프로그래밍 코드는 별도자료 첨부

▪ 컨트롤앱(App) 설정

⇩

⇩


- 56 -

아두이노로 만드는 4족보행로봇 - amazon s3 · 4. 로봇시스템 구현 ----- 44 5....

Documents