本發(fā)明涉及機械臂控制領域，具體地說，涉及一種復雜環(huán)境下七自由度仿人機械臂的雙層擬人運動規(guī)劃方法。

背景技術(shù)：

目前，基于隨機采樣的避障路徑規(guī)劃方法被廣泛地用于七自由度仿人機械臂的運動規(guī)劃中。該類方法通過碰撞檢測模塊提供采樣點的可行性信息，連接一系列從自由空間采樣的點，構(gòu)建一張可行軌跡的圖，從這張圖中搜索起始點到目標點的可行路徑。該類方法不需要在仿人機械臂的關(guān)節(jié)空間中對障礙物進行精確建模，因而計算量小，即使在高維空間中規(guī)劃仿人機械臂的運動軌跡，其效率也相當高。

Bi-RRT*(Bidirectional Rapidly-Exploring Random Tree Star，雙向快速擴展隨機樹星)算法是基于隨機采樣的避障路徑規(guī)劃方法中被廣泛使用的方法。該方法將雙向搜索策掠引入現(xiàn)有的RRT*(Rapidly-Exploring Random Tree Star，快速擴展隨機樹星)算法中，并引入一個啟發(fā)函數(shù)。與RRT*算法相比，Bi-RRT*算法具有更強的避障路徑規(guī)劃能力。

但是，現(xiàn)有基于Bi-RRT*算法的七自由度仿人機械臂路徑規(guī)劃方法因Bi-RRT*算法采用碰撞檢測方法判斷采樣點的可行性使得該路徑規(guī)劃方法只適用于仿人機械臂的關(guān)節(jié)空間而不適用于仿人機械臂的任務空間，且無法同時優(yōu)化仿人機械臂的末端執(zhí)行器的路徑和機械臂構(gòu)型。

技術(shù)實現(xiàn)要素：

本發(fā)明為解決現(xiàn)有基于Bi-RRT*算法的七自由度仿人機械臂路徑規(guī)劃方法不適用于仿人機械臂的任務空間規(guī)劃，且無法同時優(yōu)化仿人機械臂的末端執(zhí)行器的路徑和機械臂構(gòu)型的問題，提出了一種七自由度仿人機械臂的雙層擬人運動規(guī)劃方法。

本發(fā)明所述的七自由度仿人機械臂的雙層擬人運動規(guī)劃方法包括第一層運動規(guī)劃和第二層運動規(guī)劃，第一層運動規(guī)劃為仿人機械臂在復雜環(huán)境下的任務空間的擬人路徑規(guī)劃，第二層運動規(guī)劃為仿人機械臂沿末端無碰撞路徑的可用臂構(gòu)型空間的擬人臂構(gòu)型規(guī)劃；

所述方法包括：

步驟一、在所述仿人機械臂的任務空間內(nèi)，為所述仿人機械臂的末端執(zhí)行器規(guī)劃出一條從其初始位置至目標位置的擬人的無碰撞路徑；

步驟二、采用基于平滑樣條的路徑平滑方法對所述無碰撞路徑進行平滑處理，形成所述末端執(zhí)行器的任務空間路徑規(guī)劃；

步驟三、在所述末端執(zhí)行器能夠自其初始位置經(jīng)平滑處理后的無碰撞路徑到達目標位置情形下，計算并保存所有滿足關(guān)節(jié)限位關(guān)系和避碰撞約束的可用機械臂構(gòu)型；

步驟四、根據(jù)人臂的運動學特征，在沿末端執(zhí)行器無碰撞路徑的所有可用機械臂構(gòu)型中選擇出最優(yōu)機械臂構(gòu)型集；

步驟五、控制所述仿人機械臂依次完成所述最優(yōu)機械臂構(gòu)型集中的所有臂構(gòu)型，使所述末端執(zhí)行器完成所述平滑處理后的無碰撞路徑；

步驟一和步驟二構(gòu)成仿人機械臂在復雜環(huán)境下的任務空間的擬人路徑規(guī)劃，步驟三和步驟四構(gòu)成仿人機械臂沿末端無碰撞路徑的可用臂構(gòu)型空間的擬人臂構(gòu)型規(guī)劃。

作為優(yōu)選的是，步驟一包括：

步驟一一、在所述仿人機械臂的任務空間內(nèi)，采用隨機采樣的方式，以初始點為基礎構(gòu)建第一樹的節(jié)點集，以目標點為基礎構(gòu)建第二樹的節(jié)點集；

初始點和目標點分別為所述末端執(zhí)行器的初始位置和目標位置；

在六維任務空間內(nèi)通過隨機采樣的方式獲得的多個機械臂末端點相互獨立，恒等分布；

步驟一二、在機械臂的任務空間內(nèi)分別以初始點和目標點為根節(jié)點擴展第一樹和第二樹，所述第一樹和第二樹均為啟發(fā)式快速擴展隨機樹，二者以相同的擴展方式相對交替擴展，直至二者相遇；

步驟一三、當?shù)谝粯浜偷诙湎嘤鰰r，計算出此時從初始點至目標點的路徑；

步驟一四、采用基于三角不等式的路徑優(yōu)化方法對步驟一三所述路徑進行局部優(yōu)化處理，并得到局部優(yōu)化路徑；

比較步驟一三所述路徑和所述局部優(yōu)化路徑的代價，并保存代價較小的機械臂末端路徑，該路徑為無碰撞機械臂末端路徑。

進一步的是，步驟一二包括：

步驟一二一、在所述仿人機械臂的任務空間內(nèi)，采用隨機采樣的方式獲取一個機械臂末端點，并采用基于臂構(gòu)型描述的末端新生長路徑點有效性判斷方法對該點進行有效性判斷，如果該點被判斷為有效，將該點作為第一樹的新生長路徑點，并執(zhí)行步驟一二二；

