人形机器人建模与控制（三） - 机器人控制
1. Repeat
1.1 正向运动学 Forward Kinematics
目标：给定机器人的关节角度
我们可以使用齐次变换矩阵来表示末端执行器的姿态。齐次变换矩阵是一个包含旋转矩阵和平移向量的矩阵，它可以表示姿态的旋转和位置信息。
是一个齐次变换矩阵，表示末端执行器的姿态。 是一个 的旋转矩阵，表示末端执行器的姿态旋转部分。 是一个 的平移向量，表示末端执行器的姿态平移部分。 表示三维特殊欧氏群，表示三维空间中的刚体姿态。
另外，还有一种表示方法是位姿向量
通过给定机器人的关节角度
这样，我们可以得到末端执行器的位置和姿态信息，用于控制和规划机器人的运动。
在机器人中，我们可以通过链接（Links）和关节（Joints）的序列来描述机器人的结构。其中，每个链接都有一个坐标系（框架）与之关联，用于描述链接的位置和方向。这里我们可以使用DH约定（Denavit-Hartenberg约定）来为每个链接分配坐标系。
整体的变换可以通过链接之间的连续变换矩阵得到：
在这个方程中，
通过给定关节角度
或者，更常用的是，我们可以使用一个映射函数
这个映射函数
通过这个映射函数，我们可以在关节空间中控制机器人的姿态，并将其映射到任务空间中的末端执行器的位置和姿态。这使得我们能够以任务空间为导向，直接控制和规划机器人在笛卡尔空间中的运动。
1.2 微分运动学 Differential Kinematics
在微分运动学中，我们关注的是末端执行器的速度与关节速度之间的关系。通过线性化正向运动学映射，我们可以得到雅可比矩阵来描述这种关系。
正向运动学映射
根据链式法则，我们有：
其中，
雅可比矩阵描述了末端执行器速度与关节速度之间的线性映射关系。它将关节速度转化为末端执行器在任务空间中的速度。通过雅可比矩阵，我们可以根据给定的关节速度计算出末端执行器的速度，进而控制和规划机器人在笛卡尔空间中的运动。
线性速度映射公式如下：
其中，线性速度
通过线性速度映射，我们可以将末端执行器的速度分解为线性速度和角速度。这样，我们可以将速度映射为关节速度的线性组合。
External Wrench (Force & Torque) Mapping
同样的，外部力矩映射通过雅可比矩阵的转置与外部力矩向量的乘积来计算。外部力矩向量
这种映射关系使我们能够根据末端执行器的速度和外部力矩，计算出关节速度所需的力矩。这在控制和规划机器人的运动中非常有用，可以帮助我们在笛卡尔空间中对机器人进行力控制和力感知。
1.3 动力学 Dynamics
动力学中的拉格朗日函数（Lagrangian function） 是描述机械系统的标量能量函数：
通过最小作用量原理（Principle of least action），可以推导出运动方程：
其中，
以一维粒子为例：
根据最小作用量原理，我们有：
经过计算，可以得到：
这就是牛顿力学中的牛顿第二定律，其中
通过拉格朗日方程，我们可以推导出描述机械系统的运动方程，从而理解和控制系统的动力学行为。
对于机器人操作器的例子，拉格朗日函数可以表示为：
其中，
根据拉格朗日方程，我们可以得到机器人操作器的动力学方程，即矩阵形式的操作器动力学方程：
其中，
这个动力学方程描述了机器人操作器在给定关节力矩
总结如下：
所以，这篇博客的关注重点是：
- 关节空间和任务空间的控制器设计
- 重点关注操作器（机械手臂等）而不是人形机器人
- 探讨控制与优化之间的关系
2. 反馈抵消 Feedback Cancellation
一般动力学方程描述为：
在许多机器人问题中，动力学可以写成如下形式：
其中，
如果一个系统是欠驱动的，即
如果系统是完全驱动的，即
其中，
这种反馈抵消技术使得系统的闭环响应为：
从而将系统转换为一个简单的二阶线性动力学系统。
需要注意的是，计算
2.1 示例： 双摆
让我们将给定的 MATLAB 矩阵
矩阵定义
惯性矩阵
: 这里，惯性矩阵1
2
3M =
[I133 + I233 + L1^2*m1 + L1^2*m2 + L2^2*m2 + 2*L1*L2*m2*cos(q2), m2*L2^2 + L1*m2*cos(q2)*L2 + I233]
[m2*L2^2 + L1*m2*cos(q2)*L2 + I233, m2*L2^2 + I233]是一个 的对称矩阵，它包含了机器人在广义坐标 下的惯性信息。矩阵的元素与连杆的质量 、 、连杆的长度 、 、惯性张量 、 以及关节角度 相关。 科里奥利力矩阵
: 科里奥利力矩阵1
2
3C =
[-L1*L2*m2*q2p*sin(q2), -L1*L2*m2*sin(q2)*(q1p + q2p)]
[ L1*L2*m2*q1p*sin(q2), 0]是一个 矩阵，它描述了由关节速度 引起的科里奥利力和离心力。 重力向量
: 重力向量1
2
3G =
[L2*gx*m2*cos(q1 + q2) + L2*gy*m2*sin(q1 + q2) + L1*gx*m1*cos(q1) + L1*gx*m2*cos(q1) + L1*gy*m1*sin(q1) + L1*gy*m2*sin(q1)]
[L2*m2*(gx*cos(q1 + q2) + gy*sin(q1 + q2))]是一个 的向量，表示由重力 和 作用引起的广义力。这个向量包含了关节角度 和 以及各个连杆的质量和重力加速度的分量。
动力学方程
给定动力学方程为：
这个方程表示广义加速度
将动力学方程具体化
具体化方程：
其中
一维摆动的特例
对于一维摆动系统，动力学方程简化为：
在去掉非线性项的情况下：
解释
- 惯性矩阵
： - 描述了系统的惯性特性。随着关节角度
的变化，惯性矩阵的元素也会发生变化。 - 例如，
反映了两个连杆之间的耦合作用。
- 描述了系统的惯性特性。随着关节角度
- 科里奥利力矩阵
： - 反映了关节运动速度对力的影响。
- 例如，
表示由于第二关节速度 和角度 引起的科里奥利力。
- 重力向量
： - 反映了系统在重力作用下的力。
- 例如，
表示第二关节由于重力在 方向的分量。
约束力和控制力矩的关系
动力学方程中包含了约束力：
通过解约束力的显式表达式，可以得到：
3. 关节空间控制 Joint Space Control
给定机械臂的非线性动力学：
我们的目标是找到
来跟踪轨迹：
其中，反馈增益
该控制律的收敛性可以通过分析误差动力学来验证：
补偿项导致误差动力学为：
通过拉普拉斯变换可以检查稳定性：
通过选择适当的反馈增益
通常设置为临界阻尼：
所以我们得到了关节空间的控制律：
该控制律被称为：
- 逆动力学控制（Inverse-Dynamics Control）：因为它需要动力学模型。
- 计算力矩控制（Computed Torque Control）：因为它计算力矩以实现期望的加速度。
- 反馈抵消/反馈线性化（Feedback cancellation/Feedback linearization）：因为它使用状态反馈来抵消非线性项。
这种控制律根据期望的关节加速度
通过该控制律，我们可以实现期望的关节加速度，并通过对非线性项的反馈来抵消动力学的影响。因此，它可以实现对机器人系统的精确控制和非线性项的补偿。
Simpler Control Laws
简化的控制律，较少依赖于模型：
PD + G 控制
PID 控制
当模型存在较大误差时（尤其是惯性矩阵），这些控制律变得特别有趣。
4. 笛卡尔空间控制 Cartesian Space Control
通常我们在笛卡尔空间中描述问题。当给定末端执行器的参考姿态
4.1 逆运动学
首先，使用逆运动学计算出关节轨迹，以便末端执行器在给定时间范围内能够达到期望的参考姿态。
逆运动学问题是一个超非线性（非凸）问题，解起来非常复杂。它通常是计算上代价很高的，并且往往没有唯一的解（关节变量的维数大于末端执行器姿态的维数）。
4.2 直接在笛卡尔空间中制定反馈控制律
为避免逆运动学的复杂性，可以直接在笛卡尔空间中制定反馈控制律。该方法的基本思想是在笛卡尔空间中计算期望的末端执行器加速度
笛卡尔空间与关节空间之间的关系：
期望的加速度应为：
控制力矩应为：
该控制律具有以下优点：
- 增益在笛卡尔空间中定义
- 无需解决复杂的逆运动学问题
- 可用于在线实时跟踪
4.3 姿态误差计算
计算姿态误差取决于姿态表示方式：
4.3.1 四元数表示
使用对数映射：
姿态误差变为：
4.3.2 旋转矩阵表示
使用对数映射：
姿态误差变为：
4.4 控制策略
在机器人控制中，力控（Force Control）和导纳控制（Admittance Control）是两种常见的控制策略，它们分别用于不同的场景。我们将分别介绍这两种控制策略，并解释其工作原理和应用场景。
4.4.1 力控（Force Control）
力控的目标是控制机器人末端执行器的力，特别是在末端执行器与环境接触时。力控主要分为两种情况：不含测量的控制律和带力反馈的控制律。
力控的动力学方程
当末端执行器与环境接触时，动力学方程变为：
这里：
是广义力（包括重力、科里奥利力和其他非线性力）。 是关节力矩。 是末端执行器的雅可比矩阵。 是末端执行器的外力。
不含测量的控制律
一种简单的力控制律是设定期望外力
这种控制律没有使用力反馈，因此只适用于末端执行器与环境的接触力可以预先估计或已经知道的情况。
带力反馈的控制律
为了提高力控制的精度，可以引入力反馈：
这里：
是期望外力。 是外力误差，即实际测量的外力与期望外力之间的差值。 和 分别是比例和积分增益矩阵。
这种控制律通过力反馈来动态调整关节力矩，以更精确地控制末端执行器与环境的接触力。
4.4.2 导纳控制（Admittance Control）
导纳控制的目标是使机器人系统对外力作出期望的动力学响应，即使其行为类似于虚拟的弹簧-质量-阻尼系统。
导纳控制的动力学方程
导纳控制的动力学方程模拟了一个虚拟的弹簧-质量-阻尼系统：
这里：
是虚拟的惯性矩阵。 是虚拟的阻尼矩阵。 是虚拟的刚度矩阵。 是末端执行器的位置误差。 是末端执行器的外力。
期望加速度
导纳控制的目标是通过控制使系统对外力产生期望的加速度：
这里，
控制律
导纳控制的控制律与之前的轨迹跟踪控制律相似：
这里：
是雅可比矩阵的伪逆。 是雅可比矩阵的时间导数项。 是包括科里奥利力和重力项的广义力。
总结
力控和导纳控制是两种常见的机器人控制策略：
- 力控 通过直接控制关节力矩以实现末端执行器的力控制，可以使用力反馈提高控制精度。
- 导纳控制 则通过将机器人系统的动力学响应模拟为虚拟的弹簧-质量-阻尼系统，使其能够对外力作出期望的动力学响应。
5. 最优化控制 Control as Optimization
再次查看关节空间控制律：
我们知道这实际上可以表示为以下两个部分的和：
- 机器人动力学方程：
- 期望的关节加速度：
因此，可以将这个问题看作是以下优化问题的求解：
这个优化问题的目标是通过最小化关节加速度误差的平方范数，找到最优的控制力矩。
将控制问题表示为优化问题的形式，虽然在目前的情况下没有直接的优势，但它允许我们在控制器的制定中包含更多有用的因素，如约束。
我们可以将最优控制看作一个最小二乘问题（LSP）
一般形式：
也可以看做更一般的二次规划问题（QP）
于是我们可以引入以下约束：
- 执行器限制
- 关节限制
- 速度限制
- 自碰撞约束
此外，这些优化问题还可以扩展为模型预测控制（MPC），在一系列离散时间步中进行优化，实现更复杂的控制策略。
再看一次笛卡尔空间控制律：
我们知道这实际上可以表示为：
- 机器人动力学：
- 任务空间映射：
- 反馈控制律：
这个控制律等价于以下最小二乘问题：
这也是LSP的形式，也可以表示为QP.
通过这个优化问题，我们可以实现在笛卡尔空间中对末端执行器的轨迹跟踪控制。通过调整关节加速度，我们可以最小化雅可比矩阵乘以关节加速度与期望末端执行器在笛卡尔空间的加速度减去雅可比矩阵时间导数乘以关节速度之间的误差。从而使得机器人能够按照期望的轨迹在笛卡尔空间中进行运动。
这种优化问题的表达形式使得我们可以以更灵活的方式设计和调整控制器。通过调整增益矩阵和期望轨迹，我们可以改变控制器对误差的响应和对系统动力学的影响。这样，我们可以实现更精确、稳定和可靠的笛卡尔空间控制。
- Title: 人形机器人建模与控制（三） - 机器人控制
- Author: xiangyu fu
- Created at : 2024-05-26 23:34:47
- Updated at : 2024-06-22 12:18:28
- Link: https://redefine.ohevan.com/2024/05/26/legged_robots/blog-03/
- License: This work is licensed under CC BY-NC-SA 4.0.