《智能机电系统设计与开发》课件 第七章-运动学建模与轨迹规划

7.1概述7.2空间描述与坐标系7.3运动学建模与分析7.4速度传递矩阵7.5轨迹规划7.6运动学建模与轨迹规划项目实践第七章运动学建模与轨迹规划7.1概述

运动学包括两类问题：正运动学和逆运动学。正运动学研究如何将机电系统各个坐标轴的运动映射为其末端工具（刀具）在直角坐标系的运动；逆运动学研究如何将机电系统末端工具在直角坐标系的运动分解为各个坐标轴的运动。本章主要以工业机器人为例介绍其运动学建模过程和分析方法。7.1概述

运动学模型是控制工业机器人按给定要求完成作业任务的基础，在完成给定的作业任务之前，还应该先规定工业机器人的操作顺序、路径形状、起点、中点、终点和操作时间等，然后控制系统根据这些给定条件，自动确定其每个坐标轴按时间历程的位置、速度和加速度等，这就是轨迹规划。1.空间描述7.2

空间描述与坐标系

齐次矩阵

工业机器人的各个运动组件都可以看做一个刚体，要描述刚体的运动，可以在刚体上固定一个坐标系，用该坐标系原点在参考坐标系的位置矢量和坐标系姿态来描述刚体的位置和姿态。在一个机械手抓取系统中，机械手爪的参考坐标系为{A}，与手爪固定的坐标系为{B}，则手爪的位置矢量用

表示。

左上标A表示矢量的参考坐标系，

表示矢量

在坐标系{A}的三个坐标轴的投影。1.空间描述7.2

空间描述与坐标系

齐次矩阵

手爪的姿态

可以由坐标系{B}的三个坐标轴矢量

在坐标系{A}中的投影按顺序作为矩阵的列构成的旋转矩阵表示。1.空间描述7.2

空间描述与坐标系

齐次矩阵手爪的位姿由矢量

和

的三个矢量，共计四个矢量确定，记为当{B}和{A}平行时，

仅表示坐标系{B}的位置，

为单位矩阵，当{B}和{A}的原点重合时，

仅表示坐标系{B}的姿态，

为零矢量。1.空间描述7.2

空间描述与坐标系

齐次矩阵为了便于用矩阵进行运算，将其改成齐次矩阵的形式式中：

称为齐次坐标，

称为齐次变换矩阵，共有16个元素，除去4个常数，

含有3个独立元素，

中含有9个元素，但

中只有3个元素独立，因为

、

、

都是单位主向量，且两两正交，故有6个约束条件1.空间描述7.2

空间描述与坐标系

欧拉角所以，齐次矩阵只有6个独立变量，3个表示位置，3个表示姿态。实际上，旋转矩阵可以用三个参数的姿态表示，如X-Y-Z欧拉角。这种表示方法是：假设初始状态时坐标系与参考坐标系重合，然后将绕着自身坐标系的轴旋转角，再绕着旋转后新坐标系的旋转角，最后绕着第二次旋转后新坐标系的旋转角，进而形成最终的坐标系。1.空间描述7.2

空间描述与坐标系

欧拉角按照X-Y-Z欧拉角的变换方式得到坐标系相对于参考坐标系的旋转矩阵为：为使矩阵简洁起见，

和

缩写为

和

。可以根据姿态矩阵求出

，

，

即可以用这三个参数表示两个坐标系之间的姿态。2.坐标变换7.2

空间描述与坐标系

刚体的位置和姿态可以在不同的坐标系中描述，并且可以通过坐标变换公式将这些不同的描述联系在一起。坐标变换包括坐标平移变换、坐标旋转变换和复合变换。平移变换和旋转变换

2.坐标变换7.2

空间描述与坐标系

复合变换

3.坐标系设置7.2

空间描述与坐标系

如前所述，刚体的位姿可以在坐标系中描述，因此坐标系的设定非常重要。这里的坐标系都是直线运动轴两两垂直的右手笛卡尔直角坐标系。3.坐标系设置7.2

空间描述与坐标系

为了规范工业机器人的运动，需要为工业机器人和操作对象确定专门的“标准”坐标系。为了便于描述机器人的运动控制，设置了5个直角坐标系：基坐标系{B}、固定坐标系{S}、腕部坐标系{W}、工具坐标系{T}、目标坐标系{G}。7.3

运动学建模与分析

工业机器人是由连杆通过关节连接在一起的开式运动链。为了便于描述工业机器人的运动关系，需要在其每个连杆上分别固接一个连杆坐标系，然后再研究当各个连杆通过关节连接起来后，连杆坐标系之间的相对关系，最后建立以关节变量为自变量的工业机器人末端执行器位姿函数。1.连杆描述7.3

运动学建模与分析

工业机器人的连杆是连接两个相邻关节轴的刚体，在分析运动关系时，可以将连杆抽象为两个相邻关节轴之间的公垂线，这样连杆可以用两个参数描述。

连杆i-1长度：连杆i-1前端关节轴i-1和后端关节轴i之间公垂线的长度，记为ai-1

连杆i-1扭角：关节轴i-1按右手法则绕ai-1

