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

权利要求书

1.一种机器人低成本导航定位方法，其特征在于，所述机器人上设有惯性导航系统以及摄像头，所述方法包括：

S1：在机器人的工作环境中设置多个标志物并对其进行编码，将标志物的编码与位置信息进行绑定并存储于上位机；

S2：摄像头对环境中的标志物进行捕捉，将捕捉的标志物的图片发送至上位机，所述上位机根据图片上存储的编码信息提取所述标志物的位置信息，并使用视觉算法得到机器人当前的位置和姿态；

S3：采用所述位置和姿态对所述惯性导航系统的位置和姿态进行校正。

2.根据权利要求1所述的方法，其特征在于，步骤S1中对标志物进行编码具体包括：

记录标志物颜色的H、S、V分量范围以及标志物各个角点的绝对坐标值，并在标志物上设置编号。

3.根据权利要求2所述的方法，其特征在于，步骤S2中所述上位机根据图片上存储的编码信息提取所述标志物的位置信息，并使用视觉算法得到机器人当前的位置和姿态具体包括：

S21：上位机根据图片上像素的H、S、V分量将标志物与背景进行分割，得到二值图；

S22：采用图像算法获取标志物的外轮廓并采用多边形近似拟合算法获得标志物外轮廓各个角点的图像坐标p_i；

S23：将各个角点的图像坐标与通过编号获取的绝对坐标进行一一对应；

S24：将所述图像坐标与绝对坐标采用正交迭代计算获得机器人当前的位置和姿态。

4.根据权利要求3所述的方法，其特征在于，步骤S24具体为：

获取最优平移矩阵T_best，其计算公式如下：

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

获取绝对坐标的投影矩阵Q_i，Q_i＝V_i(RP_Wi+T_best)，并使用SVD分解法计算的左奇异矩阵U和右奇异矩阵V，获得迭代中的最优旋转矩阵R_best：

R_best＝VU^T

其中，为各个角点图像坐标的平均值，为各个角点绝对坐标的平均值；

根据所述最优平移矩阵T_best和最优旋转矩阵R_best获得摄像头的位置和姿态，进而获得机器人的位置和姿态：

位置：

[x y z]^T＝-R_bestT_best

其中，x，y，z为机器人的在xyz方向的位置坐标；

姿态：

θ＝arcsin(-R(3，1))

其中，θ，ψ分别为机器人的横滚角，俯仰角和艏向角，R(*，)矩阵R的第*行第列。

5.根据权利要求4所述的方法，其特征在于，正交迭代过程中还包括计算空间共线性误差E，若计算的误差与上次误差的差值小于预设值则停止迭代输出结果，其中，

6.根据权利要求1所述的方法，其特征在于，所述上位机与所述机器人通过以太网电缆连接。

7.根据权利要求1所述的方法，其特征在于，所述机器人上还设有探照灯，用于对工作环境进行照射。

8.根据权利要求1所述的方法，其特征在于，所述编码为二维码。

9.一种权利要求1～8任意一项所述的机器人低成本导航定位方法的应用，其特征在于，所述方法应用于水下结构化环境中。

10.一种机器人低成本导航定位系统，其特征在于，所述系统包括：

标志物设置模块：用于在机器人的工作环境中设置多个标志物并对标志物进行编码，将标志物的编码与位置信息进行绑定并存储于上位机；

位姿获取模块：用于摄像头对环境中的标志物进行捕捉，将捕捉的标志物的图片发送至上位机，所述上位机根据图片上存储的编码信息提取所述标志物的位置信息，并使用视觉算法得到机器人当前的位置和姿态；

位姿校正模块：用于采用所述位置和姿态对所述惯性导航系统的位置和姿态进行校正。