작성자: 박준규

IMU 센서 = Gyroscope + Accelerometer

• Basic Control Loop

Gyroscope - measures rotational rate in deg/s

Calibration may be needed (zeroing)

If filter is applied to reduce gyroscope noise, values will be delayed and responsiveness of drone may be affected.

Rotational rate를 시간에 대해 적분하면 드론의 현재 각도가 나오게 된다.

위 식을 코드에 적용하면:

uint32_t gyro_last_update = micros();    
//자이로센서 값 마지막 업뎃 시간 구하기

void compute_gyro_angles() {
	imu_sensor_read_gyro(&gyro_rates);    
	//raw 센서 값 읽기. &붙는 이유는 밑에 gyro_rates를 쓰기위해서?
	rates.x = gyro_rates.x + GYRO_X_OFFSET;    
	//gyroscope zeroing
	
	delta_t = (micros() - gyro_last_update) / 1000000;    
	//dt=현재시간-마지막업뎃시간. 백만으로 나누는 이유는 단위가 microsecond이기 때문.  
	gyro_angles.x += rates.x * delta_t;    
	//마지막 업데이트된 각도에 방금 움직인 각도 더해주기 rates.x * delta_t = 방금 움직인 각도

	gyro_last_update = micros();    
	//자이로센서 값 마지막 업뎃 시간 업뎃하기.
}

Gyroscope 만 이용할 시 센서의 drift 현상이 일어난다 (드론이 가만히 있어도 각이 계속 증가)

Accelerometer

중력가속도를 단위로 가속도를 측정

Gyroscope와 마찬가지로 zeroing이 필요하다

z축 (위아래)는 지구 중력가속도 때문에 1G로 zero를 해야한다

위 식을 이용하면 accelerometer를 갖고 드론의 각도를 구할 수 있다. Tilt Sensing Using Linear Accelerometers (nxp.com) (음... 그렇다고 합니다...)

이를 코드로 바꿔보면:

//accelerometer는 진동의 영향을 많이 받아서 accelerometer에 median 필터 적용. 
x = accel_filtered.x;
y = accel_filtered.y;
z = accel_filtered.z;

accel_angles.x = atan2(y, z) * RAD_TO_DEG;    
//atan2는 아두이노 내장된 arctan 함수
accel_angles.y = atan2(-1 * x, sqrt(y*y + z*z)) * RAD_TO_DEG

Accelerometer는 drift 현상이 없지만 진동에 의해 spiking 현상이 많이 일어나기 때문에 spike가 덜 일어나는 Gyroscope 데이터와 합쳐 드론의 각도를 구할 필요가 있다.

Complementary Filter (Not the best but it is a simple filter that works pretty well):

#define GYRO_PART 0.995
#define ACC_PART 0.005   //define 2 constants that add up to 1

dt = <time since last update>;

angles.x = GYRO_PART * (angles.x + (rates.x * dt)) + ACC_PART * accel_angles.x;

Gyroscope - Good for short durations

Accelerometer - Good for long durations