否則，執(zhí)行步驟一二一；

步驟一二二、在第一樹的節(jié)點集中選擇出第一樹的新生長路徑點的最近節(jié)點；

步驟一二三、在第一樹的新生長路徑點的附近節(jié)點集中，采用路徑代價算法選擇出路徑代價最小的附近節(jié)點，并將該節(jié)點作為第一樹的新生長路徑點的父節(jié)點；

依次連接第一樹的新生長路徑點的最近節(jié)點、第一樹的新生長路徑點的父節(jié)點和第一樹的新生長路徑點，完成第一樹的一次擴展；

步驟一二四、判斷第一樹的新生長路徑點是否與第二樹相遇，如果二者相遇，執(zhí)行步驟一三，否則，執(zhí)行步驟一二五；

第一樹的新生長路徑點與第二樹是否相遇的判斷方法為：在第二樹的節(jié)點集中選擇出第一樹的新生長路徑點的最近節(jié)點，如果該節(jié)點與第一樹的新生長路徑點的間距小于一個步長，第一樹的新生長路徑點與第二樹相遇，否則，二者未相遇；

步驟一二五、完成第二樹的一次擴展，并判斷第二樹的新生長路徑點是否與第一樹相遇，如果二者相遇，執(zhí)行步驟一三，否則，執(zhí)行步驟一二六；

第二樹的新生長路徑點與第一樹是否相遇的判斷方法為：在第一樹的節(jié)點集中選擇出第二樹的新生長路徑點的最近節(jié)點，如果該節(jié)點與第二樹的新生長路徑點的間距小于一個步長，第二樹的新生長路徑點與第一樹相遇，否則，二者未相遇；

步驟一二六、完成第二樹的一次擴展，并判斷第二樹的新生長路徑點是否與第一樹相遇，如果二者相遇，執(zhí)行步驟一三，否則，執(zhí)行步驟一二七；

步驟一二七、以擴展兩次第一樹和擴展兩次第二樹的方式循環(huán)擴展，直至第一樹和第二樹相遇，二者相遇后，執(zhí)行步驟一三。

進一步的是，步驟一二一所述的基于臂構(gòu)型描述的末端新生長路徑點有效性判斷方法包括：

步驟A、將仿人機械臂的任務空間內(nèi)的隨機采樣點假設為仿人機械臂末端新生長路徑點，計算仿人機械臂在該新生長路徑點下的滿足仿人機械臂關(guān)節(jié)限位關(guān)系的可用臂構(gòu)型集；

該步驟的具體過程為：

以第二關(guān)節(jié)為坐標原點建立三維基坐標，給定所述仿人機械臂末端新生長路徑點相對于所述三維基坐標的期望位置和期望姿態(tài)，通過建立基于臂角的仿人機械臂運動學模型獲得各個關(guān)節(jié)角與臂角的關(guān)系；

第一關(guān)節(jié)至第七關(guān)節(jié)依次分布在所述仿人機械臂的基座與末端執(zhí)行器之間；

根據(jù)各個關(guān)節(jié)角與臂角的關(guān)系，將第二關(guān)節(jié)和第六關(guān)節(jié)的運動范圍分別分為正負兩個區(qū)域，進而形成四個臂角區(qū)域；

所述四個臂角區(qū)域分別為：

第一臂角區(qū)域，滿足第二關(guān)節(jié)角和第六關(guān)節(jié)角同時小于0；

第二臂角區(qū)域，滿足第二關(guān)節(jié)角小于0，第六關(guān)節(jié)角大于0；

第三臂角區(qū)域，滿足第二關(guān)節(jié)角大于0，第六關(guān)節(jié)角小于0；

第四臂角區(qū)域，滿足第二關(guān)節(jié)角和第六關(guān)節(jié)角同時大于0；

分別計算第一臂角區(qū)域、第二臂角區(qū)域、第三臂角區(qū)域和第四臂角區(qū)域內(nèi)所述仿人機械臂的可用臂角范圍，從而計算出所述仿人機械臂在該新生長路徑點下的滿足仿人機械臂關(guān)節(jié)限位關(guān)系的可用臂構(gòu)型集；

步驟B、建立仿人機械臂在該新生長路徑點下的基于球掃掠凸體的碰撞檢測模型，計算出該新生長路徑點下同時滿足仿人機械臂關(guān)節(jié)限位和避碰撞約束的可用臂構(gòu)型集；

該步驟的具體過程為：

建立仿人機械臂在關(guān)節(jié)限位下自運動形成的掃掠體的基于球掃掠凸體的碰撞檢測模型：

當仿人機械臂自運動時，其上臂和下臂分別形成圓錐面掃掠體，根據(jù)第四關(guān)節(jié)自運動形成的軌跡，采用n個球掃掠凸體構(gòu)建所述圓錐面掃掠體；

上臂包括第一關(guān)節(jié)至第四關(guān)節(jié)，下臂包括第四關(guān)節(jié)至第七關(guān)節(jié)，n為正整數(shù)；

所述球掃掠凸體由球半徑和五個頂點構(gòu)成，該球掃掠凸體定義為：

其中，r為球掃掠凸體V₁的半徑，球掃掠凸體V₁的五個頂點分別為至為第一關(guān)節(jié)、第二關(guān)節(jié)與第三關(guān)節(jié)的旋轉(zhuǎn)軸線的交點或者第五關(guān)節(jié)、第六關(guān)節(jié)與第七關(guān)節(jié)的旋轉(zhuǎn)軸線的交點，為第四關(guān)節(jié)的位置，是自運動旋轉(zhuǎn)ψ_l/2n的點，是自運動旋轉(zhuǎn)ψ_l/n的點，ψ_l為自運動旋轉(zhuǎn)的角度；

或者，所述球掃掠凸體由球半徑和四個頂點構(gòu)成，該球掃掠凸體定義為：

