[Aerial Robotics] Geometry and Mechanics

Geometry and Mechanics

이전 Post: Introduction

2.1 Quadrotor Kinematics

Transformations

이번 강의에서는 “Rigid Body Transformations 의 주제를 다루고, 이를 위한 notation들의 정의를 한다.

주로 사용하는 transformation matrix인 $A,R$은 다음과 같이 어떤 frame에서 frame으로 바뀌는지 표현한다.

$$\begin{array}{r}{}^A{A}_B,{}^A{R}_B,{}^A{\xi }_B\\ \text{or}{A}_{ab},R_{ab}\end{array}$$

이러한 표현방식 모두 frame $B\text{or}b$ 에서 $A\text{or}a$ 로 transform 한다는 의미이다.

이제 Rigid Body Displacement 를 어떤 물체 $O\subset {\mathbb{R}}^3$ 라고 할 때 Map $g:O\to {\mathbb{R}}^3$ 라고 하자.

그렇다면 $O$ 안의 모든 점들은 $g$ 로 transformation 을 표현할 수 있다. ex) $g(p),g(q)$

vector 의 이동은 $g\ast$ 로 표현한다. 그러면 $\overrightarrow{g(p)-g(q)}=g\ast (\overrightarrow{p-q})$ 와 같이 vector 간의 transformation 도 표현된다.

벡터의 크기와 cross-product $\times$ 는 그대로 유지된다.

$$\begin{array}{r}\Vert g(p)-g(q)\Vert =\Vert p-q\Vert \\ g\ast (v)\times g\ast (w)=g\ast (v\times w)\end{array}$$

마찬가지로 orthogonal vector 끼리도 유지되어야 unit vectors(or xyz axes) 가 보존된다. $g\ast (v)\cdot g\ast (w)=g\ast (v\cdot w)$

위와 같은 Rigid body 의 unit vector transformation을 아래와 같이 linear combination으로 표현할 수 있다.

$$\begin{array}{r}{b}_1=R_{11}{a}_1+R_{12}{a}_2+R_{13}{a}_3\\ b_2=R_{21}{a}_1+R_{22}{a}_2+R_{23}{a}_3\\ b_3=R_{31}{a}_1+R_{32}{a}_2+R_{33}{a}_3\end{array}$$

각 계수들의 notation은 맨 처음 정의한 notation을 참고해서 생각해보자. (matrix의 column, row로 볼 수도 있지만)

그러면 아래와 같이 Rotation matrix $R$ 을 정의할 수 있다.

$$R=\left[\begin{array}{ccc}{R}_{11}& R_{12}& R_{13}\\ R_{21}& R_{22}& R_{23}\\ R_{31}& R_{32}& R_{33}\end{array}\right]$$

Note

• Rotation about the x-axis through $\theta$ $Rot(x,\theta )=\left[\begin{array}{ccc}1& 0& 0\\ 0& \cos \theta & -\sin \theta \\ 0& \sin \theta & \cos \theta \end{array}\right]$
• Rotation about the y-axis through $\theta$ $Rot(y,\theta )=\left[\begin{array}{ccc}\cos \theta & 0& \sin \theta \\ 0& 1& 0\\ -\sin \theta & 0& \cos \theta \end{array}\right]$
• Rotation about the z-axis through $\theta$ $Rot(z,\theta )=\left[\begin{array}{ccc}\cos \theta & -\sin \theta & 0\\ \sin \theta & \cos \theta & 0\\ 0& 0& 1\end{array}\right]$

Rotations

이러한 Rotation matrix $R\in {\mathbb{R}}^{3\times 3}$ 은 orthogonality 와 determinant가 1인 special orthogonality를 가진다.

이는 Special Orthogonal group인 $SO(3)$에 속한다.

$$SO(3)=\{R\in {\mathbb{R}}^{3\times 3}|R^{T}R=R{R}^T=I,det(R)=1\}$$

이를 표현하는 방법으로는 아래와 같다.

1. Rotation matrices
2. Euler angles
3. Axis angle parameterization
4. Exponential coordinates
5. Quaternions

이 중, 어떤 방식이 coordinates for $SO(3)$ 가 될 수 있는지 알아보자.

Euler Angles

Euler angles은 어떤 $a$ 에서 $b$ 로의 회전을 3가지의 회전의 조합으로 나타내는 것이다.

$$\begin{array}{rl}{}^A{R}_D& ={}^A{R}_B{\times }^B{R}_C{\times }^C{R}_D\\ {}^A{R}_D& =Rot(x,\psi )\times Rot(y,\varphi ),Rot(z,\theta )\end{array}$$

어떤 회전이든 이러한 연속적인 3가지의 회전을 linearly independent 한 axes 의 조합으로 볼 수 있다.

