9 자유도 센서가 장착 된 Arduino 보드가 있는데, 보드의 피치, 요 및 롤을 결정해야합니다.

다음은 9-DOF 센서의 데이터 세트 중 하나의 예입니다.

가속도계 (m / s)

• = -5,85$\text{Acc}_{X}$
• = 1,46$\text{Acc}_{Y}$
• = 17,98$\text{Acc}_{Z}$

자이로 스코프 (RPM)

• = 35,14$\text{Gyr}_{X}$
• = -40,22$\text{Gyr}_{Y}$
• = -9,86$\text{Gyr}_{Z}$

자력계 (가우스)

• = 0,18$\text{Mag}_{X}$
• = -0,04$\text{Mag}_{Y}$
• = -0,15$\text{Mag}_{Z}$

이 데이터에서 피치, 요 및 롤을 어떻게 계산할 수 있습니까?

피치, 롤 및 요는 X, Y 및 Z 축을 중심으로 한 회전으로 정의됩니다. 아래 그림은 정의를 설명하기위한 것입니다.

이전 프로젝트에서 Analog Devices의 ADXL345 가속도계를 사용하여 롤 및 피치를 계산했습니다. 다음은 롤 및 피치를 계산하는 데 사용되는 방정식입니다. 소스 코드 중 일부를 공개적으로 사용할 수 있도록 만들었습니다.

accelerationX = (signed int)(((signed int)rawData_X) * 3.9);
accelerationY = (signed int)(((signed int)rawData_Y) * 3.9);
accelerationZ = (signed int)(((signed int)rawData_Z) * 3.9);
pitch = 180 * atan (accelerationX/sqrt(accelerationY*accelerationY + accelerationZ*accelerationZ))/M_PI;
roll = 180 * atan (accelerationY/sqrt(accelerationX*accelerationX + accelerationZ*accelerationZ))/M_PI;

완전한 소스 코드는 여기 에서 찾을 수 있습니다 .

위의 정의에 기초

yaw = 180 * atan (accelerationZ/sqrt(accelerationX*accelerationX + accelerationZ*accelerationZ))/M_PI;

참고 : M_PI = 3.14159265358979323846 math.h에 정의 된 상수입니다.

다음은 Arduino 기본 소스 코드를 포함하여 도움이 될 수있는 일부 참조입니다.

---

참고 문헌 :

• 비행 역학
• 비글 본 블랙 ADXL345 소스 코드
• Adxl345 비글 본 블랙 임베디드 QT 데모
• Arduino IMU : 가속도계의 피치 앤 롤

따라서 아래의 더 긴 대답은 보드가 가속을한다고 가정 하며이 시간 동안 여전히 짧은 시간 내에 피치, 롤 및 요를 측정 할 수 있어야합니다. 보드가 모든 측정에 고정되어 있으면 Mahendra Gunawardena의 답변이 완벽하게 작동합니다. 이것이 세그웨이 또는 모델 평면 또는 멀티 로터와 같은 장치에 들어가거나 계속 움직이는 경우 계속 읽으십시오. 이 게시물은 센서 융합이라는 방법을 통해 세 가지 센서를 모두 사용하는 방법에 대해 설명합니다. 센서 융합을 통해 각 센서의 강도를 얻고 각 센서의 약점을 최소화 할 수 있습니다.

센서 특성 및 배경

먼저 가속도계는 중력뿐만 아니라 그것에 가해지는 모든 힘을 측정한다는 것을 이해하십시오. 따라서 진동이없는 정지 위치에 가속도계가 장착 된 완벽한 세상에서 Mahendra Gunawardena의 답변에서 볼 수 있듯이 기본 삼각법을 사용하여 어느 방향으로 올라가는 지 완벽하게 결정할 수 있습니다. 그러나 가속도계가 모든 힘을 흡수하므로 진동으로 인해 소음이 발생합니다. 또한 보드가 가속하는 경우 가속도계가보고하는 힘은 지구의 중력뿐만 아니라 가속하는 힘이기 때문에 간단한 삼각법을 사용할 수 없습니다.

자력계는 가속도계보다 더 간단합니다. 움직임은 문제를 일으키지 않지만 철이나 다른 자석과 같은 것은 결과에 영향을 미칩니다. 이 간섭을 일으키는 소스가 일정하지 않으면 다루기가 어렵지 않지만 이러한 소스가 일정하지 않으면 제거하기 어려운 수많은 노이즈가 발생합니다.

