CN110989592A - 一种移动机器人自动建图与定位系统 - Google Patents

Abstract

本发明属于移动对象的定位与导航相关技术领域，其公开了一种移动机器人自动建图与定位系统，所述系统包括移动机器人、库前区、货车车厢及连接所述库前区及所述货车车厢的登车桥，所述移动机器人在所述库前区、所述登车桥及所述货车车厢之间进行移动及取放货物；所述移动机器人在所述库前区时，所述系统采用激光反光板定位方式对所述移动机器人进行定位；所述移动机器人在所述货车车厢时，所述系统采用基于直线特征的SLAM方式对所述移动机器人进行定位；所述移动机器人在上下登车桥时，所述系统采用中线检测方法对所述移动机器人进行位姿检测，以避免所述移动机器人在登车桥上因打滑而造成倾斜。本发明提高了定位进度及效率，降低了成本。

Description

一种移动机器人自动建图与定位系统

技术领域

本发明属于移动对象的定位与导航相关技术领域，更具体地，涉及一种移动机器人自动建图与定位系统。

背景技术

随着工业自动化技术的提高和对生产效率的要求，越来越多的仓储物流运输都开始采用AGV来代替人工搬运以提高运输效率及降低成本。AGV实现库区自动搬运的前提就是要能够熟悉库区环境并且能够实现实时定位与自动导航的功能，在这种情况下，能否实现AGV小车的自主定位与高精度导航是能否完成自动搬运的关键。

目前，AGV在室内定位通常是采用基于激光反光板的三点定位，这种定位方式的优点是在库区等比较大的环境中可以有较高的精度以及较好的稳定性。但是这种单纯的激光反光板定位导航方式仅仅限于在环境固定的室内使用，对于需要进入货车车厢上下货的情况下则无法使用。现阶段对于大多数货车车厢内的上下货仍然采用人工搬运的方式，通过人工搬运到厂区内然后采用AGV进行自动搬运，或者通过在车厢内部改造或者加装移栽机构，难以满足要求。由此，对于需要从车厢上下货的任务，目前亟需解决车厢这种动态环境的建图与定位问题。

发明内容

针对现有技术的以上缺陷或改进需求，本发明提供了一种移动机器人自动建图与定位系统，其基于现有移动机器人的工作特点，研究及设计了一种能够在动态环境中定位的移动机器人自动建图与定位系统。所述系统结合了激光反光板定位和激光SLM定位的优势，可以完成移动机器人从库区静态环境到货车车厢等动态环境中上下货的功能，同时所述系统采用基于运动预测及匹配更新的三点定位的反光板定位方法，在环境较为复杂的库区环境也有较高的精度，且采用基于直线特征的SLAM算法，对不同大小与规格的货车车厢均有一定的适应性。此外，所述系统在移动机器人在上登车桥时采用中线检测方法，以避免移动机器人在坡上因打滑造成的车体倾斜，所述系统可以构建一种激光反光板和动态直线特征地图的融合地图形式，以满足不同位置的车厢和不同大小车厢的建图与定位功能。

为实现上述目的，按照本发明的一个方面，提供了一种移动机器人自动建图与定位系统，所述系统包括移动机器人、库前区、货车车厢及连接所述库前区及所述货车车厢的登车桥，所述移动机器人在所述库前区、所述登车桥及所述货车车厢之间进行移动及取放货物；

所述移动机器人在所述库前区时，所述系统采用激光反光板定位方式对所述移动机器人进行定位；所述移动机器人在所述货车车厢时，所述系统采用基于直线特征的SLAM方式对所述移动机器人进行定位；所述移动机器人在上下登车桥时，所述系统采用中线检测方法对所述移动机器人进行位姿检测，以避免所述移动机器人在登车桥上因打滑而造成倾斜。

进一步地，采用基于直线特征的SLAM方式对所述移动机器人进行定位时包括以下步骤：

