運(yùn)動(dòng)控制算法軌跡規(guī)劃

1、軌跡規(guī)劃器攝象機(jī)圖像分析器力傳感器操作臂運(yùn)動(dòng)學(xué)操作臂動(dòng)力學(xué)機(jī)器人控制器任務(wù)規(guī)劃器環(huán)境任務(wù)規(guī)劃器qd(t)F(t)x(t)q(t)(txk壓縮的數(shù)據(jù)I(k,e)機(jī)器人的軌跡規(guī)劃?rùn)C(jī)器人的軌跡規(guī)劃1 工業(yè)機(jī)器人的軌跡規(guī)劃工業(yè)機(jī)器人的軌跡規(guī)劃.軌跡規(guī)劃的一般性問題軌跡規(guī)劃的一般性問題 常見的機(jī)器人作業(yè)有兩種：常見的機(jī)器人作業(yè)有兩種：點(diǎn)位作業(yè)（點(diǎn)位作業(yè)（PTP=point-to-point motion）連續(xù)路徑作業(yè)（連續(xù)路徑作業(yè)（continuous-path motion），或者稱為輪廓運(yùn)動(dòng)），或者稱為輪廓運(yùn)動(dòng)（contour motion）。）。 操作臂最常用的軌跡規(guī)劃方法有兩種：操作臂最常用的軌

2、跡規(guī)劃方法有兩種：第一種第一種是要求對(duì)于選定的軌跡結(jié)點(diǎn)（插值點(diǎn)）上的位姿、速是要求對(duì)于選定的軌跡結(jié)點(diǎn)（插值點(diǎn)）上的位姿、速度和加速度給出一組顯式約束（例如連續(xù)性和光滑程度等），軌度和加速度給出一組顯式約束（例如連續(xù)性和光滑程度等），軌跡規(guī)劃器從一類函數(shù)（例如跡規(guī)劃器從一類函數(shù)（例如n次多項(xiàng)式）選取參數(shù)化軌跡，對(duì)結(jié)次多項(xiàng)式）選取參數(shù)化軌跡，對(duì)結(jié)點(diǎn)進(jìn)行插值，并滿足約束條件。點(diǎn)進(jìn)行插值，并滿足約束條件。第二種第二種方法要求給出運(yùn)動(dòng)路徑的解析式。方法要求給出運(yùn)動(dòng)路徑的解析式。軌跡規(guī)劃方法一般是在機(jī)器人的初始位置和目標(biāo)位置之間用軌跡規(guī)劃方法一般是在機(jī)器人的初始位置和目標(biāo)位置之間用多項(xiàng)式函數(shù)來多項(xiàng)式函數(shù)來

3、“內(nèi)插內(nèi)插”或或“逼近逼近”給定的路徑，并產(chǎn)生一系列的給定的路徑，并產(chǎn)生一系列的控制點(diǎn)。控制點(diǎn)。a. 三次多項(xiàng)式插值三次多項(xiàng)式插值 關(guān)節(jié)空間法計(jì)算簡(jiǎn)單、容易。再者，不會(huì)發(fā)生機(jī)構(gòu)的奇異性關(guān)節(jié)空間法計(jì)算簡(jiǎn)單、容易。再者，不會(huì)發(fā)生機(jī)構(gòu)的奇異性問題。問題。2.關(guān)節(jié)軌跡的插值關(guān)節(jié)軌跡的插值 只給定機(jī)器人起始點(diǎn)和終只給定機(jī)器人起始點(diǎn)和終止點(diǎn)的關(guān)節(jié)角度。止點(diǎn)的關(guān)節(jié)角度。為了實(shí)現(xiàn)平穩(wěn)運(yùn)動(dòng)，軌跡函數(shù)至少需要四個(gè)約束條件。即為了實(shí)現(xiàn)平穩(wěn)運(yùn)動(dòng)，軌跡函數(shù)至少需要四個(gè)約束條件。即滿足起點(diǎn)和終點(diǎn)的關(guān)節(jié)角度約束滿足起點(diǎn)和終點(diǎn)的關(guān)節(jié)角度約束滿足起點(diǎn)和終點(diǎn)的關(guān)節(jié)速度約束（滿滿足起點(diǎn)和終點(diǎn)的關(guān)節(jié)速度約束（滿足關(guān)節(jié)速度的連續(xù)性要求

