파란만장 개발이야기

MPU6050의 데이터 시트를 보면 DLPF(Digital Low Pass Filter)정도를 조작할 수 있는 레지스터가 있습니다.

아두이노 MPU6050 라이브러리를 보니까 MPU6050_DLPF_BW_xxx로 나와있네요.

xxx자리에 256, 188, 98, 42, 20, 10, 5라고 쓰여있는데 숫자가 낮을수록 더 낮은 주파수만 통과시키는 것 같습니다.

오늘 포스팅에서는 쿼드로터에 달린 MPU6050의 자이로 센서만 가지고 DLPF 성능을 비교해볼껀데요,

실험 할 때는 쿼드로터가 뜰랑 말랑할 정도까지 모터를 작동시키되, 손으로 눌러서 바닥에 고정시켰습니다.

각속도인 가속도 센서 출력은 0이 나와야 정상인데 모터로 인한 노이즈를 측정한 셈입니다.

위에서부터 BW는 5, 42, 256입니다. 대충 3개만 집어서 테스트해봤습니다.

데이터는 스케일링 해서 [deg/s^2]로 변환했습니다~

BW를 5로 해서 진동에 둔감하게 하면 반응이 느려지는가 싶어서 센서 두 개를 같이 장착하고 BW를 다르게해서 각도 변화를 살펴본 결과,

반응이 느려지지 않았습니다.

왜 느려지지 않는지 모르겠네요 ㅠ 하드웨어적으로 LPF하는 기능이 들어가있는 건지.. 

이에 관해 아시는 분 계신다면 댓글남겨주시면 감사하겠습니다 ㅠㅠ

이번 포스팅에서는 저번 포스팅(mpu6050 가속도, 자이로 센서를 융합하기[상보필터] - 2)의 개선된 사항을 포스팅합니다. 

먼저 가속도 센서를 오일러각으로 바꿀 때 저번 포스팅처럼 x/z나 y/z 하는건 단순히 각 축의 기울어짐 각도를 구하는 거였습니다.

이것을 자세 표현 방법의 한가지인 오일러각으로 변환하는 방법을 다룰 건데요

밑의 출처를 참고하여 이렇게 변환할 수 있습니다.

물론 Ax, Ay, Az는 약분될거기 때문에 스케일링을 하지 않으셔도 무방하지만, 변수의 자료형 범위를 넘지 않도록 신경써줍시다.

그리고 atan함수를 사용한다면 출력의 단위가 라디안이기 때문에 180.0 / 3.141592인 57.3을 곱해서 각도로 바꿔줍시다.

여기서 φ가 Roll, θ가 Pitch라고 정하겠습니다. 

아, 기체좌표계는 다음 그림과 같이 쿼드콥터 전방을 x축 양의 방향이라고 하고, 우측을 y축 양의방향, 지면쪽을 z축 양의방향으로 정합니다.

x축을 중심으로 한 회전이 Roll회전이고, y축을 중심으로한 회전이 Pitch회전이고, z축을 중심으로 한 회전이 yaw회전입니다. 

회전의 방향은 각 축의 양의 방향에 오른손법칙을 적용한 방향을 +로 정합니다. 글마다 계속 언급하겠지만, 이 설정을 되도록 유지할 생각입니다.

여기까지가 가속도 센서값을 오일러각으로 바꾸는 내용이고, 다음은 자이로 센서를 다뤄봅시다.

자이로 센서는 각 축의 각속도를 출력해주기 때문에 단위변환 후시간 변화량에 대해 적분해주면 각 축의 각도를 구할 수 있습니다.

하지만 자이로센서를 조금 다뤄본다면 각 축의 출력이 완전히 독립적이지 않다는걸 알게되는데요, 

예를들어 센서를 지면에 평행하게 두고 yaw회전 각도만 구한다면 실제 90도 회전에 대해 정확히 90도의 각도를 구할 수 있을겁니다. 하지만 지면에 평행하게 두지 않고 센서의 Roll각을 20도 정도 들고 90도 회전을 시행할 경우 yaw회전만 측정해서는 관성좌표계에 대한 yaw회전 각을 구할 수 없습니다.

이런 이유로 오일러각 변환이 필요하게 되었는데요

이것도 다음 식을 이용해서 변환할 수 있습니다. (AR.drone 관련 논문 참고)

Ω는 각 축의 각속도인 p, q, r이고 Q는 현재 오일러각인 φ, θ, ψ를 의미합니다. 

(여기서 현재 오일러각은 세가지를 쓸 수 있습니다. 가속도 센서로 만들어낸 오일러각을 쓸 수도, 자이로 센서로 만들어진 가장 최근 오일러각을 쓸 수도 있지만 실제 자세각을 반영하기 위해 상보 필터를 거친 오일러각을 사용하는게 바람직하겠습니다.)

이 두 정보를 가지고 오일러각의 변화량을 구할 수 있습니다. 그 후 이 변화량을 시간에 따라 적분하면 됩니다.

이 결과는 다음 그림을 보면 알 수 있습니다.

Roll축의 각도만 표시했습니다. 빨간색이 가속도센서의 오일러각, 초록색이 자이로센서의 오일러각 입니다.

