船舶在海上航行或无人飞行器执行远距离任务时，与陆地或指挥中心的通信都需要依靠地球同步通信卫星。上下行信号的传送，需要天线接收主瓣指向卫星，同时天线的极化方向与卫星发射信号的极化方向需要保持一致^[1]。因此天线姿态稳定系统被广泛使用，实现卫星天线与地球同步卫星之间的相对稳定。接收天线主瓣方向与极化方向之间是垂直的，利用这两个方向构成了一个来描述天线姿态的坐标系。理论上，坐标系在空间的定点转动有3个自由度，具有3个正交自由度的运动机构就可以实现天线姿态的伺服。现实中，为了克服运动机构奇异点问题，增加系统灵活性，采用具有4个正交运动轴的框架结构，本文研究对象是一个冗余自由度的天线姿态稳定系统。

联接天线与运载器的稳定系统将实现了天线相对于运载器的姿态运动，同时也建立了天线姿态与稳定系统关节空间的约束。对与任务空间而言使用n维向量X，X∈Rⁿ描述规划的任务；对于关节空间，使用m维向量Θ，Θ∈C^m描述关节变量。当m>n时系统即为冗余自由度系统，可以实现关节的避障、克服奇异点、完成第二任务等要求，例如实现机器人更好的灵活性^[2-3]，躲避障碍^[4-7]、防止关节运动超界^[8-11]、避免奇异性^[12-15]、减小关节运动速度及优化动力学性能等^[16-17]。对于本文中的姿态随动系统，当系统自由度为m=4时，形成姿态冗余自由度系统。对于冗余自由度机器人而言，运动学逆解有无穷多组。从无穷多组解中选择合适解的过程，也就是冗余自由度问题的求解过程。成果包括：几何求解方法^[18-19]；Pieper证明了对于连续3个轴交于一点的冗余自由度机构，一定具有解析解^[20]；Liegeois及梯度投影法把冗余自由度问题的逆解分为了特解和齐次解，利用关节的自运动实现冗余自由度机器人的次任务^[21-23]；Whitney最早采用了这种广义逆形式来求取一般机器人的逆运动学及轨迹规划问题，获得了关节角速度向量的模最小解，也成为基于速度层面求解冗余自由度问题的基础^[24]；Wampler最早提出了加权最小二乘法实现主任务与自运动的同步完成^[25]；Lee和Badler提出了可以选择合适的关节角度作为参数，表示其余的角度，进行解析解的求解方法^[26-27]。

以上方法中，除几何方法和文献外，主要集中在对关节空间的速度求解，对于关节空间的位置求解研究较少，在对于机构末端轨迹有高精度要求的应用场合，并不适用^[19]。

本文提出了一种建立在加权伪逆速度解基础上的恒定速比关节位置算法，使用该方法直接计算得到关节空间中的角位置值，计算误差不累计，能够保证天线姿态在运动过程中达到期望精度。

1 姿态稳定系统的描述

运载器、天线、稳定系统和卫星之间相互姿态关系与各坐标系的定义如图 1所示。图 1中，建立当地地理坐标系为东北天坐标系X_eY_eZ_e；运载器坐标系为与其固联的前右上坐标系X_bY_bZ_b；与天线固连的坐标系为X_aY_aZ_a，其中Z_a为天线接收主波瓣方向，Y_a为天线极化方向；建立与卫星固联的坐标系X_sY_sZ_s，其中Z_s为卫星发射信号主瓣的相反方向，Y_s为卫星发射天线极化方向。

天线姿态稳定问题等效于将天线本体坐标系X_aY_aZ_a稳定在地理坐标系X_eY_eZ_e中，当运载器坐标系X_bY_bZ_b发生变化时，通过稳定系统的伺服运动，保证坐标系X_aY_aZ_a与卫星固联坐标系X_sY_sZ_s的一致。

