가속도센서를 이용한 각도 측정과 그 한계

한 일년도 훨씬 전쯤에 가속도센서를 이용한 각도 측정에 대한 글을 올렸었습니다만, 당시엔 실험자체를 제가 수행했던 것이 아니었고, 또 그 당시 홈페이지 자료도 다 잃어서 다시 실험하고 정리했습니다. 그러다보니 좀 늦었네요. 많은 분들(3명?^ㅠ^)이 메일과 제 방명록에 요청하셨었는데 참 죄송스럽다는.... 이번 테스트에는 당시의 DSP2812에서 Cortex M3 LM8962로 프로세서를 교체하고 가속도센서도 바꾸었습니다. 당시 가속도센서 여분이 없더라는..ㅜ.ㅜ 그래서 주위를 둘러보니 withrobot사의 myAccel3LV02라는 보드가 있더군요. 판매처는 모릅니다. 알아서 찾아주세요...

꽤 소형입니다. 판매자의 소개를 잠시 빌리면

이렇게 SPI, I2C 통신을 모두 지원한다는군요. 음... 이번에 데리고 놀고있는 CortexM3 보드가 ADC가 총 4개가 있더군요... 제가 생각하는 전체 센서모듈에서 가속도센서 3축에 자이로 2축을 생각하면 모자라길래 자이로센서는 직접 ADC할 생각으로 보정에 필요한 가속도센서는 SPI를 이용하기로 했습니다. 뭐 withrobot에서 배포하는 아름다운 예제가 있길래...~~^^

그 아름다운 예제를 이전에 제가 테스트한 - 엔코더해석 결과를 일정시간간격으로 시리얼통신으로 전송하기-에 포함시켜 생각하기로 했습니다. 가속도센서나 자이로센서나 판매처나 제작사도 많고 그러다보니 사용예제도 참 잘 배포된다는 생각이 듭니다. 그러나, 한가지 아쉬운것은 실제 참값과의 오차나 혹은 참값에 도달하는 시간등을 알수있도록 예제가 꾸며져 있는건 없더군요...(아직 못찾았다는...).. 그래서 저는 참값과의 비교를 위해 엔코더를 버릴수가 없더군요. 또 미분(실제로는 차분)과 적분등을 수행하기 위해 샘플링타임이 필요하니까 딱 위 최근글에서 제가 살짝 바꾼 예제에 가속도센서 예제를 포함시키는게 제일 편하다는 생각을 하게 된겁니다. 하여간... 위 그림처럼 가속도센서 예제에만 들어있는 헤더화일과 c화일을 프로젝트에 포함시킵니다.

그리고 저의 변형예제의 타이머인터럽트 핸들러에 위 코드를 넣어주면됩니다. (풀 코드는 맨아래 넣어두겠습니다..) 그러면

엔코더값, 가속도의 X축, 가속도의 Y축, 가속도의 Z축;

이렇게 시리얼통신으로 데이터가 넘어옵니다. 콤마(,)와 세미콜론(;)으로 데이터들 사이와 줄을 구분지은건 제가 사용할 프로그램이 MATLAB이기 때문에 받을때부터 좀 쉽게 받을려고 해서 그렇습니다.^^. 시리얼 통신으로 데이터를 받는 것도 [Cortex M3] 단순 시리얼통신 테스트 ComPortMaster에서 이야기한 방법 그대로 사용할 것입니다.

먼저 흔들리는 진자의 회전중심축에 가속도센서를 장착합니다.

그러면, 위 그림처럼 보이듯이, atan 함수를 이용해서 정말 간단히 기울어진 각도를 가속도센서를 이용해서 측정할 수 있다는 사실을 알 수 있습니다. 여기서 예전에도 이야기 했지만, 많은 분들이 말씀하시는 스케일펙터의 조절이라던지 가속도센서의 출력 범위(g)의 조절등은 고려할 필요가 없습니다. (적어도 위 상황이라면) 그것은 (y성분/z성분)을 계산할때, 각 종 펙터들을 그 이전에 곱했다 하더라도 약분되어서 없어질 것이기 때문입니다. 실험결과 데이터를 MATLAB에서 처리한 코드는

