姚國(guó)林,王合闖
(1.河南農(nóng)業(yè)職業(yè)學(xué)院機(jī)電工程學(xué)院,河南鄭州 451450;2.華北水利水電大學(xué)信息工程學(xué)院,河南鄭州 450045)
采摘機(jī)器人一直是智能農(nóng)業(yè)裝備領(lǐng)域的研究熱點(diǎn),由于機(jī)械手的可操作性和精度較高,一般采用機(jī)械手作為采摘機(jī)器人的主要執(zhí)行部件[1]。采用機(jī)械手作為采摘機(jī)器人的主要執(zhí)行部件,首先需要解決機(jī)械手的運(yùn)動(dòng)規(guī)劃問題。運(yùn)動(dòng)規(guī)劃是指機(jī)械手從初始構(gòu)型移動(dòng)到目標(biāo)構(gòu)型的過程中,各個(gè)關(guān)節(jié)的運(yùn)動(dòng)軌跡規(guī)劃。在這個(gè)過程中,必須保證機(jī)械手不與工作空間內(nèi)的任何障礙物或本身發(fā)生碰撞。
國(guó)內(nèi)外研究學(xué)者對(duì)機(jī)器人的運(yùn)動(dòng)規(guī)劃問題開展了大量研究,現(xiàn)有的運(yùn)動(dòng)規(guī)劃算法包括快速隨機(jī)搜索樹(Rapidly Random Trees,RRT)[2]、概率路線圖法(Probabilistic Roadmap Method,PRM)[3]、A*算法[4]、神經(jīng)網(wǎng)絡(luò)模型[5]、人工勢(shì)場(chǎng)(Artificial Potential Field,APF)[6]。由于采摘機(jī)器人工作的環(huán)境是非結(jié)構(gòu)化的、非常復(fù)雜的、障礙物很多的環(huán)境,機(jī)械手的C空間構(gòu)造將花費(fèi)極長(zhǎng)的時(shí)間。從現(xiàn)有文獻(xiàn)來看,RRT、PRM和A*算法需要構(gòu)建C空間,因此無法滿足拾取機(jī)器人的實(shí)時(shí)性要求。而神經(jīng)網(wǎng)絡(luò)算法不僅需要長(zhǎng)時(shí)間的自我訓(xùn)練,對(duì)硬件的要求也更高,因此也無法用于拾取機(jī)器人的實(shí)時(shí)運(yùn)動(dòng)規(guī)劃。
APF算法通過笛卡爾空間建立虛擬勢(shì)場(chǎng)來規(guī)劃運(yùn)動(dòng),其中運(yùn)動(dòng)物體在從初始位置移動(dòng)到目標(biāo)位置的過程中會(huì)受到引力和斥力兩種力的影響,并避免碰撞障礙物。與RRT、PRM等全局規(guī)劃算法不同,APF是一種算法復(fù)雜度低、對(duì)計(jì)算機(jī)資源依賴性低、實(shí)時(shí)性強(qiáng)的局部運(yùn)動(dòng)規(guī)劃算法。但是,APF存在局部最小缺陷(Local Minimum Trap,LMT)和目標(biāo)不可達(dá)問題(Target Unreachable Problem,TNRP)等問題[7]。MATOUI 等[8]對(duì)APF算法進(jìn)行了改進(jìn),設(shè)計(jì)了一種非最小速度算法,改進(jìn)后的APF能有效避免出現(xiàn)LMT現(xiàn)象。彭鵬等人[9]提出一種改進(jìn)的APF和RRT結(jié)合的機(jī)械臂避碰路徑規(guī)劃算法,該算法有效避免了機(jī)械臂運(yùn)動(dòng)規(guī)劃陷入LMT問題。李卓等人[10]在傳統(tǒng)的智能車運(yùn)動(dòng)規(guī)劃方面,將速度和加速度等因素導(dǎo)入了引力勢(shì)場(chǎng),通過仿真表明該算法能有效避免TNRP問題。由于采摘機(jī)器人對(duì)運(yùn)動(dòng)規(guī)劃算法的實(shí)時(shí)性要求較高,上述研究均耗時(shí)較長(zhǎng),無法滿足采摘機(jī)器人的實(shí)時(shí)性要求。
在上述研究的基礎(chǔ)上,本文作者分別對(duì)APF的勢(shì)場(chǎng)和力的計(jì)算模型進(jìn)行了改進(jìn),在規(guī)劃?rùn)C(jī)械手運(yùn)動(dòng)時(shí)直接計(jì)算機(jī)械手關(guān)節(jié)空間的吸引力矩,提出了改進(jìn)的人工勢(shì)場(chǎng) (IAPF)優(yōu)化算法,有效地減少機(jī)械手運(yùn)動(dòng)規(guī)劃的計(jì)算時(shí)間,同時(shí)避免LMT和TPNR問題。最后通過實(shí)驗(yàn)結(jié)果,驗(yàn)證了所提優(yōu)化算法在采摘機(jī)械手無碰撞運(yùn)動(dòng)規(guī)劃中的實(shí)時(shí)性和準(zhǔn)確性。
圖1為本文作者所研究的智能采摘機(jī)器人的功能模型,主要由3個(gè)部件組成:負(fù)責(zé)果園內(nèi)運(yùn)動(dòng)的自動(dòng)移動(dòng)車輛,并作為搭載所有其他部件的平臺(tái);用于采摘水果的六連桿機(jī)械手;安裝在車輛上用于放置機(jī)器人控制柜的框架結(jié)構(gòu),包括水果貯藏箱、移動(dòng)電源以及用于檢測(cè)和定位水果的立體攝像機(jī)視覺系統(tǒng)。
圖1 智能采摘機(jī)器人實(shí)物模型
機(jī)械手運(yùn)動(dòng)學(xué)模型的建立是運(yùn)動(dòng)規(guī)劃的前提工作,本文作者采用初等變換序列對(duì)六連桿機(jī)械手進(jìn)行運(yùn)動(dòng)學(xué)建模[11],如圖2所示,di(i=1,2,…,6)表示連桿偏移量,αi(i=1,2,…,6)表示連桿扭轉(zhuǎn)角,ai(i=1,2,…,6)表示連桿長(zhǎng)度,θi(i=1,2,…,6)表示關(guān)節(jié)角度。從初等變換序列模型中,可得到機(jī)械手上每一點(diǎn)的雅可比矩陣。
圖2 六連桿機(jī)械手的初等變換序列模型
圖3所示為采摘水果的3個(gè)步驟:
圖3 采摘機(jī)械手運(yùn)動(dòng)規(guī)劃流程
步驟1,采摘機(jī)械手從初始位置移動(dòng)到采摘位置,目標(biāo)水果由末端執(zhí)行器采摘分離。在規(guī)劃運(yùn)動(dòng)時(shí),同時(shí)設(shè)計(jì)和計(jì)算采摘構(gòu)型。
步驟2,采摘機(jī)械手從采摘構(gòu)型移動(dòng)到釋放構(gòu)型,將水果放入存儲(chǔ)箱中。
步驟3,采摘機(jī)械手從釋放構(gòu)型移動(dòng)到初始構(gòu)型,等待下一個(gè)采摘周期。在采摘實(shí)驗(yàn)中,初始構(gòu)型和釋放構(gòu)型都是預(yù)先設(shè)定的,以進(jìn)一步提高實(shí)時(shí)性。
圖4所示為六連桿機(jī)械手的運(yùn)動(dòng)規(guī)劃概念圖。
圖4 六連桿機(jī)械手的運(yùn)動(dòng)規(guī)劃概念圖
APF算法是通過建立虛擬勢(shì)能場(chǎng)來規(guī)劃運(yùn)動(dòng)軌跡,以避免與工作空間中的任何障礙物發(fā)生碰撞[12]。引力勢(shì)場(chǎng)和引力的計(jì)算公式為
(1)
Fa=-?Ua=kada?da
(2)
式中:Ua∈R為引力勢(shì)場(chǎng);Fa為引力;ka為引力系數(shù);da為目標(biāo)距離。斥力勢(shì)場(chǎng)和斥力計(jì)算公式為
(3)
(4)
式中:Ur∈R為斥力勢(shì)場(chǎng);Fr為斥力;kr為斥力系數(shù);dr為障礙物距離;d0為距離的閾值。合力勢(shì)場(chǎng)Us和合力Fs計(jì)算公式為
Us=Ua+Ur
(5)
Fs=Fa+Fr
(6)
與移動(dòng)車輛不同,APF的應(yīng)用和機(jī)械手的無碰撞運(yùn)動(dòng)規(guī)劃更加復(fù)雜。移動(dòng)車輛可以看作是工作空間中的一個(gè)點(diǎn)進(jìn)行運(yùn)動(dòng)規(guī)劃,但對(duì)于機(jī)械手來說,需要考慮如何對(duì)整個(gè)機(jī)械手進(jìn)行無碰撞運(yùn)動(dòng)規(guī)劃,而不是將其作為單個(gè)點(diǎn)。機(jī)械手的路徑規(guī)劃可以在其工作空間或C空間中進(jìn)行。由于C空間的計(jì)算量大,構(gòu)建耗時(shí)長(zhǎng),所以通常采用工作空間中的路徑規(guī)劃。由于機(jī)械手上每個(gè)點(diǎn)的運(yùn)動(dòng)不能單獨(dú)規(guī)劃,因此必須在整體機(jī)械臂結(jié)構(gòu)的背景下考慮機(jī)械手上的點(diǎn)。在此研究中,引力作用于所有初等變換序列坐標(biāo)系的原點(diǎn),斥力作用于每個(gè)初等變換序列線段上距離障礙物最近的點(diǎn),如圖5所示。通過引力作用,機(jī)械手可以從初始構(gòu)型變?yōu)槟繕?biāo)構(gòu)型。通過斥力作用,可以避免在工作空間中移動(dòng)時(shí)撞到任何障礙物。
圖5 施加在機(jī)械手上的IAPF力示意
當(dāng)工作空間中有多個(gè)障礙物時(shí),合力勢(shì)場(chǎng)的合力可以計(jì)算為
(7)
(8)
為了控制機(jī)械手,通過雅可比矩陣的轉(zhuǎn)換,最終將合力映射到關(guān)節(jié)力矩上,計(jì)算關(guān)節(jié)力矩的公式為
(9)
APF中由于存在局部最小缺陷(LMT)、目標(biāo)位置不可達(dá)(TNRP)等問題,將直接導(dǎo)致兩個(gè)問題,即Fa隨著da的減小而線性減小,當(dāng)da=0時(shí),F(xiàn)a減小到零,但當(dāng)dr趨于零時(shí)Fr趨于增加,當(dāng)dr=0時(shí),F(xiàn)r增大到無窮大。首先,如果在目標(biāo)位置附近有障礙物,其斥力會(huì)遠(yuǎn)遠(yuǎn)大于引力。當(dāng)物體接近目標(biāo)位置時(shí),會(huì)不斷被斥力反彈,導(dǎo)致路徑振蕩甚至失效,這就是TNRP問題。其次,機(jī)械手關(guān)節(jié)所能承受的力矩有限,在運(yùn)動(dòng)規(guī)劃過程中無法實(shí)現(xiàn)無限的斥力。為了解決上述兩個(gè)問題,本文作者對(duì)勢(shì)場(chǎng)和力模型進(jìn)行了優(yōu)化。
為了避免TNRP問題,需要在接近目標(biāo)位置時(shí)仍然有較大的梯度,以確保Fa足夠大,能夠克服Fr的影響,連續(xù)引導(dǎo)物體到達(dá)目標(biāo)位置。同時(shí),為了使路徑規(guī)劃算法收斂,當(dāng)da=0時(shí),F(xiàn)a也應(yīng)該趨近于0。為此,IAPF算法在APF算法原有的引力勢(shì)場(chǎng)計(jì)算模型的基礎(chǔ)上,通過加入Sigmoid函數(shù)分量以提升算法的收斂性[13],即IAPF算法的引力勢(shì)場(chǎng)和引力計(jì)算為
(10)
(11)
式中:ka1和ka2為Sigmoid函數(shù)的控制因子,為避免Fr隨dr減小而不可控的問題,IAPF算法改變了原有的斥力勢(shì)場(chǎng)模型。APF算法的斥力勢(shì)場(chǎng)模型是分段函數(shù),不僅需要在分段接點(diǎn)處連續(xù)可微,而且當(dāng)dr=0時(shí)還會(huì)發(fā)生梯度爆炸。為確保IAPF算法的斥力不會(huì)無限增加,并且隨著dr的減小仍然是連續(xù)可微的,本文作者將IAPF算法的斥力變?yōu)橐粋€(gè)反S的Sigmoid函數(shù)[14],IAPF算法的斥力函數(shù)不僅在遠(yuǎn)離障礙物時(shí)趨近于零,而且隨著dr的減小而增大,仍然是連續(xù)可微的。IAPF算法的Fr會(huì)逐漸增大,并趨于最大斥力,不會(huì)出現(xiàn)斥力爆炸的問題。IAPF算法的斥力勢(shì)場(chǎng)和斥力計(jì)算為
(12)
(13)
式中:kr1和kr2為反S型Sigmoid函數(shù)的控制因子;kr3為Ur的補(bǔ)償系數(shù)。為了消除LMT帶來的影響,IAPF算法選擇在合力勢(shì)場(chǎng)梯度消失時(shí),增加臨時(shí)力Ft,打破斥力和引力的平衡。Ft不僅決定了IAPF算法結(jié)果的好壞,也決定了IAPF算法是否會(huì)再次陷入LMT。通過仿真實(shí)驗(yàn),F(xiàn)t同時(shí)垂直于Fa和Fr時(shí)效果較好。在文中,當(dāng)IAPF算法即將進(jìn)入LMT時(shí),將出現(xiàn)Ft,其值與Fa相等,其方向如圖6所示。
圖6 避免LMT的臨時(shí)力示意
通過雅可比矩陣的轉(zhuǎn)換,將工作空間中的力直接映射到關(guān)節(jié)空間中的力矩,仍然需要大量的計(jì)算,因此為了進(jìn)一步優(yōu)化,IAPF算法直接計(jì)算關(guān)節(jié)空間中的引力矩,計(jì)算公式為
(14)
(15)
文中的仿真都是在Windows 10操作系統(tǒng)下通過Python 3在Visual Studio Code中實(shí)現(xiàn)的。
APF和IAPF算法的勢(shì)場(chǎng)面和力曲線分別如圖7和圖8所示,表1給出了APF和IAPF算法的勢(shì)場(chǎng)模型和力模型的具體參數(shù),其中Pg為目標(biāo)位置,Pc為當(dāng)前位置,P0為障礙物位置。
表1 第3.1節(jié)仿真使用的具體參數(shù)
圖7 APF和IAPF的勢(shì)場(chǎng)面
圖8 APF和IAPF的力曲線
可以看出:APF的引力勢(shì)場(chǎng)梯度在目標(biāo)位置附近逐漸消失,而IAPF算法的梯度并沒有消失而是增加。因此,與APF相比,IAPF對(duì)TNRP的抵抗力更強(qiáng),收斂速度更快。由于APF斥力勢(shì)場(chǎng)中存在梯度爆炸現(xiàn)象,當(dāng)APF靠近障礙物時(shí),斥力勢(shì)場(chǎng)的梯度遠(yuǎn)大于引力勢(shì)場(chǎng)的坡度,導(dǎo)致合成勢(shì)場(chǎng)的引力勢(shì)場(chǎng)表面缺乏特征。然而,IAPF算法可以有效地克服以上問題,其合成勢(shì)場(chǎng)表面具有引力勢(shì)場(chǎng)面和斥力勢(shì)場(chǎng)面的共同特征,這使得IAPF算法不僅具有更快的收斂速度,還具有較強(qiáng)的無碰撞能力。
當(dāng)目標(biāo)位置附近有障礙物時(shí),APF會(huì)出現(xiàn)TNRP問題。因此,為了驗(yàn)證IAPF算法能夠有效避免TNRP問題,進(jìn)行了下面的仿真實(shí)驗(yàn)。仿真區(qū)域?yàn)?0×10矩形,具體參數(shù)如表2所示,各障礙物的動(dòng)作閾值相等。仿真結(jié)果如圖9所示,仿真曲線如圖10所示。
表2 第3.2節(jié)仿真使用的具體參數(shù)
圖9 TNRP的仿真結(jié)果
圖10 TNRP的仿真曲線
從圖9可以看出:當(dāng)目標(biāo)位置附近有障礙物時(shí),機(jī)械手不斷靠近目標(biāo)位置,但被障礙物的斥力推開,來回?cái)[動(dòng),無法通過APF到達(dá)目標(biāo)位置,因此APF無法收斂。從圖10可以看出:IAPF算法在第20次迭代附近接近目標(biāo)位置,獲得了增強(qiáng)的引力,避免了TNRP的發(fā)生。仿真結(jié)果表明,IAPF算法可以有效避免TNRP的發(fā)生,保證路徑規(guī)劃的成功率。
為了驗(yàn)證IAPF算法能夠避免陷入LMT問題,設(shè)計(jì)了如下仿真實(shí)驗(yàn):當(dāng)目標(biāo)與初始位置之間的直線上存在障礙物時(shí),容易陷入LMT,因此在這種情況下,APF和IAPF算法同時(shí)進(jìn)行路徑規(guī)劃。模擬區(qū)域?yàn)橐粋€(gè)10×10的矩形,具體參數(shù)見表3,各障礙物的動(dòng)作閾值相等,仿真結(jié)果如圖11所示,仿真曲線如圖12所示。
表3 第3.3節(jié)仿真使用的具體參數(shù)
圖11 LMT的仿真結(jié)果
圖12 LMT的仿真曲線
從仿真結(jié)果可以看出:APF迅速陷入LMT,并一直保持這種狀態(tài),直到路徑規(guī)劃失敗;IAPF算法沒有落入LMT,并迅速到達(dá)目標(biāo)位置;IAPF算法在第22次迭代附近到達(dá)目標(biāo)位置,而APF仍然停留在LMT中。仿真結(jié)果表明,IAPF算法能夠有效克服LMT問題,保證路徑規(guī)劃的成功率。
采用APF和IAPF算法對(duì)機(jī)械臂進(jìn)行路徑規(guī)劃。實(shí)驗(yàn)參數(shù)如表4所示,其中初始構(gòu)型為Θi,目標(biāo)構(gòu)型為Θg,障礙物坐標(biāo)為POi,障礙物半徑均為120 mm。仿真結(jié)果如表5所示,機(jī)械手各初等變換序列線段與障礙物的關(guān)節(jié)角曲線和距離曲線如圖13所示。
表4 第3.4節(jié)仿真使用的具體參數(shù)
表5 IAPF和APF算法的路徑規(guī)劃結(jié)果
圖13 采摘機(jī)械手運(yùn)動(dòng)規(guī)劃的實(shí)驗(yàn)結(jié)果
仿真結(jié)果表明:IAPF算法優(yōu)于APF算法。IAPF算法的關(guān)節(jié)角曲線更平滑,變化率幾乎相同;APF算法的關(guān)節(jié)角曲線變化比較明顯,變化率相差較大,關(guān)節(jié)角θ3經(jīng)過一個(gè)反向過程后開始收斂,而關(guān)節(jié)角θ2則出現(xiàn)波峰。APF算法的收斂速度非常慢,IAPF算法在第238次直接到達(dá)目標(biāo)構(gòu)型,而APF在第549次才收斂。與APF算法相比,IAPF算法在運(yùn)行時(shí)間上減少了55.12%,在總關(guān)節(jié)角誤差上減少了46.21%。
為進(jìn)一步驗(yàn)證IAPF算法的有效性和可行性,對(duì)智能采摘機(jī)器人進(jìn)行了運(yùn)動(dòng)規(guī)劃實(shí)驗(yàn),實(shí)驗(yàn)結(jié)果如表6所示。目標(biāo)果樹及其簡(jiǎn)化模型如圖14所示,首先用部分球面包絡(luò)(PSE)方法對(duì)目標(biāo)果樹進(jìn)行建模,該方法類似于一個(gè)包圍球算法[15-16]。PSE方法僅包含球面和線段模型,可以大大減少碰撞檢測(cè)過程中的計(jì)算量,進(jìn)一步保證IAPF算法能夠滿足智能采摘機(jī)器人的實(shí)時(shí)性要求。PSE通過球體包絡(luò)果樹的所有非采摘部分,通過直線分段對(duì)當(dāng)前采摘部分的分支進(jìn)行建模,智能采摘機(jī)器人的采摘模擬和軌跡曲線如圖15所示,應(yīng)用于智能采摘機(jī)器人的IAPF算法實(shí)驗(yàn)結(jié)果如圖16所示。
表6 IAPF應(yīng)用于智能采摘機(jī)器人的模擬實(shí)驗(yàn)結(jié)果
圖14 目標(biāo)果樹(a)及其PSE模型(b)
圖15 智能采摘機(jī)器人采摘仿真時(shí)的軌跡曲線
圖16 應(yīng)用于智能采摘機(jī)器人的IAPF實(shí)驗(yàn)結(jié)果
IAPF算法在智能采摘機(jī)器人采摘目標(biāo)果實(shí)的路徑規(guī)劃過程中分別運(yùn)動(dòng)0.38、1.08、1.11 s,在采摘、釋放和初始化階段的關(guān)節(jié)總誤差分別為1.29°、1.08°和1.21°。智能采摘機(jī)器人規(guī)劃果實(shí)采摘路徑耗時(shí)約2.56 s,由于機(jī)械手運(yùn)動(dòng)所花費(fèi)的時(shí)間比路徑規(guī)劃所花費(fèi)的時(shí)間要長(zhǎng)得多,因此IAPF算法可以在智能采摘機(jī)器人采摘過程中充分規(guī)劃下一個(gè)果實(shí)采摘路徑。從實(shí)驗(yàn)結(jié)果可以得出結(jié)論:IAPF算法具有較快的求解速度和精度,能夠滿足智能采摘機(jī)器人的實(shí)時(shí)性要求。
針對(duì)六連桿采摘機(jī)械手的無碰撞運(yùn)動(dòng)規(guī)劃,提出了改進(jìn)的人工勢(shì)場(chǎng)(IAPF)優(yōu)化算法,得出的主要結(jié)論如下:
(1)采用初等變換序列方法對(duì)機(jī)械手進(jìn)行運(yùn)動(dòng)學(xué)建模,該模型可方便地得到機(jī)械手上各點(diǎn)相對(duì)于基底坐標(biāo)系的雅可比矩陣。
(2)IAPF算法在APF算法原有的引力勢(shì)場(chǎng)計(jì)算模型的基礎(chǔ)上加入了Sigmoid函數(shù),使得路徑規(guī)劃算法收斂;在斥力勢(shì)場(chǎng)中引入了反S的Sigmoid函數(shù),防止出現(xiàn)斥力爆炸現(xiàn)象,有效地避免了局部最小缺陷問題和目標(biāo)位置不可達(dá)問題。
(3)與傳統(tǒng)的APF算法相比,IAPF算法的計(jì)算時(shí)間和關(guān)節(jié)總誤差分別減少了55.12%和46.21%。最后通過實(shí)驗(yàn)結(jié)果,驗(yàn)證了所提優(yōu)化算法在采摘機(jī)械手無碰撞運(yùn)動(dòng)規(guī)劃中的實(shí)時(shí)性和準(zhǔn)確性。