clc;
clear;
close all;

% rng(0);
% rand(1)*2*pi

%% W: world coordinate system
% 尺寸4X4，齐次矩阵，齐次坐标系
W = eye(4);
W(1:3,1:3)=W(1:3,1:3)*200; % 乘以200是为了使得坐标轴变长，便于作图时候显示 


%% 变换（零）
% 动态参考坐标系DRF，初始状态和世界坐标系完全重合
%（1）绕W的X轴旋转，（2）绕W的Y轴旋转，（3）绕W的Z轴旋转，
%（4）相对于W的坐标原点，沿着W的XYZ方向平移
roll_WtoDRF00 = pi/2;
pitch_WtoDRF00 = 0;
yaw_WtoDRF00 = 0;
EA_zyx_WtoDRF00 = [yaw_WtoDRF00 pitch_WtoDRF00 roll_WtoDRF00];
Tl_WtoDRF00 = [200,200,-1000]';
HTfM_WtoDRF00= genHTfM_useEularAngle(EA_zyx_WtoDRF00,Tl_WtoDRF00);
DRF00 = HTfM_WtoDRF00*W;


%% 变换（一）
% 动态参考坐标系DRF00，由变换（一）得到
% (0) DRF00平移回到和W完全重合。只有平移，不旋转
%（1）绕W的X轴旋转，（2）绕W的Y轴旋转，（3）绕W的Z轴旋转，
% (4) 把DRF00平移到，变换（一）结束时候的位置，得到DRF01。只有平移，不旋转
% 效果等价于，DRF00固定自己的坐标原点，沿着W的XYZ轴旋转
roll_DRF00toDRF01 = pi/2;
pitch_DRF00toDRF01 = 0;
yaw_DRF00toDRF01 = 0;
EA_zyx_DRF00toDRF01 = [yaw_DRF00toDRF01 pitch_DRF00toDRF01 roll_DRF00toDRF01];
R_DRF00toDRF01 = eul2rotm(EA_zyx_DRF00toDRF01,'ZYX');
HTfM_WtoDRF01 = HTfM_WtoDRF00;
HTfM_WtoDRF01(1:3,1:3) = R_DRF00toDRF01*HTfM_WtoDRF01(1:3,1:3);
DRF01 = HTfM_WtoDRF01*W;

% 画图
axis_limit = 1000;
axis_limitA = [-axis_limit,axis_limit,-axis_limit,axis_limit,-1500,300];

figure(1)
trplot(W,'frame',  'RW','color','r',  'axis',axis_limitA,  'text_opts',{'FontSize', 10, 'FontWeight', 'light'},  'view','auto',  'thick',1,  'dispar',0.8);
hold on;
plot3(W(1,4),W(2,4),W(3,4),'ro')
hold on;
trplot(DRF00,'frame',  'R00','color','g',  'axis',axis_limitA,  'text_opts',{'FontSize', 10, 'FontWeight', 'light'},  'view','auto',  'thick',1,  'dispar',0.8);
hold on;
plot3(DRF00(1,4),DRF00(2,4),DRF00(3,4),'go')
hold on;
trplot(DRF01,'frame',  'R01','color','b',  'axis',axis_limitA,  'text_opts',{'FontSize', 10, 'FontWeight', 'light'},  'view','auto',  'thick',1,  'dispar',0.8);
hold on;
plot3(DRF01(1,4),DRF01(2,4),DRF01(3,4),'bo')


%% 变换（二）
% 动态参考坐标系DRF00，由变换（一）得到。对DRF00做一下操作，得到DRF02
%（1）绕W的X轴旋转，（2）绕W的Y轴旋转，（3）绕W的Z轴旋转
% DRF00在旋转的过程中，其坐标原点在世界坐标系下的位置也会随之旋转。
roll_DRF00toDRF02 = pi/10;
pitch_DRF00toDRF02 = 0;
yaw_DRF00toDRF02 = 0;
EA_zyx_DRF00toDRF02 = [yaw_DRF00toDRF02 pitch_DRF00toDRF02 roll_DRF00toDRF02];
Tl_DRF00toDRF02 = [0,0,0]';
HTfM_DRF00toDRF02= genHTfM_useEularAngle(EA_zyx_DRF00toDRF02,Tl_DRF00toDRF02);
HTfM_WtoDRF02 = HTfM_DRF00toDRF02*HTfM_WtoDRF00;
DRF02 = HTfM_WtoDRF02*W;

