当前位置：首页 > 最新专利 > 一种结合gps\ins的集装箱识别定位方法
收藏

一种结合gps\ins的集装箱识别定位方法

文档序号：1612803 发布日期：2020-01-10 浏览：23次 >En<

阅读说明：本技术 一种结合gps\ins的集装箱识别定位方法 (Container identification and positioning method combined with GPS/INS ) 是由 李文锋 姜明峰 罗米 汤冯炜 吕金秋 于 2019-09-24 设计创作，主要内容包括：本发明公开了一种结合GPS\INS的集装箱识别定位方法,标定激光雷达与GPS\INS模块；激光雷达扫描整个集装箱堆场环境,根据GPS数据与点云数据对堆场环境进行建模；根据集装箱高度信息对堆场模型进行分割得到多个子点云；利用欧式距离和点云法向量作为判据对各个子点云进行聚类；利用先验集装箱尺寸信息对各个聚类进行投影、分割和匹配,结合GPS数据可以得到堆场环境内集装箱的扫描定位信息。本发明能够识别出港口环境中的集装箱并给出集装箱的绝对定位信息,从而为实现港口自动化的任务调度、自动抓箱等操作提供必要依据。(The invention discloses a container identification and positioning method combining a GPS \ INS, which is used for calibrating a laser radar and a GPS \ INS module; scanning the whole container yard environment by the laser radar, and modeling the container yard environment according to the GPS data and the point cloud data; dividing the yard model according to the container height information to obtain a plurality of sub-point clouds; clustering each sub-point cloud by using the Euclidean distance and the normal vector of the point cloud as a criterion; and projecting, segmenting and matching each cluster by using the prior container size information, and combining GPS data to obtain the scanning and positioning information of the containers in the yard environment. The invention can identify the container in the port environment and give the absolute positioning information of the container, thereby providing necessary basis for the operations of port automatic task scheduling, automatic container grabbing and the like.)

一种结合GPS\INS的集装箱识别定位方法

技术领域

本发明涉及一种集装箱识别定位方法，特别是一种结合GPS\INS的集装箱识别定位方法。

背景技术

如何进行集装箱的检测、定位和自动抓取成为了实现港口自动化一个重要的命题。在现有的港口自动化系统中，通常需要人工远程监控干预，提前将吊具移动至集装箱上方或者是提前获取集装箱的位置，不能实现港口自动化的任务调度和无人操作。在现有的集装箱识别系统中，部分使用相机检测集装箱的轮廓或是锁孔位置，该种方法通常只能给出集装箱的相对坐标值，而且受光照等环境影响较大。在部分使用激光雷达的系统中，通常需要人工将集装箱运输至指定位置，只能识别固定区域的集装箱，工作效率较低。

发明内容

本发明所要解决的技术问题是提供一种结合GPS\INS的集装箱识别定位方法，可以自动识别堆场范围内的集装箱并得到绝对定位信息。

为解决上述技术问题，本发明所采用的技术方案是：

一种结合GPS\INS的集装箱识别定位方法，其特征在于包含以下步骤：

步骤一：标定激光雷达与GPS\INS模块，得到雷达坐标系与站心坐标系的转换矩阵Tls；

步骤二：港机带动集装箱吊具围绕集装箱堆场环境一周，获取堆场的点云建模；

步骤三：根据先验集装箱高度信息和先验地面高度信息将堆场模型分割成多个子点云；

步骤四：用欧式距离和点云法向量作为判据分别对各个子点云进行聚类；

步骤五：根据先验集装箱尺寸信息分别对各个聚类进行投影、分割和匹配；

步骤六：根据GPS数据得到堆场范围内集装箱的扫描定位信息。

进一步地，所述步骤一中的激光雷达和GPS\INS模块安装在集装箱吊具下方，激光雷达用于获取点云数据，GPS\INS模块用于输出站心坐标系与大地坐标系的转换关系。

进一步地，所述步骤二具体为

2.1获取多帧时间同步的点云数据

Figure BDA0002213274920000021

和GPS\INS模块输出的对应站心坐标系到大地坐标系的转换矩阵

Figure BDA0002213274920000022

2.2通过转换矩阵Tls可以将

