Aerial Robotics 第 2 讲

写在前面

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

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

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

欢迎关注我的个人公众号「我不是药丸」,查看更多原创内容。

四旋翼运动学

参考系(reference frame)

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

$$
Q' = g(Q),P'=g(P)\\
\overrightarrow{P'Q'} = g(P) - g(Q) = g^*(\overrightarrow{PQ})
$$

注: $g$作用于点,$g^*$作用于向量。

刚体的运动符合以下特性:
  • $\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}\rightarrow{B}$ ,坐标系基向量${a_1,a_2,a_3}\rightarrow{b_1,b_2,b_3}$。
    file

    $$\overrightarrow{PQ} = q_1\vec{a_1}+ q_2\vec{a_2}+ q_3\vec{a_3}\\
    \overrightarrow{PQ'} = q_1\vec{b_1}+ q_2\vec{b_2}+ q_3\vec{b_3} = q_1'\vec{a_1'} + q_2'\vec{a_2} + q_3'\vec{a_3}$$

    旋转的四种表示方法

    旋转矩阵表示法 Rotation Matrix
    $$\vec{b_1} = R_{11}\vec{a_1} + R_{12}\vec{a_2} + R_{13}\vec{a_3}\\
    \vec{b_2} = R_{21}\vec{a_1} + R_{22}\vec{a_2} + R_{23}\vec{a_3}\\
    \vec{b_3} = R_{31}\vec{a_1} + R_{32}\vec{a_2} + R_{33}\vec{a_3}\\
    define:R\in\Bbb{R^{3\times3}} \\
    [q_1,q_2,q_3]^T = R[q_1',q_2',q_3']^T + \vec{d}
    $$

    注:
    R不一定是$\Bbb{R^{3\times3}}$的,在二维的情况下,他也有可能是$\Bbb{R^{2\times2}}$的。
    $\vec{d}$是坐标系${A}$的原点指向坐标系${B}$的原点的位移向量。

    旋转矩阵R有以下特性:
  • 正交性:$R^TR=T$
  • $det(R) = 1$
  • 对乘法封闭,两旋转矩阵相乘仍为旋转矩阵。
  • 旋转矩阵的逆仍为旋转矩阵。
    绕X,Y,Z轴的旋转矩阵
    $$
    Rot(x,\theta) = \begin{bmatrix}1&0&0\\0&C\theta&-S\theta\\0&S\theta&C\theta\end{bmatrix}\\
    Rot(y,\theta) = \begin{bmatrix}C\theta&0&S\theta\\0&1&0\\-S\theta&0&C\theta\end{bmatrix}\\
    Rot(z,\theta) = \begin{bmatrix}C\theta&-S\theta&0\\S\theta&C\theta&0\\0&0&1\end{bmatrix}
    $$

    注:此时还没有学欧拉角,故均用$\theta$表示

    旋转矩阵与角速度

    由于旋转矩阵有正交性,因此有下式:

    $$
    \left\{
    \begin{array}{}
    R^T(t)R(t)=I\\R(t)R^T(t)=I
    \end{array}
    \right.\underrightarrow{d/dt}
    \left\{
    \begin{array}{}
    \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}
    \right.
    $$
    $R^T\dot{R}$和$\dot{R}R^T$都是反对称的。
    $$
    q(t)=Rp(t) \to \dot{q} = R\dot{p} \to R^T\dot{q}=R^T\dot{R}p\\
    \left\{
    \begin{array}{}
    q = R p\\ \dot{q} = \dot{R}p
    \end{array}
    \right.\to
    \dot{q}=\dot{R}R^Tq
    $$

    物理含义
    $\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(\phi)$,则有:

    $$
    \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}_zR_x+R_x^T\dot{R}_x\\
    \hat{\omega}^s = \dot R_z R_z^T +R_z\dot R_x R_x^TR_z^T
    $$

    $\hat{\omega}^s$和$\hat{\omega}^b$有如下关系:

    $$\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$$
    数学补充

    $SO(3) = {R\in\Bbb{R^{3\times3}}|R^TR=RR^T=I,det(R)=1}$
    $SO(3)$意为3维特殊直角坐标系的集合,但总觉得这个解释怪怪的。

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

欧拉角表示法

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

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

$$
R_D^A = R_B^A*R_C^B*R_D^C = Rot(x,\psi)*Rot(y,\phi)*rot(z,\theta)
$$

上述三个角度分别为滚转角$\phi$(roll),俯仰角$\theta$(pitch),偏航角$\phi$(yaw)。

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

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

ZYZ欧拉角
$$
Rot(z,\phi) \rightarrow Rot(y,\theta) \rightarrow Rot(z,\psi)
$$

注意:当$\theta=0$的时候,第一次旋转的$z$轴和第三次旋转的$z$轴将是线性相关的,根据定义,这时的$\phi, \psi, \theta$无法构成一组欧拉角。

$$R = \begin{bmatrix}R_{11}&R_{12}&C\phi  S\theta\\R_{21}&R_{22}&S\phi S\theta\\-S\theta C\psi&S\theta S\psi&C\theta\end{bmatrix}$$

由上述旋转矩阵可以导出两组欧拉角

$$if \vert R_{33} \vert<1,\\
\theta=\sigma acos(R_{33}),\sigma = \pm1  \\ 
\psi = atan2(\frac{R_{32}}{sin(\theta)},- \frac{R_{31}}{sin(\theta)})  \\
\phi = atan2(\frac{R_{23}}{sin(\theta)}, \frac{R_{13}}{sin(\theta)}) \\
$$

由上式可以看到,当 $R{33}<1$ 时,一个旋转矩阵对应了两种欧拉角表示,同时也要注意的是,当$R{33}=\pm 1$的时候,$\theta$可以被唯一确定,但$\psi$和$\phi$有无数种组合。

ZXY欧拉角

ZXY欧拉角也是四旋翼运动学中比较常用的欧拉角系,,下式为ZXY系欧拉角的分解过程。

$$Rot(z,\psi) \rightarrow Rot(x,\phi) \rightarrow Rot(y,\theta)$$

下式为ZXY系欧拉角的旋转矩阵,无论在何种欧拉角系下,由旋转矩阵R算欧拉角,你都至少有两个解。

$$
R = \begin{bmatrix} C\psi C\theta - S\phi S\psi S\theta&-C\phi S\psi&C\psi S\theta + C\theta S\phi S\psi\\C\theta S\psi + C\psi S\phi S\theta&C\phi C\psi &S\psi S\theta -C\theta S\phi C\psi\\-C\phi S\theta&S\phi &C\phi C\theta \end{bmatrix}
$$
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点,对该旋转有下式:

$$
\overrightarrow{OP} = p_1\vec{a_1}+p_2\vec{a_2}+p_3\vec{a_3}\\
\overrightarrow{Op'} = q_1\vec{a_1}+q_2\vec{a_2}+q_3\vec{a_3}\\
\vec{q} = R\vec{p}
$$

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

$$\vec{p} = R\vec{p}\tag{1}$$

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

$$A\vec{v} = \lambda \vec{v}\tag{2}$$

比较(1)式和(2)式,发现问题转化为了证明:旋转矩阵R必有特征值1。
由于旋转矩阵R有性质$R^TR=I$,因此旋转矩阵为正交矩阵,由于正交矩阵的特征值只能是$\pm1$,又有$det(R)=\lambda_1 \lambda_2 \lambda_3 =1$,很显然,R必有特征值$\lambda = 1$。

证毕

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

file
转轴为$\vec{u}$,旋转角为$\phi$,旋转前的向量为$\vec{p}$

$$
\vec{p}cos(\phi)+uu^T(1-cos(\phi))+\vec{u}\times\vec{p}sin(\phi)
$$

在这里定义hat-operator $\hat{u}$,也有些书籍中将其写为$\omega_{[\times]}$。

$$
\vec{u} = (u_1,u_2,u_3)^T\\
\hat{\vec{u}} = \begin{bmatrix}0&-u_3&u_2\\u_3&0&-u_1\\-u_2&u_1&0 \end{bmatrix}\\
$$
        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},\phi)=Icos(\phi)+uu^T(1-cos(\phi))+\hat{u}sin{\phi}\tag{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}$都能对应该矩阵。

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

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

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


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

$$cos(\phi) = \frac{\tau-1}{2}\\ \hat{u}=\frac{1}{2sin(\phi)}(R-R^T)$$

上面这块是他上课讲的,但现在复习起来我有两个疑惑:
一、上面的第一个公式中的$\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})$$
运算

$$
p \pm q = (p_0 \pm q_0,\vec{p} \pm \vec{q})\\
pq=(p_0q_0-p^Tq,p_0\vec{q}+q_0\vec{p}+\vec{p}\times\vec{q})\\
q^{-1}=(q_0,-\vec{p})
$$
四元表示法和旋转轴-角度坐标的转换
$$
(\phi,\vec{u})\rightarrow q = (cos(\frac{\phi}{2}),sin(\frac{\phi}{2})\vec{u})\\
q \rightarrow \phi = 2acos(q_0),\vec{u}=\frac{\vec{q}}{ \sqrt{1-q_0^2}}
$$
旋转的几点性质

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

四元表示法的几个优点:

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

四旋翼动力学

注:以下内容均使用Z-X-Y系欧拉角($\psi$,$\phi$,$\theta$),惯性系为${A}\rightarrow{\vec{a_1},\vec{a_2},\vec{a_3}}$,机体系为${B}\rightarrow{\vec{b_1},\vec{b_2},\vec{b_3}}$。

升力,力矩-角速度建模:

$$
Fi=K_F\omega^2_i\\Mi=K_m\omega^2_i
$$

平衡方程

该刚心的质量中心为C,向量$\vec{r_c}=\vec{OC}$可以表示如下:

$$
\vec{r_c}=\frac{1}{m}\sum_{i=1}^{N}m_i \vec{p_i}
$$

可以有牛顿第二定理,动量定理和角动量定理对该刚体列出如下平衡方程:

$$
\vec{F}=m\frac{d\vec{v}}{dt}\\\tag{4.1}
\vec{F}=\frac{d(^A\vec{L})}{dt}\\
$$$$
\frac{d({^AH^B_C})}{dt}=M^B_C\tag{4.2},\\
^AH^B_C=I_C(^A\omega^B)
$$

左上角标为参考系,右上角标为刚体的名称,右下角标为质量中心名。
$I_C$为相对于质量中心C的惯量矩阵。
$(4.2)$式的物理含义为:刚体B相对于质量中心C在参考系A内的变化率等于合外力相对于质量中心产生的合力矩。

可以对$(4.2)$式做如下变换:

$$
\frac{d({^AH_C})}{dt} = M_C = \frac{d({^BH_C})}{dt}+^A\omega^B\times H_C,\\
\frac{d({^BH_C})}{dt}=I_{11}\dot{\omega_1} \vec{b_1}+I_{22}\dot{\omega_2} \vec{b_2}+I_{33}\dot{\omega_3} \vec{b_3}
$$

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

$$
\begin{bmatrix}I_{11}&0&0\\0&I_{22}&0\\0&0&I_{33}\end{bmatrix}
\begin{bmatrix}\dot{\omega_1\\\dot{\omega_2}}\\\dot{\omega_3}\end{bmatrix}+
\begin{bmatrix}0&-\omega_3&\omega_2\\\omega_3&0&-\omega_1\\-\omega_2&\omega_1&0\end{bmatrix}
\begin{bmatrix}I_{11}&0&0\\0&I_{22}&0\\0&0&I_{33}\end{bmatrix}
\begin{bmatrix}\omega_1\\\omega_2\\\omega_3\end{bmatrix}=
\begin{bmatrix}M_{C,1}\\M_{C,2}\\M_{C,3}\end{bmatrix}\\
\dot{\omega}=(\dot{\omega_1},\dot{\omega_2},\dot{\omega_3})^T=(\dot p,\dot q,\dot r)^T
$$

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

上式又可以写为:

$$
I_C\dot{\omega}=
\begin{bmatrix}L(F_3-F_4)\\L(F_3-F-1)\\M_1-M_2+M_3-M_4\end{bmatrix}-
\omega\times I \omega
$$

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

惯性矩:面积对某轴的二次矩:$I_x = \intAy^2dA$
惯性积:面积与其到轴1,轴2距离的乘积称为该面积对坐标轴的惯性积:$I
{xy}=\int xy dA$
惯量矩阵:

$$I=\begin{bmatrix}I_{xx}&I_{xy}&I_{xz}\\I_{yx}&I_{yy}&I_{yz}\\I_{zx}&I_{zy}&I_{zz}\end{bmatrix}$$

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


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

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

$$
m\ddot{y}=sin(\phi)u_1\\
m\ddot{z}=cos(\phi)u_1+mg\\
I_{xx}\ddot{\phi}=u_2
$$

1.找出系统的阶数,即变量的最大阶数:n=2。
2.将变量写为状态变量的形式,对每个变量从零阶写到(n-1)阶:$x_1=y,x_2=z,x_3=\phi,x_4=\dot{y},x_5=\dot{z},x_6=\dot{\phi}$。
3.将状态变量写为向量形式:$\vec{x}=[x_1,x_2,x_3,x_4,x_5,x_6]^T$。
4.写出成对的一阶微分方程。(略)
5.将其写为矩阵形式
6.

$$
\begin{bmatrix}\dot{x_1}\\\dot{x_2}\\\dot{x_3}\\\dot{x_4}\\\dot{x_5}\\\dot{x_6}\\\end{bmatrix}=
\begin{bmatrix}x_4\\x_5\\x_6\\sin(x_3)u_1/m\\cos(x_3)u_1/m+g\\u_2/I_{xx}\end{bmatrix}
$$

上式也是四旋翼二维运动方程。