附录B.4 平面拉格朗日动力学

B.4.2 单连杆的动能和势能

左图为一个质量为$m_i$的，自由的，单连杆。$p_i = (p_i^h; p_i^v) \in \mathbb{R}^2$为link-i在世界坐标系或者惯性坐标系下的坐标。 $\theta_i^{abs} \in \mathbb{S}$描述了其在惯性坐标系下的方向或者说是姿态，称之为绝对方向(absolute orientation)、绝对角度(absolute angle)，约定逆时针转动为正方向。

其运动空间为$\mathcal{Q}_{link} := \mathbb{S} \times \mathbb{R}^2$，连杆中的任何一点$(\bar{l_i^h}; \bar{l_i^v})$都可以映射到惯性坐标系中由$(\theta_i^{abs}; p_i^h; p_i^v)$表示： $ \left[\begin{matrix} \bar{p}_i^h \\ \bar{p}_i^v \end{matrix}\right] = \left[\begin{matrix} p_i^h \\ p_i^v \end{matrix}\right] + \mathbf{R}(\theta_i^{abs}) \left[\begin{matrix} \bar{l}_i^h \\ \bar{l}_i^v \end{matrix}\right] $，$ \mathbf{R}(\theta_i^{abs}) = \left[\begin{matrix} \cos{(\theta_i^{abs})} & -\sin{(\theta_i^{abs})} \\ \sin{(\theta_i^{abs})} & \cos{(\theta_i^{abs})} \end{matrix}\right] $

其速度为：$ \left[\begin{matrix} \dot{\bar{p}}_i^h \\ \dot{\bar{p}}_i^v \end{matrix}\right] = \left[\begin{matrix} \dot{p}_i^h \\ \dot{p}_i^v \end{matrix}\right] + \left[\begin{matrix} 0 & -1 \\ -1 & 0 \end{matrix}\right] \mathbf{R}(\theta_i^{abs})\left[\begin{matrix} \bar{l}_i^h \\ \bar{l}_i^v \end{matrix}\right]\dot{\theta}_i^{abs} $

连杆是自由的，所以其位姿和速度可以在状态空间$T\mathcal{Q}_{link}$的任意一点，也就是说它具有3个自由度。

质心的位置$ \left[\begin{matrix} p_{cm,i}^h \\ p_{cm,i}^v \end{matrix}\right] = \left[\begin{matrix} p_i^h \\ p_i^v \end{matrix}\right] + \mathbf{R}(\theta_i^{abs}) \left[\begin{matrix} l_{cm,i}^h \\ l_{cm,i}^v \end{matrix}\right] $，

质心的速度$ \left[\begin{matrix} \dot{p}_{cm,i}^h \\ \dot{p}_{cm,i}^v \end{matrix}\right] = \left[\begin{matrix} \dot{p}_i^h \\ \dot{p}_i^v \end{matrix}\right] + \left[\begin{matrix} 0 & -1 \\ -1 & 0 \end{matrix}\right] \mathbf{R}(\theta_i^{abs})\left[\begin{matrix} l_{cm,i}^h \\ l_{cm,i}^v \end{matrix}\right]\dot{\theta}_i^{abs} $。质心的位置和速度可以用来确定连杆的势能和动能。

我们假定重力的方向竖直向下，在这种假设的情况下，连杆的势能为： $$ \begin{equation} \label{B.102} V_i = m_i g_0 p_{cm,i}^v \end{equation} $$ 连杆的动能可以按照如下公式计算得出： $$ \begin{equation} \label{B.103} K_i = \frac{1}{2}m_i\left(\left(\dot{p}_{cm,i}^h\right)^2 + \left(\dot{p}_{cm,i}^v\right)^2\right) + \frac{1}{2}J_{cm,i}\left(\dot{\theta}_i^{abs}\right)^2 \end{equation} $$ 其中$J_{cm,i}$为连杆关于质心的转动惯量。实际上式($\ref{B.103}$)可以重写为： $$ \begin{equation} \label{B.106} K_i = \frac{1}{2}\left[\begin{matrix} \dot{\theta}_i^{abs} & \dot{p}_{cm,i}^h & \dot{p}_{cm,i}^v \end{matrix}\right] \left[\begin{matrix} J_{cm,i} & 0 & \\ 0 & m_i & 0 \\ 0 & 0 & m_i \end{matrix}\right] \left[\begin{matrix} \dot{\theta}_i^{abs} \\ \dot{p}_{cm,i}^h \\ \dot{p}_{cm,i}^v \end{matrix}\right] \end{equation} $$

对于$N$各连杆，我们只需把各个连杆的势能(式$\ref{B.102}$)和动能(式$\ref{B.103}$)累加，就可以得到总体的势能和动能。

B.4.3 自由的开运动链