其中，球掃掠凸體V₂的四個頂點分別為至是自運動旋轉(zhuǎn)ψ_l/n的點；

分別建立仿人機械人中除機械臂外的其他部件和周圍靜止環(huán)境的基于球掃掠凸體的碰撞檢測模型；

采用基于球掃掠凸體的碰撞檢測方法檢測仿人機械臂自運動形成的掃掠體的基于球掃掠凸體的碰撞檢測模型與仿人機械人的其他部件以及周圍靜止環(huán)境的基于球掃掠凸體的碰撞檢測模型是否發(fā)生碰撞以及是否存在可能的碰撞點，剔除可能產(chǎn)出碰撞的臂構(gòu)型，獲得仿人機械臂在該新生長路徑點下同時滿足仿人機械臂關(guān)節(jié)限位關(guān)系和避碰撞約束的可用臂構(gòu)型集；

步驟C、計算所述四個臂角區(qū)域中任意兩個臂角區(qū)域之間的過渡條件；

步驟D、根據(jù)仿人機械臂在該新生長路徑點下同時滿足仿人機械臂關(guān)節(jié)限位關(guān)系和避碰撞約束的可用臂構(gòu)型集以及所述任意兩個臂角區(qū)域之間的過渡條件判斷該新生長路徑點的有效性。

進一步的是，步驟D包括：

步驟a、判斷第四關(guān)節(jié)是否超限，如果第四關(guān)節(jié)超限，則新生長路徑點無效，否則，執(zhí)行步驟b；

步驟b、判斷可用臂構(gòu)型集是否為空，如果可用臂構(gòu)型集為空，則新生長路徑點無效，否則，執(zhí)行步驟c；

步驟c、判斷該新生長路徑點與前一個生長路徑點是否在同一個臂角區(qū)域內(nèi)，如果在同一個臂角區(qū)域內(nèi)，則新生長路徑點有效，否則，執(zhí)行步驟d；

步驟d、判斷新生長路徑點所在的臂角區(qū)域與前一個生長路徑點所在的臂角區(qū)域是否存在過渡點，如果存在過渡點，則新生長路徑點有效，否則，新生長路徑點無效。

優(yōu)選的是，步驟四采用冗余解優(yōu)化方法在所有可用機械臂構(gòu)型中選擇出最優(yōu)機械臂構(gòu)型集，所述冗余解優(yōu)化方法包括：

步驟四一、選擇最優(yōu)臂角區(qū)域；

步驟四二、在優(yōu)化臂角區(qū)域內(nèi)優(yōu)化擬人臂角；

步驟四三、處理不可用優(yōu)化臂角；

步驟四四、臂角區(qū)域過渡；

步驟四五、采用基于臂角的解析逆運動學方法計算各關(guān)節(jié)角度。

本發(fā)明所述的七自由度仿人機械臂的雙層擬人運動規(guī)劃方法包括雙層擬人運動規(guī)劃，第一層擬人運動規(guī)劃為仿人機械臂在復雜環(huán)境下的任務空間的擬人路徑規(guī)劃，該規(guī)劃通過實施步驟一和步驟二實現(xiàn)，第二層擬人運動規(guī)劃為仿人機械臂的可用臂構(gòu)型空間的擬人臂構(gòu)型規(guī)劃，該規(guī)劃通過實施步驟三和步驟四實現(xiàn)。所述雙層擬人運動規(guī)劃解決了現(xiàn)有基于Bi-RRT*算法的七自由度仿人機械臂路徑規(guī)劃方法無法同時優(yōu)化仿人機械臂的末端執(zhí)行器的路徑和機械臂構(gòu)型的問題。

本發(fā)明所述的七自由度仿人機械臂的雙層擬人運動規(guī)劃方法采用基于臂構(gòu)型描述的七自由度仿人機械臂末端點有效性判斷方法替代現(xiàn)有Bi-RRT*算法中的碰撞檢測方法，使得該運動規(guī)劃方法適用于仿人機械臂的任務空間規(guī)劃。

附圖說明

在下文中將基于實施例并參考附圖來對本發(fā)明所述的七自由度仿人機械臂的雙層擬人運動規(guī)劃方法進行更詳細的描述，其中：

圖1是實施例一提及的參考平面、臂平面和臂角的示意圖；

圖2是實施例一提及的基于臂角的仿人機械臂運動學模型的示意圖；

圖3是實施例一提及的仿人機械臂自運動形成的掃略體以及由兩個包含一個半徑和五個頂點的球掃略凸體構(gòu)建的碰撞檢測模型示意圖；

圖4是實施例一提及的仿人機械臂自運動形成的掃略體以及由兩個包含一個半徑和四個頂點的球掃略凸體構(gòu)建的碰撞檢測模型示意圖；

圖5是實施例一提及的仿人機械臂的下臂上端自運動形成的軌跡和包裹下臂上端的由一個半徑和四個頂點組成的球掃掠凸體的示意圖；

圖6是實施例一提及的仿人機械臂的下臂上端自運動形成的軌跡和包裹下臂上端的由一個半徑和三個頂點組成的球掃掠凸體的示意圖。

在附圖中，相同的部件使用相同的附圖標記。附圖并未按照實際的比例。

具體實施方式

下面將結(jié)合附圖對本發(fā)明所述的七自由度仿人機械臂的雙層擬人運動規(guī)劃方法作進一步說明。

實施例一：下面結(jié)合圖1至圖6詳細地說明本實施例。本實施例所述的七自由度仿人機械臂的雙層擬人運動規(guī)劃方法包括第一層運動規(guī)劃和第二層運動規(guī)劃，第一層運動規(guī)劃為仿人機械臂在復雜環(huán)境下的任務空間的擬人路徑規(guī)劃，第二層運動規(guī)劃為仿人機械臂沿末端無碰撞路徑的可用臂構(gòu)型空間的擬人臂構(gòu)型規(guī)劃；

