CN112183133B - 一种基于ArUco码引导的移动机器人自主充电方法 - Google Patents

Abstract

本发明涉及一种基于ArUco码引导的移动机器人自主充电方法，包括步骤：对机器人设置充电触点以及单目相机，并在其运行环境中布局设有ArUco码的充电桩；建立环境地图，标记充电桩的位置及朝向；设置导航目标点、目标姿态，规划移动机器人的运动轨迹；识别充电桩上的ArUco码，获取其在图像上的角点坐标；将单目相机获取的机器人位姿与机器人自身提供的线速度、角速度相结合，获取精确移动机器人位姿；采用PID方法控制机器人前往充电桩进行对接，进行自主充电。与现有技术相比，本发明使用ArUco码引导稳定，在有光的前提下，能够达到100％的充电成功率；引导流畅，能够使机器人快速、平滑地对接充电桩。

Description

一种基于ArUco码引导的移动机器人自主充电方法

技术领域

本发明涉及移动机器人技术领域，尤其是涉及一种基于ArUco码引导的移动机器人自主充电方法。

背景技术

自主移动机器人是一种使用激光、摄像头等导引装置，能够自主按照任意导引路线行驶的机器人。由于其具有自主移动的特点，也可以搭配平台、箱体、屏幕等上层建筑，移动机器人在工业、物流业、商业等领域中能够节省人力使用、降低运营成本，具备广泛的应用平台和前景。自主移动机器人一般是由车载的蓄电池进行供电，由于电池容量有限，通常在运行几个小时后就需要补充电量，因此，自主充电是其需要实现的重要功能之一，当电量较低或需要充电时，机器人需要自主的返回对接充电桩进行充电，从而具备自主的持续作业能力。对自主充电方法的研究成为了近年来的重要课题。

目前常用的自主充电方式，是使用红外传感器技术进行移动与对接，目前公开的移动机器人自主充电方法也都是使用了红外技术。该项技术中，移动机器人通过接收充电桩发出的红外信号确定相对位置，进而根据引导来向充电桩运动。由于使用多个红外传感器检测信号强度能够方便得到位置信息，因此这种自主充电方式受到了广泛的青睐。然而，使用红外技术具有以下的不足：1、红外信号具有传输距离较小的固有性质，必须距离充电桩较近时才能接收到；2、红外信号发射时，角度较大的范围发射强度很小，需要使用高精度的红外信号检测电路才能够检测；3、容易受到其他红外信号的干扰。最近，一些基于红外与摄像头融合引导的方法被提出，一定程度改善了单纯使用红外技术的范围小等问题，但使用多传感器较为复杂和冗余。

视觉定位引导的方法逐渐开始在移动机器人自主充电中占有一席之地，但目前尚未有基于纯视觉的移动机器人自主充电方法公开。卡内基梅隆大学开发出的Sage机器人，通过使用CCD识别三维路标的方式，引导机器人返回充电站。此外，还有方法通过识别特别制作的色块或图标，对移动机器人进行定位，但并没有应用在自主充电领域。最近有研究提出使用SIFT等特征点法直接识别充电桩，这种方法虽然取得了一定的效果，但消耗了大量计算资源，且具有一定的误识别率。因此，目前基于视觉引导的移动机器人自主充电方法，无论引导精度还是速度都不能满足实际应用的需求。

发明内容

本发明的目的就是为了克服上述现有技术存在的缺陷而提供一种基于ArUco码引导的移动机器人自主充电方法，该方法能够实现远距离引导回充，且引导流畅，能够使移动机器人快速、准确、平滑地对接充电桩实现稳定的自主充电过程。

本发明的目的可以通过以下技术方案来实现：

一种基于ArUco码引导的移动机器人自主充电方法，该方法包括如下步骤：

S1：对移动机器人正后方设置充电触点以及用以捕获移动机器人正后方实时图像的单目相机，并在移动机器人运行环境中的布局多个设有ArUco码的充电桩，各充电桩的充电槽与所述充电触点处于同一高度上，每一个ArUco码设有各自对应的ID。

