Quadrotor20152015. 7. 24. 23:55

포스팅이 뜸한 사이 윙윙이(쿼드로터)에 많은 변화가 있었습니다~


점프선으로 복잡하게 얽혀있던 선을 모두 자르고 컴팩트하게 납땜해서 보기좋고 가볍게 해놨고, 센서에 방진장치로 양면테이프+스펀지를 대서 방진효과를 기대할 수 있게 해놨고, 불량 모터 교체, 라즈베리파이에서 오드로이드C1로 개선, 개발환경 구축, 시리얼 통신 비용 감소, +콥터에서 x콥터로 변경, 조종 어플 개선 등등 많이도 했네요.


그리고 그 동안은 게인값 튜닝에 많은 시간 투자를 했었는데.. 홧김에 제어기를 추가하니까 확실히 성능이 개선됐습니다.


아래 영상은 수동 조종 모드로 호버링을 시도한 영상입니다.



고도는 80cm로 고정시켜 놓았고 tack-off 버튼과 landing버튼으로 이륙과 착륙을 할 수 있게 해봤습니다. yaw각도 고정입니다.


이제 시작이네요! 이걸로 뭘 하느냐가 중요한 것 같습니다.


부셔먹지 말고 조심히 잘 해야지..


------------------------------------------------- 2015.07.29 추가 -------------

위 영상의 사용된 하드웨어는 아두이노Mega2560 + 오드로이드C1 + 갤럭시s4입니다. 오드로이드 운영체제는 RTOS가 아닌 하드커널에서 제공하는 우분투를 사용했고, 갤럭시s4는 안드로이드 KitKat(4.4.2)입니다. 

무선통신은 Wifi를 사용해 UDP통신을 하는 중이고(가끔 딜레이가 생기던데 원인 파악중..), 안드로이드 쪽에서 핫스팟을 켜두면 오드로이드에서 붙는 방식을 취하고 있습니다~







Posted by 너를위한노래

댓글을 달아 주세요

  1. 라임맛케익

    가속도, 자이로 센서값을 오일러각으로 변환-1을 보고 예전 상보필터설계 부분을 봤는데요 상보필터에 사용되는 acc이나 gyro 값들은 각각 가속도를 변화하여 만든 오일러각과 자이로값을 변화하여 만든 오일러각인것 맞나요? 그리고 '가속도, 자이로 센서값을 오일러각으로 변환-2'를 보고 싶은데 혹시 올려주실수 있나요?ㅠㅠ

    2015.07.25 16:21 [ ADDR : EDIT/ DEL : REPLY ]
    • 네 각각의 오일러각이 맞습니다~ 2편은.. 상보필터에대한 내용인데요 현재 작성중입니다.ㅜ 급하시면 예전에 썼던 상보필터 내용을 참고하시면 될거같아요~ 비슷한 내용입니다.

      2015.07.26 14:49 신고 [ ADDR : EDIT/ DEL ]
  2. 쿼드콥터

    통신을 사용하신거 같은경우에 블루투스 통신을 이용해서 조종하시고 계신건가요?

    2015.07.28 14:38 [ ADDR : EDIT/ DEL : REPLY ]
  3. 비밀댓글입니다

    2015.08.13 21:30 [ ADDR : EDIT/ DEL : REPLY ]
  4. 제이기를 추가하셨다고 했는데 구체적으로 어떻게 무엇을 추가해주셨는지 설명 부탁해도 될까요?? ^^

    2015.08.26 11:22 신고 [ ADDR : EDIT/ DEL : REPLY ]
  5. 프롭가드는어떻게구매하셨아요?

    2015.09.03 22:18 [ ADDR : EDIT/ DEL : REPLY ]
  6. 비밀댓글입니다

    2016.03.30 20:51 [ ADDR : EDIT/ DEL : REPLY ]
  7. 비밀댓글입니다

    2016.08.18 13:21 [ ADDR : EDIT/ DEL : REPLY ]
    • 먼저 위 글의 영상은 optical flow를 사용하지 않은 영상인 점 알아두시구요, 이 후에 올렸던 호버링 영상을 기준으로 말씀드리겠습니다. optical flow 센서(e.g. px4flow)는 사용하지 않았고 일반 모노카메라를 사용했습니다. 모노카메라로 optical flow를 통해 속도를 추정하고 이를 가속도, 자이로, 초음파 센서의 도움을 받아 실제 m/s의 속도를 구했었습니다. 말씀하신 것처럼 단순히 optical flow만으로는 호버링이 어렵습니다. 노이즈도 심하고 기체의 회전운동과 병진운동을 구분하기도 힘드니까요..

      2016.08.18 23:07 신고 [ ADDR : EDIT/ DEL ]
    • 비밀댓글입니다

      2016.08.24 13:20 [ ADDR : EDIT/ DEL ]

