机器人及其控制第二章

1、第二章：数学和物理基础空间描述和变换预备知识0.1 向量：有向线段。向量加法满足平行四边形法则，即问题：既有大小又有方向的量是否为矢量？一般规定向量为列向量而在MATLAB中，向量指的是行向量。向量的点积(a在b上的投影)：如果a，b是单位矢量，且它们夹角为则称为夹角余弦。有了点积的概念，一个向量a在3维直角坐标系内的坐标为，即在3个坐标轴上的投影且向量的长度为（在3维直角坐标系内）：两个3维向量的叉积（只能在3维空间定义）0.2 矩阵：数的排列矩阵A，B相乘得矩阵D定义为：设，则即矩阵相乘表示矩阵A的第i行乘以矩阵B的第j列。矩阵相乘满足结合律但不满足交换律分块矩阵相乘的求法设，则这说明分块

2、矩阵相乘时，把对应的块看做矩阵元素，按照矩阵的乘法相乘即可。分块矩阵求逆设：则证明（根据逆矩阵的定义）0.3 惯性系存在。刚体：是这样一种质点组，组内任意两质点间的距离保持不变。性质：自由刚体的自由度是6，非自由刚体的自由度<6.刚体的6个自由度可以这样选取：刚体上某一定点（称为基点）和其姿态。刚体的定点一般选取其质心，而姿态为在刚体上建立坐标系，称为本体坐标系。刚体的运动方式有：平动，当刚体运动时，其上所有质点具有相同的速度和加速度，以一个质点的运动就可以表征整个刚体的运动。转动，当刚体运动时，刚体上有一点的位置保持不变。蔡斯尔定理：刚体的一般运动为平动加定点转动。（其实还有第二部分内

3、容）数学证明定义:令,若对所有均有则称h是一个(Euclid)等距变换.定理:令,为的映射,则(a) 映射h是一个等距变换当且仅当它保持点积不变;(b) 映射h是一个等距变换当且仅当它保持正交不变.定理:令,那么h为一个等距变换当且仅当它是一个正交变换后接一个平移变换的复合,即当且仅当h具有如下形式其中A为正交矩阵.证明:给定h,令,并且定义那么由直接计算得因而k是一个等距变换当且仅当h是等距变换. 由于,因此k是等距变换当且仅当其中A为正交矩阵.然而这种情况当且仅当时才会发生.0.3 Matlab相关内容向量的函数dot(a,b): cross(a,b): norm(a)a=1:3;b=7:

4、9;c=dot(a,b);d=cross(a,b);e=norm(a);c,d,ec =50d = -6 12 -6e = 3.7417符号变量syms a b c dA=a b;c d;A=sym(a b;c d);syms e1 e2A=sym('cos(e1) (-1)*sin(e1); sin(e1) cos(e1)');B=sym('cos(e2) (-1)*sin(e2); sin(e2) cos(e2)');C=A*B;CD=simple(C);DC = cos(e1)*cos(e2)-sin(e1)*sin(e2), -cos(e1)*sin(e

5、2)-sin(e1)*cos(e2) sin(e1)*cos(e2)+cos(e1)*sin(e2), cos(e1)*cos(e2)-sin(e1)*sin(e2)D = cos(e1+e2), -sin(e1+e2) sin(e1+e2), cos(e1+e2)符号矩阵的转置A=sym('a b;c d');B1=A'B2=A.'B3=transpose(A);B1,B2,B3B1 = conj(a), conj(c) conj(b), conj(d)B2 = a, cb, dB3 = a, cb, d反正切函数function z=Atan(y,x)% 此

6、函数为带象限的反正切函数if y=0 & x=0z=0;elseif x=0 & y>0z=pi/2;elseif x=0 & y<0z=3*pi/2; elseif x>0 z=atan(y/x);elseif x<0z=atan(y/x)+sign(y)*pi;endreturn;Atan(0,1),Atan(1,1),Atan(1,0)Atan(1,-1),Atan(0,-1),Atan(-1,-1)Atan(-1,0),Atan(-1,1),Atan(0,0)0 0.7854 1.57082.3562 0 -2.3562-1.5708 -

7、0.7854 02.1 概述2.2 描述：位置、姿态与坐标系2.3 从坐标系到坐标系的变换2.4 算子：平移和旋转变换2.5 总结和说明2.6 变换算法2.7 变换方程 2.8 姿态的其它描述方法2.9 自由矢量的变换2.10 计算分析2.1 概述时间 空间坐标系世界坐标系：一般指惯性参考系一般选择地球作为惯性系，时间和空间是独立的坐标系一般笛卡尔坐标系（Cartesian coordinate system）.以数值来描述某一定点的位置。2.2描述：位置、姿态与坐标系描述：形式化描述位置描述在直角坐标系A中,空间任意一点p的位置可用3x1列向量(位置矢量)表示： (2.1)姿态描述给出了刚体

