食品配料機(jī)器人調(diào)配咖啡流程運(yùn)動(dòng)規(guī)劃

鄭漫 ,金軍

(1.安徽汽車職業(yè)技術(shù)學(xué)院 機(jī)電工程系,安徽 合肥,230601;2.合肥森曼智能科技有限公司,安徽 合肥,230601)

隨著科學(xué)技術(shù)的發(fā)展,食品配料機(jī)器人技術(shù)的應(yīng)用不斷改善著人類生產(chǎn)生活,在工業(yè)領(lǐng)域廣泛應(yīng)用的機(jī)器人逐漸將人們從繁重的勞動(dòng)中解放出來[1-3]。食品配料機(jī)器逐漸開始取代傳統(tǒng)廚師,其為顧客帶來了優(yōu)質(zhì)的服務(wù)和體驗(yàn)[4]。全自動(dòng)食品配料機(jī)器人沖泡機(jī)可以在1 h 內(nèi)沖出100 杯咖啡,相當(dāng)于3 個(gè)咖啡師的產(chǎn)量。客戶可通過應(yīng)用程序訂購的美味咖啡杯,同時(shí)可以控制配料、濃度、調(diào)味料和溫度。

無論是在研究環(huán)境還是在工業(yè)應(yīng)用中,食品配料機(jī)器人的路徑規(guī)劃都是一個(gè)具有挑戰(zhàn)性的問題,特別是在高度動(dòng)態(tài)的環(huán)境中考慮人機(jī)交互。在這種情況下,食品配料機(jī)器人研究的最終目標(biāo)是為食品配料機(jī)器人提供在線、自主的運(yùn)動(dòng)規(guī)劃。與許多機(jī)器人的運(yùn)動(dòng)規(guī)劃方法相比,食品配料機(jī)器人機(jī)械手的方法假設(shè)環(huán)境是靜止的[5-7],張延恒等[8]提出了以黎曼曲面上的測(cè)地線為依據(jù)進(jìn)行操作臂與安裝基座耦合狀態(tài)下的空間機(jī)器人運(yùn)動(dòng)規(guī)劃的方法,劉龍等[9]提出了根據(jù)不同的路徑信息控制電機(jī),進(jìn)而調(diào)整小車速度和方向,最終實(shí)現(xiàn)小車沿設(shè)定路徑行駛的方法。在路徑生成之前需要離線估算障礙物,在離散化的食品配料機(jī)器人構(gòu)型空間(C 空間)中生成子目標(biāo)來指導(dǎo)規(guī)劃路徑,并行搜索有效路徑。

本文以食品配料機(jī)器人的七自由度機(jī)械臂為實(shí)例,分析了機(jī)械臂的運(yùn)動(dòng)學(xué)正反解問題[10-11]。建立運(yùn)動(dòng)學(xué)模型對(duì)研究七自由度機(jī)械臂有很大的意義,有效驗(yàn)證了機(jī)械臂數(shù)學(xué)模型以及正解、逆解的正確性。在此基礎(chǔ)上提出七自由度機(jī)械臂的規(guī)劃器,其動(dòng)態(tài)特性確保它可以處理靜止、動(dòng)態(tài)環(huán)境中的問題。

1 機(jī)械臂運(yùn)動(dòng)學(xué)建模

根據(jù)七自由度機(jī)械臂的機(jī)械結(jié)構(gòu),關(guān)節(jié)1、3、5、7 為旋轉(zhuǎn)關(guān)節(jié);關(guān)節(jié)2、4、6 為上升(或下降)關(guān)節(jié)。為了解決運(yùn)動(dòng)學(xué)冗余自由度復(fù)雜導(dǎo)致運(yùn)動(dòng)學(xué)多解,利用避障和軌跡算法來對(duì)機(jī)器人整體進(jìn)行研究。

1.1 坐標(biāo)系建立

將基座坐標(biāo)系建立于S點(diǎn)(肩部)處,各個(gè)軸建立的坐標(biāo)系相互之間關(guān)系如圖1 所示(注意,在ROS零位時(shí),L2與L3是共線的)。

圖1 7 自由度機(jī)械臂運(yùn)動(dòng)學(xué)坐標(biāo)系分布

為描述方便,在下文中將關(guān)節(jié)n所對(duì)應(yīng)的坐標(biāo)系簡(jiǎn)稱為坐標(biāo)系n,將關(guān)節(jié)i的關(guān)節(jié)值記為θi,i=1,2,3,…,7。

