塔吊与障碍物的防碰撞算法研究

吕军，缪泽峰，王威滔，丁双定，吴缙峰

（1.中电海康集团有限公司，浙江 杭州 311100；2.浙江海康科技有限公司，浙江 杭州 311100）

0 引言

塔吊是建筑工地上必不可少的一种起重设备，主要用于建筑施工中物料的垂直和水平输送，具有使用范围广、回转半径大、起升高度高和施工效率高的特点。塔吊工作于多样复杂的施工场合中，由于其工作高度高、塔臂跨度大等特点，作业覆盖面很广，又因工地障碍物的多样性，存在塔吊与障碍物之间发生碰撞的可能性。预防塔吊与障碍物的碰撞是塔吊安全监控的重中之重[1-3]。

文献[4]提出一种基于空间三维定位技术的塔吊防撞系统设计，安装在不同位置的数据传感器对数据采集后，经过无线网络传输到上位机的控制中心，使用空间三维定位技术对传感器的信息进行数学计算，通过数学模型达到防撞告警的目的。

文献[5]对单台塔机的定位问题和塔机间相对位置关系问题进行研究，提出了塔机防碰撞控制算法的设计思路与软件实现方法。

文献[6]研究了一种多传感器感知融合的塔吊防碰算法，该算法需要在塔吊上布置多个传感器获取大量冗余信息，验证多传感器信息融合理论在塔机防碰撞中的可行性和优越性。

针对目前的研究现状，上述文献[4-6]给出了算法设计的基本原理，但在算法实现方面并没有可执行的详细设计流程，实际工程上缺乏可行的操作办法。本文建立了塔吊与障碍物的相对坐标系系统，给出以防碰撞距离和防碰撞角度为二元参数的防碰撞工程算法。该算法有效克服了塔吊监控设备厂家无法或者很难获取塔吊与障碍物的坐标测绘图纸的问题，大大减少了传感器的安装数量，极大简化了工地现场的防碰撞算法，操作简单明了、易实现。

1 塔吊坐标系

以塔吊所在的安装基座为坐标原点o，塔吊起重臂为x轴，垂直塔吊起重臂的法向为y轴，塔身为z轴，建立以右手法则定义的的空间直角坐标系。障碍物的数学模型简化为一段直线或者由几段直线围合的区域。为了工程计算的简便，建立如图1 所示的相对于障碍物的坐标系x＇oy＇。该坐标系也以塔基基座为原点o，障碍物的一端边界的端点为P1，以oP1为x＇轴，oP1轴的法向为y＇轴。障碍物的另一边界端点为P2。

图1 塔吊相对坐标系Figure1 Relative coordinate system of tower crane

设oP1和oP2的长度分别为ρ1和ρ2，oP1和oP2与x 轴的夹角分别为θ1和θ2，如果将xoy 坐标系相对障碍物逆时针旋转θ1，形成了新的相对坐标系x＇oy＇。在x＇oy＇坐标系下，记Δθ=θ2-θ1。P1和P2的坐标分别简化为（ρ1，0）和（ρ2cosΔθ，ρ2sinΔθ）。

设P1P2直线方程为Ax+By+C=0，其中A=ρ2sinΔθ，B=ρ1-ρ2cosΔθ，C=-ρ1ρ2sinΔθ。在x＇oy＇坐标系中任意一点P（x，y）到直线P1P2的距离d 为：

2 预警和报警距离

在工程上，为了让塔吊避让障碍物，往往设定预警距离dwarn和报警距离dalarm，一般有dwarn＞dalarm，即预警距离要大于报警距离。

塔吊起重臂上的P（x，y）到直线P1P2的防碰撞距离d 满足d＞dwarn时，塔吊可以自由运动；当dalarm＜d≤dwarn时，处于预警状态；当d≤dalarm时，处于报警状态。

设塔吊的最大回转半径ρmax，单位为m。塔吊的变幅速度为v，单位为m/min，回转速度为n，单位为r/min。塔吊监控设备的程序扫描周期设为T，单位ms。则预警距离dwarn和报警距离dalarm满足以下式（2）和式（3）：

塔吊的运动形式包括吊钩的上升和下降运动、小车沿着起重臂来回运动的变幅运动、起重臂绕着塔吊基座旋转的回转运动这3 种运动方式。式（1）和式（2）限定了小车的变幅运动范围，小车在进行变幅运动时，如果小车超出了式（2）的限定范围，则存在小车动态碰撞的重大风险。式（3）限定了塔吊回转运动范围，超出式（3）的限定范围也会造成动态碰撞风险。

为了更直观研究回转时的防碰撞距离，引入防碰撞角度，分别为预警角度θwarm和报警角度θalarm。图1 中P2N 和P2M 的距离分别为预警距离dalarm和报警距离dwarn，有：

