本發(fā)明屬于外骨骼機(jī)器人控制相關(guān)技術(shù)領(lǐng)域，更具體地，涉及一種基于交互力識(shí)別運(yùn)動(dòng)意圖的欠驅(qū)動(dòng)康復(fù)機(jī)器人的控制方法。

背景技術(shù)：

目前，大部分康復(fù)機(jī)器人僅能完成被動(dòng)訓(xùn)練任務(wù)，無(wú)法根據(jù)患者的主動(dòng)運(yùn)動(dòng)意圖完成主動(dòng)康復(fù)訓(xùn)練，如申請(qǐng)?zhí)枮?01410627428.9的專利公開(kāi)了一種外骨骼式上肢康復(fù)訓(xùn)練系統(tǒng)，其是輔助康復(fù)醫(yī)師完成康復(fù)訓(xùn)練的一種醫(yī)療設(shè)備，可以實(shí)現(xiàn)肩部三個(gè)自由度、肘部一個(gè)自由度和腕部一個(gè)自由度的運(yùn)動(dòng)，真實(shí)再現(xiàn)患者的日常生活的動(dòng)作訓(xùn)練，所述外骨骼式上肢康復(fù)訓(xùn)練系統(tǒng)由兩部分組成：第一耦合運(yùn)動(dòng)機(jī)構(gòu)及第二耦合運(yùn)動(dòng)機(jī)構(gòu)。所述第一耦合運(yùn)動(dòng)機(jī)構(gòu)將肩部外展和內(nèi)旋兩個(gè)自由度耦合，采用一個(gè)電機(jī)驅(qū)動(dòng)；所述第二耦合運(yùn)動(dòng)機(jī)構(gòu)將肩部前伸、肘部屈肘及腕部翻轉(zhuǎn)三個(gè)自由度耦合，采用一個(gè)電機(jī)驅(qū)動(dòng)，故所述外骨骼式上肢康復(fù)訓(xùn)練系統(tǒng)采用兩個(gè)電機(jī)控制上肢五個(gè)關(guān)節(jié)的運(yùn)動(dòng)，其為欠驅(qū)動(dòng)系統(tǒng)。該外骨骼式上肢康復(fù)訓(xùn)練系統(tǒng)的優(yōu)勢(shì)在于既減少了驅(qū)動(dòng)電機(jī)的數(shù)量，使得控制可靠，又將驅(qū)動(dòng)電機(jī)轉(zhuǎn)移到了非機(jī)械臂處，減小了外骨骼的尺寸大小，提高患者的安全性，但其無(wú)法根據(jù)訓(xùn)練者的主動(dòng)運(yùn)動(dòng)意圖完成主動(dòng)康復(fù)訓(xùn)練。

然而，在肢體康復(fù)訓(xùn)練中，患者主動(dòng)運(yùn)動(dòng)康復(fù)訓(xùn)練是必須的康復(fù)過(guò)程。若要實(shí)現(xiàn)主動(dòng)康復(fù)訓(xùn)練，就要能夠準(zhǔn)確判斷患者肢體的運(yùn)動(dòng)意圖，進(jìn)行主動(dòng)訓(xùn)練的患者肌體肌力不具備支撐肢體及康復(fù)器械的能力，需要康復(fù)器械施加一定的力矩輔助患者運(yùn)動(dòng)。相比于被動(dòng)康復(fù)訓(xùn)練，主動(dòng)康復(fù)訓(xùn)練被認(rèn)為對(duì)肢體運(yùn)動(dòng)功能恢復(fù)更為有效。為了實(shí)現(xiàn)主動(dòng)康復(fù)訓(xùn)練就需要判斷患者的運(yùn)動(dòng)意向，大多采用肌電信號(hào)來(lái)實(shí)現(xiàn)判斷人體主動(dòng)運(yùn)動(dòng)意向的，但由于每個(gè)人的實(shí)際情況不同，所產(chǎn)生的肌電信號(hào)強(qiáng)弱不一致，肌電信號(hào)的干擾因素很多，所以在實(shí)際過(guò)程中要經(jīng)過(guò)反復(fù)檢測(cè)才能使用，增加了檢測(cè)成本。

