机器人建模 ¶

绪论 ¶

常见机械臂几何结构 ¶

空间描述与变换 ¶

坐标系与向量 ¶

笛卡尔直角坐标系

• 交于原点的三条不共面的数轴（常称 x 轴、y 轴和 z 轴）构成空间的仿射坐标系
• 三条数轴（主轴）上度量单位相等的仿射坐标系称为空间笛卡尔坐标系
• 下面所有坐标系均采用直角右手坐标系

向量的表示

向量 \(r_{O_AD}\)，将它分别向 \(\hat{X}_A, \hat{Y}_A, \hat{Z}_A\) 作投影，得到 3 个向量 \(d_x \hat{X}_A, d_y \hat{Y}_A, d_z \hat{Z}_A\)

\(r_{O,D} = d_x \hat{X}_A + d_y \hat{Y}_A + d_z \hat{Z}_A = \left( \hat{X}_A \quad \hat{Y}_A \quad \hat{Z}_A \right) \begin{pmatrix} d_x \\ d_y \\ d_z \end{pmatrix}\)

简洁表达 \(^A D = \begin{pmatrix} d_x \\ d_y \\ d_z \end{pmatrix}\)

​

点和刚体的描述 ¶

点

刚体

在 \(\{A\}\) 中表示出 \(\{B\}\) 的姿态：

坐标系变换 ¶

对于齐次变换矩阵，右乘是先平移后旋转，左乘是先旋转后平移。相对于基础坐标系的旋转，可能会产生平移

机器人运动学 ¶

• 使两个刚体直接接触而又能产生一定相对运动的联接称为运动副 ，机器人的运动副也称关节，连杆即指由关节所联的刚体
• 本课程中的关节仅限转动副和移动副
• 串联机构：多个连杆通过关节以串联形式连接成首尾不封闭的机械结构

为了确定末端执行器在 3 维空间的位置和姿态，串联机器人至少需要 6 个关节

改进 D-H 参数 ¶

确定坐标系的方法

• 第 1 步：确定 \(Z_i\) 轴。基本原则是：\(Z_i\) 轴沿关节 \(i\) 的轴向。
• 第 2 步：确定原点 \(O_i\)。基本原则是：\(O_i\) 在过 \(Z_i\) 和 \(Z_{i+1}\) 轴的公法线上。
• 第 3 步：确定 \(X_i\) 轴。基本原则是：\(X\) 轴沿过 \(Z_i\) 和 \(Z_{i+1}\) 轴的公法线方向，从 \(Z_i\) 指向 \(Z_{i+1}\)。
• 第 4 步：确定 \(Y_i\) 轴。基本原则是：\(Y_i = Z_i \times X_i\)，使坐标系为右手坐标系。

DH 参数的定义

1. 杆件长度 \(a_i\)，定义为从 \(Z_{i-1}\) 到 \(Z_i\) 的距离，沿 \(X_{i-1}\) 轴指向为正。
2. 杆件扭角 \(\alpha_i\)，定义为从 \(Z_{i-1}\) 到 \(Z_i\) 的转角。绕 \(X_{i-1}\) 轴正向转动为正。
3. 关节距离 \(d_i\)，定义为从 \(X_{i-1}\) 到 \(X_i\) 的距离，沿 \(Z_i\) 轴指向为正。
4. 关节转角 \(\theta_i\) 定义为从 \(X_{i-1}\) 到 \(X_i\) 的转角，绕 \(Z_i\) 轴正向转动为正。

正运动学 ¶

相邻连杆的坐标系变换

{i-1} 经四步变换成为 {i}：

• 沿联体 x 轴平移 \(a_{i-1}\)
• 沿联体 x 轴旋转 \(\alpha_{i-1}\)
• 沿联体 z 轴平移 \(d_i\)
• 沿联体 z 轴旋转 \(\theta_i\)

经过矩阵乘法后，得到的结果为：

逆运动学 ¶

解析解 ¶

代数解法 ¶

首先根据正运动学推导出齐次变换矩阵，然后在根据齐次变换矩阵中各元素之间的关系求解出关节空间参数 ( 俗称大眼瞪小眼法 )

