CN112405528A - 一种考虑引力影响的空间机械臂系统显式动力学控制方法 - Google Patents

Abstract

一种考虑引力影响的空间机械臂系统显式动力学控制方法，属于飞行器动力学与控制技术领域；融合了传统的基于拉格朗日方程的隐式动力学建模方法与基于牛顿欧拉法的链式动力学建模方法的优势，同时考虑引力影响，实现了空间多机械臂系统的显式通用化动力学建模；在此基础上，给出了一种考虑引力补偿的机械臂关节控制方法，能够同时实现自由漂浮模式下基座飞行器位姿的稳定和末端执行器的位置调整。

Description

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

技术领域

本发明涉及一种考虑引力影响的空间机械臂系统显式动力学控制方法，属于飞行器动力学与控制技术领域。

背景技术

空间机械臂对于空间碎片清除、在轨服务、在轨操作与装配及深空无人探测等空间任务至关重要。在执行近距离操作任务时，空间机械臂通常采用自由漂浮运动模式，即操作期间不对基座飞行器施加主动位姿控制，该方式能够节省推进剂，而且能够避免位姿控制执行机构开启时对机械臂运动的干扰。传统的自由漂浮模式空间机械臂动力学建模主要分为两类：基于拉格朗日方程的隐式动力学建模与基于牛顿欧拉法的链式动力学建模，前者形式简单，系统矩阵具备反对称性质，但表达形式涉及大量偏导数计算，需要依据具体机械臂构型进行求解表达，不适用于较高自由度的机械臂系统；后者利用空间算子方法能够实现机械臂系统的显式建模，但其系统方程无法从整体上把握基座和机械臂之间的动力学关系，且不具备反对称性质，不利于控制设计。

此外，自由漂浮模式下的空间机械臂与基座飞行器存在较强的动力学耦合关系，基座飞行器的初始位姿速度会影响机械臂运动，反之，机械臂运动也会引起基座飞行器的位姿变化。有鉴于此，利用多机械臂系统存在的冗余自由度通过耦合作用实现基座飞行器的位姿稳定，确保装载于飞行器的有效载荷正常工作，对于机械臂末端完成操作任务具有十分重要的意义。

进一步，传统的空间动力学建模与分析往往忽视引力影响，认为其在无重力环境下进行运动及操作。这种假设对于短时间操作，及小型机械臂而言，引力影响较小，可以忽略。但是，对于大型机械臂的运动及操作，特别对于含有高精度定位要求的舱段搬运、对接及等长时间操作，引力影响不容忽视。同时，引力影响打破了动量守恒定律，传统的基于动量守恒的控制方法不再适用，为自由漂浮模式下空间机械臂的动力学建模与控制提出了挑战。需要开展含引力影响的空间机械臂建模及主动补偿控制方法研究。

发明内容

本发明要解决的技术问题是：克服现有技术的不足，提供了一种考虑引力影响的空间机械臂系统显式动力学控制方法，融合了传统的基于拉格朗日方程的隐式动力学建模方法与基于牛顿欧拉法的链式动力学建模方法的优势，同时考虑引力影响，实现了空间多机械臂系统的显式通用化动力学建模；在此基础上，给出了一种考虑引力补偿的机械臂关节控制方法，能够同时实现自由漂浮模式下基座飞行器位姿的稳定和末端执行器的位置调整。

本发明目的通过以下技术方案予以实现：

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

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。

上述考虑引力影响的空间机械臂系统显式动力学控制方法，优选的，空间机械臂系统包括多部机械臂，每部机械臂含有多个转动关节；对于每部机械臂，转动关节和转动关节之间均由臂杆连接，且转动关节通过臂杆连接末端执行器。

上述考虑引力影响的空间机械臂系统显式动力学控制方法，优选的，S4中，当耦合控制矩阵为方阵时，利用修正系数进行修正；当耦合控制矩阵为非方阵时，对耦合控制矩阵进行分块后利用修正系数进行修正。

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

上述考虑引力影响的空间机械臂系统显式动力学控制方法，优选的，S1中，所述基座飞行器总体参数包括基座飞行器质量m₀和基座飞行器转动惯量I₀、机械臂数目N；对于第i部机械臂，关节数为n_i、第k个臂杆质量