对于左图所示的两种具有$N$个连杆的开运动链。我们对连杆从1到$N$进行标记，记$p_i = (p_i^h, p_i^v) \in \mathbb{R}^2$为连杆$i$的随体坐标系原点在惯性坐标系下的笛卡尔坐标。令$\theta_i^{abs} \in \mathbb{S}$为连杆的绝对方向(在惯性坐标系下的方向)。

对关节从1到$N-1$进行标记，用$a(j), b(j)$表示链接在关节$j$上的两个连杆。记关节$j$在连杆$a(j)$的位置为$(l_{a(j),j}^h; l_{a(j),j}^v)$，相应的其在连杆$b(j)$中的位置为$(l_{b(j),j}^h;l_{b(j),j}^v)$。

对于关节的约束可以建立$2(N-1)$个方程：$\left[\begin{matrix}p_{a(j)}^h \\ p_{a(j)}^v \end{matrix}\right] + \mathbf{R}\left(\theta_{a(j)}^{abs}\right)\left[\begin{matrix}l_{a(j),j}^h \\ l_{a(j),j}^v \end{matrix}\right] - \left[\begin{matrix} p_{b(j)}^h \\ p_{b(j)}^v \end{matrix}\right] - \mathbf{R}\left(\theta_{a(j)}^{abs}\right)\left[\begin{matrix}l_{a(j),j}^h \\ l_{a(j),j}^v \end{matrix}\right] = \left[\begin{matrix} 0 \\ 0 \end{matrix}\right]$，其中$j = 1, \cdots, N-1$。

对于一个开运动链，这些约束方程总是一致的(总是有解)(For an open kinematic chain, the set of equations (B.110) is always consistent (i.e., there always exist solutions) for arbitrary values of the absolute angles.)(按：有解就是有解嘛，不太理解为什么要称为consistent)。更进一步的，对于任意$i_0 \in \{1, \cdots, N\}$，解都可以写为：$\left[\begin{matrix} p_{i}^h \\ p_{i}^v \end{matrix}\right] = \left[\begin{matrix} p_{i_0}^h \\ p_{i_0}^v \end{matrix}\right] + \left[\begin{matrix} \check{\Upsilon}_{i_0,i}^h(\theta_1^{abs}, \cdots, \theta_N^{abs}) \\ \check{\Upsilon}_{i_0,i}^v(\theta_1^{abs}, \cdots, \theta_N^{abs}) \end{matrix}\right]$。其中$i \in \{1, \cdots, N\}, i \neq i_0$，$\check{\Upsilon}_{i_0,i}^h,\check{\Upsilon}_{i_0,i}^v$ are affine in $\cos{\theta_j^{abs}}$ and $\sin{\theta_j^{abs}}$。也就是说这$N+2$个变量$(\theta_1^{abs}; \cdots; \theta_N^{abs}; p_{i_0}^h; p_{i_0}^v)$ 完全满足关节约束方程。

$N$个自由连杆的工作空间(configuration space)是其中单个连杆工作空间的笛卡尔积(Cartesian product)，$\mathcal{Q}_{link}^N := \begin{matrix}\underbrace{\mathcal{Q}_{link} \times \cdots \times \mathcal{Q}_{link}} \\ N个 \end{matrix}$，因而具有$3N$的维度。其中满足关节约束方程的一个子集为： $$ \begin{matrix} \label{B.112} \mathcal{Q}_f := \left\{(\theta_1^{abs}; p_1^h; p_1^v; \cdots; \theta_N^{abs}; p_N^h; p_N^v) \in \mathcal{Q}_{link}^N | (B.110) holds \forall joints \right\} \end{matrix} $$ $\mathcal{Q}_f$是$\mathcal{Q}_{link}^N$的(N+2)为embedded submanifold(嵌入子流形)，更近一步的，$(\theta_1^{abs}; \cdots; \theta_N^{abs}; p_{i_0}^h; p_{i_0}^v)$ $\mathcal{Q}_f$的局部坐标系。$\mathcal{Q}_f$的局部坐标系被称为广义坐标系generalized coordinates，标记为$q_f = (q_1; \cdots; q_{N+2})$。

定义两个连杆$a(i),b(i)$之间的相对角度为关节$j$为$\theta_i^{rel} = \theta_{b(i)}^{abs} - \theta_{a(i)}^{abs}$。那么$q_f = (\theta_1^{rel}; \cdots; \theta_{N-1}^{rel}; \theta_{j_0}^{abs}; p_{i_0}^h; p_{i_0}^v), 1 ≤ j_0 ≤ N-1, 1 ≤ i_0 ≤ N$，也是一个广义坐标系。