几何解法 ¶

也是一样，观察

数值解 ¶

牛顿 - 拉夫逊迭代

给定 N 自由度机器人期望的齐次变换矩阵 \(T^d\)，求关节变量 \(\Phi = [\phi_1 \quad \phi_2 \quad \cdots \quad \phi_N]^T\) 使得：

记末端执行器的位姿为 \(X(\Phi)\)，期望位姿为 \(X^d\)，则问题转化为求关节向量 \(\Phi\) 满足 \(X(\Phi) = X^d\)。利用牛顿 - 拉夫逊（Newton-Raphson）法可以迭代求解上述方程。

记期望的关节变量为 \(\Phi^d\)，即 \(X(\Phi^d) = X^d\)，牛顿 - 拉夫逊法是从一个猜测的初始关节变量 \(\Phi^0\) 开始，迭代计算 \(\Phi^k\)，最终逼近 \(\Phi^d\)。过程中需要利用末端位姿关于关节变量的微分，这正是分析雅可比矩阵。

记 \(\delta \Phi^k = \Phi^d - \Phi^k\)，\(\delta X(\Phi^k) = X(\Phi^d) - X(\Phi^k)\)，则由一阶泰勒展开近似得到：

由此得到迭代计算式：

微分运动学与静力学 ¶

微分运动学 ¶

定义 ¶

符号表示 ¶

矩阵定义 ¶

其中 S 为反对称矩阵，具有以下性质：

三维向量与三维反对称矩阵的关系

针对三维向量 \(P = \begin{pmatrix} p_x \\ p_y \\ p_z \end{pmatrix}\)，记由 \(P\) 生成的三维反对称矩阵为 \(P^\wedge\)，则有：

三维反对称矩阵与三维向量是一一对应的，记 \(P^\wedge\) 对应的三维向量为 \(\left( P^\wedge \right)^\vee = P\)，则有：

线速度向量 ¶

若 \( {}^B\mathbf{Q} \) 是描述某个点的位置矢量，该点关于 {B} 的速度是 \( {}^B\mathbf{V}_Q \)。

速度矢量 \( ^A({}^B\mathbf{V}_Q ) \)

当两个上标相同时，无需给出外层上标，即：

角速度向量 ¶

刚体的定点转动：刚体绕体内或其外延部分的一固定点旋转

由理论力学知：刚体（其联体坐标系为 \(\{B\}\)）在参考坐标系 \(\{A\}\) 中的任何运动都可以分解为：

• 点 \({}^A O_B\) 的运动
• 刚体绕 \({}^A O_B\) 的定点转动

由理论力学知：

• 在任一瞬间，\(\{B\}\) 在 \(\{A\}\) 中的定点转动可以看作是绕瞬时转动轴（简称瞬轴，瞬轴上的每个点在该瞬时相对于 \(\{A\}\) 的速度为零）的转动。
• 瞬轴的位置可随时间变化，但原点始终在瞬轴上。

在 \(\{A\}\) 中描述 \(\{B\}\) 的定点转动可用角速度向量 \({}^A\Omega_B\) 表示：

• \({}^A\Omega_B\) 的方向是瞬轴在 \(\{A\}\) 中的方向；
• \({}^A\Omega_B\) 的大小表示在 \(\{A\}\) 中 \(\{B\}\) 绕瞬轴的旋转速度。

线速度变化 ¶

纯平移的线速度变化

一般运动的线速度变化

\(Q\) 点对 \({A}\) 的线速度为坐标系 \({B}\) 原点的线速度、\(Q\) 点在坐标系 \({B}\) 中的线速度和坐标系 \({B}\) 针对坐标系 \({A}\) 旋转形成的 \(Q\) 点切向线速度三者的向量合成

角速度变化 ¶

在参考坐标系 \(\{A\}\) 中，坐标系 \(\{C\}\) 的角速度 \({}^A\Omega_C\) 可以表示为：

在同一坐标系中，角速度可以相加

速度传递 ¶

转动型关节