第k个关节

至第k+1个臂杆质心C_k的位置矢量

第k个臂杆质心C_k至第k+1个关节

的位置矢量

第k个臂杆转动惯量

关节1的安装位置矢量

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

初始关节角度矢量Θ(t₀)。

上述考虑引力影响的空间机械臂系统显式动力学控制方法，优选的，S2中，利用当前时刻(t_j时刻)每部机械臂的关节角、关节方向矢量和臂杆长度、基座飞行器和每部机械臂臂杆的质量及转动惯量，计算t_j时刻每部机械臂末端运动学系统矩阵和机械臂末端运动学集总系统矩阵；计算t_j时刻基座飞行器动力学系统矩阵；计算t_j时刻每部机械臂动力学系统矩阵和集总系统矩阵。

上述考虑引力影响的空间机械臂系统显式动力学控制方法，优选的，S2中，利用当前时刻(t_j时刻)每部机械臂的关节角、关节方向矢量和臂杆长度、基座飞行器和每部机械臂臂杆的质量及转动惯量、地球引力常数、基座飞行器期望位置矢量、基座飞行器质心至每部机械臂臂杆的位置矢量，计算当前时刻(t_j时刻)的引力补偿量。

上述考虑引力影响的空间机械臂系统显式动力学控制方法，优选的，S2中，利用位姿增益矩阵，当前时刻(t_j时刻)基座飞行器的姿态、位置、速度和角速度，计算当前时刻(t_j时刻)的虚拟速度控制及其导数。

上述考虑引力影响的空间机械臂系统显式动力学控制方法，优选的，S2中，利用当前时刻(t_j时刻)的机械臂末端与基座飞行器集总运动耦合矩阵和机械臂集总运动学矩阵，计算当前时刻(t_j时刻)的耦合控制矩阵。

上述考虑引力影响的空间机械臂系统显式动力学控制方法，优选的，S5中，利用末端位置增益矩阵和基座飞行器增益矩阵，当前时刻(t_j时刻)的机械臂末端位置与速度矢量，机械臂末端期望位置与速度矢量，虚拟速度控制及其导数，机械臂末端与基座飞行器集总运动耦合矩阵，机械臂集总运动学矩阵，基座飞行器动力学系统矩阵，引力补偿量，耦合控制矩阵，计算当前时刻(t_j时刻)的机械臂指令关节加速度矢量和机械臂指令关节角速度矢量。

上述考虑引力影响的空间机械臂系统显式动力学控制方法，优选的，S6中，利用机械臂增益矩阵，当前时刻(t_j时刻)的机械臂末端位置矢量，机械臂末端期望位置矢量，机械臂指令关节加速度矢量与指令关节角速度矢量，基座飞行器速度与角速度矢量，机械臂末端运动学集总系统矩阵，集总系统矩阵，虚拟速度控制，引力补偿量，计算当前时刻(t_j时刻)的机械臂关节力矩。

本发明相比于现有技术具有如下有益效果：

(1)本发明方法采用迭代建模思路，引入两个矢量分解技术，在欧拉-拉格朗日框架下实现了显式形式的空间多机械臂动力学建模，基于该建模方法获得的系统矩阵具有通用化、解析等特点，同时系统参数矩阵具备反对称性质，易于控制算法设计。

(2)本发明方法给出了自由漂浮模式下航天器本体引力补偿的二阶滤波器设计方法，基于自由漂浮模式下航天器本体子动力学模型，构造一类二阶滤波器，不仅能够实现航天器位姿加速度的有效估计，避免噪声引入，而且能够构建机械臂运动与航天器运动的耦合关系，为引力补偿及本体位姿稳定提供控制基础。

(3)本发明方法给出了基座飞行器位姿稳定与机械臂末端位置调整协调运动的控制方法，能够有效补偿引力影响，同时实现机械臂末端位置调整，以及调整过程中基座飞行器位姿稳定。

(4)本发明方法给出的协调控制方法充分利用多机械臂系统冗余自由度实现控制，避免额外使用推进剂，能够有效提升在轨飞行器的使用寿命。

附图说明

图1为本发明的空间多机械臂系统建模示意图；

图2为本发明方法的工作流程图。

具体实施方式