關(guān)節(jié)1、2 和3的軸線相交于一點(diǎn),交點(diǎn)與坐標(biāo)系{1}、{2}和{3}的原點(diǎn)重合,關(guān)節(jié)軸1、2 和3 相互垂直。關(guān)節(jié)5、6 和7的軸線相交于一點(diǎn),交點(diǎn)與坐標(biāo)系{5}、{6}和{7}的原點(diǎn)重合,關(guān)節(jié)軸5、6 和7相互垂直。各個(gè)坐標(biāo)系的建立方法如下:

(1) 坐標(biāo)系1 與基坐標(biāo)系的初始位置重合,并繞其Z軸轉(zhuǎn)動(dòng),轉(zhuǎn)動(dòng)角度為θ1;

(2) 坐標(biāo)系2 與坐標(biāo)系1 初始位置重合并繞其Y軸轉(zhuǎn)動(dòng),轉(zhuǎn)動(dòng)角度為θ2;

(3) 坐標(biāo)系3 與坐標(biāo)系2 初始位置重合并繞其X軸轉(zhuǎn)動(dòng),轉(zhuǎn)動(dòng)角度為θ3;

(4) 坐標(biāo)系4 相繞其Y軸轉(zhuǎn)動(dòng),轉(zhuǎn)動(dòng)角度為θ4,并對(duì)于坐標(biāo)系3的X軸正方向平移L2,平移向量為[L20 0]T;

(5) 坐標(biāo)系5 與坐標(biāo)系4 初始位置重合并繞其X軸轉(zhuǎn)動(dòng),轉(zhuǎn)動(dòng)角度為θ5;

(6) 坐標(biāo)系6 相繞其Y軸轉(zhuǎn)動(dòng),轉(zhuǎn)動(dòng)角度為θ6,并對(duì)于坐標(biāo)系5的X軸正方向平移L3,平移向量為[L30 0]T;

(7) 坐標(biāo)系7 與坐標(biāo)系6 初始位置重合并繞其X軸轉(zhuǎn)動(dòng),轉(zhuǎn)動(dòng)角度為θ7;

(8) 坐標(biāo)系8 相對(duì)于坐標(biāo)系7 沿其X軸正方向平移L4,無轉(zhuǎn)動(dòng)。

程序默認(rèn)的運(yùn)動(dòng)參數(shù)如表1 所示。

表1 食品配料機(jī)器人關(guān)節(jié)運(yùn)動(dòng)范圍

續(xù)表1

1.2 運(yùn)動(dòng)學(xué)正解

通常把描述一個(gè)連桿和下一個(gè)連桿間相對(duì)關(guān)系的齊次變換叫做T矩陣。本文首先采用MDH 方法建立坐標(biāo)系,進(jìn)而用齊次變換矩陣來建立食品配料機(jī)器人末端執(zhí)行器坐標(biāo)系相對(duì)于基坐標(biāo)系的數(shù)學(xué)模型。根據(jù)以上這些參數(shù)可以得到連桿坐標(biāo)系之間的關(guān)系,同時(shí)獲得各個(gè)關(guān)節(jié)之間的變換矩陣。

根據(jù)各個(gè)關(guān)節(jié)的空間幾何關(guān)系可以獲取到相鄰兩個(gè)坐標(biāo)系的變換矩陣,如下所示:

總體正解為正確的運(yùn)動(dòng)學(xué)正解。

1.3 運(yùn)動(dòng)學(xué)逆解

將反向解析問題轉(zhuǎn)化為只與前7 個(gè)坐標(biāo)系相關(guān)的反向解析問題,下文所述中的“末端”如未做說明全部視為機(jī)械臂末端均為第7 軸末端而不是夾持器的末端,其向量的值即為第6 關(guān)節(jié)處質(zhì)心的空間坐標(biāo)。

由于7自由度機(jī)械臂的正向解析結(jié)果中涉及到7個(gè)關(guān)節(jié)值的正弦和余弦值,使用傳統(tǒng)的反向解析方法,相當(dāng)于處理一個(gè)七元七次方程組。因涉及到正弦與余弦,過高的次數(shù)會(huì)給計(jì)算帶來極大障礙。分析7 軸機(jī)械臂的結(jié)構(gòu),容易得出反解會(huì)有無數(shù)組解,故ROS 采用一種新型的解法,逐步將各個(gè)軸的關(guān)節(jié)值解出。