所述方法包括：

步驟一、在所述仿人機械臂的任務空間內(nèi)，為所述仿人機械臂的末端執(zhí)行器規(guī)劃出一條從其初始位置至目標位置的擬人的無碰撞路徑；

步驟二、采用基于平滑樣條的路徑平滑方法對所述無碰撞路徑進行平滑處理，形成所述末端執(zhí)行器的任務空間路徑規(guī)劃；

步驟三、在所述末端執(zhí)行器能夠自其初始位置經(jīng)平滑處理后的無碰撞路徑到達目標位置情形下，計算并保存所有滿足關(guān)節(jié)限位關(guān)系和避碰撞約束的可用機械臂構(gòu)型；

步驟四、根據(jù)人臂的運動學特征，在沿末端執(zhí)行器無碰撞路徑的所有可用機械臂構(gòu)型中選擇出最優(yōu)機械臂構(gòu)型集；

步驟五、控制所述仿人機械臂依次完成所述最優(yōu)機械臂構(gòu)型集中的所有臂構(gòu)型，使所述末端執(zhí)行器完成所述平滑處理后的無碰撞路徑；

步驟一和步驟二構(gòu)成仿人機械臂在復雜環(huán)境下的任務空間的擬人路徑規(guī)劃，步驟三和步驟四構(gòu)成仿人機械臂沿末端無碰撞路徑的可用臂構(gòu)型空間的擬人臂構(gòu)型規(guī)劃；

所述步驟一包括：

步驟一一、在所述仿人機械臂的任務空間內(nèi)，采用隨機采樣的方式，以初始點為基礎構(gòu)建第一樹的節(jié)點集，以目標點為基礎構(gòu)建第二樹的節(jié)點集；

初始點和目標點分別為所述末端執(zhí)行器的初始位置和目標位置；

在六維任務空間內(nèi)通過隨機采樣的方式獲得的多個機械臂末端點相互獨立，恒等分布；

步驟一二、在機械臂的任務空間內(nèi)分別以初始點和目標點為根節(jié)點擴展第一樹和第二樹，所述第一樹和第二樹均為啟發(fā)式快速擴展隨機樹，二者以相同的擴展方式相對交替擴展，直至二者相遇；

步驟一三、當?shù)谝粯浜偷诙湎嘤鰰r，計算出此時從初始點至目標點的路徑；

步驟一四、采用基于三角不等式的路徑優(yōu)化方法對步驟一三所述路徑進行局部優(yōu)化處理，并得到局部優(yōu)化路徑；

比較步驟一三所述路徑和所述局部優(yōu)化路徑的代價，并保存代價較小的機械臂末端路徑，該路徑為無碰撞機械臂末端路徑；

所述步驟一二包括：

步驟一二一、在所述仿人機械臂的任務空間內(nèi)，采用隨機采樣的方式獲取一個機械臂末端點，并采用基于臂構(gòu)型描述的末端新生長路徑點有效性判斷方法對該點進行有效性判斷，如果該點被判斷為有效，將該點作為第一樹的新生長路徑點，并執(zhí)行步驟一二二；

否則，執(zhí)行步驟一二一；

步驟一二二、在第一樹的節(jié)點集中選擇出第一樹的新生長路徑點的最近節(jié)點；

步驟一二三、在第一樹的新生長路徑點的附近節(jié)點集中，采用路徑代價算法選擇出路徑代價最小的附近節(jié)點，并將該節(jié)點作為第一樹的新生長路徑點的父節(jié)點；

依次連接第一樹的新生長路徑點的最近節(jié)點、第一樹的新生長路徑點的父節(jié)點和第一樹的新生長路徑點，完成第一樹的一次擴展；

步驟一二四、判斷第一樹的新生長路徑點是否與第二樹相遇，如果二者相遇，執(zhí)行步驟一三，否則，執(zhí)行步驟一二五；

第一樹的新生長路徑點與第二樹是否相遇的判斷方法為：在第二樹的節(jié)點集中選擇出第一樹的新生長路徑點的最近節(jié)點，如果該節(jié)點與第一樹的新生長路徑點的間距小于一個步長，第一樹的新生長路徑點與第二樹相遇，否則，二者未相遇；

步驟一二五、完成第二樹的一次擴展，并判斷第二樹的新生長路徑點是否與第一樹相遇，如果二者相遇，執(zhí)行步驟一三，否則，執(zhí)行步驟一二六；

第二樹的新生長路徑點與第一樹是否相遇的判斷方法為：在第一樹的節(jié)點集中選擇出第二樹的新生長路徑點的最近節(jié)點，如果該節(jié)點與第二樹的新生長路徑點的間距小于一個步長，第二樹的新生長路徑點與第一樹相遇，否則，二者未相遇；

步驟一二六、完成第二樹的一次擴展，并判斷第二樹的新生長路徑點是否與第一樹相遇，如果二者相遇，執(zhí)行步驟一三，否則，執(zhí)行步驟一二七；

步驟一二七、以擴展兩次第一樹和擴展兩次第二樹的方式循環(huán)擴展，直至第一樹和第二樹相遇，二者相遇后，執(zhí)行步驟一三；

步驟一二一所述的基于臂構(gòu)型描述的末端新生長路徑點有效性判斷方法包括：

步驟A、將仿人機械臂的任務空間內(nèi)的隨機采樣點假設為仿人機械臂末端新生長路徑點，計算仿人機械臂在該新生長路徑點下的滿足仿人機械臂關(guān)節(jié)限位關(guān)系的可用臂構(gòu)型集；

該步驟的具體過程為：

