[發明專利]一種基于BAS算法的六自由度偏置機器人逆解方法

在審

• 申請號: 202010593995.2
• 申請日: 2020-06-28
• 公開（公告）號: CN111482969A
• 公開（公告）日: 2020-08-04
• 發明（設計）人: 甘亞光;張曉龍;甘亞輝;劉元建
• 申請（專利權）人: 納博特南京科技有限公司
• 主分類號: B25J9/16
• 分類號: B25J9/16;G06F17/15
• 代理公司: 暫無信息
• 代理人: 暫無信息

1.一種基于BAS算法的六自由度偏置機器人逆解方法，其特征在于，包括以下步驟：

S1，構建手腕偏置型自動操作機器的sdh模型：手腕偏置型自動操作機器由基座、末端執行器和5個連桿以及6個旋轉關節組成；基于標準的DH參數法建立各關節坐標系，坐標系0為基坐標系，坐標系i為建立在連桿i的末端即旋轉關節i+1上的坐標系，坐標系i具有X_i、Y_i、Z_i軸，關節轉角θ_i表示X_i軸與X_i-1軸的夾角，i＝1,2,3,4,5,6，坐標系6為末端執行器坐標系；

S2，將手腕偏置型自動操作機器的末端坐標系的目標位姿當作手腕無偏置型自動操作機器的末端坐標系的位姿，求得手腕無偏置型自動操作機器的運動學逆解的解析解即旋轉關節的無偏置關節轉角θ₁′至θ₆′，作為迭代初始點X₀；

S3，構建適應度函數F(X)：

；

其中X為旋轉關節的關節轉角θ₁至θ₆，表示引入偏置參數的坐標系6相對于坐標系0的齊次坐標變換矩陣的第p行第q列元素，p=1,2,3，q=1,2,3,4，nx、ny、nz分別為坐標系6的X₆軸與坐標系0的X₀、Y₀、Z₀軸的夾角余弦值；ox、oy、oz分別為坐標系6的Y₆軸與坐標系0的X₀、Y₀、Z₀軸的夾角余弦值；ax、ay、az分別為坐標系6的Z₆軸與坐標系0的X₀、Y₀、Z₀軸的夾角余弦值；px、py、pz為坐標系6的原點在坐標系0中的笛卡爾坐標；

S4，基于BAS算法求取手腕偏置型自動操作機器的逆運動學解，具體包括以下步驟：

S41，取初始點X₀，置迭代次數k=0；

S42，根據F(X)的2-范數||F(X)||₂是否到達設定閾值或是否達到最大迭代次數，判斷是否跳出循環，是則跳出循環，否則進入下一次迭代；

S43，確定第k+1中擬更新的X_k+1，并判斷該第k+1次迭代是否有效，具體包括以下步驟：

S431，計算左須位置xl_k和右須位置xr_k：xl_k=X_k+d_k*dir_k/2，xr_k=X_k-d_k*dir_k/2，

其中，X_k表示第k次迭代時旋轉關節的各關節轉角，也是天牛個體質心的位置，d_k表示天牛個體大小，dir_k表示天牛飛行方向歸一化的向量，dir_k=dir_k′/norm(dir_k′)，dir_k′表示天牛飛行方向的隨機向量，其為rands(6,1)，用于產生6*1階隨機向量的函數，norm表示求向量范數的函數；d_k通過調整函數進行控制，其滿足d_k=d_k-1*[(n+1-k)/(n+1)]^ω ，其中n為迭代總次數，ω為常數，表示天牛個體大小遞減的程度；

S432，計算第k+1次迭代中擬更新的X_k+1的值及相應的F(X_k+1)：

X_k+1= X_k-step_k* dir_k *sign(F_left-F_right)

其中，左須函數值F_left=||F(xl_k)||，右須函數值F_right=||F(xr_k)||，即分別對F(xl_k)和F(xr_k)取范數；sign為符號函數；step_k表示步長，每步迭代中步長以負指數次冪衰減，即：

step_k=step_k-1*e^-tk，其中t為第一衰減率；

S433，計算允許概率因子ρ，判斷該第k+1次迭代是否有效：

，

其中||F(X_k)||₂、||F(X_k+1)||₂表示分別對F(X_k)、F(X_k+1)取2-范數；

當||F(X_k+1)||₂≤||F(X_k)||₂，此時ρ為1，第k+1次迭代完全有效，X_k更新為X_k+1，并轉入步驟S42；當||F(X_k+1)||₂||F(X_k)||₂時，比較ρ與用于產生(0,1)之間隨機數的隨機函數rand()的大小，若ρ大于rand()，則第k+1次迭代有效，X_k更新為X_k+1，并轉入步驟S42，否則前述第k+1次迭代無效，X_k不更新，重新進行第k+1次迭代，即轉入步驟S431；

其中exp表示自然指數，即以e為底數的指數函數；M為運動系數，其滿足M=2^-uk，其中u為第二衰減率。