机器人基于视觉的控制（机器人控制方向与机器视觉方向）

本文是自己学习《机器人学、机器人视觉与控制》最新版本的笔记，里面涉及到机器人坐标转换非常详细的算法，使用matlab工具软件举例。

分为二个部分：

1、思维方式概述

2、二维坐标系的位置姿态转换详解

按照思维习惯由浅入深的讲解，为了完成我做一个扔垃圾机器人的梦想，已经学习这个基础知识好几遍了；梦想还是比较远，但是过程中可以学到非常多的知识，过程比目的地更有趣。

有这方面兴趣的朋友，又可以静心学习的可以深入看看！学习没有捷径，该学的知识点，术语，符号都得搞懂，不然进入不了下个阶段的学习！

第一部分：思维方式概述

概述捋顺一下理解位姿的思维方式

空间中的点是数学中常见的概念，可以用坐标向量来描述，向量是既有大小又有方向的一种量；

我刚开始接触的机器人的时候，有个疑问，为什么要用向量来表示呢，直接用坐标形式x/y/z数值不是更好理解吗，后来多看了几遍资料，发现我认为的坐标数据，其实在内部算法里都是用向量矩阵表示的，所以想深入学习机器人，还是要用向量的思维去看点，比如说下图a的点P，直接就把它看成是向量P，两个分向量Py和Px相加的和，Py和Px又是有多个单位向量组成！单位向量就是长度为1的向量; 这个时候思路应该转变成向量思路了，比如P坐标（2，3），不应该理解成，X轴上是数据2，Y轴上是数据3；而是理解成P点的X轴坐标是2个单位向量组成，Y轴坐标是3个单位向量组成！

坐标系里的一个点好表示，但是大多数情况下，是坐标系里的一个物体，而这个物体又包含很多点；

分两步分析：

第一，确定这个物体的位置和方向，如图b，向量B表示小车的位置和方向；

第二步，以这个B点为原点，设定小车自己的坐标系，这样就可以知道小车上1、2、3、4这四个点的位置和方向，如图b用四个向量表示！

接下来要约定一下用符号表示坐标系相互关系的方法；

以前我都是忽略这些约定符号，感觉看着累，不直观，也不容易理解；但是越深入学习越感觉这些符号公式的重要性，因为简单的可以直观理解，或者用语言就可以描述，但是简单的是基础，最终都会参与到复杂的运算，复杂的运算只能靠符号和公式解决，不然会毫无章法！

利用上面这些约定，画个如下图所示的坐标系，有两个坐标系{A}，和{B}；

坐标系和向量的关系可以写成下面的公式，表示A坐标系下的P向量；

可以分两步表示，先表示以A坐标系作为参考坐标系表示B坐标系；

然后点乘B作为参考坐标系下的P向量；

上面例子表示出一个重要思路，那就是一个坐标系可以用另一个坐标系表示，这在机器人学里面是应用非常多的一个思路，要记住这个思路，这就说明机器人实际应用中，无论工件上设定的点在哪里，只要它有自己的工件坐标系，工件坐标系可以和机器人基坐标系关联起来，那么这个工件上的点也就可以用机器人基坐标系表示！

所以机器人坐标系可以千变万化，但是机器人最终计算的只有一组相对于世界坐标系的数值。

更复杂点的三个坐标系关联；

坐标系{A},{B},{C},如下图所示：

那么向量P的表示可以用下面的公式，A坐标系下的向量P可以分三步表示；

首先表示A坐标系下的B坐标系；

然后再表示B坐标系下的C坐标系；

最后表示C坐标系下的P向量；公式中间那个加号加圆圈代表坐标系转换关系。

接下来再举个三维坐标系的例子

帮助大家理解坐标系的转换关系；如下图所示，在世界坐标系{O}里有一个机器人坐标系为{R}，机器人上安装一台相机，相机坐标系为{C}，屋顶上有个固定的相机，固定相机坐标系为{F}，还有一个工件，工件坐标系为{B}；

