阻抗&导纳控制理解

书籍《Modern Robotics - Mechanics , Planning, and Control》中关于阻抗控制和导纳控制的部分：








下面结合上边的内容谈一谈我对导纳控制的理解。

1、质量-弹簧-阻尼

首先，不论是阻抗控制，还是导纳控制，他们同根同源，都是基于质量-弹簧-阻尼动力学模型而产生的。下图所示是一个单自由度质量-弹簧-阻尼虚拟环境：

对其进行受力分析：

根据牛顿第二定律，有$$m\ddot{x}=f-kx-b\dot{x},$$表明物块的运动状态（位移 $x$、速度 $\dot{x}$、加速度 $\ddot{x}$）和 其所受外力 $f$ 之间满足：$$m\ddot{x}+b\dot{x}+kx=f,$$对该动力学方程左右两侧进行拉普拉斯变换，得到$$(m{s}^2+bs+k)X(s)=F(s).$$阻抗被定义为位置扰动到力的传递函数 $Z(s)=F(s)/X(s)$（输入位置、输出力），所以阻抗是频率相关的，低频响应由弹簧决定，高频响应由质量决定。
导纳被定义为力扰动到位置的传递函数，是阻抗的倒数，$Y(s)=X(s)/F(s)$（输入力，输出位置）。

2、阻抗/导纳的设计初衷—实现柔顺

不论是阻抗还是导纳控制器，都是为了实现柔顺控制。
传统的误差驱动的位置控制器，例如 $PD$ 控制器、$PID$ 控制器…就是为了消除外部扰动带来的跟踪偏差，然而外力的引入势必会导致偏差的增大，两者相互对抗，机器人就像一个钢铁直男宁折不弯；
而基于阻抗/导纳控制器的柔顺体现在，当机器人受到外力干扰，机器人不会像在位置控制器模式下一样和外力作用硬碰硬，而是会顺应外力运动，当外力撤销，机器人便会恢复到原始的运动状态，主打一个接化发。
重点在于，机器人顺应外力运动的原则就是：呈现弹簧-质量-阻尼的动力学特性。

回到公式，阻抗/导纳控制的目标是实现任务空间行为$$M_d\ddot{x}+B_d\dot{x}+K_{d}x=f_{ext},$$其中 $x\in {\mathbb{R}}^n$ 是最小坐标集中的任务空间配置，例如 $x\in {\mathbb{R}}^3$；$M_d$、$B_d$ 和 $K_d$ 是期望的正定虚拟质量、阻尼和刚度矩阵，$f_{ext}$ 是施加到机器人上的力，可能是由用户施加的，还可以在机器人的受控运动中用相对于参考值的小位移 $\ddot{e}$、$\dot{e}$ 和 $e$ 替换 $\ddot{x}$、$\dot{x}$ 和 $x$ ，即$$M_d\ddot{e}+B_d\dot{e}+K_{d}e=f_{ext},$$其中，$e=x-x_0$，$x$ 是机器人位置，$x_0$ 是期望平衡轨迹 (desired equilibrium trajectory)，举个例子，假设机器人末端在笛卡尔空间跟踪一个椭圆轨迹，那么这个椭圆轨迹就是这里提到的期望平衡轨迹，此时它是随时间变化的；再举个例子，假设机器人末端在笛卡尔空间保持不动，那么机器人保持不动的这个位置就是这里提到的期望平衡轨迹，准确的说，其实是一个期望平衡点(desired equilibrium point)，此时它不随时间变化。但是，$x_0$ 不是指机器人的初始位置！

3、什么是阻抗控制和导纳控制？

下面还是以一个与环境交互的单自由度质量块系统为例，阐述阻抗控制和导纳控制。
假设质量块的质量为 $m$，$x$ 是质量块的位移，$F$ 是控制力，$F_{ext}$ 为环境作用在质量块上的外力，质量块的运动方程表达为：
$$\begin{array}{r}m\ddot{x}=F+F_{ext}\end{array}\text{\hspace{1em}(1)}$$导纳和阻抗控制的控制目标都是设计控制力 $F$ 使外部力 $F_{ext}$ 和 $e=(x-x_0)$之间建立质量-弹簧-阻尼动力学关系：$$\begin{array}{r}{M}_d\ddot{e}+D_d\dot{e}+K_{d}e=F_{ext}\end{array}\text{\hspace{1em}(2)}$$

3.1 阻抗控制(Impedance control)

在阻抗控制中，控制器是一个机械阻抗，被控对象被当作导纳对待，如下图所示：

