笛卡尔空间内的阻抗控制

1. 笛卡尔空间内的阻抗控制方程推导

目标：让机器末端执行器在笛卡尔空间内的每个方向上都体现出由弹簧阻尼质量组成的二阶系统的动态特性。
$$M_d\ddot{\tilde{x}}+D_d\dot{\tilde{x}}+K_d\tilde{x}=F_{ext},\tilde{x}=x_d-x\text{\hspace{1em}(1.1)}$$
  笛卡尔空间内的阻抗控制将机器人视为一个整体，关注机器人末端执行器的运动状态，末端的不同维度都体现出弹簧-质量-阻尼构成的二阶系统。相比于关节空间，笛卡尔空间下定义阻抗行为更加直观、实用；但对机器人的控制在关节空间内进行的，所以实现笛卡尔空间下的阻抗控制需要进行正逆运动学、动力学的解算。
  在控制系统中，$M_d$、$D_d$、$K_d$矩阵分别对应于质量矩阵、阻尼矩阵和弹簧矩阵，它们共同描述了系统的动力特性。这些矩阵可以根据设计需求自行设计和调整。矩阵的维度与机械系统的自由度密切相关。理论上，矩阵的维度不应超过系统的自由度。例如，如果一个机器人仅具有两个自由度，那么其阻抗行为最多只能在两个方向上进行定义，无论是在平面空间还是立体空间中。这意味着，阻抗控制的设计必须与系统的自由度相匹配。三个矩阵最大阶数为6，对应笛卡尔空间上的6个阻抗特性。
  同理，假定机器人的控制任务仍是跟随一条提前规划的轨迹即：${\ddot{x}}_d,{\dot{x}}_d,{\mathbf{x}}_d$，则可定义如下的关节转矩控制输入：
$$\begin{array}{rl}\tau =& \mathbf{M}\left(\mathbf{q}\right){\mathbf{J}}^{-1}\left(\mathbf{q}\right){\mathbf{M}}_d^{-1}\left({\mathbf{M}}_d{\ddot{\mathbf{x}}}_d+\mathbf{K}\dot{\bar{x}}+\mathbf{D}\tilde{\mathbf{x}}-{\mathbf{M}}_d\dot{\mathbf{J}}\left(\dot{\mathbf{q}},\mathbf{q}\right)\dot{\mathbf{q}}\right)\\ & +\left({\mathbf{J}}^T\left(\mathbf{q}\right)-\mathbf{M}(\mathbf{q}){\mathbf{J}}^{-1}\left(\mathbf{q}\right){\mathbf{M}}_d^{-1}\right){\mathbf{F}}_{ext}\\ & +\mathbf{g}(\mathbf{q})+\mathbf{C}(\mathbf{q},\dot{\mathbf{q}})\dot{\mathbf{q}}\end{array}\text{\hspace{1em}(1.2)}$$
  与关节坐标下的关节转矩控制输入函数构成相似，第一项需要与雅可比矩阵的逆相乘，雅可比矩阵反应机器人的关节角速度和末端执行器在笛卡尔空间中的速度的映射关系，括号中减去的项与雅可比矩阵求导有关，在物理意义上，通过减去该项抵消掉由关节加速度而产生的惯性力的作用。
  第二项与末端所受外力有关，相比于关节空间中关节转矩控制输入的该项，同样需要带入雅可比矩阵的转置和逆，实现从笛卡尔空间到关节空间的映射。
速度和加速度求解：
$$\begin{array}{rl}& \dot{\mathbf{x}}=\mathbf{J}\left(\mathbf{q}\right)\dot{\mathbf{q}},\\ & \ddot{\mathbf{x}}=\mathbf{J}\left(\mathbf{q}\right)\ddot{\mathbf{q}}+\dot{\mathbf{J}}\left(\dot{\mathbf{q}},\mathbf{q}\right)\dot{\mathbf{q}}\end{array}\text{\hspace{1em}(1.3)}$$
  将关节转矩控制输入表达式代回机器人动力学方程，整理后可得到所期望的二阶系统动态方程。理想情况下，机器人末端执行器可在笛卡尔空间内体现出完全解耦的阻抗特性。
  实际操作中，理想的阻抗控制实现的难度较大，需要：

