基于天花板导航的定位方法

计算;推算;计数设备的制造及其应用技术1.本发明涉及定位方法领域，尤其涉及一种基于天花板导航的定位方法。背景技术：2.目前，移动机器人技术已经有大规模的相对成熟的应用，例如仓储、物流场景中的工业机器人，和商场、饭店、酒店中的服务机器人等，为人们提供搬运、引导等服务。这些应用的前提是机器人能够对周边环境采样与建图，并获得自身在环境中的位置信息。大多数的移动机器人运行的环境处于室内平面，因此使用单线激光雷达作为传感器完成建图和导航是大多数解决方案的选择。3.然而，单线激光雷达的扫描局限于雷达所在位置的水平面内，若在这一高度上出现过多的障碍物遮挡或者移动物体，将对单线激光雷达的定位效果带来致命的影响。例如，在商场、饭店等场景中运行的服务型移动机器人，若周围有太多移动中的人员，雷达的大部分视野将被遮挡，导致定位的丢失。技术实现要素：4.针对上述现有技术中的不足，本发明提供一种基于天花板导航的定位方法，使用安装在机器人顶部、垂直面向上方的tof相机采集天花板的信息，并利用该信息实现定位与导航。5.为了实现上述目的，本发明提供一种基于天花板导航的定位方法，包括步骤：6.s1：利用垂直向上安装在一移动机器人顶部的tof相机获取一天花板的原始图像数据；7.s2：对所述原始图像数据进行预处理获得所述天花板的边缘点云坐标集；8.s3：利用所述边缘点云坐标集实现所述移动机器人的天花板导航里程计。9.优选地，所述s2步骤进一步包括步骤：10.s21：使用sobel算子提取所述原始图像数据的边缘，获得原始边缘图像；11.s22：融合所述原始边缘图像的各通道提取边缘，获得边缘信息图像；12.s23：滤除所述边缘信息图像的重复边缘；13.s24：将当前所述边缘信息图像反投影至三维空间得到边缘点云坐标集；14.优选地，所述s21步骤中分别使用两种卷积核对所述原始图像数据进行处理：[0015][0016]其中，※表示信号处理中的二维卷积操作，ski和syi分别为强调水平方向和垂直方向边缘的图像；将sxi和syi分别进行像素的平方再开方，得到分别进行像素的平方再开方，得到si表示经过sobel算子处理过后的所述原始边缘图像。[0017]优选地，所述s22步骤中：[0018]将所述原始边缘图像的可用的各通道信息提取的边缘值图像进行求平均，得到全面的边缘信息图像e，得到的e各像素点值越大，代表该像素点越有可能是构成边缘结构的像素点；设置阈值λ，将e中数值大于λ的像素保留置1，其余置0，获得所述边缘信息图像i、j表示所述边缘信息图像中像素点的坐标。[0019]优选地，所述s23步骤中：[0020]在三维空间中，所述tof相机的位置、真实边缘上的点和虚假边缘上的点接近构成一直线，因此在图像上，真实边缘与虚假边缘的距离较小；假设虚假边缘与真实边缘在图像上的距离不超过δ个像素；对于中每个像素点，若其δ半径内存在深度值小于该点深度值0.8倍的点，则认为该点为重复边缘，将中的该点处置0。[0021]优选地，所述s24步骤中：[0022]将当前中值为1的点对应一帧数据中边缘结构所处的位置；将所有中值为1的坐标对应的坐标q[i,j]作为所述边缘点云坐标集p；值为1的坐标对应的坐标q[i,j]作为所述边缘点云坐标集p；[0023]优选地，所述s3步骤进一步包括：[0024]s31：预估当前帧的位姿；[0025]s32：利用相位相关法位移粗匹配；[0026]s33：利用icp迭代求解位姿；[0027]s34：将符合条件的帧作为关键帧提交至地图。[0028]优选地，所述s31步骤中，设定当前帧的位姿变化量与上一帧的位姿增量相同。[0029]优选地，所述s34步骤中，在满足以下条件之一时，将当前帧作为关键帧提交至地图：[0030]a.当前帧的中心点与上一关键帧中心点的距离大于第一预设值λ1；[0031]b.当前帧的质心与上一关键帧的质心距离大于第一预设值λ1；[0032]c.当前的配准分数小于第二预设值λscore；第二预设值λscore小于1。[0033]本发明由于采用了以上技术方案，使其具有以下有益效果：[0034]tof相机被安装于机器人顶部并且朝向上方，相机视野几乎不受周围大量移动的人、物的影响，使得移动机器人在十分复杂的动态场景中也能获得准确而稳定的定位。使用边缘提取的方式，从tof相机的原始数据中提取出表示边缘结构的点云，使用以上点云，实现高效、高准确率的匹配，实现天花板导航里程计。本发明具有：高效、准确、鲁棒性强；适用范围广，能在各种天花板场景下使用；导航方法不受传感器所在高度的遮挡物影响，具有很强的实用性的优点。附图说明[0035]图1为本发明实施例的基于天花板导航的定位方法的流程图。具体实施方式[0036]下面根据附图图1，给出本发明的较佳实施例，并予以详细描述，使能更好地理解本发明的功能、特点。[0037]本发明要求使用场景为室内，且室内的天花板具有一定的几何构造或者纹理特征(例如不可在全白色的纯平天花板下使用)。大多数的商场、工厂、以及办公室、图书馆等公共设施中的天花板符合使用要求，因此本发明具有较为广泛的应用场景。[0038]请参阅图1，本发明实施例的一种基于天花板导航的定位方法，包括步骤：[0039]s1：利用垂直向上安装在一移动机器人顶部的tof相机获取一天花板的原始图像数据；[0040]tof相机采集的原始图像数据一般包括深度值通道、强度值通道，某些相机还提供rgb值通道或灰度值通道(即普通的彩色图像或灰度图像)。各个通道的图像大小相同。假设tof相机采集的一帧数据为f，f中包含若干通道，记为ci，i∈{深度,强度,…}，共n＝|{深度,强度,…}|个通道的数据。根据深度图像计算得到的天花板原始点云数据记为q，q中每一个像素点是一个三维坐标点(x,y,z)。[0041]s2：对原始图像数据进行预处理获得天花板的边缘点云坐标集；[0042]预处理的目标是从原始的各个通道的数据中得到三维空间中表示边缘结构的边缘点云坐标集。[0043]s2步骤进一步包括步骤：[0044]s21：使用sobel算子提取原始图像数据的边缘，获得原始边缘图像；[0045]s21步骤中分别使用两种卷积核对原始图像数据进行处理：[0046][0047]其中，※表示信号处理中的二维卷积操作，sxi和syi分别为强调水平方向和垂直方向边缘的图像；将sxi和syi分别进行像素的平方再开方，得到分别进行像素的平方再开方，得到si表示经过sobel算子处理过后的原始边缘图像，边缘所在位置的像素将有较大的数值；[0048]s22：融合原始边缘图像的各通道提取边缘，获得边缘信息图像；[0049]s22步骤中：[0050]将原始边缘图像的深度图像、强度值图像、rgb图像等可用的各通道信息提取的边缘值图像进行求平均，得到全面的边缘信息图像缘值图像进行求平均，得到全面的边缘信息图像得到的e各像素点值越大，代表该像素点越有可能是构成边缘结构的像素点；设置阈值λ，将e中数值大于λ的像素保留置1，其余置0，获得边缘信息图像i为指示函数，i、j表示边缘信息图像中像素点的坐标。[0051]s23：滤除边缘信息图像的重复边缘；[0052]s23步骤中：[0053]天花板自身结构在透视投影的视角下构成遮挡，且在遮挡边界处构成虚假的边缘结构，应当将这些错误的边缘去除。容易观察到，在三维空间中，tof相机的位置、真实边缘上的点和虚假边缘上的点接近构成一直线，因此在图像上，真实边缘与虚假边缘的距离较小；假设虚假边缘与真实边缘在图像上的距离不超过δ个像素，δ为自然数；对于中每个像素点，若其δ半径内存在深度值小于该点深度值0.8倍的点，则认为该点为重复边缘，将中的该点处置0。[0054]s24：将当前边缘信息图像反投影至三维空间得到边缘点云坐标集；[0055]s24步骤中：[0056]将当前中值为1的点对应一帧数据中边缘结构所处的位置；将所有中值为1的坐标对应的坐标q[i,j]作为边缘点云坐标集p；中值为1的坐标对应的坐标q[i,j]作为边缘点云坐标集p；[0057]s3：利用边缘点云坐标集实现移动机器人的天花板导航里程计。[0058]天花板导航的里程计部分接收预处理线程输出的点云ps，计算与前一帧pt间的位姿变化，从而得到当前帧位姿的实时估计，同时将符合条件的关键帧提交到地图。[0059]将上一帧已配准帧在世界坐标系下的位姿记为lastdelta,上一帧的位姿增量记为lastdeltainc，当前帧在世界坐标系下的位姿记为delta，当前帧的位姿增量记为deltainc。所有表示位姿的量均由一个三元组(x,y,th)构成，其中x、y为xy平面上的平移偏移量，th为角度偏移量，逆时针为正，0°为x轴正方向。天花板里程计线程在运行过程中不断更新上述变量，从而计算当前帧的全局位姿坐标。程序开始运行时，将lastdelta、lastdeltainc、delta、deltainc均初始化为(0,0,0)。[0060]s3步骤进一步包括：[0061]s31：预估当前帧的位姿；[0062]假设移动机器人的运动在比较短的时间范围内可近似为匀速直线运动，因此可猜测当前帧的位姿变化量与上一帧的位姿增量相同，即用lastdelta和lastdeltainc初始化当前帧的delta、deltainc：[0063]deltainc＝lastdeltainc[0064]rth＝lastdelta.th[0065]delta.x＝cos(rth)*deltainc.x-sin(rth)*deltainc.y+lastdelta.x[0066]delta.y＝sin(rth)*deltainc.x+cos(rth)*deltainc.y+lastdelta.y[0067]delta.th＝rth+lastdeltainc.th[0068]s32：利用相位相关法位移粗匹配；[0069]对于某一帧待匹配的天花板点云帧ps以及前一帧已匹配完成的点云帧pt，将当前帧点云进行deltainc＝(dx,dy,dth)的位姿变换得到在前一帧坐标系下的点云ps′，即：[0070]ps′[i].x＝cos(dth)*ps[i].x-sin(dth)*ps[i].y+dx[0071]ps′[i].y＝sin(dth)*ps[i].x+cos(dth)*ps[i].y+dy[0072]假设天花板高度上限为h，设置高度区间值h。可以按照高度将整个空间分为段。对于高度值k*h到(k+1)*h的范围，1≤k≤k，将ps′中所有ps′[i].z处于这一范围内的点取出。建立一副空白的边长为m的正方形图像，即一个m×m的值为全0的矩阵。对于上述选出的点ps′[i]，将其在xy平面上缩放τ倍并对m取模后投影至图像上[(ps[i].x/τ)％m,(ps[i].y/τ)％m]处的像素上，使得该点处像素值加1。将得到的图像像素值归一化至0到255。经过上述步骤将帧p′s在k*h到(k+1)*h的高度范围内转化为一张m×m的灰度图像gsk。对于其他高度，分别进行如上的处理，得到若干灰度图像gs1……gsk。对于已匹配的前一帧点云pt，同样经过上述处理，得到若干灰度图像gt1……gtk。[0073]将同一高度区间对应的gsi和gti做相位相关法得到相位谱：[0074]a.将gsi和gti中每个像素数值添加上虚部，得到gcsi和gcti；[0075]b.对gcsi和gcti进行傅里叶变换得到fsi和fti；[0076]c.记⊙为两矩阵对应元素的乘法。得到互功率谱r＝(fsi⊙fti)/|fsi⊙fti|；[0077]d.对互功率谱r进行傅里叶变换的逆变换，得到带有虚部的相位相关结果f’；[0078]e.对f’中每个元素计算模长，得到谱phi。[0079]每一对gsi和gti进行相位相关，分别得到谱phi。将所有结果叠加：。将所有结果叠加：在ph上找出数值最大的峰值位置[m,n]，可得到在xy平面上，原点云ps相对于pt的平移偏差量为[m*τ,n*τ]。进行相位相关法解粗位移后，deltainc的更新方式为：[0080]deltainc＝(deltainc.x+m*τ,deltainc.y+n*τ,deltainc.th)[0081]s33：利用icp迭代求解位姿；[0082]首先用相位相关法后得到的deltainc＝(dx,dy,dth)更新当前点云帧p′s：[0083]ps′[i].x＝cos(dth)*ps[i].x-sin(dth)*ps[i].y+dx[0084]ps′[i].y＝sin(dth)*ps[i].x+cos(dth)*ps[i].y+dy[0085]建立长度与ps′中点云数量相等的列表corres，其中第i个元素记录在pt中与ps′[i]距离最小的元素的下标，即第i个元素的最近邻下标：离最小的元素的下标，即第i个元素的最近邻下标：若pt中与ps′[i]距离最近的元素距离仍然大于135毫米，则corres[i]＝-1。[0086]将下述4个步骤重复迭代n次，其中n一般可取10：[0087]a.计算与上一帧之间的旋转变化量。[0088]首先建立一个长度与ps′中元素个数相同的列表angles，将angles中所有元素初始化为一个较大的值，例如9999。对于ps′中的第i个元素，若corres[i]为-1，则不更新angles[i]；否则将更新angles[i]：[0089]j＝corres[i][0090]angles[i]＝arcsin((pt[j].x*p′s[i].y-pt[j].y*p′s[i].x)/len(pt[j])/len(p′s[i]))[0091]即，angles[i]存储向量pt[j]和向量p′s[i]间的夹角。len(v)表示计算向量v的模长。将angles中值为9999的元素去除后，剩余元素从小到大排序得到angles’，取出排序后的第三四分位数与第一四分位数，做差，乘以2，得到经验阈值thresang。[0092]令则得到角度的更新量为deltainc.th＝deltainc.th+thinc。[0093]b.根据上一步计算得到的旋转增量更新ps′。[0094]c.计算与上一帧之间的平移变化量。[0095]重新计算corres1，corres2，分别表示ps′在pt中距离最近和距离第二近的点。即pt[j]|,s.t.corres1[i]！＝corres2[i]。点ps′[i]向corres1[i]与corres2[i]构成的直线做垂线，垂线方向记为norm[i]。令diff[i]＝corres1[i]-ps′[i]。可得关于dx和dy的二元一次非齐次线性方程组：[0096](norm[i].x)^2*dx+(norm[i].x*norm[i].y)*dy＝norm[i].x*dot(norm[i],diff[i])[0097](norm[i].y*norm[i].x)*dx+(norm[i].x)^2*dy＝norm[i].y*dot(norm[i],diff[i])[0098]其中dot(v,w)表示v和w两向量的点乘。ps′中每个点可得到上述的一组方程，将所有方程的系数累加，即初始化a0＝a1＝a2＝a3＝b0＝b1＝0，对于所有i：[0099]a0＝a0+(norm[i].x)^2[0100]a1＝a1+norm[i].x*norm[i].y[0101]a2＝a2+norm[i].y*norm[i].x[0102]a3＝a3+(norm[i].x)^2[0103]b0＝b0+norm[i].x*dot(norm[i],diff[i])[0104]b1＝b1+norm[i].y*dot(norm[i],diff[i])[0105]上述方程组的解即平移的增量：[0106][0107]deltainc.x＝deltainc.x+d1/d，deltainc.y＝deltainc.x+d2/d。[0108]d.根据上一步计算得到的平移增量更新ps′以及corres。重新计算corres时，同时计算当前配准的得分odometry_score。该得分在s34中将被使用。计算方式如下：令count＝0，weightsum＝0，对于ps′中的第i个点，若pt[j]中有有效的最近邻，则count＝count+1且weightsum＝weightsum+weightfunc(‖p′s[i]-pt[j]‖)。其中weightfunc(dist)是关于dist的函数，dist越大，函数值越小。一般可取线性函数或负指数函数等。若count/np》＝0.8，则w＝1，否则w＝gaussmf(count/np,0.3,0.8)。得分odometry_score＝weightsum/(count*w)，其中np是p′s[i]中点的个数，gaussmf(x,σ,c)是标准差为σ均值为c的高斯隶属函数。[0109]s34：将符合条件的帧作为关键帧提交至地图。[0110]经过先前步骤，当前的deltainc中存储的是当前帧在上一帧坐标系下的位姿。那么，当前帧在世界坐标系下的位姿可由前一帧的位姿lastdelta和当前帧的deltainc计算得到：[0111]rth＝lastdelta.th[0112]delta.x＝cos(rth)*deltainc.x-sin(rth)*deltainc.y+lastdelta.x[0113]delta.y＝sin(rth)*deltainc.x+cos(rth)*deltainc.y+lastdelta.y[0114]delta.th＝rth+lastdeltainc.th[0115]此时，delta中存储的是在世界坐标系下的位姿和旋转量。将deltainc的值赋值给lastdeltainc、将delta的值赋值给lastdelta，在里程计线程从预处理线程接收到下一帧时使用。[0116]在满足以下条件之一时，将当前帧作为关键帧提交至地图：[0117]a.当前帧的中心点(delta.x,delta.y)与上一关键帧中心点的距离大于第一预设值λ1；[0118]b.当前帧的质心与上一关键帧的质心距离大于第一预设值λ1；[0119]c.当前的配准分数小于第二预设值λscore；第二预设值λscore小于1。[0120]以上参数λ1可根据实际情况不同调整，例如tof相机的fov越大、天花板离相机距离越大，则λ1应当越大。λscore取小于1的值。[0121]以上结合附图实施例对本发明进行了详细说明，本领域中普通技术人员可根据上述说明对本发明做出种种变化例。因而，实施例中的某些细节不应构成对本发明的限定，本发明将以所附权利要求书界定的范围作为本发明的保护范围。









