파란만장 개발이야기

9개월간의 대장정이 끝났습니다~

맨 처음에 쿼드를 만들겠다고 하나 둘씩 알아보던게 엊그제같은데.. 시간이 벌써 이렇게 지났네요 ㅋ

생각해보면 기획 당시에는 너무 막연했던거 같아요 센서 융합만 되면 쿼드 제어는 식은죽 먹기라고 생각했거든요

한번 지난 9개월간 어떤 과정이 있었는지 정리해 볼겸 나열해 보겠습니다.

처음에.. 아두이노로 만들겠다고 생각을 했죠 그런데 요요보드가 눈에 띄어서 요요보드를 구매했습니다. 그리고 디바이스마트(개인적으로 매우 싫어합니다,, 이것들이 배송이면 배송 사후관리면 관리 제대로 할줄 아는게 없어요)에서 쿼드 프레임과 모터 세트를 샀죠. 

그당시에는 모터가 BLDC모터라는것도 모르고 변속기가 필요하다는것도 몰랐습니다. 모터가 도착해보니 선이 세개? 이건 뭐지? 하면서 알아보니 BLDC모터더라구요. 그리고 변속기가 필요하다는것도 그 후에 알았습니다. 그래서 변속기를 주문 하고 그 사이에 센서를 테스트 했습니다.

센서는 연구실에 돌아다니는 아날로그 가속도센서 달랑 하나만 가지고 테스트 했죠. 기울이니까 값이 달라지길래 와~ 이거면 충분하겠는데? 이랬습니다 ㅋㅋ 많이 기울이면 값이 많이 바뀌니까 그만큼 모터 속도만 증가시켜주면 되겠지~ 라고 생각했어요. 처음 접하시는분들이 대부분 이럴겁니다.

그런데 가속도센서를 좀 더 조사하다가 병진운동에 취약한 사실을 알게됬습니다. 센서를 기울였을때 하고 옆으로 이동시켰을때 값이 같다는걸 발견하고 이거 하나로는 안되겠다 싶더라구요 그래서 연구실선배한테 물어봤는데 칼만필터를 이용해서 가속도 자이로센서를 융합하면 그런걸 커버할 수 있다고 하더라구요. 그래서 책보고 칼만필터를 공부하고 구현했습니다.

여기까지 형상은 가속도자이로센서-요요보드-갤럭시s3 이렇게 이어져 있고 칼만필터나 PWM출력 조정 모두 폰에서 이루어 지고 있었습니다. 요요보드하고 센서 입출력 하기위해 TWI통신(I2C)구현하는것도 뼈빠지게 힘들었고 ㅠ 요요보드하고 폰하고 연결하는 어플 개발하는것도 고생했습니다.. 보드가 비주류다 보니까 인터넷에 공개된것도 별로 없고 있다고 해도 죄다 영어라 힘들더라구요 ㅠㅠ 그렇게 구축해놨는데 요요보드가 두번 고장나서 포기하고 아두이노로 갈아 탔습니다.

아두이노는 MPU6050과 연결은 쉬웠지만 휴대폰하고 연결하는게 매우 고생스러웠슴다.. 아두이노측에서는 시리얼통신만 하면 돼서 쉬운데 안드로이드측에서 보내고 받기 힘들어요. 그렇게 궁리를 하다가 usbOTG라는 젠더를 알게 돼서 구매 후 구글링을 통해 아두이노와 안드로이드간의 통신을 구현했습니다. 근데 baudrate가 9600밖에 안되더라구요.. 이때까지만 해도 모든 연산이 안드로이드폰에서 수행됬기 때문에 느린 속도는 큰 문제였습니다. 센서와 아두이노 사이의 통신, 아두이노에서 폰으로 센서 데이터 보내고 폰에서 이걸 연산한 다음 다시 보드로 pwm수치를 보내고 보드는 그걸 가지고 pwm출력을 하고.. 지금 생각해보면 진짜 번거롭고 쓸데없는데 시간을 낭비했네요 ㅠ 그 당시에 교수님이 연산을 보드에서 수행하라고 말한 이유가 있었어요.. 