現(xiàn)階段，本領(lǐng)域相關(guān)技術(shù)人員已經(jīng)做了一些研究，如申請(qǐng)?zhí)枮?01510895139.1的專利記載了根據(jù)患者的關(guān)節(jié)力矩作為判斷患者運(yùn)動(dòng)意圖的方式，用力來(lái)判斷患者運(yùn)動(dòng)意圖的方式比肌電信號(hào)可靠，但是采集患者關(guān)節(jié)力矩值的方式不僅要求患者的上肢關(guān)節(jié)完全與康復(fù)機(jī)器人的關(guān)節(jié)重合，而且要求其運(yùn)動(dòng)角度、角速度和角加速度均與外骨骼康復(fù)機(jī)器人一致，這些要求并不符合實(shí)際康復(fù)訓(xùn)練情況，偏癱患者在上肢外骨骼機(jī)器人上做康復(fù)訓(xùn)練時(shí)，上肢各關(guān)節(jié)或多或少偏離外骨骼機(jī)器人的關(guān)節(jié)，此種情況采集到的患者關(guān)節(jié)的力矩值并不準(zhǔn)確。

技術(shù)實(shí)現(xiàn)要素：

針對(duì)現(xiàn)有技術(shù)的以上缺陷或改進(jìn)需求，本發(fā)明提供了一種基于交互力識(shí)別運(yùn)動(dòng)意圖的欠驅(qū)動(dòng)康復(fù)機(jī)器人的控制方法，其基于欠驅(qū)動(dòng)康復(fù)機(jī)器人的工作特點(diǎn)，針對(duì)基于交互力識(shí)別運(yùn)動(dòng)意圖的欠驅(qū)動(dòng)康復(fù)機(jī)器人的控制方法進(jìn)行了設(shè)計(jì)。所述控制方法能夠借助六維力傳感器較容易的實(shí)時(shí)判斷出患者的運(yùn)動(dòng)意圖，并將運(yùn)動(dòng)意圖轉(zhuǎn)化為欠驅(qū)動(dòng)康復(fù)機(jī)器人的驅(qū)動(dòng)電機(jī)所應(yīng)做出的響應(yīng)，實(shí)現(xiàn)了以患者意圖主導(dǎo)的主動(dòng)康復(fù)訓(xùn)練，降低了結(jié)構(gòu)復(fù)雜程度，減小了體積，安全可靠，有利于欠驅(qū)動(dòng)康復(fù)機(jī)器人的應(yīng)用。此外，所述控制方法通過(guò)對(duì)速度矢量系數(shù)的改變可以調(diào)節(jié)欠驅(qū)動(dòng)康復(fù)機(jī)器人助力的大小，使用方便，靈活性較高。

為實(shí)現(xiàn)上述目的，本發(fā)明提供了一種基于交互力識(shí)別運(yùn)動(dòng)意圖的欠驅(qū)動(dòng)康復(fù)機(jī)器人的控制方法，其包括以下步驟：

(1)提供一個(gè)欠驅(qū)動(dòng)康復(fù)機(jī)器人，并確定所述欠驅(qū)動(dòng)康復(fù)機(jī)器人的驅(qū)動(dòng)電機(jī)數(shù)量及驅(qū)動(dòng)角度矢量，所述欠驅(qū)動(dòng)康復(fù)機(jī)器人的手柄部位設(shè)置有六維力傳感器；

(2)根據(jù)所述欠驅(qū)動(dòng)康復(fù)機(jī)器人的關(guān)節(jié)數(shù)量確定運(yùn)動(dòng)關(guān)節(jié)角度矢量；

(3)根據(jù)所述欠驅(qū)動(dòng)康復(fù)機(jī)器人的關(guān)節(jié)初始角度確定初始關(guān)節(jié)角度矢量；

(4)根據(jù)所述欠驅(qū)動(dòng)康復(fù)機(jī)器人的結(jié)構(gòu)耦合情況計(jì)算出傳動(dòng)矩陣；

