2017/4/61 概述全日制专业硕士必修课《导航系统设计》-o0tonTaTs-e第3章基于四元数的姿态解算程序设计特点:·可以全姿态工作,不受限制微分方程维数,达到了9维:运算过程中有非正交误差产生,每次需正交化1概述2四元数微分方程格g+gi-qi-q2(q.9;+9o92)(+)2(q:92-909s)gz(0)xy.z,ABB2(g:92+9o9s)(-qi+q-q)2(q29;909.)+5tsin40)ABQ(t+Ar)=Q(t)cos(-qi-q+q)2(g.9;-9092)2(29;+90g.)22ABAB3AB当→0时,→0,于是有cos=1, sin24B0(0+904cosycosw-sinysin9sincos9sinwsinycosw+cosysin9sinwQ(t+4n)=Q(01+22cosysinw+sinysingcosycos9cosysinysinw-cossincossinycos.9sin900cosy cos.9Ot+4r)-Q0O(t)Q(t)=limAfAf2四元数微分方程2四元数微分方程-0a-05-0%"o+o0O美发4-0a00do92Dnfa29h100ChL93o-0092o.-0.0L93+qi-qi-2(4,9,+409)2(g,92-9,)g-g+-g2(4293-909.)TA2(qi92 +909s)q+gi-q2-2(9:92-909)2(9:9;+909.)2(g,g-90)2(9.9+99)-qi+TE2(g192 + 409)gqi+q-gs2(929;-909)2(9.9;99.)2(929;+909)--q5+31
2017/4/6 1 第3章 基于四元数的姿态解算程序设计 全日制专业硕士必修课《导航系统设计》 0 0 0 3 1 3 2 3 3 2 1 2 2 2 3 1 1 1 2 1 3 3 1 3 2 3 3 2 1 2 2 2 3 1 1 1 2 1 3 b nbx b nby b nbx b nbz b nby b nbz T T T T T T T T T T T T T T T T T T 特点: •可以全姿态工作,不受限制; •微分方程维数高,达到了9维; •运算过程中有非正交误差产生,每次需正交化。 1 概述 b b b n n n z y x q q q q q q q q q q q q q q q q q q q q q q q q q q q q q q q q q q q q z y x 2( ) 2( ) ( ) 2( ) ( ) 2( ) 2( ) 2( ) 2 3 2 2 2 1 2 1 3 0 2 2 3 0 1 0 2 3 0 1 2 3 2 2 2 1 2 1 2 0 3 0 1 2 0 3 1 3 0 2 2 3 2 2 2 1 2 0 sin cos sin cos cos cos sin sin sin cos cos cos sin sin cos sin cos cos cos sin sin sin cos sin sin cos cos sin sin n Tb 1 概述 绕 轴 绕 轴 2 sin 2 ( ) ( ) cos 2 b Q t t Q t 当 t 0时, 0,于是有 2 cos =1, 2 2 sin b b Q t Q t t Q t Q t 2 2 2 ( ) ( ) 2 ( ) ( ) 1 b n b b b t t Q t Q t t Q t t Q t t Q t Q t 2 ( ) 2 ( ) lim 2 ( ) ( ) ( ) ( ) lim 2 2 0 0 2 四元数微分方程 Q Q 2 1 0 0 1 1 2 2 3 3 0 1 0 2 0 0 b b b nbx nby nbz b b b nbx nbz nby b b b nby nbz nbx b b b nbz nby nbx q q q q q q q q 2 3 2 2 2 1 2 1 3 0 2 2 3 0 1 0 2 3 0 1 2 3 2 2 2 1 2 1 2 0 3 0 1 2 0 3 1 3 0 2 2 3 2 2 2 1 2 0 2( ) 2( ) 2( ) 2( ) 2( ) 2( ) q q q q q q q q q q q q q q q q q q q q q q q q q q q q q q q q q q q q T n b 2 四元数微分方程 0 0 1 1 2 2 3 3 0 1 0 2 0 0 b b b nbx nby nbz b b b nbx nbz nby b b b nby nbz nbx b b b nbz nby nbx q q q q q q q q 2 3 2 2 2 1 2 1 3 0 2 2 3 0 1 0 2 3 0 1 2 3 2 2 2 1 2 1 2 0 3 0 1 2 0 3 1 3 0 2 2 3 2 2 2 1 2 0 2( ) 2( ) 2( ) 2( ) 2( ) 2( ) q q q q q q q q q q q q q q q q q q q q q q q q q q q q q q q q q q q q T n b ( ) n en e ie n e b n b ib b nb T C n in 2 四元数微分方程
2017/4/63四元数归一化3四元数归一化由于计算读整,使计算的变换四元缴的效败不等于1,(g-q)2d:距高面数·对计算的四元败阵周期性地进行规范化处理;--1=0的求条件g=29.用最小二采别摄求最优范化的变换四元数,f-极摄最优估计理论,用拉格朗日子法求在约求条件下的极值Q=+9+922+933计算的四元数Φ=d+gQ=+q,, +q2+q3g90+91++91规化的四元数Q=++h+=+++4四元数初始化4四元数初始值>方案1方案1()k/-, /1+7,-T2Ts[+q-q,=T.'TT42/= /1Ti+T2 -73-q+q2-=T22(TTa)-qi-q+qi=TIg/-- /1-7.-72 +7,g[q +qi-q-q2(9,92-909,)2(9,9,+9092)+q+q+q=1-qi+q-q190= /1g-2-92(q,92+qo9,)2(929;-99,)signgo= +-qi-qi +qs2(q,9;-9092)2(929+909)signgi= sign(T32T2)+g--qi--+95--72signq2 = sign(Tus-Ta)signg;= sign(T2Ti2)g--g+gi=+q+q+q-4四元数初始化4四元数初始化>方案2>方案2Z. (z,)载体坐标系相对导定理动坐标系(b系)绕动坐标系中的单位向量5、5、-相对静y航坐标系的三次旋转,坐标系(n系)旋转26、26,、,向量R在静系与动系中的四元1用四元数表达。Y,(Y.)数表达式为R"和R',则有下式成立99,= cos+ksinR"=QR'Q-!0.=cosg+/sing式中Q=QQ.,Q=cos0,+5sinY,(,)(如果51、52、是静坐标系(n系)中的单位向量,则Q,=cos+jsin(X,)Q=Q.0Qcos0+5,sino)(x.)X2
2017/4/6 2 • 由于计算误差,使计算的变换四元数的范数不等于1; • 对计算的四元数阵周期性地进行规范化处理; • 用最小二乘判据求最优规范化的变换四元数。 0 1 1 2 2 3 3 ˆ ˆ ˆ ˆ ˆ Q q q i q i q i 计算的四元数 0 1 1 2 2 3 3 Q q q i q i q i 规范化的四元数 3 四元数归一化 2 3 2 2 2 1 2 0 0 1 1 2 2 3 3 0 1 1 2 2 3 3 ˆ ˆ ˆ ˆ ˆ ˆ ˆ ˆ q q q q q q i q i q i Q q q i q i q i 3 0 2 (ˆ ) i d qi qi 距离函数 3 0 2 1 0 i g qi 约束条件 根据最优估计理论,用拉格朗日乘子法求在约束条件下的极值 d g 3 四元数归一化 31 32 33 21 22 23 11 12 13 T T T T T T T T T T n b 2 3 2 2 2 1 2 1 3 0 2 2 3 0 1 0 2 3 0 1 2 3 2 2 2 1 2 1 2 0 3 0 1 2 0 3 1 3 0 2 2 3 2 2 2 1 2 0 2( ) 2( ) 2( ) 2( ) 2( ) 2( ) q q q q q q q q q q q q q q q q q q q q q q q q q q q q q q q q q q q q 4 四元数初始值 11 2 3 2 2 2 1 2 q0 q q q T 22 2 3 2 2 2 1 2 q0 q q q T 33 2 3 2 2 2 1 2 q0 q q q T 1 2 3 2 2 2 1 2 q0 q q q 方案1 11 2 3 2 2 2 1 2 q0 q q q T 22 2 3 2 2 2 1 2 q0 q q q T 33 2 3 2 2 2 1 2 q0 q q q T 1 2 3 2 2 2 1 2 q0 q q q 1 1 11 22 33 2 1 q T T T 2 1 11 22 33 2 1 q T T T 3 1 11 22 33 2 1 q T T T 2 3 2 2 2 q0 1 q1 q q sign sign ( ) sign sign ( ) sign sign ( ) sign 3 21 12 2 13 31 1 32 23 0 q T T q T T q T T q 方案1 4 四元数初始化 定理 动坐标系(b 系)绕动坐标系中的单位向量 1 、 2 、.相对静 坐标系(n 系)旋转 21、2 2、.,向量 R 在静系与动系中的四元 数表达式为 n R 和 b R ,则有下式成立1 R QR Q n b 式中:Q Q1Q2 ,Qi i i i cos sin (如果1 、 2 、.是静坐标系(n 系)中的单位向量,则 Q Q2Q1,Qi i i i cos sin ) 方案2 4 四元数初始化 载体坐标系相对导 航坐标系的三次旋转, 用四元数表达。 2 sin 2 cos 1 Q k 2 sin 2 cos 2 Q i 2 sin 2 cos 3 Q j Yn 1 Y Zn Y2 Z2 ( ) Yb Xb ( ) Zb ( ) Xb ( ) Yb Zb 1 X Xn ( ) X2 方案2 4 四元数初始化
2017/4/64四元数初始化5数值积分算法方案2Yii =y, +Af(t,)1-FislsO2人Q(k+1)=Q(k)+=Q0t09sinsin9o =.0000Sk=ol...n-1sinsincossing,=cosAtI(K, +K,)Jui = y, +95in/si-Cosq=cc299sinsing+sin。A0so(k)()+o(k)(+1)O(k+1)=Q(k)+9;=cos0OS21k=0.1---n-16捷联姿态解算步骤5数值积分算法At1、初始化常用变量-(K,+2K,+2K, +K.)Yi4I = y, +6R=6378137.0;f=1.0/298.257;Wie =7.2722e-5;-Q(k)o(k)G0=9.7803;deg=180/pi;rad=pi/180:k=O2、初始化导航参数kz[Q(k)+k,Jo(k+)位置:a、L、α和h速度:VE、V和Vuk[O(k)+klok+)姿态:、9和k[Q(k)+Ak,Jo(k+A)并将位置入、L和姿态、9和转变成弧度单位。Q(k+1)=Q(k)+[k,+2k,+2k,+k]6捷联姿态解算步骤6捷联姿态解算步骤>有了初始的姿态:V、9和>有了初始的姿态:业、9和0-+si++scoscosy-sinsingsin-cos9sinysinycosy+cosysingsinT,=cossiny+sinsingcocos9coswsinysinw-cosysin gcossinycos.9sin.9cosycos.gcococsin(/==/I+T.TTghcoicisigngo = +signgi= sign(T2-T23)I92/=/1Tt+T2T3:cssi+sissignq2 = sign(Ti3 -T)I9/==/1-7-T2+73msignq, = sign(T21 -Ti2)40---4-43
2017/4/6 3 2 sin 2 cos 2 sin 2 cos 2 sin 2 cos Q k i j 2 sin 2 sin 2 sin 2 cos 2 cos 2 cos 0 q 2 sin 2 cos 2 sin 2 cos 2 sin 2 cos 1 q 2 cos 2 sin 2 sin 2 sin 2 cos 2 cos 2 q 2 cos 2 cos 2 sin 2 sin 2 sin 2 cos 3 q 方案2 4 四元数初始化 ( , ) i 1 i i i y y tf t y ( ) 2 1 K1 K2 t y y i i 0,1 1 2 1 ( 1) ( ) k n Q k Q k Q t 0,1 1 ( ) ( 1)] 2 1 ( ) ( ) 2 1 [ 2 ( 1) ( ) k n Q k k Q k k t Q k Q k 5 数值积分算法 ( 2 2 ) 6 1 K1 K2 K3 K4 t y y i i ( 1) ( ) [ 2 2 ] [ ( ) ] ( ) 2 1 [ ( ) ] ( ) 2 1 [ ( ) ] ( ) 2 1 ( ) ( ) 2 1 6 1 2 3 4 4 3 3 2 2 2 2 2 1 2 1 Q k Q k k k k k k Q k tk k t k Q k k k k Q k k k k Q k k t t t t t 5 数值积分算法 1、初始化常用变量 R =6378137.0; f=1.0 / 298.257; Wie =7.2722e-5; G0 = 9.7803; deg=180/pi; rad=pi/180; 2、初始化导航参数 位置: 、 L 、 和h 速度:VE 、VN 和VU 姿态: 、 和 并将位置 、 L 和姿态 、 和 转变成弧度单位。 6 捷联姿态解算步骤 有了初始的姿态: 、 和 sin cos sin cos cos cos sin sin sin cos cos cos sin sin cos sin cos cos cos sin sin sin cos sin sin cos cos sin sin n Tb 1 1 11 22 33 2 1 q T T T 2 1 11 22 33 2 1 q T T T 3 1 11 22 33 2 1 q T T T 2 3 2 2 2 q0 1 q1 q q sign sign ( ) sign sign ( ) sign sign ( ) sign 3 21 12 2 13 31 1 32 23 0 q T T q T T q T T q 6 捷联姿态解算步骤 有了初始的姿态: 、 和 2 sin 2 cos 2 sin 2 cos 2 sin 2 cos Q k i j 2 sin 2 sin 2 sin 2 cos 2 cos 2 cos 0 q 2 sin 2 cos 2 sin 2 cos 2 sin 2 cos 1 q 2 cos 2 sin 2 sin 2 sin 2 cos 2 cos 2 q 2 cos 2 cos 2 sin 2 sin 2 sin 2 cos 3 q 6 捷联姿态解算步骤
2017/4/66 捷联姿态解算步骤6 捷联姿态解算步骤>初始的位置:元、L、αCa3、根据初始时刻的姿态信息,计算初始姿态矩阵T”:根据初[sin α sin Lcos cosa sin ^sinasinLsin +cos acos^sinacosL始位置信息计算初始位置矩阵C,。CP=-cosasin Lcos a-sinasin^cos asin Lsin sinacosCOS(α cOs L4、根据初始姿态矩阵工,或初始姿态,计算初始四元数Q。cos Lcos ^cos Lsin^sinL5、计算子初始午面的半径和卯酉圈半径,并根据速度信息计算当α=0时,CP=C初始位置角速度%。(C-sinacos ^0C21Ca6、读入IMU的输出的陀螺数据,并计算c=Cu2CnC2sin Lsincos L-sin Lcos (CusinLJC23C33(cosLcoscos LsinAo.-of-T,(Co +o)6捷联姿态解算步骤6捷联姿态解算步票(cosycosw-sinysinsiny-cos9sinysinycosy+cosysinsiny7、更新四元数微分方程-cosysngsinysingcosyLos.sysinysing-ossingcosyIal-0-00园-sm700s.9singcosycos.90ogiG-0元00o9W± = tan"-T2[o.360-]oo9T228、分别利用欧拉法和四阶龙格-库塔法对四元数微分方程或方T32[-90,90]9± =tan向余弦微分方程进行积分。JT+T7=tan"[-180”,180]Te36捷联姿态解算步骤9、四元数归一化或方向余弦正交化0=40+4万+g+g5-+5+9+9器+蛋+强+霜10、求取姿态角,并以曲线的形式显示(利用plot函数)。4
2017/4/6 4 初始的位置: 、 L 、 n Ce L L L L L L L L L C p e cos cos cos sin sin cos sin cos sin sin cos sin sin sin cos cos cos sin sin cos cos sin sin sin sin cos cos sin cos L L L L L L C C C C C C C C C C n e cos cos cos sin sin sin cos sin sin cos sin cos 0 1 3 2 3 3 3 1 2 2 2 3 2 1 1 2 1 3 1 当 0 时, n e p Ce C 6 捷联姿态解算步骤 3、根据初始时刻的姿态信息,计算初始姿态矩阵 n Tb ;根据初 始位置信息计算初始位置矩阵 n Ce 。 4、根据初始姿态矩阵 n Tb 或初始姿态,计算初始四元数Q 。 5、计算子初始午面的半径和卯酉圈半径,并根据速度信息计算 初始位置角速度 n en 。 6、读 入 IMU 的 输 出 的 陀 螺 数 据 b ib ,并计算 ( ) n en e ie n c b n b ib b nb T C 6 捷联姿态解算步骤 7、更新四元数微分方程 8、分别利用欧拉法和四阶龙格-库塔法对四元数微分方程或方 向余弦微分方程进行积分。 0 0 1 1 2 2 3 3 0 1 0 2 0 0 b b b nbx nby nbz b b b nbx nbz nby b b b nby nbz nbx b b b nbz nby nbx q q q q q q q q 6 捷联姿态解算步骤 tan 90 ,90 2 22 2 12 1 32 T T T 主 tan 0 ,360 22 1 12 T T 主 tan 180 ,180 33 1 31 T T 主 sin cos sin cos cos cos sin sin sin cos cos cos sin sin cos sin cos cos cos sin sin sin cos sin sin cos cos sin sin 3 1 3 2 3 3 2 1 2 2 2 3 1 1 1 2 1 3 T T T T T T T T T T n b 6 捷联姿态解算步骤 9、四元数归一化或方向余弦正交化 2 3 2 2 2 1 2 0 0 1 1 2 2 3 3 0 1 1 2 2 3 3 ˆ ˆ ˆ ˆ ˆ ˆ ˆ ˆ q q q q q q i q i q i Q q q i q i q i 10、求取姿态角,并以曲线的形式显示(利用 plot 函数)。 6 捷联姿态解算步骤