如下面两个变换等式；

第一个等式表示，世界坐标系下{O}的固定相机坐标系{F}，变换到固定相机{F}坐标系下的工件坐标系{B}，等于 世界坐标系{O}下的机器人坐标系{R}，变换到机器人{R}坐标系下的机器人相机坐标系{C}，再变换到机器人相机{C}坐标系下的工件坐标系{B};

第二个等式表示，世界坐标系{O}下的固定相机坐标系{F}，变换到固定相机{F}坐标系下的机器人坐标系{R}，等于 世界坐标系{O}下的机器人坐标系{R};上图坐标系和下面等式中大家看到世界坐标系下的坐标系{F}，{R}没有左上标，那是因为定义了世界坐标系可以不写左上标！

把上面的三维坐标系关系简化成下图所示：

上面的变换关系在实际应用中还是很多地方用到的，比如一个移动机器人带着个相机的项目，就可以精确地抓取到工件了！

第二部分，详解二维空间的位姿描述

二维坐标系变换概述，需要的三个参数

用向量的思维，二维坐标系下的一个点P，应该用向量P表示，向量P又可以用X轴和Y轴的单位向量表示；如下图的公式，因为单位向量长度（模）为1，所以向量P是X个X轴单位向量与Y个Y轴单位向量的和；

下图表示了一个红色坐标系和一个蓝色坐标系的关系，图中明显可以看出，红色坐标系{B}是蓝色坐标系{A}，通过向量t=（x,y）移动得到；然后再逆时针旋转θ角度。

由上可以看出，二维坐标系下，以坐标系{A}为参考坐标系，坐标系{B}的位置与姿态的确定，需要三个向量，（x,y,θ）,这三个向量，涉及到三角函数的计算，无法直接用这三个值转换坐标系，所以下面我们用一种方法来表示旋转；

旋转变换的推导

看上图，任意点P相对于坐标系{A}和{B}的向量分别是AP和BP，我们要确定AP和BP之间的关系，要分两步走，旋转然后平移；

具体如上图所示，先只考虑旋转的情况，点P在参考坐标系{V}中，用单位向量组合可表示为如下等式：

上式最终写成行向量和列向量的点积形式，矩阵的概念马上要得到应用了；假设上图中坐标系{V}和{B}的轴长都是1，也就是单位向量，那么坐标系{V}经过旋转θ角度后，得到坐标系{B},坐标系{B}上X轴和Y轴的单位向量可表示为：

写成矩阵的形式如下：

这个单位向量旋转的关系就出来了，

接着点P在参考坐标系{B}中，用单位向量组合可表示为如下等式：

代入上式得：

和等式结合运算，最后可以写成

这个等式就是表示坐标系旋转后，参考坐标系{B}里的P点，怎么变换成参考坐标系{V}里的P点；等式中间的二维矩阵就叫旋转矩阵，用符号表示为：

上式可以写成符号表示：

同样的道理，坐标系旋转后，坐标系{V}里的点变换为坐标系{B}里的点，公式可以表示成：

上式也可以看出逆矩阵就是旋转矩阵上下标互换；

MATLAB模拟旋转变换程序

下面用号称可以节省生命的超级神器MATLAB来做一下旋转矩阵的模拟：

>> R=rot2(0.2)
R=0.9801 -0.1987
0.1987 0.9801

这段程序里rot2（θ）是2*2的旋转矩阵，θ的单位是弧度！

旋转矩阵有一些性质可以用下面这个函数实验下：

>> det(R)
ans=1
>> det(R*R)
ans=1

旋转矩阵的行列式是1，旋转矩阵乘以旋转矩阵的行列式也是1；

这里的det（A）是矩阵行列式，结果是矩阵A的行列式；

什么是行列式？

分三步来理解：

第一步、行列式det（A）是针对n*n的矩阵A而言的，A表示一个n维空间到n维空间的线性变换，线性变换就是压缩或拉伸；假设原来空间有个n维的立方体，经过线性变换变成一个新的n维立方体。

