强烈赞扬，有控火箭有希望了！

惯导的结果最终需要机械来实现，希望站友们也关注一下机械实现的问题，可以另开新帖。

现在我们知道发射的时候火箭的位置、高度和方位角。然后根据传感器的测试结果，计算得到火箭的实时位置、高度、方位角和转动速度，与预定航线比较，计算得到需要的纠正量，以适当的速率进行纠正。这一块还涉及到火箭的气动参数，以便求得舵面角度，并以适当的控制方式驱动舵面转动。舵的变动需要时间，导致响应滞后，这又需要修正。通俗的讲大概就是这个样子。

膜拜一下~  

猴子的计算果然强大唉~~~

姿态矩阵比四元数精度更高,



  

当然这部分只是INS中超简单的一部分, sculling之类的问题才是INS的难点.

计算姿态角不用kalman估计的话很蛋碎, 左边X轴陀螺测量值 右边积分值, X轴姿态角
kalman滤波, "H[这个是上标..是上标 8<- 躺下来的]" 算法. ADXRS610, AD7706. 目测漂移 0.6度/h

忽然想起量子力学测不准理论。貌似速度和位置两者不能同时测出

采用四元数法进行解算，是有一定道理的
欧拉角（三参数）法不考虑

四参数法与九参数法精度比较：
XXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXX
效率比较：
XXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXX

PS:我没有验证过上述文章的观点，今天早上刚看九参数法，中午写个程序试试

我感觉以MCU的运算速度  ~~~~用什么方法都不是问题（以目前的定位）


机械要命阿   设计不是问题  制作出来才是问题   ~~~~加工费用好可怕



我也作了一个简单的  怎么样说了  4线水平器把 （也不记得叫什么先这样以把） 以前用银作过 现在导电液体用盐水替代 电阻在  80~50KOMH 在一个圆柱瓶内 装入盐水，  盐水与地线连接  4条线成90度分布  这个一个简简单单 的 XY轴就出来

目前计划 ~~~ 这东西只能控制到垂直向上飞   控制方向的机械部分~~~直的好蛋疼阿

[s:248]看一下都蛋疼....................

关于四元数的值是如何获得的：

1.这里没有做初始对准，假设一开始载体坐标系和导航坐标系是重合的，也就是尾翼I指东，II指北，火箭头指向天顶。
这时候Q = [1 0 0 0];（实部1 虚步0） T = [1 0 0;0 1 0;0 0 1];（就是单位矩阵）。于是我们有了Q的初始值。
下一步打算用GPS结合加速度计、磁阻传感器数据来确定Q的初始值，MEMS陀螺要做初始对准还是比较难的。
2.有了初始值之后，根据更新公式，不断用角速度值来更新Q的数值。

找到一篇支持93观点的文章：

捷联惯性导航制导系统中方向余弦矩阵的递推算法.pdf 1.11MB PDF 918次下载 预览
其实更新DCM比四元数法推导还要简单，但DCM的正交化较为麻烦。

附上姿态矩阵法代码，此处只有DCM更新部分，把楼主位代码的四元数运算部分换掉即可：

更新DCM矩阵的快速算法

as = 1 - nt2/6 + nt2^2/120 - nt2^3/5040 + .....
nt2是theta的模的平方，分子按照-nt2,nt2^2,-nt2^3,nt2^4...变化，分母按照3!,5!,7!....变化
ac = 1/2 - nt2/24 +nt2^2/720 -nt2^3/40320 + .....
不同的是分母按照4!,6!,8!.....变化

似乎此方法对截断误差更加敏感，as、ac只取前两项时，精度很差。
而四元数法在只取前一项（r = [1 thetax/2 thetay/2 thetaz/2 ]）时，精度已经能满足要求。
以上只是定性分析，稍后进行更详细的比较。

gps跟得上么？？估计要飞一段查一下，飞一段查一下了吧。。
还是觉得固定航线，偏移了再修正靠谱，但是陀螺仪的温漂修正了还是会有误差，其实加速计阵列的方案也可以，虽然误差累计，但保证60s里没问题，算法有点烦，不过应该都带得起来了。
无陀螺惯导导航方法初步研究.ppt 1.13MB PPT 213次下载

现在这个就是无陀螺系统，单轴MEMS陀螺里就是一对加速度计。

