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

 

 

 

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와 동일한 의미이다.

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

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

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

 

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

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

왜 일까?

world to body 변환이 ZYX 순서의 오일러각으로 돼 있다고 가정하고 $w=C\cdot \dot{\mathrm{\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 방향 성분:

 

$\left[\begin{array}{ccc}0& 1& 0\end{array}\right]\cdot R_x^T\cdot R_y^T\cdot \left[\begin{array}{c}0\\ 0\\ g\end{array}\right]=gsin(\varphi )cos(\theta )$

 

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

 

$\left[\begin{array}{ccc}0& 1& 0\end{array}\right]\cdot R_x^T\cdot \left[\begin{array}{c}0\\ \pm \frac{v^2}{r}\\ 0\end{array}\right]=\pm \frac{v^2}{r}cos(\varphi )$

 

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

 

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

 

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

 

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

 

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

 

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

 

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

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

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

 

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

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

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

 

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

 

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

 

 

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

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

 

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

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

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

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

 

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

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

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

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

$a$ = 센서의 acc값

 

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

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

 

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

 

$R_x=\left[\begin{array}{ccc}1& 0& 0\\ 0& cos(\varphi )& -sin(\varphi )\\ 0& sin(\varphi )& cos(\varphi )\end{array}\right]$

 

$R_y=\left[\begin{array}{ccc}cos(\theta )& 0& sin(\theta )\\ 0& 1& 0\\ -sin(\theta )& 0& cos(\theta )\end{array}\right]$

 

$R_x^T\cdot R_y^T\cdot \left[\begin{array}{c}0\\ 0\\ -g\end{array}\right]=\left[\begin{array}{c}{a}_x\\ a_y\\ a_z\end{array}\right]=\left[\begin{array}{c}gsin(\theta )\\ -gsin(\varphi )cos(\theta )\\ -gcos(\varphi )cos(\theta )\end{array}\right]$

 

-> $\frac{a_y}{a_z}=tan(\varphi )$ -> $\varphi =ta{n}^{-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 =si{n}^{-1}(\frac{a_x}{\sqrt{a_x^2+a_y^2+a_z^2}})$

 

 

조종기 입력 상상

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

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

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

 

 

이렇게 해서

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

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