[发明专利]一种稳健的绝对定向算法

权利要求书

1.一种稳健的绝对定向算法，其特征在于：包括以下步骤：

S1、构建基于加权系数的绝对定向问题求解模型，

假设n个空间点在坐标系A和B之中的三维坐标分别为r_A,i和r_B,i(i＝1,2,…,n)，通过加权的方法，使用加权形式的绝对定向问题求解模型

的最优解作为旋转矩阵和平移向量的估计值，其中ω_i≥0表示第i个坐标点对应的权值且表示行列式为1的三阶正交矩阵集合，|| ||表示2范数；

S2、将每个坐标点的权值设置为即

S3、推导绝对定向问题求解模型的解析求解算法，得到(2)式的最优解，计算S2中权值对应的旋转矩阵R和平移向量t，

记A、B坐标系中的加权质心表达式为和(2)式的模型中，旋转矩阵和平移向量的求解存在耦合，为进行解耦，引入第i个空间点的新坐标和理想情况下其中R_true和t_true分别为旋转矩阵和平移向量的真值，进而有

r'_B,i＝R_truer'_A,i， (3)

成立，因此，先通过求解最优化模型

得到当前权值对应的旋转矩阵R^*，再通过

得到位移参数中的平移向量t^*，从而实现优化参数解耦的目的，

由于

前两项与旋转矩阵无关，因此，(4)式的优化模型等价于

其中tr()为矩阵的迹函数，为求解(7)式，将矩阵M进行奇异值分解得到其中U和V为3×3的正交矩阵，σ_j≥0(j＝1,2,3)为矩阵M的奇异值，当

R^*＝V^TU, (8)

(7)式取得最大值，

为得到R^*对应的平移向量，将(5)式对t求导得令该导数为0，得到权值对应的平移向量为

S4、计算重合误差e_i＝||Rr'_A,i-r'_B,i||，使用S估计方法更新权值后，进入步骤S3，根据(8)式计算当前权值对应的旋转矩阵，并计算重合误差，再依据(9)式计算当前权值对应的平移向量；

S5、不断循环步骤S4和S3，当S估计方法的迭代次数超过设置的最大迭代次数时，输出旋转矩阵R和平移向量t。

2.根据权利要求1所述一种稳健的绝对定向算法，其特征在于：所述步骤S4中S估计方法具体为，

当循环次数为1时，对误差进行归一化其中为尺度估计量，表示的|e_i|中位数，0.6754为S估计中的常数，使用

作为新权值，当循环次数大于1的时候，则使用对观测误差进行归一化使用

作为新的权值。