用最小二乘法求解 A * w = b 的 QR 因子分解答案

【问题标题】：QR-Factorization in least square sense to solve A * w = b用最小二乘法求解 A * w = b 的 QR 因子分解
【发布时间】：2021-01-10 18:42:48
【问题描述】：

我正在尝试实施 QR24 算法来校准来自 Floris Ernst（2012 年）的 paper 的法兰/工具和机器人/世界。

我需要求解一个方程M_i*X - Y*N_i = 0，其中M_i 和N_i 是已知的，从1 到测量次数的i 和X 和Y 是未知矩阵。

在论文中，他们将这个方程组合成一个线性方程组A*w = b，其中 A 由 12* 个测量行和 24 列组成，所以我有一个具有 24 个参数的线性方程组，我需要在至少 2 次测量来解决这个系统。

为了求解这个方程，我需要在最小二乘意义上使用 QR 因子分解，因为测量次数越多，这个系统的方程就越多。

我正在使用 Apache Commons Math 库中的 OLSMultipleLinearRegression 来求解方程系统：

OLSMultipleLinearRegression regression = new OLSMultipleLinearRegression();
regression.setNoIntercept(true);
regression.newSampleData(B.toArray(), A.getData());
RealVector w = new ArrayRealVector(regression.estimateRegressionParameters());

RealVector w 现在应该包含未知矩阵 X 和 Y 的条目（没有最后一行，始终是 [0 0 0 1]，因为这些矩阵是齐次变换矩阵）。

我使用 Denavit-Hartenberg 在纸上手工生成了一些测试测量数据，因为由于电晕，我目前无法访问我想使用的机器人和跟踪系统。

但是，我得到的结果 X 和 Y 矩阵（向量 w）总是如此荒谬，与我期望的结果相去甚远。例如，当我使用没有任何平移或旋转误差（除了我的计算机的计算错误）的精确变换矩阵时，我得到的矩阵的旋转部分值超过 10^14（这显然不可能）和翻译部分超过 10^17 而不是预期的 100 左右。

当我在我的矩阵中添加一些测量误差时（例如，旋转 +-0.01°，平移部分 +-0.01），我没有得到那些超高的值，但旋转部分的值仍然可以'不是真的。

你有什么想法，为什么这些值非常错误，或者有什么建议可以在这个库中使用 QR 分解吗？

这也是我使用 M_i 和 N_i 测量创建 A 的每个条目/子矩阵 Ai 的代码：

private RealMatrix createAi(RealMatrix m,RealMatrix n, boolean invert) {
    RealMatrix M = new Array2DRowRealMatrix();
    if(invert) {
         M = new QRDecomposition(m).getSolver().getInverse();
    }else {
        M = m.copy();
    }
    // getRot is a method i wrote to get the rotational part of a matrix
    RealMatrix RM = getRot(M);
    
    RealMatrix N = n.copy();
    // 12 equations per Measurement and 24 parameters to solve for
    RealMatrix Ai = new Array2DRowRealMatrix(12,24);
    RealMatrix Zero = new Array2DRowRealMatrix(3,3);
    RealMatrix Identity12 = MatrixUtils.createRealIdentityMatrix(12);
    
    // first column
    Ai.setSubMatrix(RM.scalarMultiply(N.getEntry(0, 0)).getData(), 0, 0);
    Ai.setSubMatrix(RM.scalarMultiply(N.getEntry(0, 1)).getData(), 3, 0);
    Ai.setSubMatrix(RM.scalarMultiply(N.getEntry(0, 2)).getData(), 6, 0);
    Ai.setSubMatrix(RM.scalarMultiply(N.getEntry(0, 3)).getData(), 9, 0);
                                                       
    // secondcolumn
    Ai.setSubMatrix(RM.scalarMultiply(N.getEntry(1, 0)).getData(), 0, 3);
    Ai.setSubMatrix(RM.scalarMultiply(N.getEntry(1, 1)).getData(), 3, 3);
    Ai.setSubMatrix(RM.scalarMultiply(N.getEntry(1, 2)).getData(), 6, 3);
    Ai.setSubMatrix(RM.scalarMultiply(N.getEntry(1, 3)).getData(), 9, 3);

    // third column
    Ai.setSubMatrix(RM.scalarMultiply(N.getEntry(2, 0)).getData(), 0, 6);
    Ai.setSubMatrix(RM.scalarMultiply(N.getEntry(2, 1)).getData(), 3, 6);
    Ai.setSubMatrix(RM.scalarMultiply(N.getEntry(2, 2)).getData(), 6, 6);
    Ai.setSubMatrix(RM.scalarMultiply(N.getEntry(2, 3)).getData(), 9, 6);
    
    // fourth column
    Ai.setSubMatrix(Zero.getData(), 0, 9);
    Ai.setSubMatrix(Zero.getData(), 3, 9);
    Ai.setSubMatrix(Zero.getData(), 6, 9);
    Ai.setSubMatrix(RM.getData(), 9, 9);
    
    // fifth column
    Ai.setSubMatrix(Identity12.scalarMultiply(-1d).getData(), 0, 12);
    return Ai;
}

这是我使用 M_i 测量创建 b 的每个条目/子向量 bi 的代码：

private RealVector createBEntry(RealMatrix m, boolean invert) {
    RealMatrix bi = new Array2DRowRealMatrix(1,12);
    RealMatrix negative_M = new Array2DRowRealMatrix();
    // getTrans is a method i wrote to get the translational part of a matrix
    if(invert) {
        negative_M = getTrans(new QRDecomposition(m).getSolver().getInverse()).scalarMultiply(-1d); 
    }else {
        negative_M = getTrans(m).scalarMultiply(-1d);
    }       
    bi.setSubMatrix(negative_M.getData(), 0, 9);
    
    return bi.getRowVector(0);
}

【问题讨论】：

标签： java math least-squares apache-commons-math

【解决方案1】：

我找到了解决问题的方法，我想与您分享。问题不是编程错误，而是论文提供了一个不正确的矩阵（Ai 矩阵），这是求解线性方程组所需的。我尝试利用齐次变换矩阵和旋转矩阵的特性，自己从M*X - Y*N = 0中提取出一个线性方程组。我想出了以下解决方案：

在哪里

论文中提供的向量bi很好。

由于 Ernst 教授在我的大学任教，我正在和他一起上课，我会努力让他意识到这个错误。

【讨论】：