機器人的空間描述與坐標變換

1 第二章 機器人的空間描述和坐標變換 2.1 位姿和坐標系描述 2.2平移和旋轉(zhuǎn)坐標系映射 2.3平移和旋轉(zhuǎn)齊次坐標變換 2.4物體的變換和變換方程 2.5通用旋轉(zhuǎn)變換 2 zyxA pppp 2-1圖 位 置 表 示2.1位 置 方 位 表 示 與 坐 標 系 描 述 n1.位 置 描 述 矢 量 Ap 表 示 箭 頭 指 向 點 的 位 置 矢 量 ， 其中 右 上 角 標 “A”表 示 該 點 是 用 A坐 標 系 描 述的 。 （ 2-2） n2.方 位 描 述 坐 標 系 B與 機 械 手 末 端 工 具 固 連 ， 工 具 的 姿 態(tài)可 以 由 坐 標 系 B的 方 向 來 描 述 。 而 坐 標 系 B的 方向 可 以 用 沿 三 個 坐 標 軸 的 單 位 矢 量 來 表 示 333231 232221 131211 rrr rrr rrrR BABABAAB ZYX 圖 2-2方 位 表示 （ 2-1） 旋 轉(zhuǎn) 矩 陣 描 述 坐 標 系 B的 姿 態(tài) ， 矢 量 描 述 坐 標 系 B的 原 點 位置 。 3 BoAAB RB p3.位 姿 描 述 固 連 坐 標 系 把 剛 體 位 姿 描 述 問 題 轉(zhuǎn) 化 為 坐 標 系 的 描 述 問 題 。 圖 2-3中 坐 標 系 B可 以 在 固 定 坐 標 系 A中 描 述 為 （ 2-3）RAB BoAP 4 1.平 移 坐 標 變 換 圖 2-3平 移 變 換 BP為 坐 標 系 B描 述 的 某 一 空 間 位置 ， 我 們 也 可 以 用 AP（ 坐 標 系 A） 描述 同 一 空 間 位 置 。 因 為 兩 個 坐 標 系 具 有相 同 的 姿 態(tài) ， 同 一 個 點 在 不 同 坐 標 系 下的 描 述 滿 足 以 下 關 系 A B A Bo P P P （ 2-4） 2.2平 移 和 旋 轉(zhuǎn) 坐 標 系 映射 旋 轉(zhuǎn) 坐 標 變 換 的 任 務 是 已 知 坐 標 系 B描 述 的一 個 點 的 位 置 矢 量 BP和 旋 轉(zhuǎn) 矩 陣 ， 求 在 坐 標系 A下 描 述 同 一 個 點 的 位 置 矢 量 AP。 5 2.旋 轉(zhuǎn) 坐 標 變換 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旋 轉(zhuǎn) 變 換 式 （ 2-6） 即 為 我 們 要 求 的 旋 轉(zhuǎn) 變 換 關 系 ， 該 變 換 是 通 過 兩 個 坐標 系 之 間 的 旋 轉(zhuǎn) 變 換 實 現(xiàn) 的 。 6 3.復 合 變 換 圖 2-5復 合 變 換 如 果 兩 個 坐 標 系 之 間 即 存 在 平 移又 存 在 旋 轉(zhuǎn) ， 如 何 計 算 同 一 個 空 間 點在 兩 個 坐 標 系 下 描 述 的 變 換 關 系 ？ 為 了 得 到 位 置 矢 量 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維 的 數(shù) 值 恒 為 1， 我 們 仍 然 用 原來 的 符 號 表 示 4維 位 置 矢 量 并 采 用 以 下 符 號 表 示 坐 標 變 換 矩 陣 10 BoAABAB RT P （ 2-10）PP BABA T （ 2-11）TAB 齊 次 坐 標 變 換 的 主 要 作 用 是 表 達 簡 潔 ， 同 時 在 表 示 多 個 坐 標 變 換的 時 候 比 較 方 便 。1.齊 次 變 換 8 2.齊 次 變 換 算 子 在 機 器 人 學 中 還 經(jīng) 常 用 到 下 面 的 變 換 ， 如 圖 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 同 樣 ， 我 們 可 以 研 究 矢 量 在 同 一 坐 標 系 下 的 旋 轉(zhuǎn)變 換 ， 如 圖 2 -9 ， AP1繞 Z軸 轉(zhuǎn) 角 得 到 AP2。 則 圖 2-7旋 轉(zhuǎn) 算 子12 ),( PP AA zRot （ 2 -2 0 ）Rot(z,)稱 為 旋 轉(zhuǎn) 算 子 ， 其 表 達 式 為 1000 0100 00 00),( cs sczRot （ 2 -2 1 ）同 理 ， 可 以 得 到 繞 X軸 和 Y軸 的 旋 轉(zhuǎn) 算 子 1000 00 00 0001),( cs scxRot 1000 00 0010 00),( cs scyRot 10 定 義 了 平 移 算 子 和 旋 轉(zhuǎn) 算 子 以 后 ， 可 以 將 它 們 復 合 實 現(xiàn) 復 雜 的 映 射關 系 。 變 換 算 子 與 前 面 介 紹 的 坐 標 變 換 矩 陣 形 式 完 全 相 同 ， 因 為 所 有 描述 均 在 同 一 坐 標 系 下 ， 所 以 不 需 上 下 標 描 述 （ 坐 標 系 ） 。2 1A AP T P （ 2-23）TAB AB RBoAP PP BABA T2 1A AP T P齊 次 坐 標 變 換 總 結 ： 表 示 坐 標 系 B在 坐 標 系 A下 的 描 述 ， 的 各 列 是 坐 標 系B三 個 坐 標 軸 方 向 的 單 位 矢 量 ， 而 表 示 坐 標 系 B原 點 位 置。2. 它 是 不 同 坐 標 系 間 的 坐 標 變 換 。 如3.它 是 同 一 坐 標 系 內(nèi) 的 變 換 算 子 。 齊 次 坐 標 變 換 是 復 雜 空 間 變 換 的 基 礎 ， 必 須 認 真 理 解 和 掌 握 。 具 體 應用 的 關 鍵 是 理 解 它 代 表 的 是 上 面 三 種 含 義 的 哪 一 種 ， 而 不 是 簡 單 的 套 用公 式 ！1. 它 是 坐 標 系 的 描 述 。 如 圖 2-10表 示 的 三 個 坐 標 系 ， 已 知 坐 標 系A、 B和 C之 間 的 變 換 矩 陣 和 位 置矢 量 CP， 求 在 坐 標 系 A下 表 示 同 一 個 點 的位 置 矢 量 AP。 11 3.復 合 變 換 復 合 變 換 主 要 有 兩 種 應 用 形 式 ， 一 種 是 建 立 了 多 個 坐 標 系 描 述 機 器 人的 位 姿 ， 任 務 是 確 定 不 同 坐 標 系 下 對 同 一 個 量 描 述 之 間 的 關 系 ； 另 一 種 是一 個 空 間 點 在 同 一 個 坐 標 系 內(nèi) 順 序 經(jīng) 過 多 次 平 移 或 旋 轉(zhuǎn) 變 換 ， 任 務 是 確 定多 次 變 換 后 點 的 位 置 。 圖 2-10 復 合 坐 標 變 換 TAB TBCPP CBCB T PPP CBCABBABA TTT （ 2-24）（ 2-25）TTT BCABAC 根 據(jù) 坐 標 變 換 的 定 義 得 （ 2-26） 12 (a) ZY順 序 旋 轉(zhuǎn) (b) Y Z順 序 旋 轉(zhuǎn)圖 2-11旋 轉(zhuǎn) 順 序 對變 換 結 果 影 響例 2 -3 已 知 點 u=7i+3j+2k， 先 對 它 進 行 繞 Z軸 旋 轉(zhuǎn) 9 0 o的 變 換 得 點 v， 再 對 點 v進 行 繞 Y軸 旋 轉(zhuǎn) 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) 為 復 合 旋 轉(zhuǎn) 算 子 。 13注 ： 固 定 坐 標 系 變 換 ， 矩 陣 乘 的 順 序 “ 自 右 向 左 ” 如 果 改 變 旋 轉(zhuǎn) 順 序 ， 先 對 它 進 行 繞 y軸 旋 轉(zhuǎn) 90o， 再 繞 z軸 旋 轉(zhuǎn) 90o， 結果 如 圖 2-11b所 示 。 比 較 圖 2-11a和 圖 2-11b可 以 發(fā) 現(xiàn) 最 后 的 結 果 并 不 相 同 ，即 旋 轉(zhuǎn) 順 序 影 響 變 換 結 果 。從 數(shù) 學 角 度 解 釋 就 是 矩 陣 乘 法 不 滿 足 交 換 率 ， Rot(y,90o) Rot(z,90o) Rot(z,90o) Rot(y,90o)。 和 ， 求 和給 定 計 算 142.4物 體 的 變 換 和 變 換 方 程TABTBA已 知 坐 標 系 B相 對 坐 標 系 A的 描 述求 坐 標 系 A相 對 坐 標 系 B的 描 述一 種 直 接 的 方 法 是 矩 陣 求 逆 ， 另 一 種 方 法 是 根 據(jù) 變 換 矩 陣 的 特 點 直接 得 出 逆 變 換 。 后 一 種 方 法 更 簡 單 方 便 。 即 齊 次 變 換 的 求 逆 問 題 。TAB TBA 等 價 為 ： 已 知 RAB BoAP RBA AoBP 是 坐 標 系 B的 原 點 在 坐 標 系 B中 的 描 述 ， 顯 然 為 零 矢 量 。由 （ 2 -2 8 ） 式 得 15根 據(jù) 前 面 的 討 論 ， 旋 轉(zhuǎn) 矩 陣 關 系 為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）逆 變 換 可 以 直 接 用 正 變 換 的 旋 轉(zhuǎn) 矩 陣 和 平 移 矩 陣 表 示 10 BoATABTABBA RRT P （ 2-30） 16 A沿 xA平 移 3個 單 位 ， 再 繞 新 的 zA 軸 轉(zhuǎn) 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軸 轉(zhuǎn) 90o再 繞 新 xB軸 轉(zhuǎn) 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軸 轉(zhuǎn) 90 ， 再 繞 新 xA軸 轉(zhuǎn) -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下 表示 得 旋 轉(zhuǎn) 矩 陣 ， 平 移 矢 量 為 坐 標 系 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.繞 任 意 軸 旋 轉(zhuǎn) 變 換下 面 討 論 繞 任 意 軸 f 旋 轉(zhuǎn) 矩 陣 ， 軸 在 坐 標 系 A下 表 示 為以 f 為 Z 軸 建 立 與 A固 連 的 坐 標 系 C用 n、 o和 f表 示 坐 標 系 C三 個 坐 標軸 的 單 位 矢 量 ， 在 坐 標 系 A下 表 示 為 圖 2-18繞 任 意 軸 旋 轉(zhuǎn) 變 換 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旋 轉(zhuǎn)等 價 于 繞 ZC旋 轉(zhuǎn) 。 為 此 我 們 先 將 Ap在 坐 標 系 C下表 示 ， 再 繞 ZC旋 轉(zhuǎn) 角 ， 最 后 再 把 旋 轉(zhuǎn) 得 到 的 矢 量用 坐 標 系 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通 用 旋 轉(zhuǎn) 變 換 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 根 據(jù) 坐 標 系 的 右 手 規(guī) 則 知 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 再 根 據(jù) 旋 轉(zhuǎn) 矩 陣 的 正 交 性 可 以 得 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.等 效 轉(zhuǎn) 軸 與 轉(zhuǎn) 角 前 面 討 論 了 給 定 轉(zhuǎn) 軸 和 轉(zhuǎn) 角 可 以 得 到 旋 轉(zhuǎn) 矩 陣 ， 那 么 是 否 任 意 給 定 的 旋 轉(zhuǎn) 矩陣 都 可 以 確 定 等 效 的 轉(zhuǎn) 軸 f和 轉(zhuǎn) 角 哪 ？ 也 就 是 兩 個 坐 標 原 點 重 合 的 坐 標 系 可 以 通過 繞 固 定 軸 轉(zhuǎn) 一 定 的 角 度 來 實 現(xiàn) 從 一 個 坐 標 系 轉(zhuǎn) 換 到 另 一 個 坐 標 系 。 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 正 向 旋 轉(zhuǎn) ， 且 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 分 量 的 值 在 應 用 中 需 要 注 意 的 是 ， 當 轉(zhuǎn) 角 的 值 接 近 0o或 180o時 ， 方 向 矢 量 f 各 分 量 的 值 計 算 出 現(xiàn) 問 題 ， 屬 于 奇 異 情 況 。