以第二關(guān)節(jié)為坐標原點建立三維基坐標，給定所述仿人機械臂末端新生長路徑點相對于所述三維基坐標的期望位置和期望姿態(tài)，通過建立基于臂角的仿人機械臂運動學模型獲得各個關(guān)節(jié)角與臂角的關(guān)系；

第一關(guān)節(jié)至第七關(guān)節(jié)依次分布在所述仿人機械臂的基座與末端執(zhí)行器之間；

根據(jù)各個關(guān)節(jié)角與臂角的關(guān)系，將第二關(guān)節(jié)和第六關(guān)節(jié)的運動范圍分別分為正負兩個區(qū)域，進而形成四個臂角區(qū)域；

所述四個臂角區(qū)域分別為：

第一臂角區(qū)域，滿足第二關(guān)節(jié)角和第六關(guān)節(jié)角同時小于0；

第二臂角區(qū)域，滿足第二關(guān)節(jié)角小于0，第六關(guān)節(jié)角大于0；

第三臂角區(qū)域，滿足第二關(guān)節(jié)角大于0，第六關(guān)節(jié)角小于0；

第四臂角區(qū)域，滿足第二關(guān)節(jié)角和第六關(guān)節(jié)角同時大于0；

分別計算第一臂角區(qū)域、第二臂角區(qū)域、第三臂角區(qū)域和第四臂角區(qū)域內(nèi)所述仿人機械臂的可用臂角范圍，從而計算出所述仿人機械臂在該新生長路徑點下的滿足仿人機械臂關(guān)節(jié)限位關(guān)系的可用臂構(gòu)型集；

步驟B、建立仿人機械臂在該新生長路徑點下的基于球掃掠凸體的碰撞檢測模型，計算出該新生長路徑點下同時滿足仿人機械臂關(guān)節(jié)限位和避碰撞約束的可用臂構(gòu)型集；

該步驟的具體過程為：

建立仿人機械臂在關(guān)節(jié)限位下自運動形成的掃掠體的基于球掃掠凸體的碰撞檢測模型：

當仿人機械臂自運動時，其上臂和下臂分別形成圓錐面掃掠體，根據(jù)第四關(guān)節(jié)自運動形成的軌跡，采用n個球掃掠凸體構(gòu)建所述圓錐面掃掠體；

上臂包括第一關(guān)節(jié)至第四關(guān)節(jié)，下臂包括第四關(guān)節(jié)至第七關(guān)節(jié)，n為正整數(shù)；

所述球掃掠凸體由球半徑和五個頂點構(gòu)成，該球掃掠凸體定義為：

其中，r為球掃掠凸體V₁的半徑，球掃掠凸體V₁的五個頂點分別為至為第一關(guān)節(jié)、第二關(guān)節(jié)與第三關(guān)節(jié)的旋轉(zhuǎn)軸線的交點或者第五關(guān)節(jié)、第六關(guān)節(jié)與第七關(guān)節(jié)的旋轉(zhuǎn)軸線的交點，為第四關(guān)節(jié)的位置，是自運動旋轉(zhuǎn)ψ_l/2n的點，是自運動旋轉(zhuǎn)ψ_l/n的點，ψ_l為自運動旋轉(zhuǎn)的角度；

或者，所述球掃掠凸體由球半徑和四個頂點構(gòu)成，該球掃掠凸體定義為：

其中，球掃掠凸體V₂的四個頂點分別為至是自運動旋轉(zhuǎn)ψ_l/n的點；

分別建立仿人機械人中除機械臂外的其他部件和周圍靜止環(huán)境的基于球掃掠凸體的碰撞檢測模型；

采用基于球掃掠凸體的碰撞檢測方法檢測仿人機械臂自運動形成的掃掠體的基于球掃掠凸體的碰撞檢測模型與仿人機械人的其他部件以及周圍靜止環(huán)境的基于球掃掠凸體的碰撞檢測模型是否發(fā)生碰撞以及是否存在可能的碰撞點，剔除可能產(chǎn)出碰撞的臂構(gòu)型，獲得仿人機械臂在該新生長路徑點下同時滿足仿人機械臂關(guān)節(jié)限位關(guān)系和避碰撞約束的可用臂構(gòu)型集；

步驟C、計算所述四個臂角區(qū)域中任意兩個臂角區(qū)域之間的過渡條件；

步驟D、根據(jù)仿人機械臂在該新生長路徑點下同時滿足仿人機械臂關(guān)節(jié)限位關(guān)系和避碰撞約束的可用臂構(gòu)型集以及所述任意兩個臂角區(qū)域之間的過渡條件判斷該新生長路徑點的有效性；

步驟四采用冗余解優(yōu)化方法在所有可用機械臂構(gòu)型中選擇出最優(yōu)機械臂構(gòu)型集，所述冗余解優(yōu)化方法包括：

步驟四一、選擇最優(yōu)臂角區(qū)域；

步驟四二、在優(yōu)化臂角區(qū)域內(nèi)優(yōu)化擬人臂角；

步驟四三、處理不可用優(yōu)化臂角；

步驟四四、臂角區(qū)域過渡；

步驟四五、采用基于臂角的解析逆運動學方法計算各關(guān)節(jié)角度。