动能和使能Kinetic and potential energy:给定一组广义坐标系$q_f = (q_1; \cdots; q_{N+2})$，总有一个关于$q_f$的函数： $$ \begin{equation}\label{B.118} \left[\begin{matrix} \theta_i^{abs} \\ p_i^h \\ p_i^v \end{matrix}\right] = \left[\begin{matrix} \theta_i^{abs}(q_f) \\ p_i^h(q_f) \\ p_i^v(q_f) \end{matrix}\right] \end{equation} $$ 根据链式法则(chain rule)，其速度可以如下计算： $$ \begin{equation}\label{B.119} \left[\begin{matrix} \dot{\theta}_i^{abs}(q_f,\dot{q}_f) \\ \dot{p}_i^h(q_f,\dot{q}_f) \\ \dot{p}_i^v(q_f,\dot{q}_f) \end{matrix}\right] = \left(\frac{\partial}{\partial q_f}\left[\begin{matrix} \theta_i^{abs}(q_f) \\ p_i^h(q_f) \\ p_i^v(q_f) \end{matrix}\right]\right)\dot{q}_f \end{equation} $$ 更进一步的有： $$ \begin{equation}\label{B.120} \frac{\partial}{\partial \dot{q}_i}\left[\begin{matrix} \dot{\theta}_i^{abs}(q_f,\dot{q}_f) \\ \dot{p}_i^h(q_f,\dot{q}_f) \\ \dot{p}_i^v(q_f,\dot{q}_f) \end{matrix}\right] = \frac{\partial}{\partial q_i}\left[\begin{matrix} \theta_i^{abs}(q_f) \\ p_i^h(q_f) \\ p_i^v(q_f) \end{matrix}\right] \end{equation} $$ 将公式($\ref{B.119},\ref{B.120}$)代入公式($\ref{B.102},\ref{B.103}$)后得，单杆的势能： $$ \begin{equation}\label{B.121} V_i(q_f) = m_ig_0p_{cm,i}^v(q_f) \end{equation} $$ 单杆的动能： $$ \begin{equation}\label{B.122} K_i(q_f, \dot{q}_f) = \frac{1}{2}m_i\left(\left(\dot{p}_{cm,i}^h(q_f, \dot{q}_f)\right)^2 + \left(\dot{p}_{cm,i}^v(q_f, \dot{q}_f)\right)^2\right) + \frac{1}{2}J_{cm,i}\left(\dot{\theta}_i^{abs}(q_f, \dot{q}_f)\right)^2 \end{equation} $$ 可以发现$K_i$为$\dot{q}_f$的二次、半正定的函数，即： $$ \begin{equation}\label{B.123} K_i = \frac{1}{2}\dot{q}_f^T\left[m_i\left(\frac{\partial p_{cm,i}}{\partial q_f}\right)^T\left(\frac{\partial p_{cm,i}}{\partial q_f}\right) + J_{cm,i}\left(\frac{\partial \theta_i^{abs}}{\partial q_f}\right)^T\left(\frac{\partial \theta_i^{abs}}{\partial q_f}\right) \right]\dot{q}_f \end{equation} $$ 系统总势能为： $$ \begin{equation}\label{B.124} V_f(q_f) := \sum_{i=1}^{N} V_i(q_f) = m_{tot}g_0p_{cm}^v \end{equation} $$ 而系统的总动能为： $$ \begin{equation}\label{B.125} K_f(q_f, \dot{q}_f) := \sum_{i=1}^{N} K_i(q_f, \dot{q}_f) = \frac{1}{2}\dot{q}_f^TD_f(q_f)\dot{q}_f \end{equation} $$ 其中$D_f(q_f)$是一个$(N+2)\times(N+2)$的正定矩阵，$q_f \in \mathcal{Q}_f$，称为惯性矩阵(mass-inertia matrix)。

B.4.4 固定的开链

现在我们考虑左图所示的一个固定的开链(Pinned Open Kinematic Chains)。将支点在惯性坐标系中的位置记为$(p_0^h; p_0^v)$，与之相连的连杆为$i_0$，支点相对于该连杆的随体坐标系的位置为$(l_{i_0,0}^h; l_{i_0,0}^v)$。

那么有两个约束方程：$\left[\begin{matrix} p_{i_0}^h \\ p_{i_0}^v \end{matrix}\right] + \left[\begin{matrix} \cos{(\theta_{i_0}^{abs})} & -\sin{(\theta_{i_0}^{abs})} \\ \sin{(\theta_{i_0}^{abs})} & \cos{(\theta_{i_0}^{abs})} \end{matrix} \right] \left[\begin{matrix} l_{i_0,0}^h \\ l_{i_0,0}^v \end{matrix} \right] - \left[\begin{matrix} p_0^h \\ p_0^v \end{matrix}\right] = \left[\begin{matrix} 0 \\ 0 \end{matrix} \right]$ 在这个约束下有解空间，它是$\mathcal{Q}_f$的一个$N$为嵌入子流形：$\mathcal{Q}_s := \left\{(\theta_1^{abs}; p_1^h; p_1^v; \cdots; \theta_N^{abs}; p_N^h; p_N^v) \in \mathcal{Q}_f | (B.127) holds \right\}$