结合公式(1)和公式(2)，推导得到阻抗控制律：$$\begin{array}{r}F=(\frac{m}{M_d}-1)F_{ext}+m{\ddot{x}}_0-\frac{m}{M_d}(D_d\dot{e}+K_{d}e)\end{array}\text{\hspace{1em}(3)}$$

3.2 导纳控制(Admittance control)

在导纳控制中，受控对象基于位置控制，而不再像阻抗控制器那样基于力控制，如下图所示：

其中，位置控制器可采用 $PD$ 控制器实现：$$\begin{array}{r}F=k_p(x_d-x)-k_d\dot{x}\end{array}\text{\hspace{1em}(4)}$$将公式(4)带入公式(1)，得到
$$\begin{array}{r}m\ddot{x}+k_d\dot{x}+k_p(x-x_d)=F_{ext}\end{array}\text{\hspace{1em}(5)}$$将 $x$ 替换为 $x_d$ 重写公式(2)，得到
$$\begin{array}{r}{M}_d({\ddot{x}}_d-{\ddot{x}}_0)+D_d({\dot{x}}_d-{\dot{x}}_0)+K_d(x_d-x_0)=F_{ext}\end{array}\text{\hspace{1em}(6)}$$公式(5)(6) 便是导纳控制系统的完整动力学。

4、导纳控制的实际实现

在导纳控制算法中，用户/环境施加的外力 $f_{ext}$ 由末端 F/T 传感器测量并转换到机器人基座标系下而获得，机器人以满足方程式 $M_d\ddot{x}+B_d\dot{x}+K_{d}x=f_{ext}$ 的末端执行器加速度响应，一种简单的方法是根据下式计算期望的末端执行器加速度 ${\ddot{x}}_d$：$$\begin{array}{r}{M}_d{\ddot{x}}_d+B_d\dot{x}+K_{d}x=f_{ext}\end{array}\text{\hspace{1em}(7)}$$其中 $(x,\dot{x})$ 是当前状态。求解，我们得到$$\begin{array}{r}{\ddot{x}}_d=M_d^{-1}(f_{ext}-B_d\dot{x}-K_{d}x)\end{array}\text{\hspace{1em}(8)}$$说白了，就是为了使机器人满足质量-弹簧-阻尼二阶动力学，根据机器人当前控制周期的状态 $(x,\dot{x})$ 和外部力 $f_{ext}$，计算得到满足该动力学的加速度值，而后以计算出来的加速度值进行积分，得到下一控制周期的速度指令 (适用于机器人底层是速度控制器) ；如果对得到的速度指令再进行积分，就会得到下一控制周期的位置指令 (适用于机器人底层是位置控制器) 。

在实际应用场景中，不论是定点导纳控制，还是在轨迹跟踪任务中加入导纳柔顺控制策略，这些机器人受控运动都是具有预定义的参考轨迹 (期望平衡轨迹) 的，因此，公式 (9) 形式描述的阻抗模型更为实用：$$\begin{array}{r}{M}_d\ddot{e}+D_d\dot{e}+K_{d}e=F_{ext}\end{array}\text{\hspace{1em}(9)}$$

• $M_d$：期望惯性系数对角矩阵；
• $D_d$：期望阻尼系数对角矩阵；
• $K_d$：期望刚度系数对角矩阵；
• $F_{ext}$：机器人基坐标系下的环境力。我们要知道，从末端 F/T 传感器获取的测量值是在传感器坐标系下的表示，通常传感器坐标系和末端法兰坐标系保持一致，因此，将传感器坐标系下获取的测量值左乘旋转矩阵即可获得外部环境力在机器人基坐标系下的直观对应；
• $e=x-x_0=[\Delta v,\Delta w]$ 为机器人基坐标系下描述的机器人实际位姿 $x$ 与期望平衡位姿 $x_0$ 之差。其中，位置偏差 = 当前实际位置 - 期望平衡位置，可以直接相减；但是姿态不可以直接相减，描述姿态的表达方式有多种：旋转矩阵、欧拉角、四元数、等效轴角等，旋转矩阵形式的姿态偏差求法：$R_e=R{R}_0^T$，相当于：姿态偏差 = 当前实际姿态 - 期望平衡姿态，另外程序中一般采用 $[3,1]$ 向量描述姿态，所以要将其转换为等效转轴描述的姿态偏差 $\Delta w$ (韩冰老师说：传感器采集的扭矩相当于等效转轴描述)；

            // compute error to desired equilibrium pose
            pos_error.head(3) << curr_position_ - desired_position_;																						//  位置偏差，直接相减
            if (desired_pose_orientation_.coeffs().dot(curr_orientation_.coeffs()) < 0.0) {												// 四元数形式确认符号
                curr_orientation_.coeffs() << -curr_orientation_.coeffs();
            }
            // "difference" quaternion																																						// 姿态偏差求解为四元数形式
            Eigen::Quaterniond quat_rot_err (curr_orientation_ * desired_pose_orientation_.inverse());
            if(quat_rot_err.coeffs().norm() > 1e-3)
            {
                quat_rot_err.coeffs() << quat_rot_err.coeffs()/quat_rot_err.coeffs().norm();
            }
            Eigen::AngleAxisd err_arm_des_orient(quat_rot_err);                                 																// 四元数转轴角
            pos_error.tail(3) << err_arm_des_orient.axis() * err_arm_des_orient.angle();        										// 从轴角获取 3*1 向量

