Toy Quad Rotor FCC Review 4 : STEVAL-FCU001V1 - Sensing and Fusion

 이 포스팅에서는 STEVAL-FCU001V1의 센서 데이터 획득과 센서를 융합하여 자세를 어떤방식으로 얻어내는지 간단히 알아본다.

Code Link : 

 https://github.com/STMicroelectronics-CentralLabs/ST_Drone_FCU_F401

살펴볼 코드는 Official release with BLE Remocon이며 위치는 다음과 같다. 

File Path :

ST_Drone_FCU_F401-master\ST_Drone_FCU_F401-master\STM32 FW Project\Official release with BLE Remocon - 170318\Src

핵심적으로 볼 코드는 main.c와 ahrs.c이다.

이전 포스팅에서 설명하였듯이 드론의 자세를 알아내기 위해 센서 정보를 일정한 시간마다 획득하여 적절한 과정을 통해 자세를 측정해낸다.

일정한 시간마다 탑재되어있는 센서 정보를 획득하기 위해서는 정해진 주기마다 이벤트가 생기게끔 해주는 Timer가 필요하다. 그리고 이벤트가 생겼을 때 어떠한 일을 할건지에 대한 서비스를 적절히 적어주면 더 좋을것 같다. 이런 함수가 바로 main.c에 위치하고 있는 HAL_TIM_PeriodElapsedCallback 이다(인터럽트 서비스루틴 이라는 자료에 대해 보면 간단히 이해할 수 있다). 이 함수는 단순히 사용자가 만든 것과 다르다. 해당 보드에서는 이를 위해timer 9번을 사용하고 있는데, 쉽게 개발자가 800hz 마다 해당 함수를 실행할 수 있도록 정의했다 라고 보면된다. 해당 함수의 코드들은 생각보다 길기 때문에 자세히 다루지는 않을예정이다. 크게 센서 정보를 받고 보정하는 기능과 평균필터를 사용해서 측정 데이터 잡음을 줄이는 루틴이 포함되어있다.

다음으로 ahrs.c에 있는  ahrs_fusion_ag 함수에 대해 설명하려 한다. 일단 코드에 주석을 추가하여 설명하는 것이 직관적일것 같다.

 

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 72 73 74 75 76 77 78 79 80 81 82 83 84 85 86 87

