主权项 |
1.一种运动体BTT转弯控制姿态信息的获取方法,其特征在于:运动体航行在水下环境,航行速度大于100m/s,运动体BTT转弯控制姿态信息的获取方法的步骤如下:第一步,运动体运动姿态信息的提取;所述的运动体姿态信息包括运动体位移、速度、角度姿态、角速度信息;第二步,获取运动体BTT转弯控制姿态信息;对运动体的BTT转弯控制姿态信息进行解耦,分类,将BTT转弯控制姿态信息分解为滚动通道姿态信息及俯仰-偏航通道姿态信息;分别对滚动通道姿态信息及俯仰-偏航通道姿态信息中的水动力系数进行估计,得到完整的运动体BTT转弯控制姿态信息;所述的水动力系数包括位置导数、旋转导数、鳍舵导数;所述的运动体BTT转弯控制姿态信息包括:攻角、侧滑角、纵向过载、横向过载、角速度、角加速度信息;所述的运动体运动姿态信息的提取,具体为:根据牛顿第二定律及流体动力学,从运动体绕重心移动的动力学方程及运动体绕重心转动的动力学方程中,提取运动体运动的姿态信息,姿态信息提取的原始模型如下:<maths num="0001"><![CDATA[<math><mrow><mfenced open='{' close=''><mtable><mtr><mtd><mi>m</mi><mrow><mo>(</mo><mover><mi>u</mi><mo>·</mo></mover><mo>-</mo><mi>vr</mi><mo>+</mo><mi>wq</mi><mo>)</mo></mrow><mo>=</mo><msub><mi>F</mi><mi>x</mi></msub></mtd></mtr><mtr><mtd><mi>m</mi><mrow><mo>(</mo><mover><mrow><mi>v</mi><mo>-</mo></mrow><mo>·</mo></mover><mi>wq</mi><mo>+</mo><mi>ur</mi><mo>)</mo></mrow><mo>=</mo><msub><mi>F</mi><mi>y</mi></msub></mtd></mtr><mtr><mtd><mi>m</mi><mrow><mo>(</mo><mover><mi>w</mi><mo>·</mo></mover><mo>-</mo><mi>uq</mi><mo>+</mo><mi>vp</mi><mo>)</mo></mrow><mo>=</mo><msub><mi>F</mi><mi>z</mi></msub></mtd></mtr><mtr><mtd><msub><mi>I</mi><mi>x</mi></msub><mover><mi>q</mi><mo>·</mo></mover><mo>+</mo><mrow><mo>(</mo><msub><mi>I</mi><mi>z</mi></msub><mo>-</mo><msub><mi>I</mi><mi>y</mi></msub><mo>)</mo></mrow><mi>qr</mi><mo>=</mo><msub><mi>M</mi><mi>x</mi></msub></mtd></mtr><mtr><mtd><msub><mi>I</mi><mi>y</mi></msub><mover><mi>q</mi><mo>·</mo></mover><mo>+</mo><mrow><mo>(</mo><msub><mi>I</mi><mi>x</mi></msub><mo>-</mo><msub><mi>I</mi><mi>z</mi></msub><mo>)</mo></mrow><mi>rp</mi><mo>=</mo><msub><mi>M</mi><mi>y</mi></msub></mtd></mtr><mtr><mtd><msub><mi>I</mi><mi>z</mi></msub><mover><mi>r</mi><mo>·</mo></mover><mo>+</mo><mrow><mo>(</mo><msub><mi>I</mi><mi>y</mi></msub><mo>-</mo><msub><mi>I</mi><mi>x</mi></msub><mo>)</mo></mrow><mi>pq</mi><mo>=</mo><msub><mi>M</mi><mi>z</mi></msub></mtd></mtr></mtable></mfenced><mo>-</mo><mo>-</mo><mo>-</mo><mrow><mo>(</mo><mn>1</mn><mo>)</mo></mrow></mrow></math>]]></maths>式(1)中m为运动体的质量;u、v、w分别为运动体坐标系原点的速度矢量在运动体动坐标系各轴上的分量;<img file="FDA0000457840460000012.GIF" wi="73" he="57" /><img file="FDA0000457840460000013.GIF" wi="68" he="58" /><img file="FDA0000457840460000014.GIF" wi="55" he="58" />分别为坐标原点的加速度在运动体坐标系各轴上的分量;p、q、r分别为运动体绕自身纵轴、横轴、立轴的转动角速度;<img file="FDA0000457840460000015.GIF" wi="80" he="67" /><img file="FDA0000457840460000016.GIF" wi="73" he="67" /><img file="FDA0000457840460000017.GIF" wi="46" he="62" />分别为运动体绕自身纵轴、横轴、立轴的转动角加速度;I<sub>x</sub>、I<sub>y</sub>、I<sub>z</sub>分别为运动体关于自身纵轴、横轴、立轴的转动惯量;F<sub>x</sub>、F<sub>y</sub>、F<sub>z</sub>分别为运动体受到的合力在运动体坐标系各轴下的分量;M<sub>x</sub>、M<sub>y</sub>、M<sub>z</sub>分别为运动体受到的合力矩在运动体坐标系各轴下的分量,所述的纵轴、横轴、立轴分别对应运动体坐标系的y轴、x轴和z轴;所述的运动体BTT转弯控制姿态信息获取过程如下:根据第一步中已提取的运动体姿态信息,运用运动体航行过程中的运动学方程,获得运动体攻角及侧滑角航行姿态位置信息;另外,结合运动体法向过载方程,得到运动体BTT转弯控制姿态信息;令u=v<sub>x</sub>,v=v<sub>y</sub>,w=v<sub>z</sub>,p=w<sub>x</sub>,q=w<sub>y</sub>,r=w<sub>z</sub>,并且,由于攻角α和侧滑角β较小,有<maths num="0002"><![CDATA[<math><mrow><mi>α</mi><mo>=</mo><mi>arctan</mi><mfrac><msub><mi>v</mi><mi>z</mi></msub><msub><mi>v</mi><mi>x</mi></msub></mfrac><mo>≈</mo><mo>-</mo><mfrac><msub><mi>v</mi><mi>z</mi></msub><msub><mi>v</mi><mi>z</mi></msub></mfrac><mo>,</mo></mrow></math>]]></maths><maths num="0003"><![CDATA[<math><mrow><mi>β</mi><mo>=</mo><mo>-</mo><mi>arctan</mi><mfrac><msub><mi>v</mi><mi>y</mi></msub><msub><mi>v</mi><mi>x</mi></msub></mfrac><mo>≈</mo><mo>-</mo><mfrac><msub><mi>v</mi><mi>y</mi></msub><msub><mi>v</mi><mi>x</mi></msub></mfrac><mo>,</mo></mrow></math>]]></maths>则运动体航行过程中的运动学方程为:<img file="FDA0000457840460000023.GIF" wi="1558" he="244" />式(2)中φ为横滚角,θ为俯仰角,<img file="FDA0000457840460000024.GIF" wi="46" he="55" />为偏航角;所述的法向过载方程为:<maths num="0004"><![CDATA[<math><mrow><msub><mi>n</mi><mi>y</mi></msub><mo>=</mo><mfrac><msub><mi>F</mi><mi>y</mi></msub><mi>mg</mi></mfrac><mo>,</mo><msub><mi>n</mi><mi>z</mi></msub><mo>=</mo><mfrac><msub><mi>F</mi><mi>z</mi></msub><mi>mg</mi></mfrac><mo>-</mo><mo>-</mo><mo>-</mo><mrow><mo>(</mo><mn>3</mn><mo>)</mo></mrow></mrow></math>]]></maths>其中,g为当地重力加速度,m为运动体质量,n<sub>y</sub>、n<sub>z</sub>分别为纵向过载和横向过载,F<sub>y</sub>,F<sub>z</sub>分别为运动体受到的合力在运动体坐标系y轴和z轴下的分量;则运动体BTT转弯控制姿态信息为:<maths num="0005"><![CDATA[<math><mrow><mfenced open='' close=''><mtable><mtr><mtd><mover><mi>α</mi><mo>·</mo></mover><mo>=</mo><mfrac><msub><mi>F</mi><mi>z</mi></msub><mrow><msub><mi>v</mi><mi>x</mi></msub><mi>m</mi></mrow></mfrac><mo>+</mo><mfrac><mrow><mi>g</mi><mi>cos</mi><mi></mi><mi>θ</mi><mi>cos</mi><mi>φ</mi></mrow><msub><mi>v</mi><mi>x</mi></msub></mfrac><mo>+</mo><msub><mi>w</mi><mi>y</mi></msub><mo>+</mo><msub><mi>βw</mi><mi>x</mi></msub></mtd></mtr><mtr><mtd><mover><mi>β</mi><mo>·</mo></mover><mo>=</mo><msub><mi>w</mi><mi>z</mi></msub><mo>-</mo><msub><mi>αw</mi><mi>x</mi></msub><mo>-</mo><mfrac><mi>g</mi><msub><mi>v</mi><mi>x</mi></msub></mfrac><mi>cos</mi><mi></mi><mi>θ</mi><mi>sin</mi><mi>φ</mi><mo>-</mo><mfrac><msub><mi>F</mi><mi>y</mi></msub><msub><mi>mv</mi><mi>x</mi></msub></mfrac></mtd></mtr><mtr><mtd><msub><mover><mi>w</mi><mo>·</mo></mover><mi>z</mi></msub><mo>=</mo><mfrac><msub><mi>M</mi><mi>z</mi></msub><msub><mi>I</mi><mi>z</mi></msub></mfrac><mo>+</mo><mfrac><mrow><mo>(</mo><msub><mover><mi>I</mi><mi>x</mi></mover><mi>x</mi></msub><mo>-</mo><msub><mi>I</mi><mi>y</mi></msub><mo>)</mo></mrow><msub><mi>I</mi><mi>z</mi></msub></mfrac><msub><mi>w</mi><mi>x</mi></msub><msub><mi>w</mi><mi>y</mi></msub></mtd></mtr><mtr><mtd><msub><mover><mi>w</mi><mo>·</mo></mover><mi>y</mi></msub><mo>=</mo><mfrac><msub><mi>M</mi><mi>y</mi></msub><msub><mi>I</mi><mi>y</mi></msub></mfrac><mo>+</mo><mfrac><mrow><msub><mi>I</mi><mi>z</mi></msub><mo>-</mo><msub><mi>I</mi><mi>x</mi></msub></mrow><msub><mi>I</mi><mi>y</mi></msub></mfrac><msub><mi>w</mi><mi>z</mi></msub><msub><mi>w</mi><mi>x</mi></msub></mtd></mtr><mtr><mtd><msub><mover><mi>w</mi><mo>·</mo></mover><mi>x</mi></msub><mo>=</mo><mfrac><msub><mi>M</mi><mi>x</mi></msub><msub><mi>I</mi><mi>x</mi></msub></mfrac><mo>+</mo><mfrac><mrow><msub><mi>I</mi><mi>y</mi></msub><mo>-</mo><msub><mi>I</mi><mi>z</mi></msub></mrow><msub><mi>I</mi><mi>x</mi></msub></mfrac><msub><mi>w</mi><mi>y</mi></msub><msub><mi>w</mi><mi>z</mi></msub></mtd></mtr><mtr><mtd><mover><mi>φ</mi><mo>·</mo></mover><mo>=</mo><msub><mi>w</mi><mi>x</mi></msub><mrow><mo>(</mo><msub><mi>w</mi><mi>y</mi></msub><mi>sin</mi><mi>φ</mi><mo>+</mo><msub><mi>w</mi><mi>z</mi></msub><mi>cos</mi><mi>φ</mi><mo>)</mo></mrow><mi>tan</mi><mi>θ</mi></mtd></mtr><mtr><mtd><msub><mi>n</mi><mi>y</mi></msub><mo>=</mo><msub><mi>F</mi><mi>y</mi></msub><mo>/</mo><mi>mg</mi></mtd></mtr><mtr><mtd><msub><mi>n</mi><mi>z</mi></msub><mo>=</mo><msub><mi>F</mi><mi>z</mi></msub><mo>/</mo><mi>mg</mi></mtd></mtr></mtable></mfenced><mo>-</mo><mo>-</mo><mo>-</mo><mrow><mo>(</mo><mn>4</mn><mo>)</mo></mrow><mo>;</mo></mrow></math>]]></maths>其中,<img file="FDA0000457840460000027.GIF" wi="47" he="79" />为横滚角φ的一阶导数,<img file="FDA0000457840460000028.GIF" wi="52" he="58" />和<img file="FDA0000457840460000029.GIF" wi="46" he="78" />分别为攻角和侧滑角的一阶导数,<img file="FDA00004578404600000210.GIF" wi="93" he="69" /><img file="FDA00004578404600000211.GIF" wi="65" he="73" />和<img file="FDA00004578404600000212.GIF" wi="57" he="69" />分别为w<sub>z</sub>、w<sub>y</sub>和w<sub>x</sub>的一阶导数;横滚角φ描述运动体绕其纵轴的滚动角度,攻角α描述运动体纵轴与运动体速度方向之间的夹角,侧滑角β描述运动体运动速度与运动体纵对称面之间的夹角;<img file="FDA00004578404600000213.GIF" wi="94" he="68" /><img file="FDA00004578404600000214.GIF" wi="95" he="75" /><img file="FDA00004578404600000215.GIF" wi="70" he="69" />分别为运动体绕自身横轴、纵轴、立轴的转动角加速度。 |