研究Wahba问题的算法对比以及新算法的提出

A Comparative Study of Algorithms for the Wahba Problem and the Proposal of a New Algorithm

摘要: Wahba问题是指在旋转矩阵估计中求解两组向量之间的最佳旋转矩阵，该问题常出现在航空航天工程、机器人应用和计算机视觉等领域。尤其在卫星姿态估计、图像拼接、3D重建等应用有着重要作用。在这些应用中，数据往往受到测量误差和异常值的影响，导致估计结果的精度下降，因此，如何在这些不理想条件下求解Wahba问题，成为了研究的核心问题之一。为了应对这些问题，要求算法具有更强的抗噪性和鲁棒性，以确保在不完全准确的数据条件下仍能获得可靠的结果。本文主要研究在三维空间中的Wahba问题，介绍了几种针对误差和异常值影响的算法，包括四元数解法(QUASAR)、快速全局配准算法(FGR)、RANSAC算法，此外，本文还在四元数解法的基础上提出了一种新的算法思路，旨在提高算法性能。文章简单阐述了算法的计算原理和推导过程，通过计算机进行数据仿真，对比了不同算法在存在误差和异常值情况下的精度、计算时间和鲁棒性。

Abstract: The Wahba problem refers to the estimation of the optimal rotation matrix between two sets of vectors in the context of rotation matrix estimation. This problem is commonly encountered in fields such as aerospace engineering, robotics, and computer vision. It plays an important role in applications like satellite attitude estimation, image stitching, and 3D reconstruction. In these applications, data are often affected by measurement errors and outliers, leading to a decrease in estimation accuracy. Therefore, solving the Wahba problem under these imperfect conditions has become a crucial research issue. To address these problems, algorithms are required to have stronger noise resistance and robustness, so as to ensure reliable results can still be obtained under conditions with inaccurate data. This paper primarily investigates the Wahba problem in three-dimensional space and introduces several algorithms designed to handle errors and outliers, including quaternion-based semidefinite relaxation for robust alignment (QUASAR), fast global registration (FGR), and RANSAC algorithm. Additionally, this paper proposes a new algorithmic approach based on the quaternion-based robust Wahba problem, aiming to improve algorithm performance. The computational principles and derivations of the proposed algorithms are briefly outlined, and through computer simulations, the paper compares the accuracy, computational time, and robustness of different algorithms under the influence of errors and outliers.

1. 引言

Wahba问题是由Grace Wahba [1]第一次于1965年提出，Wahba问题是指寻找最优旋转来对齐两组已知的有假定对应关系的向量观测值。给定两组向量，其中一组为观测向量，另一组为参考向量，目标是找到一个旋转矩阵，使得观测向量在经过旋转后尽可能地与参考向量对齐。是航天工程、机器人应用、计算机视觉等领域中的一个基本问题，尤其在在点云配准、图像拼接、运动估计、3D重建、卫星姿态估计等应用中有着重要作用。在理论上，Wahba问题可以通过奇异值分解的方法求出最优解，然而在这些实际应用中，例如在计算机视觉中，未经匹配的两组数据通常是采用特征匹配技术来进行粗匹配，例如在2D中使用SIFT [2]或ORB [3]方法，3D中使用FPFH方法，这些算法得到的数据结果往往存在误差和错误匹配，例如在使用FPFH [4]进行点云配准时，观察到95%的异常值并不少见[5]。在这种情况下，算法会受到误差的影响得到的结果具有很大的偏差，这需要算法具有更强的抗噪性和鲁棒性，使得结果达到一定的精度。而本文将着重介绍几种求解存在误差和异常值影响的Wahba问题的算法，包括基于四元数的半正定松弛算法(QUAternion-based Semidefinite relAxation for Robust alignment, QUASAR) [6]、快速全局配准算法(Fast Global Registration, FGR) [7]、RANSAC算法，并通过计算机数据仿真，对比不同算法的精准度、计算时间以及鲁棒性，并在四元数解法的基础上提出一种新的算法，来提高算法的性能。