figure(2)
trplot(W,'frame',  'RW','color','r',  'axis',axis_limitA,  'text_opts',{'FontSize', 10, 'FontWeight', 'light'},  'view','auto',  'thick',1,  'dispar',0.8);
hold on;
plot3(W(1,4),W(2,4),W(3,4),'ro')

hold on;
trplot(DRF00,'frame',  'R00','color','g',  'axis',axis_limitA,  'text_opts',{'FontSize', 10, 'FontWeight', 'light'},  'view','auto',  'thick',1,  'dispar',0.8);
hold on;
plot3(DRF00(1,4),DRF00(2,4),DRF00(3,4),'go')

hold on;
trplot(DRF02,'frame',  'R02','color','m',  'axis',axis_limitA,  'text_opts',{'FontSize', 10, 'FontWeight', 'light'},  'view','auto',  'thick',1,  'dispar',0.8);
hold on;
plot3(DRF02(1,4),DRF02(2,4),DRF02(3,4),'mo')

%% 变换（三）
% 动态参考坐标系DRF00，由变换（一）得到。对DRF00做一下操作，得到DRF03
%（1）DRF00相对于W的坐标原点，在原有平移的基础上，沿着W的XYZ方向平移，得到DRF03
roll_DRF00toDRF03 = 0;
pitch_DRF00toDRF03 = 0;
yaw_DRF00toDRF03 = 0;
EA_zyx_DRF00toDRF03 = [yaw_DRF00toDRF03 pitch_DRF00toDRF03 roll_DRF00toDRF03];
Tl_DRF00toDRF03 = [0,240,0]';
HTfM_DRF00toDRF03= genHTfM_useEularAngle(EA_zyx_DRF00toDRF03,Tl_DRF00toDRF03);
HTfM_WtoDRF03 = HTfM_DRF00toDRF03*HTfM_WtoDRF00;
DRF03 = HTfM_WtoDRF03*W;

figure(3)
trplot(W,'frame',  'RW','color','r',  'axis',axis_limitA,  'text_opts',{'FontSize', 10, 'FontWeight', 'light'},  'view','auto',  'thick',1,  'dispar',0.8);
hold on;
plot3(W(1,4),W(2,4),W(3,4),'ro')

hold on;
trplot(DRF00,'frame',  'R00','color','g',  'axis',axis_limitA,  'text_opts',{'FontSize', 10, 'FontWeight', 'light'},  'view','auto',  'thick',1,  'dispar',0.8);
hold on;
plot3(DRF00(1,4),DRF00(2,4),DRF00(3,4),'go')

hold on;
trplot(DRF03, 'frame', 'R03', 'color', 'k', 'axis',axis_limitA,  'text_opts',{'FontSize', 10, 'FontWeight', 'light'},  'view','auto',  'thick',1,  'dispar',0.8);
hold on;
plot3(DRF03(1,4),DRF03(2,4),DRF03(3,4),'ko')

function HTfM= genHTfM_useRotM(rotationMatrix,translation)
% HTfM homogeneous transform matrix

HTfM = eye(4,4);
HTfM(1:3,1:3) = rotationMatrix;
HTfM(1:3,4) = translation;

end

function HTfM= genHTfM_useEularAngle(eularAngle_zyx,translation)
% HTfM homogeneous transform matrix

rotationMatrix=eul2rotm(eularAngle_zyx,'ZYX');
HTfM = eye(4,4);
HTfM(1:3,1:3) = rotationMatrix;
HTfM(1:3,4) = translation;

end

​​​​​​​ 

 

 

