Drone's DIYer

아두이노(Arduino) 환경에서 전형적인 MPU6050 센서 입력에 사용되는 칼만 필터(Kalman filter)의 예제입니다.

http://blog.tkjelectronics.dk/2012/09/a-practical-approach-to-kalman-filter-and-how-to-implement-it/#comment-57783

Step 1)

위 식에서 각도(angle)의 추정치 는 이전 상태의 추정치 에 bias 되지 않은 각속도(rate)에 곱하기 미소 시간 를 더한 것과 같습니다. bias 되지 않은 각속도(rate)에 곱하기 미소 시간 은 결국 드리프트 되지 않은 각도의 증분이 됩니다. 또한 좌측항은 아직 보정되지 않았음을 기억합니다.

게다가 우리는 bias를 직접 측정할 수 없기 때문에 bias의 추정치는 이전 것을 사용합니다.

이는 다음과 같이 C 언어로 쓸 수 있습니다.

Step 2)

위 식은 다음과 같이 C 언어로 쓸 수 있습니다.

Step 3)

참고로 우측에 현재 상태변수는 보정되지 않았기 때문에 angle 변수를 그대로 사용합니다.

위 식은 다음과 같이 C 언어로 쓸 수 있습니다.

Step 4)

위 식은 다음과 같이 C 언어로 쓸 수 있습니다.

Step 5)

다른 경우에 S는 행렬이 될 수 있습니다. 그 경우에는 여러분은 간단히 S로 P를 나눌 수 없으며 역행렬을 구해서 곱해야 합니다.

위 식은 다음과 같이 C 언어로 쓸 수 있습니다.

Step 6)

위 식은 다음과 같이 C 언어로 쓸 수 있습니다.

Step 7)

상태 추정 오차가 감소되었기 때문에 오차 공분산 행렬을 다시 감소시킴을 기억하세요.

C 코드는 다음과 같습니다.

참고로 대부분의 IMU에 대해서 다음의 변수들이 완벽하게 동작합니다.

초기치로서 각도를 설정하는 것을 기억하세요 왜냐하면 필터가 안정화되는데 시간이 걸리기 때문입니다. 반대로 칼만 필터가 안정화되기 전까지는 상태의 추정치를 믿을 수 없습니다.

Arduino Uno 보드는 LED가 실장되어 있으며 이는 디지털 입출력 핀 13번에 연결되어 있습니다. 이 LED를 깜박이는 예제는 이미 아두이노 IDE에서 제공되고 있으며 '파일|예제'에서 'Blink'를 선택하여 불러올 수 있으며, 다양한 예제들은 여기서 확인할 수 있습니다.

읽어들인 소스 코드인 스케치 파일은 다음과 같습니다.

int led = 13;

void setup() {

    pinMode(led, OUTPUT);

}

void loop() {

    // turn the LED on (HIGH is the voltage level)

    digitalWrite(led, HIGH);

    delay(1000); // wait for a second

    // turn the LED off by making the voltage LOW

    digitalWrite(led, LOW); 

    delay(1000); // wait for a second

}

setup() 함수 내에서 pinMode() 함수는 첫 번째 인자로 핀의 번호를, 두 번째 인자는 입출력 모드를 지정하게 되는데, 입력이면 'INPUT'을 출력이면 'OUTPUT'을 설정하게 됩니다. 그러므로 pinMode(led, OUTPUT)는 13번 핀을 출력으로 사용하겠다는 의미입니다. 참고로 아두이노에서 제공하는 함수의 문법(Syntax)이나 인자(Parameter)에 대한 자세한 설명은 아두이노 홈페이지에서 확인할 수 있습니다.

https://www.arduino.cc/en/Reference/HomePage 

사실 전통적인 표준 라이브러리 AVR libc의 문법으로는 위의 pinMode 함수는 DDRD = 0b01000000; PORTD = 0b01000000; 이들 두 문장을 대체하게 됩니다. 아두이노에서 같은 기능을 하기 위해 고유 서브함수를 호출한 반면, 표준 라이브러리는 단순한 매크로(macro)들의 사용만으로 속도나 실행 크기면에서 우수하다는 것입니다. 하지만 표준 라이브러리는 ATmega328P에 대한 전문적인 지식이 요구되어 초보자에게 결코 쉽지 않다는 것입니다.

