主权项 |
一种空间绳系机器人三轴主动姿态控制方法,其特征在于:采用以下步骤:步骤1:计算空间绳系机器人三个方向的姿态控制力矩u<sub>x</sub>、u<sub>y</sub>及u<sub>z</sub>:<maths num="0001" id="cmaths0001"><math><![CDATA[<mfenced open='{' close=''><mtable><mtr><mtd><msub><mi>u</mi><mi>x</mi></msub><mo>=</mo><mo>-</mo><msub><mi>I</mi><mi>x</mi></msub><mrow><mo>(</mo><mo>-</mo><mfrac><msub><mi>I</mi><mi>r</mi></msub><msub><mi>I</mi><mi>y</mi></msub></mfrac><msub><mi>x</mi><mn>7</mn></msub><msub><mi>x</mi><mn>4</mn></msub><mo>+</mo><mrow><mo>(</mo><mn>1</mn><mo>-</mo><msub><mi>n</mi><mi>x</mi></msub><mo>)</mo></mrow><mi>Ω</mi><msub><mi>x</mi><mn>6</mn></msub><mo>-</mo><msub><mi>n</mi><mi>x</mi></msub><msup><mi>Ω</mi><mn>2</mn></msup><msub><mi>x</mi><mn>1</mn></msub><mo>+</mo><mfrac><msub><mi>I</mi><mi>r</mi></msub><msub><mi>I</mi><mi>x</mi></msub></mfrac><mi>Ω</mi><msub><mi>x</mi><mn>7</mn></msub><mo>)</mo></mrow><mo>+</mo><msub><mi>v</mi><mi>x</mi></msub></mtd></mtr><mtr><mtd><msub><mi>u</mi><mi>y</mi></msub><mo>=</mo><mo>-</mo><msub><mi>I</mi><mi>y</mi></msub><mrow><mo>(</mo><mfrac><msub><mi>I</mi><mi>r</mi></msub><msub><mi>I</mi><mi>y</mi></msub></mfrac><msub><mi>x</mi><mn>7</mn></msub><msub><mi>x</mi><mn>2</mn></msub><mo>-</mo><mfrac><msub><mi>I</mi><mi>r</mi></msub><msub><mi>I</mi><mi>y</mi></msub></mfrac><mi>Ω</mi><msub><mi>x</mi><mn>7</mn></msub><msub><mi>x</mi><mn>5</mn></msub><mo>)</mo></mrow><mo>+</mo><msub><mi>v</mi><mi>y</mi></msub></mtd></mtr><mtr><mtd><msub><mi>u</mi><mi>z</mi></msub><mo>=</mo><mo>-</mo><msub><mi>I</mi><mi>z</mi></msub><mrow><mo>(</mo><mrow><mo>(</mo><msub><mi>n</mi><mi>z</mi></msub><mo>-</mo><mn>1</mn><mo>)</mo></mrow><mi>Ω</mi><msub><mi>x</mi><mn>2</mn></msub><mo>-</mo><msub><mi>n</mi><mi>z</mi></msub><msup><mi>Ω</mi><mn>2</mn></msup><msub><mi>x</mi><mn>5</mn></msub><mo>+</mo><mfrac><mrow><mi>tan</mi><msub><mi>x</mi><mn>3</mn></msub></mrow><mrow><mi>cos</mi><msub><mi>x</mi><mn>1</mn></msub><msub><mi>I</mi><mi>z</mi></msub></mrow></mfrac><msub><mi>u</mi><mi>x</mi></msub><mo>-</mo><mfrac><mrow><mi>tan</mi><msub><mi>x</mi><mn>1</mn></msub></mrow><msub><mi>I</mi><mi>z</mi></msub></mfrac><msub><mi>u</mi><mi>y</mi></msub><mo>)</mo></mrow><mo>+</mo><msub><mi>v</mi><mi>z</mi></msub></mtd></mtr></mtable></mfenced>]]></math><img file="FDA0000490775370000011.GIF" wi="1185" he="461" /></maths>其中x<sub>1</sub>=φ、<img file="FDA0000490775370000012.GIF" wi="155" he="72" />x<sub>3</sub>=θ、<img file="FDA0000490775370000013.GIF" wi="154" he="72" />x<sub>5</sub>=ψ、<img file="FDA0000490775370000014.GIF" wi="150" he="63" />x<sub>7</sub>=Ω<sub>r</sub>,φ、θ及ψ为空间绳系机器人的三轴姿态角,I<sub>x</sub>、I<sub>y</sub>、I<sub>z</sub>分别为空间绳系机器人转动惯量,Ω<sub>r</sub>为反作用轮转动角速度,I<sub>r</sub>为反作用轮的转动惯量,n<sub>x</sub>和n<sub>z</sub>为系数,Ω为空间绳系机器人的轨道角速度,<maths num="0002" id="cmaths0002"><math><![CDATA[<mrow><msub><mi>v</mi><mi>x</mi></msub><mo>=</mo><msub><mover><mi>φ</mi><mrow><mo>·</mo><mo>·</mo></mrow></mover><mi>d</mi></msub><mo>-</mo><msub><mi>k</mi><mn>1</mn></msub><mrow><mo>(</mo><mi>φ</mi><mo>-</mo><msub><mi>φ</mi><mi>d</mi></msub><mo>)</mo></mrow><mo>-</mo><msub><mi>k</mi><mn>2</mn></msub><mrow><mo>(</mo><mover><mi>φ</mi><mo>·</mo></mover><mo>-</mo><msub><mover><mi>φ</mi><mo>·</mo></mover><mi>d</mi></msub><mo>)</mo></mrow><mo>,</mo><msub><mi>v</mi><mi>y</mi></msub><mo>=</mo><msub><mover><mi>θ</mi><mrow><mo>·</mo><mo>·</mo></mrow></mover><mi>d</mi></msub><mo>-</mo><msub><mi>k</mi><mn>3</mn></msub><mrow><mo>(</mo><mi>θ</mi><mo>-</mo><msub><mi>θ</mi><mi>d</mi></msub><mo>)</mo></mrow><mo>-</mo><msub><mi>k</mi><mn>4</mn></msub><mrow><mo>(</mo><mover><mi>θ</mi><mo>·</mo></mover><mo>-</mo><msub><mover><mi>θ</mi><mo>·</mo></mover><mi>d</mi></msub><mo>)</mo></mrow></mrow>]]></math><img file="FDA0000490775370000015.GIF" wi="1121" he="79" /></maths>及<maths num="0003" id="cmaths0003"><math><![CDATA[<mrow><msub><mi>v</mi><mi>z</mi></msub><mo>=</mo><msub><mover><mi>ψ</mi><mrow><mo>·</mo><mo>·</mo></mrow></mover><mi>d</mi></msub><mo>-</mo><msub><mi>k</mi><mn>5</mn></msub><mrow><mo>(</mo><mi>ψ</mi><mo>-</mo><msub><mi>ψ</mi><mi>d</mi></msub><mo>)</mo></mrow><mo>-</mo><msub><mi>k</mi><mn>6</mn></msub><mrow><mo>(</mo><mover><mi>ψ</mi><mo>·</mo></mover><mo>-</mo><msub><mover><mi>ψ</mi><mo>·</mo></mover><mi>d</mi></msub><mo>)</mo></mrow><mo>,</mo></mrow>]]></math><img file="FDA0000490775370000017.GIF" wi="603" he="63" /></maths>φ<sub>d</sub>、θ<sub>d</sub>及ψ<sub>d</sub>分别为空间绳系机器人的指令滚转角、指令俯仰角及指令偏航角,k<sub>1</sub>、k<sub>2</sub>、k<sub>3</sub>、k<sub>4</sub>、k<sub>5</sub>及k<sub>6</sub>为正值常数;步骤2:根据步骤1得到的空间绳系机器人三个方向的姿态控制力矩u<sub>x</sub>、u<sub>y</sub>及u<sub>z</sub>,由公式<maths num="0004" id="cmaths0004"><math><![CDATA[<mfenced open='{' close=''><mtable><mtr><mtd><msub><mi>M</mi><mi>x</mi></msub><mo>=</mo><msub><mi>u</mi><mi>x</mi></msub><mo>+</mo><msub><mi>M</mi><mi>dx</mi></msub><mo>=</mo><mo>-</mo><mi>sin</mi><mi></mi><mi>φ</mi><mi>cos</mi><mi>θT</mi><msub><mi>z</mi><mi>L</mi></msub><mo>+</mo><mi>cos</mi><mi></mi><mi>φ</mi><mi>cos</mi><mi>θT</mi><msub><mi>y</mi><mi>L</mi></msub><mo>+</mo><msub><mi>M</mi><mi>dx</mi></msub></mtd></mtr><mtr><mtd><msub><mi>M</mi><mi>y</mi></msub><mo>=</mo><msub><mi>u</mi><mi>y</mi></msub><mo>+</mo><msub><mi>M</mi><mi>dy</mi></msub><mo>=</mo><mo>-</mo><mi>sin</mi><mi>θT</mi><msub><mi>z</mi><mi>L</mi></msub><mo>-</mo><mi>cos</mi><mi></mi><mi>φ</mi><mi>cos</mi><mi>θT</mi><msub><mi>x</mi><mi>L</mi></msub><mo>+</mo><msub><mi>M</mi><mi>dy</mi></msub></mtd></mtr><mtr><mtd><msub><mi>M</mi><mi>z</mi></msub><mo>=</mo><mo>-</mo><msub><mi>I</mi><mi>r</mi></msub><msub><mover><mi>Ω</mi><mo>·</mo></mover><mi>r</mi></msub><mo>+</mo><mfrac><mrow><mi>tan</mi><mi>θ</mi></mrow><mrow><mi>cos</mi><mi>φ</mi></mrow></mfrac><msub><mi>u</mi><mi>x</mi></msub><mo>-</mo><mi>tan</mi><mi>φ</mi><msub><mi>u</mi><mi>y</mi></msub><mo>+</mo><msub><mi>M</mi><mi>dz</mi></msub></mtd></mtr><mtr><mtd><mi>T</mi><mo>=</mo><msub><mi>T</mi><mn>0</mn></msub><mo>-</mo><msub><mi>m</mi><mi>r</mi></msub><mover><mi>l</mi><mrow><mo>·</mo><mo>·</mo></mrow></mover></mtd></mtr><mtr><mtd><mover><mi>l</mi><mrow><mo>·</mo><mo>·</mo></mrow></mover><mo>=</mo><mo>|</mo><mrow><mo>(</mo><mo>-</mo><mi>cos</mi><mi></mi><mi>φ</mi><mi>cos</mi><mi>θ</mi><msub><mi>y</mi><mi>L</mi></msub><mo>+</mo><mi>sin</mi><mi></mi><mi>φ</mi><mi>cos</mi><mi>θ</mi><msub><mi>z</mi><mi>L</mi></msub><mo>)</mo></mrow><mover><mi>φ</mi><mrow><mo>·</mo><mo>·</mo></mrow></mover><mo>+</mo><mrow><mo>(</mo><mi>cos</mi><mi>θ</mi><msub><mi>x</mi><mi>L</mi></msub><mo>+</mo><mi>sin</mi><mi></mi><mi>φ</mi><mi>sin</mi><mi>θ</mi><msub><mi>y</mi><mi>L</mi></msub><mo>+</mo><mi>cos</mi><mi></mi><mi>φ</mi><mi>sin</mi><mi>θ</mi><msub><mi>z</mi><mi>L</mi></msub><mo>)</mo></mrow><mover><mi>θ</mi><mrow><mo>·</mo><mo>·</mo></mrow></mover></mtd></mtr><mtr><mtd><mo>+</mo><mi>sin</mi><mi>θ</mi><msub><mover><mi>x</mi><mrow><mo>·</mo><mo>·</mo></mrow></mover><mi>L</mi></msub><mo>-</mo><mi>sin</mi><mi></mi><mi>φ</mi><mi>cos</mi><mi>θ</mi><msub><mover><mi>y</mi><mrow><mo>·</mo><mo>·</mo></mrow></mover><mi>L</mi></msub><mo>+</mo><mrow><mo>(</mo><mo>-</mo><mi>sin</mi><mi>θ</mi><msub><mi>x</mi><mi>L</mi></msub><mo>+</mo><mi>sin</mi><mi></mi><mi>φ</mi><mi>cos</mi><mi>θ</mi><msub><mi>y</mi><mi>L</mi></msub><mo>+</mo><mi>cos</mi><mi></mi><mi>φ</mi><mi>cos</mi><mi>θ</mi><msub><mi>z</mi><mi>L</mi></msub><mo>)</mo></mrow><msup><mover><mi>θ</mi><mo>·</mo></mover><mn>2</mn></msup></mtd></mtr><mtr><mtd><mo>+</mo><mrow><mo>(</mo><mi>sin</mi><mi></mi><mi>φ</mi><mi>cos</mi><mi>θ</mi><msub><mi>y</mi><mi>L</mi></msub><mo>+</mo><mi>cos</mi><mi></mi><mi>φ</mi><mi>cos</mi><mi>θ</mi><msub><mi>z</mi><mi>L</mi></msub><mo>)</mo></mrow><msup><mover><mi>φ</mi><mo>·</mo></mover><mn>2</mn></msup><mo>+</mo><mn>2</mn><mi>cos</mi><mi>θ</mi><mover><mi>θ</mi><mo>·</mo></mover><msub><mover><mi>x</mi><mo>·</mo></mover><mi>L</mi></msub><mo>-</mo><mn>2</mn><mi>cos</mi><mi></mi><mi>φ</mi><mi>cos</mi><mi>θ</mi><msub><mover><mi>y</mi><mo>·</mo></mover><mi>L</mi></msub><mover><mi>φ</mi><mo>·</mo></mover></mtd></mtr><mtr><mtd><mo>+</mo><mrow><mo>(</mo><mn>2</mn><mi>cos</mi><mi></mi><mi>φ</mi><mi>sin</mi><mi>θ</mi><msub><mi>y</mi><mi>L</mi></msub><mo>-</mo><mn>2</mn><mi>sin</mi><mi></mi><mi>φ</mi><mi>sin</mi><mi>θ</mi><msub><mi>z</mi><mi>L</mi></msub><mo>+</mo><mn>2</mn><mi>sin</mi><mi></mi><mi>φ</mi><mi>sin</mi><mi>θ</mi><mo>)</mo></mrow><mover><mi>φ</mi><mo>·</mo></mover><mover><mi>θ</mi><mo>·</mo></mover><mo>+</mo><mn>2</mn><mi>sin</mi><mi></mi><mi>φ</mi><mi>sin</mi><mi>θ</mi><msub><mover><mi>y</mi><mo>·</mo></mover><mi>L</mi></msub><mover><mi>θ</mi><mo>·</mo></mover><mo>|</mo></mtd></mtr></mtable></mfenced>]]></math><img file="FDA0000490775370000018.GIF" wi="1511" he="681" /></maths>得到空间系绳连接点位置x<sub>L</sub>、y<sub>L</sub>及反作用轮的转动角速度Ω<sub>r</sub>实现空间绳系机器人三轴姿态的主动控制;其中M<sub>dx</sub>、M<sub>dy</sub>和M<sub>dz</sub>分别为空间绳系机器人三个轴向的干扰力矩,M<sub>x</sub>、M<sub>y</sub>和M<sub>z</sub>为空间绳系机器人所受到的力矩,T为空间系绳上的张力,T<sub>0</sub>为空间绳系机器人在平衡状态时,空间系绳应具有的标称张力,T<sub>0</sub>=3Ω<sup>2</sup>m<sub>r</sub>lm<sub>r</sub>空间绳系机器人的质量,l为空间系绳的长度;所述平衡状态指空间绳系机器人位于空间平台与地心连线上。 |