[发明专利]一种激光SLAM和激光反射板结合的定位导航方法

权利要求书

1.一种激光SLAM和激光反射板结合的定位导航方法，其特征是按如下步骤进行：

步骤1：利用AGV上的激光雷达采集工作场地的环境数据，并采用Catographer构图工具对环境数据进行处理，从而构建激光SLAM定位的全局地图，且所述全局地图为像素点集；

步骤2：以工作场地外接矩形一顶点为原点，以矩形长边为x轴方向，宽边为y轴方向建立全局坐标系XOY，并设定分辨率，从而将全局地图的像素点集映射成全局坐标点集；

步骤3：采用最小二乘法对所述全局地图中的坐标点集进行直线拟合，得到V个全局直线方程，并按全局直线的长度从大到小进行排序后，将其方程系数、长度和端点坐标按序存储在全局直线方程库L_global中；

步骤4：根据所述全局地图中的特征模糊区域，分别在对应的工作场地上放置有N个反射板，并对反射板的位置依次在全局坐标系XOY中进行全局位置标定，得到N个反射板的位置坐标并存储在全局位置信息库P_global中，其中，第i个反射板R_i的全局位置坐标记为(x_i，y_i)，i∈[1,N]；

步骤5：根据反射板的位置信息库P_global，计算任意两个反射板R_i，R_j之间的距离d_i,j，i，j∈[1,N]，若d_i,jδ_d，则将距离d_i,j、第i个反射板R_i的全局位置坐标和第j个R_j的全局位置坐标关联存储在全局反射板距离信息库D_global中，否则，不存储，从而建立全局反射板距离信息库D_global；其中，δ_d表示根据雷达的扫描范围设定距离阈值；

步骤6：所述AGV根据是否存储有历史定位数据判断当前定位是静态定位还是动态定位；若为静态定位，则令反射板距离信息库D为全局反射板距离信息库D_global，并跳转执行步骤10，否则执行步骤7；

步骤7：以历史定位数据的雷达全局坐标(x_h，y_h)和雷达扫描半径r为依据，判断在四个坐标点(x_h+r，y_h+r)、(x_h+r，y_h-r)、(x_h-r，y_h-r)和(x_h-r，y_h+r)连成的正方形区域内，是否存在满足激光反射板定位要求的反射板，若存在，则按照步骤5的过程对满足定位要求的反射板进行距离计算和存储判断，从而建立优化后的反射板距离信息库D_global`；若不存在，则跳转执行步骤10；

步骤8：对激光反射板进行定位，并根据雷达扫描到的环境信息，构建局部反射板导航的数据库P_local`；

步骤8.1：以激光雷达为坐标原点，以AGV正前方为x轴正方向，以AGV的左侧方向为y轴正方向，建立局部坐标系xoy；

步骤8.2：根据激光雷达采集的环境数据，首先判断是否扫描到反射板，如果扫描到反射板，则将扫描到的第i个反射板记为R_i`，其在局部坐标系xoy中的局部坐标记为(x<sub>i</sub>`，y_i`)，并将R<sub>i</sub>`和(x_i`，y<sub>i</sub>`)关联存储在所述局部位置信息库P_local`中，继续执行步骤8.3，否则，跳转执行步骤12；

步骤8.3：根据扫描到的反射板信息，判断反射板是否满足定位要求，若扫描到的反射板数量和反射板能量均大于设定值，则表示反射板满足定位要求，并在局部位置信息库P_local`中，依据能量密度从满足定位要求的反射板中筛选出n个目标反射板后，执行步骤8.4，否则跳转执行步骤12；其中，n≥3；

步骤8.4：计算任意两个目标反射板R_i`和R<sub>j</sub>`之间的距离，记为d_i,j`，并将R<sub>i</sub>`的局部坐标、R_j`的局部坐标和d<sub>i,j</sub>`关联存储在目标反射板距离库D_local中；