设每次一转动的姿态转移矩阵分别为T₁、T₂、T₃和T₄，而每一个姿态转移矩阵又仅与一个对应的关节角度有关，得到运载器坐标系与天线本体坐标系之间的姿态转移矩阵为

$ ^b{\mathit{\boldsymbol{T}}_a} = {\mathit{\boldsymbol{T}}_1} \cdot {\mathit{\boldsymbol{T}}_2} \cdot {\mathit{\boldsymbol{T}}_3} \cdot {\mathit{\boldsymbol{T}}_4} = \mathit{\boldsymbol{f}}\left( {{\theta _1},{\theta _2},{\theta _3},{\theta _4}} \right) $

(1)

式中：θ₁~θ₄为姿态稳定系统的4个运动轴角度值。尝试建立图 1中各坐标系之间的转换关系，这里用姿态转移矩阵^eT_b表示坐标系X_bY_bZ_b在坐标系X_eY_eZ_e中的表达。同理利用姿态转移矩阵写出各坐标系之间转换关系为

$ ^b{\mathit{\boldsymbol{T}}_s}{ = ^b}{\mathit{\boldsymbol{T}}_e}{ \cdot ^e}{\mathit{\boldsymbol{T}}_s}{ = ^b}{\mathit{\boldsymbol{T}}_a} $

(2)

式中：^eT_s为卫星在地理坐标系下的表示，对于给定的地球同步卫星目标，在地球上某一点观察卫星，可以认为^eT_s保持不变；^eT_b为运载器在地理坐标系下的表示；^bT_a为天线在运载器坐标系下的表示；运载器运动，^eT_b发生变化，系统完成的功能就是调整θ₁~θ₄，更新^bT_a使得(2)式保持成立^[1]

$ ^b{\mathit{\boldsymbol{T}}_a}{ = ^b}{\mathit{\boldsymbol{T}}_e}{ \cdot ^e}{\mathit{\boldsymbol{T}}_s} = \mathit{\boldsymbol{N}}\left( t \right) $

(3)

式中：将卫星在运载器坐标系下的连续运动用矩阵序列N(t)表示。则本文问题转化为如何在如式(4)的约束条件下，进行冗余自由度机构逆运动学求解。

$ f\left( {{\theta _1},{\theta _2},{\theta _3},{\theta _4}} \right) = \mathit{\boldsymbol{N}}\left( t \right) $

(4)

矩阵N(t)的值则可通过姿态的运动和卫星参数求得：

$ \begin{array}{*{20}{c}} {\mathit{\boldsymbol{N}}\left( t \right) = \left[ {\begin{array}{*{20}{c}} {\cos \gamma }&0&{ - \sin \gamma }\\ 0&1&0\\ {\sin \gamma }&0&{\cos \gamma } \end{array}} \right]\left[ {\begin{array}{*{20}{c}} 1&0&0\\ 0&{\cos \varphi }&{\sin \varphi }\\ 0&{ - \sin \varphi }&{\cos \varphi } \end{array}} \right]\left[ {\begin{array}{*{20}{c}} {\cos \psi }&{\sin \psi }&0\\ { - \sin \psi }&{\cos \varphi }&0\\ 0&0&1 \end{array}} \right] \cdot }\\ {\left[ {\begin{array}{*{20}{c}} {C{E_e}S{\sigma _e} - C{\sigma _e}S{A_e}S{E_e}}&{C{E_e}C{\sigma _e} + S{A_e}S{E_e}S{\sigma _e}}&{C{E_e}S{A_e}}\\ { - S{A_e}S{\sigma _e} - C{A_e}C{\sigma _e}S{E_e}}&{ - C{\sigma _e}S{A_e} + C{A_e}S{E_e}S{\sigma _e}}&{C{A_e}C{E_e}}\\ {C{E_e}C{\sigma _e}}&{ - C{E_e}S{\sigma _e}}&{S{E_e}} \end{array}} \right]} \end{array} $

