虚拟模型控制

什么是VMC

VMC是一种直觉控制方式，其核心思想是利用假想的虚拟构件(如弹簧、阻尼器、轴承等等)连接机器人内部作用点，或者连接作用点与外部环境，产生相应的虚拟力来“驱使”机器人实现期望的运动。这些虚拟力通过 Jacobian 矩阵计算得到期望的关节力矩，作为关节控制的输入，驱动机器人运动以产生和虚拟构件一样的作用效果。VMC的应用关键在于两点：一是在每个需要控制的自由度上构造恰当的虚拟构件以产生合适的虚拟力；二是在不同的相位状态利用相应的 Jacobian 矩阵计算期望的关节力矩。

雅可比矩阵

虚拟力不是实际存在的作用力或力矩，机器人的运动最终是通过关节力矩实现的。为了将工作空间(Task Space)的力或力矩映射成关节空间(Joint Space)的关节力矩，需要先求得这两个空间的位置映射关系，即正运动学模型： \[ \pmb{x} = f(\pmb{q}) \tag{1} \] 其中，\(\pmb{x} = [x_1\ x_2 \ ... x_m]^T\) 为机体坐标系相对于地面坐标系的\(m\)个自由度对应的位姿向量，\(\pmb{q} = [q_1 \ q_2 \ ... q_n]^T\) 为\(n\)个关节变量的位置向量。对式 （1），分别求 \(\pmb{x}\) 对 \(\pmb{q}\) 的偏导数得： \[ \begin{cases} \delta x_1 = \frac{\partial f1}{\partial q_1} \delta q1 + \ ... + \frac{\partial f1}{\partial q_n} \delta qn \\ \vdots \\ \delta x_m = \frac{\partial fm}{\partial q_1} \delta q1 + \ ... + \frac{\partial fm}{\partial q_n} \delta qn\\ \end{cases} \tag{2} \] 即 \[ \delta \pmb{x} = \begin{bmatrix} \frac{\partial f1}{\partial q_1}&...&\frac{\partial f1}{\partial q_n} \\ \vdots&\ddots&\vdots\\ \frac{\partial fm}{\partial q_1}&...&\frac{\partial fm}{\partial q_n} \\ \end{bmatrix} \delta \pmb{q} \tag{3} \] 可简写为 \[ \delta \pmb{x}_{(m,1)} = \pmb{J}_{(m,n)} \cdot \delta \pmb{q}_{(n,1)} \tag{4} \] \(\pmb{J}_{(m,n)}\) 即为雅可比矩阵，它可以将关节速度 \(\dot{q}\) 映射成机体位姿速度 \(\dot{x}\)。

外部作用力和关节力矩的关系

虚功原理：

根据虚功原理，可得： \[ \pmb{\tau}^T \delta \pmb{q} + (-\pmb{F})^T\delta \pmb{x} = 0 \tag{5} \]

其中，\(\pmb{\tau} = [\tau_1\ \tau_2 \ ... \tau_n]^T\) 为关节力矩列向量，\(\pmb{F}=[F_1 \ F_2 \ ... F_m]^T\) 为外部作用力。根据式（4）、（5）可得外部作用力和关节力矩之间的关系为：

\[ \pmb{\tau} = \pmb{J}^T \pmb{F} \tag{6} \]

其中，\(\pmb{J}\) 为雅可比矩阵。

示例

三自由度机械手臂（腿）利用 VMC 进行追点

机器人环境

采用nyu_finger机器人，该机器人来自于项目ODRI / nyu_finger。可以将nyu_finger 简化为为一个三自由度机械臂模型，如下所示：

图1 三自由度机械臂简化模型

正运动学

首先要求解三自由度机械手臂的正运动学公式，方法如下：

• 采用 MDH 法（又称克雷格法）建立坐标系（【注】原论文采用的是 DH 法），如下所示：

图2 MDH法建立坐标系

• 建立机器人的连杆参数表；

  \(i\)	\(\alpha_{i-1}\)	\(a_{i-1}\)	\(d_i\)	\(\theta_{i}\)
  1	0	0	0	\(\theta_1\)
  2	\(-\pi/2\)	0	\(D_2\)	\(\theta_2\)
  3	0	\(L_2\)	\(D_3\)	\(\theta_3\)
  4	0	\(L_3\)	0	0