(5)根據(jù)所述驅(qū)動(dòng)角度矢量、所述運(yùn)動(dòng)關(guān)節(jié)角度矢量、初始關(guān)節(jié)角度矢量及所述傳動(dòng)矩陣求得驅(qū)動(dòng)電機(jī)與關(guān)節(jié)轉(zhuǎn)角之間的關(guān)系式；

(6)根據(jù)運(yùn)動(dòng)關(guān)節(jié)角度矢量確定所述欠驅(qū)動(dòng)康復(fù)機(jī)器人的剛體雅可比矩陣；

(7)根據(jù)所述傳動(dòng)矩陣及所述雅可比矩陣確定所述欠驅(qū)動(dòng)康復(fù)機(jī)器人末端的速度映射矩陣；

(8)根據(jù)所述速度映射矩陣的列矢量構(gòu)建出驅(qū)動(dòng)電機(jī)的驅(qū)動(dòng)空間到所述欠驅(qū)動(dòng)康復(fù)機(jī)器人操作空間的投影矩陣；

(9)根據(jù)所述六維力傳感器測(cè)量出的數(shù)據(jù)計(jì)算出患者的手施加在所述欠驅(qū)動(dòng)康復(fù)機(jī)器人的手柄處的力矢量；

(10)將所述力矢量轉(zhuǎn)化為患者渴望的速度矢量；

(11)將所述速度矢量左乘所述投影矩陣以得到所述驅(qū)動(dòng)電機(jī)的轉(zhuǎn)速矢量，即得到所需控制的電機(jī)轉(zhuǎn)速。

進(jìn)一步的，所述欠驅(qū)動(dòng)康復(fù)機(jī)器人為上肢外骨骼康復(fù)機(jī)器人，其驅(qū)動(dòng)電機(jī)的數(shù)量為兩個(gè)。

進(jìn)一步的，所述欠驅(qū)動(dòng)康復(fù)機(jī)器人的關(guān)節(jié)數(shù)量為五個(gè)；所述初始關(guān)節(jié)角度矢量其中，分別表示五個(gè)關(guān)節(jié)的初始轉(zhuǎn)動(dòng)角度。

進(jìn)一步的，所述欠驅(qū)動(dòng)康復(fù)機(jī)器人包括兩個(gè)耦合部分，兩個(gè)所述耦合部分分別為肩關(guān)節(jié)耦合及肩肘關(guān)節(jié)耦合。

進(jìn)一步的，所述傳動(dòng)矩陣為各個(gè)關(guān)節(jié)的傳動(dòng)比構(gòu)成的：式中，¹μ₁、¹μ₂、²μ₁、²μ₂、²μ₃分別為各個(gè)關(guān)節(jié)的傳動(dòng)比。

進(jìn)一步的，所述六維力傳感器可采集施加在其表面的力和力矩值，并以下式輸出：

F_s＝[τ_x,τ_y,τ_z,f_x,f_y,f_z]^T，式中，τ為力矩，f為力，x,y,z的方向分別與六維力傳感器坐標(biāo)系的三個(gè)坐標(biāo)軸x_f，y_f，z_f同向，所述六維力傳感器坐標(biāo)系的坐標(biāo)原點(diǎn)位于所述六維力傳感器的幾何中心。

總體而言，通過(guò)本發(fā)明所構(gòu)思的以上技術(shù)方案與現(xiàn)有技術(shù)相比，本發(fā)明提供的基于交互力識(shí)別運(yùn)動(dòng)意圖的欠驅(qū)動(dòng)康復(fù)機(jī)器人的控制方法，其能夠借助六維力傳感器較容易的實(shí)時(shí)判斷出患者的運(yùn)動(dòng)意圖，并將運(yùn)動(dòng)意圖轉(zhuǎn)化為欠驅(qū)動(dòng)康復(fù)機(jī)器人的驅(qū)動(dòng)電機(jī)所應(yīng)做出的響應(yīng)，實(shí)現(xiàn)了以患者意圖主導(dǎo)的主動(dòng)康復(fù)訓(xùn)練，降低了結(jié)構(gòu)復(fù)雜程度，減小了體積，安全可靠，有利于欠驅(qū)動(dòng)康復(fù)機(jī)器人的應(yīng)用。此外，所述控制方法通過(guò)對(duì)速度矢量系數(shù)的改變可以調(diào)節(jié)欠驅(qū)動(dòng)康復(fù)機(jī)器人助力的大小，使用方便，靈活性較高。

