[發明專利]一種腱驅動機械手張力約束末端操作控制方法

有效

• 申請號: 201710584862.7
• 申請日: 2017-07-18
• 公開（公告）號: CN107414826B
• 公開（公告）日: 2020-03-31
• 發明（設計）人: 王邢波;王小濤;聶宏
• 申請（專利權）人: 南京郵電大學
• 主分類號: B25J9/16
• 分類號: B25J9/16
• 代理公司: 南京經緯專利商標代理有限公司 32200
• 代理人: 石艷紅

1.一種腱驅動機械手張力約束末端操作阻抗控制方法，其特征在于：包括如下步驟：

步驟1，機械手單指指尖參數設置：設定機械手單指運動的期望指尖接觸力F_d和期望指尖笛卡爾位置X_d；同時，預先設定期望腱張力f的取值范圍[f_min，f_max]；其中，f_min為保證腱繩始終處于繃緊狀態時的最小腱張力，f_max為保護腱繩不超過負荷時的最大腱張力；

步驟2，計算指尖接觸力誤差F_e：指尖接觸力誤差F_e的計算公式為：

F_e＝F_d-F_a (1)

式中，F_a為指尖接觸力傳感器測量到的指尖接觸力值，也即實測指尖接觸力值；

步驟3，計算期望指尖笛卡爾空間位置X_r，具體計算方法如下：

步驟31，計算笛卡爾空間位置補償值X_f：阻抗控制模塊采用如下公式將指尖接觸力誤差F_e轉換為笛卡爾空間位置補償值X_f；

式中，M_d、B_d、K_d分別為阻抗控制模塊中的阻抗控制器處于臨界阻尼或過阻尼狀態時的目標慣性矩陣、阻尼矩陣和剛度矩陣；s為復變量；

步驟32，計算期望指尖笛卡爾空間位置X_r：對期望指尖笛卡爾位置X_d進行補償與計算，得到期望指尖笛卡爾空間位置X_r，具體計算公式為：

X_r＝X_d+X_f (3)

步驟4，計算關節角位置誤差θ_e，具體計算方法如下：

步驟41，計算期望關節角位置θ_r，具體包括如下步驟：

步驟411，逆運動學解算模塊轉換：將步驟3計算的期望指尖笛卡爾空間位置X_r，通過逆運動學解算模塊轉換為期望關節角位置θ_r；其中，期望指尖笛卡爾空間位置X_r在手指基坐標系中的笛卡爾位置坐標為(x_T,y_T,z_T)，逆運動學解算模塊按照如下公式(4)進行轉換；

式中，L₁、L₂、L₃、L₄分別表示手指中基關節、近指節、中指節和遠指節的長度；θ₁、θ₂、θ₃、θ₄分別為側擺關節角、基關節角、中關節角和頂關節角；s_i＝sinθ_i，c_i＝cosθ_i，s_ij＝sin(θ_i+θ_j)，c_ij＝cos(θ_i+θ_j)，s_ijk＝sin(θ_i+θ_j+θ_k)，c_ijk＝cos(θ_i+θ_j+θ_k)；

步驟412，計算θ₁：由公式(4)，能直接計算出θ₁；

步驟413，計算θ₃和θ₄：頂關節通過四連桿機構與中關節相連，根據四連桿機構原理，以及如下公式(8)和(10)，并利用非線性函數數值解法求解出θ₃和θ₄；

假設頂關節與中關節相連的四連桿機構的四個角點分別為ABCD，其中，A點表示中關節，D點表示頂關節，B點為近指節上的一個固定點，C點為遠指節上的一個固定點；則公式(8)和(10)中，l_m表示四連桿機構中AD長度，也即中指節長度；l_g表示四連桿機構中BC長度；r₁表示四連桿機構中AB長度；r₂表示四連桿機構中CD長度；l表示四連桿機構中BD長度；

步驟414，計算θ₂：根據求出的θ₃和θ₄，再利用公式(4)求解θ₂；

步驟42，計算關節角位置誤差θ_e：關節角位置誤差θ_e按如下公式(11)計算；

θ_e＝θ_r-θ_a (11)

