一種六自由度咀嚼機(jī)器人的控制系統(tǒng)的制作方法
【技術(shù)領(lǐng)域】
[0001] 本專利基于對正常狀態(tài)下成年人咀嚼運(yùn)動中咀嚼肌、顳下頌關(guān)節(jié)、韌帶及參與咀 嚼運(yùn)動相關(guān)組織結(jié)構(gòu)進(jìn)行生理特征研究分析，設(shè)計一種咀嚼機(jī)器人的控制方法及系統(tǒng)，屬 于機(jī)器人領(lǐng)域。具體設(shè)計涉及基于Cortex-M3控制器的機(jī)器人控制系統(tǒng)。
【背景技術(shù)】
[0002] 口腔生物材料和新型食品開發(fā)需要對顆粒度、質(zhì)地、咀嚼性等方面進(jìn)行評估。目 前，對食物或材料的評估主要采用感官評價法和儀器評價法，感官評價法主要依賴品嘗專 家的主觀評估，過程費(fèi)時費(fèi)力，評價結(jié)果主觀性強(qiáng)、而且不穩(wěn)定；常用的儀器評價法是簡單 的機(jī)械壓縮試驗(yàn)，多屬于半經(jīng)驗(yàn)或模擬測定，評價結(jié)果與人類感知相差較大，而且不能對多 種質(zhì)地特性做出準(zhǔn)確表達(dá)，故不能反映真實(shí)的咀嚼過程。隨著食品工業(yè)的快速發(fā)展和人們 對食品品質(zhì)要求的日益提高，食品質(zhì)地評價顯得越來越重要，市場上急需快速、客觀、準(zhǔn)確、 類人的食品質(zhì)地評價方法。咀嚼機(jī)器人是一類可以模擬人類咀嚼行為的機(jī)器人，它可以真 實(shí)的復(fù)現(xiàn)人類的咀嚼動作并采集分析咀嚼的信息，包括咀嚼力、位移、速度等內(nèi)容。咀嚼機(jī) 器人研究是集機(jī)構(gòu)學(xué)、運(yùn)動學(xué)、動力學(xué)、感知系統(tǒng)、實(shí)時控制、食品科學(xué)、生物力學(xué)及機(jī)械電 子學(xué)于一體的工程科學(xué)。咀嚼系統(tǒng)的機(jī)器人化可為上述工作提供一種科學(xué)的手段。
[0003] 為了使咀嚼機(jī)器人運(yùn)動軌跡平滑，需要對驅(qū)動桿的位置進(jìn)行規(guī)劃時，驅(qū)動桿運(yùn)動 位置的插值多采用3次多項式插值，它需要力矩大，力矩過大，同等減速條件下所要求匹配 的電機(jī)功率就越大，電機(jī)功率越大，電機(jī)尺寸跟著增大，價格也可能增加，安裝電機(jī)所需要 的空間增大，增大機(jī)器人的體積。力矩過大，同等力臂條件下導(dǎo)致受力過大，需要增加驅(qū)動 桿及機(jī)構(gòu)整體的強(qiáng)度，提高設(shè)計成本。過路徑點(diǎn)的3次多項式插值在同樣起始點(diǎn)位置和同 樣的目標(biāo)位置的情況下，比點(diǎn)到點(diǎn)的3次多項式插值所需力矩小。
[0004] 目前對多自由度機(jī)器人控制多采用4軸或6軸驅(qū)控一體機(jī)如ADT-QC600?；蛘卟?用ARM7及DSP等高速處理芯片該類芯片設(shè)計機(jī)器人系統(tǒng)要求在一個控制周期內(nèi)完成數(shù)據(jù) 的采集，處理，控制電機(jī)同指令的輸出。同時需要將采集到的數(shù)據(jù)上傳到上位機(jī)，過重的負(fù) 荷影響到了機(jī)器人控制系統(tǒng)的可靠性及實(shí)時性并且成本較高。傳統(tǒng)的PID控制器結(jié)構(gòu)簡 單、實(shí)現(xiàn)簡單、控制效果良好，所以在咀嚼機(jī)器人中廣泛應(yīng)用。但由于機(jī)器人機(jī)械加工精度 問題，每個機(jī)器人都存在個體差異，很難得到一致的PID控制參數(shù)。針對傳統(tǒng)PID控制器 的特點(diǎn)，將摸糊控制技術(shù)引入到PID參數(shù)自整定過程中，根據(jù)不同情況在線自整定PID參 數(shù)。通過和傳統(tǒng)的PID控制器比較，模糊自適應(yīng)PID控制器具有具有靈活性好，控制適 應(yīng)性強(qiáng)，動態(tài)、靜態(tài)性能好等優(yōu)點(diǎn)。模糊控制無需建立精確的數(shù)學(xué)模型，具有較強(qiáng)的魯棒 性，咀嚼機(jī)器人的咬合運(yùn)動，不同階段采用不同的控制，張開閉合階段采用模糊控制，響應(yīng) 更快，咬合階段采用模糊自適應(yīng)PID控制，使軌跡更精確，適應(yīng)性更強(qiáng)，更能滿足咀嚼機(jī)器 人的要求。針對上述問題對咀嚼機(jī)器人的控制系統(tǒng)設(shè)計采用性價比高STM32F103芯片分為 主從分工協(xié)作，完成對下頌處壓力傳感器信號米集AD轉(zhuǎn)換傳輸任務(wù)的同時控制6軸電機(jī)完 成機(jī)器人動作。

