[STM32] 간단한 ICM20948 쿼터니언

main.c

0.01MB

ICM20948.c

0.01MB

ICM20948.h

0.00MB

 

드론의 Pitch, Roll, Yaw의 각을 구할 때 Yaw각의 경우 지자기 센서에 의해 계산된다. Yaw를 제외한 Pitch, Roll의 경우 자이로 센서인 mpu6050, mpu9250를 사용하여 계산한다.

 

 

ddtxrx.tistory.com/entry/STM32-LL%EB%93%9C%EB%9D%BC%EC%9D%B4%EB%B2%84-I2C-DMA-MPU6050-%EC%83%81%EB%B3%B4%ED%95%84%ED%84%B0?category=913763

[STM32] LL 드라이버 - I2C DMA로 작성한 MPU6050 상보필터

계산은 위 글에서 작성한 것처럼 가속도 센서와 자이로 센서의 값을 받아 상보 필터를 사용하여 각도를 계산한다. 글에는 안 썼지만 각속도를 약간 보정해 줘서 적용했다.

 

드론의 자세를 더 정교하게 계산해 보고 싶어 검색을 많이 해봤다. 가장 많이 본 내용은 자세를 구할 때는 일반적인 오일러 각을 사용하는 방식이 아닌 쿼터니언이라는 수 체계를 사용하여 계산한다고 한다. 오일러 회전에서 많이 발생하는 짐벌락 현상이 없고 여러 장점이 있다고 한다.

 

아무튼 여러 자세를 구하는 방식에 쿼터니언은 사용한다고 하여 기본적인 내용을 공부해 간단한 각도를 구하는 코드를 작성해봤다.

 

 

I2C 코드 생성 - (0)

ddtxrx.tistory.com/entry/STM32-LL-%EB%93%9C%EB%9D%BC%EC%9D%B4%EB%B2%84-ICM20948?category=913763

[STM32] LL 드라이버 - ICM20948

 

이미 작성해둔 코드에 쿼터니언 내용만 넣었기 때문에 기본적인 세팅은 위의 글을 보면 된다.

 

 

 

코드 - ICM20948.c (1)

 

void ICM_Quaternion(ICM20948* icm20948){
    float pitch, roll,yaw;
    float q0,q1,q2,q3;
    ICM_Readaccgyro(icm20948);
 
    icm20948->f_gyx=icm20948->f_gyx/57.29577951;
    icm20948->f_gyy=icm20948->f_gyy/57.29577951;
    icm20948->f_gyz=icm20948->f_gyz/57.29577951;
 
 
    q0=icm20948->q0 + (icm20948->q1*(-icm20948->f_gyx) + icm20948->q2*(-icm20948->f_gyy) + icm20948->q3*(-icm20948->f_gyz))*0.5*dt;
    q1=icm20948->q1 + (icm20948->q0*(icm20948->f_gyx) + icm20948->q2*(icm20948->f_gyz) + icm20948->q3*(-icm20948->f_gyy))*0.5*dt;
    q2=icm20948->q2 + (icm20948->q0*(icm20948->f_gyy) + icm20948->q1*(-icm20948->f_gyz) + icm20948->q3*(icm20948->f_gyx))*0.5*dt;
    q3=icm20948->q3 + (icm20948->q0*(icm20948->f_gyz) + icm20948->q1*(icm20948->f_gyy) + icm20948->q2*(-icm20948->f_gyx))*0.5*dt;
 
    icm20948->q0=q0;
    icm20948->q1=q1;
    icm20948->q2=q2;
    icm20948->q3=q3;
 
    q0=sqrtf(icm20948->q0 * icm20948->q0 + icm20948->q1 * icm20948->q1 + icm20948->q2 * icm20948->q2 + icm20948->q3 * icm20948->q3);
    icm20948->q0 /= q0;
    icm20948->q1 /= q0;
    icm20948->q2 /= q0;
    icm20948->q3 /= q0;
 
    pitch=asinf(2*(icm20948->q0*icm20948->q2 - icm20948->q1*icm20948->q3))*57.29577951;
    roll=atan2f(2*(icm20948->q2*icm20948->q3 + icm20948->q0*icm20948->q1),2*(icm20948->q0*icm20948->q0 + icm20948->q3*icm20948->q3)-1)*57.29577951;
    yaw=atan2f(2*(icm20948->q1*icm20948->q2 + icm20948->q0*icm20948->q3),2*(icm20948->q0*icm20948->q0 + icm20948->q1*icm20948->q1)-1)*57.29577951;
 
 
    icm20948->pitch=pitch
    icm20948->roll=roll
    icm20948->yaw=yaw;
}

 

4번째 줄에서 icm20948 ic의 데이터를 읽어 들인다.

 

6~8번째 줄은 각속도를 Deg에서 Radian으로 바꿔준다.

 

11~14번째 줄은 각속도 값을 쿼터니언 변화량으로 변경하여 더해주는데 속도*dt 는 거리인 것처럼 쿼터니언 값의 변화량에 dt값을 더해 변한 쿼터니언 값을 저장한다.

 

구한 쿼터니언 값을 오일러 각으로 변환하여 Pitch, Roll각을 구한다.

 

 

 

결과 - (2)

 

나름 각도를 잘 보여주지만 자이로 센서의 드리프트 현상에 의해 가만히 내버려 두어도 값이 커지거나 작아지기 때문에 이대로 사용하는 것은 힘들다.

 

 

 

 

-

전자과에서 쿼터니언에 대해 배워본 적이 없어 자료를 찾는데 시간이 오래 걸렸다.

 

어떤 책을 발견해서 기본적인 내용은 알게 됐지만 심화된 내용은 계속 공부해야 될 것 같다. 위의 코드도 2일도 안된 코드이다.

 

지금 Yaw제어나 고도제어를 할 때 센서 퓨전을 생각하고 있어 드론의 새로운 기능을 만들려면 시간이 오래 걸릴 것 같다...

 

 

 

추가-

ddtxrx.tistory.com/entry/Quaternion-Open-source-AHRS-%EC%95%8C%EA%B3%A0%EB%A6%AC%EC%A6%98?category=925643

Quaternion Open source AHRS 알고리즘

Open source AHRS알고리즘이다.