Matlab机器人工具箱8 雅可比-EW帮帮网

参考视频：【MATLAB机器人工具箱10.4 机械臂仿真教学（未完结）】 https://www.bilibili.com/video/BV1q44y1x7WC/?p=11&share_source=copy_web&vd_source=2c56c6a2645587b49d62e5b12b253dca

这一节将介绍常用的雅可比函数jacob0和jacobe，我们先复习速通一下位移雅可比、速度雅可比和力域雅可比，并推导不同坐标系下的雅可比如何变化。
最后用两个例子帮助大家熟悉雅可比函数的使用。

雅可比复习速通

逆解离不开雅可比，我们常常需要刻画末端执行器位姿与关节变量位姿变化的关系，这样才能知道——手要够到那个把手，我的手腕、手肘、肩膀应该如何响应？

刻画这个关系的，就是雅可比矩阵，数学上，他代表笛卡尔空间的位姿对关节空间变量的导数。

末端执行器每一维的变化都受各个关节变化的影响。这是显然的，手的位置是由各个关节共同决定的。因为这个求导过程就是一个多元函数求导法则的应用。

接下来我们一起看一下这个求导过程，以及如何计算雅可比矩阵。

末端执行器每一维的变化都受各个关节变化的影响。

因此，每一维的笛卡尔位姿都由六个关节共同决定，由微分方程得到位姿雅可比。
$y_1 = f_1(x_1, x_2, x_3, x_4, x_5, x_6) \\ y_2 = f_2(x_1, x_2, x_3, x_4, x_5, x_6) \\ \vdots \\ y_6 = f_6(x_1, x_2, x_3, x_4, x_5, x_6)$
我们可以构建出笛卡尔坐标和关节坐标的关系。
由微分法则，对两侧求微分
$\delta y_1 = \frac{\partial f_1}{\partial x_1} \delta x_1 + \frac{\partial f_1}{\partial x_2} \delta x_2 + \cdots + \frac{\partial f_1}{\partial x_6} \delta x_6 \\ \delta y_2 = \frac{\partial f_2}{\partial x_1} \delta x_1 + \frac{\partial f_2}{\partial x_2} \delta x_2 + \cdots + \frac{\partial f_2}{\partial x_6} \delta x_6 \\ \vdots \\ \delta y_6 = \frac{\partial f_6}{\partial x_1} \delta x_1 + \frac{\partial f_6}{\partial x_2} \delta x_2 + \cdots + \frac{\partial f_6}{\partial x_6} \delta x_6$
可以写完张量形式： $\delta Y = \frac{\partial F}{\partial X} \delta X$

此时，
两端同时对时间求导，得到：
$\dot{Y} = J(X) \dot{X}$
转化为
${}^0v = {}^0J(\Theta) \dot{\Theta}$
即构建出笛卡尔速度和关节速度的关系。
因此有 $\dot{\Theta} = J^{-1}(\Theta) v$ 。

这就是速度雅可比。

同理的，我们还可以用虚功原理，得到力域雅可比：

力域雅可比复习

虚功原理

功是标量，在哪做功都一样，因此在笛卡尔系的功=在关节坐标系的功

有 $\mathbf{F} \cdot \delta \chi = \tau \cdot \delta \Theta$

其中：
F是作用在末端执行器的笛卡尔力
δX是末端执行器的微位移
τ是关节力矩
δΘ是关键微位移

又，刚刚求得位移雅可比有这样的关系：
${}^0v = {}^0J(\Theta) \dot{\Theta}$
这里改为：
$\delta \chi = J \delta \Theta$
带入有：
$\mathbf{F}^T \delta \chi = \tau^T \delta \Theta$
$\mathbf{F}^T J \delta \Theta = \tau^T \delta \Theta$
$\mathbf{F}^T J = \tau^T$

最后得到力域雅可比的表达：
$\tau = J^T \mathbf{F}$

这样，我们就对外部力和关节力做了映射。他代表着，你删我一巴掌，你的手肘、肩膀、手腕要如何发力。

Matlab中，雅可比矩阵有两种形式，

基坐标系下的雅可比矩阵 (J0)：描述关节速度对基坐标系下末端执行器速度的影响。
末端执行器坐标系下的雅可比矩阵 (Je)：描述关节速度对末端执行器自身坐标系下速度的影响。

意思是，一个考虑你的手腕、手肘、肩膀运动时，“你的手相对于房间”是如何运动的。
另一个考虑你的手腕、手肘、肩膀运动时，“你的手相对于它自己”是如何运动的（即手“认为”自己在怎么动）。

他们之间肯定是存在一定关系的。

雅可比矩阵参考坐标系的变换复习速通

雅可比矩阵参考坐标系的变换在《机器人学导论》（约翰J克雷格著）第106页，如果没看过也没关系，下面我们会一起快速复习一遍。

已知坐标系 B 中的雅可比矩阵，即

$\begin{pmatrix} {}^B v \\ {}^B \omega \end{pmatrix} = {}^B v = {}^B J(\Theta) \dot{\Theta}$ ----------①