这里假设卫星方位角度为A_e，俯仰角度为E_e，极化角约定为绕卫星姿态矩阵Z_s轴经过σ_e的转动后，使得Y_s轴与地面平行，此时的σ_e为极化角，并约定极化角正方向为逆时针方向，且确定极化角时以坐标系的Y_s轴正方向为准。ψ为运载器航向角，ϕ为运载器的纵摇角，γ为运载器的横摇角。

2 加权意义下的速度最优解 2.1 雅克比矩阵的建立

天线姿态稳定是一个姿态跟踪控制过程。将初始条件位置时刻定义为t₀时刻，此时的天线姿态矩阵为N(t₀)。随着运载器运动，在t₁时刻运载器坐标系下的天线姿态应更新至N(t₁)，在任意时刻姿态矩阵为N(t_n)。根据机构的约束关系有：

$ f\left( {{\mathit{\boldsymbol{ \boldsymbol{\varTheta} }}_n}} \right) = \mathit{\boldsymbol{N}}\left( {{t_n}} \right) $

(5)

式中:f(·)框架结构与末端姿态矩阵的正运动学关系，Θ_n为t_n时刻关节空间中的角位置值Θ_n=[θ₁(t_n) θ₂(t_n) θ₃(t_n) θ₄(t_n)]^T。跟踪问题记为根据N(t_n)求得Θ_n

$ {\mathit{\boldsymbol{ \boldsymbol{\varTheta} }}_n} = {f^{ - 1}}\left( {\mathit{\boldsymbol{N}}\left( {{t_n}} \right)} \right) $

(6)

对于式(6)所描述的冗余自由度运动学逆解，f^-1(·)代表函数f(·)的反函数。利用系统的Moore-Penrose的广义逆^[24](简称M-P逆)可求得速度层面的最优解，本文中提出的恒定速比关节位置算法也是从伪逆速度的角度出发的。首先建立由雅克比矩阵J描述的关节运动与天线姿态运动的关系为

$ \mathit{\boldsymbol{\dot X}} = \mathit{\boldsymbol{J}}\left( \mathit{\boldsymbol{ \boldsymbol{\varTheta} }} \right) \cdot \mathit{\boldsymbol{ \boldsymbol{\dot \varTheta} }} $

(7)

式中：Ẋ=[ω_x ω_y ω_z]^T，在本问题中代表了在运载器坐标系下，期望天线坐标系的运动速度矢量，$\boldsymbol{\mathit{\dot \varTheta}} $代表了此时Ẋ对应的关节速度矢量。J(Θ)为关节角为Θ矢量时所对应的雅克比矩阵。后续J(Θ)简化记为J。

2.2 天线坐标系的转动速度

在前节中已经获得了卫星姿态的描述矩阵^eT_s，且通过惯性测量单元得到了t时刻的运载器姿态的欧拉角，求得了^eT_b。利用式(2)直接得到连续变化的矩阵序列N(t)。对于任意相连时刻的N(t_n)和N(t_n+1)=N(t_n+Δt)有：

$ \mathit{\boldsymbol{N}}\left( {{t_n} + \Delta t} \right) = {\rm{Rot}}\left( {q,{\rm{d}}\theta } \right)\mathit{\boldsymbol{N}}\left( {{t_n}} \right) $

(8)

式中：

$ {\rm{Rot}}\left( {q,{\rm{d}}\theta } \right) = {\rm{Rot}}\left( {x,{\rm{ \mathit{ δ} }}x} \right){\rm{Rot}}\left( {y,{\rm{ \mathit{ δ} }}y} \right){\rm{Rot}}\left( {z,{\rm{ \mathit{ δ} }}z} \right) $

(9)

Rot(q, dθ)代表坐标系绕某一个轴q转动了dθ角度，Δt为两次测量测量之间的时间间隔；Rot(x, δx)为绕当前坐标系的x轴转动δx的微分运动，相应地Rot(y, δy)和Rot(z, δz)为绕另外两个轴的微分运动。当δx、δy和δz较小时，忽略二阶小量可得