角速度：连杆 i+1 针对世界坐标系角速度在 {i+1} 坐标系的表示

其中，\(\hat{Z}_{i+1}\) 是轴 \(i+1\) 在 \({i+1}\) 中的表示；\({\theta}_{i+1}\) 是转动型关节 \(i+1\) 的关节转速。

线速度：连杆 i+1 针对世界坐标系线速度在 {i+1} 坐标系的表示

连杆的长度隐含在了 \(^iP_{i+1}\) 叉乘项当中

平动型

角速度：没有关节转动对下一个关节角速度的影响

线速度：要加一个在轴线上的速度

向外迭代法

知道了这样的变换方法，就可以从连杆 0，变换到连杆 N，一个个地计算速度和角速度

雅可比矩阵 ¶

雅可比矩阵用于描述函数的输入变量和输出变量之间的线性关系。对于函数 \(F(X)\)，其雅可比矩阵 \(J(X)\) 定义为：

在动态系统中，输出变量 \(Y\) 的时间导数 \(\dot{Y}\) 可以表示为：

几何雅可比矩阵 ¶

几何雅可比矩阵描述了操作臂的关节速度 \(\dot{\theta}\) 与末端速度（包括线速度和角速度）\(v = \begin{pmatrix} v \\ \omega \end{pmatrix}\) 之间的映射关系矩阵 \(J(\theta)\)。

前述向外迭代法计算机械臂末端速度的算法本质上是计算操作臂几何雅 可比矩阵的方法之一。

采用向量积法直接求出末端线速度和角速度，可以构造几何雅可比矩阵

假设其他关节固定不动，只有第 \(i\) 个关节运动，则由此运动产生的连杆 \(N\) 的线速度和角速度如下：

若第 \(i\) 个关节为移动关节 - 线速度 \(v_N^{(i)} = \dot{d}_i \hat{Z}_i\) - 角速度 \(\omega_N^{(i)} = 0\)

若第 \(i\) 个关节为转动关节 - 线速度 \(v_N^{(i)} = \dot{\theta}_i \hat{Z}_i \times (O_N - O_i)\) - 角速度 \(\omega_N^{(i)} = \dot{\theta}_i \hat{Z}_i\)

末端实际线速度和角速度就是各关节造成的线速度和角速度的总和：

以机械臂每个关节均为旋转关节为例构造雅可比矩阵，定义笛卡尔速度向量 \(v_N = \begin{pmatrix} v_N \\ \omega_N \end{pmatrix} \in \mathbb{R}^6\) 和关节空间角速度向量 \(\dot{\theta} = \begin{pmatrix} \dot{\theta}_1 \\ \dot{\theta}_2 \\ \vdots \\ \dot{\theta}_N \end{pmatrix} \in \mathbb{R}^N\)，则有：

其中，\(J(\theta) \in \mathbb{R}^{6 \times N}\) 即为雅可比矩阵。

向量积构造法是计算几何雅可比矩阵的方法之一。对于任意已知的操作臂位形，关节速度和操作臂末端速度的关系是线性的，然而这种线性关系仅仅是瞬时的，因为在下一刻，雅可比矩阵就会有微小的变化。雅可比矩阵是时变的。

参考坐标系变换下的雅可比矩阵

若关心 \(\{i\}\) 中的笛卡尔速度向量，则有：

可记变换后的雅可比为：

即：

逆微分运动学 ¶

一些定义 ¶

若已知末端执行器笛卡尔空间速度 \(v_N\)（包括线速度和角速度），产生 \(v_N\) 的各关节角速度如下计算：

由于对于冗余机械臂和欠驱动机械臂，雅可比矩阵不是方阵，需要考虑雅可比矩阵的伪逆（广义逆）。

若矩阵 \(A\) 的维度为 \(m \times n (m \neq n)\)，且 \(A\) 为满秩，则 \(A\) 的伪逆（广义逆） \(A^+\) 为：

