機(jī)器人運(yùn)動(dòng)規(guī)劃解析課件

1、機(jī)器人學(xué) 戰(zhàn) 強(qiáng)北京航空航天大學(xué)機(jī)器人研究所第六章、機(jī)器人軌跡規(guī)劃第六章、機(jī)器人軌跡規(guī)劃運(yùn)動(dòng)學(xué)動(dòng)力學(xué)軌跡規(guī)劃關(guān)節(jié)空間操作空間控制反饋控制軌跡控制力控制協(xié)調(diào)控制軌跡：操作臂在運(yùn)動(dòng)過程中每時(shí)每刻的位置、速度和加速度。軌跡規(guī)劃：根據(jù)作業(yè)任務(wù)要求計(jì)算出預(yù)期的運(yùn)動(dòng)軌跡，分為關(guān)節(jié)空間規(guī)劃和操作空間規(guī)劃兩類。t關(guān)節(jié)軌跡P0P1P2P3P4P5P6操作空間軌跡t某關(guān)節(jié)的反解值（線性化）反解關(guān)節(jié)值光滑函數(shù)擬合每個(gè)關(guān)節(jié)確定路徑點(diǎn)每個(gè)關(guān)節(jié)運(yùn)動(dòng)時(shí)間相同以關(guān)節(jié)角度（位置）函數(shù)描述機(jī)器人軌跡：計(jì)算簡單、無奇異性。關(guān)鍵要使關(guān)節(jié)軌跡滿足約束條件，如各點(diǎn)上的位姿、速度和加速度要求和連續(xù)性要求等，在滿足約束條件下選取不同的插值函

2、數(shù)。6-1 關(guān)節(jié)空間軌跡規(guī)劃1、三次多項(xiàng)式插值：通過起始點(diǎn)關(guān)節(jié)角和終止點(diǎn)關(guān)節(jié)角的運(yùn)動(dòng)軌跡可用一個(gè)光滑插值函數(shù)來表示。t關(guān)節(jié)角軌跡光滑為實(shí)現(xiàn)單關(guān)節(jié)的平穩(wěn)運(yùn)動(dòng)，軌跡函數(shù)至少滿足四個(gè)約束條件，兩個(gè)端點(diǎn)的角度約束和兩個(gè)端點(diǎn)的速度約束。角度約束：速度約束：唯一確定一個(gè)三次多項(xiàng)式代入四個(gè)約束條件可得到關(guān)于三次多項(xiàng)式系數(shù)的四個(gè)方程：求解可得三次多項(xiàng)式的系數(shù)2、例：一旋轉(zhuǎn)關(guān)節(jié)在3秒內(nèi)從起始點(diǎn) 運(yùn)動(dòng)到終止點(diǎn) ，起始和終止的速度皆為零，求關(guān)節(jié)的三次多項(xiàng)式插值函數(shù)?？傻萌味囗?xiàng)式的系數(shù)為：可得三次多項(xiàng)式的關(guān)節(jié)插值函數(shù)為：任何三次多項(xiàng)式函數(shù)的速度曲線皆為拋物線，加速度曲線為直線。角度時(shí)間角速度時(shí)間角加速度時(shí)間t關(guān)節(jié)角

3、軌跡光滑過路徑點(diǎn)的三次多項(xiàng)式插值函數(shù)如果對(duì)運(yùn)動(dòng)軌跡的要求更嚴(yán)格，約束條件更多，則三次多項(xiàng)式不能滿足需要，需要用更高階多項(xiàng)式進(jìn)行插值，如五次多項(xiàng)式。關(guān)節(jié)空間函數(shù)的光滑性并不表示操作空間運(yùn)動(dòng)的光滑性6-2 操作空間（笛卡兒空間）的軌跡規(guī)劃笛卡爾運(yùn)動(dòng)可以非常簡單地推廣到圓柱坐標(biāo)、球坐標(biāo)以及其他正交坐標(biāo)系統(tǒng)。運(yùn)動(dòng)在笛卡爾坐標(biāo)中是直觀的，容易定義，但是它需要對(duì)機(jī) 械手的定位點(diǎn)進(jìn)行不斷的求值，把它變換成各個(gè)關(guān)節(jié)坐標(biāo)的 運(yùn)動(dòng)。 在笛卡兒空間中，機(jī)器人末端抓手的位姿可用一系列的節(jié)點(diǎn)表示，軌跡規(guī)劃的首要問題是在路徑起始點(diǎn)和終點(diǎn)之間如何生成一系列的中間點(diǎn)。笛卡兒空間軌跡規(guī)劃的特點(diǎn)：INIT P0 原位MOVE P

4、1 接近銷釘MOVE P2 移動(dòng)到銷釘?shù)奈恢肎RASP 抓住銷釘 MOVE P3 垂直提起銷釘 MOVE P4 按一定角度接近孔眼MOVE P5 插入銷釘RELEASE 松開銷釘MOVE P6 離開 1、一個(gè)典型的笛卡兒空間的任務(wù)銷釘插孔作業(yè)P0P1P2P3P4P5P6如果兩個(gè)點(diǎn)不是在同一坐標(biāo)系下描述的，需變換到同一坐標(biāo)系下，如全局坐標(biāo)系W。假設(shè)節(jié)點(diǎn)是相對(duì)于局部坐標(biāo)系I描述的，相對(duì)于工作臺(tái)坐標(biāo)系B描述的，則：由每一個(gè)節(jié)點(diǎn)可求出一個(gè)相應(yīng)的手臂變換矩陣是：WZ0BzP0 P1從任意節(jié)點(diǎn) 到下一結(jié)點(diǎn) 的運(yùn)動(dòng)可表示為：假設(shè)節(jié)點(diǎn) 在坐標(biāo)系B中的描述為 ，則由于由節(jié)點(diǎn) 運(yùn)動(dòng)到 ，相當(dāng)于 從變化到：歸一化時(shí)

