[发明专利]一种基于矩形标识的位姿估计方法、装置及机器人

说明书

本发明适用于计算机视觉领域，提供了一种基于矩形标识的位姿估计方法、装置及机器人。所述方法包括：通过摄像头的内参数和畸变参数，对矩形标识的四个角点的图像坐标进行畸变矫正，输出畸变校正之后的四个角点；将畸变校正之后的四个角点中随机抽取三个角点作为P3P算法的求解点，另外一个角点作为校验点，产生四组点，对于TS4的情况，每组点得出两组解，通过剩余的点进行验证，从而得到8组解；通过各自组点内剩余的点进行重投影误差的计算，选取重投影误差最小的点作为最终估计的位姿。本发明相比于当前P3P算法中的随机取三个点作为计算点，将剩余的那个点作为校验点更具理论完备性，不会造成冗余，计算的位姿比较准确。

技术领域

本发明属于计算机视觉领域，尤其涉及一种基于矩形标识的位姿估计方法、装置及机器人。

背景技术

位姿估计是计算机视觉中一个重要的领域，它有很多应用，例如，增强现实、虚拟现实以及物体空间定位(如无人机的空间位姿估计)。

常用的位姿估计算法一般分为基于视觉的和基于传感器的位姿估计算法，基于视觉的位姿估计算法由于其成本低、抗电磁干扰、精度也较高的优点，已经逐步的取代了基于传感器的位姿估计。而基于视觉的位姿估计算法又分为基于单目的和基于多目的算法，基于单目的位姿估计算法与基于多目的位姿估计算法相比具有：系统简单、价格低、灵活性好的优点。因此，目前基于单目视觉的位姿估计算法已经成为研究的热点。

而位姿估计算法中常用的为PnP算法，该算法是根据n个点的图像坐标与对应的世界坐标来推算摄像头的位姿。

目前常用的位姿估计算法一般使用二维码(或者其他类似的矩形标识)作为标识物，通过捕获二维码的四个角点获取二维码的图像坐标，而由于已知二维码的世界坐标，因此可以通过PnP算法求解出摄像头的位姿。目前来说，常用的PnP算法为P3P算法，该算法通过三个不共线的点(三个不共线的点必然共面)求解出摄像头的位姿。而P3P问题中通用型的算法为TS1的情况，即对四个点的位置不做要求，不共线就可以。TS1的解为4个解，然后通过第4个点进行验证，看哪个解的重投影误差最小，便将该解作为最优解。

而平时使用最多的并非是任意分布的四个点，一般为类似于二维码的情况，即矩形标识(长方形或者正方形的标识)，如果使用TS1情况的PnP算法，就会造成一定程度的冗余，从而造成计算的位姿误差偏大。

发明内容

本发明的目的在于提供一种基于矩形标识的位姿估计方法、装置、计算机可读存储介质及机器人，旨在解决对于矩形标识，如果使用TS1情况的PnP算法，会造成一定程度的冗余，从而造成计算的位姿误差偏大的问题。

第一方面，本发明提供了一种基于矩形标识的位姿估计方法，所述方法包括：

标定摄像头的内参数和畸变参数；

获取摄像头拍摄到的图像，识别出矩形标识；

获取矩形标识的四个角点的图像坐标，根据需求建立世界坐标系，并通过测量获取矩形标识的四个角点在世界坐标系中的坐标；

通过摄像头的内参数和畸变参数，对矩形标识的四个角点的图像坐标进行畸变矫正，输出畸变校正之后的四个角点；

将畸变校正之后的四个角点中随机抽取三个角点作为P3P算法的求解点，另外一个角点作为校验点，产生四组点，对于TS4的情况，每组点得出两组解，通过剩余的点进行验证，从而得到8组解；

通过各自组点内剩余的点进行重投影误差的计算，选取重投影误差最小的点作为最终估计的位姿。

第二方面，本发明提供了一种基于矩形标识的位姿估计装置，所述装置包括：

标定模块，用于标定摄像头的内参数和畸变参数；

识别模块，用于获取摄像头拍摄到的图像，识别出矩形标识；