S2：建立移动机器人活动的环境地图，在环境地图中标记出各充电桩的位置及朝向。

S3：设置导航目标点以及充电触点和单目相机的镜头的目标姿态，规划移动机器人的运动轨迹，直到移动机器人到达目标点，并达到目标姿态。到达目标姿态后，若相机未识别到ArUco码，则通过旋转的方式寻找ArUco码。具体地：

将充电桩前方1m的位置设为导航目标点，将充电触点和单目相机的镜头正对充电桩设为目标姿态，规划移动机器人的运动轨迹，对移动机器人进行带有避障功能的导航，当移动机器人到达目标点位置后，控制移动机器人进行原地旋转，直至达到既定的目标姿态。到达目标姿态后，若单目相机未识别出ArUco码，则控制移动机器人以先逆时针，后顺时针的方式旋转，直至寻找到ArUco码。

S4：利用单目相机识别充电桩上的ArUco码，获取ArUco码在图像上的角点坐标，并通过角点间关系解算与坐标系变换，得到视觉信息提供的移动机器人与充电桩坐标系之间的位姿变换关系。

本步骤具体操作过程包括：

4.1、调用ArUco官方库中的检测函数，通过自适应阈值颜色分割以及透视变换方法识别单目相机捕获的图像中的ArUco码，进而获取角点的坐标：

调用ArUco官方库中的检测函数，检测函数首先将单目相机捕获的彩色图像转换为灰度图像，移除灰色像素，保留黑色、白色像素，获取黑白二值化图像，从黑白二值化图像中寻找所有由黑色边界包围的四边形，对四边形区域进行透视变换，获取规范形态，将规范形态的四边形区域根据大小和边界进行分格处理，通过每个格子的黑白颜色判断该区域是否为一个ArUco码，若是，则计算出其对应的字典ID，若该ID与预先固定在充电桩上的ArUco码ID相同，则表示相机捕获到了充电桩上的ArUco码，进而获取该ArUco码对应的角点在图像上的坐标。

4.2、根据实际使用的ArUco码获取ArUco码边长，获取标定的单目相机参数，包括单目相机的内参数矩阵和单目相机的畸变矩阵，利用上述参数结合上一步获取的角点坐标，计算ArUco码坐标系相对于单目相机坐标系的旋转矩阵，并基于旋转矩阵计算ArUco码坐标系相对于单目相机坐标系的变换矩阵。

4.3、根据单目相机、移动机器人坐标系间固定的变换关系，通过坐标系变换，获取移动机器人初始位姿。具体地：

固定一个世界坐标系，将该世界坐标系的原点设置为距离充电桩正前方＝移动机器人半径的点，获取ArUco码坐标系相对于世界坐标系的变换矩阵

其表达式为：

式中，r为移动机器人的半径；

同时，获取移动机器人坐标系相对于单目相机坐标系的变换矩阵

其表达式为：

式中，

为单目相机固定位置到移动机器人中心的距离；

通过

以及步骤4.2得到的ArUco码坐标系相对于单目相机坐标系的变换矩阵，计算移动机器人坐标系相对于世界坐标系的变换矩阵

其表达式为：

式中，

为ArUco码坐标系相对于单目相机坐标系的变换矩阵；

基于移动机器人坐标系相对于世界坐标系的变换矩阵，使用移动机器人在世界坐标系下的2D位姿表示移动机器人的定位，定位表达式为：

x＝p_w1

y＝p_w2

式中，p_w为世界坐标系下移动机器人的位置，p_w*为p_w对应行列处的元素值，

为移动机器人坐标系相对于世界坐标系的变换矩阵

对应行列处的元素值，(x、y、θ)为移动机器人的2D位姿。

S5：将单目相机获取的移动机器人位姿与移动机器人自身提供的移动机器人线速度、角速度相结合，获取精确移动机器人位姿。

进一步地，采用扩展卡尔曼滤波法，将单目相机提供的移动机器人位姿，与移动机器人自身轮式里程计提供的移动机器人线速度、角速度相结合，获取精确移动机器人位姿。

本发明采用的扩展卡尔曼滤波方法中，状态向量估计值

的表达式为：

