Delta 机器人的动力学建模#

Delta 机器人 是一种三自由度的常用并联型机器人. 由于其特殊设计, 能够保证其顶板始终平行于底板, 只有三个平动自由度而无转动. 这一点很容易证明, 此处不再赘述. 因此我们将按下面的模型讨论

机器人有三个可以独立活动的支部, 编号为 \(1\), \(2\), \(3\), 按右手顺序排列, 彼此相隔 \(\frac{2\pi}{3}\). 我们之后的讨论将更多只限于同一个支部, 在不引起误会的情况下, 将省略下标以求简洁. 此时我们讨论的将是适用于三支的通用关系.

约定粗体 \(\mathbf{L}\) 代表向量, 非粗体的相同字母代表其模长 \(L := \|\mathbf{L}\|\). 在模型中, \(L_i\), \(i = 1,2,3\) 都是等长的, 因此都记为 \(L\); \(g\) 与 \(l\) 同理. 单位坐标轴方向为 \(\hat{\mathbf{i}}\), \(\hat{\mathbf{j}}\), \(\hat{\mathbf{k}}\); \(\hat{\cdot}\) 表示归一化. \(P\) 为关心的末端位置.

正向运动学#

正向运动学解决的问题是如何通过已知的广义坐标 \(\theta_i\), \(i = 1,2,3\) 来得到 \(P\) 的位置. 我们将从底部开始, 逐步地依次求解 \(B\), \(M\) 直到 \(P\) 的位置.

中途向量#

\(\mathbf{g}\) 与 \(B\) 的求解是极为简单的. 记方位角 \(\phi\) 为三支在水平面上的偏角, 有 \(\phi_1 = 0\), \(\phi_2 = \frac{2\pi}{3}\), \(\phi_3 = \frac{4\pi}{3}\). 于是

若尔当标准型#

我们对若尔当标准型的讨论始于两个最基本与最常见的问题:

• 如何将线性空间 \(V\) 分解为线性变换 \(A\) 的不变子空间 \(V_i\) 的直和?
• 如何使线性变换 \(A\) 在 \(V\) 的一组基上具有最简洁的矩阵形式?

对于问题一, 一个合理的附加条件是 \(A\) 在这些不变子空间 \(V_i\) 上具有良好的性质, 这样要研究 \(A\), 只需在每个更小的空间上研究, 并且充分利用那些性质, 最后汇总为 \(V\) 即可. 问题二则是为了方便计算, 我们可以利用这种简洁矩阵的性质, 提供远比普通矩阵乘法等更优的算法.

对于这两个问题, 我们首先想到的应该是线性变化的对角化. 对于可对角化的线性变换, 充要条件之一便是线性空间 \(V\) 可以被分解为特征子空间的直和 \(V = E_{\lambda_1} \oplus E_{\lambda_2} \oplus \cdots \oplus E_{\lambda_r}\); 并且我们知道这一变换在其特征向量组成的基下是对角矩阵, 这样两个问题就都得到解决. 遗憾的是, 并非每一个矩阵都可以对角化. 我们要讨论的便是一种类似对角化的, 可以对每个变换进行的分解.

可交换的线性变换#

这篇文章讨论可交换的线性变换, 即对于线性变换 \(A\), \(B\), 有 \(AB = BA\).

引理#

对于多项式 \(f\), \(f(A)\) 与 \(B\) 可交换的充要条件是 \(A\), \(B\) 可交换.

充要条件#

复数域上线性变换 \(A\), \(B\) 可交换的充要条件是:

\(A\) 的每个广义特征空间 \(E_{\lambda}\) 都是 \(B\) 的不变子空间.

其中广义特征空间 \(E_{\lambda} = \{\mathbf{v} ~ | ~ (A - \lambda I)^m \; \mathbf{v} = \mathbf{0}, m \in \mathbb{N}_+\}\). 线性空间 \(V\) 总是可以分解为其上某一线性变换的广义特征空间的直和.