하지만 $3\times 3$ rotation matrix $\leftrightarrow$ 3 Euler angles 는 성립하지 않고 almost one-to-one 이다.

그러한 이유를 살펴보자.

Euler angles는 어떤 순서로 axes 를 조합하느냐에 따라 다르게 표현할 수 있다. 앞선 수식에서는 X-Y-Z Euler angles 라고 할 수 있다.(Roll-Pitch-Yaw 순)

Gimbal Lock 상황을 위해 Z-Y-Z Euler angles 를 예시로 들어보자.

Body-fixed frame 에서 z축에 대해 $\varphi$ 만큼, y축에 대해 $\theta$ 만큼, z축에 대해 $\Psi$ 만큼 회전시킨다.

Body-fixed frame 에서 회전하므로 회전이 이루어질 때마다 기존의 축이 바뀌기 때문에 새로운 y 혹은 z축에 대해 회전하고 있다.

이들의 회전은 모두 linearly independent 한 것을 알 수 있다. 하지만 $\theta =0$ 인 경우에는 더이상 linearly independent하지 않기 때문에, unique 하게 표현할 수 없다.

이를 Rotation matrix로 살펴보자.

$$R=Rot(z,\psi )\times Rot(y,\theta )\times Rot(z,\Psi )$$ $$R=\left[\begin{array}{ccc}\cos \varphi \cos \theta \cos \psi -\sin \varphi \sin \psi & -\cos \varphi \cos \theta \sin \psi -\sin \varphi \cos \psi & \cos \varphi \sin \theta \\ \sin \varphi \cos \theta \cos \psi +\cos \varphi \sin \psi & -\sin \varphi \cos \theta \sin \psi +\cos \varphi \cos \psi & \sin \varphi \sin \theta \\ -\sin \theta \cos \psi & \sin \theta \sin \psi & cos\theta \end{array}\right]$$

여기에서 만약 $R_{33}$ 를 알고 있다면, $\theta$ 를 계산할 수 있고 $R_{31},R_{32}$ 를 통해 $\psi$ 를, $R_{13},R_{23}$ 을 통해 $\varphi$ 를 알 수 있다.

하지만 만약 $R_{33}=\pm 1$ 이라면 $\sin \theta =0$ 이고, 나머지 항들이 $\varphi$ 와 $\psi$ 의 조합으로 이루어져 있으므로 해가 무한해진다.

이를 요약하면 아래 슬라이드와 같다.

Axis/Angle Representations for Rotations

Euler는 아래와 같이 회전을 정의하였다.

Euler's Therorem

Rotations Any displacement of a rigid body such that a point on the rigid body, say $O$, remains fixed, is equivalent to a rotation about a fixed axis through the point $O$.

앞서 Rotation matrix 나 Euler angle 에서는 회전축에 대한 이야기가 없었으나, Euler 는 위와 같이 회전을 정의하였다.

어떤 회전을 아래 사진과 같이 원점을 같게 만들어 표현해보자.

그러면 회전한 $\overrightarrow{OP}$ 를 회전하기 전과 같은 원점을 갖는 $\overrightarrow{O{P}^{\prime }}$ (blue arrow) 로 표현할 수 있고, 이는 같은 unit vector 를 공유한다.

따라서, 이들의 계수는 새로운 linear combination 이고 Rotation matrix $R$ 을 정의할 수 있다.

그렇다면 Euler’s Theorem 을 증명해보자.

$q=Rp$ 에서 자기자신에게 대응되는 $p$ 가 있을까? 그렇다면 $p=Rp$ 가 되고, 그렇다는 것은 eigenvalue 가 1인 $R$을 의미한다.

바꿔 말하면, $Rp=\lambda p$ 의 eigenvalue 문제의 해가 1이라는 것을 의미한다.

이제는 주어진 임의의 회전축 $u$ 와 회전각 $\varphi$ 가 있다고 해보자. 이를 이용해 Rotation matrix $R$ 을 유도해보자. (회전축 $u$ 가 좌표축 $x,y,z$ 중 하나라면 앞선 Euler angles 와 동일하다.)

그래서 위 사진과 같이 벡터 $\vec{p}$ 를 회전축과 평행한 요소와 수직인 요소로 분해한다. 그러면 평행한 요소 $(p\cdot u)u$ 는 회전과 상관없이 변하지 않는다.

각도 $\varphi$ 만큼 회전하면 수직한 요소인 $p-(p\cdot u)u$는 아래와 같이 표현된다.

$$(p-(p\cdot u)u)\cos \varphi +u\times (p-(p\cdot u)u)\sin \varphi$$

풀어 설명하면 $u$ 에 수직한 요소를 $v$ 라고 하였을 때, 이를 외적한 $w=u\times v$ 를 만들 수 있고 이들의 $u\cos \varphi +w\sin \varphi$ 이다.