1. 引言

  上一篇文章我们讲到了齐次变换矩阵，表面上看我们得到了一个十分简洁的表达式。如果你认为到这里关于齐次变换的问题已经十分明了那就大错特错了。相反，我认为这恰恰是混乱的开始。

  举个例子，首先一个坐标系{A}绕其x轴旋转 $\alpha$ 得到坐标系{B}，根据前面讲过的知识你很容易明白坐标系{A}和{B}之间的关系为 $T_B^A=ro{t}_x\left(\alpha \right)$。可是如果坐标系{B}再绕自身的y轴旋转 $\beta$ 得到坐标系{C}，那么坐标系{A}和坐标系{C}之间是什么关系呢？还有如果坐标系{B}绕坐标系{A}的y轴旋转 $\beta$ 得到坐标系{D}，那么坐标系{A}和坐标系{D}之间是什么关系呢？

  你可能没有答案。课堂上这个问题也总是被匆匆略过，而我认为这里才是应该花大量的精力去讲解的地方。只告诉我当绕原坐标系旋转时变换矩阵要倒着乘，当绕新坐标系旋转时变换矩阵要正着乘我是无法接受的。

  例子中的问题其实涉及到了齐次变换矩阵的几种解读，从这几种解读方式出发，我们很容易理解矩阵相乘的顺序到底是怎样的以及为什么是这样的。

2. 齐次变换矩阵的三种解读

  斯坦福大学公开课—机器人学，这门课对这三种解读给出了很恰当的总结：坐标系表示（coordinate representation）、坐标系变换（coordinate transformation）、点的操作算子（point operator）。接下来我们分别对它们进行解释。

2.1 坐标系表示

  所谓坐标系表示指的是，齐次变换矩阵可以用来表示一个坐标系。我们来看如下图两个坐标系{A}和{B}。通过上一篇文章机器人正运动学（5）—— 齐次变换矩阵的介绍我们知道坐标系{A}和坐标系{B}之间的关系为：

$${}^A{\text{T}}_B=\left[\begin{array}{cc}{}^A{\text{R}}_B& p_{Borg}^A\\ 0& 1\end{array}\right]$$

  在文章机器人正运动学（4）—— 理解变换矩阵中我们介绍了旋转矩阵 ${}^A{\text{R}}_B$（{A}和{B}的姿态关系）代表的是坐标系{B}三个轴的单位矢量在坐标系{A}中的投影（即在坐标系{A}中坐标系{B}的三轴单位矢量的坐标），在文章机器人正运动学（5）—— 齐次变换矩阵中我们也看到 $p_{Borg}^A$（{A}和{B}的位置关系）代表的是坐标系{B}的原点在坐标系{A}中的坐标。

  我们知道，要想唯一确定一个坐标系需要两个要素，一个是原点，一个是轴矢量。这两个要素在上面的齐次变换矩阵中已经全部包含了，因此我们说齐次变换矩阵可以用来表示一个坐标系，而 ${}^A{\text{T}}_B$ 就是坐标系{B}在坐标系{A}中的表示。

2.2 坐标系变换

  所谓坐标系变换指的是，齐次变换矩阵可以用来描述一个坐标系经过怎样的平移和旋转能够变换到另一个坐标系。举个例子，两个坐标系{A}和{B}之间的齐次变换矩阵可以表示为：$${}^A{\text{T}}_B=\left[\begin{array}{cc}ro{t}_x(\pi /4)& trans(1,2,3)\\ 0& 1\end{array}\right]$$

  其中 $ro{t}_x(\pi /4)$ 代表绕x轴旋转 $\pi /4$ 对应的旋转矩阵，$trans(1,2,3)$代表在$x,y,z$方向上平移1,2,3。这个时候我们就可以通过对坐标系{A}进行一些平移和旋转最终与坐标系{B}重合。这个操作过程对应于下面这张图。

  我们把这个操作过程分为两个步骤。

  第一步将坐标系{A}沿着橘色的向量平移至与坐标系{C}重合，我们很容易知道这个平移量是trans(1,2,3)；

  第二步将坐标系{C}绕它的x轴旋转至与坐标系{B}重合，同样容易知道这个旋转量是 $ro{t}_x(\pi /4)$。因此我们说齐次变换矩阵可以用来描述坐标系的变换关系，而 ${}^A{\text{T}}_B$ 就是坐标系{A}经过平移和旋转可以得到坐标系{B}。我们可以形象地描述（不一定恰当）这个变换为： { B } = { A } ⋆ A T B \{B\}=\{A\} \star ^{A}\textrm{T}_{B} {B}={A}⋆ATB​

