近接連合慣導システムモデルとシミュレーション(三)

2.プログラミング実装–姿勢解算、速度解算
2.1姿勢解算
姿勢解算(更新)の本質は姿勢を解く微分方程式である.姿勢を表す方法は異なり、例えば、オーラ角、方向余弦行列、四元数、回転ベクトルである.従って,それぞれの表現方法の下の微分方程式があり,それぞれの解算方法が得られる.

シンボルの規定:ジャイロのサンプリング時間間隔はナビゲーションソルバの中で最小の時間単位である.姿勢の更新周期--[tk,tk+1][tk,tk+1];速度の更新周期——[tm−1,tm][tm−1,tm]位置の更新周期——[tl−1,tl][tl−1,tl]

2.1.1オーラ角法
ここでは、その微分方程式についてのみ説明します.(その応用:1つはシミュレーションプログラムデータ生成時、2つは大きな不整合角の初期アライメント中)
秦書P 279
力学では,動的座標系の基準座標系に対する角位置関係を決定するのによく用いられる.例えば、キャリアの航路角Ψ Ψ ,ピッチ角θ θ ,ロールアングルγ γ 実質的には、地理的座標系(参照)に対する機体座標系(動)の角位置関係を記述するオーラ角のセットである.

地理座標系g g系、Z、X、Y Z、X、Yをそれぞれ回転Ψ,θ,γ Ψ , θ , γ

ωnb ω n bは姿勢速度、すなわち、機体座標系b b系のナビゲーション座標系n(g)n(g)系に対する角速度である.を押します.ω^bnb=ω~bib−ω^bin ω ^ n b b = ω ~ i b b − ω ^ i n b決定,

⎡⎣⎢⎢ωbnbxωbnbyωbnbz⎤⎦⎥⎥=CγCθ⎡⎣⎢00Ψ˙⎤⎦⎥+Cγ⎡⎣⎢θ˙00⎤⎦⎥+⎡⎣⎢0γ˙0⎤⎦⎥=⎡⎣⎢cosγ0sinγ010−cosθsinγsinθcosθcosγ⎤⎦⎥⎡⎣⎢θ˙γ˙Ψ˙⎤⎦⎥ [ ω n b x b ω n b y b ω n b z b ] = C γ C θ [ 0 0 Ψ ˙ ] + C γ [ θ ˙ 0 0 ] + [ 0 γ ˙ 0 ] = [ c o s γ 0 − c o s θ s i n γ 0 1 s i n θ s i n γ 0 c o s θ c o s γ ] [ θ ˙ γ ˙ Ψ ˙ ]
では、オラ角微分方程式は次のとおりです.
⎡⎣⎢θ˙γ˙Ψ˙⎤⎦⎥=C−1ω⎡⎣⎢⎢ωbnbxωbnbyωbnbz⎤⎦⎥⎥ [ θ ˙ γ ˙ Ψ ˙ ] = C ω − 1 [ ω n b x b ω n b y b ω n b z b ]
2.1.2方向コサイン行列法
ここでは、姿勢行列の微分方程式についてのみ説明します.
秦書P 280
4つの形式があり、ここでは実際によく使われる2つだけを与え、パス略を導く.
C˙bn=−ωbnb×Cbn C ˙ n b = − ω n b b × C n b
C˙nb=Cnb×ωbnb C ˙ b n = C b n × ω n b b
2.1.3四元数法
Q˙=12Q⊗ωbnb Q ˙ = 1 2 Q ⊗ ω n b b
2.1.4回転ベクトル法
四元数は、ある時点、例えばtk t k、の姿勢を表すために用いることができ、この場合を姿勢四元数と呼ぶ.(一般的に大文字Q(tk)Q(tk)と記す)は、tk t kからtk+1 t k+1までの姿勢の変化を表すために用いられ、この場合を姿勢変換四元数と呼ぶ.(一般的には、h=tk+1−tk+1−t k+1−t k)q(h)と記す.
回転ベクトルは別の姿勢変化を表す方式であるため、姿勢変化の四元数とは以下の関係がある.
q(h)=cosϕ2+Φϕsinϕ2 q ( h ) = c o s ϕ 2 + Φ ϕ s i n ϕ 2

