发明名称 |
高线加速度环境下挠性陀螺比力敏感误差系统级标定方法 |
摘要 |
本发明公开了一种高线加速度环境下挠性陀螺比力敏感误差系统级标定方法,该方法首先通过外部设备确定载体的初始位置参数并将其装订至导航计算机,然后挠性捷联惯导系统进行预热,采集挠性陀螺仪和石英加速度计的输出数据,再根据导航信息进行挠性捷联惯组的航向两位置初始对准,使挠性捷联惯组沿预先设定的轨迹运动并进行导航;最后利用差分GPS的速度、位置作为外部信息源,根据导航误差,应用卡尔曼滤波器对系统的状态变量进行估计和滤波修正,精确标定挠性陀螺比力敏感误差。该方法可以提高系统的参数辨识精度和挠性捷联惯组的实际导航性能;也可以用来辅助评估高线加速度环境下挠性陀螺比力敏感误差对导航系统实际导航性能的影响大小。<pb pnum="1" /> |
申请公布号 |
CN105659895B |
申请公布日期 |
2013.04.03 |
申请号 |
CN200910120761.X |
申请日期 |
2009.03.19 |
申请人 |
北京航空航天大学 |
发明人 |
张春熹;芦佳振;林恒;张秋莎 |
分类号 |
G06F17/10(2006.01)I;G01C25/00(2006.01)I |
主分类号 |
G06F17/10(2006.01)I |
代理机构 |
|
代理人 |
|
主权项 |
高线加速度环境下挠性陀螺比力敏感误差系统级标定方法,其特征在于,包括如下步骤:步骤一、通过外部设备确定载体的初始位置参数并将其装订至导航计算机;步骤二、挠性捷联惯导系统进行预热,然后采集挠性陀螺仪和石英加速度计的输出数据,预热时间大于5分钟;步骤三、根据外部信息源提供的导航信息进行挠性捷联惯导系统的航向两位置初始对准,对准时间为600s;所述的两位置是指载体在航向角相差180度的两个位置;进行初始对准时,首先由挠性捷联惯导系统采集载体的状态信息,然后外部信息源获取载体的航向信息,该两部分信息作为卡尔曼滤波器的输入,在卡尔曼滤波器内进行滤波处理,时间为600s;对准时使用的外部信息源是零速信息,其量测模型为:Z<sub>1</sub>(t)=H<sub>1</sub>X(t)+η<sub>1</sub>(t)=[0<sub>3×3</sub>|I<sub>3×3</sub>|0<sub>3×3</sub>]X(t)+η<sub>1</sub>(t)其中,η<sub>1</sub>(t)为零速量测噪声;步骤四、完成初始对准后,使挠性捷联惯导系统沿预先设定的轨迹进行加速和减速运动并进行导航;步骤五、利用差分GPS的速度、位置作为外部信息源,将挠性捷联惯导系统的输出与外部信息源的输出进行比对,得到挠性捷联惯导系统的导航误差,并根据导航误差,应用卡尔曼滤波器对挠性捷联惯导系统的状态变量进行估计并进行滤波修正,从而精确标定挠性陀螺比力敏感误差;步骤三和步骤五中进行挠性捷联惯导系统的航向两位置初始对准和根据观测到的导航误差精确标定挠性陀螺比力敏感误差要用到的系统状态方程如下:<maths id="cmaths0001" num="0001"><math><![CDATA[<mrow><mfrac><mi>d</mi><mrow><mi>d</mi><mi>t</mi></mrow></mfrac><mfenced open = "[" close = "]"><mtable><mtr><mtd><mrow><msub><mi>δθ</mi><mi>x</mi></msub></mrow></mtd></mtr><mtr><mtd><mrow><msub><mi>δθ</mi><mi>y</mi></msub></mrow></mtd></mtr><mtr><mtd><mrow><mi>δ</mi><mi>h</mi></mrow></mtd></mtr><mtr><mtd><mrow><msub><mi>δv</mi><mi>x</mi></msub></mrow></mtd></mtr><mtr><mtd><mrow><msub><mi>δv</mi><mi>y</mi></msub></mrow></mtd></mtr><mtr><mtd><mrow><msub><mi>δv</mi><mi>z</mi></msub></mrow></mtd></mtr><mtr><mtd><msub><mi>ψ</mi><mi>x</mi></msub></mtd></mtr><mtr><mtd><msub><mi>ψ</mi><mi>y</mi></msub></mtd></mtr><mtr><mtd><msub><mi>ψ</mi><mi>z</mi></msub></mtd></mtr></mtable></mfenced><mo>=</mo><mfenced open = "[" close = "]"><mtable><mtr><mtd><mrow><mo>-</mo><mfrac><msub><mi>v</mi><mi>z</mi></msub><mi>R</mi></mfrac></mrow></mtd><mtd><mn>0</mn></mtd><mtd><mfrac><msub><mi>v</mi><mi>y</mi></msub><msup><mi>R</mi><mn>2</mn></msup></mfrac></mtd><mtd><mn>0</mn></mtd><mtd><mrow><mo>-</mo><mfrac><mn>1</mn><mi>R</mi></mfrac></mrow></mtd><mtd><mn>0</mn></mtd><mtd><mn>0</mn></mtd><mtd><mn>0</mn></mtd><mtd><mn>0</mn></mtd></mtr><mtr><mtd><mn>0</mn></mtd><mtd><mrow><mo>-</mo><mfrac><msub><mi>v</mi><mi>z</mi></msub><mi>R</mi></mfrac></mrow></mtd><mtd><mrow><mo>-</mo><mfrac><msub><mi>v</mi><mi>x</mi></msub><msup><mi>R</mi><mn>2</mn></msup></mfrac></mrow></mtd><mtd><mfrac><mn>1</mn><mi>R</mi></mfrac></mtd><mtd><mn>0</mn></mtd><mtd><mn>0</mn></mtd><mtd><mn>0</mn></mtd><mtd><mn>0</mn></mtd><mtd><mn>0</mn></mtd></mtr><mtr><mtd><mrow><mo>-</mo><msub><mi>v</mi><mi>y</mi></msub></mrow></mtd><mtd><msub><mi>v</mi><mi>x</mi></msub></mtd><mtd><mn>0</mn></mtd><mtd><mn>0</mn></mtd><mtd><mn>0</mn></mtd><mtd><mn>1</mn></mtd><mtd><mn>0</mn></mtd><mtd><mn>0</mn></mtd><mtd><mn>0</mn></mtd></mtr><mtr><mtd><mn>0</mn></mtd><mtd><mrow><mo>-</mo><mi>g</mi></mrow></mtd><mtd><mn>0</mn></mtd><mtd><mn>0</mn></mtd><mtd><mrow><mn>2</mn><msub><mi>Ω</mi><mi>z</mi></msub></mrow></mtd><mtd><mrow><mo>-</mo><msub><mrow><mo>(</mo><mi>ρ</mi><mo>+</mo><mn>2</mn><mi>Ω</mi><mo>)</mo></mrow><mi>y</mi></msub></mrow></mtd><mtd><mn>0</mn></mtd><mtd><mrow><mo>-</mo><msubsup><mi>f</mi><mi>z</mi><mi>b</mi></msubsup></mrow></mtd><mtd><msubsup><mi>f</mi><mi>y</mi><mi>b</mi></msubsup></mtd></mtr><mtr><mtd><mi>g</mi></mtd><mtd><mn>0</mn></mtd><mtd><mn>0</mn></mtd><mtd><mrow><mo>-</mo><mn>2</mn><msub><mi>Ω</mi><mi>z</mi></msub></mrow></mtd><mtd><mn>0</mn></mtd><mtd><msub><mrow><mo>(</mo><mi>ρ</mi><mo>+</mo><mn>2</mn><mi>Ω</mi><mo>)</mo></mrow><mi>x</mi></msub></mtd><mtd><msubsup><mi>f</mi><mi>z</mi><mi>b</mi></msubsup></mtd><mtd><mn>0</mn></mtd><mtd><mrow><mo>-</mo><msubsup><mi>f</mi><mi>x</mi><mi>b</mi></msubsup></mrow></mtd></mtr><mtr><mtd><mn>0</mn></mtd><mtd><mn>0</mn></mtd><mtd><mrow><mn>2</mn><mfrac><mi>g</mi><mi>R</mi></mfrac></mrow></mtd><mtd><msub><mrow><mo>(</mo><mi>ω</mi><mo>+</mo><mi>Ω</mi><mo>)</mo></mrow><mi>y</mi></msub></mtd><mtd><mrow><mo>-</mo><msub><mrow><mo>(</mo><mi>ω</mi><mo>+</mo><mi>Ω</mi><mo>)</mo></mrow><mi>x</mi></msub></mrow></mtd><mtd><mn>0</mn></mtd><mtd><mrow><mo>-</mo><msubsup><mi>f</mi><mi>y</mi><mi>b</mi></msubsup></mrow></mtd><mtd><msubsup><mi>f</mi><mi>x</mi><mi>b</mi></msubsup></mtd><mtd><mn>0</mn></mtd></mtr><mtr><mtd><mn>0</mn></mtd><mtd><mn>0</mn></mtd><mtd><mn>0</mn></mtd><mtd><mn>0</mn></mtd><mtd><mn>0</mn></mtd><mtd><mn>0</mn></mtd><mtd><mn>0</mn></mtd><mtd><msub><mi>Ω</mi><mi>z</mi></msub></mtd><mtd><mrow><mo>-</mo><msub><mi>ω</mi><mi>y</mi></msub></mrow></mtd></mtr><mtr><mtd><mn>0</mn></mtd><mtd><mn>0</mn></mtd><mtd><mn>0</mn></mtd><mtd><mn>0</mn></mtd><mtd><mn>0</mn></mtd><mtd><mn>0</mn></mtd><mtd><mrow><mo>-</mo><msub><mi>Ω</mi><mi>z</mi></msub></mrow></mtd><mtd><mn>0</mn></mtd><mtd><msub><mi>ω</mi><mi>x</mi></msub></mtd></mtr><mtr><mtd><mn>0</mn></mtd><mtd><mn>0</mn></mtd><mtd><mn>0</mn></mtd><mtd><mn>0</mn></mtd><mtd><mn>0</mn></mtd><mtd><mn>0</mn></mtd><mtd><msub><mi>ω</mi><mi>y</mi></msub></mtd><mtd><mrow><mo>-</mo><msub><mi>ω</mi><mi>x</mi></msub></mrow></mtd><mtd><mn>0</mn></mtd></mtr></mtable></mfenced><mfenced open = "[" close = "]"><mtable><mtr><mtd><mrow><msub><mi>δθ</mi><mi>x</mi></msub></mrow></mtd></mtr><mtr><mtd><mrow><msub><mi>δθ</mi><mi>y</mi></msub></mrow></mtd></mtr><mtr><mtd><mrow><mi>δ</mi><mi>h</mi></mrow></mtd></mtr><mtr><mtd><mrow><msub><mi>δv</mi><mi>x</mi></msub></mrow></mtd></mtr><mtr><mtd><mrow><msub><mi>δv</mi><mi>y</mi></msub></mrow></mtd></mtr><mtr><mtd><mrow><msub><mi>δv</mi><mi>z</mi></msub></mrow></mtd></mtr><mtr><mtd><msub><mi>ψ</mi><mi>x</mi></msub></mtd></mtr><mtr><mtd><msub><mi>ψ</mi><mi>y</mi></msub></mtd></mtr><mtr><mtd><msub><mi>ψ</mi><mi>z</mi></msub></mtd></mtr></mtable></mfenced><mo>+</mo><mfenced open = "[" close = "]"><mtable><mtr><mtd><mn>0</mn></mtd></mtr><mtr><mtd><mn>0</mn></mtd></mtr><mtr><mtd><mn>0</mn></mtd></mtr><mtr><mtd><mrow><msubsup><mi>δf</mi><mi>x</mi><mi>b</mi></msubsup></mrow></mtd></mtr><mtr><mtd><mrow><msubsup><mi>δf</mi><mi>y</mi><mi>b</mi></msubsup></mrow></mtd></mtr><mtr><mtd><mrow><msubsup><mi>δf</mi><mi>z</mi><mi>b</mi></msubsup></mrow></mtd></mtr><mtr><mtd><mn>0</mn></mtd></mtr><mtr><mtd><mn>0</mn></mtd></mtr><mtr><mtd><mn>0</mn></mtd></mtr></mtable></mfenced><mo>+</mo><mfenced open = "[" close = "]"><mtable><mtr><mtd><mn>0</mn></mtd></mtr><mtr><mtd><mn>0</mn></mtd></mtr><mtr><mtd><mn>0</mn></mtd></mtr><mtr><mtd><mn>0</mn></mtd></mtr><mtr><mtd><mn>0</mn></mtd></mtr><mtr><mtd><mn>0</mn></mtd></mtr><mtr><mtd><mrow><mi>s</mi><mi>i</mi><mi>g</mi><mi>n</mi><mo>·</mo><msub><mi>ϵ</mi><mi>x</mi></msub></mrow></mtd></mtr><mtr><mtd><mrow><mi>s</mi><mi>i</mi><mi>g</mi><mi>n</mi><mo>·</mo><msub><mi>ϵ</mi><mi>y</mi></msub></mrow></mtd></mtr><mtr><mtd><mrow><mi>s</mi><mi>i</mi><mi>g</mi><mi>n</mi><mo>·</mo><msub><mi>ϵ</mi><mi>z</mi></msub></mrow></mtd></mtr></mtable></mfenced></mrow>]]></math><img file="FBM2016033002560000031.GIF" wi="1966" he="720" /></maths>δθ——角位置误差矢量;δh——高度误差;δv——速度误差矢量;<math><![CDATA[<mrow><msup><mfenced open = "[" close = "]"><mtable><mtr><mtd><msub><mi>ϵ</mi><mi>x</mi></msub></mtd></mtr><mtr><mtd><msub><mi>ϵ</mi><mi>y</mi></msub></mtd></mtr><mtr><mtd><msub><mi>ϵ</mi><mi>z</mi></msub></mtd></mtr></mtable></mfenced><mi>n</mi></msup><mo>=</mo><msubsup><mi>C</mi><mi>b</mi><mi>n</mi></msubsup><msup><mfenced open = "[" close = "]"><mtable><mtr><mtd><msub><mi>ϵ</mi><mi>x</mi></msub></mtd></mtr><mtr><mtd><msub><mi>ϵ</mi><mi>y</mi></msub></mtd></mtr><mtr><mtd><msub><mi>ϵ</mi><mi>z</mi></msub></mtd></mtr></mtable></mfenced><mi>b</mi></msup><mo>=</mo><mfenced open = "[" close = "]"><mtable><mtr><mtd><msub><mi>C</mi><mn>11</mn></msub></mtd><mtd><msub><mi>C</mi><mn>12</mn></msub></mtd><mtd><msub><mi>C</mi><mn>13</mn></msub></mtd></mtr><mtr><mtd><msub><mi>C</mi><mn>21</mn></msub></mtd><mtd><msub><mi>C</mi><mn>22</mn></msub></mtd><mtd><msub><mi>C</mi><mn>23</mn></msub></mtd></mtr><mtr><mtd><msub><mi>C</mi><mn>31</mn></msub></mtd><mtd><msub><mi>C</mi><mn>32</mn></msub></mtd><mtd><msub><mi>C</mi><mn>33</mn></msub></mtd></mtr></mtable></mfenced><mfenced open = "[" close = "]"><mtable><mtr><mtd><mo>(</mo><msub><mi>B</mi><mi>X</mi></msub><mo>+</mo><mi>D</mi><mi>X</mi><mi>X</mi><mo>·</mo><msubsup><mi>f</mi><mi>x</mi><mi>b</mi></msubsup><mo>+</mo><mi>D</mi><mi>X</mi><mi>Y</mi><mo>·</mo><msubsup><mi>f</mi><mi>y</mi><mi>b</mi></msubsup><mo>)</mo></mtd></mtr><mtr><mtd><mo>(</mo><msub><mi>B</mi><mi>Y</mi></msub><mo>-</mo><mi>D</mi><mi>X</mi><mi>Y</mi><mo>·</mo><msubsup><mi>f</mi><mi>x</mi><mi>b</mi></msubsup><mo>+</mo><mi>D</mi><mi>X</mi><mi>X</mi><mo>·</mo><msubsup><mi>f</mi><mi>y</mi><mi>b</mi></msubsup><mo>)</mo></mtd></mtr><mtr><mtd><mo>(</mo><msub><mi>B</mi><mi>Z</mi></msub><mo>+</mo><mi>D</mi><mi>Z</mi><mi>X</mi><mo>·</mo><msubsup><mi>f</mi><mi>x</mi><mi>b</mi></msubsup><mo>+</mo><mi>D</mi><mi>Z</mi><mi>Z</mi><mo>·</mo><msubsup><mi>f</mi><mi>z</mi><mi>b</mi></msubsup><mo>)</mo></mtd></mtr></mtable></mfenced></mrow>]]></math><img file="FBM2016033002560000041.GIF" wi="1476" he="259" /></maths>其中,B<sub>X</sub>、B<sub>Y</sub>、B<sub>Z</sub>代表挠性陀螺的常值漂移误差项;DXX、DXY、DZX、DZZ代表挠性陀螺的比力敏感误差项;在步骤五中进行滤波修正时,使用的外部信息源是差分GPS位置、速度信息,其量测模型为:<maths id="cmaths0003" num="0003"><math><![CDATA[<mrow><msub><mi>Z</mi><mn>2</mn></msub><mrow><mo>(</mo><mi>t</mi><mo>)</mo></mrow><mo>=</mo><msub><mi>H</mi><mn>2</mn></msub><mi>X</mi><mrow><mo>(</mo><mi>t</mi><mo>)</mo></mrow><mo>+</mo><msub><mi>η</mi><mn>2</mn></msub><mrow><mo>(</mo><mi>t</mi><mo>)</mo></mrow><mo>=</mo><mo>[</mo><mrow><mtable><mtr><mtd><mrow><mo>-</mo><mi>c</mi><mi>o</mi><mi>s</mi><mi>α</mi></mrow></mtd><mtd><mrow><mi>sin</mi><mi>α</mi></mrow></mtd></mtr><mtr><mtd><mrow><mi>s</mi><mi>i</mi><mi>n</mi><mi>α</mi></mrow></mtd><mtd><mrow><mi>cos</mi><mi>α</mi></mrow></mtd></mtr></mtable><mo>|</mo><msub><mn>0</mn><mrow><mn>2</mn><mo>×</mo><mn>7</mn></mrow></msub></mrow><mo>]</mo><mi>X</mi><mrow><mo>(</mo><mi>t</mi><mo>)</mo></mrow><mo>+</mo><msub><mi>η</mi><mn>2</mn></msub><mrow><mo>(</mo><mi>t</mi><mo>)</mo></mrow></mrow>]]></math><img file="FBM2016033002560000042.GIF" wi="1449" he="156" /></maths>Z<sub>3</sub>(t)=H<sub>3</sub>X(t)+η<sub>3</sub>(t)=[0<sub>2×2</sub>|I<sub>2×2</sub>|0<sub>2×2</sub>]X(t)+η<sub>3</sub>(t)其中,η<sub>2</sub>(t)、η<sub>3</sub>(t)分别为差分GPS位置噪声、速度量测噪声;步骤三、步骤五中,对系统的状态变量进行估计,需要对挠性捷联惯导系统的系统方程进行离散化,离散化采用泰勒级数展开:<maths id="cmaths0004" num="0004"><math><![CDATA[<mrow><mi>Φ</mi><mrow><mo>(</mo><mi>k</mi><mo>+</mo><mn>1</mn><mo>,</mo><mi>k</mi><mo>)</mo></mrow><mo>=</mo><mi>I</mi><mo>+</mo><msub><mi>TF</mi><mi>k</mi></msub><mo>+</mo><mfrac><msup><mi>T</mi><mn>2</mn></msup><mrow><mn>2</mn><mo>!</mo></mrow></mfrac><msup><msub><mi>F</mi><mi>k</mi></msub><mn>2</mn></msup><mo>+</mo><mfrac><msup><mi>T</mi><mn>3</mn></msup><mrow><mn>3</mn><mo>!</mo></mrow></mfrac><msup><msub><mi>F</mi><mi>k</mi></msub><mn>2</mn></msup><mo>+</mo><mo>...</mo></mrow>]]></math><img file="FBM2016033002560000043.GIF" wi="952" he="135" /></maths>其中T为滤波周期,F<sub>k</sub>为状态转移矩阵;挠性捷联惯导系统的系统噪声方差阵为<maths id="cmaths0005" num="0005"><math><![CDATA[<mrow><mi>Q</mi><mrow><mo>(</mo><mi>k</mi><mo>)</mo></mrow><mo>=</mo><mover><mi>Q</mi><mo>‾</mo></mover><mi>T</mi><mo>+</mo><mo>[</mo><msub><mi>F</mi><mi>k</mi></msub><mover><mi>Q</mi><mo>‾</mo></mover><mo>+</mo><mover><mi>Q</mi><mo>‾</mo></mover><msup><msub><mi>F</mi><mi>k</mi></msub><mi>T</mi></msup><mo>]</mo><mfrac><msup><mi>T</mi><mn>2</mn></msup><mrow><mn>2</mn><mo>!</mo></mrow></mfrac><mo>+</mo><mo>{</mo><msub><mi>F</mi><mi>k</mi></msub><mo>[</mo><msub><mi>F</mi><mi>k</mi></msub><mover><mi>Q</mi><mo>‾</mo></mover><mo>+</mo><msup><mrow><mo>(</mo><msub><mi>F</mi><mi>k</mi></msub><mover><mi>Q</mi><mo>‾</mo></mover><mo>)</mo></mrow><mi>T</mi></msup><mo>,</mo><mo>]</mo><mo>+</mo><msup><mrow><mo>[</mo><msub><mi>F</mi><mi>k</mi></msub><mrow><mo>(</mo><msub><mi>F</mi><mi>k</mi></msub><mover><mi>Q</mi><mo>‾</mo></mover><mo>+</mo><mover><mi>Q</mi><mo>‾</mo></mover><msup><msub><mi>F</mi><mi>k</mi></msub><mi>T</mi></msup><mo>)</mo></mrow><mo>]</mo></mrow><mi>T</mi></msup><mo>}</mo><mfrac><msup><mi>T</mi><mn>3</mn></msup><mrow><mn>3</mn><mo>!</mo></mrow></mfrac><mo>+</mo><mo>...</mo></mrow>]]></math><img file="FBM2016033002560000044.GIF" wi="1701" he="146" /></maths>其中<img file="FBM2016033002560000045.GIF" wi="263" he="87" />G<sub>k</sub>为噪声转移矩阵,q为噪声驱动源的方差强度阵。pb pnum="2" /> |
地址 |
100191 北京市海淀区学院路37号 |