2015년 7월 22일 수요일

IMU 센서 사용법

IMU센서 테스트 관련된 내용을 정리해 놓습니다.

참고로 아두이노, 프로세싱을 주로 사용하는 데, 이와 관련된 내용은 관련 사이트나 해당 도구 활용 사례를 정리한 레퍼런스를 참고하시길 바랍니다. 실제 실습을 위해서는 다음 프로그램 설치가 필요합니다. 아두이노 설치 후 실제 연결 시 시리얼 포트가 생성되어야, 아래 예제를 실행할 수 있습니다.
개요
IMU는 inertial measurement unit (관성측정장치)의 약자이다.
다음과 같이, 자세 변환, 위치 이동에 대한 변화속도, 변위량을 측정할 수 있다.   
IMU (www.ni.com)

그러므로, 로봇이나 측정된 센서 값의 자세나 위치를 제어(보정)할 때 많이 사용된다. IMU센서는 측정할 수 있는 값의 종류에 따라 DoF (degree of freedum. 자유도)가 높아진다. 예를 들어, Adafuit 10-DoF 칩은 L3GD20H (3축 자이로 센서), LSM303 (3축 컴파스 및 3축 가속도계), BMP180 (기압계)로 구성되어 있고, 측정 값이 3 + 3 + 3 + 1 이므로 10 DoF 센서로 표시됩니다 .아래는 SD746 IMU 센서이며, 6 DoF를 가진다.

센서에서 얻은 값은 오차가 포함되어 있다. 그러므로, 가속도를 그대로 누적한다고, 정확한 이동거리가 나오지 않는다. 그러나, 오차가 정규분포를 가진다면, 그 평균값으로 어느 정도 정밀한 이동거리를 얻을 수 있다. 이를 수학적으로 계산하는 방법이 상보필터나 칼만필터이다.

상보 필터는 각도를 계산할 때, 단순히, 자이로값(각속도값)만 적분(합산)하지 않는다. 가속도 센서값을 함께 고려해, 에러값을 보정한다. 이 내용을 수식으로 표현하면 다음과 같다(참고).

 Filtered Angle = α × (Gyroscope Angle) + (1 − α) × (Accelerometer Angle) 
α = τ/(τ + Δt)  
(Gyroscope Angle) = (Last Measured Filtered Angle) + ω×Δt

Δt = sampling rate, τ = time constant greater than timescale of typical accelerometer noise

코딩하면 다음과 같다. 
angle = 0.98 * (angle + gyroscope_data + delta_t) + 0.02 * (accelerometer)

여기서, 0.98과 0.02 상수값을 변경해, 필터를 얼마나 적용할 지 여부를 설정할 수 있다. 상세한 소스는 이 링크를 참고한다.

칼만 필터는 1960년대 루돌프 칼만이 개발한 알고리즘으로 NASA 아폴로 프로젝트 네비게이션 개발 시 사용되었다. 칼만 필터는 가우시안(Gaussian) 분포를 따를 경우 동작한다. 칼만 필터는 상태 예측과 측정 업데이트를 반복적으로 수행해 현재 값을 계산한다.

이 글에서는 IMU 센서를 이용해, 3차원 모델 표시하는 방법만 정리해 본다.

6-DoF IMU 개발
6-DoF IMU 개발을 위해 MPU-6050을 사용한다. 이 센서는 자이로(Gyro)와 엑셀로미터(Accelerometer, 가속도계)가 장착되어 있다. MotionTracking이란 회사에서 개발하였는 데, 저전력에 고성능으로 스마트폰, 드론, 산업용 장비 등에 널리 사용된다.
MPU-6050의 자이로 시스템은 다음과 같다.
자이로 센서는 ±250, 500, 1000, 2000 DPS(Degree per second) 해상도를 가진다. DPS는 초당 회전각으로 각속도이다. 각속도이므로, 가만이 두면 이 값은 0에 수렴한다.
가속도 센서의 단위는 g이다. 가속도센서는 z축 방향으로 중력 가속도를 감지할 수 있다. 가속도 해상도는 ±2, 4, 8, 16g 이다. 센서를 흔들리지 않게 고정해 놓으면 x, y 값은 0에 근접하나 z축은 18,000 정도 값을 유지한다. 해상도 기본값이 ±2이므로, 값이 18,000이면 1g를 의미하는 것이다.