2.3 点的操作算子

  所谓点的操作算子指的是，齐次变换矩阵可以用来对同一个坐标系内的点进行平移和旋转操作。注意这种理解和前面两种大不一样。前两种都是在描述两个坐标系之间的关系。在讨论齐次变换矩阵对点的操作时我们的讨论仅限于一个坐标系。

  举个例子在坐标系{A}中有一个点 $P_0$，我们希望这个点绕坐标系{A}的x轴旋转 $\pi /4$ 得到一个新的点 $P_1$。这个时候应该怎么做呢？很简单，我们写出如下的变换矩阵：$$R=ro{t}_x(\pi /4)=\left[\begin{array}{ccc}1& 0& 0\\ 0& cos(\pi /4)& -sin(\pi /4)\\ 0& sin(\pi /4)& cos(\pi /4)\end{array}\right]$$

  用这个变换矩阵去乘点 $P_0$ 就可以得到点 $P_1$ 啦。

  如果你觉得理解这个有点困难，可以倒回到第一种理解来解释对点的操作问题。这个时候我们假设有一个坐标系{B}开始时与坐标系{A}完全重合，而点 $P_0$ 又固定在坐标系{B}中，这样在把点 $P_0$ 绕坐标系{A}的x轴旋转 $\pi /4$ 时，坐标系{B}同样是绕坐标系{A}的x轴旋转 $\pi /4$。此时两个坐标系之间的变换矩阵即为 $ro{t}_x(\pi /4)$。由于点 $P_0$ 固定在坐标系{B}中，因此其在坐标系{B}中的坐标不变，因此经过旋转之后点 $P_0$ 在坐标系{A}中的坐标即为 $ro{t}_x(\pi /4)\cdot P_0$。我们可以看出这个点就是前面提到的在坐标系{A}中的 $P_1$ 点。

  光用文字来描述似乎有些凌乱，下面我们用图把上面一段话再重新捋一遍，首先我们假设空间中一点 $P_0$ 在坐标系{A}中的坐标为 $P_0(y_0,z_0)$，为了便于看图，坐标系我们只画出 $y，z$ 两个轴，所有点的x坐标均为0。与点 $P_0$ 固连的坐标系称为{B}。坐标系{B}最开始与坐标系{A}重合，如下图所示就是问题的初始状态。

  接下来开始旋转操作。我们是希望点 $P_0$ 沿坐标系{A}的x轴旋转 $\pi /4$，注意因为坐标系{B}和点 $P_0$ 是固连在一起的，所以坐标系{B}也会绕着坐标系{A}的x轴旋转 $\pi /4$，经过旋转之后就得到如下的图的状态。点 $P_1$ 就是点 $P_0$ 旋转之后对应的点。这里有一点需要特别注意，由于坐标系{B}与点 $P_0$ 固连，那么经过旋转后得到的 $P_1$ 点在坐标系{B}中的坐标数值实际上没有发生任何变化（和最开始的时候点 $P_0$ 在坐标系{A}中的坐标值是一样的），请仔细体会这句话。

  下面我们把变换前后的两种状态合并起来得到下面的图。

  至此我们可以得出这样一个结论：点 $P_0$ 在坐标系{A}中的坐标值与点 $P_1$ 在{B}中的坐标值是一致的。那么如何求解点 $P_1$ 在坐标系{A}中的坐标值呢？仔细思考一下是不是这样一种表达：$$P_1^A=R_B^A\cdot P_1^B$$

  解释一下这个公式，$P_1^A$ 代表点 $P_1$ 在坐标系{A}下的坐标。$P_1^B$ 代表 $P_1$ 在坐标系{B}下的坐标。还记得我们之前说的吗，$P_1$ 在坐标系{B}下的坐标数值上与 $P_0$ 在坐标系{A}中的坐标数值一致！以上公式可以改写为$$P_1^A=R_B^A\cdot P_0^A$$

  至此我们看到 $R_B^A$ 实现了将点 $P_0$ 旋转到 $P_1$ 点。你也可以清晰的看到上一个公式点的上标都是A，和我们之前说的同一个空间点在不同坐标系下的表达还是有区别的。