2. 文献综述

对于不存在误差和异常值的Wahba问题，Schonemann [8]提出使用奇异值分解的方法求出解析解，随后有人提出使用四元数[9]和用旋转矩阵[10]表示的方法求出Wahba问题的最优解。随着Wahba问题应用逐渐广泛，数据中存在的误差问题不容忽视，随后人们提出广义Wahba问题，假设误差服从零均值各向异性的高斯分布，对于这类问题，Cheng和Crassidis [11]提出一种局部迭代优化算法，Briales和Gonzalez-Jimenez [12]提出用凸松弛的思路求解全局的方案，Ahmed [13]等人提出利用SDP松弛的方法解决一类含有界噪音且无异常值的问题。但这类方法在解决存在错误匹配的Wahba问题时结果受异常值影响，精度大大下降[14]。随后研究包含异常值的Wahba问题逐渐成为重点，最常用的方法是RANSAC算法，该算法在低噪音和低异常值的情况下，可在较短时间内取得较为精准的结果[14] [15]，其他情况表现较差。另外，RANSAC算法在计算时存在一定的随机性，导致同样的问题每次运行结果都可能不同，其不一定能得到全局最优。Zhou [7]等人提出快速全局配准的方法，构建鲁棒目标函数迭代求解得到最优解，但随着一次次迭代，问题呈现非凸性，也并不能保证全局最优性。Hartley和Kahl [16]首次提出使用分支定界的方法进行旋转搜索，之后Bazin [17]等人提出结合共识最大化的方法来拓展分支定界的算法，使其具有鲁棒性，尽管可以求出全局最优解，但在某些问题上，其运行时间可能是指数级的。Yang和Carlone [6]提出的基于四元数的优化方法，首次使用截断最小二乘问题对Wahba问题建模，结合四元数表示和半正定松弛求解最优解。本文将着重介绍和对比RANSAC算法，FGR算法以及基于四元数的半正定松弛算法。

3. 问题研究

这部分我们将介绍Wahba问题的数学建模，并简单介绍RANSAC算法，FGR算法以及基于四元数的半正定松弛算法的原理，并提出基于四元数方法的新算法思路。

3.1. Wahba问题

假设两组匹配的向量$A={\left\{a_i\right\}}_{i=1}^N,B={\left\{b_i\right\}}_{i=1}^N$ ，其中$a_i,b_i\in {ℝ}^3$ 。Wahba问题建模为最小二乘问题，即：

$\underset{R\in SO(3)}{\min }{{\displaystyle {\sum }_{i=1}^N{\omega }_i^2\Vert b_i-R{a}_i\Vert }}^2$ (1)

求解最优旋转R使得$A,B$ 向量组对齐。其中${\left\{{\omega }_i\right\}}_{i=1}^N$ 是每一对向量的权重， $SO\left(3\right)=\left\{R\in {ℝ}^{3\times 3},R^{\text{T}}R=R{R}^{\text{T}}=I_3,\det \left(R\right)=1\right\}$ 是三维的特殊正交群，$I_3$ 表示三维单位矩阵。当$\left(a_i,b_i\right)$ 是正确的匹配时，那么可以表示为经过旋转加上误差得到，如果$\left(a_i,b_i\right)$ 是错误匹配时，则$b_i$ 与$a_i$ 没有任何关系。假设误差服从零均值的各向同性高斯分布，则向量组$A,B$ 元素之间的关系可以表示为：

