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

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

Info

Publication number
CN112405528B
CN112405528B CN202011181309.7A CN202011181309A CN112405528B CN 112405528 B CN112405528 B CN 112405528B CN 202011181309 A CN202011181309 A CN 202011181309A CN 112405528 B CN112405528 B CN 112405528B
Authority
CN
China
Prior art keywords
matrix
mechanical arm
vector
joint
base aircraft
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Active
Application number
CN202011181309.7A
Other languages
English (en)
Other versions
CN112405528A (zh
Inventor
张烽
焉宁
李扬
高朝辉
吴胜宝
唐琼
姚星合
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
China Academy of Launch Vehicle Technology CALT
Original Assignee
China Academy of Launch Vehicle Technology CALT
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by China Academy of Launch Vehicle Technology CALT filed Critical China Academy of Launch Vehicle Technology CALT
Priority to CN202011181309.7A priority Critical patent/CN112405528B/zh
Publication of CN112405528A publication Critical patent/CN112405528A/zh
Application granted granted Critical
Publication of CN112405528B publication Critical patent/CN112405528B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
    • B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
    • B25J9/00—Programme-controlled manipulators
    • B25J9/16—Programme controls
    • B25J9/1602—Programme controls characterised by the control system, structure, architecture
    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
    • B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
    • B25J9/00—Programme-controlled manipulators
    • B25J9/16—Programme controls
    • B25J9/1628—Programme controls characterised by the control loop
    • B25J9/1643—Programme controls characterised by the control loop redundant control

Landscapes

  • Engineering & Computer Science (AREA)
  • Robotics (AREA)
  • Mechanical Engineering (AREA)
  • Automation & Control Theory (AREA)
  • Numerical Control (AREA)

Abstract

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

Description

一种考虑引力影响的空间机械臂系统显式动力学控制方法
技术领域
本发明涉及一种考虑引力影响的空间机械臂系统显式动力学控制方法，属于飞行器动力学与控制技术领域。
背景技术
空间机械臂对于空间碎片清除、在轨服务、在轨操作与装配及深空无人探测等空间任务至关重要。在执行近距离操作任务时，空间机械臂通常采用自由漂浮运动模式，即操作期间不对基座飞行器施加主动位姿控制，该方式能够节省推进剂，而且能够避免位姿控制执行机构开启时对机械臂运动的干扰。传统的自由漂浮模式空间机械臂动力学建模主要分为两类：基于拉格朗日方程的隐式动力学建模与基于牛顿欧拉法的链式动力学建模，前者形式简单，系统矩阵具备反对称性质，但表达形式涉及大量偏导数计算，需要依据具体机械臂构型进行求解表达，不适用于较高自由度的机械臂系统；后者利用空间算子方法能够实现机械臂系统的显式建模，但其系统方程无法从整体上把握基座和机械臂之间的动力学关系，且不具备反对称性质，不利于控制设计。
此外，自由漂浮模式下的空间机械臂与基座飞行器存在较强的动力学耦合关系，基座飞行器的初始位姿速度会影响机械臂运动，反之，机械臂运动也会引起基座飞行器的位姿变化。有鉴于此，利用多机械臂系统存在的冗余自由度通过耦合作用实现基座飞行器的位姿稳定，确保装载于飞行器的有效载荷正常工作，对于机械臂末端完成操作任务具有十分重要的意义。
进一步，传统的空间动力学建模与分析往往忽视引力影响，认为其在无重力环境下进行运动及操作。这种假设对于短时间操作，及小型机械臂而言，引力影响较小，可以忽略。但是，对于大型机械臂的运动及操作，特别对于含有高精度定位要求的舱段搬运、对接及等长时间操作，引力影响不容忽视。同时，引力影响打破了动量守恒定律，传统的基于动量守恒的控制方法不再适用，为自由漂浮模式下空间机械臂的动力学建模与控制提出了挑战。需要开展含引力影响的空间机械臂建模及主动补偿控制方法研究。
发明内容
本发明要解决的技术问题是：克服现有技术的不足，提供了一种考虑引力影响的空间机械臂系统显式动力学控制方法，融合了传统的基于拉格朗日方程的隐式动力学建模方法与基于牛顿欧拉法的链式动力学建模方法的优势，同时考虑引力影响，实现了空间多机械臂系统的显式通用化动力学建模；在此基础上，给出了一种考虑引力补偿的机械臂关节控制方法，能够同时实现自由漂浮模式下基座飞行器位姿的稳定和末端执行器的位置调整。
本发明目的通过以下技术方案予以实现：
一种考虑引力影响的空间机械臂系统显式动力学控制方法，包括如下步骤：
S1、获取任务参数、基座飞行器总体参数、初始时刻(t0时刻)基座飞行器运动参数；
S2、计算当前时刻(tj时刻)每部机械臂末端运动学系统矩阵，计算tj时刻机械臂末端运动学集总系统矩阵、基座飞行器动力学系统矩阵，计算tj时刻每部机械臂动力学系统矩阵，计算tj时刻的集总系统矩阵、引力补偿量、虚拟速度控制、虚拟速度控制导数、耦合控制矩阵；
S3、判断耦合控制矩阵是否满足秩条件，若满足，则转入S5，否则转入S4；
S4、修正tj时刻的耦合控制矩阵，然后返回S3；
S5、计算tj时刻的机械臂指令关节角加速度矢量、机械臂指令关节角速度矢量；
S6、计算并输出tj时刻的机械臂关节力矩，用于实施控制，待下一时刻，返回S2。
上述考虑引力影响的空间机械臂系统显式动力学控制方法，优选的，空间机械臂系统包括多部机械臂，每部机械臂含有多个转动关节；对于每部机械臂，转动关节和转动关节之间均由臂杆连接，且转动关节通过臂杆连接末端执行器。
上述考虑引力影响的空间机械臂系统显式动力学控制方法，优选的，S4中，当耦合控制矩阵为方阵时，利用修正系数进行修正；当耦合控制矩阵为非方阵时，对耦合控制矩阵进行分块后利用修正系数进行修正。
上述考虑引力影响的空间机械臂系统显式动力学控制方法，优选的，S1中，所述任务参数包括基座飞行器期望位置矢量、期望姿态四元数、第i部机械臂期望末端位置和期望速度。
上述考虑引力影响的空间机械臂系统显式动力学控制方法，优选的，S1中，所述基座飞行器总体参数包括基座飞行器质量m0和基座飞行器转动惯量I0、机械臂数目N；对于第i部机械臂，关节数为ni、第k个臂杆质量
Figure BDA0002750270460000031
第k个关节
Figure BDA0002750270460000032
至第k+1个臂杆质心Ck的位置矢量
Figure BDA0002750270460000033
第k个臂杆质心Ck至第k+1个关节
Figure BDA0002750270460000034
的位置矢量
Figure BDA0002750270460000035
第k个臂杆转动惯量
Figure BDA0002750270460000036
关节1的安装位置矢量
Figure BDA0002750270460000037
上述考虑引力影响的空间机械臂系统显式动力学控制方法，优选的，S1中，所述t0时刻基座飞行器运动参数包括获取地心至基座飞行器质心的矢量rE0(t0)、基座飞行器初始位置偏差r0(t0)、基座飞行器初始线速度偏差矢量v0(t0)、基座飞行器初始姿态偏差四元数q(t0)、初始角速度偏差矢量ω0(t0)、机械臂各关节初始角速度矢量
Figure BDA0002750270460000038
初始关节角度矢量Θ(t0)。
上述考虑引力影响的空间机械臂系统显式动力学控制方法，优选的，S2中，利用当前时刻(tj时刻)每部机械臂的关节角、关节方向矢量和臂杆长度、基座飞行器和每部机械臂臂杆的质量及转动惯量，计算tj时刻每部机械臂末端运动学系统矩阵和机械臂末端运动学集总系统矩阵；计算tj时刻基座飞行器动力学系统矩阵；计算tj时刻每部机械臂动力学系统矩阵和集总系统矩阵。
上述考虑引力影响的空间机械臂系统显式动力学控制方法，优选的，S2中，利用当前时刻(tj时刻)每部机械臂的关节角、关节方向矢量和臂杆长度、基座飞行器和每部机械臂臂杆的质量及转动惯量、地球引力常数、基座飞行器期望位置矢量、基座飞行器质心至每部机械臂臂杆的位置矢量，计算当前时刻(tj时刻)的引力补偿量。
上述考虑引力影响的空间机械臂系统显式动力学控制方法，优选的，S2中，利用位姿增益矩阵，当前时刻(tj时刻)基座飞行器的姿态、位置、速度和角速度，计算当前时刻(tj时刻)的虚拟速度控制及其导数。
上述考虑引力影响的空间机械臂系统显式动力学控制方法，优选的，S2中，利用当前时刻(tj时刻)的机械臂末端与基座飞行器集总运动耦合矩阵和机械臂集总运动学矩阵，计算当前时刻(tj时刻)的耦合控制矩阵。
上述考虑引力影响的空间机械臂系统显式动力学控制方法，优选的，S5中，利用末端位置增益矩阵和基座飞行器增益矩阵，当前时刻(tj时刻)的机械臂末端位置与速度矢量，机械臂末端期望位置与速度矢量，虚拟速度控制及其导数，机械臂末端与基座飞行器集总运动耦合矩阵，机械臂集总运动学矩阵，基座飞行器动力学系统矩阵，引力补偿量，耦合控制矩阵，计算当前时刻(tj时刻)的机械臂指令关节加速度矢量和机械臂指令关节角速度矢量。
上述考虑引力影响的空间机械臂系统显式动力学控制方法，优选的，S6中，利用机械臂增益矩阵，当前时刻(tj时刻)的机械臂末端位置矢量，机械臂末端期望位置矢量，机械臂指令关节加速度矢量与指令关节角速度矢量，基座飞行器速度与角速度矢量，机械臂末端运动学集总系统矩阵，集总系统矩阵，虚拟速度控制，引力补偿量，计算当前时刻(tj时刻)的机械臂关节力矩。
本发明相比于现有技术具有如下有益效果：
(1)本发明方法采用迭代建模思路，引入两个矢量分解技术，在欧拉-拉格朗日框架下实现了显式形式的空间多机械臂动力学建模，基于该建模方法获得的系统矩阵具有通用化、解析等特点，同时系统参数矩阵具备反对称性质，易于控制算法设计。
(2)本发明方法给出了自由漂浮模式下航天器本体引力补偿的二阶滤波器设计方法，基于自由漂浮模式下航天器本体子动力学模型，构造一类二阶滤波器，不仅能够实现航天器位姿加速度的有效估计，避免噪声引入，而且能够构建机械臂运动与航天器运动的耦合关系，为引力补偿及本体位姿稳定提供控制基础。
(3)本发明方法给出了基座飞行器位姿稳定与机械臂末端位置调整协调运动的控制方法，能够有效补偿引力影响，同时实现机械臂末端位置调整，以及调整过程中基座飞行器位姿稳定。
(4)本发明方法给出的协调控制方法充分利用多机械臂系统冗余自由度实现控制，避免额外使用推进剂，能够有效提升在轨飞行器的使用寿命。
附图说明
图1为本发明的空间多机械臂系统建模示意图；
图2为本发明方法的工作流程图。
具体实施方式
为使本发明的目的、技术方案和优点更加清楚，下面将结合附图对本发明的实施方式作进一步详细描述。
一种考虑引力影响的空间机械臂系统显式动力学控制方法，包括如下步骤：
S1、获取任务参数、基座飞行器总体参数、初始时刻(t0时刻)基座飞行器运动参数；
S2、计算当前时刻(tj时刻)每部机械臂末端运动学系统矩阵，计算tj时刻机械臂末端运动学集总系统矩阵、基座飞行器动力学系统矩阵，计算tj时刻每部机械臂动力学系统矩阵，计算tj时刻的集总系统矩阵、引力补偿量、虚拟速度控制、虚拟速度控制导数、耦合控制矩阵；
S3、判断耦合控制矩阵是否满足秩条件，若满足，则转入S5，否则转入S4；
S4、修正tj时刻的耦合控制矩阵，然后返回S3；
S5、计算tj时刻的机械臂指令关节角加速度矢量、机械臂指令关节角速度矢量；
S6、计算并输出tj时刻的机械臂关节力矩，用于实施控制，待下一时刻，返回S2。
作为本发明的一种优选方案，空间机械臂系统包括多部机械臂，每部机械臂含有多个转动关节；对于每部机械臂，转动关节和转动关节之间均由臂杆连接，且转动关节通过臂杆连接末端执行器。
作为本发明的一种优选方案，S4中，当耦合控制矩阵为方阵时，利用修正系数进行修正；当耦合控制矩阵为非方阵时，对耦合控制矩阵进行分块后利用修正系数进行修正。
作为本发明的一种优选方案，S1中，所述任务参数包括基座飞行器期望位置矢量、期望姿态四元数、第i部机械臂期望末端位置和期望速度。
作为本发明的一种优选方案，S1中，所述基座飞行器总体参数包括基座飞行器质量m0和基座飞行器转动惯量I0、机械臂数目N；对于第i部机械臂，关节数为ni、第k个臂杆质量
Figure BDA0002750270460000061
第k个关节
Figure BDA0002750270460000062
至第k+1个臂杆质心Ck的位置矢量
Figure BDA0002750270460000063
第k个臂杆质心Ck至第k+1个关节
Figure BDA0002750270460000064
的位置矢量
Figure BDA0002750270460000065
第k个臂杆转动惯量
Figure BDA0002750270460000066
关节1的安装位置矢量
Figure BDA0002750270460000067
作为本发明的一种优选方案，S1中，所述t0时刻基座飞行器运动参数包括获取地心至基座飞行器质心的矢量rE0(t0)、基座飞行器初始位置偏差r0(t0)、基座飞行器初始线速度偏差矢量v0(t0)、基座飞行器初始姿态偏差四元数q(t0)、初始角速度偏差矢量ω0(t0)、机械臂各关节初始角速度矢量
Figure BDA0002750270460000068
初始关节角度矢量Θ(t0)。
作为本发明的一种优选方案，S2中，利用当前时刻(tj时刻)每部机械臂的关节角、关节方向矢量和臂杆长度，计算tj时刻每部机械臂末端运动学系统矩阵和机械臂末端运动学集总系统矩阵；利用当前时刻(tj时刻)每部机械臂的关节角、关节方向矢量和臂杆长度、基座飞行器和每部机械臂臂杆的质量及转动惯量，计算tj时刻基座飞行器动力学系统矩阵；利用当前时刻(tj时刻)每部机械臂的关节角、关节方向矢量和臂杆长度、基座飞行器和每部机械臂臂杆的质量及转动惯量，计算tj时刻每部机械臂动力学系统矩阵和集总系统矩阵；利用当前时刻(tj时刻)每部机械臂的关节角、关节方向矢量和臂杆长度、基座飞行器和每部机械臂臂杆的质量及转动惯量、地球引力常数、基座飞行器期望位置矢量、基座飞行器质心至每部机械臂臂杆的位置矢量，计算当前时刻(tj时刻)的引力补偿量；利用位姿增益矩阵，当前时刻(tj时刻)基座飞行器的姿态、位置、速度和角速度，计算当前时刻(tj时刻)的虚拟速度控制及其导数；利用位姿增益矩阵，当前时刻(tj时刻)基座飞行器的姿态、位置、速度和角速度，计算当前时刻(tj时刻)的虚拟速度控制及其导数；利用当前时刻(tj时刻)的机械臂末端与基座飞行器集总运动耦合矩阵和机械臂集总运动学矩阵，计算当前时刻(tj时刻)的耦合控制矩阵。
作为本发明的一种优选方案，S5中，利用末端位置增益矩阵和基座飞行器增益矩阵，当前时刻(tj时刻)的机械臂末端位置与速度矢量，机械臂末端期望位置与速度矢量，虚拟速度控制及其导数，机械臂末端与基座飞行器集总运动耦合矩阵，机械臂集总运动学矩阵，基座飞行器动力学系统矩阵，引力补偿量，耦合控制矩阵，计算当前时刻(tj时刻)的机械臂指令关节加速度矢量和机械臂指令关节角速度矢量。
作为本发明的一种优选方案，S6中，利用机械臂增益矩阵，当前时刻(tj时刻)的机械臂末端位置矢量，机械臂末端期望位置矢量，机械臂指令关节加速度矢量与指令关节角速度矢量，基座飞行器速度与角速度矢量，机械臂末端运动学集总系统矩阵，集总系统矩阵，虚拟速度控制，引力补偿量，计算当前时刻(tj时刻)的机械臂关节力矩。
实施例：
本发明可应用于空间近距离操作任务中多机械臂飞行器系统，解决其受引力影响下的基座飞行器位姿稳定及机械臂末端位置调整的控制问题。结合图1，基座飞行器的质心在C0，基座飞行器携带N部机械臂，每部机械臂含有ni(i＝1,2,…,N)个转动关节，均通过第一关节
Figure BDA0002750270460000081
安装在飞行器的外表面，安装的位置矢量为
Figure BDA0002750270460000082
图1以第i部机械臂作为示意，该部机械臂具有ni个转动关节，
Figure BDA0002750270460000083
每两个转动关节之间由刚性杆连接，转动关节
Figure BDA0002750270460000084
通过刚性杆
Figure BDA0002750270460000085
连接末端执行器
Figure BDA0002750270460000086
下面结合附图对本发明作进一步详细说明：
1)基座飞行器位姿运动学模型
航天器相对于惯性系的位姿运动学方程为：
Figure BDA0002750270460000087
其中，q为基座飞行器的姿态偏差，以四元数形式表征，qv为矢量部分，q0为标量部分，ω0为基座飞行器的角速度偏差，r0为基座飞行器位置偏差，v0为基座飞行器速度偏差，E为单位矩阵，
Figure BDA0002750270460000088
为矢量qv的反对称矩阵。
2)机械臂末端运动学
第i部机械臂末端的运动学方程为：
Figure BDA0002750270460000089
其中，
Figure BDA00027502704600000810
为第i部机械臂的末端位置，
Figure BDA00027502704600000811
为第i部机械臂的末端速度，V0为基座飞行器的广义速度，表征为
Figure BDA00027502704600000812
Figure BDA00027502704600000813
为第i部机械臂的关节角速度矢量，
Figure BDA00027502704600000814
和
Figure BDA00027502704600000815
分别为第i部机械臂末端与基座飞行器运动耦合矩阵和第i部机械臂运动学矩阵，表征为：
Figure BDA0002750270460000091
其中，
Figure BDA0002750270460000092
为第i部机械臂第p个臂杆至第q个臂杆的相对位置矢量，表述为
Figure BDA0002750270460000093
Figure BDA0002750270460000094
Figure BDA0002750270460000095
为第i部机械臂第k个关节
Figure BDA0002750270460000096
至第k个臂杆质心的位置矢量，
Figure BDA0002750270460000097
为第i部机械臂第k个臂杆质心关节
Figure BDA0002750270460000098
至第k+1个关节
Figure BDA0002750270460000099
的位置矢量，
Figure BDA00027502704600000910
为第i部机械臂第k个关节的方向矢量；
Figure BDA00027502704600000911
为矢量
Figure BDA00027502704600000912
的反对称矩阵。
因此，集总形式的机械臂末端运动学方程为：
Figure BDA00027502704600000913
其中，pe和ve分别为机械臂末端位置矢量和速度矢量，表征为：
Figure BDA00027502704600000914
Figure BDA00027502704600000915
和
Figure BDA00027502704600000916
分别为机械臂末端与基座飞行器集总运动耦合矩阵和机械臂集总运动学矩阵，表征为：
Figure BDA00027502704600000917
3)空间多机械臂的显式动力学模型
含有N部机械臂的飞行器动力学方程为：
Figure BDA00027502704600000918
Figure BDA00027502704600000919
其中，
Figure BDA00027502704600000920
为基座飞行器质心至第i部机械臂第k个臂杆质心的相对位置矢量，
Figure BDA00027502704600000921
为第i部机械臂的关节控制矢量，Hbb为基座飞行器惯性矩阵，
Figure BDA00027502704600000922
为基座飞行器与第i部机械臂的惯性耦合矩阵，
Figure BDA00027502704600000923
为第i部机械臂惯性矩阵，
Figure BDA00027502704600000924
为基座飞行器科氏矩阵，
Figure BDA0002750270460000101
为基座飞行器与第i部机械臂的科氏耦合矩阵，
Figure BDA0002750270460000102
为第i部机械臂与基座飞行器的科氏耦合矩阵，
Figure BDA0002750270460000103
为第i部机械臂科氏耦合矩阵，上述矩阵的显式形式分别表征为：
Figure BDA0002750270460000104
Figure BDA0002750270460000105
其中，M0为基座飞行器质量特性矩阵，Mi为第i部机械臂的质量特性矩阵，Gi为第i部机械臂的构型矩阵，
Figure BDA0002750270460000106
为第i部机械臂与基座的构型耦合矩阵，
Figure BDA0002750270460000107
为第i部机械臂的构型导数矩阵，
Figure BDA0002750270460000108
为第i部机械臂的构型对角导数矩阵，Hi为第i部机械臂的关节方向对角矩阵，
Figure BDA0002750270460000109
为第i部机械臂的关节方向矩阵，上述矩阵及矩阵
Figure BDA00027502704600001010
和Bi分别表征为：
Figure BDA00027502704600001011
其中，针对任一矢量ζ，矩阵算子G(ζ)定义为
Figure BDA0002750270460000111
另外，
Figure BDA00027502704600001113
为第i部机械臂的第k个臂杆的转动惯量，
Figure BDA00027502704600001114
为第i部机械臂的第k个臂杆的质量；I0为基座飞行器的转动惯量；m0为基座飞行器质量；
Figure BDA00027502704600001115
定义为：
Figure BDA0002750270460000112
Figure BDA0002750270460000113
为关节
Figure BDA0002750270460000114
角速度矢量的反对称矩阵，Fg0为引力对基座飞行器的广义干扰矢量，表征为：
Figure BDA0002750270460000115
Figure BDA0002750270460000116
为引力对第i部机械臂的第k个臂杆的广义干扰矢量，表征为：
Figure BDA0002750270460000117
其中，μ为地球引力常数，
Figure BDA0002750270460000118
为地心至第i部机械臂的第k个臂杆的位置矢量，rEO为基座飞行器期望位置矢量，rE0为地心至基座飞行器的相对位置矢量，
Figure BDA0002750270460000119
为基座飞行器质心至第i部机械臂的第k个臂杆的位置矢量，
Figure BDA00027502704600001110
为矢量
Figure BDA00027502704600001111
导数的反对称矩阵；R为轨道半径。
因此，集总形式下含有N部机械臂的飞行器动力学方程为：
Figure BDA00027502704600001112
Figure BDA0002750270460000121
其中，
Figure BDA0002750270460000122
为基座飞行器质心至第i部机械臂的第k个臂杆质心的位置矢量，τc为机械臂关节力矩矢量；Hbm为基座飞行器与机械臂的集总惯性耦合矩阵，Hmm为机械臂集总惯性矩阵，Cbb为基座飞行器集总科氏矩阵，Cbm为基座飞行器与机械臂的集总科氏耦合矩阵，Cmb为机械臂与基座飞行器的集总科氏耦合矩阵，Cmm为机械臂集总科氏耦合矩阵，τgm为引力对集总机械臂干扰矢量，τgm与τg相等，上述矩阵与矢量表征为：
Figure BDA0002750270460000123
其中，
Figure BDA0002750270460000124
和B0定义为：
Figure BDA0002750270460000125
Figure BDA0002750270460000126
其中，
Figure BDA0002750270460000127
为基座飞行器角速度的反对称矩阵。
4)如图2所示，一种考虑引力影响的空间机械臂系统显式动力学控制方法，包括以下步骤：
(1)获取任务参数，包括基座飞行器期望位置矢量rEO，期望姿态四元数qd，第i部机械臂期望末端位置为
Figure BDA0002750270460000131
与期望速度
Figure BDA0002750270460000132
(2)获取基座飞行器总体参数，包括基座飞行器质量m0和基座飞行器转动惯量I0、机械臂数目N，其中，第i部机械臂的关节数为ni、其第k个臂杆质量
Figure BDA0002750270460000133
其第k个关节
Figure BDA0002750270460000134
至第k+1个臂杆质心Ck的位置矢量
Figure BDA0002750270460000135
其第k个臂杆质心
Figure BDA0002750270460000136
至第k+1个关节
Figure BDA0002750270460000137
的位置矢量
Figure BDA0002750270460000138
其第k个臂杆转动惯量
Figure BDA0002750270460000139
其关节1的安装位置矢量
Figure BDA00027502704600001310
(3)获取初始时刻(t0时刻)基座飞行器运动参数，包括获取地心至基座飞行器质心的矢量rE0(t0)，基座飞行器初始位置偏差r0(t0)、基座飞行器初始线速度偏差矢量v0(t0)、基座飞行器初始姿态偏差四元数q(t0)和初始角速度偏差矢量ω0(t0)、及机械臂各关节初始角速度矢量
Figure BDA00027502704600001311
及初始关节角度矢量Θ(t0)。
(4)按下述公式计算当前时刻(tj时刻，j＝0,1,2,…)第i部机械臂末端运动学系统矩阵，包括第i部机械臂末端与基座飞行器运动耦合矩阵
Figure BDA00027502704600001312
和第i部机械臂运动学矩阵
Figure BDA00027502704600001313
Figure BDA00027502704600001314
其中，
Figure BDA00027502704600001315
Figure BDA00027502704600001316
为第i部机械臂第k个关节的方向矢量。
(5)按下述公式计算当前时刻(tj时刻，j＝0,1,2,…)机械臂末端运动学集总系统矩阵，包括机械臂末端与基座飞行器集总运动耦合矩阵Heb和机械臂集总运动学矩阵Hem：
Figure BDA00027502704600001317
(6)按下述公式计算当前时刻(tj时刻，j＝0,1,2,…)基座飞行器动力学系统矩阵，包括基座飞行器惯性矩阵Hbb，基座飞行器与第i部机械臂的惯性耦合矩阵
Figure BDA0002750270460000141
基座飞行器科氏矩阵
Figure BDA0002750270460000142
基座飞行器与第i部机械臂的科氏耦合矩阵
Figure BDA0002750270460000143
Figure BDA0002750270460000144
Figure BDA0002750270460000145
其中，M0为基座飞行器质量特性矩阵，Mi为第i部机械臂的质量特性矩阵，Gi为第i部机械臂的构型矩阵，
Figure BDA0002750270460000146
为第i部机械臂与基座的构型耦合矩阵，Gi为第i部机械臂的构型矩阵，
Figure BDA0002750270460000147
为第i部机械臂的构型导数矩阵，
Figure BDA0002750270460000148
为第i部机械臂的构型对角导数矩阵，Hi为第i部机械臂的关节方向对角矩阵，
Figure BDA0002750270460000149
为第i部机械臂的关节方向矩阵,矩阵
Figure BDA00027502704600001410
和Bi分别表征为：
Figure BDA00027502704600001411
其中，
Figure BDA00027502704600001412
(7)按下述计算当前时刻(tj时刻，j＝0,1,2,…)第i部机械臂动力学系统矩阵，包括第i部机械臂惯性矩阵
Figure BDA00027502704600001415
第i部机械臂与基座飞行器的科氏耦合矩阵
Figure BDA00027502704600001413
第i部机械臂科氏耦合矩阵
Figure BDA00027502704600001414
Figure BDA0002750270460000151
(8)计算当前时刻(tj时刻，j＝0,1,2,…)集总系统矩阵，包括基座飞行器与机械臂的集总惯性耦合矩阵Hbm，机械臂集总惯性矩阵Hmm，基座飞行器集总科氏矩阵Cbb，基座飞行器与机械臂的集总科氏耦合矩阵Cbm，机械臂与基座飞行器的集总科氏耦合矩阵Cmb，机械臂集总科氏耦合矩阵Cmm：
Figure BDA0002750270460000152
Figure BDA0002750270460000153
Figure BDA0002750270460000154
其中，
Figure BDA0002750270460000155
(9)按下式计算当前时刻(tj时刻，j＝0,1,2,…)的引力补偿量，包括基座飞行器级引力补偿矢量
Figure BDA0002750270460000156
和机械臂级引力补偿矢量τgm。
Figure BDA0002750270460000157
其中，
Figure BDA0002750270460000158
(10)按下述公式计算当前时刻(tj时刻，j＝0,1,2,…)的虚拟速度控制Vc及其导数
Figure BDA0002750270460000161
Figure BDA0002750270460000162
其中，ωc为虚拟角速度控制矢量，vc为虚拟速度控制矢量，Kx为位姿增益矩阵，而且，
Figure BDA0002750270460000163
其中
qs＝[1000]T
(11)按下述公式计算当前时刻(tj时刻，j＝0,1,2,…)的耦合控制矩阵Hc：
Figure BDA0002750270460000164
并判断其是否满足下述秩条件，
Figure BDA0002750270460000165
若满足，则进入步骤(13)，否则进入步骤(12)。
(12)判断：若Hc为方阵，那么按下式修正Hc：
Hc＝Hc+εE
其中，ε为修正系数，可取为0.001，若Hc为非方阵，则对其进行分块：
Hc＝[Hcl Hcr]
并确保Hcl为方阵，随后对其按下式进行修正：
Hcl＝Hcl+εE
并返回步骤(11)：
(13)按下述公式计算当前时刻(tj时刻，j＝0,1,2,…)的机械臂指令关节角加速度矢量
Figure BDA0002750270460000171
并计算机械臂指令关节角速度矢量
Figure BDA0002750270460000172
Figure BDA0002750270460000173
Figure BDA0002750270460000174
其中，ξ为任一矢量，
Figure BDA0002750270460000175
为矩阵Hc的广义逆，ζ表征为：
Figure BDA0002750270460000176
其中，Ke为末端位置增益矩阵，Kb为基座飞行器增益矩阵，ve为机械臂末端速度矢量，pd为机械臂末端期望位置矢量，pe为机械臂末端位置矢量。
(14)按下述公式计算并输出当前时刻(tj时刻，j＝0,1,2,…)的机械臂关节力矩τc，实施控制，待下一时刻，返回步骤(4)开始下一时刻的控制量计算，直至控制结束。
Figure BDA0002750270460000177
其中，Km为机械臂增益矩阵。
本发明说明书中未作详细描述的内容属本领域技术人员的公知技术。
本发明虽然已以较佳实施例公开如上，但其并不是用来限定本发明，任何本领域技术人员在不脱离本发明的精神和范围内，都可以利用上述揭示的方法和技术内容对本发明技术方案做出可能的变动和修改，因此，凡是未脱离本发明技术方案的内容，依据本发明的技术实质对以上实施例所作的任何简单修改、等同变化及修饰，均属于本发明技术方案的保护范围。

