自動(dòng)駕駛塔機(jī)的場(chǎng)景建模及運(yùn)行路徑規(guī)劃研究

張 志，文朝輝，李 煒

（湖北江漢建筑工程機(jī)械有限公司，湖北 荊州 434022）

塔式起重機(jī)簡(jiǎn)稱塔機(jī)，主要由塔身、起重臂、起重小車、工作機(jī)構(gòu)以及吊鉤等構(gòu)成。塔機(jī)能在較大范圍內(nèi)高效地實(shí)現(xiàn)垂直方向或水平方向的物料運(yùn)送，因此塔機(jī)在中高層建筑施工中發(fā)揮了重要作用。塔機(jī)的工作過(guò)程需要多人協(xié)同完成，其中1 人在塔機(jī)操作室內(nèi)控制塔機(jī)，1 人在地面遙控指揮，還有多人在起點(diǎn)和終點(diǎn)進(jìn)行現(xiàn)場(chǎng)勘察以及物料裝卸等操作，在工作過(guò)程中還容易受工作環(huán)境等因素的影響，因此塔機(jī)運(yùn)行效率及運(yùn)行可靠性亟待提高。隨著機(jī)器人技術(shù)的發(fā)展，通過(guò)對(duì)場(chǎng)景建模及運(yùn)動(dòng)規(guī)劃技術(shù)的研究，實(shí)現(xiàn)塔機(jī)的遠(yuǎn)程控制及自動(dòng)駕駛能有效降低塔機(jī)操作人員的勞動(dòng)強(qiáng)度，提高工作效率，保障塔機(jī)安全運(yùn)行。

1 國(guó)內(nèi)外研究現(xiàn)狀

近年來(lái)隨著自動(dòng)駕駛以及智能控制的發(fā)展，越來(lái)越多的學(xué)者開始研究復(fù)雜環(huán)境的三維重建技術(shù)。目前主流的三維場(chǎng)景建模技術(shù)有基于相機(jī)的RGBD 的圖像場(chǎng)景建模、基于深度相機(jī)的深度圖像場(chǎng)景建模、基于激光雷達(dá)的點(diǎn)云場(chǎng)景建模等。

不同的三維場(chǎng)景建模方法各有其優(yōu)缺點(diǎn)，RGBD 圖像帶有顏色信息，因此生成的場(chǎng)景模型信息更加豐富，但使用RGBD 圖像生成場(chǎng)景模型的技術(shù)難點(diǎn)在于，根據(jù)圖像使用算法生成模型所需的時(shí)間長(zhǎng)，且模型的精度受圖片質(zhì)量的影響較大。當(dāng)受到光線等因素影響導(dǎo)致圖像模糊，或受到拍攝角度影響獲取的圖像信息缺失時(shí)，生成的模型精度差。Zhao 等人[1]針對(duì)無(wú)人機(jī)傾斜攝影三維重建技術(shù)中場(chǎng)景建模精度較差的缺陷，優(yōu)化了基于圖像的三維場(chǎng)景重建技術(shù)，在重建大壩這種大場(chǎng)景的三維模型時(shí)，在現(xiàn)實(shí)場(chǎng)景中增添了多個(gè)便于算法識(shí)別的特征點(diǎn)，通過(guò)重疊多次生成含有特征點(diǎn)的圖像提高圖像重建三維點(diǎn)云模型的精度；目前主流的深度相機(jī)根據(jù)其工作原理可分為3 類：TOF、RGB 雙目以及結(jié)構(gòu)光。但在建立場(chǎng)景模型時(shí)受場(chǎng)景范圍大小，深度相機(jī)拍攝圖像的分辨率等因素制約，深度圖像建模無(wú)法兼顧建模效率以及建模質(zhì)量。Li[2]等針對(duì)增量點(diǎn)云生成場(chǎng)景模型時(shí)數(shù)據(jù)量過(guò)大的問題，在基于關(guān)鍵幀的場(chǎng)景建模中使用了增量點(diǎn)云壓縮技術(shù)，提高點(diǎn)云傳輸效率，并將其應(yīng)用于深度相機(jī)的室內(nèi)三維重建過(guò)程中。三維激光雷達(dá)因其在三維場(chǎng)景建模中精度較高，范圍大，效率高等因素，現(xiàn)如今被廣泛應(yīng)用于三維場(chǎng)景建模中，但三維激光雷達(dá)重建大場(chǎng)景的點(diǎn)云模型過(guò)程中容易因場(chǎng)景模型特征不明顯或數(shù)據(jù)量過(guò)大導(dǎo)致點(diǎn)云漂移，使得點(diǎn)云模型精度差甚至無(wú)法重建出正確的點(diǎn)云場(chǎng)景模型。Zhang Y[3]等在使用三維激光雷達(dá)建立場(chǎng)景模型時(shí)發(fā)現(xiàn)，過(guò)大的場(chǎng)景模型會(huì)導(dǎo)致激光雷達(dá)的精度誤差的累加。為了避免過(guò)高的精度誤差甚至地圖狀態(tài)向量的維度爆炸，提出了GP-SLAM+方法，將大型場(chǎng)景區(qū)域化，而后使用GPR（高斯過(guò)程回歸）進(jìn)行區(qū)域的地圖重建。Baek[4]等針對(duì)礦山環(huán)境復(fù)雜的特點(diǎn)，在提取了全局點(diǎn)云地圖特征的基礎(chǔ)上，使用RANSAC 和ICP 算法匹配全局地圖與局部地圖，實(shí)現(xiàn)礦井下的精確定位。