8、的方位研究刚体的运动一般通过固连在刚体上的坐标系的变化来获得。问题：解决方法：把排成一个矩阵，就是此矩阵就称为旋转矩阵坐标系B的每个单位矢量可以用点积表示其中第一列是根据称为旋转矩阵，也称为方向余弦矩阵由上式不难得到：这表明旋转矩阵的逆等于它的转置：即坐标系的图形表示这是旋转矩阵的第一个作用在已知坐标系A的前提下坐标系的指向表示了坐标系的转换方向。坐标系之间的变换可由位置矢量和旋转矩阵共同作用决定。2.3 从坐标系到坐标系的变换(坐标变换)坐标平移：A和B姿态一样，但原点不同现在的问题是：已知坐标系A，坐标系B原点矢量，以及矢量P在坐标系B中的坐标，求矢量P在坐标系A中的坐标。即已知求：解：从

9、数学上看，这是坐标变换。点本身没有改变。坐标旋转：现在的问题是：已知坐标系A、B的原点重合，且已知B的旋转矩阵，以及矢量P在坐标系B中的坐标，求矢量P在坐标系A中的坐标。即已知求：解：根据坐标的定义如果考虑在坐标系B中表示即例2.1：已知坐标系B相对坐标系A绕Z轴旋转Z轴30deg，且已知求解：只要写出旋转矩阵现在考虑一般情况：坐标系B和坐标系A的原点不重合，且B相对于A有旋转。即已知：求首先将映射到一个中间坐标系，这个坐标系和A的姿态相同。原点和B的原点相同。可以象上节那样，左乘得到。然后用简单的矢量加法，得到： (2.2)引入新的概念形式相当于这个向量经过矩阵的一次变换齐次变换(Homog

10、eneous Transformations) (2.3)表达更简洁，概念更明确。其中，齐次变换矩阵变为把（2.3）写开就是：因此（2.3）用于公式推导和理解欧氏几何，透视几何，摄影几何，微分几何例2.2：坐标系B沿坐标系A旋转了30deg，沿X轴平移10个单位，沿Y轴平移了5个单位，已知：求: 按照齐次矩阵的分块矩阵写法。便可写出齐次变换矩阵。因为 所以得2.4 算子：平移和旋转变换坐标变换指的是点没变，坐标变了这里的算子是指的变换平移算子（Translational Operator）：一个矢量沿着另外一个矢量移动，在同一个坐标系内，矢量可以相加写成齐次矩阵的形式其中表示距离。旋转算子记住

11、：即可以看成旋转坐标的形式，又可以看成齐次矩阵的形式。绕某一矢量旋转某一角度，记作：矢量经某一旋转R得到的旋转矩阵与一个坐标系相对于参考系经某一旋转得到的旋转矩阵相同。一般变换算子（General Operator）：平移和旋转其中经过旋转R和平移Q的齐次变换矩阵与一个坐标系相对于参考系经旋转R和平移得到的变换矩阵相同。2.5 总结和说明齐次矩阵可以表示什么？（1）坐标系描述是坐标系B相对于相对于坐标系A的描述（2）映射：坐标变换（3）变换算子2.6 变换算法：齐次变换的性质混合变换（组合变换）已知：求: 就是已知在坐标系C中的坐标，求在坐标系A中的坐标。但经过中间坐标系B。由坐标系B到坐标系

12、C的坐标变换为：由坐标系A到坐标系B的坐标变换为：联合以上两式得：由坐标系A到坐标系C的坐标变换为：定义则可求其表达式，根据分块矩阵的求法：逆变换(Inverse Transform)已知：求：第一种方法：直接利用矩阵求逆公式求第二种方法：根据物理意义来求。根据齐次矩阵的定义根据以前的讨论可知剩下的是来求，可以利用来求，令则a表示在坐标系A中的向量，可以在坐标系B中描述但是表示坐标系B的原点，因而有从而有从而得到定理：SE(3)=T|T为齐次变换矩阵，SE(3)构成一个群。证明：只要证明（1） 有单位元；（2） 封闭性（3） 结合律（4） 存在逆元。2.7 变换方程 现在有两种路线:第一种路线

