MATLAB solve() 无法求解特定值的三角矩阵

Question

我在 MATLAB 中遇到了一些我无法理解的非常奇怪的事情。

我有 2 组物体坐标，我想找到它们的欧拉角。我设置了旋转矩阵 R 和符号万向节角矩阵 R_cardan 来求解角度。对于第一组我可以简单地做 solve(R==R_cardan) 但是当我使用第二组时它不起作用。返回的解决方案是空的。

这可能是什么原因造成的？这是测试代码。

clc;clear;close all;

%% For some reason R2 does not want to give a solution
% Find rotation matrix to transform from origin to local reference frame
ex = [ 0.768  0.024 0.640].';
ey = [-0.424  0.768 0.480].';
ez = [-0.480 -0.640 0.600].';

ex2 = [ 0.612372  0.353553 -0.707107].';
ey2 = [0.280330  0.739199 0.612372].';
ez2 = [0.739199 -0.573223 0.353553].';

R = eye(3)*[ex ey ez]
R2 = eye(3)*[ex2 ey2 ez2]
% Symbolic variables
syms beta alpha gamma

% Set up rotatin matrices
R_alpha = [cos(alpha) -sin(alpha) 0; sin(alpha) cos(alpha) 0; 0 0 1]
R_beta = [cos(beta) 0 sin(beta); 0 1 0; -sin(beta) 0 cos(beta)]
R_gamma  = [1 0 0; 0 cos(gamma) -sin(gamma); 0 sin(gamma) cos(gamma)]

% Find symbolic rotation matrix
R_cardan = R_alpha*R_beta*R_gamma

[alpha, beta, gamma] = find_angles(R,R_cardan)
[alpha, beta, gamma] = find_angles(R2,R_cardan) %fails because solution is empty

function [alpha, beta, gamma] = find_angles(R,R_cardan)

% Solve for the angles
sol = solve(R == R_cardan);
alpha = double(sol.alpha(1));
beta = double(sol.beta(1));
gamma = double(sol.gamma(1));

end

我目前的解决方案是手动计算角度，这很好，但我想知道我上面的方法有什么问题。

Answer 1

问题将是您求解精确匹配R == R_cardan。当您使用有限精度工作时，这很危险，并且可能没有解决方案，就像 R2 发生的那样。您的目标是找到一个解决方案，使差异 R - R_cardan 非常小 - 理想情况下为零。

我会改为执行以下操作：创建损失函数

并最小化此功能。 R = R_cardan 处的理想解决方案会导致零损失，但即使这在数值上是不可能的，您也会得到一个尽可能接近最佳解决方案的解决方案（根据欧氏距离）。

在 MATLAB 中，这有点复杂，但在 help pages 中有很好的描述。

根据R和R_cardan定义损失函数，并将所有未知数放入向量x:
```
f = sum(sum((R - R_cardan).^2));
x = [alpha; beta; gamma];
```

解析计算损失函数的梯度和Hessian：

gradf = jacobian(f, x).'; % column gradf
hessf = jacobian(gradf, x);

将这些函数从符号函数转换为 MATLAB 函数句柄：
```
fh = matlabFunction(f, gradf, hessf, 'vars', {x});
```

设置优化器以使用梯度和 Hessian：

options = optimoptions('fminunc', 'SpecifyObjectiveGradient', true, ... 
                                  'HessianFcn', 'objective', ...
                                  'Algorithm', 'trust-region')

最小化！

solution = fminunc(fh, [0;0;0], options);
alpha_hat = solution(1);
beta_hat = solution(2);
gamma_hat = solution(3);

对于第一个示例 R，这给出了与 solve 完全相同的解决方案。对于第二个例子R2，重建矩阵R2_hat（通过将估计值代入R_cardan获得）几乎与[=17]相同=]，但在最低有效数字上有一些差异：

R2 =
    0.6124    0.2803    0.7392
    0.3536    0.7392   -0.5732
   -0.7071    0.6124    0.3536

R2_hat =
    0.6125    0.2805    0.7390
    0.3533    0.7392   -0.5734
   -0.7071    0.6123    0.3537

MATLAB solve() 无法求解特定值的三角矩阵

MATLAB solve() can't solve trigonometric matrix for specific values

matlab

symbolic-math

euler-angles