用于大尺度多站点场景的移动机器人位姿测量方法及系统与流程

本发明属于移动机器人位姿测量领域，更具体地，涉及一种用于大尺度多站点场景的移动机器人位姿测量方法及系统。

背景技术：

移动机器人因其适用性与移动性被广泛运用于大尺度多站点的复杂场景中，如电子电器制造车间、汽车零部件制造车间等，精准的位姿测量是移动机器人在该场景中实现自主移动的核心技术。通常情况下，移动机器人需要频繁地在场景内执行站点停靠任务并在站点完成精准的拾取和放置物料等操纵任务，因此为了精准、可靠地实现以上任务，移动机器人在站点附近的位姿测量精度要求相比其他工作区域要求更高。

在相对固定的空间中，利用少量的激光跟踪器、相机等传感器就可以对移动机器人进行精准的位姿测量。然而，由于移动机器人具有较高的灵活性和机动性，并运行于多站点的大尺度场景下，空间尺度大，存在站点、动态障碍物遮挡等问题，上述简单的测量方法难以满足实际需求。在过去的几十年中，多种位姿测量技术如外部观测系统，三角测量技术以及概率自定位技术等被广泛的研究，虽已被验证可以用于在大规模场景下进行移动机器人的位姿测量，但仍存在明显的不足，具体概括如下：

(1)外部观察系统，例如论文《anapproachtowardsautomatednavigationofvehiclesusingoverheadcameras》和专利cn201810433262.5《一种足式机器人本体状态测量方法》中的运动捕捉系统，是准确获得机器人位姿的常见方法。该方法需要对大量图片进行复杂的信号处理并且要求机器人在传感器观察范围内。因此在大尺度和多站点的场景中，该方法需要放置大量的外部设备(如视觉传感器)，提高了测量系统的规模、成本以及布置的时间。

(2)三角测量技术，如专利cn201621117777.7《一种可增强测量范围的激光测距装置》和cn201510467525.0《基于光电扫描的室内移动机器人位姿测量系统及测量方法》，可以在大场景中实现高精度测量。但该方法仍存在反射器布局较为复杂以及站点或障碍物遮挡导致定位失败等问题。

(3)概率定位技术，其通常采用扩展卡尔曼滤波器和蒙特卡罗定位等技术，如论文《基于自适应多提议分布粒子滤波的蒙特卡洛定位算法》所示，可以使用移动机器人自身传感器进行位姿测量，该方法测量精度较低。

综上所述，目前存在的移动机器人位姿测量系统在大尺度多站点的复杂场景中存在系统规模和成本大，布置的时间长，易受障碍物遮挡导致测量失效或者测量精度低等不足，并且没有对大尺度多站点环境的高精度位姿测量进行专门的研究。

技术实现要素：

针对现有技术的以上缺陷或改进需求，本发明提供了一种用于大尺度多站点场景的移动机器人位姿测量方法及系统，其目的在于，通过将移动机器人在站点示教，利用点云匹配技术根据多帧雷达数据以及地图点云数据，计算出其在各个站点示教位置的精准位姿，即目标位姿；然后当机器人运动在站点附近或停靠在站点时，利用环绕车体的ccd相机以及站点标志，采用视觉测量方法获得目标位姿与当前位姿的相对位姿，结合目标位姿和相对位姿即可从实现移动机器人在站点附近的精确位姿测量。

为实现上述目的，按照本发明的一方面，提出了一种用于大尺度多站点场景的移动机器人位姿测量方法，包括如下步骤：

s1移动机器人在站点示教，获取移动机器人在该站点示教位置的绝对位姿，即目标位姿；

s2利用视觉测量方法获得移动机器人在该站点目标位姿时与该站点标志的相对位姿；

s3重复步骤s1、s2，直至获得移动机器人在所有站点的目标位姿，以及在目标位姿与相应站点标志的相对位姿；

s4移动机器人在多站点场景中移动，当移动机器人停靠某一站点或在站点附近，且移动机器人上的ccd相机能够清晰观测到该站点的站点标志时，利用视觉测量方法获得移动机器人在当前绝对位姿与观测站点标志的相对位姿；

s5根据移动机器人在该观测站点目标位姿与站点标志的相对位姿，以及移动机器人在当前绝对位姿与观测站点标志的相对位姿，计算出移动机器人当前绝对位姿与该观测站点目标位姿的相对位姿，进而结合该观测站点的目标位姿得到移动机器人当前绝对位姿，完成移动机器人位姿测量。

