一种塔式起重机工作场地快速建模方法与流程

本发明属于智能驾驶，具体涉及一种塔式起重机工作场地快速建模方法。

背景技术：

1、塔式起重机通常由一根垂直的主塔和一个或多个水平的横臂组成，形状类似于塔。塔式起重机通过电动或液压系统来实现负载的起升、水平移动和旋转等操作。

2、塔式起重机在工作时为确保人员和其他设备与起重机保持安全距离，需对起重机工作场地进行确认，使起重机工作的场地具备足够的高度和强度，以容纳塔式起重机的高度和动作范围。

3、但是，由于塔式起重机工作环境在起吊位置和落地位置周边一定高度内为复杂环境，其中有人流和车流，以及不断产生的固定障碍物，不便于塔式起重机进行自动驾驶，因此我们需要提出一种塔式起重机工作场地快速建模方法，使其能够对塔吊下方区域环境地图进行快速构建，塔式起重机能根据建立的地图模型进行自动驾驶，提高塔吊的工作效率。

技术实现思路

1、本发明的目的在于提供一种塔式起重机工作场地快速建模方法，能够对塔吊下方区域环境地图进行快速构建，塔式起重机能根据建立的地图模型进行自动驾驶，提高塔吊的工作效率，以解决上述背景技术中提出的问题。

2、为实现上述目的，本发明采用了如下技术方案：

3、一种塔式起重机工作场地快速建模方法，包括如下步骤：

4、s1、在塔臂行车上安装一台能够多个自由度调整朝向位置且为面阵式的激光雷达，激光雷达开始工作；

5、s2、行车回到塔臂距离驾驶室的最近位置，到达指定位置后，激光雷达开始多角度扫描；

6、s3、激光雷达扫描一周后，塔臂旋转到下一个指定位置，再次进行扫描；

7、s4、扫描完成后，行车向外推进，到达下一指定位置，重复此过程，直到整个区域扫描完成，获得塔吊现场的点云数据；

8、s5、计算机对激光雷达采集塔吊现场的点云数据进行实时分析，将处理结果发送给塔吊自动驾驶系统，塔吊自动驾驶系统通过使用雷达数据建图软件实现塔吊下方区域环境地图的构建；

9、s6、在获得塔吊下方环境地图后，根据地面定位终端提供的数据，塔吊使用路径规划算法自动规划一条合理的运动路径，吊钩在吊装上货物以后会按照设计路径进行运动，期间自动避开周围的障碍物，最终到达指定位置。

10、优选的，所述激光雷达在安装时需通过雷达支架安装，所述雷达支架上安装有一台具备多个自由度能够自动调整激光雷达朝向位置的旋转云台，所述旋转云台安装在塔臂行车上。

11、优选的，所述激光雷达采用多级管式滑触线和供电滑轮相结合的形式进行供电，保证塔吊行车在运行过程中的电性元件能够持续供电。

12、优选的，步骤s2中，所述激光雷达在进行多角度扫描时，通过旋转云台旋转、左右和上下运动来改变激光雷达的朝向，以获取不同方位的点云数据。

13、优选的，步骤s3中，所述激光雷达扫描在一个指定点位扫描360°为一周，指定位置是根据塔吊工作现场环境设置的点云数据采集点，以获取整个工作环境的点云数据。

14、优选的，步骤s5中，所述点云数据在进行实时分析处理时包括对塔吊现场的点云数据进行三维变换，三维变换的步骤如下：

15、a1、点云平移

16、由以下矩阵计算三维空间中物体的平移，矩阵公式如下：

17、等价方程式为

18、a2、点云旋转

19、绕x轴旋转如下

20、θ为旋转角

21、绕y轴旋转如下

22、θ为旋转角

23、绕z轴旋转如下

24、θ为旋转角