为使本发明的目的、技术方案和优点更加清楚，下面将结合附图对本发明的实施方式作进一步详细描述。

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

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。

作为本发明的一种优选方案，空间机械臂系统包括多部机械臂，每部机械臂含有多个转动关节；对于每部机械臂，转动关节和转动关节之间均由臂杆连接，且转动关节通过臂杆连接末端执行器。

作为本发明的一种优选方案，S4中，当耦合控制矩阵为方阵时，利用修正系数进行修正；当耦合控制矩阵为非方阵时，对耦合控制矩阵进行分块后利用修正系数进行修正。

作为本发明的一种优选方案，S1中，所述任务参数包括基座飞行器期望位置矢量、期望姿态四元数、第i部机械臂期望末端位置和期望速度。

作为本发明的一种优选方案，S1中，所述基座飞行器总体参数包括基座飞行器质量m₀和基座飞行器转动惯量I₀、机械臂数目N；对于第i部机械臂，关节数为n_i、第k个臂杆质量

第k个关节

至第k+1个臂杆质心C_k的位置矢量

第k个臂杆质心C_k至第k+1个关节

的位置矢量

第k个臂杆转动惯量

关节1的安装位置矢量

作为本发明的一种优选方案，S1中，所述t₀时刻基座飞行器运动参数包括获取地心至基座飞行器质心的矢量r_E0(t₀)、基座飞行器初始位置偏差r₀(t₀)、基座飞行器初始线速度偏差矢量v₀(t₀)、基座飞行器初始姿态偏差四元数q(t₀)、初始角速度偏差矢量ω₀(t₀)、机械臂各关节初始角速度矢量

初始关节角度矢量Θ(t₀)。

作为本发明的一种优选方案，S2中，利用当前时刻(t_j时刻)每部机械臂的关节角、关节方向矢量和臂杆长度，计算t_j时刻每部机械臂末端运动学系统矩阵和机械臂末端运动学集总系统矩阵；利用当前时刻(t_j时刻)每部机械臂的关节角、关节方向矢量和臂杆长度、基座飞行器和每部机械臂臂杆的质量及转动惯量，计算t_j时刻基座飞行器动力学系统矩阵；利用当前时刻(t_j时刻)每部机械臂的关节角、关节方向矢量和臂杆长度、基座飞行器和每部机械臂臂杆的质量及转动惯量，计算t_j时刻每部机械臂动力学系统矩阵和集总系统矩阵；利用当前时刻(t_j时刻)每部机械臂的关节角、关节方向矢量和臂杆长度、基座飞行器和每部机械臂臂杆的质量及转动惯量、地球引力常数、基座飞行器期望位置矢量、基座飞行器质心至每部机械臂臂杆的位置矢量，计算当前时刻(t_j时刻)的引力补偿量；利用位姿增益矩阵，当前时刻(t_j时刻)基座飞行器的姿态、位置、速度和角速度，计算当前时刻(t_j时刻)的虚拟速度控制及其导数；利用位姿增益矩阵，当前时刻(t_j时刻)基座飞行器的姿态、位置、速度和角速度，计算当前时刻(t_j时刻)的虚拟速度控制及其导数；利用当前时刻(t_j时刻)的机械臂末端与基座飞行器集总运动耦合矩阵和机械臂集总运动学矩阵，计算当前时刻(t_j时刻)的耦合控制矩阵。

作为本发明的一种优选方案，S5中，利用末端位置增益矩阵和基座飞行器增益矩阵，当前时刻(t_j时刻)的机械臂末端位置与速度矢量，机械臂末端期望位置与速度矢量，虚拟速度控制及其导数，机械臂末端与基座飞行器集总运动耦合矩阵，机械臂集总运动学矩阵，基座飞行器动力学系统矩阵，引力补偿量，耦合控制矩阵，计算当前时刻(t_j时刻)的机械臂指令关节加速度矢量和机械臂指令关节角速度矢量。

作为本发明的一种优选方案，S6中，利用机械臂增益矩阵，当前时刻(t_j时刻)的机械臂末端位置矢量，机械臂末端期望位置矢量，机械臂指令关节加速度矢量与指令关节角速度矢量，基座飞行器速度与角速度矢量，机械臂末端运动学集总系统矩阵，集总系统矩阵，虚拟速度控制，引力补偿量，计算当前时刻(t_j时刻)的机械臂关节力矩。

