基于農(nóng)業(yè)倉(cāng)庫(kù)物流搬運(yùn)機(jī)器人控制算法分析與研究

霍桂利，王曉亮

（1.山西建筑職業(yè)技術(shù)學(xué)院基礎(chǔ)教學(xué)部，山西晉中030600；2.山西農(nóng)業(yè)大學(xué)工學(xué)院，山西晉中030801）

隨著傳感器技術(shù)、伺服驅(qū)動(dòng)技術(shù)的迅猛發(fā)展，高性能實(shí)時(shí)控制已經(jīng)可以滿足工業(yè)控制領(lǐng)域的相關(guān)需求。以此為依托，移動(dòng)機(jī)器人也逐漸可滿足現(xiàn)代化智能數(shù)字工廠中重載部件定位搬運(yùn)的要求。隨著移動(dòng)機(jī)器人性能的不斷完善，在農(nóng)業(yè)工程領(lǐng)域得到了良好的應(yīng)用。如何在更加復(fù)雜、邊界、極端的環(huán)境下解決導(dǎo)航控制問(wèn)題，使得移動(dòng)機(jī)器人更好地服務(wù)于生產(chǎn)與生活，已成為智能移動(dòng)機(jī)器人技術(shù)領(lǐng)域的新研究課題。

移動(dòng)機(jī)器人導(dǎo)航技術(shù)在學(xué)術(shù)界中的研究成果已經(jīng)相對(duì)成熟，例如：文獻(xiàn)［1］提出了一種新穎的方法，規(guī)劃移動(dòng)機(jī)器人通往未知環(huán)境中的目標(biāo)的路徑，使用傳感器發(fā)現(xiàn)沿途的新障礙物。該方法通過(guò)數(shù)值求解熱傳導(dǎo)偏微分方程，在整個(gè)已知環(huán)境中合成人工溫度梯度，其中在機(jī)器人導(dǎo)航期間遇到的“熱”且目標(biāo)是“冷的”。在已知環(huán)境網(wǎng)格上的所有其他點(diǎn)處的溫度被數(shù)值計(jì)算，并且連續(xù)地更新以考慮新的障礙。文獻(xiàn)［2］提出了具有異質(zhì)能力的多機(jī)器人的導(dǎo)航方法。在這種方法中，單個(gè)導(dǎo)航具有不同的平移和旋轉(zhuǎn)速度、加速度，感測(cè)距離和角度，同時(shí)保持全局連接其他機(jī)器人。文獻(xiàn)［3］使用無(wú)監(jiān)督聚類(lèi)，基于導(dǎo)航復(fù)雜性自動(dòng)檢測(cè)周?chē)h(huán)境的類(lèi)型，并限制本地控制器的采樣空間。上述文獻(xiàn)中所述方法，均依賴于激光對(duì)移動(dòng)機(jī)器人進(jìn)行導(dǎo)航控制，在狹窄環(huán)境中機(jī)器人的運(yùn)動(dòng)極易被障礙物所阻擋，導(dǎo)致機(jī)器人停留在原地，機(jī)器人運(yùn)動(dòng)控制不夠靈活。

本文通過(guò)對(duì)激光雷達(dá)數(shù)據(jù)建模，進(jìn)而采用幾何特征提取的方法，實(shí)現(xiàn)地圖的創(chuàng)建以及機(jī)器人的匹配定位，并通過(guò)激光與聲吶數(shù)據(jù)融合的手段，通過(guò)感知中遠(yuǎn)距離以及微距下障礙信息，以模糊邏輯處理的方式，篩選機(jī)器人左右輪速輸出的最優(yōu)規(guī)則，實(shí)現(xiàn)機(jī)器人避障行走，所述算法的有效性通過(guò)了機(jī)器人仿真平臺(tái)的驗(yàn)證。

1 物流移動(dòng)機(jī)器人控制平臺(tái)

1.1 物流移動(dòng)機(jī)器人控制平臺(tái)架構(gòu)

按照傳統(tǒng)自動(dòng)控制系統(tǒng)的定義，系統(tǒng)的主要組件可分為控制器、傳感器以及執(zhí)行器3大部分，現(xiàn)代智能移動(dòng)機(jī)器人摒棄了陳舊的控制系統(tǒng)結(jié)構(gòu)，基于網(wǎng)絡(luò)控制系統(tǒng)（Network Control System，NCS）實(shí)現(xiàn)執(zhí)行器群、傳感器群以及總控系統(tǒng)間的數(shù)據(jù)傳輸以及集中控制［4］。

