CN108828518A - 一种货车车厢内部堆垛车定位方法 - Google Patents

Abstract

本发明设计一种货车车厢内部堆垛车定位方法。本方法利用固定于堆垛车上的激光雷达对工作环境进行扫描，扫描数据通过无线局域网传输到中央控制计算机，经过聚类、分割、拟合、匹配和筛选等环节，取出环境中的货车车厢轮廓特征线段，用于计算堆垛车与货车车厢的相对位姿，完成定位。本发明能够快速、准确的识别货车车厢轮廓，给上位机运动控制系统提供定位信息，从而实现堆垛过程中的轨迹跟踪。

Description

一种货车车厢内部堆垛车定位方法

技术领域

本发明涉及一种基于激光传感器的货车车厢内部堆垛车定位方法。

背景技术

货车车厢堆垛是整个货车物流运输中比较重要的环节，现很多企业都是采用人工搬运或者操作叉车来完成，不仅劳动强度大而且堆垛效率低。一种带有导航系统的能够驶入货车车厢内部的堆垛车，将实现产品从产线末端到货车车厢的无人化自动搬运堆垛。其中定位问题是导航过程中最基本也是必须首先解决的环节，也是堆垛车完成堆垛任务的关键。

激光传感器在移动机器人自主定位方面应用广泛，有着精度高，距离远，性能稳定等优点，即使堆垛车进入光线较弱的货车车厢内部工作，也不会受到影响。

发明内容

本发明的目的在于针对已有技术存在的不足，提供一种货车车厢内部堆垛车定位方法，能够实时、精确的获取堆垛车与货车车厢之间的相对位姿，用于给上位机运动控制系统提供定位信息，以实现运动过程中的轨迹跟踪。

为达到上述目的，本发明采用下述技术方案：

一种货车车厢内部堆垛车定位方法，其特征在于具体操作步骤如下：

a.在堆垛车顶部中央位置安装一个二维激光传感器，用于周围环境识别，可以获取传感器所在水平位置的环境扫描轮廓数据；在堆垛车底盘四个角点各安装一个激光测距仪，用于当堆垛车进入货车车厢内部时，检测堆垛车四个角点与货车车厢内壁的距离，防止堆垛车与箱壁发生碰撞造成箱体破坏。

b.传感器扫描数据实时返回给处于同一局域网的计算机用于计算处理。

c.以激光传感器为坐标原点建立动态局部坐标系，将激光传感器周期性返回的扫描数据进行处理运算，设计算法提取出货车车厢水平截面轮廓信息。

d.根据货车车厢轮廓信息计算出堆垛车在装车环境中的位姿。

用货车车厢的两侧面以及内部堆垛面所在的直线特征组合描述货车车厢轮廓。其中，堆垛面是指货车车厢内部最外层货物表面所在的平面，堆垛车通过获取自身与堆垛面距离，从而停在堆垛面前方适当距离的位置处，以施展堆垛工作。

所述的一种货车车厢轮廓提取算法，具体步骤为：

a)将所述激光传感器返回坐标点转换成在所述局部坐标系中的直角坐标形式，并对离散的坐标点进行聚类。

b)对各点类采用IEPF法进行直线分割，并将分割后点数过少的点类剔除。

c)采用最小二乘法对分割后的点类进行直线拟合，拟合形式为y＝kx+b。

d)基于RANSAC算法思想，以相互平行且距离为货车车厢宽度的平行线为模型，匹配出货车车厢两侧面所在直线特征{L1,L3}。

e)在处于两侧面拟合直线{L1,L3}之间且拟合直线与{L1,L3}近似垂直的直线点类中，选出离所述局部坐标系原点O距离最近的点类，所对应拟合直线作为货车车厢内部堆垛面所在直线特征L2。此算法可以兼容定位时，所扫描的堆垛面堆垛不完整或者堆垛货物之间存在较大缝隙的情况。

f)根据三条直线特征{L1,L2,L3}，即可唯一确定货车车厢在局部坐标系中的位姿。

所述基于RANSAC的思想，匹配货车车厢两侧面所在直线特征的方法，具体步骤为：

(a)在所有拟合的直线中随机选取一条直线，并将该条直线在垂直方向向两侧分别移动一个货车车厢的宽度，得到三条平行直线构成的模型。

(b)用模型去测试所有坐标点，如果坐标点位于模型上，则认为它是局内点，并且标记该点属于哪条相应的直线。

