一种基于视觉定位的机器人自动回充方法及系统与流程

[0001]
本发明属于计算机视觉领域，具体涉及一种基于视觉定位的机器人自动回充方法及系统。


背景技术：

[0002]
自动回充技术是机器人自己找到充电桩然后回到充电桩进行充电的一种技术。在当前阶段，机器人充电确实比较麻烦经常需要外界帮助，对于人来说，需要时刻帮机器人充电，增加负担，而对于机器人来说，使用过程中突然没电，不仅影响了使用体验感，还会给人一种非智能的感觉，因此自动回冲技术进入了我们的视野。目前自动回冲技术有4种实现方法：
[0003]
(1)基于红外线的实现方法，该方法应用于目前70％的扫地机器人，其优点很明显定位精度高，技术比较成熟。其缺点也很明显，由于红外线无法穿透物体，所以扫地机器人很可能被灰尘碎屑等细小物体阻挡红外线。
[0004]
(2)基于超声波的实现方法，该方法主要是模仿蝙蝠通过发送声波以及反射回来的声波实现三角定位计算当前所在位置。该方法目前应用较少，成本偏高。
[0005]
(3)基于蓝牙技术的实现方法，基于蓝牙实现的定位系统，其不受视距影响，即使有阻碍物阻挡，也能实现直线距离内的定位。但蓝牙技术的实现需要提前在场景布置多个蓝牙基站。
[0006]
(4)基于摄像头的实现方法，是使用摄像头识别充电桩的位置，使用姿势估计来计算摄像头相对充电桩的位置和角度，最后进行路径导航到达其位置。
[0007]
其中，基于摄像头的实现方法是目前主流方向之一，而自动回充涉及路径的选择、规划以及避障。由于单目摄像头无法得到深度图像，因此感知周围环境会相对较弱，而其他摄像头(如双目、深度摄像头)虽然能很好感知环境，但成本较高。


技术实现要素：