旋转到与关节轴i平行的角度，记为αi-12.连杆连接的描述7.3

运动学建模与分析

连杆偏距：从公垂线ai-1与关节轴i的交点到公垂线ai与关节轴i交点的有向距离，为关节轴i上的连杆偏距，记为di。关节角：从公垂线ai-1绕关节轴i按右手法则旋转到公垂线ai的角度，为绕关节轴i的关节角，记为θi

。3.在连杆上建立坐标系7.3

运动学建模与分析

对中间连杆的坐标系做如下规定:（1）坐标系{i-1}的原点位于公垂线ai-1与关节轴i-1的交点处；（2）Zi-1轴与关节轴i-1重合；（3）Xi-1轴沿ai-1方向，由关节i-1指向关节i；（4）Yi-1轴由右手定则确定，即Yi-1=Zi-1×Xi-1。4.连杆间坐标系变换7.3

运动学建模与分析

按上述规定将坐标系固连在工业机器人的全部连杆后，可以建立任意相邻连杆坐标系{i-1}和{i}之间的变换关系，这种变换通过几个中间坐标系来实现，按照之前介绍的坐标复合变换方法，可得：考虑每一个变换矩阵，将这些变换矩阵分别带入各自相对X和Z轴的旋转和平移公式，最终可得连杆间坐标变换的通式：5.正运动学建模与分析7.3

运动学建模与分析

综上所述，对工业机器人进行正运动学建模的大致步骤如下:1)找出各个关节轴线；2)确定各连杆坐标系；3)确定各连杆D-H参数；4)求出相邻两杆间的位姿矩阵；5)求末端工具相对于基座的位姿矩阵。5.正运动学建模与分析7.3

运动学建模与分析

下面以PUMA560六轴机器人为例，介绍其运动学建模过程。首先确定机器人各连杆D-H参数：5.正运动学建模与分析7.3

运动学建模与分析

按连杆变换通式及连杆参数表，可求得各个连杆间的变换矩阵如下：5.正运动学建模与分析7.3

运动学建模与分析

将上述连杆变换矩阵依次相乘便得到PUMA560的末端工具相对于基座的位姿描述矩阵，也叫运动学方程其中，6.逆运动学建模与分析7.3

运动学建模与分析

PUMA机器人的逆运动学建模过程，就是已知之前求得的

齐次变换矩阵中的n、o、a、p四个矢量，求解对应的关节角度θ1~θ6。1）求θ1：

6.逆运动学建模与分析7.3

运动学建模与分析

2）求θ3：

其中，

由推导可知，θ3也有两个解。3）求θ2：接着上述获得的条件继续推导，可以得到θ1和θ3各有两个解，所以θ2应该有四种解。6.逆运动学建模与分析7.3

运动学建模与分析

4）求θ2：接着上述获得的条件继续推导，可以得到因为s5有两个解，所以θ5也有两个解。5）求θ4：只要s5≠0，便可求出θ4

当s5=0时，机械手处于奇异形位，此时，关节轴4和6重合，只能解出θ4与θ6的和或差。在奇形位时，可任意选取θ4的值（一般取当前值），再计算相应的θ6。6.逆运动学建模与分析7.3

运动学建模与分析

6）求θ6：计算方法和过程跟计算θ4一样。当s5≠0时当s5=0时，机械手处于奇异形位，θ4与θ6与的计算过程与上一步一样。综上，由于θ1、θ3、θ5各有两个解，所以PUMA机器人的逆解共有8个。由于机器人结构的限制，有些解不能实现，需要根据实际工况进行优化选择。1.角速度传递7.4

速度传递矩阵

连杆i+1的角速度用坐标系{i+1}的角速度表示，等于坐标系{i}的角速度加上由于关节i+1的转动引起的角速度分量。参照坐标系{i}，连杆i+1的角速度可写成将上式两边同时左乘

，得到参照坐标系{i+1}的角速度进一步，可得：其中，2.线速度传递7.4

速度传递矩阵

连杆i+1的线速度用坐标系{i+1}原点的线速度表示，等于坐标系{i}原点的线速度加上由于连杆i+1的角速度引起的新的分量。参照坐标系{i}，连杆i+1的线速度可写成：将上式两边同时左乘

，得到参照坐标系{i+1}的角速度：这里，

表示坐标系{i+1}的原点在{i}的位移矢量，可以由

的平移矢量获得。

上式是转动关节的速度传递公式，对于移动关节，相应的关系为：1.不同空间轨迹规划对比7.5

轨迹规划

在关节空间进行规划时，是将关节变量表示成时间的函数，并规划它的一阶和二阶或高阶时间导数，主要规划任务是对关节变量的插值运算，常规的关节空间轨迹规划函数一般有多项式型、线性型、抛物线过渡型等。

2.关节空间轨迹规划7.5

轨迹规划

仅考虑起止点的三次多项式

三次多项式的关节函数θ(t)形式如下：

解方程，可得：三次多项式的关节函数位置、速度、加速度的时间函数如图所示：2.关节空间轨迹规划7.5

轨迹规划