Figure BDA0002213274920000023

转换到站心坐标系下，记为

Figure BDA0002213274920000024

然后再通过

Figure BDA0002213274920000025

可以将

Figure BDA0002213274920000026

转换到大地坐标系下，记为

Figure BDA0002213274920000028

Figure BDA0002213274920000029

为不同时刻下雷达坐标系下点云数据构成的数据集；

Figure BDA00022132749200000210

为不同时刻下站心坐标系下点云数据构成的数据集；

Figure BDA00022132749200000211

为不同时刻下大地坐标系下点云数据构成的数据集；

为不同时刻下GPS站心坐标系到大地坐标系的转换矩阵，当i＝j时

Figure BDA0002213274920000031

和

Figure BDA0002213274920000032

代表两者的时间同步；

Tls是雷达坐标系到GPS站心坐标系的转换矩阵，为常值，由标定结果给出；

2.3将所有点云进行融合得到pCg，再使用体素栅格降采样法减少pCg的点云数量，得到堆场点云建模

Figure BDA0002213274920000033

Figure BDA0002213274920000034

Figure BDA0002213274920000035

Figure BDA0002213274920000036

为大地坐标系下整个点云数据集叠加融合的点云数据；

Figure BDA0002213274920000038

为经过VoxelGrid降采样后的点云数据；

leaf为VoxelGrid算法的过滤参数；

VoxelGrid方法将点云划分为以leaf为长宽高的多个矩体，每个矩体中若包含有点，则以他们的质心作为所有点的代替点，达到降采样的效果。

进一步地，所述步骤三具体为根据先验集装箱高度信息

Figure BDA0002213274920000039

和先验地面高度信息hg，将

Figure BDA00022132749200000310

分割为多个子点云

Figure BDA00022132749200000311

Figure BDA00022132749200000313

为

Figure BDA00022132749200000314

经过分割后的子点云集合；

P为

Figure BDA00022132749200000315

中一点，Pz代表该点的Z坐标；

hg为地面的GPS高度信息；

Figure BDA00022132749200000316

为集装箱可能的堆叠高度的集合。

进一步地，所述步骤四具体为

4.1将点云使用KDTree结构保存为PCkd，同时计算点云中所有点的法向量；

4.2从点云

Figure BDA0002213274920000041

中随机选取并标记一点，建立队列D；

4.3以某一距离阈值Td和角度阈值θd，在PCkd中寻找到队列首点Phead距离小于Td同时法向量夹角小于θd的点Pk，将所有满足条件同时未标记的点加入到队列中并标记，同时将队首点移出；

Figure BDA0002213274920000042

Figure BDA0002213274920000044

mark(Pk)＝1代表标记点Pk，mark(Pk)＝＝0代表Pk点未标记；

Td代表距离阈值，θd代表角度阈值；

Figure BDA0002213274920000045

为点Pk与点Phead之间的欧氏距离；

Figure BDA0002213274920000046

为点Pk与点Phead之间法向量的夹角；

nk与nhead分别为当前点Pk与队首点Phead的法向量；

4.4重复4.3，直至不能寻找到新的点，将所有寻找到的点划分为一个聚类；

4.5重复4.2-4.4，直到点云中所有的点都被划分为某一聚类，删除其中点数少于一定阈值的聚类，最后得到点云聚类集{Ci}。

进一步地，所述步骤五具体为

5.1使用RANSAC算法对聚类Ci进行平面拟合，然后去除平面外的点，使用PCA算法将得到转换矩阵

Figure BDA0002213274920000047

通过可以将Ci转换到

Figure BDA00022132749200000410

构成的平面与XY坐标平面近似平行，然后将

Figure BDA00022132749200000411

投影至二值图像

Figure BDA00022132749200000412

中；

Figure BDA0002213274920000051

Figure BDA0002213274920000052

Figure BDA0002213274920000053

Figure BDA0002213274920000054

Figure BDA0002213274920000055

代表由聚类Ci拟合得到的平面参数；

Figure BDA0002213274920000056

为经过PCA主成分分析法对聚类Ci处理后得到的转换矩阵；

Figure BDA0002213274920000057

为PCA坐标系下的聚类点云数据；

Figure BDA0002213274920000058