이 두개를 합치는 상보필터에 대한 이야기는 다음 포스팅에서 하기로 하겠습니다~!

지난 포스팅에서 다뤘지만... 잘못된 정보가 있어서 바로잡는 포스팅을 하려고 합니다.

참고 사이트

http://www.nano-i.com/salse/1272665

http://blog.daum.net/pg365 - open robotics

논문

The Navigation and Control technology inside the AR.Drone micro UAV

I²C의 개요

I²C (Inter-Integrated Circuit) 버스는 마이크로프로세서와 저속 주변 장치 사이의 통신을 위한 용도로 Philips에서 개발한 규격인데 두 가닥의 선을 사용하므로 TWI (Two Wire Interface)라고도 불리고 있다. 특허권 때문인지 잘 모르겠으나 Philips 이외의 회사들은 거의 대부분TWI라고 부른다. I²C 버스는 양방향 오픈 드레인 선인 SCL (serial clock)과 SDA (serial data)로 이루어져 있으며 매스터-슬레이브 형태로 동작한다. SCL은 통신의 동기를 위한 클럭용 선이고 SDA는 데이터용 선이다. 매스터는 SCL로 동기를 위한 클럭을 출력하며 슬레이브는 SCL로 출력되는 클럭에 맞추어 SDA를 통해 데이터를 출력하거나 입력 받는다. 단지 SDA 한 선으로만 데이터를 주고 받기 때문에 I²C 버스는 반이중 (half duplex) 통신만 가능하다. SCL 선과 SDA 선은 모두 오픈 드레인므로 두 선에는 각각 풀업 저항을 연결해 주어야 한다.

다음 그림은 I²C 매스터와 슬레이브들 사이의 연결을 보여주고 있다.

모든 I²C 매스터와 슬레이브 장치들의 SCL은 서로 연결되며 SDA 또한 서로 연결된다. 모든 장치들이 SCL과 SDA를 각각 공유하고 있으므로 매스터가 슬레이브를 개별적으로 지정하기 위한 방법이 있어야 한다. I²C 버스에서 매스터가 각 슬레이브를 구분하는 수단은 슬레이브의 주소이다. 모든 I²C 슬레이브 장치들은 7 비트의 고유한 주소를 가지고 있으며 매스터는 주소로 원하는 슬레이브를 지정한다. 주소의 길이가 7비트이므로 매스터는 최대 128 개의 슬레이브 장치들과 연결될 수 있다. 물론 슬레이브 장치들의 주소는 모두 달라야 한다.

앞의 그림에는 매스터가 하나 밖에 없지만 I²C 버스에 두 개 이상의 매스터들이 존재할 수도 있다. 버스에 연결된 장치의 수가 아무리 많아도 한 순간에는 오직 하나의 매스터와 하나의 슬레이브만이 통신할 수 있다. 현재 통신을 진행하고 있는 매스터가 버스 소유권을 가지고 있다. 두 개 이상의 매스터가 있는 경우에 현재 버스의 소유권을 가지고 있는 매스터 및 매스터와 통신을 진행하고 있는 슬레이브 장치 이외의 나머지 장치들은 이들의 통신을 방해하지 않아야 한다.

SCL과 SDA는 모두 오픈 드레인이므로 모든 장치들의 SCL과 SDA는 각각 와이어드 AND로 연결 되어 있다. 와이어드 AND 연결에서는 어느 한 장치라도 ‘0’을 출력하면 해당 신호의 상태는 논리 ‘0’이 된다. 만일 SCL이나 SDA에 연결되어 있는 어떤 장치가 논리 ‘0’을 출력한다면 다른 장치가 그 신호의 상태를 논리 ‘1’로 만들 수 있는 방법은 없다. 통신에 참여하지 않은 장치가 SCL이나 SDA로 ‘0’을 출력하면 매스터가 정상적으로 통신을 진행할 수 없다. 이것은 I²C 버스에 연결되어 있지만 현재 통신에 참여하지 않고 있는 장치들은 모두 자신의 출력을 플로우팅 상태로 유지해야 한다는 것을 뜻한다.

통신이 진행되지 않는 상황에서 모든 장치의 출력은 플로우팅 상태이므로 SCL과 SDA의 상태는 모두 논리 ‘1’이다. 이 상황에서 I²C 버스의 사용을 원하는 매스터는 SCL과 SDA로 시작 조건을 출력하며 버스 소유권을 주장하고 통신을 시작할 수 있다. 두 신호의 상태가 모두 논리‘1’이 아니라면 현재 다른 매스터가 버스 소유권을 가지고 통신을 진행 중에 있다는 것을 뜻하므로 그 매스터가 버스 소유권을 반납할 때까지는 다른 매스터가 새로운 통신을 시작해서는 안 된다.

다음 그림은 I²C 프로토콜의 시작 조건, 정지 조건, 그리고 데이터 안정 구간을 보여주고 있다.

<v></v>

