发明名称 基于光轴与惯性轴重合的地理跟踪方法
摘要 本发明公开了一种基于光轴与惯性轴重合的地理跟踪方法。本方法根据目标定位数据与自身位置信息采用空间两点几何算法,实时计算载体瞄准线的空间姿态,再结合载体空间姿态和空间坐标变换原理计算瞄准线相对于载体坐标系的空间角度,将该角度送给伺服控制单元,由伺服控制单元调整转台的相应空间姿态,从而完成对目标的地理跟踪。本方法从原理上区别于图像跟踪方法,因此不受云雾和障碍物的影响,若在飞行器上使用可避免因剧烈震动或视线遮挡而丢失目标的问题,显著提高了战场武器系统的侦察与搜跟能力。本发明也可用于各种制导导弹的导引装置,以提高对地目标的精确打击能力。
申请公布号 CN102902282A 申请公布日期 2013.01.30
申请号 CN201210361231.6 申请日期 2012.09.25
申请人 中国兵器工业第二0五研究所 发明人 陈颖;纪明;陈文建;张建峰;马忠孝;唐超;李颖娟;易科;扈宇姝;许开銮;韩峰;张夏疆
分类号 G05D3/12(2006.01)I;G01C1/00(2006.01)I 主分类号 G05D3/12(2006.01)I
代理机构 代理人
主权项 1.一种基于光轴与惯性轴重合的地理跟踪方法,其特征在于:该方法包括以下操作步骤:第一步,采集目标定位装置输出的目标点数据(λ,L,h)和当前载机位置点的数据(λ<sub>1</sub>,L<sub>1</sub>,h<sub>1</sub>,ψ<sub>A</sub>,θ<sub>A</sub>,γ<sub>A</sub>,S),其中λ,L,h分别是目标点的经度、纬度和高度;λ<sub>1</sub>,L<sub>1</sub>,h<sub>1</sub>分别是载机的当前经度、纬度和高度,ψ<sub>A</sub>,θ<sub>A</sub>,γ<sub>A</sub>分别是载机在地里坐标系下的当前航向角、俯仰角和横滚角,S是当前载机到目标点的距离;第二步,用以下公式计算载机与目标点在水平面投影中由载机投影到目标点投影矢量线段的北向夹角σ:<maths num="0001"><![CDATA[<math><mrow><mi>&sigma;</mi><mo>=</mo><mi>arctan</mi><mfrac><mrow><mo>|</mo><mi>&lambda;</mi><mo>-</mo><msub><mi>&lambda;</mi><mn>1</mn></msub><mo>|</mo><mo>&times;</mo><msub><mi>R</mi><mi>M</mi></msub><mo>&times;</mo><mi>cos</mi><msub><mi>L</mi><mn>1</mn></msub></mrow><mrow><mo>|</mo><mi>L</mi><mo>-</mo><msub><mi>L</mi><mn>1</mn></msub><mo>|</mo><mo>&times;</mo><msub><mi>R</mi><mi>N</mi></msub></mrow></mfrac></mrow></math>]]></maths>R<sub>M</sub>=R<sub>e</sub>(1-2e+3esin<sup>2</sup>L<sub>1</sub>)R<sub>N</sub>=R<sub>e</sub>(1+esin<sup>2</sup>L<sub>1</sub>)其中,R<sub>M</sub>是载机当前位置的子午圈曲率半径,R<sub>N</sub>是载机当前位置的卯酉圈曲率半径,e是扁率,R<sub>e</sub>是地球椭球体长轴半径;第三步,用以下公式计算瞄准线指向目标点的航向角ψ<sub>b</sub>:当λ>λ<sub>1</sub>且L>L<sub>1</sub>  ψ<sub>b</sub>=σ当λ>λ<sub>1</sub>且L<L<sub>1</sub>  ψ<sub>b</sub>=180°-σ当λ<λ<sub>1</sub>且L<L<sub>1</sub>  ψ<sub>b</sub>=180°+σ当λ<λ<sub>1</sub>且L>L<sub>1</sub>  ψ<sub>b</sub>=-σ当λ=λ<sub>1</sub>且L>L<sub>1</sub>  ψ<sub>b</sub>=0°当λ=λ<sub>1</sub>且L<L<sub>1</sub>  ψ<sub>b</sub>=180°当λ<λ<sub>1</sub>且L=L<sub>1</sub>  ψ<sub>b</sub>=-90°当λ>λ<sub>1</sub>且L=L<sub>1</sub>  ψ<sub>b</sub>=90°第四步,用以下公式计算瞄准线与水平面夹角δ:<maths num="0002"><![CDATA[<math><mrow><mi>&delta;</mi><mo>=</mo><mi>arcsin</mi><mfrac><mrow><mo>|</mo><mi>h</mi><mo>-</mo><msub><mi>h</mi><mn>1</mn></msub><mo>|</mo></mrow><mi>S</mi></mfrac></mrow></math>]]></maths>第五步,用以下公式计算瞄准线指向目标点的俯仰角θ<sub>b</sub>:当h>h<sub>1</sub>  θ<sub>b</sub>=δ当h<h<sub>1</sub>  θ<sub>b</sub>=-δ第六步,计算地理坐标系到载机坐标系的姿态矩阵<img file="FSA00000782611100022.GIF" wi="82" he="51" /><maths num="0003"><![CDATA[<math><mrow><msubsup><mi>C</mi><mi>A</mi><mi>n</mi></msubsup><mo>=</mo><mfenced open='[' close=']'><mtable><mtr><mtd><mi>cos</mi><msub><mi>&gamma;</mi><mi>A</mi></msub><mi>cos</mi><msub><mi>&psi;</mi><mi>A</mi></msub><mo>+</mo><mi>sin</mi><msub><mi>&gamma;</mi><mi>A</mi></msub><mi>sin</mi><msub><mi>&psi;</mi><mi>A</mi></msub><mi>sin</mi><msub><mi>&theta;</mi><mi>A</mi></msub></mtd><mtd><mi>sin</mi><msub><mi>&psi;</mi><mi>A</mi></msub><mi>cos</mi><msub><mi>&theta;</mi><mi>A</mi></msub></mtd><mtd><mi>sin</mi><msub><mi>&gamma;</mi><mi>A</mi></msub><mi>cos</mi><msub><mi>&psi;</mi><mi>A</mi></msub><mo>-</mo><mi>cos</mi><msub><mi>&gamma;</mi><mi>A</mi></msub><mi>sin</mi><msub><mi>&psi;</mi><mi>A</mi></msub><mi>sin</mi><msub><mi>&theta;</mi><mi>A</mi></msub></mtd></mtr><mtr><mtd><mo>-</mo><mi>cos</mi><msub><mi>&gamma;</mi><mi>A</mi></msub><mi>sin</mi><msub><mi>&psi;</mi><mi>A</mi></msub><mo>+</mo><mi>sin</mi><msub><mi>&gamma;</mi><mi>A</mi></msub><mi>cos</mi><msub><mi>&psi;</mi><mi>A</mi></msub><mi>sin</mi><msub><mi>&theta;</mi><mi>A</mi></msub></mtd><mtd><mi>cos</mi><msub><mi>&psi;</mi><mi>A</mi></msub><mi>cos</mi><msub><mi>&theta;</mi><mi>A</mi></msub></mtd><mtd><mo>-</mo><mi>sin</mi><msub><mi>&gamma;</mi><mi>A</mi></msub><mi>sin</mi><msub><mi>&psi;</mi><mi>A</mi></msub><mo>-</mo><mi>cos</mi><msub><mi>&gamma;</mi><mi>A</mi></msub><mi>cos</mi><msub><mi>&psi;</mi><mi>A</mi></msub><mi>sin</mi><msub><mi>&theta;</mi><mi>A</mi></msub></mtd></mtr><mtr><mtd><mo>-</mo><mi>sin</mi><msub><mi>&gamma;</mi><mi>A</mi></msub><mi>cos</mi><msub><mi>&theta;</mi><mi>A</mi></msub></mtd><mtd><mi>sin</mi><msub><mi>&theta;</mi><mi>A</mi></msub></mtd><mtd><mi>cos</mi><msub><mi>&gamma;</mi><mi>A</mi></msub><mi>cos</mi><msub><mi>&theta;</mi><mi>A</mi></msub></mtd></mtr></mtable></mfenced></mrow></math>]]></maths>第七步,计算瞄准线到地理坐标系的姿态矩阵<img file="FSA00000782611100024.GIF" wi="78" he="59" /><maths num="0004"><![CDATA[<math><mrow><msubsup><mi>C</mi><mi>n</mi><mi>b</mi></msubsup><mo>=</mo><mfenced open='[' close=']'><mtable><mtr><mtd><mi>cos</mi><msub><mi>&psi;</mi><mi>b</mi></msub></mtd><mtd><mo>-</mo><mi>sin</mi><msub><mi>&psi;</mi><mi>b</mi></msub></mtd><mtd><mn>0</mn></mtd></mtr><mtr><mtd><mi>sin</mi><msub><mi>&psi;</mi><mi>b</mi></msub><mi>cos</mi><msub><mi>&theta;</mi><mi>b</mi></msub></mtd><mtd><mi>cos</mi><msub><mi>&psi;</mi><mi>b</mi></msub><mi>cos</mi><msub><mi>&theta;</mi><mi>b</mi></msub></mtd><mtd><mi>sin</mi><msub><mi>&theta;</mi><mi>b</mi></msub></mtd></mtr><mtr><mtd><mo>-</mo><mi>sin</mi><msub><mi>&psi;</mi><mi>b</mi></msub><mi>sin</mi><msub><mi>&theta;</mi><mi>b</mi></msub></mtd><mtd><mo>-</mo><mi>cos</mi><msub><mi>&psi;</mi><mi>b</mi></msub><mi>sin</mi><msub><mi>&theta;</mi><mi>b</mi></msub></mtd><mtd><mi>cos</mi><msub><mi>&theta;</mi><mi>b</mi></msub></mtd></mtr></mtable></mfenced></mrow></math>]]></maths>第八步,计算瞄准线到载机坐标系的姿态矩阵<img file="FSA00000782611100026.GIF" wi="83" he="57" /><maths num="0005"><![CDATA[<math><mrow><msubsup><mi>C</mi><mi>A</mi><mi>b</mi></msubsup><mo>=</mo><msubsup><mi>C</mi><mi>n</mi><mi>b</mi></msubsup><mo>*</mo><msubsup><mi>C</mi><mi>A</mi><mi>n</mi></msubsup><mo>=</mo><mfenced open='[' close=']'><mtable><mtr><mtd><msub><mi>C</mi><mn>1</mn></msub><mrow><mo>(</mo><mn>1,1</mn><mo>)</mo></mrow></mtd><mtd><msub><mi>C</mi><mn>1</mn></msub><mrow><mo>(</mo><mn>2,1</mn><mo>)</mo></mrow></mtd><mtd><msub><mi>C</mi><mn>1</mn></msub><mrow><mo>(</mo><mn>3,1</mn><mo>)</mo></mrow></mtd></mtr><mtr><mtd><msub><mi>C</mi><mn>1</mn></msub><mrow><mo>(</mo><mn>1,2</mn><mo>)</mo></mrow></mtd><mtd><msub><mi>C</mi><mn>1</mn></msub><mrow><mo>(</mo><mn>2,2</mn><mo>)</mo></mrow></mtd><mtd><msub><mi>C</mi><mn>1</mn></msub><mrow><mo>(</mo><mn>3,2</mn><mo>)</mo></mrow></mtd></mtr><mtr><mtd><msub><mi>C</mi><mn>1</mn></msub><mrow><mo>(</mo><mn>1,3</mn><mo>)</mo></mrow></mtd><mtd><msub><mi>C</mi><mn>1</mn></msub><mrow><mo>(</mo><mn>2,3</mn><mo>)</mo></mrow></mtd><mtd><msub><mi>C</mi><mn>1</mn></msub><mrow><mo>(</mo><mn>3,3</mn><mo>)</mo></mrow></mtd></mtr></mtable></mfenced></mrow></math>]]></maths>式中,C<sub>1</sub>(i,j)且(i=1,2,3  j=1,2,3)是矩阵<img file="FSA00000782611100028.GIF" wi="52" he="58" />的各项元素;第九步,计算载机光轴稳定转台的俯仰角β、方位角α和横滚角χ:9.1用以下公式计算载机光轴稳定转台的俯仰角β、参考横滚角γ<sub>C</sub>和参考方位角ψ<sub>C</sub>:β=arcsin C<sub>1</sub>(3,2)<maths num="0006"><![CDATA[<math><mrow><msub><mi>&gamma;</mi><mi>C</mi></msub><mo>=</mo><mi>arctan</mi><mrow><mo>(</mo><mo>-</mo><mfrac><mrow><msub><mi>C</mi><mn>1</mn></msub><mrow><mo>(</mo><mn>3,1</mn><mo>)</mo></mrow></mrow><mrow><msub><mi>C</mi><mn>1</mn></msub><mrow><mo>(</mo><mn>3,3</mn><mo>)</mo></mrow></mrow></mfrac><mo>)</mo></mrow></mrow></math>]]></maths><maths num="0007"><![CDATA[<math><mrow><msub><mi>&psi;</mi><mi>C</mi></msub><mo>=</mo><mi>arctan</mi><mrow><mo>(</mo><mfrac><mrow><msub><mi>C</mi><mn>1</mn></msub><mrow><mo>(</mo><mn>1,2</mn><mo>)</mo></mrow></mrow><mrow><msub><mi>C</mi><mn>1</mn></msub><mrow><mo>(</mo><mn>2,2</mn><mo>)</mo></mrow></mrow></mfrac><mo>)</mo></mrow></mrow></math>]]></maths>9.2用以下判别公式计算载机光轴稳定转台方位角α:当C<sub>1</sub>(2,2)→0且C<sub>1</sub>(1,2)>0  α=90°当C<sub>1</sub>(2,2)→0且C<sub>1</sub>(1,2)<0    α=-90°当C<sub>1</sub>(2,2)>0                 α=ψ<sub>C</sub>当C<sub>1</sub>(2,2)<0且C<sub>1</sub>(1,2)>0    α=ψ<sub>C</sub>+180°当C<sub>1</sub>(2,2)<0且C<sub>1</sub>(1,2)<0    α=ψ<sub>C</sub>-180°9.3用以下判别公式计算载机光轴稳定转台横滚角χ:当C<sub>1</sub>(3,3)>0            χ=γ<sub>C</sub>当C<sub>1</sub>(3,3)<0且γ<sub>C</sub>>0    χ=γ<sub>C</sub>-180°当C<sub>1</sub>(3,3)<0且γ<sub>C</sub><0    χ=γ<sub>C</sub>+180°第十步,将计算获得的载机光轴稳定转台的方位角α、俯仰角β、横滚角χ传输给伺服控制单元;第十一步,重复执行第一步到第十步,直到上级系统发出地理跟踪结束指令后结束。
地址 710065 陕西省西安市雁塔区电子三路西段九号