基于PID算法的輔助進(jìn)食桌面機(jī)械手系統(tǒng)研究與設(shè)計(jì)*

劉祚時(shí), 羅 斌, 張 平, 饒 遠(yuǎn)

(江西理工大學(xué) 機(jī)電工程學(xué)院,江西 贛州 341000)

0 引 言

伴隨著微電子工藝技術(shù)的突破,現(xiàn)有技術(shù)已經(jīng)能夠?qū)崿F(xiàn)納米的加工工藝;同時(shí)傳感器技術(shù)的革新、現(xiàn)代控制理論的出現(xiàn)以及人工智能和計(jì)算機(jī)技術(shù)的融合,使得對(duì)機(jī)器人技術(shù)的研究已經(jīng)過(guò)渡到前沿綜合學(xué)科的研究過(guò)程中[1],而服務(wù)機(jī)器人作為未來(lái)機(jī)器人技術(shù)產(chǎn)業(yè)化的重要發(fā)展方向,可用于向老人或殘疾人提供助老助殘的幫助、為家庭做清潔衛(wèi)生工作以及一些其他特殊應(yīng)用場(chǎng)合[2]。近年來(lái),機(jī)器人技術(shù)得到了廣泛應(yīng)用,國(guó)內(nèi)學(xué)者應(yīng)用機(jī)器人技術(shù)實(shí)現(xiàn)了地板清潔與空氣凈化、微創(chuàng)外科手術(shù)[3]、自主種牙[4]以及家中巡邏[5]等。國(guó)外的研究機(jī)構(gòu)則研制了自動(dòng)喂飯機(jī)器人,可通過(guò)手按按鍵或使用腳踏模式,使得裝有不同用餐食物的餐盤相互切換,該機(jī)械手不僅可以用來(lái)為殘疾人輔助進(jìn)食,還能夠示教定位,并具有避障的功能;而Dapra資助的功能假肢項(xiàng)目DEKA Arm[6,7],利用人體神經(jīng)界面把大腦皮層的神經(jīng)運(yùn)動(dòng)的電信號(hào)轉(zhuǎn)換為一種能夠操作輔助裝置的控制信號(hào),從而使得癱瘓者恢復(fù)自理能力和運(yùn)動(dòng)能力,完成自主飲食。雖然機(jī)器人技術(shù)被廣泛運(yùn)用在醫(yī)療服務(wù)領(lǐng)域中,但實(shí)際針對(duì)上肢肢體不健全的特殊群體所應(yīng)用的輔助進(jìn)食類的機(jī)器人較少,且該服務(wù)類機(jī)器人購(gòu)買成本較高、占用的空間位置較大,不能夠很好地完成喂食任務(wù)。

為了解決上述問(wèn)題,本文將結(jié)合語(yǔ)音與機(jī)器視覺(jué)等傳感器技術(shù)設(shè)計(jì)一種輔助進(jìn)食的桌面機(jī)械手控制系統(tǒng),利用傳感器技術(shù)將系統(tǒng)的反饋信息與用戶進(jìn)行數(shù)據(jù)交互,達(dá)到讓機(jī)械手能夠按照預(yù)期效果輔助上身有缺陷的人群完成輔助喂食等一系列功能的目的。

1 桌面機(jī)械手?jǐn)?shù)學(xué)模型的建立

本文中實(shí)驗(yàn)所采用的桌面機(jī)械手平臺(tái)由5個(gè)TBS2701數(shù)字舵機(jī)所組成,其控制單元采用STM32F103ZET6作為主控芯片,并結(jié)合LD3320A人聲語(yǔ)音識(shí)別模塊與OV7725攝像頭模塊實(shí)現(xiàn)本文的預(yù)期效果。

1.1 桌面機(jī)械手正運(yùn)動(dòng)學(xué)分析

現(xiàn)采用D-H方法并結(jié)合表1中連桿參數(shù),建立機(jī)械手的連桿坐標(biāo)系,如圖1所示。

圖1 輔助進(jìn)食機(jī)械手連桿坐標(biāo)系

連桿數(shù)ai/mmαi/(°)di/mmθi關(guān)節(jié)變量范圍/(°)1109010θ1-135～135210500θ2-90～90310000θ3-135～13540900θ4-135～135500160θ5-90～90

其中,關(guān)節(jié)1～關(guān)節(jié)5為轉(zhuǎn)動(dòng)關(guān)節(jié),基座偏移量為H=72 mm。當(dāng)所有連桿坐標(biāo)系確定后,可確定相鄰連桿間的相對(duì)關(guān)系?，F(xiàn)就圖2中連桿i和i-1連桿來(lái)說(shuō)明轉(zhuǎn)換關(guān)系:

