版權(quán)說(shuō)明:本文檔由用戶提供并上傳,收益歸屬內(nèi)容提供方,若內(nèi)容存在侵權(quán),請(qǐng)進(jìn)行舉報(bào)或認(rèn)領(lǐng)
文檔簡(jiǎn)介
1、實(shí)驗(yàn)三慣性導(dǎo)航實(shí)驗(yàn)小組成員 :楊曦陳魁吳航楊少帥實(shí)驗(yàn)?zāi)康?、了解慣性導(dǎo)航設(shè)備;2、掌握慣性導(dǎo)航設(shè)備的物理連接;3、掌握慣性導(dǎo)航信息的處理方法;4、掌握慣性導(dǎo)航方法并學(xué)會(huì)用編程實(shí)現(xiàn)慣性導(dǎo)航算法二、實(shí)驗(yàn)器材YH-5000AHRS ;工業(yè)控制計(jì)算機(jī);數(shù)據(jù)采集軟件;穩(wěn)壓電源;串口連接線;三、實(shí)驗(yàn)原理(1)姿態(tài)解算基于四元數(shù)法解算姿態(tài)矩陣。Q = l Pii P2 j pXp!yp?p J-2 ,222+ R -p2 -p32(66+屹):2(p1p3-lp2)2( R6 -lp3).2222l p2 - pi - p32( p2 P3 lpi)2(Rp3 1P2)2(p2P3 -lpi)yb=C;l2
2、 +P2-Pi2-P; |lzb_Xybzb JQ = 2 Qwpb上述微分方程表示成矩陣形式:1 一一 0b-Wpbxb-Wpby-Wpbz IT l 1b0bbPi=1WpbxWpbz一 WpbyPiP2.2b Wpbyb-Wpbz0b WpbxP2J3.ib Wpbzb Wpbyb-Wpbx0(4)初始四元數(shù)的確定計(jì)算如下:中G00八.中G0 .瓦.,0c o -s-c o-s- c o -s- +s ns i ns i n-l(0) 1P1(0)P2(0)_P3(0) . TOC o 1-5 h z 222222中G0 . e00 上.中G0 瓦.0c o -s-s i nc o-s-
3、 + s i nc o-s-s i n(5)222222*G0 。0. 0.*G0,瓦,0c o-s-c o-s-si n-sinsi n co-s-222222平G0 .8Jo.*G0 瓦,0c o-s-s i nsi n-sinco-s- co-s-222222用四階龍格庫(kù)塔法解(4)的微分方程;1 L= 血(f) R (/)一0(f)PAt)(t) 1 L-K gU)已仃)田,/ 區(qū)工=4 3yds(f)Pl (t) + 砒()Pa(0/ 跖+4)上。)一好+).工皿)一 TOC o 1-5 h z lL L LITTTT入門(mén)=-%(,+彳)兒+以/+萬(wàn)),L一叫(/+彳)小 I ,丁丁
4、丁K經(jīng)二不-佻,ff+虧)兒】一如/+虧)兒)+*+丁)兒) 降=;,磯-(7+4-如3一%d4萬(wàn)-)4丁r丁 -皿 UH-(wv(/+,*r, M 3 十 22-)應(yīng) 3 pjv (/.;)式,) K32 =9 53 1J72-/% rfJ. ( / + -T23 U) +如/(,一4)小k32=彳必(/+I9- H八+空 + -T2)/i (/) 4)也 AU) -K”T c =h -r ( /,(r)-i-K r ( -ijAn K ArKj; (d_( (/+T)Cj7)G()一3七(,+K”二;見(jiàn)。+口仁+%(/+丁)一叫。+丁)仁6IK42 =;叱0+na%(葉 口。+%仃+ T)
5、G K = yE(/+DG+(+DG(/)-(/+r)G(/)AS + T) =;(,)+Kg + 2K20 +2K楠 + K;口) oP/ + T) = R(f)+LK” +2K.十2K0 + K門(mén)) 1Ra + 7) = RC)+Lf Kiw + 2凡2 + 2K:;2 + KlQ (5R(+ T) = P,i(i) +g(Ku + 2K 得 + 2K 歸 + K 6 ) 5T11T12J31T32T33由Cbp中提取匕%, 7K 主=sin t231 T21 GG = tan T22?主=-tan-(T13)T33從而可得到:九=九王中G主十幾丁22 0,T21 A 0?g主 +2nT2
6、2 0, T21 0i = p-sign(Ti3)n,T33 Data1(1)T=(Data2(1)-Data1(1)*Ti_s; elseT=(Data2(1)-Data1(1)+65536) 叮i_s; end for m=1:11Data1(m)=Data2(m); endfor i=1:3w1(i)=w1(i)*g_s/scalar*K;%?6 ? %w2(i)=w2(i)*g_s/scalar*K; a1(i)=a1(i)*a_s/32768; a2(i)=a2(i)*a_s/32768;endl=cos(yaw/2)*cos(pitch/2)*c os(roll/2)+sin(ya
7、w/2)*sin(p itch/2)*sin(roll/2); p1=cos(yaw/2)*sin(pitch/2)* cos(roll/2)+sin(yaw/2)*cos( pitch*sin(roll/2);%p2=cos(yaw/2)*cos(pitch/2)*sin(roll/2)-sin(yaw/2)*sin( pitch/2)*cos(roll/2);%p3=cos(yaw/2)*sin(pitch/2)* sin(roll/2)-sin(yaw/2)*cos( pitch/2)*cos(roll/2);%l,p1,p2,p3 = RK(w1,w2,l,p1,p2,p3,T);Cb
8、p=1A2+p1A2-p2A2-p3A22*(p1*p2-l*p3)2*(p1*p3+l*p2);2*(p1*p2+l*p3)|A2+p2A2-p1A2-p3A22*(p2*p3-l*p1);2*(p1*p3-l*p2)2*(p2*p3+l*p1) |A2+p3A2-p1A2-p2A2; Cnb=inv(Cbp);pitch=asin(Cnb(2,3);yaw=atan(Cnb(2,1)/Cnb(2,2) ;roll=-atan(Cnb(1,3)/Cnb(3,3 );if (Cnb(3,3)0)if (Cnb(1,3)0) roll=roll-pi;endendif (Cnb(3,3)=0)
9、if (Cnb(1,3)0) roll=-pi/2;endendif (Cnb(2,2)0) if (Cnb(2,1)0) yaw=yaw+2*pi;end end if (Cnb(2,2)0) yaw=yaw+pi; end if (Cnb(2,2)=0)if (Cnb(2,1)0) yaw=pi/2;endendROLL(j)=roll/K; % PITCH(j)=pitch/K;% YAW(j)=yaw/K;plot(j,Data1(2)*ang_s);hold on ;a=0.5*(a1+a2); %V,x,y,z=solvelocation(a,V ,g,x,y,z,T);% for
10、 k=1:3龍格庫(kù)塔解四元數(shù)微分方程(RK.m):function l,p1,p2,p3 = RK( w1,w2,l,p1,p2,p3,T) wx=w1(1);wy=w1(2);wz=w1(3);wx1=0.5*(w1(1)+w2(1);wy1=0.5*(w1(2)+w2(2);wz1=0.5*(w1(3)+w2(3);wx2=w2; wy2=w2(2); wz2=w2(3);k10=0.5*(-wx*p1-wy*p2-wz*p3 );k11=0.5*(wx*l+wz*p2-wy*p3);k12=0.5*(wy*l-wz*p1+wx*p3);%Vx(j)=V(1);%Vy(j)=V(2);%V
11、z(j)=V(3);% end% Dx(j)=x;% Dy(j)=y;% Dz(j)=z;endfclose(fid);j=1:N;% %plot(j,PITCH(j),r); % 6?a ?-?a? ?x?i ?% % plot(j,YAW(j),r); plot(j,ROLL(j), r); hold on ;% j=1:N;% plot(j,Vx(j),r);% hold on;% plot(j,Vy(j),b);% hold on;% plot(j,Vz(j),k);% hold on;k13=0.5*(wz*l+wy*p1-wx*p2);A0=l+k10*T/2;A1=p1+k11*
12、T/2;A2=p2+k12*T/2;A3=p3+k13*T/2;k20=0.5*(-wx1*A1-wy1*A2-wz1*A3);k21=0.5*(wx1*A0+wz1*A2-wy1*A3);k22=0.5*(wy1*A0-wz1*A1+wx1*A3);k23=0.5*(wz1*A0+wy1*A1-wx1*A2);B0=l+k20*T/2;B1=p1+k21*T/2;B2=p2+k22*T/2;B3=p3+k23*T/2;k30=0.5*(-wx1*B1-wy1*B2-wz1*B3);k31=0.5*(wx1*B0+wz1*B2-wy1*B3);k32=0.5*(wy1*B0-wz1*B1+wx
13、1*B3);k33=0.5*(wz1*B0+wy1*B1-wx1*B2);C0=l+k30*T;C1=p1+k31*T;C2=p2+k32*T;C3=p3+k33*T;k40=0.5*(-wx2*C1-wy2*C2-wz2*C3);k41=0.5*(wx2*C0+wz2*C2-wy2*C3);k42=0.5*(wy2*C0-wz2*C1+wx2*C3);k43=0.5*(wz2*C0+wy2*C1-wx2*C2);l=l+T/6*(k10+2*k20+2*k30+k40);p1=p1+T/6*(k11+2*k21+2*k31 + k41);p2=p2+T/6*(k12+2*k22+2*k32+k42);p3=p3+T/6*(k13+2*k23+2*k33+k43);end速度位置的解算(solvelocation .m) function V,x
溫馨提示
- 1. 本站所有資源如無(wú)特殊說(shuō)明,都需要本地電腦安裝OFFICE2007和PDF閱讀器。圖紙軟件為CAD,CAXA,PROE,UG,SolidWorks等.壓縮文件請(qǐng)下載最新的WinRAR軟件解壓。
- 2. 本站的文檔不包含任何第三方提供的附件圖紙等,如果需要附件,請(qǐng)聯(lián)系上傳者。文件的所有權(quán)益歸上傳用戶所有。
- 3. 本站RAR壓縮包中若帶圖紙,網(wǎng)頁(yè)內(nèi)容里面會(huì)有圖紙預(yù)覽,若沒(méi)有圖紙預(yù)覽就沒(méi)有圖紙。
- 4. 未經(jīng)權(quán)益所有人同意不得將文件中的內(nèi)容挪作商業(yè)或盈利用途。
- 5. 人人文庫(kù)網(wǎng)僅提供信息存儲(chǔ)空間,僅對(duì)用戶上傳內(nèi)容的表現(xiàn)方式做保護(hù)處理,對(duì)用戶上傳分享的文檔內(nèi)容本身不做任何修改或編輯,并不能對(duì)任何下載內(nèi)容負(fù)責(zé)。
- 6. 下載文件中如有侵權(quán)或不適當(dāng)內(nèi)容,請(qǐng)與我們聯(lián)系,我們立即糾正。
- 7. 本站不保證下載資源的準(zhǔn)確性、安全性和完整性, 同時(shí)也不承擔(dān)用戶因使用這些下載資源對(duì)自己和他人造成任何形式的傷害或損失。
最新文檔
- 2025-2030年中國(guó)二手車第三方檢測(cè)行業(yè)商業(yè)模式創(chuàng)新戰(zhàn)略制定與實(shí)施研究報(bào)告
- 2025-2030年中國(guó)3C維修行業(yè)全國(guó)市場(chǎng)開(kāi)拓戰(zhàn)略制定與實(shí)施研究報(bào)告
- 2025-2030年中國(guó)智能手表行業(yè)商業(yè)模式創(chuàng)新戰(zhàn)略制定與實(shí)施研究報(bào)告
- 2025-2030年中國(guó)汽車運(yùn)動(dòng)服務(wù)行業(yè)開(kāi)拓第二增長(zhǎng)曲線戰(zhàn)略制定與實(shí)施研究報(bào)告
- 養(yǎng)殖金蟬知識(shí)培訓(xùn)課件
- 二零二五年度房產(chǎn)局備案的房屋租賃合同規(guī)范版6篇
- 二零二五年度房東房屋租賃合同(含電梯使用費(fèi))2篇
- 二零二五年度健康體檢中心運(yùn)營(yíng)管理合同標(biāo)準(zhǔn)范本3篇
- 電氣試驗(yàn)知識(shí)培訓(xùn)課件
- 精密機(jī)械制造中心項(xiàng)目可行性研究報(bào)告模板-立項(xiàng)拿地
- 《招標(biāo)投標(biāo)法》考試題庫(kù)200題(含答案)
- 駕駛證學(xué)法減分(學(xué)法免分)試題和答案(50題完整版)1650
- 高低壓配電柜-福建寧德核電站投標(biāo)書(shū)
- 少兒繪畫(huà)之《水仙花開(kāi)迎春來(lái)》
- 《法學(xué)概論》課程教學(xué)大綱
- 成品油稅收分類編碼
- 福建省廈門(mén)市高一上學(xué)期期末考試政治試題 Word版含答案
- 山東中醫(yī)藥大學(xué)中西醫(yī)臨床(專升本)學(xué)士學(xué)位考試復(fù)習(xí)題
- 鐵路貨場(chǎng)平面設(shè)計(jì)說(shuō)明書(shū)
- 抽象函數(shù)的單調(diào)性
- 2019年血站績(jī)效考核標(biāo)準(zhǔn)
評(píng)論
0/150
提交評(píng)論