5、間 的函數(shù)，自運(yùn)動(dòng)開始的實(shí)際時(shí)間該運(yùn)動(dòng)軌跡的總時(shí)間1）在節(jié)點(diǎn) ，t=0， ，2）在節(jié)點(diǎn) ，t=T， ，則D(0)是4*4的單位陣則點(diǎn) 和 相對(duì)于目標(biāo)坐標(biāo)系B的位姿可用齊次矩陣表示：aon規(guī)定抓手坐標(biāo)系三個(gè)軸用n, o, a來表示，坐標(biāo)原點(diǎn)用P表示。則抓手坐標(biāo)系從點(diǎn) 到點(diǎn) 的運(yùn)動(dòng)包括一個(gè)平移運(yùn)動(dòng)和兩個(gè)旋轉(zhuǎn)運(yùn)動(dòng)：第一個(gè)轉(zhuǎn)動(dòng)使抓手坐標(biāo)系軸線與預(yù)期的方向a對(duì)準(zhǔn)，第二個(gè)轉(zhuǎn)動(dòng)饒a軸線轉(zhuǎn)動(dòng)使方向矢量o對(duì)準(zhǔn)；移動(dòng)是使把i點(diǎn)的坐標(biāo)原點(diǎn)移動(dòng)到i+1點(diǎn)。把 和 的齊次表達(dá)式代入下式 aon這樣就利用驅(qū)動(dòng)變換 來控制一個(gè)移動(dòng)和兩個(gè)轉(zhuǎn)動(dòng)生成兩個(gè)結(jié)點(diǎn)之間的運(yùn)動(dòng)軌跡。雖然笛卡兒空間中的路徑形狀與關(guān)節(jié)空間中的路徑有連續(xù)對(duì)應(yīng)的

6、關(guān)系，但直角空間中的路徑易出現(xiàn)與工作空間和奇異點(diǎn)有關(guān)的問題。如起始點(diǎn)和終止點(diǎn)都在工作空間內(nèi)，但路徑上的某些點(diǎn)不在工作空間內(nèi)，如果直線運(yùn)動(dòng),則軌跡不可實(shí)現(xiàn)，但在關(guān)節(jié)空間中規(guī)劃則容易實(shí)現(xiàn)。AB示例：說明如何對(duì)一個(gè)實(shí)際機(jī)器人進(jìn)行運(yùn)動(dòng)學(xué)的建模、軌跡規(guī)劃和控制機(jī)器人的運(yùn)動(dòng)。一個(gè)放在移動(dòng)導(dǎo)軌上的模塊機(jī)器人（9DOF）6-3 運(yùn)動(dòng)控制范例1、建立D-H坐標(biāo)系、計(jì)算變換矩陣正向運(yùn)動(dòng)學(xué)方程：2、確定逆解算法：分解速度法/反變換法分解速度法3、軌跡規(guī)劃/控制算法簡單描述：1）、通過已知直線始末兩點(diǎn)的位置與姿態(tài)，計(jì)算中間插補(bǔ)點(diǎn)的位置與姿態(tài)，這中間包括確定末端速度、加速度，插補(bǔ)點(diǎn)數(shù)，確定每次運(yùn)動(dòng)的步長，最后確定每個(gè)中

7、間點(diǎn)的位置與姿態(tài)(即求出末端T陣)；2）、由前后兩個(gè)插補(bǔ)點(diǎn)的位姿，求出末端微分運(yùn)動(dòng)矢量dp，再采用已選好的運(yùn)動(dòng)學(xué)逆解算法，反解出每步運(yùn)動(dòng)的關(guān)節(jié)變量的增量dq，由公式 即可求出每次插補(bǔ)所需的關(guān)節(jié)變量值，循環(huán)直到插補(bǔ)結(jié)束。算法的流程如下圖。3）、把每次計(jì)算出的關(guān)節(jié)位置變量值周期發(fā)送給機(jī)器人的伺服控制器，即可控制機(jī)器人按預(yù)定軌跡運(yùn)動(dòng)。 設(shè)置末端起始點(diǎn)位置和姿態(tài)坐標(biāo)為（0，110，1675，90，0，0），終點(diǎn)為（1060，280，1465，45，45，90）；前三個(gè)是位置坐標(biāo)，后三個(gè)是姿態(tài)坐標(biāo)，是相對(duì)于初始參考姿態(tài)的Z-Y-Z歐拉角。插補(bǔ)：關(guān)節(jié)空間：已知初始和終止關(guān)節(jié)角 通常采用三次多項(xiàng)式作 為平滑函數(shù)操作空間：已知初始和終止末端位姿 分直線插補(bǔ)和圓弧插補(bǔ)操作空間的直線軌跡插補(bǔ)方法如下：1）設(shè)定末端運(yùn)動(dòng)的速度v，加速度a，及插補(bǔ)的時(shí)間t