技术领域
[0001] 本
发明尤其涉及一种基于红外线灯立体阵列的机器人定位方法。
背景技术
[0002] 机器人的导航与定位技术是智能机器人领域的一个重要研究方向。机器人导航是指在有障碍物的环境中，以时间或距离最短为依据，从开始
位置到终止位置走出一条安全路径。机器人的导航方式已有很多种，根据诸多影响因素的不同，分为视觉导航、光反射导航、地图导航、惯性导航、路标导航等几种方式。
[0003] 作为机器人导航的
基础，定位问题一直是机器人研究领域的热点之一。机器人的定位就是推算机器人所在环境中的位置的过程，可以分为相对定位法和绝对定位法。相对定位根据
传感器的不同主要有基于惯性传感器的航迹推算定位方法以及基于码盘的航迹推算定位方法。相对定位法自身定位无需外部参考，但易造成误差累积，不适宜长时间的精确定位。绝对定位法主要包括主动灯塔、目标导航、地图匹配等方法，利用一种或多种外部传感器检测得到的周围环境特征对机器人在参考
坐标系中的绝对
位姿进行估算。路标导航定位方法由于可靠性好被广泛应用于室内机器人定位领域。
[0004] 目前基于路标定位的室内机器人定位方法一般需要在
天花板或地面大
密度地贴设路标，路标在室内坐标系中的坐标是已知的，将基于路标的绝对定位和航迹推算的相对定位得到的信息进行数据融合。这种方式致
力于得到实时精确的定位坐标，存在处理时间长、传感器数量多、当
天花板存在不便布置路标的区域时误差大等缺点。
[0005] 而常用的几种定位方式中，在地面贴二维码，机器人经过贴有二维码的地面通过识别二维码来定位，也是较为常见的。采用这种方式，一方面二维码磨损不利于长期使用，一方面在有人站立时，或有杂物
覆盖在地面上遮挡二维码时便无法识别。另外，因为二维码
像素密集，对摄像识别要求更高。
[0006] 除此之外，
现有技术中也公开了红外线识别定位的方式。但是现有的红外线识别需借助辅助装置作为参考坐标轴，需要在机器人处于识别装置正下方时，才能进行正确感应，否则则存在偏差。且因为现有技术
中红外线灯都处于同一
水平状态，且其往往只采用坐标定位，通过图像直接确定机器人位置，忽略了图像因成像造成的畸变和误差，定位精确度不高。
发明内容
[0007] 本发明的目的是解决上述背景技术中存在的问题，提供一种定位精确的基于红外线灯立体阵列的机器人定位方法。
[0008] 本发明采用了以下技术方案：
[0009] 一种基于红外线灯立体阵列的机器人定位方法，在环境中布置若干红外线灯立体阵列，任一所述的红外线灯立体阵列中的红外线灯排列方式互不相同，包括如下步骤：
[0010] 步骤S1，在机器人行进过程中，安装在所述的机器人上的红外线成像设备拍摄机器人行进路线上经过的红外线灯立体阵列的图像；
[0011] 步骤S2，对步骤S1中拍摄的所述的图像进行预处理，并判断预处理后的图像是否存在至少一个所述的红外线灯立体阵列；若存在，则存储图像，执行步骤S3，若不存在，则结束本次操作；
[0012] 步骤S3，基于步骤S2存储的图像获取所述的机器人的绝对位置和绝对方向
角，包括子步骤：
[0013] 子步骤S31，基于所述的红外线灯立体阵列图像确定红外线灯立体阵列所处区域；
[0014] 子步骤S32，针对确定的红外线灯立体阵列区域进行红外线灯数值化标记；
[0015] 子步骤S33，针对确定的红外线灯立体阵列区域进行红外线灯立体阵列识别，以获得红外线灯立体阵列的绝对坐标及红外线灯立体阵列中每一个红外线灯的绝对坐标；
[0016] 子步骤S34，基于子步骤S31-S33，计算红外线成像设备的绝对位置和绝对方向角；
[0017] 子步骤S35，基于子步骤S34，经坐标系转换，计算机器人的绝对位置和绝对方向角，作为修正机器人位置和方向角的视觉定位数据；
[0018] 步骤S4，利用设置在机器人上的
编码器和惯性传感器确定机器人相对于机器人起始点的相对位置和机器人相对于机器人起始点的相对方向角，校验机器人的绝对位置和绝对方向角。
[0019] 作为优选，所述的红外线灯立体阵列作为定位信标，任一个所述的定位信标包括多个位置
块，任一个所述的位置块包括多个信息块，任一个所述的信息块包括多个红外线灯，同一所述的定位信标的红外线灯的高度不在同一水平面。
[0020] 作为优选，所述的步骤S2包括子步骤：
[0021] 子步骤S21，针对所述的步骤S1中拍摄的图像进行去噪及二值化处理；
[0022] 子步骤S22，针对步骤S21处理后的图像进行红外线灯轮廓提取，以获得红外线灯轮廓图像；
[0023] 子步骤S23，针对步骤S22获得的红外线灯轮廓图像进行轮廓关系筛选，以获得红外线灯轮廓关系；
[0024] 子步骤S24，针对步骤S23获得的红外线灯轮廓关系进行判断，判断轮廓关系是否符合红外线灯立体阵列上的红外线灯关系；若符合，则至少存在一个所述的红外线灯立体阵列；若不符合，则不存在。
[0025] 作为优选，所述的子步骤S22中，所述的红外线灯轮廓提取采用圆形拟合
算法对轮廓曲线进行描绘。
[0026] 作为优选，所述的步骤S3中，所述的基于步骤S2存储的图像获取所述的机器人的绝对位置和绝对方向角的方法包括红外线灯立体阵列几何相似度比对及红外线灯数值化标记。
[0027] 作为优选，所述的子步骤S32中，所述的数值化标记包括如下步骤：
[0028] S32.1，在基于所述的子步骤S31确定的红外线灯立体阵列区域内，采用直角坐标系确定每一个红外线灯轮廓中心的图像像素坐标，并通过图像像素坐标计算出任意红外线灯轮廓中心的图像像素距离；
[0029] S32.2，判断图像像素距离最长的两个红外线灯轮廓中心，坐标连线作为基准线L；
[0030] S32.3，根据所述的基准线L将红外线灯立体阵列分为两侧；设基准线L上的两圆形轮廓圆心的图像像素坐标为A(xa，ya)和B(xb,yb)，设定基准线L的一侧具有n个图像像素坐标分别为(x1，y1)，(x2，y2)，…，(xi，yi)，…，(xn，yn)的红外线灯轮廓，其中，i∈[1，n]，i、n为自然数；
[0031] S32.4，判断所述的圆形轮廓顺逆
时针关系，采用如下计算式：
[0032] ki＝(xi-xa)×(yb-yi)-(yi-ya)×(xb-xi)；(公式一)
[0033] 设p为调整参数，当ki＞0-p时，红外线灯轮廓为顺时针关系，当ki＜0-p时，红外线灯轮廓为逆时针关系；
[0034] 顺时针关系或逆时针关系中分别以A(xa，ya)和B(xb,yb)作为起点和终点，取起点编号为序号1，终点编号为序号2；
[0035] S32.5，对(x1，y1)，(x2，y2)，…，(xi，yi)，…，(xn，yn)根据其与序号1的图像像素距离由近到远从序号3开始排序；
[0036] S32.6，设定基准线L的另一侧具有m个图像像素坐标分别为(x1’，y1’)(x2’，y2’)，…，(xj’，yj’)，…，(xm’，ym’)的红外线灯轮廓，j∈[1，m]，j、m为自然数；
[0037] S32.7，对(x1’，y1’)(x2’，y2’)，…，(xj’，yj’)，…，(xm’，ym’)根据其与序号2的图像像素距离由近到远从序号n+3开始排序；
[0038] S32.8，完成对所述的红外线灯轮廓的数值化标记。
[0039] 本发明无需借助辅助装置作为X轴、Y轴轴线建立坐标系，便能轻松进行编号，不受坐标系约束限制，机器人可随意在地面以任意路线和角度行走，且可实现精准定位。
[0040] 作为优选，所述的子步骤S34中，基于子步骤S33确定的红外线灯立体阵列及红外线灯的绝对坐标计算红外线成像设备的绝对位置和绝对方向角，使用相机
姿态估计算法对摄像头姿态进行估计得到旋转矢量rvec和位移矢量tvec；通过旋转矢量rvec计算出的旋转矩阵的逆矩阵rrmat_inv与位移矢量tvec计算出红外线成像设备的绝对位置坐标矩阵pmat，计算式为：
[0041] pmat＝-rrmat_inv·tvec；(公式二)
[0042] 作为优选，在所述的子步骤S35中，基于子步骤S34确定的红外线成像设备的绝对位置和绝对方向角，通过几何变换的方法实现计算出机器人的绝对位置和绝对方向角。
[0043] 一种基于红外线灯立体阵列的机器人定位起始坐标和起始方向角的方法，根据所述的一种基于红外线灯立体阵列的机器人定位方法，基于所述的步骤S3和步骤S4，对视觉定位确定的机器人绝对位置和绝对方向角与编码器和惯性传感器确定的机器人绝对位置和绝对方向角进行估计，并利用估计结果作为定位参考确定机器人当前的起始坐标和起始方向角。
[0044] 本发明的有益效果是：本发明简单易实现。因为本发明的红外线立体灯阵列可以为立体结构，本发明采用坐标和方向角同时识别，得到绝对位置和绝对方向角使定位更加精准。
[0045] 相比于传统在地面贴二维码的定位方式而言，本发明采用红外灯技术可以长期反复使用，且因往往可以把红外线立体灯阵列布置于天花板之类的不常利用空间，很少因为被遮挡而降低识别的准确率。相对于二维码的像素密集成像要求较高，本发明的成像要求非常稀疏，仅为一幅图片中的若干红外线立体灯阵列。
[0046] 相比于现有的一些红外线定位识别装置，本发明不需借助辅助装置作为参考坐标轴，也不需要在机器人处于识别装置正下方时才能进行定位，只需要拍摄到红外线立体灯阵列，随时随地能进行正确感应，使用的范围更大。本发明避免了图像因成像造成的畸变和误差，通过方向角、编码器、惯性编码器进行修正，定位精确度高。
[0047] 本发明红外线立体灯阵列，可以同时帮助多个机器人进行各自定位。
附图说明
[0049] 图2是本发明的红外线灯阵列排布图。
[0050] 图3是本发明定位方法原理示意图。
[0051] 图4是本发明数值化标记示意图一。
[0052] 图5是本发明数值化标记示意图二。
[0053] 图6是本发明数值化标记示意图三。
[0054] 图7是本发明数值化标记示意图四。
[0055] 图8是本发明数值化标记示意图五。
[0056] 图9是本发明数值化标记示意图六。
[0057] 图10是本发明机器人示意图。
具体实施方式
[0058] 以下结合
说明书附图，对本发明作进一步说明，但本发明并不局限于以下
实施例。
[0059] 如图1-10所示，本发明一种基于红外线灯立体阵列的机器人定位方法，一种基于红外线灯立体阵列的机器人定位方法，在环境中布置若干红外线灯立体阵列，任一所述的红外线灯立体阵列中的红外线灯排列方式互不相同，包括如下步骤：
[0060] 步骤S1，在机器人行进过程中，安装在所述的机器人上的红外线成像设备拍摄机器人行进路线上经过的红外线灯立体阵列的图像；
[0061] 步骤S2，对步骤S1中拍摄的所述的图像进行预处理，并判断预处理后的图像是否存在至少一个所述的红外线灯立体阵列；若存在，则存储图像，执行步骤S3，若不存在，则结束本次操作；
[0062] 步骤S3，基于步骤S2存储的图像获取所述的机器人的绝对位置和绝对方向角，包括子步骤：
[0063] 子步骤S31，基于所述的红外线灯立体阵列图像确定红外线灯立体阵列所处区域；
[0064] 子步骤S32，针对确定的红外线灯立体阵列区域进行红外线灯数值化标记；
[0065] 子步骤S33，针对确定的红外线灯立体阵列区域进行红外线灯立体阵列识别，以获得红外线灯立体阵列的绝对坐标及红外线灯立体阵列中每一个红外线灯的绝对坐标；
[0066] 子步骤S34，基于子步骤S31-S33，计算红外线成像设备的绝对位置和绝对方向角；
[0067] 子步骤S35，基于子步骤S34，经坐标系转换，计算机器人的绝对位置和绝对方向角，作为修正机器人位置和方向角的视觉定位数据；
[0068] 步骤S4，利用设置在机器人上的编码器和惯性传感器确定机器人相对于机器人起始点的相对位置和机器人相对于机器人起始点的相对方向角，校验机器人的绝对位置和绝对方向角。
[0069] 所述的红外线灯立体阵列作为定位信标，任一个所述的定位信标包括多个位置块，任一个所述的位置块包括多个信息块，任一个所述的信息块包括多个红外线灯，同一所述的定位信标的红外线灯的高度不在同一水平面。
[0070] 所述的步骤S2包括子步骤：
[0071] 子步骤S21，针对所述的步骤S1中拍摄的图像进行去噪及二值化处理；
[0072] 子步骤S22，针对步骤S21处理后的图像进行红外线灯轮廓提取，以获得红外线灯轮廓图像；
[0073] 子步骤S23，针对步骤S22获得的红外线灯轮廓图像进行轮廓关系筛选，以获得红外线灯轮廓关系；
[0074] 子步骤S24，针对步骤S23获得的红外线灯轮廓关系进行判断，判断轮廓关系是否符合红外线灯立体阵列上的红外线灯关系；若符合，则至少存在一个所述的红外线灯立体阵列；若不符合，则不存在。
[0075] 所述的子步骤S22中，所述的红外线灯轮廓提取采用圆形拟合算法对轮廓曲线进行描绘。
[0076] 所述的步骤S3中，所述的基于步骤S2存储的图像获取所述的机器人的绝对位置和绝对方向角的方法包括红外线灯立体阵列几何相似度比对及红外线灯数值化标记。
[0077] 所述的子步骤S32中，所述的数值化标记包括如下步骤：
[0078] S32.1，在基于所述的子步骤S31确定的红外线灯立体阵列区域内，采用直角坐标系确定每一个红外线灯轮廓中心的图像像素坐标，并通过图像像素坐标计算出任意红外线灯轮廓中心的图像像素距离；
[0079] S32.2，判断图像像素距离最长的两个红外线灯轮廓中心，坐标连线作为基准线L；
[0080] S32.3，根据所述的基准线L将红外线灯立体阵列分为两侧；如图4所示，找到最长线段，线段端点为A和B，设基准线L上的两圆形轮廓圆心的图像像素坐标为A(xa，ya)和B(xb,yb)，如图5所示，通过最长线段AB将其分为两部分，一部分两点为C和D，设定基准线L的一侧具有n个图像像素坐标分别为(x1，y1)，(x2，y2)，…，(xi，yi)，…，(xn，yn)的红外线灯轮廓，其中，i∈[1，n]，i、n为自然数；
[0081] S32.4，判断所述的圆形轮廓顺逆时针关系，采用如下计算式：
[0082] ki＝(xi-xa)×(yb-yi)-(yi-ya)×(xb-xi)；(公式一)
[0083] 设p为调整参数，当ki＞0-p时，红外线灯轮廓为顺时针关系，当ki＜0-p时，红外线灯轮廓为逆时针关系；
[0084] 顺时针关系或逆时针关系中分别以A(xa，ya)和B(xb,yb)作为起点和终点，取起点编号为序号1，终点编号为序号2；如图6所示，取C和D中任一点，判断其与A和B的顺逆时针关系，确定序号1和序号2。
[0085] S32.5，对(x1，y1)，(x2，y2)，…，(xi，yi)，…，(xn，yn)根据其与序号1的图像像素距离由近到远从序号3开始排序；如图7所示，判断C和D两点与序号1的图像像素距离关系，确定序号3和序号4。
[0086] S32.6，设定基准线L的另一侧具有m个图像像素坐标分别为(x1’，y1’)，(x2’，y2’)，…，(xj’，yj’)，…，(xm’，ym’)的红外线灯轮廓，j∈[1，m]，j、m为自然数；如图8所示，通过最长线段AB分为的两部分，另一部分三点为E、F和G。
[0087] S32.7，对(x1’，y1’)，(x2’，y2’)，…，(xj’，yj’)，…，(xm’，ym’)根据其与序号2的图像像素距离由近到远从序号n+3开始排序；如图9所示，判断E、F和G三点与灯2的距离关系，确定序号5、序号6和序号7。
[0088] S32.8，完成对所述的红外线灯轮廓的数值化标记。
[0089] 所述的子步骤S34中，基于子步骤S33确定的红外线灯立体阵列及红外线灯的绝对坐标计算红外线成像设备的绝对位置和绝对方向角，使用相机姿态估计算法对摄像头姿态进行估计得到旋转矢量rvec和位移矢量tvec；通过旋转矢量rvec计算出的旋转矩阵的逆矩阵rrmat_inv与位移矢量tvec计算出红外线成像设备的绝对位置坐标矩阵pmat，计算式为：
[0090] pmat＝-rrmat_inv·tvec；(公式二)
[0091] 在所述的子步骤S35中，基于子步骤S34确定的红外线成像设备的绝对位置和绝对方向角，通过几何变换的方法实现计算出机器人的绝对位置和绝对方向角。
[0092] 一种基于红外线灯立体阵列的机器人定位起始坐标和起始方向角的方法，一种基于红外线灯立体阵列的机器人定位方法，基于所述的步骤S3和步骤S4，对视觉定位确定的机器人绝对位置和绝对方向角与编码器和惯性传感器确定的机器人绝对位置和绝对方向角进行估计，并利用估计结果作为定位参考确定机器人当前的起始坐标和起始方向角。
[0093] 除此之外，作为优选，所述的子步骤S21中，所述的圆形拟合采用Blob Detection算法对轮廓曲线进行。
[0094] 除此之外，作为优选，所述的子步骤S34中，基于子步骤S33确定的红外线灯立体阵列及红外线灯的绝对坐标计算红外线成像设备的绝对位置和绝对方向角，使用SOLVEPNP_EPNP算法对摄像头姿态进行估计得到旋转矢量rvec和位移矢量tvec，使用Rodrigues算法将旋转矢量rvec转换为旋转矩阵rrmat_inv。通过旋转矩阵rrmat_inv计算出欧拉角矩阵reul，计算式为：
[0095]
[0096]
[0097] 其中ksin gular为奇异性参数，通过比较ksin gular与常量参数p的大小，决定欧拉角矩阵的计算方式：
[0098]
[0099] 通过欧拉角矩阵reul计算出四元数矩阵rquat，计算式为：
[0100]
[0101] 其中θpitch、θroll和θyaw分别为章动角、旋进角和自传角；
[0102] 通过旋转矩阵的逆矩阵rrmat_inv与位移矢量tvec计算出红外线成像设备的绝对位置坐标矩阵pmat，计算式为：
[0103] pmat＝-rrmat_inv·tvec；(公式二)
[0104] 作为优选，在所述的子步骤S35中，基于子步S34确定的红外线成像设备的绝对位置和绝对方向角，计算出机器人的绝对位置和绝对方向角。因为红外线成像设备不是安装在机器人中心处，所以红外线成像设备的绝对位置不等于机器人的绝对位置，但红外线成像设备的绝对方向角等于机器人的绝对方向角。红外线成像设备的绝对位置转换成机器人的绝对位置，采用如下公式计算：
[0105]
[0106]
[0107] 其中(xcamera，ycamera)为红外线成像设备绝对位置坐标，(xrobot，yrobot)为机器人绝对位置坐标。Δx为红外线成像设备中心与机器人中心的相对位置横坐标差，Δy为红外线成像设备中心与机器人中心的相对位置纵坐标差。θpitch为红外线成像设备欧拉角矩阵中的章动角。
[0108] 实施例一，作为具体实施方式，针对步骤S3进行展开说明，用以阐述和解释本发明的技术手段，但并不对本发明的保护范围产生限制。
[0109] 在基于子步骤S31确定的红外线灯阵队列区域内，设任意两个圆形轮廓圆心为(xk，yk)和(xk+1，yk+1)，他们之间的图像像素距离dk，dk可由下式计算：
[0110]
[0111] 将拥有最长距离的两点连线作为基准线L，此基准线L将红外线灯阵队列分为两部分。基准线L右上部分作为上半部分，基准线L左下部分作为下半部分，上半部分除基准线L上两圆形轮廓外存在n个圆形轮廓，下半部分除基准线L上两圆形轮廓外存在m个圆形轮廓；
[0112] 设基准线L上两圆形轮廓圆心的图像像素坐标为A(xa，ya)和B(xb，yb)。以基准线L上半部分n＝1，图像像素坐标为(xi，yi)为例，三圆形轮廓顺序可由顺、逆时针关系得到，计算式为：
[0113] ki＝(xi-xa)×(yb-yi)-(yi-ya)×(yb-xi)；(公式一)
[0114] ki＝0-p
[0115] 其中p为调整参数，用以降低误差，在理想状态下p＝0。三点顺逆时针关系可由ki与0-p的大小关系得到。以A或B中的一点为起点，另一点为终点，当k大于0-p时，三点为顺时针关系，圆形轮廓圆心A即为序号为1的红外线灯圆心，圆形轮廓圆心B即为序号为2的红外线灯圆心；当k小于0-p时，三点为逆时针关系，圆形轮廓圆心A即为2号红外线灯圆心，圆形轮廓圆心B即为1号红外线灯圆心。根据步骤S32.5，圆形轮廓圆心i即为3号红外线灯圆心；
[0116] 实施例二，作为另一种实施方式，设基准线L上两圆形轮廓圆心的图像像素坐标为A(xa，ya)和B(xb，yb)，以基准线L上半部分n＝2，将i取C或D，图像像素坐标为(xC，yC)和(xd，yd)为例。四圆形轮廓顺序可由顺逆时针关系及图像像素距离关系得到。取C或D任一点作为点E，原理与实施例一相同，计算式为：
[0117] k＝(xe-xa)×(yb-ye)-(ye-ya)×(xb-xe)
[0118] k＝0-p
[0119] 其中p为调整参数，多点顺逆时针关系可由k与0-p的大小关系得到。当k大于0-p时，多点为顺时针关系，圆形轮廓圆心A即为1号红外线灯圆心，圆形轮廓圆心B即为2号红外线灯圆心；当k小于0-p时，多点为逆时针关系，圆形轮廓圆心A即为2号红外线灯圆心，圆形轮廓圆心B即为1号红外线灯圆心。
[0120] 此时序号为1的红外线灯圆心图像像素坐标为(x1，y1)，序号为2的红外线灯圆心图像像素坐标为(x2，y2)。其余两个圆形轮廓顺序可由距离关系得到，计算式为：
[0121]
[0122]
[0123] 其中d1为圆形轮廓圆心C距离1号红外线灯圆心的图像像素距离，d2为圆形轮廓圆心D距离1号红外线灯圆心的图像像素距离。当d1小于d2时，圆形轮廓圆心C即序号为3的红外线灯圆心，圆形轮廓圆心D即序号为4的红外线灯圆心；当d1大于d2时，圆形轮廓圆心C即序号为4的红外线灯圆心，圆形轮廓圆心D即为序号为3的红外线灯圆心；
[0124] 同理，当n大于2或者基准线L下半部分的圆形轮廓顺序确定采用如上的相同方法；
[0125] 在子步骤S33中，基于子步骤S32确定的红外线灯顺序，确定当前红外线灯立体阵列编号。此编号用于读取
数据库中对应编号的红外线灯立体阵列的绝对坐标及该红外线灯阵队列中每一个红外线灯的绝对坐标。
[0126] 如图10所示，为本发明所采用的机器人。所述的机器人顶部设有红外线成像设备。
[0127] 本文中所描述的具体实施例仅仅是对本发明精神作举例说明。本发明所属技术领域的技术人员可以对所描述的具体实施例做各种各样的
修改或补充或采用类似的方式替代，但并不会偏离本发明的精神或者超越所附
权利要求书所定义的范围，这些改变也应视为本发明的保护范围。