附圖說(shuō)明

圖1是發(fā)明較佳實(shí)施方式提供的基于交互力識(shí)別運(yùn)動(dòng)意圖的欠驅(qū)動(dòng)康復(fù)機(jī)器人的控制方法的流程圖。

圖2是圖1中的基于交互力識(shí)別運(yùn)動(dòng)意圖的欠驅(qū)動(dòng)康復(fù)機(jī)器人的控制方法涉及的欠驅(qū)動(dòng)康復(fù)機(jī)器人的結(jié)構(gòu)示意圖。

圖3是圖2中的欠驅(qū)動(dòng)康復(fù)機(jī)器人的局部坐標(biāo)設(shè)置示意圖。

圖4是圖2中的欠驅(qū)動(dòng)康復(fù)機(jī)器人的另一局部坐標(biāo)設(shè)置示意圖。

在所有附圖中，相同的附圖標(biāo)記用來(lái)表示相同的元件或結(jié)構(gòu)，其中：1-第一電機(jī)，2-第一驅(qū)動(dòng)盤(pán)，3-第二電機(jī)，4-第二驅(qū)動(dòng)盤(pán)，5-第一肩部從動(dòng)盤(pán)，6-第一肘部從動(dòng)盤(pán)，7-六維力傳感器，8-第一繩索，9-第二繩索，10-第二肩部從動(dòng)盤(pán)，11-第三肩部從動(dòng)盤(pán)，12-第三繩索，13-第四繩索，14-第四肩部從動(dòng)盤(pán)，15-第五肩部從動(dòng)盤(pán)，16-第二肘部從動(dòng)盤(pán)，17-腕部弧形盤(pán)，18-手柄。

具體實(shí)施方式

為了使本發(fā)明的目的、技術(shù)方案及優(yōu)點(diǎn)更加清楚明白，以下結(jié)合附圖及實(shí)施例，對(duì)本發(fā)明進(jìn)行進(jìn)一步詳細(xì)說(shuō)明。應(yīng)當(dāng)理解，此處所描述的具體實(shí)施例僅僅用以解釋本發(fā)明，并不用于限定本發(fā)明。此外，下面所描述的本發(fā)明各個(gè)實(shí)施方式中所涉及到的技術(shù)特征只要彼此之間未構(gòu)成沖突就可以相互組合。

請(qǐng)參閱圖1至圖4，本發(fā)明較佳實(shí)施方式提供的基于交互力識(shí)別運(yùn)動(dòng)意圖的欠驅(qū)動(dòng)康復(fù)機(jī)器人的控制方法，所述控制方法借助六維力傳感器來(lái)達(dá)到穩(wěn)定識(shí)別患者的運(yùn)動(dòng)意圖，進(jìn)而完成欠驅(qū)動(dòng)康復(fù)機(jī)器人帶著患者主動(dòng)康復(fù)訓(xùn)練的目的。

本發(fā)明提供的基于交互力識(shí)別運(yùn)動(dòng)意圖的欠驅(qū)動(dòng)康復(fù)機(jī)器人的控制方法主要包括以下步驟：

步驟一，提供一個(gè)欠驅(qū)動(dòng)康復(fù)機(jī)器人，并確定所述欠驅(qū)動(dòng)康復(fù)機(jī)器人的驅(qū)動(dòng)電機(jī)數(shù)量及驅(qū)動(dòng)角度矢量，所述欠驅(qū)動(dòng)康復(fù)機(jī)器人的手柄部位安裝有六維力傳感器7。本實(shí)施方式中，所述欠驅(qū)動(dòng)康復(fù)機(jī)器人為上肢外骨骼康復(fù)機(jī)器人。

