CN110044358B - 基于现场场线特征的移动机器人定位方法 - Google Patents

Abstract

本发明提供了一种基于现场场线特征的移动机器人定位方法，其包括步骤S1‑S10。在本发明中，使用工作环境中的已有场线特征，保证了全局环境地图与真实工作环境的一致性；基于场线特征离散化成采样点的方式来进行匹配度的计算，有效地克服了可能存在的有效特征信息不足的问题；构建出的全局概率地图，方便后续机器人在定位过程中的定量计算，节约了计算时间；增量式边线提取算法受外界噪声干扰低、使用的参数少，减少了计算量；基于蒙特卡罗方法进行采样，使得滤波精度可以逼近最优估计，大大降低了计算复杂度；粒子滤波算法具有较强的建模能力，可以有效克服非线性情况下高斯分布的制约、能够适应现实复杂环境的要求，提高了机器人的自定位精度。

Description

基于现场场线特征的移动机器人定位方法

技术领域

本发明涉及移动机器人控制技术领域，尤其涉及一种基于现场场线特征的移动机器人定位方法。

背景技术

随着工业制造向自动化和智能化方向发展，工业环境下的自动导引车(Automatedguided vehicle，缩写为AGV)等移动式机器人应用越来越广泛。为了实现工厂环境下的自主巡航，AGV等移动机器人应具有精确的自定位能力、能够依据自身的位置和上层指令完成各项任务，因而自主定位是移动机器人的核心功能。

目前工厂环境下的移动机器人自定位具有多种方式：(1)通过室内GPS定位系统(Indoor Global Positioning System，缩写为iGPS)建立全厂范围内的位置测量场、并在移动机器人本体上安装测量点，从而实现3D空间内的精确定位，并且还可以在AGV活动范围内铺设磁条或者RFID标记装置获得自身位置(缺点是需要对环境提出特殊的要求并做出特殊的改造)；(2)通过移动机器人自身所带的传感器，通过感知环境局部信息来确定全局坐标系下的位姿状态，这些传感器包括里程计、惯性导航、视觉图像以及激光雷达等，其优势是不需要对工作环境做出较大的改造，具有显著的成本优势、对环境变化和任务变化的适应性更强，特别是视觉图像传感器能够获得大量的环境信息，成本低廉，因而成为移动机器人定位的重要方法，具有广阔的开发潜力，但是图像特征提取并实现精确定位仍没有完善的解决方案，特别是自然环境下和动态环境下的定位和建图仍旧处于研究的前沿。

现有基于计算机视觉的移动机器人定位方法，一般可分为基于标识以及基于自然特征的无标记方式。其中，后者以vSLAM技术为代表，能够解决无法预先获知环境3D模型的条件下的定位和地图建立。目前静态、刚体、光照变化不明显、无人为干扰的场景下的vSLAM技术已经趋于完善。但针对动态变化的工厂车间环境，则由于存在大量干扰而远未得到解决。前者则通过已知的环境3D模型或者人工布置标记的方法，利用已知的三个特征点位置求解P3P问题，或通过空间中的N个已知特征点位置，借助DLT算法或光束法平差优化算法(bundle adjustment，缩写为BA)实现2D-3D匹配而实现机器人的定位跟踪。然而这些方法需要大量的已知标记点，对于实际环境而言，设置这些标记点需要改变已有的环境，造成了时间和经济、人力成本上的浪费，对环境改造过大，失去了视觉系统用于定位的优势。

发明内容

鉴于背景技术中存在的问题，本发明的目的在于提供一种基于现场场线特征的移动机器人定位方法，其能有效利用工作环境中的已有场线特征构建出全局概率地图、并综合利用增量式边线提取算法、蒙特卡罗方法和粒子滤波算法，从而极大地提高了机器人的自定位精度、节省了计算时间和成本。

为了实现上述目的，本发明提供了一种基于现场场线特征的移动机器人定位方法，其包括步骤S1-S10。

S1，构建出移动机器人所需工作环境的全局环境地图，所述全局环境地图包含工作环境中的全部场线信息，其中移动机器人设置有用于采集工作环境中的场线信息的视觉系统，所述工作环境建立有全局坐标系O₁-X_wY_wZ_w。

S2，将所述全局环境地图中的场线定义为真实场线，并将所述全局环境地图分割成等大小的单元格、选取靠近真实场线最近的单元格顶点作为基准采样点，则所有的基准采样点构成第一采样集合A＝{P_si,i＝1,2,3…n}，分别获得基准采样点P_si的位置坐标(x_si,y_si)以及与基准采样点P_si对应的真实场线的倾斜角

其中i表示基准采样点的编号、n表示基准采样点的个数。

S3，预先假设所述视觉系统采集到的场线信息为虚拟场线，选取靠近虚拟场线最近的单元格顶点作为目标采样点，其中各目标采样点可以为任意的单元格顶点，则由所有的单元格顶点构成第二采样集合B＝{P_xj,j＝1,2,3,…m}，分别获得单元格顶点P_xj的位置坐标(x_sj,y_sj)，其中j表示单元格顶点的编号、m表示单元格顶点的个数。

S4，基于获得的基准采样点的位置信息以及单元格顶点的位置信息，分别计算出单元格顶点P_xj与基准采样点P_si之间的匹配度ω_ji，则单元格顶点P_xj被采样的概率

由此获得全局概率地图。