$\{\begin{array}{l}{b}_i=R{a}_i+{ϵ}_i,\left(a_i,b_i\right)是正确匹配\\ b_i=o_i,\left(a_i,b_i\right)是错误匹配\end{array}$ (2)

其中${ϵ}_i~\mathcal{N}\left(0_3,{\sigma }^2{I}_3\right)$ ，$o_i\in {ℝ}^3$ 是$a_i$ 与无关的任意值。此时(1)是关于R的极大似然估计，令${\omega }_i=\frac{1}{\sigma },i=1,\cdots ,N$ 。

3.2. RANSAC算法

RANSAC算法的基本假设是样本中含有正确数据，也包含异常数据，这些异常数据可能是由错误的测量、假设、计算产生，同时RANSAC算法假设给定一组正确的数据，存在可以计算出符合数据的模型参数的方法。其基本方法是考虑一个势为n的抽样集，n小于样本集大小，从样本集中随机抽样作为抽样集，将模型拟合到抽样集，确定参数，然后用拟合模型测试样本收集其他数据，根据某些特定于模型的损失函数，其中很好拟合模型的点被视为共识集合的一部分。重复步骤，共识集越大说明拟合的模型越好。

算法思路：在由向量组$A,B$ 构成的总体${\left\{\left(a_i,b_i\right)\right\}}_{i=i}^N$ 中随机抽取n个作为样本集，在这个样本集的基础上利用奇异值分解(Singular value decomposition，记作SVD)的方法求解最优旋转矩阵R，将R带入到总体${\left\{\left(a_i,b_i\right)\right\}}_{i=i}^N$ ，计算残差$\Vert b_i-R{a}_i\Vert$ 在允许误差$\eta$ 范围内的个数，即${\displaystyle {\sum }_{i=1}^N{\mathbb{I}}_{\left(\Vert b_i-R{a}_i\Vert <\eta \right)}}$ ，其中$\mathbb{I}$ 是指示函数，当$\Vert b_i-R{a}_i\Vert <\eta$ 时，${\mathbb{I}}_{\left(\Vert b_i-R{a}_i\Vert <\eta \right)}=1$ ，否则${\mathbb{I}}_{\left(\Vert b_i-R{a}_i\Vert <\eta \right)}=0$ 。在误差范围内的个数越多，说明旋转矩阵R拟合的越好。这样随机抽取一定次数后，取${\displaystyle {\sum }_{i=1}^N{\mathbb{I}}_{\left(\Vert b_i-R{a}_i\Vert <\eta \right)}}$ 最大的旋转矩阵R作为结果。

3.3. FGR算法

对比RANSAC算法，FGR算法[7]有着不依赖于初始配准的优点。该方法将Wahba问题与罚函数结合，转换为更具鲁棒性的目标函数，即：

$\underset{R\in SO(3)}{\min }{\displaystyle {\sum }_{i=1}^N\rho \left(\Vert b_i-R{a}_i\Vert \right)}$ (3)

其中$\rho \left(x\right)=\frac{\mu x^2}{\mu +x^2}$ 。该罚函数可以抵消异常值对目标函数的影响。然而(2)并不好求解，该算法使用Black-Rangarajan对偶方法将(2)转换为新的目标函数：

$\underset{R\in SO(3),z_i\in [0,1]}{\min }{\displaystyle {\sum }_{i=1}^N{z}_i{\Vert b_i-R{a}_i\Vert }^2+\psi \left(z_i\right)}$ (4)

其中$\psi \left(z_i\right)=\mu {\left(\sqrt{z_i}-1\right)}^2$ 。通过对目标函数进行交替优化的方法，即分别对$R$ 和$z_i$ 进行交替优化，来获得最优解。

固定$R$ 对$z_i$ 进行优化是存在解析解的，即目标函数求关于$z_i$ 的偏导得到：

$z_i={\left(\frac{\mu }{\mu +\Vert b_i-R{a}_i\Vert }\right)}^2$

固定$z_i$ 对$R$ 进行优化，该算法采用线性近似的方式求出，即：

$R\approx \left(\begin{array}{ccc}1& -\gamma & \beta \\ \gamma & 1& -\alpha \\ -\beta & \alpha & 1\end{array}\right)R^k$

其中$R^k$ 是上一次迭代得出的旋转矩阵，则目标函数转换为了关于$\zeta =\left(\alpha ,\beta ,\gamma ,a,b,c\right)$ 的最小二乘问题，使用高斯–牛顿方法求解，从而更新$R$ 。

不断迭代更新$R$ 和${\left\{z_i\right\}}_{i=1}^N$ ，直到达到最大迭代步数。

3.4. 基于四元数的半正定松弛算法[6]

该方法首次提出将Wahba问题转换为截断最小二乘的形式，即：

$\underset{R\in SO(3)}{\min }{\displaystyle {\sum }_{i=1}^N\min \left(\frac{1}{{\sigma }_i^2}{\Vert b_i-R{a}_i\Vert }^2,{\overline{c}}^2\right)}$ (5)

引用最大的误差限制${\overline{c}}^2$ 来很好的减少异常值对结果的影响。

结合单位四元数可以表示旋转，并引入二元数将(4)式变为线性，目标函数可以转化为关于四元数的二次约束二次规划问题：

$\begin{array}{l}\underset{x\in {ℝ}^{4(N+1)}}{\min }{\displaystyle {\sum }_{i=1}^N{x}^{\text{T}}{Q}_{i}x}\\ s.t.\begin{array}{cc}& x_q^{\text{T}}{x}_q=1\end{array}\\ \begin{array}{ccc}& & x_{q_i}{x}_{q_i}^{\text{T}}=x_q^{\text{T}}{x}_q,\forall i=1,\cdots ,N\end{array}\end{array}$ (6)

其中四元数$q=\left[q_1,q_2,q_3,q_4\right]\in {ℝ}^4$ ，$q_i\in \left\{q,-q\right\}\left(i=1,\cdots ,N\right)$ ，而$x={\left[q^{\text{T}},q_1^{\text{T}},\cdots ,q_N^{\text{T}}\right]}^{\text{T}}$ ，$x_q$ 表示$x$ 中对应$q$ 的四维向量。$Q_i\in {ℝ}^{4(N+1)\times 4(N+1)}\left(i=1,\cdots ,N\right)$ 是由$a_i,b_i$ 确定的已知矩阵。“$\otimes$ ”表示四元数$q$ 的乘法，两个四元数相乘，即$q_a\otimes q_b$ ，可以表示为：

$q_a\otimes q_b={\Omega }_1\left(q_a\right)q_b=q_a{\Omega }_2\left(q_b\right)$

其中

${\Omega }_1\left(q\right)=\left[\begin{array}{cccc}{q}_4& -q_3& q_2& q_1\\ q_3& q_4& -q_1& q_2\\ -q_2& q_1& q_4& q_3\\ -q_1& -q_2& -q_3& q_4\end{array}\right],{\Omega }_2\left(q\right)=\left[\begin{array}{cccc}{q}_4& q_3& -q_2& q_1\\ -q_3& q_4& q_1& q_2\\ q_2& -q_1& q_4& q_3\\ -q_1& -q_2& -q_3& q_4\end{array}\right]$

而旋转矩阵与四元数之间的相互转化可以表示为：

$\left[\begin{array}{c}R{a}_i\\ 0\end{array}\right]=q\otimes {\widehat{a}}_i\otimes q$

其中${\widehat{a}}_i={\left[a_i^{\text{T}},0\right]}^{\text{T}}$

该算法令$Z=x{x}^{\text{T}}$ ，(5)式转换为：

$\begin{array}{l}\underset{Z\succ 0}{\min }\begin{array}{cc}& tr\left(QZ\right)\end{array}\\ s.t.\begin{array}{cc}& tr\left({\left[Z\right]}_{qq}\right)=1\end{array}\\ \begin{array}{cc}\begin{array}{cc}& \end{array}& {\left[Z\right]}_{q_i{q}_i}=\end{array}{\left[Z\right]}_{qq},\forall i=1,\cdots ,N\\ \begin{array}{ccc}& & rank\left(Z\right)=1\end{array}\end{array}$ (7)

添加虚约束并使用半正定松弛的方法将问题转化为了凸优化问题：

$\begin{array}{l}\underset{Z\succ 0}{\min }\begin{array}{cc}& tr\left(QZ\right)\end{array}\\ s.t.\begin{array}{cc}& tr\left({\left[Z\right]}_{qq}\right)=1\end{array}\\ \begin{array}{cc}\begin{array}{cc}& \end{array}& {\left[Z\right]}_{q_i{q}_i}=\end{array}{\left[Z\right]}_{qq},\forall i=1,\cdots ,N\\ \begin{array}{cc}\begin{array}{cc}& \end{array}& {\left[Z\right]}_{q{q}_i}=\end{array}{\left[Z\right]}_{q{q}_i}^{\text{T}},\forall i=1,\cdots ,N\\ \begin{array}{cc}\begin{array}{cc}& \end{array}& {\left[Z\right]}_{q_i{q}_j}=\end{array}{\left[Z\right]}_{q_i{q}_j}^{\text{T}},\forall 1\le i\le j\le N\end{array}$ (8)

利用求解器求解。

3.5. 新算法

利用单位四元数表示旋转的方法，将Wahba问题转换为：

$\underset{q\in S^3}{\min }{\displaystyle {\sum }_{i=1}^N\frac{1}{{\sigma }^2}{\Vert {\widehat{b}}_i-q\otimes {\widehat{a}}_i\otimes q^{-1}\Vert }^2}$ (9)

其中$S^3=\left\{q\in {ℝ}^4:\Vert q\Vert =1\right\}$ 表示单位四元数的集合。而式(8)可以表示为关于$q$ 的二次约束二次规划问题：

$\begin{array}{l}\underset{q\in {ℝ}^4}{\min }{\displaystyle {\sum }_{i=1}^N{q}^{\text{T}}{Q}_{i}q}\\ \begin{array}{ccc}s.t.& & q^{\text{T}}q=1\end{array}\end{array}$ (10)

其中

$Q_i=q^{\text{T}}\left(\frac{\left({\Vert {\widehat{b}}_i\Vert }^2+{\Vert {\widehat{a}}_i\Vert }^2\right)I_4+2{\Omega }_1\left({\widehat{b}}_i\right){\Omega }_2\left({\widehat{a}}_i\right)}{2{\sigma }^2}\right)q,i=1,\cdots ,N$

令$Z=q{q}^{\text{T}}$ ，则(9)可以表示为：

$\begin{array}{l}\underset{Z\succ 0}{\min }\begin{array}{cc}& tr\left(QZ\right)\end{array}\\ s.t.\begin{array}{ccc}& tr\left(Z\right)=1& \end{array}\\ \begin{array}{ccc}& & rank\left(Z\right)=1\end{array}\end{array}$ (11)

经过半正定松弛，(10)松弛为凸优化问题：

$\begin{array}{l}\underset{Z\succ 0}{\min }\begin{array}{cc}& tr\left(QZ\right)\end{array}\\ s.t.\begin{array}{cc}& tr\left(Z\right)=1\end{array}\end{array}$ (12)

为减少异常值对算法的影响，该算法将会在重复的步骤中将不合理的$\left(a_i,b_i\right)$ 去除。

算法步骤：

1、初始化：数据集$\mathcal{C}={\left\{\left(a_i,b_i\right)\right\}}_{i=1}^N$

2、在数据集$\mathcal{C}$ 中，求解(12)，计算得出最优旋转矩阵$R_1$ ；

3、求出最大残差对应的向量$\left(a_m,b_m\right)=\underset{(a_i,b_i)\in \mathcal{C}}{\arg \max }\Vert b_i-R_1{a}_i\Vert$

4、在数据$\mathcal{C}-\left\{\left(a_m,b_m\right)\right\}$ 中，求解(12)，计算求出最优旋转矩阵$R_2$ ；

5、求出最大残差对应的向量$\left(a_m,b_m\right)=\underset{(a_i,b_i)\in \mathcal{C}-\{(a_m,b_m)\}}{\arg \max }\Vert b_i-R_2{a}_i\Vert$

6、如果$m$ 出现两次使得残差最大，则$\mathcal{C}=\mathcal{C}-\left\{\left(a_m,b_m\right)\right\}$ ，跳转步骤2，否则跳转步骤4。

7、重复上述过程直到$\underset{(a_i,b_i)\in \mathcal{C}}{\max }\Vert b_i-R_1{a}_i\Vert <\eta$ ，$\eta =1\times {10}^{-6}$

在收敛性上，每次迭代，算法通过求解优化问题(12)得到当前数据集中的最优旋转矩阵，因为问题(12)是凸松弛后的半正定规划，求得的解是全局最优。所以当去除残差最大的样本后，解空间更贴近真实分布，新的最优解将针对剩余数据集进行优化，其残差平方和不会劣于原数据集的最优值。在有限异常值假设的情况下，该算法通过逐步剔除异常值优化数据集，目标函数保证单调递减，最终可以在有限步内满足终止条件。

在复杂度上，每次迭代都要进行一次求解半正定规划问题以及进行残差计算，需要进行$O\left(N\right)$ ，在最坏情况下总共进行$O\left(N\right)$ 。所以总时间复杂度为$O\left(N^2\right)$ 。

该算法对比QUASAR算法，在求解半正定松弛问题上减小了目标函数的复杂性，从而提高算法的运行速度。而对比FGR算法和RANSAC算法，其全局优化特性显著提升了精度与稳定性，为需要抗噪与抗异常值的问题提供了更具鲁棒性的解决方案。

4. 算法对比

4.1. 数据选取

随机生成N = 40个服从均匀分布且大小为1的单位向量作为向量集合A，随机生成正交矩阵$R\in SO\left(3\right)$ 以及误差向量$ϵ~\mathcal{N}\left(0_3,{\sigma }^2{I}_3\right)$ ，其中根据误差的干扰程度，我们分为低噪声和高噪声，分别对应${\sigma }_1=0.01$ 和${\sigma }_2=0.1$ 。根据$b_i=R{a}_i+ϵ$ ，生成对应的向量集合B，此时B是有误差干扰但没有异常值的情况。按照一定异常值比例$\alpha$ ($\alpha =0.2,0.4,0.6$ )将B中向量替换成随机生成的且大小为1的向量，分别模拟出不同异常值干扰程度的情况。为了避免仿真结果的随机性，以上四种算法将会进行40次实验，分别计算失准角误差和统计运行时间，并对结果进行对比。

4.2. 算法对比

在算法性能的对比上，本文采用计算误差和统计计算时间的方式，从精度和处理时间的角度对比算法性能。经过计算机模拟，表1展示了四种算法在低噪音情况下不同异常值干扰情况下的误差情况。

Table 1. Under low noise condition ($\sigma =0.01$ ) with varying levels of outlier interference

表1. 低噪声情况下($\sigma =0.01$ )不同异常值干扰程度的平均误差与标准差

异常值比例	QUASAR	FGR	RANSAC	新算法
0	0.0037 ± 0.002	1.2827 ± 0.3407	0.0581 ± 0.0047	0.0036 ± 0.0020
20%	0.0037 ± 0.0021	1.1724 ± 0.3447	0.8844 ± 0.3691	0.1886 ± 0.0506
40%	0.0044 ± 0.0026	1.0663 ± 0.5910	0.9658 ± 0.2152	0.5565 ± 0.2029
60%	0.0062 ± 0.0027	1.7776 ± 0.6369	0.9789 ± 0.1581	1.0780 ± 0.4088

由表1可以看出，在低噪音和低异常值比例的情况下，QUASAR算法和新算法的误差最小，且稳定性强，其次是RANSAC算法，FGR表现最差，且随着异常值干扰程度的增加，QUASAR算法几乎不受干扰，而RANSAC算法和新算法有明显的干扰。

Table 2. Under high noise condition ($\sigma =0.01$ ) with varying levels of outlier interference

表2. 高噪声情况下($\sigma =0.01$ )不同异常值干扰程度的平均误差与标准差

异常值比例	QUASAR	FGR	RANSAC	新算法
0	0.0449 ± 0.0195	1.2989 ± 0.2996	0.1109 ± 0.0429	0.0449 ± 0.0195
20%	0.0496 ± 0.0251	1.2082 ± 0.2932	0.5447 ± 0.5409	0.2164 ± 0.0846
40%	0.0593 ± 0.0277	1.2058 ± 0.6667	0.9599 ± 0.8037	0.5982 ± 0.1909
60%	0.0742 ± 0.0463	1.7989 ± 0.6067	1.1889 ± 0.7310	1.1588 ± 0.4307

Table 3. Computation time

表3. 计算时间(秒/s)

QUASAR	FGR	RANSAC	新算法
5.6399	0.0014	0.0185	0.0888

由表2可以看出，在高噪音但异常值干扰程度低情况下，QUASAR算法和新算法的误差最小，稳定性强，其次是RAN，可以看出QUASAR和新算法在抗噪能力上比另外两种算法强，但随着异常值比例的提高，新算法明显受到干扰，结果表现上不如QUASAR算法稳定。

但在表3中显示，尽管QUASAR算法在精度上表现优秀，但在计算时间上，远高于其余三种算法。

综合以上算法对比，在不同的应用场景下可以选择不同的算法，如表4显示。

Table 4. Usage scenario recommendation

表4. 使用场景推荐

场景类型	推荐算法	理由
高精度、低实时性	QUASAR	误差最小且稳定
中精度，实时性	新算法	在低异常值下接近QUASAR精度，时间可接受
低精度、强实时性	RANSAC	快速提出异常值
超快计算、容忍误差	FGR	计算快

4.3. 结论

从上述对比中可以看出，QUASAR算法和新算法在抗噪性上比另外两种算法表现更好，但QUASAR在鲁棒性比新算法更强，结果也更加精准且稳定，但计算时间较长。提出的新算法在低噪音和低异常值比率下，计算结果准确，但随着异常值比率的增加，其计算结果受到影响而表现变差，且稳定性变差，其鲁棒性明显不如QUASAR算法。RANSAC算法在低噪音低异常值比率的情况下，计算的误差小，结果较为准确，但抗干扰能力不强，随着噪声和异常值的影响增大，结果明显变差。但在计算时间上新算法和RANSAC算法比QUASAR算法更快，在实际应用中，应结合应用问题的背景需要选取合适的算法，如表4，在精度、鲁棒性和计算时间上找到最佳算法。

5. 结语

Wahba问题旨在求解两个向量集合A，B间的最优旋转矩阵，进行姿态估计，是航天、计算机视觉等领域的基本问题。在实际问题中，向量集合之间的匹配通常会受到误差和错误匹配的干扰本文对QUASAR算法、FGR算法、RANSAC算法进行了对比，通过计算机仿真，分析不同算法在不同噪声情况和异常值干扰程度下的计算精度。

实验结果表示，QUASAR算法在抗噪声和异常值干扰方面表现优于其他算法。尽管FGR算法虽然整体上误差偏大，但在提高噪声和异常值干扰下，对比RANSAC算法更加稳定，RANSAC算法在低噪声和低异常值的情况下，结果更加准确，但当提高噪声和异常值干扰时，结果会受到比较大的影响，表现变差。但在实际应用中，算法的选取除了考虑算法的计算精度和鲁棒性，还需要考虑计算时间。经过对比FGR算法和RANSAC算法明显快于QUASAR算法。

在QUASAR算法的启发下，本文提出了一种新的求解存在噪声和异常值Wahba问题的算法。在同样的实验环境下，实验结果显示，新算法在低噪音和低异常值比率的情况下，准确度明显高于FGR算法和RANSAC算法，与QUASAR算法相近，然而，随着异常值比率的增加，新算法得到的误差变大，结果也较为不准确。未来的工作将重点提升新算法的性能，特别是在高异常值干扰的情况下，提高其抗干扰能力和计算精度。

在实际应用中，选择合适的算法应考虑具体的应用场景和需求，从而在精度、鲁棒性和计算时间之间找到最佳平衡。