coursera Aerial Robotics 无人飞行器 笔记2

写在前面

第二周的课程主要内容是四旋翼运动学和动力学，数学和力学知识比较多。重点内容是旋转的几种表示方法及其相互转换的方法。编程作业为一维的四旋翼控制仿真，比较简单。

该笔记由课程学习时记的笔记整理而成，并且由于该课程免费期已过，我现在已经无法访问，没法下载该课程的slides，因此配图都是ipad手绘，如有错误，还请指正。

温故而知新，整理这份笔记的过程中，我对此课程有了很多新的理解，也有了很多新的疑惑。欢迎朋友们在评论区留下你们的见解。

四旋翼运动学

参考系(reference frame)

对于物体O$\in$$R^3$，经过一段位移，固连在机体上的坐标系由{A}变为{B}，此时前后两参考系有如下关系。

$$\begin{gathered} Q^{\prime }=g(Q),P^{\prime }=g(P) \\ \overrightarrow{P^{\prime }{Q}^{\prime }}=g(P)-g(Q)=g^{\ast }(\overrightarrow{PQ}) \end{gathered}$$
注： $g$作用于点，$g^{\ast }$作用于向量。

刚体的运动符合以下特性：

• $\Vert g(P)-g(Q)\Vert =\Vert P-Q\Vert$ 即位移前后刚体上两点的距离不变，这是由刚体的刚性决定的。
• $g^*(\vec{v})*g(\vec{w})= g^(\vec{v} \times \vec{w}) $ 即位移前后固连在刚体两点上的向量的外积保持不变。
• 另外，$\vec{v}$和\vec{w}的内积也保持不变。

基向量

对刚体位移 { A } → { B } \{A\}\rightarrow\{B\} {A}→{B} ，坐标系基向量 { a 1 , a 2 , a 3 } → { b 1 , b 2 , b 3 } \{a_1,a_2,a_3\}\rightarrow\{b_1,b_2,b_3\} {a1​,a2​,a3​}→{b1​,b2​,b3​}。

$$\begin{gathered} \overrightarrow{PQ}=q_1\overrightarrow{a_1}+q_2\overrightarrow{a_2}+q_3\overrightarrow{a_3} \\ \overrightarrow{P{Q}^{\prime }}=q_1\overrightarrow{b_1}+q_2\overrightarrow{b_2}+q_3\overrightarrow{b_3}=q_1^{\prime }\overrightarrow{a_1^{\prime }}+q_2^{\prime }\overrightarrow{a_2}+q_3^{\prime }\overrightarrow{a_3} \end{gathered}$$

旋转的四种表示方法

旋转矩阵表示法 Rotation Matrix

$$\begin{gathered} \overrightarrow{b_1}=R_{11}\overrightarrow{a_1}+R_{12}\overrightarrow{a_2}+R_{13}\overrightarrow{a_3} \\ \overrightarrow{b_2}=R_{21}\overrightarrow{a_1}+R_{22}\overrightarrow{a_2}+R_{23}\overrightarrow{a_3} \\ \overrightarrow{b_3}=R_{31}\overrightarrow{a_1}+R_{32}\overrightarrow{a_2}+R_{33}\overrightarrow{a_3} \\ define:R\in {\mathbb{R}}^{3\times 3} \\ [q_1,q_2,q_3{]}^T=R[q_1^{\prime },q_2^{\prime },q_3^{\prime }{]}^T+\vec{d} \end{gathered}$$
注：
R不一定是${\mathbb{R}}^{3\times 3}$的，在二维的情况下，他也有可能是${\mathbb{R}}^{2\times 2}$的。
$\vec{d}$是坐标系 { A } \{A\} {A}的原点指向坐标系 { B } \{B\} {B}的原点的位移向量。

旋转矩阵R有以下特性：