4、）足關(guān)節(jié)速度的連續(xù)性要求）解上面四個(gè)方程得：解上面四個(gè)方程得：注意：這組解只適用于關(guān)節(jié)起點(diǎn)、終點(diǎn)速度為零的運(yùn)動(dòng)情況。注意：這組解只適用于關(guān)節(jié)起點(diǎn)、終點(diǎn)速度為零的運(yùn)動(dòng)情況。 設(shè)只有一個(gè)自由度的旋轉(zhuǎn)關(guān)節(jié)機(jī)械手處于靜止?fàn)顟B(tài)時(shí)，設(shè)只有一個(gè)自由度的旋轉(zhuǎn)關(guān)節(jié)機(jī)械手處于靜止?fàn)顟B(tài)時(shí)， =150，要在，要在3s內(nèi)平穩(wěn)運(yùn)動(dòng)到達(dá)終止位置：內(nèi)平穩(wěn)運(yùn)動(dòng)到達(dá)終止位置： =750，并且在終，并且在終止點(diǎn)的速度為零。止點(diǎn)的速度為零。tttttttt64.260 .40)(32.130 .40)(44. 40 .200 .15)(232將上式的已知條件代入以下四個(gè)方程得四個(gè)系數(shù)：將上式的已知條件代入以下四個(gè)方程得四個(gè)系數(shù)：因此

5、得：因此得：b. b. 過路徑點(diǎn)的三次多項(xiàng)式插值過路徑點(diǎn)的三次多項(xiàng)式插值 方法是：方法是：把所有路徑點(diǎn)都看成是把所有路徑點(diǎn)都看成是“起點(diǎn)起點(diǎn)”或或“終點(diǎn)終點(diǎn)”，求解，求解逆運(yùn)動(dòng)學(xué)，得到相應(yīng)的關(guān)節(jié)矢量值。然后確定所要求的三次多項(xiàng)逆運(yùn)動(dòng)學(xué)，得到相應(yīng)的關(guān)節(jié)矢量值。然后確定所要求的三次多項(xiàng)式插值函數(shù)，把路徑點(diǎn)平滑的連接起來。不同的是，這些式插值函數(shù)，把路徑點(diǎn)平滑的連接起來。不同的是，這些“起點(diǎn)起點(diǎn)”和和“終點(diǎn)終點(diǎn)”的關(guān)節(jié)速度不再是零。的關(guān)節(jié)速度不再是零。由上式確定的三次多項(xiàng)式描述了起始點(diǎn)和終止點(diǎn)具有任意給定位由上式確定的三次多項(xiàng)式描述了起始點(diǎn)和終止點(diǎn)具有任意給定位置和速度的運(yùn)動(dòng)軌跡。剩下的問題就是如何

6、確定路徑點(diǎn)上的關(guān)節(jié)置和速度的運(yùn)動(dòng)軌跡。剩下的問題就是如何確定路徑點(diǎn)上的關(guān)節(jié)速度，有以下三種方法：速度，有以下三種方法：fft)()0(0此時(shí)的此時(shí)的速度約速度約束條件束條件變?yōu)椋鹤優(yōu)椋和砜梢郧蟮么藭r(shí)的三次多項(xiàng)式系數(shù)：同理可以求得此時(shí)的三次多項(xiàng)式系數(shù)： （1） 根據(jù)工具坐標(biāo)系在直角坐標(biāo)空間中的瞬時(shí)線速度和角速根據(jù)工具坐標(biāo)系在直角坐標(biāo)空間中的瞬時(shí)線速度和角速度來確定每個(gè)路徑點(diǎn)的關(guān)節(jié)速度度來確定每個(gè)路徑點(diǎn)的關(guān)節(jié)速度 ；該方法工作量大。；該方法工作量大。 （2）為了保證每個(gè)路徑點(diǎn)上的加速度連續(xù)，由控制系統(tǒng)按照為了保證每個(gè)路徑點(diǎn)上的加速度連續(xù)，由控制系統(tǒng)按照此要求自動(dòng)地選擇路徑點(diǎn)的速度。此要求自動(dòng)地選

7、擇路徑點(diǎn)的速度。 （3）在直角坐標(biāo)空間或關(guān)節(jié)空間中采用某種適當(dāng)?shù)膯l(fā)式方在直角坐標(biāo)空間或關(guān)節(jié)空間中采用某種適當(dāng)?shù)膯l(fā)式方法，由控制系統(tǒng)自動(dòng)地選擇路徑點(diǎn)的速度；法，由控制系統(tǒng)自動(dòng)地選擇路徑點(diǎn)的速度； 對(duì)于方法對(duì)于方法（2），為了保證路徑點(diǎn)處的加速度連續(xù)，可以設(shè)法，為了保證路徑點(diǎn)處的加速度連續(xù)，可以設(shè)法用兩條三次曲線在路徑點(diǎn)處按照一定的規(guī)則聯(lián)系起來，拼湊成所用兩條三次曲線在路徑點(diǎn)處按照一定的規(guī)則聯(lián)系起來，拼湊成所要求的軌跡。其約束條件是：聯(lián)接處不僅速度連續(xù)，而且加速度要求的軌跡。其約束條件是：聯(lián)接處不僅速度連續(xù)，而且加速度也要連續(xù)。也要連續(xù)。 對(duì)于方法對(duì)于方法（3）， 這里所說的啟發(fā)式方法很簡(jiǎn)單，

