塔吊與障礙物的防碰撞算法研究

呂軍，繆澤峰，王威滔，丁雙定，吳縉峰

（1.中電?？导瘓F(tuán)有限公司，浙江 杭州 311100；2.浙江?？悼萍加邢薰?，浙江 杭州 311100）

0 引言

塔吊是建筑工地上必不可少的一種起重設(shè)備，主要用于建筑施工中物料的垂直和水平輸送，具有使用范圍廣、回轉(zhuǎn)半徑大、起升高度高和施工效率高的特點(diǎn)。塔吊工作于多樣復(fù)雜的施工場(chǎng)合中，由于其工作高度高、塔臂跨度大等特點(diǎn)，作業(yè)覆蓋面很廣，又因工地障礙物的多樣性，存在塔吊與障礙物之間發(fā)生碰撞的可能性。預(yù)防塔吊與障礙物的碰撞是塔吊安全監(jiān)控的重中之重[1-3]。

文獻(xiàn)[4]提出一種基于空間三維定位技術(shù)的塔吊防撞系統(tǒng)設(shè)計(jì)，安裝在不同位置的數(shù)據(jù)傳感器對(duì)數(shù)據(jù)采集后，經(jīng)過(guò)無(wú)線網(wǎng)絡(luò)傳輸?shù)缴衔粰C(jī)的控制中心，使用空間三維定位技術(shù)對(duì)傳感器的信息進(jìn)行數(shù)學(xué)計(jì)算，通過(guò)數(shù)學(xué)模型達(dá)到防撞告警的目的。

文獻(xiàn)[5]對(duì)單臺(tái)塔機(jī)的定位問(wèn)題和塔機(jī)間相對(duì)位置關(guān)系問(wèn)題進(jìn)行研究，提出了塔機(jī)防碰撞控制算法的設(shè)計(jì)思路與軟件實(shí)現(xiàn)方法。

文獻(xiàn)[6]研究了一種多傳感器感知融合的塔吊防碰算法，該算法需要在塔吊上布置多個(gè)傳感器獲取大量冗余信息，驗(yàn)證多傳感器信息融合理論在塔機(jī)防碰撞中的可行性和優(yōu)越性。

針對(duì)目前的研究現(xiàn)狀，上述文獻(xiàn)[4-6]給出了算法設(shè)計(jì)的基本原理，但在算法實(shí)現(xiàn)方面并沒(méi)有可執(zhí)行的詳細(xì)設(shè)計(jì)流程，實(shí)際工程上缺乏可行的操作辦法。本文建立了塔吊與障礙物的相對(duì)坐標(biāo)系系統(tǒng)，給出以防碰撞距離和防碰撞角度為二元參數(shù)的防碰撞工程算法。該算法有效克服了塔吊監(jiān)控設(shè)備廠家無(wú)法或者很難獲取塔吊與障礙物的坐標(biāo)測(cè)繪圖紙的問(wèn)題，大大減少了傳感器的安裝數(shù)量，極大簡(jiǎn)化了工地現(xiàn)場(chǎng)的防碰撞算法，操作簡(jiǎn)單明了、易實(shí)現(xiàn)。

1 塔吊坐標(biāo)系

以塔吊所在的安裝基座為坐標(biāo)原點(diǎn)o，塔吊起重臂為x軸，垂直塔吊起重臂的法向?yàn)閥軸，塔身為z軸，建立以右手法則定義的的空間直角坐標(biāo)系。障礙物的數(shù)學(xué)模型簡(jiǎn)化為一段直線或者由幾段直線圍合的區(qū)域。為了工程計(jì)算的簡(jiǎn)便，建立如圖1 所示的相對(duì)于障礙物的坐標(biāo)系x＇oy＇。該坐標(biāo)系也以塔基基座為原點(diǎn)o，障礙物的一端邊界的端點(diǎn)為P1，以oP1為x＇軸，oP1軸的法向?yàn)閥＇軸。障礙物的另一邊界端點(diǎn)為P2。

圖1 塔吊相對(duì)坐標(biāo)系Figure1 Relative coordinate system of tower crane

設(shè)oP1和oP2的長(zhǎng)度分別為ρ1和ρ2，oP1和oP2與x 軸的夾角分別為θ1和θ2，如果將xoy 坐標(biāo)系相對(duì)障礙物逆時(shí)針旋轉(zhuǎn)θ1，形成了新的相對(duì)坐標(biāo)系x＇oy＇。在x＇oy＇坐標(biāo)系下，記Δθ=θ2-θ1。P1和P2的坐標(biāo)分別簡(jiǎn)化為（ρ1，0）和（ρ2cosΔθ，ρ2sinΔθ）。