1) 選擇坐標(biāo)系與坐標(biāo)系i-1一致,令其沿軸zi-1平移di,并且繞軸zi-1旋轉(zhuǎn)θi,使當(dāng)前坐標(biāo)系與坐標(biāo)系i′重合;

2) 將和坐標(biāo)系i′一致的坐標(biāo)系沿x′i平移ai,并且繞x′i旋轉(zhuǎn)αi,使當(dāng)前坐標(biāo)與坐標(biāo)系i重合。

上述變換用總的變換矩陣表示為

=Trans(0,0,di)Rot(zi-1,θi)Trans(ai,0,0)Rot(xi′,αi)=

(1)

(2)

將機(jī)械手的每個(gè)坐標(biāo)轉(zhuǎn)換矩陣代入式(2)中即可求出該機(jī)械手的正運(yùn)動(dòng)方程。

1.2 桌面機(jī)械手逆運(yùn)動(dòng)學(xué)分析

逆運(yùn)動(dòng)學(xué)的求解能夠?qū)⒛┒藞?zhí)行器在操作空間的運(yùn)動(dòng)轉(zhuǎn)化成關(guān)節(jié)空間的運(yùn)動(dòng)[8]。當(dāng)給定末端執(zhí)行器的位置pw去求解關(guān)節(jié)變量的θ1,θ2,θ3,θ4,θ5。

由于機(jī)械手存在三軸平行的特征,故采用反變換法或幾何解法。在計(jì)算關(guān)節(jié)轉(zhuǎn)角時(shí)采用雙參變量的反正切函數(shù)關(guān)節(jié)變量,求得各個(gè)關(guān)節(jié)變角如下

θ1=atan 2(py,px)

θ2=atan 2(-a3sinθ3,a2+a3cosθ3)-

atan 2(cz-d1-H,ct+a1)

θ3=atan 2(a3sinθ3,a2+a3cosθ3)+

atan 2(cz-d1-H,ct+a1)

θ4=atan 2(-a3sinθ3+a4sinθ4,a2cosθ2+a4cosθ4)-

2atan (cz-d1-H,ct+a1)

θ5=atan 2(s1r11-c1r21,s1r12-c1r22)

1.3 運(yùn)動(dòng)學(xué)仿真分析

為驗(yàn)證桌面機(jī)械手的正運(yùn)動(dòng)學(xué)方程的建立和逆運(yùn)動(dòng)學(xué)求解的正確性,現(xiàn)將借助MATLAB Robotics Toolbox[9]工具箱對(duì)機(jī)械手建模,隨后進(jìn)行正逆運(yùn)動(dòng)學(xué)校驗(yàn)。首先根據(jù)表1中D-H參數(shù)表進(jìn)行建模,如圖2所示。

圖2 機(jī)械手模型

然后根據(jù)Robotics Toolbox中的函數(shù)代入關(guān)節(jié)范圍內(nèi)的任意關(guān)節(jié)角計(jì)算正運(yùn)動(dòng)學(xué)矩陣值,最后由逆運(yùn)動(dòng)學(xué)公式將已知矩陣值代入求解出關(guān)節(jié)角并與前向運(yùn)動(dòng)學(xué)的關(guān)節(jié)角比較。

其中與AB兩組關(guān)節(jié)角對(duì)應(yīng)的正運(yùn)動(dòng)學(xué)位姿矩陣分別為

2 輔助進(jìn)食控制系統(tǒng)的設(shè)計(jì)

2.1 對(duì)目標(biāo)人臉的追蹤控制設(shè)計(jì)

該輔助進(jìn)食機(jī)械手適用于動(dòng)態(tài)運(yùn)動(dòng)中的輔助進(jìn)食任務(wù),因此機(jī)械手的運(yùn)動(dòng)需要進(jìn)行動(dòng)態(tài)調(diào)整。故該機(jī)械手的工作時(shí)序是先等待人的輸入,再去執(zhí)行取食物的動(dòng)作,接著尋找人臉位置,等待人嘴型張開動(dòng)作的發(fā)生(也就是進(jìn)食動(dòng)作的完成)。由于該過(guò)程中,人是運(yùn)動(dòng)的而非靜止?fàn)顟B(tài),所以針對(duì)等待人進(jìn)食的過(guò)程,需利用機(jī)器視覺(jué)實(shí)現(xiàn)人臉的動(dòng)態(tài)跟隨,使得取食物的器具(勺子)一直朝向人臉?lè)较颉?/p>

在進(jìn)行人臉動(dòng)態(tài)跟隨之前,需完成查找人臉的搜索動(dòng)作,但由于機(jī)械手的擺放位置是確定的。因此機(jī)械手取完食物后會(huì)運(yùn)動(dòng)到默認(rèn)位置(面向人臉一側(cè)),并在指定工作空間開始人臉第一次搜索,這時(shí)人臉的位置作為人臉動(dòng)態(tài)跟隨的初始位置,直至完成進(jìn)食動(dòng)作為止。