其中，

是k时刻的移动机器人位姿估计值，

是t时刻的移动机器人速度估计值，Δt为采样间隔，k-1表示上一时刻。

状态转移矩阵F_k为上式右边矩阵的雅可比矩阵，可以求得表达式为：

由于观测得到的测量向量Y_k与X_k为一一对应关系，因此测量转换矩阵H是一个单位矩阵，表达式为：

扩展卡尔曼滤波的使用还需要根据实际情况提供演化噪声方差Q_k、测量噪声方差R_k以及初始的协方差矩阵P₀。之后，扩展卡尔曼滤波便可以使用公式，结合实时采集的测量向量和滤波参数，对卡尔曼增益K_k、协方差矩阵P_k以及融合估计的状态向量X_k进行计算，状态向量X_k中的前三个数值便对应精确估计的移动机器人的实时位姿(x_k、y_k、θ_k)，计算表达式为：

在按照上述步骤S4、S5的引导过程中，若发生单目相机漏识别ArUco码的偶发情况，则在发生偶发情况的时刻调整演化噪声方差Q_k以及测量噪声方差R_k的值，从而完全相信移动机器人自身轮式里程计提供的移动机器人线速度、角速度，使用扩展卡尔曼滤波算法计算机器人位姿。

S6：依据精确移动机器人位姿规划移动机器人每一时刻的期望朝向，以期望朝向与实际朝向之间的差值作为输入误差，采用PID方法控制机器人前往充电桩进行对接，进行移动机器人的自主充电。具体步骤包括：

6.1、选择移动机器人的朝向θ作为被控量，每个t时刻以期望朝向θ_r，t与实际朝向θ_u，t之间的差值作为输入误差θ_e，t，利用PID方法控制移动机器人的角速度w_t；

输入误差θ_e，t为期望朝向θ_r，t与实际朝向θ_u，t之间的差值，其表达式为：

θ_e，t＝θ_r，t-θ_u，t

利用PID方法控制移动机器人的角速度的控制规律为：

6.2、使用步骤6.1输出的移动机器人的角速度计算移动机器人的输出线速度，根据角速度、线速度控制移动机器人沿平滑轨迹快速前往充电桩与之对接；

6.3、当检测到某时刻移动机器人位姿在x方向的数值x_t小于0，则判断移动机器人已与充电桩实现对接，发布刹车信号制动，使移动机器人的线速度、角速度为零，进而使其始终保持对接状态，实现自主充电。

本发明提供的基于ArUco码引导的移动机器人自主充电方法，相较于现有技术至少包括如下有益效果：

1)本发明基于单目相机和ArUco码对移动机器人进行引导，相比于传统的红外技术对近距离的充电桩的限制要求，本发明能够实现远距离引导回充，相比于识别图表或色块的方式，本发明利用相机识别ArUco码可以得到更丰富的视觉信息，为移动机器人提供其在世界坐标系下的更加精准的位姿，识别更加稳定，定位精度高，且只会占用很少的计算资源，从而可以对图像实时处理、对移动机器人实时控制，引导移动机器人快速、准确、平滑地对接充电桩，对接成功率可以达到100％；

2)本发明在自主充电引导的第一阶段使用激光为主传感器，能够在地图上的任意位置导航到充电桩前方1米左右的位置，实现了机器人在任意位置的自主回充，满足实际应用的要求；

3)本发明结合了单目相机的视觉信息和轮式里程计的速度信息进行定位，避免了误差的累积，能够得到更加精确的机器人位姿，两种位姿信息结合的方式使移动机器人在其中一种信息丢失的极端状况下仍然可以继续被引导，大大增加了自主充电过程的稳定性；

4)本发明融合了ArUco码与轮式里程计的定位信息，提高了定位精度，实现了对充电桩的实时精确定位，且能够保证在看不到ArUco码的偶发情况下，移动机器人仍然可以继续定位和导航；

5)本发明使用PID运动控制方法，可保证机器人以一条平滑的路线对接充电桩，并可以尽量正对与充电桩对接，避免了路线的震荡，减少了自主充电过程的对接时间。

附图说明