(1)以移动机器人为原点在车厢内建立局部坐标系，通过激光雷达扫描车厢内壁轮廓以得到一系列激光扫描点；接着，采用迭代适应点算法将扫描点分割成不同区域点集，并采用最小二乘法将各点集中的点拟合成直线，进而建立车厢内环境的线段特征地图；

(2)根据所述线段特征地图中的直线特征，采用优化加权匹配误差的方式对所述移动机器人进行定位。

进一步地，线段特征地图中的直线特征的表达式为：

式中，

表示第i条直线特征到线段特征地图原点的距离值；

表示线段特征地图原点到第i条直线的垂线与X轴正半轴的夹角。

进一步地，步骤(2)中，首先，采用移动机器人的里程计运动模型预测移动机器人在预定时刻的姿态，继而生成直线特征的预测观测直线列表；接着，将直线特征的预测观测直线列表与实际观测直线列表进行匹配，匹配成功后，将实际观测到的直线特征根据预测的移动机器人坐标转化到世界坐标系，并求出对应的直线特征，继而依据得到的直线特征对移动机器人进行定位。

进一步地，如果满足:

则表明直线特征匹配成功；其中，ρ_i，

为预测观测的直线特征；ρ_j，

为实际观测的直线特征；ρ_th，

分别为直线特征距离和角度匹配阈值。

进一步地，采用动态窗口法在里程计预测坐标X_t+1＝(x_t+1，y_t+1，θ_t+1)附近范围进行搜索，即在x_t+1∈(x_t+1-v_x*δt，x_t+1+v_x*δt),y_t+1∈(y_t+1-v_y*δt，y_t+1+v_y*δt)的范围内进行搜索，以确定移动机器人的坐标。

进一步地，采用激光反光板定位方式对所述移动机器人的定位包括以下步骤：

S1，在所述库前区范围内随机设置多个反光板，并将所述移动机器人在初始位置时，激光雷达扫描到的反光板添加到反光板地图中，之后启动所述移动机器人以进行反光板的标定；

S2，基于所述移动机器人的运动模型预测预定时刻时所述移动机器人扫描到的反光板列表，并将得到的预测反光板列表与实际扫描得到的反光板列表进行匹配，匹配成功后根据所述反光板地图中的反光板坐标计算所述移动机器人的位姿；

S3，根据所述移动机器人的坐标继续添加新的反光板到所述反光板地图中，直至所述移动机器人遍历所述库前区，由此完成所述反光板地图的构建；

S4，依据所述反光板地图对所述移动机器人进行定位。

进一步地，在构建所述反光板地图时，只采用离所述移动机器人预定距离范围内的反光板；在利用所述反光板进行定位时，同样只用预定距离范围内的反光板进行定位计算。

进一步地，初始标定反光板时，将所述移动机器人作为为反光板地图坐标系原点，且激光雷达在预定范围内能扫描到的反光板的数量至少为3个；根据反光板的反射强度和距离比值对每组反光板计算出来的移动机器人坐标进行加权平均处理。

进一步地，采用中线检测方法对所述移动机器人进行位姿检测时，在移动机器人上下登车桥的过程中对周围环境进行扫描，并提取登车桥环境中直线特征；然后根据提取到的直线特征的相互关系判别出属于车厢内壁的两条直线，依据得到的两条直线特征检测登车桥中线的位置及移动机器人是否发生倾斜。

总体而言，通过本发明所构思的以上技术方案与现有技术相比，本发明提供的移动机器人自动建图与定位系统主要具有以下有益效果：

1.所述移动机器人在所述库前区时，所述系统采用激光反光板定位方式对所述移动机器人进行定位；所述移动机器人在所述货车车厢时，所述系统采用基于直线特征的SLAM方式对所述移动机器人进行定位，由此结合了反光板定位及SLAM定位的优势，不仅提高了小车的定位精度，而且解决了小车进出货车车厢等动态环境取放货物的问题，提高了效率，节省了人力。

2.所述移动机器人在上下登车桥时，所述系统采用中线检测方法对所述移动机器人进行位姿检测，以避免所述移动机器人在登车桥上因打滑而造成倾斜。

