[发明专利]一种标靶整体优化的空间位置与姿态解求方法

权利要求书

1.一种标靶整体优化的空间位置与姿态解求方法，其特征在于，包括以下步骤：

1)获取标靶的成像点数据，包括标靶的空间姿态R_i、空间位置T_i、成像后的标靶的影像的坐标(u，v)、标靶的实际空间上的坐标值(X，Y)；

2)通过标靶的空间姿态R_i和空间位置T_i，根据空间的几何关系进行标靶—相机公式推导，即反求出相机的空间姿态R_i^-1和空间位置-R_i^-1*T_i；

3)根据已知的相机内参矩阵矩阵A，标靶的空间姿态矩阵R_i，列向量矩阵T_i和尺度因子s，将步骤2)中的标靶—相机公式进行推导，得到标靶在实际空间的坐标值计算值计算该值与实际空间上的坐标值(X，Y)的差值，即得到残差矩阵L；

4)把步骤2)中的标靶—相机公式进行泰勒展开式，也针对公式中的多个变量进行求导，获得的相关导数求解出来的数值，作为误差矩阵B；

5)通过平差理论，求解x＝(B^TB)^-1B^TL，其中x为相机相关角度和空间位置的改正值；

6)如果改正值x小于限差，其中，x为相关角度的改正值，则角度值<0.000001弧度，x为空间位置的改正值，空间位置<0.000001，则平差收敛，通过改正后的角度值计算出优化以后的相机空间位置与姿态，并且反解出标靶的空间位置与空间姿态；

7)如果改正值x大于限差，其中，x为相关角度的改正值，则角度值﹥0.000001弧度，x为空间位置的改正值，空间位置﹥0.000001，则需要进行下一次的迭代，返回步骤3)，对步骤3)的残差矩阵L进行分析，将残差矩阵L对应的标靶空间坐标点(X，Y)通过标靶—相机公式投影到影像上来，计算其投影点与对应影像的坐标点(u，v)的差值，如果标靶点残差大于2.0像素，则该点属于噪点，则将其剔除后再次进行平差，如果剔除的数据点大于总点数的20％，则认为该标靶影像获取效果较差，将其作为候选的误差标靶；

8)根据步骤6)得到的标靶的空间姿态，将标靶空间坐标点(X，Y)通过标靶—相机公式投影到影像上来，计算其投影点与对应坐标点(u，v)的误差，并统计其平均误差，利用该平均误差对步骤7)中的候选的误差标靶进行判断其是否为错误影像帧，如果影像平均误差大于1.5像素，则认为该影像为误差影像，将其进行剔除。

2.根据权利要求1所述的标靶整体优化的空间位置与姿态解求方法，其特征在于，所述的步骤1)中获取标靶的空间姿态R_i、空间位置T_i的方法，包括以下步骤：

11)通过四轮定位仪的相机获取标靶的起始影像和实时影像；

12)提取标靶的起始影像和实时影像中的椭圆数据，对椭圆数据进行排列；

13)计算标靶的空间位置T、空间姿态R，包括起始影像的空间位置T₀、空间姿态R₀和实时影像的空间位置T_i、空间姿态R_i；

14)通过实时影像的空间姿态R_i与起始影像的空间姿态R₀，获得起始影像到实时影像的标靶的空间姿态关系：R_i0＝Ri*R₀^-1，通过罗德里格斯公式求解旋转轴，获得旋转的角度；

15)如果旋转的角度满足一定条件，将其所求解的影像的标靶点坐标(u，v)与相对应的实际的空间点坐标(X,Y)、标靶的空间位置T_i和标靶的空间姿态R_i保存，作为下一步的数据。