• 1.在线求解雅可比矩阵的（伪）逆矩阵，及雅可比矩阵的导数矩阵
• 2.精确测量机器人执行器末端受到的外力
• 3.精确的机器人运动学以及动力学模型
• 4.机器人关节位置及转速的测量
• 5.机器人关节转矩控制

注意：实际操作时，尽量不要过度接近机器人奇异点。
  从数学角度讲，机器人奇异点处的雅可比矩阵不可逆，靠近奇异点处的雅可比矩阵求逆误差较大，从物理角度讲，机器人奇异点处的关节自由度减少，靠近奇异点处的在某些方向上的运动受到较大限制。

2. 笛卡尔空间内的阻抗控制的控制框图

下面从控制角度分析笛卡尔空间内的阻抗控制框图结构：
  首先可以看到这个阻抗控制的输入信号，给定的控制设定值都是用于机器人的运动控制的运动相关的量，如速度、加速度和位移。在此背景下，阻抗行为可以等效地看作一种线性控制器，其中阻抗系数相当于控制器的增益。该控制器的作用在于尽可能减少设定值与机器人实际空间运动状态之间的误差。
  这个线性控制器将在笛卡尔空间中生成一个虚拟控制输入（通常表示为 𝑦）。接下来的任务是通过不同空间间的映射，将这个虚拟控制输入转换到关节空间坐标系中。此时，还需要结合机器人的动力学模型，计算出每个关节所需的转矩u。
  从反馈控制的角度出发，我们需要对机器人在笛卡尔空间中的运动状态进行反馈测量。实现这一反馈可以有多种方式。例如，若机器人在操作空间中配备了相应的传感器系统（如视觉系统），则可以直接测量机器人在空间中的运动状态。另一种常见的方式是通过测量机器人的关节运动，并利用机器人运动学方程计算出其末端执行器在笛卡尔空间中的运动状态。
  此外，如果期望实现更为完善的控制效果，还需要对机器人末端的外力进行反馈。基于实际情况，我们可以灵活调整笛卡尔空间中的阻抗控制形式，以满足具体的需求。

3. 一些变体

如果不能提供末端外力的实时反馈：

变体 1.1

直接删去与外力相关的部分
$$\begin{array}{rl}& \mathbf{\tau }=\mathbf{M}(\mathbf{q}){\mathbf{J}}^{-1}\left(\mathbf{q}\right){\mathbf{M}}_d^{-1}\left({\mathbf{M}}_d{\ddot{x}}_d+\mathbf{K}\dot{\tilde{x}}+\mathbf{D}\tilde{x}-{\mathbf{M}}_d\dot{\mathbf{J}}\left(\dot{\mathbf{q}},\mathbf{q}\right)\dot{\mathbf{q}}\right)\\ & +\mathbf{g}(\mathbf{q})+\mathbf{C}(\mathbf{q},\dot{\mathbf{q}})\dot{\mathbf{q}}\end{array}\text{\hspace{1em}(1.4)}$$
代入机器人动力学方程，可得：
$$M_d\ddot{\tilde{x}}+D\dot{\tilde{x}}+K\tilde{x}=M_d\mathbf{J}\left(\mathbf{q}\right){\mathbf{M}}^{-1}(\mathbf{q}){\mathbf{J}}^T\left(\mathbf{q}\right){\mathbf{F}}_{ext}\text{\hspace{1em}(1.5)}$$
结果：机器人仍能对外体现阻抗特性，但各方向之间存在耦合。

变体 1.2