3.采用动态窗口法在里程计预测坐标X_t+1＝(x_t+1，y_t+1，θ_t+1)附近范围进行搜索以确定移动机器人的坐标，提高了移动机器人坐标的计算精度。

4.根据反光板的反射强度和距离比值对每组反光板计算出来的移动机器人坐标进行加权平均处理，提高了精度，减小了激光雷达距离越大误差越大所产生的影响。

附图说明

图1是本发明提供的移动机器人自动建图与定位系统的工作示意图；

图2是移动机器人在库前区基于激光反光板的三点定位算法的示意图；

图3是图1中的移动机器人自动建图与定位系统对移动机器人在车厢内进行SLM定位的线段特征观测模型示意图；

图4是移动机器人在登车桥上行进时检测中线的示意图。

具体实施方式

为了使本发明的目的、技术方案及优点更加清楚明白，以下结合附图及实施例，对本发明进行进一步详细说明。应当理解，此处所描述的具体实施例仅仅用以解释本发明，并不用于限定本发明。此外，下面所描述的本发明各个实施方式中所涉及到的技术特征只要彼此之间未构成冲突就可以相互组合。

请参阅图1、图2、图3及图4，本发明提供的移动机器人自动建图与定位系统，所述系统主要包括移动机器人、库前区、货车车厢及连接所述库前区及所述货车车厢的登车桥，所述移动机器人在所述库前区、所述登车桥及所述货车车厢之间进行移动及搬运货物。所述移动机器人在所述库前区时，采用激光反光板进行对移动机器人进行定位及导航。所述移动机器人在所述货车车厢时，采用线段特征SLM方法对所述移动机器人进行定位。所述移动机器人在上下所述登车桥时，所述系统采用中线检测方法对所述移动机器人进行位姿检测，以避免所述移动机器人在坡上因打滑造成的车体倾斜。

所述移动机器人在所述库前区时，所述系统对所述移动小车的定位及导航主要包括以下步骤：

S1，在所述库前区范围内随机设置多个反光板，并将所述移动机器人在初始位置时，激光雷达扫描到的反光板添加到反光板地图中，之后启动所述移动机器人以进行反光板的标定。

具体地，在所述库前区范围内，采用基于激光反光板的建图定位方式。首先需要在所述库前区范围内随机布置好一定数量的反光板，如图1所示，反光板采用反光板导航区域的小圆圈表示；待反光板布置好之后开始进行反光板地图的标定。

其中，由于激光雷达受距离影响较大，在所述库前区构建所述反光板地图时，只保留离所述移动机器人预定范围内的反光板，将距离所述移动机器人较近或者较远地方探测到的反光板进行滤除，暂时不添加到反光板地图中。本实施方式中，所述预定范围为0.2m～10m。同样地，在利用所述反光板进行定位时，也同样需要对较近或者较远距离范围内的反光板进行滤除，只用预定距离范围内的反光板进行定位计算。

S2，基于所述移动机器人的运动模型预测预定时刻时所述移动机器人扫描到的反光板列表，并将得到的预测反光板列表与实际扫描得到的反光板列表进行匹配，匹配成功后根据所述反光板地图中的反光板坐标计算所述移动机器人的位姿。

具体地，初始标定反光板时，将所述移动机器人定义为反光板地图坐标系原点O_w(0，0),所述移动机器人的初始姿态定义为X₀＝(0，0，0)，此时需要保证激光雷达在有效距离范围内能够扫描到至少3块反光板，所述系统将扫描到的反光板坐标添加到反光板地图中后，开启所述移动机器人开始运动，在所述移动机器人运动的过程中一边定位一边进行反光板的标定。

在进行定位之前，需要把激光雷达扫描到的反光板与已经标定好的反光板进行匹配。其中，反光板的匹配采用基于所述移动机器人的运行模型预测δt时间时所述移动机器人扫描的反光板列表，然后将得到的预测反光板列表与实际扫描到的反光板列表进行匹配，得到的匹配误差小于匹配误差阈值则表明匹配成功。

其中，反光板地图中的i个反光板坐标集合记为：