Android2015. 2. 8. 18:59

먼저, 밑의 참고 사이트를 참고해 만든 예제 소스를 먼저 올리겠습니다.


UTubeTest.zip


검색어를 가지고 유튜브에서 검색한 후 결과를 리스트뷰로 보여주는 예제인데요.


두가지를 짚고 넘어가겠습니다.



첫번째로, StartActivity를 보시면 검색 시에 get방식으로 정보를 전달하는걸 볼 수 있습니다. 어떤 name으로 무슨 정보를 전달해야 하는지는 다음의 링크를 참고하시기 바랍니다.


https://developers.google.com/youtube/v3/docs/search


첨부한 예제에는 반환되는 리스트의 갯수를 20개로 늘려놨습니다. (기본은 5개입니다)



두번째로는, 서버 키를 얻는 방법입니다.


https://console.developers.google.com/project


에 가셔서 프로젝트를 하나 생성한 후, 그 프로젝트를 클릭하면 왼쪽에 API 및 인증이 보입니다. 그 하위메뉴인 API에서 YouTube Data API v3활성화 해야합니다.


그 후, 사용자 인증 정보에서 공개 API 액세스를 위해 새 키를 만들어야 하는데요


새 키 만들기 클릭 후 서버 키를 만드세요.


그리고 안드로이드 폰의 IP주소를 구해서(www.ipip.kr) 해당 IP주소를 요청 허용해주세요.


이제, 만들어진 서버 애플리케이션 키의 API키를 첨부한 예제의 StartActivity와 MainActivity에 각각 넣어야 정상 작동 됩니다.



구글 GCM를 사용하거나 기상청 정보를 얻을 때 이러한 방법이 종종 이용되곤 하죠.


또한 유튜브에 요청한 데이터 결과는 JSON 형식으로 돌아오는데 알맞게 파싱하여 사용해야 합니다.


다행히 관련 라이브러리가 있어서 편하게 사용할 수 있네요.



참고 사이트 : http://ondestroy.tistory.com/49

API 가이드 : https://developers.google.com/youtube/v3/







Posted by 너를위한노래

댓글을 달아 주세요

  1. 유튜브궁금해요

    안녕하세요 이 글을 보실지 모르겠는데....우선 남겨봅니다.
    현재 해당소스 실행 시 에러가 나네요 Caused by: java.lang.IllegalArgumentException: Illegal character in query at index 85: https://www.googleapis.com/youtube/v3/search?part=snippet&maxResults=20&q=1&key=Input your ServerKey
    유투브 api관련해서 공부해보려는데 좋은자료 감사합니다 : )
    혹시 저 내용 원인을 알수 있을까용?

    2015.07.01 17:04 [ ADDR : EDIT/ DEL : REPLY ]
    • Key 관련 문제인 것 같습니다. 제가 올린 소스는 아마 키 부분이 "input your serverkey"라는 문자열로 들어가 있을 텐데.. 그 부분을 직접 수정하셔야 합니다.

      2015.07.07 14:26 신고 [ ADDR : EDIT/ DEL ]
  2. 유튜브궁금해요

    아아...... 그렇군요...바보같았네요 ㅋㅋㅋ 답변감사합니다 :=)
    하다가 궁금한거 있으면 질문드려도될까요?ㅎㅎ

    2015.07.08 09:48 [ ADDR : EDIT/ DEL : REPLY ]
  3. dog발자

    우선 정말 잘 돌아가고 있습니다.
    궁금한게 있는데 자신의 핸드폰의 ip를 가지고 키를 만들게 되면
    다른 스마트 폰에서는 돌아가지 않게 되는건가요?

    2015.07.10 15:55 [ ADDR : EDIT/ DEL : REPLY ]
    • 유튜브궁금해요

      잘되시나요? 제가하면 왜 bad request라고 뜰까요ㅠㅠ서버키가 문제인가... 흠;;

      2015.07.13 08:50 [ ADDR : EDIT/ DEL ]