代表初始化全为0的二值图像，P为

Figure BDA0002213274920000059

中一点，Px和Py分别为点P的X坐标和Y坐标，minPx和minPy分别为

Figure BDA00022132749200000510

中点的最小X坐标和最小Y坐标，(x，y)代表图像坐标，xres和yres代表二值图像分辨率；

RANSAC算法通过采样迭代计算最大一致集得到平面方程，通过点到面的距离公式将Dis大于一定阈值的点剔除，然后通过主成分分析法PCA计算将Ci拟合的平面与XY坐标平面近似平行的转换矩阵

Figure BDA00022132749200000511

然后将

Figure BDA00022132749200000512

中点的Z坐标置0，则可以得到二值图像

Figure BDA00022132749200000513

5.2用旋转卡尺算法求取

Figure BDA00022132749200000514

中点的最小外接矩形Rect，根据集装箱长宽尺寸与外接矩形的长宽比nlenggth和nwidth预测最大可能存在的集装箱数Nguess；

Figure BDA00022132749200000515

Figure BDA0002213274920000061

Figure BDA0002213274920000062

Nguess＝nlength·nwidth

Containerh与Containerw分别为集装箱的长宽；

Recth和Rectw分别为最小外接矩形的长宽；

nlength和nwidth为集装箱长宽尺寸与外接矩形的长宽比；

Nguess为预测最大可能存在的集装箱数；

minAreaRect为一种求取点集最小外接矩形的算法；

5.3根据nlenath和nwidth分别对外接矩形的长宽部分进行分割，然后使用滑窗算法对分割后的矩形进行统计，当滑块包含的点占据的网格面积大于一定阈值则视为一个集装箱；

5.4当统计集装箱数达到Nguess或被包含点总数大于一定阈值后，结束统计，选取最大划分方法。

进一步地，所述步骤六具体为通过划分出来的点集计算集装箱外接矩形Recti，得到对应的四个角点的图像坐标将其从图像坐标系重新投影至大地坐标系，则可以得到集装箱的扫描定位信息

Figure BDA0002213274920000064

Figure BDA0002213274920000067

为集装箱外接矩形在二值图像

Figure BDA0002213274920000068

中的二维角点；

Figure BDA0002213274920000071

为集装箱外接矩形在PCA坐标系下的三维角点；

Figure BDA0002213274920000072

为集装箱外接矩形在大地坐标系下的三维角点；

通过将添加Z＝0从图像平面反投影至PCA坐标系下，在通过将PCA坐标系下的点转换到大地坐标系下，则可得到集装箱角点的GPS坐标

Figure BDA0002213274920000075

本发明与现有技术相比，具有以下优点和效果：本发明能够自动识别出港口环境中的集装箱并给出集装箱的绝对定位信息，从而为实现港口自动化的任务调度、自动抓箱等操作提供必要的基础，大大提高了码头集装箱运转的效率。

附图说明

图1是本发明的一种结合GPS\INS的集装箱识别定位方法的流程图。

图2是本发明的实施例的系统架构图。

具体实施方式

下面通过实施例对本发明作进一步的详细说明，以下实施例是对本发明的解释而本发明并不局限于以下实施例。

如图2所示，在港口自动化场景中的一种实例。其中基准站6负责将采集的载波相位发给GPS\INS模块3，其坐标系A称为大地坐标系。激光雷达1安装于集装箱吊具3下部，从周围环境中获取点云数据，其坐标系B称为雷达坐标系。模块2为GPS\INS模块，负责提供系统实时在大地坐标系下的位置与姿态，其坐标系称为站心坐标系。其中激光雷达与GPS\INS模块为刚性连接，塔式港机4的吊臂通过吊索连接集装箱吊具3，目标集装箱5位于集装箱吊具3下方。

如图1所示，本发明的一种结合GPS\INS的集装箱识别定位方法，包含以下步骤：

步骤一：标定激光雷达与GPS\INS模块，得到雷达坐标系与站心坐标系的转换矩阵Tls。

步骤二：港机带动集装箱吊具围绕集装箱堆场环境一周，根据GPS数据与点云数据对堆场环境进行建模；

2.1获取多帧时间同步的点云数据