L_r＝{(x₁，y₁)，(x₂，y₂)，...，(x_i，y_i)}

式中，x_i，y_i分别表示第i个反光板在反光板地图坐标系中的X轴坐标值和Y轴坐标值。

记录所述移动机器人在t时刻位姿为X_t＝(x_t，y_t，θ_t),其中x_t、y_t、θ_t分别表示移动机器人对应于测量时刻t在反光板地图坐标系中的X轴坐标值、Y轴坐标值和方向角。假设移动机器人此时能够观测到所有i个反光板，然后根据移动机器人的运动学模型X_t+1＝f(X_t，U_t)预测t+1时刻移动机器人的姿态X_t+1＝(x_t+1，y_t+1，θ_t+1)，其中，U_t为t时刻移动机器人输入的运动控制信息。

本实施方式中，所述移动机器人为AGV小车；对于单舵轮AGV而言，其运动学模型的表达式为：

式中，β为AGV的舵角，并规定从参考点向前轮看逆时针为正，顺时针为负；L为AGV车身长度；v为AGV前轮驱动速度；θ_t为AGV的航向角，即车身轴线与X轴正方向的夹角；w为其航向角速度，定义逆时针为正。根据此运动学模型可以预测移动机器人的位姿为：

θ_t+1＝θ_t+w*δt

式中，δt为计算AGV坐标的时间间隔。

根据移动机器人预测的位姿计算小车与反光板列表中第i个反光板的距离d_pi和角度θ_pi,与t+1时刻激光雷达实际扫描到的距离d_ri和角度θ_ri进行匹配，如果匹配误差|d_pi-d_ri|和|θ_pi-θ_ri|均小于设定的阈值，则表明匹配成功，可以根据反光板地图中的反光板坐标计算移动机器人的坐标。

基于反光板的三点定位方法如图2所示，假设A、B、C三块反光板在世界坐标系中的坐标分别为(x_a，y_a)，(x_b，y_b)，(x_c，y_c),与激光雷达扫描距离分别为d_a,d_b,d_c,则实际移动机器人的坐标可表示为圆A和圆B的交线与圆A和圆C的交线的交点坐标(x，y)。其中圆A和圆B的交线方程为：

2(x_b-x_a)x+2(y_b-y_a)y+x_a ²-x_b ²+y_a ²-y_b ²+d_p ²-d_a ²＝0同理可得圆A和圆C的交线方程为：

2(x_c-x_a)x+2(y_c-y_a)y+x_a ²-x_c ²+y_a ²+d_c ²-d_a ²＝0

将上述两个直线方程联立可求解得到移动机器人的坐标(x，y)。

为了进一步提高激光反光板的定位精度，综合每个反光板的距离信息和反射强度信息，对每组计算得到的移动机器人的坐标进行加权平均处理。假设A,B,C三块反光板平均扫描距离

反射强度分别为r_a,r_b,r_c,平均反射强度

根据三点定位算法计算出移动机器人坐标为(x′，y′)。同理，对于e,f,g三块反光板可以得到平均扫描距离

平均反射强度

以及计算出来的小车坐标(x″，y″)。由于激光雷达距离越远误差越大，可根据反射强度和距离比值对每组反光板计算出来的移动机器人坐标进行加权平均处理。

如上所述，对于第一组a,b,c三个反光板，其强度距离比

同理，对于第二组e,f,g三块反光板，其强度距离比为R₂，则可根据此对计算出的坐标进行加权平均，即移动机器人的最终坐标为(x_r，y_r),其中:

此法也可用于计算三组或三组以上反光板来得到的移动机器人的坐标，则移动机器人的角度为：

式中，x_wi，y_wi分别为第i个反光板在世界坐标系的坐标；θ_ri为第i个反光板在移动机器人坐标系中的角度，可根据反光板在激光雷达中扫描点顺序求得；实际计算中可以将匹配到的反光板均计算一次角度，然后取平均值来减小随机误差。

S3，根据所述移动机器人的坐标继续添加新的反光板到所述反光板地图中，直至所述移动机器人遍历所述库前区，由此完成所述反光板地图的构建。