Claims (12)

1.一种考虑引力影响的空间机械臂系统显式动力学控制方法，其特征在于，包括如下步骤：
S1、获取任务参数、基座飞行器总体参数、初始时刻基座飞行器运动参数，初始时刻记为t0；
S2、计算当前时刻每部机械臂末端运动学系统矩阵，当前时刻记为tj，计算tj时刻机械臂末端运动学集总系统矩阵、基座飞行器动力学系统矩阵，计算tj时刻每部机械臂动力学系统矩阵，计算tj时刻的集总系统矩阵、引力补偿量、虚拟速度控制、虚拟速度控制导数、耦合控制矩阵；
S3、判断耦合控制矩阵是否满足秩条件，若满足，则转入S5，否则转入S4；
S4、修正tj时刻的耦合控制矩阵，然后返回S3；
S5、计算tj时刻的机械臂指令关节角加速度矢量、机械臂指令关节角速度矢量；
S6、计算并输出tj时刻的机械臂关节力矩，用于实施控制，待下一时刻，返回S2。
2.根据权利要求1所述的一种考虑引力影响的空间机械臂系统显式动力学控制方法，其特征在于，空间机械臂系统包括多部机械臂，每部机械臂含有多个转动关节；对于每部机械臂，转动关节和转动关节之间均由臂杆连接，且转动关节通过臂杆连接末端执行器。
3.根据权利要求1所述的一种考虑引力影响的空间机械臂系统显式动力学控制方法，其特征在于，S4中，当耦合控制矩阵为方阵时，利用修正系数进行修正；当耦合控制矩阵为非方阵时，对耦合控制矩阵进行分块后利用修正系数进行修正。
4.根据权利要求1所述的一种考虑引力影响的空间机械臂系统显式动力学控制方法，S1中，所述任务参数包括基座飞行器期望位置矢量、期望姿态四元数、第i部机械臂期望末端位置和期望速度。
5.根据权利要求1所述的一种考虑引力影响的空间机械臂系统显式动力学控制方法，S1中，所述基座飞行器总体参数包括基座飞行器质量m0和基座飞行器转动惯量I0、机械臂数目N；对于第i部机械臂，关节数为ni、第k个臂杆质量
Figure FDA0003459088360000028
第k个关节
Figure FDA0003459088360000025
至第k+1个臂杆质心Ck的位置矢量
Figure FDA0003459088360000022
第k个臂杆质心Ck至第k+1个关节
Figure FDA0003459088360000026
的位置矢量
Figure FDA0003459088360000023
第k个臂杆转动惯量
Figure FDA0003459088360000024
关节1的安装位置矢量
Figure FDA0003459088360000027
6.根据权利要求1所述的一种考虑引力影响的空间机械臂系统显式动力学控制方法，S1中，所述t0时刻基座飞行器运动参数包括获取地心至基座飞行器质心的矢量rE0(t0)、基座飞行器初始位置偏差r0(t0)、基座飞行器初始线速度偏差矢量v0(t0)、基座飞行器初始姿态偏差四元数q(t0)、初始角速度偏差矢量ω0(t0)、机械臂各关节初始角速度矢量
Figure FDA0003459088360000021
初始关节角度矢量Θ(t0)。
7.根据权利要求1所述的一种考虑引力影响的空间机械臂系统显式动力学控制方法，S2中，利用tj时刻每部机械臂的关节角、关节方向矢量和臂杆长度、基座飞行器和每部机械臂臂杆的质量及转动惯量，计算tj时刻每部机械臂末端运动学系统矩阵和机械臂末端运动学集总系统矩阵；计算tj时刻基座飞行器动力学系统矩阵；计算tj时刻每部机械臂动力学系统矩阵和集总系统矩阵。
8.根据权利要求1所述的一种考虑引力影响的空间机械臂系统显式动力学控制方法，S2中，利用tj时刻每部机械臂的关节角、关节方向矢量和臂杆长度、基座飞行器和每部机械臂臂杆的质量及转动惯量、地球引力常数、基座飞行器期望位置矢量、基座飞行器质心至每部机械臂臂杆的位置矢量，计算tj时刻的引力补偿量。
9.根据权利要求1所述的一种考虑引力影响的空间机械臂系统显式动力学控制方法，S2中，利用位姿增益矩阵，tj时刻基座飞行器的姿态、位置、速度和角速度，计算tj时刻的虚拟速度控制及其导数。
10.根据权利要求1所述的一种考虑引力影响的空间机械臂系统显式动力学控制方法，S2中，利用tj时刻的机械臂末端与基座飞行器集总运动耦合矩阵和机械臂集总运动学矩阵，计算tj时刻的耦合控制矩阵。
11.根据权利要求1～10之一所述的一种考虑引力影响的空间机械臂系统显式动力学控制方法，S5中，利用末端位置增益矩阵和基座飞行器增益矩阵，tj时刻的机械臂末端位置与速度矢量，机械臂末端期望位置与速度矢量，虚拟速度控制及其导数，机械臂末端与基座飞行器集总运动耦合矩阵，机械臂集总运动学矩阵，基座飞行器动力学系统矩阵，引力补偿量，耦合控制矩阵，计算tj时刻的机械臂指令关节加速度矢量和机械臂指令关节角速度矢量。
12.根据权利要求1～10之一所述的一种考虑引力影响的空间机械臂系统显式动力学控制方法，S6中，利用机械臂增益矩阵，tj时刻的机械臂末端位置矢量，机械臂末端期望位置矢量，机械臂指令关节加速度矢量与指令关节角速度矢量，基座飞行器速度与角速度矢量，机械臂末端运动学集总系统矩阵，集总系统矩阵，虚拟速度控制，引力补偿量，计算tj时刻的机械臂关节力矩。
CN202011181309.7A 2020-10-29 2020-10-29 一种考虑引力影响的空间机械臂系统显式动力学控制方法 Active CN112405528B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202011181309.7A CN112405528B (zh) 2020-10-29 2020-10-29 一种考虑引力影响的空间机械臂系统显式动力学控制方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202011181309.7A CN112405528B (zh) 2020-10-29 2020-10-29 一种考虑引力影响的空间机械臂系统显式动力学控制方法

