我在 MATLAB 中遇到一些非常奇怪的情况,我无法弄清楚。
我有两组物体坐标,我想找到它们的欧拉角。我设置了旋转矩阵 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
我当前的解决方案是手动计算角度,这很好,但我对上面的方法有什么问题感兴趣。
最佳答案
问题在于您要解决精确匹配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
获得)几乎与R2
相同,但在最低有效数字上有一些差异:
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
关于MATLABsolve() 无法求解特定值的三角矩阵,我们在Stack Overflow上找到一个类似的问题: https://stackoverflow.com/questions/45714234/