具体地，求得移动机器人在t+1时刻的实际坐标后，就可以根据得到的新坐标继续添加新的反光板到反光板地图中，直到移动机器人遍历完整个库前区环境为止。

S4，依据所述反光板地图对所述移动机器人进行定位及导航。

所述系统在构建完库前区的反光板地图之后，在每个登车桥处设立了一个固定点，如图1中的S1及S2，固定点的坐标采用反光板地图进行计算，并作为固定登桥点保证在每个登车桥的位置。对于进入某个车厢上下货的任务，只需要确定登车桥点的坐标即可进入对应的车厢。

在货车车厢内采用基于直线特征的SLAM方式实现移动机器人在车厢内的定位，具体包括以下步骤：

(1)以移动机器人为原点在车厢内建立局部坐标系，通过激光雷达扫描车厢内壁轮廓以得到一系列激光扫描点；接着，采用迭代适应点算法将扫描点分割成不同区域点集，并采用最小二乘法将各点集中的点拟合成直线，进而建立车厢内环境的线段特征地图。

具体地，在移动机器人进入车厢时，以移动机器人为原点在车厢内建立一个局部坐标系，通过激光雷达扫描车厢内壁轮廓以得到一系列激光扫描点；然后，通过迭代适应点算法将激光扫描点分割成不同区域点集P_{1，2，...，i}，最后通过最小二乘法将各点集中的点拟合为直线y_i＝k_ix+b_i，i为提取直线数量；然后，建立车厢内环境的线段特征地图，线段特征地图中的直线特征可以表示为：

其中，

表示第i条直线特征到线段特征地图原点的距离值；

表示线段特征地图原点到第i条直线的垂线与X轴正半轴的夹角。进一步地，可以将线段特征地图原点到第i条直线的垂线与第i条直线的交点(x_wi,y_wi)作为第i条直线的特征点,其中

由于车厢内环境比较封闭且结构简单，可以考虑在原点一次性建立线段特征地图，减少建图累积误差。

(2)根据所述线段特征地图中的直线特征，采用优化加权匹配误差的方式对所述移动机器人进行定位。

线段特征地图建立完后，移动机器人开始运动，首先，采用移动机器人的里程计运动模型预测移动机器人在预定时刻的姿态，继而生成直线特征的预测观测直线列表；接着，将直线特征的预测观测直线列表与实际观测直线列表进行匹配，匹配成功后，将实际观测到的直线特征根据预测的移动机器人坐标转化到世界坐标系，并求出对应的直线特征，继而依据得到的直线特征对小车进行定位。

具体地，建立好线段特征地图后，移动机器人开始运动，同样采用里程计运动模型X_t+1＝f(X_t，U_t)预测t+1时刻移动机器人的姿态X_t+1＝(x_t+1，y_t+1，θ_t+1)；然后，生成直线特征的预测观测直线列表

其中，ρ_pi表示第i条预测观测的直线特征到移动机器人坐标系原点的距离值；

表示移动机器人坐标系原点到第i条预测直线的垂线与移动机器人坐标系X轴正半轴的夹角。假设移动机器人实际观测到的直线特征为

其中，ρ_ri表示实际观测到的第i条直线特征到移动机器人坐标系原点的距离值；

表示移动机器人坐标系原点到第i条直线的垂线与移动机器人坐标系X轴正半轴的夹角。

在得到预测观测直线列表和实际观测直线列表后，对两个列表中的直线特征进行匹配。其中，如果满足:

则表明直线特征匹配成功；其中，ρ_i，

为预测观测的直线特征；ρ_j，

为实际观测的直线特征；ρ_th，

分别为直线特征距离和角度匹配阈值。

直线特征匹配成功后，将实际观测到的直线特征

根据预测的移动机器人坐标转化到世界坐标系为

其中转化公式可以描述为：

然后求出对应直线特征点为{(x_rw1，y_rw1)，…(x_rwi，y_rwi)}。