1. 当 \(m > n\) 时，\(A^+\) 为左逆矩阵，\(A^+ = A_{\text{left}}^{-1} = (A^T A)^{-1} A^T\)，且 \(A^T A\) 满秩。
2. 当 \(m < n\) 时，\(A^+\) 为右逆矩阵，\(A^+ = A_{\text{right}}^{-1} = A^T (A A^T)^{-1}\)，且 \(A A^T\) 满秩。

逆微分运动 ¶

若 \(A\) 为 \(m \times n\) 维矩阵，且 \(A\) 为满秩，则线性方程组 \(Ax = b\) 的解：

1. 当 \(m > n\) 时

方程组是过定的，通常方程组无解。此时，使得 \(\|Ax - b\|^2\) 最小的 \(x\) 为方程的最小二乘解，由左伪逆计算：

例如：

1. 当 \(m < n\) 时

方程组是欠定的，通常方程组可能存在无数个解。此时，所有解中使得 \(\|x\|\) 范数最小的 \(x\) 为方程的最小范数解，由右伪逆计算：

例如：

零空间（Null Space）

若 \(A\) 为 \(m \times n\) 维矩阵，则 \(A\) 的零空间为线性方程组 \(Ax = 0\) 的所有解集合，记为 \(N(A) = \{x \in \mathbb{R}^n : Ax = 0\}\)

1. 当 \(m \geq n\) 时，若 \(A\) 为列满秩，\(A\) 的零空间只有零向量

1. 当 \(m < n\) 时，若 \(A\) 为行满秩，\(A\) 的零空间中的向量为

其中，\(x\) 为任意 \(n\) 维向量

例如：

逆运动学解的情况 ¶

在机械臂关节角处于 \(\theta\) 时，雅可比矩阵为 \(J(\theta)\)，由末端执行器的笛卡尔空间速度 \(v_N\) 求关节角速度的公式如下：

1. 无冗余

机械臂操作空间维度等于机械臂关节数，若雅可比矩阵满秩，则

1. 冗余

机械臂操作空间维度小于机械臂关节数，对于末端执行器的某一笛卡尔空间速度，通常会有无穷组对应的关节速度，若雅可比矩阵是行满秩的，其中满足关节速度范数最小的一个特解（最小范数解），用右伪逆计算

通解为

其中 \(\phi_f\) 遍历所有的关节速度向量

1. 欠驱动

机械臂操作空间维度大于机械臂关节数，对于末端执行器的某一笛卡尔空间速度，可能没有对应的关节速度，这时，若雅可比矩阵是列满秩的，只能得到误差范数最小的关节速度（最小二乘解），用左伪逆计算

奇异性 ¶

大多数 \(6 \times 6\) 的雅可比矩阵 \(J\) 都有使得其不可逆的 \(\theta\) 值，这些 \(\theta\) 值所对应的位姿称为机构的奇异位形或简称奇异状态。

所有的操作臂在工作空间的边界都存在奇异位形，并且大多数操作臂在它们的工作空间也有奇异位形。

对于空间机械臂，总有 \(\text{rank}(J) \leq \min(6, n)\) 对于平面机械臂，总有 \(\text{rank}(J) \leq \min(2, n)\) 其中n表示机械臂关节数

奇异点的判断条件

对于一般机械臂，奇异位形为令雅可比矩阵 \(J\) 不满秩的 \(\theta\) 值所构成的位形，此时 \(\text{rank}(J(\theta)) < \min(m, n)\)。

1. 无冗余（\(m=n\)）：在此 \(\theta\) 时 \(J\) 不可逆，即 \(\text{det}(J(\theta))=0\)
2. 冗余（\(m<n\)）：在此 \(\theta\) 时 \(J\) 不行满秩，即 \(\text{rank}(J(\theta))<m\)
3. 欠驱动（\(m>n\)）：在此 \(\theta\) 时 \(J\) 不列满秩，即 \(\text{rank}(J(\theta))<n\)

奇异位形大致分为两类：

1. 边界奇异性 工作空间边界的奇异位形。出现在操作臂完全展开或者收回使得末端执行器处于或非常接近空间边界的情况。

2. 内点奇异性 工作空间内部的奇异位形。出现在远离工作空间的边界，通常是由于两个或两个以上的关节轴线共线引起的。