步骤9：令反射板距离信息库D为优化后的反射板距离信息库D_global`；

步骤10：对反射板进行匹配，找出与目标反射板一一对应的全局反射板；

步骤10.1：将目标反射板距离库D_local中的第1个目标反射板R₁`和第2个目标反射板R<sub>2</sub>`之间的距离d_1,2`和反射板距离信息库D中的每一个距离进行比对，当存在d<sub>x1,x2</sub>＝d<sub>1,2</sub>`，则在反射板序列库S中链式存储2元序列：x₁→x₂和x₂→x₁，分别表示目标反射板R₁`、R<sub>2</sub>`对应全局反射板R_x1、R_x2、目标反射板R₁`、R<sub>2</sub>`对应全局反射板R_x2、R_x1，从而得到M组序列，并保存在反射板序列库S中，否则跳转执行步骤12；

步骤10.2：定义变量k，并初始化k＝2；

步骤10.3：定义变量m，并初始化m＝1；

步骤10.4：定义变量c，并初始化c＝M；

步骤10.5：遍历反射板距离信息库D，查找与所得到的第m个k元序列中的第k个反射板相关联的所有反射板距离，并分别与目标反射板距离库D_local中的第k个目标反射板R_k`和第k+1个目标反射板R<sub>k+1</sub>`之间的距离d_k,k+1`进行比较，若存在a个与d<sub>k,k+1</sub>`相等的距离，则将相关联的反射板分别单独添加到相应k元序列的末尾，从而得到第m个k元序列经第k次计算后产生的a个k+1元序列，并将M赋值为M+a-1；若不存在相等的距离，则在序列库S中删除第m个k元序列，并将M赋值为M-1；

步骤10.6：判断M＝0是否成立，若成立，则执行步骤12，否则，继续判断M＝1是否成立，若成立，则执行步骤11，否则继续执行步骤10.7；

步骤10.7：将m+1赋值给m后，判断mc是否成立，若成立，则表示序列库S中所有序列的第k+1个反射板匹配完成，并执行步骤10.8，否则，返回步骤10.4顺序执行；

步骤10.8：将k+1赋值给k后，判断kn是否成立，若成立，则表示序列库S中所有序列的n个反射板匹配完成，并得到反射板序列库S中的所有完整序列，即含有n个元素的序列，且整序列中的反射板与目标反射板一一对应，并执行步骤10.9；否则，返回步骤10.3顺序执行；

步骤10.9：比较反射板序列库S中所有完整序列中的第一个反射板和最后一个反射板之间的距离与目标反射板R₁`和R<sub>n</sub>`之间的距离d_1,n`是否相等，若相等，则保留相应的完整序列，否则，从序列库S中删除相应的完整序列，并将M-1赋值给M；

步骤10.10：若M＝1，则表示反射板序列库S中剩余唯一的完整序列环，并执行步骤11，否则，表示反射板定位失败，跳转执行步骤12进行激光SLAM定位；

步骤11：在唯一完整序列环中，根据能量密度筛选出3个反射板，并根据3个反射板的全局坐标信息和局部坐标信息，由三边定位原理，求解出激光雷达的实际全局坐标和角度后，跳转执行步骤16；

步骤12：按步骤2设定的分辨率将激光雷达扫描到的局部地图数据的像素点集映射为局部坐标点集；

步骤13：采用最小二乘法对局部坐标点集数据进行直线拟合，得到v个局部直线方程，并按其局部直线长度从大到小的顺序在局部直线信息库L_local中保存拟合得到的局部直线信息，即直线方程系数、直线长度、端点坐标；

步骤14：设置长度误差阈值，并按长度从大到小的顺序，将L_local中的局部直线逐个与L_global中的全局直线进行长度比对，选取满足长度误差阈值的局部直线和全局直线作为两条定位直线；

步骤15：选取两条定位直线中任意3个端点，并根据其局部坐标和全局坐标的关系进行三边定位计算，从而求解两条定位直线的位姿即为AGV的全局位姿，并结束流程，完成AGV的定位；

步骤16：获取到激光雷达的实际全局坐标和全局角度，从而得到AGV的全局位姿，完成AGV的定位。