$ \left[ {\begin{array}{*{20}{c}} 1&{ - {\omega _z}}&{{\omega _y}}\\ {{\omega _z}}&1&{ - {\omega _x}}\\ { - {\omega _y}}&{{\omega _x}}&1 \end{array}} \right] = \frac{{\mathit{\boldsymbol{N}}\left( {{t_n} + \Delta t} \right) \cdot {\mathit{\boldsymbol{N}}^{\rm{T}}}\left( {{t_n}} \right)}}{{\Delta t}} $

(10)

从式(10)得到相邻两个时刻之间天线坐标系X_aY_aZ_a的转动角速度，组成Ẋ。

2.3 瞬时关节最优速度求解

许多文献针对冗余自由度机构在速度层面的解进行了深入的探讨，其中具有加权M-P逆解具有各关节同步运动，无关节自运动的特点，本文选用此方法作为关节速度的逆解方法^[25]。对于四轴稳定系统，第1~4轴所消耗的能量逐渐减小，按照权值系数逐步降低的原则为转动关节分配转动权值，建立权值向量

$ \mathit{\boldsymbol{W}} = {\left[ {\begin{array}{*{20}{c}} {{w_1}}&{{w_2}}&{{w_3}}&{{w_4}} \end{array}} \right]^{\rm{T}}} $

(11)

此时的关节速度可表示为

$ \mathit{\boldsymbol{ \boldsymbol{\dot \varTheta} }} = {\mathit{\boldsymbol{W}}^{ - 1}}{\mathit{\boldsymbol{J}}^{\rm{T}}}{\left( {\mathit{\boldsymbol{J}} \cdot {\mathit{\boldsymbol{W}}^{ - 1}} \cdot {\mathit{\boldsymbol{J}}^{\rm{T}}}} \right)^{ - 1}}\mathit{\boldsymbol{\dot X}} $

(12)

式中：J天线稳定系统的雅克比矩阵，将Ẋ代入式(2)进行计算，获得在天线运动平均速度所对应关节速度加权最优解$\boldsymbol{\mathit{\dot \varTheta }} $。

3 恒定速比位置更新方法 3.1 姿态跟踪系统的特点

姿态跟踪系统处在另外一种情况，一方面对于末端姿态精度有较高要求；另一方面，在给定一个期望姿态的情况下，希望系统够以最快的速度实现姿态更新，实现对目标位置。

对于姿态跟踪系统，姿态测量单元每秒更新20~50次姿态信息，每个更新的姿态信息对应姿态矩阵序列N(t)中的一个矩阵。每当新的姿态矩阵出现，要求跟踪系统能够尽可能快速地实现天线姿态与期望姿态N(t)的一致。对于两个姿态矩阵之间的姿态转移过程则不关注，不需要对两个姿态之间的轨迹做精确规划，而是将关注的重点放在对应每一个期望姿态所对应的关节角上，即对式(6)快速求解的方法，获得运动学逆解。

3.2 现有方法的不足

1) 在前2节中，已经得到了局部速度最优解$\boldsymbol{\mathit{\dot \varTheta }} $，但对这样的速度直接进行积分将导致机构末端姿态误差的不断累积，无法满足对于机构末端轨迹有较高精度要求的应用场合。如果固定某一个关节角，虽然可以求取运动学逆解，但无法充分发挥冗余自由度机构灵活的特点，同时对于如何选择固定关节也缺少理论依据。

2) 姿态跟踪系统的控制不同于离线规划或示教方式规划的机器人系统，后者可以根据精度要求详细地规划轨迹上的每一个点。姿态跟踪系统关注的是期望姿态的快速建立和姿态建立精度，而对于给定的期望姿态如何直接求解关节的角位置尚没有成熟的方法。虽然可以建立姿态误差的反馈，多次计算伪逆，直至末端位姿满足精度要求。但计算量大，收敛速度慢，不适合进行在线规划。