当操作臂处于奇异位形时，操作臂的末端在笛卡尔空间中会失去一个或多个自由度，即此时无论选择多大的关节速度，操作臂的末端在笛卡尔空间的某个方向上（或某个子空间中）都不能运动。

可操作度 ¶

可操作度是衡量机器人位形与奇异位形距离的一种度量方式

若机械臂处于某位形时关节向量为 \(\phi\)，关节速度取为单位速度向量 \(\dot{\phi}_e\)，满足 \(\dot{\phi}_e^T \dot{\phi}_e = 1\) 此时，机器人末端速度记为 \(v_e\)，则满足

（1）机器人无冗余：

（2）机器人冗余：

当机器人处于某位形时，限制关节速度为单位速度向量，机器人末端速度所构成的空间称作该位形的可操作椭球体。

假设机器人有 N 个关节，末端速度空间的维数为 m，要求 \(N \geq m\) ，则 \(m \times N\) 维雅可比矩阵 \(J\) 的奇异值分解为

其中 \(\Sigma\) 是 \(m \times N\) 维矩阵， \(\Sigma = \text{diag}(\sigma_1, \sigma_2, \cdots, \sigma_m)\)

带入上面的方程得到

其中，\(\Sigma^{-2} = \text{diag}\left(\sigma_1^{-2}, \sigma_2^{-2}, \cdots, \sigma_m^{-2}\right)\)，记 \(a = U^\top v_e\) 则

上述是一个标准的椭球体方程，表明机器人此位形的可操作椭球体的轴向量 \(\sigma_i u_i\) 给出。

可操作椭球体的体积与雅可比矩阵 \(J\) 的奇异值的连乘 \(\sigma_1 \sigma_2 \cdots \sigma_m\) 成比例。 定义机器人处于位形 \(\Phi\) 时的可操作度为 \(\kappa(\Phi)\)

（1）在奇异位形，\(JJ^\top\) 不是满秩的，因此可操作度 \(\kappa = 0\)

（2）在非奇异位形，可操作度 \(\kappa > 0\)，而且 \(\kappa\) 越大，机器人改变末端位姿的可操作性越好

分析雅可比矩阵 ¶

分析雅可比矩阵：通过操作臂末端的最小表示的运动学方程对关节变量的微分计算得到的雅可比矩阵。

令 \(X = \begin{pmatrix} P(\theta) \\ \psi(\theta) \end{pmatrix}\) 表示末端执行器的位姿，其中 \(P(\theta)\) 为基座坐标系原点到末端执行器坐标系原点的一般向量，\(\psi(\theta)\) 为末端执行器坐标系相对于基座坐标系姿态的最小表示（例如固定角表示或欧拉角表示）。

分析雅可比满足以下形式：

刚体角速度与欧拉角速率的关系 ¶

由 \(\dot{R} R R^T = S\) 得到：

得到：

以 Z-Y-Z 欧拉角表示刚体在基坐标系中的姿态为例，记 \(\Psi = (\alpha \beta \gamma)^T\)，则旋转矩阵 \(R\) 可以表示为：

即：

欧拉角速率为 \(\dot{\Psi} = (\dot{\alpha} \dot{\beta} \dot{\gamma})^T\)

则：

同样有：

因此：

其中 \(\Psi = (\alpha ~~ \beta ~~ \gamma)^T\) 上述角速度与欧拉角速率的关系公式也称为“欧拉运动学方程”

静力 ¶

操作臂在静态平衡（静止或匀速直线运动）状态下，考虑力和力矩如何从一个连杆向下一个连杆传递

作用在操作臂上的静力 ¶

力 \(f\) 对原点 \(O\) 的矩在 \(i\) 中可表示为 \(^iP \times ^if \in \mathbb{R}^3\)

矩的大小 \(|^iP||^if| \sin \theta = h||^if|\)，\(\theta\) 是 \(^iP\) 与 \(^if\) 的夹角，\(h\) 是力臂

垂直于 \(^iP\) 和 \(^if\) 所在平面的矩方向意味着“矩使刚体产生绕 \(^iP \times ^if\) 旋转的趋势”

