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

摘要

本发明公开了一种全局最优的光学跟踪器位姿计算方法，改进了传统跟踪器的位姿计算方法，使用了基于全局优化思想的数学模型，使用空间点和图像点的对应关系构建线性方程组，不需要计算跟踪器相对于单个基站的位姿，也不需要进行位姿数据融合，可以直接求解跟踪器全局最优位姿；该方法不限制基站数量，充分利用了所有基站对应点的信息(即使此基站的对应点数量不足以独立计算位姿)，大大降低了跟踪器位姿的最小计算条件(将对应点数量阈值由任意一个基站至少5组对应点降低为所有基站一共4组对应点)；此外，当多个接收器与跟踪器建立联系时，可获得全局最优的位姿融合结果，并且结果更加精确、鲁棒性更强。

主权项

1.一种光学跟踪器位姿计算方法，其特征在于，包括：/n步骤1、针对每一个传感器，确定其能接收到信号的发射器，将一个传感器与其能接收到信号的一个发射器作为一个发射接收组合，遍历所有传感器，统计所有所述发射接收组合数目，并记为N；/n步骤2、针对任意一个发射接收组合，令其中的传感器序号为j，发射器序号表示为i；则确定第j个传感器在自身刚体坐标系下的三维坐标确定第j个传感器在其能接收到信号的第i个发射器中的二维图像坐标然后建立关于该发射接收组合中互相对应的三维空间点与二维图像点的有效方程组：/n /n其中，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分别表示二维图像坐标中两个坐标轴方向的坐标；其中，表示传感器刚体坐标系转换到发射器坐标系的旋转矩阵，表示传感器刚体坐标系转换到发射器坐标系的平移矩阵；/n步骤3、针对每一个发射接收组合建立一个式(1)所示的方程组，N个发射接收组合即得到N个方程组，由此形成2N维的线性方程组；/n步骤4、将步骤3形成的线型方程组改写成如下形式：/nAX＝B (2)/n其中A为2N×12的矩阵，/nX为12×1的列向量，X＝[r₁₁,r₁₂,r₁₃,t₁,r₂₁,r₂₂,r₂₃,t₂,r₃₁,r₃₂,r₃₃,t₃]^T；/nB为2N×1的列向量，/n步骤5、当4≤N≤5时，对公式(2)进行求解的具体方法为：/n在X中抽取9个元素得到旋转矩阵R，表示为：/nR＝f_R(X)/n并使得旋转矩阵R为酉矩阵，满足RR^-1＝I和R^-1＝R^T，I为3×3的单位矩阵；/n然后将式(2)所示的线性方程组求解问题转化为以下最优化问题：/n /n即：在满足s.t.f_R(X)f_R(X)^T-I＝0的约束条件下，使取最小值的X为最优解，实现位姿解算；/n当N≥6时，采用解析法对公式(2)进行求解，得到X，实现位姿解算。/n