具體地，所述欠驅(qū)動(dòng)康復(fù)機(jī)器人的關(guān)節(jié)數(shù)量大于自由度數(shù)量，其操作空間不是一個(gè)自由空間。所述欠驅(qū)動(dòng)康復(fù)機(jī)器人包括電機(jī)組件、繩索組件及上肢外骨骼組件，所述電機(jī)組件包括第一電機(jī)1、第一驅(qū)動(dòng)盤(pán)2、第二電電機(jī)3及第二驅(qū)動(dòng)盤(pán)4。所述繩索組件包括第一繩索8、第二繩索9、第三繩索12及第四繩索13。所述上肢外骨骼組件包括第一肩部從動(dòng)盤(pán)5、第二肩部從動(dòng)盤(pán)10、第三肩部從動(dòng)盤(pán)11、第四肩部從動(dòng)盤(pán)14、第五肩部從動(dòng)盤(pán)15、第一肘部從動(dòng)盤(pán)6、第二肘部從動(dòng)盤(pán)16、腕部弧形盤(pán)17、六維力傳感器7及手柄18。所述六維力傳感器7連接于所述手柄18，其用于測(cè)量患者作用于所述手柄18的力。

所述第一電機(jī)1連接于所述第一驅(qū)動(dòng)盤(pán)2，所述第二肩部從動(dòng)盤(pán)10連接于所述第三肩部從動(dòng)盤(pán)11，所述第一繩索8及所述第二繩索9連接所述第一驅(qū)動(dòng)盤(pán)2及所述第二肩部從動(dòng)盤(pán)10。所述第四肩部從動(dòng)盤(pán)14連接于所述第三肩部從動(dòng)盤(pán)11，所述第五肩部從動(dòng)盤(pán)15連接于所述第四肩部從動(dòng)盤(pán)14，所述第二肘部從動(dòng)盤(pán)16連接于所述第五肩部從動(dòng)盤(pán)15，所述腕部弧形盤(pán)17連接于所述第二肘部從動(dòng)盤(pán)16，所述六維力傳感器7連接所述腕部弧形盤(pán)17及所述手柄18。所述第二電機(jī)3連接于所述第二驅(qū)動(dòng)盤(pán)4，所述第三繩索12及所述第四繩索13連接所述第二驅(qū)動(dòng)盤(pán)4及所述第五肩部從動(dòng)盤(pán)15。

所述欠驅(qū)動(dòng)康復(fù)機(jī)器人由兩個(gè)耦合部分組成，第一耦合部分為肩關(guān)節(jié)耦合，第二耦合部分為肩肘關(guān)節(jié)耦合。當(dāng)所述欠驅(qū)動(dòng)康復(fù)機(jī)器人的肩關(guān)節(jié)部分運(yùn)動(dòng)時(shí)，所述第一電機(jī)1驅(qū)動(dòng)所述第一驅(qū)動(dòng)盤(pán)2運(yùn)動(dòng)，所述第一驅(qū)動(dòng)盤(pán)2通過(guò)所述第一繩索8及所述第二繩索9驅(qū)動(dòng)所述第二肩部從動(dòng)盤(pán)10運(yùn)動(dòng)，以完成肩部?jī)?nèi)旋的動(dòng)作。同時(shí)，由于所述第三肩部從動(dòng)盤(pán)11固定不動(dòng)，連接所述第三肩部從動(dòng)盤(pán)11及所述第四肩部從動(dòng)盤(pán)14的繩索會(huì)帶動(dòng)所述第四肩部從動(dòng)盤(pán)14轉(zhuǎn)動(dòng)以完成肩部外展的動(dòng)作，即所述第一耦合部分將肩部?jī)?nèi)旋及肩部外展兩個(gè)自由度耦合，采用所述第一電機(jī)1來(lái)控制。