力偶

• 两个大小相等、方向相反且不共线的平行力组成的力系
• 力偶的作用只改变刚体的转动状态，其转动效应可用力偶矩来度量

力偶 \((\vec{f}, -\vec{f})\) 对点 \(O\) 的矩：

对刚体上的任何点，力偶矩 \(\vec{BA} \times \vec{f}\) 不变

力偶矩向量 \(\vec{BA} \times \vec{f}\) 可在刚体上任意转移

对于静止的机械臂来说，存在力与力矩平衡

对于连杆 i 收到的力只有两个，将力相加并令其等于零：

对于连杆 i 收到的力矩有三个，分别是 \(^i n_i\)、\(^i n_{i+1}\) 以及 \(^i f_{i+1}\) 作用产生的力矩 ( 由于力 \({^i f_{i}}\) 经过原点，因此对连杆 i 的力矩为 0，因此只有三个极距 )，将绕坐标系 \(i\) 原点的力矩相加：

由此得到：

静力传递 ¶

为了平衡施加在连杆上的力和力矩，需要在关节提供多大的力矩（旋转关节）或力（移动关节）

旋转关节：

• \(f_i\) 不是约束力，它阻止连杆 \(i\) 作直线运动，\(n_i\) 阻止连杆 \(i\) 作旋转运动。在 \(\{i\}\) 中对 \(n_i\) 进行正交分解，可得到 1 个沿 \(\hat{Z}_i\) 的力矩向量和 1 个垂直于 \(\hat{Z}_i\) 的力矩向量。
• 垂直于 \(\hat{Z}_i\) 的力矩向量是约束力矩；沿 \(\hat{Z}_i\) 的力矩向量是主动力矩，主动力矩需由关节 \(i\) 的旋转驱动器提供。
• 主动力矩可表示为 \(\tau_i \hat{Z}_i\)，其中 \(\tau_i = |n_i| \cos \theta = |n_i| \hat{Z}_i| \cos \theta = n_i^T \hat{Z}_i\)。

移动关节：

• \(n_i\) 是约束力矩。在 \(\{i\}\) 中对 \(f_i\) 进行正交分解，得到 1 个主动力和 1 个约束力，需由关节 \(i\) 的直线驱动器提供的主动力表示为 \(\tau_i \hat{Z}_i\)，其中 \(\tau_i = f_i^T \hat{Z}_i\)。

轨迹规划 ¶

关节空间轨迹规划 ¶

以关节角的函数来描述轨迹的轨迹生成方法

三项式规划

为获得一条确定的光滑运动曲线，至少需要对 \(\phi(t)\) 施加四个约束条件：

1. \(\phi(0) = \phi_0\)，初始角度为 \(\phi_0\)。
2. \(\phi(t_f) = \phi_f\)，最终角度为 \(\phi_f\)。
3. \(\dot{\phi}(0) = \dot{\phi}_0\)，初始角速度为 \(\dot{\phi}_0\)。
4. \(\dot{\phi}(t_f) = \dot{\phi}_f\)，最终角速度为 \(\dot{\phi}_f\)。

这些约束条件唯一确定了一个三次多项式 \(\phi(t) = a_0 + a_1 t + a_2 t^2 + a_3 t^3\)。

解为 :

五次多项式

五次多项式可以指定 \(t_0 (= 0)\) 和 \(t_f\) 时刻关节角度和速度以及加速度来规划轨迹：

解为

确定中间点期望关节速度 ¶

法一 ¶

• 将相邻的关节中间点用直线相连，则该直线的斜率就是两个相邻关节中间点的平均速度
• 如果某一关节中间点前后两段直线的斜率符号相反，则可将该点的速度取为 0，如 \(\phi_1\) 和 \(\phi_3\) 处
• 如果某一关节中间点前后两段直线的斜率符号相同，则可将该点的速度取为两者的平均值，如 \(\phi_2\) 处

法二 ¶