【發(fā)明內(nèi)容】

[0005] 本發(fā)明所要解決的技術(shù)問題是：
[0006] 1、傳統(tǒng)的點(diǎn)到點(diǎn)的3次多項式插值所需力矩大，所需設(shè)備體積較大。
[0007] 2、由于機(jī)器人機(jī)械加工精度問題，每個咀嚼機(jī)器人都存在個體差異和六個驅(qū)動 桿對稱性，很難得到一致的PID控制參數(shù)的問題。
[0008] 為了解決上述技術(shù)問題，本發(fā)明才有以下技術(shù)方案：
[0009] 一種六自由度咀嚼機(jī)器人的控制系統(tǒng)，其包括以下步驟：
[0010] 步驟1:規(guī)劃咀嚼機(jī)器人的各分支桿的運(yùn)動軌跡，通過切牙點(diǎn)的位置和位姿求出 各驅(qū)動桿的伸縮，過路徑點(diǎn)的3次多項式插值方法求出各驅(qū)動桿期望位置和速度；
[0011] 步驟2 :根據(jù)各分支桿的運(yùn)動軌跡對機(jī)器人的咀嚼過程進(jìn)行控制，咀嚼過程包括 張開、閉合、咬合三個階段；咬合階段采用模糊PID控制。
[0012] 在張開和閉合兩個階段，使用模糊控制，通過電機(jī)光編碼器求解電機(jī)在張開閉合 時的轉(zhuǎn)速，在張開和閉合兩個階段，使用模糊控制，根據(jù)預(yù)設(shè)的張開閉合的電機(jī)的速度值， 判斷無刷直流電機(jī)轉(zhuǎn)子轉(zhuǎn)速，電機(jī)轉(zhuǎn)子轉(zhuǎn)速達(dá)到預(yù)設(shè)位置后，執(zhí)行張開和閉合動作；
[0013] 咬合階段采用模糊PID控制：以驅(qū)動桿給定位置和實(shí)際位置的偏差作為輸入，利 用PID控制器的參數(shù)當(dāng)前值，經(jīng)過運(yùn)算產(chǎn)生位置環(huán)的速度輸出。位置環(huán)的速度輸出和前饋 通道的速度輸出，經(jīng)過疊加后作為總的輸出，用作電機(jī)的速度給定。
[0014] 上述技術(shù)方案中，所述過路徑點(diǎn)的3次多項式插值方法如下：
[0015] 在t0時刻的運(yùn)動速度為^ ,在tf時刻的運(yùn)動速度為^ ,于是可以得到機(jī)器人驅(qū) 動桿運(yùn)動的邊界條件：
[0016]q(0) =q〇,q(tf) =qf (1)
[0017] q (〇)=q^ , q (tf)=qf (2)
[0018] 令驅(qū)動桿的位置為：
[0019]q(t) =a〇+a1t+a2t2+a3t3 (3)
[0020] 對其求一階導(dǎo)數(shù)得到驅(qū)動桿速度：
[0021] g(t)=a!+2a3t+3：a9：t2 祕>
[0022] 將式⑴和式⑵代入式（3)和式（4)，可以求解出系數(shù)3。~a3
[0023]
[0024] 得到過路徑點(diǎn)的三次多項式插值的期望位置表達(dá)式（5)和速度表達(dá)式（6):
[0025]
(5 )
[0026]
[0027] 上述技術(shù)方案中，PID控制器的參數(shù)當(dāng)前值是PID控制器的參數(shù)以設(shè)定值為主分 量，以模糊控制器產(chǎn)生的AKp，AKi和AKd參數(shù)修正值為次要分量，兩者相加構(gòu)成的， AKp比例參數(shù)變化量?AKi微分參數(shù)變化量、AKd積分參數(shù)變化量。
[0028] 與現(xiàn)有咀嚼機(jī)器人相比，本咀嚼機(jī)器人的顯著優(yōu)點(diǎn)是：
[0029] (1)力矩大，同等減速條件下所要求匹配的電機(jī)功率就越大，電機(jī)功率越大，電機(jī) 尺寸跟著增大，價格也可能增加，安裝電機(jī)所需要的空間增大，增大機(jī)器人的體積。在同樣 起始點(diǎn)位置和同樣的目標(biāo)位置的情況下，過路徑點(diǎn)的3次多項式插值比點(diǎn)到點(diǎn)的3次多項 式插值所需力矩小。
[0030] (2)不同階段采用不同控制方法，張開閉合階段采用模糊控制，速度更快，由于機(jī) 器人機(jī)械加工精度問題，每個咀嚼機(jī)器人都存在個體差異和六個驅(qū)動桿對稱性，很難得到 一致的PID控制參數(shù)，另外位置變化會導(dǎo)致機(jī)器人本體重心的變化，從而使被控對象的參 數(shù)發(fā)生變化，所以應(yīng)該根據(jù)對象參數(shù)的變化調(diào)整PID控制器的參數(shù)。咬合階段采用模糊自 適應(yīng)PID控制，將摸糊控制技術(shù)引入到PID參數(shù)自整定過程中，根據(jù)不同情況在線自整定 PID參數(shù)，既保證了系統(tǒng)控制的動態(tài)效果，在偏差較大時控制速度較快，采用模糊控制提 高控制的快速性，又保證了較好的穩(wěn)態(tài)性能。在設(shè)定值附近，采用PID控制算法可以有效 地克服系統(tǒng)不穩(wěn)定因素，使系統(tǒng)控制在穩(wěn)定中趨于精確，彌補(bǔ)了以前單一的控制算法的缺 陷。
【附圖說明】
[0031] 圖1表示咀嚼機(jī)器人控制系統(tǒng)硬件；
[0032] 圖2表示系統(tǒng)框圖；
[0033] 圖3表示上位機(jī)控制界面；
[0034] 圖4表示一個驅(qū)動桿實(shí)際伸縮量曲線
[0035] 圖5表示模糊PID控制系統(tǒng)原理圖；
[0036] 圖6表示位置環(huán)變參數(shù)模糊PID控制器框圖；
[0037] 圖7表示電機(jī)速度控制圖；
[0038] 圖8表示無刷直流電機(jī)調(diào)速系統(tǒng)控制框圖。
【具體實(shí)施方式】
[0039] 以下，基于附圖對本發(fā)明的實(shí)