用四元數(shù)法的捷聯(lián)慣性導(dǎo)航姿態(tài)解算程序

1、用四元數(shù)法的捷聯(lián)慣性導(dǎo)航姿態(tài)解算程序close all;clear all;%重力產(chǎn)生的加速度矢量g=9.80142;%單位9.8m/s2G=0,0,-g'%*讀入數(shù)據(jù)%*讀入陀螺儀的數(shù)據(jù)gyro_x=load('gyrox.txt');gyro_y=load('gyroy.txt');gyro_z=load('gyroz.txt');%*讀入加速度計(jì)的數(shù)據(jù)*%acc_rate=3/1024;acc_x =load('acceleratex.txt');acc_y =load('acceleratey.txt

2、9;);acc_z=load('acceleratez.txt');%加速度數(shù)字轉(zhuǎn)換為模擬電壓data_acc=acc_x;acc_y;acc_z;data_acc=data_acc/1024*3%將數(shù)據(jù)轉(zhuǎn)換為相應(yīng)的加速度值%*%加速度計(jì)三個(gè)軸向的零點(diǎn)電壓%zero_ax=?%zero_ay=?%zero_az=?%加速度計(jì)三個(gè)軸向的電壓/加速度比值%rate_ax=? %單位是m/s2/V%rate_ay=?%rate_az=?%acc_x=acc_x*acc_rate;%acc_y=acc_y*acc_rate;%acc_z=acc_z*acc_rate;aver_acc_

