navigation_angles.rar_四元数 matlab_四元数导航_四元数陀螺仪_陀螺仪 matlab_陀螺仪信号

close all; clear all; %重力产生的加速度矢量 g=9.8;%单位9.8m/s^2 G=[0,0,-g]'; %****************************读入数据 %**********读入陀螺仪的数据 gyro_x=load('gyrox.txt'); gyro_y=load('gyroy.txt'); gyro_z=load('gyroz.txt'); %****************读入加速度计的数据************** %acc_rate=3/1024; acc_x =load('acceleratex.txt'); acc_y =load('acceleratey.txt'); acc_z=load('acceleratez.txt'); %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %加速度数字转换为模拟电压 data_acc=[acc_x;acc_y;acc_z]; data_acc=data_acc/1024*3 %将数据转换为相应的加速度值 % %********************************************************* %加速度计三个轴向的零点电压 %zero_ax=? %zero_ay=? %zero_az=? %加速度计三个轴向的电压/加速度比值 %rate_ax=? %单位是m/s^2/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_x=mean(acc_x) aver_acc_y=mean(acc_y) aver_acc_z=mean(acc_z) %采样时间 dtime=0.01; tm=0:dtime:0.01* (size(gyro_x,2)-1); %个数num n_point=size(gyro_x,2); %图1 figure plot(tm,data_acc(1,:),'-',tm,data_acc(2,:),'.',tm,data_acc(3,:),'-.'); title('加速度计的采样曲线'); legend('x_ACC','Y_ACC','Z_ACC'); xlabel('Time / (10ms)'); ylabel('Accelerate/ (m/s'')'); grid on; %plot(tm,acc_x,'-',tm,acc_y,'.',tm,acc_z,'-.'); %title('加速度的计的采样曲线'): %对采样曲线进行低通滤波 a=[1,2,4,2,1]; %gyro_x=filter(a/sum(a),1,gyro_x); %gyro_y=filter(a/sum(a),1,gyro_y); %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; %零点电压--陀螺仪，取前80个数的平均电压 zero_gx=sum(gyro_x(1:80))/80 zero_gy=sum(gyro_y(1:80))/80 zero_gz=sum(gyro_z(1:80))/80 %减去零点 gyro_x=(gyro_x-zero_gx)/0.0125/180*pi; gyro_y=(gyro_y-zero_gy)/0.0125/180*pi; gyro_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; %测试数据 accelerate=zeros(3,n_point); accelerate(1,1:100)=10; accelerate(1,101:200)=-10; accelerate(1,201:300)=0; %陀螺仪数据 gyro_x=zeros(1,n_point); gyro_y=zeros(1,n_point); gyro_z=zeros(1,n_point); gyro_z(1:100)=pi/3; gyro_z(101:200)=-pi/3; %重力轴始终有加速度 accelerate(3,:)=accelerate(3,:)+9.8; figure plot(tm,accelerate(1,:),'-',tm,accelerate(2,:),'.',tm,accelerate(3,:),'-.'); title('加速度计的采样曲线'); legend('x_ACC','Y_ACC','Z_ACC'); xlabel('Time / (10ms)'); ylabel('Accelerate/ (m/s'')'); grid on; %画出陀螺仪的采样曲线 figure plot(tm,gyro_x,'r-',tm,gyro_y,'g.',tm,gyro_z,'b-.'); title('陀螺仪的采样曲线'); legend('x_Gyro','Y_Gyro','Z_Gyro'); xlabel('Time / (10ms)'); ylabel('Angel_rate/ (degree/s)'); grid on; %size(gyro_x) %size(gyro_y) %size(gyro_z) data_gyro=[gyro_x;gyro_y;gyro_z]; %转移矩阵--即方向余弦矩阵 T=eye(3); %T是3*3的单位矩阵,初始转移矩阵 %四元数矩阵，存储每步更新之后的四元数，方便以后绘图 Q=zeros(4,n_point); %四元数的初始值确定，假定一开始导航坐标系与载体坐标系是重合的，因此方向余弦矩阵，是单位矩阵，利用它们之间的关系确定四元数的初始值。 Q(1,1)=0.5*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)); %参见捷联惯性导航技术31页3.64式 在旋转90度时不适用 %Q(1,1)=0.5*sqrt(1+T(1,1)+T(2,2)+T(3,3)); %Q(2,1)=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)); %求姿态角矩阵 ANGLE=zeros(3,n_point);%angle[1]代表绕X轴转过的角度，2代表Y轴,3代表Z轴 %方向余弦矩阵到欧拉角的转换关系，这里注意旋转顺序是Z-Y-X，参考文献<<一种全新的全角度元元数与欧拉角的转换算法>> %位置矩阵 position=zeros(3,n_point); %位置矩阵 velocity=zeros(3,n_point); %速度矩阵 %重力加速度 %acc_g=[0,0,-9.8]'; qh=[0,0,0,0]; for i=1:n_point %开始循环 if i>1 velocity(:,i)=((T*accelerate(:,i-1)+T*accelerate(:,i))/2+G)*dtime+velocity(:,i-1);%要考虑到重力的影响，假定重量方向与子轴方向一致 position(:,i)=position(:,i-1)+(velocity(:,i-1)+velocity(:,i))*dtime/2; end %计算欧拉角,假定俯仰角在+_90度范围移动，而滚动角和偏航角在+-180度范围内取值 %ANGLE(1,i)=atan(T(2,3)/T(3,3)); %ANGLE(2,i)=asin(-T(1,3)); %ANGLE(3,i)=atan(T(1,2)/T(1,1)); if T(3,3)>0 %根据物理意义不可能出现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%公式似乎有误，直接按公式计算是负值 ANGLE(3,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)); %更新四元数 if i<n_point %如果还没有到超出数组范围 theta=data_gyro(:,i)*dtime;%角度向量 dtheta=sqrt(theta'*theta); %i要保证当theta为零时算法仍有关效 if dtheta==0 qh=[1,0,0,0]; else %换用简化算法试验结果 %qh=[cos(dtheta);theta*sin(dtheta/2)/dtheta]; qh=[1;0.5*theta]; end % 更新四元数 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,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,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))]; %得到姿态矩阵 end 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('Angel/ (degree)'); grid on; figure plot(tm,velocity) legend('Navigation Coordinate: X','Navigation Coordinate: Y','Navigation Coordinate: Z'); title('Velocity Calculation'); xlabel('Time / (10ms)'); ylabel('Velocity/ (m/s)'); grid on; figure plot(tm,position); legend('Navigation Coordinate: X','Navigation Coordinate: Y','Navigation Coordinate: Z'); title('Position Calculation'); xlabel('Time / (10ms)'); ylabel('Position/ (m)'); grid on; figure plot3(position(1,:),position(2,:),position(3,:)); grid on %roll：横滚角　x轴 %pitch:俯仰解：　y axis %yaw偏航角 z axis