Publications (2)

Publication Number Publication Date
CN112405528A CN112405528A (zh) 2021-02-26
CN112405528B true CN112405528B (zh) 2022-04-12

Family

ID=74840276

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202011181309.7A Active CN112405528B (zh) 2020-10-29 2020-10-29 一种考虑引力影响的空间机械臂系统显式动力学控制方法

Country Status (1)

Country Link
CN (1) CN112405528B (zh)

Families Citing this family (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN114280932B (zh) * 2021-12-14 2023-11-10 中国运载火箭技术研究院 一种考虑伺服机构动态特性的运载器位姿一体化控制方法

Family Cites Families (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JP2003196323A (ja) * 2001-12-27 2003-07-11 Toyota Motor Corp 重量・重心位置設計支援装置、重量・重心位置設計支援方法および重量・重心位置設計支援プログラム
CN105799954B (zh) * 2014-12-31 2018-06-05 上海新跃仪表厂 天基分散部署微纳载荷的模块化飞行器及其变轨制导方法
CN105759613B (zh) * 2016-03-22 2017-08-15 沈阳上博智拓科技有限公司 倾转旋翼机的控制方法和控制装置
CN208409883U (zh) * 2018-06-25 2019-01-22 天津城建大学 机器人3d视觉采集电路
CN109927032B (zh) * 2019-03-28 2022-02-11 东南大学 一种基于高阶滑模观测器的机械臂轨迹跟踪控制方法

Also Published As

Publication number Publication date
CN112405528A (zh) 2021-02-26

Similar Documents

Publication Publication Date Title
Lippiello et al. Exploiting redundancy in Cartesian impedance control of UAVs equipped with a robotic arm
CN109606753B (zh) 一种空间双臂机器人协同抓捕目标的控制方法
Kondak et al. Aerial manipulation robot composed of an autonomous helicopter and a 7 degrees of freedom industrial manipulator
Ritz et al. Carrying a flexible payload with multiple flying vehicles
Lippiello et al. Cartesian impedance control of a UAV with a robotic arm
Ruggiero et al. Impedance control of VToL UAVs with a momentum-based external generalized forces estimator
Huber et al. First analysis and experiments in aerial manipulation using fully actuated redundant robot arm
CN106094528B (zh) 一种空间柔性机械臂振动抑制算法
Meng et al. Vibration suppression control of free-floating space robots with flexible appendages for autonomous target capturing
CN112558621A (zh) 一种基于解耦控制的飞行机械臂系统
CN109591018B (zh) 一种用于目标捕获后稳定的自由漂浮空间机械臂控制方法
CN111506095B (zh) 一种双刚体特征点间饱和固定时间相对位姿跟踪控制方法
Nagarajan et al. Planning in high-dimensional shape space for a single-wheeled balancing mobile robot with arms
CN107458630B (zh) 悬挂式主动重力补偿系统悬挂点跨尺度跟踪方法
Yu et al. An over-actuated multi-rotor aerial vehicle with unconstrained attitude angles and high thrust efficiencies
CN109656132B (zh) 一种空间机器人有限时间协调控制方法
Kim et al. Passive compliance control of aerial manipulators
US20050242241A1 (en) Method and system for steering a momentum control system
Cyril et al. Postcapture dynamics of a spacecraft-manipulator-payload system
Giglio et al. Selective compliance control for an unmanned aerial vehicle with a robotic arm
Petrov et al. Dynamic modeling and adaptive motion control of a two-wheeled self-balancing vehicle for personal transport
CN116069050A (zh) 一种抗扰动的旋翼无人机吊载系统减摇控制方法及装置
Goodarzi Autonomous aerial payload delivery with quadrotor using varying length cable
Bazylev et al. Adaptive control system for quadrotor equiped with robotic arm
CN112405528B (zh) 一种考虑引力影响的空间机械臂系统显式动力学控制方法

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination
GR01 Patent grant
GR01 Patent grant