I²C 프로토콜에서 SCL이 ‘0’인 구간에서는 SDA의 상태 변화가 허용되지만 SCL이 ‘1’인 구간에서는 SDA는 안정된 논리 상태를 유지해야 한다. 매스터가 슬레이브로 데이터를 출력할 때 SCL이 ‘0’인 구간에서 SDA의 비트 전환을 하며 SCL이 ‘1’인 구간에서는 SDA의 상태를 그대로 유지한다. 슬레이브가 데이터를 출력하고 매스터가 그 데이터를 읽을 때에도 마찬가지이다. 그래서 SCL이 ‘1’인 구간은 데이터가 안정한 구간이다.

I²C 프로토콜에서 SCL이 ‘1’을 유지하고 있는 구간에서 SDA의 상태가 변하는 것은 일반 데이터 전송이 아닌 특별한 조건을 의미한다. SCL이‘1’인 동안 SDA가 ‘1’에서 ‘0’으로 바뀌는 것을 시작 조건 (start condition, S)이라고 하며 SCL이 ‘1’인 동안 SDA가 ‘0’에서 ‘1’로 바뀌는 것을 정지 조건 (stop condition, P)이라고 한다. I²C 통신을 원하는 매스터는 SCL과 SDA가 모두 논리 ‘1’일 때 SDA의 상태를 ‘0’으로 바꾸어 시작 조건을 출력하며 다른 장치들에게 통신의 시작을 알린다. 마찬가지로 통신을 끝낼 때에는 SCL이 ‘1’인 동안 SDA를 ‘0’에서 ‘1’로 바꾸어 정지 조건을 출력하며 버스 소유권의 반납을 다른 장치에게 알린다.

I²C의 패킷 형식

다음 그림은 I²C 버스에서 사용되는 데이터 패킷의 형식을 보여준다. ACK을 포함한 9 비트가 I²C 규격에서 통신의 기본 단위가 된다.

<v></v>

매스터는 SCL과 SDA가 모두 ‘1’인 상태에서 SDA를 ‘0’으로 바꾸어, 즉 시작 조건을 출력하면서 통신의 시작을 알린다. 시작 조건 이후부터는SCL의 상태가 ‘0’인 구간에서만 SDA의 논리 값이 바뀐다. 매스터가 SCL로 출력하는 클럭에 동기를 맞추어 SDA로는 데이터가 MSB부터 한 비트씩 출력된다. SCL은 항상 매스터가 출력하는 신호이지만 SDA는 현재의 동작이 매스터의 읽기냐 쓰기냐에 따라 매스터의 출력일 수도 슬레이브의 출력일 수도 있다.

8 비트 데이터가 8 클럭 싸이클 동안 SDA로 출력되면 그 데이터를 수신한 쪽에서 9 번째 클럭에 맞추어 그 8 비트 데이터의 수신 여부를 확인해 주는 ACK 비트를 출력한다. 다시 말해서 ACK 비트는 직전의 8 비트 정보를 수신한 쪽에서 그 정보의 송신자에게 직전 8 비트 정보에 대한 수신 여부를 확인하는 값이다. ACK 비트가 ‘0’이면 정상 수신을 나타내며 ACK 비트가 ‘1’이면 (항상 그런 것은 아니지만) 정상 수신이 아님을 나타낸다. 값이 ‘0’인 ACK 비트와 구분하기 위하여 값이 ‘1’인 ACK 비트를 NACK (no acknowledgement)라고 부르기도 한다.

만일 매스터가 SDA로 8 비트 정보를 출력했다면 그 정보를 수신한 슬레이브가 9 번째 클럭에 SDA로 ACK 비트를 출력하여 매스터에게 수신 결과를 알린다. 이 때 매스터가 ACK 비트를 받으려면 9 번째 클럭 싸이클 동안 자신의 SDA 출력을 ‘1’로 유지하고 있어야 한다. 반대로SDA로 출력된 8 비트 정보가 슬레이브의 출력이라면 그것을 수신한 매스터가 9 번째 클럭에서 ACK 비트를 출력한다. 이 때 매스터의 ACK비트를 받기 위해 슬레이브는 자신의 SDA 출력을 ‘1’로 유지하고 있어야 한다.

8 비트 정보와 이어지는 ACK 비트의 전송이 끝난 다음에는 매스터가 정지 조건을 출력할 수도 있고 시작 조건을 다시 출력할 수도 있으며 다음 바이트 전송이 이어질 수도 있다. 다음 동작은 현재 진행 중인 동작이 무엇인가에 따라 달라진다.

I²C의 주소 지정 형식

모든 I²C 슬레이브 장치는 7 비트의 고유한 주소를 가지고 있으며 매스터는 이 주소를 사용하여 상대 슬레이브 장치를 지정한다. 다음 그림은 매스터가 슬레이브의 주소를 지정하고 데이터를 주고 받는 방법에 대한 설명이다.

<v></v>

