施方式進(jìn)行詳細(xì)說明：
[0040] 本發(fā)明目的在于發(fā)明一種仿真人類咀嚼動(dòng)作以研究食品力學(xué)性能的咀嚼機(jī)器人 控制系統(tǒng)。人類咀嚼運(yùn)動(dòng)是二十多塊咀嚼肌在收縮受到諸多非確定因素如食物的形狀尺寸 和韌脆程度的影響，咀嚼肌在神經(jīng)控制下產(chǎn)生收縮，為下頌運(yùn)動(dòng)提供源動(dòng)力。基于人類咀嚼 肌解剖學(xué)得知，參與人類咀嚼運(yùn)動(dòng)的翼肌包括翼內(nèi)肌和翼外肌，顳肌由前、中、后三束肌肉 組成，咬肌由深層咬肌和淺層咬肌構(gòu)成，因此本專利利用力學(xué)原理的平行四邊形法則對(duì)主 要參與咀嚼運(yùn)動(dòng)的翼肌、顳肌和咬肌進(jìn)行力的合成，確定合成后翼肌、顳肌和咬肌在咀嚼運(yùn) 動(dòng)過程中阻嚼力的方向、阻嚼肌附著點(diǎn)及阻嚼運(yùn)動(dòng)中力的大小，建立參與阻嚼運(yùn)動(dòng)中阻嚼 肌的三維模型作為控制系統(tǒng)的基礎(chǔ)。
[0041] 對(duì)于咀嚼機(jī)器人來說，為了控制機(jī)器人的關(guān)節(jié)空間運(yùn)動(dòng)量，并使關(guān)節(jié)運(yùn)動(dòng)軌跡平 滑，關(guān)節(jié)運(yùn)動(dòng)平穩(wěn)，需要對(duì)機(jī)器人的關(guān)節(jié)運(yùn)動(dòng)進(jìn)行規(guī)劃，主要是包括關(guān)節(jié)運(yùn)動(dòng)軌跡的選擇和 關(guān)節(jié)運(yùn)動(dòng)位置的插值，一般采用3次多項(xiàng)式插值的方法，但是所需力矩大，力矩大對(duì)電機(jī)造 成影響，使機(jī)器不穩(wěn)定，在同樣起始點(diǎn)位置和同樣的目標(biāo)位置的情況下，過路徑點(diǎn)的3次多 項(xiàng)式插值比點(diǎn)到點(diǎn)的3次多項(xiàng)式插值所需力矩小，所以過路徑點(diǎn)的3次多項(xiàng)式插值法更適 用于咀嚼機(jī)器人系統(tǒng)。運(yùn)動(dòng)控制的目的就是使之能精確并且快速地實(shí)現(xiàn)點(diǎn)到點(diǎn)以及任意轉(zhuǎn) 角運(yùn)動(dòng)，目前，較常用的控制方式主要是PID控制法和模糊邏輯控制法。由于控制機(jī)器人六 根驅(qū)動(dòng)桿的電機(jī)的不完全對(duì)稱性以及場(chǎng)上的各種干擾因素等，機(jī)器人的運(yùn)動(dòng)具有時(shí)變、非 線性、干擾大和不確定等特性，用傳統(tǒng)的PID控制器已不能達(dá)到較好的控制效果。而模糊 控制無需建立精確的數(shù)學(xué)模型，具有較強(qiáng)的魯棒性，可用于非線性、時(shí)變及時(shí)滯系統(tǒng)的控 制，是一種對(duì)模型要求不高但又有良好控制效果的控制新策略。可是，任何一種純模糊控 制器本質(zhì)上是一種非線性ro控制，不具備積分作用，控制過程有時(shí)會(huì)出現(xiàn)不平滑現(xiàn)象， 穩(wěn)態(tài)誤差也較難減至理想程度。因此，模糊PID控制方法無疑可以取得比傳統(tǒng)PID控制或 單一的模糊邏輯控制更好的控制效果
[0042] 由于機(jī)器人機(jī)械加工精度問題，每個(gè)咀嚼機(jī)器人都存在個(gè)體差異，很難得到一致 的PID控制參數(shù)。本文提出了一種基于模糊控制理論的咀嚼機(jī)器人控制系統(tǒng)，針對(duì)傳統(tǒng) PID控制器的特點(diǎn)，將摸糊控制技術(shù)引入到PID參數(shù)自整定過程中，根據(jù)不同情況在線自 整定PID參數(shù)。通過和傳統(tǒng)的PID控制器比較，模糊自適應(yīng)PID控制器具有具有靈活性 好，控制適應(yīng)性強(qiáng)，動(dòng)態(tài)、靜態(tài)性能好等優(yōu)點(diǎn)。
[0043] 控制系統(tǒng)通過控制電機(jī)驅(qū)動(dòng)滾珠絲杠的進(jìn)行旋轉(zhuǎn)，利用滾珠絲杠將旋轉(zhuǎn)運(yùn)動(dòng)轉(zhuǎn)化 為直線運(yùn)動(dòng)，并且經(jīng)過減速機(jī)構(gòu)將轉(zhuǎn)矩輸出。控制電機(jī)的轉(zhuǎn)速和旋轉(zhuǎn)方向來改變滾珠絲杠 伸縮量的變化，從而完成驅(qū)動(dòng)桿的直線伸縮，達(dá)到來模仿咀嚼肌在咀嚼運(yùn)動(dòng)時(shí)的收縮運(yùn)動(dòng)。 該咀嚼機(jī)器人使用THK公司的滾珠絲杠，絲杠直徑4mm，導(dǎo)程為1mm。因此咀嚼機(jī)器人給定 下頌運(yùn)動(dòng)軌跡方程即給定電機(jī)轉(zhuǎn)速與時(shí)間的函數(shù)關(guān)系式，通過對(duì)六根驅(qū)動(dòng)器的電機(jī)進(jìn)行控 制以達(dá)到分別控制六根桿伸縮量，從而驅(qū)動(dòng)下頌骨模擬人的咀嚼運(yùn)動(dòng)。
[0044] 控制系統(tǒng)通過RS-485總線與上位機(jī)進(jìn)行通信，以STM32F103RB芯片作為主控制 器，主頻為72MHz，芯片內(nèi)部集成USART控制器，具有2個(gè)12位模數(shù)轉(zhuǎn)換器，5個(gè)多功能定時(shí) 器，滿足多樣和定時(shí)的要求，擁有電機(jī)控制的各個(gè)接口。采用PWM方式控制三相電橋控制電 機(jī)轉(zhuǎn)速，采用霍爾元件檢測(cè)轉(zhuǎn)子位置和電機(jī)轉(zhuǎn)速，三相電橋的低端增加一個(gè)檢流電阻測(cè)定 子電流。主控芯片不斷采集實(shí)際轉(zhuǎn)速，修正上位機(jī)給出的控制轉(zhuǎn)速，實(shí)現(xiàn)對(duì)電機(jī)的轉(zhuǎn)速閉環(huán) 控制。基于Cortex-M3內(nèi)核的STM32F103RB芯片是新型的32位嵌入式微處理器，它是不 需操作系統(tǒng)的ARM，其性能遠(yuǎn)高于51系列單片機(jī)，但開發(fā)過程與51系列單片機(jī)一樣簡(jiǎn)便， 因而在很多應(yīng)用場(chǎng)合可替代51系列單片機(jī)。Cortex-M3采用了哈佛結(jié)構(gòu)，擁有獨(dú)立的指令 總線和數(shù)據(jù)總線，可以讓取指與數(shù)據(jù)訪問并行不悖；Cortex-M3內(nèi)部含有好幾條總線接口， 每條都為自己的應(yīng)用場(chǎng)合優(yōu)化過，并且他們可以平行工作?；赟TM2F103RB主控芯片控制 6臺(tái)無刷直流電機(jī)帶動(dòng)相對(duì)應(yīng)的驅(qū)動(dòng)桿分別模擬咬肌。其中各電機(jī)的運(yùn)動(dòng)方程由上位機(jī)發(fā) 送給主控芯片以達(dá)到實(shí)現(xiàn)不同咀嚼動(dòng)作的目的。通過安裝在磨牙等處的傳感器對(duì)咀嚼力進(jìn) 行采集并進(jìn)行AD轉(zhuǎn)換反饋給上位機(jī)以待分析食物咀嚼特性之用。
[0045] 步驟1 :咀嚼機(jī)器人的運(yùn)動(dòng)軌跡規(guī)劃：通過切牙點(diǎn)的位置和位姿，求出各驅(qū)動(dòng)桿的 伸縮量，過路徑點(diǎn)的3次多項(xiàng)式插值方法求出各驅(qū)動(dòng)桿期望位置表達(dá)式和速度表達(dá)式）為 了仿真人類的咀嚼動(dòng)作，定坐標(biāo)系0S-XSYSZS固定在顱骨上，動(dòng)坐標(biāo)系0M-XMYMZM固定在下 頌骨上.使用三維空間測(cè)量?jī)x對(duì)切牙進(jìn)行測(cè)量，得到下頌骨切牙點(diǎn)的位置參數(shù)（x，y，z)T 和位姿參數(shù)（a，0，y)T，通過坐標(biāo)變換和姿態(tài)矩陣求出各分支桿的伸縮量，使用過路徑點(diǎn) 的3次多項(xiàng)式插值方法對(duì)各分支桿的運(yùn)動(dòng)軌跡規(guī)劃?？紤]某驅(qū)動(dòng)桿從t0時(shí)刻的位置qO運(yùn) 動(dòng)到tf?時(shí)刻的位置qf的情況。假設(shè)在t0時(shí)刻的運(yùn)動(dòng)速度為在tf?時(shí)刻的運(yùn)動(dòng)速度為 ^，于是可以得到機(jī)器人驅(qū)動(dòng)桿運(yùn)動(dòng)的邊界條件：
[0046] q(0) =q〇,q(tf) =qf (1) ? * * *
[0047] q(〇) ,q(tf)=^gf(2)
[0048] 令驅(qū)動(dòng)桿的位置為：
[0049] q(t) =a〇+a1t+a2t2+a3t3 (3)
[0050] 對(duì)其求一階導(dǎo)數(shù)得到驅(qū)動(dòng)桿速度：
[0051 ] <7 (t)=ai+2a；?t+3a312 (4)
[0052]將式⑴和式⑵代入式（3)和式（4)，可以求解出系數(shù)aO~a3
[0053]
[0054] 得到過路徑點(diǎn)的三次多項(xiàng)式插值的期望位置表達(dá)式（5)和速度表達(dá)式（6):
[0055]
[0057] 在同樣起始點(diǎn)位置和同樣的目標(biāo)位置的情況下，過路徑點(diǎn)的3次多項(xiàng)式插值比點(diǎn) 到點(diǎn)的3次多項(xiàng)式插值所需力矩小。
[0058] 步驟2 :對(duì)硬件進(jìn)行選擇和系統(tǒng)圖，如圖1所示，本發(fā)明的控制系統(tǒng)硬件包括機(jī)器 人控制上位機(jī)，以及基于Cortex-M3內(nèi)核的32位單片機(jī)的下位機(jī)，其中上下位機(jī)利用RS485 總線進(jìn)行通信、主控單片機(jī)、最小單片機(jī)系統(tǒng)、JTAG調(diào)試模塊、壓力傳感器、模擬信號(hào)放大電 路、SPI控制總線、三相電橋驅(qū)動(dòng)電路、無刷直流電機(jī)。
[0059] 圖2是系統(tǒng)框圖，無刷直流電機(jī)的加速度與電動(dòng)機(jī)的轉(zhuǎn)矩成正比，而轉(zhuǎn)矩又與電 動(dòng)機(jī)的電流成正比，要實(shí)現(xiàn)電機(jī)的高精度高動(dòng)態(tài)性能控制，僅僅對(duì)轉(zhuǎn)子位置和速度進(jìn)行檢 測(cè)是不夠的，還需要對(duì)電機(jī)的電流檢測(cè)和控制。采用控制器核心采用STM32F103RB芯片， 驅(qū)動(dòng)部分采用MOSFET構(gòu)成的三相電橋?qū)崿F(xiàn)電機(jī)所需的三相逆變驅(qū)動(dòng)電路；光電隔離是由F0D3181集成隔離驅(qū)動(dòng)的；采用PWM脈寬調(diào)制控制三相電橋調(diào)節(jié)電機(jī)轉(zhuǎn)速，通過電機(jī)自帶的 霍爾傳感器檢測(cè)電機(jī)轉(zhuǎn)速及電機(jī)位置。實(shí)時(shí)對(duì)電機(jī)轉(zhuǎn)速進(jìn)行檢測(cè)修正轉(zhuǎn)速表中電機(jī)速度， 實(shí)現(xiàn)對(duì)電機(jī)的閉環(huán)控制。無刷直流電機(jī)通過霍爾傳感器將三路位置信號(hào)與stm32的3個(gè) GPI0 口相連，對(duì)3個(gè)接口的電平邊沿進(jìn)行檢測(cè)，當(dāng)電平發(fā)生變化即產(chǎn)生一個(gè)中斷從而完成 對(duì)定子轉(zhuǎn)速計(jì)算及換相的功能。電流檢測(cè)是把檢流電阻上的電壓信號(hào)送入信號(hào)調(diào)理電路， 調(diào)理電路分為兩部分，第一個(gè)是把信號(hào)送入一個(gè)比較器，作為故障電流報(bào)警；第二個(gè)是把信 號(hào)送入一個(gè)差分放大電路中，然后直接送入STM32的A/D端口，轉(zhuǎn)化成數(shù)字量進(jìn)行過流判斷 以及參與電流環(huán)運(yùn)算。
[0060] 將運(yùn)動(dòng)參數(shù)輸入上位機(jī)控制界面如圖3所示。通過測(cè)量肌電圖可以推出不同時(shí)間 驅(qū)動(dòng)桿驅(qū)動(dòng)力大小，在選定Faulhaber公司2038無刷直流伺服電機(jī)的條件下，將各驅(qū)動(dòng)桿 的速度曲線離散為若干個(gè)時(shí)間節(jié)點(diǎn)，發(fā)送給下位機(jī)存入調(diào)速表。且可以通過上位機(jī)控制面 板設(shè)置咀嚼次數(shù)...