그렇다면 회전하고 난 ${\vec{p}}_{rot}$ 은 $(p\cdot u)u+u\cos \varphi +w\sin \varphi$ 가 된다.

자기 자신의 외적은 0임을 이용해 $u\times (u\cdot p)u=0$ 으로 위 수식을 정리하면,

$$\begin{array}{rl}{\vec{p}}_{rot}& =(p\cdot u)u+u\cos \varphi +w\sin \varphi \\ & =(p\cdot u)u+(p-(p\cdot u)u)\cos \varphi +u\times (p-(p\cdot u)u)\sin \varphi \\ & =(p\cdot u)u+(p-(p\cdot u)u)\cos \varphi +(u\times p)\sin \varphi \\ & =p\cos \varphi +(p\cdot u)u(1-\cos \varphi )+(u\times p)\sin \varphi \\ & =p\cos \varphi +u{u}^T(1-\cos \varphi )p+(u\times p)\sin \varphi \end{array}$$

와 같다.

이제 외적 $u\times$ 부분을 행렬 $\hat{u}$ 로 표현한다. 이는 skew-symmetric matrix 로, ${\hat{u}}^T=-\hat{u}$ 를 만족하고 이들의 diagonal element 들은 모두 0이다. ($u_{ij}=-u_{ji}$)

이제 $p$ 를 제거하면 임의의 벡터에 대해 회전축 $u$ 와 회전각 $\varphi$ 를 갖는 수식을 얻을 수 있고, 이것이 Rodrigues’ formula 이다.

$$Rot(u,\varphi )=I\cos \varphi +u{u}^T(1-\cos \varphi )+\hat{u}\sin \varphi$$

그렇다면 이러한 방식이 Rotation matrix 에 onto or one-to-one 대응일까?

앞서 Euler’s Theorem에서 onto 임은 살펴보았다.

그러나, $Rot(u,\varphi )$ 와 $Rot(-u,2\pi -\varphi )$ 의 예시를 통해 one-to-one 은 아닌 것을 알 수 있다.

Rotation matrix $R$ 로 부터 회전축 $u$ 와 회전각 $\varphi$ 를 얻어보자.

$\tau =\text{tr}(R)$ 이라고 하면 $\cos \varphi =\frac{\tau -1}{2}$ 가 된다. 그리고 $\hat{u}=\frac{1}{2\sin \varphi }(R-R^T)$ 로 $u$ 를 얻을 수 있다.

이 때 $\tau =3$ 인 경우에는 $\varphi =0$ 이 되어 unique 한 회전축 $u$ 를 얻을 수 없다. 또한, $\tau =-1$인 경우$\varphi =\pi$ 가 되어 $u\text{or}-u$ 를 얻게 됨을 알 수 있다.

따라서, 정의역을 $[0,\pi ]$ 로 하더라도 일대일 대응이 될 수 없다.

Angular Velocity

회전 속도는 회전의 변화율로, 회전의 시간에 대한 미분이다.

Rotation matrix $R(t)$ 에 대해 orthogonality 를 만족해야 하므로, $R^T(t)R(t)=I,R(t)R^T(t)=I$ 로 부터 아래와 같은 식을 유도할 수 있다.

$$\begin{array}{r}{\dot{R}}^{T}R+R^T\dot{R}=0\\ R{\dot{R}}^T+\dot{R}{R}^T=0\end{array}$$

이를 통해 $R^T\dot{R},\dot{R}{R}^T$ 은 skew-symmetric matrix 임을 알 수 있다. ($(AB{)}^T=B^T{A}^T$)

이전에 보았던 예시에서 이제는 $q(t)=R(t)p$ 가 되어 $p$ 는 여전히 시간에 변화와 관계없이 고정임을 알 수 있다.

이에 대한 미분은 $\dot{q}=\dot{R}p$ 이다. 이 때 $p$ 는 body-fixed frame 에서의 위치이지만, $\dot{q}$ 는 inertial frame 에서의 속도이다.

양변에 $R^T$ 를 곱해보자. 그러면 $R^T\dot{q}=R^T\dot{R}p$ 가 되고, 좌항은 속도를 다시 body-fixed frame 으로 transformation 시켰고 우항은 body-fixed frame 에서의 각속도가 된다. 이 때 $R^T\dot{R}$ 이 skew-symmetrix matrix 이므로 ${\hat{w}}^b$ 로 표현한다.

그리고 $q=Rp$ 의 양변에 $\dot{R}{R}^T$ 를 곱하면 $\dot{q}=\dot{R}{R}^{T}q$ 를 얻을 수 있다.

이렇게 되면 좌항은 inertial frame 에서의 속도이고, 우항은 inertial frame 에서의 각속도가 된다. 마찬가지로 $\dot{R}{R}^T$ 를 ${\hat{w}}^s$ 로 표시한다.

