[22' 전람회] 충북과학전람회 (3) - 줄다리기를 로봇으로 구현하자 (1)

앞의 글에서 이론적으로 줄다리기를 이기는 방법에 대해서 작성하였다.

2023.03.01 - [탐구 💾/2022 탐구 이야기] - [22' 전람회] 충북과학전람회 (2) - 줄다리기를 역학적으로 알아보자

[22' 전람회] 충북과학전람회 (2) - 줄다리기를 역학적으로 알아보자

그리고 이제는 그 이기는 방법을 로봇을 통해서 증명하고자 하였다. 사람이 아닌 로봇으로 진행한 가장 큰 이유는 이론상 최적의 각도를 구할 수 있지만, 그 각도까지 사람의 근육들이 버틸 수 없었고, 그 실험을 하는 도중에 줄이 풀려 다치는 안전 상의 문제가 발생하여 로봇으로 구현해야겠다는 생각을 더더욱 하게 되었다.

 

로봇으로 구현하기 전에 먼저 줄다리기 상황을 간단한 모델로 설정하고자 하였다. 정적인 평형을 넘어서 동적인 상황에 대해 고려해야 했기에, 기본 막대로 양측의 정적인 평형을 이루고 있는 상황에서 시작하였다. 가장 기본인 최적의 각도를 유지하다가 상대에 의해 끌려갔다고 가정하면, 다시 최적의 각도를 이루기 위해 발을 앞으로 주춤거리며 각도를 맞추어야 한다. 반대로 상대를 내가 끌고 오면 역시나 최적의 각도를 이루기 위해 발을 뒤로 주춤거리며 다시 각도를 맞출 것이다. 결국, 방동 역시 최적의 각도를 이룬 상황에서 부가적인 요소이므로, 각도 유지가 가장 중요하다.

 

이러한 생각의 흐름과 메커니즘에 맞는 로봇 공학적 요소를 찾기 시작했다. 특정 값에 대해 각도의 변화가 생기면 추가적인 행동을 하여 다시 그 각도를 맞추어야 한다. 이러한 모습이 지표면에 대해서 90도 각도를 이루며 장치가 흔들렸을 때 앞뒤로 움직여 다시 90도를 맞추는 세그웨이 장치와 유사하다는 의견이 도출되어 이를 모방하여 만들어보고자 하였다.

세그웨이 장치

1. 간단한 줄다리기 모델 제안

로봇으로 제작하여 실현하기 위해서는 매우 복잡한 줄다리기 상황에 대해서 각종 변수를 제거해나가며 간단한 모델로 만들 필요성을 느꼈다. 따라서 가장 기본이 되는 아이디어부터 시작하여 간단한 줄다리기 모델을 제안하기 시작했다.

평지에서 각도를 유지하는 모습

위의 간단한 자유물체도와 같이 지평면에서의 세그웨이는 각속도($\omega$)와 각가속도($\alpha$)를 측정하여 각도의 변화를 인식한다. 이를 통해 바퀴 등을 이용함으로써 기울어진 방향 쪽으로 본체의 하단 부를 이동시켜 다시 지면과 수직을 유지한다. 이는 최적의 각도를 유지하기 위해서 외력으로 인해 각도의 변화가 생겼을 때, 발을 주춤거리며 최적의 각도를 다시 유지하는 선수들의 모습과도 굉장히 유사하다. 이와 같은 특징들을 토대로, 세그웨이라는 개념을 로봇 제작의 핵심 개념으로 선정하였다.

 

줄다리기 상황처럼 기울어진 상황에서의 세그웨이

 단순하게 세그웨이를 적용하는 것만으로는 줄다리기 모델을 제대로 구현하기에는 한계가 있었다. 세그웨이가 유지하는 각도는 지표면과 수직인 90도이지만, 우리가 필요한 로봇은 약 30도 근처에서도 각도를 유지하는 모습이었다. 따라서 특정 값을 유지할 수 있는 변형된 세그웨이의 기능이 필요했고, 경사면에서 세그웨이를 놓는다면 어떤 식의 모습을 가지고 있을지에 대해서 사고 실험을 진행해 보았다.

 

 지표면에 대해서 $\theta$를 이루는 경사면에 세그웨이를 놓으면, imu 센서는 지구 중심에 대해서 인식을 하여 똑같이 지표면에 대해서 90도를 이룰 것이라는 결론에 다 달았다. 이는 다시 말하면 경사면에 애해서는 $90-\theta$의 각도를 이룰 것이라는 말과 같았다.

 

하지만 이러한 상황에서 위로 올라가야 하는 순간과 아래로 내려가서 각도를 맞추는 상황은 달랐다. 지표면과 달리 위아래의 움직임의 속도 차이는 분명히 중력에 의해서 발생할 것이었고, 따라서 각도와 물체의 중력을 고려하여 각 방향에 대한 모터 속도의 보정이 필요하였다.

 

세그웨이를 줄다리기 상황에 적용한 모습

경사면에서의 세그웨이에 약간의 변화를 준다면 위의 그림과 같은 모습이 되는 것을 볼 수 있다. 경사면에서의 $mg\sin \theta$가 장력($T$)으로, $mg\cos \theta$가 중력($mg$)으로 대응된다. 또한 경사면이 지표면과 평행하게 되면서 경사각이었던 $\theta$만큼 세그웨이가 눕는 모습이 된다. 

따라서 경사면에서의 세그웨이와 우리가 최종적으로 원한 장력이 작용하며 특정 각도를 유지하는 세그웨이가 크게 다르지 않다는 결론을 얻을 수 있었다. 따라서 경사면에서 해주었던 중력과 각도에 대한 보정이, 장력과 각도에 대한 보정으로 대응되었다. 이를 통해 줄다리기 로봇에 적용될 가장 간단한 줄다리기 모델을 제안하게 되었다.

 

2.아두이노를 이용한 세그웨이 기반 줄다리기 로봇 제작 - 요약

앞에서 얻은 모델에 맞는 로봇을 아두이노를 이용하여 제작을 시작하고자 하였다. 가장 먼저 지표면에 수직으로 유지하는 기본형태의 세그웨이 제작을 진행했다. 로봇을 구성하는 요소로 전체적인 프레임, 아두이노 UNO, 모터와 모터드라이버, 배터리 셀 그리고 각속도와 각가속도를 인식하는 MPU6050(6축 가속도 센서)를 사용하였다.

특히 가속도를 인식하는MPU6050에 대해 조사, 실행을 통해 X/Y/Z 축에 대한 각속도와 각가속도가 어떤 형태로 얻어지는지에 대해 확인하였다.

imu가 받는 데이터 값과 어떤 방식으로 이용할지에 대한 고민

인식한 값을 가공하고 모터에 명령하는 메커니즘에 대해서 알기 쉽고 처리하기 쉬운 형태로 만드는 작업이 필요했다. 센서에서 받은 x축 각속도, y축 각속도, z축 각속도, x축 각가속도, y축 각가속도, z축 각가속도를 이용하여, 이 값들을 칼만필터를 이용해 보정한 뒤, 알기 쉽게 하기 위해서 Yaw, Pitch, Roll 값으로 바꾸는 것이 도움 된다는 것을 알게 되어, 해당 개념을 코드에 적용하여 보고 1차 제작한 로봇을 가동해 보았다..

 

그러나 단순히 각도가 기울어짐에 따라 모터를 가동하는 것만으로는 양쪽으로 왔다 갔다 할 뿐, 각도를 유지하지는 않았다. 이는 단순히 on/off 형태의 출력방식이 문제라고 생각하여 이를 해결할 수 있는 제어방식에 대해 조사하였다. 그 결과 기울어진 θ에 비례한 모터 출력을 명령하는 비례 제어(P제어), 각종 에러와 급격한 변화에 대해 안정성을 높이는 미분 제어(I제어), 에러의 적분 값에 비례해서 시스템에 입력을 주어 잔류 에러는 줄이는 적분 제어(D제어)를 이용하는 즉 PID제어를 사용하고자 하였다. PID 제어에 대한 이해를 위해 더욱 조사하고 공부하여 아두이노 코딩에 적용하여 1차적인 기본 모델을 제작할 수 있었다.

 

1차 제작 로봇

요소별 성능 미달이나 제어상수 등에 의해 버벅거리거나 원하는 축 이외의 방향으로 움직이는 등의 문제점이 발생하였지만, 전체적으로 지면에 대해 90도를 유지하는 것을 관찰할 수 있었다.

 

다음 단계로써 줄을 묶어 장력이 작용하는 상태에서 기울기를 유지할 수 있는 줄다리기 로봇을 제작하였다. 본체의 프레임을 설계하여 아크릴 가공을 통해 새로운 프레임을 통해 더욱 적합한 형태로 발전시켰다.

 

 

제어상수 등 코드에 수정을 진행하여 위와 마찬가지로 약간의 버벅거림을 가지지만 원하는 특정 각도를 유지하려는 경향성을 띔을 관찰할 수 있었다. 같은 모델을 두 개 만들어 각도에 대한 분석, 반동에 대한 분석을 진행하고자 하였으나, 연구기간 부족 문제로 인하여 향후 탐구 계획으로 전환하였다.

다음 코드는 주요 아두이노 코드이다.

// PID 제어용 변수 선언
double kp = 2.8;
double ki = 0.5;
double kd = 15;

// 기울일 각도 선택 
double originalSetpoint = 250;
double setpoint = originalSetpoint;

void loop(){
    pid.Compute(); // 루프를 돌면서 pid 값을 업데이트합니다
        motorController.move(output, MIN_ABS_SPEED); // pid 연산으로 나온 output 값을 motorController로 전송

 // 센서를 통해 구한 Yaw, Pitch, Roll 값을 Serial Monitor에 표시합니다
        Serial.print("ypr\t");
        Serial.print(ypr[0] * 180/M_PI);
        Serial.print("\t");
        Serial.print(ypr[1] * 180/M_PI);
        Serial.print("\t");
        Serial.println(ypr[2] * 180/M_PI);
#endif
        // PID 제어를 하기 위해 input 변수에 Pitch 값을 넣습니다
        input = ypr[1] * 180/M_PI + 180;
}

 

또한 접어둔 글은 전체 코드이다. 

// PID 제어, 모터 제어, MPU6050센서 값을 받기 위한 선언문
#include <PID_v1.h>
#include <LMotorController.h>
#include "I2Cdev.h"
#include "MPU6050_6Axis_MotionApps20.h"
#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
    #include "Wire.h"
#endif
#define OUTPUT_TEAPOT 1                    // Processing을 통해 MPU6050 센서를 Visualize 하고 싶은 경우 1, 아니면 0으로 선언합니다
#define MIN_ABS_SPEED 30                  // 모터의 최저속도를 설정합니다.   0 ~ 255 값 중 선택
#define OUTPUT_READABLE_YAWPITCHROLL    // Yaw, Pitch, Roll 값을 얻기 위해 선언합니다
#define INTERRUPT_PIN 2                  // MPU6050 센서의 INT 핀이 꽂혀있는 번호를 설정합니다. 보통 2번
#define LED_PIN 13                         // Arudino Uno의 13번핀 LED를 동작 중에 반짝거리게 하려고 선언합니다 
//MPU 객체를 선언합니다
MPU6050 mpu;
// MPU control/status vars
bool blinkState = false;    // LED를 반짝거리게 하기 위한 변수
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
// MPU6050 센서를 통해 쿼터니언과 오일러각, Yaw, Pitch, Roll 값을 얻기 위해 선언합니다
// orientation/motion vars
Quaternion q;           // [w, x, y, z]         quaternion container
VectorInt16 aa;         // [x, y, z]            accel sensor measurements

VectorInt16 aaReal;     // [x, y, z]            gravity-free accel sensor measurements
VectorInt16 aaWorld;    // [x, y, z]            world-frame accel sensor measurements
VectorFloat gravity;    // [x, y, z]            gravity vector
float euler[3];         // [psi, theta, phi]    Euler angle container
float ypr[3];           // [yaw, pitch, roll]   yaw/pitch/roll container and gravity vector
// Processing으로 MPU6050 센서를 Visualize 하기 위한 변수
// packet structure for InvenSense teapot demo
uint8_t teapotPacket[14] = { '$', 0x02, 0,0, 0,0, 0,0, 0,0, 0x00, 0x00, '\r', '\n' };
// PID 제어용 변수 선언
double kp = 2.8;
double ki = 0.5;
double kd = 15;
// 기울일 각도 선택 
// 제가 만든 밸런싱로봇에는 184.0도가 가장 최적의 평형각도였습니다
// 각도가 180도를 기준으로 +-를 설정해주시면 됩니다
double originalSetpoint = 250;
double setpoint = originalSetpoint;
double movingAngleOffset = 0.3;
// PID 제어용 input, output 변수를 선언합니다
double input, output;
// PID값을 설정해준다
PID pid(&input, &output, &setpoint, kp, ki, kd, DIRECT);
// 모터 제어용 변수 선언
// EnA, EnB는 속도제어용(pwm), IN1,2,3,4는 방향제어용 핀입니다
int ENA = 10;
int IN1 = 12;
int IN2 = 6;
int IN3 = 9;
int IN4 = 8;
int ENB = 11;
// motorController 객체 생성, 맨 끝 파라미터 1,1은 각각 좌측, 우측모터의 최대속도(%) 입니다.
LMotorController motorController(ENA, IN1, IN2, ENB, IN3, IN4, 1, 1);
// ===               INTERRUPT DETECTION ROUTINE                ===
volatile bool mpuInterrupt = false;     // indicates whether MPU interrupt pin has gone high
 
void dmpDataReady() {
    mpuInterrupt = true;
}
immediately
 
    // initialize device
    Serial.println(F("Initializing I2C devices..."));
    mpu.initialize();
    pinMode(INTERRUPT_PIN, INPUT);
    // verify connection
    Serial.println(F("Testing device connections..."));
    Serial.println(mpu.testConnection() ? F("MPU6050 connection successful") : F("MPU6050 connection failed")); 
 
    // load and configure the DMP
    Serial.println(F("Initializing DMP..."));
    devStatus = mpu.dmpInitialize();
 
    // supply your own gyro offsets here, scaled for min sensitivity
    mpu.setXGyroOffset(220);
    mpu.setYGyroOffset(76);
    mpu.setZGyroOffset(-85);
    mpu.setZAccelOffset(1788); // 1688 factory default for my test chip
 
    // devStatus 값이 0 이면 정상작동, 0이 아니면 오작동
    // make sure it worked (returns 0 if so)
    if (devStatus == 0){
        // turn on the DMP, now that it's ready
        Serial.println(F("Enabling DMP..."));
        mpu.setDMPEnabled(true);

        // enable Arduino interrupt detection
        Serial.println(F("Enabling interrupt detection (Arduino external interrupt 0)..."));
        attachInterrupt(digitalPinToInterrupt(INTERRUPT_PIN), dmpDataReady, RISING);
        mpuIntStatus = mpu.getIntStatus();
 
        // set our DMP Ready flag so the main loop() function knows it's okay to use it
        Serial.println(F("DMP ready! Waiting for first interrupt..."));
        dmpReady = true;
 
        // get expected DMP packet size for later comparison
        packetSize = mpu.dmpGetFIFOPacketSize();
 

        // MPU6050 센서가 정삭작동하면 PID 제어용 코드를 초기화합니다
        pid.SetMode(AUTOMATIC);
        pid.SetSampleTime(10);
        pid.SetOutputLimits(-255, 255);  
    }
    else{   // MPU6050 센서가 오작동한 경우
        // ERROR!
        // 1 = initial memory load failed
        // 2 = DMP configuration updates failed
        // (if it's going to break, usually the code will be 1)
        Serial.print(F("DMP Initialization failed (code "));
        Serial.print(devStatus);
        Serial.println(F(")"));
    }
 
    // 로봇이 작동 중 13번 LED를 깜빡거리기 위해 OUTPUT으로 초기화합니다
    pinMode(LED_PIN, OUTPUT);
}
void loop(){
    // if programming failed, don't try to do anything
    if (!dmpReady) return;
 
    // wait for MPU interrupt or extra packet(s) available
    while (!mpuInterrupt && fifoCount < packetSize){
        //no mpu data - performing PID calculations and output to motors
 
        pid.Compute(); // 루프를 돌면서 pid 값을 업데이트합니다
        motorController.move(output, MIN_ABS_SPEED); // pid 연산으로 나온 output 값을 motorController로 전송
    }
    // reset interrupt flag and get INT_STATUS byte
    mpuInterrupt = false;
    mpuIntStatus = mpu.getIntStatus();
    // get current FIFO count
    fifoCount = mpu.getFIFOCount();
    // MPU6050 센서가 정상작동하는 경우에만 PID제어를 해야하므로 아래와 같이 if-else문을 작성합니다
    // 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!"));
    }
    // MPU6050 센서가 정상작동하는 경우
    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;
        // display Euler angles in degrees
        mpu.dmpGetQuaternion(&q, fifoBuffer);
        mpu.dmpGetGravity(&gravity, &q);
        mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);
#ifdef OUTPUT_READABLE_YAWPITCHROLL  // 센서를 통해 구한 Yaw, Pitch, Roll 값을 Serial Monitor에 표시합니다
        Serial.print("ypr\t");
        Serial.print(ypr[0] * 180/M_PI);
        Serial.print("\t");
        Serial.print(ypr[1] * 180/M_PI);
        Serial.print("\t");
        Serial.println(ypr[2] * 180/M_PI);
#endif
        // PID 제어를 하기 위해 input 변수에 Pitch 값을 넣습니다
        input = ypr[1] * 180/M_PI + 180;
 
#ifdef OUTPUT_TEAPOT  // Processing으로 MPU6050센서의 움직임을 Visualize 하기 위한 코드
        // display quaternion values in InvenSense Teapot demo format:
        teapotPacket[2] = fifoBuffer[0];
        teapotPacket[3] = fifoBuffer[1];
        teapotPacket[4] = fifoBuffer[4];
        teapotPacket[5] = fifoBuffer[5];
        teapotPacket[6] = fifoBuffer[8];
        teapotPacket[7] = fifoBuffer[9];
        teapotPacket[8] = fifoBuffer[12];
        teapotPacket[9] = fifoBuffer[13];
        Serial.write(teapotPacket, 14);
        teapotPacket[11]++; // packetCount, loops at 0xFF on purpose
#endif
    }
}

 

다음 글에서는 개발에 대해서 조금 더 자세하게 작성하고자 한다.

 

관련 코드들을 업로드한 깃헙 링크는 다음과 같다.

https://github.com/MOSW626/Tug_of_War_Robot_Project_-2022-.git

GitHub - MOSW626/Tug_of_War_Robot_Project_-2022-