• $\dot{e}=\dot{x}-{\dot{x}}_0$ 和 $\ddot{e}=\ddot{x}-{\ddot{x}}_0$ 为 $e$ 的一阶导数和二阶导数；

参照公式 (7)(8)的导纳控制实现理念，将公式 (9) 所示的阻抗模型改写为如下表达形式：
$$\begin{array}{r}{\ddot{e}}_t=M_d^{-1}(F_{ext}-D_d{\dot{e}}_t+K_d{e}_t)\end{array}\text{\hspace{1em}(10)}$$积分
$$\begin{array}{r}{\dot{e}}_{t+1}={\dot{e}}_t+{\ddot{e}}_t\Delta t\end{array}\text{\hspace{1em}(11)}$$再积分
$$\begin{array}{r}{e}_{t+1}=e_t+{\dot{e}}_{t+1}\Delta t\end{array}\text{\hspace{1em}(12)}$$

如果机器人底层控制器是速度接口，则下一周期速度控制指令
$$\begin{array}{r}{\dot{x}}_d^{t+1}={\dot{e}}_{t+1}+{\dot{x}}_0^{t+1}\end{array}\text{\hspace{1em}(13)}$$如果机器人底层控制器是位置接口，则下一周期的位置控制指令：
$$\begin{array}{r}{x}_d^{t+1}=e_{t+1}+x_0^{t+1}\end{array}\text{\hspace{1em}(14)}$$

举例来说，对于定点导纳控制，机器人的期望平衡轨迹实际上只有位姿，由于机器人保持不动，速度和加速度都为零，公式 (10)~(14) 就简化为：
$$\begin{gathered} {\ddot{x}}_d^t=M_d^{-1}(F_{ext}-D_d{\dot{x}}_t+K_d(x_t-x_0^t)) \\ {\dot{x}}_d^{t+1}={\dot{x}}_t+{\ddot{x}}_d^t\Delta t \\ {x}_d^{t+1}=x_t+{\dot{x}}_d^{t+1}\Delta t \end{gathered}$$

但是在以上积分求解过程中应该注意，公式 (11) 由 ${\ddot{e}}_t$ 到 ${\dot{e}}_{t+1}$ 的位置和姿态积分直接叠加即可；但是公式 (12) 由 ${\dot{e}}_{t+1}$ 到 $e_{t+1}$ 的位置积分可以直接叠加，姿态积分不可以直接叠加，必须转换成姿态旋转矩阵描述后进行旋转矩阵姿态运算叠加，叠加表达式：$R_{t+1}=\Delta R{R}_t$，其中，$\Delta R$ 由 ${\dot{e}}_{t+1}\Delta t$ 的等效转轴姿态描述转换而来，并且矩阵乘法不满足交换律，注意先后顺序。公式 (14)的姿态叠加处理方式相同。所以注意，姿态求解过程中涉及到的姿态偏差求解和姿态叠加求解，是能否正确实现姿态导纳控制的关键。

5、总结

Github 上找到的一些开源代码是仿真或者是基于实际机械臂的，但都是基于速度控制接口的机械臂，导纳控制器直接计算得到速度指令，供底层速度控制器去跟踪，鲜有看到导纳控制器计算得到位置指令，让底层位置控制器去跟踪，虽然没有什么理论深度，但是涉及到笛卡尔空间下位置和姿态一共6个自由度的时候，还是有很多要注意的点的，特此在此总结。

综上所述，阻抗控制和导纳控制是两种实现弹簧-质量-阻尼动力学的方式，根据实际条件，实际应用场景的不同，应该选择合适的方式实现所需要的柔顺特性。

参考资源

CSDN博客：【机器人基础】阻抗/导纳控制深度解析
知乎帖_韩冰老师：基于六维力传感器机器人导纳控制
知乎帖_基于位置的阻抗控制——导纳控制
知乎帖_段晋军老师：阻抗控制和导纳控制在机器人中的应用

参考论文：2010 ICRA“Unified Impedance and Admittance Control”