图1为实施例中移动机器人上相机、触点安装位置示意图；

图2为实施例中设有ArUco码的充电桩的正视结构示意图；

图3为实施例中基于ArUco码引导的移动机器人自主充电方法的流程示意图；

图4为实施例中移动机器人使用激光导航前往充电桩前方目标点的示意图；

图5为实施例中ArUco码识别定位的流程图；

图6为实施例中使用扩展卡尔曼滤波估计移动机器人位姿流程图；

图7为实施例中移动机器人自主充电运动控制流程图；

图8为实施例中移动机器人角速度PID控制框图；

图9为实施例中移动机器人的一条对接轨迹示意图。

具体实施方式

下面结合附图和具体实施例对本发明进行详细说明。显然，所描述的实施例是本发明的一部分实施例，而不是全部实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动的前提下所获得的所有其他实施例，都应属于本发明保护的范围。

实施例

本发明涉及一种基于ArUco码引导的移动机器人自主充电方法，该方法主要原理在于：当移动机器人检测到电量低于一定阈值、或接收到人为发出的充电指令后，移动机器人将自主返回对接充电桩，进行自主充电。在使用本方法进行自主充电之前，移动机器人以及环境场景应当已经完成了以下两个准备阶段的工作：

1、在移动机器人的正后方底部位置，安装一个充电触点，该触点与机载电池、移动机器人控制箱相连，在触点触碰到充电桩的充电槽后，即可同时给机器人供电、给机载电池充电。在充电触点的正上方，安装一个单目相机，该相机与控制箱通过USB接口连接，提供正后方的实时图像。移动机器人外观以及相机、触点安装位置如图1所示。在移动机器人运行环境中的任意合适位置，放置一个固定有ArUco码的充电桩，充电桩的充电槽应与移动机器人的充电触点处于同一高度上。固定有ArUco码的充电桩的外观如图2所示。

2、建立一张2D环境地图，并标出充电桩在地图上的位置与朝向。

在完成了准备阶段工作的基础上，如图3所示，本发明一种基于ArUco码引导的移动机器人自主充电方法包括以下四个步骤：

步骤一、以激光为主传感器，规划并导航到地图上充电桩前方1m左右的位置，并使单目相机大致朝向充电桩。到达目标位置和姿态后，若单目相机视野中没有ArUco码，则移动机器人通过原地旋转的方式寻找ArUco码。

该步骤可以具体分为以下两个部分：

(1)本方法能够使移动机器人在环境中的任意位置返回充电桩进行自主充电，在绝大多数情况下，移动机器人不会距离充电桩很近从而直接看到固定的ArUco码。因此，本方法首先以激光为主传感器，将充电桩前方1m的位置设置为导航目标点，将充电触点和摄像头正对充电桩设置为目标姿态，进行带有避障功能的导航。本实施例中，移动机器人使用Dijkstra算法进行全局路径规划，使用模糊PID运动控制保障移动机器人沿规划的路线行驶，使用激光以及超声波传感器检测周围障碍物，通过软件状态机在必要的时候进行避障操作。到达目标点位置后，机器人再进行原地的旋转，到达既定的目标姿态。机器人前往目标点的过程可以在地图上实时观察，如图4所示。

(2)第(1)部分的激光导航能够将机器人引导到充电桩前方1m左右的位置，位置误差能够在5cm以内，但偶然会存在旋转不到位的情况，这会导致此时机器人视野中没有ArUco码。因此在本实施例中，若完成第(1)部分后没有看到ArUco码，则机器人将采用先逆时针旋转、再顺时针旋转的方式，寻找ArUco码，此时的机器人线速度v＝0m/s，角速度的表达式为：

其中，t为第(2)部分开始到现在的时间。

一旦在某一时刻，移动机器人在图像中识别到了ArUco码，将进行步骤二。

步骤二、使用单目相机识别固定在充电桩上的ArUco码，得到ArUco码在图像上的角点坐标，并通过角点间关系解算与坐标系变换，得到视觉信息提供的机器人与充电桩坐标系之间的位姿变换关系。