路徑規(guī)劃作為機(jī)械臂智能控制的重要組成部分，根據(jù)算法原理，可以分類為：基于采樣的路徑規(guī)劃方法，包括RRT、PRM 算法等；基于圖搜索的路徑規(guī)劃方法，包括A*、D*以及Dijkstra 等方法；基于模型的路徑規(guī)劃方法，如MILP 算法；基于學(xué)習(xí)的路徑規(guī)劃方法，包括神經(jīng)網(wǎng)絡(luò)、蟻群以及粒子群算法等。

針對(duì)無(wú)人駕駛移動(dòng)小車、無(wú)人機(jī)以及機(jī)械臂等設(shè)備在不同場(chǎng)景下的規(guī)劃路徑需求，學(xué)者們不斷對(duì)現(xiàn)有的算法加以改進(jìn)以及優(yōu)化。鮑久圣[5]等人為了提高算法全局搜索效率，在A*算法的啟發(fā)函數(shù)中添加了權(quán)重項(xiàng)，并使用人工勢(shì)場(chǎng)法對(duì)運(yùn)動(dòng)中的小車做局部路徑規(guī)劃，防止小車與動(dòng)態(tài)障礙物發(fā)生碰撞；Ming-Zhong Y 等[6]為了提高粒子群的學(xué)習(xí)能力以及適應(yīng)能力，提出了一種新型學(xué)習(xí)策略，改進(jìn)了傳統(tǒng)粒子群中個(gè)人和全局最佳粒子的學(xué)習(xí)機(jī)制，提高了算法跳出局部最優(yōu)的能力，并應(yīng)用于無(wú)人機(jī)在不同場(chǎng)景中的路徑規(guī)劃。

建筑塔機(jī)一般在各種建筑工地上使用，相較于小車的路徑規(guī)劃，建筑塔機(jī)的運(yùn)行空間自由度更大，同時(shí)運(yùn)動(dòng)結(jié)構(gòu)以及運(yùn)行場(chǎng)景比無(wú)人機(jī)更加復(fù)雜。在裝卸搬運(yùn)、自動(dòng)裝配、焊接生產(chǎn)線的工業(yè)應(yīng)用中，機(jī)械臂的智能控制與本文所研究的智能建筑塔機(jī)有諸多相似之處。

隨著智能機(jī)械臂的發(fā)展，很多學(xué)者也對(duì)不同的機(jī)械臂的路徑規(guī)劃算法進(jìn)行了研究。謝龍[7]等人為了使機(jī)械臂具有動(dòng)態(tài)避障以及動(dòng)態(tài)追蹤的能力，對(duì)人工勢(shì)場(chǎng)法加以改進(jìn)，構(gòu)造了基于目標(biāo)點(diǎn)速度的引力場(chǎng)，以及動(dòng)態(tài)障礙物速度的斥力場(chǎng)；J.Waikoonvet[8]等人為了實(shí)現(xiàn)橋式起重機(jī)的路徑規(guī)劃對(duì)蟻群算法加以改進(jìn)，對(duì)蟻群中的信息素添加了蒸發(fā)系數(shù)，使舊的信息素隨蟻群的擴(kuò)張而消散，提高了算法的多樣性，并根據(jù)起重機(jī)的控制系統(tǒng)使用3 次樣條曲線優(yōu)化了蟻群算法規(guī)劃出的路徑，提高了起重機(jī)運(yùn)行的穩(wěn)定性；Li[9]等人為了提高路徑規(guī)劃算法在大型場(chǎng)景中的效率以及路徑質(zhì)量，將大場(chǎng)景分成多個(gè)小區(qū)域，并根據(jù)凸包結(jié)構(gòu)在每個(gè)小區(qū)域中生成一個(gè)最優(yōu)點(diǎn)，以最優(yōu)點(diǎn)作為路徑規(guī)劃的關(guān)鍵點(diǎn)生成路徑；李揚(yáng)等人[10]為了避免RRT 算法在局部區(qū)域的過(guò)度搜索以及在采樣過(guò)程中陷入局部最優(yōu)，在傳統(tǒng)RRT 算法中引入了目標(biāo)偏置域值以及局部擴(kuò)展機(jī)制，并在采樣環(huán)節(jié)使用了自適應(yīng)步長(zhǎng)策略優(yōu)化了搜索時(shí)間。