MPU-6050을 다음과 같이 연결한다.

그리고 다음 코드 입력후 업로드한다(상세 내용 링크 참고).
// MPU-6050 Short Example Sketch
// By Arduino User JohnChi
// August 17, 2014
// Public Domain
#include<Wire.h>
const int MPU_addr=0x68;  // I2C address of the MPU-6050
int16_t AcX,AcY,AcZ,Tmp,GyX,GyY,GyZ;
void setup(){
  Wire.begin();
  Wire.beginTransmission(MPU_addr);
  Wire.write(0x6B);  // PWR_MGMT_1 register
  Wire.write(0);     // set to zero (wakes up the MPU-6050)
  Wire.endTransmission(true);
  Serial.begin(9600);
}
void loop(){
  Wire.beginTransmission(MPU_addr);
  Wire.write(0x3B);  // starting with register 0x3B (ACCEL_XOUT_H)
  Wire.endTransmission(false);
  Wire.requestFrom(MPU_addr,14,true);  // request a total of 14 registers
  AcX=Wire.read()<<8|Wire.read();  // 0x3B (ACCEL_XOUT_H) & 0x3C (ACCEL_XOUT_L)    
  AcY=Wire.read()<<8|Wire.read();  // 0x3D (ACCEL_YOUT_H) & 0x3E (ACCEL_YOUT_L)
  AcZ=Wire.read()<<8|Wire.read();  // 0x3F (ACCEL_ZOUT_H) & 0x40 (ACCEL_ZOUT_L)
  Tmp=Wire.read()<<8|Wire.read();  // 0x41 (TEMP_OUT_H) & 0x42 (TEMP_OUT_L)
  GyX=Wire.read()<<8|Wire.read();  // 0x43 (GYRO_XOUT_H) & 0x44 (GYRO_XOUT_L)
  GyY=Wire.read()<<8|Wire.read();  // 0x45 (GYRO_YOUT_H) & 0x46 (GYRO_YOUT_L)
  GyZ=Wire.read()<<8|Wire.read();  // 0x47 (GYRO_ZOUT_H) & 0x48 (GYRO_ZOUT_L)
  Serial.print("AcX = "); Serial.print(AcX);
  Serial.print(" | AcY = "); Serial.print(AcY);
  Serial.print(" | AcZ = "); Serial.print(AcZ);
  Serial.print(" | Tmp = "); Serial.print(Tmp/340.00+36.53);  //equation for temperature in degrees C from datasheet
  Serial.print(" | GyX = "); Serial.print(GyX);
  Serial.print(" | GyY = "); Serial.print(GyY);
  Serial.print(" | GyZ = "); Serial.println(GyZ);
  delay(333);
}

그럼 IMU 값이 시리얼모니터로 출력되는 것을 확인할 수 있다.

Kalman 필터를 적용하기 위해 다음 github에서 코드 전체를 다운로드 받는다.
이제 압축파일을 푼다. 아두이노를 띄우고, MPU6050.ino를 로드한다. 그럼, 시리얼 모니터를 통해 데이터가 출력된다. 데이터는 roll, gyro X angle, 상보필터 처리 후 X angle, 칼만필터 처리 후 X angle 값, pitch, gyro Y angle 값, 상보필터 처리 후 Y angle, 칼만필터 처리 후 Y angle 값이 차례대로 출력된 것이다.

다운로드 폴더의 Graph 폴더를 Processing 프로그램으로 열어본다. 실행하면 다음과 같은 그래프가 출력될 것이다.

그래프의 각 컬러값은 다음을 의미한다.
자이로 X값 = Yellow
가속도 X값 = Green
상보필터 처리 X값 = Blue
칼만필터 처리 X값 = Red

자이로 Y값 = Purble
가속도 Y값 = Light blue
상보필터 처리 Y값 = Lawn Green
칼만필터 처리 Y값 = Black

필터처리 전후를 보면, 노이즈가 잘 제거되고 있는 것을 확인할 수 있다.

10-DoF IMU 개발
Adafuit 10-DoF IMU 센서를 사용해서, 관련 레퍼런스를 참고하여, 작업한다.

1) 우선 IMU센서 핀을 납땜하고, 아두이노 보드 입력핀과 결선한다.


