iFBalance

iFBalance-코딩으로 뽀샤버리기

6강. MPU9250 IMU 센서 제어하기

Posted by INDIFROG on 2021-03-06
Words 1.2k and Reading Time 7 Minutes
Viewed Times

6강. MPU9250 IMU 센서 제어하기

아래 그림은 iFBalance 로봇에서 사용중인 iFZero 보드에 내장된 MPU9250의 회로도입니다. MPU9250 IMU 칩셋은 출시가 된지 좀 오래 되긴 했지만 밸런스 로봇에 사용하기엔 충분한 성능을 보여줍니다.

사실 iFbalance 로봇에서 가장 중요한 부분이라고 볼 수 있는 IMU 센서인 MPU9250은 최근에 와서야 겨우 올바로 동작하는 소스를 구할 수 있습니다. 불과 2년 전만 해도 오픈 소스로 존재하는 코드들은 여러 버그들 때문에 과연 이 칩이 올바로 동작하는 것인가하는 의심마저 들 때도 있었습니다.

본 강좌를 통해 여러 편에 걸쳐서 분석을 해 보고 싶은 그런 내용이기도 합니다.

VSCODE_LIB

MPU9250를 제어하기 위해 할당된 GPIO는 다음과 같다.

  Motor Control         GPIO

  I2C SDA               IO21
  I2C SCL               IO22
  MPU9250_INT           IO5

VSCODE_LIB

위 그림처럼 실제 iFZero 보드의 중앙에 MPU9250 1번 핀이 왼쪽 상단에 오도록 배치가 되어 있습니다.
따라서 iFBalance 로봇의 앞뒤(아래 그림에서는 좌우)로 움직이는 방향이 Pitch가 됩니다.

인디프로그 로고 보드(ToF 센서보드)가 부착되어 있는 부분이 앞쪽이 되고 스위치가 있는 부분이 뒷쪽이 되겠습니다. 따라서 Pitch(각도) 값은 앞쪽으로 기울어지게 되면 + 값이 증가되고 반대로 뒤쪽으로 기울어지면 - 값이 증가됩니다.

실제로 정 중앙이 0도가 되어야 하지만 각각의 로봇마다 무게중심이 다를 수 있습니다. 특히, 로고 보드(ToF 센서보드)나 IR 센서 보드가 제거되면 무게 중심을 다시 튜닝을 해야 합니다.
기본 제공하는 앱에는 아직 옵셋 조정 기능이 구현되어 있지 않습니다. 다음 릴리즈 버전에 추가할 예정입니다.

VSCODE_LIB

MPU9250 칩은 Invensens사가 개발했지만 현재는 Invensens회사는 TDK사에 인수가 되었습니다. 이 칩은 MPU6500(Invensens사 제품)와 AK8963(TDK사 제품)이 한 패키지에 결합된 센서입니다.

  • 6축 IMU(Inertial Measurement Unit) 센서(Accelerometers, Gyroscopes) : MPU6500
  • 3축 지자기(Magnetometers) 센서 : AK8963
  • 3.3V에서 동작
  • I2C, SPI 지원
  • Application
    • AHRS(Attitude & Heading Reference System) 혹은 MARGS(Magnetic, Angular Rate, Gravity)
    • DMP(Digital Motion Processing)
    • Digital Compass

관성항법장치( INS : Inertial Navigation System )

