基于3D激光的集装箱码头场桥防碰撞控制系统及方法与流程

基于3d激光的集装箱码头场桥防碰撞控制系统及方法
技术领域
1.本发明涉及一种基于3d激光的集装箱码头场桥防碰撞控制系统及方法。


背景技术：

2.近年来，随着我国智慧港口建设的持续推进，作为场桥中的重要机械：轮胎式集装箱门式起重机(rtg)和轨道式集装箱门式起重机(rmg)，其作业安全和效率也有了更高的要求。在场桥集装箱装卸作业时，由于司机室的位置相对较高，受照明条件的限制以及吊具及其负载集装箱对司机视野的遮挡影响，使得司机在作业过程中存在视野盲区，无法及时判断下方集装箱的堆码情况。特别是在吊具负载集装箱的情况下，当司机在没有及时观察到集装箱堆码的箱高的情况时，很容易出现吊具及其负载集装箱与堆场内当前作业贝位码放的集装箱以及左右相邻贝位码放的集装箱发生碰撞事故，导致更多集装箱的倾倒，进而造成货物的损失，同时给人身安全带来巨大的隐患。
3.目前，现有的集装箱码头rtg/rmg防碰撞控制系统及方法主要有：
4.1)基于gnss定位和tos作业指令信息的场桥防碰撞系统，该类系统主要是通过利用差分定位技术(rtk)对大车位置进行定位，根据大车位置从码头的tos系统中获取当前堆场作业贝位的集装箱堆码信息，然后结合小车和吊具的编码器信息，判断吊具及其负载集装箱与当前作业贝位集装箱堆码轮廓的位置关系，进而通过控制rtg/rmg起升和小车的运行速度，实现防碰撞保护。该方法依赖的gnss数据极易受堆场集装箱、岸桥及码头高大建筑物等物体的遮挡导致定位数据不准，系统无法工作；同时当码头tos系统中的集装箱堆码信息与实际不符时，系统也无法正确防护；并且无法实现相邻贝位集装箱的防碰撞保护。
5.2)基于单个或者多个2d激光扫描仪的场桥防碰撞系统，该系统通过在rtg/rmg上装设标识物实现对2d激光扫描仪的定位，进而通过2d激光扫描仪扫描的本贝位集装箱轮廓数据实现对集装箱堆码信息的获取，然后结合rtg/rmg自身的小车和起升编码器数据判断吊具及其负载集装箱和本贝位集装箱堆码轮廓的位置关系，进而通过控制rtg/rmg起升和小车的运行速度，实现防碰撞保护。该方法需要在rtg/rmg自身机构上安装固定特征的标识物，不易实施的同时可能会对rtg/rmg设备自身机构运动产生影响；另该方法采用2d激光扫描仪无法获取邻贝位集装箱的堆码信息，进而也无法实现相邻贝位集装箱的防碰撞保护。
6.3)利用单个2d激光和单个或多个3d激光作为主要传感器的防撞系统，该类方法在利用2d激光的方案基础上增加3d激光用于对邻贝位集装箱轮廓信息进行扫描，进而再实现本贝位防撞保护的同时实现邻贝的防撞保护，但该方法在对小车进行定位时，仍需要在rtg/rmg自身机构上安装固定特征的标识物，同时该方法通过叠加传感器，极大增加了系统的成本。
7.基于以上情况，本发明以3d激光作为主要传感器，充分利用3d激光感知范围大的优点，提出基于3d激光的lidar slam的方法，对rtg/rmg的小车进行实时定位，实现不依赖特征标识物的同时有效控制硬件成本，实现对rtg/rmg堆场本贝位和相邻贝位集装箱的防撞保护。


技术实现要素：