2.2 Quadrotor Dynamics

Dynamics of a Quadrotor

이제 쿼드로터의 Dynamics 를 알아보자.

이전 강의에서 다룬대로 $F_i=k_F{w}_i^2,M+i=k_M{\omega }_i^2$ 로 힘과 모멘트가 정의된다.

그리고 Z-X-Y Euler angles convention 을 사용한다.

이에 따라 아래와 같은 Dynamics 표현이 가능하다.

Newton-Euler Equations

기체의 COM 을 $r_c=\frac{1}{m}{\sum }_{i=1,N}{m}_i{p}_i$ 로 구할 수 있고, $F={\sum }_{i=1}^N{F}_i=m\frac{dv}{dt}$ 로 표현한다.

Momentum $M$ 의 미분인 Angular momentum $H,s.t\frac{dM}{dt}=M$ 은 $H=I\cdot \omega$ 로 표현할 수 있다. $I$ 는 inertia tensor 로, 기준점에 관계없이 절대적이다.

Principal Axes and Principal Moments of Inertia

Principle axis of inertia $u$ is a unit vector along a principal axis if $I\cdot u$ is parallel to $u$. There are 3 independent principal axes.

Principal moment of inertia The moment of inertia with respect to a principal axis, $u\cdot I\cdot u$ is called a principal moment of inertia.

앞선 주축과 주모멘트 정의에 의해 inertial frame $A$ 와 body-fixed frame $B$, origin $C$ 에 대해서 위와 같이 표현해볼 수 있다.

frame $B$ 에서의 주축을 좌표축으로 잡아 각속도 ${}^A{\omega }^B$ 를 ${}^A{w}^B={\omega }_1{b}_1+{\omega }_2{b}_2+{\omega }_3{b}_3$ 로 표현할 수 있다.

$\frac{{}^{A}d{H}_c}{dt}=M_C$ 의 좌항을 분해하여 $\frac{{}^{B}d{H}_c}{dt}+{}^A{\omega }^B\times H_c$ 로 표현하여 움직이는 물체를 inertial frame $A$ 의 모멘텀을 표현할 수 있다.

앞서 주축으로 unit vector 를 구성하였으므로, $H=I\cdot \omega$ 이므로 $\frac{{}^{B}d{H}_c}{dt}=I_{11}{\dot{\omega }}_1{b}_1+I_{22}{\dot{\omega }}_2{b}_2+I_{33}{\dot{\omega }}_3{b}_3$ 로 정리된다.

이를 모두 정리한 것이 그림의 보라색 박스 이다.

Quadrotor Equations of Motion

이를 정리하여 아래의 수식으로부터 Equations of Motion 만들어보자.

$$\begin{array}{rl}F& =\sum _{i=1}^4{F}_i-mg{a}_3\\ M& =\sum _{i=1}^4{r}_i\times F_i+\sum _{i=1}^4{M}_i\end{array}$$

이를 앞선 설명들로 모두 표현하면

$$\begin{array}{rl}m{\ddot{p}}_A& =\left[\begin{array}{c}0\\ 0\\ -mg\end{array}\right]+{}^A{R}_B{\left[\begin{array}{c}0\\ 0\\ {\sum }_{i=1}^4{F}_i\end{array}\right]}_B\\ I{\left[\begin{array}{c}{\dot{w}}_x\\ {\dot{w}}_y\\ {\dot{w}}_z\end{array}\right]}_B& =\left[\begin{array}{c}L(F_2-F_4)\\ L(F_3-F_1)\\ M_1-M_2+M_3-M_4\end{array}\right]-\left[\begin{array}{c}{w}_x\\ w_y\\ w_z\end{array}\right]\times I{\left[\begin{array}{c}{w}_x\\ w_y\\ w_z\end{array}\right]}_B\end{array}$$

가 된다. ($p_A$ = position in inertial frame, $L$ = length of the airframe)

여기에서 $L,I,m$ 은 모두 기체의 길이, 질량, 관성이므로 쉽게 구할 수 있다.

그리고 앞선 Z-X-Y Euler angles 를 통해 Roll, Pitch, Yaw 로 부터 각속도를 구할 수 있다.

$$\left[\begin{array}{c}{\dot{w}}_x\\ {\dot{w}}_y\\ {\dot{w}}_z\end{array}\right]=\left[\begin{array}{ccc}c\theta & 0& -c\varphi s\theta \\ 0& 1& s\varphi \\ s\theta & 0& c\varphi c\theta \end{array}\right]\left[\begin{array}{c}\dot{\varphi }\\ \dot{\theta }\\ \dot{\psi }\end{array}\right]$$

따라서 3차원에 State Space 를 구성할 수 있다.

State Space of Quadrotors