• 正交性：$R^{T}R=T$
• $det(R)=1$
• 对乘法封闭，两旋转矩阵相乘仍为旋转矩阵。
• 旋转矩阵的逆仍为旋转矩阵。
  ######绕X,Y,Z轴的旋转矩阵
  $$\begin{gathered} Rot(x,\theta )=\left[\begin{array}{ccc}1& 0& 0\\ 0& C\theta & -S\theta \\ 0& S\theta & C\theta \end{array}\right] \\ Rot(y,\theta )=\left[\begin{array}{ccc}C\theta & 0& S\theta \\ 0& 1& 0\\ -S\theta & 0& C\theta \end{array}\right] \\ Rot(z,\theta )=\left[\begin{array}{ccc}C\theta & -S\theta & 0\\ S\theta & C\theta & 0\\ 0& 0& 1\end{array}\right] \end{gathered}$$
  注：此时还没有学欧拉角，故均用$\theta$表示

旋转矩阵与角速度

由于旋转矩阵有正交性，因此有下式：
$$\{\begin{array}{c}{R}^T(t)R(t)=I\\ R(t)R^T(t)=I\end{array}\underrightarrow{d/dt}\{\begin{array}{c}{\dot{R}}^T(t)R(t)+R^T(t)\dot{R}(t)=0\\ R(t){\dot{R}}^T(t)+\dot{R}(t)R^T(t)=0\end{array}$$
$R^T\dot{R}$和$\dot{R}{R}^T$都是反对称的。
$$\begin{gathered} q(t)=Rp(t)\to \dot{q}=R\dot{p}\to R^T\dot{q}=R^T\dot{R}p \\ \{\begin{array}{c}q=Rp\\ \dot{q}=\dot{R}p\end{array}\to \dot{q}=\dot{R}{R}^{T}q \end{gathered}$$
物理含义
$\dot{q}$：惯性系下的速度
$p$：体坐标系下的位置
$R^T\dot{q}$：体坐标系下的速度
$R^T\dot{R}$：体坐标系下的角速度${\hat{\omega }}^b$（body）
$\dot{R}{R}^T$：惯性坐标系下的角速度${\hat{\omega }}^s$（spacial）
惯性坐标系角速度$hat{\omega }^s$和体坐标系角速度${\hat{\omega }}^b$的一些性质：
设有两个旋转：$R=R_z(\theta )R_x(\varphi )$，则有：
$$\begin{gathered} {\hat{\omega }}^b=R^T\dot{R}=(R_z{R}_x{)}^T(\dot{R_z}{R}_x+R_z\dot{R_x})=R_x^T{R}_z^T{\dot{R}}_z{R}_x+R_x^T{\dot{R}}_x \\ {\hat{\omega }}^s={\dot{R}}_z{R}_z^T+R_z{\dot{R}}_x{R}_x^T{R}_z^T \end{gathered}$$
${\hat{\omega }}^s$和${\hat{\omega }}^b$有如下关系：
$$\begin{gathered} {\hat{\omega }}^b=R_T\dot{R}=R^T\dot{R}{R}^{T}R=R^T{\hat{\omega }}^{s}R \\ {\hat{\omega }}^s=R{\hat{\omega }}^b{R}^T \end{gathered}$$

数学补充

S O ( 3 ) = { R ∈ R 3 × 3 ∣ R T R = R R T = I , d e t ( R ) = 1 } SO(3) = \{R\in\Bbb{R^{3\times3}}|R^TR=RR^T=I,det(R)=1\} SO(3)={R∈R3×3∣RTR=RRT=I,det(R)=1}
$SO(3)$意为3维特殊直角坐标系的集合，但总觉得这个解释怪怪的。

injective：一一映射，对定义域中的任意两个数$a,b$，若$f(a)=f(b)$则$a=b$。
surjective：满射，对值域中的任意一个$y$，都至少有一个$x$使得$f(x)=y$。
通过改变函数的定义域或值域，可以使函数成为一一映射。

欧拉角表示法

欧拉：要描述一个一般的旋转，至少需要3个坐标。

这三个坐标被称为欧拉角。

$$R_D^A=R_B^A\ast R_C^B\ast R_D^C=Rot(x,\psi )\ast Rot(y,\varphi )\ast rot(z,\theta )$$
上述三个角度分别为滚转角$\varphi$（roll），俯仰角$\theta$（pitch），偏航角$\varphi$（yaw）。

欧拉：任何旋转都能被表示为关于三个线性无关的坐标轴的旋转的累积。

旋转矩阵和欧拉角表示法不是一一映射，由一组欧拉角一定可以写出唯一的一个旋转矩阵，但由一个旋转矩阵不一定能写出唯一的一组欧拉角。

ZYZ欧拉角

$$Rot(z,\varphi )\to Rot(y,\theta )\to Rot(z,\psi )$$
注意：当$\theta =0$的时候，第一次旋转的$z$轴和第三次旋转的$z$轴将是线性相关的，根据定义，这时的$\varphi ,\psi ,\theta$无法构成一组欧拉角。
$$R=\left[\begin{array}{ccc}{R}_{11}& R_{12}& C\varphi S\theta \\ R_{21}& R_{22}& S\varphi S\theta \\ -S\theta C\psi & S\theta S\psi & C\theta \end{array}\right]$$
由上述旋转矩阵可以导出两组欧拉角
$$\begin{gathered} if|R_{33}|<1, \\ \theta =\sigma acos(R_{33}),\sigma =\pm 1 \\ \psi =atan2(\frac{R_{32}}{sin(\theta )}，-\frac{R_{31}}{sin(\theta )}) \\ \varphi =atan2(\frac{R_{23}}{sin(\theta )},\frac{R_{13}}{sin(\theta )}) \end{gathered}$$
由上式可以看到，当$R_{33}<1$时，一个旋转矩阵对应了两种欧拉角表示，同时也要注意的是，当$R_{33}=\pm 1$的时候，$\theta$可以被唯一确定，但$\psi$和$\varphi$有无数种组合。

ZXY欧拉角

ZXY欧拉角也是四旋翼运动学中比较常用的欧拉角系,，下式为ZXY系欧拉角的分解过程。
$$Rot(z,\psi )\to Rot(x,\varphi )\to Rot(y,\theta )$$
下式为ZXY系欧拉角的旋转矩阵，无论在何种欧拉角系下，由旋转矩阵R算欧拉角，你都至少有两个解。
$$R=\left[\begin{array}{ccc}C\psi C\theta -S\varphi S\psi S\theta & -C\varphi S\psi & C\psi S\theta +C\theta S\varphi S\psi \\ C\theta S\psi +C\psi S\varphi S\theta & C\varphi C\psi & S\psi S\theta -C\theta S\varphi C\psi \\ -C\varphi S\theta & S\varphi & C\varphi C\theta \end{array}\right]$$

syms psi phi theta real;

R = [cos(psi)*cos(theta)-sin(phi)*sin(psi)*sin(theta),...
	-cos(phi)*sin(psi),cos(psi)*sin(theta)+cos(theta)*sin(phi)*sin(psi);...
     cos(theta)*sin(psi)+cos(psi)*sin(phi)*sin(theta), ...
     cos(phi)*cos(psi),sin(psi)*sin(theta)-cos(theta)*sin(phi)*cos(psi);...
     -cos(phi)*sin(theta),sin(phi),cos(phi)*cos(theta)];

转轴-角度表示法

欧拉：对刚体上的一点O，对于它的任何运动，总存在某一固定轴，使得该运动等同于O点绕该固定轴的转动。

证明

下面对欧拉的这个论断予以证明：
设P为刚体上的任意一点，P’为刚体旋转后的P点，对该旋转有下式：

$$\begin{gathered} \overrightarrow{OP}=p_1\overrightarrow{a_1}+p_2\overrightarrow{a_2}+p_3\overrightarrow{a_3} \\ \overrightarrow{O{p}^{\prime }}=q_1\overrightarrow{a_1}+q_2\overrightarrow{a_2}+q_3\overrightarrow{a_3} \\ \vec{q}=R\vec{p} \end{gathered}$$

现在将该证明转换为下列问题：是否存在一点P使得经过旋转后，P=P’（若存在，$\vec{p}=\overrightarrow{OP}$就是该旋转的转轴。），对于这样的一点P存在关系$\vec{q}=\vec{p}$，因此有

$$\vec{p}=R\vec{p}\text{\hspace{1em}(1)}$$

---

数学补充：
特征向量：方阵A的特征向量在和A相乘之后方向不改变。
特征值：某个特征向量和A相乘后该特征向量长度的变化。
特征值和特征向量的关系可由下式列出。

$$A\vec{v}=\lambda \vec{v}\text{\hspace{1em}(2)}$$

---

比较(1)式和(2)式，发现问题转化为了证明：旋转矩阵R必有特征值1。
由于旋转矩阵R有性质$R^{T}R=I$，因此旋转矩阵为正交矩阵，由于正交矩阵的特征值只能是$\pm 1$，又有$det(R)={\lambda }_1{\lambda }_2{\lambda }_3=1$，很显然，R必有特征值$\lambda =1$。
#证毕

给定一组转轴和旋转角，求旋转矩阵R

转轴为$\vec{u}$，旋转角为$\varphi$，旋转前的向量为$\vec{p}$
$$\vec{p}cos(\varphi )+u{u}^T(1-cos(\varphi ))+\vec{u}\times \vec{p}sin(\varphi )$$
在这里定义hat-operator $\hat{u}$，也有些书籍中将其写为${\omega }_{[\times ]}$。
$$\begin{gathered} \vec{u}=(u_1,u_2,u_3{)}^T \\ \hat{\vec{u}}=\left[\begin{array}{ccc}0& -u_3& u_2\\ u_3& 0& -u_1\\ -u_2& u_1& 0\end{array}\right] \end{gathered}$$

		function x_hat = hat_operator(x)
		x_hat = [0,-x(3),x(2);x(3),0,-x(1);-x(2),x(1),0];
		end

可以对应旋转矩阵的表达式：
$$Rot(\vec{u},\varphi )=Icos(\varphi )+u{u}^T(1-cos(\varphi ))+\hat{u}sin\varphi \text{\hspace{1em}(3)}$$

%given_u_phi.m
u = [3^0.5/3,3^0.5/3,3^0.5/3].';
phi = pi*3/4;
u_hat = hat_operator(u);
I = eye(3);
R = I*cos(phi) + u*u.'*(1-cos(phi)) + u_hat*sin(phi)

给定一个旋转矩阵，求解相应的转轴和角度

• 该问题是满射的：对于每一个旋转矩阵R，都至少对应一组转轴和角度。
• 该问题不是一一映射：对于同一个旋转矩阵R，显而易见，转轴$\vec{u}$和转轴$-\vec{u}$都能对应该矩阵。

前面提过，限制函数的定义域和值域，可以使其变为一一对应的函数。
首先限制值域$\varphi \in [0,\pi ]$，这时转轴$\vec{u}$和转轴$-\vec{u}$将对应不同的矩阵。这时该函数是否是一一对应的呢?
首先分析两个特殊的点，$\varphi =0,\pi$：

$\varphi =0$时，式$(3)$变为$Rot(\vec{u},\varphi )=I$，这时R和$\vec{u},\varphi$都没有关系，R对应无数多个转轴-旋转角。

$\varphi =\pi$时，式$(3)$变为$R=-I+2\vec{u}\overrightarrow{u^T}=-I+2(-\vec{u})(-{\vec{u}}^T)$，因此R对应$\vec{u}$和$-\vec{u}$两个转轴。

---

而当$\varphi \in (0,\pi )$时，每个旋转矩阵R将只对应一组转轴-转角。

$$\begin{gathered} cos(\varphi )=\frac{\tau -1}{2} \\ \hat{u}=\frac{1}{2sin(\varphi )}(R-R^T) \end{gathered}$$

上面这块是他上课讲的，但现在复习起来我有两个疑惑：
一、上面的第一个公式中的$\tau$是什么。
二、我用matlab算了一下，得到的结果是每个旋转矩阵对应两组转轴-角度，下面是我的演算程序，这和我在课上听到的结论不太符合，也希望有朋友能解答我的这个疑惑。

%Given_R.m
clc;
clear;

R = sym('R',[3,3]);
u = sym('u',[3,1]);
u_hat = hat_operator(u);
phi = sym('phi');
I = sym(eye(3));
% RR是当u = [3^0.5/3,3^0.5/3,3^0.5/3].';phi = pi*3/4;Given_u_phi.m生成的R
% RR = [    -0.1381    0.1608    0.9773;
%     0.9773   -0.1381    0.1608;
%     0.1608    0.9773   -0.1381]

Rot = I*cos(phi) + u*u.'*(1-cos(phi)) + u_hat*sin(phi);

eqns_1 = u_hat == (R - R.')/(2*sin(phi));
eqns_1 = eqns_1(:).';
eqns = [eqns_1];
vars = [u.'];
sol_1 = solve(eqns,vars);
solve_1 = [sol_1.u1,sol_1.u2,sol_1.u3];
solve_1 = simplify(solve_1)%u

eqns_2 = diag(Rot) == diag(R);
eqns_2 = eqns_2(:).';
eqns = [eqns_2];
sol_2 = solve(eqns,vars);
solve_2 = [sol_2.u1,sol_2.u2,sol_2.u3];
solve_2 = simplify(solve_2.^2);
solve_2 = solve_2(1,:)%u.^2

assume(phi>0 & phi<pi);
s1 = solve(solve_1(1).^2 == solve_2(1),phi);
s2 = solve(solve_1(2).^2 == solve_2(2),phi);
s3 = solve(solve_1(3).^2 == solve_2(3),phi);
s = [s1.',s2.',s3.'].';
s = subs(s,'R1_1',RR(1,1));
s = subs(s,'R1_2',RR(1,2));
s = subs(s,'R1_3',RR(1,3));
s = subs(s,'R2_1',RR(2,1));
s = subs(s,'R2_2',RR(2,2));
s = subs(s,'R2_3',RR(2,3));
s = subs(s,'R3_1',RR(3,1));
s = subs(s,'R3_2',RR(3,2));
s = subs(s,'R3_3',RR(3,3));

s = vpa(s,4)

四元表示法

该方法在数学知识补充中讲解，感觉是一个据说很方便很厉害，但到最后也没用到的表示方法，下面给出它的运算规则和一些性质。
表示：
$$q=(q_0,q_1,q_2,q_3)=(q_0,\vec{q})$$
运算：
$$\begin{gathered} p\pm q=(p_0\pm q_0,\vec{p}\pm \vec{q}) \\ pq=(p_0{q}_0-p^{T}q,p_0\vec{q}+q_0\vec{p}+\vec{p}\times \vec{q}) \\ {q}^{-1}=(q_0,-\vec{p}) \end{gathered}$$

四元表示法和旋转轴-角度坐标的转换

$$\begin{gathered} (\varphi ,\vec{u})\to q=(cos(\frac{\varphi }{2}),sin(\frac{\varphi }{2})\vec{u}) \\ q\to \varphi =2acos(q_0),\vec{u}=\frac{\vec{q}}{\sqrt{1-q_0^2}} \end{gathered}$$

旋转的几点性质

要旋转$\vec{p}\in {\mathbb{R}}^3$
1.$p=(0,\vec{p})$
2.$p^{\prime }=qp{q}^{-1}=(0,q^{\prime })$
将旋转$q_1$和$q_2$组合起来：$q=q_1{q}_2$
$q_1$和$-q_1$表示的是相同的旋转。

四元表示法的几个优点：

1.更紧凑，只有4个变量。
2.没有奇点。（一一映射）
3.数值计算更稳定，很适合计算机运算。

四旋翼动力学

**注：**以下内容均使用Z-X-Y系欧拉角($\psi$,$\varphi$,$\theta$)，惯性系为 { A } → { a 1 ⃗ , a 2 ⃗ , a 3 ⃗ } \{A\}\rightarrow\{\vec{a_1},\vec{a_2},\vec{a_3}\} {A}→{a1​ ​,a2​ ​,a3​ ​}，机体系为 { B } → { b 1 ⃗ , b 2 ⃗ , b 3 ⃗ } \{B\}\rightarrow\{\vec{b_1},\vec{b_2},\vec{b_3}\} {B}→{b1​ ​,b2​ ​,b3​ ​}。

升力，力矩-角速度建模：

$$\begin{gathered} Fi=K_F{\omega }_i^2 \\ Mi=K_m{\omega }_i^2 \end{gathered}$$

平衡方程

该刚心的质量中心为C，向量$\overrightarrow{r_c}=\overrightarrow{OC}$可以表示如下：

$$\overrightarrow{r_c}=\frac{1}{m}\sum _{i=1}^N{m}_i\overrightarrow{p_i}$$

可以有牛顿第二定理，动量定理和角动量定理对该刚体列出如下平衡方程：
$$\begin{gathered} \vec{F}=m\frac{d\vec{v}}{dt} \\ \vec{F}=\frac{d{(}^A\vec{L})}{dt} \\ \text{\hspace{1em}(4.1)} \end{gathered}$$$$\begin{gathered} \frac{d({}^A{H}_C^B)}{dt}=M_C^B,{ \\ }^A{H}_C^B=I_C{(}^A{\omega }^B)\text{\hspace{1em}(4.2)} \end{gathered}$$
左上角标为参考系，右上角标为刚体的名称，右下角标为质量中心名。
$I_C$为相对于质量中心C的惯量矩阵。
$(4.2)$式的物理含义为：刚体B相对于质量中心C在参考系A内的变化率等于合外力相对于质量中心产生的合力矩。

可以对$(4.2)$式做如下变换：
$$\begin{gathered} \frac{d({}^A{H}_C)}{dt}=M_C=\frac{d({}^B{H}_C)}{dt}{+}^A{\omega }^B\times H_C, \\ \frac{d({}^B{H}_C)}{dt}=I_{11}\dot{{\omega }_1}\overrightarrow{b_1}+I_{22}\dot{{\omega }_2}\overrightarrow{b_2}+I_{33}\dot{{\omega }_3}\overrightarrow{b_3} \end{gathered}$$

将以上的三个式子代入$(4.2)$式并展开，可以直观地观察到该方程的结构：

KaTeX parse error: \cr valid only within a tabular/array environment

如上所示，首先可以看到惯量矩阵$I_C$是一个对角矩阵，这应该是因为机体坐标系 { B } \{B\} {B}的三个坐标轴是主惯性轴。第三个矩阵为$\hat{\omega }$。

上式又可以写为：

$$I_C\dot{\omega }=\left[\begin{array}{c}L(F_3-F_4)\\ L(F_3-F-1)\\ M_1-M_2+M_3-M_4\end{array}\right]-\omega \times I\omega$$

---

力学补充：主惯性轴和惯量矩阵

惯性矩：面积对某轴的二次矩：$I_x={\int }_A{y}^{2}dA$
惯性积：面积与其到轴1，轴2距离的乘积称为该面积对坐标轴的惯性积：$I_{xy}=\int xydA$
惯量矩阵：

$$I=\left[\begin{array}{ccc}{I}_{xx}& I_{xy}& I_{xz}\\ I_{yx}& I_{yy}& I_{yz}\\ I_{zx}& I_{zy}& I_{zz}\end{array}\right]$$

主惯性轴坐标系是使得惯性积均为零的坐标系，因此在这样的坐标系下，惯量矩阵是对角阵。

---

状态空间表示下的动力系统

动力系统：行为的效果不会立即发生的系统。
动力系统的状态演变是由一系列常微分方程控制的，这一系列常微分方程常写为状态空间的形式，下面举一个例子来说明如何写出状态空间表示下的动力系统。

$$\begin{gathered} m\ddot{y}=sin(\varphi )u_1 \\ m\ddot{z}=cos(\varphi )u_1+mg \\ {I}_{xx}\ddot{\varphi }=u_2 \end{gathered}$$

1.找出系统的阶数，即变量的最大阶数：n=2。
2.将变量写为状态变量的形式，对每个变量从零阶写到（n-1）阶：$x_1=y,x_2=z,x_3=\varphi ,x_4=\dot{y},x_5=\dot{z},x_6=\dot{\varphi }$。
3.将状态变量写为向量形式：$\vec{x}=[x_1,x_2,x_3,x_4,x_5,x_6{]}^T$。
4.写出成对的一阶微分方程。（略）
5.将其写为矩阵形式
$$\left[\begin{array}{c}\dot{x_1}\\ \dot{x_2}\\ \dot{x_3}\\ \dot{x_4}\\ \dot{x_5}\\ \dot{x_6}\end{array}\right]=\left[\begin{array}{c}{x}_4\\ x_5\\ x_6\\ sin(x_3)u_1/m\\ cos(x_3)u_1/m+g\\ u_2/I_{xx}\end{array}\right]$$
上式也是四旋翼二维运动方程。