第03章 机器人的运动学和动力学

教案首页课程名称农业机器人任课教师李玉柱第3章机器人运动学和动力学计划学时 3教学目的和要求：1.概述，齐次坐标与动系位姿矩阵，了解平移和旋转的齐次变换；2.机器人的运动学方程的建立与求解*；3.机器人的动力学*重点：1.机器人操作机运动学方程的建立及求解；2.工业机器人运动学方程3.机器人动力学难点：1. 机器人动力学方程及雅可比矩阵基本原理思考题：1.简述齐次坐标与动系位姿矩阵基本原理。

2.连杆参数及连杆坐标系如何建立？3.机器人动力学方程及雅可比矩阵基本原理是什么？第3章机器人运动学和动力学教学主要内容：3.2 齐次坐标与动系位姿矩阵3.3 齐次变换3.4 机器操作机运动学方程的建立与求解3.5 机器人运动学方程3.6 机器人动力学本章将主要讨论机器人运动学和动力学基本问题。

先后引入了齐次坐标与动系位姿矩阵、齐次变换，通过对机器人的位姿分析，介绍了机器人运动学方程；在此基础上有对机器人运动学方程进行了较为深入的探讨。

3.1 概述机器人，尤其是关节型机器人最有代表性。

关节型机器人实质上是由一系列关节连接而成的空间连杆开式链机构，要研究关节型机器人，必须对运动学和动力学知识有一个基本的了解。

分析机器人连杆的位置和姿态与关节角之间的关系，理论称为运动学，而研究机器人运动和受力之间的关系的理论则是动力学。

3.2 齐次坐标与动系位姿矩阵3.2.1 点的位置描述在关节型机器人的位姿控制中，首先要精确描述各连杆的位置。

为此，先定义一个固定的坐标系，其原点为机器人处于初始状态的正下方地面上的那个点，如图3-1（a）所示。

记该坐标系为世界坐标系。

在选定的直角坐标系{A}中，空间任一点P的位置可以用3×1的位置向量A P表示，其左上标表示选定的坐标系{A}，此时有A P=XYZ P P P ⎡⎤⎢⎥⎢⎥⎢⎥⎣⎦式中：P X、P Y、P Z—点P在坐标系{A}中的三个位置坐标分量，如图3-1（b）。

3.2.2 齐次坐标将一个n维空间的点用n+1维坐标表示，则该n+1维坐标即为n维坐标的齐次坐标．．．．。

一般情况下ω称为该齐次坐标中的比例因子，当取ω=1时，其表示方法称为齐次坐标的规格化形式，即P=1X Y Z P a P b c P ω⎡⎤⎡⎤⎢⎥⎢⎥⎢⎥⎢⎥=⎢⎥⎢⎥⎢⎥⎢⎥⎣⎦⎣⎦ 式中;;X Y Z aP b P c P ωωω===3.2.3 坐标轴方向的描述如图3-2 所示，i 、j 、k 分别直角坐标系中X 、Y 、Z 坐标轴的单位矢量，用齐次坐标表示之，则有：由上述可知，若规定：4×1列阵[]0Ta b c 中第四个元素为零，且满足2221a b c ++=，则[]0a b c 中的a 、b 、c 表示某轴的方向。

4×1列阵[]0Ta b c 中第四个元素不为零，则[]a b c ω表示空间某点的位置。

图3-2中矢量u 的方向用4×1列可表达为：u =[]0Ta b c其中，cos ,cos ,cos a b c αβγ===。

100010,,001000X Y Z ⎡⎤⎡⎤⎡⎤⎢⎥⎢⎥⎢⎥⎢⎥⎢⎥⎢⎥===⎢⎥⎢⎥⎢⎥⎢⎥⎢⎥⎢⎥⎣⎦⎣⎦⎣⎦图3-2中矢量u 的起点O 为坐标原点，用4×1列阵可表达为：O =[]0001T【例3-1】用齐次坐标表示图3-3中所示的矢量 u ，v ，w 的坐标方向。

