一种冗余自由度机构位置更新方法与流程

技术特征：

1.一种冗余自由度机构位置更新方法，其特征在于，包括如下步骤：

(1)对载体现有的姿态进行测量，获得载体此时的航向角ψ、俯仰角φ和横摇角γ；

(2)根据卫星的参数，和第(1)步的测量结果，利用下式：

$T_s=\left[\begin{array}{ccc}cos\mathrm{\γ}& 0& -sin\mathrm{\γ}\\ 0& 1& 0\\ sin\mathrm{\γ}& 0& \cos \mathrm{\γ}\end{array}\right]\left[\begin{array}{ccc}1& 0& 0\\ 0& cos\mathrm{\φ}& sin\mathrm{\φ}\\ 0& -\sin \mathrm{\φ}& cos\mathrm{\φ}\end{array}\right]\left[\begin{array}{ccc}cos\mathrm{\ψ}& \sin \mathrm{\ψ}& 0\\ -sin\mathrm{\ψ}& cos\mathrm{\ψ}& 0\\ 0& 0& 1\end{array}\right].$

$\left[\begin{array}{ccc}{\mathrm{CE}}_e{\mathrm{S\σ}}_e-{\mathrm{C\σ}}_e{\mathrm{SA}}_e{\mathrm{SE}}_e& {\mathrm{CE}}_e{\mathrm{C\σ}}_e+{\mathrm{SA}}_e{\mathrm{SE}}_e{\mathrm{S\σ}}_e& {\mathrm{CE}}_e{\mathrm{SA}}_e\\ -{\mathrm{SA}}_e{\mathrm{S\σ}}_e-{\mathrm{CA}}_e{\mathrm{C\σ}}_e{\mathrm{SE}}_e& -{\mathrm{C\σ}}_e{\mathrm{SA}}_e+{\mathrm{CA}}_e{\mathrm{SE}}_e{\mathrm{S\σ}}_e& {\mathrm{CA}}_e{\mathrm{CE}}_e\\ {\mathrm{CE}}_e{\mathrm{C\σ}}_e& -{\mathrm{CE}}_e{\mathrm{S\σ}}_e& {\mathrm{SE}}_e\end{array}\right]$

计算出来在当前时刻卫星在载体坐标系下的描述矩阵T_s；式(1)中的航向角ψ、俯仰角φ和横摇角γ为第(1)步的测量值，E_e代表卫星参数的俯仰角，A_e代表卫星参数的方位角，σ_e代表卫星参数的极化角，C代表cos，S代表sin，例如：CE_e代表cosE_e，SE_e代表sinE_e；此时的天线系统的四个关节角为θ₁，θ₂，θ₃，θ₄；关节角度与描述矩阵T_s之间的函数关系为：T_s＝F(θ₁,θ₂,θ₃,θ₄)；

(3)当前时刻的天线框架的姿态矩阵为T_a，利用第(2)步中的卫星矩阵T_s与T_a求解出两个坐标系之间的转动微元，

$T_s\·T_a^{-1}=\left[\begin{array}{ccc}1& -\mathrm{\δ}z& \mathrm{\δ}y\\ \mathrm{\δ}z& 1& -\mathrm{\δ}x\\ -\mathrm{\δ}y& \mathrm{\δ}x& 1\end{array}\right]$

其中δx为绕x轴的转动微元，δy为绕y轴的转动微元，δz为绕z轴的转动微元；T_a^-1为矩阵T_a的逆矩阵；将其中的速度微元提取出来组成向量[δx δy δz]^T；

(4)对当前的天线系统结构计算雅克比矩阵值，记为J；

(5)求取天线系统的雅克比矩阵J的Moore-Penrose逆(简称M-P逆)，将M-P逆记为J⁺；

(6)求取四个关节角的运动微元：

Θ＝[dθ₁ dθ₂ dθ₃ dθ₄]^T＝J⁺[δx δy δz]^T

其中，Θ为关节运动微元向量，dθ₁为第一个关节角的运动微元，dθ₂为第二个关节角的运动微元，dθ₃为第三个关节角的运动微元，dθ₄为第四个关节角的运动微元；

(7)计算各关节角之前运动微元的比例值，其中k₂为dθ₂与dθ₁之间的比，其中k₃为dθ₃与dθ₁之间的比，其中k₄为dθ₄与dθ₁之间的比；

(8)进行位置更新时每个关节的运动量记为：Δθ₁，Δθ₂，Δθ₃，Δθ₄，利用再(7)中求得的k₂，k₃和k₄以及Δθ₁表示Δθ₂，Δθ₃，Δθ₄有：

Δθ₂＝k₂Δθ₁

Δθ₃＝k₃Δθ₁

Δθ₄＝k₄Δθ₁

(9)建立位置更新后的关节角度方程为：

T_a＝F(θ₁+Δθ₁,θ₂+k₂Δθ₁,θ₃+k₃Δθ₁,θ₄+k₄Δθ₁)

其中规则F是建立在关节角度和末端姿态之间的约束方程；这个约束关系的是通过建立冗余自由度运动机构的D-H模型得到的；这个模型给定了机构四个运动角度与末端姿态描述矩阵之间的对应关系；

(10)利用牛顿迭代方法，求解出第(9)步中的方程，得到Δθ₁，再利用式(4)求得Δθ₂，Δθ₃，Δθ₄；

(11)得到关节的更新位置为θ₁+Δθ₁，θ₂+Δθ₂，θ₃+Δθ₃，θ₄+Δθ₄，完成位置更新。