b-frame到n-frame坐标转移矩阵求解（四元数形式）

Harmonican

已于 2022-09-13 14:36:39 修改

阅读量282

点赞数

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

于 2022-09-13 14:09:14 首次发布

本文链接：https://blog.csdn.net/mishibadu/article/details/126831712

版权

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

4 篇文章 0 订阅

订阅专栏

四元数定义

四元数定义如下

其中η应为实部，ε应为虚部。

在欧拉角形式下，旋转矩阵可由如下求得

而在四元数形式下， η与ε分别定义为

其中λ为单位向量，应满足

则四元数q可表示为

且满足内积为1，即 $\mathbf{q}^{T}\mathbf{q}=\textbf{1}$ 。

则坐标转移矩阵为

线速度转移矩阵求解

按照之前定义，b-frame到n-frame的线速度转移方程应为

则根据上文中提到的转移矩阵的求解公式可算出

则n-frame下坐标的变化量 $\dot{p}^{n }= \left \{ \dot{n},\dot{e},\dot{d}\right \}$ 为

转移矩阵求解的MATLAB实现代码如下

function R = Rquat(quat)
    Ita = quat(1);
    eps = imag(quat(2 : 4));
    
    R = eye(3, 3) + 2 * Ita * smtrix_gen(eps) + 2 * smtrix_gen(eps)^2;
    
    % R = [1 - 2 * (eps2.^2 + eps3.^2), 2 * (eps1 * eps2 - eps3 * Ita), 2 *(eps1 * eps3 + eps2 * Ita);
    %      2 * (eps1 * eps2 + eps3 * Ita), 1 - 2 * (eps1.^2 + eps3.^2), 2 *(eps2 * eps3 - eps1 * Ita);
    %      2 * (eps1 * eps3 - eps2 * Ita), 2 * (eps2 * eps3 + eps1 * Ita), 1 - 2 * (eps1.^2 + eps2.^2)];
end

角速度转移矩阵求解

从b-frame到n-frame角速度转移方程定义如下

其中

则四元数形式下b-frame到n-frame角速度变化量 $\dot{q}=\left \{\dot{\eta}, \dot{\varepsilon}_{1},\dot{\varepsilon}_{2},\dot{\varepsilon}_{3} \right \}$ 为

向量形式可表示为

MATLAB实现代码如下

function T = angle_trans_quat(quat)
    T = zeros(4, 3);
    Ita = quat(1);
    eps = imag(quat(2 : 4));
    T(1, :) = T(1, :) - eps';
    temp_T = Ita * eye(3, 3) + smtrix_gen(eps);
    T(2 : 4, :) = temp_T;
    T = 0.5 * T;
end

总结，6自由度下，四元数形式的b-frame到n-frame 坐标转移的动力学方程组的向量（矩阵）表示方式如下

求解的MATLAB实现代码如下

function [J, J1, J2] = b_frame2n_frame_quat(quat)
    J1 = Rquat(quat);
    J2 = angle_trans_quat(quat);
    
    J = [J1, zeros(3, 3);
        zeros(4, 3), J2];
end