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

技術文章

Technical articles

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

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

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


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.dgjinghong168.com) 版權所有 備案號:京ICP備16062572號-1

在線客服 聯系方式 二維碼

服務熱線

18258330715/15270575071

掃一掃,關注我們

精品国产亚洲AV日韩