我们希望得到雅可比矩阵在另一个坐标系A下的表达式。如何刻画这个关系呢？
聪明的你一定注意到了AB坐标系间速度的关系满足下式：

$\begin{pmatrix} {}^A v \\ {}^A \omega \end{pmatrix} = \begin{pmatrix} {}^A_B R & 0 \\ 0 & {}^A_B R \end{pmatrix} \begin{pmatrix} {}^B v \\ {}^B \omega \end{pmatrix}$

此时，我们可以用B中的雅可比矩阵的等式①替换掉 $\begin{pmatrix} {}^B v \\ {}^B \omega \end{pmatrix}$

可以得到：

$\begin{pmatrix} {}^A v \\ {}^A \omega \end{pmatrix} = \begin{pmatrix} {}^A_B R & 0 \\ 0 & {}^A_B R \end{pmatrix} {}^B J(\Theta) \dot{\Theta}$

同理，
我们可以将A中的速度用雅可比替换掉：

${}^A J(\Theta) = \begin{pmatrix} {}^A_B R & 0 \\ 0 & {}^A_B R \end{pmatrix} {}^B J(\Theta)$

这就是在B坐标系下的雅可比与在A坐标系下的雅可比的关系啦！

由此，我们就可以得到基坐标系下的雅可比矩阵 (J0)与末端执行器坐标系下的雅可比矩阵 (Je)之间的变换等式。

首先给定某一时刻的关节变量q0=[0, 0, 0, 0, 0, 0]；
然后用fkine正运动学计算此时末端执行器的变换矩阵R。
假设现已知基坐标系下的雅可比，要计算在工具坐标系下的雅可比。那么显然{e}系的速度左乘上面求得的R就是{O}的速度：

$\begin{pmatrix} {}^e v \\ {}^e \omega \end{pmatrix} = \begin{pmatrix} {}^e_O R & 0 \\ 0 & {}^e_O R \end{pmatrix} \begin{pmatrix} {}^O v \\ {}^O \omega \end{pmatrix}$

而刻画工具坐标系和基坐标系的变化矩阵 ${}^e_O R$ 就是fkine得到的矩阵R。
因此根据公式，提取这个矩阵旋转部分作为小r，拼接出6X6的大R矩阵
${}^e J(\Theta) = \begin{pmatrix} {}^e_O R & 0 \\ 0 & {}^e_O R \end{pmatrix} {}^O J(\Theta)$
即可得到 $J_e=RJ_0$

分别对应两个函数：

Jacob0：计算基坐标系下q位姿的雅可比矩阵

Jacobe：计算末端执行器坐标系下q位姿的雅可比矩阵

雅可比矩阵参考坐标系的变换代码验证

先建模。

clear;
clc;

L(1) = Link('revolute','d',0.216,'a',0,'alpha',pi/2);
L(2) = Link('revolute','d',0,'a',0.5,'alpha',0,'offset',pi/2);
L(3) = Link('revolute','d',0,'a',sqrt(0.145^2+0.42746^2),'alpha',0, 'offset', -atan(427.46 / 145));
L(4) = Link('revolute','d',0,'a',0,'alpha',pi/2,'offset', atan(427.46/145));
L(5) = Link('revolute', 'd', 0.258, 'a', 0, 'alpha', 0);

Five_dof=SerialLink(L,'name','5-dof');
Five_dof.base=transl(0,0,0.28);

我们来实践一下，看看雅可比矩阵参考坐标系的变换推导是否正确：

设定关节变量为0，分别计算此时的基坐标系雅可比和工具坐标系雅可比J0、Je

q0=[0 0 0 0 0];

%% 计算基坐标系和末端执行器坐标系下的雅可比矩阵
J0 = Five_dof.jacob0(q0);  % 基坐标系下的雅可比矩阵
Je = Five_dof.jacobe(q0);   % 末端执行器坐标系下的雅可比矩阵

J0 =
   -0.0000   -0.6450   -0.1450    0.0000         0
    0.6855    0.0000    0.0000    0.0000         0
   -0.0000    0.6855    0.6855    0.2580         0
   -0.0000         0         0         0    1.0000
         0   -1.0000   -1.0000   -1.0000   -0.0000
    1.0000    0.0000    0.0000    0.0000   -0.0000

Je =
    0.0000    0.6855    0.6855    0.2580         0
   -0.6855    0.0000    0.0000         0         0
   -0.0000   -0.6450   -0.1450         0         0
    1.0000         0         0         0         0
    0.0000    1.0000    1.0000    1.0000         0
   -0.0000    0.0000    0.0000    0.0000    1.0000

接着，正运动学得到末端执行器的变换矩阵T，并获取T中的旋转矩阵r。
接着把r拼接成一个6X6的矩阵R。
最后检查J1是否等于Je
$RJ_0=J_e=J_1$

J2是否等于J0
$R^{-1}J_e=J0=J_2$

%% 计算旋转矩阵 R 和变换矩阵
T = Five_dof.fkine(q0);     % 获取末端执行器的齐次变换矩阵
r = t2r(T);                 % 提取旋转部分 r
R = [r zeros(3, 3);         % 构建 6x6 的旋转变换矩阵 R
     zeros(3, 3) r];

