基于物体轮廓跟踪的集装箱码头场桥防撞控制系统及方法与流程

本发明涉及一种基于物体轮廓跟踪的集装箱码头场桥防撞控制系统及方法。

背景技术：

轮胎式集装箱门式起重机(RTG)和轨道式集装箱门式起重机(RMG)(统称场桥)是集装箱码头作业的重要机械，其效率、安全、作业正确性对码头作业有着重要影响。场桥主要承担集装箱堆场内堆码的集装箱和集装箱水平运输设备(集装箱卡车或自动引导运输车AGV，运行于truck lane)之间的集装箱转移作业，其特点是作业环境复杂，危险系数大，司机视野不佳，且对司机依赖性高。

场桥作业时，由于司机室位置较高，下方照明条件不理想，且存在吊具遮挡视野盲区，因此司机有时无法及时、完整观察到下方堆码集装箱的情况。当码头作业繁忙，司机连续作业或夜间作业时，此种情况尤甚。由于小车和起升机构运动速度较快，且吊具所带载荷往往重达数吨到数十吨，一旦因为没有正确观察到堆码集装箱及卡车高度，不能及时减速停车，轻则着箱过快或碰箱，引起箱损；重则可能引发“打保龄”事故，导致堆码集装箱倾倒、货物损伤，甚至砸死、砸伤相关人员。

目前市场上的场桥集装箱堆码轮廓扫描系统，利用激光扫描仪对堆场内码垛的集装箱轮廓进行扫描，建立了堆码集装箱轮廓。但是，现有的场桥堆码轮廓扫描系统往往具有以下缺点：

1、系统依赖于反光标记。

现有轮廓扫描系统通常采用在场桥拉杆和/或吊具上安装3～4个经特殊设计、易于利用模式识别等算法进行识别定位的反光板，用于对激光扫描仪与场桥主体、吊具之间的相对运动进行追踪。这种方式有三个显著缺点：第一，额外安装反光板导致系统整体成本增加，且安装位置往往无保护措施，安装作业危险性较大。第二，依赖反光板的特殊外形进行识别，由于灰尘遮挡干扰、其它场桥影响等因素，常导致识别失败或识别错误。第三，为保证扫描到反光板，激光扫描仪安装位置受到限制，往往导致安装和后期维护困难。

2、系统往往依赖于外部传感器对小车和起升位置进行检测

由于算法性能和精度限制，现有系统需要依赖小车和起升编码器对当前吊具位置进行判断，从而进行防撞判断。一旦编码器损坏，将导致系统无法正常工作。

3、系统受遮挡、灰尘影响较大

在小车和吊具运行的过程中，激光扫描仪视野往往受到遮挡。同时，环境中的灰尘会积累在激光扫描仪镜头上，遮挡部分扫描区域。由于现有系统往往采用较为原始的轮廓记忆算法，如折线或直方图方法，因此当系统受遮挡影响时，导致精度下降甚至误识别。

4、系统对不规则物体识别不理想。

现有系统往往采用折线轮廓或直方图对下方堆码物轮廓进行表征，因此当下方出现不规则物体如罐箱、集卡托板、开顶箱等物体时，系统可能存在识别不准确的情况。

技术实现要素：

为了克服现有技术的上述缺点，本发明提供了一种基于物体轮廓跟踪的集装箱码头场桥防撞控制系统及方法，利用激光扫描数据处理算法、自动控制领域的最新技术，结合码头作业流程，旨在解决以下问题：

(1)防止RTG/RMG作业过程中吊具及其吊挂集装箱与场地内堆码集装箱及集卡发生碰撞；

(2)利用激光扫描数据处理算法领域最新进展，提高系统运行的独立性，减少对外部反光板标记、额外传感器信息的依赖；

(3)利用激光扫描数据处理算法领域最新进展，提高系统运行的可靠性与精度，减少误报、漏报、停车距离过远等问题。

本发明解决其技术问题所采用的技术方案是：一种基于物体轮廓跟踪的集装箱码头场桥防撞控制系统，包括激光扫描仪、控制器和场桥PLC系统，所述激光扫描仪安装在设置在场桥主体上的小车司机室侧部或前部，并通过数据电缆和电源电缆与设置在小车司机室内的控制器连接，所述激光扫描仪接受控制器发出的信号对场桥吊具及场桥下方物体轮廓进行扫描，并将扫描数据传送给控制器；所述场桥PLC系统通过PLC接口与控制器连接，负责将吊具状态信息发送给控制器，并接收控制器的指令实现保护控制的执行；所述控制器根据接收的扫描数据和吊具状态信息对场桥吊具和场桥下方物体轮廓进行识别和防碰撞运算，并根据识别和防碰撞运算结果向场桥PLC系统下发控制命令。

本发明还提供了一种基于物体轮廓跟踪的集装箱码头场桥防撞控制方法，包括如下步骤：