证明:
充分性: 任取 \(A\) 在重数 \(m\) 的广义特征空间 \(E_{\lambda}\), \(\forall \mathbf{v} \in E_{\lambda}; B \mathbf{v} \in E_{\lambda}\). 于是 \(B (A - \lambda I)^m \mathbf{v} = B \; \mathbf{0} = \mathbf{0}\), \((A - \lambda I)^m B \mathbf{v} = (A - \lambda I)^m (B \mathbf{v}) = \mathbf{0}\). 注意到 \((A - \lambda I)^m\) 是关于 \(A\) 的多项式, 由此可得 \(E_{\lambda}\) 上 \(A\) 与 \(B\) 可交换.

密度流体的动态模拟#

我们将不可压缩流体的模拟分为两类, 一类聚焦于流体本身, 另一类则聚焦依托于流场的标量场. 前者称为体积流体, 如水与空气二相的模拟, 我们更关心流体本身的运动; 后者称为密度流体, 如充斥于密闭容器中的浊液的模拟, 我们关注的是溶质的物质的量密度这一标量. 需要注意的是这两个词仅为本篇文章的约定, 而非正式的学术用语.

本文讲述了不可压缩密度流体的运动学模拟. 纵览符合这一条件的流场, 如附加温度场, 容质的密度场等, 其标量场都耦合于流体的速度场. 这是指标量会被流体输运, 例如温度的对流; 标量场同时可以反过来影响速度场, 例如渗透压导致的流体运动, 但我们一般不认为它会破坏流体的不可压缩性, 否则这将导致被求解方程的形式变得极端复杂.

写出不可压缩流体的纳维 - 斯托克斯方程:

$$\nabla \cdot \mathbf{u} = 0$$

$$\frac{\mathrm{D} \mathbf{u}}{\mathrm{D} t} = -\nabla \frac{p}{\rho} + \nu \nabla^2 \mathbf{u} + \mathbf{a}$$

前者是质量守恒定律对应的连续性方程在不可压缩, 即 \(\rho \equiv \mathrm{const}\) 时的推论; 后者是牛顿第二定律. 其中 \(\nu\) 是运动粘度, \(\nu := \mu/\rho\), \(\mu\) 是动力粘度. 流体力学中, 我们一般用 \(\mathbf{u}\) 代表速度.

后式中出现的随质导数, 或称物质导数 \(\frac{\mathrm{D}}{\mathrm{D} t} := \frac{\partial}{\partial t} + (\mathbf{u} \cdot \nabla)\). 这一式中前者表示欧拉导数; 后者表示拉格朗日导数, 即由于流体的运动带来的变化, 这个效应称为移流. 将其拆开可以写为

Mahony 非线性互补滤波器#

Mahony 互补滤波算法是一种成熟的姿态估计算法, 旨在融合多种传感器的数据来估计最优姿态. 本篇文章将介绍这一滤波器的数学推导与应用.

在第一章中, 我们先约定文章中使用的符号, 并对不同传感器的数据进行建模; 第二章, 从刚体的运动学入手, 使用李群与李代数作为分析工具; 第三章介绍优化姿态的方法, 并引出两个滤波器: 直接互补滤波器 (Direct Complementary Filter) 与被动互补滤波器 (Passive Complementary Filter); 第四章在被动互补滤波器的基础上讨论更方便的向量版本, 称为显式互补滤波器 (Explicit Complementary Filter); 在第五章中, 我们进一步讨论显式互补滤波器的四元数版本. 本文将不涉及系统的 Lyapunov 稳定性分析.

本文主要编译自 Mahony 等人发表在 IEEE Transactions on Automatic Control 上的论文¹.

符号约定与传感器数据建模#

