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

权利要求书

1.一种基于激光雷达和视觉的叉车定位方法，其特征在于，包括以下步骤：

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

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

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

2.如权利要求1所述的基于激光雷达和视觉的叉车定位方法，其特征在于：所述步骤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，从而得到激光雷达的定位信息。

3.如权利要求1或2所述的基于激光雷达和视觉的叉车定位方法，其特征在于：所述步骤S2中，基于视觉特征直接匹配进行重定位的方法如下：

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

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

步骤S23：对采集到的图像依次进行ORB特征提取，设定图像的ORB特征数量阈值T₁，将ORB特征数量达到阈值T₁的图像作为有效图像；T₁取值500～1000；

步骤S24：对当前有效图像的ORB特征f_i与上一帧有效图像的ORB特征进行特征匹配，若两者之间匹配到的特征点点对数量小于n₁，或匹配到的特征点点对之间的平均汉明距离大于n₂，则保存当前有效图像的ORB特征f_i及当前有效图像的位姿也即当前叉车的位姿；n₁取值400～500；n₂取值30～50；

步骤S25：将当前保存的有效图像的ORB特征f_i分别与除上一帧的所有历史保存的有效图像的ORB特征进行特征匹配，获取匹配上的特征点点数最高图像的ORB特征f_j，若匹配上的特征点点数与ORB特征f_i的总特征点点数的比值大于n₃，且匹配到的特征点点对之间的平均汉明距离小于n₄，则重定位成功，叉车曾经到达过当前位置附近；n₃取值0.75～0.9，n₄取值30～40；

步骤S26：将当前有效图像的ORB特征f_i与匹配上的特征点点数最高图像的ORB特征f_j进行比较计算，根据对极几何得到两者之间的旋转角度与偏移位置，根据匹配特征点点数最高图像的ORB特征f_j对应的位姿及旋转角度与偏移位置计算当前有效图像的ORB特征f'_i对应的位姿用计算得到的位姿替换当前激光雷达计算的位姿