Geometric Methods

论文阅读 Geometric Tracking Control of a Quadrotor UAV on SE(3)

这是Taeyoung Lee的经典之作

动机

普通的控制策略都是要建立空间坐标系，然后再算……但是无法用局部坐标表示的动态模型，就会出问题。因此，几何方法的目的就是coordinate independent

建模

thrust很简单，就是沿着$-\vec{b}_3$的方向，也就是$-R e_3$的方向，thrust就是$-fRe_3$

在这样的一个四旋翼问题中，直接控制的是四个电机的转速，也可以认为是四个电机的推力$f_i,i=1,\dots,4$。但是呢，也可以把输入直接当成是总thrust$f$以及三个轴上的力矩$M_1,M_2,M_3$，因为有这个可逆的关系：

$$
\begin{pmatrix}
f \\ M_1 \\M_2 \\M_3
\end{pmatrix} = \begin{pmatrix}
1 & 1& 1&1 \\
0 & -d & 0 & d \\
d & 0 & -d & 0 \\
-c & c & -c & c
\end{pmatrix} \begin{pmatrix}
f_1 \\ f_2 \\ f_3 \\ f_4
\end{pmatrix}
$$

所以为了简单，直接把输入视为$f,M_1,M_2,M_3$即可（至于怎么从转速变到$f, M_1,M_2,M_3$，是飞控的任务）

然后，原文中的(2)~(5)式就是牛顿以及欧拉动力学方程，略

控制目标如何设置

我们的目标是让无人机跟踪$x _ d(t)$以及$\vec{b} _ {1 _ d}(t)$，前者代表中心点的期望位置，后者代表$\vec{b} _ 1$轴的期望朝向。为什么目标是跟踪这两个？下面简单解释：

简单分析就知道，平动的加速度只能由thrust产生，而thrust又必须垂直于四旋翼平面，因此一旦给定了期望轨迹$x _ d (t)$，那么求导后就知道了期望加速度$a _ d (t)$，那么就知道了期望的thrust方向以及大小，其中期望的thrust方向也就是$-\vec{b} _ 3(t)$

所以，考虑刚体运动的期望值（三个平动期望+三个转动期望）该怎么设置？一旦$x _ d (t)$这个东西确定下来后，我们在平动上已经没有自由度了，而且也确定了$\vec{b} _ 3(t)$，所以只剩下一个自由度了（就是可以自由的绕着$\vec{b} _ 3(t)$旋转），因此要人为给定$\vec{b} _ {1 _ d}(t)$

不过还需要注意一点，理想情况下$\vec{b} _ {1 _ d}(t)$应该是与$x _ d(t)$有一个内部的约束关系（因为$b_1$轴必须永远和$b _ 3$垂直嘛！）但是这里$\vec{b} _ {1 _ d}(t)$是自由指定的，所以可能出现矛盾。因此，我们用这样的策略：把$\vec{b} _ {1 _ d}$投影到与$\vec{b} _ {3 _ d}$垂直的平面上！即，定义$\vec{b} _ {2 _ d} := \vec{b} _ {3 _ d}\times \vec{b} _ {1 _ d} / \Vert \vec{b} _ {3 _ d}\times \vec{b} _ {1 _ d} \Vert$，然后期望旋转阵定义为$R _ d = ( \vec{b} _ {2 _ d} \times \vec{b} _ {3 _ d} , \vec{b} _ {2 _ d}, \vec{b} _ {3 _ d})$，以后，我们将这样记：$\text{Proj} [\vec{b} _ {1 _ d}] = \vec{b} _ {2 _ d} \times \vec{b} _ {3 _ d}$

总结一下，就是在“怎么给定期望”这个问题上需要做一些思考，我们优先保证平动的轨迹追踪，再用“曲折”的方法定义期望的姿态

误差函数与误差向量

平动的误差很简单（没有用任何几何方法，就是普通的坐标依赖的方法）$e_x = x-x_d, e_v = v-v_d$

对于姿态，这个误差函数选取为$\Psi(R,R_d) = \frac{1}{2} \text{tr} (I-R_d^\top R)$为啥这样定义可以见前一篇的讲解，提到了trace的含义

现在，我们把这个误差函数扰动一下，这个扰动本身是一个角速度，记为$\eta $，满足$\hat{\eta}\in \mathfrak{so(3)}$，扰动是作用在联体坐标系下的，根据“右乘联体”，其效果是：$R\mapsto R e^{\hat{\eta}\epsilon}$

因此，对这个$\epsilon$求导，其导数即$\delta R = R\hat{\eta}$，那么，在这个扰动下，误差$\Psi$的变化率即为$D_R\Psi(R,R_d) = -\frac{1}{2} \text{tr} (R_d^\top R\hat{\eta})$。用一下恒等式变换，即可写为：

$$
D_R\Psi(R,R_d) = \frac{1}{2} (R_d^\top R -R^\top R_d)^\vee \cdot \eta
$$

这样的的话，我们选取误差向量为$e_R = \frac{1}{2} (R_d^\top R -R^\top R_d)^\vee$，这个误差向量就是$\Psi$的梯度方向