먼저 매스터는 시작 조건에 이어서 자신이 원하는 슬레이브의 7 비트 주소를 출력한다. 버스에 연결되어 있는 모든 슬레이브 장치들은 SDA선을 계속 감시하면서 매스터가 출력한 주소가 자신의 주소와 일치하는지 여부를 검사한다. 만일 매스터가 출력한 주소가 어떤 슬레이브의 주소와 같으면 그 슬레이브는 ACK 비트에 ‘0’을 출력하여 매스터에게 응답한다. 매스터가 출력한 주소를 가진 슬레이브가 없으면 아무도ACK 비트로 ‘0’을 출력하지 않으므로 ACK 비트의 상태는 ‘1’을 유지하여 매스터는 NACK을 받게 된다.

매스터가 ACK을 받으면 이것은 자신이 출력한 주소를 가진 슬레이브가 응답했다는 뜻이므로 매스터는 다음 단계를 진행한다. 반면에 매스터가 NACK을 받으면 이것은 자신의 호출에 응답하는 슬레이브가 없다는 것을 뜻하므로 매스터가 더 이상 할 수 있는 일은 없다. 매스터는 정지 조건을 출력하여 현재의 상황을 종료하고 통신을 다시 시작해야 한다.

슬레이브 주소 7 비트 다음에 오는 8 번째 비트는 다음 동작이 매스터의 읽기인지 쓰기인지를 가리킨다. 매스터가 이 비트로 0을 출력했다면 이것은 매스터 쓰기임을 뜻하므로 슬레이브 주소 다음의 1 바이트 데이터는 매스터의 출력이다. 이 데이터는 앞서 주소가 지정된 슬레이브를 위한 것이므로 그 슬레이브가 이 값을 읽어 들인다. 반면에 슬레이브 주소 7 비트 다음에 오는 8 번째 비트가 1 이면 이것은 다음 동작이 매스터의 읽기라는 것을 의미한다. 그러므로 매스터에 의해 주소가 지정된 슬레이브는 ACK을 응답한 후 이어서 매스터에게 보낼 데이터를 출력한다.

주의할 것은 슬레이브 주소는 7 비트이지만 패킷의 상위 7 비트로 정렬되고 마지막에 R/W 비트가 LSB 위치에 추가된다는 것이다. 그렇기 때문에 슬레이브의 주소를 왼쪽으로 한 비트 쉬프트 시킨 후의 8 비트 값을 슬레이브의 주소라고 생각하는 것이 더 편할 수도 있다. 예를 들면 DS1037이라는 RTC 칩의 I²C 주소는 1101000_b으로 고정되어 있다. 즉 DS1037의 I²C 주소는 0x68이다. DS1037에 데이터를 기록해야 할 때 매스터는 I²C 버스로 시작 조건을 출력한 다음 11010000_b을 출력한다. DS1037의 데이터를 읽고자 한다면 매스터는 I²C 버스로 시작 조건을 출력한 다음 11010001_b을 출력한다. 그러므로 8 비트 값인 0xd0을 DS1037의 쓰기 주소, 0xd1을 DS1037의 읽기 주소로 생각할 수도 있다.

I²C 버스 규격은 I²C 버스의 전기적 특성 및 타이밍과 함께 앞에서 설명한 시작 조건, 정지 조건, 주소 지정 방법 등에 대해서만 규정하고 있다. 구체적인 통신 방법, 즉 매스터가 슬레이브의 주소를 출력한 다음 정지 비트를 출력할 때까지 주고 받는 데이터의 바이트 수, 슬레이브의 내부 레지스터 주소 지정 방법 등은 슬레이브 장치에 따라 달라진다. 매우 다양한 종류의 I²C 슬레이브 장치가 있으니 여러 가지 다양한 방법이 있을 수 있다.

대부분의 마이크로컨트롤러는 I²C 인터페이스를 지원한다. 그렇기 때문에 UART를 사용할 때와 비슷하게 I²C 관련 몇 개의 레지스터를 설정한 후 레지스터 읽기/쓰기를 통해 간단하게 I²C 통신을 실행할 수 있다. 마이크로컨트롤러가 I²C 인터페이스를 지원하지 않으면 프로그램을 통해 비교적 쉽게 I²C 프로토콜을 구현할 수 있다. 송·수신 타이밍을 정확하게 맞추어야 하는 비동기 통신과 달리 매스터가 출력하는 클럭에 맞추어 모든 동작이 이루어지므로 타이밍에 관련된 어려움이 특별히 없기 때문이다.

I²C 통신의 예

직렬 EEPROM, ADC, DAC, RTC 등을 포함한 다양한 매우 장치들이 I²C 인터페이스를 지원하고 있다. I²C 슬레이브 장치와 통신을 하려면 그 슬레이브의 데이터쉬트를 참고하여 구체적인 통신 방법을 이해해야 한다. 다음 그림은 24LC64라는 64 Kbit 직렬 EEPROM의 내부 데이터를 읽기 위한 여러 방법 중 하나이다.

<v></v>

24LC64의 주소는 1010A₂A₁A_0b로 정해져 있다. 앞 4 비트는 1010로 고정되어 있지만 뒤의 3 비트 A₂A₁A₀는 사용자가 임의로 바꿀 수 있다.주소의 마지막 3 비트를 사용자가 조절할 수 있도록 24LC64는 이 3 비트에 각각 대응되는 3 개의 입력 핀 A₂, A₁, A₀을 가지고 있다. 주소의 일부를 사용자가 정하도록 만든 것은 24LC64에 여러 가지 다른 주소를 부여할 수 있도록 하기 위한 것이다. 만일 24LC64의 주소가 완전히 고정되어 있다면 I²C 매스터가 구동할 수 있는 24LC64는 1 개뿐이다.