設(shè)P1P2直線方程為Ax+By+C=0，其中A=ρ2sinΔθ，B=ρ1-ρ2cosΔθ，C=-ρ1ρ2sinΔθ。在x＇oy＇坐標(biāo)系中任意一點(diǎn)P（x，y）到直線P1P2的距離d 為：

2 預(yù)警和報(bào)警距離

在工程上，為了讓塔吊避讓障礙物，往往設(shè)定預(yù)警距離dwarn和報(bào)警距離dalarm，一般有dwarn＞dalarm，即預(yù)警距離要大于報(bào)警距離。

塔吊起重臂上的P（x，y）到直線P1P2的防碰撞距離d 滿足d＞dwarn時(shí)，塔吊可以自由運(yùn)動(dòng)；當(dāng)dalarm＜d≤dwarn時(shí)，處于預(yù)警狀態(tài)；當(dāng)d≤dalarm時(shí)，處于報(bào)警狀態(tài)。

設(shè)塔吊的最大回轉(zhuǎn)半徑ρmax，單位為m。塔吊的變幅速度為v，單位為m/min，回轉(zhuǎn)速度為n，單位為r/min。塔吊監(jiān)控設(shè)備的程序掃描周期設(shè)為T(mén)，單位ms。則預(yù)警距離dwarn和報(bào)警距離dalarm滿足以下式（2）和式（3）：

塔吊的運(yùn)動(dòng)形式包括吊鉤的上升和下降運(yùn)動(dòng)、小車(chē)沿著起重臂來(lái)回運(yùn)動(dòng)的變幅運(yùn)動(dòng)、起重臂繞著塔吊基座旋轉(zhuǎn)的回轉(zhuǎn)運(yùn)動(dòng)這3 種運(yùn)動(dòng)方式。式（1）和式（2）限定了小車(chē)的變幅運(yùn)動(dòng)范圍，小車(chē)在進(jìn)行變幅運(yùn)動(dòng)時(shí)，如果小車(chē)超出了式（2）的限定范圍，則存在小車(chē)動(dòng)態(tài)碰撞的重大風(fēng)險(xiǎn)。式（3）限定了塔吊回轉(zhuǎn)運(yùn)動(dòng)范圍，超出式（3）的限定范圍也會(huì)造成動(dòng)態(tài)碰撞風(fēng)險(xiǎn)。

為了更直觀研究回轉(zhuǎn)時(shí)的防碰撞距離，引入防碰撞角度，分別為預(yù)警角度θwarm和報(bào)警角度θalarm。圖1 中P2N 和P2M 的距離分別為預(yù)警距離dalarm和報(bào)警距離dwarn，有：

塔吊起重臂回轉(zhuǎn)角度為θ，塔吊向障礙物禁止區(qū)域方向行進(jìn)時(shí)，即將觸碰到障礙物的邊緣，滿足θ＞θwarn時(shí)，塔吊可以自由運(yùn)動(dòng)；當(dāng)θalarm＜θ≤θwarn時(shí)，處于預(yù)警狀態(tài)；當(dāng)θ≤θalarm時(shí)，處于報(bào)警狀態(tài)。

3 障礙物的數(shù)學(xué)模型

選取塔吊安裝位置的固定基座水平面為參考水平面，塔吊起重臂的高度為H，塔吊吊鉤高度為h。如圖2 所示障礙物的邊界點(diǎn)分別為P1，P2，P3，P4和P5。以P1，P2，P3，P4和P5包圍區(qū)域?yàn)檎系K物的區(qū)域，該區(qū)域內(nèi)因有障礙物，塔吊運(yùn)行到此區(qū)域時(shí)必須注意防碰撞的問(wèn)題。

圖2 障礙物模型Figure 2 Mode of construction site’s obstacles

由于實(shí)際工況的復(fù)雜性，障礙物在x＇oy＇水平面上，形狀各異，也存在障礙物高度不同的情況?？梢酝ㄟ^(guò)近似分割的原理，將復(fù)雜的障礙物簡(jiǎn)化為凸多邊形的障礙物區(qū)域。如果實(shí)際中遇到障礙物區(qū)域是凹多邊形，則可以通過(guò)分割方法將其分割為由幾個(gè)凸多邊形的障礙物區(qū)域組成。

