[科创航天]惯性导航原理与仿真
通俗一点描述惯导:
火箭上装有   加速度计   和   角速率陀螺仪,先测出火箭方位(倾角)。
再把 加速度 折合成地面观察到的数值,积分后得速度和位移。

[新增]有奖问答(1000kcb):
转动四元数r按原公式(倒数第三幅图)计算,出了什么问题?
提示:化简公式r = [1 thetax/2 thetay/2 thetaz/2 ] 没有这个问题。


现在,我们来讲讲具体实现:

坐标系定义
108250


信号流
108251

关键就在T(t,b)这个矩阵,它可以把火箭上的加速度转换成地面观测到的加速度。
借助四元数这个桥梁,可以大大简化计算。

108252

注意加速度变换的方法,一个矩阵乘法搞定。

上面的四元数Q怎么得到的呢?看这里
108253

  108254


四元数需要规范化处理,以保证方向余弦矩阵是一个正交矩阵
可以理解为保持数学平台(也就是方向余弦矩阵)3轴是垂直的,
平台惯导通过准确的加工安装来保证正交性
108255


到此已经解决了忽略地球自转的情况下,解算运动参数的问题。
还有3个主要问题:
1.如何得到姿态角?
2.如何确定四元数初始值?(也就是确定火箭初始方位)
3.考虑地球自转
精彩待续

附演示代码:
matlab.rar383k41次

先导入数据文件(mat格式,双击导入),再运行instest.m108256

+1000  科创币    科创论坛   2011-05-28   四元数的值是怎么来的?
+1000  科创币    ehco   2011-05-28   虽然看不懂,但知道很有用。
+1000  科创币    wldshy   2011-05-29   纯支持
来自 空间技术
 
1
2011-5-28 00:59:45
1楼
强烈赞扬,有控火箭有希望了!

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

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

猴子的计算果然强大唉~~~
折叠评论
加载评论中,请稍候...
折叠评论
3楼
姿态矩阵比四元数精度更高,

108258


108259
  

当然这部分只是INS中超简单的一部分, sculling之类的问题才是INS的难点.
折叠评论
加载评论中,请稍候...
折叠评论
4楼
计算姿态角不用kalman估计的话很蛋碎, 左边X轴陀螺测量值 右边积分值, X轴姿态角
kalman滤波, "H[这个是上标..是上标 8<- 躺下来的]" 算法. ADXRS610, AD7706. 目测漂移 0.6度/h108260

+100  科创币    jrcsh   2011-05-28   
折叠评论
加载评论中,请稍候...
折叠评论
5楼
忽然想起量子力学测不准理论。貌似速度和位置两者不能同时测出
+1  科创币    epi.clyce   2011-05-28   你NB,在这里面用测不准~~
折叠评论
加载评论中,请稍候...
折叠评论
warmonkey(作者)
6楼
采用四元数法进行解算,是有一定道理的
欧拉角(三参数)法不考虑

四参数法与九参数法精度比较:
http://wenku.baidu.com/view/e85e2ad376a20029bd642de5.html
效率比较:
http://www.cnblogs.com/soroman/archive/2006/09/19/509597.html

PS:我没有验证过上述文章的观点,今天早上刚看九参数法,中午写个程序试试
折叠评论
加载评论中,请稍候...
折叠评论
7楼
我感觉以MCU的运算速度  ~~~~用什么方法都不是问题(以目前的定位)


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



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

目前计划 ~~~ 这东西只能控制到垂直向上飞   控制方向的机械部分~~~直的好蛋疼阿
折叠评论
加载评论中,请稍候...
折叠评论
8楼
[s:248]看一下都蛋疼....................
折叠评论
加载评论中,请稍候...
折叠评论
warmonkey(作者)
9楼
关于四元数的值是如何获得的:

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观点的文章:

其实更新DCM比四元数法推导还要简单,但DCM的正交化较为麻烦。

附上姿态矩阵法代码,此处只有DCM更新部分,把楼主位代码的四元数运算部分换掉即可:
折叠评论
加载评论中,请稍候...
折叠评论
2011-5-29 13:23:08
warmonkey(作者)
10楼
更新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 ])时,精度已经能满足要求。
以上只是定性分析,稍后进行更详细的比较。
折叠评论
加载评论中,请稍候...
折叠评论
2011-6-7 21:01:31
2011-6-7 21:01:31
11楼
gps跟得上么??估计要飞一段查一下,飞一段查一下了吧。。
还是觉得固定航线,偏移了再修正靠谱,但是陀螺仪的温漂修正了还是会有误差,其实加速计阵列的方案也可以,虽然误差累计,但保证60s里没问题,算法有点烦,不过应该都带得起来了。
折叠评论
加载评论中,请稍候...
折叠评论
2011-6-8 21:29:14
2011-6-8 21:29:14
warmonkey(作者)
12楼
现在这个就是无陀螺系统,单轴MEMS陀螺里就是一对加速度计。

