机器人的空间描述与坐标变换

1 第二章 机器人的空间描述和坐标变换 2.1 位姿和坐标系描述 2.2平移和旋转坐标系映射 2.3平移和旋转齐次坐标变换 2.4物体的变换和变换方程 2.5通用旋转变换 2 zyxA pppp 2-1图 位 置 表 示2.1位 置 方 位 表 示 与 坐 标 系 描 述 n1.位 置 描 述 矢 量 Ap 表 示 箭 头 指 向 点 的 位 置 矢 量 ， 其中 右 上 角 标 “A”表 示 该 点 是 用 A坐 标 系 描 述的 。 （ 2-2） n2.方 位 描 述 坐 标 系 B与 机 械 手 末 端 工 具 固 连 ， 工 具 的 姿 态可 以 由 坐 标 系 B的 方 向 来 描 述 。 而 坐 标 系 B的 方向 可 以 用 沿 三 个 坐 标 轴 的 单 位 矢 量 来 表 示 333231 232221 131211 rrr rrr rrrR BABABAAB ZYX 图 2-2方 位 表示 （ 2-1） 旋 转 矩 阵 描 述 坐 标 系 B的 姿 态 ， 矢 量 描 述 坐 标 系 B的 原 点 位置 。 3 BoAAB RB p3.位 姿 描 述 固 连 坐 标 系 把 刚 体 位 姿 描 述 问 题 转 化 为 坐 标 系 的 描 述 问 题 。 图 2-3中 坐 标 系 B可 以 在 固 定 坐 标 系 A中 描 述 为 （ 2-3）RAB BoAP 4 1.平 移 坐 标 变 换 图 2-3平 移 变 换 BP为 坐 标 系 B描 述 的 某 一 空 间 位置 ， 我 们 也 可 以 用 AP（ 坐 标 系 A） 描述 同 一 空 间 位 置 。 因 为 两 个 坐 标 系 具 有相 同 的 姿 态 ， 同 一 个 点 在 不 同 坐 标 系 下的 描 述 满 足 以 下 关 系 A B A Bo P P P （ 2-4） 2.2平 移 和 旋 转 坐 标 系 映射 旋 转 坐 标 变 换 的 任 务 是 已 知 坐 标 系 B描 述 的一 个 点 的 位 置 矢 量 BP和 旋 转 矩 阵 ， 求 在 坐 标系 A下 描 述 同 一 个 点 的 位 置 矢 量 AP。 5 2.旋 转 坐 标 变换 RABA B T Bx AA B T By A A B T Bz Appp X PY PZ P （ 2 -5 ）将 （ 2 -5 ） 式 写 成 矩 阵 形 式 得 ： PPZYXP BABBTAB TAB TABA R （ 2 -6 ） 图 2-4旋 转 变 换 式 （ 2-6） 即 为 我 们 要 求 的 旋 转 变 换 关 系 ， 该 变 换 是 通 过 两 个 坐标 系 之 间 的 旋 转 变 换 实 现 的 。 6 3.复 合 变 换 图 2-5复 合 变 换 如 果 两 个 坐 标 系 之 间 即 存 在 平 移又 存 在 旋 转 ， 如 何 计 算 同 一 个 空 间 点在 两 个 坐 标 系 下 描 述 的 变 换 关 系 ？ 为 了 得 到 位 置 矢 量 BP和 AP之间 的 变 换 关 系 ， 我 们 建 立 一 个 中间 坐 标 系 C。 PPP BABBCBC RR A C A A B ACo B BoR P P P P P （ 2-7）（ 2-8）为 了 得 到 位 置 矢 量 BP和 AP之 间 的 变 换 关 系 ， 只 需 坐 标 系 B 在 坐 标 系下 A的 描 述 。 是 4 4 矩 阵 ， 称 为 齐 次 坐 标 变 换 矩 阵 。 可 以 理 解 为 坐 标 系 B在 固 定 坐标 系 A中 的 描 述 。 7 2.3齐 次 坐 标 变 换 坐 标 变 换 （ 2-8） 可 以 写 成 以 下 形 式 1101 PPP BBoAABA R （ 2-9） 将 位 置 矢 量 用 41矢 量 表 示 ， 增 加 1维 的 数 值 恒 为 1， 我 们 仍 然 用 原来 的 符 号 表 示 4维 位 置 矢 量 并 采 用 以 下 符 号 表 示 坐 标 变 换 矩 阵 10 BoAABAB RT P （ 2-10）PP BABA T （ 2-11）TAB 齐 次 坐 标 变 换 的 主 要 作 用 是 表 达 简 洁 ， 同 时 在 表 示 多 个 坐 标 变 换的 时 候 比 较 方 便 。1.齐 次 变 换 8 2.齐 次 变 换 算 子 在 机 器 人 学 中 还 经 常 用 到 下 面 的 变 换 ， 如 图 2-8， 矢 量 AP1沿 矢 量AQ平 移 至 的 AQ终 点 ， 得 一 矢 量 AP2。 已 知 AP1和 AQ求 AP2的 过 程 称 之 为平 移 变 换 ， 与 前 面 不 同 ， 这 里 只 涉 及 单 一 坐 标 系 。 图 2-6平 移 算 子QPP AAA 12 （ 2-12）可 以 采 用 齐 次 变 换 矩 阵 表 示 平 移 变 换12 )( PQP AAA Trans （ 2-13）)( QATrans 称 为 平 移 算 子 ， 其 表 达 式 为 1)( 0 QQ AA ITrans （ 2-14） 其 中 I是 33单 位 矩 阵 。 例 如 若 AQ=ai+bj+ck，其 中 i、 j和 k分 别 表 示 坐 标 系 A三 个 坐 标 轴 的单 位 矢 量 ， 则 平 移 算 子 表 示 为 1000 100 010 001),( cbacbaTrans 9 同 样 ， 我 们 可 以 研 究 矢 量 在 同 一 坐 标 系 下 的 旋 转变 换 ， 如 图 2 -9 ， AP1绕 Z轴 转 角 得 到 AP2。 则 图 2-7旋 转 算 子12 ),( PP AA zRot （ 2 -2 0 ）Rot(z,)称 为 旋 转 算 子 ， 其 表 达 式 为 1000 0100 00 00),( cs sczRot （ 2 -2 1 ）同 理 ， 可 以 得 到 绕 X轴 和 Y轴 的 旋 转 算 子 1000 00 00 0001),( cs scxRot 1000 00 0010 00),( cs scyRot 10 定 义 了 平 移 算 子 和 旋 转 算 子 以 后 ， 可 以 将 它 们 复 合 实 现 复 杂 的 映 射关 系 。 变 换 算 子 与 前 面 介 绍 的 坐 标 变 换 矩 阵 形 式 完 全 相 同 ， 因 为 所 有 描述 均 在 同 一 坐 标 系 下 ， 所 以 不 需 上 下 标 描 述 （ 坐 标 系 ） 。2 1A AP T P （ 2-23）TAB AB RBoAP PP BABA T2 1A AP T P齐 次 坐 标 变 换 总 结 ： 表 示 坐 标 系 B在 坐 标 系 A下 的 描 述 ， 的 各 列 是 坐 标 系B三 个 坐 标 轴 方 向 的 单 位 矢 量 ， 而 表 示 坐 标 系 B原 点 位 置。2. 它 是 不 同 坐 标 系 间 的 坐 标 变 换 。 如3.它 是 同 一 坐 标 系 内 的 变 换 算 子 。 齐 次 坐 标 变 换 是 复 杂 空 间 变 换 的 基 础 ， 必 须 认 真 理 解 和 掌 握 。 具 体 应用 的 关 键 是 理 解 它 代 表 的 是 上 面 三 种 含 义 的 哪 一 种 ， 而 不 是 简 单 的 套 用公 式 ！1. 它 是 坐 标 系 的 描 述 。 如 图 2-10表 示 的 三 个 坐 标 系 ， 已 知 坐 标 系A、 B和 C之 间 的 变 换 矩 阵 和 位 置矢 量 CP， 求 在 坐 标 系 A下 表 示 同 一 个 点 的位 置 矢 量 AP。 11 3.复 合 变 换 复 合 变 换 主 要 有 两 种 应 用 形 式 ， 一 种 是 建 立 了 多 个 坐 标 系 描 述 机 器 人的 位 姿 ， 任 务 是 确 定 不 同 坐 标 系 下 对 同 一 个 量 描 述 之 间 的 关 系 ； 另 一 种 是一 个 空 间 点 在 同 一 个 坐 标 系 内 顺 序 经 过 多 次 平 移 或 旋 转 变 换 ， 任 务 是 确 定多 次 变 换 后 点 的 位 置 。 图 2-10 复 合 坐 标 变 换 TAB TBCPP CBCB T PPP CBCABBABA TTT （ 2-24）（ 2-25）TTT BCABAC 根 据 坐 标 变 换 的 定 义 得 （ 2-26） 12 (a) ZY顺 序 旋 转 (b) Y Z顺 序 旋 转图 2-11旋 转 顺 序 对变 换 结 果 影 响例 2 -3 已 知 点 u=7i+3j+2k， 先 对 它 进 行 绕 Z轴 旋 转 9 0 o的 变 换 得 点 v， 再 对 点 v进 行 绕 Y轴 旋 转 9 0 o的 变 换 得点 w， 求 v和 w。 127312371000 0100 0001 0010)90,( uv ozRot 137212731000 0001 0010 0100)90,( vw oyRot如 果 只 关 心 最 后 的 变 换 结 果 ， 可 以 按 下 式 计 算( ,90 ) ( ,90 ) ( ,90 )o o oRot y Rot y Rot z w v u 0 0 1 0 7 21 0 0 0 3 70 1 0 0 2 30 0 0 1 1 1 计 算 结 果 与 前 面 的 相 同 ， 称 R= Rot(y,90o) Rot(z,90o) 为 复 合 旋 转 算 子 。 13注 ： 固 定 坐 标 系 变 换 ， 矩 阵 乘 的 顺 序 “ 自 右 向 左 ” 如 果 改 变 旋 转 顺 序 ， 先 对 它 进 行 绕 y轴 旋 转 90o， 再 绕 z轴 旋 转 90o， 结果 如 图 2-11b所 示 。 比 较 图 2-11a和 图 2-11b可 以 发 现 最 后 的 结 果 并 不 相 同 ，即 旋 转 顺 序 影 响 变 换 结 果 。从 数 学 角 度 解 释 就 是 矩 阵 乘 法 不 满 足 交 换 率 ， Rot(y,90o) Rot(z,90o) Rot(z,90o) Rot(y,90o)。 和 ， 求 和给 定 计 算 142.4物 体 的 变 换 和 变 换 方 程TABTBA已 知 坐 标 系 B相 对 坐 标 系 A的 描 述求 坐 标 系 A相 对 坐 标 系 B的 描 述一 种 直 接 的 方 法 是 矩 阵 求 逆 ， 另 一 种 方 法 是 根 据 变 换 矩 阵 的 特 点 直接 得 出 逆 变 换 。 后 一 种 方 法 更 简 单 方 便 。 即 齐 次 变 换 的 求 逆 问 题 。TAB TBA 等 价 为 ： 已 知 RAB BoAP RBA AoBP 是 坐 标 系 B的 原 点 在 坐 标 系 B中 的 描 述 ， 显 然 为 零 矢 量 。由 （ 2 -2 8 ） 式 得 15根 据 前 面 的 讨 论 ， 旋 转 矩 阵 关 系 为TABABBA RRR 1 （ 2-27）将 坐 标 变 换 用 于 坐 标 系 B的 原 点 得AoBBoABABoB R PPP （ 2-28）BoBP B B A A T AAo A Bo B BoR R P P P （ 2-29）逆 变 换 可 以 直 接 用 正 变 换 的 旋 转 矩 阵 和 平 移 矩 阵 表 示 10 BoATABTABBA RRT P （ 2-30） 16 A沿 xA平 移 3个 单 位 ， 再 绕 新 的 zA 轴 转 180o得 B180 180 0 1 0 0180 180 0 0 1 00 0 1 0 0 1AB c sR s c 因 此 1 0 0 30 1 0 00 0 1 00 0 0 1ABT B沿 z B平 移 2个 单 位 ， 然 后 绕 yB轴 转 90o再 绕 新 xB轴 转 150o得 C 312 23 31 12 2 2 2312 290 0 90 1 0 0 0 0 1 1 0 0 00 1 0 0 150 150 0 1 0 0 090 0 90 0 150 150 1 0 0 0 1 0 0BC c sR c ss c s c 图 2-12楔 形 块 角 点 坐 标系 例 2-4， 如 图 2-12给 出 的 楔 形 块 角 点 坐 标 系 ， 求 齐 次 坐 标 变 换 ,A B AB C CT T T, 3 31 12 2 2 23 31 12 2 2 21 0 0 3 0 0 0 30 1 0 0 0 0 0 00 0 1 0 1 0 0 2 1 0 0 20 0 0 1 0 0 0 1 0 0 0 1A A BC B CT T T 因 此 A沿 xA和 zA平 移 3和 2， 然 后 绕 yA轴 转 90 ， 再 绕 新 xA轴 转 -30 得 C也 可 以 按 以 下 方 法 计 算 31 2 23 31 12 2 2 231 2 29 0 0 9 0 1 0 00 1 0 0 3 0 3 09 0 0 9 0 0 3 0 3 00 0 1 1 0 0 00 1 0 0 01 0 0 0 1 0 0AC c sR c ss c s c 17 事 实 上 ， 对 于 像 本 例 题 这 种 简 单 的 情 况 ， 可 以 直 接 利 用 齐 次 坐 标 变 换的 定 义 得 到 变 换 矩 阵 。 即 直 接 写 出 坐 标 系 C坐 标 轴 矢 量 在 坐 标 系 A下 表示 得 旋 转 矩 阵 ， 平 移 矢 量 为 坐 标 系 C的 原 点 在 坐 标 系 A下 的 矢 量 表 示 。 18 变 换 方 程 图 2-13表 示 了 多 个 坐 标 系 的 关 系 图 ， 可 以 用 两 种 不同 的 方 式 得 到 世 界 坐 标 系 U下 坐 标 系 D的 描 述 。TTT ADUAUD TTTT CDBCUBUD （ 2-31） （ 2-32）由 （ 2-31） 和 （ 2-32） 可 以 得 到 变 换 方 程 图 2-13坐 标 变 换 序 列 可 以 利 用 变 换 方 程 （ 2-33） 求 解 其 中 任 意 一 个 未 知 变 换 。 例 如 ， 假 设除 以 外 其 余 变 换 均 为 已 知 ， 则 该 未 知 变 换 可 以 用 下 式 计 算TU B 1 1U U A C BB A D D CT T T T T 在 坐 标 系 的 图 形 表 示 方 法 中 ， 从 一 个 坐 标 系 原 点 指 向 另 一 个 坐 标 系 原 点的 箭 头 表 示 坐 标 系 的 描 述 关 系 。 TTT BCUBUC （ 2-35）1U U D DC A A CT T T T （ 2-36） 19 例 2-5假 设 已 知 图 机 械 臂 末 端 工 具 坐 标 系 T相 对 基 座坐 标 系 B的 描 述 ， 还 已 知 工 作 台 坐 标 系 S相 对 基 座坐 标 系 B的 描 述 ， 并 且 已 知 螺 栓 坐 标 系 G 相 对 工 作台 坐 标 系 S的 描 述 。 计 算 螺 栓 相 对 机 械 臂 工 具 坐 标系 的 位 姿 。 解 ： 添 加 从 工 具 坐 标 系 T原 点 到 螺 栓 坐 标 系 G原 点的 箭 头 ， 可 以 得 到 如 下 变 换 方 程TTTT TGBTSGBS （ 2-37）螺 栓 相 对 机 械 臂 工 具 坐 标 系 的 位 姿 描 述 为TTTT SGBSBTTG 1 （ 2-38） 20 x y zf f f f i j k1.绕 任 意 轴 旋 转 变 换下 面 讨 论 绕 任 意 轴 f 旋 转 矩 阵 ， 轴 在 坐 标 系 A下 表 示 为以 f 为 Z 轴 建 立 与 A固 连 的 坐 标 系 C用 n、 o和 f表 示 坐 标 系 C三 个 坐 标轴 的 单 位 矢 量 ， 在 坐 标 系 A下 表 示 为 图 2-18绕 任 意 轴 旋 转 变 换 x y zx y zx y zn n no o of f f n i j ko i j kf i j k x x xAC y y yz z zn o fR n o fn o f 因 为 固 连 的 坐 标 系 C与 A固 连 ， 所 以 绕 f旋 转等 价 于 绕 ZC旋 转 。 为 此 我 们 先 将 Ap在 坐 标 系 C下表 示 ， 再 绕 ZC旋 转 角 ， 最 后 再 把 旋 转 得 到 的 矢 量用 坐 标 系 A表 示 。 C A T AC AA CR R p p p1 ( , ) ( , ) A T AC C CRot z Rot z R p p pAp1 = Rot(f,) Ap 2.5通 用 旋 转 变 换 21 再 将 Cp1在 坐 标 系 A下 表 示1 1 ( , )A A A T AA CC C CR R Rot z R p p p 0( , ) ( , ) 00 0 1 x y zx x xA A TC C y y y x y zz z z x y zx x y y z zx x x y y y x x y y z zz z z x y z n n nn o f c sRot R Rot z R n o f s c o o on o f f f fn c o s n c o s n c o sn o f nn o f n s o c n s o c n s o cn o f f f f f 1 1 12 2 23 3 3o an o an o a 因 此123 x x x x x x x x x xx y y x x y x y x yx z z x x z x z x zn n n c n o s n o s o o c f fn n n c n o s n o s o o c f fn n n c n o s n o s o o c f f 其 中 一 个 矢 量上 式 中 的 n和 o各 分 量 是 未 知 的 ， 需 要 用 f 的 各 分 量 表 示 22 根 据 坐 标 系 的 右 手 规 则 知 no = f， 叉 积 可 以 按 下 式 计 算( ) ( ) ( )x y z y z z y z x x z x y y xx y zn n n n o n o n o n o n o n oo o o i j kn o i j k( ) , ( ) , ( )y z z y x x y x z y x y y x zn o n o f n o n o f n o n o f 再 根 据 旋 转 矩 阵 的 正 交 性 可 以 得 1, 0 x x x x x x x y x y x yn n o o f f n n o o f f ( , ) , 1x x x y z x z yx y z y y y z xx z y y z x z zf f v c f f v f s f f v f sRot f f v f s f f v c f f v f s v cf f v f s f f v f s f f v c f123 x x x x x x x x x xx y y x x y x y x yx z z x x z x z x zn n n c n o s n o s o o c f fn n n c n o s n o s o o c f fn n n c n o s n o s o o c f f x x xAC y y yz z zn o fR n o fn o f 将 上 式 对 角 线 相 加 得 r11+ r22+ r33=1+2c c=( r11+ r22+ r33 -1)/2 232.等 效 转 轴 与 转 角 前 面 讨 论 了 给 定 转 轴 和 转 角 可 以 得 到 旋 转 矩 阵 ， 那 么 是 否 任 意 给 定 的 旋 转 矩阵 都 可 以 确 定 等 效 的 转 轴 f和 转 角 哪 ？ 也 就 是 两 个 坐 标 原 点 重 合 的 坐 标 系 可 以 通过 绕 固 定 轴 转 一 定 的 角 度 来 实 现 从 一 个 坐 标 系 转 换 到 另 一 个 坐 标 系 。 11 12 1321 22 2331 32 33 x x x y z x z yAC x y z y y y z xx z y y z x z zf f v c f f v f s f f v f sr r rR r r r f f v f s f f v c f f v f sr r r f f v f s f f v f s f f v c 将 关 于 对 角 线 对 称 的 两 个 元 素 分 别 相 减 得r32-r23=2fxs, r13-r31=2fys, r21-r12=2fzs 将 上 式 平 方 求 和 得： 4s2=( r32-r23)2+( r13-r31)2+(r21-r12)2 假 设 限 定 绕 矢 量 f 正 向 旋 转 ， 且 0180o， 则2 2 232 23 12 31 21 12( ) ( ) ( ) / 2s r r r r r r 可 得 的 值 =atan(s/c) 24 32 2313 3121 12( )/ 2( )/ 2( )/ 2xyzf r r sf r r sf r r s 可 得 矢 量 f 分 量 的 值 在 应 用 中 需 要 注 意 的 是 ， 当 转 角 的 值 接 近 0o或 180o时 ， 方 向 矢 量 f 各 分 量 的 值 计 算 出 现 问 题 ， 属 于 奇 异 情 况 。