[发明专利]基于激光雷达和视觉的叉车定位方法

说明书

基于激光雷达和视觉的叉车定位方法，包括以下步骤：步骤S1：基于迭代最近点算法实现激光雷达的扫描匹配；步骤S2：基于视觉特征直接匹配进行重定位；步骤S3：基于图优化算法实现定位误差优化。本发明结合激光雷达和视觉进行定位，先通过激光雷达实现初定位，然后通过视觉特征直接匹配进行视觉重定位，减小叉车定位过程中的累计误差，提高定位精度。

技术领域

本发明涉及叉车定位方法，具体是涉及一种基于激光雷达和视觉的叉车定位方法。

背景技术

叉车是工业搬运车辆，是指对成件托盘货物进行装卸、堆垛和短距离运输作业的各种轮式搬运车辆。叉车在企业的物流系统中扮演着非常重要的角色，是物料搬运设备中的主力军。

现有的叉车大多是人工驾驶操作，对于AGV类叉车，定位单一地采用激光雷达或视觉来实现，没有采用其他方式进行重定位，将会导致定位不精准或定位出现错误的现象。

发明内容

本发明所要解决的技术问题是，克服上述背景技术的不足，提供一种基于激光雷达和视觉的叉车定位方法，先通过激光雷达实现初定位，然后通过视觉特征直接匹配进行视觉重定位，减小叉车定位过程中的累计误差，提高定位精度。

本发明解决其技术问题采用的技术方案是，一种基于激光雷达和视觉的叉车定位方法，包括以下步骤：

步骤S1：基于迭代最近点算法实现激光雷达的扫描匹配；

步骤S2：基于视觉特征直接匹配进行重定位；

步骤S3：基于图优化算法实现定位误差优化。

进一步，所述步骤S1中，基于迭代最近点算法实现激光雷达的扫描匹配包括以下步骤：

步骤S11：预设激光雷达的激光距离和角度范围；设定激光距离为0.3～40m，角度为0～360°；

步骤S12：利用激光雷达扫描当前环境，将激光雷达扫描到的当前帧数据投影到参考帧坐标系中，参考帧为当前帧的上一帧；

步骤S13：根据预设好的激光距离和角度范围将不符合要求的激光点数据剔除；

步骤S14：对于当前帧的每一点X_i,i＝1,2,3…M,M表示每一帧的激光点数，根据欧氏距离在参考帧中寻找对应的最小点X_j1和第二小点X_j2，并计算得到点X_j1和点X_j2连线的法向量n_i，从而得到一组数据<X_i,X_j1,n_i>；

步骤S15：将点X_i到点X_j1的欧氏距离从小到大进行排序，保留前60～80％的数据点；

步骤S16：利用高斯-牛顿优化法迭代求解当前帧与参考帧之间的旋转矩阵R和平移向量T，从而得到当前帧在世界坐标系下的位姿世界坐标系为第一帧参考帧对应的坐标系，x表示世界坐标系的横坐标，y表示世界坐标系的纵坐标，θ表示叉车的航向角，其中，高斯-牛顿优化法优化目标为：

步骤S17：将当前帧设置为参考帧，重复步骤S12～S16，得到激光雷达的数据帧的位姿信息P₁,P₂,P₃…P_n，从而得到激光雷达的定位信息。

进一步，所述步骤S2中，基于视觉特征直接匹配进行重定位的方法如下：

步骤S21：在叉车行进过程中，根据激光雷达的定位信息得到叉车的位姿信息，当前叉车的位姿为

步骤S22：在叉车行进过程中，通过鱼眼镜头相机在行进路线上采集图像数据；利用时间戳信息将激光雷达的数据和鱼眼镜头相机的数据同步；