工廠化循環(huán)水養(yǎng)殖魚(yú)池清刷機(jī)器人的定位精度分析

胡勇兵，倪 琦，黃 達(dá)，李金剛

(1 上海海洋大學(xué)工程學(xué)院，上海 201306；2中國(guó)水產(chǎn)科學(xué)研究院漁業(yè)機(jī)械儀器研究所，上海 200092)

工廠化循環(huán)水養(yǎng)殖作為工業(yè)化程度最高的一種先進(jìn)水產(chǎn)養(yǎng)殖模式，雖在國(guó)內(nèi)起步較晚，但無(wú)論是技術(shù)還是在產(chǎn)業(yè)應(yīng)用上均取得了長(zhǎng)足進(jìn)步[1-3]。工廠化循環(huán)水養(yǎng)殖的魚(yú)池規(guī)格一般有邊長(zhǎng)為3.75 m的方切角池和直徑6.4 m的圓形池等，其材質(zhì)一般為玻璃鋼和混凝土。傳統(tǒng)清洗方式需要人工手持板刷或者拖著水槍機(jī)四周環(huán)繞清刷，工作煩瑣重復(fù)且池底易滑。為了節(jié)約人力成本和安全考慮，需要研發(fā)養(yǎng)殖過(guò)程中進(jìn)行自動(dòng)清洗池底的智能化機(jī)器人[4-7]。由于工廠化循環(huán)水養(yǎng)殖車(chē)間不同于室外環(huán)境，無(wú)法采用全球定位系統(tǒng)(GPS)定位策略，機(jī)器人移動(dòng)導(dǎo)航和路徑規(guī)劃清掃難度也相應(yīng)增加，因此實(shí)現(xiàn)機(jī)器人的精準(zhǔn)定位是魚(yú)池機(jī)器人具有自主運(yùn)動(dòng)的關(guān)鍵環(huán)節(jié)。其中，SLAM(Simultaneous Localization and Mapping)同步定位和地圖構(gòu)建技術(shù)[8-10]是指機(jī)器人配置傳感器提取環(huán)境信息，在移動(dòng)過(guò)程中完成環(huán)境地圖的構(gòu)建并不斷修正自身位姿實(shí)現(xiàn)定位的過(guò)程，是實(shí)現(xiàn)機(jī)器人定位導(dǎo)航的關(guān)鍵技術(shù)，這對(duì)機(jī)器人的運(yùn)動(dòng)控制和路徑規(guī)劃任務(wù)完成具有重要意義。

目前室內(nèi)移動(dòng)機(jī)器人定位[11-12]研究一直是熱點(diǎn)話(huà)題，壽佳鑫等[13]利用激光雷達(dá)實(shí)現(xiàn)了室內(nèi)移動(dòng)機(jī)器人的定位和導(dǎo)航；侯家林等[14]研制了基于雙激光雷達(dá)的溫室機(jī)器人導(dǎo)航系統(tǒng)；水下環(huán)境復(fù)雜，不像陸地上方便控制和觀測(cè)，Wu等[15]闡述了水下機(jī)器人定位導(dǎo)航的方法以及李渝[16]提出了利用聲吶來(lái)提取環(huán)境特征的SLAM算法研究；其中SLAM是實(shí)現(xiàn)機(jī)器人定位導(dǎo)航的關(guān)鍵技術(shù)，從傳感器輸入信號(hào)角度出發(fā)，SLAM技術(shù)分為基于距離傳感器和基于視覺(jué)傳感器兩大類(lèi)[17]；早期Grisetti等[18]提出了改進(jìn)的Rao-Blackwellized粒子過(guò)濾器的Gmapping算法利用RB粒子濾波對(duì)匹配進(jìn)行優(yōu)化；再到Kohlbrecher等[19]提出的Hector SLAM利用高斯牛頓法進(jìn)行掃描匹配；近年Hess等[20]發(fā)表的Catographer在局部范圍內(nèi)掃描匹配實(shí)現(xiàn)了回環(huán)檢測(cè)及全局優(yōu)化；蒙特卡羅定位(Monte Carlo Localization，MCL)算法最早由 Dellaert等[21]提出，主要是利用粒子濾波結(jié)合移動(dòng)機(jī)器人傳感器和里程計(jì)進(jìn)行定位，SLAM技術(shù)為機(jī)器人建圖和定位研究提供了理論基礎(chǔ)。

