[發(fā)明專利]一種繩索康復(fù)機(jī)器人運(yùn)動學(xué)參數(shù)標(biāo)定和解算方法及系統(tǒng)

有效

• 申請?zhí)枺?/td>: 202110562915.1
• 申請日: 2021-05-24
• 公開（公告）號: CN113509352B
• 公開（公告）日: 2022-09-30
• 發(fā)明（設(shè)計(jì)）人: 宋愛國;厲葉;李會軍;石珂
• 申請（專利權(quán)）人: 東南大學(xué)
• 主分類號: A61H1/02
• 分類號: A61H1/02
• 代理公司: 南京眾聯(lián)專利代理有限公司 32206
• 代理人: 葉涓涓

1.一種繩索康復(fù)機(jī)器人運(yùn)動學(xué)參數(shù)標(biāo)定和解算系統(tǒng)，包括存儲器、處理器及存儲在存儲器上并可在處理器上運(yùn)行的計(jì)算機(jī)程序，其特征在于：繩索康復(fù)機(jī)器人包括4根繩索，4根繩索穿過肩部支架后連接到上臂環(huán)，上臂環(huán)是一個(gè)穿戴在手臂上的套筒，上臂環(huán)與患者上肢結(jié)合形成一個(gè)完全約束的機(jī)械系統(tǒng)，可控制上肢肩關(guān)節(jié)3自由度運(yùn)動；機(jī)器人繩索的長度通過編碼器測量，上肢的3自由度旋轉(zhuǎn)通過IMU測量，所述計(jì)算機(jī)程序被加載至處理器時(shí)用于實(shí)現(xiàn)繩索康復(fù)機(jī)器人運(yùn)動學(xué)參數(shù)標(biāo)定和解算方法，方法包括如下步驟：

步驟一，運(yùn)動學(xué)參數(shù)標(biāo)定

(1)首先取患者手臂處于某一姿態(tài)下的狀態(tài)為初始狀態(tài)，此時(shí)的繩長即為初始繩長，亦是待求參數(shù)，并記此時(shí)手臂的旋轉(zhuǎn)矩陣為即無相對旋轉(zhuǎn)；

(2)患者進(jìn)行手臂各種動作，記錄患者運(yùn)動過程中的編碼器數(shù)據(jù)和IMU數(shù)據(jù)，即可得到繩長變化量Δl_i和上肢旋轉(zhuǎn)矩陣參數(shù)，患者手臂在任一位姿下的Δl_i和為一組數(shù)據(jù)，采集n組數(shù)據(jù)；

(3)對算法迭代量賦初值，即對待求解參數(shù)初始化，初始值通過手動測量近似確定，分別賦值為其中，為肩關(guān)節(jié)中心相對于系統(tǒng)坐標(biāo)原點(diǎn)的偏移值初始值；為手臂處于某一姿態(tài)下的四根繩索初始繩長，其中i＝1,2,3,4,為上臂環(huán)中心到肩關(guān)節(jié)中心的距離；

(4)進(jìn)行迭代，算得迭代參數(shù)的修正值，對迭代參數(shù)進(jìn)行修正；具體迭代過程如下：

考慮任意其中一組數(shù)據(jù)，忽略下標(biāo)j，則在該組數(shù)據(jù)中，第i根繩索的繩長相對于初始繩長的繩長差用如下方程表示

而l_i＝||^OS_i^OU_i|| (2)

將式(1)代入式(2)，可得

假定肩關(guān)節(jié)中心坐標(biāo)估計(jì)值上臂環(huán)中心到肩關(guān)節(jié)中心的距離估計(jì)值值為第i根繩索的初始繩長估計(jì)值為

在待求解參數(shù)附近進(jìn)行泰勒展開，并取至一次項(xiàng)；則

將式(4)代入式(3)，展開后并寫成

假設(shè)共采集了n組數(shù)據(jù)，則根據(jù)公式(5)可列出4n組方程，對應(yīng)的待求解參數(shù)為3n組肩關(guān)節(jié)中心偏移[^OG_jx,^OG_jy,^OG_jz]^T，4組初始繩長和1組上臂環(huán)中心到肩關(guān)節(jié)中心的距離共3n+5個(gè)變量，其中，j＝1,2,...,n，i＝1,2,3,4；

因此至少需要采集5組數(shù)據(jù)方可解出方程，即n≥5：

記

其中

記

記

記

則AX＝Y(jié)

根據(jù)最小二乘法有

X＝(A^TA)^-1A^TY (6)

最后對待求解參數(shù)估計(jì)值進(jìn)行迭代

上述各式中，^OG_j＝[^OG_jx,^OG_jy,^OG_jz]^T為在坐標(biāo)系{O}下第j組數(shù)據(jù)中肩關(guān)節(jié)中心坐標(biāo)；表示第j組數(shù)據(jù)中通過IMU測得的手臂的旋轉(zhuǎn)矩陣；為在坐標(biāo)系{G}下第i根繩索在上臂環(huán)上的連接點(diǎn)坐標(biāo)初始值；^OS_i為在坐標(biāo)系{O}下第i根繩索在肩部支架的連接點(diǎn)坐標(biāo)；^OU_ij為在坐標(biāo)系{O}下第j組數(shù)據(jù)中第i根繩索在上臂環(huán)上的連接點(diǎn)坐標(biāo)，會隨著上肢運(yùn)動相對于坐標(biāo)系{O}運(yùn)動，即l_ij為第j組數(shù)據(jù)中第i根繩索的長度，即||^OS_i^OU_ij||；Δl_ij為第j組數(shù)據(jù)中第i根繩索長度相對于初始繩長的繩長差；為在運(yùn)動學(xué)參數(shù)標(biāo)定過程中,第j次測量中得到的第i根繩索的繩長相對于初始繩長的繩長差的估計(jì)值；δl_ij為在運(yùn)動學(xué)參數(shù)標(biāo)定過程中,第j次測量中得到的第i根繩索的繩長差理論值和估計(jì)值的差；

(5)將第m次迭代后的迭代參數(shù)D^m與第m-1次的迭代參數(shù)D^m-1進(jìn)行比較，當(dāng)||D^m-D^m-1||_∞＜ε時(shí)，停止迭代，待求解參數(shù)的解即為當(dāng)前迭代參數(shù)，為否則返回步驟(4)，繼續(xù)迭代；其中D^m是第m次迭代后的迭代參數(shù)，ε是設(shè)定的誤差限，m為迭代次數(shù)；

步驟二，運(yùn)動學(xué)參數(shù)解算

對n組和進(jìn)行多元線性回歸

記標(biāo)定過程中的數(shù)據(jù)和的關(guān)系系數(shù)

則BJ＝Z

根據(jù)最小二乘法可解得J

J＝(B^TB)^-1B^TY

在實(shí)際訓(xùn)練過程中，通過實(shí)時(shí)的手臂旋轉(zhuǎn)矩陣可算得患者肩關(guān)節(jié)中心偏移量