引入GPS和红外地平仪,就可以实时修正误差了。还可以对器件进行实时校准
折叠评论
加载评论中,请稍候...
折叠评论
2011-6-14 13:01:08
2011-6-14 13:01:08
13楼
目前用在KC的火箭上还比较难。建议虎哥赞助猴子一套涡喷系统,先搞个巡航导弹研究研究。
折叠评论
加载评论中,请稍候...
折叠评论
14楼
回 14楼(小俊) 的帖子
涡喷有压力啊。。。建议赞助一套冲压,后面绑个大号固体发动机和几个助推器。
折叠评论
加载评论中,请稍候...
折叠评论
2011-6-15 08:48:17
warmonkey(作者)
15楼
我有带TVC的水箭一套
折叠评论
加载评论中,请稍候...
折叠评论
2011-7-3 21:32:58
2011-7-3 21:32:58
16楼
引用第16楼warmonkey于2011-06-15 08:48发表的  :
我有带TVC的水箭一套

仿燃气舵方式的TVC?
折叠评论
加载评论中,请稍候...
折叠评论
2011-7-4 11:39:00
warmonkey(作者)
17楼
喷嘴摆动TVC
折叠评论
加载评论中,请稍候...
折叠评论
2011-7-14 11:45:14
2011-7-14 11:45:14
18楼
我闻到弹道导弹的味道了
折叠评论
加载评论中,请稍候...
折叠评论
2011-8-27 15:42:42
2011-8-27 15:42:42
19楼
用四元数中的三子样解算就够用了,现在大部分用四元数是道理的,因为他没有奇点的问题,数据更新性适中。 另外看到有个仁兄说ADXRS160有0.6°/h的稳定性,我表示非常不认同,请看我发的测试报告。
折叠评论
加载评论中,请稍候...
折叠评论
2011-9-23 14:44:01
2011-9-23 14:44:01
20楼
觉得在算法上,还是用龙格库塔法解那个四元数微分方程好些,最好是四阶,关于位置和速度可以用一阶欧拉法就可以了 。可是我还不会用这个办法解,请高人支个招。毕卡法,也不会。我就在用数学工具处蛋痛。。。[s:274]
折叠评论
加载评论中,请稍候...
折叠评论
2011-9-26 11:43:44
2011-9-26 11:43:44
21楼
民间的导弹专家么?虽然看不懂,也要举双手双脚支持你!希望你将来创办一个中国首家民办军工厂!
折叠评论
加载评论中,请稍候...
折叠评论
2011-10-11 11:01:29
2011-10-11 11:01:29
22楼
回 楼主(warmonkey) 的帖子
好像没有把那个正弦和余弦用泰勒级数展开,我在一本书上看到,好像说不展开时,则要求必须是什么时间用那个式子,就什么时候计算,这样当然完美,但计算机的处理是要时间的,为了能在计算机中有效处理,得展开舍去高级项。




看到了,版主给出来了,就这个。
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 ])时,精度已经能满足要求。
以上只是定性分析,稍后进行更详细的比较。
折叠评论
加载评论中,请稍候...
折叠评论
23楼
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;
}
折叠评论
加载评论中,请稍候...
折叠评论
24楼
请有兴趣的砖家都来会讨一下,期待中。。。
+1  科创币    jrcsh   2012-02-08   高质量发帖
折叠评论
加载评论中,请稍候...
折叠评论
2011-10-12 09:09:46
25楼
回 10楼(warmonkey) 的帖子
有没有试过,不用GPS,直接用加表在静止时的测量值算出姿态里的俯仰和倾斜角,再用欧拉角表示四元数,这样就有了更合理的四元数初值,再用四元数表示姿态阵,当然姿态阵也是比较理想的。这时的位置矩阵就直接输入所在地的精确值。
折叠评论
加载评论中,请稍候...
折叠评论
2011-10-31 11:35:06
2011-10-31 11:35:06
26楼
资料都收了,谢谢各位大侠
折叠评论
加载评论中,请稍候...
折叠评论
2011-11-8 15:05:52
2011-11-8 15:05:52
warmonkey(作者)
27楼
回复24楼,26楼:

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

