1. 오픈 소스 소프트웨어

오픈소스 플랫폼은 결국 크게는 두 가지로 분류 해볼 수 있다. Non-OS 기반의 펌웨어로 구성된 소프트웨어와 그에 맞는 FC 보드가 있고, OS(Embedded Linux, RTOS) 기반의 소프트웨어와 그에 맞는 FC 보드가 있습니다.

    1. ArduPilot(https://ardupilot.org/)

ArduPilot은 세계 최대 아마추어 자작 드론 커뮤니티 DIY Drones(http://diydrones.com)에서 2007년부터 시작된 GNU GPL v3 라이선스의 오픈 소스 드론 프로젝트이다. 하드웨어로는 APM(ArduPilot Mega), pixhawk, pixhawk2이라 하여 Arduino 기반의 하드웨어를 자체 개발하여 사용하고 있습니다. 3D Robotics와는 자작 드론 커뮤니티 설립부터 함께 해왔기 때문에 3D Robotics 제품에 ArduPilot 기술이 포함되어 커뮤니티와 함께 성장하고 있다. ArduPilot은 드론용 제어 펌웨어는 물론 APM 미션 플래너(APM Mission Planner)라고 하는 그라운드 스테이션용 프로그램도 오픈 소스로 개발/제공하고 있습니다. 뿐만 아니라 ArduPilot은 드론 외에도 일반적인 헬리콥터, 고정익 비행기, 자동차 형태의 로버도 제어할 수 있게 제작되었습니다.

소스코드 https://github.com/diydrones/ardupilot

B. 드론코드(https://www.dronecode.org)

3D Robotics, 퀄컴, Walkera, 패럿, 바이두, Intel, 유닉 등등의 1,200개 이상의 업체가 참여하며 산업용 드론을 포함한 드론 코드로 발전하였습니다. 리눅스 재단이 2014년 Ardupilot, Pixhawk를 체계화 하여 독립적인 오픈 소스, 오픈 하드웨어를 갖는 Dronecode 프로젝트의 하나로서 취리히 연방 공과대학교(ETH Zurich) 출신의 Lorenz Meier가 중심이 되어 진행 중인 자동항법 시스템으로 학계와 마니아 커뮤니티에 표준화된 자동 조종 장치를 제공하는 것을 목표로 하고 있습니다. 2007년부터 DIY Drones(http://diydrones.com)에서 오픈 소스로 진행 중인 프로젝트인 Ardupilot과 양대 산맥을 이루는 오픈 소스 프로젝트라고 볼 수 있습니다. 대표적인 하드웨어로는 Pixhawk와 같은 제어기가 있으며 거의 모든 종류의 비행기 뿐만 아니라 고급 기술이라고도 할 수 있는 수직이착륙형(VTOL) 기체에도 사용할 수 있도록 개발되었습니다.

DroneCode에서는 APM과 더불어, PX4 프로젝트도 통합할 예정으로 알려지며, Pixhawk3 이하 버전에서는 Ardupilot과 PX4가 모두 지원되었지만 Pixhawk4에서는 Ardupilot가 완벽하게 지원할지 모릅니다.

C. PX4 Autopilot(http://px4.io/)

Pixhawk 이후에 하드웨어 소프트웨어적으로 완전한 오픈 소스를 제공하는 Platform으로 다른 오픈 소스와의 가장 큰 차이점은 BSD 3-clause라는 라이선스를 사용하고 있다는 것인데, 이는 GNU GPL 류의 라이선스와는 달리 상업적으로 사용하고 수정하여도 공개할 의무가 없어 예를 들어, 퀄컴(Qualcomm)의 경우 스마트폰용 스냅드래곤 칩을 내장한 Snapdragon Flight를 내놓았으며, 강아지 목줄과 비슷한 형태의 Fotokite, 3DR사의 신제품 Solo drone, 액션 스포츠 촬영용 무인 항공기로 유명세를 치르고 있는 하늘을 나는 개 에어도그(Airdog), 연구용으로 많이 사용되는 AR. Drone 등의 보조 제어기로 사용되어집니다.

민간용 드론에는 각 사가 개발한 Embedded OS가 사용되었습니다. Embedded OS는 한정된 임무만 수행할 수 있었으며, 소프트웨어가 제각각이기 때문에 다른 드론 여러 대를 한번에 조종하는 데 한계가 있었습니다. 최근 드론이 수행하는 역할의 범위가 확대되고 소프트웨어가 관리할 센서와 부품수가 많아지면서 전용 OS의 필요성이 절실해진 상황이 되었고 개발을 진행 중에 있습니다.

소스코드 https://github.com/PX4/Firmware

D. MultiWii(http://www.multiwii.com)

MultiWii는 Multi-rotor RC 모델을 제어하기 위한 범용의 소프트웨어으로 초기에는 Nitendo Wii 콘솔의 자이로와 가속도 센서를 이용하는 것으로부터 시작되었으며 Arduino 환경에서 개발 가능한 8-bit 기반의 AVR 시리즈 마이크로컨트롤러를 사용하는 GNU GPL v3 라이선스의 오픈 소스 FC가 탄생하였습니다.

소스코드 https://code.google.com/p/multiwii

E. Afroflight32(https://github.com/multiwii/baseflight)

BaseFlight로 불리는 Multiwii에 STM32 시리즈 MCU를 채용한 32비트 펌웨어 버전입니다.

F. Cleanflight(http://cleanflight.com)

Cleanflight는 오리지날 8비트 MultiWii 코드의 32비트 버전인 BaseFlight가 이후에 다시 정리된 오픈 소스 비행제어 소프트웨어이고 레이싱 드론을 위한 소프트웨어로 자리매김 하였습니다. 그 이후 더 나아가 자이로센서의 정보를 동기화하는 기술을 접목시켜 BetaFlight를 완성하게 됩니다. 애시당초 CleanFlight에도 GPS 기능이나 RTH(Return to Home) 기능, Waypoint 기능이 있었지만 레이싱 드론의 특성상 진화의 수순이라는 것입니다. 이와 같은 기능들을 유지하고 강화한 iNav는 고정익 드론에서 널리 사용하게 되었습니다. 이외에도 연장선에서 변속기(ESC)도 32-bit STM32 시리즈를 이용한 FC 내부에 포함하는 RaceFlight가 탄생하였고 Kiss와 같은 완성도가 높은 유료 FC가 예입니다.

G. OpenPilot(http://www.openpilot.org)

OpenPilot은 일반 민간인과 연구용으로 사용할 목적으로 OpenPilot 커뮤니티에서 생성된 GNU GPL v3 라이선스의 오픈 소스 UAV Autopilot입니다. 멀티 콥터, 헬리콥터, 고정익 항공기 및 기타 차량을 위한 고성능 플랫폼입니다. 전세계 6,000여 명의 개발자가 모인 드론 OS 개발을 위한 큰 축 중 하나로 관련 기업들 중심인 드론코드와 달리 개발자 중심의 드론 OS 프로젝트입니다. 개발자들 중심으로 운영돼 커뮤니티 성격이 강하며, OS 뿐 아니라 드론 관련 하드웨어를 함께 개발하고 있습니다.

OpenPilot은 2009년에 시작된 FC 펌웨어(On-board firmware)와 지상 조종 스테이션(Ground Control Station)으로 구분해서 개발된 FC입니다. BaseFlight보다 더 다양한 확장성을 가진 LibrePilot을 탄생시켰고 로보틱스까지 다양한 하드웨어를 지원하였지만 2015년 사라지게 되었습니다. 이후 드론 연구와 드론을 이용한 연구에 적합한 소프트웨어인 TauLabs로 발전하였고 비행 본연를 즐기기 위해 오픈 소스인 dRonin이 개발되기도 하였는데, 손쉬운 설정과 함께 골치 아픈 PID 설정을 위한 자동 튜닝(Auto Tune) 기능을 자랑합니다.

H. AeroQuad(http://aeroquad.com)

AeroQuad는 GNU GPLv3 라이선스의 오픈 소스 하드웨어와 소프트웨어 프로젝트입니다. STM32 기반의 AeroQuad32 FC 보드와 AVR Arduino 기반의 보드로 구성되어 있다. 최신 버전은 2013년 1월 이후 개발이 중지된 상태입니다.

소스코드 https://github.com/AeroQuad/AeroQuad

I. Emlid(https://emlid.com)

RaspberryPi 보드에 확장보드 형태로 연결하여 Linux 기반으로 드론을 제어할 수 있도록 한 것입니다.

소스코드 https://github.com/emlid

J. Crazyflie(http://www.bitcraze.io)

GNU GPL v3 라이선스로 다른 오픈 소스 프로젝트들이 대부분 30cm 이상의 중/대형을 목적으로 만들고 있으나, Crazyfly의 경우는 Parrot의 나라 프랑스에서 2003년에 시작된 Paparazzi 등, 작은 소형 기체를 목적으로 하고 있습니다.

소스코드 https://github.com/bitcraze


2. 비 오픈 소스 소프트웨어

A. KK 보드

ATmega644PA MCU를 사용합니다.

B. NAZA

DJI사의 제품입니다.


나. Airware 드론 OS(https://www.airware.com)

Airware 비행제어 시스템은 가상의 어떤 상용 비행체에도 설치가 가능하게 하는 유연하고 확장가능한 모듈라 아키텍처 구조를 지향하는 드론 OS의 개념입니다. 그리고, 안전 민감한 비행 제어부분과는 별도로 어플리케이션 개발이 분리되어 있습니다. 구글벤처스, 인텔캐피탈, GE로부터 자금을 투자받았습니다.


4. 오픈 소스 하드웨어

가. Ardupilot 계열 콘트롤러

오픈 소스 FC 보드의 계열 중에 많이 사용하는 것 중에 하나로서, Arduino기반의 APM(ArduPilot Mega) 계열이 있고, 32비트 ARM 프로세서를 사용하는 Pixhawk 계열이 있습니다. 이들의 확장 버전으로 Dronecode가 있습니다.

나. MultiWii 계열 콘트롤러

가장 많은 사용자들이 사용하고 있는 오픈 소스 프로젝트 중의 하나입니다. 8비트 AVR MCU 기반의 CRIUS나 Flexbot이 있으며, 32비트 ARM 기반의 NAZE32가 있고, 그중 Cleanflight 펌웨어는 상당히 작고 안정된 펌웨어로 알려집니다.

다. OpenPilot 계열 콘트롤러

OpenPilot 계열은 32비트 ARM 기반의 CC3D가 있습니다.

라. Crazyflie 계열 콘트롤러

비트크레이즈사의 Crazyfly는 32비트 ARM 계열의 콘트롤러입니다.




'Radio Control > Flight Controller' 카테고리의 다른 글

비행제어기(FC)란?  (0) 2017.03.07
AutoQuad 6 비행제어기의 스펙  (0) 2015.12.02
AutoQuad 사이트에서 소소 코드를 확인하는 법  (0) 2015.12.02
AutoQuad란?  (0) 2015.11.29
APM v2.5 vs. Crius AIOP  (0) 2014.03.04
Posted by Nature & Life


STM32 Nucleo-64 개발보드는 STM32F446RE MCU를 실장하며 Arduino와 ST morpho 연결이 가능합니다. 다음은 ST사의 보드 설명과 간단한 스펙 그리고 사용자 메뉴얼입니다.


NUCLEO-F446RE

Brief specification

사용자 메뉴얼


요약하면 STM32 누클레오 보드는 사용자에게 STM32 MCU를 이용하여 새로운 아이디어와 프로토타입을 만드는데 저렴하고 유연한 방법을 제공합니다. 성능과 전력 소모 그리고 스펙의 다양한 조합의 선택해서 말입니다. 호환되는 보드로서 SMPS는 Run 모드에서 전력 소모를 크게 줄여줍니다.


Arduino Uno V3 연결이 가능하고 ST morpho 헤더는 광범위한 특성화된 쉴드 보드의 선택과 함께 STM32 누클레오 오픈 개발 플랫폼의 기능의 손쉬운 확장을 가능하게 합니다.


STM32 누클레오 보드는 별도의 프로브를 필요로 하지 않는데 이는 ST-LINK/V2-1 디버거/프로그래머를 탑재하기 때문입니다.


STM32 누클레오 보드는 다양한 패키지 형태의 소프트웨어 예제와 함께 STM32의 이해가 쉬운 소프트웨어 HAL 라이브러리와 제공되며 게다가 http://mbed.org의 Arm Mbed 온라인 리소스에 곧바로 접근할 수 있습니다.


Key Features

    • STM32 microcontroller in LQFP64 package

    • External SMPS to generate Vcore logic supply (only available on '-P' suffixed boards)

    • 1 user LED shared with Arduino™

    • 1 user and 1 reset push-buttons

    • 32.768 kHz LSE crystal oscillator

    • Board expansion connectors:

      • Arduino™ Uno V3

      • ST morpho extension pin headers for full access to all STM32 I/Os

      • External SMPS experimentation dedicated connector (only available on '-P' suffixed boards)

    • Flexible power-supply options: ST-LINK USB VBUS or external sources

    • On-board ST-LINK/V2-1 debugger/programmer with USB re-enumeration capability. Three different interfaces supported on USB: mass storage, virtual COM port and debug port

    • Comprehensive free software libraries and examples available with the STM32Cube MCU Package

    • Support of a wide choice of Integrated Development Environments (IDEs) including IAR™ , Keil® , GCC-based IDEs, Arm® Mbed™

    • Arm® Mbed Enabled™ compliant (only for some Nucleo part numbers)


NUCLEO-F446RE


위 보드는 상단과 하단의 PCB로 구분되며 상단의 MCU는 ST-LINK/V2-1 디버거/프로그래머가 구현된 부분으로, 온보드(on-board 혹은 embedded) 디버거/프로그래머라 합니다. 이 PCB 상의 USB 포트에 사용자 개발환경(IDE)이 설치된 PC와 연결되고 각 사용자 개발환경에 따라서 USB 드라이버를 설치(www.st.com)해야 합니다.


P-NUCLEO-IHM001



'Embedded Programming > STM32 Cortex-M4' 카테고리의 다른 글

NUCLEO-F446RE 보드 레이아웃  (0) 2018.01.14
NUCLEO-F446RE 보드 설정(1)  (0) 2018.01.13
STM32F4 시리즈  (1) 2018.01.13
STM32 MCU 누클레오 보드  (0) 2018.01.12
Discovery vs. Nucleo board  (1) 2018.01.11
Posted by Nature & Life


영국의 ARM사가 주도하는 mbed는 ARM-cortex기반의 MCU를 사용하여 IoT제품이나 여러 전자제품의 프로토타이핑(prototyping)을 쉽게 제작하고 Cloud 서비스 테스트까지 할수 있는 플랫폼으로, Arduino와 같은 해에 시작하고 2009년에 베타서비스를 시작했지만 2013년에야 mbed를 오픈하기로 결정하고 주변 디바이스, API, 기판 설계 데이터, 펌웨어 등을 공개하면서 주목받기 시작하였습니다.



사실 기존에는 Arduino라는 가볍고 쉬운 AVR이 존재했지만 ARM 계열에서는 Arduino Due 제외하고는 가볍고 쉬운 AVR은 없었고 대부분 전문 컴파일러를 사용하여 제작하였기 때문에 전문가가 아니면 사용하기가 어려웠었습니다. 그렇기 때문에 ARM에서도 Arduino와 같이 접근성이 좋고 빠르게 개발이 가능하도록 만든 소프트웨어가 바로 "mbed"라는 것입니다. 아직까지 국내에서는 Arduino보다 인지도가 적은 편이지만 Ardunio보다 더 좋은 성능으로 IoT개발 보드 시장 영역을 넓혀가고 있다는 것입니다.


참고로 ARM사의 cortex-M4를 MCU의 경우에는 IoT에서 가장 중요한 화두인 전력문제에 있어서 저전력기술을 활용함으로써, 100~180MHz로 동작하는 동안 매우 낮은 동적 전력 사용량을 제공하며, 경쟁사 유사제품에 비해 7배 낮은 정적 소비 전력을 보여줍니다. 


웹브라우저를 통한 온라인 컴파일 및 소스 버전 관리 기능(Web-IDE)을 제공하여 어떤 OS에서든 웹브라우저에서 온라인으로 컴파일이 가능하고, 프로그램 업로드는 별도 장비없이 USB에 연결만 하면 가능하도록 되어 있다는 것입니다. 게다가 커뮤니티를 통한 라이브러리 공개 및 방대한 개인 위키 페이지 제공으로 협업에 유익하고 무엇보다, 모든 사용자가 기본적으로 같은 하드웨어를 이용하고 있기에 확장 보드가 아닌 이상 바로 적용 가능하다는 것입니다.



mbed의 장점으로는 기본적으로 MCU의 성능이 Arduino보다 좋기 때문에 고성능으로 더 높은 사양대를 커버할 수 있지만, Ardunio에 비해 상대적으로 사용자가 적다는 단점이 있습니다. 온라인 컴파일러에서 브레이크 포인터를 사용하여 스텝별 실행 및 내부 레지스터, 변수 등의 디버깅은 불가능하고 프로그램 업로드 방식에 있어서도 파일을 추출하고 이 파일을 다시 보드에 업로드 시켜야하는 상대적으로 Arduino에 비해 귀찮은 과정도 단점으로 여겨질 수 있습니다.


Arduino와 mbed 차이점으로, 모든 Arduino는 Atmel MCU 사용하여 작은 메모리와 제한된 기능을 가지고 있으며 느리다는 것입니다. 대부분의 Arduino 보드들은 ATmega328이고, Mega보드는 ATmega2560이며, 새로운 Due보드는 ARM Cortex-M3를 사용한다. 반면에 mbed 플랫폼은 더 빠르고, 메모리도 크고, 더 많은 기능을 가진 ARM Cortex MCU를 사용합니다. 사실 공식적인 mbed 플랫폼들은 Cortex-M0, M3와 M4를 기반으로 만들어진다는 것입니다.


프로토타입을 만드는데는 둘 다 우수하지만 프로토타입을 만든 후에 생산을 하고자 할 때는 mbed가 더 scalable한 플랫폼이기 때문에 더 낮고, 아주 간단한 응용제품을 제외하고는 ARM MCU들이 같거나 더 싼 가격에 더 낮은 전력 소모를 하며, 더 많은 기능을 가지고 있기 때문에 그 입지가 점점 좁아지고 있다는 것입니다.



Posted by Nature & Life


MPU6050 센서는 가속도계(Accelerometer)와 자이로(Gyroscope)가 1개의 칩에 모두 포함하고 있는 6DOF(Degrees of Freedom) MEMS(Micro Electro Mechanical Systems) 센서로, I2C(Inter Integrated Circuit) 통신 프로토콜을 통해서 데이터를 가져올 수 있습니다. 다음은 MPU6050의 메뉴얼입니다.


http://www.invensense.com/wp-content/uploads/2015/02/MPU-6000-Datasheet1.pdf



MPU6050의 특징입니다.

      • 3-axis Accel + 3-axis Gyroscope DMP(Digital Motion Processor)

      • ±1% Temperature Sensor( Digital output )

      • 7개의 16bit ADC를 내장하여 16bit 정교한 기울기 출력

      • ±250,±500,±1000,±2000(˚/sec) dps 자이로, ±2,±4,±8,±16g 가속도 (User programable)

      • 1024byte fifo buffer

      • I2C 400KHz

      • Programable Interrupts

      • High-G Interrupts

      • VDD: 2.375V ~ 3.46V

      • Gyroscope Operating current 3.6mA( Standby 5uA )

      • Accelerometer Operating current 500uA( Low Power mode 10uA~ )

      • Programable Low-Pass Filter

      • -40℃ ~ +85℃ (TA 25℃ )

      • StartUp time 30msec

      • Self Test

      • I2C Address : 0x68 ( except R/W 0x1 )

      • I2C Master or Slave

      • Auxiliary I2C bus for communicating to an off-chip 3-Axis digital output magnetometer(지자계 센서) or other sensors.


MPU6050은 24pin QFN 패키지로 내부 timing generator를 사용하지 않는 경우 외장 32.768kHz 혹은 19.2MHz 클럭이 필요합니다. 따라서 사용이 용이하도록 주변 소자를 내장한 MPU6050의 Breakout 보드가 있으며 GY-521의 그 중의 하나입니다.


GY-521의 회로도


GY-521 모듈

(가속도계와 자이로의 방향은 MPU6050 칩의 1번 핀을 기준으로 3차원 축이 결정되며 MPU6050과 동일함을 알 수 있습니다)


MPU6050(GY-521)은 가속도계를 포함하므로 가속도를 측정하는 센서는 아닙니다. 단지 가속도를 이용하여 3차원 공간상 X, Y 그리고 Z 축을 중심으로 기울어진 각도(기울기)를 얻는 센서입니다. 가속도는 중력 방향과의 반대 방향일 때 양(+)이고 아래에서 각 축에 곡선 화살표는 자이로의 회전방향을 나타냅니다. 가속도계와 자이로 외에도 온도도 측정할 수 있는데 이는 이와같은 센서가 온도에 따라 약간 변화하기 때문에 이를 보정하는 목적으로 제공합니다.


만일 MPU6050을 비행기에 탑재하고 진행방향이 Y축 방향과 같다면 가속도계 출력 AccX는 롤(Roll), AccY는 피치(Pitch)그리고 AccZ는 요(Yaw)가 됩니다. 즉 AccX는 X축을 기준으로 기울어진 각도를 의미합니다. 가속도계는 X, Y축에 대해서 기울어진 정도를 중력가속도[g]의 단위로 출력합니다. 이때 기준 방향은 중력방향입니다. 그러나 Z축이 중력방향이 일치하는 경우 요를 구할 수 없다는 것입니다.


만일 움직이지 않고 이동하는 경우 진행 방향의 가속도의 영향으로 중력 방향이 변하게 되어 부정확하게 된다는 것입니다. 이러한 이유로 자이로의 측정 결과를 참조하게 되는데, 자이로는 짧은 시간은 정확하기 때문입니다. 하지만 긴 시간에 대해서는 자이로 센서가 측정시 함유하는 잡음 등을 각속도를 적분하여 기울기를 얻기 때문에 적분하는데, 이 과정에서 오차(적분상수)는 누적되고 시간에 따라 자이로 측정값은 드리프트하게 됩니다. 이때 변화분을 bias라고도 부릅니다.



위 그림에서 X축을 중심으로 회전한 각도 φ와 Y축을 중심으로 회전한 각도 ρ의 계산식입니다. 만일 X축 자체가 기울어지지 않았다면, 중력이 X축 상에 기여도는 없어 φ는 arctan(Ay/Az)으로 간략하게 됩니다. 여기서 Ax, Ay, Az는 AccX, AccY, AccZ입니다. ρ에서 음의 부호는 X축 중심으로 회전각도는 Y축이 위쪽으로 기울어져야 양이지만, Y축 중심으로 회전각도는 X축이 아래로 기울어져야 양이기 때문입니다.


MPU6050을 사용해 실시간으로 기울기를 요구하는 시스템은 가속도계의 측정값과 자이로의 측정값을 적절히 잡음을 고려하여 융합하고 최적의 가장 정확한 기울기를 얻어냅니다. 이때 사용하는 필터는 보상필터(Complementary filter; 혹은 상보필터)와 칼만필터(Kalman filter)로 알려집니다.


MPU6050은 7개의 채널에 대해서 16bit 크기의 값을 출력해주는 고성능 ADC를 내장하므로 각 축의 센서 출력값에 대해서 int16_t(-32768~32767)의 자료형으로 접근해야 합니다. 또한 MPU6050은 update rate(sampling time)이 가속도, 자이로에 대해서 각각 4~1000Hz, 4~8000Hz으로 출력값을 제공합니다.


MPU6050은 내부 레지스터를 이용해서 출력 값의 범위를 조정할 수 있습니다. 예를 들어 가속도계에서 AFS_SEL=0으로 설정함으로써 출력은 ±2[g]까지 나타낼 수 있으며 이를 2byte 크기로 나타내게 됩니다. 만일 AFS_SEL=0과 FS_SEL=0을 설정하였다면 다음과 같습니다.



가속도계에서는 최대 ±2[g]이고(-2g에서 +2g까지 측정하여 -32768에서 +32767까지 매핑Scale Factor가 1g당 16,384로 출력에 이를 나누어주면 실제 [g] 단위를 얻을 수 있습니다. 그러나 우리가 원하는 것은 기울어지 각도이므로 arctan에서는 비율(ratio)만을 사용하므로 단위는 의미가 없게 됩니다. 자이로에서는 최대 ±250[deg/s]이고(-250에서 +250까지 측정하여 -32768에서 32767까지 매핑Scale Factor가 131(32767/250)로 출력에 이를 나누어주면 실제 [deg/s] 단위의 각가속도를 얻을 수 있습니다. 각 센서는 감도(Sensitivity)를 증가시킬수록 미세하게 측정 가능하지만 정확도는 떨어집니다.


다음은 아두이노(Arduino) 보드와 GY-521(MPU6050) 모듈과의 연결 방법과 Wire 라이브러리를 이용하여 실행한 MPU6050의 데이터 출력의 예제입니다.


아두이노 보드와 연결 방법



// 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 라이브러리 초기화
  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); 
}




'Flight Controller 이해 > 센서' 카테고리의 다른 글

관성측정장치(IMU)의 원리  (0) 2017.03.14
드론에 요구되는 각종 센서들  (0) 2017.02.26
Posted by Nature & Life
Embedded Lecture/Arduino2017. 11. 30. 18:04


아두이노에서 기본적으로 제공하는 Wire 라이브러리를 이용해도 되지만 I2C 라이브러리를 이용하여 MPU6050의 X축, Y축 그리고 Z축의 기울기를 구하였습니다.


i2c.ino

const uint8_t IMUAddress = 0x68; // AD0 is logic low on the PCB

const uint16_t I2C_TIMEOUT = 1000; // Used to check for errors in I2C communication


uint8_t i2cWrite(uint8_t registerAddress, uint8_t data, bool sendStop) {

  return i2cWrite(registerAddress,&data,1,sendStop); // Returns 0 on success

}


uint8_t i2cWrite(uint8_t registerAddress, uint8_t* data, uint8_t length, bool sendStop) {

  Wire.beginTransmission(IMUAddress);

  Wire.write(registerAddress);

  Wire.write(data, length);

  return Wire.endTransmission(sendStop); // Returns 0 on success

}


uint8_t i2cRead(uint8_t registerAddress, uint8_t* data, uint8_t nbytes) {

  uint32_t timeOutTimer;

  Wire.beginTransmission(IMUAddress);

  Wire.write(registerAddress);

  if(Wire.endTransmission(false)) // Don't release the bus

    return 1; // Error in communication

  Wire.requestFrom(IMUAddress, nbytes,(uint8_t)true); // Send a repeated start and then release the bus after reading

  for(uint8_t i = 0; i < nbytes; i++) {

    if(Wire.available())

      data[i] = Wire.read();

    else {

      timeOutTimer = micros();

      while(((micros() - timeOutTimer) < I2C_TIMEOUT) && !Wire.available());

      if(Wire.available())

        data[i] = Wire.read();

      else

        return 2; // Error in communication

    }

  }

  return 0; // Success

}


Kalman.h

/* Copyright (C) 2012 Kristian Lauszus, TKJ Electronics-> All rights reserved->

 This software may be distributed and modified under the terms of the GNU

 General Public License version 2 (GPL2) as published by the Free Software

 Foundation and appearing in the file GPL2->TXT included in the packaging of

 this file-> Please note that GPL2 Section 2[b] requires that all works based

 on this software must also be made publicly available under the terms of

 the GPL2 ("Copyleft")->


 Contact information

 -------------------

 Kristian Lauszus, TKJ Electronics

 Web      :  http://www->tkjelectronics->com

 e-mail   :  kristianl@tkjelectronics->com

 */


#ifndef _Kalman_h

#define _Kalman_h


struct Kalman {

    /* Kalman filter variables */

    double Q_angle; // Process noise variance for the accelerometer

    double Q_bias; // Process noise variance for the gyro bias

    double R_measure; // Measurement noise variance - this is actually the variance of the measurement noise


    double angle; // The angle calculated by the Kalman filter - part of the 2x1 state vector

    double bias; // The gyro bias calculated by the Kalman filter - part of the 2x1 state vector

    double rate; // Unbiased rate calculated from the rate and the calculated bias - you have to call getAngle to update the rate


    double P[2][2]; // Error covariance matrix - This is a 2x2 matrix

    double K[2]; // Kalman gain - This is a 2x1 vector

    double y; // Angle difference

    double S; // Estimate error

};


void Init(struct Kalman* klm){

    /* We will set the variables like so, these can also be tuned by the user */

    klm->Q_angle = 0.001;

    klm->Q_bias = 0.003;

    klm->R_measure = 0.03;


    klm->angle = 0; // Reset the angle

    klm->bias = 0; // Reset bias

   klm->P[0][0] = 0; // Since we assume that the bias is 0 and we know the starting angle (use setAngle), the error covariance matrix is set like so - see: http://en->wikipedia->org/wiki/Kalman_filter#Example_application->2C_technical

    klm->P[0][1] = 0;

    klm->P[1][0] = 0;

    klm->P[1][1] = 0;

}


// The angle should be in degrees and the rate should be in degrees per second and the delta time in seconds

double getAngle(struct Kalman * klm, double newAngle, double newRate, double dt) {

    // KasBot V2  -  Kalman filter module - http://www->x-firm->com/?page_id=145

    // Modified by Kristian Lauszus

   // See my blog post for more information: http://blog->tkjelectronics->dk/2012/09/a-practical-approach-to-kalman-filter-and-how-to-implement-it


    float P00_temp;

    float P01_temp;


    // Discrete Kalman filter time update equations - Time Update ("Predict")

    // Update xhat - Project the state ahead


   /* Step 1 */

    klm->rate = newRate - klm->bias;

    klm->angle += dt * klm->rate;


    // Update estimation error covariance - Project the error covariance ahead


    /* Step 2 */

    klm->P[0][0] += dt * (dt*klm->P[1][1] - klm->P[0][1] - klm->P[1][0] + klm->Q_angle);

    klm->P[0][1] -= dt * klm->P[1][1];

    klm->P[1][0] -= dt * klm->P[1][1];

    klm->P[1][1] += klm->Q_bias * dt;


    // Discrete Kalman filter measurement update equations - Measurement Update ("Correct")

    // Calculate Kalman gain - Compute the Kalman gain


    /* Step 4 */

    klm->S = klm->P[0][0] + klm->R_measure;


    /* Step 5 */

    klm->K[0] = klm->P[0][0] / klm->S;

    klm->K[1] = klm->P[1][0] / klm->S;


    // Calculate angle and bias - Update estimate with measurement zk (newAngle)


    /* Step 3 */

    klm->y = newAngle - klm->angle;

 

   /* Step 6 */

    klm->angle += klm->K[0] * klm->y;

    klm->bias += klm->K[1] * klm->y;


    // Calculate estimation error covariance - Update the error covariance


    /* Step 7 */

    P00_temp = klm->P[0][0];

    P01_temp = klm->P[0][1];


    klm->P[0][0] -= klm->K[0] * P00_temp;

    klm->P[0][1] -= klm->K[0] * P01_temp;

    klm->P[1][0] -= klm->K[1] * P00_temp;

    klm->P[1][1] -= klm->K[1] * P01_temp;

 

   return klm->angle;

}

// Used to set angle, this should be set as the starting angle

void setAngle(struct Kalman* klm, double newAngle) { klm->angle = newAngle; }


// Return the unbiased rate

double getRate(struct Kalman* klm) { return klm->rate; }

 

/* These are used to tune the Kalman filter */

void setQangle(struct Kalman* klm, double newQ_angle) { klm->Q_angle = newQ_angle; }


/* Default value is (0.003f), raise this to follow input more closely, lower this to smooth result of kalman filter */

void setQbias(struct Kalman* klm, double newQ_bias) { klm->Q_bias = newQ_bias; }

void setRmeasure(struct Kalman* klm, double newR_measure) { klm->R_measure = newR_measure; }

double getQangle(struct Kalman* klm) { return klm->Q_angle; }

double getQbias(struct Kalman* klm) { return klm->Q_bias; }

double getRmeasure(struct Kalman* klm) { return klm->R_measure; }

#endif


mpu6050.ino

/* Copyright (C) 2012 Kristian Lauszus, TKJ Electronics. All rights reserved.

This software may be distributed and modified under the terms of the GNU

General Public License version 2 (GPL2) as published by the Free Software

Foundation and appearing in the file GPL2.TXT included in the packaging of

this file. Please note that GPL2 Section 2[b] requires that all works based

on this software must also be made publicly available under the terms of

the GPL2 ("Copyleft").

Contact information

-------------------

Kristian Lauszus, TKJ Electronics

Web : http://www.tkjelectronics.com

e-mail : kristianl@tkjelectronics.com

Updated by Joe YOON in 2017.12.02

*/


#include <Wire.h>

#include "Kalman.h"


struct kalman Kal_struct_X, Kal_struct_Y; // Create the Kalman instances


/* IMU Data */

int16_t accX, accY, accZ; // 3-axis accelerometer

int16_t tempRaw;

int16_t gyroX, gyroY, gyroZ; // 3-axis gyroscope


double accXangle, accYangle; // Angle calculate using the accelerometer

double temp; // Temperature

double gyroXangle, gyroYangle; // Rate calculate using the gyro

double compAngleX, compAngleY; // Calculate the angle using a complementary filter

double kalAngleX, kalAngleY; // Calculate the angle using a Kalman filter


uint32_t timer;

uint8_t i2cData[14]; // Buffer for I2C data


void setup() {

  Init(&Kal_struct_X); // Initialize Kalman filter for X-axis

  Init(&Kal_struct_Y); // Initialize Kalman filter for Y-axis

  Serial.begin(9600);

  Wire.begin();

  i2cData[0] = 7; // Set the sample rate to 1000Hz - 8kHz/(7+1) = 1000Hz

  i2cData[1] = 0x00; // Disable FSYNC and set 260 Hz Acc filtering, 256 Hz Gyro filtering, 8 KHz sampling

  i2cData[2] = 0x00; // Set Gyro Full Scale Range to +/-250[deg/s]

  i2cData[3] = 0x00; // Set Accelerometer Full Scale Range to +/-2[g]

  while(i2cWrite(0x19,i2cData,4,false)); // Write to all four registers at once

  while(i2cWrite(0x6B,0x01,true)); // PLL with X axis gyroscope reference and disable sleep mode

  while(i2cRead(0x75,i2cData,1));

  if(i2cData[0] != 0x68) { // Read "WHO_AM_I" register

    Serial.print(F("Error reading sensor"));

    while(1);

  }

  

  delay(100); // Wait for sensor to stabilize

  

  /* Set kalman and gyro starting angle */

  while(i2cRead(0x3B,i2cData,6));

  accX = ((i2cData[0] << 8) | i2cData[1]);

  accY = ((i2cData[2] << 8) | i2cData[3]);

  accZ = ((i2cData[4] << 8) | i2cData[5]);


  // atan2 outputs the value of -? to ? (radians) - see http://en.wikipedia.org/wiki/Atan2

  // We then convert it to 0 to 2? and then from radians to degrees

  accYangle = atan2(-1*accX/sqrt(pow(accY,2) + pow(accZ,2)))*RAD_TO_DEG;

  accXangle = atan2(accY/sqrt(pow(accX,2) + pow(accZ,2)))*RAD_TO_DEG;

   

  setAngle(&Kal_struct_X, accXangle); // Set starting angle

  setAngle(&Kal_struct_Y, accYangle);


  gyroXangle = accXangle;

  gyroYangle = accYangle;

  compAngleX = accXangle;

  compAngleY = accYangle;

  

  timer = micros();

}


void loop() {

  /* Update all the values */

  while(i2cRead(0x3B,i2cData,14));

  accX = ((i2cData[0] << 8) | i2cData[1]);

  accY = ((i2cData[2] << 8) | i2cData[3]);

  accZ = ((i2cData[4] << 8) | i2cData[5]);

  tempRaw = ((i2cData[6] << 8) | i2cData[7]);

  gyroX = ((i2cData[8] << 8) | i2cData[9]);

  gyroY = ((i2cData[10] << 8) | i2cData[11]);

  gyroZ = ((i2cData[12] << 8) | i2cData[13]);

  

  // atan2 outputs the value of -? to ? (radians) - see http://en.wikipedia.org/wiki/Atan2

  // We then convert it to 0 to 2? and then from radians to degrees

  accYangle = atan2(-1*accX/sqrt(pow(accY,2) + pow(accZ,2)))*RAD_TO_DEG;

  accXangle = atan2(accY/sqrt(pow(accX,2) + pow(accZ,2)))*RAD_TO_DEG;

   

  double gyroXrate = (double)gyroX/131.0;

  double gyroYrate = -((double)gyroY/131.0);

  gyroXangle += gyroXrate*((double)(micros()-timer)/1000000); // Calculate gyro angle without any filter

  gyroYangle += gyroYrate*((double)(micros()-timer)/1000000);

  //gyroXangle += kalmanX.getRate()*((double)(micros()-timer)/1000000); // Calculate gyro angle using the unbiased rate

  //gyroYangle += kalmanY.getRate()*((double)(micros()-timer)/1000000);

  

  compAngleX = (0.93*(compAngleX+(gyroXrate*(double)(micros()-timer)/1000000)))+(0.07*accXangle); // Calculate the angle using a Complimentary filter

  compAngleY = (0.93*(compAngleY+(gyroYrate*(double)(micros()-timer)/1000000)))+(0.07*accYangle);

  

  kalAngleX = getAngle(&Kal_struct_X, accXanglegyroXrate(double)(micros()-timer)/1000000); // Calculate the angle using a Kalman filter

  kalAngleY = getAngle(&Kal_struct_Y, accYanglegyroYrate(double)(micros()-timer)/1000000);

  timer = micros();

  

  temp = ((double)tempRaw + 12412.0) / 340.0;

  

  /* Print Data */

  display_formatted_float(accX, 5, 0, 3, false);

  display_formatted_float(accY, 5, 0, 3, false);

  display_formatted_float(accZ, 5, 0, 3, false);

  display_formatted_float(gyroX, 5, 0, 3, false);

  display_formatted_float(gyroY, 5, 0, 3, false);

  display_formatted_float(gyroZ, 5, 0, 3, false);


  Serial.print("\t");


  display_formatted_float(accXangle, 5, 2, 3, false);

  display_formatted_float(gyroXangle, 5, 2, 3, false);

  display_formatted_float(compAngleX, 5, 2, 3, false);

  display_formatted_float(kalAngleX, 5, 2, 3, false);


  Serial.print("\t");


  display_formatted_float(accYangle, 5, 2, 3, false);

  display_formatted_float(gyroYangle, 5, 2, 3, false);

  display_formatted_float(compAngleY, 5, 2, 3, false);

  display_formatted_float(kalAngleY, 5, 2, 3, false);


  //Serial.print(temp);Serial.print("\t");


  Serial.print("\r\n");

  delay(1);

}


void display_formatted_float(double val, int characteristic, int mantissa, int blank, boolean linefeed) {

  char outString[16];

  int len;


  dtostrf(val, characteristic, mantissa, outString);

  len = strlen(outString);

  for(int i = 0; i < ((characteristic+mantissa+blank)-len); i++) Serial.print(F(" "));

  Serial.print(outString);

  if(linefeed)

    Serial.print(F("\n"));

}


Posted by Nature & Life


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

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


또한 이 칼만 필터는 가속도계(accelerometer) 혹은 지자계(magnetometer)와 그리고 자이로(gyroscope)로부터 각도(angle), 각속도(rate) 그리고 bias를 계산하는데 사용될 수 있습니다(C++ version).

https://github.com/TKJElectronics/KalmanFilter


다음은 C version입니다.

http://www.cnblogs.com/zjutlitao/p/3915786.html



Kalman.h

/* Copyright (C) 2012 Kristian Lauszus, TKJ Electronics-> All rights reserved->


 This software may be distributed and modified under the terms of the GNU

 General Public License version 2 (GPL2) as published by the Free Software

 Foundation and appearing in the file GPL2->TXT included in the packaging of

 this file-> Please note that GPL2 Section 2[b] requires that all works based

 on this software must also be made publicly available under the terms of

 the GPL2 ("Copyleft")->


 Contact information

 -------------------


 Kristian Lauszus, TKJ Electronics

 Web      :  http://www->tkjelectronics->com

 e-mail   :  kristianl@tkjelectronics->com

 */


#ifndef _Kalman_h

#define _Kalman_h

struct Kalman {

    /* Kalman filter variables */

    double Q_angle; // Process noise variance for the accelerometer

    double Q_bias; // Process noise variance for the gyro bias

    double R_measure; // Measurement noise variance - this is actually the variance of the measurement noise


    double angle; // The angle calculated by the Kalman filter - part of the 2x1 state vector

    double bias; // The gyro bias calculated by the Kalman filter - part of the 2x1 state vector

    double rate; // Unbiased rate calculated from the rate and the calculated bias - you have to call getAngle to update the rate


    double P[2][2]; // Error covariance matrix - This is a 2x2 matrix

    double K[2]; // Kalman gain - This is a 2x1 vector

    double y; // Angle difference

    double S; // Estimate error

};


void Init(struct Kalman* klm){

    /* We will set the variables like so, these can also be tuned by the user */

    klm->Q_angle = 0.001;

    klm->Q_bias = 0.003;

    klm->R_measure = 0.03;


    klm->angle = 0; // Reset the angle

    klm->bias = 0; // Reset bias

   klm->P[0][0] = 0; // Since we assume that the bias is 0 and we know the starting angle (use setAngle), the error covariance matrix is set like so - see: http://en->wikipedia->org/wiki/Kalman_filter#Example_application->2C_technical

    klm->P[0][1] = 0;

    klm->P[1][0] = 0;

    klm->P[1][1] = 0;

}


// The angle should be in degrees and the rate should be in degrees per second and the delta time in seconds

double getAngle(struct Kalman * klm, double newAngle, double newRate, double dt) {

    // KasBot V2  -  Kalman filter module - http://www->x-firm->com/?page_id=145

    // Modified by Kristian Lauszus

   // See my blog post for more information: http://blog->tkjelectronics->dk/2012/09/a-practical-approach-to-kalman-filter-and-how-to-implement-it


    float P00_temp;

    float P01_temp;


    // Discrete Kalman filter time update equations - Time Update ("Predict")

    // Update xhat - Project the state ahead

    /* Step 1 */

    klm->rate = newRate - klm->bias;

    klm->angle += dt * klm->rate;

    

    // Update estimation error covariance - Project the error covariance ahead

    /* Step 2 */

    klm->P[0][0] += dt * (dt*klm->P[1][1] - klm->P[0][1] - klm->P[1][0] + klm->Q_angle);

    klm->P[0][1] -= dt * klm->P[1][1];

    klm->P[1][0] -= dt * klm->P[1][1];

    klm->P[1][1] += klm->Q_bias * dt;

    

    // Discrete Kalman filter measurement update equations - Measurement Update ("Correct")

    // Calculate Kalman gain - Compute the Kalman gain

    /* Step 4 */

    klm->S = klm->P[0][0] + klm->R_measure;


    /* Step 5 */

    klm->K[0] = klm->P[0][0] / klm->S;

    klm->K[1] = klm->P[1][0] / klm->S;

    

    // Calculate angle and bias - Update estimate with measurement zk (newAngle)

    /* Step 3 */

    klm->y = newAngle - klm->angle;


    /* Step 6 */

    klm->angle += klm->K[0] * klm->y;

    klm->bias += klm->K[1] * klm->y;

    

    // Calculate estimation error covariance - Update the error covariance

    /* Step 7 */

   P00_temp = klm->P[0][0];

   P01_temp = klm->P[0][1];


    klm->P[0][0] -= klm->K[0] * P00_temp;

    klm->P[0][1] -= klm->K[0] * P01_temp;

    klm->P[1][0] -= klm->K[1] * P00_temp;

    klm->P[1][1] -= klm->K[1] * P01_temp;


    return klm->angle;

}


 // Used to set angle, this should be set as the starting angle

void setAngle(struct Kalman* klm, double newAngle) { klm->angle = newAngle; }

// Return the unbiased rate

double getRate(struct Kalman* klm) { return klm->rate; }
 

/* These are used to tune the Kalman filter */

void setQangle(struct Kalman* klm, double newQ_angle) { klm->Q_angle = newQ_angle; }


/* Default value is (0.003f), raise this to follow input more closely, lower this to smooth result of kalman filter */

void setQbias(struct Kalman* klm, double newQ_bias) { klm->Q_bias = newQ_bias; }


void setRmeasure(struct Kalman* klm, double newR_measure) { klm->R_measure = newR_measure; }

double getQangle(struct Kalman* klm) { return klm->Q_angle; }

double getQbias(struct Kalman* klm) { return klm->Q_bias; }

double getRmeasure(struct Kalman* klm) { return klm->R_measure; }


#endif


Kalman.h



Posted by Nature & Life


아두이노(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 언어로 쓸 수 있습니다.

rate = newRate - bias;

angle += dt * rate;


Step 2)



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

P[0][0] += dt * (dt*P[1][1] - P[0][1] - P[1][0] + Q_angle);

P[0][1] -= dt * P[1][1];

P[1][0] -= dt * P[1][1];

P[1][1] += Q_gyroBias * dt;


Step 3)



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

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


y = newAngle - angle;


Step 4)




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

S = P[0][0] + R_measure;


Step 5)



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

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

K[0] = P[0][0] / S;

K[1] = P[1][0] / S;


Step 6)



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

angle += K[0] * y;

bias += K[1] * y;


Step 7)



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

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

float P00_temp = P[0][0];

float P01_temp = P[0][1];


P[0][0] -= K[0] * P00_temp;

P[0][1] -= K[0] * P01_temp;

P[1][0] -= K[1] * P00_temp;

P[1][1] -= K[1] * P01_temp;


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

float Q_angle = 0.001;

float Q_gyroBias = 0.003;

float R_measure = 0.03;


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



Posted by Nature & Life


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

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


한편 의 관측 혹은 측정을 위한 식은 다음과 같이 주어집니다.



위 식에서와 같이 는 현재 상태 가  행렬과 곱하여지고 측정 잡음 와 합하여 집니다.


는 관측 모델로 불려지며 실제 상태 공간을 관측된 상태의 공간으로 매핑하기 위해서 사용되어집니다. 실제 상태는 관측될 수 없는데 이는 측정이 단지 가속도계로부터의 측정에 불과하기 때문입니다.


는 다음과 같습니다.



측정 잡음은 게다가 '0'의 평균과 다음과 같이 의 공분산을 가져야 합니다. 



그러나 은 행렬이 아니기 때문에 측정 잡음은 단지 측정값의 분산과 같습니다. 왜냐하면 같은 변수의 공분산은 분산과 같기 때문입니다. 그러므로 우리는 R을 다음과 같이 쓸 수 있습니다.



우리는 측정 잡음은 동일하고 시간 k에 의존하지 않는다고 가정합니다.



만일 여러분이 측정 잡음 분산 을 너무 높게 설정하면 필터는 너무 늦게 응답하는데 이는 새로운 측정값을 덜 믿기 때문이며, 반면에 너무 낮게 설정한다면 값은 오버슈트(overshoot)가 발생하고 잡음이 많은데 이는 우리가 가속도계 측정값을 너무 많이 믿기 때문임을 참고하세요.


그래서 여러분은 프로세스 잡음 분산 와  그리고 측정 잡음 의 측정 분산을 찾아야만 합니다.


Time Update("Predict")


처음 2개의 방정식에서 시간 k에서 우리는 현재 상태와 오차 공분산 행렬을 예측할 것입니다. 우선 필터는 모든 이전 상태들과 자이로 측정으로부터 근거 된 현재 상태를 추정할 것입니다.



위 식의 우측 두번째 항을 제어 입력이라고 부르는 이유는 우리가 현재 시간 k에서 상태를 추정하기 위해 이를 추가적인 입력으로 사용했기 때문입니다. 다음은 우리가 이전 오차 공부산 행렬 에 근거하여 다음에 정의된 것처럼 오차 공분산 행렬 을 추정할 것입니다.



이 행렬은 추정된 상태의 현재 값을 얼마나 많이 신뢰하는가를 추정하기 위해서 사용되어집니다. 작을수록 우리는 좀더 현재 추정된 상태를 신뢰하게 됩니다. 위 방정식의 원리는 실제로 이해하기에 매우 쉽습니다. 이는 우리가 상태의 추정을 마지막 갱신한 후에 오차 공분산은 증가하는 것이 매우 분명하기 때문입니다. 그러므로 우리는 오차 공분산 행렬을 상태 전이 모델 과 그 행렬의 전치행렬 을 곱하고 시간 k에서 현재 프로세스 잡음 를 더합니다.


오차 공분산 행렬 는 2x2 행렬입니다.



Measurement Update("Correct")


우리가 계산해야 할 첫번째 일은 측정치 와 보정되지 않은 이전 상태로부터 예측된 상태 변수 사이에 차를 계산하는 것입니다.



여기서 관측 모델 는 이전 상태 추정치를 관측된 공간과 매핑하게 위해서 사용되어집니다. 관측 공간이라 함은 가속도계로부터의 측정치로 그러므로 위 식은 행렬이 아닙니다.



다음에 할 것은 를 계산하는 것입니다.



이전 오차 공분산 행렬 와 측정 공분산 행렬 에 근거한 측정을 얼마나 믿을 수 있는지 예측하는 것입니다. 관측 모델 는 이전 오차 공분산 행렬 를 관측 공간으로 매핑하기 위해서 사용되어집니다.

큰 측정 잡음은 값을 크게 합니다. 이는 입력되는 측정값을 그렇게 많이 신뢰할 수 없음을 의미합니다. 이 경우 는 행렬이 아니고 다음과 같이 쓸 수 있습니다.



다음 단계는 칼만 이득(Kalman gain)을 계산하는 것입니다. 칼만 게인은 측정치 와 이전 상태 변수와의 차이를 얼마나 신뢰할 수 있는지를 나타내는데 사용되어집니다.



만일 우리가 그 차이를 도저히 신뢰하지 못한다면, 차이에 대한 공분산 는 높게 될 것입니다. 그리고 만일 우리가 추정된 상태를 신뢰한다면 오차 공분산 행렬 는 작게 될 것이고 칼만 이득도 작아질 것입니다. 반대로 차이를 신뢰하지만 현재 상태의 추정치를 믿지 못한다면 반대가 될 것입니다.

여기서 관측 모델 H의 전치행렬이 오차 공분산 행렬 P를 관측 공간으로 매핑하는데 사용되어짐을 알 수 있습니다. 그다음 우리는 공분산 S로 나누어 오차 공분산 행렬을 비교합니다.

이는 관측 모델 H를 상태 오차 공분산을 빼기 위해서 그리고 S의 현재 추정치와 비교하기 때문에 의미가 있다는 것입니다. 만일 여러분이 초기에 상태를 모른다면 오차 공분산 행렬을 다음과 같이 설정할 수 있다.



여기서 은 큰 숫자를 대표하며 초기에 상태가 알려진다면 오차 공분산 행렬을 다음과 같습니다.



칼만 이득은 2x1 행렬입니다.



우리는 추정된 현재 상태를 보상합니다.




이는 yk와 칼만 이득을 곱하여 보상되지 않은 현재 상태에 더하는 것입니다.


는 측정된 와 보상되지 않은 추정된 현재 상태의 차이로 양수나 음수가 될 수 있습니다.


가장 간단화된 방정식은 보상되지 않은 현재 추정 상태 를 가속도계 측정값으로 간단하게 바로잡는 것으로 이해할 수 있습니다.

여기서 는 이전 상태와 자이로 측정값으로부터 계산되어진 것입니다.


마지막으로 우리는 보상되지 않은 오차 공분산 행렬을 갱신하는 것입니다.



여기서 I는 항등행렬입니다.



필터가 하는 일은 기본적으로 얼마나 많이 우리가 추정치를 바로잡았는가에 근거하여 오차 공분산 행렬을 스스로 바로잡는 것입니다. 이것은 우리가 이전의 오차 공분산 과 공분산 에 근거된 상태를 바로잡았기 때문에 의미가 있다는 것입니다. 



Posted by Nature & Life


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

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


MPU6050은 가속도와 자이로센서가 1개의 센서에 모두 포함하고 있는 6DOF(Degrees of Freedom) 센서로, I2C(Inter Integrated Circuit) 통신 프로토콜을 통해서 데이터를 추출 할 수 있습니다. 위 모듈은 MPU6050을 포함하며 사용하기 쉽게 만든 모듈입니다. 


MPU6050의 간단한 스펙입니다.


가속도계(accelerometer)는 움직임이 있을때 일반적으로 많은 잡음(noise)을 포함합니다. 자이로(gyroscope)의 문제는 시간이 누적되면 DC 성분(혹은 이를 bias라 표현)이 누적된다는 것입니다. 결론적으로 자이로는 짧은 시간에 믿을 수 있지만, 반면 가속도계는 긴 시간에 더 신뢰성이 있다는 것입니다.



그렇기 때문에 상보 필터(complimentary filter, 혹은 보상 필터)의 적용이 쉽습니다. 기본적으로 가속도계에 대해서 디지털 low pass filter(LPF), 자이로에 대해서는 디지털 high pass filter(HPF)를 적용하는 것입니다. 그러나 칼만 필터(Kalman filter) 만큼 정확하지는 않다는 것입니다. 근래에 아두이노를 이용하여 처리가 가능하지만 칼만 필터는 아두이노가 주로 제공하는 MCU에서는 무겁다는 단점이 있습니다.


k 시간에 시스템의 상태는 다음과 같습니다.



여기서 는 상태 행렬로 다음과 같습니다.



는 각도 의 시간에 따라 누적된 양으로 bias라고 하며 자이로에서 측정된 값에서 이 bias를 빼는 것에 의하여 현재의 각도를 얻을 수 있습니다.


그러므로 이전 상태 로부터의 전이 행렬 는 다음과 같이 쓸 수 있습니다.



는 제어 입력으로 k 시점에서 자이로의 측정값으로 [˚/s]의 단위를 가지며 이 로 상태방정식을 다음과 같이 다시 쓸 수 있습니다.



여기서  제어 입력 행렬로 다음과 같이 나타낼 수 있습니다.



는 프로세스 잡음으로, 주어진 k 시간에서 '0'의 평균과 의 공분산(covariance)을 갖는 가우시안 분포(Gaussian distribution)(혹은 정규분포)를 갖습니다.



는 프로세스 잡음 공분산 행렬이고, 이 경우에 가속도계와 bias의 상태 추정치 공분산 행렬이 됩니다. 우리는 bias와 가속도계의 추정이 서로 독립적임을 가정합니다.



위 식에서 처럼  공분산 행렬은 현재 시간 k에 의존합니다. 따라서 가속도계 분산 와 bias의 분산 은 미소 시간 에 곱하여집니다.

프로세스 잡음은 시간이 길어질수록 커지게 됨을 의미합니다. 이는 상태의 마지막 업데이트이기 때문입니다. 예를 들어 자이로는 드리프트(drift) 할 수 있습니다. 우리는 칼만 필터의 동작을 위해서 이들 상수들을 알아야 합니다. 만일 여러분이 큰 값을 설정한다면 상태 추정에 잡음이 커진다는 것을 기억해야 합니다. 예를 들어, 추정 각도가 드리프트 하기 시작하면 여러분은 의 값을 증가시켜야 합니다. 반면에 추정이 여러분이 생각하는 것보다 너무 많이 느려진다면 반응이 빠르도록 의 값을 감소시켜 볼 수 있습니다.



Posted by Nature & Life
Embedded Lecture/Arduino2017. 5. 8. 20:34


인터럽트(interrupt)란 지정된 핀의 신호가 원하는 조건과 일치하면 미리 등록한 인터럽트 callback 함수(ISR; Interrupt Service Routines)를 자동으로 호출해주는 기능입니다. 이 ISR 함수는 사용자가 만든 함수이며, 이때 실행 중이던 loop() 함수 내부의 루틴은 인터럽트 callback 함수가 끝날 때까지 멈추게 됩니다.


즉, 특정 핀의 입력 상태가 바뀔 때 Arduino는 이를 자동으로 감지해서 모든 동작을 잠시 멈춘 다음, ISR(Interrupt Service Routines) 이라 불리는 미리 지정된 함수를 실행하고, 다시 원래 작업으로 복귀한다는 것입니다. 이를 '하드웨어 인터럽트'라 부릅니다. 이 외에도 비슷한 타이머(timer) 인터럽트가 있는데 이는 사용법이 전혀 다르므로 이 글에서 다루진 않습니다.

♧ 타이머 인터럽트 - AVR 칩을 비롯한 임베디드 시스템에서는 시간을 재기 위해서 타이머(Timer)/카운터(Counter)를 내장합니다. 만일 타이머/카운터가 없다면, 소프트웨어적으로 시간지연 함수를 사용해야 함으로써 MCU는 동작을 멈추게 되고, 이로 인해 MCU의 효율을 떨어뜨리게 된다는 것입니다.

뿐만 아니라 이와 같은 내장 타이머/카운터는 정확한 시간을 잴 수 있으며, MCU가 다른 작업과 병행할 수 있고, 미리 설정한 조건에서 타이머 인터럽트도 발생하게 할 수 있다는 것입니다. 이러한 각각 인터럽트들은 동시에 발생할 수도 있으므로 우선 순위를 갖습니다.


Arduino의 인터럽트 핀

Arduino Uno 기준으로 2개의 인터럽트 핀이 할당되어 있습니다. 즉, Number 0 (D2), Number 1 (D3). 몇개의 보드별 지원되는 인터럽트 핀은 다음과 같습니다. 인터럽트 callback(ISR)을 등록할 때 유의할 점은 핀 번호가 아니라 반드시 인터럽트 넘버를 사용한다는 것입니다. 예를 들어, 다음 표에서 Leonardo 보드는 Uno 보다 많은 5개의 인터럽트 핀을 제공합니다. 그리고 다섯번째 인터럽트 핀(int4)은 Number 7이라는 것입니다. 참고로 Arduino Due 보드는 모든 핀에 인터럽트가 지원됩니다.


 보드

 int0

 int1

 int2

 int3

 int4

 int5

 Uno/Ethernet

 2

 3

 x

 x

 x

 x

 Mega2560

 2

 3

 21

 20

 19

 18

 Leonardo

 3

 2

 0

 1

 7

 x


인터럽트 callback 함수(ISR)는 입력 인자가 없고 반환값이 없습니다. 즉 파라미터를 전달하거나 리턴할 수가 없다는 것입니다. 게다가 ISR 함수 내에서는 delay() 함수를 사용할 수 없습니다. 또한 milli second의 시각을 가져오는 함수인 millis()를 사용하더라도 값이 증가하지는 않습니다. delayMicroseconds()의 경우에는 인터럽트에 독립적이므로 정상 동작합니다.


뿐만 아니라 ISR 함수 내에서는 Serial data를 읽을 경우 값이 소실되며, 이전 글에서 언급했던 것처럼 ISR 함수 내에서 업데이트 되는 전역 변수는 volatile로 반드시 선언되어야 합니다. ISR 함수를 만드는 요령은 최대한 짧고 빠르게 수행되도록 간결하게 작성해야 합니다. 왜냐면 이 코드가 길어지는 만큼 CPU는 멈추어 메인 작업을 수행하지 않아 효율적이지 않다는 것입니다.


또한 여러 개의 ISR 함수가 등록되어 있더라도 동시에 수행되지는 않습니다. 블루투스 모듈과의 통신을 위해 주로 사용되는 Software Serial 라이브러리의 경우, 내부적으로 인터럽트를 사용하는 것으로 알려져 있습니다. 따라서 Uno 보드의 경우 D2, D3 에 연결해야만 정상 동작 가능합니다. 따라서 이를 회피하기 위해서 D0, D1 핀에 연결해서 Hardware Serial로 동작시킬 수 있습니다.



Posted by Nature & Life