S5，在工作环境中，移动机器人利用视觉系统采集出工作环境的不同位置的原始照片，其中，移动机器人建立有机器人坐标系O₂-X_rY_rZ_r、视觉系统建立有相机坐标系O₃-X_cY_cZ_c、原始照片建立有图像坐标系O₄-XY。

S6，基于视觉系统采集到的原始照片，提取出实际场线信息，包括步骤：

S61，将原始照片转化为多值图像且所述多值图像建立有像素坐标系O₅-UV；

S62，利用增量式边线提取算法提取出多值图像中的实际场线信息；

S63，对得到的实际场线信息进行滤波处理；

S64，将提取出的实际场线信息转换到机器人坐标系下。

S7，采用蒙特卡罗方法在全局环境地图上选取一定数量的单元格顶点作为初始样本，并由初始样本中的所有单元格顶点构建粒子集合D₀＝{(x_w,y_w,α_w)_k ⁽⁰⁾,k＝1,2…f,f＜n}、一个粒子对应一个单元格顶点，则移动机器人位于粒子集合D₀中的每个粒子处的初始权重均为ω⁽⁰⁾＝1/f，其中x_w、y_w分别是粒子在全局环境地图的全局坐标系下的X轴坐标和Y轴坐标、α_w是移动机器人在对应粒子处相对于全局坐标系的X轴正方向的旋转角、k表示粒子集合D₀中的粒子编号、f表示粒子集合D₀中的粒子个数。

S8，基于粒子集合D₀，分别求出每个粒子对应的全局坐标系与机器人坐标系变换之间的旋转矩阵和平移矩阵，并将步骤S6中得到的机器人坐标系下的实际场线信息转换到全局坐标系下，即：

由此获得每个粒子对应的全局实测地图，其中[x_r,y_r,z_r]^T是机器人坐标系下的坐标、[x_w,y_w,z_w]^T是全局坐标系下的坐标、R_wr ^k表示第k个粒子对应的全局坐标系与机器人坐标系变换之间的旋转矩阵、T_wr ^k表示第k个粒子对应的全局坐标系与机器人坐标系变换之间的平移矩阵。

S9，在每个全局实测地图中，选取靠近实际场线最近的单元格顶点作为实际采样点并构成第三采样集合C＝{P_j',j'＝1,2,3,…e}，且在步骤S4中获得的全局概率地图中找出实际采样点P_j'对应的被采样概率ω_j'、j'为每个全局实测地图中实际采样点的编号，则第k个粒子的计算权重

并将粒子集合D₀中的所有粒子进行归一化处理、且归一化处理后第k个粒子的计算权重

由此获得粒子集合D₀的粒子概率分布结果

S10，根据获得的粒子集合D₀的粒子概率分布结果

并基于粒子滤波算法重新调整粒子分布，由此获得的新的粒子集合D₁以及粒子集合D₁的粒子概率分布结果

以此类推不断重新调整粒子分布并获得调整后的粒子集合对应的粒子概率分布结果，直至粒子概率分布结果收敛，则粒子概率分布结果收敛时的极值为最终的移动机器人定位结果。

在步骤S2中，所述真实场线包括直线和曲线，则直线上的基准采样点的倾斜角通过求直线的斜率的反正切值得到、曲线上的基准采样点的倾斜角通过求曲线在该基准采样点处的切线斜率的反正切值得到。

在步骤S4中，包括步骤：

S41，计算出单元格顶点P_xj与基准采样点P_si之间的欧式距离L_ji；

S42，单元格顶点P_xj对应的虚拟场线与基准采样点P_si对应的真实场线之间的倾斜角之差为θ_ji，其中θ_ji∈[-π,π]且每间隔Δθ＝π/18进行离散取值，此时单元格顶点P_xj对应的虚拟场线的倾斜角

S43，计算出单元格顶点P_xj与基准采样点P_si之间的匹配度ω_ji，则计算公式为：

其中，ω₀为P_xj和P_si完全重合时的匹配度且为常值，L_o为设定的阈值且L_ji≥L₀时ω_ji＝0；

S44，计算出单元格顶点P_xj被采样的概率

由此获得全局概率地图。

在步骤S62中，所述多值图像至少包括第一种颜色、第二种颜色和第三种颜色、且第二种颜色表示原始照片中的实际场线，且提取过程包括步骤：

S621，由所述多值图像的所有像素点构成矩阵M_c×d，其中c表示矩阵的行数、b表示矩阵的列数；

S622，沿水平方向开始逐像素点扫描，当在第a(a＝1,2,3…c)行检测到多值图像出现第一种颜色-第二种颜色-第一种颜色的颜色跳变时，将每一次跳变中的第二种颜色对应的两个端点作为两个初始点；

S623，由第a行中每一次跳变中的第二种颜色对应的两个初始点构造出一条场线模型，计算出每条场线模型的斜率grad、斜率倒数invgrad以及中点坐标(xMean,yMean)；

S624，继续沿水平方向扫描第a+1行，当检测到第一种颜色-第二种颜色-第一种颜色的颜色跳变时，选取每一次跳变中的第二种颜色部分的中点作为代表点、并计算出代表点的坐标(xMid,yMid)；

S625，校准第a+1行中选取的每个代表点与步骤S623中得到的所有场线模型之间的匹配关系，包括步骤：

S6251，计算出代表点与步骤S623中得到的所有场线模型的匹配程度，且每个代表点与任意的场线模型的匹配程度xErr的计算公式为：