目前正在做进一步的研究,打算增加更多姿态参考数据源
折叠评论
加载评论中,请稍候...
折叠评论
2012-6-11 10:12:22
2012-6-11 10:12:22
warmonkey(作者)
28楼
回 24楼(fkepdcjgd) 的帖子
角速度叉乘写错了
折叠评论
加载评论中,请稍候...
折叠评论
warmonkey(作者)
29楼
关于算法精度选择的补充
江南航天集团302所某人的文章《捷联式惯性导航积分算法设计》中提到,把算法误差控制在传感器误差的5%以下即可。
我们把标准抬高到1%,MEMS陀螺噪声最少也有1E-4deg/s,零漂5deg/h,量程一般少于500deg/s,故算法精度达到1E-7已经足够了
按照这个标准,改进的三子样算法足以对付。
折叠评论
加载评论中,请稍候...
折叠评论
2013-3-8 20:03:03
2013-3-8 20:03:03
warmonkey(作者)
30楼
DCM方法难以实现3子样解算,因为难以用rung kutta得到更新公式,计算量巨大,也不能直接EKF
折叠评论
加载评论中,请稍候...
折叠评论
2013-3-28 10:13:52
2013-3-28 10:13:52
31楼
非常好的技术帖子,真是我一直思索的问题。
折叠评论
加载评论中,请稍候...
折叠评论
2013-4-23 22:10:26
2013-4-23 22:10:26
32楼
管同学要了几个组合导航的ppt和matlab程序,供参考
折叠评论
加载评论中,请稍候...
折叠评论
2013-4-24 19:28:22
33楼
回 14楼(小俊) 的帖子
搞涡喷的话,我可以配合做ECU哦
折叠评论
加载评论中,请稍候...
折叠评论
2013-12-5 13:37:24
2013-12-5 13:37:24
34楼
楼主,四元数转动微分方程的1/2哪去了?
折叠评论
加载评论中,请稍候...
折叠评论
2013-12-9 21:54:15
2013-12-9 21:54:15
35楼
给你补个图吧。

实物图是这样的
207813




折叠评论
加载评论中,请稍候...
折叠评论
2014-5-1 18:33:55
2014-5-1 18:33:55
36楼
我擦,看不懂啊。这个是设计矩阵的?高中学的是极坐标、不等式,没学这个,看的好复杂。
折叠评论
加载评论中,请稍候...
折叠评论
2014-5-3 04:04:31
2014-5-3 04:04:31
37楼
季节逝去 发表于 2014-5-1 18:33
我擦,看不懂啊。这个是设计矩阵的?高中学的是极坐标、不等式,没学这个,看的好复杂。


嗯嗯. 四元数这个是挺难看懂,尤其是没人教 上网查的时候更本看不懂,  我看了半年才搞懂了什么是四元数. 四元数跟欧拉角跟轴角表示跟姿态矩阵都能表示旋转, 就是一个坐标轴旋转到另外一个坐标轴. 并不是需要很清楚是怎么出来的. 知道怎么转换,各种的优点,然后怎么把你陀螺仪加速计转换为这些角度,融合的过程才是最困难的、
折叠评论
加载评论中,请稍候...
折叠评论
2014-5-9 13:51:08
2014-5-9 13:51:08
38楼
本帖最后由 大号是中华 于 2014-5-9 13:52 编辑

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

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


虎哥,科创商城不是由卖惯导的吗?性能如何?
和平时用的中兴环宇ZX-NA620B哪个好?
折叠评论
加载评论中,请稍候...
折叠评论
39楼
惯导性能主要看什么啊?
定位精度?分辨率?动态精度?
折叠评论
加载评论中,请稍候...
折叠评论
2014-9-26 13:50:13
2014-9-26 13:50:13
40楼
数学功底很重要
折叠评论
加载评论中,请稍候...
折叠评论
2014-9-30 11:38:25
2014-9-30 11:38:25
41楼
惯导平台那一堆东西,超贵的
折叠评论
加载评论中,请稍候...
折叠评论
42楼
36楼是真的火箭内部吗?
折叠评论
加载评论中,请稍候...
折叠评论
2014-10-12 10:46:56
2014-10-12 10:46:56
43楼
为什么觉得四楼旋转矩阵方法太浪费资源了、、、、计算三角函数时间可以省去的话(固定值可以再循环开始之前计算好)乘法数量也会大于四元数法吧。。。。
折叠评论
加载评论中,请稍候...
折叠评论

想参与大家的讨论?现在就 登录 或者 注册

ID:{{user.uid}}
{{user.username}}
{{user.info.certsName}}
{{user.description}}
{{format("YYYY/MM/DD", user.toc)}}注册,{{fromNow(user.tlv)}}活动
{{submitted?"":"投诉"}}
请选择违规类型:
{{reason.description}}
支持的图片格式:jpg, jpeg, png