移動(dòng)機(jī)器人的控制系統(tǒng)架構(gòu)為：控制平臺(tái)通過(guò)ECAT總線建立硬實(shí)時(shí)環(huán)境，NCS中提供多種硬件接口（I2C、PCI-E、USB以及RJ45等），保障了多種傳感器與控制核心的實(shí)時(shí)通訊。NCS分為輸入、輸出雙通道，多種傳感器的反饋數(shù)據(jù)與伺服驅(qū)動(dòng)電機(jī)的控制信號(hào)并行于控制網(wǎng)絡(luò)內(nèi)部［5］。移動(dòng)機(jī)器人運(yùn)動(dòng)的過(guò)程中，通過(guò)激光傳感器完成環(huán)境中的偽定位，輔以電機(jī)位置傳感器來(lái)記錄機(jī)器人的里程信息，根據(jù)當(dāng)前的路徑、運(yùn)動(dòng)方向等信息，可實(shí)現(xiàn)最優(yōu)路徑的規(guī)劃以及地圖信息的匹配，最終實(shí)現(xiàn)響應(yīng)迅速、控制靈活的導(dǎo)航。

2.2 雙輪差動(dòng)控制

移動(dòng)機(jī)器人采用雙輪差動(dòng)方式運(yùn)動(dòng)，輪速分別為Vl、Vr，車(chē)寬為d，運(yùn)動(dòng)軌跡圓半徑為r，在Vl≤Vr的狀態(tài)下，差速所形成的運(yùn)動(dòng)軌跡分析如下：

推導(dǎo)為

當(dāng)Vl＞Vr時(shí)，情況與此類(lèi)似，則公式變?yōu)閞=r趨向正負(fù)無(wú)窮情況下，車(chē)運(yùn)動(dòng)軌跡為直線；r=0時(shí)，車(chē)運(yùn)動(dòng)軌跡為原地旋轉(zhuǎn)；r＞0時(shí)，車(chē)體運(yùn)動(dòng)軌跡為向左側(cè)旋轉(zhuǎn)；r＜0時(shí)，車(chē)體運(yùn)動(dòng)軌跡為向右側(cè)旋轉(zhuǎn)。

此公式用于理想情況下的機(jī)器人運(yùn)動(dòng)軌跡估算，但在實(shí)際應(yīng)用時(shí)，還有機(jī)器人運(yùn)動(dòng)左右輪轉(zhuǎn)動(dòng)方向的摩擦力、機(jī)器人左右輪裝配加工的誤差、機(jī)器人軸向受力時(shí)，機(jī)器人軸向被擠壓；時(shí)，機(jī)器人軸向被拉伸；只有當(dāng)時(shí)，機(jī)器人軸向力會(huì)消失）、機(jī)器人質(zhì)量分布不均勻（導(dǎo)致機(jī)器人左右輪所受摩擦力不同）、溫濕度等因素的影響（材質(zhì)摩擦系數(shù)變化），所以需要根據(jù)實(shí)際情況調(diào)整Vl和Vr的值，并非是理論給定的輪速。

3 導(dǎo)航定位

3.1 激光雷達(dá)數(shù)學(xué)模型

通過(guò)激光雷達(dá)按照一定的角度分辨率對(duì)機(jī)器人的工作環(huán)境進(jìn)行掃描，則各掃描點(diǎn)的平面直角坐標(biāo)為

式中：ρn為掃描點(diǎn)距離激光中心的距離。

在實(shí)際工況中，激光數(shù)據(jù)受到高斯白噪聲的干擾，且每組測(cè)量值相互獨(dú)立。令?u為噪聲us的雅可比矩陣分別為掃描距離與角度的方差，則掃描數(shù)據(jù)的協(xié)方差為

將激光雷達(dá)獲取的數(shù)據(jù)轉(zhuǎn)換至世界坐標(biāo)系中，并令激光掃描點(diǎn)的位姿為Ps=(xs，ys，θs)，傳感器坐標(biāo)下的數(shù)據(jù)為則其在機(jī)器人坐標(biāo)系下的位姿為

