技术领域
[0001] 本
发明属于
机器人定位技术领域，提供了一种融合折角板的自然定位方法及系统。
背景技术
[0002] 随着社会的发展和技术的进步，
移动机器人越来越深的介入到人类的日常生活中，例如家庭中的清洁机器人、工厂中的搬运机器人以及餐馆中的送餐机器人等。移动机器人想要实现上述功能必须准确的知道自身所在的
位置，移动机器人定位导航常用的
传感器有磁导航、摄像机和
激光雷达等。其中磁导航的导航路径单一，不利于扩展和更改，使用的局限性较大。基于视觉的定位导航方法受
光源条件影响大，
稳定性和
精度较差，无法让移动机器人进行稳定、精密的操作。基于激光雷达的定位导航方法可以分为基于反光板的定位导航方法和基于轮廓的定位导航方法，其中基于反光板的定位导航方法需要在环境中布设大量的路标，应用场景受限制较多。基于轮廓的自然定位导航方法由于路径灵活、不需要布置人工标志，越来越广泛的应用在移动机器人定位导航中。目前基于轮廓定位的主流方法是基于粒子滤波的栅格定位方法，即通过运动模型对机器人
位姿进行
采样生成大量粒子，依据传感器的观测结果更新粒子的权重并进行重采样，不断
迭代使粒子收敛。但是由于栅格地图
分辨率及雷达探测精度的影响，基于粒子滤波的栅格地图定位方法精度较差，无法满足移动机器人叉、放货及高精度抓取的需求。
发明内容
[0003] 本发明
实施例提供一种融合折角板自然定位方法，仅在需要精定位的区域布置折角板，在粗定位区域无需布置折角板，既满足了定位精度的需求，也减小了对应用场景的依赖。
[0004] 本发明是这样实现的，一种融合折角板自然定位方法，所述方法具体包括如下步骤：
[0005] S1、记载栅格地图及折角板地图；
[0006] S2、在非精定位区域，通过粒子滤波来计算移动机器人当前的位姿，即进行移动机器人的粗定位；
[0007] S3、在进入精定位区域时，将上一时刻粗定位的获取位姿作为精定位的初始位姿，在精定位区域，基于折角板来进行移动机器人的精定位；
[0008] 进一步的，精定位过程具体包括如下步骤：
[0010] S22、基于折角板来计算激光雷达当前在地图
坐标系中的位姿。
[0011] 进一步的，激光雷达当前在地图坐标系中的位姿获取方法具体包括如下步骤：
[0012] S221、在
指定距离内提取距激光雷达最近的折角板b′(p′(x′,y′),v1′(x1′,y1′),v2′(x2′,y2′))；
[0013] S222、基于激光雷达上一帧中的位姿将折角板b′的交线投影点p′(x′,y′)投影到栅格地图中，并在栅格地图中提取距离交线投影点p′(x′,y′)距离最近的折角板b(p(x,y),v1(x1,y1),v2(x2,y2))，折角板b即为折角板b′在栅格地图中的坐标；
[0014] S223、当前帧中折角板b′(p′(x′,y′),v1′(x1′,y1′),v2′(x2′,y2′))的位置及在栅格地图中的匹配折角板b(p(x,y),v1(x1,y1),v2(x2,y2))的位置来计算机器人当前在地图坐标系中的位姿，即激光雷达当前在地图坐标系中的位姿P(x,y,θ)。
[0015] 进一步的，折角板的提取方法具体包括如下步骤：
[0016] S211、当前帧扫描数据从最小扫描角到最大扫描角顺序输出，依次顺序提取N条满足长度
阈值的线段：
[0017] S212、遍历N条线段，计算任意相邻两条线段的长度及两条线段的首尾距离，两条线段的首尾距离是指第一条线段的尾部距第二条线段头部的距离Dist，将满足L1-L2小于阈值，Dist小于预设阈值的两相邻线段点集执行步骤S213，其中，L1、L2分别是指第一条线段和第二条线段的距离。
[0018] S213、计算两相邻线段的夹角θ，及夹角θ与折角板夹角的差值，若该差值位于允许的偏差范围，则认定该相邻线段即为折角板两面板的投影线段；
[0019] S214、计算各折角板中第一条线段的剔除比例系数div，基于剔除比例系数div在第一点段的尾部来剔除属于第二线段的点集，并执行步骤S15；
[0020] S215、对各折角板的两相邻线段分别进行RANSAC内点提取，找到内点最多的两条直线，对内点进行直线拟合，形成折角板两面板的投影线段，计算各折角板两面板投影线段的交点p′(x′,y′)及折角板两面板投影线段的矢量v1′(x1′,y1′),v2′(x2′,y2′)。
[0021] 进一步的，所述S211中的线段提取方法具体如下
[0022] S2111、基于当前帧的前两点来计算线段L；
[0023] S2112、检测下一个点到线段L的距离是否小于距离阈值T，若检测结果为是，执行步骤S2113，若检测结果为否，则执行步骤S2114；
[0024] S2113、将下一个点放入线段的点集中，对点集进行线性最小二乘拟合，更新线段L，执行步骤S2112；
[0025] S2114、计算线段L的长度DL，若legth_min≤DL≤length_max，则更新线段L，将点集保存到线集合，将点集中最后一个点的后面两个点作为下一条线段的线段起点，并计算线段L，执行步骤S2112，直至遍历完当前帧扫描数据中的所有点。
[0026] 进一步的，所述步骤S1具体包括如下步骤：
[0027] S11、初始化粒子分布：确定移动机器人在栅格地图上的初始位姿，在初始位姿周围产生随机粒子
[0028] S12、运动模型更新：根据
里程计反馈的位移增量(Δx,Δy,Δθ)，更新每一个粒子的位姿；
[0029] S13、激光传感器观测模型更新：根据随机粒子在全局地图中的位置，采用似然域计算各随机粒子的权重，并归一化随机粒子的权重值
[0030] S14、进行重采样：筛选出权重值大的随机粒子，同时增加随机粒子筛选出权重值大的随机粒子，同时增加随机粒子
[0031] S15、粒子
滤波器迭代更新：重采样后，更新粒子分布，使得每个粒子的权重相等，更新粒子簇的最大权重，若粒子的最大权重大于权重阈值，则最大权重粒子簇的平均位置即为最佳位置，即移动机器人当前在全局地图中的位置。
附图说明
[0032] 图1为本发明实施例提供的融合折角板自然定位方法
流程图；
[0033] 图2为本发明实施例提供的折角板的结构示意图。
具体实施方式
[0034] 为了使本发明的目的、技术方案及优点更加清楚明白，以下结合附图及实施例，对本发明进行进一步详细说明。应当理解，此处所描述的具体实施例仅仅用以解释本发明，并不用于限定本发明。
[0035] 图1为本发明实施例提供的融合折角板自然定位方法流程图，该方法包括如下步骤：
[0036] S1、加载栅格地图及折角板地图，栅格地图是通过SLAM
算法构建的，并进行存储，供自然导航使用的地图；折角板地图是构建栅格地图时识别到的折角板，并计算得到在栅格地图坐标系中的直线方程集合。
[0037] S2、在非精定位区域，通过粒子滤波来计算移动机器人当前的位姿，即进行移动机器人的粗定位，其定位方法具体包括如下步骤：
[0038] S11、初始化粒子分布：确定移动机器人在栅格地图上的初始位姿，在初始位姿周围产生随机粒子；
[0039] S12、运动模型更新：根据里程计反馈的位移增量(Δx,Δy,Δθ)，更新每一个粒子的位姿，对于粒子pi(xi,yi,θi)，位姿更新如下所示：
[0040]
[0041] S13、激光传感器观测模型更新：根据随机粒子在全局地图中的位置，采用似然域计算各随机粒子的权重，并归一化随机粒子的权重值，在本发明实施例中，似然域模型表示具体如下：
[0042]
[0043]
[0044] 其中，dist表示随机粒子距最近障碍物距离，q表示测距似然，代表粒子的权重，(xk,yk)为随机粒子在全局地图(栅格地图)的坐标，(x,y)为距离最近的障碍物坐标，zhit、zranddom及zmax分别代表测距误差混合权重的不同部分，分别代表测量噪声、无法解释的随机测量及测量失败，σhit为测量噪声的标准差。
[0045] S14、进行重采样：筛选出权重值大的随机粒子，同时增加随机粒子，随机粒子以以下概率增加，概率的计算模型具体如下：
[0046] max{0.0，1.0-wfast/wslaw}
[0047] 其中，wslow表示短期似然平均，wfast表示长期似然平均，其计算公式具体如下：
[0048]
[0049] 其中，wavg表示经验测量似然，αslow表示短期平均的指数滤波器的衰减率，αfast表示长期平均的指数滤波器的衰减率。
[0050] S15、粒子滤波器迭代更新：重采样后，更新粒子分布，使得每个粒子的权重相等，更新粒子簇的最大权重，若粒子的最大权重大于权重阈值，则最大权重粒子簇的平均位置即为最佳位置，即移动机器人当前在全局地图中的位置。
[0051] 在本发明实施例中，粒子簇的权重为粒子簇中各粒子权重的总和。
[0052] S3、在精定位区域，将上一时刻粗定位的获取位姿作为精定位的初始位姿，通过折角板来进行精定位；
[0053] 在本发明实施例中，折角板是由两个呈一定角度的单平面构成，其结构如图2所示，精定位过程具体包括如下步骤：
[0054] S21、提取当前帧中的折角板，折角板的提取方法具体如下：
[0055] S211、当前帧扫描数据从最小扫描角到最大扫描角顺序输出，依次顺序提取N条满足长度阈值的线段，线段提取方法具体如下：
[0056] S2111、基于当前帧的前两点来计算线段L；
[0057] S2112、检测下一个点到线段L的距离是否小于距离阈值T，若检测结果为是，执行步骤S2113，若检测结果为否，则执行步骤S2114；
[0058] S2113、将下一个点放入线段的点集中，对点集进行线性最小二乘拟合，更新线段L，执行步骤S2112；
[0059] S2114、计算线段L的长度DL，若legth_min≤DL≤length_max，则更新线段L，将点集保存到线集合，将点集中最后一个点的后面两个点作为下一条线段的线段起点，并计算线段L，执行步骤S2112，直至遍历完当前帧扫描数据中的所有点，线集合中提取有N条满足线段长度阈值要求的线段点集，长度阈值的最小长度设为legth_min，长度阈值的最大长度设为length_max。
[0060] S212、遍历N条线段，计算任意相邻两条线段的长度及两条线段的首尾距离，两条线段的首尾距离是指第一条线段的尾部距第二条线段头部的距离Dist，将满足L1-L2小于阈值，Dist小于预设阈值的两相邻线段点集执行步骤S213，其中，L1、L2分别是指第一条线段和第二条线段的距离。
[0061] S213、计算两相邻线段的夹角θ，及夹角θ与折角板夹角的差值，若该差值位于允许的偏差范围，则认定该相邻线段即为折角板两面板的投影线段；
[0062] 计算两相邻线段的交点，由交点和两相邻线段点集中的两数据点分别构建两单位矢量 和 使用矢量点乘计算夹角θ：
[0063]
[0064] S214、计算各折角板中第一条线段的剔除比例系数div，基于剔除比例系数div在第一点段的尾部来剔除属于第二线段的点集，并执行步骤S15；
[0065] 筛选折角板的第一条线段的原始点集，将原本属于第二条线段的点集剔除，剔除比例系数div由折角板夹角θ和距离阈值T得出：
[0066]
[0067] 其中，a为折角板单面板在投影方向上的实际距离。
[0068] S215、对各折角板的两相邻线段分别进行RANSAC内点提取，找到内点最多的两条直线，对内点进行直线拟合，形成折角板两面板的投影线段，计算各折角板两面板投影线段的交点p′(x′,y′)及折角板两面板投影线段的矢量v1′(x1′,y1′),v2′(x2′,y2′)；
[0069] 分别对两个线段点集进行RANSAC内点提取，具体设定了RANSAC直线内点判定阈值，设定迭代停止次数阈值，最终在线段原始点集中找到两点所在的直线内点数最多，保存内点点集，重新进行线性最小二乘拟合，得到折角板的两条高精度线段，计算两线段的交点和矢量，折角板两面板投影线段的交点p′(x′,y′)及折角板两面板投影线段的矢量v1′(x1′,y1′),v2′(x2′,y2′)。
[0070] S22、基于折角板来计算激光雷达当前在地图坐标系中的位姿；
[0071] 在本发明实施例中，激光雷达当前在地图坐标系中的位姿计算方法具体如下：
[0072] S221、在指定距离内提取距激光雷达最近的折角板b′(p′(x′,y′),v1′(x1′,y1′),v2′(x2′,y2′))，其中，p′(x′,y′)是折角板b′两单平面交线的二维投影点，v1′(x1′,y1′)，v2′(x2′,y2′)分别为折角板的两单平面的二维投影；
[0073] S222、基于激光雷达上一帧中的位姿将折角板b′的交线投影点p′(x′,y′)投影到栅格地图中，并在栅格地图中提取距离交线投影点p′(x′,y′)距离最近的折角板b(p(x,y),v1(x1,y1),v2(x2,y2))，折角板b即为折角板b′在栅格地图中的坐标；
[0074] S223、当前帧中折角板b′(p′(x′,y′),v1′(x1′,y1′),v2′(x2′,y2′))的位置及在栅格地图中的匹配折角板b(p(x,y),v1(x1,y1),v2(x2,y2))的位置来计算机器人当前在地图坐标系中的位姿，即激光雷达当前在地图坐标系中的位姿P(x,y,θ)，其计算公式具体如下：
[0075]
[0076] 其中，R、T分别为地图坐标系到雷达坐标系的旋转矩阵和平移向量，基于R、T计算得到机器人的位姿P(x,y,θ)：
[0077] pos＝-R-1*T
[0078] θ＝arctan(R1,0/R0,0)
[0079] 其中，R1,0/R0,0为旋转矩阵R第二行第一列除以旋转矩阵第一行第一列，pos为移动机器人位置坐标，θ为移动机器人的
姿态角，移动机器人的位置坐标及姿态角即构成为机器人的位姿。
[0080] 本发明还提供了一种融合折角板的自然定位系统，该系统包括：
[0081] 设于精定位区域的折角板，折角板是由两个呈一定角度的平面构成，折角板垂直于地面布置，设于平设于移动机器上的激光雷达，激光雷达与处理器连接，处理器与
存储器连接，激光雷达扫描行驶区域的行驶环境(行驶区域有精定位区域和非精定位区域组成)，并将扫描数据发送至处理器，处理器基于上述融合折角板的自然定位方法来进行移动机器人的定位；存储器中存储有栅格地图及折角板地图，栅格地图是通过
SLAM算法构建的，并保存到存储器，供自然导航使用的地图；折角板地图是构建栅格地图时识别到的折角板，并计算得到在栅格地图坐标系中的直线方程集合。
[0082] 本发明提供的融合折角板的自然定位方法具有如有益效果：1)基于一个折角板即可实现移动机器人的定位，减小对应用场景的依赖；2)仅在需要精定位的区域布置折角板，在粗定位区域无需布置折角板，既满足了定位精度的需求，也减小了对应用场景的依赖。
[0083] 以上所述仅为本发明的较佳实施例而已，并不用以限制本发明，凡在本发明的精神和原则之内所作的任何
修改、等同替换和改进等，均应包含在本发明的保护范围之内。