考虑到激光雷达的观测精度与距离有关，则匹配误差可以根据观测直线距离定义如下：

其中，x_wi,y_wi为与观测直线匹配的地图直线特征的特征点坐标。

由于里程计运动模型估计的移动机器人坐标X_t+1有一定误差，所以采用动态窗口法在里程计预测坐标X_t+1＝(x_t+1，y_t+1，θ_t+1)附近范围进行搜索，即在x_t+1∈(x_t+1-v_x*δt，x_t+1+v_x*δt),y_t+1∈(y_t+1-v_y*δt，y_t+1+v_y*δt)的范围内进行搜索。其中，v_x，v_y分别为移动机器人沿x轴方向速度和沿y方向速度；δt为每次计算的时间间隔。

假设需要达到的精度为c，则可以取搜索步长为2c。假设在坐标搜索范围内求得的所有匹配误差为{E_f1,E_f2,…,E_fn}，则真实移动机器人坐标为min{E_f1,E_f2,…,E_fn}对应的X_t+1，角度θ_t+1可直接根据公式θ_t+1=

计算得到。

在从登车桥进入车厢时，由于所述登车桥具有一定的斜度，可以采用预先安装于移动机器人上的陀螺仪来检测小车是否在登车桥上，如果俯仰角大于0，则移动机器人继续向前行走；否则，说明移动机器人已经进入车厢，此时停止移动机器人的运行。

本实施方式中，为了防止移动机器人在上登车桥过程中车体发生倾斜，采用一种基于激光雷达的中线检测方法来对移动机器人进行中线检测，具体为：在移动机器人登车桥的过程中对周围环境进行扫描，并采用上面所述方法提取登车桥环境中直线特征，然后根据提取到的直线特征的相互关系判别出属于车厢内壁的两条直线。具体为：即两条直线若满足条件：

则可根据这两条直线特征检测登车桥中线的位置以及移动机器人是否发生倾斜；L为车厢宽度；ρ₁、ρ₂为移动机器人距车厢两内壁直线距离；

为机器人与内壁两直线夹角。其中，移动机器人距登车桥中线的距离δd＝ρ₁-L/2；倾斜角度为

其中

从车厢出来经过登车桥到库前区时，有可能移动机器人位置X₁与上桥时的位置X₀发生了一定程度的偏移，导致反光板定位无法恢复。所以可采用窗口搜索法，在X₀附近1m范围内进行坐标搜索，x坐标和y坐标以及角度θ的步长可根据反光板匹配误差阈值来确定，直到与反光板地图中的反光板匹配成功则停止搜索，得到移动机器人此时的真实位置X₁，反光板定位功能恢复成功，则移动机器人可继续在反光板地图中行驶。

在所述系统中，反光板地图为静态地图，固定在库前区范围内；登车桥长度固定，同样可以建立在静态地图中，其位置可以根据移动机器人的里程计进行测算。如图1所示，在进出车厢的过程中，正在进行SLAM导航的区域采用实线表示，暂未进行导航的区域采用虚线表示，在地图中非常直观。

本发明提供的移动机器人自动建图与定位系统将反光板定位与SLAM定位结合起来，不仅提高了移动机器人的定位精度，而且解决了移动机器人进出货车车厢等动态环境取放货物的问题，提高了效率，节省了人力物力。

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

Claims (10)

1.一种移动机器人自动建图与定位系统，其特征在于：

所述系统包括移动机器人、库前区、货车车厢及连接所述库前区及所述货车车厢的登车桥，所述移动机器人在所述库前区、所述登车桥及所述货车车厢之间进行移动及取放货物；

所述移动机器人在所述库前区时，所述系统采用激光反光板定位方式对所述移动机器人进行定位；所述移动机器人在所述货车车厢时，所述系统采用基于直线特征的SLAM方式对所述移动机器人进行定位；所述移动机器人在上下登车桥时，所述系统采用中线检测方法对所述移动机器人进行位姿检测，以避免所述移动机器人在登车桥上因打滑而造成倾斜。