8、即假設(shè)用這里所說的啟發(fā)式方法很簡(jiǎn)單，即假設(shè)用直線段把這些路徑點(diǎn)依次連接起來，如果相鄰線段的斜率在路徑直線段把這些路徑點(diǎn)依次連接起來，如果相鄰線段的斜率在路徑點(diǎn)處改變符號(hào)，則把速度選定為零；如果相鄰線段不改變符號(hào)，點(diǎn)處改變符號(hào)，則把速度選定為零；如果相鄰線段不改變符號(hào)，則選擇路徑點(diǎn)兩側(cè)的線段斜率的平均值作為該點(diǎn)的速度。則選擇路徑點(diǎn)兩側(cè)的線段斜率的平均值作為該點(diǎn)的速度。路徑點(diǎn)上速度的自動(dòng)生成ttAtDtCtBt00ABCD 如果對(duì)于運(yùn)動(dòng)軌跡的要求更為嚴(yán)格，約束條件增多，那么如果對(duì)于運(yùn)動(dòng)軌跡的要求更為嚴(yán)格，約束條件增多，那么三次多項(xiàng)式就不能滿足需要，必須用更高階的多項(xiàng)式對(duì)運(yùn)動(dòng)軌三次多項(xiàng)式就不能滿足需

9、要，必須用更高階的多項(xiàng)式對(duì)運(yùn)動(dòng)軌跡的路徑段進(jìn)行插值。例如，對(duì)某段路徑的起點(diǎn)和終點(diǎn)都規(guī)定跡的路徑段進(jìn)行插值。例如，對(duì)某段路徑的起點(diǎn)和終點(diǎn)都規(guī)定了關(guān)節(jié)的位置、速度和加速度（有六個(gè)未知的系數(shù)），則要用了關(guān)節(jié)的位置、速度和加速度（有六個(gè)未知的系數(shù)），則要用一個(gè)五次多項(xiàng)式進(jìn)行插值。一個(gè)五次多項(xiàng)式進(jìn)行插值。5544332210)(tatatatataatc、用拋物線過渡的線性插值、用拋物線過渡的線性插值 單純線性插值將導(dǎo)致在結(jié)點(diǎn)處關(guān)節(jié)運(yùn)動(dòng)速度不連續(xù)，加速度單純線性插值將導(dǎo)致在結(jié)點(diǎn)處關(guān)節(jié)運(yùn)動(dòng)速度不連續(xù)，加速度無限大。無限大。帶拋物線過渡的線性插值（2）tthtf0f0h帶拋物線過渡的線性插值（1）ttbtf

10、tf-tb0f0對(duì)于多解情況，如右圖所示。加對(duì)于多解情況，如右圖所示。加速度的值越大，過渡長(zhǎng)度越短。速度的值越大，過渡長(zhǎng)度越短。解決辦法：解決辦法：在使用線性插值時(shí)，在使用線性插值時(shí)，把每個(gè)結(jié)點(diǎn)的鄰域內(nèi)增加一段拋把每個(gè)結(jié)點(diǎn)的鄰域內(nèi)增加一段拋物線的物線的“緩沖區(qū)段緩沖區(qū)段”，從而使整，從而使整個(gè)軌跡上的位移和速度都連續(xù)。個(gè)軌跡上的位移和速度都連續(xù)。d、過路徑點(diǎn)的用拋物線過渡的線性插值、過路徑點(diǎn)的用拋物線過渡的線性插值 如圖所示，某個(gè)關(guān)節(jié)在運(yùn)動(dòng)中設(shè)有如圖所示，某個(gè)關(guān)節(jié)在運(yùn)動(dòng)中設(shè)有n個(gè)路徑點(diǎn)，其中三個(gè)相個(gè)路徑點(diǎn)，其中三個(gè)相鄰的路徑點(diǎn)表示為鄰的路徑點(diǎn)表示為j，k和和l，每?jī)蓚€(gè)相鄰的路徑點(diǎn)之間都以線性每