本實施例選用臂角作為冗余參數(shù)來描述仿人機械臂的自運動。圖1是參考平面、臂平面和臂角的示意圖，如圖1所示，P_s、P_e和P_w分別為仿人機械臂的肩關(guān)節(jié)、肘關(guān)節(jié)和腕關(guān)節(jié)，ψ為臂角。圖2是基于臂角的仿人機械臂運動學模型的示意圖，如圖2所示，q₁至q₇分別為仿人機械臂的第一關(guān)節(jié)至第七關(guān)節(jié)，第一關(guān)節(jié)q₁至第三關(guān)節(jié)q₃、第四關(guān)節(jié)q₄和第五關(guān)節(jié)q₅至第七關(guān)節(jié)q₇分別對應肩關(guān)節(jié)P_s、肘關(guān)節(jié)P_e和腕關(guān)節(jié)P_w，坐標原點O、X軸、Y軸和Z軸構(gòu)成三維基坐標。由肩關(guān)節(jié)P_s、肘關(guān)節(jié)P_e和腕關(guān)節(jié)P_w構(gòu)成的平面為臂平面。當仿人機械臂的末端執(zhí)行器的位姿不變時，由于仿人機械臂包含一個冗余自由度，肘關(guān)節(jié)P_e能夠以肩關(guān)節(jié)P_s和腕關(guān)節(jié)P_w共同的軸線為軸旋轉(zhuǎn)，進而形成多個角度不同的臂平面。定義第三關(guān)節(jié)q₃為0度時的臂平面為參考平面，參考平面和臂平面的夾角為臂角ψ。

仿人機械臂自運動的范圍會受到關(guān)節(jié)限位和障礙物位置的影響，當仿人機械臂自運動的范圍為空時，表明仿人機械臂受關(guān)節(jié)限位和障礙物位置的限制無法到達該位置。本實施例基于該原理提出基于臂構(gòu)型描述的末端新生長路徑點有效性判斷方法。

在給定末端位姿下，各個關(guān)節(jié)角與臂角ψ的關(guān)系為：

θ₁＝a tan2{sign(sinθ₂)(-a_s22sinψ-b_s22cosψ-c_s22),

sign(sinθ₂)(-a_s12sinψ-b_s12cosψ-c_s12)}

θ₂＝±a cos(-a_s32sinψ-b_s32cosψ-c_s32)

θ₃＝a tan2{sign(sinθ₂)(a_s33sinψ+b_s33cosψ+c_s33),

sign(sinθ₂)(-a_s31sinψ-b_s31cosψ-c_s31)}

θ₅＝a tan2{sign(sinθ₆)(a_w23sinψ+b_w23cosψ+c_w23),

sign(sinθ₆)(a_w13sinψ+b_w13cosψ+c_w13)}

θ₆＝±a cos(a_w33sinψ+b_w33cosψ+c_w33)

θ₇＝a tan2{sign(sinθ₆)(a_w32sinψ+b_w32cosψ+c_w32),

sign(sinθ₆)(-a_w31sinψ-b_w31cosψ-c_w31)}

其中，θ₁至θ₇分別為第一關(guān)節(jié)角至第七關(guān)節(jié)角，a_sij,b_sij,c_sij,a_wij,b_wij,c_wij分別為矩陣A_s,B_s,C_s,A_w,B_w，C_w的(i,j)元素，⁰x_sw是肩關(guān)節(jié)P_s到腕關(guān)節(jié)P_w的向量，d_se是肩關(guān)節(jié)P_s到肘關(guān)節(jié)P_e的向量，d_ew是肘關(guān)節(jié)P_e到腕關(guān)節(jié)的向量，d_se和d_ew是常向量。

根據(jù)上述各個關(guān)節(jié)角與臂角ψ的關(guān)系可知：第四關(guān)節(jié)角θ₄只與仿人機械臂的末端執(zhí)行器的位姿有關(guān)，第二關(guān)節(jié)角θ₂和第六關(guān)節(jié)角θ₆與仿人機械臂的末端執(zhí)行器的位姿和臂角ψ有關(guān)，第一關(guān)節(jié)角θ₁、第三關(guān)節(jié)角θ₃、第五關(guān)節(jié)角θ₅和第七關(guān)節(jié)角θ₇除了與仿人機械臂的末端執(zhí)行器的位姿和臂角ψ有關(guān)，還與第二關(guān)節(jié)角θ₂和第六關(guān)節(jié)角θ₆的正負有關(guān)。

假設第i關(guān)節(jié)角的范圍為(i＝1,2,…,7)θ_i∈[-π,π]，其中，和分別是第i關(guān)節(jié)的下限和上限。為了在[-π,π]的范圍內(nèi)確定各個關(guān)節(jié)角與臂角ψ的唯一對應關(guān)系，將第二關(guān)節(jié)角θ₂和第六關(guān)節(jié)角θ₆分別分為正負兩個區(qū)域，從而形成四個臂角區(qū)域：

第一臂角區(qū)域：即θ₂<0,θ₆<0，所述仿人機械臂在該臂角區(qū)域內(nèi)的可用臂角范圍為：

第二臂角區(qū)域：即θ₂<0,θ₆>0，所述仿人機械臂在該臂角區(qū)域內(nèi)的可用臂角范圍為：

第三臂角區(qū)域：即θ₂>0,θ₆<0，所述仿人機械臂在該臂角區(qū)域內(nèi)的可用臂角范圍為：

第四臂角區(qū)域：即θ₂>0,θ₆>0，所述仿人機械臂在該臂角區(qū)域內(nèi)的可用臂角范圍為：

其中，(i＝1,3,5,7)是第二關(guān)節(jié)角或第六關(guān)節(jié)角為正時，第i關(guān)節(jié)的有效臂角范圍，(i＝1,3,5,7)是第二關(guān)節(jié)角或第六關(guān)節(jié)角為負時，第i關(guān)節(jié)的有效臂角范圍，ψ_j<0，(j＝2,6)為第j關(guān)節(jié)在關(guān)節(jié)運動范圍內(nèi)的有效臂角范圍，ψ_j>0，(j＝2,6)為第j關(guān)節(jié)在關(guān)節(jié)運動范圍內(nèi)的有效臂角范圍；

所述仿人機械臂末端點在該新生長路徑點下滿足關(guān)節(jié)限位關(guān)系的臂構(gòu)型集為；