1.3.1 反向解析第4 軸

機(jī)械臂的結(jié)構(gòu)是以人類手臂為原型設(shè)計(jì)的,通過分析7 自由度機(jī)械臂的空間幾何特點(diǎn)可以發(fā)現(xiàn):當(dāng)末端位姿與第4軸的關(guān)節(jié)值相對(duì)應(yīng),對(duì)于食品配料機(jī)器人的某一位姿,其各個(gè)關(guān)節(jié)值存在無數(shù)組可行解。第4 關(guān)節(jié)可行位置在空間中形成一個(gè)圓,圓面與第2 關(guān)節(jié)和第6 關(guān)節(jié)中心連線相垂直,各軸之間存在一定的幾何關(guān)系。

1.3.2 反向解析第3 軸

第4 關(guān)節(jié)的位置可以位于一個(gè)圓周上,在所有的可行解中,第3 軸的關(guān)節(jié)值得取值范圍覆蓋了-180°～180°,故在不考慮機(jī)械限位的情況下,第3 軸可以任意取值,本算法中通常取值為第3 軸機(jī)械限位的中位數(shù)。

1.3.3 反向解析第2 軸

由于已經(jīng)解出θ3、θ4,以此為基礎(chǔ)來反向解析θ2,可做如下的處理:

機(jī)械臂的示意圖如圖2 所示。為便于直觀理解,對(duì)圖中幾何關(guān)系進(jìn)行說明,保持第4、5、6、7 軸關(guān)節(jié)值不變,轉(zhuǎn)動(dòng)第3 軸,可以形成如下圖所示的一個(gè)圓周。取G、S、e3 點(diǎn)形成的平面(平面A′、B′、C′、D′垂直于水平面)為研究對(duì)象,點(diǎn)w′為點(diǎn)w在該平面上的投影,點(diǎn)w′與點(diǎn)w的Z軸坐標(biāo)相同的。

圖2 各軸幾何關(guān)系示意

上述分析只考慮θ3及θ4＞0的情況,采用幾何關(guān)系求解需要分別討論θ2、θ3及θ4的多種情況,計(jì)算過程相關(guān)比較繁瑣,因此采用運(yùn)動(dòng)學(xué)逆解的方式進(jìn)行計(jì)算。

1.3.4 反向解析第1 軸

1.3.5 反向解析第5 至7 軸

反解前4 軸主要借助于末端位姿矩陣的p向量,而反解第5 至7 軸只需使用末端位姿矩陣中的n,o,a3 個(gè)向量。由坐標(biāo)變換規(guī)則有如下關(guān)系:已知末端位姿矩陣,相當(dāng)于已知同時(shí)由于上文已經(jīng)解出前4 軸關(guān)節(jié)值,相當(dāng)于已知將上式兩側(cè)同時(shí)乘以可得如下關(guān)系:

由于反解第5 至第7 軸只需使用n,o,a3 個(gè)向量,故上式可以簡(jiǎn)化為只做旋轉(zhuǎn)矩陣的乘法,忽略平移向量,即其中由于為單位正交陣,故有如下關(guān)系:

至此,已將7 個(gè)軸的關(guān)節(jié)值全部得到反向解析。

2 分布式動(dòng)態(tài)路徑規(guī)劃

盡管食品配料機(jī)器人調(diào)配咖啡師具有上述優(yōu)點(diǎn),但食品配料機(jī)器人咖啡店要完全取代傳統(tǒng)的咖啡店還存在一定的爭(zhēng)議。盡管AI 有所進(jìn)步,但食品配料機(jī)器人并不能像人類一樣善于區(qū)分口味的微妙之處。由于食品配料機(jī)器人與環(huán)境的實(shí)時(shí)交互,其規(guī)劃系統(tǒng)必須能夠在在線和動(dòng)態(tài)的方式中獲取關(guān)于障礙物的信息。

目前還沒有能夠動(dòng)態(tài)集成障礙物信息的高維規(guī)劃問題的實(shí)時(shí)解決方案,神經(jīng)網(wǎng)絡(luò)已被證明能夠有效地解決多達(dá)3 維的高維規(guī)劃問題。這促使將最初的6D 規(guī)劃問題分解為2 個(gè)3D 問題,2 個(gè)3D 規(guī)劃都涉及共同的現(xiàn)實(shí)環(huán)境限制,使得計(jì)劃者的障礙景觀不得不依賴于其它當(dāng)前配置。

