主权项 |
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>σ</mi><mo>=</mo><mi>arctan</mi><mfrac><mrow><mo>|</mo><mi>λ</mi><mo>-</mo><msub><mi>λ</mi><mn>1</mn></msub><mo>|</mo><mo>×</mo><msub><mi>R</mi><mi>M</mi></msub><mo>×</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>×</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>δ</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>γ</mi><mi>A</mi></msub><mi>cos</mi><msub><mi>ψ</mi><mi>A</mi></msub><mo>+</mo><mi>sin</mi><msub><mi>γ</mi><mi>A</mi></msub><mi>sin</mi><msub><mi>ψ</mi><mi>A</mi></msub><mi>sin</mi><msub><mi>θ</mi><mi>A</mi></msub></mtd><mtd><mi>sin</mi><msub><mi>ψ</mi><mi>A</mi></msub><mi>cos</mi><msub><mi>θ</mi><mi>A</mi></msub></mtd><mtd><mi>sin</mi><msub><mi>γ</mi><mi>A</mi></msub><mi>cos</mi><msub><mi>ψ</mi><mi>A</mi></msub><mo>-</mo><mi>cos</mi><msub><mi>γ</mi><mi>A</mi></msub><mi>sin</mi><msub><mi>ψ</mi><mi>A</mi></msub><mi>sin</mi><msub><mi>θ</mi><mi>A</mi></msub></mtd></mtr><mtr><mtd><mo>-</mo><mi>cos</mi><msub><mi>γ</mi><mi>A</mi></msub><mi>sin</mi><msub><mi>ψ</mi><mi>A</mi></msub><mo>+</mo><mi>sin</mi><msub><mi>γ</mi><mi>A</mi></msub><mi>cos</mi><msub><mi>ψ</mi><mi>A</mi></msub><mi>sin</mi><msub><mi>θ</mi><mi>A</mi></msub></mtd><mtd><mi>cos</mi><msub><mi>ψ</mi><mi>A</mi></msub><mi>cos</mi><msub><mi>θ</mi><mi>A</mi></msub></mtd><mtd><mo>-</mo><mi>sin</mi><msub><mi>γ</mi><mi>A</mi></msub><mi>sin</mi><msub><mi>ψ</mi><mi>A</mi></msub><mo>-</mo><mi>cos</mi><msub><mi>γ</mi><mi>A</mi></msub><mi>cos</mi><msub><mi>ψ</mi><mi>A</mi></msub><mi>sin</mi><msub><mi>θ</mi><mi>A</mi></msub></mtd></mtr><mtr><mtd><mo>-</mo><mi>sin</mi><msub><mi>γ</mi><mi>A</mi></msub><mi>cos</mi><msub><mi>θ</mi><mi>A</mi></msub></mtd><mtd><mi>sin</mi><msub><mi>θ</mi><mi>A</mi></msub></mtd><mtd><mi>cos</mi><msub><mi>γ</mi><mi>A</mi></msub><mi>cos</mi><msub><mi>θ</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>ψ</mi><mi>b</mi></msub></mtd><mtd><mo>-</mo><mi>sin</mi><msub><mi>ψ</mi><mi>b</mi></msub></mtd><mtd><mn>0</mn></mtd></mtr><mtr><mtd><mi>sin</mi><msub><mi>ψ</mi><mi>b</mi></msub><mi>cos</mi><msub><mi>θ</mi><mi>b</mi></msub></mtd><mtd><mi>cos</mi><msub><mi>ψ</mi><mi>b</mi></msub><mi>cos</mi><msub><mi>θ</mi><mi>b</mi></msub></mtd><mtd><mi>sin</mi><msub><mi>θ</mi><mi>b</mi></msub></mtd></mtr><mtr><mtd><mo>-</mo><mi>sin</mi><msub><mi>ψ</mi><mi>b</mi></msub><mi>sin</mi><msub><mi>θ</mi><mi>b</mi></msub></mtd><mtd><mo>-</mo><mi>cos</mi><msub><mi>ψ</mi><mi>b</mi></msub><mi>sin</mi><msub><mi>θ</mi><mi>b</mi></msub></mtd><mtd><mi>cos</mi><msub><mi>θ</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>γ</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>ψ</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°第十步,将计算获得的载机光轴稳定转台的方位角α、俯仰角β、横滚角χ传输给伺服控制单元;第十一步,重复执行第一步到第十步,直到上级系统发出地理跟踪结束指令后结束。 |