[发明专利]一种基于ArUco码的移动机器人视觉定位与导航方法

权利要求书

1.一种基于ArUco码的移动机器人视觉定位与导航方法，其特征在于：包括如下步骤：

步骤1：创建地图；

利用相机对环境中随机分布的ArUco码进行视觉建图，获得ArUco码分布地图；

步骤2：创建路径；

在计算机中生成导航路径的坐标点，获得由一系列三维坐标点组成的导航路径文件；

步骤3：定位与导航；

根据步骤1和步骤2创建的ArUco码分布地图和导航路径文件，利用相机实现移动机器人在三维空间中的视觉定位与导航。

2.根据权利要求1所述的基于ArUco码的移动机器人视觉定位与导航方法，其特征在于：在步骤1中，具体包括如下步骤：

步骤1.1：将打印好的ArUco码随机地粘贴在包括墙面、门面、地面、天花板或立柱在内的光照条件良好的平整位置；

步骤1.2：用相机对室内随机分布的ArUco码进行视觉建图，确定一个参考世界坐标系，并获得所有ArUco码的ID信息及其对应四个角点在世界坐标系下的三维坐标。

3.根据权利要求2所述的基于ArUco码的移动机器人视觉定位与导航方法，其特征在于：在步骤1.2中，用相机对室内随机分布的ArUco码进行视觉建图，具体包括如下步骤：

步骤1.2.1：用棋盘格预先标定出相机的内参数；

步骤1.2.2：打开ArUco建图工具，传入相机内参，并指定环境中的一个ArUco码作为参考，程序开始运行；

步骤1.2.3：手持或机载相机缓慢移动，使相机能遍历所有ArUco码；

步骤1.2.4：建图程序运行结束，得到ArUco码的分布地图，分布地图中包含环境中所有ArUco码的ID信息及其对应四个角点的三维坐标信息，另外包括一个分布点云。

4.根据权利要求1所述的基于ArUco码的移动机器人视觉定位与导航方法，其特征在于：在步骤2中，具体包括如下步骤：

步骤2.1：根据周围环境和移动机器人的任务规划机器人的行走路线；

步骤2.2：将移动机器人的行走路线在世界坐标系下用数学方程标出；

步骤2.3：根据导航路径的起止点坐标，在计算机中将用数学方程表示的行走路线离散化为一系列三维坐标点，所有点的坐标按顺序保存在一个文本文档里，得到导航路径文件。

5.根据权利要求1所述的基于ArUco码的移动机器人视觉定位与导航方法，其特征在于：在步骤3中，具体包括如下步骤：

步骤3.1：加载ArUco码分布地图和导航路径文件，并将ArUco码的分布点云以及导航路径显示到可视化界面中；

步骤3.2：根据图像中每个ArUco码角点的像素坐标和其对应的三维世界坐标，利用PnP迭代算法估计相机的位姿，从而实时地获取机器人在世界坐标系下的三维空间坐标和姿态；

步骤3.3：判断机器人是否到达目标位置；

若：判断结果是机器人到达目标位置，则结束；

或判断结果是机器人没有到达目标位置，则计算出与机器人当前位置距离最近的路径点，并确定下一个参考路径点的位置；

步骤3.4：计算机器人当前位置与下一个参考路径点之间的方向向量V′，并计算机器人当前姿态向量V与方向向量V′之间的夹角α；

步骤3.5：机器人跟据偏航角α调整自身的姿态，以速度v沿着导航路径前进，然后执行步骤3.2。

6.根据权利要求5所述的基于ArUco码的移动机器人视觉定位与导航方法，其特征在于：在步骤3.2中，具体包括如下子步骤：

步骤3.2.1：相机采集一帧图像，对图像进行包括灰度转换、二值化和轮廓提取在内的预处理操作，然后对每个轮廓进行筛选、透视变换、内部编码提取；

步骤3.2.2：判断检测图像中是否出现至少一个ArUco码；

若：判断结果是检测图像中出现至少一个ArUco码，则根据识别出的所有ArUco码的ID信息，确定每个ArUco码的角点像素坐标和与之相对应的三维空间坐标，利用PnP迭代算法估计相机在世界坐标系中的位置和姿态；

或判断结果是检测图像中没有出现ArUco码，则执行步骤3.2.1；

步骤3.2.3：根据相机与机器人本体的相对位姿关系和相机在世界坐标系中的位姿进行坐标变换，计算出机器人在世界坐标系下的位置P和姿态向量V。