北航导航课程设计捷联惯导.docx

/*/该程序为导航专业课程设计程序/该程序内容为：通过捷联惯性导航系统已知的陀螺仪和加速度计的输出通过四元数以及四阶龙格库塔法来计算经纬度以及姿态角（没有高度通道）/该程序版本为：1.4/该版本的更新为：数据改为实时读取，修正位置矩阵微分中错误/注意事项：该程序需要把惯性器件输出文件（imu_ENU.txt）放入程序根目录中/*/*以下是宏定义部分*/# include # include # include # include # define initial_alignment 0 /条件编译命令，0：使用已有姿态角；1：使用初始对准# define R 6378137.0 /地球半径# define fe (1.0/298.257) /地球椭率# define G 9.7803 /重力加速度# define deg (180/pi) /弧度转化为度的系数# define rad (pi/180) /弧度转化为度的系数# define pi 3.14159265358979323846 /圆周率# define wie (7292115e-11) /地球自转角速度# define t 0.020 /采样周期20ms# define times 60000 /陀螺仪、加速度计以及外部高度输出数据数目# define lambda0 (1.1615326e+002)*rad /初始经度# define L0 (3.9813330e+001)*rad /初始纬度# define h0 7.0000000e+001 /初始高度# define Ve0 0 /初始东向速度# define Vn0 0 /初始北向速度# define Vu0 0 /初始天向速度# define fe0 0 /初始东向加速度# define fn0 0 /初始北向加速度# define fu0 0 /初始天向加速度# define psi0 (8.9850004e+001)*rad /初始偏航角# define theta0 (1.9113951e+000)*rad /初始俯仰角# define gamma0 (1.0572407e+000)*rad /初始滚转角# define init_times 200 /初始对准时用的数据平均的数/*以下是常用全局变量的定义*/long double time;long double a3,w3; /x方向加速度，y方向加速度，z方向加速度，x轴上角速度，y轴上角速度，z轴上角速度long double T33; /姿态矩阵Tbnlong double C33; /位置矩阵Cenlong double q4; /四元数参数long double psi,theta,gamma; /偏航角、俯仰角、滚转角long double lambda,L,h; /经度、纬度、高度long double wcmd3; /平台指令角速度long double Rm,Rn; /子午面半径，卯酉圈半径long int i=0; /计算次数long double dq4; /四元数参数的微分long double W44; /用于计算四元数参数微分的矩阵long double temp3; /三行三列的矩阵与三行一列的矩阵相乘的结果long double wepp3; /x,y,zlong double wnbb3; /x,y,zlong double Cwepp33; /用于C矩阵的更新long double dC33; /C矩阵的微分long double dV3; /速度的微分，x，y，z，分别对应东，北，天long double V3; /速度，x，y，z，分别对应东，北，天long double wiep3; /地球自转角速度char flag1=1; /用于四元数的四阶龙格库塔法积分的标志位char flag2=1; /用于速度的二阶龙格库塔法积分的标志位long double k14;long double k24;long double k34;long double k53;/*以下是程序声明部分*/void renew_T(); /姿态矩阵更新void renew_attitude_angle(); /姿态角更新void init_T(); /姿态矩阵初始化void renew_dV(); /加速度初始化void _3x3multiply3x3(long double a33,long double b33,long double temp233); /3x3矩阵与3x3矩阵相乘void quaternion_differential(); /计算四元数Q的微分long double integral_C(long double a,long double b); /位置矩阵的积分（使用一阶）void forth_order_Runge_Kutta_method(); /四阶龙格库塔法积分计算姿态角void second_order_Runge_Kutta_method(); /二阶龙格库塔法积分计算速度void _3x3multiply3x1(long double a33,long double b3); /3x3矩阵与3行列向量乘积void outer_product_of_vector(long double a3,long double b3,long double c3); /向量外积long double sign(long double i); /符号函数void C_differential(); /位置矩阵微分void quaternion_normalize(); /四元数归一化void renew_wcmd(); /平台指令角速度更新void renew_wepp(); void renew_wiep(); /地球自转角速度更新void renew_R(); /地球半径更新void renew1(); /更新1void init_alignment(); /初始对准（粗对准）void init_T(); /姿态矩阵初始化void init_C(); /位置矩阵初始化void init_T_using_data(); /使用已知数据初始化姿态矩阵void init(); /总初始化void renew_wnbb(); /更新姿态矩阵速率void renew_T(); /更新姿态矩阵void renew_dV(); /更新加速度void renew_attitude_angle(); /更新姿态角void renew_position(); /更新位置void renew_C(); /更新位置矩阵void renew2(); /更新2/*以下是常用数学程序部分*/void _3x3multiply3x3(long double a33,long double b33,long double temp233) /三行三列的矩阵与三行三列的矩阵相乘，结果写入temp233中char s,d,k;for(s=0;s3;s+)for(d=0;d3;d+)temp2sd=0;for(k=0;k3;k+)temp2sd+=ask*bkd;void quaternion_differential() /计算四元数参数的微分W00=0;W01=-wnbb0;W02=-wnbb1;W03=-wnbb2;W10=wnbb0;W11=0;W12=wnbb2;W13=-wnbb1;W20=wnbb1;W21=-wnbb2;W22=0;W23=wnbb0;W30=wnbb2;W31=wnbb1;W32=-wnbb0;W33=0;char j,k;for(j=0;j4;j+)dqj=0;for(j=0;j4;j+)for(k=0;k4;k+)dqj+=0.5*Wjk*qk;long double integral_C(long double a,long double b) /用于位置矩阵更新的积分return (a+b*t/2); /一阶积分void forth_order_Runge_Kutta_method() /四阶龙格库塔法求四元数char p;long double tmp4;for(p=0;p4;p+)tmpp=qp;if(flag1=1)quaternion_differential(); /微分得到dqfor(p=0;p4;p+)k1p=dqp;flag1+;elseif(flag1=2)for(p=0;p4;p+)qp=qp+t/2*k1p;quaternion_differential(); /微分得到dqfor(p=0;p4;p+)k2p=dqp;qp=tmpp;for(p=0;p4;p+)qp=qp+t/2*k2p;quaternion_differential(); /微分得到dqfor(p=0;p4;p+)k3p=dqp;qp=tmpp;flag1+;elseif(flag1=3)for(p=0;p4;p+)qp=qp+t*k3p;quaternion_differential(); /微分得到dqfor(p=0;p4;p+)qp=tmpp+t/6*(k1p+2*k2p+2*k3p+dqp); /积分flag1=2;quaternion_differential();for(p=0;p4;p+)k1p=dqp;quaternion_normalize(); /四元数归一化renew_T(); /姿态矩阵更新void second_order_Runge_Kutta_method() /用二阶龙格库塔法更新速度char p;long double tmp3;for(p=0;p3;p+)tmpp=Vp;if(flag2=1)renew_dV();for(p=0;p3;p+)k5p=dVp;flag2+;elsefor(p=0;p3;p+)Vp=Vp+t/2*dVp;renew_dV();for(p=0;p3;p+)Vp=tmpp+t/4*(k5p+dVp); /积分renew_dV();for(p=0;p3;p+)k5p=dVp;void _3x3multiply3x1(long double a33,long double b3) /三行三列的矩阵与三行一列的矩阵相乘，结果写入temp3中char i,j;long double c3;for(i=0;i3;i+)ci=bi;for(i=0;i3;i+)tempi=0;for(j=0;j3;j+)tempi+=aij*cj;void outer_product_of_vector(long double a3,long double b3,long double c3) /c=axb，向量外积long double k3;char i;for(i=0;i=0)return 1.0;elsereturn -1.0;void C_differential() /位置矩阵的微分Cwepp00=0;Cwepp01=wepp2;Cwepp02=-wepp1;Cwepp10=-wepp2;Cwepp11=0;Cwepp12=wepp0;Cwepp20=wepp1;Cwepp21=-wepp0;Cwepp22=0;_3x3multiply3x3(Cwepp,C,dC); /结果写入dC33void quaternion_normalize() /四元数归一化long double temp;temp=sqrt(q0*q0+q1*q1+q2*q2+q3*q3);q0=q0/temp;q1=q1/temp;q2=q2/temp;q3=q3/temp;/*以下是更新1部分内容*/void renew_wcmd() /平台指令角速度更新（wipp）wcmd0=wepp0+wiep0;wcmd1=wiep1+wepp1;wcmd2=wiep2+wepp2;void renew_wepp()wepp0=-V1/(h+Rm);wepp1=V0/(Rn+h);wepp2=V0/(Rn+h)*tan(L);void renew_wiep() /更新地球自转角速度wiep0=0;wiep1=wie*cos(L);wiep2=wie*sin(L);void renew_R() /地球半径更新Rm=R*(1-2*fe+3*fe*sin(L)*sin(L); /更新子午面半径Rn=R*(1+fe*sin(L)*sin(L); /更新卯酉圈半径void renew1() /更新1（地理信息和平台指令角速度更新）renew_R(); /更新地球半径renew_wepp();renew_wiep(); /更新地球自转角速度renew_wcmd(); /更新平台指令角速度/*以下是初始化部分*/void init_alignment() /初始对准（粗对准）long double Vbt33=0,0,0,0,0,0,0,0,0;long double Vnt_inverse33=0,0,0,0,0,0,0,0,0;int i;long double fx=0,fy=0,fz=0,Wx=0,Wy=0,Wz=0,a,b,c,d,e,f,g;FILE *tmp;if(tmp=fopen(imu_ENU.txt,r)=NULL) /打开文件，读入数据模式printf(无法打开文件imu_ENU.txtn);exit(0);for(i=0;iinit_times;i+) /求一段时间的平均值来算初始姿态角fscanf(tmp,%lf %lf %lf %lf %lf %lf %lf ,&a,&b,&c,&d,&e,&f,&g); /时间，x轴加速度，y轴加速度，z轴加速度，x轴角速度，y轴角速度，z轴角速度fx-=b; /加速度均取负值fy-=c;fz-=d;Wx+=e;Wy+=f;Wz+=g;fclose(tmp); /关闭文件指针fx=fx/init_times;fy=fy/init_times;fz=fz/init_times;Wx=Wx/init_times;Wy=Wy/init_times;Wz=Wz/init_times;Vbt00=fx;Vbt01=fy;Vbt02=fz;Vbt10=Wx;Vbt11=Wy;Vbt12=Wz;Vbt20=fy*Wz-fz*Wy;Vbt21=fz*Wx-fx*Wz;Vbt22=fx*Wy-fy*Wx;Vnt_inverse02=1/(G*wie*cos(L0);Vnt_inverse10=tan(L0)/G;Vnt_inverse11=1/(wie*cos(L0);Vnt_inverse20=-1/G;_3x3multiply3x3(Vnt_inverse,Vbt,T); /求出姿态矩阵，此姿态矩阵未经正交化renew_attitude_angle(); /计算姿态角（正交化）init_T(); /姿态矩阵初始化，至此，正交化完成void init_T() /姿态矩阵初始化T00=cos(gamma)*cos(psi)-sin(gamma)*sin(theta)*sin(psi); /通过姿态角求姿态矩阵T01=-cos(theta)*sin(psi);T02=sin(gamma)*cos(psi)+cos(gamma)*sin(theta)*sin(psi);T10=cos(gamma)*sin(psi)+sin(gamma)*sin(theta)*cos(psi);T11=cos(theta)*cos(psi);T12=sin(gamma)*sin(psi)-cos(gamma)*sin(theta)*cos(psi);T20=-sin(gamma)*cos(theta);T21=sin(theta);T22=cos(gamma)*cos(theta);q1=sign(T21-T12)*0.5*sqrt(1+T00-T11-T22); /求四元数各项q2=sign(T02-T20)*0.5*sqrt(1-T00+T11-T22);q3=sign(T10-T01)*0.5*sqrt(1-T00-T11+T22);q0=sqrt(1-q1*q1-q2*q2-q3*q3);quaternion_normalize(); /四元数正交化renew_T(); /重新通过四元数得到姿态矩阵void init_C() /位置矩阵初始化C00=-sin(lambda);C01=cos(lambda);C02=0;C10=-sin(L)*cos(lambda);C11=-sin(L)*sin(lambda);C12=cos(L);C20=cos(lambda)*cos(L);C21=cos(L)*sin(lambda);C22=sin(L);void init_T_using_data()psi=psi0; /初始偏航角theta=theta0; /初始俯仰角gamma=gamma0; /初始滚转角init_T(); /姿态矩阵初始化void init() /初始化lambda=lambda0; /初始经度L=L0; /初始纬度h=h0; /初始高度V0=Ve0; /初始东向速度V1=Vn0; /初始北向速度V2=Vu0; /初始天向速度a0=fe0; /初始东向加速度a1=fn0; /初始北向加速度a2=fu0; /初始天向加速度#if initial_alignmentinit_alignment(); /初始对准得到T#elseinit_T_using_data(); /采用已知数据得到T#endifinit_C(); /初始化位置矩阵renew_R(); /初始化地球半径renew_wepp();renew_wiep(); /初始化地球自转角速度renew_wcmd(); /初始化平台指令角速度/*以下是更新2部分*/void renew_wnbb()long double Tnb33;char z;char y;for(z=0;z3;z+) /求T的转置for(y=0;y3;y+)Tnbzy=Tyz;_3x3multiply3x1(Tnb,wcmd); /改变temp3for(z=0;z3;z+)wnbbz=wz-tempz;void renew_T() /通过四元数求得姿态矩阵T00=q0*q0+q1*q1-q2*q2-q3*q3;T01=2*(q1*q2-q0*q3);T02=2*(q1*q3+q0*q2);T10=2*(q1*q2+q0*q3);T11=q0*q0-q1*q1+q2*q2-q3*q3;T12=2*(q2*q3-q0*q1);T20=2*(q1*q3-q2*q0);T21=2*(q2*q3+q0*q1);T22=q0*q0-q1*q1-q2*q2+q3*q3;void renew_dV() /更新加速度long double tmp13,tmp23;char k;for(k=0;k3;k+)tmp2k=2*wiepk+weppk;outer_product_of_vector(tmp2,V,tmp1);_3x3multiply3x1(T,a); /改变temp3for(k=0;k3;k+)dVk=tempk-tmp1k;dV2=dV2-G; /减去重力加速度void renew_attitude_angle() /计算姿态角psi=atan(-T01/T11); /偏航角if(psi0) /偏航角主值psi=psi+2*pi;else if(T11=0)psi=psi+pi;theta=atan(T21/sqrt(T01*T01+T11*T11); /俯仰角gamma=atan(-T20/T22); /横滚角if(T220)gamma=gamma-pi;elsegamma=gamma+pi;void renew_position() /计算经纬度renew_C(); /位置矩阵更新lambda=atan(C21/C20); /经度if(C200)lambda=lambda-pi;elselambda=lambda+pi;L=atan(C22/sqrt(C20*C20+C21*C21); /纬度void renew_C() /位置矩阵更新char s,d;C_differential(); /位置