[发明专利]角速率输入条件下单子样旋转矢量姿态方法

权利要求书

1.一种角速率输入条件下的单子样旋转矢量姿态方法，具体包括如下步骤：

步骤S1.获取设当前k时刻的姿态角为att_k=[θγψ]^T，θ、γ和ψ分别为俯仰角、横滚角和偏航角；

步骤S2、计算旋转矢量

步骤S3、计算当前k时刻的姿态四元数Q(t_k)，具体过程为：

姿态角与坐标转换矩阵的关系为：

Cnb=cosγcosΨ-sinγsinψsinθcosγsinΨ+sinγcosΨsinθ-sinγcosθ-sinΨcosθcosΨcosθsinθsinγcosΨ+cosγsinψsinθsinγsinψ-cosγcosψsinθcosγcosθ]]>

设：Cnb=T11T21T31T12T22T23T13T23T33]]>

则坐标转换矩阵与姿态四元数Q(t_k)的关系为：

|q1|=121+T11-T22-T33|q2|=121-T11+T22-T33|q3|=121-T11-T22+T33|q4|=121+T11+T22+T33]]>

其中，Q(t_k)=[q₀  q₁  q₂  q₃]^T，sign(q₀)的符号可任选，q₁,q₂,q₃的符号按下式确定：

sign(q1)=sign(q0)[sign(T32-T23)]sign(q2)=sign(q0)[sign(T13-T31)]sign(q3)=sign(q0)[sign(T21-T12)]]]>

由此可以由当前的姿态角att_k计算当前的姿态四元数Q(t_k)；

步骤S4、姿态变化四元数q与旋转矢量的关系和计算步骤S2中得到的旋转矢量对应的姿态变化四元数q，其中，表示转过的角度，u表示旋转瞬轴和方向；

步骤S5、根据更新得到k+1时刻的姿态四元数Q(t_k+h)，其中，q(h)为(t_k)时刻至(t_k+h)时刻对应的坐标系姿态变化四元数，即步骤S4得到的姿态变化四元数q；

步骤S6、根据步骤S3中姿态变化四元数与坐标转换矩阵的关系以及坐标转换矩阵与姿态角的关系，计算出k+1时刻的姿态角att_k，完成一次姿态更新；

步骤S7、重复步骤S1至步骤S6即实现了姿态更新。