運動控制算法軌跡規(guī)劃

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

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

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

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

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

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

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

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

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

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

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

12、的方法有：基于模型的方法有：c-空間空間法、自由空間法、網(wǎng)格法、四叉法、自由空間法、網(wǎ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） 自主移動機器人的導航問題要解決的是：自主移動機器人的導航問題要解決的是：（

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