文章中 \([\cdot]_\times\) 表示向量的外积矩阵, 它是一个反对称矩阵. 使用 \(\dot{R} := \partial R := \frac{\mathrm{d} R}{\mathrm{d} t}\) 表示求导. 线性函数 \(\mathrm{vex}(\cdot)\) 表示将李代数, 即反对称矩阵转为向量; 类似地, 我们有 \(\mathrm{wedge}(\cdot)\) 将向量转为李代数. 另外, 约定线性函数 \(\mathbb{P}_a(A) := \frac{1}{2} (A - A^T)\) 将矩阵映射为一个反对称矩阵.

三维空间中刚体运动的描述#

刚体运动由旋转与平移组成, 与旋转类似, 可以用矩阵来表示:

$$ T = \begin{pmatrix} R & \mathbf{\rho} \\ & 1 \end{pmatrix} $$

其中 \(R\) 是旋转矩阵, \(\mathbf{v}\) 是表征平移的向量. 刚体运动矩阵作用于三维向量的齐次坐标上.

同样与旋转类似, 所有刚体运动矩阵构成一个群, 称为 特殊欧氏群 \(SE(3)\).

\(SE(3)\) 也是一个 李群, 它的 李代数 \(\mathfrak{se} (3)\) 中的元素为如下形式:

$$\begin{pmatrix} \theta [\hat{\mathbf{\omega}}]_\times & \mathbf{v} \\ & 0 \end{pmatrix}$$

这里 \([\hat{\mathbf{\omega}}]_\times\) 表示单位向量的外积矩阵.

本篇文章的工作是推导 \(SE(3)\), \(\mathfrak{se}(3)\) 与 \(\mathbb{R}^6\) 中元素的互相转换. \(\mathbb{R}^6\) 中的元素也被称为 运动旋量.

引理#

$$\boxed{ \begin{pmatrix} \theta [\hat{\mathbf{\omega}}]_\times & \mathbf{v} \\ & 0 \end{pmatrix}^n = \begin{pmatrix} \theta^n [\hat{\mathbf{\omega}}]_\times^n & \theta^{n-1} [\hat{\mathbf{\omega}}]_\times^{n-1} \mathbf{v} \\ & 0 \end{pmatrix} }$$

直接计算便可得到.

三维空间中旋转的描述#

三维空间中所有正交且行列式为 \(1\) 的矩阵组成一个群, 称为 特殊正交群 \(SO(3)\), 其中的每个元素都代表一种三维空间中的旋转.

另外, \(SO(3)\) 也是一个 李群, 它的 李代数 \(\mathfrak{so}(3)\) 中的元素是轴角向量的外积矩阵.

本篇文章的工作是推导 \(SO(3)\), \(\mathfrak{so}(3)\) 与 \(\mathbb{R}^3\) 中元素的互相转换.

引理#

$$\boxed{ [\mathbf{\omega}]_\times^2 = \mathbf{\omega} \mathbf{\omega}^T - \|\mathbf{\omega}\|^2 I }$$$$\begin{align*} ([\mathbf{\omega}]_\times^2)_{ij} &= \varepsilon_{imk} \omega_m ~ \varepsilon_{knj} \omega_n \\ &= \varepsilon_{kim} \varepsilon_{knj} ~ \omega_m \omega_n \\ &= (\delta_{in} \delta_{mj} - \delta_{ij} \delta_{mn}) \omega_m \omega_n \\ &= \omega_i \omega_j - \delta_{ij} \omega_m \omega_m \\ &= (\mathbf{\omega} \mathbf{\omega}^T - \|\mathbf{\omega}\|^2 I)_{ij} \end{align*}$$

---

$$\boxed{ [\mathbf{\omega}]_\times^3 = - \|\mathbf{\omega}\|^2 [\mathbf{\omega}]_\times }$$$$\begin{align*} [\mathbf{\omega}]_\times^3 &= (\mathbf{\omega} \mathbf{\omega}^T - \|\mathbf{\omega}\|^2 I) [\mathbf{\omega}]_\times \\ &= \mathbf{\omega} \mathbf{\omega}^T [\mathbf{\omega}]_\times - \|\mathbf{\omega}\|^2 [\mathbf{\omega}]_\times \\ &= \mathbf{\omega} ([\mathbf{\omega}]_\times \mathbf{\omega})^T - \|\mathbf{\omega}\|^2 [\mathbf{\omega}]_\times \\ &= \mathbf{\omega} (\mathbf{\omega} \times \mathbf{\omega})^T - \|\mathbf{\omega}\|^2 [\mathbf{\omega}]_\times \\ &= - \|\mathbf{\omega}\|^2 [\mathbf{\omega}]_\times \end{align*}$$