上述文獻(xiàn)對(duì)路徑規(guī)劃算法的優(yōu)化都基于搜索效率、搜索質(zhì)量以及魯棒性3 個(gè)角度，而在塔機(jī)吊點(diǎn)的路徑規(guī)劃實(shí)現(xiàn)中，不僅僅要考慮這3 個(gè)問題，還要結(jié)合塔機(jī)的控制以及運(yùn)動(dòng)特點(diǎn)對(duì)路徑規(guī)劃方法進(jìn)行優(yōu)化。相較于機(jī)械臂的路徑規(guī)劃，建筑工地中的塔機(jī)運(yùn)行場(chǎng)景更大且更復(fù)雜，同時(shí)塔機(jī)的工作需要嚴(yán)格遵守操作規(guī)范保證安全。因此算法需要具備較好的約束或者優(yōu)化能力，使得算法生成的路徑符合塔機(jī)的操作規(guī)范。人工勢(shì)場(chǎng)法以及RRT 算法都是常見的傳統(tǒng)路徑規(guī)劃的算法，它們的隨機(jī)性較強(qiáng)，且并不具備路徑優(yōu)化的能力。因此本文選擇使用粒子群算法，針對(duì)塔機(jī)的運(yùn)動(dòng)特點(diǎn)加以改進(jìn)，使其能夠生成用于塔機(jī)吊點(diǎn)運(yùn)動(dòng)的路徑。

2 塔機(jī)運(yùn)行場(chǎng)景模型的構(gòu)建

塔機(jī)運(yùn)行場(chǎng)景模型不僅能為塔機(jī)吊點(diǎn)的運(yùn)行路徑規(guī)劃提供基本信息，還能用于仿真環(huán)境，直觀地顯示塔機(jī)的運(yùn)行，從而驗(yàn)證路徑規(guī)劃結(jié)果的有效性。場(chǎng)景模型的構(gòu)建還能實(shí)現(xiàn)塔機(jī)自動(dòng)駕駛的數(shù)字場(chǎng)景與現(xiàn)實(shí)場(chǎng)景的交互，是塔機(jī)智能控制的基礎(chǔ)。

本文通過(guò)塔機(jī)模型與環(huán)境模型的構(gòu)建以及二者模型的融合，構(gòu)建塔機(jī)運(yùn)行場(chǎng)景模型。在塔機(jī)模型的構(gòu)建中，為了在運(yùn)行場(chǎng)景中準(zhǔn)確描述塔機(jī)每一時(shí)刻的位姿，并實(shí)現(xiàn)后續(xù)的路徑規(guī)劃與運(yùn)動(dòng)控制，根據(jù)塔機(jī)的運(yùn)動(dòng)特點(diǎn)建立了1 個(gè)柱坐標(biāo)系，用坐標(biāo)值描述塔機(jī)3 個(gè)運(yùn)動(dòng)關(guān)節(jié)的位置；在環(huán)境模型的構(gòu)建中，獲取完整的環(huán)境點(diǎn)云；然后根據(jù)激光雷達(dá)掃描得到的點(diǎn)云信息特點(diǎn)，制定了合適的點(diǎn)云拼接流程；為融合塔機(jī)模型與環(huán)境模型的信息，實(shí)現(xiàn)了不同模型數(shù)據(jù)的統(tǒng)一描述，定義了統(tǒng)一的坐標(biāo)系。塔機(jī)運(yùn)行場(chǎng)景模型的構(gòu)建流程如圖1 所示。

圖1 場(chǎng)景建模流程圖

2.1 塔機(jī)模型的建立

在塔機(jī)運(yùn)動(dòng)過(guò)程中，主要有起重臂、起重小車以及吊鉤共3 個(gè)運(yùn)動(dòng)部件。為了創(chuàng)建出與真實(shí)塔機(jī)有相同運(yùn)動(dòng)結(jié)構(gòu)與尺寸的塔機(jī)模型，首先生成塔身、起重臂、起重小車和吊鉤的三維模型，并將其轉(zhuǎn)化為URDF（統(tǒng)一機(jī)器人描述格式）模型。根據(jù)塔機(jī)3 個(gè)運(yùn)動(dòng)部件的運(yùn)動(dòng)特點(diǎn)，建立1個(gè)柱坐標(biāo)系用于描述以及控制仿真環(huán)境中塔機(jī)的運(yùn)動(dòng)以及當(dāng)前位姿。以塔機(jī)基座中心位置為柱坐標(biāo)系原點(diǎn)，塔機(jī)起重臂在水平面的投影與x軸的夾角為φ，起重小車與原點(diǎn)在水平面的距離為r，吊點(diǎn)在豎直方向的高度為z，則塔機(jī)吊點(diǎn)的坐標(biāo)可表達(dá)為（ri，φi，zi)，3 個(gè)運(yùn)動(dòng)關(guān)節(jié)此時(shí)的位置分別對(duì)應(yīng)坐標(biāo)點(diǎn)中的3 個(gè)值，由柱坐標(biāo)系描述的塔機(jī)模型如圖2 所示。

