zoukankan      html  css  js  c++  java
  • MATLAB 旋转矩阵、欧拉角、四元数互转

    欧拉角转旋转矩阵公式:

    旋转矩阵转欧拉角公式:

    旋转矩阵转四元数公式,其中1+r11+r22+r33>0:

    四元数转旋转矩阵公式,q0^2+q1^2+q2^2+q3^2=1:

    欧拉角转四元数公式:

    四元数转欧拉角公式:

    matlab代码如下:

     1 clear all;
     2 close all;
     3 clc;
     4 
     5 %欧拉角
     6 x = 0.5;
     7 y = 0.6;
     8 z = 0.7;
     9 Ang1 = [x y z];
    10 
    11 %欧拉角转旋转矩阵
    12 Rx = [1      0      0;
    13     0 cos(x) -sin(x);
    14     0 sin(x) cos(x)];
    15 Ry = [cos(y)  0 sin(y);
    16     0       1      0;
    17     -sin(y) 0 cos(y)];
    18 Rz = [cos(z) -sin(z) 0;
    19     sin(z) cos(z)  0;
    20     0      0       1];
    21 R = Rz*Ry*Rx;
    22 R1 = R;
    23 
    24 %旋转矩阵转欧拉角
    25 x = atan2(R(3,2),R(3,3));
    26 y = atan2(-R(3,1),sqrt(R(3,2)^2+R(3,3)^2));
    27 z = atan2(R(2,1),R(1,1));
    28 Ang2 = [x y z];
    29 
    30 %旋转矩阵转四元数
    31 t=sqrt(1+R(1,1)+R(2,2)+R(3,3))/2;
    32 q=[t (R(3,2)-R(2,3))/(4*t) (R(1,3)-R(3,1))/(4*t) (R(2,1)-R(1,2))/(4*t)];
    33 Q1 = q;
    34 
    35 %四元数转旋转矩阵
    36 R=[ 2*q(1).^2-1+2*q(2)^2    2*(q(2)*q(3)-q(1)*q(4)) 2*(q(2)*q(4)+q(1)*q(3));
    37     2*(q(2)*q(3)+q(1)*q(4)) 2*q(1)^2-1+2*q(3)^2     2*(q(3)*q(4)-q(1)*q(2));
    38     2*(q(2)*q(4)-q(1)*q(3)) 2*(q(3)*q(4)+q(1)*q(2)) 2*q(1)^2-1+2*q(4)^2];
    39 R2 = R;
    40 
    41 %欧拉角转四元数
    42 q = [cos(x/2)*cos(y/2)*cos(z/2) + sin(x/2)*sin(y/2)*sin(z/2) ...
    43     sin(x/2)*cos(y/2)*cos(z/2) - cos(x/2)*sin(y/2)*sin(z/2) ...
    44     cos(x/2)*sin(y/2)*cos(z/2) + sin(x/2)*cos(y/2)*sin(z/2) ...
    45     cos(x/2)*cos(y/2)*sin(z/2) - sin(x/2)*sin(y/2)*cos(z/2)];
    46 Q2 = q;
    47 
    48 %四元数转欧拉角
    49 x = atan2(2*(q(1)*q(2)+q(3)*q(4)),1 - 2*(q(2)^2+q(3)^2));
    50 y = asin(2*(q(1)*q(3) - q(2)*q(4)));
    51 z = atan2(2*(q(1)*q(4)+q(2)*q(3)),1 - 2*(q(3)^2+q(4)^2));
    52 Ang3 = [x y z];
    53 
    54 Ang1
    55 Ang2
    56 Ang3
    57 
    58 R1
    59 R2
    60 
    61 Q1
    62 Q2
  • 相关阅读:
    导出数据到Excel
    VB中导入数据到Excel内容换行
    上传附件导入,确认弹框取消后,无法上传附件
    扫描审核,出错声音报警
    C语言中,隐藏结构体的细节
    C语言 符号的不同意义
    IAR 和 keil 使用外部 SDRAM 的区别
    C语言 ringBuffer 实现
    C语言 malloc 内存泄漏
    STM32 硬件CRC和软件CRC速度比较
  • 原文地址:https://www.cnblogs.com/ybqjymy/p/13645549.html
Copyright © 2011-2022 走看看