xProj＝xMean+invgrad×(yMid-yMean)

xErr＝|xMid-xProj|

S6252，设定阈值N，当xErr≤N时，将该代表点加入对应的场线模型中、更新该场线模型的中点坐标；当xErr＞N时，以该代表点构建新的场线模型；

S626，按照步骤S624-S625的方式继续扫描、计算和判断，直至完成对水平方向上所有行的扫描，最终获得处于水平方向上的所有场线模型；

S627，沿竖直方向开始逐像素点扫描，当在第b(b＝1,2,3…d)列检测到多值图像出现第一种颜色-第二种颜色-第一种颜色的颜色跳变时，将每一次跳变中的第二种颜色对应的两个端点作为两个初始点；

S628，由第b列中每一次跳变中的第二种颜色对应的两个初始点构造出一条场线模型，计算出每条场线模型的斜率grad'、斜率倒数invgrad'以及中点坐标(x'Mean,y'Mean)；

S629，继续沿竖直方向扫描第b+1列，当检测到第一种颜色-第二种颜色-第一种颜色的颜色跳变时，选取每一次跳变中的第二种颜色部分的中点作为代表点、并计算出代表点的坐标(x'Mid,y'Mid)；

S630，校准第b+1列中选取的每个代表点与步骤S628中得到的所有场线模型之间的匹配关系，包括步骤：

S6301，计算出代表点与步骤S628中得到的所有场线模型的匹配程度，且每个代表点与任意的场线模型的匹配程度x'Err的计算公式为：

x'Proj＝y'Mean+grad'×(x'Mid-x'Mean)

x'Err＝|j-yProj|

S6302，设定阈值N'，当x'Err≤N'时，将该代表点加入对应的场线模型中、更新该场线模型的中点坐标；当x'Err＞N'时，以该代表点构建新的场线模型；

S631，按照步骤S629-S630的方式继续扫描、计算和判断，直至完成对竖直方向上所有列的扫描，最终获得处于竖直方向上的所有场线模型。

在步骤S63中，将步骤S62中得到的处于水平方向上的所有场线模型以及处于竖直方向上的所有场线模型进行滤波处理。

在步骤S64中，将从多值图像中提取出的实际场线信息转换到机器人坐标系下的转换公式为：

其中，R_rc、T_rc分别是机器人坐标系与相机坐标系变换的旋转矩阵和平移矩阵，f是相机中的传感器焦距，dx、dy是像素点对应的真实物理尺寸，[x_c,y_c,z_c]^T是相机坐标系下任意点的坐标、[x_r,y_r,z_r]^T是机器人坐标系下任意点的坐标、[u,v]是像素坐标系下任意点的坐标，[u₀,v₀]是像素坐标系的原点在图像坐标系中的坐标。

本发明的有益效果如下：

本发明尽可能使用工作环境中的已有场线特征，其与传统的设置信标方式相比，减少了张贴标识的工作量，保证了全局环境地图与真实工作环境的一致性，有助于提高移动机器人的自定位精度、节省了成本。并且，基于场线特征离散化成采样点的方式来进行匹配度的计算，有效地克服了可能存在的有效特征信息不足的问题；构建出的全局概率地图，方便后续移动机器人在具体的定位过程中的定量计算，节约了计算时间。由于增量式边线提取算法受外界噪声干扰低、使用的参数少，从而减少了计算量。基于蒙特卡罗方法进行采样，使得滤波精度可以逼近最优估计，且大大降低了计算复杂度。此外，粒子滤波算法具有较强的建模能力，可以有效克服非线性情况下高斯分布的制约，且能够更有效地适应现实复杂环境的要求，提高了移动机器人的自定位精度。

附图说明

图1是本发明的基于现场场线特征的移动机器人定位方法中的真实场线和虚拟场线之间的位置关系示意图。

图2是图1中的真实场线和虚拟场线离散后的采样点分布示意图，其中仅示出一条真实场线。

图3是所有的真实场线离散后的基准采样点分布示意图。

图4是本发明的基于现场场线特征的移动机器人定位方法中的不同坐标系之间的位置关系示意图。

图5是

时的全局概率地图。

图6是

时的全局概率地图。

图7是

时的全局概率地图。

具体实施方式

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

参照图1至图7，根据本发明的基于现场场线特征的移动机器人定位方法包括步骤S1-S10。

步骤S1-S4是在移动机器人的视觉系统采集工作环境中的场线信息之前做出的准备工作，详细说明如下。

S1，构建出移动机器人所需工作环境的全局环境地图，所述全局环境地图包含工作环境中的全部场线信息。具体地，所述全部场线可包括直线以及任意的自由曲线(包括圆)。移动机器人设置有用于采集工作环境中的场线信息的视觉系统(包括相机)。其中，所述工作环境建立有全局坐标系O₁-X_wY_wZ_w，所述全局坐标系用于表征工作环境中的场线在实际空间中的绝对位置、且可以其为基准来描述移动机器人和工作环境中的场线在实际空间中的相对位置。

在步骤S1中，本发明尽可能使用工作环境中的已有场线特征，其与传统的设置信标方式相比，减少了张贴标识的工作量，且保证了全局环境地图与真实工作环境的一致性，有助于提高移动机器人的自定位精度、节省了成本。