圖2 塔機(jī)模型

2.2 環(huán)境模型的生成

塔機(jī)一般應(yīng)用于建筑工地，其運(yùn)行環(huán)境的特點(diǎn)是工作范圍大且環(huán)境復(fù)雜，而且隨著時(shí)間的推移，建筑工地的場(chǎng)景還在不斷地變化。根據(jù)以上特點(diǎn)，選擇將兩個(gè)三維激光雷達(dá)安裝在塔機(jī)起重臂上，通過(guò)起重臂的旋轉(zhuǎn)完成環(huán)境模型的獲取。由于塔機(jī)的工作空間的范圍有限，因此使用直通濾波將無(wú)效區(qū)域的點(diǎn)云（塔機(jī)運(yùn)行空間外的點(diǎn)云）以及塔機(jī)塔身以及起重臂的點(diǎn)云模型剔除，設(shè)塔機(jī)運(yùn)行空間半徑為R，塔機(jī)對(duì)周邊建筑物的安全距離為Rsafe，起重臂高度為H，則有效點(diǎn)（xi，yi，zi)應(yīng)滿足

由于激光雷達(dá)的激光發(fā)射頻率高，塔機(jī)起重臂的旋轉(zhuǎn)速度緩慢，因此掃描獲取的點(diǎn)云密度較大。為了獲取密度適中的點(diǎn)云，使用體素濾波降低點(diǎn)云密度，體素濾波將點(diǎn)云劃分為連續(xù)的三維體素方格，然后用每個(gè)體素方格的重心來(lái)近似表達(dá)體素中的其它點(diǎn)，可以有效降低點(diǎn)云密度，但保留點(diǎn)云的幾何特征。

因工地的環(huán)境較為復(fù)雜，空氣中也含有較多塵埃，因此掃描獲取的點(diǎn)云中含有較多的噪點(diǎn)，因此使用統(tǒng)計(jì)濾波處理點(diǎn)云，剔除點(diǎn)云中的噪點(diǎn)，設(shè)點(diǎn)云中點(diǎn)（xi，yi，zi)與其K鄰域點(diǎn)的平均距離為

根據(jù)式（2)計(jì)算出整個(gè)點(diǎn)集距離容器的平均值并計(jì)算樣本標(biāo)準(zhǔn)差，超出樣本標(biāo)準(zhǔn)差的點(diǎn)被視為離群點(diǎn)剔除。

在處理完單個(gè)激光雷達(dá)的點(diǎn)云后，進(jìn)行點(diǎn)云拼接。因激光雷達(dá)在掃描環(huán)境時(shí)傾斜向下安裝，且受安裝誤差以及運(yùn)行過(guò)程中振動(dòng)等的影響，不同激光雷達(dá)生成的點(diǎn)云無(wú)法確定準(zhǔn)確的相對(duì)位置關(guān)系，因此為了提高點(diǎn)云拼接的效率以及精度，使用先粗匹配再精匹配的方法完成點(diǎn)云拼接。粗匹配的目的是獲取點(diǎn)云之間粗略的相對(duì)位置關(guān)系，為精配準(zhǔn)提供一個(gè)比較好的初始位置，好的初始位置能提高點(diǎn)云拼接效率以及精度。設(shè)激光雷達(dá)獲取的點(diǎn)云分別為Pn與Pm，使用RANSAC方法擬合點(diǎn)云中的最大平面即為地面，并計(jì)算出點(diǎn)云Pn的地面法向量與點(diǎn)云Pm的地面法向量則粗匹配點(diǎn)云Pm的旋轉(zhuǎn)矩陣為Tmn，Tmn為一個(gè)4×4 的旋轉(zhuǎn)矩陣，根據(jù)Pn以及Pm的夾角確定。由羅德里格斯公式可得

其中I為單位向量，r為的叉乘向量。然后根據(jù)兩激光雷達(dá)的相對(duì)距離，移動(dòng)點(diǎn)云Pm完成點(diǎn)云粗匹配。最后使用ICP 算法進(jìn)行精匹配，生成環(huán)境模型的點(diǎn)云Pn。

2.3 塔機(jī)運(yùn)行場(chǎng)景坐標(biāo)系的定義

將塔機(jī)模型與環(huán)境模型融合，構(gòu)建塔機(jī)運(yùn)行場(chǎng)景模型，將二者信息在統(tǒng)一的坐標(biāo)系中進(jìn)行描述，可為塔機(jī)吊點(diǎn)的運(yùn)動(dòng)路徑規(guī)劃提供準(zhǔn)確的空間場(chǎng)景以及塔機(jī)姿態(tài)信息，因此構(gòu)建塔機(jī)運(yùn)行場(chǎng)景坐標(biāo)系，需要將柱坐標(biāo)系下塔機(jī)模型與笛卡爾坐標(biāo)系下環(huán)境模型數(shù)據(jù)進(jìn)行統(tǒng)一描述。實(shí)現(xiàn)塔機(jī)運(yùn)行場(chǎng)景下不同坐標(biāo)系數(shù)據(jù)的統(tǒng)一描述步驟如下。

