机器人矩阵运算MATLAB计算
在学习机器人的过程中，动不动就是多个矩阵的计算，计算过程繁琐且不小心容易出错，下面结合原理给出常见计算的MATLAB代码以简化计算过程（有兴趣的读者可以自行"转译"成C语言小或者python小程序，最好利用JavaScript语言写成HTML网页版的程序这样在手机上也可以进行操作）
旋转&平移矩阵
MATLAB完整代码见最后
数学原理
1. 基本旋转矩阵（绕参考系坐标轴，右手定则）
绕 X 轴旋转 α\alphaα 角：
Rx(α)=[1000cosα−sinα0sinαcosα] R_x(\alpha) = \begin{bmatrix} 1 & 0 & 0 \\ 0 & \cos\alpha & -\sin\alpha \\ 0 & \sin\alpha & \cos\alpha \end{bmatrix} Rx(α)= 1000cosαsinα0−sinαcosα
绕 Y 轴旋转 β\betaβ 角：
Ry(β)=[cosβ0sinβ010−sinβ0cosβ] R_y(\beta) = \begin{bmatrix} \cos\beta & 0 & \sin\beta \\ 0 & 1 & 0 \\ -\sin\beta & 0 & \cos\beta \end{bmatrix} Ry(β)= cosβ0−sinβ010sinβ0cosβ
绕 Z 轴旋转 γ\gammaγ 角：
Rz(γ)=[cosγ−sinγ0sinγcosγ0001] R_z(\gamma) = \begin{bmatrix} \cos\gamma & -\sin\gamma & 0 \\ \sin\gamma & \cos\gamma & 0 \\ 0 & 0 & 1 \end{bmatrix} Rz(γ)= cosγsinγ0−sinγcosγ0001
注：代码中角度以"度"输入，内部自动转换为弧度。
2. 平移矩阵（沿参考系坐标轴）
沿 X 轴平移 xpx_pxp：
Txp=[100xp010000100001] T_{x_p} = \begin{bmatrix} 1 & 0 & 0 & x_p \\ 0 & 1 & 0 & 0 \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1 \end{bmatrix} Txp= 100001000010xp001
同理 TypT_{y_p}Typ、TzpT_{z_p}Tzp。
3. 齐次变换组合
每个操作（旋转或平移）对应一个 4×44\times44×4 齐次矩阵：
T=[Rt0T1] T = \begin{bmatrix} R & \mathbf{t} \\ \mathbf{0}^T & 1 \end{bmatrix} T=[R0Tt1]
组合顺序 ：若操作序列为 {op1,op2,...,opN}\{op_1, op_2, \dots, op_N\}{op1,op2,...,opN}，则总变换矩阵为：
Ttotal=TopN⋯Top2⋅Top1 T_{\text{total}} = T_{op_N} \cdots T_{op_2} \cdot T_{op_1} Ttotal=TopN⋯Top2⋅Top1
即先执行的操作放在乘积右侧（左乘规则）。
用法说明
参数设置
alpha,beta,gamma：绕 X, Y, Z 轴的旋转角（度），可设为数值或符号（如sym('theta')）。x_p,y_p,z_p：沿 X, Y, Z 轴的平移量，可设为数值或符号。rot_seq：操作序列 cell 数组，元素为：'x','y','z'-- 绕对应轴旋转'x_p','y_p','z_p'-- 沿参考系对应轴平移
运行流程
- 根据
alpha, beta, gamma计算三个旋转矩阵。 - 遍历
rot_seq，为每个操作构造 4×44\times44×4 齐次矩阵。 - 左乘累积得到总变换矩阵
out。 - 可将
out应用到任意齐次矩阵origin（如示例中的初始位姿）。
示例
matlab
rot_seq = {'y_p', 'x', 'z'}; % 先沿 Y 平移，再绕 X 旋转，最后绕 Z 旋转
alpha = 90; beta = 0; gamma = 0;
x_p = 0; y_p = 100; z_p = 0;完整 MATLAB 代码
matlab
% === 角度定义（符号量，便于解析/化简/符号推导） ===
% 使用符号变量可以在符号工具箱中进行解析、简化和 pretty 打印。
% alpha/beta/gamma 表示分别绕参考坐标系的 X/Y/Z 轴的旋转角（单位：度）。
% 这里给出默认值；你可以直接修改为符号表达式或其它数值。
alpha = sym(90); % 绕 X 轴旋转角 α（度），可改为 sym('theta1') 等符号表达式
beta = sym(0); % 绕 Y 轴旋转角 β（度）
gamma = sym(0); % 绕 Z 轴旋转角 γ（度）
% === 将角度转换为弧度（符号三角函数使用弧度） ===
% 在符号计算（sym）中，sin/cos 等函数的输入应为弧度，因此这里做单位换算。
alpha_rad = alpha * pi / 180; % α (rad)
beta_rad = beta * pi / 180; % β (rad)
gamma_rad = gamma * pi / 180; % γ (rad)
% === 构造绕各轴的基础旋转矩阵（符号形式） ===
% 采用右手坐标系，下面的矩阵表示将矢量在参考系中绕对应轴旋转。
% 注意：这些是 3x3 的纯旋转矩阵，不包含平移。
R_x = [1, 0, 0;
0, cos(alpha_rad), -sin(alpha_rad);
0, sin(alpha_rad), cos(alpha_rad)];
R_y = [ cos(beta_rad), 0, sin(beta_rad);
0, 1, 0;
-sin(beta_rad), 0, cos(beta_rad)];
R_z = [cos(gamma_rad), -sin(gamma_rad), 0;
sin(gamma_rad), cos(gamma_rad), 0;
0, 0, 1];
% === 可视化单步旋转矩阵（便于验证） ===
% 使用 pretty 展示符号矩阵，double(alpha) 仅用于格式化输出角度值。
fprintf('绕 X 轴旋转矩阵 (Rx, α = %.2f°):\n', double(alpha));
pretty(R_x)
fprintf('绕 Y 轴旋转矩阵 (Ry, β = %.2f°):\n', double(beta));
pretty(R_y)
fprintf('绕 Z 轴旋转矩阵 (Rz, γ = %.2f°):\n', double(gamma));
pretty(R_z)
% ---------------- 操作序列定义 ----------------
% rot_seq: 支持混合旋转和沿轴平移的操作序列。
% 有效条目包括：'x','y','z'（分别表示绕 X/Y/Z 旋转）
% 以及 'x_p','y_p','z_p'（分别表示沿参考坐标系 X/Y/Z 方向的平移）。
% 例如: {'z','x','y_p'} 表示先绕 Z 旋转，再绕 X 旋转，最后沿 Y 平移。
rot_seq = {'y_p', 'x', 'z'}; % 可修改为任意组合
% 定义沿轴的平移量（符号量，便于推导和替换）
% 你可以修改这些默认值，或将它们设为符号变量以做更一般的推导。
x_p = sym(0); % 沿 X 方向的平移量（用于 'x_p' 条目）
y_p = sym(100); % 沿 Y 方向的平移量（用于 'y_p' 条目）
z_p = sym(0); % 沿 Z 方向的平移量（用于 'z_p' 条目）
% 准备一个 cell 数组保存每步的 4x4 齐次变换矩阵
N = numel(rot_seq);
Ts = cell(1, N);
for i = 1:N
op = rot_seq{i};
switch op
case 'x'
R = R_x; t = sym([0;0;0]);
case 'y'
R = R_y; t = sym([0;0;0]);
case 'z'
R = R_z; t = sym([0;0;0]);
case 'x_p'
R = eye(3); t = sym([x_p; 0; 0]);
case 'y_p'
R = eye(3); t = sym([0; y_p; 0]);
case 'z_p'
R = eye(3); t = sym([0; 0; z_p]);
otherwise
error("rot_seq 中只允许 'x','y','z','x_p','y_p','z_p'");
end
T = [R, t; sym([0,0,0,1])];
Ts{i} = T;
% 如果是沿参考坐标系的平移操作，打印该平移对应的齐次矩阵
if any(strcmp(op, {'x_p','y_p','z_p'}))
switch op
case 'x_p'
fprintf('沿 X 方向平移矩阵 (T_x_p, x_p = %s):\n', char(x_p));
case 'y_p'
fprintf('沿 Y 方向平移矩阵 (T_y_p, y_p = %s):\n', char(y_p));
case 'z_p'
fprintf('沿 Z 方向平移矩阵 (T_z_p, z_p = %s):\n', char(z_p));
end
pretty(T)
end
end
% === 组合所有齐次变换（修正顺序：左乘，先执行的在右边）===
% 物理顺序：rot_seq 中第一个操作最先作用在点上。
% 数学表达：最终变换矩阵 = T_N * ... * T_2 * T_1
% 因此用左乘累积：out = T_i * out
out = eye(4);
for i = 1:N
out = Ts{i} * out; % 注意点：先执行的矩阵在乘积的右侧
end
% 构造用于打印的字符串（按照实际乘法顺序：先执行的变换在右边）
% 为了显示清晰，这里按 rot_seq 从左到右的顺序显示（即先罗列最先执行的操作），
% 但乘法顺序已在代码中正确实现。下面字符串仅用于说明序列内容。
seq_desc = strjoin(rot_seq, ' → ');
fprintf('\n操作序列（从左到右依次执行）: %s\n', seq_desc);
% 打印参数值列表
paramList = {};
for i = 1:N
op = rot_seq{i};
switch op
case 'x'
name = 'α'; val = alpha;
case 'y'
name = 'β'; val = beta;
case 'z'
name = 'γ'; val = gamma;
case 'x_p'
name = 'x_p'; val = x_p;
case 'y_p'
name = 'y_p'; val = y_p;
case 'z_p'
name = 'z_p'; val = z_p;
otherwise
name = op; val = sym('?');
end
% 格式化值
if isa(val, 'sym')
vstr = char(val);
elseif isnumeric(val) && isscalar(val)
if any(strcmp(op, {'x','y','z'}))
vstr = sprintf('%.2f°', double(val));
else
vstr = sprintf('%.4g', double(val));
end
else
vstr = char(val);
end
paramList{end+1} = sprintf('%s=%s', name, vstr);
end
params_str = strjoin(paramList, ', ');
fprintf('组合齐次变换矩阵 T = %s\n', params_str);
pretty(out)
% ----------------- 应用变换到指定矩阵 origin -----------------
% origin: 示例四维齐次变换矩阵（具体数值参数，可直接替换为你需要的值）
origin = sym([0, 0, 1, 1070;
0, -1, 0, 0;
1, 0, 0, 1300;
0, 0, 0, 1]);
fprintf('\n原矩阵 origin（示例具体参数）:\n');
pretty(origin)
% 计算变换后的矩阵（按顺序应用组合变换 T）
transformed = simplify(out * origin);
fprintf('变换后矩阵 T * origin:\n');
pretty(transformed)
% ---------------------------------------------------------------微分转动&平移
MATLAB完整代码见最后
-
总体目标 ：该代码用于符号计算多个微小平移与微小旋转的齐次变换组合 ，采用一阶近似（旋转矩阵近似为 I+[δθ]×\mathbf{I} + [\delta\theta]\timesI+[δθ]×）。用户可自由定义若干组微分旋转角度（绕 X、Y、Z）和微分平移向量，并指定这些运动的时间顺序 ，最终得到总变换矩阵 Ttotal\mathbf{T}{\text{total}}Ttotal。
-
定义微分运动变量 ：在代码开头，用户设置三个方向的微分转角
dalpha、dbeta、dgamma（单位弧度），以及多组微分平移量（如dx1,dy1,dz1、dx2,dy2,dz2）。所有变量均通过sym定义为符号量，既支持数值也支持代数符号。如需更多平移组，可继续添加dx3,dy3,dz3。 -
操作序列与矩阵构造 ：用户通过元胞数组
rot_seq按时间先后 列出操作，例如{'y', 't1', 'x'}表示先绕 Y 轴微分旋转，再执行第 1 组微分平移，最后绕 X 轴微分旋转。代码依次为每个操作构造 4×44\times44×4 齐次矩阵：旋转操作使用对应的微分旋转矩阵，平移操作使用单位旋转矩阵与平移向量。这些矩阵存入Ts待组合。 -
组合顺序（关键） ：按照机器人学惯例，先发生的变换在矩阵乘法的最右侧 。代码采用循环
out = Ts{i} * out从第一个操作开始右乘累积，即总变换为 Tlast×⋯×Tfirst\mathbf{T}{\text{last}} \times \dots \times \mathbf{T}{\text{first}}Tlast×⋯×Tfirst。例如对{'y','t1','x'}，最终 Ttotal=Tx⋅Tt1⋅Ty\mathbf{T}_{\text{total}} = \mathbf{T}x \cdot \mathbf{T}{t1} \cdot \mathbf{T}_yTtotal=Tx⋅Tt1⋅Ty，保证应用顺序为"先 Y 旋转、再平移、最后 X 旋转"。 -
示例应用 ：代码提供了一个任意初始齐次矩阵
origin，通过simplify(out * origin)计算经过上述微分运动组合后的新位姿，便于直观验证。输出还包括每一步的详细矩阵、总矩阵符号结果以及操作顺序说明，方便用户修改参数或序列来研究微小运动的合成与顺序依赖关系。
一、微分运动的基本概念
1.1 微分转动（小角度旋转）
当旋转角 δθ\delta\thetaδθ 非常小时（单位：弧度），有近似：
cosδθ≈1,sinδθ≈δθ \cos\delta\theta \approx 1,\quad \sin\delta\theta \approx \delta\theta cosδθ≈1,sinδθ≈δθ
忽略高阶小量（δθ2\delta\theta^2δθ2 及以上），绕坐标轴的旋转矩阵可线性化。
绕 X 轴旋转 δα\delta\alphaδα ：
Rx(δα)=[1000cosδα−sinδα0sinδαcosδα]≈[10001−δα0δα1] R_x(\delta\alpha) = \begin{bmatrix} 1 & 0 & 0 \\ 0 & \cos\delta\alpha & -\sin\delta\alpha \\ 0 & \sin\delta\alpha & \cos\delta\alpha \end{bmatrix} \approx \begin{bmatrix} 1 & 0 & 0 \\ 0 & 1 & -\delta\alpha \\ 0 & \delta\alpha & 1 \end{bmatrix} Rx(δα)= 1000cosδαsinδα0−sinδαcosδα ≈ 10001δα0−δα1
绕 Y 轴旋转 δβ\delta\betaδβ （注意符号方向，保持右手定则）：
Ry(δβ)≈[10δβ010−δβ01] R_y(\delta\beta) \approx \begin{bmatrix} 1 & 0 & \delta\beta \\ 0 & 1 & 0 \\ -\delta\beta & 0 & 1 \end{bmatrix} Ry(δβ)≈ 10−δβ010δβ01
绕 Z 轴旋转 δγ\delta\gammaδγ ：
Rz(δγ)≈[1−δγ0δγ10001] R_z(\delta\gamma) \approx \begin{bmatrix} 1 & -\delta\gamma & 0 \\ \delta\gamma & 1 & 0 \\ 0 & 0 & 1 \end{bmatrix} Rz(δγ)≈ 1δγ0−δγ10001
这些矩阵可以统一表达为：
R≈I+[δθ]× R \approx I + [\delta\boldsymbol{\theta}]_\times R≈I+[δθ]×
其中 [δθ]×[\delta\boldsymbol{\theta}]_\times[δθ]× 是微分旋转向量 δθ=[δα, δβ, δγ]T\delta\boldsymbol{\theta}=[\delta\alpha,\;\delta\beta,\;\delta\gamma]^Tδθ=[δα,δβ,δγ]T 的反对称矩阵。
1.2 微分平移（小位移）
沿坐标轴平移 dx,dy,dzdx,dy,dzdx,dy,dz 的齐次变换矩阵为：
Ttrans=[I3×3[dxdydz]01] T_{\text{trans}} = \begin{bmatrix} I_{3\times3} & \begin{bmatrix} dx \\ dy \\ dz \end{bmatrix} \\ 0 & 1 \end{bmatrix} Ttrans= I3×30 dxdydz 1
没有旋转部分，只有平移。
二、齐次变换矩阵的组合
2.1 基本形式
每个操作（旋转或平移）都表示为 4×44\times44×4 齐次矩阵：
T=[Rt0T1] T = \begin{bmatrix} R & \mathbf{t} \\ \mathbf{0}^T & 1 \end{bmatrix} T=[R0Tt1]
2.2 组合顺序（至关重要）
-
物理理解 ：操作序列
rot_seq从左到右表示 时间顺序（先发生 → 后发生）。 -
数学实现 ：对一个点 p\mathbf{p}p 应用第一个变换 T1T_1T1，再应用 T2T_2T2，得到：
p′=T2 (T1 p)=(T2T1) p \mathbf{p}' = T_2\,(T_1\,\mathbf{p}) = (T_2 T_1)\,\mathbf{p} p′=T2(T1p)=(T2T1)p因此总变换矩阵 为：
Ttotal=TN⋯T2 T1 T_{\text{total}} = T_N \cdots T_2 \, T_1 Ttotal=TN⋯T2T1即"后发生的左乘，先发生的右乘"。
代码中通过左乘累积实现：
matlab
out = eye(4);
for i = 1:N
out = Ts{i} * out; % 先执行的 Ts{1} 在最右边
end完整MATLAB代码示例
matlab
%% 支持多个微分平移的齐次变换组合（符号计算）
% 微分转动：一阶近似 R ≈ I + [δθ]×（单位：弧度）
% 微分平移：Trans(dx, dy, dz) = [I, [dx;dy;dz]; 0,1]
% 注意：操作序列按时间顺序（先做什么后做什么），矩阵乘法自动调整为从右向左。
clear; clc;
% ==================== 1. 定义微分运动变量 ====================
% 微分转动角（小角度，单位：弧度）
dalpha = sym(0); % 绕 X 轴转动 δθ_x (rad)
dbeta = sym(0.2); % 绕 Y 轴转动 δθ_y (rad)
dgamma = sym(0); % 绕 Z 轴转动 δθ_z (rad)
% 定义多组微分平移量（每组包含 dx, dy, dz）
% 平移组 1
dx1 = sym(0.2); dy1 = sym(0.1); dz1 = sym(0.1);
% 平移组 2
dx2 = sym(0.3); dy2 = sym(0.2); dz2 = sym(0.05);
% 如果需要更多组，可以继续定义 dx3, dy3, dz3 等；此时也应相应的修改下面的相关参数
% ==================== 2. 构造微分旋转矩阵 ====================
R_x_d = [1, 0, 0;
0, 1, -dalpha;
0, dalpha, 1];
R_y_d = [1, 0, dbeta;
0, 1, 0;
-dbeta, 0, 1];
R_z_d = [1, -dgamma, 0;
dgamma, 1, 0;
0, 0, 1];
fprintf('====== 微分旋转矩阵（一阶近似，角度单位：rad）======\n');
fprintf('绕 X 轴 (δθ_x = %s rad):\n', char(dalpha)); pretty(R_x_d)
fprintf('绕 Y 轴 (δθ_y = %s rad):\n', char(dbeta)); pretty(R_y_d)
fprintf('绕 Z 轴 (δθ_z = %s rad):\n', char(dgamma)); pretty(R_z_d)
% ==================== 3. 定义操作序列（按时间顺序） ====================
% 支持的旋转操作：'x', 'y', 'z'
% 支持的平移操作：'t1', 't2', ... （对应上面定义的平移组）
% 示例：rot_seq = {'z', 't1', 'x', 't2', 'y'}先绕 Z 旋转，再平移组1，再绕 X 旋转，再平移组2，再绕 Y 旋转
rot_seq = {'y', 't1', 'x'};
% ==================== 4. 逐步骤生成齐次变换矩阵 ====================
N = numel(rot_seq);
Ts = cell(1, N); % 存储每一步的 4x4 齐次矩阵
fprintf('\n====== 各步骤微分变换矩阵（按时间顺序） ======\n');
for i = 1:N
op = rot_seq{i};
switch op
case 'x'
R = R_x_d; t = sym([0;0;0]);
fprintf('步骤 %d: 绕 X 轴微分旋转 (δθ_x = %s rad)\n', i, char(dalpha));
case 'y'
R = R_y_d; t = sym([0;0;0]);
fprintf('步骤 %d: 绕 Y 轴微分旋转 (δθ_y = %s rad)\n', i, char(dbeta));
case 'z'
R = R_z_d; t = sym([0;0;0]);
fprintf('步骤 %d: 绕 Z 轴微分旋转 (δθ_z = %s rad)\n', i, char(dgamma));
case 't1'
R = eye(3); t = sym([dx1; dy1; dz1]);
fprintf('步骤 %d: 三维微分平移 Trans(%s, %s, %s)\n', i, char(dx1), char(dy1), char(dz1));
case 't2'
R = eye(3); t = sym([dx2; dy2; dz2]);
fprintf('步骤 %d: 三维微分平移 Trans(%s, %s, %s)\n', i, char(dx2), char(dy2), char(dz2));
% 如果需要更多平移组，继续添加 case 't3', 't4', ... 并定义对应的 dx3,dy3,dz3
otherwise
error('无效操作：%s。允许的操作：x, y, z, t1, t2, ...', op);
end
T = [R, t; sym([0,0,0,1])];
Ts{i} = T;
pretty(T)
end
% ==================== 5. 组合所有变换（修正乘法顺序） ====================
% 按照时间顺序：先发生的变换先乘到右侧（离点最近），后发生的左乘。
% 因此总变换矩阵 T_total = T_last * ... * T_first。
% 循环中采用 out = T_i * out 实现从右向左累积。
out = eye(4);
for i = 1:N
out = Ts{i} * out; % 注意：顺序，第一个操作位于最右侧
end
% 构造组合顺序字符串和参数列表（用于显示）
order_str = strjoin(cellfun(@(c) sprintf('T_%s', c), rot_seq, 'UniformOutput', false), ' → ');
paramList = {};
for i = 1:N
op = rot_seq{i};
switch op
case 'x', name='δθ_x'; val=dalpha;
case 'y', name='δθ_y'; val=dbeta;
case 'z', name='δθ_z'; val=dgamma;
case 't1', name='Trans1'; val=sprintf('(%s,%s,%s)', char(dx1),char(dy1),char(dz1));
case 't2', name='Trans2'; val=sprintf('(%s,%s,%s)', char(dx2),char(dy2),char(dz2));
otherwise, name=op; val='';
end
if ischar(val), paramList{end+1}=sprintf('%s=%s', name,val);
else, paramList{end+1}=sprintf('%s=%s', name,char(val));
end
end
params_str = strjoin(paramList, ', ');
fprintf('\n====== 组合后的微分齐次变换矩阵 ======\n');
fprintf('操作顺序（时间顺序）：%s\n', order_str);
fprintf('对应参数：%s\n', params_str);
fprintf('总变换矩阵 T = T_last * ... * T_first（先应用 T_first，后应用 T_last）\n');
pretty(out)
% ==================== 6. 示例：对任意初始矩阵应用微分变换 ====================
origin = sym([0, 0, 1, 8;
1, 0, 0, 3;
0, 1, 0, 5;
0, 0, 0, 1]);
fprintf('\n====== 示例：初始矩阵 origin ======\n'); pretty(origin)
transformed = simplify(out * origin);
fprintf('====== 变换后矩阵 T * origin ======\n'); pretty(transformed)
fprintf('\n说明：\n');
fprintf('1. 序列中的操作按时间顺序从左到右执行。\n');
fprintf('2. 总变换矩阵的乘法顺序R1=TR，符合机器人学惯例。\n');
fprintf('3. 支持多个微分平移，只需添加平移组并在序列中使用 tN 即可。\n');微分平移&旋转及其等效
MATLAB完整代码见最后
微分变换的理论依据
1. 齐次变换矩阵与微分运动
在机器人学中，一个坐标系（如工具坐标系）相对于参考坐标系的位姿可用 4×44 \times 44×4 齐次变换矩阵表示：
C=[Rp0T1],R∈SO(3), p∈R3 \mathbf{C} = \begin{bmatrix} \mathbf{R} & \mathbf{p} \\ \mathbf{0}^T & 1 \end{bmatrix}, \quad \mathbf{R} \in SO(3),\ \mathbf{p} \in \mathbb{R}^3 C=[R0Tp1],R∈SO(3), p∈R3
其中 R\mathbf{R}R 为旋转矩阵，p\mathbf{p}p 为平移向量。
当坐标系经历一个微小的运动（平移和旋转）时，其齐次变换矩阵的变化量 dC\mathrm{d}\mathbf{C}dC 可表示为微分运动的结果。这个微分运动既可以在固定参考坐标系下描述，也可以在运动坐标系本身（即当前坐标系 C\mathbf{C}C）下描述。两种描述之间存在数学变换关系，这正是本脚本所实现的内容。
2. 参考坐标系下的微分运动与微分算子 F\mathbf{F}F
设参考坐标系下微小平移为 d=[dx, dy, dz]T\mathbf{d} = [\mathrm{d}x,\ \mathrm{d}y,\ \mathrm{d}z]^Td=[dx, dy, dz]T，微小旋转向量（用角轴表示，按小角度近似）为 δ=[δx, δy, δz]T\boldsymbol{\delta} = [\delta_x,\ \delta_y,\ \delta_z]^Tδ=[δx, δy, δz]T（单位为弧度）。则该微分运动对应的微分算子 F\mathbf{F}F 定义为：
F=[0−δzδydxδz0−δxdy−δyδx0dz0000] \mathbf{F} = \begin{bmatrix} 0 & -\delta_z & \delta_y & \mathrm{d}x \\ \delta_z & 0 & -\delta_x & \mathrm{d}y \\ -\delta_y & \delta_x & 0 & \mathrm{d}z \\ 0 & 0 & 0 & 0 \end{bmatrix} F= 0δz−δy0−δz0δx0δy−δx00dxdydz0
- 左上角 3×33 \times 33×3 为旋转的反对称矩阵 [δ]×[\boldsymbol{\delta}]_\times[δ]×，代表微小旋转。
- 右上角 3×13 \times 13×1 为微小平移 d\mathbf{d}d。
- 最后一行全零，保持齐次形式。
物理意义 ：对于任意从参考系到目标系的变换矩阵 C\mathbf{C}C，在参考系下施加微分运动后，变换矩阵的变化量为：
dC=F⋅C \mathrm{d}\mathbf{C} = \mathbf{F} \cdot \mathbf{C} dC=F⋅C
这是因为微分运动相当于在变换矩阵左侧左乘一个微小变换 I+F\mathbf{I} + \mathbf{F}I+F（忽略高阶无穷小）：
Cnew=(I+F)C⇒dC=FC \mathbf{C}_{\text{new}} = (\mathbf{I} + \mathbf{F})\mathbf{C} \quad \Rightarrow \quad \mathrm{d}\mathbf{C} = \mathbf{F}\mathbf{C} Cnew=(I+F)C⇒dC=FC
3. 相对当前坐标系的等效微分运动
有时需要在工具坐标系（即 C\mathbf{C}C 所描述的当前坐标系）下表达微分运动。设相对当前坐标系的等效微分平移为 d′=[dx′, dy′, dz′]T\mathbf{d}' = [\mathrm{d}x',\ \mathrm{d}y',\ \mathrm{d}z']^Td′=[dx′, dy′, dz′]T，等效微分旋转为 δ′=[δx′, δy′, δz′]T\boldsymbol{\delta}' = [\delta_x',\ \delta_y',\ \delta_z']^Tδ′=[δx′, δy′, δz′]T，则对应的微分算子记作 D\mathbf{D}D，形式与 F\mathbf{F}F 相同：
D=[0−δz′δy′dx′δz′0−δx′dy′−δy′δx′0dz′0000] \mathbf{D} = \begin{bmatrix} 0 & -\delta_z' & \delta_y' & \mathrm{d}x' \\ \delta_z' & 0 & -\delta_x' & \mathrm{d}y' \\ -\delta_y' & \delta_x' & 0 & \mathrm{d}z' \\ 0 & 0 & 0 & 0 \end{bmatrix} D= 0δz′−δy′0−δz′0δx′0δy′−δx′00dx′dy′dz′0
该微分运动在参考坐标系下的效果为：
dC=C⋅D \mathrm{d}\mathbf{C} = \mathbf{C} \cdot \mathbf{D} dC=C⋅D
即先在当前坐标系下施加微小运动（右乘算子），再变换到参考坐标系。这与参考系下的左乘算子 F\mathbf{F}F 得到相同的变化 dC\mathrm{d}\mathbf{C}dC。
由 FC=CD\mathbf{F}\mathbf{C} = \mathbf{C}\mathbf{D}FC=CD 可解得：
D=C−1FC \mathbf{D} = \mathbf{C}^{-1} \mathbf{F} \mathbf{C} D=C−1FC
这是一个相似变换 ，它将固定参考系下的微分算子 F\mathbf{F}F 映射到当前坐标系下的微分算子 D\mathbf{D}D。该变换保持了微分运动的物理本质，但旋转和平移分量在不同坐标系下的坐标表达不同。
4. 从 D\mathbf{D}D 提取等效微分平移与旋转
由于 D\mathbf{D}D 具有与 F\mathbf{F}F 相同的结构，其元素直接给出了相对当前坐标系的微分运动参数：
-
等效微分平移：
d′=[dx′dy′dz′]=[D14D24D34] \mathbf{d}' = \begin{bmatrix} \mathrm{d}x' \\ \mathrm{d}y' \\ \mathrm{d}z' \end{bmatrix} = \begin{bmatrix} D_{14} \\ D_{24} \\ D_{34} \end{bmatrix} d′= dx′dy′dz′ = D14D24D34 -
等效微分旋转：
δ′=[δx′δy′δz′]=[D32D13D21] \boldsymbol{\delta}' = \begin{bmatrix} \delta_x' \\ \delta_y' \\ \delta_z' \end{bmatrix} = \begin{bmatrix} D_{32} \\ D_{13} \\ D_{21} \end{bmatrix} δ′= δx′δy′δz′ = D32D13D21
这是因为反对称部分满足：
D32=δx′,D13=δy′,D21=δz′. D_{32} = \delta_x', \quad D_{13} = \delta_y', \quad D_{21} = \delta_z'. D32=δx′,D13=δy′,D21=δz′.
5. 小角度近似的适用性
上述推导基于一阶小角度近似 ，即 sinθ≈θ\sin\theta \approx \thetasinθ≈θ，cosθ≈1\cos\theta \approx 1cosθ≈1，且旋转矩阵的微分变化为 dR=[δ]×R\mathrm{d}\mathbf{R} = [\boldsymbol{\delta}]_\times \mathbf{R}dR=[δ]×R。实际工程中要求 ∥δ∥≪1\|\boldsymbol{\delta}\| \ll 1∥δ∥≪1 rad。若角度较大，需使用精确的指数映射，但微分运动学通常只考虑瞬时运动。
6. 验证关系
根据推导应有 CD=FC=dC\mathbf{C} \mathbf{D} = \mathbf{F} \mathbf{C} = \mathrm{d}\mathbf{C}CD=FC=dC。脚本中通过数值比较验证这一恒等式，确认了理论的一致性。
7. 与雅可比矩阵的联系
在机器人运动学中，参考系下的微分运动与关节速度的关系由雅可比矩阵 J\mathbf{J}J 描述：[dT, δT]T=Jq˙[\mathbf{d}^T,\ \boldsymbol{\delta}^T]^T = \mathbf{J} \dot{\mathbf{q}}[dT, δT]T=Jq˙。而相对当前坐标系的等效微分运动则对应于工具坐标系下的雅可比矩阵 Jtool=AdC(J)\mathbf{J}{\text{tool}} = \mathrm{Ad}{\mathbf{C}}(\mathbf{J})Jtool=AdC(J)，其中 AdC\mathrm{Ad}_{\mathbf{C}}AdC 为伴随变换。实际上 D\mathbf{D}D 的向量形式 [d′; δ′][\mathbf{d}';\ \boldsymbol{\delta}'][d′; δ′] 正是通过伴随矩阵将参考系下的速度旋量变换到工具系：
d′δ′\]=\[RT−RT\[p\]×0RT\]\[dδ\] \\begin{bmatrix} \\mathbf{d}' \\\\ \\boldsymbol{\\delta}' \\end{bmatrix} = \\begin{bmatrix} \\mathbf{R}\^T \& -\\mathbf{R}\^T \[\\mathbf{p}\]_\\times \\\\ \\mathbf{0} \& \\mathbf{R}\^T \\end{bmatrix} \\begin{bmatrix} \\mathbf{d} \\\\ \\boldsymbol{\\delta} \\end{bmatrix} \[d′δ′\]=\[RT0−RT\[p\]×RT\]\[dδ
其中 C=[Rp01]\mathbf{C} = \begin{bmatrix} \mathbf{R} & \mathbf{p} \\ \mathbf{0} & 1 \end{bmatrix}C=[R0p1]。这正是脚本中 D=C−1FC\mathbf{D} = \mathbf{C}^{-1} \mathbf{F} \mathbf{C}D=C−1FC 计算的等价结果。
8. 总结
本脚本实现了微分运动在不同坐标系间的精确线性变换，其核心依据为：
- 微分算子 F\mathbf{F}F 定义了参考坐标系下的微小运动。
- 相似变换 D=C−1FC\mathbf{D} = \mathbf{C}^{-1} \mathbf{F} \mathbf{C}D=C−1FC 将微分算子映射到当前坐标系。
- 从 D\mathbf{D}D 的特定位置可读取出等效的微分平移和旋转分量。
- 该变换在机器人运动学、误差分析和速度变换中具有广泛用途。
以上理论保证了脚本结果的正确性与可解释性。
MATLAB完整代码示范
以下代码实现了上述理论，并验证了 CD=FC\mathbf{C}\mathbf{D} = \mathbf{F}\mathbf{C}CD=FC。
matlab
%% 微分变换：参考坐标系微分运动 -> 相对当前坐标系的等效微分运动
% 已知：原始齐次变换矩阵 C (4x4)
% 参考系下微分平移向量 d = [dx; dy; dz] (默认全部为 1)
% 参考系下微分旋转向量 δ = [δx; δy; δz] (弧度，默认全部为 1 rad)
% 求： 微分变换 dC = F * C
% 相对坐标系 C 的等效微分运动 dC' = C * D，并提取等效微分平移和旋转。
clear; clc;
%% ==================== 1. 定义原始矩阵 C（可任意修改） ====================
% 示例：一个旋转齐次矩阵（绕 Z 轴 -90°？），平移 [10,5,1]
C = [0, 1, 0, 10;
-1, 0, 0, 5;
0, 0, 1, 1;
0, 0, 0, 1];
% 如果需要符号运算（如保留 π 等），请使用 sym(C)，例如：
% C = sym([0, 1, 0, 10; -1, 0, 0, 5; 0, 0, 1, 1; 0, 0, 0, 1]);
fprintf('原始齐次变换矩阵 C：\n');
disp(C); % 直接显示数值矩阵，避免 pretty 报错
%% ==================== 2. 定义微分运动参数（默认全部为 1，可随时修改） ====================
% 微分平移向量（参考坐标系下）
dx = 0.5; % 可修改为任意数值
dy = 0.2; % 可修改
dz = -0.1; % 可修改
% 微分旋转向量（弧度，参考坐标系下）
dtheta_x = 0.1; % 单位：rad，可修改
dtheta_y = -0.2; % 单位：rad，可修改
dtheta_z = 0.1; % 单位：rad，可修改
fprintf('\n微分运动参数（参考坐标系）：\n');
fprintf('平移: dx = %g, dy = %g, dz = %g\n', dx, dy, dz);
fprintf('旋转: δx = %g rad, δy = %g rad, δz = %g rad\n', dtheta_x, dtheta_y, dtheta_z);
%% ==================== 3. 构造参考坐标系下的微分算子 F ====================
% F = [ 0, -δz, δy, dx;
% δz, 0, -δx, dy;
% -δy, δx, 0, dz;
% 0, 0, 0, 0 ]
F = [ 0, -dtheta_z, dtheta_y, dx;
dtheta_z, 0, -dtheta_x, dy;
-dtheta_y, dtheta_x, 0, dz;
0, 0, 0, 0 ];
fprintf('\n参考坐标系微分算子 F：\n');
disp(F);
%% ==================== 4. 计算微分变换 dC = F * C ====================
dC = F * C; % 注意：若 C 是数值矩阵，这里无需 simplify
fprintf('\n微分变换 dC = F * C：\n');
disp(dC);
%% ==================== 5. 计算相对坐标系 C 的等效微分算子 D ====================
% D = inv(C) * F * C
C_inv = inv(C);
D = C_inv * F * C;
fprintf('\n相对坐标系 C 的等效微分算子 D = inv(C) * F * C：\n');
disp(D);
%% ==================== 6. 从 D 中提取等效微分平移和微分旋转 ====================
% D 的形式：
% D = [ 0, -δz', δy', dx';
% δz', 0, -δx', dy';
% -δy', δx', 0, dz';
% 0, 0, 0, 0 ]
dx_prime = D(1,4);
dy_prime = D(2,4);
dz_prime = D(3,4);
dtheta_x_prime = D(3,2); % δx' = D(3,2)
dtheta_y_prime = D(1,3); % δy' = D(1,3)
dtheta_z_prime = D(2,1); % δz' = D(2,1)
fprintf('\n====== 相对坐标系 C 的等效微分运动 ======\n');
fprintf('等效微分平移: dx'' = %g, dy'' = %g, dz'' = %g\n', dx_prime, dy_prime, dz_prime);
fprintf('等效微分旋转: δx'' = %g rad, δy'' = %g rad, δz'' = %g rad\n', ...
dtheta_x_prime, dtheta_y_prime, dtheta_z_prime);
%% ==================== 7. 验证: C * D 是否等于 dC ====================
C_times_D = C * D;
if norm(C_times_D - dC) < 1e-10
fprintf('\n验证成功: C * D == dC\n');
else
fprintf('\n注意: 存在数值误差，但理论上应相等。\n');
end
%% ==================== 8. 使用说明 ====================
fprintf('\n【使用说明】\n');
fprintf('1. 如需修改微分运动参数，直接更改第 2 节中的 dx, dy, dz 等变量值即可。\n');
fprintf('2. 原始矩阵 C 也可按需修改（第 1 节）。\n');
fprintf('3. 若需要符号计算（如保留 π 或变量），请在第 1 节将 C 定义为 sym(C)。\n');
fprintf('4. 当前演示中旋转角度为 1 rad（较大），实际工程应用通常要求小角度。\n');