一種基于STM32的力反饋型康復(fù)機(jī)器人控制系統(tǒng)設(shè)計(jì) - 全文

康復(fù)醫(yī)療機(jī)器人不僅是當(dāng)前機(jī)器人研究的前沿，而且是涉及自動(dòng)控制、計(jì)算機(jī)、智能儀器、機(jī)械學(xué)、康復(fù)醫(yī)學(xué)、神經(jīng)科學(xué)等跨學(xué)科的研究領(lǐng)域，是一個(gè)新興學(xué)科?？祻?fù)醫(yī)療機(jī)器人的研究具有重要的科學(xué)意義?；谏现祻?fù)訓(xùn)練的要求，筆者研制一種基于STM32的力反饋型上肢康復(fù)訓(xùn)練系統(tǒng)。在設(shè)計(jì)控制系統(tǒng)時(shí)，保證系統(tǒng)的安全性與穩(wěn)定性是最基本的原則⑷。為了避免在突發(fā)情況下對(duì)患肢造成二次損傷，通常采用限定機(jī)器人末端和受訓(xùn)患肢之間作用力大小的辦法。經(jīng)過實(shí)驗(yàn)驗(yàn)證，本系統(tǒng)設(shè)計(jì)合理，性能穩(wěn)定，可以實(shí)現(xiàn)康復(fù)機(jī)器人的平穩(wěn)控制，具有的力反饋功能可以在保證系統(tǒng)安全性的前提下充分激發(fā)患者殘存的功能。

1、上肢康復(fù)機(jī)器人系統(tǒng)構(gòu)成

上肢康復(fù)機(jī)器人的機(jī)械臂是關(guān)于軸心對(duì)稱的，能夠繞軸心進(jìn)行反復(fù)的弧線運(yùn)動(dòng)，既可以由計(jì)算機(jī)控制訓(xùn)練，也可以由治療師直接操作機(jī)器人引導(dǎo)患者進(jìn)行訓(xùn)練。圖1為該上肢康復(fù)訓(xùn)練機(jī)器人的實(shí)物。

?

圖1單自由度上肢康復(fù)訓(xùn)練機(jī)器人

該機(jī)器人系統(tǒng)主要由機(jī)械臂機(jī)械結(jié)構(gòu)、電機(jī)、位置傳感器、扭矩傳感器、電機(jī)驅(qū)動(dòng)器、機(jī)器人控制器和上位機(jī)組成，系統(tǒng)框圖如圖2所示。扭矩傳感器、驅(qū)動(dòng)電機(jī)和位置傳感器沿機(jī)械臂軸心依次放置。位置傳感器用于檢測(cè)機(jī)械臂的運(yùn)動(dòng)角度;扭矩傳感器用于檢測(cè)患者與機(jī)械臂的交互作用力，在患者主動(dòng)能力不足時(shí)提供更大的輔助，而在患者有能力完成動(dòng)作時(shí)，適當(dāng)減小輔助甚至施加阻力，以便充分激發(fā)患者殘存的功能;電機(jī)驅(qū)動(dòng)器，用于產(chǎn)生康復(fù)訓(xùn)練的輔助力或阻尼力，推動(dòng)或是阻礙機(jī)械臂和患肢的運(yùn)動(dòng)。

圖2單自由度的上肢康復(fù)訓(xùn)練機(jī)器人系統(tǒng)框圖