第二步、原来立方体体积是V1,新的立方体体积是V2！

第三步、V2除以V1就是det(A)的值。

工具箱也支持用符号代替数字计算，如下声明一个符号theta：

>> syms theta
>> R=rot2(theta)
R=[ cos(theta), -sin(theta)]
[ sin(theta), cos(theta)]

矩阵的指数和对数计算

下面还是把0.3的弧度转换为旋转矩阵；

>> R=rot2(0.3)
ans=0.9553 -0.2955
0.2955 0.9553

我们可以用logm函数计算矩阵的对数

>> S=logm(R)
S=0.0000 -0.3000
0.3000 0.0000

然后用工具箱函数vex来解析S矩阵；

> > vex(S)
ans=0.3000

得出的结果是又算出了旋转角度的弧度值0.3；

矩阵对数logm的倒数（逆）是矩阵的指数函数expm；

>> expm(S)
ans=0.9553 -0.2955
0.2955 0.9553

结果得到了原来的旋转矩阵，还原了logm计算之前的数值；

矩阵的对数也就是矩阵指数的倒数（逆）！矩阵的指数或对数的计算还是比较复杂的，我觉得我会用这个函数就行了，想深入了解的要去看看《李群论》！

进而推导出二维的齐次变换矩阵

上面讲了旋转矩阵，这里旋转后再加上平移讲一下坐标系原点之间的平移，二维坐标系平移，就是简单的向量加法结合下面两图得出：

进而可以简写成：

上公式中t=（x,y）,方向是{A}到{B}的变换；

再写成P点的变换：

其中的T就是齐次变换矩阵：

MATLAB函数计算齐次变换 矩阵

下面用MATLAB工具箱里面的函数具体计算一下;

程序是表示平移（1，2），再旋转30度，得到其次变换矩阵T1;

>> T1=transl2(1, 2) * trot2(30, 'deg')
T1=0.8660 -0.5000 1.0000
0.5000 0.8660 2.0000
0 0 1.0000

transl2函数表示二维坐标系下的平移，trot2函数表示二维坐标系下的旋转；

MATLAB图形化输出程序：

Plotvol(0 5 0 5)表示创建一个X轴长度为5，Y轴长度也为5的二维坐标系；

trplot2(T1, ‘frame’, ‘1’, ‘color’, ‘b’) 表示建立一个坐标系T1，显示坐标系名称为1，颜色是b，蓝色；

>> plotvol([0 5 0 5]);
>> trplot2(T1, 'frame', '1', 'color', 'b')

如下图所示的蓝色坐标系：

接着再创建一个T2坐标系，只从原点平移（2，1）；输出图形为上图的红色坐标系：

>> T2=transl2(2, 1)
T2=1 0 2
0 1 1
0 0 1
>> trplot2(T2, 'frame', '2', 'color', 'r');

再创建一个绿色的坐标系T3，它的其次变换是T1*T2；也就是先平移（1，2），再旋转30度，再在坐标系{1}的基础上平移（2，1），上图中可以看出最终绿色坐标系{3}的位置并不是（3，3），那是因为第二次平移是在坐标系{1}的基础上平移的！程序如下：

>> T3=T1*T2
T3=0.8660 -0.5000 2.2321
0.5000 0.8660 3.8660
0 0 1.0000
>> trplot2(T3, 'frame', '3', 'color', 'g');

在创建一个T4，和T3对比下，T4的变换是T2*T1，变换顺序就相反了，它是先平移(2,1),再平移（1，2），再旋转30度；如上图中的坐标系{4}所示，程序如下：

>> T4=T2*T1;
>> trplot2(T4, 'frame', '4', 'color', 'c');

图中可见，其次变换矩阵左乘和右乘的结果是完全不同的;

我们再创建一个点P（3，2）；

如上图所示；程序如下：

>> P=[3 ; 2 ];
>> plot_point(P, 'label', 'P', 'solid', 'ko');