其中，

3.2 自主建圖

在地圖創(chuàng)建過(guò)程中，機(jī)器人每一時(shí)刻的相對(duì)位姿可由機(jī)器人起始運(yùn)動(dòng)處（基坐標(biāo)）的原點(diǎn)根據(jù)機(jī)器人模型參數(shù)求解，此過(guò)程稱為偽定位。將激光掃描的臨近點(diǎn)經(jīng)過(guò)特征提取后，進(jìn)行擬合，將獲取的環(huán)境特征映射至基坐標(biāo)可得到周?chē)h(huán)境中所有障礙相對(duì)于基坐標(biāo)的位置以及方向，將全部特征數(shù)據(jù)統(tǒng)一進(jìn)行存儲(chǔ)并記錄所有特征片段相對(duì)于坐標(biāo)原點(diǎn)的距離以及夾角，即可完成地圖的創(chuàng)建。

移動(dòng)機(jī)器人在地圖創(chuàng)建后，可通過(guò)對(duì)當(dāng)前位置所掃描的環(huán)境信息與地圖中可提取的特征信息進(jìn)行相似性比對(duì)，用匹配特征信息計(jì)算獲取當(dāng)前機(jī)器人在基坐標(biāo)中的位姿，即可完成機(jī)器人的定位。室內(nèi)的主要環(huán)境特征信息為相對(duì)連續(xù)的直線隔斷，上述方法可以較好地還原激光雷達(dá)對(duì)于室內(nèi)環(huán)境的掃描結(jié)果。

3.3 定位匹配

地圖創(chuàng)建后，根據(jù)當(dāng)前位姿激光雷達(dá)的掃描結(jié)果，提取前后2幀掃描之間的相對(duì)位姿，并對(duì)距離最近的m對(duì)匹配點(diǎn)進(jìn)行誤差最小化：

可通過(guò)當(dāng)前位置與多段特征直線的垂線的交點(diǎn)的擬合結(jié)果，推算出當(dāng)前所處的位姿：

定位時(shí)，越靠近于基坐標(biāo)原點(diǎn)（即開(kāi)始創(chuàng)建地圖的偽定位起始點(diǎn)）所能得到的直線匹配信息與地圖中的特征信息越接近，且各段特征線段中與定位點(diǎn)所形成的夾角越接近于基坐標(biāo)原點(diǎn)處的結(jié)果，機(jī)器人處于創(chuàng)建地圖起始點(diǎn)處可獲得最佳的定位精度以及速度。由于地圖創(chuàng)建過(guò)程，是根據(jù)機(jī)器人相對(duì)于基坐標(biāo)原點(diǎn)的位置和角度偏移主要依賴于激光以及機(jī)器人里程信息，故高精度的激光、里程計(jì)可減小地圖創(chuàng)建以及定位的誤差。

4 數(shù)據(jù)融合避障

4.1 模糊邏輯

基于人類(lèi)的思考習(xí)慣實(shí)現(xiàn)的模糊邏輯控制免去了復(fù)雜的數(shù)學(xué)模型建立過(guò)程，通過(guò)專(zhuān)家知識(shí)轉(zhuǎn)變?yōu)榭刂浦噶钚盘?hào)的則被簡(jiǎn)化，適用于復(fù)雜環(huán)境的地圖環(huán)境。模糊規(guī)則呈現(xiàn)了機(jī)器人的行走動(dòng)作與其外部傳感器（聲吶、激光雷達(dá)等）的反饋數(shù)據(jù)之間的映射關(guān)系。通過(guò)動(dòng)作的結(jié)果，可以測(cè)算當(dāng)前模糊規(guī)則的適應(yīng)度；通過(guò)優(yōu)勝劣汰的方式，逐漸篩選出最優(yōu)規(guī)則。