4 防碰撞工程算法

利用上述的防碰撞模型和障礙物的數(shù)學(xué)模型，提出工程實(shí)際應(yīng)用上的一種快捷便利的算法。以P1，P2，P3，P4和P5包圍區(qū)域?yàn)檎系K物的區(qū)域?yàn)槔?，具體闡述防碰撞工程算法。

4.1 建立相對(duì)坐標(biāo)系

觀察障礙物的邊界點(diǎn)，選取圖2 所示的oP1為x＇軸，起重臂的法向方向?yàn)閥＇軸，逆時(shí)針旋轉(zhuǎn)角度記為θ。工程上，司機(jī)駕駛塔吊收回小車(chē)，將吊鉤上升至最高高度，采用最慢檔速度緩慢地使塔吊起重臂回轉(zhuǎn)到障礙物P1的臨近位置，標(biāo)記為x 軸。

4.2 測(cè)量障礙物邊界點(diǎn)

（1）當(dāng)障礙物的高度低于塔吊起重臂高度H時(shí)，標(biāo)記P1P2P3P4P5區(qū)域?yàn)椴煌耆箙^(qū)域，即塔吊的起重臂可跨越障礙物或者吊鉤可跨越障礙物區(qū)域進(jìn)行作業(yè)。

不完全禁止區(qū)域的邊界確認(rèn)方法，需確定邊界點(diǎn)P1，P2，P3，P4和P5的坐標(biāo)。

第一步，按照4.1 的方法，建立坐標(biāo)系；

第二步，將小車(chē)緩緩向外駛出，接近P1點(diǎn)時(shí)記下幅度傳感器的幅度值ρ1，即P1坐標(biāo)為（ρ1，0）；

第三步，將塔吊逆時(shí)針從P1點(diǎn)回轉(zhuǎn)到P5，回轉(zhuǎn)傳感器測(cè)量出的旋轉(zhuǎn)角度為θ5，塔吊的幅度傳感器測(cè)量出幅度為ρ5，即P5坐標(biāo)為（ρ5cosθ5，ρ5sinθ5）；

第四步，依次測(cè)量出P4，P3和P2對(duì)應(yīng)的回轉(zhuǎn)角度θ4，θ3，和θ2以及幅度傳感器的幅度值ρ4，ρ3和ρ2，即P4，P3和P2坐標(biāo)分別為（ρ4cosθ4，ρ4sinθ4）、（ρ3cosθ3，ρ3sinθ3）和（ρ2cosθ2，ρ2sinθ2）。

至此可按式（1）計(jì)算出任意點(diǎn)P 到直線P1P2，P2P3，P3P4，P4P5和P1P5的距離，分別記為dp，L12，dp，L23，dp，L34，dp，L45和dp，L15；

（2）當(dāng)障礙物的高度超過(guò)塔吊起重臂高度H時(shí)，標(biāo)記P1P2P3P4P5多邊形區(qū)域?yàn)橥耆箙^(qū)域，即塔吊的起重臂不可跨越障礙物在該區(qū)域內(nèi)作業(yè)。

完全禁止區(qū)域的邊界確認(rèn)方法，較為簡(jiǎn)單，只需確定邊界點(diǎn)P1和P5。

第一步，按照4.1 的方法，建立坐標(biāo)系；

第二步，將小車(chē)緩緩向外駛出，接近P1點(diǎn)時(shí)記下幅度傳感器的幅度值ρ1，即P1坐標(biāo)為（ρ1，0）；

第三步，因障礙物限制，塔吊起重臂只能順時(shí)針旋轉(zhuǎn)，臨近障礙物的P5點(diǎn)，采用最慢檔緩慢靠近P5點(diǎn)，回轉(zhuǎn)傳感器測(cè)量出的旋轉(zhuǎn)角度為α；塔吊的幅度傳感器測(cè)量出幅度為ρ5；

第四步，計(jì)算點(diǎn)P5的角度θ5=2π-α，即P5坐標(biāo)為（ρ5cosθ5，ρ5sinθ5）。

至此可計(jì)算出任意點(diǎn)P 到直線P1P5的距離，記為dp，L15。

4.3 塔吊吊鉤與障礙物區(qū)域的相對(duì)位置

從目標(biāo)點(diǎn)出發(fā)引一條射線，看這條射線和多邊形所有邊的交點(diǎn)數(shù)目。如果有奇數(shù)個(gè)交點(diǎn)，則說(shuō)明目標(biāo)點(diǎn)在凸多邊形內(nèi)部；如果有偶數(shù)個(gè)交點(diǎn)，則說(shuō)明目標(biāo)點(diǎn)在凸多邊形外部。