3. 解决问题

3.1 齐次变换矩阵的逆

  上一篇文章机器人正运动学（5）—— 齐次变换矩阵中我们遗留了一个问题就是齐次变换矩阵的逆。通常而言求任意矩阵的逆是比较困难的。但是由于齐次变换矩阵的良好性质以及很强的几何特征，我们很容易直接写出它的逆。

  已知如下的齐次变换矩阵：$${}^A{\text{T}}_B=\left[\begin{array}{cc}{}^A{\text{R}}_B& p_{Borg}^A\\ 0& 1\end{array}\right]$$

  首先看旋转部分，在文章机器人正运动学（4）—— 理解变换矩阵中我们提到过 ${}^A{\text{R}}_B$ 的列代表坐标系{B}的三轴矢量在坐标系{A}中的投影，行代表坐标系{A}的三轴矢量在坐标系{B}中的投影，因此，${}^A{\text{R}}_B$ 的转置就是其逆。

  其次来看平移部分，$p_{Borg}^A$ 代表坐标系{B}的原点在坐标系{A}中的坐标，而我们需要的是坐标系{A}的原点在坐标系{B}中的坐标，即 $p_{Aorg}^B$，从几何的角度看，这两个向量恰好是相反向量，因此我们首先把向量 $p_{Borg}^A$ 取反得到 $-p_{Borg}^A$。到这一步还没有完，因为目前为止这个向量是在坐标系{A}中描述的，因此我们要将它变换到坐标系{B}中的描述，即为 ${-}^A{\text{R}}_B^T\cdot p_{Borg}^A$，其中 ${}^A{\text{R}}_B^T$ 代表 ${}^A{\text{R}}_B$ 的转置（其实也就是 ${}^A{\text{R}}_B$ 的逆）。

  至此我们得到了坐标系{A}相对于坐标系{B}的姿态${}^A{\text{R}}_B^T$，以及坐标系{B}中描述的坐标系{A}原点坐标 $p_{Aorg}^B={-}^A{\text{R}}_B^T\cdot p_{Borg}^A$。因此变换矩阵的逆即为：$${}^A{\text{T}}_B^{-1}=\left[\begin{array}{cc}{}^A{\text{R}}_B^T& {-}^A{\text{R}}_B^T\cdot p_{Borg}^A\\ 0& 1\end{array}\right]{=}^B{\text{T}}_A=\left[\begin{array}{cc}{}^B{\text{R}}_A& {-}^B{\text{R}}_A\cdot p_{Borg}^A\\ 0& 1\end{array}\right]$$