3、x=mean(acc_x)aver_acc_y=mean(acc_y)aver_acc_z=mean(acc_z)%采樣時(shí)間dtime=0.01;tm=0:dtime:0.01* (size(gyro_x,2)-1);%個(gè)數(shù)numn_point=size(gyro_x,2);%圖1figureplot(tm,data_acc(1,:),'-',tm,data_acc(2,:),'.',tm,data_acc(3,:),'-.');title('加速度計(jì)的采樣曲線');legend('x_ACC','Y_AC

4、C','Z_ACC');xlabel('Time / (10ms)');ylabel('Accelerate/ (m/s'')');grid on;%plot(tm,acc_x,'-',tm,acc_y,'.',tm,acc_z,'-.');%title('加速度的計(jì)的采樣曲線'):%對(duì)采樣曲線進(jìn)行低通濾波a=1,2,4,2,1;%gyro_x=filter(a/sum(a),1,gyro_x);%gyro_y=filter(a/sum(a),1,gyro_y

5、);%gyro_z=filter(a/sum(a),1,gyro_z);%比例變換gyro_x=gyro_x/1024*3/0.6;gyro_y=gyro_y/1024*3/0.6;gyro_z=gyro_z/1024*3/0.6;%零點(diǎn)電壓-陀螺儀，取前80個(gè)數(shù)的平均電壓zero_gx=sum(gyro_x(1:80)/80zero_gy=sum(gyro_y(1:80)/80zero_gz=sum(gyro_z(1:80)/80%減去零點(diǎn)gyro_x=(gyro_x-zero_gx)/0.0125/180*pi;gyro_y=(gyro_y-zero_gy)/0.0125/180*pi;g

6、yro_z=(gyro_z-zero_gz)/0.0125/180*pi;%gyro_x=(gyro_x-2.5)/0.0125/180*pi;%gyro_y=(gyro_y-2.5)/0.0125/180*pi;%gyro_z=(gyro_z-2.5)/0.0125/180*pi;%測(cè)試數(shù)據(jù)accelerate=zeros(3,n_point);accelerate(1,1:100)=10;accelerate(1,101:200)=-10; accelerate(1,201:300)=0;%陀螺儀數(shù)據(jù)gyro_x=zeros(1,n_point);gyro_y=zeros(1,n_poin

7、t);gyro_z=zeros(1,n_point);gyro_z(1:100)=pi/3;gyro_z(101:200)=-pi/3;%重力軸始終有加速度accelerate(3,:)=accelerate(3,:)+9.8;figureplot(tm,accelerate(1,:),'-',tm,accelerate(2,:),'.',tm,accelerate(3,:),'-.');title('加速度計(jì)的采樣曲線');legend('x_ACC','Y_ACC','Z_ACC'

8、;);xlabel('Time / (10ms)');ylabel('Accelerate/ (m/s'')');grid on;%畫出陀螺儀的采樣曲線figureplot(tm,gyro_x,'r-',tm,gyro_y,'g.',tm,gyro_z,'b-.');title('陀螺儀的采樣曲線');legend('x_Gyro','Y_Gyro','Z_Gyro');xlabel('Time / (10ms)');y

9、label('Angel_rate/ (degree/s)');grid on;%size(gyro_x)%size(gyro_y)%size(gyro_z)data_gyro=gyro_x;gyro_y;gyro_z;%轉(zhuǎn)移矩陣-即方向余弦矩陣T=eye(3); %T是3*3的單位矩陣,初始轉(zhuǎn)移矩陣 %四元數(shù)矩陣，存儲(chǔ)每步更新之后的四元數(shù)，方便以后繪圖Q=zeros(4,n_point);%四元數(shù)的初始值確定，假定一開始導(dǎo)航坐標(biāo)系與載體坐標(biāo)系是重合的，因此方向余弦矩陣，是單位矩陣，利用它們之間的關(guān)系確定四元數(shù)的初始值。    Q(1,1)=0.5

10、*sqrt(1+T(1,1)+T(2,2)+T(3,3);    Q(2,1)=0.5*sqrt(1+T(1,1)-T(2,2)-T(3,3);    Q(3,1)=0.5*sqrt(1-T(1,1)+T(2,2)-T(3,3);    Q(4,1)=0.5*sqrt(1-T(1,1)-T(2,2)+T(3,3);%參見捷聯(lián)慣性導(dǎo)航技術(shù)31頁(yè)3.64式 在旋轉(zhuǎn)90度時(shí)不適用    %Q(1,1)=0.5*sqrt(1+T(1,1)+T(2,2)+T(3,3);%Q(2,1)=

11、1/4/Q(1,1)*(T(3,2)-T(2,3);%Q(3,1)=1/4/Q(1,1)*(T(1,3)-T(3,1);%Q(4,1)=1/4/Q(1,1)*(T(2,1)-T(1,2);%求姿態(tài)角矩陣ANGLE=zeros(3,n_point);%angle1代表繞X軸轉(zhuǎn)過的角度，2代表Y軸,3代表Z軸%方向余弦矩陣到歐拉角的轉(zhuǎn)換關(guān)系，這里注意旋轉(zhuǎn)順序是Z-Y-X，參考文獻(xiàn)<<一種全新的全角度元元數(shù)與歐拉角的轉(zhuǎn)換算法>>%位置矩陣position=zeros(3,n_point);       &#

12、160;                       %位置矩陣velocity=zeros(3,n_point);%速度矩陣%重力加速度%acc_g=0,0,-9.8'qh=0,0,0,0;for i=1:n_point %開始循環(huán)    if i>1  velocity(:,i)=(T*accelerate(:,i-1)

13、+T*accelerate(:,i)/2+G)*dtime+velocity(:,i-1);%要考慮到重力的影響，假定重量方向與子軸方向一致  position(:,i)=position(:,i-1)+(velocity(:,i-1)+velocity(:,i)*dtime/2;    end    %計(jì)算歐拉角,假定俯仰角在+_90度范圍移動(dòng)，而滾動(dòng)角和偏航角在+-180度范圍內(nèi)取值  %ANGLE(1,i)=atan(T(2,3)/T(3,3);  %ANGLE(2,i)=asin(-T(1,3);

14、  %ANGLE(3,i)=atan(T(1,2)/T(1,1);  if T(3,3)>0  %根據(jù)物理意義不可能出現(xiàn)0    ANGLE(1,i)=-atan(T(2,3)/T(3,3);else    ANGLE(1,i)=-pi*sign(T(2,3)-atan(T(2,3)/T(3,3);  end%俯仰角ANGLE(2,i)=-asin(-T(1,3);%偏航角if T(1,1)>0%公式似乎有誤，直接按公式計(jì)算是負(fù)值    ANGLE(3,

15、i)=-atan(T(1,2)/T(1,1);else    ANGLE(3,i)=-pi*sign(T(1,2)-atan(T(1,2)/T(1,1);end      ANGLE(1,i)=atan(T(3,2)/T(3,3);  ANGLE(2,i)=asin(-T(3,1);  ANGLE(3,i)=atan(T(2,1)/T(1,1);%更新四元數(shù)   if i<n_point %如果還沒有到超出數(shù)組范圍       t

16、heta=data_gyro(:,i)*dtime;%角度向量       dtheta=sqrt(theta'*theta);       %i要保證當(dāng)theta為零時(shí)算法仍有關(guān)效       if dtheta=0           qh=1,0,0,0;    &

17、#160;  else           %換用簡(jiǎn)化算法試驗(yàn)結(jié)果       %qh=cos(dtheta);theta*sin(dtheta/2)/dtheta;       qh=1;0.5*theta;       end       % 更

18、新四元數(shù)       Q(:,i+1)=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)*Q(:,i);       %更新方向方向余弦矩陣       T=1-2*(Q(3,i+1)*Q(3,i+1)+Q(4,i)*Q(4,i+1)  2*(Q(2

19、,i+1)*Q(3,i+1)-Q(1,i+1)*Q(4,i+1)     2*(Q(2,i+1)*Q(4,i+1)+Q(1,i+1)*Q(3,i+1);       2*(Q(2,i+1)*Q(3,i+1)+Q(1,i+1)*Q(4,i+1)           1-2*(Q(2,i+1)*Q(2,i+1)+Q(4,i+1)*Q(4,i+1)   2*(Q(3,i+1)*Q(4

20、,i+1)-Q(1,i+1)*Q(2,i+1);       2*(Q(2,i+1)*Q(4,i+1)-Q(1,i+1)*Q(3,i+1)           2*(Q(3,i+1)*Q(4,i+1)+Q(1,i+1)*Q(2,i+1)     1-2*(Q(2,i+1)*Q(2,i+1)+Q(3,i+1)*Q(3,i+1); %得到姿態(tài)矩陣    end 

21、                    end figure ANGLE=ANGLE*180/pi;plot(tm,ANGLE(1,:),'r-',tm,ANGLE(2,:),'g.',tm,ANGLE(3,:),'b-.');legend('Pitch Angel','Roll Angel','Yaw Angel');title('Gesture Calculation');xlabel('Time / (10ms)');ylabel(&