S2，将所述全局环境地图中的场线定义为真实场线(如图1和图2中的实线L_s)，并将所述真实场线离散成点的集合。具体地，将所述全局环境地图分割成等大小的单元格、选取靠近真实场线最近的单元格顶点作为基准采样点(如图2所示)，则所有的基准采样点构成第一采样集合A＝{P_si,i＝1,2,3…n}，此时第一采样集合A即表示所述真实场线离散后的点的集合。并且，基于全局坐标系O₁-X_wY_wZ_w，分别获得基准采样点P_si的位置坐标(x_si,y_si)以及与基准采样点P_si对应的真实场线的倾斜角

其中i表示基准采样点的编号、n表示基准采样点的个数。

在步骤S2中，当所述真实场线为直线时，直线上的基准采样点的倾斜角通过求直线的斜率的反正切值得到；当真实场线为曲线时，曲线上的基准采样点的倾斜角通过求曲线在该基准采样点处的切线斜率的反正切值得到，即切线斜率为曲线方程在该点的一阶导数。

S3，在移动机器人的视觉系统采集工作环境中的场线信息之前，预先假设所述视觉系统采集到的场线信息为虚拟场线(如图1和图2中的虚线L_x)，并将所述虚拟场线离散成点的集合。具体地，选取靠近虚拟场线最近的单元格顶点作为目标采样点(如图2所示)，其中各目标采样点可以为任意的单元格顶点(换句话说，每一个单元格顶点都有成为目标采样点的可能)，则由所有的单元格顶点构成第二采样集合B＝{P_xj,j＝1,2,3,…m}，此时第二采样集合B即表示所述虚拟场线离散后的点的集合。并且，基于全局坐标系O₁-X_wY_wZ_w，分别获得单元格顶点P_xj的位置坐标(x_sj,y_sj)，其中j表示单元格顶点的编号、m表示单元格顶点的个数。

S4，基于获得的所有基准采样点的位置信息以及所有单元格顶点的位置信息，分别计算出单元格顶点P_xj与基准采样点P_si之间的匹配度ω_ji，则单元格顶点P_xj被采样的概率

由此获得全局概率地图。

参照图3，由于被采集到的虚拟场线可能出现在工作环境的任意位置，则单元格顶点P_xj存在与第一采样集合A＝{P_si,i＝1,2,3…n}中任何一点对应的可能，因此需要计算出单元格顶点P_xj与{P_s1,P_s2,P_s3…P_sn}中每一点之间的匹配度。同时，由于全局环境地图中的每一个单元格顶点都有成为目标采样点的可能，因此全局概率地图是对所有单元格顶点被采样概率的描述。

在步骤S4中，具体包括步骤S41-S44。

S41，计算出单元格顶点P_xj与基准采样点P_si之间的欧式距离L_ji。其中，若欧式距离L_ji越小，则可以认为单元格顶点P_xj与基准采样点P_si重合的可能性越高，二者之间的匹配度越高；若欧式距离L_ji足够大，意味着单元格顶点P_xj与基准采样点P_si之间的匹配度为0。

S42，单元格顶点P_xj对应的虚拟场线与基准采样点P_si对应的真实场线之间的倾斜角之差为θ_ji，其中θ_ji∈[-π_,π]且每间隔Δθ＝π/18进行离散取值，此时单元格顶点P_xj对应的虚拟场线的倾斜角

其中，若θ_ji越小，则可认为单元格顶点P_xj对应的虚拟场线与基准采样点P_si对应的真实场线的斜率相近，二者接近于重合或平行，此时表明单元格顶点P_xj与基准采样点P_si之间的匹配度越高；若θ_ji足够大，则意味着单元格顶点P_xj与基准采样点P_si之间的匹配度为0。

S43，计算出单元格顶点P_xj与基准采样点P_si之间的匹配度ω_ji，则计算公式为：

其中，ω₀为P_xj和P_si完全重合时的匹配度且为常值，L_o为设定的阈值且L_ji≥L₀时ω_ji＝0。

S44，计算出单元格顶点P_xj被采样的概率

由此获得全局概率地图。由于第一采样集合A由精确位置的基准采样点组成，因而与基准采样点P_si匹配度高的单元格顶点P_xj，其在一般情况下被采样的概率高；与基准采样点P_si匹配度低的单元格顶点P_xj，其在一般情况下被采样的概率低，因此全局概率地图代表着全局环境地图中的任意位置出现目标采样点的概率。

为了便于理解，将步骤S4中的全局概率地图可视化，如图5至图7所示，其中图5至图7分别展示了当

时全局环境地图中的任意位置(即对应某个单元格顶点)被采样的概率分布情况，且图中白色光亮的地方表明该处出现目标采样点的概率高，而黑色的地方表明该出现目标采样点的概率低。

具体地，参照图5，在

的情况下，与基准采样点P_si匹配度较高的单元格顶点P_xj的倾斜角也应当是在π/2左右。且从图5中可以看出，只有位于竖直场线周围以及切线方向为π/2的曲线场线周围的单元格顶点才具有较高的被采样概率，且这一概率随着距离这些场线的距离增加而减小，在距离这些场线较远的区域颜色很深(呈现黑色)，说明这些地方出现目标采样点的概率很低。同理，从图6中可以看出，只有位于水平场线周围以及切线方向为水平的曲线场线周围的单元格顶点才具有较高的被采样概率。同理，从图7中可以看出，只有位于切线方向为π/4的曲线场线周围的单元格顶点才具有较高的被采样概率。

