반응형
학부때 졸업작품 전에 도전해봤지만 아쉽게도 미완성에 머물었던 Balancing Robot에 대해 포스팅하고자 한다. 추후에 Quad-copter에서 동일하게 IMU 센서(MPU6050)를 사용했다. Filter와 제어기로는 Complementary filter와 Standard PID를 사용하였다. (Quad-copter에서는 상대적으로 더 성능이 좋은 Mahony filter와 Dual-Loop PID를 사용했다.) 이번 글에서는 주로 PID에 대해 이야기 할 것이다. (PID Gain Tuning 중에서 프로젝트가 마무리 됬으므로)
· Standard PID
- P제어 : 현제 상태에서 오차 값의 크기에 비례하게 제어 작용
- I 제어 : 정상상태에서 오차를 없애는 작용을 한다.
- D제어 : 출력 값의 급격한 변화에 제동을 걸러 오버슈트를 줄이고 안정성을 향상시킨다.
사전적인 설명으로 이해가 어렵다면 다음과 같다.
p-control은 기울어진만큼에 비례하는 힘이다. 즉, Balancing Robot이 얼만큼 기울어져있는지에 대해 비례적으로 보상한다. d-control에서 d는 differential(미분)이다. 미분은 한 지점에 대한 변화율을 의미한다. 즉, 순간적인 변화에 반응하는 힘이다. 짧은 시간에 대한 안정성을 보상하는(short-term stability) 제어이다. i-control은 i는 Integral(적분)이다. 적분은 변화량을 누적하여 합계를 구하는(long-term stability)의 제어이다. 즉, 긴 시간에 대해 보상하는 힘이다.
P control
P Gain이 높을 때는 Overshoot가 발생하는 모습을 볼 수 있다. 반면 P Gain이 낮을 때는 반응이 상당히 느린 모습을 볼 수 있다. Tuning을 통해 알게된 사실이지만 i-control은 제어에 큰 영향을 미치지 않는다. 즉 p와 d만으로도 충분한 제어가 가능하다. 추후 영상와 프로그램에서는 i-control을 하지 않았다.
각도 값 확인하기
중요한 과정 중 하나이다. 제대로 각도 값이 나와야 PID 제어도 의미가 있다. 하지만 각도가 90도까지 올라가는 부분에서 제대로 표현하지 못하는 문제가 발생한다. 그리고 급격한 각도 변화에 대해 overshoot가 발생한다. (Complementary filter의 한계)
최종 (미완)
Gain Tuning을 통해 최적의 Gain을 찾고자 했지만 찾지 못했다. 원인으로 생각하는 것들은 여러가지가 있다.
첫 번째는 Clock이다. 현재는 8M 내부클럭을 사용하고 있는데 성능적으로 따라가지 못하는 점이 있을 수 있다. 8M가 안되는 것은 아니지만 성능적으로 외부클럭 16M를 사용한다고 했을 때 떨어질 수 있다는 것이다.
두 번째는, 제어 주기이다. 데이터를 받아오는 것과 PID 제어주기가 Balaning 동작 조건에 부합해야 한다. 데이터(각도)를 읽어오는 것은 반드시 제어주기보다 빠른 시간안에 해결되야 하며 적당한 제어주기를 통해서 모터를 제어해야 한다. 하지만 이 부분들은 이론적으로 알아내는 것이 아니라 여러 차례의 실험을 통해 해결해야될 문제이다. (i2C에서 Data를 읽어오는 것이 느리다. 프로그램상의 문제)
세 번째는 제어기와 필터의 문제이다. Quad-copter는 Mahony filter와 Dual-Loop PID를 사용하였다. 상대적으로 Complementary filter와 Standard PID에 비해 훨씬 우수한 성능을 지닌 필터와 제어기다. 완벽히 제어하기 위해 필터와 제어기의 선정에 대해 간과한 점이 문제가 될 수도 있다.
네 번째는 I2C에서 Data를 읽어오는 문제이다.
Balacing Robot programming
* 코드까지 같이 포스팅하였으나 정리가 제대로 된 코드가 아닌 점을 감안하길 바란다.
/*
Balancing Robot
*/
#define F_CPU 8000000UL
#define sbi(PORTX,bitX) PORTX|=(1<<bitX)
#define cbi(PORTX,bitX) PORTX&=~(1<<bitX)
#define tbi(PORTX,bitX) PORTX^=(1<<bitX)
#define FS_SEL 131
#include <avr/io.h>
#include <avr/interrupt.h>
#include <util/delay.h>
#include <math.h>
void twi_write(unsigned char address,unsigned char data);
unsigned char twi_read(char addressr);
void USART_Transmit(unsigned char tx_data);
void USART_Transmit_init4(int data);
void get_raw_data();
void calibrate();
void cal_angle();
volatile double dt = 0.000, dt2=0.000;
volatile int temp;
volatile unsigned char a_x_l,a_x_h,a_y_l,a_y_h,a_z_l,a_z_h;
volatile unsigned char g_x_l,g_x_h,g_y_l,g_y_h,g_z_l,g_z_h;
volatile double bas_a_x,bas_a_y,bas_a_z;
volatile double bas_g_x,bas_g_y,bas_g_z;
volatile double a_x,a_y,a_z;
volatile double g_x,g_y,g_z;
volatile double las_angle_gx,las_angle_gy,las_angle_gz;
volatile double angle_ax,angle_ay,angle_az;
volatile double angle_gx,angle_gy,angle_gz;
volatile double roll,pitch,yaw;
volatile double alpha;
volatile float error_p, kp, error_d, kd, Kp_term, Kd_term, control;
volatile float prev_error;
ISR(TIMER3_COMPA_vect){
dt += 0.001;
dt += 0.001;
}
int main(){
kp = 19;
kd = 1;
//PORT
DDRA = 0xff;
DDRB |=0b01100000;
//UART
UCSR0A = 0x00;
UCSR0B = (1<<TXEN0);
UCSR0C = (3<<UCSZ00);
UBRR0H = 0;
UBRR0L = 51; //9600
//TWI(I2C)
TWCR = (1<<TWEN);
TWBR = 2; //400khz
//TIMER3 8M 에서 1ms 마다 인터럽트
TCCR3B=0x09;
OCR3AH=(7999>>8);
OCR3AL=(7999&0xFF);
ETIMSK=0x10;
// TCCR3B=0x0A; OCR3AH= 999>>8; OCR3AL= 999&0xFF; ETIMSK=0x10; //8000000/ 8/(1+ 1999)=1000Hz=1ms
//MPU6050 init
twi_write(0x1B, 0x00); // 자이로 config 레지스터 설정 셀프테스트 없고 +-250도 범위
twi_write(0x1C, 0x00); //가속도 설정
twi_write(0x6B, 0x00); //sleep 끔
twi_write(0x1A, 0x05); //DLPF 10Hz
// For DC moter PWM timer1 A B
//TCCR1A = 0b10101001;
//TCCR1B = 0b00001011;
TCCR1A = 0b10100010;
TCCR1B = 0b00011010; // Fast PWM 14 mode(TOP = ICR1), 분주비 8로 설정
ICR1 = 1000; // ICR1 1ms 주기 1khz
//OCR1AL = 0;
//OCR1BL = 0;
calibrate();
sei();
while(1){
get_raw_data(); // 센싱값 가져옴
cal_angle();
//USART_Transmit_init4(a_x);
//USART_Transmit('\t');
//USART_Transmit_init4(g_x);
//USART_Transmit('\t');
USART_Transmit_init4(roll);
USART_Transmit('\t');
// PID 제어
error_p = 0 - roll;
Kp_term = kp * error_p;
error_d = (error_p-prev_error)/dt; // 오차미분 = (현재오차-이전오차)/dt
Kd_term = kd * error_d; // d항 = Kd*오차미분
prev_error = error_p; // 현재오차를 이전오차로
control = Kp_term + Kd_term; // 제어량 = p항+d항
if (control < 0 ) {control *= -1.000;}
if (control>1000) { control = 1000;}
if (roll < 0) { PORTA = 0b00001010;}
else if (roll >0) { PORTA = 0b00000101; }
if ((error_p >= -2) && (error_p <= 2)) {control = 0;}
USART_Transmit_init4(control); USART_Transmit('\t'); USART_Transmit('\n'); USART_Transmit('\r');
OCR1B = control;
OCR1A = control;
}
}
void calibrate() //초기값 읽기
{
int cal = 10;
for(int i=0; i<cal; i++) //평균 10번 읽고 평균
{
get_raw_data();
temp = (a_x_h<<8) | a_x_l;
a_x += temp;
temp = (a_y_h<<8) | a_y_l;
a_y += temp;
temp = (a_z_h<<8) | a_z_l;
a_z += temp;
temp = (g_x_h<<8) | g_x_l;
g_x += temp;
temp = (g_y_h<<8) | g_y_l;
g_y += temp;
temp = (g_z_h<<8) | g_z_l;
g_z += temp;
_delay_ms(100);
}
//평균 계산 저장 /10...
a_x /= cal;
a_y /= cal;
a_z /= cal;
g_x /= cal;
g_y /= cal;
g_z /= cal;
//초기 값으로 저장
bas_a_x = a_x;
bas_a_y = a_y;
bas_a_z = a_z;
bas_g_x = g_x;
bas_g_y = g_y;
bas_g_z = g_z;
}
void get_raw_data() // 자이로 , 가속도 값 읽어오기
{
a_x_h = twi_read(0x3B);// ACCEL High bit X
a_x_l = twi_read(0x3C);// low bit X
a_y_h = twi_read(0x3D);// ACCEL High bit Y
a_y_l = twi_read(0x3E);// low bit Y
a_z_h = twi_read(0x3F);// ACCEL High bit Z
a_z_l = twi_read(0x40);// low bit Z
g_x_h = twi_read(0x43);// GYRO High bit X
g_x_l = twi_read(0x44);// low bit X
g_y_h = twi_read(0x45);// GYRO High bit Y
g_y_l = twi_read(0x46);// low bit Y
g_z_h = twi_read(0x47);// GYRO High bit Z
g_z_l = twi_read(0x48);// low bit Z
}
void twi_write(unsigned char address,unsigned char data)
{
TWCR = (1<<TWINT)|(1<<TWSTA)|(1<<TWEN); //START
while(!(TWCR & (1<<TWINT))); //TWINT flag 기다림
while((TWSR&0xF8) != 0x08); //START 상태(08) 기다림
TWDR = 0b11010000; //AD(1101000)+W(0)
TWCR = (1<<TWINT)|(1<<TWEN); //전송
while(!(TWCR & (1<<TWINT)));
while((TWSR&0xF8) != 0x18); //SLA+W ACK 상태(18) 기다림
TWDR = address; //register address
TWCR = (1<<TWINT)|(1<<TWEN); //전송
while(!(TWCR & (1<<TWINT)));
while((TWSR&0xF8) != 0x28); //Data ACK 상태(28) 기다림
TWDR = data; //data
TWCR = (1<<TWINT)|(1<<TWEN); //전송
while(!(TWCR & (1<<TWINT)));
while((TWSR&0xF8) != 0x28);
TWCR = (1<<TWINT)|(1<<TWSTO)|(1<<TWEN); //STOP
}
unsigned char twi_read(char address)
{
unsigned char data;
TWCR = (1<<TWINT)|(1<<TWSTA)|(1<<TWEN); //START
while(!(TWCR & (1<<TWINT))); //TWINT flag 기다림
while((TWSR&0xF8) != 0x08); //START 상태(08) 기다림
TWDR = 0b11010000; //AD(1101000)+W(0)
TWCR = (1<<TWINT)|(1<<TWEN); //전송
while(!(TWCR & (1<<TWINT)));
while((TWSR&0xF8) != 0x18); //SLA+W ACK 상태(18) 기다림
TWDR = address; //register address
TWCR = (1<<TWINT)|(1<<TWEN); //전송
while(!(TWCR & (1<<TWINT)));
while((TWSR&0xF8) != 0x28); //Data ACK 상태(28) 기다림
TWCR = (1<<TWINT)|(1<<TWSTA)|(1<<TWEN); //Repeat START
while(!(TWCR & (1<<TWINT)));
while((TWSR&0xF8) != 0x10); //Repeat START 상태(08) 기다림
TWDR = 0b11010001; //AD(1101000)+R(1)
TWCR = (1<<TWINT)|(1<<TWEN); //전송
while(!(TWCR & (1<<TWINT)));
while((TWSR&0xF8) != 0x40); //SLA+R ACK 상태(40) 기다림
TWCR = (1<<TWINT)|(1<<TWEN); //전송
while(!(TWCR & (1<<TWINT)));
while((TWSR&0xF8) != 0x58); //ACK 상태(58) 기다림
data = TWDR;
TWCR = (1<<TWINT)|(1<<TWSTO)|(1<<TWEN); //STOP
return data;
}
void USART_Transmit(unsigned char tx_data)
{
while(!(UCSR0A & (1<<UDRE0)));
UDR0 = tx_data;
}
void USART_Transmit_init4(int data) //상수출력 +48
{
if(data < 0)
{
data = -data;
USART_Transmit('-');
}
else
USART_Transmit(' ');
int temp = 0;
temp = data/10000;
USART_Transmit(temp+48);
temp = (data%10000)/1000;
USART_Transmit(temp+48);
temp = (data%1000)/100;
USART_Transmit(temp+48);
temp = (data%100)/10;
USART_Transmit(temp+48);
temp = data%10;
USART_Transmit(temp+48);
}
void cal_angle()
{
//8비트 8비트 총 16비트이기에 H , L 비트 합쳐줌
//각 축 값에 저장..
temp = (a_x_h<<8) | a_x_l;
a_x = temp ;
temp = (a_y_h<<8) | a_y_l;
a_y = temp;
temp = (a_z_h<<8) | a_z_l;
a_z = temp;
temp = (g_x_h<<8) | g_x_l;
g_x = temp;
temp = (g_y_h<<8) | g_y_l;
g_y = temp;
temp = (g_z_h<<8) | g_z_l;
g_z = temp;
// 측정된 gyro 값에서 앞의 평균값을 빼고 131로 나누어 줌.
// 자이로 센서의 값도 -32768에서 32767 값을 가질 수 있다. FS_SEL // 레지스터가 0일 경우 -250 에서 +250까지 측정되며 이 값은
// - 32768에서 32767까지 매핑된다. 1초 동안 일정한 속도로 1도
// 회전하였다고 한다면 250도/s 는 32767 이므로 1도/s는 (32767/250) =
// 131이 된다.
// 측정된 gyro 값에서 앞의 평균값을 빼고 131로 나누어 줌.
g_x = (g_x - bas_g_x)/FS_SEL;
g_y = (g_y - bas_g_y)/FS_SEL;
g_z = (g_z - bas_g_z)/FS_SEL;
//자이로값 각변환
// gyro에서 측정된 값은 각속도 이므로 시간(dt)를 곱해주면 dt시간동안
// 돌아간 각도가 나옴. 그 값을 이전값과 더해줌
angle_gx = g_x*dt + las_angle_gx;
angle_gy = g_y*dt + las_angle_gy;
angle_gz = g_z*dt + las_angle_gz;
dt = 0.000; //시간 초기화
//가속도값을 각변환
// 가속도 센서로부터 나오는 값은 radian값이므로 degree로 바꾸려면 180/pi를 곱해줌
// 가속도 센서로 Z축의 틀어진 각도를 알수는 없으므로 0임. 하지만 편법으로 구하는방법이 있다함 ..
angle_ax = atan(a_y/sqrt(pow(a_x,2) + pow(a_z,2)))*180/3.141592;
angle_ay = atan(-1.000*a_x/sqrt(pow(a_y,2) + pow(a_z,2)))*180/3.141592;
angle_az = 0;
//상보필터
// 원래 공식 angle = (0.98 * (last_angle + (gy_x_data* dt))) + (0.02 * deg) ; 0.98과 0.02은 자체 파라미터값 으로 조정해야함
alpha = 0.96;
roll = alpha*angle_gx + (1.000 - alpha)*angle_ax;
pitch = alpha*angle_gy + (1.000 - alpha)*angle_ay;
yaw = angle_gz;
las_angle_gx = roll; //최근값 누적
las_angle_gy = pitch;
las_angle_gz = yaw;
}
반응형
'Embedded System > ATmega128' 카테고리의 다른 글
| C-LCD 8-bit Control and 4-bit Control (4) | 2020.04.04 |
|---|---|
| NEO-7M (GPS module) (1) | 2020.03.28 |
| HC06 Bluetooth Module (Master / Slave 연동하기) (0) | 2020.03.28 |
| HC06 (Bluetooth Module) Mobile 연동하기 (0) | 2020.03.27 |
| HC-SR04 Module (초음파 센서 모듈) (3) | 2020.03.27 |
댓글