七自由度仿人機械臂的雙層擬人運動規劃方法與流程
本發明涉及機械臂控制領域，具體地說，涉及一種復雜環境下七自由度仿人機械臂的雙層擬人運動規劃方法。
背景技術：
目前，基于隨機采樣的避障路徑規劃方法被廣泛地用于七自由度仿人機械臂的運動規劃中。該類方法通過碰撞檢測模塊提供采樣點的可行性信息，連接一系列從自由空間采樣的點，構建一張可行軌跡的圖，從這張圖中搜索起始點到目標點的可行路徑。該類方法不需要在仿人機械臂的關節空間中對障礙物進行精確建模，因而計算量小，即使在高維空間中規劃仿人機械臂的運動軌跡，其效率也相當高。
Bi-RRT*(Bidirectional Rapidly-Exploring Random Tree Star，雙向快速擴展隨機樹星)算法是基于隨機采樣的避障路徑規劃方法中被廣泛使用的方法。該方法將雙向搜索策掠引入現有的RRT*(Rapidly-Exploring Random Tree Star，快速擴展隨機樹星)算法中，并引入一個啟發函數。與RRT*算法相比，Bi-RRT*算法具有更強的避障路徑規劃能力。
但是，現有基于Bi-RRT*算法的七自由度仿人機械臂路徑規劃方法因Bi-RRT*算法采用碰撞檢測方法判斷采樣點的可行性使得該路徑規劃方法只適用于仿人機械臂的關節空間而不適用于仿人機械臂的任務空間，且無法同時優化仿人機械臂的末端執行器的路徑和機械臂構型。
技術實現要素：
本發明為解決現有基于Bi-RRT*算法的七自由度仿人機械臂路徑規劃方法不適用于仿人機械臂的任務空間規劃，且無法同時優化仿人機械臂的末端執行器的路徑和機械臂構型的問題，提出了一種七自由度仿人機械臂的雙層擬人運動規劃方法。
本發明所述的七自由度仿人機械臂的雙層擬人運動規劃方法包括第一層運動規劃和第二層運動規劃，第一層運動規劃為仿人機械臂在復雜環境下的任務空間的擬人路徑規劃，第二層運動規劃為仿人機械臂沿末端無碰撞路徑的可用臂構型空間的擬人臂構型規劃；
所述方法包括：
步驟一、在所述仿人機械臂的任務空間內，為所述仿人機械臂的末端執行器規劃出一條從其初始位置至目標位置的擬人的無碰撞路徑；
步驟二、采用基于平滑樣條的路徑平滑方法對所述無碰撞路徑進行平滑處理，形成所述末端執行器的任務空間路徑規劃；
步驟三、在所述末端執行器能夠自其初始位置經平滑處理后的無碰撞路徑到達目標位置情形下，計算并保存所有滿足關節限位關系和避碰撞約束的可用機械臂構型；
步驟四、根據人臂的運動學特征，在沿末端執行器無碰撞路徑的所有可用機械臂構型中選擇出最優機械臂構型集；
步驟五、控制所述仿人機械臂依次完成所述最優機械臂構型集中的所有臂構型，使所述末端執行器完成所述平滑處理后的無碰撞路徑；
步驟一和步驟二構成仿人機械臂在復雜環境下的任務空間的擬人路徑規劃，步驟三和步驟四構成仿人機械臂沿末端無碰撞路徑的可用臂構型空間的擬人臂構型規劃。
作為優選的是，步驟一包括：
步驟一一、在所述仿人機械臂的任務空間內，采用隨機采樣的方式，以初始點為基礎構建第一樹的節點集，以目標點為基礎構建第二樹的節點集；
初始點和目標點分別為所述末端執行器的初始位置和目標位置；
在六維任務空間內通過隨機采樣的方式獲得的多個機械臂末端點相互獨立，恒等分布；
步驟一二、在機械臂的任務空間內分別以初始點和目標點為根節點擴展第一樹和第二樹，所述第一樹和第二樹均為啟發式快速擴展隨機樹，二者以相同的擴展方式相對交替擴展，直至二者相遇；
步驟一三、當第一樹和第二樹相遇時，計算出此時從初始點至目標點的路徑；
步驟一四、采用基于三角不等式的路徑優化方法對步驟一三所述路徑進行局部優化處理，并得到局部優化路徑；
比較步驟一三所述路徑和所述局部優化路徑的代價，并保存代價較小的機械臂末端路徑，該路徑為無碰撞機械臂末端路徑。
進一步的是，步驟一二包括：
步驟一二一、在所述仿人機械臂的任務空間內，采用隨機采樣的方式獲取一個機械臂末端點，并采用基于臂構型描述的末端新生長路徑點有效性判斷方法對該點進行有效性判斷，如果該點被判斷為有效，將該點作為第一樹的新生長路徑點，并執行步驟一二二；
否則，執行步驟一二一；
步驟一二二、在第一樹的節點集中選擇出第一樹的新生長路徑點的最近節點；
步驟一二三、在第一樹的新生長路徑點的附近節點集中，采用路徑代價算法選擇出路徑代價最小的附近節點，并將該節點作為第一樹的新生長路徑點的父節點；
依次連接第一樹的新生長路徑點的最近節點、第一樹的新生長路徑點的父節點和第一樹的新生長路徑點，完成第一樹的一次擴展；
步驟一二四、判斷第一樹的新生長路徑點是否與第二樹相遇，如果二者相遇，執行步驟一三，否則，執行步驟一二五；
第一樹的新生長路徑點與第二樹是否相遇的判斷方法為：在第二樹的節點集中選擇出第一樹的新生長路徑點的最近節點，如果該節點與第一樹的新生長路徑點的間距小于一個步長，第一樹的新生長路徑點與第二樹相遇，否則，二者未相遇；
步驟一二五、完成第二樹的一次擴展，并判斷第二樹的新生長路徑點是否與第一樹相遇，如果二者相遇，執行步驟一三，否則，執行步驟一二六；
第二樹的新生長路徑點與第一樹是否相遇的判斷方法為：在第一樹的節點集中選擇出第二樹的新生長路徑點的最近節點，如果該節點與第二樹的新生長路徑點的間距小于一個步長，第二樹的新生長路徑點與第一樹相遇，否則，二者未相遇；
步驟一二六、完成第二樹的一次擴展，并判斷第二樹的新生長路徑點是否與第一樹相遇，如果二者相遇，執行步驟一三，否則，執行步驟一二七；
步驟一二七、以擴展兩次第一樹和擴展兩次第二樹的方式循環擴展，直至第一樹和第二樹相遇，二者相遇后，執行步驟一三。
進一步的是，步驟一二一所述的基于臂構型描述的末端新生長路徑點有效性判斷方法包括：
步驟A、將仿人機械臂的任務空間內的隨機采樣點假設為仿人機械臂末端新生長路徑點，計算仿人機械臂在該新生長路徑點下的滿足仿人機械臂關節限位關系的可用臂構型集；
該步驟的具體過程為：
以第二關節為坐標原點建立三維基坐標，給定所述仿人機械臂末端新生長路徑點相對于所述三維基坐標的期望位置和期望姿態，通過建立基于臂角的仿人機械臂運動學模型獲得各個關節角與臂角的關系；
第一關節至第七關節依次分布在所述仿人機械臂的基座與末端執行器之間；
根據各個關節角與臂角的關系，將第二關節和第六關節的運動范圍分別分為正負兩個區域，進而形成四個臂角區域；
所述四個臂角區域分別為：
第一臂角區域，滿足第二關節角和第六關節角同時小于0；
第二臂角區域，滿足第二關節角小于0，第六關節角大于0；
第三臂角區域，滿足第二關節角大于0，第六關節角小于0；
第四臂角區域，滿足第二關節角和第六關節角同時大于0；
分別計算第一臂角區域、第二臂角區域、第三臂角區域和第四臂角區域內所述仿人機械臂的可用臂角范圍，從而計算出所述仿人機械臂在該新生長路徑點下的滿足仿人機械臂關節限位關系的可用臂構型集；
步驟B、建立仿人機械臂在該新生長路徑點下的基于球掃掠凸體的碰撞檢測模型，計算出該新生長路徑點下同時滿足仿人機械臂關節限位和避碰撞約束的可用臂構型集；
該步驟的具體過程為：
建立仿人機械臂在關節限位下自運動形成的掃掠體的基于球掃掠凸體的碰撞檢測模型：
當仿人機械臂自運動時，其上臂和下臂分別形成圓錐面掃掠體，根據第四關節自運動形成的軌跡，采用n個球掃掠凸體構建所述圓錐面掃掠體；
上臂包括第一關節至第四關節，下臂包括第四關節至第七關節，n為正整數；
所述球掃掠凸體由球半徑和五個頂點構成，該球掃掠凸體定義為：
其中，r為球掃掠凸體V1的半徑，球掃掠凸體V1的五個頂點分別為至為第一關節、第二關節與第三關節的旋轉軸線的交點或者第五關節、第六關節與第七關節的旋轉軸線的交點，為第四關節的位置，是自運動旋轉ψl/2n的點，是自運動旋轉ψl/n的點，ψl為自運動旋轉的角度；
或者，所述球掃掠凸體由球半徑和四個頂點構成，該球掃掠凸體定義為：
其中，球掃掠凸體V2的四個頂點分別為至是自運動旋轉ψl/n的點；
分別建立仿人機械人中除機械臂外的其他部件和周圍靜止環境的基于球掃掠凸體的碰撞檢測模型；
采用基于球掃掠凸體的碰撞檢測方法檢測仿人機械臂自運動形成的掃掠體的基于球掃掠凸體的碰撞檢測模型與仿人機械人的其他部件以及周圍靜止環境的基于球掃掠凸體的碰撞檢測模型是否發生碰撞以及是否存在可能的碰撞點，剔除可能產出碰撞的臂構型，獲得仿人機械臂在該新生長路徑點下同時滿足仿人機械臂關節限位關系和避碰撞約束的可用臂構型集；
步驟C、計算所述四個臂角區域中任意兩個臂角區域之間的過渡條件；
步驟D、根據仿人機械臂在該新生長路徑點下同時滿足仿人機械臂關節限位關系和避碰撞約束的可用臂構型集以及所述任意兩個臂角區域之間的過渡條件判斷該新生長路徑點的有效性。
進一步的是，步驟D包括：
步驟a、判斷第四關節是否超限，如果第四關節超限，則新生長路徑點無效，否則，執行步驟b；
步驟b、判斷可用臂構型集是否為空，如果可用臂構型集為空，則新生長路徑點無效，否則，執行步驟c；
步驟c、判斷該新生長路徑點與前一個生長路徑點是否在同一個臂角區域內，如果在同一個臂角區域內，則新生長路徑點有效，否則，執行步驟d；
步驟d、判斷新生長路徑點所在的臂角區域與前一個生長路徑點所在的臂角區域是否存在過渡點，如果存在過渡點，則新生長路徑點有效，否則，新生長路徑點無效。
優選的是，步驟四采用冗余解優化方法在所有可用機械臂構型中選擇出最優機械臂構型集，所述冗余解優化方法包括：
步驟四一、選擇最優臂角區域；
步驟四二、在優化臂角區域內優化擬人臂角；
步驟四三、處理不可用優化臂角；
步驟四四、臂角區域過渡；
步驟四五、采用基于臂角的解析逆運動學方法計算各關節角度。
本發明所述的七自由度仿人機械臂的雙層擬人運動規劃方法包括雙層擬人運動規劃，第一層擬人運動規劃為仿人機械臂在復雜環境下的任務空間的擬人路徑規劃，該規劃通過實施步驟一和步驟二實現，第二層擬人運動規劃為仿人機械臂的可用臂構型空間的擬人臂構型規劃，該規劃通過實施步驟三和步驟四實現。所述雙層擬人運動規劃解決了現有基于Bi-RRT*算法的七自由度仿人機械臂路徑規劃方法無法同時優化仿人機械臂的末端執行器的路徑和機械臂構型的問題。
本發明所述的七自由度仿人機械臂的雙層擬人運動規劃方法采用基于臂構型描述的七自由度仿人機械臂末端點有效性判斷方法替代現有Bi-RRT*算法中的碰撞檢測方法，使得該運動規劃方法適用于仿人機械臂的任務空間規劃。
附圖說明
在下文中將基于實施例并參考附圖來對本發明所述的七自由度仿人機械臂的雙層擬人運動規劃方法進行更詳細的描述，其中：
圖1是實施例一提及的參考平面、臂平面和臂角的示意圖；
圖2是實施例一提及的基于臂角的仿人機械臂運動學模型的示意圖；
圖3是實施例一提及的仿人機械臂自運動形成的掃略體以及由兩個包含一個半徑和五個頂點的球掃略凸體構建的碰撞檢測模型示意圖；
圖4是實施例一提及的仿人機械臂自運動形成的掃略體以及由兩個包含一個半徑和四個頂點的球掃略凸體構建的碰撞檢測模型示意圖；
圖5是實施例一提及的仿人機械臂的下臂上端自運動形成的軌跡和包裹下臂上端的由一個半徑和四個頂點組成的球掃掠凸體的示意圖；
圖6是實施例一提及的仿人機械臂的下臂上端自運動形成的軌跡和包裹下臂上端的由一個半徑和三個頂點組成的球掃掠凸體的示意圖。
在附圖中，相同的部件使用相同的附圖標記。附圖并未按照實際的比例。
具體實施方式
下面將結合附圖對本發明所述的七自由度仿人機械臂的雙層擬人運動規劃方法作進一步說明。
實施例一：下面結合圖1至圖6詳細地說明本實施例。本實施例所述的七自由度仿人機械臂的雙層擬人運動規劃方法包括第一層運動規劃和第二層運動規劃，第一層運動規劃為仿人機械臂在復雜環境下的任務空間的擬人路徑規劃，第二層運動規劃為仿人機械臂沿末端無碰撞路徑的可用臂構型空間的擬人臂構型規劃；
所述方法包括：
步驟一、在所述仿人機械臂的任務空間內，為所述仿人機械臂的末端執行器規劃出一條從其初始位置至目標位置的擬人的無碰撞路徑；
步驟二、采用基于平滑樣條的路徑平滑方法對所述無碰撞路徑進行平滑處理，形成所述末端執行器的任務空間路徑規劃；
步驟三、在所述末端執行器能夠自其初始位置經平滑處理后的無碰撞路徑到達目標位置情形下，計算并保存所有滿足關節限位關系和避碰撞約束的可用機械臂構型；
步驟四、根據人臂的運動學特征，在沿末端執行器無碰撞路徑的所有可用機械臂構型中選擇出最優機械臂構型集；
步驟五、控制所述仿人機械臂依次完成所述最優機械臂構型集中的所有臂構型，使所述末端執行器完成所述平滑處理后的無碰撞路徑；
步驟一和步驟二構成仿人機械臂在復雜環境下的任務空間的擬人路徑規劃，步驟三和步驟四構成仿人機械臂沿末端無碰撞路徑的可用臂構型空間的擬人臂構型規劃；
所述步驟一包括：
步驟一一、在所述仿人機械臂的任務空間內，采用隨機采樣的方式，以初始點為基礎構建第一樹的節點集，以目標點為基礎構建第二樹的節點集；
初始點和目標點分別為所述末端執行器的初始位置和目標位置；
在六維任務空間內通過隨機采樣的方式獲得的多個機械臂末端點相互獨立，恒等分布；
步驟一二、在機械臂的任務空間內分別以初始點和目標點為根節點擴展第一樹和第二樹，所述第一樹和第二樹均為啟發式快速擴展隨機樹，二者以相同的擴展方式相對交替擴展，直至二者相遇；
步驟一三、當第一樹和第二樹相遇時，計算出此時從初始點至目標點的路徑；
步驟一四、采用基于三角不等式的路徑優化方法對步驟一三所述路徑進行局部優化處理，并得到局部優化路徑；
比較步驟一三所述路徑和所述局部優化路徑的代價，并保存代價較小的機械臂末端路徑，該路徑為無碰撞機械臂末端路徑；
所述步驟一二包括：
步驟一二一、在所述仿人機械臂的任務空間內，采用隨機采樣的方式獲取一個機械臂末端點，并采用基于臂構型描述的末端新生長路徑點有效性判斷方法對該點進行有效性判斷，如果該點被判斷為有效，將該點作為第一樹的新生長路徑點，并執行步驟一二二；
否則，執行步驟一二一；
步驟一二二、在第一樹的節點集中選擇出第一樹的新生長路徑點的最近節點；
步驟一二三、在第一樹的新生長路徑點的附近節點集中，采用路徑代價算法選擇出路徑代價最小的附近節點，并將該節點作為第一樹的新生長路徑點的父節點；
依次連接第一樹的新生長路徑點的最近節點、第一樹的新生長路徑點的父節點和第一樹的新生長路徑點，完成第一樹的一次擴展；
步驟一二四、判斷第一樹的新生長路徑點是否與第二樹相遇，如果二者相遇，執行步驟一三，否則，執行步驟一二五；
第一樹的新生長路徑點與第二樹是否相遇的判斷方法為：在第二樹的節點集中選擇出第一樹的新生長路徑點的最近節點，如果該節點與第一樹的新生長路徑點的間距小于一個步長，第一樹的新生長路徑點與第二樹相遇，否則，二者未相遇；
步驟一二五、完成第二樹的一次擴展，并判斷第二樹的新生長路徑點是否與第一樹相遇，如果二者相遇，執行步驟一三，否則，執行步驟一二六；
第二樹的新生長路徑點與第一樹是否相遇的判斷方法為：在第一樹的節點集中選擇出第二樹的新生長路徑點的最近節點，如果該節點與第二樹的新生長路徑點的間距小于一個步長，第二樹的新生長路徑點與第一樹相遇，否則，二者未相遇；
步驟一二六、完成第二樹的一次擴展，并判斷第二樹的新生長路徑點是否與第一樹相遇，如果二者相遇，執行步驟一三，否則，執行步驟一二七；
步驟一二七、以擴展兩次第一樹和擴展兩次第二樹的方式循環擴展，直至第一樹和第二樹相遇，二者相遇后，執行步驟一三；
步驟一二一所述的基于臂構型描述的末端新生長路徑點有效性判斷方法包括：
步驟A、將仿人機械臂的任務空間內的隨機采樣點假設為仿人機械臂末端新生長路徑點，計算仿人機械臂在該新生長路徑點下的滿足仿人機械臂關節限位關系的可用臂構型集；
該步驟的具體過程為：
以第二關節為坐標原點建立三維基坐標，給定所述仿人機械臂末端新生長路徑點相對于所述三維基坐標的期望位置和期望姿態，通過建立基于臂角的仿人機械臂運動學模型獲得各個關節角與臂角的關系；
第一關節至第七關節依次分布在所述仿人機械臂的基座與末端執行器之間；
根據各個關節角與臂角的關系，將第二關節和第六關節的運動范圍分別分為正負兩個區域，進而形成四個臂角區域；
所述四個臂角區域分別為：
第一臂角區域，滿足第二關節角和第六關節角同時小于0；
第二臂角區域，滿足第二關節角小于0，第六關節角大于0；
第三臂角區域，滿足第二關節角大于0，第六關節角小于0；
第四臂角區域，滿足第二關節角和第六關節角同時大于0；
分別計算第一臂角區域、第二臂角區域、第三臂角區域和第四臂角區域內所述仿人機械臂的可用臂角范圍，從而計算出所述仿人機械臂在該新生長路徑點下的滿足仿人機械臂關節限位關系的可用臂構型集；
步驟B、建立仿人機械臂在該新生長路徑點下的基于球掃掠凸體的碰撞檢測模型，計算出該新生長路徑點下同時滿足仿人機械臂關節限位和避碰撞約束的可用臂構型集；
該步驟的具體過程為：
建立仿人機械臂在關節限位下自運動形成的掃掠體的基于球掃掠凸體的碰撞檢測模型：
當仿人機械臂自運動時，其上臂和下臂分別形成圓錐面掃掠體，根據第四關節自運動形成的軌跡，采用n個球掃掠凸體構建所述圓錐面掃掠體；
上臂包括第一關節至第四關節，下臂包括第四關節至第七關節，n為正整數；
所述球掃掠凸體由球半徑和五個頂點構成，該球掃掠凸體定義為：
其中，r為球掃掠凸體V1的半徑，球掃掠凸體V1的五個頂點分別為至為第一關節、第二關節與第三關節的旋轉軸線的交點或者第五關節、第六關節與第七關節的旋轉軸線的交點，為第四關節的位置，是自運動旋轉ψl/2n的點，是自運動旋轉ψl/n的點，ψl為自運動旋轉的角度；
或者，所述球掃掠凸體由球半徑和四個頂點構成，該球掃掠凸體定義為：
其中，球掃掠凸體V2的四個頂點分別為至是自運動旋轉ψl/n的點；
分別建立仿人機械人中除機械臂外的其他部件和周圍靜止環境的基于球掃掠凸體的碰撞檢測模型；
采用基于球掃掠凸體的碰撞檢測方法檢測仿人機械臂自運動形成的掃掠體的基于球掃掠凸體的碰撞檢測模型與仿人機械人的其他部件以及周圍靜止環境的基于球掃掠凸體的碰撞檢測模型是否發生碰撞以及是否存在可能的碰撞點，剔除可能產出碰撞的臂構型，獲得仿人機械臂在該新生長路徑點下同時滿足仿人機械臂關節限位關系和避碰撞約束的可用臂構型集；
步驟C、計算所述四個臂角區域中任意兩個臂角區域之間的過渡條件；
步驟D、根據仿人機械臂在該新生長路徑點下同時滿足仿人機械臂關節限位關系和避碰撞約束的可用臂構型集以及所述任意兩個臂角區域之間的過渡條件判斷該新生長路徑點的有效性；
步驟四采用冗余解優化方法在所有可用機械臂構型中選擇出最優機械臂構型集，所述冗余解優化方法包括：
步驟四一、選擇最優臂角區域；
步驟四二、在優化臂角區域內優化擬人臂角；
步驟四三、處理不可用優化臂角；
步驟四四、臂角區域過渡；
步驟四五、采用基于臂角的解析逆運動學方法計算各關節角度。
本實施例選用臂角作為冗余參數來描述仿人機械臂的自運動。圖1是參考平面、臂平面和臂角的示意圖，如圖1所示，Ps、Pe和Pw分別為仿人機械臂的肩關節、肘關節和腕關節，ψ為臂角。圖2是基于臂角的仿人機械臂運動學模型的示意圖，如圖2所示，q1至q7分別為仿人機械臂的第一關節至第七關節，第一關節q1至第三關節q3、第四關節q4和第五關節q5至第七關節q7分別對應肩關節Ps、肘關節Pe和腕關節Pw，坐標原點O、X軸、Y軸和Z軸構成三維基坐標。由肩關節Ps、肘關節Pe和腕關節Pw構成的平面為臂平面。當仿人機械臂的末端執行器的位姿不變時，由于仿人機械臂包含一個冗余自由度，肘關節Pe能夠以肩關節Ps和腕關節Pw共同的軸線為軸旋轉，進而形成多個角度不同的臂平面。定義第三關節q3為0度時的臂平面為參考平面，參考平面和臂平面的夾角為臂角ψ。
仿人機械臂自運動的范圍會受到關節限位和障礙物位置的影響，當仿人機械臂自運動的范圍為空時，表明仿人機械臂受關節限位和障礙物位置的限制無法到達該位置。本實施例基于該原理提出基于臂構型描述的末端新生長路徑點有效性判斷方法。
在給定末端位姿下，各個關節角與臂角ψ的關系為：
θ1＝a tan2{sign(sinθ2)(-as22sinψ-bs22cosψ-cs22),
sign(sinθ2)(-as12sinψ-bs12cosψ-cs12)}
θ2＝±a cos(-as32sinψ-bs32cosψ-cs32)
θ3＝a tan2{sign(sinθ2)(as33sinψ+bs33cosψ+cs33),
sign(sinθ2)(-as31sinψ-bs31cosψ-cs31)}
θ5＝a tan2{sign(sinθ6)(aw23sinψ+bw23cosψ+cw23),
sign(sinθ6)(aw13sinψ+bw13cosψ+cw13)}
θ6＝±a cos(aw33sinψ+bw33cosψ+cw33)
θ7＝a tan2{sign(sinθ6)(aw32sinψ+bw32cosψ+cw32),
sign(sinθ6)(-aw31sinψ-bw31cosψ-cw31)}
其中，θ1至θ7分別為第一關節角至第七關節角，asij,bsij,csij,awij,bwij,cwij分別為矩陣As,Bs,Cs,Aw,Bw，Cw的(i,j)元素，0xsw是肩關節Ps到腕關節Pw的向量，dse是肩關節Ps到肘關節Pe的向量，dew是肘關節Pe到腕關節的向量，dse和dew是常向量。
根據上述各個關節角與臂角ψ的關系可知：第四關節角θ4只與仿人機械臂的末端執行器的位姿有關，第二關節角θ2和第六關節角θ6與仿人機械臂的末端執行器的位姿和臂角ψ有關，第一關節角θ1、第三關節角θ3、第五關節角θ5和第七關節角θ7除了與仿人機械臂的末端執行器的位姿和臂角ψ有關，還與第二關節角θ2和第六關節角θ6的正負有關。
假設第i關節角的范圍為(i＝1,2,…,7)θi∈[-π,π]，其中，和分別是第i關節的下限和上限。為了在[-π,π]的范圍內確定各個關節角與臂角ψ的唯一對應關系，將第二關節角θ2和第六關節角θ6分別分為正負兩個區域，從而形成四個臂角區域：
第一臂角區域：即θ2<0,θ6<0，所述仿人機械臂在該臂角區域內的可用臂角范圍為：
第二臂角區域：即θ2<0,θ6>0，所述仿人機械臂在該臂角區域內的可用臂角范圍為：
第三臂角區域：即θ2>0,θ6<0，所述仿人機械臂在該臂角區域內的可用臂角范圍為：
第四臂角區域：即θ2>0,θ6>0，所述仿人機械臂在該臂角區域內的可用臂角范圍為：
其中，(i＝1,3,5,7)是第二關節角或第六關節角為正時，第i關節的有效臂角范圍，(i＝1,3,5,7)是第二關節角或第六關節角為負時，第i關節的有效臂角范圍，ψj<0，(j＝2,6)為第j關節在關節運動范圍內的有效臂角范圍，ψj>0，(j＝2,6)為第j關節在關節運動范圍內的有效臂角范圍；
所述仿人機械臂末端點在該新生長路徑點下滿足關節限位關系的臂構型集為；
隨后建立各個關節限位和臂角的關系，對于第二關節角θ2和第六關節角θ6，其逆解和臂角ψ的關系可描述為如下函數：
cosθi＝a sinψ+b cosψ+c
a、b和c均為系數，從而建立如下不等式解得ψj<0和ψj>0：
對于第一關節角θ1、第三關節角θ3、第五關節角θ5和第七關節角θ7在各個臂角區域內，其角度與臂角一一對應，可描述為如下正切函數：
an、bn、cn、ad、bd、cd均為系數，tanθi在[-π,π]范圍內為周期函數，為了建立臂角ψ和關節上下限的關系，需要根據關節運動范圍分段建立不等式關系。例如時，將分為三段建立不等式組：
a)正切函數單調連續，分子、分母小于零，因此第i關節，i∈(1,3,5,7)在該范圍的可用臂角可以通過如下不等式組獲得:
其中+和-符號的選擇取決于對應余弦關節的正負。
b)θi∈[-π/2,π/2],正切函數分母大于零，因此第i關節在該范圍的可用臂角滿足如下不等式組
c)在該范圍的可用臂角滿足如下不等式組
最終第i關節，i∈(1,3,5,7)在關節限位下的可用臂角范圍為
為了剔除碰撞對仿人機械臂的自運動的影響引入碰撞檢測技術。傳統的離散碰撞檢測將仿人機械臂的自運動范圍分成若干個離散臂構型來判斷是否發生碰撞，計算量大，會遺漏兩檢測點間的碰撞，產生“隧道效應”。因此本實施例采用球掃略凸體構建仿人機械臂自運動形成的掃略體，可以有效地提高了碰撞檢測算法的實時性，解決“隧道效應”。
球掃掠凸體是由半徑為r球體與凸體的Minkowski和,定義如下：其中是由點集形成的凸體。
采用球掃掠凸體構建仿人機械臂的上臂和下臂的碰撞檢測模型，仿人機械臂的上臂包括第一關節q1至第四關節q4，仿人機械臂的下臂包括第四關節q4至第七關節q7。圖3是仿人機械臂自運動形成的掃略體以及由兩個包含一個半徑和五個頂點的球掃略凸體構建的碰撞檢測模型示意圖。圖4是仿人機械臂自運動形成的掃略體以及由兩個包含一個半徑和四個頂點的球掃略凸體構建的碰撞檢測模型示意圖。
當所述仿人機械臂自運動時，其上臂和下臂分別形成圓錐面掃掠體，采用多個球掃掠凸體近似所述圓錐面掃掠體。假設近似允許的誤差為ε，首先計算需要的球掃掠凸體個數n＝min{1,2,3,....}，即滿足下式中變量n的最小值：
圖5是仿人機械臂的下臂上端自運動形成的軌跡和包裹下臂上端的由一個半徑和四個頂點組成的球掃掠凸體的示意圖，如圖5所示，所述仿人機械臂下臂的上端自運動形成的軌跡可用兩個球掃掠體近似，每個球掃掠凸體可由一個半徑和四個頂點組成，因此下臂形成的掃掠體可以表示為：
其中，是點自運動旋轉ψl/4的點；是點自運動旋轉ψl/2的點；同理可以獲得球掃掠凸體的各頂點。
圖6是仿人機械臂的下臂上端自運動形成的軌跡和包裹下臂上端的由一個半徑和三個頂點組成的球掃掠凸體的示意圖，如圖6所示，每個球掃掠凸體還可由一個半徑和三個頂點組成，此時仿人機械臂下臂自運動形成的掃掠體可以表示為：
其中，
球掃掠凸體可以使用更少的點構建出精確的碰撞檢測模型，調整球掃掠體凸體的點集和半徑以最小的體積包含機器人各構件三維模型表面上的所有點，能夠有效地增加碰撞檢測的實時性。
由于仿人機械臂的可用臂角范圍被分成了四個臂角區域，每個臂角區域都是獨立的，只有在滿足一定條件的過渡點上，仿人機械臂才能從一個臂角區域過渡到另一個臂角區域。
第一臂角區域與第二臂角區域之間的過渡條件和第三臂角區域與第四臂角區域之間的過渡條件相同，所述過渡條件為在第一臂角區域與第二臂角區域或第三臂角區域與第四臂角區域的重疊部分存在一個滿足如下關系的可用臂角ψcond2：
1＝aw33sinψcond2+bw33cosψcond2+cw33
在該點處對應的兩組逆運動學解中第六關節角θ6均為0度，從而使第六關節能連續地從負值運動到正值，不會產生關節角度的跳變。
第一臂角區域與第三臂角區域之間的過渡條件和第二臂角區域與第四臂角區域之間的過渡條件相同，涉及到第二關節角θ2的正負轉換，所述過渡條件是在第一臂角區域與第三臂角區域或第二臂角區域與第四臂角區域的重疊部分存在一個滿足如下關系的可用臂角ψcond1：
1＝-as32sinψcond1-bs32cosψcond1-cs32
同理，第一臂角區域與第四臂角區域，第二臂角區域與第三臂角區域的過渡條件是在兩個區域的重疊部分存在一個同時滿足上面兩個過渡條件的可用臂角。
通過實施步驟一得到的無碰撞路徑為分段的線性路徑，成鋸齒狀，不光滑，可能導致機械臂在運行中出現停頓，降低工作效率。步驟二對步驟一中的無碰撞路徑進行平滑處理，能夠有效地解決該問題。
使用C++語言和open inventor編程實施本發明的復雜環境下仿人機器人臂雙層擬人運動規劃系統用于一個擁有19個構件的仿人機器人上，其具體步驟如下：
(1)將仿人機器人和復雜環境的pre模型轉換成iv格式導入程序，open inventor讀取模型建立仿人機器人和復雜環境的虛擬場景。根據讀入的三維模型構建復雜環境的基于球掃掠凸體的碰撞檢測模型。保存仿人機械臂各關節的運動范圍；
(2)輸入仿人機械臂的初始構型和目標位姿，采用改進的RRT*算法在笛卡爾空間內搜索新的生長點；
(3)計算新的生長點下仿人機械臂滿足關節限位約束的自運動范圍，采用多個臂角區域描述該運動范圍；
(4)采用本發明給出的方法構建仿人機械臂在關節限位下自運動形成的掃略體，該實例中采用多個包含5個點和1個半徑的球掃略凸體表示自運動范圍的碰撞檢測模型；
(5)根據正運動學和機器人三維模型構建仿人機器人其他部分的基于球掃掠凸體碰撞檢測模型，建立碰撞檢測對；
(6)采用基于球掃掠凸體的碰撞檢測方法計算各個碰撞檢測對之間的最小距離以及可能的碰撞點，根據碰撞點計算出引起碰撞的機械臂自運動臂構型，剔除相應的臂角，獲得在生長點處滿足關節限位和避碰撞的所有可用臂構型，即可用臂角范圍；
(7)根據獲得的可用的臂角范圍和過渡條件確定新生長點的有效性，如果新生長點可用，加入樹中；否則，重新采樣。從而獲得一點到達期望位姿的安全末端軌跡。對于采樣、雙向搜索以及局部路徑優化等步驟；
(8)引入基于平滑樣條的路徑平滑方法進行路徑平滑處理，解決RRT*規劃的路徑是分段的線性路徑,存在鋸齒和不光滑的問題。完成仿人機械臂在任務空間的仿人路徑規劃；
(9)保存末端路徑對應的可用臂構型集，將在該空間內進行臂構型的優化；
(10)根據機械臂初始構型和任務需要從獲得的可用臂構型集中選擇優化臂角區域；
(11)采用操作橢球優化在優化臂角區域內獲得擬人的優化臂角；
(12)處理不可用的優化臂角，防止機械臂運動的不連續；
(13)如果需要臂角區域的過渡，則進行兩個臂角區域過渡，修改過渡點附近的優化臂角；
(14)根據獲得優化臂角，代入基于臂角參數的七自由度仿人機械臂解析逆運動學方程，獲得連續的各個關節角度。
實施例二：本實施例是對本實施例一所述的七自由度仿人機械臂的雙層擬人運動規劃方法作進一步限定。
本實施例所述的七自由度仿人機械臂的雙層擬人運動規劃方法，步驟D包括：
步驟a、判斷第四關節是否超限，如果第四關節超限，則新生長路徑點無效，否則，執行步驟b；
步驟b、判斷可用臂構型集是否為空，如果可用臂構型集為空，則新生長路徑點無效，否則，執行步驟c；
步驟c、判斷該新生長路徑點與前一個生長路徑點是否在同一個臂角區域內，如果在同一個臂角區域內，則新生長路徑點有效，否則，執行步驟d；
步驟d、判斷新生長路徑點所在的臂角區域與前一個生長路徑點所在的臂角區域是否存在過渡點，如果存在過渡點，則新生長路徑點有效，否則，新生長路徑點無效。
雖然在本文中參照了特定的實施方式來描述本發明，但是應該理解的是，這些實施例僅是本發明的原理和應用的示例。因此應該理解的是，可以對示例性的實施例進行許多修改，并且可以設計出其他的布置，只要不偏離所附權利要求所限定的本發明的精神和范圍。應該理解的是，可以通過不同于原始權利要求所描述的方式來結合不同的從屬權利要求和本文中所述的特征。還可以理解的是，結合單獨實施例所描述的特征可以使用在其他所述實施例中。
- 還沒有人留言評論。精彩留言會獲得點贊！