특히 속도의 측면에서 빠른 처리나 인터럽트 처리를 요구하는 장치를 동작시키는 경우에는 아두이노는 표준 라이브러리에 비해서 상당한 열쇠에 놓이게 됩니다. AVR 칩이 C 언어를 지원함에도 불구하고 아직도 어셈블러(Assembler)로 개발하는 경우가 있는데, 이것도 같은 이유에서라는 것입니다. 하지만 점차 MCU의 속도는 끊임없이 진화하고 리소스도 풍부해지기 때문에 크게 문제는 되지 않을 것입니다.

loop() 함수내에서 digitalWrite() 함수는 첫 번째 인자로 지정된 핀에, 두 번째 인자로 HIGH 혹은 LOW 값을 쓰는데 사용하는 함수로 digitalWrite(led, HIGH)는 이미 출력으로 지정된 13번 핀에 HIGH 값을 내보냄을 의미합니다. 입출력의 지정없이 이 함수를 사용할 수는 없으며 전원 전압이 5V이면 13번 핀은 5V의 전압이 인가되어 LED를 점등하게 됩니다.

또한 delay() 함수는 입력된 시간만큼 동작을 멈추는 용도로 사용되어 delay(1000)은 1000ms 동안 지연을 줄 목적으로 사용됩니다. 여기서 단위는 ms임을 주목해야 합니다.

결국 loop() 함수 내의 동작은 13번 핀을 점등하고 1s를 기다렸다가 다시 소등하고 1s를 기다리는 코드로, loop() 함수는 영원히 반복하게 되므로 끊임 없이 LED를 1초 간격으로 점등과 소등을 반복하게 됩니다,

아두이노(Arduino) 코딩(coding)을 위해서 아두이노 통합 환경 즉, IDE(Integrated development envirionment)를 다음의 링크에서 사용자의 OS 환경에 맞게 설치하여 합니다.

https://www.arduino.cc/en/Main/Software

사실 IDE에는 2가지가 존재합니다. 첫번째는 사용자 PC에 직접 설치하는 전통적 환경이며, 두번째는 Arduino Web Editor으로 온라인 버젼입니다. 일반적인 설치 버젼은 아두이노가 업데이트 되면 업데이트 작업이 매번 필요하지만, Web Editor 버젼은 항상 아두이노 보드의 모든 라이브러리나 지원 사항을 포함하여 최신 버젼을 유지하고 클라우드에 스케치(sketch)를 저장하게 됩니다. 여기서 스케치는 자신이 코딩한 소스 파일을 의미합니다.

이외에도 사용자의 로컬 PC에서 설치 버젼을 사용시 빠른 속도와 편의기능 등이 있으에도 불구하고 온라인 버젼이 출시되는 이유는 여러 사람이 코딩이 함께 하는 협업에 유리하고 자신의 작성한 코드나 라이브러리를 공유할 수 있으며, 이러한 코드를 효율적으로 안전하게 관리할 수 있기 때문이라는 것입니다.

이는 아두이노 IDE의 첫 화면이며 여기서 프로그램의 코딩과 컴파일(compile) 그리고 아두이노 보드가 PC에 USB를 통해서 제대로 연결되었다면 아두이노 보드로의 다운로드 까지 가능하게 됩니다. 아두이노 보드를 PC에 연결하려면, USB로 아두이노 보드와 연결하고 IDE를 실행한 다음, 메뉴에서 '도구|보드'에 자신이 연결한 아두이노 보드의 종류를 선택하고, '도구|시리얼 포트'에 가상 시리얼 포트의 번호를 선택합니다. 참고로 아두이노 포트 번호는 Windows의 경우 장치관리자에서 확인할 수 있습니다.

위의 아두이노 IDE의 첫 화면에서 아래와 같이 아두이도 코딩의 프로토타입(prototype)을 보여줍니다. 사실 사용자가 이마저도 직접 써야 하지만, 아두이노는 친절하게 반드시 있어야 할 코드는 미리 써주었다는 것입니다. C 언어에 익숙한 사용자라면 프로그램은 반드시 main() 함수로 시작해야 한다고 알고 있는데, 이는 없는 것이 아니고 아두이노 IDE가 사용자 편의를 위해서 내부적으로 처리하게끔 하였다는 것입니다.

void setup() {
  // put your setup code here, to run once:

}