[0008]
本发明的目的在于提供一种基于视觉定位的机器人自动回充方法及系统，用以解决现有技术中的单目摄像头低成本但效果不好等问题。
[0009]
为了实现上述任务，本发明采用以下技术方案：
[0010]
一种基于视觉定位的机器人自动回充方法，用于通过实时采集机器人视野图片，当任意时刻的视野图片包含的目标不为空时进行判断：
[0011]
若机器人和目标之间的距离大于第一阈值且小于等于第二阈值，则执行远距策略；
[0012]
若机器人和目标之间的距离小于等于第一阈值，则执行中近距策略；
[0013]
所述的远距策略包括如下子步骤：
[0014]
步骤a1：根据该任意时刻的视野图片获取目标数据和障碍物数据，若目标数据不为空且障碍物数据为空，则转动角度使摄像头对准目标中心并前进一个步长，获取当前视
野图片并执行步骤a2；
[0015]
若目标数据为空且障碍物数据为空，则原地旋转360
°
并寻找目标，旋转期间找到目标则停止旋转，获取当前视野图片并执行步骤a2；原地旋转360
°
后仍没有找到目标，则执行固定次数的随机动作，执行完后获取当前视野图片并执行步骤a2；
[0016]
若目标数据不为空且障碍物数据不为空，则获取前进至与障碍物平行，获取当前视野图片并执行步骤a2；
[0017]
若目标数据为空且障碍物数据不为空，则离开障碍物范围，获取当前视野图片并执行步骤a2；
[0018]
步骤a2：若当前视野图片中机器人和目标之间的距离小于等于第一阈值，则执行中距策略，否则令当前视野图片为该任意时刻的视野图片返回步骤a1；
[0019]
所述的中近距策略包括如下子步骤：
[0020]
步骤b1：根据该任意时刻的视野图片获取目标数据，根据该任意时刻的目标数据获得机器人在目标坐标系中的方向角θ，若θ＝0
°
，则执行步骤b3，否则，执行步骤b2；
[0021]
步骤b2：根据该任意时刻的目标数据，获得机器人朝向目标中心的偏移角α，根据偏移角α调整位置，直至θ＝0
°
时获得当前时刻的视野图片，则执行步骤b3；
[0022]
步骤b3：计算当前机器人和目标之间的距离并前进，进行充电。
[0023]
进一步的，步骤a1中离开障碍物范围的动作是指：后退三个步长，并左转100
±
10
°
，然后再前进三个步长。
[0024]
进一步的，步长为0.03～0.04m。
[0025]
进一步的，所述第一阈值为50
±
10cm，所述第二阈值为400
±
25cm。
[0026]
进一步的，所述固定次数为30～50次，所述随机动作包括旋转动作和直走动作。
[0027]
一种基于视觉定位的机器人自动回充系统，包括视野采集模块、距离判断模块、远距策略模块和中近距策略模块；
[0028]
所述视野采集模块用于实时采集机器人视野图片，当任意时刻的视野图片包含的目标不为空时执行距离判断模块；
[0029]
所述距离判断模块用于判决机器人距离目标属于哪个距离区间，包括两个判决条件：若机器人和目标之间的距离大于第一阈值且小于等于第二阈值，则属于远距，执行远距策略模块；若机器人和目标之间的距离小于等于第一阈值，则属于中近距，执行中近距策略模块；
[0030]
所述的远距策略模块用于根据该任意时刻的视野图片获取目标数据和障碍物数据，若目标数据不为空且障碍物数据为空，则转动角度使摄像头对准目标中心并前进一个步长，获取当前视野图片；若目标数据为空且障碍物数据为空，则原地旋转360
°
并寻找目标，旋转期间找到目标则停止旋转，获取当前视野图片；原地旋转360
°
后仍没有找到目标，则执行固定次数的随机动作，执行完后获取当前视野图片；若目标数据不为空且障碍物数据不为空，则获取前进至与障碍物平行，获取当前视野图片；若目标数据为空且障碍物数据不为空，则离开障碍物范围，获取当前视野图片；
[0031]
所述的远距策略模块还用于持续判断当前视野图片中机器人和目标之间的距离小于等于第一阈值，则执行中距策略，否则令当前视野图片为该任意时刻的视野图片重新执行远距策略模块；
[0032]
所述的中近距策略模块用于根据该任意时刻的视野图片获取目标数据，根据该任意时刻的目标数据获得机器人在目标坐标系中的方向角θ和机器人朝向目标中心的偏移角α，根据偏移角α调整位置，直至θ＝0
°
时获得当前时刻的视野图片，计算当前机器人和目标之间的距离并前进，进行充电。
[0033]
进一步的，远距策略模块中离开障碍物范围的动作是指：后退三个步长，并左转100
±
10
°
，然后再前进三个步长。
[0034]
进一步的，步长为0.03～0.04m。
[0035]
进一步的，所述第一阈值为50
±
10cm，所述第二阈值为400
±
25cm。
[0036]
进一步的，所述固定次数为30～50次，所述随机动作包括旋转动作和直走动作。
[0037]
本发明与现有技术相比具有以下技术特点：
[0038]
(1)本发明借助aruco进行姿态估计，从而建立3维空间坐标系。然后通过aruco在屏幕中的位置来不断调整摄像头的位置从而找到充电桩位置，完成这一功能只需要一个单目摄像头，成本低廉。
[0039]
(2)根据本发明找充电桩的步骤来逐步逼近充电桩，到达充电桩。相较于现有的回冲算法成本低廉，实现难度大大降低，产品迭代周期较短。
[0040]
(3)由于现有的单目摄像头实现回充技术对环境感知较弱，所以效果不太好，因此本发明在算法策略上进行发明优化，在低成本的单目摄像头下，利用机器人在不同距离下的不同状态采取不同动作算法的策略，实现了低成本高效的机器人回充技术。
附图说明
[0041]
图1是本发明方法的流程图；
[0042]
图2是远距策略下的机器人与目标位置关系示意图；
[0043]
图3是远距策略下机器人未丢失目标有障碍状态示意图；
[0044]
图4是中距策略下机器人未在aruco码得正前方状态示意图；
[0045]
图5是近距策略下机器人充电示意图；
[0046]
图6是远距策略下机器人在当前的不同状态下执行不同的行为策略示意图；
[0047]
图7是中距策略状态转移图；
[0048]
图8是中距策略行为流程图。
具体实施方式
[0049]
首先给出本发明出现的技术词语的含义：
[0050]
wandering状态：随机做固定次数的随机动作。在本发明中，wandering状态有0.6的概率进入旋转动作，0.4的概率直走一步，旋转动作包括左旋和右旋，有0.45的概率进行右旋转一步，有0.55的概率进行左旋转一步。每个动作前获得一个0-1随机数，根据随机数进入不同的动作，每做一个动作，动作计数加一，如果动作累计到固定次数，则退出wandering状态。
[0051]
aruco码：aruco码是一个周围是黑色边框，内部有确定该标记id的二维矩阵组合而成的类二维码。其黑色边框加速标记图像中检测速度，内部二维编码能唯一标定识别标识。在本发明中主要利用其黑色边框的快速检测速度(相比于二维码而言)来检测其四角位
置，来判断其中心位置，进而判断其相对于摄像头的位置状况。
[0052]
实施例1
[0053]
本实施例公开了一种基于视觉定位的机器人自动回充方法，用于通过实时采集机器人视野图片，当任意时刻的视野图片包含的目标不为空时进行判断：
[0054]
若机器人和目标之间的距离大于第一阈值且小于等于第二阈值，则执行远距策略；
[0055]
若机器人和目标之间的距离小于等于第一阈值，则执行中近距策略；
[0056]
如图6所示，所述的远距策略包括如下子步骤：
[0057]
步骤a1：根据该任意时刻的视野图片获取目标数据和障碍物数据，若目标数据不为空且障碍物数据为空，则转动角度使摄像头对准目标中心并前进一个步长，获取当前视野图片并执行步骤a2；
[0058]
在远距策略下定义丢失目标无障碍状态、未丢失目标无障碍状态、丢失目标有障碍状态和未丢失目标有障碍状态这四种状态；
[0059]
若目标数据为空且障碍物数据为空，则原地旋转360
°
并寻找目标，旋转期间找到目标则停止旋转，获取当前视野图片并执行步骤a2；原地旋转360
°
后仍没有找到目标，则执行固定次数的随机动作，执行完后获取当前视野图片并执行步骤a2；
[0060]
若目标数据不为空且障碍物数据不为空，则获取前进至与障碍物平行，获取当前视野图片并执行步骤a2；
[0061]
若目标数据为空且障碍物数据不为空，则离开障碍物范围，获取当前视野图片并执行步骤a2；
[0062]
步骤a2：若当前视野图片中机器人和目标之间的距离小于等于第一阈值，则执行中距策略，否则令当前视野图片为该任意时刻的视野图片返回步骤a1；
[0063]
所述的中近距策略包括如下子步骤：
[0064]
步骤b1：根据该任意时刻的视野图片获取目标数据，根据该任意时刻的目标数据获得机器人在目标坐标系中的方向角θ，范围为(-90
°
,90
°
)，若θ＝0
°
，则执行步骤b3，否则，执行步骤b2；
[0065]
步骤b2：根据该任意时刻的目标数据，获得机器人朝向目标中心的偏移角α，范围为(-90
°
,90
°
)，根据偏移角α调整位置，直至θ＝0
°
时获得当前时刻的视野图片，则执行步骤b3；
[0066]
步骤b3：计算当前机器人和目标之间的距离并前进，进行充电。
[0067]
具体的，目标数据是指aruco码数据，摄像头检测到aruco码后可计算出直线距离，因此定义机器人和目标之间的距离为机器人的摄像头到aruco码中心的直线距离，aruco码设置在充电桩正后方。
[0068]
具体的，步骤a1中离开障碍物范围的动作是指：后退三个步长，并左转100
±
10
°
，然后再前进三个步长。
[0069]
具体的，步长为0.03～0.04m。
[0070]
具体的，所述第一阈值为50
±
10cm，所述第二阈值为400
±
25cm。
[0071]
具体的，所述固定次数为30～50次，所述随机动作包括旋转动作和直走动作，优选的固定次数为30次。
[0072]
实施例2
[0073]
本实施例公开了一种基于视觉定位的机器人自动回充系统，包括视野采集模块、距离判断模块、远距策略模块和中近距策略模块；
[0074]
所述视野采集模块用于实时采集机器人视野图片，当任意时刻的视野图片包含的目标不为空时执行距离判断模块；
[0075]
所述距离判断模块用于判决机器人距离目标属于哪个距离区间，包括两个判决条件：若机器人和目标之间的距离大于第一阈值且小于等于第二阈值，则属于远距，执行远距策略模块；若机器人和目标之间的距离小于等于第一阈值，则属于中近距，执行中近距策略模块；
[0076]
所述的远距策略模块用于根据该任意时刻的视野图片获取目标数据和障碍物数据，若目标数据不为空且障碍物数据为空，则转动角度使摄像头对准目标中心并前进一个步长，获取当前视野图片；若目标数据为空且障碍物数据为空，则原地旋转360
°
并寻找目标，旋转期间找到目标则停止旋转，获取当前视野图片；原地旋转360
°
后仍没有找到目标，则执行固定次数的随机动作，执行完后获取当前视野图片；若目标数据不为空且障碍物数据不为空，则获取前进至与障碍物平行，获取当前视野图片；若目标数据为空且障碍物数据不为空，则离开障碍物范围，获取当前视野图片；
[0077]
所述的远距策略模块还用于持续判断当前视野图片中机器人和目标之间的距离小于等于第一阈值，则执行中距策略，否则令当前视野图片为该任意时刻的视野图片重新执行远距策略模块；
[0078]
所述的中近距策略模块用于根据该任意时刻的视野图片获取目标数据，根据该任意时刻的目标数据获得机器人在目标坐标系中的方向角θ和机器人朝向目标中心的偏移角α，根据偏移角α调整位置，直至θ＝0
°
时获得当前时刻的视野图片，计算当前机器人和目标之间的距离并前进，进行充电。
[0079]
具体的，远距策略模块中离开障碍物范围的动作是指：后退三个步长，并左转100
±
10
°
，然后再前进三个步长。
[0080]
具体的，步长为0.03～0.04m。
[0081]
具体的，所述第一阈值为50
±
10cm，所述第二阈值为400
±
25cm。
[0082]
具体的，所述固定次数为30～50次，所述随机动作包括旋转动作和直走动作。
[0083]
具体的，机器人在摄像头的垂直下方2cm处设置tof距离传感器，所述tof距离传感器与地面形成120
°
角度，用于获取前方的障碍物数据。
[0084]
具体的，tof距离传感器检测到的tof距离<300mm时，则代表正前方有障碍物，需要离开障碍物范围。障碍物一般指门或墙。
[0085]
具体的，所述中近距策略包括中距策略和近距策略，如图4所示，中距策略是在距aruco码50
±
10cm的距离，由于是远距策略执行完后达至，所以在中距策略的开始，机器人是朝向目标，但是停止的位置可能会不同，因此定义在中距策略下定义未正对aruco码状态和正对aruco码状态这两种状态，若当机器人正对aruco码状态时则结束中距策略进入近距策略，近距策略为走到充电桩的正上方并且完成充电动作，由于是中距策略执行完后达至，所以在近距策略的开始，是正向面对着aruco码，并距离aruco码仅50
±
10cm的距离，所以近距策略定义未充电状态、尝试充电状态和充上电状态。
[0086]
具体的，步骤b1包括如下子步骤：
[0087]
检测当前aruco码获得到的姿态数据，θ角度(机器人在aruco码坐标系中的方向角,aruco码正前方为θ＝0
°
，范围为(-90
°
,90
°
))，根据当前数据判断机器人是否在aruco码得正前方，若未在正前方进入步骤b2，若在正前方则结束中距策略进入近距策略，进入步骤b3。
[0088]
具体的，步骤b2包括如下子步骤：
[0089]
计算出α角度(摄像头朝向aruco码中心的偏移角度，例如摄像头需左转30
°
才能对准aruco码的正中心，则α＝30
°
，范围为(-90
°
,90
°
))根据偏移角α调整位置，如果α的绝对值大于等于20，则直接往左(右)旋转一步(旋转度数为α*0.4)，其中根据alpha的正(负)分别对应左(右)；若α的绝对值小于20情况下θ的绝对值大于等于10，则往左(右)平移一步；若上述两条件都不满足，则：
[0090]
if abs(α)>＝8:旋转一步且旋转度数为α*0.5；
[0091]
else if sgn(θ)＝＝sgn(α):往左(右)平移一步；
[0092]
else if sgn(θ)！＝sgn(α):旋转一步(旋转度数为α*0.4)；
[0093]
直至θ＝0
°
时获得当前时刻的视野图片，则执行步骤b3。
[0094]
具体的，对于不同近距状态下包括以下步骤：
[0095]
步骤c1：根据当前aruco码获得的距离，计算出机器人走至充电桩的正上方应该走多远，并执行步骤c2。
[0096]
步骤c2：当前为尝试充电状态，计机器人的充电动作，当完成动作后，检测是否充电，若充上电则整个自动回充完成，若未充上电，则复位动作，并进入尝试充电状态，直到进入成功充电状态。