工業(yè)機(jī)器人運(yùn)動(dòng)軌跡

8.1 引 言 8.2 目 標(biāo) 物 體 的 描 述 8.3 任 務(wù) 的 描 述 8.4 視 覺8.5 程 序 8.6 傳 送 帶 跟 蹤8.7 位 置 之 間 的 運(yùn) 動(dòng) 8.8 關(guān) 節(jié) 運(yùn) 動(dòng)8.9 笛 卡 爾 運(yùn) 動(dòng) 8.10 本 章 小 結(jié)第 八 章 運(yùn) 動(dòng) 軌 跡工 業(yè) 機(jī) 器 人 http:/ 本 章 是 機(jī) 器 人 運(yùn) 動(dòng) 控 制 的 基 礎(chǔ) ， 它 分 為 四 個(gè) 主 要 部 分 : 第 一 部 分 ： 利 用 齊 次 坐 標(biāo) 變 換 構(gòu) 造 任 務(wù) 第 二 部 分 ： 基 于 時(shí) 間 坐 標(biāo) 軌 跡 的 運(yùn) 動(dòng) 控 制 描 述 第 三 部 分 ： 關(guān) 節(jié) 坐 標(biāo) 運(yùn) 動(dòng) 的 描 述 第 四 部 分 ： 笛 卡 兒 運(yùn) 動(dòng) 控 制 描 述8.1 引 言 （ Introduction） R=0.5x z y6圖 8.1 銷 釘 的 描 述8.2 目 標(biāo) 物 體 的 描 述 （ Object Description） 利 用 齊 次 變 換 來(lái) 描 述 一 個(gè) 任 務(wù) 。 任 務(wù) 內(nèi) 容 是 抓 取 如 圖 8.1所 示的 一 些 銷 釘 ， 然 后 把 它 們 插 入 一 個(gè) 裝 配 部 件 的 孔 中 ( 見 圖 8.2)。 圖 8.2 任 務(wù) 的 描 述 8.3 任 務(wù) 的 描 述 （ Task Description）z 規(guī) 定 機(jī) 械 手 末 端 執(zhí) 行 器 （ 手 爪 ） 的 一 系 列 位 置 Pn(見 圖 8.3)， 就 能 把 這 一 任 務(wù)描 述 為 相 應(yīng) 于 這 些 編 號(hào) 位 置 的 機(jī) 械 手 運(yùn) 動(dòng) 和 動(dòng) 作 的 序 列 。 圖 8.3 末 端 執(zhí) 行 器 的 位 置 MOVE P1 接 近 銷 釘MOVE P2 移 動(dòng) 到 銷 釘 的 位 置G RASP 抓 住 銷 釘 MOVE P3 垂 直 提 起 銷 釘 MOVE P4 按 一 定 角 度 接 近 孔 眼MOVE P5 接 觸 到 孔 眼 時(shí) 停 止MOVE P6 調(diào) 整 銷 釘 的 位 置MOVE P7 插 入 銷 釘RELEASE 松 開 銷 釘MOVE P8 離 開 下 面 通 過(guò) 規(guī) 定 機(jī) 械 手 的 結(jié) 構(gòu) 來(lái) 確 定 任 務(wù) 結(jié) 構(gòu) 。 我 們 用 三 個(gè) 變 換 的乘 積 描 述 機(jī) 械 手 ， 從 而 任 務(wù) 描 述 中 的 位 置 就 由 下 式 取 代 MOVE pn = MOVE Z T6 E (8.1) 其 中 Z ： 表 示 機(jī) 械 手 相 對(duì) 于 任 務(wù) 坐 標(biāo) 系 的 位 置 ； T6： 表 示 機(jī) 械 手 末 端 相 對(duì) 于 機(jī) 械 手 坐 標(biāo) 的 位 置 ； E ： 表 示 末 端 執(zhí) 行 器 （ 手 爪 ） 相 對(duì) 于 機(jī) 械 手 末 端 坐 標(biāo) 的 位 置 。 按 照 上 述 描 述 ， 機(jī) 械 手 的 位 置 由 Z來(lái) 確 定 ， 任 務(wù) 的 執(zhí) 行 就 是改 變 抓 手 的 位 置 。 現(xiàn) 在 利 用 下 列 符 號(hào) 來(lái) 描 述 任 務(wù) 的 變 化 : P 銷 釘 在 基 坐 標(biāo) 中 的 位 置 ； H 帶 有 兩 孔 眼 的 金 屬 塊 在 基 坐 標(biāo) 中 的 位 置 ； H HRi 金 屬 塊 上 第 i個(gè) 孔 相 對(duì) H坐 標(biāo) 系 的 位 置 ； P PG 抓 取 銷 釘 的 抓 手 相 對(duì) 于 銷 釘 的 位 置 ； P PA 抓 手 接 近 銷 釘 ； P PD 抓 手 提 起 銷 釘 ； HR PHA 銷 釘 接 近 第 i個(gè) 孔 眼 ； HR PCH 銷 釘 接 觸 孔 眼 ； HR PAL 銷 釘 開 始 插 入 ； HR PN 插 入 后 的 銷 釘 。 現(xiàn) 在 ， 任 務(wù) 可 由 一 系 列 變 換 式 來(lái) 描 述 ， 由 此 解 出 機(jī) 械 手 的 控制 輸 入 T6 ， 這 些 變 換 式 如 下 : P1: Z T6 E = P PA 接 近 銷 釘 P2: Z T6 E = P PG 到 達(dá) 抓 取 銷 釘 的 位 置 GRASP 抓 取 銷 釘 P3: Z T6 E = P PD PG 提 起 銷 釘 P4: Z T6 E = H HRi PHA PG 接 近 第 i個(gè) 孔 眼 P5: Z T 6 E = H HRi PCH PG 接 觸 第 i個(gè) 孔 眼 P6: Z T6 E = H HRi PAL PG 插 入 銷 釘 P7: Z T6 E = H HRi PN PG 插 入 完 成 RELEASE 松 開 手 爪 P8: ZT6E = H HRi PN PA 回 到 起 始 位 置 手 爪 相 對(duì) 于 銷釘 的 位 置 P手 爪 相 對(duì) 于 第 i個(gè)孔 眼 的 位 置 HRi 任 務(wù) 位 置變 換 圖 如 圖 8.4所 示 。 盡 管 這 樣表 示 可 能 顯 得復(fù) 雜 , 但 是 說(shuō)明 了 任 務(wù) 的 基本 結(jié) 構(gòu) 。 而 且每 一 個(gè) 變 換 表示 了 一 個(gè) 獨(dú) 立的 情 況 。 圖 8.4 任 務(wù) 位 置 變 換 圖 相 應(yīng) 的 P , H和 Z坐 標(biāo)系 如 圖 8.5所 示 。 由 圖 8.5可 知 ， Z為機(jī) 械 手 坐 標(biāo) 系 ， 它 定 位在 肩 關(guān) 節(jié) 上 , 因 此 工 作 坐標(biāo) 系 （ 基 坐 標(biāo) 系 ） 位 于機(jī) 械 手 坐 標(biāo) 的 位 置 為 T6 p z = -50 由 于 機(jī) 械 手 不 能 到達(dá) 它 自 己 的 基 座 ， 所 以我 們 把 它 放 在 基 坐 標(biāo) 系原 點(diǎn) 的 后 面 ， 這 樣 T6 px= 30 。 使 T6py = 0 ,并 且 保持 兩 個(gè) 坐 標(biāo) 系 的 平 行 。 圖 8.5 任 務(wù) 坐 標(biāo) 系 P, H 和 Z 1000 50100 0010 30001Z由 圖 8.5可 知 : (8.2) 下 面 通 過(guò) 相 對(duì) 于 機(jī) 械 手末 端 的 變 換 來(lái) 定 義 末 端 執(zhí) 行器 , 我 們 沿 著 這 樣 的 表 示 習(xí)慣 : 末 端 執(zhí) 行 器 的 z軸 指 向 執(zhí)行 任 務(wù) 的 方 向 ， 而 y軸 表 示 手爪 的 開 合 方 向 ， 于 是 如 圖 8.6所 示 的 抓 手 就 可 描 述 為 圖 8.6 手 爪 變 換 1000 10100 0010 00016 ET (8.3) 我 們 已 經(jīng) 在 圖 8.1中 描 述了 銷 釘 ， 現(xiàn) 在 再 看 一 下 帶 有兩 個(gè) 孔 眼 的 金 屬 塊 H。 H的 正 視 圖 如 圖 8.7所 示 ，借 助 于 變 換 矩 陣 HRi（ i 1、2， 是 孔 眼 的 序 號(hào) ） 來(lái) 描 述它 的 特 征 。 1000 15010 0100 10001HR1 = HR2 = 1000 5010 0100 10001(8.4) (8.5)圖 8.7 帶 有 兩 個(gè) 孔 眼 的 金 屬 塊 HR1 HR2y xxx zy10 5 10H注 意 ： 式 （ 8.4） 和 式 （ 8.5） 是 分 別 沿 H坐 標(biāo) 的 x軸 旋 轉(zhuǎn) 90 再 平 移 后 得 到 。 最 重 要 的 變 換 是 銷 釘 插 入 一 個(gè) 孔眼 (見 圖 8.8)。 銷 釘 的 z軸 必 須 與 孔 眼 的軸 一 致 。 由 于 銷 釘 具 有 圓 柱 的 對(duì) 稱 性 ，x、 y軸 的 方 向 就 可 任 意 了 。 最 后 一 個(gè) 變 換 必 須 按 照 手 爪 在 銷釘 上 的 部 位 來(lái) 確 定 （ 見 圖 8.9） 。 1000 4100 0010 0001PN HR xyHR x z yzPN圖 8.8 銷 釘 插 入 孔 眼 xx z z P yyPG 5圖 8.9 手 爪 在 銷 釘 上 的 位 置 1000 57.007.0 0010 07.007.0PGP (8.6)(8.7) 現(xiàn) 在 我 們 通 過(guò) 示 教 方 式 利 用 機(jī) 械 手 來(lái) 確 定 前 面 的 變 換 關(guān) 系 。 將 末 端 執(zhí) 行器 放 在 銷 釘 上 面 ， 處 于 它 的 抓 取 位 置 （ 圖 8.3中 的 P2） ， 可 得 到 下 列 變 換 式 。 Z T6E = P PG （ P2） (8.8) 上 式 可 確 定 P P = Z T6 E PG-1 (8.9) 手 爪 返 回 到 靠 近 銷 釘 的 位 置 P1， 于 是 有 Z T 6 E = P PA （ P1） (8.10) 從 而 確 定 了 PA PA = P-1 ZT6 E (8.11)關(guān) 于 P的 起 始 點(diǎn) 可 這 樣 確 定 ： 把 手 爪 中 的 銷 釘 提 起 ， 移 到 起 始 位 置 P3， 于 是 有 Z T6 E = P PD PG （ P3） (8.12)由 上 式 確 定 了 PD PD = P-1 Z T6 E PG-1 (8.13) 金 屬 塊 H的 位 置 由 下 式 確 定Z T6 E = H HR1 PN PG （ P7） （ 8.14）解 出 HH = Z T6 E（ HR1 PN PG） -1 （ 8.15）銷 釘 開 始 插 入 ， PAL（ 見 圖 5.10） 由 下 式 確 定 Z T6 E = H HR1 PAL PG （ P6） (8.16）從 而 PAL =（ H HR 1） -1Z T6 E PG-1 （ 8.17）接 觸 孔 眼 的 銷 釘 位 置 PCH以 及 接 近 觸 點(diǎn) 的 點(diǎn)PHA可 確 定 為PCH =（ HHR1） -1 Z T6 E PG-1 （ P5） (8.18）PHA =（ HHR1） -1 Z T6 E PG-1 （ P4） （ 8.19）這 樣 就 定 義 所 有 的 變 換 。 PHA PCHPAL X yzHR圖 8.10 接 近 接 觸 和 開 始 插 入 問(wèn) 題 ： 式 （ 8.17） （ 8.19） 三 個(gè) 表達(dá) 式 完 全 相 同 ， 為 什 么 能 描 述 三 個(gè)不 同 的 工 位 點(diǎn) ？ 在 求 取 任 務(wù) 執(zhí) 行 過(guò) 程 中 的 銷 釘 位 置 時(shí) ， 我 們 是 把 機(jī) 械 手 放 在 銷 釘 的 上方 ， 處 于 正 確 的 抓 取 位 置 ， 然 后 求 解 變 換 式 P。 在 實(shí) 際 系 統(tǒng) 中 ， 我 們 還 可 以通 過(guò) 視 覺 獲 得 銷 釘 等 物 體 的 位 置 。 假 如 有 一 個(gè) 位 置 固 定 的 攝 像 機(jī) ， 它 能 把銷 釘 的 方 位 表 示 為 相 對(duì) 于 它 自 身 的 坐 標(biāo) 系 的 一 個(gè) 齊 次 變 換 , 比 如 說(shuō) PC， 那么 銷 釘 相 對(duì) 于 基 坐 標(biāo) 的 位 置 就 可 表 示 為 P = CAM PC (8.20) 其 中 CAM是 一 個(gè) 變 換 ， 表 示 攝 像 機(jī) 坐 標(biāo) 系 在 基 坐 標(biāo) 中 的 位 置 。 在 通 過(guò) 機(jī) 械 手 確 定 P之 后 ， CAM的 值 能 通 過(guò) 攝 取 一 幅 圖 象 來(lái) 得 到 PC，從 而 解 得 CAM = P PC -1 (8.21) 攝 像 機(jī) 的 輸 入 通 過(guò) READ讀 取 得 到 ， READ (CAMERA, PC) P: = CAM + PC 而 P是 要 被 確 定 的 ， 我 們 用 標(biāo) 識(shí) 符 來(lái) 表 示 程 序 中 的 變 換 ， “ + ”號(hào) 表 示 矩 陣相 乘 ， “ ” 號(hào) 表 示 與 逆 陣 相 乘 。 8.4 視 覺 （ Vision） 現(xiàn) 在 可 以 利 用 求 解 T6的 變 換 式 (見 圖 8.4)來(lái) 確 定 任 務(wù) 程 序 中 從 P1到 P8的 每 個(gè) 位 置 。 為 了 把 機(jī) 械 手 移 到 P1, 要 求 Z T6 E = P PA (8.22) 這 就 意 味 著 T6 = Z-1 P PA E-1 (8.23) 要 到 達(dá) P2則 需 Z T 6 E = P PG (8.24) 如 此 等 等 。 8.5 程 序 （ Program） 在 我 們 用 變 換 表 達(dá) 式 代 替 程 序 中 P1P8的 各 個(gè) 位 置 時(shí) ， 先 定 義 兩 個(gè) 變 量COORD和 TOOL, 它 分 別 表 示 工 作 坐 標(biāo) 系 的 一 般 表 達(dá) 式 和 抓 手 坐 標(biāo) 系 的 一 般表 達(dá) 式 。 然 后 , 把 所 有 的 位 置 表 達(dá) 式 寫 成 如 下 形 式 T6 TOOL = COORD POS (8.25) 對(duì) 于 第 一 步 運(yùn) 動(dòng) , COORD和 TOOL定 義 為 COORD: = -Z + P ; TOOL: = E ; 因 而 第 一 步 運(yùn) 動(dòng) 就 是 MOVE PA TOOL : = E; 固 定 抓 手FOR I : = 1, 2 DOBEGIN READ (CAMERA, PC) 銷 釘 位 置 P : = CAM + PC ; 建 立 P COORD : = -Z + P； 相 對(duì) 于 銷 釘 的 位 置 MOVE PA ; 接 近 MOVE PG; 移 至 銷 釘 上 方 GRASP ; 抓 取 TOOL : = E PC ; 抓 手 在 銷 釘 末 端 MOVE PD ; 起 始 點(diǎn) HT : = HRI; 孔 眼 的 位 置 COORD : = -Z+H+HT; 相 對(duì) 于 孔 眼 的 坐 標(biāo) MOVE PHA ; 接 近 孔 眼 MOVE PCH ; 接 觸 孔 眼 MOVE PAL ; 調(diào) 整 銷 釘 MOVE PN ; 插 入 銷 釘 RELEASE ; 松 開 銷 釘 COORD : = -Z+H+HT+PN； 相 對(duì) 于 銷 釘 的 坐 標(biāo) TOOL : = E MOVE PA 脫 離 銷 釘 END 8.6 傳 送 帶 跟 蹤 （ Conveyor tracking） 在 任 務(wù) 執(zhí) 行 過(guò) 程 中 , T6 變 成 傳 送 帶 變 量 的 函 數(shù) T6 = Z-1 CONV(S) OBJ F G E-1 (8.29) 如 果 不 斷 地 對(duì) T6求 值 , 并 轉(zhuǎn) 換 成 關(guān) 節(jié) 角 度 （ 解 逆 運(yùn) 動(dòng) 方程 ） , 同 時(shí) 機(jī) 械 手 跟 隨 這 些 角 度 , 那 么 機(jī) 械 手 就 會(huì) 跟 蹤 運(yùn) 動(dòng)著 的 物 體 。 如 果 傳 送 帶 停 止 運(yùn) 動(dòng) ， 機(jī) 械 手 也 會(huì) 停 止 動(dòng) 作 。 這樣 , 執(zhí) 行 任 務(wù) 的 程 序 就 可 寫 成 TOOL : = E 固 定 抓 手FOR I : = 1, 2 DO BEGIN READ(CAMERA,PC) ; 插 入 銷 釘 位 置 P : = -CONVS+CAM+P ; 建 立 P COORD : = -Z+CONVS+P ; 相 對(duì) 于 銷 釘 的 坐 標(biāo) MOVE PA; 接 近 MOVE PC ; 在 銷 釘 上 方 GRASP ; 抓 取 銷 釘 TOOL : = E PG ; 抓 手 在 銷 釘 末 端 MOVE PD ; 起 始 位 置 HT:=HRI ; 孔 眼 的 位 置COORD : = -Z+H+HT ; 相 對(duì) 于 孔 眼 的 坐 標(biāo)MOVE PHA ; 接 近 孔 眼MOVE PCH ; 接 觸 孔 眼MOVE PAI ; 調(diào) 整 銷 釘MOVE PN : 插 入RELEASE ; 松 開 銷 釘COORD : = -Z+H +HT+PN ; 相 對(duì) 于 銷 釘 的 坐 標(biāo)TOOL: = E ; MOVE PA ; 離 開 銷 釘 END ; 機(jī) 械 手 的 任 務(wù) 位 置 可 以 表 示 為 一 般 形 式 T6 TOOL = COORD POS (8.30) 其 中 T6 表 示 機(jī) 械 手 六 個(gè) 關(guān) 節(jié) 的 變 換 ; TOOL 變 換 表 達(dá) 式 ， 描 述 手 爪 端 點(diǎn) ， 或 運(yùn) 動(dòng) 受 到 控 制 的 物 體 ; COORD 變 換 表 達(dá) 式 ， 表 示 工 作 坐 標(biāo) 系 ; POS 其 余 的 變 換 表 達(dá) 式 ， 描 述 所 期 望 的 手 爪 端 點(diǎn) 或 物 體 的 位 置 ； 為 了 到 達(dá) 所 確 定 的 機(jī) 械 手 任 務(wù) 的 任 何 位 置 , 機(jī) 械 手 的 一 系 列 位 置 運(yùn) 動(dòng)可 由 下 列 方 程 確 定 T 6 TOOL1 = COORD1 POS1 T6 TOOL2 = COORD2 POS2 (8.31) T6 TOOL3 = COORD3 POS3 8.7 位 置 之 間 的 運(yùn) 動(dòng) （ Motion Between Positions） 雖 然 任 務(wù) 位 置 的 序 列 確 定 了 任 務(wù) ， 但 并 沒 有 說(shuō) 明 機(jī) 械 手 是 如 何 從 一 個(gè) 位置 移 動(dòng) 到 另 一 個(gè) 位 置 的 。 利 用 下 一 個(gè) 坐 標(biāo) 系 統(tǒng) 和 抓 手 ， 另 外 再 定 義 一 個(gè) POS變 換 ， 就 可 以 完 成 上 述 工 作 。 在 位 置 1有 1T6 TOOL1 = COORD1(s|t=0) 1POS1 （ 8.32） 下 面 利 用 終 點(diǎn) 位 置 TOOL2和 COORD2再 寫 一 個(gè) 變 換 表 達(dá) 式 1T6 TOOL2 = COORD2(s|t=0) 2POS1 （ 8.33） 由 這 兩 個(gè) 方 程 我 們 能 解 得 利 用 COORD2和 TOOL2定 義 的 對(duì) 于 位 置 1的 變 換 2POS1 2POS1 = COORD2-1(s|t=0) COORD1(s|t=0) 1POS1 TOOL1-1 TOOL2 （ 8.34） 于 是 ， 任 何 兩 點(diǎn) i和 i +1之 間 的 運(yùn) 動(dòng) 就 是 從 T6 = COORD2(s|t=0) 2POS1 TOOL2-1 （ 8.35） 到 T6 = COORD2(s|t=0) 2POS2TOOL2-1 （ 8.36） 的 運(yùn) 動(dòng) 。 注 意 ： 在 這 些 方 程 中 ， COORDi+1是 運(yùn) 動(dòng) 坐 標(biāo) 系 統(tǒng) 變 量 （ S） 的 函 數(shù) 。 當(dāng) 然 ， 要 把 機(jī) 械 手 從 一 個(gè) 位 置 移 到下 一 個(gè) 位 置 可 以 有 許 多 方 法 。 但 是 ， 每一 個(gè) 實(shí) 際 系 統(tǒng) 必 須 提 供 連 續(xù) 的 位 置 和 連續(xù) 的 速 度 。 為 了 防 止 振 蕩 和 蠕 動(dòng) ， 還 要求 連 續(xù) 的 加 速 度 。 以 從 位 置 到 位 置 的 運(yùn) 動(dòng) 時(shí) 間 Ti為 基準(zhǔn) ， 我 們 可 以 畫 出 直 線 坐 標(biāo) 、 角 坐 標(biāo) 或者 關(guān) 節(jié) 坐 標(biāo) ， 作 為 時(shí) 間 的 函 數(shù) 。 圖 8.11是 一 個(gè) 典 型 的 坐 標(biāo) 圖 。 由 于 并 不 需 要 在 每 一 個(gè) 中 間 點(diǎn) 停 留 ?？?以 先 對(duì) 這 條 軌 跡 進(jìn) 行 線 性 近 似 ， 如 圖8.12所 示 。 但 是 ， 由 于 速 度 和 加 速 度 在 所 有 的軌 跡 定 義 點(diǎn) 上 都 是 不 連 續(xù) 的 。 因 而 我 們并 不 能 實(shí) 現(xiàn) 這 樣 一 條 線 性 軌 跡 。 t圖 8.11 起 動(dòng) 和 停 止 運(yùn) 動(dòng) t圖 8.12 分 段 線 性 近 似 A- B T1 T2C DC ttacc taccB tacc tacc0 圖 8.13 路 徑 過(guò) 渡 在 對(duì) 軌 跡 路 徑 段 的 時(shí) 間 進(jìn) 行 估 計(jì) 時(shí) ， 我 們 假 設(shè) 在 時(shí) 間 tacc內(nèi) 把 機(jī) 械 手 從 靜止 狀 態(tài) 加 速 度 到 最 大 速 度 。 在 路 徑 段 之 間 的 過(guò) 渡 時(shí) 間 為 2tacc。 如 果 把 f（ t） 選擇 為 一 個(gè) 多 項(xiàng) 式 ， 由 于 過(guò) 渡 階 段 的 對(duì) 稱 性 ， 實(shí) 際 上 只 需 要 一 個(gè) 四 次 多 項(xiàng) 式 ： q = a4t4 + a3t3 + a2t2 + a1t + a0 (8.37) 其 中 q表 示 廣 義 的 位 置 ： 關(guān) 節(jié) 的 直 線 位 移 或 角 位 移 。 通 過(guò) 求 導(dǎo) 并 運(yùn) 用 約 束 條 件 ，求 出 式 （ 8.37） 中 各 項(xiàng) 系 數(shù) ， 可 以 得 到 表 示 過(guò) 渡 期 間 的 位 置 q， 速 度 以 及 加速 度 的 函 數(shù) 。 BBhBhhBTtCq acc 2)2)( 21q qaccacc tBhhBTtCq 12)5.1)( 21 (8.38) (8.39) (8.40)accacc t hhBTtCq 21 3)1)( 約束條件 其 中 C = C B (8.41) B = A B (8.42) (8.43)在 t = tacc時(shí) 刻 過(guò) 渡 結(jié) 束 。 已 知 值 由 下 列 各 式 給 定 q =C h + B (8.44) accaccttth 2 1/TCq (8.45)0q (8.46)1Tth (8.47) 根 據(jù) 確 定 的 加 速 時(shí) 間 tacc, 我 們 可 以 得 到 使 整 個(gè) 機(jī) 械 手 能 實(shí) 現(xiàn) 的 坐標(biāo) 軌 跡 （ 見 圖 8.14） 。 規(guī) 劃 機(jī) 械 手 的 全 部 運(yùn) 動(dòng) 并 非 必 要 ， 我 們 只 需 要提 前 考 慮 一 個(gè) 位 置 。 當(dāng) 時(shí) 間 t = T1 tacc時(shí) ， 就 根 據(jù) 下 列 賦 值 句 ， 以 便 開始 向 下 一 路 徑 段 過(guò) 渡 。 T1： = T2 ； A ： = X ； 當(dāng) 前 位 置 B ： = C ； C ： = D ； C： = C B ； B： = A B ； t : = -TACC ; 重 新 啟 動(dòng) 時(shí) 刻 時(shí) 間圖 8.14 平 滑 軌 跡 運(yùn) 動(dòng) 本 節(jié) ， 我 們 要 考 察 機(jī) 械 手 的 關(guān) 節(jié) 運(yùn) 動(dòng) ， 此 時(shí) ， 運(yùn) 動(dòng) 變 量 q、 和 由式 （ 8.38） （ 8.47） 給 定 ， 它 們 表 示 關(guān) 節(jié) 坐 標(biāo) 。 在 旋 轉(zhuǎn) 關(guān) 節(jié) 的 情 況 下 ，表 示 關(guān) 節(jié) 角 度 i ， 而 在 滑 動(dòng) 關(guān) 節(jié) 的 情 況 下 ， 則 表 示 關(guān) 節(jié) 位 移 di 。 在 一 次 過(guò) 渡 開 始 時(shí) （ 如 在 圖 8.13的 A點(diǎn) ） 來(lái) 描 述 這 種 類 型 的 運(yùn) 動(dòng) ，并 且 假 設(shè) ： 在 t = T1 tacc時(shí) 刻 的 當(dāng) 前 關(guān) 節(jié) 坐 標(biāo) 為 J。 當(dāng) 前 路 徑 終 點(diǎn) C的 相應(yīng) 坐 標(biāo) 為 JC。 如 果 D點(diǎn) 用 一 個(gè) 運(yùn) 動(dòng) 坐 標(biāo) 系 統(tǒng) COORD（ S） 來(lái) 表 示 ， 那 么就 令 S=S1， 對(duì) COORD（ S） 求 值 ， S1為 S在 當(dāng) 前 時(shí) 刻 t T1tacc的 值 。 從而 J D = Solve(COORD(S1) POS TOOL-1) (8.48) 其 中 ， Solve為 求 解 關(guān) 節(jié) 坐 標(biāo) （ 即 解 逆 運(yùn) 動(dòng) 方 程 ） 的 函 數(shù) ， 它 是 以 一 個(gè)變 換 表 達(dá) 式 為 自 變 量 。 q8.8 關(guān) 節(jié) 運(yùn) 動(dòng) （ Joint Motion） q 于 是 ， 在 向 D點(diǎn) 運(yùn) 動(dòng) 的 過(guò) 程 中 ， 每 個(gè) 關(guān) 節(jié) 的 運(yùn) 動(dòng) 時(shí) 間 就 由 下 式 求 得 ： ti = | JDi JCi | / vi (8.49) 其 中 ， Vi為 關(guān) 節(jié) i的 最 大 速 度 ， 路 徑 段 的 時(shí) 間 取 為 T2 = max ( ti , 2tacc ) (8.50) 然 后 , 對(duì) 完 整 的 路 徑 段 進(jìn) 行 賦 值 T1 : = T2 ; JA : = J ; 當(dāng) 前 位 置 JB : = JC ; JC : = JD ; JC : = JC JB ; JB : = JA JB ; T : = - Tacc ; 重 新 起 動(dòng) 時(shí) 刻 再 利 用 式 （ 8.38） （ 8.48） , 把 JA, JB, JB代 替 A, B, B, 就 可 求 解 機(jī) 械 手 關(guān) 節(jié) 的 定 位 點(diǎn) 。 然 而 , 我 們 還 沒 有 考 慮 由 于 運(yùn) 動(dòng) 坐 標(biāo) 系 COORD(S)的 影 響 ， 為 此 , 可 再對(duì) 相 對(duì) 于 終 點(diǎn) D的 關(guān) 節(jié) 坐 標(biāo) 進(jìn) 行 求 值 。 我 們 稱 所 求 的 關(guān) 節(jié) 坐 標(biāo) 為 JC2。 此 時(shí) t = T1， 在 這 一 時(shí) 刻 ， 我 們 把 運(yùn) 動(dòng) 坐 標(biāo) 變 量 S估 計(jì) 為 S2= S|t=T1 JC2 = Solve(COORD(S2)POSTOOL-1) (8.51) 計(jì) 算 諸 定 位 點(diǎn) J時(shí) , 如 果 簡(jiǎn) 單 地 給 J附 加 一 個(gè) 修 正 項(xiàng) 以 說(shuō) 明 C點(diǎn) 的 運(yùn) 動(dòng) , 那么 在 機(jī) 械 手 達(dá) 到 C點(diǎn) 時(shí) , 我 們 就 會(huì) 得 到 追 蹤 C點(diǎn) 的 運(yùn) 動(dòng) 。 J ： = J+(JC2 JC)*(SS1)/(S2 S1) 說(shuō) 明 C點(diǎn) 運(yùn) 動(dòng) 的 這 一 修 正 變 化 導(dǎo) 致 了 一 個(gè) 比 較 次 要 的 問(wèn) 題 ： 在 過(guò) 渡 的 開 始點(diǎn) A， 我 們 利 用 下 式 不 連 續(xù) 地 改 變 了 速 度 accCC tSS SSJJ 112 10)( 2 其 中 ， S0為 S | t=0 。 對(duì) 于 B點(diǎn) ， 可 將 表 達(dá) 式 修 改 為 JB： = JB +(JC2JC )*(S0 S1)/(S2 S1) 如 果 初 始 條 件 保 持 不 變 , 機(jī) 械 手 仍 然 會(huì) 象 原 先 一 樣 向 B點(diǎn) 運(yùn) 動(dòng) 。 如 果 初 始 條件 不 能 保 持 ， 那 我 們 還 是 要 規(guī) 定 速 度 的 連 續(xù) 性 。 因 為 機(jī) 械 手 要 追 蹤 C點(diǎn) 的 運(yùn) 動(dòng) ， 所 以 在 修 改 B點(diǎn) 的 值 以 后 ， 還 必 須 重新 計(jì) 算 JB和 JC 。 最 后 ， 在 t = T1tacc時(shí) 刻 接 近 C點(diǎn) 時(shí) ， 我 們 處 于 定 位 點(diǎn)J ， 對(duì) C點(diǎn) 求 值 得 到 JC ： = JC + (JC2 JC) * (S3-S1) / (S2-S1) 其 中 ， 最 終 的 路 徑 段 賦 值 句 就 變 為 JA： = J； 當(dāng) 前 位 置 JB： = JC + (JC2 JC) * (S3S1) / (S2 S1); JC： = Solve (COORD(S1) + PDS - TOOL )； T1： = T2 ； JC2： = Solve (COORD(S2) +PDS - TOOL)； JB： = JB (JC2 JC) * (S0S1) / (S2S1); JC: = JC JB ; JB: = JA JB ; t : = -TACC 重 新 起 動(dòng) 時(shí) 刻12 TtSS 笛 卡 爾 運(yùn) 動(dòng) 可 以 非 常 簡(jiǎn) 單 地 推 廣 到 圓 柱 坐 標(biāo) 、 球 坐 標(biāo) 以 及 其 他正 交 坐 標(biāo) 系 統(tǒng) 。 如 同 關(guān) 節(jié) 坐 標(biāo) 運(yùn) 動(dòng) ， 軌 跡 線 段 是 在 由 齊 次 變 換 式 描述 的 位 置 之 間 定 義 的 。 笛 卡 爾 運(yùn) 動(dòng) 與 關(guān) 節(jié) 坐 標(biāo) 運(yùn) 動(dòng) 的 差 別 在 于 笛 卡 爾 運(yùn) 動(dòng) ： 運(yùn) 動(dòng) 在 笛 卡 爾 坐 標(biāo) 中 是 直 觀 的 。 關(guān) 節(jié) 坐 標(biāo) 運(yùn) 動(dòng) ： 運(yùn) 動(dòng) 在 關(guān) 節(jié) 坐 標(biāo) 中 是 線 性 的 。 笛 卡 爾 運(yùn) 動(dòng) 的 優(yōu) 點(diǎn) ： 軌 跡 段 端 點(diǎn) 之 間 的 運(yùn) 動(dòng) 很 直 觀 ， 容 易 定 義 ，因 而 特 別 適 合 于 最 初 和 最 終 的 軌 跡 線 段 。 笛 卡 爾 運(yùn) 動(dòng) 的 缺 點(diǎn) ： 它 需 要 對(duì) 機(jī) 械 手 的 定 位 點(diǎn) 進(jìn) 行 不 斷 的 求 值 ，把 它 變 換 成 各 個(gè) 關(guān) 節(jié) 坐 標(biāo) 的 運(yùn) 動(dòng) 。 8.9 笛 卡 爾 運(yùn) 動(dòng) （ Cartesian Motion） 現(xiàn) 在 我 們 來(lái) 研 究 機(jī) 械 手 通 過(guò) 一 次 平 移 和 兩 次 轉(zhuǎn) 動(dòng) 時(shí) 的 情 況 。 第 一 次轉(zhuǎn) 動(dòng) 是 使 抓 手 對(duì) 準(zhǔn) 目 標(biāo) 的 方 向 。 第 二 次 轉(zhuǎn) 動(dòng) 則 控 制 抓 手 具 有 合 適 的 抓 取姿 態(tài) （ 定 向 ） 。 利 用 兩 次 轉(zhuǎn) 動(dòng) ， 關(guān) 節(jié) 運(yùn) 動(dòng) 的 范 圍 就 比 較 簡(jiǎn) 單 ， 而 且 也 可以 預(yù) 測(cè) 。 從 點(diǎn) 1到 點(diǎn) 2的 運(yùn) 動(dòng) 可 以 用 一 個(gè) 驅(qū) 動(dòng) 變 換 D()來(lái) 表 示 ， 這 是 一 個(gè) 與 有 關(guān) 的 運(yùn) 動(dòng) 函 數(shù) T6 = COORD2(S) 2POS1 D() TOOL2-1 (8.52) 其 中 = t/T， t是 從 運(yùn) 動(dòng) 開 始 起 算 的 時(shí) 間 ， 而 T是 整 個(gè) 運(yùn) 動(dòng) 的 總 時(shí) 間 。 當(dāng) 開 始 運(yùn) 動(dòng) 時(shí) ， = 0， 我 們 要 求 D(0) I ， I為 單 位 變 換 ， 在 運(yùn) 動(dòng) 終了 時(shí) ， = 1， 我 們 要 求 2POS2 = 2POS1 D(1) (8.53) 8.9.1 位 置 之 間 的 運(yùn) 動(dòng) （ Motion Between Positions） 從 而 D(1) = 2POS1-1 2POS2 (8.54) 如 果 令 2POS1=P1 ， 而 2POS2 = P2 ， 并 且 把 P1和 P2的 各 列 表 示為 向 量 P1n， P10， P1a， P1p 以 及 P2n， P2a， P2p就 有 1000 1111 1111 11111 zzzz yyyy xxxx paon paon paon pppp pppp ppppp (8.55) 1000 2222 2222 22222 zzzz yyyy xxxx paon paon paon pppp pppp ppppp (8.56) 然 后 對(duì) P1進(jìn) 行 符 號(hào) 求 逆 ， 再 右 乘 P2， 得 到 1000 )( )( )()1( 121212121 121212121 121212121 pnaaaoana pnoaooono pnnanonnn ppppppppp ppppppppp pppppppppD （ 8.57） 平 移 沿 著 連 接 P1和 P2的 直 線 進(jìn) 行 并 且 由 變 換 T()來(lái) 表 示 ， 第 一次 轉(zhuǎn) 動(dòng) 的 作 用 是 將 抓 手 指 向 接 近 矢 量 P1a， 再 從 P1旋 轉(zhuǎn) 到 P2位 置 上的 接 近 矢 量 P 2a， 這 次 旋 轉(zhuǎn) 是 繞 矢 量 k進(jìn) 行 的 。 而 k是 通 過(guò) P1和 y軸 圍繞 Z軸 旋 轉(zhuǎn) 一 個(gè) 角 度 而 取 得 的 ， 因 此 矢 量 P1k就 由 下 列 給 出 (8.58) 10101000 0100 00 00101 CS SCCSkP這 一 旋 轉(zhuǎn) 可 表 示 成 Ra()。 而 由 式 （ 2.70） 給 出 ， 其 中 k由 上 式 確 定 。 第 二 次 旋 轉(zhuǎn) 是 把 表 示 抓 手 定 向 矢 量 P10 ， 從 P1旋 轉(zhuǎn) 成 P2位 置 上 的定 向 矢 量 P20 ， 即 為 Ro()， 于 是 ， D()就 表 示 為 D() = T() Ra() Ro() （ 8.59） 其 中 T、 Ra和 Ro具 有 下 列 形 式 ： 式 中 x， y和 z表 示 從 P 1到 P2的 平 移 分 量 。 1000 100 010 001)( zyxT (8.60) 1000 0)()()( 0)()()()( 0)()()()()( 22 CSSSC SSCVCVCS SCVCSCVSRa )(1()()( CosVersV )()( CosC )()( SinS (8.62)(8.63)(8.64)Ra() 表 示 繞 P1k的 角 度 為 的 旋 轉(zhuǎn) （ 見 式 8.58） 。 (8.61) 繞 接 近 矢 量 的 旋 轉(zhuǎn) Ro()只 是 繞 Z軸 的 一 次 旋 轉(zhuǎn) ， 可 由 式 （ 2.34） 給 定Ro()表 示 繞 抓 手 的 接 近 矢 量 角 度 為 的 旋 轉(zhuǎn) 。式 中 1000 0100 00)()( 00)()()( CS SCRo (8.65)sin()( S (8.67)cos()( C (8.66) D()的 右 邊 三 列 為 令 =1， 我 們 就 可 按 下 列 步 驟 求 解 x， y， z， ， 和 ， 用 Ro(1)-1Ra(1)-1同 時(shí)右 乘 式 8.59的 兩 邊 ， 使 右 邊 一 列 的 元 素 與 式 8.57的 元 素 相 等 。 我 們 就 得 到 平 移 變換 T的 x， y， z的 值 D(1) Ro(1) -1Ra(1)-1 = T(1) (8.69) x = p1n( p2p p1p ) (8.70) y = p1o( p2p p1p ) (8.71) z = p1a( p2p p1p ) (8.72) 1000 )()()()()(? )()()()()()(? )()()()()()(?)( 22 zCSSCSCS ySSCVCCVCSS xSCVCSCCVSSD （ 8.68） 用 Ro(1)-1右 乘 式 （ 8.59） 的 兩 邊 ， 然 后 用 T(1)左 乘 兩 邊 ， 使 第 二 、三 、 四 列 的 元 素 與 式 （ 8.57） 的 元 素 相 等 。 我 們 就 可 求 解 和 從 而 有 an ao PP PP 21 21tan (8.77) T(1)-1D(1)Ro(1)-1 = Ra(1) (8.73) anSC pp 21)( (8.74) (8.76)aoSS pp 21)( aaC pp 21)( (8.75) aa aoan PP PPPP 21 21221221 )()(tan 及 0 （ 8.78） 1000 0100 00 00)1(1000 00022 CS SCDCSSSC SSCVCVCS SCVCSCVS即 為 (8.80)最 后 ， 先 用 T(1)-1， 再 用 Ra(1)-1左 乘 式 （ 8.59） 的 兩 邊 ， 得 到 Ra(1)-1 T(1)-1 D(1) = Ro(1) (8.79) 再 使 上 式 兩 邊 的 兩 個(gè) 矩 陣 中 第 二 行 第 一 列 、 第 二 行 第 二 列 的 元素 對(duì) 應(yīng) 相 等 ， 就 可 得 到 )()()( 2121221 naaonn PPSSPPCVCPPVCSS )()()( 2121221 oaoooa PPSSPPCVCPPVCSC CStan （ 8.81）（ 8.82） 在 上 小 節(jié) 中 ， 我 們 利 用 驅(qū) 動(dòng) 變 換 D()控 制 的 一 次 平 移 和 兩 次 旋 轉(zhuǎn) 來(lái)描 述 路 徑 與 運(yùn) 動(dòng) 本 節(jié) 要 描 述 路 徑 段 之 間 的 過(guò) 渡 ， 描 述 方 式 與 在 關(guān) 節(jié) 坐 標(biāo) 中 路 徑 過(guò) 渡的 情 況 （ 8.8節(jié) ） 相 似 。 這 就 是 說(shuō) ， 假 定 機(jī) 械 手 在 T1 tacc時(shí) 刻 處 于 由 JA描述 的 位 置 ， 向 JB運(yùn) 動(dòng) 。 在 T1時(shí) 刻 達(dá) 到 JB， 再 向 位 置 POSC運(yùn) 動(dòng) ， POSC由 下列 描 述 T 6C TOOL = COORD(S) POSC (8.84) 其 中 T6C是 當(dāng) 機(jī) 械 手 位 于 POSC時(shí) T6的 值 。T6 = COORD2 (S) 2POS1 D() TOOL2-1 (8.83) 8.9.2 路 徑 段 之 間 的 過(guò) 渡 （ Transitions Between Paths Segments） 首 先 ， 先 求 出 分 別 由 JA和 JB表 示 T6的 兩 個(gè) 值 。 T6A和 T6B然 后 解 出 相 應(yīng)的 位 置 變 換 POSA和 POSB POSA = COORD( S|t=-tacc )-1 T6A TOOL (8.85) POSB = COORD( S| t =0 )-1 T6B TOOL (8.86) 從 B點(diǎn) 到 C點(diǎn) 的 運(yùn) 動(dòng) 可 用 一 個(gè) 驅(qū) 動(dòng) 變 換 DBC來(lái) 描 述 DBC(1) = POSB-1 POSC (8.87) T 6 = COORD (S) POSB DBC() TOOLC-1 (8.88) 用 另 一 個(gè) 驅(qū) 動(dòng) 變 換 DBA描 述 從 B點(diǎn) 返 回 A點(diǎn) 的 運(yùn) 動(dòng) DBA(1) = POSB-1 POSA (8.89) 根 據(jù) 上 述 兩 個(gè) 驅(qū) 動(dòng) 變 換 表 達(dá) 式 ， 描 述 從 A點(diǎn) 經(jīng) 過(guò) B點(diǎn) 到 達(dá) C點(diǎn)的 運(yùn) 動(dòng) 可 這 樣 進(jìn) 行 ： 把 D()的 x, y, z, 和 從 xA、 yA、 zA、A和 A xC、 yC、 zC、 C和 C 。 這 樣 就 可 以 把 坐 標(biāo) 畫成 時(shí) 間 的 函 數(shù) ， 如 圖 8.15所 示 。 tT 1taccA-taccB CCBq 圖 8.15 笛 卡 爾 路 徑 過(guò) 渡 現(xiàn) 在 ， 我 們 可 以 定 義 B和 C， 并 利 用 式 （ 8.38） （ 8.47）來(lái) 描 述 過(guò) 渡 問(wèn) 題 以 及 向 著 C點(diǎn) 的 直 線 運(yùn) 動(dòng) 。 驅(qū) 動(dòng) 變 換 的 坐 標(biāo) 由 下 式 給 定 (8.91) accaccttth 2 (8.93)accacc tBhhBTtCx 12)5.1)( 21 (8.92)21 3)1)( accacc t hhBTtCx BhBhhBTtCx acc 2)2)( 21 (8.90) 在 t = tacc時(shí) 刻 ， 過(guò) 渡 結(jié) 束 ， 各 個(gè) 定 位 點(diǎn) 由 下 式 給 定 AAC h )( (8.98) (8.94)0 x 1Tth 1TCx (8.95)(8.96)(8.97)Chx 如 果 對(duì) 于 兩 個(gè) 驅(qū) 動(dòng) 變 換 并 不 相 同 ， 那 么 就 必 須 在 過(guò) 渡 期 間 把它 由 A變 成 C， 我 們 可 以 作 一 線 性 變 化 1000 10100 10010 00011POS 1000 10100 0110 100013POS 1000 10010 0001 01002POS 如 圖 8.16所 示 ， 在 從 點(diǎn)POS1到 POS2的 運(yùn) 動(dòng) 過(guò) 程 中 ， 我們 利 用 式 (8.70) (8.82)來(lái) 確 定x、 y、 z、 、 和 。 x = 0 y = 0 z = 0 18010tan 900)01(tan 9001tan 圖 8.16 笛 卡 爾 運(yùn) 動(dòng) 的 例 子 yzx yz xpos1z xy pos2y z xpos3 我 們 假 定 機(jī) 械 手 正 在 從 POS1接 近 POS2， 而 0.9， 驅(qū) 動(dòng) 變 換 于 是就 由 式 (8.68)確 定 而 機(jī) 械 手 位 置 為 這 樣 ， 機(jī) 械 手 的 運(yùn) 動(dòng) 就 是 從 當(dāng) 前 位 置 開 始 ， 經(jīng) 過(guò) POS2， 到 達(dá) POS3分 別相 應(yīng) 于 第 8.9.2節(jié) 中 的 位 置 A， B， C 1000 0156.976.155. 00.90156.988. 0988.155.024.D 1000 00.10156.976.155. 00.10156.988. 0988.155.024.)9.0(1DPOS 驅(qū) 動(dòng) 變 換 DBA通 過(guò) 從 B到 A的 運(yùn) 動(dòng) 來(lái) 確 定 1000 10010 0001 01001POSB 1000 00.10156.976.155. 00.10156.988. 0988.155.024. 2POSA 驅(qū) 動(dòng) 參 數(shù) xA、 yA、 zA、 A、 A和 A可 由 式 (8.70)(8.82)求 得 ，其 中 B = POS1， A = POS2 xA = 1.00 yA = 0 zA = 0 900156.tan AA 9988.156.tan AA 9988.179tan AA 從 B點(diǎn) 到 C點(diǎn) 的 運(yùn) 動(dòng) 由 驅(qū) 動(dòng) 變 換 DBC描 述 ， 這 時(shí) 1000 10010 0001 01001POSB 再 一 次 使 用 (8.70)(8.82)， 我 們 得 到 xC = 0 yC = 0 zC = 0 1000 10100 0010 100012POSC 9001tan CC 9001tan CC 9001tan CC ,我 們 給 加 上 ， 考 慮A 180 A90| AC 由 于 的 符 號(hào) ， 得 到 xA=1.00 yA=0 zA=0n 現(xiàn) 在 ， 我 們 能 根 據(jù) 從 A到 C的 過(guò) 渡 ， 求 得 其 中 任 何 一 點(diǎn) 的 運(yùn)動(dòng) 情 況 （ 圖 8.17） 。90A 9A 9A T 1T1 T1T1T 19090 -acc10yz 圖 8.17 笛 卡 爾 運(yùn) 動(dòng) 變 量 -acc-acc-acc-acc x 1 例 如 ， 當(dāng) t = 0， T1 = 10tacc， 由 式 (5.94)確 定 為 0.5， 那 么 式 (5.91)就給 出 各 個(gè) 分 量 的 值 188.0 x 000.0y 188.0z 0 0 1000 000.10010 188.0001 188.0100)(BD當(dāng) t = t acc ， 根 據(jù) 式 (5.98)， = 0.1， 運(yùn) 動(dòng) 變 量 則 由 式 (8.94)確 定 為000.0 x 000.0y 000.1z 9 9 1000 000.10156.0976.0154.0 00156.09881.0 000.1988.0155.0024.0)(BD 當(dāng) t = T1-tacc, = 0.9 時(shí) ， 有 000.0 x 000.0y 000.9z 81 81 1000 000.10988.0024.0154.0 00988.0157.0 000.9156.0155.0976.0)(BD 按 照 這 種 運(yùn) 動(dòng) 方 式 ， 機(jī) 械 手 實(shí) 際 上 并 不 經(jīng) 過(guò) 軌 跡 上 任 何 一 點(diǎn) ，而 是 從 這 些 點(diǎn) 的 附 近 經(jīng) 過(guò) （ 見 圖 8.18中 的 B點(diǎn) ） ， 機(jī) 械 手 僅 當(dāng) 停 止 在某 一 軌 跡 點(diǎn) 時(shí) ， 它 才 經(jīng) 過(guò) 該 點(diǎn) 。0-t acc taccBA B=CC D tx 圖 8.18 經(jīng) 過(guò) B的 點(diǎn) 運(yùn) 動(dòng) 如 果 試 圖 使 機(jī) 械 手 以 非 零 速 度 經(jīng) 過(guò) 一 些 中 間 軌 跡 點(diǎn) ， 那 么 ，在 所 有 的 軌 跡 極 值 點(diǎn) 處 ， 機(jī) 械 手 都 會(huì) 偏 離 軌 跡 點(diǎn) 。 這 是 因 為 機(jī) 械手 在 經(jīng) 過(guò) 極 值 點(diǎn) 的 前 后 ， 速 度 的 正 負(fù) 號(hào) 都 必 須 改 變 （ 圖 8.19） 。要 解 決 這 個(gè) 問(wèn) 題 ， 就 要 在 每 個(gè) 極 值 點(diǎn) 處 ， 速 度 都 為 零 。 當(dāng) 然 ， 這樣 就 與 前 面 所 述 的 方 法 效 果 相 當(dāng) 了 。 tx 圖 8.19 超 調(diào) 運(yùn) 動(dòng) 為 了 驅(qū) 動(dòng) 機(jī) 械 手 ， 我 們 將 以 某 個(gè) 固 定 的 采 樣 速 度 fsolve來(lái) 完 成 這些 運(yùn) 算 。 在 這 一 節(jié) 就 要 涉 及 對(duì) 任 何 機(jī) 械 手 進(jìn) 行 fsolve的 估 計(jì) 問(wèn) 題 。 串 鏈 機(jī) 械 手 是 一 種 極 其 復(fù) 雜 的 機(jī) 械 結(jié) 構(gòu) ， 對(duì) 于 一 個(gè) 機(jī) 械 手 進(jìn) 行粗 略 的 簡(jiǎn) 單 描 述 ， 是 把 它 看 作 一 種 具 有 諧 振 頻 率 fstructural和 阻 尼 系 數(shù)的 結(jié) 構(gòu) ， 假 定 計(jì) 算 一 個(gè) 解 J要 花 費(fèi) 的 時(shí) 間 為 1/fsolve， 而 使 fsolve處 的 加 速度 成 為 脈 沖 ， 為 了 使 這 個(gè) 脈 沖 不 激 起 機(jī) 械 手 的 任 何 機(jī) 構(gòu) 諧 振 ， 應(yīng) 該保 證 : f solve 10 fstructural (8.99) 8.9.3 求 解 計(jì) 算 周 期 （ Solution Evaluation Period） 圖 5.20 跟 蹤 精 度 R / 2R / 2 路 徑X 路 徑 yX 圖 8.20 跟 蹤 精 度 如 圖 8.20所 示 ， 假 定 機(jī) 械 手 由 一 個(gè) 半 徑 為 R的 連 桿 構(gòu) 成 ， 它 位 于 距運(yùn) 動(dòng) 物 體 R/2之 處 ， 該 物 體 以 恒 定 的 速 度 沿 y軸 前 進(jìn) 在 這 種 情 況 下 我 們 可 以 說(shuō) 明 ， 最 大 跟 蹤 誤 差 x是 兩 個(gè) 求 解 點(diǎn) 之 間 的距 離 y的 函 數(shù) 如 果 被 跟 蹤 物 體 以 恒 定 速 度 VC運(yùn) 動(dòng) ， 那 么 誤 差 與 求 解 頻 率 的 關(guān) 系 為 因 此 ， 對(duì) 于 斯 坦 福 手 ， 如 果 R=1m， V C=10cm/s， 那 么 x=5 10 4 mm ， 這 是 一 個(gè) 非 常 小 的 數(shù) 字 。 Ryx 8 2 (8.100)solveRfVx C2 28 (8.101) 一 個(gè) 關(guān) 節(jié) 伺 服 系 統(tǒng) 應(yīng) 當(dāng) 具 有 大 約 為 15fstructural的 采 樣 速 率 ， 就斯 坦 福 手 來(lái) 說(shuō) ， 這 個(gè) 速 率 高 達(dá) 300Hz。 每 隔 1/fsolve得 到 一 個(gè) 解 的 定位 點(diǎn) ， 在 每 個(gè) 定 位 點(diǎn) 之 間 進(jìn) 行 插 值 計(jì) 算 ， 就 可 得 到 上 述 這 些 中 間 伺服 定 位 點(diǎn) ， 假 定 求 取 一 個(gè) 解 的 時(shí) 間 為 1/fsolve時(shí) 刻 就 開 始 求 解 ， 這 樣的 做 法 在 運(yùn) 動(dòng) 坐 標(biāo) 系 統(tǒng) 突 然 停 止 的 情 況 下 會(huì) 帶 來(lái) 一 些 問(wèn) 題 ， 即 機(jī) 械手 會(huì) 超 越 正 常 的 位 置 solveCfVdy 2 (8.102) 對(duì) 于 斯 坦 福 手 來(lái) 說(shuō) ， 跟 蹤 一 個(gè) 運(yùn) 動(dòng) 速 度 為 10cm/s的 傳 送 帶 ， 超調(diào) 量 就 是 4mm。 8.9.4 插 值 （ Interpolation ） 本 章 我 們 介 紹 了 三 個(gè) 主 要 內(nèi) 容 ： 任 務(wù) 的 描 述 ， 適 宜 于 中 間 路 徑運(yùn) 動(dòng) 的 關(guān) 節(jié) 運(yùn) 動(dòng) 控 制 方 法 ， 以 及 適 宜 于 起 始 端 運(yùn) 動(dòng) 控 制 的 笛 卡 爾 運(yùn)動(dòng) 控 制 方 法 。 控 制 系 統(tǒng) 通 過(guò) 中 間 點(diǎn) 不 停 地 按 式 (8.38)(8.47)計(jì) 算 和 確 定 下一 步 運(yùn) 動(dòng) ， 從 而 能 連 續(xù) 地 完 成 笛 卡 爾 運(yùn) 動(dòng) 。 笛 卡 爾 坐 標(biāo) 運(yùn) 動(dòng) 是 由 驅(qū) 動(dòng) 變 換 D( )控 制 (參 看 式 8.68)。 關(guān) 節(jié) 坐 標(biāo) 運(yùn) 動(dòng) 把 運(yùn) 動(dòng) 學(xué) 與 動(dòng) 力 學(xué) 分 離 開 來(lái) ， 它 要 求 坐 標(biāo) 系 統(tǒng) 之間 的 變 換 僅 僅 以 運(yùn) 動(dòng) 學(xué) 條 件 為 根 據(jù) ， 而 不 象 笛 卡 爾 運(yùn) 動(dòng) 系 統(tǒng) 那 樣 ，要 考 慮 伺 服 速 度 ， 通 過(guò) 使 笛 卡 爾 坐 標(biāo) 和 關(guān) 節(jié) 坐 標(biāo) 之 間 的 變 換 速 率 與外 形 組 態(tài) 變 化 （ 而 不 是 加 速 度 的 變 化 ） 發(fā) 生 一 定 的 關(guān) 系 ， 就 能 很 有意 義 地 提 高 計(jì) 算 頻 率 。8.10 本 章 小 結(jié) （ Summary）