基于激光和视觉的移动机器人的混合定位方法
【专利摘要】本发明揭示了一种基于激光和视觉的移动机器人的混合定位方法，所述移动机器人包括激光雷达和视觉传感器，本发明根据激光雷达采集的数据和视觉传感器采集的数据对各粒子的预测位置的权值进行更新，然后对权值高的粒子进行重采样，得到移动机器人在t时刻的真实位置分布。与现有技术相比，本发明技术方案综合了激光雷达的精度高特性和视觉的信息完整性，可适用范围更广，并弥补了视觉定位不稳定的缺点；此外，在本发明的一个实施例中还对传统的粒子滤波采样模型进行了改进，使粒子的多样性得到了保证。
【专利说明】
基于激光和视觉的移动机器人的混合定位方法
技术领域
[0001] 本发明涉及移动机器人的定位方法，特别是基于激光和视觉的移动机器人的混合 定位方法。
【背景技术】
[0002] 随着移动机器人的不断发展，同时定位与建图（SLAM)已经成为了移动机器人发展 的一个重大命题。如何解决此问题，有多种方法，从模型上分，分为概率定位模型(如粒子滤 波)和绝对定位模型(如GPS)等;从地图来分有栅格地图，拓扑地图等;从传感器角度分有超 声定位，激光雷达定位，视觉定位等。
[0003] 概率模型内存和CPU的限制，绝对定位模型需要一些设备如：人工信标，或者一些 特殊环境，如室内就不适合GPS;栅格地图会存在着栅格误差，拓扑地图维护成本比较大，摧 毁后难以恢复;超声存在着精度问题；目前正常的激光雷达都是二维激光雷达，维度低，对 环境的识别度不高;视觉定位主要有基于特征的定位，基于三维图匹配的定位，基于三维点 簇的定位等，视觉定位算法目前不是很成熟。
[0004] 但是，对于移动机器人来说，定位不仅要有一定的精度，并且对于快速性也有很高 的要求。
[0005] 基于上述问题，发明人分别基于激光雷达定位和视觉定位的现有成果，综合考虑 激光雷达的快速准确优点和视觉传感器信息丰富的优点，通过粒子滤波算法，实现移动机 器人的快速、精确的定位。
【发明内容】
[0006] 本发明专利申请的目的在于提供一种用于移动机器人的快速、精确的定位方法， 以解决【背景技术】中的问题。
[0007] 为达到以上目的，本发明专利申请的技术方案如下： S1:初始化各粒子位置及各粒子的栅格地图和视觉特征地图，建立各粒子的全局栅格 地图和全局视觉特征地图； S2:初始化移动机器人的位置PtQ及建立移动机器人的激光雷达全局点图； S3:根据里程计和陀螺仪得到移动机器人在t时刻的预测位置Pt; S4:根据移动机器人的估计位置Pt和激光雷达在t时刻采集到的数据得到修正后的预 测位置P't; S5:根据移动机器人在t-1时刻和t时刻的位置变化和各粒子在t-1时刻的位置，采样得 至Ijt时刻各粒子的估计位置； S6:根据t时刻激光雷达采集的数据得到t时刻各粒子的估计位置的概率PL; S7:根据t时刻视觉传感器采集的图像得到t时刻各粒子的估计位置的概率PV; S8:根据各粒子的估计位置的概率PL、PV对粒子权值进行更新； S9:对权值高的粒子进行重采样，得到移动机器人在t时刻的真实位置分布。
[0008] 与现有技术相比，本发明申请具有以下优点： 1、 本发明技术方案综合了激光雷达的精度高特性和视觉的信息完整性，可适用范围更 广(室内室外都可），通过激光雷达与视觉传感器的结合，弥补了视觉定位不稳定的缺点； 2、 本发明技术方案对传统的粒子滤波采样模型进行了改进，使粒子的多样性得到了保 证； 3、 本发明技术方案因引入自适应因子，可根据环境的情况实时调度激光雷达与视觉传 感器的权重。
【附图说明】
[0009] 图1为本发明申请的流程图； 图2为本发明实施例中的移动机器人的示意图。
【具体实施方式】
[0010]下面结合附图和【具体实施方式】对本发明方案进行进一步详细说明。
[0011] 如图2所示，本实施例中的移动机器人包括里程计、陀螺仪、激光雷达、摄像机(视 觉传感器）、驱动系统和核心控制板(M⑶），具体地，本实施例中的驱动系统由左、右轮构成， 并分别由不同的马达驱动，应该可以理解的是，移动机器人还可包括其他部分（如吸尘系 统、传感器系统、报警系统等），由于与本发明的技术方案无关，因此不在此说明。
[0012] 如图1所示，本发明移动机器人的基于粒子滤波算法的定位方法包括以下步骤： S1:初始化各粒子位置及各粒子的栅格地图和视觉特征地图，建立各粒子的全局栅格 地图和全局视觉特征地图。
[0013] 初始化各粒子位置及各粒子的栅格地图的过程如下： 在栅格地图中，将移动机器人的初始位置坐标设定为(〇，〇，〇)，则在坐标点(〇，〇，〇)附 近随机生成由N个粒子s\、权值为Λ的组成的粒子集StHsV w\}，其中k=l，2,. . .N，这N个 粒子都是移动机器人下一时刻可能达到的位置，由于不知道移动机器人的运动速度和方 向，假定移动机器人下个时刻到这些粒子的概率相等，因此各粒子的概率为1/N，在本实施 例中，粒子N的个数为50个，因此&={Λ，0.02}。由于此时移动机器人的各粒子只有一个栅 格地图，因此可将该栅格地图作为各粒子的全局栅格地图进行存储。
[0014] 初始化各粒子的视觉特征地图的过程如下： 将视觉传感器(如摄像机)采集的图像通过SIFT算法，建立图像金字塔，并对每层金字 塔提取特征，然后得到128维描述子的视觉特征地图。同样地，由于此时各粒子只有一个视 觉特征地图，因此将其作为各粒子的全局视觉特征地图进行存储。
[0015] S2:初始化移动机器人的位置PtQ及建立移动机器人的激光雷达全局点图。
[0016] 激光雷达可以直接获得环境的水平剖面图，测得的环境点的信息是2D极坐标，表 示为，其中，r表示2D激光雷达扫描获得的距离值，Ψ为水平扫描角。激光雷达通过T0F、三角 测距等原理将移动机器人在位置Pto的障碍信息转换为特征点坐标，由此建立起初始的激光 雷达全局点图并存储。
[0017] S3:根据里程计和陀螺仪得到移动机器人在t时刻的预测位置Pt。
[0018] 本步骤是通过以下子步骤实现的： S31:通过航迹推算得到移动机器人的预测位置Pt=[x(t)，y(t)，theta(t)]，航迹推算 公式如下：
其中，[x(t)，y(t)]为移动机器人t时刻的位置，theta(t)为移动机器人t时刻航向，[X (t-1) y(t-l)]为移动机器人t-Ι时刻的位置，theta(t-l)为移动机器人t-Ι时刻航向，sr (卜1)、81(卜1)为移动机器人卜1时刻到切寸刻右轮、左轮行走的距离，13为左、右轮之间的轮 轴距。
[0019] 为了便于理解，假设t-Ι时刻为移动机器人初始化的时刻，移动机器人在初始化时 朝着叉轴正方向，因此，1^(1：-1)，7(1：-1)，1:116七3(1：-1)] = [0,0,0]，进而得到|^(1:)，7(1:) ,theta(t)]= [(sr(t-l)+sl(t-l))/2,0, (sr(t~l)+sl(t-l))/b]〇
[0020] S32:陀螺仪更新航迹推算所得到的航向theta(t); 1±6七3(1：)=1：116七36(1：)，其中1：116七36(1：)为1：时刻陀螺仪的读数，本实施例中直接用陀螺 仪读数替换了里程计航迹推导出来的角度theta(t)，原因是陀螺仪得到的角度非常高。
[0021] S4:根据移动机器人的估计位置Pt和激光雷达在t时刻采集到的数据得到修正后 的预测位置P't。
[0022] 步骤S4是通过以下方式实现的： S41:将t时刻采集到的激光雷达数据坐标转换到激光雷达全局点图； XG(k)=XL(k)*cos(theta(t))_YL(k)*sin(theta(t))+x(t); YG(k)=XL(k)*sin(theta(t))+YL(k)*cos(theta(t))+y(t); 其中[XL(k)，YL(k)]为当前移动机器人位置激光雷达采到的某一个(第k个)数据；[XG (k)，YL(k)]为将第k个数据转换到全局的全局坐标；[奴0,7(0，也的&(0]为丨时刻移动机 器人的位置和航向，即Pt。
[0023] S42:对转换后的各激光雷达数据坐标寻找满足阈值条件的匹配点，并保存所有的 匹配对； 每个转换后的激光雷达数据坐标只有一个匹配点。比如一个点在l〇cm范围（阈值一般 10cm或者5cm)内有多个匹配点存在，那就选取离它最近的那个匹配点，如果存在两个或两 个以上的最近匹配点，则随机选择一个。
[0024] S43:通过最小二乘法求得使匹配对模型函数最小的移动机器人位置； 即通过里程计和陀螺仪结合运动模型计算出来的初始预测Pt，再融合激光定位算法， 通过ICP匹配相连俩帧激光数据，通过最小二乘法得到修正后的移动机器人预测位置，具体 如下：
其中X为所有匹配对的局部坐标，即激光雷达采集到的数据，坐标转换之前的坐标所组 成的矩阵;X形式为
；Y为所有匹配对的全局坐标，即采集到的数据匹配 上的全局地图上的点组成的矩阵，Υ的形式为：
，ΧΤ，ΥΤ为Χ，γ的转置;Α为转换关系，A 的形式为
，其中[x(t)，y(t)，theta(t)]为t时刻移动机器人位 置； S44:重复步骤S41至S43直到移动机器人位置收敛，得到修正后的移动机器人位姿P't。
[0025] 此处的收敛条件是通过迭代计算得到的移动机器人位置和航向几乎不再发生变 化，即在在极小的范围（可以忽略不计的范围）内波动或者不再变化。
[0026] S5:根据移动机器人在t-Ι时刻和t时刻的位置变化和各粒子在t-Ι时刻的位置，采 样得到t时刻各粒子的估计位置。
[0027] 如果t时刻是第一次对移动个机器人进行定位，在t-Ι时刻为移动机器人初始化的 时刻，具体实现过程如下： xp(i,t)=xp(i,t-l)+(x(t)-x(t-l))；yp(i,t)=yp(i,t-l)+(y(t)-y(t-l))； 其中[xp(i ,t) yp(i,t)]，[xp(i ,t_l) 7口(;[，1：-1)]分别为某一个粒子(第;[个粒子)1:时 刻和t-1时刻的位置；[x(t) y(t)]，[x(t_l) y(t-l)]为ICP定位出来的移动机器人t时刻和 t_l时刻的位置。
[0028] S6:根据t时刻激光雷达采集的数据得到t时刻各粒子的估计位置的概率PL; 步骤S6在本实施例中根据scan matching算法得到t时刻各粒子位置坐标的估计概率 PL，具体实现过程如下： S61:采集t时刻激光雷达扫描的数据信息并通过某一粒子的位置坐标转换到该粒子全 局坐标系； XG(k)=XL(k)*cos(thetap(i,t))-YL(k)*sin(theta(i))+xp(i,t); YG(k)=XL(k)*sin(thetap(i，t))+YL(k)*cos(theta(i))+yp(i，t); 其中[XL(k)，YL(k)]为当前移动机器人位置激光雷达采到的某一个(第k个)数据；[XG (k)，YL(k)]为将第k个数据转换到全局坐标系（即世界坐标系）的全局坐标，即在全局坐标 系（即世界坐标系）形成一个坐标点，在下面简称为点图；[xp(i，t)，yp(i，t)，thetap(i，t)] 为某一个粒子(第i个粒子)t时刻的位置和航向。
[0029] S62:将转换后得到的点图转换为该粒子的概率栅格地图； 该步骤在本实施例中的具体如下： 首先，将转换后的点图中各位置的概率设置为0.5;接着，将0.5用log算子log化得到 1〇区形式概率?以18,78)=1<^(?(18,78)/(1-?(18,78)))，其中？(1，7)为1<^化之后位置(叉区， yg)的log概率，P(xg，yg)为真实概率即转换之前的概率;然后，转换采集到的数据到栅格地 图： xgrid(k)=XG(k)/d;ygrid(k)=YG(k)/d;其中，[XG(k)，YG(k)]步骤S61 得到的某一个数 据的在全局坐标系下的全局坐标;d为栅格的分辨率(一般为5cm) ; [xgrid(k) ygrid(k)]为 转换之后的栅格位置;再按照二维高斯分布得到此栅格位置周围3*3范围内的概率;最后将 得到的概率用log算子公式转换为log形式概率;从而各栅格位置log形式概率为 PL(xg，yg)=PLb(xg，yg)+PLa(xg，yg);其中PL(xg，yg)，PLb(xg，yg)，PLa(xg，yg)分别为 位置[xg，yg]处的更新之后，更新之前，和要更新进去(加入进去）的概率。
[0030] S63:通过条件概率公式和log算子得到该粒子的位置坐标的估计概率pi; 先将各栅格位置的概率从log形式转换到真实概率，即用上面log化算子的反函数转换 到真实概率； P(xg，yg) = l-1/ (l + exp(PL(xg，yg));其中exp为自然指数函数；然后根据
[0031] S64:重复步骤S61至S63直到完成所有粒子的位置坐标的估计概率。
[0032] S7:根据t时刻视觉传感器采集的图像得到t时刻各粒子的估计位置的概率PV。
[0033] S71:对t时刻采集到的视觉图像进行高斯滤波得到高斯金字塔； 将摄像机采集的图像通过SIFT算法，建立图像金字塔，并对每层金字塔提取特征，然后 得到128维描述子的视觉特征地图。
[0034] S72:寻找关键点并删除错误的关键点； 关键点就是突变点，或者说局部极值点（分界点），错误的关键点即是不能完全满足突 变点的条件的点，它满足一部分，有可能一开始会被误选中。由于关键点的寻找和错误点的 删除是本领域技术人员的一般常识，在这里就不详细说明了。
[0035] S73:求得关键点的主方向并生成SIFT描述子，从而得到特征地图； S74:由t时刻的各粒子的位置坐标将特征地图进行坐标转换，从而得到各粒子的视觉 特征地图； S75:通过匹配各粒子的视觉特征地图和全局视觉特征地图得到各粒子的估计位置的 概率PV; S8:根据各粒子的估计位置的概率PL、PV对粒子权值进行更新。
[0036]对各粒子的权值更新过程如下： S81:根据t时刻各粒子的栅格地图匹配率得到t时刻栅格地图匹配率的分布； S82:根据t时刻各粒子的视觉特征地图匹配率得到t时刻视觉特征地图匹配率的分布； S83:通过联合概率抽样统计方法得到自适应因子omega;
S84:由自适应因子omega加权各粒子的激光雷达栅格地图估计概率PL和视觉特征地图 估计概率PV，得到各粒子的概率P(k)，计算公式如下： 其中，P(k)为第k个粒子的权值，PL(k)，PV(k)分别为第k个粒子激光雷达地图概率和视觉特 征地图概率。
[0037] S9:对权值高的粒子进行重采样，得到移动机器人在t时刻的真实位置分布。
[0038] 通过设定阈值，舍弃权值小于阈值的粒子，复制权值高的粒子，使得粒子总数保持 不变。
[0039]以上是本发明的较佳实施例的详细说明，不认定本发明只局限于这些说明。对于 本发明所属技术领域的普通技术人员来说，在不脱离本发明构思的前提下所作出的等同替 代或明显变形，且性能或用途相同，都应当视为本发明所提交的权利要求书确定的保护范 围内。
【主权项】
1. 一种基于激光和视觉的移动机器人的混合定位方法，其特征在于，所述移动机器人 包括里程计和陀螺仪、激光雷达和视觉传感器，所述方法包括以下步骤： S1:初始化各粒子位置及各粒子的栅格地图和视觉特征地图，建立各粒子的全局栅格 地图和全局视觉特征地图； S2:初始化移动机器人的位置PtQ及建立移动机器人的激光雷达全局点图； S3:根据里程计和陀螺仪得到移动机器人在t时刻的预测位置Pt; S4:根据移动机器人的估计位置Pt和激光雷达在t时刻采集到的数据得到修正后的预测 位置P't; S5:根据移动机器人在t-1时刻和t时刻的位置变化和各粒子在t-1时刻的位置，采样得 到t时刻各粒子的估计位置； S6:根据t时刻激光雷达采集的数据得到t时刻各粒子的估计位置的概率PL; S7:根据t时刻视觉传感器采集的图像得到t时刻各粒子的估计位置的概率PV; S8:根据各粒子的估计位置的概率PL、PV对粒子权值进行更新； S9:对权值高的粒子进行重采样，得到移动机器人在t时刻的真实位置分布。2. 如权利要求1所述的基于激光和视觉的移动机器人的混合定位方法，其特征在于，所 述移动机器人为包括左右轮的移动机器人，所述步骤S2包括以下子步骤： S31:通过航迹推算得到移动机器人的位姿Pi[x(t)，y(t)，theta(t)]，航迹推算公式如 下：其中，[x(t)，y(t)]为移动机器人t时刻的位置，theta(t)为移动机器人t时刻航向，[X (t-1) y(t-l)]为移动机器人t-Ι时刻的位置，theta(t-l)为移动机器人t-Ι时刻航向，sr (t-1 )、sl (t-1)为移动机器人t-1时刻到t时刻右轮、左轮行走的距离，b为左、右轮之间的轮 轴距； S32:陀螺仪更新航迹推算所得到的航向theta(t)。3. 如权利要求1所述的基于激光和视觉的移动机器人的混合定位方法，其特征在于，所 述步骤S4包括以下子步骤： S41:将t时刻采集到的激光雷达数据坐标转换到激光雷达全局点图； S42:对转换后的各激光雷达数据坐标寻找满足阈值条件的匹配点，并保存所有的匹配 对； S43:通过最小二乘法求得使匹配对模型函数最小的移动机器人位置； S44:重复步骤S41至S43直到移动机器人位置收敛，得到修正后的移动机器人位姿P't。4. 如权利要求1所述的基于激光和视觉的移动机器人的混合定位方法，其特征在于，所 述步骤S6包括： S61:采集t时刻激光雷达扫描的数据信息并通过某一粒子的位置坐标转换到该粒子全 局坐标系； S62:将转换后得到的点图转换为该粒子的概率栅格地图； S63:通过条件概率公式和log算子得到该粒子的位置坐标的估计概率pi; S64:重复步骤S61至S63直到完成所有粒子的位置坐标的估计概率。5. 如权利要求1所述的基于激光和视觉的移动机器人的混合定位方法，其特征在于，所 述步骤S7包括： S71:对t时刻采集到的视觉图像进行高斯滤波得到高斯金字塔； S72:寻找关键点并删除错误的关键点； S73:求得关键点的主方向并生成SIFT描述子，从而得到特征地图； S74:由t时刻的各粒子的位置坐标将特征地图进行坐标转换，从而得到各粒子的视觉 特征地图； S75:通过匹配各粒子的视觉特征地图和全局视觉特征地图得到各粒子的估计位置的 概率PV。6. 如权利要求1所述的基于激光和视觉的移动机器人的混合定位方法，其特征在于，所 述步骤S8包括： S81:根据t时刻各粒子的栅格地图匹配率得到t时刻栅格地图匹配率的分布； S82:根据t时刻各粒子的视觉特征地图匹配率得到t时刻视觉特征地图匹配率的分布； S83:通过联合概率抽样统计方法得到自适应因子omega; S84:由自适应因子omega加权各粒子的激光雷达栅格地图估计概率PL和视觉特征地图 估计概率PV，得到各粒子的概率P(k)，计算公式如下：其中，P(k)为第k个粒子的权值，PL(k)，PV(k)分别为第k个粒子激光雷达地图概率和视 觉特征地图概率。
【文档编号】G01C21/20GK105865449SQ201610198296
【公开日】2016年8月17日
【申请日】2016年4月1日
【发明人】邓龙, 李崇国, 杨勇, 宫海涛
【申请人】深圳杉川科技有限公司