在步骤S2-S4中，基于场线特征(虚拟场线和真实场线)离散化成采样点的方式来进行匹配度的计算，有效地克服了可能存在的有效特征信息不足的问题。并且，构建出的全局概率地图，能够有效地衡量单元格顶点P_xj与基准采样点P_si之间的匹配度ω_ji与单元格顶点P_xj被采样的概率之间的关系，从而方便后续移动机器人在具体的定位过程中的定量计算，由此节约了计算时间。此外，本发明还将所得的全局概率地图以多维数组的形式(如可视化的图片)保存起来，从而方便在定位过程中调用，由此进一步提高了计算效率和响应时间。

步骤S5-S10是移动机器人的具体定位过程，详细说明如下。

S5，在工作环境中，移动机器人利用视觉系统采集出工作环境的不同位置的原始照片，参照图4，移动机器人建立有机器人坐标系O₂-X_rY_rZ_r(以移动机器人所处位置为原点，由全局坐标系O₁-X_wY_wZ_w绕Z_w轴旋转一定角度、并在X_w-Y_w平面内平移得到)、视觉系统建立有相机坐标系O₃-X_cY_cZ_c(以相机的光心为原点，Z_c轴与光轴重合且垂直于像平面，X_c轴、Y_c轴分别与图像坐标系的X轴、Y轴平行)、原始照片建立有图像坐标系O₄-XY(以光轴和成像平面的交点为原点，X轴、Y轴分别与像素坐标系的u轴、v轴相互平行，其坐标平面位于摄像头成像平面上)。

S6，基于视觉系统采集到的原始照片，提取出实际场线信息。具体包括步骤：S61，将原始照片转化为多值图像且多值图像建立有像素坐标系O₅-UV(原点位于左上角，u轴、v轴分别与图像的两条邻边平行、用于表征像素的列数和行数，像素坐标系反映了视觉系统获得的原始照片中像素的排列情况)；S62，利用增量式边线提取算法提取出多值图像中的实际场线信息；S63，对得到的实际场线信息进行滤波处理；S64，将提取出的实际场线信息转换到机器人坐标系下。

在步骤S61中，根据不同颜色的对应关系，将原始照片转化为多值图像，则得到的多值图像至少包括第一种颜色、第二种颜色和第三种颜色、且用第二种颜色表示原始照片中的实际场线。具体地，如移动机器人所需工作环境为一球场，则在多值图像中将球场的地面统一标定为绿色色块、实际场线统一标定为白色、其他障碍物统一标定为黑色等。

在步骤S62中，所述增量式边线提取算法的提取过程包括步骤S621-S629。

S621，由所述多值图像的所有像素点构成矩阵M_c×d，其中c表示矩阵的行数、d表示矩阵的列数。

S622，沿水平方向开始逐像素点扫描，当在第a(a＝1,2,3…c)行检测到多值图像出现第一种颜色-第二种颜色-第一种颜色(如绿色-白色-绿色)的颜色跳变时，将每一次跳变中的第二种颜色对应的两个端点作为两个初始点。

S623，由第a行中每一次跳变中的第二种颜色对应的两个初始点构造出一条场线模型，计算出每条场线模型的斜率grad、斜率倒数invgrad以及中点坐标(xMean,yMean)。

S624，继续沿水平方向扫描第a+1行，当检测到第一种颜色-第二种颜色-第一种颜色的颜色跳变时，选取每一次跳变中的第二种颜色部分的中点作为代表点、并计算出代表点的坐标(xMid,yMid)。

S625，校准第a+1行中选取的每个代表点与步骤S623中得到的所有场线模型之间的匹配关系，且包括步骤：

S6251，计算出代表点与步骤S623中得到的所有场线模型的匹配程度，且每个代表点与任意的场线模型的匹配程度xErr的计算公式为：

xProj＝xMean+invgrad×(yMid-yMean)

xErr＝|xMid-xProj|；

S6252，设定阈值N，当xErr≤N时，将该代表点加入对应的场线模型中、更新该场线模型的中点坐标；当xErr＞N时，以该代表点构建新的场线模型；

S626，按照步骤S624-S625的方式继续扫描、计算和判断，直至完成对水平方向上所有行的扫描，最终获得处于水平方向上的所有场线模型(即从多值图像中提取出的实际场线信息)。

S627，沿竖直方向开始逐像素点扫描，当在第b(b＝1,2,3…d)列检测到多值图像出现第一种颜色-第二种颜色-第一种颜色(如绿色-白色-绿色)的颜色跳变时，将每一次跳变中的第二种颜色对应的两个端点作为两个初始点。

S628，由第b列中每一次跳变中的第二种颜色对应的两个初始点构造出一条场线模型，计算出每条场线模型的斜率grad'、斜率倒数invgrad'以及中点坐标(x'Mean,y'Mean)。

S629，继续沿竖直方向扫描第b+1列，当检测到第一种颜色-第二种颜色-第一种颜色的颜色跳变时，选取每一次跳变中的第二种颜色部分的中点作为代表点、并计算出代表点的坐标(x'Mid,y'Mid)。

S630，校准第b+1列中选取的每个代表点与步骤S628中得到的所有场线模型之间的匹配关系，且包括步骤：

S6301，计算出代表点与步骤S628中得到的所有场线模型的匹配程度，且每个代表点与任意的场线模型的匹配程度x'Err的计算公式为：