作为进一步优选的，所述步骤s1中，获取移动机器人在站点示教位置的目标位姿，包括如下步骤：

s11获取地图点云作为站点处的源点集q；通过移动机器人上的激光雷达采集周围环境的点云数据，然后将其与移动机器人的初始位姿p0结合，得到移动机器人在站点处的目标点集pn，n＝1；

s12计算源点集q到目标点集pn之间，使误差函数达到最小值的变换矩阵tn；

s13通过变换矩阵tn校正当前的目标点集pn，得到新的目标点集pn+1与当前变换矩阵tn，并执行pn＝pn+1，n＝n+1；

s14判断是否满足收敛条件，否则返回步骤s12，是则计算完成当前迭代的总变换矩阵tt＝t1·t2·t3…tn；

s15重复s11～s14若干次，得到多个总变换矩阵，将其分别与初始位姿p0结合，得到多个绝对位姿，对该多个绝对位姿求平均值得到移动机器人在站点示教位置的目标位姿。

作为进一步优选的，所述步骤s11中，将由移动机器人建图功能得到的地图进行栅格化处理得到栅格化地图，将该栅格化地图转化为地图点云，并对该地图点云进行去噪，以去噪后的地图点云作为站点处的源点集q。

作为进一步优选的，对栅格化地图中存在障碍物的部分进行中心点选取，从而将栅格化地图转化为地图点云。

作为进一步优选的，采用具有噪声的基于密度的聚类方法对地图点云进行去噪。

作为进一步优选的，通过蒙特卡洛算法估算出移动机器人的初始绝对位姿，以该初始绝对位姿作为移动机器人的初始位姿p0。

作为进一步优选的，所述误差函数e(r,t)的计算公式如下：

其中，qj为源点集中的点，nm为源点集中点的数量；pi为目标点集中的点，nd为目标点集中点的数量。

作为进一步优选的，采用svd分解法计算误差函数的最小值。

作为进一步优选的，所述步骤s2中，获取移动机器人在站点目标位姿时与站点标志的相对位姿，包括如下步骤：

s21移动机器人在站点示教位置时，通过移动机器人上的ccd相机拍摄站点标志并提取站点标志上的特征点，且特征点数不小于4个；

s22根据张正友标定法建立特征点在相机坐标系的坐标位置与其像素位置的线性关系，从而获得ccd相机与站点标志特征点的相对位置；

s23根据ccd相机与站点标志特征点的相对位置，以及移动机器人坐标系与相机坐标系的对应关系，得到移动机器人与站点标志特征点的相对位置，即移动机器人与站点标志的相对位姿。

按照本发明的另一方面，提供了一种用于实现上述用于大尺度多站点场景的移动机器人位姿测量方法的系统，包括不少于两个激光雷达、不少于四个ccd相机，以及多个站点标志，其中：所述站点标志安装在各站点处；两个所述激光雷达分别安装在移动机器人车体对角方向，且均位于移动机器人车体高度的中间位置；四个所述ccd相机分别安装在移动机器人车体侧面前、后、左、右四个方向，且安装高度与所述站点标志高度相同。

总体而言，通过本发明所构思的以上技术方案与现有技术相比，主要具备以下的技术优点：

1.本发明提出的方法使用改进的激光雷达点云匹配技术和视觉测量技术获的移动机器人在站点附近精准的绝对位姿，巧妙的利用示教时无严格实时性要求的特点，进行多次点云匹配技术得到的精准的目标位姿。

2.本发明提出的激光雷达点云匹配结合视觉测量的方法实时、精准的获得移动机器人当前绝对位姿，视觉测量精准的当前位姿与点云匹配获得的站点示教位置的目标位姿间的相对位姿，结合目标位姿以及相对位姿得到移动机器人在站点附近的精准绝对位姿，在使用少量硬件资源的情况下，实现了理想的位姿测量精度，相比概率定位技术，位姿测量精度极大提升。

3.本发明方法使得移动机器人与站点标志物之间的距离比较接近，避免了其他站点或障碍物遮挡的问题，同时在多站点场景下测量系统的测量范围很大，非常适用大尺度多站点场景。

4.本发明提出的测量系统主要使用了移动机器人自身的传感器以及布置便捷的站点标志(人工标志或站点原有标志)，并对传感器的布局方式进行设计，使得移动机器人能有效的获取环境信息并且无需变换机器人姿态就能较好地观测到车体附近的站点标志，相比外部观察系统和三角测量，极大的降低了测量系统的成本和规模以及布置系统的难度和时间，在大尺度场景下也无需增加额外的设备。