实施例：

本发明可应用于空间近距离操作任务中多机械臂飞行器系统，解决其受引力影响下的基座飞行器位姿稳定及机械臂末端位置调整的控制问题。结合图1，基座飞行器的质心在C₀，基座飞行器携带N部机械臂，每部机械臂含有n_i(i＝1,2,…,N)个转动关节，均通过第一关节

安装在飞行器的外表面，安装的位置矢量为

图1以第i部机械臂作为示意，该部机械臂具有n_i个转动关节，

每两个转动关节之间由刚性杆连接，转动关节

通过刚性杆

连接末端执行器

下面结合附图对本发明作进一步详细说明：

1)基座飞行器位姿运动学模型

航天器相对于惯性系的位姿运动学方程为：

其中，q为基座飞行器的姿态偏差，以四元数形式表征，q_v为矢量部分，q₀为标量部分，ω₀为基座飞行器的角速度偏差，r₀为基座飞行器位置偏差，v₀为基座飞行器速度偏差，E为单位矩阵，

为矢量q_v的反对称矩阵。

2)机械臂末端运动学

第i部机械臂末端的运动学方程为：

其中，

为第i部机械臂的末端位置，

为第i部机械臂的末端速度，V₀为基座飞行器的广义速度，表征为

为第i部机械臂的关节角速度矢量，

和

分别为第i部机械臂末端与基座飞行器运动耦合矩阵和第i部机械臂运动学矩阵，表征为：

其中，

为第i部机械臂第p个臂杆至第q个臂杆的相对位置矢量，表述为

为第i部机械臂第k个关节

至第k个臂杆质心的位置矢量，

为第i部机械臂第k个臂杆质心关节

至第k+1个关节

的位置矢量，

为第i部机械臂第k个关节的方向矢量；

为矢量

的反对称矩阵。

因此，集总形式的机械臂末端运动学方程为：

其中，p_e和v_e分别为机械臂末端位置矢量和速度矢量，表征为：

和

分别为机械臂末端与基座飞行器集总运动耦合矩阵和机械臂集总运动学矩阵，表征为：

3)空间多机械臂的显式动力学模型

含有N部机械臂的飞行器动力学方程为：

其中，

为基座飞行器质心至第i部机械臂第k个臂杆质心的相对位置矢量，

为第i部机械臂的关节控制矢量，H_bb为基座飞行器惯性矩阵，

为基座飞行器与第i部机械臂的惯性耦合矩阵，

为第i部机械臂惯性矩阵，

为基座飞行器科氏矩阵，

为基座飞行器与第i部机械臂的科氏耦合矩阵，

为第i部机械臂与基座飞行器的科氏耦合矩阵，

为第i部机械臂科氏耦合矩阵，上述矩阵的显式形式分别表征为：

其中，M₀为基座飞行器质量特性矩阵，Mⁱ为第i部机械臂的质量特性矩阵，Gⁱ为第i部机械臂的构型矩阵，

为第i部机械臂与基座的构型耦合矩阵，

为第i部机械臂的构型导数矩阵，

为第i部机械臂的构型对角导数矩阵，Hⁱ为第i部机械臂的关节方向对角矩阵，

为第i部机械臂的关节方向矩阵，上述矩阵及矩阵

和Bⁱ分别表征为：

其中，针对任一矢量ζ，矩阵算子G(ζ)定义为

另外，

为第i部机械臂的第k个臂杆的转动惯量，

为第i部机械臂的第k个臂杆的质量；I₀为基座飞行器的转动惯量；m₀为基座飞行器质量；

定义为：

为关节

角速度矢量的反对称矩阵，F_g0为引力对基座飞行器的广义干扰矢量，表征为：

为引力对第i部机械臂的第k个臂杆的广义干扰矢量，表征为：

其中，μ为地球引力常数，

为地心至第i部机械臂的第k个臂杆的位置矢量，r_EO为基座飞行器期望位置矢量，r_E0为地心至基座飞行器的相对位置矢量，

为基座飞行器质心至第i部机械臂的第k个臂杆的位置矢量，

为矢量