위의 그림은 24LC64의 저장된 값을 특정 주소부터 연속으로 읽는 방법이다. 맨 먼저 매스터는 시작 조건을 출력한 다음 24LC64의 7 비트 주소를 출력하고 8 번째 비트로 ‘0’을 출력하여 다음 동작이 쓰기라는 것을 알린다. 매스터가 지정한 주소와 동일한 주소를 가진 24LC64가 있다면 그 IC는 ACK을 출력하여 매스터에게 응답한다. ACK을 받은 매스터는 이어서 24LC64의 저장 공간 중에서 자신이 원하는 주소를 MSB부터 연속해서 출력한다. 24LC64의 저장 용량은 64 Kbits, 즉 8 K 바이트이므로 내부 주소 지정을 위해서는 13 비트의 주소 정보가 필요하다. 그러므로 매스터는 두 번에 걸쳐 주소를 출력한다. 매 바이트를 받을 때마다 24LC64는 매스터에게 ACK을 보낸다.

24LC64 내부의 주소를 출력한 후 ACK을 받으면 매스터는 시작 조건과 슬레이브 주소를 다시 한 번 더 출력한다. 그런데 이번에는 매스터가24LC64의 데이터를 읽을 차례이므로 7 비트 슬레이브 주소 다음에 오는 8 번째 비트는 ‘1’로 설정한다. 24LC64가 다시 매스터에게 ACK으로 응답하면 그 다음부터 매스터는 24LC64가 출력하는 데이터를 연속해서 읽는다. 24LC64의 출력을 1 바이트 받을 때마다 매스터는 ACK을 출력하여 이를 24LC64에게 알린다. ACK을 받은 24LC64는 내부에서 주소를 1 증가시키고 새로운 주소의 데이터를 읽어서 다음 SCL 클럭에 맞추어 출력하게 된다. 데이터를 연속해서 수신한 매스터는 자신이 원하는 마지막 데이터를 받으면 이번에는 NACK을 출력하고 이어 정지 조건을 출력하여 통신을 끝낸다.

위의 예는 I²C 통신을 사용하는 24LC64를 사용하기 위한 많은 방법들 중 하나일 뿐이다. I²C 인터페이스를 가진 장치마다 사용법이 다르기도 하거니와 한 장치에서도 명령의 종류에 따라 다양한 방법이 있을 수 있다.

I²C의 장·단점

I²C 버스의 가장 큰 장점으로 슬레이브 장치의 수에 관계 없이 두 가닥의 선만으로 매스터와 슬레이브 사이의 양방향 통신이 가능하다는 것을 들 수 있다. 두 개 이상의 매스터를 사용할 수 있다는 사실은 I²C 버스가 가진 또 다른 장점이다.

반면에 I²C 버스의 양방향 특성은 오픈 드레인 구조에 기반을 두고 있으므로 근본적으로 동시 양방향 통신이 불가능하고 통신 속도를 높이기 어렵다. 표준 모드에서 I²C 통신의 최대 속도는 100 kHz이며 고속 모드에서는 최고 400 kHz까지 가능하다. 다른 동기 통신 프로토콜, 예를 들어 뒤에서 설명할 SPI와 비교하면 매우 낮은 속도이므로 I²C 버스는 저속 주변 장치와의 데이터 통신이나 주변 장치의 동작 모드 설정 등의 용도로 많이 쓰인다. 동작 모드 설정은 사용 빈도가 높지 않기 때문에 낮은 속도가 별 문제가 되지 않는다.

I²C 인터페이스를 사용하는 제품의 수는 무척 많은 반면 슬레이브의 주소는 7 비트로 그 길이가 제한되어 있어서 다수의 슬레이브 장치를 사용할 때 슬레이브 주소가 충돌할 가능성이 있다. 이런 문제에 대한 해결책으로 슬레이브 주소의 하위 2 ~ 3 비트를 사용자가 임의로 설정할 수 있는 제품들이 많기는 하지만 주소가 7 비트로 제한되어 있어서 근본적인 해결책이 될 수는 없다.

출처 : http://blog.naver.com/eslectures?Redirect=Log&logNo=80137860924

(내용을 보완한 글을 새로 올렸습니다~! 하단부 알림글을 먼저 읽어보시기 바랍니다~!)

오늘은 상보필터(complementary filter)또는 보상필터라고 하는 센서 융합방법에 대해 포스팅 하겠습니다~

이전 포스팅에서는 칼만필터로 융합했었는데 수박 겉햝기식이라 내부 구조를 변경하고 이해하기엔 좀 힘들었는데 요 방법은 좀 더 직관적이고 이해하기 쉬워서 제가 맘대로 조절하기도 편하더라고요