當(dāng)所述欠驅(qū)動(dòng)康復(fù)機(jī)器人的肘關(guān)節(jié)運(yùn)動(dòng)時(shí)，所述第二電機(jī)3驅(qū)動(dòng)所述第二驅(qū)動(dòng)盤(pán)4運(yùn)動(dòng)，所述第二驅(qū)動(dòng)盤(pán)4通過(guò)第三繩索12及所述第四繩索13來(lái)驅(qū)動(dòng)所述第五肩部從動(dòng)盤(pán)15轉(zhuǎn)動(dòng)以完成肩部前伸的動(dòng)作；此時(shí)，由于所述第一肩部從動(dòng)盤(pán)5固定不動(dòng)，連接所述第一肩部從動(dòng)盤(pán)5及所述第一肘部從動(dòng)盤(pán)6的繩索會(huì)帶動(dòng)所述第一肘部從動(dòng)盤(pán)6轉(zhuǎn)動(dòng)以完成肘部屈肘的動(dòng)作；同時(shí)，所述第二肘部從動(dòng)盤(pán)16與機(jī)械臂固定連接，在肘部屈肘時(shí)，連接所述第二肘部從動(dòng)盤(pán)16及所述腕部弧形盤(pán)17之間的繩索會(huì)帶動(dòng)所述腕部弧形盤(pán)17轉(zhuǎn)動(dòng)，以完成腕部翻轉(zhuǎn)的動(dòng)作，即第二耦合部分將肩部前伸、肘部屈肘及腕部翻轉(zhuǎn)三個(gè)自由度耦合，采用所述第二電機(jī)3來(lái)控制。

建立空間坐標(biāo)系{s}及手柄坐標(biāo)系{h}，所述空間坐標(biāo)系在空間固定，其與機(jī)械臂的上端的位置關(guān)系相對(duì)固定。所述空間坐標(biāo)系的三個(gè)坐標(biāo)軸分別為x_s，y_s，z_s。所述手柄坐標(biāo)系的原點(diǎn)固定在所述手柄的幾何中心，其三個(gè)坐標(biāo)軸分別為x_h，y_h，z_h。ω₁，ω₂，ω₃，ω₄，ω₅分別為所述第二肩部從動(dòng)盤(pán)10、所述第四肩部從動(dòng)盤(pán)14、所述肩部從動(dòng)盤(pán)15、所述第二肘部從動(dòng)盤(pán)16和所述腕部弧形盤(pán)17的轉(zhuǎn)軸方向，其中ω₁，ω₂，ω₃兩兩垂直且交于空間一點(diǎn)，ω₄與ω₃平行且與ω₁相交；位于初始位置時(shí)，ω₅與ω₁共線。

此外，建立六維力傳感器坐標(biāo)系{f}，所述六維力傳感器坐標(biāo)系的原點(diǎn)固定在所述六維力傳感器7的幾何中心，x_f，y_f，z_f為所述六維力傳感器的三個(gè)坐標(biāo)軸。

所述欠驅(qū)動(dòng)康復(fù)機(jī)器人的驅(qū)動(dòng)電機(jī)的數(shù)量為兩個(gè)，分別是所述第一電機(jī)1及所述第二電機(jī)3；分別用u₁，u₂來(lái)分別表示所述第一電機(jī)1及所述第二電機(jī)3的角位移，則驅(qū)動(dòng)角度矢量為u＝(u₁,u₂)^T。

步驟二，根據(jù)所述欠驅(qū)動(dòng)康復(fù)機(jī)器人的關(guān)節(jié)數(shù)量確定運(yùn)動(dòng)關(guān)節(jié)角度矢量。具體地，所述欠驅(qū)動(dòng)康復(fù)機(jī)器人的關(guān)節(jié)數(shù)量為5個(gè)，用θ₁，θ₂，θ₃，θ₄，θ₅分別表示ω₁，ω₂，ω₃，ω₄，ω₅所對(duì)應(yīng)關(guān)節(jié)的轉(zhuǎn)動(dòng)角度，運(yùn)動(dòng)關(guān)節(jié)角度矢量為θ＝(θ₁,θ₂,θ₃,θ₄,θ₅)^T。

