【正文】 ：連續(xù)軌跡離散化 B： 離散點處的姿態(tài)計算 已知 A B 1 2 3 4 5 6 7 8 9 ????????????????????????????????????????????????????????ccscssccssccssscssscsccssscccaonaonaonRzzzyyyxxx 運動控制 機器人及其控制原理 * 第 4章 機器人控制技術 關節(jié)運動伺服指令的生成 軌跡規(guī)劃的實現(xiàn)過程 （ 2） CP下的軌跡規(guī)劃 第一步：連續(xù)軌跡離散化 B： 離散點處的姿態(tài)計算 當給定 R的值， 則有： A B 1 2 3 4 5 6 7 8 9 ?????????????????????????????????????????????xyyxzzznnnnnao12211tantantan??? 運動控制 機器人及其控制原理 * 第 4章 機器人控制技術 關節(jié)運動伺服指令的生成 軌跡規(guī)劃的實現(xiàn)過程 （ 2） CP下的軌跡規(guī)劃 第一步：連續(xù)軌跡離散化 B： 離散點處的姿態(tài)計算 根據(jù)以上計算公式，由路徑兩個端點 A、 B的姿態(tài)矩陣即可得機器人手部在整個路徑上繞三個坐標軸的旋轉角度變化為： A B 1 2 3 4 5 6 7 8 9 ???????????ABABAB????????? 運動控制 機器人及其控制原理 * 第 4章 機器人控制技術 關節(jié)運動伺服指令的生成 軌跡規(guī)劃的實現(xiàn)過程 （ 2） CP下的軌跡規(guī)劃 第一步：連續(xù)軌跡離散化 B： 離散點處的姿態(tài)計算 由作業(yè)任務的要求可將其用時間 t的參數(shù)方程表示為： A B 1 2 3 4 5 6 7 8 9 ????????)()()(ttt?????? 運動控制 機器人及其控制原理 * 第 4章 機器人控制技術 關節(jié)運動伺服指令的生成 軌跡規(guī)劃的實現(xiàn)過程 （ 2） CP下的軌跡規(guī)劃 第一步：連續(xù)軌跡離散化 B： 離散點處的姿態(tài)計算 同樣利用弧長公式將時間化為弧長的函數(shù)，并以等間隔弧長等分整個路徑，則任一離散點處繞三個坐標軸的旋轉角度為： A B 1 2 3 4 5 6 7 8 9 ??????????????)()()()()()(000snssnssnsnnn????????? 運動控制 機器人及其控制原理 * 第 4章 機器人控制技術 關節(jié)運動伺服指令的生成 軌跡規(guī)劃的實現(xiàn)過程 （ 2） CP下的軌跡規(guī)劃 第一步：連續(xù)軌跡離散化 B： 離散點處的姿態(tài)計算 由此可得任一離散點處的位姿為： A B 1 2 3 4 5 6 7 8 9 ???????????????????nnnnnnnnnnnnnnnnnnnnnnnnnnnnnnnnnccscssccssccssscssscsccssscccxRotyRotzRotR???????????????????????????????? ),(),(),( 運動控制 機器人及其控制原理 * 第 4章 機器人控制技術 關節(jié)運動伺服指令的生成 軌跡規(guī)劃的實現(xiàn)過程 （ 2） CP下的軌跡規(guī)劃 第一步：連續(xù)軌跡離散化 根據(jù)以上計算，最終 可得離散點 n處的位姿為： A B 1 2 3 4 5 6 7 8 9 ???????10nnnpRM? 運動控制 機器人及其控制原理 * 第 4章 機器人控制技術 關節(jié)運動伺服指令的生成 軌跡規(guī)劃的實現(xiàn)過程 （ 2） CP下的軌跡規(guī)劃 第二步： PTP下的軌跡規(guī)劃 有了各個離散點處的位姿，就可以用 PTP下的軌跡規(guī)劃實現(xiàn)方法，從而完成 CP下的軌跡規(guī)劃。 第二步 ： PTP下的軌跡規(guī)劃。 關節(jié)坐標空間的軌跡規(guī)劃是直角坐標空間軌跡規(guī)劃的基礎。 5544332210)( tatatatataatq ?????? 運動控制 機器人及其控制原理 * 第 4章 機器人控制技術 關節(jié)運動伺服指令的生成 軌跡規(guī)劃的實現(xiàn)過程 （ 1） PTP下的軌跡規(guī)劃 第三步：已知機器人關節(jié)的運動速度和加速度 由此可得關節(jié)的驅動力（矩）。 A C B t01 t02 運動控制 機器人及其控制原理 * 第 4章 機器人控制技術 關節(jié)運動伺服指令的生成 軌跡規(guī)劃的實現(xiàn)過程 （ 1） PTP下的軌跡規(guī)劃 第二步：已知機器人起點和終點的關節(jié)變量取值 B、多點的三次多項式插值運算 設 A點到 C點的關節(jié)變量變化為： 設 C點到 B點的關節(jié)變量變化為： A C B t10 t20 31321211101 )( tatataatq ???? 32322221202 )( tatataatq ???? 運動控制 機器人及其控制原理 * 第 4章 機器人控制技術 關節(jié)運動伺服指令的生成 軌跡規(guī)劃的實現(xiàn)過程 （ 1） PTP下的軌跡規(guī)劃 第二步：已知機器人起點和終點的關節(jié)變量取值 B、多點的三次多項式插值運算 三點處的位移約束方程： ?????????????????320232202320232020310132101210111010tatataaqaqtatataaqaqBCCA 運動控制 機器人及其控制原理 * 第 4章 機器人控制技術 關節(jié)運動伺服指令的生成 軌跡規(guī)劃的實現(xiàn)過程 （ 1） PTP下的軌跡規(guī)劃 第二步：已知機器人起點和終點的關節(jié)變量取值 B、多點的三次多項式插值運算 起點和終點處的 速度約束方程： 中間點處的速度 和加速度約束方程： ???????22023202321113200tataaa?? ?????22101312212101310121126232ataaatataa 運動控制 機器人及其控制原理 * 第 4章 機器人控制技術 關節(jié)運動伺服指令的生成 軌跡規(guī)劃的實現(xiàn)過程 （ 1） PTP下的軌跡規(guī)劃 第二步：已知機器人起點和 終點的關節(jié)變量取值 B、多點的三次多項式插值運算 聯(lián)立上述 8個線性方程， 若令 ， 則方程組的解為： 02023 ttt ????????????????????????????????????????)538(41)2(23)(43)835(41)34(4303023202202120301320121110BACCBAABCCBABACAqqqtaqqqtaqqtaqaqqqtaqqqtaaqa 運動控制 機器人及其控制原理 * 第 4章 機器人控制技術 關節(jié)運動伺服指令的生成 軌跡規(guī)劃的實現(xiàn)過程 （ 1） PTP下的軌跡規(guī)劃 第二步：已知機器人起點和終點的關節(jié)變量取值 B、多點的三次多項式插值運算 將其代入以下兩式： 可得兩段曲線，將其拼接起來，就是該關節(jié)變量隨時間的變化規(guī)律。此函數(shù)表達式適用于關節(jié)起始點和終止點速度為零的運動情況。 運動控制 機器人及其控制原理 * 第 4章 機器人控制技術 關節(jié)運動伺服指令的生成 軌跡規(guī)劃的實現(xiàn)過程 （ 1） PTP下的軌跡規(guī)劃 第一步：已知機器人起點和終點的位姿 得到機器人對應的關節(jié)變量的取值。 ? CP下的軌跡規(guī)劃是在 直角坐標空間 進行。 *示教再現(xiàn)控制生產(chǎn)方法 ——示教生成 運動控制 機器人及其控制原理 * 第 4章 機器人控制技術 關節(jié)運動伺服指令的生成 軌跡規(guī)劃的實現(xiàn)過程 在對機器人進行軌跡規(guī)劃時，首先要對機器人的作業(yè)任務進行描述，得到機器人手部在空間的位姿變化，然后根據(jù)機器人運動學方程及其逆解并通過適當?shù)牟逖a運算求出機器人各個關節(jié)的位移、速度等運動參數(shù)的變化，再通過動力學運算最終生成機器人關節(jié)運動所需的伺服指令。 第二步 ： 關節(jié)運動的伺服控制 ，即采用一定的控制算法跟蹤執(zhí)行第一步所生成的關節(jié)運動伺服指令，這是在線完成 的。 驅動 動力學 正解 運動學 逆解 作業(yè)任務 手的運動 關節(jié)位移、 速度、加速度 關節(jié)驅動力（矩） 驅動裝置 關節(jié)產(chǎn)生運動 控制系統(tǒng) 反饋 運動控制 機器人及其控制原理