计算角速度

最新推荐文章于 2023-09-18 18:58:12 发布

小代码大用处

最新推荐文章于 2023-09-18 18:58:12 发布

阅读量920

点赞数

分类专栏：机器人文章标签：矩阵线性代数

本文链接：https://blog.csdn.net/qq_34317499/article/details/127010462

版权

机器人专栏收录该内容

4 篇文章 1 订阅

订阅专栏

% 测试四元数
% Quaternion  2022.09.23
% 小角度才可以这样算
clc  
clear
cycletime=0.004;
R0=rpy2r(pi/4, pi/6, pi/3,'zyx');
R1=R0*rpy2r(0, 0, 0.001,'zyx');  
% 1、通过旋转矩阵获得旋转角速度
delta_rm = R1*R0'-diag([1,1,1]);
delta_w = [-delta_rm(2,3),delta_rm(1,3),-delta_rm(1,2)];
Vel_w1 = delta_w./ cycletime %差分获得的空间旋转速度
Vel_w1 = 1×3
    0.1973   -0.0118    0.1531
% 2.1、通过四元数获得旋转角速度
q0 = UnitQuaternion(R0);
q1 = UnitQuaternion(R1);
dq_dt=(q1-q0)/cycletime;
% 参考：https://blog.csdn.net/CAIYUNFREEDOM/article/details/53738090
q=double(q0);
dq_dt_value=double(dq_dt);
mat=[-2*q(2) 2*q(1) -2*q(4) 2*q(3);
    -2*q(3) 2*q(4) 2*q(1) -2*q(2);
    -2*q(4) -2*q(3) 2*q(2) 2*q(1);];
Vel_w2=mat*dq_dt_value'
Vel_w2 = 3×1
    0.1973
   -0.0118
    0.1531
% 2.2、通过四元数获得旋转角速度
% 参考 https://blog.csdn.net/zhoupian/article/details/96974091
Vel_w3=2*dq_dt*q0.conj
 
Vel_w3 = 
 
-6.25e-05 << 0.19729, -0.011842, 0.15309 >>