[发明专利]一种基于三维坐标变换的前摆心喷管铰链耦合解耦算法

权利要求书

1.一种基于三维坐标变换的前摆心喷管铰链耦合解耦算法，其特征于：两台伺服伺服作动器均成90°配置，定义静坐标系O-XYZ以喷管摆心为原点，固连于喷管摆心；X轴、Y轴分别为喷管2个单摆方向；动坐标系在初始状态下与静坐标系O-X″Y″Z″一致，与喷管固连，随着喷管姿态的变化而变化，从喷管尾部看，静坐标系和动坐标系Z(Z″)轴垂直纸面向外；零位时，两个坐标系原点重合，坐标轴方向相平行；

本发明包括以下步骤：

步骤1.根据姿态角得到坐标系转换矩阵；

步骤2.计算上述姿态角状态下作动器的位置；

步骤3.得到作动器活塞杆的伸缩量。

2.如权利要求1所述的一种基于三维坐标变换的前摆心喷管铰链耦合解耦算法，其特征在于：所述步骤一中

若姿态角为α，β为喷管在2个方向上的单摆角度,坐标系转换矩阵为：

。

3.如权利要求1所述的一种基于三维坐标变换的前摆心喷管铰链耦合解耦算法，其特征在于：所述步骤二中作喷管摆动时，作动器下支点会随着喷管运动，作动器上支点始终保持不动；作动器1、2的上支点坐标分别为(c₁,c₂,c₃)、(d₁,d₂,d₃)

作动器1、2下支点在喷管运动前，相对于动坐标为(a₁,a₂,a₃)，(b₁,b₂,b₃)，定义下支点计算坐标为：

作动器下支点在喷管运动后，相对于静坐标系使用向量表示为：

则作动器1、2方向分别摆动角度α,β之后两作动器的长度为：

。

4.如权利要求1所述的一种基于三维坐标变换的前摆心喷管铰链耦合解耦算法，其特征在于：作动器活塞杆的伸缩量，即位移可由作动器的上、下支点之间的距离减去作动器初始长l₀来确定为：

Δl_i＝l_i-l₀ (i＝1，2) 。