<rp id="jckgo"><acronym id="jckgo"></acronym></rp>
    <rp id="jckgo"><object id="jckgo"><input id="jckgo"></input></object></rp>

  • <li id="jckgo"><tr id="jckgo"></tr></li>

    <dd id="jckgo"></dd>

    當前位置:主頁 > 技術文章 > 旋轉向量與姿態矩陣之間相互轉換的數值計算問題分析

    技術文章

    Technical articles

    旋轉向量與姿態矩陣之間相互轉換的數值計算問題分析

    時間:2023-10-22 19:43:16 點擊次數:1481

                                                                                                                                旋轉向量與姿態矩陣之間相互轉換的數值計算問題分析


    1、概述

    在做新算法期間仔細考量了嚴老師的psins工具箱中關于旋轉向量RV和姿態矩陣M的轉換關系,其中的數值計算存在分母為零的問題,為了更穩定的結果可以做一些變通,但是也存在類似問題。

    常規的旋轉向量RV與姿態矩陣之間的關系[T1][C1],

    m = I + sin(|rv|)/|rv|*(rvx) + [1-cos(|rv|)]/|rv|^2*(rvx)^2            (1)

    其中存在sin(x)/x,[1-cos(x)]/x^2這兩個典型數值計算函數的問題。

    2、傳統數值計算處理方案

    sin(x)/x,[1-cos(x)]/x^2兩個函數的計算一般采取泰勒展開形式,定下一個小量閾值eps,小于這個eps閾值采取泰勒分解,把分母消去,代碼參見附錄[C1]。


    3、矩陣近似處理方案

    對于頻繁的這種近似數值計算,可能也是有代價的,為了避開這個方案,需要采取一些新的思路[T2],使用矩陣指數計算方法。Matlab提供了矩陣指數計算expm[T3],底層采用了Pade近似方法[T4][C2],避開分母為零方法,


    4、解決數值穩定性

    Pade數值計算穩定性應該更好。

    [T1]: 捷聯慣導算法與組合導航原理(2nd),嚴恭敏,chap2.2,p12,(2.2.23)

    [T2]:捷聯慣導算法與組合導航原理(2nd),嚴恭敏,chap2.1.2,p9,(2.1.20)

    [T3]:https://ww2.mathworks.cn/help/matlab/ref/expm.html

    [T3]: Matrix Computations(3rd),Gene H.Golub.Charles F.Van Loan,chap11.3.1,p572


    附錄代碼:

    [代碼C1]:

    function m = rv2m(rv)

    % Convert rotation vector to transformation matrix.

    %

    % Prototype: m = rv2m(rv)

    % Input: rv - rotation vector

    % Output: m - corresponding DCM, such that

    %     m = I + sin(|rv|)/|rv|*(rvx) + [1-cos(|rv|)]/|rv|^2*(rvx)^2

    %     where rvx is the askew matrix or rv.

    % See also  m2rv, rv2q, q2rv, a2mat, rotv.


    % Copyright(c) 2009-2014, by Gongmin Yan, All rights reserved.

    % Northwestern Polytechnical University, Xi An, P.R.China

    % 05/02/2009, 22/05/2014

    xx = rv(1)*rv(1); yy = rv(2)*rv(2); zz = rv(3)*rv(3);

    n2 = xx+yy+zz;

        if n2<1.e-8

            a = 1-n2*(1/6-n2/120); b = 0.5-n2*(1/24-n2/720);  % a->1, b->0.5

        else

            n = sqrt(n2);

            a = sin(n)/n;  b = (1-cos(n))/n2;

        end

    arvx = a*rv(1);  arvy = a*rv(2);  arvz = a*rv(3);

    bxx = b*xx;  bxy = b*rv(1)*rv(2);  bxz = b*rv(1)*rv(3);

    byy = b*yy;  byz = b*rv(2)*rv(3);  bzz = b*zz;

    m = zeros(3,3);

    % m = I + a*(rvx) + b*(rvx)^2;

    m(1)=1     -byy-bzz; m(4)= -arvz+bxy;     m(7)=  arvy+bxz;

    m(2)=  arvz+bxy;     m(5)=1     -bxx-bzz; m(8)= -arvx+byz;

    m(3)= -arvy+bxz;     m(6)=  arvx+byz;     m(9)=1     -bxx-byy;

    [代碼C2]:

    Matlab version E = expm(A)

    A=[ -0.130000000000000   0.020000000000000   0.030000000000000

       0.020000000000000  -0.100000000000000   0.060000000000000

       0.030000000000000   0.060000000000000  -0.050000000000000];

    % Scale A by power of 2 so that its norm is < 1/2 .

    [f,e] = log2(norm(A,'inf'));

    s = max(0,e+1);

    A = A/2^s;


    % Pade approximation for exp(A)

    X = A;

    c = 1/2;

    E = eye(size(A)) + c*A;

    D = eye(size(A)) - c*A;

    q = 6;

    p = 1;

    for k = 2:q

       c = c * (q-k+1) / (k*(2*q-k+1));

       X = A*X;

       cX = c*X;

       E = E + cX;

       if p

          D = D + cX;

       else

          D = D - cX;

       end

       p = ~p;

    end

    E = D\E;


    % Undo scaling by repeated squaring

    for k = 1:s

       E = E*E;

    end


    Copyright ?2019-2020 北京騰盛科技有限公司 (http://www.943158.com) 版權所有 備案號:京ICP備16062572號-1

    在線客服 聯系方式 二維碼

    服務熱線

    18258330715/15270575071

    掃一掃,關注我們

    春色校园自拍小说亚洲,亚洲日本黄色视频,亚洲黄色网站大全最新,亚洲综合伊人久久综合