(c)如果局内点的个数大于设定的阈值η，则认为模型是合理模型，用局内点更新模型。选择中间一条以及另外两条中局内点个数较多的一条，并用局内点重新估算直线模型，估算方法采用最小二乘法，得到由两条直线构成的新模型。再次测试所有坐标点，如果新模型的局内点与模型错误率更优，则更新现有最优模型，否则不做更新。

(d)以指定次数重复前述步骤(a)到(c)，所得模型即为货车车厢两侧面对应的拟合直线。

在所述堆垛车位姿计算算法中，用参数(μ,θ,ρ)来描述堆垛车在装车环境中的所处位姿。其中ρ表示堆垛车与堆垛面的距离，θ表示堆垛车中轴线与货车车厢中轴线的夹角，μ表示堆垛车几何中心与货车车厢中轴线的距离。这样设计的目的在于，ρ可以用于控制堆垛车的前行距离，μ和θ可以共同作为运动控制器的输入从而减小μ和θ的值，实现堆垛车跟随货车车厢中轴线行走运动。

具体计算方法为：根据所求货车车厢两侧面特征直线L1和L3，计算出货车车厢中轴线L。直线L与所述局部坐标系Y轴夹角即为所述参数θ，堆垛车几何中心点C与直线L之间的垂直距离即为所述参数μ，堆垛车前端中心点M与直线L2之间的垂直距离即为所述参数ρ。

与现有技术相比，本发明的有益效果是：

本发明利用安装在堆垛车顶部的激光雷达对作业环境进行扫描，所设计的数据处理算法通过聚类、分割、拟合、匹配、筛选等环节，提取出货车车厢轮廓信息用于堆垛车定位。匹配算法基于RANSAC算法思想，建立基于货车车厢几何特征的模型来进行快速有效的匹配，筛选算法可以成功兼容堆垛面垛型不完整或者堆垛货物之间存在较大缝隙的情况。本发明算法鲁棒性高、速度快、精度高，适用于货车车厢内部堆垛车定位，为实现堆垛车的自主导航奠定基础。

附图说明

附图用来提供对本发明的进一步理解，实施示例用于解释本发明，并不构成本发明的不当限定。

图1为本发明中堆垛车系统环境。

图2为本发明中堆垛车俯视图。

图3为本发明中堆垛车定位参数示意图。

图4为激光传感器扫描数据处理结果图。

具体实施方式

下面结合附图对本发明的优选实施例做进一步的解释说明。

实施例一：

参见图1～图4，本货车车厢内部堆垛车定位方法，具体操作步骤为：

a.在堆垛车1顶部中央位置安装一个二维激光传感器2，用于周围环境识别；在堆垛车1底盘四个角点各安装一个激光测距仪6，用于当堆垛车1进入货车车厢4内部时，检测堆垛车1四个角点与货车车厢4内壁的距离，防止堆垛车1与货车车厢箱壁发生碰撞造成箱体破坏；

b.激光传感器2扫描数据实时返回给处于同一局域网的计算机5用于计算处理；

c.以激光传感器2为坐标原点建立动态局部坐标系XOY，将激光传感器2周期性返回的扫描数据进行处理运算，设计算法提取出货车车厢4水平截面轮廓信息；

d.根据货车车厢4轮廓信息计算出堆垛车1在装车环境中的位姿。

实施例二：本实施例与实施例一基本相同，其特征之处如下：

所述步骤d中的货车车厢4轮廓是：用货车车厢4的两侧面以及内部堆垛面所在的直线特征组合{L1,L2,L3}描述货车车厢4轮廓。

所述步骤d中的货车车厢轮廓提取算法，具体步骤为：

a)将所述激光传感器2返回坐标点转换成在所述局部坐标系XOY中的直角坐标形式，并对离散的坐标点进行聚类；

b)对各点类进行直线分割，并将分割后点数过少的点类剔除；

c)采用最小二乘法对分割后的点类进行直线拟合；

d)基于RANSAC算法思想，以相互平行且距离为货车车厢4宽度的平行线组合为模型，匹配出货车车厢4两侧面所在直线特征{L1,L3}；

e)在处于两侧面拟合直线{L1,L3}之间且拟合直线与{L1,L3}近似垂直的直线点类中，选出离所述局部坐标系原点O距离最近的点类，所对应拟合直线作为货车车厢4内部堆垛面所在直线特征L2；

f)根据三条直线特征{L1,L2,L3}，即可唯一确定货车车厢4在局部坐标系XOY中的位姿。