图片声明：本站部分配图来自人工智能系统AI生成,觅知网授权图片,PxHere摄影无版权图库。本站只作为美观性配图使用,无任何非法侵犯第三方意图,一切解释权归图片著作权方,本站不承担任何责任。如有恶意碰瓷者,必当奉陪到底严惩不贷!




内容声明：本文中引用的各种信息及资料（包括但不限于文字、数据、图表及超链接等）均来源于该信息及资料的相关主体（包括但不限于公司、媒体、协会等机构）的官方网站或公开发表的信息。部分内容参考包括:(百度百科,百度知道,头条百科,中国民法典,刑法,牛津词典,新华词典,汉语词典,国家院校,科普平台)等数据,内容仅供参考使用,不准确地方联系删除处理！本站为非盈利性质站点,发布内容不收取任何费用也不接任何广告!




免责声明：我们致力于保护作者版权，注重分享，被刊用文章因无法核实真实出处，未能及时与作者取得联系，或有版权异议的，请联系管理员，我们会立即处理，本文部分文字与图片资源来自于网络，部分文章是来自自研大数据AI进行生成,内容摘自(百度百科,百度知道,头条百科,中国民法典,刑法,牛津词典,新华词典,汉语词典,国家院校,科普平台)等数据,内容仅供学习参考,不准确地方联系删除处理!的,若有来源标注错误或侵犯了您的合法权益，请立即通知我们，情况属实，我们会第一时间予以删除，并同时向您表示歉意,谢谢!