x'Proj＝y'Mean+grad'×(x'Mid-x'Mean)

x'Err＝|j-yProj|；

S6302，设定阈值N'，当x'Err≤N'时，将该代表点加入对应的场线模型中、更新该场线模型的中点坐标；当x'Err≤N'时，以该代表点构建新的场线模型。

S631，按照步骤S629-S630的方式继续扫描、计算和判断，直至完成对竖直方向上所有列的扫描，最终获得处于竖直方向上的所有场线模型(即从多值图像中提取出的实际场线信息)。

在步骤S63中，将步骤S62中得到的处于水平方向上的所有场线模型以及处于竖直方向上的所有场线模型进行滤波处理，最终得到符合长度要求的实际场线信息(包含始末坐标)。

在步骤S64中，将从多值图像中提取出的实际场线信息转换到机器人坐标系下，且具体的转换公式为：

其中，R_rc、T_rc分别是机器人坐标系与相机坐标系变换的旋转矩阵和平移矩阵，f是相机中的传感器焦距，dx、dy是像素点对应的真实物理尺寸，[x_c,y_c,z_c]^T是相机坐标系下任意点的坐标、[x_r,y_r,z_r]^T是机器人坐标系下任意点的坐标、[u,v]是像素坐标系下任意点的坐标，[u₀,v₀]是像素坐标系的原点在图像坐标系中的坐标。

在步骤S6中，采用增量式边线提取算法的优势在于：1)使用的参数少，计算量小，易于实现；2)空间复杂度低，具有较好的鲁棒性；3)能够合理的判断出场线的完整性，可以检测出局部被遮挡的场线，增加特征信息的完整性；4)受外界噪声干扰低，且与Hough变换相比，有效的避免了Hough变换中可能存在的“漏检”和“虚峰”问题。

S7，采用蒙特卡罗方法在全局环境地图上选取一定数量的单元格顶点作为初始样本，并由初始样本中的所有单元格顶点构建粒子集合D₀＝{(x_w,y_w,α_w)_k ⁽⁰⁾,k＝1,2…f,f＜n}、一个粒子对应一个单元格顶点，则移动机器人位于粒子集合D₀中的每个粒子处的初始权重(即初始概率大小)均为ω⁽⁰⁾＝1/f(即在初始时刻，认为每个粒子的初始权重都是相等的)，其中x_w、y_w分别是粒子在全局环境地图的全局坐标系下的X轴坐标和Y轴坐标、α_w是移动机器人在对应粒子处相对于全局坐标系的X轴正方向的旋转角、k表示粒子集合D₀中的粒子编号、f表示粒子集合D₀中的粒子个数。

在步骤S7中，基于蒙特卡罗方法进行采样，使得滤波精度可以逼近最优估计；并通过选取的随机样本来近似的表示概率密度函数、用样本均值代替积分运算，大大降低了计算复杂度。

S8，基于粒子集合D₀，分别求出每个粒子对应的全局坐标系与机器人坐标系变换之间的旋转矩阵和平移矩阵，并将步骤S6中得到的机器人坐标系下的实际场线信息转换到全局坐标系下，由此获得每个粒子对应的全局实测地图，且机器人坐标系与全局坐标系之间的转换关系如下：

其中，[x_r,y_r,z_r]^T是机器人坐标系下的坐标、[x_w,y_w,z_w]^T是全局坐标系下的坐标、R_wr ^k表示第k个粒子对应的全局坐标系与机器人坐标系变换之间的旋转矩阵、T_wr ^k表示第k个粒子对应的全局坐标系与机器人坐标系变换之间的平移矩阵。

S9，在每个全局实测地图中，选取靠近实际场线(即步骤S6中获得的)最近的单元格顶点作为实际采样点并构成第三采样集合C＝{P_j',j'＝1,2,3,…e}，且在步骤S4中获得的全局概率地图中找出实际采样点P_j'对应的被采样概率ω_j'、j'为每个全局实测地图中实际采样点的编号，则第k个粒子的计算权重

并将粒子集合D₀中的所有粒子进行归一化处理、且归一化处理后第k个粒子的计算权重

由此获得粒子集合D₀的粒子概率分布结果

S10，根据获得的粒子集合D₀的粒子概率分布结果

并基于粒子滤波算法重新调整粒子分布，由此获得的新的粒子集合D₁以及粒子集合D₁的粒子概率分布结果

以此类推不断重新调整粒子分布并获得调整后的粒子集合对应的粒子概率分布结果，直至粒子概率分布结果收敛，则粒子概率分布结果收敛时的极值为最终的移动机器人定位结果(即移动机器人的当前位姿)。

具体地，由粒子集合D₀、并通过粒子滤波算法获得新的粒子集合D₁的过程为：淘汰粒子集合D₀中的权重较小的粒子、选取粒子集合D₀中的权重较大的粒子以及全局环境地图上的靠近所述权重较大的粒子的单元格顶点作为新的粒子集合D₁中的粒子。因此，对于后续不断更新的粒子集合，权重较大的地方、粒子分布较以往密集，权重较小的地方、粒子分布较以往稀疏，最终使得粒子概率分布结果朝着权重大的地方收敛。

此外，新的粒子集合D₁以及不断更新的粒子集合，其概率分布结果的计算过程均与粒子集合D₀的粒子概率分布结果