再去看对应“速度误差向量”的部分，我们企图把$\dot{R}$与$\dot{R}_d$直接相减，但是那样做是违法的。所以先得把它俩放到同一个切空间里头，最方便的是都找到其对应的$so(3)$中的元素，分别是$\dot{R}R^\top$以及$\dot{R} _ dR _ d^\top$，两个的含义都是世界系下看的角速度，因此当然可以直接相减！所以就有$\hat{\omega} _ {s _ \text{error}} = \dot{R}R^\top - \dot{R} _ dR _ d^\top$

但是，实际能指导无人机的一定是相对于本体的坐标系的角速度！所以我们把$\hat{\omega} _ {s _ \text{error}}$变到$\hat{\omega} _ {b _ \text{error}}$去！根据前一篇的 定理 （$R\hat{\omega}R^\top = \widehat{R\omega}$），可以知道这个相差的角速度在$R$时的本体系下看应当是 $\hat{\omega} _ {b _ \text{error}} = \widehat{R^\top \omega} _ {s _ \text{error}} = R^\top \hat{\omega} _ {s _ \text{error}} R = R^\top \dot{R}-R^\top \dot{R} _ dR _ d^\top R$

所以，我们选取$e_\Omega = \omega _ {b _ \text{error}} = (R^\top \dot{R}-R^\top\dot{R}_dR_d^\top R)^\vee $利用vee算符的线性性，进一步得到：

$$
e_\Omega = \Omega -( R^\top\dot{R}_dR_d^\top R )^\vee = \Omega -( R^\top R_d R_d^\top \dot{R}_d R_d^\top R )^\vee = \Omega -( R^\top R_d \hat{\Omega}_d R_d^\top R )^\vee
$$

再倒着利用一下 定理 （$R\hat{\omega}R^\top = \widehat{R\omega}$），即得到：

$$
e_\Omega =\Omega - R^\top R_d \Omega_d
$$

这就是文中的结果！

控制律

控制律中需要用$R_d$，所以先来看看$R_d$怎么得到：

一个最显然的控制目标就是：
$$
\vec{b}_{3_d} = \frac{-mge_3+ m\ddot{x}_d}{\Vert -mge_3+ m\ddot{x}_d\Vert}
$$

这是直接根据$f_d = m \ddot{x}_d -mge_3$得来的

但是这是理想的情况，实际可能会偏移，所以该文章加了一个类似于PD控制的反馈项：
$$
\vec{b}_{3_d} = \frac{-k_x e_x -k_v e_v -mge_3+ m\ddot{x}_d}{\Vert -k_x e_x -k_v e_v -mge_3+ m\ddot{x}_d\Vert}
$$

所以，由这个式子给出$\vec{b} _ {3 _ d}$，再结合前面的小结“控制目标如何设置”的分析，用这个式子定义$\vec{b} _ {2 _ d}$：$\vec{b} _ {2 _ d} := \vec{b} _ {3 _ d}\times \vec{b} _ {1 _ d} / \Vert \vec{b} _ {3 _ d}\times \vec{b} _ {1 _ d} \Vert$。最后，据此定义期望的姿态阵$R _ d = ( \vec{b} _ {2 _ d} \times \vec{b} _ {3 _ d} , \vec{b} _ {2 _ d}, \vec{b} _ {3 _ d})$，至此，$R _ d$就定义好了

具体的控制律：

$$
\begin{align*}
f &= −(−k _ x e _ x − k _ v e _ v − mge _ 3 + m \ddot{x} _ d) \cdot Re _ 3 \\
M &= −k_R e _ R − k _ \Omega e _ \Omega + \Omega \times J\Omega − J (\hat{\Omega} R^\top R _ d \Omega_d − R^\top R _ d \dot{\Omega} _ d)
\end{align*}
$$

为啥凭空冒出来这么一个控制律？可以这样想，整体的控制是串级的结构，内环是姿态，外环是平动（可以看看文章的几个proposition，大致意思就是先要实现姿态的稳定追踪，然后在姿态差不多ok的基础上再实现平动的稳定追踪）

先来看内环，这要求内环的$M$是有快速调整姿态的效果，因此内环控制律视为$M = (\dots)−k_R e_R − k_\Omega e_\Omega$就是一个前馈加上PD控制而已。你可能会感到困惑，这个$(\dots)$这么复杂的一堆东西为啥是前馈？答案在arxiv上这篇文章的appendix（CDC的是不含appendix的）的(30)和(31)式里，原来代入后确实得到：
$$
J \dot{e}_\Omega = -k_R e_R -k_\Omega e_\Omega
$$
可见，补偿完，确实成为PD控制了

再看外环，$f$是“一堆东西”点乘$Re_3 $，也就是一个东西点乘当前的四旋翼的法线，“这堆东西”其实是在姿态完美追踪的情况下的控制律，仍然是前馈加PD控制（很明显了，前馈是$− mge_3 + m \ddot{x}_d$）。不过现实问题在于，姿态没有完美跟踪，所以就要点乘一下。如果姿态偏的越大，那么夹角越大，那么点乘后值就越小，意思就是“内环没有调好的时候，外环别乱动”，这很符合常识