但是，針對(duì)養(yǎng)殖車(chē)間魚(yú)池機(jī)器人的定位技術(shù)的研究還比較少。根據(jù)以上背景和國(guó)內(nèi)外對(duì)于機(jī)器人定位技術(shù)的研究發(fā)展分析，結(jié)合工廠化循環(huán)水養(yǎng)殖車(chē)間的魚(yú)池環(huán)境，設(shè)計(jì)了一款能夠?qū)崿F(xiàn)地圖構(gòu)建和定位的魚(yú)池清刷機(jī)器人。

1 材料與方法

1.1 系統(tǒng)組成與工作原理

魚(yú)池清刷機(jī)器人用于養(yǎng)殖車(chē)間的魚(yú)池池底污染物清理，主要由機(jī)器人車(chē)體、驅(qū)動(dòng)機(jī)構(gòu)、清掃機(jī)構(gòu)和激光雷達(dá)傳感器等組成(圖1)。魚(yú)池清刷機(jī)器人搭載傳感器提取魚(yú)池環(huán)境特征，該傳感器支持12 m半徑測(cè)量范圍，通過(guò)測(cè)量反射光的運(yùn)行時(shí)間而確定目標(biāo)的距離，連續(xù)360°掃描測(cè)距收集環(huán)境中目標(biāo)點(diǎn)的數(shù)據(jù)信息，同時(shí)機(jī)器人攜帶的慣性傳感器獲取姿態(tài)、速度和方向信息，通過(guò)串口輸出數(shù)據(jù)到電腦(PC)端。在機(jī)器人操作系統(tǒng)(Robot Operating System,ROS)平臺(tái)上進(jìn)行數(shù)據(jù)信息的處理和同步仿真機(jī)器人的位姿狀態(tài)，實(shí)現(xiàn)即時(shí)構(gòu)建地圖和定位功能。

圖1 魚(yú)池清刷機(jī)器人組成結(jié)構(gòu)圖

1.2 試驗(yàn)設(shè)計(jì)與方法

1.2.1 試驗(yàn)條件

試驗(yàn)于2020年9—10月在中國(guó)水產(chǎn)科學(xué)研究院漁業(yè)機(jī)械儀器研究所如東試驗(yàn)基地的養(yǎng)殖車(chē)間里進(jìn)行(圖2)，選擇的區(qū)域?yàn)殚L(zhǎng)3.75 m 、寬3.75 m的無(wú)水方切角形養(yǎng)殖魚(yú)池，墻壁高度為1.5 m，池底坡度有8°～10°，面積約為12.9 m2。對(duì)照組為平整的地面，沒(méi)有坡度，高度為0.5 m，形狀尺寸規(guī)格同魚(yú)池一致。

圖2 魚(yú)池清刷機(jī)器人樣機(jī)試驗(yàn)

1.2.2 試驗(yàn)設(shè)計(jì)與測(cè)定指標(biāo)

試驗(yàn)主要研究不同算法、魚(yú)池坡度以及機(jī)器人移動(dòng)速度等對(duì)定位精度的影響。首先實(shí)地勘測(cè)魚(yú)池環(huán)境數(shù)據(jù)，在ROS平臺(tái)的gazebo插件里搭建魚(yú)池世界模型如圖3(a)所示?？紤]到魚(yú)池的面積和傳感器的精度，加載安裝好的導(dǎo)航slam包，利用Gmapping算法進(jìn)行實(shí)時(shí)仿真建圖。選擇上述試驗(yàn)環(huán)境，將機(jī)器人隨機(jī)放置在魚(yú)池任意位置。機(jī)器人上電后驅(qū)動(dòng)電機(jī)和傳感器正常運(yùn)行，同時(shí)與PC端的ROS形成通信機(jī)制。通過(guò)機(jī)器人操作系統(tǒng)來(lái)控制機(jī)器人的行走速度，保持接收傳感器發(fā)來(lái)的數(shù)據(jù)信息，在RVIZ插件里即時(shí)反映魚(yú)池環(huán)境，結(jié)果如圖3(b)所示。在建圖過(guò)程中，控制機(jī)器人以0.1 m/s的速度移動(dòng)。