具有中间点的三次多项式

假设工业机器人在由位置A到位置B的过程中经过了C、D等中间点，如果机器人在中间点做了停留，则仍可以用上述的轨迹规划方法，求出每两个点之间的关节规划函数。确定中间点期望速度的方法有以下几种：1）利用速度雅可比逆矩阵，将工具坐标系在直角坐标空间中的瞬时线速度和角速度转换为每个路径点的关节速度。2）在操作空间或关节空间中采用适当的启发式方法，由控制系统自动选择中间点的速度。3）采用使中间点处的加速度连续的方法，系统自动选取中间点速度。3.操作空间轨迹规划7.5

轨迹规划

操作空间规划的轨迹是机器人末端工具沿直角坐标空间运动的轨迹，除了前面介绍的直线轨迹以外，也可控制机器人在不同点之间沿其他类型的轨迹运动。实际上所有用于关节空间轨迹规划的方法都可用于操作空间的轨迹规划。对于关节空间轨迹规划，规划函数生成的值就是关节值，而直角坐标空间轨迹规划函数生成的值是机器人末端工具的位姿，它们需要通过求解逆运动方程才能化为关节量。3.操作空间轨迹规划7.5

轨迹规划

前述过程可以简化为如下的计算循环:(1)为机器人末端工具各个自由度选择合适的轨迹函数；(2)利用所选择的轨迹函数，按时间顺序计算出末端工具的位姿；(3)利用机器人逆运动方程计算与末端工具位姿对应的关节信息；(4)将关节信息传递给控制器；(5)重复步骤（2）到（4），直至轨迹执行完毕。在工业应用中，最实用的操作空间轨迹是点到点之间的直线运动，但也经常遇到多目标点(例如有中间点)间需要平滑过渡的情况。4.工业机器人仿真7.5

轨迹规划

现代仿真技术不仅能够实现单台机器人在特定应用场景下的几何路径规划、运动过程仿真、碰撞干涉检查、离线编程和动力学分析等，甚至能够模拟复杂的物理环境和多机器人协作。4.工业机器人仿真7.5

轨迹规划

1.机器人运动学求解项目实践7.6

运动学建模与轨迹规划项目实践

本实验拟采用matlab中Robotics

Toolbox机器人工具箱进行机器人逆运动学求解，这是一套功能强大的MATLAB工具箱，专门用于机器人学研究。本章的项目实践中，主要使用了工具箱中的Link()类函数和SerialLink()类函数，关于这两个类函数的使用方法，请参考官网的在线说明。1.机器人运动学求解项目实践7.6

运动学建模与轨迹规划项目实践

a.正运动学求解具体步骤(1)建立DH参数，确定关节移动范围；(2)在Link()函数中输入DH参数建立连杆，设置offset参数指定连杆初始偏置，每个连杆DH参数的输入顺序为：关节角，关节距离d，连杆长度a，连杆转角，连杆初始偏置offset，DH参数类型）；(3)根据机器人关节角移动范围，在Link()函数的qlim参数指定关节的转动极值；(4)通过SerialLink()函数连接各连杆建立机器人并指定其标题名称(5)通过SerialLink.fkine()函数进行机器人正运动学计算，得出机器人末端执行器位姿。1.机器人运动学求解项目实践7.6

运动学建模与轨迹规划项目实践

b.逆运动学求解具体步骤(1)前4个步骤参考正运动学求解步骤进行；(2)通过SerialLink.ikine()函数进行机器人逆运动学计算，得出机器人六个关节角。%%逆运动学matlab代码init_ang=robot0.ikine(T_1); %计算第一个位姿对应的关节角targ_ang=robot0.ikine(T_2); %计算第二个位姿对应的关节角1.机器人运动学求解项目实践7.6

运动学建模与轨迹规划项目实践

c.雅可比矩阵求解具体步骤1(1)前4个步骤参考正运动学求解步骤进行；(2)通过SerialLink.jacob0()函数求出某个位姿下关节速度到基坐标系运动速度的雅可比矩阵；(3)通过SerialLink.jacobn()函数求出某个位姿下关节速度到工具坐标系运动速度的雅可比矩阵。1.机器人运动学求解项目实践7.6

运动学建模与轨迹规划项目实践

c.雅可比矩阵求解具体步骤2%%雅可比矩阵求解matlab代码J_1=robot0.jacob0(T_1); %计算第一个位姿下相对于基坐标系的雅可比矩阵J_2=robot0.jacob0(T_2); %计算第二个位姿下相对于基坐标系的雅可比矩阵J_3=robot0.jacobn(T_1); %计算第一个位姿下相对于工具坐标系的雅可比矩阵J_4=robot0.jacobn(T_2); %计算第二个位姿下相对于工具坐标系的雅可比矩阵2.笛卡尔空间轨迹规划项目实践7.6

运动学建模与轨迹规划项目实践

笛卡尔空间轨迹规划是通过规划末端执行器的位姿来控制机器人运动，即保证机器人末端以确定的姿态在规定轨迹上运动。采用matlab中Robotics

Toolbox机器人工具箱进行机器人笛卡尔空间