融合折角板及占据栅格的混合导航地图构建方法及系统与流程

本发明属于地图构建技术领域，提供了一种融合折角板及占据栅格的混合导航地图构建方法及系统。

背景技术：

随着社会的发展和技术的进步，移动机器人越来越深的介入到人类的日常生活中，例如家庭中的清洁机器人、工厂中的搬运机器人以及餐馆中的送餐机器人等。移动机器人想要实现上述功能必须准确的知道自身所在的位置，即实时定位。而移动机器人实时定位的先决条件是建立地图，这是机器人导航和其它智能行为的关键。激光雷达传感器因为测量精度高、不受光照条件影响等优点，被广泛的应用于移动机器人定位导航中。基于激光雷达的定位导航方法可以分为基于反光标识的定位导航方法和基于轮廓的定位导航方法，其中基于反光标识的激光雷达定位导航方法有着精度高、稳定性好等优点，但是有些场景可能会不方便布设反光标识，此外大量布设反光标识会增加成本。

技术实现要素：

本发明实施例提供一种融合折角板及占据栅格的混合导航地图构建方法，在不方便布置反光标识的区域构建栅格地图，基于栅格地图进行定位，增加普适性。

本发明是这样实现的，一种融合折角板与占据栅格的混合导航地图构建方法，其特征在于，所述方法包括如下步骤：

s1、检测当前帧中是否存在折角板，若检测结果为是，执行步骤s2，若检测结果为否，则执行步骤s3；

s2、基于折角板来计算激光雷达当前在地图坐标系中的位姿，并基于激光雷达当前在地图坐标系中的位姿来更新栅格地图及折角板地图；

s3、将当前帧的扫描数据与栅格地图进行匹配，获取激光雷达的当前在地图坐标系中位姿，并基于激光雷达在地图坐标系中的位姿来更新栅格地图。

进一步的，折角板的检测方法具有包括如下步骤：

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

s12、遍历n条线段，计算任意相邻两条线段的长度及两条线段的首尾距离，两条线段的首尾距离是指第一条线段的尾部距第二条线段头部的距离dist，将满足l1-l2小于阈值，dist小于预设阈值的两相邻线段点集执行步骤s13，其中，l1、l2分别是指第一条线段和第二条线段的距离。

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

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

s15、对各折角板的两相邻线段分别进行ransac内点提取，找到内点最多的两条直线，对内点进行直线拟合，形成折角板两面板的投影线段，计算各折角板两面板投影线段的交点p′(x′,y′)及折角板两面板投影线段的矢量v1′(x1′,y1′),v2′(x2′,y2′)。

进一步的，所述s11中的线段提取方法具体如下

s111、基于当前帧的前两点来计算线段l；

s112、检测下一个点到线段l的距离是否小于距离阈值t，若检测结果为是，执行步骤s113，若检测结果为否，则执行步骤s114；

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

s114、计算线段l的长度dl，若legth_min≤dl≤length_max，则更新线段l，将点集保存到线集合，将点集中最后一个点的后面两个点作为下一条线段的线段起点，并计算线段l，执行步骤s112，直至遍历完当前帧扫描数据中的所有点。

进一步的，步骤s2中激光雷达当前在地图坐标系中的位姿计算方法具体如下：

s21、在指定距离内提取距激光雷达最近的未被标记的折角板b′(p′(x′,y′),v1′(x1′,y1′),v2′(x2′,y2′))，将已提取的折角板b′进行标记，其中，p′(x′,y′)是折角板b′两单平面交线的二维投影点，v1′(x1′,y1′)，v2′(x2′,y2′)分别为折角板的两单平面的二维投影；

s22、基于激光雷达上一帧中的位姿将折角板b′的交线投影点p′(x′,y′)投影到栅格地图中，并在栅格地图中提取距离交线投影点p′(x′,y′)距离最近的折角板b(p(x,y),v1(x1,y1),v2(x2,y2))，若交线投影点p′(x′,y′)距p(x,y)的距离小于距离阈值，则折角板b即为折角板b′在栅格地图中的坐标，即折角板b′为已知折角板，若交线投影点p′(x′,y′)距p(x,y)的距离大于距离阈值，则折角板b′为未知折角板，返回步骤s21；