8.为了克服现有技术的上述缺点，本发明提出了一种基于3d激光的集装箱码头场桥防碰撞控制系统及方法。
9.本发明解决其技术问题所采用的技术方案是：一种基于3d激光的集装箱码头场桥防碰撞控制系统，包括设置在大车上的小车，以及设置在小车上的3d激光和吊具，在小车的司机室中设置场桥plc和控制器，在大车的联系梁上设置有电气房，其中：所述场桥plc将吊具的起升编码器数据和小车的实时坐标数据发送给控制器，所述3d激光将采集的激光点云数据发送给控制器，所述控制器对3d激光和场桥plc提供的数据进行融合，对小车进行实时定位、计算吊具的实时位置、实时速度和集装箱顶面的角点坐标值，并在发生碰撞危险时向场桥plc发出控制指令，对当前贝位和相邻贝位的集装箱进行防碰撞保护。
10.本发明还提供了一种基于3d激光的集装箱码头场桥防碰撞控制方法，包括如下步骤：
11.步骤一、3d激光的参数标定；
12.步骤二、集装箱堆场rtg/rmg小车的实时定位；
13.步骤三、集装箱顶面角点的提取；
14.步骤四、根据实时定位的小车位置信息和场桥plc发送的吊具起升编码器的值计算吊具的实时位置和实时速度，利用提取出的集装箱顶面角点的坐标值进行判断，在发生碰撞危险时向场桥plc发出控制指令，对当前贝位和相邻贝位的集装箱进行防碰撞保护。
15.与现有技术相比，本发明的积极效果是：
16.本发明在对于rtg/rmg的小车实时定位时不再使用gnss技术以及具有特定特征的标识物，而是采用改进的loam算法实现对小车的实时定位，同时实现本贝和邻贝的防碰撞保护。本发明的系统结构更为简洁，没有使用特定的标识物，避免了额外的成本开销。
附图说明
17.本发明将通过实例并参照附图的方式说明，其中：
18.图1是本发明系统的整体安装结构主视框图；
19.图2是本发明中rtg/rmg小车运动结构侧视框图；
20.图3是本发明的基于标靶球的激光外参标定；
21.图4是本发明中基于激光lidar slam小车定位算法流程图；
22.图5是本发明中点云数据处理算法主要流程图。
具体实施方式
23.本发明提出了一种基于3d激光的集装箱码头场桥防碰撞控制系统及方法，所用到的3d激光个数至少为一台，可安装在小车的底部。两台为最佳，可安装在小车底部的左、右两侧，一前、一后安装。多台时，可根据现场实际情况进行安装。本发明解决上述系统问题所采用的技术方案是：首先，利用标靶球检测的方法配准两台3d激光获得外参数据：旋转矩阵r3×3和平移矩阵t3×1。将两台3d激光数据配准到同一个坐标系下，并将所有点云数据偏移到小车中心。其次，根据配准和偏移之后的点云数据，利用改进的loam(lidar odometry and mapping in real-time.)算法对小车进行实时定位，计算得到小车实时位置。再次，对点云
数据进行处理，利用平面检测的方法提取集装箱的顶面数据并计算顶面的四个角点信息，利用集装箱的长度、宽度等信息剔除掉干扰的面数据。最后，根据实时计算的集装箱顶面的角点坐标值、小车的位置信息、场桥plc发送的吊具起升编码器的值，对当前贝位和相邻贝位的集装箱进行防碰撞保护。
24.下面将结合本发明中的附图对本发明中的系统进行具体的描述，本实例以两台3d激光进行实验说明：
25.图1、图2为本发明的基于3d激光的集装箱码头场桥防碰撞控制系统的整体安装结构框图，包括：大车1、小车2、第一台3d激光3、集装箱4、司机室5、场桥plc 6、吊具7、控制器8、第二台3d激光9、电气房10、联系梁11等，其中：第一台3d激光3、第二台3d激光9分别安装在小车2的底部，并以吊具7为中心成对角线安装。司机室5中安装有plc 6和控制器8，plc 6将提供吊具7的起升数据并接收小车2的坐标值等信息，控制器8发布控制指令，控制plc 6执行指令。本发明通过将两台3d激光数据和plc提供的小车的实时位置数据、吊具的起升编码器数据等进行融合，计算吊具7的实时位置和实时速度，并通过本发明中提供的方法计算本贝位和相邻贝位集装箱4的角点坐标值，获得集装箱的位置信息。在大车1、小车2的作业过程中，当吊具7吊有集装箱，与本贝位或者相邻贝位的集装箱有发生碰撞的危险时，本发明提供的防碰撞方法能够实时对rtg/rmg进行防碰撞保护，实现安全警告，从而避免事故的发生，增强系统的安全性。
26.本发明的基于3d激光的集装箱码头场桥防碰撞控制方法的主要流程如下：
27.1.3d激光的参数标定
28.1.1 3d激光的内参标定
29.3d激光的内参标定指的是对于安装之后的3d激光的三个欧拉角(yaw,pitch,roll)进行标定，本发明通过自动标定与手动标定相结合的方式来逐个标定3d激光。
30.1.1.1 3d激光内参的自动标定
31.3d激光的自动标定主要包括：1》在没有集装箱的堆场中采集相对较为平整的地面点云数据。2》对地面点云数据进行平面拟合(最小二乘法、ransac等)，得到拟合平面的法线，根据法线计算俯仰角pitch和翻滚角roll(atan值)。
32.1.1.2 3d激光内参的手动微调
33.因为拟合误差、计算误差的存在，只用自动标定的方法不能完全满足内参标定精度的需要，因此还需要对其进行手动的标定，具体可根据3d激光所自带的调试软件进行微调以满足精度的需求。
34.1.2 3d激光的外参标定
35.参照图3所示，3d激光的外参标定的目的是将两台3d激光(3、9)采集的点云数据配准到一个激光的坐标系下，来获得坐标统一的堆场点云数据，本发明中采用通用的点云标靶球(12、13、14、15)检测的方式对激光的外参进行两两标定得到旋转矩阵和平移矩阵t3×1，3d激光通过激光扫描线16，获得堆场内的两个标靶球的点云数据并计算旋转和平移矩阵，即：
[0036][0037]
其中：r＝r
zryrx
,t＝[t
x t
y tz]
t
[0038][0039]
r为旋转矩阵，α,β,γ分别为待配准的3d激光坐标系与参考的3d激光坐标系沿着(xc,yc,zc)轴方向的偏转角度。t为平移矩阵，表示的是两坐标系之间的相对位置关系。通过选取两台3d激光中的一台作为基准，另一台激光作为被配准对象，通过两两计算旋转、平移矩阵，得到堆场中的统一的坐标数据。
[0040]
2.集装箱堆场rtg/rmg小车的实时定位
[0041]
2.1点云数据的配准与偏移
[0042]
针对两台3d激光分别获取的点云数据，按照公式(1)中的步骤对两台3d激光的数据进行配准。为了后续计算的方便和对所有的点云数据进行全局统一的坐标表示，将配准之后的点云数据进行偏移，偏移到小车中心点处。即：
[0043][0044]
其中：(x0,y0)为完成配准的点云坐标，(o
x_offset
,o
y_offset
)是作为基准的3d激光到小车中心在x轴、y轴的偏移值。(x
sc
,y
sc
)为偏移到小车中心之后的坐标。
[0045]
2.2基于激光lidar slam的小车实时定位
[0046]
本发明系统中采用基于激光lidar slam的技术实现对小车的实时定位，参照图4、5所示，堆场中rtg/rmg的小车在固定的位置运行，其运行开始位置、运动方向、总长度、轨迹作为先验地图信息输入到控制器中。本发明中用到的3d激光的点云数据容易受到运动目标的干扰，并且存在同一帧点云数据之间彼此的采集时间存在差异，导致点云扭曲等等问题。针对上述问题，本发明在参照已有loam算法的基础之上做了改进，利用rtg/rmg这一特定的作业场景中的特定物体(电气房、联系梁等)，对此类固定物体进行实时检测，避免使用地面、集装箱等易受遮挡或会在作业过程中发生移动的物体。回环检测阶段加入这些特定物体提取的平面、边缘点坐标信息用以增强回环检测的精度，以消除点云匹配带来的累积误差。主要处理流程参照图4所示。
[0047]
2.2.1点云的预处理
[0048]
针对经过配准和偏移之后的点云数据，在进行后续的处理之前需要对点云进行滤波预处理，以得到可用点云数据，滤波因素包括：点云的偏转角度、反射强度、入射角度。
[0049]
由于3d激光的fov的限制，对于扫描视场角边缘区域的点云数据应该摒弃，参见公式(3)。
[0050]
点point
(x,y,z)
的偏转角度计算:
[0051][0052]
点云强度过大或过小的点云数据也会造成影响,参见公式(4)、(5)。
[0053]
点point
(x,y,z)
的深度计算:
[0054][0055]
点point
(x,y,z)
的强度信息为:
[0056]ipoint
＝r
point
/dis
point (5)
[0057]
其中：r
point
为点云的反射强度，3d激光可以测量得到该值。入射角度为0或者为π时，此处的点为噪声点云数据，参见公式(6)。
[0058]
点point
(x,y,z)
入射角度θ为:
[0059][0060]
其中：(pa,pb,pc)为连续的三个点坐标。
[0061]
基于以上滤波准侧，对配准、偏移之后的每帧数据进行上述处理，得到滤波之后的“可用点”。
[0062]
2.2.2点云的特征提取
[0063]
本发明中采用loam算法中通用的点云特征提取方法，根据曲率来计算平面的光滑度作为提取当前帧的特征信息的指标:
[0064][0065]
其中：c代表该点的光滑程度，代表的是在集合s内点i、j的曲率。通过计算一个集合s中的所有点之间的关系来判断点的特征：平面点、边缘点。平面点曲率较低，平滑度较低。边缘点，曲率较高，平滑度较高。
[0066]
对于rtg/rmg中的电气房、联系梁这些拥有明显平面特征的物体，在原始点云中预设一定的高度(h
limit
)、截取特定的范围(x
limit
,y
limit
)内的点云数据，并采用滤波算法对其进行切割再按照上述公式(7)，获得电气房、联系梁的平面点和边缘点。
[0067]
2.2.3迭代位姿优化
[0068]
本发明中分别用一种迭代的姿态优化程序来解算lidar位姿，可以实现实时的里程计和建图频率达到20hz以上。位姿的迭代优化包括：点与线的残差和点与面的残差。
[0069]
点到线的残差，使用εk、εm分别代表当前帧和当前地图的所有边线特征，对于每一个εk中的点，在εm中寻找它的最近邻点(5个)。现假设p
l
是εk中的一个点，在εm中的点是经过旋转、平移、偏移之后的全局坐标系，所以需要对其进行上述步骤1.2、2.1中的变换操作。假设pi是点p
l
在εm上的投影点坐标pw的第i个最近邻点。计算5个点的均值μ和协方差矩阵σ。当σ的最大特征值比第二大特征值大3倍时，则点pi为一条直线上的点。进而计算关联点到边线的残差，将残差加入位姿优化当中。点到线的残差计算公式为：
[0070]
[0071]
点到面的残差与点到线的残差计算公式近似：
[0072][0073]
其中，pw表示的是点w的点坐标。
[0074]
2.2.4帧内间的运动补偿
[0075]
随着3d激光的运动，同一帧采集的点云数据在采集时间上存在差异，使点云运动呈现模糊状态。为了消除影响将传入的一帧数据分成三段数据来处理，即可消除此误差的影响。并将步骤2.2.2中提取的电气房、联系梁此类具有明显平面特征的物体，提取出的平面、边缘点云数据加入到位姿的优化中，做整体的姿态优化，实时解算小车的位姿。
[0076]
2.2.5外点剔除，动态目标剔除
[0077]
在更新每次的位姿优化迭代时，重新查找每个特征点的最邻近点，并将点到线残差和点到面残差加入到目标优化函数当中。先执行少数几次的迭代位姿优化。利用计算的优化结果，计算两种残差，去除前20％最大的残差值，再继续进行后续的迭代位姿优化。
[0078]
每个新帧都直接与全局地图(小车的开始位置、运动方向、总长度提前在控制器中给出)匹配，并提供里程计输出(当前位置在整个全局地图中的位置)。匹配的位姿结果被用来将帧注册到全局地图，使得里程计输出和地图更新的速率相同，根据里程计对小车的位置进行实时的计算，以此实现对rtg/rmg小车的定位。
[0079]
3.集装箱顶面角点的提取
[0080]
本发明中对于偏移之后的点云数据按照图5中s1步骤采用欧式聚类的方法对点云数据做聚类处理，得到整个堆场中集装箱点云的聚类分割数据，然后再按照图5中s2的方法根据地面点的高度，集装箱不同层(第一层、第二层、第三层等)的高度数据，对于一些聚类的比较小的平面点云块进行剔除，只保留符合条件的集装箱的顶面数据。最后，按照图5中的s3步骤计算得到集装箱顶面角点信息。
[0081]
4.基于集装箱顶面角点提取的rtg/rmg防碰撞保护
[0082]
本发明采用的防护策略为：结合小车和吊具的实时运动状态，根据吊具以及所带集装箱与堆场内码放的本贝及相邻贝位存在碰撞风险集装箱的最小水平距离和最小高度差，分别对小车和吊具进行动态限速，根据预先获得的小车在全速运行状态下的速度vq和运行距离sq(从全速vq减速为0时，小车的运行距离)，通过实时计算上述最小水平距离s
t
，动态地调节小车的运行速度v
t
,即：
[0083][0084][0085]
通过公式(11)，对小车做动态的速度调整，当最小距离小于40cm时，v
t
降为0，从而达到对本贝及邻贝集装箱防碰撞保护的目的。