%% 雅可比矩阵之间的转换
J1 = R * J0;                % 使用 R 将 J0 转换为 Je
J2 = inv(R) * Je;           % 使用 R 的逆将 Je 转换回 J0
J3 = R \ Je;                % 使用左除运算符实现相同的效果

这里J3和J2是一个效果，只是inv(R)计算逆速度比直接左除慢。

检查输出显然是一样的，这就证明我们的坐标系雅可比变换是成功的！

J1 =
   -0.0000    0.6855    0.6855    0.2580         0
   -0.6855   -0.0000   -0.0000   -0.0000         0
    0.0000   -0.6450   -0.1450    0.0000         0
    1.0000    0.0000    0.0000    0.0000    0.0000
   -0.0000    1.0000    1.0000    1.0000    0.0000
   -0.0000   -0.0000   -0.0000   -0.0000    1.0000

J2 =
   -0.0000   -0.6450   -0.1450    0.0000         0
    0.6855   -0.0000   -0.0000   -0.0000         0
    0.0000    0.6855    0.6855    0.2580         0
   -0.0000    0.0000    0.0000    0.0000    1.0000
   -0.0000   -1.0000   -1.0000   -1.0000    0.0000
    1.0000   -0.0000   -0.0000   -0.0000   -0.0000

J2 =
   -0.0000   -0.6450   -0.1450    0.0000         0
    0.6855   -0.0000   -0.0000   -0.0000         0
    0.0000    0.6855    0.6855    0.2580         0
   -0.0000    0.0000    0.0000    0.0000    1.0000
   -0.0000   -1.0000   -1.0000   -1.0000    0.0000
    1.0000   -0.0000   -0.0000   -0.0000   -0.0000

利用雅可比绘制末端执行器速度变化图

思路很简单，给定初末位姿后调用jtraj，jtraj会返回三样东西——
Q每个插值点的关节变量；
dQ每个插值点的速度；
ddQ每个插值点的加速度；

[Q, dQ, ddQ] = jtraj(q0, q1, t);

要获取末端速度，就需要获取该插值点的雅可比，以及关节变量。
${}^0v = {}^0J(\Theta) \dot{\Theta}$
$\dot\Theta$ 就是速度dQ， $J(\Theta)$ 求的是基坐标系下的雅可比，因此需要调用jacob0函数获取。

那么就从jtraj的返回值中取出Q的第i项作为关节变量q，dQ的第i项作为关节速度，J0由jacob0计算得到，自然速度也就可以由J0和dq求得了！

q = Q(i, :).';       % 当前时刻的关节角度
dq = dQ(i, :).';     % 当前时刻的关节速度
J0 = Five_dof.jacob0(q); % 基坐标系下的雅可比矩阵
v(:, i) = J0 * dq;   % 计算末端执行器的速度

接着，把这一刻的求取方式扩展到整个插值过程，用for循环：

for i = 1:length(t)
    q = Q(i, :).';       % 当前时刻的关节角度
    dq = dQ(i, :).';     % 当前时刻的关节速度
    J0 = Five_dof.jacob0(q); % 基坐标系下的雅可比矩阵
    v(:, i) = J0 * dq;   % 计算末端执行器的速度
end

最后绘图即可。

完整代码

%%

q1 = [pi/2 0 0 0 0]; % 目标关节角度
t = 0:0.05:2;        % 时间向量，从0到2秒，步长为0.05秒

[Q, dQ, ddQ] = jtraj(q0, q1, t); % 生成关节轨迹及其导数
Five_dof.plot(Q);                % 绘制机械臂在不同时间点的姿态

v = zeros(6, length(t)); % 初始化速度矩阵

for i = 1:length(t)
    q = Q(i, :).';       % 当前时刻的关节角度
    dq = dQ(i, :).';     % 当前时刻的关节速度
    J0 = Five_dof.jacob0(q); % 基坐标系下的雅可比矩阵
    v(:, i) = J0 * dq;   % 计算末端执行器的速度
end

plot(t, v(1:3, :)'); % 绘制线速度分量
legend('vx', 'vy', 'vz');
xlabel('Time (s)');
ylabel('Linear Velocity (m/s)');
title('End-effector Linear Velocities');

figure;
plot(t, v(4:6, :)'); % 绘制角速度分量
legend('\theta_x', '\theta_y', '\theta_z');
xlabel('Time (s)');
ylabel('Angular Velocity (rad/s)');
title('End-effector Angular Velocities');

运行效果

启动后会获得如下图像：

我们设定的目标是绕z转90度，从图一看出，只有vx、vy有变化；
从图二看出，角速度也只有z轴有。

Matlab机器人工具箱8 雅可比

雅可比复习速通

力域雅可比复习

虚功原理

雅可比矩阵参考坐标系的变换复习速通

雅可比矩阵参考坐标系的变换代码验证

利用雅可比绘制末端执行器速度变化图

完整代码

运行效果

写在后面

网站公告

今日签到

热门文章

最新发布