Geometric Methods
论文阅读 Geometric Impedance Control on SE(3) for Robotic Manipulators
原本的机器人动力学公式为：
$$
\tilde{M}(q) \dot{V}^b + \tilde{C}(q,
\dot{q}) V^b +\tilde{G}(q) = \tilde{T}+\tilde{T}_e
$$
此处$\tilde{T}_e$代表扰动，代表不可控的因素。而$\tilde{T}$代表控制输入，我们最终的目标，就是给出$\tilde{T}$的表达式
下面开始：
误差函数
意义：误差函数的意义是，作为一个标量，用于量化系统当前状态与期望状态之间的差异，将来要用于Lyapunov稳定性分析
在Taeyoung Lee的无人机几何控制的文章中，用了$\psi_R(R,R_d) =\frac{1}{2}\text{tr}(I-R_d^\top R)$作为orientation的误差函数。（题外话：这个东西的意义其实是$1-\cos\theta_\text{error}$，其中$\theta_\text{error}$就是从$R$转到$R_d$的角度）
注意到，上式其实等于$\frac{1}{2}\Vert I- R_d^{-1} R\Vert_F^2$，其中这个$\Vert\Vert_F$代表Frobenius范数，也就是把矩阵铺平成一个向量，然后该向量的$l^2$范数即Frobenius范数
写成Frobenius范数的好处是便于推广，我们直接用齐次阵$g$（本文中用$g$这个符号指代齐次阵）替换掉$R$，然后定义：
$$
\Psi(g,g_d) = \frac{1}{2}\Vert I- g_d^{-1}g \Vert^2
$$
算一通展开后，发现上式即$\Psi(g,g_d) = \text{tr}(I-R_d^\top R) +\frac{1}{2} (p-p_d)^\top (p-p_d)$，这个形式很漂亮，显然是原本orientation误差函数推广到pose误差函数的结果
该误差函数有左不变性质：$\Psi(g_l g,g_l g_d)=\Psi(g,g_d)$。这其中的$g_l$的含义是换了一个世界坐标系（比如本来的世界坐标系选的是附着在这栋房子上的，现在换成另一栋房子），这就是左不变的意义，世界坐标系的变更完全不会影响
位置误差向量
意义：误差向量的意义是，为实际控制提供指导，要用在实际控制律里面
现在，让我们施加一个扰动twist，这个扰动$\hat{\eta}\in \mathfrak{se}(3)$，$\eta = \begin{pmatrix}\eta_1 \\ \eta_2 \end{pmatrix}\in \mathbb{R}^6$（注意，我上一篇遵循的是Lynch书里的记号，这里本应当用$[\eta]$表示，但是遵循本文中，是加$\hat{ \quad}$表示的hat运算）
这个扰动是在当前的位姿$g$下看的（符合实际工程），由于“右乘联体”，所以这个扰动将右乘在$g$上，表示为$g e^{\hat{\eta}\epsilon}$ （这一段也和Taeyoung Lee的一样操作）
因此，对于小微扰参数$\epsilon$求导，得到$\delta g = g\hat{\eta}$此式写为具体形式就是$\begin{pmatrix}
\delta R & \delta p \\ 0 & 0 \\
\end{pmatrix} = \begin{pmatrix}
R \hat{\eta}_2 & R \eta_1 \\ 0 & 0 \\
\end{pmatrix} \in T_g SE(3)$
同样道理，扰动一下之前定义的误差函数$\Psi(g,g_d)$，计算一下就得到：
$$
\delta \Psi(g,g_d) = -\text{tr}(R_d^\top R \hat{\eta}_2) + (p-p_d)^\top R \eta_1 = (R_d ^\top R - R^\top R_d)^\vee \eta_2 + (p-p_d)^\top R \eta_1
$$
我们企图把上式写为$\delta \Psi(g,g_d) = e_g^\top \eta$的形式，所以可以定义位置误差向量：
$$
e_g = \begin{pmatrix}
e_p \\ e_R
\end{pmatrix} = \begin{pmatrix}
R^\top (p-p_d) \\ (R_d ^\top R - R^\top R_d)^\vee
\end{pmatrix}
$$
速度误差向量
意义：误差向量的意义是，为实际控制提供指导，要用在实际控制律里面
先定义$g_{ed} = g^{-1} g_d$。这个东西的作用是，它右乘在$g$上后，能得到$g_d$。因此，其含义就是在本体系下看，$g$到$g_d$需要做的那个齐次变换
根据上一篇中所讲的Adjoint相关内容，可以知道，两个不同坐标系（$g$和$g _ d$）下描述同一个twist，会得到不同的六维向量，但是这两个六维向量就可以通过$\text{Ad} _ {g _ {ed}}$来转换
所以，如果$g _ d$下描述的期望twist向量记作$V _ d^b$的话，那么该向量在$g$下描述就是$\text{Ad} _ {g _ {ed}} V _ d^b$，我们把这个东西记作$V _ d^\ast$
而此时此刻机器人处在$g$状态，其传感器测得的实际为$V^b$，因此合适的速度误差向量应当是$e_V = V^b - V_d^\ast$。我再陈述一遍，这个$e_V$就代表在$g$下看，理想的twist和此前的twist之间差了多少
所以$\dot{g} - \dot{g _ d} (g _ d^{-1}g)= g \hat{e} _ V \in T _ g SE(3) $也是很自然的，其中$(g _ d^{-1}g)$就是一个从$T _ {g _ d} SE(3) \to T _ {g} SE(3) $的映射。$e _ V$可以用来表示$g$处切空间上的误差
稳定性分析概要
我们觉得Lyapunov函数是机械能的形式会比较好，即动能+势能。定义动能项$K = \frac{1}{2} e_V^\top \tilde{M} e_V$以及势能项$P =\text{tr}(K_R (I-R_d^\top R)) +\frac{1}{2} (p-p_d)^\top R_d K_p R_d^\top (p-p_d)$（为啥这个势能长这样呢？是模仿先前的$\Psi$的基础上，加上权重$K_R,K_p$）
此外，这个势能是左不变的
最后，我们定义的Lyapunov函数即$V=K+P$
控制律如何得到
最直观得到的一个控制律就是
$$
\tilde{T} = \tilde{M} \dot{V}_d^\ast + \tilde{C} V_d^\ast +\tilde{G} - K_g e_g -K_d e_V
$$
仔细看，$\tilde{M} \dot{V}_d^\ast + \tilde{C} V_d^\ast +\tilde{G}$这一部分是理想情况下的前馈补偿，而剩余的部分是一个PD控制
然而，这个$K_g e_g$本来是要作为弹性力项的，但遗憾的是它实际没有和$P$这个势能对应起来。我们需要的控制目标是搞一个类似于弹簧阻尼的系统！因此，“势能函数的负梯度是弹性力”这一点我们希望它要成立，但是简单的使用$K_g e_g$并没有做到这一点（因为我们加上了权重$K_R,K_p$，如果不加的话，就是成立的）。所以我们得求一下$P$的梯度……（参见该文章的Appendix A），算出来就是
$$
\delta P = \begin{pmatrix}
f_p^\top & f_R^\top
\end{pmatrix} \begin{pmatrix}
\eta_1 \\ \eta_2
\end{pmatrix}= f_g^\top \eta
$$
其中$f_g$具体表达式就是文章中的（31）式子。这样的$f_g$就是我需要的
为了防止你绕晕，可以读这一段微积分基础回顾
回顾：为啥这样的$f_g$就是梯度呢？因为对标量函数$P$沿着$\eta$方向移动一小步$\Delta x =\epsilon\eta$，如果结果能表示成$\Delta P = f_g \cdot \Delta x= f_g \cdot (\epsilon\eta)$的话，这个$f_g$的含义就是梯度！也就是说$\delta P := \Delta P/\epsilon =f_g^\top \eta$满足这样的$f_g$就是梯度！
结合上述的种种原因，最后的控制律就是：
$$
\tilde{T} = \tilde{M} \dot{V}_d^\ast + \tilde{C} V_d^\ast +\tilde{G} - f_g -K_d e_V
$$
稳定性分析的具体内容
“耗散性”算一下就完了（参见theorem 4处过程）发现确实在$\tilde{T}_e$为零的情况下会耗散掉，且耗散的速率和$e_V$有关，即$\dot{V} = -e_V^\top \tilde{M} e_V $，这和简单弹簧阻尼模型中，机械能耗散功率的表达式完全一样
但是theorem4仅仅证明了这个$\dot{V}$是半负定的（不是负定的），所以又补充了一个theorem5去证明渐进稳定的
section3.3 另一种方法
为什么在3.3节又要来另外一个方法呢？原因是之前3.2节构造的那个Lyapunov函数的导数其实是半负定的，所以在theorem5中修改了一下表达式，才能证明是负定的。这样其实不够简练，所以3.3节给出一个更简单的选择。而且，当$\lambda_g \to 0$时可以回到3.2节的控制律，所以其实是3.2节的推广