解 矢量：[][][]TTT:cos 0,cos 0.866,cos 0.500.8660.50:cos 0.866,cos 0,cos 0.50.86600.50:cos 0.866,cos 0.5,cos 00.8660.500αβγαβγαβγ============u u v v w w3.2.4 动系的位姿表示在机器人的坐标系中，运动时相对于连杆不懂的坐标系称为静坐标系，简称静系；跟随连杆运动的坐标系称为动坐标系，简称动系。

动系位置与姿态的描述是对动系原点位置及各坐标轴方向的描述。

（1）连杆的位姿描述设有一个机器人的连杆，若给定了了连杆PL 上某点的位置和该连杆在空间的姿态，则称该连杆在空间是完全确定的。

如图3-4所示，O '为连杆上一点，O X Y Z ''''为与连杆固接的一个动坐标系，即为动系。

连杆PL 在固定坐标系中的位置可用一齐次坐标表示为0001X Y P Z ⎡⎤⎢⎥⎢⎥=⎢⎥⎢⎥⎣⎦（3-4）连杆的姿态可由动系的坐标轴方向表示。

令n,o,a 分别为X Y Z '''、、坐标轴的单位矢量，各单位方向矢量在静系上的分量为动系各坐标轴的方向余弦，以齐次坐标形式分别表示为[]TX n n =Y Z n n 0 []T X o o o o =Y Z0 []TX a a a a =Y Z 0 （3-5）由此可知，连杆的位姿可用下述齐次矩阵表示：[]0000001X XX Y YY Z Z Z n o a X n o a Y T n o a P n o a Z ⎡⎤⎢⎥⎢⎥==⎢⎥⎢⎥⎢⎥⎣⎦（3-6）显然，连杆的位姿表示就是对固连于连杆上的动系的位姿表示。

【例3-2】图3-5表示固连于连杆上的坐标系{B }位于O B 点，2B X =,1B Y =，0B Z =。

在XOY 平面内，坐标系{B }相对于固定坐标系{A }有一个30°的偏转，试写出表示连杆位姿的坐标系{B }的4×4矩阵表达式。

解 X B 的方向列阵[][]Tcos30cos 60cos9000.8660.500=︒︒︒=nY B 的方向列阵[][]Tcos120cos30cos9000.50.86600=︒︒︒=-oZ B 的方向列阵[]T0010=a 坐标系{B }的位置阵列 []T2101=P 则动坐标系{B }的4×4矩阵表达式为0.8660.5020.50.8660100.8661001T -⎡⎤⎢⎥⎢⎥=⎢⎥⎢⎥⎣⎦(2) 手部位姿的描述机器人手部的位姿如图3-6所示，可用固连于手部的坐标系{B }的位姿来表示。

坐标系{B }由原点位置和三个单位矢量唯一确定如下。

①.原点 取手部中心点为原点O B 。

②.接近矢量 关节轴方向的单位矢量a 。

③.姿态矢量 手指连线方向的单位矢量o 。

④.法向矢量 n 为法向单位矢量，同时垂直于a 、o 矢量，即n =a ×o 。

手部位姿矢量为从固定参考坐标系OXYZ 原点指向于手部坐标系{B }原点的矢量P 。

手部的位姿可由4×4矩阵表示：[]0000001X XX Y YY Z ZZ n o a X n o a Y T n o a P n o a Z ⎡⎤⎢⎥⎢⎥==⎢⎥⎢⎥⎢⎥⎣⎦（3-7）【例3-3】 图3-7表示手部抓握物体Q ，物体为边长2个单位的正方体，写出表达该手部位姿的矩阵式。