세 개의 센서 중 자이로 스코프는 가장 신뢰할 수 있으며 회전 속도 측정에 일반적으로 매우 뛰어납니다. 철원과 같은 것에 영향을받지 않으며 가속은 기본적으로 회전 속도 측정 능력에 영향을 미치지 않습니다. 그들은 장치가 회전하는 속도를보고하는 데 매우 효과적이지만 절대 각도를 찾고 있기 때문에 위치를 얻기 위해 속도를 통합해야합니다. 통합은 기본적으로 하나의 측정에 대한 오류가 초당 0.01도 떨어져 있어도 100 개의 측정에서 위치에 있기 때문에 마지막 측정의 오류는 새로운 측정의 오류에 추가됩니다. 1도, 1000 회 측정, 10 도씩 해제 할 수 있습니다. 초당 수백 번 측정하는 경우 이것이 문제를 일으키는 것을 볼 수 있습니다. 이것을 자이로 드리프트라고합니다.

센서 융합

이제이 모든 센서가 함께 작동하는 장점은 가속도계와 자력계의 정보를 사용하여 자이로 드리프트를 취소 할 수 있다는 것입니다. 결국 자이로 드리프트의 치명적인 결함없이 자이로의 정확도와 속도를 얻을 수 있습니다.

이 세 가지 센서의 데이터를 한 가지 이상의 방식으로 결합 할 수 있습니다. 보완 필터를 사용하는 방법은 칼만 필터와 칼만 필터가 내장 시스템에서 훨씬 더 많은 리소스를 소비하기 때문에 보완 필터 사용에 대해 이야기하겠습니다. 보완 필터는 종종 구현하기에 충분하고 (사전 빌드 된 라이브러리를 사용하지 않는다고 가정) 데이터를 더 빠르게 처리 할 수 ​​있습니다.

이제 프로세스에. 가장 먼저해야 할 일은 자이로 스코프 출력을 통합하여 각속도를 각 위치로 변환하는 것입니다. 또한 출력의 노이즈를 처리하기 위해 가속도계 및 자력계에 저역 통과 필터를 적용해야합니다. 아래에 표시된 것과 같은 간단한 FIR 필터가 여기에서 작동합니다. 일부 삼각법에서는 가속도계로 피치와 롤을 찾을 수 있고 자력계로 요를 찾을 수 있습니다.

filteredData = (1-weight)*filteredData + weight*newData

가중은 처리해야 할 소음의 양에 따라 조정될 수있는 상수이며, 소음이 클수록 무게 값이 작아집니다. 이제 센서의 데이터를 다음 코드 라인으로 결합 할 수 있습니다.

fusedData = (1-weight)*gyroData + weight*accelMagData

데이터는 피치, 롤 및 요의 벡터라는 것에주의해야한다. 원하는 경우 배열 대신 세 가지 변수를 사용 하여이 작업을 수행 할 수 있습니다. 이 계산을 위해 자이로는 피치, 롤 및 요의 각도로 위치를 제공하고, 자력계는 요에 대한 각도를 제공하며 가속도계는 자체 피치 및 롤에 대한 숫자를 제공합니다.

그래도 더 많은 정보를 원한다면 "보완 필터와 센서 융합"에 관한 기사가 많이 있습니다.

가속기 센서 데이터에서 피치 및 롤만 계산할 수 있습니다. Freescale의 다음 문서는 필요한 정보를 충분히 설명합니다.

AN3461-3 축 가속도계를 사용한 기울기 감지

문서의 말을 바탕으로

$$\tan \phi_{xyz} = \frac{G_{py}}{G_{pz}}$$

$$\tan \theta_{xyz} = \frac{-G_{px}}{G_{py}\sin \phi + G_{pz}\cos \phi} = \frac{-G_{px}}{\sqrt{G_{py}^2 + G_{pz}^2}}$$

이는 다음과 같습니다.

roll = atan2(accelerationY, accelerationZ)

pitch = atan2(-accelerationX, sqrt(accelerationY*accelerationY + accelerationZ*accelerationZ))

물론 회전이 특정 순서로 발생하는 경우에만 결과가 나타납니다 (Rxyz).

1. $\phi$
2. 각도 만큼 y 축 주위의 피치$\theta$
3. 각도 z 축 주위의 요$\psi$

$R_{xyz}$$\psi$