3.3 恒定速比位置算法

本文提出了一种基于关节最优速度比恒定和姿态约束方程的关节位置算法，简称为恒定速比(CRR)位置算法。

对于给定末端坐标系运动速度，基于式(12)可获得关节瞬时最优速度。冗余自由度最优速度意义本质上是将末端姿态运动合理地分解到各运动关节。角位移是有角速率和运动时间决定的，在相同的采样时间间隔上，如果角速率保持不变，那么关节空间角位移将正比于角速率。各轴的瞬时速率值，也代表了各轴瞬时角度运动量的大小。各关节瞬时速度之比，即瞬时关节运动量之比。将姿态转移过程中关节运动速度比设定为恒定，即恒定速比，是本文算法的核心思想。将每一个关节的运动速度通过某一个关节运动速度和关节之间速度的比值关系表示，并设定这一速比关系在每一次姿态转移过程中保持恒定。通过这样的表达，可以将采样周期内关节角位置增量表达出来，将这个待求角度增量的表达式直接带入姿态约束方程，求得每个关节角的增量值，进而获得关节角位置。

将此方法应用于天线姿态稳定系统，求解关节角位置的步骤为：

1) 根据卫星的参数，计算矩阵^eT_s；

2) 与运载器固连的姿态测量单元检测到运载器的运动姿态，利用欧拉角ψ、φ和γ描述航向角、纵摇角和横摇角，计算得到^bT_e，结合^eT_s求得此时卫星在运载器坐标系下的姿态矩阵记为N(t_n+1)，相应的前一时刻的姿态矩阵记为N(t_n);

3) 在工作空间中进行运动微分的提取。计算连续两个位姿转动之间的差值，即两位姿之间的姿态转移矩阵为

$ \mathit{\boldsymbol{N}}\left( {{t_{n + 1}}} \right) = {\rm{Rot}}\left( {q,{\rm{d}}\theta } \right)\mathit{\boldsymbol{N}}\left( {{t_n}} \right) $

(13)

根据式(9)可以求得以上转动过程末端姿态变化的平均速度Ẋ_n=[ω_x ω_y ω_z]^T;

4) 进行冗余自由度结构运动学即时速度最优求解。将从式(9)中提出去出来的微分运动向量，代入式(11)，计算出加权最小乘意义下的最优解${\boldsymbol{\mathit{\dot \varTheta}} _n} = {\left[ {{{\begin{array}{*{20}{c}} {{{\dot \theta }_1}}&{{{\dot \theta }_2}}&{{{\dot \theta }_3}}&{\dot \theta } \end{array}}_4}} \right]^{\rm{T}}} $。这里的$ \boldsymbol{\mathit{\dot \varTheta}} _n$为t_n测量时刻所对应的瞬时最优速度。

$ {{\mathit{\boldsymbol{ \boldsymbol{\dot \varTheta} }}}_n} = {\mathit{\boldsymbol{W}}^{ - 1}}\mathit{\boldsymbol{J}}_n^{\rm{T}}{\left( {{\mathit{\boldsymbol{J}}_n} \cdot {\mathit{\boldsymbol{W}}^{ - 1}} \cdot \mathit{\boldsymbol{J}}_n^{\rm{T}}} \right)^{ - 1}}{{\mathit{\boldsymbol{\dot X}}}_n} $

(14)

式中:J_n为Θ_n所对应的雅克比矩阵，J_n^T为该矩阵的转置，W^-1代表矩阵W的逆运算。

5) 根据建立的关节空间变量与工作空间之间的关系，在第n次姿态测量的时刻，有：

$ f\left( {{\mathit{\boldsymbol{ \boldsymbol{\varTheta} }}_n}} \right) = \mathit{\boldsymbol{N}}\left( {{t_n}} \right) $