解 因为物体Q 形心与手部坐标系O X Y Z ''''的坐标原点O '，所以手部位置的4×1列阵为[]T1111p =手部坐标系X '轴的方向可以用单位矢量a 来表示：n ：90,180,90αβγ=︒=︒=︒cos 0cos 1cos 0X Y Z n n n αβγ====-==同理，手部坐标系Y '轴与Z '轴的方向分别用单位矢量o a 和表示：Y Z Y Z : = 1 ,=0 , =0: =0 ,=0 , =1X X o o o a a a --o a根据式（3-7）可知，手部位姿可用矩阵表达为：[]01011001T 00110001-⎡⎤⎢⎥-⎢⎥==⎢⎥-⎢⎥⎣⎦no ap（3）目标物位姿的描述任何一个物体在空间的位姿和姿态都可以用齐次矩阵来表示。

【例3-4】如图3-8所示。

楔块Q 在图（a ）的情况下可用6个点描述，矩阵表达式为：111111000044002200111111Q ---⎡⎤⎢⎥⎢⎥=⎢⎥⎢⎥⎣⎦若让其绕Z 轴旋转90°，记为Rot （Z ，90°）；再绕Y 轴旋转90°，记为Rot （Y ，90°）然后沿X 轴平移4，即Trans （4，0，0），则楔块成为图（b ）位姿，其齐次矩阵表达式为446644111111000044111111Q ⎡⎤⎢⎥---⎢⎥'=⎢⎥⎢⎥⎣⎦可见，用符号表示目标物的变换方式，不但可以记录物体移动的过程，也便于矩阵的运算。

3.3 齐次变换在机器人中，手臂、手腕等被视为（连杆）刚体。

而刚体的运动一般可以包括平移运动、旋转运动和平移加旋转运动。

把刚体每次简单的运动用一个变换矩阵来表示，那么，多次运动即可用多个变换矩阵的积来表示，表示这个积的矩阵称为齐次变换矩阵。

这样，用连杆的初始位姿矩阵乘以齐次变换矩阵，即可得到经过多次变换后该连杆的最终位姿矩阵。

通过多个连杆位姿的传递，可以得到机器人末端操作器的位姿，即进行机器人正运动学的讨论。

3.3.1 平移的齐次变换首先，介绍点在空间直角坐标系中的平移。

如图3-9所示，空间某一点A ，坐标为（X A ,X B ,X C ）当它平移至点A '后，坐标为（,,AB C X X X '''）其中： AA A A AA X X X Y Y Y Z Z Z '=+∆'=+∆'=+∆ (3-10) 或写成如下形式：100010001000111AA A A A A X X X Y Y Y Z Z Z '⎡⎤∆⎡⎤⎡⎤⎡⎤⎢⎥⎢⎥⎢⎥⎢⎥'∆⎢⎥⎢⎥⎢⎥⎢⎥=⎢⎥⎢⎥⎢⎥⎢⎥'∆⎢⎥⎢⎥⎢⎥⎢⎥⎢⎥⎣⎦⎣⎦⎣⎦⎣⎦也可简写成：()Trans ,,A X Y Z A '=∆∆∆ (3-11) 式中()Trans ,,X Y Z ∆∆∆表示齐次坐标变换的平移算子，且100010Trans 0010001X Y Z ∆⎡⎤⎢⎥∆⎢⎥∆∆∆=⎢⎥∆⎢⎥⎣⎦（X ,Y ,Z ） (3-12)式中，第四列元素X Y Z ∆∆∆、、分别表示沿坐标轴X Y Z 、、的移动量。

若算子左乘，表示坐标变换是相对于固定坐标系进行的，加入相对动坐标系机型坐标变换，则算子应该右乘。

由上述推导可以看出，平移变换的数学实质是求两个矢量的和，T 为平移变换矩阵。

对于二维情况，它是3×3单位方阵。

它是由一个2×2单位矩阵，和所求点的列矢量以及满足齐次坐标表达式而增加的一个行矢量[]001组成。

对于三维情况，它是4×4单位方阵。

完成平移变换的关键在于要构造一个变换矩阵T 。

平移的齐次变换公式（3-12）同样适用于坐标系、物体等的变换。