---

$$\boxed{ \mathrm{rank} [\mathbf{\omega}]_\times = 2 }$$

其中 \(\mathbf{\omega} \in \mathbb{R}^3 \backslash \{0\}\).

使用泰勒展开的运动预测#

在 \(n\) 维空间运动的物体, 其运动方程总可以写成 \(n\) 个关于时间 \(t\) 的参数方程.

进一步地, 将关于 \(t\) 的参数方程泰勒展开为关于 \(t\) 的多项式函数, 可以显式得到其表达式, 并且可以计算出超过当前时刻, 也就是未来时刻的位置. 当超出当前时刻的时间不长, 运动轨迹是平滑的且分辨率足够高时, 可以认为此级数收敛于实际的位置.

泰勒展开所需的各阶导数可以通过数值计算获得.

参数获取#

假如我们已经获得位置关于时间的一系列数据 \(\{r_n\}\), 其中 \(r_n\) 是当前时刻的数据. 它也可以被记作 \(r_n^{(0)}\). 对应的时刻记为 \(t_n\), 或 \(t_n^{(0)}\).

根据函数导数的公式, 自然地我们想到当前时刻位置的一阶导数为 \(r_n^{(1)} = \frac{r_n - r_{n-1}}{\Delta t_n}\), 其中 \(\Delta t_n := t_n - t_{n-1}\), 以及过去时刻的导数同理.

于是, 我们获得了 \(n-1\) 个一阶导数组成的序列 \(\{r_n^{(1)}\}\). 进一步可以计算二阶乃至 \(n-1\) 阶导数:

$$r_n^{(m)} = \frac{r_n^{(m-1)} - r_{n-1}^{(m-1)}}{\Delta t_n^{(m-1)}}$$$$\Delta t_n^{(m)} = \frac{\Delta t_n^{(m-1)} + \Delta t_{n-1}^{(m-1)}}{2}$$

另外 \(\Delta t_n^{(0)} := \Delta t_n\).

抛体运动弹道解算#

模型#

本文考虑使用 线性阻力 模型. 事实上, 在空气中运动的阻力应该由 二次阻力 描述, 关于二者的对比将在 后文 给出.

之所以使用线性阻力, 是因为如此一来物体的运动微分方程

$$m \dot{\mathbf{v}} = -k \mathbf{v} - m g \hat{\mathbf{j}}$$

有封闭解. 进一步令 \(\mu := \frac{k}{m}\), 上式化为

$$\dot{\mathbf{v}} = -\mu \mathbf{v} - g \hat{\mathbf{j}}$$

容易解得

$$\mathbf{v} = \mathrm{e}^{-\mu t} \mathbf{v}_0 - \mu^{-1} (1-\mathrm{e}^{-\mu t}) g \hat{\mathbf{j}}$$

积分得

$$ \mathbf{r} = \mathbf{r}_0 - \mu^{-1} (1-\mathrm{e}^{-\mu t}) \mathbf{v}_0 - \mu^{-2} (\mu t + \mathrm{e}^{-\mu t} - 1) g \, \hat{\mathbf{j}} $$

这就是线性阻力情况下的运动方程. 它也可以很轻松地分解为两个方向来运算.

与二次阻力的对比#

当速度较大 (\(\sim 20 ~ \mathrm{m/s}\)) 时, 弹丸在空气中运动的 雷诺数 很大, 并不适用于线性阻力. 但是在上升过程中, 速度逐渐下降, 雷诺数也逐渐变小, 于是在上升阶段用线性阻力模型的误差也不是很大.