本发明涉及一种冗余自由度机构位置更新方法。
背景技术：
天线伺服系统为研究对象，将其抽象为一个四自由度的运动平台。提出了一种基于关节空间运动恒定速比假设的冗余自由度机械臂关节更新方法，将研究重点放在冗余自由度机械臂姿态随动系统的关节角位置求解上，获得了基于局部速度最优的一步位置更新数值解。能够快速计算出专利中所涉及的四个自由度的姿态伺服的运动学位置逆解。对于冗余自由度的运动机构而言，常用的优化方法是基于关节空间的速度，可以得到关节局部的速度最优解，但是对于位置层面的关注较少。本专利给出了一种具有广泛适用性的冗余自由度机构的运动学逆解位置解法。
技术实现要素：
本发明的目的在于提供一种冗余自由度机构位置更新方法。
本发明的目的是这样实现的：
(1)对载体现有的姿态进行测量，获得载体此时的航向角ψ、俯仰角φ和横摇角γ；
(2)根据卫星的参数，和第(1)步的测量结果，利用下式：
计算出来在当前时刻卫星在载体坐标系下的描述矩阵Ts；式(1)中的航向角ψ、俯仰角φ和横摇角γ为第(1)步的测量值，Ee代表卫星参数的俯仰角，Ae代表卫星参数的方位角，σe代表卫星参数的极化角，C代表cos，S代表sin，例如：CEe代表cosEe，SEe代表sinEe；此时的天线系统的四个关节角为θ1，θ2，θ3，θ4；关节角度与描述矩阵Ts之间的函数关系为：Ts＝F(θ1,θ2,θ3,θ4)；
(3)当前时刻的天线框架的姿态矩阵为Ta，利用第(2)步中的卫星矩阵Ts与Ta求解出两个坐标系之间的转动微元，
其中δx为绕x轴的转动微元，δy为绕y轴的转动微元，δz为绕z轴的转动微元；为矩阵Ta的逆矩阵；将其中的速度微元提取出来组成向量[δx δy δz]T；
(4)对当前的天线系统结构计算雅克比矩阵值，记为J；
(5)求取天线系统的雅克比矩阵J的Moore-Penrose逆(简称M-P逆)，将M-P逆记为J+；
(6)求取四个关节角的运动微元：
Θ＝[dθ1 dθ2 dθ3 dθ4]T＝J+[δx δy δz]T
其中，Θ为关节运动微元向量，dθ1为第一个关节角的运动微元，dθ2为第二个关节角的运动微元，dθ3为第三个关节角的运动微元，dθ4为第四个关节角的运动微元；
(7)计算各关节角之前运动微元的比例值，其中k2为dθ2与dθ1之间的比，其中k3为dθ3与dθ1之间的比，其中k4为dθ4与dθ1之间的比；
(8)进行位置更新时每个关节的运动量记为：Δθ1，Δθ2，Δθ3，Δθ4，利用再(7)中求得的k2，k3和k4以及Δθ1表示Δθ2，Δθ3，Δθ4有：
Δθ2＝k2Δθ1
Δθ3＝k3Δθ1
Δθ4＝k4Δθ1
(9)建立位置更新后的关节角度方程为：
Ta＝F(θ1+Δθ1,θ2+k2Δθ1,θ3+k3Δθ1,θ4+k4Δθ1)
其中规则F是建立在关节角度和末端姿态之间的约束方程；这个约束关系的是通过建立冗余自由度运动机构的D-H模型得到的；这个模型给定了机构四个运动角度与末端姿态描述矩阵之间的对应关系；
(10)利用牛顿迭代方法，求解出第(9)步中的方程，得到Δθ1，再利用式(4)求得Δθ2，Δθ3，Δθ4；
(11)得到关节的更新位置为θ1+Δθ1，θ2+Δθ2，θ3+Δθ3，θ4+Δθ4，完成位置更新。
本发明的有益效果在于：
本专利的方法具有直接计算得到冗余自由度机构运动学位置逆解的有点，逆解的精度高，适用于对末端姿态有较高精度的应用场合。同时本专利的方法适合具有跟踪结构的应用，在算法进行位置更新的过程中，重点是求解目标位置状态下所对应的各关节位置值，而不用去费力规划两个状态之间的位置状态值，大大化简了运算步骤，依靠给定的一个目标位置就可以方便地进行冗余自由度机构的位置更新。
附图说明
图1为本发明流程图。
具体实施方式
下面结合附图对本发明做进一步描述。
本专利发明算法的解法的步骤：
本专利提出了一种基于最优速比和关节约束方程的关节空间关节角度值数值解法，称为恒定速比(RCC)位置更新方法。方法旨在解决现有冗余自由度机构运动速度规划方法无法约束末端姿态的问题。方法的过程是这样：
(1)对载体现有的姿态进行测量，获得载体此时的航向角ψ、俯仰角φ和横摇角γ；
(2)根据卫星的参数，和第(1)步的测量结果，利用下式：
计算出来在当前时刻卫星在载体坐标系下的描述矩阵Ts；式(1)中的航向角ψ、俯仰角φ和横摇角γ为第(1)步的测量值，Ee代表卫星参数的俯仰角，Ae代表卫星参数的方位角，σe代表卫星参数的极化角，C代表cos，S代表sin，例如：CEe代表cosEe，SEe代表sinEe。此时的天线系统的四个关节角为θ1，θ2，θ3，θ4；关节角度与描述矩阵Ts之间的函数关系为：Ts＝F(θ1,θ2,θ3,θ4)。
(3)当前时刻的天线框架的姿态矩阵为Ta，利用第(2)步中的卫星矩阵Ts与Ta求解出两个坐标系之间的转动微元，
其中δx为绕x轴的转动微元，δy为绕y轴的转动微元，δz为绕z轴的转动微元。为矩阵Ta的逆矩阵。将其中的速度微元提取出来组成向量[δx δy δz]T。
(4)对当前的天线系统结构计算雅克比矩阵值，记为J。
(5)求取天线系统的雅克比矩阵J的Moore-Penrose逆(简称M-P逆)，将M-P逆记为J+。
(6)求取四个关节角的运动微元：
Θ＝[dθ1 dθ2 dθ3 dθ4]T＝J+[δx δy δz]T (3)
其中，Θ为关节运动微元向量，dθ1为第一个关节角的运动微元，dθ2为第二个关节角的运动微元，dθ3为第三个关节角的运动微元，dθ4为第四个关节角的运动微元。
(7)计算各关节角之前运动微元的比例值，其中k2为dθ2与dθ1之间的比，其中k3为dθ3与dθ1之间的比，其中k4为dθ4与dθ1之间的比。
(8)进行位置更新时每个关节的运动量记为：Δθ1，Δθ2，Δθ3，Δθ4，利用再(7)中求得的k2，k3和k4以及Δθ1表示Δθ2，Δθ3，Δθ4有：
(9)建立位置更新后的关节角度方程为：
Ta＝F(θ1+Δθ1,θ2+k2Δθ1,θ3+k3Δθ1,θ4+k4Δθ1)
其中规则F是建立在关节角度和末端姿态之间的约束方程。这个约束关系的是通过建立冗余自由度运动机构的D-H模型得到的。这个模型给定了机构四个运动角度与末端姿态描述矩阵之间的对应关系。
(10)利用牛顿迭代方法，求解出第(9)步中的方程，得到Δθ1，再利用式(4)求得Δθ2，Δθ3，Δθ4。
(11)得到关节的更新位置为θ1+Δθ1，θ2+Δθ2，θ3+Δθ3，θ4+Δθ4，完成位置更新。
可以看出，以上分析的RCC方法，不仅适用于本专利中涉及的结构，同时也可以推广至使用转动关节描述的冗余自由度机械臂中，具有广泛的适用性。本专利的方法具有直接计算得到冗余自由度机构运动学位置逆解的有点，逆解的精度高，适用于对末端姿态有较高精度的应用场合。同时本专利的方法适合具有跟踪结构的应用，在算法进行位置更新的过程中，重点是求解目标位置状态下所对应的各关节位置值，而不用去费力规划两个状态之间的位置状态值，大大化简了运算步骤，依靠给定的一个目标位置就可以方便地进行冗余自由度机构的位置更新。