자세 해산 지식 (3) - 팽이 가속도 계 6 축 데이터 융합

자세 해산 지식 (3) - 팽이 가속도 계 6 축 데이터 융합
2015-11-16
이렇게 오 랜 습관 적 인 감독 은 결국 헛 보지 않 았 고 블 로그 의 지적 을 받 아 요 며칠 동안 Mahony 의 9 축 데이터 융합 알고리즘 을 알 아 봤 습 니 다.안 타 깝 게 도 2 판 하드웨어 가 아직 도착 하지 않 았 습 니 다. 자력 계 는 사용 할 수 없고 효 과 를 검증 할 수 없습니다. 오늘 은 먼저 자 이 로 와 가속도 계 의 6 축 데이터 융합 을 정리 하 겠 습 니 다.
가 계 와 자 이 로 는 자 세 를 계산 할 수 있 지만 왜 이들 을 융합 시 켜 야 하 는 것 일 까? 가속도 계 는 진동 과 같은 교란 에 민감 하지만 장기 적 인 데이터 로 계 산 된 자 세 는 믿 을 수 있 고 자 이 로 는 진동 에 민감 하지 않 지만 장기 적 으로 자 이 로 를 사용 하면 표류 할 수 있 기 때문에 우 리 는 상호 보완 을 해 야 한다. 단기 적 으로 팽 이 를 믿 고 장기 적 으로 가 계 를 믿 어야 한다.그러나 사실은 가 계 는 항행 방향 각 을 수정 할 수 없고 항행 방향 각 을 수정 하려 면 자력 계 가 필요 하 다. 즉, 다음 에 정리 해 야 할 9 축 데이터 융합 이다.
융합 하기 전에 먼저 센서 의 원시 데 이 터 를 처리 해 야 한다.이상 적 인 상황 에서 가속도 계 를 수평 으로 배치 할 때 XY 축 은 0 으로 출력 되 어야 합 니 다. Z 축 만 1 개의 G 를 출력 해 야 합 니 다. 따라서 우 리 는 가속도 계 에 대해 XY 축의 0 점 교정 을 해 야 합 니 다 (Z 축 은 함께 교정 할 수 없 음 을 주의 하 세 요 ~).마찬가지 로 자 이 로 가 수평 으로 정지 되 어 있 을 때 각 축의 출력 은 0 이 어야 하기 때문에 자 이 로 를 세 축 으로 교정 해 야 한다.방법 은 기체 표준 수준 을 정지 시 킬 때 1200 번 의 데 이 터 를 수집 하여 평균 적 으로 교정 값 으로 저장 하 는 것 이다. 그리고 작업 상태 에서 각 축 이 출력 한 데 이 터 는 수집 한 데 이 터 를 교정 값 으로 줄 이 는 것 이다.이것 만 으로 는 부족 합 니 다. 자 이 로 는 필 터 를 하지 않 아 도 받 아들 일 수 있 지만 가속도 계 의 소음 이 비교적 크기 때문에 적어도 미끄럼 창 필 터 를 해 야 합 니 다. 저 는 20 깊이 의 미끄럼 창 을 사 용 했 습 니 다. 데 이 터 는 큰 파동 이 있 었 습 니 다. 그러나 마지막 으로 계 산 된 자세 각 은 0.3 도 정도 의 떨 림 만 있 었 습 니 다.(일반적으로 8 깊이 만 권장 하면 되 는 것 같 아서 미끄럼 창 필터 효 과 는 더 좋 을 수 없습니다. 칼 만 필 터 를 추가 하 는 것 을 고려 할 수 있 습 니 다. 아직 연구 하지 않 았 지만 이렇게 프로세서 의 연산 량 이 올 라 갑 니 다)(11.28 보충: 뒤에 8 깊이 슬라이딩 필 터 를 사용 하고 자 이 로 기 를 16 단계 카 이 저 창 필 터 를 한 후에 0.1 도 안 되 는 파동 만 있 지만 카 이 저 필터 의 매개 변 수 는 아직 정 해 지지 않 았 습 니 다. 다른 사람 이 정 한 매개 변 수 를 사용 하여 최근 에 계속 연구 하고 있 습 니 다). 필터 효 과 는 그림 과 같 습 니 다. 다음 6 축 데이터 융합 코드:(코드 소스 네트워크, 나 는 이전 사람들의 기초 위 에 주석 을 더 해서 조금 바 꾸 었 다)

1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71

