[發明專利]一種6自由度機械臂逆運動學求解方法

有效

• 申請號: 201811098865.0
• 申請日: 2018-09-20
• 公開（公告）號: CN109344477B
• 公開（公告）日: 2022-06-24
• 發明（設計）人: 張明;李春泉;尚玉玲;黨選舉;黃紅艷;李彩林
• 申請（專利權）人: 桂林電子科技大學
• 主分類號: G06F30/17
• 分類號: G06F30/17
• 代理公司: 南寧東智知識產權代理事務所(特殊普通合伙) 45117
• 代理人: 巢雄輝;汪治興

1.一種6自由度機械臂逆運動學求解方法，其特征在于：包括以下步驟：

S1、根據機械臂的參數建立機械臂的D-H矩陣；

S2、根據機械臂的結構和機械臂的參數，確定S3的閾值，即機械臂末端位置與目標位置P^E的最大距離；

S3、采用蒙特卡洛方法為機械臂全部關節賦予隨機角度生成隨機樣本，通過所述D-H矩陣計算每個所述隨機樣本對應的機械臂末端位置，保留滿足條件的機械臂末端位置為樣本；

所述S3具體包括以下步驟：

S31、為機械臂全部關節賦予隨機角度，生成隨機樣本；

S32、將所述隨機樣本代入S1的D-H矩陣，計算對應的機械臂末端的位置

S33、判斷保留P^M同時滿足如下條件，如公式(1)所示；

式(1)中，th_x，th_y，th_z為所述閾值在x，y，z三個方向的標識；

S34、若所述隨機樣本對應的末端位置P^M滿足S33的條件，則將所述對應樣本保存至種群數組D1；

S35、循環S32-S34的操作，直至完成全部隨機樣本的計算；

S36、循環結束，得到種群數組D1；

S4、以S3保留的樣本為種群組，增加新的隨機種群組合成遺傳算法的初始種群，利用遺傳算法求得運動學逆解；

所述S4具體包括以下步驟：

S40、在種群數組D1的基礎，增加新的關節角度的隨機樣本組D2，組成遺傳算法的初始種群D；

S41、為初始種群D編碼；

S42、設定遺傳算法的參數；

S43、計算種群中每個個體的對應的機械臂末端位姿矩陣的適應度函數值；

S44、根據S43的適應度函數值，對S41種群中的個體進行遺傳選擇；

所述S44中的適應度的函數的構造如公式(3)所示：

Fit＝|ΔP|+|ΔO| (3)

式(3)中，|ΔP|為位置偏移量，|ΔO|為姿態偏移量；

式(4)中，為遺傳個體對應的機械臂末端位置，為遺傳個體對應的機械臂末端位置的x，y，z坐標，P^E為機械臂末端的目標位置；

式(5)中，為遺傳個體對應的機械臂末端姿態矩陣的第j列第i行，為機械臂末端目標體姿態矩陣的第j列第i行；

S45、判斷S44得到的遺傳選擇的個體是否滿足交叉概率，如果所述遺傳選擇的個體滿足交叉概率，則進行遺傳交叉并保存到新的種群中；如果所述遺傳選擇的個體不滿足交叉概率，則進入S46；

S46、判斷不滿足交叉概率的遺傳選擇的個體是否滿足變異概率，如果滿足變異概率，則進行變異交叉并保存到新的種群中，否則去除；

S47、在經過S45和S46操作得到的新種群的基礎上增加新的隨機個體，使得新種群保持原有種群的規模；

S48、判斷S47得到的種群是否達到完成條件，完成條件包括種群中最佳個體的適應度函數值Fitε或者S43-S47循環的次數在100-300次內，達到任意一個條件均視為達到完成條件，ε的取值范圍為1*10^-7-2*10^-7；若沒有達到完成條件，則用S47所得新種群替換前一次迭代的種群，重復S43-S47的操作；若達到完成條件則退出；

S49、輸出結果，即運動學逆解。

2.根據權利要求1所述的6自由度機械臂逆運動學求解方法，其特征在于：所述機械臂的參數包括：每個所述機械臂的長度、扭轉角度、偏移量及關節角度。

3.根據權利要求1所述的6自由度機械臂逆運動學求解方法，其特征在于：所述S41的編碼方式為實數編碼。

4.根據權利要求1所述的6自由度機械臂逆運動學求解方法，其特征在于：所述S44中的選擇采用競賽選擇法，舍棄適應度函數值過大的個體。

5.根據權利要求1所述的6自由度機械臂逆運動學求解方法，其特征在于：所述S45交叉選用多點交叉。

6.根據權利要求1所述的6自由度機械臂逆運動學求解方法，其特征在于：所述S40中，設定種群D中個體的數量為T，隨機種群D2中個體的數量T₂的計算公式為：

式(2)中，T₁為S36所得的種群數組D1中個體的數量。