Figure BDA0002213274920000081

和GPS\INS模块输出的对应站心坐标系到大地坐标系的转换矩阵

Figure BDA0002213274920000082

2.2通过转换矩阵Tls可以将转换到站心坐标系下，记为然后再通过

Figure BDA0002213274920000085

可以将

Figure BDA0002213274920000086

转换到大地坐标系下，记为

Figure BDA0002213274920000087

Figure BDA0002213274920000088

Figure BDA0002213274920000089

为不同时刻下雷达坐标系下点云数据构成的数据集；

Figure BDA00022132749200000810

为不同时刻下站心坐标系下点云数据构成的数据集；

Figure BDA00022132749200000811

为不同时刻下大地坐标系下点云数据构成的数据集；

Figure BDA00022132749200000812

为不同时刻下GPS站心坐标系到大地坐标系的转换矩阵，当i＝j时

Figure BDA00022132749200000813

和

Figure BDA00022132749200000814

代表两者的时间同步；

Tls是雷达坐标系到GPS站心坐标系的转换矩阵，为常值，由标定结果给出。

2.3将所有点云进行融合得到PCg，再使用体素栅格降采样法减少PCg的点云数量，得到堆场点云建模

Figure BDA00022132749200000815

Figure BDA0002213274920000091

Figure BDA0002213274920000093

为大地坐标系下整个

Figure BDA0002213274920000094

点云数据集叠加融合的点云数据；

为经过VoxelGrid降采样后的点云数据；

leaf为VoxelGrid算法的过滤参数；

VoxelGrid方法将点云划分为以leaf为长宽高的多个矩体，每个矩体中若包含有点，则以他们的质心作为所有点的代替点，达到降采样的效果。

步骤三：根据先验集装箱高度信息

Figure BDA0002213274920000096

和先验地面高度信息hg，将

Figure BDA0002213274920000097

分割为多个子点云

Figure BDA0002213274920000098

Figure BDA0002213274920000099

Figure BDA00022132749200000910

为

Figure BDA00022132749200000911

经过分割后的子点云集合；

P为中一点，Pz代表该点的Z坐标；

hg为地面的GPS高度信息；

Figure BDA00022132749200000913

为集装箱可能的堆叠高度的集合。

步骤四：用欧式距离和点云法向量作为判据分别对各个子点云

Figure BDA00022132749200000914

进行聚类；

4.1将点云使用KDTree结构保存为PCkd，同时计算点云中所有点的法向量；

4.2从点云

Figure BDA00022132749200000915

中随机选取并标记一点，建立队列D；

4.3以某一距离阈值Td和角度阈值θd，在PCkd中寻找到队列首点Phead距离小于Td同时法向量夹角小于θd的点Pk，将所有满足条件同时未标记的点加入到队列中并标记，同时将队首点移出；

Figure BDA0002213274920000101

Figure BDA0002213274920000102

Figure BDA0002213274920000103

mark(Pk)＝1代表标记点Pk，mark(Pk)＝＝0代表Pk点未标记；

Td代表距离阈值，θd代表角度阈值；

Figure BDA0002213274920000104

为点Pk与点Phead之间的欧氏距离；

Figure BDA0002213274920000105

为点Pk与点Phead之间法向量的夹角；

nk与nhead分别为当前点Pk与队首点Phead的法向量。

4.4使用BFS算法，重复4.3，直至不能寻找到新的点，将所有寻找到的点划分为一个聚类；

4.5重复4.2-4.4，直到点云中所有的点都被划分为某一聚类，删除其中点数少于一定阈值的聚类。

步骤五：根据先验集装箱尺寸信息分别对各个聚类Ci进行投影、分割和匹配；

5.1使用RANSAC算法对聚类Ci进行平面拟合，然后去除平面外的点，使用PCA算法将得到转换矩阵

Figure BDA0002213274920000106

通过

Figure BDA0002213274920000107

可以将Ci转换到

Figure BDA0002213274920000108

Figure BDA0002213274920000109

构成的平面与XY坐标平面近似平行，然后将投影至二值图像中；

Figure BDA00022132749200001013

Figure BDA00022132749200001014

Figure BDA0002213274920000111

Figure BDA0002213274920000112