• 求末坐标系和初坐标系之间得变换矩阵 \(^0_4T\)

  两个相邻坐标系之间的变换矩阵通式为： \[ ^{i-1}_iT = \begin{bmatrix} cos\theta_i&-sin\theta_i&0&a_{i-1}\\ cos\alpha_{i-1} sin\theta_i&cos\alpha_{i-1} cos\theta_i&-sin\alpha_{i-1} &-d_isin\alpha_{i-1} \\ sin\alpha_{i-1} sin\theta_i&sin\alpha_{i-1} cos\theta_i&cos\alpha_{i-1} &d_icos\alpha_{i-1} \\ 0&0&0&1\\ \end{bmatrix} \\ \tag{7} \]

  首先，根据式（7），求出所有的两个相邻坐标系之间的变换矩阵为： \[ ^0_1T = \begin{bmatrix} cos\theta_1&-sin\theta_1&0&0\\ sin\theta_1&cos\theta_1&0 &0 \\ 0 &0 &1 &0 \\ 0 &0 &0 &1\\ \end{bmatrix} \\ \tag{8} \]

  \[ ^1_2T = \begin{bmatrix} cos\theta_2&-sin\theta_2&0&0\\ 0 &0 &1 &D_2\\ -sin\theta_2 &-cos\theta_2 &0 &0 \\ 0 &0 &0 &1\\ \end{bmatrix} \\ \tag{9} \]

  \[ ^2_3T = \begin{bmatrix} cos\theta_3&-sin\theta_3&0&L_2\\ sin\theta_3&cos\theta_3&0 &0 \\ 0 &0 &1 &D_3 \\ 0 &0 &0 &1\\ \end{bmatrix} \\ \tag{10} \]

  \[ ^3_4T = \begin{bmatrix} 1 &0 &0 &L_3\\ 0 &1 &0 &0 \\ 0 &0 &1 &0 \\ 0 &0 &0 &1\\ \end{bmatrix} \\ \tag{11} \]

  然后，将上述变换矩阵累乘得： \[ ^0_4T = ^0_1T ^1_2T^2_3T^3_4T \\ = \begin{bmatrix} c_1c_{23} &-c_1s_{23} &-s_1 &c_1(L_2c_2+L_3c_{23})-s_1(D_2+D_3)\\ s_1c_{23} &-s_1s_{23} &c_1 &s_1(L_2c_2+L_3c_{23})+c_1(D_2+D_3) \\ -s_{23} &-c_{23} &0 &-L_2s_2-L_3s_{23} \\ 0 &0 &0 &1\\ \end{bmatrix} \\ \tag{12} \]

  【注】\(c_1\) 为 \(cos\theta_1\)的简写，\(c_{23}\) 为 \(cos(\theta_2+\theta_3)\)的简写，其余类似。

• 求末端点在0号坐标系中的表示

  根据式（12），在0号坐标系表示末端点（4号坐标系的原点）为 \[ ^0P_{origin_4} = \begin{bmatrix} c_1(L_2c_2+L_3c_{23})-s_1(D_2+D_3)\\ s_1(L_2c_2+L_3c_{23})+c_1(D_2+D_3) \\ -L_2s_2-L_3s_{23} \\ \end{bmatrix} \\ \tag{13} \]

  式（13）即为式（1）描述的正运动学模型 \(\pmb{x} = f(\pmb{q}) \tag{1}\)，其中，\(\pmb{x} =\ ^0P_{origin_4}= [^0x_{origin_4}\ ^0y_{origin_4}\ ^0z_{origin_4}]^T\)，\(\pmb{q} = [\theta_1 \ \theta_2 \ \theta_3]^T\) 。

当然，上述矩阵的求解没有人会希望通过手算来解决，可以利用matlab写一个简单脚本进行计算：

% 用于生成变换矩阵.
% @filename: generate_T.m
% @functionname: generate_T
% @args: 
%   alpha, a, d, theta.
% @return:
%   T.

function T = generate_T(alpha, a, d, theta)
% 这里的T矩阵是式（7）的矩阵
T = [cos(theta) -sin(theta) 0 a;
     cos(alpha)*sin(theta) cos(alpha)*cos(theta) -sin(alpha) -d*sin(alpha);
     sin(alpha)*sin(theta) sin(alpha)*cos(theta) cos(alpha) d*cos(alpha);
     0 0 0 1];