센서 보드 핀의 SCL핀은 아두이노 보드 A5로 결선.
센서 보드 핀의 SDA핀은 아두이노 보드 A4로 결선.
센서 보드 핀의 VIN은 아두이노 보드 5V로 결선.
센서 보드 핀의 GND는 아두이노 보드 GND로 결선.


참고로, 나머지 센서 핀들을 인터럽트 및 READY 핀들이다. 자세한 사항은 Adafuit IMU 10-DoF 센서를 구성하는 아래 데이터시트를 참고한다.
2) 아래의 Adafruit 센서 라이브러리를 아두이노 라이브러리에 설치한다.

아울러, 센서 예제 데모인 Adafruit_AHRS 소스를 아래 링크에서 다운로드 받아, 아두이노 폴더의 라이브러리에 설치한다.
그 결과, 라이브러리 폴더 안에는 다음과 같은 폴더들이 포함될 것이다.


3) Adafruit 센서 예제 데모를 실행하기 위해 필요한 아래의 프로세싱 라이브러리를 설치한다.

4) Adafruit_AHRS 예제 데모 소스에서 ahrs_10dof 폴더의 소스를 아두이노에 로드하고 빌드해, 아두이노 보드에 전송한다.

5) Adafruit_AHRS 예제 데모 소스에서 processing 폴더의 bunnyrotate 폴더 내 10-DoF 센서 프로세싱 예제를 실행한다.

6) 다음과 같은 3차원 Bunny 모델이 센서 자세에 따라 회전하는 것을 확인할 수 있다.

 그림. 좌측 그림(Roll=0도 일때 모습)   우측 그림(Roll=90도 일때 모습)

만약, 제대로 동작 안되면, 테스터 등을 이용해 결선이 제대로 되었는 지 등을 확인한다.


센서에서 몇몇 튀는 값들이 있어, 이 부분은 필터링해야 한다. 본인은 간단히 아래 함수로 평균값 처리하였다.

void smoothIMUdata(String[] list)
{
  roll  = float(list[1]);
  pitch = float(list[2]);
  yaw   = float(list[3]);
  
  if(totalIMUdata < 10)
  {
    rolls[totalIMUdata] = roll;
    pitchs[totalIMUdata] = pitch;
    yaws[totalIMUdata] = yaw;        
    totalIMUdata++;
  }
  else
  {
    int i = 0;
    for(; i < 9; i++)
    {
      rolls[i] = rolls[i + 1];
      pitchs[i] = pitchs[i + 1];
      yaws[i] = yaws[i + 1];
    }
    rolls[i] = roll;
    pitchs[i] = pitch;
    yaws[i] = yaw;

    roll = pitch = yaw = 0.0F;
    for(i = 0; i < 10; i++)
    {
      roll += rolls[i];
      pitch += pitchs[i];
      yaw += yaws[i];
    }  
    
    roll = roll / (float)totalIMUdata;
    pitch = pitch / (float)totalIMUdata;
    yaw = yaw / (float)totalIMUdata;
  }  
}

정밀도는 IMU센서 가격이 비례하는 부분이 있다. 다만, 시작품 개발 정도는 이정도도 충분하다.

이제, 3차원 포인트 클라우드를 취득하는 라이다나 RGB-D센서 자세를 고려해, 그 좌표값들을 보정할 수 있다. 다음부터는 RGB-D 및 RPLiDAR를 이용해, 좌표값 보정을 해보도록 한다. 가속도를 사용해, SLAM을 처리하는 것은 그 다음이다.

참고자료

댓글 2개:

  1. 아두이노와 저렇게 연결하고 프로세싱하면 한점에서 고정된채 움직이는것이 아니라 3차원으로 움직이는것도 가능한가요?

    답글삭제
  2. 늦게 발견해 보았네요. 프로세싱은 3차원은 물론 지원하고, 다양한 라이브러리를 통해 키넥트, 센서 등도 쉽게 지원됩니다. 매우 강력하고, 맥 환경에서 syphon 을 사용하면, 프로젝션 맵핑도 됩니다. 관련 내용은 아래 링크를 참고하시면 됩니다.

    1. https://www.youtube.com/watch?v=bujvmvPyQiM
    2. http://daddynkidsmakers.blogspot.nl/2016/01/processing-kinect-syphon-vpt.html?view=sidebar

    답글삭제