2.3.1 將環(huán)境模型融入塔機(jī)運(yùn)行場(chǎng)景坐標(biāo)系

設(shè)點(diǎn)云Pn的地面法向量在塔機(jī)運(yùn)行場(chǎng)景坐標(biāo)系的xoy平面投影與xoz平面的夾角為θz，在塔機(jī)運(yùn)行場(chǎng)景坐標(biāo)系的yoz平面投影與xoz平面的夾角為θy。則三維點(diǎn)云的坐標(biāo)系下坐標(biāo)與運(yùn)行場(chǎng)景坐標(biāo)系下坐標(biāo)的變換矩陣如式4、式5 所示，點(diǎn)云繞原點(diǎn)旋轉(zhuǎn)如式6 所示。

式中Pc為旋轉(zhuǎn)前的點(diǎn)云，Pd為旋轉(zhuǎn)后的點(diǎn)云。最后將處理好的三維點(diǎn)云模型轉(zhuǎn)化為柵格地圖，并通過(guò)旋轉(zhuǎn)點(diǎn)云將柵格地圖融合到塔機(jī)運(yùn)行場(chǎng)景中。

2.3.2 將塔機(jī)模型融入塔機(jī)運(yùn)行場(chǎng)景坐標(biāo)系

設(shè)塔機(jī)運(yùn)行柱坐標(biāo)系原點(diǎn)與塔機(jī)運(yùn)行場(chǎng)景坐標(biāo)系原點(diǎn)在3 個(gè)方向上的相對(duì)距離為（dx，dy，dz)，則任意一點(diǎn)在塔機(jī)運(yùn)行柱坐標(biāo)系與塔機(jī)運(yùn)行場(chǎng)景坐標(biāo)系下的換算如式7 所示，其中（x，y，z)為點(diǎn)在塔機(jī)運(yùn)行場(chǎng)景坐標(biāo)系下坐標(biāo)，（R，φ，Z)為點(diǎn)在塔機(jī)運(yùn)行柱坐標(biāo)系下坐標(biāo)。

融合了塔機(jī)模型與柵格地圖的運(yùn)行場(chǎng)景模型如圖3 所示。

圖3 塔機(jī)運(yùn)行場(chǎng)景模型

3 塔機(jī)吊點(diǎn)運(yùn)動(dòng)路徑規(guī)劃

粒子群算法是一種根據(jù)鳥群覓食行為衍生出來(lái)的智能算法，是通過(guò)群體中個(gè)體之間的協(xié)作和信息共享來(lái)尋找最優(yōu)解。它具有優(yōu)化之初優(yōu)化速度快且優(yōu)化效果較好等優(yōu)點(diǎn)。但將粒子群算法運(yùn)用到三維空間中建筑塔機(jī)吊點(diǎn)的路徑規(guī)劃中，存在2 個(gè)關(guān)鍵問題：①算法在大型復(fù)雜三維空間中難以找到一條無(wú)碰撞的初始路徑用于優(yōu)化；②算法生成的吊點(diǎn)路徑不符合塔機(jī)的操作要求以及安全規(guī)范?；谝陨蠁栴}對(duì)粒子群算法進(jìn)行改進(jìn)，提高算法的整體效率以及路徑質(zhì)量。改進(jìn)的算法流程如圖4 所示。

圖4 改進(jìn)粒子群算法流程圖

為保證最終生成的塔機(jī)吊點(diǎn)運(yùn)動(dòng)路徑符合操作規(guī)范，將塔機(jī)吊點(diǎn)的運(yùn)動(dòng)路徑分為3 段：①吊點(diǎn)的起升路徑：塔機(jī)吊點(diǎn)提升至安全高度；②吊鉤、起重小車和起重臂的聯(lián)合運(yùn)動(dòng)路徑：以吊點(diǎn)的當(dāng)前位置為起點(diǎn)，目標(biāo)點(diǎn)上方預(yù)定位置處為終點(diǎn)，使用算法規(guī)劃出一條路徑；③吊點(diǎn)的下降路徑：塔機(jī)吊鉤下落至目標(biāo)點(diǎn)。將3 段路徑拼接在一起即為塔機(jī)吊點(diǎn)的運(yùn)動(dòng)路徑。

3.1 粒子質(zhì)量的提高