void loop() {
  // put your main code here, to run repeatedly:

일반적인 C 프로그램과 달리 아두이노 하드웨어 프로그램은 위의 2가지 함수인 setup()과 loop()가 반드시 필요합니다. 코딩이란 사용자가 이들 함수를 기반으로 새로운 코드나 함수를 삽입하여 원하는 기능이나 동작을 구현하는 과정이라는 것입니다. setup() 함수는 아두이노 보드의 MCU가 처음 시작할 때 단 한번 실행됩니다. loop() 함수는 setup() 함수가 실행된 이후에 영원히 반복하게 됩니다. 이런 무한 반복을 하드웨어적으로 멈추게 하는 것은 아두이노 MCU의 'RESET' 버튼을 누르는 것인데, 이때 하드웨어는 다시 초기화되고 처음 setup() 함수부터 실행하게 됩니다.

과거에는 PC DOS 응용프로그램 개발시 어떻게 메모리를 효율적으로 관리하느냐가 관건이었던 적이 있습니다. 당시에는 메모리 집적기술의 한계로 PC에 1Mbyte를 설치하였다면 큰 자랑꺼리였으며 가격도 비쌌던 시절였습니다. 그러므로 응용프로그램은 DOS가 사용하는 영역의 나머지인 상위메모리를 사용하곤 하였습니다.

하지만 근래에는 메모리가 부족하여 응용프로그램을 실행하지 못하는 경우는 거의 없습니다. CPU의 속도와 메모리 용량 그리고 대용량 하드디스크의 발전은 더이상 가독성이 떨어지는 낮은 수준(low level)의 언어 사용은 물론 엄격한 메모리 관리의 부담을 덜어주게 되었습니다.

그러므로 언제부턴가 어셈블러(ASM)나 C-언어가 아닌 사용자에 즉, 사람에게 친숙한 높은 수준(high level)의 언어인 비쥬얼베이직(Visual Basic), C++ 언어 등이 대세가 되었습니다. 그럼에도 불구하고 AVR이나 PIC계열의 마이크로콘트롤러(MCU)에서는 여전히 낮은 수준의 프로그램 언어를 사용하고 있습니다.

MCU의 속도나 메모리 용량 등과 같은 성능의 발전과 avr-gcc와 같은 컴파일러의 지속적인 향상에 힘입어 요즈음은 어셈블러보다는 C-언어를 선호하게 된 것도 사실입니다. C-언어로 작성해 굳이 오버헤드(overhead)가 있을지라도 속도와 메모리 같은 개선된 MCU 성능으로 적용하는데 크게 문제가 되지 않은다는 것입니다.

하지만 아직도 상업적인 용도의 AVR 펌웨어(firmware)는 여전히 어셈블러를 사용하고 있습니다. 고성능의 MCU를 사용하여 단가를 높이기 보다는 합리적인 성능의 자원에 펌웨어를 어셈블러로 정교하게 작성하고 최적화하여 시장 경쟁력을 얻는다는 것입니다.

그러므로 어셈블러는 적어도 한번은 다루어봐야 하는 언어로 C-언어와 비교하여 다음과 같은 장점을 갖습니다.

1) 새로운 언어를 배운다는 스트레스는 문법을 익혀야 한다는 부담일 것입니다. 하지만 어셈블러는 문법이 C-언어에 비해서 매우 간단하다는 것입니다. 어셈블러는 원하는 코드 구현 자체가 까다로운 것이지 문법을 처음 익히기는 매우 쉽다는 것입니다. 

구현이 까다롭다는 것은 하드웨어에 대해 일일이 알아야 한다는 것과 아마도 120여개에 이르는 다양한 AVR 명령어가 존재하기 때문일 것입니다. 그러나 다양한 명령어는 기능이 유사한 소위 파생된 명령어로 인하여 용도별로 분류하면 사실상 몇 종류가 되지 않으며, 필요시 그때 그때 가져다 사용하면 됩니다.

2) 어셈블러를 다루면 특히 메모리 영역과 같은 하드웨어를 자세하게 익힐 수 있으며 이러한 지식은 향후 C-언어로 구현시 최적화된 코드를 생성할 수 있는 기회를 제공한다는 것입니다.

기존의 C-언어에서는 하드웨어를 자세히 다룰 필요는 없었습니다. 왜냐하면 사용코자 하는 AVR를 지정하면 컴파일러가 알아서 해주기 때문입니다. 메모리가 부족하면 컴파일러는 이를 사용자에게 일러주고 사용자는 불필요한 변수를 삭제해주면 되었기 때문입니다.

