MATLAB：我的 quatrotate 脚本有什么问题？

Question

我的 MATLAB 版本不包含四轮旋转函数，因此我使用 MathWorks 提供的等式编写了自己的函数 here。问题是，我没有得到他们在我的函数示例中或在我手动计算时得到的相同答案。

在他们的示例下，如果我输入以下内容，我应该得到一个 n 向量 [-1 1 1]:

q = [1 0 1 0]; r = [1 1 1]; n = quatrotate(q, r)

n =

-1.0000 1.0000 1.0000

在我的函数中，我手动得到：

[-3 1 1]

我在这里错过了什么？我搜索得越多，我就越困惑。据我所知，答案应该是 [-3 1 1].

这是我写的函数：

function [n] = quatrotate(q,r)
%Rotate a given acceleration vector by a given quaternion
%
%Inputs:
% q:      A matrix containing a set of quaternion vectors of the 
%         form q = [w,x,y,z]
% r:      A matrix containing a set of linear acceleration vectors 
%         of the form r= [i,j,k] (also known as [x,y,z])
% 
% Outputs:
% n:      The solved matrix containing the rotated vector of each linear 
%         acceleration component
% 

%This assumes that the quaternion is normalised (sqw + sqx + sqy + sqz =1), 
%if not it should be normalised before doing the conversion. 
%To normalise divide qx, qy, qz and qw by n where n=sqrt(qx2 + qy2 + qz2 + qw2)


 for k = 1:size(q,1)
     rot=[(1-2.*q(k,3).^2-2.*q(k,4).^2) 2.*(q(k,2).*q(k,3)+q(k,1).*q(k,4))...
          2.*(q(k,2).*q(k,4)-q(k,1).*q(k,3));2.*(q(k,2).*q(k,3)-q(k,1).*q(k,4))...
          (1-2.*q(k,2).^2-2.*q(k,4).^2) 2.*(q(k,3).*q(k,4)+q(k,1).*q(k,2));...
         2.*(q(k,2).*q(k,4)+q(k,1).*q(k,3)) 2.*(q(k,3).*q(k,4)-q(k,1).*q(k,2))...
          (1-2.*q(k,2).^2-2.*q(k,3).^2)];
     n(k,:) = rot*r(k,:)';
 end

提前致谢！

Answer 1

首先你需要计算给定四元数的模q:

for index = size(q,1):-1:1
    mod(index,:) = norm(q(index,:),2);
end

然后归一化：
```
qn = q./(mod* ones(1,4));
```
现在使用这些公式计算正余弦矩阵：

    dcm = zeros(3,3,size(qn,1));
    dcm(1,1,:) = qn(:,1).^2 + qn(:,2).^2 - qn(:,3).^2 - qn(:,4).^2;
    dcm(1,2,:) = 2.*(qn(:,2).*qn(:,3) + qn(:,1).*qn(:,4));
    dcm(1,3,:) = 2.*(qn(:,2).*qn(:,4) - qn(:,1).*qn(:,3));
    dcm(2,1,:) = 2.*(qn(:,2).*qn(:,3) - qn(:,1).*qn(:,4));
    dcm(2,2,:) = qn(:,1).^2 - qn(:,2).^2 + qn(:,3).^2 - qn(:,4).^2;
    dcm(2,3,:) = 2.*(qn(:,3).*qn(:,4) + qn(:,1).*qn(:,2));
    dcm(3,1,:) = 2.*(qn(:,2).*qn(:,4) + qn(:,1).*qn(:,3));
    dcm(3,2,:) = 2.*(qn(:,3).*qn(:,4) - qn(:,1).*qn(:,2));
    dcm(3,3,:) = qn(:,1).^2 - qn(:,2).^2 - qn(:,3).^2 + qn(:,4).^2;

根据MATLAB文档，一个向量r通过计算得到dcm的旋转可以得到如下：

if ( size(q,1) == 1 ) 
    % Q is 1-by-4
    qout = (dcm*r')';
elseif (size(r,1) == 1) 
    % R is 1-by-3
    for i = size(q,1):-1:1
        qout(i,:) = (dcm(:,:,i)*r')';
    end
else
    % Q is M-by-4 and R is M-by-3
    for i = size(q,1):-1:1
        qout(i,:) = (dcm(:,:,i)*r(i,:)')';
    end
end

Answer 2

首先，为了使 quatrotate 工作，您需要使用单位四元数（即长度为 1）。

其次，我看到您使用的是MATLAB页面提供的矩阵。我最近自己推导了那个矩阵，发现 MATLAB 页面有错误的矩阵。

根据this (page 45)，将向量旋转四元数是

p' = p + 2w(v × p)+2(v × (v × p))

其中，

p'为旋转后的输出向量，
p是要旋转的起始向量，
w是四元数中的第一个系数，
v是四元数的[第二个系数，第三个系数，第四个系数]的向量，
×是叉积操作数。

我鼓励你去 link 自己推导矩阵。你会看到MATLAB页面提供的矩阵加减错了

这是 MATLAB 页面上所说的：

这是我的推导（这里的四元数是[q0, q1, q2, q3]，向量是[x, y, z]）：

第一行：

第二行：

第三行：

这里可以看到MATLAB网站上的标志是不正确的。我已通过电子邮件将错误告知他们，正在等待回复。

MATLAB：我的 quatrotate 脚本有什么问题？

MATLAB: What's wrong with my quatrotate script?

matlab

vector

quaternions