\ # defineKp 10.0 f / / 이곳 의 KpKi 는 가속도 계 수정 팽이 계 의 속 도 를 조정 하 는 데 쓰 인 다 \ # defineKi 0.008f \ # definehalfT 0.001f / / 샘플링 주기의 절반 으로 4 원수 미분 방정식 을 풀 때 각 증 가량 floatq 0 = 1, q1 = 0, q2 = 0, q3 = 0; / / 초기 자세 4 원수 변환 을 위 한 4 원수 공식 으로 floatexInt = 0, eyInt = 0, ezInt = 0; / 현재 가 계 측 된 중력 가속도 가 세 축 에 있 는 분량 / / 현재 자세 로 계 산 된 중력 이 세 축 에 있 는 분량 의 오차 의 적분 voidIMUupdate (float gx, float gy, float gz, float ax, float ay, float az) / / g 표 팽이 계, a 표 가산 계{float q0temp, q1temp, q2temp, q3temp;양 적 인 오차 / / 이 값 으로 얻 은 값 을 먼저 계산 해 float q0q0 = q0 * q0 = q0 * q0; float q0q1 = q0 * q1 = q0 * q1; float q0 q2 = q0 * q2; float q1q1 = q1 * q1; float q1q1 q3 = q1 * q3; float q2 q2 = q2 * q2; float q2q2 q3 = q2 * q2 * q3; float q3q3 q3 = q3 * q3 = q3 * q3; if (ax * ay * * az= = 0) / / 가산 이 자유 낙 체 상태 에 있 을 때 자세 해산 을 하지 않 을 때 자세 해산 을 하지 않 으 면 분산 이 생 길 수 있 기 때문에 분 산 이 생 길 수 암컷 무한대 의 경우 return; norm = sqrt(ax * ax + ay * ay + az * az); / 단위 화 가속도 계, ax = ax / norm; / 이렇게 양 적 정 도 를 변경 해도 KP 파 라 메 터 를 수정 할 필요 가 없습니다. 여기 서 ay = ay / norm; az = az / norm; / 현재 자세 로 중력 이 세 축 에 있 는 분량 을 계산 합 니 다.(박문 1 에서 언급) vx = 2 * (q1q 3 - q0q 2), vy = 2 * (q0q 1 + q2q 3), vz = q0q 0 - q1q 1 - q2q 2 + q3q 3;(az*vx - ax*vz) ; ez = (ax*vy - ay*vx); exInt = exInt + ex * Ki; / 오 차 를 포인트 eyInt = eyInt + ey * Ki; ezInt = ezInt + ez * Ki; / adjusted gyroscope measurements gx = gx + Kp * ex + exInt;이것 은 바로 포인트 가 증가 하거나 감소 하 는 것 입 니 다. / / 아래 에서 자 세 를 업데이트 하 는 것 입 니 다. 즉, 4 원 수 미분 방정식 의 풀이 q0 temp = q0; / 현재 값 을 잠시 저장 하여 q1 temp = q1 을 계산 하 는 데 사용 합 니 다. / / 네트워크 에 올 라 온 이 알고리즘 은 대부분 이 문 제 를 주의 하지 않 았 습 니 다. 여기 서 q2 temp = q2; q3 temp = q3; / 1 단계 필 카드 해법 을 사용 하고 관련 지식 은 을 참조 할 수 있 습 니 다.P212 q0 = q0temp + (-q1temp*gx - q2temp*gy -q3temp*gz)*halfT; q1 = q1temp + (q0temp*gx + q2temp*gz -q3temp*gy)*halfT; q2 = q2temp + (q0temp*gy - q1temp*gz +q3temp*gx)*halfT; q3 = q3temp + (q0temp*gz + q1temp*gy -q2temp*gx)* halfT; / 단위 화 4 원 수 는 공간 에서 회전 할 때 늘 어 나 지 않 고 회전 각도 만 있 습 니 다. 이 는 선형 대수 에서 의 양 교 변환 norm = sqrt (q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3) 와 유사 합 니 다.; q0 = q0 / norm; q1 = q1 / norm; q2 = q2 / norm; q3 = q3 / norm;(2 * q2 * q3 + 2 * q0 * q1,-2 * q1 * q1 - 2 * q2* q2 + 1)* 57.3; // roll }
 
상기 코드 도해: 시 나 닷 컴 블 로그 의 코드 삽입 이 불편 하고 형식 이 엉망 이 되 었 습 니 다. 마지막 으로 잊 어 버 렸 습 니 다. 들 어 오 는 다 자 이 로 데 이 터 는 하나의 계 수 를 곱 하여 라디안 속도 로 바 꿔 야 한 다 는 것 을 잊 지 마 세 요!! 보충, 경사도 하강 법 6 축 융합 도해: