第 三 章 工 業(yè) 機(jī) 器 人 運(yùn) 動(dòng) 學(xué) 引 言 要 實(shí) 現(xiàn) 對(duì) 工 業(yè) 機(jī) 器 人 在 空 間 運(yùn) 動(dòng) 軌 跡 的 控 制 ，完 成 預(yù) 定 的 作 業(yè) 任 務(wù) ， 就 必 須 知 道 機(jī) 器 人 在 空 間瞬 時(shí) 的 位 置 與 姿 態(tài) 。 如 何 計(jì) 算 機(jī) 器 人 手 部 在 空 間的 位 姿 是 實(shí) 現(xiàn) 對(duì) 機(jī) 器 人 的 控 制 首 先 要 解 決 的 問(wèn) 題 。本 章 討 論 機(jī) 器 人 運(yùn) 動(dòng) 學(xué) 的 基 本 問(wèn) 題 ， 將 引 入 齊 次坐 標(biāo) 變 換 。 推 導(dǎo) 出 坐 標(biāo) 變 換 方 程 ； 利 用 DH參 數(shù) 法 ，進(jìn) 行 機(jī) 器 人 的 位 姿 分 析 ； 介 紹 機(jī) 器 人 正 向 和 逆 運(yùn)動(dòng) 學(xué) 的 基 礎(chǔ) 知 識(shí) 。 主 要 內(nèi) 容 u 數(shù) 學(xué) 基 礎(chǔ) 齊 次 坐 標(biāo) 變 換u 機(jī) 器 人 運(yùn) 動(dòng) 學(xué) 方 程 的 建 立 （ 正 運(yùn) 動(dòng) 學(xué) ）u 機(jī) 器 人 逆 運(yùn) 動(dòng) 學(xué) 分 析 一 、 機(jī) 器 人 數(shù) 學(xué) 基 礎(chǔ) 齊 次 坐 標(biāo) 變 換1.1 引 言 1.2 點(diǎn) 向 量 和 平 面 的 描 述1.3 變 換 1.4 平 移 變 換1.5 旋 轉(zhuǎn) 變 換 1.6 坐 標(biāo) 系1.7 相 對(duì) 變 換 1.8 物 體 的 描 述1.9 逆 變 換 1.10 一 般 性 旋 轉(zhuǎn) 變 換1.11 等 價(jià) 旋 轉(zhuǎn) 角 與 旋 轉(zhuǎn) 軸 1.12 擴(kuò) 展 與 縮 小1.13 透 視 變 換 1.14 變 換 方 程 1.15 小 結(jié) 1.1 引 言 (Introduction) 機(jī) 器 人 操 作 涉 及 到 各 物 體 之 間 的 關(guān) 系 和 各 物 體 與 機(jī) 械 手 之 間 的關(guān) 系 。 這 一 章 將 給 出 描 述 這 些 關(guān) 系 必 須 的 表 達(dá) 方 法 。 類 似 這 種 表 示方 法 在 計(jì) 算 機(jī) 圖 形 學(xué) 中 已 經(jīng) 解 決 。 在 計(jì) 算 機(jī) 圖 形 學(xué) 和 計(jì) 算 機(jī) 視 覺(jué) 中 ，物 體 之 間 的 關(guān) 系 是 用 齊 次 坐 標(biāo) 變 換 來(lái) 描 述 的 。 在 本 課 程 我 們 將 采 用齊 次 坐 標(biāo) 變 換 來(lái) 描 述 機(jī) 械 手 各 關(guān) 節(jié) 坐 標(biāo) 之 間 、 各 物 體 之 間 以 及 各 物體 與 機(jī) 械 手 之 間 的 關(guān) 系 。 本 章 首 先 介 紹 向 量 和 平 面 的 表 示 方 法 ， 然 后 引 出 向 量 和 平 面 的坐 標(biāo) 變 換 ， 這 些 變 換 基 本 上 是 由 平 移 和 旋 轉(zhuǎn) 組 成 ， 因 此 可 以 用 坐 標(biāo) 系 來(lái) 描 述 各 種 物 體 和 機(jī) 械 手 的 空 間 位 置 和 姿 態(tài) 。 稍 后 還 要 介 紹 逆 變換 ， 逆 變 換 是 運(yùn) 動(dòng) 學(xué) 求 解 的 基 礎(chǔ) 。 a 0 vz yx z yxpc b 0uE H圖 1.1 點(diǎn) 向 量 的 描 述1.2 點(diǎn) 向 量 和 平 面 的 描 述 （ Notation of point vectors and planes） 1.2.1 點(diǎn) 向 量 （ Point vectors） 點(diǎn) 向 量 描 述 空 間 的 一 個(gè) 點(diǎn) 在 某 個(gè) 坐 標(biāo) 系 的 空 間 位置 。 同 一 個(gè) 點(diǎn) 在 不 同 坐 標(biāo) 系 的 描 述 及 位 置 向 量 的 值 也不 同 。 如 圖 1.1中 ， 點(diǎn) p在 E坐 標(biāo) 系 上 表 示 為 Ev， 在 H坐標(biāo) 系 上 表 示 為 Hu， 且 v u。 一 個(gè) 點(diǎn) 向 量 可 表 示 為 v = ai + bj + ck 通 常 用 一 個(gè) （ n + 1） 維 列 矩 陣 表 示 ， 即 除 x、 y、z 三 個(gè) 方 向 上 的 分 量 外 ， 再 加 一 個(gè) 比 例 因 子 w ， 即 v = x y z w T 其 中 a = x/w, b = y/w, c = z/w。 改 變 比 例 因 子 w， 則 分 量 a、 b、 c 的 數(shù) 值 相 應(yīng) 改 變 ， 但 描 述 的 還 是 同 一 個(gè) 點(diǎn) 向 量 。 如 v = 3i + 4j + 5k 可 表 示 為 v = 3 4 5 1 T = 6 8 10 2 T = -3 -4 -5 -1T 在 向 量 中 增 加 一 個(gè) 比 例 因 子 w 是 為 了 方 便 坐 標(biāo) 變 換 中 的 矩 陣 運(yùn) 算 。 已 知 兩 個(gè) 向 量 a = ax i + ay j + az k b = bx i + by j + bz k （ 1.1） 向 量 的 點(diǎn) 積 是 標(biāo) 量 。 用 “ ”來(lái) 定 義 向 量 點(diǎn) 積 ， 即 a b = ax bx + ay by + az bz （ 1.2 ） 向 量 的 叉 積 是 一 個(gè) 垂 直 于 由 叉 積 的 兩 個(gè) 向 量 構(gòu) 成 的 平 面 的 向 量 。 用“ ” 表 示 叉 積 ， 即 a b = ( a y bz az by ) i + ( az bx ax bz ) j + ( ax by ay by ) k （ 1.3） 可 用 行 列 式 表 示 為 i j k a b = ax ay az （ 1.4） bx by bz 1.2.2 平 面 （ Planes） 平 面 可 用 一 個(gè) 行 矩 陣 表 示 ， 即 p = a b c d （ 1.5）它 表 示 了 平 面 p的 法 線 方 向 ， 且 距 坐 標(biāo) 原 點(diǎn) 的距 離 為 d / m， 其 中 m = （ 1.6） 如 圖 1.2所 示 ， 如 果 將 x y 平 面 沿 z 軸 正方 向 平 移 一 個(gè) 單 位 距 離 ， 構(gòu) 成 平 面 p， 則 p = 0 0 1 -1 即 a = 0, b = 0, c = 1, d = -1, m = = 1 平 面 p上 任 一 點(diǎn) v為 v = x y 1 1 T， 它 與 平 面 p的 點(diǎn) 乘 為 零 ， 即 p v = 0 平 面 p上 方 任 一 點(diǎn) v， 如 v = 0 0 2 1 T， 它 與 平 面 p的 點(diǎn) 乘 為 一 個(gè) 正 數(shù) ， 即 p v = 1 平 面 p下 方 任 一 點(diǎn) v， 如 v = 0 0 0 1 T， 它 與 平 面 p的 點(diǎn) 乘 為 一 個(gè) 負(fù) 數(shù) ， 即 p v = -1注 意 ： 平 面 0 0 0 0 無(wú) 定 義 。a2 + b2 + c2 a2 +b2 + c2 圖 1.2 平 面 的 描 述0 v pz yx 1 yx H空 間 的 變 換 是 由 4 4矩 陣 來(lái) 完 成 的 ， 它 可 以 表 示 平 移 、 旋 轉(zhuǎn) 、 擴(kuò) 展 和 透 視等 各 種 變 換 。 如 已 知 點(diǎn) u（ 在 平 面 p上 ） ,它 的 變 換 v（ 在 平 面 q上 ） 用 矩 陣 積 表 示 為 v = H u （ 1.7） 其 中 H為 4 4 變 換 矩 陣 ， u和 v為 4 1的 點(diǎn) 列 向 量 ， 相 應(yīng) 的 平 面 p到 q的 變 換 是 q = p H-1 （ 1.8） 其 中 H-1為 H的 逆 陣 ， p和 q為 1 4 的 平 面 行 向 量 。 經(jīng) 變 換 后 的 平 面 向 量 q與 點(diǎn) 向 量 v的 點(diǎn) 乘 為 q v = p H -1 H u p u （ 1.9） 與 變 換 前 平 面 p與 點(diǎn) u的 點(diǎn) 乘 相 等 ， 證 明 了 變 換 的 等 效 性 。 1.3 變 換 (Transformation) 1.4 平 移 變 換 （ Translation transformation） 用 向 量 h a i + b j + c k 進(jìn) 行 平 移 ， 其 相 應(yīng) 的 H變 換 矩 陣 是 1 0 0 a 0 1 0 b H = Trans ( a b c ) = 0 0 1 c (1.10) 0 0 0 1 因 此 對(duì) 向 量 u = x y z w T， 經(jīng) H變 換 為 向 量 v可 表 示 為 x + aw x / w + a y + bw y / w + b v = z + cw = z / w + c (1.11) w 1 可 見(jiàn) ， 平 移 實(shí) 際 上 是 對(duì) 已 知 向 量 u = x y z w T 與 平 移 向 量 h = a b c 1 T 相 加 。 【 例 1.1】 對(duì) 點(diǎn) 向 量 u = 2 3 2 1 T 進(jìn) 行 平 移 ， 平 移 向 量 為 h = 4 -3 7 1 T， 則 平移 后 的 向 量 為 v = 6 0 9 1 T， 或 1 0 0 4 2 6 0 1 0 3 3 0 v = H u = 0 0 1 7 2 = 9 0 0 0 1 1 1 點(diǎn) 向 量 的 平 移 過(guò) 程 如 圖 1.3所 示 。 對(duì) 平 面 的 平 移 則 用 H 1 進(jìn) 行 變 換 ， 如 對(duì) 平 面 p = 1 0 0 -2 進(jìn) 行 H 變 換 為 平 面 q， 則 根 據(jù) 變 換 原 理 有 1 0 0 -4 0 1 0 3 q p H 1 1 0 0 -2 0 0 1 -7 0 0 0 1 1 0 0 -6 平 面 p 1 0 0 -2 是 y z 平 面 沿 x 正 方 向 移 動(dòng) 2個(gè) 單 位 形 成 的 平 面 （ 圖 1.3） ， 點(diǎn) u = 2 3 2 1 T 是 平 面 p上 的 一 個(gè) 點(diǎn) ， 它 們 的 點(diǎn) 乘 p u = 0。 經(jīng) H 變 換 后 的 平 面 q 1 0 0 -6 是 y z 平 面 沿 x 正 方 向 移 動(dòng) 6個(gè) 單 位 形 成 的 平 面 ， 點(diǎn) v = 6 0 9 1T 是 平 面 q上 一 個(gè) 點(diǎn) ， 平 面 q 與點(diǎn) v 的 點(diǎn) 乘 也 應(yīng) 是 零 ， 即 q v 0， 說(shuō) 明 變 換 前 后 的 結(jié) 果 不 變 ， 證 明 H 變 換 是 正 確 的 。 u0z yx 3P 22圖 1.3 點(diǎn) 向 量 的 平 移v 6 9q p 1.5 旋 轉(zhuǎn) 變 換 （ Rotation transformation） 如 圖 1.4所 示 ， 繞 x, y, z 軸 旋 轉(zhuǎn) 一 個(gè) 角的 相 應(yīng) 變 換 是 1 0 0 0 0 cos - sin 0Rot ( x, ) = 0 sin cos 0 （ 1.12） 0 0 0 1 cos 0 sin 0 0 1 0 0Rot ( y, ) = - sin 0 cos 0 (1.13) 0 0 0 1 cos - sin 0 0 sin cos 0 0Rot ( z, ) = 0 0 1 0 (1.14) 0 0 0 1 注 意 ： 角 旋 轉(zhuǎn) 的 正 方 向 遵循 右 手 螺 旋 法 則 （ 如 圖 1.4所 示 ）圖 1.4 旋 轉(zhuǎn) 變 換 0z yx 【 例 1.2】 點(diǎn) u = 7i + 3j + 2k， 它 繞 z軸 旋 轉(zhuǎn) 90 為 v， 經(jīng) 式 （ 1.14） 變 換 得 到 （ sin =1， cos =0） 0 -1 0 0 7 -3 1 0 0 0 3 7 v = Rot ( z, 90 ) = 0 0 1 0 2 2 0 0 0 1 1 1 起 始 點(diǎn) u和 終 點(diǎn) v如 圖 1.5所 示 。 如 將 v點(diǎn) 再 繞 y軸旋 轉(zhuǎn) 90 得 到 w。 用 式 （ 1.13） 變 換 得 到 0 0 1 0 -3 2 0 1 0 0 7 7w = Rot ( y, 90 ) = -1 0 0 0 2 3 0 0 0 1 1 1 結(jié) 果 如 圖 1.6所 示 。 如 果 將 上 述 兩 次 旋 轉(zhuǎn) 結(jié) 合 起 來(lái) ，寫 成 一 個(gè) 表 達(dá) 式 得 到 w = Rot ( y, 90 ) v Rot ( y, 90 ) Rot ( z, 90 ) u 用 兩 個(gè) 變 換 矩 陣 Rot ( y, 90 ) 、 Rot ( z, 90 ) 和 起 始點(diǎn) u代 入 上 式 計(jì) 算 的 結(jié) 果 與 前 面 分 兩 次 計(jì) 算 的 結(jié) 果 相 同 。 2 u z yx v0圖 1.5 Rot ( z, 90 ) yu v0zx w 圖 1.6 Rot ( y, 90 ) Rot ( z, 90 )2 7 為 此 ， 先 將 點(diǎn) u繞 z軸 旋 轉(zhuǎn) 90 ， 然 后 再 繞 y軸 旋 轉(zhuǎn) 90 ， 我 們 得 到 0 0 1 0 0 -1 0 0 7 2 0 1 0 0 1 0 0 0 3 7w Rot ( y, 90 ) Rot ( z, 90 ) u = -1 0 0 0 0 0 1 0 2 3 0 0 0 1 0 0 0 1 1 1如 果 按 著 逆 序 旋 轉(zhuǎn) ， 首 先 繞 y軸 旋 轉(zhuǎn) 90 ， 然 后 再 繞 z軸 旋 轉(zhuǎn) 90 ， 其 結(jié) 果 為 0 -1 0 0 0 0 1 0 7 -3 1 0 0 0 0 1 0 0 3 2w = Rot ( z, 90 ) Rot ( y, 90 ) u = 0 1 0 0 -1 0 0 0 2 = -7 0 0 0 1 0 0 0 1 1 1 逆 序 旋 轉(zhuǎn) 的 結(jié) 果 如 圖 1.7所 示 。 顯 然 ， 變 換 的 順 序 不 同 ， 其 結(jié) 果 也 不 同 。 這 從矩 陣 相 乘 是 不 可 交 換 的 （ ABBA） 也 可 以 得 到 證 明 。 如 對(duì) 經(jīng) 過(guò) 兩 次 旋 轉(zhuǎn) 變 換 得 到 的 點(diǎn) 向 量 w再 進(jìn) 行 一 次 平 移 （ 平 移 向 量 為 h 4 -3 7 1T ） ，則 可 得 到 如 圖 1.8所 示 的 點(diǎn) 向 量 n。 變 換 過(guò) 程 如 下 1 0 0 4 2 6 0 1 0 -3 7 4 n = Trans (4, 3, 7) w = 0 0 1 7 3 = 10 0 0 0 1 1 1 z u v0 yx w 圖 1.8 Trans(4, -3, 7)Rot(y, 90 ) Rot(z, 90 ) n 72w0z yx u圖 1.7 Rot ( z, 90 ) Rot ( y, 90 )2-7 v 1.6 坐 標(biāo) 系 (Coordinate frames) 齊 次 變 換 矩 陣 H由 四 個(gè) 列 向 量 組 成 ， 它 的 前 三 個(gè) 列 向 量 稱 為 方 向 向 量 ， 由 式（ 1.12） 到 式 （ 1.14） 的 旋 轉(zhuǎn) 變 換 （ 分 別 繞 x、 y、 z 軸 旋 轉(zhuǎn) 角 ） 確 定 ， 第 四 個(gè) 列 向量 稱 為 平 移 向 量 ， 它 的 平 移 分 量 （ 沿 x、 y、 z 軸 的 平 移 量 ） 由 式 （ 1.10） 第 四 列 的 前三 個(gè) 元 素 確 定 。 如 0 0 1 4 1 0 0 -3 H Trans ( 4, -3, 7 ) Rot ( y, 90 ) Rot ( z, 90 ) = 0 1 0 7 （ 1.15） 0 0 0 1 坐 標(biāo) 系 的 原 點(diǎn) ， 即 零 向 量 0 0 0 1 T 的 H 變 換 是 4 -3 7 1 T， 相 當(dāng) 于 將 原 點(diǎn) 按 平 移向 量 的 各 個(gè) 分 量 進(jìn) 行 平 移 的 結(jié) 果 （ 如 圖 1.9 所示 ） 。 如 果 對(duì) x、 y、 z 軸 的 單 位 向 量 進(jìn) 行 H變換 ， 分 別 得 到 4 -2 7 1 T 、 4 -3 8 1 T 和 5 -3 7 1 T。 這 四 個(gè) 向 量 在 圖 1.9中 標(biāo) 出 ， 并形 成 了 一 個(gè) 新 坐 標(biāo) 系 。 0z y x z yx0 Trans ( 4, -3, 7 )Rot ( z, 90 )Rot ( y, 90 )圖 1.9 坐 標(biāo) 原 點(diǎn) 與 單 位 向 量 的 H 變 換 這 個(gè) 新 坐 標(biāo) 系 的 x、 y、 z 軸 的 方 向 分 別 是 0， 1， 0， 0 T、 0， 0， 1， 0 T 和 1， 0， 0， 0 T， 它 是 由 單 位 向 量 的 H變 換 減 去 這 個(gè) 坐 標(biāo) 原 點(diǎn) 的 向 量 得 到 的 。這 些 方 向 向 量 相 應(yīng) 于 變 換 矩 陣 的 前 三 列 （ 見(jiàn) 式 （ 1.15） ） 。 可 見(jiàn) ， H變 換 矩 陣 描 述了 一 個(gè) 坐 標(biāo) 系 繞 原 參 考 坐 標(biāo) 系 旋 轉(zhuǎn) 和 對(duì) 參 考 坐 標(biāo) 系 平 移 的 三 個(gè) 軸 的 方 向 和 原 點(diǎn) 的位 置 （ 見(jiàn) 圖 1.9） 。 如 圖 1.10所 示 ， 當(dāng) 對(duì) 一 個(gè) 向 量 n 進(jìn) 行 式 （ 1.15） 給 出 的 H 變 換時(shí) ， 原 向 量 n 可 以 被 認(rèn) 為 是 在 新 坐 標(biāo) 系 描 述 的 那 個(gè) 向 量 u ， 即 被 變 換 了 的 向 量 u 就 是 相 對(duì) 于 參 考 坐 標(biāo) 系 描 述 的 同 一 個(gè) 向 量 n 。 0 0z zy yxx u ( 7, 3, 2, 1 ) n ( 6, 4, 10, 1 )圖 1.10 向 量 的 H 變 換 1.7 相 對(duì) 變 換 （ Relative transformation） 我 們 剛 剛 描 述 的 旋 轉(zhuǎn) 和 平 移 都 是 相 對(duì) 于 一 個(gè) 固 定 的 坐 標(biāo) 系 而 進(jìn) 行 的 。 這 樣 ， 在已 給 的 例 子 里 0 0 1 4 1 0 0 -3 Trans ( 4, -3, 7 ) Rot ( y, 90 ) Rot ( z, 90 ) = 0 1 0 7 （ 1.16） 0 0 0 1 坐 標(biāo) 系 首 先 繞 參 考 坐 標(biāo) 系 z 軸 旋 轉(zhuǎn) 90 ， 然 后 繞 y 軸 旋 轉(zhuǎn) 90 ， 最 后 平 移 4i 3j+7k，如 圖 2.9所 示 。 如 果 以 相 反 次 序 從 左 到 右 來(lái) 進(jìn) 行 這 些 操 作 ： 首 先 對(duì) 坐 標(biāo) 平 移 4i3j+7k， 然后 將 它 繞 當(dāng) 前 坐 標(biāo) 系 的 y 軸 旋 轉(zhuǎn) 90 ， 此 時(shí) 當(dāng) 前 坐 標(biāo) 系 的 y 軸 與 參 考 坐 標(biāo) 系 的 y 軸 是 相 同的 。 然 后 再 繞 著 新 坐 標(biāo) 系 （ 當(dāng) 前 的 ） 坐 標(biāo) 系 的 z 軸 旋 轉(zhuǎn) 90 ， 所 得 結(jié) 果 與 前 面 的 方 法 相 同（ 見(jiàn) 圖 1.11） 。 0 0z z z zy y y yx x x x Rot ( y, 90 )Rot ( z, 90 ) Trans ( 4, -3, 7 )坐 標(biāo) 原 點(diǎn) 圖 1.11 相 對(duì) 變 換 一 般 的 情 況 下 ， 如 果 我 們 用 一 個(gè) 旋 轉(zhuǎn) 和 /或 平 移 變 換 矩 陣 右 乘 一 個(gè) 坐 標(biāo) 系 的 變換 ， 那 么 產(chǎn) 生 的 平 移 和 /或 旋 轉(zhuǎn) 是 相 對(duì) 于 前 一 個(gè) 變 換 的 坐 標(biāo) 系 （ 當(dāng) 前 坐 標(biāo) 系 ） 的 軸來(lái) 說(shuō) 的 。 如 果 我 們 用 一 個(gè) 描 述 平 移 和 /或 旋 轉(zhuǎn) 的 變 換 矩 陣 左 乘 一 個(gè) 坐 標(biāo) 系 的 變 換 ，那 么 產(chǎn) 生 的 平 移 和 /或 旋 轉(zhuǎn) 是 相 對(duì) 于 基 坐 標(biāo) 系 來(lái) 說(shuō) 的 ?！?例 1.3】 給 一 個(gè) 坐 標(biāo) 系 C和 一 個(gè) 變 換 T， T為 繞 z 軸 旋 轉(zhuǎn) 90 ， 并 在 x 軸 方 向 上 平 移 10個(gè) 單 位 ， 當(dāng) 變 換 是 相 對(duì) 于 基 坐 標(biāo) 系 產(chǎn) 生 時(shí) ， 我 們 用 T 左 乘 C 得 到 新 的 位 置 x 為 0 -1 0 10 1 0 0 20 0 0 1 0 1 0 0 0 0 0 -1 10 1 0 0 20 x = T C = 0 0 1 0 0 1 0 0 0 1 0 0 （ 1.17） 0 0 0 1 0 0 0 1 0 0 0 1 當(dāng) 變 換 是 相 對(duì) 于 當(dāng) 前 坐 標(biāo) 系 C 軸 產(chǎn) 生 時(shí) ， 我 們 用 T 右 乘 C 得 到 新 的 位 置 y 為 1 0 0 20 0 -1 0 10 0 -1 0 30 0 0 -1 10 1 0 0 0 0 0 -1 10 y = C T = 0 1 0 0 0 0 1 0 1 0 0 0 （ 1.18） 0 0 0 1 0 0 0 1 0 0 0 1 結(jié) 果 如 圖 1.12所 示 。 Y XTrans (10, 0, 0 )Rot ( z, 90 ) 0z yx x x xx yy y yzz zz Rot ( z, 90 )Trans (10, 0, 0 )圖 1.12 相 對(duì) 于 基 坐 標(biāo) 系 和 當(dāng) 前 坐 標(biāo) 系 的 變 換 1.8 物 體 的 描 述 （ Object representation） 變 換 可 用 來(lái) 描 述 物 體 的 位 置 與 方 向 （ 方 位 ） 。 如 圖 1.13所 示 的 楔 形 物 體 用 六 個(gè) 角 點(diǎn)來(lái) 描 述 ， 這 六 個(gè) 角 點(diǎn) 是 相 對(duì) 于 物 體 所 在 的 參 考 坐 標(biāo) 系 的 。 如 果 把 物 體 繞 z 軸 旋 轉(zhuǎn) 90 ，然 后 繞 y 軸 旋 轉(zhuǎn) 90 ， 接 著 沿 x 方 向 平 移 4個(gè) 單 位 ， 我 們 可 以 描 述 這 個(gè) 變 換 為 0 0 1 4 1 0 0 0 Trans ( 4, 0, 0 ) Rot ( y, 90 ) Rot ( z, 90 ) = 0 1 0 0 0 0 0 1 這 個(gè) 變 換 表 示 了 對(duì) 參 考 坐 標(biāo) 系 的 旋 轉(zhuǎn) 和 平 移 操 作 ， 變 換 后 物 體 的 六 個(gè) 角 點(diǎn) 為 4 4 6 6 4 4 0 0 1 4 1 -1 -1 1 1 -1 1 -1 -1 1 1 -1 1 0 0 0 0 0 0 0 4 4 0 0 0 0 4 4 = 0 1 0 0 0 0 0 2 2 0 1 1 1 1 1 1 0 0 0 1 1 1 1 1 1 1 變 換 后 該 物 體 在 坐 標(biāo) 上 的 方 位 如 圖 1.13所 示 。 從 圖 1.13可 以 看 出 ， 由 于 楔 形 物 體 的 角 點(diǎn) 與 它 所 在 的 坐 標(biāo) 系 有 固 定 的 關(guān) 系 ，因 此 沒(méi) 有 必 要 對(duì) 所 有 的 角 點(diǎn) 進(jìn) 行 變 換 ， 只 要 對(duì) 物 體 所 在 的 坐 標(biāo) 系 進(jìn) 行 變 換 ， 就 可得 到 變 換 后 的 各 個(gè) 角 點(diǎn) 在 基 坐 標(biāo) 中 的 位 置 ， 將 這 些 角 點(diǎn) 用 直 線 連 接 起 來(lái) 就 可 得 到楔 形 物 體 的 邊 緣 ， 它 與 逐 點(diǎn) 變 換 的 結(jié) 果 完 全 相 同 （ 見(jiàn) 圖 1.14） 。 ( -1, 0, 0 )( -1, 0, 2 )( 1, 0, 2 ) ( 1, 4, 0 )( -1, 4, 0 )( 1, 0, 0 ) z yx 0圖 1.13 楔 形 物 體 圖 1.14 被 變 換 的 楔 形 物 體( 4, 1, 0 )( 4, -1, 4 ) ( 4, 1, 4 )( 6, 1, 0 )( 6, -1, 0 )( 4, -1, 0 ) yx 0y xz z 1.9 逆 變 換 （ Inverse transformation) 所 謂 逆 變 換 就 是 將 被 變 換 的 坐 標(biāo) 系 返 回 到 原 來(lái) 的 坐 標(biāo) 系 ， 在 數(shù) 學(xué) 上 就 是 求 變換 矩 陣 的 逆 。 下 面 我 們 寫 出 變 換 矩 陣 的 一 般 表 達(dá) 形 式 nx ox ax px ny oy ay py T = nz oz az pz （ 1.19） 0 0 0 1 式 中 n, o, a 是 旋 轉(zhuǎn) 變 換 列 向 量 ， p 是 平 移 向 量 ， 其 逆 是 n x ny nz - p.n ox oy oz - p.o T-1 = ax ay az - p.a （ 1.20） 0 0 0 1 式 中 的 “ . ” 表 示 向 量 的 點(diǎn) 積 。 這 個(gè) 結(jié) 果 很 容 易 用 式 1.19右 乘 式 1.20是 單 位 矩 陣來(lái) 證 明 。 1.10 一 般 性 旋 轉(zhuǎn) 變 換 （ General rotation transformation） 前 面 我 們 介 紹 的 旋 轉(zhuǎn) 變 換 都 是 繞 x， y， z 軸 旋 轉(zhuǎn) 的 旋 轉(zhuǎn) 變 換 ， 這 些 變 換 都 有一 個(gè) 簡(jiǎn) 單 的 幾 何 解 釋 。 例 如 ： 在 繞 z 軸 旋 轉(zhuǎn) 的 情 況 下 ， 表 示 z 軸 保 持 恒 定 ， x 軸 和 y 軸 將 如 圖 1.15所 示 那 樣 變 化 。 圖 1.15 繞 z 軸 的 旋 轉(zhuǎn) z 0z y yxx Cos Sin Sin Cos 如 圖 1.16所 示 ， 給 出 一 個(gè) 變 換 矩 陣 C， 它 繞任 意 向 量 k 旋 轉(zhuǎn) ， 我 們 把 k當(dāng) 作 C坐 標(biāo) 系 的 z 軸 單位 向 量 。 nx ox ax 0 ny oy ay 0 C = nz oz az 0 （ 1.21） 0 0 0 1 k = ax i + ay j + az k （ 1.22）繞 k 旋 轉(zhuǎn) 就 相 等 于 繞 C 坐 標(biāo) 系 的 z 軸 旋 轉(zhuǎn) 。 Rot（ k ， ） = Rot（ Cz， ） （ 1.23） 如 果 我 們 給 一 個(gè) 坐 標(biāo) 系 T， 它 在 參 考 坐 標(biāo)系 里 被 描 述 ， 它 在 C坐 標(biāo) 系 里 用 X描 述 ， 這 樣 T = C X （ 1.24）其 中 X描 述 T相 對(duì) C的 位 姿 ， 求 X， 我 們 得 到 X = C-1 T （ 1.25） kT z zy yx xx 0 0圖 1.16 一 般 性 旋 轉(zhuǎn) 變 換C T 繞 k 旋 轉(zhuǎn) 就 等 于 繞 坐 標(biāo) 系 的 z 軸 旋 轉(zhuǎn) Rot（ k, ） C Rot（ z, ） X （ 1.26） Rot（ k, ） C Ro t（ z, ） C-1 T （ 1.27）這 樣 Rot（ k, ） C Rot（ z, ） C-1 （ 1.28） 展 開(kāi) 式 （ 1.28） ， 我 們 發(fā) 現(xiàn) C Rot（ z, ） C-1 僅 是 k 的 函 數(shù) 。用 C-1右 乘 Rot（ z, ） ， 我 們 得 到 cos -sin 0 0 nx ny nz 0 sin cos 0 0 o x oy oz 0 Rot（ z, ） C-1 0 0 1 0 ax ay az 0 0 0 0 1 0 0 0 1 nx cos ox sin ny cosoy sin nz cosoz sin 0 nx cos + ox sin ny cos + oy sin nz cos+ oz sin 0 = ax ay az 0 （ 1.29） 0 0 0 1再 用 C左 乘 nx ox ax 0 ny oy ay 0 C = n z oz az 0 （ 1.30） 0 0 0 1 得 到 C Rot（ z, ） C-1 = nxnx cos nxox sin+ nxox sin+ oxox cos+ ax ax nynx cos nyox sin+ nxoy sin+ oyox cos+ ay ax nznx cos nzox sin+ nxoz sin+ oz ox cos+ az ax 0 nxny cos nxoy sin+ nyox sin+ oyox cos+ ax ay n yny cos nyoy sin+ nyoy sin+ oyoy cos+ ay ay nzny cos nzoy sin+ nyoz sin+ oyoz cos+ az ay 0 nxnz cos nxoz sin+ nzox sin+ ozox cos+ ax az 0 nynz cos nyoz sin+ nzoy sin+ ozoy cos+ ay az 0 nznz cos nzoz sin+ nzoz sin+ ozoz cos+ az az 0 （ 1.31） 0 1 應(yīng) 用 下 列 關(guān) 系 進(jìn) 行 簡(jiǎn) 化 ： C 坐 標(biāo) 系 任 意 的 行 或 列 與 其 他 行 或 列 的 點(diǎn) 積 為 零 ， 因 為 這 些 向 量 是 正 交 的 ； C 坐 標(biāo) 系 任 意 的 行 或 列 與 其 自 身 的 點(diǎn) 積 為 I ， 因 為 它 們 是 單 位 量 ； z 向 量 是 x 和 y 向 量 的 叉 積 ： a = n o， 它 有 下 列 分 量 ax = ny oz nz oy ay = nz ox nx o z az = nx oy ny ox 正 矢 Vers=（ 1cos） ， 簡(jiǎn) 寫 成 Vers， 且 kx = ax ， ky = ay ， kz = az 。 由 此 可 得 到簡(jiǎn) 化 式 為 Rot ( k, ) = k x kx Vers+ cos ky kx Verskz sin kz kx Vers + kysin 0 kx kyVers+ kz sin ky ky Vers+ cos kz kyVers kzxsin 0 kx kzVerskysin ky kz Vers + kxsin kz kzVers+ cos 0 （ 1.32） 0 0 0 1上 式 是 一 般 性 的 旋 轉(zhuǎn) 變 換 的 重 要 結(jié) 論 。 從 這 個(gè) 結(jié) 論 可 以 得 出 每 一 個(gè) 基 本 旋 轉(zhuǎn) 變 換 。 例 如 ：Rot ( x, )就 是 Rot ( k, )當(dāng) kx= 1， ky= 0, kz= 0 的 情 況 ， 將 這 些 值 代 入 式 （ 1.32） 得 到 1 0 0 0 0 cos -sin 0 Rot ( x, ) = 0 sin cos 0 （ 1.33） 0 0 0 1這 個(gè) 結(jié) 果 與 以 前 一 樣 。 1.11 等 價(jià) 旋 轉(zhuǎn) 角 與 旋 轉(zhuǎn) 軸 （ Equivalent angle and axis of rotation） 任 給 一 個(gè) 旋 轉(zhuǎn) 變 換 ， 從 （ 1.32） 方 程 得 到 一 個(gè) 軸 ， 繞 這 個(gè) 軸 旋 轉(zhuǎn) 的 等 價(jià) 旋 轉(zhuǎn) 角 可 由如 下 方 法 得 到 。 已 知 一 個(gè) 旋 轉(zhuǎn) 變 換 R nx ox ax 0 ny oy ay 0 R = nz oz az 0 （ 1.34） 0 0 0 1令 R 和 式 （ 1.32） 的 Rot ( k, ) 相 等 ， 并 將 對(duì) 角 線 各 項(xiàng) 相 加 得 到n x + oy + az +1 = k2x Vers+ cos+ k2yVers+ cos + k2z Vers+ cos+1 （ 1.35） nx + oy + az = ( k2x + k2y + k2z ) Vers+ 3cos = 1 + 2cos （ 1.36）由 此 可 得 到 旋 轉(zhuǎn) 角 的 余 弦 是 cos = 1/2（ nx + oy + az1） （ 1.37）對(duì) 非 對(duì) 角 線 項(xiàng) 相 減 ， 我 們 得 到 oz ay = 2 kx sin （ 1.38） ax nz = 2 ky sin （ 1.39） ny ox = 2 kz sin （ 1.40）把 式 （ 1.38） 到 式 （ 1.40） 兩 邊 平 方 并 相 加 有 （ o z ay） 2 +（ ax nz） 2 +（ ny ox ） 2 = 4 sin2 （ 1.41） 我 們 得 到 了 sin的 表 達(dá) 式 sin = 1/2（ oz ay） 2 +（ ax nz） 2 +（ ny ox ） 2 （ 1.42）規(guī) 定 這 個(gè) 旋 轉(zhuǎn) 是 繞 k 正 方 向 旋 轉(zhuǎn) ， 當(dāng) 0180 時(shí) ， 在 上 式 中 取 十 號(hào) 是 合 理 的 。這 個(gè) 旋 轉(zhuǎn) 角 被 唯 一 定 義 為 tan =（ oz ay） 2 +（ ax nz） 2 +（ ny ox ） 2 /（ nx + oy + az1 ） （ 1.43） k的 各 分 量 為 k x =（ ozay） / 2 sin （ 1.44） ky =（ axnz） / 2 sin （ 1.45） kz =（ nyox） / 2 sin （ 1.46）注 意 ： 當(dāng) 旋 轉(zhuǎn) 角 較 小 或 接 近 180 時(shí) ， 上 述 三 個(gè) 式 子 的 分 子 和 分 母 都 很 小 ， 所 計(jì) 算的 k值 是 不 精 確 的 。 為 此 可 繼 續(xù) 根 據(jù) 式 （ 1.32） 和 式 （ 1.33） 對(duì) 應(yīng) 元 素 以 及 它 們 的代 數(shù) 和 相 等 的 關(guān) 系 來(lái) 求 出 k的 各 個(gè) 分 量 。 1.12 擴(kuò) 展 與 縮 小 （ Stretching and scaling） 一 個(gè) 變 換 T a 0 0 0 0 b 0 0 T = 0 0 c 0 （ 1.47） 0 0 0 1 將 沿 著 x 軸 以 a 因 子 ， 沿 著 y 軸 以 b 因 子 ， 沿 著 z 軸 c 因 子 均 勻 擴(kuò) 展 著 各 種 物體 。 假 定 在 一 個(gè) 物 體 上 任 意 一 個(gè) 點(diǎn) x i + y j + z k ， 它 的 變 換 是 a x a 0 0 0 x b y 0 b 0 0 y c z = 0 0 c 0 z （ 1.48） 1 0 0 0 1 1這 個(gè) 正 好 表 示 出 所 說(shuō) 的 擴(kuò) 展 。 這 樣 ， 一 個(gè) 正 方 體 可 以 由 這 個(gè) 變 換 變 成 長(zhǎng) 方 體 。 變 換 s s 0 0 0 0 s 0 0 s = 0 0 s 0 （ 1.49） 0 0 0 1將 以 s為 比 例 因 子 來(lái) 擴(kuò) 展 或 縮 小 任 一 物 體 。 1.13 透 視 變 換 （ Perspective transformation） 假 設(shè) 由 一 個(gè) 簡(jiǎn) 單 透 鏡 把 一 個(gè) 物 體 形 成 的 像如 圖 1.17所 示 。 透 鏡 的 軸 沿 著 y 的 方 向 ， 焦 距為 f ， 物 體 上 的 一 個(gè) 點(diǎn) x, y, z 成 象 為 x/, y/, z/。y/ 表 示 象 距 ， 它 隨 著 物 距 y 而 變 化 。 如 果 在 通過(guò) y/ 而 垂 直 于 y 的 平 面 （ 照 相 機(jī) 的 底 片 ） 上 畫出 各 個(gè) 點(diǎn) ， 那 么 就 形 成 了 一 個(gè) 透 視 像 。 射 線 穿過(guò) 透 鏡 中 心 不 偏 轉(zhuǎn) ， 則 z / y = z / / y/ （ 1.50） x / y = x/ / y/ （ 1.51） 根 據(jù) 平 行 透 鏡 的 軸 的 射 線 通 過(guò) 焦 點(diǎn) ， 我 們可 以 寫 出 z / f = z/ / ( y/ + f ) （ 1.52） x / f = x/ / ( y/ + f ) （ 1.53）x/ y/ 和 z/ 是 負(fù) 數(shù) ， 而 f 是 正 數(shù) 。 用 式 （ 1.50）和 式 （ 1.52） 消 去 y/ ， 得 z / f = z / ( z/ y / z + f ) （ 1.54） z yx 0( x, y, z ) ( x, y, z ) f 圖 1.17 透 視 變 換 求 出 x/ = x ( 1y/f ) （ 1.55） y/ = y ( 1y/f ) （ 1.56） z/ = z ( 1y/f ) （ 1.57）齊 次 變 換 p 能 導(dǎo) 出 同 樣 結(jié) 果 ， 變 換 p 是 1 0 0 0 0 1 0 0 p = 0 0 1 0 （ 1.58） 0 -1/f 0 1任 何 一 點(diǎn) x i + y j + z k 變 換 為 x 1 0 0 0 x y 0 1 0 0 y z = 0 0 1 0 z （ 1.59） 1y/f 0 -1/f 0 1 1 用 比 例 因 子 1 y / f 除 得 到 的 象 點(diǎn) x/, y/, z/ 有 x /(1y/f) i + y/(1y/f) j + z/(1y/f) k （ 1.60） 這 個(gè) 結(jié) 果 與 前 面 利 用 透 視 原 理 的 結(jié) 果 完 全 相 同 。 在 p 變 換 的 第 二 列 最 底 一 元 素 為1/f， 則 導(dǎo) 出 一 個(gè) 沿 著 y 軸 的 一 透 視 變 換 。 如 果 1/f 是 第 三 列 最 底 一 項(xiàng) ， 那 就 是 沿 z 軸 的 透 視 變 換 。 1.14 變 換 方 程 （ Transform equations） 研 究 一 下 圖 1.18描 述 的 一 個(gè) 物 體 與 機(jī) 械 手 情況 ， 機(jī) 械 手 用 變 換 Z 相 對(duì) 于 基 坐 標(biāo) 系 被 定 位 。機(jī) 械 手 的 端 點(diǎn) 用 變 換 ZT6 來(lái) 描 述 ， 而 末 端 執(zhí) 行 器用 變 換 T6E 來(lái) 描 述 。 物 體 用 變 換 B 相 對(duì) 于 基 坐標(biāo) 系 被 定 位 。 最 后 ， 機(jī) 械 手 末 端 抓 手 用 變 換 BG相 對(duì) 于 物 體 被 定 位 。 末 端 抓 手 位 置 的 描 述 有 兩 種方 式 ， 一 種 是 相 對(duì) 于 物 體 的 描 述 ， 一 種 是 相 對(duì) 于機(jī) 械 手 的 描 述 。 由 于 兩 種 方 式 描 述 的 是 同 一 個(gè)點(diǎn) ， 我 們 可 以 把 這 個(gè) 描 述 等 同 起 來(lái) ， 得 到 Z ZT6 T6E = B BG （ 1.61） 這 個(gè) 方 程 可 以 用 有 向 變 換 圖 來(lái) 表 示 （ 見(jiàn) 圖1.19） 。 圖 的 每 一 段 弧 表 示 一 個(gè) 變 換 。 從 它 的 定義 的 坐 標(biāo) 系 向 外 指 向 。 用 Z-1左 乘 和 用 E-1右 乘 方 程 （ 1.61） ， 得 到 T6 = Z-1 B G E-1 （ 1.62） 0 E GBZ T6 z yx 圖 1.18 一 個(gè) 物 體 與 機(jī) 械 手 圖 1.19 有 向 變 換 圖 GBET6 Z 0 從 有 向 變 換 圖 上 我 們 可 以 直 接 得 到 上 述 結(jié) 果 ， 從 T6弧 線 的 尾 部 開(kāi) 始 ， 沿 著 圖 形 順 時(shí)針 依 次 列 出 各 個(gè) 變 換 ， 直 到 T6弧 的 箭 頭 為 止 。 在 逆 變 換 時(shí) ， 我 們 從 T6弧 的 箭 頭 開(kāi) 始 ， 按逆 時(shí) 針 方 向 依 次 列 出 各 個(gè) 變 換 ， 直 到 T6弧 的 起 始 點(diǎn) 為 止 ， 則 可 得 到 T6的 逆 T6-1 = E G-1 B-1 Z （ 1.63） 對(duì) 上 式 求 逆 得 到 與 式 （ 1.62） 完 全 相 同 的 結(jié) 果 。 作 為 進(jìn) 一 步 的 例 子 ， 假 設(shè) 一 個(gè) 物 體 B 的 位 置 不 知 道 ， 但 機(jī) 械 手 移 動(dòng) ， 使 得 末 端 抓手 正 好 定 位 在 物 體 上 面 。 然 后 用 G-1 右 乘 式 （ 1.61） 求 出 B 。 或 者 在 有 向 變 換 圖 中 從 B 的 尾 部 沿 著 逆 時(shí) 針 方 向 到 達(dá) 弧 B 的 箭 頭 ， 直 接 得 到 同 樣 結(jié) 果 。 B = Z T 6 E G-1 （ 1.64） 同 樣 ， 我 們 可 以 用 有 向 變 換 圖 求 出 變 換 的 連 接 組 。 例 如 Z T6 = B G E-1 （ 1.65） 用 有 向 變 換 圖 簡(jiǎn) 化 了 變 換 方 程 的 求 解 ， 可 以 直 接 寫 出 變 換 結(jié) 果 。 為 了 避 免 畫 圓 ， 我們 用 圖 1.20所 示 的 形 式 表 示 這 個(gè) 變 換 圖 ， 其 中 虛 線 表 示 那 兩 個(gè) 節(jié) 點(diǎn) 是 被 連 在 一 起 的 ， 中間 各 垂 線 段 表 示 相 對(duì) 坐 標(biāo) 系 。 BGET 6Z圖 1.20 有 向 變 換 圖 的 另 一 種 形 式 1.15 小 結(jié) （ Summary） l 齊 次 變 換 可 以 用 來(lái) 描 述 空 間 坐 標(biāo) 系 的 位 置 與 方 向 。 如 果 坐 標(biāo) 系 被 固定 在 物 體 或 機(jī) 械 手 連 桿 上 ， 那 么 該 物 體 或 機(jī) 械 手 的 位 置 與 方 向 同 樣很 容 易 被 描 述 。l 物 體 A相 對(duì) 于 物 體 B的 齊 次 變 換 可 以 求 其 逆 ， 來(lái) 獲 得 物 體 B相 對(duì) 于 物體 A的 描 述 。 l 變 換 可 以 表 示 為 旋 轉(zhuǎn) 變 換 和 /或 平 移 變 換 的 乘 積 。 如 果 變 換 是 從 左 到右 ， 那 么 旋 轉(zhuǎn) 和 /或 平 移 是 相 對(duì) 于 當(dāng) 前 的 坐 標(biāo) 系 。 如 果 變 換 是 從 右 到左 ， 那 么 旋 轉(zhuǎn) 和 /或 平 移 是 相 對(duì) 于 參 考 坐 標(biāo) 系 進(jìn) 行 。l 齊 次 變 換 用 正 交 分 量 來(lái) 描 述 坐 標(biāo) 系 ， 即 用 角 度 的 正 弦 和 余 弦 。 這 種描 述 可 與 旋 轉(zhuǎn) 聯(lián) 系 起 來(lái) 。 在 一 般 性 旋 轉(zhuǎn) 的 情 況 下 ， 旋 轉(zhuǎn) 是 繞 任 意 向量 旋 轉(zhuǎn) 角 。