的计算过程一致。

在步骤S9-S10中，基于粒子滤波算法来确定粒子概率分布结果的有益之处在于：1)具有较强的建模能力，可以有效克服非线性情况下高斯分布的制约，在非线性、非高斯系统下也能获得很好的计算结果；2)相较于传统的Kalman滤波，粒子滤波算法能够更有效地适应现实复杂环境的要求，提高了移动机器人的自定位精度；3)粒子数目可以根据现有计算资源进行调节，进而能够实现精度和运算资源的动态平衡。

Claims (6)

1.一种基于现场场线特征的移动机器人定位方法，其特征在于，包括步骤：

S1，构建出移动机器人所需工作环境的全局环境地图，所述全局环境地图包含工作环境中的全部场线信息，其中移动机器人设置有用于采集工作环境中的场线信息的视觉系统，所述工作环境建立有全局坐标系O₁-X_wY_wZ_w；

S2，将所述全局环境地图中的场线定义为真实场线，并将所述全局环境地图分割成等大小的单元格、选取靠近真实场线最近的单元格顶点作为基准采样点，则所有的基准采样点构成第一采样集合A＝{P_si,i＝1,2,3…n}，分别获得基准采样点P_si的位置坐标(x_si,y_si)以及与基准采样点P_si对应的真实场线的倾斜角

其中i表示基准采样点的编号、n表示基准采样点的个数；

S3，预先假设所述视觉系统采集到的场线信息为虚拟场线，选取靠近虚拟场线最近的单元格顶点作为目标采样点，其中各目标采样点可以为任意的单元格顶点，则由所有的单元格顶点构成第二采样集合B＝{P_xj,j＝1,2,3,…m}，分别获得单元格顶点P_xj的位置坐标(x_sj,y_sj)，其中j表示单元格顶点的编号、m表示单元格顶点的个数；

S4，基于获得的基准采样点的位置信息以及单元格顶点的位置信息，分别计算出单元格顶点P_xj与基准采样点P_si之间的匹配度ω_ji，则单元格顶点P_xj被采样的概率

由此获得全局概率地图；

S5，在工作环境中，移动机器人利用视觉系统采集出工作环境的不同位置的原始照片，其中，移动机器人建立有机器人坐标系O₂-X_rY_rZ_r、视觉系统建立有相机坐标系O₃-X_cY_cZ_c、原始照片建立有图像坐标系O₄-XY；

S6，基于视觉系统采集到的原始照片，提取出实际场线信息，包括步骤：

S61，将原始照片转化为多值图像且所述多值图像建立有像素坐标系O₅-UV；

S62，利用增量式边线提取算法提取出多值图像中的实际场线信息；

S63，对得到的实际场线信息进行滤波处理；

S64，将提取出的实际场线信息转换到机器人坐标系下；

S7，采用蒙特卡罗方法在全局环境地图上选取一定数量的单元格顶点作为初始样本，并由初始样本中的所有单元格顶点构建粒子集合D₀＝{(x_w,y_w,α_w)_k ⁽⁰⁾,k＝1,2…f,f＜n}、一个粒子对应一个单元格顶点，则移动机器人位于粒子集合D₀中的每个粒子处的初始权重均为ω⁽⁰⁾＝1/f，其中x_w、y_w分别是粒子在全局环境地图的全局坐标系下的X轴坐标和Y轴坐标、α_w是移动机器人在对应粒子处相对于全局坐标系的X轴正方向的旋转角、k表示粒子集合D₀中的粒子编号、f表示粒子集合D₀中的粒子个数；

S8，基于粒子集合D₀，分别求出每个粒子对应的全局坐标系与机器人坐标系变换之间的旋转矩阵和平移矩阵，并将步骤S6中得到的机器人坐标系下的实际场线信息转换到全局坐标系下，即：

由此获得每个粒子对应的全局实测地图，其中[x_r,y_r,z_r]^T是机器人坐标系下的坐标、[x_w,y_w,z_w]^T是全局坐标系下的坐标、R_wr ^k表示第k个粒子对应的全局坐标系与机器人坐标系变换之间的旋转矩阵、T_wr ^k表示第k个粒子对应的全局坐标系与机器人坐标系变换之间的平移矩阵；

S9，在每个全局实测地图中，选取靠近实际场线最近的单元格顶点作为实际采样点并构成第三采样集合C＝{P_j',j'＝1,2,3,…e}，且在步骤S4中获得的全局概率地图中找出实际采样点P_j'对应的被采样概率ω_j'、j'为每个全局实测地图中实际采样点的编号，则第k个粒子的计算权重

并将粒子集合D₀中的所有粒子进行归一化处理、且归一化处理后第k个粒子的计算权重

由此获得粒子集合D₀的粒子概率分布结果

S10，根据获得的粒子集合D₀的粒子概率分布结果

并基于粒子滤波算法重新调整粒子分布，由此获得的新的粒子集合D₁以及粒子集合D₁的粒子概率分布结果

以此类推不断重新调整粒子分布并获得调整后的粒子集合对应的粒子概率分布结果，直至粒子概率分布结果收敛，则粒子概率分布结果收敛时的极值为最终的移动机器人定位结果。

2.根据权利要求1所述的基于现场场线特征的移动机器人定位方法，其特征在于，在步骤S2中，所述真实场线包括直线和曲线，则直线上的基准采样点的倾斜角通过求直线的斜率的反正切值得到、曲线上的基准采样点的倾斜角通过求曲线在该基准采样点处的切线斜率的反正切值得到。