2.1.1 比例—積分—微分控制對(duì)象分析

比例—積分—微分(PID)的控制目標(biāo)(輸入信號(hào))是OpenMV采集人臉中心相對(duì)于圖像中心的像素差,并可通過(guò)輸出脈沖寬度調(diào)制(pulse width modulation,PWM)來(lái)控制舵機(jī)機(jī)械手運(yùn)動(dòng),實(shí)現(xiàn)機(jī)械手動(dòng)態(tài)跟隨人臉的效果。在控制過(guò)程中可將誤差轉(zhuǎn)換為期望PWM值輸出,同時(shí)對(duì)采集識(shí)別到的人臉圖像進(jìn)行處理,而識(shí)別到的人臉圖像在圖片區(qū)域中表示為4個(gè)參數(shù)(x,y,w,h),即人臉對(duì)象的起點(diǎn)為(x,y),人臉區(qū)域?qū)挾葹閣,高度為h,所有的單位為像素值。選用的分辨率為160像素×120像素(QQVGA),像素格式為灰度圖,如圖3所示。

圖3 人臉坐標(biāo)分析

圖3中,當(dāng)人臉對(duì)象被人臉檢測(cè)算法識(shí)別出來(lái)之后,返回人臉對(duì)象在圖像中的感興趣區(qū)域(region of interest,ROI)。該區(qū)域由4個(gè)參數(shù)表征,人臉對(duì)象的中心坐標(biāo)如圖中十字型標(biāo)記所示,則人臉對(duì)象的中心坐標(biāo)為

cx=x+w/2,cy=y+h/2

(3)

而人臉對(duì)象中心坐標(biāo)和整個(gè)圖像的中心坐標(biāo)的差值就作為舵機(jī)云臺(tái)PID控制器的控制量,此時(shí)方向誤差分別為

Δx=x+w/2-W/2,Δy=y+h/2-H/2

(4)

2.1.2 舵機(jī)位置PID控制器設(shè)計(jì)

設(shè)計(jì)舵機(jī)位置PID控制器是為了在視覺(jué)單元檢測(cè)到人臉之后,順利完成對(duì)目標(biāo)人臉的搜索任務(wù),隨后等待人的交互動(dòng)作(如張嘴動(dòng)作)?？紤]到目標(biāo)用戶為行動(dòng)不便的特殊人群,所以需避免因人動(dòng)作變化而導(dǎo)致人臉的位置發(fā)生改變的情況發(fā)生,防止機(jī)械手重新在工作空間中進(jìn)行人臉?biāo)阉鳌Ｒ虼?設(shè)計(jì)該舵機(jī)位置PID控制器需在機(jī)械手完成取食物的動(dòng)作且搜索到人臉,并在人張嘴進(jìn)食動(dòng)作發(fā)生之前完成目標(biāo)人臉的追蹤,同時(shí)確保末端操作器始終在人臉正前方。

控制器設(shè)計(jì)規(guī)則：1)關(guān)節(jié)0與關(guān)節(jié)1使用相同的PID控制器;2)當(dāng)控制變量較小時(shí),PID控制器不生效;3)控制器的響應(yīng)速度快。

由上設(shè)計(jì)了位置型PID控制器,可通過(guò)調(diào)節(jié)PID控制器的參數(shù),滿足實(shí)際的控制要求與執(zhí)行元件的響應(yīng)需要。

2.1.3 舵機(jī)位置型PID控制器實(shí)際控制效果測(cè)試及分析

本文進(jìn)行2組實(shí)驗(yàn)來(lái)測(cè)試機(jī)械手PID控制器在水平方向上的跟隨效果,并分別用機(jī)械手檢測(cè)在人臉相對(duì)于機(jī)械手進(jìn)行左偏和右偏運(yùn)動(dòng)的情況下,對(duì)PID控制器像素誤差數(shù)據(jù)進(jìn)行采集和分析。其中誤差像素?cái)?shù)據(jù)是由控制器串行外設(shè)接口傳到上位機(jī)得到的。

從圖4可以看出:控制量為像素值,經(jīng)過(guò)PID參數(shù)整定后,敏感變量PID控制器的響應(yīng)速度很快,能夠達(dá)到人臉移動(dòng)的追蹤要求,并達(dá)到預(yù)期的控制效果。

圖4 人臉PID控制效果

2.2 對(duì)機(jī)械手的語(yǔ)音控制設(shè)計(jì)