圖3 魚(yú)池環(huán)境仿真和實(shí)時(shí)建圖

在地圖已知條件下，分別采用自適應(yīng)蒙特卡羅定位(Adaptive Monte Carlo Localization,AMCL)算法和慣性里程計(jì)方法進(jìn)行定位試驗(yàn)，試驗(yàn)環(huán)境相同，傳感器能360°全方位檢測(cè)。自適應(yīng)蒙特卡羅定位[22-23]是MCL的擴(kuò)展形式，在構(gòu)建的地圖中采用粒子來(lái)代表機(jī)器人的位姿，在重采樣階段自適應(yīng)調(diào)整粒子數(shù)量以達(dá)到更準(zhǔn)確的定位。慣性里程計(jì)方法是基于里程計(jì)的航跡推算方法進(jìn)行定位，在機(jī)器人位姿初始值給定的前提下，基于內(nèi)部傳感器信息計(jì)算出每一時(shí)刻位姿相對(duì)于上一時(shí)刻位姿的距離以及方向角的變化，從而實(shí)現(xiàn)位姿的實(shí)時(shí)估計(jì)。在魚(yú)池中隨機(jī)選取10個(gè)位置，與上述ROS平臺(tái)上構(gòu)建的環(huán)境地圖進(jìn)行坐標(biāo)匹配，并在池底貼上標(biāo)記。開(kāi)始時(shí)機(jī)器人放置在魚(yú)池起始位置，在rviz界面適當(dāng)調(diào)整機(jī)器人初始狀態(tài)，在PC端顯示的地圖中隨機(jī)選取一個(gè)定位目標(biāo)點(diǎn)進(jìn)行導(dǎo)航移動(dòng)，待機(jī)器人停穩(wěn)后測(cè)量記錄下實(shí)際位姿誤差數(shù)據(jù)。采用控制變量策略，分別在有坡度的魚(yú)池中和平整的對(duì)照組場(chǎng)地，在考慮移動(dòng)速度和障礙物的情況下進(jìn)行定位試驗(yàn)。

根據(jù)PC端構(gòu)建的地圖和魚(yú)池真實(shí)環(huán)境，以機(jī)器人當(dāng)前到達(dá)的位置為中心，并從朝向角和位置坐標(biāo)點(diǎn)兩方面進(jìn)行比較評(píng)價(jià)。a)角度誤差：機(jī)器人到達(dá)位置的朝向與PC端預(yù)定朝向之間的角度差；b)距離誤差：機(jī)器人到達(dá)位置的坐標(biāo)點(diǎn)與PC端預(yù)定位置的坐標(biāo)點(diǎn)之間的距離差。

1.2.3 軟件操作平臺(tái)與統(tǒng)計(jì)分析

所用電腦配置為:CPU為i5-8250U，主頻為1.8 GHz，內(nèi)存8 G，系統(tǒng)為 Ubuntu18.08，ROS版本為melodic。在實(shí)際測(cè)試中將傳感器搭載在魚(yú)池清刷機(jī)器人上進(jìn)行定位試驗(yàn)，在ROS仿真平臺(tái)的gazebo插件里搭建魚(yú)池環(huán)境模型，在rviz插件里即時(shí)反映地圖。試驗(yàn)數(shù)據(jù)用“平均值”表示，無(wú)顯著性差異(P>0.05),用Excel軟件對(duì)所有數(shù)據(jù)進(jìn)行計(jì)算及繪制圖表工作。

2 結(jié)果與分析

2.1 不同算法在魚(yú)池定位中的比較