附图说明

图1为本发明实施例移动机器人位姿测量系统布局示意图；

图2为本发明实施例用于大尺度多站点场景的移动机器人位姿测量方法流程图；

图3是本发明实施例改进点云匹配获取移动机器人在站点示教位置绝对位姿的算法流程图。

具体实施方式

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

本发明实施例提供的一种用于大尺度多站点场景的移动机器人位姿测量系统，如图1所示，包括至少两个激光雷达和至少四个ccd相机，以及各个站点的站点标志，其中：

站点标志是每个站点都具有的，标志要求可以能被视觉系统轻松、准确的提取出四个及以上特征点；具体的，站点标志可以由人工在站点侧面粘贴二维码、黑色方块等具有明显区分度的人工标志，也可以是站点上标志如螺钉、门把手等具有明显特征物体；站点标志的大小在20mm*20mm以上，保证ccd相机距离其300mm的位置依然可以清晰拍摄。

两个激光雷达分别安装在移动机器人车体对角方向并安装在车体高度的中间位置；具体的，雷达的测量角度应该大于等于180°，保证两个激光雷达的测量范围尽可能的涵盖移动机器人车体周围360°场景，使其都可被激光雷达覆盖；雷达的扫描周期为40hz，即可以在2.5秒内完成100帧雷达数据的采集。

四个ccd相机环绕在车体前、后、左、右四个方向，保证移动机器人以任意姿态停靠或经过站点时都能观测到站点标志；具体的，四个ccd相机在车体侧面的安装高度需要和站点标志高度相同；四个ccd相机的视场角为45°，焦距为150mm，并具有较大的景深，能保证移动机器人距离站点标志70mm～300mm范围内，标志物的成像清晰，能精准提取出特征点。

该测量系统的应用场景为大尺度且多站点的场景，这种场景下外部观测系统和三角测量难以实际应用的同时概率定位技术测量精度不足；其中，大尺度指的是场景的平面空间范围理论上可以无限大，实际中指的是超过10万平方米的室内场景，如cnc加工车间，汽车零部件制造车间；多站点场景指的是有很多停靠站点任务的场景，例如cnc加工车间中从a号cnc到b号cnc执行物料搬运工作。

当移动机器人车体在停靠站点或者运动在站点附近时，总会有一个ccd相机能捕捉到站点标志，例如机器人在站点1附近时，ccd相机1可以观测到站点n的标志，机器人在站点n附近时，ccd相机4可以观测到站点n的标志，同时车体姿态不需要调整。

对于本发明实施例所针对的2-d测量区域，在此明确本发明中所需描述的各个与位姿有关的名词的含义：

(1)位姿：移动机器人的位置(x，y坐标)及其姿态(旋转角度θ)；

(2)绝对位姿：移动机器人在世界坐标系下，即已确定的地图坐标系中的机器人位姿，表示为p～(x,y,θ)；

(3)目标位姿：移动机器人在站点示教位置时，通过改进点云匹配的方法得到的移动机器人绝对位姿，表示为pg～(xg,yg,θg)；

(4)相对位置：在移动机器人坐标系下，移动机器人和站点标志中特征点的相对位置p～(x,y)；

(5)相对位姿：移动机器人当前的绝对位姿与其在目标站点的目标位姿之间的相对位姿，表示为e～(xe,ye,θe)；

(6)当前绝对位姿：移动机器人通过目标位姿与相对位姿，计算得到的当前实际所处在的绝对位姿，表示为pa～(xa,ya,θa)。

位姿测量系统在大尺度多站点场景进行移动机器人位姿测量，如图2所示，包括如下步骤：

步骤1：移动机器人在站点示教，利用改进的激光雷达点云匹配技术获得移动机器人在站点示教位置的绝对位姿，即目标位姿，其中示教位置保证四个环绕车身的ccd相机中至少一个ccd相机正对站点标志；移动机器人在站点示教位置的位姿可以在地图坐标系中描述为pg～(xg,yg,θg)，并作为在移动机器人再次停放或在该站点附近运动时的目标位姿。

进一步的，移动机器人在站点示教位置采用改进点云匹配技术获得移动机器人在站点示教位置精准的目标位姿，如图3所示，包括如下步骤：