s23、当前帧中折角板b′(p′(x′,y′),v1′(x1′,y1′),v2′(x2′,y2′))的位置及在栅格地图中的匹配折角板b(p(x,y),v1(x1,y1),v2(x2,y2))的位置来计算机器人当前在地图坐标系中的位姿。

进一步的，折角板地图的更新方法具体如下：

基于激光雷达当前在地图坐标系中的位姿计算激光雷达当前相对于地图坐标系的旋转矩阵r和平移向量t；

基于激光雷达当前相对于地图坐标系的旋转矩阵r和平移向量t来计算未知折角板在激光雷达坐标系中的坐标，并更新折角板地图。

进一步的，栅格地图的更新方法具体如下：

对光束终点所在的栅格进行占据更新，对从到所经过的栅格进行自由更新；

计算占据更新栅格及自由更栅格的栅格占据概率，并更新栅格地图。

本发明是这样实现的，一种基于折角板及占据栅格的混合导航地图构建系统，所述系统包括：

设于精确定位的行驶区域的折角板，折角板是由两个呈一定角度的平面构成，折角板垂直于地面布置，

设于平设于移动机器上的激光雷达，激光雷达与处理器连接，激光雷达采集行驶区域内的环境数据，并将扫描数据发送至处理器，处理器基于上述融合折角板与占据栅格的混合导航地图构建方法来构建栅格地图或折角板地图；处理器中存储有行驶区域的环境数据。

本发明提供的融合折角板与占据栅格的混合导航地图构建方法：在需要精确定位的区域利用折角板构建精度较高的地图，便于获取高精度的位姿信息，在不方便布设折角板的区域或者不需要精定位的区域构建栅格地图，可以基于栅格地图进行定位，普适性更强。

附图说明

图1为本发明实施例提供的融合折角板和占据栅格的混合导航地图构建方法流程图

图2为本发明实施例提供的折角板的结构示意图。

具体实施方式

为了使本发明的目的、技术方案及优点更加清楚明白，以下结合附图及实施例，对本发明进行进一步详细说明。应当理解，此处所描述的具体实施例仅仅用以解释本发明，并不用于限定本发明。

图1为本发明实施例提供的融合折角板和占据栅格的混合导航地图构建方法流程图，该方法包括如下步骤：

s1、检测当前帧中是否存在折角板，若检测结果为是，执行步骤s2，若检测结果为否，则执行步骤s3；

在本发明实施例中，折角板是由两个呈一定角度的单平面构成，折角板的结构示意图如图2所示，折角板的检测方法具有包括如下步骤：

s11、当前帧扫描数据从最小扫描角到最大扫描角顺序输出，依次顺序提取n条满足长度阈值的线段，线段提取方法具体如下：

s111、基于当前帧的前两点来计算线段l；

s112、检测下一个点到线段l的距离是否小于距离阈值t，若检测结果为是，执行步骤s113，若检测结果为否，则执行步骤s114；

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

s114、计算线段l的长度dl，若legth_min≤dl≤length_max，则更新线段l，将点集保存到线集合，将点集中最后一个点的后面两个点作为下一条线段的线段起点，并计算线段l，执行步骤s112，直至遍历完当前帧扫描数据中的所有点，线集合中提取有n条满足线段长度阈值要求的线段点集，长度阈值的最小长度设为legth_min，长度阈值的最大长度设为length_max。

212、遍历n条线段，计算任意相邻两条线段的长度及两条线段的首尾距离，两条线段的首尾距离是指第一条线段的尾部距第二条线段头部的距离dist，将满足l1-l2小于阈值，dist小于预设阈值的两相邻线段点集执行步骤s213，其中，l1、l2分别是指第一条线段和第二条线段的距离。

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

计算两相邻线段的交点，由交点和两相邻线段点集中的两数据点分别构建两单位矢量和使用矢量点乘计算夹角θ：

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

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

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

s15、对各折角板的两相邻线段分别进行ransac内点提取，找到内点最多的两条直线，对内点进行直线拟合，形成折角板两面板的投影线段，计算各折角板两面板投影线段的交点p′(x′,y′)及折角板两面板投影线段的矢量v1′(x1′,y1′),v2′(x2′,y2′)；