粒子群算法首先會(huì)在空間中隨機(jī)生成第一代粒子，并根據(jù)第一代中每個(gè)粒子的優(yōu)劣進(jìn)行粒子優(yōu)化。因此第一代粒子的優(yōu)劣能極大影響后續(xù)優(yōu)化的效率以及質(zhì)量，若第一代粒子處于無(wú)效區(qū)域或者粒子的質(zhì)量過(guò)差，會(huì)降低算法的優(yōu)化效率甚至導(dǎo)致優(yōu)化失敗。為了避免以上情況的發(fā)生，對(duì)粒子的生成與運(yùn)動(dòng)的范圍進(jìn)行約束，并提高第一代粒子的質(zhì)量，從而提高算法的整體效率。具體方法如下。

1）確定路徑生成有效區(qū)域 在塔機(jī)的運(yùn)行規(guī)劃中，為了提高粒子群算法對(duì)路徑的優(yōu)化效率，應(yīng)避免粒子生成在無(wú)效區(qū)域以及在優(yōu)化過(guò)程中向無(wú)效區(qū)域運(yùn)動(dòng)。在小車或無(wú)人機(jī)的路徑規(guī)劃中，為了提高算法的全局性，防止陷入局部最優(yōu)，路徑生成的有效區(qū)域通常為整張地圖。但在塔機(jī)吊點(diǎn)的路徑規(guī)劃中，根據(jù)塔機(jī)的操作規(guī)范，在已知起始點(diǎn)與目標(biāo)點(diǎn)的情況下，為了提高搬運(yùn)物料的效率，塔機(jī)起重臂總向轉(zhuǎn)角更小的方向旋轉(zhuǎn)。因此路徑生成的有效區(qū)域應(yīng)為一個(gè)角度不超過(guò)180°的扇形（圖5），扇形的半徑為起重小車可到達(dá)的最遠(yuǎn)距離，高度為吊點(diǎn)可提升到的最高高度。通過(guò)將路徑規(guī)劃的有效區(qū)域由塔機(jī)吊點(diǎn)的運(yùn)行空間縮小為一個(gè)與路徑規(guī)劃起始點(diǎn)與目標(biāo)點(diǎn)有關(guān)的扇形區(qū)域可加快算法的收斂速度，提高算法的效率。

圖5 塔機(jī)吊點(diǎn)路徑生成有效區(qū)域

2）第一代粒子的重排序 在粒子群算法中，第一代粒子的每個(gè)點(diǎn)依次生成在隨機(jī)位置，因此粒子中相鄰點(diǎn)的連線很大概率相互交錯(cuò)，如圖6（a)所示。依照塔機(jī)的操作規(guī)范，塔機(jī)在搬運(yùn)物料時(shí)，應(yīng)避免起重臂的來(lái)回?cái)[動(dòng)。因此交錯(cuò)的路徑既不符合塔機(jī)的操作規(guī)范，同時(shí)增加了粒子群算法的優(yōu)化難度，降低了優(yōu)化效率。為了保證粒子取值的隨機(jī)性，同時(shí)防止塔機(jī)起重臂在此路徑上運(yùn)動(dòng)時(shí)起重臂回轉(zhuǎn)運(yùn)動(dòng)，提高算法的優(yōu)化效率。根據(jù)粒子中每個(gè)點(diǎn)與起重臂旋轉(zhuǎn)中心連線與起始點(diǎn)于起重臂旋轉(zhuǎn)中心連線的夾角，對(duì)粒子中的點(diǎn)重新排序。排序后路徑如圖6（b)所示。

圖6 第一代粒子重排序前后對(duì)比

3.2 路徑質(zhì)量的提高

傳統(tǒng)粒子群算法通常以整條路徑的長(zhǎng)度作為唯一因素選取最優(yōu)粒子。然而在塔機(jī)吊點(diǎn)的路徑規(guī)劃中，僅使用路徑長(zhǎng)度描述粒子的優(yōu)劣過(guò)于片面。塔機(jī)運(yùn)行過(guò)程的操作規(guī)范要求：為了提高吊裝效率同時(shí)提高吊裝過(guò)程的安全性，起重臂總朝轉(zhuǎn)角更小的方向轉(zhuǎn)動(dòng)，同時(shí)起重臂在旋轉(zhuǎn)過(guò)程中不應(yīng)改變旋轉(zhuǎn)方向；為了減少吊點(diǎn)在運(yùn)動(dòng)過(guò)程中速度方向的突變，起重小車應(yīng)盡量減少運(yùn)動(dòng)方向的改變次數(shù)；因塔機(jī)在運(yùn)送物料過(guò)程中無(wú)法避免吊索的輕微晃動(dòng)，為了防止在晃動(dòng)過(guò)程中物料與障礙物發(fā)生碰撞，塔機(jī)在運(yùn)動(dòng)過(guò)程中應(yīng)與障礙物應(yīng)保持安全距離Dsafe；為了提高工作效率，應(yīng)盡量減少運(yùn)動(dòng)路徑的長(zhǎng)度。為了達(dá)到上述目的，對(duì)粒子群算法中對(duì)粒子的評(píng)價(jià)函數(shù)加以改進(jìn)，如式（8）

