[发明专利]一种全局最优的光学跟踪器位姿计算方法

权利要求书

1.一种光学跟踪器位姿计算方法，其特征在于，包括：

步骤1、针对每一个传感器，确定其能接收到信号的发射器，将一个传感器与其能接收到信号的一个发射器作为一个发射接收组合，遍历所有传感器，统计所有所述发射接收组合数目，并记为N；

步骤2、针对任意一个发射接收组合，令其中的传感器序号为j，发射器序号表示为i；则确定第j个传感器在自身刚体坐标系下的三维坐标确定第j个传感器在其能接收到信号的第i个发射器中的二维图像坐标然后建立关于该发射接收组合中互相对应的三维空间点与二维图像点的有效方程组：

其中，p_i1、p_i2、p_i3和p_i4表示传感器刚体坐标系与第i个发射器的图像坐标系之间的投影关系矩阵P_i中的元素；a_ij＝[0,-1,v_ij]^T，b_ij＝[1,0,-u_ij]^T，其中，u_ij和v_ij分别表示二维图像坐标中两个坐标轴方向的坐标；其中，表示传感器刚体坐标系转换到发射器坐标系的旋转矩阵，表示传感器刚体坐标系转换到发射器坐标系的平移矩阵；

步骤3、针对每一个发射接收组合建立一个式(1)所示的方程组，N个发射接收组合即得到N个方程组，由此形成2N维的线性方程组；

步骤4、将步骤3形成的线型方程组改写成如下形式：

AX＝B (2)

其中A为2N×12的矩阵，

X为12×1的列向量，X＝[r₁₁,r₁₂,r₁₃,t₁,r₂₁,r₂₂,r₂₃,t₂,r₃₁,r₃₂,r₃₃,t₃]^T；

B为2N×1的列向量，

步骤5、当4≤N≤5时，对公式(2)进行求解的具体方法为：

在X中抽取9个元素得到旋转矩阵R，表示为：

R＝f_R(X)

并使得旋转矩阵R为酉矩阵，满足RR^-1＝I和R^-1＝R^T，I为3×3的单位矩阵；

然后将式(2)所示的线性方程组求解问题转化为以下最优化问题：

即：在满足s.t.f_R(X)f_R(X)^T-I＝0的约束条件下，使取最小值的X为最优解，实现位姿解算；

当N≥6时，采用解析法对公式(2)进行求解，得到X，实现位姿解算。

2.如权利要求1所述的一种光学跟踪器位姿计算方法，其特征在于，所述步骤5中，采用Levenberg-Marquardt算法求解所述最优化问题。