因此，其广义坐标系为$(\theta_1^{abs};\cdots;\theta_N^{abs})$或者是$(\theta_1^{rel};\cdots;\theta_{N-1}^{rel};\theta_{j_0}^{abs})$。

动能和使能Kinetic and potential energy:给定一组广义坐标系$q_s = (q_1; \cdots; q_N)$，总有一个关于$q_s$的函数： $$ \begin{equation}\label{B.131} \left[\begin{matrix} \theta_i^{abs} \\ p_i^h \\ p_i^v \end{matrix}\right] = \left[\begin{matrix} \theta_i^{abs}(q_s) \\ p_i^h(q_s) \\ p_i^v(q_s) \end{matrix}\right] \end{equation} $$ 根据链式法则(chain rule)，其速度可以如下计算： $$ \begin{equation}\label{B.132} \left[\begin{matrix} \dot{\theta}_i^{abs}(q_s,\dot{q}_s) \\ \dot{p}_i^h(q_s,\dot{q}_s) \\ \dot{p}_i^v(q_s,\dot{q}_s) \end{matrix}\right] = \left(\frac{\partial}{\partial q_s}\left[\begin{matrix} \theta_i^{abs}(q_s) \\ p_i^h(q_s) \\ p_i^v(q_s) \end{matrix}\right]\right)\dot{q}_s \end{equation} $$ 更进一步的有： $$ \begin{equation}\label{B.133} \frac{\partial}{\partial \dot{q}_i}\left[\begin{matrix} \dot{\theta}_i^{abs}(q_s,\dot{q}_s) \\ \dot{p}_i^h(q_s,\dot{q}_s) \\ \dot{p}_i^v(q_s,\dot{q}_s) \end{matrix}\right] = \frac{\partial}{\partial q_i}\left[\begin{matrix} \theta_i^{abs}(q_s) \\ p_i^h(q_s) \\ p_i^v(q_s) \end{matrix}\right] \end{equation} $$ 将公式($\ref{B.132},\ref{B.133}$)代入公式($\ref{B.102},\ref{B.103}$)后得，单杆的势能： $$ \begin{equation}\label{B.134} V_i(q_s) = m_ig_0p_{cm,i}^v(q_s) \end{equation} $$ 单杆的动能： $$ \begin{equation}\label{B.135} K_i(q_s, \dot{q}_s) = \frac{1}{2}m_i\left(\left(\dot{p}_{cm,i}^h(q_s, \dot{q}_s)\right)^2 + \left(\dot{p}_{cm,i}^v(q_s, \dot{q}_s)\right)^2\right) + \frac{1}{2}J_{cm,i}\left(\dot{\theta}_i^{abs}(q_s, \dot{q}_s)\right)^2 \end{equation} $$ 可以发现$K_i$为$\dot{q}_s$的二次、半正定的函数，即： $$ \begin{equation}\label{B.136} K_i = \frac{1}{2}\dot{q}_s^T\left[m_i\left(\frac{\partial p_{cm,i}}{\partial q_f}\right)^T\left(\frac{\partial p_{cm,i}}{\partial q_s}\right) + J_{cm,i}\left(\frac{\partial \theta_i^{abs}}{\partial q_s}\right)^T\left(\frac{\partial \theta_i^{abs}}{\partial q_s}\right) \right]\dot{q}_s \end{equation} $$ 系统总势能为： $$ \begin{equation}\label{B.137} V_s(q_s) := \sum_{i=1}^{N} V_i(q_s) = m_{tot}g_0p_{cm}^v \end{equation} $$ 而系统的总动能为： $$ \begin{equation}\label{B.138} K_s(q_s, \dot{q}_s) := \sum_{i=1}^{N} K_i(q_s, \dot{q}_s) = \frac{1}{2}\dot{q}_s^TD_s(q_s)\dot{q}_s \end{equation} $$ 其中$D_s(q_s)$是一个$N\times N$的正定矩阵，$q_s \in \mathcal{Q}_s$，称为惯性矩阵(mass-inertia matrix)。

B.4.5 拉格朗日方程

Lagrangian是一个实数函数，$K(q, \dot{q})$为动能函数，$V(q)$为势能函数，有: $$ \begin{equation} \label{B.140} \mathcal{L}(q, \dot{q}) := K(q, \dot{q}) - V(q) \end{equation} $$ 于是有拉格朗日方程(Lagrange's equation)： $$ \begin{equation} \label{B.141} \frac{d}{dt}\frac{\partial \mathcal{L}}{\partial \dot{q}} - \frac{\partial \mathcal{L}}{\partial q} = \Gamma \end{equation} $$ 其中$\Gamma$为广义力。