步骤一、初始化激光扫描仪安装参数和场桥结构参数：

激光扫描仪安装参数包括安装高度、安装倾角、激光到吊具中心距离；场桥结构参数包括场桥高度和场桥跨距；

步骤二、确定占用概率栅格图边界及激光初始位置：

利用场桥高度h、场桥跨距w，分别乘以2，作为占用概率栅格图的大小；激光初始位置作为平面坐标系原点,位于栅格覆盖区域的横向中心、纵向7/10处，由此确定栅格左上角对应平面坐标为(-w,0.6h)，栅格右下角对应平面坐标为(w,-1.4h)；

步骤三、利用激光扫描仪的安装参数，以激光扫描仪的安装位置为坐标系原点，将激光扫描极坐标数据转换到激光直角坐标系；

步骤四、利用已有物体轮廓与当前激光扫描数据进行匹配，求得激光相对于开机时所处位置的位移OffsetX，将激光直角坐标数据平移转换到地面直角坐标系；

步骤五、根据吊具特征识别出吊具在地面直角坐标系中的位置；

步骤六、防撞控制：当障碍物与吊具之间的距离小于设定的安全距离时，进行减速或停车控制。

与现有技术相比，本发明的积极效果是：通过对现有的RTG/RMG吊具防碰箱系统进行改进，应用最新的激光扫描轮廓识别技术，消除系统对外部反光板、传感器的依赖，并扩展可识别物体轮廓种类，提高识别精度。具体表现如下：

1、完全采用激光扫描仪对RMG/RTG下方物体轮廓进行完整扫描，采用最新轮廓跟踪与定位原理，不依赖于外部特定标记及小车编码器；

2、对吊具轮廓及位置进行实时跟踪，不依赖于起升编码器；

3、采用基于格点占用概率的障碍物轮廓表征方法，实现对不规则轮廓的高精度建模与跟踪；

4、对下方物体轮廓进行持续跟踪、实现基于统计学原理的历史记忆及更新，实现盲区记忆及自动初始化。

附图说明

本发明将通过例子并参照附图的方式说明，其中：

图1为系统结构及作业环境示意图；

图2是显示激光传播路径的格点地图与平面坐标示意图；

图3是本发明的坐标系定义示意图；

图4为激光扫描数据的占用概率栅格图；

图5为激光扫描点组成的轮廓示意图；

图6为激光扫描数据与扫描轮廓进行匹配的示意图。

具体实施方式

系统结构及作业环境如图1所示，包括场桥主体M-1，拉杆M-2，主驱动轮M-3，小车T1，起升卷扬机构T-2，司机室T-3，钢缆R-1，吊具S-1，激光扫描仪L-1，激光扫描光束L-2，堆码集装箱Y-1、Y-2。

基于物体轮廓跟踪的集装箱码头场桥防撞控制系统包括激光扫描仪L-1、控制器及场桥的PLC系统。

小车T1设置在场桥主体M-1上，在小车驾驶室T-3内安装所述控制器及其所需电源，所述控制器与场桥PLC控制系统相连接；所述激光扫描仪L-1安装在小车司机室T-3侧部或前部视野良好区域，并通过数据和电源电缆与小车司机室T-3内的控制器连接。激光扫描仪L-1接受控制器发出的信号对吊具和下方物体轮廓进行扫描，并将扫描数据传送给控制器进行数据处理。

控制器的功能是实现激光扫描仪的控制和数据采集、下方物体及吊具轮廓的识别和防碰撞运算等功能，并通过与PLC接口实现信息的获取和控制命令的下发。

PLC为RMG/RTG的核心电气控制系统，负责将吊具的开闭锁状态、着箱状态、运行等信息发送给控制器，并接收控制器的指令实现保护控制的执行。

激光扫描仪对其正下方进行连续扫描，可以获取堆场内集装箱的堆码轮廓和集装箱卡车轮廓等扫描数据。扫描数据经过控制器处理后，形成可靠的下方物体轮廓信息。激光扫描仪包括但不限于二维激光扫描仪/激光雷达和三维激光扫描仪/激光雷达。

一、占用概率栅格图及其更新方法

1.占用概率栅格图定义

占用概率栅格图是本系统的重要概念。因此首先对其进行定义。

占用概率栅格图是一种描述传感器周围环境中障碍物位置分布及存在概率的方式。

“栅格”是指将二维平面xoy采用平行于纵横坐标的格线分成的格子。每个栅格对应平面中的一部分区域。栅格在计算机程序中表示为二维数组。

每个栅格有纵横下标m,n，与格点中心的平面坐标具有一一对应关系。栅格总数为mmax*nmax。起始栅格下标为(0，0)，对应平面的左上方，终止栅格下标为(mmax-1，nmax-1)，对应平面右下方。