Android2014. 11. 28. 16:10

이번에는 안드로이드 NDK개발환경 구축에 관해서 간단하게(스크린샷 없이) 포스팅 할건데요, 제 얕은 지식으로 이해한 바를 최대한 써보겠습니다. ㅜㅜ


개발환경 구축에 필요한 툴과 라이브러리는 다음과 같습니다.


JDK

JDK_7_51 - (http://www.oracle.com/technetwork/java/javase/downloads/java-archive-downloads-javase7-521261.html#jdk-7u51-oth-JPR)

환경변수 설정

이클립스

케플러 - (http://www.eclipse.org/downloads/packages/release/Kepler/SR2)

안드로이드 ADT설치 (help - install new software)

안드로이드

SDK - adt-bundle-windows-x86_64-20140321

안드로이드용 OpenCV

OpenCV-2.4.9-android-sdk

안드로이드 NDK

android-ndk-r10c-windows-x86_64


다운로드, 설치 방법과 경로 설정 등은 다른 블로그에 많이 나와있으니 생략하겠습니다.


모두 설치하셨다면, 프로젝트를 만들어야 할텐데요. 맨땅에다가 만들지 않고 예제를 수정하는 방법으로 만드려고 합니다.


OpenCV 예제 중 OpenCV Tutorial 2 - Mixed Processing 이라는 예제가 있을텐데요, 이 안에서는 CameraBridgeViewBase로 카메라를 다루지만 AutoFocus기능을 사용하기 까다롭더라구요 (Camera객체를 만들어서 start시키려고 하면 CameraBridgeViewBase와 충돌하는 듯 합니다). 하지만 NDK개발이 가능합니다.


그리고 OpenCV Tutorial 3 - Camera Control 이라는 예제는 CameraBridgeViewBase를 상속받은 JavaCameraView를 사용해서 카메라를 다루는데, 이 클래스 안에 mCamera가 Camera 객체이기 때문에 AutoFocus기능을 사용하기 수월합니다. 하지만 NDK개발 구축이 안되어있습니다.


이 두 예제를 합칠건데요, 일단 그냥 Tutorial 2 의 패키지를 지워버리고  Tutorial 3의 패키지를 가져오는 식으로 하셔도 됩니다. 


다만 두 프로젝트간에 다른건 조정 해줘야합니다. Menifest파일의 패키지와 액티비티 이름, 옮긴 후 액티비티의 onCreate에서 레이아웃 이름 등등은 알아서 조절하시길 바랍니다.


이렇게 해서 만들어진 프로젝트는 JavaCameraView의 mCamera를 이용한 AutoFocus가 가능하고, JavaCameraView의 onCameraFrame에서 OpenCV영상처리도 가능하며 onCameraFrame에서 native 함수를 호출함으로써 NDK 영상처리 개발도 가능합니다.




Tip : native 함수는 Java_com_abc_core_MainActivity_func() 처럼 생겼는데 맨 처음 Java는 고정이고 다음은 com.abc.core.MainActivity라는 액티비티의 func()함수로 사용하겠다는 의미입니다. 즉 위처럼 선언된 네이티브 함수는 com.abc.core.MainActivity 액티비티에 public native void func(); 이렇게 선언되어있어야 합니다. 오타가 나거나 경로가 틀리면 native 함수를 찾을 수 없다고 출력되며 앱이 종료됩니다.





Posted by 너를위한노래

댓글을 달아 주세요

Quadrotor20132013. 11. 4. 17:09

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게인값이 너무 커서 그런게 아닐까~ 하는 생각이 드네요.


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


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


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


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


Posted by 너를위한노래

댓글을 달아 주세요

  1. 날마

    안녕하세요 저도 쿼드콥터를 만들고있는데요 yaw때문에 작업이 멈추었습니다 ㅠ
    자이로센서만으로 yaw를 측정하셨다고 하셨는데 초당 몇도정도 각도의 차이가 나나요??
    저는 가만히 놔두면 초당 거의 1도정도 차이가나서.... 지자기센서만 이용해서 yaw를 측정하려하니
    roll이나 pitch로 회전시키면 yaw의 값도 변해서 지자기센서는 사용을 못하겠더라구요 ㅠ

    2014.03.06 22:40 [ ADDR : EDIT/ DEL : REPLY ]
    • 쿼드콥터의 모터를 작동하기 전에 정지상태의 자이로센서 데이터를 각 축마다 N개 추출합니다. 그리고 그 합을 N개로 나눈 a를 각 축의 출력값에 매번 빼주면 드리프트를 줄일 수 있습니다. 저는 N을 1000으로 했네요. 그럼에도 드리프트는 존재하는데 이걸 줄이는 방법으로 HDR등이 있겠습니다~ 이 내용에 관해 곧 포스팅할 예정입니다.

      2014.03.14 14:51 신고 [ ADDR : EDIT/ DEL ]
  2. 비밀댓글입니다

    2014.03.25 22:47 [ ADDR : EDIT/ DEL : REPLY ]
  3. 방향

    아두이노랑 mpu6050 사용해서 쿼드콥터 구현하려는 학생입니다
    BLDC 모터 구동이랑 MPU6050센서값 읽는 콘솔창 까지띄운것 까지는 완료한 상태인데
    그 뒤로 1주일째 진전이 없는 상태입니다........
    센서값을 적용해서 PID 소스코딩을 해야되는데
    혹시 공부하는데 도움이 많이됬던 서적이나 자료 좀 알려 주실수 있나요?

    2015.07.21 23:02 [ ADDR : EDIT/ DEL : REPLY ]
  4. 초보자가 많은 도움이 되었습니다.
    고맙습니다.

    2015.09.28 10:17 신고 [ ADDR : EDIT/ DEL : REPLY ]
  5. 중얼거북이

    정말대단하시네요 .... 글잘봤습니다.

    2017.03.06 09:40 [ ADDR : EDIT/ DEL : REPLY ]
  6. 감자

    스스로 드론을 제작해보려고 참고할 만한 블로그 찾는 중이었는데요

    좋은 정보 감사합니다.

    2017.09.19 18:29 [ ADDR : EDIT/ DEL : REPLY ]

Android2013. 4. 1. 15:08

 

칼만필터 적용 과정중에 구현한 행렬 연산 소스입니다.

 

안드로이드에서 만든건데 당연히 자바에서도 되겠네요

 

4x4 행렬의 덧셈, 뺄셈, 곱셈을 이중 for문으로 계산해서 반환해줍니다.

 

행렬 연산하시는분 참고하세요~

 

 함수 정의 부

 // 4x4 행렬 덧셈
  double[][] matrix_plus(double[][] A, double[][]B) {
   double[][] C = new double[4][4];
   for(int i=0; i<4; i++) {
    for(int j=0; j<4; j++) {
      C[i][j] = A[i][j] + B[i][j];
     }
     
    }
   return C;
  }
  // 1x4 행렬 덧셈
  double[] matrix_14plus14(double[] A, double[] B) {
   double[] C = new double[4];
   for(int i=0; i<4; i++) {
      C[i] = A[i] + B[i];
    }
   return C;
  }
  // 4x4 행렬 뺄셈
  double[][] matrix_minus(double[][] A, double[][]B) {
   double[][] C = new double[4][4];
   for(int i=0; i<4; i++) {
    for(int j=0; j<4; j++) {
      C[i][j] = A[i][j] - B[i][j];
     }
     
    }
   return C;
  }
  // 1x4 행렬 뺄셈
  double[] matrix_14minus14(double[] A, double[]B) {
   double[] C = new double[4];
   for(int i=0; i<4; i++) {
      C[i] = A[i] - B[i];
     }
   return C;
  }
  // 4x4 행렬 곱셈
  double[][] matrix_mul(double[][] A, double[][]B) {
   double[][] C = new double[4][4];
   for(int i=0; i<4; i++) {
    for(int j=0; j<4; j++) {
     for(int k=0; k<4; k++) {
      C[i][j] += A[i][k] * B[k][j];
     }
     
    }
   }
   return C;
  }
  // 1x4 * 4x4 행렬 곱셈
  double[] matrix_14mul44(double[] A, double[][]B) {
   double[] C = new double[4];
   for(int i=0; i<4; i++) {
     for(int j=0; j<4; j++) {
      C[i] += A[j] * B[j][i];
     }
   }
   return C;
  }
  // 4x4 행렬 전치
  double[][] matrix_transed(double[][] A) {
   double[][] C = new double[4][4];
   for(int i=0; i<4; i++) {
    for(int j=0; j<4; j++) {
      C[i][j] = A[j][i];
    }
   }
   
   return C;
  }
  // 4x4 행렬 역행렬
  double[][] matrix_inverse(double[][] A) {
   
   double[][] C = new double[4][4];
   
   Matrix mat = new Matrix(4,4);     // Matrix.java는 파일첨부
   for(int i=0; i<4; i++) {
    for(int j=0; j<4; j++) {
     mat.values[i][j] = A[i][j];
    }
   }
   Matrix.Invers inv = mat.new Invers();
   C = inv.Invers(mat);
   
   return C;
  }

 

밑에 소스는 구글링에서 퍼온건데.. 기억이 안나네요 ㅠㅠ (아시는분 댓글 달아주시면 바로 출처 올리겠습니다~)

 

4x4 역행렬 연산에 쓰이는 Matrix 클래스입니다.

 

 Matrix.java

 public class Matrix {
 public int row; // 행 전역변수 선언
 public int col; // 열 전역변수 선언
 public double[][] values; // 2차원 배열 행렬 전역변수 선언

 public Matrix(int row, int col) { // 생성자 Matrix 생성
  this.row = row; // 행의 값을 받는다.
  this.col = col; // 열의 값을 받는다.
  values = new double[row][col];// 전역변수 values에 row값과 col값을 넣어준다.
 }

 class Invers // Invers 메소드
 {
  determi det = new determi(); // 행렬식을 이용하기 위해 불러옴

  public double[][] Invers(Matrix mat) {
   Matrix inv = new Matrix(mat.row, mat.col); // 역행렬을 저장한 행렬 생성
   Matrix cofactor = new Matrix(mat.row, mat.col);
   double de = (double) 1 / det.data(mat);

   if (det.data(mat) == 0) // 행렬식이 0일 경우
   {
    System.out.println("역행렬이  있지 않습니다.");// 메세지 툴력
    System.exit(0); // 그리고 시스템 exit
   }

   for (int i = 0; i < mat.row; i++) { // 0부터 행 값까지 반복
    for (int j = 0; j < mat.col; j++) {// 0부터 열 값까지 반복
     cofactor.values[i][j] = Mino(mat, i, j);
     inv.values[j][i] = de * Math.pow(-1, i + j)
       * cofactor.values[i][j];
    }
   }
   return inv.values;// inv를 리턴한다.
  }
  
  public double Mino(Matrix mat, int r, int c) {// 여인수전개를 위한 메소드
   double cof;
   Matrix minor = new Matrix(mat.row - 1, mat.col - 1);
   int k = 0;
   if (k == r) {
    k++;
   } // C_ij의 소행렬은 i행j열을 제외하므로 이런 if를 쓴다.
   for (int i = 0; i < minor.row; i++) // 0부터 행값까지 반복
   {
    int l = 0; // l은 초기값으로 0
    if (l == c) {
     l++;
    }// l값이 열값과 같다면 l증가
    if (k == r) {
     k++;
    }// k값이 행값과 같다면 k증가
    for (int j = 0; j < minor.col; j++) {
     if (l == c) {
      l++;
     } // 열값과 같아면 l증가
     minor.values[i][j] = mat.values[k][l];
     l++;
    }
    k++;
   }
   cof = det.data(minor);// 소행렬을 구해 반환
   return cof;
  }
 }

 static class determi {
  static int order;
  static double[][] matrix;

  public double data(Matrix mat) // 행렬식을 구할 행렬을 받는다.
  {
   matrix = new double[mat.row][mat.col];
   order = mat.row; // order는 받은 행렬의 행의 수이다.
   for (int i = 0; i < mat.row; i++)// i를 행의수만큼 돌리고
   {
    for (int j = 0; j < mat.col; j++) // j는 열의수만큼 돌려서
    {
     matrix[i][j] = mat.values[i][j]; // matrix에 mat과 같은 값을 넣는다.
    }
   }
   return compute(); // 리턴을 compute함수로 시켜준다.
  }

  public static double compute() // 그럼 여기로 오는데,
  {
   double check[] = new double[20]; // check란 20칸짜리 배열을 만든다.
   double det = 0; // det는 초기화하고
   int row, col = 0; // row와 col도 초기화선언한다.
   for (row = 0; row < order; row++) // row는 받은 행의 수 만큼 돌리고
   {
    check[row] = 1; // 이때 check의 행의수만큼의 index를 1로 지정
   }

   if (order == 1)// 만약 order가 1이면
   {
    det = matrix[0][0]; // det는 a11값이 된다.
   }

   for (row = 0; row < order; row++) // 다시 row를 수만큼 돌림
   {
    check[row] = 0;// row[i]에 0을 집어넣음
    det += matrix[row][0] * Math.pow(-1, row)
      * minor(row, col + 1, check);
    // det를 1열의 수 곱하기 minor함수에서 받은 값으로 온다
    check[row] = 1; // 이렇게 계산 되면 다시 check를 1로 해준다.
   }
   return det; // det 값을 반환
  }

  public static double minor(int Row, int Col, double trans[]) {
   double result = 0;// 결과값으로 초기값을 0을 준다.
   int i = 0;
   if ((order - Col) == 0) {
    return 1;
   }
   for (int row = 0; row < order; row++)// 0부터
   {
    if (trans[row] != 0)// 만약 trans[row]가 0이 아니라면
    {
     trans[row] = 0; // 0을 넣어준다.
     result += matrix[row][Col] * Math.pow(-1, (double) i)
       * minor(row, Col + 1, trans);
     // 여인수전개에 의한 식이다. 재귀함수로 계속 돌리게된다.
     trans[row] = 1; // trans[row]에 1을 넣어준다.
     i++;// i 값 증가
    }
   }
   return result; // result 반환
  }
 }

}


 

 

Posted by 너를위한노래

댓글을 달아 주세요

Quadrotor20132013. 3. 13. 18:01

 

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

 

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

쿼드콥터 초기 계획안 

재료, 부품 구매 현황

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

 

 

 

 

Posted by 너를위한노래

댓글을 달아 주세요

  1. GTS

    고무줄을 이용한 방진장치.. 작은 키캠에 사용하기엔 정말 딱일것 같습니다. 고무줄을 조금 느슨하게 하면 조금이라도 진동을 흡수하지않을려나요?ㅎㅎ

    2013.04.01 22:45 [ ADDR : EDIT/ DEL : REPLY ]
    • 아 네이버 블로그에 있던 그 학생이시군요~ 현재 고무줄도 조금 느슨하게 바꿨고 중간 센서 밑에 납으로된 추도 달아 놨는데.. 자잘한 진동에는 효과있을지 몰라도 큰 진동에는 역시 변화가 없네요.. ㅠ 일단 자세제어부터 하고 다시 손 봐야겠습니다. ㅠ

      2013.04.02 13:17 신고 [ ADDR : EDIT/ DEL ]