塔吊起重臂回转角度为θ，塔吊向障碍物禁止区域方向行进时，即将触碰到障碍物的边缘，满足θ＞θwarn时，塔吊可以自由运动；当θalarm＜θ≤θwarn时，处于预警状态；当θ≤θalarm时，处于报警状态。

3 障碍物的数学模型

选取塔吊安装位置的固定基座水平面为参考水平面，塔吊起重臂的高度为H，塔吊吊钩高度为h。如图2 所示障碍物的边界点分别为P1，P2，P3，P4和P5。以P1，P2，P3，P4和P5包围区域为障碍物的区域，该区域内因有障碍物，塔吊运行到此区域时必须注意防碰撞的问题。

图2 障碍物模型Figure 2 Mode of construction site’s obstacles

由于实际工况的复杂性，障碍物在x＇oy＇水平面上，形状各异，也存在障碍物高度不同的情况。可以通过近似分割的原理，将复杂的障碍物简化为凸多边形的障碍物区域。如果实际中遇到障碍物区域是凹多边形，则可以通过分割方法将其分割为由几个凸多边形的障碍物区域组成。

4 防碰撞工程算法

利用上述的防碰撞模型和障碍物的数学模型，提出工程实际应用上的一种快捷便利的算法。以P1，P2，P3，P4和P5包围区域为障碍物的区域为例，具体阐述防碰撞工程算法。

4.1 建立相对坐标系

观察障碍物的边界点，选取图2 所示的oP1为x＇轴，起重臂的法向方向为y＇轴，逆时针旋转角度记为θ。工程上，司机驾驶塔吊收回小车，将吊钩上升至最高高度，采用最慢档速度缓慢地使塔吊起重臂回转到障碍物P1的临近位置，标记为x 轴。

4.2 测量障碍物边界点

（1）当障碍物的高度低于塔吊起重臂高度H时，标记P1P2P3P4P5区域为不完全禁止区域，即塔吊的起重臂可跨越障碍物或者吊钩可跨越障碍物区域进行作业。

不完全禁止区域的边界确认方法，需确定边界点P1，P2，P3，P4和P5的坐标。

第一步，按照4.1 的方法，建立坐标系；

第二步，将小车缓缓向外驶出，接近P1点时记下幅度传感器的幅度值ρ1，即P1坐标为（ρ1，0）；

第三步，将塔吊逆时针从P1点回转到P5，回转传感器测量出的旋转角度为θ5，塔吊的幅度传感器测量出幅度为ρ5，即P5坐标为（ρ5cosθ5，ρ5sinθ5）；

第四步，依次测量出P4，P3和P2对应的回转角度θ4，θ3，和θ2以及幅度传感器的幅度值ρ4，ρ3和ρ2，即P4，P3和P2坐标分别为（ρ4cosθ4，ρ4sinθ4）、（ρ3cosθ3，ρ3sinθ3）和（ρ2cosθ2，ρ2sinθ2）。

至此可按式（1）计算出任意点P 到直线P1P2，P2P3，P3P4，P4P5和P1P5的距离，分别记为dp，L12，dp，L23，dp，L34，dp，L45和dp，L15；

（2）当障碍物的高度超过塔吊起重臂高度H时，标记P1P2P3P4P5多边形区域为完全禁止区域，即塔吊的起重臂不可跨越障碍物在该区域内作业。

完全禁止区域的边界确认方法，较为简单，只需确定边界点P1和P5。

第一步，按照4.1 的方法，建立坐标系；

第二步，将小车缓缓向外驶出，接近P1点时记下幅度传感器的幅度值ρ1，即P1坐标为（ρ1，0）；

第三步，因障碍物限制，塔吊起重臂只能顺时针旋转，临近障碍物的P5点，采用最慢档缓慢靠近P5点，回转传感器测量出的旋转角度为α；塔吊的幅度传感器测量出幅度为ρ5；

第四步，计算点P5的角度θ5=2π-α，即P5坐标为（ρ5cosθ5，ρ5sinθ5）。

至此可计算出任意点P 到直线P1P5的距离，记为dp，L15。

4.3 塔吊吊钩与障碍物区域的相对位置

从目标点出发引一条射线，看这条射线和多边形所有边的交点数目。如果有奇数个交点，则说明目标点在凸多边形内部；如果有偶数个交点，则说明目标点在凸多边形外部。

设塔吊小车P 点坐标为（ρcosθ，ρsinθ），P 点坐标值通过回转传感器和幅度传感器获得，是已知值。过P 做一条平行于y 轴的射线分别交直线P1P5和P3P4于点P15和点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θ 记录下交点的个数，从而可以判断出P 点所处凸多边形的位置。

