고정익 무인기 비행제어 시스템

 

 

 

https://github.com/akswnd98/matek-fixedwing

GitHub - akswnd98/matek-fixedwing

 

 

이 프로젝트는 px4나 inav 같은 오픈소스 프로젝트들의 angle mode (acro mode 아님) 를 모방하는 프로젝트이다.

유명한 오픈소스 프로젝트들을 벤치마킹한 이유가 있는데, 그 이유는 시스템 구성을 싸게 할 수 있기 때문이다.

hobby 시장의 규모 덕분이라고 할 수 있다.

 

 

제어 시스템

https://docs.px4.io/main/ko/flight_stack/controller_diagrams.html

 

시스템 블록도는 정확하게 px4의 것을 차용하였다.

 

간단하게 설명하자면

아래첨자 sp는 setpoint를 의미한다. 즉 reference, target value와 동일한 의미이다.

${\phi}_{sp} - {\phi}$와 ${\theta}_{sp} - {\theta}$는 P gain을 거쳐 ${\dot {\phi}_{sp}}$와 ${\dot {\theta}_{sp}}$를 생성한다.

${\dot {\psi}_{sp}}$생성에 대한 부분이 정말 재미있다.

이 부분은 coordinated turn을 센서 부족에도 불구하고 근사적이고 간단하게 구현하는 방법과 연관되어 있다. 이따가 후술하겠다.

 

그렇게 ${\dot {\Psi}_{sp}}$를 생성했으면 world에서 body로의 좌표계 변환을 통해 $w_{sp}$를 생성한다.

좌표계 변환을 해야하니 당연히 두 오일러각 $\phi$와 $\theta$값이 필요하다. $\psi$는 필요가 없다.

왜 일까?

world to body 변환이 ZYX 순서의 오일러각으로 돼 있다고 가정하고 $w = C \cdot {\dot \Psi}$의 $C$를 구해보면

명확하게 알 수 있을 것이다.

이 것은 당연하면서도 다행인 것인데,

왜냐하면 보통 우리가 쓰는 imu의 상보필터 원리상 $\psi$를 누적오차 보정 없이 구하는 건 불가능하기 때문이다.

$\psi$ 를 아무리 변화 시켜도 body 좌표계 기준으로 본 중력이 변하지는 않기 때문에 상보 필터를 $\psi$에 적용할 수 없다.

$w_{sp}$를 구했으면 PIFF 보상기를 거쳐 $\dot w_{sp}$을 생성하고 이 것은 그대로 roll, pitch yaw를 담당하는 각각의 방향타의 각도로 들어가게 된다.

 

coordinated turn

2 가지의 관점에서 이 주제를 바라볼 수 있다.

 

1. $\dot {\psi}_{ref}$의 생성 관점

2. side acceleration -> 0으로 만드는 관점 (여기서 side 란 body 좌표계 기준으로 y방향을 말함)

 

즉 이 두 가지를 조합하여 side acceleration을 0으로 만들기 위해 어떤 $\dot {\psi}_{ref}$를 넣어 줘야 할까?

즉 coordinated turn을 하기 위해 어떤 $\dot {\psi}_{ref}$를 넣어 줘야 할까?

 

 

W -> B: ZYX euler 변환을 가정한다.

 

중력가속도의 side 방향 성분:

 

$\begin {bmatrix} 0 & 1 & 0 \end {bmatrix} \cdot R_x^T \cdot R_y^T \cdot \begin {bmatrix} 0 \\ 0 \\ g \end {bmatrix} = gsin(\phi)cos(\theta)$

 

coordinated turn을 가정했을 때 기체의 가속도의 side 방향 성분:

 

$\begin {bmatrix} 0 & 1 & 0 \end {bmatrix} \cdot R_x^T \cdot \begin {bmatrix} 0 \\ \pm {\frac {v ^ 2} r} \\ 0 \end {bmatrix} = \pm {\frac {v ^ 2} r} cos(\phi)$

 

여기서 $\pm$이 붙는 이유는 선회 방향에 따라 구심가속도의 방향도 달라지기 때문이다.

 

coordinated turn을 가정했으므로 side에 중력 외에 작용하는 힘이 없다고 가정하면

 

$gsin(\phi)cos(\theta) = \pm {\frac {v ^ 2} r} cos(\phi)$ and ${\vec r} {\dot {\psi}_{sp}} = v$ (각속도와 속도의 관계)

 

$\pm$을 $\frac {\vec r} r$로 치환 ->

 

-> $gtan(\phi)cos(\theta) = {\frac {\vec r} r} \cdot {\frac {({\vec r}{\dot {\psi}_{sp}}) \cdot v} r}$

 

