b-frame到n-frame坐标转移矩阵求解（欧拉角形式）

最新推荐文章于 2023-09-01 19:01:04 发布

Harmonican

最新推荐文章于 2023-09-01 19:01:04 发布

阅读量337

点赞数

分类专栏：水下机器人建模基础文章标签：矩阵线性代数 matlab 机器人智能硬件

本文链接：https://blog.youkuaiyun.com/mishibadu/article/details/126829099

版权

水下机器人建模基础专栏收录该内容

4 篇文章

订阅专栏

Skew-Symmetry(斜对称)矩阵的定义

MATLAB实现

function S = smtrix_gen(lambda)
    S = [0, -lambda(3), lambda(2); 
        lambda(3), 0, -lambda(1); 
        -lambda(2), lambda(1), 0];
end

旋转矩阵求解

将旋转过程沿x,y,z轴分为三次进行，方向向量分别为[1, 0, 0]T，[0, 1, 0]T与[0, 0, 1]T，角度分别为φ，θ与ψ。由此可得三个旋转矩阵分别如下所示：

则

展开得

而

展开得

同时由于旋转矩阵的正交性，可得

相应的轨迹可由欧拉积分近似得到

在MATLAB中，由欧拉角求得旋转矩阵可使用函数angle2dcm()实现，示例如下

phi = 0;
theta = 0.1;
psi = 0.7854;

dcm = angle2dcm(psi, theta, phi); %z to y to x.

示例结果如下

角速度转移矩阵求解

设b-frame下角速度向量为 $\omega _{nb}^{b} =[p, q, r]^{T}$ ，而n-frame下欧拉速度矢量(Euler rate vector)为 $\dot{\Theta } = \left [ \dot{\phi }, \dot{\theta}, \dot{\psi} \right ] ^{T}$ ，则可得如下关系

根据上述旋转过程（即分三次计算旋转过程）则可得

左边展开可得

则欧拉姿态角的变化量（即欧拉速度矢量）为

角速度转移矩阵的MATLAB实现代码如下

function T = angle_trans_euler(phi, theta)
    T = [1, sin(phi)*tan(theta), cos(phi)*tan(theta);
        0, cos(phi), -sin(phi);
        0, sin(phi)/cos(theta), cos(phi)/cos(theta)];
end

总结，6自由度下b-frame到n-frame的转移矩阵计算分为线速度的变化以及角速度的变换，其动力学方程组可表示为向量（矩阵）形式，如下

具体MATLAB代码实现如下

function [J, J1, J2] = b_frame2n_frame_eulerangle(phi, theta, psi)
    J1 = angle2dcm(psi, theta, phi)'; %z to y to x.
    J2 = angle_trans_euler(phi, theta);
    
    J = [J1, zeros(3, 3);
        zeros(3, 3), J2];
end