SBOT1 아두이노 세그웨이 밸런싱로봇 제작(Feat. MPU6050, PID제어)
여기서는 세그웨이(Segway)와 같이 두 바퀴로 균형을 유지하여 넘어지지 않는 로봇을 제작합니다.
이 로봇은 Self-balancing robot 또는 평행자동차, 밸런싱로봇, 세그웨이 로봇으로 불리우며 여기서 실험하는 로봇은 매우 단순한 구조로 최소한의 부품으로 밸런싱로봇을 구현해보고 원리를 이해하는데 목적을 두고 있습니다.
완전히 넘어지지 않는 좀더 완벽한 밸런싱 로봇의 구현을 위해서는 DC모터(엔코더 포함), 배터리를 보완하여야 하고 고난위도의 프로그래밍이 필요합니다(ARBOT2/3 버전을 기대하세요~).
두 바퀴로 균형을 유지하기 위해서는 지속적으로 로봇의 기울어짐 정도를 알아내고 이를 토대로 두 개의 모터와 바퀴로 힘을 전달함으로서 로봇을 바로 세우게 하는 알고리즘이 필요합니다.
기본 동작원리는 로봇의 기울어진 정도를 알아내기 위해 MPU6050 자이로 가속도 센서를 사용하고 PID 제어 알고리즘으로 로봇이 평행을 유지하게 하는 것으로 요약해 볼 수 있습니다.
조립 동영상
SBOT1 차체 및 하드웨어 조립
먼저 아크릴판에 브라켓을 볼트와 너트를 이용하여 고정시켜 줍니다.
볼트와 너트를 이용하여 브라켓에 기어드모터를 장착해줍니다. 이 때 전선이 안쪽으로 향하게 합니다.
기어드모터에 바퀴를 장착해줍니다.
아크릴판 위에 스페이서와 너트를 이용하여 배터리가 들어갈 공간을 만들어주고 또 다른 아크릴판을 올려준 뒤 볼트로 고정시킵니다.
폼 양면테이프를 이용해 아두이노보드, L298N 모터 드라이버보드, 비행센서모듈(MPU6050)을 부착합니다.
비행센서모듈(MPU6050)은 12C 인터페이스를 통해 아두이노 우노 보드와 통신하고 아두이노의 SPI 통신핀 A4와 A5를 사용합니다.
기어드 모터에 달린 전선을 아래 사진과 같이 구멍을 통해 밖으로 빼내어 주고, L298N 모터 드라이버보드에 결선합니다.
수-수 점퍼케이블을 아래 사진과 같이 연결합니다.
바퀴를 제어하기 위해 L298N 모터드라이버 보드의 1N1~IN4와 아두이노의 PWM핀인 D6, D9, D10, D11를 각각 연결합니다.
PWM 신호의 듀티비를 사용하여 모터의 속도를 제어하게 됩니다.
암-수 점퍼케이블을 아래 사진과 같이 연결해줍니다.
양면테이프를 이용해 배터리를 부착하고 전원을 연결하면 조립이 완성 되었습니다.
조립이 완료 된 모습
SBOT1 구동 방법 및 프로그램 설명
SBOT1 밸런싱로봇은 MPU6050 자이로+가속도 센서를 사용하여 로봇의 기울어짐 정도를 알아내고 PID 제어 알고리즘을 사용하여 로봇이 평행을 유지하게 합니다.
두 바퀴로 균형을 유지하기 위해서는 지속적으로 로봇의 기울어짐 정도를 알아내고 이를 토대로 두 개의 모터와 바퀴로 힘을 전달함으로서 로봇을 바로 세우게 하는 알고리즘이 필요합니다.
MPU6050은 I2C 인터페이스를 통해 아두이노 우노 보드와 통신하고 아두이노의 SPI 통신핀 A4와 A5를 사용합니다. 바퀴를 제어하기 위해 L298N 모터드라이버 보드의 1N1~IN4와 아두이노의 PWM핀인 D6, D9, D10, D11를 각각 연결합니다. PWM 신호의 듀티비를 사용하여 모터의 속도를 제어하게 됩니다.
MPU6050센서를 통해 로봇이 앞쪽으로 또는 뒤쪽으로 기울어졌는지 확인한 후 앞쪽으로 기울어져 있으면 앞쪽으로 바퀴를 회전(이동)시키고 뒤쪽으로 기울어져 있으면 뒤쪽으로 바퀴를 회전(이동)시켜야 합니다. 이와 동시에 바퀴의 회전속도로 제어합니다. 즉, 로봇이 중앙에서 벗어난 정도에 따라 바퀴의 회전속도가 달라지며 중앙에서 많이 벗어날수록 속도가 빨라집니다.
이를 구현하기 위해 PID 제어 알고리즘을 사용하며 MPU6050 센서에서 전달받은 기울어짐 정도와 크기를 바탕으로 바퀴의 방향과 속도를 결정하게 됩니다.
PID 제어
PID는 Proportional(비례), Integral(적분), Derivative(미분)의 약자로서 산업 제어시스템에서 널리 사용되는 제어 루프 피드백 메카니즘으로서 원하는 설정값과 실제 측정값의 차이를 오차를 지속적으로 계산하고 P,I,D 알고리즘으로 제어하여 원하는 설정값을 유지하게 합니다.
비례(P) 제어를 기본으로 하고 여기에 적분(I)과 미분(D) 제어를 통하여 누적오차를 줄이고 응답속도를 높이며 최대한 안정된 상태로 만들어 줍니다.
일반적인 PID 제어기는 아래의 식으로 표현됩니다.
여기서 e는 설정값과 실제 측정값과의 오차를 나타내며, Kp, Ki, Kd는 제어 파라미터로서 이 값을 수정하여 제어기가 최적으로 동작할 수 있도록 하는 제어기 튜닝작업이 필요합니다. 제어기 튜닝을 통해 최적의 Kp, Ki, Kp 값을 찾아내야 합니다.
아래 사진은 Kp, Ki, Kd 파라메타에 따라 어떻게 응답효과가 달라지는 지를 보여주고 있습니다(출처:www.wikipedia.org).
여기서는 공개된 아두이노용 PID제어기 라이브러리를 사용하며 아래 코드 부분에서 자신에게 맞는 Kp, Ki, Kd 값을 수정하면 됩니다.
// 아래의 4개의 값을 본인의 로봇에 맞게 튜닝합니다. /*********파라메터 튜닝 시작*********/ double setpoint= 180; //로봇이 지면에서 평형을 유지하는 상태의 값입니다. //다음은 PID 제어기의 Kp, Ki, Kd 파라메타를 설정합니다. 아래의 순서대로 설정합니다. double Kp = 21; double Kd = 0.8; double Ki = 140; /******파라메터 튜닝 끝*********/
Kp, Ki, Kd 값을 수정하는 방법은 아래와 같습니다.
1. 먼저 Kp를 조정합니다. Kp값이 너무 작으면 로봇이 쉽게 넘어지며 Kp값이 너무 크면 로봇이 앞뒤로 심하게 흔들리게 됩니다. 로봇이 앞뒤로 조금씩 흔들리는 상태가 최적의 상태입니다.
2. Kp가 설정되면 다음은 Kd를 조정합니다. 최적은 Kd값은 로봇이 안정을 유지하는 동안 진동을 감소시키며 손으로 밀어도 로봇이 바로 복귀되게 합니다.
3. 마지막으로 Ki를 조정합니다. Kp와 Kd가 설정되더라도 안정된 상태로 도달하는 동안 진동을 하게 됩니다. 최적의 Ki값은 로봇이 안정되는 데 걸리는 시간을 단축시킬 수 있습니다.
라이브러리는 아래 링크에서 다운로드하고 아두이노 프로그램 메뉴에서 스케치->라이브러리 포함하기->.ZIP 라이브러리 추가를 누르고, 파일선택창이 나오면 다운받은 라이브러리 파일을 선택하시면 됩니다.
라이브러리
소스코드
아래는 소스코드입니다. 아두이노 IDE에 붙여넣기 하여 컴파일하고 업로드합니다.
// 아두이노 밸런싱로봇 프로그램 (참조 및 출처 : circuitdigest.com)
#include "I2Cdev.h"
#include <PID_v1.h>
#include "MPU6050_6Axis_MotionApps20.h"
MPU6050 mpu;
// MPU control/status vars
bool dmpReady = false; // set true if DMP init was successful
uint8_t mpuIntStatus; // holds actual interrupt status byte from MPU
uint8_t devStatus; // return status after each device operation (0 = success, !0 = error)
uint16_t packetSize; // expected DMP packet size (default is 42 bytes)
uint16_t fifoCount; // count of all bytes currently in FIFO
uint8_t fifoBuffer[64]; // FIFO storage buffer
// orientation/motion vars
Quaternion q; // [w, x, y, z] quaternion container
VectorFloat gravity; // [x, y, z] gravity vector
float ypr[3]; // [yaw, pitch, roll] yaw/pitch/roll container and gravity vector
// 아래의 4개의 값을 본인의 로봇에 맞게 튜닝합니다.
/*********파라메터 튜닝 시작*********/
double setpoint= 180; //로봇이 지면에서 평형을 유지하는 상태의 값입니다.
//다음은 PID 제어기의 Kp, Ki, Kd 파라메타를 설정합니다. 아래의 순서대로 설정합니다.
double Kp = 21;
double Kd = 0.8;
double Ki = 140;
/******파라메터 튜닝 끝*********/
double input, output;
PID pid(&input, &output, &setpoint, Kp, Ki, Kd, DIRECT);
volatile bool mpuInterrupt = false; // MPU6050의 인터럽트 발생유무 확인
void dmpDataReady()
{
mpuInterrupt = true;
}
void setup() {
Serial.begin(115200);
// MPU6050 초기화
Serial.println(F("Initializing I2C devices..."));
mpu.initialize();
// MPU6050 통신확인
Serial.println(F("Testing device connections..."));
Serial.println(mpu.testConnection() ? F("MPU6050 connection successful") : F("MPU6050 connection failed"));
// DMP 초기화
devStatus = mpu.dmpInitialize();
// 기본 옵셋값 설정
mpu.setXGyroOffset(220);
mpu.setYGyroOffset(76);
mpu.setZGyroOffset(-85);
mpu.setZAccelOffset(1688);
// 정상동작하는 경우
if (devStatus == 0)
{
// DMP 가동
Serial.println(F("Enabling DMP..."));
mpu.setDMPEnabled(true);
// 아두이노 인터럽트 설정
Serial.println(F("Enabling interrupt detection (Arduino external interrupt 0)..."));
attachInterrupt(0, dmpDataReady, RISING);
mpuIntStatus = mpu.getIntStatus();
// DMP 사용가능 상태 설정
Serial.println(F("DMP ready! Waiting for first interrupt..."));
dmpReady = true;
// 패킷사이즈 가져오기
packetSize = mpu.dmpGetFIFOPacketSize();
//PID 설정
pid.SetMode(AUTOMATIC);
pid.SetSampleTime(10);
pid.SetOutputLimits(-255, 255);
}
else
{
// 오류시
// 1 = 초기 메모리 에러
// 2 = DMP 설정 오류
Serial.print(F("DMP Initialization failed (code "));
Serial.print(devStatus);
Serial.println(F(")"));
}
//모터 출력핀 초기화
pinMode (6, OUTPUT);
pinMode (9, OUTPUT);
pinMode (10, OUTPUT);
pinMode (11, OUTPUT);
//모터 동작 OFF
analogWrite(6,LOW);
analogWrite(9,LOW);
analogWrite(10,LOW);
analogWrite(11,LOW);
}
void loop() {
// 오류시 작업중지
if (!dmpReady) return;
// MPU 인터럽트나 패킷 대기
while (!mpuInterrupt && fifoCount < packetSize)
{
//MPU6050 데이터가 없는 경우 PID 계산
pid.Compute();
//시리얼 모니터로 현재 상태 출력
Serial.print(input); Serial.print(" =>"); Serial.println(output);
if (input>150 && input<200){//로봇이 기울어지는 경우(각도 범위내에서만)
if (output>0) //앞으로 기울어지는 경우
Forward(); //전진
else if (output<0) //뒤로 기울어지는 경우
Reverse(); //후진
}
else //로봇이 기울어지지 않은 경우
Stop(); //모터 정지
}
// reset interrupt flag and get INT_STATUS byte
mpuInterrupt = false;
mpuIntStatus = mpu.getIntStatus();
// get current FIFO count
fifoCount = mpu.getFIFOCount();
// check for overflow (this should never happen unless our code is too inefficient)
if ((mpuIntStatus & 0x10) || fifoCount == 1024)
{
// reset so we can continue cleanly
mpu.resetFIFO();
Serial.println(F("FIFO overflow!"));
// otherwise, check for DMP data ready interrupt (this should happen frequently)
}
else if (mpuIntStatus & 0x02)
{
// wait for correct available data length, should be a VERY short wait
while (fifoCount < packetSize) fifoCount = mpu.getFIFOCount();
// read a packet from FIFO
mpu.getFIFOBytes(fifoBuffer, packetSize);
// track FIFO count here in case there is > 1 packet available
// (this lets us immediately read more without waiting for an interrupt)
fifoCount -= packetSize;
mpu.dmpGetQuaternion(&q, fifoBuffer); //get value for q
mpu.dmpGetGravity(&gravity, &q); //get value for gravity
mpu.dmpGetYawPitchRoll(ypr, &q, &gravity); //get value for ypr
input = ypr[1] * 180/M_PI + 180;
}
}
void Forward() //전진
{
analogWrite(6,output);
analogWrite(9,0);
analogWrite(10,output);
analogWrite(11,0);
Serial.print("F");
}
void Reverse() //후진
{
analogWrite(6,0);
analogWrite(9,output*-1);
analogWrite(10,0);
analogWrite(11,output*-1);
Serial.print("R");
}
void Stop() //정지
{
analogWrite(6,0);
analogWrite(9,0);
analogWrite(10,0);
analogWrite(11,0);
Serial.print("S");
}
Author
scipia
싸이피아