代表由聚类Ci拟合得到的平面参数；

为经过PCA主成分分析法对聚类Ci处理后得到的转换矩阵；

Figure BDA0002213274920000114

为PCA坐标系下的聚类点云数据；

代表初始化全为0的二值图像，P为

Figure BDA0002213274920000116

中一点，Px和Py分别为点P的X坐标和Y坐标，minPx和minPy分别为

Figure BDA0002213274920000117

中点的最小X坐标和最小Y坐标，(x，y)代表图像坐标，xres和yres代表二值图像分辨率。

RANSAC算法通过采样迭代计算最大一致集得到平面方程，通过点到面的距离公式将Dis大于一定阈值的点剔除，然后通过主成分分析法PCA计算将Ci拟合的平面与XY坐标平面近似平行的转换矩阵

Figure BDA0002213274920000118

然后将

Figure BDA0002213274920000119

中点的Z坐标置0，则可以得到二值图像

5.2用旋转卡尺算法求取中点的最小外接矩形Rect，根据集装箱长宽尺寸与外接矩形的长宽比nlength和nwidth预测最大可能存在的集装箱数Nguess；

Figure BDA00022132749200001112

Figure BDA00022132749200001113

Figure BDA00022132749200001114

Nguess＝nlength·nwidth

Containerh与Containerw分别为集装箱的长宽；

Recth和Rectw分别为最小外接矩形的长宽；

nlength和nwidth为集装箱长宽尺寸与外接矩形的长宽比；

Nguess为预测最大可能存在的集装箱数；

minAreaRect为一种求取点集最小外接矩形的算法。

5.3根据nlenath和nwidth分别对外接矩形的长宽部分进行分割，然后使用滑窗算法对分割后的矩形进行统计，当滑块包含的点占据的网格面积大于一定阈值则视为一个集装箱；

5.4当统计集装箱数达到Nguess或被包含点总数大于一定阈值后，结束统计，选取最大划分方法。

步骤六：计算集装箱外接矩形的中心坐标和姿态，将其从图像坐标系重新投影至大地坐标系，则可以得到集装箱的扫描定位信息。

通过划分出来的点集计算集装箱外接矩形Recti，得到对应的四个角点的图像坐标

Figure BDA0002213274920000121

将其从图像坐标系重新投影至大地坐标系，则可以得到集装箱的扫描定位信息

Figure BDA0002213274920000122

Figure BDA0002213274920000124

Figure BDA0002213274920000125

为集装箱外接矩形在二值图像

Figure BDA0002213274920000126

中的二维角点；

Figure BDA0002213274920000127

为集装箱外接矩形在PCA坐标系下的三维角点；

Figure BDA0002213274920000128

为集装箱外接矩形在大地坐标系下的三维角点；

通过将

Figure BDA0002213274920000129

添加Z＝0从图像平面反投影至PCA坐标系下，在通过

Figure BDA0002213274920000131

将PCA坐标系下的点转换到大地坐标系下，则可得到集装箱角点的GPS坐标

Figure BDA0002213274920000132

当获得集装箱的扫描定位信息

Figure BDA0002213274920000133

后，则可进入下一个阶段抓箱阶段，选取一个目标集装箱5，然后进行如下步骤：

1、根据目标集装箱5的扫描定位信息，可以使用塔式港机4将集装箱吊具3移动至目标集装箱5上方；

2、逐渐向目标集装箱靠近，将点云数据投影成深度图Idepth，详细步骤如下：

2.1结合时间同步的GPS数据将点云数据转换到大地坐标系下；

2.2过滤点云数据中的无效点和离群点；

2.3根据雷达的水平扫描精度、垂直扫描精度、水平扫描范围和垂直扫描范围建立空图I0；

Figure BDA0002213274920000134

Figure BDA0002213274920000135

VerticalRange和HorizontalRange分别为雷达的垂直扫描范围和水平扫描范围；

VerticalResolution和HorizontalResolution分别为雷达的垂直扫描精度、水平扫描精度；

I0.height和I0.width为空图I0的长宽。

2.4以点云数据{Pi}的高度为判据写入到I0之中，形成深度图Idepth；

Figure BDA0002213274920000141