왜 다시 융합으로 돌아왔느냐~ 하면요 ㅠ roll, pitch, yaw를 센서가 알아서 계산해서 제공해주는 값을 사용했는데 그게 update rate(샘플링타임)가 100Hz였습니다. 근데 쿼드콥터 네이버 카페에 쌍둥아빠이기성이라는 분이 댓글을 달아주셨는데 최소 200Hz는 되어야 뜬다고 하더라구요.. 전 그것도 모르고 왜안되나~ 삽질만하고.. 오히려 50Hz로 낮춰보기까지.. ㅠ 제 센서가 100Hz인데 더 높일수가 없어서 다시 mpu6050으로 돌아왔습니다. 

mpu6050의 update rate는 가속도, 자이로 각각 4~1000Hz, 4~8000Hz인데 왜 범위냐면 내부의 LPF(Low Pass Filter)를 사용할 여지가 있기 때문인거 같습니다.

아무튼, 더 빠른 센서가 필요해서 MPU6050으로 돌아왔는데, 예전 칼만필터 소스를 아두이노에서 돌리려니까 한 루프에 10ms가 넘더라구요..;;

그래서 다른 필터를 찾다가 상보 필터가 있길래 공부해서 적용시켜봤습니다.

상보 - [명사] 서로 모자란 부분을 보충함. - 네이버 사전

두 센서의 서로 모자란 부분을 보충해서 더 좋은 결과를 만들어내는게 상보 필터입니다.

가속도 센서와 자이로 센서에 대해서는 이전 포스팅을 참고해주세요. -  mpu6050 가속도, 자이로 센서를 융합하기[상보필터] - 1 

이 블록선도를 구현할 건데요. 

먼저, 가속도 센서값을 roll, pitch로 바꿔야 합니다.

바꾸는 수식은 되게 많은데.. 제가 사용한 방법은 아래과 같습니다.

xAngle이 roll이 됩니다. 마찬가지로 pitch도 구할 수 있는데 yValue대신 xValue를 사용하면 되겠죠?

스케일링을 안하는 이유는 어차피 약분될거기 때문입니다. 

다음으로는 자이로 센서를 roll, pitch로 바꿔보겠습니다.

mpu6050은 가속도 자이로 모두 -90도~90도의 값을 -16383~16383로 주기 때문에 스케일링을 해야합니다.  -> (오류 : 하단참고)

 -> (오류 : 하단참고)

참 쉽죠? ㅋ

이걸 dt(시간변화량)으로 곱해서 누적시키면 각도가 나오게 됩니다. 적분하는거죠.

그럼 이제 가속도와 자이로센서 각각의 각도를 구했습니다. mpu6050 가속도, 자이로 센서를 융합하기[상보필터] - 1 제 포스팅에 있는 그래프처럼 서로의 장단점을 보완해 봅시다.

자이로 값이 기준이 됩니다. 가속도 값이 보정값이 되는거구요. 

간단하게 말하면 자이로 값과 가속도센서의 오차를 누적시키고 누적된 오차를 자이로센서값에 더해서 가속도센서값에 근접시키는 겁니다.

다만 얼마나 빠르게 근접시키느냐에 PI제어를 추가한 거라고 보시면 되겠습니다. P게인 I게인 값을 조절하는데, 가속도 값에 빠르게 근접시키면 노이즈까지 따라갈테고 느리게 근접시키면 노이즈는 무시되겠지만 반응이 느려지는 관계를 잘 절충해서 조절해야합니다.

PID제어에 관한 개념도 제 블로그에 있으니 참고바랍니다. - 쿼드콥터 PID제어

식으로 표현하면 

입니다. 이해하기 쉽게 써볼라고 길게 늘려봤는데 이게 수학적 문법에 맞는 식인지 모르겠네요..ㄷㄷ 

식 그대로네요.. 여기에 글로 해설해 봤자 식을 읽는거에 불과하니 괄호 하나하나 이해하면서 해석해보시기 바랍니다.

High Pass Filter와 Low Pass Filter식을 유도하는거는 pinkwink님의 블로그를 참고하시기 바랍니다. - http://pinkwink.kr/78

아두이노에 사용한 소스 코드도 올리니까 한줄 한줄 이해해 보시기 바랍니다.

코드 -> (오류 : 하단참고)

결과는,

이렇습니다. 빨간색이 가속도, 초록색이 자이로, 파란색이 상보필터네요. 여기서는 P게인이 낮아서 부드럽지만 다소 지연이 발생하는걸 보실 수 있습니다. 자기가 사용하는 상황에 맞게 튜닝 해야겠죠? ㅠㅠ 이부분이 제일 어려운거 같습니다 ㅜㅜ

팁으로 아두이노에서 dt(한 루프 걸리는 시간)를 구할 때는 millis()나 micros()를 사용하시면 됩니다.

아, delayMicroseconds(int n)함수는 n에 숫자 제한이 있더라구요.. 그것도모르고 사용하다가 고생했네요 ㅠ

------------------------------------------------------------------ 10월 11일 수정 사항 -------------------

코드에서 에러 적분하는 부분에서 +를 하나 빼먹었네요.. 어쩐지 I가 잘 안되더라 ㅠ 수정했슴니당