本实施例中，如图5所示，该步骤可以具体细分为以下三个部分：

(1)调用ArUco官方库中的检测函数，通过自适应阈值颜色分割以及透视变换方法识别图中的ArUco码，得到角点的坐标。具体地：

检测函数首先将相机采集到的彩色图像转换为灰度图像，再使用Otsu算法移除灰色的像素，只留下黑色、白色像素，得到黑白二值化的图像，然后从中找出所有由黑色边界包围的四边形，接下来，对四边形区域进行透视变换，来得到规范的形态，这一区域根据大小和边界分为了8*8的格子，通过每个格子的黑白颜色来得到该区域是否是一个ArUco码，若是一个ArUco码，再计算出它的字典ID。

本实施例中，使用的ArUco码的字典ID号为2，若单目相机捕获到的图像中找到了字典ID号为2的ArUco码，那么就说明它就是固定在充电桩上的ArUco码，进而获取它的角点在图像上的坐标。

(2)调用ArUco官方库中的位姿计算函数，通过角点坐标的关系解算，结合标定的相机参数，算出相机与ArUco码坐标系间的坐标变换。具体地：

计算位姿需要知道4个输入量：相机的内参数矩阵mK、畸变矩阵mDistCoef、ArUco码实际边长l和角点坐标corners。在安装相机之前，首先使用张氏标定法对相机进行参数标定，得到相机的内参数矩阵mK和相机的畸变矩阵mDistCoef，边长l由实际使用的ArUco码决定，在本实施例中，l＝0.063m，角点坐标corners则已经在第(1)部分中得到。

输入上述4个变量，位姿计算函数会返回ArUco码的旋转向量rvecs和平移向量tvecs，根据上述的向量，计算ArUco码坐标系相对于相机坐标系的旋转矩阵

表达式为：

其中，α和n分别为旋转向量的旋转角和旋转轴，满足rvecs＝αn，符号^是向量到反对称的转换符。

最后计算得到ArUco码坐标系相对于相机坐标系的变换矩阵

表达式为：

(3)根据相机、机器人坐标系间固定的变换关系，通过坐标系变换，得到机器人位姿。具体地：

人为固定一个世界坐标系，该坐标系的原点为充电桩正前方r距离的点，坐标系朝向与ArUco码坐标系一致，其中r为机器人半径，也就是说，为了最准确的对接充电桩，机器人需要把世界坐标系原点设为目标点。ArUco码坐标系相对于世界坐标系的变换矩阵

是固定的，表达式为：

同样，机器人坐标系相对于相机坐标系的变换矩阵

也是固定的，表达式为：

其中，

为相机固定位置到机器人中心的距离。

通过

以及第(2)部分中得到的

这三个变换矩阵，可以计算出机器人坐标系相对于世界坐标系的变换矩阵

表达式为：

为了直观的表示机器人的位姿，同时移动机器人只受控于2D平面，因此本发明方法使用机器人在世界坐标系下的2D位姿来最终表示机器人的定位，2D位姿可以通过(x、y、θ)三个值来表示，根据

可以求得机器人位姿，表达式为：

x＝p_w1

y＝p_w2

其中，p_w为世界坐标系下机器人的位置，p_w*、

都表示矩阵对应行列处的元素值。

综合以上三个部分的内容，在步骤二中，单目相机的视觉信息为机器人提供了其在世界坐标系下的位姿(x、y、θ)，并且该位姿随着图像的实时采集而更新，在本实施例中，更新频率可以达到20Hz，充分满足机器人自身定位实时性的要求。

步骤三、使用扩展卡尔曼滤波方法，将相机提供的机器人位姿，与轮式里程计提供的机器人线速度、角速度相结合，得到更加精确的机器人位姿。

在步骤二中，相机的视觉信息为机器人提供了其在世界坐标系下的位姿(x、y、θ)，通过相机可以直接观测得到机器人位姿，因此相机可以提供位姿的观测值。由于不可避免的机器人晃动等因素，相机采集到的图像一般会伴随一定程度的运动模糊，从而影响步骤二中的识别准确率，对位姿估计产生误差，该误差可以近似认为服从高斯分布。