式中的參數(shù)含義如圖7 所示，在塔機(jī)運(yùn)行場(chǎng)景坐標(biāo)系下，fitnessi為吊點(diǎn)從粒子中第i-1 個(gè)節(jié)點(diǎn)運(yùn)動(dòng)到第i個(gè)節(jié)點(diǎn)Si的代價(jià)；Sc為起重臂旋轉(zhuǎn)中心在xoy 平面的投影；Sstart和Send分別為路徑規(guī)劃的起點(diǎn)與目標(biāo)點(diǎn)；ai為水平面上吊點(diǎn)從粒子中第i-1 個(gè)節(jié)點(diǎn)運(yùn)動(dòng)到第i個(gè)節(jié)點(diǎn)過(guò)程中起重臂轉(zhuǎn)過(guò)的弧度；ci為三維空間中之間夾角的弧度；α和β為系數(shù)。粒子中吊點(diǎn)由Si-1點(diǎn)運(yùn)動(dòng)到Si點(diǎn)的代價(jià)由兩部分組成，一部分為，代表起重臂轉(zhuǎn)過(guò)單位弧度時(shí)吊點(diǎn)運(yùn)行的無(wú)效路徑長(zhǎng)度。

圖7 參數(shù)關(guān)系示意圖

為了防止塔機(jī)起重臂在運(yùn)行過(guò)程中往復(fù)運(yùn)動(dòng)以及與障礙物的距離過(guò)于接近，在路徑點(diǎn)代價(jià)函數(shù)中設(shè)置了違規(guī)系數(shù)α，α的值隨起重臂的轉(zhuǎn)動(dòng)方向以及路徑與障礙物間的距離決定。當(dāng)塔機(jī)起重臂由Si-1位置轉(zhuǎn)向Si位置時(shí)，起重臂的轉(zhuǎn)向與起重臂由路徑起始位置轉(zhuǎn)向目標(biāo)點(diǎn)的方向不同，或路徑與障礙物之間的距離小于Dsafe時(shí)α為無(wú)窮大，否則α為一個(gè)固定值，固定值的大小代表路徑長(zhǎng)度這一代價(jià)在整體代價(jià)中所占的比重。另一部分為用于描述在吊點(diǎn)由節(jié)點(diǎn)Si-1運(yùn)行至節(jié)點(diǎn)Si時(shí)起重小車的平均速率以及轉(zhuǎn)向次數(shù)，其目的是為了將起重小車在運(yùn)動(dòng)中運(yùn)動(dòng)速度限制在安全區(qū)域內(nèi)以及盡量減少起重小車的運(yùn)動(dòng)速度的改變次數(shù)，當(dāng)起重小車速度不在安全閾值內(nèi)，則β為無(wú)窮大，否則β=1。同時(shí)為了防止起重小車往復(fù)運(yùn)動(dòng)，在路徑點(diǎn)代價(jià)函數(shù)中設(shè)置了起重小車轉(zhuǎn)向代價(jià)γ，γ的值隨起重小車的運(yùn)動(dòng)方向變化而增大。

4 實(shí)驗(yàn)結(jié)果分析

為了驗(yàn)證改進(jìn)粒子群算法在塔機(jī)吊點(diǎn)路徑規(guī)劃中的有效性，進(jìn)行了對(duì)比實(shí)驗(yàn)。設(shè)柵格地圖的尺寸為200×200×50mm，塔機(jī)位于柵格地圖的正中心，路徑規(guī)劃算法碰撞檢測(cè)的安全距離Dsafe=3，起重臂旋轉(zhuǎn)方向系數(shù)α=1，起重小車速度與起重臂轉(zhuǎn)速比值的最大值為30，且塔機(jī)在運(yùn)行過(guò)程中起重臂的旋轉(zhuǎn)速度不變。

表1 給出了粒子群算法和改進(jìn)的粒子群算法在相同時(shí)間內(nèi)進(jìn)行路徑規(guī)劃的比較。由表1 可知在搜索時(shí)間相同的情況下，改進(jìn)的粒子群算法與粒子群算法相比，平均優(yōu)化次數(shù)提升了7.10%；在路徑質(zhì)量方面，路徑長(zhǎng)度增加了14.11%，但起重臂旋轉(zhuǎn)方向改變次數(shù)降為零，起重小車運(yùn)動(dòng)方向改變次數(shù)降低了66.07%，起重小車速度與起重臂轉(zhuǎn)速的比值的平均值下降了476.01%。以上數(shù)據(jù)表明改進(jìn)的粒子群算法相比于粒子群算法，在效率以及對(duì)塔機(jī)吊點(diǎn)路徑規(guī)劃的適用性上都更加優(yōu)秀。改進(jìn)粒子群算法在相同的時(shí)間內(nèi)優(yōu)化次數(shù)更多，其優(yōu)化出的路徑使得塔機(jī)起重臂不再做往復(fù)回轉(zhuǎn)運(yùn)動(dòng)；起重小車運(yùn)動(dòng)方向的改變次數(shù)有所下降；起重小車的速度與起重臂轉(zhuǎn)速的比值限制在安全區(qū)域內(nèi)，在塔機(jī)運(yùn)行中，將會(huì)更有利于增加吊裝過(guò)程的平穩(wěn)性。