그래서 연산과정을 보드로 옮기기 시작했습니다. 이 때 센서도 바꿨네요. mpu6050에서 mysen-L센서로 바꿨습니다. mysen-L은 내부에서 roll, pitch, yaw값을 계산한 후 시리얼 통신으로 결과값만 넘겨줍니다. 센서 융합을 해주는 셈이죠. 그리고 샘플링타임도 100Hz였구요. 이게 충분히 빠른건줄 알았습니다.ㅠ 바꾼 센서의 값을 통해 쿼드 날개의 속도를 조절해주는 제어를 해봤습니다. 지금 보면 P제어만 수행한거네요.ㅋ 제어가 잘 안되죠 당연히.. 그래서 쿼드 제어를 조사하기 시작했습니다. 네이버의 쿼드콥터 자작카페를 많이 참고 했네요. 그리고 교수님이 말씀하신 논문도 찾아보고 해석하고 공부하다보니 PID제어가 필요하다는걸 알게 되었습니다. PID제어기를 아두이노에 구현하고 1축 지그(시소놀이)를 만들어서 계속 테스트했습니다. 센서 노이즈와 싸우기도 하고 제어 주기를 줄여도보고 늘려도 보고 진동에 따른 오차를 잡기위해 밤낮없이 고생했던 기억이 있네요. mySen-L센서 자체가 진동에 민감하게 설계되었고 최대 샘플링타임이 100Hz라 진동을 잡기위해 LPF나 이동평균필터를 쓰자니 반응이 너무 느리고, 반응을 빠르게하기위해 필터를 안쓰자니 진동때문에 D제어가 너무 불안정하고.. 이걸 조절하기위해 진짜 엄청난 시간과 노력을 쏟아부었네요 ㅠ 전 100Hz면 충분히 뜰거같았어요 근데 쿼드콥터 자작 카페 글을 읽다보니까 300Hz는 되어야 제어가 된다 라고 하더라구요.. 근처에 이런걸 해본사람이 한마디 해줬으면 훨씬 수월했을텐데.. ㅠ 센서의 한계가 100Hz니까 이 센서 말고 다시 MPU6050으로 돌아왔습니다. 이거는 1000Hz가 넘더라구요. 이제 가속도 자이로 센서를 융합해서 roll, pitch, yaw를 구해야 하는데,

아두이노로 칼만필터를 옮겨봤지만 돌아가질 않아서(이유는 불명.. 부동소수점연산이나 행렬연산이 잘 안됐었는지.. 형변환이 잘 안됐었는지 모르겠네요) 다른 융합방법을 알아보다가 상보필터라는걸 알았습니다. 그리고 그걸 공부하고 구현한 다음 필터를 쿼드에 맞게 설계및 조정했습니다. 이것 또한 PID게인값을 조절하는거라서 많은 테스트 후에 쿼드만을 위한 상보필터를 구현했죠. 기울어진 정도는 시간지연 없이 바로 나오고 모터 진동의 영향도 거의 안받게 설계 했습니다. 그리고 이 값을 이용해서 PID로 자세제어를 하니 뜨긴 뜨더라구요. 그리고 지자기계를 이용해서 yaw축을 제어하려고 했는데, 왜인지 너무 불안정하더라구요.. 자꾸 틀어지고 특히 0에서 360도로 넘어가는 그 순간이 불안하기도 했구요. 그리고 알아보니 주변 환경에 매우 민감하다고 하더라구요.. 그래서 과감히 지자기계를 버리고 자이로센서값을 누적해서 yaw제어를 하니까 아주 잘됐습니다. 오차가 쌓이긴 하겠지만 생각보다 거의 없더라구요. 모터가 안돌아가는 상황보다 돌아갈때 자이로 적분 오차가 덜 누적되는 경향이 있었습니다.

그리고 이제 무선으로 제어하기 위해 장착된 폰과 조종기 폰의 통신을 만들어야하는데, 처음엔 wifi-direct로 만드려고 했습니다. 근데 이게 나온지 얼마 되지 않은 기술이라 그런지 정보가 얼마 없더라구요.. 예제도 달랑 하나 있는데 그대로 실행시켜봐도 잘 안돌아가고.. 그래서 결국 블루투스로 구현했습니다. 무선 환경 갖추고 무게중심 맞춰주고 실내에서 띄우려고 했지만 이게 조종도 어렵고 마음대로 가지도 않고.. 

실내에서 안되는거같아서 실 외로 나가니까 그나마 좀 잘 날아서 동영상 찍고 졸작 준비를 했습니다.

조종이 잘 안됬던게 d게인값이 너무 커서 그런게 아닐까~ 하는 생각이 드네요.

그 동안 더 자잘한 문제도 많고 힘든 과정도 많았지만 대충의 맥락은 이상이네요.. ㅋ 정말 많은걸 배웠습니다. 센서로 현실 물리량을 측정해서 현실 물리량을 제어해 내니까 어려웠지만 보람도 있고 공부도 많이 되었습니다.

최초 목적은 쿼드는 기본적으로 쉽게 날게 하고 폰으로 영상처리하는게 주 목적이었지만 쿼드 제어가 주 목적이 되어버렸어요 ㅠ 좀 아쉽지만 그래도 많이 배웠고, 제가 하고싶은 공부를 했으니 후회는 없습니다 ㅋ

그리고 포기하고 싶은때도 있었지만 포기 안하고 꿋꿋하게 해 낸 제가 자랑스럽기도 하네요 ㅋ 