11、兩個(gè)相鄰的路徑點(diǎn)之間都以線性函數(shù)相連，而所有的路徑點(diǎn)附近則有拋物線過渡。函數(shù)相連，而所有的路徑點(diǎn)附近則有拋物線過渡。（同樣存在多（同樣存在多解）解）多段帶有拋物線過渡的線性插值軌跡t0lkj 如果要求機(jī)器人通過某個(gè)結(jié)點(diǎn)，同時(shí)速度不為零，怎么辦？如果要求機(jī)器人通過某個(gè)結(jié)點(diǎn)，同時(shí)速度不為零，怎么辦？可以在此結(jié)點(diǎn)兩端規(guī)定兩個(gè)可以在此結(jié)點(diǎn)兩端規(guī)定兩個(gè)“偽結(jié)點(diǎn)偽結(jié)點(diǎn)”，令該結(jié)點(diǎn)在兩偽結(jié)點(diǎn)的，令該結(jié)點(diǎn)在兩偽結(jié)點(diǎn)的連線上，并位于兩過渡域之間的線性域上。連線上，并位于兩過渡域之間的線性域上。2 移動(dòng)機(jī)器人的軌跡規(guī)劃移動(dòng)機(jī)器人的軌跡規(guī)劃 a.基于模型和基于傳感器的路徑規(guī)劃基于模型和基于傳感器的路徑規(guī)劃 基于模型

12、的方法有：基于模型的方法有：c-空間空間法、自由空間法、網(wǎng)格法、四叉法、自由空間法、網(wǎng)格法、四叉樹法、矢量場(chǎng)流的幾何表示法等。樹法、矢量場(chǎng)流的幾何表示法等。相應(yīng)的搜索算法有相應(yīng)的搜索算法有A*、遺傳算法、遺傳算法等。等。圖中圖中A區(qū)域的位置碼區(qū)域的位置碼 (Location Code:LC)為為3031。 BCD問：圖中問：圖中B，C，D區(qū)域的位置碼區(qū)域的位置碼 LC為為? b.全局路徑規(guī)劃（全局路徑規(guī)劃（Global Path Planning）和局部路徑規(guī)劃）和局部路徑規(guī)劃（Local Path Planning） 自主移動(dòng)機(jī)器人的導(dǎo)航問題要解決的是：自主移動(dòng)機(jī)器人的導(dǎo)航問題要解決的是：（

13、1）“我現(xiàn)在何處？我現(xiàn)在何處？”；（2）“我要往何處去？我要往何處去？”；（3）“要如何到該處去？要如何到該處去？”。 局部路徑規(guī)劃局部路徑規(guī)劃主要解決（主要解決（1）和（）和（3）兩個(gè)問題，即機(jī)器人）兩個(gè)問題，即機(jī)器人定位和路徑跟蹤問題；方法主要有：人工勢(shì)場(chǎng)法定位和路徑跟蹤問題；方法主要有：人工勢(shì)場(chǎng)法 、模糊邏輯算、模糊邏輯算法等法等 。 全局路徑規(guī)劃全局路徑規(guī)劃主要解決（主要解決（2），即全局目標(biāo)分解為局部目），即全局目標(biāo)分解為局部目標(biāo)，再由局部規(guī)劃實(shí)現(xiàn)局部目標(biāo)。主要有：可視圖法標(biāo)，再由局部規(guī)劃實(shí)現(xiàn)局部目標(biāo)。主要有：可視圖法 、環(huán)境分、環(huán)境分割法（自由空間法割法（自由空間法 、柵格法、柵格法 ）等）等 ；c.離線路徑規(guī)劃和在線路徑規(guī)劃離線路徑規(guī)劃和在線路徑規(guī)劃 離線路徑規(guī)劃離線路徑規(guī)劃是基于環(huán)境先驗(yàn)完全信息的路徑路徑規(guī)劃。是基于環(huán)境先驗(yàn)完全信息的路徑路徑規(guī)劃。完整的先驗(yàn)信息只能適用于靜態(tài)環(huán)境，這種情況下，路徑是離完整的先驗(yàn)信息只能適用于靜態(tài)環(huán)境，這種情況下，路徑是離線規(guī)劃的；線規(guī)劃的；在線路徑規(guī)劃在線路徑規(guī)劃是基于傳感器信息的不確定環(huán)境的路是基于傳感器信息的不確定環(huán)境的路徑規(guī)劃。在這種情況下，路徑必須是在線規(guī)劃的。徑規(guī)劃。在這種情況下，路徑必須是在線規(guī)劃的。2. 機(jī)器人