2.如权利要求1所述的移动机器人自动建图与定位系统，其特征在于：采用基于直线特征的SLAM方式对所述移动机器人进行定位时包括以下步骤：

(1)以移动机器人为原点在车厢内建立局部坐标系，通过激光雷达扫描车厢内壁轮廓以得到一系列激光扫描点；接着，采用迭代适应点算法将扫描点分割成不同区域点集，并采用最小二乘法将各点集中的点拟合成直线，进而建立车厢内环境的线段特征地图；

(2)根据所述线段特征地图中的直线特征，采用优化加权匹配误差的方式对所述移动机器人进行定位。

3.如权利要求2所述的移动机器人自动建图与定位系统，其特征在于：线段特征地图中的直线特征的表达式为：

式中，

表示第i条直线特征到线段特征地图原点的距离值；

表示线段特征地图原点到第i条直线的垂线与X轴正半轴的夹角。

4.如权利要求3所述的移动机器人自动建图与定位系统，其特征在于：步骤(2)中，首先，采用移动机器人的里程计运动模型预测移动机器人在预定时刻的姿态，继而生成直线特征的预测观测直线列表；接着，将直线特征的预测观测直线列表与实际观测直线列表进行匹配，匹配成功后，将实际观测到的直线特征根据预测的移动机器人坐标转化到世界坐标系，并求出对应的直线特征，继而依据得到的直线特征对移动机器人进行定位。

5.如权利要求4所述的移动机器人自动建图与定位系统，其特征在于：如果满足:

则表明直线特征匹配成功；其中，ρ_i，

为预测观测的直线特征；ρ_j，

为实际观测的直线特征；ρ_th，

分别为直线特征距离和角度匹配阈值。

6.如权利要求4所述的移动机器人自动建图与定位系统，其特征在于：采用动态窗口法在里程计预测坐标X_t+1＝(x_t+1，y_t+1，θ_t+1)附近范围进行搜索，即在x_t+1∈(x_t+1-v_x*δt，x_t+1+v_x*δt),y_t+1∈(y_t+1-v_y*δt，y_t+1+v_y*δt)的范围内进行搜索，以确定移动机器人的坐标。

7.如权利要求1所述的移动机器人自动建图与定位系统，其特征在于：采用激光反光板定位方式对所述移动机器人的定位包括以下步骤：

S1，在所述库前区范围内随机设置多个反光板，并将所述移动机器人在初始位置时，激光雷达扫描到的反光板添加到反光板地图中，之后启动所述移动机器人以进行反光板的标定；

S2，基于所述移动机器人的运动模型预测预定时刻时所述移动机器人扫描到的反光板列表，并将得到的预测反光板列表与实际扫描得到的反光板列表进行匹配，匹配成功后根据所述反光板地图中的反光板坐标计算所述移动机器人的位姿；

S3，根据所述移动机器人的坐标继续添加新的反光板到所述反光板地图中，直至所述移动机器人遍历所述库前区，由此完成所述反光板地图的构建；

S4，依据所述反光板地图对所述移动机器人进行定位。

8.如权利要求7所述的移动机器人自动建图与定位系统，其特征在于：在构建所述反光板地图时，只采用离所述移动机器人预定距离范围内的反光板；在利用所述反光板进行定位时，同样只用预定距离范围内的反光板进行定位计算。

9.如权利要求7所述的移动机器人自动建图与定位系统，其特征在于：初始标定反光板时，将所述移动机器人作为为反光板地图坐标系原点，且激光雷达在预定范围内能扫描到的反光板的数量至少为3个；根据反光板的反射强度和距离比值对每组反光板计算出来的移动机器人坐标进行加权平均处理。

10.如权利要求1所述的移动机器人自动建图与定位系统，其特征在于：采用中线检测方法对所述移动机器人进行位姿检测时，在移动机器人上下登车桥的过程中对周围环境进行扫描，并提取登车桥环境中直线特征；然后根据提取到的直线特征的相互关系判别出属于车厢内壁的两条直线，依据得到的两条直线特征检测登车桥中线的位置及移动机器人是否发生倾斜。

Images