最小二乘意义上的 QR 分解来求解 A * w = b

Question

我正在尝试实施 QR24 算法以根据 Floris Ernst（2012 年）paper 校准 flange/tool 和 robot/world。

我需要求解一个方程 M_i*X - Y*N_i = 0，其中 M_i 和 N_i 是已知的，我从 1 到测量次数，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 的每个 entry/submatrix 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 度量创建每个 entry/subvector 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);
}

Answer 1

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

在哪里

论文中提供的向量bi就可以了

既然 Ernst 教授在我的大学任教，而我正在和他一起上课，我会尽量让他意识到这个错误。

最小二乘意义上的 QR 分解来求解 A * w = b

QR-Factorization in least square sense to solve A * w = b

java

math

least-squares

apache-commons-math