機(jī)器人在移動(dòng)導(dǎo)航過(guò)程中，算法是影響定位的關(guān)鍵因素之一。本試驗(yàn)采用傳統(tǒng)慣性里程計(jì)和AMCL兩種方法，保持其他因素與基礎(chǔ)試驗(yàn)組一致，進(jìn)行不同算法對(duì)定位效果的試驗(yàn)研究。結(jié)果如圖4所示，在選擇慣性里程計(jì)的方法定位時(shí)，平均距離誤差為10.67 cm，平均角度誤差為6.8°；選擇自適應(yīng)蒙特卡羅全局定位算法時(shí)，平均距離誤差為9.02 cm，平均角度誤差為4.6°。由試驗(yàn)數(shù)據(jù)可知，AMCL較慣性里程計(jì)平均距離誤差減少1.65 cm,平均角度誤差減少2.2°，AMCL算法定位精準(zhǔn)度更高。

圖4 不同算法下的定位誤差

2.2 魚(yú)池坡度對(duì)定位精度的影響

為了方便魚(yú)池污垢集中向中心排污口排出，工廠化循環(huán)水養(yǎng)殖車(chē)間魚(yú)池池底都會(huì)有坡度存在。本試驗(yàn)將選擇坡度為8°～10°的方切角魚(yú)池，同時(shí)安排對(duì)照組進(jìn)行定位效果的比較試驗(yàn)，保持其他因素與基礎(chǔ)試驗(yàn)組一致。結(jié)果如圖5所示，在有坡度的方切角魚(yú)池中，平均距離誤差為9.02 cm，平均角度誤差為4.6°；在無(wú)坡度的對(duì)照組中，平均距離誤差為6.29 cm，平均角度誤差為3.9°。由試驗(yàn)數(shù)據(jù)可知，有坡度明顯比對(duì)照組的定位誤差更大(P<0.05)，且各位置之間定位誤差更散亂，因此坡度會(huì)影響到定位性能。

圖5 有無(wú)坡度情況下的定位誤差

2.3 移動(dòng)速度對(duì)定位精度的影響

本試驗(yàn)將魚(yú)池清潔機(jī)器人的移動(dòng)速度分別設(shè)置成0.1 m/s、0.2 m/s和0.3 m/s三種速度，保持其他因素與基礎(chǔ)試驗(yàn)組一致，分別進(jìn)行目標(biāo)位置點(diǎn)的定位試驗(yàn)。結(jié)果如圖6所示，在移動(dòng)速度為0.1 m/s時(shí)，平均距離誤差為9.02 cm，平均角度誤差為4.6°；在移動(dòng)速度為0.2 m/s時(shí)，平均距離誤差為9.65 cm，平均角度誤差為4.3°；在移動(dòng)速度為0.3 m/s時(shí)，平均距離誤差為9.77 cm，平均角度誤差為5°；由試驗(yàn)數(shù)據(jù)可知，速度對(duì)于定位精度沒(méi)有顯著影響(P>0.05)。

圖6 不同速度下的定位誤差

2.4 障礙物對(duì)定位精度的影響

在環(huán)境地圖已知的條件下，移動(dòng)定位過(guò)程中隨機(jī)添加小型靜態(tài)障礙物，測(cè)試障礙物的存在對(duì)定位精度的影響，保持其他因素與基礎(chǔ)試驗(yàn)組一致。有無(wú)障礙物情況下的定位誤差結(jié)果如圖7所示。

圖7 有無(wú)障礙物情況下的定位誤差

在有障礙物的情況下，平均距離誤差為9.61 cm，平均角度誤差為5.7°；在無(wú)障礙物的情況下，平均距離誤差為9.02 cm，平均角度誤差為4.6°；由試驗(yàn)數(shù)據(jù)可知，障礙物對(duì)于定位目標(biāo)點(diǎn)位置影響不大(P>0.05)，但朝向角誤差會(huì)出現(xiàn)偏大情況。

3 討論

3.1 機(jī)器人定位算法的探討