技术特征：
1.一种基于3d激光的集装箱码头场桥防碰撞控制系统，其特征在于：包括设置在大车上的小车，以及设置在小车上的3d激光和吊具，在小车的司机室中设置场桥plc和控制器，在大车的联系梁上设置有电气房，其中：所述场桥plc将吊具的起升编码器数据和小车的实时坐标数据发送给控制器，所述3d激光将采集的激光点云数据发送给控制器，所述控制器对3d激光和场桥plc提供的数据进行融合，对小车进行实时定位、计算吊具的实时位置、实时速度和集装箱顶面的角点坐标值，并在发生碰撞危险时向场桥plc发出控制指令，对当前贝位和相邻贝位的集装箱进行防碰撞保护。2.根据权利要求1所述的基于3d激光的集装箱码头场桥防碰撞控制系统，其特征在于：所述3d激光安装在小车的底部。3.根据权利要求1所述的基于3d激光的集装箱码头场桥防碰撞控制系统，其特征在于：所述3d激光为两台，以吊具为中心成对角线安装在小车的底部。4.一种基于3d激光的集装箱码头场桥防碰撞控制方法，其特征在于：包括如下步骤：步骤一、3d激光的参数标定；步骤二、集装箱堆场rtg/rmg小车的实时定位；步骤三、集装箱顶面角点的提取；步骤四、根据实时定位的小车位置信息和场桥plc发送的吊具起升编码器的值计算吊具的实时位置和实时速度，利用提取出的集装箱顶面角点的坐标值进行判断，在发生碰撞危险时向场桥plc发出控制指令，对当前贝位和相邻贝位的集装箱进行防碰撞保护。5.根据权利要求4所述的基于3d激光的集装箱码头场桥防碰撞控制方法，其特征在于：步骤一中对3d激光的内参的标定方法为：对采集的无集装箱堆场的地面点云数据进行平面拟合，得到拟合平面的法线，再根据法线计算俯仰角和翻滚角；对3d激光的外参的标定方法为：在堆场内设置四个标靶球，3d激光通过激光扫描线获得堆场内的标靶球点云数据，以一台3d激光作为基准，另一台3d激光作为被配准对象，通过两两计算旋转、平移矩阵，得到堆场中统一的坐标数据。6.根据权利要求4所述的基于3d激光的集装箱码头场桥防碰撞控制方法，其特征在于：步骤二中对小车的实时定位方法包括：第一步、对两台激光获取的点云数据进行配准，并将配准之后的点云数据偏移到小车中心点处；第二步、对配准和偏移之后的点云数据进行滤波处理，得到可用点云数据；第三步、对可用点云数据进行特征提取，获得电气房、联系梁的平面点和边缘点；第四步、对点与线的残差和点与面的残差进行迭代位姿优化、帧内的运动补偿、外点的剔除和动态目标滤除；第五步、将每帧的优化结果直接与全局地图匹配，并提供里程计输出；然后将匹配的位姿结果用来将帧注册到全局地图，使得里程计输出和地图更新的速率相同，根据里程计对小车的位置进行实时计算，以此实现对小车的实时定位。7.根据权利要求6所述的基于3d激光的集装箱码头场桥防碰撞控制方法，其特征在于：第二步中的滤波处理包括：(1)按如下公式计算点point
(x,y,z)
的偏转角度φ
point
：
(2)按如下公式计算点point
(x,y,z)
的强度信息i
point
：i
point
＝r
point
/dis
point
其中：r
point
为点云的反射强度，dis
point
为点云的深度，按如下公式计算得到：(3)按如下公式计算点point
(x，y，z)
的入射角度的入射角度其中，(p
a
，p
b
，p
c
)为连续的三个点坐标。8.根据权利要求6所述的基于3d激光的集装箱码头场桥防碰撞控制方法，其特征在于：第三步中，采用如下公式来计算平面的光滑度，以用于区分平面点和边缘点：其中：c表示该点的光滑程度，分别表示在集合s内点i、j的曲率。9.根据权利要求6所述的基于3d激光的集装箱码头场桥防碰撞控制方法，其特征在于：第四步中，在进行迭代位姿优化时，(1)按如下公式计算点到线的残差：(2)按如下公式计算点到面的残差：其中，p
w
表示点w的点坐标。10.根据权利要求6所述的基于3d激光的集装箱码头场桥防碰撞控制方法，其特征在于：第四步中，在进行对点与线的残差和点与面的残差进行迭代位姿优化、帧内的运动补偿时，将输入的每一帧数据分成三段进行处理，以消除采集时间差异存在的误差影响，同时将第三步提取的电气房和联系梁的平面点和边缘点数据加入到位姿的优化中，实时解算小车的位姿。

技术总结
本发明公开了一种基于3D激光的集装箱码头场桥防碰撞控制系统及方法，控制系统包括设置在大车上的小车，以及设置在小车上的3D激光和吊具，在小车的司机室中设置场桥PLC和控制器，在大车的联系梁上设置有电气房，其中：所述场桥PLC将吊具的起升编码器数据和小车的实时坐标数据发送给控制器，所述3D激光将采集的激光点云数据发送给控制器，所述控制器对3D激光和场桥PLC提供的数据进行融合，对小车进行实时定位、计算吊具的实时位置、实时速度和集装箱顶面的角点坐标值，并在发生碰撞危险时向场桥PLC发出控制指令，对当前贝位和相邻贝位的集装箱进行防碰撞保护。本发明不必使用特定的标识物，系统结构更为简洁。系统结构更为简洁。系统结构更为简洁。


技术研发人员：王伟
受保护的技术使用者：北京国泰星云科技有限公司
技术研发日：2023.06.09
技术公布日：2023/8/14