冗余和柔性关节机器人的笛卡尔阻抗控制————Ott

目录

1. 引言
1.1 服务机器人应用
1.2 DLR 轻型机器人
1.3 柔顺运动控制
1.3.1 阻抗控制
1.3.2 导纳控制
1.3.3 保守一致性变换
1.4 冗余机器人控制
1.5 柔性关节机器人控制
1.6 概述
2. 柔性关节机器人建模
2.1 机器人运动学概要
2.1.1 刚体运动
2.1.2 正向运动学
2.2 柔性关节机器人的动力学模型
2.2.1 完整模型的建模假设
2.2.2 完整模型的推导
2.2.3 简化模型
2.2.4 动力学模型的性质
2.2.5 关于摩擦力的若干说明
2.3 总结
3. 笛卡尔阻抗控制：刚体情况
3.1 完全解耦
3.1.1 任务表述
3.1.2 任务坐标中的机器人模型
3.1.3 经典阻抗控制器
3.2 避免惯性整形
3.3 阻尼矩阵设计
3.4 奇异性处理
3.5 关于刚度实现的说明
3.5.1 平移刚度
3.5.2 旋转刚度
3.5.3 对闭环动力学的影响
3.6 总结
4. 零空间刚度
4.1 零空间基矩阵的计算
4.1.1 一般情况
4.1.2 一维零空间的情况
4.2 基于投影的方法
4.3 任务空间扩展方法
4.4 关节空间分解方法
4.4.1 控制器设计
4.4.2 稳定性分析
4.5 总结
5. 奇异摄动方法
5.1 奇异摄动理论
5.2 柔性关节机器人的奇异摄动模型
5.2.1 准稳态模型
5.2.2 边界层系统
5.3 控制器设计
5.4 改进的控制器设计
5.4.1 改进的准稳态系统
5.4.2 改进的边界层系统
5.4.3 笛卡尔阻抗控制器的设计
5.5 总结
6. 基于级联结构的控制器设计
6.1 基于解耦的方法
6.1.1 级联系统的控制理论
6.1.2 扭矩动力学的解耦
6.1.3 阻抗控制器设计
6.1.4 稳定性分析
6.1.5 控制器讨论
6.2 基于反步法的方法
6.2.1 反步法设计步骤
6.2.2 应用于柔性关节模型
6.2.3 控制器讨论
6.3 总结
7. 基于无源性的方法
7.1 设计思路
7.2 应用于柔性关节模型
7.2.1 推广到笛卡尔坐标
7.3 重力补偿
7.3.1 重力Hessian矩阵的有界性
7.3.2 重力补偿项的构建
7.3.3 重力补偿势能的推导
7.4 分析
7.4.1 无源性
7.4.2 稳定性
7.4.3 控制器讨论
7.5 精确刚度设计
7.5.1 精确关节级刚度的设计
7.5.2 精确笛卡尔刚度的设计
7.6 进一步推广
7.6.1 包含关节阻尼
7.6.2 推广到全状态反馈
7.6.3 关于速度滤波的说明
7.6.4 惯性整形
7.7 跟踪
7.7.1 无重力情况
7.7.2 包含重力
7.7.3 控制器讨论
7.8 与其他方法的关系
7.8.1 与奇异摄动控制器的比较
7.8.2 与Albu-Schaffer控制器的比较
7.8.3 与IDA-PBC和控制拉格朗日方法的关系
7.9 总结
8. 评估
8.1 仿真结果
8.1.1 基于奇异摄动的控制器
8.1.2 基于解耦的控制器
8.1.3 基于反步法的控制器
8.1.4 基于无源性的控制器
8.1.5 鲁棒性
8.2 DLR-Lightweight-Robot-II 实验
8.2.1 基于奇异摄动的控制器
8.2.2 基于无源性的控制器
9. 应用
9.1 作为服务机器人任务的擦桌子
9.2 医疗场景：椎弓根螺钉置入
9.3 柔顺移动操作
9.4 开门
9.5 双臂操作
10. 控制器比较与结论
参考文献
附录 A
A.1 时变系统
A.2 条件稳定性
A.3 无源性定义
A.4 一些矩阵引理和定义
A.5 扩展雅可比矩阵 (J_{N}(q)) 的逆
A.6 DLR-Lightweight-Robot-II 的模型参数
A.7 使用的符号列表

---

1 引言

机器人系统是控制理论的一个研究成熟的应用领域。传统上，为工业应用设计的机器人手臂被视为机械多体系统，其中几个刚体通过驱动的关节连接。在过去几年中，机器人学界对设计用于服务机器人场景的机器人产生了越来越大的兴趣，在这些场景中，机器人将需要在人类附近工作。对于这类应用，期望机器人系统构建得非常轻。因此，这些系统通常具有相当大的弹性。因此，需要不仅考虑刚体动力学还要考虑关节柔性的控制策略。

第1.1节简要概述了典型的服务机器人应用及其特点。第1.2节描述了DLR轻型机器人，本书中报告的控制器就是在这些机器人上实现和测试的。

柔顺运动控制方案以及柔性关节机器人控制方法的最新进展分别在第1.3节和第1.5节中回顾。第1.6节最后给出了本书的概述并总结了贡献。

1.1 服务机器人应用

虽然对于大多数工业应用，机器人的环境可以预先知道，但在服务机器人和医疗机器人等领域情况则大不相同。在这些应用中，机器人与人类共享环境。因此，不仅需要与已知环境直接交互，还需要与人类用户直接交互。

虽然移动机器人在实验室环境之外的实际应用已经有一段时间了（例如，室内地板清洁机器人、泳池清洁机器人等），但对于必须同时具备操作能力的系统来说，情况则不同。这种通用服务机器人的典型任务包括：擦拭任务、不同的取放操作，或开门和开抽屉。从概念的角度来看，已经提出了从移动机器人与拟人化手臂和手的组合到更高级的人形机器人的不同方法。它们的共同点是包含一只或两只机器人手臂和手，这些手臂和手必须建造得非常轻，以便与系统的移动部分兼容。

对轻型手臂（和手）的需求不仅源于这些实际要求，还源于安全要求。在典型的服务机器人场景中，机器人将不得不在人类附近工作，甚至与他们进行物理交互。一种减少意外碰撞时可能发生的伤害的方法是在机器人结构上使用柔顺覆盖层。在ZKRS02中表明，这种解决方案在许多情况下不实用。另一种方法，即在DLR轻型机器人的构造中遵循的方法，是减少手臂的有效惯性。

毕竟，现代轻型手臂的设计和控制显然是开发适用于新兴服务机器人领域的机器人系统的关键点。这类系统的轻型结构自然会导致弹性，这些弹性必须在控制器设计中予以考虑。

除了服务机器人应用之外，当高性能要求用于非常快速的运动时，对设计用于具有不可忽略的关节弹性的机器人模型的控制器的需求也出现在工业机器人中。

1.2 DLR 轻型机器人

七关节DLR轻型机器人特别针对服务机器人应用进行了优化。与典型的工业机器人相比，所有电子设备都包含在手臂结构中。这对于允许将这些手臂与用于移动的系统相结合也很重要。Lightweight-Robot-II（图1.1（左））于2001年首次亮相。其主要设计目标是实现与人类手臂相当的负载重量比。因此，机器人的所有关键部件都针对低重量进行了优化。在结构中使用了大传动比的Harmonic Drive®谐波齿轮和直流无刷电机。关节中的Harmonic Drive®齿轮是弹性的主要来源。虽然这些齿轮原则上是可以反向驱动的，但由于高传动比，移动关节所需的外部力非常高。为了应对这种情况，关节中包含了基于应变片的关节扭矩传感器。这些传感器允许测量连杆侧非常小的力，这些力由于静摩擦在电机侧很难测量到。扭矩传感器的刚度大约比齿轮的刚度高一个数量级，因此关节的总刚度由齿轮主导。从控制的角度来看，关节扭矩传感器与电机位置测量值一起，允许在考虑柔性关节机器人模型时实现全状态反馈（见第2章）。

作为上述考虑的结果，Lightweight-Robot-II的重量约为 $18\mathrm{kg}$ ，可承载高达 $7\mathrm{kg}$ 的负载。组件的进一步发展和追求模块化结构导致了Lightweight-Robot-III（图1.1（右））的设计，其总重量为 $14\mathrm{kg}$ ，负载重量比约为1:1。

由于如上所述，Harmonic Drive®齿轮是DLR轻型机器人柔性的主要来源，因此将简要解释这种齿轮类型的工作原理。齿轮基本上由三部分组成：波发生器、柔轮和刚轮（图1.2）。柔轮和刚轮是带有齿的部分。柔轮特别地比刚轮少两个齿，并且是可变形的。波发生器是驱动元件，具有椭圆形状。假设刚轮是固定的。通过转动波发生器，柔轮变形，波发生器椭圆形状的主轴决定了柔轮齿和刚轮齿接触的区域。例如，考虑波发生器转动一整圈的情况。由于柔轮比刚轮少两个齿，在波发生器每转一圈期间，柔轮将相对于刚轮转动大约两个齿的角度。这样就实现了波发生器和柔轮之间的非常高的传动比。

在本工作中，只考虑谐波齿轮的弹性。更完整的模型可以在例如TS96, KG97中找到。

1.3 柔顺运动控制

实现柔顺行为是机器人学中的一个经典问题。每当机器人与环境接触时，特别是当环境仅部分已知时，这一点就很重要。

下面讨论三种不同的方法。这些方法与经典的位置或力控制概念之间的区别在于，除了设定点控制任务之外，还必须确保对外力的期望扰动响应。在阻抗控制文献中，期望设定点 $\pmb{x}_{d}$ 通常被称为虚拟期望设定点（或平衡点），因为它只应在自由运动时达到，即没有外力作用在机器人上时。

1.3.1 阻抗控制

实现柔顺行为的一种可能方法是由阻抗控制（图1.3）给出的。阻抗控制的一般概念由Hogan在开创性工作Hog85a, Hog85b, Hog85c中引入，如今可以被认为是机器人学中的一种经典控制方法。阻抗控制方法论的核心陈述之一是控制器应调节操纵器的机械阻抗。这由以下观察证明：对于操作任务，机器人的环境被恰当地描述为机械导纳。在阻抗控制的更强定义中（有时也使用术语基于力的阻抗控制），进一步要求不仅受控的操纵器，而且控制器本身也应具有阻抗因果关系。在本工作中，术语阻抗控制指的是这种更强的定义。

从实际的角度来看，期望的机器人动态行为，即扰动响应（将外力视为扰动），通常用描述末端执行器位姿的笛卡尔坐标来定义。操作空间公式化Kha87提供了关于笛卡尔坐标的控制器设计方法。在典型实现中，控制器以末端执行器的笛卡尔位置 $\mathbf{x}$ 和速度 $\dot{\mathbf{x}}$ 为输入，并给出电机扭矩 $\tau_{m}$ 作为输出。

笛卡尔阻抗控制器设计中的一个相关问题是如何选择适当的末端执行器方向表示。Caccavale等人在CNSV99和Natale在Nat03中给出了不同方向表示在笛卡尔阻抗控制背景下的详细比较。构建适用于阻抗控制器的空间刚度的问题由Fasse在FB97, FH95, Fas97中提出。这些工作的进一步扩展由Stramigioli在SD01, Str01中提出。

1.3.2 导纳控制

另一种可能的方法，通常在工业机器人中遵循，是导纳控制的概念（图1.4），有时也称为基于位置的阻抗控制。这里机器人由位置控制器控制。期望的柔顺行为由外部控制回路实现。基于广义环境力 $\mathbf{F}_{ext}$ 的测量，为位置控制回路生成期望设定点 $\mathbf{x}_0$ 。因此，导纳控制器具有机械导纳的因果关系，即映射 $\mathbf{F}_{ext} \to \dot{\mathbf{x}}_0$ 。导纳控制的概念非常适用于工业机器人。这些机器人通常配备现成的位置或速度控制器，而用户没有提供关节扭矩或电机电流接口。此外，在通常的工业场景中，环境力和力矩可以通过安装在机器人末端执行器上的六自由度力-力矩传感器来测量。众所周知，非共位力（在机器人末端执行器处测量）的反馈会导致（具有挑战性的）稳定性问题CH89。如果使用刚性力-力矩传感器，例如广泛使用的JR³，这一点尤其重要。或者，也可以使用柔顺力-力矩传感器。

此外，Mason在Mas81中引入并由Raibert和Craig在RC81改进的混合位置/力控制也与导纳控制概念密切相关。混合位置/力控制方案中的坐标不变性问题在LD88和Duf90中讨论。

1.3.3 保守一致性变换

除了上述两种方法之外，将笛卡尔阻抗特性（如刚度矩阵或阻尼矩阵）投影到关节坐标中的相关量上，代表了实现柔顺运动控制器的第三种可能性CK02。刚度矩阵的变换定律也称为保守一致性变换。如果机器人配备有关节级阻抗控制器，这尤其值得关注。

机器人学界目前热烈讨论的一个有趣的相关概念是在每个关节中使用两个执行器的拮抗布置，以实现可变的关节刚度LKCC91, BTBP03, BRT01。这个概念允许实现没有非对角线元素的关节级刚度矩阵。然而，实现期望笛卡尔刚度的可能性是有限的ASFS+04。

1.4 冗余机器人控制

当任务坐标数量 $m$ 小于配置坐标数量 $n$ 时，问题是如何控制剩余的 $n - m$ 个冗余自由度。在这种情况下，保持末端执行器固定的关节运动称为零空间运动。这种冗余机器人的控制是机器人文献中广泛讨论的一个话题。以下简短文献概述仅涉及与本书主题密切相关的那些工作。

不同冗余分辨率技术的良好概述由Sic90给出。一种基于投影的方法，通过零空间运动优化给定成本函数，在HHS89中提出。一种使用扩展雅可比矩阵的逆运动学方法在Bai85中引入。冗余机器人逆运动学中零空间基矩阵的计算及其应用在HV91和CW93中分析。

在操作空间公式化Kha87中，使用零空间投影技术来处理冗余机器人。

冗余操纵器的阻抗控制问题在NSV99中结合基于四元数的空间刚度进行了考虑。关节空间分解方法在PCY99, PCY00中表述，并应用于OCY97, OCY98, OCY98中的阻抗控制问题。

1.5 柔性关节机器人控制

具有柔性关节的机器人系统在机器人文献中已经被研究了二十多年。与具有弹性连杆的机器人模型相比，关节弹性通常被建模为位于电机和后续连杆之间的集总线性弹簧Spo87a。

TomeiTom91表明，这种系统可以通过一个简单的电机角度PD控制器以及期望平衡点处的重力扭矩补偿来稳定。在KS98, SK97a中，基于无源性论证提出了类似的控制器。此外，在GFG99中，给出了无重力效应的柔性关节机器人混合位置/力控制器的稳定性分析。

在实践中，对于柔性关节机器人，限制于纯电机位置（和速度）反馈控制器（没有额外的非共位反馈）只能得到相当不理想的结果。在一些工作中，考虑了基于关节扭矩以及连杆侧位置反馈的控制器结构，并表明这确实可以带来更好的结果（例如，见Spo89）。从理论角度来看，这种方法通常（对于足够高的关节刚度）通过基于奇异摄动理论的近似分析来证明其合理性Spo89, Spo95, GHS89, Ge96。

在ASH01a, AS01中，引入了一个具有完整静态状态反馈（位置和扭矩及其一阶导数）的控制器，基于控制器的无源性特性，证明了（类似于Tom91）渐近稳定性。与经典的PD控制器相反，电机惯性和关节刚度与状态反馈控制器包含在同一个无源模块中，从而可以实现关节振荡的有效阻尼。

在DL00中提出了一种通过适当的前馈项扩展简单PD控制器的方法，以处理跟踪情况。在DLSZ05, ZSL+03, ZLS04, ZSL+05中，解决了PD类调节控制器的重力补偿问题。

输出反馈控制问题在OKL95, NT95中处理。在DLL98中，提出了完整柔性关节机器人模型的动态反馈线性化算法。

另一种方法，包括对扭矩动力学进行部分反馈线性化并添加外部控制回路，在LG95, LG96中提出。跟踪控制问题也在许多工作中得到了考虑，参见BOL95的出色比较。一种基于无源性的方法在LB92中用于自适应情况。在OL97中使用了反步法，在NT92中提出了自适应跟踪控制器。

此外，变结构控制器在SRS88和HG93中进行了分析。

1.6 概述

虽然大多数关于柔性关节机器人控制的工作处理的是位置控制问题，但关于阻抗控制问题的已发表工作很少。如上一节所述，位置控制问题的解决方案范围从基于奇异摄动方法的简单解决方案到基于反馈线性化或反步法等现代方法的更复杂解决方案。注意，所有这些结果都可以很容易地用于导纳控制器的设计。对于机器人与环境接触的稳定性分析，情况与刚体机器人的情况类似。然而，导纳方法基本上依赖于外力测量。在实践中，这些力通常由安装在末端执行器上的力-力矩传感器测量。作用在机器人结构上而不是末端执行器上的外力通常无法测量。因此，基于导纳概念，只能相对于作用在末端执行器上的力实现柔顺行为。但在许多应用中，例如在服务机器人中，人类用户与机器人接触，这可能是不可接受的。此外，对于冗余机器人，如DLR轻型机器人，除了笛卡尔阻抗之外，零空间刚度也值得关注。

本专著专注于阻抗控制方法。这里不需要先验地测量外力。因此，实现零空间刚度也将是可能的。

作为期望的机器人行为，将考虑质量-弹簧-阻尼器类系统。此外，由于即使在刚体情况下惯性整形也需要外力反馈（见第3章），因此详细分析了不需要整形惯性矩阵的特定期望阻抗行为情况。这也导致了有关阻尼设计和冗余情况下零空间运动控制的问题。本书的组织结构总结如下。刚体情况在第3章和第4章中处理。然后在第5至第7章中分析了为柔性关节机器人模型设计笛卡尔阻抗控制器的不同方法。

第2章处理柔性关节机器人的建模。这里，MLS94中的建模方法被应用于具有柔性关节的特定机器人情况。

第3章讨论了不考虑关节柔性的刚体机器人的阻抗控制问题。更详细地分析了不期望惯性整形的期望阻抗行为情况。对于这种情况，给出了阻尼设计问题的解决方案。此外，还讨论了一些实际相关的问题，即奇异性避免和笛卡尔坐标的选择。本章的结果也为第5章至第7章中柔性关节情况下笛卡尔阻抗控制器的构建奠定了基础。

第4章处理了为冗余机器人设计零空间刚度的问题。除了第3章的笛卡尔阻抗控制器外，还设计了一个零空间阻抗控制律。与前一章类似，分析了无惯性整形的阻抗控制器情况。首先分析了使用适当的投影矩阵来设计零空间刚度和阻尼。然后更详细地处理了基于任务空间扩展方法以及关节空间分解方法的零空间阻抗控制器的实现。在后一种方法中，提出了一种控制器，基于半正定李雅普诺夫函数的稳定性理论给出了渐近稳定性的证明。

在第5章中，将奇异摄动理论应用于第2章的柔性关节机器人模型。不同于Spo89, Spo95, GHS89, Ge96中使用的常见复合控制器设计方法，提出了一种改进的控制律，该控制律允许在不显式计算稳态扭矩的情况下增强边界层系统的性能。基于此奇异摄动控制器，设计了一个笛卡尔阻抗控制器。虽然控制器可以很容易地实现，但稳定性分析仅限于奇异摄动近似。

在第6章的第一部分，级联控制理论被用于设计笛卡尔阻抗控制器。该控制器基于一个内反馈回路，该回路将扭矩动力学与连杆动力学解耦，从而将系统转换为三角形式。对于自由运动情况，证明了全局一致渐近稳定性。对于调节情况，还显示了闭环系统的有用无源性特性。该控制器的设计思路与Lin和GoldenbergLG95, LG96的工作相关，但应该提到的是，LG95, LG96的稳定性分析不能扩展到所考虑的阻抗控制问题。相反，基于级联系统理论的方法用于稳定性分析。

此外，在本章的第二部分，应用反步法设计了一个笛卡尔阻抗控制器。然而，由此产生的控制器比基于解耦的控制器稍微复杂一些。

第7章提出了一种基于无源性理论的方法。该控制器的设计思路依赖于扭矩反馈的物理解释。提出了一种在线重力补偿项，与文献中所有相关的先前解决方案相反，它不会对控制器增益施加任何约束，因此也适用于冗余机器人。结果表明，闭环系统可以看作是无源子系统的反馈互连。因此，该控制器具有非常有利的鲁棒性。此外，其复杂度与简单的奇异摄动控制器相当。

还应该提到，所得控制器可以看作是ASH01a, AS01中提出的状态反馈控制器的推广。ASH01a, AS01中的控制器是为具有恒定重力补偿项的位置控制问题设计的。扭矩反馈的物理解释与在线重力补偿项一起，基本上允许在笛卡尔坐标中表述控制器，并且对于阻抗控制器（刚度值可低至零）实现非常低的控制器增益。

此外，还展示了如何将关节刚度考虑在内用于阻抗设计。还展示了当存在不可忽略的关节阻尼时，控制器必须如何扩展。最后，还讨论了跟踪情况的扩展。

第8章介绍了所提出控制器的仿真研究和在DLR轻型机器人上的实验验证。

在第9章中，介绍了一些使用笛卡尔阻抗控制器的典型应用。

第10章最后给出了简短的总结和结论。它还包含对所提出的不同控制方法的简短比较。

---

2 柔性关节机器人建模

本章将推导具有柔性关节的机器人动力学模型。表述仅限于开放式运动链操纵器，其中连杆形成无环路的串联运动链。此外，假设机器人的连杆通过旋转关节或移动关节连接。

在推导弹性机器人的动力学模型时，基本上可以区分两种不同的弹性来源。由机器人连杆引起的分布弹性和通常由机器人齿轮引起的关节中的集中弹性。对于前者，可以基于欧拉-伯努利梁的通常假设推导出一个无限维模型，称为弹性连杆机器人模型。而后者则导致一个有限维模型，称为柔性关节机器人模型。

对于DLR轻型机器人，柔性的主要来源是Harmonic Drive®齿轮。这些机器人的连杆刚度明显高于齿轮的刚度。因此，下面将考虑柔性关节机器人模型。

首先，第2.1节处理机器人运动学。第2.1.1节简要回顾了刚体运动描述的基础。在此基础上，第2.1节描述了串联运动链的运动学建模。这些运动学关系不仅对第2.2节动力学模型的推导感兴趣，而且对后续章节中笛卡尔控制器的设计也感兴趣。第2.2节推导了柔性关节机器人的完整和简化动力学模型，并讨论了它们的控制特性。

2.1 机器人运动学概要

以下机器人运动学的处理基于MLS94和Sel96。以下两节不是详细解释机器人学中的基本运动学关系，而只是为了澄清符号。

2.1.1 刚体运动

单个刚体的运动可以用相对于惯性系 $S$ 的体固定坐标系 $B$ 的相对运动来表示（见图2.1）。在机器人应用中，惯性系 $S$ 通常称为基坐标系或世界坐标系。刚体的平移运动可以用从 $S$ 的原点到 $B$ 的原点的向量 $\pmb{p}_{sb}\in \mathbb{R}^{3}$ 来描述。符号 $\pmb{p}_{sb}$ 表示该向量以 $S$ 的坐标表示，而 $b\pmb{p}_{sb}$ 表示同一向量在坐标系 $B$ 中表示。

坐标系 $B$ 相对于世界坐标系 $S$ 的旋转可以用正交矩阵 $\pmb{R}_{sb} = [\pmb{e}_x,\pmb{e}_y,\pmb{e}_z]$ 来描述，其中 $\pmb{e}_i$ 是与坐标系 $B$ 的第 $i$ 个坐标方向共线的单位向量，在坐标系 $S$ 中表示。由于所有考虑的坐标系都按惯例是右手系，旋转矩阵的行列式为 $+1$ 。所有此类旋转矩阵的集合表示为 $SO(3)$ ，即特殊正交群（即 $\mathbb{R}^{3}$ 的旋转群）。注意 $\pmb{R}_{sb}$ 的逆可以通过 $\pmb{R}_{bs} = \pmb{R}_{sb}^{- 1} = \pmb{R}_{sb}^{T}$ 计算。

刚体的空间配置是平移和旋转的组合，可以用特殊欧几里得群 $SE(3)$ 来描述。群 $SE(3)$ 是 $SO(3)$ 和 $\mathbb{R}^{3}$ 的半直积：

$$SE(3) = SO(3) \ltimes \mathbb{R}^{3}. \quad (2.1)$$

通过这种表示， $SE(3)$ 的群运算由矩阵乘法给出。

$$h_{sb} = \begin{bmatrix} R_{sb} & p_{sb} \\ 0 & 1 \end{bmatrix}, \quad h_{bs} = \begin{bmatrix} R_{sb}^{T} & -R_{sb}^{T} p_{sb} \\ 0 & 1 \end{bmatrix}. \quad (2.2)$$

虽然 $h_{sb} \in SE(3)$ 表示刚体的配置，但其速度可以由李代数 $se(3)$ 描述，它是李群 $SE(3)$ 的无穷小生成元。刚体速度的一种常见表示是体旋量 $\mathbf{V}^{b} \in se(3)$ 。体旋量的分量具有以下物理意义：向量 $b \mathbf{v}_{sb} \in \mathbb{R}^{3}$ 是坐标系 $B$ 的原点相对于 $S$ 的速度，在 $B$ 中表示。向量 $b \boldsymbol{\omega}_{sb} \in \mathbb{R}^{3}$ 是 $B$ 相对于 $S$ 的角速度，在 $B$ 中表示。

令 $h_{sb}(t) \in SE(3)$ 是表示刚体轨迹的单参数曲线（以时间为参数）。体旋量 $\mathbf{V}^{b}$ 与运动 $h_{sb}$ 之间的关系由 $\tilde{\mathbf{V}}^{b} = h_{sb}^{- 1} \dot{h}_{sb}$ 给出，其中作用于旋量的算子 $\tilde{(\cdot)}$ 定义为：

$$\tilde{\mathbf{V}}^{b} = \begin{bmatrix} \tilde{\boldsymbol{\omega}}^{b} & \mathbf{v}^{b} \\ 0 & 0 \end{bmatrix}, \quad \mathbf{V}^{b} = \begin{bmatrix} \mathbf{v}^{b} \\ \boldsymbol{\omega}^{b} \end{bmatrix}.$$

刚体速度的另一种常见表示是空间旋量 $\mathbf{V}^{s}$ ，它与体旋量 $\mathbf{V}^{b}$ 通过 $\mathbf{V}^{s} = \operatorname{Ad}_{h_{sb}} \mathbf{V}^{b}$ 相关。这里 $\operatorname{Ad}_{h_{sb}}$ 是与 $h_{sb}$ 相关联的伴随变换。这个变换及其逆 $\operatorname{Ad}_{h_{sb}} = \operatorname{Ad}_{h_{sb}}^{-1}$ 由下式给出：

$$\operatorname{Ad}_{h_{sb}} := \begin{bmatrix} R_{sb} & \tilde{p}_{sb} R_{sb} \\ 0 & R_{sb} \end{bmatrix}, \quad \operatorname{Ad}_{h_{sb}^{-1}} = \begin{bmatrix} R_{sb}^{T} & -R_{sb}^{T} \tilde{p}_{sb} \\ 0 & R_{sb}^{T} \end{bmatrix}.$$

2.1.2 正向运动学

本节考虑具有 $n$ 个关节的开放式运动链操纵器的正向运动学映射。为每个关节引入一个关节变量 $q_{i}\in \mathbb{R}$ 。对于旋转关节，该变量表示两个相邻连杆之间的角度。注意，或者，该角度也可以被认为是集合 $S^{1}$ （单位圆）的元素，而不是 $\mathbb{R}$ 的元素。在这种情况下，角度 $q_{i}$ 和 $q_{i} + 2\pi$ 将不被区分。移动关节则简单地由两个相邻连杆之间的线性位移描述。

配置空间 $\mathcal{Q}$ 是关节空间的笛卡尔积，即 $\mathcal{Q}:= \mathbb{R}^{n}$ 。各个关节变量 $q_{i}$ 汇总在向量中：

$$\mathbf{q} = (q_{1},\dots,q_{n})^{T}\in \mathcal{Q}.$$

为了简化移动关节和旋转关节之间的区分，定义集合 $\mathcal{P}$ ，使其包含所有涉及移动关节关节变量 $q_{i}$ 的索引 $i$ 。

任务将相对于工具坐标系来表述，该坐标系是附着在机器人末端执行器上的一个坐标系。正向运动学映射 $\mathbf{h}_{s t}:\mathcal{Q}\rightarrow SE(3)$ 给出了给定关节配置下工具坐标系的配置。正向运动学的计算对于笛卡尔控制器的实现以及下一节动力学模型的推导都很重要。

首先注意，单个关节 $i$ 的运动（所有其他关节变量固定为 $q_{j} = 0$ ， $j\neq i$ ）可以由关节旋量 $\boldsymbol{\xi}_{i}$ 表示，其形式为：

$$\boldsymbol{\xi}_{i} = \begin{bmatrix} \mathbf{v}_{i} \\ \boldsymbol{\omega}_{i} \end{bmatrix}, \quad \text{其中} \quad \boldsymbol{\omega}_{i} \in \mathbb{R}^{3}, \quad \mathbf{v}_{i} = -\boldsymbol{\omega}_{i} \times \mathbf{l}_{i}.$$

对于旋转关节， $\boldsymbol{\xi}_{i}$ 的第一部分由 $\mathbf{v}_{i} = - \boldsymbol{\omega}_{i}\times \mathbf{l}_{i}$ 给出，其中 $\omega_{i}$ 是与关节轴对齐的单位向量， $l_{i}$ 是关节轴上的任意点MLS94，两者都在 $S$ 中表示。对于移动关节，有 $||\mathbf{v}_{i}|| = 1$ 和 $\boldsymbol{\omega}_{i} = \mathbf{0}$ ，其中向量 $\mathbf{v}_{i}$ 指向平移方向。

给定各个关节旋量以及工具框架对于 $\mathbf{q} = \mathbf{0}$ 的参考位置 $\mathbf{h}_{s t}(\mathbf{0})$ ，正向运动学映射 $\mathbf{h}_{s t}(\mathbf{q})$ 可以通过著名的指数积公式计算：

$$\mathbf{h}_{s t}(\mathbf{q}) = e^{\boldsymbol{\xi}_{1}q_{1}}e^{\boldsymbol{\xi}_{2}q_{2}}\dots e^{\boldsymbol{\xi}_{n}q_{n}}\mathbf{h}_{s t}(\mathbf{0}), \quad (2.3)$$

其中旋量 $\boldsymbol{\xi}_{i}$ 的指数项 $e^{\boldsymbol{\xi}_{i}q_{i}}\in SE(3)$ 定义为：

$$e^{\boldsymbol{\xi}_{i}q_{i}} = \begin{cases} \begin{bmatrix} e^{\tilde{\boldsymbol{\omega}}_{i}q_{i}} & \left(\mathbf{I} - e^{\tilde{\boldsymbol{\omega}}_{i}q_{i}}\right)\left(\boldsymbol{\omega}_{i}\times \mathbf{v}_{i}\right) + \boldsymbol{\omega}_{i}\boldsymbol{\omega}_{i}^{T}\mathbf{v}_{i}q_{i} \\ 0 & 1 \end{bmatrix} & \text{对于} \quad ||\boldsymbol{\omega}_{i}|| = 1 \\ \begin{bmatrix} \mathbf{I} & \mathbf{v}_{i}q_{i} \\ 0 & 1 \end{bmatrix} & \text{对于} \quad \boldsymbol{\omega}_{i} = \mathbf{0} \end{cases}.$$

此外，单位向量 $\omega_{i}$ 的指数项 $e^{\tilde{\omega}_{i}q_{i}} \in SO(3)$ 由罗德里格斯公式给出：

$$e^{\tilde{\omega}_{i}q_{i}} = I + \tilde{\omega}_{i}\sin(q_{i}) + \tilde{\omega}_{i}^{2}(1 - \cos(q_{i})).$$

方程（2.3）给出了工具坐标系的配置。工具坐标系的实际速度由体旋量 $\mathbf{V}^{b}$ 给出。如MLS94所述，体旋量 $\mathbf{V}^{b}$ 也可以基于关节旋量计算。它由下式给出：

$$\begin{array}{l} \mathbf{V}^{b} = \mathbf{J}^{b}(\mathbf{q})\dot{\mathbf{q}}, \\ \mathbf{J}^{b}(\mathbf{q}) = \left[\boldsymbol{\xi}_{1}^{\dagger}(\mathbf{q}) \cdots \boldsymbol{\xi}_{n}^{\dagger}(\mathbf{q})\right], \\ \boldsymbol{\xi}_{i}^{\dagger}(\mathbf{q}) = \operatorname{Ad}_{e^{\boldsymbol{\xi}_{i}q_{i}\ldots e^{\boldsymbol{\xi}_{n}q_{n}}\mathbf{h}_{s t}(\mathbf{0})}}^{-1} \boldsymbol{\xi}_{i}, \end{array} \quad (2.5)$$

其中矩阵 $\mathbf{J}^{b}(\mathbf{q})$ 通常称为体雅可比矩阵或几何雅可比矩阵。

2.2 柔性关节机器人的动力学模型

文献中用于柔性关节机器人建模的模型有两种，即完整模型和简化模型。简化模型由Spong在Spo87b中提出。它与完整模型的不同之处在于假设转子的动能仅由其自身旋转决定。下面给出了导致完整模型和简化模型的所有建模假设，以及基于拉格朗日运动方程Arn89, MLS94和上一节中运动学表述的模型的详细推导。

2.2.1 完整模型的建模假设

为了推导完整的动力学模型，做出以下三个假设：

假设 2.1. 电机转子是旋转对称刚体，每个关节的旋转轴和对称轴重合。

第 $i$ 个电机的转子角度（描述绕对称轴的旋转）将表示为 $\theta_{\mathrm{motor},i}$ 。

假设 2.2. 动力学的电气部分（即电机的内部电流控制回路）足够快，可以忽略。

因此，电机被建模为理想扭矩源，作用于第 $i$ 个转子惯量上的电机 $i$ 的扭矩由 $\tau_{\mathrm{motor},i}$ 给出。

为了获得与齿轮传动比 $N_{i}$ 无关的描述，引入了电机扭矩变量 $\tau_{m,i}$ 和电机角度变量 $\theta_{i}$ 。这些变量通过缩放值 $N_{i}$ 和 $1 / N_{i}$ 与物理值 $\tau_{\mathrm{motor},i}$ 和 $\theta_{\mathrm{motor},i}$ 相关，即：

$$\begin{array}{rcl} \tau_{m,i} = N_i\tau_{\mathrm{motor},i} & {} & {i = 1,\dots,n,} \\ \theta_i = \frac{1}{N_i}\theta_{\mathrm{motor},i} & {} & {i = 1,\dots,n.} \end{array} \quad (2.6)$$

与 $\mathbf{q}\in \mathcal{Q}$ 类似，电机角度 $\theta_{i}$ 和电机扭矩 $\tau_{m,i}$ 汇总在向量 $\boldsymbol{\theta}:= (\theta_{1},\dots,\theta_{n})^{T}\in \mathbb{R}^{n}$ 和 $\boldsymbol{\tau}_{m}:= (\tau_{m,1},\dots,\tau_{m,n})^{T}\in \mathbb{R}^{n}$ 中。

假设 2.3. 每个关节的齿轮弹性被建模为位于转子和后续连杆之间的线性弹簧。

假设2.3在图2.2中为DRL-Lightweight-Robot-III的第二个关节进行了说明。决定下一个连杆位置的关节角度由 $q_{i}$ 给出。转子和连杆之间的弹簧由线性关系 $\tau_{i} = K_{i}(\theta_{i} - q_{i})$ 描述，其中 $k_{i}$ 是弹簧的刚度系数， $\tau_{i}$ 是关节扭矩。第 $i$ 个关节中弹簧的刚度势能由 $V_{k,i}(q_i,\theta_i):= 1 / 2K_i(\theta_i - q_i)^2$ 给出。

2.2.2 完整模型的推导

对于具有 $n$ 个柔性关节的机器人，必须考虑 $2n$ 个刚体的动力学：

• $n$ 个连杆，构成串联运动链，以及
• $n$ 个电机转子。

下面的推导基于拉格朗日运动方程Arn89, MLS94。因此，必须计算系统的动能 $T(\mathbf{q}, \dot{\mathbf{q}}, \dot{\boldsymbol{\theta}})$ 和势能 $V_{pot}(\mathbf{q}, \boldsymbol{\theta})$ 。

刚体的动能

首先考虑任意刚体的动能。设 $m_{b}$ 为质量， $\mathcal{I}_{b}$ 为在位于刚体质心的体固定坐标系 $\mathcal{B}$ 中表示的惯性张量。则该刚体的动能 $T_{b}$ 可以写成平移和旋转分量之和。它由下式给出：

$$\begin{array}{l} T_{b} = \frac{1}{2} m_{b}(^{b}\mathbf{v}_{s b})^{T}(^{b}\mathbf{v}_{s b}) + \frac{1}{2} (^{b}\boldsymbol{\omega}_{s b})^{T}\mathcal{I}_{b}(^{b}\boldsymbol{\omega}_{s b}) \\ = \frac{1}{2} (\mathbf{V}^{b})^{T}\mathcal{M}_{b}\mathbf{V}^{b}, \end{array} \quad (2.9)$$

其中广义惯性矩阵为：

$$\mathcal{M}_{b} = \begin{bmatrix} m_{b}\mathbf{I} & \mathbf{0} \\ \mathbf{0} & \mathcal{I}_{b} \end{bmatrix}, \quad (2.10)$$

其中 $\mathbf{I}$ 是 $3\times 3$ 单位矩阵。向量 $^{b}\mathbf{v}_{s b}$ 和 $^{b}\boldsymbol{\omega}_{s b}$ 是坐标系 $\mathcal{B}$ 相对于坐标系 $\mathcal{S}$ 的平移和角速度向量，在坐标系 $\mathcal{B}$ 中表示，因此 $\mathbf{V}^{b}$ 是坐标系 $\mathcal{B}$ 的体旋量。

因此，计算动能可以简化为计算转子和连杆的体旋量。为此，指定以下坐标系：为 $n$ 个连杆中的每一个分配一个坐标系 $\mathcal{L}_{i}$ ，固定在连杆 $i$ 上，使其原点与连杆的质心重合。对于 $n$ 个转子，选择坐标系 $\mathcal{R}_{i}$ ，使其不附着在转子上，而是附着在前一个连杆上，并且 $\mathcal{R}_{i}$ 的 $z$ 坐标轴与第 $i$ 个关节轴相同（见图2.3）。根据这些坐标系，可以通过方程（2.3）和（2.4）计算连杆和转子的正向运动学映射和体旋量。

连杆的动能

可以很容易地观察到，连杆的体旋量 $\mathbf{V}_{l,i}^{b}$ 不依赖于电机角度，可以写成：

$$\mathbf{V}_{l,i}^{b} = \mathbf{J}_{l,i}^{b}(\mathbf{q})\dot{\mathbf{q}} \quad i = 1,\dots,n, \quad (2.11)$$

其中 $\mathbf{J}_{l,i}^{b}(q)$ 是坐标系 $\mathcal{L}_{i}$ 的体雅可比矩阵。因此，连杆的动能由下式给出：

$$T_{l}(q,\dot{q}) = \frac{1}{2}\sum_{i = 1}^{n}(\mathbf{V}_{l,i}^{b})^{T}\mathcal{M}_{l,i}\mathbf{V}_{l,i}^{b},$$

其中 $\mathcal{M}_{l,i}$ 是连杆 $i$ 的广义惯性矩阵。通过引入连杆的惯性矩阵 $\mathcal{M}_{l}(q)$ ：

$$M_{l}(q) = \sum_{i = 1}^{n}\mathbf{J}_{l,i}^{b}(q)^{T}\mathcal{M}_{l,i}\mathbf{J}_{l,i}^{b}(q),$$

动能 $T_{l}(q,\dot{q})$ 可以简单地写成：

$$T_{l}(q,\dot{q}) = \frac{1}{2}\dot{q}^{T}M_{l}(q)\dot{q}.$$

转子的动能

转子的体旋量具有以下形式：

$$\mathbf{V}_{r,i}^{b} = \begin{pmatrix} {}^{r}\mathbf{v}_{sr} \\ {}^{r}\boldsymbol{\omega}_{sr} \end{pmatrix} = \mathbf{J}_{r,i}^{b}(q)\dot{q} + \begin{pmatrix} 0 \\ 0 \\ 0 \\ 1 \end{pmatrix} \dot{\theta}_{i} \quad i = 1,\dots,n, \quad (2.12)$$

其中 $\mathbf{J}_{r,i}^{b}(\mathbf{q})$ 是坐标系 $\mathcal{R}_{i}$ 的体雅可比矩阵。因此，转子的动能 $T_{r}(q,\dot{q},\dot{\theta})$ 可以计算为：

$$T_{r}(q,\dot{q},\dot{\theta}) = \frac{1}{2}\sum_{i = 1}^{n}(\mathbf{V}_{r,i}^{b})^{T}\mathcal{M}_{r,i}\mathbf{V}_{r,i}^{b},$$

其中 $\mathcal{M}_{r,i}$ 是转子 $i$ 的广义惯性矩阵。由于对称性假设2.1，当在坐标系 $\mathcal{R}_{i}$ 中表示时，转子的惯性张量具有以下对角形式：

$$\mathcal{I}_{r,i} = \begin{bmatrix} A_i & 0 & 0 \\ 0 & A_i & 0 \\ 0 & 0 & B_i \end{bmatrix} \quad i = 1, \dots, n. \quad (2.13)$$

因此，转子的动能具有以下形式：

$$T_{r}(\dot{\theta},q,\dot{q}) = \frac{1}{2} \begin{pmatrix} \dot{q} \\ \dot{\theta} \end{pmatrix}^{T} \begin{bmatrix} M_{r}(q) & S(q) \\ S(q)^{T} & B \end{bmatrix} \begin{pmatrix} \dot{q} \\ \dot{\theta} \end{pmatrix}, \quad (2.14)$$

其中矩阵 $M_{r}(q)$ 、 $S(q)$ 和 $B$ 由下式给出：

$$\begin{array}{l} M_{r}(q):= \sum_{i = 1}^{n}\mathbf{J}_{r,i}^{b}(q)^{T}\mathcal{M}_{r,i}\mathbf{J}_{r,i}^{b}(q), \\ S(q):= [s_{1}(q)\cdots s_{n}(q)], \\ s_{i}(q):= \mathbf{J}_{r,i}^{b}(q)^{T} \begin{pmatrix} 0 \\ 0 \\ 0 \\ B_{i} \end{pmatrix}, \\ B:= \mathrm{diag}(B_{i})\in \mathbb{R}^{n\times n}. \end{array} \quad (2.18)$$

可以证明 $S(q)$ 是一个严格上三角矩阵（例如，参见DLT96）。最后， $2n$ 个刚体的总动能为：

$$T(q,\dot{q},\dot{\theta}) = T_{l}(q,\dot{q}) + T_{r}(q,\dot{q},\dot{\theta}) = \frac{1}{2} \begin{pmatrix} \dot{q} \\ \dot{\theta} \end{pmatrix}^{T} \begin{bmatrix} M_{r}(q) + M_{l}(q) & S(q) \\ S(q)^{T} & B \end{bmatrix} \begin{pmatrix} \dot{q} \\ \dot{\theta} \end{pmatrix}.$$

势能

势能由两部分组成：

• 弹簧的刚度势能 $V_{k}$ ，以及
• 重力势能 $V_{g}$ 。

所有关节的组合刚度势能由下式给出：

$$V_{k}(\mathbf{q},\boldsymbol{\theta}) = \sum_{i = 1}^{n}V_{k,i}(q_{i},\theta_{i}) \quad (2.19)$$ $$= \frac{1}{2} (\boldsymbol{\theta} - \mathbf{q})^{T}\mathbf{K}(\boldsymbol{\theta} - \mathbf{q}), \quad (2.20)$$

其中 $\mathbf{K}$ 是包含关节刚度值作为对角线元素的对角矩阵：

$$\mathbf{K}:= \mathrm{diag}(K_i)\in \mathbb{R}^{n\times n}. \quad (2.21)$$

由于假设2.1，重力势能与 $\theta$ 无关。设 $m_{l,i}$ 和 $m_{r,i}$ 分别表示连杆和转子的质量。向量 $\mathbf{e}_g\in \mathbb{R}^3$ 是指向重力方向的单位向量， $g\in \mathbb{R}$ 是重力加速度。则重力势能 $V_{g}(\mathbf{q})$ 由下式给出：

$$V_{g}(\mathbf{q}) = -g\mathbf{e}_{g}^{T}\left(\sum_{i = 1}^{n}m_{l,i}\mathbf{p}_{s l,i}(\mathbf{q}) + m_{r,i}\mathbf{p}_{s r,i}(\mathbf{q})\right), \quad (2.22)$$

其中 $\mathbf{p}_{s l,i}(\mathbf{q})\in \mathbb{R}^{3}$ 和 $\mathbf{p}_{s r,i}(\mathbf{q})\in \mathbb{R}^{3}$ 是从 $\mathcal{S}$ 的原点指向 $\mathcal{L}_{i}$ 和 $\mathcal{R}_{i}$ 的原点的向量，可以使用（2.3）计算。柔性关节机器人模型的完整势能 $V_{pot}(\mathbf{q},\boldsymbol{\theta})$ 由刚度势能和重力势能之和给出：

$$\begin{array}{rl} & V_{pot}(\mathbf{q},\boldsymbol{\theta}) = V_{g}(\mathbf{q}) + V_{k}(\mathbf{q},\boldsymbol{\theta}) \\ & = V_{g}(\mathbf{q}) + \frac{1}{2} (\boldsymbol{\theta} - \mathbf{q})^{T}\mathbf{K}(\boldsymbol{\theta} - \mathbf{q}). \end{array} \quad (2.24)$$

拉格朗日方程

为了简化符号，引入配置向量：

$$\hat{\mathbf{q}} = \begin{pmatrix} \mathbf{q} \\ \boldsymbol{\theta} \end{pmatrix} \in \mathbb{R}^{2n}. \quad (2.25)$$

则柔性关节机器人模型的拉格朗日量 $L(\hat{\mathbf{q}},\dot{\hat{\mathbf{q}}})$ 由动能和势能之差给出：

$$L(\hat{\mathbf{q}},\dot{\hat{\mathbf{q}}}) = T(\mathbf{q},\dot{\mathbf{q}},\dot{\boldsymbol{\theta}}) - V_{pot}(\mathbf{q},\boldsymbol{\theta}). \quad (2.26)$$

完整模型的运动方程通过下式计算：

$$\frac{\mathrm{d}}{\mathrm{d}t}\left(\frac{\partial L(\hat{\mathbf{q}},\dot{\hat{\mathbf{q}}})}{\partial \dot{\hat{\mathbf{q}}}}\right) - \frac{\partial L(\hat{\mathbf{q}},\dot{\hat{\mathbf{q}}})}{\partial \hat{\mathbf{q}}} = \mathbf{Q}, \quad (2.27)$$

其中 $\mathbf{Q}\in \mathbb{R}^{2n}$ 是对应于 $\dot{\hat{\mathbf{q}}}$ 的广义力。电机扭矩 $\tau_{m}$ 显然对应于作用在转子上的广义力。对于阻抗控制器的设计，还考虑了作用在机器人连杆上的附加外部扭矩 $\tau_{ext}\in \mathbb{R}^{n}$ 。因此，广义力由下式给出：

$$\mathbf{Q} = \begin{pmatrix} \boldsymbol{\tau}_{ext} \\ \boldsymbol{\tau}_{m} \end{pmatrix}.$$

通过评估（2.27），得到以下形式的运动方程：

$$\begin{bmatrix} \mathbf{H}(q) & \begin{pmatrix} \ddot{q} \\ \ddot{\theta} \end{pmatrix} + \boldsymbol{\Gamma}(q,\dot{q}) \begin{pmatrix} \dot{q} \\ \dot{\theta} \end{pmatrix} + \begin{pmatrix} g(q) - K(\theta - q) \\ K(\theta - q) \end{pmatrix} = \begin{pmatrix} \boldsymbol{\tau}_{ext} \\ \boldsymbol{\tau}_{m} \end{pmatrix} \end{bmatrix}, \quad (2.28)$$

其中 $\mathbf{H}(\mathbf{q}) = (H_{ij}(\mathbf{q}))$ 是完整的惯性矩阵：

$$\mathbf{H}(\mathbf{q}) = \begin{bmatrix} M_{r}(q) + M_{l}(q) & S(q) \\ S(q)^{T} & B \end{bmatrix},$$

重力扭矩 $g(q)$ 由重力势能给出：

$$g(q) = \left(\frac{\partial V_{g}(q)}{\partial q}\right)^{T}. \quad (2.29)$$

项 $\boldsymbol{\Gamma}(q,\dot{q})\dot{\mathbf{q}}$ 由离心力和科里奥利力组成。矩阵 $\boldsymbol{\Gamma}(q,\dot{q}) = (\Gamma_{ij}(q,\dot{q}))$ ，有时称为科里奥利/离心矩阵，通常不是唯一的。但可以选择它，使其元素通过以下方式与克里斯托费尔符号相关MLS94, SS96：

$$\begin{array}{l} \Gamma_{ij}(q,\dot{q}) = \sum_{k=1}^{2n}\Gamma_{ijk}(q)\dot{q}_{k}, \\ \Gamma_{ijk}(q) = \frac{1}{2}\left(\frac{\partial H_{ij}(q)}{\partial \dot{q}_{k}} + \frac{\partial H_{ik}(q)}{\partial \dot{q}_{j}} - \frac{\partial H_{kj}(q)}{\partial \dot{q}_{i}}\right). \end{array} \quad (2.30)$$

此外，应该提到的是， $\boldsymbol{\Gamma}(q,\dot{q})$ 的右下角 $n\times n$ 块在结构上是零。

方程（2.28）表示所谓的柔性关节机器人的完整模型。可以看出，关节角度 $q$ 的方程和电机角度 $\theta$ 的方程通过刚度项 $\mathbf{K}(\boldsymbol{\theta} - \mathbf{q})$ 以及惯性矩阵 $\mathbf{H}(q)$ 的外对角线部分 $S(q)$ 耦合。这个外对角线部分在文献中经常被忽略，并导致接下来考虑的简化模型。

2.2.3 简化模型

除了导致完整模型的前述建模假设之外，还做出了以下假设。

假设 2.4. 转子的动能旋转部分仅由转子相对于前一个连杆的相对运动决定。

对于DLR轻型机器人，这个假设由以下事实证明：齿轮传动比 $N_{i}$ 相当高。对于DLR-Lightweight-Robot-II，它们的值在100-160范围内。因此，转子将比机器人的连杆转动得快得多，因此转子的动能不会受到连杆运动的显著影响。注意，如果机器人的所有电机都位于机器人的基座中，这个建模假设也会自动满足。对于电缆驱动机器人，例如Dexter手臂ZSL+03，就是这种情况。

从假设2.4可以得出，（2.12）中转子的角速度由下式给出：

$$^{r}\boldsymbol{\omega}_{sr,i} \approx \begin{pmatrix} 0 \\ 0 \\ 1 \end{pmatrix} \dot{\theta}_i \quad i = 1,\dots,n, \quad (2.32)$$

而平移速度 $^{r}\mathbf{v}_{sr,i}$ 与（2.12）中的相同。通过计算转子的新动能，可以看出惯性矩阵中的耦合部分 $S(q)$ 消失，即 $S(q) = 0$ 。因此，转子的动能（2.14）简化为：

$$T_{r}(\dot{\theta},q,\dot{q}) = \frac{1}{2} \begin{pmatrix} \dot{q} \\ \dot{\theta} \end{pmatrix}^{T} \begin{bmatrix} \bar{M}_{r}(q) & 0 \\ 0 & B \end{bmatrix} \begin{pmatrix} \dot{q} \\ \dot{\theta} \end{pmatrix},$$

其中转子的连杆侧惯性矩阵 $\bar{M}_{r}(q)$ 由下式给出：

$$\begin{array}{l} \bar{M}_{r}(q) = \sum_{i = 1}^{n} m_{r,i} \bar{\mathbf{J}}_{r,i}^{b}(q)^{T} \bar{\mathbf{J}}_{r,i}^{b}(q), \\ \bar{\mathbf{J}}_{r,i}^{b}(q) = [I_{3\times 3} \quad 0_{3\times 3}] \mathbf{J}_{r,i}^{b}(q). \end{array} \quad (2.34)$$

然后可以通过遵循与完整模型相同的步骤来推导简化模型。这导致运动方程：

$$\boxed{ M(q)\ddot{q} + C(q,\dot{q})\dot{q} + g(q) = K(\theta - q) + \tau_{ext} }$$ $$B\ddot{\theta} + K(\theta - q) = \tau_{m} \quad (2.35)$$

这里，连杆侧惯性矩阵 $M(q)$ 由 $M(q) = M_{l}(q) + \bar{M}_{r}(q)$ 给出， $C(q,\dot{q})$ 是关于 $M(q)$ 的科里奥利/离心矩阵。

在接下来的章节中，简化模型（2.35）将被用于控制器设计。与完整模型相比，简化模型中 $q$ 的动力学和 $\theta$ 的动力学之间的唯一耦合是关节刚度。这种特殊结构可以被利用于控制器设计。

2.2.4 动力学模型的性质

在本节中，给出了动力学模型的一些有趣性质。除非另有说明，所有性质都适用于完整模型和简化模型。

显然，这两个模型都是欠驱动的。有 $2n$ 个配置变量，但只有 $n$ 个控制输入，即电机扭矩。此外，这两个模型都是微分平坦的FLMR95，并且简化模型甚至可以通过静态状态反馈进行线性化Spo87b, DLT96。完整模型可以通过动态状态反馈进行线性化DLL98。

从控制的角度来看，以下两个众所周知的性质也很重要。

性质 2.5. 惯性矩阵 $M(q) \in \mathbb{R}^{n \times n}$ 是对称且正定的：

$$M(q) = M(q)^{T}, \quad y^{T}M(q)y > 0 \quad \forall q, y \neq 0 \in \mathbb{R}^{n}.$$

性质 2.6. 如果 $C(q,\dot{q})$ 像方程（2.30）中那样通过克里斯托费尔符号选择，则矩阵 $\dot{M}(q) - 2C(q,\dot{q}) \in \mathbb{R}^{n \times n}$ 是斜对称的：

$$y^{T}(\dot{M}(q) - 2C(q,\dot{q}))y = 0 \quad \forall y,q,\dot{q} \in \mathbb{R}^{n}.$$

这两个性质的证明在刚体机器人的背景下是标准的（例如，参见SS96），因此在此省略。只应该提到，性质2.6与系统的无源性性质密切相关，并且取决于通过克里斯托费尔符号对 $C(q,\dot{q})$ 的特殊选择。

如果机器人不仅包含旋转关节还包含移动关节，则考虑一个子集 $\mathcal{Q}^{p} \subseteq \mathcal{Q}$ 是有用的，在该子集中所有移动关节 $i \in \mathcal{P}$ 保持在某个下限 $q_{i,min}$ 和上限 $q_{i,max}$ 之间。该子集表示为：

$$\mathcal{Q}^{p}:= \{q\in \mathcal{Q} \mid q_{i,min}\leq q_{i}\leq q_{i,max} \quad \forall i\in \mathcal{P}\}. \quad (2.36)$$

那么以下性质（将在第3.2节期望阻抗行为的分析和第6章控制器设计中用到）对于所有 $\mathbf{q}\in \mathcal{Q}^{p}$ 成立：

性质 2.7. 在 $\mathcal{Q}^{p}$ 内，惯性矩阵 $M(\mathbf{q})$ 的特征值 $\lambda_{i}(M(\mathbf{q}))$ 保持有界，即：

$$0 < \lambda_{m,M} \leq \lambda_{i}(M(\mathbf{q})) \leq \lambda_{M,M} < \infty \quad \forall i = 1,\dots,n,\ \forall \mathbf{q}\in \mathcal{Q}^{p},$$

其中

$$\lambda_{m,M}:= \inf_{\mathbf{q}\in \mathcal{Q}^{p}} \lambda_{\min}(M(\mathbf{q})) \quad (2.37)$$

和

$$\lambda_{M,M}:= \sup_{\mathbf{q}\in \mathcal{Q}^{p}} \lambda_{\max}(M(\mathbf{q})) \quad (2.38)$$

是 $M(\mathbf{q})$ 在 $\mathcal{Q}^{p}$ 中的严格正最小和最大特征值。

注意（对于同时具有旋转和移动关节的机器人），惯性矩阵特征值的特定值取决于为旋转和移动关节选择的物理单位。然而，在本书的分析中，边界 $\lambda_{m,M}$ 和 $\lambda_{M,M}$ 的特定值并不重要。只有这些边界的存在才是重要的。当然，这对于任何物理单位的选择都是有保证的。

最后，给出了重力模型的一个性质，这将在第7章的基于无源性的控制器中引起关注。

性质 2.8. 重力扭矩 $g(\mathbf{q})$ 由势能函数 $V_{g}(\mathbf{q})$ 以 $g(\mathbf{q}) = (\partial V_{g}(\mathbf{q}) / \partial \mathbf{q})^{T}$ 的形式给出，并且对于每个矩阵范数 $||\cdot ||$ ，存在一个 $\alpha > 0$ 使得：

$$\left\| \frac{\partial g(\mathbf{q})}{\partial \mathbf{q}} \right\| < \alpha \qquad \forall \mathbf{q}\in \mathcal{Q}^{p}$$

成立。

重力势能Hessian矩阵上界的存在性源于以下事实：如（2.22）所定义的 $V_{g}(\mathbf{q})$ 由关节变量 $q_{i}$ 的三角函数（对于旋转关节）和仿射函数（对于移动关节）组成dWSB96。注意，这种上界的存在性对于任何矩阵范数都是有保证的。然而， $\alpha$ 的特定值将受到该范数选择的影响。这将在第7章中更详细地讨论。

2.2.5 关于摩擦力的若干说明

在第2.2.2节和第2.2.3节模型的推导中，没有考虑摩擦。DLR轻型机器人电机侧摩擦的建模和补偿在AS01中详细处理。有趣的是，连杆侧和电机侧的粘性摩擦都不会破坏简化模型或完整模型的反馈线性化特性。

本书中推导的控制算法专门处理上述无摩擦模型。在实践中，它们显然应该与摩擦补偿结合使用，例如AS01中提出的那种。注意，这种摩擦补偿也用于第8章和第9章中介绍的实验。

机器人模型的进一步推广是引入关节阻尼，即与关节弹簧并联的阻尼。在拉格朗日量 $L(\hat{\mathbf{q}},\dot{\hat{\mathbf{q}}})$ 中，关节阻尼可以通过使用瑞利耗散函数 $1 / 2(\dot{\hat{\boldsymbol{\theta}}} - \dot{\hat{\mathbf{q}}})^T D(\dot{\hat{\boldsymbol{\theta}}} - \dot{\hat{\mathbf{q}}})$ 来形式化地包含，其中 $\mathbf{D}$ 是对角半正定阻尼矩阵。带有关节阻尼的简化模型的系统方程由下式给出：

$$\begin{array}{c} M(\mathbf{q})\ddot{\mathbf{q}} + C(\mathbf{q},\dot{\mathbf{q}})\dot{\mathbf{q}} + g(\mathbf{q}) = K(\boldsymbol{\theta} - \mathbf{q}) + D(\dot{\boldsymbol{\theta}} - \dot{\mathbf{q}}) + \boldsymbol{\tau}_{ext} \\ B\ddot{\boldsymbol{\theta}} + K(\boldsymbol{\theta} - \mathbf{q}) + D(\dot{\boldsymbol{\theta}} - \dot{\mathbf{q}}) = \boldsymbol{\tau}_{m}. \end{array} \quad (2.39)$$

这个稍微更一般的模型只会在第7章中考虑，其中提出了基于无源性的控制器。然而，关节阻尼通常非常低，因此可以在控制器设计中忽略。

2.3 总结

本章讨论了柔性关节机器人的运动学和动力学建模。首先报告了机器人学界常见的机器人运动学预备知识。基于这些运动学关系，推导了柔性关节机器人的两个动力学模型，即完整模型和简化模型。详细描述了导致这些模型的所有假设。应用MLS94中的建模方法推导了具有柔性关节的机器人的数学模型。最后，简要讨论了动力学模型的一些对控制器设计非常有用的性质。

---

3 笛卡尔阻抗控制：刚体情况

本章描述了刚体机器人的经典阻抗控制理论。第3.1节的表述基于Hogan关于阻抗控制概念的 seminal 工作Hog85a, Hog85b, Hog85c以及Khatib的操作空间公式化Kha87。在第3.2节中，分析了不需要显式整形惯性行为的期望阻抗情况。这带来了如何适当设计阻尼矩阵的问题。此外，还讨论了有关奇异性避免、坐标选择和刚度设计的一些额外方面。

本章涉及机器人模型的刚体部分，不考虑关节弹性。所讨论的主题作为第5、6和7章中柔性关节机器人模型笛卡尔阻抗控制器设计的先决条件，并且可以很容易地与这些控制器结合。

3.1 完全解耦

在本章中，关节的柔性被忽略。虽然在前一章中，关节扭矩 $\tau \in \mathbb{R}^{n}$ 与电机侧位置 $\theta$ 和连杆角度 $\mathbf{q}$ 通过 $\boldsymbol{\tau} = \mathbf{K}(\boldsymbol{\theta} - \mathbf{q})$ 相关，但现在它们被视为控制输入。关节角度汇总在向量 $\mathbf{q}\in \mathcal{Q}^{p}$ 中。使用第2章的符号，所考虑的机器人动力学模型由下式给出：

$$M(\mathbf{q})\ddot{\mathbf{q}} + C(\mathbf{q},\dot{\mathbf{q}})\dot{\mathbf{q}} + g(\mathbf{q}) = \boldsymbol{\tau} + \boldsymbol{\tau}_{ext}, \quad (3.1)$$

其中 $M(\mathbf{q})$ 是惯性矩阵， $C(\mathbf{q},\dot{\mathbf{q}})$ 是科里奥利/离心矩阵， $g(\mathbf{q})$ 是重力扭矩向量， $\boldsymbol{\tau}_{ext}$ 是外部扭矩向量。

3.1.1 任务表述

阻抗控制的目标是实现机器人运动与外部扭矩之间的特定期望动态关系。在笛卡尔阻抗控制问题的情况下，这种关系是用描述末端执行器运动的坐标来指定的。通常，末端执行器的实际配置可以用齐次矩阵表示（见第2.1节），即作为 $SE(3)$ 的元素，这可以通过指数积公式（2.3）计算。然而，为了控制器设计的目的，通常更喜欢使用 $m$ 个末端执行器坐标 $\mathbf{x} \in \mathbb{R}^{m}$ 的最小表示。如果任务中考虑了末端执行器运动的所有自由度，则有 $m = 6$ 。下面假设这些笛卡尔坐标 $\mathbf{x}$ 与配置坐标 $\mathbf{q} \in \mathcal{Q}$ 之间的关系由一个已知函数 $\mathbf{f}: \mathcal{Q} \to \mathbb{R}^{m}$ 给出，即 $\mathbf{x} = \mathbf{f}(\mathbf{q})$ 。关于笛卡尔坐标 $\mathbf{x}$ 的可能选择的一些更多细节在第3.5节中给出。

本章只考虑非冗余情况，即关节角数量 $n$ 和笛卡尔坐标数量 $m$ 相同， $m = n$ 。冗余情况的可能扩展将在下一章处理。

为了用笛卡尔坐标 $\mathbf{x}$ 表述期望的动态行为，还考虑了一阶和二阶时间导数 $\dot{\mathbf{x}}$ 和 $\ddot{\mathbf{x}}$ 。利用雅可比矩阵 $J(\mathbf{q}) = \frac{\partial \mathbf{f}(\mathbf{q})}{\partial \mathbf{q}}$ ，这些导数可以写成：

$$\begin{array}{l} \dot{\mathbf{x}} = \mathbf{J}(\mathbf{q})\dot{\mathbf{q}}, \\ \ddot{\mathbf{x}} = \mathbf{J}(\mathbf{q})\ddot{\mathbf{q}} + \dot{\mathbf{J}}(\mathbf{q})\dot{\mathbf{q}}. \end{array} \quad (3.2)$$

考虑到（2.3），可以看出笛卡尔坐标 $\mathbf{x} = \mathbf{f}(\mathbf{q})$ 通过三角函数（对于旋转关节）和仿射函数（对于移动关节）依赖于关节变量 $q_{i}$ 。因此，可以得出结论，雅可比矩阵的最大可能奇异值 $\sigma_{M,J}$ 在 $\mathcal{Q}^{p}$ 内保持有界，即：

$$\sigma_{M,J}:= \sup_{\mathbf{q}\in \mathcal{Q}^{p}} \sigma_{\max}(\mathbf{J}(\mathbf{q})) < \infty. \quad (3.4)$$

除了已经提到的非冗余情况限制之外，还将假设雅可比矩阵 $\mathbf{J}(\mathbf{q})$ 是非奇异的。通常， $\mathbf{J}(\mathbf{q})$ 当然不会在整个配置空间 $\mathcal{Q}^{p}$ 中都是非奇异的。那么本章的分析被限制在确保可逆性的区域 $\mathcal{Q}^{p}$ 。这里通过要求雅可比矩阵的最小可能奇异值：

$$\sigma_{m,J}:= \inf_{\mathbf{q}\in \mathcal{Q}^{p}} \sigma_{\min}(\mathbf{J}(\mathbf{q})) \quad (3.5)$$

必须大于某个值 $\sigma_0 > 0$ 来建立这一点。此外，假设映射 $\mathbf{f}(\mathbf{q})$ 在 $\bar{\mathcal{Q}}^{p}$ 中是单射。笛卡尔坐标中的对应区域，即 $\bar{\mathcal{Q}}^{p}$ 通过 $\mathbf{f}$ 的像，表示为 $\bar{\mathcal{Q}}_{c}^{p}$ ：

$$\begin{array}{rl} & \bar{\mathcal{Q}}^{p}:= \{\mathbf{q}\in \mathcal{Q}^{p} \mid \sigma_{\min}(\mathbf{J}(\mathbf{q})) > \sigma_{0} \text{ 且 } \mathbf{f}(\mathbf{q}) \text{ 是单射}\}, \\ & \bar{\mathcal{Q}}_{c}^{p}:= \mathbf{f}(\bar{\mathcal{Q}}^{p}) = \{\mathbf{x}\in \mathbb{R}^{n} \mid \exists \mathbf{q}\in \bar{\mathcal{Q}}^{p}, \mathbf{f}(\mathbf{q}) = \mathbf{x}\}. \end{array} \quad (3.6)$$

在集合 $\bar{\mathcal{Q}}_{c}^{p}$ 中，笛卡尔坐标 $\mathbf{x}$ 可以用作系统（3.1）的广义坐标。通常，当然不可能找到笛卡尔坐标使得 $\bar{\mathcal{Q}}_{c}^{p}$ 对应于整个状态空间 $\mathbb{R}^{n}$ 。然而，对于笛卡尔控制器的分析，如果在假设 $\bar{\mathcal{Q}}_{c}^{p} = \mathbb{R}^{n}$ 的情况下，稳定性结论是否（局部）成立，通常是有趣的。如果这是真的，稳定性区域仅受坐标特定选择的限制。

外部扭矩 $\boldsymbol{\tau}_{ext}$ 应与广义外力向量 $\mathbf{F}_{ext}$ 通过 $\boldsymbol{\tau}_{ext} = \mathbf{J}(\mathbf{q})^{T}\mathbf{F}_{ext}$ 相关。因此， $\mathbf{F}_{ext}$ 是 $\dot{\mathbf{x}}$ 的对偶变量，因此机器人与环境之间交换的功率由 $\boldsymbol{\tau}_{ext}^{T}\dot{\mathbf{q}} = \mathbf{F}_{ext}^{T}\dot{\mathbf{x}}$ 给出。

为了指定期望的阻抗行为，引入位置误差 $\tilde{\mathbf{x}} = \mathbf{x} - \mathbf{x}_{d}$ ，其中 $\mathbf{x}_{d}$ 是（可能时变的）虚拟平衡位置。那么控制目标是实现以下形式的动态关系：

$$\boldsymbol{\Lambda}_{d} \ddot{\mathbf{x}} + \mathbf{D}_{d} \dot{\mathbf{x}} + \mathbf{K}_{d} \tilde{\mathbf{x}} = \mathbf{F}_{ext} \quad (3.8)$$

其中 $\mathbf{K}_{d}$ 、 $\mathbf{D}_{d}$ 和 $\boldsymbol{\Lambda}_{d}$ 是期望刚度、阻尼和惯性的对称正定矩阵。原则上，当然也可以考虑更一般的阻抗行为。但在大多数机器人应用中，将阻抗控制器限制为这种质量-弹簧-阻尼器系统的期望行为就足够了。

3.1.2 任务坐标中的机器人模型

机器人模型（3.1）是用关节坐标 $\mathbf{q}$ 编写的，而期望行为（3.8）是用任务坐标 $\mathbf{x}$ 定义的。为了控制器设计，将模型（3.1）也重写为任务坐标更容易，如操作空间公式化Kha87中所做的那样。将 $\ddot{\mathbf{q}} = \mathbf{J}(\mathbf{q})^{-1}(\ddot{\mathbf{x}} - \dot{\mathbf{J}}(\mathbf{q})\dot{\mathbf{q}})$ 从（3.3）和 $\boldsymbol{\tau}_{ext} = \mathbf{J}(\mathbf{q})^{T}\mathbf{F}_{ext}$ 代入方程（3.1）得到：

$$M(\mathbf{q})J(\mathbf{q})^{-1}(\ddot{\mathbf{x}} - \dot{\mathbf{J}}(\mathbf{q})\dot{\mathbf{q}}) + C(\mathbf{q},\dot{\mathbf{q}})\dot{\mathbf{q}} + g(\mathbf{q}) = \boldsymbol{\tau} + \mathbf{J}(\mathbf{q})^{T}\mathbf{F}_{ext}.$$

将 $\dot{\mathbf{q}} = \mathbf{J}(\mathbf{q})^{-1}\dot{\mathbf{x}}$ 从（3.2）代入第二项和第三项，并将所得方程左乘 $\mathbf{J}(\mathbf{q})^{-T}$ ，得到：

$$\begin{array}{rl} & \mathbf{J}(\mathbf{q})^{-T} M(\mathbf{q}) \mathbf{J}(\mathbf{q})^{-1} \ddot{\mathbf{x}} + \mathbf{J}(\mathbf{q})^{-T} C(\mathbf{q},\dot{\mathbf{q}}) \mathbf{J}(\mathbf{q})^{-1} \dot{\mathbf{x}} - \\ & \mathbf{J}(\mathbf{q})^{-T} M(\mathbf{q}) \mathbf{J}(\mathbf{q})^{-1} \dot{\mathbf{J}}(\mathbf{q}) \mathbf{J}(\mathbf{q})^{-1} \dot{x} + \mathbf{J}(\mathbf{q})^{-T} g(\mathbf{q}) = \mathbf{J}(\mathbf{q})^{-T} \boldsymbol{\tau} + \mathbf{F}_{ext}. \end{array}$$

这个方程现在可以写成：

$$\Lambda(\mathbf{x})\ddot{\mathbf{x}} + \boldsymbol{\mu}(\mathbf{x},\dot{\mathbf{x}})\dot{\mathbf{x}} + \mathbf{J}(\mathbf{q})^{-T} g(\mathbf{q}) = \mathbf{J}(\mathbf{q})^{-T} \boldsymbol{\tau} + \mathbf{F}_{ext}, \quad (3.9)$$

其中矩阵 $\Lambda(\mathbf{x})$ 和 $\mu(\mathbf{x},\dot{\mathbf{x}})$ 由下式给出：

$$\begin{array}{rl} & \Lambda(\mathbf{x}) = \mathbf{J}(\mathbf{q})^{-T} M(\mathbf{q}) \mathbf{J}(\mathbf{q})^{-1}, \\ & \boldsymbol{\mu}(\mathbf{x},\dot{\mathbf{x}}) = \mathbf{J}(\mathbf{q})^{-T} \left(C(\mathbf{q},\dot{\mathbf{q}}) - M(\mathbf{q}) \mathbf{J}(\mathbf{q})^{-1} \dot{\mathbf{J}}(\mathbf{q})\right) \mathbf{J}(\mathbf{q})^{-1}, \end{array} \quad (3.10)$$

其中 $\mathbf{q} = \mathbf{f}^{-1}(\mathbf{x})$ 且 $\dot{\mathbf{q}} = \mathbf{J}(\mathbf{f}^{-1}(\mathbf{x}))\dot{\mathbf{x}}$ 。从控制的角度来看，在（3.10）和（3.11）中同时使用关节变量 $\mathbf{q}$ 和笛卡尔坐标 $\mathbf{x}$ 有点误导，因为本章中考虑的状态变量只是 $\mathbf{x}$ 和 $\dot{\mathbf{x}}$ 。另一方面，某些分量直接用 $\mathbf{q}$ 表示有时会更清晰和简单。当两种表示混合使用时， $\mathbf{q}$ 和 $\dot{\mathbf{q}}$ 可以认为分别被 $\mathbf{q} = \mathbf{f}^{-1}(\mathbf{x})$ 和 $\dot{\mathbf{q}} = \mathbf{J}(\mathbf{f}^{-1}(\mathbf{x}))\dot{\mathbf{x}}$ 替换。

与外部扭矩类似，重力扭矩 $g(\mathbf{q})$ 和关节扭矩 $\boldsymbol{\tau}$ 也可以重写为等效的任务空间重力力 $\mathbf{F}_{g}(\mathbf{x}) = \mathbf{J}(\mathbf{q})^{-T} g(\mathbf{q})$ 和新的输入向量 $\mathbf{F}_{\tau}$ ，它们通过 $\boldsymbol{\tau} = \mathbf{J}(\mathbf{q})^{T}\mathbf{F}_{\tau}$ 与 $\boldsymbol{\tau}$ 相关。因此，系统方程最终具有以下形式：

$$\Lambda(\mathbf{x})\ddot{\mathbf{x}} + \boldsymbol{\mu}(\mathbf{x},\dot{\mathbf{x}})\dot{\mathbf{x}} + \mathbf{F}_{g}(\mathbf{x}) = \mathbf{F}_{\tau} + \mathbf{F}_{ext}. \quad (3.12)$$

矩阵 $\Lambda(\mathbf{x})$ 和 $\mu(\mathbf{x},\dot{\mathbf{x}})$ 是关于坐标 $\mathbf{x}$ 的惯性矩阵和科里奥利/离心矩阵。

在给出模型（3.12）的经典阻抗控制律之前，先给出模型（3.12）的两个重要引理，它们直接从性质2.5和性质2.6得出（见dWSB96）。

引理 3.1. 如（3.10）所定义的矩阵 $\Lambda(\mathbf{x})$ 对于所有位置 $\mathbf{x}\in \bar{\mathcal{Q}}_{c}^{p}$ 是对称且正定的。

证明. 由于 $\mathbf{x}$ 限制在 $\bar{\mathcal{Q}}_{c}^{p}$ ，矩阵 $\Lambda(\mathbf{x})$ 是良定义的。 $\Lambda(\mathbf{x})$ 的对称性和正定性直接从其定义（3.10）和性质2.5得出。

引理 3.2. 矩阵 $\dot{\Lambda}(\mathbf{x}) - 2\mu(\mathbf{x},\dot{\mathbf{x}})$ ，其中 $\Lambda(\mathbf{x})$ 和 $\mu(\mathbf{x},\dot{\mathbf{x}})$ 如（3.10）和（3.11）所定义，对于所有 $\mathbf{x}\in \bar{\mathcal{Q}}_{c}^{p}$ 和所有 $\dot{\mathbf{x}}\in \mathbb{R}^{m}$ 是斜对称的。

证明. 由于 $\mathbf{x}$ 限制在 $\bar{\mathcal{Q}}_{c}^{p}$ ，矩阵 $\Lambda(\mathbf{x})$ 和 $\mu(\mathbf{x},\dot{\mathbf{x}})$ 都是良定义的。根据引理A.22，需要证明等式 $\dot{\Lambda}(\mathbf{x}) = \mu(\mathbf{x},\dot{\mathbf{x}}) + \mu(\mathbf{x},\dot{\mathbf{x}})^{T}$ 成立。在证明的剩余部分，为了简化符号，将省略矩阵的参数。从（3.10）和（3.11）以及 $M$ 的对称性（性质2.5）可以得出，矩阵 $\dot{\Lambda}$ 和 $\mu + \mu^{T}$ 由下式给出：

$$\dot{\Lambda} = \frac{\mathrm{d}}{\mathrm{d}t}(J^{-T} M J^{-1}) = \frac{\mathrm{d}}{\mathrm{d}t}J^{-T} M J^{-1} + J^{-T} \dot{M} J^{-1} + J^{-T} M \frac{\mathrm{d}}{\mathrm{d}t}J^{-1},$$ $$\mu + \mu^{T} = J^{-T}(C + C^{T})J^{-1} - J^{-T} M J^{-1} \dot{J} J^{-1} - J^{-T} \dot{J}^{T} J^{-T} M J^{-1}.$$

考虑到等式 $\dot{M} = C + C^{T}$ （由性质2.6和引理A.22得出），项 $\dot{\Lambda} - \mu - \mu^{T}$ 的结果为：

$$\dot{\Lambda} - \mu - \mu^{T} = \frac{\mathrm{d}}{\mathrm{d}t}J^{-T} M J^{-1} + J^{-T} M \frac{\mathrm{d}}{\mathrm{d}t}J^{-1} + J^{-T} M J^{-1} \dot{J} J^{-1} + J^{-T} \dot{J}^{T} J^{-T} M J^{-1}$$ $$= \left(\frac{\mathrm{d}}{\mathrm{d}t}J^{-T} + J^{-T} \dot{J}^{T} J^{-T}\right) M J^{-1} + J^{-T} M \left(\frac{\mathrm{d}}{\mathrm{d}t}J^{-1} + J^{-1} \dot{J} J^{-1}\right).$$

由等式

$$0 = \frac{\mathrm{d}}{\mathrm{d}t}(I)J^{-1} = \frac{\mathrm{d}}{\mathrm{d}t}(J^{-1}J)J^{-1} = \frac{\mathrm{d}}{\mathrm{d}t}J^{-1} + J^{-1} \dot{J} J^{-1}$$

可以得出 $\dot{\Lambda} - \mu - \mu^{T} = 0$ ，这完成了证明。

注意，这些性质并不令人惊讶，因为（3.12）无非是用另一组坐标编写的模型（3.1）。然而，（3.12）中的矩阵 $\mu(\mathbf{x},\dot{\mathbf{x}})$ 也可以不同于（3.11）选择。（3.11）中的特殊形式具有确保引理3.2有效性的优点。方程（3.11）对应于将克里斯托费尔符号Fra97, Boo03从（2.30）变换到笛卡尔坐标，这意味着如果 $\mu(\mathbf{x},\dot{\mathbf{x}})$ 通过 $\Lambda(\mathbf{x})$ 的克里斯托费尔符号计算，也会得到与（3.11）相同的形式。方程（3.10）显然是二阶协变张量的经典坐标变换Fra97, Boo03。

限制在 $\bar{\mathcal{Q}}_{c}^{p}$ 也确保了，类似于性质2.7，笛卡尔惯性矩阵的特征值由某个非零边界从上方和下方保持有界。

性质 3.3. 在 $\bar{\mathcal{Q}}_{c}^{p}$ 内，笛卡尔惯性矩阵的特征值 $\lambda_{i}(\Lambda(\mathbf{x}))$ 保持有界，即：

$$0 < \frac{\lambda_{m,M}}{\sigma_{M,J}^{2}} \leq \lambda_{i}(\Lambda(\mathbf{x})) \leq \frac{\lambda_{M,M}}{\sigma_{m,J}^{2}} < \infty, \quad i = 1, \cdots, n,\ \forall \mathbf{x}\in \bar{\mathcal{Q}}_{c}^{p},$$

其中 $\lambda_{m,M}$ 和 $\lambda_{M,M}$ 是如（2.37）和（2.38）所定义的惯性矩阵的最小和最大可能特征值， $\sigma_{m,J}$ 和 $\sigma_{M,J}$ 是如（3.4）和（3.5）所定义的雅可比矩阵的最小和最大可能奇异值。

证明. 这个性质直接从 $\Lambda(\mathbf{x})$ 的定义（3.10）以及性质2.7和 $\bar{\mathcal{Q}}_{c}^{p}$ 的定义得出。

这个性质将有助于第3.2节期望阻抗行为的分析以及第6章的控制器设计。

3.1.3 经典阻抗控制器

经典阻抗控制律Hog85a, Hog85b可以从方程（3.12）直接计算得出Kha87。导致期望闭环系统（3.8）的控制输入 $\mathbf{F}_{\tau}$ 由下式给出：

$$\begin{array}{rl} & \mathbf{F}_{\tau} = \mathbf{F}_{g}(\mathbf{x}) + \Lambda(\mathbf{x})\ddot{\mathbf{x}}_{d} + \mu(\mathbf{x},\dot{\mathbf{x}})\dot{\mathbf{x}} - \Lambda(\mathbf{x})\Lambda_{d}^{-1}\left(\mathbf{K}_{d}\tilde{\mathbf{x}} + \mathbf{D}_{d}\dot{\mathbf{x}}\right) + \\ & \left(\Lambda(\mathbf{x})\Lambda_{d}^{-1} - \mathbf{I}\right)\mathbf{F}_{ext}. \end{array} \quad (3.13)$$

然后这个笛卡尔阻抗控制器实际上通过关节扭矩 $\boldsymbol{\tau}$ 实现如下：

$$\begin{array}{rl} & \boldsymbol{\tau} = \mathbf{J}(\mathbf{q})^{T}\mathbf{F}_{\tau} \\ & = g(\mathbf{q}) + \mathbf{J}(\mathbf{q})^{T}\left(\Lambda(\mathbf{x})\ddot{\mathbf{x}}_{d} + \mu(\mathbf{x},\dot{\mathbf{x}})\dot{\mathbf{x}}\right) - \\ & \mathbf{J}(\mathbf{q})^{T}\Lambda(\mathbf{x})\Lambda_{d}^{-1}\left(\mathbf{K}_{d}\tilde{\mathbf{x}} + \mathbf{D}_{d}\dot{\mathbf{x}}\right) + \\ & \mathbf{J}(\mathbf{q})^{T}\left(\Lambda(\mathbf{x})\Lambda_{d}^{-1} - \mathbf{I}\right)\mathbf{F}_{ext}. \end{array} \quad (3.14)$$

可以看出，在这种情况下，期望阻抗的整形也包含了外力 $\mathbf{F}_{ext}$ 的反馈。这些力通常通过安装在末端执行器上的力-力矩传感器测量。但通常也可能存在不作用在工具上而是直接作用在机器人结构上的外力，因此无法测量。由于这些力不包括在 $\mathbf{F}_{ext}$ 的测量中，很明显，闭环阻抗行为相对于这些力将完全不同。注意，从实际角度来看，这在服务机器人中通常比在工业应用中更为相关。显然，当机器人与人类交互时，这种交互不一定局限于机器人的工具。

外力反馈的需求源于不仅应该整形刚度和阻尼行为，还应该整形惯性行为的要求。然而，在许多应用中，这不是必需的。因此，下一节将考虑一个简化的控制目标，其中不强调视在惯性的显式整形，而是将重点放在刚度和阻尼的整形上。

3.2 避免惯性整形

当期望惯性 $\Lambda_{d}$ 与机器人惯性 $\Lambda(\mathbf{x})$ 相同时，可以避免外力 $\mathbf{F}_{ext}$ 的反馈：

$$\Lambda_{d} = \Lambda(\mathbf{x}). \quad (3.15)$$

由于期望惯性依赖于位置 $\mathbf{x}$ ，因此在指定期望的闭环行为时，还应考虑相关的离心项和科里奥利项。这是必要的，以满足引理3.2，从而确保系统在调节情况下的无源性（另见命题3.5）。 $\tilde{\mathbf{x}}$ 和 $\mathbf{F}_{ext}$ 之间的期望动态关系由下式给出：

$$\Lambda(\mathbf{x})\ddot{\mathbf{x}} + (\mu(\mathbf{x},\dot{\mathbf{x}}) + \mathbf{D}_{d})\dot{\mathbf{x}} + \mathbf{K}_{d}\tilde{\mathbf{x}} = \mathbf{F}_{ext}, \quad (3.16)$$

其中 $\mathbf{K}_{d}$ 和 $\mathbf{D}_{d}$ 再次是期望刚度和期望阻尼的对称正定矩阵。在调节情况下（即对于 $\dot{\mathbf{x}}_{d} = \mathbf{0}$ ），这个控制目标通常被称为柔顺控制问题。

在给出导致闭环系统（3.16）的控制器之前，将首先与原始期望动态（3.8）进行简要比较，以证明（3.16）的选择是合理的。首先考虑自由运动情况，即 $\mathbf{F}_{ext} = \mathbf{0}$ 。原始期望动态（3.8）的渐近稳定性在这种情况下仅仅因为期望刚度、阻尼和惯性矩阵是正定的而得到保证。原始期望动态甚至是线性和时不变的。新动态（3.16）的稳定性性质则不那么明显。显然，由于使用了位置相关的惯性矩阵，系统（3.16）是非线性的。此外，由于虚拟平衡位置 $\mathbf{x}_{d}(t)$ 可能是时间 $t$ 的二阶连续可微函数，它还是时变的。关于时变系统稳定性分析的一些相关定义和引理可以在附录A.1中找到。

不深入细节，应该提到，（3.16）在自由运动情况下的稳定性可以通过考虑（时变）李雅普诺夫函数来证明：

$$V(\tilde{\mathbf{x}},\dot{\tilde{\mathbf{x}}},t) = \frac{1}{2}\dot{\tilde{\mathbf{x}}}^{T}\Lambda(\mathbf{x})\dot{\tilde{\mathbf{x}}} + \frac{1}{2}\tilde{\mathbf{x}}^{T}\mathbf{K}_{d}\tilde{\mathbf{x}}. \quad (3.17)$$

这个李雅普诺夫函数沿（3.16）解的变化是负半定的，这意味着稳定性，但不是渐近稳定性。然而，对于自由运动，（3.16）中的动态等价于以任务坐标编写的著名的 $PD+$ 控制器PP88的闭环系统。 $PD+$ 控制器在配置空间中的渐近稳定性在PP88中得到了证明。此外，这个系统的严格李雅普诺夫函数在SK97b中提出。在这两项工作中，都需要惯性矩阵 $M(\mathbf{q})$ 的有界性，如性质2.7所表述。笛卡尔惯性矩阵 $\Lambda(\mathbf{x})$ 的等价陈述由于限制在 $\mathcal{Q}_{c}^{p}$ 而在性质3.3中得到保证。根据SK97b中的证明（针对配置空间中的相同控制器），如果 $\mathcal{Q}_{c}^{p}$ 对应于整个状态空间 $\mathbb{R}^{n}$ ，则以下命题中表述的稳定性结论甚至全局成立。

命题 3.4. 设期望轨迹 $\mathbf{x}_{d}(t)$ 是二阶连续可微的。进一步假设笛卡尔坐标全局有效，即 $\mathcal{Q}_{c}^{p} = \mathbb{R}^{n}$ 。那么对于 $\mathbf{F}_{ext} = \mathbf{0}$ ，具有对称正定矩阵 $\mathbf{K}_{d}$ 和 $\mathbf{D}_{d}$ 的系统（3.16）是一致全局渐近稳定的。

原始期望动态（3.8）的另一个重要特性是，在调节情况下（即 $\dot{\mathbf{x}}_{d} = \mathbf{0}$ ），它表示从外部广义力 $\mathbf{F}_{ext}$ 到速度 $\dot{\mathbf{x}}$ 的无源映射。如果考虑机器人与无源环境的交互，这个性质尤其重要。通过将（3.17）视为存储函数，可以证明在调节情况下，系统（3.16）也表示从外力 $\mathbf{F}_{ext}$ 到速度 $\dot{\mathbf{x}}$ 的无源映射。

命题 3.5. 对于 $\dot{\mathbf{x}}_{d}(t) = \mathbf{0}$ ，具有对称正定矩阵 $\mathbf{K}_{d}$ 和 $\mathbf{D}_{d}$ 的系统（3.16）是时不变的，并表示从外力 $\mathbf{F}_{ext}$ 到速度 $\dot{\mathbf{x}}$ 的无源映射。

通过遵循与上一节经典阻抗控制器相同的论证，得到控制律：

$$\mathbf{F}_{\tau} = \mathbf{F}_{g}(\mathbf{x}) + \Lambda(\mathbf{x})\ddot{\mathbf{x}}_{d} + \mu(\mathbf{x},\dot{\mathbf{x}})\dot{\mathbf{x}}_{d} - \mathbf{K}_{d}\tilde{\mathbf{x}} - \mathbf{D}_{d}\dot{\mathbf{x}},$$

以实现动态（3.16）。为了实现这个控制器，在关节级别的表述：

$$\begin{array}{rl} & \boldsymbol{\tau} = \mathbf{J}(\mathbf{q})^{T}\mathbf{F}_{\tau} \\ & = g(\mathbf{q}) + \mathbf{J}(\mathbf{q})^{T}\left(\Lambda(\mathbf{x})\ddot{\mathbf{x}}_{d} + \mu(\mathbf{x},\dot{\mathbf{x}})\dot{\mathbf{x}}_{d} - \mathbf{K}_{d}\tilde{\mathbf{x}} - \mathbf{D}_{d}\dot{\mathbf{x}}\right) \end{array} \quad (3.18)$$

更方便。

期望刚度通常由应用给出。新的期望阻抗行为（3.16）然后带来了如何选择阻尼矩阵的问题。这个问题将在下一节讨论。

3.3 阻尼矩阵设计

在原始期望动态（3.8）的情况下，期望惯性矩阵 $\Lambda_{d}$ 是常数，并且原则上可以设计得使其特征向量与 $\mathbf{K}_{d}$ 的特征向量重合。阻尼矩阵的设计然后可以简化为设计 $n$ 个解耦的线性时不变二阶系统的阻尼系数，每个系统描述系统沿一个特征向量的动态。

对于新的期望动态（3.16），情况有所不同。期望刚度矩阵必须是常数，并且通常由所考虑的应用定义。而期望阻尼矩阵的设计则不那么明确。注意，常数对角矩阵 $\mathbf{D}_{d}$ 通常不是一个好的选择，因为惯性矩阵是非对角的且时变的。相反，还应该考虑 $\Lambda(\mathbf{x})$ 的特定结构和在运动过程中的变化。

对于阻尼矩阵的设计，应该提到，当（3.16）中的常数矩阵 $\mathbf{D}_{d}$ 被任意正定的位置依赖矩阵 $\mathbf{D}_{d}(\mathbf{x})$ 替换时，上述所有的稳定性和无源性性质仍然成立。

在ASOFH03中，针对给定的对称正定刚度矩阵 $\mathbf{K}_{d}$ ，提出了两种不同的选择矩阵 $\mathbf{D}_{d}(\mathbf{x})$ 的方法，并在DLR轻型机器人上进行了评估。下面只详细介绍基于对称矩阵广义特征值分解的方法。为此，表述以下引理Har97。

引理 3.6. 给定对称正定矩阵 $\mathbf{A}\in \mathbb{R}^{n\times n}$ 和对称矩阵 $\mathbf{B}\in \mathbb{R}^{n\times n}$ 。则可以找到一个非奇异矩阵 $\mathbf{Q}\in \mathbb{R}^{n\times n}$ 和一个对角矩阵 $\mathbf{B}_{0}\in \mathbb{R}^{n\times n}$ ，使得 $Q^{T}Q = A$ 且 $\mathbf{B} = Q^{T}B_{0}Q$ 。

这个对角矩阵 $\mathbf{B}_{0}$ 的元素称为 $\mathbf{B}$ 相对于 $\mathbf{A}$ 的广义特征值。如果矩阵 $\mathbf{B}$ 是正定的，广义特征值将是正的。为了设计（3.16）中的矩阵 $\mathbf{D}_{d}(\mathbf{x})$ ，进行准静态分析，这意味着在每个位置 $\mathbf{x}_{0}$ ，系统由以下线性时不变系统近似：

$$\Lambda(\mathbf{x}_0)\ddot{\tilde{\mathbf{x}}} + \mathbf{D}_d(\mathbf{x}_0)\dot{\tilde{\mathbf{x}}} + \mathbf{K}_d\tilde{\mathbf{x}} = \mathbf{F}_{ext}, \quad (3.19)$$

其中科里奥利/离心矩阵被忽略，矩阵 $\Lambda(\mathbf{x}_0)$ 和 $\mathbf{D}_{d}(\mathbf{x}_{0})$ 被视为常数。通过应用引理3.6（其中 $\Lambda(\mathbf{x}_0)$ 和 $\mathbf{K}_{d}$ 分别对应于 $\mathbf{A}$ 和 $\mathbf{B}$ ），矩阵 $\mathbf{K}_{d}$ 和 $\Lambda(\mathbf{x}_0)$ 可以通过非奇异矩阵 $\mathbf{Q}(\mathbf{x}_0)$ 同时对角化，使得系统（3.19）可以写成以下形式：

$$Q(\mathbf{x}_0)^T Q(\mathbf{x}_0)\ddot{\tilde{\mathbf{x}}} + \mathbf{D}_d(\mathbf{x}_0)\dot{\tilde{\mathbf{x}}} + Q(\mathbf{x}_0)^T \mathbf{B}_0(\mathbf{x}_0) Q(\mathbf{x}_0)\tilde{\mathbf{x}} = \mathbf{F}_{ext}, \quad (3.20)$$

其中 $\mathbf{B}_{0}(\mathbf{x}_{0})$ 是正定对角矩阵。

设 $\lambda_{K,i}^{\Lambda}$ 是 $\mathbf{B}_{0}$ 的第 $i$ 个对角元素。这是 $\mathbf{K}_{d}$ 相对于 $\Lambda(\mathbf{x}_0)$ 的第 $i$ 个广义特征值。然后矩阵 $\mathbf{D}_{d}(\mathbf{x}_{0})$ 可以选择为 $\mathbf{D}_{d}(\mathbf{x}_{0}) = 2Q(\mathbf{x}_{0})^{T}\mathrm{diag}(\xi_{i}\sqrt{\lambda_{K,i}^{\Lambda}}) Q(\mathbf{x}_{0})$ ，其中 $\xi_{i}$ 是要在[0, 1]范围内选择的阻尼因子。

显然，通过这种选择，系统可以在状态变量 $\mathbf{z} = Q(\mathbf{x}_0)\tilde{\mathbf{x}}$ 中写成以下形式：

$$\ddot{\mathbf{z}} + 2\mathrm{diag}(\xi_i\sqrt{\lambda_{K,i}^{\Lambda}})\dot{\mathbf{z}} + \mathrm{diag}(\lambda_{K,i}^{\Lambda})\mathbf{z} = Q(\mathbf{x}_0)^{-T}\mathbf{F}_{ext}. \quad (3.21)$$

应该再次提到，上述近似仅用于阻尼矩阵的设计，并不影响系统的稳定性性质，因为确保了阻尼矩阵始终是正定的。

3.4 奇异性处理

在前面的章节中，假设 $J(q)$ 在所考虑的工作空间内是非奇异的。通常，需要区分两种不同类型的奇异性：表示奇异性和运动学奇异性。第一种是用于描述工具框架配置 $\mathbf{h}_{st}(\mathbf{q})\in SE(3)$ 的局部坐标 $\mathbf{x}$ 的特定选择的结果。众所周知， $SO(3)$ 的每一个最小表示，因此也是 $SE(3)$ 的，都会有这样的奇异性。在实践中，可以通过在工作空间的不同区域使用不同的表示来处理它们。然而，在这些不同表示之间切换是一个关键问题。完全避免这种奇异性的唯一方法是使用非最小参数化。如何基于这种非最小参数化构造无奇异性的笛卡尔刚度的方法将在第3.5.2节中回顾。

第二种类型的奇异性，即运动学奇异性，则无法避免。它们与操纵器的运动学结构固有地相关，代表了在某些关节配置下无法产生末端执行器的任意运动。从数学角度来看，运动学奇异性是体雅可比矩阵的奇异性。很明显，在奇异配置附近，期望的阻抗行为（3.16）通常无法精确实现，而是会失真。使用像（3.18）这样的控制律，机器人可能会因为（3.18）中的第二项消失而卡住。

在CK95中提出了一种在运动学奇异性处控制操纵器的方法。在这项工作中，需要对体雅可比矩阵的行列式进行分解，其中每一项对应于机器人的一种不同奇异配置。当接近这种奇异配置时，控制器分成两部分。控制器中的一部分用于通过分解中的相关项控制关节配置到下一个奇异配置的距离。另一部分基本上通过经典的操作空间公式化Kha87控制正交于奇异方向的运动。虽然这是一种非常有效的方法，特别是（但不限于）用于位置控制器，但在阻抗控制器的情况下，有时宁愿简单地避免奇异配置。

在本节中，介绍了一种避免奇异性的可能解决方案。这将隐含地限制操纵器的工作空间，但这种限制将以一种通用方式进行，无需事先知道所有奇异配置。

为了实现奇异性避免，将第3.2节的笛卡尔阻抗控制器与第二个阻抗控制器结合，该控制器迫使操纵器远离奇异配置。这两个控制器的组合可以通过考虑阻抗的叠加原理Hog85a, BS98来实现。根据这个原理，即使阻抗的行为是非线性的，耦合到导纳的多个阻抗分量也可以通过简单地相加它们的输出扭矩来组装。

为了构建奇异性避免势能，使用运动学可操作性度量Yos90：

$$m_{kin}(\mathbf{q}) = \sqrt{\det(\mathbf{J}(\mathbf{q})\mathbf{J}(\mathbf{q})^{T})}. \quad (3.22)$$

奇异性避免势能选择为：

$$V_{m}(\mathbf{q}) = \begin{cases} k_{s}(m_{kin}(\mathbf{q}) - m_{0})^{2} & m_{kin}(\mathbf{q}) \leq m_{0} \\ 0 & m_{kin}(\mathbf{q}) > m_{0} \end{cases}, \quad (3.23)$$

其中 $k_{s} \in \mathbb{R}$ 是一个正标量因子，控制奇异性避免的增益。 $m_{kin}(\mathbf{q})$ 的上界 $m_{0} > 0 \in \mathbb{R}$ 决定了奇异性避免将激活的奇异配置周围区域。这两个参数是设计参数。

令 $\tau_{c}$ 表示上一节笛卡尔阻抗控制器的输出扭矩，则带有奇异性避免的完整控制律由下式给出：

$$\boldsymbol{\tau} = \boldsymbol{\tau}_{c} - \frac{\partial V_{m}(\mathbf{q})}{\partial \mathbf{q}}. \quad (3.24)$$

注意，除了 $m_{kin}(\mathbf{q})$ ，也可以使用动态可操作性度量 $m_{dyn}(\mathbf{q}) = \sqrt{\det(\mathbf{J}(\mathbf{q})M(\mathbf{q})^{-1}M(\mathbf{q})^{-T}\mathbf{J}(\mathbf{q})^{T})}$ Yos90。关于实现，使用运动学度量的优点是消耗更少的计算能力。

关于奇异性避免对闭环系统稳定性性质的影响，还需要做一些补充说明。注意，只有当操纵器配置接近奇异性时，奇异性避免才会被激活。远离奇异性时，只有笛卡尔阻抗控制器是激活的。如果假设虚拟平衡位置远离奇异性，并且期望刚度不是太低，那么奇异性避免不会影响系统的（局部）稳定性。另一方面，应该提到，在奇异性附近，确实可能发生由于笛卡尔刚度势能和奇异性避免势能产生的扭矩相互抵消的情况。因此，完整的势能函数可能具有除期望平衡位置以外的局部最小值。然后机器人显然会在穿过这个局部最小值时卡住，然而，这将靠近奇异配置。

3.5 关于刚度实现的说明

在前面的章节中，假设末端执行器位姿可以用某些末端执行器坐标 $\mathbf{x} = \mathbf{f}(\mathbf{q})$ 来描述，并且笛卡尔误差 $\tilde{\mathbf{x}}$ 相应地读作 $\tilde{\mathbf{x}} = \mathbf{x} - \mathbf{x}_{d}$ 。与第3.3节讨论的阻尼矩阵相反，期望刚度矩阵 $\mathbf{K}_{d}$ 被要求是常数矩阵。否则，命题3.4的稳定性陈述将不再有效。

在本节中，将解释（3.18）中使用的特定笛卡尔坐标 $\mathbf{f}(\mathbf{q})$ 的选择如何影响最终的刚度行为。为此，假设笛卡尔坐标 $\mathbf{x}\in \mathbb{R}^{6}$ 可以分解为两个分量 $\mathbf{x}_{t}\in \mathbb{R}^{3}$ 和 $\mathbf{x}_{r}\in \mathbb{R}^{3}$ ，分别描述末端执行器的位置和方向。类似地，平移误差表示为 $\tilde{\mathbf{x}}_{t}\in \mathbb{R}^{3}$ ，旋转误差表示为 $\tilde{\mathbf{x}}_{r}\in \mathbb{R}^{3}$ 。根据平移和旋转误差，刚度矩阵 $\mathbf{K}_{d}$ 被划分为平移刚度 $\mathbf{K}_{t}$ 、旋转刚度 $\mathbf{K}_{r}$ 和耦合刚度 $\mathbf{K}_{c}$ ，即：

$$\mathbf{K}_{d} = \begin{bmatrix} \mathbf{K}_{t} & \mathbf{K}_{c} \\ \mathbf{K}_{c}^{T} & \mathbf{K}_{r} \end{bmatrix}.$$

如第2.1节所述，末端执行器的位姿，即工具框架 $\mathcal{T}$ 的位姿，由正向运动学映射 $\mathbf{h}_{st}(\mathbf{q}):\mathcal{Q}\rightarrow SE(3)$ 给出，这可以通过指数积公式（2.3）计算。根据第2章的符号， $\mathbf{h}_{st}(\mathbf{q})$ 的旋转部分表示为 $\mathbf{R}_{st}(\mathbf{q})\in SO(3)$ ，平移部分表示为 $\mathbf{p}_{st}(\mathbf{q})\in \mathbb{R}^{3}$ 。给定一个（时变的）虚拟位姿 $\mathbf{h}_{sd}(t)\in SE(3)$ ，对应于一个（时变的）期望框架 $\mathcal{D}$ ， $\mathbf{h}_{st}(\mathbf{q})$ 与 $\mathbf{h}_{sd}(t)$ 的偏差可以由下式描述：

$$\mathbf{h}_{dt}(\mathbf{q},t) = \mathbf{h}_{sd}^{-1}(t)\mathbf{h}_{st}(\mathbf{q}) = \begin{bmatrix} \mathbf{R}_{dt}(\mathbf{q},t) & \mathbf{p}_{dt}(\mathbf{q},t) \\ \mathbf{0} & 1 \end{bmatrix}. \quad (3.25)$$

以下讨论分为两部分。第3.5.1节阐明了一些关于平移刚度 $\mathbf{K}_{t}$ 实现的方面，第3.5.2节讨论了旋转刚度的实现。

3.5.1 平移刚度

（3.25）中的向量 $^{d}\mathbf{p}_{dt}(\mathbf{q},t)$ 对应于从期望框架 $\mathcal{D}$ 的原点到工具框架 $\mathcal{T}$ 的原点的向量，在框架 $\mathcal{D}$ 中表示。由此，可以通过将该向量旋转到基坐标系 $\mathcal{S}$ 来获得一组适当的平移末端执行器坐标：

$$\mathbf{p}_{dt}(\mathbf{q},t) = \mathbf{R}_{sd}(t)^{d}\mathbf{p}_{dt}(\mathbf{q},t) = \mathbf{p}_{st}(\mathbf{q}) - \mathbf{p}_{sd}(t). \quad (3.26)$$

然后，可以选择 $\mathbf{x}_{t} = \mathbf{p}_{st}(\mathbf{q})$ 作为平移坐标， $\mathbf{p}_{sd}(t)$ 作为其对应的虚拟平衡位置。通过这种选择，刚度矩阵的平移部分对应于在基坐标系 $\mathcal{S}$ 中表示的刚度，因此 $\mathbf{K}_{t}$ 的特征向量（对应于刚度矩阵的主轴）是在基坐标系中表示的常数向量。

对于某些应用，可能需要使用这样一种刚度实现，其中刚度矩阵的主轴在期望框架 $\mathcal{D}$ 或工具框架 $\mathcal{T}$ 中（作为常数向量）定义。然后，必须参考一组不同的坐标。例如，如果向量 $^{d}\mathbf{p}_{dt}(\mathbf{q},t)$ 被用作笛卡尔误差 $\tilde{\mathbf{x}}$ 的平移部分 $\tilde{\mathbf{x}}_{t}$ ，这对应于在 $\mathcal{D}$ 中的刚度表示。对于在 $\mathcal{T}$ 中的刚度表示，可以相应地使用形式为 $^{t}\mathbf{p}_{dt}(\mathbf{q},t) = \mathbf{R}_{td}(\mathbf{q},t)^{d}\mathbf{p}_{dt}(\mathbf{q},t)$ 的平移坐标。注意（除了选择 $\mathbf{x}_{t} = \mathbf{p}_{st}(\mathbf{q})$ ），不能将 $\tilde{\mathbf{x}}_{t}$ 写为配置依赖坐标 $\mathbf{x}_{t}(\mathbf{q})$ 与时变（但配置无关）虚拟平衡位置 $\mathbf{x}_{d}(t)$ 之差。然而，上一节的阻抗控制器当然可以很容易地适应这种情况。这将在第3.5.3节中更详细地处理。

3.5.2 旋转刚度

虽然平移刚度的过程相当直接，但方向表示的情况更为复杂。众所周知，不存在 $SO(3)$ 的全局最小表示。在Nat03中详细分析了笛卡尔控制器中不同方向坐标选择。在本节中，简要讨论了两种旋转刚度的实现方法，即欧拉角和单位四元数。

考虑一组欧拉角 $\phi_{st}(\mathbf{q}) = \phi(\mathbf{R}_{st}(\mathbf{q}))\in \mathbb{R}^{3}$ ，直接从旋转矩阵 $\mathbf{R}_{st}(\mathbf{q})$ 计算。可以构建经典的基于欧拉角的方向误差 $\phi_{dt}^{*}(\mathbf{q},t) = \phi_{st}(\mathbf{q}) - \phi_{sd}(t)$ 。但这种表述方向误差的方法有一个主要缺点：无论机器人方向 $\phi_{st}(q)$ 还是期望平衡方向 $\phi_{sd}(t)$ 奇异时，都会遇到奇异性。这可能发生在任意小的方向误差下。更好的方法是从差动框架 $\mathbf{h}_{td}(\mathbf{q},t)$ 通过 $\iota \phi_{td}(\mathbf{q},t) = \phi(\mathbf{R}_{td}(\mathbf{q},t))$ 计算欧拉角。这种表示，有时称为改进的欧拉角表示，对奇异性要鲁棒得多。例如，如果使用众所周知的滚转-俯仰-偏航表示，那么该表示在方向误差达到 $\pi /2$ 之前不包含任何奇异性。

基于欧拉角的刚度的主要缺点是表示奇异性的出现。只有参考末端执行器方向的非最小表示才能完全避免这些奇异性。这里，只简要讨论所谓的单位四元数的使用。关于单位四元数的详细阐述超出了本节的范围，只报告了一些相关性质。更多关于单位四元数使用的细节可以在例如CNSV99, Nat03及其引用的参考文献中找到。

单位四元数 $(\eta,\epsilon)$ 由标量部分 $\eta \in \mathbb{R}$ 和向量部分 $\epsilon \in \mathbb{R}^{3}$ 组成，满足条件 $\eta^{2} + \epsilon^{T}\epsilon = 1$ 。旋转矩阵与单位四元数之间的关系如下。假设一个旋转矩阵由绕轴 $\mathbf{r}$ （且 $||\mathbf{r}||_{2} = 1$ ）旋转角度 $\alpha$ 指定，则对应的单位四元数由 $\eta = \cos(\alpha /2)$ 、 $\epsilon = \mathbf{r}\sin(\alpha /2)$ 给出。将 $\alpha$ 限制在区间 $[-\pi,\pi]$ 内，单位四元数集合是 $SO(3)$ 的单射覆盖。与欧拉角相反，单位四元数给出了 $SO(3)$ 的无奇异性表示。

设 $\epsilon_{dt}(q,t)$ 是根据旋转矩阵 $\mathbf{R}_{dt}(q,t)$ 的单位四元数的向量部分。为了实现旋转刚度，可以参考以下方向误差 $\tilde{\mathbf{x}}_{r}(\mathbf{q},t):= 2\epsilon_{dt}(\mathbf{q},t)$ 。注意，这个量（以及由此产生的刚度项）对于 $\alpha$ 是周期的。

这里只讨论了最常用的刚度实现类型。它们与本书中控制器的关系将在下一节中阐明。关于另一种有趣的刚度实现类别，读者可以参考FH95, FB97, Fas97, ZF00, SD01, Str01，其中讨论了所谓的空间刚度。

3.5.3 对闭环动力学的影响

前两节得出了一个笛卡尔误差 $\tilde{\mathbf{x}}(\mathbf{q},t)$ ，它不能写成实际量和期望量之差。下面将展示如何调整第3.1.2节的模型以应对这种情况。

误差向量 $\tilde{\mathbf{x}}(\mathbf{q},t)$ 的一阶和二阶导数由下式给出：

$$\begin{array}{l} \dot{\tilde{\mathbf{x}}} = \frac{\partial \tilde{\mathbf{x}}(\mathbf{q},t)}{\partial \mathbf{q}}\dot{\mathbf{q}} + \frac{\partial \tilde{\mathbf{x}}(\mathbf{q},t)}{\partial t}, \\ \ddot{\tilde{\mathbf{x}}} = \frac{\partial \tilde{\mathbf{x}}(\mathbf{q},t)}{\partial \mathbf{q}}\ddot{\mathbf{q}} + \frac{\mathrm{d}}{\mathrm{d}t}\left(\frac{\partial \tilde{\mathbf{x}}(\mathbf{q},t)}{\partial \mathbf{q}}\right)\dot{\mathbf{q}} + \frac{\mathrm{d}}{\mathrm{d}t}\frac{\partial \tilde{\mathbf{x}}(\mathbf{q},t)}{\partial t}. \end{array} \quad (15)$$

这些方程替换了（3.2）和（3.3）。误差向量 $\tilde{\mathbf{x}}$ 的运动方程可以通过遵循第3.1.2节的相同步骤推导。为了简化表述，进行以下替换：

$$\frac{\partial \tilde{x}(q,t)}{\partial q} = \mathbf{J}_x(q,t),$$ $$\frac{\partial \tilde{x}(q,t)}{\partial t} = -\mathbf{v}_t(q,t).$$

因此，得到以下运动方程：

$$\Lambda(q,t)(\ddot{\tilde{\mathbf{x}}} + \dot{\mathbf{v}}_t(q,t)) + \mu(q,\dot{\mathbf{q}},t)(\dot{\tilde{\mathbf{x}}} + \mathbf{v}_t(q,t)) + \mathbf{J}_x(q,t)^{-T} g(q) - \mathbf{J}_x(q,t)^{-T}(\boldsymbol{\tau} + \boldsymbol{\tau}_{ext}) = \mathbf{0},$$

其中 $\Lambda(q,t)$ 和 $\mu(q,\dot{q},t)$ 是相关的笛卡尔惯性矩阵和科里奥利/离心矩阵：

$$\Lambda(q,t) = \mathbf{J}_x(q,t)^{-T} M(q) \mathbf{J}_x(q,t)^{-1},$$ $$\mu(q,\dot{q},t) = \mathbf{J}_x(q,t)^{-T}\left(C(q,\dot{q}) - M(q) \mathbf{J}_x(q,t)^{-1} \dot{\mathbf{J}}_x(q,t)\right) \mathbf{J}_x(q,t)^{-1},$$

类似于（3.10）和（3.11）。将此方程与（3.12）比较，可以看出，当进行以下替换时，可以使用与前面章节相同的控制器：

$$\mathbf{J}(q) \rightarrow \mathbf{J}_x(q,t),$$ $$\dot{\mathbf{x}}_d \rightarrow -\mathbf{v}_t(q,t),$$ $$\ddot{\mathbf{x}}_d \rightarrow -\dot{\mathbf{v}}_t(q,t).$$

注意，虽然这些量现在变成时变的，但这些替换不会改变本书中提出的稳定性陈述。

不同局部坐标集合的 $\mathbf{J}_x(q,t)$ 和 $\mathbf{v}_t(q,t)$ 的显式公式可以用体雅可比矩阵 $\mathbf{J}^b(q)$ 给出。因此注意，如第2.1节所定义的 $\mathbf{J}^b(q)$ 用于计算在工具框架中表示的平移速度 $^{t}\mathbf{v}_{st} = \mathbf{R}_{ts}\dot{\mathbf{p}}_{st}$ 和角速度 $^{t}\boldsymbol{\omega}_{st} = \mathbf{R}_{ts}\boldsymbol{\omega}_{st}$ ：

$$\begin{pmatrix} {}^{t}\mathbf{v}_{st} \\ {}^{t}\boldsymbol{\omega}_{st} \end{pmatrix} = \mathbf{J}^b(q)\dot{\mathbf{q}}. \quad (3.27)$$

3.6 总结

本章讨论了（传统的）无关节柔性机器人模型的笛卡尔阻抗控制问题。首先处理了根据期望的广义质量-弹簧-阻尼器系统形式的阻抗的一般解决方案。然后详细分析了无惯性整形的阻抗控制器的情况。特别地，这带来了适当设计笛卡尔阻尼矩阵的问题。针对这个问题，提出了基于正定矩阵同时对角化的解决方案。此外，讨论了奇异性避免问题，并基于阻抗叠加原理Hog85c提出了解决方案。然后讨论了一些关于笛卡尔坐标选择的额外细节，这些细节导致了相对于不同框架的刚度表示。这些问题虽然实际上相关，但在机器人文献中却很少被提及。最后，简要讨论了一种替代的方向刚度实现，它避免了任何 $SO(3)$ 的最小表示所固有的解析奇异性，以供进一步参考。

---

4 零空间刚度

在前一章中，处理了任务坐标数量 $m$ 等于配置坐标数量 $n$ （ $m = n$ ）的非冗余情况。而DLR轻型机器人则有 $n = 7$ 个关节，而末端执行器运动仅由 $m = 6$ 个自由度描述。剩余的 $n - m$ 个自由度可以用于所谓的零空间运动，这些运动保持末端执行器固定。因此，问题是如何控制这些冗余自由度。

在本章中，使用与第3章相同的符号。因此，假设末端执行器任务可以由 $m$ 个局部坐标 $\mathbf{x}\in \mathbb{R}^{m}$ 描述，而机器人配置由 $n$ 个坐标 $\mathbf{q}\in \mathcal{Q}$ 描述。从配置空间到任务空间的正向运动学映射 $\mathbf{f}:\mathcal{Q}\to \mathbb{R}^{m}$ 和相关的（解析）雅可比矩阵 $\mathbf{J}(\mathbf{q}) = \frac{\partial \mathbf{f}(\mathbf{q})}{\partial \mathbf{q}} \in \mathbb{R}^{m\times n}$ 是已知的：

$$\begin{array}{l} \mathbf{x} = \mathbf{f}(\mathbf{q}), \\ \dot{\mathbf{x}} = \mathbf{J}(\mathbf{q})\dot{\mathbf{q}}. \end{array} \quad (4.1)$$

在冗余情况下，有 $m < n$ ，因此末端执行器坐标只部分指定了机器人配置。

在本章中，只处理非奇异情况。因此，假设雅可比矩阵 $\mathbf{J}(\mathbf{q})$ （现在是一个矩形矩阵）在所考虑的工作空间内具有满秩，即 $\operatorname{rank}(\mathbf{J}(\mathbf{q})) = m$ 。如何处理运动学奇异性的适当方法在第3.4节中描述。

本章再次假设操纵器动力学的刚体模型。因此，所考虑的机器人动力学由下式给出：

$$M(\mathbf{q})\ddot{\mathbf{q}} + C(\mathbf{q},\dot{\mathbf{q}})\dot{\mathbf{q}} + g(\mathbf{q}) = \boldsymbol{\tau} + \boldsymbol{\tau}_{ext}, \quad (4.3)$$

其中符号与第3章相同。

处理冗余机器人的一种方法是通过 $r = n - m$ 个辅助变量（零空间坐标）来扩充任务坐标 $\mathbf{x}$ 。这种扩充可以针对广义位置（通过零空间位置坐标 $\mathbf{n}\in \mathbb{R}^{r}$ ）进行，如任务空间扩展方法Bai85, Sic90中所做的那样，或者针对广义速度（通过零空间速度坐标 $\mathbf{v}_n\in \mathbb{R}^r$ ）进行，如关节空间分解方法Par99, OCY98中所做的那样。对于这两种方法，将讨论找到适当坐标以及任务动态与零空间动态解耦的问题。

额外的零空间坐标将使用零空间基矩阵 $\mathbf{Z}(\mathbf{q})\in \mathbb{R}^{r\times n}$ 构建，该矩阵由 $r$ 个线性无关的行向量组成，这些行向量张成雅可比矩阵 $\mathbf{J}(\mathbf{q})$ 的（右）零空间。第4.1节处理如何计算这样的矩阵的问题。此后，首先在第4.2节中基于阻抗叠加原理描述了一种控制零空间运动的简单方法。这种方法易于实现，因为它不依赖于零空间坐标的显式构建。然而，它无法实现完整的稳定性分析。然后，在第4.3节和第4.4节中描述了上述两种生成零空间坐标的方法，并分析了它们在阻抗控制问题中的应用。

4.1 零空间基矩阵的计算

在本节中，将展示如何为雅可比矩阵 $\mathbf{J}(\mathbf{q})\in \mathbb{R}^{m\times n}$ 构造一个零空间基矩阵 $\mathbf{Z}(\mathbf{q})\in \mathbb{R}^{r\times n}$ 。表述重点放在一种以符号形式给出零空间基矩阵的方法上。

4.1.1 一般情况

如前所述，本章引言中已经提到，假设矩阵 $\mathbf{J}(\mathbf{q})$ 在所考虑的工作空间中具有满行秩，即避免了奇异性。通过重新排列 $\mathbf{J}(\mathbf{q})$ 的列，总是可以将其写成以下分块形式：

$$\mathbf{J}(\mathbf{q}) = [\mathbf{J}_m(\mathbf{q}) \quad \mathbf{J}_r(\mathbf{q})], \quad (4.4)$$

使得左部分，即二次矩阵 $\mathbf{J}_m(\mathbf{q})\in \mathbb{R}^{m\times m}$ ，（至少局部地）是可逆的。进一步将期望的零空间基矩阵相应地划分为 $\mathbf{Z}(\mathbf{q}) = [\mathbf{Z}_m(\mathbf{q}) \quad \mathbf{Z}_r(\mathbf{q})]$ ，其中矩阵 $\mathbf{Z}_m(\mathbf{q})\in \mathbb{R}^{r\times m}$ 和 $\mathbf{Z}_r(\mathbf{q})\in \mathbb{R}^{r\times r}$ 待定，使得 $\mathbf{J}(\mathbf{q})\mathbf{Z}(\mathbf{q})^T = \mathbf{0}$ 成立，并且使得 $\mathbf{Z}(\mathbf{q})$ 具有满行秩。

使用所选择的分块，条件 $\mathbf{J}(\mathbf{q})\mathbf{Z}(\mathbf{q})^T = \mathbf{0}$ 可以写成：

$$\mathbf{J}_m(\mathbf{q})\mathbf{Z}_m(\mathbf{q})^T + \mathbf{J}_r(\mathbf{q})\mathbf{Z}_r(\mathbf{q})^T = \mathbf{0}. \quad (4.5)$$

该方程的一个可能解由以下特定选择给出：

$$\mathbf{Z}_m(\mathbf{q})^T = -\mathbf{J}_m(\mathbf{q})^{-1}\mathbf{J}_r(\mathbf{q}),$$ $$\mathbf{Z}_r(\mathbf{q})^T = \mathbf{I}, \quad (4.6)$$

根据构造，这给出了一个满秩的零空间基矩阵。矩阵 $Z(\mathbf{q})$ 则由下式给出：

$$Z(\mathbf{q}) = [-J_{r}(\mathbf{q})^{T}J_{m}(\mathbf{q})^{-T} \quad I]. \quad (4.8)$$

这种形式的 $Z(\mathbf{q})$ 最初由Huang和Varma在HV91中提出，此后被许多作者使用。但矩阵 $Z(\mathbf{q})$ 显然不是唯一的，可以有不同的选择。例如，Chen和Walker在CW93中提出了一种值得注意的修改，他们建议通过因子 $\det(J_{m}(\mathbf{q}))$ 来缩放 $Z(\mathbf{q})$ ，即 $\bar{Z}(\mathbf{q}) = \det(J_{m}(\mathbf{q}))Z(\mathbf{q})$ 。注意， $J_{m}(\mathbf{q})$ 的逆可以写成：

$$J_{m}(\mathbf{q})^{-1} = \operatorname{adj}(J_{m}(\mathbf{q})) / \det(J_{m}(\mathbf{q})),$$

其中 $\operatorname{adj}(J_{m}(\mathbf{q}))$ 是 $J_{m}(\mathbf{q})$ 的伴随矩阵（例如，参见HJ90）。因此，Chen和Walker提出的新零空间基矩阵由下式给出：

$$\bar{Z}(\mathbf{q}) = [-J_{r}(\mathbf{q})^{T}\mathrm{adj}(J_{m}(\mathbf{q}))^{T} \quad \det(J_{m}(\mathbf{q}))I]. \quad (4.9)$$

这个零空间基矩阵的一个非常有趣的性质是等式 $m_{kin}(\mathbf{q}) = \det(J(\mathbf{q})J(\mathbf{q})^{T}) = \det(\bar{Z}(\mathbf{q})\bar{Z}(\mathbf{q})^{T})$ 。这个性质和其他有用性质的证明可以在CW93中找到。

4.1.2 一维零空间的情况

当冗余度 $r = n - m$ 为1时，可以避免上一节中寻找可逆子矩阵 $J_{m}(\mathbf{q})$ 的要求。例如，DLR轻型机器人就是这种情况。然后矩阵 $\bar{Z}(\mathbf{q}) \in \mathbb{R}^{r \times n}$ 简化为行向量 $z(\mathbf{q}) = [z_{1}(\mathbf{q}) \dots z_{n}(\mathbf{q})]$ 。考虑矩阵：

$$J_{z}(\mathbf{q}) = \begin{pmatrix} J(\mathbf{q}) \\ z(\mathbf{q}) \end{pmatrix}. \quad (4.10)$$

下面将构造向量 $z(\mathbf{q})$ ，使得矩阵 $J_{z}(\mathbf{q})$ 是可逆的。注意，然后 $J_{z}(\mathbf{q})$ 的逆可以写成 $J_{z}(\mathbf{q})^{-1} = \operatorname{adj}(J_{z}(\mathbf{q})) / \det(J_{z}(\mathbf{q}))$ 。由于 $J_{z}(\mathbf{q})\mathrm{adj}(J_{z}(\mathbf{q})) = \det(J_{z}(\mathbf{q}))I$ ， $\operatorname{adj}(J_{z}(\mathbf{q}))$ 的最后一列 $c_{n}(\mathbf{q})$ 显然满足条件 $J(\mathbf{q})c_{n}(\mathbf{q}) = \mathbf{0}$ 。

注意，伴随矩阵是余子式矩阵的转置，因此列向量 $c_{n}(\mathbf{q})$ 的元素由 $J_{z}(q)$ 的 $(n, i)$ 阶余子式给出。由于这些 $(n, i)$ 阶余子式仅由子矩阵 $J(q)$ 计算，并且不依赖于 $z(q)$ ，因此可以选择 $z(q) = c_{n}(q)^{T}$ 。向量 $z(q)$ 的元素 $z_{i}(q)$ 可以通过下式计算：

$$z_{i}(q) = (-1)^{n + i} \det(J_{i}(q)), \quad (4.11)$$

其中 $J_{i}(q) \in \mathbb{R}^{m \times m}$ 是 $J(q)$ 去掉第 $i$ 列后的矩阵。

4.2 基于投影的方法

阻抗叠加原理为组合不同阻抗行为提供了一个合适的框架。正如HoganHog85a, Hog85b, Hog85c详细描述的那样，这种叠加可以很容易地通过将不同阻抗的输出（即关节扭矩）相加来实现（另见第3.4节）。第3.2节的笛卡尔阻抗控制器给出以下关节扭矩作为输出，参见（3.18）：

$$\begin{array}{c} \boldsymbol{\tau}_{c} = g(q) + J(q)^{T}F_{imp}, \\ F_{imp} = \Lambda(x)\ddot{x}_{d} + \mu(x,\dot{x})\dot{x}_{d} - K_{d}\tilde{x} - D_{d}\dot{\tilde{x}}. \end{array} \quad (4.12)$$

因此，与产生关节扭矩输出向量 $\tau_{n}$ 的零空间阻抗的组合可以简单地通过以下方式完成：

$$\tau = \tau_{c} + \tau_{n}, \quad (4.14)$$

如图4.1所示。在本节中，将展示如何以直观的方式设计这样一个输出为 $\tau_{n}$ 的零空间阻抗。首先，注意对于选择的虚拟平衡位置 $q_{d,0}$ ，可以定义一个简单的关节空间阻抗：

$$\tau_{0} = -D_{n}\dot{q} - K_{n}(q - q_{d,0}), \quad (4.15)$$

其中对称正定矩阵 $K_{n} \in \mathbb{R}^{n \times n}$ 和 $D_{n} \in \mathbb{R}^{n \times n}$ 表示关于配置坐标的期望刚度和阻尼行为。注意，考虑到与笛卡尔阻抗的组合， $q_{d,0}$ 的选择应适当，使其与虚拟笛卡尔平衡位置 $\mathbf{x}_{d}$ 一致。这意味着它应满足条件 $f(q_{d,0}) = \mathbf{x}_{d}$ 。如果不满足这一点，显然不可能同时（在自由运动情况下静态地）达到期望的笛卡尔位置和期望的零空间配置。相反，只能在满足（通常更重要的）笛卡尔平衡条件 $\mathbf{x} = \mathbf{x}_{d}$ 的同时，实现约束成本函数 $(q - q_{d,0})^{T}K_{n}(q - q_{d,0})|_{f(q) = \mathbf{x}_{d}}$ 的（局部）最小化。

（4.15）中的阻抗不能直接使用，因为它会扭曲期望的笛卡尔阻抗行为。为了将关节空间阻抗与笛卡尔阻抗行为在运动学上解耦，可以通过矩阵 $P(q)$ 投影扭矩 $\tau_{0}$ ，即：

$$\tau_{n} = P(q)\tau_{0}, \quad (4.16)$$

到 $J(q)^{T}$ 的值域的补空间上。下面，将更详细地研究三种不同的投影矩阵 $P_{i}(q)$ 。因此，首先考虑关于投影矩阵构造的以下命题BM92（另见定义A.18和A.19）。

命题 4.1. 考虑 $\mathbb{R}^{n}$ 的一个 $m$ 维子空间 $\mathcal{A}$ ，描述为满秩的 $n\times m$ 矩阵 $A\in \mathbb{R}^{n\times m}$ 的值域，即 $\mathcal{A}:= \{\mathbf{y}\in \mathbb{R}^{n} \mid \exists \mathbf{x}\in \mathbb{R}^{m}, \mathbf{y} = A\mathbf{x}\}$ 。设 $G\in \mathbb{R}^{n\times n}$ 是定义共域空间 $\mathbb{R}^{n}$ 度量的对称正定矩阵。则矩阵：

$$P = A(A^{T}G A)^{-1}A^{T}G \in \mathbb{R}^{n\times n} \quad (4.17)$$

是一个投影矩阵（即满足幂等性质 $PP = P$ ），并且它将 $\mathbb{R}^{n}$ 的一个元素（相对于 $G$ ）正交投影到 $\mathcal{A}$ 上（见图4.2）。因此，它满足条件 $PA = A$ 。此外，将 $\mathbb{R}^{n}$ 的一个元素投影到 $\mathcal{A}$ 的正交补 $\mathcal{A}^{\perp}$ 上的投影矩阵由下式给出：

$$P_{c} = I - P \in \mathbb{R}^{n\times n}. \quad (4.18)$$

注意，条件 $P_{c}A = 0$ 和 $P_{c}P_{c} = P_{c}$ 都成立。

方程（4.17）和（4.18）提供了构造零空间阻抗投影矩阵的不同方法。在接下来的两个小节中，基于零空间基矩阵 $Z(q)$ ，使用（4.17）设计一个投影到 $J(q)^{T}$ 的零空间上的投影矩阵 $P(q)$ 。然后将此投影与来自操作空间公式化的动态一致零空间投影（基于（4.18））进行比较。

静态零空间投影

期望的投影 $P(q)$ 应该被设计成使得任何形式为 $\tau_{n} = P(q)\tau_{0}$ 的扭矩向量 $\tau_{n}$ 都正交于（相对于某个至今任意的度量 $G(q)$ ）笛卡尔扭矩 $\tau_{c} = J(q)^{T}F_{imp}$ 。这意味着 $\tau_{c}^{T}G(q)\tau_{n} = 0$ 应该成立，并且显然导致了要求：

$$J(q)G(q)P(q) = 0. \quad (4.19)$$

假设矩阵 $G(q)$ 是对称正定的，以符合度量的资格。矩阵 $Z(q)$ 由 $r$ 个向量 $z_{i}(q)$ 组成，这些向量张成雅可比矩阵的（右）零空间，即 $Z(q) = [z_{1}(q)\cdots z_{r}(q)]^{T}$ 。那么，确保条件（4.19）的 $P(q)$ 的合适选择是投影到 $G(q)^{-1}Z(q)^{T}$ 的值域上（见图4.3）。根据命题4.1（其中 $G(q)^{-1}Z(q)^{T}$ 对应于 $A$ ， $G(q)$ 为所考虑的度量），可以计算这样的投影矩阵为：

$$P_{1}(q) = G(q)^{-1}Z(q)^{T}\left(Z(q)G(q)^{-1}Z(q)^{T}\right)^{-1}Z(q). \quad (4.20)$$

注意，投影 $P_{1}(q)$ 仅基于操纵器的运动学，尚未考虑动力学，因此度量 $G(q)$ 可以任意选择。例如，如果选择欧几里得度量并且向量 $z_{i}(q)$ 在 $Z(q)$ 中形成一个标准正交基，则投影具有简单形式 $Z(q)^{T}Z(q)$ 。

动态一致投影

上一节中度量 $G(q)$ 可以任意选择的事实是仅考虑运动学而未考虑动力学的结果。为了也考虑操纵器的动力学，可以考虑笛卡尔加速度 $\ddot{x}$ 与关节扭矩 $\boldsymbol{\tau}$ 之间的关系。因此，将操纵器动力学（4.3）左乘 $\mathbf{J}(q)M(q)^{-1}$ ，并利用 $\ddot{x} = \mathbf{J}(q)\ddot{q} + \dot{\mathbf{J}}(q)\dot{q}$ ，得到方程：

$$\ddot{x} - \dot{\mathbf{J}}(q)\dot{q} + \mathbf{J}(q)M(q)^{-1}(C(q,\dot{q})\dot{q} + g(q)) = \mathbf{J}(q)M(q)^{-1}(\boldsymbol{\tau} + \boldsymbol{\tau}_{ext}).$$

由此可以很容易地看出，由 $\mathbf{P}_{1}(q)$ 投影的扭矩 $\boldsymbol{\tau}_{0}$ 通常也可能产生末端执行器的加速度 $\ddot{x}$ 。但这是不期望的，因为笛卡尔阻抗控制器和零空间阻抗控制器不应相互干扰。不允许从 $\boldsymbol{\tau}_{0}$ 到 $\ddot{x}$ 的直接前馈的投影称为动态一致的。这显然在以下条件成立时实现：

$$\mathbf{J}(q)M(q)^{-1}\mathbf{P}(q) = \mathbf{0}. \quad (4.21)$$

从（4.21）可以看出，通过将 $\mathbf{P}_{1}(q)$ 左乘 $M(q)$ ，得到的矩阵变得动态一致：

$$\mathbf{P}_{2}(q) = M(q)\mathbf{P}_{1}(q). \quad (4.22)$$

但这种选择有一个缺点，即 $\mathbf{P}_{2}(q)$ 包含一个缩放，因此不再是一个投影矩阵，因为它不满足幂等性质 $\mathbf{P}(q)\mathbf{P}(q) = \mathbf{P}(q)$ 。

一个更优雅的解决方案是选择逆惯性矩阵 $M(q)^{-1}$ 作为关节扭矩的度量 $G(q)$ 。这个度量被许多作者称为自然度量（例如，参见BK00），并导致投影：

$$\mathbf{P}_{3}(q) = M(q)Z(q)^{T}\left(Z(q)M(q)Z(q)^{T}\right)^{-1}Z(q). \quad (4.23)$$

操作空间公式化中的投影

另一种动态一致零空间投影矩阵的表述可以通过考虑命题4.1中的（4.18）找到。根据这个，以 $M(q)^{-1}$ 为关节扭矩的相关度量，投影到 $J(q)^{T}$ 的正交补上的矩阵由下式给出：

$$\mathbf{P}_{4}(q) = \mathbf{I} - \mathbf{J}(q)^{T}\left(\mathbf{J}(q)M(q)^{-1}\mathbf{J}(q)^{T}\right)^{-1}\mathbf{J}(q)M(q)^{-1}. \quad (4.24)$$

这种投影矩阵形式是在操作空间形式主义Kha87中引入的。这里避免了计算零空间基矩阵 $\mathbf{Z}(q)$ 。另一方面，它包含惯性矩阵的逆，因此从计算角度来看，在实时实现上更为困难。（4.23）中必须求逆的矩阵 $Z(q)M(q)Z(q)^{T}$ ，相比之下只是一个 $r \times r$ 的矩阵。然而，得到的矩阵是等价的，即 $P_{3}(q) = P_{4}(q)$ 。这可以通过验证性质 $(P_{3}(q) - P_{4}(q))[J(q)^{T} \quad M(q)Z(q)^{T}] = 0$ 来看出，其中矩阵 $[J(q)^{T} \quad M(q)Z(q)^{T}]$ 是可逆的。

稳定性性质的讨论

乍一看，似乎可以将阻抗叠加原理应用于零空间阻抗的实现而没有任何问题。阻抗由关节空间刚度和阻尼矩阵以及期望的零空间平衡 $q_{d,0}$ 参数化，因此允许相当直观的设计。

如果考虑稳定性分析，只要所有叠加的阻抗分量都是从 $\dot{q}$ 到输出扭矩的无源映射，这种设计方法似乎并不关键。那么它们的叠加（无非是并联互连）也给出了一个无源阻抗，并且可以通过检查各个存储函数之和来照常对整个系统进行稳定性分析。这种论证的问题在于，映射 $\dot{q} \to \tau_{n}$ （表示零空间阻抗）不一定是无源的（由于通过 $P(q)$ 的投影）。这个概念问题是下一节考虑其他零空间处理方法的主要原因，尽管这些方法实现起来更困难。

4.3 任务空间扩展方法

任务空间扩展方法旨在直接扩展任务坐标 $x = f(q)$ 。如果可以找到一组合适的坐标 $n = n(q)$ ，使得对应于新坐标的雅可比矩阵 $J_{e}(q)$ ：

$$\begin{array}{l} y = \begin{pmatrix} x \\ n \end{pmatrix}, \\ \dot{y} = J_{e}(q)\dot{q} = \begin{pmatrix} J(q) \\ \frac{\partial n(q)}{\partial q} \end{pmatrix} \dot{q} \end{array} \quad (4.26)$$

是可逆的，那么阻抗控制问题可以简化为非冗余情况。然而，选择一组适当的附加坐标 $\mathbf{n}(\mathbf{q})$ 是有问题的，因为 $\mathbf{J}_{e}(\mathbf{q})$ 通常比 $\mathbf{J}(\mathbf{q})$ 具有更多的奇异配置。

例如，在Baillieul提出的扩展雅可比方法Bai85中，引入了零空间坐标：

$$n(q) = Z(q)\frac{\partial V_n(q)^T}{\partial q} \quad (4.27)$$

其中目标函数 $V_{n}(q)$ 是要通过零空间运动局部优化的期望函数。势能函数 $V_{n}(q)$ 应被选择为具有约束局部最优：

$$\mathbf{q}^{*} = \arg_{\mathbf{q}} \min_{\mathbf{f}(\mathbf{q}) = \mathbf{x}} V_{n}(\mathbf{q}).$$

优化目标函数 $V_{n}(q)$ 意味着跟踪形式为 $\mathbf{n}_{d}(t) = \mathbf{0}$ 的期望零空间运动，这显然简化为一个调节问题。

这种方法的主要缺点是它通常不会导致动态一致的零空间投影，因为 $\mathbf{J}(\mathbf{q})M(\mathbf{q})^{-1}(\partial \mathbf{n}(\mathbf{q}) / \partial \mathbf{q})^{T} \neq 0$ 。因此，零空间动态与原始任务动态完全耦合，这通常是不期望的。此外，每当 $(\partial \mathbf{n}(\mathbf{q}) / \partial \mathbf{q})Z(\mathbf{q})^{T} = \mathbf{0}$ Bai85时，这种方法也会引入新的奇异性。

下面，描述了一种可能的方法，即使在运动学上不一致的微分 $\partial \mathbf{n}(\mathbf{q}) / \partial \mathbf{q}$ 被使用时，也可以强制笛卡尔阻抗与零空间阻抗解耦。任务空间坐标中的动力学方程（4.3）可以通过将雅可比矩阵 $\mathbf{J}(\mathbf{q})$ 替换为 $\mathbf{J}_{e}(\mathbf{q})$ ，使用方程（3.10）和（3.11）计算。进一步将关节扭矩 $\boldsymbol{\tau}$ 用笛卡尔分量 $\mathbf{F}_{x}\in \mathbb{R}^{m}$ 和零空间分量 $\mathbf{F}_{n}\in \mathbb{R}^{r}$ 表示，它们通过以下方式与 $\boldsymbol{\tau}$ 相关：

$$\boldsymbol{\tau} = \mathbf{J}_{e}(\mathbf{q})^{T} \begin{pmatrix} \mathbf{F}_{x} \\ \mathbf{F}_{n} \end{pmatrix}. \quad (4.28)$$

外部扭矩 $\boldsymbol{\tau}_{ext}$ 以相同的方式用 $\mathbf{F}_{ext,x}$ 和 $\mathbf{F}_{ext,n}$ 分解。由（3.10）-（3.11）得到的动力学方程可以写成：

$$\Lambda_{e}(\mathbf{q}) \begin{pmatrix} \ddot{\mathbf{x}} \\ \ddot{\mathbf{n}} \end{pmatrix} + \mu_{e}(\mathbf{q},\dot{\mathbf{q}}) \begin{pmatrix} \dot{\mathbf{x}} \\ \dot{\mathbf{n}} \end{pmatrix} + \mathbf{J}_{e}(\mathbf{q})^{-T} g(\mathbf{q}) = \begin{pmatrix} \mathbf{F}_{x} + \mathbf{F}_{ext,x} \\ \mathbf{F}_{n} + \mathbf{F}_{ext,n} \end{pmatrix} \quad (4.29)$$

其中惯性矩阵 $\Lambda_{e}(\mathbf{q})$ 和科里奥利/离心矩阵 $\mu_{e}(\mathbf{q},\dot{\mathbf{q}})$ 可以相应地分块为：

$$\Lambda_{e}(\mathbf{q}) = \begin{bmatrix} \Lambda_{e,x}(\mathbf{q}) & \Lambda_{e,c}(\mathbf{q}) \\ \Lambda_{e,c}(\mathbf{q})^{T} & \Lambda_{e,n}(\mathbf{q}) \end{bmatrix}, \quad (4.30)$$ $$\mu_{e}(q,\dot{q}) = \begin{bmatrix} \mu_{e,xx}(q,\dot{q}) & \mu_{e,xn}(q,\dot{q}) \\ \mu_{e,nx}(q,\dot{q}) & \mu_{e,nn}(q,\dot{q}) \end{bmatrix}. \quad (4.31)$$

使用第3.2节的控制器将得到一个具有完全占满惯性矩阵的闭环动态。相反，期望的是 $\mathbf{x}$ 的动态与 $\mathbf{n}$ 的动态解耦。这至少可以通过以下形式的前馈补偿近似实现：

$$\begin{array}{rl} & \begin{pmatrix} F_{x} \\ F_{n} \end{pmatrix} = J_{e}(q)^{-T}g(q) + \bar{\mu}(q,\dot{q}) \begin{pmatrix} \dot{x} \\ \dot{n} \end{pmatrix} - \begin{pmatrix} F_{ext,x} \\ F_{ext,n} \end{pmatrix} \\ & + \Lambda_{e}(q) \begin{bmatrix} \Lambda_{e,x}(q) & 0 \\ 0 & \Lambda_{e,n}(q) \end{bmatrix}^{-1} \begin{pmatrix} \bar{F}_{x} + F_{ext,x} \\ \bar{F}_{n} + F_{ext,n} \end{pmatrix}, \end{array} \quad (4.32)$$

其中 $\bar{\mu}(q,\dot{q})$ 由下式给出：

$$\bar{\mu}(q,\dot{q}) = \mu_{e}(q,\dot{q}) - \Lambda_{e}(q) \begin{bmatrix} \Lambda_{e,x}(q) & 0 \\ 0 & \Lambda_{e,n}(q) \end{bmatrix}^{-1} \begin{bmatrix} \mu_{e,xx}(q,\dot{q}) & 0 \\ 0 & \mu_{e,nn}(q,\dot{q}) \end{bmatrix},$$

并且 $\bar{F}_{x}$ 和 $\bar{F}_{n}$ 是中间控制输入。因此，得到以下系统方程：

$$\begin{array}{rl} & \Lambda_{e,x}(q)\ddot{\mathbf{x}} + \mu_{e,xx}(q,\dot{q})\dot{\mathbf{x}} = \bar{\mathbf{F}}_{x} + \mathbf{F}_{ext,x}, \\ & \Lambda_{e,n}(q)\ddot{\mathbf{n}} + \mu_{e,nn}(q,\dot{q})\dot{\mathbf{n}} = \bar{\mathbf{F}}_{n} + \mathbf{F}_{ext,n}. \end{array} \quad (4.33)$$

注意，由于惯性矩阵和科里奥利/离心矩阵对关节位置和速度的依赖，这些方程仍然是耦合的。现在可以通过输入 $\bar{F}_{n}$ 为坐标 $\mathbf{n}$ 设计一个零空间控制器，并通过输入 $\bar{F}_{x}$ 以与第3.2节相同的方式设计一个笛卡尔阻抗控制器。使用正定的刚度和阻尼矩阵 $\mathbf{K}_{d}$ 、 $\mathbf{D}_{d}$ 、 $\mathbf{K}_{n}$ 和 $\mathbf{D}_{n}$ 分别用于笛卡尔阻抗和零空间阻抗，完整的控制律由（4.28）、（4.32）和以下方程给出：

$$\begin{pmatrix} \bar{F}_{x} \\ \bar{F}_{n} \end{pmatrix} = \begin{pmatrix} \Lambda_{e,x}(q)\ddot{x}_{d} + \mu_{e,xx}(q,\dot{q})\dot{x}_{d} - K_{d}\tilde{x} - D_{d}\dot{\tilde{x}} \\ -K_{n}n - D_{n}\dot{n} \end{pmatrix}. \quad (4.35)$$

对于最终闭环系统的稳定性性质，可以直接参考命题3.4和命题3.5，因为闭环系统具有与第3.2节期望动态相同的形式。注意，该控制器在（4.32）中包含了外部扭矩 $\boldsymbol{\tau}_{ext}$ 的反馈，因为（4.32）重塑了有效惯性矩阵。然而，从实际角度来看，通常只有作用在末端执行器上的外力和力矩可以通过安装在机器人末端执行器上的力-力矩传感器测量。但对于控制律（4.32）的实现，还需要零空间力 $\mathbf{F}_{ext,n}$ 。这个问题已经在第3.2节中更详细地讨论过，以及 $\mathbf{J}_{e}(\mathbf{q})$ 的奇异性，是这种方法的主要限制。相比之下，在关节空间分解方法中，可以通过使用动态一致的零空间投影矩阵，在不反馈外力的情况下实现分块对角线（因此解耦的）惯性矩阵。

4.4 关节空间分解方法

关节空间分解方法不是扩充任务坐标 $\mathbf{x}$ ，而是扩充任务速度 $\dot{\mathbf{x}}$ 。这是通过引入额外的零空间速度 $\mathbf{v}_{n} = \mathbf{N}(\mathbf{q})\dot{\mathbf{q}}$ ，通过适当的矩阵 $\mathbf{N}(\mathbf{q})$ 来实现的。与 $\dot{\mathbf{x}}$ 一起，有：

$$\begin{pmatrix} \dot{\mathbf{x}} \\ \mathbf{v}_{n} \end{pmatrix} = \mathbf{J}_{N}(\mathbf{q})\dot{\mathbf{q}} = \begin{pmatrix} \mathbf{J}(\mathbf{q}) \\ \mathbf{N}(\mathbf{q}) \end{pmatrix} \dot{\mathbf{q}}. \quad (4.36)$$

零空间矩阵 $\mathbf{N}(\mathbf{q})$ 必须被选择为使扩展雅可比矩阵 $\mathbf{J}_{N}(\mathbf{q})$ 是可逆的。

原则上，也可以积分零空间速度 $\mathbf{v}_{n}(t)$ 以获得新的零空间坐标 $\mathbf{n}(t) = \int_{0}^{t} \mathbf{v}_{n}(\mathbf{q}(\tau)) \mathrm{d}\tau$ 。然后，零空间坐标的构造允许人们使用所有为非冗余情况开发的技术，类似于任务空间扩展方法，也适用于冗余情况。然而，这有两个重要的含义：首先，量 $\mathbf{n}(t)$ 没有任何有用的几何意义。因此，不可能表述一个有意义的位置期望轨迹 $\mathbf{n}_{d}(t)$ 。相反，期望轨迹必须表述为期望速度HHS89。其次，由于零空间速度的积分，得到的控制器不描述静态状态反馈，而是动态状态反馈。

在这一点上，重要的是要注意，通常零空间基矩阵 $\mathbf{Z}(\mathbf{q})$ 不能表示为函数 $\mathbf{n}(\mathbf{q})$ 的微分，因为这必然要求 $\mathbf{z}_{k}(\mathbf{q})$ 满足可积性条件 $\frac{\partial \mathbf{z}_{k,i}(\mathbf{q})}{\partial q_{j}} = \frac{\partial \mathbf{z}_{k,j}(\mathbf{q})}{\partial q_{i}}$ 。当然，这通常不满足。

为了获得动态一致的零空间矩阵，可以选择：

$$\mathbf{N}(\mathbf{q}) = \left(\mathbf{Z}(\mathbf{q})M(\mathbf{q})\mathbf{Z}(\mathbf{q})^{T}\right)^{-1}\mathbf{Z}(\mathbf{q})M(\mathbf{q}), \quad (4.37)$$

其中 $\mathbf{Z}(q)$ 是 $\mathbf{J}(q)$ 的零空间基矩阵。注意，为了计算 $\mathbf{N}(q)$ ，必须求逆矩阵 $\mathbf{Z}(q)M(q)\mathbf{Z}(q)^{T}$ ，这只是一个 $r\times r$ 的矩阵。这个扩展雅可比矩阵 $\mathbf{J}_{N}(q)$ 的可逆性在A.5节中展示。其中还展示了 $\mathbf{J}_{N}(q)$ 的逆由下式给出：

$$\mathbf{J}_{N}(q)^{-1} = [\mathbf{J}^{M+}(q) \quad \mathbf{Z}(q)^{T}], \quad (4.38)$$

其中加权伪逆 $\mathbf{J}^{M+}(q)$ 定义为：

$$\mathbf{J}^{M+}(q) = M(q)^{-1}\mathbf{J}(q)^{T}(\mathbf{J}(q)M(q)^{-1}\mathbf{J}(q)^{T})^{-1}. \quad (4.39)$$

因此，关节速度 $\dot{q}$ 可以由笛卡尔速度 $\dot{x}$ 和零空间速度 $\mathbf{v}_{n}$ 通过下式计算：

$$\dot{q} = \mathbf{J}^{M+}(q)\dot{x} + \mathbf{Z}(q)^{T}\mathbf{v}_{n}. \quad (4.40)$$

动力学方程（4.3）可以通过（3.10）和（3.11）用扩展速度坐标编写。关节扭矩 $\boldsymbol{\tau}$ 用笛卡尔力 $\mathbf{F}_{x}$ 和零空间力 $\mathbf{F}_{n}$ 表示，它们通过以下方式与 $\boldsymbol{\tau}$ 相关：

$$\boldsymbol{\tau} = [\mathbf{J}(q)^{T} \quad \mathbf{N}(q)^{T}] \begin{pmatrix} \mathbf{F}_{x} \\ \mathbf{F}_{n} \end{pmatrix}. \quad (4.41)$$

广义力 $\mathbf{F}_{x}$ 和 $\mathbf{F}_{n}$ 被用作新的输入。外部扭矩 $\boldsymbol{\tau}_{ext}$ 以相同方式用广义外力 $\mathbf{F}_{ext,x}$ 和 $\mathbf{F}_{ext,n}$ 分解。动力学方程则具有以下形式：

$$\Lambda_{N}(q) \begin{pmatrix} \ddot{\mathbf{x}} \\ \dot{\mathbf{v}}_{n} \end{pmatrix} + \mu_{N}(q,\dot{q}) \begin{pmatrix} \dot{\mathbf{x}} \\ \mathbf{v}_{n} \end{pmatrix} + \mathbf{J}_{N}(q)^{-T} \mathbf{g}(q) = \begin{pmatrix} \mathbf{F}_{x} + \mathbf{F}_{ext,x} \\ \mathbf{F}_{n} + \mathbf{F}_{ext,n} \end{pmatrix} \quad (4.42)$$

由于 $\mathbf{N}(q)$ 是动态一致的，惯性矩阵 $\Lambda_{N}(q)$ 是分块对角的：

$$\begin{array}{rl} & \Lambda_{N}(q) = \mathbf{J}_{N}(q)^{-T} M(q) \mathbf{J}_{N}(q)^{-1} \\ & = \begin{bmatrix} (\mathbf{J}(q)M(q)^{-1}\mathbf{J}(q)^{T})^{-1} & \mathbf{0} \\ \mathbf{0} & \mathbf{Z}(q)M(q)\mathbf{Z}(q)^{T} \end{bmatrix} \\ & = \begin{bmatrix} \Lambda_{x}(q) & \mathbf{0} \\ \mathbf{0} & \Lambda_{n}(q) \end{bmatrix}, \end{array} \quad (4.45)$$

并且 $\mu_{N}(q, \dot{q})$ 具有以下形式：

$$\begin{array}{rl} & \mu_{N} = \begin{bmatrix} \mu_{x} & \mu_{xn} \\ \mu_{nx} & \mu_{n} \end{bmatrix} \\ & = \begin{bmatrix} \Lambda_{x}(J M^{-1}C - \dot{J})J^{M+} & \Lambda_{x}(J M^{-1}C - \dot{J})Z^{T} \\ -(\dot{Z} M + Z C^{T})J^{M+} & \Lambda_{n}Z(C Z^{T} + M\dot{Z}^{T}) \end{bmatrix}. \end{array} \quad (4.46)$$

注意，等式 $\mu_{xn}(q, \dot{q}) = -\mu_{nx}(q, \dot{q})^T$ 成立，这对应于引理3.2，因为 $\Lambda_{N}(q)$ 是分块对角的。

4.4.1 控制器设计

与文献中其他冗余分辨率方法相比，以下控制算法侧重于无惯性矩阵整形的笛卡尔阻抗控制器（原因与第3.2节讨论的相同）。此外，零空间阻抗控制器的目标是实现零空间运动与外部（广义）力之间的动态关系，该关系可以用刚度和阻尼系数来指定。

以下分析将基于具有状态变量 $q, \dot{x}$ 和 $v_{n}$ 的动力学方程（4.42）。

在动力学方程（4.42）中，通过使用动态一致的零空间矩阵 $N(q)$ ，已经消除了惯性矩阵中非对角线项导致的笛卡尔坐标和零空间坐标之间的耦合。此外，科里奥利/离心矩阵 $\mu_{N}(q, \dot{x}, v_{n})$ 中仍然存在耦合项。这些项与重力扭矩一起，可以通过以下形式的反馈律消除，而无需力反馈：

$$\begin{array}{rl} & \begin{pmatrix} F_{x} \\ F_{n} \end{pmatrix} = \mathbf{J}_{N}(q)^{-T} g(q) + \begin{pmatrix} \bar{F}_{x} \\ \bar{F}_{n} \end{pmatrix} \\ & + \begin{bmatrix} 0 & \mu_{xn}(q,\dot{x},v_{n}) \\ \mu_{nx}(q,\dot{x},v_{n}) & 0 \end{bmatrix} \begin{pmatrix} \dot{x} \\ v_{n} \end{pmatrix}, \end{array} \quad (4.48)$$

其中新的输入变量为 $\bar{F}_{x}$ 和 $\bar{F}_{n}$ 。注意，从无源性的角度来看，由于斜对称性质 $\mu_{xn}(q, \dot{x}, v_{n}) = -\mu_{nx}(q, \dot{x}, v_{n})^T$ ，即使在模型参数不确定的情况下，消除（4.48）中不期望的科里奥利和离心项预计也不会非常关键。到目前为止的闭环系统由下式给出：

$$\begin{array}{rl} & \Lambda_{x}(q)\ddot{\mathbf{x}} + \mu_{x}(q,\dot{\mathbf{x}},\mathbf{v}_{n})\dot{\mathbf{x}} = \bar{\mathbf{F}}_{x} + \mathbf{F}_{ext,x}, \\ & \Lambda_{n}(q)\dot{\mathbf{v}}_{n} + \mu_{n}(q,\dot{\mathbf{x}},\mathbf{v}_{n})\mathbf{v}_{n} = \bar{\mathbf{F}}_{n} + \mathbf{F}_{ext,n}, \\ & \dot{\mathbf{q}} = \mathbf{J}^{+M}(q)\dot{\mathbf{x}} + \mathbf{Z}(q)^{T}\mathbf{v}_{n}. \end{array} \quad (4.51)$$

下面只处理虚拟笛卡尔平衡位置 $\mathbf{x}_{d}$ 固定的调节情况。对于系统的笛卡尔部分，使用类似于第3.2节的控制器，具有对称正定的刚度和阻尼矩阵 $\mathbf{K}_{d}$ 和 $\mathbf{D}_{d}$ ：

$$\bar{\mathbf{F}}_{x} = -\mathbf{K}_{d}(\mathbf{f}(\mathbf{q}) - \mathbf{x}_{d}) - \mathbf{D}_{d}\dot{\mathbf{x}}. \quad (4.52)$$

此外，输入 $\bar{\mathbf{F}}_{n}$ 用于控制零空间运动。下面讨论 $\bar{\mathbf{F}}_{n}$ 的两种不同反馈律。第一种对应于文献中常见的零空间控制器，因此只作简要回顾。这种零空间控制器的思想是为期望速度 $\mathbf{v}_{n,d} = - \mathbf{Z}(q)(\frac{\partial V_{n}(q)}{\partial q})^{T}$ 实现一个速度控制器，其中 $V_{n}(q)$ 是通过零空间运动局部优化的给定成本函数。所考虑的控制器由下式给出：

$$\bar{\mathbf{F}}_{n} = \Lambda_{n}(q)\dot{\mathbf{v}}_{n,d} + \mu_{n}(q,\dot{\mathbf{x}},\mathbf{v}_{n})\mathbf{v}_{n,d} - \mathbf{D}_{n}(\mathbf{v}_{n} - \mathbf{v}_{n,d}), \quad (4.53)$$

其中 $\mathbf{D}_{n}\in \mathbb{R}^{r\times r}$ 是一个正定控制器增益矩阵。完整的闭环系统（其中 $\mathbf{e}_{n} = \mathbf{v}_{n} - \mathbf{v}_{n,d}$ ）由（4.51）和以下方程给出：

$$\begin{array}{rl} & \Lambda_{x}(q)\ddot{\mathbf{x}} + (\mu_{x}(q,\dot{\mathbf{x}},\mathbf{v}_{n}) + \mathbf{D}_{d})\dot{\mathbf{x}} + \mathbf{K}_{d}(\mathbf{f}(q) - \mathbf{x}_{d}) = \mathbf{F}_{ext,x}, \\ & \Lambda_{n}(q)\dot{\mathbf{e}}_{n} + (\mu_{n}(q,\dot{\mathbf{x}},\mathbf{v}_{n}) + \mathbf{D}_{n})\mathbf{e}_{n} = \mathbf{F}_{ext,n}. \end{array} \quad (4.54)$$

对于这个系统，可以证明在自由运动情况下（即 $\mathbf{F}_{ext,x} = \mathbf{0}$ 且 $\mathbf{F}_{ext,n} = \mathbf{0}$ ），变量 $\dot{\mathbf{x}}$ 、 $\mathbf{f}(q) - \mathbf{x}_{d}$ 和 $\mathbf{e}_{n}$ 收敛到零。基本上，这在HHS89, OCY98, NSV99的类似情况中得到了证明。此外，还可以证明，当笛卡尔误差为零时（即 $\mathbf{f}(q) = \mathbf{x}_{d}$ ），关节角度 $\mathbf{q}$ 将局部收敛到 $V_{n}(q)$ 的约束最小值HHS89。

（4.53）中控制器的一个问题是它不允许明确地用刚度和阻尼系数指定零空间行为，而是用优化成本函数来指定。因此，这种方法并不真正适合实现零空间阻抗。此外，除非使用如OCY98中的完全质量解耦，否则只能证明 $\mathbf{e}_{n}$ 的收敛性（而非稳定性）。因此，下面将讨论 $\bar{\mathbf{F}}_{n}$ 的另一种选择，它特别设计用于实现无外力反馈的零空间阻抗问题。因此，考虑反馈律：

$$\bar{\mathbf{F}}_{n} = -\mathbf{D}_{n}\mathbf{v}_{n} + \mathbf{u}_{n}(q), \quad (4.56)$$

其中 $D_{n}$ 是对应于期望零空间阻尼的正定矩阵。项 $\mathbf{u}_{n}(\mathbf{q})$ 将根据特定的零空间刚度项来设计，该项允许（至少局部地）证明自由运动情况下闭环系统的渐近稳定性。对于控制器（4.56），闭环系统由下式给出：

$$\begin{array}{rl} & \Lambda_{x}(\mathbf{q})\ddot{\mathbf{x}} + (\mu_{x}(\mathbf{q},\dot{\mathbf{x}},\mathbf{v}_{n}) + \mathbf{D}_{d})\dot{\mathbf{x}} + \mathbf{K}_{d}(\mathbf{f}(\mathbf{q}) - \mathbf{x}_{d}) = \mathbf{F}_{ext,x}, \\ & \Lambda_{n}(\mathbf{q})\dot{\mathbf{v}}_{n} + (\mu_{n}(\mathbf{q},\dot{\mathbf{x}},\mathbf{v}_{n}) + \mathbf{D}_{n})\mathbf{v}_{n} - \mathbf{u}_{n}(\mathbf{q}) = \mathbf{F}_{ext,n}, \end{array} \quad (4.57)$$

以及：

$$\dot{\mathbf{q}} = \mathbf{J}^{+M}(\mathbf{q})\dot{\mathbf{x}} + \mathbf{Z}(\mathbf{q})^{T}\mathbf{v}_{n}. \quad (4.59)$$

在下一节中，项 $\mathbf{u}_{n}(\mathbf{q})$ 将被设计为使得可以给出闭环系统的（局部）稳定性证明。

4.4.2 稳定性分析

（4.57）-（4.59）的稳定性分析将基于以下两个关于半正定李雅普诺夫函数稳定性分析的理论。所使用的条件稳定性符号在附录A.2中定义。

定理 4.2 考虑形式为 $\dot{\mathbf{z}} = \mathbf{f}(\mathbf{z})$ 、 $\mathbf{z}\in \mathbb{R}^{n}$ 的系统，平衡点为 $z^{*}$ 。设 $V(z)$ 是一个 $C^{1}$ 正半定函数，它沿系统解的时间导数是负半定的，即：

$$\dot{V}(z) = \frac{\partial V(z)}{\partial z} f(z) \leq 0. \quad (4.60)$$

设 $\mathcal{A}$ 是包含在 $\{z\in \mathbb{R}^{n} \mid \dot{V}(z) = 0\}$ 中的最大正不变量。如果 $z^{*}$ 相对于 $\mathcal{A}$ 是条件渐近稳定的，那么它是 $\dot{\mathbf{z}} = \mathbf{f}(\mathbf{z})$ 的稳定平衡点。

证明. 这个定理归功于Iggidr等人IKO96。证明也可以在SJK97中找到。

在vdS00中，对无源系统给出了该定理的进一步扩展。严格输出无源性的特殊情况，这将用于（4.57）-（4.59）的稳定性分析，在以下定理中陈述。该定理的证明可以在vdS00中找到。

定理 4.3 设系统：

$$\begin{array}{r} \dot{\mathbf{z}} = \mathbf{g}_1(\mathbf{z}) + \mathbf{g}_2(\mathbf{z})\mathbf{u} \\ \mathbf{y} = \mathbf{h}(\mathbf{z}) \end{array} \quad (4.61)$$

状态为 $z\in \mathbb{R}^{n}$ ，输入为 $\mathbf{u}\in \mathbb{R}^{m}$ ，输出为 $\mathbf{y}\in \mathbb{R}^{m}$ ，对于输出 $\mathbf{y} = \mathbf{h}(\mathbf{z})$ 是严格输出无源的。进一步设 $\mathcal{A}$ 是包含在 $\{z\in \mathbb{R}^{n} \mid \mathbf{h}(z) = \mathbf{0}\}$ 中的最大正不变量。如果平衡点 $z^{*}$ 相对于 $\mathcal{A}$ 是条件渐近稳定的，那么对于 $\mathbf{u} = \mathbf{0}$ ，它是渐近稳定的。

定理4.3用于以下（4.57）-（4.59）的稳定性分析。零空间阻抗项 $u(q)$ 将被设计为使得对于给定的关节配置 $q_{d}$ （必须满足条件 $f(q_{d}) = x_{d}$ ），点 $(q, \dot{x}, v_{n}) = (q_{d}, \dot{x}_{d}, \dot{v}_{n,d})$ ，其中 $\dot{x}_{d} = 0$ 且 $\dot{v}_{n,d} = 0$ ，在无外力 $F_{ext,x}$ 和 $F_{ext,n}$ 作用时，是（4.57）-（4.59）的渐近稳定平衡点。在下面的推导中，外力 $F_{ext,x}$ 将对应于定理4.3中的输入 $u$ 。

考虑以下正半定李雅普诺夫函数：

$$S(q, \dot{x}, v_{n}) = \frac{1}{2} \dot{x}^{T} \Lambda_{x}(q) \dot{x} + \frac{1}{2} (f(q) - x_{d})^{T} K_{d} (f(q) - x_{d}). \quad (4.63)$$

$S(q, \dot{x}, v_{n})$ 沿（4.57）-（4.59）解的时间导数由下式给出：

$$\dot{S}(q, \dot{x}, v_{n}) = -\dot{x}^{T} D_{d} \dot{x} - \dot{x}^{T} F_{ext,x}. \quad (4.64)$$

由此可以得出，在情况 $F_{ext,x} = 0$ 下，定理4.2中的条件 $\dot{S}(q, \dot{x}, v_{n}) \leq 0$ 成立。此外，通过将 $S(q, \dot{x}, v_{n})$ 视为存储函数，可以得出该系统对于输出 $\dot{x}$ （对应输入 $F_{ext,x}$ ）是严格输出无源的，其中 $\beta = \lambda_{\min}(D_{d})$ 。根据定理4.3，接下来必须证明该系统在 $\{(q, \dot{x}, v_{n}) \in \mathbb{R}^{2n} \mid \dot{x} = 0\}$ 中的最大正不变量内是条件渐近稳定的。进一步考虑自由运动情况，即 $F_{ext,x} = 0$ 和 $F_{ext,n} = 0$ 。对于 $\dot{x} = 0$ 成立的最大正不变量可以从（4.57）推导出，并给出：

$$\mathcal{A} = \{(q, \dot{x}, v_{n}) \mid f(q) = x_{d}, \dot{x} = 0\}. \quad (4.65)$$

还要注意，由于 $\mathcal{A}$ 是一个正不变量，所有从 $\mathcal{A}$ 开始的轨迹都将保持在 $\mathcal{A}$ 中。然后考虑李雅普诺夫函数候选：

$$V_{A}(q, v_{n}) = \frac{1}{2} v_{n}^{T} \Lambda_{n}(q) v_{n} + \frac{1}{2} (q - q_{d})^{T} K_{n} (q - q_{d}) \quad (4.66)$$

其中 $K_{n}$ 是一个对称正定关节刚度矩阵。在集合 $\mathcal{A}$ 中，函数 $V_{A}(q, v_{n})$ 显然在 $(q_{d}, 0, 0)$ 附近是正定的。 $V_{A}(q, v_{n})$ 在 $\mathcal{A}$ 中沿（4.57）-（4.59）解的时间导数由下式给出：

$$\begin{array}{rl} \dot{V}_{A}(q, v_{n}) & = -v_{n}^{T} D_{n} v_{n} + v_{n}^{T} u_{n}(q) + (q - q_{d})^{T} K_{n} \dot{q} \\ & = -v_{n}^{T} D_{n} v_{n} + v_{n}^{T} u_{n}(q) + (q - q_{d})^{T} K_{n} Z(q)^{T} v_{n} \\ & = -v_{n}^{T} D_{n} v_{n} + v_{n}^{T} (u_{n}(q) + Z(q) K_{n} (q - q_{d})) \end{array} \quad (4.67)$$

通过选择：

$$u_{n}(q) = -Z(q) K_{n} (q - q_{d}) \quad (4.68)$$

相对于 $\mathcal{A}$ 的稳定性由 $\dot{V}_{A}(\mathbf{q},\mathbf{v}_{n}) = -\mathbf{v}_{n}^{T}\mathbf{D}_{n}\mathbf{v}_{n} \leq 0$ 得出。为了也证明相对于 $\mathcal{A}$ 的渐近稳定性，可以参考拉萨尔不变性原理。可以证明， $\mathcal{A}$ 中的所有轨迹将收敛到包含在以下集合中的最大正不变量：

$$\{(q, \dot{x}, v_{n}) \mid f(q) = x_{d}, \dot{x} = 0, v_{n} = 0\}. \quad (4.69)$$

因此，关节变量 $\mathbf{q}$ 将收敛到一个满足条件 $\mathbf{f}(\mathbf{q}) = \mathbf{x}_{d}$ 和 $\mathbf{u}_{n}(\mathbf{q}) = - \mathbf{Z}(\mathbf{q})\mathbf{K}_{n}(\mathbf{q} - \mathbf{q}_{d}) = \mathbf{0}$ 的集合。这不仅适用于 $\mathbf{q} = \mathbf{q}_{d}$ ，也适用于所有满足 $\mathbf{Z}(\mathbf{q})\mathbf{K}_{n}(\mathbf{q} - \mathbf{q}_{d}) = \mathbf{0}$ 的位置 $\mathbf{q}$ 。前者对应于期望平衡点，而后者对应于零空间基向量 $\mathbf{Z}(\mathbf{q})$ 正交于 $\mathbf{K}_{n}(\mathbf{q} - \mathbf{q}_{d})$ 的点。注意，点 $\mathbf{q} = \mathbf{q}_{d}$ 对应于成本函数 $V_{q}(\mathbf{q}) = (\mathbf{q} - \mathbf{q}_{d})^{T}\mathbf{K}_{n}(\mathbf{q} - \mathbf{q}_{d})$ 的全局最小值，而满足 $\mathbf{Z}(\mathbf{q})\mathbf{K}_{n}(\mathbf{q} - \mathbf{q}_{d}) = \mathbf{0}$ 的点对应于 $V_{q}(q)$ 的约束局部最小值 $\arg_{q} \min_{f(q) = x_{d}} V_{q}(q)$ 。利用定理4.3，可以得出结论，系统（4.57）-（4.59）的解曲线局部地渐近趋近于 $V_{q}(\mathbf{q})$ 的约束局部最小值集合。上述分析的结果可以总结如下。

命题 4.4. 考虑系统（4.57）-（4.59），其中 $\mathbf{u}(\mathbf{q}) = - \mathbf{Z}(\mathbf{q})\mathbf{K}_{n}(\mathbf{q} - \mathbf{q}_{d})$ 。矩阵 $\mathbf{K}_{d}, \mathbf{D}_{d} \in \mathbb{R}^{m\times m}$ 和 $\mathbf{K}_{n}, \mathbf{D}_{n} \in \mathbb{R}^{r\times r}$ 都假定为对称正定。假设雅可比矩阵 $\mathbf{J}(\mathbf{q})$ 在所考虑的工作空间内非奇异，并且关节配置 $\mathbf{q}_{d}$ 满足 $\mathbf{f}(\mathbf{q}_{d}) = \mathbf{x}_{d}$ 。则系统对于输入 $\mathbf{F}_{ext,x}$ 和输出 $\dot{\mathbf{x}}$ 是严格输出无源的。此外，解曲线局部地渐近趋近于 $V_{q}(\mathbf{q}) = (\mathbf{q} - \mathbf{q}_{d})^{T}\mathbf{K}_{n}(\mathbf{q} - \mathbf{q}_{d})$ 的约束局部最小值集合。特别地，点 $\mathbf{q}_{d}$ （它是一个孤立的约束最小值）是渐近稳定的。

推导控制器的讨论

通过总结前面的所有部分，可以看出完整的控制器方程由下式给出：

$$\begin{array}{rl} & \tau = g(q) + J(q)^{T}\hat{F}_{x} + N(q)^{T}\hat{F}_{n} \\ & \hat{F}_{x} = \mu_{xn}(q,\dot{x},v_{n})v_{n} - D_{d}\dot{x} - K_{d}(f(q) - x_{d}) \\ & \hat{F}_{n} = \mu_{nx}(q,\dot{x},v_{n})\dot{x} - D_{n}v_{n} - Z(q)K_{n}(q - q_{d}). \end{array} \quad (4.70)$$

值得指出的是，最后一项 $N(q)^{T}Z(q)K_{n}(q - q_{d})$ ，实际上代表了零空间刚度的实现，当使用（4.23）中的投影矩阵时，它恰好对应于（4.16）中的相关项。

本节的目的是推导一个控制器，该控制器实现与笛卡尔动态解耦的零空间阻抗（以刚度和阻尼表示），同时避免外力反馈。应该提到的是，如果不需要这种解耦，或者可以使用外力反馈，那么可以改用第4.3节中讨论的控制器。本节稳定性分析中最关键的一点是它只允许局部稳定性陈述，并且不清楚如何估计吸引域。还要注意，函数 $S_{A}(\mathbf{q},\dot{\mathbf{x}},\mathbf{v}_{n}) = S(\mathbf{q},\dot{\mathbf{x}},\mathbf{v}_{n}) + V_{A}(\mathbf{q},\mathbf{v}_{n})$ 不能用作闭环系统的李雅普诺夫函数。 $S_{A}(\mathbf{q},\dot{\mathbf{x}},\mathbf{v}_{n})$ 沿（4.57）-（4.59）解的时间导数由下式给出：

$$\dot{S}_{A} = - \begin{pmatrix} \dot{\mathbf{x}} \\ \mathbf{v}_{n} \end{pmatrix}^{T} \begin{bmatrix} \mathbf{D}_{d} & \mathbf{0} \\ \mathbf{0} & \mathbf{D}_{n} \end{bmatrix} \begin{pmatrix} \dot{\mathbf{x}} \\ \mathbf{v}_{n} \end{pmatrix} + (\mathbf{q} - \mathbf{q}_{d})^{T}\mathbf{K}_{n}\mathbf{J}^{M+}(\mathbf{q})\dot{\mathbf{x}}.$$

额外的项 $(\mathbf{q} - \mathbf{q}_{d})^{T}\mathbf{K}_{n}\mathbf{J}^{M+}(\mathbf{q})\dot{\mathbf{x}}$ 阻止了导数 $\dot{S}_{A}$ 成为负半定，当系统被约束到集合 $\mathcal{A}$ 时它会消失，因此没有干扰上述基于定理4.3的稳定性分析。此外，分析仅限于虚拟平衡点 $\mathbf{x}_{d}$ 恒定的调节情况。注意，对于第4.3节的控制器，这些限制并不存在。

尽管如此，本节的分析允许为一个零空间阻抗控制器提供稳定性证明，该控制器除了在（4.48）中取消了一些耦合项之外，与简单的动态一致投影基于控制器相同。此外，还证明了严格输出无源性。

4.5 总结

本章讨论了冗余机器人的阻抗控制问题。基于上一章的讨论，将无惯性整形的期望笛卡尔阻抗作为主要任务。

作为预备步骤，讨论了零空间基矩阵的构造。基于零空间基矩阵，研究了不同的零空间投影矩阵在实现零空间刚度和阻尼方面的应用。通过选择适当的度量，将这种投影矩阵的动态一致性概念纳入所提出的投影矩阵中。最终的动态一致投影矩阵等价于操作空间公式化中的投影矩阵，但其优点在于实现时只需要求逆一个 $r \times r$ 的矩阵，而不需要求逆惯性矩阵。然而，使用这种投影矩阵来实现零空间刚度和阻尼的缺点在于它无法实现一致的稳定性分析。

作为实现零空间刚度和阻尼的另一种方法，讨论了任务空间扩展方法。这种方法允许通过选择额外的零空间坐标，将上一章的笛卡尔阻抗控制技术扩展到冗余情况。尽管在理论上基础扎实，但应用此方法的主要问题在于选择适当的坐标。此外，得到的笛卡尔动态和零空间动态通常彼此并不解耦。只有在所有外力（即笛卡尔力和零空间力）都可以测量的情况下，才能实现这些动态的解耦。

最后，将关节空间分解方法作为第三种方法进行了讨论。基于这种方法，提出了一个零空间刚度控制器，并给出了局部稳定性证明。所提出的控制器与使用动态一致零空间投影非常相似，但这里包含了一个额外的项，它通过消除科里奥利/离心矩阵中的交叉对角部分来解耦笛卡尔动态和零空间动态。使用这一项，以及由此产生的解耦，允许应用半正定李雅普诺夫函数稳定性理论的一些结果，从而促进了稳定性证明。

---

5 奇异摄动方法

在前面的章节中，分析了忽略关节弹性的刚体机器人模型的阻抗控制问题。现在将考虑第2.2.3节中的柔性关节机器人模型。

本章讨论奇异摄动方法。这是一种为具有所谓双时间尺度特性的系统设计的近似方法。这意味着系统可以虚拟地分成两个耦合子系统，它们分别描述更快和更慢的部分。然后，奇异摄动理论允许根据分离子系统的性质得出稳定性结论，类似于线性情况下级联控制系统分析的过程。它也提供了忽略未建模的“高频”动态的理论依据。

在柔性关节机器人的情况下，模型的较快部分由内部扭矩控制回路形成，而较慢部分由刚体机器人动力学形成。奇异摄动方法允许将前面章节中讨论的所有刚体机器人控制方法扩展到柔性关节情况。

在第5.1节中，将简要回顾一般的奇异摄动理论。其应用于柔性关节模型在第5.2节中描述。在此基础上，第5.3节和第5.4节分别描述了常用的复合控制器设计方法和来自OASH02的改进控制器公式化方法，后者更适合柔性关节机器人模型。

5.1 奇异摄动理论

这里只给出奇异摄动理论的基本思想。更全面的处理可以在Kha02和KKO86中找到。

奇异摄动理论是为了分析依赖于小标量参数 $\epsilon \in \mathbb{R}$ 的模型而设计的，这些模型可以写成以下形式：

$$\begin{array}{c} \dot{z}_1 = f(z_1, z_2, \epsilon, t), \\ \dot{z}_2 = g(z_1, z_2, \epsilon, t), \end{array} \quad (5.1)$$

其中 $z_{1}\in \mathbb{R}^{n_{1}}$ 和 $z_{2}\in \mathbb{R}^{n_{2}}$ 是系统的状态变量。对于 $\epsilon$ 的小值，系统具有上述双时间尺度特性，两个方程分别描述系统的较慢部分（5.1）和较快部分（5.2）。

如果 $\epsilon$ 设为零（即假设模型的快速部分处于稳态），（5.2）变成一个代数方程，因此系统的阶数发生变化。下面假设可以唯一地求解这个代数方程，得到状态变量 $z_{2}$ 。对于 $\epsilon = 0$ 的系统解将表示为 $\bar{z}_{1}$ 和 $\bar{z}_{2}$ 。 $\bar{z}_{1}$ 和 $\bar{z}_{2}$ 之间的关系由函数 $\bar{z}_{2} = h(\bar{z}_{1},t)$ 给出，它是方程的解：

$$\mathbf{0} = \mathbf{g}(\bar{z}_{1}, \bar{z}_{2}, \mathbf{0}, t) \quad \Rightarrow \quad \bar{z}_{2} = \mathbf{h}(\bar{z}_{1}, t). \quad (5.3)$$

通过将变量 $\epsilon$ 设为零，并将（5.1）与（5.3）结合，得到所谓的准稳态模型：

$$\dot{\bar{z}}_{1} = f(\bar{z}_{1}, h(\bar{z}_{1}, t), 0, t). \quad (5.4)$$

模型（5.4）基本上描述了系统在慢时间尺度上的行为。

为了进一步分析，引入系统的快速部分的新坐标。新坐标 $y\in \mathbb{R}^{n_{2}}$ 表示 $z_{2}$ 与其稳态值 $h(z_{1},t)$ 的偏差，即：

$$y = z_{2} - h(z_{1}, t). \quad (5.5)$$

然后，完整模型可以用新状态变量 $z_{1}$ 和 $y$ 重写如下：

$$\begin{array}{l} \dot{z}_{1} = f(z_{1}, y + h(z_{1}, t), \epsilon, t), \\ \epsilon \dot{y} = g(z_{1}, y + h(z_{1}, t), \epsilon, t) - \epsilon \frac{\partial h(z_{1}, t)}{\partial t} \\ - \epsilon \frac{\partial h(z_{1}, t)}{\partial z_{1}} f(z_{1}, y + h(z_{1}, t), \epsilon, t). \end{array} \quad (5.6)$$

显然，通过将 $y$ 设为零，可以从（5.6）获得准稳态模型。为了也获得系统在快时间尺度上的模型，通过引入新时间变量 $\nu = (t - t_{0}) / \epsilon$ 来进行时间基变换，其中 $t_{0}$ 是一个（任意的）初始时间点。参数 $\epsilon$ 描述了时间基 $t$ 和 $\nu$ 之间的缩放。利用：

$$\mathrm{d}t = \epsilon \mathrm{d}\nu, \quad (5.8)$$

方程（5.7）可以在新时间基中写为：

$$\begin{array}{l} \frac{\mathrm{d}y}{\mathrm{d}\nu} = g(z_{1}, y + h(z_{1}, t), \epsilon, t) - \epsilon \frac{\partial h(z_{1}, t)}{\partial t} \\ - \epsilon \frac{\partial h(z_{1}, t)}{\partial z_{1}} f(z_{1}, y + h(z_{1}, t), \epsilon, t). \end{array} \quad (5.9)$$

变量 $t$ 和 $\mathbf{z}_{1}$ 是慢变的，因为在 $\nu$ 时间尺度上，它们由 $t = t_{0} + \epsilon \nu$ 和 $\mathbf{z}_{1} = \mathbf{z}_{1}(t_{0} + \epsilon \nu)$ 给出。将 $\epsilon = 0$ 会将这些变量冻结在 $t = t_{0}$ 和 $\mathbf{z}_{1} = \mathbf{z}_{1}(t_{0})$ ，并将（5.9）简化为：

$$\frac{\mathrm{d}\mathbf{y}}{\mathrm{d}\nu} = \mathbf{g}(\mathbf{z}_{1}(t_{0}), \mathbf{y} + \mathbf{h}(\mathbf{z}_{1}(t_{0}), t_{0}), 0, t_{0}). \quad (5.10)$$

为了分析当慢变变量 $(t, \mathbf{z}_{1})$ 偏离其初始值 $(t_{0}, \mathbf{z}_{1}(t_{0}))$ 时的情况，假设（5.4）的解 $\bar{\mathbf{z}}_{1}(t)$ 定义在 $t\in [0,t_{1}]$ 上，并且 $\bar{\mathbf{z}}_{1}(t)\in D_{1}\subset \mathbb{R}^{n_{1}}$ ，其中 $D_{1}$ 是某个域。然后将（5.10）写成以下形式：

$$\frac{\mathrm{d}\mathbf{y}}{\mathrm{d}\nu} = \mathbf{g}(\mathbf{z}_{1}, \mathbf{y} + \mathbf{h}(\mathbf{z}_{1}, t), 0, t), \quad (5.11)$$

这就是所谓的边界层模型。注意，在（5.11）中， $t$ 和 $\mathbf{z}_{1}$ 被视为固定参数。下面报告一个由于Tychonov的重要定理。这个定理基于方程（5.4）和（5.11），对于控制器设计的论证很有用。该定理要求边界层模型（5.11）的平衡点在冻结参数中一致指数稳定，如下定义。

定义 5.1. 边界层系统（5.11）的平衡点 $\mathbf{y} = \mathbf{0}$ 在 $(t,\mathbf{z}_{1})\in [0,t_{1}]\times D_{1}$ 中一致指数稳定，如果存在正常数 $k,\gamma$ 和 $\rho_{0}$ ，使得（5.11）的解满足：

$$\| \mathbf{y}(\nu)\| \leq k \| \mathbf{y}(0)\| \exp(-\gamma \nu) \quad \forall \| \mathbf{y}(0)\| < \rho_{0}, \forall (t,\mathbf{z}_{1})\in [0,t_{1}]\times D_{1}, \forall \nu \geq 0.$$

定理 5.2. （Tychonov定理Kha02）

给定系统（5.1）和（5.2），初始状态为 $\mathbf{z}_{1}(t_{0},\epsilon) = \mathbf{z}_{1,0}(\epsilon)$ 和 $\mathbf{z}_{2}(t_{0},\epsilon) = \mathbf{z}_{2,0}(\epsilon)$ 。设 $\mathbf{z}_{2} = \mathbf{h}(\mathbf{z}_{1},t)$ 是 $\mathbf{0} = \mathbf{g}(\mathbf{z}_{1},\mathbf{z}_{2},0,t)$ 的孤立根。假设对于所有：

$$(t,\mathbf{z}_{1},\mathbf{z}_{2} - \mathbf{h}(\mathbf{z}_{1},t),\epsilon) \in [0,t_{1}[\times D_{1}\times D_{y}\times [0,\epsilon_{0}]$$

在某些域 $D_{1}\subset \mathbb{R}^{n_{1}}$ 和 $D_{y}\subset \mathbb{R}^{n_{2}}$ 中，其中 $D_{1}$ 是凸的， $D_{y}$ 包含原点，以下条件成立：

函数 $\mathbf{f}$ 、 $\mathbf{g}$ 、它们关于 $(\mathbf{z}_{1},\mathbf{z}_{2},\epsilon)$ 的一阶偏导数，以及 $\mathbf{g}$ 关于 $t$ 的一阶偏导数是连续的。函数 $\mathbf{h}(\mathbf{z}_{1},t)$ 和雅可比矩阵 $\partial \mathbf{g}(\mathbf{z}_{1},\mathbf{z}_{2},0,t) / \partial \mathbf{z}_{2}$ 具有关于其参数连续的一阶偏导数。初始数据 $\mathbf{z}_{1,0}(\epsilon)$ 和 $\mathbf{z}_{2,0}(\epsilon)$ 是 $\epsilon$ 的光滑函数。简化问题（5.4）在 $t\in [t_{0},t_{1}]$ 上有唯一解 $\bar{\mathbf{z}}_{1}(t)\in S$ ，其中 $S$ 是 $D_{1}$ 的紧子集。边界层模型（5.11）的原点在 $(t,\mathbf{z}_{1})$ 中一致指数稳定。设 $R_{y}\subset D_{y}$ 是（5.10）的吸引域， $\Omega_{y}$ 是 $R_{y}$ 的紧子集。

则存在正常数 $\epsilon^{*}$ ，使得对于所有形式为 $\mathbf{z}_{2,0}(0) - \mathbf{h}(\mathbf{z}_{1,0}(0),t_{0}) \in \Omega_{y}$ 的初始条件和 $0 < \epsilon < \epsilon^{*}$ ，奇异摄动问题（5.1）-（5.2）在 $[t_{0},t_{1}]$ 上有唯一解 $\mathbf{z}_{1}(t,\epsilon)$ 、 $\mathbf{z}_{2}(t,\epsilon)$ ，并且：

$$\mathbf{z}_{1}(t,\epsilon) - \bar{\mathbf{z}}_{1}(t) = O(\epsilon)$$ $$\mathbf{z}_{2}(t,\epsilon) - \mathbf{h}(\bar{\mathbf{z}}_{1}(t),t) - \hat{\mathbf{y}}(t / \epsilon) = O(\epsilon)$$

对 $t\in [t_{0},t_{1}]$ 一致成立，其中 $\hat{\mathbf{y}}(\nu)$ 是边界层模型（5.11）的解。此外，给定任何 $t_{b} > t_{0}$ ，存在 $\epsilon^{**} \leq \epsilon^{*}$ ，使得：

$$\mathbf{z}_{2}(t,\epsilon) - \mathbf{h}(\bar{\mathbf{z}}_{1}(t),t) = O(\epsilon)$$

对 $t\in [t_{b},t_{1}]$ 一致成立，只要 $\epsilon < \epsilon^{**}$ 。

关于上述定理的证明，见Kha02。

注意，Tychonov定理只给出了有限时间区间上的结论。对于无限时间区间，该定理有扩展，但它们要求准稳态模型是指数稳定的。在阻抗控制问题（第3.2节）的情况下，即使在刚体情况下，也无法证明期望动态的指数稳定性。因此，下一节控制器设计的论证将仅基于Tychonov定理。注意，尽管这个定理从概念上非常重要，因为它允许为许多物理论证的建模简化技术提供理论基础，但它对于稳定性结论的价值是有限的。

在下一节中，将展示如何将奇异摄动理论应用于柔性关节机器人模型。

5.2 柔性关节机器人的奇异摄动模型

上一节概述的奇异摄动理论现在将应用于第2.2.3节的柔性关节机器人模型。使用第2.2.3节的符号，无阻尼柔性关节机器人的简化模型由下式给出：

$$\begin{array}{r} M(\mathbf{q})\ddot{\mathbf{q}} + C(\mathbf{q},\dot{\mathbf{q}})\dot{\mathbf{q}} + g(\mathbf{q}) = K(\boldsymbol{\theta} - \mathbf{q}) + \boldsymbol{\tau}_{ext}, \end{array} \quad (5.12)$$ $$\begin{array}{r} \mathbf{B}\ddot{\boldsymbol{\theta}} + \mathbf{K}(\boldsymbol{\theta} - \mathbf{q}) = \boldsymbol{\tau}_{m}. \end{array} \quad (5.13)$$

为了将这个系统转换为标准的奇异摄动形式，可以执行以下坐标变换。选择关节角度 $q$ 和关节扭矩 $\tau = K(\theta - q)$ 及其对时间的一阶导数作为坐标 $z_{1}$ 和 $z_{2}$ ：

$$z_{1} = \begin{pmatrix} q \\ \dot{q} \end{pmatrix}, \qquad z_{2} = \begin{pmatrix} \tau \\ \dot{\tau} \end{pmatrix}. \quad (5.14)$$

因此，可以将 $K(\theta - q) = \tau$ 和 $\ddot{\theta} = K^{-1}\ddot{\tau} + \ddot{q}$ 代入方程（5.12）和（5.13）中，得到：

$$\begin{array}{r} M(q)\ddot{q} + C(q,\dot{q})\dot{q} + g(q) = \tau + \tau_{ext}, \\ B K^{-1} \ddot{\tau} + \tau = \tau_{m} - B \ddot{q}. \end{array} \quad (5.16)$$

（5.16）中的连杆角度 $q$ 的二阶导数可以通过使用（5.15）来消除，因为惯性矩阵 $M(q)$ 是正定的，因此总是可逆的。为了将系统置于奇异摄动形式，必须引入参数 $\epsilon$ 。关节刚度值 $K_{i}$ 通常非常高。这通过形式上将矩阵 $K$ 替换为 $K = \frac{K_{\epsilon}}{\epsilon^{2}}$ 来纳入模型，其中 $K_{\epsilon}$ 是正定对角矩阵。 $\epsilon$ 的小值表示高的关节刚度值。

还要注意，上一节的一般奇异摄动方法是针对没有控制输入的自由系统描述的。在柔性关节机器人模型的情况下，电机扭矩 $\tau_{m}$ （即控制输入）被视为状态变量 $z_{1}$ 和 $z_{2}$ 的（至今未知）函数。此外，外部扭矩 $\tau_{ext}$ 被视为扰动输入。

根据上述考虑，可以将模型重写为（标准）奇异摄动形式：

$$\begin{array}{r} M(q)\ddot{q} + C(q,\dot{q})\dot{q} + g(q) = \tau + \tau_{ext}, \\ \epsilon^{2} \ddot{\tau} + K_{\epsilon}(B^{-1} + M(q)^{-1})\tau = K_{\epsilon}B^{-1}\tau_{m} + K_{\epsilon}M(q)^{-1}(C(q,\dot{q})\dot{q} + g(q) - \tau_{ext}). \end{array} \quad (5.17)$$

通过将这个方程重写为一阶微分方程组，可以很容易地看出，由于引入了参数 $\epsilon$ ，该模型确实现在与方程（5.1）和（5.2）具有相同的形式。

按照上一节的步骤，通过将 $\epsilon$ 设为零（如（5.3）中所做的那样），计算扭矩的准稳态 $\bar{\tau} = h(\bar{q}, \dot{\bar{q}}, t)$ ：

$$\begin{array}{rl} & h(\bar{q}, \dot{\bar{q}}, t) = (I + B M(\bar{q})^{-1})^{-1} \left( \bar{\tau}_{m} + B M(\bar{q})^{-1} (C(\bar{q}, \dot{\bar{q}}) \dot{\bar{q}} + g(\bar{q}) - \bar{\tau}_{ext}) \right) \\ & = (I + B M(\bar{q})^{-1})^{-1} \bar{\tau}_{m} + (I + M(\bar{q})B^{-1})^{-1} (C(\bar{q}, \dot{\bar{q}}) \dot{\bar{q}} + g(\bar{q}) - \bar{\tau}_{ext}). \end{array} \quad (5.19)$$

变量 $\bar{\tau}_{m}$ 和 $\bar{\tau}_{ext}$ 对应于当参数 $\epsilon$ 设为零时的输入变量 $\tau_{m}$ 和 $\tau_{ext}$ 。

5.2.1 准稳态模型

准稳态模型通过将 $\bar{\tau}$ 代入方程（5.17）得到。所得方程可以通过左乘因子 $(I + B M(\bar{q})^{-1})$ 来简化。因此，得到系统方程：

$$(M(\bar{q}) + B)\ddot{q} + C(\bar{q}, \dot{q}) \dot{q} + g(\bar{q}) = \bar{\tau}_{m} + \bar{\tau}_{ext}. \quad (5.20)$$

注意，该模型具有惯性矩阵为 $M(\bar{q}) + B$ 的刚体模型形式。

5.2.2 边界层系统

为了推导边界层模型，引入新坐标 $y \in \mathbb{R}^{n}$ （见方程（5.5））：

$$y = \tau - h(q, \dot{q}, t) \quad (5.21)$$

它们对应于实际扭矩与其准稳态值的偏差。与一般情况下一样，将这些新坐标代入方程（5.18），引入快时间尺度 $\nu = (t - t_{0}) / \epsilon$ ，并将参数 $\epsilon$ 设为零。这导致：

$$\frac{\mathrm{d}^{2}y}{\mathrm{d}\nu^{2}} + K_{\epsilon}(B^{-1} + M(q)^{-1})(y + h(q, \dot{q}, t)) = K_{\epsilon}B^{-1}\tau_{m} + K_{\epsilon}M(q)^{-1}(C(q, \dot{q}) \dot{q} + g(q) - \tau_{ext}).$$

这里，来自系统慢速部分的状态变量和扰动输入 $\tau_{ext}$ 相对于时间变量 $\nu$ 被视为常数。使用恒等式：

$$(B^{-1} + M(q)^{-1})h(q, \dot{q}, t) = B^{-1}\tau_{m} + M(q)^{-1}(C(q, \dot{q}) \dot{q} + g(q) - \tau_{ext}),$$

它直接来自 $h(q, \dot{q}, t)$ 的定义（5.19），最终得到：

$$\frac{\mathrm{d}^{2}y}{\mathrm{d}\nu^{2}} + K_{\epsilon}(B^{-1} + M(q)^{-1})y = K_{\epsilon}B^{-1}(\tau_{m} - \bar{\tau}_{m}). \quad (5.22)$$

注意，由于变量 $q$ 在此方程中被视为常数，柔性关节机器人模型的边界层系统是线性和时不变的。

5.3 控制器设计

下面描述复合反馈设计方法的设计思路。这种设计方法已被许多作者用于基于奇异摄动方法的柔性关节机器人控制器设计。最后，提出了一种替代的控制器结构，与常见的复合设计相比具有一些优势。

复合控制器设计方法（图5.1）的思想是将控制输入 $\tau_{m}$ 分解为一个慢速部分和一个快速部分 $\tau_{m,slow}$ 和 $\tau_{m,fast}$ ，即 $\tau_{m} = \tau_{m,slow} + \tau_{m,fast}$ 。这两个控制输入应该被设计成使得慢速部分只影响准稳态模型，而快速部分只影响边界层系统。然后可以使用这两个控制输入分别控制两个子系统。形式上，这可以通过选择 $\tau_{m,slow} = \tau_{m}|_{\epsilon \to 0} = \bar{\tau}$ 和 $\tau_{m,fast} = \tau_{m} - \bar{\tau}$ 来实现。

将控制输入分成两部分对设计有两个重要影响。一方面，控制输入 $\tau_{m,slow}$ 必须被选择为使系统处于标准形式，即准稳态 $\bar{\tau}$ 必须有唯一解。例如，如果 $\tau_{m,slow}$ 仅为准稳态模型状态 $(\mathbf{q},\dot{\mathbf{q}})$ 的函数，并且不包含关节扭矩 $\boldsymbol{\tau}$ 的反馈，则可以实现这一点。另一方面，控制输入 $\tau_{m,fast}$ 应该被设计为满足条件 $\tau_{m,fast}|_{\epsilon \to 0} = \mathbf{0}$ ，即它不应该影响准稳态模型。

虽然描述扭矩动力学的边界层系统是线性和时不变的，但准稳态模型具有等效刚体模型的形式。因此，可以将刚体机器人文献中的经典控制器方法应用于设计 $\tau_{m,slow}$ 。

根据Tychonov定理，控制器设计的目标是一方面获得指数稳定的边界层系统。另一方面，期望准稳态系统的跟踪误差收敛。注意，Tychonov定理只确保跟踪误差在有限时间区间内不会偏离其准稳态超过 $O(\epsilon)$ 量级。这个结论也只在 $\epsilon$ 足够小的情况下有效，这意味着边界层系统必须比准稳态系统快得多。

在大多数先前关于柔性关节机器人奇异摄动方法的工作中，电机扭矩的快速部分被选择为形式为 $\tau_{m,fast} = - \epsilon D_{\tau}\dot{\mathbf{y}}$ 的项，其中 $\mathbf{D}_{\tau}$ 是正定阻尼矩阵。此外，慢速部分 $\tau_{m,slow}$ 根据刚体机器人模型（5.20）的控制律 $\tau_{rigid}(\mathbf{q},\dot{\mathbf{q}})$ 设计。注意，项 $- \epsilon D_{\tau}\dot{\mathbf{y}}$ 可以修改为 $- \epsilon D_{\tau}\dot{\boldsymbol{\tau}}$ ，因为时间 $t$ 和准稳态系统的状态变量 $\mathbf{q}$ 和 $\dot{\mathbf{q}}$ 在边界层系统中被视为固定参数。在这两种情况下，都保证了条件 $\tau_{m,fast}|_{\epsilon \to 0} = \mathbf{0}$ 成立。柔性关节机器人模型的复合反馈控制器由下式给出：

$$\boldsymbol{\tau}_{m} = \boldsymbol{\tau}_{rigid}(\mathbf{q}, \dot{\mathbf{q}}) - \epsilon \mathbf{D}_{\tau} \dot{\boldsymbol{\tau}}. \quad (5.23)$$

这种方法的问题在于，控制律（5.23）并没有真正改善边界层系统的动态，而只是为其增加了阻尼。另一种选择是也在反馈中使用扭矩误差 $K_{\tau}\mathbf{y} = K_{\tau}(\boldsymbol{\tau} - \mathbf{h}(\mathbf{q},\dot{\mathbf{q}};t))$ ，其中 $K_{\tau}$ 是正定控制器增益矩阵，以增强边界层动态的性能。然而，为了实现这样的反馈，必须显式计算稳态 $h(q, \dot{q}, t)$ 。这在许多情况下是不期望的，因为它需要测量外力 $\tau_{ext}$ 并求逆惯性矩阵 $M(q)$ 。

5.4 改进的控制器设计

下面，根据OASH02提出一种控制器的改进，以避免显式计算 $h(q, \dot{q}, t)$ 。

将控制输入分解为：

$$\tau_{m} = \tau_{ss}(q, \dot{q}) - K_{\tau} \tau - \epsilon D_{\tau} \dot{\tau}, \quad (5.24)$$

其中 $K_{\tau}$ 和 $D_{\tau}$ 是正定控制器增益矩阵， $\tau_{ss}(q, \dot{q})$ 是用于准稳态系统的中间控制输入，稍后将定义。形式上，在（5.24）中，项 $K_{\tau} \tau$ 被用来代替项 $K_{\tau} y$ 。该项显然同时影响控制输入的快速部分 $\tau_{m,fast}$ 和慢速部分 $\tau_{m,slow}$ 。下面给出该控制律的闭环准稳态系统和边界层系统，并展示如何为第3.2节的笛卡尔阻抗控制问题选择项 $\tau_{ss}(q, \dot{q})$ 。

5.4.1 改进的准稳态系统

控制输入的慢速部分 $\tau_{m,slow}$ 通过在表达式（5.24）中将 $\epsilon$ 设为零得到。这导致：

$$\begin{array}{rl} & \tau_{m,slow} = \tau_{ss}(\bar{q}, \dot{\bar{q}}) - K_{\tau} h(\bar{q}, \dot{\bar{q}}, t) \\ & = \tau_{ss}(\bar{q}, \dot{\bar{q}}) - K_{\tau} \bar{\tau}. \end{array} \quad (5.25)$$

在（5.25）中可以看出， $\tau_{m,slow}$ 现在也包含了准稳态扭矩 $\bar{\tau}$ ，并且 $\bar{\tau}$ 的表达式（来自（5.19））变成了一个隐式方程。然而，可以很容易地解出稳态扭矩 $\bar{\tau}$ ，并得到：

$$\begin{array}{rl} & \bar{\tau} = (I + K_{\tau} + B M(\bar{q})^{-1})^{-1} \\ & (\bar{\tau}_{ss}(\bar{q}, \dot{\bar{q}}) + B M(\bar{q})^{-1}(C(\bar{q}, \dot{\bar{q}}) \dot{\bar{q}} + g(\bar{q}) - \bar{\tau}_{ext})) \end{array} \quad (5.26)$$

使用这个关系以及（5.20），最终得到以下稳态系统：

$$(M(\bar{q}) + (I + K_{\tau})^{-1} B)\ddot{\bar{q}} + C(\bar{q}, \dot{\bar{q}}) \dot{\bar{q}} + g(\bar{q}) = (I + K_{\tau})^{-1} \tau_{ss}(\bar{q}, \dot{\bar{q}}). \quad (5.26)$$

现在可以选择函数 $\tau_{ss}(\bar{q}, \dot{\bar{q}})$ 为 $\tau_{ss}(\bar{q}, \dot{\bar{q}}) = (I + K_{\tau})\tau_{d}$ ，其中 $\tau_{d}$ 是一个新的控制输入，可以根据刚体机器人的控制律（如第3.2节的控制器）来设计。通过这个替换，完整的控制律（5.24）可以写成以下形式（另见图5.2）：

$$\tau_{m} = \tau_{d} - K_{\tau}(\tau - \tau_{d}) - \epsilon D_{\tau} \dot{\tau}, \quad (5.27)$$

并且闭环系统由下式给出：

$$(M(\bar{q}) + (I + K_{\tau})^{-1} B)\ddot{\bar{q}} + C(\bar{q}, \dot{\bar{q}}) \dot{\bar{q}} + g(\bar{q}) = \tau_{d}. \quad (5.28)$$

与（5.20）相比，新准稳态系统的惯性矩阵现在也依赖于控制器增益矩阵 $K_{\tau}$ ，因此，通过 $K_{\tau}$ ，表观电机惯量被缩放。扭矩反馈的这种物理解释将在第7章中用于设计基于无源性的控制器。注意， $\tau_{d}$ 并不对应于边界层系统的稳态值 $\bar{\tau}$ ，正如人们从（5.27）可能期望的那样。

5.4.2 改进的边界层系统

新的边界层系统直接来自（5.22）和（5.24）。考虑到控制输入的快速部分具有以下形式：

$$\tau_{m,fast} = -K_{\tau}(\tau - \bar{\tau}) - \epsilon D_{\tau} \dot{\tau} = -K_{\tau} y - \epsilon D_{\tau} \dot{\tau}, \quad (5.29)$$

得到以下边界层系统：

$$\frac{\mathrm{d}^{2}\mathbf{y}}{\mathrm{d}\nu^{2}} + K_{\epsilon}B^{-1}D_{\tau} \frac{\mathrm{d}\mathbf{y}}{\mathrm{d}\nu} + K_{\epsilon}B^{-1}(I + B M(\mathbf{q})^{-1} + K_{\tau})\mathbf{y} = \mathbf{0}. \quad (5.30)$$

现在，正定矩阵 $K_{\tau}$ 和 $D_{\tau}$ 可以用于整形线性和时不变边界层系统的动态，以获得符合Tychonov定理的指数稳定性。

5.4.3 笛卡尔阻抗控制器的设计

现在将基于奇异摄动方法处理第3.2节的柔性关节情况下的阻抗控制问题。

从本章的分析可以得出，刚体情况下的控制器（3.18）现在可以通过中间控制输入 $\tau_{d}$ 连同（5.27）的内环控制器应用于准稳态系统。在设计中，现在重要的是基于（5.28）中修改后的准稳态系统来计算（3.18）中的所有分量，如笛卡尔惯性矩阵 $\Lambda(\mathbf{x})$ 和相应的科里奥利/离心矩阵 $\mu(\mathbf{x},\dot{\mathbf{x}})$ 。然后，第3章的所有稳定性考虑都可以直接应用于准稳态系统。在非冗余和非奇异情况下，完整的控制器最终由下式给出：

$$\begin{array}{l} \boldsymbol{\tau}_{m} = \boldsymbol{\tau}_{d} - \mathbf{K}_{\tau}(\boldsymbol{\tau} - \boldsymbol{\tau}_{d}) - \epsilon \mathbf{D}_{\tau} \dot{\boldsymbol{\tau}} \\ \boldsymbol{\tau}_{d} = g(\mathbf{q}) + \mathbf{J}(\mathbf{q})^{T} (\Lambda(\mathbf{x}) \ddot{\mathbf{x}}_{d} + \mu(\mathbf{x}, \dot{\mathbf{x}}) \dot{\mathbf{x}}_{d} - \mathbf{K}_{d} \tilde{\mathbf{x}} - \mathbf{D}_{d} \dot{\tilde{\mathbf{x}}}), \end{array} \quad (5.31)$$

其中笛卡尔误差 $\tilde{\mathbf{x}}$ 、期望位置 $\mathbf{x}_{d}$ 和雅可比矩阵 $\mathbf{J}(\mathbf{q})$ 与第3章相同。矩阵 $\mathbf{K}_{d}$ 和 $\mathbf{D}_{d}$ 再次是期望刚度和阻尼的对称正定矩阵。

还要注意，第3章和第4章中为刚体情况设计的所有控制器都可以通过控制输入 $\boldsymbol{\tau}_{d}$ 应用于柔性关节机器人的奇异摄动模型。虽然奇异摄动方法从实际角度来看非常灵活，但其主要局限性显然在于所有论证仅基于Tychonov定理，该定理没有给出严格的稳定性陈述。

此外，似乎奇异摄动理论中更全面的定理都无法应用于所考虑的阻抗控制问题。原因是它们不仅要求边界层系统指数稳定，还要求准稳态系统指数稳定。对于第3.2节所考虑的期望动态阻抗行为（在自由运动情况下），无法证明指数稳定性。

5.5 总结

本章讨论了奇异摄动方法及其在简化柔性关节机器人模型中的应用。该方法允许将模型分成两个独立的子系统，分别用于动态的快速和慢速部分。柔性关节机器人模型的快速部分由扭矩动力学组成，而慢速部分具有刚体机器人模型的结构。在讨论了常用的复合控制器之后，提出了一种改进的控制律，该控制律允许改善扭矩动态，但避免了计算关节扭矩准稳态 $\mathbf{h}(\mathbf{q},\dot{\mathbf{q}},t)$ 。使用所提出的控制器，系统动态的慢速部分也具有（修改后的）刚体系统的结构。因此，该控制器可以看作是一个内环扭矩控制器和一个为刚体机器人模型设计的外环阻抗控制器的级联。奇异摄动方法的主要缺点是稳定性方面的理论论证较弱，这是由于Tychonov定理的局限性。因此，下一章将讨论一些更复杂的方法，这些方法将带来更强的稳定性陈述，但也会导致更复杂的控制器结构。

---

6 基于级联结构的控制器设计

在前一章中，讨论了一种用于柔性关节机器人控制的相当通用的方法，它完全不局限于笛卡尔阻抗控制的特殊问题。奇异摄动方法的主要缺点是只能做出非常弱的稳定性陈述。原因在于奇异摄动分析中忽略了慢速和快速动态之间的一些耦合项。在本章中，将研究控制器必须如何修改才能允许更强的稳定性陈述。

将分析两种不同的方法。第一种方法在第6.1节中讨论，基于级联系统的控制理论。虽然第2.2.3节的柔性关节机器人模型不是级联形式，但可以通过内反馈回路将其转换为这种形式。第二种方法基于反步法，将在第6.2节中讨论。

6.1 基于解耦的方法

首先，在第6.1.1节中给出一些关于级联系统控制的相关结果的概述。然后，在第6.1.2节中介绍其在柔性关节机器人模型上的应用。

6.1.1 级联系统的控制理论

考虑以下形式的自治三角系统（见图6.1）：

$$\begin{array}{r} \dot{z}_1 = f_1(z_1, z_2), \\ \dot{z}_2 = f_2(z_2), \end{array} \quad (6.1)$$

其中 $z_{1}\in \mathbb{R}^{n_{1}}$ 和 $z_{2}\in \mathbb{R}^{n_{2}}$ 是状态变量。假设函数 $f_{1}(z_{1},z_{2})$ 和 $f_{2}(z_{2})$ 是局部Lipschitz的，并且所有解对于 $t\geq 0$ 都存在。此外，假设原点是（6.1）-（6.2）的平衡点，即 $f_{1}(0,0) = 0$ 和 $f_{2}(0) = 0$ 。下面分析当解耦系统（6.2）和：

$$\dot{z}_{1} = f_{1}(z_{1}, 0) \quad (6.3)$$

是全局渐近稳定时的情况。那么问题是在什么条件下耦合系统（6.1）-（6.2）也是渐近稳定的。局部地，这总是成立的（参见SS90中的参考文献）。为了确保这也全局成立，在SS90中证明了证明耦合系统的所有解保持有界就足够了。这在以下定理中以更一般的形式表述，该定理取自SS90。

定理 6.1 如果系统（6.3）是全局渐近稳定的，并且如果（6.2）是渐近稳定的，其吸引域为 $\mathcal{A}\subseteq \mathbb{R}^{n_2}$ ，并且如果（6.1）-（6.2）的每条解曲线（初始点在 $\mathbb{R}^{n_1}\times \mathcal{A}$ 中）对于 $t > 0$ 是有界的，那么系统（6.1）-（6.2）是渐近稳定的，其吸引域为 $\mathbb{R}^{n_1}\times \mathcal{A}$ 。

该定理的证明可以在SS90中找到。注意，如果（6.2）的吸引域是整个状态空间 $\mathbb{R}^{n_2}$ ，定理6.1也处理全局渐近稳定的情况。

定理6.1考虑的是自治情况。而第3.2节的阻抗控制问题即使在刚体机器人的情况下也会导致时变控制问题。因此，也将考虑时变情况。时变系统稳定性分析的相关定义和引理可以在附录A.1中找到。

Lor01将定理6.1扩展到了一类特殊的时变系统。这个来自Lor01的贡献以以下定理的形式给出。

定理 6.2 考虑系统：

$$\begin{array}{r} \dot{z}_1 = f_1(t, z_1) + h(t, z) z_2 \\ \dot{z}_2 = f_2(t, z_2) \end{array} \quad (6.4)$$

状态为 $z = (z_{1}, z_{2}) \in \mathbb{R}^{n_{1}} \times \mathbb{R}^{n_{2}}$ 。函数 $f_{1}(t,z_{1})$ 、 $f_{2}(t,z_{2})$ 和 $h(t,z)$ 在它们的参数上是连续的，在 $z$ 上是局部Lipschitz的，在 $t$ 上是一致连续的，并且 $\mathbf{f}_1(t,z_1)$ 在两种参数上都是连续可微的。当且仅当以下条件成立时，该系统是一致全局渐近稳定的：

存在一个非递减函数 $H(\cdot)$ ，使得：

$$||h(z,t)|| \leq H(||z||). \quad (6.6)$$

系统：

$$\dot{z}_{1} = \mathbf{f}_{1}(t, z_{1})$$ $$\dot{z}_{2} = \mathbf{f}_{2}(t, z_{2})$$

是一致全局渐近稳定的。

（6.4）-（6.5）的解是一致全局有界的。

定理6.2给出了从三角系统（6.4）-（6.5）可以得出一致全局渐近稳定性的充分必要条件。注意，如果解耦系统甚至可以证明是指数稳定的，那么文献中也有其他结果可以应用于这样的三角系统（参见Vid93）。而第3.2节的阻抗控制问题导致了一个闭环系统，对于该系统只能证明（一致全局）渐近稳定性，而不是指数稳定性（见命题3.4）。下面将展示如何将定理6.2用于设计柔性关节机器人模型的笛卡尔阻抗控制器。

6.1.2 扭矩动力学的解耦

第2.2.3节推导的（无阻尼）柔性关节机器人简化模型由下式给出：

$$\begin{array}{r} M(\mathbf{q})\ddot{\mathbf{q}} + C(\mathbf{q},\dot{\mathbf{q}})\dot{\mathbf{q}} + g(\mathbf{q}) = K(\boldsymbol{\theta} - \mathbf{q}) + \boldsymbol{\tau}_{ext}, \\ B\ddot{\boldsymbol{\theta}} + K(\boldsymbol{\theta} - \mathbf{q}) = \boldsymbol{\tau}_{m}. \end{array} \quad (6.7)$$

为了将该系统与（6.1）-（6.2）或（6.4）-（6.5）的三角形式相关联，可以执行以下坐标变换。选择关节角度 $\mathbf{q}$ 和关节扭矩 $\boldsymbol{\tau} = K(\boldsymbol{\theta} - \mathbf{q})$ 及其对时间的一阶导数作为坐标 $z_{1}$ 和 $z_{2}$ ：

$$z_{1} = \begin{pmatrix} q \\ \dot{q} \end{pmatrix}, \qquad z_{2} = \begin{pmatrix} \tau \\ \dot{\tau} \end{pmatrix}. \quad (6.9)$$

注意，这些是与上一章奇异摄动方法中使用的相同的状态变量。可以将 $K(\boldsymbol{\theta} - \mathbf{q}) = \boldsymbol{\tau}$ 和 $\ddot{\boldsymbol{\theta}} = K^{-1}\ddot{\boldsymbol{\tau}} + \ddot{\mathbf{q}}$ 代入方程（6.8）中，得到：

$$B K^{-1} \ddot{\boldsymbol{\tau}} + \boldsymbol{\tau} = \boldsymbol{\tau}_{m} - B \ddot{\mathbf{q}}. \quad (6.10)$$

连杆加速度可以通过使用（6.7）从（6.10）中消除，因为惯性矩阵 $M(q)$ 是正定的，因此总是可逆的（性质2.5）。因此，得到新的系统方程：

$$\begin{array}{c} M(q)\ddot{q} + C(q,\dot{q})\dot{q} + g(q) = \tau + \tau_{ext}, \\ B K^{-1} \ddot{\tau} + \tau = \tau_{m} - B M(q)^{-1} (\tau + \tau_{ext} - C(q,\dot{q})\dot{q} - g(q)). \end{array} \quad (6.11)$$

到目前为止，模型已类似于上一章进行了重新表述。显然，模型（6.11）-（6.12）不具有像（6.4）-（6.5）那样的三角结构。但可以通过以下反馈补偿将其转换为这种形式：

$$\tau_{m} = \mathbf{u} + B M(q)^{-1}(\boldsymbol{\tau} + \boldsymbol{\tau}_{ext} - C(q,\dot{q})\dot{q} - g(q)), \quad (6.13)$$

其中 $\mathbf{u}\in \mathbb{R}^{n}$ 是一个中间控制输入。具有解耦扭矩动力学的系统可以写成：

$$\begin{array}{c} M(q)\ddot{q} + C(q,\dot{q})\dot{q} + g(q) = \tau + \tau_{ext}, \\ B K^{-1} \ddot{\tau} + \tau = \mathbf{u}. \end{array} \quad (6.14)$$

为了将扭矩动力学方程（6.15）的状态转移到零，引入期望扭矩变量 $\tau_{d}$ 和扭矩误差变量 $e_{\tau}$ ：

$$e_{\tau} = \tau - \tau_{d}. \quad (6.16)$$

期望扭矩 $\tau_{d}$ 将在后面用于设计笛卡尔阻抗控制器。使用这个新状态变量，系统方程写成如下：

$$\begin{array}{c} M(q)\ddot{q} + C(q,\dot{q})\dot{q} + g(q) = e_{\tau} + \tau_{d} + \tau_{ext}, \\ B K^{-1}(\ddot{e}_{\tau} + \ddot{\tau}_{d}) + e_{\tau} + \tau_{d} = \mathbf{u}. \end{array} \quad (6.18)$$

中间输入 $\mathbf{u}$ 应该被设计为使扭矩误差动态全局渐近稳定。使用反馈律：

$$\mathbf{u} = \boldsymbol{\tau}_{d} + B K^{-1} \left( \ddot{\boldsymbol{\tau}}_{d} - \mathbf{D}_{\tau} \dot{\boldsymbol{\tau}}_{\tau} - \mathbf{K}_{\tau} \mathbf{e}_{\tau} \right), \quad (6.19)$$

其中 $K_{\tau}$ 和 $D_{\tau}$ 是正定控制器增益矩阵，扭矩误差动态由以下线性时不变系统给出：

$$\ddot{e}_{\tau} + D_{\tau} \dot{e}_{\tau} + (K_{\tau} + K B^{-1}) e_{\tau} = \mathbf{0}, \quad (6.20)$$

对于正定控制器增益矩阵 $D_{\tau}$ 和 $K_{\tau}$ ，该系统显然是指数稳定的。

6.1.3 阻抗控制器设计

闭环系统由方程（6.17）和（6.20）给出，它们呈三角形式，前提是扭矩 $\tau_{d}$ 仅是状态变量 $q$ 和 $\dot{q}$ 的函数。在本章开始时，分析了解耦系统（6.3）渐近稳定的情况。在柔性关节机器人模型的情况下，系统（6.3）对应于 $\mathbf{e}_{\tau} = \mathbf{0}$ 时的（6.17）。变量 $\boldsymbol{\tau}_{d}$ 现在将被用来使这个系统渐近稳定。

下面，通过在所考虑的工作空间限制为 $\bar{\mathcal{Q}}_{c}^{p}$ ，来考虑非冗余和非奇异情况下的第3.2节期望阻抗。本章使用与第3章相同的符号。为了方便参考，这里复述笛卡尔坐标 $\mathbf{x} = \mathbf{f}(\mathbf{q})$ 和虚拟平衡位置 $\mathbf{x}_{d}$ 的期望动态行为。在所考虑的非冗余和非奇异情况下，解析雅可比矩阵 $\mathbf{J}(\mathbf{q}) = \partial \mathbf{f}(\mathbf{q}) / \partial \mathbf{q}$ 被假定为可逆的。期望的笛卡尔广义力 $\mathbf{F}_{ext}$ （与 $\boldsymbol{\tau}_{ext}$ 通过 $\boldsymbol{\tau}_{ext} = \mathbf{J}(\mathbf{q})^{T}\mathbf{F}_{ext}$ 相关）与笛卡尔位置误差 $\tilde{\mathbf{x}} = \mathbf{x} - \mathbf{x}_{d}$ 之间的期望阻抗关系由下式给出（参见（3.16））：

$$\Lambda(\mathbf{x}) \ddot{\mathbf{x}} + (\mu(\mathbf{x}, \dot{\mathbf{x}}) + \mathbf{D}_{d}) \dot{\mathbf{x}} + \mathbf{K}_{d} \tilde{\mathbf{x}} = \mathbf{F}_{ext}. \quad (6.21)$$

矩阵 $\mathbf{K}_{d}$ 和 $\mathbf{D}_{d}$ 再次是期望刚度和阻尼的对称正定矩阵。笛卡尔惯性矩阵 $\Lambda(\mathbf{x})$ 和科里奥利/离心矩阵 $\mu(\mathbf{x}, \dot{\mathbf{x}})$ 在（3.10）和（3.11）中定义。假设期望轨迹 $\mathbf{x}_{d}(t)$ 是四阶连续可微的。通过执行与第3.1.2节相同的步骤，可以验证系统方程（6.17）和（6.20）可以写成笛卡尔坐标形式：

$$\begin{array}{rl} & \Lambda(\mathbf{x}) \ddot{\mathbf{x}} + \mu(\mathbf{x}, \dot{\mathbf{x}}) \dot{\mathbf{x}} + \mathbf{J}(\mathbf{q})^{-T} g(\mathbf{q}) = \mathbf{F}_{ext} + \mathbf{J}(\mathbf{q})^{-T} (\mathbf{e}_{\tau} + \boldsymbol{\tau}_{d}), \\ & \ddot{\mathbf{e}}_{\tau} + \mathbf{D}_{\tau} \dot{\mathbf{e}}_{\tau} + (\mathbf{K}_{\tau} + \mathbf{K} \mathbf{B}^{-1}) \mathbf{e}_{\tau} = \mathbf{0}. \end{array} \quad (6.23)$$

现在可以将（3.18）中为刚体模型设计的控制器用于期望扭矩 $\boldsymbol{\tau}_{d}$ 。因此，柔性关节机器人模型的完整控制器由方程（6.13）、（6.19）和（3.18）给出，即为：

$$\begin{array}{rl} & \boldsymbol{\tau}_{m} = \boldsymbol{\tau}_{d} + \mathbf{B} \mathbf{K}^{-1} (\ddot{\boldsymbol{\tau}}_{d} - \mathbf{D}_{\tau} (\dot{\boldsymbol{\tau}} - \dot{\boldsymbol{\tau}}_{d}) - \mathbf{K}_{\tau} (\boldsymbol{\tau} - \boldsymbol{\tau}_{d})) \\ & + \mathbf{B} \mathbf{M}(\mathbf{q})^{-1} (\boldsymbol{\tau} + \boldsymbol{\tau}_{ext} - \mathbf{C}(\mathbf{q}, \dot{\mathbf{q}}) \dot{\mathbf{q}} - \mathbf{g}(\mathbf{q})), \\ & \boldsymbol{\tau}_{d} = \mathbf{g}(\mathbf{q}) + \mathbf{J}(\mathbf{q})^{T} (\Lambda(\mathbf{x}) \ddot{\mathbf{x}}_{d} + \mu(\mathbf{x}, \dot{\mathbf{x}}) \dot{\mathbf{x}}_{d} - \mathbf{K}_{d} \tilde{\mathbf{x}} - \mathbf{D}_{d} \dot{\tilde{\mathbf{x}}}). \end{array}$$

将 $\boldsymbol{\tau}_{d}$ 代入系统方程（6.22）-（6.23）得到相应的闭环动态：

$$\begin{array}{rl} & \Lambda(\mathbf{x}) \ddot{\mathbf{x}} + (\mu(\mathbf{x}, \dot{\mathbf{x}}) + \mathbf{D}_{d}) \dot{\mathbf{x}} + \mathbf{K}_{d} \tilde{\mathbf{x}} = \mathbf{F}_{ext} + \mathbf{J}(\mathbf{q})^{-T} \mathbf{e}_{\tau}, \\ & \ddot{\mathbf{e}}_{\tau} + \mathbf{D}_{\tau} \dot{\mathbf{e}}_{\tau} + (\mathbf{K}_{\tau} + \mathbf{K} \mathbf{B}^{-1}) \mathbf{e}_{\tau} = \mathbf{0}. \end{array} \quad (6.24)$$

注意，当耦合项 $\mathbf{J}(\mathbf{q})^{-T} \mathbf{e}_{\tau}$ 在（6.24）中被忽略时，它恰好对应于期望动态（6.21），根据命题3.4，该动态是一致全局渐近稳定的。还要注意，闭环系统（6.24）-（6.25）是时变的，这并不奇怪，因为期望动态也是如此。

6.1.4 稳定性分析

对于（6.21）的期望动态，关于稳定性和无源性的两个重要性质在命题3.4和命题3.5中给出。下面将证明这些陈述也适用于柔性关节模型的闭环动态（6.24）-（6.25）。

命题 6.3. 设期望轨迹 $\mathbf{x}_{d}(t)$ 是四阶连续可微的。进一步假设笛卡尔坐标全局有效，即 $\bar{\mathcal{Q}}_{c}^{p} = \mathbb{R}^{n}$ 。那么，在自由运动情况下（ $\mathbf{F}_{ext} = 0$ ），具有对称正定矩阵 $\mathbf{K}_{d}$ 、 $\mathbf{D}_{d}$ 、 $\mathbf{K}_{\tau}$ 和 $\mathbf{D}_{\tau}$ 的系统（6.24）-（6.25）是一致全局渐近稳定的。

证明. 为了将（6.24）-（6.25）在 $\mathbf{F}_{ext} = 0$ 时重写为仅关于状态变量 $z = (\tilde{\mathbf{x}}, \dot{\tilde{\mathbf{x}}}, e_{\tau}, \dot{e}_{\tau})$ 的系统，进行以下替换很方便： $J(\tilde{\mathbf{x}}, t) := J(f^{-1}(\tilde{\mathbf{x}} + \mathbf{x}_{d}(t))) = J(\mathbf{q})$ 、 $\Lambda(\tilde{\mathbf{x}}, t) := \Lambda(\tilde{\mathbf{x}} + \mathbf{x}_{d}(t)) = \Lambda(\mathbf{x})$ 和 $\mu(\tilde{\mathbf{x}}, \dot{\tilde{\mathbf{x}}}, t) := \mu(\tilde{\mathbf{x}} + \mathbf{x}_{d}(t), \dot{\tilde{\mathbf{x}}} + \dot{\mathbf{x}}_{d}(t)) = \mu(\mathbf{x}, \dot{\mathbf{x}})$ 。对于系统的线性部分，也进行替换 $w = (w_{1}^{T}, w_{2}^{T})^{T} = (e_{\tau}^{T}, \dot{e}_{\tau}^{T})^{T}$ 和：

$$A = \begin{bmatrix} 0 & I \\ -D_{\tau} & -(K_{\tau} + K B^{-1}) \end{bmatrix}.$$

这导致了系统：

$$\Lambda(\tilde{\mathbf{x}}, t) \ddot{\tilde{\mathbf{x}}} + (\mu(\tilde{\mathbf{x}}, \dot{\tilde{\mathbf{x}}}, t) + D_{d}) \dot{\tilde{\mathbf{x}}} + K_{d} \tilde{\mathbf{x}} = J(\tilde{\mathbf{x}}, t)^{-T} w_{1}, \quad (6.26)$$ $$\dot{w} = A w. \quad (6.27)$$

注意，这个系统具有级联结构，因为线性系统 $\dot{w} = A w$ 不依赖于状态变量 $\tilde{\mathbf{x}}$ 和 $\dot{\tilde{\mathbf{x}}}$ 。对于这种级联形式的非线性时不变系统要渐近稳定，必须证明耦合系统的所有解保持有界，并且解耦子系统是渐近稳定的（定理6.1）。在时变情况下，必须满足定理6.2的条件。

对于系统（6.26）-（6.27），定理6.2的第一个条件（存在一个非递减函数 $H(\cdot)$ 使得（6.6）成立）由于假设雅可比矩阵 $J(\tilde{\mathbf{x}}, t)$ 非奇异而得到满足。因此，存在一个 $\delta \in \mathbb{R}$ ， $0 < \delta < \infty$ ，使得：

$$||J(\tilde{\mathbf{x}}, t)^{-T}|| \leq \sup_{t \in [0, \infty)} \sqrt{\lambda_{\max}(J(\tilde{\mathbf{x}}, t)^{-1} J(\tilde{\mathbf{x}}, t)^{-T})} < \delta,$$

其中 $\lambda_{\max}(H(t))$ 是矩阵 $H(t)$ 在时间 $t$ 的最大特征值。两个解耦子系统的一致全局渐近稳定性由命题3.4以及线性系统 $\dot{w} = A w$ （对于正定矩阵 $D_{\tau}$ 和 $K_{\tau}$ 甚至是全局指数稳定的）给出。

因此，证明耦合系统的所有解是一致全局有界的就足够了。因此，考虑以下连续可微、正定、递减且径向无界的函数（见定义A.2）：

$$V_{c}(\tilde{\mathbf{x}}, \dot{\tilde{\mathbf{x}}}, w, t) = \frac{1}{2} \dot{\tilde{\mathbf{x}}}^{T} \Lambda(\tilde{\mathbf{x}}, t) \dot{\tilde{\mathbf{x}}} + \frac{1}{2} \tilde{\mathbf{x}}^{T} K_{d} \tilde{\mathbf{x}} + \frac{1}{2} w^{T} P w \quad (6.28)$$

其中 $P \in \mathbb{R}^{2n \times 2n}$ 是一个正定矩阵。在这一点上值得一提的是， $V_{c}(\tilde{\mathbf{x}}, \dot{\tilde{\mathbf{x}}}, w, t)$ 由某个时不变、径向无界且正定的函数 $W_{1}(\tilde{\mathbf{x}}, \dot{\tilde{\mathbf{x}}}, w)$ 和 $W_{2}(\tilde{\mathbf{x}}, \dot{\tilde{\mathbf{x}}}, w)$ 从上方和下方有界：

$$W_{1}(\tilde{\mathbf{x}}, \dot{\tilde{\mathbf{x}}}, w) \leq V_{c}(\tilde{\mathbf{x}}, \dot{\tilde{\mathbf{x}}}, w, t) \leq W_{2}(\tilde{\mathbf{x}}, \dot{\tilde{\mathbf{x}}}, w)$$ $$W_{1}(\tilde{\mathbf{x}}, \dot{\tilde{\mathbf{x}}}, w) = \frac{1}{2} \lambda_{1} ||\dot{\tilde{\mathbf{x}}}||_{2}^{2} + \frac{1}{2} \tilde{\mathbf{x}}^{T} K_{d} \tilde{\mathbf{x}} + \frac{1}{2} w^{T} P w$$ $$W_{2}(\tilde{\mathbf{x}}, \dot{\tilde{\mathbf{x}}}, w) = \frac{1}{2} \lambda_{2} ||\dot{\tilde{\mathbf{x}}}||_{2}^{2} + \frac{1}{2} \tilde{\mathbf{x}}^{T} K_{d} \tilde{\mathbf{x}} + \frac{1}{2} w^{T} P w$$

其中：

$$0 < \lambda_{1} < \inf_{t \in [0, \infty)} \lambda_{\min}(\Lambda(\tilde{\mathbf{x}}, t)) < \sup_{t \in [0, \infty)} \lambda_{\max}(\Lambda(\tilde{\mathbf{x}}, t)) < \lambda_{2} < \infty,$$

其中 $\lambda_{\min}(A(t))$ 和 $\lambda_{\max}(A(t))$ 是 $A(t)$ 在时间 $t$ 的最小和最大特征值。这由性质3.3保证。

从众所周知的引理3.2中的斜对称性质可以得出， $V_{c}(\tilde{\mathbf{x}}, \dot{\tilde{\mathbf{x}}}, w, t)$ 沿（6.24）-（6.25）解的时间导数由下式给出：

$$\dot{V}_{c}(\tilde{\mathbf{x}}, \dot{\tilde{\mathbf{x}}}, w, t) = -\dot{\tilde{\mathbf{x}}}^{T} D_{d} \dot{\tilde{\mathbf{x}}} - \frac{1}{2} w^{T} Q w + \dot{\tilde{\mathbf{x}}}^{T} J(\tilde{\mathbf{x}}, t)^{-T} w_{1},$$

其中矩阵 $Q \in \mathbb{R}^{2n \times 2n}$ 通过李雅普诺夫方程与 $P$ 相关：

$$Q = -(P A + A^{T} P).$$

对于下面的论证，重要的是要注意，由于引理A.21，也可以为 $Q$ 选择任意正定矩阵，并计算唯一的正定矩阵 $P$ ，使得 $Q = -(P A + A^{T} P)$ 成立。这是因为对于正定矩阵 $D_{\tau}$ 和 $K_{\tau}$ ， $A$ 是Hurwitz的（见引理A.21）。

显然， $\dot{V}_{c}(\tilde{\mathbf{x}}, \dot{\tilde{\mathbf{x}}}, w, t)$ 可以写成矩阵形式：

$$\dot{V}_{c}(\tilde{\mathbf{x}}, \dot{\tilde{\mathbf{x}}}, w, t) = - \begin{pmatrix} \dot{\tilde{\mathbf{x}}} \\ w_{1} \\ w_{2} \end{pmatrix}^{T} N(\tilde{\mathbf{x}}, t) \begin{pmatrix} \dot{\tilde{\mathbf{x}}} \\ w_{1} \\ w_{2} \end{pmatrix}$$

其中：

$$N(\tilde{\mathbf{x}}, t) = \begin{bmatrix} D_{d} & -\frac{1}{2} J(\tilde{\mathbf{x}}, t)^{-T} & 0 \\ -\frac{1}{2} J(\tilde{\mathbf{x}}, t)^{-1} & Q_{11} & Q_{12} \\ 0 & Q_{21} & Q_{22} \end{bmatrix}.$$

从引理A.20可以得出， $N(\tilde{\mathbf{x}}, t)$ 正定的充要条件是：

$$Q - \frac{1}{4} J^{-1}(\tilde{\mathbf{x}}, t) D_{d}^{-1} J^{-T}(\tilde{\mathbf{x}}, t) > 0,$$

由于假设 $J(\tilde{\mathbf{x}}, t)$ 不会变得奇异，并且 $Q$ 是某个可以任意选择的正定矩阵，因此对于每个正定矩阵 $D_{d}$ ，这个条件都可以满足。因此，可以得出结论：

$$\dot{V}_{c}(\tilde{\mathbf{x}}, \dot{\tilde{\mathbf{x}}}, \mathbf{w}, t) \leq 0.$$

利用 $V_{c}(\tilde{\mathbf{x}}, \dot{\tilde{\mathbf{x}}}, \mathbf{w}, t)$ 的上述性质，可以应用引理A.8来证明（6.24）-（6.25）的解是一致全局有界的。因此，可以从定理6.2得出结论，命题6.3成立。

注意，在这个稳定性证明中需要引用定理6.2，原因是一方面所考虑的系统是时变的，另一方面所选函数 $V_{c}(\tilde{\mathbf{x}}, \dot{\tilde{\mathbf{x}}}, \mathbf{w}, t)$ 的时间导数不是负定的，而只是负半定的。这一事实，连同 $Q$ 可以任意选择的备注，是与LG96和LG95中证明的最重要区别。

命题 6.4. 对于 $\dot{x}_{d}(t) = 0$ ，具有正定矩阵 $K_{d}$ 、 $D_{d}$ 、 $K_{\tau}$ 和 $D_{\tau}$ 的系统（6.24）-（6.25）变为时不变的，并表示从外力 $F_{ext}$ 到速度误差 $\dot{\tilde{x}}$ 的无源映射。

证明. 函数 $V_{c}(\tilde{\mathbf{x}}, \dot{\tilde{\mathbf{x}}}, \mathbf{w}, t)$ 可以被选为合适的存储函数来证明无源性。在 $F_{ext} \neq 0$ 的情况下，它沿（6.24）-（6.25）解曲线的时间导数由下式给出：

$$\dot{V}_{c}(\tilde{\mathbf{x}}, \dot{\tilde{\mathbf{x}}}, \mathbf{w}, t) = - \begin{pmatrix} \dot{\tilde{\mathbf{x}}} \\ \mathbf{w}_{1} \\ \mathbf{w}_{2} \end{pmatrix}^{T} N(\tilde{\mathbf{x}}, t) \begin{pmatrix} \dot{\tilde{\mathbf{x}}} \\ \mathbf{w}_{1} \\ \mathbf{w}_{2} \end{pmatrix} + \dot{\tilde{\mathbf{x}}}^{T} F_{ext} \leq \dot{\tilde{\mathbf{x}}}^{T} F_{ext}. \quad (6.29)$$

矩阵 $N(\tilde{\mathbf{x}}, t)$ 已被证明是正定的。显然，不等式（6.29）证明了命题6.4的无源性性质。

6.1.5 控制器讨论

完整的控制器方程由下式给出：

$$\begin{array}{l} \boldsymbol{\tau}_{m} = \boldsymbol{\tau}_{d} + \mathbf{B} \mathbf{K}^{-1} \left( \ddot{\boldsymbol{\tau}}_{d} - \mathbf{D}_{\tau} (\dot{\boldsymbol{\tau}} - \dot{\boldsymbol{\tau}}_{d}) - \mathbf{K}_{\tau} (\boldsymbol{\tau} - \boldsymbol{\tau}_{d}) \right) \\ \boldsymbol{\tau}_{d} = \mathbf{g}(\mathbf{q}) + \mathbf{J}(\mathbf{q})^{T} \left( \Lambda(\mathbf{x}) \ddot{\mathbf{x}}_{d} + \mu(\mathbf{x}, \dot{\mathbf{x}}) \dot{\mathbf{x}}_{d} - \mathbf{K}_{d} \tilde{\mathbf{x}} - \mathbf{D}_{d} \dot{\mathbf{x}} \right) \end{array} \quad (6.30)$$

注意，控制器结构与上一章的控制器非常相似。与该控制器相比，该控制器的主要优势在于它允许在不参考近似分析的情况下证明渐近稳定性。与基于奇异摄动的控制器相比，该控制器还包含了用于内扭矩控制环的前馈项 $\dot{\boldsymbol{\tau}}_{d}$ 和 $\ddot{\boldsymbol{\tau}}_{d}$ ，以及基本上建立了部分反馈线性化的项 $\mathbf{B}\dot{\mathbf{q}}$ 。由于 $\ddot{\mathbf{q}}$ 在实践中通常无法直接测量，因此在上述控制器方程中通过 $\mathbf{q}$ 、 $\dot{\mathbf{q}}$ 、 $\boldsymbol{\tau}$ 和外部扭矩 $\boldsymbol{\tau}_{ext}$ 来表示。因此，即使对于（6.21）中的简化期望动态，也必须测量外部扭矩。与更简单的基于奇异摄动的控制器相比，这显然是一个缺点。此外，项 $\dot{\boldsymbol{\tau}}_{d}$ 和 $\ddot{\boldsymbol{\tau}}_{d}$ 包含连杆加速度 $\ddot{\mathbf{q}}$ 和加加速度 $\mathbf{q}^{(3)}$ ，因为 $\boldsymbol{\tau}_{d}$ 是 $\mathbf{q}$ 和 $\dot{\mathbf{q}}$ （或等价地 $\mathbf{x}$ 和 $\dot{\mathbf{x}}$ ）的函数。对于只有几个关节（例如一到三个关节）的机器人，这些信号的计算是可以做到的，但对于具有六个或七个关节的机器人来说则非常困难。原则上，可以基于模型方程通过以下方式计算这些信号：

$$\begin{array}{l} \ddot{\mathbf{q}} = \mathbf{M}(\mathbf{q})^{-1} (\boldsymbol{\tau} + \boldsymbol{\tau}_{ext} - \mathbf{C}(\mathbf{q}, \dot{\mathbf{q}}) \dot{\mathbf{q}} - \mathbf{g}(\mathbf{q})), \\ \mathbf{q}^{(3)} = \mathbf{M}(\mathbf{q})^{-1} \left( \dot{\boldsymbol{\tau}} - \dot{\mathbf{M}}(\mathbf{q}) \ddot{\mathbf{q}} + \frac{\mathrm{d}}{\mathrm{d}t} (\boldsymbol{\tau}_{ext} - \mathbf{C}(\mathbf{q}, \dot{\mathbf{q}}) \dot{\mathbf{q}} - \mathbf{g}(\mathbf{q})) \right). \end{array} \quad (6.31)$$

这还需要额外计算 $\dot{\boldsymbol{\tau}}_{ext}$ （例如通过滤波）和 $\dot{\mathbf{C}}(\mathbf{q},\dot{\mathbf{q}})$ 。另一个关键点是在这个计算中使用逆惯性矩阵。虽然惯性矩阵总是正定的（因此可逆），但其特征值具有不同的数量级，并且根据关节配置，其变化可能达到几个数量级Fea04。因此，求逆可能在数值上是不适定的。

计算 $\ddot{\mathbf{q}}$ 和 $\mathbf{q}^{(3)}$ 的另一种可能方法是使用加速度传感器和/或适当的滤波技术。关节加速度传感器的使用似乎是实现所讨论控制器的一个有前途的方法。然而，事实证明，将关节加速度传感器恰当地集成到机器人的机械结构中是很困难的Gei03。

为了总结这个讨论，可以说从实现的角度来看，这个控制器确实相当困难。对于一个只有少量自由度的机器人，实现似乎是可行的，但需要对外部扭矩或加速度传感器的信号进行适当的滤波。另一方面，所讨论的控制器回答了基于奇异摄动的控制器必须扩展哪些项才能允许严格的稳定性分析。

6.2 基于反步法的方法

首先，在第6.2.1节中描述了（积分器）反步法的基本设计思路。然后，在第6.2.2节中介绍其在柔性关节机器人模型上的应用。

6.2.1 反步法设计步骤

积分器反步法是一种用于构造李雅普诺夫函数的逐步设计过程，可以应用于一大类系统。下面，针对一类特定的系统描述基本思路，其中一步反步法就足够了。但该过程可以轻松地递归应用于更复杂的系统。

考虑以下形式的系统（图6.2）：

$$\begin{array}{r} \dot{z}_1 = \mathbf{f}_1(z_1) + \mathbf{F}_2(z_1) z_2, \\ \dot{z}_2 = \mathbf{u}, \end{array} \quad (6.31)$$

其中状态变量为 $z_{1}\in \mathbb{R}^{n_{1}}$ 、 $z_{2}\in \mathbb{R}^{n_{2}}$ ，输入为 $\mathbf{u}\in \mathbb{R}^{n_{2}}$ 。假设函数 $\mathbf{f}_1(z_1): \mathbb{R}^{n_1} \to \mathbb{R}^{n_1}$ 和 $\mathbf{F}_2(z_1): \mathbb{R}^{n_1} \to \mathbb{R}^{n_2 \times n_2}$ 是已知的。注意，状态变量 $z_{1}$ 的系统方程不直接受控制输入 $\mathbf{u}$ 的影响，而只是间接地通过 $z_{2}$ 。为了设计一个稳定该系统的控制器，首先将变量 $z_{2}$ 视为（6.31）的虚拟控制输入。现在假设已知一个可微的控制律 $z_{2} = k(z_{1})$ ，它稳定了（6.31）。那么存在一个正定李雅普诺夫函数 $V_{1}(z_{1})$ ，用于系统：

$$\dot{z}_{1} = \mathbf{f}_{1}(z_{1}) + \mathbf{F}_{2}(z_{1}) \mathbf{k}(z_{1}) \quad (6.33)$$

其导数是负半定的：

$$\dot{V}_{1}(z_{1}) = \frac{\partial V_{1}(z_{1})}{\partial z_{1}} (\mathbf{f}_{1}(z_{1}) + \mathbf{F}_{2}(z_{1}) \mathbf{k}(z_{1})) \leq 0. \quad (6.34)$$

然后引入误差变量 $e_{2} = z_{2} - k(z_{1})$ ，使得系统（6.31）-（6.32）可以在状态 $(z_{1}, e_{2})$ 中写成：

$$\begin{array}{l} \dot{z}_{1} = f_{1}(z_{1}) + F_{2}(z_{1})(e_{2} + k(z_{1})), \\ \dot{e}_{2} = u - \frac{\partial k(z_{1})}{\partial z_{1}} (f_{1}(z_{1}) + F_{2}(z_{1})(e_{2} + k(z_{1}))). \end{array} \quad (6.36)$$

对于这个系统，函数 $V_{2}(z_{1}, e_{2}) = V_{1}(z_{1}) + \frac{1}{2} e_{2}^{T} e_{2}$ 被考虑为李雅普诺夫函数候选。 $V_{2}(z_{1}, z_{2})$ 沿（6.35）-（6.36）解的导数由下式给出：

$$\begin{array}{l} \dot{V}_{2}(z_{1}, e_{2}) = \frac{\partial V_{1}(z_{1})}{\partial z_{1}} (f_{1}(z_{1}) + F_{2}(z_{1}) k(z_{1})) + \frac{\partial V_{1}(z_{1})}{\partial z_{1}} F_{2}(z_{1}) e_{2} + \\ e_{2}^{T} u - e_{2}^{T} \frac{\partial k(z_{1})}{\partial z_{1}} (f_{1}(z_{1}) + F_{2}(z_{1})(e_{2} + k(z_{1}))). \end{array} \quad (6.36)$$

由于（6.34）， $\dot{V}_{2}(z_{1}, e_{2})$ 中的第一项在 $z_{1}$ 中是负半定的。为了使 $\dot{V}_{2}(z_{1}, e_{2})$ 在 $(z_{1}, e_{2})$ 中成为负半定，可以使用以下控制律：

$$\begin{array}{l} u = -K_{e} e_{2} + \frac{\partial k(z_{1})}{\partial z_{1}} (f_{1}(z_{1}) + F_{2}(z_{1})(e_{2} + k(z_{1}))) - \left( \frac{\partial V_{1}(z_{1})}{\partial z_{1}} F_{2}(z_{1}) \right)^{T} \\ = -K_{e}(z_{2} - k(z_{1})) + \frac{\partial k(z_{1})}{\partial z_{1}} (f_{1}(z_{1}) + F_{2}(z_{1}) z_{2}) - \left( \frac{\partial V_{1}(z_{1})}{\partial z_{1}} F_{2}(z_{1}) \right)^{T}, \end{array} \quad (6.37)$$

其中 $K_{e}$ 是任意正定矩阵。

这个过程，对应于一个反步法步骤，在以下引理中形式化。

引理 6.5. 考虑系统（6.31）-（6.32）。设 $k(z_{1})$ 是 $z_{2}$ 的可微状态反馈律，它（渐近地）稳定了（6.31），并且 $V_{1}(z_{1})$ 是（6.33）的（严格）李雅普诺夫函数。则反馈律（6.37）（其中 $K_{e}$ 是正定矩阵）（渐近地）稳定了系统（6.31）-（6.32），并且函数 $V_{2}(z_{1}, z_{2}) = V_{1}(z_{1}) + \frac{1}{2} (z_{2} - k(z_{1}))^{T} (z_{2} - k(z_{1}))$ 可以用作（严格）李雅普诺夫函数。

注意，为了处理更广泛的系统类别，可以对这个过程进行许多扩展（例如，参见SJK97）。例如，引理6.5可以很容易地推广到时变情况。

6.2.2 应用于柔性关节模型

上一节的过程现在将应用于第2.2.3节的系统：

$$\begin{array}{r} M(\mathbf{q})\ddot{\mathbf{q}} + C(\mathbf{q},\dot{\mathbf{q}})\dot{\mathbf{q}} + g(\mathbf{q}) = K(\boldsymbol{\theta} - \mathbf{q}) + \boldsymbol{\tau}_{ext}, \\ B\ddot{\boldsymbol{\theta}} + K(\boldsymbol{\theta} - \mathbf{q}) = \boldsymbol{\tau}_{m}. \end{array} \quad (6.38)$$

这些系统方程的结构并不完全对应于第6.2.1节中讨论的那类系统。因此，本节中的以下推导将不直接依赖于引理6.5。基本设计过程将与推导引理6.5的过程非常相似。

反步法已被应用于OL97和NT92中的柔性关节机器人位置控制。而本工作的兴趣在于设计一个阻抗控制器。与上一节的状态变换类似，该模型可以用笛卡尔坐标 $\mathbf{x} = \mathbf{f}(\mathbf{q})$ 和 $\dot{\mathbf{x}} = \mathbf{J}(\mathbf{q})\dot{\mathbf{q}}$ 以及扭矩变量 $\boldsymbol{\tau} = \mathbf{K}(\boldsymbol{\theta} - \mathbf{q})$ 和 $\dot{\boldsymbol{\tau}}$ 写成：

$$\begin{array}{r} \Lambda(\mathbf{x}) \ddot{\mathbf{x}} + \mu(\mathbf{x}, \dot{\mathbf{x}}) \dot{\mathbf{x}} + \mathbf{J}(\mathbf{q})^{-T} g(\mathbf{q}) = \mathbf{J}(\mathbf{q})^{-T} (\boldsymbol{\tau} + \boldsymbol{\tau}_{ext}), \\ \mathbf{B} \mathbf{K}^{-1} \ddot{\boldsymbol{\tau}} + \boldsymbol{\tau} = \boldsymbol{\tau}_{m} - \mathbf{B} \ddot{\mathbf{q}}, \end{array} \quad (6.41)$$

其中连杆加速度 $\ddot{\mathbf{q}}$ 也可以写成形式 $\ddot{\mathbf{q}} = \mathbf{M}(\mathbf{q})^{-1} (\boldsymbol{\tau} + \boldsymbol{\tau}_{ext} - \mathbf{C}(\mathbf{q}, \dot{\mathbf{q}}) \dot{\mathbf{q}} - \mathbf{g}(\mathbf{q}))$ ，如上一节所做的那样。在本节中，再次处理非冗余和非奇异情况。对于刚体情况，在 $\boldsymbol{\tau}_{ext} = \mathbf{0}$ 时，（3.18）中的控制律渐近地稳定了系统（6.40）。在第3.2节中给出了一个（非严格的）李雅普诺夫函数：

$$V_{1}(\tilde{\mathbf{x}}, \dot{\mathbf{x}}, t) = \frac{1}{2} \dot{\mathbf{x}}^{T} \Lambda(\mathbf{x}) \dot{\mathbf{x}} + \frac{1}{2} \tilde{\mathbf{x}}^{T} \mathbf{K}_{d} \tilde{\mathbf{x}}. \quad (6.42)$$

通过将扭矩 $\boldsymbol{\tau}$ 视为虚拟输入向量，可以看出以下形式的反馈律：

$$\boldsymbol{\tau} = \boldsymbol{\tau}_{d} = \mathbf{g}(\mathbf{q}) + \mathbf{J}(\mathbf{q})^{T} (\Lambda(\mathbf{x}) \dot{\mathbf{x}}_{d} + \mu(\mathbf{x}, \dot{\mathbf{x}}) \dot{\mathbf{x}}_{d} - \mathbf{K}_{d} \tilde{\mathbf{x}} - \mathbf{D}_{d} \dot{\mathbf{x}})$$

将导致：

$$\dot{V}_{1}(\tilde{\mathbf{x}}, \dot{\mathbf{x}}, t) = -\dot{\mathbf{x}}^{T} \mathbf{D}_{d} \dot{\mathbf{x}} + \dot{\mathbf{x}}^{T} \mathbf{J}(\mathbf{q})^{-T} \boldsymbol{\tau}_{ext}.$$

这显示了自由运动情况下（即 $\boldsymbol{\tau}_{ext} = \mathbf{0}$ ）的稳定性。由于在柔性关节情况下不能直接将 $\boldsymbol{\tau}$ 用作输入变量，通常会有 $\boldsymbol{\tau} \neq \boldsymbol{\tau}_{d}$ ，并且令 $\mathbf{e}_{t} = \boldsymbol{\tau} - \boldsymbol{\tau}_{d}$ ，则 $V_{1}(\tilde{\mathbf{x}}, \dot{\mathbf{x}}, t)$ 的时间导数由下式给出：

$$\dot{V}_{1}(\tilde{\mathbf{x}}, \dot{\mathbf{x}}, e_{t}, t) = -\dot{\mathbf{x}}^{T} \mathbf{D}_{d} \dot{\mathbf{x}} + \dot{\mathbf{x}}^{T} \mathbf{J}(\mathbf{q})^{-T} \boldsymbol{\tau}_{ext} + \dot{\mathbf{x}}^{T} \mathbf{J}(\mathbf{q})^{-T} e_{t}.$$

反步法（步骤1）建议在李雅普诺夫函数中引入一个附加项来抵消交叉项。令 $\mathbf{C}_{t}$ 为对称正定矩阵，并且：

$$V_{2}(\tilde{\mathbf{x}}, \dot{\mathbf{x}}, e_{t}, t) = \frac{1}{2} \dot{\mathbf{x}}^{T} \Lambda(\mathbf{x}) \dot{\mathbf{x}} + \frac{1}{2} \tilde{\mathbf{x}}^{T} \mathbf{K}_{d} \tilde{\mathbf{x}} + \frac{1}{2} \mathbf{e}_{t}^{T} \mathbf{C}_{t} e_{t}$$

作为新的李雅普诺夫函数候选， $V_{2}(\tilde{\mathbf{x}}, \dot{\tilde{\mathbf{x}}}, e_{t}, t)$ 沿系统解的时间导数由下式给出：

$$\dot{V}_{2}(\tilde{\mathbf{x}}, \dot{\tilde{\mathbf{x}}}, e_{l}, e_{t}, t) = -\dot{\tilde{\mathbf{x}}}^{T} \mathbf{D}_{d} \dot{\tilde{\mathbf{x}}} + \dot{\tilde{\mathbf{x}}}^{T} \mathbf{J}(q)^{-T} \boldsymbol{\tau}_{ext} + \dot{\tilde{\mathbf{x}}}^{T} \mathbf{J}(q)^{-T} e_{t} + \mathbf{e}_{t}^{T} \mathbf{C}_{t} \dot{\mathbf{e}}_{t}.$$

接下来，误差项 $\dot{\mathbf{e}}_{t}$ 被视为虚拟输入。通过选择：

$$\dot{\mathbf{e}}_{t} = \mathbf{v}_{t}^{*} = -C_{t}^{-1} \mathbf{J}(q)^{-1} \dot{\tilde{\mathbf{x}}} - C_{t}^{-1} \mathbf{K}_{t} e_{t},$$

其中 $\mathbf{K}_{t}$ 是正定控制器增益矩阵，将理想地消除 $\dot{V}_{2}(\tilde{\mathbf{x}}, \dot{\tilde{\mathbf{x}}}, e_{t}, e_{t}, t)$ 中的交叉项，并获得：

$$\dot{V}_{2}(\tilde{\mathbf{x}}, \dot{\tilde{\mathbf{x}}}, e_{t}, t) = -\dot{\tilde{\mathbf{x}}}^{T} \mathbf{D}_{d} \dot{\tilde{\mathbf{x}}} + \dot{\tilde{\mathbf{x}}}^{T} \mathbf{J}(q)^{-T} \boldsymbol{\tau}_{ext} - \mathbf{e}_{t}^{T} \mathbf{K}_{t} e_{t} + \mathbf{e}_{t}^{T} \mathbf{C}_{t} e_{s}.$$

但由于通常 $\dot{\mathbf{e}}_{t} \neq \mathbf{v}_{t}^{*}$ ，通过引入误差变量 $\mathbf{e}_{s} = \dot{\mathbf{e}}_{t} - \mathbf{v}_{t}^{*}$ ，实际上得到李雅普诺夫函数沿解曲线的变化为：

$$\dot{V}_{2}(\tilde{\mathbf{x}}, \dot{\tilde{\mathbf{x}}}, e_{t}, e_{s}, t) = -\dot{\tilde{\mathbf{x}}}^{T} \mathbf{D}_{d} \dot{\tilde{\mathbf{x}}} + \dot{\tilde{\mathbf{x}}}^{T} \mathbf{J}(q)^{-T} \boldsymbol{\tau}_{ext} - \mathbf{e}_{t}^{T} \mathbf{K}_{t} e_{t} + \mathbf{e}_{t}^{T} \mathbf{C}_{t} e_{s}.$$

为了也消除剩余的交叉项，必须执行额外的反步法步骤（步骤2），首先通过使用对称正定矩阵 $C_{s}$ 在李雅普诺夫函数中引入一个新项：

$$V_{3}(\tilde{\mathbf{x}}, \dot{\tilde{\mathbf{x}}}, e_{t}, e_{s}, t) = \frac{1}{2} \dot{\tilde{\mathbf{x}}}^{T} \Lambda(\mathbf{x}) \dot{\tilde{\mathbf{x}}} + \frac{1}{2} \tilde{\mathbf{x}}^{T} \mathbf{K}_{d} \tilde{\mathbf{x}} + \frac{1}{2} \mathbf{e}_{t}^{T} \mathbf{C}_{t} e_{t} + \frac{1}{2} \mathbf{e}_{s}^{T} \mathbf{C}_{s} e_{s},$$

使得其时间导数读作：

$$\dot{V}_{3}(\tilde{\mathbf{x}}, \dot{\tilde{\mathbf{x}}}, e_{t}, e_{s}, t) = -\dot{\tilde{\mathbf{x}}}^{T} \mathbf{D}_{d} \dot{\tilde{\mathbf{x}}} + \dot{\tilde{\mathbf{x}}}^{T} \mathbf{J}(q)^{-T} \boldsymbol{\tau}_{ext} - \mathbf{e}_{t}^{T} \mathbf{K}_{t} e_{t} + \mathbf{e}_{t}^{T} \mathbf{C}_{t} e_{s} + \mathbf{e}_{s}^{T} \mathbf{C}_{s} \dot{\mathbf{e}}_{s}.$$

利用 $\dot{\mathbf{e}}_{s} = \ddot{\boldsymbol{\tau}} - \ddot{\boldsymbol{\tau}}_{d} - \dot{\mathbf{v}}_{t}^{*}$ 并代入扭矩动力学方程（6.41），得到：

$$\dot{V}_{3}(\tilde{\mathbf{x}}, \dot{\tilde{\mathbf{x}}}, e_{t}, e_{s}, t) = -\dot{\tilde{\mathbf{x}}}^{T} \mathbf{D}_{d} \dot{\tilde{\mathbf{x}}} + \dot{\tilde{\mathbf{x}}}^{T} \mathbf{J}(q)^{-T} \boldsymbol{\tau}_{ext} - \mathbf{e}_{t}^{T} \mathbf{K}_{t} e_{t} + \mathbf{e}_{t}^{T} \mathbf{C}_{t} e_{s} + \mathbf{e}_{s}^{T} \mathbf{C}_{s} (\mathbf{K} \mathbf{B}^{-1} (\boldsymbol{\tau}_{m} - \mathbf{B} \ddot{\mathbf{q}} - \boldsymbol{\tau}) - \ddot{\boldsymbol{\tau}}_{d} - \dot{\mathbf{v}}_{t}^{*}).$$

控制器设计： 通过完成反步法步骤，现在可以选择控制器为：

$$\boldsymbol{\tau}_{m} = \mathbf{B} \ddot{\mathbf{q}} + \boldsymbol{\tau} + \mathbf{B} \mathbf{K}^{-1} (\ddot{\boldsymbol{\tau}}_{d} + \dot{\mathbf{v}}_{t}^{*} - \mathbf{C}_{s}^{-1} \mathbf{C}_{t} e_{t} - \mathbf{C}_{s}^{-1} \mathbf{K}_{s} e_{s}) \quad (6.43)$$

其中 $\mathbf{K}_{s}$ 是正定矩阵。这导致：

$$\dot{V}_{3}(\tilde{\mathbf{x}}, \dot{\tilde{\mathbf{x}}}, e_{t}, e_{s}, t) = -\dot{\tilde{\mathbf{x}}}^{T} \mathbf{D}_{d} \dot{\tilde{\mathbf{x}}} + \dot{\tilde{\mathbf{x}}}^{T} \mathbf{J}(q)^{-T} \boldsymbol{\tau}_{ext} - \mathbf{e}_{t}^{T} \mathbf{K}_{t} e_{t} - \mathbf{e}_{s}^{T} \mathbf{K}_{s} e_{s},$$

因此，在自由运动情况下， $\dot{V}_{3}(\tilde{\mathbf{x}}, \dot{\tilde{\mathbf{x}}}, e_{t}, e_{s}, t)$ 是负半定的。因此，可以得出结论，平衡点在李雅普诺夫意义下是稳定的。

通过替换表达式 $\mathbf{e}_{s} = \dot{\mathbf{e}}_{t} - \mathbf{v}_{t}^{*}$ 和 $\mathbf{v}_{t}^{*} = - \mathbf{C}_{t}^{-1} \mathbf{J}(\mathbf{q})^{-1} \dot{\mathbf{x}} - \mathbf{C}_{t}^{-1} \mathbf{K}_{t} \mathbf{e}_{t}$ ，可以将（6.43）中的控制器转换为更便于处理的形式：

$$\begin{array}{rl} & \boldsymbol{\tau}_{m} = \mathbf{B} \ddot{\mathbf{q}} + \boldsymbol{\tau} + \mathbf{B} \mathbf{K}^{-1} (\ddot{\boldsymbol{\tau}}_{d} - \mathbf{C}_{t}^{-1} \frac{\mathrm{d}}{\mathrm{d}t} (\mathbf{J}(\mathbf{q})^{-1} \dot{\mathbf{x}}) - \mathbf{C}_{t}^{-1} \mathbf{K}_{t} \dot{\mathbf{e}}_{t} - \mathbf{C}_{s}^{-1} \mathbf{C}_{t} \mathbf{e}_{t}) \\ & - \mathbf{B} \mathbf{K}^{-1} \mathbf{C}_{s}^{-1} \mathbf{K}_{s} (\dot{\mathbf{e}}_{t} + \mathbf{C}_{t}^{-1} \mathbf{J}(\mathbf{q})^{-1} \dot{\mathbf{x}} + \mathbf{C}_{t}^{-1} \mathbf{K}_{t} \mathbf{e}_{t}). \end{array} \quad (6.44)$$

通过重新排列项，并使用替换 $\bar{K}_{t} = C_{t}^{-1} K_{t}$ 和 $\bar{K}_{s} = C_{s}^{-1} K_{s}$ ，控制器最终可以写成：

$$\begin{array}{rl} & \boldsymbol{\tau}_{m} = \mathbf{B} \ddot{\mathbf{q}} + \boldsymbol{\tau} + \mathbf{B} \mathbf{K}^{-1} \left( \ddot{\boldsymbol{\tau}}_{d} - (\bar{K}_{s} \bar{K}_{t} + C_{s}^{-1} C_{t}) \mathbf{e}_{t} - (\bar{K}_{s} + \bar{K}_{t}) \dot{\mathbf{e}}_{t} \right) \\ & - \mathbf{B} \mathbf{K}^{-1} (C_{t}^{-1} \frac{\mathrm{d}}{\mathrm{d}t} (\mathbf{J}(\mathbf{q})^{-1} \dot{\mathbf{x}}) + \bar{K}_{s} C_{t}^{-1} \mathbf{J}(\mathbf{q})^{-1} \dot{\mathbf{x}}), \end{array} \quad (6.44)$$

其中 $\boldsymbol{\tau}_{d}$ 由下式给出：

$$\boldsymbol{\tau}_{d} = \mathbf{g}(\mathbf{q}) + \mathbf{J}(\mathbf{q})^{T} (\Lambda(\mathbf{x}) \ddot{\mathbf{x}}_{d} + \mu(\mathbf{x}, \dot{\mathbf{x}}) \dot{\mathbf{x}}_{d} - \mathbf{K}_{d} \tilde{\mathbf{x}} - \mathbf{D}_{d} \dot{\mathbf{x}}).$$

结果可以总结为以下命题。

命题 6.6. 设期望轨迹 $\mathbf{x}_{d}(t)$ 是四阶连续可微的。进一步假设笛卡尔坐标全局有效，即 $\bar{\mathcal{Q}}_{c}^{p} = \mathbb{R}^{n}$ 。那么，在自由运动情况下（ $\mathbf{F}_{ext} = \mathbf{0}$ ），控制器（6.44）具有对称正定矩阵 $\mathbf{K}_{d}$ 、 $\mathbf{D}_{d}$ 、 $\mathbf{C}_{t}$ 、 $\mathbf{C}_{s}$ 、 $\mathbf{K}_{t}$ 和 $\mathbf{K}_{s}$ ，全局地稳定了系统（6.40）-（6.41）。

证明.（概要） 可以通过证明函数 $V_{3}(\mathbf{e}_{x}, \dot{\mathbf{e}}_{x}, \mathbf{e}_{t}, \mathbf{e}_{s}, t)$ 是一个（时变的）李雅普诺夫函数（其沿闭环系统解曲线的导数为负半定）来轻松完成证明。这可以通过遵循上述基于反步法的控制器推导步骤来完成。

除了这个稳定性陈述之外，类似于命题3.5的无源性性质也很容易得出。

命题 6.7. 考虑根据控制律（6.44）的闭环系统，该系统具有对称正定矩阵 $\mathbf{K}_{d}$ 、 $\mathbf{D}_{d}$ 、 $\mathbf{C}_{t}$ 、 $\mathbf{C}_{s}$ 、 $\mathbf{K}_{t}$ 和 $\mathbf{K}_{s}$ ，应用于系统（6.40）-（6.41）。对于 $\dot{\mathbf{x}}_{d}(t) = \mathbf{0}$ ，该闭环系统变为时不变的，并表示从外力 $\mathbf{F}_{ext}$ 到速度误差 $\dot{\mathbf{x}}$ 的无源映射。

证明. 这可以通过将 $V_{3}(\mathbf{e}_{x}, \dot{\mathbf{e}}_{x}, \mathbf{e}_{t}, \mathbf{e}_{s})$ （对于 $\dot{\mathbf{x}}_{d}(t) = \mathbf{0}$ 是时不变的）视为闭环系统的存储函数来证明。

6.2.3 控制器讨论

注意控制器（6.44）与上一节控制器（6.30）的相似性。通过对应关系 $\mathbf{K}_{\tau} = \bar{\mathbf{K}}_{s} \bar{\mathbf{K}}_{t} + \mathbf{C}_{s}^{-1} \mathbf{C}_{t} - \mathbf{K} \mathbf{B}^{-1}$ 和 $\mathbf{D}_{\tau} = \bar{\mathbf{K}}_{s} + \bar{\mathbf{K}}_{t}$ ，并通过替换 $\ddot{q} = M(q)^{-1} (\tau + \tau_{ext} - C(q,\dot{q})\dot{q} - g(q))$ ，控制器（6.44）可以写成：

$$\begin{array}{rl} & \tau_{m} = \tau_{d} + B K^{-1} \left( \ddot{\tau}_{d} - K_{\tau} e_{t} - D_{\tau} \dot{e}_{t} \right) \\ & B M(q)^{-1} \left( \tau + \tau_{ext} - C(q,\dot{q}) \dot{q} - g(q) \right) \\ & - B K^{-1} (C_{t}^{-1} \frac{\mathrm{d}}{\mathrm{d}t} (J(q)^{-1} \dot{\tilde{x}}) + \bar{K}_{s} C_{t}^{-1} J(q)^{-1} \dot{\tilde{x}}). \end{array} \quad (6.30)$$

因此，该控制器的第一部分与（6.30）具有相同的形式。但应该注意，通常控制器增益 $K_{\tau}$ 和 $D_{\tau}$ 必须选择不同。对于基于解耦的控制器，这些矩阵只需是正定的，但在基于反步法的控制器中，它们必须被选择为使矩阵 $K_{t}$ 、 $K_{s}$ 、 $C_{t}$ 和 $C_{s}$ 都是对称正定的。

从计算的角度来看，本章中的两个控制器是相当的。主要困难在于实现时需要确定连杆加速度 $\ddot{q}$ 和加加速度 $q^{(3)}$ （另见第6.1.5节）。

基于反步法控制器的参数选择更困难。显然，除了给定的阻抗参数 $K_{d}$ 和 $D_{d}$ 之外，还必须额外设计矩阵 $K_{t}$ 、 $K_{s}$ 、 $C_{t}$ 和 $C_{s}$ ，这不太直观。当固定其中两个参数时，可以简化。在该控制器的仿真中（另见第8章），结果表明选择 $C_{s} = C_{t} = I$ 可以获得良好的结果。因此，设计简化为选择正定矩阵 $K_{t}$ 和 $K_{s}$ 。

注意，上述分析允许给出稳定性的证明，但没有提及渐近稳定性。原则上，相同的过程可以应用于刚体部分的严格李雅普诺夫函数。Santibanez和Kelly在SK97b中为刚体部分提出了这样的严格李雅普诺夫函数。这将允许处理渐近稳定性情况，但会进一步增加控制器的复杂性。

6.3 总结

本章提出了两种笛卡尔阻抗控制器，它们利用了简化柔性关节机器人模型的特殊结构。

在第一种方法中，建立了扭矩动力学与连杆侧动力学的精确解耦。结合连杆侧动力学的阻抗控制律，这允许基于时变级联系统的控制理论给出稳定性证明。与上一章的奇异摄动方法相比，不需要对简化柔性关节机器人模型进行进一步的简化或近似。

在本章的第二部分，应用了一种构造性方法，即反步法。在这种方法中，刚体阻抗控制器的李雅普诺夫函数被用作构建柔性关节机器人模型李雅普诺夫函数的基础。这只有通过关节扭矩的额外反馈才可能实现。

本章的方法基于系统动力学的特定结构。通过适当的反馈消除了扭矩动力学和连杆侧动力学之间不期望的耦合项。下一章将讨论一种相当不同的方法，该方法基于机器人模型的无源性特性。

---

7 基于无源性的方法

本章描述了一种用于柔性关节机器人阻抗控制的基于无源性的方法。控制器公式化背后的基本思想是扭矩反馈的物理解释。这允许基于系统的无源性特性来分析稳定性。

柔性关节机器人的能量整形方法首先在Tom91中引入，其中证明了基于电机位置的PD控制器会导致类似于刚体情况的稳定闭环系统。推广到笛卡尔情况由Goldsmith等人在GFG99中处理，其中对无重力效应的柔性关节机器人模型的笛卡尔混合位置/力控制器的稳定性分析进行了介绍。PD类调节控制器的重力补偿问题在DLSZ05, ZSL+03, ZLS04, ZSL+05中得到了解决。

然而，已经证明，在实践中，对于柔性关节机器人，限制于纯电机位置（和速度）反馈控制器（没有额外的非共位反馈）只能得到相当不理想的结果。在一些工作中，考虑了基于关节扭矩以及连杆侧位置反馈的控制器结构，并表明这确实可以带来更好的结果（例如，参见Spo89）。这也通过DLR轻型机器人的实验得到验证ASOFH03。从理论角度来看，使用内扭矩反馈回路通常通过基于奇异摄动理论的近似分析来证明其合理性（见第5章）。

在本章中，取而代之的是给出扭矩反馈的物理解释。因此，可以基于系统的无源性特性给出稳定性证明。重要的是要注意，所描述的控制器本身由于关节扭矩的反馈而不是无源的，但将表明，受控的电机动力学与扭矩反馈的组合是无源的OASK+05, OASK+04a。与无源的（连杆侧）刚体动力学一起，闭环系统因此可以表示为两个无源系统的反馈互连。

在DLR先前的工作中，在ASH00中引入了一个具有完整静态状态反馈（位置和扭矩及其一阶导数）的控制器，基于控制器的无源性特性证明了渐近稳定性。类似于Tom91，在ASH00中使用了基于期望配置的重力补偿项。对于阻抗控制器，这是不合适的，因为对于低期望刚度，与期望配置可能会有很大的偏差。在第7.3节中，将设计一个基于电机位置测量的重力补偿项。这对于阻抗控制问题要好得多。

一种类似的基于无源性的控制器结构，可以在不测量关节速度的情况下实现，由Str01中的IPC给出。应该提到，本章工作的最初想法是关于如何为柔性关节机器人最好地实现这种为刚体机器人设计的基于无源性的控制器（如简单的PD控制器或更复杂的IPC）的思考结果。

所提出的控制器也与ASH00中的状态反馈控制器密切相关，如第7.8节中进一步所述。

本章组织如下：第7.1节描述了基于一些简单物理观察的设计思路。第7.2节将设计思路应用于柔性关节机器人模型。第7.3节处理重力补偿问题。基于无源性特性的详细稳定性分析在第7.4节中给出。此外，在第7.5节中更详细地处理了刚度设计。第7.6节给出了一些直接的控制器推广。第7.7节更详细地考虑了跟踪情况。最后，在第7.8节中简要讨论了该控制器与其他一些设计方法的关系。

7.1 设计思路

在本节中，描述了控制器设计的基本思路。它由一维模型的一些简单考虑所激发。

首先考虑图7.1中为DLR-Lightweight-Robot-III的第二个关节所描绘的单个柔性关节模型。电机扭矩 $\tau_{m}$ 作用在电机的转子惯量 $B$ 上。电机转子与机器人后续连杆之间的传动装置的弹性被建模为线性弹簧，刚度为 $K$ 。

阻抗控制器的目标是实现相对于作用在连杆侧的外力 $F_{ext}$ 的期望动态行为。在下面的讨论中，假设这个动态行为由一个二阶微分方程给出，表示一个质量-弹簧-阻尼器系统，其质量为 $M$ ，期望刚度为 $K_{\theta}$ ，期望阻尼为 $D_{\theta}$ 。对于具有刚性（即非弹性）关节的机器人，这种行为可以通过简单的PD控制器实现。如果在具有如图7.2所示弹性关节的机器人情况下使用基于电机位置的PD控制器，那么最终的动力学显然也会受到关节弹性和电机惯性的影响。直观地说，当转子质量 $B$ 减小且关节刚度 $K$ 增大时，与期望行为的偏差将不那么显著。

在这一点上，应该提到的是，典型柔性关节机器人的关节刚度值确实相当大，但不可忽略。通过关节扭矩 $\tau$ 的负反馈，现在可以缩小（转子的）视在惯量，这意味着闭环系统对外力的反应就好像转子惯量更小一样。转子的视在惯量越小，期望动态行为的近似就越好。如图7.2所示，这种方法将在下一节中针对柔性关节机器人模型具体化。此外，在第7.5节中还将介绍一种补偿弹簧 $K$ 影响的方法。

7.2 应用于柔性关节模型

在第2.2.3节中，推导了柔性关节操纵器的简化模型。为了方便参考，这里复述模型方程：

$$\begin{array}{c} M(q)\ddot{q} + C(q,\dot{q})\dot{q} + g(q) = K(\theta - q) + \tau_{ext}, \\ B\ddot{\theta} + K(\theta - q) = \tau_{m}, \end{array} \quad (7.1)$$

其中使用与第2章相同的符号。如上一节所述，通过反馈关节扭矩 $\tau = K(\theta - q)$ ，视在电机惯量可以从 $B$ 减小到 $B_{\theta}$ 。这通过反馈律实现：

$$\tau_{m} = B B_{\theta}^{-1} u + (I - B B_{\theta}^{-1}) \tau, \quad (7.3)$$

其中 $\mathbf{u}$ 是一个新的输入变量。得到的系统动力学由下式给出：

$$\begin{array}{c} M(q)\ddot{q} + C(q,\dot{q})\dot{q} + g(q) = K(\theta - q) + \tau_{ext}, \\ B_{\theta} \ddot{\theta} + K(\theta - q) = u. \end{array} \quad (7.5)$$

显然，（7.4）和（7.5）具有与原始方程相同的形式，但电机惯量被缩放。输入变量 $\mathbf{u}$ 进一步分为一项 $\mathbf{u}_{imp}$ （实际实现刚度和阻尼）和另一项 $\mathbf{u}_{g}$ （作为重力补偿）：

$$\mathbf{u} = \mathbf{u}_{imp} + \mathbf{u}_{g}. \quad (7.6)$$

在本节的剩余部分，设计项 $\mathbf{u}_{imp}$ 。适当的重力补偿项 $\mathbf{u}_{g}$ 的构建将在第7.3.2节中处理。如果期望的阻抗行为是用关节坐标定义的，其形式为正定刚度和阻尼矩阵 $K_{\theta}$ 和 $D_{\theta}$ ，以及一个（常数）虚拟平衡配置 $\theta_{d}$ ，则可以使用基于电机位置的PD控制器：

$$\mathbf{u}_{imp} = -K_{\theta}(\theta - \theta_{d}) - D_{\theta} \dot{\theta} \quad (7.7)$$

因此，得到以下闭环方程：

$$\begin{array}{c} M(q)\ddot{q} + C(q,\dot{q})\dot{q} + g(q) = \tau + \tau_{ext}, \\ B_{\theta} \ddot{\theta} + D_{\theta} \dot{\theta} + K_{\theta}(\theta - \theta_{d}) + \tau = u_{g}. \end{array} \quad (7.9)$$

7.2.1 推广到笛卡尔坐标

通常，期望的阻抗行为是用笛卡尔坐标 $\mathbf{x}\in \mathbb{R}^{m}$ （描述机器人末端执行器的位置和方向）定义的，而不是用关节坐标。下面假设从关节空间坐标 $\mathbf{q}$ 到笛卡尔坐标 $\mathbf{x} = \mathbf{f}(\mathbf{q})$ 的正向运动学映射以及雅可比矩阵 $\mathbf{J}(\mathbf{q}) = \frac{\partial \mathbf{f}(\mathbf{q})}{\partial \mathbf{q}}$ 是已知的。

控制器（7.7）可以通过使用正向运动学中的电机角度 $\theta$ （而不是连杆侧角度 $\mathbf{q}$ ）轻松推广到笛卡尔坐标，即 $\mathbf{x}_{\theta} = \mathbf{f}(\theta)$ 。期望的刚度和阻尼矩阵由正定矩阵 $\mathbf{K}_{d}$ 和 $\mathbf{D}_{d}$ 给出。则反馈律：

$$\begin{array}{rl} & \mathbf{u}_{imp} = -\mathbf{J}(\boldsymbol{\theta})^{T} (\mathbf{K}_{d} \tilde{\mathbf{x}}_{\theta} + \mathbf{D}_{d} \dot{\mathbf{x}}_{\theta}), \\ & \tilde{\mathbf{x}}_{\theta} = \mathbf{f}(\boldsymbol{\theta}) - \mathbf{x}_{\theta,d}, \\ & \dot{\mathbf{x}}_{\theta} = \mathbf{J}(\boldsymbol{\theta}) \dot{\boldsymbol{\theta}} \end{array} \quad (7.10)$$

将（7.7）推广到笛卡尔坐标。这里， $\mathbf{x}_{\theta ,d}$ 是以笛卡尔坐标表示的虚拟电机侧位置。注意，在设计 $\mathbf{x}_{\theta ,d}$ 时，应考虑由于重力导致的电机和连杆侧角度在静态（即对于 $\boldsymbol{\tau}_{ext} = \mathbf{0}$ 的平衡状态）下的差异。这意味着，对于对应于期望笛卡尔位置 $\mathbf{x}_{d} = \mathbf{f}(\mathbf{q}_{d})$ 的给定连杆侧位置 $\mathbf{q}_{d}$ ， $\mathbf{x}_{\theta ,d}$ 应选择为 $\mathbf{x}_{\theta ,d} = \mathbf{f}(\mathbf{q}_{d} + \mathbf{K}^{-1} \mathbf{g}(\mathbf{q}_{d}))$ 。

然后，（7.10）中的控制器与（7.3）一起，导致闭环系统：

$$\begin{array}{r} M(\mathbf{q})\ddot{\mathbf{q}} + C(\mathbf{q},\dot{\mathbf{q}})\dot{\mathbf{q}} + \mathbf{g}(\mathbf{q}) = \boldsymbol{\tau} + \boldsymbol{\tau}_{ext}, \\ B_{\theta} \ddot{\theta} + \mathbf{J}(\theta)^{T} (K_{d} \tilde{\mathbf{x}}_{\theta} + \mathbf{D}_{d} \dot{\mathbf{x}}_{\theta}) + \boldsymbol{\tau} = \mathbf{u}_{g}. \end{array} \quad (7.13)$$

7.3 重力补偿

在控制律（7.10）中，没有考虑重力扭矩 $\mathbf{g}(\mathbf{q})$ 的影响。在Tom91中已经表明，对于基于电机位置的PD控制器，可以使用期望稳态下的重力扭矩前馈项。这在位置控制器的情况下通常确实能带来良好的性能，因为与期望位置的偏差可以保持很小。然而，对于阻抗控制器，情况并非如此。这里，纯前馈动作进行重力补偿不能给出令人满意的结果，因为在期望刚度 $\mathbf{K}_{d}$ 较小的情况下，与稳态位置可能会有很大的偏差。

在ZSL+03中首次处理了基于电机位置为柔性关节机器人构建在线重力补偿的问题。然而，ZSL+03中的解决方案仍然导致 $\mathbf{K}_{d}$ 的下界，限制了阻抗控制器的通用性。与此相反，本文提出的解决方案不需要这样的附加约束OASK+04b, ASOH04b, OASKH08。下面，在第7.3.1节中更详细地分析重力项 $\mathbf{g}(\mathbf{q})$ 的一些性质。实际的重力补偿将在第7.3.2节中表述，相应的势能函数的推导在第7.3.3节中给出。

7.3.1 重力Hessian矩阵的有界性

根据其第2章的推导，重力项 $\mathbf{g}(\mathbf{q})$ 对应于重力势能 $V_{g}(\mathbf{q})$ 的微分，即 $\mathbf{g}(\mathbf{q}) = (\partial V_{g}(\mathbf{q}) / \partial \mathbf{q})^{T}$ 。性质2.8保证了重力势能的Hessian矩阵在 $\mathcal{Q}^{p}$ 上的有界性。然而，对于稳定性分析，将在以下使用加权范数。

设 $H(q)$ 是重力势能的Hessian矩阵：

$$H(q) := \frac{\partial^2 V_g(q)}{\partial q^2}. \quad (7.19)$$

进一步设 $\alpha_{g}$ 是 $\mathcal{Q}^{p}$ 中该Hessian矩阵的 $K$ 范数的一个上界，即：

$$\alpha_{g} := \sup_{\forall q \in \mathcal{Q}^{p}} \| H(q) \|_{K}. \quad (7.20)$$

这个上界 $\alpha_{g} < \infty$ 的存在性由性质2.8保证，并且它对重力势能意味着以下陈述。

性质 7.1. 设 $\alpha_{g}$ 如（7.20）所定义，是重力势能 $V_{g}(q)$ 的Hessian矩阵关于 $K$ 范数的上界。则不等式：

$$| V_{g}(q_{1}) - V_{g}(q_{2}) + g(q_{1})^{T}(q_{2} - q_{1}) | \leq \frac{1}{2} \alpha_{g} || q_{2} - q_{1} ||_{K}^{2} \quad (7.21)$$

对所有 $q_{1}, q_{2} \in \mathcal{Q}^{p}$ 成立。

证明. 点 $q_{2}$ 处的重力势能 $V_{g}(q)$ 可以写成从 $q_{1}$ 开始的微分 $g(q)$ 的积分：

$$\begin{array}{l} V_{g}(q_{2}) = V_{g}(q_{1}) + \int_{q_{1}}^{q_{2}} \frac{\partial V_{g}(q)}{\partial q} \mathrm{d}q \\ = V_{g}(q_{1}) + \int_{q_{1}}^{q_{2}} g(q)^{T} \mathrm{d}q. \end{array} \quad (7.22)$$

类似地，微分 $g(q)$ 可以写成：

$$g(q) = g(q_{1}) + \int_{q_{1}}^{q} H(\xi) \mathrm{d}\xi, \quad (7.23)$$

其中 $H(\xi)$ 是Hessian矩阵 $H(\xi) := (\frac{\partial g(q)}{\partial q})_{q = \xi} = (\frac{\partial^{2} V_{g}(q)}{\partial q^{2}})_{q = \xi}$ 。结合（7.22）和（7.23）得到：

$$V_{g}(q_{2}) = V_{g}(q_{1}) + \int_{q_{1}}^{q_{2}} \left( g(q_{1}) + \int_{q_{1}}^{q} H(\xi) \mathrm{d}\xi \right)^{T} \mathrm{d}q, \quad (7.23)$$

由此可以得出：

$$V_{g}(q_{2}) - V_{g}(q_{1}) - g(q_{1})^{T}(q_{2} - q_{1}) = \int_{q_{1}}^{q_{2}} \left( \int_{q_{1}}^{q} H(\xi) \mathrm{d}\xi \right)^{T} \mathrm{d}q.$$

通过使用变换 $z = R q$ 和 $z_{\xi} = R \xi$ ，右边的积分可以写成：

$$\int_{q_{1}}^{q_{2}} \left( \int_{q_{1}}^{q} H(\xi) \mathrm{d}\xi \right)^{T} \mathrm{d}q = \int_{z_{1}}^{z_{2}} \left( \int_{z_{1}}^{z} H(R^{-1} z_{\xi}) R^{-1} \mathrm{d}z_{\xi} \right)^{T} R^{-1} \mathrm{d}z$$ $$= \int_{z_{1}}^{z_{2}} \left( \int_{z_{1}}^{z} R^{-T} H(R^{-1} z_{\xi}) R^{-1} \mathrm{d}z_{\xi} \right)^{T} \mathrm{d}z.$$

利用引理A.23，可以得出结论：

$$\begin{array}{rl} & | V_{g}(q_{2}) - V_{g}(q_{1}) - g(q_{1})^{T}(q_{2} - q_{1}) | \leq \frac{1}{2} \sup_{\forall q \in \mathbb{R}^{n}} || R^{-T} H(q) R^{-1} ||_{i2} || z_{2} - z_{1} ||_{2}^{2} \\ & = \frac{1}{2} \alpha_{g} || q_{2} - q_{1} ||_{K}^{2}. \end{array} \quad (7.24)$$

性质7.1是性质2.8的直接结果，并将在后面的稳定性分析中引起关注。此外，还需要对重力势能做一个进一步的假设。

假设 7.2. 重力势能 $V_{g}(q)$ 的Hessian矩阵 $H(q) = \frac{\partial^{2} V_{g}(q)}{\partial q^{2}}$ 满足条件：

$$\alpha_{g} = \sup_{\forall q \in \mathcal{Q}^{p}} || H(q) ||_{K} < || K ||_{K} = 1. \quad (7.24)$$

注意，这个假设完全没有限制性。直观地说，它无非说明了操纵器应该被正确设计。特别是，这意味着关节刚度足够高，以防止操纵器在自身重量下掉落。对于真实的操纵器， $|| H(q) ||_{K} < 1$ 成立的范围将远大于 $\mathcal{Q}^{p}$ 。

还应该提到，量 $\alpha_{g}$ 是无量纲的，因为它是通过范数 $|| \cdot ||_{K}$ 定义的。

7.3.2 重力补偿项的构建

下面，为重力项 $g(q)$ 的静态效应构造一个补偿。该补偿仅基于电机位置，并且可以（以准静态方式）补偿连杆侧重力扭矩。首先考虑集合 $\Omega := \{(q,\theta) \mid K(\theta - q) = g(q)\}$ （对于 $\tau_{ext} = 0$ ）的静态点，其中关节弹性产生的扭矩平衡了连杆侧重力扭矩。现在，重力补偿的目标是构造一个补偿项 $\bar{g}(\theta)$ ，使得在 $\Omega$ 中，平衡条件：

$$\bar{g}(\theta) = g(q) \quad \forall (q,\theta) \in \Omega \quad (7.25)$$

成立。这意味着重力补偿项在所有静态点都平衡了连杆侧重力扭矩。

注意，对于任何点 $(q_{0}, \theta_{0}) \in \Omega$ ，电机位置可以唯一地表示为连杆侧位置的函数：

$$\theta_{0} = \mathbf{q}_{0} + \mathbf{K}^{-1} \mathbf{g}(\mathbf{q}_{0}) =: \mathbf{h}_{g}(\mathbf{q}_{0}). \quad (7.26)$$

此外，通过使用压缩映射定理（更多细节见命题7.3的证明），可以证明 $h_{g}(q_{0})$ 的逆函数存在。则：

$$\mathbf{q}_{0} = \mathbf{h}_{g}^{-1}(\boldsymbol{\theta}_{0}) =: \bar{\mathbf{q}}(\boldsymbol{\theta}_{0}) \quad (7.27)$$

可以用于构造如下形式的重力补偿项：

$$\mathbf{u}_{g} = \bar{\mathbf{g}}(\boldsymbol{\theta}) := \mathbf{g}(\bar{\mathbf{q}}(\boldsymbol{\theta})), \quad (7.28)$$

这显然满足（7.25）。最后，关于函数 $\bar{q}(\theta)$ 的存在性问题由以下命题回答。

命题 7.3. 如果（7.24）中的假设7.2全局成立（即对于 $\mathcal{Q}^{p} = \mathbb{R}^{n}$ ），则函数 $\mathbf{h}_{g}^{-1}(\boldsymbol{\theta}) = \bar{\mathbf{q}}(\boldsymbol{\theta})$ （即 $\mathbf{h}_{g}(\mathbf{q}) = \mathbf{q} + \mathbf{K}^{-1} \mathbf{g}(\mathbf{q}): \mathbb{R}^{n} \to \mathbb{R}^{n}$ 的逆）全局存在。此外，迭代：

$$\hat{\mathbf{q}}_{n + 1} = \mathbf{T}_{g}(\hat{\mathbf{q}}_{n}) \quad (7.29)$$

其中 $\mathbf{T}_{g}(\mathbf{q}) := \boldsymbol{\theta} - \mathbf{K}^{-1} \mathbf{g}(\mathbf{q})$ ，对于每个固定的 $\theta$ 和每个起始点 $\hat{\mathbf{q}}_{0}$ 收敛到 $\bar{\mathbf{q}}(\boldsymbol{\theta})$ 。

证明. 该命题可以通过首先证明映射 $\mathbf{T}_{g}(\mathbf{q}): \mathbb{R}^{n} \to \mathbb{R}^{n}$ 对于向量范数 $|| \cdot ||_{K}$ 是全局压缩的来证明（见Vid93）。由于向量空间 $\mathbb{R}^{n}$ 连同范数 $|| \cdot ||_{K}$ 无疑是一个Banach空间，只需证明存在一个 $\rho < 1$ 使得 $\mathbf{T}_{g}(\mathbf{q})$ 满足条件：

$$\| \mathbf{T}_{g}(\mathbf{q}_{2}) - \mathbf{T}_{g}(\mathbf{q}_{1}) \|_{K} \leq \rho \| \mathbf{q}_{2} - \mathbf{q}_{1} \|_{K} \quad \forall \mathbf{q}_{1}, \mathbf{q}_{2} \in \mathbb{R}^{n}.$$

函数 $\mathbf{T}_{g}(\mathbf{q})$ 在 $\mathbf{q}_{2}$ 处可以写成积分：

$$\begin{array}{l} T_{g}(q_{2}) = T_{g}(q_{1}) + \int_{q_{1}}^{q_{2}} \frac{\partial T_{g}(q)}{\partial q} \mathrm{d}q \\ = T_{g}(q_{1}) - \int_{q_{1}}^{q_{2}} K^{-1} H(q) \mathrm{d}q, \end{array} \quad (10)$$

由此可以得出结论（通过使用变换 $z = R q$ 和引理A.23）：

$$T_{g}(q_{2}) - T_{g}(q_{1}) = -\int_{q_{1}}^{q_{2}} K^{-1} H(q) \mathrm{d}q$$ $$R(T_{g}(q_{2}) - T_{g}(q_{1})) = -\int_{q_{1}}^{q_{2}} R K^{-1} H(q) \mathrm{d}q$$ $$R(T_{g}(q_{2}) - T_{g}(q_{1})) = -\int_{z_{1}}^{z_{2}} R^{-T} H(R^{-1} z) R^{-1} \mathrm{d}z$$ $$\| R(T_{g}(q_{2}) - T_{g}(q_{1})) \|_{2} \leq \sup_{\forall q \in \mathbb{R}^{n}} \| R^{-T} H(q) R^{-1} \|_{i2} \| z_{2} - z_{1} \|_{2}$$ $$\| T_{g}(q_{2}) - T_{g}(q_{1}) \|_{K} \leq \sup_{\forall q \in \mathbb{R}^{n}} \| H(q) \|_{K} \| q_{2} - q_{1} \|_{K}.$$

从假设7.2可以得出， $T_{g}(q)$ 是一个（全局）压缩。根据压缩映射定理（例如，参见Vid93），可以得出结论，映射 $T_{g}(q)$ 有一个唯一的不动点 $q^{*} = T_{g}(q^{*})$ ，并且（7.29）的迭代收敛到这个不动点：

$$\lim_{n \to \infty} \hat{q}_{n} = q^{*}.$$

通过比较 $T_{g}(q)$ 和 $h_{g}(q)$ ，可以很容易地看出（对于每个特定的 $\theta$ 值），这个不动点 $q^{*}$ 对应于 $\bar{q}(\theta)$ 。

虽然在实践中通常无法直接计算逆函数 $h_{g}^{-1}(\theta)$ ，但可以通过迭代以任意精度近似它。从实际的角度来看，一两次迭代步长已经可以产生相当令人满意的结果。注意，通过以 $\hat{q}_{0} = q_{d}$ 进行一阶近似，将获得ZSL+03中的在线重力补偿项。

因此，在下面的分析中，假设逆函数 $h_{g}^{-1}(\theta)$ 被精确地知道，尽管实际上只能近似它。

关于性质7.3成立的范围的另一个备注是重要的。假设 $\mathcal{Q}^{p} = \mathbb{R}^{n}$ （例如，当机器人只有旋转关节时成立），这是为了确保 $T_{g}(q)$ 是全局压缩所必需的。如果 $\mathcal{Q}^{p} \subset \mathbb{R}^{n}$ ，则必须额外确保迭代（7.29）中的点 $\hat{q}_{i}$ 保持在 $|| H(q) ||_{K} < || K ||_{K} = 1$ 成立的区域内。虽然从实际角度来看这不是一个关键问题，但通常很难一般地证明。

由于 $\bar{g}(\theta)$ 是（静态地）防止机器人因自身重量而下落所需的电机扭矩，可以看出 $\bar{g}(\theta)$ 必须由势能函数 $V_{\bar{g}}(\theta)$ 的微分给出，该势能与机器人的势能（重力加关节刚度）相关。这个势能函数将在第7.4节的无源性和稳定性分析中引起关注。因此，下一节将详细推导 $V_{\bar{g}}(\theta)$ 。

7.3.3 重力补偿势能的推导

在本节中，推导重力补偿项 $\bar{g}(\theta)$ 的势能函数 $V_{\bar{g}}(\theta)$ ，使得 $\bar{g}(\theta) = (\partial V_{\bar{g}}(\theta) / \partial \theta)^{T}$ 成立。记住，在上一节中为了构造 $\bar{g}(\theta) = g(\bar{q}(\theta))$ ，使用了函数 $\bar{q}(\theta) = h_{g}^{-1}(\theta)$ ，即 $h_{g}(q) = q + K^{-1} g(q)$ 的逆。 $h_{g}^{-1}(\theta)$ 的存在性和唯一性通过使用假设7.2在命题7.1中确定。

在下面的推导中，将需要雅可比矩阵 $\partial \bar{q}(\theta) / \partial \theta$ 。首先考虑函数 $h_{g}(q)$ 的雅可比矩阵：

$$\frac{\partial h_{g}(q)}{\partial q} = \left( I + K^{-1} \frac{\partial g(q)}{\partial q} \right). \quad (7.30)$$

由于 $h_{g}(\bar{q}(\theta)) = \theta$ ，有：

$$\frac{\partial h_{g}(\bar{q}(\theta))}{\partial \theta} = \frac{\partial h_{g}(\bar{q})}{\partial \bar{q}} \frac{\partial \bar{q}(\theta)}{\partial \theta} = I,$$

因此雅可比矩阵 $\frac{\partial \bar{q}(\theta)}{\partial \theta}$ 由下式给出：

$$\frac{\partial \bar{q}(\theta)}{\partial \theta} = \left( I + K^{-1} \frac{\partial g(\bar{q})}{\partial \bar{q}} \right)_{q = \bar{q}(\theta)}^{-1}. \quad (7.31)$$

势能函数 $V_{\bar{g}}(\theta)$ 显然可以写成以下形式：

$$V_{\bar{g}}(\theta) = V_{\bar{g}}(h_{g}(\bar{q}(\theta))) =: V_{\bar{g}h}(\bar{q}(\theta)).$$

对于微分 $\partial V_{\bar{g}}(\theta) / \partial \theta$ ，得到：

$$\frac{\partial V_{\bar{g}}(\theta)}{\partial \theta} = \left( \frac{\partial V_{\bar{g}h}(\bar{q})}{\partial \bar{q}} \right)_{q = \bar{q}(\theta)} \frac{\partial \bar{q}(\theta)}{\partial \theta}.$$

通过代入 $\frac{\partial V_{\bar{g}}(\theta)}{\partial \theta} = \bar{g}(\theta) = g(\bar{q}(\theta))^{T}$ 和来自（7.31）的 $\frac{\partial \bar{q}(\theta)}{\partial \theta}$ ，得到：

$$\begin{array}{l} \frac{\partial V_{\bar{g}h}(\bar{q})}{\partial \bar{q}} = g(\bar{q})^{T} \left( I + K^{-1} \frac{\partial g(\bar{q})}{\partial \bar{q}} \right), \\ = g(\bar{q})^{T} + g(\bar{q})^{T} K^{-1} \frac{\partial g(\bar{q})}{\partial \bar{q}}. \end{array} \quad (7.32)$$

然后可以将这个微分积分到 $V_{\bar{g}h}(\bar{q}) = V_{g}(\bar{q}) + \frac{1}{2} g(\bar{q})^{T} K^{-1} g(\bar{q}) + c$ ，其中 $c \in \mathbb{R}^{n}$ 是任意常数。令 $c = 0$ ，得到重力补偿势能：

$$V_{\bar{g}}(\theta) = V_{\bar{g}h}(\bar{q}(\theta)) = V_{g}(\bar{q}(\theta)) + \frac{1}{2} g(\bar{q}(\theta))^{T} K^{-1} g(\bar{q}(\theta)). \quad (7.32)$$

还要注意，来自（2.24）的操纵器势能 $V_{pot}(\mathbf{q},\boldsymbol{\theta})$ 对于所有静态点都与重力补偿势能相同，即：

$$V_{pot}(\mathbf{q},\boldsymbol{\theta}) = V_{\bar{g}}(\boldsymbol{\theta}) \quad \forall (\mathbf{q},\boldsymbol{\theta}) \in \Omega. \quad (7.33)$$

由此可以得出， $V_{\bar{g}}(\boldsymbol{\theta})$ 也可以写成：

$$V_{\bar{g}}(\boldsymbol{\theta}) = V_{pot}(\bar{\mathbf{q}}(\boldsymbol{\theta}), \boldsymbol{\theta}) = V_{\bar{g}}(\bar{\mathbf{q}}(\boldsymbol{\theta})) + V_{k}(\bar{\mathbf{q}}(\boldsymbol{\theta}), \boldsymbol{\theta}). \quad (7.34)$$

7.4 分析

带有重力补偿的完整控制律由下式给出：

$$\begin{array}{rl} & \tau_{m} = B B_{\theta}^{-1} \mathbf{u} + (I - B B_{\theta}^{-1}) \boldsymbol{\tau}, \\ & \mathbf{u} = \mathbf{u}_{imp} + \mathbf{u}_{g} = -J(\boldsymbol{\theta})^{T} (K_{d} \tilde{\mathbf{x}}_{\theta} + D_{d} \dot{\tilde{\mathbf{x}}}_{\theta}) + \bar{\mathbf{g}}(\boldsymbol{\theta}), \end{array} \quad (7.35)$$

并导致以下闭环系统方程：

$$\begin{array}{r} M(\mathbf{q})\ddot{\mathbf{q}} + C(\mathbf{q},\dot{\mathbf{q}})\dot{\mathbf{q}} + \mathbf{g}(\mathbf{q}) = \boldsymbol{\tau} + \boldsymbol{\tau}_{ext}, \\ B_{\theta} \ddot{\boldsymbol{\theta}} + \mathbf{J}(\boldsymbol{\theta})^{T} (K_{d} \tilde{\mathbf{x}}_{\theta} + D_{d} \dot{\tilde{\mathbf{x}}}_{\theta}) + \boldsymbol{\tau} = \bar{\mathbf{g}}(\boldsymbol{\theta}). \end{array} \quad (7.38)$$

在本节中，将首先证明在重力势能 $V_{g}(\mathbf{q})$ 全局有界的情况下，闭环系统可以写成两个无源子系统的反馈互连。注意，这意味着整个系统的无源性vdS00。此外，即使在没有重力势能有界假设的情况下，也在第7.4.2节中给出了渐近稳定性的证明。

7.4.1 无源性

对于本节的无源性分析，将假设存在一个实数 $\beta > 0$ ，使得：

$$| V_{g}(\mathbf{q}) | < \beta \qquad \forall \mathbf{q} \in \mathbb{R}^{n} \quad (7.39)$$

成立。例如，对于所有只有旋转关节（即没有移动关节）的机器人，这都满足。由于性质2.8，重力扭矩向量 $g(\mathbf{q})$ 也将是全局有界的。此外，（7.39）还意味着 $V_{\bar{g}}(\boldsymbol{\theta})$ 和 $\bar{g}(\boldsymbol{\theta})$ 的有界性。注意，有界重力势能的要求仅对无源性分析是必需的，而第7.4.2节中渐近稳定性的证明也适用于一般势能。

根据定义A.17，系统（具有输入 $\mathbf{u}$ 和输出 $\mathbf{y}$ ）无源的充分条件是存在一个连续存储函数 $S$ [vdS00, Kug01]，它从下方有界，并且其沿系统解对时间的导数满足不等式 $\dot{S} \leq \dot{y}^{T} u$ 。

下面将证明，如图7.3所示，系统（7.37）-（7.38）由两个处于反馈互连的无源子系统组成。通常假设机器人的环境也可以由无源映射 $(\dot{q} \rightarrow - \tau_{ext})$ 描述。（7.37）作为映射 $(\tau + \tau_{ext}) \rightarrow \dot{q}$ 的无源性由于纯粹的物理原因而众所周知，并且可以通过存储函数：

$$S_{q}(\mathbf{q}, \dot{\mathbf{q}}) = \frac{1}{2} \dot{\mathbf{q}}^{T} \mathbf{M}(\mathbf{q}) \dot{\mathbf{q}} + V_{g}(\mathbf{q}) \quad (7.40)$$

来证明，其沿（7.37）解的导数由下式给出：

$$\dot{S}_{q}(\mathbf{q}, \dot{\mathbf{q}}) = \dot{q}^{T} (\boldsymbol{\tau} + \boldsymbol{\tau}_{ext}). \quad (7.41)$$

类似地，（7.38）作为映射 $\dot{q} \rightarrow - \tau$ 的无源性可以通过存储函数：

$$\begin{array}{l} S_{\theta}(\mathbf{q}, \boldsymbol{\theta}, \dot{\boldsymbol{\theta}}) = \frac{1}{2} \dot{\boldsymbol{\theta}}^{T} \mathbf{B}_{\theta} \dot{\boldsymbol{\theta}} + \frac{1}{2} (\boldsymbol{\theta} - \mathbf{q})^{T} \mathbf{K} (\boldsymbol{\theta} - \mathbf{q}) \\ + \frac{1}{2} \dot{\mathbf{x}}_{\theta}^{T} \mathbf{K}_{d} \dot{\mathbf{x}}_{\theta} - V_{\bar{\theta}}(\boldsymbol{\theta}) \end{array} \quad (7.42)$$

来证明。 $S_{\theta}(\mathbf{q}, \boldsymbol{\theta}, \dot{\boldsymbol{\theta}})$ 沿（7.38）解的导数则由下式给出：

$$\dot{S}_{\theta}(\mathbf{q}, \boldsymbol{\theta}, \dot{\boldsymbol{\theta}}) = -\dot{x}_{d}^{T} \mathbf{D}_{d} \dot{x}_{\theta} - \dot{q}^{T} \boldsymbol{\tau}. \quad (7.43)$$

期望的无源性性质直接从（7.41）和（7.43）以及无源系统的反馈互连仍然是无源的事实得出。还应该提到，如果（7.36）中的PD控制器被任何其他（相对于 $\dot{\boldsymbol{\theta}} \rightarrow - \mathbf{u}$ ）无源控制器替换，这些无源性性质仍然有效。注意，如图7.3所示的闭环系统结构（形式为无源子系统的反馈互连）带来了非常有利的鲁棒性性质。

7.4.2 稳定性

以下稳定性分析仅限于非冗余情况（ $m = n$ ）。此外，必须避免奇异配置。因此，进一步的分析被限制在工作空间中的一个区域，在该区域中雅可比矩阵 $J(\theta)$ 非奇异，并且 $\mathbf{x}_{\theta} = \mathbf{f}(\boldsymbol{\theta})$ 的逆映射可以唯一求解。

下面将证明闭环系统在自由运动情况下（即 $\boldsymbol{\tau}_{ext} = \mathbf{0}$ ）是渐近稳定的。

稳态的确定

系统（7.37）-（7.38）的稳态条件由下式给出：

$$\begin{array}{r} K(\boldsymbol{\theta}_{0} - \mathbf{q}_{0}) = \mathbf{g}(\mathbf{q}_{0}), \\ K(\boldsymbol{\theta}_{0} - \mathbf{q}_{0}) + \mathbf{J}(\boldsymbol{\theta}_{0})^{T} \mathbf{K}_{d} (\mathbf{f}(\boldsymbol{\theta}_{0}) - \mathbf{x}_{\theta,d}) = \bar{\mathbf{g}}(\boldsymbol{\theta}_{0}). \end{array} \quad (7.44)$$

这里矩阵 $\mathbf{K}_{d}$ 是正定的。由于（7.25），可以得出：

$$\mathbf{J}(\boldsymbol{\theta}_{0})^{T} \mathbf{K}_{d} (\mathbf{f}(\boldsymbol{\theta}_{0}) - \mathbf{x}_{\theta,d}) = \mathbf{0} \quad (7.46)$$

必须在稳态下满足。如上所述，稳定性分析被限制在该条件可以唯一求解 $\boldsymbol{\theta}_{0}$ 的区域。稳态则由下式给出：

$$\begin{array}{l} \boldsymbol{\theta}_{0} = \mathbf{f}^{-1}(\mathbf{x}_{\theta,d}), \\ \mathbf{q}_{0} = \mathbf{h}_{g}^{-1}(\boldsymbol{\theta}_{0}), \\ \dot{\mathbf{q}}_{0} = \mathbf{0} \\ \dot{\boldsymbol{\theta}}_{0} = \mathbf{0}, \end{array} \quad (7.49)$$

并且 $\boldsymbol{\tau}_{m,0} = \boldsymbol{\tau}_{0} = \bar{\mathbf{g}}(\boldsymbol{\theta}_{0})$ 是稳态输入扭矩。

李雅普诺夫函数

考虑以下函数 $V(\mathbf{q}, \dot{\mathbf{q}}, \boldsymbol{\theta}, \dot{\boldsymbol{\theta}})$ 作为李雅普诺夫函数候选：

$$V(\mathbf{q}, \dot{\mathbf{q}}, \boldsymbol{\theta}, \dot{\boldsymbol{\theta}}) = S_{q}(\mathbf{q}, \dot{\mathbf{q}}) + S_{\theta}(\mathbf{q}, \boldsymbol{\theta}, \dot{\boldsymbol{\theta}}). \quad (7.51)$$

在稳态下，有以下关系成立（由于（7.34））：

$$V(\mathbf{q}_{0}, \mathbf{0}, \boldsymbol{\theta}_{0}, \mathbf{0}) = V_{g}(\mathbf{q}_{0}) - V_{\bar{g}}(\boldsymbol{\theta}_{0}) + \frac{1}{2} (\boldsymbol{\theta}_{0} - \mathbf{q}_{0})^{T} \mathbf{K} (\boldsymbol{\theta}_{0} - \mathbf{q}_{0}) = 0.$$

$V(\mathbf{q}, \dot{\mathbf{q}}, \boldsymbol{\theta}, \dot{\boldsymbol{\theta}})$的动能部分：

$$V_{kin}(\mathbf{q}, \dot{\mathbf{q}}, \dot{\boldsymbol{\theta}}) = \frac{1}{2} \dot{\mathbf{q}}^{T} \mathbf{M}(\mathbf{q}) \dot{\mathbf{q}} + \frac{1}{2} \dot{\boldsymbol{\theta}}^{T} \mathbf{B}_{\theta} \dot{\boldsymbol{\theta}}$$

关于 $\dot{\mathbf{q}}$ 和 $\dot{\boldsymbol{\theta}}$ 是正定的，因为惯性矩阵是正定的。为了证明 $V(\mathbf{q}, \dot{\mathbf{q}}, \boldsymbol{\theta}, \dot{\boldsymbol{\theta}})$ 是正定的，只需证明势能部分：

$$V_{pot}(\mathbf{q}, \boldsymbol{\theta}) = V(\mathbf{q}, \dot{\mathbf{q}}, \boldsymbol{\theta}, \dot{\boldsymbol{\theta}}) - V_{kin}(\mathbf{q}, \dot{\mathbf{q}}, \dot{\boldsymbol{\theta}}) \quad (7.52)$$

关于 $\mathbf{q}$ 和 $\boldsymbol{\theta}$ 是正定的。

首先只考虑由 $\mathbf{K}$ 引起的势能部分。在本节的剩余部分，为了简化符号，将 $\bar{\mathbf{q}}$ 写为 $\bar{\mathbf{q}}(\boldsymbol{\theta})$ 。

$$\begin{array}{l} V_{k}(\mathbf{q}, \boldsymbol{\theta}) = \frac{1}{2} (\boldsymbol{\theta} - \mathbf{q})^{T} K(\boldsymbol{\theta} - \mathbf{q}) \\ = \frac{1}{2} (\boldsymbol{\theta} - \bar{\mathbf{q}} + \bar{\mathbf{q}} - \mathbf{q})^{T} K(\boldsymbol{\theta} - \bar{\mathbf{q}} + \bar{\mathbf{q}} - \mathbf{q}) \\ = \frac{1}{2} g(\bar{\mathbf{q}})^{T} K^{-1} g(\bar{\mathbf{q}}) + \frac{1}{2} (\bar{\mathbf{q}} - \mathbf{q})^{T} K(\bar{\mathbf{q}} - \mathbf{q}) + (\bar{\mathbf{q}} - \mathbf{q})^{T} g(\bar{\mathbf{q}}) \end{array} \quad (7.25)$$

这里使用了关系 $K(\boldsymbol{\theta} - \bar{\mathbf{q}}) = \mathbf{g}(\bar{\mathbf{q}})$ ，它（与（7.25）相反）处处成立（即不仅在 $\Omega$ 中）。然后，势能可以（利用（7.34））写成如下：

$$\begin{array}{l} V_{pot}(\mathbf{q}, \boldsymbol{\theta}) = V_{k}(\mathbf{q}, \boldsymbol{\theta}) + \frac{1}{2} \tilde{\mathbf{x}}_{\theta}^{T} \mathbf{K}_{d} \tilde{\mathbf{x}}_{\theta} + V_{g}(\mathbf{q}) - V_{\bar{g}}(\boldsymbol{\theta}) \\ = V_{k}(\mathbf{q}, \boldsymbol{\theta}) + \frac{1}{2} \tilde{\mathbf{x}}_{\theta}^{T} \mathbf{K}_{d} \tilde{\mathbf{x}}_{\theta} + V_{g}(\mathbf{q}) - V_{g}(\bar{\mathbf{q}}) - \frac{1}{2} \mathbf{g}(\bar{\mathbf{q}})^{T} \mathbf{K}^{-1} \mathbf{g}(\bar{\mathbf{q}}) \end{array} \quad (7.34)$$

由于性质7.1，以下不等式成立：

$$\begin{array}{rl} & V_{pot}(\mathbf{q}, \boldsymbol{\theta}) \geq \frac{1}{2} (\bar{\mathbf{q}} - \mathbf{q})^{T} \mathbf{K} (\bar{\mathbf{q}} - \mathbf{q}) + \frac{1}{2} \tilde{\mathbf{x}}_{\theta}^{T} \mathbf{K}_{d} \tilde{\mathbf{x}}_{\theta} \\ & - | V_{g}(\mathbf{q}) - V_{g}(\bar{\mathbf{q}}) + (\bar{\mathbf{q}} - \mathbf{q})^{T} \mathbf{g}(\bar{\mathbf{q}}) | \\ & \geq \frac{1}{2} (\bar{\mathbf{q}} - \mathbf{q})^{T} \mathbf{K} (\bar{\mathbf{q}} - \mathbf{q}) - \frac{1}{2} \alpha_{g} || \bar{\mathbf{q}} - \mathbf{q} ||_{K}^{2} + \frac{1}{2} \tilde{\mathbf{x}}_{\theta}^{T} \mathbf{K}_{d} \tilde{\mathbf{x}}_{\theta} \\ & \geq \frac{1}{2} (1 - \alpha_{g}) || \bar{\mathbf{q}} - \mathbf{q} ||_{K}^{2} + \frac{1}{2} \tilde{\mathbf{x}}_{\theta}^{T} \mathbf{K}_{d} \tilde{\mathbf{x}}_{\theta}. \end{array} \quad (7.34)$$

由于假设7.2中 $\alpha_{g} < 1$ ，最后一个不等式的右边对于所有 $(\mathbf{q}, \boldsymbol{\theta})$ 都是非负的。项 $\tilde{\mathbf{x}}_{\theta}^{T} \mathbf{K}_{d} \tilde{\mathbf{x}}_{\theta}$ （在 $\boldsymbol{\theta}$ 中）正定的区域最终决定了李雅普诺夫函数正定的区域。对于一般的正向运动学映射，因此只能做出局部陈述。

李雅普诺夫函数的导数

$V(\mathbf{q}, \dot{\mathbf{q}}, \boldsymbol{\theta}, \dot{\boldsymbol{\theta}})$沿系统（7.37）-（7.38）（对于 $\tau_{ext} = 0$ ）解的导数由下式给出：

$$\dot{V}(\mathbf{q}, \dot{\mathbf{q}}, \boldsymbol{\theta}, \dot{\boldsymbol{\theta}}) = \dot{S}_{q}(\mathbf{q}, \dot{\mathbf{q}}) + \dot{S}_{\theta}(\mathbf{q}, \boldsymbol{\theta}, \dot{\boldsymbol{\theta}}) = -\dot{\mathbf{x}}_{d}^{T} \mathbf{D}_{d} \dot{\mathbf{x}}_{\theta}. \quad (7.53)$$

由于矩阵 $\mathbf{D}_{d}$ 是正定的，可以得出结论，平衡点（7.47）-（7.50）是稳定的。此外，可以通过使用拉萨尔不变性原理来证明渐近稳定性。根据该原理，系统状态将收敛到 $\dot{\mathbf{x}}_{\theta} = \mathbf{0}$ 成立的的最大正不变量。从系统方程可以得出，除了限制到平衡点之外，不存在 $\dot{\mathbf{x}}_{\theta} = \mathbf{0}$ 成立的轨迹。因此，可以得出渐近稳定性的结论。

7.4.3 控制器讨论

第7.4.1节的无源性分析表明，闭环系统可以看作是无源子系统的反馈互连。在许多应用中，环境也可以被视为关于输入 $\dot{\mathbf{q}}$ 和输出 $- \boldsymbol{\tau}_{ext}$ 的无源系统。因此，可以得出整个系统非常有利的鲁棒性性质。例如，对于质量矩阵 $\mathbf{M}(\mathbf{q})$ 和 $\mathbf{B}$ 的动态参数中的任意误差，稳定性都有保证。

关于重力补偿项的表述，应该提到的是，与任何相关的先前工作相比，稳定性对正定矩阵 $\mathbf{K}_{d}$ 没有施加任何限制，这意味着刚度可以任意地接近于零。

注意，通过同样的论证，原则上也可以在冗余情况（ $m < n$ ）下证明笛卡尔误差 $\tilde{\mathbf{x}} \rightarrow \mathbf{0}$ 的收敛性（只要避免奇异配置）。然而，当然还需要一个零空间阻尼项来确保 $\dot{\boldsymbol{\theta}} \rightarrow \mathbf{0}$ （例如，参见Kha87）。

还应该提到，对于关节空间阻抗控制器，稳定性分析将得出全局性结论。而笛卡尔阻抗控制器是在局部笛卡尔坐标中定义阻抗的。这就是为什么对于笛卡尔控制器只能证明局部稳定性的原因。在旋转关节的情况下，任何描述末端执行器位置和方向的笛卡尔坐标集在 $\boldsymbol{\theta}$ 中必然是周期的。此外，众所周知，由于拓扑原因，不可能在 $SE(3)$ 中设计一个只有单个全局极值的势能函数。

7.5 精确刚度设计

在前面的章节中，阻抗控制器基本上被设计为关节刚度 $\mathbf{K}$ 和期望刚度的串联互连。下面，将展示如何在设计 $\mathbf{K}_{\theta}$ （或 $\mathbf{K}_{d}$ ）时考虑关节刚度，以实现关节级阻抗和笛卡尔阻抗情况下的特定外部刚度值。

7.5.1 精确关节级刚度的设计

首先考虑关节级阻抗的情况，其刚度为 $\mathbf{K}_{\theta}$ ，参见（7.7）。最终刚度 $\mathbf{K}_{\theta,ext}$ ，即 $\boldsymbol{\tau}_{ext}$ 与 $\mathbf{q} - \mathbf{q}_{d}$ 之间的静态关系，由下式给出：

$$K_{\theta,ext} = \left( K_{\theta}^{-1} + K^{-1} \right)^{-1}.$$

当关节刚度 $\mathbf{K}$ 远大于 $\mathbf{K}_{\theta}$ 时，最终刚度 $\mathbf{K}_{\theta,ext}$ 将接近 $\mathbf{K}_{\theta}$ 。但 $\mathbf{K}_{\theta,ext}$ 与 $\mathbf{K}_{\theta}$ 之间的关系当然也可以明确地用于设计 $\mathbf{K}_{\theta}$ 。为了精确实现 $\mathbf{K}_{\theta,ext}$ 的刚度，（7.7）中的控制器刚度必须设置为：

$$K_{\theta} = \left( K_{\theta,ext}^{-1} - K^{-1} \right)^{-1}.$$

由于 $\mathbf{K}_{\theta}$ 必须是正定的， $\mathbf{K}$ 是（7.7）形式控制器可实现的最大刚度。

对于来自（7.10）的笛卡尔阻抗控制器，同样的事情只能在 $\mathbf{q}_{d}$ 处局部完成。可以很容易地验证，在 $\mathbf{q}_{d}$ 处，（7.10）关于关节坐标的局部刚度由 $\mathbf{J}(\mathbf{q}_{d})^{T} \mathbf{K}_{d} \mathbf{J}(\mathbf{q}_{d})$ 给出。对于期望的外部刚度 $\mathbf{K}_{d,ext}$ （必须选择使得矩阵 $\mathbf{K}_{d,ext}^{-1} - \mathbf{J}(\mathbf{q}_{d}) \mathbf{K}^{-1} \mathbf{J}(\mathbf{q}_{d})^{T}$ 是正定的），矩阵 $\mathbf{K}_{d}$ 读作：

$$K_{d} = \left( K_{d,ext}^{-1} - J(q_{d}) K^{-1} J(q_{d})^{T} \right)^{-1}.$$

但这个简单的设计法则只在局部匹配期望刚度，与关节级情况不同。当应用在一个笛卡尔方向上需要高刚度而在另一个方向上需要低刚度时，这确实在实际中可能相关。为了克服这个问题，下面将制定一个不同于（7.36）的控制器结构。

7.5.2 精确笛卡尔刚度的设计

对于重力补偿项 $\bar{g}(\theta)$ 的推导，只使用了自由（ $\boldsymbol{\tau}_{ext} = \mathbf{0}$ ）稳态条件。在ASOH04a和OASKH08中，展示了如何通过扩展OASK+04b, ASOH04b的设计思路，实现精确的笛卡尔连杆侧刚度以及精确的重力补偿。

考虑一个恒定的广义外力 $\mathbf{F}_{ext}$ 作用在机器人上的情况。在位置 $\mathbf{q}_{0}$ 的稳态下，广义外力 $\mathbf{F}_{ext}$ 通过 $\boldsymbol{\tau}_{ext} = \mathbf{J}(\mathbf{q}_{0})^{T} \mathbf{F}_{ext}$ 与外部扭矩 $\boldsymbol{\tau}_{ext}$ 相关。这种情况下的平衡条件是：

$$\begin{array}{rl} & K(\boldsymbol{\theta}_{0} - \mathbf{q}_{0}) = \mathbf{g}(\mathbf{q}_{0}) - \mathbf{J}(\mathbf{q}_{0})^{T} \mathbf{F}_{ext}, \\ & K(\boldsymbol{\theta}_{0} - \mathbf{q}_{0}) = \mathbf{u}_{0}, \end{array} \quad (7.54)$$

其中 $\mathbf{u}_{0}$ 是 $\mathbf{u}$ 的静态值。下面，期望的刚度关系：

$$K_{d}(\mathbf{x}(\mathbf{q}_{0}) - \mathbf{x}_{d}) = \mathbf{F}_{ext} \quad (7.56)$$

将在静态下实现。通过结合（7.56）和（7.54），得到条件：

$$K(\boldsymbol{\theta}_{0} - \mathbf{q}_{0}) = \mathbf{g}(\mathbf{q}_{0}) - \mathbf{J}(\mathbf{q}_{0})^{T} \mathbf{K}_{d} (\mathbf{x}(\mathbf{q}_{0}) - \mathbf{x}_{d}). \quad (7.57)$$

这个条件可以看作是静态电机侧位置 $\boldsymbol{\theta}_{0}$ 和静态连杆侧位置 $\mathbf{q}_{0}$ 之间的关系。为了强调以下推导与第7.3.2节重力补偿项推导的相似性，定义函数 $\mathbf{l}(\mathbf{q})$ 为：

$$l(\mathbf{q}) := \mathbf{g}(\mathbf{q}) - \mathbf{J}(\mathbf{q})^{T} \mathbf{K}_{d} (\mathbf{x}(\mathbf{q}) - \mathbf{x}_{d}). \quad (7.58)$$

然后，以下过程与第7.3.2节重力补偿项的设计完全类似。函数 $\mathbf{l}(\mathbf{q})$ 现在扮演与第7.3.2节中重力函数 $\mathbf{g}(\mathbf{q})$ 相同的角色。注意，方程（7.57）也可以写成 $\mathbf{K}(\boldsymbol{\theta}_{0} - \mathbf{q}_{0}) = \mathbf{l}(\mathbf{q}_{0})$ 。

通过定义函数：

$$\mathbf{h}_{l}(\mathbf{q}) := \mathbf{q} + \mathbf{K}^{-1} \mathbf{l}(\mathbf{q}), \quad (7.59)$$

静态电机侧位置 $\boldsymbol{\theta}_{0}$ 可以表示为 $\boldsymbol{\theta}_{0} = \mathbf{h}_{l}(\mathbf{q}_{0})$ 。在这一点上，假设 $\mathbf{h}_{l}(\mathbf{q})$ 的逆函数存在，并将其表示为：

$$\bar{q}_{l}(\boldsymbol{\theta}) := \mathbf{h}_{l}^{-1}(\boldsymbol{\theta}). \quad (7.60)$$

这个逆函数存在的充分条件以及迭代计算过程将在后面的命题7.5中给出。通过 $\bar{q}_{l}(\boldsymbol{\theta})$ ，可以设计一个结合重力补偿和静态精确刚度的控制律，形式为：

$$\begin{array}{rl} & \mathbf{u} = \mathbf{l}(\bar{q}_{l}(\boldsymbol{\theta})) - \mathbf{J}(\bar{q}_{l}(\boldsymbol{\theta}))^{T} \mathbf{D}_{d} \mathbf{J}(\bar{q}_{l}(\boldsymbol{\theta})) \dot{\boldsymbol{\theta}} \\ & = \mathbf{g}(\bar{q}_{l}(\boldsymbol{\theta})) - \mathbf{J}(\bar{q}_{l}(\boldsymbol{\theta}))^{T} \left( \mathbf{K}_{d} (\mathbf{x}(\bar{q}_{l}(\boldsymbol{\theta})) - \mathbf{x}_{d}) + \mathbf{D}_{d} \mathbf{J}(\bar{q}_{l}(\boldsymbol{\theta})) \dot{\boldsymbol{\theta}} \right) \end{array} \quad (7.61)$$

如（7.58）所定义的函数 $\mathbf{l}(\mathbf{q})$ 是势能函数的微分：

$$V_{l}(\mathbf{q}) = V_{g}(\mathbf{q}) - \frac{1}{2} (\mathbf{x}(\mathbf{q}) - \mathbf{x}_{d})^{T} \mathbf{K}_{d} (\mathbf{x}(\mathbf{q}) - \mathbf{x}_{d}), \quad (7.62)$$

即 $\mathbf{l}(\mathbf{q}) = (\partial V_{l}(\mathbf{q}) / \partial \mathbf{q})^{T}$ 。类似于假设7.2，现在需要以下假设。

假设 7.4. 势能函数 $V_{l}(\mathbf{q})$ 的Hessian矩阵 $H_{l}(\mathbf{q}) = \frac{\partial^{2} V_{l}(\mathbf{q})}{\partial \mathbf{q}^{2}}$ 满足条件：

$$\alpha_{l} := \sup_{\forall \mathbf{q} \in \mathbb{R}^{n}} || \mathbf{H}_{l}(\mathbf{q}) ||_{K} < || \mathbf{K} ||_{K} = 1. \quad (7.63)$$

注意，这个假设隐含地包含了期望刚度 $K_{d}$ 的上界。这并不奇怪，因为控制器基本上实现了一个与关节刚度 $K$ 串联互连的刚度。因此，组合刚度 $K_{d}$ 必须小于 $K$ 。然而，假设7.4确保了逆函数 $\mathbf{h}_{l}^{-1}(\boldsymbol{\theta})$ 的存在性，如下命题所述，该命题类似于命题7.3。

命题 7.5. 在假设7.4下，函数 $\mathbf{h}_{l}^{-1}(\boldsymbol{\theta}) := \bar{q}_{l}(\boldsymbol{\theta})$ （即 $h_{l}(\mathbf{q}) = \mathbf{q} + \mathbf{K}^{-1} \mathbf{l}(\mathbf{q}): \mathbb{R}^{n} \to \mathbb{R}^{n}$ 的逆）存在。此外，迭代：

$$\hat{\mathbf{q}}_{l,n + 1} = \mathbf{T}_{l}(\hat{\mathbf{q}}_{l,n}) \quad (7.64)$$

其中 $T_{l}(\mathbf{q}) := \boldsymbol{\theta} - \mathbf{K}^{-1} \mathbf{l}(\mathbf{q})$ ，对于每个固定的 $\boldsymbol{\theta}$ 和每个起始点 $\hat{\mathbf{q}}_{l,0}$ 收敛到 $\bar{q}_{l}(\boldsymbol{\theta})$ 。

证明. 当 $g(\mathbf{q})$ 替换为 $l(\mathbf{q})$ 时，该命题可以完全按照命题7.3的方式证明。

此外，通过遵循与第7.3.3节相同的推导（用 $l(\mathbf{q})$ 代替 $g(\mathbf{q})$ ），可以证明控制器项 $l(\bar{q}_{l}(\boldsymbol{\theta}))$ 可以写成势能函数的微分：

$$V_{l}(\boldsymbol{\theta}) = V_{l}(\bar{q}_{l}(\boldsymbol{\theta})) + \frac{1}{2} l(\bar{q}_{l}(\boldsymbol{\theta}))^{T} K^{-1} l(\bar{q}_{l}(\boldsymbol{\theta})), \quad (7.65)$$

即 $l(\bar{q}_{l}(\boldsymbol{\theta})) = (\partial V_{l}(\boldsymbol{\theta}) / \partial \boldsymbol{\theta})^{T}$ 。对于下面的稳定性分析，类似于性质7.1的陈述也很重要。

性质 7.6. 设 $\alpha_{l}$ 如假设7.4所定义，是势能函数 $V_{l}(\mathbf{q})$ 的Hessian矩阵的上界。则不等式：

$$| V_{l}(\mathbf{q}_{1}) - V_{l}(\mathbf{q}_{2}) + l(\mathbf{q}_{1})^{T}(\mathbf{q}_{2} - \mathbf{q}_{1}) | \leq \frac{1}{2} \alpha_{l} || \mathbf{q}_{2} - \mathbf{q}_{1} ||_{K}^{2} \quad (7.66)$$

对所有 $\mathbf{q}_{1}, \mathbf{q}_{2} \in \mathbb{R}^{n}$ 成立。

证明. 这个性质可以完全按照性质7.1的方式证明。

本节控制器的无源性和稳定性陈述也可以以类似的方式证明。下面只简要概述稳定性证明。使用（7.61），闭环系统由下式给出：

$$\begin{array}{r} M(\mathbf{q})\ddot{\mathbf{q}} + C(\mathbf{q},\dot{\mathbf{q}})\dot{\mathbf{q}} + g(\mathbf{q}) = K(\boldsymbol{\theta} - \mathbf{q}) + \boldsymbol{\tau}_{ext} \\ B_{\theta} \ddot{\boldsymbol{\theta}} + K(\boldsymbol{\theta} - \mathbf{q}) = l(\bar{q}_{l}(\boldsymbol{\theta})) - J(\bar{q}_{l}(\boldsymbol{\theta}))^{T} D_{d} J(\bar{q}_{l}(\boldsymbol{\theta})) \dot{\boldsymbol{\theta}}. \end{array} \quad (7.68)$$

自由运动情况下的稳定性（ $\tau_{ext} = 0$ ）可以通过考虑李雅普诺夫函数候选来证明：

$$V_{e}(q, \dot{q}, \theta, \dot{\theta}) = \frac{1}{2} \dot{q}^{T} M(q) \dot{q} + \frac{1}{2} \dot{\theta}^{T} B_{\theta} \dot{\theta} + V_{g}(q) + V_{k}(q, \theta) - V_{l}(\theta). \quad (7.69)$$

为了证明该函数是正定的，可以将：

$$V_{g}(q) = V_{l}(q) + \frac{1}{2} (x(q) - x_{d})^{T} K_{d} (x(q) - x_{d})$$

从（7.62）和：

$$V_{k}(q, \theta) = \frac{1}{2} (\theta - q)^{T} K(\theta - q)$$ $$= \frac{1}{2} (\bar{q}_{l}(\theta) - q)^{T} K(\bar{q}_{l}(\theta) - q) + \frac{1}{2} l(\bar{q}_{l}(\theta))^{T} K^{-1} l(\bar{q}_{l}(\theta)) + l(\bar{q}_{l}(\theta))^{T} (\bar{q}_{l} - q),$$

（这来自 $l(q)$ 的定义）代入 $V_{e}(q, \dot{q}, \theta, \dot{\theta})$ 的表达式。结合来自（7.65）的 $V_{l}(\theta)$ 的表达式，并使用性质7.6，可以证明这个李雅普诺夫函数满足不等式：

$$V_{e}(q, \dot{q}, \theta, \dot{\theta}) \geq \frac{1}{2} \dot{q}^{T} M(q) \dot{q} + \frac{1}{2} \dot{\theta}^{T} B_{\theta} \dot{\theta} + \frac{1}{2} (1 - \alpha_{l}) || q - \bar{q}_{l}(\theta) ||_{K}^{2} + \frac{1}{2} (x(q) - x_{d})^{T} K_{d} (x(q) - x_{d})$$

由此可以得出 $V_{e}(q, \dot{q}, \theta, \dot{\theta})$ 是正定的。它沿（7.67）-（7.68）解曲线的时间导数可以计算为：

$$\dot{V}_{e}(q, \dot{q}, \theta, \dot{\theta}) = -\dot{\theta}^{T} J(\bar{q}_{l}(\theta))^{T} D_{d} J(\bar{q}_{l}(\theta)) \dot{\theta} \quad (7.70)$$

因此是负半定的。这确保了稳定性。渐近稳定性可以通过再次引用拉萨尔不变性原理来证明。

7.6 进一步推广

在前面的章节中，详细描述了所提出的阻抗控制器的基本设置。该控制器的几种扩展是可能的。其中一些将在本节中讨论。首先，在第7.6.1节中展示如何将齿轮阻尼包含在模型中，或者包含在控制器中。之后，在第7.6.2节中考虑一种更一般的控制器结构，其中也包含扭矩导数 $\dot{\tau}$ 的反馈。最后，在第7.6.3节和第7.6.4节中对滤波传感器信号的使用以及有关惯性矩阵整形的问题进行了一些评论。

7.6.1 包含关节阻尼

由于控制器的分析基于扭矩反馈的物理解释，因此也很容易包含关节阻尼，即齿轮阻尼。考虑带有关节阻尼的模型在（2.39）-（2.40）中给出。为了方便参考，这里重复这些方程：

$$\begin{array}{c} M(q)\ddot{q} + C(q,\dot{q})\dot{q} + g(q) = K(\theta - q) + D(\dot{\theta} - \dot{q}) + \tau_{ext}, \\ B\ddot{\theta} + K(\theta - q) + D(\dot{\theta} - \dot{q}) = \tau_{m}, \\ \tau = K(\theta - q). \end{array} \quad (7.72)$$

矩阵 $D\in \mathbb{R}^{n\times n}$ 是一个对角正定阻尼矩阵。对于这个模型，可以使用与上一节相同类型的控制器，当使用控制律：

$$\boldsymbol{\tau}_{m} = B B_{\theta}^{-1} \mathbf{u} + (I - B B_{\theta}^{-1}) \left( \boldsymbol{\tau} + D K^{-1} \dot{\boldsymbol{\tau}} \right) \quad (7.73)$$

代替（7.3）时。这导致闭环系统：

$$\begin{array}{c} M(q)\ddot{q} + C(q,\dot{q})\dot{q} + g(q) = K(\theta - q) + D(\dot{\theta} - \dot{q}) + \tau_{ext} \\ B_{\theta} \ddot{\theta} + K(\theta - q) + D(\dot{\theta} - \dot{q}) = \mathbf{u} \end{array} \quad (7.75)$$

对于该系统，中间控制输入 $\mathbf{u}$ 可以按照与上一节相同的方式选择。特别地，下面更详细地考虑来自（7.36）的控制律。在相同的条件下，可以证明与第7.4节类似的无源性和稳定性性质。下面将只考虑无源性性质。将证明与（7.40）和（7.42）中相同的存储函数 $S_{q}(q,\dot{q})$ 和 $S_{\theta}(q,\theta ,\dot{\theta})$ 可以使用。对于存储函数 $S_{q}(q,\dot{q})$ 沿（7.74）的导数，现在得到：

$$\dot{S}_{q}(q, \dot{q}) = \dot{q}^{T} (\boldsymbol{\tau} + D K^{-1} \dot{\boldsymbol{\tau}} + \boldsymbol{\tau}_{ext}),$$

由此可以得出，如果重力势能 $V_{g}(q)$ 有界，系统（7.74）表示一个无源映射 $(\boldsymbol{\tau} + D K^{-1} \dot{\boldsymbol{\tau}} + \boldsymbol{\tau}_{ext}) \rightarrow \dot{q}$ 。此外， $S_{\theta}(q,\theta ,\dot{\theta})$ 沿（7.75）的解（带有控制律（7.36））的导数由下式给出：

$$\dot{S}_{\theta}(q, \theta, \dot{\theta}) = -\dot{x}_{\theta}^{T} D_{d} \dot{x}_{\theta} - (\dot{\theta} - \dot{q})^{T} D(\dot{\theta} - \dot{q}) - \dot{q}^{T} (\boldsymbol{\tau} + D K^{-1} \dot{\boldsymbol{\tau}}).$$

因此，可以得出（7.75）是一个无源映射 $\dot{q} \rightarrow - (\boldsymbol{\tau} + D K^{-1} \dot{\boldsymbol{\tau}})$ 。因此，闭环系统可以再次看作是两个无源子系统的反馈互连，唯一的区别是端口变量 $\boldsymbol{\tau}$ 被变量 $\boldsymbol{\tau} + D K^{-1} \dot{\boldsymbol{\tau}}$ 取代。

此外，通过考虑 $S_{q}(q,\dot{q}) + S_{\theta}(q,\theta ,\dot{\theta})$ 作为李雅普诺夫函数，可以证明自由运动情况下的渐近稳定性。由于证明可以与第7.4.2节完全类似地进行，这里省略。

7.6.2 推广到全状态反馈

除了使用控制律（7.73），还可以考虑更一般的控制器：

$$\boldsymbol{\tau}_{m} = B B_{\theta}^{-1} \mathbf{u} + (I - B B_{\theta}^{-1}) \left( \boldsymbol{\tau} + \mathbf{K}_{s} \mathbf{K}^{-1} \dot{\boldsymbol{\tau}} \right), \quad (7.76)$$

其中 $K_{s}$ 是一个正定控制器增益矩阵，可能与 $D$ 不同。与（7.36）一起，闭环系统由下式给出：

$$\begin{array}{c} M(\mathbf{q})\ddot{\mathbf{q}} + C(\mathbf{q},\dot{\mathbf{q}})\dot{\mathbf{q}} + g(\mathbf{q}) = \boldsymbol{\tau} + D \mathbf{K}^{-1} \dot{\boldsymbol{\tau}} + \boldsymbol{\tau}_{ext}, \\ B_{\theta} \ddot{\boldsymbol{\theta}} + \boldsymbol{\tau} + (B_{\theta} B^{-1} D - B_{\theta} B^{-1} K_{s} + K_{s}) K^{-1} \dot{\boldsymbol{\tau}} = \mathbf{u}, \\ \mathbf{u} = \bar{\mathbf{g}}(\boldsymbol{\theta}) - J(\boldsymbol{\theta})^{T} (K_{d} \dot{\mathbf{x}}_{\theta} + D_{d} \dot{\mathbf{x}}_{\theta}). \end{array} \quad (7.76)$$

为了分析该系统的稳定性，可以考虑与第7.4.2节中相同的李雅普诺夫函数：

$$V(q, \dot{q}, \theta, \dot{\theta}) = \frac{1}{2} \dot{q}^{T} M(q) \dot{q} + \frac{1}{2} \dot{\theta}^{T} B_{\theta} \dot{\theta} + V_{pot}(q, \theta)$$

通过冗长的计算可以得出， $V(q, \dot{q}, \theta , \dot{\theta})$ 的时间导数现在可以写成：

$$\dot{V}(q, \dot{q}, \theta, \dot{\theta}) = - \begin{pmatrix} \dot{\theta} - \dot{q} \\ \dot{\theta} \end{pmatrix}^{T} Q(\theta) \begin{pmatrix} \dot{\theta} - \dot{q} \\ \dot{\theta} \end{pmatrix}$$ $$Q(\theta) = \begin{bmatrix} D & \frac{1}{2} (B_{\theta} B^{-1} - I)(D - K_{s}) \\ \frac{1}{2} (B_{\theta} B^{-1} - I)(D - K_{s}) & J(\theta)^{T} D_{d} J(\theta) \end{bmatrix}$$

从引理A.20可以看出，如果条件：

$$J(\theta)^{T} D_{d} J(\theta) - \frac{1}{4} (D - K_{s})(B_{\theta} B^{-1} - I) D^{-1} (B_{\theta} B^{-1} - I)(D - K_{s}) > 0$$

对所有 $\theta \in \mathbb{R}^{n}$ 成立，则 $\dot{V}(q, \dot{q}, \theta , \dot{\theta})$ 是负半定的，这是对 $K_{s}$ 和 $D_{d}$ 的隐式条件。

7.6.3 关于速度滤波的说明

本章控制器的另一个推广可以通过使用滤波器来实现阻尼项。例如，对于DLR轻型机器人，只能测量电机位置和关节扭矩，而这些信号的时间导数必须通过数值微分或滤波来计算。在KS98中，提出了一大类线性位置滤波器，包括例如简单的基于非模型的一阶和二阶滤波器。其中表明，使用这些滤波器来估计 $\dot{\theta}$ 不会危及基于能量整形的柔性关节机器人控制器的稳定性性质。由于第7.1节中扭矩反馈的物理解释，这个结果确实也可以应用于本章提出的控制器。更多关于这种速度观测器的细节可以在KS98及其引用的参考文献中找到。

7.6.4 惯性整形

所提出控制器的一个缺点是其在惯性整形方面的局限性。一方面，扭矩反馈可以看作是电机惯性的整形，正如本章引言中更详细讨论的那样。另一方面，整形视在连杆侧惯性的可能性似乎是有限的。至少可以通过参考如图7.4所示的控制器结构来近似具有更大惯性的系统行为。通过选择高的（即 $K_{c} > > K_{\theta}$ ）刚度值 $K_{c}$ （以及适当的阻尼因子 $D_{c}$ ），闭环行为在低频域将由质量为 $M_{d} + B_{\theta} + M$ 的弹簧-质量-阻尼器系统的行为主导。

7.7 跟踪

第7.2节中带有第7.3.2节重力补偿项的控制器是专门为调节而设计的，其中期望的虚拟平衡位置 $\mathbf{x}_{d} = \mathbf{f}(\mathbf{q}_{d})$ （或 $\mathbf{x}_{\theta,d}$ ）是恒定的。然而，将该控制器直接扩展到跟踪情况（具有时变的虚拟平衡位置）并不明确。

对于重力扭矩，设计了一个补偿项，它在所有静态点补偿重力效应。这个补偿项 $\mathbf{u}_{g} = \bar{\mathbf{g}}(\boldsymbol{\theta})$ 基于准静态考虑，因此它仅依赖于电机侧位置 $\boldsymbol{\theta}$ 。这允许对调节情况进行基于无源性的稳定性分析。除了无源性考虑之外，另一个在重力补偿背景下出现的问题是，补偿项 $\mathbf{u}_{g} = \mathbf{g}(\mathbf{q})$ 是否也会导致稳定的闭环系统。这个问题仍然是开放的。

下面，讨论一种不同的基于无源性的方法。与本章前面的章节相反，现在设计将集中在跟踪情况。但期望的是，当简化到调节情况时，应该获得类似于第7.2节控制器的控制器。最终控制器与BOL95中基于无源性的跟踪控制器的关系将在第7.7.3节中讨论。此外，最终控制器也将给出一个关于上述使用重力补偿项 $\mathbf{u}_{g} = \mathbf{g}(\mathbf{q})$ 进行调节的可使用性问题的提示——而不是答案。

为简化表述，本节仅考虑关节级阻抗的情况。矩阵 $\mathbf{K}_{\theta}$ 和 $\mathbf{D}_{\theta}$ 是期望的对称正定刚度和阻尼矩阵。所提出的控制器到笛卡尔情况的扩展可以与第7.2节完全相同的方式进行。假设虚拟平衡位置 $\mathbf{q}_{d}$ 直到四阶时间导数都是连续可微的。

7.7.1 无重力情况

首先考虑来自（7.1）-（7.2）的动力学无重力版本：

$$\begin{array}{r} M(\mathbf{q})\ddot{\mathbf{q}} + C(\mathbf{q},\dot{\mathbf{q}})\dot{\mathbf{q}} = \boldsymbol{\tau} + \boldsymbol{\tau}_{ext}, \\ B\ddot{\boldsymbol{\theta}} + \boldsymbol{\tau} = \boldsymbol{\tau}_{m}, \end{array} \quad (7.77)$$

其中 $\boldsymbol{\tau}$ 再次由 $\boldsymbol{\tau} = \mathbf{K}(\boldsymbol{\theta} - \mathbf{q})$ 给出。与第7.2节类似，应用以下形式的扭矩反馈：

$$\boldsymbol{\tau}_{m} = B B_{\theta}^{-1} \mathbf{u} + (I - B B_{\theta}^{-1}) \boldsymbol{\tau}, \quad (7.78)$$

其中 $\mathbf{u}$ 是一个新的输入变量，以将视在电机惯量缩放到 $\mathbf{B}_{\theta}$ 。这导致：

$$\begin{array}{r} M(\mathbf{q})\ddot{\mathbf{q}} + C(\mathbf{q},\dot{\mathbf{q}})\dot{\mathbf{q}} = \boldsymbol{\tau} + \boldsymbol{\tau}_{ext}, \\ B_{\theta} \ddot{\boldsymbol{\theta}} + \boldsymbol{\tau} = \mathbf{u}. \end{array} \quad (7.79)$$

第7.2节中基于无源性的控制器设计然后基于以下事实：（7.79）表示一个无源映射 $(\boldsymbol{\tau} + \boldsymbol{\tau}_{ext}) \rightarrow \dot{\mathbf{q}}$ ，并且（7.80）表示一个无源映射 $\dot{\mathbf{q}} \rightarrow - \boldsymbol{\tau}$ 。（7.79）的无源性可以很容易地用存储函数 $S_{1} = 1 / 2 \dot{\mathbf{q}}^{T} \mathbf{M}(\mathbf{q}) \dot{\mathbf{q}}$ 来证明。对于跟踪情况，速度误差 $\dot{\tilde{\mathbf{q}}} = \dot{\mathbf{q}} - \dot{\mathbf{q}}_{d}$ 比速度 $\dot{\mathbf{q}}$ 更相关。因此，考虑：

$$\bar{S}_{q}(\tilde{q}, \dot{\tilde{q}}, t) = \frac{1}{2} \dot{\tilde{q}}^{T} M(\mathbf{q}) \dot{\tilde{q}} \quad (7.81)$$

作为（7.79）的（时变）存储函数是有意义的。由于：

$$\dot{\bar{S}}_{q}(\tilde{q}, \dot{\tilde{q}}, t) = \dot{\tilde{q}}^{T} (\boldsymbol{\tau} - \mathbf{M}(\mathbf{q}) \ddot{\tilde{q}}_{d} - \mathbf{C}(\mathbf{q}, \dot{\tilde{q}}) \dot{\tilde{q}}_{d} + \boldsymbol{\tau}_{ext}) \quad (7.82)$$

可以看出，刚体系统（7.79）表示从输入 $(\boldsymbol{\tau} - \mathbf{M}(\mathbf{q}) \ddot{\mathbf{q}}_{d} - \mathbf{C}(\mathbf{q}, \dot{\mathbf{q}}) \dot{\mathbf{q}}_{d} + \boldsymbol{\tau}_{ext})$ 到输出 $\dot{\tilde{q}}$ 的无源映射。为了将电机动力学（7.80）转换为关于输入 $\ddot{\mathbf{q}}$ 和输出 $- (\boldsymbol{\tau} - \mathbf{M}(\mathbf{q}) \ddot{\mathbf{q}}_{d} - \mathbf{C}(\mathbf{q}, \dot{\mathbf{q}}) \dot{\mathbf{q}}_{d})$ 的无源映射，即实现端口匹配，考虑存储函数：

$$\bar{S}_{\theta}(\bar{q}, \bar{\theta}, \dot{\bar{\theta}}) = \frac{1}{2} \dot{\bar{\theta}}^{T} B_{\theta} \dot{\bar{\theta}} + \frac{1}{2} (\bar{\theta} - \bar{q})^{T} K(\bar{\theta} - \bar{q}) + \frac{1}{2} \bar{\theta}^{T} K_{\theta} \bar{\theta} \quad (7.83)$$

其中电机位置误差 $\bar{\theta} = \theta - \theta_{s}$ 。期望的电机位置 $\theta_{s}$ 稍后确定。则存储函数 $\bar{S}_{\theta}(\bar{q}, \bar{\theta}, \dot{\bar{\theta}})$ 沿（7.80）的时间导数由下式给出：

$$\begin{array}{rl} & \dot{\bar{S}}_{\theta}(\bar{q}, \bar{\theta}, \dot{\bar{\theta}}) = \dot{\bar{\theta}}^{T} B_{\theta} \dot{\bar{\theta}} + (\bar{\theta} - \bar{q})^{T} K(\dot{\bar{\theta}} - \dot{\bar{q}}) + \bar{\theta}^{T} K_{\theta} \dot{\bar{\theta}} \\ & = \dot{\bar{\theta}}^{T} \left( u - K(\theta - q) - B_{\theta} \dot{\theta}_{s} \right) + (\bar{\theta} - \bar{q})^{T} K(\dot{\bar{\theta}} - \dot{\bar{q}}) + \bar{\theta}^{T} K_{\theta} \dot{\bar{\theta}}. \end{array} \quad (7.84)$$

以下形式的控制器：

$$\mathbf{u} = B_{\theta} \dot{\theta}_{s} + K(\theta_{s} - q_{d}) - K_{\theta} \bar{\theta} - D_{\theta} \dot{\bar{\theta}} \quad (7.84)$$

然后导致：

$$\begin{array}{rl} & \dot{\bar{S}}_{\theta}(\bar{q}, \bar{\theta}, \dot{\bar{\theta}}) = -\dot{\bar{\theta}}^{T} D_{\theta} \dot{\bar{\theta}} - \dot{\bar{\theta}}^{T} K(\bar{\theta} - \bar{q}) + (\dot{\bar{\theta}} - \dot{\bar{q}})^{T} K(\bar{\theta} - \bar{q}) \\ & = -\dot{\bar{\theta}}^{T} D_{\theta} \dot{\bar{\theta}} - \dot{\bar{q}}^{T} K(\bar{\theta} - \bar{q}) \\ & = -\dot{\bar{\theta}}^{T} D_{\theta} \dot{\bar{\theta}} - \dot{\bar{q}}^{T} (\tau - K(\theta_{s} - q_{d})). \end{array} \quad (7.85)$$

为了确保期望的无源性性质，现在通过关系式：

$$M(q)\ddot{q}_{d} + C(q,\dot{q})\dot{q}_{d} = K(\theta_{s} - q_{d}) \quad (7.88)$$

将期望的电机侧位置 $\theta_{s}$ 定义为：

$$\theta_{s} = q_{d} + K^{-1} \left( M(q) \ddot{q}_{d} + C(q, \dot{q}) \dot{q}_{d} \right) \quad (7.89)$$

因此它是 $\mathbf{q}$ 和 $\dot{\mathbf{q}}$ 的函数，即 $\theta_{s} = \theta_{s}(q, \dot{q}, t)$ 。由此获得了所需的无源性性质。可以很容易地验证，该控制器导致时变闭环系统：

$$\begin{array}{r} M(q)\ddot{q} + C(q,\dot{q})\dot{q} = K(\bar{\theta} - \bar{q}) + \tau_{ext}, \\ B_{\theta} \ddot{\bar{\theta}} + K(\bar{\theta} - \bar{q}) + D_{\theta} \dot{\bar{\theta}} + K_{\theta} \bar{\theta} = 0. \end{array} \quad (7.91)$$

注意，由于 $\theta_{s}$ 的特定选择（7.89），项 $- K(\bar{\theta} - \bar{q})$ 对应于电机动力学的端口输出变量 $- (\tau - M(q) \ddot{q}_{d} - C(q, \dot{q}) \dot{q}_{d})$ ，另见图7.5。此外，通过将 $\bar{S}_{q}(\bar{q}, \dot{\bar{q}}, t) + \bar{S}_{\theta}(\bar{q}, \dot{\bar{\theta}}, \dot{\bar{\theta}})$ 视为时变李雅普诺夫函数，可以很容易地证明，在自由运动情况下（ $\tau_{ext} = 0$ ），原点 $\bar{\theta} = \bar{q} = \dot{\bar{\theta}} = \dot{\bar{q}} = 0$ 是一个稳定的平衡点。

7.7.2 包含重力

现在考虑带重力的连杆侧动力学：

$$M(\mathbf{q})\ddot{\mathbf{q}} + C(\mathbf{q}, \dot{\mathbf{q}})\dot{\mathbf{q}} + \mathbf{g}(\mathbf{q}) = \boldsymbol{\tau} + \boldsymbol{\tau}_{ext} \quad (7.92)$$

而不是（7.79）。则 $\dot{S}_{q}(\bar{q}, \dot{q}, t)$ 由下式给出：

$$\dot{S}_{q}(\bar{q}, \dot{q}, t) = \dot{q}^{T} (\tau - M(\mathbf{q}) \ddot{q}_{d} - C(\mathbf{q}, \dot{q}) \dot{q}_{d} - g(\mathbf{q}) + \boldsymbol{\tau}_{ext}) \quad (7.93)$$

可以看出，连杆侧刚体动力学（7.92）可以被视为无源映射 $(\tau - M(\mathbf{q}) \ddot{q}_{d} - C(\mathbf{q}, \dot{q}) \dot{q}_{d} - g(\mathbf{q})) \to \ddot{q}$ 。因此，可以通过选择 $\theta_{s}$ 将重力补偿项包含在控制器中：

$$M(\mathbf{q})\ddot{q}_{d} + C(\mathbf{q}, \dot{q}) \dot{q}_{d} + g(\mathbf{q}) = K(\theta_{s} - \mathbf{q}_{d}) \quad (7.94)$$

代替（7.88）。完整的跟踪控制器由下式给出：

$$\begin{array}{l} \tau_{m} = B B_{\theta}^{-1} u + (I - B B_{\theta}^{-1}) \tau, \\ u = B_{\theta} \ddot{\theta}_{s} + K(\theta_{s} - q_{d}) - K_{\theta} \tilde{\theta} - D_{\theta} \dot{\tilde{\theta}}, \end{array} \quad (7.95)$$ $$\theta_{s} = q_{d} + K^{-1} (M(\mathbf{q}) \ddot{q}_{d} + C(\mathbf{q}, \dot{q}) \dot{q}_{d} + g(\mathbf{q})), \quad (7.97)$$

并导致与（7.90）-（7.91）形式相同的闭环系统。

7.7.3 控制器讨论

注意，第7.7.1节中的无重力版本是第7.2节中调节控制器的直接扩展，因为对于 $\dot{q}_{d} = 0$ ，它简化到相同的反馈律。然而，第7.7.2节中包含重力的解决方案是不同的。对于 $\dot{q}_{d} = 0$ ，控制器（7.96）-（7.97）简化为：

$$\begin{array}{l} \tau_{m} = B B_{\theta}^{-1} u + (I - B B_{\theta}^{-1}) \tau, \\ u = B_{\theta} K^{-1} \dot{g}(q) + g(q) - K_{\theta} (\theta - \theta_{s}) - D_{\theta} (\dot{\theta} - \dot{\theta}_{s}), \\ \theta_{s} = q_{d} + K^{-1} g(q). \end{array} \quad (7.100)$$

这表明，在调节情况下使用 $u_{g} = g(q)$ 而不是第7.3.2节中的重力补偿项不足以确保稳定性。在（7.99）中，还需要二阶导数 $\dot{g}(q)$ 。

将第7.3.2节中基于电机位置的在线重力补偿与本节中的跟踪控制器相结合似乎很困难，因为第7.3.2节中重力补偿项的设计基于准静态考虑。

还要注意，到笛卡尔阻抗控制器情况的扩展可以与第7.2节类似地进行。

还应该提到，所提出的跟踪控制器与BOL95和LO95中基于无源性的跟踪控制器有关。在BOL95中，提出了LB92中控制器的简化版本，其中基本上使用Slotine和LiSL87的刚体跟踪控制律代替了（7.94）中的关系。与本文的表述类似，扭矩反馈的物理解释（第7.1节）允许通过内扭矩反馈回路来增强这些控制器，而BOL95, LO95, LB92中没有包含该回路，从而提高了控制性能。此外，BOL95中的控制器通过再次应用SL87的控制器结构，对电机动力学使用了稍微不同的控制器方程（7.96）。

7.8 与其他方法的关系

在本节中，简要讨论了所提出的基于无源性的阻抗控制器与文献中其他一些控制器设计方法的关系。

7.8.1 与奇异摄动控制器的比较

扭矩反馈的物理解释也可以用来解释第5.4.1节奇异摄动分析中稳态系统的有效惯性。 （5.26）中的有效惯性由 $M(q) + (I + K_{\tau})^{-1} B$ 给出。第5.4.1节中的扭矩反馈增益 $- K_{\tau}$ 显然对应于（7.3）中的增益 $I - B B_{\theta}^{-1}$ ，通过该增益实现了电机惯量从 $B$ 到 $B_{\theta}$ 的缩放。因此，奇异摄动方法的新有效惯性可以看作是连杆侧惯性矩阵和缩放电机惯量 $M(q) + B_{\theta}$ 之和。

7.8.2 与Albu-Schaffer控制器的比较

本章提出的笛卡尔阻抗控制器具有与AS01, ASH01a, ASH00, ASH01b中控制器非常相似的结构。在这些工作中，为柔性关节机器人模型开发了一个位置控制器，它由关节扭矩和电机侧关节位置误差的反馈组成。对于重力扭矩，使用了一个基于期望连杆侧位置的补偿项，类似于Tom91。与此相反，在本工作中，开发了一种更适用于阻抗控制的在线重力补偿项。此外，第7.2节中扭矩反馈的物理解释随后允许将实际上与AS01非常相似的关节级阻抗控制器推广到笛卡尔情况（另见ASmOH07）。

7.8.3 与IDA-PBC和控制拉格朗日方法的关系

IDA-PBC OvdSME02, OSGEB02 和控制拉格朗日方法BLM00, BCLM01是最近在控制文献中提出的能量整形方法的两种扩展。这两种方法的出色比较在BOvdS02中给出。注意到本章提出的控制器与这些方法的关系是有趣的。因此，考虑第7.2节中在第7.4节详细分析的笛卡尔阻抗控制器。由于第7.4节的稳定性分析基于系统的无源性性质，可以很容易地使用第7.4.1节的存储函数来为IDA-PBC或控制拉格朗日方法设置能量函数。例如，第7.4.2节的李雅普诺夫函数正是在欠驱动系统控制拉格朗日框架下导致第7.2节控制器的目标拉格朗日量。注意，本章中使用的存储函数被隐含地设计为满足欠驱动系统控制拉格朗日方法的匹配条件。基于IDA-PBC框架的控制器设计的一个额外自由度是使用附加的斜对称矩阵来整形闭环系统的内部互连结构OvdSME02, OSGEB02, BOvdS02。不在此详述，应该提到，例如（4.48）中的零空间解耦就是一个有趣的应用。

7.9 总结

本章讨论了一种基于无源性的控制方法。受第3章刚体情况讨论的启发，考虑了无惯性整形的阻抗行为。所提出控制器背后的基本思想是将扭矩反馈物理解释为电机惯量的缩放。除了内扭矩反馈回路，外阻抗控制器实现了期望的笛卡尔刚度和阻尼。此外，仅基于电机侧位置的测量，设计了重力扭矩的补偿项。

结果表明，闭环系统可以看作是无源子系统的反馈互连。这带来了非常有利的鲁棒性性质。基于系统的无源性性质，给出了渐近稳定性的证明。

在讨论了基本控制器之后，提出了关于全状态反馈、刚度设计和跟踪情况的若干扩展。

---

8 评估

本章报告了验证和比较前几章不同控制律的仿真和实验。首先在第8.1节的仿真研究中比较了这些控制器。然后通过DLR-Lightweight-Robot-II的实验更详细地评估了基于第5章和第7章结果的笛卡尔阻抗控制器。与典型的工业机器人相反，该机器人配备了关节扭矩传感器，因此非常适合实现所提出的控制器。

8.1 仿真结果

在本节中，报告了一些仿真结果，通过这些结果对本书中的不同控制器进行了比较。对于仿真，考虑了根据DLR-Lightweight-Robot-II的前六个关节的柔性关节机器人模型。该机器人的运动学结构在附录A.6中给出。对于相关的笛卡尔坐标，选择了误差变量：

$$\tilde{\mathbf{x}} = \begin{pmatrix} {}^{i}\mathbf{p}_{td}(\mathbf{q},t) \\ {}^{t}\boldsymbol{\phi}_{td}(\mathbf{q},t) \end{pmatrix} =: \begin{pmatrix} e_{x} \\ e_{y} \\ e_{z} \\ \phi_{x} \\ \phi_{y} \\ \phi_{z} \end{pmatrix} \quad (8.1)$$

（另见第3.5节），其中对于欧拉角 $^{t}\phi_{td}(\mathbf{q},t)$ 使用了滚转-俯仰-偏航表示。因此，期望的刚度和阻尼矩阵是相对于工具框架 $\mathcal{T}$ 定义的。

然后，期望的笛卡尔阻抗由下式给出：

$$\Lambda(\mathbf{x})\ddot{\mathbf{x}} + (\mu(\mathbf{x}, \dot{\mathbf{x}}) + \mathbf{D}_{d}) \dot{\mathbf{x}} + \mathbf{K}_{d} \tilde{\mathbf{x}} = \mathbf{F}_{ext}, \quad (8.2)$$

其中惯性矩阵 $\Lambda(\mathbf{x})$ 和科里奥利/离心矩阵 $\mu(\mathbf{x}, \dot{\mathbf{x}})$ 是来自DLR-Lightweight-Robot-II连杆侧刚体模型的相关矩阵。

为了评估控制器的标称性能，在随后的仿真研究中假设所有需要的测量信号都可用且不受噪声污染。对于虚拟平衡位置在 $x$ 方向上的 $5\mathrm{cm}$ 阶跃，评估了阶跃响应。阶跃之后，虚拟平衡位姿 $\mathbf{x}_{d}$ 是恒定的，即 $\dot{\mathbf{x}}_{d} = \mathbf{0}$ ，这导致了一个调节问题。起始配置如图8.1所示。对于期望的刚度和阻尼矩阵，使用了表8.1中对角线条目的对角矩阵。这些值在所有仿真中保持不变。

所有控制器都包含关节扭矩 $\boldsymbol{\tau}$ 的反馈。为了获得控制器之间有意义的比较，扭矩反馈的比例增益 $\bar{K}_{\tau}$ 和微分增益 $\bar{D}_{\tau}$ 在所有控制器中选择了相同的值。首先，在低扭矩控制增益下比较控制器的性能。因此， $\bar{K}_{\tau}$ 和 $\bar{D}_{\tau}$ 被选择为对角矩阵，其对角线元素值来自表8.2。在第二次仿真中，使用了来自表8.3的较高增益值。在这两种情况下，阻尼值都被选择为 $\bar{D}_{\tau,i} = 2\xi_{\tau}\sqrt{K_{\tau,i} K_{i} / B_{i}}$ ，其公共阻尼因子为 $\xi_{\tau} = 1.1$ 。选择这个相对较高的 $\xi_{\tau}$ 值是为了确保得到的增益矩阵 $\bar{D}_{\tau}$ 也可以用于反步法控制器。这将在第8.1.3节中更详细地解释。

表8.1. 仿真的刚度和阻尼值

坐标	e x	e y	e z	φ x	φ y	φ z
刚度	2000 N/m	2000 N/m	2000 N/m	100 Nm/rad	100 Nm/rad	100 Nm/rad
阻尼	313.05 Ns/m	313.05 Ns/m	313.05 Ns/m	70.0 Nms/rad	70.0 Nms/rad	70.0 Nms/rad

表8.2. 低扭矩控制器增益（情况I）仿真的刚度和阻尼值

i	1	2	3	4	5	6
K_{τ,i}	0.5	0.5	0.5	0.5	0.6	0.5
D_{τ,i}	0.041	0.047	0.041	0.033	0.034	0.041

表8.3. 高扭矩控制器增益（情况II）仿真的刚度和阻尼值

i	1	2	3	4	5	6
K_{τ,i}	3	3	3	3	3	3
D_{τ,i}	0.067	0.077	0.067	0.054	0.055	0.067

8.1.1 基于奇异摄动的控制器

对于调节情况，基于奇异摄动的控制器（5.32）可以写成：

$$\boldsymbol{\tau}_{m} = \boldsymbol{\tau}_{d} - \mathbf{K}_{\tau} (\boldsymbol{\tau} - \boldsymbol{\tau}_{d}) - \epsilon \mathbf{D}_{\tau} \dot{\boldsymbol{\tau}}$$ $$\boldsymbol{\tau}_{d} = \mathbf{g}(\mathbf{q}) - \mathbf{J}(\mathbf{q})^{T} \left( \mathbf{K}_{d} \tilde{\mathbf{x}} + \mathbf{D}_{d} \dot{\tilde{\mathbf{x}}} \right),$$

其中 $\mathbf{K}_{\tau}$ 和 $\mathbf{D}_{\tau}$ 是正定控制器增益矩阵， $\epsilon$ 是来自奇异摄动分析的时间缩放因子。为了简化与其他控制器的比较，增益矩阵选择为 $\mathbf{K}_{\tau} = \bar{\mathbf{K}}_{\tau}$ 和 $\epsilon \mathbf{D}_{\tau} = \bar{\mathbf{D}}_{\tau}$ ，其中对角矩阵 $\bar{\mathbf{K}}_{\tau}$ 和 $\bar{D}_{\tau}$ 根据表8.2和表8.3的值。阶跃响应的两种情况下的仿真结果如图8.2所示。其中，基于奇异摄动的控制器的闭环动态与根据（8.2）的期望闭环行为（图8.2中的虚线）进行了比较。注意，期望的闭环行为（8.2）不使用来自奇异摄动分析的（5.28）中修改后的惯性矩阵 $\mathbf{M}(\mathbf{q}) + (\mathbf{I} + \mathbf{K}_{\tau})^{-1} \mathbf{B}$ 。

在图8.2中可以看出，由于惯性矩阵中的耦合，所有坐标在瞬态阶段都偏离了虚拟平衡位置，但它们的稳态值为零。对于较高的扭矩控制增益（情况II，实线），期望行为的近似更好。在低增益情况下（情况I，虚线），与期望阶跃响应的差异更大。

8.1.2 基于解耦的控制器

接下来，评估第6章中基于解耦的控制器。为了方便阅读，这里重复调节情况下的控制器方程（6.30）：

$$\boldsymbol{\tau}_{m} = \boldsymbol{\tau}_{d} + \mathbf{B} \mathbf{K}^{-1} \left( \ddot{\boldsymbol{\tau}}_{d} - \mathbf{D}_{\tau} (\dot{\boldsymbol{\tau}} - \dot{\boldsymbol{\tau}}_{d}) - \mathbf{K}_{\tau} (\boldsymbol{\tau} - \boldsymbol{\tau}_{d}) \right)$$ $$+ \mathbf{B} \mathbf{M}(\mathbf{q})^{-1} (\boldsymbol{\tau} + \boldsymbol{\tau}_{ext} - \mathbf{C}(\mathbf{q}, \dot{\mathbf{q}}) \dot{\mathbf{q}} - \mathbf{g}(\mathbf{q})),$$ $$\boldsymbol{\tau}_{d} = \mathbf{g}(\mathbf{q}) - \mathbf{J}(\mathbf{q})^{T} \left( \mathbf{K}_{d} \tilde{\mathbf{x}} + \mathbf{D}_{d} \dot{\tilde{\mathbf{x}}} \right).$$

矩阵 $\mathbf{K}_{\tau}$ 和 $\mathbf{D}_{\tau}$ 通过 $\bar{K}_{\tau} = \mathbf{B} \mathbf{K}^{-1} \mathbf{K}_{\tau}$ 和 $\bar{D}_{\tau} = \mathbf{B} \mathbf{K}^{-1} \mathbf{D}_{\tau}$ 分别与第8.1节的 $\bar{K}_{\tau}$ 和 $\bar{D}_{\tau}$ 相关。扭矩反馈矩阵 $\bar{K}_{\tau}$ 和 $\bar{D}_{\tau}$ 以与基于奇异摄动的控制器相同的方式根据表8.2和表8.3选择。

在图8.3中，显示了基于解耦的控制器在两种不同情况（低和高扭矩反馈增益）下的仿真结果。虚线显示笛卡尔坐标对于期望动态（8.2）的轨迹。可以看出，与期望阶跃响应的偏差明显小于图8.2中基于奇异摄动的控制器。即使在低控制增益（情况I）下，仿真结果也与期望结果相当吻合。

注意，图8.3显示了虚拟平衡位置阶跃响应的结果。关于闭环动态（6.24）-（6.25），这对应于扭矩误差动态（6.25）的阶跃激励。还应该提到，对于外力阶跃响应（而不是通过虚拟平衡位置的扭矩误差动态阶跃激励），基于解耦的控制器的闭环动态完全符合期望行为。在这种情况下，扭矩误差始终为零，轨迹将完全符合期望轨迹。当然，这仅在标称情况下所有测量信号都精确可用时才成立。

8.1.3 基于反步法的控制器

同样的仿真也针对来自（6.44）的基于反步法的控制器进行，这里也重复该方程：

$$\begin{array}{rl} & \boldsymbol{\tau}_{m} = \mathbf{B} \ddot{\mathbf{q}} + \boldsymbol{\tau} + \mathbf{B} \mathbf{K}^{-1} \left( \ddot{\boldsymbol{\tau}}_{d} - (\bar{\mathbf{K}}_{s} \bar{\mathbf{K}}_{t} + \mathbf{C}_{s}^{-1} \mathbf{C}_{t}) \mathbf{e}_{t} + (\bar{\mathbf{K}}_{s} + \bar{\mathbf{K}}_{t}) \dot{\mathbf{e}}_{t} \right) \\ & - B \mathbf{K}^{-1} (\mathbf{C}_{t}^{-1} \frac{\mathrm{d}}{\mathrm{d}t} (\mathbf{J}(\mathbf{q})^{-1} \dot{\mathbf{x}}) + \bar{\mathbf{K}}_{s} \mathbf{C}_{t}^{-1} \mathbf{J}(\mathbf{q})^{-1} \dot{\mathbf{x}}). \end{array} \quad (8.1.3)$$

注意，控制器参数矩阵 $C_{s}$ 、 $C_{t}$ 、 $\bar{K}_{t}$ 和 $\bar{K}_{s}$ 都必须正定。这里，参数 $C_{s}$ 和 $C_{t}$ 选择为 $C_{s} = I$ 和 $C_{t} = I$ 。控制器增益矩阵 $\bar{K}_{t}$ 和 $\bar{K}_{s}$ 被设计为使得控制器可以与基于解耦的和基于奇异摄动的控制器进行比较。因此，增益可以通过以下方式与 $\bar{K}_{\tau}$ 和 $\bar{D}_{\tau}$ 相关：

$$\begin{array}{c} B K^{-1} (\bar{K}_{t} \bar{K}_{s} + C_{s}^{-1} C_{t}) = \bar{K}_{\tau} \\ B K^{-1} (\bar{K}_{t} + \bar{K}_{s}) = \bar{D}_{\tau}. \end{array} \quad (B) \quad (B)$$

增益矩阵 $\bar{K}_{\tau}$ 和 $\bar{D}_{\tau}$ 根据表8.2和表8.3选择。在这些增益的设计中，必须确保上述方程对于实值正定矩阵 $\bar{K}_{t}$ 和 $\bar{K}_{s}$ 成立。因此，在第8.1节中，阻尼因子被选择得相当高，为 $\xi_{\tau} = 1.1$ 。

基于反步法控制器的结果显示在图8.4中。与基于解耦控制器的结果相比，笛卡尔末端执行器偏差更大。两种控制器之间的差异也可以通过比较关节扭矩看出。图8.5显示了在低扭矩控制增益（情况I）下，基于解耦和基于反步法的控制器中扭矩 $\tau_{i}$ （ $i = 1,\dots,6$ ）与它们起始值 $\tau_{i}|_{t = 0}$ 的偏差 $\delta \tau_{i} = \tau_{i} - \tau_{i}|_{t = 0}$ 。可以看出，基于反步法的控制器导致的扭矩差异比基于解耦的控制器小。

在当前比较中，假设所有测量信号都可用且不受噪声污染。注意，在真实机器人上实现基于解耦和基于反步法的控制器的主要困难在于需要测量或估计连杆侧加速度和加加速度，正如第6章中更详细解释的那样。

8.1.4 基于无源性的控制器

同样的仿真也针对第7章的基于无源性的控制器进行。基于奇异摄动的控制器与基于无源性的控制器之间的关系已经在第7.8.1节中描述。因此，控制律（7.76）可以写成：

$$\begin{array}{rl} & \boldsymbol{\tau}_{m} = \boldsymbol{\tau}_{d} - \bar{\mathbf{K}}_{\tau} (\boldsymbol{\tau} - \boldsymbol{\tau}_{d}) - \bar{\mathbf{D}}_{\tau} \dot{\boldsymbol{\tau}} \\ & \boldsymbol{\tau}_{d} = \mathbf{g}(\bar{\mathbf{q}}(\boldsymbol{\theta})) - \mathbf{J}(\boldsymbol{\theta})^{T} \left( \mathbf{K}_{d} \tilde{\mathbf{x}}_{\theta}(\boldsymbol{\theta}) + \mathbf{D}_{d} \dot{\tilde{\mathbf{x}}}_{\theta}(\boldsymbol{\theta}) \right), \end{array} \quad (7.76)$$

其中控制器增益矩阵 $\bar{K}_{\tau}$ 和 $\bar{D}_{\tau}$ 分别通过 $\bar{K}_{\tau} = B B_{\theta}^{-1} - I$ 和 $\bar{D}_{\tau} = (I - B B_{\theta}^{-1}) K_{s} K^{-1}$ 与（7.76）中的参数 $B_{\theta}$ 和 $K_{s}$ 相关。

为了实现重力补偿，必须计算函数 $\bar{q}(\theta)$ 。这是通过命题7.3中描述的迭代过程完成的。迭代以 $\bar{q}_{0} = \theta$ 开始，并在两次迭代步后停止。为了考虑关节弹性，笛卡尔控制器刚度矩阵 $K_{d}$ 是根据第7.5.1节中描述的方法选择的。

评估了控制器的两种不同配置，它们的区别在于 $\bar{D}_{\tau}$ 的选择。在两种配置中，增益矩阵 $\bar{K}_{\tau}$ 的选择方式与基于奇异摄动的控制器相同，根据表8.2和表8.3，这分别导致电机缩放因子为 $B B_{\theta}^{-1} = 1.5I$ 和 $B B_{\theta}^{-1} = 4I$ 。在第一种配置中，根据第7.2节，反馈增益矩阵 $\bar{D}_{\tau}$ 设为零，而在第二种配置中，它被设置为表8.2和表8.3的值。

$\bar{D}_{\tau} = \mathbf{0}$ 时的控制器性能如图8.6所示。图8.7显示了 $\bar{D}_{\tau} \neq \mathbf{0}$ 时的结果。可以看出，非零 $\bar{D}_{\tau}$ 的控制器在振荡阻尼方面具有更好的性能。而第7.5.2节的稳定性分析是针对 $\bar{D}_{\tau} = \mathbf{0}$ 的控制器给出的。

图8.7中 $\bar{D}_{\tau} \neq 0$ 的控制器的性能与基于奇异摄动的控制器非常相似。这并不奇怪，因为这两种控制器具有非常相似的结构。

8.1.5 鲁棒性

除了控制器标称性能的仿真之外，还考虑了控制器对参数不确定性的鲁棒性。

基于奇异摄动的控制器不显式包含模型参数。类似地，基于无源性控制器的稳定性陈述也独立于模型参数。例如，如果电机惯量 $B$ 的值不精确知道，这仅意味着缩放后的电机惯量 $B_{\theta}$ 将不同于期望值。但稳定性不受影响。

基于解耦的控制器和反步法控制器的情况则不同。这些控制器需要关节加速度和加加速度。在上述仿真中，假设这些信号可以精确获得。但在实践中这是不可能的，必须通过其他测量值并使用模型参数通过以下方式计算这些信号：

$$\begin{array}{l} \ddot{\mathbf{q}} = \mathbf{M}(\mathbf{q})^{-1} (\boldsymbol{\tau} + \boldsymbol{\tau}_{ext} - \mathbf{C}(\mathbf{q}, \dot{\mathbf{q}}) \dot{\mathbf{q}} - \mathbf{g}(\mathbf{q})), \\ \mathbf{q}^{(3)} = \mathbf{M}(\mathbf{q})^{-1} \left( \dot{\boldsymbol{\tau}} - \dot{\mathbf{M}}(\mathbf{q}) \ddot{\mathbf{q}} + \frac{\mathrm{d}}{\mathrm{d}t} (\boldsymbol{\tau}_{ext} - \mathbf{C}(\mathbf{q}, \dot{\mathbf{q}}) \dot{\mathbf{q}} - \mathbf{g}(\mathbf{q})) \right). \end{array} \quad (8.4)$$

除此之外，控制器还包含电机惯量。即使加速度和加加速度可以精确获得，电机惯量的不确定性也可能影响闭环系统的稳定性。

在下面的仿真中，分析了基于解耦的控制器和反步法控制器对电机惯量不确定性的鲁棒性。与之前的仿真类似，假设关节加速度和加加速度可以通过（8.3）和（8.4）精确获得。控制器使用根据表8.2的扭矩反馈。图8.8显示了不同电机惯量值下的仿真结果。控制器使用电机惯量的标称值 $B_{nom}$ ，而模型参数设置为 $B = B_{nom} / f_{m}$ ，其中 $f_{m} = 1$ 、 $f_{m} = 1.1$ 和 $f_{m} = 1.3$ 。

同样的仿真也针对反步法控制器进行。结果如图8.9所示。可以看出，反步法控制器对电机惯量参数的不确定性比基于解耦的控制器更敏感。

这是由于反步法控制器包含取决于 $B$ 的额外反馈项。

为了评估基于奇异摄动的控制器和基于无源性的控制器的鲁棒性，也使用这些控制器在相同的控制器增益下进行了相同的实验。结果如图8.10和图8.11所示。可以看出，这些控制器对于所考虑的模型不确定性是鲁棒的。

8.2 DLR-Lightweight-Robot-II 实验

在本节中，展示了关于七关节DLR-Lightweight-Robot-II实现笛卡尔阻抗的一些实验。图8.12描绘了该机器人的实时控制器结构。该机器人的每个关节都配备了一个信号处理器。这允许以高采样率（3 kHz）实现各个关节扭矩控制器。测量数据（电机位置和关节扭矩）通过光总线传输到实时计算机系统（500 MHz PowerPC，使用VxWorks作为操作系统）。在这台计算机上，笛卡尔阻抗控制器以1 kHz的采样率实现。连杆侧位置 $\mathbf{q}$ 基于测量值和关节刚度通过 $\mathbf{q} = \boldsymbol{\theta} - \mathbf{K}^{-1} \boldsymbol{\tau}$ 计算。时间导数 $\dot{\boldsymbol{\theta}}$ 、 $\dot{\mathbf{q}}$ 和 $\dot{\boldsymbol{\tau}}$ 通过滤波数值微分获得。

对于更复杂实体（即惯性矩阵、体雅可比矩阵、重力项和科里奥利/离心矩阵）的计算，使用了一个采样时间为6 ms的补充任务（见图8.12）。对于相关的笛卡尔坐标，选择了误差变量：

$$\tilde{\mathbf{x}} = \begin{pmatrix} {}^{i}\mathbf{p}_{td}(\mathbf{q},t) \\ {}^{t}\boldsymbol{\phi}_{td}(\mathbf{q},t) \end{pmatrix} =: \begin{pmatrix} e_{x} \\ e_{y} \\ e_{z} \\ \phi_{x} \\ \phi_{y} \\ \phi_{z} \end{pmatrix} \quad (8.5)$$

（另见第3.5节），其中对于欧拉角 $^{t}\phi_{td}(\mathbf{q},t)$ 使用了滚转-俯仰-偏航表示。

第8.2.1节介绍了基于奇异摄动控制器的实验结果。这包括对实现的刚度和阻尼行为的验证，以及对第4章中不同零空间投影的验证。第8.2.2节介绍了基于无源性控制器的结果。基于解耦的控制器与本文讨论的基于奇异摄动控制器的实验比较在OASKH03, OASKH05中给出。

表8.4. 基于奇异摄动控制器阻抗评估的刚度值

坐标	e x	e y	e z	φ x	φ y	φ z
刚度	800	5000	5000	300	300	300
	N/m	N/m	N/m	Nm/rad	Nm/rad	Nm/rad

8.2.1 基于奇异摄动的控制器

对于基于奇异摄动的控制器，报告了两个实验。在第一个实验中，评估了控制器获得的笛卡尔阻抗。第二个实验涉及零空间刚度。

根据第5章，控制器可以写成以下形式：

$$\begin{array}{rl} & \boldsymbol{\tau}_{m} = \boldsymbol{\tau}_{d} - \mathbf{K}_{\tau} (\boldsymbol{\tau} - \boldsymbol{\tau}_{d}) - \epsilon \mathbf{D}_{\tau} \dot{\boldsymbol{\tau}} \\ & \boldsymbol{\tau}_{d} = \mathbf{g}(\mathbf{q}) + \mathbf{J}(\mathbf{q})^{T} \left( \Lambda(\mathbf{x}) \ddot{\mathbf{x}}_{d} + \mu(\mathbf{x}, \dot{\mathbf{x}}) \dot{\mathbf{x}}_{d} - \mathbf{K}_{d} \tilde{\mathbf{x}} - \mathbf{D}_{d} \dot{\tilde{\mathbf{x}}} \right) \end{array} \quad (1)$$

其中 $K_{\tau}$ 、 $D_{\tau}$ 、 $K_{d}$ 和 $D_{d}$ 是正定控制器增益矩阵。对于扭矩控制增益矩阵 $K_{\tau}$ 和 $D_{\tau}$ ，选择了对角矩阵，使得内扭矩控制器可以在本地信号处理器上以3 kHz的高采样率实现（见图8.12）。期望扭矩 $\tau_{d}$ 以较低的1 kHz采样率计算。

笛卡尔刚度和阻尼评估

在第一个实验中，对调节情况 $\dot{x}_{d} = 0$ 下的笛卡尔刚度和阻尼进行了评估。作为期望的刚度矩阵，选择了表8.4中对角线元素的对角矩阵。对于阻尼矩阵的设计，应用了第3.3节的方法，阻尼因子为 $\xi = 0.7$ 。在实验中，人类用户主要通过拉动和推动（主要在 $x-$ 和 $y-$ 坐标方向）对机器人末端执行器施加（广义）力。图8.1显示了初始配置。交互力由安装在末端执行器上的六自由度力-力矩传感器测量。注意，该传感器不用于阻抗控制器的实现，而是仅用于评估。

图8.13显示了在 $x -$ 、 $y -$ 和 $z$ 方向测量的力。相应的笛卡尔末端执行器偏差如图8.14所示。为了评估最终的刚度和阻尼，图8.15和图8.16分别显示了 $x -$ 和 $y -$ 方向的力和位移。根据表8.4相关刚度值的相应静态特性线由虚线显示。注意，与静态值的类似迟滞的偏差是由笛卡尔阻尼引起的。虚线还显示了简单仿真的结果。

在低（图8.15）和高（图8.16）笛卡尔刚度下，实验结果与期望阻抗的仿真结果相当吻合。

零空间刚度

在下一个实验中，评估了第4章的零空间投影。DLR-Lightweight-Robot-II有七个关节，而笛卡尔阻抗只需要六个自由度。因此，留下了一个一维零空间。在这个实验中，使用了由表8.5中的刚度值以及0.7的阻尼因子表征的笛卡尔阻抗。实验中不存在外力。

表8.5. 零空间实验的刚度值

坐标	e x	e y	e z	φ x	φ y	φ z
刚度	1000	1000	1000	300	300	300
	N/m	N/m	N/m	Nm/rad	Nm/rad	Nm/rad

下面，对来自（4.20）的投影矩阵 $P_{1}(\mathbf{q})$ 、来自（4.22）的 $P_{2}(\mathbf{q})$ 和来自（4.23）的 $P_{3}(\mathbf{q})$ 进行比较。对于 $P_{1}(\mathbf{q})$ ，使用欧几里得度量作为考虑的度量。理想情况下，应该没有来自虚拟平衡位置 $\mathbf{x}_{d}$ 的笛卡尔末端执行器偏差。通过以下过程进行评估。对于来自（4.15）的关节空间刚度和阻尼，选择了对角矩阵 $K_{n} = k_{n} I$ 和 $D_{n} = d_{n} I$ ，并且根据（4.16），最终的关节扭矩被不同的投影矩阵左乘。为了产生振荡的零空间运动，使用了正的零空间刚度 $k_{n}$ 以及负的阻尼因子 $d_{n}$ 。

$$\begin{array}{l} k_{n} = 50 \mathrm{Nm / rad} \\ d_{n} = -\frac{1}{2} \sqrt{k_{n}} \mathrm{Nm / rad / s} \end{array} \quad (8.6)$$

零空间运动的虚拟平衡点被选为 $\mathbf{q}_{d,0} = \mathbf{0}$ 。选择负阻尼值会导致不稳定的零空间运动。当一个关节达到其机械终点时，关节会弹回。这样就在机械终点之间产生了振荡的零空间运动。

为了评估不同的零空间投影，使用这种振荡零空间运动期间的最终末端执行器运动。在这个比较中，只考虑末端执行器的平移运动，笛卡尔误差 $|| \tilde{\mathbf{x}} ||_{t}$ 表示 $\tilde{\mathbf{x}}$ 的平移分量的欧几里得范数。在图8.17中，这个笛卡尔误差显示为不同零空间投影矩阵的结果。这里，在零空间振荡期间，投影矩阵在线切换为 $P_{1}(\mathbf{q})$ 、 $P_{3}(\mathbf{q})$ 和 $P_{2}(\mathbf{q})$ 。可以看出，在静态零空间投影 $P_{1}(q)$ 的情况下，笛卡尔误差明显更大，而使用动态一致投影矩阵 $P_{2}(q)$ 和 $P_{3}(q)$ 的误差幅度相似。

为了也可视化生成图8.17期间存在的零空间运动，图8.18中给出了关节位置偏差 $\mathbf{q} - \mathbf{q}_{d,0}$ 投影到零空间的投影。虽然 $P_{1}(q)$ 在零空间中的振荡幅度最小（图8.18），但它们导致高的笛卡尔误差（图8.17）。注意，在所考虑的设置中，零空间运动不是对称的，因此进入不同方向的运动幅度不相同。还可以看出，不同零空间投影的振荡周期略有不同。

8.2.2 基于无源性的控制器

最后，在两个实验中评估了来自第7章的基于无源性的阻抗控制器（7.36）。图8.19显示了两个实验起始配置中的DLR-Lightweight-Robot-II。为了实现，使用了图8.12的控制器方案。控制律（7.3）以333 μs的采样率实现。来自（7.36）的中间控制输入 $u$ 以1 ms的采样率计算，并传输到本地信号处理器。

刚度和阻尼

在这个实验中，当机器人与人类用户交互时，评估笛卡尔阻抗的准确性。命令的对角笛卡尔刚度矩阵的值显示在表8.6中。对于阻尼矩阵，应用了第3.3节的设计方法，总阻尼因子为0.7。在实验过程中，人类用户通过拉动和推动（主要在水平方向）对末端执行器施加力。这些力由安装在机器人末端执行器上的六自由度力-力矩传感器测量。该传感器仅用于评估，不用于控制器实现。

测量的力如图8.20所示。图8.21显示了末端执行器的笛卡尔位置。在图8.22和图8.23中， $x$ 方向和 $y$ 方向的力-位置测量值与期望静态特性线一起绘制。可以看出，平均斜率与期望值吻合良好。注意，测量值还包含由于期望阻尼引起的力，这产生了类似迟滞的特性。通过与期望动态的简单仿真模型进行比较来评估阻尼。点线显示了期望阻抗仿真的结果，其中测量的力被用作输入。该仿真还包含一个简单的库仑摩擦模型，并通过使用恒定的笛卡尔惯性矩阵进一步简化。可以看出，仿真与测量值吻合良好，尽管仿真结果对力测量中的偏移误差相当敏感。

表8.6. 基于无源性控制器实验的刚度值

坐标	e x	e y	e z	φ x	φ y	φ z
刚度	800	5000	5000	300	300	300
	N/m	N/m	N/m	N/m	N/m/rad	N/m/rad
				Nm/rad	Nm/rad	Nm/rad

冲击实验

第二个实验展示了在冲击刚性表面期间阻抗控制器的效率。操纵器被命令在垂直 $z$ 方向移动。在与木质表面冲击后，尖端的力按比例增加到位置误差。该实验使用的刚度值总结在表8.7中。垂直方向的期望刚度选择得较小，以保持冲击力较小。期望阻尼根据阻尼因子0.7选择。图8.24和8.25显示了 $z$ 方向的位置和力。在冲击时，垂直速度约为 $85 \mathrm{mm / s}$ 。可以看出，接触力的初始峰值下降得相当快。

表8.7. 冲击实验的刚度值

坐标	e x	e y	e z	φ x	φ y	φ z
刚度	5000	5000	700	300	300	300
	N/m	N/m	N/m	Nm/rad	Nm/rad	Nm/rad

---

9 应用

在本简短章节中，介绍了一些使用本书笛卡尔阻抗控制器的典型应用。阻抗控制无疑是为成功执行高级操作任务所需的核心构建块之一Hog85a, Hog85b, Hog85c, BS98。注意，在所有这些应用中，笛卡尔阻抗控制只是一个更大设置的一部分。因此，在实践中，如何将机器人手臂的阻抗控制与路径规划或计算机视觉等其他机器人相关主题相结合，也是一个重要问题。本章的目的是概述设计这些控制器的应用。因此，在介绍中，只对应用进行粗略描述，重点放在笛卡尔阻抗控制器的应用上。除此之外，阻抗控制概念在操作任务中使用的通用性也变得显而易见。

本章前四个实验是在不同设置下使用DRL-Lightweight-Robot-II进行的。第9.1节介绍了一个典型的服务机器人任务：擦桌子。然后，在第9.2节中，描述了一个医疗机器人应用，其中机器人手臂的阻抗控制用于脊柱手术。此后，在第9.3节中描述了DLR-Lightweight-Robot-II与移动平台的组合。在第9.4节中，DLR-Lightweight-Robot-II与移动平台和人工四指手一起用于开门。最后，在第9.5节中，通过考虑人形上半身系统的双臂操作，举例说明了扩展到更复杂运动链的情况。

9.1 作为服务机器人任务的擦桌子

清洁是服务机器人场景中的典型任务。下面考虑擦桌子。图9.1显示了带有简单刚性末端执行器的DLR-Lightweight-Robot-II，用于基本的擦拭应用。

整个应用分为两个步骤。在第一步（示教步骤）中，人类用户可以向机器人示教期望的擦拭运动。因此，为机器人使用了一种笛卡尔阻抗行为，其中方向刚度设置得很高（100 Nm/rad），而平移刚度设置为零，使得机器人末端执行器可以沿着期望路径被引导。末端执行器路径被存储，并在应用的第二步（重放步骤）中用作期望笛卡尔阻抗的虚拟平衡点。在重放步骤中，期望刚度被选择为使得沿桌面（表面）水平方向的平移刚度中等高度（2000 N/m）。对应于垂直运动的平移刚度则设置为零。为了确保机器人与桌面表面保持接触，将期望力 $f_{d} = - 10 \mathrm{N}$ 垂直方向添加到阻抗控制器的笛卡尔力中。

为了实现笛卡尔阻抗，使用了第7章的控制器。由于在示教步骤和重放步骤中方向刚度都设置得很高，因此可以使用第3.5.2节中基于欧拉角的刚度。如果在示教步骤中机器人进入奇异配置，那么机器人在重放步骤中可能会卡在该点。因此，必须避免奇异性。这个应用确实是实现第3.4节奇异性避免的原因。此外，在重放步骤中使用了第4章中基于投影的零空间刚度。零空间刚度选择得相当低，主要用于将关节配置拉离关节限位。

在该应用的基本设置中，机器人只需沿着先前示教的路径擦拭。在最近的发展中，这个应用随后与视觉系统和路径规划系统相结合，以实现更现实的设置。视觉系统用于检测桌面上的污渍。然后，第一步中的示教路径被路径规划系统修改和概括，使得阻抗控制器的命令虚拟平衡位置覆盖检测到的污渍UASS04。此外，DLR-Hand-II被用作末端执行器，而不是图9.1中的刚性末端执行器。

擦拭应用在多个展览会（图9.2）和实验室演示中以不同设置展示。在这些情况下，基于无源性的笛卡尔阻抗控制器的鲁棒性性质非常有利。

9.2 医疗场景：椎弓根螺钉置入

在本场景中，描述了一个医疗机器人应用，其中第7章的基于无源性的笛卡尔阻抗控制器与第3.5.2节中基于四元数的刚度结合使用。所呈现的应用是德国航空航天中心（DLR e.V.）机器人与机电一体化研究所、BrainLAB AG公司、慕尼黑工业大学（TUM）和BGU Murnau之间一个联合项目的主题。

这个简短描述的重点在于笛卡尔阻抗控制器的使用，而不是从医学角度详细描述应用。注意，这个场景实际上是DLR开发的一种新型医疗机器人感兴趣的应用。整个应用和工作流程事先用DLR-Lightweight-Robot-II进行了测试。在这方面，值得指出的是，计划在医疗机器人上也使用与本文讨论的相同类型的控制器，因为其机械结构（从控制角度来看）在某些方面与DLR轻型机器人相当相似。

该应用的目的是在脊柱手术中支持外科医生进行椎弓根螺钉置入。在某些情况下，当椎骨骨折或损坏时，可以通过调整金属板或销钉来稳定结构。这些板通过两个螺钉固定在脊柱上，这两个螺钉被钻入相邻椎骨的椎弓根（见图9.3）。图9.4显示了脊柱模型上四个椎弓根螺钉的典型放置。钻孔点和钻孔轴的位置通常在术前基于CT扫描进行规划。在手术过程中，外科医生必须在术中在患者脊柱上找到术前规划的位置。在没有导航系统的传统手术中，外科医生通过拍摄几次X光扫描来迭代调整钻头的位置并检测其相对于脊柱的位置。在所提出的基于导航的机器人场景中，机器人现在应该用于在先前规划的位置为螺钉钻孔。标记附着在椎骨和机器人上（见图9.5）。因此，（配准后）椎骨相对于机器人末端执行器的相对位姿 $\mathbf{h}_{tb}$ 可以由检测这些标记的导航系统测量。当机器人支持钻头在椎骨上规划位置放置时，钻孔本身由外科医生用安装在机器人末端执行器上的可移动钻头手动进行。手动钻孔是因为这允许外科医生在手术关键部分感受接触力。在这个任务中使用机器人有三个主要优点。一方面，在手术过程中验证螺钉规划位置时，外科医生较少暴露于X射线辐射。另一方面，机器人（至少在一定程度上）防止钻头从椎弓根滑落。与导航系统结合，还可以在钻孔任务中补偿脊柱的位移。基于导航的机器人脊柱手术的另一个重要优点是螺钉放置的手眼协调比传统手术更容易。

给定椎骨和工具框架之间的差异 $\mathbf{h}_{tb}$ ，椎骨的位姿可以通过 $\mathbf{h}_{sb}(t) = \mathbf{h}_{st}(q) \mathbf{h}_{tb}$ 计算。关于控制，椎骨位置被视为常数。

手术的工作流程分为几个步骤（见图9.6）。通过前五个步骤，机器人末端执行器（以及钻头）被带到脊柱上的期望位姿，在那里可以执行实际的钻孔动作。这个期望位姿在下面被称为钻框架。钻孔动作本身发生在第五步。之后，机器人可以在第6步和第7步中从患者身上移除。在每个步骤中，选择不同的笛卡尔刚度矩阵。

• 步骤1：在第一步中，机器人手臂具有零笛卡尔刚度，因此可以由用户自由移动。在这种模式下，机器人可以被带入适当的起始位置。- 步骤2：在这种模式下，期望的刚度矩阵被定义为使得机器人末端执行器可以在朝向钻孔轴的方向上移动。刚度矩阵有五个正特征值，剩余第六个特征值设为零。因此，机器人不会自主移动到钻框架，而是可以由外科医生以直观的方式朝向期望位姿移动。末端执行器的方向会自动调整。当末端执行器到钻孔轴的距离小于某个阈值时，系统切换到下一步。- 步骤3：与上一步相比，现在还规定了沿钻孔轴方向的刚度，并且刚度值增加到中等水平。末端执行器现在可以被拉向钻框架。如果末端执行器到钻框架的距离下降到某个阈值以下，系统进入下一步。- 步骤4：现在期望的刚度矩阵的所有特征值都是正的。此外，还施加一个小的吸力，以进一步将末端执行器拉向钻框架。当末端执行器与钻框架之间的距离足够小时，进入下一步。- 步骤5：在这个钻孔步骤中，沿钻孔轴方向的刚度被设置为零，以允许外科医生手动钻孔。在开始钻孔之前，末端执行器被锁定在相对于所有其他方向的钻框架中。通过这种方式，钻头的位姿被保持，但外科医生可以在钻孔轴方向上进行进给运动。- 步骤6：钻孔完成后，刚度被设置为非常低的值，并且机器人可以被移开。手术中钻头被移除。- 步骤7：最后，机器人被移回其起始位置。

该程序概述了椎弓根钻孔应用的基本步骤。基于所提出的阻抗控制概念，如果需要，该程序可以适应外科医生的需求。

9.3 柔顺移动操作

在本书中，分析了机器人手臂的笛卡尔阻抗控制问题。在许多应用中，期望将机器人手臂与移动平台相结合，以扩展可达工作空间。在移动机器人仅具有完整约束的情况下，可以将第3章的刚体阻抗控制概念直接扩展到这样的系统（例如，参见HK00, KYC+95）。在移动机器人也具有非完整约束的情况下，问题则更加困难。

在某些应用中，例如之前描述的擦桌子，DLR轻型机器人的这种组合也是期望的。在本节中，介绍了一种简单的（即相当实用的）解决方案，说明如何将笛卡尔阻抗控制器与非完整移动平台相结合。

所考虑的系统如图9.7所示。移动平台有四个可转向轮。与脚轮相反，转向轴和车轮的旋转轴相交。因此，该系统是非完整的Bla01。下面考虑两种不同的情况。在第一个场景中，笛卡尔阻抗是相对于世界固定坐标系定义的。在第二种情况下，它则是相对于固定在移动平台上的坐标系定义的。

情况1： 对于组合平台-手臂系统的控制，采用了一种相当实用的方法。注意，为了简化表述，以下解释仅限于平移运动，但可以很容易地扩展到末端执行器方向。设移动平台的位置为 $\mathbf{r}_{mob}$ ，末端执行器位置为 $\mathbf{r}_{eef} = \mathbf{r}_{mob} + \mathbf{p}_{st}(\mathbf{q})$ 。手臂相对于移动平台的初始位置为 $\mathbf{p}_{st}(\mathbf{q}_0)$ 。为了控制移动平台，使用了速度控制器，其中手臂和移动平台之间的交互被视为扰动。机器人手臂的控制器则基于相对于世界固定坐标系的末端执行器位置 $\mathbf{r}_{eef}$ 设计的笛卡尔阻抗控制器。移动平台速度控制器的设定点 $\mathbf{v}_{mob,des}$ 然后通过简单的反馈律 $\mathbf{v}_{mob,des} = k_{mob}(\mathbf{p}_{st}(\mathbf{q}) - \mathbf{p}_{st}(\mathbf{q}_0))$ 命令，其中 $k_{mob}$ 是正控制增益，使得机器人手臂远离其（平台固定的）工作空间边界。这里假设在初始配置中，末端执行器远离工作空间边界。这个简单反馈律的控制增益 $k_{mob}$ 设置得很小，使得平台的速度保持很小，并且平台的运动在笛卡尔阻抗控制器的设计中可以忽略。

所描述的平台-手臂系统控制策略非常简单，其动机是为非完整移动操纵器系统寻找笛卡尔阻抗控制器的实用扩展。在实践中，这种控制策略在擦桌子等应用中效果很好。然而，从理论角度来看，这个概念是不够的，实际上将笛卡尔阻抗控制器扩展到具有非完整约束的系统可以被认为是未来工作的一个挑战性问题。

情况2： 图9.8显示了移动操纵器系统在一个应用中的情况，其中笛卡尔阻抗控制器是相对于平台固定坐标系而不是世界固定坐标系实现的。所使用的笛卡尔阻抗控制器是根据基于奇异摄动的方法（第5章）实现的，笛卡尔坐标根据改进的欧拉角表示（见第3.5.2节）选择。方向刚度值选择得很高（100 Nm/rad），而平移刚度值选择为中等硬度（500 N/m）。移动平台如上所述进行控制。这导致机器人手臂具有类似操纵杆的行为。通过拉动或推动末端执行器，移动操纵器系统可以移动，因为速度控制器跟随末端执行器与虚拟平衡点的偏差，该虚拟平衡点现在固定在移动平台上。

9.4 开门

在下一个应用中，机器人应该打开一扇门并穿过它。因此，DLR-Lightweight-Robot-II与移动平台和灵巧的四指手（DLR-Hand-II）结合使用，另见OBH+05。同一个系统之前被用于使用视觉系统进行物体识别的一些瓶子和玻璃杯操作任务HOB+04。虽然那些操作任务是用手臂的位置控制器完成的，但在考虑的开门的任务中，手臂的阻抗控制要好得多。使用阻抗控制器（具有中等刚度值）可以在环境不完全已知的情况下保持较低的接触力。

与该应用的建模假设相反，机器人手臂安装在移动底座上。然而，假设移动底座的运动足够慢且平滑，使得平台运动对手臂的动态影响可以忽略。此外，末端执行器（即四指手）以简化形式作为刚体包含在机器人模型中。这意味着手指运动对手臂动态的影响也被忽略。因此，机器人手臂作为一个独立的系统进行控制，并且移动底座、手臂和人工手之间的所有动态耦合都被忽略。基于这些建模假设，所提出的笛卡尔阻抗控制器可以不加修改地使用。为了实现这个应用，使用了第7章的基于无源性的控制器结构。

在门可以被打开之前，机器人定位其相对于门铰链的相对位置，并且门把手通过计算机视觉检测。手臂基于计算机视觉的坐标接近把手，使得手被定位在把手的右侧，如图9.9所示。然后整个应用分为两个子任务。门把手的转动和打开（子任务1）以及穿过门直到其足够宽打开（子任务2）。此外，对于人工手的四个手指，使用了关节级阻抗控制器（具有中等刚度），其平衡配置如图9.9所示。在整个应用中，手保持在这种配置中。

在操作过程中，应观察手与门把手之间的接触力。由于机器人在此设置中没有连接到末端执行器的力-力矩传感器，因此必须从其他测量值估计接触力。因此，进行粗略的准静态估计如下。在静态平衡中，外部扭矩 $\tau_{ext}$ 可以从关节扭矩 $\tau$ 的测量值和重力扭矩 $\mathbf{g}(\mathbf{q})$ 的计算通过 $\boldsymbol{\tau}_{ext} = \boldsymbol{\tau} - \mathbf{g}(\mathbf{q})$ 计算。如果假设外部扭矩是通过广义力 $\mathbf{F}_{ext}$ 施加的，那么关系式 $\mathbf{J}(\mathbf{q})^{T} \mathbf{F}_{ext} = \boldsymbol{\tau} - \mathbf{g}(\mathbf{q})$ 成立。这个方程可以用于粗略的（即准静态的） $\mathbf{F}_{ext}$ 估计。广义接触力的估计允许实现简单的停止条件以建立接触。为简单起见，这里使用 $\mathbf{F}_{ext}$ 平移分量的欧几里得范数作为这些停止条件。

门把手的转动和打开分为几个步骤，如图9.10所示。对于实际的阻抗实现，应用了第3.5.2节中基于四元数的刚度。通过前两个步骤，建立了手与把手的接触（图9.10中的步骤1和2）。这两个步骤中的每一个在估计接触力超过10 N的预定义阈值时结束。然后是实际的门把手转动运动（步骤3），当接触力超过另一个阈值30 N（表示把手已转到其最大角度）时结束。之后，通过向门表面法线方向的水平运动来打开门（步骤4），并且把手被转回其静止配置（步骤5和6）。现在，开门应用的第一部分完成了。

在第二部分中，门应被移动直到完全打开，以便移动平台可以穿过。对于机器人手臂，使用了一个笛卡尔阻抗控制器，该控制器允许在移动平台穿过门铰链时将门保持在一定距离。期望的笛卡尔阻抗行为通过如下势能函数定义。设 $\mathbf{r}(\mathbf{q}) = (r_x(\mathbf{q}), r_y(\mathbf{q}), r_z(\mathbf{q}))^T$ 表示末端执行器相对于平台固定坐标系的位置。期望的阻抗应被设计为使得末端执行器保持其初始高度，并且可以沿着固定在移动平台上的水平圆形路径自由移动。

通过选择正刚度因子 $k_{r}$ 和 $k_{z}$ ，可以很容易地通过势能函数来表述：

$$\begin{array}{l} V_{d}(\mathbf{q}) = k_{r} \left( \sqrt{r_{x}(\mathbf{q})^{2} + r_{y}(\mathbf{q})^{2}} - d_{0} \right)^{2} + k_{z} (r_{z}(\mathbf{q}) - r_{z}(\mathbf{q}_{0}))^{2}, \\ d_{0} = \sqrt{r_{x}(\mathbf{q}_{0})^{2} + r_{y}(\mathbf{q}_{0})^{2}}, \end{array} \quad (1)$$

它取代了第3.5节中刚度的势能函数。这个势能函数没有孤立的极小值，而是沿圆形路径取最小值（见图9.11）。根据这个势能函数，可以通过简单地计算 $V_{d}(\mathbf{q})$ 关于 $\mathbf{q}$ 的微分来实现平移刚度。除了这种平移刚度外，没有实现方向刚度。这样做的原因是，当门把手被人工手抓住时，末端执行器的方向将自动调整到把手方向。通过使用上述势能函数来实现期望的阻抗行为，并结合适当设计的阻尼项，末端执行器将被保持在与平台固定点 $\mathbf{r}_{c}$ 的起始距离处。因此，当移动机器人穿过门铰链时，手臂将自动将门移开（见图9.12）。注意，这不需要任何关于门宽度或门旋转轴位置的知识。

9.5 双臂操作

最后，作为最后一个应用，展示了DLR人形操纵器“Justin”OEF+06的双臂操作。该系统由两个安装在可移动躯干上的DLR-Lightweight-Robots-III、两只人工手和一个安装在二自由度平移-倾斜单元上的传感器头组成（见图9.13）。总体而言，Justin有43个自由度。此外，除了常见的电机位置传感器外，手臂、手和躯干的所有关节都配备了关节扭矩传感器。以下讨论将仅限于手臂和躯干的控制。利用铰接式手的进一步扩展可以在WmOH06, WmOH07中找到。

所考虑的任务是用双臂协调抓取和操作一个大型物体。作为基础控制方法，应用了第7.2.1节的控制器。然而，对于双臂系统，使用比连接到末端执行器的单个空间弹簧更一般的阻抗结构是有用的。因此，采用了一个更抽象的观点。在（7.10）中，刚度项现在被一个更一般的势能函数 $V_{imp}(\theta)$ 的微分所取代，阻尼通过关节空间中适当的阻尼矩阵 $D_j(\theta)$ 实现，即代替（7.10），控制律为：

$$\mathbf{u}_{imp} = -\frac{\partial V_{imp}(\boldsymbol{\theta})}{\partial \boldsymbol{\theta}} - D_j(\boldsymbol{\theta}) \dot{\boldsymbol{\theta}} \quad (9.1)$$

为了设计势能函数 $V_{imp}(\theta)$ ，应根据一个空间弹簧的势能函数 $V_{S}(\cdot)$ 作为基本构建块。这个势能函数 $V_{S}(h_{s1}, h_{s2}, \mathcal{K})$ 依赖于两个框架 $h_{s1} := [R_{s1}, p_{s1}] \in SE(3)$ 和 $h_{s2} := [R_{s2}, p_{s2}] \in SE(3)$ （弹簧作用在它们之间），以及一些与配置无关的内部参数 $\kappa$ ，如刚度值或初始长度。这种空间弹簧的适当实现可以在Fasse和ZhangFB97、Caccavale和NataleCNSV99以及StramigioliSD01的工作中找到。对于Justin的双臂操作，这个势能函数被选为：

$$V_{S}(h_{s1}, h_{s2}, \mathcal{K}) := \frac{1}{2} e_{12}^{T} \mathcal{K}_{t} e_{12} + 2 \epsilon_{12}^{T} \mathcal{K}_{r} \epsilon_{12}, \quad (9.2)$$

其中 $\mathbf{e}_{12} := \mathbf{p}_{s1} - \mathbf{p}_{s2} - \mathbf{p}_{12,0}$ 是位移 $\mathbf{p}_{s1} - \mathbf{p}_{s2}$ 与其期望初始长度 $\mathbf{p}_{12,0}$ 的平移偏差。矩阵 $\mathbf{K}_{t} \in \mathbb{R}^{3\times 3}$ 和 $\mathbf{K}_{r} \in \mathbb{R}^{3\times 3}$ 代表平移和旋转刚度矩阵。此外， $\boldsymbol{\epsilon}_{12}$ 表示 $\mathbf{R}_{v1,v2} := \mathbf{R}_{1,v1}^{T} \mathbf{R}_{s1}^{T} \mathbf{R}_{s2} \mathbf{R}_{2,v2}$ 的单位四元数表示，其中 $\mathbf{R}_{1,v1} \in SO(3)$ 和 $\mathbf{R}_{2,v2} \in SO(3)$ 是弹簧的“旋转初始长度”。因此， $\kappa$ 包含刚度矩阵 $\mathbf{K}_{t}$ 、 $\mathbf{K}_{r}$ 以及平移和旋转初始长度 $\mathbf{p}_{12,0}$ 、 $\mathbf{R}_{1,v1}$ 、 $\mathbf{R}_{2,v2}$ 。对于阻尼矩阵 $\mathbf{D}_{j}(\boldsymbol{\theta})$ 的设计，可以再次利用第3.3节中讨论的方法。

左右手臂相对于躯干基座的正向运动学映射可以通过（2.3）计算，并分别表示为 $\mathbf{h}_{sr}(\cdot)$ 和 $\mathbf{h}_{sl}(\cdot)$ 。

基于上述 $V_{S}(\cdot)$ 的定义，可以通过使用两个空间弹簧 $\kappa_{r}$ 和 $\kappa_{l}$ 分别为左右手臂设计一个双臂操纵器的阻抗行为，这两个弹簧将末端执行器连接到虚拟平衡框架 $\mathbf{h}_{r,d}$ 和 $\mathbf{h}_{l,d}$ 。如果弹簧实现为完整的6D弹簧（即具有满秩的平移和旋转刚度矩阵），那么仅仅通过这两个弹簧就可以影响手臂的完整笛卡尔运动。对于某些应用，如果运动的一部分是通过手臂之间的额外耦合弹簧 $\kappa_{c}$ 来定义的，则更有用。显然，在这种配置中（如图9.14所示），各个弹簧和耦合弹簧的初始长度以及刚度值都应该以兼容的方式选择，使得弹簧不会相互干扰。通过（9.1）实现这种双臂阻抗的完整势能函数由下式给出：

$$\begin{array}{r} V_{imp}(\boldsymbol{\theta}) = V_S(\mathbf{h}_{sr}(\boldsymbol{\theta}), \mathbf{h}_{r,d}, \mathcal{K}_r) + V_S(\mathbf{h}_{sl}(\boldsymbol{\theta}), \mathbf{h}_{l,d}, \mathcal{K}_l) + V_S(\mathbf{h}_{sr}(\boldsymbol{\theta}), \mathbf{h}_{sl}(\boldsymbol{\theta}), \mathcal{K}_c). \end{array} \quad (9.3)$$

通过如图9.14所示的阻抗结构，可以实现不同的行为，从通过 $\mathcal{K}_r$ 和 $\mathcal{K}_l$ 独立控制手臂到基于 $\mathcal{K}_c$ 的纯耦合。然而，这种结构的一个缺点是弹簧不应独立设计，而应如上所述以兼容的方式设计。

不依赖于连接到末端执行器的两个独立弹簧，可以定义一个虚拟对象框架 $\mathbf{h}_{so}(\mathbf{h}_{sr}(\boldsymbol{\theta}), \mathbf{h}_{sl}(\boldsymbol{\theta}))$ ，它取决于两个末端执行器框架，如图9.15所示。这样的虚拟对象被Natale在Nat03中使用。对象框架被选为 $\mathbf{h}_{so} = [\mathbf{R}_{so}, \mathbf{p}_{so}]$ ，其中 $\mathbf{p}_{so} = \frac{1}{2} (\mathbf{p}_{sr} + \mathbf{p}_{sl})$ 和 $R_{so} = R_{sr} \bar{R}_{rl}$ ，其中 $\bar{R}_{rl}$ 表示旋转矩阵 $R_{sr}^{-1} R_{sl}$ 的一半。这个虚拟对象通过空间弹簧 $\mathcal{K}_{o}$ 连接到虚拟平衡位姿 $h_{o,d}$ 。与耦合刚度结合，可以直观地定义一个对用双臂抓取大型物体有用的阻抗行为。相关的势能函数由下式给出：

$$\begin{array}{r} V_{imp}(\boldsymbol{\theta}) = V_{S}(h_{so}(h_{sr}(\boldsymbol{\theta}), h_{sl}(\boldsymbol{\theta})), h_{o,d}, \mathcal{K}_{o}) + V_{S}(h_{sr}(\boldsymbol{\theta}), h_{sl}(\boldsymbol{\theta}), \mathcal{K}_{c}). \end{array} \quad (9.4)$$

作为这些阻抗行为的一个示例任务，已经实现了抓取和操纵一个圆柱形垃圾桶。在实验中，耦合弹簧 $\mathcal{K}_{c}$ 实现了一个仅作用在水平面上的平移刚度。弹簧 $\mathcal{K}_{l}$ 和 $\mathcal{K}_{r}$ 则根据手臂的垂直运动实现了平移刚度，以及方向刚度。对于手，使用了简单的关节阻抗控制器。这样，抓取基本上是由手臂完成的。

---

10 控制器比较与结论

在第5章至第7章中，详细讨论了实现柔性关节机器人笛卡尔阻抗行为的几种控制方法。这些控制器的实验评估已经在第8章中给出。最后，在这个结论章节中，将给出一个总体比较，突出所提出控制方法各自的优缺点。表10.1总结了不同控制器的特性。这样的比较在某些方面显然是主观的，因此对评级的以下几句评论是必要的。

表10.1. 所提出控制器的比较

	奇异摄动	解耦与反步法	基于无源性
模型	奇异摄动	简化	简化（+D）
理论论证	-	+	+
鲁棒性	0	-	+
跟踪与惯性整形	+	+	-
低复杂度	+	-	+
状态变量	τ, τ, q, q	τ, τ, q, q	θ, θ, q, q
反馈变量	τ, τ, q, q	τ, τ, q, q, q(3)	τ, (τ), θ, θ

模型： 在奇异摄动方法中，控制器设计仅基于一个近似模型，其中内扭矩控制回路在阻抗控制器的设计中被忽略。而其他方法则参考柔性关节机器人的简化模型。对于基于无源性的控制器，甚至可以轻松扩展到包含关节阻尼的模型。

理论论证： 这个评级是所考虑模型假设的结果。在这里，基于奇异摄动的控制器显然有其主要缺点。

鲁棒性： 基于无源性控制器的结构特性意味着对连杆侧动力学建模不确定性的鲁棒性。对于基于解耦和反步法的控制器，如果关节加速度和加加速度不能测量而必须从其他测量值计算，则无法进行鲁棒性分析。在调节情况下，基于奇异摄动的控制器具有与基于无源性控制器相似的鲁棒性特性。但这仅适用于系统的近似奇异摄动模型。

跟踪与惯性整形： 对于基于奇异摄动的控制器以及基于解耦和反步法的控制器，可以以类似的方式扩展到跟踪情况和惯性整形，但会导致更复杂的控制器。基于无源性的控制器最初是为调节情况设计的。给出了跟踪情况的扩展，但这里控制器还需要连杆加速度和加加速度的测量。

低复杂度： 复杂度评级指的是控制器的实现成本。在这里，基于解耦和反步法的控制器由于其额外的连杆加速度和加加速度反馈而具有主要缺点。虽然这些信号原则上可以基于其他测量数据和模型参数计算，但从实际角度来看，这种计算很关键。

状态变量和反馈变量： 在分析基于奇异摄动的、基于解耦的和反步法的控制器时，使用了扭矩和连杆侧位置。这些控制器被（隐含地）设计为具有内扭矩控制环和外阻抗控制环的级联控制结构形式。而基于无源性控制器的分析则使用电机和连杆的状态变量。术语反馈变量则指的是实际实现控制器所需的变量。由于连杆侧加速度和加加速度通常无法测量，它们必须基于模型和状态变量计算。

总而言之，可以说基于解耦和反步法的控制器在实践中难以实现，但它们代表了（简化）柔性关节机器人模型阻抗控制问题的一个完整解决方案，包括跟踪情况和惯性整形。而从实际角度来看，奇异摄动方法和基于无源性的控制器因其低复杂度和鲁棒性特性而更受青睐。虽然基于奇异摄动的控制器允许直接扩展到跟踪情况和惯性整形，但其基于Tychonov定理的理论论证较弱。另一方面，基于无源性的控制器显然在理论论证和低复杂度方面有其主要优势。

通过所提出的控制器，为柔性关节机器人的笛卡尔阻抗控制问题提供了几种解决方案，并且可以用这些控制器处理广泛的应用。一些典型应用在第9章中讨论。

如果期望阻抗的定义与本书中考虑的不同，通常可以通过仅进行微小的修改来相应地调整控制器，类似于第9.1节中的示例。然而，如何为给定任务定义期望阻抗的问题本身就是一个有趣的问题。

---

参考文献

</span>[Arn89] Arnold, V.I.: Mathematical Methods of Classical Mechanics, 2nd edn. Springer, Heidelberg (1989)
[AS01] Albu-Schäffer, A.: Regelung von Robotern mit elastischen Gelenken am Beispiel der DLR-Leichtbauarme. PhD thesis, Technische Universität München (2001)
</span>[ASFS+04] Albu-Schäffer, A., Fischer, M., Schreiber, G., Schoeppe, F., Hirzinger, G.: Soft robotics: What cartesian stiffness can we obtain with passively compliant, uncoupled joints? In: IEEE/RSJ International Conference on Intelligent Robots and Systems, pp. 3295–3301 (2004)
[ASH00] Albu-Schäffer, A., Hirzinger, G.: State feedback controller for flexible joint robots: A globally stable approach implemented on DLR’s light-weight robots. In: IEEE/RSJ International Conference on Intelligent Robots and Systems, pp. 1087–1094 (2000)
</span>[ASH01a] Albu-Schäffer, A., Hirzinger, G.: A globally stable state-feedback controller for flexible joint robots. Journal of Advanced Robotics, Special Issue: Selected Papers from IROS 2000 15(8), 799–814 (2001)
[ASH01b] Albu-Schäffer, A., Hirzinger, G.: Parameter identification and passivity based joint control for a 7dof torque controlled light weight robot. In: IEEE International Conference on Robotics and Automation, pp. 1087–1093 (2001)
</span>[ASmOH07] Albu-Schäffer, A., Ott, C., Hirzinger, G.: A unified passivity-based control framework for position, torque and impedance control of flexible joint robots. The International Journal of Robotics Research 26(1), 23–39 (2007)
[ASOFH03] Albu-Schäffer, A., Ott, C., Frese, U., Hirzinger, G.: Cartesian impedance control of redundant robots: Recent results with the DLR-light-weight-arms. In: IEEE International Conference on Robotics and Automation, pp. 3704–3709 (2003)
</span>[ASOH04a] Albu-Schäffer, A., Ott, C., Hirzinger, G.: Passivity based cartesian impedance control for flexible joint manipulators. In: IFAC Symposium on Nonlinear Control Systems (NOLCOS) (2004)
[ASOH04b] Albu-Schäffer, A., Ott, C., Hirzinger, G.: A passivity based cartesian impedance controller - part II: Full state feedback, impedance design and experiments. In: IEEE International Conference on Robotics and Automation, pp. 2666–2672 (2004)

</span>[Bai85] Baillieul, J.: Kinematic programming alternatives for redundant manipulators. In: IEEE International Conference on Robotics and Automation, pp. 722–728 (1985)
[BCLM01] Bloch, A.M., Chang, D.E., Leonard, N.E., Marsden, J.E.: Controlled lagrangians and the stabilization of mechanical systems ii: Potential shaping. IEEE Transactions on Automatic Control 46, 1556–1571 (2001)
</span>[BK00] Bruyninckx, H., Khatib, O.: Gauss’ principle and the dynamics of redundant and constrained manipulators. In: IEEE International Conference on Robotics and Automation, pp. 2563–2568 (2000)
[Bla01] Blaimschein, P.: Modellierung und Regelung mobiler Robotersysteme. PhD thesis, Johannes Kepler Universität Linz (2001)
</span>[BLM00] Bloch, A.M., Leonard, N.E., Marsden, J.E.: Controlled lagrangians and the stabilization of mechanical systems i: The first matching theorem. IEEE Transactions on Automatic Control 45(12), 2253–2270 (2000)
[BM92] Basile, G., Marro, G.: Controlled and Conditioned Invariants in Linear Systems Theory. Prentice Hall, Englewood Cliffs (1992)
</span>[BOL95] Brogliato, B., Ortega, R., Lozano, R.: Global tracking controllers for flexible-joint manipulators: a comparative study. Automatica 31(7), 941–956 (1995)
[Boo03] Boothby, W.M.: An Introduction to Differentiable Manifolds and Riemannian Geometry, 2nd edn. Academic Press, Elsevier Science (2003)
</span>[BOvdS02] Blankenstein, G., Ortega, R., van der Schaft, A.J.: The matching conditions of controlled lagrangians and interconnection and damping assignment passivity based control. International Journal of Control 75(9), 645–665 (2002)
[BRT01] Bicchi, A., Lodi Rizzini, S., Tonietti, G.: Compliant design for intrinsic safety: General issues and preliminary design. In: IEEE/RSJ International Conference on Intelligent Robots and Systems, pp. 1864–1869 (2001)
</span>[BS98] Beccari, G., Stramigioli, S.: Impedance control as merging mechanism for a behavior-based architecture. In: IEEE International Conference on Robotics and Automation, pp. 1429–1434 (1998)
[BTBP03] Bicchi, A., Tonietti, G., Bavaro, M., Piccigallo, M.: Variable stiffness actuators for fast and safe motion control. In: Siciliano, B., Khatib, O., Groen, F.C.A. (eds.) Proceedings of ISRR 2003, Springer Tracts in Advanced Robotics (STAR). Springer, Heidelberg (2003)
</span>[CH89] Colgate, E., Hogan, N.: An analysis of contact instability in terms of passive physical equivalents. In: IEEE International Conference on Robotics and Automation, pp. 404–409 (1989)
[CK95] Chang, K.-S., Khatib, O.: Manipulator control at kinematic singularities: A dynamically consistent strategy. In: IEEE/RSJ International Conference on Intelligent Robots and Systems, pp. 84–88 (1995)
</span>[CK02] Chen, S.F., Kao, I.: Geometric approach to the conservative congruence transformation (cct) for robotic stiffness control. In: IEEE International Conference on Robotics and Automation, pp. 544–549 (2002)
[CNSV99] Caccavale, F., Natale, C., Siciliano, B., Villani, L.: Six-dof impedance control based on angle/axis representations. IEEE Transactions on Robotics and Automation 15(2), 289–299 (1999)
</span>[CW93] Chen, Y.-C., Walker, I.D.: A consistent null-space based approach to inverse kinematics of redundant robots. In: IEEE International Conference on Robotics and Automation, pp. 374–381 (1993)
[DL00] De Luca, A.: Feedforward/feedback laws for the control of flexible robots. In: IEEE International Conference on Robotics and Automation, pp. 233-240 (2000)
</span>[DLL98] De Luca, A., Lucibello, P.: A general algorithm for dynamic feedback linearization of robots with elastic joints. In: IEEE International Conference on Robotics and Automation, pp. 504-510 (1998)
[DLSZ05] De Luca, A., Siciliano, B., Zollo, L.: Pd control with on-line gravity compensation for robots with elastic joints: Theory and experiments. Automatica 41(10), 1809-1819 (2005)
</span>[DLT96] De Luca, A., Tomei, P.: Theory of Robot Control. In: Elastic Joints, pp. 179-218. Springer, London (1996)
[Duf90] Duffy, J.: The fallacy of modern hybrid control theory that is based on orthogonal complements of twist and wrench spaces. Journal of Robotic Systems 7(2), 139-144 (1990)
</span>[dWSB96] de Wit, C., Siciliano, B., Bastin, G. (eds.): Theory of Robot Control. Communications and Control Engineering. Springer, London (1996)
[Fas97] Fasse, E.D.: On the spatial compliance of robotic manipulators. ASME Journal of Dynamic Systems, Measurement, and Control 119, 839-844 (1997)
</span>[FB97] Fasse, E.D., Broenink, J.F.: A spatial impedance controller for robotic manipulation. IEEE Transactions on Robotics and Automation 13(4), 546-556 (1997)
[Fea04] Featherstone, R.: An empirical study of the joint space inertia matrix. The International Journal of Robotics Research 23(9), 856-871 (2004)
</span>[FH95] Fasse, E.D., Hogan, N.: Control of physical contact and dynamic interaction. In: 7th International Symposium of Robotics Research, pp. 28-38 (1995)
[FLMR95] Fliess, M., Levine, J., Martin, P., Rouchon, P.: Flatness and defect of nonlinear systems: introductory theory and examples. International Journal of Control 61(6), 1327-1361 (1995)
</span>[Fra97] Frankel, T.: The Geometry of Physics. Cambridge University Press, Cambridge (1997)
[Ge96] Ge, S.S.: Adaptive controller design for flexible joint manipulators. Automatica 32(2), 273-278 (1996)
</span>[Gei03] Geist, C.: Untersuchung des Einsatzes von Gelenk-Beschleunigungssensoren zur Regelung eines elastischen Robotergekenkes. Master’s thesis, Fachhochschule Niederrhein (2003)
[GFG99] Goldsmith, P.B., Francis, B.A., Goldenberg, A.A.: Stability of hybrid position/force control applied to manipulators with flexible joints. International Journal of Robotics and Automation 14(4), 146-160 (1999)
</span>[GHS89] Ghorbel, F., Hung, J.Y., Spong, M.W.: Adaptive control of flexible joint manipulators. In: IEEE International Conference on Robotics and Automation, pp. 1188-1193 (1989)
[Har97] Harville, D.: Matrix Algebra from a Statistician’s Perspective. Springer, Heidelberg (1997)
</span>[HG93] Hsu, L., Guenther, R.: Variable structure adaptive cascade control of multi-link robot manipulators with flexible joints: the case of arbitrary uncertain flexibilities. In: IEEE International Conference on Robotics and Automation, pp. 340-345 (1993)
[HHS89] Hsu, P., Hauser, J., Sastry, S.: Dynamic control of redundant manipulators. Journal of Robotic Systems 6(2), 133-148 (1989)
</span>[HJ90] Horn, R.A., Johnson, C.R.: Matrix Analysis. Cambridge University Press, Cambridge (1990)
[HK00] Holmberg, R., Khatib, O.: Development and control of a holonomic mobile robot for mobile manipulation tasks. The International Journal of Robotic Research 19(11), 1066-1074 (2000)
</span>[HOB+04] Hillenbrand, U., Ott, C., Brunner, B., Borst, C., Hirzinger, G.: Towards service robots for the human environment: the robuter. In: Mechatronics and Robotics (MECHROB), pp. 1497-1502 (2004)
[Hog85a] Hogan, N.: Impedance control: An approach to manipulation, part I - theory. ASME Journal of Dynamic Systems, Measurement, and Control 107, 1-7 (1985)
</span>[Hog85b] Hogan, N.: Impedance control: An approach to manipulation, part II - implementation. ASME Journal of Dynamic Systems, Measurement, and Control 107, 8-16 (1985)
[Hog85c] Hogan, N.: Impedance control: An approach to manipulation, part III - applications. ASME Journal of Dynamic Systems, Measurement, and Control 107, 17-24 (1985)
</span>[HV91] Huang, M.Z., Varma, H.: Optimal rate allocation in kinematically redundant manipulators - the dual projection method. In: IEEE International Conference on Robotics and Automation, pp. 702-707 (1991)
[IKO96] Iggidr, A., Kalitine, B., Outbib, R.: Semidefinite lyapunov functions: Stability and stabilization. Mathematics of Control, Signals, and Systems 9, 95-106 (1996)
</span>[KG97] Kircanski, N.M., Goldenberg, A.A.: An experimental study of nonlinear stiffness, hysteresis, and friction effects in robotic joints with harmonic drives and torque sensors. The International Journal of Robotics Research 16(2), 214-239 (1997)
[Kha87] Khatib, O.: A unified approach for motion and force control of robot manipulators: The operational space formulation. IEEE Journal of Robotics and Automation 3(1), 1115-1120 (1987)
</span>[Kha02] Khalil, H.K.: Nonlinear Systems, 3rd edn. Prentice Hall, Englewood Cliffs (2002)
[KKO86] Kokotovic, P.V., Khalil, H.K., O’Reilly, J.: Singular Perturbation Methods in Control: Analysis and Design. Academic Press, London (1986)
</span>[Koe01] Koeppe, R.: Robot Compliant Motion based on Human Skill. PhD thesis, ETH Zurich (2001)
[KS98] Kelly, R., Santibanez, V.: Global regulation of elastic joint robots based on energy shaping. IEEE Transactions on Automatic Control 43(10), 1451-1456 (1998)
</span>[KS02a] Kugi, A., Schlacher, K.: Analyse und synthese nichtlinearer dissipativer systeme: Ein überblick (teil 1). at - Automatisierungstechnik 2, 63-69 (2002)
[KS02b] Kugi, A., Schlacher, K.: Analyse und synthese nichtlinearer dissipativer systeme: Ein überblick (teil 2). at - Automatisierungstechnik 3, 103-111 (2002)
</span>[Kug01] Kugi, A.: Non-linear Control Based on Physical Models. Springer, Heidelberg (2001)
[KYC+95] Khatib, O., Yokoi, K., Chang, K., Ruspini, D., Holmberg, R., Casal, A., Baader, A.: Force strategies for cooperative tasks in multiple mobile manipulation systems. In: Int. Symp. of Robotics Research (1995)
</span>[LB92] Lozano, R., Brogliato, B.: Adaptive control of robot manipulators with flexible joints. IEEE Transactions on Automatic Control 37(2), 174-181 (1992)
[LD88] Lipkin, H., Duffy, J.: Hybrid twist and wrench control for a robotic manipulator. ASME Journal of Mechanisms, Transmissions, and Automation in Design 110, 138-144 (1988)
</span>[LG95] Lin, T., Goldenberg, A.A.: Robust adaptive control of flexible joint robots with joint torque feedback. In: IEEE International Conference on Robotics and Automation, pp. 1229-1234 (1995)
[LG96] Lin, T., Goldenberg, A.A.: A unified approach to motion and force control of flexible joint robots. In: IEEE International Conference on Robotics and Automation, pp. 1115-1120 (1996)
</span>[LKCC91] Laurin-Kovitz, K.F., Colgate, J.E., Carnes, S.D.R.: Design of programmable passive impedance. In: IEEE International Conference on Robotics and Automation, pp. 1476-1481 (1991)
[LO95] Loria, A., Ortega, R.: On tracking control of rigid and flexible joint robots. Applied Mathematics and Computer Science, special issue on Mathematical Methods in Robotics 5(2), 101-113 (1995)
</span>[Lor01] Loria, A.: Cascaded nonlinear time-varying systems: analysis and design. Minicourse at the Congreso Internacional de Computacion, Cd. Mexico (2001)
[Mas81] Mason, M.T.: Compliance and force control for computer controlled manipulators. IEEE Transactions on Systems, Man, and Cybernetics 11(6), 418-432 (1981)
</span>[Mil63] Milnor, J.: Morse Theory. Princton University Press (1963)
[MLS94] Murray, R.M., Li, Z., Sastry, S.S.: A Mathematical Introduction to Robotic Manipulation. CRC Press, Boca Raton (1994)
</span>[MLSD99] Middelanis, I., Liehn, M., Steinmüller, L., Döhler, R.: OP-Handbuch - Grundlagen, Instrumentarium, OP-Ablauf, 2nd edn. Springer, Heidelberg (1999)
[Nat03] Natale, C.: Interaction Control of Robot Manipulators: Six-Degrees-of-Freedom Tasks. Springer Tracts in Advanced Robotics (STAR), vol. 3. Springer, Heidelberg (2003)
</span>[NSV99] Natale, C., Siciliano, B., Villani, L.: Spatial impedance control of redundant manipulators. In: IEEE International Conference on Robotics and Automation, pp. 1788-1793 (1999)
[NT92] Nicosia, S., Tomei, P.: A method to design adaptive controllers for flexible joint robots. In: IEEE International Conference on Robotics and Automation, pp. 701-706 (1992)
</span>[NT95] Nicosia, S., Tomei, P.: A global output feedback controller for flexible joint robots. Automatica 31(10), 1465-1469 (1995)
[OASH02] Ott, C., Albu-Schäffer, A., Hirzinger, G.: Comparison of adaptive and nonadaptive tracking control laws for a flexible joint manipulator. In: IEEE/RSJ International Conference on Intelligent Robots and Systems, pp. 2018-2024 (2002)
</span>[OASK+04a] Ott, C., Albu-Schäffer, A., Kugi, A., Stramigioli, S., Hirzinger, G.: Ein passivitätsbasierter ansatz zur kartesischen impedanzregelung von robotern mit elastischen gelenken. In: Robotik 2004, München, June 2004. VDI Berichte Nr. 1841, pp. 71-79. VDI/VDE-Gesellschaft Mess- und Automatisierungstechnik, VDI Verlag (2004)
[OASK+04b] Ott, C., Albu-Schäffer, A., Kugi, A., Stramigioli, S., Hirzinger, G.: A passivity based cartesian impedance controller - part I: Torque feedback and gravity compensation. In: IEEE International Conference on Robotics and Automation, pp. 2659-2665 (2004)
</span>[OASK+05] Ott, C., Albu-Schäffer, A., Kugi, A., Stramigioli, S., Hirzinger, G.: Cartesian impedance control of flexible joint robots: A passivity based approach. at-Automatisierungstechnik, 378-388 (2005)
[OASKH03] Ott, C., Albu-Schäffer, A., Kugi, A., Hirzinger, G.: Decoupling based cartesian impedance control of flexible joint robots. In: IEEE International Conference on Robotics and Automation, pp. 3101-3107 (2003)
</span>[OASKH05] Ott, C., Albu-Schäffer, A., Kugi, A., Hirzinger, G.: Cutting Edge Robotics. In: Cartesian Impedance Control of Flexible Joint Robots: A Decoupling Approach, Advanced Robotic Systems International, pp. 671-682. Pro literatur Verlag (2005)
[OASKH08] Ott, C., Albu-Schäffer, A., Kugi, A., Hirzinger, G.: On the passivity based impedance control of flexible joint robots. IEEE Transactions on Robotics 24(2), 416-429 (2008)
</span>[OBH+05] Ott, C., Borst, C., Hillenbrand, U., Brunner, B., Bauml, B., Hirzinger, G.: The robutler: Towards service robots for the human environment. In: Video, IEEE International Conference on Robotics and Automation Video Proceedings (2005)
[OCY97] Oh, Y., Chung, W.K., Youm, Y.: Extended impedance control of redundant manipulators using joint space decomposition. In: IEEE International Conference on Robotics and Automation, pp. 1080-1087 (1997)
</span>[OCY98] Oh, Y., Chung, W., Youm, Y.: Extended impedance control of redundant manipulators based on weighted decomposition of joint space. Journal of Robotic Systems 15(5), 231-258 (1998)
[OCYS98] Oh, Y., Chung, W.K., Youm, Y., Suh, I.H.: Motion/force decomposition of redundant manipulator and its application to hybrid impedance control. In: IEEE International Conference on Robotics and Automation, pp. 1441-1446 (1998)
</span>[OEF+06] Ott, C., Eiberger, O., Friedl, W., Bauml, B., Hillenbrand, U., Borst, C., Albu-Schäffer, A., Brunner, B., Hirschmüller, H., Kielhöfer, S., Konietschke, R., Suppa, M., Wimböck, T., Zacharias, F., Hirzinger, G.: A humanoid two-arm system for dexterous manipulation. In: IEEE-RAS International Conference on Humanoid Robots, pp. 276-283 (2006)
[OKL95] Ortega, R., Kelly, R., Loria, A.: A class of output feedback globally stabilizing controllers for flexible joints robots. IEEE Transactions on Robotics and Automatic 11(5), 766-770 (1995)
</span>[OL97] Oh, J.H., Lee, J.S.: Control of flexible joint robot system by backstepping design approach. In: IEEE International Conference on Robotics and Automation, pp. 3435-3440 (1997)
[OSGEB02] Ortega, R., Spong, M.W., Gomez-Estern, F., Blankenstein, G.: Stabilization of a class of underactuated mechanical systems via interconnection and damping assignment. IEEE Transactions on Automatic Control 47(8), 1218-1233 (2002)
</span>[OvdSME02] Ortega, R., van der Schaft, A.J., Maschke, B., Escobar, G.: Interconnection and damping assignment passivity-based control of port-controlled hamiltonian systems. Automatica 38(4), 585-596 (2002)
[Par99] Park, J.: Motion/force control of redundant manipulators. In: Proceedings of the Korean Automatic Control Conference, pp. 67-72 (1999)
</span>[PCY99] Park, J., Chung, W.K., Youm, Y.: Null space motion control of redundant manipulators based on the joint space decomposition. In: IEEE International Conference on Robotics and Automation, pp. 1694-1699 (1999)
[PCY00] Park, J., Chung, W.K., Youm, Y.: Weighted decomposition of joint space for redundant manipulators. In: IEEE International Conference on Robotics and Automation, pp. 3579-3584 (2000)
</span>[PP88] Paden, B., Panja, R.: Globally asymptotically stable “PD+” controller for robot manipulators. International Journal of Control 47(6), 1697-1712 (1988)
[RC81] Raibert, M.H., Craig, J.J.: Hybrid position/force control of manipulators. ASME Journal of Dynamic Systems, Measurement, and Control 103, 126-133 (1981)
</span>[SD01] Stramigioli, S., Duindam, V.: A novel spatial impedance controller for robot manipulators. In: IEEE International Conference on Robotics and Automation, pp. 2449-2454 (2001)
[Sel96] Selig, J.M.: Geometrical Methods in Robotics. Springer, New York (1996)
</span>[Sic90] Siciliano, B.: Kinematic control of redundant robot manipulators: A tutorial. Journal of Robotic Systems 7(6), 879-890 (1990)
[SJK97] Sepulchre, R., Jankovic, M., Kokotovic, P.V.: Constructive Nonlinear Control. Springer, Heidelberg (1997)
</span>[SK97a] Santibanez, V., Kelly, R.: A class of nonlinear PID global regulators for robot manipulators. In: IEEE International Conference on Robotics and Automation, pp. 3412-3417 (1997)
[SK97b] Santibanez, V., Kelly, R.: Strict lyapunov functions for PD+ control of robot manipulators. In: IEEE International Conference on Robotics and Automation, pp. 507-512 (1997)
</span>[SL87] Slotine, J.-J.E., Li, W.: On the adaptive control of robot manipulators. International Journal of Robotics Research 6(3), 49-59 (1987)
[Spo87a] Spong, M.W.: Modeling and control of elastic joint robots. ASME Journal of Dynamic Systems, Measurement, and Control 109, 310-319 (1987)
</span>[Spo87b] Spong, M.W.: Adaptive control of flexible joint manipulators. In: Proceedings of the Symposium on Robot Control, pp. 195-200 (1987)
[Spo89] Spong, M.W.: The control of flexible joint robots with joint torque feedback. In: Proc. of the 28th Conference on Decision and Control, pp. 1188-1193 (1989)
</span>[Spo95] Spong, M.W.: A combined adaptive-control and singular-perturbation approach to flexible-joint manipulators. In: Proc. of the 1995 American Control Conference, pp. 267-271 (1995)
[SRS88] Spong, M.W., Reed, J.S., Sastry, S.S.: Variable structure control of flexible joint manipulators. In: IEEE International Conference on Robotics and Automation, pp. 66-71 (1988)
</span>[SS90] Sontag, E.D., Sussmann, H.J.: Further comments on the stability of the angular velocity of a rigid body. Systems & Control Letters 12(3), 213-217 (1990)
[SS96] Sciavicco, L., Siciliano, B.: Modelling and Control of Robot Manipulators. Springer, London (1996)
</span>[Str01] Stramigioli, S.: From differentiable manifolds to interactive robot control. PhD thesis, Delft University of Technology (2001)
[Tom91] Tomei, P.: A simple pd controller for robots with elastic joints. IEEE Transactions on Automatic Control 35, 1208-1213 (1991)
</span>[TS96] Tuttle, T.D., Seering, W.P.: A nonlinear model of a harmonic drive gear transmission. IEEE Transactions on Robotics and Automation 12(3), 368-374 (1996)
[UASS04] Urbanek, H., Albu-Schaffer, A., Smagt, P.v.d.: Learning from demonstration repetitive movements for autonomous service robotics. In: IEEE/RSJ International Conference on Intelligent Robots and Systems, pp. 3495-3500 (2004)
</span>[vdS00] van der Schaft, A.: (L_{2})-Gain and Passivity Techniques in Nonlinear Control, 2nd edn. Springer, Heidelberg (2000)
[Vid93] Vidyasagar, M.: Nonlinear Systems Analysis, 2nd edn. Prentice Hall, Englewood Cliffs (1993)
</span>[WmOH06] Wimböck, T., Ott, C., Hirzinger, G.: Passivity-based object-level impedance control for a multifingered hand. In: IEEE/RSJ International Conference on Intelligent Robots and Systems, pp. 4621-4627 (2006)
[WmOH07] Wimböck, T., Ott, C., Hirzinger, G.: Impedance behaviors for two-handed manipulation: Design and experiments. In: IEEE International Conference on Robotics and Automation, pp. 4182-4189 (2007)
</span>[Yos90] Yoshikawa, T.: Foundations of Robotics. The MIT Press, Cambridge (1990)
[ZF00] Zhang, S., Fasse, E.D.: Spatial compliance modeling using a quaternion-based potential function method. Multibody System Dynamics 4, 75-101 (2000)
</span>[ZKRS02] Zinn, M., Khatib, O., Roth, B., Salisbury, J.K.: Towards a human-centered intrinsically-safe robotic manipulator. In: Second IARP - IEEE/RAS Joint Workshop on Technical Challenges for Dependable Robots in Human Environments, Toulouse (2002)
[ZLS04] Zollo, L., De Luca, A., Siciliano, B.: Regulation with on-line gravity compensation for robots with elastic joints. In: IEEE International Conference on Robotics and Automation, pp. 2687-2692 (2004)
</span>[ZSL+03] Zollo, L., Siciliano, B., De Luca, A., Guglielmelli, E., Dario, P.: Compliance control for a robot with elastic joints. In: IEEE International Conference of Advanced Robotics, pp. 1411-1416 (2003)
[ZSL+05] Zollo, L., Siciliano, B., De Luca, A., Guglielmelli, E., Dario, P.: Compliance control for an anthropomorphic robot with elastic joints: Theory and experiments. ASME Journal of Dynamic Systems, Measurements, and Control 127(3), 321-328 (2005)

---

附录 A

A.1 时变系统

在本节中，报告了一些与时变系统稳定性分析相关的定义和引理。本附录旨在澄清本书中使用的一些术语，而不深入细节。时变系统稳定性理论的更全面处理可以在许多非线性控制书籍中找到，例如Kha02或Vid93。

在时变系统的稳定性理论中，正定函数起着重要作用。因此，应定义正定性的概念。这首先针对时不变情况在定义A.1中完成，随后针对时变情况在定义A.2中完成。

定义 A.1. 给定一个包含原点 $\mathbf{0}\in \mathcal{D}$ 的开集 $\mathcal{D}\subseteq \mathbb{R}^{n}$ 。一个连续函数 $V:\mathcal{D}\to \mathbb{R}$ 被称为局部正半定的，如果它满足条件 $V(\mathbf{0}) = 0$ 和 $V(\mathbf{x}) \geq 0 \quad \forall \mathbf{x}\in \mathcal{D}$ 。如果满足更强的条件 $V(\mathbf{x}) > 0 \quad \forall \mathbf{x}\in \mathcal{D}\backslash \{0\}$ ，则称其为局部正定的。

如果 $\mathcal{D} = \mathbb{R}^{n}$ 并且存在一个常数 $r > 0$ 使得：

$$\inf_{||\mathbf{x}|| \geq r} V(\mathbf{x}) > 0$$

成立，则它是正定的。此外，如果满足条件：

$$\lim_{||\mathbf{x}|| \rightarrow \infty} V(\mathbf{x}) = \infty$$

则正定函数 $V(\mathbf{x})$ 被称为径向无界的。

如果 $- V(\mathbf{x})$ 是正（半）定的，则函数 $V(\mathbf{x})$ 被称为负（半）定的。

上述定义可以扩展到时变情况如下。为简洁起见，这里只给出全局定义。

定义 A.2. 一个（时变）函数 $V:\mathbb{R}\times \mathbb{R}^{n}\to \mathbb{R}$ 被称为正半定的，如果它满足条件 $V(t,\mathbf{0}) = 0 \quad \forall t\in \mathbb{R}$ 和 $V(t,\mathbf{x}) \geq 0 \quad \forall (t,\mathbf{x})\in \mathbb{R}\times \mathbb{R}^{n}$ 。

如果它额外满足条件：

$$V(t,\mathbf{x}) \geq W_{1}(\mathbf{x}) \quad \forall (t,\mathbf{x})\in \mathbb{R}\times \mathbb{R}^{n}$$

对于某个正定函数 $W_{1}:\mathbb{R}^{n}\to \mathbb{R}$ ，则称其为正定的。此外，如果 $W_{1}(\mathbf{x})$ 是径向无界的，则它是径向无界的。

最后，如果 $V(t,\mathbf{x})$ 满足条件：

$$V(t,\mathbf{x}) \leq W_{2}(\mathbf{x}) \quad \forall (t,\mathbf{x})\in \mathbb{R}\times \mathbb{R}^{n}$$

对于某个正定函数 $W_{2}:\mathbb{R}^{n}\to \mathbb{R}$ ，则称其为递减的。

在时变情况下，常微分方程的解取决于初始状态和初始时间 $t_{0}$ 。通常，期望所有收敛和稳定性性质在初始时间上都是一致的。下面给出特殊比较函数（即类 $\kappa$ （或 $\kappa_{\infty}$ ）函数）的定义。这些函数将允许对一致有界性和一致（渐近）稳定性进行透明的定义。

定义 A.3. 一个定义在区间 $\mathcal{A} = [0,a)$ （其中 $a > 0$ ）上的连续函数 $\alpha : \mathcal{A} \to \mathbb{R}$ ，如果它是严格递增的并且 $\alpha (0) = 0$ ，则称它属于类 $\kappa$ 。如果 $\mathcal{A} = [0,\infty)$ 并且当 $a \to \infty$ 时 $\alpha (a) \to \infty$ ，则称它属于类 $\kappa_{\infty}$ 。

类 $\kappa$ （或 $\kappa_{\infty}$ ）函数具有以下重要性质Kha02。

引理 A.4. 设 $\alpha_{1}:\mathcal{A}\to \mathbb{R}$ 和 $\alpha_{2}:\mathcal{A}\to \mathbb{R}$ 是类 $\kappa$ （或 $\kappa_{\infty}$ ）函数。则逆 $\alpha_{1}^{-1}$ 和复合 $\alpha_{1}\circ \alpha_{2}$ 也是类 $\kappa$ （或 $\kappa_{\infty}$ ）函数。

以下引理取自Kha02，给出了正定函数和类 $\kappa$ 函数之间的重要关系。

引理 A.5. 设 $V:\mathcal{D}\to \mathbb{R}$ 是一个连续正定函数，定义在包含原点的集合 $\mathcal{D}\subset \mathbb{R}^{n}$ 上，即 $\mathbf{0}\in \mathcal{D}$ 。设球 $B_{r} = \{\mathbf{x}\in \mathcal{D} \mid ||\mathbf{x}|| < r\} \subset \mathcal{D}$ 包含在该域中，其中 $r > 0$ 。则存在定义在区间 $[0,r)$ 上的类 $\kappa$ 函数 $\alpha_{1}$ 和 $\alpha_{2}$ ，使得不等式：

$$\alpha_{1}(||\mathbf{x}||) \leq V(\mathbf{x}) \leq \alpha_{2}(||\mathbf{x}||) \quad \forall \mathbf{x}\in \mathcal{B}_{r}$$

成立。在 $\mathcal{D} = \mathbb{R}^{n}$ 的情况下，函数 $\alpha_{1}$ 和 $\alpha_{2}$ 将定义在 $[0,\infty)$ 上，并且上述不等式将对所有 $\mathbf{x}\in \mathbb{R}^{n}$ 成立。此外，如果函数 $V(\mathbf{x})$ 是径向无界的，则可以找到 $\alpha_{1}$ 和 $\alpha_{2}$ 使得它们属于类 $\kappa_{\infty}$ 。

对于时变函数也可以给出类似的陈述，如下一个引理所示，该引理仅处理全局情况。

推论 A.6. 设 $V:\mathbb{R}\times \mathbb{R}^{n}\to \mathbb{R}$ 是一个连续、正定、径向无界且递减的函数。则存在类 $\kappa_{\infty}$ 函数 $\alpha_{1}$ 和 $\alpha_{2}$ ，使得不等式：

$$\alpha_{1}(||\mathbf{x}||) \leq V(t,\mathbf{x}) \leq \alpha_{2}(||\mathbf{x}||) \quad \forall (t,\mathbf{x})\in \mathbb{R}\times \mathbb{R}^{n}$$

成立。

证明. 从函数 $V(t,\mathbf{x})$ 是正定和径向无界的事实可以得出，存在一个正定且径向无界的函数 $W_{1}(\mathbf{x})$ 使得 $V(t,\mathbf{x}) \geq W_{1}(\mathbf{x}) \quad \forall (t,\mathbf{x})\in \mathbb{R}\times \mathbb{R}^{n}$ 。根据引理A.5，存在一个类 $\kappa_{\infty}$ 函数 $\alpha_{1}$ ，使得 $V(t,\mathbf{x}) \geq W_{1}(\mathbf{x}) \geq \alpha_{1}(||\mathbf{x}||) \quad \forall (t,\mathbf{x})\in \mathbb{R}\times \mathbb{R}^{n}$ 成立。因此，下界的存在性得到了证明。由于 $V(t,\mathbf{x})$ 的递减性（见定义A.2），也存在一个正定函数 $W_{2}(\mathbf{x})$ ，它满足条件 $V(t,\mathbf{x}) \leq W_{2}(\mathbf{x}) \quad \forall (t,\mathbf{x})\in \mathbb{R}\times \mathbb{R}^{n}$ 。再次根据引理A.5，可以找到一个类 $\kappa_{\infty}$ 函数 $\alpha_{2}$ ，使得 $V(t,\mathbf{x}) \leq W_{2}(\mathbf{x}) \leq \alpha_{2}(||\mathbf{x}||) \quad \forall (t,\mathbf{x})\in \mathbb{R}\times \mathbb{R}^{n}$ ，这完成了证明。

使用类 $\kappa$ （或 $\kappa_{\infty}$ ）函数的概念，可以如下定义轨迹的一致全局有界性。为此，考虑以下形式的系统：

$$\dot{\mathbf{x}} = \mathbf{f}(t, \mathbf{x}) \quad (A.1)$$

假设其解 $\mathbf{x}(t, t_0, \mathbf{x}_0)$ 对于 $t \geq t_0$ 存在，并且对于任何初始时间 $t_0$ 和任何初始状态 $\mathbf{x}(t_0) = \mathbf{x}_0$ 都成立。

定义 A.7. 具有初始状态 $\mathbf{x}_0$ 和初始时间 $t_0$ 的（A.1）的解 $\mathbf{x}(t, t_0, \mathbf{x}_0)$ 被称为一致全局有界的，如果存在一个类 $\kappa_{\infty}$ 函数 $\alpha$ 和一个常数 $c > 0$ ，使得：

$$||\mathbf{x}(t, t_0, \mathbf{x}_0)|| \leq \alpha (||\mathbf{x}_0||) + c \quad \forall t \geq t_0 \quad (A.2)$$

成立。

为了证明动力系统解的一致全局有界性，以下引理是有用的。

引理 A.8. 如果存在一个连续可微、正定、径向无界且递减的函数 $V(t,\mathbf{x})$ ，其沿（A.1）解对时间的导数：

$$\dot{V}(t,\mathbf{x}) = \frac{\partial V(t,\mathbf{x})}{\partial \mathbf{x}} \mathbf{f}(t,\mathbf{x}) + \frac{\partial V(t,\mathbf{x})}{\partial t} \quad (A.3)$$

是负半定的，即 $\dot{V}(t,\mathbf{x}) \leq 0$ ，则（A.1）的解将是一致全局有界的。

证明. 根据推论A.6，存在类 $\mathcal{K}_{\infty}$ 函数 $\alpha_{1}$ 和 $\alpha_{2}$ ，使得：

$$\alpha_{1}(||\mathbf{x}||) \leq V(t,\mathbf{x}) \leq \alpha_{2}(||\mathbf{x}||) \quad \forall (t,\mathbf{x})\in \mathbb{R}\times \mathbb{R}^{n}$$

成立。令 $\mathbf{x}(t, t_0, \mathbf{x}_0)$ 是（A.1）在时间 $t \geq t_0$ 具有初始状态 $\mathbf{x}_0$ 和初始时间 $t_0$ 的解。则由于 $\dot{V}(t,\mathbf{x}) \leq 0$ ，不等式 $V(t,\mathbf{x}(t, t_0, \mathbf{x}_0)) \leq V(t_0, \mathbf{x}_0)$ 对于 $t \geq t_0$ 必须成立。由 $\alpha_{1}(||\mathbf{x}(t, t_0, \mathbf{x}_0)||) \leq V(t,\mathbf{x}(t, t_0, \mathbf{x}_0))$ 和 $V(t_0, \mathbf{x}_0) \leq \alpha_{2}(||\mathbf{x}_0||)$ 可以得出，不等式：

$$\alpha_{1}(||\mathbf{x}(t, t_0, \mathbf{x}_0)||) \leq \alpha_{2}(||\mathbf{x}_0||) \quad \forall t \geq t_0$$

也必须成立。根据引理A.4以及类 $\mathcal{K}_{\infty}$ 函数是严格递增的事实（见定义A.3），可以得出不等式：

$$||\mathbf{x}(t, t_0, \mathbf{x}_0)|| \leq \alpha (||\mathbf{x}_0||) \quad \forall t \geq t_0 \quad (A.4)$$

对于类 $\mathcal{K}_{\infty}$ 函数 $\alpha = \alpha_{1}^{-1} \circ \alpha_{2}$ 成立。因此，定义A.7的要求得到满足，并且 $\mathbf{x}(t, t_0, \mathbf{x}_0)$ 的一致全局有界性由（A.4）得出。

引理A.8用于第6.1节基于解耦控制器的稳定性证明。

如上所述，在时变情况下，对在初始时间 $t_0$ 上一致的稳定性性质也感兴趣。为此，下面假设原点 $\mathbf{x} = \mathbf{0}$ 是（A.1）的平衡点。

定义 A.9. （A.1）的平衡点 $\mathbf{x} = \mathbf{0}$ 是：

• 稳定的，如果对于每个 $\epsilon > 0$ ，存在 $\delta = \delta (\epsilon, t_0) > 0$ ，使得： $$||\mathbf{x}(t_0)|| < \delta \Rightarrow ||\mathbf{x}(t)|| < \epsilon, \quad \forall t > t_0 \geq 0. \quad (A.5)$$
• 一致稳定的，如果对于每个 $\epsilon > 0$ ，存在 $\delta = \delta (\epsilon) > 0$ ，独立于 $t_0$ ，使得（A.5）成立。
• 不稳定的，如果它不是稳定的。
• 渐近稳定的，如果它是稳定的，并且存在一个正常数 $c = c(t_0)$ ，使得对于所有 $||\mathbf{x}(t_0)|| < c$ ， $\mathbf{x}(t) \to \mathbf{0}$ 当 $t \to \infty$ 时成立。
• 一致渐近稳定的，如果它是一致稳定的，并且存在一个正常数 $c$ （独立于 $t_0$ ），使得对于所有 $||\mathbf{x}(t_0)|| < c$ ， $\mathbf{x}(t) \to \mathbf{0}$ 当 $t \to \infty$ 时关于 $t_0$ 一致成立；即对于每个 $\eta > 0$ ，存在 $T = T(\eta) > 0$ ，使得： $$||\mathbf{x}(t)|| < \eta, \quad \forall t > t_0 + T(\eta), \forall ||\mathbf{x}(t_0)|| < c.$$
• 一致全局渐近稳定的，如果它是一致稳定的， $\delta (\epsilon)$ 可以选择满足 $\lim_{\epsilon \to \infty} \delta (\epsilon) = \infty$ ，并且对于每对正数 $\eta$ 和 $c$ ，存在 $T = T(\eta, c) > 0$ ，使得： $$||\mathbf{x}(t)|| < \eta, \quad \forall t > t_0 + T(\eta, c), \forall ||\mathbf{x}(t_0)|| < c.$$

基于类 $\mathcal{K}$ 函数的等价一致稳定性和一致渐近稳定性定义可以在Kha02中找到。以下两个定理取自Kha02，展示了自治系统的李雅普诺夫稳定性理论如何扩展到非自治系统。

定理 A.10 设 $\mathbf{x} = \mathbf{0}$ 是（A.1）的平衡点， $D\subset \mathbb{R}^{n}$ 是一个包含 $\mathbf{x} = \mathbf{0}$ 的域。设 $V:[0,\infty) \times D \to \mathbb{R}$ 是一个连续可微函数，使得：

$$\frac{\partial V(t,\mathbf{x})}{\partial t} + \frac{\partial V(t,\mathbf{x})}{\partial \mathbf{x}} f(t,\mathbf{x}) \leq 0$$

对所有 $t \geq 0$ 和所有 $\mathbf{x}\in D$ 成立，其中 $W_{1}(\mathbf{x})$ 和 $W_{2}(\mathbf{x})$ 是 $D$ 上的连续正定函数。则 $\mathbf{x} = \mathbf{0}$ 是一致稳定的。

定理 A.11 假设定理A.10的假设成立，并且额外满足条件：

$$\frac{\partial V(t,\mathbf{x})}{\partial t} + \frac{\partial V(t,\mathbf{x})}{\partial \mathbf{x}} f(t,\mathbf{x}) \leq -W_{3}(\mathbf{x}) \quad \forall t \geq 0, \forall \mathbf{x}\in D \quad (A.6)$$

对于某个连续正定函数 $W_{3}:\mathbb{R}^{n}\to \mathbb{R}$ 成立。则 $\mathbf{x} = \mathbf{0}$ 是一致渐近稳定的。此外，如果 $D = \mathbb{R}^{n}$ 且 $W_{1}(\mathbf{x})$ 是径向无界的，则 $\mathbf{x} = \mathbf{0}$ 是一致全局渐近稳定的。

满足定理A.10条件的函数 $V(t,\mathbf{x})$ 被称为（时变）李雅普诺夫函数。如果它还满足定理A.11的条件（A.6），则称为严格（时变）李雅普诺夫函数。

A.2 条件稳定性

在第4章中，半正定李雅普诺夫函数用于稳定性分析。因此，需要条件稳定性的概念，如SJK97中所定义。报告以下定义是为了澄清所使用的稳定性性质。

因此，考虑以下形式的时不变系统：

$$\dot{\mathbf{x}} = \mathbf{f}(\mathbf{x}) \quad (A.7)$$

状态为 $\mathbf{x}\in \mathbb{R}^{n}$ 。假设点 $\mathbf{x}_{s}$ 是（A.7）的静止点，即 $\mathbf{f}(\mathbf{x}_{s}) = \mathbf{0}$ 。假设（A.7）具有初始状态 $\mathbf{x}(0) = \mathbf{x}_{0}$ 的解 $\mathbf{x}(t)$ 对所有 $t > 0$ 存在。对于条件稳定性，稳定性定义的所有要求只需对位于特定集合 $\mathcal{A}\subset \mathbb{R}^{n}$ 中的初始条件成立。因此，条件稳定性的概念比通常的（李雅普诺夫）稳定性弱。

定义 A.12. （A.7）的静止点 $x_{s}$ 被称为相对于 $\mathcal{A}\subset \mathbb{R}^{n}$ 是条件稳定的，如果 $x_{s}\in \mathcal{A}$ 并且对于每个 $\epsilon > 0$ 存在 $\delta (\epsilon) > 0$ ，使得对于任何初始条件 $x_{0}\in \mathcal{A}$ ，以下蕴含式成立：

$$\| \mathbf{x}_{0} - \mathbf{x}_{s} \| < \delta (\epsilon) \Rightarrow \| \mathbf{x}(t) - \mathbf{x}_{s} \| < \epsilon, \quad \forall t \geq 0. \quad (A.8)$$

定义 A.13. （A.7）的静止点 $x_{s}$ 被称为相对于 $\mathcal{A}\subset \mathbb{R}^{n}$ 是条件吸引的，如果 $x_{s}\in \mathcal{A}$ 并且存在一个 $\eta (\mathbf{x}_{s}) > 0$ ，使得对于任何初始条件 $x_{0}\in \mathcal{A}$ ，以下蕴含式成立：

$$\| \mathbf{x}_{0} - \mathbf{x}_{s} \| < \eta (\mathbf{x}_{s}) \Rightarrow \lim_{t \to \infty} \mathbf{x}(t) = \mathbf{x}_{s}. \quad (A.9)$$

定义 A.14. （A.7）的静止点 $x_{s}$ 被称为相对于 $\mathcal{A}\subset \mathbb{R}^{n}$ 是条件渐近稳定的，如果它相对于 $\mathcal{A}$ 既是条件稳定的又是条件吸引的。

定义 A.15. （A.7）的静止点 $x_{s}$ 被称为相对于 $\mathcal{A}\subset \mathbb{R}^{n}$ 是全局条件渐近稳定的，如果它相对于 $\mathcal{A}$ 是条件渐近稳定的并且 $\eta (\mathbf{x}_{s}) = +\infty$ 。

A.3 无源性定义

在本书的几个部分中，使用了一些系统的无源性性质。因此，这里报告了[Kug01, vdS00, KS02a, KS02b, SJK97]中给出的耗散性和无源性的定义，以供进一步参考。考虑以下形式的时不变系统：

$$\begin{array}{r} \dot{\mathbf{x}} = \mathbf{f}(\mathbf{x}, \mathbf{u}) \\ \mathbf{y} = \mathbf{h}(\mathbf{x}, \mathbf{u}) \end{array} \quad (A.10)$$

状态为 $\mathbf{x}\in \mathbb{R}^{n}$ ，输入为 $\mathbf{u}\in \mathbb{R}^{m}$ ，输出为 $\mathbf{y}\in \mathbb{R}^{m}$ 。

定义 A.16. 如果存在一个非负函数 $S(\mathbf{x})$ （称为存储函数），使得不等式：

$$S(\mathbf{x}(t)) - S(\mathbf{x}(t_{0})) \leq \int_{t_{0}}^{t} s(\mathbf{u}(\tau), \mathbf{y}(\tau)) \mathrm{d}\tau$$

对所有可容许的 $\mathbf{u}$ 、所有初始条件 $\mathbf{x}(t_{0})$ 和所有 $t > t_{0}$ 成立，则系统（A.10）-（A.11）被称为关于供给率 $s(\mathbf{u},\mathbf{y})$ 是耗散的。

定义 A.17. 系统（A.10）-（A.11）被称为：

• 无源的，如果它关于供给率 $s(\mathbf{u},\mathbf{y}) = \mathbf{y}^{T} \mathbf{u}$ 是耗散的。
• 严格输入无源的，如果存在 $\alpha > 0$ 使得系统关于供给率 $s(\mathbf{u},\mathbf{y}) = \mathbf{y}^{T} \mathbf{u} - \alpha ||\mathbf{u}||^{2}$ 是耗散的。
• 严格输出无源的，如果存在 $\beta > 0$ 使得系统关于供给率 $s(\mathbf{u},\mathbf{y}) = \mathbf{y}^{T} \mathbf{u} - \beta ||\mathbf{y}||^{2}$ 是耗散的。

A.4 一些矩阵引理和定义

一个矩阵 $A\in \mathbb{R}^{n\times m}$ 可以看作是从 $\mathbb{R}^{m}$ 到 $\mathbb{R}^{n}$ 的线性变换。以下定义澄清了一些相关空间。

定义 A.18. 考虑一个矩阵 $A\in \mathbb{R}^{n\times m}$ ，它表示从 $\mathbb{R}^{m}$ 到 $\mathbb{R}^{n}$ 的线性变换。这个变换的定义域是 $\mathbb{R}^{m}$ ；它的共域是 $\mathbb{R}^{n}$ 。 $A$ 的核（或零空间）定义为：

$$\ker(A) := \{\mathbf{v}\in \mathbb{R}^{m} \mid A\mathbf{v} = \mathbf{0}\},$$

它是定义域的一个子空间。最后， $A$ 的值域定义为：

$$\mathrm{range}(A) := \{\mathbf{y}\in \mathbb{R}^{n} \mid \exists \mathbf{v}\in \mathbb{R}^{n} : \mathbf{y} = A\mathbf{v}\},$$

它是共域的一个子空间。

定义 A.19. 设 $A$ 是 $\mathbb{R}^{n}$ 的一个子集。给定一个正定矩阵 $G\in \mathbb{R}^{n\times n}$ 作为 $\mathbb{R}^{n}$ 的度量， $A$ 的正交补定义为：

$$A^{\perp} := \{\mathbf{y}\in \mathbb{R}^{n} \mid \mathbf{y}^{T} G \mathbf{x} = 0 \quad \forall \mathbf{x}\in \mathcal{A}\}.$$

正定矩阵的性质对线性和非线性系统的稳定性分析都很重要。本书中使用的以下两个引理是众所周知的，这里不再证明。

引理 A.20. 假设一个对称矩阵 $M\in \mathbb{R}^{n\times n}$ 被分块为：

$$M = \begin{bmatrix} A & C \\ C^{T} & B \end{bmatrix} \quad (A.12)$$

其中 $A$ 和 $B$ 是对称方阵。则矩阵 $M$ 是正定的（ $M > 0$ ）当且仅当 $A$ 是正定的并且 $B - C^{T} A^{-1} C$ 是正定的。

证明. 参见HJ90。

引理 A.21. 给定一个任意的正定矩阵 $Q$ ，当且仅当矩阵 $A$ 是Hurwitz的，才能找到方程 $A^{T}P + PA = - Q$ 的唯一正定解 $P$ 。

证明. 参见Vid93。

最后，从vdS00中给出了一个关于斜对称矩阵的有用引理，不再证明。注意，这个引理对于性质2.6和引理3.2很有意义。

引理 A.22. 给定一个对称矩阵 $A\in \mathbb{R}^{n\times n}$ 和另一个相同维数的方阵 $B\in \mathbb{R}^{n\times n}$ 。则矩阵 $C = A - 2B$ 是斜对称的，当且仅当等式 $A = B + B^{T}$ 成立。

引理 A.23. 给定一个函数 $A(x): \mathbb{R}^{n} \to \mathbb{R}^{n\times n}$ 。假设积分：

$$i_{1}(x_{1}, x_{2}) := \int_{x_{1}}^{x_{2}} A(x) \mathrm{d}x$$ $$i_{2}(x_{1}, x_{2}) := \int_{x_{1}}^{x_{2}} \left( \int_{x_{1}}^{x} A(\xi) \mathrm{d}\xi \right)^{T} \mathrm{d}x$$

对于所有 $x_{1}, x_{2} \in \mathbb{R}^{n}$ 是路径无关的。则不等式：

$$|| i_{1}(x_{1}, x_{2}) ||_{2} \leq \sup_{\forall x \in \mathbb{R}^{n}} || A(x) ||_{i2} || x_{2} - x_{1} ||_{2}$$ $$| i_{2}(x_{1}, x_{2}) | \leq \frac{1}{2} \sup_{\forall x \in \mathbb{R}^{n}} || A(x) ||_{i2} || x_{2} - x_{1} ||_{2}^{2}$$

成立，其中 $|| \cdot ||_{2}$ 和 $|| \cdot ||_{i2}$ 分别是向量的欧几里得范数和矩阵的谱范数。

证明. 可以通过选择特定的积分路径来证明。考虑路径 $\mathbf{x}(s):[0,1] \to \mathbb{R}^{n}$ 和 $\boldsymbol{\xi}(t):[0,1] \to \mathbb{R}^{n}$ ：

$$x(s) := x_{1} + (x_{2} - x_{1}) s$$ $$\xi(t) := x_{1} + (x - x_{1}) t$$

通过评估 $i_{1}$ 的积分，得到：

$$i_{1}(x_{1}, x_{2}) = \int_{0}^{1} A(x_{1} + (x_{2} - x_{1})s) (x_{2} - x_{1}) \mathrm{d}s$$ $$|| i_{1}(x_{1}, x_{2}) ||_{2} \leq \int_{0}^{1} || A(x_{1} + (x_{2} - x_{1})s) (x_{2} - x_{1}) ||_{2} \mathrm{d}s$$ $$|| i_{1}(x_{1}, x_{2}) ||_{2} \leq \sup_{\forall x \in \mathbb{R}^{n}} || A(x) ||_{i2} || x_{2} - x_{1} ||_{2}.$$

类似地，对于 $i_{2}$ 可以计算：

$$i_{2}(x_{1}, x_{2}) = \int_{0}^{1} \left( \int_{0}^{1} A(x_{1} + (x_{2} - x_{1}) s t) (x_{2} - x_{1}) s \mathrm{d}t \right)^{T} (x_{2} - x_{1}) \mathrm{d}s$$ $$| i_{2}(x_{1}, x_{2}) | \leq \int_{0}^{1} \int_{0}^{1} | (x_{2} - x_{1})^{T} A(x_{1} + (x_{2} - x_{1}) s t) (x_{2} - x_{1}) | s \mathrm{d}t \mathrm{d}s$$ $$| i_{2}(x_{1}, x_{2}) | \leq \int_{0}^{1} \int_{0}^{1} \sup_{\forall x \in \mathbb{R}^{n}} || A(x) ||_{i2} || x_{2} - x_{1} ||_{2}^{2} s \mathrm{d}t \mathrm{d}s$$ $$| i_{2}(x_{1}, x_{2}) | \leq \frac{1}{2} \sup_{\forall x \in \mathbb{R}^{n}} || A(x) ||_{i2} || x_{2} - x_{1} ||_{2}^{2}.$$

A.5 扩展雅可比矩阵 (J_{N}(q)) 的逆

在本节中，推导了来自第4.4节的矩阵：

$$J_{N}(q) = \begin{pmatrix} J(q) \\ (Z(q)M(q)Z(q)^{T})^{-1} Z(q) M(q) \end{pmatrix} = \begin{pmatrix} J(q) \\ N(q) \end{pmatrix} \quad (A.13)$$

的逆的显式形式。假设矩阵 $J(q) \in \mathbb{R}^{m \times n}$ 、 $Z(q) \in \mathbb{R}^{(n - m) \times n}$ 和 $M(q) \in \mathbb{R}^{n \times n}$ 具有以下性质：

• 矩阵 $J(q)$ 具有满行秩。
• 矩阵 $Z(q)$ 的行向量张成 $J(q)$ 的（右）零空间，使得 $J(q)Z(q)^{T} = 0$ 成立，并且 $Z(q)$ 具有满行秩。
• 矩阵 $M(q)$ 是正定的。

首先，通过以下引理证明 $J_{N}(q)$ 的可逆性。

引理 A.24. 设 $J \in \mathbb{R}^{m \times n}$ （其中 $n > m$ ）是一个具有满行秩的矩形矩阵。进一步设 $Z \in \mathbb{R}^{(n - m) \times n}$ 是 $J$ 的满秩右零化子，使得 $JZ^{T} = 0$ 成立。则对于任何正定矩阵 $W \in \mathbb{R}^{n \times n}$ ，矩阵：

$$\begin{pmatrix} J \\ (ZWZ^{T})^{-1} Z W \end{pmatrix}$$

是非奇异的。

证明. 首先注意，由于 $Z$ 的行向量张成 $J$ 的右零空间，对于任何正定矩阵 $W$ ，矩阵 $(ZWZ^{T})^{-1} Z$ 也是如此。引理的证明通过反证法进行。假设 $J_{N}$ 是奇异的。则存在一个向量 $v \neq 0$ 使得 $J_{N}v = 0$ 。由此可以得出：

$$Jv = 0 \Rightarrow v \in \ker(J),$$ $$\left( Z W Z^{T} \right)^{-1} Z W v = 0 \Rightarrow W v \perp \ker(J).$$

但这意味着：

$$v \perp W v \Rightarrow v^{T} W v = 0$$

必须成立，这显然与矩阵 $W$ 是正定的假设相矛盾。因此，通过反证法可以得出结论，矩阵 $J_{N}$ 不可能是奇异的。

方阵 $J_{N}(q)$ 的逆的存在性直接由上述引理得出。现在将逆 $J_{N}(q)^{-1}$ 分块为：

$$J_{N}(q)^{-1} = [A(q) \quad B(q)], \quad (A.14)$$

其中非方阵 $A(q) \in \mathbb{R}^{n \times m}$ 和 $B(q) \in \mathbb{R}^{n \times (n - m)}$ 。由：

$$\begin{array}{r} J_{N}(q) J_{N}(q)^{-1} = \begin{bmatrix} I & 0 \\ 0 & I \end{bmatrix} \\ = \begin{bmatrix} J(q)A(q) & J(q)B(q) \\ N(q)A(q) & N(q)B(q) \end{bmatrix} \end{array} \quad (A.15)$$

得到四个恒等式：

$$J(q)A(q) = I, \quad (A.16)$$ $$(Z(q)M(q)Z(q)^{T})^{-1} Z(q) M(q) A(q) = 0, \quad (A.17)$$ $$(Z(q)M(q)Z(q)^{T})^{-1} Z(q) M(q) B(q) = I, \quad (A.18)$$ $$J(q)B(q) = 0. \quad (A.19)$$

现在可以从前两个恒等式确定矩阵 $A(q)$ ，从后两个恒等式确定矩阵 $B(q)$ ，如下所示：

方程A.17显然对于任何形式为：

$$A(q) = M(q)^{-1} J(q)^{T} Q_{A}(q) \quad (A.20)$$

的矩阵 $A(q)$ 都成立，其中 $Q_{A}(q) \in \mathbb{R}^{m \times m}$ 是任意矩阵。然后可以从方程A.16轻松确定矩阵 $Q_{A}(q)$ 为 $Q_{A}(q) = (J(q)M(q)^{-1} J(q)^{T})^{-1}$ ，因此：

$$A(q) = M(q)^{-1} J(q)^{T} (J(q) M(q)^{-1} J(q)^{T})^{-1}. \quad (A.21)$$

同样的过程可以应用于得到 $B(q)$ 。方程A.19对于任何形式为：

$$B(q) = Z(q)^{T} Q_{B}(q) \quad (A.22)$$

的矩阵 $B(q)$ 都成立，其中 $Q_{B}(q) \in \mathbb{R}^{(n - m) \times (n - m)}$ 是任意矩阵。然后可以从方程A.18确定矩阵 $Q_{B}(q)$ 为 $Q_{B}(q) = I$ ，因此：

$$B(q) = Z(q)^{T}. \quad (A.23)$$

因此，解由下式给出：

$$J_{N}(q)^{-1} = [M(q)^{-1} J(q)^{T} (J(q) M(q)^{-1} J(q)^{T})^{-1} \quad Z(q)^{T}]. \quad (A.24)$$

A.6 DLR-Lightweight-Robot-II 的模型参数

图A.1显示了DLR-Lightweight-Robot-II的运动学结构。计算第2.1章机器人运动学所需的参数在表A.1中给出。

表A.1. DLR-Lightweight-Robot-II的运动学数据： $d_{1} = 0.213 \mathrm{m}$ ， $d_{2} = 0.463 \mathrm{m}$ ， $d_{3} = d_{4} = 0.723 \mathrm{m}$ ， $d_{5} = 1.0615 \mathrm{m}$ ， $d_{6} = 1.14 \mathrm{m}$

i	1	2	3	4	5	6	7
ω_i	(0,0,1)	(0,1,0)	(0,0,1)	(0,1,0)	(0,0,-1)	(0,1,0)	(0,0,1)
v_i	(0,0,0)	(-d1,0,0)	(0,0,-d2)	(-d3,0,0)	(0,0,-d4)	(-d5,0,0)	(0,0,-d6)

表A.2. DLR-Lightweight-Robot-II的电机惯量和关节刚度值。传动比的值对于所有关节均为160，并且在电机惯量值中已经考虑。

关节 i	电机惯量 B_i [kgm²]	关节刚度 K_i [Nm/rad]
1	1.83	68000.0
2	4.44	71500.0
3	3.24	21400.0
4	1.81	71200.0
5	1.39	28800.0
6	1.40	26000.0
7	1.39	28800.0

A.7 使用的符号列表

在本书中，标量用普通字母表示（例如 $m, n, \delta , \epsilon$ ）。向量和矩阵量则使用粗体字母。点表示对时间 $t$ 直到二阶的导数（即 $\dot{q} := \frac{\mathrm{d}}{\mathrm{d}t} q, \ddot{q} := \frac{\mathrm{d}^2}{\mathrm{d}t^2} q$ ）。更高的时间导数以 $q^{(k)} := \frac{\mathrm{d}^k}{\mathrm{d}t^k} q, k > 2$ 的形式表示。

以下列表包含在多个章节中使用的符号。

• $n \in \mathbb{Z}$ ：关节数量。
• $m \in \mathbb{Z}$ ：笛卡尔坐标数量（通常 $m = 6$ ）。
• $r \in \mathbb{Z}$ ：冗余度 $r = n - m$ 。
• $t \in \mathbb{R}$ ：时间变量。
• $S$ ：基座（或世界）坐标系。
• $T$ ：机器人末端执行器处的工具坐标系。
• $D$ ：期望坐标系，即虚拟平衡坐标系。
• $\lambda_{i}(A)$ ：对称矩阵 $A$ 的第 $i$ 个特征值。
• $\lambda_{\min}(A)$ ：对称矩阵 $A$ 的最小特征值。
• $\lambda_{\max}(A)$ ：对称矩阵 $A$ 的最大特征值。
• $\lambda_{m,M}$ ：惯性矩阵的最小特征值，即 $\lambda_{m,M} := \inf_{q \in \mathcal{Q}^{p}} \min_{i = 1..n} \lambda_i(\mathcal{M}(q))$ 。
• $\lambda_{M,M}$ ：惯性矩阵的最大特征值，即 $\lambda_{M,M} := \sup_{q \in \mathcal{Q}^{p}} \max_{i = 1..n} \lambda_i(\mathcal{M}(q))$ 。
• $\sigma_{i}(A)$ ：矩阵 $A$ 的第 $i$ 个奇异值。
• $\sigma_{\min}(A)$ ：矩阵 $A$ 的最小奇异值。
• $\sigma_{\max}(A)$ ：矩阵 $A$ 的最大奇异值。
• $\sigma_{m,J}$ ：雅可比矩阵的最小奇异值，即 $\sigma_{m,J} := \inf_{q \in \mathcal{Q}^{p}} \min_{i = 1..n} \sigma_i(\mathbf{J}(q))$ 。
• $\sigma_{M,J}$ ：雅可比矩阵的最大奇异值，即 $\sigma_{M,J} := \sup_{q \in \mathcal{Q}^{p}} \max_{i = 1..n} \sigma_i(\mathbf{J}(q))$ 。
• $\mathcal{Q}$ ：配置空间。
• $\mathcal{Q}^{p}$ ：配置空间的子集，其中所有移动关节保持有界，参见（2.36）。
• $\bar{\mathcal{Q}}^{p}$ ：配置空间的子集，其中雅可比矩阵非奇异，参见（3.6）。
• $\mathcal{Q}_{c}^{p} := \mathbf{f}(\bar{\mathcal{Q}}^{p})$ 。
• $\mathbf{q} \in \mathbb{R}^{n}$ ：连杆侧关节角度向量。
• $\boldsymbol{\theta} \in \mathbb{R}^{n}$ ：电机侧关节角度向量。
• $\boldsymbol{\tau} \in \mathbb{R}^{n}$ ：关节扭矩向量， $\boldsymbol{\tau} = \mathbf{K}(\boldsymbol{\theta} - \mathbf{q})$ 。
• $\boldsymbol{\tau}_{m} \in \mathbb{R}^{n}$ ：电机扭矩向量。
• $\boldsymbol{\tau}_{ext} \in \mathbb{R}^{n}$ ：外部扭矩向量。
• $\mathbf{f}(\mathbf{q}) \in \mathbb{R}^{m}$ ：一组笛卡尔坐标的正向运动学。
• $\mathbf{J}(\mathbf{q}) \in \mathbb{R}^{m \times n}$ ：解析雅可比矩阵， $\mathbf{J}(\mathbf{q}) = \partial \mathbf{f}(\mathbf{q}) / \partial \mathbf{q}$ 。
• $\mathbf{J}^{b}(\mathbf{q}) \in \mathbb{R}^{m \times n}$ ：体（或几何）雅可比矩阵。
• $\mathbf{V}^{b}$ ：体旋量。
• $\mathbf{F}_{ext} \in \mathbb{R}^{m}$ ：广义外力向量。
• $\mathbf{x} \in \mathbb{R}^{m}$ ：笛卡尔坐标向量， $\mathbf{x} = \mathbf{f}(\mathbf{q})$ 。
• $\mathbf{x} \in \mathbb{R}^{n}$ ：通用状态向量（仅在附录中使用）。
• $\mathbf{x}_{d} \in \mathbb{R}^{m}$ ：期望位置，即虚拟平衡位置。
• $\tilde{\mathbf{x}} \in \mathbb{R}^{m}$ ：笛卡尔误差， $\tilde{\mathbf{x}} = \mathbf{x} - \mathbf{x}_{d}$ 。
• $\phi \in \mathbb{R}^{3}$ ：滚转-俯仰-偏航欧拉角。
• $\mathbf{h}_{ab} \in SE(3)$ ：从坐标系 $\mathcal{B}$ 到坐标系 $\mathcal{A}$ 的齐次变换。
• $\mathbf{R}_{ab} \in SO(3)$ ：对应于 $\mathbf{h}_{ab}$ 的旋转矩阵。
• ${}^{a}\mathbf{P}_{ab} \in \mathbb{R}^{3}$ ：对应于 $\mathbf{h}_{ab}$ 的平移向量。
• $\mathbf{p}_{ab} \in \mathbb{R}^{3}$ ：从坐标系 $\mathcal{A}$ 的原点到 $\mathcal{B}$ 的原点的向量，在基坐标系 $\mathcal{S}$ 中表示， $\mathbf{p}_{ab} = \mathbf{R}_{sa} {}^{a}\mathbf{p}_{ab}$ 。
• ${}^{b}\mathbf{v}_{sb} \in \mathbb{R}^{3}$ ：坐标系 $\mathcal{B}$ 相对于 $\mathcal{B}$ 的速度，在 $\mathcal{B}$ 中表示。
• ${}^{b}\boldsymbol{\omega}_{sb} \in \mathbb{R}^{3}$ ：坐标系 $\mathcal{B}$ 相对于 $\mathcal{B}$ 的角速度，在 $\mathcal{B}$ 中表示。
• $\mathbf{I}$ ：单位矩阵。
• $B_{i}$ ：电机 $i$ 的转子惯量，对于 $i = 1,\dots,n$ 。
• $K_{i}$ ：关节 $i$ 的刚度，对于 $i = 1,\dots,n$ 。
• $\mathbf{B} \in \mathbb{R}^{n\times n}$ ：操纵器的电机惯性矩阵， $\mathbf{B} = \mathrm{diag}(B_{i})$ 。
• $\mathbf{K} \in \mathbb{R}^{n\times n}$ ：关节刚度矩阵， $\mathbf{K} = \mathrm{diag}(K_{i})$ 。
• $\mathbf{K}_{d} \in \mathbb{R}^{m\times m}$ ：期望刚度矩阵。
• $\mathbf{K}_{n} \in \mathbb{R}^{n\times n}$ ：零空间刚度矩阵。
• $\mathbf{D}_{d} \in \mathbb{R}^{m\times m}$ ：期望阻尼矩阵。
• $\mathbf{D}_{n} \in \mathbb{R}^{r\times r}$ ：零空间阻尼矩阵。
• $\mathbf{\Lambda}_{d} \in \mathbb{R}^{m\times m}$ ：期望惯性矩阵。
• $M(q) \in \mathbb{R}^{n\times n}$ ：操纵器的连杆侧惯性矩阵。
• $C(q,\dot{q}) \in \mathbb{R}^{n\times n}$ ：操纵器的连杆侧科里奥利/离心矩阵。
• $g(q) \in \mathbb{R}^{n}$ ：重力扭矩。
• $\Lambda(\mathbf{x})$ ：笛卡尔惯性矩阵。
• $\mu(\mathbf{x}, \dot{\mathbf{x}})$ ：笛卡尔科里奥利/离心矩阵。

---

返回目录