式中，θ_a為四個關節角位置傳感器分別測量得到的側擺關節角、基關節角、中關節角和頂關節角；

步驟5，計算最佳期望關節力矩τ：最佳期望關節力矩τ的計算方法如下：

步驟51，計算期望關節力矩τ_d：采用雅克比矩陣J，實現期望指尖接觸力F_d到期望關節力矩τ_d的轉換，該期望關節力矩τ_d能使手指指尖接觸物體且保持靜態平衡；期望關節力矩τ_d的計算公式如下：

τ_d＝J^TF_d (12)

其中，雅克比矩陣J的計算公式如下：

步驟52，計算補償關節力矩τ_e：采用如下公式(14)，將步驟4計算出的關節角位置誤差θ_e轉化為補償關節力矩τ_e，實現關節力矩的補償：

τ_e＝K(θ_r-θ_a) (14)

其中K為比例控制系數，θ_r-θ_a＝0，τ_e＝0；

步驟53，計算最佳期望關節力矩τ：按如下公式(15)，計算出最佳期望關節力矩τ；

τ＝τ_d+τ_e (15)

步驟6，計算期望腱張力f_d：腱張力分配模塊將步驟5計算出的最佳期望關節力矩τ，轉換為期望腱張力f_d；

腱張力分配模塊將最佳期望關節力矩τ轉換為期望腱張力f_d的轉換方法如下：

步驟61，根據f_min確定t的值，使其滿足如下公式(17)：

式中，t為腱內部張力值；f_min為保證腱繩始終處于繃緊狀態時的最小腱張力；A_i和a_i分別表示A的行向量和a的列元素；通過將步驟5計算出的最佳期望關節力矩τ代入公式(17)中，即可確定腱內部張力值t；

步驟62，將步驟61確定的腱內部張力值t及步驟5計算出的最佳期望關節力矩τ，代入如下的張力分配等式(16)，得到4維張力列向量f，且

f＝R⁺τ+W^Tt＝Aτ+at (16)

式中，R為腱映射矩陣，W^T正交于R的行空間且正定；和為預先計算出來的常值，A＝R⁺,a＝W⁺；

步驟63，令f_l為張力最小元素，f_h為張力最大元素，將步驟62中4維張力列向量f的四個元素f₁、f₂、f₃和f₄，通過逐元素比較法，得出張力最小元素f_l和張力最大元素f_h；

步驟64，判斷張力最大元素f_h是否超過上界f_max，如果f_h≤f_max，表明沒有超界，則步驟62中4維張力列向量f即為期望腱張力f_d，將4維張力列向量f中的四個元素f₁、f₂、f₃和f₄依次分配給對應的四根腱繩，并結束張力分配過程；

步驟65，如果f_h>f_max，則采用比例縮放方式構造如下新方程，得到新的4維張力列向量f：

將步驟63計算得到的f_l和f_h及其對應的相關參量A_l、a_l和A_h、a_h，代入式(18)中，得到參量α和t′：

其中，d＝a_hf_l-a_lf_h (20)

公式(21)中的t為步驟61確定的腱內部張力值；然后，令t＝t′，此時，t為更新后的腱內部張力；

步驟66，將步驟65中新的4維張力列向量f中的四個元素，通過逐元素比較法，得出新的張力最小元素f_l和新的張力最大元素f_h；

步驟67，若新的張力最大元素f_h≤f_max且新的張力最小元素f_l≥f_min，則步驟65中新的4維張力列向量f即為期望腱張力f_d，將新的4維張力列向量f中的四個元素依次分配給對應的四根腱繩，并結束張力分配過程；否則從步驟65開始進行迭代處理；

步驟7，計算腱張力偏差f_e：腱張力偏差f_e的計算公式為：

f_e＝f_d-f_a (22)

式中，f_a為實際腱張力，由各個腱張力傳感器測量所得到；

步驟8，腱張力偏差控制：張力控制模塊先按照如下公式(23)，將步驟7計算出的腱張力偏差f_e轉化為腱位置偏差△x；

式中，K_p、K_d和f_d各代表比例系數、微分系數和腱張力期望值；

然后，張力控制模塊把腱位置偏差△x輸送給腱驅動器，腱驅動器控制腱繩的拉伸。