隨后建立各個關(guān)節(jié)限位和臂角的關(guān)系，對于第二關(guān)節(jié)角θ₂和第六關(guān)節(jié)角θ₆，其逆解和臂角ψ的關(guān)系可描述為如下函數(shù)：

cosθ_i＝a sinψ+b cosψ+c

a、b和c均為系數(shù)，從而建立如下不等式解得ψ_j<0和ψ_j>0：

對于第一關(guān)節(jié)角θ₁、第三關(guān)節(jié)角θ₃、第五關(guān)節(jié)角θ₅和第七關(guān)節(jié)角θ₇在各個臂角區(qū)域內(nèi)，其角度與臂角一一對應，可描述為如下正切函數(shù)：

a_n、b_n、c_n、a_d、b_d、c_d均為系數(shù)，tanθ_i在[-π,π]范圍內(nèi)為周期函數(shù)，為了建立臂角ψ和關(guān)節(jié)上下限的關(guān)系，需要根據(jù)關(guān)節(jié)運動范圍分段建立不等式關(guān)系。例如時，將分為三段建立不等式組：

a)正切函數(shù)單調(diào)連續(xù)，分子、分母小于零，因此第i關(guān)節(jié)，i∈(1,3,5,7)在該范圍的可用臂角可以通過如下不等式組獲得:

其中+和-符號的選擇取決于對應余弦關(guān)節(jié)的正負。

b)θ_i∈[-π/2,π/2],正切函數(shù)分母大于零，因此第i關(guān)節(jié)在該范圍的可用臂角滿足如下不等式組

c)在該范圍的可用臂角滿足如下不等式組

最終第i關(guān)節(jié)，i∈(1,3,5,7)在關(guān)節(jié)限位下的可用臂角范圍為

為了剔除碰撞對仿人機械臂的自運動的影響引入碰撞檢測技術(shù)。傳統(tǒng)的離散碰撞檢測將仿人機械臂的自運動范圍分成若干個離散臂構(gòu)型來判斷是否發(fā)生碰撞，計算量大，會遺漏兩檢測點間的碰撞，產(chǎn)生“隧道效應”。因此本實施例采用球掃略凸體構(gòu)建仿人機械臂自運動形成的掃略體，可以有效地提高了碰撞檢測算法的實時性，解決“隧道效應”。

球掃掠凸體是由半徑為r球體與凸體的Minkowski和,定義如下：其中是由點集形成的凸體。

采用球掃掠凸體構(gòu)建仿人機械臂的上臂和下臂的碰撞檢測模型，仿人機械臂的上臂包括第一關(guān)節(jié)q₁至第四關(guān)節(jié)q₄，仿人機械臂的下臂包括第四關(guān)節(jié)q₄至第七關(guān)節(jié)q₇。圖3是仿人機械臂自運動形成的掃略體以及由兩個包含一個半徑和五個頂點的球掃略凸體構(gòu)建的碰撞檢測模型示意圖。圖4是仿人機械臂自運動形成的掃略體以及由兩個包含一個半徑和四個頂點的球掃略凸體構(gòu)建的碰撞檢測模型示意圖。

當所述仿人機械臂自運動時，其上臂和下臂分別形成圓錐面掃掠體，采用多個球掃掠凸體近似所述圓錐面掃掠體。假設近似允許的誤差為ε，首先計算需要的球掃掠凸體個數(shù)n＝min{1,2,3,....}，即滿足下式中變量n的最小值：

圖5是仿人機械臂的下臂上端自運動形成的軌跡和包裹下臂上端的由一個半徑和四個頂點組成的球掃掠凸體的示意圖，如圖5所示，所述仿人機械臂下臂的上端自運動形成的軌跡可用兩個球掃掠體近似，每個球掃掠凸體可由一個半徑和四個頂點組成，因此下臂形成的掃掠體可以表示為：

其中，是點自運動旋轉(zhuǎn)ψ_l/4的點；是點自運動旋轉(zhuǎn)ψ_l/2的點；同理可以獲得球掃掠凸體的各頂點。

圖6是仿人機械臂的下臂上端自運動形成的軌跡和包裹下臂上端的由一個半徑和三個頂點組成的球掃掠凸體的示意圖，如圖6所示，每個球掃掠凸體還可由一個半徑和三個頂點組成，此時仿人機械臂下臂自運動形成的掃掠體可以表示為：

其中，

球掃掠凸體可以使用更少的點構(gòu)建出精確的碰撞檢測模型，調(diào)整球掃掠體凸體的點集和半徑以最小的體積包含機器人各構(gòu)件三維模型表面上的所有點，能夠有效地增加碰撞檢測的實時性。

由于仿人機械臂的可用臂角范圍被分成了四個臂角區(qū)域，每個臂角區(qū)域都是獨立的，只有在滿足一定條件的過渡點上，仿人機械臂才能從一個臂角區(qū)域過渡到另一個臂角區(qū)域。

第一臂角區(qū)域與第二臂角區(qū)域之間的過渡條件和第三臂角區(qū)域與第四臂角區(qū)域之間的過渡條件相同，所述過渡條件為在第一臂角區(qū)域與第二臂角區(qū)域或第三臂角區(qū)域與第四臂角區(qū)域的重疊部分存在一個滿足如下關(guān)系的可用臂角ψ_cond2：

1＝a_w33sinψ_cond2+b_w33cosψ_cond2+c_w33

在該點處對應的兩組逆運動學解中第六關(guān)節(jié)角θ₆均為0度，從而使第六關(guān)節(jié)能連續(xù)地從負值運動到正值，不會產(chǎn)生關(guān)節(jié)角度的跳變。

第一臂角區(qū)域與第三臂角區(qū)域之間的過渡條件和第二臂角區(qū)域與第四臂角區(qū)域之間的過渡條件相同，涉及到第二關(guān)節(jié)角θ₂的正負轉(zhuǎn)換，所述過渡條件是在第一臂角區(qū)域與第三臂角區(qū)域或第二臂角區(qū)域與第四臂角區(qū)域的重疊部分存在一個滿足如下關(guān)系的可用臂角ψ_cond1：