所述步骤d中的堆垛车位姿计算算法：用参数(μ,θ,ρ)来描述堆垛车1在装车环境中的位姿，其中ρ表示堆垛车1与堆垛面的距离，θ表示堆垛车1中轴线与货车车厢4中轴线的夹角，μ表示堆垛车1几何中心与货车车厢4中轴线的距离；具体计算方法为：根据所求货车车厢4两侧面特征直线L1和L3，计算出货车车厢4中轴线L，该中轴线L与所述局部坐标系Y轴夹角即为所述参数θ，堆垛车1几何中心点C与中轴线L之间的垂直距离即为所述参数μ，堆垛车1前端中心点M与侧面拟合线L2之间的垂直距离即为所述参数ρ。

实施例三：

本实施例应用于如图1所示的系统，该系统由堆垛车1，激光传感器2，激光测距仪6，计算机5，货车车厢4和堆垛货物3组成。激光传感器1选用2D激光传感器，通过无线通讯方式与处于同一局域网的计算机5进行数据交互。

图2为堆垛车1俯视图，结合图1可以看到激光传感器2安装于堆垛车1顶部中轴线位置，激光测距仪6a,6b,6c,6d安装于堆垛车底盘四个角点位置。其中激光传感器2可以用于周围环境识别，返回其所在水平位置的环境扫描轮廓数据，激光测距仪6除了可以弥补激光传感器2盲区，检测障碍物之外，还可以用于当堆垛车1进入货车车厢4内部时，检测堆垛车1四个角点与货车车厢4内壁的距离，防止堆垛车1与箱壁发生碰撞造成箱体破坏。

图3为堆垛车1于工作环境中的参数示意图，以激光传感器2为中心，建立局部坐标系XOY，点M为堆垛车1前端中心点，点C为堆垛车1几何中心，L1、L3为货车车厢4两侧面，L2为堆垛面所在直线，L为货车车厢4中轴线。(μ,θ,ρ)用于表示堆垛车1在作业环境中的位姿，其中μ为点C到直线L的距离，θ为堆垛车1中轴线与直线L的夹角，ρ为点M到L2的距离。

图4表示针对某次扫描数据的处理过程图，结合图4，运用本发明提供的一种货车车厢堆垛车定位方法，对激光传感器2返回数据进行处理，具体步骤为：

(1)图4(a)表示原始扫描数据，通过将极坐标形式表示的数据点转换成直角坐标系XOY下的坐标表示。(x_i，y_i)表示第i个数据点在局部坐标系XOY中的坐标，(r_i，α_i)表示第i个数据点在以激光传感器2为坐标原点的极坐标。

(2)对步骤(1)得到的数据点进行聚类，具体操作为：从第一个点(x₁，y₁)开始，依次计算当前点与下一点的距离，如果距离小于设定的阈值γ，则将下一点归于同一类，直到距离大于阈值γ，判断当前类中坐标点的个数，如果大于阈值σ，则标记为有效类，否则为无效类。以此类推，按照扫描顺序判断完所有的坐标点。阈值γ和σ为关键的预设参数，需要根据应用实际场合和激光传感器2分辨率、精度等参数，反复试验进行调整以达到最佳效果。聚类结果如图4(b)所示，从图中可以看出，数据点被聚成三个点类。

(3)采用IEPF法对各点类进行直线分割，并将分割后点数过少的点类剔除。分割结果如图4(c)所示，从图中可以看出，各点类被分割成直线点类。

(4)采用最小二乘法对分割后的点类进行直线拟合，拟合形式为y＝kx+b。

(5)基于RANSAC算法思想，以相互平行且距离为货车车厢4宽度的平行线为模型，匹配出货车车厢4两侧面所在直线特征{L1,L3}。再在处于两侧面拟合直线{L1,L3}之间且拟合直线与{L1,L3}近似垂直的直线点类中，选出离所述坐标坐标系XOY原点距离最近的点类，所对应拟合直线作为货车车厢4内部堆垛面所在直线特征L2。匹配与筛选结果如图4(d)所示，从图中可以看出，算法成功选出了货车车厢轮廓特征线{L1,L2,L3}。

(6)用参数(μ,θ,ρ)来描述堆垛车1在装车环境中的所处位姿。基于货车车厢4轮廓特征线{L1,L2,L3}拟合参数，位姿计算公式如下：

所述基于RANSAC算法思想提取货车车厢4两侧面所在的特征直线L1和L3的方法步骤如下：