• 不直接指定关节中间点处的速度，而是以保证相邻两段三次多项式加速度连续为原则选取三次多项式系数
• 考虑三个相邻的关节中间点，依次为 \(\phi_i\)，\(\phi_j\) 和 \(\phi_k\)
• 连接 \(\phi_i\) 和 \(\phi_j\) 的三次多项式为

• 连接 \(\phi_j\) 和 \(\phi_k\) 的三次多项式为

!!! attention " 这里将第二段三次多项式的起始时间定为 0，目的是简化系数计算 "

满足条件的等式（求解参数）

考虑关节中间点的带抛物线过渡的直线段 ¶

一些定义

用 \(j\)，\(k\) 和 \(l\) 表示三个相邻的路径点 位于路径点\(k\)处的拟合区段的时间间隔为\(t_k\) 位于点\(j\)和\(k\)之间的直线段的时间间隔为\(t_{jk}\) 点\(j\)和\(k\)之间总的时间间隔为\(t_{ajk}\) 直线段的速度为\(\dot{\phi}_{jk}\) 点\(j\)处拟合区段的加速度为\(\ddot{\phi}_j\)

内部路径点计算公式

第一个路径点

• 对于第一个路径段，由 \(\frac{\phi_2 - \phi_1}{t_{d12} - \frac{1}{2}t_1} = \ddot{\phi}_1 t_1\)，取 \(\ddot{\phi}_1 = \text{SGN}(\phi_2 - \phi_1)|\dot{\phi}_1|\)

• 可计算得到

• 进而

最后一个路径点

• 对于最后一个路径段，同样由 \(\ddot{\phi}_n = \text{SGN}(\phi_{n-1} - \phi_n)|\ddot{\phi}_n|\) 和 \(\frac{\phi_{n-1} - \phi_n}{t_{d(n-1)n} - \frac{1}{2}t_n} = \ddot{\phi}_n t_n\)

• 可计算得到

• 进而

机器人动力学 ¶

加速度的传递 ¶

线加速度的传递 ¶

我们考虑矢量 \(^B Q\) 保持不动，即 \(^B V_Q = 0\)、\(^B\dot{V}_Q = 0\)，可以得到：

角加速度的传递 ¶

具体公式如下：

惯性张量 ¶

考虑离散的质点构成的刚体，惯性张量矩阵如下 :

\(C^ P = [x_i, y_i, z_i]^T\) ，代表刚体上离散的第 i 个质点到质心 C 的矢量，m_i 代表第 i 个质点的质量

记为：

考虑质量连续分布的刚体，用密度函数 \(\rho(x,y,z)\) 和微分单元体 \(dV\) 的乘积替代点质量，用积分运算替代求和运算，可得：

牛顿 - 欧拉动力学方程 ¶

转动型关节

外推 :i: 0 → n

内推 :i: n+1 → 1 \(\quad \quad\) \(^{n+1} f_{n+1} = 0, \ ^{n+1} n_{n+1} = 0\)

平动型关节

外推：

内推：

拉格朗日法 ¶

方程如下

\(M(\Phi)\) 公式如下，计算时，可以先计算每一个分量后，再加到一起

对于参数 \(B\):\(B = \text{diag}(b_1, ..., b_N), \quad b_i \text{为折算到关节} i \text{的粘滞摩擦参数}，一般为0\)

对于矩阵 \(C(\Phi, \dot{\Phi})\), 先计算 Christoffel 符号，再写出 \(C(\Phi, \dot{\Phi})\)

Christoffel 符号计算公式如下：

矩阵 \(C\) 计算如下

最后是矩阵 \(G\) 的计算，计算公式如下

运动控制 ¶

独立关节控制 ¶

关节模型如下：

式中，关节 \(i\) 的总等效惯量 \(J_{ci}\)、等效阻尼 \(B_{ci}\) 和控制系数 \(K_{ci}\) 的表达式为

从形式上看，单关节模型式是一个控制输入为 \(U_{ci}\)、干扰输入为 \(T_{ci}\)、输出为 \(\theta_i\) 的线性系统。

经过拉式变换之后，可以得到其传递函数模型如下：

阶跃输入的 PD 控制

模型如下：

