MATLAB:我的四分体脚本有什么问题?

时间:2017-03-07 04:19:01

标签: matlab vector quaternions

我的MATLAB版本没有包含quatrotate函数,因此我使用MathWorks提供的{$ 3}等式编写了自己的函数。麻烦的是,我没有在我的功能中得到他们在他们的例子中得到的相同答案,或者当我手工计算它时。

在他们的例子中,如果我输入以下内容,我应该得到一个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

提前致谢!

2 个答案:

答案 0 :(得分:1)

  1. 首先,您需要计算给定四元数q的模数:

    for index = size(q,1):-1:1
        mod(index,:) = norm(q(index,:),2);
    end
    
  2. 然后将其标准化:

    qn = q./(mod* ones(1,4));
    
  3. 现在使用以下公式计算直接余弦矩阵:

  4. enter image description here

        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;
    
    1. 根据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
      

答案 1 :(得分:0)

首先,为了使quatrotate工作,你需要使用单位四元数(即长度为1)。

其次,我看到你正在使用MATLAB页面提供的矩阵。我最近自己派生了这个矩阵,发现MATLAB页面有错误的矩阵。

根据this (page 45),通过四元数旋转矢量

  

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

其中,

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

我鼓励您转到该链接并自行推导出矩阵。您将看到MATLAB页面上提供的矩阵具有错误的加法和减法。

这就是MATLAB页面上的内容:

Matrix on MATLAB's page

这是我的推导(这里四元数是[q0,q1,q2,q3],向量是[x,y,z]):

第一行:

First row of my derivation

第二行:

Second row of my derivation

第三行:

Third row of my derivation

在这里你可以看到MATLAB网站上的标志是不正确的。我已经通过电子邮件向他们发送了有关错误的信息,并等待收到回复。