一般的移动机器人都配备有轮式里程计，如编码器或霍尔传感器等，轮式里程计通过计算一小段时间内轮子的转动幅度，来得到该段时间内机器人的平均移动速度，并将它近似为该时刻的瞬时速度，从而实时提供机器人的线速度和角速度(v、ω)，而根据上一时刻机器人的位姿和机器人当前时刻的速度，可以估算出当前时刻机器人的位姿，因此轮式里程计也可以提供位姿的观测值。由于轮式里程计固有的工作机理，其得到的机器人位姿也存在一定的误差，该误差也可以近似认为服从高斯分布。

因此，对于视觉信息和里程计信息的融合问题，本发明将其转化成了一个典型的扩展卡尔曼滤波问题，对视觉和里程计分别得到的位姿进行了融合滤波，流程如图6所示。通过扩展卡尔曼滤波，得到的机器人位姿更加精确，且不会有误差的累计问题，确保误差始终在一个很小的范围内。

本发明采用的扩展卡尔曼滤波方法中，状态向量估计值

的表达式为：

其中，

是k时刻的机器人位姿估计值，

是t时刻的机器人速度估计值，Δt为采样间隔，k-1表示上一时刻。

状态转移矩阵F_k为上式右边矩阵的雅可比矩阵，可以求得表达式为：

由于观测得到的测量向量Y_k与X_k为一一对应关系，因此测量转换矩阵H是一个单位矩阵，表达式为：

进一步，扩展卡尔曼滤波的使用还需要提供演化噪声方差Q_k、测量噪声方差R_k以及初始的协方差矩阵P₀。在本实施例中，在相机能够看见并识别到ArUco码的一般情况下，三者的表达式为：

之后，扩展卡尔曼滤波便可以使用公式，结合实时采集的测量向量和滤波参数，对卡尔曼增益K_k、协方差矩阵P_k以及融合估计的状态向量X_k进行计算，向量X_k中的前三个数值便对应精确估计的机器人实时位姿(x_k、y_k、θ_k)，计算表达式为：

此外，由于遮挡或机器人抖动，偶然会出现漏识别ArUco码，进而无法提供视觉信息的机器人位姿的情况，本发明使用扩展卡尔曼滤波一并解决这个问题。当这种情况出现时，本发明调整演化噪声方差Q_k以及测量噪声方差R_k的值，从而选择完全相信并采用轮式里程计提供的位姿信息，表达式为：

这种情况下，扩展卡尔曼滤波将完全通过轮式里程计提供的上一时刻速度估计值

来计算当前的机器人位姿估计值

可对丢失视觉位姿信息的情况有一个很好的应急处理。

步骤四、根据位姿规划机器人每一时刻的期望朝向，以期望朝向与实际朝向之间的差值作为输入误差，使用PID方法控制机器人前往充电桩，并精确对接。

步骤三得到了精确的机器人在世界坐标系下的实时位姿(x_t、y_t、θ_t)，为了能够精准、快速、平滑地对接充电桩，还需要基于实时位姿进行精准的运动控制。本发明使用了PID运动控制方法控制机器人前往充电桩并精确对接，如图7所示，该步骤可以具体分为以下三个部分：

(1)选择机器人的朝向θ作为被控量，以期望朝向θ_r与实际朝向θ_u之间的差值作为输入误差θ_e，使用PID方法控制机器人的角速度，如图8所示。

具体地，在t时刻，机器人与世界坐标系原点即目标点的角度

的表达式为：

本发明令每一时刻的期望朝向θ_r，t为角度

的1.5倍，表达式为：

这样能够让机器人以一个圆弧状的轨迹前往充电桩，使得y可以首先接近0，从而让机器人以一个正对的姿态对接充电桩。

本发明的输入误差θ_e，t为期望朝向θ_r，t与实际朝向θ_u，t之间的差值，表达式为：

θ_e，t＝θ_r，t-θ_u，t

PID控制规律为：

在本实施例中，设定K_p＝0.33，K_i＝0，K_d＝0.17。通过PID控制规律实时得到当前时刻的输出角速度w_t。

