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