圖3 所示工作空間內(nèi)的圓形障礙物在角空間中誘導(dǎo)出不同的障礙物構(gòu)型,其具體取決于實(shí)際的末端執(zhí)行器在笛卡爾空間中的位置。手臂的當(dāng)前方向分別由黑色箭頭和字母C表示。這里將三維構(gòu)型空間分解為二維位置空間和一維角度空間,分別表示末端執(zhí)行器的笛卡爾位置和姿態(tài)。如果從灰色箭頭所示的角度空間來看,位置空間中顯示的3 種構(gòu)型會(huì)導(dǎo)致不同的障礙物構(gòu)型。當(dāng)觀察圖中額外顯示離散角軸時(shí),禁止區(qū)域在角空間中的位移變得明顯,其中禁止角配置用x標(biāo)記。反之,禁止末端執(zhí)行器在二維笛卡爾空間中的位置隨著末端執(zhí)行器的方向變化而變化。

圖3 工作空間內(nèi)的圓形障礙物在角空間

3 實(shí)驗(yàn)和結(jié)果

3.1 仿真驗(yàn)證

用螺旋線生成一條機(jī)器人位置軌跡曲線將機(jī)器人的位置值輸入到運(yùn)動(dòng)學(xué)求解器,通過逆解求出的一系列關(guān)節(jié)值。將關(guān)節(jié)值通過運(yùn)動(dòng)學(xué)求解器,通過正解求出一系列位置軌跡。將螺旋線生成的軌跡與運(yùn)動(dòng)學(xué)正解求取的位置軌跡進(jìn)行比較,如圖4 所示。

圖4 中藍(lán)色曲線代表規(guī)劃的螺旋線,紅色曲線代表用反解求出的關(guān)節(jié)值通過運(yùn)動(dòng)學(xué)正解求解出的位置螺旋線,從圖4 可知,兩條曲線處于重合狀態(tài),證明了機(jī)器人運(yùn)動(dòng)學(xué)計(jì)算的正確性。

圖4 正反解程序驗(yàn)證對(duì)比

3.2 實(shí)物驗(yàn)證

根據(jù)咖啡的調(diào)配流程和機(jī)器人運(yùn)動(dòng)學(xué)進(jìn)行了如圖5的實(shí)驗(yàn)流程。

圖5 機(jī)器人制作咖啡流程

食品配料機(jī)器人非常擅長(zhǎng)以完全相同的方式處理事情。三葉草倒立的圈子完全相同,其是在實(shí)驗(yàn)室中的無數(shù)實(shí)驗(yàn)確定的節(jié)拍節(jié)奏,因此計(jì)時(shí)非常精確。咖啡師所要做的就是提供正確劑量的新鮮磨碎的咖啡,確保將其正確分配在過濾器中,然后按一下按鈕,機(jī)器完成其余的工作。自動(dòng)化咖啡站提供快速智能的服務(wù),無需全天候的人工干預(yù)即可為客戶提供愉悅的體驗(yàn)。

4 結(jié)論

本文提出了一種7 自由度輕量化關(guān)節(jié)的機(jī)器人手臂。針對(duì)機(jī)器人本體結(jié)構(gòu)特性對(duì)機(jī)器人選定坐標(biāo)系,以輕量一體化關(guān)節(jié)食品配料機(jī)器人的七自由度機(jī)械臂為實(shí)例,利用球坐標(biāo)獲得正逆運(yùn)動(dòng)學(xué)方程。同時(shí)設(shè)計(jì)了一款規(guī)劃器,結(jié)合末端執(zhí)行器位置和方向的兩個(gè)部分。路徑規(guī)劃算法專為高度動(dòng)態(tài)環(huán)境下的路徑生成而設(shè)計(jì)。測(cè)試結(jié)果表明,求解機(jī)械臂各個(gè)軸的順序依次為θ4、θ3、θ2、θ1,在每個(gè)步驟中都可能存在多個(gè)符合條件的角,因此導(dǎo)致最終解出多組解,可以根據(jù)機(jī)械限位的情況篩選掉不在此范圍內(nèi)的解。