[发明专利]一种机器人低成本导航定位方法、系统及应用

说明书

本发明属于机器人导航相关技术领域，其公开了一种机器人低成本导航定位方法、系统及应用，方法包括：在机器人的工作环境中设置多个标志物并对标志物进行编码，将标志物的编码与位置信息进行绑定并存储于上位机；摄像头对环境中的标志物进行捕捉，将捕捉的标志物的图片发送至上位机，上位机根据图片上存储的编码信息提取标志物的位置信息，并使用视觉算法得到机器人当前的位置和姿态；采用位置和姿态对惯性导航系统的位置和姿态进行校正。本申请采用视觉算法精确计算机器人的位置和姿态，实现对惯性导航系统的定期校正，避免了惯性导航系统的误差累积和扩散。

技术领域

本发明属于机器人导航相关技术领域，更具体地，涉及一种机器人低成本导航定位方法、系统及应用。

背景技术

随着科学技术的发展，人类越来越重视海洋资源的开发，水产养殖是全球增长最快的食品行业之一，其产量超过全世界鱼类消费的50％，目前，水产养殖多采用深海网箱的形式，为了提高养殖效率，对网箱的定期清洗、检查工作必不可少，若长时间得不到清洗，深海网箱的网衣在海水中经长时间浸泡后，大量的藻类、贝类等容易附着在网衣上，导致网眼堵塞，不仅影响水体交换还会污染水体，不利于鱼类的生长。水下清洗机器人可以在设定深度下重复地进行清洗、检查工作，不仅有着极高的清洗效率，还不受环境的影响，因此，如何实现清洗机器人在养殖网箱内的定位与导航成为了关键问题。

目前水下机器人常用的定位方法有：惯性导航、GPS全球导航、声学导航等，惯性导航是一种自主式导航系统，基本原理是利用角速度、加速度，通过积分运算得到载体的角度、位置，由于惯性器件存在误差会导致惯导系统在更新计算的过程中误差随时间累积的问题。GPS导航系统具有实时、准确的特点，但是信号无法在水下传播，不适用于进行深海作业。声波可以在水中长距离的传输，因此声学导航广泛应用于水下机器人的导航中。但是声学基阵的布置成本高昂，实际应用时维护较为困难。

发明内容

针对现有技术的以上缺陷或改进需求，本发明提供了一种机器人低成本导航定位方法、系统及应用，本申请采用视觉算法精确计算机器人的位置和姿态，实现对惯性导航系统的定期校正，避免了惯性导航系统的误差累积和扩散。且本方法成本低廉，适合在水下结构化环境中布置使用。

为实现上述目的，按照本发明的一个方面，提供了一种机器人低成本导航定位方法，所述机器人上设有惯性导航系统以及摄像头，所述方法包括：S1：在机器人的工作环境中设置多个标志物并对其进行编码，将标志物的编码与位置信息进行绑定并存储于上位机；S2：摄像头对环境中的标志物进行捕捉，将捕捉的标志物的图片发送至上位机，所述上位机根据图片上存储的编码信息提取所述标志物的位置信息，并使用视觉算法得到机器人当前的位置和姿态；S3：采用所述位置和姿态对所述惯性导航系统的位置和姿态进行校正。

优选地，步骤S1中对标志物进行编码具体包括：记录标志物颜色的H、S、V分量范围以及标志物各个角点的绝对坐标值，并在标志物上设置编号。

优选地，步骤S2中所述上位机根据图片上存储的编码信息提取所述标志物的位置信息，并使用视觉算法得到机器人当前的位置和姿态具体包括：S21：上位机根据图片上像素的H、S、V分量将标志物与背景进行分割，得到二值图；S22：采用图像算法获取标志物的外轮廓并采用多边形近似拟合算法获得标志物外轮廓各个角点的图像坐标p_i；S23：将各个角点的图像坐标与通过编号获取的绝对坐标进行一一对应；S24：将所述图像坐标与绝对坐标采用正交迭代计算获得机器人当前的位置和姿态。

优选地，步骤S24具体为：获取最优平移矩阵T_best，其计算公式如下：

其中，n为角点个数，I为单位矩阵，V_i为根据所述图像坐标计算得到的投影矩阵，其计算公式为p_i为第i个角点的图像坐标，R为惯性导航系统返回的起始旋转矩阵，P_Wi为角点的绝对坐标；