室內(nèi)移動(dòng)機(jī)器人擁有穩(wěn)定可靠的定位是實(shí)現(xiàn)自主運(yùn)動(dòng)的關(guān)鍵環(huán)節(jié)[24]。在室外環(huán)境下，GPS導(dǎo)航系統(tǒng)是解決定位問(wèn)題的有效途徑[25]。一旦進(jìn)入室內(nèi)無(wú)線電波信號(hào)會(huì)非常差甚至失去，無(wú)法在室內(nèi)正常使用，室內(nèi)定位需要尋找其他解決方法?，F(xiàn)階段室內(nèi)移動(dòng)機(jī)器人定位主要是依據(jù)環(huán)境地圖和給定的傳感器獲取環(huán)境信息，確定自身在環(huán)境地圖中的位姿[11]。以激光雷達(dá)和里程計(jì)等為傳感器的2D激光SLAM技術(shù)較為成熟[26]，已經(jīng)應(yīng)用于多種工作場(chǎng)景，例如AGV(Automated Guided Vehicle)自動(dòng)導(dǎo)航車(chē)等。目前視覺(jué)SLAM具有很大的發(fā)展空間，應(yīng)用于低速的產(chǎn)品如清潔機(jī)器人等，徐曉蘇等[27]研究了室內(nèi)環(huán)境下視覺(jué)SLAM的定位效果。由于Hector SLAM對(duì)傳感器的要求比較高，需具備高更新頻率且測(cè)量噪聲小的激光掃描儀。Catographer是基于圖優(yōu)化的方法，需要復(fù)雜的矩陣運(yùn)算，計(jì)算量和內(nèi)存需求大。而視覺(jué)SLAM中環(huán)境光對(duì)于視覺(jué)傳感器影響大，暗處或者渾濁的區(qū)域無(wú)法工作?？紤]到魚(yú)池環(huán)境的面積、觀測(cè)傳感器的精度和適用范圍，本研究采用Gmapping算法和激光雷達(dá)來(lái)實(shí)現(xiàn)地圖構(gòu)建。傳統(tǒng)慣性里程計(jì)方法需要基于航跡推算確定機(jī)器人位置，由于里程計(jì)的累計(jì)誤差會(huì)導(dǎo)致定位誤差逐漸變大，影響定位精度。采用基本的蒙特卡羅算法時(shí)，小概率假設(shè)會(huì)因粒子貧乏而丟失導(dǎo)致定位失敗。AMCL[22]能夠解決機(jī)器人“綁架問(wèn)題”(突然變換位置)，克服粒子數(shù)貧乏和固定的問(wèn)題，在運(yùn)動(dòng)模型中使用KLD(Kullback-Leibler Divergence)對(duì)粒子進(jìn)行重新采樣。本研究在已知環(huán)境地圖下采用AMCL算法，在重采樣階段優(yōu)化粒子數(shù)量提高機(jī)器人定位效果。由于目前魚(yú)池機(jī)器人搭載的是激光雷達(dá)觀測(cè)傳感器，發(fā)射出的光信號(hào)在水中容易被散射吸收，無(wú)法準(zhǔn)確探測(cè)到物體，現(xiàn)階段只是在無(wú)水池底下進(jìn)行試驗(yàn)，在未來(lái)的工作中水下SLAM技術(shù)是重點(diǎn)研究方向。

3.2 魚(yú)池清刷機(jī)器人定位試驗(yàn)效果