分别对两个线段点集进行ransac内点提取，具体设定了ransac直线内点判定阈值，设定迭代停止次数阈值，最终在线段原始点集中找到两点所在的直线内点数最多，保存内点点集，重新进行线性最小二乘拟合，得到折角板的两条高精度线段，计算两线段的交点和矢量，折角板两面板投影线段的交点p′(x′,y′)及折角板两面板投影线段的矢量v1′(x1′,y1′),v2′(x2′,y2′)。

s2、基于折角板来计算激光雷达当前在地图坐标系中的位姿，并基于激光雷达当前在地图坐标系中的位姿来更新栅格地图及折角板地图；

在本发明实施例中，激光雷达当前在地图坐标系中的位姿计算方法具体如下：

s21、在指定距离内提取距激光雷达最近的未被标记的折角板b′(p′(x′,y′),v1′(x1′,y1′),v2′(x2′,y2′))，将已提取的折角板b′进行标记，其中，p′(x′,y′)是折角板b′两单平面交线的二维投影点，v1′(x1′,y1′)，v2′(x2′,y2′)分别为折角板的两单平面的二维投影；

s22、基于激光雷达上一帧中的位姿将折角板b′的交线投影点p′(x′,y′)投影到栅格地图中，并在栅格地图中提取距离交线投影点p′(x′,y′)距离最近的折角板b(p(x,y),v1(x1,y1),v2(x2,y2))，若交线投影点p′(x′,y′)距p(x,y)的距离小于距离阈值，则折角板b即为折角板b′在栅格地图中的坐标，即折角板b′为已知折角板，执行步骤s23，若交线投影点p′(x′,y′)距p(x,y)的距离大于距离阈值，则折角板b′为未知折角板，即在栅格地图中不存在与之匹配的折角板，返回步骤s21；

在本发明实施例中，地图坐标系包括了栅格地图及直角板地图，已经纳入地图坐标系的折角板称为已知折角板，未纳入地图坐标系的折角板称为未知折角板，混合地图的构建过程就是将未知折角板不断纳入地图坐标系。

s23、当前帧中折角板b′(p′(x′,y′),v1′(x1′,y1′),v2′(x2′,y2′))的位置及在栅格地图中的匹配折角板b(p(x,y),v1(x1,y1),v2(x2,y2))的位置来计算机器人当前在地图坐标系中的位姿，即激光雷达当前在地图坐标系中的位姿p(x,y,θ)，其计算公式具体如下：

其中，r、t分别为地图坐标系到雷达坐标系的旋转矩阵和平移向量，基于r、t计算得到机器人的位姿p(x,y,θ)：

pos＝-r^-1*t

θ＝arctan(r1,0/r0,0)

其中，r1,0/r0,0为旋转矩阵r第二行第一列除以旋转矩阵第一行第一列，pos为移动机器人位置坐标，θ为移动机器人的姿态角，移动机器人的位置坐标及姿态角即构成为机器人的位姿。

基于激光雷达当前在地图坐标系中的位姿来计算未知折角板在地图坐标系中的坐标，其计算方法具体如下：

基于激光雷达当前在地图坐标系中的位姿计算激光雷达相对于地图坐标系的旋转矩阵r和平移向量t；

假定未知折角板在激光雷达坐标系中的坐标b′(p′(x′,y′),v1′(x1′,y1′),v2′(x2′,y2′))，则未知折角板在地图坐标b(p(x,y),v1(x1,y1),v2(x2,y2))计算方法如下：

为了提高折角板位置计算的精确性，可以取50次计算的坐标平均值作为最终的折角板坐标。

基于激光雷达当前在地图坐标系统中的位姿来更新栅格地图，其更新方法具体如下：

已知激光雷达在地图坐标系中的位姿p(x,y,θ)，则激光雷达在栅格地图中的位置为l为单个栅格的边长的实际尺寸，激光雷达坐标系中的扫描点到栅格地图坐标系的转换矩阵为m，m的表达式具体如下：

其中p^m即是激光雷达的光束起点，已知第i个激光扫描点在激光雷达坐标系中的齐次坐标为pi(xi,yi,1)，则第i个光束的终点在栅格地图中的坐标为