VSCODE_LIB
( MIT에서 개발 된 1950 년대 관성 항법 제어 장치 )
WiKi 참조 ( https://en.wikipedia.org/wiki/Inertial_navigation_system )

관성항법장치(Inertial Navigation System, INS)는 잠수함, 항공기, 미사일 등에 장착하여 자기의 위치를 감지하고 목적지까지 유도하기 위한 장치라고 합니다.

동작원리는 자이로스코프에서 방위 기준을 정하고, 가속도계를 이용하여 이동 변위를 구합니다. 처음 있던 위치를 입력하면 이동해도 자기의 위치와 속도를 항상 계산해 파악할 수 있는 방법입니다.
악천후나 전파 방해의 영향을 받지 않는다고 하는 장점을 가지지만 긴 거리를 이동하면 오차가 누적되어 커지므로 GPS나 능동 레이다 유도 등에 의한 보정을 더해 사용합니다.

실제로 90년대 초반만 해도 이러한 기능을 구현한 반도체 센서들은 수출 금지 품목이었습니다. 마이크로마우스 대회가 한창 붐이었을 때만 해도 Gyroscope 칩을 구하는 것이 하늘의 별따기 였던 때가 있었습니다.
하지만, 지금은 마우스로 클릭만해도 바로 집 앞까지 전세계의 어떤 제품이라도 배달이 되는 엄청난 신세계에 살고 있습니다.

VSCODE_LIB

iFZero 보드는 MPU9250 뿐만아니라 기압계 센서인 BMP388 칩을 내장하고 있고 iFBalance는 ToF센서도 가지고 있기 때문에 근거리 관성항법장치를 구현할 수 있습니다.

VSCODE_LIB

MPU9250 칩으로 부터 x,y,z축의 가속도 aX, aY, aZ, 각속도 ωX, ωY, ωZ, 그리고 지자기 벡터 mX, mY, mZ 를 얻을 수 있습니다. 따라서 이들 9축 성분으로 부터 센서 융합(sensor fusion)을 통해 3차원 공간상의 자세인 Roll, Pitch, Yaw 각도를 구해 AHRS를 구현할 수 있습니다.

실제로는 내부적으로는 짐볼락을 피하기 위해 오일러 공식에 의해 퀀터니언 값으로 계산을 합니다.

MPU9250 칩을 제어하는 코드는 두가지 방식이 존재합니다.

   (1) registry를 직접 읽어 오는 방법
   (2) dmp FIFO 기능을 이용하는 방법

좀 있어 보이게 구분을 했지만 하나 하나가 삽질을 하게 했던 방법들입니다.
위 두가지 버전 모두 오일러의 쿼터니언 공식을 직접 활용하고 있습니다.

1번 방법에는 kriswiner(https://github.com/kriswiner/MPU9250)가 제공하는 소스를 활용하게 되는데 Magdwick과 Mahony 알고리즘 두가지가 사용됩니다.
다만, Magdwick 알고리즘은 엄청 느리고 Mahony 알고리즘은 그나마 속도가 나옵니다만 칩사양이 다소 느리다보니 약 5 ms 정도 걸립니다.
(주 : 단, 하기 라이브러리를 사용할 경우 속도의 차이를 느낄수 없었습니다. )
물론, 이 방법은 9축 센서 데이터를 모두 활용하기 때문에 느릴 수 있습니다. 일부 드론 제품들이 실제 사용을 하고 있기 때문에 성능에 대해서는 왈가불가할 필요는 없습니다.

2번 방법은 6축 센서 데이터와 FIFO 기능을 써서 빠른 메모리 엑세스를 하다보니 속도는 조금 향상되었습니다. 약 3 ~ 4 ms 정도 걸립니다.
iFBalance 로봇을 제어하기 위해서는 MPU9250처럼 느린 IMU 장치를 사용하고서도 밸런싱을 유지할 수 있는 고도(?)의 테크닉이 필요합니다. 덩치를 좀 크게 제작했더라면 밸런싱을 유지하는데 시간적으로 여유가 있었을 텐데 높이가 낮다보니 최대 5ms 이내에서 밸런싱에 필요한 처리를 해야 합니다.

그런데 다행스럽게도 이 글을 작성하는 동안에 새로운 라이브러리를 테스트하다가 깜짝 놀랄 정도로 안정적인 라이브러리를 발견했습니다. https://github.com/hideakitai/MPU9250

VSCODE_LIB

iFBalance에 위 라이브러리를 적용한 예제입니다. 먼저 설명한 알고리즘과 한가지 중요한 차이점이 있습니다. 이 새로운 라이브러리에서는 Magdwick 알고리즘이 훨씬 안정적입니다. 차후 원인을 살펴봐야 겠습니다.

MPU9250 내부의 FIFO 메모리에 새로운 데이터가 갱신이 될 때에만 읽어올 필요가 있기 때문에 loop함수에서
mpu.update() 함수를 체크하여 TRUE이면 데이터를 읽어 옵니다. 시리얼 디버그 로그를 보시면 거의 5ms로 동작을 합니다.
iFBalance에서 밸런싱을 유지하기 위해서 사용하는 데이터는 기본적으로 Pitch 각도입니다. 추후 Yaw값도
방향을 체크하거나 실제 미세한 자세 제어를 위해 필요합니다.

사실 MPU9250을 사용한 경험이 있던 사용자라면 해당 라이브러리가 Compass 기능으로 거의 완벽하게 동작한다는 사실에 매우 놀랄 것이리고 생각합니다.
기존 스파크펀이나 기타 유명한 오픈소스들이 약간씩 코드를 꼬아 놓은 채로 오픈한 것으로 보입니다. Yaw가 흔들린다거나 일부 데이터들이 느려지거나 튀는 경향이 있습니다. 저도 얼마전에야 이러한 문제들을 수정했었는데
제가 직접 코딩한 것 보다 훨씬 깔끔한 라이브러리 코드로 예제를 정리하는 것이 좋겠다고 생각해서 내용을 다시 정리했습니다.

MPU9250에 대한 보다 자세한 설명은 추후 다시 정리하기로 하겠습니다.
일단, 기본적으로 안정한 Pitch값을 얻는 방법으로 시작하여 PID 제어에 집중하는 것이 빠르게 밸런싱 로봇을 개발하는 지름길이라고 생각합니다.

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
 /******************************************************************************
main.cpp - iFLine, iFBalance Arduino Library 
hkkim@koreaits.com ( KITS Lab )
original creation date: April 3, 2020

Development environment specifics:
Arduino IDE 1.8.x
iFBalance
******************************************************************************/

#include <Arduino.h>
#include "iFLED.h"
#include "iFMotors.h"
#include "iFEncoder.h"
#include "MPU9250.h"

#define SerialDebug   false  // Set to true to get Serial output for debugging
#define I2Cclock      400000
#define MPU_INT_PIN   5 // Interrupt Pin definitions

iFLED         ifLed;
iFMotors      ifMotors;
iFEncoder     ifEncoder;
MPU9250       mpu;

long  _nCurTime, _nPrevTime;
int   _nLoopTime;

void setup()
{ 
  Serial.begin( 921600 );

  Wire.begin(21, 22, I2Cclock);

  mpu.selectFilter( QuatFilterSel::MADGWICK);
  mpu.setup(0x68);  // change to your own address
  delay(2000);      // to get stable data of MPU9250

  ///////////////////////////////////////////////////////
  // Color LED
  ifLed.init();  
  ifLed.setMode( BLINK );

  ///////////////////////////////////////////////////////
  // Motor
  ifMotors.init();  

  ///////////////////////////////////////////////////////
  // Encoder
  ifEncoder.init(25, 14, 13, 15);  

  _nCurTime = _nPrevTime = millis();
}

void loop() {
  _nCurTime = millis();
  _nLoopTime = _nCurTime - _nPrevTime ;    

  if( _nLoopTime < 5) return;

  if (mpu.update()) {
    

    Serial.println("P = " + String(mpu.getPitch()) + " R = " + String(mpu.getRoll()) + 
                   " Y = " + String(mpu.getYaw()) + " T = " + String(_nLoopTime) );

    _nPrevTime = _nCurTime;
  }
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
P = 49.53 R = 1.56 Y = 12.83 T = 5
P = 48.35 R = 1.61 Y = 12.93 T = 5
P = 46.91 R = 1.70 Y = 13.08 T = 5
P = 45.76 R = 1.62 Y = 13.10 T = 5
P = 44.41 R = 1.61 Y = 13.18 T = 5
P = 43.34 R = 1.57 Y = 13.23 T = 5
P = 42.18 R = 1.61 Y = 13.34 T = 5
P = 41.30 R = 1.67 Y = 13.44 T = 5
P = 40.44 R = 1.54 Y = 13.43 T = 5
P = 39.84 R = 1.27 Y = 13.31 T = 5
P = 39.23 R = 0.93 Y = 13.15 T = 5
P = 38.65 R = 0.74 Y = 13.08 T = 5
P = 37.96 R = 0.69 Y = 13.10 T = 5
P = 37.40 R = 0.79 Y = 13.20 T = 5
P = 36.69 R = 0.68 Y = 13.18 T = 5
P = 35.93 R = 0.53 Y = 13.13 T = 5
P = 34.94 R = 0.55 Y = 13.17 T = 5
P = 33.99 R = 0.55 Y = 13.19 T = 5