-> ${\dot {\psi}_{sp}} = {\frac g v}tan(\phi)cos(\theta)$

 

이 ${\dot {\psi}_{sp}}$을 시스템이 추종해야 side acceleration이 발생하지 않는 다는 결론에 이를 수 있다.

물론 선회하면서 고도가 낮아짐에 따라 공기역학적 side acceleration이 발생하겠지만 그 정도는 조종 난이도에

큰 영향을 미치지 않으니 고려하지 않은 것 같다.

 

$\phi$나 $\theta$는 어차피 ${\phi}_{sp}$랑 ${\theta}_{sp}$를 추종할 것이므로

처음 부터 ${\phi}_{sp}$랑 ${\theta}_{sp}$를 넣으면 좋을 거 같다.

기체가 더 빠르게 coordinated turn으로 수렴할 것 이기 때문이다.

 

이 내용은 px4에서 채택하고 있는 방식이며 다음과 같이 소개하고 있다.

 

https://docs.px4.io/main/ko/flight_stack/controller_diagrams.html

 

 

오일러 각 및 각속도 상태 업데이트

한 step당 알고리즘은 다음과 같다.

 

1. ${\phi}_{t - 1}$ ${\theta}_{t - 1}$ ${\dot {\phi}_{t - 1}}$ ${\dot {\theta}_{t - 1}}$ -> ${\phi}_{hf,t}$ ${\theta}_{hf,t}$

2. $a$ -> ${\phi}_{lf,t}$ ${\theta}_{lf,t}$

3. ${\phi}_{hf,t}$ ${\phi}_{lf,t}$ ${\theta}_{hf,t}$ ${\theta}_{lf,t}$ -> ${\phi}_t$ ${\theta}_t$

4. ${\phi}_t$ ${\theta}_t$ ${w_t}$ -> ${\dot {\phi}_t}$ ${\dot {\theta}_t}$

 

${\phi}_{hf,t}$의 hf는 high frequency라는 뜻이다. t는 time을 나타낸다.

왜 high frequency인가? 적분으로 구한 angle은 high frequency를 담당하고

누적오차 극복을 위한 acc값으로 구한 angle은 low frequency의 정확도를 담당하기 때문이다. (상보필터)

당연히 ${\phi}_{lf,t}$의 lf는 low frequency.

$a$ = 센서의 acc값

 

이 알고리즘은 값을 구하기 위한 재료와 그로 인해 탄생하는 요리만을 기술해 놓은 것이다.

구체적인 유도는 당연히 좌표계 변환과 행렬이 필요할 것이다.

 

맛 보기로 ${\phi}_{lf,t}$ ${\theta}_{hf,t}$ 구해보자면,

 

$R_x = \begin {bmatrix} 1 & 0 & 0 \\ 0 & cos(\phi) & -sin(\phi) \\ 0 & sin(\phi) & cos(\phi) \end {bmatrix}$

 

$R_y = \begin {bmatrix} cos(\theta) & 0 & sin(\theta) \\ 0 & 1 & 0 \\ -sin(\theta) & 0 & cos(\theta) \end {bmatrix}$

 

$R_x^T \cdot R_y^T \cdot \begin {bmatrix} 0 \\ 0 \\ {-g} \end {bmatrix} = \begin {bmatrix} a_x \\ a_y \\ a_z \end {bmatrix} = \begin {bmatrix} gsin(\theta) \\ -gsin(\phi)cos(\theta) \\ -gcos(\phi)cos(\theta) \end {bmatrix}$

 

-> ${\frac {a_y} {a_z}} = tan(\phi)$ -> $\phi = tan^{-1} (\frac {a_y} {a_x})$

 

$\sqrt {a_x ^ 2 + a_y ^ 2 + a_z ^ 2} = g$ and $sin(\theta) = \frac {a_x} g$ -> $\theta = sin ^ {-1} (\frac {a_x} {\sqrt {a_x ^ 2 + a_y ^ 2 + a_z ^ 2}})$

 

 

조종기 입력 상상

위의 제어시스템은 최초 setpoint 입력은 ${\phi}_{sp}$ 와 ${\theta}_{sp}$ 뿐이다.

따라서 조종 자체도 저 두 개만 입력 받을 것이다.

thrust (bldc 추력 조절용) 까지 합하면 3개면 비행기를 조종할 수 있다!.

 

 

이렇게 해서

원하는 자세를 유지하고 조악하게 나마 cordinated turn을 할 수 있는 비행기로 실제 비행이 가능할 지,

직접 한 번 제작해서 확인해보자!