3.2 多重变换时的顺序

  在此我们解决文章开头提出的多重变换时如何排列变换矩阵。其实就是两种情况，第一种情况是所有的变换都相对于最开始的坐标系；第二种情况是所有变换都相对于新得到的坐标系。我们分情况讨论。

  首先来看第一种情况，坐标系{A}绕其x轴旋转 $\alpha$ 得到坐标系{B}，之后坐标系{B}又绕坐标系{A}的y轴旋转 $\beta$ 得到坐标系{C}。此时我们用对齐次变换矩阵的第三种理解来求得坐标系{C}和坐标系{A}之间的关系。我们假设在坐标系{A}中有一个点 $P_0$，在第一次变换时我们假设有一个坐标系{T1}与坐标系{A}重合，同时点 $P_0$ 固定在坐标系{T1}中，经过第一次变换后得到在{A}中新的点 $P_1$ 的坐标为 $ro{t}_x(\alpha )\cdot P_0$（注意这是 $P_1$ 点在坐标系{A}中的坐标）。在第二次变换时我们假设又有一个坐标系{T2}与坐标系{A}重合，同时点 $P_1$ 又固定在坐标系{T2}中，经过第二次变换后得到在{A}中新的点 $P_2$ 的坐标为 $ro{t}_y(\beta )\cdot P_1=ro{t}_y(\beta )\cdot ro{t}_x(\alpha )\cdot P_0$，因此我们可以最终确定坐标系{A}和{C}之间的变换关系为：$${}^A{\text{R}}_C=ro{t}_y(\beta )\cdot ro{t}_x(\alpha )$$

  再来看第二种情况，坐标系{A}绕其x轴旋转 $\alpha$ 得到坐标系{B}，之后坐标系{B}又绕其y轴旋转 $\beta$ 得到坐标系{C}（注意这里是绕坐标系{B}的y轴）。此时我们用齐次变换矩阵的第二种理解来求坐标系{C}和坐标系{A}之间的关系。设空间中有一个点 $P$，它在坐标系{C}中的坐标为 $P_C$，我们首先看坐标系{B}和坐标系{C}之间的关系，由于{B}绕自身的y轴旋转 $\beta$ 就可以得到{C}，因此，同一个空间点 $P$ 在坐标系{B}中的坐标为 $P_B=ro{t}_y(\beta )\cdot P_C$。我们又知道坐标系{A}绕其x轴旋转 $\alpha$ 得到坐标系{B}，因此，同一个空间点P在坐标系{A}中的坐标为 $P_A=ro{t}_x(\alpha )\cdot P_B=ro{t}_x(\alpha )\cdot ro{t}_y(\beta )\cdot P_C$，因此我们可以最终确定坐标系{A}和{C}之间的变换关系为：$${}^A{\text{R}}_C=ro{t}_x(\alpha )\cdot ro{t}_y(\beta )$$

  这也验证了文章开头提出的结论，那就是绕着原坐标系旋转就倒着乘，绕着新坐标系旋转就正着乘。

4. 总结

  这篇文章主要介绍了齐次变换矩阵的三种理解以及解决了一个比较容易造成混乱的多重变换时变换矩阵的顺序问题。由于个人能力有限，所述内容难免存在疏漏，欢迎指出，欢迎讨论。

下一篇：机器人正运动学（7）—— 连杆坐标系与DH参数

目录

 

1. 引言

2. 齐次变换矩阵的三种解读

2.1 坐标系表示

2.2 坐标系变换

2.3 点的操作算子

3. 解决问题

3.1 齐次变换矩阵的逆

3.2 多重变换时的顺序

4. 总结

---

1. 引言

上一篇文章我们讲到了齐次变换矩阵，表面上看我们得到了一个十分简洁的表达式。如果你认为到这里关于齐次变换的问题已经十分明了那就大错特错了。相反，我认为这恰恰是混乱的开始。

举个例子，首先一个坐标系{A}绕其轴旋转得到坐标系{B}，根据前面讲过的知识你很容易明白坐标系{A}和{B}之间的关系为。可是如果坐标系{B}再绕自身的轴旋转得到坐标系{C}，那么坐标系{A}和坐标系{C}之间是什么关系呢？还有如果坐标系{B}绕坐标系{A}的轴旋转得到坐标系{D}，那么坐标系{A}和坐标系{D}之间是什么关系呢？

你可能没有答案。课堂上这个问题也总是被匆匆略过，而我认为这里才是应该花大量的精力去讲解的地方。只告诉我当绕原坐标系旋转时变换矩阵要倒着乘，当绕新坐标系旋转时变换矩阵要正着乘我是无法接受的。

例子中的问题其实涉及到了齐次变换矩阵的几种解读，从这几种解读方式出发，我们很容易理解矩阵相乘的顺序到底是怎样的以及为什么是这样的。

2. 齐次变换矩阵的三种解读

斯坦福大学公开课---机器人学，这门课对这三种解读给出了很恰当的总结：坐标系表示（coordinate representation）、坐标系变换（coordinate transformation）、点的操作算子（point operator）。接下来我们分别对它们进行解释。

2.1 坐标系表示

所谓坐标系表示指的是，齐次变换矩阵可以用来表示一个坐标系。我们来看如下图两个坐标系{A}和{B}。通过上一篇文章机器人正运动学---齐次变换矩阵的介绍我们知道坐标系{A}和坐标系{B}之间的关系为：

                                                                          