激光傳感器具有精度高、檢測(cè)距離較長(zhǎng)、檢測(cè)范圍廣的優(yōu)勢(shì)，但由于激光并不能覆蓋移動(dòng)機(jī)器人車(chē)身360°，通常會(huì)在機(jī)器人后方留下大約100°左右的“死角”，限于高精度激光的高昂成本，通過(guò)激光傳感器來(lái)全方位覆蓋車(chē)身難以實(shí)施。聲吶通過(guò)超聲測(cè)距，可以檢測(cè)傳感器端面前方約50 cm的錐形空間是否有障礙物遮擋，且其成本低廉，可以大量裝載。將8個(gè)聲吶均勻安裝與車(chē)體的周?chē)?，根?jù)聲吶的分布可將車(chē)體周?chē)臻g細(xì)分為8份。通過(guò)聲吶的反饋數(shù)據(jù)可以獲取車(chē)體周?chē)系K物所處的區(qū)域以及距離信息，結(jié)合移動(dòng)機(jī)器人運(yùn)動(dòng)目標(biāo)，可規(guī)劃移動(dòng)機(jī)器人底盤(pán)雙輪輸出速度。

模糊控制器的信息主要由3部分組成：模糊IO變量、模糊規(guī)則與模糊推理原則。

定義模糊輸入輸出量如表1所示。

表1 模糊量定義Tab.1 Fuzzy quantity definition

式中：第n條模糊規(guī)則用Rn表示；第i個(gè)區(qū)域及對(duì)應(yīng)的模糊量用Li、li分別表示；i為目標(biāo)點(diǎn)方向及應(yīng)區(qū)域；ωL為左側(cè)的輪速；ωR為右側(cè)的輪速。

采用MIN-MAX合成法，設(shè)當(dāng)前實(shí)際的各個(gè)區(qū)域障礙距離為有從0～n-1共n條規(guī)則被激活，則當(dāng)前左輪的輸出速度為

同理可得右輪的輸出速度為

模糊規(guī)則的匹配需要考慮其目標(biāo)方向是否相同，μ表示隸屬度，ω1，ω2，…，ω8表示與當(dāng)前規(guī)則的適配度，即規(guī)則被激活的程度，采用重心法，可得對(duì)應(yīng)的左右輪輸出為

4.2 最優(yōu)規(guī)則

在NCS的每個(gè)控制周期內(nèi)，需通過(guò)模糊邏輯推算相應(yīng)的輪速輸出量為

式中：Dms、Dme分別為每周期運(yùn)動(dòng)開(kāi)始與結(jié)束時(shí)機(jī)器人與運(yùn)動(dòng)目標(biāo)間的距離為物體運(yùn)動(dòng)起始與結(jié)束兩個(gè)時(shí)間節(jié)點(diǎn)相對(duì)于障礙物的距離；i為傳感器區(qū)域分割編號(hào)；ki為權(quán)重系數(shù)。

此時(shí)，可使用機(jī)器人預(yù)設(shè)的避碰半徑來(lái)代替公式中消失項(xiàng)。經(jīng)過(guò)仿真實(shí)驗(yàn)驗(yàn)證發(fā)現(xiàn)在同一環(huán)境狀態(tài)下，輸出輪速大的規(guī)則更易被篩選到，通過(guò)不斷地迭代，可得最大速度的可行解，更易于減少導(dǎo)航控制的時(shí)間。

5 算法仿真驗(yàn)證

5.1 導(dǎo)航算法驗(yàn)證

通過(guò)ROS仿真平臺(tái)，編寫(xiě)定位匹配算法與避障算法的節(jié)點(diǎn)，通過(guò)處理虛擬傳感器反饋數(shù)據(jù)可得到如圖1所示的仿真效果圖。

圖1 地圖匹配和定位成功Fig.1 Map matching and location successful

圖1 展示了機(jī)器人通過(guò)激光雷達(dá)掃描獲取環(huán)境信息到成功建立地圖的過(guò)程，從模糊到清晰的過(guò)程就是地圖中特征逐漸完成匹配實(shí)現(xiàn)定位過(guò)程。由圖1可見(jiàn)，通過(guò)特征提取的方式可以實(shí)現(xiàn)機(jī)器人位置與地圖的匹配，進(jìn)而實(shí)現(xiàn)定位功能，此過(guò)程中處理地圖數(shù)據(jù)的匹配需要有一段較長(zhǎng)的耗時(shí)。地圖定位精度以及激光密度共同決定了匹配的數(shù)據(jù)維度，進(jìn)而影響匹配的精度。若采用精度較低的激光雷達(dá)，則可以在犧牲定位精度的前提下，縮短定位時(shí)間。

5.2 避障算法導(dǎo)航算法驗(yàn)證