1＝-a_s32sinψ_cond1-b_s32cosψ_cond1-c_s32

同理，第一臂角區(qū)域與第四臂角區(qū)域，第二臂角區(qū)域與第三臂角區(qū)域的過渡條件是在兩個區(qū)域的重疊部分存在一個同時滿足上面兩個過渡條件的可用臂角。

通過實施步驟一得到的無碰撞路徑為分段的線性路徑，成鋸齒狀，不光滑，可能導致機械臂在運行中出現(xiàn)停頓，降低工作效率。步驟二對步驟一中的無碰撞路徑進行平滑處理，能夠有效地解決該問題。

使用C++語言和open inventor編程實施本發(fā)明的復雜環(huán)境下仿人機器人臂雙層擬人運動規(guī)劃系統(tǒng)用于一個擁有19個構(gòu)件的仿人機器人上，其具體步驟如下：

(1)將仿人機器人和復雜環(huán)境的pre模型轉(zhuǎn)換成iv格式導入程序，open inventor讀取模型建立仿人機器人和復雜環(huán)境的虛擬場景。根據(jù)讀入的三維模型構(gòu)建復雜環(huán)境的基于球掃掠凸體的碰撞檢測模型。保存仿人機械臂各關(guān)節(jié)的運動范圍；

(2)輸入仿人機械臂的初始構(gòu)型和目標位姿，采用改進的RRT*算法在笛卡爾空間內(nèi)搜索新的生長點；

(3)計算新的生長點下仿人機械臂滿足關(guān)節(jié)限位約束的自運動范圍，采用多個臂角區(qū)域描述該運動范圍；

(4)采用本發(fā)明給出的方法構(gòu)建仿人機械臂在關(guān)節(jié)限位下自運動形成的掃略體，該實例中采用多個包含5個點和1個半徑的球掃略凸體表示自運動范圍的碰撞檢測模型；

(5)根據(jù)正運動學和機器人三維模型構(gòu)建仿人機器人其他部分的基于球掃掠凸體碰撞檢測模型，建立碰撞檢測對；

(6)采用基于球掃掠凸體的碰撞檢測方法計算各個碰撞檢測對之間的最小距離以及可能的碰撞點，根據(jù)碰撞點計算出引起碰撞的機械臂自運動臂構(gòu)型，剔除相應的臂角，獲得在生長點處滿足關(guān)節(jié)限位和避碰撞的所有可用臂構(gòu)型，即可用臂角范圍；

(7)根據(jù)獲得的可用的臂角范圍和過渡條件確定新生長點的有效性，如果新生長點可用，加入樹中；否則，重新采樣。從而獲得一點到達期望位姿的安全末端軌跡。對于采樣、雙向搜索以及局部路徑優(yōu)化等步驟；

(8)引入基于平滑樣條的路徑平滑方法進行路徑平滑處理，解決RRT*規(guī)劃的路徑是分段的線性路徑,存在鋸齒和不光滑的問題。完成仿人機械臂在任務空間的仿人路徑規(guī)劃；

(9)保存末端路徑對應的可用臂構(gòu)型集，將在該空間內(nèi)進行臂構(gòu)型的優(yōu)化；

(10)根據(jù)機械臂初始構(gòu)型和任務需要從獲得的可用臂構(gòu)型集中選擇優(yōu)化臂角區(qū)域；

(11)采用操作橢球優(yōu)化在優(yōu)化臂角區(qū)域內(nèi)獲得擬人的優(yōu)化臂角；

(12)處理不可用的優(yōu)化臂角，防止機械臂運動的不連續(xù)；

(13)如果需要臂角區(qū)域的過渡，則進行兩個臂角區(qū)域過渡，修改過渡點附近的優(yōu)化臂角；

(14)根據(jù)獲得優(yōu)化臂角，代入基于臂角參數(shù)的七自由度仿人機械臂解析逆運動學方程，獲得連續(xù)的各個關(guān)節(jié)角度。

實施例二：本實施例是對本實施例一所述的七自由度仿人機械臂的雙層擬人運動規(guī)劃方法作進一步限定。

本實施例所述的七自由度仿人機械臂的雙層擬人運動規(guī)劃方法，步驟D包括：

步驟a、判斷第四關(guān)節(jié)是否超限，如果第四關(guān)節(jié)超限，則新生長路徑點無效，否則，執(zhí)行步驟b；

步驟b、判斷可用臂構(gòu)型集是否為空，如果可用臂構(gòu)型集為空，則新生長路徑點無效，否則，執(zhí)行步驟c；

步驟c、判斷該新生長路徑點與前一個生長路徑點是否在同一個臂角區(qū)域內(nèi)，如果在同一個臂角區(qū)域內(nèi)，則新生長路徑點有效，否則，執(zhí)行步驟d；

步驟d、判斷新生長路徑點所在的臂角區(qū)域與前一個生長路徑點所在的臂角區(qū)域是否存在過渡點，如果存在過渡點，則新生長路徑點有效，否則，新生長路徑點無效。

雖然在本文中參照了特定的實施方式來描述本發(fā)明，但是應該理解的是，這些實施例僅是本發(fā)明的原理和應用的示例。因此應該理解的是，可以對示例性的實施例進行許多修改，并且可以設計出其他的布置，只要不偏離所附權(quán)利要求所限定的本發(fā)明的精神和范圍。應該理解的是，可以通過不同于原始權(quán)利要求所描述的方式來結(jié)合不同的從屬權(quán)利要求和本文中所述的特征。還可以理解的是，結(jié)合單獨實施例所描述的特征可以使用在其他所述實施例中。