在文章机器人正运动学---理解变换矩阵中我们介绍了旋转矩阵（{A}和{B}的姿态关系）代表的是坐标系{B}三个轴的单位矢量在坐标系{A}中的投影（即在坐标系{A}中坐标系{B}的三轴单位矢量的坐标），在文章机器人正运动学---齐次变换矩阵中我们也看到（{A}和{B}的位置关系）代表的是坐标系{B}的原点在坐标系{A}中的坐标。

我们知道，要想唯一确定一个坐标系需要两个要素，一个是原点，一个是轴矢量。这两个要素在上面的齐次变换矩阵中已经全部包含了，因此我们说齐次变换矩阵可以用来表示一个坐标系，而就是坐标系{B}在坐标系{A}中的表示

2.2 坐标系变换

所谓坐标系变换指的是，齐次变换矩阵可以用来描述一个坐标系经过怎样的平移和旋转能够变换到另一个坐标系。举个例子，两个坐标系{A}和{B}之间的齐次变换矩阵可以表示为：

                                                                

其中代表绕轴旋转对应的旋转矩阵，，代表在方向上平移1,2,3。这个时候我们就可以通过对坐标系{A}进行一些平移和旋转最终与坐标系{B}重合。这个操作过程对应于下面这张图。

我们把这个操作过程分为两个步骤。

第一步将坐标系{A}沿着橘色的向量平移至与坐标系{C}重合，我们很容易知道这个平移量是；

第二步将坐标系{C}绕它的轴旋转至与坐标系{B}重合，同样容易知道这个旋转量是。因此我们说齐次变换矩阵可以用来描述坐标系的变换关系，而就是坐标系{A}经过平移和旋转可以得到坐标系{B}。我们可以形象地描述（不一定恰当）这个变换为：

                                                                          

2.3 点的操作算子

所谓点的操作算子指的是，齐次变换矩阵可以用来对同一个坐标系内的点进行平移和旋转操作。注意这种理解和前面两种大不一样。前两种都是在描述两个坐标系之间的关系。在讨论齐次变换矩阵对点的操作时我们的讨论仅限于一个坐标系。

举个例子在坐标系{A}中有一个点，我们希望这个点绕坐标系{A}的轴旋转得到一个新的点。这个时候应该怎么做呢？很简单，我们写出如下的变换矩阵：

                                                        

用这个变换矩阵去乘点就可以得到点啦。

如果你觉得理解这个有点困难，可以倒回到第一种理解来解释对点的操作问题。这个时候我们假设有一个坐标系{B}开始时与坐标系{A}完全重合，而点又固定在坐标系{B}中，这样在把点绕坐标系{A}的轴旋转时，坐标系{B}同样是绕坐标系{A}的轴旋转。此时两个坐标系之间的变换矩阵即为。由于点固定在坐标系{B}中，因此其在坐标系{B}中的坐标不变，因此经过旋转之后点在坐标系{A}中的坐标即为。我们可以看出这个点就是前面提到的在坐标系{A}中的点。

光用文字来描述似乎有些凌乱，下面我们用图把上面一段话再重新捋一遍，首先我们假设空间中一点在坐标系{A}中的坐标为，为了便于看图，坐标系我们只画出y，z两个轴，所有点的x坐标均为0。与点固连的坐标系称为{B}。坐标系{B}最开始与坐标系{A}重合，如下图所示就是问题的初始状态。

接下来开始旋转操作。我们是希望点沿坐标系{A}的轴旋转，注意因为坐标系{B}和点是固连在一起的，所以坐标系{B}也会绕着坐标系{A}的轴旋转，经过旋转之后就得到如下的图的状态。点就是点旋转之后对应的点。这里有一点需要特别注意，由于坐标系{B}与点固连，那么经过旋转后得到的点在坐标系{B}中的坐标数值实际上没有发生任何变化（和最开始的时候点在坐标系{A}中的坐标值是一样的），请仔细体会这句话。

 下面我们把变换前后的两种状态合并起来得到下面的图

至此我们可以得出这样一个结论，点在坐标系{A}中的坐标值与点在{B}中的坐标值是一致的。那么如何求解点在坐标系{A}中的坐标值呢？仔细思考一下是不是这样一种表达：

                              