(2)使用输出角速度w_t计算输出线速度v_t。

具体地，输出线速度的大小则由输出角速度决定，应满足随着输出角速度绝对值的增大而减小，且最小不小于0，最大不超过提前给定的最大线速度v_max。

在本实施例中，输出线速度v_t的计算公式为：

通过步骤四的(1)、(2)两部分能够得到t时刻实时的输出速度(v_t、w_t)，这样可以控制机器人沿一条平滑的轨迹，快速前往充电桩与之对接。如图9所示，是机器人在初始位姿(x₀、y₀、θ₀)开始前往充电桩的轨迹。

(3)在检测到某时刻移动机器人位姿在x方向的数值x_t小于0时，表示机器人已经与充电桩对接，发布刹车信号制动，使v_t＝0，w_t＝0，从而始终保持对接状态，进行充电。

本发明基于单目相机和ArUco码对移动机器人进行引导，相比于传统的红外技术对近距离的充电桩的限制要求，能够实现远距离引导回充，相比于识别图表或色块的方式，本发明利用相机识别ArUco码可以得到更丰富的视觉信息，为移动机器人提供其在世界坐标系下的更加精准的位姿，识别更加稳定，定位精度高；在引导过程中只会占用很少的计算资源，从而可以对图像实时处理、对移动机器人实时控制，引导移动机器人快速、准确、平滑地对接充电桩。

以上所述，仅为本发明的具体实施方式，但本发明的保护范围并不局限于此，任何熟悉本技术领域的工作人员在本发明揭露的技术范围内，可轻易想到各种等效的修改或替换，这些修改或替换都应涵盖在本发明的保护范围之内。因此，本发明的保护范围应以权利要求的保护范围为准。

Claims (8)

1.一种基于ArUco码引导的移动机器人自主充电方法，其特征在于，包括下列步骤：

1)于移动机器人正后方设置充电触点以及用以捕获移动机器人正后方实时图像的单目相机，并在移动机器人运行环境中的布局多个设有ArUco码的充电桩，各充电桩的充电槽与所述充电触点处于同一高度上；

2)建立移动机器人活动的环境地图，在环境地图中标记出各充电桩的位置及朝向；

3)设置导航目标点以及充电触点和单目相机的镜头的目标姿态，规划移动机器人的运动轨迹，直到移动机器人到达目标点，并达到目标姿态；

4)利用单目相机识别充电桩上的ArUco码，获取ArUco码在图像上的角点坐标，并通过角点间关系解算与坐标系变换，得到视觉信息提供的移动机器人与充电桩坐标系之间的位姿变换关系；

5)采用扩展卡尔曼滤波法，将单目相机获取的移动机器人位姿与移动机器人自身轮式里程计提供的移动机器人线速度、角速度相结合，获取精确移动机器人位姿；

6)依据精确移动机器人位姿规划移动机器人每一时刻的期望朝向，以期望朝向与实际朝向之间的差值作为输入误差，采用PID方法控制移动机器人前往充电桩进行对接，进行移动机器人的自主充电；

步骤4)包括下列步骤：

41)调用ArUco官方库中的检测函数，通过自适应阈值颜色分割以及透视变换方法识别单目相机捕获的图像中的ArUco码，进而获取角点的坐标；

42)调用ArUco官方库中的位姿计算函数，通过角点坐标的关系解算，结合标定的单目相机参数，获取单目相机与ArUco码坐标系间的坐标变换，得到基于旋转矩阵计算ArUco码坐标系相对于单目相机坐标系的变换矩阵；

43)根据单目相机、移动机器人坐标系间固定的变换关系，通过坐标系变换，获取移动机器人初始位姿；具体内容为：

固定一个世界坐标系，将该世界坐标系的原点设置为距离充电桩正前方一个移动机器人半径长度的点，获取ArUco码坐标系相对于世界坐标系的变换矩阵

其表达式为：

式中，r为移动机器人的半径；

同时，获取移动机器人坐标系相对于单目相机坐标系的变换矩阵

其表达式为：

式中，

为单目相机固定位置到移动机器人中心的距离；

通过

