1 刚性机器人的运动学与动力学

机器人阻抗控制是机器人力控制中的一种重要方式。了解机器人的阻抗控制需要首先了解刚性机器人的阻抗控制方法，并且首先需要了解刚性机器人的动力学方程和运动学方程。在本栏中给出的机器人为非冗余机器人，即机器人的关节空间自由度与笛卡尔空间运动自由度是相等的。

刚性机器人的动力学方程可以表示如下：

$M(q)\ddot q+C(q,\dot q)\dot q+g(q)=\tau+\tau_{ext}$

进而考虑刚性机器人的运动方程，一般来说运动方程包含位置级-速度级和加速度级，机器人关节 $q$ 和笛卡尔 $x$ 之间的关系具体表示如下：

$x=f(q)\\\dot x =J\dot q\\ \ddot x=J\ddot q+\dot J\dot q$

$J$ 为机器人的运动学雅可比矩阵，其可以计算机器人的逆向运动学。此外雅可比矩阵可以将关节空间的外部作用力映射和笛卡尔空间的外部作用力关联，即 $\tau_{ext}=J^TF_{ext}$ ,考虑机器人笛卡尔空间轨迹误差 $e=x-x_d$ 则笛卡尔空间的阻抗控制率如下所示

$M_d\ddot e+D_d\dot e+K_de= F_{ext}$

2 刚性机器人笛卡尔动力学方程

上述给出的是机器人关节空间的运动力学方程，为了更好的给出机器人的笛卡尔孔家的阻抗空置率，首先给出机器人的逆向运动学

$\dot q =J^{-1}\dot x\\\ddot q=J^{-1}\ddot x-\dot J\dot q$

则关节空间机器人的动力学方程具体如下所示

$M(q)(J^{-1}\ddot x-J^{-1}\dot J\dot q)+C(q,\dot q)J^{-1}\dot x+g(q)=\tau+J^TF_{ext}\\=> M(q)(J^{-1}\ddot x-J^{-1}\dot JJ^{-1}\dot x)+C(q,\dot q)J^{-1}\dot x+g(q)=\tau+J^TF_{ext}\\ =>M(q)J^{-1}\ddot x-M(q)J^{-1}\dot JJ^{-1}\dot x+C(q,\dot q)J^{-1}\dot x+g(q)=\tau+J^TF_{ext}$

对转化后的方程两边同时乘以 $J^{-T}$

$J^{-T}M(q)J^{-1}\ddot x-J^{-T}M(q)J^{-1}\dot JJ^{-1}\dot x+J^{-T}C(q,\dot q)J^{-1}\dot x+J^{-T}g(q)=J^{-T}\tau+J^{-T}J^TF_{ext}\\ =>J^{-T}M(q)J^{-1}\ddot x-J^{-T}M(q)J^{-1}\dot JJ^{-1}\dot x+J^{-T}C(q,\dot q)J^{-1}\dot x+J^{-T}g(q)=J^{-T}\tau+F_{ext}$

由此得到机器人的笛卡尔动力学方程

$N(x)\ddot x+V(x，\dot x)\dot x+F_g(x)=F_{t}+F_{ext}$

其中

$N(x)=J^{-T}M(q)J^{-1}$ ,

$V(x,\dot x) =-J^{-T}M(q)J^{-1}\dot JJ^{-1}+J^{-T}C(q,\dot q)J^{-1}\\ =J^{-T}C(q,\dot q)J^{-1}-J^{-T}M(q)J^{-1}\dot JJ^{-1}$

$F_g(x)=J^{-T}g(q)$

3 笛卡尔阻抗控制

机器人笛卡尔阻抗控制示意图

图片来源：here

机器人笛卡尔阻抗控制

由笛卡尔阻抗控制律可以得到：

$M_d\ddot e+D_d\dot e+K_de= F_{ext}\\ =>M_d\ddot e+D_d\dot e+K_de= F_{ext}\\=>\ddot x=\ddot x_d+{M_d}^{-1}({F_{ext}-D_d\dot e-K_de})$

将其带入到机器人笛卡尔动力学方程中

$N(x)\ddot x+V(x，\dot x)\dot x+F_g(x)=F_{t}+F_{ext}\\ => N(x)(\ddot x_d+{M_d}^{-1}({F_{ext}-D_d\dot e-K_de}))+V(x，\dot x)\dot x+F_g(x)=F_{t}+F_{ext}\\ =>F_{t}=N(x)(\ddot x_d+{M_d}^{-1}({F_{ext}-D_d\dot e-K_de}))+V(x，\dot x)\dot x+F_g(x)-F_{ext}\\ =>F_{t}=N(x)\ddot x_d+N(x){M_d}^{-1}({F_{ext}-D_d\dot e-K_de})+V(x，\dot x)\dot x+F_g(x)-F_{ext}$

由此可以得到：

$F_{t}=N(x)\ddot x_d+N(x){M_d}^{-1}({F_{ext}-D_d\dot e-K_de})+V(x，\dot x)\dot x+F_g(x)-F_{ext}\\ =>F_{t}=N(x)\ddot x_d+N(x){M_d}^{-1}({-D_d\dot e-K_de})+V(x，\dot x)\dot x+F_g(x)+(N(x){M_d}^{-1}-1)F_{ext}$

假设 $M_d=N(x)$ ,则上式转化为

$F_{t}=N(x)\ddot x_d+N(x){M_d}^{-1}({-D_d\dot e-K_de})+V(x，\dot x)\dot x+F_g(x)$

刚性机器人笛卡尔阻抗控制算法

1 刚性机器人的运动学与动力学

2 刚性机器人笛卡尔动力学方程

3 笛卡尔阻抗控制