设计$M_d={\mathbf{J}}^{-T}\left(\mathbf{q}\right)\mathbf{M}(\mathbf{q}){\mathbf{J}}^{-1}\left(\mathbf{q}\right)$得
$$\begin{array}{rl}\mathbf{\tau }=& \mathbf{M}(\mathbf{q}){\mathbf{J}}^{-1}\left(\mathbf{q}\right){\mathbf{M}}_d^{-1}\left({\mathbf{M}}_d{\ddot{x}}_d+\mathbf{K}\dot{\tilde{x}}+\mathbf{D}\tilde{x}-{\mathbf{M}}_d\dot{\mathbf{J}}\left(\dot{\mathbf{q}},\mathbf{q}\right)\dot{\mathbf{q}}\right)\\ & +\underset{=0}{\underbrace{\left({\mathbf{J}}^T\left(\mathbf{q}\right)-\mathbf{M}(\mathbf{q}){\mathbf{J}}^{-1}\left(\mathbf{q}\right){\mathbf{M}}_d^{-1}\right)}}{\mathbf{F}}_{ext}\\ & +\mathbf{g}(\mathbf{q})+\mathbf{C}(\mathbf{q},\dot{\mathbf{q}})\dot{\mathbf{q}}\end{array}\text{\hspace{1em}(1.6)}$$
代入机器人动力学方程，可得
$${\mathbf{J}}^{-T}\left(\mathbf{q}\right)\mathbf{M}\left(\mathbf{q}\right){\mathbf{J}}^{-1}\left(\mathbf{q}\right)\ddot{\widetilde{\stackrel{~}{x}}}+{\mathbf{K}}_D\dot{\tilde{x}}+{\mathbf{K}}_P\tilde{\mathbf{x}}={\mathbf{F}}_{ext}\text{\hspace{1em}(1.7)}$$
结果：机器人仍能对外体现阻抗特性，但与质量相关的项存在耦合。

变体 2

PD+控制
$$\tau ={\mathbf{J}}^T(\mathbf{q})\left(\mathbf{K}\tilde{\mathbf{x}}+\mathbf{D}\dot{\tilde{x}}\right)+\mathbf{g}(\mathbf{q})+\mathbf{C}(\mathbf{q},\dot{\mathbf{q}})\dot{\mathbf{q}}\text{\hspace{1em}(1.8)}$$
PD控制器+运动过程中产生的非线性力的补偿
雅可比矩阵的转置通常用于将力/力矩从关节空间映射到笛卡尔空间。
代入机器人动力学方程，可得：
$$-J^{-T}\left(q\right)M\left(q\right)\ddot{q}+D_d\dot{\tilde{x}}+K_d\tilde{x}=F_{ext}\text{\hspace{1em}(1.9)}$$
结果：机器人仍能对外体现阻抗特性，但与设定值误差较大。
引申问题：如何设计阻尼系数D,从而减小部分耦合作用：
  阻尼矩阵和弹性矩阵通常设计为对角矩阵，这是基于机器人在不同方向上的运动不存在耦合的假设。然而，既然我们已经知道这种耦合作用确实存在，那么通过设计阻尼矩阵或弹性矩阵的非对角元素，是否可以抵消部分由耦合作用引起的影响呢？
  这种方法是可行的。通过引入非对角元素，我们可以考虑不同运动方向之间的相互作用，从而补偿由耦合作用带来的影响。在实际应用中，非对角元素可以用于表达各个运动方向间的相互耦合关系。通过精确设计这些非对角元素，系统可以更好地处理复杂的动力学耦合问题，从而提高控制的稳定性与精确度。
推荐阅读：Ott,C.(2008).Cartesian impedance control of redundant and flexible-joint robots.Springer.。它是Ott,C写的一本关于完全关于这个笛卡尔空间的阻抗控制的一本学术专著，Ott,C是德国宇航局一个非常有名的机器人专家，研究方向主要就是阻抗控制。

4.笛卡尔空间内的阻抗控制方法总结

目的：控制机器人运动与外力之间的动态关系，使其等效为由弹簧-阻尼质量组成的二阶系统。
  阻抗控制它最根本的目的既不是去控制机器人的运动，也不是去控制机器人的外力。它所希望达到的是机器人的运动和外力之间的某种动态关系，而这种动态关系是可以等效的，由一个弹簧-阻尼-质量所组成的二阶系统来描述。
实现：机器人的运动控制（间接力控）
  从阻抗控制的实现角度来看，关注的仍然是机器人的运动控制。尽管需要处理的是机器人与外界接触的情况，即存在力的相互作用，但我们并没有指定任何关于力的设定值。因此，阻抗控制属于间接力控制的范畴。
需要：
1.机器人动力学及运动学模型
2.关节角及角速度的反馈
3.关节转矩控制
4.外力（矩）的测量（非必需）
  实际操作中，可以根据需要对控制输入进行简化。一般来说，控制输入越简单，体现的阻抗特性误差越大（耦合作用）。

参考资料

笛卡尔空间内的阻抗控制