------------------------------------------------------------------ 14년 2월 2일 오류 정정 -------------------

 "-> (오류 : 하단참고)" 라고 쓴 부분 모두 한가지 오류를 범했습니다.

MPU6050은 내부 레지스터를 이용해서 출력 값의 범위를 조정할 수 있습니다. (데이터시트 참고)

"-90도~90도의 값을 -16383~16383로 주기 때문에" 이 부분이 바뀔수도 있다는 겁니다.

레지스터 조작에 따라 가속도센서같은 경우는 -16383~16393범위에서 -2g~2g의 값을 출력시킬수도 있고  -8g~8g의 값을 출력시킬수도 있습니다.

자이로센서는 -32768~32768로 -250~250[degree/sec]을 출력하던가 -500~500의 값을 출력할수도 있습니다.

물론 범위를 넓히면 정확도는 그만큼 줄겠죠. 

데이터시트부터 차근차근 볼걸 너무 대충봤다는 생각이 듭니다..

논문 쓰면서 다시 돌아보다가 틀린부분이 있어서 바로잡습니다.

예전부터 이상하다고 여긴것에 대한 수정사항입니다.

이 글에서 두 센서의 측정치를 각각 각도으로 변환하는데, 오일러각이 아니고 각 축에 대한 각을 따로따로 구한겁니다.

이 부분 혼동하지 마시길 바랍니다. 작년에는 이런거 모르고 그냥 했는데 쿼드가 떴네요... 아마 0도에서 거의 움직이지 않기때문에 두 좌표계 변환을 고려하지 않아도 된 듯 싶습니다.

책에서는 두 센서 모두 오일러각으로 바꿔주던데.. 변환 식을 얼핏 보니 탄젠트가 들어갔던걸로 기억합니다. 0도에서는 탄젠트로 어떻게 계산하는지 이해가 안가서 덮어뒀던 기억이 나네요.

------------------------------------------------------------------ 15년 4월 25일 추가사항 --------------------

글에 잘못된 점이 많아서 새로 포스팅하고 있습니다. 이 글을 지울까 했지만, 이것도 하나의 시행착오고.. 혹시라도 다른 분들이 겪지 않았음 하는 마음에 그대로 두기로 했습니다. 새로운 포스팅은 

가속도, 자이로 센서값을 오일러각으로 변환 - 1

을 참고해주세요~

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

저번 포스팅에서 못다한 1차 저역 통과 필터 후 칼만필터 거친 포스팅을 해보겠습니다.

칼만 필터만 사용	저역 통과 필터만 사용

합치면..!!

저역 통과 필터 + 칼만필터

그냥 칼만필터와 차이가 없죠?? 저도 실망했었습니다 ㅠ 그래도 혹시나 하는 마음에 모터를 작동시켜서 테스트 해봤는데요

정지한 상태

위 그림은 기체를 움직이지 않고 측정한 결과입니다.. 0에서 요동치는 노란색 보이시나요? 저러면 안돼요 ㅠㅠ

근데 스치는 생각에 '혹시 정지하지 않고 움직이면 어떨까? 어떤 패턴이라도 나오지 않을까?" 해서 움직여 봤는데 왠걸!

칼만필터만 통한 결과

짠~!  그냥 손으로 최대한 일정하게 기울였다 세웠다 한건데요 어느정도 패턴이 보입니다!

저역통과필터 + 칼만필터 결과

필터 두개를 켜고 측정해봤는데!!

생각보다 깔끔하네요!!

정지했을 때는 0에 수렴하진 않지만 움직이니까 어느정도 봐줄 만 하네요

근데 이게 사람 눈에는 그럴듯 해 보이는데 피드백으로서는 유효할진 모르겠습니다.

그래도 가능성이 보이니까!! 이대로 한번 자세추정 해서 제어 해보려고 합니다. 안돼면 그때 다시 문제점을 진단하도록 하구요 ㅋ

이번 시도는 수확이 있어서 다행이네요~

아.. 다 쓰고 보니까 가속도센서에 대해서만 LPF하고 자이로센서는 하질 않았네요??

음.. 더 좋아질까요?! 다음 포스팅때는 자이로센서도 같이 필터링해서 찾아오겠습니다!

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

여기다 추가할게요

자이로도 LPFing해봤는데 성능이 오히려 안좋게 나타나네요ㅋㅋ

왜지.. 코딩이 잘못됬었나...

간만에 연구실에 놀러오신 선배한테 어떻게하면 진동을 잡을 수 있을까요~ 하고 진동때문에 기울기가 제대로 나오지 않는 그래프를 보여드렸더니, 칼만필터 말고 진동에 대해 강한 필터를 사용하는게 어떠냐고 그러시더라구요

그러고 집에와서 곰곰이 생각해보니 진동도 엄연한 물리 작용이더라구요. 무슨 말이냐면, 진동을 감지하기 위해 자이로, 가속도센서를 사용한다 이겁니다.

스마트폰이 대표적인 사례겠네요. 흔들어서 잠금해제하기, 만보기 어플 등등 진동을 응용해서 만든 어플이잖아요