在独立关节 PD 控制下，当扰动为 0 时，可以做到阶跃响应无静差

阶跃输入的 PID 控制

模型如下

输入二阶可导的 PID 前馈控制

PID+ 前馈控制

模型如下：

前馈的引入是为了完全消除参考输入对偏差的影响

转矩前馈控制 ¶

先计算出机械臂的动力学模型，根据动力学模型的公式，去带入独立关节控制，推导出实际的干扰项 \(T_{ci}\) 的值

如下所示，在上面的前馈控制的基础上加了对干扰项的补偿

集中控制 ¶

电流反馈 ¶

采用电流反馈的方法，转矩与电流成正比 , 控制律表达如下：

式中，\(V_{ci}\) 为期望电压

关节模型建立如下：

其中

被控对象模型 ¶

其中

重力补偿 PD 控制

控制律：

逆动力学控制 ¶

一般来说，设计系统的控制律时，把控制器分为基于模型的控制部分和位置矫正部分

例如，对于一个二阶线性系统 ( 质量弹簧阻尼系统 )，其动力学模型如下 :

假设可检测质量块的位置和速度，则设计控制律：\( f = -k_p x - k_v \dot{x} \)

两式联立，得：\( m\ddot{x} + (b + k_v)\dot{x} + (k + k_p)x = 0 \)

\(\alpha\)-\(\beta\) 分解

为了消除非线性项的误差，可以采用 *\(\alpha\)-\(\beta\) 分解的方法进行建模，即对 f 进行处理，抵消掉非线性项

取 \(\beta = b\dot{x} + kx\)，\(\alpha = m\)

[ ddot{x} = f' ] 简化为质量模型

同样，设计控制律 \(f' = -k_v \dot{x} - k_p x\)

代入上式，得 \(\ddot{x} + k_v \dot{x} + k_p x = 0\)

根据期望的控制性能确定 \(k_p\) 和 \(k_v\)

内环控制律

对每个双积分系统，可实施“前馈 +PD 控制”

即 \(a_{\Phi} = \ddot{\Phi}_d + K_P (\Phi_d - \Phi) + K_D (\dot{\Phi}_d - \dot{\Phi})\)

其中 \(K_P\) 和 \(K_D\) 是对角矩阵

逆运动学控制的缺点是系统参数必须是精确已知的。

鲁棒控制 ¶

对于逆运动学控制的改进——对参数进行合理估计值

故，非线性反馈值变为如下情况：

式中，\(\hat{M}(\Phi)\)、\(\hat{C}(\Phi, \dot{\Phi})\)、\(\hat{L}\) 和 \(\hat{G}(\Phi)\) 分别是关于 \(M(\Phi)\)、\(C(\Phi, \dot{\Phi})\)、\(L\) 和 \(G(\Phi)\) 的估计项。记各项估计偏差为

因此，鲁棒控制的模型如下：

\(\Delta\) 代表不确定性

外环控制律 \(a_{\Phi} = \ddot{\Phi}_d + K_P (_d - \Phi) + K_D (\dot{\Phi}_d - \dot{\Phi}) + \Xi\)

自适应控制 ¶

自适应控制，被控对象模型采用参数线性化形式，即

式中，\(\Psi\) 包含了机器人未知的全部参数 ( 即，不包含关节角，由未知参数组成的不相关的式子 )。

参数线性化后，基于 \(\Psi\) 的估计改进逆动力学控制如下 :

内环控制律 :

外环控制律 :

力控制 ¶

力位混合控制器 ¶

小测 ¶

Q1- 旋转矩阵性质 ¶

证明，\(R(a \times b) = (Ra) \times (Rb)\)，其中 \(R\) 是旋转矩阵，\(a, b \in \mathbb{R}^3\)。

定义法证明补全

• 首先，我们知道向量叉积 \(a \times b\) 的性质是：

• 现在，考虑 \(R \in SO(3)\)：

• 由于 \(R\) 是旋转矩阵，满足：

• 因此，有：

• 又因为：

所以：

• 由于对于任意 \(x \in \mathbb{R}^3\) 都成立：

根据内积的性质，可得：

评论