自動駕駛塔機的場景建模及運行路徑規劃研究

張 志，文朝輝，李 煒

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

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

1 國內外研究現狀

近年來隨著自動駕駛以及智能控制的發展，越來越多的學者開始研究復雜環境的三維重建技術。目前主流的三維場景建模技術有基于相機的RGBD 的圖像場景建模、基于深度相機的深度圖像場景建模、基于激光雷達的點云場景建模等。

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

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

針對無人駕駛移動小車、無人機以及機械臂等設備在不同場景下的規劃路徑需求，學者們不斷對現有的算法加以改進以及優化。鮑久圣[5]等人為了提高算法全局搜索效率，在A*算法的啟發函數中添加了權重項，并使用人工勢場法對運動中的小車做局部路徑規劃，防止小車與動態障礙物發生碰撞；Ming-Zhong Y 等[6]為了提高粒子群的學習能力以及適應能力，提出了一種新型學習策略，改進了傳統粒子群中個人和全局最佳粒子的學習機制，提高了算法跳出局部最優的能力，并應用于無人機在不同場景中的路徑規劃。

建筑塔機一般在各種建筑工地上使用，相較于小車的路徑規劃，建筑塔機的運行空間自由度更大，同時運動結構以及運行場景比無人機更加復雜。在裝卸搬運、自動裝配、焊接生產線的工業應用中，機械臂的智能控制與本文所研究的智能建筑塔機有諸多相似之處。

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

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

2 塔機運行場景模型的構建

塔機運行場景模型不僅能為塔機吊點的運行路徑規劃提供基本信息，還能用于仿真環境，直觀地顯示塔機的運行，從而驗證路徑規劃結果的有效性。場景模型的構建還能實現塔機自動駕駛的數字場景與現實場景的交互，是塔機智能控制的基礎。

本文通過塔機模型與環境模型的構建以及二者模型的融合，構建塔機運行場景模型。在塔機模型的構建中，為了在運行場景中準確描述塔機每一時刻的位姿，并實現后續的路徑規劃與運動控制，根據塔機的運動特點建立了1 個柱坐標系，用坐標值描述塔機3 個運動關節的位置；在環境模型的構建中，獲取完整的環境點云；然后根據激光雷達掃描得到的點云信息特點，制定了合適的點云拼接流程；為融合塔機模型與環境模型的信息，實現了不同模型數據的統一描述，定義了統一的坐標系。塔機運行場景模型的構建流程如圖1 所示。

圖1 場景建模流程圖

2.1 塔機模型的建立

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

圖2 塔機模型

2.2 環境模型的生成

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

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

因工地的環境較為復雜，空氣中也含有較多塵埃，因此掃描獲取的點云中含有較多的噪點，因此使用統計濾波處理點云，剔除點云中的噪點，設點云中點（xi，yi，zi)與其K鄰域點的平均距離為

根據式（2)計算出整個點集距離容器的平均值并計算樣本標準差，超出樣本標準差的點被視為離群點剔除。

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

其中I為單位向量，r為的叉乘向量。然后根據兩激光雷達的相對距離，移動點云Pm完成點云粗匹配。最后使用ICP 算法進行精匹配，生成環境模型的點云Pn。

2.3 塔機運行場景坐標系的定義

將塔機模型與環境模型融合，構建塔機運行場景模型，將二者信息在統一的坐標系中進行描述，可為塔機吊點的運動路徑規劃提供準確的空間場景以及塔機姿態信息，因此構建塔機運行場景坐標系，需要將柱坐標系下塔機模型與笛卡爾坐標系下環境模型數據進行統一描述。實現塔機運行場景下不同坐標系數據的統一描述步驟如下。

2.3.1 將環境模型融入塔機運行場景坐標系

設點云Pn的地面法向量在塔機運行場景坐標系的xoy平面投影與xoz平面的夾角為θz，在塔機運行場景坐標系的yoz平面投影與xoz平面的夾角為θy。則三維點云的坐標系下坐標與運行場景坐標系下坐標的變換矩陣如式4、式5 所示，點云繞原點旋轉如式6 所示。

式中Pc為旋轉前的點云，Pd為旋轉后的點云。最后將處理好的三維點云模型轉化為柵格地圖，并通過旋轉點云將柵格地圖融合到塔機運行場景中。