下面再来研究这个点P相对与各个坐标系的坐标

看看P点在坐标系{1}里的表达，前面学到的术语在这里用到了，在坐标系{0}里的P点，等于坐标系{0}变换到坐标系{1}，再表示坐标系{1}下的P点，等式如下：

然后转换一下，变成坐标系{1}下面的P点坐标的表达，也就是坐标系{1}变换为坐标系{0}，再点乘坐标系{0}下的P点坐标向量！

用程序表示为下，得出P点的其次变换形式，P点的矩阵里面加个1就是为了和其次变换格式相同，函数inv是矩阵求逆；

>> P1=inv(T1) * [P; 1]
P1=1.7321
-1.0000
1.0000

也可以试试下面的形式，函数e2h是将欧几里得坐标系转换成其次变换坐标形式，h2e再逆转换，把其次变换形式转换成普通坐标形式；

>> h2e( inv(T1) * e2h(P) )
ans=1.7321
-1.0000

详细探究纯旋转的旋转中心例子

首先创建一个X坐标轴是-5到4，Y坐标轴是-1到5的坐标系{0}；再创建一个目标坐标系{X};函数eye是创建单位矩阵，下面程序中说明T0是3*3的单位矩阵；

>> plotvol([-5 4 -1 5]);
>> T0=eye(3,3);
>> trplot2(T0, 'frame', '0');
>> X=transl2(2, 3);
>> trplot2(X, 'frame', 'X');

再创建一个2弧度的旋转；

>> R=trot2(2);

输出如下两种组合的图形，如下图所示，坐标系{RX}是围绕坐标系{O}原点旋转2弧度（115度），坐标系{XR}是围绕坐标系{X}旋转2弧度，可以看出两种不同的组合的输出结果不同了吧！

>> trplot2(R*X, 'framelabel', 'RX', 'color', 'r');
>> trplot2(X*R, 'framelabel', 'XR', 'color', 'r');

深入点应用，如果我们想绕任何一点旋转该怎么做呢？

我们先创建任意一点C，如上图所示，程序如下：

>> C=[1 2]';
>> plot_point(C, 'label', ' C', 'solid', 'ko')

然后写一下绕C点旋转的程序，理解为先平移到C点，再按照旋转矩阵R旋转，再把矩阵里面的每个元素变成负数，再平移,得到了{X}绕C点旋转的坐标系{XC},如上图所示；这个在实际应用中很有用，比如想让机器人绕抓手上任意一点旋转，TCP的理论基础！

>> RC=transl2(C) * R * transl2(-C)
RC=-0.4161 -0.9093 3.2347
0.9093 -0.4161 1.9230
0 0 1.0000

输出图形结果：

>> trplot2(RC*X, 'framelabel', 'XC', 'color', 'r');

扭转函数Twist

绕任意点旋转在机器人工具箱里有个更方便的函数Twist，例如我们想创建上面的绕C点的旋转2弧度，可以写成，

>> tw=Twist('R', C)
tw=( 2 -1; 1 )

Twist函数里面包含很多参数，我们看看它生成的旋转矩阵是啥样的：

>> tw.T(2)
ans=-0.4161 -0.9093 3.2347
0.9093 -0.4161 1.9230
0 0 1.0000

是不是和上面一堆公式推导出的RC旋转矩阵是一样的，这就是MATLAB工具箱的牛逼地方，节省生命的工具！我觉得会用这些函数就好，如果应用到实际机器上，MATLAB可以转换成很多种语言，包括PLC的ST语言！

二维坐标系的姿态描述就这么多了，连翻译带学习了皮特的《机器人学、机器人视觉与控制》，受益匪浅，二维的适合移动小车机器人的算法，三维也会用到，对于自己开发一款运动机器人，是很重要的基本功；实际工业机器人使用中也很有用，我想写个坐标系转换的算法，就到这里来调用函数，再生成需要的语言，稍微改动下就可以使用，可以增加效率！