[发明专利]一种融合折角板的自然定位方法及系统

权利要求书

1.一种融合折角板的自然定位方法，其特征在于，所述方法具体包括如下步骤：

S1、加载栅格地图及折角板地图；

S2、在非精定位区域，通过粒子滤波来计算移动机器人当前的位姿，即进行移动机器人的粗定位；

S3、在进入精定位区域时，将上一时刻粗定位的位姿作为精定位的初始位姿，在精定位区域，基于折角板来进行移动机器人的精定位；

精定位过程具体包括如下步骤：

S21、提取当前激光帧中的折角板；

S22、基于折角板来计算激光雷达当前在地图坐标系中的位姿；

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

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

S212、遍历N条线段，计算任意相邻两条线段的长度及两条线段的首尾距离，两条线段的首尾距离是指第一条线段的尾部距第二条线段头部的距离Dist，将满足L1-L2小于阈值，Dist小于预设阈值的两相邻线段点集执行步骤S213，其中，L1、L2分别是指第一条线段和第二条线段的距离；

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

S214、计算各折角板中第一条线段的剔除比例系数div，基于剔除比例系数div在第一线段的尾部来剔除属于第二线段的点集，并执行步骤S15；

S215、对各折角板的两相邻线段分别进行RANSAC内点提取，找到内点最多的两条直线，对内点进行直线拟合，形成折角板两面板的投影线段，计算各折角板两面板投影线段的交点p′(x′,y′)及折角板两面板投影线段的矢量v₁′(x₁′,y₁′),v₂′(x₂′,y₂′)；

比例系数div的计算公式如下：

其中，a为折角板单面板在投影方向上的实际距离，θ为折角板夹角，T为距离阈值。

2.如权利要求1所述融合折角板的自然定位方法，其特征在于，激光雷达当前在地图坐标系中的位姿获取方法具体包括如下步骤：

S221、在指定距离内提取距激光雷达最近的折角板b′(p′(x′,y′),v₁′(x₁′,y₁′),v₂′(x₂′,y₂′))；

S222、基于激光雷达上一帧中的位姿将折角板b′的交线投影点p′(x′,y′)投影到栅格地图中，并在栅格地图中提取距离交线投影点p′(x′,y′)距离最近的折角板b(p(x,y),v₁(x₁,y₁),v₂(x₂,y₂))，折角板b即为折角板b′在栅格地图中的坐标；

S223、当前帧中折角板b′(p′(x′,y′),v₁′(x₁′,y₁′),v₂′(x₂′,y₂′))的位置及在栅格地图中的匹配折角板b(p(x,y),v₁(x₁,y₁),v₂(x₂,y₂))的位置来计算机器人当前在地图坐标系中的位姿，即激光雷达当前在地图坐标系中的位姿P(x,y,θ)。