机器人学—数学基础.pdf

机器人运动学及其数学基础 参考教材 美 付京逊 机器人学 中南大学 蔡自兴 机器人学 美 理查德 鲍尔 机器人操作手 数学 编程 与控制 参考教材 中南大学 蔡自兴 中南大学教授 我国人工 智能和机器人领域著名专 家 中南大学教授 我国人工 智能和机器人领域著名专 家 中国人工智能学会智能机 器人专委会理事长 中国人工智能学会智能机 器人专委会理事长 曾与付京逊教授一起工作 过 曾与付京逊教授一起工作 过 一 机器人位置和姿态的描述 Justin catch ball 串联机器人可以用一个开环关节链来建模 由数个驱动器驱动的转动或移动关节串联而成 一端固定在基座上 另一端是自由的 安装工具 末端 执行器 用以操纵物体 或完成各种任务 运动学问题 B H坐标系 位置 H的原点在B坐标 系中的坐标表示 姿态 H坐标系相对于B 坐标系的姿态 i n o a 关节变量末端执行器位置和姿态 机器人坐标系的定义 Global Reference coordinate system frame Joint Reference coordinate system frame Tool Reference coordinate system frame 运动学研究的两个问题 Where is my hand Direct Kinematics HERE How do I put my hand here Inverse Kinematics Choose these angles 运动学正问题运动学正问题 运动学逆问题运动学逆问题 运动学正问题运动学正问题 运动学逆问题运动学逆问题 轴线平行及相交 研究运动学的方法研究运动学的方法 轴线异面 轴线平行时采用几何分析方法 1955年丹纳维特 Denavit 和哈顿伯格 Hartenberg 提出了一种采用矩阵代数方法解决 机器人的运动学问题 D H方法 具有直观的几何意义 能表达动力学 计算机视觉和比例变换问题 研究运动学的方法研究运动学的方法 数学基础是齐次变换 二数学基础 齐次坐标和齐次变换 2 1 点和面的齐次坐标 2 1 1 点的齐次坐标 用n 1个变量表示n维空间的几何元素 引入齐次坐标的目的是为了表示几何变换的旋转 平移和缩放 显然 齐次坐标表达并不是唯一的 随w值的不 同而不同 在计算机图学中 w 作为通用比例因子 它可取任意正值 但在机器人的运动分析中 总 是取w 1 kc j b i av 一个点矢 一个点矢 T x y z Vw 列矩阵列矩阵 式中式中i j k为为x y z 轴上的单位矢量 轴上的单位矢量 a b c w为比例系数为比例系数 w x w y w z 例1 kjiV 543 可以表示为 V 3 4 5 1 T 或V 6 8 10 2 T 或V 12 16 20 4 T 可以表示为 V 3 4 5 1 T 或V 6 8 10 2 T 或V 12 16 20 4 T 齐次坐标与三维直角坐标的区别 V点在 OXYZ坐标系中表示 是唯一的 a b c 而在齐次坐标中表示可以 是多值的 不同的表示方 法代表的V点在空间位置 上不变 几个特定意义的齐次坐标 0 0 0 n T 坐标原点矢量的齐次坐标 n为任意 非零比例系数 1 0 0 0 T 指向无穷远处的OX轴 0 1 0 0 T 指向无穷远处的OY轴 0 0 1 0 T 指向无穷远处的OZ轴 0 0 0 0 T 没有意义 2个常用的公式 个常用的公式 zzyyxx babababa kbabajbabaibaba bbb aaa kji ba xyyxzxxzyzzy zyx zyx 点乘点乘 叉乘叉乘 2 1 2 平面的齐次坐标 平面齐次坐标由行矩阵P a b c d 来表示 当点v x y z w T处于平面P内时 矩阵乘积PV 0 或记为 0 dwczbyax w z y x dcbaPV 如果定义一个常数 m 则有 222 cba xyzabcx ay bz cd ijkijk wwwmmmw mw mw mm 可以把矢量解释为某个平面的外法线 此 平面沿着法线方向与坐标原点的距离为 d m k m c j m b i m a 点和平面间的位置关系 设一个平行于设一个平行于x y轴 且在轴 且在z轴上的坐标为单位距离的平 面 轴上的坐标为单位距离的平 面P可以表示为 或 有 可以表示为 或 有 PV 1100 P 2200 P v0 v0 v0 点在平面下方 点在平面上 点在平面上方 与点矢相仿 平面也没有意义与点矢相仿 平面也没有意义 T 0000 0000 例如 点例如 点 V 10 20 1 1 T必定处于此平面内 而点必定处于此平面内 而点 V 0 0 2 1 T 处于平处于平 P的上方 点的上方 点V 0 0 0 1 T处于处于P平面下方 因为 平面下方 因为 10 20 0010100 1 1 0 1 1 2 0 0 1100 0 1 1 0 0 0 1 100 2 1 3 平移变换平移变换 1 二维坐标平移变换 二维坐标平移变换 11 1 1 by ax y x a b y x y1 x1 P x1 y1 o o1 沿坐标轴平移 沿坐标轴平移 a b P位于位于 O1坐标系中 坐标系中 O为绝对坐标系为绝对坐标系 人 人 P 坐在汽车里运动 坐在汽车里运动 11 11 10 01 110011 xxax yTyby 2 三维坐标平移变换 三维坐标平移变换 11000 100 010 001 11 1 1 1 1 1 1 z y x c b a z y x T z y x z y x o o w u v a b c w v u o 沿坐标轴方向平移 a b c 沿坐标轴方向平移 a b c 车绕盘山公路行驶车绕盘山公路行驶 平移齐次变换矩阵平移齐次变换矩阵 100 010 TTrans a b c 001 0001 a b c 对任意向量u x y z w 进行T变换后为 对任意向量u x y z w 进行T变换后为 100 010 001 00011 axxawx wa byybwy wb VTu czzcwz wc ww 对已知任意向量u x y z w 进行T变换对已知任意向量u x y z w 进行T变换实质实质是将u 与平移向量 a b c 1 相加 是将u 与平移向量 a b c 1 相加 2 2 旋转矩阵及旋转齐次变换 2 2 1 旋转矩阵 设固定参考坐标系直角坐标为 旋转矩阵及旋转齐次变换 2 2 1 旋转矩阵 设固定参考坐标系直角坐标为 Oxyz 动坐标系为 动坐标系为 O uvw 研究旋转变换情况 研究旋转变换情况 x y z w v u P o O 初始位置时 动静坐标系重合 初始位置时 动静坐标系重合 O O 重合 如图 各轴 对应重合 设 重合 如图 各轴 对应重合 设P点是动坐标系点是动坐标系 O uvw中的一点 且固定不变 则 中的一点 且固定不变 则P点在点在 O uvw中可表示为 中可表示为 wwvvuuuvw kPjPiPP 为坐标系 为坐标系 O uvw的单位矢 量 则 的单位矢 量 则P点在点在 oxyz中可表示为 中可表示为 u i v j w k zzyyxxxyz kPjPiPP xyzuvw PP 例 动坐标系中一点P绕坐标轴Z旋转 cossin ABB ppp xxy sincos ABB ppp yxy AB pp zz 2 2 2 三个基本旋转矩阵和合成旋转矩阵 写成矩阵形式 0 0 001 AB pp AB pp AB pp xcsx yscy zz A B Z 图中动坐标系换成uvw cos B p x sin B p y B p xB p y A p x A p y cos B p y sin B p x 0 0 001 zw cs kksc 方向余弦阵方向余弦阵 由图2 5可知 在x轴上的投影为 在y轴上的投影为 在x轴上 的投影为 在y轴上的投影为 由图2 5可知 在x轴上的投影为 在y轴上的投影为 在x轴上 的投影为 在y轴上的投影为 u i cos u i sin v j v j u isin u i v j cos v j 绕Z轴的基本旋转矩阵 对对x轴投影轴投影 对对y轴投影轴投影 对对z轴投影轴投影 u在在x y z轴的投影 由图2 5可知 在x轴上的投影为 在x轴上的投影为 在y轴上 的投影为 在y轴上的投影为 轴的投影 由图2 5可知 在x轴上的投影为 在x轴上的投影为 在y轴上 的投影为 在y轴上的投影为 u i cos u i sin v j v j v j sin u i u i cos v j 同理 同理 cos0sin 010 sin0cos y R 100 0cossin 0sin cos z R ssin0 sincos0 001 R x co 三个基本旋转矩阵 三个基本旋转矩阵 x y z o u v w U W O x y z o u v w U V W O 总结总结 三个基本旋转矩阵三个基本旋转矩阵 R z 即动坐标系求的旋转矩阵 也就是 求出坐标系中各轴单位矢量在固定坐标系 中各轴的投影分量 很容易得到在两个坐标系重合时 有 即动坐标系求的旋转矩阵 也就是 求出坐标系中各轴单位矢量在固定坐标系 中各轴的投影分量 很容易得到在两个坐标系重合时 有 OvwOZ 绕轴转动 角 vwO wv kji Oxyz R z 100 010 001 R 当动坐标系 当动坐标系 O uvw绕任意过原点的单位矢量绕任意过原点的单位矢量 f 回转回转 时 求 时 求P点在固定坐标系点在固定坐标系 oxyz中的位置中的位置 y z x o O u v w P Pw Pv Pu 图 已知 已知 P点在点在 O uvw中是不变的仍然 成立 由于 中是不变的仍然 成立 由于 O uvw回转 则 回转 则 wwvvuuuvw kPjPiPP xwwvvuuxuvwx fkPjPiPfP P ywwvvuuyuvwy fkPjPiPfP P zwwvvuuzuvwz fkPjPiPfP P 用矩阵表示为用矩阵表示为 y xxvxw x yyyvwv zw zzvzw f ifjf k PP Pf ifjf kP PP f ifjf k 2 7 R xyzuvw Rot fpRP 其中vers 1 cos 定义 旋转矩阵为 则 2 2 2 旋转齐次变换 用齐次坐标变换来表示式 用齐次坐标变换来表示式 2 7 11000 0 0 0 1 w v u z y x P P P R P P P 11000 0 0 0 1 1 z y x w v u P P P R P P P 0 0 0 0001 xxyxzzxy xyzyyzyx xzyyzxzz f f verscf f versf sf f versf s f f versf sf f verscf f versf s Rot f f versf sf f versf sf f versc f 以绕Z轴的基本旋转矩阵为例验证 0 0 001 zw cs kksc 0 0 0 0001 xxyxzzxy xyzyyzyx xzyyzxzz f f verscf f versf sf f versf s f f versf sf f verscf f versf s Rot f f versf sf f versf sf f versc f 合成旋转矩阵 例 合成旋转矩阵 例1 在动坐标中有一固定点 相对固定参 考坐标系做如下运动 在动坐标中有一固定点 相对固定参 考坐标系做如下运动 R x 90 R z 90 R y 90 求运动后点在固定参考坐标系 下的位置 求运动后点在固定参考坐标系 下的位置 T uvwPo1321 Oxyz uvwPo Oxyz 解解1 用画图的简单方法 用画图的简单方法 3 1 2 1 1 3 2 1 2 1 3 1 解解2 用分步计算的方法 用分步计算的方法 matlab 编程编程 R x 90 R z 90 R y 90 1 2 3 1 1 3 2 1 1000 0010 01 00 0001 P 1 2 1 3 1 2 3 1 1000 0100 0001 001 0 P 1 3 1 2 1 2 1 3 1000 0001 0010 0100 P 2 14 2 15 2 16 上述计算方法非常繁琐 可以通过一系列计算得到上述 结果 将式 上述计算方法非常繁琐 可以通过一系列计算得到上述 结果 将式 2 14 2 15 2 16 联写为如下形式 联写为如下形式 3 3 0 0 0 100011 xu yv zw PP PRP PP R4x4为二者之间的关系矩阵 我们令 为二者之间的关系矩阵 我们令 3 3 RR yR zR x 定义定义1 当动坐标系绕固定坐标系各坐标轴顺序有限次 转动时 其合成旋转矩阵为各基本旋转矩阵依旋转顺序 当动坐标系绕固定坐标系各坐标轴顺序有限次 转动时 其合成旋转矩阵为各基本旋转矩阵依旋转顺序左乘左乘 注意 旋转矩阵间不可以交换 平移矩阵间可以交换注意 旋转矩阵间不可以交换 平移矩阵间可以交换 uvwO Oxyz 2 2 4 相对变换 举例说明 例 举例说明 例1 动坐标系 动坐标系 0 起始位置与固定参考坐标系起始位置与固定参考坐标系 0重合重合 动坐标系动坐标系 0 做如下运动 做如下运动 R Z 90 R Y 90 Trans 4 3 7 求合成矩阵 解 求合成矩阵 解1 用画图的方法 用画图的方法 o z y x 7 4 3 o w u v v u w z y x o o o x y z u v w z y x u w o o v 对照此图写出合成变换矩阵对照此图写出合成变换矩阵 解解2 用计算的方法 根据定义 用计算的方法 根据定义1 我们有 我们有 TTrans 4 3 7 R Y 90 R Z 90 start 0014 1003 0107 0001 以上均以固定坐标系各轴为变换基准 因此矩阵左乘 如果我们做如下变换 例 以上均以固定坐标系各轴为变换基准 因此矩阵左乘 如果我们做如下变换 例2 先平移 先平移Trans 4 3 7 绕当前轴转动 绕当前轴转动90 绕当前轴转动 绕当前轴转动90 求合成旋转矩阵 求合成旋转矩阵 v w 2 20 2 20 解解1 用画图的方法 用画图的方法 z y x o o v w u z y x o o w u v o z y x o w v u x y z o o w u v 解解2 用计算的方法 用计算的方法 o 0014 1003 T Trans 4 3 7 R v 90 R w 90 0107 0001 o start 2 21 对照此图写出合成变换矩阵 2 21 对照此图写出合成变换矩阵 0014 1003 TTrans 4 3 7 R Y 90 R Z 90 start 220 0107 0001 o 0014 1003 T Trans 4 3 7 R v 90 R w 90 0107 0001 221 o start R Z 90 R Y 90 Trans 4 3 7 求合成矩阵 求合成矩阵 o z y x 7 4 3 o w u v v u w z y x o o o x y z u v w z y x u w o o v TTrans 4 3 7 R Y 90 R Z 90 start 先平移 先平移Trans 4 3 7 绕当前轴转动 绕当前轴转动90 绕当前轴转动 绕当前轴转动90 v w z y x o o v w u z y x o o w u v o z y x o w v u x y z o o w u v 式 2 20 和式 2 21 无论在形式上 还是在结果上都是 一致的 因此我们有如下的结论 动坐标系在固定坐标系中的齐次变换有2种情况 定义1 如果所有的变换都是相对于 式 2 20 和式 2 21 无论在形式上 还是在结果上都是 一致的 因此我们有如下的结论 动坐标系在固定坐标系中的齐次变换有2种情况 定义1 如果所有的变换都是相对于固定坐标系固定坐标系中各坐标轴旋 转或平移 则依次 中各坐标轴旋 转或平移 则依次左乘 称为绝对变换左乘 称为绝对变换 结果均为动坐标系在固定坐标中的位姿 位置 姿态 相对于固定坐标系 结果均为动坐标系在固定坐标中的位姿 位置 姿态 相对于固定坐标系 轴 轴相当于轴 轴相对于轴 轴相当于ZYXwv 也就是说 动坐标系绕自身坐标轴做齐次变换 要达到绕固 定坐标系相等的结果 就应该用相反的顺序 也就是说 动坐标系绕自身坐标轴做齐次变换 要达到绕固 定坐标系相等的结果 就应该用相反的顺序 定义2 如果动坐标系相对于定义2 如果动坐标系相对于自身坐标系自身坐标系的当前坐标轴旋转或 平移 则齐次变换为依次 的当前坐标轴旋转或 平移 则齐次变换为依次右乘 称为相对变换右乘 称为相对变换 右乘的意义 机器人用到相对变换的 时候比较多 例如机械手抓一个杯子 如右图所示 手爪需要 转动一个角度才抓的牢 相对于固定坐标系表达 太麻烦 可以直接根据 手爪的坐标系表示 x y z o H 2 2 5 齐次变换矩阵的几何意义 设 有一个手爪 即动坐标系 O 已知 初始位置 重合 那么 O 在 O中的齐次坐标变换为 如果手爪转了一个角度 则 设 有一个手爪 即动坐标系 O 已知 初始位置 重合 那么 O 在 O中的齐次坐标变换为 如果手爪转了一个角度 则 111 cbao 1000 100 010 001 T 1 1 1 1 c b a 1000 p p p T z yyy xxx zzz y x w w w T反映了 O 在 O中的位置和姿态 即表示了该坐标系原 点和各坐标轴单位矢量在固定坐标系中的位置和姿态 该矩阵可以由4个子矩阵组成 写成如下形式 反映了 O 在 O中的位置和姿态 即表示了该坐标系原 点和各坐标轴单位矢量在固定坐标系中的位置和姿态 该矩阵可以由4个子矩阵组成 写成如下形式 比例系数透视矩阵 位置矢量旋转矩阵 1131 1333 wf PR T zzz yyy xxx w w w 33 R 为姿态矩阵 旋转矩阵 表示动坐标系 O 在固定参考坐标系 O中的姿态 即表示 O 各坐标轴单位矢量在 O各轴上的投影 为位置矢量矩阵 代表动坐标系 O 坐标 原点在固定参考坐标系 O中的位置 为姿态矩阵 旋转矩阵 表示动坐标系 O 在固定参考坐标系 O中的姿态 即表示 O 各坐标轴单位矢量在 O各轴上的投影 为位置矢量矩阵 代表动坐标系 O 坐标 原点在固定参考坐标系 O中的位置 T zyx ppp P 13 为透视变换矩阵 在视觉中进行图像计算 一般置为0 为透视变换矩阵 在视觉中进行图像计算 一般置为0 000 f 31 为比例系数为比例系数 1 11 w 如果需要求解 O在 O 中的位置和姿态 此时的齐次变换矩 阵为 即求逆矩阵 如果需要求解 O在 O 中的位置和姿态 此时的齐次变换矩 阵为 即求逆矩阵 1 T 1000 R T T T1 33 T 1 pw pv p kpjpipp zyx kji zyx kvjvivv zyx kwjwiww zyx 其中 这些式子以后经常遇到 在机器人计算中 所要 求的就是齐次变换矩阵 其中 这些式子以后经常遇到 在机器人计算中 所要 求的就是齐次变换矩阵 知识点 1 点和面的齐次坐标和齐次变换 2 三个基本旋转矩阵 3 绝对变换 如果所有的变换都是相对于固定坐标 系中各坐标轴旋转或平移 则依次左乘 称为绝 对变换 4 相对变换 如果动坐标系相对于自身坐标系的当 前坐标轴旋转或平移 则齐次变换为依次右乘 称为相对变换 知识点 三个基本旋转 矩阵 cos0sin 010 sin0cos y R 100 0cossin 0sin cos z R ssin0 sincos0 001 R x co 例题1 O 与 O初始重合 O 作如下运动 绕Z轴转动30 例题1 O 与 O初始重合 O 作如下运动 绕Z轴转动30 绕X轴转动60 绕X轴转动60 绕Y轴转动90 绕Y轴转动90 求T 求T z cos30sin3000 sin30cos3000 R 0010 0001 1000 0cos60sin600 0sin60cos600 0001 x R cos900sin900 0100 sin900cos900 0001 y R 3 43 41 20 1 43 43 20 3 21 200 0001 yxz TR R R yxz TR R Rstart 改为相对变换 旋转顺序 v uw TstartR R R yxz TR R Rstart 例题2 O 与 O初始重合 O 作如下运动 绕X轴转动90 例题2 O 与 O初始重合 O 作如下运动 绕X轴转动90 绕 w 轴转动90 绕 w 轴转动90 绕Y轴转动90 绕Y轴转动90 求 T 改变旋转顺序 如何旋转才能获得相同的结果 求 T 改变旋转顺序 如何旋转才能获得相同的结果 x 1000 0cos90 sin900 R 0sin90cos900 0001 cos90sin9000 sin90cos9000 0010 0001 w R cos900sin900 0100 sin900cos900 0001 y R