25、设任意轴l上有一点q(xq,yq,zq)，此轴的方向余弦为(a，b，c)，则将空间中一点p(x,y,z)通过绕此轴旋转θ角而得到点p(x'，y'，z')，其变换矩阵r可通过如下几个变换步骤求出：平移坐标点使q点为新坐标原点，t为

26、

27、在新坐标中，将轴l绕x轴旋转a角，其变换矩阵为rx

28、其中

29、在新坐标中，将轴l绕y轴旋转β角，其变换矩阵为ry

30、其中

31、最后得到点云绕任意轴l旋转的总的变换矩阵r为

32、

33、a3、点云缩放

34、在三维齐次坐标中，任意一点p(x,y,z)由以下矩阵运算而获得p(x',y',z')，如下式所示：

35、

36、其中，参数，sy，sz，为指定的在三个坐标轴方向上的缩放比例。

37、优选的，在对塔吊下方区域环境地图构建时包括使用icp算法对塔吊雷达扫描得到的三维点云数据拼接生成场地三维点云全景地图，具体计算过程如下：

38、1)给出两个点集，第一个点集{pi|pi∈r3,i＝1,2,…,np}，用p表示，第二个点集

39、{xi|xi∈r3,i＝1,2,…,nx}用x表示；

40、2)迭代初始化，设p0＝p,q0＝[1,0,0,0,0,0,0]和k＝0，配准矢量相对初始点集p0被定义，以便最终的配准能代表完整的变换，重复以下步骤，直到迭代收敛于给定的公差τ；

41、stepl：最近点计算：yk＝c(pk,x)；

42、step2：配准计算：(qk,dk)＝q(p0,yk)；

43、step3：运用配准：pk+t＝q(p0,yk)；

44、step4：当最小二乘误差小于事先设定的阈值dk-dk+1＜τ,τ＞0时，迭代终止。

45、通过以上迭代公式计算，可将塔吊上雷达扫描的结果进行拼接，获得塔吊施工区域整体环境点云图，后期可根据此点云图规划运动路径以及分析施工环境特征。

46、优选的，在迭代计算中，需要计算点到给定实体的最近距离；

47、b1、设a表示一个含有na个点的数据点集a＝{ai},i＝1,2,…,na，点p到点集a的距离为

48、

49、a中最近的点ai满足等式d(p,a)＝d(p,a)；

50、b2、点到线段之间的距离

51、设l是包含n1条线段的集合，l＝{li},i＝1,2,…n1，点p到l的距离表示为

52、

53、l中最接近的点yj满足等式d(p,y)＝d(p,l)

54、b3、点到三角面片之间的距离

55、设t是包含nt个三角面片的集合，t＝{ti},i＝1,2,…nt点p到t的距离表示为

56、

57、t中最近的点gj满足等式d(p,gj)＝d(p,t)

58、b4、点到参数实体的距离

59、参数曲线和曲面被处理为单参数实体r(u)对参数曲线u＝u∈r'，参数曲面u＝(u,v∈r2)，一个给定点p到参数实体e的距离可表示为：

60、

61、优选的，在塔吊下方区域环境地图的构建时，采用建模数据历史叠加处理，算法采用迭代基准化，后期建模数据范围扩大阈值化处理判定，如果建模数据随高度增加建模数据较历史数据向外延伸，则追溯历史数据迭代无变化边缘外延数据做为该区域准确高度以及范围数据，未锁定区域继续迭代更新建模数据。

62、本发明提出的一种塔式起重机工作场地快速建模方法，与现有技术相比，具有以下优点：

63、本发明通过激光雷达开始工作；行车回到塔臂距离驾驶室的最近位置，到达指定位置后，旋转云台左右、上下旋转，同时激光雷达开始进行扫描，激光雷达扫描一周后，塔臂旋转到下一个指定位置，旋转云台与激光雷达重复上述过程，再次进行扫描；扫描完成后，行车向外推进，到达下一个指定位置，重复此过程，直到整个区域扫描完成；计算机会对激光雷达采集塔吊现场的点云数据进行实时分析处理，能够对塔吊下方区域环境地图进行快速构建，塔式起重机能根据建立的地图模型进行自动驾驶，提高塔吊的工作效率。