그렇게 생각하니까 진동도 감지하기 위해 만든 가속도, 자이로 센서인데 주위에다 진동 방지 장치를 하는게 무슨 소용인가 싶더라구요(물론 어느정도는 감소하긴 하지만 한계가.. ㅠ)

그래서!

소프트웨어적으로 접근해보기로 했습니다.

아주 간단히 이동평균 필터를 구현하려다가 별로 효과가 없을거 같아서 저역통과필터(Low Pass Filter)를 구현해 봤습니다.

구현은 이동평균필터보다 간단해요.

(weight) : 가중치

(measured) : 측정치

(value) : 현재 값

(pre_value) : 이전 값

n이 2인 이동평균필터는 위 식에 w만 0.5입니다.  라고 생각했다면 오산입니다.

은 여태까지 계산했던 모든 값에 영향을 받아요.

은 에 만큼 영향을 주고

는 에 만큼 영향을 줬으니까 는 에 만큼 영향을 주고

이런식으로 계속 누적되어 가죠 (제곱으로)

는 상황에 맞게 조절해야 합니다. 위 식에서 가 크면 클수록 측정치에 영향을 덜 받게 되겠죠

주절주절 설명을 길게 했는데, 결론은

w = 0.3	w = 0.7
초록, 빨강 : 필터 통과 전      자주, 노랑 : 필터 통과 후

잘 보이시나요? 왼쪽에 비해 오른쪽이 더 괜찮은 결과가 나왔습니다. 이 결과를 가지고 칼만필터 통과한건 다음 포스팅에... ㅋ

그리고 혹시나 해서 제가 만든 ㅋㅋ 이름은 거창한 미분필터.. 검색은 안해봤지만 진짜 있다면 만든이한테 미안요 ㅋ

이름은 어렵게 미분이라고 써놨지만 실제로는 차(Difference)필터가 더 맞겠네요.

"바로 직전 값과 현재 측정치가 d이하라면 현재 측정치를 무시해라" 이거라면 위의 그림처럼 순간적으로 팍 튀는건 무시할 수 있지 않을까 해서 만들어 봤습니다.

결과는... 실패..ㅋㅋ 현재 측정치를 무시하니까 이상한 값에서 움직이질 않네요.. 혹시 d를 조정하면 좀 더 나은 결과가 나올까요? ㅋㅋ

일단 이 두가지 기본적인 필터 중 바로 쓸 수 있는건 저역통과필터네요. 다음 포스팅에 LPT + kalman으로 돌아오겠습니다~!

d도 한번 조절해봐야겠네요 지금 보니 가능성이 ...

------------------------------------------ 2013.08.20 추가

위의 미분필터라고 되어있는 결과는 코딩을 잘못한 결과입니다. 착오 없으시길 바랍니다 ㅠ

--------------------------------------------------------2014.06.02 추가

1차 저역 통과 필터(LPF)는 지수 가중 이동 평균 필터라고도 합니다~

ps. 지금보니까 미분필터는 아무짝에서 쓸모없는 실패작이네요.. 그래도 지우는것보단 나을거같아서 냅두고 추가 코멘트만 달아놓습니다.

원래 네이버 블로그에 있는 글을 모두 옮겨오려고 했지만.. 굳이 그럴 필요 없을거 같아서 이 글에 제목과 링크만 올려두겠습니다.

네이버 블로그 주요 포스팅:

쿼드콥터 초기 계획안 

재료, 부품 구매 현황

MPU-6050 3축 자이로 + 3축 가속도 센서 구매 

[스크랩]오일러각 

[스크랩] I2C (Inter-Integrated Circuit) 버스 

[스크랩]칼만 필터(Kalman Filter) - Part 1 : Linear Systems | Robotics 

[스크랩]칼만 필터(Kalman Filter) - Part 2 : Kalman Filter & Algorithm | Robotics 

현재는 가속도, 자이로 센서 값을 이용해서 칼만필터로 융합한 다음, Q행렬, H행렬을 조정하는 단계입니다.

노랑, 자주 : 가속도 빨강, 초록 : 가속도 + 자이로 using 칼만필터

여러가지 상황에 대해 테스트하고 있는데 쿼드콥터 진동이 장난이 아니더라구요.. ㅠ

어느 정도는 칼만필터로 보정할 수 있겠거니~ 했는데 이건 너무 심해서 기구적으로 손을 보고 있습니다.

그래서 스펀지로도 방진 해보고 고무줄로 진동을 줄여보고자 노력했지만 별로 효과는 없더라구요..

RPM은 모두 동일
스펀지 장착	고무줄 장치 장착

위의 테스트에서는 RPM이 낮은데 높게하면 +90도~-90도 화이트노이즈수준으로 심하게 감지되더라구요 ㅠ

모터랑 센서가 싸구려라 그런가... ㅠ

아예 모터에서 발생하는 진동을 제거하는 방향으로도 해봐야겠습니다.

안드로이드 소스파일(accel gyro Kalman Filter, PWM generate):

SimpleAnalogInputActivity.java

GraphView.java

Matrix.java

SnowCube.java

SnowRenderer.java