导数的反对称矩阵；R为轨道半径。

因此，集总形式下含有N部机械臂的飞行器动力学方程为：

其中，

为基座飞行器质心至第i部机械臂的第k个臂杆质心的位置矢量，τ_c为机械臂关节力矩矢量；H_bm为基座飞行器与机械臂的集总惯性耦合矩阵，H_mm为机械臂集总惯性矩阵，C_bb为基座飞行器集总科氏矩阵，C_bm为基座飞行器与机械臂的集总科氏耦合矩阵，C_mb为机械臂与基座飞行器的集总科氏耦合矩阵，C_mm为机械臂集总科氏耦合矩阵，τ_gm为引力对集总机械臂干扰矢量，τ_gm与τ_g相等，上述矩阵与矢量表征为：

其中，

和B₀定义为：

其中，

为基座飞行器角速度的反对称矩阵。

4)如图2所示，一种考虑引力影响的空间机械臂系统显式动力学控制方法，包括以下步骤：

(1)获取任务参数，包括基座飞行器期望位置矢量r_EO，期望姿态四元数q_d，第i部机械臂期望末端位置为

与期望速度

(2)获取基座飞行器总体参数，包括基座飞行器质量m₀和基座飞行器转动惯量I₀、机械臂数目N，其中，第i部机械臂的关节数为n_i、其第k个臂杆质量

其第k个关节

至第k+1个臂杆质心C_k的位置矢量

其第k个臂杆质心

至第k+1个关节

的位置矢量

其第k个臂杆转动惯量

其关节1的安装位置矢量

(3)获取初始时刻(t₀时刻)基座飞行器运动参数，包括获取地心至基座飞行器质心的矢量r_E0(t₀)，基座飞行器初始位置偏差r₀(t₀)、基座飞行器初始线速度偏差矢量v₀(t₀)、基座飞行器初始姿态偏差四元数q(t₀)和初始角速度偏差矢量ω₀(t₀)、及机械臂各关节初始角速度矢量

及初始关节角度矢量Θ(t₀)。

(4)按下述公式计算当前时刻(t_j时刻，j＝0,1,2,…)第i部机械臂末端运动学系统矩阵，包括第i部机械臂末端与基座飞行器运动耦合矩阵

和第i部机械臂运动学矩阵

其中，

为第i部机械臂第k个关节的方向矢量。

(5)按下述公式计算当前时刻(t_j时刻，j＝0,1,2,…)机械臂末端运动学集总系统矩阵，包括机械臂末端与基座飞行器集总运动耦合矩阵H_eb和机械臂集总运动学矩阵H_em：

(6)按下述公式计算当前时刻(t_j时刻，j＝0,1,2,…)基座飞行器动力学系统矩阵，包括基座飞行器惯性矩阵H_bb，基座飞行器与第i部机械臂的惯性耦合矩阵

基座飞行器科氏矩阵

基座飞行器与第i部机械臂的科氏耦合矩阵

其中，M₀为基座飞行器质量特性矩阵，Mⁱ为第i部机械臂的质量特性矩阵，Gⁱ为第i部机械臂的构型矩阵，

为第i部机械臂与基座的构型耦合矩阵，Gⁱ为第i部机械臂的构型矩阵，

为第i部机械臂的构型导数矩阵，

为第i部机械臂的构型对角导数矩阵，Hⁱ为第i部机械臂的关节方向对角矩阵，

为第i部机械臂的关节方向矩阵,矩阵

和Bⁱ分别表征为：

其中，

(7)按下述计算当前时刻(t_j时刻，j＝0,1,2,…)第i部机械臂动力学系统矩阵，包括第i部机械臂惯性矩阵

第i部机械臂与基座飞行器的科氏耦合矩阵

第i部机械臂科氏耦合矩阵

(8)计算当前时刻(t_j时刻，j＝0,1,2,…)集总系统矩阵，包括基座飞行器与机械臂的集总惯性耦合矩阵H_bm，机械臂集总惯性矩阵H_mm，基座飞行器集总科氏矩阵C_bb，基座飞行器与机械臂的集总科氏耦合矩阵C_bm，机械臂与基座飞行器的集总科氏耦合矩阵C_mb，机械臂集总科氏耦合矩阵C_mm：

其中，