(1)在所有直线点类的拟合直线构成的集合中随机选取一条直线，将该直线在垂直方向向两侧分别移动一个货车车厢4的宽度，从而得到三条平行直线构成的模型。

(2)用模型去匹配所有的坐标点，如果某坐标点属于三条直线中的一条，也就是与这条直线之间的距离小于阈值ε，则认为它是局内点，并且标记属于哪条直线。

(3)如果局内点的个数大于设定的阈值η，则认为模型是合理模型，用局内点更新模型。选择中间一条以及另外两条中局内点个数较多的一条，并用局内点重新估算直线模型，估算方法采用最小二乘法，得到由两条直线构成的新模型。再次测试所有坐标点，如果新模型的局内点与模型错误率更优，则更新现有最优模型，否则不做更新。

(4)以指定次数重复前述步骤(1)～(3)，所得模型即为货车车厢两侧面对应的拟合直线。

令上述计算步骤为一次计算周期，将每次计算周期得到的计算结果(μ,θ,ρ)与前一次(μ’,θ’,ρ’)进行比较，如果在一定的差值范围内，则用新的计算结果来更新当前位姿；否则，增加一次错误计数，如果错误计数超过设定的阈值τ，则认为定位失败，发出定位失败信号。

Claims (4)

1.一种货车车厢内部堆垛车定位方法，其特征在于具体操作步骤为：

a. 在堆垛车（1）顶部中央位置安装一个二维激光传感器（2），用于周围环境识别；在堆垛车（1）底盘四个角点各安装一个激光测距仪（6），用于当堆垛车（1）进入货车车厢（4）内部时，检测堆垛车（1）四个角点与货车车厢（4）内壁的距离，防止堆垛车（1）与货车车厢箱壁发生碰撞造成箱体破坏；

b. 激光传感器（2）扫描数据实时返回给处于同一局域网的计算机（5）用于计算处理；

c. 以激光传感器（2）为坐标原点建立动态局部坐标系XOY，将激光传感器（2）周期性返回的扫描数据进行处理运算，设计算法提取出货车车厢（4）水平截面轮廓信息；

d. 根据货车车厢（4）轮廓信息计算出堆垛车（1）在装车环境中的位姿。

2.根据权利要求1所述的货车车厢内部堆垛车定位方法，其特征在于：所述步骤d中的货车车厢（4）轮廓是：用货车车厢（4）的两侧面以及内部堆垛面所在的直线特征组合{L1,L2,L3}描述货车车厢（4）轮廓。

3.根据权利要求1所述的货车车厢内部堆垛车定位方法，其特征在于：所述步骤d中的货车车厢轮廓提取算法，具体步骤为：

a)将所述激光传感器（2）返回坐标点转换成在所述局部坐标系XOY中的直角坐标形式，并对离散的坐标点进行聚类；

b)对各点类进行直线分割，并将分割后点数过少的点类剔除；

c)采用最小二乘法对分割后的点类进行直线拟合；

d)基于RANSAC算法思想，以相互平行且距离为货车车厢（4）宽度的平行线组合为模型，匹配出货车车厢（4）两侧面所在直线特征{L1,L3}；

e)在处于两侧面拟合直线{L1,L3}之间且拟合直线与{L1,L3}近似垂直的直线点类中，选出离所述局部坐标系原点O距离最近的点类，所对应拟合直线作为货车车厢（4）内部堆垛面所在直线特征L2；

f)根据三条直线特征{L1,L2,L3}，即可唯一确定货车车厢（4）在局部坐标系XOY中的位姿。

4.根据权利要求1所述的货车车厢内部堆垛车定位方法，其特征在于：所述步骤d中的堆垛车位姿计算算法：用参数（μ,θ,ρ）来描述堆垛车（1）在装车环境中的位姿，其中ρ表示堆垛车（1）与堆垛面的距离，θ表示堆垛车（1）中轴线与货车车厢（4）中轴线的夹角，μ表示堆垛车（1）几何中心与货车车厢（4）中轴线的距离；具体计算方法为：根据所求货车车厢（4）两侧面特征直线L1和L3，计算出货车车厢（4）中轴线L，该中轴线L与所述局部坐标系Y轴夹角即为所述参数θ，堆垛车（1）几何中心点C与中轴线L之间的垂直距离即为所述参数μ，堆垛车（1）前端中心点M与侧面拟合线L2之间的垂直距离即为所述参数ρ。