物流機(jī)器人在實(shí)際工作當(dāng)中障礙物的分布有疏密之分，為驗(yàn)證所設(shè)計(jì)算法對(duì)于障礙物分布的空間尺度具有較強(qiáng)的適應(yīng)性，分別選取了寬闊場(chǎng)景中大面積障礙物以及狹窄場(chǎng)景中的通路邊緣兩種典型環(huán)境進(jìn)行仿真，路徑通過(guò)ROS的物理引擎進(jìn)行呈現(xiàn)。在寬闊及狹窄的環(huán)境避障算法的效果分別如圖2所示。

圖2 寬闊和狹窄環(huán)境避障效果Fig.2 Wide and narrow environment obstacle avoidance effect

通過(guò)圖2（a）可發(fā)現(xiàn)，運(yùn)用避障算法，在寬闊環(huán)境中通過(guò)激光數(shù)據(jù)可檢測(cè)到較遠(yuǎn)距離的障礙物分布，同時(shí)，根據(jù)聲吶數(shù)據(jù)選擇運(yùn)動(dòng)偏移的方向，此時(shí)激光數(shù)據(jù)起主導(dǎo)作用，聲吶數(shù)據(jù)只是提供輔助信息，便于為機(jī)器人快速定位運(yùn)動(dòng)方向；而在如圖2（b）所示的狹窄環(huán)境中，激光雷達(dá)所掃描到的區(qū)域幾乎被障礙物所包圍，此時(shí)僅通過(guò)激光數(shù)據(jù)已經(jīng)無(wú)法尋找到合適的行走規(guī)則，通過(guò)多個(gè)區(qū)域內(nèi)聲吶所檢測(cè)到的障礙物信息，搜索適應(yīng)度最佳的規(guī)則，確定機(jī)器人雙輪速度的最優(yōu)解。值得注意的是，聲吶安裝的數(shù)量越多，分布越密集，則在該方向?qū)τ谡系K物檢測(cè)效果越好，在實(shí)際工作中在處理時(shí)會(huì)犧牲較長(zhǎng)的計(jì)算時(shí)間，在盡量保證最優(yōu)的同時(shí)也會(huì)使得機(jī)器人運(yùn)動(dòng)遲緩。過(guò)多的聲吶，會(huì)提升模糊邏輯數(shù)據(jù)的維度，感知的效果與導(dǎo)航算法的快速性之間是負(fù)相關(guān)的。因此，聲吶的數(shù)量需要實(shí)際應(yīng)用需求和系統(tǒng)的硬件水平來(lái)進(jìn)行部署，合理地調(diào)節(jié)聲吶的分布可在最優(yōu)化行走路徑與最短時(shí)間這對(duì)矛盾中取得較為合理的折中。

6 結(jié)語(yǔ)

在現(xiàn)代化農(nóng)業(yè)倉(cāng)庫(kù)物流搬運(yùn)產(chǎn)業(yè)布局中，通過(guò)移動(dòng)機(jī)器人取代人來(lái)實(shí)現(xiàn)產(chǎn)業(yè)升級(jí)是一條必由之路。本文針對(duì)移動(dòng)機(jī)器人導(dǎo)航控制問(wèn)題進(jìn)行了深入研究。首先對(duì)移動(dòng)機(jī)器人控制平臺(tái)進(jìn)行了闡述，并分析了移動(dòng)機(jī)器人的經(jīng)典雙輪差動(dòng)模型；其次，通過(guò)激光雷達(dá)感知環(huán)境并進(jìn)行特征提取的手段實(shí)現(xiàn)了機(jī)器人的定位與地圖匹配；最后通過(guò)聲吶與激光兩種傳感器信息融合的方法，基于模糊邏輯機(jī)器人自主避障的最優(yōu)化避障算法融合了激光雷達(dá)與聲吶的數(shù)據(jù)，使得對(duì)于狹窄環(huán)境下微距障礙物的感知增強(qiáng)，適用于激光避障無(wú)法奏效的情況，以引導(dǎo)機(jī)器人的避障行走至目標(biāo)位置。算法實(shí)現(xiàn)所依賴的主要反饋數(shù)據(jù)來(lái)源為價(jià)格較為低廉的聲吶傳感器，使得本算法在工程上具有一定的實(shí)踐價(jià)值。