設(shè)塔吊小車(chē)P 點(diǎn)坐標(biāo)為（ρcosθ，ρsinθ），P 點(diǎn)坐標(biāo)值通過(guò)回轉(zhuǎn)傳感器和幅度傳感器獲得，是已知值。過(guò)P 做一條平行于y 軸的射線分別交直線P1P5和P3P4于點(diǎn)P15和點(diǎn)P34，如圖3 所示。將ρcosθ 值代入直線P1P5與P3P4的方程式中，求出y15和y34的值，并判斷y15和y34的值是否滿足式（6）和式（7）：

圖3 塔吊吊鉤位置的模型Figure 3 Mode of crane hook’s position

取射線方向與y 軸同向，判斷y34＞ρsinθ，y15＞ρsinθ 記錄下交點(diǎn)的個(gè)數(shù)，從而可以判斷出P 點(diǎn)所處凸多邊形的位置。

如果目標(biāo)點(diǎn)剛好在凸多變形的一條邊上，立即執(zhí)行回轉(zhuǎn)和變幅禁止操作，報(bào)警并且介入人工輔助，察看現(xiàn)場(chǎng)的吊鉤位置和小車(chē)幅度。

4.4 防碰撞算法

（1）不完全禁止區(qū)域的防碰撞算法如下：第一步判斷塔吊當(dāng)前回轉(zhuǎn)角度θ、小車(chē)幅度值ρ 和吊鉤高度值h，障礙物的最高高度為Hbarrier，防碰撞高度余量ΔH；定義塔吊回轉(zhuǎn)逆時(shí)針旋轉(zhuǎn)方向?yàn)檎较?，順時(shí)針?lè)较驗(yàn)樨?fù)方向；塔吊小車(chē)遠(yuǎn)離塔吊安裝基座為正方向運(yùn)動(dòng)，靠近固定基座為負(fù)方向運(yùn)動(dòng)；塔吊的吊鉤上升為正方向運(yùn)動(dòng)，下降為負(fù)方向運(yùn)動(dòng)。

當(dāng)塔吊起重臂高度H、吊鉤高度h 和回轉(zhuǎn)角度θ 滿足式（8）時(shí)：

此時(shí)，塔吊產(chǎn)生預(yù)警信號(hào)?；剞D(zhuǎn)的高速檔禁用，只能最低速運(yùn)行。塔吊慢速回轉(zhuǎn)靠近障礙物。

當(dāng)塔吊起重臂高度H、吊鉤高度h 和回轉(zhuǎn)角度θ 滿足式（9）時(shí)：

此時(shí)，塔吊產(chǎn)生報(bào)警信號(hào)，回轉(zhuǎn)完全禁止操作。

當(dāng)塔吊起重臂高度H、吊鉤高度h 和回轉(zhuǎn)角度θ 滿足式（10）時(shí)：

此時(shí)，判斷小車(chē)是否在多邊形障礙物的防碰撞區(qū)域內(nèi)。如果小車(chē)在區(qū)域內(nèi)，小車(chē)幅度和吊鉤高度受限受控，不能自由運(yùn)動(dòng)，小車(chē)和吊鉤的運(yùn)動(dòng)由式（10）限制，小車(chē)和吊鉤處在比障礙物較高的高度位置，可以最慢檔實(shí)現(xiàn)跨越障礙物。如果小車(chē)在多邊形障礙物的防碰撞區(qū)域外，則計(jì)算小車(chē)距離障礙物的距離dp，L12和dp，L15,當(dāng)dp，L15＞dwarn或者dp，L12＞dwarn，小車(chē)可以做變幅運(yùn)動(dòng)，吊鉤可以做高度變化操作。當(dāng)dp，L15∈[dwarn，dalarm]或者dp，L12∈[dwarn，dalarm]時(shí)，小車(chē)幅度處于預(yù)警狀態(tài)，小車(chē)降至最低檔慢速運(yùn)行，吊鉤也降速至最低檔運(yùn)行。當(dāng)dp，L15∈（0，dalarm]或者dp，L12∈（0，dalarm]時(shí)，小車(chē)幅度處于報(bào)警狀態(tài)，小車(chē)幅度禁止操作，吊鉤上升至最高處。

同理，在扇形區(qū)域M2oM3，M3oM4和M4oM5執(zhí)行上述操作。