在本系统中，为了满足算法需要、减少内存需求量，坐标原点位于栅格区域中心偏上位置，约(0.3*mmax,0.5*nmax)处，这样可以保证栅格区域覆盖整个场桥下方区域，同时兼顾小车、拉杆等区域。

每个栅格中有一个数字，代表本格点中有障碍物的概率。0表示完全不可能有障碍物，1表示一定有障碍物，0.5表示有一半可能性有障碍物。通过利用概率对障碍物轮廓进行建模，相比“有”、“无”两个状态更精确，同时也方便了后续算法进行轮廓匹配。

2.概率更新方法

激光扫描仪原理为发射一束激光并接收回波。激光遇到障碍物会被反射，从而被传感器接收。往返时间t可以被测定，激光传播速度为c，因此障碍物距离为(t/2)*c。

因此激光扫描仪的一次扫描包含的信息有：激光传播路径上没有障碍物，激光传播的路径终点上有障碍物，终点之后信息无法获得。

通过激光扫描仪内部编码器，可以得知激光扫描的角度，加上距离可以测得，因此激光传播路径的终点平面坐标可以知晓。由于概率格点地图与平面坐标存在已知对应关系，我们可以得知激光传播路径经过哪些栅格。假设如图2所示，激光从A1出发，到G7被反射。

我们根据这次测量可以得知，标减号的栅格内有障碍物的可能性较小，而G7栅格内的可能性较大。

由于任何测量都有噪声，因此本系统不直接将G7有障碍物概率设置为1，也不将途径格点有障碍物概率设为0。而是采用一个缩放系数，乘以原来的概率。本系统缩放系数典型值为0.4和1.5。也就是说，假设所有格点内原概率为0.5，经过此次激光扫描，G7变为0.5*1.5＝0.75，A1\B2\C3\D4\E5\F6变为0.5*0.4＝0.2。概率的极限值分别设置为0.01和0.95，防止过度收缩/扩大。其它格点概率不变。

由于激光扫描频率很高，所以如果一个区域原来有障碍物而现在没有障碍物，那么格点概率很快就可以缩小到下极限值；如果原来没有障碍物而现在有障碍物，那么很快就可以提高到上极限值。即使过程中有噪声干扰出现，由于噪声在同一个扫描点连续出现几率较小，所以很快就会被正确扫描数据掩盖。

如果出现遮挡情况，由于更新策略不会修改被遮挡处概率值，因此障碍物轮廓得以被记忆。

二、场桥下方物体轮廓识别方法如下：

1、初始化安装和结构参数

激光扫描仪安装参数包括安装高度、安装倾角、激光到吊具中心距离；结构参数包括场桥高度、场桥跨距等。

2、占用概率栅格图边界及激光位置初始定义

利用场桥高度h、场桥跨距w，分别乘以2，作为占用率栅格图的大小。初始激光位置作为平面坐标系o2原点，位于栅格覆盖区域的横向中心、纵向7/10处，即栅格左上角对应平面坐标(-w,0.6h)，栅格右下角对应平面坐标(w,-1.4h)。

栅格初始化占用概率为0.5，表示环境中任意位置存在障碍物的概率为0.5。

3、将激光扫描极坐标数据转换到激光直角坐标系：

首先利用激光扫描仪的安装参数，以激光扫描仪的安装位置为坐标系原点，将激光扫描极坐标数据转换到笛卡尔坐标系(直角坐标系)：

x＝radial*cos(angle-AngleOffset)；y＝radial*sin(angle-AngleOffset)；

其中，x、y表示笛卡尔坐标系中激光扫描点位置，radial、angle表示极坐标下扫描点到激光扫描仪距离和角度，AngleOffset表示激光安装角度补偿值。

4、将激光直角坐标数据转换到地面直角坐标系：

本系统主要用到两个坐标系，激光坐标系和地面坐标系，如图3所示，其中，激光坐标系o1以激光扫描仪为原点，水平方向朝向小车前方为x轴正方向，竖直向下为y轴正方向。上述第二步坐标转换出来的激光数据点坐标为此坐标系下数值。

由于激光扫描仪安装在小车上，随小车移动，因此为了后续计算，需要将激光点坐标转换到与地面固定不动的坐标系O2下。O2以开机时激光所处位置为原点，与地面固连不动，坐标轴方向与O1一致。第二步所述占用概率栅格图即在O2坐标系平面内绘制。由于激光源点本身不断移动，激光坐标系O1与地面坐标系O2之间存在一个平移，此距离定义为OffsetX，即激光相对于开机时所处位置的位移。

只需求得此位移，即可将激光数据从激光直角坐标系转换到地面直角坐标系。为求此位移，需要利用已有物体轮廓与当前激光扫描数据进行匹配。第一次扫描时，由于周围环境未知，因此直接令OffsetX＝0。此时，利用第一部分中所述更新方法，对占用概率地图进行更新。由于初始所有格点均为0.5，因此一次更新后，视野内的障碍物边缘所在的栅格概率会提高，而视野内激光扫描线穿过的部分，栅格占用率降低。