(9)按下式计算当前时刻(t_j时刻，j＝0,1,2,…)的引力补偿量，包括基座飞行器级引力补偿矢量

和机械臂级引力补偿矢量τ_gm。

其中，

(10)按下述公式计算当前时刻(t_j时刻，j＝0,1,2,…)的虚拟速度控制V_c及其导数

其中，ω_c为虚拟角速度控制矢量，v_c为虚拟速度控制矢量，K_x为位姿增益矩阵，而且，

其中

q_s＝[1 0 0 0]^T

(11)按下述公式计算当前时刻(t_j时刻，j＝0,1,2,…)的耦合控制矩阵H_c：

并判断其是否满足下述秩条件，

若满足，则进入步骤(13)，否则进入步骤(12)。

(12)判断：若H_c为方阵，那么按下式修正H_c：

H_c＝H_c+εE

其中，ε为修正系数，可取为0.001，若H_c为非方阵，则对其进行分块：

H_c＝[H_cl H_cr]

并确保H_cl为方阵，随后对其按下式进行修正：

H_cl＝H_cl+εE

并返回步骤(11)：

(13)按下述公式计算当前时刻(t_j时刻，j＝0,1,2,…)的机械臂指令关节角加速度矢量

并计算机械臂指令关节角速度矢量

其中，ξ为任一矢量，

为矩阵H_c的广义逆，ζ表征为：

其中，K_e为末端位置增益矩阵，K_b为基座飞行器增益矩阵，v_e为机械臂末端速度矢量，p_d为机械臂末端期望位置矢量，p_e为机械臂末端位置矢量。

(14)按下述公式计算并输出当前时刻(t_j时刻，j＝0,1,2,…)的机械臂关节力矩τ_c，实施控制，待下一时刻，返回步骤(4)开始下一时刻的控制量计算，直至控制结束。

其中，K_m为机械臂增益矩阵。

本发明说明书中未作详细描述的内容属本领域技术人员的公知技术。

本发明虽然已以较佳实施例公开如上，但其并不是用来限定本发明，任何本领域技术人员在不脱离本发明的精神和范围内，都可以利用上述揭示的方法和技术内容对本发明技术方案做出可能的变动和修改，因此，凡是未脱离本发明技术方案的内容，依据本发明的技术实质对以上实施例所作的任何简单修改、等同变化及修饰，均属于本发明技术方案的保护范围。

Claims (12)

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时刻)的引力补偿量。

9.根据权利要求1所述的一种考虑引力影响的空间机械臂系统显式动力学控制方法，S2中，利用位姿增益矩阵，当前时刻(t_j时刻)基座飞行器的姿态、位置、速度和角速度，计算当前时刻(t_j时刻)的虚拟速度控制及其导数。

10.根据权利要求1所述的一种考虑引力影响的空间机械臂系统显式动力学控制方法，S2中，利用当前时刻(t_j时刻)的机械臂末端与基座飞行器集总运动耦合矩阵和机械臂集总运动学矩阵，计算当前时刻(t_j时刻)的耦合控制矩阵。

11.根据权利要求1～10之一所述的一种考虑引力影响的空间机械臂系统显式动力学控制方法，S5中，利用末端位置增益矩阵和基座飞行器增益矩阵，当前时刻(t_j时刻)的机械臂末端位置与速度矢量，机械臂末端期望位置与速度矢量，虚拟速度控制及其导数，机械臂末端与基座飞行器集总运动耦合矩阵，机械臂集总运动学矩阵，基座飞行器动力学系统矩阵，引力补偿量，耦合控制矩阵，计算当前时刻(t_j时刻)的机械臂指令关节加速度矢量和机械臂指令关节角速度矢量。

12.根据权利要求1～10之一所述的一种考虑引力影响的空间机械臂系统显式动力学控制方法，S6中，利用机械臂增益矩阵，当前时刻(t_j时刻)的机械臂末端位置矢量，机械臂末端期望位置矢量，机械臂指令关节加速度矢量与指令关节角速度矢量，基座飞行器速度与角速度矢量，机械臂末端运动学集总系统矩阵，集总系统矩阵，虚拟速度控制，引力补偿量，计算当前时刻(t_j时刻)的机械臂关节力矩。

Images