ROBOTICS TEACHING PLAN-CH05(机器人学-机器人运动学反解)20100110

x = l1 cosθ 1 + l 2 cos(θ 1 + θ 2 ) y = l1 sin θ 1 + l 2 sin(θ 1 + θ 2 )
式中： 是机械手两个连杆的长度， 式中：l1，l2是机械手两个连杆的长度，(x,y)是机械 是机械 臂末端端点的位置矢量， 是关节矢量。 臂末端端点的位置矢量，(θ1,θ2)是关节矢量。 是关节矢量 运动学反解是给定位置(x,y)， 求出相应的关节矢 ， 运动学反解是给定位置 量(θ1,θ2)。 。
2011-3-6
(a)
(b)
(c) (d) 11PUMA型操作机的不同位形 图11-13 PUMA型操作机的不同位形
a-臂左，肘上；b-臂右，肘上；c-臂左，肘 臂左，肘上； 臂右，肘上； 臂左， 臂右， 下；d-臂右，肘下
华南农业大学工程学院
作业
本章
式 中 ： n,o,a 表 示 手 腕 的 方 位 ， p 代 表 腕 部 的 位 置 ， q=[q1,q2,…,qn]T是关节矢量，n是关节数。 是关节矢量， 是关节数 是关节数。
反解的存在性和唯一性
机器人运动学方程式一般是超越(非线性 方程组 机器人运动学方程式一般是超越 非线性)方程组。 非线性 方程组。 例如对于n=6的 6自由度操作臂 ， 运动学方程式中 的 自由度操作臂 自由度操作臂， 例如对于 个未知量q=[q1,q2,…,q6]T，而总共有 个方程。 而总共有12个方程 个方程。 有6个未知量 个未知量 其中[a,o,a]所代表的 个方程中有 个是独立的；位 所代表的9个方程中有 个是独立的； 其中 所代表的 个方程中有3个是独立的 置矢量p代表的 个方程是独立的，因此有6个独立 代表的3个方程是独立的 置矢量 代表的 个方程是独立的 ， 因此有 个独立 的方程和6个未知量 个未知量。 的方程和 个未知量。
为求解θ 为求解 2，在前面矩阵方程的两边左乘逆变换 0T3−1 ：
令上面矩阵方程两边的元素(1,4)和 (2,4)分别对应相等可 解得 和 分别对应相等可解得 令上面矩阵方程两边的元素 分别对应相等可 s23和c23 和
求θ2 (续1) 续
因为s 表达式的分母相等，且为正。于是： 因为 23和c23表达式的分母相等，且为正。于是： θ23=θ2+θ3 =atan2[–(a3+a2c3)px+(c1px+s1py)(a2s3–d4),(–d4+a2s3)pz +(c1px+s1py)(a2c3+a3)] 根据θ 解的四种可能组合， 可得四种可能值θ 根据 1 和 θ3 解的四种可能组合 ， 可得四种可能值 23 ， 于是得θ 于是得 2的四种可能解 θ2=θ23–θ3
Chapt.5
机器人运动学反解
张建瓴
反解的含义
由操作空间到关节空间的映射 称为反向运动学。 称为反向运动学。
§5.1 概述
一、运动学反解
具有n个自由度的机器人的运动学方程一般都可以写成： 具有 个自由度的机器人的运动学方程一般都可以写成： 个自由度的机器人的运动学方程一般都可以写成
nx n y n z 0
ox a x p x oy a y py 0 = T ( q ) ⋅ 1T ( q ) ⋯ 1 1 2 2 oz a z p z 0 0 1
n −1 n
T ( qn )
方程左边的矩阵表示末端连杆相对于基坐标系{0}的位姿。 方程左边的矩阵表示末端连杆相对于基坐标系 的位姿。 的位姿
一、三个相邻关节轴交于一点
1、最后三个关节轴相交于一点 、 此时，机器人的运动学方程可表示为： 此时，机器人的运动学方程可表示为：
0 6
T = 0T ⋅ 3T 3 6
式中： 规定三轴交点(称为腕部)的位置， 式中： 0T 规定三轴交点(称为腕部)的位置，而 3T 规 3 6 定手腕的方位。 定手腕的方位。
1、工作空间
工作空间是操作臂的末端抓手能够到达的空间范围， 工作空间是操作臂的末端抓手能够到达的空间范围，即抓 手能够到达的目标点的集合，一般分为两类： 手能够到达的目标点的集合，一般分为两类： （1）灵活（工作）空间 ）灵活（工作）
（2）可达（工作）空间 ）可达（工作） 指机器人抓手至少在一个方位上能够到达目标点的集合。 指机器人抓手至少在一个方位上能够到达目标点的集合。 显然，灵活空间是可达空间的子集。 显然，灵活空间是可达空间的子集。
1、最后三个关节轴相交于一点 、
求运动学反解的步骤分为两步： 求运动学反解的步骤分为两步： （1）由腕部位置求解 1、θ2和θ3； ）由腕部位置求解θ （2）由手腕的方位求解 4、θ5和θ6。 ）由手腕的方位求解θ
§5.4 PUMA560机器人的运动学反解 机器人的运动学反解
PUMA560机器人的运动方程为： 机器人的运动方程为： 机器人的运动方程为
由上矩阵方程两边元素(1,3)和(3,3)分别对应相等，可得： 和 分别对应相等， 由上矩阵方程两边元素 分别对应相等 可得：
由此得到θ5的封闭解： 的封闭解：
6、求θ6 、
将机器人运动学方程改写为： 将机器人运动学方程改写为：
令上式矩阵方程两边元素(3,1)和(1,1)分别对应相等，可得： 和 分别对应相等， 令上式矩阵方程两边元素 分别对应相等 可得：
§5.2 运动学反解的 存在性和唯一性
对于n个关节的操作臂，运动学方程可以写成： 对于 个关节的操作臂，运动学方程可以写成： 个关节的操作臂 n=n(q)=n(q1,q2,…,qn) o=o(q)=o(q1,q2,…,qn) a=a(q)=a(q1,q2,…,qn) p=p(q)=p(q1,q2,…,qn) 或 X=X(q)=X(q1,q2,…,qn) （6.2.1） ）
4、求θ4 、
由求解θ2的矩阵方程两边元素 的矩阵方程两边元素(1,3)和(3,3)分别对应相等， 和 分别对应相等， 两边元素 分别对应相等 可得： 可得：
只要s 只要 5≠0，便可求出 4 ，便可求出θ
5、求θ5 、
将运动学方程式两端左乘逆变换 0T4−1 (θ1 , θ 2 , θ 3 , θ 4 ) ，
2、求θ3 、
选定θ 的一个解之后， 选定 1的一个解之后，再令前面的矩阵方程两端的元素 (1,4)和(3,4)分别对应相等，得两个方程： 分别对应相等， 和 分别对应相等 得两个方程：
由上式可解ቤተ መጻሕፍቲ ባይዱθ 由上式可解得 3
式中： 式中：正、负号对应于θ3的两个可能解。 负号对应于 的两个可能解。
3、求θ2 、
对于6个自由度的机器人而言，运动学反解非常复杂， 对于 个自由度的机器人而言，运动学反解非常复杂， 个自由度的机器人而言 一般都没有封闭解。 但在某些特殊情况下， 一般都没有封闭解 。 但在某些特殊情况下 ， 可以得 到封闭解。 到封闭解。 如果满足下列条件之一，就可以得到封闭解。 如果满足下列条件之一，就可以得到封闭解。 （1）三个相邻关节轴交于一点； ）三个相邻关节轴交于一点； （2）三个相邻关节轴相互平行。 ）三个相邻关节轴相互平行。
二、反解的唯一性和最优解
在求操作手的运动学反解时， 碰到的一个问题 在求操作手的运动学反解时 ， 是反解不唯一，称为多重解。 是反解不唯一，称为多重解。 机器人操作臂运动学反解的数目取决于关节数 目和连杆数目，以及关节变量的活动范围。 目和连杆数目，以及关节变量的活动范围。
三、运动学反解的求解方法
若末端连杆的位姿已经给定， 为已知， 若末端连杆的位姿已经给定 ， 即 n， o， a和 p为已知 ， 则 ， ， 和 为已知 求关节变量θ 的值称为运动反解。 求关节变量 1,θ2,…,θ6的值称为运动反解。 具体求反解步骤如下： 具体求反解步骤如下：
运动方程的求解
开始求解关节位置。 从 T6 开始求解关节位置 。 使 T6 的符号表达式的各元 素等于T 的一般形式，并据此确定θ 并且有： 素等于 6的一般形式，并据此确定 1角。并且有： 式中： 左边为θ 各元的函数。 式中 ： 左边为 1 和 T6 各元的函数 。 此式可用来求解其他 各关节变量，如θ2等。 各关节变量， 不断地用A的逆矩阵左乘上式，得下列四个矩阵方程式： 不断地用 的逆矩阵左乘上式，得下列四个矩阵方程式： 的逆矩阵左乘上式
2、工作空间和反解存在性 、
如果腕系{W}的位姿处于工作空间内 ， 则 的位姿处于工作空间内， 如果腕系 的位姿处于工作空间内 运动学反解是存在的，否则反解不存在。 运动学反解是存在的，否则反解不存在。
[例5-1] 2R机械手的运动学反解 例 机械手的运动学反解
2R机械手的运动学方程为 机械手的运动学方程为
从而可求出θ 的封闭解： 从而可求出 6的封闭解：
关节角(θ)的多解问题 关节角 的多解问题
求位姿逆解时， 求位姿逆解时 ， 关节角的 解都是多解( 多值) 解都是多解 ( 多值 ) 的 。 如 用几何法 ， 这种多值可以 方便地由解图直接判定。 方便地由解图直接判定。 为达到目标点， 如图 ， 为达到目标点 ， 操 作 机 的 上 臂 ( 杆 2) 和 下 臂 ( 杆 3) 可有两种位形关系。 可有两种位形关系。 共有4种不同的位形。而 PUMA机器人共有 种反解。 机器人共有8 PUMA机器人共有8种反解。
1、求θ1 、
可用逆变换 0T1−1 (θ1 ) 左乘运动学方程两边
令上面矩阵方程两端的元素(2,4)对应相等，可得： 对应相等，可得： 令上面矩阵方程两端的元素 对应相等
求θ1 (续1) 续
做代换， 的解： 做代换，可得到θ1的解：
式中正、负号对应于 的两个可能解。 式中正、负号对应于θ1的两个可能解。
机器人运动学反解的求解方法分为两类： 机器人运动学反解的求解方法分为两类 ： 封闭解法 和数值解法。 和数值解法。 1、封闭解法 操作臂的运动学封闭解可以通过两种途径获得： 操作臂的运动学封闭解可以通过两种途径获得 ： 代 数法和几何解。 数法和几何解。 （1）代数解法 （2）几何解法