一种基于Tsai算法的改进位姿估计方法

摘要

本发明公开了一种基于Tsai算法的改进位姿估计方法，包括步骤，对使用的相机进行标定得到相机焦距；在相机的当前使用位姿下，对空间中的共面的至少5个特征点进行成像，以特征点中任意一个特征点P₀为原点建立世界坐标系，并获取特征点的世界坐标和图像坐标；利用Tsai算法求解相机所在相机坐标系相对于世界坐标系的旋转矩阵R；根据旋转矩阵R、相机焦距、以及求解得到的特征点之间线段长度，求解相机所在相机坐标系相对于世界坐标系的平移向量T；根据旋转矩阵R和平移向量T，求解相机的位姿参数。有益效果：在Tsai算法求解旋转矩阵R的基础上，改进平移向量T的求解过程，以提高位姿估计的精度和实时性，从而大幅提高适用范围。

主权项

一种基于Tsai算法的改进位姿估计方法，其特征在于，包括以下步骤：步骤1、对使用的相机进行标定，得到相机焦距；步骤2、在相机的当前使用位姿下，对空间中的共面的至少5个特征点进行成像，以特征点中任意一个特征点P₀为原点建立世界坐标系，并获取特征点的世界坐标和图像坐标；步骤3、根据特征点的世界坐标和图像坐标，利用Tsai算法求解相机所在相机坐标系相对于世界坐标系的旋转矩阵R；步骤4、根据步骤3求得的旋转矩阵R、步骤1标定得到的相机焦距、以及求解得到的特征点之间线段长度，求解相机所在相机坐标系相对于世界坐标系的平移向量T；具体为，步骤4‑1、根据特征点在等效成像平面上的投影点坐标、即特征点的图像坐标，选取投影点间的任意一条线段、记线段长度为l，并获取构成该线段的两个投影点对应的两个特征点、记两个特征点分别为P₁和P₂；步骤4‑2、将步骤3求得的旋转矩阵R作用于世界坐标系，得到旋转后的新坐标系，该新坐标系平面与相机等效成像平面平行；根据旋转矩阵R和特征点P₁、P₂的世界坐标，求解特征点P₁、P₂在新坐标系中的坐标，进而获取特征点P₁、P₂在新坐标系平面上的投影点间的线段、记新线段长度为L；步骤4‑3、根据线段长度l和新线段长度L、步骤1标定得到的相机焦距、以及步骤2获取的特征点P₀的图像坐标，由以下公式求解平移向量T，$\frac{x_{0i}}{x_{0c}}=\frac{y_{0i}}{y_{0c}}=\frac{f}{z_{0c}}=\frac{l}{L}$T＝(x_0c,y_0c,z_0c)^T其中，f为相机焦距，x_0i、y_0i为特征点P₀的图像坐标，x_0c、y_0c、z_0c为相机坐标系的相机坐标；步骤5、根据步骤3求得的旋转矩阵R和步骤4求得的平移向量T，求解相机的位姿参数。