2.3.2 將塔機模型融入塔機運行場景坐標系

設塔機運行柱坐標系原點與塔機運行場景坐標系原點在3 個方向上的相對距離為（dx，dy，dz)，則任意一點在塔機運行柱坐標系與塔機運行場景坐標系下的換算如式7 所示，其中（x，y，z)為點在塔機運行場景坐標系下坐標，（R，φ，Z)為點在塔機運行柱坐標系下坐標。

融合了塔機模型與柵格地圖的運行場景模型如圖3 所示。

圖3 塔機運行場景模型

3 塔機吊點運動路徑規劃

粒子群算法是一種根據鳥群覓食行為衍生出來的智能算法，是通過群體中個體之間的協作和信息共享來尋找最優解。它具有優化之初優化速度快且優化效果較好等優點。但將粒子群算法運用到三維空間中建筑塔機吊點的路徑規劃中，存在2 個關鍵問題：①算法在大型復雜三維空間中難以找到一條無碰撞的初始路徑用于優化；②算法生成的吊點路徑不符合塔機的操作要求以及安全規范。基于以上問題對粒子群算法進行改進，提高算法的整體效率以及路徑質量。改進的算法流程如圖4 所示。

圖4 改進粒子群算法流程圖

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

3.1 粒子質量的提高

粒子群算法首先會在空間中隨機生成第一代粒子，并根據第一代中每個粒子的優劣進行粒子優化。因此第一代粒子的優劣能極大影響后續優化的效率以及質量，若第一代粒子處于無效區域或者粒子的質量過差，會降低算法的優化效率甚至導致優化失敗。為了避免以上情況的發生，對粒子的生成與運動的范圍進行約束，并提高第一代粒子的質量，從而提高算法的整體效率。具體方法如下。

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

圖5 塔機吊點路徑生成有效區域

2）第一代粒子的重排序 在粒子群算法中，第一代粒子的每個點依次生成在隨機位置，因此粒子中相鄰點的連線很大概率相互交錯，如圖6（a)所示。依照塔機的操作規范，塔機在搬運物料時，應避免起重臂的來回擺動。因此交錯的路徑既不符合塔機的操作規范，同時增加了粒子群算法的優化難度，降低了優化效率。為了保證粒子取值的隨機性，同時防止塔機起重臂在此路徑上運動時起重臂回轉運動，提高算法的優化效率。根據粒子中每個點與起重臂旋轉中心連線與起始點于起重臂旋轉中心連線的夾角，對粒子中的點重新排序。排序后路徑如圖6（b)所示。

圖6 第一代粒子重排序前后對比

3.2 路徑質量的提高

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

式中的參數含義如圖7 所示，在塔機運行場景坐標系下，fitnessi為吊點從粒子中第i-1 個節點運動到第i個節點Si的代價；Sc為起重臂旋轉中心在xoy 平面的投影；Sstart和Send分別為路徑規劃的起點與目標點；ai為水平面上吊點從粒子中第i-1 個節點運動到第i個節點過程中起重臂轉過的弧度；ci為三維空間中之間夾角的弧度；α和β為系數。粒子中吊點由Si-1點運動到Si點的代價由兩部分組成，一部分為，代表起重臂轉過單位弧度時吊點運行的無效路徑長度。

圖7 參數關系示意圖

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

4 實驗結果分析

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

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

表1 改進前后的粒子群算法比較

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

表2 不同α值對改進的粒子群算法影響

為了進一步驗證改進的粒子群算法的優缺點，將RRT 算法中樹節點的生成空間以及父節點選取的代價函數做出改進，改進的方式與改進的粒子群算法相同，并取不同的α值，對比RRT算法與粒子群算法在改進之后生成的路徑質量。因為RRT 算法生成樹節點時完全隨機，同時不具備優化或學習的能力，對比表2 以及表3 可知，RRT 算法生成路徑的速度快，隨機性較強，起重小車的速度變化大，相較于改進的粒子群算法，RRT 算法生成的路徑不利于塔機的運動控制。

表3 使用相同代價函數的RRT算法

在塔機運行場景中使用改進的粒子群算法快速生成一條符合塔機操作規范的路徑實現垂直和水平方向運輸建筑施工中的物料，吊點的運動路徑如圖8 實線所示。

圖8 塔機吊點運動路徑圖

5 結語

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