本試驗(yàn)的影響因素從魚(yú)池環(huán)境和機(jī)器人本體兩部分進(jìn)行分析。除了定位技術(shù)的優(yōu)化選擇是影響室內(nèi)機(jī)器人的主要因素外，魚(yú)池坡度和機(jī)器人系統(tǒng)也是造成定位誤差的重要原因。本試驗(yàn)結(jié)果表明，相比較于傳統(tǒng)慣性里程計(jì)的方法，采用AMCL算法能夠有效提高定位精度。有坡度的魚(yú)池平均定位誤差會(huì)增加2.73 cm,平均角度誤差增加0.7°，魚(yú)池坡度對(duì)定位精度影響較大。魚(yú)池清刷機(jī)器人在0.1 m/s、0.2 m/s和0.3 m/s三種速度情況下，平均定位誤差差距不大，但在建圖過(guò)程中用低速移動(dòng)，建圖效果更好，環(huán)境特征覆蓋更全面。已知環(huán)境地圖下，添加障礙物對(duì)于定位精度影響不大，因?yàn)锳MCL算法在全局定位的同時(shí)，也會(huì)有局部規(guī)劃器，能夠?qū)崟r(shí)發(fā)現(xiàn)障礙物的情況，但朝向角可能會(huì)出現(xiàn)較大誤差，有時(shí)陷入局部死點(diǎn)。根據(jù)誤差規(guī)律將定位誤差分為隨機(jī)誤差和系統(tǒng)誤差，隨機(jī)誤差是由試驗(yàn)對(duì)象與環(huán)境的相互作用引起的，葉敏等[28]對(duì)采摘機(jī)器人定位誤差的研究中得出結(jié)論，環(huán)境下的隨機(jī)誤差是造成定位不準(zhǔn)確的主要原因。本試驗(yàn)中魚(yú)池存在坡度，池底會(huì)打滑或有阻礙機(jī)器人旋轉(zhuǎn)的障礙物。針對(duì)地形問(wèn)題，本研究中證實(shí)了坡度等環(huán)境因素會(huì)造成定位誤差，但目前尚缺乏地形匹配模型來(lái)修正坡度對(duì)定位精度的影響。郭磊明[29]提出了一種利用地形坡度特征來(lái)確定機(jī)器人空間位置的方法，使用機(jī)器人地形傾斜模型提取機(jī)器人位姿信息實(shí)現(xiàn)機(jī)器人定位。后續(xù)研究中會(huì)在構(gòu)建的地圖模型中加入坡度特征，形成立體地圖來(lái)修正機(jī)器人的位置。系統(tǒng)誤差主要是由于系統(tǒng)設(shè)計(jì)的不完整和不可預(yù)期的機(jī)械特性引起，Borenstein等[30]經(jīng)過(guò)長(zhǎng)時(shí)間的實(shí)驗(yàn)發(fā)現(xiàn)里程計(jì)的系統(tǒng)誤差來(lái)源主要是兩驅(qū)動(dòng)輪輪距的誤差和輪徑不相等。達(dá)興鵬等[31]提出了一種里程計(jì)系統(tǒng)誤差及激光雷達(dá)安裝誤差迭代標(biāo)定方法，實(shí)現(xiàn)定位誤差大幅度降低。里程計(jì)的累計(jì)誤差不可避免，隨著機(jī)器人的移動(dòng)距離越遠(yuǎn)，轉(zhuǎn)向動(dòng)作越多，定位誤差也越大。為了盡量減少系統(tǒng)誤差，定位試驗(yàn)前都會(huì)檢查機(jī)器人本身性能以及正確調(diào)整驅(qū)動(dòng)機(jī)構(gòu)和激光雷達(dá)，并且進(jìn)行多次里程計(jì)校正準(zhǔn)備試驗(yàn)。在這些影響因素下，基礎(chǔ)試驗(yàn)組定位精度達(dá)到預(yù)期，基本滿(mǎn)足試驗(yàn)要求。但是該機(jī)器人也存在一定問(wèn)題，傳感器的選擇、里程計(jì)誤差的校正和機(jī)器人系統(tǒng)結(jié)構(gòu)的優(yōu)化等需要進(jìn)一步調(diào)試改進(jìn)。

4 結(jié)論

Gmapping算法構(gòu)建小場(chǎng)景地圖時(shí)計(jì)算量小且精度高，能夠真實(shí)反映魚(yú)池環(huán)境。通過(guò)不同因素定位試驗(yàn)分析，在基礎(chǔ)試驗(yàn)組條件下采用自適應(yīng)蒙特卡羅定位算法，目標(biāo)點(diǎn)定位平均距離誤差為9.02 cm，平均角度誤差為4.6°，機(jī)器人能夠有效估計(jì)自身位姿，同時(shí)也能完成導(dǎo)航任務(wù)，為路徑規(guī)劃清掃任務(wù)提供參考。現(xiàn)階段設(shè)計(jì)的魚(yú)池清刷機(jī)器人定位試驗(yàn)主要在方切角形魚(yú)池里展開(kāi)，在后續(xù)研究中增加魚(yú)池環(huán)境的復(fù)雜度，增強(qiáng)機(jī)器人的通用性和實(shí)際性，以期進(jìn)一步提高魚(yú)池清刷機(jī)器人的定位性能。

□