機(jī)器人控制器通過USB接口與上位機(jī)實(shí)現(xiàn)數(shù)據(jù)交換，一方面控制電路接收上位機(jī)發(fā)送來的控制信息，解析上位機(jī)命令控制電機(jī)驅(qū)動(dòng)器;另一方面，機(jī)器人控制器將采集到的扭矩傳感器和位置傳感器數(shù)據(jù)發(fā)送到上位機(jī)?？祻?fù)醫(yī)師可根據(jù)上位機(jī)人機(jī)交互界面顯示的{言息’對(duì)患者的康復(fù)訓(xùn)練情況進(jìn)行定量的科學(xué)分析與評(píng)價(jià)，滅者細(xì)膝卿計(jì)耀供決策依據(jù)。

2、系統(tǒng)硬件設(shè)計(jì)

本機(jī)器人控制器采用基于AKM-M3內(nèi)核的STM32微控制器。ARM-M3內(nèi)核的設(shè)計(jì)具有低功耗、高性能、實(shí)時(shí)性以及具有競(jìng)爭(zhēng)力的價(jià)格，該內(nèi)核配備了緊耦合的嵌套矢量中斷控制器，因此能夠更加快速地響應(yīng)中斷事件。微控制器自帶I2位的ADC和DAC，可以很好地滿足系統(tǒng)的需要。

圖3為本系統(tǒng)的電機(jī)驅(qū)動(dòng)器硬件電路。微控制器DAC的輸出引腳接到圖3中的MOTORIN端。由于微控制器的DAC只能輸出單極性電壓信號(hào)，而機(jī)械臂需要做往復(fù)運(yùn)動(dòng)，需要雙極性信號(hào)驅(qū)動(dòng)電機(jī)正反轉(zhuǎn)，故將MOTORIN端接入的信號(hào)先進(jìn)行調(diào)理，該調(diào)理部分所用OP4177芯片為4路運(yùn)放。

圖3 ?電機(jī)驅(qū)動(dòng)器硬件電路

筆者通過減法電路將DAC輸出轉(zhuǎn)換成雙極性信號(hào)，圖中OP4177芯片的左上角運(yùn)放構(gòu)成該減法電路。運(yùn)放正輸人端接人DAC輸出信號(hào)，負(fù)輸入端接人1.65V電壓。減法電路輸出為V。=（R3/R8）（^motorin-1.65），當(dāng)l/M。TQRW》1.65V時(shí)，減法電路輸出正電壓控制電機(jī)正轉(zhuǎn);當(dāng)hcmwm《1.65V時(shí)，減法電路輸出負(fù)電壓控制電機(jī)反轉(zhuǎn);Kmotorin=1.65V時(shí)，減法電路輸出0V電壓，此時(shí)電機(jī)保持靜止?fàn)顟B(tài)。圖中尺|5~尺？構(gòu)成了分壓電路，經(jīng)過OP4177芯片左下角運(yùn)放構(gòu)成的電壓跟隨器獲得I.65V基準(zhǔn)電壓。減法電路輸出信號(hào)接人OP4177芯片右上角運(yùn)放組成的運(yùn)算放大電路，其輸出為V，=［-（R4+R7）/R5］xV0。

經(jīng)過運(yùn)算放大后的電壓信號(hào)輸人到功率放大器，此處采用集成功率放大器為丨，M3886，該芯片具有過電流保護(hù)功能，且輸出功率較大，可輸出60W。圖3屮的MOTOROUT端為功放輸出的驅(qū)動(dòng)電機(jī)的電壓信號(hào)。功率放大器輸出的驅(qū)動(dòng)電壓MOTOROUT與微控制器DAC輸出MOTORIN比例關(guān)系為

筆者采用的扭矩傳感器為差分信號(hào)輸出，其輸出為K+和L，電壓幅值為毫伏級(jí)，故需要進(jìn)行差分放大后，才能送人微控制器進(jìn)行A/D轉(zhuǎn)換。該差分放大器采用小尺寸、低功耗、高精度的AD620。差分輸出為：

式中RG為增益的外部控制電阻，選擇合適的RG使扭矩信號(hào)放大至伏級(jí)。由于扭矩傳感器輸出的信號(hào)是雙極性的，經(jīng)AD620放大后仍為雙極性信號(hào)，而微控制器的A/D轉(zhuǎn)換范圍為0~3.3V，故須將AD620輸出的電壓加上1.65V抬升到0~3.3V，使之適合A/D的輸人范圍。

位置傳感器輸出為正交脈沖信號(hào)，STM32微控制器的每個(gè)定時(shí)器都有正交脈沖信號(hào)的輸入接口，通過配置其定時(shí)器的工作模式，即可以對(duì)位置傳感器的輸出脈沖進(jìn)行正向/反向計(jì)數(shù)，從而得到機(jī)械臂的位置。