步驟三，根據(jù)欠驅(qū)動(dòng)康復(fù)機(jī)器人的關(guān)節(jié)初始角度確定初始關(guān)節(jié)角度矢量。具體地，用分別表示ω₁，ω₂，ω₃，ω₄，ω₅所對(duì)應(yīng)關(guān)節(jié)上的初始轉(zhuǎn)動(dòng)角度，初始關(guān)節(jié)角度矢量初始關(guān)節(jié)角度根據(jù)使患者上肢佩戴所述欠驅(qū)動(dòng)康復(fù)機(jī)器人時(shí)舒適的角度確定，根據(jù)所述欠驅(qū)動(dòng)康復(fù)機(jī)器人確定如下角度，

步驟四，根據(jù)所述欠驅(qū)動(dòng)康復(fù)機(jī)器人的結(jié)構(gòu)耦合情況計(jì)算出傳動(dòng)矩陣。具體地，根據(jù)所述欠驅(qū)動(dòng)康復(fù)機(jī)器人的兩個(gè)耦合部分，可得傳動(dòng)矩陣如下：

傳動(dòng)比

各個(gè)盤(pán)半徑¹r₂＝40，¹r₁₀＝50¹r₁₁＝66，¹r₁₄＝75²r₄＝40，²r₁₅＝50²r₅＝74，²r₆＝56，²r₁₆＝37，²r₁₇＝74，單位為毫米，將上述參數(shù)帶入可得傳動(dòng)矩陣：

步驟五，根據(jù)所述驅(qū)動(dòng)角度矢量、所述運(yùn)動(dòng)關(guān)節(jié)角度矢量、初始關(guān)節(jié)角度矢量及所述傳動(dòng)矩陣獲得驅(qū)動(dòng)電機(jī)轉(zhuǎn)角與關(guān)節(jié)轉(zhuǎn)角之間的關(guān)系式。具體地，驅(qū)動(dòng)電機(jī)轉(zhuǎn)角與關(guān)節(jié)轉(zhuǎn)角之間的關(guān)系式確定為展開(kāi)如下:

步驟六，根據(jù)運(yùn)動(dòng)關(guān)節(jié)角度矢量確定所述欠驅(qū)動(dòng)康復(fù)機(jī)器人的剛體雅可比矩陣。具體地，對(duì)于純轉(zhuǎn)動(dòng)關(guān)節(jié)，由每個(gè)角速度方向的單位矢量ω可以算出該關(guān)節(jié)的運(yùn)動(dòng)旋量：

其中，q為末端轉(zhuǎn)動(dòng)坐標(biāo)系的原點(diǎn)到關(guān)節(jié)旋轉(zhuǎn)軸線的向量，對(duì)于本欠驅(qū)動(dòng)康復(fù)機(jī)器人來(lái)說(shuō)為手柄坐標(biāo)系原點(diǎn)到關(guān)節(jié)旋轉(zhuǎn)軸線的向量，

若則

由運(yùn)動(dòng)旋量B₁,B₂,B₃,B₄,B₅和關(guān)節(jié)角位移θ₁，θ₂，θ₃，θ₄，θ₅可以求得剛體的雅可比矩陣J＝[J₁ J₂ J₃ J₄ J₅]；

其中，

J₅＝B₅。

由剛體螺旋運(yùn)動(dòng)公式可知

式中，對(duì)于純轉(zhuǎn)動(dòng)關(guān)節(jié)，h為0

若

步驟七，根據(jù)所述傳動(dòng)矩陣及所述雅可比矩陣確定所述欠驅(qū)動(dòng)康復(fù)機(jī)器人末端的速度映射矩陣。具體地，所述欠驅(qū)動(dòng)康復(fù)機(jī)器人末端的速度映射矩陣為Γ＝Jη。

步驟八，根據(jù)所述速度映射矩陣的列矢量構(gòu)建出驅(qū)動(dòng)電機(jī)的驅(qū)動(dòng)空間到所述欠驅(qū)動(dòng)康復(fù)機(jī)器人操作空間的投影矩陣。具體地，Riemannian矩陣由公式可知α和β分別是角速度和線速度對(duì)投影速度的影響因子。

對(duì)于所述欠驅(qū)動(dòng)康復(fù)機(jī)器人α取0.3，β取1，使得所述欠驅(qū)動(dòng)康復(fù)機(jī)器人對(duì)線速度更敏感，得Riemannian矩陣G為