다음 포스팅부터 당분간은 쿼드 만들기에 필요한 기초 지식, 제가 겪은 경험을 바탕으로 한 조언을 해볼 예정입니다. 자작하시는 분들에게 도움이 됐음 좋겠네요.

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

오늘은 상보필터(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

을 참고해주세요~

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

상보필터 센서 융합에 관한 글을 적다가 센서 설명하는 부분이 길어져서 아예 따로 포스팅하려고 합니다 ㅋ

쿼드콥터의 자세를 측정하기위해 센서가 필요한데, roll, pitch, yaw중 roll, pitch를 측정하기 위한 센서인 가속도, 자이로센서에 대해 포스팅하겠습니다. yaw는 지자기센서가 있어야 정확한 측정이 되므로 제외하구요 ㅋ

roll, pitch, yaw를 모르신다면 이전 포스팅을 참고하시구요. - 쿼드콥터 동역학 모델링_1(기본원리)

가속도 센서는, --------------------------------------------------------------------------------------

말그대로 가속도를 측정하는 센서입니다. 가만히 고정되어 움직이지 않는 상태라면 센서에 중력 가속도만 작용하는 상태겠죠. 그럼 그 중력 가속도를 x, y, z축 벡터 세개로 나누어서 수치로 크기를 표현해줍니다. MPU6050같은 경우는 각 축에대해 -16383~16383까지의 값이 있네요. 딱 부호있는 16비트네요

밑의 그림을 참고하시면 도움이 되실거 같습니다. (회색 상자가 센서입니다)

하지만 중력가속도 g뿐만이 아니라 다른 외력에도 너무 민감하다는게 단점입니다. 다른 글에는 병진 운동에 취약하다 라고 되어있는데 이해를 돕기위해 좀 극단적인 예를 하나 들어보면 위 그림의 센서가 자유낙하 하고 있는 경우(공기저항무시) g가 0이겠죠? 그럼 센서가 아무리 기울어져 있어도 측정이 안될겁니다. 이처럼 센서의 회전이 아니라 이동에 취약하고 더 나아가 진동에도 약한 단점이 있습니다. 진동을 변위가 작은 이동이라고 보면 되겠네요. 하지만 두번째 그림인 상태에서 아무리 센서를 흔들고 던지고 회전시킨다음에 다시 두번째 상태로 돌아오더라도 흔들기 전과 후의 값이 같다는 특징이 있습니다. 

출력값을 보시면, (가속도센서값을 roll로 변환한 후 출력)

제가 손으로 임의로 실험한거라 손노이즈(?)도 있는거 같은데 여기서 주의깊게 보실 점은 구불구불한 노이즈들(밑의 자이로출력과 비교해보세요)과 멈췄을 때 값이 일정하게 유지된다는 것입니다.

정리하면 가속도 센서는 노이즈가 심하고 움직임의 값이 왜곡될 수 있지만(=부정확하지만) 시간이 지나도 오차가 누적되지 않는다.

자이로센서는, --------------------------------------------------------------------------------------

가속도센서와는 다르게 각속도를 검출하는 센서입니다. 선속도와 각속도의 차이, 속도와 위치의 관계를 안다는 가정 하에 설명할게요~

그림그리기가 좀 힘드네요..;; 위 그림에서 빨간선과 파란선의 각도의 변화량을 x, y, z축 각각의 변화량으로 표현해줍니다. 즉, 아무 움직임도 없다면 x, y, z축 값이 모두 0이라는 거죠. 여기서 중요한 점은 각 속도이기 때문에 기울어진 각도(위치)를 구하려면 적분을 해줘야한다는겁니다. 

출력값을 보시면, (자이로센서값을 roll로 변환한 후 출력)

위의 가속도센서 출력값과 같은 구간입니다. 처음보시는 분들은 가속도센서값이 더 정확해 보이시겠지만 자이로센서값이 실제 제 손 움직임과 더 비슷합니다. 다만, 계속 밑으로(마이너스 쪽으로) 발산해 가는데 이것이 자이로센서가 가진 단점입니다. 계속 적분하는 과정에서 센서의 노이즈도 같이 적분되기 때문에 이렇게 드리프트가 생기더라구요. 

정리하면, 자이로 센서는 실제 값과 비슷하지만(=정확하지만) 누적오차가 발생한다.

자 여기서, 가속도 자이로 값으로 같은 현상(roll, pitch같은)을 측정할 수 있지만 서로 장단점이 있다는걸 알 수 있죠, 그리고 서로의 단점이 상대의 장점이 된다는것도요

이 두 센서의 장점을 합치면 아래 그림의 파란색처럼 됩니다.

이걸 합치는 상보 필터에 대해 다음에 포스팅 하겠습니다.

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

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

쿼드콥터 초기 계획안 

재료, 부품 구매 현황

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