为大地坐标系下的雷达点云；

Pz(x，y)为投影至(x，y)的雷达点的Z坐标，maxPz和minPz分别为点云

Figure BDA0002213274920000143

中最大Z坐标和最小Z坐标。

3、对深度图Idepth使用漫水填充算法进行分割，详细步骤如下：

3.1对Idepth进行膨胀处理，得到

Figure BDA0002213274920000144

Figure BDA0002213274920000145

kernel为膨胀函数的核函数。

3.2根据先验集装箱高度信息对

Figure BDA0002213274920000147

进行二值化阈值处理，得到

Figure BDA0002213274920000148

再对使用二值图像轮廓寻找算法，得到轮廓集合{Coi}；

Figure BDA00022132749200001410

Figure BDA00022132749200001411

Figure BDA00022132749200001412

为集装箱可能堆叠的高度集，

Figure BDA00022132749200001413

为

Figure BDA00022132749200001414

在深度图

Figure BDA00022132749200001415

中对应的深度值；

Figure BDA00022132749200001416

是由

Figure BDA00022132749200001417

作为阈值对

Figure BDA00022132749200001418

进行二值化产生的二值图像；

{Coi}是由二值图像轮廓寻找算法FindContours在

Figure BDA00022132749200001419

中寻找到的轮廓信息。

3.3求Coi的质心

Figure BDA00022132749200001420

以

Figure BDA00022132749200001421

为种子使用漫水填充算法，则可以得到各轮廓集合所包围的像素点{Pixi}，继而得到其对应源点云数据中的点{Pi}；

Figure BDA0002213274920000151

Figure BDA0002213274920000152

Figure BDA0002213274920000153

为轮廓Coi的质心；

Figure BDA0002213274920000154

为经过对

Figure BDA0002213274920000155

使用种子为

Figure BDA0002213274920000156

的FloodFill算法后产生的二值图像，

Figure BDA0002213274920000157

位于轮廓Coi内的像素点被置为255；

Pix(x，y)为Idepth(x，y)中(x，y)处的像素点，

Figure BDA0002213274920000158

代表该像素点处于轮廓Coi之中，当Idepth(x，y)！＝0代表{Pi}中有一点Pj与其对应，通过像素集{Pixi}则可以得到对应的聚类点集

Figure BDA0002213274920000159

3.4通过处理多组轮廓{Coi}可以得到多组点集

Figure BDA00022132749200001510

通过将

Figure BDA00022132749200001511

拟合平面、PCA处理，再投影到二值图片中，可以求取最小外接矩形{Recti}和对应的角点

Figure BDA00022132749200001512

结合先验集装箱尺寸信息进行匹配，最后再重投影至大地坐标系，则可以得到集装箱的定位信息

Figure BDA00022132749200001513

3.5最后以匹配程度σ作为权重，对目标集装箱5的定位信息做进一步修正；

Figure BDA00022132749200001514

Figure BDA00022132749200001515

σ为匹配权重；

和

Figure BDA00022132749200001517

分别为这一次的集装箱角点定位信息和上一次的集装箱角点定位信息，Pcornerg为最终的集装箱角点定位信息。

重复3.2-3.5，直到集装箱吊具3与目标集装箱5的距离小于一定阈值。

上述minAreaRect、Dilate、Threshold、FindContours、FloodFill皆为图像处理常用算法，可以在OpenCV库中找到对应实现。

本发明能够自动识别出港口环境中的集装箱并给出集装箱的绝对定位信息，从而为实现港口自动化的任务调度、自动抓箱等操作提供必要的基础，大大提高了码头集装箱运转的效率。

本说明书中所描述的以上内容仅仅是对本发明所作的举例说明。本发明所属技术领域的技术人员可以对所描述的具体实施例做各种修改或补充或采用类似的方式替代，只要不偏离本发明说明书的内容或者超越本权利要求书所定义的范围，均应属于本发明的保护范围。

18页详细技术资料下载
上一篇：一种医用注射器针头装配设备
下一篇：一种基于车辆的高精度定位系统及方法

相关技术

网友询问留言

已有0条留言

还没有人留言评论。精彩留言会获得点赞！

精彩留言，会给你点赞！

技术分类