(15)

那么对于t_n+1有：

$ \begin{array}{*{20}{c}} {f\left( {{\mathit{\boldsymbol{ \boldsymbol{\varTheta} }}_{n + 1}}} \right) = f\left( {{\mathit{\boldsymbol{ \boldsymbol{\varTheta} }}_n} + \Delta \mathit{\boldsymbol{ \boldsymbol{\varTheta} }}} \right) = }\\ {f\left( {{\mathit{\boldsymbol{ \boldsymbol{\varTheta} }}_n} + \mathit{\boldsymbol{ \boldsymbol{\varPhi} }} \cdot \Delta t} \right) = \mathit{\boldsymbol{N}}\left( {{t_{n + 1}}} \right)} \end{array} $

(16)

式中:Δt为关节空间增量，为本问题所需要求解的值；Δt为姿态测量时间间隔；Φ为关节空间运动的平均速度矢量，Φ=[ξ₁ ξ₂ ξ₃ ξ₄]^T，ξ_i为对应关节在Δt时间内的平均速度；

6) 建立恒等速比的关系式为

$ \frac{{{{\dot \theta }_1}\left( {{t_n}} \right)}}{{{\xi _1}}} = \frac{{{{\dot \theta }_2}\left( {{t_n}} \right)}}{{{\xi _2}}} = \frac{{{{\dot \theta }_3}\left( {{t_n}} \right)}}{{{\xi _3}}} = \frac{{{{\dot \theta }_4}\left( {{t_n}} \right)}}{{{\xi _4}}} $

(17)

利用第一轴表示其余轴的速度，有：

$ \left\{ \begin{array}{l} {\xi _2} = \frac{{{{\dot \theta }_2}\left( {{t_n}} \right)}}{{{{\dot \theta }_1}\left( {{t_n}} \right)}}{\xi _1} = {k_2}{\xi _1}\\ {\xi _3} = \frac{{{{\dot \theta }_3}\left( {{t_n}} \right)}}{{{{\dot \theta }_1}\left( {{t_n}} \right)}}{\xi _1} = {k_3}{\xi _1}\\ {\xi _4} = \frac{{{{\dot \theta }_4}\left( {{t_n}} \right)}}{{{{\dot \theta }_1}\left( {{t_n}} \right)}}{\xi _1} = {k_4}{\xi _1} \end{array} \right. $

(18)

式中:k₂、k₃、k₄为关节速度比例系数，在单次关节位置计算过程中保持不变，即保持恒定速比。关节增量表达式可写为

$ \Delta \mathit{\boldsymbol{ \boldsymbol{\varTheta} }} = \left[ {\begin{array}{*{20}{c}} {{\xi _1}}\\ {{k_2}{\xi _1}}\\ {{k_3}{\xi _1}}\\ {{k_4}{\xi _1}} \end{array}} \right]\Delta t $

(19)

式(16)具有意义的前提是ξ_i≠0, i=1、2、3、4和$ {{{\dot \theta }_i}}$≠0, i=1、2、3、4。当ξ_i或是$ {{{\dot \theta }_i}}$其中一个或是多个为零时，也就表示具有一个或多个关节处于锁定状态，那么系统处于非冗余状态，直接通过式(5)的约束进行求解，本文方法针对的是ξ_i和$ {{{\dot \theta }_i}}$均不为零的情况。

7) 将式(18)的增量表达式代入式(15)的方程中，将Θ作为初始，利用牛顿迭代方法即可获得ξ₁。将ξ₁代入式(19)，求得ΔΘ。

将算法总结为流程图，如图 2所示。

4 姿态稳定系统运动仿真结果 4.1 仿真条件

本文为了验证算法在运载器大动态特性下的结果，设定了一个跟踪目标，并仿真运载器的运动规律，观察关节空间的运动情况及末端位姿的误差。仿真时间间隔为ΔT=0.01 s。