引入GPS和红外地平仪，就可以实时修正误差了。还可以对器件进行实时校准

目前用在KC的火箭上还比较难。建议虎哥赞助猴子一套涡喷系统，先搞个巡航导弹研究研究。

涡喷有压力啊。。。建议赞助一套冲压，后面绑个大号固体发动机和几个助推器。

我有带TVC的水箭一套

引用第16楼warmonkey于2011-06-15 08:48发表的  :
我有带TVC的水箭一套

仿燃气舵方式的TVC？

喷嘴摆动TVC

我闻到弹道导弹的味道了

用四元数中的三子样解算就够用了，现在大部分用四元数是道理的，因为他没有奇点的问题，数据更新性适中。 另外看到有个仁兄说ADXRS160有0.6°/h的稳定性，我表示非常不认同，请看我发的测试报告。

觉得在算法上，还是用龙格库塔法解那个四元数微分方程好些，最好是四阶，关于位置和速度可以用一阶欧拉法就可以了 。可是我还不会用这个办法解，请高人支个招。毕卡法，也不会。我就在用数学工具处蛋痛。。。[s:274]

民间的导弹专家么？虽然看不懂，也要举双手双脚支持你！希望你将来创办一个中国首家民办军工厂！

好像没有把那个正弦和余弦用泰勒级数展开，我在一本书上看到，好像说不展开时，则要求必须是什么时间用那个式子，就什么时候计算，这样当然完美，但计算机的处理是要时间的，为了能在计算机中有效处理，得展开舍去高级项。




看到了，版主给出来了，就这个。
as = 1 - nt2/6 + nt2^2/120 - nt2^3/5040 + .....
nt2是theta的模的平方，分子按照-nt2,nt2^2,-nt2^3,nt2^4...变化，分母按照3!,5!,7!....变化
ac = 1/2 - nt2/24 +nt2^2/720 -nt2^3/40320 + .....
不同的是分母按照4!,6!,8!.....变化

似乎此方法对截断误差更加敏感，as、ac只取前两项时，精度很差。
而四元数法在只取前一项（r = [1 thetax/2 thetay/2 thetaz/2 ]）时，精度已经能满足要求。
以上只是定性分析，稍后进行更详细的比较。

void init_Q(void)
{////////pitch  roll  yaw
q0=cos(roll/2)*cos(pitch/2)*cos(yaw/2)+sin(roll/2)*sin(yaw/2)*sin(yaw/2);

}
void angle()  //姿态矩阵
{
float DX,DX1,DX2,DX3,DY,DY1,DY2,DY3,DZ,DZ1,DZ2,DZ3;
float model,h0,h1,h2,h3;  //h表示增量四元数
float q0,q1,q2,q3;          //q表示更新前的四元数
float model_d,d0,d1,d2,d3;   //d表示更新后的四元数
float T11,T12,T13,T21,T22,T23,T31,T32,T33;

//三子样采样优化 D表示角增量
DX=DX1+DX2+DX3+(9/20)*(DY1*DZ3+DZ1*DY3)+(27/40)*(DY2*(DZ3-DZ1)+DX2*(DX3-DX1));

DY=DY1+DY2+DY3+(9/20)*(DY1*DY3+DX1*DZ3)+(27/40)*(DZ2*(DX3-DX1)+DY2*(DY3-DY1));

DZ=DZ1+DZ2+DZ3+(9/20)*(DZ1*DZ3+DY1*DX3)+(27/40)*(DX2*(DY3-DY1)+DY2*(DZ3-DZ1));

//计算增量四元数h
model=sqrt(DX*DX+DY*DY+DZ*DZ);
if(model==0)
{
  h0=1;  
  h1=0;
  h2=0;
  h3=0;
}
else
{
  h0=cos(model/2);
  h1=(DX/model)*sin(model/2);
  h2=(DY/model)*sin(model/2);    
  h3=(DZ/model)*sin(model/2);    
}

//四元数更新
d0=q0*h0-q1*h1-q2*h2-q3*h3;
///////////////////////////
d1=q1*h0+q0*h1-q3*h2+q2*h3;      
d2=q2*h0+q3*h1+q0*h2-q1*h3;    
d3=q3*h0-q2*h1+q1*h2+q0*h3;
//进行规范化
model_d=sqrt(d0*d0+d1*d1+d2*d2+d3*d3);
d0=d0/model_d;
d1=d1/model_d;
d2=d2/model_d;
d3=d3/model_d;
//将更新和规范化之后的四元数d赋给q,以便下一次的更新
q0=d0;
q1=d1;
q2=d2;
q3=d3;

//将四元数转化成方向余弦矩阵
T11=q0*q0+q1*q1-q2*q2-q3*q3;
T12=2*(q1*q2-q0*q3);
T13=2*(q1*q3+q0*q2);

T21=2*(q1*q2+q0*q3);
T22=q0*q0-q1*q1+q2*q2-q3*q3;
T23=2*(q2*q3-q0*q1);

T31=2*(q1*q3-q0*q2);
T32=2*(q2*q3+q0*q1);
T33=q0*q0-q1*q1-q2*q2+q3*q3;

//提取姿态角
pitch=asin(T32);
if(T22==0&&T12>0)
yaw=pi/2;
if(T22==0&&T12<0)
yaw=-pi/2;
if(T22>0&&T12>0)
yaw=atan(-T31/T33);
if(T22>0&&T12<0)
yaw=atan(-T31/T33);
if(T22<0&&T12>0)
yaw=atan(-T31/T33)+2*pi;
if(T22<0&&T12<0)
yaw=atan(-T31/T33)-2*pi;
}