Acc = load('AccelTest01.txt');

ts = 0.01;
EncAngle = Acc(:,1)*360/2000;
AccY = Acc(:,3);
AccZ = Acc(:,4);
[N, temp] = size(EncAngle);
t = 0:ts:ts*(N-1);

AccAngle = -atan(AccY./AccZ)*180/pi+2.6;

figure
plot(t, EncAngle)
grid on
hold on
xlabel('second');
ylabel('degree');
plot(t, AccAngle, 'r')
legend('Encoder','Accelerometer');
hold off

위 코드에서 8번행은 그래프를 그리기 위한 시간축을 생성한 것이구요. 10번행은 atan함수를 이용해서 각도를 검출한 부분입니다. 부호가 '-'가 붙은 이유는 엔코더의 '+'방향과 반대로 연결되었더군요...^^... 그리고 10번행 마지막의 2.6도를 더하는 것은 가속도센서를 연결할 때 딱 센터를 맞추지 못하고 좀 기울어졌기 때문입니다. 뭐 프로세서단계에서 보정할 수 있었지만, 그놈의 귀찮음때문에....^^  예전 자료에 비하면, 일단 이번 가속도센서의 출력은 정말 깨끗하다는 사실을 알 수 있습니다. 노이즈를 많이 줄였다고 하던데 정말 그래보입니다.

위 그래프에서 파랑색선과 빨강색선이 거의 겹쳐잘 안보이긴 하는데요. 파랑색선은 진자에 직접연결된 엔코더에서 각도를 잡은겁니다. 즉, 참값이라고 봐도 무방합니다. 빨강선은 가속도센서에서 각도를 잡은겁니다. 이제 가속도센서가 각도를 검출할수있는 원리는 이야기했습니다. 그러나 가속도센서만으로 그 각도를 검출할 수 있는건 딱 위의 상황... 전 가속도센서 회전 중심축에 연결되어야하고 그 회전중심축은 제자리에 가만히 있어야만 할 때입니다.

위 그림처럼 만약 가속도센서가 회전중심축에 있지 못하면, 가속도센서는 접선방향의 가속도 성분과 법선방향의 가속도성분을 측정하게 되어서 회전각도를 검출하는데 문제가 생기게 됩니다.

위 그림처럼 가속도센서를 진자의 끝에 연결했습니다.  또 연결할때 실수(^^)로 이번엔 X축이 회전하게 되었다는...ㅋ... 뭐 그래도 상관은 없으니까요...

결과 그래프입니다. 파랑색 엔코더에서 읽어들인 회전각도와 상당한 오차가 발생한다는 사실을 알수있습니다. 중간에 팍 하고 튀는 부분은 제가 손으로 살짝 친겁니다. 가속도센서이니 충격에대해서는 당연히 다른 센서(속도성분을 검출하거나 각도성분을 검출하는 센서보다) 더 민감할 수 밖에 없을겁니다.

위 두 실험의 결론을 말하면, 가속도센서 만으로 각도를 검출하는것은 병진운동성분이 있는 경우는 힘들다는 것입니다. 이제 자이로가 등장해야지요...^^ 위 두 실험데이터는

gettingAngleUsingAccelerometor.zip

으로 올려놓았습니다. MATLAB화일과 시리얼통신으로 받은 데이터는 txt화일입니다. 마지막으로 아래에는 다시 메인 c 코드 전체입니다.

//*****************************************************************************
//
// Withrobot에서 배포하는 엔코더 테스트화일에 
// 시리얼 통신 예제와 타이머 인터럽트 사용예를 통합하고
// 다시 myAccel3LV02보드 테스트용 예제를 통합하고
// 출력양식을 조금 변경함.
//                                              -PinkWink-    2009.06.08
//
//
//*****************************************************************************

#include 
#include 
#include 
#include 
#include "../../../hw_types.h"
#include "hw_ssi.h"
#include "../../../hw_memmap.h"
#include "sysctl.h"
#include "gpio.h"
#include "uart.h"
#include "ssi.h"
#include "qei.h"
#include "ustdlib.h"
#include "timer.h"
#include "interrupt.h"