解释一下这个公式，代表点在坐标系{A}下的坐标。代表在坐标系{B}下的坐标。还记得我们之前说的吗，在坐标系{B}下的坐标数值上与在坐标系{A}中的坐标数值一致！以上公式可以改写为

                             

至此我们看到实现了将点旋转到点。你也可以清晰的看到上一个公式点的上标都是A，和我们之前说的同一个空间点在不同坐标系下的表达还是有区别的。

3. 解决问题

3.1 齐次变换矩阵的逆

上一篇文章机器人正运动学---齐次变换矩阵中我们遗留了一个问题就是齐次变换矩阵的逆。通常而言求任意矩阵的逆是比较困难的。但是由于齐次变换矩阵的良好性质以及很强的几何特征，我们很容易直接写出它的逆。

已知如下的齐次变换矩阵：

                                                                            

首先看旋转部分，在文章机器人正运动学---理解变换矩阵中我们提到过的列代表坐标系{B}的三轴矢量在坐标系{A}中的投影，行代表坐标系{A}的三轴矢量在坐标系{B}中的投影，因此，的转置就是其逆。

其次来看平移部分，代表坐标系{B}的原点在坐标系{A}中的坐标，而我们需要的是坐标系{A}的原点在坐标系{B}中的坐标，即，从几何的角度看，这两个向量恰好是相反向量，因此我们首先把向量取反得到。到这一步还没有完，因为目前为止这个向量是在坐标系{A}中描述的，因此我们要将它变换到坐标系{B}中的描述，即为，其中代表的转置。

至此我们得到了坐标系{A}相对于坐标系{B}的姿态，以及坐标系{B}中描述的坐标系{A}原点坐标。因此变换矩阵的逆即为：

                                     

3.2 多重变换时的顺序

在此我们解决文章开头提出的多重变换时如何排列变换矩阵。其实就是两种情况，第一种情况是所有的变换都相对于最开始的坐标系；第二种情况是所有变换都相对于新得到的坐标系。我们分情况讨论。

首先来看第一种情况，坐标系{A}绕其轴旋转得到坐标系{B}，之后坐标系{B}又绕坐标系{A}的轴旋转得到坐标系{C}。此时我们用对齐次变换矩阵的第三种理解来求得坐标系{C}和坐标系{A}之间的关系。我们假设在坐标系{A}中有一个点，在第一次变换时我们假设有一个坐标系{T1}与坐标系{A}重合，同时点固定在坐标系{T1}中，经过第一次变换后得到在{A}中新的点的坐标为（注意这是点在坐标系{A}中的坐标）。在第二次变换时我们假设又有一个坐标系{T2}与坐标系{A}重合，同时点又固定在坐标系{T2}中，经过第二次变换后得到在{A}中新的点的坐标为，因此我们可以最终确定坐标系{A}和{C}之间的变换关系为：

                                                                          

再来看第二种情况，坐标系{A}绕其轴旋转得到坐标系{B}，之后坐标系{B}又绕其轴旋转（注意这里是绕坐标系{B}的轴）。此时我们用齐次变换矩阵的第二种理解来求坐标系{C}和坐标系{A}之间的关系。设空间中有一个点，它在坐标系{C}中的坐标为，我们首先看坐标系{B}和坐标系{C}之间的关系，由于{B}绕自身的轴旋转就可以得到{C}，因此，同一个空间点在坐标系{B}中的坐标为。我们又知道坐标系{A}绕其轴旋转得到坐标系{B}，因此，同一个空间点在坐标系{A}中的坐标为，因此我们可以最终确定坐标系{A}和{C}之间的变换关系为：

                                                                          

这也验证了文章开头提出的结论，那就是绕着原坐标系旋转就倒着乘，绕着新坐标系旋转就正着乘。

4. 总结

这篇文章主要介绍了齐次变换矩阵的三种理解以及解决了一个比较容易造成混乱的多重变换时变换矩阵的顺序问题。由于个人能力有限，所述内容难免存在疏漏，欢迎指出，欢迎讨论。

下一篇：7. 机器人正运动学---连杆坐标系与DH参数