void ahrs_fusion_ag(AxesRaw_TypeDef_Float *acc, AxesRaw_TypeDef_Float *gyro, AHRS_State_TypeDef *ahrs) {//변수 생성   float axf,ayf,azf,gxf,gyf,gzf;//가속도 3축, 자이로 3축   float norm;//vector norm을 구하기위한 변수   float vx, vy, vz;   float ex, ey, ez;   float q0q0, q0q1, q0q2, /*q0q3,*/ q1q1, /*q1q2,*/ q1q3, q2q2, q2q3, q3q3; //quaternion 연산을 위한 변수   float halfT;// dt/2      if(gTHR<MIN_THR)//throtle 명령이 최소치(MIN_THR) 보다 작을 경우   {//적분 이득을 올리는 듯 하다. 이건 제어 성능차원에서 넣은듯 싶다.    //자세 제어기 이득은 아니지만 결국엔 자세제어기 입력에 영향을 주게된다. //확실치는 않지만 지면효과에 의한 외란을 줄이기 위함이라 생각된다     ahrs_kp = AHRS_KP_BIG;   }   else   {     ahrs_kp = AHRS_KP_NORM;   }  //3축 가속도 센서 정보를 저장   axf = acc->AXIS_X;   ayf = acc->AXIS_Y;   azf = acc->AXIS_Z;     // mdps convert to rad/s   gxf = ((float)gyro->AXIS_X) * ((float)COE_MDPS_TO_RADPS);     gyf = ((float)gyro->AXIS_Y) * ((float)COE_MDPS_TO_RADPS);     gzf = ((float)gyro->AXIS_Z) * ((float)COE_MDPS_TO_RADPS);      // auxiliary variables to reduce number of repeated operations   q0q0 = q0*q0;   q0q1 = q0*q1;   q0q2 = q0*q2;   //q0q3 = q0*q3;   q1q1 = q1*q1;   //q1q2 = q1*q2;   q1q3 = q1*q3;   q2q2 = q2*q2;   q2q3 = q2*q3;   q3q3 = q3*q3;     // normalise the accelerometer measurement   norm = invSqrt(axf*axf+ayf*ayf+azf*azf); // 1/sqrt(...) 연산 bit 연산으로 빠르게 하드웨어친화적 계산     axf = axf * norm;//vector nomalization routine   ayf = ayf * norm;//vector nomalization routine   azf = azf * norm;//vector nomalization routine     // estimated direction of gravity and flux (v and w) // 지금 자세 쿼터니언을 이용하여 중력 방향의 벡터를 계산하는 것 참고문헌[1]의 식(19) 참고! // 참고식에서 x, y 가 바뀌어 보이는 이유는 센서 축을 맞추기 위한 것   vx = 2*(q1q3 - q0q2);   vy = 2*(q0q1 + q2q3);   vz = q0q0 - q1q1 - q2q2 + q3q3;     // 구한 가속도 벡터 axf, ayf, azf와 중력 벡터사이의 오차를 구하는 부분 // 외적 연산 -> // | ex ey ez | // | ax ay az | // | vx vy vz | ad-bc 해서 해보면 됩니다. 만약 중력방향과 가속도 벡터가 일치하면 // 올바르게 hovering하고 있는 거니까 error인 ex, ey, ez 가 0이 되겠지요   ex = (ayf*vz - azf*vy);   ey = (azf*vx - axf*vz);   ez = (axf*vy - ayf*vx);   // 이 다음 부터는 가속도 벡터와 중력방향 사이의 오차를 적분하고   // integral error scaled integral gain   exInt = exInt + ex*AHRS_KI*SENSOR_SAMPLING_TIME;   eyInt = eyInt + ey*AHRS_KI*SENSOR_SAMPLING_TIME;   ezInt = ezInt + ez*AHRS_KI*SENSOR_SAMPLING_TIME;   // 이를 자이로 측정치와 같이 하여 적분한다.   // adjusted gyroscope measurements   gxf = gxf + ahrs_kp*ex + exInt;   gyf = gyf + ahrs_kp*ey + eyInt;   gzf = gzf + ahrs_kp*ez + ezInt; //왜 이렇게 작성한지는 모르겠지만 아마도 이런 방식은 자세제어의 반응성을 더 빠르 //게 할 수 있다는 생각이 든다.   //앞의 식은 quaternion kinematics equation (discrete) 을 코드로 작성한 것이다. //밑에 식에 대한 설명 추가   // integrate quaternion rate and normalise   halfT = 0.5f*SENSOR_SAMPLING_TIME;   q0 = q0 + (-q1*gxf - q2*gyf - q3*gzf)*halfT;   q1 = q1 + ( q0*gxf + q2*gzf - q3*gyf)*halfT;   q2 = q2 + ( q0*gyf - q1*gzf + q3*gxf)*halfT;   q3 = q3 + ( q0*gzf + q1*gyf - q2*gxf)*halfT;     // normalise quaternion : 맞아요 quaternion의 특성을 만족시키기위해 nomalization   norm = invSqrt(q0q0 + q1q1 + q2q2 + q3q3);    q0 *= norm;   q1 *= norm;   q2 *= norm;   q3 *= norm; // 자세 추정?은 아니고 계산 결과를 ahrs 구조체에 저장!   ahrs->q.q0 = q0;   ahrs->q.q1 = q1;   ahrs->q.q2 = q2;   ahrs->q.q3 = q3; // q0, q1, q2, q3 변수는 전역 변수로 설정되어 있기 때문에 계속 이 루틴을 // 반복하며 계속 자세가 누적되며 갱신된다! }   Colored by Color Scripter

cs

quaternion kinematics equation은 흔히 다음과 같이 표현한다.

$$\begin{bmatrix}\hat{q_0}  \\\hat{q_1} \\\hat{q_2}\\\hat{q_3} \end{bmatrix}=\frac{1}{2}\begin{bmatrix}0 & -p & -q & -r\\ p & 0 & r & -q\\q & -r & 0 &p\\r&q&-p&0 \end{bmatrix}\begin{bmatrix} q_0 \\ q_1 \\ q_2 \\ q_3 \end{bmatrix}$$

그리고 이를 이산식(discrete) 형태로 표현하면 다음과 같다.

$$\begin{bmatrix}q_0 \\ q_1 \\ q_2 \\ q_3 \end{bmatrix}_{t+\triangle t}=\left[I_{4\times4} + \frac{\triangle t}{2}\begin{bmatrix}0 & -p & -q &-r \\ p & 0 & r &-q\\ q &-r & 0 &p\\ r & q & -p & 0 \end{bmatrix}\right]\begin{bmatrix}q_0 \\ q_1 \\ q_2 \\ q_3 \end{bmatrix}_{t}$$

여기서 쿼터니언$q$와 동체 각속도 $\omega$는 다음과 같이 정의된다. $q_0$는 크기성분이고  $q_1$, $q_2$, $q_3$은 방향 성분이다. 그리고 $p$, $q$, $r$은 동체 각 축(x, y, z)축에 해당하는 회전 각속도를 의미한다.

$$         q = \begin{bmatrix}q_0 & q_1 & q_2 & q_3 \end{bmatrix}^{T}$$

$$\omega = \begin{bmatrix}p & q & r\end{bmatrix}^{T}$$

이 식을 확인하고 다시 코드를 확인해보자,

1 2 3 4 5

halfT = 0.5f*SENSOR_SAMPLING_TIME;   q0 = q0 + (-q1*gxf - q2*gyf - q3*gzf)*halfT;   q1 = q1 + ( q0*gxf + q2*gzf - q3*gyf)*halfT;   q2 = q2 + ( q0*gyf - q1*gzf + q3*gxf)*halfT;   q3 = q3 + ( q0*gzf + q1*gyf - q2*gxf)*halfT;

cs

적분을 하기때문에 기존 쿼터니언 q0, q1, q2, q3에 계속 누적되는 방법 forward euler method 방식으로 적분된다.

[1] Valenti, Roberto G., Ivan Dryanovski, and Jizhong Xiao. "Keeping a good attitude: A quaternion-based orientation filter for IMUs and MARGs." Sensors 15.8 (2015): 19302-19330.