s24、对光束终点对所在的栅格进行占据更新，对从到所经过的栅格进行自由更新；

已知一个光束的起点和终点k是光束经过的栅格的数量，可以利用优势对数积分法对光束经过栅格的概率进行更新。对所在的栅格进行占据更新，对从到所经过的栅格进行自由更新。设置自由更新的值oddsfree＝0.4，占据更新的值oddsocc＝0.6。

s25、计算占据更新栅格及自由更栅格的栅格占据概率，并更新栅格地图。

假定更新前的栅格占据概率为p，则更新前栅格的odds值为odds＝log(p/(1-p))，若当前栅格是进行自由更新，则令odds＝odds+oddsfree，若当前栅格是进行占据更新，令odds＝odds+oddsocc，更新后，栅格的占据概率为p＝exp(odds)/(exp(odds)+1)。

s3、基于扫描数据与栅格地图的匹配来获取激光雷达的当前在地图坐标系中位姿，并基于激光雷达在地图坐标系中的位姿来更新栅格地图。

根据激光雷达上一时刻的位姿，将每一个扫描点映射到栅格地图上，基于所有的扫描点在栅格地图的，可以构造如下的目标函数：

si(ξ)表示将t时刻第i个激光点变换到栅格地图坐标系下，m(si(ξ))表示第i个激光点在栅格地图上的位置被占据的概率，这样就把位姿计算变换成最小二乘求解问题；利用双线性差值计算m(si(ξ))，并对m(si(ξ))求偏导，将位姿计算的最小二乘求解转换为迭代的位姿增量计算的问题，基于位姿增量来确定激光雷达当前帧在栅格地图中的定位。

基于激光雷达当前在地图坐标系中位姿来更新栅格地图：

已知激光雷达在地图坐标系中的位姿p(x,y,θ)，则激光雷达在栅格地图中的位置为l为单个栅格的边长的实际尺寸，激光雷达坐标系中的扫描点到栅格地图坐标系的转换矩阵为m，m的表达式具体如下：

其中p^m即是激光雷达的光束起点，已知第i个激光扫描点在激光雷达坐标系中的齐次坐标为pi(xi,yi,1)，则第i个光束的终点在栅格地图中的坐标为

s31、对光束终点所在的栅格进行占据更新，对从到所经过的栅格进行自由更新；

已知一个光束的起点和终点k是光束经过的栅格的数量，可以利用优势对数积分法对光束经过栅格的概率进行更新。对所在的栅格进行占据更新，对从到所经过的栅格进行自由更新。设置自由更新的值oddsfree＝0.4，占据更新的值oddsocc＝0.6。

s32、计算占据更新栅格及自由更栅格的栅格占据概率，并更新栅格地图。

假定更新前的栅格占据概率为p，则更新前栅格的odds值为odds＝log(p/(1-p))，若当前栅格是进行自由更新，则令odds＝odds+oddsfree，若当前栅格是进行占据更新，令odds＝odds+oddsocc，更新后，栅格的占据概率为p＝exp(odds)/(exp(odds)+1)。

本发明还提供了一种基于折角板及占据栅格的混合导航地图构建系统，该系统包括：

设于精确定位的行驶区域的折角板，折角板是由两个呈一定角度的平面构成，折角板垂直于地面布置，设于平设于移动机器上的激光雷达，激光雷达与处理器连接，激光雷达采集行驶区域内的环境数据，并将扫描数据发送至处理器，处理器基于上述融合折角板与占据栅格的混合导航地图构建方法来构建栅格地图或折角板地图；处理器中存储有行驶区域的环境数据。

本发明提供的融合折角板与占据栅格的混合导航地图构建方法：在需要精确定位的区域利用折角板构建精度较高的地图，便于获取高精度的位姿信息，在不方便布设折角板的区域或者不需要精定位的区域构建栅格地图，可以基于栅格地图进行定位，普适性更强。

以上所述仅为本发明的较佳实施例而已，并不用以限制本发明，凡在本发明的精神和原则之内所作的任何修改、等同替换和改进等，均应包含在本发明的保护范围之内。