#include "myAccel3LV02.h"
#include "myAccel3LV02_hal_spi.h"

#ifdef DEBUG
void
__error__(char *pcFilename, unsigned long ulLine)
{
}
#endif

#define BUFFER_LEN              32

static void UARTPutString(char * str);
static void InitTIMERINT(void);
static void InitUART(void);
static void InitQEI(void);
static void TimerIntHandler(void);

int main(void)
{
    SysCtlClockSet(SYSCTL_SYSDIV_4 | SYSCTL_USE_PLL | SYSCTL_OSC_MAIN | SYSCTL_XTAL_8MHZ);

    InitTIMERINT();		// Initialize Timer Interrupt
    InitUART();			// Initialize Serial
    InitQEI();			// Initialize Encoder Module
    MA3_INIT();			// Initialize Accelerometer

    // Loop Start.
    while(1)
    {
    }
}

static void TimerIntHandler(void)
{
    unsigned long pos;
    char buffer[BUFFER_LEN];
    short data_short_array[3];

	// Clear interrupt flag
	TimerIntClear( TIMER0_BASE, TIMER_TIMA_TIMEOUT );

	// Read Encoder Value
	pos = QEIPositionGet(QEI0_BASE);
	usnprintf(buffer, BUFFER_LEN, "%d, ", pos);
	buffer[BUFFER_LEN - 1] = 0;
	UARTPutString(buffer);

	// Read Accel Data
	MA3_CS_ON();
	MA3_READ(MA3_REG_OUTX_L, (unsigned char*)data_short_array, sizeof(data_short_array));
	MA3_CS_OFF();
	usnprintf(buffer, BUFFER_LEN, "%d,  %d,  %d;\n", data_short_array[0], data_short_array[1], data_short_array[2]);
	UARTPutString(buffer);
}

static void UARTPutString(char * str)
{
    while(*str)
        UARTCharPut(UART0_BASE, *str++);
}

static void InitTIMERINT(void)
{
    SysCtlPeripheralEnable(SYSCTL_PERIPH_TIMER0);
    TimerConfigure( TIMER0_BASE, TIMER_CFG_32_BIT_PER );
    TimerLoadSet( TIMER0_BASE, TIMER_A, SysCtlClockGet() / 100 );	// 10ms
    TimerIntRegister( TIMER0_BASE, TIMER_A, TimerIntHandler );
    IntMasterEnable();
    TimerIntEnable( TIMER0_BASE, TIMER_TIMA_TIMEOUT );
    TimerEnable( TIMER0_BASE, TIMER_A );
}

static void InitUART(void)
{
    SysCtlPeripheralEnable(SYSCTL_PERIPH_UART0);
    SysCtlPeripheralEnable(SYSCTL_PERIPH_GPIOA);
    GPIOPinTypeUART(GPIO_PORTA_BASE, GPIO_PIN_0 | GPIO_PIN_1);
    UARTConfigSetExpClk(UART0_BASE, SysCtlClockGet(), 115200, (UART_CONFIG_WLEN_8 | UART_CONFIG_STOP_ONE | UART_CONFIG_PAR_NONE));
}

static void InitQEI(void)
{
    SysCtlPeripheralEnable(SYSCTL_PERIPH_QEI);
    SysCtlPeripheralEnable(SYSCTL_PERIPH_GPIOC);

    GPIOPinTypeQEI(GPIO_PORTC_BASE, GPIO_PIN_4 | GPIO_PIN_6);
    QEIConfigure(QEI0_BASE, (QEI_CONFIG_CAPTURE_A_B | QEI_CONFIG_NO_RESET | QEI_CONFIG_QUADRATURE | QEI_CONFIG_NO_SWAP), 0xffffffff);
    QEIVelocityConfigure(QEI0_BASE, QEI_VELDIV_1, SysCtlClockGet() / 100);

    QEIEnable(QEI0_BASE);
    QEIVelocityEnable(QEI0_BASE);
}