[发明专利]一种考虑引力影响的空间机械臂系统显式动力学控制方法

权利要求书

1.一种考虑引力影响的空间机械臂系统显式动力学控制方法，其特征在于，包括如下步骤：

S1、获取任务参数、基座飞行器总体参数、初始时刻基座飞行器运动参数，初始时刻记为t₀；

S2、计算当前时刻每部机械臂末端运动学系统矩阵，当前时刻记为t_j，计算t_j时刻机械臂末端运动学集总系统矩阵、基座飞行器动力学系统矩阵，计算t_j时刻每部机械臂动力学系统矩阵，计算t_j时刻的集总系统矩阵、引力补偿量、虚拟速度控制、虚拟速度控制导数、耦合控制矩阵；

S3、判断耦合控制矩阵是否满足秩条件，若满足，则转入S5，否则转入S4；

S4、修正t_j时刻的耦合控制矩阵，然后返回S3；

S5、计算t_j时刻的机械臂指令关节角加速度矢量、机械臂指令关节角速度矢量；

S6、计算并输出t_j时刻的机械臂关节力矩，用于实施控制，待下一时刻，返回S2。

2.根据权利要求1所述的一种考虑引力影响的空间机械臂系统显式动力学控制方法，其特征在于，空间机械臂系统包括多部机械臂，每部机械臂含有多个转动关节；对于每部机械臂，转动关节和转动关节之间均由臂杆连接，且转动关节通过臂杆连接末端执行器。

3.根据权利要求1所述的一种考虑引力影响的空间机械臂系统显式动力学控制方法，其特征在于，S4中，当耦合控制矩阵为方阵时，利用修正系数进行修正；当耦合控制矩阵为非方阵时，对耦合控制矩阵进行分块后利用修正系数进行修正。

4.根据权利要求1所述的一种考虑引力影响的空间机械臂系统显式动力学控制方法，S1中，所述任务参数包括基座飞行器期望位置矢量、期望姿态四元数、第i部机械臂期望末端位置和期望速度。

5.根据权利要求1所述的一种考虑引力影响的空间机械臂系统显式动力学控制方法，S1中，所述基座飞行器总体参数包括基座飞行器质量m₀和基座飞行器转动惯量I₀、机械臂数目N；对于第i部机械臂，关节数为n_i、第k个臂杆质量第k个关节至第k+1个臂杆质心C_k的位置矢量第k个臂杆质心C_k至第k+1个关节的位置矢量第k个臂杆转动惯量关节1的安装位置矢量

6.根据权利要求1所述的一种考虑引力影响的空间机械臂系统显式动力学控制方法，S1中，所述t₀时刻基座飞行器运动参数包括获取地心至基座飞行器质心的矢量r_E0(t₀)、基座飞行器初始位置偏差r₀(t₀)、基座飞行器初始线速度偏差矢量v₀(t₀)、基座飞行器初始姿态偏差四元数q(t₀)、初始角速度偏差矢量ω₀(t₀)、机械臂各关节初始角速度矢量初始关节角度矢量Θ(t₀)。

7.根据权利要求1所述的一种考虑引力影响的空间机械臂系统显式动力学控制方法，S2中，利用t_j时刻每部机械臂的关节角、关节方向矢量和臂杆长度、基座飞行器和每部机械臂臂杆的质量及转动惯量，计算t_j时刻每部机械臂末端运动学系统矩阵和机械臂末端运动学集总系统矩阵；计算t_j时刻基座飞行器动力学系统矩阵；计算t_j时刻每部机械臂动力学系统矩阵和集总系统矩阵。

8.根据权利要求1所述的一种考虑引力影响的空间机械臂系统显式动力学控制方法，S2中，利用t_j时刻每部机械臂的关节角、关节方向矢量和臂杆长度、基座飞行器和每部机械臂臂杆的质量及转动惯量、地球引力常数、基座飞行器期望位置矢量、基座飞行器质心至每部机械臂臂杆的位置矢量，计算t_j时刻的引力补偿量。