[发明专利]一种基于单折角板的移动机器人定位方法及系统

权利要求书

1.一种单折角板的移动机器人定位方法，其特征在于，所述方法具体包括如下步骤：

S1、提取激光雷达当前帧扫描数据中的折角板；

S2、读取折角板在激光雷达坐标系中的位置，基于折角板在激光雷达坐标系中的位置及其在路标地图中的位置来计算移动机器人在路标地图中的位姿P(x,y,theta)，(x,y)为移动机器人在路标地图中的位置，theta为移动机器人的偏航角；

其中，折角板由两个呈一定角度的面板组成，折角板位置由两面板投影线段的交点坐标及两面板的投影线段的单位矢量构成，折角板两面板投影线段的交点即折角板的角点坐标，折角板垂直于地面设置；

移动机器人的偏航角的计算具体如下：

其中，和分别为折角板两面板投影线段在激光雷达坐标系中的单位矢量，和分别为折角板两面板投影线段在路标坐标系中的单位矢量；

激光雷达在路标地图中的位置坐标P_g(x′,y′)计算公式具体如下：

P_g＝p_m-R*p_l

其中，p_m为折角板在路标坐标系中的角点坐标，p_l为折角板在激光雷达坐标系中的角点坐标，R为由theta转化得到的旋转矩阵，最终得到机器人在路标地图中的位姿P(x,y,theta)；

折角板的提取方法具体包括如下步骤：

S11、当前帧扫描数据从最小扫描角到最大扫描角顺序输出，依次顺序提取N条满足长度阈值的线段；

S12、遍历N条线段，计算任意相邻两条线段的长度L1、L2及两条线段的首尾距离Dist，将满足L1-L2小于阈值，且Dist小于预设阈值的两相邻线段点集执行步骤S13；

S13、计算两相邻线段的夹角θ，并计算夹角θ与折角板实际夹角的差值，若该差值位于允许的偏差范围内，则认定该两相邻线段即为折角板两面板的投影线段；

S14、计算第一条线段的剔除比例系数div，基于剔除比例系数div在第一条线段的尾部来剔除属于第二条线段的点集；

筛选折角板的第一条线段的原始点集，将原本属于第二条线段的点集剔除，剔除比例系数div由夹角θ和距离阈值T得出：

其中，a为折角板单面板在投影方向上的实际距离；

S15、对两相邻线段分别进行RANSAC内点提取，找到内点最多的两条直线，对内点进行直线拟合，形成折角板两面板的投影线段，计算折角板两面板投影线段的交点及折角板两面板投影线段的单位矢量

2.如权利要求1所述单折角板的移动机器人定位方法，其特征在于，所述线段提取方法具体如下：

S111、基于当前帧的前两点来计算线段L；

S112、检测下一个点到线段L的距离是否小于距离阈值T，若检测结果为是，执行步骤S113，若检测结果为否，则执行步骤S114；

S113、将下一个点放入线段的点集中，对点集进行线性最小二乘拟合，更新线段L，执行步骤S112；

S114、计算线段L的长度D_L，若legth_min≤D_L≤length_max，则更新线段L，将点集保存到线集合，基于点集中最后一个点的后面两个点计算出下一条线段，执行步骤S112，直至遍历完当前帧扫描数据中的所有点。

3.如权利要求1所述单折角板的移动机器人定位方法，其特征在于，两相邻线段的夹角θ的计算方法具体如下：

计算两相邻线段的交点，由交点和两相邻线段点集中的两数据点分别构建两单位矢量和基于单位矢量和的点乘计算夹角θ，计算公式具体如下：

4.一种基于单折角板的移动机器人定位系统，其特征在于，所述系统包括：

设于定位区域的一个折角板，折角板垂直于地面设置，折角板由两个呈一定角度的面板组成；

平行设于移动机器人前端的激光雷达，激光雷达与处理器连接，处理器与存储器连接，存储器中存储有路标地图，激光雷达扫描行驶区域的环境，将扫描到的环境数据发送至处理器，处理器基于权利要求1至3任一项所述基于单折角板的移动机器人定位方法来对机动移动机器人进行定位。