のうち、Φ Φ すなわち、座標系がtk t kからtk+1 t k+1までの時点での姿勢変化の等価回転ベクトルである、ϕ=|Φ| ϕ = | Φ |

ここで簡単に導くと、姿勢変換四元数は、姿勢更新に用いられる:rn(k+1)=Cn(k+1)b(k+1)rb(k+1)r n(k+1)=Cb(k+1)n(k+1)r b(k+1)r b(k+1)に対応し、rn(k+1)=Q(tk+1)rb(k+1)上の2つの式はそれぞれ余弦行列と四元数乗でベクトルの座標変換を表す.一方、rn(k+1)=Cn(k+1)n(k)n(k)Cn(k)b(k)Cb(k)Cb(k)b(k+1)rb(k+1)rb(k+1)rb(k+1)r n(k+1)=Cn(k)n(k)n(k)n(k)Cb(k)Cb(k)rb(k)rb(k)rb(k+1)rb(k+1)=p h)⊗rb(k+1)⊗q∗(h)]⊗Q(tk)∗}⊗p(h)r n(k+1)=p∗(h){Q(t k){q(h){r b(k+1){q(h)]{Q(t){q(q){r)}r b(k+1){q(h)}Q(t k)}p(h)は、
Q(tk+1)=p∗(h)⊗Q(tk)⊗q(h) Q ( t k + 1 ) = p ∗ ( h ) ⊗ Q ( t k ) ⊗ q ( h )
一つの姿勢更新周期において,Cn(k＋1)n(k)≒ICn(k)n(k＋1)≒I,すなわち,p(h)≒1 p(h)≒1の場合に近似する.
Q(tk+1)=Q(tk)⊗q(h)=M(Q(tk))q(h)=M′(q(h))Q(tk) Q ( t k + 1 ) = Q ( t k ) ⊗ q ( h ) = M ( Q ( t k ) ) q ( h ) = M ′ ( q ( h ) ) Q ( t k )
【注】:見落としたから
q(h)q(h)なので、何度か姿勢を更新した後、適切な修正をしなければなりません.詳しくはP 260を参照してください.
そこで,問題の鍵は姿勢変化四元数q(h)q(h),あるいは回転ベクトルに落ちた.Φ Φ と呼びます.

回転ベクトル微分方程式:ここでは導出せず、工学的によく用いられる近似式のみを与える(P 264):
Φ˙=ωbnb+12Φ×ωbnb+112Φ×(Φ×ωbnb) Φ ˙ = ω n b b + 1 2 Φ × ω n b b + 1 12 Φ × ( Φ × ω n b b )

回転ベクトル微分方程式の解まず、コンピュータプログラミングが実現されると、数値的に解くしかなく、解析的に解くことができないことを肝に銘じる必要がある.同様に、ここでは導出せず、回転ベクトルのみを与えるΦ Φ の解法結果(P 267):
Φ(h)=ΔΘ1+ΔΘ2+ΔΘ3+3380ΔΘ1×ΔΘ3+5780ΔΘ2×(ΔΘ3−ΔΘ1) Φ ( h ) = Δ Θ 1 + Δ Θ 2 + Δ Θ 3 + 33 80 Δ Θ 1 × Δ Θ 3 + 57 80 Δ Θ 2 × ( Δ Θ 3 − Δ Θ 1 )
【注】:(1).これは回転ベクトルの三子様解であり,[tk,tk+1][tk,tk+1]時間帯にキャリアを運ぶ角速度を仮定するωbnb ω n b bは放物線フィッティングにより得る、すなわち、ωbnb(tk+τ)=a+2bτ+3cτ2 ω n b b ( t k + τ ) = a + 2 b τ + 3 c τ 2 . (2). ΔΘ1,ΔΘ2,ΔΘ3 Δ Θ 1 , Δ Θ 2 , Δ Θ 3はそれぞれ3つの時間帯[tk,tk+h 3],[tk+h 3,tk+2 h 3],[tk,tk+h 3],[tk+h 3,tk+2 h 3],[tk+2 h 3,tk+1][tk+2 h 3,tk+1][tk+2 h 3,tk+1]の角増分である.(3).定数,直線,放物線,三次放物線フィット角速度を用いて得られた回転ベクトル式はP 268.

回転ベクトルアルゴリズムの最適化しかし、実際のキャリアの角速度が時間とともに変化する曲線は上記で与えられたフィット曲線ではないので、フィット曲線から導出された回転ベクトルアルゴリズムには当然誤差がある.以下、キャリアの角速度がテーパ運動であると仮定し、最適化アルゴリズムを導出する.なぜなら、アルゴリズムがテーパでの動作を保証できるならば運動下でのドリフトが最小であれば、残りの環境条件下でもアルゴリズムのドリフトが最小である.同様に、ここでは導出せず、最適化アルゴリズムで求めた回転ベクトルのみを与えるΦ Φ の結果(P 278):単一サブサンプル、二重サブサンプルは変化しなかった;最適化三サブサンプルアルゴリズムは回転ベクトルを求めた:
Φ(h)=ΔΘ1+ΔΘ2+ΔΘ3+920ΔΘ1×ΔΘ3+2740ΔΘ2×(ΔΘ3−ΔΘ1) Φ ( h ) = Δ Θ 1 + Δ Θ 2 + Δ Θ 3 + 9 20 Δ Θ 1 × Δ Θ 3 + 27 40 Δ Θ 2 × ( Δ Θ 3 − Δ Θ 1 )

2.1.4-2回転ベクトル法によるプログラミング実装
ここでは、2つの関数を記述します:(できるだけ独立し、モジュール化する必要があります)

第1の姿勢四元数を実現するための更新関数入力パラメータ:tk t k時点での姿勢四元数-Q(tk)Q(tk);tk t kからtk+1 t k+1時点での姿勢変化を反応させる姿勢変化四元数-q(h)q(h);関数出力パラメータ:tk+1 t k+1時点での姿勢四元数-Q(tk+1)Q(tk+1) ;

function Qk1=updateQk(Qk,qh)
% This function is to update Q(tk) to Q(tk+1)
% Qk1 = Qk \otimes qh = M(Qk)qh
% Make sure Qk,qh are both a 4-by-1 column vector
% changshen_xu
% 2018-07-17
% 
% M = [Qk(1),-Qk(2),-Qk(3),-Qk(4);
%      Qk(2), Qk(1),-Qk(4), Qk(3);
%      Qk(3), Qk(4), Qk(1),-Qk(2);
%      Qk(4),-Qk(3), Qk(2), Qk(1)];
% temp = M*qh;
% Qk1 = temp/sqrt(temp'*temp);

% Qk1 = Qk \otimes qh = M'(qh)Qk
Mq = [qh(1),-qh(2),-qh(3),-qh(4);
      qh(2), qh(1), qh(4),-qh(3);
      qh(3),-qh(4), qh(1), qh(2);
      qh(4), qh(3),-qh(2), qh(1)];
temp2 = Mq*Qk;
Qk1 = temp2/sqrt(temp2'*temp2);
end

回転ベクトルの解を実現するために2番目に使用され、対応する姿勢変化四元数関数入力パラメータを出力する:(ここでは最適化三サブサンプルアルゴリズムを用いる)ジャイロの3つの期間の角増分ΔΘ1,ΔΘ2,ΔΘ3 Δ Θ 1 , Δ Θ 2 , Δ Θ 3関数出力パラメータ:姿勢変化四元数q(h)q(h)

function qh=getqh(theta1,theta2,theta3)
% This function is to get the qh.
% By changshen_xu
% Date: 2018-07-17

% Optimized three-sample algorithm to get the rotation vector \Phi.
s1 = cross(theta1,theta3);
s2 = theta3-theta1;
s3 = cross(theta2,s2);
Phi = theta1+theta2+theta3+0.45*s1+0.675*s3;

phi = sqrt(Phi'*Phi);  % phi = |Phi|
qh = zeros(4,1);
qh(1) = cos(phi/2);
qh(2) = Phi(1)/phi*sin(phi/2);
qh(3) = Phi(2)/phi*sin(phi/2);
qh(4) = Phi(3)/phi*sin(phi/2);
end

ここで、シミュレーションの過程において、ジャイロのサンプリング値もデータ生成関数によって生成されることに注意する.そのため、秦書で述べた角増分を直接得ることはできず、実際には角速度である.従って、ここでは角速度から角増分への積分変換関数が必要である.