處于報(bào)警或者預(yù)警狀態(tài)時(shí)，小車(chē)、吊鉤和起重臂回轉(zhuǎn)運(yùn)行方向判定尤為關(guān)鍵，運(yùn)行方向判斷失誤，會(huì)造成事故擴(kuò)大。為了提高安全等級(jí)，引入小車(chē)、吊鉤和回轉(zhuǎn)運(yùn)動(dòng)方向的再確認(rèn)機(jī)制。

當(dāng)塔吊小車(chē)P 點(diǎn)（ρcosθ，ρsinθ）處于報(bào)警或者預(yù)警狀態(tài)時(shí)，采用4.3 方法判斷P 點(diǎn)的相對(duì)位置在障礙物區(qū)域外，小車(chē)P 點(diǎn)沿著塔吊起重臂正方向運(yùn)行Δρ 距離，回轉(zhuǎn)角度不變，即P 點(diǎn)坐標(biāo)為[（ρ+Δρ）cosθ，（ρ+Δρ）sinθ]，此時(shí)計(jì)算小車(chē)距離障礙物距離是否變大。如變大，為小車(chē)正確的運(yùn)行方向，此時(shí)小車(chē)才能開(kāi)始運(yùn)動(dòng)，遠(yuǎn)離障礙物。

塔吊沿著回轉(zhuǎn)正方向運(yùn)行Δθ，小車(chē)幅度不變，即P 點(diǎn)坐標(biāo)為[ρcos（θ+Δθ），ρsin（θ+Δθ）]，此時(shí)計(jì)算小車(chē)距離障礙物距離是否變大。如變大，為塔吊正確的回轉(zhuǎn)運(yùn)行方向，此時(shí)塔吊才能開(kāi)始回轉(zhuǎn)運(yùn)動(dòng)，遠(yuǎn)離障礙物。

（2）完全禁止區(qū)域的防碰撞控制流程如下：第一步判斷塔吊當(dāng)前回轉(zhuǎn)角度θ、小車(chē)幅度值ρ 和吊鉤高度值h。

當(dāng)塔吊起重臂高度H 和回轉(zhuǎn)角度θ 滿足式（11）時(shí)，塔吊產(chǎn)生預(yù)警信號(hào)，回轉(zhuǎn)的高速檔禁用，只能最低速運(yùn)行。

當(dāng)塔吊起重臂高度H 和回轉(zhuǎn)角度θ 滿足式（12）時(shí)，塔吊產(chǎn)生報(bào)警信號(hào)，回轉(zhuǎn)完全禁止操作。

當(dāng)塔吊旋轉(zhuǎn)角度θ∈（θ5+θwarn，2π-θwarn），因塔吊不在障礙物的防碰撞范圍內(nèi)，可以自由運(yùn)動(dòng)，解除防碰撞限制。

5 原型模型驗(yàn)證

實(shí)際中搭建了一個(gè)原型模型，在浙江?？悼萍加邢薰狙兄频乃醣O(jiān)控設(shè)備HKTC 型上驗(yàn)證塔吊與障礙物的防碰撞工程算法。在工地環(huán)境中，塔吊上安裝塔吊監(jiān)控設(shè)備、1 個(gè)回轉(zhuǎn)傳感器、1 個(gè)幅度傳感器和1 個(gè)高度傳感器。在地面上設(shè)置虛擬的障礙物區(qū)域，保證試驗(yàn)的安全。

實(shí)際驗(yàn)證結(jié)果表明，該算法有效地建立了相對(duì)坐標(biāo)系和障礙物區(qū)域，實(shí)現(xiàn)了預(yù)警和報(bào)警功能，有效避開(kāi)了障礙物的區(qū)域。

6 結(jié)語(yǔ)

本文建立了一種塔吊與障礙物的相對(duì)坐標(biāo)系，給出了以防碰撞距離和防碰撞角度為二元參數(shù)的防碰撞工程算法。該算法有效克服了塔吊監(jiān)控設(shè)備廠家無(wú)法或者很難獲取塔吊與障礙物的坐標(biāo)測(cè)繪圖紙的問(wèn)題，大大減少了傳感器的安裝數(shù)量，極大簡(jiǎn)化了工地現(xiàn)場(chǎng)的防碰撞算法，操作簡(jiǎn)單明了、易實(shí)現(xiàn)。工地現(xiàn)場(chǎng)的塔吊上也驗(yàn)證了該算法的有效性和實(shí)用性。