机器人技术二、齐次坐标变换

齐 次 坐 标 变 换主 讲 ： 吴 海 彬福 州 大 学 机 械 工 程 及 自 动 化 学 院第 二 讲 主 要 内 容引 言点 的 向 量 表 示单 位 向 量点 和 向 量 的 齐 次 表 示坐 标 系 的 位 姿刚 体 的 位 姿平 移 变 换旋 转 变 换一 般 变 换 相 对 参 考 坐 标 系 的 变 换相 对 自 身 坐 标 系 的 变 换 引 言 (Introduction) 机 器 人 运 动 学 解 决 的 基 本 问 题 ： 正 向 运 动 学 逆 向 运 动 学 机 器 人 机 构 一 个 自 由 度 情 况 多 个 自 由 度 情 况 误 差 的 反 馈 点 、 向 量 和 坐 标 系 的 传 统 表 示 坐 标 轴 的 定 义kcjbiaP zyx zyxcbaP或kcjbiaP zyx zyxcbaP或 Pzaon Paon PaonT zzz yyyy xxxx非 方 阵 相 乘 结 果 的 维 数 发 生 变 化 点 、 向 量 和 坐 标 系 的 齐 次 表 示 在 三 维 向 量 中 加 入 一 比 例 因 子 w； 其 物 理 意 义 是 ， 随 着 W的 改 变 ， 向 量 的 大 小 会 发 生 变 化 ， 而 方 向 不 变 ； W大 于 1， 向 量 的 分 量 变 大 ； W小 于 1， 向 量 的 分 量 变 小 ； 若 W 1， 各 分 量 大 小 不 变 ； 若 W 0， 则 表 示 一 个 无 穷 小 的 向 量 ， 其 方 向 不 变 。 第 二 章 机 器 人 运 动 学 zyxcbaP wzyxP wxax wyby wzcz 其 中齐 次 坐 标 与 传 统 坐 标 的 关 系 点 、 向 量 和 坐 标 系 的 齐 次 表 示 第 二 章 机 器 人 运 动 学 因 此 ， 习 惯 上 用 W 1表 示 向 量 的 长 度 ， 用 W 0表 示 向 量 的 方向 ， 而 且 方 向 向 量 一 般 表 示 成 单 位 向 量 的 形 式 。 形 式 如 下 ： 1 zyxcbaP 0 222 222 222 zyx z zyx y zyx x cba c cba b cba aP 例 ： 有 一 向 量 P（ 3， 5， 2） ， 请 按 如下 要 求 表 示 成 矩 阵 形 式 ：1、 比 例 因 子 为 2；2、 表 示 为 方 向 的 单 位 向 量 。 点 、 向 量 和 坐 标 系 的 齐 次 表 示 原 点 重 合 情 况坐 标 系 的 齐 次 表 示 是 由 坐 标 系 的 三 个 方 向 向 量 和 原 点 位 置齐 次 坐 标 组 成 ： 1000 zzzz yyyy xxxx Paon Paon PaonF例 ： 如 图 所 示 为 F坐 标 系 位 于 参 考 坐 标系 中 （ 3， 5， 7） 的 位 置 ， 它 的 n轴 与 x轴 平 行 ， o轴 相 对 于 y轴 的 角 度 为 45度 ， a轴相 对 于 z的 角 度 为 45度 。 请 写 出 该 坐 标 的齐 次 表 达 形 式 。 点 、 向 量 和 坐 标 系 的 齐 次 表 示 第 二 章 机 器 人 运 动 学刚 体 的 表 示 一 个 刚 体 在 空 间 的 表 示 可 以 这 样 实 现 ： 通 过 在 它 上 面 固 连 一 个 坐 标 系 ， 再 将 该固 连 的 坐 标 系 在 空 间 表 示 出 来 。 由 于 这 个 坐 标 系 一 直 固 连 在 该 刚 体 上 ， 所 以 该 刚 体相 对 于 坐 标 系 的 位 姿 是 已 知 的 。 因 此 ， 只 要 这 个 坐 标 系 可 以 在 空 间 表 示 出 来 ， 那 么这 个 刚 体 相 对 于 固 定 坐 标 系 的 位 姿 也 就 已 知 了 。 由 此 可 知 ， 刚 体 在 参 考 坐 标 系 的 表示 与 坐 标 系 是 完 全 一 样 的 。 1000 zzzz yyyy xxxxobject Paon Paon PaonF 图 约 束 变 量点 、 向 量 和 坐 标 系 的 齐 次 表 示 第 二 章 机 器 人 运 动 学由 刚 体 （ 坐 标 系 ） 在 参 考 坐 标 系 的 齐 次 矩 阵 表 达 可 知 ， 该 矩阵 有 12个 变 量 ， 但 描 述 刚 体 位 姿 只 需 要 6个 变 量 （ 自 由 度 ） 就足 够 了 ， 因 此 ， 齐 次 矩 阵 中 12个 变 量 之 间 并 不 是 相 互 独 立 的 ，而 是 有 约 束 的 ， 约 束 条 件 为 ：1、 三 个 方 向 向 量 相 互 垂 直 ；2、 每 个 单 位 向 量 的 长 度 均 为 1。 即 ： 0on 0an 0oa1n 1o 1a 已 知 两 个 向 量 a = ax i + ay j + az k b = bx i + by j + bz k 向 量 的 点 积 是 标 量 。 用 “ ”来 定 义 向 量 点 积 ， 即 a b = ax bx + ay by + az bz 向 量 的 叉 积 是 一 个 垂 直 于 由 叉 积 的 两 个 向 量 构 成 的 平 面 的 向 量 。用 “ ”表 示 叉 积 ， 即 a b = ( a y bz az by ) i + ( az bx ax bz ) j + ( ax by ay bx ) k 可 用 行 列 式 表 示 为 i j k a b = ax ay az bx by bz 例 题 点 、 向 量 和 坐 标 系 的 齐 次 表 示 第 二 章 机 器 人 运 动 学对 于 下 列 坐 标 系 ， 求 解 所 缺 元 素 的 值 ， 并 用 矩 阵 来 表 示 这 个 坐 标 系 。 1000 20? 3?707.0 5?0?F kajaiaooo nnn kji zyxzyx zyx aon 注 ： 三 个 点 积 约 束 条 件 可 以 用 叉 积 代 替 ， 即 ：进 一 步 有 齐 次 变 换 矩 阵 变 换 定 义 为 空 间 的 一 个 运 动 ； 当 空 间 的 一 个 坐 标 系 （ 向 量 、 刚 体 、 运 动 坐标 系 ） 相 对 于 固 定 的 参 考 坐 标 系 运 动 时 ， 这一 运 动 可 以 用 类 似 于 表 示 坐 标 系 的 方 式 来 表示 ； 变 换 有 如 下 几 种 形 式 ： 纯 平 移 ， 纯 旋 转 ， 平 移 和 旋 转 的 结 合 。 第 二 章 机 器 人 运 动 学 纯 平 移 齐 次 变 换 矩 阵 第 二 章 机 器 人 运 动 学特 点 ： 运 动 过 程 中 姿 态 不 变 ， 坐 标 方 向 单 位 向 量 保 持 同 一 方 向 不 变 。),(1000 100 010 001 zyxzyx dddTransdddT 变 换 矩 阵 可 表 示 为 100010001000 100 010 001 zzzzz yyyyy xxxxxzzzz yyyy xxxxzyxnew dPaon dPaon dPaonPaon Paon PaondddF变 换 过 程 为 ：注 ： 相 对 固 定 坐 标 系 的 平 移 ， 变 换 矩 阵左 乘 ， 公 式 为 oldzyxnew FdddTransF ),( 例 纯 旋 转 (相 对 坐 标 绕 参 考 坐 标 X轴 ）齐 次 变 换 矩 阵 第 二 章 机 器 人 运 动 学nx PP sincos21 aoy PPllP cossin43 aoz PPllP aonzyx PPPPPP cossin0 sincos0 001 例 必 须 从 原 点 开 始 变 换 ！ 纯 旋 转 齐 次 变 换 矩 阵 第 二 章 机 器 人 运 动 学noaxyz PxRotP ),( cossin0 sincos0 001),(xRot cos0sin 010 sin0cos),(yRot 100 0cossin 0sincos),( zRot也 就 相 当 于 旋 转 变 换 前 在 固 定 参 考 坐 标 系 的 初 始 位 置 。式 中 noaP PTP RRUU 图 、 例注 ： 相 对 固 定 坐 标 系 的 旋 转 ， 变 换 矩 阵 左 乘 ， 公 式 为绕 x轴 旋 转 可 简 写 成其 中 同 理 纯 旋 转 例 题齐 次 变 换 矩 阵 第 二 章 机 器 人 运 动 学旋 转 坐 标 系 中 有 一 点 P（ 2， 3， 4） ， 此 坐 标 系 绕 参 考 坐 标 系 x轴 旋 转 90度 。 求 旋 转 后 该 点 相 对 于 参 考 坐 标 系 的 坐 标 。 复 合 变 换 齐 次 变 换 矩 阵 第 二 章 机 器 人 运 动 学特 点 ： 既 有 平 移 ， 又 有 旋 转 ， 而 且 可 以 多 次 。假 设 坐 标 系 （ n， o， a） 相 对 于 参 考 坐 标 系 （ x， y， z） 依 次 进 行 如 下 变 换 ：1、 绕 x轴 旋 转 角 ；2、 平 移 ；3、 再 绕 y轴 旋 转 角 。 321 lll noaxyz PxRotlllTransyRotP ),(),(),( 321 注 ： 矩 阵 的 顺 序 不 能 变 ； 相 对 固 定 坐 标 系 的 平 移 和 旋 转 ， 变 换 矩 阵 左 乘 。 例 复 合 变 换 例 题 齐 次 变 换 矩 阵 相 对 坐 标 系 的 齐 次 矩 阵固 连 在 坐 标 系 （ n， o， a） 上 的 点 P（ 7， 3， 2） 经 历 如 下 变 换 ， 求 出 变换 后 该 点 相 对 于 参 考 坐 标 系 的 坐 标 。1、 绕 z轴 旋 转 90度 ；2、 接 着 绕 y轴 旋 转 90度 ；3、 接 着 再 平 移 （ 4， -3， 7） 。 复 合 变 换 例 题 齐 次 变 换 矩 阵 第 二 章 机 器 人 运 动 学假 设 （ n， o， a） 坐 标 系 上 的 点 P（ 7， 3， 2） 也 经 历 相 同 变 换 ， 但 变 换 顺 序 按 如 下进 行 ， 求 出 变 换 后 该 点 相 对 于 参 考 坐 标 系 的 坐 标 。1、 绕 z轴 旋 转 90度 ；2、 接 着 平 移 （ 4， -3， 7） ；3、 接 着 再 绕 y轴 旋 转 90度 。 相 对 动 坐 标 系 的 变 换齐 次 变 换 矩 阵 第 二 章 机 器 人 运 动 学相 对 运 动 坐 标 系 的 变 换 与 相 对 固 定 参 考 坐 标 系 不 同 ， 这 时 需要 右 乘 变 换 矩 阵 而 不 是 左 乘 。相 对 自 身 的 运 动 即 是 相 对 动 坐 标 。相 对 动 坐 标 是 指 动 坐 标 系 本 身 相 对 自 身 的 运 动 ， 而 不 是 动 坐标 系 中 的 点 相 对 动 坐 标 系 的 运 动 。如 果 在 一 个 变 换 过 程 中 ， 既 有 相 对 固 定 坐 标 系 的 变 换 ， 也 有相 对 于 动 坐 标 系 的 变 换 ， 则 应 先 写 出 第 一 个 变 换 因 子 ， 在 根 据变 换 的 具 体 过 程 ， 依 次 左 乘 或 右 乘 变 换 因 子 ， 最 后 乘 以 被 变 换的 对 象 （ 点 或 坐 标 ） 。 相 对 动 坐 标 系 的 变 换 例 题齐 次 变 换 矩 阵 第 二 章 机 器 人 运 动 学假 设 与 上 例 相 同 的 点 现 在 进 行 相 同 的 变 换 ， 但 所 有 变 换 都 是 相 对 当 前 运 动 坐 标系 的 ， 具 体 变 换 如 下 ， 求 变 换 完 成 后 该 点 相 对 于 参 考 坐 标 系 的 坐 标 。1、 绕 a轴 旋 转 90度 ；2、 然 后 沿 n、 o、 a轴 平 移 （ 4， -3， 7） ；3、 接 着 绕 o轴 旋 转 90度 。 相 对 动 坐 标 系 的 变 换 例 题齐 次 变 换 矩 阵 第 二 章 机 器 人 运 动 学坐 标 系 B绕 x轴 旋 转 90度 ， 然 后 沿 当 前 坐 标 系 a轴 做 了 3英 寸的 平 移 ， 然 后 再 绕 z轴 旋 转 90度 ， 最 后 沿 当 前 坐 标 系 o轴 做 5英 寸 的 平 移 。1、 写 出 描 述 该 运 动 的 方 程 ；2、 求 坐 标 系 中 的 点 P（ 1， 5， 4） 相 对 于 参 考 坐 标 系 的 最 终位 置 。提 示 ： 先 求 ， 再 求 BUT PTP BBUU 变 换 矩 阵 的 逆 第 二 章 机 器 人 运 动 学钻 孔 点 位 置 的 描 述 ： EPPUEHHRRUEU TTTTTT 式 中 ： 只 有 是 未 知 的 ， 其 它 都 可 以 通 过 传 感 器 获 得 ， 或本 身 就 是 已 知 的 。 因 此 ， 通 过 求 逆 阵 就 可 以 求 得 。HRT HRT 求 矩 阵 逆 例 题变 换 矩 阵 的 逆 第 二 章 机 器 人 运 动 学在 一 个 具 有 六 自 由 度 的 机 器 人 的 第 五 个 连 杆 上 装 有 照 相 机 ，照 相 机 观 察 物 体 并 测 定 它 相 对 于 照 相 机 坐 标 系 的 位 置 ， 然后 根 据 以 下 数 据 来 确 定 末 端 执 行 器 要 到 达 物 体 所 必 须 完 成的 运 动 。 1000 5001 0010 31005 camT 1000 4100 0001 00105 HT 1000 4010 2001 2100objcamT 1000 3100 0010 0001EHT objcamcamRobjEEHHR TTTTTTT 5555 objET提 示 ： 根 据 求 ， 这 可 以 用 于 测 距 变 换 矩 阵 的 逆求 逆 阵 的 步 骤 ： 第 二 章 机 器 人 运 动 学1、 计 算 矩 阵 的 行 列 式 ；2、 将 矩 阵 转 置 ；3、 将 转 置 矩 阵 的 每 个 元 素 用 它 的 子 行 列 式 （ 伴 随 矩 阵 ） 代 替 ；4、 用 转 换 后 的 矩 阵 除 以 行 列 式AAA *1 即 cossin0 sincos0 001),(xRot例 ： 求 的 逆 阵 。满 足 TAA 1 的 矩 阵 称 为 酉 矩 阵 。 齐 次 矩 阵 的 逆变 换 矩 阵 的 逆 第 二 章 机 器 人 运 动 学 对 于 4X4齐 次 变 换 矩 阵 ， 可 以 将 矩 阵 分 成 两 部 分 求 逆 。其 旋 转 部 分 仍 是 酉 矩 阵 ， 只 需 要 简 单 的 转 置 ； 矩 阵 的 位 置部 分 是 向 量 P分 别 与 n、 o、 a向 量 点 积 的 取 反 。 10001 aPaaa oPooo nPnnnT zyx zyx zyx 1000 zzzz yyyy xxxx Paon Paon PaonT即 的 逆 阵 为 1000 5010 25.00866.0 3866.005.0T例 ： 求 的 逆 阵 。 图 2.12所 示 为 点 A绕 任 意 过 原 点 的 单 位 矢 量 此 旋 转 角 的 情况 。 kx， ky， kz分 别 为 此 矢 量 在 固 定 参 考 系 坐 标 轴 X、 Y、Z上 的 三 个 分 量 ， 可 以 证 得 ， 绕 任 意 过 原 点 的 单 位 矢 量 k转 角 的 旋 转 齐 次 变换 公 式 为 式 (2-18)称 为 一 般 旋 转 齐 次 变 换 通 式 ， 它 概 括了 绕 X轴 、 Y轴 、 Z轴 进 行 旋 转 齐 次 变 换 的 各 种特 殊 情 况 ， 例 如 ： 当 kx=1， 即 ky=kz=0时 ， 则 由 式 (2-18)可 得 到 式(2-16)； 当 ky=1， 即 kx=kz=0时 ， 则 由 式 (2-18)可 得 到 式(2-17)； 当 kz=1， 即 kx=ky=0时 ， 则 由 式 (2-18)可 得 到 式(2-15)。 反 之 ， 若 给 出 某 个 旋 转 齐 次 矩 阵则 可 根 据 式 (2-18)求 出 其 等 效 矢 量 k及 等 效转 角 式 中 ： 当 取 0到 180。 之 间 的 值 时 ， 式 中 的 符 号 取 +号 ；当 转 角 时 很 小 时 ， 公 式 很 难 确 定 转 轴 ； 当 接 近 0。 或180。 时 ， 转 轴 完 全 不 确 定 。 与 平 移 变 换 一 样 ， 旋 转 变 换 算 子 公 式 (2-15)、 (2-16)、 (2-17)以 及 一 般 旋 转 变 换 算 子 公 式 (2-18)， 不 仅仅 适 用 于 点 的 旋 转 变 换 ,而 且 也 适 用 于 矢 量 、 坐 标 系 、物 体 等 旋 转 变 换 计 算 。 若 相 对 固 定 坐 标 系 进 行 变 换 ,则算 子 左 乘 ； 若 相 对 动 坐 标 系 进 行 变 换 ， 则 算 子 右 乘 。 例 2-5 已 知 坐 标 系 中 点 U的 位 置 矢 量 u=7 3 2 1T 将 此 点 绕 Z轴 旋 转 90， 再 绕 Y轴 旋 转 90， 如 图 2-13所 示 ，求 旋 转 变 换 后 所 得 的 点 W。 2-6 如 图 2-14所 示 单 臂 操 作 手 ， 手 腕 也 具 有 一个 自 由 度 。 已 知 手 部 起 始 位 姿 矩 阵 为若 手 臂 绕 Z0轴 旋 转 +90， 则 手 部 到 达 G2若 手 臂 不 动 ，仅 手 部 绕 手 腕 Z l轴 旋 转 +90,则 手 部 到 达 G3。 写 出 手 部坐 标 系 G2及 G3的 矩 阵 表 达 式 。