% 1. 计算末坐标系相对于初坐标系的变换矩阵
% 2. 在初坐标系中描述末坐标系的原点
% @filename: mian.m

clc,clear,close all;
syms th1 th2 th3;
syms l1 l2 l3;
syms d1 d2 d3;
syms PI;

T01 = generate_T(0, 0, 0, th1);
T12 = generate_T(-PI/2, 0, d2, th2);
T23 = generate_T(0, l2, d3, th3);
T34 = generate_T(0, l3, 0, 0);

P = T01*T12*T23*T34;
P = subs(P,PI,pi); % For solving the problem: cos(pi/2) != 0.
P_simplify = simplify(P)

pos = P_simplify(1:3, 4)

% jacobian(pos, [th1, th2, th3])

求雅可比矩阵

根据式（2）对式（13）求偏导得： \[ \delta \pmb{x} = \begin{bmatrix} -s_1(L_2c_2+L_3c_{23})-c_1(D_2+D_3) &-c_1(L_2s_2+L_3s_{23}) &-L_3c_1s_{23}\\ c_1(L_2c_2+L_3c_{23})-s_1(D_2+D_3) &-s_1(L_2s_2+L_3s_{23}) &-L_3s_1s_{23}\\ 0 &-L_2c_2-L_3c_{23} &-L_3c_{23}\\ \end{bmatrix} \delta \pmb{q} \tag{14} \]

由式（14）得雅可比矩阵为 \[ \pmb{J} = \begin{bmatrix} -s_1(L_2c_2+L_3c_{23})-c_1(D_2+D_3) &-c_1(L_2s_2+L_3s_{23}) &-L_3c_1s_{23}\\ c_1(L_2c_2+L_3c_{23})-s_1(D_2+D_3) &-s_1(L_2s_2+L_3s_{23}) &-L_3s_1s_{23}\\ 0 &-L_2c_2-L_3c_{23} &-L_3c_{23}\\ \end{bmatrix} \tag{15} \]

当然，雅可比的计算也可用代码完成，如下所示：

% 1. 计算末坐标系相对于初坐标系的变换矩阵
% 2. 在初坐标系中描述末坐标系的原点
% 3. 计算雅可比矩阵
% @filename: mian.m

clc,clear,close all;
syms th1 th2 th3;
syms l1 l2 l3;
syms d1 d2 d3;
syms PI;

T01 = generate_T(0, 0, 0, th1);
T12 = generate_T(-PI/2, 0, d2, th2);
T23 = generate_T(0, l2, d3, th3);
T34 = generate_T(0, l3, 0, 0);

P = T01*T12*T23*T34;
P = subs(P,PI,pi);
P_simplify = simplify(P)

pos = P_simplify(1:3, 4)

jacobian(pos, [th1, th2, th3])

假设虚拟力

假设用一个虚拟构件（弹簧阻尼器）连接机械臂末端和外界，将该虚拟构件产生的作用在机械臂末端点的虚拟力记为\(F\)，可将虚拟力\(F\)分解到三个方向，即 \[ \pmb{F} = \begin{bmatrix} F_x\\ F_y\\ F_z\\ \end{bmatrix} = \begin{bmatrix} k_x &0 &0\\ 0 &k_y &0\\ 0 &0 &k_z\\ \end{bmatrix} \begin{bmatrix} x_d - x\\ y_d - y\\ z_d - z\\ \end{bmatrix} + \begin{bmatrix} b_x &0 &0\\ 0 &b_y &0\\ 0 &0 &b_z\\ \end{bmatrix} \begin{bmatrix} \dot{x_d} - \dot{x}\\ \dot{y_d} - \dot{y}\\ \dot{z_d} - \dot{z}\\ \end{bmatrix} \tag{16} \]

求解扭矩

将式（15）、（16）代入式（6）可得关节力矩。

代码

demo_quadruped_VMC

参考

[1] 谢惠祥. 四足机器人对角小跑步态虚拟模型直觉控制方法研究[D]. 国防科学技术大学[2025]

@phdthesis{谢惠祥0四足机器人对角小跑步态虚拟模型直觉控制方法研究,
  title={四足机器人对角小跑步态虚拟模型直觉控制方法研究},
  author={谢惠祥},
  school={国防科学技术大学},
}