第一步：将栅格地图转化为地图点云，然后对其进行去噪处理，将去噪后的地图点云作为目标站点处的源点集q。

其中，栅格化地图是将移动机器人的建图功能所预先得到的高精度地图进行栅格化处理所得到的地图，而栅格化处理会使地图中障碍物所处的位置灰度值较大，而空旷部分灰度值较小。

其中，地图点云化过程为对栅格化地图中存在障碍物的部分(灰度较高部分)进行中心点的选取，从而将原高精度的地图变为地图点云。

其中，地图点云的去噪处理通过dbscan算法(density-basedspatialclusteringofapplicationswithnoise，具有噪声的基于密度的聚类方法)去除地图点云中的噪声点，用于提升点云匹配的精度。

第二步：通过蒙特卡洛算法估算出移动机器人的初始绝对位姿，以该初始绝对位姿作为移动机器人的初始位姿p0。

第三步：获得激光雷达采集周围环境的点云数据，然后将其与初始位姿p0结合，得到机器人在目标站点处的目标点集pn，初始时n＝1。

其中，激光雷达测量的数据点集处于移动机器人坐标系中，通过将该点集中所有的点与初始位姿p0合并计算，得到处于世界坐标系中的目标点集pn。

第四步：计算源点集q到目标点集pn之间，使误差函数达到最小值的转换矩阵tn。

其中，对于先前已经选定的源点集(参考集)和目标点集nm,nd∈n，在每次迭代过程中，寻找两个点集中距离最近的点，求取欧氏距离平方和，当欧式距离平方和最小时，得到两组点集数据之间的变换矩阵tn；目标函数在变换之后与源点集的误差函数可表示为：

其中，计算误差函数的最小值的方法选取为svd分解法。

第五步：通过变换矩阵tn校正当前输出的目标点集pn，即pn+1＝tn·pn，得到新的目标点集pn+1与当前变换矩阵tn，最后执行pn＝pn+1，n＝n+1。

第六步：判断是否满足收敛条件？收敛条件为误差函数e(r,t)达到预设的最小阈值；是则进行下一步，否则返回到第四步继续计算新的目标点集。

第七步：计算完成当前迭代的总变换矩阵tt＝t1·t2·t3…tn；将每次迭代计算得到的tt存储起来备用。

第八步：判断是否执行了100次？是则继续进行下一步，否则返回到第三步使用新的雷达数据进行循环计算。

其中，100次为本发明实例的一个测试次数，实际情况下为提升精度或提升速度，可以相应地增加或减少循环次数。

第九步：首先汇总所有迭代总变换矩阵tt，结合初始位姿p0计算得到目标位姿，然后对100组目标位姿取平均值以消除随机误差，获得移动机器在该站点示教位置的精准绝对位姿，即目标位姿pg～(xg,yg,θg)。

步骤2：使用环绕车身的ccd相机以及站点标志，利用视觉测量技术获得移动机器人在该站点目标位姿时与该站点标志中特征点之间的相对位置^tp～(xt,yt)，具体包括如下步骤：

第一步，移动机器人在站点示教位置使用ccd相机拍摄站点标志并提取特征点，特征点数大于等于4，且能获得4个特征点之间的位置关系；其中，特征点数大于4能有效提升定位精度并且是满足标定的特征点需求，这里选取特征点数量为4个。

第二步：根据张正友标定法来建立特征点在相机坐标系的坐标位置与其像素位置的线性关系，从而获得ccd相机与站点标志的相对位置。

其中，移动机器人与站点标志之间的相对位置计算过程主要包括相机校准和坐标变换。可以基于以下描述的针孔相机模型，通过站点标志的参考点与其在平面中的投影之间的线性关系，建立相机坐标系与站点标志特征点的对应关系：

其中，[u,v]^t表示投影面上的点，[xc,yc,zc]^t是相机坐标系中的点；fx＝-mxf，fy＝-myf，其中，f是焦距，mx是像素宽度和单位之间的比例(例如，以米为单位)，my是像素高度和单位之间的比例；u0和v0是投影面上原点的坐标。

第三步，根据ccd相机和机器人坐标的相对位姿，推算出移动机器人与站点标志特征点的相对位置，即移动机器人与站点标志的相对位姿。

其中，考虑到相机和移动机器人具有不同参考帧，对相机坐标系中的点添加欧几里得变换^crr，得到移动机器人坐标系与相机坐标系的非线性方程型对应关系如下：

其中，[xr,yr,zr]^t是移动机器人坐标系中的点；^crr可以通过机械装配获得。