有没有试过，不用GPS，直接用加表在静止时的测量值算出姿态里的俯仰和倾斜角，再用欧拉角表示四元数，这样就有了更合理的四元数初值，再用四元数表示姿态阵，当然姿态阵也是比较理想的。这时的位置矩阵就直接输入所在地的精确值。

资料都收了，谢谢各位大侠

回复24楼，26楼：

这个程序不错，该有的东西都有了，三子样精度要好些。
现在我是用加表测倾角，地磁测方向，GPS差分测位置，完成初始对准过程。

目前正在做进一步的研究，打算增加更多姿态参考数据源

角速度叉乘写错了

关于算法精度选择的补充
江南航天集团302所某人的文章《捷联式惯性导航积分算法设计》中提到，把算法误差控制在传感器误差的5%以下即可。
我们把标准抬高到1%，MEMS陀螺噪声最少也有1E-4deg/s，零漂5deg/h，量程一般少于500deg/s，故算法精度达到1E-7已经足够了
按照这个标准，改进的三子样算法足以对付。

DCM方法难以实现3子样解算，因为难以用rung kutta得到更新公式，计算量巨大，也不能直接EKF

非常好的技术帖子，真是我一直思索的问题。

管同学要了几个组合导航的ppt和matlab程序，供参考
integrated navigation.rar 1.23MB RAR 97次下载

搞涡喷的话，我可以配合做ECU哦

楼主，四元数转动微分方程的1/2哪去了？

给你补个图吧。

实物图是这样的

我擦，看不懂啊。这个是设计矩阵的？高中学的是极坐标、不等式，没学这个，看的好复杂。

季节逝去 发表于 2014-5-1 18:33
我擦，看不懂啊。这个是设计矩阵的？高中学的是极坐标、不等式，没学这个，看的好复杂。

嗯嗯. 四元数这个是挺难看懂，尤其是没人教 上网查的时候更本看不懂，  我看了半年才搞懂了什么是四元数. 四元数跟欧拉角跟轴角表示跟姿态矩阵都能表示旋转， 就是一个坐标轴旋转到另外一个坐标轴. 并不是需要很清楚是怎么出来的. 知道怎么转换，各种的优点，然后怎么把你陀螺仪加速计转换为这些角度，融合的过程才是最困难的、

本帖最后由 大号是中华 于 2014-5-9 13:52 编辑

虎哥 发表于 2011-5-28 00:59
强烈赞扬，有控火箭有希望了！

惯导的结果最终需要机械来实现，希望站友们也关注一下机械实现的问题，可以 ...

虎哥，科创商城不是由卖惯导的吗？性能如何？
和平时用的中兴环宇ZX-NA620B哪个好？

惯导性能主要看什么啊？
定位精度？分辨率？动态精度？

数学功底很重要

惯导平台那一堆东西，超贵的

36楼是真的火箭内部吗？

为什么觉得四楼旋转矩阵方法太浪费资源了、、、、计算三角函数时间可以省去的话（固定值可以再循环开始之前计算好）乘法数量也会大于四元数法吧。。。。