3.根据权利要求1所述的基于现场场线特征的移动机器人定位方法，其特征在于，在步骤S4中，包括步骤：

S41，计算出单元格顶点P_xj与基准采样点P_si之间的欧式距离L_ji；

S42，单元格顶点P_xj对应的虚拟场线与基准采样点P_si对应的真实场线之间的倾斜角之差为θ_ji，其中θ_ji∈[-π,π]且每间隔Δθ＝π/18进行离散取值，此时单元格顶点P_xj对应的虚拟场线的倾斜角

S43，计算出单元格顶点P_xj与基准采样点P_si之间的匹配度ω_ji，则计算公式为：

其中，ω₀为P_xj和P_si完全重合时的匹配度且为常值，L_o为设定的阈值且L_ji≥L₀时ω_ji＝0；

S44，计算出单元格顶点P_xj被采样的概率

由此获得全局概率地图。

4.根据权利要求1所述的基于现场场线特征的移动机器人定位方法，其特征在于，在步骤S62中，所述多值图像至少包括第一种颜色、第二种颜色和第三种颜色、且第二种颜色表示原始照片中的实际场线，且提取过程包括步骤：

S621，由所述多值图像的所有像素点构成矩阵M_c×d，其中c表示矩阵的行数、d表示矩阵的列数；

S622，沿水平方向开始逐像素点扫描，当在第a(a＝1,2,3…c)行检测到多值图像出现第一种颜色-第二种颜色-第一种颜色的颜色跳变时，将每一次跳变中的第二种颜色对应的两个端点作为两个初始点；

S623，由第a行中每一次跳变中的第二种颜色对应的两个初始点构造出一条场线模型，计算出每条场线模型的斜率grad、斜率倒数invgrad以及中点坐标(xMean,yMean)；

S624，继续沿水平方向扫描第a+1行，当检测到第一种颜色-第二种颜色-第一种颜色的颜色跳变时，选取每一次跳变中的第二种颜色部分的中点作为代表点、并计算出代表点的坐标(xMid,yMid)；

S625，校准第a+1行中选取的每个代表点与步骤S623中得到的所有场线模型之间的匹配关系，包括步骤：

S6251，计算出代表点与步骤S623中得到的所有场线模型的匹配程度，且每个代表点与任意的场线模型的匹配程度xErr的计算公式为：

xProj＝xMean+invgrad×(yMid-yMean)

xErr＝|xMid-xProj|

S6252，设定阈值N，当xErr≤N时，将该代表点加入对应的场线模型中、更新该场线模型的中点坐标；当xErr＞N时，以该代表点构建新的场线模型；

S626，按照步骤S624-S625的方式继续扫描、计算和判断，直至完成对水平方向上所有行的扫描，最终获得处于水平方向上的所有场线模型；

S627，沿竖直方向开始逐像素点扫描，当在第b(b＝1,2,3…d)列检测到多值图像出现第一种颜色-第二种颜色-第一种颜色的颜色跳变时，将每一次跳变中的第二种颜色对应的两个端点作为两个初始点；

S628，由第b列中每一次跳变中的第二种颜色对应的两个初始点构造出一条场线模型，计算出每条场线模型的斜率grad'、斜率倒数invgrad'以及中点坐标(x'Mean,y'Mean)；

S629，继续沿竖直方向扫描第b+1列，当检测到第一种颜色-第二种颜色-第一种颜色的颜色跳变时，选取每一次跳变中的第二种颜色部分的中点作为代表点、并计算出代表点的坐标(x'Mid,y'Mid)；

S630，校准第b+1列中选取的每个代表点与步骤S628中得到的所有场线模型之间的匹配关系，包括步骤：

S6301，计算出代表点与步骤S628中得到的所有场线模型的匹配程度，且每个代表点与任意的场线模型的匹配程度x'Err的计算公式为：

x'Proj＝y'Mean+grad'×(x'Mid-x'Mean)

x'Err＝|y'Mid-x'Proj|

S6302，设定阈值N'，当x'Err≤N'时，将该代表点加入对应的场线模型中、更新该场线模型的中点坐标；当x'Err＞N'时，以该代表点构建新的场线模型；

S631，按照步骤S629-S630的方式继续扫描、计算和判断，直至完成对竖直方向上所有列的扫描，最终获得处于竖直方向上的所有场线模型。

5.根据权利要求4所述的基于现场场线特征的移动机器人定位方法，其特征在于，在步骤S63中，将步骤S62中得到的处于水平方向上的所有场线模型以及处于竖直方向上的所有场线模型进行滤波处理。

6.根据权利要求1所述的基于现场场线特征的移动机器人定位方法，其特征在于，在步骤S64中，将从多值图像中提取出的实际场线信息转换到机器人坐标系下的转换公式为：

其中，R_rc、T_rc分别是机器人坐标系与相机坐标系变换的旋转矩阵和平移矩阵，f是相机中的传感器焦距，dx、dy是像素点对应的真实物理尺寸，[x_c,y_c,z_c]^T是相机坐标系下任意点的坐标、[x_r,y_r,z_r]^T是机器人坐标系下任意点的坐标、[u,v]是像素坐标系下任意点的坐标，[u₀,v₀]是像素坐标系的原点在图像坐标系中的坐标。

Images