进而根据式(2)和(3)推导得出移动机器人坐标系中的点与站点标志之间的点的对应关系如下：

其中，对于式(4)，假设移动机器人在目标站点的目标位姿处，测得的特征点在移动机器人坐标系中的坐标为^tp＝(xt,yt)，则对应的四个不同特征点的坐标位置为a(atx,aty)、b(btx,bty)、c(ctx,cty)和d(dtx,dty)。

步骤3：重复步骤1和步骤2，直至获得机器人在所有站点的目标位姿及其与相应站点标志的相对位姿。

步骤4：移动机器人在场景的站点间执行任务，并开启测量系统，测量系统指机器人配备的两个激光雷达和四个ccd相机。

步骤5：判断移动机器人是否停靠某一站点或在站点附近，并且环绕车身的ccd相机可较为清晰地观测到该站点的站点标志；否则返回步骤4，是则进行下一步。

具体的，移动机器人再次停靠站点或在站点附近时，前后左右任意方向上的ccd相机可观察到任意站点标志即可进行该步骤。

步骤6：利用视觉测量方法获得移动机器人当前位姿与该站点标志之间的相对位姿。

具体的，利用步骤2的视觉测量方法，获得当前机器人位姿与站点标志特征点的相对位置。

其中，利用步骤2的视觉测量方法，假设移动机器人在目标站点附近的当前位姿处，测得的特征点在移动机器人坐标系中的坐标为^ap＝(xa,ya)(a(aax,aay)、b(bax,bay)、c(cax,cay)和d(dax,day))，对应步骤2中测得目标位姿处的特征点坐标^tp＝(xt,yt).

步骤7：根据步骤3得到的该站点的目标位姿与站点标志所有特征点的相对位置以及和步骤6得到的当前绝对位姿与同一站点标志所有特征点的相对位置，计算出移动机器人当前绝对位姿与该站点目标位姿的相对位姿。

具体的，建立特征点在目标位姿处与当前位姿处的齐次变换矩阵^tea，用以下两个方程描述^tp和^ap之间的坐标变换关系：

其中，基于式子(6)，在目标移动机器人坐标系中，当前位姿与目标位姿之间的相对位姿可以描述为^te～(xte,yte,θte)。为了计算^tea，需要至少两个参考点来找到^tea的三个参数的解。本实施例中选用的即为步骤2中所确定的标志物上的四个参考点abcd，故结合四个参考点坐标与式子(6)，式(5)可以变换如下：

其中，(atx,aty)和(aax,aay)、(btx,bty)和(bax,bay)、(ctx,cty)和(cax,cay)、(dtx,dty)和(dax,day)分别是移动机器人处于目标位姿处和当前绝对位姿处的坐标系中参考点a、b、c、d的坐标。通过式子(7)、(8)、(9)、(10)可以得到八个方程，来求解此处的xte、yte和θte三个变量，形成了一个超静定方程如下：

根据式(11)可知此时该矩阵可以转换为ax＝b的标准格式，对其进行分解重构后如下：

在式(12)中标准格式ax＝b进行了因式转换和qr分解，最终得到参考点的值为矩阵a分解后的r^-1q^t与目标值矩阵b的乘积。所以根据此时的方程即可计算得出参考点的xte、yte和θte三个参数，从而确定移动机器人在当前绝对位姿与目标位姿的相对位姿^te～(xte,yte,θte)。

步骤8：根据该站点的目标位姿以及当前绝对位姿与目标位姿的相对位姿，计算得到移动机器人的当前绝对位姿。

其中，根据之前计算得到的目标位姿pg～(xg,yg,θg)和相对位姿^te，通过式(13)、(14)组合目标位姿和相对位姿来生成当前的绝对位姿pa～(xa,ya,θa)，该当前的绝对位姿即目前移动机器人所处的精确的位姿。

步骤9：判断是否结束位姿测量，否则执行步骤4，是则停止。

采用上述位姿测量系统和方法，即可在任意站点附近以任意姿态，获得移动机机器人的当前绝对位姿，从而在大面积和多站点的场景中，当移动机器人停靠站点或运动到站点附近时，就可以方便准确地得到移动机器人当前绝对位姿，并进行有益的后续操作，如利用卡尔曼滤波融合当前绝对位姿和小车自身定位数据提升机器人的定位精度、评价机器人的停靠精度等、评价机器人轨迹跟踪精度等。

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