微控制器將扭矩信息與位置信息通過USB數(shù)據(jù)線上傳到上位機(jī)，進(jìn)行康復(fù)機(jī)器人狀態(tài)的監(jiān)控;同時(shí)接收上位機(jī)下傳的驅(qū)動(dòng)電機(jī)的命令，進(jìn)行相應(yīng)的D/A轉(zhuǎn)換，通過電機(jī)驅(qū)動(dòng)器控制電機(jī)工作。

3、系統(tǒng)軟件設(shè)計(jì)

系統(tǒng)上位機(jī)與微控制器之間通過USB進(jìn)行數(shù)據(jù)交互。系統(tǒng)上電后，微控制器開始工作，首先初始化系統(tǒng)時(shí)鐘、I/O端口、外部中斷、ADC、DAC、定時(shí)器和USB等配置。

首先在上位機(jī)中設(shè)置康復(fù)機(jī)器人的訓(xùn)練模式，本系統(tǒng)的訓(xùn)練模式分為主動(dòng)運(yùn)動(dòng)訓(xùn)練模式和被動(dòng)運(yùn)動(dòng)訓(xùn)練模式兩種［6］。設(shè)置成功后，上位機(jī)的控制指令通過USB傳輸?shù)轿⒖刂破髦校⑦M(jìn)入微控制器的USB中斷，微控制器在中斷處理程序中處理上位機(jī)發(fā)出的指令，根據(jù)上位機(jī)發(fā)出控制指令，設(shè)定相應(yīng)訓(xùn)練模式的標(biāo)志，以供主程序進(jìn)行機(jī)械臂運(yùn)動(dòng)類型的判斷。

軟件主程序流程如圖4所示，若判斷為被動(dòng)運(yùn)動(dòng)訓(xùn)練模式，則進(jìn)人PID控制子程序，對(duì)機(jī)械臂的位置進(jìn)行閉環(huán)控制，使機(jī)械臂往復(fù)運(yùn)動(dòng);若判斷為主動(dòng)運(yùn)動(dòng)訓(xùn)練模式，則進(jìn)入自主訓(xùn)練過程，患者帶動(dòng)機(jī)械臂進(jìn)行主動(dòng)訓(xùn)練;其他則判斷為機(jī)械臂無運(yùn)動(dòng)模式。

主程序中同時(shí)進(jìn)行機(jī)械臂的扭矩信息和位置信息的采集。微控制器的ADC對(duì)差分放大后的扭矩信號(hào)進(jìn)行數(shù)字量的轉(zhuǎn)換;當(dāng)機(jī)械臂轉(zhuǎn)動(dòng)時(shí)，位置傳感器輸出相互正交的脈沖信號(hào)，微控制器通過配置定時(shí)器工作模式對(duì)其進(jìn)行計(jì)數(shù)。

最后微控制器將扭矩信息與位置信息通過USB腦社位I上位聽撒賄他醒方式直觀地顯示出來，便于醫(yī)師對(duì)機(jī)械臂的工作狀態(tài)和患者的康復(fù)狀況進(jìn)行客觀判斷。上位機(jī)接收到的扭矩信號(hào)也是本系統(tǒng)的力反饋信號(hào)，當(dāng)扭矩過小時(shí)，可以適當(dāng)增大驅(qū)動(dòng)電壓，使驅(qū)動(dòng)電機(jī)的輸出扭矩增大;當(dāng)扭矩過大時(shí)，可以適當(dāng)減小驅(qū)動(dòng)電壓，減小驅(qū)動(dòng)電機(jī)的輸出扭矩，從而保證了患者康復(fù)訓(xùn)練的有效性與安全性。

圖4 微控制器主程序流程圖

4、PID參數(shù)整定

微控制器DAC輸出的電壓值是通過PID箅法計(jì)，4算出來的。PID算法的流程圖如圖5所示。

圖5 PID算法原理框圖

首先，由上位機(jī)給定康復(fù)機(jī)器人機(jī)械臂的運(yùn)動(dòng)范圍，即圖5中所示的目標(biāo)位置將位置傳感器采集的當(dāng)前位置信息輸入到PID控制器中，目標(biāo)位置與當(dāng)前位置X（A）的差值e（A）送人PID控制器中，將其輸出值送人微控制器的DAC進(jìn)行轉(zhuǎn)換產(chǎn)生電機(jī)的驅(qū)動(dòng)信號(hào)，進(jìn)而帶動(dòng)機(jī)械臂做與上位機(jī)控制指令相一致的運(yùn)動(dòng)。