13、及相应的变换为第二种路线及相应的变换为得到一个变换方程 (2.3)如果求在（2.3）两边求逆矩阵，就可以得到注意：未知变换个数方程个数也可以图形进行求解，用图形求解时，一定要注意：箭头所指的方向表示在已知的坐标系描述。比如表示已知，如果用了相反的方向：就用表示。现在求例：操作臂（机器人），求零件（螺栓）坐标系G相对指端坐标系T的转换关系。即求：可用图形表示进行求解：2.8 姿态的其它描述方法旋转矩阵方向余弦（有时候如下表示）有如下约束：这说明正交矩阵有三个独立参数。凯莱公式：对于任何一个正交矩阵R，存在一个反对称矩阵S，使得其中旋转矩阵的变换是不可交换的 从而说明有限时间的旋转不是矢量。问题：

14、旋转和平移是否可以交换？旋转矩阵既可作为算子（数学上）也可看作对姿态（物理上）的描述。作为算子，和矢量相乘，表示对矢量起旋转作用。但作为姿态描述时，如对机械手的期望姿态时：输入三个元素比输入九个元素（注意：这九个元素不独立）更简便些。特别这三个元素具有明确的物理意义时。X-Y-Z固定坐标系首先将坐标系B和已知参考坐标系A重合。先将B绕旋转角，再绕旋转角,最后绕旋转角。记住两点：（1） 始终在绕A的坐标轴在旋转；（2） 旋转的顺序为X、Y、Z。Roll, Pitch, Yaw Angles滚动、俯仰、偏航角 下面求的旋转矩阵设一向量在A和B的坐标系表示为则容易知道即从旋转矩阵求得绕固定坐标系XY

15、Z的旋转角即逆问题（Inverse Problem），即给定：求出，现在进行求解：给定：从而在求出之后综合得到如果，这时，将发生奇异第一种情况：从而如果令容易知道综合得到第二种情况：从而如果令容易知道综合得到Z-Y-X Euler角首先将坐标系B和已知参考坐标系A重合。先将B绕旋转角，再绕旋转角,最后绕旋转角。记住两点：（1）始终在绕新的坐标系的坐标轴在旋转；（2）旋转的顺序为Z、Y、X。这三个一组的旋转角称为Euler角。每次所绕的轴的方位取决于上次的旋转。这种表示方法称为Z-Y-X Euler角。通过坐标系描述求得旋转矩阵写出具体表达式就是结论：三次绕固定旋转的最终姿态和以相反顺序绕运动坐

16、标轴旋转的最终姿态相同。因此给定旋转矩阵求解Euler角和上面完全一样。其他角坐标系的表示方法：Z-Y-Z Euler角等固定坐标系旋转 12种旋转坐标系旋转 12种一共24种但由于固定的和旋转的具有对偶性，因此旋转矩阵一共有12种。下面图形演示的是Z-X-Z Euler角等效轴角坐标系表示法（Euler定理）刚体定点的运动的任意位移可以通过绕过该定点的某个轴的一次转动实现。证明：Euler定理等价于旋转矩阵有等于1的特征值。即： 设为特征多项式，只需证明即：。首先将坐标系B和一个已知坐标系A重合，将坐标系B绕矢量按右手定则旋转角。注意：只有两个独立参数，因为有这样一个约束。再加上这个参数，一

17、共三个独立参数。对于特殊的情况，绕X、Y、Z轴是容易求出的。现在考虑一般情况，对于一般旋转，如何求出旋转矩阵。可用下图表示：可做如下旋转其中就得到其中现在问题是给定：如何求首先：因此其次从而同理最后得到如果，将出现奇异，无法确定。绕固定轴旋转还可以用指数表示，即下面的定理：其中证明例2.3：坐标系B最初与坐标系A重合，使坐标系B绕矢量（此矢量经过）旋转，转角，求坐标系B的描述。解：引入两个中间坐标系且满足如下条件：具有相同的方向（姿态），它们之间只是平移关系： 现在坐标系的转换关系为：而为绕固定轴旋转，因此有根据坐标系的描述关系：Euler参数（四元数）满足关系式可以看作4维空间单位球面上的一点。这种表示方法称为四元数表示法四元数表示法的旋转矩阵为：已知旋转矩阵求四元数表示法可以求得： 我们来看具体求解过程：从而其他参数可容易解决：容易知道可类似求出。如果，怎么办？可以令 从而进而求得：所以用四元数表示永远不会出现奇异。2.9 自由矢量的变换线矢量：与作用线（作用点）有关的矢量