自动驾驶塔机的场景建模及运行路径规划研究

张 志，文朝辉，李 炜

（湖北江汉建筑工程机械有限公司，湖北 荆州 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 中的路径点完成塔机的自动控制，实现了塔机的无人驾驶。