如果目标点刚好在凸多变形的一条边上，立即执行回转和变幅禁止操作，报警并且介入人工辅助，察看现场的吊钩位置和小车幅度。

4.4 防碰撞算法

（1）不完全禁止区域的防碰撞算法如下：第一步判断塔吊当前回转角度θ、小车幅度值ρ 和吊钩高度值h，障碍物的最高高度为Hbarrier，防碰撞高度余量ΔH；定义塔吊回转逆时针旋转方向为正方向，顺时针方向为负方向；塔吊小车远离塔吊安装基座为正方向运动，靠近固定基座为负方向运动；塔吊的吊钩上升为正方向运动，下降为负方向运动。

当塔吊起重臂高度H、吊钩高度h 和回转角度θ 满足式（8）时：

此时，塔吊产生预警信号。回转的高速档禁用，只能最低速运行。塔吊慢速回转靠近障碍物。

当塔吊起重臂高度H、吊钩高度h 和回转角度θ 满足式（9）时：

此时，塔吊产生报警信号，回转完全禁止操作。

当塔吊起重臂高度H、吊钩高度h 和回转角度θ 满足式（10）时：

此时，判断小车是否在多边形障碍物的防碰撞区域内。如果小车在区域内，小车幅度和吊钩高度受限受控，不能自由运动，小车和吊钩的运动由式（10）限制，小车和吊钩处在比障碍物较高的高度位置，可以最慢档实现跨越障碍物。如果小车在多边形障碍物的防碰撞区域外，则计算小车距离障碍物的距离dp，L12和dp，L15,当dp，L15＞dwarn或者dp，L12＞dwarn，小车可以做变幅运动，吊钩可以做高度变化操作。当dp，L15∈[dwarn，dalarm]或者dp，L12∈[dwarn，dalarm]时，小车幅度处于预警状态，小车降至最低档慢速运行，吊钩也降速至最低档运行。当dp，L15∈（0，dalarm]或者dp，L12∈（0，dalarm]时，小车幅度处于报警状态，小车幅度禁止操作，吊钩上升至最高处。

同理，在扇形区域M2oM3，M3oM4和M4oM5执行上述操作。

处于报警或者预警状态时，小车、吊钩和起重臂回转运行方向判定尤为关键，运行方向判断失误，会造成事故扩大。为了提高安全等级，引入小车、吊钩和回转运动方向的再确认机制。

当塔吊小车P 点（ρcosθ，ρsinθ）处于报警或者预警状态时，采用4.3 方法判断P 点的相对位置在障碍物区域外，小车P 点沿着塔吊起重臂正方向运行Δρ 距离，回转角度不变，即P 点坐标为[（ρ+Δρ）cosθ，（ρ+Δρ）sinθ]，此时计算小车距离障碍物距离是否变大。如变大，为小车正确的运行方向，此时小车才能开始运动，远离障碍物。

塔吊沿着回转正方向运行Δθ，小车幅度不变，即P 点坐标为[ρcos（θ+Δθ），ρsin（θ+Δθ）]，此时计算小车距离障碍物距离是否变大。如变大，为塔吊正确的回转运行方向，此时塔吊才能开始回转运动，远离障碍物。

（2）完全禁止区域的防碰撞控制流程如下：第一步判断塔吊当前回转角度θ、小车幅度值ρ 和吊钩高度值h。

当塔吊起重臂高度H 和回转角度θ 满足式（11）时，塔吊产生预警信号，回转的高速档禁用，只能最低速运行。

当塔吊起重臂高度H 和回转角度θ 满足式（12）时，塔吊产生报警信号，回转完全禁止操作。

当塔吊旋转角度θ∈（θ5+θwarn，2π-θwarn），因塔吊不在障碍物的防碰撞范围内，可以自由运动，解除防碰撞限制。

5 原型模型验证

实际中搭建了一个原型模型，在浙江海康科技有限公司研制的塔吊监控设备HKTC 型上验证塔吊与障碍物的防碰撞工程算法。在工地环境中，塔吊上安装塔吊监控设备、1 个回转传感器、1 个幅度传感器和1 个高度传感器。在地面上设置虚拟的障碍物区域，保证试验的安全。

实际验证结果表明，该算法有效地建立了相对坐标系和障碍物区域，实现了预警和报警功能，有效避开了障碍物的区域。

6 结语

本文建立了一种塔吊与障碍物的相对坐标系，给出了以防碰撞距离和防碰撞角度为二元参数的防碰撞工程算法。该算法有效克服了塔吊监控设备厂家无法或者很难获取塔吊与障碍物的坐标测绘图纸的问题，大大减少了传感器的安装数量，极大简化了工地现场的防碰撞算法，操作简单明了、易实现。工地现场的塔吊上也验证了该算法的有效性和实用性。