3) 정밀한 타이밍을 요구하는 펌웨어를 작성할 수 있습니다. 

어셈블러 명령어는 1~2 사이클의 클럭을 필요로 하며 사용자는 이를 직접 보면서 다루기 때문입니다. 하지만 C-언어를 사용할 때는 해당코드가 컴파일 후에 얼마의 클럭을 요구하는지 알 수가 없고, 알아낸다 하더라도 1us 정도의 정밀한 타이밍은 사실상 불가능하다는 것입니다. 예를 들어, 브러쉬리스(brushless) 모터를 제어시에 C-언어로 작성된 펌웨어는 고속 회전 영역에서 제어 불능상태가 될 수 있다는 것입니다.

사용자가 Assembler나 C 언어를 기반으로 접근 가능한 ATmega8 칩의 메모리 구조입니다.

1) Program Memory

8 Kbyte의 비휘발성(Non-volatile) Flash Memory로 프로그램을 적재하거나 데이터를 저장하며 최대 10,000번까지 변경이 가능합니다.

* 이 영역에서는 boot 프로그램을 저장할 수 있는 공간이 따로 존재하는데 이는 직접 MCU 핀을 접속하여 프로그램 및 데이터를 변경할 수 없을 때 시리얼 통신 등으로 Flash Memory 내의 프로그램 및 데이터를 변경할 수 있게 해 줍니다.

참고: http://sharehobby.tistory.com/entry/%EB%B6%80%ED%8A%B8%EB%A1%9C%EB%8D%94%EB%9E%80

2) Data Memory

General Purpose Registers(GPRs) 그리고 I/O register와 같은 Special Function Registers(SFRs) 그리고 Internal SRAM, External SRAM 등으로 구성되며 External SRAM은 별도로 장착된 SRAM으로 MCUCR register를 설정해야 합니다. 여기서 SFRs는 사용자가 데이터를 저장하는 메모리가 아닙니다.

• General Purpose Registers(GPRs) : 32개의 register를 가지고 있으며 CPU의 연산 결과를 메모리(SRAM, Flash Memory, EEPROM)에 옮기기 전에 임시로 저장하는 장소로 접근 속도가 가장 빠르며 Assembler 아닌 C 언어를 사용하는 경우 Compiler가 register를 자동으로 관리합니다.

• Internal SRAM : 1Kbyte 내부 SRAM으로 프로그램 실행 중 변하는 변수의 값을 저장하는데 사용되며 전원이 끊기면 내용은 지워집니다(휘발성, volatile). 

• EEPROM : 512 byte의 비휘발성으로 프로그램 실행 중 변하는 변수의 값이 전원이 끊긴 이후에도 지속적으로 유지하는데 사용되며 최대 100,000번까지 변경이 가능하며 내장된 주변장치처럼 register를 통하므로 접근 속도는 느립니다.

ESC는 Electronic Speed Controller의 약자로 우리말로 '전자변속기'라고 부릅니다. ESC는 밧데리로부터 전기를 전기모터에 공급하는 장치로 멀티콥터(Multicopter)를 비롯한 각종 RC 기체에 회전 동력을 필요로 하는 곳에 사용하여 로터(rotor)를 회전시켜 양력을 얻거나, 멀티콥터의 짐벌(gimbal)에 응용하여 기계적 장치를 움직이게 합니다.

과거의 엔진기체에는 전기모터가 필요없지만 근래에는 밧데리 용량과 방전 특성이 크게 개선되어 전기모터를 동력원으로 하는 기체가 대중화되었습니다. 기존의 DC 모터를 사용하는 경우, 기계적 브러쉬(brush)로 회전자에 전기를 급전하기에 기계적 접점이 불가피하여 모터가 회전시 스파크나 소음이 발생하는 등 효율이 좋지 않고 모터의 수명 또한 단축되었습니다.

최근에는 이러한 기계적 접점을 없앤 브러쉬리스(brushless; BLDC) 모터가 등장하여 소음도 현저히 줄어들고, 반영구적이며, 효율이 개선되어 RC 기체의 정숙비행과 체공시간의 증대로 전동기체가 범람하게 되었습니다. 기존의 DC 모터를 구동하기 위해서는 스로틀(throttle)의 위치에 따라 트랜지스터(transistor)와 같은 액티브 스위치(active switch)를 PWM 형태로 개폐하므로 장치가 간단하지만 브러쉬리스 모터를 사용하는 경우 장치가 매우 복잡해지고 단가가 상승하게 됩니다.