对跟踪目标做如下假设：目标方位角度：223.49°，俯仰角：60°，极化角度：-28.8°。按照本文中建模规则，初始位置下目标对应的关节角度为

$ {\mathit{\boldsymbol{ \boldsymbol{\varTheta} }}_0} = {\left[ {\begin{array}{*{20}{c}} {136.51}&{ - 60}&0&{ - 28.8} \end{array}} \right]^{\rm{T}}} $

(20)

运载器的航向角、纵横摇角运动组照如下的描述为

$ \begin{array}{l} \psi = 40\sin \left[ {\left( {2{\rm{ \mathit{ π}} /30\; 000}} \right)t} \right] + 0.01t\\ \varphi = 40\sin \left[ {\left( {2{\rm{ \mathit{ π} /800}}} \right)t} \right]\\ \gamma = 45\sin \left[ {\left( {2{\rm{ \mathit{ π}} /1\;000}} \right)t} \right] \end{array} $

(21)

仿真过程利用运载器的连续运动和卫星信息，根据式(4)生产连续的矩阵^bT_s，作为冗余自由度机构跟踪的目标矩阵序列。另外，根据连续的目标矩阵序列，差分生成工作空间姿态运动速度Ẋ=[ω_x ω_y ω_z]^T。利用第3节中的算法求得稳定系统运动学逆解。最终，利用求解的关节空间角度值进行运动学正计算，将结果与期望的姿态矩阵^bT_s做差，计算差矩阵的Frobenius范数，作为算法精度的衡量指标。

4.2 伪逆方法仿真结果

按照仿真条件，分别使用恒等速比方法和加权M-P逆方法进行了稳定系统关节角、关节角速度和姿态误差的计算。

直接利用Ẋ=[ω_x ω_y ω_z]^T和M-P逆，直接计算出各关节的角速度，并对角速度直接积分获得关节角位置，用于更新Jacobian矩阵，获得的连续角速度如图 3所示。

直接利用关节角速度积分或的关节角位置，如图 4所示。

将计算得到的各关节角度带入运动学方程计算，获得末端的姿态矩阵，将此姿态矩阵与期望姿态矩阵进行做差，计算差矩阵的Frobenius范数值，如图 5所示。

可以看到单纯利用伪逆速度积分获得的末端姿态矩阵误差会快速发散。

使用恒定速比方法进行计算，计算结果为每一步的关节角更新位置，直接获得关节角位置变化曲线如图 6所示。

本文解决的重点在关节空间角位置的计算，暂时不考虑关节的限位问题。在恒定速比方法求解过程中每一步计算得到的M-P逆速度如图 7所示。

同样地根据各关节角度值进行运动学正计算，将获得姿态与期望姿态进行做差，求得误差矩阵的Frobenius范数曲线如图 8所示。由于是利用恒速比方法直接求得的位置值，关节角的速度值是通过位置差分得到的平均速度值，差分结果如图 9所示。

从图 9误差结果可以看出，利用恒定速比方法计算关节角位置，机构的末端姿态误差不累加。虽然在某些位置出现误差增加点，但整体末端姿态始终保持较高精度。同时，姿态精度取决于算法中牛顿迭代的精度，换而言之，根据末端姿态精度要求的不同，可以灵活选择牛顿迭代法的精度。

5 结论

1) 建立了具有4个运动自由度的天线稳定系统运动模型，给出了天线运动、运载器运动及目标卫星之间的关系；给出了天线姿态矩阵序列的计算方法。

2) 分析了现有的冗余自由度运动学逆解方法在轨迹跟踪方面的不足。提出了一种基于关节恒定速比的关节位置算法，给出了算法的流程图。

3) 对基于M-P逆的速度求解方法和本文提出的方法进行了仿真验证，仿真结果表明恒定速比方法得到的关节角，保证了系统末端姿态在运动过程中具有足够的精度。