以及步骤42)得到的ArUco码坐标系相对于单目相机坐标系的变换矩阵，计算移动机器人坐标系相对于世界坐标系的变换矩阵

其表达式为：

式中，

为ArUco码坐标系相对于单目相机坐标系的变换矩阵；

基于移动机器人坐标系相对于世界坐标系的变换矩阵，使用移动机器人在世界坐标系下的2D位姿表示移动机器人的定位。

2.根据权利要求1所述的基于ArUco码引导的移动机器人自主充电方法，其特征在于，步骤3)的具体内容为：

将充电桩前方1m的位置设为导航目标点，将充电触点和单目相机的镜头正对充电桩设为目标姿态，规划移动机器人的运动轨迹，对移动机器人进行带有避障功能的导航，当移动机器人到达目标点位置后，控制移动机器人进行原地旋转，直至达到既定的目标姿态。

3.根据权利要求1所述的基于ArUco码引导的移动机器人自主充电方法，其特征在于，步骤41)的具体内容为：

调用ArUco官方库中的检测函数，检测函数首先将单目相机捕获的彩色图像转换为灰度图像，移除灰色像素，保留黑色、白色像素，获取黑白二值化图像，从黑白二值化图像中寻找所有由黑色边界包围的四边形，对四边形区域进行透视变换，获取规范形态，将规范形态的四边形区域根据大小和边界进行分格处理，通过每个格子的黑白颜色判断该区域是否为一个ArUco码，若是，则计算出其对应的字典ID，若该ID与预先固定在充电桩上的ArUco码ID相同，则表示单目相机捕获到了充电桩上的ArUco码，进而获取该ArUco码在单目相机捕获的图像上对应的角点坐标。

4.根据权利要求1所述的基于ArUco码引导的移动机器人自主充电方法，其特征在于，步骤42)的具体内容为：

根据实际使用的ArUco码获取ArUco码边长，获取标定的单目相机参数，包括单目相机的内参数矩阵和单目相机的畸变矩阵，利用上述参数结合步骤41)获取的角点坐标，计算ArUco码坐标系相对于单目相机坐标系的旋转矩阵，并基于旋转矩阵计算ArUco码坐标系相对于单目相机坐标系的变换矩阵。

5.根据权利要求1所述的基于ArUco码引导的移动机器人自主充电方法，其特征在于，移动机器人在世界坐标系下的2D位姿表示移动机器人的定位的表达式为：

x＝p_w1

y＝p_w2

式中，p_w为世界坐标系下移动机器人的位置，p_w*为p_w对应行列处的元素值，

为移动机器人坐标系相对于世界坐标系的变换矩阵

对应行列处的元素值，(x、y、θ)为移动机器人的2D位姿。

6.根据权利要求1所述的基于ArUco码引导的移动机器人自主充电方法，其特征在于，若引导过程中发生单目相机漏识别ArUco码的偶发情况，则在发生漏识别ArUco码的当前时刻完全相信里程计提供的移动机器人线速度、角速度，使用扩展卡尔曼滤波算法计算移动机器人位姿。

7.根据权利要求1所述的基于ArUco码引导的移动机器人自主充电方法，其特征在于，步骤6)包括下列步骤：

61)选择移动机器人的朝向θ作为被控量，以期望朝向θ_r与实际朝向θ_u之间的差值作为输入误差θ_e，利用PID方法控制移动机器人的角速度；

62)使用步骤61)输出的移动机器人的角速度计算移动机器人的输出线速度，根据角速度、线速度控制移动机器人沿平滑轨迹快速前往充电桩与之对接；

63)当检测到某时刻移动机器人位姿在x方向的数值x_t小于0，则判断移动机器人已与充电桩实现对接，发布刹车信号制动，使移动机器人的线速度、角速度为零，进而使其始终保持对接状态，实现自主充电。

8.根据权利要求1所述的基于ArUco码引导的移动机器人自主充电方法，其特征在于，执行步骤3)时，若到达目标姿态后，单目相机未识别到ArUco码，则控制移动机器人以先逆时针，后顺时针的方式旋转，直至寻找到ArUco码。

Images