進(jìn)而求得投影矩陣P為

步驟九，根據(jù)所述六維力傳感器7測(cè)量出的數(shù)據(jù)計(jì)算出患者的手施加在所述欠驅(qū)動(dòng)康復(fù)機(jī)器人的手柄處的力矢量。具體地，所述六維力傳感器7可采集施加在其表面的力和力矩值，并以如下形式輸出：

F_s＝[τ_x,τ_y,τ_z,f_x,f_y,f_z]^T，τ為力矩，f為力，x,y,z方向分別與坐標(biāo)軸x_f，y_f，z_f相同。

將六維力傳感器坐標(biāo)系中的力F_s變換到手柄坐標(biāo)系F_h中

其中，

R_hs為手柄坐標(biāo)系到六維力傳感器坐標(biāo)系的旋轉(zhuǎn)矩陣，p_hs為手柄坐標(biāo)系原點(diǎn)到六維力傳感器坐標(biāo)系原點(diǎn)的向量，R為旋轉(zhuǎn)矩陣。

步驟十，將所述力矢量轉(zhuǎn)化為患者所渴望的速度矢量。具體地，將力矢量轉(zhuǎn)化為速度矢量可利用公式V^d＝ACF，其中V^d是主動(dòng)訓(xùn)練時(shí)患者所渴望的速度，F(xiàn)是患者手施加在手柄上的力，AC是導(dǎo)納矩陣，為了不改變力的方向，導(dǎo)納矩陣為對(duì)角矩陣。

A＝λI_6×6，為了放慢速度，λ取0.05。

其中α和β分別是對(duì)所采集的力矩和力的放大倍數(shù)，α＝10，β＝1，使其將力矩放大。

步驟十一，將所述速度矢量左乘所述投影矩陣以得到所述驅(qū)動(dòng)電機(jī)的轉(zhuǎn)速矢量，即得到所需控制的電機(jī)轉(zhuǎn)速。具體地，將步驟十中所獲得的速度V^d投影到機(jī)器人的操作空間的平面上其中P是步驟八中所獲得的投影矩陣，是驅(qū)動(dòng)電機(jī)的轉(zhuǎn)速矢量，和分別是所述第一電機(jī)1及所述第二電機(jī)3的轉(zhuǎn)速。

經(jīng)過(guò)如上步驟就建立起了患者施加于手柄的力和電機(jī)轉(zhuǎn)速的關(guān)系，也就是患者運(yùn)動(dòng)意圖和機(jī)器運(yùn)動(dòng)的關(guān)系。

本發(fā)明提供的基于交互力識(shí)別運(yùn)動(dòng)意圖的欠驅(qū)動(dòng)康復(fù)機(jī)器人的控制方法，其能夠借助六維力傳感器較容易的實(shí)時(shí)判斷出患者的運(yùn)動(dòng)意圖，并將運(yùn)動(dòng)意圖轉(zhuǎn)化為欠驅(qū)動(dòng)康復(fù)機(jī)器人的驅(qū)動(dòng)電機(jī)所應(yīng)做出的響應(yīng)，實(shí)現(xiàn)了以患者意圖主導(dǎo)的主動(dòng)康復(fù)訓(xùn)練，降低了結(jié)構(gòu)復(fù)雜程度，減小了體積，安全可靠，有利于欠驅(qū)動(dòng)康復(fù)機(jī)器人的應(yīng)用。此外，所述控制方法通過(guò)對(duì)速度矢量系數(shù)的改變可以調(diào)節(jié)欠驅(qū)動(dòng)康復(fù)機(jī)器人助力的大小，使用方便，靈活性較高。

本領(lǐng)域的技術(shù)人員容易理解，以上所述僅為本發(fā)明的較佳實(shí)施例而已，并不用以限制本發(fā)明，凡在本發(fā)明的精神和原則之內(nèi)所作的任何修改、等同替換和改進(jìn)等，均應(yīng)包含在本發(fā)明的保護(hù)范圍之內(nèi)。