由于語(yǔ)音交互便于使用,且適合應(yīng)用在人機(jī)交互要求較高的場(chǎng)合,同時(shí)語(yǔ)音交互也便于學(xué)習(xí)使用。通過(guò)LD3320A人聲語(yǔ)音識(shí)別模塊,輸入有效的語(yǔ)音時(shí),語(yǔ)音模塊會(huì)根據(jù)輸入的語(yǔ)音詞條匹配預(yù)定義詞條,完成相應(yīng)的功能動(dòng)作。

本文采用LD3320的循環(huán)識(shí)別模式,能夠滿足語(yǔ)音隨機(jī)輸入的要求,同時(shí)通過(guò)口令觸發(fā)作為觸發(fā)源。LD3320驅(qū)動(dòng)包括以下步驟：1)芯片復(fù)位;2)語(yǔ)音識(shí)別;3)聲音播放。其操作順序如圖5所示。

圖5 語(yǔ)音識(shí)別操作順序

語(yǔ)音識(shí)別通用初始化中主要是對(duì)LD3320進(jìn)行軟復(fù)位、數(shù)字信號(hào)處理器(DSP)休眠低功耗設(shè)置、模/數(shù)轉(zhuǎn)換器(ADC)初始化以及時(shí)鐘率的初始化設(shè)置。在語(yǔ)音識(shí)別初始化中主要是使控制寄存器進(jìn)入自動(dòng)語(yǔ)音識(shí)別(automatic speech recognition,ASR)模塊,然后對(duì)添加詞條的先入先出(FIFO)緩存器進(jìn)行初始化,如圖6(a)所示。

選擇語(yǔ)音循環(huán)識(shí)別模式,可以讓用戶隨時(shí)根據(jù)需求發(fā)出語(yǔ)音指令。文中的程序在FreeRTOS系統(tǒng)中創(chuàng)建一個(gè)優(yōu)先級(jí)較高的語(yǔ)音識(shí)別任務(wù),使得系統(tǒng)能夠讓每個(gè)任務(wù)在任務(wù)調(diào)度器的管理下高效運(yùn)行。當(dāng)有語(yǔ)音輸入產(chǎn)生識(shí)別結(jié)果時(shí);同時(shí)產(chǎn)生中斷。因此需要設(shè)置中斷處理,具體流程如圖6(b)所示。

圖6 詞條寫入與循環(huán)識(shí)別模式

3 實(shí)驗(yàn)調(diào)試過(guò)程

在完成整個(gè)輔助進(jìn)食機(jī)械手系統(tǒng)的軟硬件設(shè)計(jì)與測(cè)試后,對(duì)機(jī)械手進(jìn)行系統(tǒng)調(diào)試,如圖7所示。

圖7 輔助進(jìn)食機(jī)械手系統(tǒng)工作流程

在系統(tǒng)調(diào)試過(guò)程中,分別截取了機(jī)械手取食物、機(jī)械手喂食物和動(dòng)態(tài)人臉跟隨的實(shí)驗(yàn)照片,如圖8所示。

圖8 三組動(dòng)作實(shí)驗(yàn)照

在實(shí)驗(yàn)過(guò)程中,輔助進(jìn)食機(jī)械手能夠先去指定的食物儲(chǔ)存器中取食物(實(shí)驗(yàn)中食物用燕麥片代替),然后人臉動(dòng)態(tài)跟隨送達(dá)目的位置,在嘴張開且喂食動(dòng)作結(jié)束后,繼續(xù)重復(fù)上次動(dòng)作,經(jīng)實(shí)驗(yàn)證實(shí)該機(jī)械手可以達(dá)到輔助進(jìn)食的效果。

4 結(jié)束語(yǔ)

本文設(shè)計(jì)的五自由度輔助進(jìn)食桌面機(jī)械手系統(tǒng),主要服務(wù)特殊人群,在考慮人機(jī)交互體驗(yàn)的前提下,采用了語(yǔ)音識(shí)別作為系統(tǒng)的輸入,能夠滿足大部分場(chǎng)合的應(yīng)用需求;同時(shí)在喂食的環(huán)節(jié)采用了人臉檢測(cè)和模板匹配技術(shù),并在取食物之后、等待進(jìn)食之前使用了人臉追蹤,通過(guò)PID算法實(shí)現(xiàn)機(jī)械手的動(dòng)態(tài)人臉跟隨,大大地提升了人機(jī)交互體驗(yàn)。而面對(duì)多傳感器數(shù)據(jù)融合以及多任務(wù)應(yīng)用場(chǎng)景的問(wèn)題,本文選擇嵌入式系統(tǒng)(FreeRTOS)開發(fā)軟件應(yīng)用程序能夠提高系統(tǒng)的實(shí)時(shí)性,并使用信號(hào)量等機(jī)制解決任務(wù)同步、共享資源訪問(wèn)的問(wèn)題,提高了開發(fā)效率。