表1 改進(jìn)前后的粒子群算法比較

表2 給出了不同α下，粒子群算法在一定時(shí)間內(nèi)對(duì)路徑的優(yōu)化次數(shù)，以及路徑的長(zhǎng)度，起重小車運(yùn)動(dòng)方向改變次數(shù)和起重小車速度與起重臂轉(zhuǎn)速比值的平均值以及最大值的比較。由表2 可知，隨著α值的不斷減小，路徑長(zhǎng)度在不斷增加，起重小車速度與起重臂轉(zhuǎn)速的平均值以及最大值都在不斷下降，這說(shuō)明在起重臂轉(zhuǎn)速一定時(shí)，α值的大小對(duì)起重小車速度有一定的影響，α的減小能有效防止吊點(diǎn)依據(jù)規(guī)劃出的路徑運(yùn)動(dòng)時(shí)起重小車速度超出安全限制。在實(shí)際的工程應(yīng)用中，可依據(jù)起重臂的旋轉(zhuǎn)速度以及實(shí)驗(yàn)結(jié)果選擇合適的α值，生成更符合塔機(jī)運(yùn)動(dòng)控制的路徑。

表2 不同α值對(duì)改進(jìn)的粒子群算法影響

為了進(jìn)一步驗(yàn)證改進(jìn)的粒子群算法的優(yōu)缺點(diǎn)，將RRT 算法中樹節(jié)點(diǎn)的生成空間以及父節(jié)點(diǎn)選取的代價(jià)函數(shù)做出改進(jìn)，改進(jìn)的方式與改進(jìn)的粒子群算法相同，并取不同的α值，對(duì)比RRT算法與粒子群算法在改進(jìn)之后生成的路徑質(zhì)量。因?yàn)镽RT 算法生成樹節(jié)點(diǎn)時(shí)完全隨機(jī)，同時(shí)不具備優(yōu)化或?qū)W習(xí)的能力，對(duì)比表2 以及表3 可知，RRT 算法生成路徑的速度快，隨機(jī)性較強(qiáng)，起重小車的速度變化大，相較于改進(jìn)的粒子群算法，RRT 算法生成的路徑不利于塔機(jī)的運(yùn)動(dòng)控制。

表3 使用相同代價(jià)函數(shù)的RRT算法

在塔機(jī)運(yùn)行場(chǎng)景中使用改進(jìn)的粒子群算法快速生成一條符合塔機(jī)操作規(guī)范的路徑實(shí)現(xiàn)垂直和水平方向運(yùn)輸建筑施工中的物料，吊點(diǎn)的運(yùn)動(dòng)路徑如圖8 實(shí)線所示。

圖8 塔機(jī)吊點(diǎn)運(yùn)動(dòng)路徑圖

5 結(jié)語(yǔ)

本文對(duì)塔機(jī)運(yùn)行場(chǎng)景的建模以及塔機(jī)的運(yùn)行路徑規(guī)劃技術(shù)進(jìn)行了研究。首先使用三維激光雷達(dá)掃描塔機(jī)運(yùn)行環(huán)境，生成點(diǎn)云環(huán)境模型。同時(shí)建立塔機(jī)的三維模型，根據(jù)塔機(jī)的運(yùn)動(dòng)特點(diǎn)建立塔機(jī)運(yùn)行柱坐標(biāo)系，將處理后的環(huán)境模型以及塔機(jī)模型融合到塔機(jī)運(yùn)行場(chǎng)景模型中；使用改進(jìn)的粒子群算法實(shí)現(xiàn)了塔機(jī)吊點(diǎn)的運(yùn)動(dòng)路徑規(guī)劃。改進(jìn)的粒子群算法縮小了塔機(jī)路徑生成的有效區(qū)域并對(duì)第一代粒子中的點(diǎn)重新排序，以此提升算法的效率；根據(jù)塔機(jī)運(yùn)行的實(shí)際情況以及控制要求優(yōu)化了算法中的代價(jià)函數(shù)，使塔機(jī)在運(yùn)行過(guò)程中更符合操作規(guī)范，并能有效提高塔機(jī)自動(dòng)運(yùn)行的效率。最后在仿真環(huán)境中驗(yàn)證路徑可行性后，將路徑上的路徑點(diǎn)依次提取并保存，并發(fā)到PLC的指定地址，在試驗(yàn)場(chǎng)地中由塔機(jī)控制系統(tǒng)讀取PLC 中的路徑點(diǎn)完成塔機(jī)的自動(dòng)控制，實(shí)現(xiàn)了塔機(jī)的無(wú)人駕駛。