◈ MPU-6050를 이용한 3축 회전각도 계산

 

 

▲  MPU 6050은 3축 가속도 센서+자이로 센서

MPU-6050 모듈은 가속도/자이로를 측정할 수 있는 센서 가속도는 지구 중력을 기준으로 x, y, z 축의 가속도 크기를 구할 수 있다는 원리 입니다. 
자이로(각속도)는 시간당 x, y, z 축의 회전속도 즉 각속도를 구할 수 있습니다.

구분	가속도	자이로	비고
Register Name	ACCEL_XOUT ACCEL_YOUT ACCEL_ZOUT	GYRO_XOUT GYRO_YOUT GYRO_ZOUT	16Bit  (High 8bit, Low 8bit)
Minimum	-32768	-32768
Maximum	+32768	+32768
Consept

 

▲ 가속도 출력 값을 이용한  X, Y 축 기준 회전 각도

 중력 가속도를 기반으로 X, Y, Z축에 대한 직선 가속도를 측정한다. 중력은 지구 방향으로 mg로 일정하지만 센서의 기울어짐에 따라서 센서의 X, Y, Z축으로 분해되는 크기가 달라지게 되는데, 이 분해되는 값들을 백터 분해수식을 이용하여 각도를 계산할수있다. 외력에 의해 측정값이 왜곡될 수 있지만 시간이 흘러도 오차가 커지지 않는다.

회전 각	수 식
Y축 기준 회전 각도
X축 기준 회전 각도

 

▲ 자이로 출력 값을 이용한 Z 기준 회전 각도 계산 

  X, Y, Z축의 각속도 변화량을 측정한다. 순간 각속도이므로 각도를 알기 위해서는 이 값을 측정시간 단위로 적분해야 한다. 그런데 적분을 하게되면 오차가 누적되어, 가속도 센서보다는 실제 센서의 움직임과 같은 값을 보이지만 시간이 흐를수록 오차가 커지게 된다. 드론 제어 시 센서의 오차로 시스템이 불안정해 지는 것으로 보인다. 

회전 각	수   식
X축 기준 회전 각도
Y축 기준 회전 각도
Z축 기준 회전 각도
ω	X, Y and Z 축을 중심으로 하는 각속도
	측정 간격  센서에서 자이로 값을 읽어오는 시간 간격
각도변화량

 

▲ 상보필터

위의 두 센서의 값의 오차와 오류를 보정하기위한 필터가 사용된다 칼만필터가 사용이 된다, 그러나 칼만필터는 연산시 시간이 오래 걸려 저가의 MCU에 적용하면 시간지연으로 제어속도를 낼수 없다. 이를 좀 가볍게 만든 필터가 상보필터 이다. 적용 수식은 다음과 같다.

 

상보필터 측정 값 공식
	필터에의해 계산된 현재 각도
	이전 필터 각도
ω	각속도 (현재)
	입력 주기 (측정주기)
	시간상수