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

劉祚時, 羅 斌, 張 平, 饒 遠

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

0 引 言

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

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

1 桌面機械手數(shù)學模型的建立

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

1.1 桌面機械手正運動學分析

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

圖1 輔助進食機械手連桿坐標系

連桿數(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)動關(guān)節(jié),基座偏移量為H=72 mm。當所有連桿坐標系確定后,可確定相鄰連桿間的相對關(guān)系。現(xiàn)就圖2中連桿i和i-1連桿來說明轉(zhuǎn)換關(guān)系:

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

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

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

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

(1)

(2)

將機械手的每個坐標轉(zhuǎn)換矩陣代入式(2)中即可求出該機械手的正運動方程。

1.2 桌面機械手逆運動學分析

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

由于機械手存在三軸平行的特征,故采用反變換法或幾何解法。在計算關(guān)節(jié)轉(zhuǎn)角時采用雙參變量的反正切函數(shù)關(guān)節(jié)變量,求得各個關(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 運動學仿真分析

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

圖2 機械手模型

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

其中與AB兩組關(guān)節(jié)角對應的正運動學位姿矩陣分別為

2 輔助進食控制系統(tǒng)的設計

2.1 對目標人臉的追蹤控制設計

該輔助進食機械手適用于動態(tài)運動中的輔助進食任務,因此機械手的運動需要進行動態(tài)調(diào)整。故該機械手的工作時序是先等待人的輸入,再去執(zhí)行取食物的動作,接著尋找人臉位置,等待人嘴型張開動作的發(fā)生(也就是進食動作的完成)。由于該過程中,人是運動的而非靜止狀態(tài),所以針對等待人進食的過程,需利用機器視覺實現(xiàn)人臉的動態(tài)跟隨,使得取食物的器具(勺子)一直朝向人臉方向。

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

2.1.1 比例—積分—微分控制對象分析

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

圖3 人臉坐標分析

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

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

(3)

而人臉對象中心坐標和整個圖像的中心坐標的差值就作為舵機云臺PID控制器的控制量,此時方向誤差分別為

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

(4)

2.1.2 舵機位置PID控制器設計

設計舵機位置PID控制器是為了在視覺單元檢測到人臉之后,順利完成對目標人臉的搜索任務,隨后等待人的交互動作(如張嘴動作)。考慮到目標用戶為行動不便的特殊人群,所以需避免因人動作變化而導致人臉的位置發(fā)生改變的情況發(fā)生,防止機械手重新在工作空間中進行人臉搜索。因此,設計該舵機位置PID控制器需在機械手完成取食物的動作且搜索到人臉,并在人張嘴進食動作發(fā)生之前完成目標人臉的追蹤,同時確保末端操作器始終在人臉正前方。

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

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

2.1.3 舵機位置型PID控制器實際控制效果測試及分析

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

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

圖4 人臉PID控制效果

2.2 對機械手的語音控制設計

由于語音交互便于使用,且適合應用在人機交互要求較高的場合,同時語音交互也便于學習使用。通過LD3320A人聲語音識別模塊,輸入有效的語音時,語音模塊會根據(jù)輸入的語音詞條匹配預定義詞條,完成相應的功能動作。

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

圖5 語音識別操作順序

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

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

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

3 實驗調(diào)試過程

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

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

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

圖8 三組動作實驗照

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

4 結(jié)束語

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