BLDC 모터를 구동하는 ESC는 8-bit PIC나 AVR 시리즈 등의 마이컴(Micom)으로 정교하게 제어하는 방법을 채택하고, 상용 ESC의 제조사는 하드웨어를 제작하고 여기에 펌웨어(Firmware)를 적절히 튜닝하여 시판하게 됩니다. 최근에는 멀티콥터가 대중화되면서 이에 걸맞는 성능을 갖춘 예를 들어, 빠른 응답 특성을 가진 ESC를 필요로 하게 되었습니다.

이러한 요구는 전 세계적으로 ESC를 자작(DIY)하려는 매니아나 동호회를 등장시켰습니다. 대다수는 ESC는 동일 클럭에서 속도가 빠르고 내부에 A/D 컨버터나 비교기 등의 고기능을 지원하는 AVR를 사용하는데, 대부분 어셈블러(Assembler) 수준에서 펌웨어를 개발하기 때문에 일반인이 접근하기에는 쉽지 않다는 것입니다.

Wii-ESC는 멀티위(MultiWii) 등의 멀티콥터에 최적화된 ESC 펌웨어를 만들기 위한 오프 소스(open source) 펌웨어 개발 프로젝트로 전 세계적으로 여러 사람이 참여하고 있으며, 어셈블러가 아닌 C 언어 기반이므로 일반 매니아층도 펌웨어의 이해와 수정이 가능하여 자기만의 멀티콥터에 최적화된 펌웨어를 구현할 수 있다는 장점이 있습니다.

다음은 Wii-ESC 프로젝트의 링크이며 소개를 간단히 번역한 것입니다.

(번역이 원문과 상이하거나 매끄럽지 못한 부분은 댓글로 남겨 주시면 감사하겠습니다)

http://code.google.com/p/wii-esc/

About

This firmware designed as a replacement for many commercially available ESC designs based on the AVR MCU. It implements scalar sensor less method to drive Brushless Motor by detecting BEMF zero-crossing instants. The goal of this project is to create firmware most suitable to use in multi-rotors, using cheap and commercially available hardware.

이 펌웨어는 AVR MCU에 기반을 둔 많은 상용 ESC를 위한 대체용으로 개발되었습니다. 이것은 역기전력(BEMF)이 '0' 레벨을 지나가는 순간(ZC point)를 감지함으로서 BLDC 모터를 구동하는 sensorless method를 구현하였습니다. 이 프로젝트의 목표는 저렴한 상용 하드웨어를 사용하여 멀티콥터에 가장 적합한 펌웨어를 만드는 것입니다.

* Sensorless method

BLDC 모터는 센서(sensor)의 유무에 따라 크게 두 가지 구분하는데, CD-ROM 모터로 대표되는 센서를 가진 BLDC 모터는 제어기가 간편해질 수는 있으나 모터에 센서가 장착되어 단가가 올라가고, 혹한 환경에서 센서의 정밀도가 떨어지며, 모터 외부로 추가적인 배선이 요구되는 등 고장이 쉽다는 것입니다. 하지만 센서 없는 BLDC 모터는 이를 구동하는 제어기가 복잡해지는 단점은 있지만, MCU의 지속적인 성능 개선으로 얼마든지 이를 극복할 수 있으므로 근래에 보다 선호하게 되었다는 것입니다.

Features:

• Fastest possible power response.

• Up to 4000 steps of resolution.

• Low noise with comparatively high efficiency. (Sigma-delta modulator, instead of fixed frequency PWM)

• Linear power response. (completely no "bump" at 100%)

• Jitter-free input PWM measurement without harware assisted input capture.

• Accepts any PWM update rate.

• Sync recovery.

• Safe stall detection.

• Complimentary PWM support. (AKA: active freewheeling, active rectification)

• Fixed throttle end-points. No need to calibrate. (since version 2.0.9 it is also possible to calibrate end-points using stick programming procedure)

• Automatic oscillator calibration.

• Enhanced PPM filter, preventing accidental motor startup. (when FC is rebooted, for example)

• Configurable. The configuration parameters are stored in EEPROM. The Wii-ESC flash tool has visual parameters editor. No more stick programming.

• Modularity. The high-level implementation is separated from actual hardware with HAL layer.

• Portability. The firmware is written in C++, which means it can be easilly ported to different platform.