筆者利用傳統(tǒng)方式求出本系統(tǒng)中直流電機(jī)模型參數(shù)。本系統(tǒng)的被控對(duì)象是電機(jī)模型，傳遞函數(shù)為：

式中，G為電機(jī)電勢(shì)系數(shù);Ta為電氣時(shí)間常數(shù);Tm為機(jī)械時(shí)間常數(shù)。所選直流電機(jī)的參數(shù)如表1所示。

表1直流電機(jī)參數(shù)

采用Matlab進(jìn)行仿真。系統(tǒng)單位階躍信號(hào)的響應(yīng)曲線如圖6（a）所示，系統(tǒng)的穩(wěn)定值在1.0之上，不滿足系統(tǒng)要求。

圖6 ?校正前后系統(tǒng)階躍響應(yīng)曲線

PID控制器的參數(shù)值是在實(shí)驗(yàn)過程中不斷調(diào)整與完善的。經(jīng)過多次實(shí)驗(yàn)，系統(tǒng)最終選定的PID參數(shù)是o4t/s246t/s0（a）系統(tǒng)單位階躍響應(yīng)曲線KP=0.7241KI=0.0001，KD=10。

5、實(shí)驗(yàn)測(cè)試與結(jié)果

為了測(cè)試康復(fù)機(jī)器人控制系統(tǒng)的穩(wěn)定性，筆者進(jìn)行了反復(fù)實(shí)驗(yàn)。圖7（a）所示的是系統(tǒng)的主動(dòng)運(yùn)動(dòng)訓(xùn)練模式?；颊邘?dòng)機(jī)械臂進(jìn)行往復(fù)運(yùn)動(dòng)，機(jī)械臂所轉(zhuǎn)動(dòng)的角度在框圖中顯示出來。

系統(tǒng)的被動(dòng)運(yùn)動(dòng)訓(xùn)練模式分為4種：慢速小范圍、慢速大細(xì)、快速小細(xì)和快速大細(xì)，可以根據(jù)患者的需求設(shè)定合刪訓(xùn)練類型。圖7（b）是慢速大細(xì)訓(xùn)練模式下的實(shí)驗(yàn)結(jié)果，圖7（c）是快速小范酬練模式下的實(shí)驗(yàn)結(jié)果。

對(duì)于系統(tǒng)中的力反饋功能，除了可以根據(jù)扭矩大小調(diào)節(jié)驅(qū)動(dòng)電壓大小外，還可以避免在突發(fā)情況下機(jī)構(gòu)對(duì)患者肢體造成的二次損傷，如當(dāng)患者的肢體發(fā)生痙攣時(shí)，系統(tǒng)的工作狀態(tài)可以轉(zhuǎn)變?yōu)橥Ｖ?，從而保護(hù)患者。實(shí)驗(yàn)測(cè)試結(jié)果如圖7（d）所示。圖中尖峰狀的曲線為扭矩信號(hào)，當(dāng)機(jī)械臂的運(yùn)動(dòng)被阻止時(shí)，扭矩傳感器可以檢測(cè)到很大的扭矩信號(hào)，上位機(jī)接收到此扭矩信號(hào)后，對(duì)機(jī)械臂狀態(tài)做出判斷，繼而向下位機(jī)發(fā)送停止電機(jī)工作的指令，使機(jī)械臂處于放松狀態(tài)，從而避免了對(duì)患肢的損傷。

6、結(jié)束語

設(shè)計(jì)了一種基于STM32的具有力反收的康復(fù)機(jī)器人控制系統(tǒng)。采用扭矩傳感器實(shí)現(xiàn)系統(tǒng)的力反饋功能，充分發(fā)揮患者殘存的功目采用位置傳感器采集康：復(fù)機(jī)械臂的位置信息，配合PID算法進(jìn)行位置反饋，使控制信號(hào)更加準(zhǔn)確。