5、激光扫描数据与轮廓进行匹配

OffsetX是轮廓匹配的输出。轮廓匹配将当前激光坐标系内的扫描轮廓在水平方向进行平移，与占用概率栅格图进行匹配，并使匹配评估值最大，从而得到当前激光最佳平移位置，即求出了激光位置。具体做法如下。

记上次匹配结果为LastOffsetX。此数值说明，激光上次扫描时的位置与开机初始化时的位置距离为LastOffsetX。由于激光频率较高，因此两次激光扫描之间时间较短。根据码头小车一般运行速度，可以假设运动距离不超过0.2米。因此，可以假设当前OffsetX在(LastOffsetX-0.2，LastOffsetX+0.2)范围内。

迭代求解最佳匹配过程如下：

1.tempOffsetX＝LastOffsetX-0.2。

2.将激光扫描轮廓平移tempOffsetX

3.计算此时所有激光扫描点所在栅格的概率和，记为sumP

4.重复1，2，3直到tempOffsetX＝LastOffsetX+0.2

5.找到sumP最大时，tempOffsetX的值

6.OffsetX＝tempOffsetX

假设占用概率栅格图如图4所示，标值为0.1的栅格区域表示占用概率较低较可能为自由空间；标值为0.9的栅格区域表示占用概率较高，更可能为障碍物边缘；标值为0.5的栅格区域为未知区域，目前没有激光射线扫描到此区域。

假设激光扫描点组成的轮廓如图5所示，假设tempOffsetX＝LastOffsetX时，平移轮廓后处于深灰色位置。从图6可以明显看出，当深灰色轮廓与图4中标值为0.9的栅格区域重合时，概率和最大。因此，此时tempOffsetX应为LastOffsetX减去两倍栅格边长。

6、更新占用概率栅格图

利用第一部分所述方法，对占用概率地图进行更新。更新后，视野内的障碍物边缘所在的栅格概率会提高，而视野内激光扫描线穿过的部分，栅格占用率降低。

由于激光扫描频率很高，所以如果一个区域原来有障碍物而现在没有障碍物，那么格点概率很快就可以缩小到下极限值；如果原来没有障碍物而现在有障碍物，那么很快就可以提高到上极限值。即使过程中有噪声干扰出现，由于噪声在同一个扫描点连续出现几率较小，所以很快就会被正确扫描数据掩盖；以激光扫描频率25Hz为例，格点的更新延迟约为500ms。

如果出现遮挡情况，由于更新策略不会修改被遮挡处概率值，因此障碍物轮廓得以被记忆。

7、吊具识别

依激光扫描仪与小车安装几何位置，确定吊具中心相对于激光的水平位置SpreaderXcenter；吊具宽度取为固定值2.5米，同时考虑吊具晃动，以吊具中心水平位置SpreaderXcenter前后2米来确定吊具在集装箱工作区内的可能区域；对落入该区域的激光扫描点，进行点的聚合分析，可能会形成多个聚合子区域，包括吊具、吊具钢丝绳、集装箱、雨雪干扰点、杂物点等；之后对所有聚合子区域的落入点数、区域纵横比、几何形心数据等做综合分析，并与已知吊具的经验区域模型数据(吊具宽度、吊具高度、吊具宽度高度的纵横比)做比较，剔除钢丝绳、集装箱、雨雪干扰点、杂物点等无效子区域，从而得到吊具所在位置的真实子区域的点数据；最后，对吊具子区域的所有点数据做几何质心计算，从而得到一个计算点(SpreaderX，SpreaderY)，作为吊具的准确位置，即吊具在激光直角坐标系O1中的位置。

三、防撞实现方法如下：

1、前述已经根据吊具特征(与激光的x方向距离一定，切边沿激光射线长度突变)识别出了吊具在Y方向位置；根据激光当前匹配得到的OffsetX和安装参数中吊具中心到激光距离，可以计算出吊具在地面直角坐标系O2中的Y方向位置。

2、对激光数据建立的占用概率栅格图进行遍历，如果有障碍物离吊具位置s太近(小于用户指定的安全距离)，则进行减速或停止控制。

本发明的工作原理是：系统硬件设备包括控制器、激光扫描仪，及电源适配器、线缆等附件。系统硬件核心处理单元为控制器，与激光扫描仪相连接，并与场桥PLC系统共同实现防碰撞控制。

该系统利用激光扫描仪，通过持续扫描场桥下物体轮廓并对其进行跟踪，建立固定坐标系；结合场桥PLC控制系统提供的吊具开闭锁信息和激光扫描轮廓，实现场桥吊具与下放堆码物体的防撞。